starting to move to a generic state structure
[chaz/yoink] / src / Moof / RK4.hh
1
2 /*******************************************************************************
3
4 Copyright (c) 2009, Charles McGarvey
5 All rights reserved.
6
7 Redistribution and use in source and binary forms, with or without
8 modification, are permitted provided that the following conditions are met:
9
10 * Redistributions of source code must retain the above copyright notice,
11 this list of conditions and the following disclaimer.
12 * Redistributions in binary form must reproduce the above copyright notice,
13 this list of conditions and the following disclaimer in the documentation
14 and/or other materials provided with the distribution.
15
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
17 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
18 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
20 FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
21 DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
22 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
23 CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
24 OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
25 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26
27 *******************************************************************************/
28
29 #ifndef _MOOF_RK4_HH_
30 #define _MOOF_RK4_HH_
31
32 #include <vector>
33
34 #include <boost/bind.hpp>
35 #include <boost/function.hpp>
36
37 #include <Moof/Math.hh>
38
39
40 namespace Mf {
41
42
43 // Generic implementations of a few simple integrators. To use, you need one
44 // type representing the state and another containing the derivatives of the
45 // primary state variables. The state class must implement these methods:
46 //
47 // void getDerivative(Derivative_Type& derivative, Scalar absoluteTime);
48 // void step(const Derivative_Type& derivative, Scalar deltaTime);
49 //
50 // Additionally, the derivative class must overload a few operators:
51 //
52 // Derivative_Type operator+(const Derivative_Type& other) const
53 // Derivative_Type operator*(const Derivative_Type& other) const
54
55 template<typename S, typename D>
56 inline D evaluate(const S& state, Scalar t)
57 {
58 D derivative;
59 state.getDerivative(derivative, t);
60 return derivative;
61 }
62
63 template<typename S, typename D>
64 inline D evaluate(S state, Scalar t, Scalar dt, const D& derivative)
65 {
66 state.step(derivative, dt);
67 return evaluate<S,D>(state, t + dt);
68 }
69
70
71 template<typename S, typename D>
72 inline void euler(S& state, Scalar t, Scalar dt)
73 {
74 D a = evaluate<S,D>(state, t);
75
76 state.step(a, dt);
77 }
78
79 template<typename S, typename D>
80 inline void rk2(S& state, Scalar t, Scalar dt)
81 {
82 D a = evaluate<S,D>(state, t);
83 D b = evaluate<S,D>(state, t, dt * SCALAR(0.5), a);
84
85 state.step(b, dt);
86 }
87
88 template<typename S, typename D>
89 inline void rk4(S& state, Scalar t, Scalar dt)
90 {
91 D a = evaluate<S,D>(state, t);
92 D b = evaluate<S,D>(state, t, dt * SCALAR(0.5), a);
93 D c = evaluate<S,D>(state, t, dt * SCALAR(0.5), b);
94 D d = evaluate<S,D>(state, t, dt, c);
95
96 state.step((a + (b + c) * SCALAR(2.0) + d) * SCALAR(1.0/6.0), dt);
97 }
98
99
100 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
101
102
103 template <int D = 3>
104 struct LinearState
105 {
106 typedef cml::vector< Scalar, cml::fixed<D> > Vector;
107 typedef boost::function<const Vector& (const LinearState&)> ForceFunction;
108
109 // primary
110
111 Vector position;
112 Vector momentum;
113
114 // secondary
115
116 Vector velocity;
117
118 // user
119 //
120 Vector force;
121 std::vector<ForceFunction> forces;
122
123 // constant
124
125 Scalar mass;
126 Scalar inverseMass;
127
128
129 void recalculateLinear()
130 {
131 velocity = momentum * inverseMass;
132 }
133
134
135 struct GravityForce
136 {
137 explicit GravityForce(Scalar a = -9.8)
138 {
139 force.zero();
140 acceleration = a;
141 }
142
143 const Vector& operator () (const LinearState& state)
144 {
145 force[1] = state.mass * acceleration;
146 return force;
147 }
148
149 private:
150
151 Vector force;
152 Scalar acceleration;
153 };
154
155
156 void init()
157 {
158 position.zero();
159 momentum.zero();
160
161 velocity.zero();
162
163 force.zero();
164 forces.clear();
165
166 mass = SCALAR(1.0);
167 inverseMass = 1.0 / mass;
168 }
169
170
171 struct Derivative
172 {
173 Vector velocity;
174 Vector force;
175
176 Derivative operator*(Scalar dt) const
177 {
178 Derivative derivative;
179 derivative.velocity = dt * velocity;
180 derivative.force = dt * force;
181 return derivative;
182 }
183
184 Derivative operator+(const Derivative& other) const
185 {
186 Derivative derivative;
187 derivative.velocity = velocity + other.velocity;
188 derivative.force = force + other.force;
189 return derivative;
190 }
191 };
192
193
194 Vector getForce() const
195 {
196 Vector f(force);
197
198 for (size_t i = 0; i < forces.size(); ++i)
199 {
200 f += forces[i](*this);
201 }
202
203 return f;
204 }
205
206 void getDerivative(Derivative& derivative, Scalar t) const
207 {
208 derivative.velocity = velocity;
209 derivative.force = getForce();
210 }
211
212 void step(const Derivative& derivative, Scalar dt)
213 {
214 position += dt * derivative.velocity;
215 momentum += dt * derivative.force;
216 recalculateLinear();
217 }
218 };
219
220
221 struct RotationalState2
222 {
223 // primary
224
225 Scalar orientation;
226 Scalar angularMomentum;
227
228 // secondary
229
230 Scalar angularVelocity;
231
232 // constant
233
234 Scalar inertia;
235 Scalar inverseInertia;
236
237
238 void recalculateRotational()
239 {
240 angularVelocity = angularMomentum * inertia;
241 }
242
243
244 struct Derivative
245 {
246 Scalar angularVelocity;
247 Scalar torque;
248 };
249
250 void step(const Derivative& derivative, Scalar dt)
251 {
252 orientation += dt * derivative.angularVelocity;
253 angularMomentum += dt * derivative.torque;
254 recalculateRotational();
255 }
256 };
257
258 struct RotationalState3
259 {
260 // primary
261
262 Quaternion orientation;
263 Vector3 angularMomentum;
264
265 // secondary
266
267 Quaternion spin;
268 Vector3 angularVelocity;
269
270 // constant
271
272 Scalar inertia;
273 Scalar inverseInertia;
274 };
275
276
277 struct State2 : public LinearState<2>, public RotationalState2
278 {
279 void recalculate()
280 {
281 recalculateLinear();
282 recalculateRotational();
283 }
284
285 void integrate(Scalar t, Scalar dt)
286 {
287 rk4<LinearState<2>,LinearState<2>::Derivative>(*this, t, dt);
288 }
289 };
290
291 struct State3 : public LinearState<3>, public RotationalState3 {};
292
293
294 } // namespace Mf
295
296 #endif // _MOOF_RK4_HH_
297
298 /** vim: set ts=4 sw=4 tw=80: *************************************************/
299
This page took 0.046625 seconds and 4 git commands to generate.