Dogcows Code - chaz/yoink/blob - src/Moof/RigidBody.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_RIGIDBODY_HH_
  13 #define _MOOF_RIGIDBODY_HH_
  14
  15 #include <vector>
  16
  17 #include <boost/bind.hpp>
  18 #include <boost/function.hpp>
  19
  20 #include <Moof/Entity.hh>
  21 #include <Moof/Math.hh>
  22
  23
  24 namespace Mf {
  25
  26
  27 template <int D = 3>
  28 struct LinearState
  29 {
  30         typedef cml::vector< Scalar, cml::fixed<D> >    Vector;
  31         typedef boost::function<const Vector& (const LinearState&)>
  32                                                                                                         ForceFunction;
  33
  34         // primary
  35
  36         Vector          position;
  37         Vector          momentum;
  38
  39         // secondary
  40
  41         Vector          velocity;
  42
  43         // user
  44
  45         Vector          force;
  46         std::vector<ForceFunction> forces;
  47
  48         // constant
  49
  50         Scalar          mass;
  51         Scalar          inverseMass;
  52
  53
  54         void recalculateLinear()
  55         {
  56                 velocity = momentum * inverseMass;
  57         }
  58
  59
  60         struct GravityForce
  61         {
  62                 explicit GravityForce(Scalar a = -9.8)
  63                 {
  64                         force.zero();
  65                         acceleration = a;
  66                 }
  67
  68                 const Vector& operator () (const LinearState& state)
  69                 {
  70                         force[1] = state.mass * acceleration;
  71                         return force;
  72                 }
  73
  74         private:
  75
  76                 Vector force;
  77                 Scalar acceleration;
  78         };
  79
  80
  81         void init()
  82         {
  83                 position.zero();
  84                 momentum.zero();
  85
  86                 velocity.zero();
  87
  88                 force.zero();
  89                 forces.clear();
  90
  91                 mass = SCALAR(1.0);
  92                 inverseMass = 1.0 / mass;
  93         }
  94
  95
  96         struct Derivative
  97         {
  98                 Vector  velocity;
  99                 Vector  force;
 100
 101                 Derivative operator*(Scalar dt) const
 102                 {
 103                         Derivative derivative;
 104                         derivative.velocity = dt * velocity;
 105                         derivative.force = dt * force;
 106                         return derivative;
 107                 }
 108
 109                 Derivative operator+(const Derivative& other) const
 110                 {
 111                         Derivative derivative;
 112                         derivative.velocity = velocity + other.velocity;
 113                         derivative.force = force + other.force;
 114                         return derivative;
 115                 }
 116         };
 117
 118
 119         Vector getForce() const
 120         {
 121                 Vector f(force);
 122
 123                 for (size_t i = 0; i < forces.size(); ++i)
 124                 {
 125                         f += forces[i](*this);
 126                 }
 127
 128                 return f;
 129         }
 130
 131         void getDerivative(Derivative& derivative, Scalar t) const
 132         {
 133                 derivative.velocity = velocity;
 134                 derivative.force = getForce();
 135         }
 136
 137         void step(const Derivative& derivative, Scalar dt)
 138         {
 139                 position += dt * derivative.velocity;
 140                 momentum += dt * derivative.force;
 141                 recalculateLinear();
 142         }
 143 };
 144
 145
 146 struct RotationalState2
 147 {
 148         // primary
 149
 150         Scalar          orientation;
 151         Scalar          angularMomentum;
 152
 153         // secondary
 154
 155         Scalar          angularVelocity;
 156
 157         // constant
 158
 159         Scalar          inertia;
 160         Scalar          inverseInertia;
 161
 162
 163         void recalculateRotational()
 164         {
 165                 angularVelocity = angularMomentum * inertia;
 166         }
 167
 168
 169         struct Derivative
 170         {
 171                 Scalar  angularVelocity;
 172                 Scalar  torque;
 173         };
 174
 175         void step(const Derivative& derivative, Scalar dt)
 176         {
 177                 orientation += dt * derivative.angularVelocity;
 178                 angularMomentum += dt * derivative.torque;
 179                 recalculateRotational();
 180         }
 181 };
 182
 183 struct RotationalState3
 184 {
 185         // primary
 186
 187         Quaternion      orientation;
 188         Vector3         angularMomentum;
 189
 190         // secondary
 191
 192         Quaternion      spin;
 193         Vector3         angularVelocity;
 194
 195         // constant
 196
 197         Scalar          inertia;
 198         Scalar          inverseInertia;
 199
 200
 201         void recalculateRotational()
 202         {
 203                 angularVelocity = angularMomentum * inertia;
 204         }
 205 };
 206
 207
 208 struct State2 : public LinearState<2>, public RotationalState2
 209 {
 210         void recalculate()
 211         {
 212                 recalculateLinear();
 213                 recalculateRotational();
 214         }
 215
 216         void update(Scalar t, Scalar dt)
 217         {
 218                 rk4<LinearState<2>,LinearState<2>::Derivative>(*this, t, dt);
 219         }
 220 };
 221
 222 struct State3 : public LinearState<3>, public RotationalState3
 223 {
 224         void recalculate()
 225         {
 226                 recalculateLinear();
 227                 recalculateRotational();
 228         }
 229
 230         void update(Scalar t, Scalar dt)
 231         {
 232                 rk4<LinearState<3>,LinearState<3>::Derivative>(*this, t, dt);
 233         }
 234 };
 235
 236
 237 template <class T>
 238 inline T interpolate(const T& a, const T& b, Scalar alpha)
 239 {
 240         return cml::lerp(a, b, alpha);
 241 }
 242
 243 template <>
 244 inline State2 interpolate<State2>(const State2& a, const State2& b,
 245                                                                   Scalar alpha)
 246 {
 247         State2 state(b);
 248         state.position = interpolate(a.position, b.position, alpha);
 249         state.momentum = interpolate(a.momentum, b.momentum, alpha);
 250         state.orientation = interpolate(a.orientation, b.orientation, alpha);
 251         state.angularMomentum = interpolate(a.angularMomentum,
 252                                                                                 b.angularMomentum, alpha);
 253         return state;
 254 }
 255
 256 template <>
 257 inline State3 interpolate<State3>(const State3& a, const State3& b,
 258                                                                   Scalar alpha)
 259 {
 260         State3 state(b);
 261         state.position = interpolate(a.position, b.position, alpha);
 262         state.momentum = interpolate(a.momentum, b.momentum, alpha);
 263         state.orientation = cml::slerp(a.orientation, b.orientation, alpha);
 264         state.angularMomentum = interpolate(a.angularMomentum,
 265                                                                                 b.angularMomentum, alpha);
 266         return state;
 267 }
 268
 269
 270
 271 /**
 272  * Interface for anything that can move.
 273  */
 274
 275 template <class T>
 276 class RigidBody : public Entity
 277 {
 278 protected:
 279
 280         T       mState;
 281         T       mPrevState;
 282
 283 public:
 284
 285         virtual ~RigidBody() {}
 286
 287         virtual void update(Scalar t, Scalar dt)
 288         {
 289                 mPrevState = mState;
 290                 mState.update(t, dt);
 291         }
 292
 293         const T& getState() const
 294         {
 295                 return mState;
 296         }
 297
 298         T getState(Scalar alpha) const
 299         {
 300                 return interpolate(mPrevState, mState, alpha);
 301         }
 302
 303         const T& getLastState() const
 304         {
 305                 return mPrevState;
 306         }
 307 };
 308
 309 typedef RigidBody<State2> RigidBody2;
 310 typedef RigidBody<State3> RigidBody3;
 311
 312
 313 } // namespace Mf
 314
 315 #endif // _MOOF_RIGIDBODY_HH_
 316