1 /* 2 Bullet Continuous Collision Detection and Physics Library 3 Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ 4 5 This software is provided 'as-is', without any express or implied warranty. 6 In no event will the authors be held liable for any damages arising from the use of this software. 7 Permission is granted to anyone to use this software for any purpose, 8 including commercial applications, and to alter it and redistribute it freely, 9 subject to the following restrictions: 10 11 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. 12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 13 3. This notice may not be removed or altered from any source distribution. 14 */ 15 16 //#define COMPUTE_IMPULSE_DENOM 1 17 //#define BT_ADDITIONAL_DEBUG 18 19 //It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms. 20 21 #include "btSequentialImpulseConstraintSolver.h" 22 #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" 23 24 #include "LinearMath/btIDebugDraw.h" 25 #include "LinearMath/btCpuFeatureUtility.h" 26 27 //#include "btJacobianEntry.h" 28 #include "LinearMath/btMinMax.h" 29 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" 30 #include <new> 31 #include "LinearMath/btStackAlloc.h" 32 #include "LinearMath/btQuickprof.h" 33 //#include "btSolverBody.h" 34 //#include "btSolverConstraint.h" 35 #include "LinearMath/btAlignedObjectArray.h" 36 #include <string.h> //for memset 37 38 int gNumSplitImpulseRecoveries = 0; 39 40 #include "BulletDynamics/Dynamics/btRigidBody.h" 41 42 43 ///This is the scalar reference implementation of solving a single constraint row, the innerloop of the Projected Gauss Seidel/Sequential Impulse constraint solver 44 ///Below are optional SSE2 and SSE4/FMA3 versions. We assume most hardware has SSE2. For SSE4/FMA3 we perform a CPU feature check. 45 static btSimdScalar gResolveSingleConstraintRowGeneric_scalar_reference(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c) 46 { 47 btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm; 48 const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity()); 49 const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity()); 50 51 // const btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn; 52 deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; 53 deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; 54 55 const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse; 56 if (sum < c.m_lowerLimit) 57 { 58 deltaImpulse = c.m_lowerLimit - c.m_appliedImpulse; 59 c.m_appliedImpulse = c.m_lowerLimit; 60 } 61 else if (sum > c.m_upperLimit) 62 { 63 deltaImpulse = c.m_upperLimit - c.m_appliedImpulse; 64 c.m_appliedImpulse = c.m_upperLimit; 65 } 66 else 67 { 68 c.m_appliedImpulse = sum; 69 } 70 71 body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(), c.m_angularComponentA, deltaImpulse); 72 body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(), c.m_angularComponentB, deltaImpulse); 73 74 return deltaImpulse; 75 } 76 77 78 static btSimdScalar gResolveSingleConstraintRowLowerLimit_scalar_reference(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c) 79 { 80 btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm; 81 const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity()); 82 const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity()); 83 84 deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; 85 deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; 86 const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse; 87 if (sum < c.m_lowerLimit) 88 { 89 deltaImpulse = c.m_lowerLimit - c.m_appliedImpulse; 90 c.m_appliedImpulse = c.m_lowerLimit; 91 } 92 else 93 { 94 c.m_appliedImpulse = sum; 95 } 96 body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(), c.m_angularComponentA, deltaImpulse); 97 body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(), c.m_angularComponentB, deltaImpulse); 98 99 return deltaImpulse; 100 } 101 102 103 104 #ifdef USE_SIMD 105 #include <emmintrin.h> 106 107 108 #define btVecSplat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e,e,e,e)) 109 static inline __m128 btSimdDot3( __m128 vec0, __m128 vec1 ) 110 { 111 __m128 result = _mm_mul_ps( vec0, vec1); 112 return _mm_add_ps( btVecSplat( result, 0 ), _mm_add_ps( btVecSplat( result, 1 ), btVecSplat( result, 2 ) ) ); 113 } 114 115 #if defined (BT_ALLOW_SSE4) 116 #include <intrin.h> 117 118 #define USE_FMA 1 119 #define USE_FMA3_INSTEAD_FMA4 1 120 #define USE_SSE4_DOT 1 121 122 #define SSE4_DP(a, b) _mm_dp_ps(a, b, 0x7f) 123 #define SSE4_DP_FP(a, b) _mm_cvtss_f32(_mm_dp_ps(a, b, 0x7f)) 124 125 #if USE_SSE4_DOT 126 #define DOT_PRODUCT(a, b) SSE4_DP(a, b) 127 #else 128 #define DOT_PRODUCT(a, b) btSimdDot3(a, b) 129 #endif 130 131 #if USE_FMA 132 #if USE_FMA3_INSTEAD_FMA4 133 // a*b + c 134 #define FMADD(a, b, c) _mm_fmadd_ps(a, b, c) 135 // -(a*b) + c 136 #define FMNADD(a, b, c) _mm_fnmadd_ps(a, b, c) 137 #else // USE_FMA3 138 // a*b + c 139 #define FMADD(a, b, c) _mm_macc_ps(a, b, c) 140 // -(a*b) + c 141 #define FMNADD(a, b, c) _mm_nmacc_ps(a, b, c) 142 #endif 143 #else // USE_FMA 144 // c + a*b 145 #define FMADD(a, b, c) _mm_add_ps(c, _mm_mul_ps(a, b)) 146 // c - a*b 147 #define FMNADD(a, b, c) _mm_sub_ps(c, _mm_mul_ps(a, b)) 148 #endif 149 #endif 150 151 // Project Gauss Seidel or the equivalent Sequential Impulse 152 static btSimdScalar gResolveSingleConstraintRowGeneric_sse2(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c) 153 { 154 __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse); 155 __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); 156 __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); 157 btSimdScalar 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))); 158 __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128, body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128, body1.internalGetDeltaAngularVelocity().mVec128)); 159 __m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128, body2.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128, body2.internalGetDeltaAngularVelocity().mVec128)); 160 deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel1Dotn, _mm_set1_ps(c.m_jacDiagABInv))); 161 deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel2Dotn, _mm_set1_ps(c.m_jacDiagABInv))); 162 btSimdScalar sum = _mm_add_ps(cpAppliedImp, deltaImpulse); 163 btSimdScalar resultLowerLess, resultUpperLess; 164 resultLowerLess = _mm_cmplt_ps(sum, lowerLimit1); 165 resultUpperLess = _mm_cmplt_ps(sum, upperLimit1); 166 __m128 lowMinApplied = _mm_sub_ps(lowerLimit1, cpAppliedImp); 167 deltaImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse)); 168 c.m_appliedImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum)); 169 __m128 upperMinApplied = _mm_sub_ps(upperLimit1, cpAppliedImp); 170 deltaImpulse = _mm_or_ps(_mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied)); 171 c.m_appliedImpulse = _mm_or_ps(_mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1)); 172 __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128, body1.internalGetInvMass().mVec128); 173 __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal2).mVec128, body2.internalGetInvMass().mVec128); 174 __m128 impulseMagnitude = deltaImpulse; 175 body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude)); 176 body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude)); 177 body2.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude)); 178 body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude)); 179 return deltaImpulse; 180 } 181 182 183 // Enhanced version of gResolveSingleConstraintRowGeneric_sse2 with SSE4.1 and FMA3 184 static btSimdScalar gResolveSingleConstraintRowGeneric_sse4_1_fma3(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c) 185 { 186 #if defined (BT_ALLOW_SSE4) 187 __m128 tmp = _mm_set_ps1(c.m_jacDiagABInv); 188 __m128 deltaImpulse = _mm_set_ps1(c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm); 189 const __m128 lowerLimit = _mm_set_ps1(c.m_lowerLimit); 190 const __m128 upperLimit = _mm_set_ps1(c.m_upperLimit); 191 const __m128 deltaVel1Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal1.mVec128, body1.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos1CrossNormal.mVec128, body1.internalGetDeltaAngularVelocity().mVec128)); 192 const __m128 deltaVel2Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal2.mVec128, body2.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos2CrossNormal.mVec128, body2.internalGetDeltaAngularVelocity().mVec128)); 193 deltaImpulse = FMNADD(deltaVel1Dotn, tmp, deltaImpulse); 194 deltaImpulse = FMNADD(deltaVel2Dotn, tmp, deltaImpulse); 195 tmp = _mm_add_ps(c.m_appliedImpulse, deltaImpulse); // sum 196 const __m128 maskLower = _mm_cmpgt_ps(tmp, lowerLimit); 197 const __m128 maskUpper = _mm_cmpgt_ps(upperLimit, tmp); 198 deltaImpulse = _mm_blendv_ps(_mm_sub_ps(lowerLimit, c.m_appliedImpulse), _mm_blendv_ps(_mm_sub_ps(upperLimit, c.m_appliedImpulse), deltaImpulse, maskUpper), maskLower); 199 c.m_appliedImpulse = _mm_blendv_ps(lowerLimit, _mm_blendv_ps(upperLimit, tmp, maskUpper), maskLower); 200 body1.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal1.mVec128, body1.internalGetInvMass().mVec128), deltaImpulse, body1.internalGetDeltaLinearVelocity().mVec128); 201 body1.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentA.mVec128, deltaImpulse, body1.internalGetDeltaAngularVelocity().mVec128); 202 body2.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal2.mVec128, body2.internalGetInvMass().mVec128), deltaImpulse, body2.internalGetDeltaLinearVelocity().mVec128); 203 body2.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentB.mVec128, deltaImpulse, body2.internalGetDeltaAngularVelocity().mVec128); 204 return deltaImpulse; 205 #else 206 return gResolveSingleConstraintRowGeneric_sse2(body1,body2,c); 207 #endif 208 } 209 210 211 212 static btSimdScalar gResolveSingleConstraintRowLowerLimit_sse2(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c) 213 { 214 __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse); 215 __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); 216 __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); 217 btSimdScalar 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))); 218 __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128, body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128, body1.internalGetDeltaAngularVelocity().mVec128)); 219 __m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128, body2.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128, body2.internalGetDeltaAngularVelocity().mVec128)); 220 deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel1Dotn, _mm_set1_ps(c.m_jacDiagABInv))); 221 deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel2Dotn, _mm_set1_ps(c.m_jacDiagABInv))); 222 btSimdScalar sum = _mm_add_ps(cpAppliedImp, deltaImpulse); 223 btSimdScalar resultLowerLess, resultUpperLess; 224 resultLowerLess = _mm_cmplt_ps(sum, lowerLimit1); 225 resultUpperLess = _mm_cmplt_ps(sum, upperLimit1); 226 __m128 lowMinApplied = _mm_sub_ps(lowerLimit1, cpAppliedImp); 227 deltaImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse)); 228 c.m_appliedImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum)); 229 __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128, body1.internalGetInvMass().mVec128); 230 __m128 linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128, body2.internalGetInvMass().mVec128); 231 __m128 impulseMagnitude = deltaImpulse; 232 body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude)); 233 body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude)); 234 body2.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude)); 235 body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude)); 236 return deltaImpulse; 237 } 238 239 240 // Enhanced version of gResolveSingleConstraintRowGeneric_sse2 with SSE4.1 and FMA3 241 static btSimdScalar gResolveSingleConstraintRowLowerLimit_sse4_1_fma3(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c) 242 { 243 #ifdef BT_ALLOW_SSE4 244 __m128 tmp = _mm_set_ps1(c.m_jacDiagABInv); 245 __m128 deltaImpulse = _mm_set_ps1(c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm); 246 const __m128 lowerLimit = _mm_set_ps1(c.m_lowerLimit); 247 const __m128 deltaVel1Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal1.mVec128, body1.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos1CrossNormal.mVec128, body1.internalGetDeltaAngularVelocity().mVec128)); 248 const __m128 deltaVel2Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal2.mVec128, body2.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos2CrossNormal.mVec128, body2.internalGetDeltaAngularVelocity().mVec128)); 249 deltaImpulse = FMNADD(deltaVel1Dotn, tmp, deltaImpulse); 250 deltaImpulse = FMNADD(deltaVel2Dotn, tmp, deltaImpulse); 251 tmp = _mm_add_ps(c.m_appliedImpulse, deltaImpulse); 252 const __m128 mask = _mm_cmpgt_ps(tmp, lowerLimit); 253 deltaImpulse = _mm_blendv_ps(_mm_sub_ps(lowerLimit, c.m_appliedImpulse), deltaImpulse, mask); 254 c.m_appliedImpulse = _mm_blendv_ps(lowerLimit, tmp, mask); 255 body1.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal1.mVec128, body1.internalGetInvMass().mVec128), deltaImpulse, body1.internalGetDeltaLinearVelocity().mVec128); 256 body1.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentA.mVec128, deltaImpulse, body1.internalGetDeltaAngularVelocity().mVec128); 257 body2.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal2.mVec128, body2.internalGetInvMass().mVec128), deltaImpulse, body2.internalGetDeltaLinearVelocity().mVec128); 258 body2.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentB.mVec128, deltaImpulse, body2.internalGetDeltaAngularVelocity().mVec128); 259 return deltaImpulse; 260 #else 261 return gResolveSingleConstraintRowLowerLimit_sse2(body1,body2,c); 262 #endif //BT_ALLOW_SSE4 263 } 264 265 266 #endif //USE_SIMD 267 268 269 270 btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) 271 { 272 #ifdef USE_SIMD 273 return m_resolveSingleConstraintRowGeneric(body1, body2, c); 274 #else 275 return resolveSingleConstraintRowGeneric(body1,body2,c); 276 #endif 277 } 278 279 // Project Gauss Seidel or the equivalent Sequential Impulse 280 btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) 281 { 282 return gResolveSingleConstraintRowGeneric_scalar_reference(body1, body2, c); 283 } 284 285 btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) 286 { 287 #ifdef USE_SIMD 288 return m_resolveSingleConstraintRowLowerLimit(body1, body2, c); 289 #else 290 return resolveSingleConstraintRowLowerLimit(body1,body2,c); 291 #endif 292 } 293 294 295 btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) 296 { 297 return gResolveSingleConstraintRowLowerLimit_scalar_reference(body1,body2,c); 298 } 299 300 301 void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFriendly( 302 btSolverBody& body1, 303 btSolverBody& body2, 304 const btSolverConstraint& c) 305 { 306 if (c.m_rhsPenetration) 307 { 308 gNumSplitImpulseRecoveries++; 309 btScalar deltaImpulse = c.m_rhsPenetration-btScalar(c.m_appliedPushImpulse)*c.m_cfm; 310 const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetPushVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetTurnVelocity()); 311 const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(body2.internalGetPushVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetTurnVelocity()); 312 313 deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; 314 deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; 315 const btScalar sum = btScalar(c.m_appliedPushImpulse) + deltaImpulse; 316 if (sum < c.m_lowerLimit) 317 { 318 deltaImpulse = c.m_lowerLimit-c.m_appliedPushImpulse; 319 c.m_appliedPushImpulse = c.m_lowerLimit; 320 } 321 else 322 { 323 c.m_appliedPushImpulse = sum; 324 } 325 body1.internalApplyPushImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); 326 body2.internalApplyPushImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); 327 } 328 } 329 330 void btSequentialImpulseConstraintSolver::resolveSplitPenetrationSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) 331 { 332 #ifdef USE_SIMD 333 if (!c.m_rhsPenetration) 334 return; 335 336 gNumSplitImpulseRecoveries++; 337 338 __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedPushImpulse); 339 __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); 340 __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); 341 __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))); 342 __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128,body1.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetTurnVelocity().mVec128)); 343 __m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128,body2.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetTurnVelocity().mVec128)); 344 deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv))); 345 deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv))); 346 btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse); 347 btSimdScalar resultLowerLess,resultUpperLess; 348 resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1); 349 resultUpperLess = _mm_cmplt_ps(sum,upperLimit1); 350 __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp); 351 deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) ); 352 c.m_appliedPushImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) ); 353 __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128,body1.internalGetInvMass().mVec128); 354 __m128 linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128,body2.internalGetInvMass().mVec128); 355 __m128 impulseMagnitude = deltaImpulse; 356 body1.internalGetPushVelocity().mVec128 = _mm_add_ps(body1.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); 357 body1.internalGetTurnVelocity().mVec128 = _mm_add_ps(body1.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); 358 body2.internalGetPushVelocity().mVec128 = _mm_add_ps(body2.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); 359 body2.internalGetTurnVelocity().mVec128 = _mm_add_ps(body2.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude)); 360 #else 361 resolveSplitPenetrationImpulseCacheFriendly(body1,body2,c); 362 #endif 363 } 364 365 366 btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver() 367 : m_resolveSingleConstraintRowGeneric(gResolveSingleConstraintRowGeneric_scalar_reference), 368 m_resolveSingleConstraintRowLowerLimit(gResolveSingleConstraintRowLowerLimit_scalar_reference), 369 m_btSeed2(0) 370 { 371 372 #ifdef USE_SIMD 373 m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_sse2; 374 m_resolveSingleConstraintRowLowerLimit=gResolveSingleConstraintRowLowerLimit_sse2; 375 #endif //USE_SIMD 376 377 #ifdef BT_ALLOW_SSE4 378 int cpuFeatures = btCpuFeatureUtility::getCpuFeatures(); 379 if ((cpuFeatures & btCpuFeatureUtility::CPU_FEATURE_FMA3) && (cpuFeatures & btCpuFeatureUtility::CPU_FEATURE_SSE4_1)) 380 { 381 m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_sse4_1_fma3; 382 m_resolveSingleConstraintRowLowerLimit = gResolveSingleConstraintRowLowerLimit_sse4_1_fma3; 383 } 384 #endif//BT_ALLOW_SSE4 385 386 } 387 388 btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver() 389 { 390 } 391 392 btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getScalarConstraintRowSolverGeneric() 393 { 394 return gResolveSingleConstraintRowGeneric_scalar_reference; 395 } 396 397 btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getScalarConstraintRowSolverLowerLimit() 398 { 399 return gResolveSingleConstraintRowLowerLimit_scalar_reference; 400 } 401 402 403 #ifdef USE_SIMD 404 btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE2ConstraintRowSolverGeneric() 405 { 406 return gResolveSingleConstraintRowGeneric_sse2; 407 } 408 btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE2ConstraintRowSolverLowerLimit() 409 { 410 return gResolveSingleConstraintRowLowerLimit_sse2; 411 } 412 #ifdef BT_ALLOW_SSE4 413 btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE4_1ConstraintRowSolverGeneric() 414 { 415 return gResolveSingleConstraintRowGeneric_sse4_1_fma3; 416 } 417 btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE4_1ConstraintRowSolverLowerLimit() 418 { 419 return gResolveSingleConstraintRowLowerLimit_sse4_1_fma3; 420 } 421 #endif //BT_ALLOW_SSE4 422 #endif //USE_SIMD 423 424 unsigned long btSequentialImpulseConstraintSolver::btRand2() 425 { 426 m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff; 427 return m_btSeed2; 428 } 429 430 431 432 //See ODE: adam's all-int straightforward(?) dRandInt (0..n-1) 433 int btSequentialImpulseConstraintSolver::btRandInt2 (int n) 434 { 435 // seems good; xor-fold and modulus 436 const unsigned long un = static_cast<unsigned long>(n); 437 unsigned long r = btRand2(); 438 439 // note: probably more aggressive than it needs to be -- might be 440 // able to get away without one or two of the innermost branches. 441 if (un <= 0x00010000UL) { 442 r ^= (r >> 16); 443 if (un <= 0x00000100UL) { 444 r ^= (r >> 8); 445 if (un <= 0x00000010UL) { 446 r ^= (r >> 4); 447 if (un <= 0x00000004UL) { 448 r ^= (r >> 2); 449 if (un <= 0x00000002UL) { 450 r ^= (r >> 1); 451 } 452 } 453 } 454 } 455 } 456 457 return (int) (r % un); 458 } 459 460 461 462 void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject, btScalar timeStep) 463 { 464 465 btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0; 466 467 solverBody->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); 468 solverBody->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); 469 solverBody->internalGetPushVelocity().setValue(0.f,0.f,0.f); 470 solverBody->internalGetTurnVelocity().setValue(0.f,0.f,0.f); 471 472 if (rb) 473 { 474 solverBody->m_worldTransform = rb->getWorldTransform(); 475 solverBody->internalSetInvMass(btVector3(rb->getInvMass(),rb->getInvMass(),rb->getInvMass())*rb->getLinearFactor()); 476 solverBody->m_originalBody = rb; 477 solverBody->m_angularFactor = rb->getAngularFactor(); 478 solverBody->m_linearFactor = rb->getLinearFactor(); 479 solverBody->m_linearVelocity = rb->getLinearVelocity(); 480 solverBody->m_angularVelocity = rb->getAngularVelocity(); 481 solverBody->m_externalForceImpulse = rb->getTotalForce()*rb->getInvMass()*timeStep; 482 solverBody->m_externalTorqueImpulse = rb->getTotalTorque()*rb->getInvInertiaTensorWorld()*timeStep ; 483 484 } else 485 { 486 solverBody->m_worldTransform.setIdentity(); 487 solverBody->internalSetInvMass(btVector3(0,0,0)); 488 solverBody->m_originalBody = 0; 489 solverBody->m_angularFactor.setValue(1,1,1); 490 solverBody->m_linearFactor.setValue(1,1,1); 491 solverBody->m_linearVelocity.setValue(0,0,0); 492 solverBody->m_angularVelocity.setValue(0,0,0); 493 solverBody->m_externalForceImpulse.setValue(0,0,0); 494 solverBody->m_externalTorqueImpulse.setValue(0,0,0); 495 } 496 497 498 } 499 500 501 502 503 504 505 btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution) 506 { 507 btScalar rest = restitution * -rel_vel; 508 return rest; 509 } 510 511 512 513 void btSequentialImpulseConstraintSolver::applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode) 514 { 515 516 517 if (colObj && colObj->hasAnisotropicFriction(frictionMode)) 518 { 519 // transform to local coordinates 520 btVector3 loc_lateral = frictionDirection * colObj->getWorldTransform().getBasis(); 521 const btVector3& friction_scaling = colObj->getAnisotropicFriction(); 522 //apply anisotropic friction 523 loc_lateral *= friction_scaling; 524 // ... and transform it back to global coordinates 525 frictionDirection = colObj->getWorldTransform().getBasis() * loc_lateral; 526 } 527 528 } 529 530 531 532 533 void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip) 534 { 535 536 537 btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA]; 538 btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB]; 539 540 btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody; 541 btRigidBody* body1 = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody; 542 543 solverConstraint.m_solverBodyIdA = solverBodyIdA; 544 solverConstraint.m_solverBodyIdB = solverBodyIdB; 545 546 solverConstraint.m_friction = cp.m_combinedFriction; 547 solverConstraint.m_originalContactPoint = 0; 548 549 solverConstraint.m_appliedImpulse = 0.f; 550 solverConstraint.m_appliedPushImpulse = 0.f; 551 552 if (body0) 553 { 554 solverConstraint.m_contactNormal1 = normalAxis; 555 btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal1); 556 solverConstraint.m_relpos1CrossNormal = ftorqueAxis1; 557 solverConstraint.m_angularComponentA = body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor(); 558 }else 559 { 560 solverConstraint.m_contactNormal1.setZero(); 561 solverConstraint.m_relpos1CrossNormal.setZero(); 562 solverConstraint.m_angularComponentA .setZero(); 563 } 564 565 if (body1) 566 { 567 solverConstraint.m_contactNormal2 = -normalAxis; 568 btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal2); 569 solverConstraint.m_relpos2CrossNormal = ftorqueAxis1; 570 solverConstraint.m_angularComponentB = body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor(); 571 } else 572 { 573 solverConstraint.m_contactNormal2.setZero(); 574 solverConstraint.m_relpos2CrossNormal.setZero(); 575 solverConstraint.m_angularComponentB.setZero(); 576 } 577 578 { 579 btVector3 vec; 580 btScalar denom0 = 0.f; 581 btScalar denom1 = 0.f; 582 if (body0) 583 { 584 vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); 585 denom0 = body0->getInvMass() + normalAxis.dot(vec); 586 } 587 if (body1) 588 { 589 vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); 590 denom1 = body1->getInvMass() + normalAxis.dot(vec); 591 } 592 btScalar denom = relaxation/(denom0+denom1); 593 solverConstraint.m_jacDiagABInv = denom; 594 } 595 596 { 597 598 599 btScalar rel_vel; 600 btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity+solverBodyA.m_externalForceImpulse:btVector3(0,0,0)) 601 + solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0)); 602 btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0)) 603 + solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0)); 604 605 rel_vel = vel1Dotn+vel2Dotn; 606 607 // btScalar positionalError = 0.f; 608 609 btScalar velocityError = desiredVelocity - rel_vel; 610 btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv; 611 solverConstraint.m_rhs = velocityImpulse; 612 solverConstraint.m_rhsPenetration = 0.f; 613 solverConstraint.m_cfm = cfmSlip; 614 solverConstraint.m_lowerLimit = -solverConstraint.m_friction; 615 solverConstraint.m_upperLimit = solverConstraint.m_friction; 616 617 } 618 } 619 620 btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip) 621 { 622 btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing(); 623 solverConstraint.m_frictionIndex = frictionIndex; 624 setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, 625 colObj0, colObj1, relaxation, desiredVelocity, cfmSlip); 626 return solverConstraint; 627 } 628 629 630 void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis1,int solverBodyIdA,int solverBodyIdB, 631 btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2, 632 btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, 633 btScalar desiredVelocity, btScalar cfmSlip) 634 635 { 636 btVector3 normalAxis(0,0,0); 637 638 639 solverConstraint.m_contactNormal1 = normalAxis; 640 solverConstraint.m_contactNormal2 = -normalAxis; 641 btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA]; 642 btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB]; 643 644 btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody; 645 btRigidBody* body1 = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody; 646 647 solverConstraint.m_solverBodyIdA = solverBodyIdA; 648 solverConstraint.m_solverBodyIdB = solverBodyIdB; 649 650 solverConstraint.m_friction = cp.m_combinedRollingFriction; 651 solverConstraint.m_originalContactPoint = 0; 652 653 solverConstraint.m_appliedImpulse = 0.f; 654 solverConstraint.m_appliedPushImpulse = 0.f; 655 656 { 657 btVector3 ftorqueAxis1 = -normalAxis1; 658 solverConstraint.m_relpos1CrossNormal = ftorqueAxis1; 659 solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor() : btVector3(0,0,0); 660 } 661 { 662 btVector3 ftorqueAxis1 = normalAxis1; 663 solverConstraint.m_relpos2CrossNormal = ftorqueAxis1; 664 solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor() : btVector3(0,0,0); 665 } 666 667 668 { 669 btVector3 iMJaA = body0?body0->getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal:btVector3(0,0,0); 670 btVector3 iMJaB = body1?body1->getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal:btVector3(0,0,0); 671 btScalar sum = 0; 672 sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal); 673 sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal); 674 solverConstraint.m_jacDiagABInv = btScalar(1.)/sum; 675 } 676 677 { 678 679 680 btScalar rel_vel; 681 btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity+solverBodyA.m_externalForceImpulse:btVector3(0,0,0)) 682 + solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0)); 683 btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0)) 684 + solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0)); 685 686 rel_vel = vel1Dotn+vel2Dotn; 687 688 // btScalar positionalError = 0.f; 689 690 btSimdScalar velocityError = desiredVelocity - rel_vel; 691 btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv); 692 solverConstraint.m_rhs = velocityImpulse; 693 solverConstraint.m_cfm = cfmSlip; 694 solverConstraint.m_lowerLimit = -solverConstraint.m_friction; 695 solverConstraint.m_upperLimit = solverConstraint.m_friction; 696 697 } 698 } 699 700 701 702 703 704 705 706 707 btSolverConstraint& btSequentialImpulseConstraintSolver::addRollingFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip) 708 { 709 btSolverConstraint& solverConstraint = m_tmpSolverContactRollingFrictionConstraintPool.expandNonInitializing(); 710 solverConstraint.m_frictionIndex = frictionIndex; 711 setupRollingFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, 712 colObj0, colObj1, relaxation, desiredVelocity, cfmSlip); 713 return solverConstraint; 714 } 715 716 717 int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body,btScalar timeStep) 718 { 719 720 int solverBodyIdA = -1; 721 722 if (body.getCompanionId() >= 0) 723 { 724 //body has already been converted 725 solverBodyIdA = body.getCompanionId(); 726 btAssert(solverBodyIdA < m_tmpSolverBodyPool.size()); 727 } else 728 { 729 btRigidBody* rb = btRigidBody::upcast(&body); 730 //convert both active and kinematic objects (for their velocity) 731 if (rb && (rb->getInvMass() || rb->isKinematicObject())) 732 { 733 solverBodyIdA = m_tmpSolverBodyPool.size(); 734 btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); 735 initSolverBody(&solverBody,&body,timeStep); 736 body.setCompanionId(solverBodyIdA); 737 } else 738 { 739 740 if (m_fixedBodyId<0) 741 { 742 m_fixedBodyId = m_tmpSolverBodyPool.size(); 743 btSolverBody& fixedBody = m_tmpSolverBodyPool.expand(); 744 initSolverBody(&fixedBody,0,timeStep); 745 } 746 return m_fixedBodyId; 747 // return 0;//assume first one is a fixed solver body 748 } 749 } 750 751 return solverBodyIdA; 752 753 } 754 #include <stdio.h> 755 756 757 void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstraint& solverConstraint, 758 int solverBodyIdA, int solverBodyIdB, 759 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, 760 btScalar& relaxation, 761 const btVector3& rel_pos1, const btVector3& rel_pos2) 762 { 763 764 // const btVector3& pos1 = cp.getPositionWorldOnA(); 765 // const btVector3& pos2 = cp.getPositionWorldOnB(); 766 767 btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA]; 768 btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB]; 769 770 btRigidBody* rb0 = bodyA->m_originalBody; 771 btRigidBody* rb1 = bodyB->m_originalBody; 772 773 // btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); 774 // btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin(); 775 //rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); 776 //rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin(); 777 778 relaxation = 1.f; 779 780 btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB); 781 solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); 782 btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB); 783 solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0); 784 785 { 786 #ifdef COMPUTE_IMPULSE_DENOM 787 btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB); 788 btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB); 789 #else 790 btVector3 vec; 791 btScalar denom0 = 0.f; 792 btScalar denom1 = 0.f; 793 if (rb0) 794 { 795 vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); 796 denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec); 797 } 798 if (rb1) 799 { 800 vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); 801 denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec); 802 } 803 #endif //COMPUTE_IMPULSE_DENOM 804 805 btScalar denom = relaxation/(denom0+denom1); 806 solverConstraint.m_jacDiagABInv = denom; 807 } 808 809 if (rb0) 810 { 811 solverConstraint.m_contactNormal1 = cp.m_normalWorldOnB; 812 solverConstraint.m_relpos1CrossNormal = torqueAxis0; 813 } else 814 { 815 solverConstraint.m_contactNormal1.setZero(); 816 solverConstraint.m_relpos1CrossNormal.setZero(); 817 } 818 if (rb1) 819 { 820 solverConstraint.m_contactNormal2 = -cp.m_normalWorldOnB; 821 solverConstraint.m_relpos2CrossNormal = -torqueAxis1; 822 }else 823 { 824 solverConstraint.m_contactNormal2.setZero(); 825 solverConstraint.m_relpos2CrossNormal.setZero(); 826 } 827 828 btScalar restitution = 0.f; 829 btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop; 830 831 { 832 btVector3 vel1,vel2; 833 834 vel1 = rb0? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0); 835 vel2 = rb1? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0); 836 837 // btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0); 838 btVector3 vel = vel1 - vel2; 839 btScalar rel_vel = cp.m_normalWorldOnB.dot(vel); 840 841 842 843 solverConstraint.m_friction = cp.m_combinedFriction; 844 845 846 restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution); 847 if (restitution <= btScalar(0.)) 848 { 849 restitution = 0.f; 850 }; 851 } 852 853 854 ///warm starting (or zero if disabled) 855 if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) 856 { 857 solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; 858 if (rb0) 859 bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse); 860 if (rb1) 861 bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse); 862 } else 863 { 864 solverConstraint.m_appliedImpulse = 0.f; 865 } 866 867 solverConstraint.m_appliedPushImpulse = 0.f; 868 869 { 870 871 btVector3 externalForceImpulseA = bodyA->m_originalBody ? bodyA->m_externalForceImpulse: btVector3(0,0,0); 872 btVector3 externalTorqueImpulseA = bodyA->m_originalBody ? bodyA->m_externalTorqueImpulse: btVector3(0,0,0); 873 btVector3 externalForceImpulseB = bodyB->m_originalBody ? bodyB->m_externalForceImpulse: btVector3(0,0,0); 874 btVector3 externalTorqueImpulseB = bodyB->m_originalBody ?bodyB->m_externalTorqueImpulse : btVector3(0,0,0); 875 876 877 btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(bodyA->m_linearVelocity+externalForceImpulseA) 878 + solverConstraint.m_relpos1CrossNormal.dot(bodyA->m_angularVelocity+externalTorqueImpulseA); 879 btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(bodyB->m_linearVelocity+externalForceImpulseB) 880 + solverConstraint.m_relpos2CrossNormal.dot(bodyB->m_angularVelocity+externalTorqueImpulseB); 881 btScalar rel_vel = vel1Dotn+vel2Dotn; 882 883 btScalar positionalError = 0.f; 884 btScalar velocityError = restitution - rel_vel;// * damping; 885 886 887 btScalar erp = infoGlobal.m_erp2; 888 if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) 889 { 890 erp = infoGlobal.m_erp; 891 } 892 893 if (penetration>0) 894 { 895 positionalError = 0; 896 897 velocityError -= penetration / infoGlobal.m_timeStep; 898 } else 899 { 900 positionalError = -penetration * erp/infoGlobal.m_timeStep; 901 } 902 903 btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; 904 btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; 905 906 if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) 907 { 908 //combine position and velocity into rhs 909 solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;//-solverConstraint.m_contactNormal1.dot(bodyA->m_externalForce*bodyA->m_invMass-bodyB->m_externalForce/bodyB->m_invMass)*solverConstraint.m_jacDiagABInv; 910 solverConstraint.m_rhsPenetration = 0.f; 911 912 } else 913 { 914 //split position and velocity into rhs and m_rhsPenetration 915 solverConstraint.m_rhs = velocityImpulse; 916 solverConstraint.m_rhsPenetration = penetrationImpulse; 917 } 918 solverConstraint.m_cfm = 0.f; 919 solverConstraint.m_lowerLimit = 0; 920 solverConstraint.m_upperLimit = 1e10f; 921 } 922 923 924 925 926 } 927 928 929 930 void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, 931 int solverBodyIdA, int solverBodyIdB, 932 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal) 933 { 934 935 btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA]; 936 btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB]; 937 938 btRigidBody* rb0 = bodyA->m_originalBody; 939 btRigidBody* rb1 = bodyB->m_originalBody; 940 941 { 942 btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex]; 943 if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) 944 { 945 frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor; 946 if (rb0) 947 bodyA->internalApplyImpulse(frictionConstraint1.m_contactNormal1*rb0->getInvMass()*rb0->getLinearFactor(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse); 948 if (rb1) 949 bodyB->internalApplyImpulse(-frictionConstraint1.m_contactNormal2*rb1->getInvMass()*rb1->getLinearFactor(),-frictionConstraint1.m_angularComponentB,-(btScalar)frictionConstraint1.m_appliedImpulse); 950 } else 951 { 952 frictionConstraint1.m_appliedImpulse = 0.f; 953 } 954 } 955 956 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) 957 { 958 btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1]; 959 if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) 960 { 961 frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor; 962 if (rb0) 963 bodyA->internalApplyImpulse(frictionConstraint2.m_contactNormal1*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse); 964 if (rb1) 965 bodyB->internalApplyImpulse(-frictionConstraint2.m_contactNormal2*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-(btScalar)frictionConstraint2.m_appliedImpulse); 966 } else 967 { 968 frictionConstraint2.m_appliedImpulse = 0.f; 969 } 970 } 971 } 972 973 974 975 976 void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal) 977 { 978 btCollisionObject* colObj0=0,*colObj1=0; 979 980 colObj0 = (btCollisionObject*)manifold->getBody0(); 981 colObj1 = (btCollisionObject*)manifold->getBody1(); 982 983 int solverBodyIdA = getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep); 984 int solverBodyIdB = getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep); 985 986 // btRigidBody* bodyA = btRigidBody::upcast(colObj0); 987 // btRigidBody* bodyB = btRigidBody::upcast(colObj1); 988 989 btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverBodyIdA]; 990 btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverBodyIdB]; 991 992 993 994 ///avoid collision response between two static objects 995 if (!solverBodyA || (solverBodyA->m_invMass.fuzzyZero() && (!solverBodyB || solverBodyB->m_invMass.fuzzyZero()))) 996 return; 997 998 int rollingFriction=1; 999 for (int j=0;j<manifold->getNumContacts();j++) 1000 { 1001 1002 btManifoldPoint& cp = manifold->getContactPoint(j); 1003 1004 if (cp.getDistance() <= manifold->getContactProcessingThreshold()) 1005 { 1006 btVector3 rel_pos1; 1007 btVector3 rel_pos2; 1008 btScalar relaxation; 1009 1010 1011 int frictionIndex = m_tmpSolverContactConstraintPool.size(); 1012 btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing(); 1013 btRigidBody* rb0 = btRigidBody::upcast(colObj0); 1014 btRigidBody* rb1 = btRigidBody::upcast(colObj1); 1015 solverConstraint.m_solverBodyIdA = solverBodyIdA; 1016 solverConstraint.m_solverBodyIdB = solverBodyIdB; 1017 1018 solverConstraint.m_originalContactPoint = &cp; 1019 1020 const btVector3& pos1 = cp.getPositionWorldOnA(); 1021 const btVector3& pos2 = cp.getPositionWorldOnB(); 1022 1023 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); 1024 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin(); 1025 1026 btVector3 vel1;// = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0); 1027 btVector3 vel2;// = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0); 1028 1029 solverBodyA->getVelocityInLocalPointNoDelta(rel_pos1,vel1); 1030 solverBodyB->getVelocityInLocalPointNoDelta(rel_pos2,vel2 ); 1031 1032 btVector3 vel = vel1 - vel2; 1033 btScalar rel_vel = cp.m_normalWorldOnB.dot(vel); 1034 1035 setupContactConstraint(solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, relaxation, rel_pos1, rel_pos2); 1036 1037 1038 1039 // const btVector3& pos1 = cp.getPositionWorldOnA(); 1040 // const btVector3& pos2 = cp.getPositionWorldOnB(); 1041 1042 /////setup the friction constraints 1043 1044 solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size(); 1045 1046 btVector3 angVelA(0,0,0),angVelB(0,0,0); 1047 if (rb0) 1048 angVelA = rb0->getAngularVelocity(); 1049 if (rb1) 1050 angVelB = rb1->getAngularVelocity(); 1051 btVector3 relAngVel = angVelB-angVelA; 1052 1053 if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0)) 1054 { 1055 //only a single rollingFriction per manifold 1056 rollingFriction--; 1057 if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold) 1058 { 1059 relAngVel.normalize(); 1060 applyAnisotropicFriction(colObj0,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); 1061 applyAnisotropicFriction(colObj1,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); 1062 if (relAngVel.length()>0.001) 1063 addRollingFrictionConstraint(relAngVel,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); 1064 1065 } else 1066 { 1067 addRollingFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); 1068 btVector3 axis0,axis1; 1069 btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1); 1070 applyAnisotropicFriction(colObj0,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); 1071 applyAnisotropicFriction(colObj1,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); 1072 applyAnisotropicFriction(colObj0,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); 1073 applyAnisotropicFriction(colObj1,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); 1074 if (axis0.length()>0.001) 1075 addRollingFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); 1076 if (axis1.length()>0.001) 1077 addRollingFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); 1078 1079 } 1080 } 1081 1082 ///Bullet has several options to set the friction directions 1083 ///By default, each contact has only a single friction direction that is recomputed automatically very frame 1084 ///based on the relative linear velocity. 1085 ///If the relative velocity it zero, it will automatically compute a friction direction. 1086 1087 ///You can also enable two friction directions, using the SOLVER_USE_2_FRICTION_DIRECTIONS. 1088 ///In that case, the second friction direction will be orthogonal to both contact normal and first friction direction. 1089 /// 1090 ///If you choose SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION, then the friction will be independent from the relative projected velocity. 1091 /// 1092 ///The user can manually override the friction directions for certain contacts using a contact callback, 1093 ///and set the cp.m_lateralFrictionInitialized to true 1094 ///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2) 1095 ///this will give a conveyor belt effect 1096 /// 1097 if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized) 1098 { 1099 cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel; 1100 btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2(); 1101 if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON) 1102 { 1103 cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel); 1104 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); 1105 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); 1106 addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); 1107 1108 if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) 1109 { 1110 cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB); 1111 cp.m_lateralFrictionDir2.normalize();//?? 1112 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); 1113 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); 1114 addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); 1115 } 1116 1117 } else 1118 { 1119 btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2); 1120 1121 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); 1122 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); 1123 addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); 1124 1125 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) 1126 { 1127 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); 1128 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); 1129 addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); 1130 } 1131 1132 1133 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION)) 1134 { 1135 cp.m_lateralFrictionInitialized = true; 1136 } 1137 } 1138 1139 } else 1140 { 1141 addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_contactCFM1); 1142 1143 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) 1144 addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2); 1145 1146 } 1147 setFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal); 1148 1149 1150 1151 1152 } 1153 } 1154 } 1155 1156 void btSequentialImpulseConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal) 1157 { 1158 int i; 1159 btPersistentManifold* manifold = 0; 1160 // btCollisionObject* colObj0=0,*colObj1=0; 1161 1162 1163 for (i=0;i<numManifolds;i++) 1164 { 1165 manifold = manifoldPtr[i]; 1166 convertContact(manifold,infoGlobal); 1167 } 1168 } 1169 1170 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) 1171 { 1172 m_fixedBodyId = -1; 1173 BT_PROFILE("solveGroupCacheFriendlySetup"); 1174 (void)debugDrawer; 1175 1176 m_maxOverrideNumSolverIterations = 0; 1177 1178 #ifdef BT_ADDITIONAL_DEBUG 1179 //make sure that dynamic bodies exist for all (enabled) constraints 1180 for (int i=0;i<numConstraints;i++) 1181 { 1182 btTypedConstraint* constraint = constraints[i]; 1183 if (constraint->isEnabled()) 1184 { 1185 if (!constraint->getRigidBodyA().isStaticOrKinematicObject()) 1186 { 1187 bool found=false; 1188 for (int b=0;b<numBodies;b++) 1189 { 1190 1191 if (&constraint->getRigidBodyA()==bodies[b]) 1192 { 1193 found = true; 1194 break; 1195 } 1196 } 1197 btAssert(found); 1198 } 1199 if (!constraint->getRigidBodyB().isStaticOrKinematicObject()) 1200 { 1201 bool found=false; 1202 for (int b=0;b<numBodies;b++) 1203 { 1204 if (&constraint->getRigidBodyB()==bodies[b]) 1205 { 1206 found = true; 1207 break; 1208 } 1209 } 1210 btAssert(found); 1211 } 1212 } 1213 } 1214 //make sure that dynamic bodies exist for all contact manifolds 1215 for (int i=0;i<numManifolds;i++) 1216 { 1217 if (!manifoldPtr[i]->getBody0()->isStaticOrKinematicObject()) 1218 { 1219 bool found=false; 1220 for (int b=0;b<numBodies;b++) 1221 { 1222 1223 if (manifoldPtr[i]->getBody0()==bodies[b]) 1224 { 1225 found = true; 1226 break; 1227 } 1228 } 1229 btAssert(found); 1230 } 1231 if (!manifoldPtr[i]->getBody1()->isStaticOrKinematicObject()) 1232 { 1233 bool found=false; 1234 for (int b=0;b<numBodies;b++) 1235 { 1236 if (manifoldPtr[i]->getBody1()==bodies[b]) 1237 { 1238 found = true; 1239 break; 1240 } 1241 } 1242 btAssert(found); 1243 } 1244 } 1245 #endif //BT_ADDITIONAL_DEBUG 1246 1247 1248 for (int i = 0; i < numBodies; i++) 1249 { 1250 bodies[i]->setCompanionId(-1); 1251 } 1252 1253 1254 m_tmpSolverBodyPool.reserve(numBodies+1); 1255 m_tmpSolverBodyPool.resize(0); 1256 1257 //btSolverBody& fixedBody = m_tmpSolverBodyPool.expand(); 1258 //initSolverBody(&fixedBody,0); 1259 1260 //convert all bodies 1261 1262 1263 for (int i=0;i<numBodies;i++) 1264 { 1265 int bodyId = getOrInitSolverBody(*bodies[i],infoGlobal.m_timeStep); 1266 1267 btRigidBody* body = btRigidBody::upcast(bodies[i]); 1268 if (body && body->getInvMass()) 1269 { 1270 btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId]; 1271 btVector3 gyroForce (0,0,0); 1272 if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT) 1273 { 1274 gyroForce = body->computeGyroscopicForceExplicit(infoGlobal.m_maxGyroscopicForce); 1275 solverBody.m_externalTorqueImpulse -= gyroForce*body->getInvInertiaTensorWorld()*infoGlobal.m_timeStep; 1276 } 1277 if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD) 1278 { 1279 gyroForce = body->computeGyroscopicImpulseImplicit_World(infoGlobal.m_timeStep); 1280 solverBody.m_externalTorqueImpulse += gyroForce; 1281 } 1282 if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY) 1283 { 1284 gyroForce = body->computeGyroscopicImpulseImplicit_Body(infoGlobal.m_timeStep); 1285 solverBody.m_externalTorqueImpulse += gyroForce; 1286 1287 } 1288 1289 1290 } 1291 } 1292 1293 if (1) 1294 { 1295 int j; 1296 for (j=0;j<numConstraints;j++) 1297 { 1298 btTypedConstraint* constraint = constraints[j]; 1299 constraint->buildJacobian(); 1300 constraint->internalSetAppliedImpulse(0.0f); 1301 } 1302 } 1303 1304 //btRigidBody* rb0=0,*rb1=0; 1305 1306 //if (1) 1307 { 1308 { 1309 1310 int totalNumRows = 0; 1311 int i; 1312 1313 m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints); 1314 //calculate the total number of contraint rows 1315 for (i=0;i<numConstraints;i++) 1316 { 1317 btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i]; 1318 btJointFeedback* fb = constraints[i]->getJointFeedback(); 1319 if (fb) 1320 { 1321 fb->m_appliedForceBodyA.setZero(); 1322 fb->m_appliedTorqueBodyA.setZero(); 1323 fb->m_appliedForceBodyB.setZero(); 1324 fb->m_appliedTorqueBodyB.setZero(); 1325 } 1326 1327 if (constraints[i]->isEnabled()) 1328 { 1329 } 1330 if (constraints[i]->isEnabled()) 1331 { 1332 constraints[i]->getInfo1(&info1); 1333 } else 1334 { 1335 info1.m_numConstraintRows = 0; 1336 info1.nub = 0; 1337 } 1338 totalNumRows += info1.m_numConstraintRows; 1339 } 1340 m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows); 1341 1342 1343 ///setup the btSolverConstraints 1344 int currentRow = 0; 1345 1346 for (i=0;i<numConstraints;i++) 1347 { 1348 const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i]; 1349 1350 if (info1.m_numConstraintRows) 1351 { 1352 btAssert(currentRow<totalNumRows); 1353 1354 btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow]; 1355 btTypedConstraint* constraint = constraints[i]; 1356 btRigidBody& rbA = constraint->getRigidBodyA(); 1357 btRigidBody& rbB = constraint->getRigidBodyB(); 1358 1359 int solverBodyIdA = getOrInitSolverBody(rbA,infoGlobal.m_timeStep); 1360 int solverBodyIdB = getOrInitSolverBody(rbB,infoGlobal.m_timeStep); 1361 1362 btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA]; 1363 btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB]; 1364 1365 1366 1367 1368 int overrideNumSolverIterations = constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations; 1369 if (overrideNumSolverIterations>m_maxOverrideNumSolverIterations) 1370 m_maxOverrideNumSolverIterations = overrideNumSolverIterations; 1371 1372 1373 int j; 1374 for ( j=0;j<info1.m_numConstraintRows;j++) 1375 { 1376 memset(¤tConstraintRow[j],0,sizeof(btSolverConstraint)); 1377 currentConstraintRow[j].m_lowerLimit = -SIMD_INFINITY; 1378 currentConstraintRow[j].m_upperLimit = SIMD_INFINITY; 1379 currentConstraintRow[j].m_appliedImpulse = 0.f; 1380 currentConstraintRow[j].m_appliedPushImpulse = 0.f; 1381 currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA; 1382 currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB; 1383 currentConstraintRow[j].m_overrideNumSolverIterations = overrideNumSolverIterations; 1384 } 1385 1386 bodyAPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); 1387 bodyAPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); 1388 bodyAPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f); 1389 bodyAPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f); 1390 bodyBPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); 1391 bodyBPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); 1392 bodyBPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f); 1393 bodyBPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f); 1394 1395 1396 btTypedConstraint::btConstraintInfo2 info2; 1397 info2.fps = 1.f/infoGlobal.m_timeStep; 1398 info2.erp = infoGlobal.m_erp; 1399 info2.m_J1linearAxis = currentConstraintRow->m_contactNormal1; 1400 info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal; 1401 info2.m_J2linearAxis = currentConstraintRow->m_contactNormal2; 1402 info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal; 1403 info2.rowskip = sizeof(btSolverConstraint)/sizeof(btScalar);//check this 1404 ///the size of btSolverConstraint needs be a multiple of btScalar 1405 btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint)); 1406 info2.m_constraintError = ¤tConstraintRow->m_rhs; 1407 currentConstraintRow->m_cfm = infoGlobal.m_globalCfm; 1408 info2.m_damping = infoGlobal.m_damping; 1409 info2.cfm = ¤tConstraintRow->m_cfm; 1410 info2.m_lowerLimit = ¤tConstraintRow->m_lowerLimit; 1411 info2.m_upperLimit = ¤tConstraintRow->m_upperLimit; 1412 info2.m_numIterations = infoGlobal.m_numIterations; 1413 constraints[i]->getInfo2(&info2); 1414 1415 ///finalize the constraint setup 1416 for ( j=0;j<info1.m_numConstraintRows;j++) 1417 { 1418 btSolverConstraint& solverConstraint = currentConstraintRow[j]; 1419 1420 if (solverConstraint.m_upperLimit>=constraints[i]->getBreakingImpulseThreshold()) 1421 { 1422 solverConstraint.m_upperLimit = constraints[i]->getBreakingImpulseThreshold(); 1423 } 1424 1425 if (solverConstraint.m_lowerLimit<=-constraints[i]->getBreakingImpulseThreshold()) 1426 { 1427 solverConstraint.m_lowerLimit = -constraints[i]->getBreakingImpulseThreshold(); 1428 } 1429 1430 solverConstraint.m_originalContactPoint = constraint; 1431 1432 { 1433 const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal; 1434 solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld()*ftorqueAxis1*constraint->getRigidBodyA().getAngularFactor(); 1435 } 1436 { 1437 const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal; 1438 solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld()*ftorqueAxis2*constraint->getRigidBodyB().getAngularFactor(); 1439 } 1440 1441 { 1442 btVector3 iMJlA = solverConstraint.m_contactNormal1*rbA.getInvMass(); 1443 btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal; 1444 btVector3 iMJlB = solverConstraint.m_contactNormal2*rbB.getInvMass();//sign of normal? 1445 btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal; 1446 1447 btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal1); 1448 sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal); 1449 sum += iMJlB.dot(solverConstraint.m_contactNormal2); 1450 sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal); 1451 btScalar fsum = btFabs(sum); 1452 btAssert(fsum > SIMD_EPSILON); 1453 solverConstraint.m_jacDiagABInv = fsum>SIMD_EPSILON?btScalar(1.)/sum : 0.f; 1454 } 1455 1456 1457 1458 { 1459 btScalar rel_vel; 1460 btVector3 externalForceImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalForceImpulse : btVector3(0,0,0); 1461 btVector3 externalTorqueImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalTorqueImpulse : btVector3(0,0,0); 1462 1463 btVector3 externalForceImpulseB = bodyBPtr->m_originalBody ? bodyBPtr->m_externalForceImpulse : btVector3(0,0,0); 1464 btVector3 externalTorqueImpulseB = bodyBPtr->m_originalBody ?bodyBPtr->m_externalTorqueImpulse : btVector3(0,0,0); 1465 1466 btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(rbA.getLinearVelocity()+externalForceImpulseA) 1467 + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity()+externalTorqueImpulseA); 1468 1469 btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(rbB.getLinearVelocity()+externalForceImpulseB) 1470 + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity()+externalTorqueImpulseB); 1471 1472 rel_vel = vel1Dotn+vel2Dotn; 1473 btScalar restitution = 0.f; 1474 btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2 1475 btScalar velocityError = restitution - rel_vel * info2.m_damping; 1476 btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; 1477 btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; 1478 solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; 1479 solverConstraint.m_appliedImpulse = 0.f; 1480 1481 1482 } 1483 } 1484 } 1485 currentRow+=m_tmpConstraintSizesPool[i].m_numConstraintRows; 1486 } 1487 } 1488 1489 convertContacts(manifoldPtr,numManifolds,infoGlobal); 1490 1491 } 1492 1493 // btContactSolverInfo info = infoGlobal; 1494 1495 1496 int numNonContactPool = m_tmpSolverNonContactConstraintPool.size(); 1497 int numConstraintPool = m_tmpSolverContactConstraintPool.size(); 1498 int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size(); 1499 1500 ///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints 1501 m_orderNonContactConstraintPool.resizeNoInitialize(numNonContactPool); 1502 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) 1503 m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool*2); 1504 else 1505 m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool); 1506 1507 m_orderFrictionConstraintPool.resizeNoInitialize(numFrictionPool); 1508 { 1509 int i; 1510 for (i=0;i<numNonContactPool;i++) 1511 { 1512 m_orderNonContactConstraintPool[i] = i; 1513 } 1514 for (i=0;i<numConstraintPool;i++) 1515 { 1516 m_orderTmpConstraintPool[i] = i; 1517 } 1518 for (i=0;i<numFrictionPool;i++) 1519 { 1520 m_orderFrictionConstraintPool[i] = i; 1521 } 1522 } 1523 1524 return 0.f; 1525 1526 } 1527 1528 1529 btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/) 1530 { 1531 1532 int numNonContactPool = m_tmpSolverNonContactConstraintPool.size(); 1533 int numConstraintPool = m_tmpSolverContactConstraintPool.size(); 1534 int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size(); 1535 1536 if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER) 1537 { 1538 if (1) // uncomment this for a bit less random ((iteration & 7) == 0) 1539 { 1540 1541 for (int j=0; j<numNonContactPool; ++j) { 1542 int tmp = m_orderNonContactConstraintPool[j]; 1543 int swapi = btRandInt2(j+1); 1544 m_orderNonContactConstraintPool[j] = m_orderNonContactConstraintPool[swapi]; 1545 m_orderNonContactConstraintPool[swapi] = tmp; 1546 } 1547 1548 //contact/friction constraints are not solved more than 1549 if (iteration< infoGlobal.m_numIterations) 1550 { 1551 for (int j=0; j<numConstraintPool; ++j) { 1552 int tmp = m_orderTmpConstraintPool[j]; 1553 int swapi = btRandInt2(j+1); 1554 m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi]; 1555 m_orderTmpConstraintPool[swapi] = tmp; 1556 } 1557 1558 for (int j=0; j<numFrictionPool; ++j) { 1559 int tmp = m_orderFrictionConstraintPool[j]; 1560 int swapi = btRandInt2(j+1); 1561 m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi]; 1562 m_orderFrictionConstraintPool[swapi] = tmp; 1563 } 1564 } 1565 } 1566 } 1567 1568 if (infoGlobal.m_solverMode & SOLVER_SIMD) 1569 { 1570 ///solve all joint constraints, using SIMD, if available 1571 for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) 1572 { 1573 btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]]; 1574 if (iteration < constraint.m_overrideNumSolverIterations) 1575 resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint); 1576 } 1577 1578 if (iteration< infoGlobal.m_numIterations) 1579 { 1580 for (int j=0;j<numConstraints;j++) 1581 { 1582 if (constraints[j]->isEnabled()) 1583 { 1584 int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep); 1585 int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep); 1586 btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid]; 1587 btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid]; 1588 constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep); 1589 } 1590 } 1591 1592 ///solve all contact constraints using SIMD, if available 1593 if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) 1594 { 1595 int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); 1596 int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)? 2 : 1; 1597 1598 for (int c=0;c<numPoolConstraints;c++) 1599 { 1600 btScalar totalImpulse =0; 1601 1602 { 1603 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]]; 1604 resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); 1605 totalImpulse = solveManifold.m_appliedImpulse; 1606 } 1607 bool applyFriction = true; 1608 if (applyFriction) 1609 { 1610 { 1611 1612 btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier]]; 1613 1614 if (totalImpulse>btScalar(0)) 1615 { 1616 solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); 1617 solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; 1618 1619 resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); 1620 } 1621 } 1622 1623 if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) 1624 { 1625 1626 btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier+1]]; 1627 1628 if (totalImpulse>btScalar(0)) 1629 { 1630 solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); 1631 solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; 1632 1633 resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); 1634 } 1635 } 1636 } 1637 } 1638 1639 } 1640 else//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS 1641 { 1642 //solve the friction constraints after all contact constraints, don't interleave them 1643 int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); 1644 int j; 1645 1646 for (j=0;j<numPoolConstraints;j++) 1647 { 1648 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; 1649 resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); 1650 1651 } 1652 1653 1654 1655 ///solve all friction constraints, using SIMD, if available 1656 1657 int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size(); 1658 for (j=0;j<numFrictionPoolConstraints;j++) 1659 { 1660 btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]]; 1661 btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; 1662 1663 if (totalImpulse>btScalar(0)) 1664 { 1665 solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); 1666 solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; 1667 1668 resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); 1669 } 1670 } 1671 1672 1673 int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size(); 1674 for (j=0;j<numRollingFrictionPoolConstraints;j++) 1675 { 1676 1677 btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j]; 1678 btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse; 1679 if (totalImpulse>btScalar(0)) 1680 { 1681 btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse; 1682 if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction) 1683 rollingFrictionMagnitude = rollingFrictionConstraint.m_friction; 1684 1685 rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude; 1686 rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude; 1687 1688 resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint); 1689 } 1690 } 1691 1692 1693 } 1694 } 1695 } else 1696 { 1697 //non-SIMD version 1698 ///solve all joint constraints 1699 for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) 1700 { 1701 btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]]; 1702 if (iteration < constraint.m_overrideNumSolverIterations) 1703 resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint); 1704 } 1705 1706 if (iteration< infoGlobal.m_numIterations) 1707 { 1708 for (int j=0;j<numConstraints;j++) 1709 { 1710 if (constraints[j]->isEnabled()) 1711 { 1712 int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep); 1713 int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep); 1714 btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid]; 1715 btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid]; 1716 constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep); 1717 } 1718 } 1719 ///solve all contact constraints 1720 int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); 1721 for (int j=0;j<numPoolConstraints;j++) 1722 { 1723 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; 1724 resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); 1725 } 1726 ///solve all friction constraints 1727 int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size(); 1728 for (int j=0;j<numFrictionPoolConstraints;j++) 1729 { 1730 btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]]; 1731 btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; 1732 1733 if (totalImpulse>btScalar(0)) 1734 { 1735 solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); 1736 solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; 1737 1738 resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); 1739 } 1740 } 1741 1742 int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size(); 1743 for (int j=0;j<numRollingFrictionPoolConstraints;j++) 1744 { 1745 btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j]; 1746 btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse; 1747 if (totalImpulse>btScalar(0)) 1748 { 1749 btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse; 1750 if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction) 1751 rollingFrictionMagnitude = rollingFrictionConstraint.m_friction; 1752 1753 rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude; 1754 rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude; 1755 1756 resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint); 1757 } 1758 } 1759 } 1760 } 1761 return 0.f; 1762 } 1763 1764 1765 void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) 1766 { 1767 int iteration; 1768 if (infoGlobal.m_splitImpulse) 1769 { 1770 if (infoGlobal.m_solverMode & SOLVER_SIMD) 1771 { 1772 for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++) 1773 { 1774 { 1775 int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); 1776 int j; 1777 for (j=0;j<numPoolConstraints;j++) 1778 { 1779 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; 1780 1781 resolveSplitPenetrationSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); 1782 } 1783 } 1784 } 1785 } 1786 else 1787 { 1788 for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++) 1789 { 1790 { 1791 int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); 1792 int j; 1793 for (j=0;j<numPoolConstraints;j++) 1794 { 1795 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; 1796 1797 resolveSplitPenetrationImpulseCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); 1798 } 1799 } 1800 } 1801 } 1802 } 1803 } 1804 1805 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) 1806 { 1807 BT_PROFILE("solveGroupCacheFriendlyIterations"); 1808 1809 { 1810 ///this is a special step to resolve penetrations (just for contacts) 1811 solveGroupCacheFriendlySplitImpulseIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer); 1812 1813 int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations; 1814 1815 for ( int iteration = 0 ; iteration< maxIterations ; iteration++) 1816 //for ( int iteration = maxIterations-1 ; iteration >= 0;iteration--) 1817 { 1818 solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer); 1819 } 1820 1821 } 1822 return 0.f; 1823 } 1824 1825 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal) 1826 { 1827 int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); 1828 int i,j; 1829 1830 if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) 1831 { 1832 for (j=0;j<numPoolConstraints;j++) 1833 { 1834 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j]; 1835 btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint; 1836 btAssert(pt); 1837 pt->m_appliedImpulse = solveManifold.m_appliedImpulse; 1838 // float f = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; 1839 // printf("pt->m_appliedImpulseLateral1 = %f\n", f); 1840 pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; 1841 //printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1); 1842 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) 1843 { 1844 pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse; 1845 } 1846 //do a callback here? 1847 } 1848 } 1849 1850 numPoolConstraints = m_tmpSolverNonContactConstraintPool.size(); 1851 for (j=0;j<numPoolConstraints;j++) 1852 { 1853 const btSolverConstraint& solverConstr = m_tmpSolverNonContactConstraintPool[j]; 1854 btTypedConstraint* constr = (btTypedConstraint*)solverConstr.m_originalContactPoint; 1855 btJointFeedback* fb = constr->getJointFeedback(); 1856 if (fb) 1857 { 1858 fb->m_appliedForceBodyA += solverConstr.m_contactNormal1*solverConstr.m_appliedImpulse*constr->getRigidBodyA().getLinearFactor()/infoGlobal.m_timeStep; 1859 fb->m_appliedForceBodyB += solverConstr.m_contactNormal2*solverConstr.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/infoGlobal.m_timeStep; 1860 fb->m_appliedTorqueBodyA += solverConstr.m_relpos1CrossNormal* constr->getRigidBodyA().getAngularFactor()*solverConstr.m_appliedImpulse/infoGlobal.m_timeStep; 1861 fb->m_appliedTorqueBodyB += solverConstr.m_relpos2CrossNormal* constr->getRigidBodyB().getAngularFactor()*solverConstr.m_appliedImpulse/infoGlobal.m_timeStep; /*RGM ???? */ 1862 1863 } 1864 1865 constr->internalSetAppliedImpulse(solverConstr.m_appliedImpulse); 1866 if (btFabs(solverConstr.m_appliedImpulse)>=constr->getBreakingImpulseThreshold()) 1867 { 1868 constr->setEnabled(false); 1869 } 1870 } 1871 1872 1873 1874 for ( i=0;i<m_tmpSolverBodyPool.size();i++) 1875 { 1876 btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody; 1877 if (body) 1878 { 1879 if (infoGlobal.m_splitImpulse) 1880 m_tmpSolverBodyPool[i].writebackVelocityAndTransform(infoGlobal.m_timeStep, infoGlobal.m_splitImpulseTurnErp); 1881 else 1882 m_tmpSolverBodyPool[i].writebackVelocity(); 1883 1884 m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity( 1885 m_tmpSolverBodyPool[i].m_linearVelocity+ 1886 m_tmpSolverBodyPool[i].m_externalForceImpulse); 1887 1888 m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity( 1889 m_tmpSolverBodyPool[i].m_angularVelocity+ 1890 m_tmpSolverBodyPool[i].m_externalTorqueImpulse); 1891 1892 if (infoGlobal.m_splitImpulse) 1893 m_tmpSolverBodyPool[i].m_originalBody->setWorldTransform(m_tmpSolverBodyPool[i].m_worldTransform); 1894 1895 m_tmpSolverBodyPool[i].m_originalBody->setCompanionId(-1); 1896 } 1897 } 1898 1899 m_tmpSolverContactConstraintPool.resizeNoInitialize(0); 1900 m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0); 1901 m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize(0); 1902 m_tmpSolverContactRollingFrictionConstraintPool.resizeNoInitialize(0); 1903 1904 m_tmpSolverBodyPool.resizeNoInitialize(0); 1905 return 0.f; 1906 } 1907 1908 1909 1910 /// btSequentialImpulseConstraintSolver Sequentially applies impulses 1911 btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btDispatcher* /*dispatcher*/) 1912 { 1913 1914 BT_PROFILE("solveGroup"); 1915 //you need to provide at least some bodies 1916 1917 solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer); 1918 1919 solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer); 1920 1921 solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal); 1922 1923 return 0.f; 1924 } 1925 1926 void btSequentialImpulseConstraintSolver::reset() 1927 { 1928 m_btSeed2 = 0; 1929 } 1930
