]>
Dogcows Code - chaz/yoink/blob - src/moof/rigid_body.hh
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 * Classes and functions for simulating rigid body physics.
20 #include <boost/bind.hpp>
21 #include <boost/function.hpp>
23 #include <moof/entity.hh>
24 #include <moof/math.hh>
33 typedef moof::vector
< scalar
, fixed
<D
> > vector
;
34 typedef boost::function
<const vector
& (const linear_state
&)>
46 std::vector
<force_function
> forces
;
54 velocity
= momentum
* inverse_mass
;
59 explicit gravity_force(scalar a
= -9.8)
65 const vector
& operator () (const linear_state
& state
)
67 force
[1] = state
.mass
* acceleration
;
88 inverse_mass
= 1.0 / mass
;
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 for (size_t i
= 0; i
< forces
.size(); ++i
)
119 f
+= forces
[i
](*this);
125 void calculate_derivative(derivative
& derivative
, scalar t
) const
127 derivative
.velocity
= velocity
;
128 derivative
.force
= total_force();
131 void step(const derivative
& derivative
, scalar dt
)
133 position
+= dt
* derivative
.velocity
;
134 momentum
+= dt
* derivative
.force
;
139 struct rotational_state2
143 scalar angular_momentum
;
146 scalar angularVelocity
;
150 scalar inverse_inertia
;
154 angularVelocity
= angular_momentum
* inertia
;
159 scalar angularVelocity
;
163 void step(const derivative
& derivative
, scalar dt
)
165 orientation
+= dt
* derivative
.angularVelocity
;
166 angular_momentum
+= dt
* derivative
.torque
;
171 struct rotational_state3
174 quaternion orientation
;
175 vector3 angular_momentum
;
179 vector3 angularVelocity
;
183 scalar inverse_inertia
;
187 angularVelocity
= angular_momentum
* inertia
;
192 struct state2
: public linear_state
<2>, public rotational_state2
196 linear_state
<2>::recalculate();
197 rotational_state2::recalculate();
200 void update(scalar t
, scalar dt
)
202 rk4
<linear_state
<2>,linear_state
<2>::derivative
>(*this, t
, dt
);
206 struct state3
: public linear_state
<3>, public rotational_state3
210 linear_state
<3>::recalculate();
211 rotational_state3::recalculate();
214 void update(scalar t
, scalar dt
)
216 rk4
<linear_state
<3>,linear_state
<3>::derivative
>(*this, t
, dt
);
221 inline T
interpolate(const T
& a
, const T
& b
, scalar alpha
)
223 return lerp(a
, b
, alpha
);
228 interpolate
<state2
>(const state2
& a
, const state2
& b
, scalar alpha
)
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
);
241 interpolate
<state3
>(const state3
& a
, const state3
& b
, scalar alpha
)
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
);
253 * Interface for anything that can move.
256 class rigid_body
: public entity
265 virtual ~rigid_body() {}
267 virtual void update(scalar t
, scalar dt
)
269 prev_state_
= state_
;
270 state_
.update(t
, dt
);
273 const T
& state() const
278 T
state(scalar alpha
) const
280 return interpolate(prev_state_
, state_
, alpha
);
283 const T
& getLastState() const
289 typedef rigid_body
<state2
> rigid_body2
;
290 typedef rigid_body
<state3
> rigid_body3
;
295 #endif // _MOOF_RIGID_BODY_HH_
This page took 0.052248 seconds and 5 git commands to generate.