Dogcows Code - chaz/yoink/blob - src/moof/rigid_body.hh

   1
   2 /*]  Copyright (c) 2009-2011, Charles McGarvey  [*****************************
   3 **]  All rights reserved.
   4 *
   5 * Distributable under the terms and conditions of the 2-clause BSD license;
   6 * see the file COPYING for a complete text of the license.
   7 *
   8 *****************************************************************************/
   9
  10 #ifndef _MOOF_RIGID_BODY_HH_
  11 #define _MOOF_RIGID_BODY_HH_
  12
  13 #include <vector>
  14
  15 #include <boost/bind.hpp>
  16 #include <boost/function.hpp>
  17
  18 #include <moof/entity.hh>
  19 #include <moof/math.hh>
  20
  21
  22 /**
  23  * \file rigid_body.hh
  24  * Classes and functions for simulating rigid body physics.
  25  */
  26
  27 namespace moof {
  28
  29
  30 template <int D = 3>
  31 struct linear_state
  32 {
  33         typedef moof::vector< scalar, fixed<D> > vector;
  34         typedef boost::function<const vector& (const linear_state&)>
  35                 force_function;
  36
  37         // primary
  38         vector          position;
  39         vector          momentum;
  40
  41         // secondary
  42         vector          velocity;
  43
  44         // user
  45         vector          force;
  46         std::vector<force_function> forces;
  47
  48         // constant
  49         scalar          mass;
  50         scalar          inverse_mass;
  51
  52         void recalculate()
  53         {
  54                 velocity = momentum * inverse_mass;
  55         }
  56
  57         struct gravity_force
  58         {
  59                 explicit gravity_force(scalar a = -9.8)
  60                 {
  61                         force.zero();
  62                         acceleration = a;
  63                 }
  64
  65                 const vector& operator () (const linear_state& state)
  66                 {
  67                         force[1] = state.mass * acceleration;
  68                         return force;
  69                 }
  70
  71         private:
  72
  73                 vector force;
  74                 scalar acceleration;
  75         };
  76
  77         void init()
  78         {
  79                 position.zero();
  80                 momentum.zero();
  81
  82                 velocity.zero();
  83
  84                 force.zero();
  85                 forces.clear();
  86
  87                 mass = SCALAR(1.0);
  88                 inverse_mass = 1.0 / mass;
  89         }
  90
  91         struct derivative
  92         {
  93                 vector  velocity;
  94                 vector  force;
  95
  96                 derivative operator * (scalar dt) const
  97                 {
  98                         derivative derivative;
  99                         derivative.velocity = dt * velocity;
 100                         derivative.force = dt * force;
 101                         return derivative;
 102                 }
 103
 104                 derivative operator + (const derivative& other) const
 105                 {
 106                         derivative derivative;
 107                         derivative.velocity = velocity + other.velocity;
 108                         derivative.force = force + other.force;
 109                         return derivative;
 110                 }
 111         };
 112
 113         vector total_force() const
 114         {
 115                 vector f(force);
 116
 117                 for (size_t i = 0; i < forces.size(); ++i)
 118                 {
 119                         f += forces[i](*this);
 120                 }
 121
 122                 return f;
 123         }
 124
 125         void calculate_derivative(derivative& derivative, scalar t) const
 126         {
 127                 derivative.velocity = velocity;
 128                 derivative.force = total_force();
 129         }
 130
 131         void step(const derivative& derivative, scalar dt)
 132         {
 133                 position += dt * derivative.velocity;
 134                 momentum += dt * derivative.force;
 135                 recalculate();
 136         }
 137 };
 138
 139 struct rotational_state2
 140 {
 141         // primary
 142         scalar          orientation;
 143         scalar          angular_momentum;
 144
 145         // secondary
 146         scalar          angularVelocity;
 147
 148         // constant
 149         scalar          inertia;
 150         scalar          inverse_inertia;
 151
 152         void recalculate()
 153         {
 154                 angularVelocity = angular_momentum * inertia;
 155         }
 156
 157         struct derivative
 158         {
 159                 scalar  angularVelocity;
 160                 scalar  torque;
 161         };
 162
 163         void step(const derivative& derivative, scalar dt)
 164         {
 165                 orientation += dt * derivative.angularVelocity;
 166                 angular_momentum += dt * derivative.torque;
 167                 recalculate();
 168         }
 169 };
 170
 171 struct rotational_state3
 172 {
 173         // primary
 174         quaternion      orientation;
 175         vector3         angular_momentum;
 176
 177         // secondary
 178         quaternion      spin;
 179         vector3         angularVelocity;
 180
 181         // constant
 182         scalar          inertia;
 183         scalar          inverse_inertia;
 184
 185         void recalculate()
 186         {
 187                 angularVelocity = angular_momentum * inertia;
 188         }
 189 };
 190
 191
 192 struct state2 : public linear_state<2>, public rotational_state2
 193 {
 194         void recalculate()
 195         {
 196                 linear_state<2>::recalculate();
 197                 rotational_state2::recalculate();
 198         }
 199
 200         void update(scalar t, scalar dt)
 201         {
 202                 rk4<linear_state<2>,linear_state<2>::derivative>(*this, t, dt);
 203         }
 204 };
 205
 206 struct state3 : public linear_state<3>, public rotational_state3
 207 {
 208         void recalculate()
 209         {
 210                 linear_state<3>::recalculate();
 211                 rotational_state3::recalculate();
 212         }
 213
 214         void update(scalar t, scalar dt)
 215         {
 216                 rk4<linear_state<3>,linear_state<3>::derivative>(*this, t, dt);
 217         }
 218 };
 219
 220 template <class T>
 221 inline T interpolate(const T& a, const T& b, scalar alpha)
 222 {
 223         return lerp(a, b, alpha);
 224 }
 225
 226 template <>
 227 inline state2
 228 interpolate<state2>(const state2& a, const state2& b, scalar alpha)
 229 {
 230         state2 state(b);
 231         state.position = interpolate(a.position, b.position, alpha);
 232         state.momentum = interpolate(a.momentum, b.momentum, alpha);
 233         state.orientation = interpolate(a.orientation, b.orientation, alpha);
 234         state.angular_momentum = interpolate(a.angular_momentum,
 235                         b.angular_momentum, alpha);
 236         return state;
 237 }
 238
 239 template <>
 240 inline state3
 241 interpolate<state3>(const state3& a, const state3& b, scalar alpha)
 242 {
 243         state3 state(b);
 244         state.position = interpolate(a.position, b.position, alpha);
 245         state.momentum = interpolate(a.momentum, b.momentum, alpha);
 246         state.orientation = slerp(a.orientation, b.orientation, alpha);
 247         state.angular_momentum = interpolate(a.angular_momentum,
 248                         b.angular_momentum, alpha);
 249         return state;
 250 }
 251
 252 /**
 253  * Interface for anything that can move.
 254  */
 255 template <class T>
 256 class rigid_body : public entity
 257 {
 258 protected:
 259
 260         T       state_;
 261         T       prev_state_;
 262
 263 public:
 264
 265         virtual ~rigid_body() {}
 266
 267         virtual void update(scalar t, scalar dt)
 268         {
 269                 prev_state_ = state_;
 270                 state_.update(t, dt);
 271         }
 272
 273         const T& state() const
 274         {
 275                 return state_;
 276         }
 277
 278         T state(scalar alpha) const
 279         {
 280                 return interpolate(prev_state_, state_, alpha);
 281         }
 282
 283         const T& getLastState() const
 284         {
 285                 return prev_state_;
 286         }
 287 };
 288
 289 typedef rigid_body<state2> rigid_body2;
 290 typedef rigid_body<state3> rigid_body3;
 291
 292
 293 } // namespace moof
 294
 295 #endif // _MOOF_RIGID_BODY_HH_
 296