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