C++ Point::y方法代码示例

bool Environment::occluded(Geometry2d::Point ball, Geometry2d::Point camera) {
    float camZ = 4;
    float ballZ = Ball_Radius;
    float intZ = Robot_Height;

    // FIXME: use actual physics engine to determine line of sight

    // Find where the line from the camera to the ball intersects the
    // plane at the top of the robots.
    //
    // intZ = (ballZ - camZ) * t + camZ
    float t = (intZ - camZ) / (ballZ - camZ);
    Geometry2d::Point intersection;
    intersection.x() = (ball.x() - camera.x()) * t + camera.x();
    intersection.y() = (ball.y() - camera.y()) * t + camera.y();

    // Return true if the intersection point is inside any robot
    for (const Robot* r : _blue) {
        if (intersection.nearPoint(r->getPosition(), Robot_Radius)) {
            return true;
        }
    }
    for (const Robot* r : _yellow) {
        if (intersection.nearPoint(r->getPosition(), Robot_Radius)) {
            return true;
        }
    }
    return false;
}

void Environment::sendVision() {
    SSL_WrapperPacket wrapper;
    SSL_DetectionFrame* det = wrapper.mutable_detection();
    det->set_frame_number(_frameNumber++);
    det->set_camera_id(0);

    struct timeval tv;
    gettimeofday(&tv, nullptr);
    det->set_t_capture(tv.tv_sec + (double)tv.tv_usec * 1.0e-6);
    det->set_t_sent(det->t_capture());

    for (Robot* robot : _yellow) {
        if ((rand() % 100) < robot->visibility) {
            SSL_DetectionRobot* out = det->add_robots_yellow();
            convert_robot(robot, out);
        }
    }

    for (Robot* robot : _blue) {
        if ((rand() % 100) < robot->visibility) {
            SSL_DetectionRobot* out = det->add_robots_blue();
            convert_robot(robot, out);
        }
    }

    for (const Ball* b : _balls) {
        Geometry2d::Point ballPos = b->getPosition();

        // bool occ;
        // if (ballPos.x < 0)
        // {
        // 	occ = occluded(ballPos, cam0);
        // } else {
        // 	occ = occluded(ballPos, cam1);
        // }

        if ((rand() % 100) < ballVisibility) {
            SSL_DetectionBall* out = det->add_balls();
            out->set_confidence(1);
            out->set_x(ballPos.x() * 1000);
            out->set_y(ballPos.y() * 1000);
            out->set_pixel_x(ballPos.x() * 1000);
            out->set_pixel_y(ballPos.y() * 1000);
        }
    }

    std::string buf;
    wrapper.SerializeToString(&buf);

    if (sendShared) {
        _visionSocket.writeDatagram(&buf[0], buf.size(), MulticastAddress,
                                    SharedVisionPort);
    } else {
        _visionSocket.writeDatagram(&buf[0], buf.size(), LocalAddress,
                                    SimVisionPort);
        _visionSocket.writeDatagram(&buf[0], buf.size(), LocalAddress,
                                    SimVisionPort + 1);
    }
}

void Environment::addBall(Geometry2d::Point pos) {
    Ball* b = new Ball(this);
    b->initPhysics();
    b->position(pos.x(), pos.y());

    _balls.append(b);

    printf("New Ball: %f %f\n", pos.x(), pos.y());
}

void Environment::convert_robot(const Robot* robot, SSL_DetectionRobot* out) {
    Geometry2d::Point pos = robot->getPosition();
    out->set_confidence(1);
    out->set_robot_id(robot->shell);
    out->set_x(pos.x() * 1000);
    out->set_y(pos.y() * 1000);
    out->set_orientation(robot->getAngle());
    out->set_pixel_x(pos.x() * 1000);
    out->set_pixel_y(pos.y() * 1000);
}

void OurRobot::move(Geometry2d::Point goal, Geometry2d::Point endVelocity) {
    if (!visible) return;

    // sets flags for future movement
    if (verbose)
        cout << " in OurRobot::move(goal): adding a goal (" << goal.x() << ", "
             << goal.y() << ")" << std::endl;

    _motionCommand = std::make_unique<Planning::PathTargetCommand>(
        MotionInstant(goal, endVelocity));

    *_cmdText << "move(" << goal.x() << ", " << goal.y() << ")" << endl;
    *_cmdText << "endVelocity(" << endVelocity.x() << ", " << endVelocity.y()
              << ")" << endl;
}

