b8b9048c358e92fa70f01ab9d43af899308545eb
[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 template <int D = 3>
31 struct linear_state
32 {
33 typedef moof::vector< scalar, fixed<D> > vector;
34 typedef boost::function<const vector& (const linear_state&)>
35 force_function;
36
37 // primary
38 vector position;
39 vector momentum;
40
41 // secondary
42 vector velocity;
43
44 // user
45 vector force;
46 std::vector<force_function> forces;
47
48 // constant
49 scalar mass;
50 scalar inverse_mass;
51
52 void recalculate()
53 {
54 velocity = momentum * inverse_mass;
55 }
56
57 struct gravity_force
58 {
59 explicit gravity_force(scalar a = -9.8)
60 {
61 force.zero();
62 acceleration = a;
63 }
64
65 const vector& operator () (const linear_state& state)
66 {
67 force[1] = state.mass * acceleration;
68 return force;
69 }
70
71 private:
72
73 vector force;
74 scalar acceleration;
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 = 1.0 / mass;
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 for (size_t i = 0; i < forces.size(); ++i)
118 {
119 f += forces[i](*this);
120 }
121
122 return f;
123 }
124
125 void calculate_derivative(derivative& derivative, scalar t) const
126 {
127 derivative.velocity = velocity;
128 derivative.force = total_force();
129 }
130
131 void step(const derivative& derivative, scalar dt)
132 {
133 position += dt * derivative.velocity;
134 momentum += dt * derivative.force;
135 recalculate();
136 }
137 };
138
139 struct rotational_state2
140 {
141 // primary
142 scalar orientation;
143 scalar angular_momentum;
144
145 // secondary
146 scalar angularVelocity;
147
148 // constant
149 scalar inertia;
150 scalar inverse_inertia;
151
152 void recalculate()
153 {
154 angularVelocity = angular_momentum * inertia;
155 }
156
157 struct derivative
158 {
159 scalar angularVelocity;
160 scalar torque;
161 };
162
163 void step(const derivative& derivative, scalar dt)
164 {
165 orientation += dt * derivative.angularVelocity;
166 angular_momentum += dt * derivative.torque;
167 recalculate();
168 }
169 };
170
171 struct rotational_state3
172 {
173 // primary
174 quaternion orientation;
175 vector3 angular_momentum;
176
177 // secondary
178 quaternion spin;
179 vector3 angularVelocity;
180
181 // constant
182 scalar inertia;
183 scalar inverse_inertia;
184
185 void recalculate()
186 {
187 angularVelocity = angular_momentum * inertia;
188 }
189 };
190
191
192 struct state2 : public linear_state<2>, public rotational_state2
193 {
194 void recalculate()
195 {
196 linear_state<2>::recalculate();
197 rotational_state2::recalculate();
198 }
199
200 void update(scalar t, scalar dt)
201 {
202 rk4<linear_state<2>,linear_state<2>::derivative>(*this, t, dt);
203 }
204 };
205
206 struct state3 : public linear_state<3>, public rotational_state3
207 {
208 void recalculate()
209 {
210 linear_state<3>::recalculate();
211 rotational_state3::recalculate();
212 }
213
214 void update(scalar t, scalar dt)
215 {
216 rk4<linear_state<3>,linear_state<3>::derivative>(*this, t, dt);
217 }
218 };
219
220 template <class T>
221 inline T interpolate(const T& a, const T& b, scalar alpha)
222 {
223 return lerp(a, b, alpha);
224 }
225
226 template <>
227 inline state2
228 interpolate<state2>(const state2& a, const state2& b, scalar alpha)
229 {
230 state2 state(b);
231 state.position = interpolate(a.position, b.position, alpha);
232 state.momentum = interpolate(a.momentum, b.momentum, alpha);
233 state.orientation = interpolate(a.orientation, b.orientation, alpha);
234 state.angular_momentum = interpolate(a.angular_momentum,
235 b.angular_momentum, alpha);
236 return state;
237 }
238
239 template <>
240 inline state3
241 interpolate<state3>(const state3& a, const state3& b, scalar alpha)
242 {
243 state3 state(b);
244 state.position = interpolate(a.position, b.position, alpha);
245 state.momentum = interpolate(a.momentum, b.momentum, alpha);
246 state.orientation = slerp(a.orientation, b.orientation, alpha);
247 state.angular_momentum = interpolate(a.angular_momentum,
248 b.angular_momentum, alpha);
249 return state;
250 }
251
252 /**
253 * Interface for anything that can move.
254 */
255 template <class T>
256 class rigid_body : public entity
257 {
258 protected:
259
260 T state_;
261 T prev_state_;
262
263 public:
264
265 virtual ~rigid_body() {}
266
267 virtual void update(scalar t, scalar dt)
268 {
269 prev_state_ = state_;
270 state_.update(t, dt);
271 }
272
273 const T& state() const
274 {
275 return state_;
276 }
277
278 T state(scalar alpha) const
279 {
280 return interpolate(prev_state_, state_, alpha);
281 }
282
283 const T& getLastState() const
284 {
285 return prev_state_;
286 }
287 };
288
289 typedef rigid_body<state2> rigid_body2;
290 typedef rigid_body<state3> rigid_body3;
291
292
293 } // namespace moof
294
295 #endif // _MOOF_RIGID_BODY_HH_
296
This page took 0.042215 seconds and 3 git commands to generate.