00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
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>
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 vec_splat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e,e,e,e))
00052 static inline __m128 _vmathVfDot3( __m128 vec0, __m128 vec1 )
00053 {
00054 __m128 result = _mm_mul_ps( vec0, vec1);
00055 return _mm_add_ps( vec_splat( result, 0 ), _mm_add_ps( vec_splat( result, 1 ), vec_splat( result, 2 ) ) );
00056 }
00057 #endif//USE_SIMD
00058
00059
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(_vmathVfDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
00068 __m128 deltaVel2Dotn = _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),_vmathVfDot3((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
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
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(_vmathVfDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
00131 __m128 deltaVel2Dotn = _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),_vmathVfDot3((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
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(_vmathVfDot3(c.m_contactNormal.mVec128,body1.internalGetPushVelocity().mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetTurnVelocity().mVec128));
00219 __m128 deltaVel2Dotn = _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetTurnVelocity().mVec128),_vmathVfDot3((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
00252 int btSequentialImpulseConstraintSolver::btRandInt2 (int n)
00253 {
00254
00255 const unsigned long un = static_cast<unsigned long>(n);
00256 unsigned long r = btRand2();
00257
00258
00259
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
00322 btVector3 loc_lateral = frictionDirection * colObj->getWorldTransform().getBasis();
00323 const btVector3& friction_scaling = colObj->getAnisotropicFriction();
00324
00325 loc_lateral *= friction_scaling;
00326
00327 frictionDirection = colObj->getWorldTransform().getBasis() * loc_lateral;
00328 }
00329 }
00330
00331
00332
00333 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)
00334 {
00335
00336
00337 btRigidBody* body0=btRigidBody::upcast(colObj0);
00338 btRigidBody* body1=btRigidBody::upcast(colObj1);
00339
00340 btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing();
00341
00342 solverConstraint.m_contactNormal = normalAxis;
00343
00344 solverConstraint.m_solverBodyA = body0 ? body0 : &getFixedBody();
00345 solverConstraint.m_solverBodyB = body1 ? body1 : &getFixedBody();
00346 solverConstraint.m_frictionIndex = frictionIndex;
00347
00348 solverConstraint.m_friction = cp.m_combinedFriction;
00349 solverConstraint.m_originalContactPoint = 0;
00350
00351 solverConstraint.m_appliedImpulse = 0.f;
00352 solverConstraint.m_appliedPushImpulse = 0.f;
00353
00354 {
00355 btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal);
00356 solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
00357 solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor() : btVector3(0,0,0);
00358 }
00359 {
00360 btVector3 ftorqueAxis1 = rel_pos2.cross(-solverConstraint.m_contactNormal);
00361 solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
00362 solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor() : btVector3(0,0,0);
00363 }
00364
00365 #ifdef COMPUTE_IMPULSE_DENOM
00366 btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal);
00367 btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal);
00368 #else
00369 btVector3 vec;
00370 btScalar denom0 = 0.f;
00371 btScalar denom1 = 0.f;
00372 if (body0)
00373 {
00374 vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
00375 denom0 = body0->getInvMass() + normalAxis.dot(vec);
00376 }
00377 if (body1)
00378 {
00379 vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
00380 denom1 = body1->getInvMass() + normalAxis.dot(vec);
00381 }
00382
00383
00384 #endif //COMPUTE_IMPULSE_DENOM
00385 btScalar denom = relaxation/(denom0+denom1);
00386 solverConstraint.m_jacDiagABInv = denom;
00387
00388 #ifdef _USE_JACOBIAN
00389 solverConstraint.m_jac = btJacobianEntry (
00390 rel_pos1,rel_pos2,solverConstraint.m_contactNormal,
00391 body0->getInvInertiaDiagLocal(),
00392 body0->getInvMass(),
00393 body1->getInvInertiaDiagLocal(),
00394 body1->getInvMass());
00395 #endif //_USE_JACOBIAN
00396
00397
00398 {
00399 btScalar rel_vel;
00400 btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0?body0->getLinearVelocity():btVector3(0,0,0))
00401 + solverConstraint.m_relpos1CrossNormal.dot(body0?body0->getAngularVelocity():btVector3(0,0,0));
00402 btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(body1?body1->getLinearVelocity():btVector3(0,0,0))
00403 + solverConstraint.m_relpos2CrossNormal.dot(body1?body1->getAngularVelocity():btVector3(0,0,0));
00404
00405 rel_vel = vel1Dotn+vel2Dotn;
00406
00407
00408
00409 btSimdScalar velocityError = desiredVelocity - rel_vel;
00410 btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
00411 solverConstraint.m_rhs = velocityImpulse;
00412 solverConstraint.m_cfm = cfmSlip;
00413 solverConstraint.m_lowerLimit = 0;
00414 solverConstraint.m_upperLimit = 1e10f;
00415 }
00416
00417 return solverConstraint;
00418 }
00419
00420 int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body)
00421 {
00422 #if 0
00423 int solverBodyIdA = -1;
00424
00425 if (body.getCompanionId() >= 0)
00426 {
00427
00428 solverBodyIdA = body.getCompanionId();
00429 } else
00430 {
00431 btRigidBody* rb = btRigidBody::upcast(&body);
00432 if (rb && rb->getInvMass())
00433 {
00434 solverBodyIdA = m_tmpSolverBodyPool.size();
00435 btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
00436 initSolverBody(&solverBody,&body);
00437 body.setCompanionId(solverBodyIdA);
00438 } else
00439 {
00440 return 0;
00441 }
00442 }
00443 return solverBodyIdA;
00444 #endif
00445 return 0;
00446 }
00447 #include <stdio.h>
00448
00449
00450
00451 void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal)
00452 {
00453 btCollisionObject* colObj0=0,*colObj1=0;
00454
00455 colObj0 = (btCollisionObject*)manifold->getBody0();
00456 colObj1 = (btCollisionObject*)manifold->getBody1();
00457
00458
00459 btRigidBody* solverBodyA = btRigidBody::upcast(colObj0);
00460 btRigidBody* solverBodyB = btRigidBody::upcast(colObj1);
00461
00462
00464 if (!solverBodyA && !solverBodyB)
00465 return;
00466
00467 btVector3 rel_pos1;
00468 btVector3 rel_pos2;
00469 btScalar relaxation;
00470
00471 for (int j=0;j<manifold->getNumContacts();j++)
00472 {
00473
00474 btManifoldPoint& cp = manifold->getContactPoint(j);
00475
00476 if (cp.getDistance() <= manifold->getContactProcessingThreshold())
00477 {
00478
00479 const btVector3& pos1 = cp.getPositionWorldOnA();
00480 const btVector3& pos2 = cp.getPositionWorldOnB();
00481
00482 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
00483 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
00484
00485
00486 relaxation = 1.f;
00487 btScalar rel_vel;
00488 btVector3 vel;
00489
00490 int frictionIndex = m_tmpSolverContactConstraintPool.size();
00491
00492 {
00493 btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing();
00494 btRigidBody* rb0 = btRigidBody::upcast(colObj0);
00495 btRigidBody* rb1 = btRigidBody::upcast(colObj1);
00496
00497 solverConstraint.m_solverBodyA = rb0? rb0 : &getFixedBody();
00498 solverConstraint.m_solverBodyB = rb1? rb1 : &getFixedBody();
00499
00500 solverConstraint.m_originalContactPoint = &cp;
00501
00502 btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
00503 solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
00504 btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);
00505 solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
00506 {
00507 #ifdef COMPUTE_IMPULSE_DENOM
00508 btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
00509 btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
00510 #else
00511 btVector3 vec;
00512 btScalar denom0 = 0.f;
00513 btScalar denom1 = 0.f;
00514 if (rb0)
00515 {
00516 vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
00517 denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
00518 }
00519 if (rb1)
00520 {
00521 vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
00522 denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
00523 }
00524 #endif //COMPUTE_IMPULSE_DENOM
00525
00526 btScalar denom = relaxation/(denom0+denom1);
00527 solverConstraint.m_jacDiagABInv = denom;
00528 }
00529
00530 solverConstraint.m_contactNormal = cp.m_normalWorldOnB;
00531 solverConstraint.m_relpos1CrossNormal = rel_pos1.cross(cp.m_normalWorldOnB);
00532 solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(-cp.m_normalWorldOnB);
00533
00534
00535 btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
00536 btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
00537
00538 vel = vel1 - vel2;
00539
00540 rel_vel = cp.m_normalWorldOnB.dot(vel);
00541
00542 btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop;
00543
00544
00545 solverConstraint.m_friction = cp.m_combinedFriction;
00546
00547 btScalar restitution = 0.f;
00548
00549 if (cp.m_lifeTime>infoGlobal.m_restingContactRestitutionThreshold)
00550 {
00551 restitution = 0.f;
00552 } else
00553 {
00554 restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution);
00555 if (restitution <= btScalar(0.))
00556 {
00557 restitution = 0.f;
00558 };
00559 }
00560
00561
00563 if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
00564 {
00565 solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
00566 if (rb0)
00567 rb0->internalApplyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
00568 if (rb1)
00569 rb1->internalApplyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-solverConstraint.m_appliedImpulse);
00570 } else
00571 {
00572 solverConstraint.m_appliedImpulse = 0.f;
00573 }
00574
00575 solverConstraint.m_appliedPushImpulse = 0.f;
00576
00577 {
00578 btScalar rel_vel;
00579 btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rb0?rb0->getLinearVelocity():btVector3(0,0,0))
00580 + solverConstraint.m_relpos1CrossNormal.dot(rb0?rb0->getAngularVelocity():btVector3(0,0,0));
00581 btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rb1?rb1->getLinearVelocity():btVector3(0,0,0))
00582 + solverConstraint.m_relpos2CrossNormal.dot(rb1?rb1->getAngularVelocity():btVector3(0,0,0));
00583
00584 rel_vel = vel1Dotn+vel2Dotn;
00585
00586 btScalar positionalError = 0.f;
00587 positionalError = -penetration * infoGlobal.m_erp/infoGlobal.m_timeStep;
00588 btScalar velocityError = restitution - rel_vel;
00589 btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
00590 btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
00591 if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
00592 {
00593
00594 solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
00595 solverConstraint.m_rhsPenetration = 0.f;
00596 } else
00597 {
00598
00599 solverConstraint.m_rhs = velocityImpulse;
00600 solverConstraint.m_rhsPenetration = penetrationImpulse;
00601 }
00602 solverConstraint.m_cfm = 0.f;
00603 solverConstraint.m_lowerLimit = 0;
00604 solverConstraint.m_upperLimit = 1e10f;
00605 }
00606
00607
00609
00610
00611
00612 if (1)
00613 {
00614 solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
00615 if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
00616 {
00617 cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
00618 btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
00619 if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
00620 {
00621 cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel);
00622 if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
00623 {
00624 cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
00625 cp.m_lateralFrictionDir2.normalize();
00626 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
00627 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
00628 addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
00629 }
00630
00631 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
00632 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
00633 addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
00634 cp.m_lateralFrictionInitialized = true;
00635 } else
00636 {
00637
00638 btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
00639 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
00640 {
00641 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
00642 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
00643 addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
00644 }
00645
00646 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
00647 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
00648 addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
00649
00650 cp.m_lateralFrictionInitialized = true;
00651 }
00652
00653 } else
00654 {
00655 addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_contactCFM1);
00656 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
00657 addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2);
00658 }
00659
00660 if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
00661 {
00662 {
00663 btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
00664 if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
00665 {
00666 frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
00667 if (rb0)
00668 rb0->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
00669 if (rb1)
00670 rb1->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse);
00671 } else
00672 {
00673 frictionConstraint1.m_appliedImpulse = 0.f;
00674 }
00675 }
00676
00677 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
00678 {
00679 btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
00680 if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
00681 {
00682 frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
00683 if (rb0)
00684 rb0->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
00685 if (rb1)
00686 rb1->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse);
00687 } else
00688 {
00689 frictionConstraint2.m_appliedImpulse = 0.f;
00690 }
00691 }
00692 } else
00693 {
00694 btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
00695 frictionConstraint1.m_appliedImpulse = 0.f;
00696 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
00697 {
00698 btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
00699 frictionConstraint2.m_appliedImpulse = 0.f;
00700 }
00701 }
00702 }
00703 }
00704
00705
00706 }
00707 }
00708 }
00709
00710
00711 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** ,int ,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
00712 {
00713 BT_PROFILE("solveGroupCacheFriendlySetup");
00714 (void)stackAlloc;
00715 (void)debugDrawer;
00716
00717
00718 if (!(numConstraints + numManifolds))
00719 {
00720
00721 return 0.f;
00722 }
00723
00724 if (1)
00725 {
00726 int j;
00727 for (j=0;j<numConstraints;j++)
00728 {
00729 btTypedConstraint* constraint = constraints[j];
00730 constraint->buildJacobian();
00731 }
00732 }
00733
00734
00735
00736 {
00737 {
00738
00739 int totalNumRows = 0;
00740 int i;
00741
00742 m_tmpConstraintSizesPool.resize(numConstraints);
00743
00744 for (i=0;i<numConstraints;i++)
00745 {
00746 btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
00747 constraints[i]->getInfo1(&info1);
00748 totalNumRows += info1.m_numConstraintRows;
00749 }
00750 m_tmpSolverNonContactConstraintPool.resize(totalNumRows);
00751
00752
00754 int currentRow = 0;
00755
00756 for (i=0;i<numConstraints;i++)
00757 {
00758 const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
00759
00760 if (info1.m_numConstraintRows)
00761 {
00762 btAssert(currentRow<totalNumRows);
00763
00764 btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];
00765 btTypedConstraint* constraint = constraints[i];
00766
00767
00768
00769 btRigidBody& rbA = constraint->getRigidBodyA();
00770 btRigidBody& rbB = constraint->getRigidBodyB();
00771
00772
00773 int j;
00774 for ( j=0;j<info1.m_numConstraintRows;j++)
00775 {
00776 memset(¤tConstraintRow[j],0,sizeof(btSolverConstraint));
00777 currentConstraintRow[j].m_lowerLimit = -FLT_MAX;
00778 currentConstraintRow[j].m_upperLimit = FLT_MAX;
00779 currentConstraintRow[j].m_appliedImpulse = 0.f;
00780 currentConstraintRow[j].m_appliedPushImpulse = 0.f;
00781 currentConstraintRow[j].m_solverBodyA = &rbA;
00782 currentConstraintRow[j].m_solverBodyB = &rbB;
00783 }
00784
00785 rbA.internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
00786 rbA.internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
00787 rbB.internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
00788 rbB.internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
00789
00790
00791
00792 btTypedConstraint::btConstraintInfo2 info2;
00793 info2.fps = 1.f/infoGlobal.m_timeStep;
00794 info2.erp = infoGlobal.m_erp;
00795 info2.m_J1linearAxis = currentConstraintRow->m_contactNormal;
00796 info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal;
00797 info2.m_J2linearAxis = 0;
00798 info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal;
00799 info2.rowskip = sizeof(btSolverConstraint)/sizeof(btScalar);
00801 btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint));
00802 info2.m_constraintError = ¤tConstraintRow->m_rhs;
00803 currentConstraintRow->m_cfm = infoGlobal.m_globalCfm;
00804 info2.cfm = ¤tConstraintRow->m_cfm;
00805 info2.m_lowerLimit = ¤tConstraintRow->m_lowerLimit;
00806 info2.m_upperLimit = ¤tConstraintRow->m_upperLimit;
00807 info2.m_numIterations = infoGlobal.m_numIterations;
00808 constraints[i]->getInfo2(&info2);
00809
00811 for ( j=0;j<info1.m_numConstraintRows;j++)
00812 {
00813 btSolverConstraint& solverConstraint = currentConstraintRow[j];
00814 solverConstraint.m_originalContactPoint = constraint;
00815
00816 {
00817 const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
00818 solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld()*ftorqueAxis1*constraint->getRigidBodyA().getAngularFactor();
00819 }
00820 {
00821 const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal;
00822 solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld()*ftorqueAxis2*constraint->getRigidBodyB().getAngularFactor();
00823 }
00824
00825 {
00826 btVector3 iMJlA = solverConstraint.m_contactNormal*rbA.getInvMass();
00827 btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal;
00828 btVector3 iMJlB = solverConstraint.m_contactNormal*rbB.getInvMass();
00829 btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal;
00830
00831 btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal);
00832 sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
00833 sum += iMJlB.dot(solverConstraint.m_contactNormal);
00834 sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
00835
00836 solverConstraint.m_jacDiagABInv = btScalar(1.)/sum;
00837 }
00838
00839
00842 {
00843 btScalar rel_vel;
00844 btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rbA.getLinearVelocity()) + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity());
00845 btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rbB.getLinearVelocity()) + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity());
00846
00847 rel_vel = vel1Dotn+vel2Dotn;
00848
00849 btScalar restitution = 0.f;
00850 btScalar positionalError = solverConstraint.m_rhs;
00851 btScalar velocityError = restitution - rel_vel;
00852 btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
00853 btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
00854 solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
00855 solverConstraint.m_appliedImpulse = 0.f;
00856
00857 }
00858 }
00859 }
00860 currentRow+=m_tmpConstraintSizesPool[i].m_numConstraintRows;
00861 }
00862 }
00863
00864 {
00865 int i;
00866 btPersistentManifold* manifold = 0;
00867
00868
00869
00870 for (i=0;i<numManifolds;i++)
00871 {
00872 manifold = manifoldPtr[i];
00873 convertContact(manifold,infoGlobal);
00874 }
00875 }
00876 }
00877
00878 btContactSolverInfo info = infoGlobal;
00879
00880
00881
00882 int numConstraintPool = m_tmpSolverContactConstraintPool.size();
00883 int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
00884
00886 m_orderTmpConstraintPool.resize(numConstraintPool);
00887 m_orderFrictionConstraintPool.resize(numFrictionPool);
00888 {
00889 int i;
00890 for (i=0;i<numConstraintPool;i++)
00891 {
00892 m_orderTmpConstraintPool[i] = i;
00893 }
00894 for (i=0;i<numFrictionPool;i++)
00895 {
00896 m_orderFrictionConstraintPool[i] = i;
00897 }
00898 }
00899
00900 return 0.f;
00901
00902 }
00903
00904 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** ,int ,btPersistentManifold** , int ,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* ,btStackAlloc* )
00905 {
00906 BT_PROFILE("solveGroupCacheFriendlyIterations");
00907
00908 int numConstraintPool = m_tmpSolverContactConstraintPool.size();
00909 int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
00910
00911
00912 int iteration;
00913 {
00914 for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
00915 {
00916
00917 int j;
00918 if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
00919 {
00920 if ((iteration & 7) == 0) {
00921 for (j=0; j<numConstraintPool; ++j) {
00922 int tmp = m_orderTmpConstraintPool[j];
00923 int swapi = btRandInt2(j+1);
00924 m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
00925 m_orderTmpConstraintPool[swapi] = tmp;
00926 }
00927
00928 for (j=0; j<numFrictionPool; ++j) {
00929 int tmp = m_orderFrictionConstraintPool[j];
00930 int swapi = btRandInt2(j+1);
00931 m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
00932 m_orderFrictionConstraintPool[swapi] = tmp;
00933 }
00934 }
00935 }
00936
00937 if (infoGlobal.m_solverMode & SOLVER_SIMD)
00938 {
00940 for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
00941 {
00942 btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
00943 resolveSingleConstraintRowGenericSIMD(*constraint.m_solverBodyA,*constraint.m_solverBodyB,constraint);
00944 }
00945
00946 for (j=0;j<numConstraints;j++)
00947 {
00948 constraints[j]->solveConstraintObsolete(constraints[j]->getRigidBodyA(),constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
00949 }
00950
00952 int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
00953 for (j=0;j<numPoolConstraints;j++)
00954 {
00955 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
00956 resolveSingleConstraintRowLowerLimitSIMD(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
00957
00958 }
00960 int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
00961 for (j=0;j<numFrictionPoolConstraints;j++)
00962 {
00963 btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
00964 btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
00965
00966 if (totalImpulse>btScalar(0))
00967 {
00968 solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
00969 solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
00970
00971 resolveSingleConstraintRowGenericSIMD(*solveManifold.m_solverBodyA, *solveManifold.m_solverBodyB,solveManifold);
00972 }
00973 }
00974 } else
00975 {
00976
00978 for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
00979 {
00980 btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
00981 resolveSingleConstraintRowGeneric(*constraint.m_solverBodyA,*constraint.m_solverBodyB,constraint);
00982 }
00983
00984 for (j=0;j<numConstraints;j++)
00985 {
00986 constraints[j]->solveConstraintObsolete(constraints[j]->getRigidBodyA(),constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
00987 }
00989 int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
00990 for (j=0;j<numPoolConstraints;j++)
00991 {
00992 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
00993 resolveSingleConstraintRowLowerLimit(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
00994 }
00996 int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
00997 for (j=0;j<numFrictionPoolConstraints;j++)
00998 {
00999 btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
01000 btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
01001
01002 if (totalImpulse>btScalar(0))
01003 {
01004 solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
01005 solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
01006
01007 resolveSingleConstraintRowGeneric(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
01008 }
01009 }
01010 }
01011
01012 }
01013
01014 if (infoGlobal.m_splitImpulse)
01015 {
01016 if (infoGlobal.m_solverMode & SOLVER_SIMD)
01017 {
01018 for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
01019 {
01020 {
01021 int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
01022 int j;
01023 for (j=0;j<numPoolConstraints;j++)
01024 {
01025 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
01026
01027 resolveSplitPenetrationSIMD(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
01028 }
01029 }
01030 }
01031 }
01032 else
01033 {
01034 for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
01035 {
01036 {
01037 int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
01038 int j;
01039 for (j=0;j<numPoolConstraints;j++)
01040 {
01041 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
01042
01043 resolveSplitPenetrationImpulseCacheFriendly(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
01044 }
01045 }
01046 }
01047 }
01048 }
01049
01050 }
01051 return 0.f;
01052 }
01053
01054
01055
01057 btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* )
01058 {
01059
01060
01061
01062 BT_PROFILE("solveGroup");
01063
01064
01065 btAssert(bodies);
01066 btAssert(numBodies);
01067
01068 int i;
01069
01070 solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
01071 solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
01072
01073 int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
01074 int j;
01075
01076 for (j=0;j<numPoolConstraints;j++)
01077 {
01078
01079 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j];
01080 btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
01081 btAssert(pt);
01082 pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
01083 if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
01084 {
01085 pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
01086 pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
01087 }
01088
01089
01090 }
01091
01092 numPoolConstraints = m_tmpSolverNonContactConstraintPool.size();
01093 for (j=0;j<numPoolConstraints;j++)
01094 {
01095 const btSolverConstraint& solverConstr = m_tmpSolverNonContactConstraintPool[j];
01096 btTypedConstraint* constr = (btTypedConstraint*)solverConstr.m_originalContactPoint;
01097 btScalar sum = constr->internalGetAppliedImpulse();
01098 sum += solverConstr.m_appliedImpulse;
01099 constr->internalSetAppliedImpulse(sum);
01100 }
01101
01102
01103 if (infoGlobal.m_splitImpulse)
01104 {
01105 for ( i=0;i<numBodies;i++)
01106 {
01107 btRigidBody* body = btRigidBody::upcast(bodies[i]);
01108 if (body)
01109 body->internalWritebackVelocity(infoGlobal.m_timeStep);
01110 }
01111 } else
01112 {
01113 for ( i=0;i<numBodies;i++)
01114 {
01115 btRigidBody* body = btRigidBody::upcast(bodies[i]);
01116 if (body)
01117 body->internalWritebackVelocity();
01118 }
01119 }
01120
01121
01122 m_tmpSolverContactConstraintPool.resize(0);
01123 m_tmpSolverNonContactConstraintPool.resize(0);
01124 m_tmpSolverContactFrictionConstraintPool.resize(0);
01125
01126 return 0.f;
01127 }
01128
01129
01130
01131
01132
01133
01134
01135
01136
01137 void btSequentialImpulseConstraintSolver::reset()
01138 {
01139 m_btSeed2 = 0;
01140 }
01141
01142
