18 # define BT_ADDITIONAL_DEBUG
50 const btScalar deltaVel1Dotn =
c.m_contactNormal1.dot(bodyA.internalGetDeltaLinearVelocity()) +
c.m_relpos1CrossNormal.dot(bodyA.internalGetDeltaAngularVelocity());
51 const btScalar deltaVel2Dotn =
c.m_contactNormal2.dot(bodyB.internalGetDeltaLinearVelocity()) +
c.m_relpos2CrossNormal.dot(bodyB.internalGetDeltaAngularVelocity());
54 deltaImpulse -= deltaVel1Dotn *
c.m_jacDiagABInv;
55 deltaImpulse -= deltaVel2Dotn *
c.m_jacDiagABInv;
58 if (
sum <
c.m_lowerLimit)
60 deltaImpulse =
c.m_lowerLimit -
c.m_appliedImpulse;
61 c.m_appliedImpulse =
c.m_lowerLimit;
63 else if (
sum >
c.m_upperLimit)
65 deltaImpulse =
c.m_upperLimit -
c.m_appliedImpulse;
66 c.m_appliedImpulse =
c.m_upperLimit;
70 c.m_appliedImpulse =
sum;
73 bodyA.internalApplyImpulse(
c.m_contactNormal1 * bodyA.internalGetInvMass(),
c.m_angularComponentA, deltaImpulse);
74 bodyB.internalApplyImpulse(
c.m_contactNormal2 * bodyB.internalGetInvMass(),
c.m_angularComponentB, deltaImpulse);
76 return deltaImpulse * (1. /
c.m_jacDiagABInv);
82 const btScalar deltaVel1Dotn =
c.m_contactNormal1.dot(bodyA.internalGetDeltaLinearVelocity()) +
c.m_relpos1CrossNormal.dot(bodyA.internalGetDeltaAngularVelocity());
83 const btScalar deltaVel2Dotn =
c.m_contactNormal2.dot(bodyB.internalGetDeltaLinearVelocity()) +
c.m_relpos2CrossNormal.dot(bodyB.internalGetDeltaAngularVelocity());
85 deltaImpulse -= deltaVel1Dotn *
c.m_jacDiagABInv;
86 deltaImpulse -= deltaVel2Dotn *
c.m_jacDiagABInv;
88 if (
sum <
c.m_lowerLimit)
90 deltaImpulse =
c.m_lowerLimit -
c.m_appliedImpulse;
91 c.m_appliedImpulse =
c.m_lowerLimit;
95 c.m_appliedImpulse =
sum;
97 bodyA.internalApplyImpulse(
c.m_contactNormal1 * bodyA.internalGetInvMass(),
c.m_angularComponentA, deltaImpulse);
98 bodyB.internalApplyImpulse(
c.m_contactNormal2 * bodyB.internalGetInvMass(),
c.m_angularComponentB, deltaImpulse);
100 return deltaImpulse * (1. /
c.m_jacDiagABInv);
104 #include <emmintrin.h>
106 #define btVecSplat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e, e, e, e))
107 static inline __m128 btSimdDot3(__m128 vec0, __m128 vec1)
109 __m128
result = _mm_mul_ps(vec0, vec1);
110 return _mm_add_ps(btVecSplat(
result, 0), _mm_add_ps(btVecSplat(
result, 1), btVecSplat(
result, 2)));
113 #if defined(BT_ALLOW_SSE4)
117 #define USE_FMA3_INSTEAD_FMA4 1
118 #define USE_SSE4_DOT 1
120 #define SSE4_DP(a, b) _mm_dp_ps(a, b, 0x7f)
121 #define SSE4_DP_FP(a, b) _mm_cvtss_f32(_mm_dp_ps(a, b, 0x7f))
124 #define DOT_PRODUCT(a, b) SSE4_DP(a, b)
126 #define DOT_PRODUCT(a, b) btSimdDot3(a, b)
130 #if USE_FMA3_INSTEAD_FMA4
132 #define FMADD(a, b, c) _mm_fmadd_ps(a, b, c)
134 #define FMNADD(a, b, c) _mm_fnmadd_ps(a, b, c)
137 #define FMADD(a, b, c) _mm_macc_ps(a, b, c)
139 #define FMNADD(a, b, c) _mm_nmacc_ps(a, b, c)
143 #define FMADD(a, b, c) _mm_add_ps(c, _mm_mul_ps(a, b))
145 #define FMNADD(a, b, c) _mm_sub_ps(c, _mm_mul_ps(a, b))
152 __m128 cpAppliedImp = _mm_set1_ps(
c.m_appliedImpulse);
153 __m128 lowerLimit1 = _mm_set1_ps(
c.m_lowerLimit);
154 __m128 upperLimit1 = _mm_set1_ps(
c.m_upperLimit);
155 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)));
156 __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(
c.m_contactNormal1.mVec128, bodyA.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(
c.m_relpos1CrossNormal.mVec128, bodyA.internalGetDeltaAngularVelocity().mVec128));
157 __m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(
c.m_contactNormal2.mVec128, bodyB.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(
c.m_relpos2CrossNormal.mVec128, bodyB.internalGetDeltaAngularVelocity().mVec128));
158 deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel1Dotn, _mm_set1_ps(
c.m_jacDiagABInv)));
159 deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel2Dotn, _mm_set1_ps(
c.m_jacDiagABInv)));
162 resultLowerLess = _mm_cmplt_ps(
sum, lowerLimit1);
163 resultUpperLess = _mm_cmplt_ps(
sum, upperLimit1);
164 __m128 lowMinApplied = _mm_sub_ps(lowerLimit1, cpAppliedImp);
165 deltaImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse));
166 c.m_appliedImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess,
sum));
167 __m128 upperMinApplied = _mm_sub_ps(upperLimit1, cpAppliedImp);
168 deltaImpulse = _mm_or_ps(_mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied));
169 c.m_appliedImpulse = _mm_or_ps(_mm_and_ps(resultUpperLess,
c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1));
170 __m128 linearComponentA = _mm_mul_ps(
c.m_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128);
171 __m128 linearComponentB = _mm_mul_ps((
c.m_contactNormal2).mVec128, bodyB.internalGetInvMass().mVec128);
172 __m128 impulseMagnitude = deltaImpulse;
173 bodyA.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(bodyA.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude));
174 bodyA.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(bodyA.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(
c.m_angularComponentA.mVec128, impulseMagnitude));
175 bodyB.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(bodyB.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude));
176 bodyB.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(bodyB.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(
c.m_angularComponentB.mVec128, impulseMagnitude));
177 return deltaImpulse.m_floats[0] /
c.m_jacDiagABInv;
183 #if defined(BT_ALLOW_SSE4)
184 __m128 tmp = _mm_set_ps1(
c.m_jacDiagABInv);
185 __m128 deltaImpulse = _mm_set_ps1(
c.m_rhs -
btScalar(
c.m_appliedImpulse) *
c.m_cfm);
186 const __m128 lowerLimit = _mm_set_ps1(
c.m_lowerLimit);
187 const __m128 upperLimit = _mm_set_ps1(
c.m_upperLimit);
188 const __m128 deltaVel1Dotn = _mm_add_ps(DOT_PRODUCT(
c.m_contactNormal1.mVec128, bodyA.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(
c.m_relpos1CrossNormal.mVec128, bodyA.internalGetDeltaAngularVelocity().mVec128));
189 const __m128 deltaVel2Dotn = _mm_add_ps(DOT_PRODUCT(
c.m_contactNormal2.mVec128, bodyB.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(
c.m_relpos2CrossNormal.mVec128, bodyB.internalGetDeltaAngularVelocity().mVec128));
190 deltaImpulse = FMNADD(deltaVel1Dotn, tmp, deltaImpulse);
191 deltaImpulse = FMNADD(deltaVel2Dotn, tmp, deltaImpulse);
192 tmp = _mm_add_ps(
c.m_appliedImpulse, deltaImpulse);
193 const __m128 maskLower = _mm_cmpgt_ps(tmp, lowerLimit);
194 const __m128 maskUpper = _mm_cmpgt_ps(upperLimit, tmp);
195 deltaImpulse = _mm_blendv_ps(_mm_sub_ps(lowerLimit,
c.m_appliedImpulse), _mm_blendv_ps(_mm_sub_ps(upperLimit,
c.m_appliedImpulse), deltaImpulse, maskUpper), maskLower);
196 c.m_appliedImpulse = _mm_blendv_ps(lowerLimit, _mm_blendv_ps(upperLimit, tmp, maskUpper), maskLower);
197 bodyA.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(
c.m_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128), deltaImpulse, bodyA.internalGetDeltaLinearVelocity().mVec128);
198 bodyA.internalGetDeltaAngularVelocity().mVec128 = FMADD(
c.m_angularComponentA.mVec128, deltaImpulse, bodyA.internalGetDeltaAngularVelocity().mVec128);
199 bodyB.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(
c.m_contactNormal2.mVec128, bodyB.internalGetInvMass().mVec128), deltaImpulse, bodyB.internalGetDeltaLinearVelocity().mVec128);
200 bodyB.internalGetDeltaAngularVelocity().mVec128 = FMADD(
c.m_angularComponentB.mVec128, deltaImpulse, bodyB.internalGetDeltaAngularVelocity().mVec128);
202 return deltaImp.m_floats[0] * (1. /
c.m_jacDiagABInv);
204 return gResolveSingleConstraintRowGeneric_sse2(bodyA, bodyB,
c);
210 __m128 cpAppliedImp = _mm_set1_ps(
c.m_appliedImpulse);
211 __m128 lowerLimit1 = _mm_set1_ps(
c.m_lowerLimit);
212 __m128 upperLimit1 = _mm_set1_ps(
c.m_upperLimit);
213 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)));
214 __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(
c.m_contactNormal1.mVec128, bodyA.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(
c.m_relpos1CrossNormal.mVec128, bodyA.internalGetDeltaAngularVelocity().mVec128));
215 __m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(
c.m_contactNormal2.mVec128, bodyB.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(
c.m_relpos2CrossNormal.mVec128, bodyB.internalGetDeltaAngularVelocity().mVec128));
216 deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel1Dotn, _mm_set1_ps(
c.m_jacDiagABInv)));
217 deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel2Dotn, _mm_set1_ps(
c.m_jacDiagABInv)));
220 resultLowerLess = _mm_cmplt_ps(
sum, lowerLimit1);
221 resultUpperLess = _mm_cmplt_ps(
sum, upperLimit1);
222 __m128 lowMinApplied = _mm_sub_ps(lowerLimit1, cpAppliedImp);
223 deltaImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse));
224 c.m_appliedImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess,
sum));
225 __m128 linearComponentA = _mm_mul_ps(
c.m_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128);
226 __m128 linearComponentB = _mm_mul_ps(
c.m_contactNormal2.mVec128, bodyB.internalGetInvMass().mVec128);
227 __m128 impulseMagnitude = deltaImpulse;
228 bodyA.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(bodyA.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude));
229 bodyA.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(bodyA.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(
c.m_angularComponentA.mVec128, impulseMagnitude));
230 bodyB.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(bodyB.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude));
231 bodyB.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(bodyB.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(
c.m_angularComponentB.mVec128, impulseMagnitude));
232 return deltaImpulse.m_floats[0] /
c.m_jacDiagABInv;
239 __m128 tmp = _mm_set_ps1(
c.m_jacDiagABInv);
240 __m128 deltaImpulse = _mm_set_ps1(
c.m_rhs -
btScalar(
c.m_appliedImpulse) *
c.m_cfm);
241 const __m128 lowerLimit = _mm_set_ps1(
c.m_lowerLimit);
242 const __m128 deltaVel1Dotn = _mm_add_ps(DOT_PRODUCT(
c.m_contactNormal1.mVec128, bodyA.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(
c.m_relpos1CrossNormal.mVec128, bodyA.internalGetDeltaAngularVelocity().mVec128));
243 const __m128 deltaVel2Dotn = _mm_add_ps(DOT_PRODUCT(
c.m_contactNormal2.mVec128, bodyB.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(
c.m_relpos2CrossNormal.mVec128, bodyB.internalGetDeltaAngularVelocity().mVec128));
244 deltaImpulse = FMNADD(deltaVel1Dotn, tmp, deltaImpulse);
245 deltaImpulse = FMNADD(deltaVel2Dotn, tmp, deltaImpulse);
246 tmp = _mm_add_ps(
c.m_appliedImpulse, deltaImpulse);
247 const __m128
mask = _mm_cmpgt_ps(tmp, lowerLimit);
248 deltaImpulse = _mm_blendv_ps(_mm_sub_ps(lowerLimit,
c.m_appliedImpulse), deltaImpulse,
mask);
249 c.m_appliedImpulse = _mm_blendv_ps(lowerLimit, tmp,
mask);
250 bodyA.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(
c.m_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128), deltaImpulse, bodyA.internalGetDeltaLinearVelocity().mVec128);
251 bodyA.internalGetDeltaAngularVelocity().mVec128 = FMADD(
c.m_angularComponentA.mVec128, deltaImpulse, bodyA.internalGetDeltaAngularVelocity().mVec128);
252 bodyB.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(
c.m_contactNormal2.mVec128, bodyB.internalGetInvMass().mVec128), deltaImpulse, bodyB.internalGetDeltaLinearVelocity().mVec128);
253 bodyB.internalGetDeltaAngularVelocity().mVec128 = FMADD(
c.m_angularComponentB.mVec128, deltaImpulse, bodyB.internalGetDeltaAngularVelocity().mVec128);
255 return deltaImp.m_floats[0] * (1. /
c.m_jacDiagABInv);
257 return gResolveSingleConstraintRowLowerLimit_sse2(bodyA, bodyB,
c);
291 if (
c.m_rhsPenetration)
294 deltaImpulse =
c.m_rhsPenetration -
btScalar(
c.m_appliedPushImpulse) *
c.m_cfm;
295 const btScalar deltaVel1Dotn =
c.m_contactNormal1.dot(bodyA.internalGetPushVelocity()) +
c.m_relpos1CrossNormal.dot(bodyA.internalGetTurnVelocity());
296 const btScalar deltaVel2Dotn =
c.m_contactNormal2.dot(bodyB.internalGetPushVelocity()) +
c.m_relpos2CrossNormal.dot(bodyB.internalGetTurnVelocity());
298 deltaImpulse -= deltaVel1Dotn *
c.m_jacDiagABInv;
299 deltaImpulse -= deltaVel2Dotn *
c.m_jacDiagABInv;
301 if (
sum <
c.m_lowerLimit)
303 deltaImpulse =
c.m_lowerLimit -
c.m_appliedPushImpulse;
304 c.m_appliedPushImpulse =
c.m_lowerLimit;
308 c.m_appliedPushImpulse =
sum;
310 bodyA.internalApplyPushImpulse(
c.m_contactNormal1 * bodyA.internalGetInvMass(),
c.m_angularComponentA, deltaImpulse);
311 bodyB.internalApplyPushImpulse(
c.m_contactNormal2 * bodyB.internalGetInvMass(),
c.m_angularComponentB, deltaImpulse);
313 return deltaImpulse * (1. /
c.m_jacDiagABInv);
319 if (!
c.m_rhsPenetration)
324 __m128 cpAppliedImp = _mm_set1_ps(
c.m_appliedPushImpulse);
325 __m128 lowerLimit1 = _mm_set1_ps(
c.m_lowerLimit);
326 __m128 upperLimit1 = _mm_set1_ps(
c.m_upperLimit);
327 __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)));
328 __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(
c.m_contactNormal1.mVec128, bodyA.internalGetPushVelocity().mVec128), btSimdDot3(
c.m_relpos1CrossNormal.mVec128, bodyA.internalGetTurnVelocity().mVec128));
329 __m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(
c.m_contactNormal2.mVec128, bodyB.internalGetPushVelocity().mVec128), btSimdDot3(
c.m_relpos2CrossNormal.mVec128, bodyB.internalGetTurnVelocity().mVec128));
330 deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel1Dotn, _mm_set1_ps(
c.m_jacDiagABInv)));
331 deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel2Dotn, _mm_set1_ps(
c.m_jacDiagABInv)));
334 resultLowerLess = _mm_cmplt_ps(
sum, lowerLimit1);
335 resultUpperLess = _mm_cmplt_ps(
sum, upperLimit1);
336 __m128 lowMinApplied = _mm_sub_ps(lowerLimit1, cpAppliedImp);
337 deltaImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse));
338 c.m_appliedPushImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess,
sum));
339 __m128 linearComponentA = _mm_mul_ps(
c.m_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128);
340 __m128 linearComponentB = _mm_mul_ps(
c.m_contactNormal2.mVec128, bodyB.internalGetInvMass().mVec128);
341 __m128 impulseMagnitude = deltaImpulse;
342 bodyA.internalGetPushVelocity().mVec128 = _mm_add_ps(bodyA.internalGetPushVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude));
343 bodyA.internalGetTurnVelocity().mVec128 = _mm_add_ps(bodyA.internalGetTurnVelocity().mVec128, _mm_mul_ps(
c.m_angularComponentA.mVec128, impulseMagnitude));
344 bodyB.internalGetPushVelocity().mVec128 = _mm_add_ps(bodyB.internalGetPushVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude));
345 bodyB.internalGetTurnVelocity().mVec128 = _mm_add_ps(bodyB.internalGetTurnVelocity().mVec128, _mm_mul_ps(
c.m_angularComponentB.mVec128, impulseMagnitude));
347 return deltaImp.m_floats[0] * (1. /
c.m_jacDiagABInv);
402 return gResolveSingleConstraintRowGeneric_sse2;
406 return gResolveSingleConstraintRowLowerLimit_sse2;
411 return gResolveSingleConstraintRowGeneric_sse4_1_fma3;
415 return gResolveSingleConstraintRowLowerLimit_sse4_1_fma3;
430 const unsigned long un =
static_cast<unsigned long>(n);
435 if (un <= 0x00010000UL)
438 if (un <= 0x00000100UL)
441 if (un <= 0x00000010UL)
444 if (un <= 0x00000004UL)
447 if (un <= 0x00000002UL)
456 return (
int)(
r % un);
463 solverBody->internalGetDeltaLinearVelocity().setValue(0.f, 0.f, 0.f);
464 solverBody->internalGetDeltaAngularVelocity().setValue(0.f, 0.f, 0.f);
465 solverBody->internalGetPushVelocity().setValue(0.f, 0.f, 0.f);
466 solverBody->internalGetTurnVelocity().setValue(0.f, 0.f, 0.f);
470 solverBody->m_worldTransform = rb->getWorldTransform();
472 solverBody->m_originalBody = rb;
482 solverBody->m_worldTransform.setIdentity();
483 solverBody->internalSetInvMass(
btVector3(0, 0, 0));
484 solverBody->m_originalBody = 0;
485 solverBody->m_angularFactor.setValue(1, 1, 1);
486 solverBody->m_linearFactor.setValue(1, 1, 1);
487 solverBody->m_linearVelocity.setValue(0, 0, 0);
488 solverBody->m_angularVelocity.setValue(0, 0, 0);
489 solverBody->m_externalForceImpulse.setValue(0, 0, 0);
490 solverBody->m_externalTorqueImpulse.setValue(0, 0, 0);
497 if (
btFabs(rel_vel) < velocityThreshold)
500 btScalar rest = restitution * -rel_vel;
506 if (colObj && colObj->hasAnisotropicFriction(frictionMode))
509 btVector3 loc_lateral = frictionDirection * colObj->getWorldTransform().getBasis();
510 const btVector3& friction_scaling = colObj->getAnisotropicFriction();
512 loc_lateral *= friction_scaling;
514 frictionDirection = colObj->getWorldTransform().getBasis() * loc_lateral;
518 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,
const btContactSolverInfo&
infoGlobal,
btScalar desiredVelocity,
btScalar cfmSlip)
520 btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
521 btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
523 btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody;
524 btRigidBody* bodyA = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody;
526 solverConstraint.m_solverBodyIdA = solverBodyIdA;
527 solverConstraint.m_solverBodyIdB = solverBodyIdB;
530 solverConstraint.m_originalContactPoint = 0;
532 solverConstraint.m_appliedImpulse = 0.f;
533 solverConstraint.m_appliedPushImpulse = 0.f;
537 solverConstraint.m_contactNormal1 = normalAxis;
538 btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal1);
539 solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
544 solverConstraint.m_contactNormal1.setZero();
545 solverConstraint.m_relpos1CrossNormal.setZero();
546 solverConstraint.m_angularComponentA.setZero();
551 solverConstraint.m_contactNormal2 = -normalAxis;
552 btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal2);
553 solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
558 solverConstraint.m_contactNormal2.setZero();
559 solverConstraint.m_relpos2CrossNormal.setZero();
560 solverConstraint.m_angularComponentB.setZero();
569 vec = (solverConstraint.m_angularComponentA).
cross(rel_pos1);
570 denom0 = body0->
getInvMass() + normalAxis.dot(vec);
574 vec = (-solverConstraint.m_angularComponentB).
cross(rel_pos2);
575 denom1 = bodyA->
getInvMass() + normalAxis.dot(vec);
577 btScalar denom = relaxation / (denom0 + denom1);
578 solverConstraint.m_jacDiagABInv = denom;
583 btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0 ? solverBodyA.m_linearVelocity + solverBodyA.m_externalForceImpulse :
btVector3(0, 0, 0)) + solverConstraint.m_relpos1CrossNormal.dot(body0 ? solverBodyA.m_angularVelocity :
btVector3(0, 0, 0));
584 btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(bodyA ? solverBodyB.m_linearVelocity + solverBodyB.m_externalForceImpulse :
btVector3(0, 0, 0)) + solverConstraint.m_relpos2CrossNormal.dot(bodyA ? solverBodyB.m_angularVelocity :
btVector3(0, 0, 0));
586 rel_vel = vel1Dotn + vel2Dotn;
590 btScalar velocityError = desiredVelocity - rel_vel;
591 btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
599 penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv;
602 solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
603 solverConstraint.m_rhsPenetration = 0.f;
604 solverConstraint.m_cfm = cfmSlip;
605 solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
606 solverConstraint.m_upperLimit = solverConstraint.m_friction;
610 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,
const btContactSolverInfo&
infoGlobal,
btScalar desiredVelocity,
btScalar cfmSlip)
613 solverConstraint.m_frictionIndex = frictionIndex;
615 colObj0, colObj1, relaxation,
infoGlobal, desiredVelocity, cfmSlip);
616 return solverConstraint;
627 solverConstraint.m_contactNormal1 = normalAxis;
628 solverConstraint.m_contactNormal2 = -normalAxis;
629 btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
630 btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
632 btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody;
633 btRigidBody* bodyA = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody;
635 solverConstraint.m_solverBodyIdA = solverBodyIdA;
636 solverConstraint.m_solverBodyIdB = solverBodyIdB;
638 solverConstraint.m_friction = combinedTorsionalFriction;
639 solverConstraint.m_originalContactPoint = 0;
641 solverConstraint.m_appliedImpulse = 0.f;
642 solverConstraint.m_appliedPushImpulse = 0.f;
646 solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
651 solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
659 sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
660 sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
661 solverConstraint.m_jacDiagABInv =
btScalar(1.) /
sum;
666 btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0 ? solverBodyA.m_linearVelocity + solverBodyA.m_externalForceImpulse :
btVector3(0, 0, 0)) + solverConstraint.m_relpos1CrossNormal.dot(body0 ? solverBodyA.m_angularVelocity :
btVector3(0, 0, 0));
667 btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(bodyA ? solverBodyB.m_linearVelocity + solverBodyB.m_externalForceImpulse :
btVector3(0, 0, 0)) + solverConstraint.m_relpos2CrossNormal.dot(bodyA ? solverBodyB.m_angularVelocity :
btVector3(0, 0, 0));
669 rel_vel = vel1Dotn + vel2Dotn;
675 solverConstraint.m_rhs = velocityImpulse;
676 solverConstraint.m_cfm = cfmSlip;
677 solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
678 solverConstraint.m_upperLimit = solverConstraint.m_friction;
682 btSolverConstraint&
btSequentialImpulseConstraintSolver::addTorsionalFrictionConstraint(
const btVector3& normalAxis,
int solverBodyIdA,
int solverBodyIdB,
int frictionIndex,
btManifoldPoint& cp,
btScalar combinedTorsionalFriction,
const btVector3& rel_pos1,
const btVector3& rel_pos2,
btCollisionObject* colObj0,
btCollisionObject* colObj1,
btScalar relaxation,
btScalar desiredVelocity,
btScalar cfmSlip)
685 solverConstraint.m_frictionIndex = frictionIndex;
687 colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
688 return solverConstraint;
694 int solverBodyId = -1;
696 const bool isStaticOrKinematic = body.isStaticOrKinematicObject();
697 const bool isKinematic = body.isKinematicObject();
698 if (isRigidBodyType && !isStaticOrKinematic)
702 solverBodyId = body.getCompanionId();
703 if (solverBodyId < 0)
705 solverBodyId = m_tmpSolverBodyPool.size();
706 btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
708 body.setCompanionId(solverBodyId);
711 else if (isRigidBodyType && isKinematic)
720 int uniqueId = body.getWorldArrayIndex();
721 const int INVALID_SOLVER_BODY_ID = -1;
728 if (solverBodyId == INVALID_SOLVER_BODY_ID)
731 solverBodyId = m_tmpSolverBodyPool.
size();
732 btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
741 if (!isMultiBodyType)
743 btAssert(body.isStaticOrKinematicObject());
755 btAssert(solverBodyId >= 0 && solverBodyId < m_tmpSolverBodyPool.size());
759 int solverBodyIdA = -1;
761 if (body.getCompanionId() >= 0)
764 solverBodyIdA = body.getCompanionId();
765 btAssert(solverBodyIdA < m_tmpSolverBodyPool.size());
771 if (rb && (rb->
getInvMass() || rb->isKinematicObject()))
773 solverBodyIdA = m_tmpSolverBodyPool.size();
774 btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
776 body.setCompanionId(solverBodyIdA);
791 return solverBodyIdA;
797 int solverBodyIdA,
int solverBodyIdB,
805 btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA];
806 btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB];
854 #ifdef COMPUTE_IMPULSE_DENOM
863 vec = (solverConstraint.m_angularComponentA).
cross(rel_pos1);
868 vec = (-solverConstraint.m_angularComponentB).
cross(rel_pos2);
873 btScalar denom = relaxation / (denom0 + denom1 + cfm);
874 solverConstraint.m_jacDiagABInv = denom;
880 solverConstraint.m_relpos1CrossNormal = torqueAxis0;
884 solverConstraint.m_contactNormal1.setZero();
885 solverConstraint.m_relpos1CrossNormal.setZero();
890 solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
894 solverConstraint.m_contactNormal2.setZero();
895 solverConstraint.m_relpos2CrossNormal.setZero();
925 bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1 * bodyA->internalGetInvMass(), solverConstraint.m_angularComponentA, solverConstraint.m_appliedImpulse);
927 bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2 * bodyB->internalGetInvMass(), -solverConstraint.m_angularComponentB, -(
btScalar)solverConstraint.m_appliedImpulse);
931 solverConstraint.m_appliedImpulse = 0.f;
934 solverConstraint.m_appliedPushImpulse = 0.f;
937 btVector3 externalForceImpulseA = bodyA->m_originalBody ? bodyA->m_externalForceImpulse :
btVector3(0, 0, 0);
938 btVector3 externalTorqueImpulseA = bodyA->m_originalBody ? bodyA->m_externalTorqueImpulse :
btVector3(0, 0, 0);
939 btVector3 externalForceImpulseB = bodyB->m_originalBody ? bodyB->m_externalForceImpulse :
btVector3(0, 0, 0);
940 btVector3 externalTorqueImpulseB = bodyB->m_originalBody ? bodyB->m_externalTorqueImpulse :
btVector3(0, 0, 0);
942 btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(bodyA->m_linearVelocity + externalForceImpulseA) + solverConstraint.m_relpos1CrossNormal.dot(bodyA->m_angularVelocity + externalTorqueImpulseA);
943 btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(bodyB->m_linearVelocity + externalForceImpulseB) + solverConstraint.m_relpos2CrossNormal.dot(bodyB->m_angularVelocity + externalTorqueImpulseB);
944 btScalar rel_vel = vel1Dotn + vel2Dotn;
947 btScalar velocityError = restitution - rel_vel;
953 velocityError -= penetration * invTimeStep;
957 positionalError = -penetration * erp * invTimeStep;
960 btScalar penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv;
961 btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
966 solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
967 solverConstraint.m_rhsPenetration = 0.f;
972 solverConstraint.m_rhs = velocityImpulse;
973 solverConstraint.m_rhsPenetration = penetrationImpulse;
975 solverConstraint.m_cfm = cfm * solverConstraint.m_jacDiagABInv;
976 solverConstraint.m_lowerLimit = 0;
977 solverConstraint.m_upperLimit = 1e10f;
982 int solverBodyIdA,
int solverBodyIdB,
988 frictionConstraint1.m_appliedImpulse = 0.f;
995 frictionConstraint2.m_appliedImpulse = 0.f;
1012 btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverBodyIdA];
1013 btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverBodyIdB];
1016 if (!solverBodyA || (solverBodyA->m_invMass.fuzzyZero() && (!solverBodyB || solverBodyB->m_invMass.fuzzyZero())))
1019 int rollingFriction = 1;
1020 for (
int j = 0; j < manifold->getNumContacts(); j++)
1024 if (cp.
getDistance() <= manifold->getContactProcessingThreshold())
1032 solverConstraint.m_solverBodyIdA = solverBodyIdA;
1033 solverConstraint.m_solverBodyIdB = solverBodyIdB;
1035 solverConstraint.m_originalContactPoint = &cp;
1040 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
1041 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
1046 solverBodyA->getVelocityInLocalPointNoDelta(rel_pos1, vel1);
1047 solverBodyB->getVelocityInLocalPointNoDelta(rel_pos2, vel2);
1061 addTorsionalFrictionConstraint(cp.
m_normalWorldOnB, solverBodyIdA, solverBodyIdB, frictionIndex, cp, cp.
m_combinedSpinningFriction, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
1071 if (axis0.length() > 0.001)
1074 if (axis1.length() > 0.001)
1105 addFrictionConstraint(cp.
m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation,
infoGlobal);
1113 addFrictionConstraint(cp.
m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation,
infoGlobal);
1122 addFrictionConstraint(cp.
m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation,
infoGlobal);
1128 addFrictionConstraint(cp.
m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation,
infoGlobal);
1139 addFrictionConstraint(cp.
m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation,
infoGlobal, cp.
m_contactMotion1, cp.
m_frictionCFM);
1142 addFrictionConstraint(cp.
m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation,
infoGlobal, cp.
m_contactMotion2, cp.
m_frictionCFM);
1164 const btTypedConstraint::btConstraintInfo1& info1,
1169 const btRigidBody& rbA = constraint->getRigidBodyA();
1172 const btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
1173 const btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
1175 int overrideNumSolverIterations = constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() :
infoGlobal.
m_numIterations;
1179 for (
int j = 0; j < info1.m_numConstraintRows; j++)
1184 currentConstraintRow[j].m_appliedImpulse = 0.f;
1185 currentConstraintRow[j].m_appliedPushImpulse = 0.f;
1186 currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA;
1187 currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB;
1188 currentConstraintRow[j].m_overrideNumSolverIterations = overrideNumSolverIterations;
1192 btAssert(bodyAPtr->getDeltaLinearVelocity().isZero());
1193 btAssert(bodyAPtr->getDeltaAngularVelocity().isZero());
1194 btAssert(bodyAPtr->getPushVelocity().isZero());
1195 btAssert(bodyAPtr->getTurnVelocity().isZero());
1196 btAssert(bodyBPtr->getDeltaLinearVelocity().isZero());
1197 btAssert(bodyBPtr->getDeltaAngularVelocity().isZero());
1198 btAssert(bodyBPtr->getPushVelocity().isZero());
1199 btAssert(bodyBPtr->getTurnVelocity().isZero());
1209 btTypedConstraint::btConstraintInfo2 info2;
1212 info2.m_J1linearAxis = currentConstraintRow->m_contactNormal1;
1213 info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal;
1214 info2.m_J2linearAxis = currentConstraintRow->m_contactNormal2;
1215 info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal;
1219 info2.m_constraintError = ¤tConstraintRow->m_rhs;
1222 info2.cfm = ¤tConstraintRow->m_cfm;
1223 info2.m_lowerLimit = ¤tConstraintRow->m_lowerLimit;
1224 info2.m_upperLimit = ¤tConstraintRow->m_upperLimit;
1226 constraint->getInfo2(&info2);
1229 for (
int j = 0; j < info1.m_numConstraintRows; j++)
1233 if (solverConstraint.m_upperLimit >= constraint->getBreakingImpulseThreshold())
1235 solverConstraint.m_upperLimit = constraint->getBreakingImpulseThreshold();
1238 if (solverConstraint.m_lowerLimit <= -constraint->getBreakingImpulseThreshold())
1240 solverConstraint.m_lowerLimit = -constraint->getBreakingImpulseThreshold();
1243 solverConstraint.m_originalContactPoint = constraint;
1246 const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
1247 solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld() * ftorqueAxis1 * constraint->getRigidBodyA().getAngularFactor();
1250 const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal;
1251 solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld() * ftorqueAxis2 * constraint->getRigidBodyB().getAngularFactor();
1260 btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal1);
1261 sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
1262 sum += iMJlB.dot(solverConstraint.m_contactNormal2);
1263 sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
1267 solverConstraint.m_jacDiagABInv = fsum >
SIMD_EPSILON ? sorRelaxation /
sum : 0.f;
1272 btVector3 externalForceImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalForceImpulse :
btVector3(0, 0, 0);
1273 btVector3 externalTorqueImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalTorqueImpulse :
btVector3(0, 0, 0);
1275 btVector3 externalForceImpulseB = bodyBPtr->m_originalBody ? bodyBPtr->m_externalForceImpulse :
btVector3(0, 0, 0);
1276 btVector3 externalTorqueImpulseB = bodyBPtr->m_originalBody ? bodyBPtr->m_externalTorqueImpulse :
btVector3(0, 0, 0);
1282 rel_vel = vel1Dotn + vel2Dotn;
1284 btScalar positionalError = solverConstraint.m_rhs;
1285 btScalar velocityError = restitution - rel_vel * info2.m_damping;
1286 btScalar penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv;
1287 btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
1288 solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
1289 solverConstraint.m_appliedImpulse = 0.f;
1300 constraint->buildJacobian();
1301 constraint->internalSetAppliedImpulse(0.0f);
1304 int totalNumRows = 0;
1314 fb->m_appliedForceBodyA.setZero();
1315 fb->m_appliedTorqueBodyA.setZero();
1316 fb->m_appliedForceBodyB.setZero();
1317 fb->m_appliedTorqueBodyB.setZero();
1326 info1.m_numConstraintRows = 0;
1329 totalNumRows += info1.m_numConstraintRows;
1340 if (info1.m_numConstraintRows)
1342 btAssert(currentRow < totalNumRows);
1354 currentRow += info1.m_numConstraintRows;
1363 bodies[i]->setCompanionId(-1);
1369 m_tmpSolverBodyPool.reserve(
numBodies + 1);
1370 m_tmpSolverBodyPool.resize(0);
1382 btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
1392 solverBody.m_externalTorqueImpulse += gyroForce;
1397 solverBody.m_externalTorqueImpulse += gyroForce;
1419 #ifdef BT_ADDITIONAL_DEBUG
1424 if (constraint->isEnabled())
1426 if (!constraint->getRigidBodyA().isStaticOrKinematicObject())
1431 if (&constraint->getRigidBodyA() == bodies[
b])
1439 if (!constraint->getRigidBodyB().isStaticOrKinematicObject())
1444 if (&constraint->getRigidBodyB() == bodies[
b])
1509 for (i = 0; i < numNonContactPool; i++)
1513 for (i = 0; i < numConstraintPool; i++)
1517 for (i = 0; i < numFrictionPool; i++)
1529 btScalar leastSquaresResidual = 0.f;
1539 for (
int j = 0; j < numNonContactPool; ++j)
1550 for (
int j = 0; j < numConstraintPool; ++j)
1558 for (
int j = 0; j < numFrictionPool; ++j)
1573 if (iteration < constraint.m_overrideNumSolverIterations)
1576 leastSquaresResidual =
btMax(leastSquaresResidual, residual * residual);
1600 for (
int c = 0;
c < numPoolConstraints;
c++)
1607 leastSquaresResidual =
btMax(leastSquaresResidual, residual * residual);
1609 totalImpulse = solveManifold.m_appliedImpulse;
1611 bool applyFriction =
true;
1619 solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
1620 solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
1623 leastSquaresResidual =
btMax(leastSquaresResidual, residual * residual);
1633 solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
1634 solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
1637 leastSquaresResidual =
btMax(leastSquaresResidual, residual * residual);
1649 for (j = 0; j < numPoolConstraints; j++)
1653 leastSquaresResidual =
btMax(leastSquaresResidual, residual * residual);
1659 for (j = 0; j < numFrictionPoolConstraints; j++)
1666 solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
1667 solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
1670 leastSquaresResidual =
btMax(leastSquaresResidual, residual * residual);
1676 for (
int j = 0; j < numRollingFrictionPoolConstraints; j++)
1682 btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction * totalImpulse;
1683 if (rollingFrictionMagnitude > rollingFrictionConstraint.m_friction)
1684 rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
1686 rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
1687 rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
1689 btScalar residual =
resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA], m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB], rollingFrictionConstraint);
1690 leastSquaresResidual =
btMax(leastSquaresResidual, residual * residual);
1694 return leastSquaresResidual;
1699 BT_PROFILE(
"solveGroupCacheFriendlySplitImpulseIterations");
1706 btScalar leastSquaresResidual = 0.f;
1710 for (j = 0; j < numPoolConstraints; j++)
1715 leastSquaresResidual =
btMax(leastSquaresResidual, residual * residual);
1718 if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration >= (
infoGlobal.
m_numIterations - 1))
1720 #ifdef VERBOSE_RESIDUAL_PRINTF
1721 printf(
"residual = %f at iteration #%d\n", leastSquaresResidual, iteration);
1732 BT_PROFILE(
"solveGroupCacheFriendlyIterations");
1740 for (
int iteration = 0; iteration < maxIterations; iteration++)
1747 #ifdef VERBOSE_RESIDUAL_PRINTF
1767 for (
int j = iBegin; j < iEnd; j++)
1787 for (
int j = iBegin; j < iEnd; j++)
1794 fb->m_appliedForceBodyA += solverConstr.m_contactNormal1 * solverConstr.m_appliedImpulse * constr->getRigidBodyA().getLinearFactor() /
infoGlobal.
m_timeStep;
1795 fb->m_appliedForceBodyB += solverConstr.m_contactNormal2 * solverConstr.m_appliedImpulse * constr->getRigidBodyB().getLinearFactor() /
infoGlobal.
m_timeStep;
1796 fb->m_appliedTorqueBodyA += solverConstr.m_relpos1CrossNormal * constr->getRigidBodyA().getAngularFactor() * solverConstr.m_appliedImpulse /
infoGlobal.
m_timeStep;
1797 fb->m_appliedTorqueBodyB += solverConstr.m_relpos2CrossNormal * constr->getRigidBodyB().getAngularFactor() * solverConstr.m_appliedImpulse /
infoGlobal.
m_timeStep;
1800 constr->internalSetAppliedImpulse(solverConstr.m_appliedImpulse);
1801 if (
btFabs(solverConstr.m_appliedImpulse) >= constr->getBreakingImpulseThreshold())
1803 constr->setEnabled(
false);
1810 for (
int i = iBegin; i < iEnd; i++)
1812 btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody;
1818 m_tmpSolverBodyPool[i].writebackVelocity();
1824 m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(
1829 m_tmpSolverBodyPool[i].m_originalBody->setWorldTransform(m_tmpSolverBodyPool[i].
m_worldTransform);
1831 m_tmpSolverBodyPool[i].m_originalBody->setCompanionId(-1);
1853 m_tmpSolverBodyPool.resizeNoInitialize(0);
_GL_VOID GLfloat value _GL_VOID_RET _GL_VOID const GLuint GLboolean *residences _GL_BOOL_RET _GL_VOID GLsizei GLfloat GLfloat GLfloat GLfloat const GLubyte *bitmap _GL_VOID_RET _GL_VOID GLenum const void *lists _GL_VOID_RET _GL_VOID const GLdouble *equation _GL_VOID_RET _GL_VOID GLdouble GLdouble blue _GL_VOID_RET _GL_VOID GLfloat GLfloat blue _GL_VOID_RET _GL_VOID GLint GLint blue _GL_VOID_RET _GL_VOID GLshort GLshort blue _GL_VOID_RET _GL_VOID GLubyte GLubyte blue _GL_VOID_RET _GL_VOID GLuint GLuint blue _GL_VOID_RET _GL_VOID GLushort GLushort blue _GL_VOID_RET _GL_VOID GLbyte GLbyte GLbyte alpha _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble alpha _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat alpha _GL_VOID_RET _GL_VOID GLint GLint GLint alpha _GL_VOID_RET _GL_VOID GLshort GLshort GLshort alpha _GL_VOID_RET _GL_VOID GLubyte GLubyte GLubyte alpha _GL_VOID_RET _GL_VOID GLuint GLuint GLuint alpha _GL_VOID_RET _GL_VOID GLushort GLushort GLushort alpha _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLint GLsizei GLsizei GLenum type _GL_VOID_RET _GL_VOID GLsizei GLenum GLenum const void *pixels _GL_VOID_RET _GL_VOID const void *pointer _GL_VOID_RET _GL_VOID GLdouble v _GL_VOID_RET _GL_VOID GLfloat v _GL_VOID_RET _GL_VOID GLint GLint i2 _GL_VOID_RET _GL_VOID GLint j _GL_VOID_RET _GL_VOID GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble GLdouble GLdouble zFar _GL_VOID_RET _GL_UINT GLdouble *equation _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLenum GLfloat *v _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLfloat *values _GL_VOID_RET _GL_VOID GLushort *values _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLenum GLdouble *params _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_BOOL GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLushort pattern _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble u2 _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLdouble GLdouble v2 _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLdouble GLdouble nz _GL_VOID_RET _GL_VOID GLfloat GLfloat nz _GL_VOID_RET _GL_VOID GLint GLint nz _GL_VOID_RET _GL_VOID GLshort GLshort nz _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const GLfloat *values _GL_VOID_RET _GL_VOID GLsizei const GLushort *values _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID const GLuint const GLclampf *priorities _GL_VOID_RET _GL_VOID GLdouble y _GL_VOID_RET _GL_VOID GLfloat y _GL_VOID_RET _GL_VOID GLint y _GL_VOID_RET _GL_VOID GLshort y _GL_VOID_RET _GL_VOID GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLfloat GLfloat z _GL_VOID_RET _GL_VOID GLint GLint z _GL_VOID_RET _GL_VOID GLshort GLshort z _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble w _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat w _GL_VOID_RET _GL_VOID GLint GLint GLint w _GL_VOID_RET _GL_VOID GLshort GLshort GLshort w _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble y2 _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat y2 _GL_VOID_RET _GL_VOID GLint GLint GLint y2 _GL_VOID_RET _GL_VOID GLshort GLshort GLshort y2 _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLuint *buffer _GL_VOID_RET _GL_VOID GLdouble t _GL_VOID_RET _GL_VOID GLfloat t _GL_VOID_RET _GL_VOID GLint t _GL_VOID_RET _GL_VOID GLshort t _GL_VOID_RET _GL_VOID GLdouble GLdouble r _GL_VOID_RET _GL_VOID GLfloat GLfloat r _GL_VOID_RET _GL_VOID GLint GLint r _GL_VOID_RET _GL_VOID GLshort GLshort r _GL_VOID_RET _GL_VOID GLdouble GLdouble r
SIMD_FORCE_INLINE bool isStaticOrKinematicObject() const
@ CF_ANISOTROPIC_FRICTION
@ CF_ANISOTROPIC_ROLLING_FRICTION
const btRigidBody & getRigidBodyA() const
const btRigidBody & getRigidBodyB() const
btFixedConstraint btRigidBody & rbB
@ BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED
@ BT_CONTACT_FLAG_HAS_CONTACT_ERP
@ BT_CONTACT_FLAG_HAS_CONTACT_CFM
@ BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING
@ BT_CONTACT_FLAG_FRICTION_ANCHOR
SIMD_FORCE_INLINE const T & btMax(const T &a, const T &b)
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject **bodies, int numBodies, const btContactSolverInfo &infoGlobal)
virtual btScalar solveSingleIteration(int iteration, btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
SIMD_FORCE_INLINE const btCollisionObject * getBody1() const
SIMD_FORCE_INLINE const btCollisionObject * getBody0() const
@ BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY
@ BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT
@ BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
SIMD_FORCE_INLINE btScalar btFabs(btScalar x)
SIMD_FORCE_INLINE btScalar btSqrt(btScalar y)
btSequentialImpulseConstraintSolverMt int btPersistentManifold int btTypedConstraint ** constraints
btSequentialImpulseConstraintSolverMt int btPersistentManifold ** manifoldPtr
btSequentialImpulseConstraintSolverMt int btPersistentManifold int btTypedConstraint int numConstraints
btSequentialImpulseConstraintSolverMt int numBodies
btSequentialImpulseConstraintSolverMt int btPersistentManifold int btTypedConstraint int const btContactSolverInfo & infoGlobal
btSequentialImpulseConstraintSolverMt int btPersistentManifold int numManifolds
static btScalar gResolveSingleConstraintRowGeneric_scalar_reference(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &c)
static btScalar gResolveSingleConstraintRowLowerLimit_scalar_reference(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &c)
static btScalar gResolveSplitPenetrationImpulse_sse2(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &c)
int gNumSplitImpulseRecoveries
static btScalar gResolveSplitPenetrationImpulse_scalar_reference(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &c)
btSingleConstraintRowSolver getScalarConstraintRowSolverLowerLimit()
Various implementations of solving a single constraint row using an inequality (lower limit) constrai...
void initSolverBody(btSolverBody *solverBody, btCollisionObject *collisionObject, btScalar timeStep)
virtual void convertJoints(btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal)
btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverGeneric()
btSolverConstraint & 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, const btContactSolverInfo &infoGlobal, btScalar desiredVelocity=0., btScalar cfmSlip=0.)
void writeBackJoints(int iBegin, int iEnd, const btContactSolverInfo &infoGlobal)
btConstraintArray m_tmpSolverContactRollingFrictionConstraintPool
btAlignedObjectArray< int > m_orderTmpConstraintPool
btScalar(* btSingleConstraintRowSolver)(btSolverBody &, btSolverBody &, const btSolverConstraint &)
btAlignedObjectArray< int > m_orderNonContactConstraintPool
int m_maxOverrideNumSolverIterations
virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
virtual void convertContacts(btPersistentManifold **manifoldPtr, int numManifolds, const btContactSolverInfo &infoGlobal)
virtual ~btSequentialImpulseConstraintSolver()
void convertJoint(btSolverConstraint *currentConstraintRow, btTypedConstraint *constraint, const btTypedConstraint::btConstraintInfo1 &info1, int solverBodyIdA, int solverBodyIdB, const btContactSolverInfo &infoGlobal)
btAlignedObjectArray< int > m_kinematicBodyUniqueIdToSolverBodyTable
virtual btScalar solveGroup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifold, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &info, btIDebugDraw *debugDrawer, btDispatcher *dispatcher)
btSolverAnalyticsData m_analyticsData
btSolverConstraint & addTorsionalFrictionConstraint(const btVector3 &normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, btManifoldPoint &cp, btScalar torsionalFriction, const btVector3 &rel_pos1, const btVector3 &rel_pos2, btCollisionObject *colObj0, btCollisionObject *colObj1, btScalar relaxation, btScalar desiredVelocity=0, btScalar cfmSlip=0.f)
btScalar m_leastSquaresResidual
btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverLowerLimit()
btSingleConstraintRowSolver m_resolveSplitPenetrationImpulse
btSingleConstraintRowSolver getSSE2ConstraintRowSolverLowerLimit()
btScalar resolveSingleConstraintRowLowerLimit(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
void setupSolverFunctions(bool useSimd)
unsigned long m_btSeed2
m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
static void applyAnisotropicFriction(btCollisionObject *colObj, btVector3 &frictionDirection, int frictionMode)
btSingleConstraintRowSolver m_resolveSingleConstraintRowGeneric
btScalar resolveSingleConstraintRowGeneric(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
void setupTorsionalFrictionConstraint(btSolverConstraint &solverConstraint, const btVector3 &normalAxis, int solverBodyIdA, int solverBodyIdB, btManifoldPoint &cp, btScalar combinedTorsionalFriction, const btVector3 &rel_pos1, const btVector3 &rel_pos2, btCollisionObject *colObj0, btCollisionObject *colObj1, btScalar relaxation, btScalar desiredVelocity=0., btScalar cfmSlip=0.)
void convertContact(btPersistentManifold *manifold, const btContactSolverInfo &infoGlobal)
btScalar resolveSplitPenetrationImpulse(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
btSingleConstraintRowSolver getScalarConstraintRowSolverGeneric()
Various implementations of solving a single constraint row using a generic equality constraint,...
btSingleConstraintRowSolver getSSE2ConstraintRowSolverGeneric()
btAlignedObjectArray< int > m_orderFrictionConstraintPool
void writeBackBodies(int iBegin, int iEnd, const btContactSolverInfo &infoGlobal)
void 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, const btContactSolverInfo &infoGlobal, btScalar desiredVelocity=0., btScalar cfmSlip=0.)
btConstraintArray m_tmpSolverContactFrictionConstraintPool
btConstraintArray m_tmpSolverContactConstraintPool
btSequentialImpulseConstraintSolver()
btSingleConstraintRowSolver m_resolveSingleConstraintRowLowerLimit
virtual void convertBodies(btCollisionObject **bodies, int numBodies, const btContactSolverInfo &infoGlobal)
void setFrictionConstraintImpulse(btSolverConstraint &solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint &cp, const btContactSolverInfo &infoGlobal)
btScalar resolveSingleConstraintRowLowerLimitSIMD(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
int getOrInitSolverBody(btCollisionObject &body, btScalar timeStep)
btScalar restitutionCurve(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold)
btScalar resolveSingleConstraintRowGenericSIMD(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
void writeBackContacts(int iBegin, int iEnd, const btContactSolverInfo &infoGlobal)
btConstraintArray m_tmpSolverNonContactConstraintPool
void setupContactConstraint(btSolverConstraint &solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint &cp, const btContactSolverInfo &infoGlobal, btScalar &relaxation, const btVector3 &rel_pos1, const btVector3 &rel_pos2)
btAlignedObjectArray< btTypedConstraint::btConstraintInfo1 > m_tmpConstraintSizesPool
static T sum(const btAlignedObjectArray< T > &items)
btSolverBody
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
btVector3 m_externalTorqueImpulse
btVector3 m_externalForceImpulse
btTransform m_worldTransform
btVector3 m_angularVelocity
#define btSimdScalar
Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later,...
btVector3 m_linearVelocity
btSolverConstraint
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
btTypedConstraint(btTypedConstraintType type, btRigidBody &rbA)
SIMD_FORCE_INLINE void btPlaneSpace1(const T &n, T &p, T &q)
btVector3
btVector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-by...
SIMD_FORCE_INLINE void resizeNoInitialize(int newsize)
SIMD_FORCE_INLINE int size() const
return the number of elements in the array
SIMD_FORCE_INLINE void resize(int newsize, const T &fillData=T())
SIMD_FORCE_INLINE T & expandNonInitializing()
static int getCpuFeatures()
btScalar m_combinedSpinningFriction
btScalar m_combinedRollingFriction
btScalar getDistance() const
btScalar m_combinedContactStiffness1
const btVector3 & getPositionWorldOnB() const
btScalar m_combinedRestitution
btVector3 m_lateralFrictionDir2
btScalar m_combinedContactDamping1
btScalar m_appliedImpulseLateral2
btScalar m_appliedImpulse
btScalar m_appliedImpulseLateral1
const btVector3 & getPositionWorldOnA() const
btVector3 m_normalWorldOnB
btScalar m_combinedFriction
btScalar m_contactMotion2
btVector3 m_lateralFrictionDir1
btScalar m_contactMotion1
const btVector3 & getAngularVelocity() const
btVector3 getVelocityInLocalPoint(const btVector3 &rel_pos) const
const btVector3 & getLinearFactor() const
btScalar getInvMass() const
static const btRigidBody * upcast(const btCollisionObject *colObj)
const btVector3 & getTotalTorque() const
btVector3 computeGyroscopicImpulseImplicit_World(btScalar dt) const
perform implicit force computation in world space
btVector3 m_angularFactor
btVector3 computeGyroscopicImpulseImplicit_Body(btScalar step) const
perform implicit force computation in body space (inertial frame)
SIMD_FORCE_INLINE btScalar computeImpulseDenominator(const btVector3 &pos, const btVector3 &normal) const
const btVector3 & getLinearVelocity() const
const btVector3 & getTotalForce() const
const btMatrix3x3 & getInvInertiaTensorWorld() const
void setLinearVelocity(const btVector3 &lin_vel)
const btVector3 & getAngularFactor() const
btVector3 computeGyroscopicForceExplicit(btScalar maxGyroscopicForce) const
explicit version is best avoided, it gains energy
SyclQueue void void size_t num_bytes void
BLI_INLINE float fb(float length, float L)
ccl_gpu_kernel_postfix ccl_global float int int int int float bool reset
clear internal cached data and reset random seed
ccl_device_inline float4 mask(const int4 &mask, const float4 &a)
T dot(const vec_base< T, Size > &a, const vec_base< T, Size > &b)
vec_base< T, 3 > cross(const vec_base< T, 3 > &a, const vec_base< T, 3 > &b)
T distance(const T &a, const T &b)
static const pxr::TfToken b("b", pxr::TfToken::Immortal)
int m_numContactManifolds
double m_remainingLeastSquaresResidual