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