C:/Users/Andreas Schiffler/Desktop/SVN/NewGamePhysics/NewGamePhysics/Mathematics/NystromIntegrator.cs

//-----------------------------------------------------------------------------
// NystromIntegrator.cs
// (A. Schiffler, 2009)
//-----------------------------------------------------------------------------

using System;
using System.Collections.Generic;
using System.Text;

namespace NewGamePhysics.Mathematics
{
    public class NystromIntegrator
    {
        ISecondDerivative secondDerivative;

        private double currentTime;

        private VectorN currentPosition;

        private VectorN currentVelocity;

        private VectorN currentAcceleration;

        double timeStepsize;

        public NystromIntegrator(
            ISecondDerivative secondDerivative, 
            double timeStepsize,
            VectorN initialPosition, 
            VectorN initialVelocity)
        {
            this.secondDerivative = secondDerivative;
            this.timeStepsize = timeStepsize;
            this.Reset(initialPosition, initialVelocity);
        }

        public VectorN Reset(
            VectorN initialPosition,
            VectorN initialVelocity)
        {
            // Set initial conditions
            this.currentTime = 0.0;
            this.currentPosition = initialPosition;
            this.currentVelocity = initialVelocity;

            // Calculate current acceleration
            this.currentAcceleration = secondDerivative.GetValue(
                this.currentTime, 
                this.currentPosition, 
                this.currentVelocity);

            return this.currentAcceleration;
        }

        public void Step(
            out double newTime, 
            out VectorN newPosition, 
            out VectorN newVelocity, 
            out VectorN newAcceleration)
        {
            double timeStepsizeSquared = timeStepsize * timeStepsize;
            double timeStepsizeHalf = timeStepsize / 2.0;
            VectorN k1 = currentAcceleration;
            VectorN k2 = secondDerivative.GetValue(currentTime + timeStepsizeHalf, currentPosition + timeStepsizeHalf * currentVelocity + timeStepsizeSquared / 8 * k1, currentVelocity + timeStepsizeHalf * k1);
            VectorN k3 = secondDerivative.GetValue(currentTime + timeStepsizeHalf, currentPosition + timeStepsizeHalf * currentVelocity + timeStepsizeSquared/8 * k2, currentVelocity + timeStepsizeHalf * k2);
            VectorN k4 = secondDerivative.GetValue(currentTime + timeStepsize,   currentPosition + timeStepsize   * currentVelocity + timeStepsizeSquared/2 * k3, currentVelocity + timeStepsize   * k3);

            newTime = currentTime + timeStepsize;
            newPosition = currentPosition + timeStepsize * currentVelocity + timeStepsizeSquared/6 * (k1 + k2 + k3);
            newVelocity = currentVelocity + timeStepsize/6 * (k1 + 2 * k2 + 2 * k3 + k4);
            newAcceleration = secondDerivative.GetValue(newTime, newPosition, newVelocity);

            currentTime = newTime;
            currentPosition = newPosition;
            currentVelocity = newVelocity;
            currentAcceleration = newAcceleration;
        }
    }
}

---