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

 /** saturates the magnitude of a vector */
 static Geometry2d::Point saturate(Geometry2d::Point value, float max) {
     float mag = value.mag();
     if (mag > fabs(max)) {
         return value.normalized() * fabs(max);
     }
     return value;
 }

//// Fixed Step Tree ////
Tree::Point* FixedStepTree::extend(Geometry2d::Point pt, Tree::Point* base) {
    // if we don't have a base point, try to find a close point
    if (!base) {
        base = nearest(pt);
        if (!base) {
            return nullptr;
        }
    }

    Geometry2d::Point delta = pt - base->pos;
    float d = delta.mag();

    Geometry2d::Point pos;
    if (d < step) {
        pos = pt;
    } else {
        pos = base->pos + delta / d * step;
    }

    // Check for obstacles.

    // moveHit is the set of obstacles that this move touches.
    // If this move touches any obstacles that the starting point didn't already
    // touch,
    // it has entered an obstacle and will be rejected.
    std::set<shared_ptr<Geometry2d::Shape>> moveHit =
        _obstacles->hitSet(Geometry2d::Segment(pos, base->pos));
    if (!moveHit.empty()) {
        // We only care if there are any items in moveHit that are not in
        // point->hit, so
        // we don't store the result of set_difference.
        try {
            set_difference(
                moveHit.begin(), moveHit.end(), base->hit.begin(),
                base->hit.end(),
                ExceptionIterator<std::shared_ptr<Geometry2d::Shape>>());
        } catch (exception& e) {
            // We hit a new obstacle
            return nullptr;
        }
    }

    // Allow this point to be added to the tree
    Point* p = new Point(pos, base);
    p->hit = std::move(moveHit);
    points.push_back(p);

    return p;
}

/**
 * Generates a Cubic Bezier Path based on Albert's random Bezier Velocity Path
 * Algorithm
 */
