src/BulletMultiThreaded/btParallelConstraintSolver.h

   1 /*
   2    Copyright (C) 2010 Sony Computer Entertainment Inc.
   3    All rights reserved.
   4
   5 This software is provided 'as-is', without any express or implied warranty.
   6 In no event will the authors be held liable for any damages arising from the use of this software.
   7 Permission is granted to anyone to use this software for any purpose,
   8 including commercial applications, and to alter it and redistribute it freely,
   9 subject to the following restrictions:
  10
  11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
  12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  13 3. This notice may not be removed or altered from any source distribution.
  14
  15 */
  16
  17 #ifndef __BT_PARALLEL_CONSTRAINT_SOLVER_H
  18 #define __BT_PARALLEL_CONSTRAINT_SOLVER_H
  19
  20 #include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
  21
  22
  23
  24
  25 #include "LinearMath/btScalar.h"
  26 #include "PlatformDefinitions.h"
  27
  28
  29 #define PFX_MAX_SOLVER_PHASES 64
  30 #define PFX_MAX_SOLVER_BATCHES 16
  31 #define PFX_MAX_SOLVER_PAIRS  128
  32 #define PFX_MIN_SOLVER_PAIRS  16
  33
  34 #ifdef __CELLOS_LV2__
  35 ATTRIBUTE_ALIGNED128(struct) PfxParallelBatch {
  36 #else
  37 ATTRIBUTE_ALIGNED16(struct) PfxParallelBatch {
  38 #endif
  39         uint16_t pairIndices[PFX_MAX_SOLVER_PAIRS];
  40 };
  41
  42 #ifdef __CELLOS_LV2__
  43 ATTRIBUTE_ALIGNED128(struct) PfxParallelGroup {
  44 #else
  45 ATTRIBUTE_ALIGNED16(struct) PfxParallelGroup {
  46 #endif
  47         uint16_t numPhases;
  48         uint16_t numBatches[PFX_MAX_SOLVER_PHASES];
  49         uint16_t numPairs[PFX_MAX_SOLVER_PHASES*PFX_MAX_SOLVER_BATCHES];
  50 };
  51
  52
  53
  54 ATTRIBUTE_ALIGNED16(struct) PfxSortData16 {
  55         union {
  56                 uint8_t   i8data[16];
  57                 uint16_t  i16data[8];
  58                 uint32_t  i32data[4];
  59 #ifdef __SPU__
  60                 vec_uint4 vdata;
  61 #endif
  62         };
  63
  64 #ifdef __SPU__
  65         void set8(int elem,uint8_t data)   {vdata=(vec_uint4)spu_insert(data,(vec_uchar16)vdata,elem);}
  66         void set16(int elem,uint16_t data) {vdata=(vec_uint4)spu_insert(data,(vec_ushort8)vdata,elem);}
  67         void set32(int elem,uint32_t data) {vdata=(vec_uint4)spu_insert(data,(vec_uint4)vdata,elem);}
  68         uint8_t get8(int elem)   const {return spu_extract((vec_uchar16)vdata,elem);}
  69         uint16_t get16(int elem) const {return spu_extract((vec_ushort8)vdata,elem);}
  70         uint32_t get32(int elem) const {return spu_extract((vec_uint4)vdata,elem);}
  71 #else
  72         void set8(int elem,uint8_t data)   {i8data[elem] = data;}
  73         void set16(int elem,uint16_t data) {i16data[elem] = data;}
  74         void set32(int elem,uint32_t data) {i32data[elem] = data;}
  75         uint8_t get8(int elem)   const {return i8data[elem];}
  76         uint16_t get16(int elem) const {return i16data[elem];}
  77         uint32_t get32(int elem) const {return i32data[elem];}
  78 #endif
  79 };
  80
  81 typedef PfxSortData16 PfxConstraintPair;
  82
  83
  84 //J     PfxBroadphasePair\82Æ\8b¤\92Ê
  85
  86 SIMD_FORCE_INLINE void pfxSetConstraintId(PfxConstraintPair &pair,uint32_t i)   {pair.set32(2,i);}
  87 SIMD_FORCE_INLINE void pfxSetNumConstraints(PfxConstraintPair &pair,uint8_t n)  {pair.set8(7,n);}
  88
  89 SIMD_FORCE_INLINE uint32_t pfxGetConstraintId1(const PfxConstraintPair &pair)   {return pair.get32(2);}
  90 SIMD_FORCE_INLINE uint8_t  pfxGetNumConstraints(const PfxConstraintPair &pair)  {return pair.get8(7);}
  91
  92 typedef PfxSortData16 PfxBroadphasePair;
  93
  94 SIMD_FORCE_INLINE void pfxSetRigidBodyIdA(PfxBroadphasePair &pair,uint16_t i)   {pair.set16(0,i);}
  95 SIMD_FORCE_INLINE void pfxSetRigidBodyIdB(PfxBroadphasePair &pair,uint16_t i)   {pair.set16(1,i);}
  96 SIMD_FORCE_INLINE void pfxSetMotionMaskA(PfxBroadphasePair &pair,uint8_t i)             {pair.set8(4,i);}
  97 SIMD_FORCE_INLINE void pfxSetMotionMaskB(PfxBroadphasePair &pair,uint8_t i)             {pair.set8(5,i);}
  98 SIMD_FORCE_INLINE void pfxSetBroadphaseFlag(PfxBroadphasePair &pair,uint8_t f)  {pair.set8(6,(pair.get8(6)&0xf0)|(f&0x0f));}
  99 SIMD_FORCE_INLINE void pfxSetActive(PfxBroadphasePair &pair,bool b)                     {pair.set8(6,(pair.get8(6)&0x0f)|((b?1:0)<<4));}
 100 SIMD_FORCE_INLINE void pfxSetContactId(PfxBroadphasePair &pair,uint32_t i)              {pair.set32(2,i);}
 101
 102 SIMD_FORCE_INLINE uint16_t pfxGetRigidBodyIdA(const PfxBroadphasePair &pair)    {return pair.get16(0);}
 103 SIMD_FORCE_INLINE uint16_t pfxGetRigidBodyIdB(const PfxBroadphasePair &pair)    {return pair.get16(1);}
 104 SIMD_FORCE_INLINE uint8_t  pfxGetMotionMaskA(const PfxBroadphasePair &pair)             {return pair.get8(4);}
 105 SIMD_FORCE_INLINE uint8_t  pfxGetMotionMaskB(const PfxBroadphasePair &pair)             {return pair.get8(5);}
 106 SIMD_FORCE_INLINE uint8_t  pfxGetBroadphaseFlag(const PfxBroadphasePair &pair)  {return pair.get8(6)&0x0f;}
 107 SIMD_FORCE_INLINE bool     pfxGetActive(const PfxBroadphasePair &pair)                  {return (pair.get8(6)>>4)!=0;}
 108 SIMD_FORCE_INLINE uint32_t pfxGetContactId1(const PfxBroadphasePair &pair)              {return pair.get32(2);}
 109
 110
 111
 112 #if defined(__PPU__) || defined (__SPU__)
 113 ATTRIBUTE_ALIGNED128(struct) PfxSolverBody {
 114 #else
 115 ATTRIBUTE_ALIGNED16(struct) PfxSolverBody {
 116 #endif
 117         vmVector3 mDeltaLinearVelocity;
 118         vmVector3 mDeltaAngularVelocity;
 119         vmMatrix3 mInertiaInv;
 120         vmQuat    mOrientation;
 121         float   mMassInv;
 122         float   friction;
 123         float   restitution;
 124         float   unused;
 125         float   unused2;
 126         float   unused3;
 127         float   unused4;
 128         float   unused5;
 129 };
 130
 131
 132 #ifdef __PPU__
 133 #include "SpuDispatch/BulletPE2ConstraintSolverSpursSupport.h"
 134 #endif
 135
 136 static SIMD_FORCE_INLINE vmVector3 btReadVector3(const double* p)
 137 {
 138         float tmp[3] = {float(p[0]),float(p[1]),float(p[2])};
 139         vmVector3 v;
 140         loadXYZ(v, tmp);
 141         return v;
 142 }
 143
 144 static SIMD_FORCE_INLINE vmQuat btReadQuat(const double* p)
 145 {
 146         float tmp[4] = {float(p[0]),float(p[1]),float(p[2]),float(p[4])};
 147         vmQuat vq;
 148         loadXYZW(vq, tmp);
 149         return vq;
 150 }
 151
 152 static SIMD_FORCE_INLINE void btStoreVector3(const vmVector3 &src, double* p)
 153 {
 154         float tmp[3];
 155         vmVector3 v = src;
 156         storeXYZ(v, tmp);
 157         p[0] = tmp[0];
 158         p[1] = tmp[1];
 159         p[2] = tmp[2];
 160 }
 161
 162
 163 static SIMD_FORCE_INLINE vmVector3 btReadVector3(const float* p)
 164 {
 165         vmVector3 v;
 166         loadXYZ(v, p);
 167         return v;
 168 }
 169
 170 static SIMD_FORCE_INLINE vmQuat btReadQuat(const float* p)
 171 {
 172         vmQuat vq;
 173         loadXYZW(vq, p);
 174         return vq;
 175 }
 176
 177 static SIMD_FORCE_INLINE void btStoreVector3(const vmVector3 &src, float* p)
 178 {
 179         vmVector3 v = src;
 180         storeXYZ(v, p);
 181 }
 182
 183
 184
 185
 186 class btPersistentManifold;
 187
 188 enum {
 189         PFX_CONSTRAINT_SOLVER_CMD_SETUP_SOLVER_BODIES,
 190         PFX_CONSTRAINT_SOLVER_CMD_SETUP_CONTACT_CONSTRAINTS,
 191         PFX_CONSTRAINT_SOLVER_CMD_WRITEBACK_APPLIED_IMPULSES_CONTACT_CONSTRAINTS,
 192         PFX_CONSTRAINT_SOLVER_CMD_SETUP_JOINT_CONSTRAINTS,
 193         PFX_CONSTRAINT_SOLVER_CMD_SOLVE_CONSTRAINTS,
 194         PFX_CONSTRAINT_SOLVER_CMD_POST_SOLVER
 195 };
 196
 197
 198 struct PfxSetupContactConstraintsIO {
 199         PfxConstraintPair *offsetContactPairs;
 200         uint32_t numContactPairs1;
 201         btPersistentManifold*   offsetContactManifolds;
 202         btConstraintRow* offsetContactConstraintRows;
 203         class TrbState *offsetRigStates;
 204         struct PfxSolverBody *offsetSolverBodies;
 205         uint32_t numRigidBodies;
 206         float separateBias;
 207         float timeStep;
 208         class btCriticalSection* criticalSection;
 209 };
 210
 211
 212
 213 struct PfxSolveConstraintsIO {
 214         PfxParallelGroup *contactParallelGroup;
 215         PfxParallelBatch *contactParallelBatches;
 216         PfxConstraintPair *contactPairs;
 217         uint32_t numContactPairs;
 218         btPersistentManifold *offsetContactManifolds;
 219         btConstraintRow*        offsetContactConstraintRows;
 220         PfxParallelGroup *jointParallelGroup;
 221         PfxParallelBatch *jointParallelBatches;
 222         PfxConstraintPair *jointPairs;
 223         uint32_t numJointPairs;
 224         struct btSolverConstraint* offsetSolverConstraints;
 225         TrbState *offsetRigStates1;
 226         PfxSolverBody *offsetSolverBodies;
 227         uint32_t numRigidBodies;
 228         uint32_t iteration;
 229
 230         uint32_t        taskId;
 231
 232         class btBarrier* barrier;
 233
 234 };
 235
 236 struct PfxPostSolverIO {
 237         TrbState *states;
 238         PfxSolverBody *solverBodies;
 239         uint32_t numRigidBodies;
 240 };
 241
 242 ATTRIBUTE_ALIGNED16(struct) btConstraintSolverIO {
 243         uint8_t cmd;
 244         union {
 245                 PfxSetupContactConstraintsIO setupContactConstraints;
 246                 PfxSolveConstraintsIO solveConstraints;
 247                 PfxPostSolverIO postSolver;
 248         };
 249
 250         //SPU only
 251         uint32_t barrierAddr2;
 252         uint32_t criticalsectionAddr2;
 253         uint32_t maxTasks1;
 254 };
 255
 256
 257
 258
 259 void    SolverThreadFunc(void* userPtr,void* lsMemory);
 260 void*   SolverlsMemoryFunc();
 261 ///The btParallelConstraintSolver performs computations on constraint rows in parallel
 262 ///Using the cross-platform threading it supports Windows, Linux, Mac OSX and PlayStation 3 Cell SPUs
 263 class btParallelConstraintSolver : public btSequentialImpulseConstraintSolver
 264 {
 265
 266 protected:
 267         struct btParallelSolverMemoryCache*     m_memoryCache;
 268
 269         class btThreadSupportInterface* m_solverThreadSupport;
 270
 271         struct btConstraintSolverIO* m_solverIO;
 272         class btBarrier*                        m_barrier;
 273         class btCriticalSection*        m_criticalSection;
 274
 275
 276 public:
 277
 278         btParallelConstraintSolver(class btThreadSupportInterface* solverThreadSupport);
 279
 280         virtual ~btParallelConstraintSolver();
 281
 282         virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher);
 283
 284 };
 285
 286
 287
 288 #endif //__BT_PARALLEL_CONSTRAINT_SOLVER_H