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 extern scalar global_acceleration;
  31
  32 template <int D>
  33 struct linear_state
  34 {
  35         typedef moof::vector< scalar, fixed<D> > vector;
  36         typedef boost::function<const vector& (const linear_state&)>
  37                 force_function;
  38
  39         // primary
  40         vector          position;
  41         vector          momentum;
  42
  43         // secondary
  44         vector          velocity;
  45
  46         // user
  47         vector          force;
  48         std::vector<force_function> forces;
  49
  50         // constant
  51         scalar          mass;
  52         scalar          inverse_mass;
  53
  54         void recalculate()
  55         {
  56                 velocity = momentum * inverse_mass;
  57         }
  58
  59         struct gravity_force
  60         {
  61                 gravity_force()
  62                 {
  63                         force.zero();
  64                 }
  65
  66                 const vector& operator () (const linear_state& state)
  67                 {
  68                         force[1] = state.mass * global_acceleration;
  69                         return force;
  70                 }
  71
  72         private:
  73
  74                 vector  force;
  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 = SCALAR(1.0);
  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                 typename std::vector<force_function>::const_iterator it;
 118                 for (it = forces.begin(); it != forces.end(); ++it)
 119                 {
 120                         f += (*it)(*this);
 121                 }
 122
 123                 return f;
 124         }
 125
 126         void calculate_derivative(derivative& derivative, scalar t) const
 127         {
 128                 derivative.velocity = velocity;
 129                 derivative.force = total_force();
 130         }
 131
 132         void step(const derivative& derivative, scalar dt)
 133         {
 134                 position += dt * derivative.velocity;
 135                 momentum += dt * derivative.force;
 136                 recalculate();
 137         }
 138 };
 139
 140 struct rotational_state2
 141 {
 142         // primary
 143         scalar          orientation;
 144         scalar          angular_momentum;
 145
 146         // secondary
 147         scalar          angular_velocity;
 148
 149         // constant
 150         scalar          inertia;
 151         scalar          inverse_inertia;
 152
 153         void recalculate()
 154         {
 155                 angular_velocity = angular_momentum * inertia;
 156         }
 157
 158         struct derivative
 159         {
 160                 scalar  angular_velocity;
 161                 scalar  torque;
 162         };
 163
 164         void step(const derivative& derivative, scalar dt)
 165         {
 166                 orientation += dt * derivative.angular_velocity;
 167                 angular_momentum += dt * derivative.torque;
 168                 recalculate();
 169         }
 170 };
 171
 172 struct rotational_state3
 173 {
 174         // primary
 175         quaternion      orientation;
 176         vector3         angular_momentum;
 177
 178         // secondary
 179         quaternion      spin;
 180         vector3         angular_velocity;
 181
 182         // constant
 183         scalar          inertia;
 184         scalar          inverse_inertia;
 185
 186         void recalculate()
 187         {
 188                 angular_velocity = angular_momentum * inertia;
 189         }
 190 };
 191
 192
 193 struct state2 : public linear_state<2>, public rotational_state2
 194 {
 195         void recalculate()
 196         {
 197                 linear_state<2>::recalculate();
 198                 rotational_state2::recalculate();
 199         }
 200
 201         void update(scalar t, scalar dt)
 202         {
 203                 rk4<linear_state<2>,linear_state<2>::derivative>(*this, t, dt);
 204         }
 205 };
 206
 207 struct state3 : public linear_state<3>, public rotational_state3
 208 {
 209         void recalculate()
 210         {
 211                 linear_state<3>::recalculate();
 212                 rotational_state3::recalculate();
 213         }
 214
 215         void update(scalar t, scalar dt)
 216         {
 217                 rk4<linear_state<3>,linear_state<3>::derivative>(*this, t, dt);
 218         }
 219 };
 220
 221 template <class T>
 222 inline T interpolate(const T& a, const T& b, scalar alpha)
 223 {
 224         return lerp(a, b, alpha);
 225 }
 226
 227 template <>
 228 inline state2
 229 interpolate<state2>(const state2& a, const state2& b, scalar alpha)
 230 {
 231         state2 state(b);
 232         state.position = interpolate(a.position, b.position, alpha);
 233         state.momentum = interpolate(a.momentum, b.momentum, alpha);
 234         state.orientation = interpolate(a.orientation, b.orientation, alpha);
 235         state.angular_momentum = interpolate(a.angular_momentum,
 236                         b.angular_momentum, alpha);
 237         return state;
 238 }
 239
 240 template <>
 241 inline state3
 242 interpolate<state3>(const state3& a, const state3& b, scalar alpha)
 243 {
 244         state3 state(b);
 245         state.position = interpolate(a.position, b.position, alpha);
 246         state.momentum = interpolate(a.momentum, b.momentum, alpha);
 247         state.orientation = slerp(a.orientation, b.orientation, alpha);
 248         state.angular_momentum = interpolate(a.angular_momentum,
 249                         b.angular_momentum, alpha);
 250         return state;
 251 }
 252
 253 /**
 254  * Interface for anything that can move.
 255  */
 256 template <class T>
 257 class rigid_body : public entity
 258 {
 259 public:
 260
 261         virtual ~rigid_body() {}
 262
 263         virtual void update(scalar t, scalar dt)
 264         {
 265                 prev_state_ = state_;
 266                 state_.update(t, dt);
 267         }
 268
 269         const T& state() const
 270         {
 271                 return state_;
 272         }
 273
 274         T state(scalar alpha) const
 275         {
 276                 return interpolate(prev_state_, state_, alpha);
 277         }
 278
 279         const T& getLastState() const
 280         {
 281                 return prev_state_;
 282         }
 283
 284 protected:
 285
 286         T       state_;
 287         T       prev_state_;
 288 };
 289
 290 typedef rigid_body<state2> rigid_body2;
 291 typedef rigid_body<state3> rigid_body3;
 292
 293
 294 } // namespace moof
 295
 296 #endif // _MOOF_RIGID_BODY_HH_
 297