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

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