00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
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
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
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