- Main Page
- Classes
- Files

- File List

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

    00001 //-----------------------------------------------------------------------------
    00002 // NystromIntegrator.cs
    00003 // (A. Schiffler, 2009)
    00004 //-----------------------------------------------------------------------------
    00005
    00006 using System;
    00007 using System.Collections.Generic;
    00008 using System.Text;
    00009
    00010 namespace NewGamePhysics.Mathematics
    00011 {
    00020     public class NystromIntegrator
    00021     {
    00025         ISecondDerivative secondDerivative;
    00026
    00030         private double currentTime;
    00031
    00035         private VectorN currentPosition;
    00036
    00040         private VectorN currentVelocity;
    00041
    00045         private VectorN currentAcceleration;
    00046
    00050         double timeStepsize;
    00051
    00060         public NystromIntegrator(
    00061             ISecondDerivative secondDerivative,
    00062             double timeStepsize,
    00063             VectorN initialPosition,
    00064             VectorN initialVelocity)
    00065         {
    00066             this.secondDerivative = secondDerivative;
    00067             this.timeStepsize = timeStepsize;
    00068             this.Reset(initialPosition, initialVelocity);
    00069         }
    00070
    00078         public VectorN Reset(
    00079             VectorN initialPosition,
    00080             VectorN initialVelocity)
    00081         {
    00082             // Set initial conditions
    00083             this.currentTime = 0.0;
    00084             this.currentPosition = initialPosition;
    00085             this.currentVelocity = initialVelocity;
    00086
    00087             // Calculate current acceleration
    00088             this.currentAcceleration = secondDerivative.GetValue(
    00089                 this.currentTime,
    00090                 this.currentPosition,
    00091                 this.currentVelocity);
    00092
    00093             return this.currentAcceleration;
    00094         }
    00095
    00104         public void Step(
    00105             out double newTime,
    00106             out VectorN newPosition,
    00107             out VectorN newVelocity,
    00108             out VectorN newAcceleration)
    00109         {
    00110             double timeStepsizeSquared = timeStepsize * timeStepsize;
    00111             double timeStepsizeHalf = timeStepsize / 2.0;
    00112             VectorN k1 = currentAcceleration;
    00113             VectorN k2 = secondDerivative.GetValue(currentTime + timeStepsizeHalf, currentPosition + timeStepsizeHalf * currentVelocity + timeStepsizeSquared / 8 * k1, currentVelocity + timeStepsizeHalf * k1);
    00114             VectorN k3 = secondDerivative.GetValue(currentTime + timeStepsizeHalf, currentPosition + timeStepsizeHalf * currentVelocity + timeStepsizeSquared/8 * k2, currentVelocity + timeStepsizeHalf * k2);
    00115             VectorN k4 = secondDerivative.GetValue(currentTime + timeStepsize,   currentPosition + timeStepsize   * currentVelocity + timeStepsizeSquared/2 * k3, currentVelocity + timeStepsize   * k3);
    00116
    00117             newTime = currentTime + timeStepsize;
    00118             newPosition = currentPosition + timeStepsize * currentVelocity + timeStepsizeSquared/6 * (k1 + k2 + k3);
    00119             newVelocity = currentVelocity + timeStepsize/6 * (k1 + 2 * k2 + 2 * k3 + k4);
    00120             newAcceleration = secondDerivative.GetValue(newTime, newPosition, newVelocity);
    00121
    00122             currentTime = newTime;
    00123             currentPosition = newPosition;
    00124             currentVelocity = newVelocity;
    00125             currentAcceleration = newAcceleration;
    00126         }
    00127     }
    00128 }