Bullet Collision Detection & Physics Library
btSequentialImpulseConstraintSolver.cpp
Go to the documentation of this file.
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 
23 
26 
27 //#include "btJacobianEntry.h"
28 #include "LinearMath/btMinMax.h"
30 #include <new>
32 #include "LinearMath/btQuickprof.h"
33 //#include "btSolverBody.h"
34 //#include "btSolverConstraint.h"
36 #include <string.h> //for memset
37 
39 
41 
42 //#define VERBOSE_RESIDUAL_PRINTF 1
46 {
47  btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse) * c.m_cfm;
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;
60  }
61  else if (sum > c.m_upperLimit)
62  {
63  deltaImpulse = c.m_upperLimit - c.m_appliedImpulse;
65  }
66  else
67  {
69  }
70 
73 
74  return deltaImpulse * (1. / c.m_jacDiagABInv);
75 }
76 
78 {
79  btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse) * c.m_cfm;
82 
83  deltaImpulse -= deltaVel1Dotn * c.m_jacDiagABInv;
84  deltaImpulse -= deltaVel2Dotn * c.m_jacDiagABInv;
85  const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
86  if (sum < c.m_lowerLimit)
87  {
88  deltaImpulse = c.m_lowerLimit - c.m_appliedImpulse;
90  }
91  else
92  {
94  }
97 
98  return deltaImpulse * (1. / c.m_jacDiagABInv);
99 }
100 
101 #ifdef USE_SIMD
102 #include <emmintrin.h>
103 
104 #define btVecSplat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e, e, e, e))
105 static inline __m128 btSimdDot3(__m128 vec0, __m128 vec1)
106 {
107  __m128 result = _mm_mul_ps(vec0, vec1);
108  return _mm_add_ps(btVecSplat(result, 0), _mm_add_ps(btVecSplat(result, 1), btVecSplat(result, 2)));
109 }
110 
111 #if defined(BT_ALLOW_SSE4)
112 #include <intrin.h>
113 
114 #define USE_FMA 1
115 #define USE_FMA3_INSTEAD_FMA4 1
116 #define USE_SSE4_DOT 1
117 
118 #define SSE4_DP(a, b) _mm_dp_ps(a, b, 0x7f)
119 #define SSE4_DP_FP(a, b) _mm_cvtss_f32(_mm_dp_ps(a, b, 0x7f))
120 
121 #if USE_SSE4_DOT
122 #define DOT_PRODUCT(a, b) SSE4_DP(a, b)
123 #else
124 #define DOT_PRODUCT(a, b) btSimdDot3(a, b)
125 #endif
126 
127 #if USE_FMA
128 #if USE_FMA3_INSTEAD_FMA4
129 // a*b + c
130 #define FMADD(a, b, c) _mm_fmadd_ps(a, b, c)
131 // -(a*b) + c
132 #define FMNADD(a, b, c) _mm_fnmadd_ps(a, b, c)
133 #else // USE_FMA3
134 // a*b + c
135 #define FMADD(a, b, c) _mm_macc_ps(a, b, c)
136 // -(a*b) + c
137 #define FMNADD(a, b, c) _mm_nmacc_ps(a, b, c)
138 #endif
139 #else // USE_FMA
140 // c + a*b
141 #define FMADD(a, b, c) _mm_add_ps(c, _mm_mul_ps(a, b))
142 // c - a*b
143 #define FMNADD(a, b, c) _mm_sub_ps(c, _mm_mul_ps(a, b))
144 #endif
145 #endif
146 
147 // Project Gauss Seidel or the equivalent Sequential Impulse
148 static btScalar gResolveSingleConstraintRowGeneric_sse2(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c)
149 {
150  __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
151  __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
152  __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
153  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)));
154  __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128, bodyA.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128, bodyA.internalGetDeltaAngularVelocity().mVec128));
155  __m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128, bodyB.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128, bodyB.internalGetDeltaAngularVelocity().mVec128));
156  deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel1Dotn, _mm_set1_ps(c.m_jacDiagABInv)));
157  deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel2Dotn, _mm_set1_ps(c.m_jacDiagABInv)));
158  btSimdScalar sum = _mm_add_ps(cpAppliedImp, deltaImpulse);
159  btSimdScalar resultLowerLess, resultUpperLess;
160  resultLowerLess = _mm_cmplt_ps(sum, lowerLimit1);
161  resultUpperLess = _mm_cmplt_ps(sum, upperLimit1);
162  __m128 lowMinApplied = _mm_sub_ps(lowerLimit1, cpAppliedImp);
163  deltaImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse));
164  c.m_appliedImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum));
165  __m128 upperMinApplied = _mm_sub_ps(upperLimit1, cpAppliedImp);
166  deltaImpulse = _mm_or_ps(_mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied));
167  c.m_appliedImpulse = _mm_or_ps(_mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1));
168  __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128);
169  __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal2).mVec128, bodyB.internalGetInvMass().mVec128);
170  __m128 impulseMagnitude = deltaImpulse;
171  bodyA.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(bodyA.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude));
172  bodyA.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(bodyA.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude));
173  bodyB.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(bodyB.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude));
174  bodyB.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(bodyB.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude));
175  return deltaImpulse.m_floats[0] / c.m_jacDiagABInv;
176 }
177 
178 // Enhanced version of gResolveSingleConstraintRowGeneric_sse2 with SSE4.1 and FMA3
179 static btScalar gResolveSingleConstraintRowGeneric_sse4_1_fma3(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c)
180 {
181 #if defined(BT_ALLOW_SSE4)
182  __m128 tmp = _mm_set_ps1(c.m_jacDiagABInv);
183  __m128 deltaImpulse = _mm_set_ps1(c.m_rhs - btScalar(c.m_appliedImpulse) * c.m_cfm);
184  const __m128 lowerLimit = _mm_set_ps1(c.m_lowerLimit);
185  const __m128 upperLimit = _mm_set_ps1(c.m_upperLimit);
186  const __m128 deltaVel1Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal1.mVec128, bodyA.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos1CrossNormal.mVec128, bodyA.internalGetDeltaAngularVelocity().mVec128));
187  const __m128 deltaVel2Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal2.mVec128, bodyB.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos2CrossNormal.mVec128, bodyB.internalGetDeltaAngularVelocity().mVec128));
188  deltaImpulse = FMNADD(deltaVel1Dotn, tmp, deltaImpulse);
189  deltaImpulse = FMNADD(deltaVel2Dotn, tmp, deltaImpulse);
190  tmp = _mm_add_ps(c.m_appliedImpulse, deltaImpulse); // sum
191  const __m128 maskLower = _mm_cmpgt_ps(tmp, lowerLimit);
192  const __m128 maskUpper = _mm_cmpgt_ps(upperLimit, tmp);
193  deltaImpulse = _mm_blendv_ps(_mm_sub_ps(lowerLimit, c.m_appliedImpulse), _mm_blendv_ps(_mm_sub_ps(upperLimit, c.m_appliedImpulse), deltaImpulse, maskUpper), maskLower);
194  c.m_appliedImpulse = _mm_blendv_ps(lowerLimit, _mm_blendv_ps(upperLimit, tmp, maskUpper), maskLower);
195  bodyA.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128), deltaImpulse, bodyA.internalGetDeltaLinearVelocity().mVec128);
196  bodyA.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentA.mVec128, deltaImpulse, bodyA.internalGetDeltaAngularVelocity().mVec128);
197  bodyB.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal2.mVec128, bodyB.internalGetInvMass().mVec128), deltaImpulse, bodyB.internalGetDeltaLinearVelocity().mVec128);
198  bodyB.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentB.mVec128, deltaImpulse, bodyB.internalGetDeltaAngularVelocity().mVec128);
199  btSimdScalar deltaImp = deltaImpulse;
200  return deltaImp.m_floats[0] * (1. / c.m_jacDiagABInv);
201 #else
202  return gResolveSingleConstraintRowGeneric_sse2(bodyA, bodyB, c);
203 #endif
204 }
205 
206 static btScalar gResolveSingleConstraintRowLowerLimit_sse2(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c)
207 {
208  __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
209  __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
210  __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
211  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)));
212  __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128, bodyA.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128, bodyA.internalGetDeltaAngularVelocity().mVec128));
213  __m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128, bodyB.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128, bodyB.internalGetDeltaAngularVelocity().mVec128));
214  deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel1Dotn, _mm_set1_ps(c.m_jacDiagABInv)));
215  deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel2Dotn, _mm_set1_ps(c.m_jacDiagABInv)));
216  btSimdScalar sum = _mm_add_ps(cpAppliedImp, deltaImpulse);
217  btSimdScalar resultLowerLess, resultUpperLess;
218  resultLowerLess = _mm_cmplt_ps(sum, lowerLimit1);
219  resultUpperLess = _mm_cmplt_ps(sum, upperLimit1);
220  __m128 lowMinApplied = _mm_sub_ps(lowerLimit1, cpAppliedImp);
221  deltaImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse));
222  c.m_appliedImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum));
223  __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128);
224  __m128 linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128, bodyB.internalGetInvMass().mVec128);
225  __m128 impulseMagnitude = deltaImpulse;
226  bodyA.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(bodyA.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude));
227  bodyA.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(bodyA.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude));
228  bodyB.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(bodyB.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude));
229  bodyB.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(bodyB.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude));
230  return deltaImpulse.m_floats[0] / c.m_jacDiagABInv;
231 }
232 
233 // Enhanced version of gResolveSingleConstraintRowGeneric_sse2 with SSE4.1 and FMA3
234 static btScalar gResolveSingleConstraintRowLowerLimit_sse4_1_fma3(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c)
235 {
236 #ifdef BT_ALLOW_SSE4
237  __m128 tmp = _mm_set_ps1(c.m_jacDiagABInv);
238  __m128 deltaImpulse = _mm_set_ps1(c.m_rhs - btScalar(c.m_appliedImpulse) * c.m_cfm);
239  const __m128 lowerLimit = _mm_set_ps1(c.m_lowerLimit);
240  const __m128 deltaVel1Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal1.mVec128, bodyA.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos1CrossNormal.mVec128, bodyA.internalGetDeltaAngularVelocity().mVec128));
241  const __m128 deltaVel2Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal2.mVec128, bodyB.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos2CrossNormal.mVec128, bodyB.internalGetDeltaAngularVelocity().mVec128));
242  deltaImpulse = FMNADD(deltaVel1Dotn, tmp, deltaImpulse);
243  deltaImpulse = FMNADD(deltaVel2Dotn, tmp, deltaImpulse);
244  tmp = _mm_add_ps(c.m_appliedImpulse, deltaImpulse);
245  const __m128 mask = _mm_cmpgt_ps(tmp, lowerLimit);
246  deltaImpulse = _mm_blendv_ps(_mm_sub_ps(lowerLimit, c.m_appliedImpulse), deltaImpulse, mask);
247  c.m_appliedImpulse = _mm_blendv_ps(lowerLimit, tmp, mask);
248  bodyA.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128), deltaImpulse, bodyA.internalGetDeltaLinearVelocity().mVec128);
249  bodyA.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentA.mVec128, deltaImpulse, bodyA.internalGetDeltaAngularVelocity().mVec128);
250  bodyB.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal2.mVec128, bodyB.internalGetInvMass().mVec128), deltaImpulse, bodyB.internalGetDeltaLinearVelocity().mVec128);
251  bodyB.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentB.mVec128, deltaImpulse, bodyB.internalGetDeltaAngularVelocity().mVec128);
252  btSimdScalar deltaImp = deltaImpulse;
253  return deltaImp.m_floats[0] * (1. / c.m_jacDiagABInv);
254 #else
255  return gResolveSingleConstraintRowLowerLimit_sse2(bodyA, bodyB, c);
256 #endif //BT_ALLOW_SSE4
257 }
258 
259 #endif //USE_SIMD
260 
262 {
263  return m_resolveSingleConstraintRowGeneric(bodyA, bodyB, c);
264 }
265 
266 // Project Gauss Seidel or the equivalent Sequential Impulse
268 {
269  return m_resolveSingleConstraintRowGeneric(bodyA, bodyB, c);
270 }
271 
273 {
274  return m_resolveSingleConstraintRowLowerLimit(bodyA, bodyB, c);
275 }
276 
278 {
279  return m_resolveSingleConstraintRowLowerLimit(bodyA, bodyB, c);
280 }
281 
283  btSolverBody& bodyA,
284  btSolverBody& bodyB,
285  const btSolverConstraint& c)
286 {
287  btScalar deltaImpulse = 0.f;
288 
289  if (c.m_rhsPenetration)
290  {
292  deltaImpulse = c.m_rhsPenetration - btScalar(c.m_appliedPushImpulse) * c.m_cfm;
295 
296  deltaImpulse -= deltaVel1Dotn * c.m_jacDiagABInv;
297  deltaImpulse -= deltaVel2Dotn * c.m_jacDiagABInv;
298  const btScalar sum = btScalar(c.m_appliedPushImpulse) + deltaImpulse;
299  if (sum < c.m_lowerLimit)
300  {
301  deltaImpulse = c.m_lowerLimit - c.m_appliedPushImpulse;
303  }
304  else
305  {
307  }
310  }
311  return deltaImpulse * (1. / c.m_jacDiagABInv);
312 }
313 
315 {
316 #ifdef USE_SIMD
317  if (!c.m_rhsPenetration)
318  return 0.f;
319 
321 
322  __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedPushImpulse);
323  __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
324  __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
325  __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)));
326  __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128, bodyA.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128, bodyA.internalGetTurnVelocity().mVec128));
327  __m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128, bodyB.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128, bodyB.internalGetTurnVelocity().mVec128));
328  deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel1Dotn, _mm_set1_ps(c.m_jacDiagABInv)));
329  deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel2Dotn, _mm_set1_ps(c.m_jacDiagABInv)));
330  btSimdScalar sum = _mm_add_ps(cpAppliedImp, deltaImpulse);
331  btSimdScalar resultLowerLess, resultUpperLess;
332  resultLowerLess = _mm_cmplt_ps(sum, lowerLimit1);
333  resultUpperLess = _mm_cmplt_ps(sum, upperLimit1);
334  __m128 lowMinApplied = _mm_sub_ps(lowerLimit1, cpAppliedImp);
335  deltaImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse));
336  c.m_appliedPushImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum));
337  __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128);
338  __m128 linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128, bodyB.internalGetInvMass().mVec128);
339  __m128 impulseMagnitude = deltaImpulse;
340  bodyA.internalGetPushVelocity().mVec128 = _mm_add_ps(bodyA.internalGetPushVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude));
341  bodyA.internalGetTurnVelocity().mVec128 = _mm_add_ps(bodyA.internalGetTurnVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude));
342  bodyB.internalGetPushVelocity().mVec128 = _mm_add_ps(bodyB.internalGetPushVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude));
343  bodyB.internalGetTurnVelocity().mVec128 = _mm_add_ps(bodyB.internalGetTurnVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude));
344  btSimdScalar deltaImp = deltaImpulse;
345  return deltaImp.m_floats[0] * (1. / c.m_jacDiagABInv);
346 #else
347  return gResolveSplitPenetrationImpulse_scalar_reference(bodyA, bodyB, c);
348 #endif
349 }
350 
352 {
353  m_btSeed2 = 0;
354  m_cachedSolverMode = 0;
355  setupSolverFunctions(false);
356 }
357 
359 {
363 
364  if (useSimd)
365  {
366 #ifdef USE_SIMD
367  m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_sse2;
368  m_resolveSingleConstraintRowLowerLimit = gResolveSingleConstraintRowLowerLimit_sse2;
370 
371 #ifdef BT_ALLOW_SSE4
372  int cpuFeatures = btCpuFeatureUtility::getCpuFeatures();
374  {
375  m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_sse4_1_fma3;
376  m_resolveSingleConstraintRowLowerLimit = gResolveSingleConstraintRowLowerLimit_sse4_1_fma3;
377  }
378 #endif //BT_ALLOW_SSE4
379 #endif //USE_SIMD
380  }
381 }
382 
384 {
385 }
386 
388 {
390 }
391 
393 {
395 }
396 
397 #ifdef USE_SIMD
399 {
400  return gResolveSingleConstraintRowGeneric_sse2;
401 }
403 {
404  return gResolveSingleConstraintRowLowerLimit_sse2;
405 }
406 #ifdef BT_ALLOW_SSE4
408 {
409  return gResolveSingleConstraintRowGeneric_sse4_1_fma3;
410 }
412 {
413  return gResolveSingleConstraintRowLowerLimit_sse4_1_fma3;
414 }
415 #endif //BT_ALLOW_SSE4
416 #endif //USE_SIMD
417 
419 {
420  m_btSeed2 = (1664525L * m_btSeed2 + 1013904223L) & 0xffffffff;
421  return m_btSeed2;
422 }
423 
424 //See ODE: adam's all-int straightforward(?) dRandInt (0..n-1)
426 {
427  // seems good; xor-fold and modulus
428  const unsigned long un = static_cast<unsigned long>(n);
429  unsigned long r = btRand2();
430 
431  // note: probably more aggressive than it needs to be -- might be
432  // able to get away without one or two of the innermost branches.
433  if (un <= 0x00010000UL)
434  {
435  r ^= (r >> 16);
436  if (un <= 0x00000100UL)
437  {
438  r ^= (r >> 8);
439  if (un <= 0x00000010UL)
440  {
441  r ^= (r >> 4);
442  if (un <= 0x00000004UL)
443  {
444  r ^= (r >> 2);
445  if (un <= 0x00000002UL)
446  {
447  r ^= (r >> 1);
448  }
449  }
450  }
451  }
452  }
453 
454  return (int)(r % un);
455 }
456 
458 {
459  btRigidBody* rb = collisionObject ? btRigidBody::upcast(collisionObject) : 0;
460 
461  solverBody->internalGetDeltaLinearVelocity().setValue(0.f, 0.f, 0.f);
462  solverBody->internalGetDeltaAngularVelocity().setValue(0.f, 0.f, 0.f);
463  solverBody->internalGetPushVelocity().setValue(0.f, 0.f, 0.f);
464  solverBody->internalGetTurnVelocity().setValue(0.f, 0.f, 0.f);
465 
466  if (rb)
467  {
468  solverBody->m_worldTransform = rb->getWorldTransform();
469  solverBody->internalSetInvMass(btVector3(rb->getInvMass(), rb->getInvMass(), rb->getInvMass()) * rb->getLinearFactor());
470  solverBody->m_originalBody = rb;
471  solverBody->m_angularFactor = rb->getAngularFactor();
472  solverBody->m_linearFactor = rb->getLinearFactor();
473  solverBody->m_linearVelocity = rb->getLinearVelocity();
474  solverBody->m_angularVelocity = rb->getAngularVelocity();
475  solverBody->m_externalForceImpulse = rb->getTotalForce() * rb->getInvMass() * timeStep;
476  solverBody->m_externalTorqueImpulse = rb->getTotalTorque() * rb->getInvInertiaTensorWorld() * timeStep;
477  }
478  else
479  {
480  solverBody->m_worldTransform.setIdentity();
481  solverBody->internalSetInvMass(btVector3(0, 0, 0));
482  solverBody->m_originalBody = 0;
483  solverBody->m_angularFactor.setValue(1, 1, 1);
484  solverBody->m_linearFactor.setValue(1, 1, 1);
485  solverBody->m_linearVelocity.setValue(0, 0, 0);
486  solverBody->m_angularVelocity.setValue(0, 0, 0);
487  solverBody->m_externalForceImpulse.setValue(0, 0, 0);
488  solverBody->m_externalTorqueImpulse.setValue(0, 0, 0);
489  }
490  }
491 
493 {
494  //printf("rel_vel =%f\n", rel_vel);
495  if (btFabs(rel_vel) < velocityThreshold)
496  return 0.;
497 
498  btScalar rest = restitution * -rel_vel;
499  return rest;
500 }
501 
503 {
504  if (colObj && colObj->hasAnisotropicFriction(frictionMode))
505  {
506  // transform to local coordinates
507  btVector3 loc_lateral = frictionDirection * colObj->getWorldTransform().getBasis();
508  const btVector3& friction_scaling = colObj->getAnisotropicFriction();
509  //apply anisotropic friction
510  loc_lateral *= friction_scaling;
511  // ... and transform it back to global coordinates
512  frictionDirection = colObj->getWorldTransform().getBasis() * loc_lateral;
513  }
514 }
515 
516 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)
517 {
518  btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
519  btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
520 
521  btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody;
522  btRigidBody* bodyA = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody;
523 
524  solverConstraint.m_solverBodyIdA = solverBodyIdA;
525  solverConstraint.m_solverBodyIdB = solverBodyIdB;
526 
527  solverConstraint.m_friction = cp.m_combinedFriction;
528  solverConstraint.m_originalContactPoint = 0;
529 
530  solverConstraint.m_appliedImpulse = 0.f;
531  solverConstraint.m_appliedPushImpulse = 0.f;
532 
533  if (body0)
534  {
535  solverConstraint.m_contactNormal1 = normalAxis;
536  btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal1);
537  solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
538  solverConstraint.m_angularComponentA = body0->getInvInertiaTensorWorld() * ftorqueAxis1 * body0->getAngularFactor();
539  }
540  else
541  {
542  solverConstraint.m_contactNormal1.setZero();
543  solverConstraint.m_relpos1CrossNormal.setZero();
544  solverConstraint.m_angularComponentA.setZero();
545  }
546 
547  if (bodyA)
548  {
549  solverConstraint.m_contactNormal2 = -normalAxis;
550  btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal2);
551  solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
552  solverConstraint.m_angularComponentB = bodyA->getInvInertiaTensorWorld() * ftorqueAxis1 * bodyA->getAngularFactor();
553  }
554  else
555  {
556  solverConstraint.m_contactNormal2.setZero();
557  solverConstraint.m_relpos2CrossNormal.setZero();
558  solverConstraint.m_angularComponentB.setZero();
559  }
560 
561  {
562  btVector3 vec;
563  btScalar denom0 = 0.f;
564  btScalar denom1 = 0.f;
565  if (body0)
566  {
567  vec = (solverConstraint.m_angularComponentA).cross(rel_pos1);
568  denom0 = body0->getInvMass() + normalAxis.dot(vec);
569  }
570  if (bodyA)
571  {
572  vec = (-solverConstraint.m_angularComponentB).cross(rel_pos2);
573  denom1 = bodyA->getInvMass() + normalAxis.dot(vec);
574  }
575  btScalar denom = relaxation / (denom0 + denom1);
576  solverConstraint.m_jacDiagABInv = denom;
577  }
578 
579  {
580  btScalar rel_vel;
581  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));
582  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));
583 
584  rel_vel = vel1Dotn + vel2Dotn;
585 
586  // btScalar positionalError = 0.f;
587 
588  btScalar velocityError = desiredVelocity - rel_vel;
589  btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
590 
591  btScalar penetrationImpulse = btScalar(0);
592 
594  {
595  btScalar distance = (cp.getPositionWorldOnA() - cp.getPositionWorldOnB()).dot(normalAxis);
596  btScalar positionalError = -distance * infoGlobal.m_frictionERP / infoGlobal.m_timeStep;
597  penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv;
598  }
599 
600  solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
601  solverConstraint.m_rhsPenetration = 0.f;
602  solverConstraint.m_cfm = cfmSlip;
603  solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
604  solverConstraint.m_upperLimit = solverConstraint.m_friction;
605  }
606 }
607 
608 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)
609 {
611  solverConstraint.m_frictionIndex = frictionIndex;
612  setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2,
613  colObj0, colObj1, relaxation, infoGlobal, desiredVelocity, cfmSlip);
614  return solverConstraint;
615 }
616 
617 void btSequentialImpulseConstraintSolver::setupTorsionalFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis1, int solverBodyIdA, int solverBodyIdB,
618  btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2,
619  btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation,
620  btScalar desiredVelocity, btScalar cfmSlip)
621 
622 {
623  btVector3 normalAxis(0, 0, 0);
624 
625  solverConstraint.m_contactNormal1 = normalAxis;
626  solverConstraint.m_contactNormal2 = -normalAxis;
627  btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
628  btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
629 
630  btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody;
631  btRigidBody* bodyA = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody;
632 
633  solverConstraint.m_solverBodyIdA = solverBodyIdA;
634  solverConstraint.m_solverBodyIdB = solverBodyIdB;
635 
636  solverConstraint.m_friction = combinedTorsionalFriction;
637  solverConstraint.m_originalContactPoint = 0;
638 
639  solverConstraint.m_appliedImpulse = 0.f;
640  solverConstraint.m_appliedPushImpulse = 0.f;
641 
642  {
643  btVector3 ftorqueAxis1 = -normalAxis1;
644  solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
645  solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld() * ftorqueAxis1 * body0->getAngularFactor() : btVector3(0, 0, 0);
646  }
647  {
648  btVector3 ftorqueAxis1 = normalAxis1;
649  solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
650  solverConstraint.m_angularComponentB = bodyA ? bodyA->getInvInertiaTensorWorld() * ftorqueAxis1 * bodyA->getAngularFactor() : btVector3(0, 0, 0);
651  }
652 
653  {
654  btVector3 iMJaA = body0 ? body0->getInvInertiaTensorWorld() * solverConstraint.m_relpos1CrossNormal : btVector3(0, 0, 0);
655  btVector3 iMJaB = bodyA ? bodyA->getInvInertiaTensorWorld() * solverConstraint.m_relpos2CrossNormal : btVector3(0, 0, 0);
656  btScalar sum = 0;
657  sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
658  sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
659  solverConstraint.m_jacDiagABInv = btScalar(1.) / sum;
660  }
661 
662  {
663  btScalar rel_vel;
664  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));
665  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));
666 
667  rel_vel = vel1Dotn + vel2Dotn;
668 
669  // btScalar positionalError = 0.f;
670 
671  btSimdScalar velocityError = desiredVelocity - rel_vel;
672  btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
673  solverConstraint.m_rhs = velocityImpulse;
674  solverConstraint.m_cfm = cfmSlip;
675  solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
676  solverConstraint.m_upperLimit = solverConstraint.m_friction;
677  }
678 }
679 
680 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)
681 {
683  solverConstraint.m_frictionIndex = frictionIndex;
684  setupTorsionalFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, combinedTorsionalFriction, rel_pos1, rel_pos2,
685  colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
686  return solverConstraint;
687 }
688 
690 {
691 #if BT_THREADSAFE
692  int solverBodyId = -1;
693  bool isRigidBodyType = btRigidBody::upcast(&body) != NULL;
694  if (isRigidBodyType && !body.isStaticOrKinematicObject())
695  {
696  // dynamic body
697  // Dynamic bodies can only be in one island, so it's safe to write to the companionId
698  solverBodyId = body.getCompanionId();
699  if (solverBodyId < 0)
700  {
701  solverBodyId = m_tmpSolverBodyPool.size();
702  btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
703  initSolverBody(&solverBody, &body, timeStep);
704  body.setCompanionId(solverBodyId);
705  }
706  }
707  else if (isRigidBodyType && body.isKinematicObject())
708  {
709  //
710  // NOTE: must test for kinematic before static because some kinematic objects also
711  // identify as "static"
712  //
713  // Kinematic bodies can be in multiple islands at once, so it is a
714  // race condition to write to them, so we use an alternate method
715  // to record the solverBodyId
716  int uniqueId = body.getWorldArrayIndex();
717  const int INVALID_SOLVER_BODY_ID = -1;
719  {
720  m_kinematicBodyUniqueIdToSolverBodyTable.resize(uniqueId + 1, INVALID_SOLVER_BODY_ID);
721  }
723  // if no table entry yet,
724  if (solverBodyId == INVALID_SOLVER_BODY_ID)
725  {
726  // create a table entry for this body
727  solverBodyId = m_tmpSolverBodyPool.size();
728  btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
729  initSolverBody(&solverBody, &body, timeStep);
731  }
732  }
733  else
734  {
735  bool isMultiBodyType = (body.getInternalType() & btCollisionObject::CO_FEATHERSTONE_LINK);
736  // Incorrectly set collision object flags can degrade performance in various ways.
737  if (!isMultiBodyType)
738  {
740  }
741  //it could be a multibody link collider
742  // all fixed bodies (inf mass) get mapped to a single solver id
743  if (m_fixedBodyId < 0)
744  {
746  btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
747  initSolverBody(&fixedBody, 0, timeStep);
748  }
749  solverBodyId = m_fixedBodyId;
750  }
751  btAssert(solverBodyId >= 0 && solverBodyId < m_tmpSolverBodyPool.size());
752  return solverBodyId;
753 #else // BT_THREADSAFE
754 
755  int solverBodyIdA = -1;
756 
757  if (body.getCompanionId() >= 0)
758  {
759  //body has already been converted
760  solverBodyIdA = body.getCompanionId();
761  btAssert(solverBodyIdA < m_tmpSolverBodyPool.size());
762  }
763  else
764  {
765  btRigidBody* rb = btRigidBody::upcast(&body);
766  //convert both active and kinematic objects (for their velocity)
767  if (rb && (rb->getInvMass() || rb->isKinematicObject()))
768  {
769  solverBodyIdA = m_tmpSolverBodyPool.size();
770  btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
771  initSolverBody(&solverBody, &body, timeStep);
772  body.setCompanionId(solverBodyIdA);
773  }
774  else
775  {
776  if (m_fixedBodyId < 0)
777  {
779  btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
780  initSolverBody(&fixedBody, 0, timeStep);
781  }
782  return m_fixedBodyId;
783  // return 0;//assume first one is a fixed solver body
784  }
785  }
786 
787  return solverBodyIdA;
788 #endif // BT_THREADSAFE
789 }
790 #include <stdio.h>
791 
793  int solverBodyIdA, int solverBodyIdB,
794  btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
795  btScalar& relaxation,
796  const btVector3& rel_pos1, const btVector3& rel_pos2)
797 {
798  // const btVector3& pos1 = cp.getPositionWorldOnA();
799  // const btVector3& pos2 = cp.getPositionWorldOnB();
800 
801  btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA];
802  btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB];
803 
804  btRigidBody* rb0 = bodyA->m_originalBody;
805  btRigidBody* rb1 = bodyB->m_originalBody;
806 
807  // btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
808  // btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
809  //rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
810  //rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
811 
812  relaxation = infoGlobal.m_sor;
813  btScalar invTimeStep = btScalar(1) / infoGlobal.m_timeStep;
814 
815  //cfm = 1 / ( dt * kp + kd )
816  //erp = dt * kp / ( dt * kp + kd )
817 
818  btScalar cfm = infoGlobal.m_globalCfm;
819  btScalar erp = infoGlobal.m_erp2;
820 
822  {
824  cfm = cp.m_contactCFM;
826  erp = cp.m_contactERP;
827  }
828  else
829  {
831  {
833  if (denom < SIMD_EPSILON)
834  {
835  denom = SIMD_EPSILON;
836  }
837  cfm = btScalar(1) / denom;
838  erp = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1) / denom;
839  }
840  }
841 
842  cfm *= invTimeStep;
843 
844  btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
845  solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld() * torqueAxis0 * rb0->getAngularFactor() : btVector3(0, 0, 0);
846  btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);
847  solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld() * -torqueAxis1 * rb1->getAngularFactor() : btVector3(0, 0, 0);
848 
849  {
850 #ifdef COMPUTE_IMPULSE_DENOM
851  btScalar denom0 = rb0->computeImpulseDenominator(pos1, cp.m_normalWorldOnB);
852  btScalar denom1 = rb1->computeImpulseDenominator(pos2, cp.m_normalWorldOnB);
853 #else
854  btVector3 vec;
855  btScalar denom0 = 0.f;
856  btScalar denom1 = 0.f;
857  if (rb0)
858  {
859  vec = (solverConstraint.m_angularComponentA).cross(rel_pos1);
860  denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
861  }
862  if (rb1)
863  {
864  vec = (-solverConstraint.m_angularComponentB).cross(rel_pos2);
865  denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
866  }
867 #endif //COMPUTE_IMPULSE_DENOM
868 
869  btScalar denom = relaxation / (denom0 + denom1 + cfm);
870  solverConstraint.m_jacDiagABInv = denom;
871  }
872 
873  if (rb0)
874  {
875  solverConstraint.m_contactNormal1 = cp.m_normalWorldOnB;
876  solverConstraint.m_relpos1CrossNormal = torqueAxis0;
877  }
878  else
879  {
880  solverConstraint.m_contactNormal1.setZero();
881  solverConstraint.m_relpos1CrossNormal.setZero();
882  }
883  if (rb1)
884  {
885  solverConstraint.m_contactNormal2 = -cp.m_normalWorldOnB;
886  solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
887  }
888  else
889  {
890  solverConstraint.m_contactNormal2.setZero();
891  solverConstraint.m_relpos2CrossNormal.setZero();
892  }
893 
894  btScalar restitution = 0.f;
895  btScalar penetration = cp.getDistance() + infoGlobal.m_linearSlop;
896 
897  {
898  btVector3 vel1, vel2;
899 
900  vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0, 0, 0);
901  vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0, 0, 0);
902 
903  // btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
904  btVector3 vel = vel1 - vel2;
905  btScalar rel_vel = cp.m_normalWorldOnB.dot(vel);
906 
907  solverConstraint.m_friction = cp.m_combinedFriction;
908 
909  restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution, infoGlobal.m_restitutionVelocityThreshold);
910  if (restitution <= btScalar(0.))
911  {
912  restitution = 0.f;
913  };
914  }
915 
917  if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
918  {
919  solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
920  if (rb0)
921  bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1 * bodyA->internalGetInvMass(), solverConstraint.m_angularComponentA, solverConstraint.m_appliedImpulse);
922  if (rb1)
923  bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2 * bodyB->internalGetInvMass(), -solverConstraint.m_angularComponentB, -(btScalar)solverConstraint.m_appliedImpulse);
924  }
925  else
926  {
927  solverConstraint.m_appliedImpulse = 0.f;
928  }
929 
930  solverConstraint.m_appliedPushImpulse = 0.f;
931 
932  {
933  btVector3 externalForceImpulseA = bodyA->m_originalBody ? bodyA->m_externalForceImpulse : btVector3(0, 0, 0);
934  btVector3 externalTorqueImpulseA = bodyA->m_originalBody ? bodyA->m_externalTorqueImpulse : btVector3(0, 0, 0);
935  btVector3 externalForceImpulseB = bodyB->m_originalBody ? bodyB->m_externalForceImpulse : btVector3(0, 0, 0);
936  btVector3 externalTorqueImpulseB = bodyB->m_originalBody ? bodyB->m_externalTorqueImpulse : btVector3(0, 0, 0);
937 
938  btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(bodyA->m_linearVelocity + externalForceImpulseA) + solverConstraint.m_relpos1CrossNormal.dot(bodyA->m_angularVelocity + externalTorqueImpulseA);
939  btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(bodyB->m_linearVelocity + externalForceImpulseB) + solverConstraint.m_relpos2CrossNormal.dot(bodyB->m_angularVelocity + externalTorqueImpulseB);
940  btScalar rel_vel = vel1Dotn + vel2Dotn;
941 
942  btScalar positionalError = 0.f;
943  btScalar velocityError = restitution - rel_vel; // * damping;
944 
945  if (penetration > 0)
946  {
947  positionalError = 0;
948 
949  velocityError -= penetration * invTimeStep;
950  }
951  else
952  {
953  positionalError = -penetration * erp * invTimeStep;
954  }
955 
956  btScalar penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv;
957  btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
958 
959  if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
960  {
961  //combine position and velocity into rhs
962  solverConstraint.m_rhs = penetrationImpulse + velocityImpulse; //-solverConstraint.m_contactNormal1.dot(bodyA->m_externalForce*bodyA->m_invMass-bodyB->m_externalForce/bodyB->m_invMass)*solverConstraint.m_jacDiagABInv;
963  solverConstraint.m_rhsPenetration = 0.f;
964  }
965  else
966  {
967  //split position and velocity into rhs and m_rhsPenetration
968  solverConstraint.m_rhs = velocityImpulse;
969  solverConstraint.m_rhsPenetration = penetrationImpulse;
970  }
971  solverConstraint.m_cfm = cfm * solverConstraint.m_jacDiagABInv;
972  solverConstraint.m_lowerLimit = 0;
973  solverConstraint.m_upperLimit = 1e10f;
974  }
975 }
976 
978  int solverBodyIdA, int solverBodyIdB,
979  btManifoldPoint& cp, const btContactSolverInfo& infoGlobal)
980 {
981  {
982  btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
983 
984  frictionConstraint1.m_appliedImpulse = 0.f;
985  }
986 
988  {
989  btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex + 1];
990 
991  frictionConstraint2.m_appliedImpulse = 0.f;
992  }
993 }
994 
996 {
997  btCollisionObject *colObj0 = 0, *colObj1 = 0;
998 
999  colObj0 = (btCollisionObject*)manifold->getBody0();
1000  colObj1 = (btCollisionObject*)manifold->getBody1();
1001 
1002  int solverBodyIdA = getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep);
1003  int solverBodyIdB = getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep);
1004 
1005  // btRigidBody* bodyA = btRigidBody::upcast(colObj0);
1006  // btRigidBody* bodyB = btRigidBody::upcast(colObj1);
1007 
1008  btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverBodyIdA];
1009  btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverBodyIdB];
1010 
1012  if (!solverBodyA || (solverBodyA->m_invMass.fuzzyZero() && (!solverBodyB || solverBodyB->m_invMass.fuzzyZero())))
1013  return;
1014 
1015  int rollingFriction = 1;
1016  for (int j = 0; j < manifold->getNumContacts(); j++)
1017  {
1018  btManifoldPoint& cp = manifold->getContactPoint(j);
1019 
1020  if (cp.getDistance() <= manifold->getContactProcessingThreshold())
1021  {
1022  btVector3 rel_pos1;
1023  btVector3 rel_pos2;
1024  btScalar relaxation;
1025 
1026  int frictionIndex = m_tmpSolverContactConstraintPool.size();
1028  solverConstraint.m_solverBodyIdA = solverBodyIdA;
1029  solverConstraint.m_solverBodyIdB = solverBodyIdB;
1030 
1031  solverConstraint.m_originalContactPoint = &cp;
1032 
1033  const btVector3& pos1 = cp.getPositionWorldOnA();
1034  const btVector3& pos2 = cp.getPositionWorldOnB();
1035 
1036  rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
1037  rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
1038 
1039  btVector3 vel1;
1040  btVector3 vel2;
1041 
1042  solverBodyA->getVelocityInLocalPointNoDelta(rel_pos1, vel1);
1043  solverBodyB->getVelocityInLocalPointNoDelta(rel_pos2, vel2);
1044 
1045  btVector3 vel = vel1 - vel2;
1046  btScalar rel_vel = cp.m_normalWorldOnB.dot(vel);
1047 
1048  setupContactConstraint(solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, relaxation, rel_pos1, rel_pos2);
1049 
1051 
1053 
1054  if ((cp.m_combinedRollingFriction > 0.f) && (rollingFriction > 0))
1055  {
1056  {
1057  addTorsionalFrictionConstraint(cp.m_normalWorldOnB, solverBodyIdA, solverBodyIdB, frictionIndex, cp, cp.m_combinedSpinningFriction, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
1058  btVector3 axis0, axis1;
1059  btPlaneSpace1(cp.m_normalWorldOnB, axis0, axis1);
1060  axis0.normalize();
1061  axis1.normalize();
1062 
1067  if (axis0.length() > 0.001)
1068  addTorsionalFrictionConstraint(axis0, solverBodyIdA, solverBodyIdB, frictionIndex, cp,
1069  cp.m_combinedRollingFriction, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
1070  if (axis1.length() > 0.001)
1071  addTorsionalFrictionConstraint(axis1, solverBodyIdA, solverBodyIdB, frictionIndex, cp,
1072  cp.m_combinedRollingFriction, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
1073  }
1074  }
1075 
1080 
1091 
1093  {
1094  cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
1095  btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
1097  {
1098  cp.m_lateralFrictionDir1 *= 1.f / btSqrt(lat_rel_vel);
1101  addFrictionConstraint(cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
1102 
1104  {
1106  cp.m_lateralFrictionDir2.normalize(); //??
1109  addFrictionConstraint(cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
1110  }
1111  }
1112  else
1113  {
1115 
1118  addFrictionConstraint(cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
1119 
1121  {
1124  addFrictionConstraint(cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
1125  }
1126 
1128  {
1130  }
1131  }
1132  }
1133  else
1134  {
1135  addFrictionConstraint(cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM);
1136 
1138  addFrictionConstraint(cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM);
1139  }
1140  setFrictionConstraintImpulse(solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
1141  }
1142  }
1143  }
1144 
1146 {
1147  int i;
1148  btPersistentManifold* manifold = 0;
1149  // btCollisionObject* colObj0=0,*colObj1=0;
1150 
1151  for (i = 0; i < numManifolds; i++)
1152  {
1153  manifold = manifoldPtr[i];
1154  convertContact(manifold, infoGlobal);
1155  }
1156 }
1157 
1159  btTypedConstraint* constraint,
1161  int solverBodyIdA,
1162  int solverBodyIdB,
1163  const btContactSolverInfo& infoGlobal)
1164 {
1165  const btRigidBody& rbA = constraint->getRigidBodyA();
1166  const btRigidBody& rbB = constraint->getRigidBodyB();
1167 
1168  const btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
1169  const btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
1170 
1171  int overrideNumSolverIterations = constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations;
1172  if (overrideNumSolverIterations > m_maxOverrideNumSolverIterations)
1173  m_maxOverrideNumSolverIterations = overrideNumSolverIterations;
1174 
1175  for (int j = 0; j < info1.m_numConstraintRows; j++)
1176  {
1177  memset(&currentConstraintRow[j], 0, sizeof(btSolverConstraint));
1178  currentConstraintRow[j].m_lowerLimit = -SIMD_INFINITY;
1179  currentConstraintRow[j].m_upperLimit = SIMD_INFINITY;
1180  currentConstraintRow[j].m_appliedImpulse = 0.f;
1181  currentConstraintRow[j].m_appliedPushImpulse = 0.f;
1182  currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA;
1183  currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB;
1184  currentConstraintRow[j].m_overrideNumSolverIterations = overrideNumSolverIterations;
1185  }
1186 
1187  // these vectors are already cleared in initSolverBody, no need to redundantly clear again
1188  btAssert(bodyAPtr->getDeltaLinearVelocity().isZero());
1189  btAssert(bodyAPtr->getDeltaAngularVelocity().isZero());
1190  btAssert(bodyAPtr->getPushVelocity().isZero());
1191  btAssert(bodyAPtr->getTurnVelocity().isZero());
1192  btAssert(bodyBPtr->getDeltaLinearVelocity().isZero());
1193  btAssert(bodyBPtr->getDeltaAngularVelocity().isZero());
1194  btAssert(bodyBPtr->getPushVelocity().isZero());
1195  btAssert(bodyBPtr->getTurnVelocity().isZero());
1196  //bodyAPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
1197  //bodyAPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
1198  //bodyAPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f);
1199  //bodyAPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
1200  //bodyBPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
1201  //bodyBPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
1202  //bodyBPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f);
1203  //bodyBPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
1204 
1206  info2.fps = 1.f / infoGlobal.m_timeStep;
1207  info2.erp = infoGlobal.m_erp;
1208  info2.m_J1linearAxis = currentConstraintRow->m_contactNormal1;
1209  info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal;
1210  info2.m_J2linearAxis = currentConstraintRow->m_contactNormal2;
1211  info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal;
1212  info2.rowskip = sizeof(btSolverConstraint) / sizeof(btScalar); //check this
1214  btAssert(info2.rowskip * sizeof(btScalar) == sizeof(btSolverConstraint));
1215  info2.m_constraintError = &currentConstraintRow->m_rhs;
1216  currentConstraintRow->m_cfm = infoGlobal.m_globalCfm;
1217  info2.m_damping = infoGlobal.m_damping;
1218  info2.cfm = &currentConstraintRow->m_cfm;
1219  info2.m_lowerLimit = &currentConstraintRow->m_lowerLimit;
1220  info2.m_upperLimit = &currentConstraintRow->m_upperLimit;
1221  info2.m_numIterations = infoGlobal.m_numIterations;
1222  constraint->getInfo2(&info2);
1223 
1225  for (int j = 0; j < info1.m_numConstraintRows; j++)
1226  {
1227  btSolverConstraint& solverConstraint = currentConstraintRow[j];
1228 
1229  if (solverConstraint.m_upperLimit >= constraint->getBreakingImpulseThreshold())
1230  {
1231  solverConstraint.m_upperLimit = constraint->getBreakingImpulseThreshold();
1232  }
1233 
1234  if (solverConstraint.m_lowerLimit <= -constraint->getBreakingImpulseThreshold())
1235  {
1236  solverConstraint.m_lowerLimit = -constraint->getBreakingImpulseThreshold();
1237  }
1238 
1239  solverConstraint.m_originalContactPoint = constraint;
1240 
1241  {
1242  const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
1243  solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld() * ftorqueAxis1 * constraint->getRigidBodyA().getAngularFactor();
1244  }
1245  {
1246  const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal;
1247  solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld() * ftorqueAxis2 * constraint->getRigidBodyB().getAngularFactor();
1248  }
1249 
1250  {
1251  btVector3 iMJlA = solverConstraint.m_contactNormal1 * rbA.getInvMass();
1252  btVector3 iMJaA = rbA.getInvInertiaTensorWorld() * solverConstraint.m_relpos1CrossNormal;
1253  btVector3 iMJlB = solverConstraint.m_contactNormal2 * rbB.getInvMass(); //sign of normal?
1254  btVector3 iMJaB = rbB.getInvInertiaTensorWorld() * solverConstraint.m_relpos2CrossNormal;
1255 
1256  btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal1);
1257  sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
1258  sum += iMJlB.dot(solverConstraint.m_contactNormal2);
1259  sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
1260  btScalar fsum = btFabs(sum);
1261  btAssert(fsum > SIMD_EPSILON);
1262  btScalar sorRelaxation = 1.f; //todo: get from globalInfo?
1263  solverConstraint.m_jacDiagABInv = fsum > SIMD_EPSILON ? sorRelaxation / sum : 0.f;
1264  }
1265 
1266  {
1267  btScalar rel_vel;
1268  btVector3 externalForceImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalForceImpulse : btVector3(0, 0, 0);
1269  btVector3 externalTorqueImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalTorqueImpulse : btVector3(0, 0, 0);
1270 
1271  btVector3 externalForceImpulseB = bodyBPtr->m_originalBody ? bodyBPtr->m_externalForceImpulse : btVector3(0, 0, 0);
1272  btVector3 externalTorqueImpulseB = bodyBPtr->m_originalBody ? bodyBPtr->m_externalTorqueImpulse : btVector3(0, 0, 0);
1273 
1274  btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(rbA.getLinearVelocity() + externalForceImpulseA) + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity() + externalTorqueImpulseA);
1275 
1276  btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(rbB.getLinearVelocity() + externalForceImpulseB) + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity() + externalTorqueImpulseB);
1277 
1278  rel_vel = vel1Dotn + vel2Dotn;
1279  btScalar restitution = 0.f;
1280  btScalar positionalError = solverConstraint.m_rhs; //already filled in by getConstraintInfo2
1281  btScalar velocityError = restitution - rel_vel * info2.m_damping;
1282  btScalar penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv;
1283  btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
1284  solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
1285  solverConstraint.m_appliedImpulse = 0.f;
1286  }
1287  }
1288 }
1289 
1290 void btSequentialImpulseConstraintSolver::convertJoints(btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal)
1291 {
1292  BT_PROFILE("convertJoints");
1293  for (int j = 0; j < numConstraints; j++)
1294  {
1295  btTypedConstraint* constraint = constraints[j];
1296  constraint->buildJacobian();
1297  constraint->internalSetAppliedImpulse(0.0f);
1298  }
1299 
1300  int totalNumRows = 0;
1301 
1303  //calculate the total number of contraint rows
1304  for (int i = 0; i < numConstraints; i++)
1305  {
1307  btJointFeedback* fb = constraints[i]->getJointFeedback();
1308  if (fb)
1309  {
1314  }
1315 
1316  if (constraints[i]->isEnabled())
1317  {
1318  constraints[i]->getInfo1(&info1);
1319  }
1320  else
1321  {
1322  info1.m_numConstraintRows = 0;
1323  info1.nub = 0;
1324  }
1325  totalNumRows += info1.m_numConstraintRows;
1326  }
1328 
1330  int currentRow = 0;
1331 
1332  for (int i = 0; i < numConstraints; i++)
1333  {
1335 
1336  if (info1.m_numConstraintRows)
1337  {
1338  btAssert(currentRow < totalNumRows);
1339 
1340  btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];
1341  btTypedConstraint* constraint = constraints[i];
1342  btRigidBody& rbA = constraint->getRigidBodyA();
1343  btRigidBody& rbB = constraint->getRigidBodyB();
1344 
1345  int solverBodyIdA = getOrInitSolverBody(rbA, infoGlobal.m_timeStep);
1346  int solverBodyIdB = getOrInitSolverBody(rbB, infoGlobal.m_timeStep);
1347 
1348  convertJoint(currentConstraintRow, constraint, info1, solverBodyIdA, solverBodyIdB, infoGlobal);
1349  }
1350  currentRow += info1.m_numConstraintRows;
1351  }
1352 }
1353 
1355 {
1356  BT_PROFILE("convertBodies");
1357  for (int i = 0; i < numBodies; i++)
1358  {
1359  bodies[i]->setCompanionId(-1);
1360  }
1361 #if BT_THREADSAFE
1363 #endif // BT_THREADSAFE
1364 
1365  m_tmpSolverBodyPool.reserve(numBodies + 1);
1367 
1368  //btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
1369  //initSolverBody(&fixedBody,0);
1370 
1371  for (int i = 0; i < numBodies; i++)
1372  {
1373  int bodyId = getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep);
1374 
1375  btRigidBody* body = btRigidBody::upcast(bodies[i]);
1376  if (body && body->getInvMass())
1377  {
1378  btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
1379  btVector3 gyroForce(0, 0, 0);
1381  {
1382  gyroForce = body->computeGyroscopicForceExplicit(infoGlobal.m_maxGyroscopicForce);
1383  solverBody.m_externalTorqueImpulse -= gyroForce * body->getInvInertiaTensorWorld() * infoGlobal.m_timeStep;
1384  }
1386  {
1387  gyroForce = body->computeGyroscopicImpulseImplicit_World(infoGlobal.m_timeStep);
1388  solverBody.m_externalTorqueImpulse += gyroForce;
1389  }
1391  {
1392  gyroForce = body->computeGyroscopicImpulseImplicit_Body(infoGlobal.m_timeStep);
1393  solverBody.m_externalTorqueImpulse += gyroForce;
1394  }
1395  }
1396  }
1397 }
1398 
1399 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
1400 {
1401  m_fixedBodyId = -1;
1402  BT_PROFILE("solveGroupCacheFriendlySetup");
1403  (void)debugDrawer;
1404 
1405  // if solver mode has changed,
1406  if (infoGlobal.m_solverMode != m_cachedSolverMode)
1407  {
1408  // update solver functions to use SIMD or non-SIMD
1409  bool useSimd = !!(infoGlobal.m_solverMode & SOLVER_SIMD);
1410  setupSolverFunctions(useSimd);
1411  m_cachedSolverMode = infoGlobal.m_solverMode;
1412  }
1414 
1415 #ifdef BT_ADDITIONAL_DEBUG
1416  //make sure that dynamic bodies exist for all (enabled) constraints
1417  for (int i = 0; i < numConstraints; i++)
1418  {
1419  btTypedConstraint* constraint = constraints[i];
1420  if (constraint->isEnabled())
1421  {
1422  if (!constraint->getRigidBodyA().isStaticOrKinematicObject())
1423  {
1424  bool found = false;
1425  for (int b = 0; b < numBodies; b++)
1426  {
1427  if (&constraint->getRigidBodyA() == bodies[b])
1428  {
1429  found = true;
1430  break;
1431  }
1432  }
1433  btAssert(found);
1434  }
1435  if (!constraint->getRigidBodyB().isStaticOrKinematicObject())
1436  {
1437  bool found = false;
1438  for (int b = 0; b < numBodies; b++)
1439  {
1440  if (&constraint->getRigidBodyB() == bodies[b])
1441  {
1442  found = true;
1443  break;
1444  }
1445  }
1446  btAssert(found);
1447  }
1448  }
1449  }
1450  //make sure that dynamic bodies exist for all contact manifolds
1451  for (int i = 0; i < numManifolds; i++)
1452  {
1453  if (!manifoldPtr[i]->getBody0()->isStaticOrKinematicObject())
1454  {
1455  bool found = false;
1456  for (int b = 0; b < numBodies; b++)
1457  {
1458  if (manifoldPtr[i]->getBody0() == bodies[b])
1459  {
1460  found = true;
1461  break;
1462  }
1463  }
1464  btAssert(found);
1465  }
1466  if (!manifoldPtr[i]->getBody1()->isStaticOrKinematicObject())
1467  {
1468  bool found = false;
1469  for (int b = 0; b < numBodies; b++)
1470  {
1471  if (manifoldPtr[i]->getBody1() == bodies[b])
1472  {
1473  found = true;
1474  break;
1475  }
1476  }
1477  btAssert(found);
1478  }
1479  }
1480 #endif //BT_ADDITIONAL_DEBUG
1481 
1482  //convert all bodies
1483  convertBodies(bodies, numBodies, infoGlobal);
1484 
1485  convertJoints(constraints, numConstraints, infoGlobal);
1486 
1487  convertContacts(manifoldPtr, numManifolds, infoGlobal);
1488 
1489  // btContactSolverInfo info = infoGlobal;
1490 
1491  int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
1492  int numConstraintPool = m_tmpSolverContactConstraintPool.size();
1493  int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
1494 
1498  m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool * 2);
1499  else
1500  m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool);
1501 
1503  {
1504  int i;
1505  for (i = 0; i < numNonContactPool; i++)
1506  {
1508  }
1509  for (i = 0; i < numConstraintPool; i++)
1510  {
1511  m_orderTmpConstraintPool[i] = i;
1512  }
1513  for (i = 0; i < numFrictionPool; i++)
1514  {
1516  }
1517  }
1518 
1519  return 0.f;
1520 }
1521 
1522 btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */, int /*numBodies*/, btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* /*debugDrawer*/)
1523 {
1524  BT_PROFILE("solveSingleIteration");
1525  btScalar leastSquaresResidual = 0.f;
1526 
1527  int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
1528  int numConstraintPool = m_tmpSolverContactConstraintPool.size();
1529  int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
1530 
1531  if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
1532  {
1533  if (1) // uncomment this for a bit less random ((iteration & 7) == 0)
1534  {
1535  for (int j = 0; j < numNonContactPool; ++j)
1536  {
1537  int tmp = m_orderNonContactConstraintPool[j];
1538  int swapi = btRandInt2(j + 1);
1540  m_orderNonContactConstraintPool[swapi] = tmp;
1541  }
1542 
1543  //contact/friction constraints are not solved more than
1544  if (iteration < infoGlobal.m_numIterations)
1545  {
1546  for (int j = 0; j < numConstraintPool; ++j)
1547  {
1548  int tmp = m_orderTmpConstraintPool[j];
1549  int swapi = btRandInt2(j + 1);
1551  m_orderTmpConstraintPool[swapi] = tmp;
1552  }
1553 
1554  for (int j = 0; j < numFrictionPool; ++j)
1555  {
1556  int tmp = m_orderFrictionConstraintPool[j];
1557  int swapi = btRandInt2(j + 1);
1559  m_orderFrictionConstraintPool[swapi] = tmp;
1560  }
1561  }
1562  }
1563  }
1564 
1566  for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++)
1567  {
1569  if (iteration < constraint.m_overrideNumSolverIterations)
1570  {
1572  leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
1573  }
1574  }
1575 
1576  if (iteration < infoGlobal.m_numIterations)
1577  {
1578  for (int j = 0; j < numConstraints; j++)
1579  {
1580  if (constraints[j]->isEnabled())
1581  {
1582  int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(), infoGlobal.m_timeStep);
1583  int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(), infoGlobal.m_timeStep);
1584  btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
1585  btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
1586  constraints[j]->solveConstraintObsolete(bodyA, bodyB, infoGlobal.m_timeStep);
1587  }
1588  }
1589 
1592  {
1593  int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1594  int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) ? 2 : 1;
1595 
1596  for (int c = 0; c < numPoolConstraints; c++)
1597  {
1598  btScalar totalImpulse = 0;
1599 
1600  {
1603  leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
1604 
1605  totalImpulse = solveManifold.m_appliedImpulse;
1606  }
1607  bool applyFriction = true;
1608  if (applyFriction)
1609  {
1610  {
1612 
1613  if (totalImpulse > btScalar(0))
1614  {
1615  solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
1616  solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
1617 
1619  leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
1620  }
1621  }
1622 
1624  {
1626 
1627  if (totalImpulse > btScalar(0))
1628  {
1629  solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
1630  solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
1631 
1633  leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
1634  }
1635  }
1636  }
1637  }
1638  }
1639  else //SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
1640  {
1641  //solve the friction constraints after all contact constraints, don't interleave them
1642  int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1643  int j;
1644 
1645  for (j = 0; j < numPoolConstraints; j++)
1646  {
1649  leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
1650  }
1651 
1653 
1654  int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
1655  for (j = 0; j < numFrictionPoolConstraints; j++)
1656  {
1658  btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
1659 
1660  if (totalImpulse > btScalar(0))
1661  {
1662  solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
1663  solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
1664 
1666  leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
1667  }
1668  }
1669  }
1670 
1671  int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
1672  for (int j = 0; j < numRollingFrictionPoolConstraints; j++)
1673  {
1675  btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
1676  if (totalImpulse > btScalar(0))
1677  {
1678  btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction * totalImpulse;
1679  if (rollingFrictionMagnitude > rollingFrictionConstraint.m_friction)
1680  rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
1681 
1682  rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
1683  rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
1684 
1685  btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA], m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB], rollingFrictionConstraint);
1686  leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
1687  }
1688  }
1689  }
1690  return leastSquaresResidual;
1691 }
1692 
1693 void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
1694 {
1695  BT_PROFILE("solveGroupCacheFriendlySplitImpulseIterations");
1696  int iteration;
1697  if (infoGlobal.m_splitImpulse)
1698  {
1699  {
1700  for (iteration = 0; iteration < infoGlobal.m_numIterations; iteration++)
1701  {
1702  btScalar leastSquaresResidual = 0.f;
1703  {
1704  int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1705  int j;
1706  for (j = 0; j < numPoolConstraints; j++)
1707  {
1709 
1710  btScalar residual = resolveSplitPenetrationImpulse(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
1711  leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
1712  }
1713  }
1714  if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration >= (infoGlobal.m_numIterations - 1))
1715  {
1716 #ifdef VERBOSE_RESIDUAL_PRINTF
1717  printf("residual = %f at iteration #%d\n", leastSquaresResidual, iteration);
1718 #endif
1719  break;
1720  }
1721  }
1722  }
1723  }
1724 }
1725 
1726 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
1727 {
1728  BT_PROFILE("solveGroupCacheFriendlyIterations");
1729 
1730  {
1732  solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
1733 
1735 
1736  for (int iteration = 0; iteration < maxIterations; iteration++)
1737  //for ( int iteration = maxIterations-1 ; iteration >= 0;iteration--)
1738  {
1739  m_leastSquaresResidual = solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
1740 
1741  if (m_leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1)))
1742  {
1743 #ifdef VERBOSE_RESIDUAL_PRINTF
1744  printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration);
1745 #endif
1747  m_analyticsData.m_numIterationsUsed = iteration+1;
1749  if (numBodies>0)
1750  m_analyticsData.m_islandId = bodies[0]->getCompanionId();
1751  m_analyticsData.m_numBodies = numBodies;
1752  m_analyticsData.m_numContactManifolds = numManifolds;
1754  break;
1755  }
1756  }
1757  }
1758  return 0.f;
1759 }
1760 
1762 {
1763  for (int j = iBegin; j < iEnd; j++)
1764  {
1765  const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j];
1767  btAssert(pt);
1768  pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
1769  // float f = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
1770  // printf("pt->m_appliedImpulseLateral1 = %f\n", f);
1772  //printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
1774  {
1775  pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex + 1].m_appliedImpulse;
1776  }
1777  //do a callback here?
1778  }
1779 }
1780 
1782 {
1783  for (int j = iBegin; j < iEnd; j++)
1784  {
1787  btJointFeedback* fb = constr->getJointFeedback();
1788  if (fb)
1789  {
1790  fb->m_appliedForceBodyA += solverConstr.m_contactNormal1 * solverConstr.m_appliedImpulse * constr->getRigidBodyA().getLinearFactor() / infoGlobal.m_timeStep;
1791  fb->m_appliedForceBodyB += solverConstr.m_contactNormal2 * solverConstr.m_appliedImpulse * constr->getRigidBodyB().getLinearFactor() / infoGlobal.m_timeStep;
1792  fb->m_appliedTorqueBodyA += solverConstr.m_relpos1CrossNormal * constr->getRigidBodyA().getAngularFactor() * solverConstr.m_appliedImpulse / infoGlobal.m_timeStep;
1793  fb->m_appliedTorqueBodyB += solverConstr.m_relpos2CrossNormal * constr->getRigidBodyB().getAngularFactor() * solverConstr.m_appliedImpulse / infoGlobal.m_timeStep; /*RGM ???? */
1794  }
1795 
1796  constr->internalSetAppliedImpulse(solverConstr.m_appliedImpulse);
1797  if (btFabs(solverConstr.m_appliedImpulse) >= constr->getBreakingImpulseThreshold())
1798  {
1799  constr->setEnabled(false);
1800  }
1801  }
1802 }
1803 
1805 {
1806  for (int i = iBegin; i < iEnd; i++)
1807  {
1808  btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody;
1809  if (body)
1810  {
1811  if (infoGlobal.m_splitImpulse)
1812  m_tmpSolverBodyPool[i].writebackVelocityAndTransform(infoGlobal.m_timeStep, infoGlobal.m_splitImpulseTurnErp);
1813  else
1814  m_tmpSolverBodyPool[i].writebackVelocity();
1815 
1816  m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(
1817  m_tmpSolverBodyPool[i].m_linearVelocity +
1818  m_tmpSolverBodyPool[i].m_externalForceImpulse);
1819 
1820  m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(
1821  m_tmpSolverBodyPool[i].m_angularVelocity +
1822  m_tmpSolverBodyPool[i].m_externalTorqueImpulse);
1823 
1824  if (infoGlobal.m_splitImpulse)
1825  m_tmpSolverBodyPool[i].m_originalBody->setWorldTransform(m_tmpSolverBodyPool[i].m_worldTransform);
1826 
1827  m_tmpSolverBodyPool[i].m_originalBody->setCompanionId(-1);
1828  }
1829  }
1830 }
1831 
1833 {
1834  BT_PROFILE("solveGroupCacheFriendlyFinish");
1835 
1836  if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
1837  {
1839  }
1840 
1842  writeBackBodies(0, m_tmpSolverBodyPool.size(), infoGlobal);
1843 
1848 
1850  return 0.f;
1851 }
1852 
1854 btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer, btDispatcher* /*dispatcher*/)
1855 {
1856  BT_PROFILE("solveGroup");
1857  //you need to provide at least some bodies
1858 
1859  solveGroupCacheFriendlySetup(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
1860 
1861  solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
1862 
1863  solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal);
1864 
1865  return 0.f;
1866 }
1867 
1869 {
1870  m_btSeed2 = 0;
1871 }
@ SOLVER_SIMD
@ SOLVER_ENABLE_FRICTION_DIRECTION_CACHING
@ SOLVER_USE_WARMSTARTING
@ SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
@ SOLVER_RANDMIZE_ORDER
@ SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION
@ SOLVER_USE_2_FRICTION_DIRECTIONS
@ 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
const T & btMax(const T &a, const T &b)
Definition: btMinMax.h:27
int maxIterations
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:888
#define BT_PROFILE(name)
Definition: btQuickprof.h:198
static int uniqueId
Definition: btRigidBody.cpp:27
@ BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY
Definition: btRigidBody.h:47
@ BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT
BT_ENABLE_GYROPSCOPIC_FORCE flags is enabled by default in Bullet 2.83 and onwards.
Definition: btRigidBody.h:45
@ BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD
Definition: btRigidBody.h:46
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
btScalar btSqrt(btScalar y)
Definition: btScalar.h:466
btScalar btFabs(btScalar x)
Definition: btScalar.h:497
#define SIMD_INFINITY
Definition: btScalar.h:544
#define SIMD_EPSILON
Definition: btScalar.h:543
#define btAssert(x)
Definition: btScalar.h:153
static btScalar gResolveSingleConstraintRowGeneric_scalar_reference(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &c)
This is the scalar reference implementation of solving a single constraint row, the innerloop of the ...
static btScalar gResolveSingleConstraintRowLowerLimit_scalar_reference(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &c)
static btScalar gResolveSplitPenetrationImpulse_sse2(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &c)
static btScalar gResolveSplitPenetrationImpulse_scalar_reference(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &c)
btScalar(* btSingleConstraintRowSolver)(btSolverBody &, btSolverBody &, const btSolverConstraint &)
static T sum(const btAlignedObjectArray< T > &items)
#define btSimdScalar
Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later,...
Definition: btSolverBody.h:99
void btPlaneSpace1(const T &n, T &p, T &q)
Definition: btVector3.h:1251
void resizeNoInitialize(int newsize)
resize changes the number of elements in the array.
int size() const
return the number of elements in the array
T & expand(const T &fillValue=T())
void resize(int newsize, const T &fillData=T())
btCollisionObject can be used to manage collision detection objects.
bool isStaticOrKinematicObject() const
btTransform & getWorldTransform()
const btVector3 & getAnisotropicFriction() const
int getInternalType() const
reserved for Bullet internal usage
int getWorldArrayIndex() const
bool hasAnisotropicFriction(int frictionMode=CF_ANISOTROPIC_FRICTION) const
void setCompanionId(int id)
bool isKinematicObject() const
int getCompanionId() const
The btDispatcher interface class can be used in combination with broadphase to dispatch calculations ...
Definition: btDispatcher.h:77
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations.
Definition: btIDebugDraw.h:27
ManifoldContactPoint collects and maintains persistent contactpoints.
btScalar m_frictionCFM
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
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
const btCollisionObject * getBody1() const
const btCollisionObject * getBody0() const
const btManifoldPoint & getContactPoint(int index) const
btScalar getContactProcessingThreshold() const
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:60
const btVector3 & getAngularVelocity() const
Definition: btRigidBody.h:402
btVector3 getVelocityInLocalPoint(const btVector3 &rel_pos) const
Definition: btRigidBody.h:419
int getFlags() const
Definition: btRigidBody.h:561
const btVector3 & getLinearFactor() const
Definition: btRigidBody.h:254
btScalar getInvMass() const
Definition: btRigidBody.h:263
static const btRigidBody * upcast(const btCollisionObject *colObj)
to keep collision detection and dynamics separate we don't store a rigidbody pointer but a rigidbody ...
Definition: btRigidBody.h:189
const btVector3 & getTotalTorque() const
Definition: btRigidBody.h:284
btVector3 computeGyroscopicImpulseImplicit_World(btScalar dt) const
perform implicit force computation in world space
btVector3 computeGyroscopicImpulseImplicit_Body(btScalar step) const
perform implicit force computation in body space (inertial frame)
const btVector3 & getLinearVelocity() const
Definition: btRigidBody.h:398
const btVector3 & getTotalForce() const
Definition: btRigidBody.h:279
const btMatrix3x3 & getInvInertiaTensorWorld() const
Definition: btRigidBody.h:265
btScalar computeImpulseDenominator(const btVector3 &pos, const btVector3 &normal) const
Definition: btRigidBody.h:435
const btVector3 & getAngularFactor() const
Definition: btRigidBody.h:532
btVector3 computeGyroscopicForceExplicit(btScalar maxGyroscopicForce) const
explicit version is best avoided, it gains energy
btSingleConstraintRowSolver getScalarConstraintRowSolverLowerLimit()
Various implementations of solving a single constraint row using an inequality (lower limit) constrai...
void initSolverBody(btSolverBody *solverBody, btCollisionObject *collisionObject, btScalar timeStep)
btSingleConstraintRowSolver getSSE2ConstraintRowSolverGeneric()
virtual void convertBodies(btCollisionObject **bodies, int numBodies, const btContactSolverInfo &infoGlobal)
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)
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject **bodies, int numBodies, const btContactSolverInfo &infoGlobal)
virtual void convertJoints(btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal)
void convertJoint(btSolverConstraint *currentConstraintRow, btTypedConstraint *constraint, const btTypedConstraint::btConstraintInfo1 &info1, int solverBodyIdA, int solverBodyIdB, const btContactSolverInfo &infoGlobal)
virtual btScalar solveGroup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifold, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &info, btIDebugDraw *debugDrawer, btDispatcher *dispatcher)
btSequentialImpulseConstraintSolver Sequentially applies impulses
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)
virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
unsigned long m_btSeed2
m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
virtual void reset()
clear internal cached data and reset random seed
virtual void convertContacts(btPersistentManifold **manifoldPtr, int numManifolds, const btContactSolverInfo &infoGlobal)
btScalar resolveSingleConstraintRowLowerLimit(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
static void applyAnisotropicFriction(btCollisionObject *colObj, btVector3 &frictionDirection, int frictionMode)
btSingleConstraintRowSolver getSSE2ConstraintRowSolverLowerLimit()
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)
btAlignedObjectArray< btSolverBody > m_tmpSolverBodyPool
btSingleConstraintRowSolver m_resolveSingleConstraintRowLowerLimit
btSingleConstraintRowSolver getScalarConstraintRowSolverGeneric()
Various implementations of solving a single constraint row using a generic equality constraint,...
btAlignedObjectArray< btTypedConstraint::btConstraintInfo1 > m_tmpConstraintSizesPool
btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverGeneric()
void writeBackBodies(int iBegin, int iEnd, const btContactSolverInfo &infoGlobal)
btAlignedObjectArray< int > m_kinematicBodyUniqueIdToSolverBodyTable
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
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.)
btSingleConstraintRowSolver m_resolveSingleConstraintRowGeneric
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)
btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverLowerLimit()
void writeBackContacts(int iBegin, int iEnd, 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)
btScalar resolveSplitPenetrationImpulse(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
void setupContactConstraint(btSolverConstraint &solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint &cp, const btContactSolverInfo &infoGlobal, btScalar &relaxation, const btVector3 &rel_pos1, const btVector3 &rel_pos2)
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:108
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:113
void setIdentity()
Set this transformation to the identity.
Definition: btTransform.h:166
TypedConstraint is the baseclass for Bullet constraints and vehicles.
virtual void solveConstraintObsolete(btSolverBody &, btSolverBody &, btScalar)
internal method used by the constraint solver, don't use them directly
void setEnabled(bool enabled)
int getOverrideNumSolverIterations() const
virtual void buildJacobian()
internal method used by the constraint solver, don't use them directly
const btJointFeedback * getJointFeedback() const
const btRigidBody & getRigidBodyA() const
virtual void getInfo2(btConstraintInfo2 *info)=0
internal method used by the constraint solver, don't use them directly
void internalSetAppliedImpulse(btScalar appliedImpulse)
internal method used by the constraint solver, don't use them directly
bool isEnabled() const
btScalar getBreakingImpulseThreshold() const
virtual void getInfo1(btConstraintInfo1 *info)=0
internal method used by the constraint solver, don't use them directly
const btRigidBody & getRigidBodyB() const
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:82
bool isZero() const
Definition: btVector3.h:683
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:303
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:257
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:380
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:229
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:640
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:251
void setZero()
Definition: btVector3.h:671
bool fuzzyZero() const
Definition: btVector3.h:688
btVector3 m_appliedForceBodyA
btVector3 m_appliedTorqueBodyA
btVector3 m_appliedForceBodyB
btVector3 m_appliedTorqueBodyB
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
Definition: btSolverBody.h:105
const btVector3 & getDeltaLinearVelocity() const
Definition: btSolverBody.h:174
btVector3 m_linearFactor
Definition: btSolverBody.h:111
btVector3 m_invMass
Definition: btSolverBody.h:112
btVector3 m_angularVelocity
Definition: btSolverBody.h:116
btRigidBody * m_originalBody
Definition: btSolverBody.h:120
void internalApplyPushImpulse(const btVector3 &linearComponent, const btVector3 &angularComponent, btScalar impulseMagnitude)
Definition: btSolverBody.h:165
const btVector3 & getDeltaAngularVelocity() const
Definition: btSolverBody.h:179
const btVector3 & getTurnVelocity() const
Definition: btSolverBody.h:189
btVector3 m_linearVelocity
Definition: btSolverBody.h:115
void getVelocityInLocalPointNoDelta(const btVector3 &rel_pos, btVector3 &velocity) const
Definition: btSolverBody.h:131
const btVector3 & getPushVelocity() const
Definition: btSolverBody.h:184
btTransform m_worldTransform
Definition: btSolverBody.h:107
btVector3 & internalGetDeltaAngularVelocity()
Definition: btSolverBody.h:202
void internalSetInvMass(const btVector3 &invMass)
Definition: btSolverBody.h:217
btVector3 & internalGetTurnVelocity()
Definition: btSolverBody.h:227
btVector3 m_angularFactor
Definition: btSolverBody.h:110
void internalApplyImpulse(const btVector3 &linearComponent, const btVector3 &angularComponent, const btScalar impulseMagnitude)
Definition: btSolverBody.h:243
btVector3 m_externalTorqueImpulse
Definition: btSolverBody.h:118
const btVector3 & internalGetInvMass() const
Definition: btSolverBody.h:212
btVector3 & internalGetPushVelocity()
Definition: btSolverBody.h:222
btVector3 m_externalForceImpulse
Definition: btSolverBody.h:117
btVector3 & internalGetDeltaLinearVelocity()
some internal methods, don't use them
Definition: btSolverBody.h:197
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
btVector3 m_relpos1CrossNormal
btSimdScalar m_appliedImpulse
btSimdScalar m_appliedPushImpulse
btVector3 m_relpos2CrossNormal