TEST(WorldRobot, two_robot) {
    RJ::Time t = RJ::now();
    Geometry2d::Point p1 = Geometry2d::Point(1,1);
    Geometry2d::Point p2 = Geometry2d::Point(2,2);
    double th1 = 1;
    double th2 = 2;
    int rID = 1;
    CameraRobot b1 = CameraRobot(t, p1, th1, rID);
    CameraRobot b2 = CameraRobot(t, p2, th2, rID);
    int cID = 1;
    WorldRobot w;

    KalmanRobot kb1 = KalmanRobot(cID, t, b1, w);
    KalmanRobot kb2 = KalmanRobot(cID, t, b2, w);

    std::list<KalmanRobot> kbl;
    kbl.push_back(kb1);
    kbl.push_back(kb2);

    WorldRobot wb = WorldRobot(t, WorldRobot::Team::BLUE, rID, kbl);

    Geometry2d::Point rp = wb.getPos();
    double rt = wb.getTheta();
    Geometry2d::Point rv = wb.getVel();
    double ro = wb.getOmega();
    double rpc = wb.getPosCov();
    double rvc = wb.getVelCov();

    std::list<KalmanRobot> list = wb.getRobotComponents();

    EXPECT_TRUE(wb.getIsValid());
    EXPECT_EQ(rp.x(), (p1.x() + p2.x()) / 2);
    EXPECT_EQ(rp.y(), (p1.y() + p2.y()) / 2);
    EXPECT_EQ(rt, (th1 + th2) / 2);
    EXPECT_EQ(rv.x(), 0);
    EXPECT_EQ(rv.y(), 0);
    EXPECT_EQ(ro, 0);
    EXPECT_GT(rpc, 0);
    EXPECT_GT(rvc, 0);
    EXPECT_LT(rpc, 10000);
    EXPECT_LT(rvc, 10000);
    EXPECT_EQ(list.size(), 2);
}

Geometry2d::Point gaussianPoint(int n, float scale) {
    Geometry2d::Point pt;
    for (int i = 0; i < n; ++i) {
        pt.x() += drand48() - 0.5;
        pt.y() += drand48() - 0.5;
    }
    pt *= scale / n;

    return pt;
}

void OurRobot::pivot(Geometry2d::Point pivotTarget) {
    _rotationCommand = std::make_unique<Planning::EmptyAngleCommand>();

    const float radius = Robot_Radius * 1;
    Geometry2d::Point pivotPoint = _state->ball.pos;

    // reset other conflicting motion commands
    _motionCommand = std::make_unique<Planning::PivotCommand>(
        pivotPoint, pivotTarget, radius);

    *_cmdText << "pivot(" << pivotTarget.x() << ", " << pivotTarget.y() << ")"
              << endl;
}

void OurRobot::moveDirect(Geometry2d::Point goal, float endSpeed) {
    if (!visible) return;

    // sets flags for future movement
    if (verbose)
        cout << " in OurRobot::moveDirect(goal): adding a goal (" << goal.x()
             << ", " << goal.y() << ")" << endl;

    _motionCommand = std::make_unique<Planning::DirectPathTargetCommand>(
        MotionInstant(goal, (goal - pos).normalized() * endSpeed));

    *_cmdText << "moveDirect(" << goal << ")" << endl;
    *_cmdText << "endSpeed(" << endSpeed << ")" << endl;
}

void Environment::addRobot(bool blue, int id, Geometry2d::Point pos,
                           Robot::RobotRevision rev) {
    Robot* r = new Robot(this, id, rev, pos);
    r->initPhysics(blue);

    if (blue) {
        _blue.insert(id, r);
    } else {
        _yellow.insert(id, r);
    }

    Geometry2d::Point actPos = r->getPosition();

    switch (rev) {
        case Robot::rev2008:
            printf("New 2008 Robot: %d : %f %f\n", id, actPos.x(), actPos.y());
            break;
        case Robot::rev2011:
            printf("New 2011 Robot: %d : %f %f\n", id, actPos.x(), actPos.y());
    }

    QVector3D pos3(pos.x(), pos.y(), 0.0);
    QVector3D axis(0.0, 0.0, 1.0);
}

