+++ /dev/null
-
-/*] Copyright (c) 2009-2010, Charles McGarvey [**************************
-**] All rights reserved.
-*
-* vi:ts=4 sw=4 tw=75
-*
-* Distributable under the terms and conditions of the 2-clause BSD license;
-* see the file COPYING for a complete text of the license.
-*
-**************************************************************************/
-
-#ifndef _MOOF_RIGIDBODY_HH_
-#define _MOOF_RIGIDBODY_HH_
-
-#include <vector>
-
-#include <boost/bind.hpp>
-#include <boost/function.hpp>
-
-#include <Moof/Entity.hh>
-#include <Moof/Math.hh>
-
-
-namespace Mf {
-
-
-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;
-
-
- void recalculateRotational()
- {
- angularVelocity = angularMomentum * inertia;
- }
-};
-
-
-struct State2 : public LinearState<2>, public RotationalState2
-{
- void recalculate()
- {
- recalculateLinear();
- recalculateRotational();
- }
-
- void update(Scalar t, Scalar dt)
- {
- rk4<LinearState<2>,LinearState<2>::Derivative>(*this, t, dt);
- }
-};
-
-struct State3 : public LinearState<3>, public RotationalState3
-{
- void recalculate()
- {
- recalculateLinear();
- recalculateRotational();
- }
-
- void update(Scalar t, Scalar dt)
- {
- rk4<LinearState<3>,LinearState<3>::Derivative>(*this, t, dt);
- }
-};
-
-
-template <class T>
-inline T interpolate(const T& a, const T& b, Scalar alpha)
-{
- return cml::lerp(a, b, alpha);
-}
-
-template <>
-inline State2 interpolate<State2>(const State2& a, const State2& b,
- Scalar alpha)
-{
- State2 state(b);
- state.position = interpolate(a.position, b.position, alpha);
- state.momentum = interpolate(a.momentum, b.momentum, alpha);
- state.orientation = interpolate(a.orientation, b.orientation, alpha);
- state.angularMomentum = interpolate(a.angularMomentum,
- b.angularMomentum, alpha);
- return state;
-}
-
-template <>
-inline State3 interpolate<State3>(const State3& a, const State3& b,
- Scalar alpha)
-{
- State3 state(b);
- state.position = interpolate(a.position, b.position, alpha);
- state.momentum = interpolate(a.momentum, b.momentum, alpha);
- state.orientation = cml::slerp(a.orientation, b.orientation, alpha);
- state.angularMomentum = interpolate(a.angularMomentum,
- b.angularMomentum, alpha);
- return state;
-}
-
-
-
-/**
- * Interface for anything that can move.
- */
-
-template <class T>
-class RigidBody : public Entity
-{
-protected:
-
- T mState;
- T mPrevState;
-
-public:
-
- virtual ~RigidBody() {}
-
- virtual void update(Scalar t, Scalar dt)
- {
- mPrevState = mState;
- mState.update(t, dt);
- }
-
- const T& getState() const
- {
- return mState;
- }
-
- T getState(Scalar alpha) const
- {
- return interpolate(mPrevState, mState, alpha);
- }
-
- const T& getLastState() const
- {
- return mPrevState;
- }
-};
-
-typedef RigidBody<State2> RigidBody2;
-typedef RigidBody<State3> RigidBody3;
-
-
-} // namespace Mf
-
-#endif // _MOOF_RIGIDBODY_HH_
-
projects / chaz / yoink / blobdiff