|
Blender
V2.59
|
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_appliedImpulse = _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 00744 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc) 00745 { 00746 BT_PROFILE("solveGroupCacheFriendlySetup"); 00747 (void)stackAlloc; 00748 (void)debugDrawer; 00749 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 } 00792 } 00793 //btRigidBody* rb0=0,*rb1=0; 00794 00795 //if (1) 00796 { 00797 { 00798 00799 int totalNumRows = 0; 00800 int i; 00801 00802 m_tmpConstraintSizesPool.resize(numConstraints); 00803 //calculate the total number of contraint rows 00804 for (i=0;i<numConstraints;i++) 00805 { 00806 btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i]; 00807 constraints[i]->getInfo1(&info1); 00808 totalNumRows += info1.m_numConstraintRows; 00809 } 00810 m_tmpSolverNonContactConstraintPool.resize(totalNumRows); 00811 00812 00814 int currentRow = 0; 00815 00816 for (i=0;i<numConstraints;i++) 00817 { 00818 const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i]; 00819 00820 if (info1.m_numConstraintRows) 00821 { 00822 btAssert(currentRow<totalNumRows); 00823 00824 btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow]; 00825 btTypedConstraint* constraint = constraints[i]; 00826 00827 00828 00829 btRigidBody& rbA = constraint->getRigidBodyA(); 00830 btRigidBody& rbB = constraint->getRigidBodyB(); 00831 00832 00833 int j; 00834 for ( j=0;j<info1.m_numConstraintRows;j++) 00835 { 00836 memset(¤tConstraintRow[j],0,sizeof(btSolverConstraint)); 00837 currentConstraintRow[j].m_lowerLimit = -FLT_MAX; 00838 currentConstraintRow[j].m_upperLimit = FLT_MAX; 00839 currentConstraintRow[j].m_appliedImpulse = 0.f; 00840 currentConstraintRow[j].m_appliedPushImpulse = 0.f; 00841 currentConstraintRow[j].m_solverBodyA = &rbA; 00842 currentConstraintRow[j].m_solverBodyB = &rbB; 00843 } 00844 00845 rbA.internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); 00846 rbA.internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); 00847 rbB.internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); 00848 rbB.internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); 00849 00850 00851 00852 btTypedConstraint::btConstraintInfo2 info2; 00853 info2.fps = 1.f/infoGlobal.m_timeStep; 00854 info2.erp = infoGlobal.m_erp; 00855 info2.m_J1linearAxis = currentConstraintRow->m_contactNormal; 00856 info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal; 00857 info2.m_J2linearAxis = 0; 00858 info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal; 00859 info2.rowskip = sizeof(btSolverConstraint)/sizeof(btScalar);//check this 00861 btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint)); 00862 info2.m_constraintError = ¤tConstraintRow->m_rhs; 00863 currentConstraintRow->m_cfm = infoGlobal.m_globalCfm; 00864 info2.m_damping = infoGlobal.m_damping; 00865 info2.cfm = ¤tConstraintRow->m_cfm; 00866 info2.m_lowerLimit = ¤tConstraintRow->m_lowerLimit; 00867 info2.m_upperLimit = ¤tConstraintRow->m_upperLimit; 00868 info2.m_numIterations = infoGlobal.m_numIterations; 00869 constraints[i]->getInfo2(&info2); 00870 00872 for ( j=0;j<info1.m_numConstraintRows;j++) 00873 { 00874 btSolverConstraint& solverConstraint = currentConstraintRow[j]; 00875 solverConstraint.m_originalContactPoint = constraint; 00876 00877 { 00878 const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal; 00879 solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld()*ftorqueAxis1*constraint->getRigidBodyA().getAngularFactor(); 00880 } 00881 { 00882 const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal; 00883 solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld()*ftorqueAxis2*constraint->getRigidBodyB().getAngularFactor(); 00884 } 00885 00886 { 00887 btVector3 iMJlA = solverConstraint.m_contactNormal*rbA.getInvMass(); 00888 btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal; 00889 btVector3 iMJlB = solverConstraint.m_contactNormal*rbB.getInvMass();//sign of normal? 00890 btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal; 00891 00892 btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal); 00893 sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal); 00894 sum += iMJlB.dot(solverConstraint.m_contactNormal); 00895 sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal); 00896 00897 solverConstraint.m_jacDiagABInv = btScalar(1.)/sum; 00898 } 00899 00900 00903 { 00904 btScalar rel_vel; 00905 btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rbA.getLinearVelocity()) + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity()); 00906 btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rbB.getLinearVelocity()) + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity()); 00907 00908 rel_vel = vel1Dotn+vel2Dotn; 00909 00910 btScalar restitution = 0.f; 00911 btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2 00912 btScalar velocityError = restitution - rel_vel * info2.m_damping; 00913 btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; 00914 btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; 00915 solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; 00916 solverConstraint.m_appliedImpulse = 0.f; 00917 00918 } 00919 } 00920 } 00921 currentRow+=m_tmpConstraintSizesPool[i].m_numConstraintRows; 00922 } 00923 } 00924 00925 { 00926 int i; 00927 btPersistentManifold* manifold = 0; 00928 // btCollisionObject* colObj0=0,*colObj1=0; 00929 00930 00931 for (i=0;i<numManifolds;i++) 00932 { 00933 manifold = manifoldPtr[i]; 00934 convertContact(manifold,infoGlobal); 00935 } 00936 } 00937 } 00938 00939 btContactSolverInfo info = infoGlobal; 00940 00941 00942 00943 int numConstraintPool = m_tmpSolverContactConstraintPool.size(); 00944 int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size(); 00945 00947 m_orderTmpConstraintPool.resize(numConstraintPool); 00948 m_orderFrictionConstraintPool.resize(numFrictionPool); 00949 { 00950 int i; 00951 for (i=0;i<numConstraintPool;i++) 00952 { 00953 m_orderTmpConstraintPool[i] = i; 00954 } 00955 for (i=0;i<numFrictionPool;i++) 00956 { 00957 m_orderFrictionConstraintPool[i] = i; 00958 } 00959 } 00960 00961 return 0.f; 00962 00963 } 00964 00965 btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/) 00966 { 00967 00968 int numConstraintPool = m_tmpSolverContactConstraintPool.size(); 00969 int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size(); 00970 00971 int j; 00972 00973 if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER) 00974 { 00975 if ((iteration & 7) == 0) { 00976 for (j=0; j<numConstraintPool; ++j) { 00977 int tmp = m_orderTmpConstraintPool[j]; 00978 int swapi = btRandInt2(j+1); 00979 m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi]; 00980 m_orderTmpConstraintPool[swapi] = tmp; 00981 } 00982 00983 for (j=0; j<numFrictionPool; ++j) { 00984 int tmp = m_orderFrictionConstraintPool[j]; 00985 int swapi = btRandInt2(j+1); 00986 m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi]; 00987 m_orderFrictionConstraintPool[swapi] = tmp; 00988 } 00989 } 00990 } 00991 00992 if (infoGlobal.m_solverMode & SOLVER_SIMD) 00993 { 00995 for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) 00996 { 00997 btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j]; 00998 resolveSingleConstraintRowGenericSIMD(*constraint.m_solverBodyA,*constraint.m_solverBodyB,constraint); 00999 } 01000 01001 for (j=0;j<numConstraints;j++) 01002 { 01003 constraints[j]->solveConstraintObsolete(constraints[j]->getRigidBodyA(),constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep); 01004 } 01005 01007 int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); 01008 for (j=0;j<numPoolConstraints;j++) 01009 { 01010 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; 01011 resolveSingleConstraintRowLowerLimitSIMD(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold); 01012 01013 } 01015 int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size(); 01016 for (j=0;j<numFrictionPoolConstraints;j++) 01017 { 01018 btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]]; 01019 btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; 01020 01021 if (totalImpulse>btScalar(0)) 01022 { 01023 solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); 01024 solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; 01025 01026 resolveSingleConstraintRowGenericSIMD(*solveManifold.m_solverBodyA, *solveManifold.m_solverBodyB,solveManifold); 01027 } 01028 } 01029 } else 01030 { 01031 01033 for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) 01034 { 01035 btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j]; 01036 resolveSingleConstraintRowGeneric(*constraint.m_solverBodyA,*constraint.m_solverBodyB,constraint); 01037 } 01038 01039 for (j=0;j<numConstraints;j++) 01040 { 01041 constraints[j]->solveConstraintObsolete(constraints[j]->getRigidBodyA(),constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep); 01042 } 01044 int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); 01045 for (j=0;j<numPoolConstraints;j++) 01046 { 01047 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; 01048 resolveSingleConstraintRowLowerLimit(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold); 01049 } 01051 int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size(); 01052 for (j=0;j<numFrictionPoolConstraints;j++) 01053 { 01054 btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]]; 01055 btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; 01056 01057 if (totalImpulse>btScalar(0)) 01058 { 01059 solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); 01060 solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; 01061 01062 resolveSingleConstraintRowGeneric(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold); 01063 } 01064 } 01065 } 01066 return 0.f; 01067 } 01068 01069 01070 void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc) 01071 { 01072 int iteration; 01073 if (infoGlobal.m_splitImpulse) 01074 { 01075 if (infoGlobal.m_solverMode & SOLVER_SIMD) 01076 { 01077 for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++) 01078 { 01079 { 01080 int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); 01081 int j; 01082 for (j=0;j<numPoolConstraints;j++) 01083 { 01084 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; 01085 01086 resolveSplitPenetrationSIMD(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold); 01087 } 01088 } 01089 } 01090 } 01091 else 01092 { 01093 for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++) 01094 { 01095 { 01096 int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); 01097 int j; 01098 for (j=0;j<numPoolConstraints;j++) 01099 { 01100 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; 01101 01102 resolveSplitPenetrationImpulseCacheFriendly(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold); 01103 } 01104 } 01105 } 01106 } 01107 } 01108 } 01109 01110 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc) 01111 { 01112 BT_PROFILE("solveGroupCacheFriendlyIterations"); 01113 01114 01115 //should traverse the contacts random order... 01116 int iteration; 01117 { 01118 for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++) 01119 { 01120 solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc); 01121 } 01122 01123 solveGroupCacheFriendlySplitImpulseIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc); 01124 } 01125 return 0.f; 01126 } 01127 01128 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies ,int numBodies,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** /*constraints*/,int /* numConstraints*/,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/) 01129 { 01130 int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); 01131 int i,j; 01132 01133 for (j=0;j<numPoolConstraints;j++) 01134 { 01135 01136 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j]; 01137 btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint; 01138 btAssert(pt); 01139 pt->m_appliedImpulse = solveManifold.m_appliedImpulse; 01140 if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING) 01141 { 01142 pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; 01143 pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse; 01144 } 01145 01146 //do a callback here? 01147 } 01148 01149 numPoolConstraints = m_tmpSolverNonContactConstraintPool.size(); 01150 for (j=0;j<numPoolConstraints;j++) 01151 { 01152 const btSolverConstraint& solverConstr = m_tmpSolverNonContactConstraintPool[j]; 01153 btTypedConstraint* constr = (btTypedConstraint*)solverConstr.m_originalContactPoint; 01154 btScalar sum = constr->internalGetAppliedImpulse(); 01155 sum += solverConstr.m_appliedImpulse; 01156 constr->internalSetAppliedImpulse(sum); 01157 } 01158 01159 01160 if (infoGlobal.m_splitImpulse) 01161 { 01162 for ( i=0;i<numBodies;i++) 01163 { 01164 btRigidBody* body = btRigidBody::upcast(bodies[i]); 01165 if (body) 01166 body->internalWritebackVelocity(infoGlobal.m_timeStep); 01167 } 01168 } else 01169 { 01170 for ( i=0;i<numBodies;i++) 01171 { 01172 btRigidBody* body = btRigidBody::upcast(bodies[i]); 01173 if (body) 01174 body->internalWritebackVelocity(); 01175 } 01176 } 01177 01178 01179 m_tmpSolverContactConstraintPool.resize(0); 01180 m_tmpSolverNonContactConstraintPool.resize(0); 01181 m_tmpSolverContactFrictionConstraintPool.resize(0); 01182 01183 return 0.f; 01184 } 01185 01186 01187 01189 btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /*dispatcher*/) 01190 { 01191 01192 BT_PROFILE("solveGroup"); 01193 //you need to provide at least some bodies 01194 btAssert(bodies); 01195 btAssert(numBodies); 01196 01197 solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc); 01198 01199 solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc); 01200 01201 solveGroupCacheFriendlyFinish(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc); 01202 01203 return 0.f; 01204 } 01205 01206 void btSequentialImpulseConstraintSolver::reset() 01207 { 01208 m_btSeed2 = 0; 01209 } 01210 01211 btRigidBody& btSequentialImpulseConstraintSolver::getFixedBody() 01212 { 01213 static btRigidBody s_fixed(0, 0,0); 01214 s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.))); 01215 return s_fixed; 01216 } 01217