- Main Page
- Classes
- Files

- File List

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

    00001 //-----------------------------------------------------------------------------
    00002 // DoubleSquarePendulumSimulation.cs
    00003 // (A. Schiffler, 2009)
    00004 //-----------------------------------------------------------------------------
    00005
    00006 namespace NewGamePhysics.PhysicalElements
    00007 {
    00008     using System;
    00009     using System.Collections.Generic;
    00010     using System.Text;
    00011
    00012     using Microsoft.Xna.Framework;
    00013     using Microsoft.Xna.Framework.Graphics;
    00014
    00015     using NewGamePhysics.Utilities;
    00016     using NewGamePhysics.GraphicalElements;
    00017     using NewGamePhysics.Mathematics;
    00018     using NewGamePhysics.Physics;
    00019
    00023     public class DoubleSquarePendulumSimulation : DoublePendulumSimulationBase
    00024     {
    00033         public DoubleSquarePendulumSimulation(
    00034             Vector2 origin,
    00035             double l,
    00036             double m,
    00037             double g,
    00038             RotationalFrictionType f)
    00039         {
    00040             // Origin
    00041             this.origin = origin;
    00042
    00043             // Time and stepsize
    00044             this.T = 0;
    00045             this.H = 0.01;
    00046
    00047             // Pendulum
    00048             this.PendulumAcceleration =
    00049                 new DoubleSquarePendulumAcceleration(l, m, g);
    00050
    00051             // Allocate state and reset
    00052             this.theta = new VectorN(2);
    00053             this.omega = new VectorN(2);
    00054             this.SetInitialConditionsAtRest();
    00055
    00056             // Initialize friction model
    00057             this.FrictionModel = new RotationalFrictionModel(f);
    00058         }
    00059
    00064         public override void SetInitialConditionsAtRest()
    00065         {
    00066             // Set top angle so positions are at rest
    00067             double thetaAtRest = Math.Atan(0.5);
    00068             SetInitialConditions(thetaAtRest, 0.0, 0.0, 0.0);
    00069         }
    00070
    00075         public override Vector2[] GetPosition()
    00076         {
    00077             // Allocate vectors for the position state of the pendulum components
    00078             Vector2[] pendulumPoints = new Vector2[8];
    00079             for (int i = 0; i < pendulumPoints.Length; i++)
    00080             {
    00081                 pendulumPoints[i] = new Vector2();
    00082             }
    00083
    00084             // Get size of components
    00085             double[] size = this.GetSize();
    00086
    00087             // Calculate pendulum component lengths and angles
    00088             double boxSize = size[0];
    00089             double boxDiagonal = boxSize * sqrt2;
    00090             double angle = this.theta[0];
    00091             double offset = Math.PI / 4.0;
    00092             double anglePlusOffset = angle + offset;
    00093             double angleMinusOffset = angle - offset;
    00094
    00095             // Set anchor/top of pendulum
    00096             pendulumPoints[0].X = this.origin.X;
    00097             pendulumPoints[0].Y = this.origin.Y;
    00098
    00099             // Calculate other positions of first square
    00100             pendulumPoints[1].X =
    00101                 (float)(pendulumPoints[0].X + (boxSize * Math.Sin(anglePlusOffset)));
    00102             pendulumPoints[1].Y =
    00103                 (float)(pendulumPoints[0].Y + (boxSize * Math.Cos(anglePlusOffset)));
    00104             pendulumPoints[2].X =
    00105                 (float)(pendulumPoints[0].X + (boxDiagonal * Math.Sin(theta[0])));
    00106             pendulumPoints[2].Y =
    00107                 (float)(pendulumPoints[0].Y + (boxDiagonal * Math.Cos(theta[0])));
    00108             pendulumPoints[3].X =
    00109                 (float)(pendulumPoints[0].X + (boxSize * Math.Sin(angleMinusOffset)));
    00110             pendulumPoints[3].Y =
    00111                 (float)(pendulumPoints[0].Y + (boxSize * Math.Cos(angleMinusOffset)));
    00112
    00113             // Top of second square
    00114             pendulumPoints[4].X = pendulumPoints[3].X;
    00115             pendulumPoints[4].Y = pendulumPoints[3].Y;
    00116
    00117             // Update angle values
    00118             angle = this.theta[1];
    00119             anglePlusOffset = angle + offset;
    00120             angleMinusOffset = angle - offset;
    00121
    00122             // Calculate other positions of second square
    00123             pendulumPoints[5].X =
    00124                 (float)(pendulumPoints[4].X + (boxSize * Math.Sin(anglePlusOffset)));
    00125             pendulumPoints[5].Y =
    00126                 (float)(pendulumPoints[4].Y + (boxSize * Math.Cos(anglePlusOffset)));
    00127             pendulumPoints[6].X =
    00128                 (float)(pendulumPoints[4].X + (boxDiagonal * Math.Sin(angle)));
    00129             pendulumPoints[6].Y =
    00130                 (float)(pendulumPoints[4].Y + (boxDiagonal * Math.Cos(angle)));
    00131             pendulumPoints[7].X =
    00132                 (float)(pendulumPoints[4].X + (boxSize * Math.Sin(angleMinusOffset)));
    00133             pendulumPoints[7].Y =
    00134                 (float)(pendulumPoints[4].Y + (boxSize * Math.Cos(angleMinusOffset)));
    00135
    00136             return pendulumPoints;
    00137         }
    00138
    00145         public override Vector2[] GetPosition(Vector2 screenOrigin, double screenScale)
    00146         {
    00147             // Update
    00148             Vector2[] pendulumPoints = this.GetPosition();
    00149
    00150             // Scale in place
    00151             for (int i=0; i<pendulumPoints.Length; i++)
    00152             {
    00153                 pendulumPoints[i].X = screenOrigin.X + (float)(screenScale * pendulumPoints[i].X);
    00154                 pendulumPoints[i].Y = screenOrigin.Y + (float)(screenScale * pendulumPoints[i].Y);
    00155             }
    00156
    00157             return pendulumPoints;
    00158         }
    00159
    00165         public override double[] GetSize()
    00166         {
    00167             DoubleSquarePendulumAcceleration pendulumAcceleration =
    00168                 this.PendulumAcceleration as DoubleSquarePendulumAcceleration;
    00169
    00170             // Calculate pendulum component lengths
    00171             double boxSize = pendulumAcceleration.L;
    00172             double[] size = new double[2];
    00173             size[0] = boxSize;
    00174             size[1] = boxSize;
    00175
    00176             return size;
    00177         }
    00178
    00185         public override double[] GetSize(double scale)
    00186         {
    00187             double[] size = this.GetSize();
    00188             for (int i = 0; i < size.Length; i++)
    00189             {
    00190                 size[i] *= scale;
    00191             }
    00192
    00193             return size;
    00194         }
    00195
    00200         public override double GetEnergy()
    00201         {
    00202             DoubleSquarePendulumAcceleration pendulumAcceleration =
    00203                 this.PendulumAcceleration as DoubleSquarePendulumAcceleration;
    00204
    00205             double quarterPI = 0.25 * Math.PI;
    00206             double phi = quarterPI - theta[0] + theta[1];
    00207             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));
    00208
    00209             double f0 = 0.12596795110235792;
    00210             double f1 = (1.0 - Math.Cos(theta[0]));
    00211             double f2 = (1.0 - Math.Cos(theta[1]));
    00212             double f3 = (1.0 - Math.Cos(quarterPI - theta[0]));
    00213             double f = (0.5 * sqrt2 * f1 + 0.5 * sqrt2 * f2 + f3 - f0);
    00214
    00215             double v = pendulumAcceleration.M * pendulumAcceleration.G * pendulumAcceleration.L * f;
    00216
    00217             return (k + v);
    00218         }
    00219     }
    00220 }
    00221