Robot::Robot(Environment* env, unsigned int id, Robot::RobotRevision rev,
             Geometry2d::Point startPos)
    : Entity(env),
      shell(id),
      _rev(rev),
      _robotChassis(nullptr),
      _robotVehicle(nullptr),
      _wheelShape(nullptr),
      _controller(nullptr),
      _brakingForce(0),
      _targetVel(0, 0, 0),
      _targetRot(0),
      _simEngine(env->getSimEngine()) {
    visibility = 100;

    _startTransform.setIdentity();
    _startTransform.setOrigin(btVector3(startPos.y(), 0, startPos.x()) *
                              scaling);
    setEngineForce(0);
}

void OurRobot::worldVelocity(Geometry2d::Point v) {
    _motionCommand = std::make_unique<Planning::WorldVelTargetCommand>(v);
    setPath(nullptr);
    *_cmdText << "worldVel(" << v.x() << ", " << v.y() << ")" << endl;
}

void OurRobot::face(Geometry2d::Point pt) {
    _rotationCommand = std::make_unique<Planning::FacePointCommand>(pt);

    *_cmdText << "face(" << pt.x() << ", " << pt.y() << ")" << endl;
}

WorldBall::WorldBall(RJ::Time calcTime, std::list<KalmanBall> kalmanBalls) 
    : isValid(true), time(calcTime){
        
    Geometry2d::Point posAvg = Geometry2d::Point(0, 0);
    Geometry2d::Point velAvg = Geometry2d::Point(0, 0);
    double totalPosWeight = 0;
    double totalVelWeight = 0;

    // Below 1 would invert the ratio of scaling
    // Above 2 would just be super noisy
    if (*ball_merger_power < 1 || *ball_merger_power > 2) {

        std::cout
             << "WARN: ball_merger_power should be between 1 and 2"
             << std::endl;
    }

    if (kalmanBalls.size() == 0) {
        std::cout
             << "ERROR: Zero balls are given to the WorldBall constructor"
             << std::endl;

        isValid = false;
        pos = posAvg;
        vel = velAvg;
        posCov = 0;
        velCov = 0;

        return;
    }

    for (KalmanBall& ball : kalmanBalls) {
        // Get the covariance of everything
        // AKA how well we can predict the next measurement
        Geometry2d::Point posCov = ball.getPosCov();
        Geometry2d::Point velCov = ball.getVelCov();

        // Std dev of each state
        // Lower std dev gives better idea of true values
        Geometry2d::Point posStdDev;
        Geometry2d::Point velStdDev;
        posStdDev.x() = std::sqrt(posCov.x());
        posStdDev.y() = std::sqrt(posCov.y());
        velStdDev.x() = std::sqrt(velCov.x());
        velStdDev.y() = std::sqrt(velCov.y());

        // Inversely proportional to how much the filter has been updated
        double filterUncertantity = 1.0 / ball.getHealth();

        // How good of pos/vel estimation in total
        // (This is less efficient than just doing the sqrt(x_cov + y_cov),
        //  but it's a little more clear math-wise)
        double posUncertantity = posStdDev.mag();
        double velUncertantity = velStdDev.mag();

        // Weight better estimates higher
        double filterPosWeight = std::pow(posUncertantity * filterUncertantity,
                                          -*ball_merger_power);

        double filterVelWeight = std::pow(velUncertantity * filterUncertantity,
                                          -*ball_merger_power);

        posAvg += filterPosWeight * ball.getPos();
        velAvg += filterVelWeight * ball.getVel();

        totalPosWeight += filterPosWeight;
        totalVelWeight += filterVelWeight;
    }

    posAvg /= totalPosWeight;
    velAvg /= totalVelWeight;

    pos = posAvg;
    vel = velAvg;
    posCov = totalPosWeight / kalmanBalls.size();
    velCov = totalVelWeight / kalmanBalls.size();
    ballComponents = kalmanBalls;
}