public static Vector3D Move(this IAgent agent, double maxSpeed = double.PositiveInfinity, double agility = 1.0)
{
var a = agent.Face <IMoveableAgent>();
if (a.HistoryCount < 1)
{
a.HistoryCount = 1;
}
var velocity = a.Velocity;
velocity += agility * (a.ForceSum);
if (VMath.Polar(velocity).z > maxSpeed)
{
velocity = maxSpeed * ~velocity;
}
if (a.HistoryCount > 1)
{
var pos = a.PositionHistory();
pos.Insert(0, velocity + a.Position);
while (pos.Count > a.HistoryCount)
{
pos.RemoveAt(a.HistoryCount);
}
}
else
{
a.Position += velocity;
}
if (a.HistoryCount > 1)
{
var vel = a.VelocityHistory();
vel.Insert(0, velocity);
while (vel.Count > a.HistoryCount)
{
vel.RemoveAt(a.HistoryCount);
}
}
else
{
a.Velocity = velocity;
}
if (a.HistoryCount > 1)
{
var forceSum = a.ForceSumHistory();
forceSum.Insert(0, velocity);
while (forceSum.Count > a.HistoryCount)
{
forceSum.RemoveAt(a.HistoryCount);
}
}
else
{
a.ForceSum *= 0;
}
return(a.Position);
}
