1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
|
// Geometric Tools, LLC
// Copyright (c) 1998-2014
// Distributed under the Boost Software License, Version 1.0.
// http://www.boost.org/LICENSE_1_0.txt
// http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt
//
// File Version: 5.0.0 (2010/01/01)
#include "PhysicsModule.h"
#include "Wm5Math.h"
#include "Wm5Memory.h"
#include "Wm5OdeRungeKutta4.h"
//----------------------------------------------------------------------------
PhysicsModule::PhysicsModule ()
:
Gravity(0.0),
Mass(0.0),
mTime(0.0),
mDeltaTime(0.0),
mEccentricity(0.0),
mRho(0.0),
mMajorAxis(0.0),
mMinorAxis(0.0),
mSolver(0)
{
mState[0] = 0.0;
mState[1] = 0.0;
mState[2] = 0.0;
mState[3] = 0.0;
mAux[0] = 0.0;
mAux[1] = 0.0;
mAux[2] = 0.0;
mAux[3] = 0.0;
mAux[4] = 0.0;
}
//----------------------------------------------------------------------------
PhysicsModule::~PhysicsModule ()
{
delete0(mSolver);
}
//----------------------------------------------------------------------------
void PhysicsModule::Initialize (double time, double deltaTime, double radius,
double theta, double radiusDot, double thetaDot)
{
mTime = time;
mDeltaTime = deltaTime;
// state variables
mState[0] = theta;
mState[1] = thetaDot;
mState[2] = radius;
mState[3] = radiusDot;
// Compute c0 and c1 in the potential energy function V(theta).
double gm = Gravity*Mass;
double gm2 = gm*Mass;
double radiusSqr = radius*radius;
double alpha = Mass*radiusSqr*thetaDot;
double g2m4da2 = gm2*gm2/(alpha*alpha);
double v0 = -gm/radius;
double dv0 = gm2*radiusDot/alpha;
double v0Plus = v0 + g2m4da2;
double sinTheta0 = Mathd::Sin(theta);
double cosTheta0 = Mathd::Cos(theta);
double c0 = v0Plus*sinTheta0 + dv0*cosTheta0;
double c1 = v0Plus*cosTheta0 - dv0*sinTheta0;
// Auxiliary variables needed by function DTheta(...).
mAux[0] = c0;
mAux[1] = c1;
mAux[2] = g2m4da2;
mAux[3] = alpha/(gm*gm2);
// ellipse parameters
double gamma0 = radiusSqr*Mathd::FAbs(thetaDot);
double tmp0 = radiusSqr*radius*thetaDot*thetaDot - gm;
double tmp1 = radiusSqr*radiusDot*thetaDot;
double gamma1 = Mathd::Sqrt(tmp0*tmp0 + tmp1*tmp1);
mEccentricity = gamma1/gm;
mRho = gamma0*gamma0/gamma1;
double tmp2 = 1.0 - mEccentricity*mEccentricity;
assertion(tmp2 > 0.0, "Invalid eccentricity.\n");
mMajorAxis = mRho*mEccentricity/tmp2;
mMinorAxis = mMajorAxis*Mathd::Sqrt(tmp2);
// RK4 differential equation solver. Since mSolver is a base class
// pointer, you can instead create a solver of whatever class you prefer.
delete0(mSolver);
mSolver = new0 OdeRungeKutta4d(1, mDeltaTime, OdeFunction, mAux);
}
//----------------------------------------------------------------------------
double PhysicsModule::GetPeriod () const
{
double powValue = Mathd::Pow(mMajorAxis, 1.5);
double sqrtValue = Mathd::Sqrt(Gravity*Mass);
return Mathd::TWO_PI*powValue/sqrtValue;
}
//----------------------------------------------------------------------------
void PhysicsModule::Update ()
{
if (mSolver)
{
// Apply a single step of the ODE solver.
mSolver->Update(mTime, mState, mTime, mState);
// Compute dot(theta) for application access.
double sn = Mathd::Sin(mState[0]);
double cs = Mathd::Cos(mState[0]);
double v = mAux[0]*sn + mAux[1]*cs - mAux[2];
mState[1] = mAux[3]*v*v;
// Compute radius for application access.
mState[2] = mEccentricity*mRho/(1.0 + mEccentricity*cs);
// Compute dot(radius) for application access.
mState[3] = mState[2]*mState[2]*mState[1]*sn/mRho;
}
}
//----------------------------------------------------------------------------
void PhysicsModule::OdeFunction (double, const double* state, void* data,
double* output)
{
double* aux = (double*)data;
double sn = Mathd::Sin(state[2]);
double cs = Mathd::Cos(state[2]);
double v = aux[0]*sn + aux[1]*cs - aux[2];
double thetaDotFunction = aux[3]*v*v;
// dot(theta) function
output[0] = thetaDotFunction;
}
//----------------------------------------------------------------------------
|
