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::setupTorsionalFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis1,int solverBodyIdA,int solverBodyIdB,
631  btManifoldPoint& cp,btScalar combinedTorsionalFriction, 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 = combinedTorsionalFriction;
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::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)
708 {
710  solverConstraint.m_frictionIndex = frictionIndex;
711  setupTorsionalFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, combinedTorsionalFriction,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 = infoGlobal.m_sor;
779  btScalar invTimeStep = btScalar(1)/infoGlobal.m_timeStep;
780 
781  //cfm = 1 / ( dt * kp + kd )
782  //erp = dt * kp / ( dt * kp + kd )
783 
784  btScalar cfm = infoGlobal.m_globalCfm;
785  btScalar erp = infoGlobal.m_erp2;
786 
788  {
790  cfm = cp.m_contactCFM;
792  erp = cp.m_contactERP;
793  } else
794  {
796  {
798  if (denom < SIMD_EPSILON)
799  {
800  denom = SIMD_EPSILON;
801  }
802  cfm = btScalar(1) / denom;
803  erp = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1) / denom;
804  }
805  }
806 
807  cfm *= invTimeStep;
808 
809 
810  btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
811  solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
812  btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);
813  solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
814 
815  {
816 #ifdef COMPUTE_IMPULSE_DENOM
817  btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
818  btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
819 #else
820  btVector3 vec;
821  btScalar denom0 = 0.f;
822  btScalar denom1 = 0.f;
823  if (rb0)
824  {
825  vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
826  denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
827  }
828  if (rb1)
829  {
830  vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
831  denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
832  }
833 #endif //COMPUTE_IMPULSE_DENOM
834 
835  btScalar denom = relaxation/(denom0+denom1+cfm);
836  solverConstraint.m_jacDiagABInv = denom;
837  }
838 
839  if (rb0)
840  {
841  solverConstraint.m_contactNormal1 = cp.m_normalWorldOnB;
842  solverConstraint.m_relpos1CrossNormal = torqueAxis0;
843  } else
844  {
845  solverConstraint.m_contactNormal1.setZero();
846  solverConstraint.m_relpos1CrossNormal.setZero();
847  }
848  if (rb1)
849  {
850  solverConstraint.m_contactNormal2 = -cp.m_normalWorldOnB;
851  solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
852  }else
853  {
854  solverConstraint.m_contactNormal2.setZero();
855  solverConstraint.m_relpos2CrossNormal.setZero();
856  }
857 
858  btScalar restitution = 0.f;
859  btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop;
860 
861  {
862  btVector3 vel1,vel2;
863 
864  vel1 = rb0? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
865  vel2 = rb1? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
866 
867  // btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
868  btVector3 vel = vel1 - vel2;
869  btScalar rel_vel = cp.m_normalWorldOnB.dot(vel);
870 
871 
872 
873  solverConstraint.m_friction = cp.m_combinedFriction;
874 
875 
876  restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution);
877  if (restitution <= btScalar(0.))
878  {
879  restitution = 0.f;
880  };
881  }
882 
883 
885  if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
886  {
887  solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
888  if (rb0)
889  bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
890  if (rb1)
891  bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
892  } else
893  {
894  solverConstraint.m_appliedImpulse = 0.f;
895  }
896 
897  solverConstraint.m_appliedPushImpulse = 0.f;
898 
899  {
900 
901  btVector3 externalForceImpulseA = bodyA->m_originalBody ? bodyA->m_externalForceImpulse: btVector3(0,0,0);
902  btVector3 externalTorqueImpulseA = bodyA->m_originalBody ? bodyA->m_externalTorqueImpulse: btVector3(0,0,0);
903  btVector3 externalForceImpulseB = bodyB->m_originalBody ? bodyB->m_externalForceImpulse: btVector3(0,0,0);
904  btVector3 externalTorqueImpulseB = bodyB->m_originalBody ?bodyB->m_externalTorqueImpulse : btVector3(0,0,0);
905 
906 
907  btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(bodyA->m_linearVelocity+externalForceImpulseA)
908  + solverConstraint.m_relpos1CrossNormal.dot(bodyA->m_angularVelocity+externalTorqueImpulseA);
909  btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(bodyB->m_linearVelocity+externalForceImpulseB)
910  + solverConstraint.m_relpos2CrossNormal.dot(bodyB->m_angularVelocity+externalTorqueImpulseB);
911  btScalar rel_vel = vel1Dotn+vel2Dotn;
912 
913  btScalar positionalError = 0.f;
914  btScalar velocityError = restitution - rel_vel;// * damping;
915 
916 
917 
918  if (penetration>0)
919  {
920  positionalError = 0;
921 
922  velocityError -= penetration *invTimeStep;
923  } else
924  {
925  positionalError = -penetration * erp*invTimeStep;
926 
927  }
928 
929  btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
930  btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
931 
932  if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
933  {
934  //combine position and velocity into rhs
935  solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;//-solverConstraint.m_contactNormal1.dot(bodyA->m_externalForce*bodyA->m_invMass-bodyB->m_externalForce/bodyB->m_invMass)*solverConstraint.m_jacDiagABInv;
936  solverConstraint.m_rhsPenetration = 0.f;
937 
938  } else
939  {
940  //split position and velocity into rhs and m_rhsPenetration
941  solverConstraint.m_rhs = velocityImpulse;
942  solverConstraint.m_rhsPenetration = penetrationImpulse;
943  }
944  solverConstraint.m_cfm = cfm*solverConstraint.m_jacDiagABInv;
945  solverConstraint.m_lowerLimit = 0;
946  solverConstraint.m_upperLimit = 1e10f;
947  }
948 
949 
950 
951 
952 }
953 
954 
955 
957  int solverBodyIdA, int solverBodyIdB,
958  btManifoldPoint& cp, const btContactSolverInfo& infoGlobal)
959 {
960 
961  btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA];
962  btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB];
963 
964  btRigidBody* rb0 = bodyA->m_originalBody;
965  btRigidBody* rb1 = bodyB->m_originalBody;
966 
967  {
968  btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
969  if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
970  {
971  frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
972  if (rb0)
973  bodyA->internalApplyImpulse(frictionConstraint1.m_contactNormal1*rb0->getInvMass()*rb0->getLinearFactor(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
974  if (rb1)
975  bodyB->internalApplyImpulse(-frictionConstraint1.m_contactNormal2*rb1->getInvMass()*rb1->getLinearFactor(),-frictionConstraint1.m_angularComponentB,-(btScalar)frictionConstraint1.m_appliedImpulse);
976  } else
977  {
978  frictionConstraint1.m_appliedImpulse = 0.f;
979  }
980  }
981 
983  {
984  btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
985  if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
986  {
987  frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
988  if (rb0)
989  bodyA->internalApplyImpulse(frictionConstraint2.m_contactNormal1*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
990  if (rb1)
991  bodyB->internalApplyImpulse(-frictionConstraint2.m_contactNormal2*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-(btScalar)frictionConstraint2.m_appliedImpulse);
992  } else
993  {
994  frictionConstraint2.m_appliedImpulse = 0.f;
995  }
996  }
997 }
998 
999 
1000 
1001 
1003 {
1004  btCollisionObject* colObj0=0,*colObj1=0;
1005 
1006  colObj0 = (btCollisionObject*)manifold->getBody0();
1007  colObj1 = (btCollisionObject*)manifold->getBody1();
1008 
1009  int solverBodyIdA = getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep);
1010  int solverBodyIdB = getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep);
1011 
1012 // btRigidBody* bodyA = btRigidBody::upcast(colObj0);
1013 // btRigidBody* bodyB = btRigidBody::upcast(colObj1);
1014 
1015  btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverBodyIdA];
1016  btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverBodyIdB];
1017 
1018 
1019 
1021  if (!solverBodyA || (solverBodyA->m_invMass.fuzzyZero() && (!solverBodyB || solverBodyB->m_invMass.fuzzyZero())))
1022  return;
1023 
1024  int rollingFriction=1;
1025  for (int j=0;j<manifold->getNumContacts();j++)
1026  {
1027 
1028  btManifoldPoint& cp = manifold->getContactPoint(j);
1029 
1030  if (cp.getDistance() <= manifold->getContactProcessingThreshold())
1031  {
1032  btVector3 rel_pos1;
1033  btVector3 rel_pos2;
1034  btScalar relaxation;
1035 
1036 
1037  int frictionIndex = m_tmpSolverContactConstraintPool.size();
1039  btRigidBody* rb0 = btRigidBody::upcast(colObj0);
1040  btRigidBody* rb1 = btRigidBody::upcast(colObj1);
1041  solverConstraint.m_solverBodyIdA = solverBodyIdA;
1042  solverConstraint.m_solverBodyIdB = solverBodyIdB;
1043 
1044  solverConstraint.m_originalContactPoint = &cp;
1045 
1046  const btVector3& pos1 = cp.getPositionWorldOnA();
1047  const btVector3& pos2 = cp.getPositionWorldOnB();
1048 
1049  rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
1050  rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
1051 
1052  btVector3 vel1;// = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
1053  btVector3 vel2;// = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
1054 
1055  solverBodyA->getVelocityInLocalPointNoDelta(rel_pos1,vel1);
1056  solverBodyB->getVelocityInLocalPointNoDelta(rel_pos2,vel2 );
1057 
1058  btVector3 vel = vel1 - vel2;
1059  btScalar rel_vel = cp.m_normalWorldOnB.dot(vel);
1060 
1061  setupContactConstraint(solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, relaxation, rel_pos1, rel_pos2);
1062 
1063 
1064 
1065 // const btVector3& pos1 = cp.getPositionWorldOnA();
1066 // const btVector3& pos2 = cp.getPositionWorldOnB();
1067 
1069 
1071 
1072  if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0))
1073  {
1074 
1075  {
1076  addTorsionalFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,cp.m_combinedSpinningFriction, rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
1077  btVector3 axis0,axis1;
1078  btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1);
1079  axis0.normalize();
1080  axis1.normalize();
1081 
1086  if (axis0.length()>0.001)
1087  addTorsionalFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,
1088  cp.m_combinedRollingFriction, rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
1089  if (axis1.length()>0.001)
1090  addTorsionalFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,
1091  cp.m_combinedRollingFriction, rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
1092 
1093  }
1094  }
1095 
1100 
1112  {
1113  cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
1114  btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
1116  {
1117  cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel);
1120  addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
1121 
1123  {
1128  addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
1129  }
1130 
1131  } else
1132  {
1134 
1137  addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
1138 
1140  {
1143  addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
1144  }
1145 
1146 
1148  {
1150  }
1151  }
1152 
1153  } else
1154  {
1155  addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_frictionCFM);
1156 
1158  addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_frictionCFM);
1159 
1160  }
1161  setFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
1162 
1163 
1164 
1165 
1166  }
1167  }
1168 }
1169 
1171 {
1172  int i;
1173  btPersistentManifold* manifold = 0;
1174 // btCollisionObject* colObj0=0,*colObj1=0;
1175 
1176 
1177  for (i=0;i<numManifolds;i++)
1178  {
1179  manifold = manifoldPtr[i];
1180  convertContact(manifold,infoGlobal);
1181  }
1182 }
1183 
1184 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
1185 {
1186  m_fixedBodyId = -1;
1187  BT_PROFILE("solveGroupCacheFriendlySetup");
1188  (void)debugDrawer;
1189 
1191 
1192 #ifdef BT_ADDITIONAL_DEBUG
1193  //make sure that dynamic bodies exist for all (enabled) constraints
1194  for (int i=0;i<numConstraints;i++)
1195  {
1196  btTypedConstraint* constraint = constraints[i];
1197  if (constraint->isEnabled())
1198  {
1199  if (!constraint->getRigidBodyA().isStaticOrKinematicObject())
1200  {
1201  bool found=false;
1202  for (int b=0;b<numBodies;b++)
1203  {
1204 
1205  if (&constraint->getRigidBodyA()==bodies[b])
1206  {
1207  found = true;
1208  break;
1209  }
1210  }
1211  btAssert(found);
1212  }
1213  if (!constraint->getRigidBodyB().isStaticOrKinematicObject())
1214  {
1215  bool found=false;
1216  for (int b=0;b<numBodies;b++)
1217  {
1218  if (&constraint->getRigidBodyB()==bodies[b])
1219  {
1220  found = true;
1221  break;
1222  }
1223  }
1224  btAssert(found);
1225  }
1226  }
1227  }
1228  //make sure that dynamic bodies exist for all contact manifolds
1229  for (int i=0;i<numManifolds;i++)
1230  {
1231  if (!manifoldPtr[i]->getBody0()->isStaticOrKinematicObject())
1232  {
1233  bool found=false;
1234  for (int b=0;b<numBodies;b++)
1235  {
1236 
1237  if (manifoldPtr[i]->getBody0()==bodies[b])
1238  {
1239  found = true;
1240  break;
1241  }
1242  }
1243  btAssert(found);
1244  }
1245  if (!manifoldPtr[i]->getBody1()->isStaticOrKinematicObject())
1246  {
1247  bool found=false;
1248  for (int b=0;b<numBodies;b++)
1249  {
1250  if (manifoldPtr[i]->getBody1()==bodies[b])
1251  {
1252  found = true;
1253  break;
1254  }
1255  }
1256  btAssert(found);
1257  }
1258  }
1259 #endif //BT_ADDITIONAL_DEBUG
1260 
1261 
1262  for (int i = 0; i < numBodies; i++)
1263  {
1264  bodies[i]->setCompanionId(-1);
1265  }
1266 
1267 
1268  m_tmpSolverBodyPool.reserve(numBodies+1);
1270 
1271  //btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
1272  //initSolverBody(&fixedBody,0);
1273 
1274  //convert all bodies
1275 
1276 
1277  for (int i=0;i<numBodies;i++)
1278  {
1279  int bodyId = getOrInitSolverBody(*bodies[i],infoGlobal.m_timeStep);
1280 
1281  btRigidBody* body = btRigidBody::upcast(bodies[i]);
1282  if (body && body->getInvMass())
1283  {
1284  btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
1285  btVector3 gyroForce (0,0,0);
1287  {
1288  gyroForce = body->computeGyroscopicForceExplicit(infoGlobal.m_maxGyroscopicForce);
1289  solverBody.m_externalTorqueImpulse -= gyroForce*body->getInvInertiaTensorWorld()*infoGlobal.m_timeStep;
1290  }
1292  {
1293  gyroForce = body->computeGyroscopicImpulseImplicit_World(infoGlobal.m_timeStep);
1294  solverBody.m_externalTorqueImpulse += gyroForce;
1295  }
1297  {
1298  gyroForce = body->computeGyroscopicImpulseImplicit_Body(infoGlobal.m_timeStep);
1299  solverBody.m_externalTorqueImpulse += gyroForce;
1300 
1301  }
1302 
1303 
1304  }
1305  }
1306 
1307  if (1)
1308  {
1309  int j;
1310  for (j=0;j<numConstraints;j++)
1311  {
1312  btTypedConstraint* constraint = constraints[j];
1313  constraint->buildJacobian();
1314  constraint->internalSetAppliedImpulse(0.0f);
1315  }
1316  }
1317 
1318  //btRigidBody* rb0=0,*rb1=0;
1319 
1320  //if (1)
1321  {
1322  {
1323 
1324  int totalNumRows = 0;
1325  int i;
1326 
1328  //calculate the total number of contraint rows
1329  for (i=0;i<numConstraints;i++)
1330  {
1332  btJointFeedback* fb = constraints[i]->getJointFeedback();
1333  if (fb)
1334  {
1339  }
1340 
1341  if (constraints[i]->isEnabled())
1342  {
1343  }
1344  if (constraints[i]->isEnabled())
1345  {
1346  constraints[i]->getInfo1(&info1);
1347  } else
1348  {
1349  info1.m_numConstraintRows = 0;
1350  info1.nub = 0;
1351  }
1352  totalNumRows += info1.m_numConstraintRows;
1353  }
1355 
1356 
1358  int currentRow = 0;
1359 
1360  for (i=0;i<numConstraints;i++)
1361  {
1363 
1364  if (info1.m_numConstraintRows)
1365  {
1366  btAssert(currentRow<totalNumRows);
1367 
1368  btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];
1369  btTypedConstraint* constraint = constraints[i];
1370  btRigidBody& rbA = constraint->getRigidBodyA();
1371  btRigidBody& rbB = constraint->getRigidBodyB();
1372 
1373  int solverBodyIdA = getOrInitSolverBody(rbA,infoGlobal.m_timeStep);
1374  int solverBodyIdB = getOrInitSolverBody(rbB,infoGlobal.m_timeStep);
1375 
1376  btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
1377  btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
1378 
1379 
1380 
1381 
1382  int overrideNumSolverIterations = constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations;
1383  if (overrideNumSolverIterations>m_maxOverrideNumSolverIterations)
1384  m_maxOverrideNumSolverIterations = overrideNumSolverIterations;
1385 
1386 
1387  int j;
1388  for ( j=0;j<info1.m_numConstraintRows;j++)
1389  {
1390  memset(&currentConstraintRow[j],0,sizeof(btSolverConstraint));
1391  currentConstraintRow[j].m_lowerLimit = -SIMD_INFINITY;
1392  currentConstraintRow[j].m_upperLimit = SIMD_INFINITY;
1393  currentConstraintRow[j].m_appliedImpulse = 0.f;
1394  currentConstraintRow[j].m_appliedPushImpulse = 0.f;
1395  currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA;
1396  currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB;
1397  currentConstraintRow[j].m_overrideNumSolverIterations = overrideNumSolverIterations;
1398  }
1399 
1400  bodyAPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
1401  bodyAPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
1402  bodyAPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f);
1403  bodyAPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
1404  bodyBPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
1405  bodyBPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
1406  bodyBPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f);
1407  bodyBPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
1408 
1409 
1411  info2.fps = 1.f/infoGlobal.m_timeStep;
1412  info2.erp = infoGlobal.m_erp;
1413  info2.m_J1linearAxis = currentConstraintRow->m_contactNormal1;
1414  info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal;
1415  info2.m_J2linearAxis = currentConstraintRow->m_contactNormal2;
1416  info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal;
1417  info2.rowskip = sizeof(btSolverConstraint)/sizeof(btScalar);//check this
1419  btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint));
1420  info2.m_constraintError = &currentConstraintRow->m_rhs;
1421  currentConstraintRow->m_cfm = infoGlobal.m_globalCfm;
1422  info2.m_damping = infoGlobal.m_damping;
1423  info2.cfm = &currentConstraintRow->m_cfm;
1424  info2.m_lowerLimit = &currentConstraintRow->m_lowerLimit;
1425  info2.m_upperLimit = &currentConstraintRow->m_upperLimit;
1426  info2.m_numIterations = infoGlobal.m_numIterations;
1427  constraints[i]->getInfo2(&info2);
1428 
1430  for ( j=0;j<info1.m_numConstraintRows;j++)
1431  {
1432  btSolverConstraint& solverConstraint = currentConstraintRow[j];
1433 
1434  if (solverConstraint.m_upperLimit>=constraints[i]->getBreakingImpulseThreshold())
1435  {
1436  solverConstraint.m_upperLimit = constraints[i]->getBreakingImpulseThreshold();
1437  }
1438 
1439  if (solverConstraint.m_lowerLimit<=-constraints[i]->getBreakingImpulseThreshold())
1440  {
1441  solverConstraint.m_lowerLimit = -constraints[i]->getBreakingImpulseThreshold();
1442  }
1443 
1444  solverConstraint.m_originalContactPoint = constraint;
1445 
1446  {
1447  const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
1448  solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld()*ftorqueAxis1*constraint->getRigidBodyA().getAngularFactor();
1449  }
1450  {
1451  const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal;
1452  solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld()*ftorqueAxis2*constraint->getRigidBodyB().getAngularFactor();
1453  }
1454 
1455  {
1456  btVector3 iMJlA = solverConstraint.m_contactNormal1*rbA.getInvMass();
1457  btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal;
1458  btVector3 iMJlB = solverConstraint.m_contactNormal2*rbB.getInvMass();//sign of normal?
1459  btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal;
1460 
1461  btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal1);
1462  sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
1463  sum += iMJlB.dot(solverConstraint.m_contactNormal2);
1464  sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
1465  btScalar fsum = btFabs(sum);
1466  btAssert(fsum > SIMD_EPSILON);
1467  solverConstraint.m_jacDiagABInv = fsum>SIMD_EPSILON?btScalar(1.)/sum : 0.f;
1468  }
1469 
1470 
1471 
1472  {
1473  btScalar rel_vel;
1474  btVector3 externalForceImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalForceImpulse : btVector3(0,0,0);
1475  btVector3 externalTorqueImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalTorqueImpulse : btVector3(0,0,0);
1476 
1477  btVector3 externalForceImpulseB = bodyBPtr->m_originalBody ? bodyBPtr->m_externalForceImpulse : btVector3(0,0,0);
1478  btVector3 externalTorqueImpulseB = bodyBPtr->m_originalBody ?bodyBPtr->m_externalTorqueImpulse : btVector3(0,0,0);
1479 
1480  btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(rbA.getLinearVelocity()+externalForceImpulseA)
1481  + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity()+externalTorqueImpulseA);
1482 
1483  btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(rbB.getLinearVelocity()+externalForceImpulseB)
1484  + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity()+externalTorqueImpulseB);
1485 
1486  rel_vel = vel1Dotn+vel2Dotn;
1487  btScalar restitution = 0.f;
1488  btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2
1489  btScalar velocityError = restitution - rel_vel * info2.m_damping;
1490  btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
1491  btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
1492  solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
1493  solverConstraint.m_appliedImpulse = 0.f;
1494 
1495 
1496  }
1497  }
1498  }
1499  currentRow+=m_tmpConstraintSizesPool[i].m_numConstraintRows;
1500  }
1501  }
1502 
1503  convertContacts(manifoldPtr,numManifolds,infoGlobal);
1504 
1505  }
1506 
1507 // btContactSolverInfo info = infoGlobal;
1508 
1509 
1510  int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
1511  int numConstraintPool = m_tmpSolverContactConstraintPool.size();
1512  int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
1513 
1517  m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool*2);
1518  else
1519  m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool);
1520 
1522  {
1523  int i;
1524  for (i=0;i<numNonContactPool;i++)
1525  {
1527  }
1528  for (i=0;i<numConstraintPool;i++)
1529  {
1530  m_orderTmpConstraintPool[i] = i;
1531  }
1532  for (i=0;i<numFrictionPool;i++)
1533  {
1535  }
1536  }
1537 
1538  return 0.f;
1539 
1540 }
1541 
1542 
1543 btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/)
1544 {
1545 
1546  int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
1547  int numConstraintPool = m_tmpSolverContactConstraintPool.size();
1548  int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
1549 
1550  if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
1551  {
1552  if (1) // uncomment this for a bit less random ((iteration & 7) == 0)
1553  {
1554 
1555  for (int j=0; j<numNonContactPool; ++j) {
1556  int tmp = m_orderNonContactConstraintPool[j];
1557  int swapi = btRandInt2(j+1);
1559  m_orderNonContactConstraintPool[swapi] = tmp;
1560  }
1561 
1562  //contact/friction constraints are not solved more than
1563  if (iteration< infoGlobal.m_numIterations)
1564  {
1565  for (int j=0; j<numConstraintPool; ++j) {
1566  int tmp = m_orderTmpConstraintPool[j];
1567  int swapi = btRandInt2(j+1);
1569  m_orderTmpConstraintPool[swapi] = tmp;
1570  }
1571 
1572  for (int j=0; j<numFrictionPool; ++j) {
1573  int tmp = m_orderFrictionConstraintPool[j];
1574  int swapi = btRandInt2(j+1);
1576  m_orderFrictionConstraintPool[swapi] = tmp;
1577  }
1578  }
1579  }
1580  }
1581 
1582  if (infoGlobal.m_solverMode & SOLVER_SIMD)
1583  {
1585  for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
1586  {
1588  if (iteration < constraint.m_overrideNumSolverIterations)
1590  }
1591 
1592  if (iteration< infoGlobal.m_numIterations)
1593  {
1594  for (int j=0;j<numConstraints;j++)
1595  {
1596  if (constraints[j]->isEnabled())
1597  {
1598  int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep);
1599  int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
1600  btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
1601  btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
1602  constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
1603  }
1604  }
1605 
1608  {
1609  int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1610  int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)? 2 : 1;
1611 
1612  for (int c=0;c<numPoolConstraints;c++)
1613  {
1614  btScalar totalImpulse =0;
1615 
1616  {
1619  totalImpulse = solveManifold.m_appliedImpulse;
1620  }
1621  bool applyFriction = true;
1622  if (applyFriction)
1623  {
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 
1638  {
1639 
1641 
1642  if (totalImpulse>btScalar(0))
1643  {
1644  solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
1645  solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
1646 
1648  }
1649  }
1650  }
1651  }
1652 
1653  }
1654  else//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
1655  {
1656  //solve the friction constraints after all contact constraints, don't interleave them
1657  int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1658  int j;
1659 
1660  for (j=0;j<numPoolConstraints;j++)
1661  {
1664 
1665  }
1666 
1667 
1668 
1670 
1671  int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
1672  for (j=0;j<numFrictionPoolConstraints;j++)
1673  {
1675  btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
1676 
1677  if (totalImpulse>btScalar(0))
1678  {
1679  solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
1680  solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
1681 
1683  }
1684  }
1685 
1686 
1687  int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
1688  for (j=0;j<numRollingFrictionPoolConstraints;j++)
1689  {
1690 
1692  btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
1693  if (totalImpulse>btScalar(0))
1694  {
1695  btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
1696  if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
1697  rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
1698 
1699  rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
1700  rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
1701 
1702  resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
1703  }
1704  }
1705 
1706 
1707  }
1708  }
1709  } else
1710  {
1711  //non-SIMD version
1713  for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
1714  {
1716  if (iteration < constraint.m_overrideNumSolverIterations)
1718  }
1719 
1720  if (iteration< infoGlobal.m_numIterations)
1721  {
1722  for (int j=0;j<numConstraints;j++)
1723  {
1724  if (constraints[j]->isEnabled())
1725  {
1726  int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep);
1727  int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
1728  btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
1729  btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
1730  constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
1731  }
1732  }
1734  int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1735  for (int j=0;j<numPoolConstraints;j++)
1736  {
1739  }
1741  int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
1742  for (int j=0;j<numFrictionPoolConstraints;j++)
1743  {
1745  btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
1746 
1747  if (totalImpulse>btScalar(0))
1748  {
1749  solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
1750  solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
1751 
1753  }
1754  }
1755 
1756  int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
1757  for (int j=0;j<numRollingFrictionPoolConstraints;j++)
1758  {
1760  btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
1761  if (totalImpulse>btScalar(0))
1762  {
1763  btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
1764  if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
1765  rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
1766 
1767  rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
1768  rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
1769 
1770  resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
1771  }
1772  }
1773  }
1774  }
1775  return 0.f;
1776 }
1777 
1778 
1779 void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
1780 {
1781  int iteration;
1782  if (infoGlobal.m_splitImpulse)
1783  {
1784  if (infoGlobal.m_solverMode & SOLVER_SIMD)
1785  {
1786  for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
1787  {
1788  {
1789  int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1790  int j;
1791  for (j=0;j<numPoolConstraints;j++)
1792  {
1794 
1796  }
1797  }
1798  }
1799  }
1800  else
1801  {
1802  for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
1803  {
1804  {
1805  int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1806  int j;
1807  for (j=0;j<numPoolConstraints;j++)
1808  {
1810 
1812  }
1813  }
1814  }
1815  }
1816  }
1817 }
1818 
1819 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
1820 {
1821  BT_PROFILE("solveGroupCacheFriendlyIterations");
1822 
1823  {
1825  solveGroupCacheFriendlySplitImpulseIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
1826 
1828 
1829  for ( int iteration = 0 ; iteration< maxIterations ; iteration++)
1830  //for ( int iteration = maxIterations-1 ; iteration >= 0;iteration--)
1831  {
1832  solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
1833  }
1834 
1835  }
1836  return 0.f;
1837 }
1838 
1840 {
1841  int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1842  int i,j;
1843 
1844  if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
1845  {
1846  for (j=0;j<numPoolConstraints;j++)
1847  {
1848  const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j];
1850  btAssert(pt);
1851  pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
1852  // float f = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
1853  // printf("pt->m_appliedImpulseLateral1 = %f\n", f);
1855  //printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
1857  {
1859  }
1860  //do a callback here?
1861  }
1862  }
1863 
1864  numPoolConstraints = m_tmpSolverNonContactConstraintPool.size();
1865  for (j=0;j<numPoolConstraints;j++)
1866  {
1869  btJointFeedback* fb = constr->getJointFeedback();
1870  if (fb)
1871  {
1872  fb->m_appliedForceBodyA += solverConstr.m_contactNormal1*solverConstr.m_appliedImpulse*constr->getRigidBodyA().getLinearFactor()/infoGlobal.m_timeStep;
1873  fb->m_appliedForceBodyB += solverConstr.m_contactNormal2*solverConstr.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/infoGlobal.m_timeStep;
1874  fb->m_appliedTorqueBodyA += solverConstr.m_relpos1CrossNormal* constr->getRigidBodyA().getAngularFactor()*solverConstr.m_appliedImpulse/infoGlobal.m_timeStep;
1875  fb->m_appliedTorqueBodyB += solverConstr.m_relpos2CrossNormal* constr->getRigidBodyB().getAngularFactor()*solverConstr.m_appliedImpulse/infoGlobal.m_timeStep; /*RGM ???? */
1876 
1877  }
1878 
1879  constr->internalSetAppliedImpulse(solverConstr.m_appliedImpulse);
1880  if (btFabs(solverConstr.m_appliedImpulse)>=constr->getBreakingImpulseThreshold())
1881  {
1882  constr->setEnabled(false);
1883  }
1884  }
1885 
1886 
1887 
1888  for ( i=0;i<m_tmpSolverBodyPool.size();i++)
1889  {
1890  btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody;
1891  if (body)
1892  {
1893  if (infoGlobal.m_splitImpulse)
1894  m_tmpSolverBodyPool[i].writebackVelocityAndTransform(infoGlobal.m_timeStep, infoGlobal.m_splitImpulseTurnErp);
1895  else
1896  m_tmpSolverBodyPool[i].writebackVelocity();
1897 
1898  m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(
1899  m_tmpSolverBodyPool[i].m_linearVelocity+
1900  m_tmpSolverBodyPool[i].m_externalForceImpulse);
1901 
1902  m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(
1903  m_tmpSolverBodyPool[i].m_angularVelocity+
1904  m_tmpSolverBodyPool[i].m_externalTorqueImpulse);
1905 
1906  if (infoGlobal.m_splitImpulse)
1907  m_tmpSolverBodyPool[i].m_originalBody->setWorldTransform(m_tmpSolverBodyPool[i].m_worldTransform);
1908 
1909  m_tmpSolverBodyPool[i].m_originalBody->setCompanionId(-1);
1910  }
1911  }
1912 
1917 
1919  return 0.f;
1920 }
1921 
1922 
1923 
1925 btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btDispatcher* /*dispatcher*/)
1926 {
1927 
1928  BT_PROFILE("solveGroup");
1929  //you need to provide at least some bodies
1930 
1931  solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer);
1932 
1933  solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer);
1934 
1935  solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal);
1936 
1937  return 0.f;
1938 }
1939 
1941 {
1942  m_btSeed2 = 0;
1943 }
btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverGeneric()
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:203
btVector3 m_angularVelocity
Definition: btSolverBody.h:120
#define SIMD_EPSILON
Definition: btScalar.h:495
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 computeImpulseDenominator(const btVector3 &pos, const btVector3 &normal) const
Definition: btRigidBody.h:403
const btVector3 & getAngularVelocity() const
Definition: btRigidBody.h:365
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:650
const btVector3 & getTotalTorque() const
Definition: btRigidBody.h:292
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:255
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
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:257
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
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:1282
void resizeNoInitialize(int newsize)
resize changes the number of elements in the array.
btScalar btSqrt(btScalar y)
Definition: btScalar.h:419
btScalar m_appliedImpulse
void setIdentity()
Set this transformation to the identity.
Definition: btTransform.h:172
#define btAssert(x)
Definition: btScalar.h:114
btVector3 m_relpos2CrossNormal
bool isKinematicObject() const
btSingleConstraintRowSolver getSSE2ConstraintRowSolverLowerLimit()
btSingleConstraintRowSolver m_resolveSingleConstraintRowGeneric
static btSimdScalar gResolveSingleConstraintRowLowerLimit_scalar_reference(btSolverBody &body1, btSolverBody &body2, const btSolverConstraint &c)
const btVector3 & getLinearFactor() const
Definition: btRigidBody.h:264
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:223
btScalar m_frictionCFM
btSimdScalar resolveSingleConstraintRowLowerLimit(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
const btJointFeedback * getJointFeedback() const
ManifoldContactPoint collects and maintains persistent contactpoints.
btScalar m_contactMotion1
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:504
btScalar restitutionCurve(btScalar rel_vel, btScalar restitution)
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:307
bool fuzzyZero() const
Definition: btVector3.h:699
btVector3 m_appliedForceBodyB
int getOrInitSolverBody(btCollisionObject &body, btScalar timeStep)
const btManifoldPoint & getContactPoint(int index) const
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
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.)
#define SIMD_INFINITY
Definition: btScalar.h:496
btTransform & getWorldTransform()
btVector3 m_normalWorldOnB
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:117
btSimdScalar resolveSingleConstraintRowGeneric(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
btVector3 m_appliedForceBodyA
const btVector3 & getLinearVelocity() const
Definition: btRigidBody.h:362
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:114
btScalar m_appliedImpulseLateral2
btScalar getInvMass() const
Definition: btRigidBody.h:273
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:387
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
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:112
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations...
Definition: btIDebugDraw.h:29
void setZero()
Definition: btVector3.h:681
btVector3 m_invMass
Definition: btSolverBody.h:116
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:62
int getFlags() const
Definition: btRigidBody.h:533
btScalar getContactProcessingThreshold() const
void setCompanionId(int id)
btSimdScalar resolveSingleConstraintRowGenericSIMD(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
void getVelocityInLocalPointNoDelta(const btVector3 &rel_pos, btVector3 &velocity) const
Definition: btSolverBody.h:137
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 size() const
return the number of elements in the array
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:382
#define BT_PROFILE(name)
Definition: btQuickprof.h:203
btScalar m_combinedContactDamping1
btVector3 m_appliedTorqueBodyB
btSimdScalar m_appliedPushImpulse
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
Definition: btSolverBody.h:108
btAlignedObjectArray< btTypedConstraint::btConstraintInfo1 > m_tmpConstraintSizesPool
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 btCollisionObject * getBody1() 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)
btScalar m_contactMotion2
btScalar m_combinedFriction
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:233
bool hasAnisotropicFriction(int frictionMode=CF_ANISOTROPIC_FRICTION) const
const btVector3 & getTotalForce() const
Definition: btRigidBody.h:287
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
const btMatrix3x3 & getInvInertiaTensorWorld() const
Definition: btRigidBody.h:274
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:69
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
int maxIterations
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:279
btScalar m_combinedSpinningFriction
void resolveSplitPenetrationImpulseCacheFriendly(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:263
btScalar btFabs(btScalar x)
Definition: btScalar.h:450
btSimdScalar(* btSingleConstraintRowSolver)(btSolverBody &, btSolverBody &, const btSolverConstraint &)
btTransform m_worldTransform
Definition: btSolverBody.h:111