C:/Users/Andreas Schiffler/Desktop/SVN/NewGamePhysics/NewGamePhysics/PhysicalElements/DoublePendulumSimulationBase.cs

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

namespace NewGamePhysics.PhysicalElements
{
    using System;
    
    using Microsoft.Xna.Framework;
    using Microsoft.Xna.Framework.Graphics;

    using NewGamePhysics.Mathematics;
    using NewGamePhysics.Physics;

    public enum DoublePendulumHinges
    {
        Hinge1 = 0,

        Hinge2 = 1,
    }

    public enum DoublePendulumStateValues
    {
        Angle1 = 0,

        Angle2 = 1,

        Velocity1 = 2,

        Velocity2 = 3,

        Acceleration1 = 4,

        Acceleration2 = 5,
    }

    public abstract class DoublePendulumSimulationBase
    {
        private ISecondDerivative pendulumAcceleration;

        internal NystromIntegrator pendulumIntegrator;

        private double t;

        private double h;

        private RotationalFrictionModel frictionModel;

        internal Vector2 origin;

        internal VectorN theta;

        internal VectorN omega;

        internal VectorN acceleration;

        internal static double sqrt2 = Math.Sqrt(2.0);

        #region public_properties

        public double H
        {
            get { return h; }
            set { h = value; }
        }

        public double T
        {
            get { return t; }
            set { t = value; }
        }

        #endregion

        #region internal_properties

        internal RotationalFrictionModel FrictionModel
        {
            get { return this.frictionModel; }
            set { this.frictionModel = value; }
        }

        internal ISecondDerivative PendulumAcceleration
        {
            get { return this.pendulumAcceleration; }
            set { this.pendulumAcceleration = value; }
        }

        #endregion

        #region abstract_methods

        public abstract void SetInitialConditionsAtRest();

        public abstract Vector2[] GetPosition();

        public abstract Vector2[] GetPosition(Vector2 origin, double scale);

        public abstract double[] GetSize();

        public abstract double[] GetSize(double scale);

        public abstract double GetEnergy();

        #endregion

        #region common_public_methods

        public void SetInitialConditions(double theta1, double omega1, double theta2, double omega2)
        {
            // Store values
            this.theta[0] = theta1;
            this.theta[1] = theta2;
            this.omega[0] = omega1;
            this.omega[1] = omega2;

            // (Re)create integrator
            this.pendulumIntegrator = new NystromIntegrator(
                (ISecondDerivative)this.pendulumAcceleration,
                this.H,
                this.theta,
                this.omega);
        }

        public double Drive(DoublePendulumHinges which, double force)
        {
            // Apply a force to the hinges modifying speed
            double deltaOmega = force * this.H;
            this.omega[(int)which] += deltaOmega;
            return deltaOmega;
        }

        public void Animate()
        {
            // Animate pendulum
            // double t; VectorN theta, omega, acceleration;
            this.pendulumIntegrator.Step(
                out this.t,
                out this.theta,
                out this.omega,
                out this.acceleration);

            for (int i = 0; i < 2; i++)
            {
                this.omega[i] = this.FrictionModel.ApplyFriction(this.omega[i]);
            }
        }

        public double[] GetAngle()
        {
            double[] angles = new double[2];
            angles[0] = this.theta[0];
            angles[1] = this.theta[1];
            return angles;
        }

        public double GetAngle(DoublePendulumHinges which)
        {
            return this.theta[(int)which];
        }

        public double[] GetVelocity()
        {
            double[] omega = new double[2];
            omega[0] = this.omega[0];
            omega[1] = this.omega[1];
            return omega;
        }

        public double GetVelocity(DoublePendulumHinges which)
        {
            return this.omega[(int)which];

        }

        public double[] GetAcceleration()
        {
            double[] omega = new double[2];
            omega[0] = this.acceleration[0];
            omega[1] = this.acceleration[1];
            return omega;
        }

        public double GetAcceleration(DoublePendulumHinges which)
        {
            return this.acceleration[(int)which];
        }

        public double[] GetState()
        {
            double[] values = new double[6];
            values[(int)DoublePendulumStateValues.Angle1] =
                this.theta[(int)DoublePendulumHinges.Hinge1];
            values[(int)DoublePendulumStateValues.Angle2] =
                this.theta[(int)DoublePendulumHinges.Hinge2];
            values[(int)DoublePendulumStateValues.Velocity1]
                = this.omega[(int)DoublePendulumHinges.Hinge1];
            values[(int)DoublePendulumStateValues.Velocity2]
                = this.omega[(int)DoublePendulumHinges.Hinge2];
            values[(int)DoublePendulumStateValues.Acceleration1]
                = this.theta[(int)DoublePendulumHinges.Hinge1];
            values[(int)DoublePendulumStateValues.Acceleration2]
                = this.theta[(int)DoublePendulumHinges.Hinge2];
            return values;
        }

        public double GetState(DoublePendulumStateValues which)
        {
            return this.GetState()[(int)which];
        }

        #endregion
    }
}

---