--- /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_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_
+
projects / chaz / yoink / blobdiff