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

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

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

    using NewGamePhysics.Utilities;
    using NewGamePhysics.GraphicalElements;
    using NewGamePhysics.Mathematics;
    using NewGamePhysics.Physics;

    public class DoubleRegularPendulumSimulation : DoublePendulumSimulationBase
    {
        public DoubleRegularPendulumSimulation(
            Vector2 origin,
            double l1, 
            double m1, 
            double l2, 
            double m2, 
            double g, 
            RotationalFrictionType f)
        {
            // Origin
            this.origin = origin;

            // Time and stepsize
            this.T = 0.0;
            this.H = 0.01;

            // Construct pendulum
            this.PendulumAcceleration =
                (ISecondDerivative)new DoubleRegularPendulumAcceleration(l1, m1, l2, m2, g);
            
            // Reset initial conditions
            this.theta = new VectorN(2);
            this.omega = new VectorN(2);
            this.SetInitialConditionsAtRest();

            // Initialize friction model
            this.FrictionModel = new RotationalFrictionModel(f);
        }

        public override void SetInitialConditionsAtRest()
        {
            SetInitialConditions(0.0, 0.0, 0.0, 0.0);
        }

        public override Vector2[] GetPosition()
        {
            // Allocate cache for position state of the pendulum components
            Vector2[] pendulumPoints = new Vector2[3];
            for (int i = 0; i < pendulumPoints.Length; i++)
            {
                pendulumPoints[i] = new Vector2();
            }

            // Set angle for top component
            double angle = this.theta[0];

            // Set anchor/top of pendulum
            pendulumPoints[0].X = this.origin.X;
            pendulumPoints[0].Y = this.origin.Y;

            double[] pendulumSize = this.GetSize();

            // Calculate top component positions
            pendulumPoints[1].X = 
                (float)(pendulumPoints[0].X + (pendulumSize[0] * Math.Sin(angle)));
            pendulumPoints[1].Y = 
                (float)(pendulumPoints[0].Y + (pendulumSize[0] * Math.Cos(angle)));

            // Set angle for bottom component
            angle = this.theta[1];

            // Calculate bottom component positions
            pendulumPoints[2].X = 
                (float)(pendulumPoints[1].X + (pendulumSize[1] * Math.Sin(angle)));
            pendulumPoints[2].Y = 
                (float)(pendulumPoints[1].Y + (pendulumSize[1] * Math.Cos(angle)));

            return pendulumPoints;
        }

        public override Vector2[] GetPosition(Vector2 screenOrigin, double screenScale)
        {
            // Update
            Vector2[] pendulumPoints = this.GetPosition();

            // Scale in place
            for (int i=0; i<pendulumPoints.Length; i++)
            {
                pendulumPoints[i].X = screenOrigin.X + (float)(screenScale * pendulumPoints[i].X);
                pendulumPoints[i].Y = screenOrigin.Y + (float)(screenScale * pendulumPoints[i].Y);
            }

            return pendulumPoints;
        }

        public override double[] GetSize()
        {
            DoubleRegularPendulumAcceleration pendulumAcceleration =
                this.PendulumAcceleration as DoubleRegularPendulumAcceleration;

            double[] pendulumSize = new double[2];
            pendulumSize[0] = pendulumAcceleration.L1;
            pendulumSize[1] = pendulumAcceleration.L2;

            return pendulumSize;
        }

        public override double[] GetSize(double scale)
        {
            double[] size = this.GetSize();
            for (int i = 0; i < size.Length; i++)
            {
                size[i] *= scale;
            }

            return size;
        }

        public override double GetEnergy()
        {
            DoubleRegularPendulumAcceleration pendulumAcceleration =
                this.PendulumAcceleration as DoubleRegularPendulumAcceleration;

            double f1 = (1 - Math.Cos(theta[0]));
            double f2 = (1 - Math.Cos(theta[1]));
            double v = pendulumAcceleration.M1 * pendulumAcceleration.G *  (pendulumAcceleration.L1 * f1) +
                       pendulumAcceleration.M2 * pendulumAcceleration.G * ((pendulumAcceleration.L1 * f1) + (pendulumAcceleration.L2 * f2));
            double k = 0.5 * (pendulumAcceleration.M1 + pendulumAcceleration.M2) * pendulumAcceleration.L1 * pendulumAcceleration.L1 * omega[0] * omega[0] +
                       0.5 * pendulumAcceleration.M2 * pendulumAcceleration.L2 * pendulumAcceleration.L2 * omega[1] * omega[1] +
                       pendulumAcceleration.M2 * pendulumAcceleration.L1 * pendulumAcceleration.L2 * Math.Cos(theta[0] - theta[1]) * omega[0] * omega[1];
            return (v + k);
        }
    }
}

---