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