Extras/PhysicsEffects/include/BulletPhysicsEffects/btLowLevelConstraintSolver.cpp

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