#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>
-#include <Moof/State.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 physical things with mass, momentum, yada yada yada.
+ * Interface for anything that can move.
*/
-template <typename T>
-class RigidBody
+template <class T>
+class RigidBody : public Entity
{
+protected:
+
+ T mState;
+ T mPrevState;
+
public:
+
virtual ~RigidBody() {}
virtual void update(Scalar t, Scalar dt)
{
- prevState_ = currentState_;
- currentState_.integrate(t, dt);
+ mPrevState = mState;
+ mState.update(t, dt);
}
- T getInterpolatedState(Scalar alpha) const
+ const T& getState() const
{
- return currentState_.interpolate(alpha, prevState_);
+ return mState;
}
-protected:
- T prevState_;
- T currentState_;
+ 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
/** vim: set ts=4 sw=4 tw=80: *************************************************/
-
projects / chaz / yoink / blobdiff