C++ geometry2d::Point类代码示例

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

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

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

void SimFieldView::drawTeamSpace(QPainter& p) {
    FieldView::drawTeamSpace(p);

    // Simulator drag-to-shoot
    std::shared_ptr<LogFrame> frame = currentFrame();
    if (_dragMode == DRAG_SHOOT && frame) {
        p.setPen(QPen(Qt::white, 0.1f));
        Geometry2d::Point ball = frame->ball().pos();
        p.drawLine(ball.toQPointF(), _dragTo.toQPointF());

        if (ball != _dragTo) {
            p.setPen(QPen(Qt::gray, 0.1f));

            _shot = (ball - _dragTo) * ShootScale;
            float speed = _shot.mag();
            Geometry2d::Point shotExtension = ball + _shot / speed * 8;

            p.drawLine(ball.toQPointF(), shotExtension.toQPointF());

            p.setPen(Qt::black);
            drawText(p, _dragTo.toQPointF(),
                     QString("%1 m/s").arg(speed, 0, 'f', 1));
        }
    }
}

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

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

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

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::Behaviors::Idle::run()
{
	if (!ball().valid || robots.empty())
	{
		// If we can't find the ball, leave the robots where they are
		return false;
	}
	
	// Arrange all robots close together around the ball
	
	// We really should exclude parts of the circle that aren't accessible due to field or rules,
	// but I think it doesn't matter when there are only four robots.
	
	// Radius of the circle that will contain the robot centers
	float radius = Field_CenterRadius + Robot_Radius + .01;
	
	// Angle between robots, as seen from the ball
	float perRobot = (Robot_Diameter * 1.25) / radius * RadiansToDegrees;
	
	// Direction from the ball to the first robot.
	// Center the robots around the line from the ball to our goal
	Geometry2d::Point dir = (Geometry2d::Point() - ball().pos).normalized() * radius;
	dir.rotate(Geometry2d::Point(), -perRobot * (robots.size() - 1) / 2);
	
	state()->drawLine(ball().pos, Geometry2d::Point());
	
	for (OurRobot *r :  robots)
	{
		if (r->visible)
		{
			state()->drawLine(ball().pos, ball().pos + dir);
			
			r->move(ball().pos + dir);
			r->face(ball().pos);
			
			// Avoid the ball even on our restart
			r->avoidBallRadius(Field_CenterRadius);
			// Don't leave gaps
			r->avoidAllTeammates(true);
		}
		
		// Move to the next robot's position
		dir.rotate(Geometry2d::Point(), perRobot);
	}
	
	return false;
}

void Gameplay::WindowEvaluator::obstacleRobot(Geometry2d::Point pos)
{
	Geometry2d::Point n = (pos - _origin).normalized();
	Geometry2d::Point t = n.perpCCW();
	const float r = Robot_Radius + Ball_Radius;
	Geometry2d::Segment seg(pos - n * Robot_Radius + t * r,
							pos - n * Robot_Radius - t * r);
	
	if (debug)
	{
//		_state->debugLines.push_back(seg);
	}
	Geometry2d::Point intersection[2];
	float extent[2] = {0, _end};
	bool valid[2] = {false, false};
	
	for (int i = 0; i < 2; ++i)
	{
		Geometry2d::Line edge(_origin, seg.pt[i]);
		float d = edge.delta().magsq();
		if (edge.intersects(_target, &intersection[i]) && (intersection[i] - _origin).dot(edge.delta()) > d)
		{
			valid[i] = true;
			float t = (intersection[i] - _target.pt[0]).dot(_target.delta());
			
			if (t < 0)
			{
				extent[i] = 0;
			} else if (t > _end)
			{
				extent[i] = _end;
			} else {
				extent[i] = t;
			}
		}
	}
	
	if (!valid[0] || !valid[1])
	{
		// Obstacle has no effect
		return;
	}
	
	obstacleRange(extent[0], extent[1]);
}