global gravity for rigid bodies

[chaz/yoink] / src / moof / rigid_body.hh

diff --git a/src/moof/rigid_body.hh b/src/moof/rigid_body.hh
index 65e502c8483ae1530c594882b1adb4e1258254f1..7ed0a88131089dc45badd3c872eb7570928c67dc 100644 (file)
--- a/src/moof/rigid_body.hh
+++ b/src/moof/rigid_body.hh
@@ -1,22 +1,15 @@
 
-/*]  Copyright (c) 2009-2010, Charles McGarvey  [**************************
+/*]  Copyright (c) 2009-2011, 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>
@@ -26,64 +19,61 @@
 #include <moof/math.hh>
 
 
+/**
+ * \file rigid_body.hh
+ * Classes and functions for simulating rigid body physics.
+ */
+
 namespace moof {
 
 
-template <int D = 3>
+extern scalar global_acceleration;
+
+template <int D>
 struct linear_state
 {
        typedef moof::vector< scalar, fixed<D> > vector;
        typedef boost::function<const vector& (const linear_state&)>
-                                                                                                       force_function;
+               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)
+               gravity_force()
                {
                        force.zero();
-                       acceleration = a;
                }
 
                const vector& operator () (const linear_state& state)
                {
-                       force[1] = state.mass * acceleration;
+                       force[1] = state.mass * global_acceleration;
                        return force;
                }
 
-
        private:
 
-               vector force;
-               scalar acceleration;
+               vector  force;
        };
 
-
        void init()
        {
                position.zero();
@@ -95,10 +85,9 @@ struct linear_state
                forces.clear();
 
                mass = SCALAR(1.0);
-               inverse_mass = 1.0 / mass;
+               inverse_mass = SCALAR(1.0);
        }
 
-
        struct derivative
        {
                vector  velocity;
@@ -121,14 +110,14 @@ struct linear_state
                }
        };
 
-
        vector total_force() const
        {
                vector f(force);
 
-               for (size_t i = 0; i < forces.size(); ++i)
+               typename std::vector<force_function>::const_iterator it;
+               for (it = forces.begin(); it != forces.end(); ++it)
                {
-                       f += forces[i](*this);
+                       f += (*it)(*this);
                }
 
                return f;
@@ -148,39 +137,33 @@ struct linear_state
        }
 };
 
-
 struct rotational_state2
 {
        // primary
-
        scalar          orientation;
        scalar          angular_momentum;
 
        // secondary
-
-       scalar          angularVelocity;
+       scalar          angular_velocity;
 
        // constant
-
        scalar          inertia;
        scalar          inverse_inertia;
 
-
        void recalculate()
        {
-               angularVelocity = angular_momentum * inertia;
+               angular_velocity = angular_momentum * inertia;
        }
 
-
        struct derivative
        {
-               scalar  angularVelocity;
+               scalar  angular_velocity;
                scalar  torque;
        };
 
        void step(const derivative& derivative, scalar dt)
        {
-               orientation += dt * derivative.angularVelocity;
+               orientation += dt * derivative.angular_velocity;
                angular_momentum += dt * derivative.torque;
                recalculate();
        }
@@ -189,24 +172,20 @@ struct rotational_state2
 struct rotational_state3
 {
        // primary
-
        quaternion      orientation;
        vector3         angular_momentum;
 
        // secondary
-
        quaternion      spin;
-       vector3         angularVelocity;
+       vector3         angular_velocity;
 
        // constant
-
        scalar          inertia;
        scalar          inverse_inertia;
 
-
        void recalculate()
        {
-               angularVelocity = angular_momentum * inertia;
+               angular_velocity = angular_momentum * inertia;
        }
 };
 
@@ -239,7 +218,6 @@ struct state3 : public linear_state<3>, public rotational_state3
        }
 };
 
-
 template <class T>
 inline T interpolate(const T& a, const T& b, scalar alpha)
 {
@@ -247,45 +225,37 @@ inline T interpolate(const T& a, const T& b, scalar alpha)
 }
 
 template <>
-inline state2 interpolate<state2>(const state2& a, const state2& b,
-                                                                 scalar alpha)
+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);
+                       b.angular_momentum, alpha);
        return state;
 }
 
 template <>
-inline state3 interpolate<state3>(const state3& a, const state3& b,
-                                                                 scalar alpha)
+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);
+                       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() {}
@@ -310,6 +280,11 @@ public:
        {
                return prev_state_;
        }
+
+protected:
+
+       T       state_;
+       T       prev_state_;
 };
 
 typedef rigid_body<state2> rigid_body2;