Bullet Collision Detection & Physics Library
btParallelConstraintSolver.cpp
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 
18 #include "btParallelConstraintSolver.h"
19 #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
20 #include "BulletCollision/BroadphaseCollision/btDispatcher.h"
21 #include "LinearMath/btPoolAllocator.h"
22 #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
23 #include "BulletMultiThreaded/vectormath2bullet.h"
24 
25 #include "LinearMath/btQuickprof.h"
26 #include "BulletMultiThreaded/btThreadSupportInterface.h"
27 #ifdef PFX_USE_FREE_VECTORMATH
28 #include "vecmath/vmInclude.h"
29 #else
30 #include "vectormath/vmInclude.h"
31 #endif //PFX_USE_FREE_VECTORMATH
32 
33 #include "HeapManager.h"
34 
35 #include "PlatformDefinitions.h"
36 
37 //#include "PfxSimdUtils.h"
38 #include "LinearMath/btScalar.h"
39 
40 #include "TrbStateVec.h"
41 
42 
43 
45 
46 
47 #define TMP_BUFF_BYTES (15*1024*1024)
48 unsigned char ATTRIBUTE_ALIGNED128(tmp_buff[TMP_BUFF_BYTES]);
49 
50 
51 
52 // Project Gauss Seidel or the equivalent Sequential Impulse
53  inline void resolveSingleConstraintRowGeneric(PfxSolverBody& body1,PfxSolverBody& body2,const btSolverConstraint& c)
54 {
55 
56  btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
57  const btScalar deltaVel1Dotn = c.m_contactNormal.dot(getBtVector3(body1.mDeltaLinearVelocity)) + c.m_relpos1CrossNormal.dot(getBtVector3(body1.mDeltaAngularVelocity));
58  const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(getBtVector3(body2.mDeltaLinearVelocity)) + c.m_relpos2CrossNormal.dot(getBtVector3(body2.mDeltaAngularVelocity));
59 // const btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn;
60  deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
61  deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv;
62 
63  const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
64  if (sum < c.m_lowerLimit)
65  {
66  deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
67  c.m_appliedImpulse = c.m_lowerLimit;
68  }
69  else if (sum > c.m_upperLimit)
70  {
71  deltaImpulse = c.m_upperLimit-c.m_appliedImpulse;
72  c.m_appliedImpulse = c.m_upperLimit;
73  }
74  else
75  {
76  c.m_appliedImpulse = sum;
77  }
78 
79 
80  if (body1.mMassInv)
81  {
82  btVector3 linearComponent = c.m_contactNormal*body1.mMassInv;
83  body1.mDeltaLinearVelocity += vmVector3(linearComponent.getX()*deltaImpulse,linearComponent.getY()*deltaImpulse,linearComponent.getZ()*deltaImpulse);
84  btVector3 tmp=c.m_angularComponentA*(btVector3(deltaImpulse,deltaImpulse,deltaImpulse));
85  body1.mDeltaAngularVelocity += vmVector3(tmp.getX(),tmp.getY(),tmp.getZ());
86  }
87 
88  if (body2.mMassInv)
89  {
90  btVector3 linearComponent = -c.m_contactNormal*body2.mMassInv;
91  body2.mDeltaLinearVelocity += vmVector3(linearComponent.getX()*deltaImpulse,linearComponent.getY()*deltaImpulse,linearComponent.getZ()*deltaImpulse);
92  btVector3 tmp = c.m_angularComponentB*((btVector3(deltaImpulse,deltaImpulse,deltaImpulse)));//*m_angularFactor);
93  body2.mDeltaAngularVelocity += vmVector3(tmp.getX(),tmp.getY(),tmp.getZ());
94  }
95 
96  //body1.internalApplyImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
97  //body2.internalApplyImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
98 
99 }
100 
101 
102 static SIMD_FORCE_INLINE
103 void pfxSolveLinearConstraintRow(btConstraintRow &constraint,
104  vmVector3 &deltaLinearVelocityA,vmVector3 &deltaAngularVelocityA,
105  float massInvA,const vmMatrix3 &inertiaInvA,const vmVector3 &rA,
106  vmVector3 &deltaLinearVelocityB,vmVector3 &deltaAngularVelocityB,
107  float massInvB,const vmMatrix3 &inertiaInvB,const vmVector3 &rB)
108 {
109  const vmVector3 normal(btReadVector3(constraint.m_normal));
110  btScalar deltaImpulse = constraint.m_rhs;
111  vmVector3 dVA = deltaLinearVelocityA + cross(deltaAngularVelocityA,rA);
112  vmVector3 dVB = deltaLinearVelocityB + cross(deltaAngularVelocityB,rB);
113  deltaImpulse -= constraint.m_jacDiagInv * dot(normal,dVA-dVB);
114  btScalar oldImpulse = constraint.m_accumImpulse;
115  constraint.m_accumImpulse = btClamped(oldImpulse + deltaImpulse,constraint.m_lowerLimit,constraint.m_upperLimit);
116  deltaImpulse = constraint.m_accumImpulse - oldImpulse;
117  deltaLinearVelocityA += deltaImpulse * massInvA * normal;
118  deltaAngularVelocityA += deltaImpulse * inertiaInvA * cross(rA,normal);
119  deltaLinearVelocityB -= deltaImpulse * massInvB * normal;
120  deltaAngularVelocityB -= deltaImpulse * inertiaInvB * cross(rB,normal);
121 
122 }
123 
124 void btSolveContactConstraint(
125  btConstraintRow &constraintResponse,
126  btConstraintRow &constraintFriction1,
127  btConstraintRow &constraintFriction2,
128  const vmVector3 &contactPointA,
129  const vmVector3 &contactPointB,
130  PfxSolverBody &solverBodyA,
131  PfxSolverBody &solverBodyB,
132  float friction
133  )
134 {
135  vmVector3 rA = rotate(solverBodyA.mOrientation,contactPointA);
136  vmVector3 rB = rotate(solverBodyB.mOrientation,contactPointB);
137 
138  pfxSolveLinearConstraintRow(constraintResponse,
139  solverBodyA.mDeltaLinearVelocity,solverBodyA.mDeltaAngularVelocity,solverBodyA.mMassInv,solverBodyA.mInertiaInv,rA,
140  solverBodyB.mDeltaLinearVelocity,solverBodyB.mDeltaAngularVelocity,solverBodyB.mMassInv,solverBodyB.mInertiaInv,rB);
141 
142  float mf = friction*fabsf(constraintResponse.m_accumImpulse);
143  constraintFriction1.m_lowerLimit = -mf;
144  constraintFriction1.m_upperLimit = mf;
145  constraintFriction2.m_lowerLimit = -mf;
146  constraintFriction2.m_upperLimit = mf;
147 
148  pfxSolveLinearConstraintRow(constraintFriction1,
149  solverBodyA.mDeltaLinearVelocity,solverBodyA.mDeltaAngularVelocity,solverBodyA.mMassInv,solverBodyA.mInertiaInv,rA,
150  solverBodyB.mDeltaLinearVelocity,solverBodyB.mDeltaAngularVelocity,solverBodyB.mMassInv,solverBodyB.mInertiaInv,rB);
151 
152  pfxSolveLinearConstraintRow(constraintFriction2,
153  solverBodyA.mDeltaLinearVelocity,solverBodyA.mDeltaAngularVelocity,solverBodyA.mMassInv,solverBodyA.mInertiaInv,rA,
154  solverBodyB.mDeltaLinearVelocity,solverBodyB.mDeltaAngularVelocity,solverBodyB.mMassInv,solverBodyB.mInertiaInv,rB);
155 }
156 
157 
158 void CustomSolveConstraintsTaskParallel(
159  const PfxParallelGroup *contactParallelGroup,const PfxParallelBatch *contactParallelBatches,
160  PfxConstraintPair *contactPairs,uint32_t numContactPairs,
161  btPersistentManifold* offsetContactManifolds,
162  btConstraintRow* offsetContactConstraintRows,
163  const PfxParallelGroup *jointParallelGroup,const PfxParallelBatch *jointParallelBatches,
164  PfxConstraintPair *jointPairs,uint32_t numJointPairs,
165  btSolverConstraint* offsetSolverConstraints,
166  TrbState *offsetRigStates,
167  PfxSolverBody *offsetSolverBodies,
168  uint32_t numRigidBodies,
169  int iteration,unsigned int taskId,unsigned int numTasks,btBarrier *barrier)
170 {
171 
172  PfxSolverBody staticBody;
173  staticBody.mMassInv = 0.f;
174  staticBody.mDeltaAngularVelocity=vmVector3(0,0,0);
175  staticBody.mDeltaLinearVelocity =vmVector3(0,0,0);
176 
177 
178  for(int k=0;k<iteration+1;k++) {
179  // Joint
180  for(uint32_t phaseId=0;phaseId<jointParallelGroup->numPhases;phaseId++) {
181  for(uint32_t batchId=0;batchId<jointParallelGroup->numBatches[phaseId];batchId++) {
182  uint32_t numPairs = jointParallelGroup->numPairs[phaseId*PFX_MAX_SOLVER_BATCHES+batchId];
183  if(batchId%numTasks == taskId && numPairs > 0) {
184  const PfxParallelBatch &batch = jointParallelBatches[phaseId*PFX_MAX_SOLVER_BATCHES+batchId];
185  for(uint32_t i=0;i<numPairs;i++) {
186  PfxConstraintPair &pair = jointPairs[batch.pairIndices[i]];
187  uint16_t iA = pfxGetRigidBodyIdA(pair);
188  uint16_t iB = pfxGetRigidBodyIdB(pair);
189 
190 
191  PfxSolverBody &solverBodyA = iA != 65535 ? offsetSolverBodies[iA] : staticBody;
192  PfxSolverBody &solverBodyB = iB != 65535 ? offsetSolverBodies[iB] : staticBody;
193 
194  if(k==0) {
195 
196  }
197  else {
198  btSolverConstraint* constraintRow = &offsetSolverConstraints[pfxGetContactId1(pair)];
199  int numRows = pfxGetNumConstraints(pair);
200  int i;
201  for (i=0;i<numRows;i++)
202  {
203  resolveSingleConstraintRowGeneric(solverBodyA,solverBodyB,constraintRow[i]);
204  }
205 
206  }
207  }
208  }
209  }
210 
211  barrier->sync();
212  }
213 
214  // Contact
215  for(uint32_t phaseId=0;phaseId<contactParallelGroup->numPhases;phaseId++) {
216  for(uint32_t batchId=0;batchId<contactParallelGroup->numBatches[phaseId];batchId++) {
217  uint32_t numPairs = contactParallelGroup->numPairs[phaseId*PFX_MAX_SOLVER_BATCHES+batchId];
218  if(batchId%numTasks == taskId && numPairs > 0) {
219  const PfxParallelBatch &batch = contactParallelBatches[phaseId*PFX_MAX_SOLVER_BATCHES+batchId];
220  for(uint32_t i=0;i<numPairs;i++) {
221  PfxConstraintPair &pair = contactPairs[batch.pairIndices[i]];
222  uint16_t iA = pfxGetRigidBodyIdA(pair);
223  uint16_t iB = pfxGetRigidBodyIdB(pair);
224 
225  uint32_t contactIndex = pfxGetConstraintId1(pair);
226  btPersistentManifold& contact = offsetContactManifolds[contactIndex];
227  btConstraintRow* contactConstraintRows = &offsetContactConstraintRows[contactIndex*12];
228 
229  PfxSolverBody &solverBodyA = offsetSolverBodies[iA];
230  PfxSolverBody &solverBodyB = offsetSolverBodies[iB];
231 
232  for(int j=0;j<contact.getNumContacts();j++) {
233  btManifoldPoint& cp = contact.getContactPoint(j);
234 
235  if(k==0) {
236  vmVector3 rA = rotate(solverBodyA.mOrientation,btReadVector3(cp.m_localPointA));
237  vmVector3 rB = rotate(solverBodyB.mOrientation,btReadVector3(cp.m_localPointB));
238 
239  float imp[3] =
240  {
241  cp.m_appliedImpulse,
242  cp.m_appliedImpulseLateral1,
243  cp.m_appliedImpulseLateral2
244  };
245  for(int k=0;k<3;k++)
246  {
247  vmVector3 normal = btReadVector3(contactConstraintRows[j*3+k].m_normal);
248  contactConstraintRows[j*3+k].m_accumImpulse = imp[k];
249  float deltaImpulse = contactConstraintRows[j*3+k].m_accumImpulse;
250  solverBodyA.mDeltaLinearVelocity += deltaImpulse * solverBodyA.mMassInv * normal;
251  solverBodyA.mDeltaAngularVelocity += deltaImpulse * solverBodyA.mInertiaInv * cross(rA,normal);
252  solverBodyB.mDeltaLinearVelocity -= deltaImpulse * solverBodyB.mMassInv * normal;
253  solverBodyB.mDeltaAngularVelocity -= deltaImpulse * solverBodyB.mInertiaInv * cross(rB,normal);
254  }
255  }
256  else {
257  btSolveContactConstraint(
258  contactConstraintRows[j*3],
259  contactConstraintRows[j*3+1],
260  contactConstraintRows[j*3+2],
261  btReadVector3(cp.m_localPointA),
262  btReadVector3(cp.m_localPointB),
263  solverBodyA,
264  solverBodyB,
265  cp.m_combinedFriction
266  );
267  }
268  }
269  }
270  }
271  }
272 
273  if (barrier)
274  barrier->sync();
275  }
276  }
277 }
278 
279 void CustomPostSolverTask(
280  TrbState *states,
281  PfxSolverBody *solverBodies,
282  uint32_t numRigidBodies)
283 {
284  for(uint32_t i=0;i<numRigidBodies;i++) {
285  TrbState &state = states[i];
286  PfxSolverBody &solverBody = solverBodies[i];
287  state.setLinearVelocity(state.getLinearVelocity()+solverBody.mDeltaLinearVelocity);
288  state.setAngularVelocity(state.getAngularVelocity()+solverBody.mDeltaAngularVelocity);
289  }
290 }
291 
292 void* SolverlsMemoryFunc()
293 {
294  //don't create local store memory, just return 0
295  return 0;
296 }
297 
298 
299 static SIMD_FORCE_INLINE
300 void pfxGetPlaneSpace(const vmVector3& n, vmVector3& p, vmVector3& q)
301 {
302  if(fabsf(n[2]) > 0.707f) {
303  // choose p in y-z plane
304  float a = n[1]*n[1] + n[2]*n[2];
305  float k = 1.0f/sqrtf(a);
306  p[0] = 0;
307  p[1] = -n[2]*k;
308  p[2] = n[1]*k;
309  // set q = n x p
310  q[0] = a*k;
311  q[1] = -n[0]*p[2];
312  q[2] = n[0]*p[1];
313  }
314  else {
315  // choose p in x-y plane
316  float a = n[0]*n[0] + n[1]*n[1];
317  float k = 1.0f/sqrtf(a);
318  p[0] = -n[1]*k;
319  p[1] = n[0]*k;
320  p[2] = 0;
321  // set q = n x p
322  q[0] = -n[2]*p[1];
323  q[1] = n[2]*p[0];
324  q[2] = a*k;
325  }
326 }
327 
328 
329 
330 #define PFX_CONTACT_SLOP 0.001f
331 
332 void btSetupContactConstraint(
333  btConstraintRow &constraintResponse,
334  btConstraintRow &constraintFriction1,
335  btConstraintRow &constraintFriction2,
336  float penetrationDepth,
337  float restitution,
338  float friction,
339  const vmVector3 &contactNormal,
340  const vmVector3 &contactPointA,
341  const vmVector3 &contactPointB,
342  const TrbState &stateA,
343  const TrbState &stateB,
344  PfxSolverBody &solverBodyA,
345  PfxSolverBody &solverBodyB,
346  const vmVector3& linVelA,
347  const vmVector3& angVelA,
348  const vmVector3& linVelB,
349  const vmVector3& angVelB,
350 
351  float separateBias,
352  float timeStep
353  )
354 {
355  vmVector3 rA = rotate(solverBodyA.mOrientation,contactPointA);
356  vmVector3 rB = rotate(solverBodyB.mOrientation,contactPointB);
357 
358  vmMatrix3 K = vmMatrix3::scale(vmVector3(solverBodyA.mMassInv + solverBodyB.mMassInv)) -
359  crossMatrix(rA) * solverBodyA.mInertiaInv * crossMatrix(rA) -
360  crossMatrix(rB) * solverBodyB.mInertiaInv * crossMatrix(rB);
361 
362  //use the velocities without the applied (gravity and external) forces for restitution computation
363  vmVector3 vArestitution = linVelA + cross(angVelA,rA);
364  vmVector3 vBrestitution = linVelB + cross(angVelB,rB);
365  vmVector3 vABrestitution = vArestitution-vBrestitution;
366 
367  vmVector3 vA = stateA.getLinearVelocity() + cross(stateA.getAngularVelocity(),rA);
368  vmVector3 vB = stateB.getLinearVelocity() + cross(stateB.getAngularVelocity(),rB);
369  vmVector3 vAB = vA-vB;
370 
371 
372  vmVector3 tangent1,tangent2;
373  btPlaneSpace1(contactNormal,tangent1,tangent2);
374 
375 // constraintResponse.m_accumImpulse = 0.f;
376 // constraintFriction1.m_accumImpulse = 0.f;
377 // constraintFriction2.m_accumImpulse = 0.f;
378 
379  // Contact Constraint
380  {
381  vmVector3 normal = contactNormal;
382 
383  float denom = dot(K*normal,normal);
384 
385  constraintResponse.m_rhs = -(1.0f+restitution)*dot(vAB,normal); // velocity error
386  constraintResponse.m_rhs -= (separateBias * btMin(0.0f,penetrationDepth+PFX_CONTACT_SLOP)) / timeStep; // position error
387  constraintResponse.m_rhs /= denom;
388  constraintResponse.m_jacDiagInv = 1.0f/denom;
389  constraintResponse.m_lowerLimit = 0.0f;
390  constraintResponse.m_upperLimit = SIMD_INFINITY;
391  btStoreVector3(normal,constraintResponse.m_normal);
392  }
393 
394  // Friction Constraint 1
395  {
396  vmVector3 normal = tangent1;
397 
398  float denom = dot(K*normal,normal);
399 
400  constraintFriction1.m_jacDiagInv = 1.0f/denom;
401  constraintFriction1.m_rhs = -dot(vAB,normal);
402  constraintFriction1.m_rhs *= constraintFriction1.m_jacDiagInv;
403  constraintFriction1.m_lowerLimit = 0.0f;
404  constraintFriction1.m_upperLimit = SIMD_INFINITY;
405  btStoreVector3(normal,constraintFriction1.m_normal);
406  }
407 
408  // Friction Constraint 2
409  {
410  vmVector3 normal = tangent2;
411 
412  float denom = dot(K*normal,normal);
413 
414  constraintFriction2.m_jacDiagInv = 1.0f/denom;
415  constraintFriction2.m_rhs = -dot(vAB,normal);
416  constraintFriction2.m_rhs *= constraintFriction2.m_jacDiagInv;
417  constraintFriction2.m_lowerLimit = 0.0f;
418  constraintFriction2.m_upperLimit = SIMD_INFINITY;
419  btStoreVector3(normal,constraintFriction2.m_normal);
420  }
421 }
422 
423 
424 void CustomSetupContactConstraintsTask(
425  PfxConstraintPair *contactPairs,uint32_t numContactPairs,
426  btPersistentManifold* offsetContactManifolds,
427  btConstraintRow* offsetContactConstraintRows,
428  TrbState *offsetRigStates,
429  PfxSolverBody *offsetSolverBodies,
430  uint32_t numRigidBodies,
431  float separateBias,
432  float timeStep)
433 {
434  for(uint32_t i=0;i<numContactPairs;i++) {
435  PfxConstraintPair &pair = contactPairs[i];
436  if(!pfxGetActive(pair) || pfxGetNumConstraints(pair) == 0 ||
437  ((pfxGetMotionMaskA(pair)&PFX_MOTION_MASK_STATIC) && (pfxGetMotionMaskB(pair)&PFX_MOTION_MASK_STATIC)) ) {
438  continue;
439  }
440 
441  uint16_t iA = pfxGetRigidBodyIdA(pair);
442  uint16_t iB = pfxGetRigidBodyIdB(pair);
443 
444  int id = pfxGetConstraintId1(pair);
445  btPersistentManifold& contact = offsetContactManifolds[id];
446  btConstraintRow* contactConstraintRows = &offsetContactConstraintRows[id*12];
447 
448  TrbState &stateA = offsetRigStates[iA];
449 // PfxRigBody &bodyA = offsetRigBodies[iA];
450  PfxSolverBody &solverBodyA = offsetSolverBodies[iA];
451 
452  TrbState &stateB = offsetRigStates[iB];
453 // PfxRigBody &bodyB = offsetRigBodies[iB];
454  PfxSolverBody &solverBodyB = offsetSolverBodies[iB];
455 
456  float restitution = 0.5f * (solverBodyA.restitution + solverBodyB.restitution);
457  //if(contact.getDuration() > 1) restitution = 0.0f;
458 
459  float friction = sqrtf(solverBodyA.friction * solverBodyB.friction);
460 
461  for(int j=0;j<contact.getNumContacts();j++) {
462  btManifoldPoint& cp = contact.getContactPoint(j);
463 
464  //pass the velocities without the applied (gravity and external) forces for restitution computation
465  const btRigidBody* rbA = btRigidBody::upcast(contact.getBody0());
466  const btRigidBody* rbB = btRigidBody::upcast(contact.getBody1());
467 
468  btVector3 linVelA, linVelB;
469  btVector3 angVelA, angVelB;
470 
471  if (rbA && (rbA->getInvMass()>0.f))
472  {
473  linVelA = rbA->getLinearVelocity();
474  angVelA = rbA->getAngularVelocity();
475  } else
476  {
477  linVelA.setValue(0,0,0);
478  angVelA.setValue(0,0,0);
479  }
480 
481  if (rbB && (rbB->getInvMass()>0.f))
482  {
483  linVelB = rbB->getLinearVelocity();
484  angVelB = rbB->getAngularVelocity();
485  } else
486  {
487  linVelB.setValue(0,0,0);
488  angVelB.setValue(0,0,0);
489  }
490 
491 
492 
493  btSetupContactConstraint(
494  contactConstraintRows[j*3],
495  contactConstraintRows[j*3+1],
496  contactConstraintRows[j*3+2],
497  cp.getDistance(),
498  restitution,
499  friction,
500  btReadVector3(cp.m_normalWorldOnB),//.mConstraintRow[0].m_normal),
501  btReadVector3(cp.m_localPointA),
502  btReadVector3(cp.m_localPointB),
503  stateA,
504  stateB,
505  solverBodyA,
506  solverBodyB,
507  (const vmVector3&)linVelA, (const vmVector3&)angVelA,
508  (const vmVector3&)linVelB, (const vmVector3&)angVelB,
509  separateBias,
510  timeStep
511  );
512  }
513 
514  //contact.setCompositeFriction(friction);
515  }
516 }
517 
518 
519 void CustomWritebackContactConstraintsTask(
520  PfxConstraintPair *contactPairs,uint32_t numContactPairs,
521  btPersistentManifold* offsetContactManifolds,
522  btConstraintRow* offsetContactConstraintRows,
523  TrbState *offsetRigStates,
524  PfxSolverBody *offsetSolverBodies,
525  uint32_t numRigidBodies,
526  float separateBias,
527  float timeStep)
528 {
529  for(uint32_t i=0;i<numContactPairs;i++) {
530  PfxConstraintPair &pair = contactPairs[i];
531  if(!pfxGetActive(pair) || pfxGetNumConstraints(pair) == 0 ||
532  ((pfxGetMotionMaskA(pair)&PFX_MOTION_MASK_STATIC) && (pfxGetMotionMaskB(pair)&PFX_MOTION_MASK_STATIC)) ) {
533  continue;
534  }
535  int id = pfxGetConstraintId1(pair);
536  btPersistentManifold& contact = offsetContactManifolds[id];
537  btConstraintRow* contactConstraintRows = &offsetContactConstraintRows[id*12];
538  for(int j=0;j<contact.getNumContacts();j++) {
539  btManifoldPoint& cp = contact.getContactPoint(j);
540  cp.m_appliedImpulse = contactConstraintRows[j*3+0].m_accumImpulse;
541  cp.m_appliedImpulseLateral1 = contactConstraintRows[j*3+1].m_accumImpulse;
542  cp.m_appliedImpulseLateral2 = contactConstraintRows[j*3+2].m_accumImpulse;
543  }
544  //contact.setCompositeFriction(friction);
545  }
546 }
547 
548 void SolverThreadFunc(void* userPtr,void* lsMemory)
549 {
550  btConstraintSolverIO* io = (btConstraintSolverIO*)(userPtr);//arg->io);
551  btCriticalSection* criticalsection = io->setupContactConstraints.criticalSection;
552 
553 
554  //CustomCriticalSection *criticalsection = &io->m_cs;
555  switch(io->cmd) {
556 
557  case PFX_CONSTRAINT_SOLVER_CMD_SOLVE_CONSTRAINTS:
558  CustomSolveConstraintsTaskParallel(
559  io->solveConstraints.contactParallelGroup,
560  io->solveConstraints.contactParallelBatches,
561  io->solveConstraints.contactPairs,
562  io->solveConstraints.numContactPairs,
563  io->solveConstraints.offsetContactManifolds,
564  io->solveConstraints.offsetContactConstraintRows,
565 
566  io->solveConstraints.jointParallelGroup,
567  io->solveConstraints.jointParallelBatches,
568  io->solveConstraints.jointPairs,
569  io->solveConstraints.numJointPairs,
570  io->solveConstraints.offsetSolverConstraints,
571  io->solveConstraints.offsetRigStates1,
572  io->solveConstraints.offsetSolverBodies,
573  io->solveConstraints.numRigidBodies,
574  io->solveConstraints.iteration,
575 
576  io->solveConstraints.taskId,
577  io->maxTasks1,
578  io->solveConstraints.barrier
579  );
580  break;
581 
582  case PFX_CONSTRAINT_SOLVER_CMD_POST_SOLVER:
583  CustomPostSolverTask( io->postSolver.states,io->postSolver.solverBodies, io->postSolver.numRigidBodies);
584  break;
585 
586 
587  case PFX_CONSTRAINT_SOLVER_CMD_SETUP_CONTACT_CONSTRAINTS:
588  {
589  bool empty = false;
590  while(!empty) {
591  int start,batch;
592 
593  criticalsection->lock();
594 
595  start = (int)criticalsection->getSharedParam(0);
596  batch = (int)criticalsection->getSharedParam(1);
597 
598  //PFX_PRINTF("taskId %d start %d num %d\n",arg->taskId,start,batch);
599 
600  // ŽŸ‚̃oƒbƒtƒ@‚ðƒZƒbƒg
601  int nextStart = start + batch;
602  int rest = btMax((int)io->setupContactConstraints.numContactPairs1 - nextStart,0);
603  int nextBatch = (rest > batch)?batch:rest;
604 
605  criticalsection->setSharedParam(0,nextStart);
606  criticalsection->setSharedParam(1,nextBatch);
607 
608  criticalsection->unlock();
609 
610  if(batch > 0) {
611  CustomSetupContactConstraintsTask(
612  io->setupContactConstraints.offsetContactPairs+start,batch,
613  io->setupContactConstraints.offsetContactManifolds,
614  io->setupContactConstraints.offsetContactConstraintRows,
615  io->setupContactConstraints.offsetRigStates,
616 // io->setupContactConstraints.offsetRigBodies,
617  io->setupContactConstraints.offsetSolverBodies,
618  io->setupContactConstraints.numRigidBodies,
619  io->setupContactConstraints.separateBias,
620  io->setupContactConstraints.timeStep);
621  }
622  else {
623  empty = true;
624  }
625  }
626  }
627  break;
628 
629  case PFX_CONSTRAINT_SOLVER_CMD_WRITEBACK_APPLIED_IMPULSES_CONTACT_CONSTRAINTS:
630  {
631  bool empty = false;
632  while(!empty) {
633  int start,batch;
634 
635  criticalsection->lock();
636 
637  start = (int)criticalsection->getSharedParam(0);
638  batch = (int)criticalsection->getSharedParam(1);
639 
640  //PFX_PRINTF("taskId %d start %d num %d\n",arg->taskId,start,batch);
641 
642  // ŽŸ‚̃oƒbƒtƒ@‚ðƒZƒbƒg
643  int nextStart = start + batch;
644  int rest = btMax((int)io->setupContactConstraints.numContactPairs1 - nextStart,0);
645  int nextBatch = (rest > batch)?batch:rest;
646 
647  criticalsection->setSharedParam(0,nextStart);
648  criticalsection->setSharedParam(1,nextBatch);
649 
650  criticalsection->unlock();
651 
652  if(batch > 0) {
653  CustomWritebackContactConstraintsTask(
654  io->setupContactConstraints.offsetContactPairs+start,batch,
655  io->setupContactConstraints.offsetContactManifolds,
656  io->setupContactConstraints.offsetContactConstraintRows,
657  io->setupContactConstraints.offsetRigStates,
658 // io->setupContactConstraints.offsetRigBodies,
659  io->setupContactConstraints.offsetSolverBodies,
660  io->setupContactConstraints.numRigidBodies,
661  io->setupContactConstraints.separateBias,
662  io->setupContactConstraints.timeStep);
663  }
664  else {
665  empty = true;
666  }
667  }
668  }
669  break;
670 
671  default:
672  {
673  btAssert(0);
674  }
675  }
676 
677 }
678 
679 
680 void CustomSetupContactConstraintsNew(
681  PfxConstraintPair *contactPairs1,uint32_t numContactPairs,
682  btPersistentManifold *offsetContactManifolds,
683  btConstraintRow* offsetContactConstraintRows,
684  TrbState *offsetRigStates,
685  PfxSolverBody *offsetSolverBodies,
686  uint32_t numRigidBodies,
687  float separationBias,
688  float timeStep,
689  class btThreadSupportInterface* threadSupport,
690  btCriticalSection* criticalSection,
691  btConstraintSolverIO *io ,
692  uint8_t cmd
693  )
694 {
695  int maxTasks = threadSupport->getNumTasks();
696 
697  int div = (int)maxTasks * 4;
698  int batch = ((int)numContactPairs + div - 1) / div;
699 #ifdef __PPU__
700  BulletPE2ConstraintSolverSpursSupport* spursThread = (BulletPE2ConstraintSolverSpursSupport*) threadSupport;
701 #endif
702  if (criticalSection)
703  {
704  criticalSection->setSharedParam(0,0);
705  criticalSection->setSharedParam(1,btMin(batch,64)); // batched number
706  } else
707  {
708 #ifdef __PPU__
709  spursThread->setSharedParam(0,0);
710  spursThread->setSharedParam(1,btMin(batch,64)); // batched number
711 #endif //__PPU__
712  }
713 
714  for(int t=0;t<maxTasks;t++) {
715  io[t].cmd = cmd;
716  io[t].setupContactConstraints.offsetContactPairs = contactPairs1;
717  io[t].setupContactConstraints.numContactPairs1 = numContactPairs;
718  io[t].setupContactConstraints.offsetRigStates = offsetRigStates;
719  io[t].setupContactConstraints.offsetContactManifolds = offsetContactManifolds;
720  io[t].setupContactConstraints.offsetContactConstraintRows = offsetContactConstraintRows;
721  io[t].setupContactConstraints.offsetSolverBodies = offsetSolverBodies;
722  io[t].setupContactConstraints.numRigidBodies = numRigidBodies;
723  io[t].setupContactConstraints.separateBias = separationBias;
724  io[t].setupContactConstraints.timeStep = timeStep;
725  io[t].setupContactConstraints.criticalSection = criticalSection;
726  io[t].maxTasks1 = maxTasks;
727 #ifdef __PPU__
728  io[t].barrierAddr2 = (unsigned int)spursThread->getBarrierAddress();
729  io[t].criticalsectionAddr2 = (unsigned int)spursThread->getCriticalSectionAddress();
730 #endif
731 
732 
733 //#define SEQUENTIAL_SETUP
734 #ifdef SEQUENTIAL_SETUP
735  CustomSetupContactConstraintsTask(contactPairs1,numContactPairs,offsetContactManifolds,offsetRigStates,offsetSolverBodies,numRigidBodies,separationBias,timeStep);
736 #else
737  threadSupport->sendRequest(1,(ppu_address_t)&io[t],t);
738 #endif
739 
740  }
741 #ifndef SEQUENTIAL_SETUP
742  unsigned int arg0,arg1;
743  for(int t=0;t<maxTasks;t++) {
744  arg0 = t;
745  threadSupport->waitForResponse(&arg0,&arg1);
746  }
747 #endif //SEQUENTIAL_SETUP
748 
749 }
750 
751 
752 void CustomSplitConstraints(
753  PfxConstraintPair *pairs,uint32_t numPairs,
754  PfxParallelGroup &group,PfxParallelBatch *batches,
755  uint32_t numTasks,
756  uint32_t numRigidBodies,
757  void *poolBuff,
758  uint32_t poolBytes
759  )
760 {
761  HeapManager pool((unsigned char*)poolBuff,poolBytes);
762 
763  // ƒXƒe[ƒgƒ`ƒFƒbƒN—pƒrƒbƒgƒtƒ‰ƒOƒe[ƒuƒ‹
764  int bufSize = sizeof(uint8_t)*numRigidBodies;
765  bufSize = ((bufSize+127)>>7)<<7; // 128 bytes alignment
766  uint8_t *bodyTable = (uint8_t*)pool.allocate(bufSize,HeapManager::ALIGN128);
767 
768  // ƒyƒAƒ`ƒFƒbƒN—pƒrƒbƒgƒtƒ‰ƒOƒe[ƒuƒ‹
769  uint32_t *pairTable;
770  size_t allocSize = sizeof(uint32_t)*((numPairs+31)/32);
771  pairTable = (uint32_t*)pool.allocate(allocSize);
772  memset(pairTable,0,allocSize);
773 
774  // –Ú•W‚Æ‚·‚镪Š„”
775  uint32_t targetCount = btMax(uint32_t(PFX_MIN_SOLVER_PAIRS),btMin(numPairs / (numTasks*2),uint32_t(PFX_MAX_SOLVER_PAIRS)));
776  uint32_t startIndex = 0;
777 
778  uint32_t phaseId;
779  uint32_t batchId;
780  uint32_t totalCount=0;
781 
782  uint32_t maxBatches = btMin(numTasks,uint32_t(PFX_MAX_SOLVER_BATCHES));
783 
784  for(phaseId=0;phaseId<PFX_MAX_SOLVER_PHASES&&totalCount<numPairs;phaseId++) {
785  bool startIndexCheck = true;
786 
787  group.numBatches[phaseId] = 0;
788 
789  uint32_t i = startIndex;
790 
791  // ƒ`ƒFƒbƒN—pƒrƒbƒgƒtƒ‰ƒOƒe[ƒuƒ‹‚ðƒNƒŠƒA
792  memset(bodyTable,0xff,bufSize);
793 
794  for(batchId=0;i<numPairs&&totalCount<numPairs&&batchId<maxBatches;batchId++) {
795  uint32_t pairCount=0;
796 
797  PfxParallelBatch &batch = batches[phaseId*PFX_MAX_SOLVER_BATCHES+batchId];
798  uint32_t pairId = 0;
799 
800  for(;i<numPairs&&pairCount<targetCount;i++) {
801  uint32_t idxP = i>>5;
802  uint32_t maskP = 1L << (i & 31);
803 
804  //pair is already assigned to a phase/batch
805  if(pairTable[idxP] & maskP) {
806  continue;
807  }
808 
809  uint32_t idxA = pfxGetRigidBodyIdA(pairs[i]);
810  uint32_t idxB = pfxGetRigidBodyIdB(pairs[i]);
811 
812  // —¼•û‚Æ‚àƒAƒNƒeƒBƒu‚Å‚È‚¢A‚Ü‚½‚ÍÕ“Ë“_‚ª‚O‚̃yƒA‚Í“o˜^‘ÎÛ‚©‚ç‚Í‚¸‚·
813  if(!pfxGetActive(pairs[i]) || pfxGetNumConstraints(pairs[i]) == 0 ||
814  ((pfxGetMotionMaskA(pairs[i])&PFX_MOTION_MASK_STATIC) && (pfxGetMotionMaskB(pairs[i])&PFX_MOTION_MASK_STATIC)) ) {
815  if(startIndexCheck)
816  startIndex++;
817  //assign pair -> skip it because it has no constraints
818  pairTable[idxP] |= maskP;
819  totalCount++;
820  continue;
821  }
822 
823  // ˆË‘¶«‚̃`ƒFƒbƒN
824  if( (bodyTable[idxA] != batchId && bodyTable[idxA] != 0xff) ||
825  (bodyTable[idxB] != batchId && bodyTable[idxB] != 0xff) ) {
826  startIndexCheck = false;
827  //bodies of the pair are already assigned to another batch within this phase
828  continue;
829  }
830 
831  // ˆË‘¶«”»’èƒe[ƒuƒ‹‚É“o˜^
832  if(pfxGetMotionMaskA(pairs[i])&PFX_MOTION_MASK_DYNAMIC)
833  bodyTable[idxA] = batchId;
834  if(pfxGetMotionMaskB(pairs[i])&PFX_MOTION_MASK_DYNAMIC)
835  bodyTable[idxB] = batchId;
836 
837  if(startIndexCheck)
838  startIndex++;
839 
840  pairTable[idxP] |= maskP;
841  //add the pair 'i' to the current batch
842  batch.pairIndices[pairId++] = i;
843  pairCount++;
844  }
845 
846  group.numPairs[phaseId*PFX_MAX_SOLVER_BATCHES+batchId] = (uint16_t)pairId;
847  totalCount += pairCount;
848  }
849 
850  group.numBatches[phaseId] = batchId;
851  }
852 
853  group.numPhases = phaseId;
854 
855  pool.clear();
856 }
857 
858 
859 
860 void CustomSolveConstraintsParallel(
861  PfxConstraintPair *contactPairs,uint32_t numContactPairs,
862 
863  PfxConstraintPair *jointPairs,uint32_t numJointPairs,
864  btPersistentManifold* offsetContactManifolds,
865  btConstraintRow* offsetContactConstraintRows,
866  btSolverConstraint* offsetSolverConstraints,
867  TrbState *offsetRigStates,
868  PfxSolverBody *offsetSolverBodies,
869  uint32_t numRigidBodies,
870  struct btConstraintSolverIO* io,
871  class btThreadSupportInterface* threadSupport,
872  int iteration,
873  void* poolBuf,
874  int poolBytes,
875  class btBarrier* barrier)
876  {
877 
878  int maxTasks = threadSupport->getNumTasks();
879 // config.taskManager->setTaskEntry(PFX_SOLVER_ENTRY);
880 
881  HeapManager pool((unsigned char*)poolBuf,poolBytes);
882 
883  {
884  PfxParallelGroup *cgroup = (PfxParallelGroup*)pool.allocate(sizeof(PfxParallelGroup));
885  PfxParallelBatch *cbatches = (PfxParallelBatch*)pool.allocate(sizeof(PfxParallelBatch)*(PFX_MAX_SOLVER_PHASES*PFX_MAX_SOLVER_BATCHES),128);
886  PfxParallelGroup *jgroup = (PfxParallelGroup*)pool.allocate(sizeof(PfxParallelGroup));
887  PfxParallelBatch *jbatches = (PfxParallelBatch*)pool.allocate(sizeof(PfxParallelBatch)*(PFX_MAX_SOLVER_PHASES*PFX_MAX_SOLVER_BATCHES),128);
888 
889  uint32_t tmpBytes = poolBytes - 2 * (sizeof(PfxParallelGroup) + sizeof(PfxParallelBatch)*(PFX_MAX_SOLVER_PHASES*PFX_MAX_SOLVER_BATCHES) + 128);
890  void *tmpBuff = pool.allocate(tmpBytes);
891 
892  {
893  BT_PROFILE("CustomSplitConstraints");
894  CustomSplitConstraints(contactPairs,numContactPairs,*cgroup,cbatches,maxTasks,numRigidBodies,tmpBuff,tmpBytes);
895  CustomSplitConstraints(jointPairs,numJointPairs,*jgroup,jbatches,maxTasks,numRigidBodies,tmpBuff,tmpBytes);
896  }
897 
898  {
899  BT_PROFILE("PFX_CONSTRAINT_SOLVER_CMD_SOLVE_CONSTRAINTS");
900 //#define SOLVE_SEQUENTIAL
901 #ifdef SOLVE_SEQUENTIAL
902  CustomSolveConstraintsTask(
903  io->solveConstraints.contactParallelGroup,
904  io->solveConstraints.contactParallelBatches,
905  io->solveConstraints.contactPairs,
906  io->solveConstraints.numContactPairs,
907  io->solveConstraints.offsetContactManifolds,
908 
909  io->solveConstraints.jointParallelGroup,
910  io->solveConstraints.jointParallelBatches,
911  io->solveConstraints.jointPairs,
912  io->solveConstraints.numJointPairs,
913  io->solveConstraints.offsetSolverConstraints,
914 
915  io->solveConstraints.offsetRigStates1,
916  io->solveConstraints.offsetSolverBodies,
917  io->solveConstraints.numRigidBodies,
918  io->solveConstraints.iteration,0,1,0);//arg->taskId,1,0);//,arg->maxTasks,arg->barrier);
919 #else
920  for(int t=0;t<maxTasks;t++) {
921  io[t].cmd = PFX_CONSTRAINT_SOLVER_CMD_SOLVE_CONSTRAINTS;
922  io[t].solveConstraints.contactParallelGroup = cgroup;
923  io[t].solveConstraints.contactParallelBatches = cbatches;
924  io[t].solveConstraints.contactPairs = contactPairs;
925  io[t].solveConstraints.numContactPairs = numContactPairs;
926  io[t].solveConstraints.offsetContactManifolds = offsetContactManifolds;
927  io[t].solveConstraints.offsetContactConstraintRows = offsetContactConstraintRows;
928  io[t].solveConstraints.jointParallelGroup = jgroup;
929  io[t].solveConstraints.jointParallelBatches = jbatches;
930  io[t].solveConstraints.jointPairs = jointPairs;
931  io[t].solveConstraints.numJointPairs = numJointPairs;
932  io[t].solveConstraints.offsetSolverConstraints = offsetSolverConstraints;
933  io[t].solveConstraints.offsetRigStates1 = offsetRigStates;
934  io[t].solveConstraints.offsetSolverBodies = offsetSolverBodies;
935  io[t].solveConstraints.numRigidBodies = numRigidBodies;
936  io[t].solveConstraints.iteration = iteration;
937  io[t].solveConstraints.taskId = t;
938  io[t].solveConstraints.barrier = barrier;
939 
940  io[t].maxTasks1 = maxTasks;
941 #ifdef __PPU__
942  BulletPE2ConstraintSolverSpursSupport* spursThread = (BulletPE2ConstraintSolverSpursSupport*) threadSupport;
943  io[t].barrierAddr2 = (unsigned int) spursThread->getBarrierAddress();
944  io[t].criticalsectionAddr2 = (unsigned int)spursThread->getCriticalSectionAddress();
945 #endif
946 
947  threadSupport->sendRequest(1,(ppu_address_t)&io[t],t);
948  }
949 
950  unsigned int arg0,arg1;
951  for(int t=0;t<maxTasks;t++) {
952  arg0 = t;
953  threadSupport->waitForResponse(&arg0,&arg1);
954  }
955 #endif
956  }
957  pool.clear();
958  }
959 
960  {
961  BT_PROFILE("PFX_CONSTRAINT_SOLVER_CMD_POST_SOLVER");
962  int batch = ((int)numRigidBodies + maxTasks - 1) / maxTasks;
963  int rest = (int)numRigidBodies;
964  int start = 0;
965 
966  for(int t=0;t<maxTasks;t++) {
967  int num = (rest - batch ) > 0 ? batch : rest;
968  io[t].cmd = PFX_CONSTRAINT_SOLVER_CMD_POST_SOLVER;
969  io[t].postSolver.states = offsetRigStates + start;
970  io[t].postSolver.solverBodies = offsetSolverBodies + start;
971  io[t].postSolver.numRigidBodies = (uint32_t)num;
972  io[t].maxTasks1 = maxTasks;
973 #ifdef __PPU__
974  BulletPE2ConstraintSolverSpursSupport* spursThread = (BulletPE2ConstraintSolverSpursSupport*) threadSupport;
975  io[t].barrierAddr2 = (unsigned int)spursThread->getBarrierAddress();
976  io[t].criticalsectionAddr2 = (unsigned int)spursThread->getCriticalSectionAddress();
977 #endif
978 
979 #ifdef SOLVE_SEQUENTIAL
980  CustomPostSolverTask( io[t].postSolver.states,io[t].postSolver.solverBodies, io[t].postSolver.numRigidBodies);
981 #else
982  threadSupport->sendRequest(1,(ppu_address_t)&io[t],t);
983 #endif
984  rest -= num;
985  start += num;
986  }
987 
988  unsigned int arg0,arg1;
989  for(int t=0;t<maxTasks;t++) {
990 #ifndef SOLVE_SEQUENTIAL
991  arg0 = t;
992  threadSupport->waitForResponse(&arg0,&arg1);
993 #endif
994  }
995  }
996 
997 }
998 
999 
1000 
1001 void BPE_customConstraintSolverSequentialNew(unsigned int new_num, PfxBroadphasePair *new_pairs1 ,
1002  btPersistentManifold* offsetContactManifolds,
1003  PfxConstraintRow* offsetContactConstraintRows,
1004  TrbState* states,int numRigidBodies,
1005  struct PfxSolverBody* solverBodies,
1006  PfxConstraintPair* jointPairs, unsigned int numJoints,
1007  btSolverConstraint* offsetSolverConstraints,
1008  float separateBias,
1009  float timeStep,
1010  int iteration,
1011  btThreadSupportInterface* solverThreadSupport,
1012  btCriticalSection* criticalSection,
1013  struct btConstraintSolverIO* solverIO,
1014  btBarrier* barrier
1015  )
1016 {
1017 
1018  {
1019  BT_PROFILE("pfxSetupConstraints");
1020 
1021  for(uint32_t i=0;i<numJoints;i++) {
1022  // î•ñ‚ÌXV
1023  PfxConstraintPair &pair = jointPairs[i];
1024  int idA = pfxGetRigidBodyIdA(pair);
1025 
1026  if (idA != 65535)
1027  {
1028  pfxSetMotionMaskA(pair,states[pfxGetRigidBodyIdA(pair)].getMotionMask());
1029  }
1030  else
1031  {
1032  pfxSetMotionMaskA(pair,PFX_MOTION_MASK_STATIC);
1033  }
1034  int idB = pfxGetRigidBodyIdB(pair);
1035  if (idB!= 65535)
1036  {
1037  pfxSetMotionMaskB(pair,states[pfxGetRigidBodyIdB(pair)].getMotionMask());
1038  } else
1039  {
1040  pfxSetMotionMaskB(pair,PFX_MOTION_MASK_STATIC);
1041  }
1042  }
1043 
1044 // CustomSetupJointConstraintsSeq( jointPairs,numJoints,joints, states, solverBodies, numRigidBodies, timeStep);
1045 
1046 #ifdef SEQUENTIAL_SETUP
1047  CustomSetupContactConstraintsSeqNew(
1048  (PfxConstraintPair*)new_pairs1,new_num,contacts,
1049  states,
1050  solverBodies,
1051  numRigidBodies,
1052  separateBias,
1053  timeStep);
1054 #else
1055 
1056  CustomSetupContactConstraintsNew(
1057  (PfxConstraintPair*)new_pairs1,new_num,
1058  offsetContactManifolds,
1059  offsetContactConstraintRows,
1060  states,
1061  solverBodies,
1062  numRigidBodies,
1063  separateBias,
1064  timeStep,
1065  solverThreadSupport,
1066  criticalSection,solverIO,
1067  PFX_CONSTRAINT_SOLVER_CMD_SETUP_CONTACT_CONSTRAINTS
1068  );
1069 
1070 #endif //SEQUENTIAL_SETUP
1071 
1072  }
1073  {
1074  BT_PROFILE("pfxSolveConstraints");
1075 
1076 //#define SEQUENTIAL
1077 #ifdef SEQUENTIAL
1078  CustomSolveConstraintsSeq(
1079  (PfxConstraintPair*)new_pairs1,new_num,contacts,
1080  jointPairs,numJoints,
1081  states,
1082  solverBodies,
1083  numRigidBodies,
1084  separateBias,
1085  timeStep,
1086  iteration);
1087 #else //SEQUENTIAL
1088  CustomSolveConstraintsParallel(
1089  (PfxConstraintPair*)new_pairs1,new_num,
1090  jointPairs,numJoints,
1091  offsetContactManifolds,
1092  offsetContactConstraintRows,
1093  offsetSolverConstraints,
1094  states,
1095  solverBodies,
1096  numRigidBodies,
1097  solverIO, solverThreadSupport,
1098  iteration,
1099  tmp_buff,
1100  TMP_BUFF_BYTES,
1101  barrier
1102  );
1103 
1104 #endif //SEQUENTIAL
1105  }
1106 
1107  {
1108  BT_PROFILE("writeback appliedImpulses");
1109 
1110  CustomSetupContactConstraintsNew(
1111  (PfxConstraintPair*)new_pairs1,new_num,
1112  offsetContactManifolds,
1113  offsetContactConstraintRows,
1114  states,
1115  solverBodies,
1116  numRigidBodies,
1117  separateBias,
1118  timeStep,
1119  solverThreadSupport,
1120  criticalSection,solverIO,
1121  PFX_CONSTRAINT_SOLVER_CMD_WRITEBACK_APPLIED_IMPULSES_CONTACT_CONSTRAINTS
1122  );
1123  }
1124 
1125 }
1126 
1127 
1128 struct btParallelSolverMemoryCache
1129 {
1130  btAlignedObjectArray<TrbState> m_mystates;
1131  btAlignedObjectArray<PfxSolverBody> m_mysolverbodies;
1132  btAlignedObjectArray<PfxBroadphasePair> m_mypairs;
1133  btAlignedObjectArray<PfxConstraintPair> m_jointPairs;
1134  btAlignedObjectArray<PfxConstraintRow> m_constraintRows;
1135 
1136 };
1137 
1138 
1139 btConstraintSolverIO* createSolverIO(int numThreads)
1140 {
1141  return new btConstraintSolverIO[numThreads];
1142 }
1143 
1144 btParallelConstraintSolver::btParallelConstraintSolver(btThreadSupportInterface* solverThreadSupport)
1145 {
1146 
1147  m_solverThreadSupport = solverThreadSupport;//createSolverThreadSupport(maxNumThreads);
1148  m_solverIO = createSolverIO(m_solverThreadSupport->getNumTasks());
1149 
1150  m_barrier = m_solverThreadSupport->createBarrier();
1151  m_criticalSection = m_solverThreadSupport->createCriticalSection();
1152 
1153  m_memoryCache = new btParallelSolverMemoryCache();
1154 }
1155 
1156 btParallelConstraintSolver::~btParallelConstraintSolver()
1157 {
1158  delete m_memoryCache;
1159  delete m_solverIO;
1160  m_solverThreadSupport->deleteBarrier(m_barrier);
1161  m_solverThreadSupport->deleteCriticalSection(m_criticalSection);
1162 }
1163 
1164 
1165 
1166 btScalar btParallelConstraintSolver::solveGroup(btCollisionObject** bodies1,int numRigidBodies,btPersistentManifold** manifoldPtr,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher)
1167 {
1168 
1169 /* int sz = sizeof(PfxSolverBody);
1170  int sz2 = sizeof(vmVector3);
1171  int sz3 = sizeof(vmMatrix3);
1172  int sz4 = sizeof(vmQuat);
1173  int sz5 = sizeof(btConstraintRow);
1174  int sz6 = sizeof(btSolverConstraint);
1175  int sz7 = sizeof(TrbState);
1176 */
1177 
1178  btPersistentManifold* offsetContactManifolds= (btPersistentManifold*) dispatcher->getInternalManifoldPool()->getPoolAddress();
1179 
1180 
1181  m_memoryCache->m_mysolverbodies.resize(numRigidBodies);
1182  m_memoryCache->m_mystates.resize(numRigidBodies);
1183 
1184  {
1185  BT_PROFILE("create states and solver bodies");
1186  for (int i=0;i<numRigidBodies;i++)
1187  {
1188  btCollisionObject* obj = bodies1[i];
1189  obj->setCompanionId(i);
1190 
1191  PfxSolverBody& solverBody = m_memoryCache->m_mysolverbodies[i];
1192  btRigidBody* rb = btRigidBody::upcast(obj);
1193  TrbState& state = m_memoryCache->m_mystates[i];
1194 
1195  state.reset();
1196  const btQuaternion& orgOri = obj->getWorldTransform().getRotation();
1197  vmQuat orn(orgOri.getX(),orgOri.getY(),orgOri.getZ(),orgOri.getW());
1198  state.setPosition(getVmVector3(obj->getWorldTransform().getOrigin()));
1199  state.setOrientation(orn);
1200  state.setPosition(state.getPosition());
1201  state.setRigidBodyId(i);
1202  state.setAngularDamping(0);
1203  state.setLinearDamping(0);
1204 
1205 
1206  solverBody.mOrientation = state.getOrientation();
1207  solverBody.mDeltaLinearVelocity = vmVector3(0.0f);
1208  solverBody.mDeltaAngularVelocity = vmVector3(0.0f);
1209  solverBody.friction = obj->getFriction();
1210  solverBody.restitution = obj->getRestitution();
1211 
1212  state.resetSleepCount();
1213 
1214  //if(state.getMotionMask()&PFX_MOTION_MASK_DYNAMIC) {
1215  if (rb && (rb->getInvMass()>0.f))
1216  {
1217  btVector3 angVelPlusForces = rb->getAngularVelocity()+rb->getTotalTorque()*rb->getInvInertiaTensorWorld()*infoGlobal.m_timeStep;
1218  btVector3 linVelPlusForces = rb->getLinearVelocity()+rb->getTotalForce()*rb->getInvMass()*infoGlobal.m_timeStep;
1219 
1220  state.setAngularVelocity((const vmVector3&)angVelPlusForces);
1221  state.setLinearVelocity((const vmVector3&) linVelPlusForces);
1222 
1223  state.setMotionType(PfxMotionTypeActive);
1224  vmMatrix3 ori(solverBody.mOrientation);
1225  vmMatrix3 localInvInertia = vmMatrix3::identity();
1226  localInvInertia.setCol(0,vmVector3(rb->getInvInertiaDiagLocal().getX(),0,0));
1227  localInvInertia.setCol(1,vmVector3(0, rb->getInvInertiaDiagLocal().getY(),0));
1228  localInvInertia.setCol(2,vmVector3(0,0, rb->getInvInertiaDiagLocal().getZ()));
1229 
1230  solverBody.mMassInv = rb->getInvMass();
1231  solverBody.mInertiaInv = ori * localInvInertia * transpose(ori);
1232  } else
1233  {
1234  state.setAngularVelocity(vmVector3(0));
1235  state.setLinearVelocity(vmVector3(0));
1236 
1237  state.setMotionType(PfxMotionTypeFixed);
1238  m_memoryCache->m_mysolverbodies[i].mMassInv = 0.f;
1239  m_memoryCache->m_mysolverbodies[i].mInertiaInv = vmMatrix3(0.0f);
1240  }
1241 
1242  }
1243  }
1244 
1245 
1246 
1247  int totalPoints = 0;
1248 #ifndef USE_C_ARRAYS
1249  m_memoryCache->m_mypairs.resize(numManifolds);
1250  //4 points per manifold and 3 rows per point makes 12 rows per manifold
1251  m_memoryCache->m_constraintRows.resize(numManifolds*12);
1252  m_memoryCache->m_jointPairs.resize(numConstraints);
1253 #endif//USE_C_ARRAYS
1254 
1255  int actualNumManifolds= 0;
1256  {
1257  BT_PROFILE("convert manifolds");
1258  for (int i1=0;i1<numManifolds;i1++)
1259  {
1260  if (manifoldPtr[i1]->getNumContacts()>0)
1261  {
1262  btPersistentManifold* m = manifoldPtr[i1];
1263  btCollisionObject* obA = (btCollisionObject*)m->getBody0();
1264  btCollisionObject* obB = (btCollisionObject*)m->getBody1();
1265  bool obAisActive = !obA->isStaticOrKinematicObject() && obA->isActive();
1266  bool obBisActive = !obB->isStaticOrKinematicObject() && obB->isActive();
1267 
1268  if (!obAisActive && !obBisActive)
1269  continue;
1270 
1271 
1272  //int contactId = i1;//actualNumManifolds;
1273 
1274  PfxBroadphasePair& pair = m_memoryCache->m_mypairs[actualNumManifolds];
1275  //init those
1276  // float compFric = obA->getFriction()*obB->getFriction();//@todo
1277  int idA = obA->getCompanionId();
1278  int idB = obB->getCompanionId();
1279 
1280  m->m_companionIdA = idA;
1281  m->m_companionIdB = idB;
1282 
1283 
1284  // if ((mysolverbodies[idA].mMassInv!=0)&&(mysolverbodies[idB].mMassInv!=0))
1285  // continue;
1286  int numPosPoints=0;
1287  for (int p=0;p<m->getNumContacts();p++)
1288  {
1289  //btManifoldPoint& pt = m->getContactPoint(p);
1290  //float dist = pt.getDistance();
1291  //if (dist<0.001)
1292  numPosPoints++;
1293  }
1294 
1295 
1296  totalPoints+=numPosPoints;
1297  pfxSetRigidBodyIdA(pair,idA);
1298  pfxSetRigidBodyIdB(pair,idB);
1299  pfxSetMotionMaskA(pair,m_memoryCache->m_mystates[idA].getMotionMask());
1300  pfxSetMotionMaskB(pair,m_memoryCache->m_mystates[idB].getMotionMask());
1301  pfxSetActive(pair,numPosPoints>0);
1302 
1303  pfxSetBroadphaseFlag(pair,0);
1304  int contactId = m-offsetContactManifolds;
1305  //likely the contact pool is not contiguous, make sure to allocate large enough contact pool
1306  btAssert(contactId>=0);
1307  btAssert(contactId<dispatcher->getInternalManifoldPool()->getMaxCount());
1308 
1309  pfxSetContactId(pair,contactId);
1310  pfxSetNumConstraints(pair,numPosPoints);//manifoldPtr[i]->getNumContacts());
1311  actualNumManifolds++;
1312  }
1313 
1314  }
1315  }
1316 
1317  PfxConstraintPair* jointPairs=0;
1318  jointPairs = numConstraints? &m_memoryCache->m_jointPairs[0]:0;
1319  int actualNumJoints=0;
1320 
1321 
1322  btSolverConstraint* offsetSolverConstraints = 0;
1323 
1324  //if (1)
1325  {
1326 
1327  {
1328  BT_PROFILE("convert constraints");
1329 
1330  int totalNumRows = 0;
1331  int i;
1332 
1333  m_tmpConstraintSizesPool.resize(numConstraints);
1334  //calculate the total number of contraint rows
1335  for (i=0;i<numConstraints;i++)
1336  {
1337  btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
1338  constraints[i]->getInfo1(&info1);
1339  totalNumRows += info1.m_numConstraintRows;
1340  }
1341  m_tmpSolverNonContactConstraintPool.resize(totalNumRows);
1342  offsetSolverConstraints =totalNumRows? &m_tmpSolverNonContactConstraintPool[0]:0;
1343 
1344 
1346  int currentRow = 0;
1347 
1348  for (i=0;i<numConstraints;i++)
1349  {
1350  const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
1351 
1352  if (info1.m_numConstraintRows)
1353  {
1354  btAssert(currentRow<totalNumRows);
1355  btTypedConstraint* constraint = constraints[i];
1356  btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];
1357 
1358  btRigidBody& rbA = constraint->getRigidBodyA();
1359  btRigidBody& rbB = constraint->getRigidBodyB();
1360 
1361  int idA = constraint->getRigidBodyA().getCompanionId();
1362  int idB = constraint->getRigidBodyB().getCompanionId();
1363 
1364 
1365  int j;
1366  for ( j=0;j<info1.m_numConstraintRows;j++)
1367  {
1368  memset(&currentConstraintRow[j],0,sizeof(btSolverConstraint));
1369  currentConstraintRow[j].m_lowerLimit = -FLT_MAX;
1370  currentConstraintRow[j].m_upperLimit = FLT_MAX;
1371  currentConstraintRow[j].m_appliedImpulse = 0.f;
1372  currentConstraintRow[j].m_appliedPushImpulse = 0.f;
1373  currentConstraintRow[j].m_solverBodyIdA = idA;
1374  currentConstraintRow[j].m_solverBodyIdB = idB;
1375  }
1376 
1377 
1378 
1379 
1380 
1381  btTypedConstraint::btConstraintInfo2 info2;
1382  info2.fps = 1.f/infoGlobal.m_timeStep;
1383  info2.erp = infoGlobal.m_erp;
1384  info2.m_J1linearAxis = currentConstraintRow->m_contactNormal;
1385  info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal;
1386  info2.m_J2linearAxis = 0;
1387  info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal;
1388  info2.rowskip = sizeof(btSolverConstraint)/sizeof(btScalar);//check this
1390  btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint));
1391  info2.m_constraintError = &currentConstraintRow->m_rhs;
1392  currentConstraintRow->m_cfm = infoGlobal.m_globalCfm;
1393  info2.cfm = &currentConstraintRow->m_cfm;
1394  info2.m_lowerLimit = &currentConstraintRow->m_lowerLimit;
1395  info2.m_upperLimit = &currentConstraintRow->m_upperLimit;
1396  info2.m_numIterations = infoGlobal.m_numIterations;
1397  constraints[i]->getInfo2(&info2);
1398 
1399 
1400 
1401 
1403  for ( j=0;j<info1.m_numConstraintRows;j++)
1404  {
1405  btSolverConstraint& solverConstraint = currentConstraintRow[j];
1406  solverConstraint.m_originalContactPoint = constraint;
1407 
1408  solverConstraint.m_solverBodyIdA = idA;
1409  solverConstraint.m_solverBodyIdB = idB;
1410 
1411  {
1412  const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
1413  solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld()*ftorqueAxis1*constraint->getRigidBodyA().getAngularFactor();
1414  }
1415  {
1416  const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal;
1417  solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld()*ftorqueAxis2*constraint->getRigidBodyB().getAngularFactor();
1418  }
1419 
1420  {
1421  btVector3 iMJlA = solverConstraint.m_contactNormal*rbA.getInvMass();
1422  btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal;
1423  btVector3 iMJlB = solverConstraint.m_contactNormal*rbB.getInvMass();//sign of normal?
1424  btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal;
1425 
1426  btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal);
1427  sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
1428  sum += iMJlB.dot(solverConstraint.m_contactNormal);
1429  sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
1430 
1431  solverConstraint.m_jacDiagABInv = btScalar(1.)/sum;
1432  }
1433 
1434 
1437  {
1438  btScalar rel_vel;
1439  btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rbA.getLinearVelocity()) + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity());
1440  btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rbB.getLinearVelocity()) + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity());
1441 
1442  rel_vel = vel1Dotn+vel2Dotn;
1443 
1444  btScalar restitution = 0.f;
1445  btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2
1446  btScalar velocityError = restitution - rel_vel;// * damping;
1447  btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
1448  btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
1449  solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
1450  solverConstraint.m_appliedImpulse = 0.f;
1451 
1452  }
1453  }
1454 
1455  PfxConstraintPair& pair = jointPairs[actualNumJoints];
1456 
1457  int numConstraintRows= info1.m_numConstraintRows;
1458  pfxSetNumConstraints(pair,numConstraintRows);
1459 
1460 
1461 
1462  pfxSetRigidBodyIdA(pair,idA);
1463  pfxSetRigidBodyIdB(pair,idB);
1464  //is this needed?
1465  if (idA>=0)
1466  pfxSetMotionMaskA(pair,m_memoryCache->m_mystates[idA].getMotionMask());
1467  if (idB>=0)
1468  pfxSetMotionMaskB(pair,m_memoryCache->m_mystates[idB].getMotionMask());
1469 
1470  pfxSetActive(pair,true);
1471  int id = currentConstraintRow-offsetSolverConstraints;
1472  pfxSetContactId(pair,id);
1473  actualNumJoints++;
1474 
1475 
1476  }
1477  currentRow+=m_tmpConstraintSizesPool[i].m_numConstraintRows;
1478  }
1479  }
1480  }
1481 
1482 
1483 
1484  float separateBias=0.1;//info.m_erp;//or m_erp2?
1485  float timeStep=infoGlobal.m_timeStep;
1486  int iteration=infoGlobal.m_numIterations;
1487 
1488  //create a pair for each constraints, copy over info etc
1489 
1490 
1491 
1492 
1493 
1494  {
1495  BT_PROFILE("compute num contacts");
1496  int totalContacts =0;
1497 
1498  for (int i=0;i<actualNumManifolds;i++)
1499  {
1500  PfxConstraintPair* pair = &m_memoryCache->m_mypairs[i];
1501  totalContacts += pfxGetNumConstraints(*pair);
1502  }
1503  //printf("numManifolds = %d\n",numManifolds);
1504  //printf("totalContacts=%d\n",totalContacts);
1505  }
1506 
1507 
1508 
1509 // printf("actualNumManifolds=%d\n",actualNumManifolds);
1510  {
1511  BT_PROFILE("BPE_customConstraintSolverSequentialNew");
1512  if (numRigidBodies>0 && (actualNumManifolds+actualNumJoints)>0)
1513  {
1514 // PFX_PRINTF("num points = %d\n",totalPoints);
1515 // PFX_PRINTF("num points PFX = %d\n",total);
1516 
1517 
1518  PfxConstraintRow* contactRows = actualNumManifolds? &m_memoryCache->m_constraintRows[0] : 0;
1519  PfxBroadphasePair* actualPairs = m_memoryCache->m_mypairs.size() ? &m_memoryCache->m_mypairs[0] : 0;
1520  BPE_customConstraintSolverSequentialNew(
1521  actualNumManifolds,
1522  actualPairs,
1523  offsetContactManifolds,
1524  contactRows,
1525  &m_memoryCache->m_mystates[0],numRigidBodies,
1526  &m_memoryCache->m_mysolverbodies[0],
1527  jointPairs,actualNumJoints,
1528  offsetSolverConstraints,
1529  separateBias,timeStep,iteration,
1530  m_solverThreadSupport,m_criticalSection,m_solverIO,m_barrier);
1531  }
1532  }
1533 
1534  //copy results back to bodies
1535  {
1536  BT_PROFILE("copy back");
1537  for (int i=0;i<numRigidBodies;i++)
1538  {
1539  btCollisionObject* obj = bodies1[i];
1540  btRigidBody* rb = btRigidBody::upcast(obj);
1541  TrbState& state = m_memoryCache->m_mystates[i];
1542  if (rb && (rb->getInvMass()>0.f))
1543  {
1544  rb->setLinearVelocity(btVector3(state.getLinearVelocity().getX(),state.getLinearVelocity().getY(),state.getLinearVelocity().getZ()));
1545  rb->setAngularVelocity(btVector3(state.getAngularVelocity().getX(),state.getAngularVelocity().getY(),state.getAngularVelocity().getZ()));
1546  }
1547  }
1548  }
1549 
1550 
1551  return 0.f;
1552 }
Generated on Thu Oct 3 2013 18:25:06 for Bullet Collision Detection & Physics Library by   doxygen 1.8.4