/// <summary>
/// Helper function for the RK4 solver.
/// </summary>
private static AxisMotionDerivative RK4Evaluate(Trial trial, AxisMotionState initial, double dt,
AxisMotionDerivative d, ControlInput controlInput, AccelerationFunction accelFunction)
{
AxisMotionState state;
state = new AxisMotionState(initial.position + d.dAngle * dt, initial.velocity + d.dVelocity * dt,
initial.acceleration);
return(new AxisMotionDerivative(state.velocity, accelFunction(trial, state, controlInput)));
}
/// <summary>
/// RK4 solver.
/// </summary>
/// <param name="trial">The current trial</param>
/// <param name="state">State of the system (input / output)</param>
/// <param name="dt">Time since last evaluation</param>
/// <param name="accelFunction">Function to calculate acceleration</param>
/// <param name="controlInput">The current joystick or forcing function input</param>
private static void Integrate(Trial trial, AxisMotionState state, double dt, AccelerationFunction accelFunction,
ControlInput controlInput)
{
AxisMotionDerivative a;
AxisMotionDerivative b;
AxisMotionDerivative c;
AxisMotionDerivative d;
double dadt;
double dvdt;
a = new AxisMotionDerivative(state.velocity, accelFunction(trial, state, controlInput));
b = RK4Evaluate(trial, state, dt * 0.5, a, controlInput, accelFunction);
c = RK4Evaluate(trial, state, dt * 0.5, b, controlInput, accelFunction);
d = RK4Evaluate(trial, state, dt, c, controlInput, accelFunction);
dadt = 1.0 / 6.0 * (a.dAngle + 2.0 * (b.dAngle + c.dAngle) + d.dAngle);
dvdt = 1.0 / 6.0 * (a.dVelocity + 2.0 * (b.dVelocity + c.dVelocity) + d.dVelocity);
state.position += dadt * dt;
state.velocity += dvdt * dt;
state.acceleration = dvdt;
}
