X-Git-Url: https://git.dogcows.com/gitweb?p=chaz%2Fyoink;a=blobdiff_plain;f=src%2FMoof%2FRigidBody.hh;h=e7dee3861652d4c0baa30cd43002db8ae4a8e8da;hp=11c635420b42b16349c30b38c094dddc999d86ee;hb=c9e20ac06383b20ceb5404c9237e319c2e90d157;hpb=bfa6212d09d8735d8fd5e2638188e4a99f21ada4

diff --git a/src/Moof/RigidBody.hh b/src/Moof/RigidBody.hh
index 11c6354..e7dee38 100644
--- a/src/Moof/RigidBody.hh
+++ b/src/Moof/RigidBody.hh
@@ -29,39 +29,300 @@
 #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:
-	inline virtual ~RigidBody() {}
+
+	virtual ~RigidBody() {}
 
 	virtual void update(Scalar t, Scalar dt)
 	{
-		prevState_ = currentState_;
-		currentState_.integrate(t, dt);
+		mPrevState = mState;
+		mState.update(t, dt);
 	}
 
-	inline 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
 
@@ -69,4 +330,3 @@ protected:
 
 /** vim: set ts=4 sw=4 tw=80: *************************************************/
 
-