C++ Vector2::GetAngleTo方法代码示例

bool Eagle::DoesRobotHaveBall(RobotState robotState, Vector2 ballPos) 
{
	Vector2 robotPos = robotState.Position();

    float angleThresh;
	float distanceThresh;

    if (robotState.HasBall())
    {
        angleThresh = M_PI_2;
        distanceThresh = 70.0f;
    }
    else
    {
        angleThresh = M_PI_4;
        distanceThresh = 45.0f;
    }
	
	float angleToBall = fmod(robotPos.GetAngleTo(&ballPos), (2*M_PI));
	float robotOrientation = fmod(robotState.Orientation(), (2*M_PI));

	float distanceToBall = robotPos.Distance(&ballPos);
	
    float orientationDiff = fmod(fabs(robotOrientation - angleToBall), (2*M_PI));
    bool withinOrientationThresh = (orientationDiff < angleThresh) || (2*M_PI - orientationDiff < angleThresh);
    bool withinProximityThresh = distanceToBall < distanceThresh;
	
	if(withinOrientationThresh && withinProximityThresh)
	{
		return true;
	} 
	
	return false;
}

bool Eagle::ShouldWeShoot(RobotState ourRobotState, RobotState enemyRobotState, Vector2 ballPos)
{
// In simplistic terms, we should shoot if we're close enough to the goal,
	// we've got the ball, and there is clear line-of-sight to goal
    Vector2 goalPosition;

    if (m_pitchSide == eLeftSide)
    {
        goalPosition = GoalCentrePosition(eRightSide);
    }
    else
    {
        goalPosition = GoalCentrePosition(eLeftSide);
    }

    Vector2 ourRobotPos = ourRobotState.Position();

    const float angleThresh = M_PI_4;

	float distToGoal = ourRobotState.Position().Distance(&goalPosition);

	// Ian reckons this should be 1/4 of the pitch, but I'm living dangerously and have gone for 1/3.
	float distanceThreshold = KICKING_THRESHOLD * m_pitchSizeX;

	// Commenting this out now - we need to account for the ball being invisible.
	//bool weHaveBall = DoWeHaveBall(ourRobotState, ballPos);
	bool closeToGoal = distToGoal < distanceThreshold;
	
	// Check that the enemy robot isn't obstructing the ball.
    bool isGoalClear = false;

    float angleToGoal = fmod(ourRobotPos.GetAngleTo(&goalPosition), 2*M_PI);
    float ourOrientation = fmod(ourRobotState.Orientation(), 2*M_PI);

    float angleDifference = fmod(fabs(angleToGoal - ourOrientation), 2*M_PI);
   //bool isFacingGoal = (angleDifference < angleThresh) || (angleDifference > (2*M_PI) - angleThresh);

	// Check if we're facing somewhere into the goal.
	// Extrapolate to the goal-line position we're facing.
    float extrapolatedYCoord = ourRobotPos.Y() + (ourRobotPos.Gradient(&ballPos) * (goalPosition.X() - ourRobotPos.X()));

	// The goal is in the middle of the pitch, constituting half its length.
	float goalTop = 0.75 * m_pitchSizeY;
	float goalBottom = 0.25 * m_pitchSizeY;

	bool isFacingGoal = (extrapolatedYCoord >= goalBottom) && (extrapolatedYCoord <= goalTop);
	
	// The position we're aiming for.
	Vector2 goalTarget(m_pitchSizeX, extrapolatedYCoord);

    if (!m_intersection.LineCircleIntersection(ourRobotPos, goalTarget, enemyRobotState.Position(), ROBOT_RADIUS))
	{
		isGoalClear = true;
	}

    if (closeToGoal && isGoalClear && isFacingGoal)
	{
		return true;
	}
	
	return false;
}

/*!
* Identify the target state that we wish the robot to be in. This will be the target which the A* algorithm plots towards.
*/
RobotState Eagle::IdentifyTarget(RobotState &ourRobotState, RobotState &enemyRobotState, Vector2 ballPos, bool doWeHaveBall, bool &isMovingToBall)
{
	RobotState targetState;

	Vector2 ourRobotPos = ourRobotState.Position();
	Vector2 enemyRobotPos = enemyRobotState.Position();

    Vector2 ourGoalCentre = GoalCentrePosition(m_pitchSide);
    Vector2 enemyGoalCentre;

    if (m_pitchSide == eLeftSide)
    {
        enemyGoalCentre = GoalCentrePosition(eRightSide);
    }
    else
    {
        enemyGoalCentre = GoalCentrePosition(eLeftSide);
    }
    
    ballPos.Clamp(Vector2(0,0), Vector2(m_pitchSizeX-1, m_pitchSizeY-1));
	isMovingToBall = false;

	// doWeHaveBall is a value which comes from the robot's rotational sensors.
    //ourRobotState.SetHasBall(doWeHaveBall);
	ourRobotState.SetHasBall(DoesRobotHaveBall(ourRobotState, ballPos));
    enemyRobotState.SetHasBall(DoesRobotHaveBall(enemyRobotState, ballPos));

    if (m_state == ePenaltyAttack)
	{
		m_isKickingPosSet = false;

         // When taking a penalty, we want to find a free position to kick to.
        // Position should stay the same - we only want to re-orientate.
        targetState.SetPosition(ourRobotPos);

        // We'll do this by checking for intersections between us and three positions on the goal line.
        Vector2 targetPositions[3];

        targetPositions[0] = enemyGoalCentre;
        targetPositions[1] = enemyGoalCentre - Vector2(0,50);
        targetPositions[2] = enemyGoalCentre + Vector2(0,50);

        int arrayLength = sizeof(targetPositions)/sizeof(Vector2);

        Vector2 optimalShootingTarget;
        float bestDistanceFromEnemy = 0;

        // Iterate through the positions, finding the best one, based on if it's unblocked and how far it is from the enemy robot.
        for (int i=0; i < arrayLength; i++)
        {
            // Check if the target is unblocked.
            bool isBlocked = m_intersection.LineCircleIntersection(ourRobotPos, targetPositions[i], enemyRobotPos, ROBOT_RADIUS);

            if (isBlocked)
            {
                continue;
            }

            float distanceSqdToEnemy = enemyRobotPos.DistanceSquared(&targetPositions[i]);

            // Check if this beats our previous best distance.
            if (distanceSqdToEnemy > bestDistanceFromEnemy)
            {
                bestDistanceFromEnemy = distanceSqdToEnemy;
                optimalShootingTarget = targetPositions[i];
            }

            // Check that we do actually have a target set.
            if (optimalShootingTarget.IsSet())
            {
                float angleToTarget = ourRobotPos.GetAngleTo(&optimalShootingTarget);

                targetState.SetOrientation(angleToTarget);
            }
            else
            {
                targetState.SetOrientation(ourRobotState.Orientation());
            }
        }


	}
    else if (m_state == ePenaltyDefend)
	{
		m_isKickingPosSet = false;
	
		// When defending, we're permitted to move up and down the goalline.
		// Orientation should stay the same.
		targetState.SetOrientation(ourRobotState.Orientation());

		// X-axis position should be the same, y-axis should be a position extrapolated in the direction of the enemy robot.
		//float extrapolationGradient = tan(enemyRobotState.Orientation());
		// I'm experimenting with extrapolating on a line between the robot and ball instead.
		float extrapolationGradient = enemyRobotPos.Gradient(&ballPos);

		int extrapolatedY = enemyRobotPos.Y() + ((ourRobotPos.X() - enemyRobotPos.X()) * extrapolationGradient); 

//.........这里部分代码省略.........