btParallelConstraintSolver.cpp

Go to the documentation of this file.
00001 /*
00002    Copyright (C) 2010 Sony Computer Entertainment Inc.
00003    All rights reserved.
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose, 
00008 including commercial applications, and to alter it and redistribute it freely, 
00009 subject to the following restrictions:
00010 
00011 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.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
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 //#include "PfxSimdUtils.h"
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 // Project Gauss Seidel or the equivalent Sequential Impulse
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 //      const btScalar delta_rel_vel    =       deltaVel1Dotn-deltaVel2Dotn;
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)));//*m_angularFactor);
00093                 body2.mDeltaAngularVelocity += vmVector3(tmp.getX(),tmp.getY(),tmp.getZ());
00094         }
00095 
00096         //body1.internalApplyImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
00097         //body2.internalApplyImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
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                 // Joint
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                 // Contact
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         //don't create local store memory, just return 0
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                 // choose p in y-z plane
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                 // set q = n x p
00310                 q[0] = a*k;
00311                 q[1] = -n[0]*p[2];
00312                 q[2] = n[0]*p[1];
00313         }
00314         else {
00315                 // choose p in x-y plane
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                 // set q = n x p
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         //use the velocities without the applied (gravity and external) forces for restitution computation
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 //      constraintResponse.m_accumImpulse = 0.f;
00376 //      constraintFriction1.m_accumImpulse = 0.f;
00377 //      constraintFriction2.m_accumImpulse = 0.f;
00378 
00379         // Contact Constraint
00380         {
00381                 vmVector3 normal = contactNormal;
00382 
00383                 float denom = dot(K*normal,normal);
00384 
00385                 constraintResponse.m_rhs = -(1.0f+restitution)*dot(vAB,normal); // velocity error
00386                 constraintResponse.m_rhs -= (separateBias * btMin(0.0f,penetrationDepth+PFX_CONTACT_SLOP)) / timeStep; // position error
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         // Friction Constraint 1
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         // Friction Constraint 2
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 //              PfxRigBody &bodyA = offsetRigBodies[iA];
00450                 PfxSolverBody &solverBodyA = offsetSolverBodies[iA];
00451 
00452                 TrbState &stateB = offsetRigStates[iB];
00453 //              PfxRigBody &bodyB = offsetRigBodies[iB];
00454                 PfxSolverBody &solverBodyB = offsetSolverBodies[iB];
00455                 
00456                 float restitution = 0.5f * (solverBodyA.restitution + solverBodyB.restitution);
00457                 //if(contact.getDuration() > 1) restitution = 0.0f;
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                         //pass the velocities without the applied (gravity and external) forces for restitution computation
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),//.mConstraintRow[0].m_normal),
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                 //contact.setCompositeFriction(friction);
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                 //contact.setCompositeFriction(friction);
00545         }
00546 }
00547 
00548 void    SolverThreadFunc(void* userPtr,void* lsMemory)
00549 {
00550         btConstraintSolverIO* io = (btConstraintSolverIO*)(userPtr);//arg->io);
00551         btCriticalSection* criticalsection = io->setupContactConstraints.criticalSection;
00552         
00553 
00554         //CustomCriticalSection *criticalsection = &io->m_cs;
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                                 //PFX_PRINTF("taskId %d start %d num %d\n",arg->taskId,start,batch);
00599 
00600                                 // ŽŸ‚̃oƒbƒtƒ@‚ðƒZƒbƒg
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 //                                              io->setupContactConstraints.offsetRigBodies,
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                                 //PFX_PRINTF("taskId %d start %d num %d\n",arg->taskId,start,batch);
00641 
00642                                 // ŽŸ‚̃oƒbƒtƒ@‚ðƒZƒbƒg
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 //                                              io->setupContactConstraints.offsetRigBodies,
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)); // batched number
00706         } else
00707         {
00708 #ifdef __PPU__
00709                 spursThread->setSharedParam(0,0);
00710                 spursThread->setSharedParam(1,btMin(batch,64)); // batched number
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 //#define SEQUENTIAL_SETUP
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         // ƒXƒe[ƒgƒ`ƒFƒbƒN—pƒrƒbƒgƒtƒ‰ƒOƒe[ƒuƒ‹
00764         int bufSize = sizeof(uint8_t)*numRigidBodies;
00765         bufSize = ((bufSize+127)>>7)<<7; // 128 bytes alignment
00766         uint8_t *bodyTable = (uint8_t*)pool.allocate(bufSize,HeapManager::ALIGN128);
00767 
00768         // ƒyƒAƒ`ƒFƒbƒN—pƒrƒbƒgƒtƒ‰ƒOƒe[ƒuƒ‹
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         // –Ú•W‚Æ‚·‚é•ªŠ„”
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         // ƒ`ƒFƒbƒN—pƒrƒbƒgƒtƒ‰ƒOƒe[ƒuƒ‹‚ðƒNƒŠƒA
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                                 //pair is already assigned to a phase/batch
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                                 // —¼•û‚Æ‚àƒAƒNƒeƒBƒu‚łȂ¢A‚Ü‚½‚ÍÕ“Ë“_‚ª‚O‚̃yƒA‚Í“o˜^‘ÎÛ‚©‚ç‚Í‚¸‚·
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                                         //assign pair -> skip it because it has no constraints
00818                                         pairTable[idxP] |= maskP;
00819                                         totalCount++;
00820                                         continue;
00821                                 }
00822                                 
00823                                 // ˆË‘¶«‚̃`ƒFƒbƒN
00824                                 if( (bodyTable[idxA] != batchId && bodyTable[idxA] != 0xff) || 
00825                                         (bodyTable[idxB] != batchId && bodyTable[idxB] != 0xff) ) {
00826                                         startIndexCheck = false;
00827                                         //bodies of the pair are already assigned to another batch within this phase
00828                                         continue;
00829                                 }
00830                                 
00831                                 // ˆË‘¶«”»’èƒe[ƒuƒ‹‚É“o˜^
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                                 //add the pair 'i' to the current batch
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 //      config.taskManager->setTaskEntry(PFX_SOLVER_ENTRY);
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 //#define SOLVE_SEQUENTIAL
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);//arg->taskId,1,0);//,arg->maxTasks,arg->barrier);
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                         // î•ñ‚ÌXV
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 //              CustomSetupJointConstraintsSeq(                 jointPairs,numJoints,joints,                    states,                 solverBodies,                   numRigidBodies,                 timeStep);
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 //#define SEQUENTIAL
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;//createSolverThreadSupport(maxNumThreads);
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 /*      int sz = sizeof(PfxSolverBody);
01170         int sz2 = sizeof(vmVector3);
01171         int sz3 = sizeof(vmMatrix3);
01172         int sz4 = sizeof(vmQuat);
01173         int sz5 = sizeof(btConstraintRow);
01174         int sz6 = sizeof(btSolverConstraint);
01175         int sz7 = sizeof(TrbState);
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                 //if(state.getMotionMask()&PFX_MOTION_MASK_DYNAMIC) {
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         //4 points per manifold and 3 rows per point makes 12 rows per manifold
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                                 //int contactId = i1;//actualNumManifolds;
01273                                 
01274                                 PfxBroadphasePair& pair = m_memoryCache->m_mypairs[actualNumManifolds];
01275                                 //init those
01276                 //              float compFric = obA->getFriction()*obB->getFriction();//@todo
01277                                 int idA = obA->getCompanionId();
01278                                 int idB = obB->getCompanionId();
01279                                 
01280                                 m->m_companionIdA = idA;
01281                                 m->m_companionIdB = idB;
01282                                 
01283                                 
01284                         //      if ((mysolverbodies[idA].mMassInv!=0)&&(mysolverbodies[idB].mMassInv!=0))
01285                         //              continue;
01286                                 int numPosPoints=0;
01287                                 for (int p=0;p<m->getNumContacts();p++)
01288                                 {
01289                                         //btManifoldPoint& pt = m->getContactPoint(p);
01290                                         //float dist = pt.getDistance();
01291                                         //if (dist<0.001)
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                                 //likely the contact pool is not contiguous, make sure to allocate large enough contact pool
01306                                 btAssert(contactId>=0);
01307                                 btAssert(contactId<dispatcher->getInternalManifoldPool()->getMaxCount());
01308                                 
01309                                 pfxSetContactId(pair,contactId);
01310                                 pfxSetNumConstraints(pair,numPosPoints);//manifoldPtr[i]->getNumContacts());
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         //if (1)
01325         {
01326                 
01327                 {
01328                         BT_PROFILE("convert constraints");
01329 
01330                         int totalNumRows = 0;
01331                         int i;
01332                         
01333                         m_tmpConstraintSizesPool.resize(numConstraints);
01334                         //calculate the total number of contraint rows
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(&currentConstraintRow[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);//check this
01390                                         btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint));
01391                                         info2.m_constraintError = &currentConstraintRow->m_rhs;
01392                                         currentConstraintRow->m_cfm = infoGlobal.m_globalCfm;
01393                                         info2.cfm = &currentConstraintRow->m_cfm;
01394                                         info2.m_lowerLimit = &currentConstraintRow->m_lowerLimit;
01395                                         info2.m_upperLimit = &currentConstraintRow->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();//sign of normal?
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;//already filled in by getConstraintInfo2
01446                                                         btScalar        velocityError = restitution - rel_vel;// * damping;
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                                         //is this needed?
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;//info.m_erp;//or m_erp2?
01485         float timeStep=infoGlobal.m_timeStep;
01486         int iteration=infoGlobal.m_numIterations;
01487 
01488         //create a pair for each constraints, copy over info etc
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                 //printf("numManifolds = %d\n",numManifolds);
01504                 //printf("totalContacts=%d\n",totalContacts);
01505         }
01506         
01507 
01508 
01509 //      printf("actualNumManifolds=%d\n",actualNumManifolds);
01510         {
01511                 BT_PROFILE("BPE_customConstraintSolverSequentialNew");
01512                 if (numRigidBodies>0 && (actualNumManifolds+actualNumJoints)>0)
01513                 {
01514 //                      PFX_PRINTF("num points = %d\n",totalPoints);
01515 //                      PFX_PRINTF("num points PFX = %d\n",total);
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         //copy results back to bodies
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 }