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