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

//-----------------------------------------------------------------------------
// DoubleSquarePendulumSimulation.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 DoubleSquarePendulumSimulation : DoublePendulumSimulationBase
    {
        public DoubleSquarePendulumSimulation(
            Vector2 origin,
            double l,             
            double m, 
            double g, 
            RotationalFrictionType f)
        {
            // Origin
            this.origin = origin;

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

            // Pendulum
            this.PendulumAcceleration = 
                new DoubleSquarePendulumAcceleration(l, m, g);

            // Allocate state and reset
            this.theta = new VectorN(2);
            this.omega = new VectorN(2);
            this.SetInitialConditionsAtRest();

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

        public override void SetInitialConditionsAtRest()
        {
            // Set top angle so positions are at rest
            double thetaAtRest = Math.Atan(0.5);
            SetInitialConditions(thetaAtRest, 0.0, 0.0, 0.0);
        }

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

            // Get size of components
            double[] size = this.GetSize();

            // Calculate pendulum component lengths and angles
            double boxSize = size[0];
            double boxDiagonal = boxSize * sqrt2;
            double angle = this.theta[0];
            double offset = Math.PI / 4.0;
            double anglePlusOffset = angle + offset;
            double angleMinusOffset = angle - offset;

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

            // Calculate other positions of first square
            pendulumPoints[1].X =
                (float)(pendulumPoints[0].X + (boxSize * Math.Sin(anglePlusOffset)));
            pendulumPoints[1].Y =
                (float)(pendulumPoints[0].Y + (boxSize * Math.Cos(anglePlusOffset)));
            pendulumPoints[2].X =
                (float)(pendulumPoints[0].X + (boxDiagonal * Math.Sin(theta[0])));
            pendulumPoints[2].Y =
                (float)(pendulumPoints[0].Y + (boxDiagonal * Math.Cos(theta[0])));
            pendulumPoints[3].X =
                (float)(pendulumPoints[0].X + (boxSize * Math.Sin(angleMinusOffset)));
            pendulumPoints[3].Y =
                (float)(pendulumPoints[0].Y + (boxSize * Math.Cos(angleMinusOffset)));

            // Top of second square
            pendulumPoints[4].X = pendulumPoints[3].X;
            pendulumPoints[4].Y = pendulumPoints[3].Y;

            // Update angle values
            angle = this.theta[1];
            anglePlusOffset = angle + offset;
            angleMinusOffset = angle - offset;

            // Calculate other positions of second square
            pendulumPoints[5].X =
                (float)(pendulumPoints[4].X + (boxSize * Math.Sin(anglePlusOffset)));
            pendulumPoints[5].Y =
                (float)(pendulumPoints[4].Y + (boxSize * Math.Cos(anglePlusOffset)));
            pendulumPoints[6].X =
                (float)(pendulumPoints[4].X + (boxDiagonal * Math.Sin(angle)));
            pendulumPoints[6].Y =
                (float)(pendulumPoints[4].Y + (boxDiagonal * Math.Cos(angle)));
            pendulumPoints[7].X =
                (float)(pendulumPoints[4].X + (boxSize * Math.Sin(angleMinusOffset)));
            pendulumPoints[7].Y =
                (float)(pendulumPoints[4].Y + (boxSize * Math.Cos(angleMinusOffset)));

            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()
        {
            DoubleSquarePendulumAcceleration pendulumAcceleration =
                this.PendulumAcceleration as DoubleSquarePendulumAcceleration;

            // Calculate pendulum component lengths
            double boxSize = pendulumAcceleration.L;
            double[] size = new double[2];
            size[0] = boxSize;
            size[1] = boxSize;

            return size;
        }

        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()
        {
            DoubleSquarePendulumAcceleration pendulumAcceleration =
                this.PendulumAcceleration as DoubleSquarePendulumAcceleration;

            double quarterPI = 0.25 * Math.PI;
            double phi = quarterPI - theta[0] + theta[1];
            double k = 0.5 * pendulumAcceleration.M * pendulumAcceleration.L * ((5.0 / 3.0) * omega[0] * omega[0] + (2.0 / 3.0) * omega[1] * omega[1] + sqrt2 * omega[0] * omega[1] * Math.Cos(phi));

            double f0 = 0.12596795110235792;
            double f1 = (1.0 - Math.Cos(theta[0]));
            double f2 = (1.0 - Math.Cos(theta[1]));
            double f3 = (1.0 - Math.Cos(quarterPI - theta[0]));
            double f = (0.5 * sqrt2 * f1 + 0.5 * sqrt2 * f2 + f3 - f0);

            double v = pendulumAcceleration.M * pendulumAcceleration.G * pendulumAcceleration.L * f;

            return (k + v);
        }
    }
}

---