]>
Dogcows Code - chaz/yoink/blob - src/Moof/RigidBody.hh
2 /*******************************************************************************
4 Copyright (c) 2009, Charles McGarvey
7 Redistribution and use in source and binary forms, with or without
8 modification, are permitted provided that the following conditions are met:
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.
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.
27 *******************************************************************************/
29 #ifndef _MOOF_RIGIDBODY_HH_
30 #define _MOOF_RIGIDBODY_HH_
34 #include <boost/bind.hpp>
35 #include <boost/function.hpp>
37 #include <Moof/Entity.hh>
38 #include <Moof/Math.hh>
47 typedef cml::vector
< Scalar
, cml::fixed
<D
> > Vector
;
48 typedef boost::function
<const Vector
& (const LinearState
&)> ForceFunction
;
62 std::vector
<ForceFunction
> forces
;
70 void recalculateLinear()
72 velocity
= momentum
* inverseMass
;
78 explicit GravityForce(Scalar a
= -9.8)
84 const Vector
& operator () (const LinearState
& state
)
86 force
[1] = state
.mass
* acceleration
;
108 inverseMass
= 1.0 / mass
;
117 Derivative
operator*(Scalar dt
) const
119 Derivative derivative
;
120 derivative
.velocity
= dt
* velocity
;
121 derivative
.force
= dt
* force
;
125 Derivative
operator+(const Derivative
& other
) const
127 Derivative derivative
;
128 derivative
.velocity
= velocity
+ other
.velocity
;
129 derivative
.force
= force
+ other
.force
;
135 Vector
getForce() const
139 for (size_t i
= 0; i
< forces
.size(); ++i
)
141 f
+= forces
[i
](*this);
147 void getDerivative(Derivative
& derivative
, Scalar t
) const
149 derivative
.velocity
= velocity
;
150 derivative
.force
= getForce();
153 void step(const Derivative
& derivative
, Scalar dt
)
155 position
+= dt
* derivative
.velocity
;
156 momentum
+= dt
* derivative
.force
;
162 struct RotationalState2
167 Scalar angularMomentum
;
171 Scalar angularVelocity
;
176 Scalar inverseInertia
;
179 void recalculateRotational()
181 angularVelocity
= angularMomentum
* inertia
;
187 Scalar angularVelocity
;
191 void step(const Derivative
& derivative
, Scalar dt
)
193 orientation
+= dt
* derivative
.angularVelocity
;
194 angularMomentum
+= dt
* derivative
.torque
;
195 recalculateRotational();
199 struct RotationalState3
203 Quaternion orientation
;
204 Vector3 angularMomentum
;
209 Vector3 angularVelocity
;
214 Scalar inverseInertia
;
217 void recalculateRotational()
219 angularVelocity
= angularMomentum
* inertia
;
224 struct State2
: public LinearState
<2>, public RotationalState2
229 recalculateRotational();
232 void update(Scalar t
, Scalar dt
)
234 rk4
<LinearState
<2>,LinearState
<2>::Derivative
>(*this, t
, dt
);
238 struct State3
: public LinearState
<3>, public RotationalState3
243 recalculateRotational();
246 void update(Scalar t
, Scalar dt
)
248 rk4
<LinearState
<3>,LinearState
<3>::Derivative
>(*this, t
, dt
);
254 inline T
interpolate(const T
& a
, const T
& b
, Scalar alpha
)
256 return cml::lerp(a
, b
, alpha
);
260 inline State2 interpolate
<State2
>(const State2
& a
, const State2
& b
, Scalar alpha
)
263 state
.position
= interpolate(a
.position
, b
.position
, alpha
);
264 state
.momentum
= interpolate(a
.momentum
, b
.momentum
, alpha
);
265 state
.orientation
= interpolate(a
.orientation
, b
.orientation
, alpha
);
266 state
.angularMomentum
= interpolate(a
.angularMomentum
, b
.angularMomentum
,
272 inline State3 interpolate
<State3
>(const State3
& a
, const State3
& b
, Scalar alpha
)
275 state
.position
= interpolate(a
.position
, b
.position
, alpha
);
276 state
.momentum
= interpolate(a
.momentum
, b
.momentum
, alpha
);
277 state
.orientation
= cml::slerp(a
.orientation
, b
.orientation
, alpha
);
278 state
.angularMomentum
= interpolate(a
.angularMomentum
, b
.angularMomentum
,
286 * Interface for anything that can move.
290 class RigidBody
: public Entity
299 virtual ~RigidBody() {}
301 virtual void update(Scalar t
, Scalar dt
)
304 mState
.update(t
, dt
);
307 const T
& getState() const
312 T
getState(Scalar alpha
) const
314 return interpolate(mPrevState
, mState
, alpha
);
317 const T
& getLastState() const
323 typedef RigidBody
<State2
> RigidBody2
;
324 typedef RigidBody
<State3
> RigidBody3
;
329 #endif // _MOOF_RIGIDBODY_HH_
331 /** vim: set ts=4 sw=4 tw=80: *************************************************/
This page took 0.052249 seconds and 4 git commands to generate.