src/BulletMultiThreaded/btParallelConstraintSolver.cpp

   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
  44 /////////////////
  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                                 // \8e\9f\82Ì\83o\83b\83t\83@\82ð\83Z\83b\83g
 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                                 // \8e\9f\82Ì\83o\83b\83t\83@\82ð\83Z\83b\83g
 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         // \83X\83e\81[\83g\83`\83F\83b\83N\97p\83r\83b\83g\83t\83\89\83O\83e\81[\83u\83\8b
 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         // \83y\83A\83`\83F\83b\83N\97p\83r\83b\83g\83t\83\89\83O\83e\81[\83u\83\8b
 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         // \96Ú\95W\82Æ\82·\82é\95ª\8a\84\90\94
 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         // \83`\83F\83b\83N\97p\83r\83b\83g\83t\83\89\83O\83e\81[\83u\83\8b\82ð\83N\83\8a\83A
 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                                 // \97¼\95û\82Æ\82à\83A\83N\83e\83B\83u\82Å\82È\82¢\81A\82Ü\82½\82Í\8fÕ\93Ë\93_\82ª\82O\82Ì\83y\83A\82Í\93o\98^\91Î\8fÛ\82©\82ç\82Í\82¸\82·
 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                                 // \88Ë\91¶\90«\82Ì\83`\83F\83b\83N
 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                                 // \88Ë\91¶\90«\94»\92è\83e\81[\83u\83\8b\82É\93o\98^
 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                         // \8fî\95ñ\82Ì\8dX\90V
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
1345                         ///setup the btSolverConstraints
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
1389                                         ///the size of btSolverConstraint needs be a multiple of btScalar
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
1402                                         ///finalize the constraint setup
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
1435                                                 ///fix rhs
1436                                                 ///todo: add force/torque accelerators
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 }