X-Git-Url: https://git.dogcows.com/gitweb?p=chaz%2Fyoink;a=blobdiff_plain;f=src%2FMoof%2FRK4.hh;h=453cf1ef4e1d00766bcb4b27383784a4082917e6;hp=a2e0f1cbe713c62e0eb9b6267c2b2febaecd065a;hb=fcb40aa40c6a13ca0e0962b35973ac4574779574;hpb=d50942708db230dc5c43b8df89ede45525e1c394

diff --git a/src/Moof/RK4.hh b/src/Moof/RK4.hh
index a2e0f1c..453cf1e 100644
--- a/src/Moof/RK4.hh
+++ b/src/Moof/RK4.hh
@@ -29,17 +29,23 @@
 #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 implementation of the RK4 integrator.  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:
+
+// 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 applyDerivative(const Derivative_Type& derivative, Scalar deltaTime);
+// void step(const Derivative_Type& derivative, Scalar deltaTime);
 //
 // Additionally, the derivative class must overload a few operators:
 //
@@ -55,26 +61,236 @@ inline D evaluate(const S& state, Scalar t)
 }
 
 template<typename S, typename D>
-inline D evaluate(const S& state,  Scalar t, Scalar dt, const D& derivative)
+inline D evaluate(S state,  Scalar t, Scalar dt, const D& derivative)
 {
-	S temp = state;
-	temp.applyDerivative(derivative, dt);
-	return evaluate<S,D>(temp, t + dt);
+	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 integrate(S& state, Scalar t, Scalar dt)
+inline void rk4(S& state, Scalar t, Scalar dt)
 {
 	D a = evaluate<S,D>(state, t);
-	D b = evaluate<S,D>(state, t, dt * 0.5, a);
-	D c = evaluate<S,D>(state, t, dt * 0.5, b);
+	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.applyDerivative((a + (b + c) * 2.0 + d) * (1.0/6.0), dt);
+	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_