private void InternalMove(double time, double dt)
{
if (!MathUtils.NearlyEqual(RollTo, roll.ToValue))
{
roll.Set(time, RollTo);
}
bool changed =
!MathUtils.NearlyEqual(ThrustTo, thrust.ToValue) ||
!MathUtils.NearlyEqual(HeadingTo, heading.ToValue) ||
thrust.WillReset(time) ||
heading.WillReset(time);
var t = time - t0;
var t2 = t * t / 2;
var a = thrust.FromValue;
var phi = heading.FromValue;
var AS = thrust.Direction * thrust.Speed;
var omega = heading.Direction * heading.Speed;
var om2 = omega * omega;
var a1 = a + AS * t;
var phi1 = phi + omega * t;
var v1 = VolatileShip.Speed;
if (heading.Direction == 0)
{
// no rotation, linear acceleration
// a = a_0 + A*t
VolatileShip.Speed = v0 + (a * t + AS * t2) * Vector.Direction(phi);
}
else if (thrust.Direction == 0)
{
// constant acceleration, rotating ship
// integrated by Wolfram Aloha
VolatileShip.Speed = v0 + (a / omega) * new Vector(Math.Cos(phi) - Math.Cos(phi1), -Math.Sin(phi) + Math.Sin(phi1));
}
else
{
// rotating and powering
// running Archimedean spiral
// integrated by Wolfram Aloha
var dVx = (-a1 * Math.Cos(phi1) + a * Math.Cos(phi)) / omega + AS * (Math.Sin(phi1) - Math.Sin(phi)) / om2;
var dVy = (a1 * Math.Sin(phi1) - a * Math.Sin(phi)) / omega + AS * (Math.Cos(phi1) - Math.Cos(phi)) / om2;
VolatileShip.Speed = v0 + new Vector(dVx, dVy);
}
// I failed to integrate position properly
// so we'll use trapezium rule
VolatileShip.Position += (VolatileShip.Speed + v1) * (dt / 2);
if (changed)
{
thrust.Set(time, ThrustTo);
heading.Set(time, HeadingTo);
v0 = VolatileShip.Speed;
t0 = time;
}
VolatileShip.Thrust = thrust.Get(time);
VolatileShip.Heading = heading.Get(time);
VolatileShip.Roll = roll.Get(time);
}
public void UpdateTime(double time)
{
if (rollTo.HasValue)
{
Roll.Set(time, rollTo.Value);
rollTo = null;
}
RollValue = Roll.Get(time);
var t = time - t0;
var t2 = t * t / 2;
var a = Acceleration.FromValue;
var phi = Heading.FromValue;
var AS = Acceleration.Direction * Acceleration.Speed;
var omega = Heading.Direction * Heading.Speed;
var om2 = omega * omega;
var a1 = a + AS * t;
var phi1 = phi + omega * t;
var v1 = V;
if (Heading.Direction == 0)
{
// no rotation, linear acceleration
// a = a_0 + A*t
V = v0 + (a * t + AS * t2) * Vector.Direction(phi);
}
else if (Acceleration.Direction == 0)
{
// constant acceleration, rotating ship
// integrated by Wolfram Aloha
V = v0 + (a / omega) * new Vector(Math.Cos(phi) - Math.Cos(phi1), -Math.Sin(phi) + Math.Sin(phi1));
}
else
{
// rotating and powering
// running Archimedean spiral
// integrated by Wolfram Aloha
var dVx = (-a1 * Math.Cos(phi1) + a * Math.Cos(phi)) / omega + AS * (Math.Sin(phi1) - Math.Sin(phi)) / om2;
var dVy = (a1 * Math.Sin(phi1) - a * Math.Sin(phi)) / omega + AS * (Math.Cos(phi1) - Math.Cos(phi)) / om2;
V = v0 + new Vector(dVx, dVy);
}
// I failed to integrate position properly
// so we'll use trapezium rule
var dt = time - t1;
S = S + (V + v1) * (dt / 2);
t1 = time;
// end of the current arc
bool changed = (headingTo.HasValue || accelerateTo.HasValue || Acceleration.WillReset(time) || Heading.WillReset(time));
if (changed)
{
v0 = V;
t0 = time;
Heading.Set(time, HeadingTo);
Acceleration.Set(time, AccelerateTo);
headingTo = null;
accelerateTo = null;
}
AccelerationValue = a1;
HeadingValue = phi1;
}