+++ /dev/null
-
-/*******************************************************************************
-
- Copyright (c) 2009, Charles McGarvey
- All rights reserved.
-
- Redistribution and use in source and binary forms, with or without
- modification, are permitted provided that the following conditions are met:
-
- * Redistributions of source code must retain the above copyright notice,
- this list of conditions and the following disclaimer.
- * Redistributions in binary form must reproduce the above copyright notice,
- this list of conditions and the following disclaimer in the documentation
- and/or other materials provided with the distribution.
-
- THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
- DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
- FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
- DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
- SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
- OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
- OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-
-*******************************************************************************/
-
-#ifndef _MOOF_RK4_HH_
-#define _MOOF_RK4_HH_
-
-#include <vector>
-
-#include <boost/bind.hpp>
-#include <boost/function.hpp>
-
-#include <Moof/Math.hh>
-
-
-namespace Mf {
-
-
-// Generic implementations of a few simple integrators. To use, you need one
-// type representing the state and another containing the derivatives of the
-// primary state variables. The state class must implement these methods:
-//
-// void getDerivative(Derivative_Type& derivative, Scalar absoluteTime);
-// void step(const Derivative_Type& derivative, Scalar deltaTime);
-//
-// Additionally, the derivative class must overload a few operators:
-//
-// Derivative_Type operator+(const Derivative_Type& other) const
-// Derivative_Type operator*(const Derivative_Type& other) const
-
-template<typename S, typename D>
-inline D evaluate(const S& state, Scalar t)
-{
- D derivative;
- state.getDerivative(derivative, t);
- return derivative;
-}
-
-template<typename S, typename D>
-inline D evaluate(S state, Scalar t, Scalar dt, const D& derivative)
-{
- state.step(derivative, dt);
- return evaluate<S,D>(state, t + dt);
-}
-
-
-template<typename S, typename D>
-inline void euler(S& state, Scalar t, Scalar dt)
-{
- D a = evaluate<S,D>(state, t);
-
- state.step(a, dt);
-}
-
-template<typename S, typename D>
-inline void rk2(S& state, Scalar t, Scalar dt)
-{
- D a = evaluate<S,D>(state, t);
- D b = evaluate<S,D>(state, t, dt * SCALAR(0.5), a);
-
- state.step(b, dt);
-}
-
-template<typename S, typename D>
-inline void rk4(S& state, Scalar t, Scalar dt)
-{
- D a = evaluate<S,D>(state, t);
- D b = evaluate<S,D>(state, t, dt * SCALAR(0.5), a);
- D c = evaluate<S,D>(state, t, dt * SCALAR(0.5), b);
- D d = evaluate<S,D>(state, t, dt, c);
-
- state.step((a + (b + c) * SCALAR(2.0) + d) * SCALAR(1.0/6.0), dt);
-}
-
-
-//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-
-
-template <int D = 3>
-struct LinearState
-{
- typedef cml::vector< Scalar, cml::fixed<D> > Vector;
- typedef boost::function<const Vector& (const LinearState&)> ForceFunction;
-
- // primary
-
- Vector position;
- Vector momentum;
-
- // secondary
-
- Vector velocity;
-
- // user
-
- Vector force;
- std::vector<ForceFunction> forces;
-
- // constant
-
- Scalar mass;
- Scalar inverseMass;
-
-
- void recalculateLinear()
- {
- velocity = momentum * inverseMass;
- }
-
-
- struct GravityForce
- {
- explicit GravityForce(Scalar a = -9.8)
- {
- force.zero();
- acceleration = a;
- }
-
- const Vector& operator () (const LinearState& state)
- {
- force[1] = state.mass * acceleration;
- return force;
- }
-
- private:
-
- Vector force;
- Scalar acceleration;
- };
-
-
- void init()
- {
- position.zero();
- momentum.zero();
-
- velocity.zero();
-
- force.zero();
- forces.clear();
-
- mass = SCALAR(1.0);
- inverseMass = 1.0 / mass;
- }
-
-
- struct Derivative
- {
- Vector velocity;
- Vector force;
-
- Derivative operator*(Scalar dt) const
- {
- Derivative derivative;
- derivative.velocity = dt * velocity;
- derivative.force = dt * force;
- return derivative;
- }
-
- Derivative operator+(const Derivative& other) const
- {
- Derivative derivative;
- derivative.velocity = velocity + other.velocity;
- derivative.force = force + other.force;
- return derivative;
- }
- };
-
-
- Vector getForce() const
- {
- Vector f(force);
-
- for (size_t i = 0; i < forces.size(); ++i)
- {
- f += forces[i](*this);
- }
-
- return f;
- }
-
- void getDerivative(Derivative& derivative, Scalar t) const
- {
- derivative.velocity = velocity;
- derivative.force = getForce();
- }
-
- void step(const Derivative& derivative, Scalar dt)
- {
- position += dt * derivative.velocity;
- momentum += dt * derivative.force;
- recalculateLinear();
- }
-};
-
-
-struct RotationalState2
-{
- // primary
-
- Scalar orientation;
- Scalar angularMomentum;
-
- // secondary
-
- Scalar angularVelocity;
-
- // constant
-
- Scalar inertia;
- Scalar inverseInertia;
-
-
- void recalculateRotational()
- {
- angularVelocity = angularMomentum * inertia;
- }
-
-
- struct Derivative
- {
- Scalar angularVelocity;
- Scalar torque;
- };
-
- void step(const Derivative& derivative, Scalar dt)
- {
- orientation += dt * derivative.angularVelocity;
- angularMomentum += dt * derivative.torque;
- recalculateRotational();
- }
-};
-
-struct RotationalState3
-{
- // primary
-
- Quaternion orientation;
- Vector3 angularMomentum;
-
- // secondary
-
- Quaternion spin;
- Vector3 angularVelocity;
-
- // constant
-
- Scalar inertia;
- Scalar inverseInertia;
-};
-
-
-struct State2 : public LinearState<2>, public RotationalState2
-{
- void recalculate()
- {
- recalculateLinear();
- recalculateRotational();
- }
-
- void integrate(Scalar t, Scalar dt)
- {
- rk4<LinearState<2>,LinearState<2>::Derivative>(*this, t, dt);
- }
-};
-
-struct State3 : public LinearState<3>, public RotationalState3 {};
-
-
-} // namespace Mf
-
-#endif // _MOOF_RK4_HH_
-
-/** vim: set ts=4 sw=4 tw=80: *************************************************/
-
projects / chaz / yoink / blobdiff