Dogcows Code - chaz/yoink/blob - src/Moof/RigidBody.hh

   1
   2 /*******************************************************************************
   3
   4  Copyright (c) 2009, Charles McGarvey
   5  All rights reserved.
   6
   7  Redistribution   and   use  in  source  and  binary  forms,  with  or  without
   8  modification, are permitted provided that the following conditions are met:
   9
  10    * Redistributions  of  source  code  must retain the above copyright notice,
  11      this list of conditions and the following disclaimer.
  12    * Redistributions  in binary form must reproduce the above copyright notice,
  13      this  list of conditions and the following disclaimer in the documentation
  14      and/or other materials provided with the distribution.
  15
  16  THIS  SOFTWARE  IS  PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  17  AND  ANY  EXPRESS  OR  IMPLIED  WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  18  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  19  DISCLAIMED.  IN  NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
  20  FOR  ANY  DIRECT,  INDIRECT,  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  21  DAMAGES  (INCLUDING,  BUT  NOT  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
  22  SERVICES;  LOSS  OF  USE,  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  23  CAUSED  AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
  24  OR  TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  25  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  26
  27 *******************************************************************************/
  28
  29 #ifndef _MOOF_RIGIDBODY_HH_
  30 #define _MOOF_RIGIDBODY_HH_
  31
  32 #include <vector>
  33
  34 #include <boost/bind.hpp>
  35 #include <boost/function.hpp>
  36
  37 #include <Moof/Entity.hh>
  38 #include <Moof/Math.hh>
  39
  40
  41 namespace Mf {
  42
  43
  44 template <int D = 3>
  45 struct LinearState
  46 {
  47         typedef cml::vector< Scalar, cml::fixed<D> >                            Vector;
  48         typedef boost::function<const Vector& (const LinearState&)>     ForceFunction;
  49
  50         // primary
  51
  52         Vector          position;
  53         Vector          momentum;
  54
  55         // secondary
  56
  57         Vector          velocity;
  58
  59         // user
  60
  61         Vector          force;
  62         std::vector<ForceFunction> forces;
  63
  64         // constant
  65
  66         Scalar          mass;
  67         Scalar          inverseMass;
  68
  69
  70         void recalculateLinear()
  71         {
  72                 velocity = momentum * inverseMass;
  73         }
  74
  75
  76         struct GravityForce
  77         {
  78                 explicit GravityForce(Scalar a = -9.8)
  79                 {
  80                         force.zero();
  81                         acceleration = a;
  82                 }
  83
  84                 const Vector& operator () (const LinearState& state)
  85                 {
  86                         force[1] = state.mass * acceleration;
  87                         return force;
  88                 }
  89
  90         private:
  91
  92                 Vector force;
  93                 Scalar acceleration;
  94         };
  95
  96
  97         void init()
  98         {
  99                 position.zero();
 100                 momentum.zero();
 101
 102                 velocity.zero();
 103
 104                 force.zero();
 105                 forces.clear();
 106
 107                 mass = SCALAR(1.0);
 108                 inverseMass = 1.0 / mass;
 109         }
 110
 111
 112         struct Derivative
 113         {
 114                 Vector  velocity;
 115                 Vector  force;
 116
 117                 Derivative operator*(Scalar dt) const
 118                 {
 119                         Derivative derivative;
 120                         derivative.velocity = dt * velocity;
 121                         derivative.force = dt * force;
 122                         return derivative;
 123                 }
 124
 125                 Derivative operator+(const Derivative& other) const
 126                 {
 127                         Derivative derivative;
 128                         derivative.velocity = velocity + other.velocity;
 129                         derivative.force = force + other.force;
 130                         return derivative;
 131                 }
 132         };
 133
 134
 135         Vector getForce() const
 136         {
 137                 Vector f(force);
 138
 139                 for (size_t i = 0; i < forces.size(); ++i)
 140                 {
 141                         f += forces[i](*this);
 142                 }
 143
 144                 return f;
 145         }
 146
 147         void getDerivative(Derivative& derivative, Scalar t) const
 148         {
 149                 derivative.velocity = velocity;
 150                 derivative.force = getForce();
 151         }
 152
 153         void step(const Derivative& derivative, Scalar dt)
 154         {
 155                 position += dt * derivative.velocity;
 156                 momentum += dt * derivative.force;
 157                 recalculateLinear();
 158         }
 159 };
 160
 161
 162 struct RotationalState2
 163 {
 164         // primary
 165
 166         Scalar          orientation;
 167         Scalar          angularMomentum;
 168
 169         // secondary
 170
 171         Scalar          angularVelocity;
 172
 173         // constant
 174
 175         Scalar          inertia;
 176         Scalar          inverseInertia;
 177
 178
 179         void recalculateRotational()
 180         {
 181                 angularVelocity = angularMomentum * inertia;
 182         }
 183
 184
 185         struct Derivative
 186         {
 187                 Scalar  angularVelocity;
 188                 Scalar  torque;
 189         };
 190
 191         void step(const Derivative& derivative, Scalar dt)
 192         {
 193                 orientation += dt * derivative.angularVelocity;
 194                 angularMomentum += dt * derivative.torque;
 195                 recalculateRotational();
 196         }
 197 };
 198
 199 struct RotationalState3
 200 {
 201         // primary
 202
 203         Quaternion      orientation;
 204         Vector3         angularMomentum;
 205
 206         // secondary
 207
 208         Quaternion      spin;
 209         Vector3         angularVelocity;
 210
 211         // constant
 212
 213         Scalar          inertia;
 214         Scalar          inverseInertia;
 215
 216
 217         void recalculateRotational()
 218         {
 219                 angularVelocity = angularMomentum * inertia;
 220         }
 221 };
 222
 223
 224 struct State2 : public LinearState<2>, public RotationalState2
 225 {
 226         void recalculate()
 227         {
 228                 recalculateLinear();
 229                 recalculateRotational();
 230         }
 231
 232         void update(Scalar t, Scalar dt)
 233         {
 234                 rk4<LinearState<2>,LinearState<2>::Derivative>(*this, t, dt);
 235         }
 236 };
 237
 238 struct State3 : public LinearState<3>, public RotationalState3
 239 {
 240         void recalculate()
 241         {
 242                 recalculateLinear();
 243                 recalculateRotational();
 244         }
 245
 246         void update(Scalar t, Scalar dt)
 247         {
 248                 rk4<LinearState<3>,LinearState<3>::Derivative>(*this, t, dt);
 249         }
 250 };
 251
 252
 253 template <class T>
 254 inline T interpolate(const T& a, const T& b, Scalar alpha)
 255 {
 256         return cml::lerp(a, b, alpha);
 257 }
 258
 259 template <>
 260 inline State2 interpolate<State2>(const State2& a, const State2& b, Scalar alpha)
 261 {
 262         State2 state(b);
 263         state.position = interpolate(a.position, b.position, alpha);
 264         state.momentum = interpolate(a.momentum, b.momentum, alpha);
 265         state.orientation = interpolate(a.orientation, b.orientation, alpha);
 266         state.angularMomentum = interpolate(a.angularMomentum, b.angularMomentum,
 267                         alpha);
 268         return state;
 269 }
 270
 271 template <>
 272 inline State3 interpolate<State3>(const State3& a, const State3& b, Scalar alpha)
 273 {
 274         State3 state(b);
 275         state.position = interpolate(a.position, b.position, alpha);
 276         state.momentum = interpolate(a.momentum, b.momentum, alpha);
 277         state.orientation = cml::slerp(a.orientation, b.orientation, alpha);
 278         state.angularMomentum = interpolate(a.angularMomentum, b.angularMomentum,
 279                         alpha);
 280         return state;
 281 }
 282
 283
 284
 285 /**
 286  * Interface for anything that can move.
 287  */
 288
 289 template <class T>
 290 class RigidBody : public Entity
 291 {
 292 protected:
 293
 294         T       mState;
 295         T       mPrevState;
 296
 297 public:
 298
 299         virtual ~RigidBody() {}
 300
 301         virtual void update(Scalar t, Scalar dt)
 302         {
 303                 mPrevState = mState;
 304                 mState.update(t, dt);
 305         }
 306
 307         const T& getState() const
 308         {
 309                 return mState;
 310         }
 311
 312         T getState(Scalar alpha) const
 313         {
 314                 return interpolate(mPrevState, mState, alpha);
 315         }
 316
 317         const T& getLastState() const
 318         {
 319                 return mPrevState;
 320         }
 321 };
 322
 323 typedef RigidBody<State2> RigidBody2;
 324 typedef RigidBody<State3> RigidBody3;
 325
 326
 327 } // namespace Mf
 328
 329 #endif // _MOOF_RIGIDBODY_HH_
 330
 331 /** vim: set ts=4 sw=4 tw=80: *************************************************/
 332