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

float Planning::Path::length(const Geometry2d::Point &pt) const
{
	float dist = -1;
	float length = 0;
	if (points.empty())
	{
		return 0;
	}
	
	for (unsigned int i = 0; i < (points.size() - 1); ++i)
    {
		Geometry2d::Segment s(points[i], points[i+1]);
				
		//add the segment length
		length += s.length();
		
		const float d = s.distTo(pt);
		
		//if point closer to this segment
		if (dist < 0 || d < dist)
		{
			//closest point on segment
			Geometry2d::Point p = s.nearestPoint(pt);
			
			//new best distance
			dist = d;
			
			//reset running length count
			length = 0;
			length += p.distTo(s.pt[1]);
		}
	}
	
	return length;
}

// Returns the index of the point in this path nearest to pt.
int Planning::Path::nearestIndex(const Geometry2d::Point &pt) const
{
	if (points.size() == 0)
	{
		return -1;
	}
	
	int index = 0;
	float dist = pt.distTo(points[0]);
	
	for (unsigned int i=1 ; i<points.size(); ++i)
	{
		float d = pt.distTo(points[i]);
		if (d < dist)
		{
			dist = d;
			index = i;
		}
	}
	
	return index;
}

 Geometry2d::ShapeSet createRobotObstacles(const std::vector<ROBOT*>& robots,
                                           const RobotMask& mask,
                                           Geometry2d::Point currentPosition,
                                           float checkRadius) const {
     Geometry2d::ShapeSet result;
     for (size_t i = 0; i < mask.size(); ++i)
         if (mask[i] > 0 && robots[i] && robots[i]->visible) {
             if (currentPosition.distTo(robots[i]->pos) <= checkRadius) {
                 result.add(std::shared_ptr<Geometry2d::Shape>(
                     new Geometry2d::Circle(robots[i]->pos, mask[i])));
             }
         }
     return result;
 }

void SimFieldView::mousePressEvent(QMouseEvent* me) {
    Geometry2d::Point pos = _worldToTeam * _screenToWorld * me->pos();

    std::shared_ptr<LogFrame> frame = currentFrame();
    if (me->button() == Qt::LeftButton && frame) {
        _dragRobot = -1;
        for (const LogFrame::Robot& r : frame->self()) {
            if (pos.nearPoint(r.pos(), Robot_Radius)) {
                _dragRobot = r.shell();
                _dragRobotBlue = frame->blue_team();
                break;
            }
        }
        for (const LogFrame::Robot& r : frame->opp()) {
            if (pos.nearPoint(r.pos(), Robot_Radius)) {
                _dragRobot = r.shell();
                _dragRobotBlue = !frame->blue_team();
                break;
            }
        }

        if (_dragRobot < 0) {
            placeBall(me->pos());
        }

        _dragMode = DRAG_PLACE;
    } else if (me->button() == Qt::RightButton && frame) {
        if (frame->has_ball() &&
            pos.nearPoint(frame->ball().pos(), Ball_Radius)) {
            // Drag to shoot the ball
            _dragMode = DRAG_SHOOT;
            _dragTo = pos;
        } else {
            // Look for a robot selection
            int newID = -1;
            for (int i = 0; i < frame->self_size(); ++i) {
                if (pos.distTo(frame->self(i).pos()) < Robot_Radius) {
                    newID = frame->self(i).shell();
                    break;
                }
            }

            if (newID != frame->manual_id()) {
                robotSelected(newID);
            }
        }
    }
}

bool Circle::tangentPoints(const Geometry2d::Point &src, 
	Geometry2d::Point* p1, Geometry2d::Point* p2) const
{
	if (!p1 && !p2)
	{
		return false;
	}
	
	const float dist = src.distTo(center);
	
	if (dist < _r)
	{
		return false;
	}
	else if (dist == _r)
	{
		if (p1)
		{
			*p1 = src;
		}
		
		if (p2)
		{
			*p2 = src;
		}
	}
	else
	{
		//source is outside of circle
		const float theta = asin(_r/dist);
		const float degT = theta * 180.0f / M_PI;
		
		if (p1)
		{
			Point final = center;
			final.rotate(src, degT);
			*p1 = final;
		}
		
		if (p2)
		{
			Point final = center;
			final.rotate(src, -degT);
			*p2 = final;
		}

/**
 * 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());
//.........这里部分代码省略.........

 double distance(const Geometry2d::Point& from,
                 const Geometry2d::Point& to) const {
     return from.distTo(to);
 }