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