Bullet Collision Detection & Physics Library
btParallelConstraintSolver.h
Go to the documentation of this file.
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‚Æ‹¤’Ê
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();
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
Generated on Thu Oct 3 2013 18:25:06 for Bullet Collision Detection & Physics Library by   doxygen 1.8.4