global gravity for rigid bodies
[chaz/yoink] / src / moof / rigid_body.hh
1
2 /*] Copyright (c) 2009-2011, Charles McGarvey [*****************************
3 **] All rights reserved.
4 *
5 * Distributable under the terms and conditions of the 2-clause BSD license;
6 * see the file COPYING for a complete text of the license.
7 *
8 *****************************************************************************/
9
10 #ifndef _MOOF_RIGID_BODY_HH_
11 #define _MOOF_RIGID_BODY_HH_
12
13 #include <vector>
14
15 #include <boost/bind.hpp>
16 #include <boost/function.hpp>
17
18 #include <moof/entity.hh>
19 #include <moof/math.hh>
20
21
22 /**
23 * \file rigid_body.hh
24 * Classes and functions for simulating rigid body physics.
25 */
26
27 namespace moof {
28
29
30 extern scalar global_acceleration;
31
32 template <int D>
33 struct linear_state
34 {
35 typedef moof::vector< scalar, fixed<D> > vector;
36 typedef boost::function<const vector& (const linear_state&)>
37 force_function;
38
39 // primary
40 vector position;
41 vector momentum;
42
43 // secondary
44 vector velocity;
45
46 // user
47 vector force;
48 std::vector<force_function> forces;
49
50 // constant
51 scalar mass;
52 scalar inverse_mass;
53
54 void recalculate()
55 {
56 velocity = momentum * inverse_mass;
57 }
58
59 struct gravity_force
60 {
61 gravity_force()
62 {
63 force.zero();
64 }
65
66 const vector& operator () (const linear_state& state)
67 {
68 force[1] = state.mass * global_acceleration;
69 return force;
70 }
71
72 private:
73
74 vector force;
75 };
76
77 void init()
78 {
79 position.zero();
80 momentum.zero();
81
82 velocity.zero();
83
84 force.zero();
85 forces.clear();
86
87 mass = SCALAR(1.0);
88 inverse_mass = SCALAR(1.0);
89 }
90
91 struct derivative
92 {
93 vector velocity;
94 vector force;
95
96 derivative operator * (scalar dt) const
97 {
98 derivative derivative;
99 derivative.velocity = dt * velocity;
100 derivative.force = dt * force;
101 return derivative;
102 }
103
104 derivative operator + (const derivative& other) const
105 {
106 derivative derivative;
107 derivative.velocity = velocity + other.velocity;
108 derivative.force = force + other.force;
109 return derivative;
110 }
111 };
112
113 vector total_force() const
114 {
115 vector f(force);
116
117 typename std::vector<force_function>::const_iterator it;
118 for (it = forces.begin(); it != forces.end(); ++it)
119 {
120 f += (*it)(*this);
121 }
122
123 return f;
124 }
125
126 void calculate_derivative(derivative& derivative, scalar t) const
127 {
128 derivative.velocity = velocity;
129 derivative.force = total_force();
130 }
131
132 void step(const derivative& derivative, scalar dt)
133 {
134 position += dt * derivative.velocity;
135 momentum += dt * derivative.force;
136 recalculate();
137 }
138 };
139
140 struct rotational_state2
141 {
142 // primary
143 scalar orientation;
144 scalar angular_momentum;
145
146 // secondary
147 scalar angular_velocity;
148
149 // constant
150 scalar inertia;
151 scalar inverse_inertia;
152
153 void recalculate()
154 {
155 angular_velocity = angular_momentum * inertia;
156 }
157
158 struct derivative
159 {
160 scalar angular_velocity;
161 scalar torque;
162 };
163
164 void step(const derivative& derivative, scalar dt)
165 {
166 orientation += dt * derivative.angular_velocity;
167 angular_momentum += dt * derivative.torque;
168 recalculate();
169 }
170 };
171
172 struct rotational_state3
173 {
174 // primary
175 quaternion orientation;
176 vector3 angular_momentum;
177
178 // secondary
179 quaternion spin;
180 vector3 angular_velocity;
181
182 // constant
183 scalar inertia;
184 scalar inverse_inertia;
185
186 void recalculate()
187 {
188 angular_velocity = angular_momentum * inertia;
189 }
190 };
191
192
193 struct state2 : public linear_state<2>, public rotational_state2
194 {
195 void recalculate()
196 {
197 linear_state<2>::recalculate();
198 rotational_state2::recalculate();
199 }
200
201 void update(scalar t, scalar dt)
202 {
203 rk4<linear_state<2>,linear_state<2>::derivative>(*this, t, dt);
204 }
205 };
206
207 struct state3 : public linear_state<3>, public rotational_state3
208 {
209 void recalculate()
210 {
211 linear_state<3>::recalculate();
212 rotational_state3::recalculate();
213 }
214
215 void update(scalar t, scalar dt)
216 {
217 rk4<linear_state<3>,linear_state<3>::derivative>(*this, t, dt);
218 }
219 };
220
221 template <class T>
222 inline T interpolate(const T& a, const T& b, scalar alpha)
223 {
224 return lerp(a, b, alpha);
225 }
226
227 template <>
228 inline state2
229 interpolate<state2>(const state2& a, const state2& b, scalar alpha)
230 {
231 state2 state(b);
232 state.position = interpolate(a.position, b.position, alpha);
233 state.momentum = interpolate(a.momentum, b.momentum, alpha);
234 state.orientation = interpolate(a.orientation, b.orientation, alpha);
235 state.angular_momentum = interpolate(a.angular_momentum,
236 b.angular_momentum, alpha);
237 return state;
238 }
239
240 template <>
241 inline state3
242 interpolate<state3>(const state3& a, const state3& b, scalar alpha)
243 {
244 state3 state(b);
245 state.position = interpolate(a.position, b.position, alpha);
246 state.momentum = interpolate(a.momentum, b.momentum, alpha);
247 state.orientation = slerp(a.orientation, b.orientation, alpha);
248 state.angular_momentum = interpolate(a.angular_momentum,
249 b.angular_momentum, alpha);
250 return state;
251 }
252
253 /**
254 * Interface for anything that can move.
255 */
256 template <class T>
257 class rigid_body : public entity
258 {
259 public:
260
261 virtual ~rigid_body() {}
262
263 virtual void update(scalar t, scalar dt)
264 {
265 prev_state_ = state_;
266 state_.update(t, dt);
267 }
268
269 const T& state() const
270 {
271 return state_;
272 }
273
274 T state(scalar alpha) const
275 {
276 return interpolate(prev_state_, state_, alpha);
277 }
278
279 const T& getLastState() const
280 {
281 return prev_state_;
282 }
283
284 protected:
285
286 T state_;
287 T prev_state_;
288 };
289
290 typedef rigid_body<state2> rigid_body2;
291 typedef rigid_body<state3> rigid_body3;
292
293
294 } // namespace moof
295
296 #endif // _MOOF_RIGID_BODY_HH_
297
This page took 0.044223 seconds and 4 git commands to generate.