protected override Vector2 OnUpdateSteeringForce(float elapsedTime, Steerable movingEntity)
{
int count = 0;
Vector2 center = Vector2.Zero;
var boundingSphere = new BoundingSphere(new Vector3(movingEntity.Position, 0), Range);
Neighbors.FindAll(ref boundingSphere, Partners);
foreach (var partner in Partners)
{
if (partner != null && partner != movingEntity)
{
center += partner.Position;
if (++count >= SteeringHelper.MaxAffectingEntities)
{
break;
}
}
}
Partners.Clear();
if (count > 0)
{
center /= count;
movingEntity.Target = center;
return(SteeringHelper.Seek(elapsedTime, movingEntity));
}
return(Vector2.Zero);
}
protected override Vector2 OnUpdateSteeringForce(float elapsedTime, Steerable movingEntity)
{
Vector2 result = Vector2.Zero;
// Min X
if (movingEntity.Position.X < Bounds.X + Skin)
{
result.X = movingEntity.MaxSpeed * Evaluate((Bounds.X + Skin - movingEntity.Position.X) / Skin);
}
// Min Y
if (movingEntity.Position.Y < Bounds.Y + Skin)
{
result.Y = movingEntity.MaxSpeed * Evaluate((Bounds.Y + Skin - movingEntity.Position.Y) / Skin);
}
// Max X
if (movingEntity.Position.X > Bounds.X + Bounds.Width - Skin)
{
result.X = -movingEntity.MaxSpeed * Evaluate((movingEntity.Position.X - Bounds.X - Bounds.Width + Skin) / Skin);
}
// Max Y
if (movingEntity.Position.Y > Bounds.Y + Bounds.Height - Skin)
{
result.Y = -movingEntity.MaxSpeed * Evaluate((movingEntity.Position.Y - Bounds.Y - Bounds.Height + Skin) / Skin);
}
return(result);
}
protected override Vector2 OnUpdateSteeringForce(float elapsedTime, Steerable movingEntity)
{
//this behavior is dependent on the update rate, so this line must
//be included when using time independent framerate.
float JitterThisTimeSlice = Jitter * elapsedTime;
//first, add a small random vector to the target's position
wanderTarget += new Vector2((float)(Random.NextDouble() - Random.NextDouble()) * JitterThisTimeSlice,
(float)(Random.NextDouble() - Random.NextDouble()) * JitterThisTimeSlice);
//reproject this new vector back on to a unit circle
wanderTarget.Normalize();
//increase the length of the vector to the same as the radius
//of the wander circle
wanderTarget *= Radius;
//move the target into a position WanderDist in front of the agent
Vector2 target = wanderTarget + new Vector2(Distance, 0);
//project the target into world space
Vector2 Target = Math2D.LocalToWorld(target, movingEntity.Position, (float)Math.Atan2(
movingEntity.Forward.Y,
movingEntity.Forward.X));
//and steer towards it
return(Vector2.Normalize(Target - movingEntity.Position) * movingEntity.MaxForce);
}
protected override Vector2 OnUpdateSteeringForce(float elapsedTime, Steerable movingEntity)
{
if (MovesPerMinute < 0f)
{
MovesPerMinute = 0f;
}
// Check if we are outside our moving range
if (Vector2.Subtract(movingEntity.Position, location).LengthSquared() > PatrolRange * PatrolRange * 1.2F)
{
timer = (float)(Random.NextDouble() * 60 / (MovesPerMinute + 0.001f));
location = movingEntity.Position;
}
// Select a random location when the timer expires
timer -= (float)elapsedTime;
if (timer < 0)
{
Vector2 target = new Vector2();
float a = (float)(Random.NextDouble() * Math.PI * 2);
float r = (float)(Random.NextDouble());
target.X = (float)(Math.Cos(a) * PatrolRange * r);
target.Y = (float)(Math.Sin(a) * PatrolRange * r);
movingEntity.Target = target + location;
timer = (float)(Random.NextDouble() * 60 / (MovesPerMinute + 0.001f));
}
return(SteeringHelper.Arrive(elapsedTime, movingEntity));
}
/// <summary>
/// Calculates the steering force to arrive at the target.
/// </summary>
public static Vector2 Arrive(float elapsedTime, Steerable movingEntity)
{
if (!movingEntity.Target.HasValue)
{
return(Vector2.Zero);
}
Vector2 toTarget = movingEntity.Target.Value - movingEntity.Position;
// Stop when the moving entity has passed target.
if (toTarget.Length() <= movingEntity.BoundingRadius + movingEntity.Skin + movingEntity.DecelerationRange)
{
if (movingEntity.Velocity != Vector2.Zero && Vector2.Dot(toTarget, movingEntity.Velocity) <= 0)
{
movingEntity.Target = null;
return(Vector2.Zero);
}
}
// Stop when the moving entity is close enough.
float distance = toTarget.Length();
if (distance <= movingEntity.DecelerationRange)
{
movingEntity.Target = null;
return(Vector2.Zero);
}
return(SteeringHelper.Seek(elapsedTime, movingEntity));
}
private void AdjustTargetPositionWhenOverlapped(float elapsedTime, Steerable movingEntity)
{
var boundingSphere = new BoundingSphere(new Vector3(movingEntity.Target.Value, 0), movingEntity.BoundingRadius);
Neighbors.FindAll(ref boundingSphere, Partners);
foreach (var partner in Partners)
{
if (partner == null || partner == movingEntity)
{
continue;
}
Vector2 toTarget = Vector2.Normalize(movingEntity.Target.Value - movingEntity.Position);
if (float.IsNaN(toTarget.X))
{
continue;
}
if (Vector2.Dot(movingEntity.Forward, toTarget) < 0.8f &&
new BoundingCircle(partner.Position, partner.BoundingRadius).Contains(movingEntity.Target.Value) != ContainmentType.Disjoint)
{
Vector2 normal = Math2D.Rotate90DegreesCcw(toTarget);
if (Vector2.Dot(normal, movingEntity.Forward) < 0)
{
normal = -normal;
}
normal = (normal - toTarget) * 0.5f * 1.414f;
movingEntity.Target = partner.Position + normal * (movingEntity.BoundingRadius + partner.BoundingRadius);
break;
}
}
Partners.Clear();
}
protected override Vector2 OnUpdateSteeringForce(float elapsedTime, Steerable movingEntity)
{
//if the evader is ahead and facing the agent then we can just seek
//for the evader's current position.
Vector2 toEvader = Evader.Position - movingEntity.Position;
float relativeHeading = Vector2.Dot(movingEntity.Forward, Evader.Forward);
if ((Vector2.Dot(toEvader, movingEntity.Forward) > 0) &&
(relativeHeading < -0.95f)) //acos(0.95)=18 degs
{
movingEntity.Target = Evader.Position + Offset;
return(SteeringHelper.Seek(elapsedTime, movingEntity));
}
//Not considered ahead so we predict where the evader will be.
//the lookahead time is propotional to the distance between the evader
//and the pursuer; and is inversely proportional to the sum of the
//agent's velocities
float lookAheadTime = toEvader.Length() / (movingEntity.MaxSpeed + Evader.Speed);
//now seek to the predicted future position of the evader
movingEntity.Target = Evader.Position + Evader.Velocity * lookAheadTime + Offset;
return(SteeringHelper.Seek(elapsedTime, movingEntity));
}
protected override Vector2 OnUpdateSteeringForce(float elapsedTime, Steerable movingEntity)
{
int count = 0;
Vector2 totalForce = Vector2.Zero;
var boundingSphere = new BoundingSphere(new Vector3(movingEntity.Position, 0), Range);
Neighbors.FindAll(ref boundingSphere, Partners);
foreach (var partner in Partners)
{
if (partner != null && partner != movingEntity)
{
totalForce += partner.Forward;
if (++count >= SteeringHelper.MaxAffectingEntities)
{
break;
}
}
}
Partners.Clear();
if (count > 0)
{
totalForce /= count;
totalForce -= movingEntity.Forward;
}
return(totalForce);
}
public Vector2 UpdateSteeringForce(float elapsedTime, Steerable movingEntity)
{
if (!Enabled)
{
return(Vector2.Zero);
}
return(OnUpdateSteeringForce(elapsedTime, movingEntity));
}
public float?Collides(Vector2 from, Vector2 to, float elapsedTime, Steerable movingEntity)
{
if (!Enabled)
{
return(null);
}
return(OnCollides(from, to, elapsedTime, movingEntity));
}
protected override Vector2 OnUpdateSteeringForce(float elapsedTime, Steerable movingEntity)
{
int count = 0;
Steerable nearestPartner = null;
Vector2 nearestToPartner = new Vector2();
float minDistanceToPartner = float.MaxValue;
// Make sure seperation gets called earlier then steerable avoidance.
float detectorLength = movingEntity.BoundingRadius + movingEntity.Skin * 2;
var boundingSphere = new BoundingSphere(new Vector3(movingEntity.Position, 0), detectorLength);
Neighbors.FindAll(ref boundingSphere, Partners);
foreach (var partner in Partners)
{
if (partner == null || partner == movingEntity)
{
continue;
}
Vector2 toPartner = partner.Position - movingEntity.Position;
// Ignore entities moving away
//if (Vector2.Dot(toPartner, partner.Velocity) >= 0)
// continue;
float distance = toPartner.Length() - detectorLength - partner.BoundingRadius;
if (distance <= 0 && distance < minDistanceToPartner)
{
nearestToPartner = toPartner;
nearestPartner = partner;
minDistanceToPartner = distance;
}
if (++count >= SteeringHelper.MaxAffectingEntities)
{
break;
}
}
Partners.Clear();
if (nearestPartner != null)
{
return(-Vector2.Normalize(nearestToPartner) * movingEntity.MaxForce);
}
return(Vector2.Zero);
}
protected override Vector2 OnUpdateSteeringForce(float elapsedTime, Steerable movingEntity)
{
if (movingEntity.IsStucked)
{
stuckedFramesForRandomize++;
stuckedFramesForStop++;
if (stuckedFramesForStop >= StuckedFramesBeforeStop)
{
movingEntity.Target = null;
return(Vector2.Zero);
}
if (stuckedFramesForRandomize >= StuckedFramesBeforeRandomize)
{
stuckedFramesForRandomize = 0;
float randomAngle = (float)(Random.NextDouble() * MathHelper.Pi * 2);
Vector2 randomDirection = new Vector2();
randomDirection.X = (float)(Math.Cos(randomAngle));
randomDirection.Y = (float)(Math.Sin(randomAngle));
movingEntity.Forward = randomDirection;
return(randomDirection * movingEntity.MaxForce);
}
}
else
{
stuckedFramesForRandomize = 0;
stuckedFramesForStop = 0;
}
if (movingEntity.Target == null || movingEntity.IsStucked)
{
spinCount = 0;
}
else if (Vector2.Dot(movingEntity.Forward, trackedForward) <= 0)
{
if (++spinCount >= SpinCountBeforeStop)
{
// Avoid sudden turn when stopped.
movingEntity.Forward = trackedForward;
movingEntity.Target = null;
spinCount = 0;
}
trackedForward = movingEntity.Forward;
}
return(Vector2.Zero);
}
/// <summary>
/// Calculates the steering force to flee from the target.
/// </summary>
public static Vector2 Flee(float elapsedTime, Steerable movingEntity, Vector2 target)
{
Vector2 toTarget = Vector2.Normalize(movingEntity.Position - target);
float distance = toTarget.Length();
if (distance > 0)
{
Vector2 desiredForce = toTarget * movingEntity.MaxSpeed - movingEntity.Velocity;
if (-Vector2.Dot(Math2D.Rotate90DegreesCcw(toTarget), movingEntity.Velocity) < movingEntity.MaxForce * elapsedTime)
{
return(toTarget * movingEntity.MaxForce);
}
return(Vector2.Normalize(desiredForce) * movingEntity.MaxForce);
}
return(Vector2.Zero);
}
/// <summary>
/// Gets the forward vector that are influenced by the target.
/// </summary>
public static Vector2 GetTargetedForward(Steerable movingEntity)
{
Vector2 targetedForward = movingEntity.Forward;
if (movingEntity.Target.HasValue)
{
if (movingEntity.Speed > 0)
{
targetedForward += Vector2.Normalize(movingEntity.Target.Value - movingEntity.Position) * HintRatio;
targetedForward.Normalize();
}
else
{
targetedForward = Vector2.Normalize(movingEntity.Target.Value - movingEntity.Position);
}
}
return(targetedForward);
}
/// <summary>
/// Gets the forward vector that are influenced by the target.
/// </summary>
public static Vector2 GetTargetedForward(Steerable movingEntity, Vector2 relativeForward, float hintRatio, bool zeroSpeed)
{
Vector2 targetedForward = relativeForward;
if (movingEntity.Target.HasValue)
{
if (zeroSpeed)
{
targetedForward = Vector2.Normalize(movingEntity.Target.Value - movingEntity.Position);
}
else
{
targetedForward += Vector2.Normalize(movingEntity.Target.Value - movingEntity.Position) * hintRatio;
targetedForward.Normalize();
}
}
return(targetedForward);
}
protected override Vector2 OnUpdateSteeringForce(float elapsedTime, Steerable movingEntity)
{
// Not necessary to include the check for facing direction this time
Vector2 toPursuer = Pursuer.Position - movingEntity.Position;
//uncomment the following two lines to have Evade only consider pursuers
//within a 'threat range'
if (toPursuer.LengthSquared() > ThreatRange * ThreatRange)
{
return(Vector2.Zero);
}
//the lookahead time is propotional to the distance between the pursuer
//and the pursuer; and is inversely proportional to the sum of the
//agents' velocities
float LookAheadTime = toPursuer.Length() / (movingEntity.MaxSpeed + Pursuer.Speed);
//now flee away from predicted future position of the pursuer
return(SteeringHelper.Flee(elapsedTime, movingEntity, Pursuer.Position + Pursuer.Velocity * LookAheadTime));
}
/// <summary>
/// Calculates the steering force to seek to the target.
/// </summary>
public static Vector2 Seek(float elapsedTime, Steerable movingEntity)
{
if (!movingEntity.Target.HasValue)
{
return(Vector2.Zero);
}
Vector2 toTarget = Vector2.Normalize(movingEntity.Target.Value - movingEntity.Position);
float distance = toTarget.Length();
if (distance > 0)
{
Vector2 desiredForce = toTarget * movingEntity.MaxSpeed - movingEntity.Velocity;
if (Math.Abs(Vector2.Dot(Math2D.Rotate90DegreesCcw(toTarget), movingEntity.Velocity)) < movingEntity.MaxForce * elapsedTime)
{
return(toTarget * movingEntity.MaxForce);
}
return(Vector2.Normalize(desiredForce) * movingEntity.MaxForce);
}
return(Vector2.Zero);
}
protected virtual float?OnCollides(Vector2 from, Vector2 to, float elapsedTime, Steerable movingEntity)
{
return(null);
}
protected abstract Vector2 OnUpdateSteeringForce(float elapsedTime, Steerable movingEntity);
protected override Vector2 OnUpdateSteeringForce(float elapsedTime, Steerable movingEntity)
{
if (!movingEntity.Target.HasValue)
{
return(Vector2.Zero);
}
AdjustTargetPositionWhenOverlapped(elapsedTime, movingEntity);
Steerable nearestSteerable = null;
float minDistanceToSteerable = float.MaxValue;
float detectorLength = movingEntity.BoundingRadius + movingEntity.Skin;
// FindAll nearest steerable
var boundingSphere = new BoundingSphere(new Vector3(movingEntity.Position, 0), detectorLength);
Neighbors.FindAll(ref boundingSphere, Partners);
foreach (var partner in Partners)
{
if (partner == null || partner == movingEntity)
{
continue;
}
Vector2 toTarget = partner.Position - movingEntity.Position;
// Ignore entities behind us.
if (Vector2.Dot(movingEntity.TargetedForward, toTarget) < 0)
{
continue;
}
float distance = toTarget.Length();
// Ignore entities too far away
if (distance > movingEntity.BoundingRadius + partner.BoundingRadius + movingEntity.Skin)
{
continue;
}
if (distance < minDistanceToSteerable)
{
minDistanceToSteerable = distance;
nearestSteerable = partner;
}
}
Partners.Clear();
if (nearestSteerable != null)
{
Steerable partner = nearestSteerable;
// FindAll the adjacent steerable
Steerable secondNearestSteerable = null;
float minDistanceToSecondSteerable = float.MaxValue;
float secondDetectorLength = movingEntity.BoundingRadius * 2 + movingEntity.Skin * 2 + partner.BoundingRadius;
boundingSphere = new BoundingSphere(new Vector3(movingEntity.Position, 0), secondDetectorLength);
Neighbors.FindAll(ref boundingSphere, Partners);
foreach (var secondPartner in Partners)
{
if (secondPartner == null || secondPartner == partner || secondPartner == movingEntity)
{
continue;
}
float minAcceptableDistance = secondDetectorLength + secondPartner.BoundingRadius;
if (Vector2.Subtract(secondPartner.Position, partner.Position).LengthSquared() > minAcceptableDistance * minAcceptableDistance)
{
continue;
}
float distance = Math.Abs(Vector2.Dot(secondPartner.Position - movingEntity.Position, movingEntity.Forward));
if (distance < 0)
{
continue;
}
if (distance <= minDistanceToSecondSteerable)
{
minDistanceToSecondSteerable = distance;
secondNearestSteerable = secondPartner;
}
}
Partners.Clear();
if (secondNearestSteerable != null)
{
// Avoid the tangent line segment from two partners
Vector2 start, end;
Vector2 lineNormal = Math2D.Rotate90DegreesCcw(nearestSteerable.Position - secondNearestSteerable.Position);
if (Vector2.Dot(lineNormal, movingEntity.Position - nearestSteerable.Position) < 0)
{
lineNormal = -lineNormal;
start = nearestSteerable.Position + lineNormal * nearestSteerable.BoundingRadius;
end = secondNearestSteerable.Position + lineNormal * secondNearestSteerable.BoundingRadius;
}
else
{
start = secondNearestSteerable.Position + lineNormal * nearestSteerable.BoundingRadius;
end = nearestSteerable.Position + lineNormal * secondNearestSteerable.BoundingRadius;
}
Vector2 lineDirection = Vector2.Normalize(end - start);
lineNormal = Math2D.Rotate90DegreesCcw(lineDirection);
// Determine which direction to move across the wall that might takes less time to reach the target.
if (Vector2.Dot(lineDirection, movingEntity.TargetedForward) < 0)
{
lineDirection = -lineDirection;
}
// Moves the entity along the wall.
float penetration = -Vector2.Dot(movingEntity.Forward, lineNormal);
if (Vector2.Dot(lineDirection, movingEntity.Forward) > SteeringHelper.AvoidanceAngularEpsilon && penetration < 0)
{
return(lineDirection * movingEntity.MaxForce);
}
// If somehow the entity has penetrate the wall, this force will pull the entity out.
if (penetration > 0)
{
lineDirection += penetration * lineNormal;
}
Vector2 desiredForce = lineDirection * movingEntity.MaxSpeed - movingEntity.Velocity;
return(Vector2.Normalize(desiredForce) * movingEntity.MaxForce);
}
else
{
// Avoid steerable
Vector2 toTarget = partner.Position - movingEntity.Position;
toTarget.Normalize();
Vector2 lineDirection = Math2D.Rotate90DegreesCcw(toTarget);
if (Vector2.Dot(lineDirection, movingEntity.TargetedForward) < 0)
{
lineDirection = -lineDirection;
}
float penetration = movingEntity.BoundingRadius + partner.BoundingRadius + movingEntity.Skin - minDistanceToSteerable;
if (Vector2.Dot(lineDirection, movingEntity.Forward) > SteeringHelper.AvoidanceAngularEpsilon && penetration < 0)
{
return(lineDirection * movingEntity.MaxForce);
}
// If somehow the entity has penetrate the wall, this force will pull the entity out.
if (penetration > 0)
{
lineDirection += penetration / -movingEntity.Skin * toTarget;
}
Vector2 desiredForce = lineDirection * movingEntity.MaxSpeed - movingEntity.Velocity;
return(Vector2.Normalize(desiredForce) * movingEntity.MaxForce);
}
}
return(Vector2.Zero);
}
protected override float?OnCollides(Vector2 from, Vector2 to, float elapsedTime, Steerable movingEntity)
{
float detectorLength = movingEntity.BoundingRadius + movingEntity.Skin;
var boundingSphere = new BoundingSphere(new Vector3(movingEntity.Position, 0), detectorLength);
Neighbors.FindAll(ref boundingSphere, Partners);
foreach (var partner in Partners)
{
if (partner == null || partner == movingEntity)
{
continue;
}
BoundingCircle obstacle = new BoundingCircle(partner.Position, partner.BoundingRadius);
if (obstacle.Contains(new BoundingCircle(from, movingEntity.BoundingRadius)) != ContainmentType.Disjoint)
{
continue;
}
if (obstacle.Contains(new BoundingCircle(to, movingEntity.BoundingRadius)) == ContainmentType.Disjoint)
{
continue;
}
Partners.Clear();
return(0);
}
Partners.Clear();
return(null);
}
protected override float?OnCollides(Vector2 from, Vector2 to, float elapsedTime, Steerable movingEntity)
{
float detectorLength = movingEntity.BoundingRadius;
var boundingSphere = new BoundingSphere(new Vector3(movingEntity.Position, 0), detectorLength);
Walls.FindAll(ref boundingSphere, Lines);
foreach (var line in Lines)
{
if (Vector2.Dot(Vector2.Subtract(to, from), line.Normal) > 0)
{
continue;
}
if (Math2D.PointLineRelation(to - line.Normal * movingEntity.BoundingRadius, line.Start, line.Normal) == Math2D.SpanType.Front)
{
continue;
}
if (Math2D.PointLineRelation(from + line.Normal * movingEntity.BoundingRadius, line.Start, line.Normal) == Math2D.SpanType.Back)
{
continue;
}
if (Math2D.DistanceToLineSegment(line.Start, line.End, to) < movingEntity.BoundingRadius)
{
Lines.Clear();
return(0);
}
}
Lines.Clear();
return(null);
}
protected override Vector2 OnUpdateSteeringForce(float elapsedTime, Steerable movingEntity)
{
return(SteeringHelper.Arrive(elapsedTime, movingEntity));
}
protected override Vector2 OnUpdateSteeringForce(float elapsedTime, Steerable movingEntity)
{
if (!movingEntity.Target.HasValue)
{
return(Vector2.Zero);
}
// Check if the entity has approached the target
Vector2 toTarget = movingEntity.Target.Value - movingEntity.Position;
if (toTarget.Length() <= movingEntity.BoundingRadius + movingEntity.Skin)
{
return(Vector2.Zero);
}
LineSegment?nearestLineSegment = null;
float minDistanceToLineSq = float.MaxValue;
float detectorLength = movingEntity.BoundingRadius + movingEntity.DecelerationRange + movingEntity.Skin;
Vector2 targetedForward = movingEntity.TargetedForward;
var boundingSphere = new BoundingSphere(new Vector3(movingEntity.Position, 0), detectorLength);
Walls.FindAll(ref boundingSphere, Lines);
foreach (var line in Lines)
{
// Allow the entity to move across from back to front.
if (Vector2.Dot(targetedForward, line.Normal) > SteeringHelper.AvoidanceAngularEpsilon)
{
continue;
}
// Check if the entity has already moved through.
if (Math2D.PointLineRelation(movingEntity.Position + line.Normal * movingEntity.BoundingRadius, line.Start, line.Normal) == Math2D.SpanType.Back)
{
continue;
}
float distanceSq = Math2D.DistanceToLineSegmentSquared(line.Start, line.End, movingEntity.Position + targetedForward * movingEntity.MaxSpeed * elapsedTime);
if (distanceSq < minDistanceToLineSq)
{
minDistanceToLineSq = distanceSq;
nearestLineSegment = line;
}
}
Lines.Clear();
if (nearestLineSegment.HasValue)
{
LineSegment line = nearestLineSegment.Value;
// Check if the entity wants to move through.
// Ignore when both target position and entity position are in front of the wall.
if (Math2D.PointLineRelation(movingEntity.Target.Value - line.Normal * movingEntity.BoundingRadius, line.Start, line.Normal) != Math2D.SpanType.Back &&
Math2D.PointLineRelation(movingEntity.Position - line.Normal * movingEntity.BoundingRadius, line.Start, line.Normal) != Math2D.SpanType.Back)
{
return(Vector2.Zero);
}
// Check if the target position is in front of the line but the distance to line is less than bounding radius.
Vector2 lineToEntity;
if (Math2D.DistanceToLine(line.Start, line.End, movingEntity.Position) > movingEntity.BoundingRadius)
{
float targetToLine = Math2D.DistanceToLineSegment(line.Start, line.End, movingEntity.Target.Value, out lineToEntity);
if (targetToLine <= movingEntity.BoundingRadius)
{
movingEntity.Target = movingEntity.Target.Value + lineToEntity * (movingEntity.BoundingRadius - targetToLine);
return(Vector2.Zero);
}
}
float distance = Math2D.DistanceToLineSegment(line.Start, line.End, movingEntity.Position, out lineToEntity);
float decelerateRange = Vector2.Dot(movingEntity.Forward, -line.Normal) * movingEntity.DecelerationRange * 2;
// If deceleration range is too small, like when the moving entity has a maximum acceleration, there won't be
// enough space for it to turn or stop.
if (decelerateRange < movingEntity.Skin)
{
decelerateRange = movingEntity.Skin;
}
if (decelerateRange + movingEntity.Skin + movingEntity.BoundingRadius >= distance)
{
Vector2 lineDirection = Math2D.Rotate90DegreesCcw(lineToEntity);
// Determine which direction to move across the wall that might takes less time to reach the target.
if (Vector2.Dot(lineDirection, targetedForward) < 0)
{
lineDirection = -lineDirection;
}
// Moves the entity along the wall.
float penetration = movingEntity.BoundingRadius + movingEntity.Skin - distance;
if (Vector2.Dot(lineDirection, movingEntity.Forward) > SteeringHelper.AvoidanceAngularEpsilon && penetration < 0)
{
return(lineDirection * movingEntity.MaxForce);
}
// If somehow the entity has penetrate the wall, this force will pull the entity out.
if (penetration > 0)
{
lineDirection += penetration / movingEntity.Skin * lineToEntity;
}
Vector2 desiredForce = lineDirection * movingEntity.MaxSpeed - movingEntity.Velocity;
return(Vector2.Normalize(desiredForce) * movingEntity.MaxForce);
}
}
return(Vector2.Zero);
}
/// <summary>
/// Finds the miminum distance required to fully stop the moving entity from top speed.
/// </summary>
public static float GetDecelerateRange(Steerable movingEntity)
{
return(movingEntity.Speed * movingEntity.Speed * 0.5f / movingEntity.Acceleration);
}
/// <summary>
/// Calculates the steering force to avoid a line segment.
/// </summary>
public static Vector2 AvoidWall(LineSegment line, float elapsedTime, Steerable movingEntity, Vector2 targetedForward)
{
System.Diagnostics.Debug.Assert(movingEntity.Target.HasValue);
// Check if the entity has approached the target
Vector2 toTarget = movingEntity.Target.Value - movingEntity.Position;
if (toTarget.Length() <= movingEntity.BoundingRadius + movingEntity.Skin)
{
return(Vector2.Zero);
}
// Allow the entity to move across from back to front.
if (Vector2.Dot(targetedForward, line.Normal) > AvoidanceAngularEpsilon)
{
return(Vector2.Zero);
}
// Check if the entity has already moved through.
if (Math2D.PointLineRelation(movingEntity.Position + line.Normal * movingEntity.BoundingRadius, line.Start, line.Normal) == Math2D.SpanType.Back)
{
return(Vector2.Zero);
}
// Check if the entity wants to move through.
if (Math2D.PointLineRelation(movingEntity.Target.Value - line.Normal * movingEntity.BoundingRadius, line.Start, line.Normal) != Math2D.SpanType.Back &&
Math2D.PointLineRelation(movingEntity.Position - line.Normal * movingEntity.BoundingRadius, line.Start, line.Normal) != Math2D.SpanType.Back)
{
return(Vector2.Zero);
}
// Check if the target position is in front of the line but the distance to line is less than bounding radius.
Vector2 lineToEntity;
if (Math2D.DistanceToLine(line.Start, line.End, movingEntity.Position) > movingEntity.BoundingRadius)
{
float targetToLine = Math2D.DistanceToLineSegment(line.Start, line.End, movingEntity.Target.Value, out lineToEntity);
if (targetToLine <= movingEntity.BoundingRadius)
{
movingEntity.Target = movingEntity.Target.Value + lineToEntity * (movingEntity.BoundingRadius - targetToLine);
return(Vector2.Zero);
}
}
float distance = Math2D.DistanceToLineSegment(line.Start, line.End, movingEntity.Position, out lineToEntity);
float decelerateRange = Vector2.Dot(movingEntity.Forward, -line.Normal) * movingEntity.DecelerationRange * 2;
// If deceleration range is too small, like when the moving entity has a maximum acceleration, there won't be
// enough space for it to turn or stop.
if (decelerateRange < movingEntity.Skin)
{
decelerateRange = movingEntity.Skin;
}
if (decelerateRange + movingEntity.Skin + movingEntity.BoundingRadius >= distance)
{
Vector2 lineDirection = Math2D.Rotate90DegreesCcw(lineToEntity);
// Determine which direction to move across the wall that might takes less time to reach the target.
if (Vector2.Dot(lineDirection, targetedForward) < 0)
{
lineDirection = -lineDirection;
}
// Moves the entity along the wall.
float penetration = movingEntity.BoundingRadius + movingEntity.Skin - distance;
if (Vector2.Dot(lineDirection, movingEntity.Forward) > AvoidanceAngularEpsilon && penetration < 0)
{
return(lineDirection * movingEntity.MaxForce);
}
// If somehow the entity has penetrate the wall, this force will pull the entity out.
if (penetration > 0)
{
lineDirection += penetration / movingEntity.Skin * lineToEntity;
}
Vector2 desiredForce = lineDirection * movingEntity.MaxSpeed - movingEntity.Velocity;
return(Vector2.Normalize(desiredForce) * movingEntity.MaxForce);
}
return(Vector2.Zero);
}
/// <summary>
/// Calculates the steering force to avoid a circular obstacle.
/// </summary>
public static Vector2 AvoidObstacle(BoundingCircle obstacle, float elapsedTime, Steerable movingEntity, Vector2 targetedForward)
{
return(Vector2.Zero);
}
