btParallelConstraintSolver.h

Go to the documentation of this file.
00001 /*
00002    Copyright (C) 2010 Sony Computer Entertainment Inc.
00003    All rights reserved.
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose, 
00008 including commercial applications, and to alter it and redistribute it freely, 
00009 subject to the following restrictions:
00010 
00011 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.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
00014 
00015 */
00016 
00017 #ifndef __BT_PARALLEL_CONSTRAINT_SOLVER_H
00018 #define __BT_PARALLEL_CONSTRAINT_SOLVER_H
00019 
00020 #include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
00021 
00022 
00023 
00024 
00025 #include "LinearMath/btScalar.h"
00026 #include "PlatformDefinitions.h"
00027 
00028 
00029 #define PFX_MAX_SOLVER_PHASES 64
00030 #define PFX_MAX_SOLVER_BATCHES 16
00031 #define PFX_MAX_SOLVER_PAIRS  128
00032 #define PFX_MIN_SOLVER_PAIRS  16
00033 
00034 #ifdef __CELLOS_LV2__
00035 ATTRIBUTE_ALIGNED128(struct) PfxParallelBatch {
00036 #else
00037 ATTRIBUTE_ALIGNED16(struct) PfxParallelBatch {
00038 #endif
00039         uint16_t pairIndices[PFX_MAX_SOLVER_PAIRS];
00040 };
00041 
00042 #ifdef __CELLOS_LV2__
00043 ATTRIBUTE_ALIGNED128(struct) PfxParallelGroup {
00044 #else
00045 ATTRIBUTE_ALIGNED16(struct) PfxParallelGroup {
00046 #endif
00047         uint16_t numPhases;
00048         uint16_t numBatches[PFX_MAX_SOLVER_PHASES];
00049         uint16_t numPairs[PFX_MAX_SOLVER_PHASES*PFX_MAX_SOLVER_BATCHES];
00050 };
00051 
00052 
00053 
00054 ATTRIBUTE_ALIGNED16(struct) PfxSortData16 {
00055         union {
00056                 uint8_t   i8data[16];
00057                 uint16_t  i16data[8];
00058                 uint32_t  i32data[4];
00059 #ifdef __SPU__
00060                 vec_uint4 vdata;
00061 #endif
00062         };
00063 
00064 #ifdef __SPU__
00065         void set8(int elem,uint8_t data)   {vdata=(vec_uint4)spu_insert(data,(vec_uchar16)vdata,elem);}
00066         void set16(int elem,uint16_t data) {vdata=(vec_uint4)spu_insert(data,(vec_ushort8)vdata,elem);}
00067         void set32(int elem,uint32_t data) {vdata=(vec_uint4)spu_insert(data,(vec_uint4)vdata,elem);}
00068         uint8_t get8(int elem)   const {return spu_extract((vec_uchar16)vdata,elem);}
00069         uint16_t get16(int elem) const {return spu_extract((vec_ushort8)vdata,elem);}
00070         uint32_t get32(int elem) const {return spu_extract((vec_uint4)vdata,elem);}
00071 #else
00072         void set8(int elem,uint8_t data)   {i8data[elem] = data;}
00073         void set16(int elem,uint16_t data) {i16data[elem] = data;}
00074         void set32(int elem,uint32_t data) {i32data[elem] = data;}
00075         uint8_t get8(int elem)   const {return i8data[elem];}
00076         uint16_t get16(int elem) const {return i16data[elem];}
00077         uint32_t get32(int elem) const {return i32data[elem];}
00078 #endif
00079 };
00080 
00081 typedef PfxSortData16 PfxConstraintPair;
00082 
00083 
00084 //J     PfxBroadphasePair‚Æ‹¤’Ê
00085 
00086 SIMD_FORCE_INLINE void pfxSetConstraintId(PfxConstraintPair &pair,uint32_t i)   {pair.set32(2,i);}
00087 SIMD_FORCE_INLINE void pfxSetNumConstraints(PfxConstraintPair &pair,uint8_t n)  {pair.set8(7,n);}
00088 
00089 SIMD_FORCE_INLINE uint32_t pfxGetConstraintId1(const PfxConstraintPair &pair)   {return pair.get32(2);}
00090 SIMD_FORCE_INLINE uint8_t  pfxGetNumConstraints(const PfxConstraintPair &pair)  {return pair.get8(7);}
00091 
00092 typedef PfxSortData16 PfxBroadphasePair;
00093 
00094 SIMD_FORCE_INLINE void pfxSetRigidBodyIdA(PfxBroadphasePair &pair,uint16_t i)   {pair.set16(0,i);}
00095 SIMD_FORCE_INLINE void pfxSetRigidBodyIdB(PfxBroadphasePair &pair,uint16_t i)   {pair.set16(1,i);}
00096 SIMD_FORCE_INLINE void pfxSetMotionMaskA(PfxBroadphasePair &pair,uint8_t i)             {pair.set8(4,i);}
00097 SIMD_FORCE_INLINE void pfxSetMotionMaskB(PfxBroadphasePair &pair,uint8_t i)             {pair.set8(5,i);}
00098 SIMD_FORCE_INLINE void pfxSetBroadphaseFlag(PfxBroadphasePair &pair,uint8_t f)  {pair.set8(6,(pair.get8(6)&0xf0)|(f&0x0f));}
00099 SIMD_FORCE_INLINE void pfxSetActive(PfxBroadphasePair &pair,bool b)                     {pair.set8(6,(pair.get8(6)&0x0f)|((b?1:0)<<4));}
00100 SIMD_FORCE_INLINE void pfxSetContactId(PfxBroadphasePair &pair,uint32_t i)              {pair.set32(2,i);}
00101 
00102 SIMD_FORCE_INLINE uint16_t pfxGetRigidBodyIdA(const PfxBroadphasePair &pair)    {return pair.get16(0);}
00103 SIMD_FORCE_INLINE uint16_t pfxGetRigidBodyIdB(const PfxBroadphasePair &pair)    {return pair.get16(1);}
00104 SIMD_FORCE_INLINE uint8_t  pfxGetMotionMaskA(const PfxBroadphasePair &pair)             {return pair.get8(4);}
00105 SIMD_FORCE_INLINE uint8_t  pfxGetMotionMaskB(const PfxBroadphasePair &pair)             {return pair.get8(5);}
00106 SIMD_FORCE_INLINE uint8_t  pfxGetBroadphaseFlag(const PfxBroadphasePair &pair)  {return pair.get8(6)&0x0f;}
00107 SIMD_FORCE_INLINE bool     pfxGetActive(const PfxBroadphasePair &pair)                  {return (pair.get8(6)>>4)!=0;}
00108 SIMD_FORCE_INLINE uint32_t pfxGetContactId1(const PfxBroadphasePair &pair)              {return pair.get32(2);}
00109 
00110 
00111 
00112 #if defined(__PPU__) || defined (__SPU__)
00113 ATTRIBUTE_ALIGNED128(struct) PfxSolverBody {
00114 #else
00115 ATTRIBUTE_ALIGNED16(struct) PfxSolverBody {
00116 #endif
00117         vmVector3 mDeltaLinearVelocity;
00118         vmVector3 mDeltaAngularVelocity;
00119         vmMatrix3 mInertiaInv;
00120         vmQuat    mOrientation;
00121         float   mMassInv;
00122         float   friction;
00123         float   restitution;
00124         float   unused;
00125         float   unused2;
00126         float   unused3;
00127         float   unused4;
00128         float   unused5;
00129 };
00130 
00131 
00132 #ifdef __PPU__
00133 #include "SpuDispatch/BulletPE2ConstraintSolverSpursSupport.h"
00134 #endif
00135 
00136 static SIMD_FORCE_INLINE vmVector3 btReadVector3(const double* p)
00137 {
00138         float tmp[3] = {float(p[0]),float(p[1]),float(p[2])};
00139         vmVector3 v;
00140         loadXYZ(v, tmp);
00141         return v;
00142 }
00143 
00144 static SIMD_FORCE_INLINE vmQuat btReadQuat(const double* p)
00145 {
00146         float tmp[4] = {float(p[0]),float(p[1]),float(p[2]),float(p[4])};
00147         vmQuat vq;
00148         loadXYZW(vq, tmp);
00149         return vq;
00150 }
00151 
00152 static SIMD_FORCE_INLINE void btStoreVector3(const vmVector3 &src, double* p)
00153 {
00154         float tmp[3];
00155         vmVector3 v = src;
00156         storeXYZ(v, tmp);
00157         p[0] = tmp[0];
00158         p[1] = tmp[1];
00159         p[2] = tmp[2];
00160 }
00161 
00162 
00163 static SIMD_FORCE_INLINE vmVector3 btReadVector3(const float* p)
00164 {
00165         vmVector3 v;
00166         loadXYZ(v, p);
00167         return v;
00168 }
00169 
00170 static SIMD_FORCE_INLINE vmQuat btReadQuat(const float* p)
00171 {
00172         vmQuat vq;
00173         loadXYZW(vq, p);
00174         return vq;
00175 }
00176 
00177 static SIMD_FORCE_INLINE void btStoreVector3(const vmVector3 &src, float* p)
00178 {
00179         vmVector3 v = src;
00180         storeXYZ(v, p);
00181 }
00182 
00183 
00184 
00185 
00186 class btPersistentManifold;
00187 
00188 enum {
00189         PFX_CONSTRAINT_SOLVER_CMD_SETUP_SOLVER_BODIES,
00190         PFX_CONSTRAINT_SOLVER_CMD_SETUP_CONTACT_CONSTRAINTS,
00191         PFX_CONSTRAINT_SOLVER_CMD_WRITEBACK_APPLIED_IMPULSES_CONTACT_CONSTRAINTS,
00192         PFX_CONSTRAINT_SOLVER_CMD_SETUP_JOINT_CONSTRAINTS,
00193         PFX_CONSTRAINT_SOLVER_CMD_SOLVE_CONSTRAINTS,
00194         PFX_CONSTRAINT_SOLVER_CMD_POST_SOLVER
00195 };
00196 
00197 
00198 struct PfxSetupContactConstraintsIO {
00199         PfxConstraintPair *offsetContactPairs;
00200         uint32_t numContactPairs1;
00201         btPersistentManifold*   offsetContactManifolds;
00202         btConstraintRow* offsetContactConstraintRows;
00203         class TrbState *offsetRigStates;
00204         struct PfxSolverBody *offsetSolverBodies;
00205         uint32_t numRigidBodies;
00206         float separateBias;
00207         float timeStep;
00208         class btCriticalSection* criticalSection;
00209 };
00210 
00211 
00212 
00213 struct PfxSolveConstraintsIO {
00214         PfxParallelGroup *contactParallelGroup;
00215         PfxParallelBatch *contactParallelBatches;
00216         PfxConstraintPair *contactPairs;
00217         uint32_t numContactPairs;
00218         btPersistentManifold *offsetContactManifolds;
00219         btConstraintRow*        offsetContactConstraintRows;
00220         PfxParallelGroup *jointParallelGroup;
00221         PfxParallelBatch *jointParallelBatches;
00222         PfxConstraintPair *jointPairs;
00223         uint32_t numJointPairs;
00224         struct btSolverConstraint* offsetSolverConstraints;
00225         TrbState *offsetRigStates1;
00226         PfxSolverBody *offsetSolverBodies;
00227         uint32_t numRigidBodies;
00228         uint32_t iteration;
00229 
00230         uint32_t        taskId;
00231         
00232         class btBarrier* barrier;
00233 
00234 };
00235 
00236 struct PfxPostSolverIO {
00237         TrbState *states;
00238         PfxSolverBody *solverBodies;
00239         uint32_t numRigidBodies;
00240 };
00241 
00242 ATTRIBUTE_ALIGNED16(struct) btConstraintSolverIO {
00243         uint8_t cmd;
00244         union {
00245                 PfxSetupContactConstraintsIO setupContactConstraints;
00246                 PfxSolveConstraintsIO solveConstraints;
00247                 PfxPostSolverIO postSolver;
00248         };
00249         
00250         //SPU only
00251         uint32_t barrierAddr2;
00252         uint32_t criticalsectionAddr2;
00253         uint32_t maxTasks1;
00254 };
00255 
00256 
00257 
00258 
00259 void    SolverThreadFunc(void* userPtr,void* lsMemory);
00260 void*   SolverlsMemoryFunc();
00263 class btParallelConstraintSolver : public btSequentialImpulseConstraintSolver
00264 {
00265         
00266 protected:
00267         struct btParallelSolverMemoryCache*     m_memoryCache;
00268 
00269         class btThreadSupportInterface* m_solverThreadSupport;
00270 
00271         struct btConstraintSolverIO* m_solverIO;
00272         class btBarrier*                        m_barrier;
00273         class btCriticalSection*        m_criticalSection;
00274 
00275 
00276 public:
00277 
00278         btParallelConstraintSolver(class btThreadSupportInterface* solverThreadSupport);
00279         
00280         virtual ~btParallelConstraintSolver();
00281 
00282         virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher);
00283 
00284 };
00285 
00286 
00287 
00288 #endif //__BT_PARALLEL_CONSTRAINT_SOLVER_H