|
public static Vec2DNormalize ( |
||
| v | ||
| return | ||
/// <summary>
/// Given a target, this behavior returns a steering force which will
/// allign the agent with the target and move the agent in the desired
/// direction
/// </summary>
/// <returns></returns>
protected Vector2D calculateSeekVector(Vector2D target)
{
Vector2D desiredVelocity = Vector2D.Vec2DNormalize(target - _player.Position)
* _player.MaxSpeed;
return(desiredVelocity - _player.Velocity);
}
private void calculateNormal()
{
Vector2D temp = Vector2D.Vec2DNormalize(_vectorTo - _vectorFrom);
_vectorNormal.X = -temp.Y;
_vectorNormal.Y = temp.X;
}
public override void Execute(GoalKeeper keeper)
{
PlayerBase receiver = null;
Vector2D BallTarget;
//test if there are players further forward on the field we might
//be able to pass to. If so, make a pass.
if (keeper.Team.FindPass(keeper,
out receiver,
out BallTarget,
ParameterManager.Instance.MaxPassingForce,
ParameterManager.Instance.GoalkeeperMinPassDist))
{
//make the pass
keeper.Ball.Kick(Vector2D.Vec2DNormalize(BallTarget - keeper.Ball.Position),
ParameterManager.Instance.MaxPassingForce);
//goalkeeper no longer has ball
keeper.Pitch.GoalKeeperHasBall = false;
//let the receiving player know the ball's comin' at him
MessageDispatcher.Instance.DispatchMsg(new TimeSpan(0), keeper.ObjectId,
receiver.ObjectId,
(int)SoccerGameMessages.ReceiveBall,
BallTarget);
//go back to tending the goal
keeper.StateMachine.ChangeState(TendGoalState.Instance);
return;
}
keeper.Velocity = new Vector2D(0, 0);
}
/// <summary>
/// This calculates a force repelling from the other neighbors
/// </summary>
/// <returns></returns>
protected Vector2D calculateSeparationVector()
{
//iterate through all the neighbors and calculate the vector from the
Vector2D steeringForce = new Vector2D();
List <PlayerBase> allPlayers = AutoList <PlayerBase> .GetAllMembers();
for (int playerIndex = 0; playerIndex < allPlayers.Count; playerIndex++)
{
//make sure this agent isn't included in the calculations and that
//the agent is close enough
if (allPlayers[playerIndex] != _player && allPlayers[playerIndex].SteeringBehaviors.Tagged)
{
Vector2D toAgent = _player.Position - allPlayers[playerIndex].Position;
//scale the force inversely proportional to the agents distance
//from its neighbor.
if (Math.Abs(toAgent.Length) > double.Epsilon)
{
steeringForce += Vector2D.Vec2DNormalize(toAgent) / toAgent.Length;
}
}
}
return(steeringForce);
}
public bool RotateHeadingToFacePosition(Vector2D target)
{
Vector2D delta = target - Position;
Vector2D toTarget = Vector2D.Vec2DNormalize(delta);
double dot = _heading.GetDotProduct(toTarget);
//some compilers lose acurracy so the value is clamped to ensure it
//remains valid for the acos
if (dot < -1)
{
dot = -1;
}
else if (dot > 1)
{
dot = 1;
}
//first determine the angle between the heading vector and the target
double angle = Math.Acos(dot);
//return true if the player is facing the target
if (angle < .00001)
{
return(true);
}
//clamp the amount to turn to the max turn rate
if (angle > _maxTurnRate)
{
angle = _maxTurnRate;
}
//The next few lines use a rotation matrix to rotate the player's heading
//vector accordingly
Matrix2D rotationMatrix = new Matrix2D();
//notice how the direction of rotation has to be determined when creating
//the rotation matrix
rotationMatrix.Rotate(angle * _heading.Sign(toTarget));
rotationMatrix.TransformVector2Ds(_heading);
rotationMatrix.TransformVector2Ds(_velocity);
//finally recreate m_vSide
_side = _heading.Perp;
return(false);
}
/// <summary>
/// given a line segment AB and a circle position and radius, this function
/// determines if there is an intersection and stores the position of the
/// closest intersection in the reference IntersectionPoint
///
/// returns false if no intersection point is found
/// </summary>
/// <param name="A"></param>
/// <param name="B"></param>
/// <param name="pos"></param>
/// <param name="radius"></param>
/// <param name="IntersectionPoint"></param>
/// <returns></returns>
public static bool GetLineSegmentCircleClosestIntersectionPoint(Vector2D A,
Vector2D B,
Vector2D pos,
double radius,
ref Vector2D IntersectionPoint)
{
Vector2D toBNorm = Vector2D.Vec2DNormalize(B - A);
//move the circle into the local space defined by the vector B-A with origin
//at A
Vector2D LocalPos = Transformations.PointToLocalSpace(pos, toBNorm, toBNorm.Perp, A);
bool ipFound = false;
//if the local position + the radius is negative then the circle lays behind
//point A so there is no intersection possible. If the local x pos minus the
//radius is greater than length A-B then the circle cannot intersect the
//line segment
if ((LocalPos.X + radius >= 0) &&
((LocalPos.X - radius) * (LocalPos.X - radius) <= Vector2D.Vec2DDistanceSq(B, A)))
{
//if the distance from the x axis to the object's position is less
//than its radius then there is a potential intersection.
if (Math.Sqrt(LocalPos.Y) < radius)
{
//now to do a line/circle intersection test. The center of the
//circle is represented by A, B. The intersection points are
//given by the formulae x = A +/-sqrt(r^2-B^2), y=0. We only
//need to look at the smallest positive value of x.
double a = LocalPos.X;
double b = LocalPos.Y;
double ip = a - Math.Sqrt(radius * radius - b * b);
if (ip <= 0)
{
ip = a + Math.Sqrt(radius * radius - b * b);
}
ipFound = true;
IntersectionPoint = A + toBNorm * ip;
}
}
return(ipFound);
}
/// <summary>
/// Given a time this method returns the ball position at that time in the
/// future
/// </summary>
/// <param name="time"></param>
/// <returns></returns>
public Vector2D CalculateFuturePosition(double time)
{
//using the equation s = ut + 1/2at^2, where s = distance, a = friction
//u=start velocity
//calculate the ut term, which is a vector
Vector2D ut = Velocity * time;
//calculate the 1/2at^2 term, which is scalar
double half_a_t_squared = 0.5 * ParameterManager.Instance.Friction * time * time;
//turn the scalar quantity into a vector by multiplying the value with
//the normalized velocity vector (because that gives the direction)
Vector2D scalarToVector = half_a_t_squared * Vector2D.Vec2DNormalize(Velocity);
//the predicted position is the balls position plus these two terms
return(Position + ut + scalarToVector);
}
public override void Update()
{
//run the logic for the current state
_stateMachine.Update();
//calculate the combined force from each steering behavior
Vector2D SteeringForce = _steeringBehaviors.CalculateSteeringForce();
//Acceleration = Force/Mass
Vector2D Acceleration = SteeringForce / Mass;
//update velocity
Velocity += Acceleration;
//make sure player does not exceed maximum velocity
Velocity.Truncate(MaxSpeed);
//update the position
Position += Velocity;
//enforce a non-penetration constraint if desired
if (ParameterManager.Instance.NonPenetrationConstraint)
{
EntityManager.EnforceNonPenetrationContraint <PlayerBase>(this, AutoList <PlayerBase> .GetAllMembers());
}
//update the heading if the player has a non zero velocity
if (!Velocity.IsZero)
{
Heading = Vector2D.Vec2DNormalize(Velocity);
Side = Heading.Perp;
}
//look-at vector always points toward the ball
if (!Team.Pitch.GoalKeeperHasBall)
{
_lookAt = Vector2D.Vec2DNormalize(Ball.Position - Position);
}
}
/// <summary>
/// This function calculates how much of its max steering force the
/// vehicle has left to apply and then applies that amount of the
/// force to add.
/// </summary>
/// <param name="steeringForce"></param>
/// <param name="additionalForce"></param>
/// <returns></returns>
protected bool accumulateForce(ref Vector2D steeringForce, Vector2D additionalForce)
{
bool accumulated = false;
//first calculate how much steering force we have left to use
double magnitudeSoFar = steeringForce.Length;
double magnitudeRemaining = _player.MaxForce - magnitudeSoFar;
//return false if there is no more force left to use
if (magnitudeRemaining <= 0.0)
{
return(false);
}
//calculate the magnitude of the force we want to add
double magnitudeToAdd = additionalForce.Length;
//now calculate how much of the force we can really add
if (magnitudeToAdd > magnitudeRemaining)
{
magnitudeToAdd = magnitudeRemaining;
}
//add it to the steering force
Vector2D add = (Vector2D.Vec2DNormalize(additionalForce) * magnitudeToAdd);
steeringForce += add;
//steeringForce.X += add.X;
//steeringForce.Y += add.Y;
accumulated = true;
return(accumulated);
}
/// <summary>
/// updates the ball physics, tests for any collisions and adjusts
/// the ball's velocity accordingly
///
/// </summary>
public override void Update()
{
//keep a record of the old position so the goal::scored method
//can utilize it for goal testing
_oldPosition = Position;
//Test for collisions
TestCollisionWithWalls(_pitchBoundary);
//Simulate Prm.Friction. Make sure the speed is positive
//first though
if (Velocity.LengthSquared > ParameterManager.Instance.Friction * ParameterManager.Instance.Friction)
{
Velocity += Vector2D.Vec2DNormalize(Velocity) * ParameterManager.Instance.Friction;
Position += Velocity;
//update heading
Heading = Vector2D.Vec2DNormalize(Velocity);
}
}
public override void Execute(FieldPlayer player)
{
Random random = new Random();
//calculate the dot product of the vector pointing to the ball
//and the player's heading
Vector2D ToBall = player.Ball.Position - player.Position;
double dot = player.Heading.GetDotProduct(Vector2D.Vec2DNormalize(ToBall));
//cannot kick the ball if the goalkeeper is in possession or if it is
//behind the player or if there is already an assigned receiver. So just
//continue chasing the ball
if (player.Team.ReceivingPlayer != null ||
player.Pitch.GoalKeeperHasBall ||
(dot < 0))
{
System.Diagnostics.Debug.WriteLine("Goaly has ball / ball behind player");
player.StateMachine.ChangeState(ChaseBallState.Instance);
return;
}
/* Attempt a shot at the goal */
//if a shot is possible, this vector will hold the position along the
//opponent's goal line the player should aim for.
Vector2D BallTarget = new Vector2D();
//the dot product is used to adjust the shooting force. The more
//directly the ball is ahead, the more forceful the kick
double power = ParameterManager.Instance.MaxShootingForce * dot;
double distance = player.Position.Distance(player.Team.OpponentsGoal.GoalLineCenter);
//if it is determined that the player could score a goal from this position
//OR if he should just kick the ball anyway, the player will attempt
//to make the shot
if (player.Team.CanShoot(player.Ball.Position,
power,
ref BallTarget) ||
(random.NextDouble() < ParameterManager.Instance.ChancePlayerAttemptsPotShot) ||
distance < player.Pitch.PlayingArea.Width / 8)
{
System.Diagnostics.Debug.WriteLine(string.Format("Player {0} attempts a shot at ({1}, {2})", player.ObjectId, BallTarget.X, BallTarget.Y));
//add some noise to the kick. We don't want players who are
//too accurate! The amount of noise can be adjusted by altering
//Prm.PlayerKickingAccuracy
BallTarget = SoccerBall.AddNoiseToKick(player.Ball.Position, BallTarget);
//this is the direction the ball will be kicked in
Vector2D KickDirection = BallTarget - player.Ball.Position;
player.Ball.Kick(KickDirection, power);
//change state
player.StateMachine.ChangeState(WaitState.Instance);
player.FindSupport();
return;
}
/* Attempt a pass to a player */
//if a receiver is found this will point to it
PlayerBase receiver = null;
power = ParameterManager.Instance.MaxPassingForce * dot;
//test if there are any potential candidates available to receive a pass
if (player.IsThreatened() &&
player.Team.FindPass(player,
out receiver,
out BallTarget,
power,
ParameterManager.Instance.MinPassDist))
{
//add some noise to the kick
BallTarget = SoccerBall.AddNoiseToKick(player.Ball.Position, BallTarget);
Vector2D KickDirection = BallTarget - player.Ball.Position;
player.Ball.Kick(KickDirection, power);
System.Diagnostics.Debug.WriteLine(string.Format("Player {0} passes the ball with force {1} to player {2} Target is ({3},{4})", player.ObjectId, power, receiver.ObjectId, BallTarget.X, BallTarget.Y));
//let the receiver know a pass is coming
MessageDispatcher.Instance.DispatchMsg(
new TimeSpan(0),
player.ObjectId,
receiver.ObjectId,
(int)SoccerGameMessages.ReceiveBall,
BallTarget);
//the player should wait at his current position unless instruced
//otherwise
player.StateMachine.ChangeState(WaitState.Instance);
player.FindSupport();
return;
}
//cannot shoot or pass, so dribble the ball upfield
else
{
player.FindSupport();
player.StateMachine.ChangeState(DribbleState.Instance);
}
}
/// <summary>
/// Tests to see if the ball has collided with a ball and reflects
/// the ball's velocity accordingly
/// </summary>
/// <param name="walls"></param>
public void TestCollisionWithWalls(List <Wall2D> walls)
{
//test ball against each wall, find out which is closest
int closestIndex = -1;
Vector2D normalVector = Vector2D.Vec2DNormalize(Velocity);
Vector2D intersectionPoint = new Vector2D(), collisionPoint = new Vector2D();
double distToIntersection = double.MaxValue;
////iterate through each wall and calculate if the ball intersects.
////If it does then store the index into the closest intersecting wall
for (int wallIndex = 0; wallIndex < walls.Count; wallIndex++)
{
//assuming a collision if the ball continued on its current heading
//calculate the point on the ball that would hit the wall. This is
//simply the wall's normal(inversed) multiplied by the ball's radius
//and added to the balls center (its position)
Vector2D thisCollisionPoint = Position - (walls[wallIndex].VectorNormal * BoundingRadius);
// //calculate exactly where the collision point will hit the plane
if (Geometry.WhereIsPoint(thisCollisionPoint,
walls[wallIndex].VectorFrom,
walls[wallIndex].VectorNormal) == Geometry.PlaneLocation.Behind)
{
double distToWall = Geometry.DistanceToRayPlaneIntersection(thisCollisionPoint, walls[wallIndex].VectorNormal, walls[wallIndex].VectorFrom, walls[wallIndex].VectorNormal);
intersectionPoint = thisCollisionPoint + (distToWall * walls[wallIndex].VectorNormal);
}
else
{
double distToWall = Geometry.DistanceToRayPlaneIntersection(thisCollisionPoint,
normalVector,
walls[wallIndex].VectorFrom,
walls[wallIndex].VectorNormal);
intersectionPoint = thisCollisionPoint + (distToWall * normalVector);
}
//check to make sure the intersection point is actually on the line
//segment
bool onLineSegment = false;
if (Geometry.LineIntersection2D(walls[wallIndex].VectorFrom,
walls[wallIndex].VectorTo,
thisCollisionPoint - walls[wallIndex].VectorNormal * 20.0,
thisCollisionPoint + walls[wallIndex].VectorNormal * 20.0))
{
onLineSegment = true;
}
//Note, there is no test for collision with the end of a line segment
//now check to see if the collision point is within range of the
//velocity vector. [work in distance squared to avoid sqrt] and if it
//is the closest hit found so far.
//If it is that means the ball will collide with the wall sometime
//between this time step and the next one.
double distSq = Vector2D.Vec2DDistanceSq(thisCollisionPoint, intersectionPoint);
if ((distSq <= Velocity.LengthSquared) && (distSq < distToIntersection) && onLineSegment)
{
distToIntersection = distSq;
closestIndex = wallIndex;
collisionPoint = intersectionPoint;
}
} // next wall
//to prevent having to calculate the exact time of collision we
//can just check if the velocity is opposite to the wall normal
//before reflecting it. This prevents the case where there is overshoot
//and the ball gets reflected back over the line before it has completely
//reentered the playing area.
if ((closestIndex >= 0) && normalVector.GetDotProduct(walls[closestIndex].VectorNormal) < 0)
{
Velocity.Reflect(walls[closestIndex].VectorNormal);
}
}
/// <summary>
/// test if a pass from 'from' to 'to' can be intercepted by an opposing player
///
/// </summary>
/// <param name="from"></param>
/// <param name="target"></param>
/// <param name="receiver"></param>
/// <param name="opp"></param>
/// <param name="PassingForce"></param>
/// <returns></returns>
public bool IsPassSafeFromOpponent(Vector2D from,
Vector2D target,
PlayerBase receiver,
PlayerBase opp,
double PassingForce)
{
//move the opponent into local space.
Vector2D ToTarget = target - from;
Vector2D ToTargetNormalized = Vector2D.Vec2DNormalize(ToTarget);
Vector2D LocalPosOpp = Transformations.PointToLocalSpace(opp.Position,
ToTargetNormalized,
ToTargetNormalized.Perp,
from);
//if opponent is behind the kicker then pass is considered okay(this is
//based on the assumption that the ball is going to be kicked with a
//velocity greater than the opponent's max velocity)
if (LocalPosOpp.X < 0)
{
return(true);
}
//if the opponent is further away than the target we need to consider if
//the opponent can reach the position before the receiver.
if (Vector2D.Vec2DDistanceSq(from, target) < Vector2D.Vec2DDistanceSq(opp.Position, from))
{
if (receiver != null)
{
if (Vector2D.Vec2DDistanceSq(target, opp.Position) >
Vector2D.Vec2DDistanceSq(target, receiver.Position))
{
return(true);
}
else
{
return(false);
}
}
else
{
return(true);
}
}
//calculate how long it takes the ball to cover the distance to the
//position orthogonal to the opponents position
double TimeForBall =
Pitch.Ball.CalucateTimeToCoverDistance(new Vector2D(0, 0),
new Vector2D(LocalPosOpp.X, 0),
PassingForce);
//now calculate how far the opponent can run in this time
double reach = opp.MaxSpeed * TimeForBall +
Pitch.Ball.BoundingRadius +
opp.BoundingRadius;
//if the distance to the opponent's y position is less than his running
//range plus the radius of the ball and the opponents radius then the
//ball can be intercepted
if (Math.Abs(LocalPosOpp.Y) < reach)
{
return(false);
}
return(true);
}
/// <summary>
/// Given an opponent and an object position this method returns a
/// force that attempts to position the agent between them
/// </summary>
/// <param name="ball"></param>
/// <param name="target"></param>
/// <param name="distFromTarget"></param>
/// <returns></returns>
protected Vector2D calculateInterposeVector(SoccerBall ball,
Vector2D target,
double distFromTarget)
{
return(calculateArriveVector(target + Vector2D.Vec2DNormalize(ball.Position - target) * distFromTarget, DecelerationState.Normal));
}
public void TrackTarget()
{
Heading = Vector2D.Vec2DNormalize(SteeringBehaviors.Target - Position);
}