X-Git-Url: https://git.dogcows.com/gitweb?p=chaz%2Fyoink;a=blobdiff_plain;f=src%2Fmoof%2Frigid_body.hh;fp=src%2Fmoof%2Frigid_body.hh;h=65e502c8483ae1530c594882b1adb4e1258254f1;hp=0000000000000000000000000000000000000000;hb=831f04d4bc19a390415ac0bbac4331c7a65509bc;hpb=299af4f2047e767e5d79501c26444473bda64c64

diff --git a/src/moof/rigid_body.hh b/src/moof/rigid_body.hh
new file mode 100644
index 0000000..65e502c
--- /dev/null
+++ b/src/moof/rigid_body.hh
@@ -0,0 +1,322 @@
+
+/*]  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_RIGID_BODY_HH_
+#define _MOOF_RIGID_BODY_HH_
+
+/**
+ * \file rigid_body.hh
+ * Classes and functions for simulating rigid body physics.
+ */
+
+#include <vector>
+
+#include <boost/bind.hpp>
+#include <boost/function.hpp>
+
+#include <moof/entity.hh>
+#include <moof/math.hh>
+
+
+namespace moof {
+
+
+template <int D = 3>
+struct linear_state
+{
+	typedef moof::vector< scalar, fixed<D> > vector;
+	typedef boost::function<const vector& (const linear_state&)>
+													force_function;
+
+	// primary
+	
+	vector		position;
+	vector		momentum;
+
+	// secondary
+
+	vector		velocity;
+
+	// user
+
+	vector		force;
+	std::vector<force_function> forces;
+
+	// constant
+	
+	scalar		mass;
+	scalar		inverse_mass;
+
+
+	void recalculate()
+	{
+		velocity = momentum * inverse_mass;
+	}
+
+
+	struct gravity_force
+	{
+		explicit gravity_force(scalar a = -9.8)
+		{
+			force.zero();
+			acceleration = a;
+		}
+
+		const vector& operator () (const linear_state& 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);
+		inverse_mass = 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 total_force() const
+	{
+		vector f(force);
+
+		for (size_t i = 0; i < forces.size(); ++i)
+		{
+			f += forces[i](*this);
+		}
+
+		return f;
+	}
+
+	void calculate_derivative(derivative& derivative, scalar t) const
+	{
+		derivative.velocity = velocity;
+		derivative.force = total_force();
+	}
+
+	void step(const derivative& derivative, scalar dt)
+	{
+		position += dt * derivative.velocity;
+		momentum += dt * derivative.force;
+		recalculate();
+	}
+};
+
+
+struct rotational_state2
+{
+	// primary
+
+	scalar		orientation;
+	scalar		angular_momentum;
+
+	// secondary
+
+	scalar		angularVelocity;
+
+	// constant
+
+	scalar		inertia;
+	scalar		inverse_inertia;
+
+
+	void recalculate()
+	{
+		angularVelocity = angular_momentum * inertia;
+	}
+
+
+	struct derivative
+	{
+		scalar	angularVelocity;
+		scalar	torque;
+	};
+
+	void step(const derivative& derivative, scalar dt)
+	{
+		orientation += dt * derivative.angularVelocity;
+		angular_momentum += dt * derivative.torque;
+		recalculate();
+	}
+};
+
+struct rotational_state3
+{
+	// primary
+
+	quaternion	orientation;
+	vector3		angular_momentum;
+
+	// secondary
+
+	quaternion	spin;
+	vector3		angularVelocity;
+
+	// constant
+
+	scalar		inertia;
+	scalar		inverse_inertia;
+
+
+	void recalculate()
+	{
+		angularVelocity = angular_momentum * inertia;
+	}
+};
+
+
+struct state2 : public linear_state<2>, public rotational_state2
+{
+	void recalculate()
+	{
+		linear_state<2>::recalculate();
+		rotational_state2::recalculate();
+	}
+
+	void update(scalar t, scalar dt)
+	{
+		rk4<linear_state<2>,linear_state<2>::derivative>(*this, t, dt);
+	}
+};
+
+struct state3 : public linear_state<3>, public rotational_state3
+{
+	void recalculate()
+	{
+		linear_state<3>::recalculate();
+		rotational_state3::recalculate();
+	}
+
+	void update(scalar t, scalar dt)
+	{
+		rk4<linear_state<3>,linear_state<3>::derivative>(*this, t, dt);
+	}
+};
+
+
+template <class T>
+inline T interpolate(const T& a, const T& b, scalar alpha)
+{
+	return 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.angular_momentum = interpolate(a.angular_momentum,
+										b.angular_momentum, 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 = slerp(a.orientation, b.orientation, alpha);
+	state.angular_momentum = interpolate(a.angular_momentum,
+										b.angular_momentum, alpha);
+	return state;
+}
+
+
+
+/**
+ * Interface for anything that can move.
+ */
+
+template <class T>
+class rigid_body : public entity
+{
+protected:
+
+	T	state_;
+	T	prev_state_;
+
+public:
+
+	virtual ~rigid_body() {}
+
+	virtual void update(scalar t, scalar dt)
+	{
+		prev_state_ = state_;
+		state_.update(t, dt);
+	}
+
+	const T& state() const
+	{
+		return state_;
+	}
+
+	T state(scalar alpha) const
+	{
+		return interpolate(prev_state_, state_, alpha);
+	}
+
+	const T& getLastState() const
+	{
+		return prev_state_;
+	}
+};
+
+typedef rigid_body<state2> rigid_body2;
+typedef rigid_body<state3> rigid_body3;
+
+
+} // namespace moof
+
+#endif // _MOOF_RIGID_BODY_HH_
+