00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 #include "btParallelConstraintSolver.h"
00019 #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
00020 #include "BulletCollision/BroadphaseCollision/btDispatcher.h"
00021 #include "LinearMath/btPoolAllocator.h"
00022 #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
00023 #include "BulletMultiThreaded/vectormath2bullet.h"
00024
00025 #include "LinearMath/btQuickprof.h"
00026 #include "BulletMultiThreaded/btThreadSupportInterface.h"
00027 #ifdef PFX_USE_FREE_VECTORMATH
00028 #include "vecmath/vmInclude.h"
00029 #else
00030 #include "vectormath/vmInclude.h"
00031 #endif //PFX_USE_FREE_VECTORMATH
00032
00033 #include "HeapManager.h"
00034
00035 #include "PlatformDefinitions.h"
00036
00037
00038 #include "LinearMath/btScalar.h"
00039
00040 #include "TrbStateVec.h"
00041
00042
00043
00045
00046
00047 #define TMP_BUFF_BYTES (15*1024*1024)
00048 unsigned char ATTRIBUTE_ALIGNED128(tmp_buff[TMP_BUFF_BYTES]);
00049
00050
00051
00052
00053 inline void resolveSingleConstraintRowGeneric(PfxSolverBody& body1,PfxSolverBody& body2,const btSolverConstraint& c)
00054 {
00055
00056 btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
00057 const btScalar deltaVel1Dotn = c.m_contactNormal.dot(getBtVector3(body1.mDeltaLinearVelocity)) + c.m_relpos1CrossNormal.dot(getBtVector3(body1.mDeltaAngularVelocity));
00058 const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(getBtVector3(body2.mDeltaLinearVelocity)) + c.m_relpos2CrossNormal.dot(getBtVector3(body2.mDeltaAngularVelocity));
00059
00060 deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
00061 deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv;
00062
00063 const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
00064 if (sum < c.m_lowerLimit)
00065 {
00066 deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
00067 c.m_appliedImpulse = c.m_lowerLimit;
00068 }
00069 else if (sum > c.m_upperLimit)
00070 {
00071 deltaImpulse = c.m_upperLimit-c.m_appliedImpulse;
00072 c.m_appliedImpulse = c.m_upperLimit;
00073 }
00074 else
00075 {
00076 c.m_appliedImpulse = sum;
00077 }
00078
00079
00080 if (body1.mMassInv)
00081 {
00082 btVector3 linearComponent = c.m_contactNormal*body1.mMassInv;
00083 body1.mDeltaLinearVelocity += vmVector3(linearComponent.getX()*deltaImpulse,linearComponent.getY()*deltaImpulse,linearComponent.getZ()*deltaImpulse);
00084 btVector3 tmp=c.m_angularComponentA*(btVector3(deltaImpulse,deltaImpulse,deltaImpulse));
00085 body1.mDeltaAngularVelocity += vmVector3(tmp.getX(),tmp.getY(),tmp.getZ());
00086 }
00087
00088 if (body2.mMassInv)
00089 {
00090 btVector3 linearComponent = -c.m_contactNormal*body2.mMassInv;
00091 body2.mDeltaLinearVelocity += vmVector3(linearComponent.getX()*deltaImpulse,linearComponent.getY()*deltaImpulse,linearComponent.getZ()*deltaImpulse);
00092 btVector3 tmp = c.m_angularComponentB*((btVector3(deltaImpulse,deltaImpulse,deltaImpulse)));
00093 body2.mDeltaAngularVelocity += vmVector3(tmp.getX(),tmp.getY(),tmp.getZ());
00094 }
00095
00096
00097
00098
00099 }
00100
00101
00102 static SIMD_FORCE_INLINE
00103 void pfxSolveLinearConstraintRow(btConstraintRow &constraint,
00104 vmVector3 &deltaLinearVelocityA,vmVector3 &deltaAngularVelocityA,
00105 float massInvA,const vmMatrix3 &inertiaInvA,const vmVector3 &rA,
00106 vmVector3 &deltaLinearVelocityB,vmVector3 &deltaAngularVelocityB,
00107 float massInvB,const vmMatrix3 &inertiaInvB,const vmVector3 &rB)
00108 {
00109 const vmVector3 normal(btReadVector3(constraint.m_normal));
00110 btScalar deltaImpulse = constraint.m_rhs;
00111 vmVector3 dVA = deltaLinearVelocityA + cross(deltaAngularVelocityA,rA);
00112 vmVector3 dVB = deltaLinearVelocityB + cross(deltaAngularVelocityB,rB);
00113 deltaImpulse -= constraint.m_jacDiagInv * dot(normal,dVA-dVB);
00114 btScalar oldImpulse = constraint.m_accumImpulse;
00115 constraint.m_accumImpulse = btClamped(oldImpulse + deltaImpulse,constraint.m_lowerLimit,constraint.m_upperLimit);
00116 deltaImpulse = constraint.m_accumImpulse - oldImpulse;
00117 deltaLinearVelocityA += deltaImpulse * massInvA * normal;
00118 deltaAngularVelocityA += deltaImpulse * inertiaInvA * cross(rA,normal);
00119 deltaLinearVelocityB -= deltaImpulse * massInvB * normal;
00120 deltaAngularVelocityB -= deltaImpulse * inertiaInvB * cross(rB,normal);
00121
00122 }
00123
00124 void btSolveContactConstraint(
00125 btConstraintRow &constraintResponse,
00126 btConstraintRow &constraintFriction1,
00127 btConstraintRow &constraintFriction2,
00128 const vmVector3 &contactPointA,
00129 const vmVector3 &contactPointB,
00130 PfxSolverBody &solverBodyA,
00131 PfxSolverBody &solverBodyB,
00132 float friction
00133 )
00134 {
00135 vmVector3 rA = rotate(solverBodyA.mOrientation,contactPointA);
00136 vmVector3 rB = rotate(solverBodyB.mOrientation,contactPointB);
00137
00138 pfxSolveLinearConstraintRow(constraintResponse,
00139 solverBodyA.mDeltaLinearVelocity,solverBodyA.mDeltaAngularVelocity,solverBodyA.mMassInv,solverBodyA.mInertiaInv,rA,
00140 solverBodyB.mDeltaLinearVelocity,solverBodyB.mDeltaAngularVelocity,solverBodyB.mMassInv,solverBodyB.mInertiaInv,rB);
00141
00142 float mf = friction*fabsf(constraintResponse.m_accumImpulse);
00143 constraintFriction1.m_lowerLimit = -mf;
00144 constraintFriction1.m_upperLimit = mf;
00145 constraintFriction2.m_lowerLimit = -mf;
00146 constraintFriction2.m_upperLimit = mf;
00147
00148 pfxSolveLinearConstraintRow(constraintFriction1,
00149 solverBodyA.mDeltaLinearVelocity,solverBodyA.mDeltaAngularVelocity,solverBodyA.mMassInv,solverBodyA.mInertiaInv,rA,
00150 solverBodyB.mDeltaLinearVelocity,solverBodyB.mDeltaAngularVelocity,solverBodyB.mMassInv,solverBodyB.mInertiaInv,rB);
00151
00152 pfxSolveLinearConstraintRow(constraintFriction2,
00153 solverBodyA.mDeltaLinearVelocity,solverBodyA.mDeltaAngularVelocity,solverBodyA.mMassInv,solverBodyA.mInertiaInv,rA,
00154 solverBodyB.mDeltaLinearVelocity,solverBodyB.mDeltaAngularVelocity,solverBodyB.mMassInv,solverBodyB.mInertiaInv,rB);
00155 }
00156
00157
00158 void CustomSolveConstraintsTaskParallel(
00159 const PfxParallelGroup *contactParallelGroup,const PfxParallelBatch *contactParallelBatches,
00160 PfxConstraintPair *contactPairs,uint32_t numContactPairs,
00161 btPersistentManifold* offsetContactManifolds,
00162 btConstraintRow* offsetContactConstraintRows,
00163 const PfxParallelGroup *jointParallelGroup,const PfxParallelBatch *jointParallelBatches,
00164 PfxConstraintPair *jointPairs,uint32_t numJointPairs,
00165 btSolverConstraint* offsetSolverConstraints,
00166 TrbState *offsetRigStates,
00167 PfxSolverBody *offsetSolverBodies,
00168 uint32_t numRigidBodies,
00169 int iteration,unsigned int taskId,unsigned int numTasks,btBarrier *barrier)
00170 {
00171
00172 PfxSolverBody staticBody;
00173 staticBody.mMassInv = 0.f;
00174 staticBody.mDeltaAngularVelocity=vmVector3(0,0,0);
00175 staticBody.mDeltaLinearVelocity =vmVector3(0,0,0);
00176
00177
00178 for(int k=0;k<iteration+1;k++) {
00179
00180 for(uint32_t phaseId=0;phaseId<jointParallelGroup->numPhases;phaseId++) {
00181 for(uint32_t batchId=0;batchId<jointParallelGroup->numBatches[phaseId];batchId++) {
00182 uint32_t numPairs = jointParallelGroup->numPairs[phaseId*PFX_MAX_SOLVER_BATCHES+batchId];
00183 if(batchId%numTasks == taskId && numPairs > 0) {
00184 const PfxParallelBatch &batch = jointParallelBatches[phaseId*PFX_MAX_SOLVER_BATCHES+batchId];
00185 for(uint32_t i=0;i<numPairs;i++) {
00186 PfxConstraintPair &pair = jointPairs[batch.pairIndices[i]];
00187 uint16_t iA = pfxGetRigidBodyIdA(pair);
00188 uint16_t iB = pfxGetRigidBodyIdB(pair);
00189
00190
00191 PfxSolverBody &solverBodyA = iA != 65535 ? offsetSolverBodies[iA] : staticBody;
00192 PfxSolverBody &solverBodyB = iB != 65535 ? offsetSolverBodies[iB] : staticBody;
00193
00194 if(k==0) {
00195
00196 }
00197 else {
00198 btSolverConstraint* constraintRow = &offsetSolverConstraints[pfxGetContactId1(pair)];
00199 int numRows = pfxGetNumConstraints(pair);
00200 int i;
00201 for (i=0;i<numRows;i++)
00202 {
00203 resolveSingleConstraintRowGeneric(solverBodyA,solverBodyB,constraintRow[i]);
00204 }
00205
00206 }
00207 }
00208 }
00209 }
00210
00211 barrier->sync();
00212 }
00213
00214
00215 for(uint32_t phaseId=0;phaseId<contactParallelGroup->numPhases;phaseId++) {
00216 for(uint32_t batchId=0;batchId<contactParallelGroup->numBatches[phaseId];batchId++) {
00217 uint32_t numPairs = contactParallelGroup->numPairs[phaseId*PFX_MAX_SOLVER_BATCHES+batchId];
00218 if(batchId%numTasks == taskId && numPairs > 0) {
00219 const PfxParallelBatch &batch = contactParallelBatches[phaseId*PFX_MAX_SOLVER_BATCHES+batchId];
00220 for(uint32_t i=0;i<numPairs;i++) {
00221 PfxConstraintPair &pair = contactPairs[batch.pairIndices[i]];
00222 uint16_t iA = pfxGetRigidBodyIdA(pair);
00223 uint16_t iB = pfxGetRigidBodyIdB(pair);
00224
00225 uint32_t contactIndex = pfxGetConstraintId1(pair);
00226 btPersistentManifold& contact = offsetContactManifolds[contactIndex];
00227 btConstraintRow* contactConstraintRows = &offsetContactConstraintRows[contactIndex*12];
00228
00229 PfxSolverBody &solverBodyA = offsetSolverBodies[iA];
00230 PfxSolverBody &solverBodyB = offsetSolverBodies[iB];
00231
00232 for(int j=0;j<contact.getNumContacts();j++) {
00233 btManifoldPoint& cp = contact.getContactPoint(j);
00234
00235 if(k==0) {
00236 vmVector3 rA = rotate(solverBodyA.mOrientation,btReadVector3(cp.m_localPointA));
00237 vmVector3 rB = rotate(solverBodyB.mOrientation,btReadVector3(cp.m_localPointB));
00238
00239 float imp[3] =
00240 {
00241 cp.m_appliedImpulse,
00242 cp.m_appliedImpulseLateral1,
00243 cp.m_appliedImpulseLateral2
00244 };
00245 for(int k=0;k<3;k++)
00246 {
00247 vmVector3 normal = btReadVector3(contactConstraintRows[j*3+k].m_normal);
00248 contactConstraintRows[j*3+k].m_accumImpulse = imp[k];
00249 float deltaImpulse = contactConstraintRows[j*3+k].m_accumImpulse;
00250 solverBodyA.mDeltaLinearVelocity += deltaImpulse * solverBodyA.mMassInv * normal;
00251 solverBodyA.mDeltaAngularVelocity += deltaImpulse * solverBodyA.mInertiaInv * cross(rA,normal);
00252 solverBodyB.mDeltaLinearVelocity -= deltaImpulse * solverBodyB.mMassInv * normal;
00253 solverBodyB.mDeltaAngularVelocity -= deltaImpulse * solverBodyB.mInertiaInv * cross(rB,normal);
00254 }
00255 }
00256 else {
00257 btSolveContactConstraint(
00258 contactConstraintRows[j*3],
00259 contactConstraintRows[j*3+1],
00260 contactConstraintRows[j*3+2],
00261 btReadVector3(cp.m_localPointA),
00262 btReadVector3(cp.m_localPointB),
00263 solverBodyA,
00264 solverBodyB,
00265 cp.m_combinedFriction
00266 );
00267 }
00268 }
00269 }
00270 }
00271 }
00272
00273 if (barrier)
00274 barrier->sync();
00275 }
00276 }
00277 }
00278
00279 void CustomPostSolverTask(
00280 TrbState *states,
00281 PfxSolverBody *solverBodies,
00282 uint32_t numRigidBodies)
00283 {
00284 for(uint32_t i=0;i<numRigidBodies;i++) {
00285 TrbState &state = states[i];
00286 PfxSolverBody &solverBody = solverBodies[i];
00287 state.setLinearVelocity(state.getLinearVelocity()+solverBody.mDeltaLinearVelocity);
00288 state.setAngularVelocity(state.getAngularVelocity()+solverBody.mDeltaAngularVelocity);
00289 }
00290 }
00291
00292 void* SolverlsMemoryFunc()
00293 {
00294
00295 return 0;
00296 }
00297
00298
00299 static SIMD_FORCE_INLINE
00300 void pfxGetPlaneSpace(const vmVector3& n, vmVector3& p, vmVector3& q)
00301 {
00302 if(fabsf(n[2]) > 0.707f) {
00303
00304 float a = n[1]*n[1] + n[2]*n[2];
00305 float k = 1.0f/sqrtf(a);
00306 p[0] = 0;
00307 p[1] = -n[2]*k;
00308 p[2] = n[1]*k;
00309
00310 q[0] = a*k;
00311 q[1] = -n[0]*p[2];
00312 q[2] = n[0]*p[1];
00313 }
00314 else {
00315
00316 float a = n[0]*n[0] + n[1]*n[1];
00317 float k = 1.0f/sqrtf(a);
00318 p[0] = -n[1]*k;
00319 p[1] = n[0]*k;
00320 p[2] = 0;
00321
00322 q[0] = -n[2]*p[1];
00323 q[1] = n[2]*p[0];
00324 q[2] = a*k;
00325 }
00326 }
00327
00328
00329
00330 #define PFX_CONTACT_SLOP 0.001f
00331
00332 void btSetupContactConstraint(
00333 btConstraintRow &constraintResponse,
00334 btConstraintRow &constraintFriction1,
00335 btConstraintRow &constraintFriction2,
00336 float penetrationDepth,
00337 float restitution,
00338 float friction,
00339 const vmVector3 &contactNormal,
00340 const vmVector3 &contactPointA,
00341 const vmVector3 &contactPointB,
00342 const TrbState &stateA,
00343 const TrbState &stateB,
00344 PfxSolverBody &solverBodyA,
00345 PfxSolverBody &solverBodyB,
00346 const vmVector3& linVelA,
00347 const vmVector3& angVelA,
00348 const vmVector3& linVelB,
00349 const vmVector3& angVelB,
00350
00351 float separateBias,
00352 float timeStep
00353 )
00354 {
00355 vmVector3 rA = rotate(solverBodyA.mOrientation,contactPointA);
00356 vmVector3 rB = rotate(solverBodyB.mOrientation,contactPointB);
00357
00358 vmMatrix3 K = vmMatrix3::scale(vmVector3(solverBodyA.mMassInv + solverBodyB.mMassInv)) -
00359 crossMatrix(rA) * solverBodyA.mInertiaInv * crossMatrix(rA) -
00360 crossMatrix(rB) * solverBodyB.mInertiaInv * crossMatrix(rB);
00361
00362
00363 vmVector3 vArestitution = linVelA + cross(angVelA,rA);
00364 vmVector3 vBrestitution = linVelB + cross(angVelB,rB);
00365 vmVector3 vABrestitution = vArestitution-vBrestitution;
00366
00367 vmVector3 vA = stateA.getLinearVelocity() + cross(stateA.getAngularVelocity(),rA);
00368 vmVector3 vB = stateB.getLinearVelocity() + cross(stateB.getAngularVelocity(),rB);
00369 vmVector3 vAB = vA-vB;
00370
00371
00372 vmVector3 tangent1,tangent2;
00373 btPlaneSpace1(contactNormal,tangent1,tangent2);
00374
00375
00376
00377
00378
00379
00380 {
00381 vmVector3 normal = contactNormal;
00382
00383 float denom = dot(K*normal,normal);
00384
00385 constraintResponse.m_rhs = -(1.0f+restitution)*dot(vAB,normal);
00386 constraintResponse.m_rhs -= (separateBias * btMin(0.0f,penetrationDepth+PFX_CONTACT_SLOP)) / timeStep;
00387 constraintResponse.m_rhs /= denom;
00388 constraintResponse.m_jacDiagInv = 1.0f/denom;
00389 constraintResponse.m_lowerLimit = 0.0f;
00390 constraintResponse.m_upperLimit = SIMD_INFINITY;
00391 btStoreVector3(normal,constraintResponse.m_normal);
00392 }
00393
00394
00395 {
00396 vmVector3 normal = tangent1;
00397
00398 float denom = dot(K*normal,normal);
00399
00400 constraintFriction1.m_jacDiagInv = 1.0f/denom;
00401 constraintFriction1.m_rhs = -dot(vAB,normal);
00402 constraintFriction1.m_rhs *= constraintFriction1.m_jacDiagInv;
00403 constraintFriction1.m_lowerLimit = 0.0f;
00404 constraintFriction1.m_upperLimit = SIMD_INFINITY;
00405 btStoreVector3(normal,constraintFriction1.m_normal);
00406 }
00407
00408
00409 {
00410 vmVector3 normal = tangent2;
00411
00412 float denom = dot(K*normal,normal);
00413
00414 constraintFriction2.m_jacDiagInv = 1.0f/denom;
00415 constraintFriction2.m_rhs = -dot(vAB,normal);
00416 constraintFriction2.m_rhs *= constraintFriction2.m_jacDiagInv;
00417 constraintFriction2.m_lowerLimit = 0.0f;
00418 constraintFriction2.m_upperLimit = SIMD_INFINITY;
00419 btStoreVector3(normal,constraintFriction2.m_normal);
00420 }
00421 }
00422
00423
00424 void CustomSetupContactConstraintsTask(
00425 PfxConstraintPair *contactPairs,uint32_t numContactPairs,
00426 btPersistentManifold* offsetContactManifolds,
00427 btConstraintRow* offsetContactConstraintRows,
00428 TrbState *offsetRigStates,
00429 PfxSolverBody *offsetSolverBodies,
00430 uint32_t numRigidBodies,
00431 float separateBias,
00432 float timeStep)
00433 {
00434 for(uint32_t i=0;i<numContactPairs;i++) {
00435 PfxConstraintPair &pair = contactPairs[i];
00436 if(!pfxGetActive(pair) || pfxGetNumConstraints(pair) == 0 ||
00437 ((pfxGetMotionMaskA(pair)&PFX_MOTION_MASK_STATIC) && (pfxGetMotionMaskB(pair)&PFX_MOTION_MASK_STATIC)) ) {
00438 continue;
00439 }
00440
00441 uint16_t iA = pfxGetRigidBodyIdA(pair);
00442 uint16_t iB = pfxGetRigidBodyIdB(pair);
00443
00444 int id = pfxGetConstraintId1(pair);
00445 btPersistentManifold& contact = offsetContactManifolds[id];
00446 btConstraintRow* contactConstraintRows = &offsetContactConstraintRows[id*12];
00447
00448 TrbState &stateA = offsetRigStates[iA];
00449
00450 PfxSolverBody &solverBodyA = offsetSolverBodies[iA];
00451
00452 TrbState &stateB = offsetRigStates[iB];
00453
00454 PfxSolverBody &solverBodyB = offsetSolverBodies[iB];
00455
00456 float restitution = 0.5f * (solverBodyA.restitution + solverBodyB.restitution);
00457
00458
00459 float friction = sqrtf(solverBodyA.friction * solverBodyB.friction);
00460
00461 for(int j=0;j<contact.getNumContacts();j++) {
00462 btManifoldPoint& cp = contact.getContactPoint(j);
00463
00464
00465 const btRigidBody* rbA = btRigidBody::upcast(contact.getBody0());
00466 const btRigidBody* rbB = btRigidBody::upcast(contact.getBody1());
00467
00468 btVector3 linVelA, linVelB;
00469 btVector3 angVelA, angVelB;
00470
00471 if (rbA && (rbA->getInvMass()>0.f))
00472 {
00473 linVelA = rbA->getLinearVelocity();
00474 angVelA = rbA->getAngularVelocity();
00475 } else
00476 {
00477 linVelA.setValue(0,0,0);
00478 angVelA.setValue(0,0,0);
00479 }
00480
00481 if (rbB && (rbB->getInvMass()>0.f))
00482 {
00483 linVelB = rbB->getLinearVelocity();
00484 angVelB = rbB->getAngularVelocity();
00485 } else
00486 {
00487 linVelB.setValue(0,0,0);
00488 angVelB.setValue(0,0,0);
00489 }
00490
00491
00492
00493 btSetupContactConstraint(
00494 contactConstraintRows[j*3],
00495 contactConstraintRows[j*3+1],
00496 contactConstraintRows[j*3+2],
00497 cp.getDistance(),
00498 restitution,
00499 friction,
00500 btReadVector3(cp.m_normalWorldOnB),
00501 btReadVector3(cp.m_localPointA),
00502 btReadVector3(cp.m_localPointB),
00503 stateA,
00504 stateB,
00505 solverBodyA,
00506 solverBodyB,
00507 (const vmVector3&)linVelA, (const vmVector3&)angVelA,
00508 (const vmVector3&)linVelB, (const vmVector3&)angVelB,
00509 separateBias,
00510 timeStep
00511 );
00512 }
00513
00514
00515 }
00516 }
00517
00518
00519 void CustomWritebackContactConstraintsTask(
00520 PfxConstraintPair *contactPairs,uint32_t numContactPairs,
00521 btPersistentManifold* offsetContactManifolds,
00522 btConstraintRow* offsetContactConstraintRows,
00523 TrbState *offsetRigStates,
00524 PfxSolverBody *offsetSolverBodies,
00525 uint32_t numRigidBodies,
00526 float separateBias,
00527 float timeStep)
00528 {
00529 for(uint32_t i=0;i<numContactPairs;i++) {
00530 PfxConstraintPair &pair = contactPairs[i];
00531 if(!pfxGetActive(pair) || pfxGetNumConstraints(pair) == 0 ||
00532 ((pfxGetMotionMaskA(pair)&PFX_MOTION_MASK_STATIC) && (pfxGetMotionMaskB(pair)&PFX_MOTION_MASK_STATIC)) ) {
00533 continue;
00534 }
00535 int id = pfxGetConstraintId1(pair);
00536 btPersistentManifold& contact = offsetContactManifolds[id];
00537 btConstraintRow* contactConstraintRows = &offsetContactConstraintRows[id*12];
00538 for(int j=0;j<contact.getNumContacts();j++) {
00539 btManifoldPoint& cp = contact.getContactPoint(j);
00540 cp.m_appliedImpulse = contactConstraintRows[j*3+0].m_accumImpulse;
00541 cp.m_appliedImpulseLateral1 = contactConstraintRows[j*3+1].m_accumImpulse;
00542 cp.m_appliedImpulseLateral2 = contactConstraintRows[j*3+2].m_accumImpulse;
00543 }
00544
00545 }
00546 }
00547
00548 void SolverThreadFunc(void* userPtr,void* lsMemory)
00549 {
00550 btConstraintSolverIO* io = (btConstraintSolverIO*)(userPtr);
00551 btCriticalSection* criticalsection = io->setupContactConstraints.criticalSection;
00552
00553
00554
00555 switch(io->cmd) {
00556
00557 case PFX_CONSTRAINT_SOLVER_CMD_SOLVE_CONSTRAINTS:
00558 CustomSolveConstraintsTaskParallel(
00559 io->solveConstraints.contactParallelGroup,
00560 io->solveConstraints.contactParallelBatches,
00561 io->solveConstraints.contactPairs,
00562 io->solveConstraints.numContactPairs,
00563 io->solveConstraints.offsetContactManifolds,
00564 io->solveConstraints.offsetContactConstraintRows,
00565
00566 io->solveConstraints.jointParallelGroup,
00567 io->solveConstraints.jointParallelBatches,
00568 io->solveConstraints.jointPairs,
00569 io->solveConstraints.numJointPairs,
00570 io->solveConstraints.offsetSolverConstraints,
00571 io->solveConstraints.offsetRigStates1,
00572 io->solveConstraints.offsetSolverBodies,
00573 io->solveConstraints.numRigidBodies,
00574 io->solveConstraints.iteration,
00575
00576 io->solveConstraints.taskId,
00577 io->maxTasks1,
00578 io->solveConstraints.barrier
00579 );
00580 break;
00581
00582 case PFX_CONSTRAINT_SOLVER_CMD_POST_SOLVER:
00583 CustomPostSolverTask( io->postSolver.states,io->postSolver.solverBodies, io->postSolver.numRigidBodies);
00584 break;
00585
00586
00587 case PFX_CONSTRAINT_SOLVER_CMD_SETUP_CONTACT_CONSTRAINTS:
00588 {
00589 bool empty = false;
00590 while(!empty) {
00591 int start,batch;
00592
00593 criticalsection->lock();
00594
00595 start = (int)criticalsection->getSharedParam(0);
00596 batch = (int)criticalsection->getSharedParam(1);
00597
00598
00599
00600
00601 int nextStart = start + batch;
00602 int rest = btMax((int)io->setupContactConstraints.numContactPairs1 - nextStart,0);
00603 int nextBatch = (rest > batch)?batch:rest;
00604
00605 criticalsection->setSharedParam(0,nextStart);
00606 criticalsection->setSharedParam(1,nextBatch);
00607
00608 criticalsection->unlock();
00609
00610 if(batch > 0) {
00611 CustomSetupContactConstraintsTask(
00612 io->setupContactConstraints.offsetContactPairs+start,batch,
00613 io->setupContactConstraints.offsetContactManifolds,
00614 io->setupContactConstraints.offsetContactConstraintRows,
00615 io->setupContactConstraints.offsetRigStates,
00616
00617 io->setupContactConstraints.offsetSolverBodies,
00618 io->setupContactConstraints.numRigidBodies,
00619 io->setupContactConstraints.separateBias,
00620 io->setupContactConstraints.timeStep);
00621 }
00622 else {
00623 empty = true;
00624 }
00625 }
00626 }
00627 break;
00628
00629 case PFX_CONSTRAINT_SOLVER_CMD_WRITEBACK_APPLIED_IMPULSES_CONTACT_CONSTRAINTS:
00630 {
00631 bool empty = false;
00632 while(!empty) {
00633 int start,batch;
00634
00635 criticalsection->lock();
00636
00637 start = (int)criticalsection->getSharedParam(0);
00638 batch = (int)criticalsection->getSharedParam(1);
00639
00640
00641
00642
00643 int nextStart = start + batch;
00644 int rest = btMax((int)io->setupContactConstraints.numContactPairs1 - nextStart,0);
00645 int nextBatch = (rest > batch)?batch:rest;
00646
00647 criticalsection->setSharedParam(0,nextStart);
00648 criticalsection->setSharedParam(1,nextBatch);
00649
00650 criticalsection->unlock();
00651
00652 if(batch > 0) {
00653 CustomWritebackContactConstraintsTask(
00654 io->setupContactConstraints.offsetContactPairs+start,batch,
00655 io->setupContactConstraints.offsetContactManifolds,
00656 io->setupContactConstraints.offsetContactConstraintRows,
00657 io->setupContactConstraints.offsetRigStates,
00658
00659 io->setupContactConstraints.offsetSolverBodies,
00660 io->setupContactConstraints.numRigidBodies,
00661 io->setupContactConstraints.separateBias,
00662 io->setupContactConstraints.timeStep);
00663 }
00664 else {
00665 empty = true;
00666 }
00667 }
00668 }
00669 break;
00670
00671 default:
00672 {
00673 btAssert(0);
00674 }
00675 }
00676
00677 }
00678
00679
00680 void CustomSetupContactConstraintsNew(
00681 PfxConstraintPair *contactPairs1,uint32_t numContactPairs,
00682 btPersistentManifold *offsetContactManifolds,
00683 btConstraintRow* offsetContactConstraintRows,
00684 TrbState *offsetRigStates,
00685 PfxSolverBody *offsetSolverBodies,
00686 uint32_t numRigidBodies,
00687 float separationBias,
00688 float timeStep,
00689 class btThreadSupportInterface* threadSupport,
00690 btCriticalSection* criticalSection,
00691 btConstraintSolverIO *io ,
00692 uint8_t cmd
00693 )
00694 {
00695 int maxTasks = threadSupport->getNumTasks();
00696
00697 int div = (int)maxTasks * 4;
00698 int batch = ((int)numContactPairs + div - 1) / div;
00699 #ifdef __PPU__
00700 BulletPE2ConstraintSolverSpursSupport* spursThread = (BulletPE2ConstraintSolverSpursSupport*) threadSupport;
00701 #endif
00702 if (criticalSection)
00703 {
00704 criticalSection->setSharedParam(0,0);
00705 criticalSection->setSharedParam(1,btMin(batch,64));
00706 } else
00707 {
00708 #ifdef __PPU__
00709 spursThread->setSharedParam(0,0);
00710 spursThread->setSharedParam(1,btMin(batch,64));
00711 #endif //__PPU__
00712 }
00713
00714 for(int t=0;t<maxTasks;t++) {
00715 io[t].cmd = cmd;
00716 io[t].setupContactConstraints.offsetContactPairs = contactPairs1;
00717 io[t].setupContactConstraints.numContactPairs1 = numContactPairs;
00718 io[t].setupContactConstraints.offsetRigStates = offsetRigStates;
00719 io[t].setupContactConstraints.offsetContactManifolds = offsetContactManifolds;
00720 io[t].setupContactConstraints.offsetContactConstraintRows = offsetContactConstraintRows;
00721 io[t].setupContactConstraints.offsetSolverBodies = offsetSolverBodies;
00722 io[t].setupContactConstraints.numRigidBodies = numRigidBodies;
00723 io[t].setupContactConstraints.separateBias = separationBias;
00724 io[t].setupContactConstraints.timeStep = timeStep;
00725 io[t].setupContactConstraints.criticalSection = criticalSection;
00726 io[t].maxTasks1 = maxTasks;
00727 #ifdef __PPU__
00728 io[t].barrierAddr2 = (unsigned int)spursThread->getBarrierAddress();
00729 io[t].criticalsectionAddr2 = (unsigned int)spursThread->getCriticalSectionAddress();
00730 #endif
00731
00732
00733
00734 #ifdef SEQUENTIAL_SETUP
00735 CustomSetupContactConstraintsTask(contactPairs1,numContactPairs,offsetContactManifolds,offsetRigStates,offsetSolverBodies,numRigidBodies,separationBias,timeStep);
00736 #else
00737 threadSupport->sendRequest(1,(ppu_address_t)&io[t],t);
00738 #endif
00739
00740 }
00741 #ifndef SEQUENTIAL_SETUP
00742 unsigned int arg0,arg1;
00743 for(int t=0;t<maxTasks;t++) {
00744 arg0 = t;
00745 threadSupport->waitForResponse(&arg0,&arg1);
00746 }
00747 #endif //SEQUENTIAL_SETUP
00748
00749 }
00750
00751
00752 void CustomSplitConstraints(
00753 PfxConstraintPair *pairs,uint32_t numPairs,
00754 PfxParallelGroup &group,PfxParallelBatch *batches,
00755 uint32_t numTasks,
00756 uint32_t numRigidBodies,
00757 void *poolBuff,
00758 uint32_t poolBytes
00759 )
00760 {
00761 HeapManager pool((unsigned char*)poolBuff,poolBytes);
00762
00763
00764 int bufSize = sizeof(uint8_t)*numRigidBodies;
00765 bufSize = ((bufSize+127)>>7)<<7;
00766 uint8_t *bodyTable = (uint8_t*)pool.allocate(bufSize,HeapManager::ALIGN128);
00767
00768
00769 uint32_t *pairTable;
00770 size_t allocSize = sizeof(uint32_t)*((numPairs+31)/32);
00771 pairTable = (uint32_t*)pool.allocate(allocSize);
00772 memset(pairTable,0,allocSize);
00773
00774
00775 uint32_t targetCount = btMax(uint32_t(PFX_MIN_SOLVER_PAIRS),btMin(numPairs / (numTasks*2),uint32_t(PFX_MAX_SOLVER_PAIRS)));
00776 uint32_t startIndex = 0;
00777
00778 uint32_t phaseId;
00779 uint32_t batchId;
00780 uint32_t totalCount=0;
00781
00782 uint32_t maxBatches = btMin(numTasks,uint32_t(PFX_MAX_SOLVER_BATCHES));
00783
00784 for(phaseId=0;phaseId<PFX_MAX_SOLVER_PHASES&&totalCount<numPairs;phaseId++) {
00785 bool startIndexCheck = true;
00786
00787 group.numBatches[phaseId] = 0;
00788
00789 uint32_t i = startIndex;
00790
00791
00792 memset(bodyTable,0xff,bufSize);
00793
00794 for(batchId=0;i<numPairs&&totalCount<numPairs&&batchId<maxBatches;batchId++) {
00795 uint32_t pairCount=0;
00796
00797 PfxParallelBatch &batch = batches[phaseId*PFX_MAX_SOLVER_BATCHES+batchId];
00798 uint32_t pairId = 0;
00799
00800 for(;i<numPairs&&pairCount<targetCount;i++) {
00801 uint32_t idxP = i>>5;
00802 uint32_t maskP = 1L << (i & 31);
00803
00804
00805 if(pairTable[idxP] & maskP) {
00806 continue;
00807 }
00808
00809 uint32_t idxA = pfxGetRigidBodyIdA(pairs[i]);
00810 uint32_t idxB = pfxGetRigidBodyIdB(pairs[i]);
00811
00812
00813 if(!pfxGetActive(pairs[i]) || pfxGetNumConstraints(pairs[i]) == 0 ||
00814 ((pfxGetMotionMaskA(pairs[i])&PFX_MOTION_MASK_STATIC) && (pfxGetMotionMaskB(pairs[i])&PFX_MOTION_MASK_STATIC)) ) {
00815 if(startIndexCheck)
00816 startIndex++;
00817
00818 pairTable[idxP] |= maskP;
00819 totalCount++;
00820 continue;
00821 }
00822
00823
00824 if( (bodyTable[idxA] != batchId && bodyTable[idxA] != 0xff) ||
00825 (bodyTable[idxB] != batchId && bodyTable[idxB] != 0xff) ) {
00826 startIndexCheck = false;
00827
00828 continue;
00829 }
00830
00831
00832 if(pfxGetMotionMaskA(pairs[i])&PFX_MOTION_MASK_DYNAMIC)
00833 bodyTable[idxA] = batchId;
00834 if(pfxGetMotionMaskB(pairs[i])&PFX_MOTION_MASK_DYNAMIC)
00835 bodyTable[idxB] = batchId;
00836
00837 if(startIndexCheck)
00838 startIndex++;
00839
00840 pairTable[idxP] |= maskP;
00841
00842 batch.pairIndices[pairId++] = i;
00843 pairCount++;
00844 }
00845
00846 group.numPairs[phaseId*PFX_MAX_SOLVER_BATCHES+batchId] = (uint16_t)pairId;
00847 totalCount += pairCount;
00848 }
00849
00850 group.numBatches[phaseId] = batchId;
00851 }
00852
00853 group.numPhases = phaseId;
00854
00855 pool.clear();
00856 }
00857
00858
00859
00860 void CustomSolveConstraintsParallel(
00861 PfxConstraintPair *contactPairs,uint32_t numContactPairs,
00862
00863 PfxConstraintPair *jointPairs,uint32_t numJointPairs,
00864 btPersistentManifold* offsetContactManifolds,
00865 btConstraintRow* offsetContactConstraintRows,
00866 btSolverConstraint* offsetSolverConstraints,
00867 TrbState *offsetRigStates,
00868 PfxSolverBody *offsetSolverBodies,
00869 uint32_t numRigidBodies,
00870 struct btConstraintSolverIO* io,
00871 class btThreadSupportInterface* threadSupport,
00872 int iteration,
00873 void* poolBuf,
00874 int poolBytes,
00875 class btBarrier* barrier)
00876 {
00877
00878 int maxTasks = threadSupport->getNumTasks();
00879
00880
00881 HeapManager pool((unsigned char*)poolBuf,poolBytes);
00882
00883 {
00884 PfxParallelGroup *cgroup = (PfxParallelGroup*)pool.allocate(sizeof(PfxParallelGroup));
00885 PfxParallelBatch *cbatches = (PfxParallelBatch*)pool.allocate(sizeof(PfxParallelBatch)*(PFX_MAX_SOLVER_PHASES*PFX_MAX_SOLVER_BATCHES),128);
00886 PfxParallelGroup *jgroup = (PfxParallelGroup*)pool.allocate(sizeof(PfxParallelGroup));
00887 PfxParallelBatch *jbatches = (PfxParallelBatch*)pool.allocate(sizeof(PfxParallelBatch)*(PFX_MAX_SOLVER_PHASES*PFX_MAX_SOLVER_BATCHES),128);
00888
00889 uint32_t tmpBytes = poolBytes - 2 * (sizeof(PfxParallelGroup) + sizeof(PfxParallelBatch)*(PFX_MAX_SOLVER_PHASES*PFX_MAX_SOLVER_BATCHES) + 128);
00890 void *tmpBuff = pool.allocate(tmpBytes);
00891
00892 {
00893 BT_PROFILE("CustomSplitConstraints");
00894 CustomSplitConstraints(contactPairs,numContactPairs,*cgroup,cbatches,maxTasks,numRigidBodies,tmpBuff,tmpBytes);
00895 CustomSplitConstraints(jointPairs,numJointPairs,*jgroup,jbatches,maxTasks,numRigidBodies,tmpBuff,tmpBytes);
00896 }
00897
00898 {
00899 BT_PROFILE("PFX_CONSTRAINT_SOLVER_CMD_SOLVE_CONSTRAINTS");
00900
00901 #ifdef SOLVE_SEQUENTIAL
00902 CustomSolveConstraintsTask(
00903 io->solveConstraints.contactParallelGroup,
00904 io->solveConstraints.contactParallelBatches,
00905 io->solveConstraints.contactPairs,
00906 io->solveConstraints.numContactPairs,
00907 io->solveConstraints.offsetContactManifolds,
00908
00909 io->solveConstraints.jointParallelGroup,
00910 io->solveConstraints.jointParallelBatches,
00911 io->solveConstraints.jointPairs,
00912 io->solveConstraints.numJointPairs,
00913 io->solveConstraints.offsetSolverConstraints,
00914
00915 io->solveConstraints.offsetRigStates1,
00916 io->solveConstraints.offsetSolverBodies,
00917 io->solveConstraints.numRigidBodies,
00918 io->solveConstraints.iteration,0,1,0);
00919 #else
00920 for(int t=0;t<maxTasks;t++) {
00921 io[t].cmd = PFX_CONSTRAINT_SOLVER_CMD_SOLVE_CONSTRAINTS;
00922 io[t].solveConstraints.contactParallelGroup = cgroup;
00923 io[t].solveConstraints.contactParallelBatches = cbatches;
00924 io[t].solveConstraints.contactPairs = contactPairs;
00925 io[t].solveConstraints.numContactPairs = numContactPairs;
00926 io[t].solveConstraints.offsetContactManifolds = offsetContactManifolds;
00927 io[t].solveConstraints.offsetContactConstraintRows = offsetContactConstraintRows;
00928 io[t].solveConstraints.jointParallelGroup = jgroup;
00929 io[t].solveConstraints.jointParallelBatches = jbatches;
00930 io[t].solveConstraints.jointPairs = jointPairs;
00931 io[t].solveConstraints.numJointPairs = numJointPairs;
00932 io[t].solveConstraints.offsetSolverConstraints = offsetSolverConstraints;
00933 io[t].solveConstraints.offsetRigStates1 = offsetRigStates;
00934 io[t].solveConstraints.offsetSolverBodies = offsetSolverBodies;
00935 io[t].solveConstraints.numRigidBodies = numRigidBodies;
00936 io[t].solveConstraints.iteration = iteration;
00937 io[t].solveConstraints.taskId = t;
00938 io[t].solveConstraints.barrier = barrier;
00939
00940 io[t].maxTasks1 = maxTasks;
00941 #ifdef __PPU__
00942 BulletPE2ConstraintSolverSpursSupport* spursThread = (BulletPE2ConstraintSolverSpursSupport*) threadSupport;
00943 io[t].barrierAddr2 = (unsigned int) spursThread->getBarrierAddress();
00944 io[t].criticalsectionAddr2 = (unsigned int)spursThread->getCriticalSectionAddress();
00945 #endif
00946
00947 threadSupport->sendRequest(1,(ppu_address_t)&io[t],t);
00948 }
00949
00950 unsigned int arg0,arg1;
00951 for(int t=0;t<maxTasks;t++) {
00952 arg0 = t;
00953 threadSupport->waitForResponse(&arg0,&arg1);
00954 }
00955 #endif
00956 }
00957 pool.clear();
00958 }
00959
00960 {
00961 BT_PROFILE("PFX_CONSTRAINT_SOLVER_CMD_POST_SOLVER");
00962 int batch = ((int)numRigidBodies + maxTasks - 1) / maxTasks;
00963 int rest = (int)numRigidBodies;
00964 int start = 0;
00965
00966 for(int t=0;t<maxTasks;t++) {
00967 int num = (rest - batch ) > 0 ? batch : rest;
00968 io[t].cmd = PFX_CONSTRAINT_SOLVER_CMD_POST_SOLVER;
00969 io[t].postSolver.states = offsetRigStates + start;
00970 io[t].postSolver.solverBodies = offsetSolverBodies + start;
00971 io[t].postSolver.numRigidBodies = (uint32_t)num;
00972 io[t].maxTasks1 = maxTasks;
00973 #ifdef __PPU__
00974 BulletPE2ConstraintSolverSpursSupport* spursThread = (BulletPE2ConstraintSolverSpursSupport*) threadSupport;
00975 io[t].barrierAddr2 = (unsigned int)spursThread->getBarrierAddress();
00976 io[t].criticalsectionAddr2 = (unsigned int)spursThread->getCriticalSectionAddress();
00977 #endif
00978
00979 #ifdef SOLVE_SEQUENTIAL
00980 CustomPostSolverTask( io[t].postSolver.states,io[t].postSolver.solverBodies, io[t].postSolver.numRigidBodies);
00981 #else
00982 threadSupport->sendRequest(1,(ppu_address_t)&io[t],t);
00983 #endif
00984 rest -= num;
00985 start += num;
00986 }
00987
00988 unsigned int arg0,arg1;
00989 for(int t=0;t<maxTasks;t++) {
00990 #ifndef SOLVE_SEQUENTIAL
00991 arg0 = t;
00992 threadSupport->waitForResponse(&arg0,&arg1);
00993 #endif
00994 }
00995 }
00996
00997 }
00998
00999
01000
01001 void BPE_customConstraintSolverSequentialNew(unsigned int new_num, PfxBroadphasePair *new_pairs1 ,
01002 btPersistentManifold* offsetContactManifolds,
01003 PfxConstraintRow* offsetContactConstraintRows,
01004 TrbState* states,int numRigidBodies,
01005 struct PfxSolverBody* solverBodies,
01006 PfxConstraintPair* jointPairs, unsigned int numJoints,
01007 btSolverConstraint* offsetSolverConstraints,
01008 float separateBias,
01009 float timeStep,
01010 int iteration,
01011 btThreadSupportInterface* solverThreadSupport,
01012 btCriticalSection* criticalSection,
01013 struct btConstraintSolverIO* solverIO,
01014 btBarrier* barrier
01015 )
01016 {
01017
01018 {
01019 BT_PROFILE("pfxSetupConstraints");
01020
01021 for(uint32_t i=0;i<numJoints;i++) {
01022
01023 PfxConstraintPair &pair = jointPairs[i];
01024 int idA = pfxGetRigidBodyIdA(pair);
01025
01026 if (idA != 65535)
01027 {
01028 pfxSetMotionMaskA(pair,states[pfxGetRigidBodyIdA(pair)].getMotionMask());
01029 }
01030 else
01031 {
01032 pfxSetMotionMaskA(pair,PFX_MOTION_MASK_STATIC);
01033 }
01034 int idB = pfxGetRigidBodyIdB(pair);
01035 if (idB!= 65535)
01036 {
01037 pfxSetMotionMaskB(pair,states[pfxGetRigidBodyIdB(pair)].getMotionMask());
01038 } else
01039 {
01040 pfxSetMotionMaskB(pair,PFX_MOTION_MASK_STATIC);
01041 }
01042 }
01043
01044
01045
01046 #ifdef SEQUENTIAL_SETUP
01047 CustomSetupContactConstraintsSeqNew(
01048 (PfxConstraintPair*)new_pairs1,new_num,contacts,
01049 states,
01050 solverBodies,
01051 numRigidBodies,
01052 separateBias,
01053 timeStep);
01054 #else
01055
01056 CustomSetupContactConstraintsNew(
01057 (PfxConstraintPair*)new_pairs1,new_num,
01058 offsetContactManifolds,
01059 offsetContactConstraintRows,
01060 states,
01061 solverBodies,
01062 numRigidBodies,
01063 separateBias,
01064 timeStep,
01065 solverThreadSupport,
01066 criticalSection,solverIO,
01067 PFX_CONSTRAINT_SOLVER_CMD_SETUP_CONTACT_CONSTRAINTS
01068 );
01069
01070 #endif //SEQUENTIAL_SETUP
01071
01072 }
01073 {
01074 BT_PROFILE("pfxSolveConstraints");
01075
01076
01077 #ifdef SEQUENTIAL
01078 CustomSolveConstraintsSeq(
01079 (PfxConstraintPair*)new_pairs1,new_num,contacts,
01080 jointPairs,numJoints,
01081 states,
01082 solverBodies,
01083 numRigidBodies,
01084 separateBias,
01085 timeStep,
01086 iteration);
01087 #else //SEQUENTIAL
01088 CustomSolveConstraintsParallel(
01089 (PfxConstraintPair*)new_pairs1,new_num,
01090 jointPairs,numJoints,
01091 offsetContactManifolds,
01092 offsetContactConstraintRows,
01093 offsetSolverConstraints,
01094 states,
01095 solverBodies,
01096 numRigidBodies,
01097 solverIO, solverThreadSupport,
01098 iteration,
01099 tmp_buff,
01100 TMP_BUFF_BYTES,
01101 barrier
01102 );
01103
01104 #endif //SEQUENTIAL
01105 }
01106
01107 {
01108 BT_PROFILE("writeback appliedImpulses");
01109
01110 CustomSetupContactConstraintsNew(
01111 (PfxConstraintPair*)new_pairs1,new_num,
01112 offsetContactManifolds,
01113 offsetContactConstraintRows,
01114 states,
01115 solverBodies,
01116 numRigidBodies,
01117 separateBias,
01118 timeStep,
01119 solverThreadSupport,
01120 criticalSection,solverIO,
01121 PFX_CONSTRAINT_SOLVER_CMD_WRITEBACK_APPLIED_IMPULSES_CONTACT_CONSTRAINTS
01122 );
01123 }
01124
01125 }
01126
01127
01128 struct btParallelSolverMemoryCache
01129 {
01130 btAlignedObjectArray<TrbState> m_mystates;
01131 btAlignedObjectArray<PfxSolverBody> m_mysolverbodies;
01132 btAlignedObjectArray<PfxBroadphasePair> m_mypairs;
01133 btAlignedObjectArray<PfxConstraintPair> m_jointPairs;
01134 btAlignedObjectArray<PfxConstraintRow> m_constraintRows;
01135
01136 };
01137
01138
01139 btConstraintSolverIO* createSolverIO(int numThreads)
01140 {
01141 return new btConstraintSolverIO[numThreads];
01142 }
01143
01144 btParallelConstraintSolver::btParallelConstraintSolver(btThreadSupportInterface* solverThreadSupport)
01145 {
01146
01147 m_solverThreadSupport = solverThreadSupport;
01148 m_solverIO = createSolverIO(m_solverThreadSupport->getNumTasks());
01149
01150 m_barrier = m_solverThreadSupport->createBarrier();
01151 m_criticalSection = m_solverThreadSupport->createCriticalSection();
01152
01153 m_memoryCache = new btParallelSolverMemoryCache();
01154 }
01155
01156 btParallelConstraintSolver::~btParallelConstraintSolver()
01157 {
01158 delete m_memoryCache;
01159 delete m_solverIO;
01160 m_solverThreadSupport->deleteBarrier(m_barrier);
01161 m_solverThreadSupport->deleteCriticalSection(m_criticalSection);
01162 }
01163
01164
01165
01166 btScalar btParallelConstraintSolver::solveGroup(btCollisionObject** bodies1,int numRigidBodies,btPersistentManifold** manifoldPtr,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher)
01167 {
01168
01169
01170
01171
01172
01173
01174
01175
01176
01177
01178 btPersistentManifold* offsetContactManifolds= (btPersistentManifold*) dispatcher->getInternalManifoldPool()->getPoolAddress();
01179
01180
01181 m_memoryCache->m_mysolverbodies.resize(numRigidBodies);
01182 m_memoryCache->m_mystates.resize(numRigidBodies);
01183
01184 {
01185 BT_PROFILE("create states and solver bodies");
01186 for (int i=0;i<numRigidBodies;i++)
01187 {
01188 btCollisionObject* obj = bodies1[i];
01189 obj->setCompanionId(i);
01190
01191 PfxSolverBody& solverBody = m_memoryCache->m_mysolverbodies[i];
01192 btRigidBody* rb = btRigidBody::upcast(obj);
01193 TrbState& state = m_memoryCache->m_mystates[i];
01194
01195 state.reset();
01196 const btQuaternion& orgOri = obj->getWorldTransform().getRotation();
01197 vmQuat orn(orgOri.getX(),orgOri.getY(),orgOri.getZ(),orgOri.getW());
01198 state.setPosition(getVmVector3(obj->getWorldTransform().getOrigin()));
01199 state.setOrientation(orn);
01200 state.setPosition(state.getPosition());
01201 state.setRigidBodyId(i);
01202 state.setAngularDamping(0);
01203 state.setLinearDamping(0);
01204
01205
01206 solverBody.mOrientation = state.getOrientation();
01207 solverBody.mDeltaLinearVelocity = vmVector3(0.0f);
01208 solverBody.mDeltaAngularVelocity = vmVector3(0.0f);
01209 solverBody.friction = obj->getFriction();
01210 solverBody.restitution = obj->getRestitution();
01211
01212 state.resetSleepCount();
01213
01214
01215 if (rb && (rb->getInvMass()>0.f))
01216 {
01217 btVector3 angVelPlusForces = rb->getAngularVelocity()+rb->getTotalTorque()*rb->getInvInertiaTensorWorld()*infoGlobal.m_timeStep;
01218 btVector3 linVelPlusForces = rb->getLinearVelocity()+rb->getTotalForce()*rb->getInvMass()*infoGlobal.m_timeStep;
01219
01220 state.setAngularVelocity((const vmVector3&)angVelPlusForces);
01221 state.setLinearVelocity((const vmVector3&) linVelPlusForces);
01222
01223 state.setMotionType(PfxMotionTypeActive);
01224 vmMatrix3 ori(solverBody.mOrientation);
01225 vmMatrix3 localInvInertia = vmMatrix3::identity();
01226 localInvInertia.setCol(0,vmVector3(rb->getInvInertiaDiagLocal().getX(),0,0));
01227 localInvInertia.setCol(1,vmVector3(0, rb->getInvInertiaDiagLocal().getY(),0));
01228 localInvInertia.setCol(2,vmVector3(0,0, rb->getInvInertiaDiagLocal().getZ()));
01229
01230 solverBody.mMassInv = rb->getInvMass();
01231 solverBody.mInertiaInv = ori * localInvInertia * transpose(ori);
01232 } else
01233 {
01234 state.setAngularVelocity(vmVector3(0));
01235 state.setLinearVelocity(vmVector3(0));
01236
01237 state.setMotionType(PfxMotionTypeFixed);
01238 m_memoryCache->m_mysolverbodies[i].mMassInv = 0.f;
01239 m_memoryCache->m_mysolverbodies[i].mInertiaInv = vmMatrix3(0.0f);
01240 }
01241
01242 }
01243 }
01244
01245
01246
01247 int totalPoints = 0;
01248 #ifndef USE_C_ARRAYS
01249 m_memoryCache->m_mypairs.resize(numManifolds);
01250
01251 m_memoryCache->m_constraintRows.resize(numManifolds*12);
01252 m_memoryCache->m_jointPairs.resize(numConstraints);
01253 #endif//USE_C_ARRAYS
01254
01255 int actualNumManifolds= 0;
01256 {
01257 BT_PROFILE("convert manifolds");
01258 for (int i1=0;i1<numManifolds;i1++)
01259 {
01260 if (manifoldPtr[i1]->getNumContacts()>0)
01261 {
01262 btPersistentManifold* m = manifoldPtr[i1];
01263 btCollisionObject* obA = (btCollisionObject*)m->getBody0();
01264 btCollisionObject* obB = (btCollisionObject*)m->getBody1();
01265 bool obAisActive = !obA->isStaticOrKinematicObject() && obA->isActive();
01266 bool obBisActive = !obB->isStaticOrKinematicObject() && obB->isActive();
01267
01268 if (!obAisActive && !obBisActive)
01269 continue;
01270
01271
01272
01273
01274 PfxBroadphasePair& pair = m_memoryCache->m_mypairs[actualNumManifolds];
01275
01276
01277 int idA = obA->getCompanionId();
01278 int idB = obB->getCompanionId();
01279
01280 m->m_companionIdA = idA;
01281 m->m_companionIdB = idB;
01282
01283
01284
01285
01286 int numPosPoints=0;
01287 for (int p=0;p<m->getNumContacts();p++)
01288 {
01289
01290
01291
01292 numPosPoints++;
01293 }
01294
01295
01296 totalPoints+=numPosPoints;
01297 pfxSetRigidBodyIdA(pair,idA);
01298 pfxSetRigidBodyIdB(pair,idB);
01299 pfxSetMotionMaskA(pair,m_memoryCache->m_mystates[idA].getMotionMask());
01300 pfxSetMotionMaskB(pair,m_memoryCache->m_mystates[idB].getMotionMask());
01301 pfxSetActive(pair,numPosPoints>0);
01302
01303 pfxSetBroadphaseFlag(pair,0);
01304 int contactId = m-offsetContactManifolds;
01305
01306 btAssert(contactId>=0);
01307 btAssert(contactId<dispatcher->getInternalManifoldPool()->getMaxCount());
01308
01309 pfxSetContactId(pair,contactId);
01310 pfxSetNumConstraints(pair,numPosPoints);
01311 actualNumManifolds++;
01312 }
01313
01314 }
01315 }
01316
01317 PfxConstraintPair* jointPairs=0;
01318 jointPairs = numConstraints? &m_memoryCache->m_jointPairs[0]:0;
01319 int actualNumJoints=0;
01320
01321
01322 btSolverConstraint* offsetSolverConstraints = 0;
01323
01324
01325 {
01326
01327 {
01328 BT_PROFILE("convert constraints");
01329
01330 int totalNumRows = 0;
01331 int i;
01332
01333 m_tmpConstraintSizesPool.resize(numConstraints);
01334
01335 for (i=0;i<numConstraints;i++)
01336 {
01337 btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
01338 constraints[i]->getInfo1(&info1);
01339 totalNumRows += info1.m_numConstraintRows;
01340 }
01341 m_tmpSolverNonContactConstraintPool.resize(totalNumRows);
01342 offsetSolverConstraints =totalNumRows? &m_tmpSolverNonContactConstraintPool[0]:0;
01343
01344
01346 int currentRow = 0;
01347
01348 for (i=0;i<numConstraints;i++)
01349 {
01350 const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
01351
01352 if (info1.m_numConstraintRows)
01353 {
01354 btAssert(currentRow<totalNumRows);
01355 btTypedConstraint* constraint = constraints[i];
01356 btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];
01357
01358 btRigidBody& rbA = constraint->getRigidBodyA();
01359 btRigidBody& rbB = constraint->getRigidBodyB();
01360
01361 int idA = constraint->getRigidBodyA().getCompanionId();
01362 int idB = constraint->getRigidBodyB().getCompanionId();
01363
01364
01365 int j;
01366 for ( j=0;j<info1.m_numConstraintRows;j++)
01367 {
01368 memset(¤tConstraintRow[j],0,sizeof(btSolverConstraint));
01369 currentConstraintRow[j].m_lowerLimit = -FLT_MAX;
01370 currentConstraintRow[j].m_upperLimit = FLT_MAX;
01371 currentConstraintRow[j].m_appliedImpulse = 0.f;
01372 currentConstraintRow[j].m_appliedPushImpulse = 0.f;
01373 currentConstraintRow[j].m_solverBodyIdA = idA;
01374 currentConstraintRow[j].m_solverBodyIdB = idB;
01375 }
01376
01377
01378
01379
01380
01381 btTypedConstraint::btConstraintInfo2 info2;
01382 info2.fps = 1.f/infoGlobal.m_timeStep;
01383 info2.erp = infoGlobal.m_erp;
01384 info2.m_J1linearAxis = currentConstraintRow->m_contactNormal;
01385 info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal;
01386 info2.m_J2linearAxis = 0;
01387 info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal;
01388 info2.rowskip = sizeof(btSolverConstraint)/sizeof(btScalar);
01390 btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint));
01391 info2.m_constraintError = ¤tConstraintRow->m_rhs;
01392 currentConstraintRow->m_cfm = infoGlobal.m_globalCfm;
01393 info2.cfm = ¤tConstraintRow->m_cfm;
01394 info2.m_lowerLimit = ¤tConstraintRow->m_lowerLimit;
01395 info2.m_upperLimit = ¤tConstraintRow->m_upperLimit;
01396 info2.m_numIterations = infoGlobal.m_numIterations;
01397 constraints[i]->getInfo2(&info2);
01398
01399
01400
01401
01403 for ( j=0;j<info1.m_numConstraintRows;j++)
01404 {
01405 btSolverConstraint& solverConstraint = currentConstraintRow[j];
01406 solverConstraint.m_originalContactPoint = constraint;
01407
01408 solverConstraint.m_solverBodyIdA = idA;
01409 solverConstraint.m_solverBodyIdB = idB;
01410
01411 {
01412 const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
01413 solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld()*ftorqueAxis1*constraint->getRigidBodyA().getAngularFactor();
01414 }
01415 {
01416 const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal;
01417 solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld()*ftorqueAxis2*constraint->getRigidBodyB().getAngularFactor();
01418 }
01419
01420 {
01421 btVector3 iMJlA = solverConstraint.m_contactNormal*rbA.getInvMass();
01422 btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal;
01423 btVector3 iMJlB = solverConstraint.m_contactNormal*rbB.getInvMass();
01424 btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal;
01425
01426 btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal);
01427 sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
01428 sum += iMJlB.dot(solverConstraint.m_contactNormal);
01429 sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
01430
01431 solverConstraint.m_jacDiagABInv = btScalar(1.)/sum;
01432 }
01433
01434
01437 {
01438 btScalar rel_vel;
01439 btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rbA.getLinearVelocity()) + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity());
01440 btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rbB.getLinearVelocity()) + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity());
01441
01442 rel_vel = vel1Dotn+vel2Dotn;
01443
01444 btScalar restitution = 0.f;
01445 btScalar positionalError = solverConstraint.m_rhs;
01446 btScalar velocityError = restitution - rel_vel;
01447 btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
01448 btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
01449 solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
01450 solverConstraint.m_appliedImpulse = 0.f;
01451
01452 }
01453 }
01454
01455 PfxConstraintPair& pair = jointPairs[actualNumJoints];
01456
01457 int numConstraintRows= info1.m_numConstraintRows;
01458 pfxSetNumConstraints(pair,numConstraintRows);
01459
01460
01461
01462 pfxSetRigidBodyIdA(pair,idA);
01463 pfxSetRigidBodyIdB(pair,idB);
01464
01465 if (idA>=0)
01466 pfxSetMotionMaskA(pair,m_memoryCache->m_mystates[idA].getMotionMask());
01467 if (idB>=0)
01468 pfxSetMotionMaskB(pair,m_memoryCache->m_mystates[idB].getMotionMask());
01469
01470 pfxSetActive(pair,true);
01471 int id = currentConstraintRow-offsetSolverConstraints;
01472 pfxSetContactId(pair,id);
01473 actualNumJoints++;
01474
01475
01476 }
01477 currentRow+=m_tmpConstraintSizesPool[i].m_numConstraintRows;
01478 }
01479 }
01480 }
01481
01482
01483
01484 float separateBias=0.1;
01485 float timeStep=infoGlobal.m_timeStep;
01486 int iteration=infoGlobal.m_numIterations;
01487
01488
01489
01490
01491
01492
01493
01494 {
01495 BT_PROFILE("compute num contacts");
01496 int totalContacts =0;
01497
01498 for (int i=0;i<actualNumManifolds;i++)
01499 {
01500 PfxConstraintPair* pair = &m_memoryCache->m_mypairs[i];
01501 totalContacts += pfxGetNumConstraints(*pair);
01502 }
01503
01504
01505 }
01506
01507
01508
01509
01510 {
01511 BT_PROFILE("BPE_customConstraintSolverSequentialNew");
01512 if (numRigidBodies>0 && (actualNumManifolds+actualNumJoints)>0)
01513 {
01514
01515
01516
01517
01518 PfxConstraintRow* contactRows = actualNumManifolds? &m_memoryCache->m_constraintRows[0] : 0;
01519 PfxBroadphasePair* actualPairs = m_memoryCache->m_mypairs.size() ? &m_memoryCache->m_mypairs[0] : 0;
01520 BPE_customConstraintSolverSequentialNew(
01521 actualNumManifolds,
01522 actualPairs,
01523 offsetContactManifolds,
01524 contactRows,
01525 &m_memoryCache->m_mystates[0],numRigidBodies,
01526 &m_memoryCache->m_mysolverbodies[0],
01527 jointPairs,actualNumJoints,
01528 offsetSolverConstraints,
01529 separateBias,timeStep,iteration,
01530 m_solverThreadSupport,m_criticalSection,m_solverIO,m_barrier);
01531 }
01532 }
01533
01534
01535 {
01536 BT_PROFILE("copy back");
01537 for (int i=0;i<numRigidBodies;i++)
01538 {
01539 btCollisionObject* obj = bodies1[i];
01540 btRigidBody* rb = btRigidBody::upcast(obj);
01541 TrbState& state = m_memoryCache->m_mystates[i];
01542 if (rb && (rb->getInvMass()>0.f))
01543 {
01544 rb->setLinearVelocity(btVector3(state.getLinearVelocity().getX(),state.getLinearVelocity().getY(),state.getLinearVelocity().getZ()));
01545 rb->setAngularVelocity(btVector3(state.getAngularVelocity().getX(),state.getAngularVelocity().getY(),state.getAngularVelocity().getZ()));
01546 }
01547 }
01548 }
01549
01550
01551 return 0.f;
01552 }