]>
Dogcows Code - chaz/yoink/blob - src/Moof/RK4.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 *******************************************************************************/
34 #include <boost/bind.hpp>
35 #include <boost/function.hpp>
37 #include <Moof/Math.hh>
43 // Generic implementations of a few simple integrators. To use, you need one
44 // type representing the state and another containing the derivatives of the
45 // primary state variables. The state class must implement these methods:
47 // void getDerivative(Derivative_Type& derivative, Scalar absoluteTime);
48 // void step(const Derivative_Type& derivative, Scalar deltaTime);
50 // Additionally, the derivative class must overload a few operators:
52 // Derivative_Type operator+(const Derivative_Type& other) const
53 // Derivative_Type operator*(const Derivative_Type& other) const
55 template<typename S
, typename D
>
56 inline D
evaluate(const S
& state
, Scalar t
)
59 state
.getDerivative(derivative
, t
);
63 template<typename S
, typename D
>
64 inline D
evaluate(S state
, Scalar t
, Scalar dt
, const D
& derivative
)
66 state
.step(derivative
, dt
);
67 return evaluate
<S
,D
>(state
, t
+ dt
);
71 template<typename S
, typename D
>
72 inline void euler(S
& state
, Scalar t
, Scalar dt
)
74 D a
= evaluate
<S
,D
>(state
, t
);
79 template<typename S
, typename D
>
80 inline void rk2(S
& state
, Scalar t
, Scalar dt
)
82 D a
= evaluate
<S
,D
>(state
, t
);
83 D b
= evaluate
<S
,D
>(state
, t
, dt
* SCALAR(0.5), a
);
88 template<typename S
, typename D
>
89 inline void rk4(S
& state
, Scalar t
, Scalar dt
)
91 D a
= evaluate
<S
,D
>(state
, t
);
92 D b
= evaluate
<S
,D
>(state
, t
, dt
* SCALAR(0.5), a
);
93 D c
= evaluate
<S
,D
>(state
, t
, dt
* SCALAR(0.5), b
);
94 D d
= evaluate
<S
,D
>(state
, t
, dt
, c
);
96 state
.step((a
+ (b
+ c
) * SCALAR(2.0) + d
) * SCALAR(1.0/6.0), dt
);
100 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
106 typedef cml::vector
< Scalar
, cml::fixed
<D
> > Vector
;
107 typedef boost::function
<const Vector
& (const LinearState
&)> ForceFunction
;
121 std::vector
<ForceFunction
> forces
;
129 void recalculateLinear()
131 velocity
= momentum
* inverseMass
;
137 explicit GravityForce(Scalar a
= -9.8)
143 const Vector
& operator () (const LinearState
& state
)
145 force
[1] = state
.mass
* acceleration
;
167 inverseMass
= 1.0 / mass
;
176 Derivative
operator*(Scalar dt
) const
178 Derivative derivative
;
179 derivative
.velocity
= dt
* velocity
;
180 derivative
.force
= dt
* force
;
184 Derivative
operator+(const Derivative
& other
) const
186 Derivative derivative
;
187 derivative
.velocity
= velocity
+ other
.velocity
;
188 derivative
.force
= force
+ other
.force
;
194 Vector
getForce() const
198 for (size_t i
= 0; i
< forces
.size(); ++i
)
200 f
+= forces
[i
](*this);
206 void getDerivative(Derivative
& derivative
, Scalar t
) const
208 derivative
.velocity
= velocity
;
209 derivative
.force
= getForce();
212 void step(const Derivative
& derivative
, Scalar dt
)
214 position
+= dt
* derivative
.velocity
;
215 momentum
+= dt
* derivative
.force
;
221 struct RotationalState2
226 Scalar angularMomentum
;
230 Scalar angularVelocity
;
235 Scalar inverseInertia
;
238 void recalculateRotational()
240 angularVelocity
= angularMomentum
* inertia
;
246 Scalar angularVelocity
;
250 void step(const Derivative
& derivative
, Scalar dt
)
252 orientation
+= dt
* derivative
.angularVelocity
;
253 angularMomentum
+= dt
* derivative
.torque
;
254 recalculateRotational();
258 struct RotationalState3
262 Quaternion orientation
;
263 Vector3 angularMomentum
;
268 Vector3 angularVelocity
;
273 Scalar inverseInertia
;
277 struct State2
: public LinearState
<2>, public RotationalState2
282 recalculateRotational();
285 void integrate(Scalar t
, Scalar dt
)
287 rk4
<LinearState
<2>,LinearState
<2>::Derivative
>(*this, t
, dt
);
291 struct State3
: public LinearState
<3>, public RotationalState3
{};
296 #endif // _MOOF_RK4_HH_
298 /** vim: set ts=4 sw=4 tw=80: *************************************************/
This page took 0.042734 seconds and 5 git commands to generate.