- Generated on Fri May 11 2012 11:24:00 for Bullet Collision Detection & Physics Library by
1.7.3
|
Bullet Collision Detection & Physics Library
|
00001 /* 00002 Bullet Continuous Collision Detection and Physics Library 00003 Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ 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 //#define COMPUTE_IMPULSE_DENOM 1 00017 //It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms. 00018 00019 #include "btSequentialImpulseConstraintSolver.h" 00020 #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" 00021 #include "BulletDynamics/Dynamics/btRigidBody.h" 00022 #include "btContactConstraint.h" 00023 #include "btSolve2LinearConstraint.h" 00024 #include "btContactSolverInfo.h" 00025 #include "LinearMath/btIDebugDraw.h" 00026 #include "btJacobianEntry.h" 00027 #include "LinearMath/btMinMax.h" 00028 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" 00029 #include <new> 00030 #include "LinearMath/btStackAlloc.h" 00031 #include "LinearMath/btQuickprof.h" 00032 #include "btSolverBody.h" 00033 #include "btSolverConstraint.h" 00034 #include "LinearMath/btAlignedObjectArray.h" 00035 #include <string.h> //for memset 00036 00037 int gNumSplitImpulseRecoveries = 0; 00038 00039 btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver() 00040 :m_btSeed2(0) 00041 { 00042 00043 } 00044 00045 btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver() 00046 { 00047 } 00048 00049 #ifdef USE_SIMD 00050 #include <emmintrin.h> 00051 #define btVecSplat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e,e,e,e)) 00052 static inline __m128 btSimdDot3( __m128 vec0, __m128 vec1 ) 00053 { 00054 __m128 result = _mm_mul_ps( vec0, vec1); 00055 return _mm_add_ps( btVecSplat( result, 0 ), _mm_add_ps( btVecSplat( result, 1 ), btVecSplat( result, 2 ) ) ); 00056 } 00057 #endif//USE_SIMD 00058 00059 // Project Gauss Seidel or the equivalent Sequential Impulse 00060 void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c) 00061 { 00062 #ifdef USE_SIMD 00063 __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse); 00064 __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); 00065 __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); 00066 __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm))); 00067 __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128)); 00068 __m128 deltaVel2Dotn = _mm_sub_ps(btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),btSimdDot3((c.m_contactNormal).mVec128,body2.internalGetDeltaLinearVelocity().mVec128)); 00069 deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv))); 00070 deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv))); 00071 btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse); 00072 btSimdScalar resultLowerLess,resultUpperLess; 00073 resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1); 00074 resultUpperLess = _mm_cmplt_ps(sum,upperLimit1); 00075 __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp); 00076 deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) ); 00077 c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) ); 00078 __m128 upperMinApplied = _mm_sub_ps(upperLimit1,cpAppliedImp); 00079 deltaImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied) ); 00080 c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1) ); 00081 __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128); 00082 __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128); 00083 __m128 impulseMagnitude = deltaImpulse; 00084 body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); 00085 body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); 00086 body2.internalGetDeltaLinearVelocity().mVec128 = _mm_sub_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); 00087 body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude)); 00088 #else 00089 resolveSingleConstraintRowGeneric(body1,body2,c); 00090 #endif 00091 } 00092 00093 // Project Gauss Seidel or the equivalent Sequential Impulse 00094 void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c) 00095 { 00096 btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm; 00097 const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity()); 00098 const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity()); 00099 00100 // const btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn; 00101 deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; 00102 deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; 00103 00104 const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse; 00105 if (sum < c.m_lowerLimit) 00106 { 00107 deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse; 00108 c.m_appliedImpulse = c.m_lowerLimit; 00109 } 00110 else if (sum > c.m_upperLimit) 00111 { 00112 deltaImpulse = c.m_upperLimit-c.m_appliedImpulse; 00113 c.m_appliedImpulse = c.m_upperLimit; 00114 } 00115 else 00116 { 00117 c.m_appliedImpulse = sum; 00118 } 00119 body1.internalApplyImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); 00120 body2.internalApplyImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); 00121 } 00122 00123 void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c) 00124 { 00125 #ifdef USE_SIMD 00126 __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse); 00127 __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); 00128 __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); 00129 __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm))); 00130 __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128)); 00131 __m128 deltaVel2Dotn = _mm_sub_ps(btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),btSimdDot3((c.m_contactNormal).mVec128,body2.internalGetDeltaLinearVelocity().mVec128)); 00132 deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv))); 00133 deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv))); 00134 btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse); 00135 btSimdScalar resultLowerLess,resultUpperLess; 00136 resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1); 00137 resultUpperLess = _mm_cmplt_ps(sum,upperLimit1); 00138 __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp); 00139 deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) ); 00140 c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) ); 00141 __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128); 00142 __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128); 00143 __m128 impulseMagnitude = deltaImpulse; 00144 body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); 00145 body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); 00146 body2.internalGetDeltaLinearVelocity().mVec128 = _mm_sub_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); 00147 body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude)); 00148 #else 00149 resolveSingleConstraintRowLowerLimit(body1,body2,c); 00150 #endif 00151 } 00152 00153 // Project Gauss Seidel or the equivalent Sequential Impulse 00154 void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c) 00155 { 00156 btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm; 00157 const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity()); 00158 const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity()); 00159 00160 deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; 00161 deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; 00162 const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse; 00163 if (sum < c.m_lowerLimit) 00164 { 00165 deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse; 00166 c.m_appliedImpulse = c.m_lowerLimit; 00167 } 00168 else 00169 { 00170 c.m_appliedImpulse = sum; 00171 } 00172 body1.internalApplyImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); 00173 body2.internalApplyImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); 00174 } 00175 00176 00177 void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFriendly( 00178 btRigidBody& body1, 00179 btRigidBody& body2, 00180 const btSolverConstraint& c) 00181 { 00182 if (c.m_rhsPenetration) 00183 { 00184 gNumSplitImpulseRecoveries++; 00185 btScalar deltaImpulse = c.m_rhsPenetration-btScalar(c.m_appliedPushImpulse)*c.m_cfm; 00186 const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.internalGetPushVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetTurnVelocity()); 00187 const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.internalGetPushVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetTurnVelocity()); 00188 00189 deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; 00190 deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; 00191 const btScalar sum = btScalar(c.m_appliedPushImpulse) + deltaImpulse; 00192 if (sum < c.m_lowerLimit) 00193 { 00194 deltaImpulse = c.m_lowerLimit-c.m_appliedPushImpulse; 00195 c.m_appliedPushImpulse = c.m_lowerLimit; 00196 } 00197 else 00198 { 00199 c.m_appliedPushImpulse = sum; 00200 } 00201 body1.internalApplyPushImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); 00202 body2.internalApplyPushImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); 00203 } 00204 } 00205 00206 void btSequentialImpulseConstraintSolver::resolveSplitPenetrationSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c) 00207 { 00208 #ifdef USE_SIMD 00209 if (!c.m_rhsPenetration) 00210 return; 00211 00212 gNumSplitImpulseRecoveries++; 00213 00214 __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedPushImpulse); 00215 __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); 00216 __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); 00217 __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhsPenetration), _mm_mul_ps(_mm_set1_ps(c.m_appliedPushImpulse),_mm_set1_ps(c.m_cfm))); 00218 __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal.mVec128,body1.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetTurnVelocity().mVec128)); 00219 __m128 deltaVel2Dotn = _mm_sub_ps(btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetTurnVelocity().mVec128),btSimdDot3((c.m_contactNormal).mVec128,body2.internalGetPushVelocity().mVec128)); 00220 deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv))); 00221 deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv))); 00222 btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse); 00223 btSimdScalar resultLowerLess,resultUpperLess; 00224 resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1); 00225 resultUpperLess = _mm_cmplt_ps(sum,upperLimit1); 00226 __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp); 00227 deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) ); 00228 c.m_appliedPushImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) ); 00229 __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128); 00230 __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128); 00231 __m128 impulseMagnitude = deltaImpulse; 00232 body1.internalGetPushVelocity().mVec128 = _mm_add_ps(body1.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); 00233 body1.internalGetTurnVelocity().mVec128 = _mm_add_ps(body1.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); 00234 body2.internalGetPushVelocity().mVec128 = _mm_sub_ps(body2.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); 00235 body2.internalGetTurnVelocity().mVec128 = _mm_add_ps(body2.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude)); 00236 #else 00237 resolveSplitPenetrationImpulseCacheFriendly(body1,body2,c); 00238 #endif 00239 } 00240 00241 00242 00243 unsigned long btSequentialImpulseConstraintSolver::btRand2() 00244 { 00245 m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff; 00246 return m_btSeed2; 00247 } 00248 00249 00250 00251 //See ODE: adam's all-int straightforward(?) dRandInt (0..n-1) 00252 int btSequentialImpulseConstraintSolver::btRandInt2 (int n) 00253 { 00254 // seems good; xor-fold and modulus 00255 const unsigned long un = static_cast<unsigned long>(n); 00256 unsigned long r = btRand2(); 00257 00258 // note: probably more aggressive than it needs to be -- might be 00259 // able to get away without one or two of the innermost branches. 00260 if (un <= 0x00010000UL) { 00261 r ^= (r >> 16); 00262 if (un <= 0x00000100UL) { 00263 r ^= (r >> 8); 00264 if (un <= 0x00000010UL) { 00265 r ^= (r >> 4); 00266 if (un <= 0x00000004UL) { 00267 r ^= (r >> 2); 00268 if (un <= 0x00000002UL) { 00269 r ^= (r >> 1); 00270 } 00271 } 00272 } 00273 } 00274 } 00275 00276 return (int) (r % un); 00277 } 00278 00279 00280 #if 0 00281 void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject) 00282 { 00283 btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0; 00284 00285 solverBody->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); 00286 solverBody->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); 00287 solverBody->internalGetPushVelocity().setValue(0.f,0.f,0.f); 00288 solverBody->internalGetTurnVelocity().setValue(0.f,0.f,0.f); 00289 00290 if (rb) 00291 { 00292 solverBody->internalGetInvMass() = btVector3(rb->getInvMass(),rb->getInvMass(),rb->getInvMass())*rb->getLinearFactor(); 00293 solverBody->m_originalBody = rb; 00294 solverBody->m_angularFactor = rb->getAngularFactor(); 00295 } else 00296 { 00297 solverBody->internalGetInvMass().setValue(0,0,0); 00298 solverBody->m_originalBody = 0; 00299 solverBody->m_angularFactor.setValue(1,1,1); 00300 } 00301 } 00302 #endif 00303 00304 00305 00306 00307 00308 btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution) 00309 { 00310 btScalar rest = restitution * -rel_vel; 00311 return rest; 00312 } 00313 00314 00315 00316 void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection); 00317 void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection) 00318 { 00319 if (colObj && colObj->hasAnisotropicFriction()) 00320 { 00321 // transform to local coordinates 00322 btVector3 loc_lateral = frictionDirection * colObj->getWorldTransform().getBasis(); 00323 const btVector3& friction_scaling = colObj->getAnisotropicFriction(); 00324 //apply anisotropic friction 00325 loc_lateral *= friction_scaling; 00326 // ... and transform it back to global coordinates 00327 frictionDirection = colObj->getWorldTransform().getBasis() * loc_lateral; 00328 } 00329 } 00330 00331 00332 void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyB,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip) 00333 { 00334 00335 00336 btRigidBody* body0=btRigidBody::upcast(colObj0); 00337 btRigidBody* body1=btRigidBody::upcast(colObj1); 00338 00339 solverConstraint.m_contactNormal = normalAxis; 00340 00341 solverConstraint.m_solverBodyA = body0 ? body0 : &getFixedBody(); 00342 solverConstraint.m_solverBodyB = body1 ? body1 : &getFixedBody(); 00343 00344 solverConstraint.m_friction = cp.m_combinedFriction; 00345 solverConstraint.m_originalContactPoint = 0; 00346 00347 solverConstraint.m_appliedImpulse = 0.f; 00348 solverConstraint.m_appliedPushImpulse = 0.f; 00349 00350 { 00351 btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal); 00352 solverConstraint.m_relpos1CrossNormal = ftorqueAxis1; 00353 solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor() : btVector3(0,0,0); 00354 } 00355 { 00356 btVector3 ftorqueAxis1 = rel_pos2.cross(-solverConstraint.m_contactNormal); 00357 solverConstraint.m_relpos2CrossNormal = ftorqueAxis1; 00358 solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor() : btVector3(0,0,0); 00359 } 00360 00361 #ifdef COMPUTE_IMPULSE_DENOM 00362 btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal); 00363 btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal); 00364 #else 00365 btVector3 vec; 00366 btScalar denom0 = 0.f; 00367 btScalar denom1 = 0.f; 00368 if (body0) 00369 { 00370 vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); 00371 denom0 = body0->getInvMass() + normalAxis.dot(vec); 00372 } 00373 if (body1) 00374 { 00375 vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); 00376 denom1 = body1->getInvMass() + normalAxis.dot(vec); 00377 } 00378 00379 00380 #endif //COMPUTE_IMPULSE_DENOM 00381 btScalar denom = relaxation/(denom0+denom1); 00382 solverConstraint.m_jacDiagABInv = denom; 00383 00384 #ifdef _USE_JACOBIAN 00385 solverConstraint.m_jac = btJacobianEntry ( 00386 rel_pos1,rel_pos2,solverConstraint.m_contactNormal, 00387 body0->getInvInertiaDiagLocal(), 00388 body0->getInvMass(), 00389 body1->getInvInertiaDiagLocal(), 00390 body1->getInvMass()); 00391 #endif //_USE_JACOBIAN 00392 00393 00394 { 00395 btScalar rel_vel; 00396 btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0?body0->getLinearVelocity():btVector3(0,0,0)) 00397 + solverConstraint.m_relpos1CrossNormal.dot(body0?body0->getAngularVelocity():btVector3(0,0,0)); 00398 btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(body1?body1->getLinearVelocity():btVector3(0,0,0)) 00399 + solverConstraint.m_relpos2CrossNormal.dot(body1?body1->getAngularVelocity():btVector3(0,0,0)); 00400 00401 rel_vel = vel1Dotn+vel2Dotn; 00402 00403 // btScalar positionalError = 0.f; 00404 00405 btSimdScalar velocityError = desiredVelocity - rel_vel; 00406 btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv); 00407 solverConstraint.m_rhs = velocityImpulse; 00408 solverConstraint.m_cfm = cfmSlip; 00409 solverConstraint.m_lowerLimit = 0; 00410 solverConstraint.m_upperLimit = 1e10f; 00411 } 00412 } 00413 00414 00415 00416 btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip) 00417 { 00418 btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing(); 00419 solverConstraint.m_frictionIndex = frictionIndex; 00420 setupFrictionConstraint(solverConstraint, normalAxis, solverBodyA, solverBodyB, cp, rel_pos1, rel_pos2, 00421 colObj0, colObj1, relaxation, desiredVelocity, cfmSlip); 00422 return solverConstraint; 00423 } 00424 00425 int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body) 00426 { 00427 #if 0 00428 int solverBodyIdA = -1; 00429 00430 if (body.getCompanionId() >= 0) 00431 { 00432 //body has already been converted 00433 solverBodyIdA = body.getCompanionId(); 00434 } else 00435 { 00436 btRigidBody* rb = btRigidBody::upcast(&body); 00437 if (rb && rb->getInvMass()) 00438 { 00439 solverBodyIdA = m_tmpSolverBodyPool.size(); 00440 btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); 00441 initSolverBody(&solverBody,&body); 00442 body.setCompanionId(solverBodyIdA); 00443 } else 00444 { 00445 return 0;//assume first one is a fixed solver body 00446 } 00447 } 00448 return solverBodyIdA; 00449 #endif 00450 return 0; 00451 } 00452 #include <stdio.h> 00453 00454 00455 void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstraint& solverConstraint, 00456 btCollisionObject* colObj0, btCollisionObject* colObj1, 00457 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, 00458 btVector3& vel, btScalar& rel_vel, btScalar& relaxation, 00459 btVector3& rel_pos1, btVector3& rel_pos2) 00460 { 00461 btRigidBody* rb0 = btRigidBody::upcast(colObj0); 00462 btRigidBody* rb1 = btRigidBody::upcast(colObj1); 00463 00464 const btVector3& pos1 = cp.getPositionWorldOnA(); 00465 const btVector3& pos2 = cp.getPositionWorldOnB(); 00466 00467 // btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); 00468 // btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin(); 00469 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); 00470 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin(); 00471 00472 relaxation = 1.f; 00473 00474 btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB); 00475 solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); 00476 btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB); 00477 solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0); 00478 00479 { 00480 #ifdef COMPUTE_IMPULSE_DENOM 00481 btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB); 00482 btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB); 00483 #else 00484 btVector3 vec; 00485 btScalar denom0 = 0.f; 00486 btScalar denom1 = 0.f; 00487 if (rb0) 00488 { 00489 vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); 00490 denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec); 00491 } 00492 if (rb1) 00493 { 00494 vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); 00495 denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec); 00496 } 00497 #endif //COMPUTE_IMPULSE_DENOM 00498 00499 btScalar denom = relaxation/(denom0+denom1); 00500 solverConstraint.m_jacDiagABInv = denom; 00501 } 00502 00503 solverConstraint.m_contactNormal = cp.m_normalWorldOnB; 00504 solverConstraint.m_relpos1CrossNormal = rel_pos1.cross(cp.m_normalWorldOnB); 00505 solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(-cp.m_normalWorldOnB); 00506 00507 00508 00509 00510 btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0); 00511 btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0); 00512 vel = vel1 - vel2; 00513 rel_vel = cp.m_normalWorldOnB.dot(vel); 00514 00515 btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop; 00516 00517 00518 solverConstraint.m_friction = cp.m_combinedFriction; 00519 00520 btScalar restitution = 0.f; 00521 00522 if (cp.m_lifeTime>infoGlobal.m_restingContactRestitutionThreshold) 00523 { 00524 restitution = 0.f; 00525 } else 00526 { 00527 restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution); 00528 if (restitution <= btScalar(0.)) 00529 { 00530 restitution = 0.f; 00531 }; 00532 } 00533 00534 00536 if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) 00537 { 00538 solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; 00539 if (rb0) 00540 rb0->internalApplyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse); 00541 if (rb1) 00542 rb1->internalApplyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse); 00543 } else 00544 { 00545 solverConstraint.m_appliedImpulse = 0.f; 00546 } 00547 00548 solverConstraint.m_appliedPushImpulse = 0.f; 00549 00550 { 00551 btScalar rel_vel; 00552 btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rb0?rb0->getLinearVelocity():btVector3(0,0,0)) 00553 + solverConstraint.m_relpos1CrossNormal.dot(rb0?rb0->getAngularVelocity():btVector3(0,0,0)); 00554 btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rb1?rb1->getLinearVelocity():btVector3(0,0,0)) 00555 + solverConstraint.m_relpos2CrossNormal.dot(rb1?rb1->getAngularVelocity():btVector3(0,0,0)); 00556 00557 rel_vel = vel1Dotn+vel2Dotn; 00558 00559 btScalar positionalError = 0.f; 00560 btScalar velocityError = restitution - rel_vel;// * damping; 00561 00562 if (penetration>0) 00563 { 00564 positionalError = 0; 00565 velocityError -= penetration / infoGlobal.m_timeStep; 00566 } else 00567 { 00568 positionalError = -penetration * infoGlobal.m_erp/infoGlobal.m_timeStep; 00569 } 00570 00571 btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; 00572 btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; 00573 if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) 00574 { 00575 //combine position and velocity into rhs 00576 solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; 00577 solverConstraint.m_rhsPenetration = 0.f; 00578 } else 00579 { 00580 //split position and velocity into rhs and m_rhsPenetration 00581 solverConstraint.m_rhs = velocityImpulse; 00582 solverConstraint.m_rhsPenetration = penetrationImpulse; 00583 } 00584 solverConstraint.m_cfm = 0.f; 00585 solverConstraint.m_lowerLimit = 0; 00586 solverConstraint.m_upperLimit = 1e10f; 00587 } 00588 00589 00590 00591 00592 } 00593 00594 00595 00596 void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, 00597 btRigidBody* rb0, btRigidBody* rb1, 00598 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal) 00599 { 00600 if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING) 00601 { 00602 { 00603 btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex]; 00604 if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) 00605 { 00606 frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor; 00607 if (rb0) 00608 rb0->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse); 00609 if (rb1) 00610 rb1->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-frictionConstraint1.m_angularComponentB,-(btScalar)frictionConstraint1.m_appliedImpulse); 00611 } else 00612 { 00613 frictionConstraint1.m_appliedImpulse = 0.f; 00614 } 00615 } 00616 00617 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) 00618 { 00619 btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1]; 00620 if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) 00621 { 00622 frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor; 00623 if (rb0) 00624 rb0->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse); 00625 if (rb1) 00626 rb1->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-(btScalar)frictionConstraint2.m_appliedImpulse); 00627 } else 00628 { 00629 frictionConstraint2.m_appliedImpulse = 0.f; 00630 } 00631 } 00632 } else 00633 { 00634 btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex]; 00635 frictionConstraint1.m_appliedImpulse = 0.f; 00636 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) 00637 { 00638 btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1]; 00639 frictionConstraint2.m_appliedImpulse = 0.f; 00640 } 00641 } 00642 } 00643 00644 00645 00646 00647 void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal) 00648 { 00649 btCollisionObject* colObj0=0,*colObj1=0; 00650 00651 colObj0 = (btCollisionObject*)manifold->getBody0(); 00652 colObj1 = (btCollisionObject*)manifold->getBody1(); 00653 00654 00655 btRigidBody* solverBodyA = btRigidBody::upcast(colObj0); 00656 btRigidBody* solverBodyB = btRigidBody::upcast(colObj1); 00657 00659 if ((!solverBodyA || !solverBodyA->getInvMass()) && (!solverBodyB || !solverBodyB->getInvMass())) 00660 return; 00661 00662 for (int j=0;j<manifold->getNumContacts();j++) 00663 { 00664 00665 btManifoldPoint& cp = manifold->getContactPoint(j); 00666 00667 if (cp.getDistance() <= manifold->getContactProcessingThreshold()) 00668 { 00669 btVector3 rel_pos1; 00670 btVector3 rel_pos2; 00671 btScalar relaxation; 00672 btScalar rel_vel; 00673 btVector3 vel; 00674 00675 int frictionIndex = m_tmpSolverContactConstraintPool.size(); 00676 btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing(); 00677 btRigidBody* rb0 = btRigidBody::upcast(colObj0); 00678 btRigidBody* rb1 = btRigidBody::upcast(colObj1); 00679 solverConstraint.m_solverBodyA = rb0? rb0 : &getFixedBody(); 00680 solverConstraint.m_solverBodyB = rb1? rb1 : &getFixedBody(); 00681 solverConstraint.m_originalContactPoint = &cp; 00682 00683 setupContactConstraint(solverConstraint, colObj0, colObj1, cp, infoGlobal, vel, rel_vel, relaxation, rel_pos1, rel_pos2); 00684 00685 // const btVector3& pos1 = cp.getPositionWorldOnA(); 00686 // const btVector3& pos2 = cp.getPositionWorldOnB(); 00687 00689 00690 solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size(); 00691 00692 if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized) 00693 { 00694 cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel; 00695 btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2(); 00696 if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON) 00697 { 00698 cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel); 00699 if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) 00700 { 00701 cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB); 00702 cp.m_lateralFrictionDir2.normalize();//?? 00703 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2); 00704 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2); 00705 addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); 00706 } 00707 00708 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1); 00709 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1); 00710 addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); 00711 cp.m_lateralFrictionInitialized = true; 00712 } else 00713 { 00714 //re-calculate friction direction every frame, todo: check if this is really needed 00715 btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2); 00716 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) 00717 { 00718 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2); 00719 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2); 00720 addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); 00721 } 00722 00723 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1); 00724 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1); 00725 addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); 00726 00727 cp.m_lateralFrictionInitialized = true; 00728 } 00729 00730 } else 00731 { 00732 addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_contactCFM1); 00733 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) 00734 addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2); 00735 } 00736 00737 setFrictionConstraintImpulse( solverConstraint, rb0, rb1, cp, infoGlobal); 00738 00739 } 00740 } 00741 } 00742 00743 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc) 00744 { 00745 BT_PROFILE("solveGroupCacheFriendlySetup"); 00746 (void)stackAlloc; 00747 (void)debugDrawer; 00748 00749 m_maxOverrideNumSolverIterations = 0; 00750 00751 if (!(numConstraints + numManifolds)) 00752 { 00753 // printf("empty\n"); 00754 return 0.f; 00755 } 00756 00757 if (infoGlobal.m_splitImpulse) 00758 { 00759 for (int i = 0; i < numBodies; i++) 00760 { 00761 btRigidBody* body = btRigidBody::upcast(bodies[i]); 00762 if (body) 00763 { 00764 body->internalGetDeltaLinearVelocity().setZero(); 00765 body->internalGetDeltaAngularVelocity().setZero(); 00766 body->internalGetPushVelocity().setZero(); 00767 body->internalGetTurnVelocity().setZero(); 00768 } 00769 } 00770 } 00771 else 00772 { 00773 for (int i = 0; i < numBodies; i++) 00774 { 00775 btRigidBody* body = btRigidBody::upcast(bodies[i]); 00776 if (body) 00777 { 00778 body->internalGetDeltaLinearVelocity().setZero(); 00779 body->internalGetDeltaAngularVelocity().setZero(); 00780 } 00781 } 00782 } 00783 00784 if (1) 00785 { 00786 int j; 00787 for (j=0;j<numConstraints;j++) 00788 { 00789 btTypedConstraint* constraint = constraints[j]; 00790 constraint->buildJacobian(); 00791 constraint->internalSetAppliedImpulse(0.0f); 00792 } 00793 } 00794 //btRigidBody* rb0=0,*rb1=0; 00795 00796 //if (1) 00797 { 00798 { 00799 00800 int totalNumRows = 0; 00801 int i; 00802 00803 m_tmpConstraintSizesPool.resize(numConstraints); 00804 //calculate the total number of contraint rows 00805 for (i=0;i<numConstraints;i++) 00806 { 00807 btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i]; 00808 if (constraints[i]->isEnabled()) 00809 { 00810 constraints[i]->getInfo1(&info1); 00811 } else 00812 { 00813 info1.m_numConstraintRows = 0; 00814 info1.nub = 0; 00815 } 00816 totalNumRows += info1.m_numConstraintRows; 00817 } 00818 m_tmpSolverNonContactConstraintPool.resize(totalNumRows); 00819 00820 00822 int currentRow = 0; 00823 00824 for (i=0;i<numConstraints;i++) 00825 { 00826 const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i]; 00827 00828 if (info1.m_numConstraintRows) 00829 { 00830 btAssert(currentRow<totalNumRows); 00831 00832 btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow]; 00833 btTypedConstraint* constraint = constraints[i]; 00834 btRigidBody& rbA = constraint->getRigidBodyA(); 00835 btRigidBody& rbB = constraint->getRigidBodyB(); 00836 00837 00838 int overrideNumSolverIterations = constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations; 00839 if (overrideNumSolverIterations>m_maxOverrideNumSolverIterations) 00840 m_maxOverrideNumSolverIterations = overrideNumSolverIterations; 00841 00842 00843 int j; 00844 for ( j=0;j<info1.m_numConstraintRows;j++) 00845 { 00846 memset(¤tConstraintRow[j],0,sizeof(btSolverConstraint)); 00847 currentConstraintRow[j].m_lowerLimit = -SIMD_INFINITY; 00848 currentConstraintRow[j].m_upperLimit = SIMD_INFINITY; 00849 currentConstraintRow[j].m_appliedImpulse = 0.f; 00850 currentConstraintRow[j].m_appliedPushImpulse = 0.f; 00851 currentConstraintRow[j].m_solverBodyA = &rbA; 00852 currentConstraintRow[j].m_solverBodyB = &rbB; 00853 currentConstraintRow[j].m_overrideNumSolverIterations = overrideNumSolverIterations; 00854 } 00855 00856 rbA.internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); 00857 rbA.internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); 00858 rbB.internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); 00859 rbB.internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); 00860 00861 00862 00863 btTypedConstraint::btConstraintInfo2 info2; 00864 info2.fps = 1.f/infoGlobal.m_timeStep; 00865 info2.erp = infoGlobal.m_erp; 00866 info2.m_J1linearAxis = currentConstraintRow->m_contactNormal; 00867 info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal; 00868 info2.m_J2linearAxis = 0; 00869 info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal; 00870 info2.rowskip = sizeof(btSolverConstraint)/sizeof(btScalar);//check this 00872 btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint)); 00873 info2.m_constraintError = ¤tConstraintRow->m_rhs; 00874 currentConstraintRow->m_cfm = infoGlobal.m_globalCfm; 00875 info2.m_damping = infoGlobal.m_damping; 00876 info2.cfm = ¤tConstraintRow->m_cfm; 00877 info2.m_lowerLimit = ¤tConstraintRow->m_lowerLimit; 00878 info2.m_upperLimit = ¤tConstraintRow->m_upperLimit; 00879 info2.m_numIterations = infoGlobal.m_numIterations; 00880 constraints[i]->getInfo2(&info2); 00881 00883 for ( j=0;j<info1.m_numConstraintRows;j++) 00884 { 00885 btSolverConstraint& solverConstraint = currentConstraintRow[j]; 00886 00887 if (solverConstraint.m_upperLimit>=constraints[i]->getBreakingImpulseThreshold()) 00888 { 00889 solverConstraint.m_upperLimit = constraints[i]->getBreakingImpulseThreshold(); 00890 } 00891 00892 if (solverConstraint.m_lowerLimit<=-constraints[i]->getBreakingImpulseThreshold()) 00893 { 00894 solverConstraint.m_lowerLimit = -constraints[i]->getBreakingImpulseThreshold(); 00895 } 00896 00897 solverConstraint.m_originalContactPoint = constraint; 00898 00899 { 00900 const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal; 00901 solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld()*ftorqueAxis1*constraint->getRigidBodyA().getAngularFactor(); 00902 } 00903 { 00904 const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal; 00905 solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld()*ftorqueAxis2*constraint->getRigidBodyB().getAngularFactor(); 00906 } 00907 00908 { 00909 btVector3 iMJlA = solverConstraint.m_contactNormal*rbA.getInvMass(); 00910 btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal; 00911 btVector3 iMJlB = solverConstraint.m_contactNormal*rbB.getInvMass();//sign of normal? 00912 btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal; 00913 00914 btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal); 00915 sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal); 00916 sum += iMJlB.dot(solverConstraint.m_contactNormal); 00917 sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal); 00918 00919 solverConstraint.m_jacDiagABInv = btScalar(1.)/sum; 00920 } 00921 00922 00925 { 00926 btScalar rel_vel; 00927 btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rbA.getLinearVelocity()) + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity()); 00928 btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rbB.getLinearVelocity()) + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity()); 00929 00930 rel_vel = vel1Dotn+vel2Dotn; 00931 00932 btScalar restitution = 0.f; 00933 btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2 00934 btScalar velocityError = restitution - rel_vel * info2.m_damping; 00935 btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; 00936 btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; 00937 solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; 00938 solverConstraint.m_appliedImpulse = 0.f; 00939 00940 } 00941 } 00942 } 00943 currentRow+=m_tmpConstraintSizesPool[i].m_numConstraintRows; 00944 } 00945 } 00946 00947 { 00948 int i; 00949 btPersistentManifold* manifold = 0; 00950 // btCollisionObject* colObj0=0,*colObj1=0; 00951 00952 00953 for (i=0;i<numManifolds;i++) 00954 { 00955 manifold = manifoldPtr[i]; 00956 convertContact(manifold,infoGlobal); 00957 } 00958 } 00959 } 00960 00961 btContactSolverInfo info = infoGlobal; 00962 00963 00964 int numNonContactPool = m_tmpSolverNonContactConstraintPool.size(); 00965 int numConstraintPool = m_tmpSolverContactConstraintPool.size(); 00966 int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size(); 00967 00969 m_orderNonContactConstraintPool.resize(numNonContactPool); 00970 m_orderTmpConstraintPool.resize(numConstraintPool); 00971 m_orderFrictionConstraintPool.resize(numFrictionPool); 00972 { 00973 int i; 00974 for (i=0;i<numNonContactPool;i++) 00975 { 00976 m_orderNonContactConstraintPool[i] = i; 00977 } 00978 for (i=0;i<numConstraintPool;i++) 00979 { 00980 m_orderTmpConstraintPool[i] = i; 00981 } 00982 for (i=0;i<numFrictionPool;i++) 00983 { 00984 m_orderFrictionConstraintPool[i] = i; 00985 } 00986 } 00987 00988 return 0.f; 00989 00990 } 00991 00992 btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/) 00993 { 00994 00995 int numNonContactPool = m_tmpSolverNonContactConstraintPool.size(); 00996 int numConstraintPool = m_tmpSolverContactConstraintPool.size(); 00997 int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size(); 00998 00999 int j; 01000 01001 if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER) 01002 { 01003 if ((iteration & 7) == 0) { 01004 for (j=0; j<numNonContactPool; ++j) { 01005 int tmp = m_orderNonContactConstraintPool[j]; 01006 int swapi = btRandInt2(j+1); 01007 m_orderNonContactConstraintPool[j] = m_orderNonContactConstraintPool[swapi]; 01008 m_orderNonContactConstraintPool[swapi] = tmp; 01009 } 01010 01011 //contact/friction constraints are not solved more than 01012 if (iteration< infoGlobal.m_numIterations) 01013 { 01014 for (j=0; j<numConstraintPool; ++j) { 01015 int tmp = m_orderTmpConstraintPool[j]; 01016 int swapi = btRandInt2(j+1); 01017 m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi]; 01018 m_orderTmpConstraintPool[swapi] = tmp; 01019 } 01020 01021 for (j=0; j<numFrictionPool; ++j) { 01022 int tmp = m_orderFrictionConstraintPool[j]; 01023 int swapi = btRandInt2(j+1); 01024 m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi]; 01025 m_orderFrictionConstraintPool[swapi] = tmp; 01026 } 01027 } 01028 } 01029 } 01030 01031 if (infoGlobal.m_solverMode & SOLVER_SIMD) 01032 { 01034 for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) 01035 { 01036 btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]]; 01037 if (iteration < constraint.m_overrideNumSolverIterations) 01038 resolveSingleConstraintRowGenericSIMD(*constraint.m_solverBodyA,*constraint.m_solverBodyB,constraint); 01039 } 01040 01041 if (iteration< infoGlobal.m_numIterations) 01042 { 01043 for (j=0;j<numConstraints;j++) 01044 { 01045 constraints[j]->solveConstraintObsolete(constraints[j]->getRigidBodyA(),constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep); 01046 } 01047 01049 int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); 01050 for (j=0;j<numPoolConstraints;j++) 01051 { 01052 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; 01053 resolveSingleConstraintRowLowerLimitSIMD(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold); 01054 01055 } 01056 01058 int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size(); 01059 for (j=0;j<numFrictionPoolConstraints;j++) 01060 { 01061 btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]]; 01062 btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; 01063 01064 if (totalImpulse>btScalar(0)) 01065 { 01066 solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); 01067 solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; 01068 01069 resolveSingleConstraintRowGenericSIMD(*solveManifold.m_solverBodyA, *solveManifold.m_solverBodyB,solveManifold); 01070 } 01071 } 01072 } 01073 } else 01074 { 01075 01077 for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) 01078 { 01079 btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]]; 01080 if (iteration < constraint.m_overrideNumSolverIterations) 01081 resolveSingleConstraintRowGeneric(*constraint.m_solverBodyA,*constraint.m_solverBodyB,constraint); 01082 } 01083 01084 if (iteration< infoGlobal.m_numIterations) 01085 { 01086 for (j=0;j<numConstraints;j++) 01087 { 01088 constraints[j]->solveConstraintObsolete(constraints[j]->getRigidBodyA(),constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep); 01089 } 01091 int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); 01092 for (j=0;j<numPoolConstraints;j++) 01093 { 01094 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; 01095 resolveSingleConstraintRowLowerLimit(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold); 01096 } 01098 int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size(); 01099 for (j=0;j<numFrictionPoolConstraints;j++) 01100 { 01101 btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]]; 01102 btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; 01103 01104 if (totalImpulse>btScalar(0)) 01105 { 01106 solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); 01107 solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; 01108 01109 resolveSingleConstraintRowGeneric(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold); 01110 } 01111 } 01112 } 01113 } 01114 return 0.f; 01115 } 01116 01117 01118 void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc) 01119 { 01120 int iteration; 01121 if (infoGlobal.m_splitImpulse) 01122 { 01123 if (infoGlobal.m_solverMode & SOLVER_SIMD) 01124 { 01125 for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++) 01126 { 01127 { 01128 int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); 01129 int j; 01130 for (j=0;j<numPoolConstraints;j++) 01131 { 01132 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; 01133 01134 resolveSplitPenetrationSIMD(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold); 01135 } 01136 } 01137 } 01138 } 01139 else 01140 { 01141 for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++) 01142 { 01143 { 01144 int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); 01145 int j; 01146 for (j=0;j<numPoolConstraints;j++) 01147 { 01148 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; 01149 01150 resolveSplitPenetrationImpulseCacheFriendly(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold); 01151 } 01152 } 01153 } 01154 } 01155 } 01156 } 01157 01158 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc) 01159 { 01160 BT_PROFILE("solveGroupCacheFriendlyIterations"); 01161 01162 { 01164 solveGroupCacheFriendlySplitImpulseIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc); 01165 01166 int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations; 01167 01168 for ( int iteration = 0 ; iteration< maxIterations ; iteration++) 01169 //for ( int iteration = maxIterations-1 ; iteration >= 0;iteration--) 01170 { 01171 solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc); 01172 } 01173 01174 } 01175 return 0.f; 01176 } 01177 01178 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies ,int numBodies,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** /*constraints*/,int /* numConstraints*/,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/) 01179 { 01180 int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); 01181 int i,j; 01182 01183 for (j=0;j<numPoolConstraints;j++) 01184 { 01185 01186 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j]; 01187 btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint; 01188 btAssert(pt); 01189 pt->m_appliedImpulse = solveManifold.m_appliedImpulse; 01190 if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING) 01191 { 01192 pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; 01193 pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse; 01194 } 01195 01196 //do a callback here? 01197 } 01198 01199 numPoolConstraints = m_tmpSolverNonContactConstraintPool.size(); 01200 for (j=0;j<numPoolConstraints;j++) 01201 { 01202 const btSolverConstraint& solverConstr = m_tmpSolverNonContactConstraintPool[j]; 01203 btTypedConstraint* constr = (btTypedConstraint*)solverConstr.m_originalContactPoint; 01204 constr->internalSetAppliedImpulse(solverConstr.m_appliedImpulse); 01205 if (btFabs(solverConstr.m_appliedImpulse)>=constr->getBreakingImpulseThreshold()) 01206 { 01207 constr->setEnabled(false); 01208 } 01209 } 01210 01211 01212 if (infoGlobal.m_splitImpulse) 01213 { 01214 for ( i=0;i<numBodies;i++) 01215 { 01216 btRigidBody* body = btRigidBody::upcast(bodies[i]); 01217 if (body) 01218 body->internalWritebackVelocity(infoGlobal.m_timeStep); 01219 } 01220 } else 01221 { 01222 for ( i=0;i<numBodies;i++) 01223 { 01224 btRigidBody* body = btRigidBody::upcast(bodies[i]); 01225 if (body) 01226 body->internalWritebackVelocity(); 01227 } 01228 } 01229 01230 01231 m_tmpSolverContactConstraintPool.resize(0); 01232 m_tmpSolverNonContactConstraintPool.resize(0); 01233 m_tmpSolverContactFrictionConstraintPool.resize(0); 01234 01235 return 0.f; 01236 } 01237 01238 01239 01241 btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /*dispatcher*/) 01242 { 01243 01244 BT_PROFILE("solveGroup"); 01245 //you need to provide at least some bodies 01246 btAssert(bodies); 01247 btAssert(numBodies); 01248 01249 solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc); 01250 01251 solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc); 01252 01253 solveGroupCacheFriendlyFinish(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc); 01254 01255 return 0.f; 01256 } 01257 01258 void btSequentialImpulseConstraintSolver::reset() 01259 { 01260 m_btSeed2 = 0; 01261 } 01262 01263 btRigidBody& btSequentialImpulseConstraintSolver::getFixedBody() 01264 { 01265 static btRigidBody s_fixed(0, 0,0); 01266 s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.))); 01267 return s_fixed; 01268 } 01269
1.7.3