2 /*] Copyright (c) 2009-2011, Charles McGarvey [*****************************
3 **] All rights reserved.
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.
8 *****************************************************************************/
10 #ifndef _MOOF_RIGID_BODY_HH_
11 #define _MOOF_RIGID_BODY_HH_
15 #include <boost/bind.hpp>
16 #include <boost/function.hpp>
18 #include <moof/entity.hh>
19 #include <moof/math.hh>
24 * Classes and functions for simulating rigid body physics.
30 extern scalar global_acceleration
;
35 typedef moof::vector
< scalar
, fixed
<D
> > vector
;
36 typedef boost::function
<const vector
& (const linear_state
&)>
48 std::vector
<force_function
> forces
;
56 velocity
= momentum
* inverse_mass
;
66 const vector
& operator () (const linear_state
& state
)
68 force
[1] = state
.mass
* global_acceleration
;
88 inverse_mass
= SCALAR(1.0);
96 derivative
operator * (scalar dt
) const
98 derivative derivative
;
99 derivative
.velocity
= dt
* velocity
;
100 derivative
.force
= dt
* force
;
104 derivative
operator + (const derivative
& other
) const
106 derivative derivative
;
107 derivative
.velocity
= velocity
+ other
.velocity
;
108 derivative
.force
= force
+ other
.force
;
113 vector
total_force() const
117 typename
std::vector
<force_function
>::const_iterator it
;
118 for (it
= forces
.begin(); it
!= forces
.end(); ++it
)
126 void calculate_derivative(derivative
& derivative
, scalar t
) const
128 derivative
.velocity
= velocity
;
129 derivative
.force
= total_force();
132 void step(const derivative
& derivative
, scalar dt
)
134 position
+= dt
* derivative
.velocity
;
135 momentum
+= dt
* derivative
.force
;
140 struct rotational_state2
144 scalar angular_momentum
;
147 scalar angular_velocity
;
151 scalar inverse_inertia
;
155 angular_velocity
= angular_momentum
* inertia
;
160 scalar angular_velocity
;
164 void step(const derivative
& derivative
, scalar dt
)
166 orientation
+= dt
* derivative
.angular_velocity
;
167 angular_momentum
+= dt
* derivative
.torque
;
172 struct rotational_state3
175 quaternion orientation
;
176 vector3 angular_momentum
;
180 vector3 angular_velocity
;
184 scalar inverse_inertia
;
188 angular_velocity
= angular_momentum
* inertia
;
193 struct state2
: public linear_state
<2>, public rotational_state2
197 linear_state
<2>::recalculate();
198 rotational_state2::recalculate();
201 void update(scalar t
, scalar dt
)
203 rk4
<linear_state
<2>,linear_state
<2>::derivative
>(*this, t
, dt
);
207 struct state3
: public linear_state
<3>, public rotational_state3
211 linear_state
<3>::recalculate();
212 rotational_state3::recalculate();
215 void update(scalar t
, scalar dt
)
217 rk4
<linear_state
<3>,linear_state
<3>::derivative
>(*this, t
, dt
);
222 inline T
interpolate(const T
& a
, const T
& b
, scalar alpha
)
224 return lerp(a
, b
, alpha
);
229 interpolate
<state2
>(const state2
& a
, const state2
& b
, scalar alpha
)
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
);
242 interpolate
<state3
>(const state3
& a
, const state3
& b
, scalar alpha
)
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
);
254 * Interface for anything that can move.
257 class rigid_body
: public entity
261 virtual ~rigid_body() {}
263 virtual void update(scalar t
, scalar dt
)
265 prev_state_
= state_
;
266 state_
.update(t
, dt
);
269 const T
& state() const
274 T
state(scalar alpha
) const
276 return interpolate(prev_state_
, state_
, alpha
);
279 const T
& getLastState() const
290 typedef rigid_body
<state2
> rigid_body2
;
291 typedef rigid_body
<state3
> rigid_body3
;
296 #endif // _MOOF_RIGID_BODY_HH_