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