internal void Tick(TimeSpan deltaTime)
{
lock (this)
{
double deltaSpeed = SpeedDemand - Speed;
if (Math.Abs(deltaSpeed) > Parameters.MaxAcceleration * deltaTime.TotalSeconds)
deltaSpeed = Math.Sign(deltaSpeed) * Parameters.MaxAcceleration * deltaTime.TotalSeconds;
Speed = Math.Max(Math.Min(Speed + deltaSpeed, Parameters.MaxSpeed), Parameters.MinSpeed);
double deltaSteer = SteerDemand - Steer;
if (Math.Abs(deltaSteer) > Parameters.MaxSteerRate * deltaTime.TotalSeconds)
deltaSteer = Math.Sign(deltaSteer) * Parameters.MaxSteerRate * deltaTime.TotalSeconds;
Steer = Math.Max(Math.Min(Steer + deltaSteer, Parameters.MaxSteer), -Parameters.MaxSteer);
Location = new Pose(Location.X + Speed * deltaTime.TotalSeconds * Location.Heading.Cos,
Location.Y + Speed * deltaTime.TotalSeconds * Location.Heading.Sin,
Location.Heading + Steer * deltaTime.TotalSeconds);
}
}
public Pose AddRelative(Pose delta)
{
return new Pose(AddRelative(delta.Position), Heading + delta.Heading);
}