Пример #1
0
        /// <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)));
        }
Пример #2
0
        /// <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;
        }