Extras/PhysicsEffects/include/physics_effects/base_level/solver/pfx_integrate.h

   1 /*\r
   2 Physics Effects Copyright(C) 2010 Sony Computer Entertainment Inc.\r
   3 All rights reserved.\r
   4 \r
   5 Physics Effects is open software; you can redistribute it and/or\r
   6 modify it under the terms of the BSD License.\r
   7 \r
   8 Physics Effects is distributed in the hope that it will be useful,\r
   9 but WITHOUT ANY WARRANTY; without even the implied warranty of\r
  10 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.\r
  11 See the BSD License for more details.\r
  12 \r
  13 A copy of the BSD License is distributed with\r
  14 Physics Effects under the filename: physics_effects_license.txt\r
  15 */\r
  16 \r
  17 #ifndef _SCE_PFX_INTEGRATE_H_\r
  18 #define _SCE_PFX_INTEGRATE_H_\r
  19 \r
  20 #include "../base/pfx_common.h"\r
  21 #include "../rigidbody/pfx_rigid_body.h"\r
  22 #include "../rigidbody/pfx_rigid_state.h"\r
  23 \r
  24 namespace sce {\r
  25 namespace PhysicsEffects {\r
  26 \r
  27 template <typename T>\r
  28 static SCE_PFX_FORCE_INLINE T pfxRungeKutta(const T &deriv,PfxFloat dt)\r
  29 {\r
  30         T       k0, k1, k2, k3;\r
  31         k0 = deriv * dt;\r
  32         k1 = (deriv + k0 * 0.5f) * dt;\r
  33         k2 = (deriv + k1 * 0.5f) * dt;\r
  34         k3 = (deriv + k2) * dt;\r
  35         return (k0 + k1*2.0f + k2*2.0f + k3) / 6.0f;\r
  36 }\r
  37 \r
  38 #define SCE_PFX_MOTION_MASK_INTEGRATE ((1<<kPfxMotionTypeFixed)|(1<<kPfxMotionTypeTrigger))\r
  39 \r
  40 static SCE_PFX_FORCE_INLINE\r
  41 void pfxIntegrate(\r
  42         PfxRigidState &state,\r
  43         const PfxRigidBody &body,\r
  44         PfxFloat timeStep\r
  45         )\r
  46 {\r
  47         (void) body;\r
  48         if(((1<<state.getMotionMask())&SCE_PFX_MOTION_MASK_INTEGRATE) || state.isAsleep()) return;\r
  49 \r
  50         PfxVector3 position = state.getPosition();\r
  51         PfxQuat    orientation = state.getOrientation();\r
  52         PfxVector3 linearVelocity = state.getLinearVelocity();\r
  53         PfxVector3 angularVelocity = state.getAngularVelocity();\r
  54 \r
  55         PfxVector3 dx = linearVelocity;\r
  56         PfxQuat    dq = PfxQuat(angularVelocity,0) * orientation * 0.5f;\r
  57 \r
  58 #ifdef SCE_PFX_ODE_EULER\r
  59         position += dx * timeStep;\r
  60         orientation += dq * timeStep;\r
  61         orientation = normalize(orientation);\r
  62 #else\r
  63         position += pfxRungeKutta(dx,timeStep);\r
  64         orientation += pfxRungeKutta(dq,timeStep);\r
  65         orientation = normalize(orientation);\r
  66 #endif\r
  67 \r
  68         state.setPosition(position);\r
  69         state.setOrientation(orientation);\r
  70 }\r
  71 \r
  72 #define SCE_PFX_MOTION_MASK_APPLYFORCE ((1<<kPfxMotionTypeFixed)|(1<<kPfxMotionTypeTrigger)|(1<<kPfxMotionTypeKeyframe))\r
  73 \r
  74 static SCE_PFX_FORCE_INLINE\r
  75 void pfxApplyExternalForce(\r
  76         PfxRigidState &state,\r
  77         const PfxRigidBody &body,\r
  78         const PfxVector3 &extForce,\r
  79         const PfxVector3 &extTorque,\r
  80         PfxFloat timeStep\r
  81         )\r
  82 {\r
  83         if(((1<<state.getMotionType())&SCE_PFX_MOTION_MASK_APPLYFORCE) || state.isAsleep()) return;\r
  84         \r
  85         PfxVector3 angularVelocity = state.getAngularVelocity();\r
  86         PfxMatrix3 m(state.getOrientation());\r
  87         PfxMatrix3 worldInertia = m * body.getInertia() * transpose(m);\r
  88         PfxMatrix3 worldInertiaInv = m * body.getInertiaInv() * transpose(m);\r
  89         PfxVector3 dv = extForce * body.getMassInv();\r
  90         PfxVector3 dw = worldInertiaInv * (cross(worldInertia * angularVelocity, angularVelocity) + extTorque);\r
  91         \r
  92         PfxVector3 nv = state.getLinearVelocity();\r
  93         PfxVector3 nw = state.getAngularVelocity();\r
  94 \r
  95 #ifdef SCE_PFX_ODE_EULER\r
  96         nv += dv * timeStep;\r
  97         nw += dw * timeStep;\r
  98 #else\r
  99         nv += pfxRungeKutta(dv,timeStep);\r
 100         nw += pfxRungeKutta(dw,timeStep);\r
 101 #endif\r
 102 \r
 103         nv *= state.getLinearDamping();\r
 104         nw *= state.getAngularDamping();\r
 105 \r
 106         if(length(nv) > state.getMaxLinearVelocity())\r
 107         {\r
 108                 nv = normalize( nv ) * state.getMaxLinearVelocity();\r
 109         }\r
 110         if(length(nw) > state.getMaxAngularVelocity())\r
 111         {\r
 112                 nw = normalize( nw ) * state.getMaxAngularVelocity();\r
 113         }\r
 114 \r
 115         state.setLinearVelocity(nv);\r
 116         state.setAngularVelocity(nw);\r
 117 }\r
 118 } //namespace PhysicsEffects\r
 119 } //namespace sce\r
 120 \r
 121 #endif /* _SCE_PFX_INTEGRATE_H_ */\r