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

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 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 Gameplay::Plays::DemoAttack::run()
{
	Geometry2d::Point ballPos = ball().pos;

	set<OurRobot *> available = _gameplay->playRobots();
	assignNearest(_kicker.robot, available, ballPos);
	// if we have kicked, we want to reset
	if (_kicker.done() &&  ball().valid &&
			(!ballPos.nearPoint(Geometry2d::Point(0, Field_Length), Field_ArcRadius)))
	{
		_kicker.restart();
	}

	// set flags from parameters
	if (_calculateChipPower->value())
		_kicker.calculateChipPower(ball().pos.distTo(_kicker.target().center()));

	_kicker.use_chipper = *_useChip;
	_kicker.minChipRange = *_minChipRange;
	_kicker.maxChipRange = *_maxChipRange;
	_kicker.dribbler_speed = *_dribblerSpeed;
	_kicker.kick_power = *_kickPower;
	_kicker.use_line_kick = *_useLineKick;

	_kicker.run();

	return true;
}

bool Gameplay::Plays::DemoBump::run()
{
	Geometry2d::Point ballPos = ball().pos;

	set<OurRobot *> available = _gameplay->playRobots();
	assignNearest(_bump.robot, available, ballPos);

	// if we have kicked, we want to reset
	if (_bump.done() &&  ball().valid &&
			(!ballPos.nearPoint(Geometry2d::Point(0, Field_Length), Field_ArcRadius)))
	{
		_bump.restart();
	}

	_bump.run();

	return true;
}

bool Gameplay::Plays::DemoTouchKick::run()
{
	set<OurRobot *> available = _gameplay->playRobots();
	assignNearest(_kicker.robot, available, Geometry2d::Point());
	assignNearest(_passer.robot, available, Geometry2d::Point());

	Geometry2d::Point ballPos = ball().pos;

	_passer.setTarget(_kicker.robot->kickerBar());
	_passer.use_line_kick = true;

	// if we have kicked, we want to reset
	if (_kicker.done() &&  ball().valid && 
			(!ballPos.nearPoint(Geometry2d::Point(0, Field_Length), Field_ArcRadius)))
	{
		_kicker.restart();
		_passer.restart();
	}
	
	if (_passer.done() && ball().valid)
	{

	}

	if (_kicker.robot->pos.distTo(_passTarget) < 0.5)
	{
		_kicker.run();
	}
	else
	{
		_kicker.robot->move(_passTarget);
	}

	_kicker.use_chipper = *_use_chipper;
	_kicker.kick_power = *_kick_power;

	_passer.run();

	return true;
}

bool Gameplay::Plays::DemoYank::run()
{
	Geometry2d::Point ballPos = ball().pos;

	set<OurRobot *> available = _gameplay->playRobots();
	assignNearest(_yank.robot, available, ballPos);

	// if we have kicked, we want to reset
	if (_yank.done() &&  ball().valid &&
			(!ballPos.nearPoint(Geometry2d::Point(0, Field_Length), Field_ArcRadius)))
	{
		_yank.restart();
	}

	// set flags from parameters
	_yank.dribble_speed = *_dribblerSpeed;
	_yank.enable_bump = *_enableBump;

	_yank.run();

	return true;
}

Geometry2d::Point OurRobot::findGoalOnPath(const Geometry2d::Point& pose,
		const Planning::Path& path,	const ObstacleGroup& obstacles) {
		const bool blend_verbose = false;

		// empty path case - leave robot stationary
		if (path.empty())
			return pose;

		// find closest point on path to pose
		float max = path.points[0].distTo(pose);
		unsigned int ip = 0;
		for (unsigned i=0; i<path.points.size(); i++) {
			if (path.points[i].distTo(pose) < max) {
				max = path.points[i].distTo(pose);
				ip = i;
			}
		}

		if (blend_verbose) addText(QString("cur pt %1=(%2,%3)").arg(ip).arg(path.points[ip].x).arg(path.points[ip].y));

		// go to nearest point if only point or closest point is the goal
		if (path.size() == 1) {
			if (blend_verbose) addText(QString("blend:simple_path"));
			return path.points[0];
		}

		// can't mix, just go to endpoint
		if (path.size() == 2) {
			if (blend_verbose) addText(QString("blend:size2"));
			return path.points[1];
		}

		// FIXME: does not blend the last segment
		// All other cases: proportionally blend the next two points together for a smoother
		// path, so long as it is still viable

		if (blend_verbose) addText(QString("blend:segments=%1").arg(path.points.size()-1));

		// pull out relevant points
		Point p0 = pos;
		Point p1;
		Point p2;

		if (path.size() > ip+2) {
			p1 = path.points[ip+1];
			p2 = path.points[ip+2];
		} else if (path.size() > ip+1) {
			p1 = path.points[ip];
			p2 = path.points[ip+1];
		} else {
			p1 = path.points[ip-1];
			p2 = path.points[ip];
		}

		Geometry2d::Segment target_seg(p1, p2);

		if (blend_verbose) addText(QString("pos=(%1,%2)").arg(pos.x,5).arg(pos.y,5));
		if (blend_verbose) addText(QString("path[0]=(%1,%2)").arg(path.points[0].x).arg(path.points[0].y));
		if (blend_verbose) addText(QString("p1=(%1,%2)").arg(p1.x,5).arg(p1.y,5));
		if (blend_verbose) addText(QString("p2=(%1,%2)").arg(p2.x,5).arg(p2.y,5));

		// final endpoint handling
		if (target_seg.nearPointPerp(p0, 0.02) && p0.nearPoint(p2, 0.03)) {
			if (2 == path.size()-1) {
				if (blend_verbose) addText(QString("blend:at_end"));
				return p2;
			} else {
				// reset this segment to next one
				if (blend_verbose) addText(QString("blend:reset_segment"));
				Point temp(p1);
				p1 = p2;
				p2 = temp;
				target_seg = Geometry2d::Segment(p1, p2);
			}
		}

		float dist1 = p0.distTo(p1), dist2 = p1.distTo(p2);
		if (blend_verbose) addText(QString("blend:d1=%1,d2=%2").arg(dist1).arg(dist2));

		// endpoint handling
		if (dist1 < 0.02) {
			if (blend_verbose) addText(QString("blend:dist1small=%1").arg(dist1));
			return p2; /// just go to next point
		}

		// short segment handling
		if (p1.distTo(p2) < 0.05) {
			if (blend_verbose) addText(QString("blend:dist2small=%1").arg(dist2));
			return p2; /// just go to next point
		}

		// close to segment - go to end of segment
		if (target_seg.nearPoint(p0, 0.03)) {
			if (blend_verbose) addText(QString("blend:closeToSegment"));
			return p2;
		}

		// mix the next point between the first and second point
		// if we are far away from p1, want scale to be closer to p1
		// if we are close to p1, want scale to be closer to p2
//.........这里部分代码省略.........