std::vector<InterpolatedPath::Entry> RRTPlanner::generateVelocityPath(
    const std::vector<CubicBezierControlPoints>& controlPoints,
    const MotionConstraints& motionConstraints, Geometry2d::Point vi,
    Geometry2d::Point vf, int interpolations) {
    // Interpolate Through Bezier Path
    vector<Point> newPoints, newPoints1stDerivative, newPoints2ndDerivative;
    vector<float> newPointsCurvature, newPointsDistance, newPointsSpeed;

    float totalDistance = 0;
    const float maxAceleration = motionConstraints.maxAcceleration;

    for (const CubicBezierControlPoints& controlPoint : controlPoints) {
        Point p0 = controlPoint.p0;
        Point p1 = controlPoint.p1;
        Point p2 = controlPoint.p2;
        Point p3 = controlPoint.p3;
        for (int j = 0; j < interpolations; j++) {
            float t = (((float)j / (float)(interpolations)));
            Geometry2d::Point pos =
                pow(1.0 - t, 3) * p0 + 3.0 * pow(1.0 - t, 2) * t * p1 +
                3 * (1.0 - t) * pow(t, 2) * p2 + pow(t, 3) * p3;

            // Derivitive 1
            // 3 k (-(A (-1 + k t)^2) + k t (2 C - 3 C k t + D k t) + B (1 - 4 k
            // t + 3 k^2 t^2))
            Geometry2d::Point d1 = 3 * pow(1 - t, 2) * (p1 - p0) +
                                   6 * (1 - t) * t * (p2 - p1) +
                                   3 * pow(t, 2) * (p3 - p2);

            // Derivitive 2
            // https://en.wikipedia.org/wiki/B%C3%A9zier_curve#Cubic_B.C3.A9zier_curves
            // B''(t) = 6(1-t)(P2 - 2*P1 + P0) + 6*t(P3 - 2 * P2 + P1)
            Geometry2d::Point d2 =
                6 * (1 - t) * (p2 - 2 * p1 + p0) + 6 * t * (p3 - 2 * p2 + p1);

            // https://en.wikipedia.org/wiki/Curvature#Local_expressions
            // K = |x'*y'' - y'*x''| / (x'^2 + y'^2)^(3/2)
            float curvature =
                std::abs(d1.x * d2.y - d1.y * d2.x) /
                std::pow(std::pow(d1.x, 2) + std::pow(d1.y, 2), 1.5);

            // Handle 0 velocity case
            if (isnan(curvature)) {
                curvature = 0;
            }

            assert(curvature >= 0);
            if (!newPoints.empty()) {
                float distance = pos.distTo(newPoints.back());
                totalDistance += distance;
            }
            newPointsDistance.push_back(totalDistance);

            newPoints.push_back(pos);
            newPoints1stDerivative.push_back(d1);
            newPoints2ndDerivative.push_back(d2);

            newPointsCurvature.push_back(curvature);

            // Isolated maxSpeed based on Curvature
            // Curvature = 1/Radius of Curvature
            // vmax = sqrt(acceleartion/abs(Curvature))

            float constantMaxSpeed = std::sqrt(maxAceleration / curvature);
            newPointsSpeed.push_back(constantMaxSpeed);
        }
    }
    // Get last point in Path
    CubicBezierControlPoints lastControlPoint = controlPoints.back();

    Point p0 = lastControlPoint.p0;
    Point p1 = lastControlPoint.p1;
    Point p2 = lastControlPoint.p2;
    Point p3 = lastControlPoint.p3;

    Point pos = p3;
    Point d1 = vf;
    Geometry2d::Point d2 = 6 * (1) * (p3 - 2 * p2 + p1);
    float curvature = std::abs(d1.x * d2.y - d1.y * d2.x) /
                      std::pow(std::pow(d1.x, 2) + std::pow(d1.y, 2), 1.5);

    // handle 0 velcoity case
    if (isnan(curvature)) {
        curvature = 0;
    }

    totalDistance += pos.distTo(newPoints.back());
    newPoints.push_back(pos);
    newPoints1stDerivative.push_back(vf);
    newPoints2ndDerivative.push_back(d2);
    newPointsCurvature.push_back(curvature);

    newPointsDistance.push_back(totalDistance);

    newPointsSpeed[0] = vi.mag();
    newPointsSpeed.push_back(vf.mag());
//.........这里部分代码省略.........

vector<CubicBezierControlPoints> RRTPlanner::generateCubicBezierPath(
    const vector<Geometry2d::Point>& points,
    const MotionConstraints& motionConstraints, Geometry2d::Point vi,
    Geometry2d::Point vf, const boost::optional<vector<float>>& times) {
    size_t length = points.size();
    size_t curvesNum = length - 1;
    vector<double> pointsX(length);
    vector<double> pointsY(length);
    vector<double> ks(length - 1);
    vector<double> ks2(length - 1);

    for (int i = 0; i < length; i++) {
        pointsX[i] = points[i].x;
        pointsY[i] = points[i].y;
    }
    const float startSpeed = vi.mag();

    const float endSpeed = vf.mag();

    if (times) {
        assert(times->size() == points.size());
        for (int i = 0; i < curvesNum; i++) {
            ks[i] = 1.0 / (times->at(i + 1) - times->at(i));
            ks2[i] = ks[i] * ks[i];
            if (std::isnan(ks[i])) {
                debugThrow(
                    "Something went wrong. Points are too close to each other "
                    "probably");
                return vector<CubicBezierControlPoints>();
            }
        }
    } else {
        for (int i = 0; i < curvesNum; i++) {
            ks[i] = 1.0 / (getTime(points, i + 1, motionConstraints, startSpeed,
                                   endSpeed) -
                           getTime(points, i, motionConstraints, startSpeed,
                                   endSpeed));
            ks2[i] = ks[i] * ks[i];
            if (std::isnan(ks[i])) {
                debugThrow(
                    "Something went wrong. Points are too close to each other "
                    "probably");
                return vector<CubicBezierControlPoints>();
            }
        }
    }

    VectorXd solutionX =
        RRTPlanner::cubicBezierCalc(vi.x, vf.x, pointsX, ks, ks2);
    VectorXd solutionY =
        RRTPlanner::cubicBezierCalc(vi.y, vf.y, pointsY, ks, ks2);

    vector<CubicBezierControlPoints> path;

    for (int i = 0; i < curvesNum; i++) {
        Point p0 = points[i];
        Point p1 = Geometry2d::Point(solutionX(i * 2), solutionY(i * 2));
        Point p2 =
            Geometry2d::Point(solutionX(i * 2 + 1), solutionY(i * 2 + 1));
        Point p3 = points[i + 1];
        path.emplace_back(p0, p1, p2, p3);
    }
    return path;
}

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;
}

