/// <summary>
/// Get closest moving entities based on distance radius.
/// </summary>
public List <MovingEntity> GetClosestMovingEntities(MovingEntity entityRef, int distance)
{
var _entities = new List <MovingEntity>();
foreach (BaseEntity entity in _map.Values)
{
// Try to cast to MovingEntiy
var movingEntity = entity as MovingEntity;
// If not a movingEntity just continue the loop
if (movingEntity != null)
{
// Get distance from ref
Vector2 posDiff = movingEntity.Pos - entityRef.Pos;
// Account for bounding radius (physic boundaries of the object)
double radius = distance + movingEntity.BRadius;
// Tag if different from reference and close enough to reference
if (movingEntity != entityRef && (posDiff.LengthSquared() < (radius * radius)))
{
_entities.Add(movingEntity);
}
}
}
// Return all entities closed enough
return(_entities);
}
/// <summary>
/// Evade from another agent. Opposite to pursuit. This time predicted
/// next position is used to Flee from agent.
/// </summary>
private Vector2 Evade(MovingEntity pursuer)
{
Vector2 pursuerPos = pursuer.Pos;
Vector2 notDesiredDirection = pursuerPos - Agent.Pos;
// Assuming MaxSpeed for Agent and current speed for pursuer
float timeNextPosition = notDesiredDirection.Length() /
(Agent.MaxSpeed + pursuer.Speed);
// Flee predicted position
return(Flee(pursuerPos + Vector2.Multiply(pursuer.Velocity, timeNextPosition)));
}
/// <summary>
/// Make orientation of agent factor by some time.
/// A coefficient to scale the returned value
/// The higher the max turn rate of the vehicle, the higher `coefDelay` value should be.
/// If the vehicle is heading in the opposite direction to its target position,
/// a `coefDelay` of 0.5 return a delay of 1 second.
/// </summary>
/// <param name="entity">The entity direction that must be delay.</param>
/// <param name="targetPos">The position that the target must reached.</param>
/// <param name="coefDelay">A coefficient to delay the modification of direction.</param>
public double TurnArroundTime(MovingEntity entity, Vector2 targetPos, double coefDelay = 0.5)
{
Vector2 desiredDirection = Vector2.Normalize(targetPos - entity.Pos);
// Give a value in [-1, 1] where 1 means ahead target and -1 behind
float cos_angle = Vector2.Dot(entity.Heading, desiredDirection);
// Substrate 1 to add no change when already facing desired orientation
// Multiply by coef to define the scaling
// Multiply by -1 to get a positive value
return((cos_angle - 1) * coefDelay * -1);
}
/// <summary>
/// Pursuit account for target next position to intercept it.
/// </summary>
private Vector2 Pursuit(MovingEntity target)
{
Vector2 targetPos = target.Pos;
Vector2 toTarget = targetPos - Agent.Pos;
float cos_angle = Vector2.Dot(Agent.Heading, target.Heading);
// Face if angle between agents head is lower than 20°
// (180 - 20 used to get angle when both facing each other)
if (cos_angle < Math.Cos(MathHelper.ToRadians(180 - 20)) &&
Vector2.Dot(toTarget, Agent.Heading) > 0)
{
// Target is ahead and facing the agent
return(Seek(targetPos));
}
// Target is not ahead, find next position
// Assuming MaxSpeed for Agent and current speed for target
double timeNextPosition = toTarget.Length() /
(Agent.MaxSpeed + target.Speed);
// Delay orientation
timeNextPosition += TurnArroundTime(Agent, target.Pos, 0.05);
return(Seek(target.Pos + Vector2.Multiply(target.Velocity, (float)timeNextPosition)));
}