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 }
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:543
btSequentialImpulseConstraintSolver::m_tmpSolverContactFrictionConstraintPool
btConstraintArray m_tmpSolverContactFrictionConstraintPool
Definition: btSequentialImpulseConstraintSolver.h:59
btTypedConstraint::internalSetAppliedImpulse
void internalSetAppliedImpulse(btScalar appliedImpulse)
internal method used by the constraint solver, don't use them directly
Definition: btTypedConstraint.h:181
btSolverConstraint::m_lowerLimit
btScalar m_lowerLimit
Definition: btSolverConstraint.h:51
btSequentialImpulseConstraintSolver::writeBackContacts
void writeBackContacts(int iBegin, int iEnd, const btContactSolverInfo &infoGlobal)
Definition: btSequentialImpulseConstraintSolver.cpp:1761
btSolverBody
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
Definition: btSolverBody.h:103
btTypedConstraint
TypedConstraint is the baseclass for Bullet constraints and vehicles.
Definition: btTypedConstraint.h:74
btContactSolverInfoData::m_linearSlop
btScalar m_linearSlop
Definition: btContactSolverInfo.h:57
btSolverConstraint::m_relpos2CrossNormal
btVector3 m_relpos2CrossNormal
Definition: btSolverConstraint.h:37
btCollisionObject
btCollisionObject can be used to manage collision detection objects.
Definition: btCollisionObject.h:48
btSolverBody::m_originalBody
btRigidBody * m_originalBody
Definition: btSolverBody.h:120
btRigidBody
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:59
btManifoldPoint::m_lateralFrictionDir2
btVector3 m_lateralFrictionDir2
Definition: btManifoldPoint.h:140
SOLVER_SIMD
Definition: btContactSolverInfo.h:30
btManifoldPoint::m_contactMotion2
btScalar m_contactMotion2
Definition: btManifoldPoint.h:123
btRigidBody::getTotalTorque
const btVector3 & getTotalTorque() const
Definition: btRigidBody.h:284
btSolverBody::internalSetInvMass
void internalSetInvMass(const btVector3 &invMass)
Definition: btSolverBody.h:217
btSolverBody::m_linearVelocity
btVector3 m_linearVelocity
Definition: btSolverBody.h:115
btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD
btScalar resolveSingleConstraintRowGenericSIMD(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
Definition: btSequentialImpulseConstraintSolver.cpp:261
btContactSolverInfoData::m_splitImpulsePenetrationThreshold
btScalar m_splitImpulsePenetrationThreshold
Definition: btContactSolverInfo.h:55
btSequentialImpulseConstraintSolver::solveGroup
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
Definition: btSequentialImpulseConstraintSolver.cpp:1854
btCpuFeatureUtility::CPU_FEATURE_FMA3
Definition: btCpuFeatureUtility.h:29
BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT
BT_ENABLE_GYROPSCOPIC_FORCE flags is enabled by default in Bullet 2.83 and onwards.
Definition: btRigidBody.h:45
dot
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:888
btPlaneSpace1
void btPlaneSpace1(const T &n, T &p, T &q)
Definition: btVector3.h:1251
btSolverConstraint::m_friction
btScalar m_friction
Definition: btSolverConstraint.h:46
btManifoldPoint::m_contactCFM
btScalar m_contactCFM
Definition: btManifoldPoint.h:126
btSequentialImpulseConstraintSolver::addFrictionConstraint
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.)
Definition: btSequentialImpulseConstraintSolver.cpp:608
btSequentialImpulseConstraintSolver::getSSE4_1ConstraintRowSolverLowerLimit
btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverLowerLimit()
btSolverConstraint::m_appliedPushImpulse
btSimdScalar m_appliedPushImpulse
Definition: btSolverConstraint.h:43
btVector3::length
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:257
btManifoldPoint::m_contactERP
btScalar m_contactERP
Definition: btManifoldPoint.h:131
btSolverBody::internalGetInvMass
const btVector3 & internalGetInvMass() const
Definition: btSolverBody.h:212
btManifoldPoint::m_contactPointFlags
int m_contactPointFlags
Definition: btManifoldPoint.h:116
BT_CONTACT_FLAG_HAS_CONTACT_CFM
Definition: btManifoldPoint.h:43
btContactSolverInfo
Definition: btContactSolverInfo.h:72
btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulse
btScalar resolveSplitPenetrationImpulse(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
Definition: btSequentialImpulseConstraintSolver.h:139
maxIterations
int maxIterations
Definition: btQuantizedBvh.cpp:349
btVector3::setValue
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:640
btSolverConstraint
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
Definition: btSolverConstraint.h:29
btManifoldPoint::getPositionWorldOnA
const btVector3 & getPositionWorldOnA() const
Definition: btManifoldPoint.h:151
btTypedConstraint::getInfo1
virtual void getInfo1(btConstraintInfo1 *info)=0
internal method used by the constraint solver, don't use them directly
btRigidBody::getAngularVelocity
const btVector3 & getAngularVelocity() const
Definition: btRigidBody.h:402
btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations
virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
Definition: btSequentialImpulseConstraintSolver.cpp:1726
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric
btScalar resolveSingleConstraintRowGeneric(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
Definition: btSequentialImpulseConstraintSolver.cpp:267
btTypedConstraint::getRigidBodyA
const btRigidBody & getRigidBodyA() const
Definition: btTypedConstraint.h:214
btSolverBody::getPushVelocity
const btVector3 & getPushVelocity() const
Definition: btSolverBody.h:184
btDispatcher
The btDispatcher interface class can be used in combination with broadphase to dispatch calculations ...
Definition: btDispatcher.h:76
btSolverBody::internalGetTurnVelocity
btVector3 & internalGetTurnVelocity()
Definition: btSolverBody.h:227
SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION
Definition: btContactSolverInfo.h:28
btSequentialImpulseConstraintSolver::m_orderFrictionConstraintPool
btAlignedObjectArray< int > m_orderFrictionConstraintPool
Definition: btSequentialImpulseConstraintSolver.h:64
btTypedConstraint::getBreakingImpulseThreshold
btScalar getBreakingImpulseThreshold() const
Definition: btTypedConstraint.h:191
gResolveSplitPenetrationImpulse_scalar_reference
static btScalar gResolveSplitPenetrationImpulse_scalar_reference(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &c)
Definition: btSequentialImpulseConstraintSolver.cpp:282
btRigidBody::computeGyroscopicForceExplicit
btVector3 computeGyroscopicForceExplicit(btScalar maxGyroscopicForce) const
explicit version is best avoided, it gains energy
Definition: btRigidBody.cpp:279
btSequentialImpulseConstraintSolver::setupFrictionConstraint
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.)
Definition: btSequentialImpulseConstraintSolver.cpp:516
btManifoldPoint::m_normalWorldOnB
btVector3 m_normalWorldOnB
Definition: btManifoldPoint.h:100
btVector3::cross
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:380
btSequentialImpulseConstraintSolver::restitutionCurve
btScalar restitutionCurve(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold)
Definition: btSequentialImpulseConstraintSolver.cpp:492
btPersistentManifold::getBody0
const btCollisionObject * getBody0() const
Definition: btPersistentManifold.h:105
btSingleConstraintRowSolver
btScalar(* btSingleConstraintRowSolver)(btSolverBody &, btSolverBody &, const btSolverConstraint &)
Definition: btSequentialImpulseConstraintSolver.h:30
btSequentialImpulseConstraintSolver::m_btSeed2
unsigned long m_btSeed2
m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
Definition: btSequentialImpulseConstraintSolver.h:108
btSequentialImpulseConstraintSolver::applyAnisotropicFriction
static void applyAnisotropicFriction(btCollisionObject *colObj, btVector3 &frictionDirection, int frictionMode)
Definition: btSequentialImpulseConstraintSolver.cpp:502
btSolverConstraint::m_contactNormal2
btVector3 m_contactNormal2
Definition: btSolverConstraint.h:38
btContactSolverInfoData::m_splitImpulseTurnErp
btScalar m_splitImpulseTurnErp
Definition: btContactSolverInfo.h:56
SOLVER_USE_WARMSTARTING
Definition: btContactSolverInfo.h:25
btCpuFeatureUtility::CPU_FEATURE_SSE4_1
Definition: btCpuFeatureUtility.h:30
btTypedConstraint::getJointFeedback
const btJointFeedback * getJointFeedback() const
Definition: btTypedConstraint.h:267
btSolverBody::m_externalForceImpulse
btVector3 m_externalForceImpulse
Definition: btSolverBody.h:117
btSequentialImpulseConstraintSolver::writeBackJoints
void writeBackJoints(int iBegin, int iEnd, const btContactSolverInfo &infoGlobal)
Definition: btSequentialImpulseConstraintSolver.cpp:1781
btSolverConstraint::m_cfm
btScalar m_cfm
Definition: btSolverConstraint.h:49
BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD
Definition: btRigidBody.h:46
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:229
btPersistentManifold::getNumContacts
int getNumContacts() const
Definition: btPersistentManifold.h:120
btContactSolverInfoData::m_sor
btScalar m_sor
Definition: btContactSolverInfo.h:46
btContactSolverInfoData::m_damping
btScalar m_damping
Definition: btContactSolverInfo.h:40
btContactSolverInfoData::m_maxGyroscopicForce
btScalar m_maxGyroscopicForce
Definition: btContactSolverInfo.h:63
btRigidBody.h
btSolverBody::m_linearFactor
btVector3 m_linearFactor
Definition: btSolverBody.h:111
btSequentialImpulseConstraintSolver::m_tmpSolverContactRollingFrictionConstraintPool
btConstraintArray m_tmpSolverContactRollingFrictionConstraintPool
Definition: btSequentialImpulseConstraintSolver.h:60
btRigidBody::computeGyroscopicImpulseImplicit_World
btVector3 computeGyroscopicImpulseImplicit_World(btScalar dt) const
perform implicit force computation in world space
Definition: btRigidBody.cpp:332
btSequentialImpulseConstraintSolver::m_kinematicBodyUniqueIdToSolverBodyTable
btAlignedObjectArray< int > m_kinematicBodyUniqueIdToSolverBodyTable
Definition: btSequentialImpulseConstraintSolver.h:75
btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit
btScalar resolveSingleConstraintRowLowerLimit(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
Definition: btSequentialImpulseConstraintSolver.cpp:277
BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY
Definition: btRigidBody.h:47
btJointFeedback
Definition: btTypedConstraint.h:63
btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION
Definition: btCollisionObject.h:160
btSolverConstraint::m_rhsPenetration
btScalar m_rhsPenetration
Definition: btSolverConstraint.h:53
btContactSolverInfoData::m_warmstartingFactor
btScalar m_warmstartingFactor
Definition: btContactSolverInfo.h:58
btContactSolverInfoData::m_frictionERP
btScalar m_frictionERP
Definition: btContactSolverInfo.h:51
btSolverBody::internalApplyImpulse
void internalApplyImpulse(const btVector3 &linearComponent, const btVector3 &angularComponent, const btScalar impulseMagnitude)
Definition: btSolverBody.h:243
btVector3::setZero
void setZero()
Definition: btVector3.h:671
btManifoldPoint::getDistance
btScalar getDistance() const
Definition: btManifoldPoint.h:142
btSolverConstraint::m_originalContactPoint
void * m_originalContactPoint
Definition: btSolverConstraint.h:55
btCollisionObject::isKinematicObject
bool isKinematicObject() const
Definition: btCollisionObject.h:200
btCpuFeatureUtility.h
btTypedConstraint::btConstraintInfo1::m_numConstraintRows
int m_numConstraintRows
Definition: btTypedConstraint.h:115
btSequentialImpulseConstraintSolver::reset
virtual void reset()
clear internal cached data and reset random seed
Definition: btSequentialImpulseConstraintSolver.cpp:1868
btSolverBody::getTurnVelocity
const btVector3 & getTurnVelocity() const
Definition: btSolverBody.h:189
btManifoldPoint::m_combinedRestitution
btScalar m_combinedRestitution
Definition: btManifoldPoint.h:106
btSolverBody::m_angularFactor
btVector3 m_angularFactor
Definition: btSolverBody.h:110
btSequentialImpulseConstraintSolver::m_resolveSingleConstraintRowGeneric
btSingleConstraintRowSolver m_resolveSingleConstraintRowGeneric
Definition: btSequentialImpulseConstraintSolver.h:77
btSequentialImpulseConstraintSolver::writeBackBodies
void writeBackBodies(int iBegin, int iEnd, const btContactSolverInfo &infoGlobal)
Definition: btSequentialImpulseConstraintSolver.cpp:1804
btSequentialImpulseConstraintSolver::solveSingleIteration
virtual btScalar solveSingleIteration(int iteration, btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
Definition: btSequentialImpulseConstraintSolver.cpp:1522
btSolverConstraint::m_angularComponentB
btVector3 m_angularComponentB
Definition: btSolverConstraint.h:41
btSolverConstraint::m_contactNormal1
btVector3 m_contactNormal1
Definition: btSolverConstraint.h:35
btTransform::setIdentity
void setIdentity()
Set this transformation to the identity.
Definition: btTransform.h:166
btMax
const T & btMax(const T &a, const T &b)
Definition: btMinMax.h:27
BT_CONTACT_FLAG_FRICTION_ANCHOR
Definition: btManifoldPoint.h:46
btTypedConstraint::getRigidBodyB
const btRigidBody & getRigidBodyB() const
Definition: btTypedConstraint.h:218
btSolverBody::m_angularVelocity
btVector3 m_angularVelocity
Definition: btSolverBody.h:116
btCollisionObject::isStaticOrKinematicObject
bool isStaticOrKinematicObject() const
Definition: btCollisionObject.h:205
btSolverAnalyticsData::m_islandId
int m_islandId
Definition: btSequentialImpulseConstraintSolver.h:41
btCollisionObject::getWorldTransform
btTransform & getWorldTransform()
Definition: btCollisionObject.h:367
btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations
virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
Definition: btSequentialImpulseConstraintSolver.cpp:1693
btSolverAnalyticsData::m_remainingLeastSquaresResidual
double m_remainingLeastSquaresResidual
Definition: btSequentialImpulseConstraintSolver.h:46
btJointFeedback::m_appliedForceBodyB
btVector3 m_appliedForceBodyB
Definition: btTypedConstraint.h:69
btSolverBody::m_invMass
btVector3 m_invMass
Definition: btSolverBody.h:112
btVector3::isZero
bool isZero() const
Definition: btVector3.h:683
btAssert
#define btAssert(x)
Definition: btScalar.h:153
btSequentialImpulseConstraintSolver::getSSE4_1ConstraintRowSolverGeneric
btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverGeneric()
btSolverAnalyticsData::m_numIterationsUsed
int m_numIterationsUsed
Definition: btSequentialImpulseConstraintSolver.h:45
btRigidBody::getLinearFactor
const btVector3 & getLinearFactor() const
Definition: btRigidBody.h:254
btFabs
btScalar btFabs(btScalar x)
Definition: btScalar.h:497
btJointFeedback::m_appliedForceBodyA
btVector3 m_appliedForceBodyA
Definition: btTypedConstraint.h:67
btSequentialImpulseConstraintSolver::convertContacts
virtual void convertContacts(btPersistentManifold **manifoldPtr, int numManifolds, const btContactSolverInfo &infoGlobal)
Definition: btSequentialImpulseConstraintSolver.cpp:1145
btManifoldPoint::m_combinedContactDamping1
btScalar m_combinedContactDamping1
Definition: btManifoldPoint.h:132
btSimdScalar
#define btSimdScalar
Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later,...
Definition: btSolverBody.h:99
btTypedConstraint::isEnabled
bool isEnabled() const
Definition: btTypedConstraint.h:201
btIDebugDraw
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations.
Definition: btIDebugDraw.h:26
btManifoldPoint
ManifoldContactPoint collects and maintains persistent contactpoints.
Definition: btManifoldPoint.h:51
btManifoldPoint::m_contactMotion1
btScalar m_contactMotion1
Definition: btManifoldPoint.h:122
btManifoldPoint::m_combinedSpinningFriction
btScalar m_combinedSpinningFriction
Definition: btManifoldPoint.h:105
btSequentialImpulseConstraintSolver::getScalarConstraintRowSolverGeneric
btSingleConstraintRowSolver getScalarConstraintRowSolverGeneric()
Various implementations of solving a single constraint row using a generic equality constraint,...
Definition: btSequentialImpulseConstraintSolver.cpp:387
btTypedConstraint.h
btMinMax.h
btAlignedObjectArray::resize
void resize(int newsize, const T &fillData=T())
Definition: btAlignedObjectArray.h:203
btTransform::getBasis
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:108
btContactSolverInfoData::m_timeStep
btScalar m_timeStep
Definition: btContactSolverInfo.h:42
btPersistentManifold::getContactProcessingThreshold
btScalar getContactProcessingThreshold() const
Definition: btPersistentManifold.h:145
btTypedConstraint::getOverrideNumSolverIterations
int getOverrideNumSolverIterations() const
Definition: btTypedConstraint.h:150
btRigidBody::getAngularFactor
const btVector3 & getAngularFactor() const
Definition: btRigidBody.h:532
btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject **bodies, int numBodies, const btContactSolverInfo &infoGlobal)
Definition: btSequentialImpulseConstraintSolver.cpp:1832
btSequentialImpulseConstraintSolver::getSSE2ConstraintRowSolverGeneric
btSingleConstraintRowSolver getSSE2ConstraintRowSolverGeneric()
btSolverBody::internalApplyPushImpulse
void internalApplyPushImpulse(const btVector3 &linearComponent, const btVector3 &angularComponent, btScalar impulseMagnitude)
Definition: btSolverBody.h:165
btSequentialImpulseConstraintSolver::getScalarConstraintRowSolverLowerLimit
btSingleConstraintRowSolver getScalarConstraintRowSolverLowerLimit()
Various implementations of solving a single constraint row using an inequality (lower limit) constrai...
Definition: btSequentialImpulseConstraintSolver.cpp:392
btCollisionObject::getAnisotropicFriction
const btVector3 & getAnisotropicFriction() const
Definition: btCollisionObject.h:169
btSequentialImpulseConstraintSolver::m_resolveSingleConstraintRowLowerLimit
btSingleConstraintRowSolver m_resolveSingleConstraintRowLowerLimit
Definition: btSequentialImpulseConstraintSolver.h:78
btSequentialImpulseConstraintSolver::setupContactConstraint
void setupContactConstraint(btSolverConstraint &solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint &cp, const btContactSolverInfo &infoGlobal, btScalar &relaxation, const btVector3 &rel_pos1, const btVector3 &rel_pos2)
Definition: btSequentialImpulseConstraintSolver.cpp:792
btSolverConstraint::m_rhs
btScalar m_rhs
Definition: btSolverConstraint.h:48
gResolveSingleConstraintRowLowerLimit_scalar_reference
static btScalar gResolveSingleConstraintRowLowerLimit_scalar_reference(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &c)
Definition: btSequentialImpulseConstraintSolver.cpp:77
btSolverBody::internalGetDeltaAngularVelocity
btVector3 & internalGetDeltaAngularVelocity()
Definition: btSolverBody.h:202
btVector3::fuzzyZero
bool fuzzyZero() const
Definition: btVector3.h:688
btVector3::m_floats
btScalar m_floats[4]
Definition: btVector3.h:111
btManifoldPoint::m_frictionCFM
btScalar m_frictionCFM
Definition: btManifoldPoint.h:135
btRigidBody::getInvMass
btScalar getInvMass() const
Definition: btRigidBody.h:263
BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING
Definition: btManifoldPoint.h:45
btSequentialImpulseConstraintSolver::m_orderTmpConstraintPool
btAlignedObjectArray< int > m_orderTmpConstraintPool
Definition: btSequentialImpulseConstraintSolver.h:62
btCollisionObject::setCompanionId
void setCompanionId(int id)
Definition: btCollisionObject.h:451
btSolverConstraint::m_jacDiagABInv
btScalar m_jacDiagABInv
Definition: btSolverConstraint.h:47
btSequentialImpulseConstraintSolver::m_analyticsData
btSolverAnalyticsData m_analyticsData
Definition: btSequentialImpulseConstraintSolver.h:212
btContactSolverInfoData::m_splitImpulse
int m_splitImpulse
Definition: btContactSolverInfo.h:54
SIMD_INFINITY
#define SIMD_INFINITY
Definition: btScalar.h:544
btSolverBody::getVelocityInLocalPointNoDelta
void getVelocityInLocalPointNoDelta(const btVector3 &rel_pos, btVector3 &velocity) const
Definition: btSolverBody.h:131
btSolverBody::internalGetPushVelocity
btVector3 & internalGetPushVelocity()
Definition: btSolverBody.h:222
btSolverBody::internalGetDeltaLinearVelocity
btVector3 & internalGetDeltaLinearVelocity()
some internal methods, don't use them
Definition: btSolverBody.h:197
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:80
btManifoldPoint::getPositionWorldOnB
const btVector3 & getPositionWorldOnB() const
Definition: btManifoldPoint.h:157
btContactSolverInfoData::m_globalCfm
btScalar m_globalCfm
Definition: btContactSolverInfo.h:50
btTypedConstraint::btConstraintInfo1
Definition: btTypedConstraint.h:113
btSolverAnalyticsData::m_numBodies
int m_numBodies
Definition: btSequentialImpulseConstraintSolver.h:42
btSequentialImpulseConstraintSolver::m_maxOverrideNumSolverIterations
int m_maxOverrideNumSolverIterations
Definition: btSequentialImpulseConstraintSolver.h:66
btSequentialImpulseConstraintSolver::convertJoint
void convertJoint(btSolverConstraint *currentConstraintRow, btTypedConstraint *constraint, const btTypedConstraint::btConstraintInfo1 &info1, int solverBodyIdA, int solverBodyIdB, const btContactSolverInfo &infoGlobal)
Definition: btSequentialImpulseConstraintSolver.cpp:1158
btManifoldPoint::m_combinedRollingFriction
btScalar m_combinedRollingFriction
Definition: btManifoldPoint.h:104
btSequentialImpulseConstraintSolver::getSSE2ConstraintRowSolverLowerLimit
btSingleConstraintRowSolver getSSE2ConstraintRowSolverLowerLimit()
btContactSolverInfoData::m_solverMode
int m_solverMode
Definition: btContactSolverInfo.h:60
btPersistentManifold
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
Definition: btPersistentManifold.h:63
btTransform::getOrigin
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:113
btRigidBody::getLinearVelocity
const btVector3 & getLinearVelocity() const
Definition: btRigidBody.h:398
btRigidBody::getVelocityInLocalPoint
btVector3 getVelocityInLocalPoint(const btVector3 &rel_pos) const
Definition: btRigidBody.h:419
btSequentialImpulseConstraintSolver::convertContact
void convertContact(btPersistentManifold *manifold, const btContactSolverInfo &infoGlobal)
Definition: btSequentialImpulseConstraintSolver.cpp:995
btRigidBody::getFlags
int getFlags() const
Definition: btRigidBody.h:561
btSolverConstraint::m_upperLimit
btScalar m_upperLimit
Definition: btSolverConstraint.h:52
btContactSolverInfoData::m_leastSquaresResidualThreshold
btScalar m_leastSquaresResidualThreshold
Definition: btContactSolverInfo.h:65
BT_CONTACT_FLAG_HAS_CONTACT_ERP
Definition: btManifoldPoint.h:44
btSequentialImpulseConstraintSolver::m_fixedBodyId
int m_fixedBodyId
Definition: btSequentialImpulseConstraintSolver.h:67
btCollisionObject::getWorldArrayIndex
int getWorldArrayIndex() const
Definition: btCollisionObject.h:456
btSolverConstraint::m_overrideNumSolverIterations
int m_overrideNumSolverIterations
Definition: btSolverConstraint.h:60
btManifoldPoint::m_appliedImpulseLateral1
btScalar m_appliedImpulseLateral1
Definition: btManifoldPoint.h:120
btCollisionObject::CO_FEATHERSTONE_LINK
Definition: btCollisionObject.h:153
btTypedConstraint::btConstraintInfo1::nub
int nub
Definition: btTypedConstraint.h:115
btContactSolverInfoData::m_erp
btScalar m_erp
Definition: btContactSolverInfo.h:47
btSequentialImpulseConstraintSolver::convertJoints
virtual void convertJoints(btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal)
Definition: btSequentialImpulseConstraintSolver.cpp:1290
btSequentialImpulseConstraintSolver::convertBodies
virtual void convertBodies(btCollisionObject **bodies, int numBodies, const btContactSolverInfo &infoGlobal)
Definition: btSequentialImpulseConstraintSolver.cpp:1354
btSolverConstraint::m_solverBodyIdA
int m_solverBodyIdA
Definition: btSolverConstraint.h:62
btSolverAnalyticsData::m_numContactManifolds
int m_numContactManifolds
Definition: btSequentialImpulseConstraintSolver.h:43
btSequentialImpulseConstraintSolver::m_tmpSolverContactConstraintPool
btConstraintArray m_tmpSolverContactConstraintPool
Definition: btSequentialImpulseConstraintSolver.h:57
btSolverBody::m_externalTorqueImpulse
btVector3 m_externalTorqueImpulse
Definition: btSolverBody.h:118
btSequentialImpulseConstraintSolver.h
btPersistentManifold.h
btTypedConstraint::solveConstraintObsolete
virtual void solveConstraintObsolete(btSolverBody &, btSolverBody &, btScalar)
internal method used by the constraint solver, don't use them directly
Definition: btTypedConstraint.h:212
btJointFeedback::m_appliedTorqueBodyB
btVector3 m_appliedTorqueBodyB
Definition: btTypedConstraint.h:70
btAlignedObjectArray::expand
T & expand(const T &fillValue=T())
Definition: btAlignedObjectArray.h:242
btSequentialImpulseConstraintSolver::m_tmpConstraintSizesPool
btAlignedObjectArray< btTypedConstraint::btConstraintInfo1 > m_tmpConstraintSizesPool
Definition: btSequentialImpulseConstraintSolver.h:65
btSequentialImpulseConstraintSolver::m_orderNonContactConstraintPool
btAlignedObjectArray< int > m_orderNonContactConstraintPool
Definition: btSequentialImpulseConstraintSolver.h:63
btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD
btScalar resolveSingleConstraintRowLowerLimitSIMD(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
Definition: btSequentialImpulseConstraintSolver.cpp:272
btSequentialImpulseConstraintSolver::m_cachedSolverMode
int m_cachedSolverMode
Definition: btSequentialImpulseConstraintSolver.h:80
btSequentialImpulseConstraintSolver::m_tmpSolverBodyPool
btAlignedObjectArray< btSolverBody > m_tmpSolverBodyPool
Definition: btSequentialImpulseConstraintSolver.h:56
btIDebugDraw.h
btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
Definition: btSequentialImpulseConstraintSolver.cpp:1399
btSequentialImpulseConstraintSolver::m_resolveSplitPenetrationImpulse
btSingleConstraintRowSolver m_resolveSplitPenetrationImpulse
Definition: btSequentialImpulseConstraintSolver.h:79
btAlignedObjectArray::reserve
void reserve(int _Count)
Definition: btAlignedObjectArray.h:280
btSolverConstraint::m_solverBodyIdB
int m_solverBodyIdB
Definition: btSolverConstraint.h:63
btQuickprof.h
btCollisionObject::getInternalType
int getInternalType() const
reserved for Bullet internal usage
Definition: btCollisionObject.h:362
btSolverAnalyticsData::m_numSolverCalls
int m_numSolverCalls
Definition: btSequentialImpulseConstraintSolver.h:44
btSolverBody::getDeltaLinearVelocity
const btVector3 & getDeltaLinearVelocity() const
Definition: btSolverBody.h:174
btCpuFeatureUtility::getCpuFeatures
static int getCpuFeatures()
Definition: btCpuFeatureUtility.h:34
btTypedConstraint::setEnabled
void setEnabled(bool enabled)
Definition: btTypedConstraint.h:206
btContactSolverInfoData::m_restitutionVelocityThreshold
btScalar m_restitutionVelocityThreshold
Definition: btContactSolverInfo.h:66
btSolverConstraint::m_relpos1CrossNormal
btVector3 m_relpos1CrossNormal
Definition: btSolverConstraint.h:34
btTypedConstraint::btConstraintInfo2
Definition: btTypedConstraint.h:120
SOLVER_ENABLE_FRICTION_DIRECTION_CACHING
Definition: btContactSolverInfo.h:27
btSequentialImpulseConstraintSolver::m_tmpSolverNonContactConstraintPool
btConstraintArray m_tmpSolverNonContactConstraintPool
Definition: btSequentialImpulseConstraintSolver.h:58
btRigidBody::getInvInertiaTensorWorld
const btMatrix3x3 & getInvInertiaTensorWorld() const
Definition: btRigidBody.h:265
btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver
virtual ~btSequentialImpulseConstraintSolver()
Definition: btSequentialImpulseConstraintSolver.cpp:383
btJointFeedback::m_appliedTorqueBodyA
btVector3 m_appliedTorqueBodyA
Definition: btTypedConstraint.h:68
btPersistentManifold::getContactPoint
const btManifoldPoint & getContactPoint(int index) const
Definition: btPersistentManifold.h:130
btTypedConstraint::btConstraintInfo2::fps
btScalar fps
Definition: btTypedConstraint.h:124
SOLVER_USE_2_FRICTION_DIRECTIONS
Definition: btContactSolverInfo.h:26
btAlignedObjectArray.h
btManifoldPoint::m_lateralFrictionDir1
btVector3 m_lateralFrictionDir1
Definition: btManifoldPoint.h:139
gResolveSingleConstraintRowGeneric_scalar_reference
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 ...
Definition: btSequentialImpulseConstraintSolver.cpp:45
btAlignedObjectArray::resizeNoInitialize
void resizeNoInitialize(int newsize)
resize changes the number of elements in the array.
Definition: btAlignedObjectArray.h:194
btCollisionObject::hasAnisotropicFriction
bool hasAnisotropicFriction(int frictionMode=CF_ANISOTROPIC_FRICTION) const
Definition: btCollisionObject.h:179
btPersistentManifold::getBody1
const btCollisionObject * getBody1() const
Definition: btPersistentManifold.h:106
btSequentialImpulseConstraintSolver::addTorsionalFrictionConstraint
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)
Definition: btSequentialImpulseConstraintSolver.cpp:680
btSequentialImpulseConstraintSolver::m_leastSquaresResidual
btScalar m_leastSquaresResidual
Definition: btSequentialImpulseConstraintSolver.h:83
btManifoldPoint::m_appliedImpulse
btScalar m_appliedImpulse
Definition: btManifoldPoint.h:118
SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
Definition: btContactSolverInfo.h:31
btRigidBody::getTotalForce
const btVector3 & getTotalForce() const
Definition: btRigidBody.h:279
btSolverConstraint::m_appliedImpulse
btSimdScalar m_appliedImpulse
Definition: btSolverConstraint.h:44
btSequentialImpulseConstraintSolver::btRand2
unsigned long btRand2()
Definition: btSequentialImpulseConstraintSolver.cpp:418
gNumSplitImpulseRecoveries
int gNumSplitImpulseRecoveries
Definition: btSequentialImpulseConstraintSolver.cpp:38
btSequentialImpulseConstraintSolver::getOrInitSolverBody
int getOrInitSolverBody(btCollisionObject &body, btScalar timeStep)
Definition: btSequentialImpulseConstraintSolver.cpp:689
btSequentialImpulseConstraintSolver::initSolverBody
void initSolverBody(btSolverBody *solverBody, btCollisionObject *collisionObject, btScalar timeStep)
Definition: btSequentialImpulseConstraintSolver.cpp:457
btRigidBody::computeImpulseDenominator
btScalar computeImpulseDenominator(const btVector3 &pos, const btVector3 &normal) const
Definition: btRigidBody.h:435
btSqrt
btScalar btSqrt(btScalar y)
Definition: btScalar.h:466
btAlignedObjectArray::expandNonInitializing
T & expandNonInitializing()
Definition: btAlignedObjectArray.h:230
btCollisionObject::CF_ANISOTROPIC_FRICTION
Definition: btCollisionObject.h:159
btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse
void setFrictionConstraintImpulse(btSolverConstraint &solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint &cp, const btContactSolverInfo &infoGlobal)
Definition: btSequentialImpulseConstraintSolver.cpp:977
btSequentialImpulseConstraintSolver::setupTorsionalFrictionConstraint
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.)
Definition: btSequentialImpulseConstraintSolver.cpp:617
btContactSolverInfoData::m_numIterations
int m_numIterations
Definition: btContactSolverInfo.h:44
SOLVER_RANDMIZE_ORDER
Definition: btContactSolverInfo.h:23
btVector3::normalize
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:303
btStackAlloc.h
uniqueId
static int uniqueId
Definition: btRigidBody.cpp:27
gResolveSplitPenetrationImpulse_sse2
static btScalar gResolveSplitPenetrationImpulse_sse2(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &c)
Definition: btSequentialImpulseConstraintSolver.cpp:314
BT_PROFILE
#define BT_PROFILE(name)
Definition: btQuickprof.h:197
btSolverConstraint::m_frictionIndex
int m_frictionIndex
Definition: btSolverConstraint.h:61
btSolverBody::m_worldTransform
btTransform m_worldTransform
Definition: btSolverBody.h:107
btContactSolverInfoData::m_erp2
btScalar m_erp2
Definition: btContactSolverInfo.h:48
sum
static T sum(const btAlignedObjectArray< T > &items)
Definition: btSoftBodyHelpers.cpp:94
btManifoldPoint::m_appliedImpulseLateral2
btScalar m_appliedImpulseLateral2
Definition: btManifoldPoint.h:121
btRigidBody::upcast
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
btSequentialImpulseConstraintSolver::btRandInt2
int btRandInt2(int n)
Definition: btSequentialImpulseConstraintSolver.cpp:425
btManifoldPoint::m_combinedFriction
btScalar m_combinedFriction
Definition: btManifoldPoint.h:103
BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED
Definition: btManifoldPoint.h:42
btAlignedObjectArray::size
int size() const
return the number of elements in the array
Definition: btAlignedObjectArray.h:142
btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver
btSequentialImpulseConstraintSolver()
Definition: btSequentialImpulseConstraintSolver.cpp:351
btSolverBody::getDeltaAngularVelocity
const btVector3 & getDeltaAngularVelocity() const
Definition: btSolverBody.h:179
btManifoldPoint::m_combinedContactStiffness1
btScalar m_combinedContactStiffness1
Definition: btManifoldPoint.h:127
btTypedConstraint::getInfo2
virtual void getInfo2(btConstraintInfo2 *info)=0
internal method used by the constraint solver, don't use them directly
btVector3::length2
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:251
btSolverConstraint::m_angularComponentA
btVector3 m_angularComponentA
Definition: btSolverConstraint.h:40
btRigidBody::computeGyroscopicImpulseImplicit_Body
btVector3 computeGyroscopicImpulseImplicit_Body(btScalar step) const
perform implicit force computation in body space (inertial frame)
Definition: btRigidBody.cpp:293
btTypedConstraint::buildJacobian
virtual void buildJacobian()
internal method used by the constraint solver, don't use them directly
Definition: btTypedConstraint.h:163
btCollisionObject::getCompanionId
int getCompanionId() const
Definition: btCollisionObject.h:446
btSequentialImpulseConstraintSolver::setupSolverFunctions
void setupSolverFunctions(bool useSimd)
Definition: btSequentialImpulseConstraintSolver.cpp:358