void FieldView::drawTeamSpace(QPainter& p) {
    // Get the latest LogFrame
    const LogFrame* frame = _history->at(0).get();

    if (showTeamNames) {
        // Draw Team Names
        QFont savedFont = p.font();
        QFont fontstyle = p.font();
        fontstyle.setPointSize(20);
        p.setFont(fontstyle);
        p.setPen(bluePen);
        drawText(p, QPointF(0, 4.75), QString(frame->team_name_blue().c_str()),
                 true);  // Blue
        p.setPen(yellowPen);
        drawText(p, QPointF(0, 1.75),
                 QString(frame->team_name_yellow().c_str()),
                 true);  // Yellow
        p.setFont(savedFont);
    }

    // Block off half the field
    if (!frame->use_our_half()) {
        const float FX = Field_Dimensions::Current_Dimensions.FloorWidth() / 2;
        const float FY1 = -Field_Dimensions::Current_Dimensions.Border();
        const float FY2 = Field_Dimensions::Current_Dimensions.Length() / 2;
        p.fillRect(QRectF(QPointF(-FX, FY1), QPointF(FX, FY2)),
                   QColor(0, 0, 0, 128));
    }
    if (!frame->use_opponent_half()) {
        const float FX = Field_Dimensions::Current_Dimensions.FloorWidth() / 2;
        const float FY1 = Field_Dimensions::Current_Dimensions.Length() / 2;
        const float FY2 = Field_Dimensions::Current_Dimensions.Length() +
                          Field_Dimensions::Current_Dimensions.Border();
        p.fillRect(QRectF(QPointF(-FX, FY1), QPointF(FX, FY2)),
                   QColor(0, 0, 0, 128));
    }

    if (showCoords) {
        drawCoords(p);
    }

    // History
    p.setBrush(Qt::NoBrush);
    QPainterPath ballTrail;
    for (unsigned int i = 0; i < 200 && i < _history->size(); ++i) {
        const LogFrame* oldFrame = _history->at(i).get();
        if (oldFrame && oldFrame->has_ball()) {
            QPointF pos = qpointf(oldFrame->ball().pos());

            if (i == 0)
                ballTrail.moveTo(pos);
            else
                ballTrail.lineTo(pos);
        }
    }
    QPen ballTrailPen(ballColor, 0.03);
    ballTrailPen.setCapStyle(Qt::RoundCap);
    p.setPen(ballTrailPen);
    p.drawPath(ballTrail);

    // Debug lines
    for (const DebugPath& path : frame->debug_paths()) {
        if (path.layer() < 0 || layerVisible(path.layer())) {
            tempPen.setColor(qcolor(path.color()));
            p.setPen(tempPen);
            std::vector<QPointF> pts;
            for (int i = 0; i < path.points_size(); ++i) {
                pts.push_back(qpointf(path.points(i)));
            }
            p.drawPolyline(pts.data(), pts.size());
        }
    }

    for (const DebugRobotPath& path : frame->debug_robot_paths()) {
        if (path.layer() < 0 || layerVisible(path.layer())) {
            for (int i = 0; i < path.points_size() - 1; ++i) {
                const DebugRobotPath::DebugRobotPathPoint& from =
                    path.points(i);
                const DebugRobotPath::DebugRobotPathPoint& to =
                    path.points(i + 1);

                Geometry2d::Point avgVel =
                    (Geometry2d::Point(path.points(i).vel()) +
                     Geometry2d::Point(path.points(i + 1).vel())) /
                    2;
                float pcntMaxSpd =
                    avgVel.mag() / MotionConstraints::defaultMaxSpeed();
                QColor mixedColor(std::min((int)(255 * pcntMaxSpd), 255), 0,
                                  std::min((int)(255 * (1 - pcntMaxSpd)), 255));
                QPen pen(mixedColor);
                pen.setCapStyle(Qt::RoundCap);
                pen.setWidthF(0.03);
                p.setPen(pen);

                const Geometry2d::Point fromPos = Geometry2d::Point(from.pos());
                const Geometry2d::Point toPos = Geometry2d::Point(to.pos());
                p.drawLine(fromPos.toQPointF(), toPos.toQPointF());
            }
        }
    }
//.........这里部分代码省略.........

bool Gameplay::Behaviors::GoalDefender::run()
{
	if (!assigned() || !allVisible())
	{
		return false;
	}

	Geometry2d::Point ballFuture = ball().pos + ball().vel*0.1;
	const float radius = .7;

	//goal line, for intersection detection
	Geometry2d::Segment goalLine(Geometry2d::Point(-Constants::Field::GoalWidth / 2.0f, 0),
								  Geometry2d::Point(Constants::Field::GoalWidth / 2.0f, 0));

	// Update the target window
	_winEval->exclude.clear();
	for (Robot *r :  _robots)
	{
		_winEval->exclude.push_back(r->pos());
	}

	_winEval->run(ballFuture, goalLine);

	Window* best = 0;
	float bestDist = 0;

	// finds the closest segment to the ball
	for (Window* window :  _winEval->windows)
	{
		Geometry2d::Segment seg(window->segment.center(), ball().pos);
		float newDist = seg.distTo(Behavior::robot()->pos());

		if (seg.length() > Constants::Robot::Radius && (!best || newDist < bestDist))
		{
			best = window;
			bestDist = newDist;
		}
	}

	if (!best)
	{
	    //FIXME - What if there are no windows?
	    return true;
	}

	Geometry2d::Circle arc(Geometry2d::Point(), radius);
	Geometry2d::Line ballTravel(ball().pos, ballFuture);
	Geometry2d::Point dest[2];
	bool ballTravelIntersects = ballTravel.intersects(arc, &dest[0], &dest[1]);
	Geometry2d::Point blockPoint = (dest[0].y > 0 ? dest[0] : dest[1]);
	bool movingTowardsGoal = ballFuture.mag() < ball().pos.mag();

	if(ballTravelIntersects && blockPoint.y > 0 && movingTowardsGoal)
	{
		// If ball is traveling towards the goal, block it.
		state()->drawText("YY", blockPoint, Qt::magenta);
	}
	else
	{	// Stand in the largest open window
		Geometry2d::Line bestWindowLine(best->segment.center(), Geometry2d::Point(0,0));
		Geometry2d::Point blockPoint = (best->segment.center().normalized()) * radius;
		state()->drawText("XX", blockPoint, Qt::white);
	}



	/*
	_winEval->exclude.push_back(Behavior::robot()->pos());

	//exclude robots that arn't the defender
	//_winEval->run(ball().pos, goalLine);

	for (Defender *f :  otherDefenders)
	{
		if (f->robot())
		{
			_winEval->exclude.push_back(f->robot()->pos());
		}
	}

	_winEval->run(ballFuture, goalLine);

	Window* best = 0;

	Behavior* goalie = _gameplay->goalie();

	bool needTask = false;

	//pick biggest window on appropriate side
	if (goalie && goalie->robot())
	{
		for (Window* window :  _winEval->windows)
		{
			if (_side == Left)
			{
				if (!best || window->segment.center().x < goalie->robot()->pos().x)
				{
					best = window;
				}
			}
//.........这里部分代码省略.........