Bullet Collision Detection & Physics Library
btSequentialImpulseConstraintSolver.cpp
Go to the documentation of this file.
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
4 
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose,
8 including commercial applications, and to alter it and redistribute it freely,
9 subject to the following restrictions:
10 
11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 */
15 
16 //#define COMPUTE_IMPULSE_DENOM 1
17 //#define BT_ADDITIONAL_DEBUG
18 
19 //It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms.
20 
23 
26 
27 //#include "btJacobianEntry.h"
28 #include "LinearMath/btMinMax.h"
30 #include <new>
32 #include "LinearMath/btQuickprof.h"
33 //#include "btSolverBody.h"
34 //#include "btSolverConstraint.h"
36 #include <string.h> //for memset
37 
39 
41 
42 //#define VERBOSE_RESIDUAL_PRINTF 1
46 {
47  btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm;
50 
51  // const btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn;
52  deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
53  deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv;
54 
55  const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
56  if (sum < c.m_lowerLimit)
57  {
58  deltaImpulse = c.m_lowerLimit - c.m_appliedImpulse;
60  }
61  else if (sum > c.m_upperLimit)
62  {
63  deltaImpulse = c.m_upperLimit - c.m_appliedImpulse;
65  }
66  else
67  {
69  }
70 
73 
74  return deltaImpulse;
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  return m_resolveSingleConstraintRowGeneric(body1, body2, c);
273 }
274 
275 // Project Gauss Seidel or the equivalent Sequential Impulse
277 {
278  return m_resolveSingleConstraintRowGeneric(body1, body2, c);
279 }
280 
282 {
283  return m_resolveSingleConstraintRowLowerLimit(body1, body2, c);
284 }
285 
286 
288 {
289  return m_resolveSingleConstraintRowLowerLimit(body1, body2, c);
290 }
291 
292 
294  btSolverBody& body1,
295  btSolverBody& body2,
296  const btSolverConstraint& c)
297 {
298  btScalar deltaImpulse = 0.f;
299 
300  if (c.m_rhsPenetration)
301  {
303  deltaImpulse = c.m_rhsPenetration-btScalar(c.m_appliedPushImpulse)*c.m_cfm;
306 
307  deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
308  deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv;
309  const btScalar sum = btScalar(c.m_appliedPushImpulse) + deltaImpulse;
310  if (sum < c.m_lowerLimit)
311  {
312  deltaImpulse = c.m_lowerLimit-c.m_appliedPushImpulse;
314  }
315  else
316  {
318  }
321  }
322  return deltaImpulse;
323 }
324 
326 {
327 #ifdef USE_SIMD
328  if (!c.m_rhsPenetration)
329  return 0.f;
330 
332 
333  __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedPushImpulse);
334  __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
335  __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
336  __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)));
337  __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128,body1.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetTurnVelocity().mVec128));
338  __m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128,body2.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetTurnVelocity().mVec128));
339  deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
340  deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
341  btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
342  btSimdScalar resultLowerLess,resultUpperLess;
343  resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
344  resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
345  __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
346  deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
347  c.m_appliedPushImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
348  __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128,body1.internalGetInvMass().mVec128);
349  __m128 linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128,body2.internalGetInvMass().mVec128);
350  __m128 impulseMagnitude = deltaImpulse;
351  body1.internalGetPushVelocity().mVec128 = _mm_add_ps(body1.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
352  body1.internalGetTurnVelocity().mVec128 = _mm_add_ps(body1.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
353  body2.internalGetPushVelocity().mVec128 = _mm_add_ps(body2.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
354  body2.internalGetTurnVelocity().mVec128 = _mm_add_ps(body2.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
355  return deltaImpulse;
356 #else
358 #endif
359 }
360 
361 
363 {
364  m_btSeed2 = 0;
365  m_cachedSolverMode = 0;
366  setupSolverFunctions( false );
367 }
368 
370 {
374 
375  if ( useSimd )
376  {
377 #ifdef USE_SIMD
378  m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_sse2;
379  m_resolveSingleConstraintRowLowerLimit = gResolveSingleConstraintRowLowerLimit_sse2;
381 
382 #ifdef BT_ALLOW_SSE4
383  int cpuFeatures = btCpuFeatureUtility::getCpuFeatures();
385  {
386  m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_sse4_1_fma3;
387  m_resolveSingleConstraintRowLowerLimit = gResolveSingleConstraintRowLowerLimit_sse4_1_fma3;
388  }
389 #endif//BT_ALLOW_SSE4
390 #endif //USE_SIMD
391  }
392 }
393 
395  {
396  }
397 
399  {
401  }
402 
404  {
406  }
407 
408 
409 #ifdef USE_SIMD
411  {
412  return gResolveSingleConstraintRowGeneric_sse2;
413  }
415  {
416  return gResolveSingleConstraintRowLowerLimit_sse2;
417  }
418 #ifdef BT_ALLOW_SSE4
420  {
421  return gResolveSingleConstraintRowGeneric_sse4_1_fma3;
422  }
424  {
425  return gResolveSingleConstraintRowLowerLimit_sse4_1_fma3;
426  }
427 #endif //BT_ALLOW_SSE4
428 #endif //USE_SIMD
429 
431 {
432  m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff;
433  return m_btSeed2;
434 }
435 
436 
437 
438 //See ODE: adam's all-int straightforward(?) dRandInt (0..n-1)
440 {
441  // seems good; xor-fold and modulus
442  const unsigned long un = static_cast<unsigned long>(n);
443  unsigned long r = btRand2();
444 
445  // note: probably more aggressive than it needs to be -- might be
446  // able to get away without one or two of the innermost branches.
447  if (un <= 0x00010000UL) {
448  r ^= (r >> 16);
449  if (un <= 0x00000100UL) {
450  r ^= (r >> 8);
451  if (un <= 0x00000010UL) {
452  r ^= (r >> 4);
453  if (un <= 0x00000004UL) {
454  r ^= (r >> 2);
455  if (un <= 0x00000002UL) {
456  r ^= (r >> 1);
457  }
458  }
459  }
460  }
461  }
462 
463  return (int) (r % un);
464 }
465 
466 
467 
469 {
470 
471  btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0;
472 
473  solverBody->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
474  solverBody->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
475  solverBody->internalGetPushVelocity().setValue(0.f,0.f,0.f);
476  solverBody->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
477 
478  if (rb)
479  {
480  solverBody->m_worldTransform = rb->getWorldTransform();
481  solverBody->internalSetInvMass(btVector3(rb->getInvMass(),rb->getInvMass(),rb->getInvMass())*rb->getLinearFactor());
482  solverBody->m_originalBody = rb;
483  solverBody->m_angularFactor = rb->getAngularFactor();
484  solverBody->m_linearFactor = rb->getLinearFactor();
485  solverBody->m_linearVelocity = rb->getLinearVelocity();
486  solverBody->m_angularVelocity = rb->getAngularVelocity();
487  solverBody->m_externalForceImpulse = rb->getTotalForce()*rb->getInvMass()*timeStep;
488  solverBody->m_externalTorqueImpulse = rb->getTotalTorque()*rb->getInvInertiaTensorWorld()*timeStep ;
489 
490  } else
491  {
492  solverBody->m_worldTransform.setIdentity();
493  solverBody->internalSetInvMass(btVector3(0,0,0));
494  solverBody->m_originalBody = 0;
495  solverBody->m_angularFactor.setValue(1,1,1);
496  solverBody->m_linearFactor.setValue(1,1,1);
497  solverBody->m_linearVelocity.setValue(0,0,0);
498  solverBody->m_angularVelocity.setValue(0,0,0);
499  solverBody->m_externalForceImpulse.setValue(0,0,0);
500  solverBody->m_externalTorqueImpulse.setValue(0,0,0);
501  }
502 
503 
504 }
505 
506 
507 
508 
509 
510 
512 {
513  //printf("rel_vel =%f\n", rel_vel);
514  if (btFabs(rel_vel)<velocityThreshold)
515  return 0.;
516 
517  btScalar rest = restitution * -rel_vel;
518  return rest;
519 }
520 
521 
522 
524 {
525 
526 
527  if (colObj && colObj->hasAnisotropicFriction(frictionMode))
528  {
529  // transform to local coordinates
530  btVector3 loc_lateral = frictionDirection * colObj->getWorldTransform().getBasis();
531  const btVector3& friction_scaling = colObj->getAnisotropicFriction();
532  //apply anisotropic friction
533  loc_lateral *= friction_scaling;
534  // ... and transform it back to global coordinates
535  frictionDirection = colObj->getWorldTransform().getBasis() * loc_lateral;
536  }
537 
538 }
539 
540 
541 
542 
543 void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
544 {
545 
546 
547  btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
548  btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
549 
550  btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody;
551  btRigidBody* body1 = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody;
552 
553  solverConstraint.m_solverBodyIdA = solverBodyIdA;
554  solverConstraint.m_solverBodyIdB = solverBodyIdB;
555 
556  solverConstraint.m_friction = cp.m_combinedFriction;
557  solverConstraint.m_originalContactPoint = 0;
558 
559  solverConstraint.m_appliedImpulse = 0.f;
560  solverConstraint.m_appliedPushImpulse = 0.f;
561 
562  if (body0)
563  {
564  solverConstraint.m_contactNormal1 = normalAxis;
565  btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal1);
566  solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
567  solverConstraint.m_angularComponentA = body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor();
568  }else
569  {
570  solverConstraint.m_contactNormal1.setZero();
571  solverConstraint.m_relpos1CrossNormal.setZero();
572  solverConstraint.m_angularComponentA .setZero();
573  }
574 
575  if (body1)
576  {
577  solverConstraint.m_contactNormal2 = -normalAxis;
578  btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal2);
579  solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
580  solverConstraint.m_angularComponentB = body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor();
581  } else
582  {
583  solverConstraint.m_contactNormal2.setZero();
584  solverConstraint.m_relpos2CrossNormal.setZero();
585  solverConstraint.m_angularComponentB.setZero();
586  }
587 
588  {
589  btVector3 vec;
590  btScalar denom0 = 0.f;
591  btScalar denom1 = 0.f;
592  if (body0)
593  {
594  vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
595  denom0 = body0->getInvMass() + normalAxis.dot(vec);
596  }
597  if (body1)
598  {
599  vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
600  denom1 = body1->getInvMass() + normalAxis.dot(vec);
601  }
602  btScalar denom = relaxation/(denom0+denom1);
603  solverConstraint.m_jacDiagABInv = denom;
604  }
605 
606  {
607 
608 
609  btScalar rel_vel;
610  btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity+solverBodyA.m_externalForceImpulse:btVector3(0,0,0))
611  + solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0));
612  btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0))
613  + solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0));
614 
615  rel_vel = vel1Dotn+vel2Dotn;
616 
617 // btScalar positionalError = 0.f;
618 
619  btScalar velocityError = desiredVelocity - rel_vel;
620  btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
621 
622  btScalar penetrationImpulse = btScalar(0);
623 
625  {
626  btScalar distance = (cp.getPositionWorldOnA() - cp.getPositionWorldOnB()).dot(normalAxis);
627  btScalar positionalError = -distance * infoGlobal.m_frictionERP/infoGlobal.m_timeStep;
628  penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
629  }
630 
631  solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
632  solverConstraint.m_rhsPenetration = 0.f;
633  solverConstraint.m_cfm = cfmSlip;
634  solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
635  solverConstraint.m_upperLimit = solverConstraint.m_friction;
636 
637  }
638 }
639 
640 btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
641 {
643  solverConstraint.m_frictionIndex = frictionIndex;
644  setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2,
645  colObj0, colObj1, relaxation, infoGlobal, desiredVelocity, cfmSlip);
646  return solverConstraint;
647 }
648 
649 
650 void btSequentialImpulseConstraintSolver::setupTorsionalFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis1,int solverBodyIdA,int solverBodyIdB,
651  btManifoldPoint& cp,btScalar combinedTorsionalFriction, const btVector3& rel_pos1,const btVector3& rel_pos2,
652  btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation,
653  btScalar desiredVelocity, btScalar cfmSlip)
654 
655 {
656  btVector3 normalAxis(0,0,0);
657 
658 
659  solverConstraint.m_contactNormal1 = normalAxis;
660  solverConstraint.m_contactNormal2 = -normalAxis;
661  btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
662  btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
663 
664  btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody;
665  btRigidBody* body1 = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody;
666 
667  solverConstraint.m_solverBodyIdA = solverBodyIdA;
668  solverConstraint.m_solverBodyIdB = solverBodyIdB;
669 
670  solverConstraint.m_friction = combinedTorsionalFriction;
671  solverConstraint.m_originalContactPoint = 0;
672 
673  solverConstraint.m_appliedImpulse = 0.f;
674  solverConstraint.m_appliedPushImpulse = 0.f;
675 
676  {
677  btVector3 ftorqueAxis1 = -normalAxis1;
678  solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
679  solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor() : btVector3(0,0,0);
680  }
681  {
682  btVector3 ftorqueAxis1 = normalAxis1;
683  solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
684  solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor() : btVector3(0,0,0);
685  }
686 
687 
688  {
689  btVector3 iMJaA = body0?body0->getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal:btVector3(0,0,0);
690  btVector3 iMJaB = body1?body1->getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal:btVector3(0,0,0);
691  btScalar sum = 0;
692  sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
693  sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
694  solverConstraint.m_jacDiagABInv = btScalar(1.)/sum;
695  }
696 
697  {
698 
699 
700  btScalar rel_vel;
701  btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity+solverBodyA.m_externalForceImpulse:btVector3(0,0,0))
702  + solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0));
703  btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0))
704  + solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0));
705 
706  rel_vel = vel1Dotn+vel2Dotn;
707 
708 // btScalar positionalError = 0.f;
709 
710  btSimdScalar velocityError = desiredVelocity - rel_vel;
711  btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
712  solverConstraint.m_rhs = velocityImpulse;
713  solverConstraint.m_cfm = cfmSlip;
714  solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
715  solverConstraint.m_upperLimit = solverConstraint.m_friction;
716 
717  }
718 }
719 
720 
721 
722 
723 
724 
725 
726 
727 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)
728 {
730  solverConstraint.m_frictionIndex = frictionIndex;
731  setupTorsionalFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, combinedTorsionalFriction,rel_pos1, rel_pos2,
732  colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
733  return solverConstraint;
734 }
735 
736 
738 {
739 #if BT_THREADSAFE
740  int solverBodyId = -1;
741  if ( !body.isStaticOrKinematicObject() )
742  {
743  // dynamic body
744  // Dynamic bodies can only be in one island, so it's safe to write to the companionId
745  solverBodyId = body.getCompanionId();
746  if ( solverBodyId < 0 )
747  {
748  if ( btRigidBody* rb = btRigidBody::upcast( &body ) )
749  {
750  solverBodyId = m_tmpSolverBodyPool.size();
751  btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
752  initSolverBody( &solverBody, &body, timeStep );
753  body.setCompanionId( solverBodyId );
754  }
755  }
756  }
757  else if (body.isKinematicObject())
758  {
759  //
760  // NOTE: must test for kinematic before static because some kinematic objects also
761  // identify as "static"
762  //
763  // Kinematic bodies can be in multiple islands at once, so it is a
764  // race condition to write to them, so we use an alternate method
765  // to record the solverBodyId
766  int uniqueId = body.getWorldArrayIndex();
767  const int INVALID_SOLVER_BODY_ID = -1;
769  {
770  m_kinematicBodyUniqueIdToSolverBodyTable.resize(uniqueId + 1, INVALID_SOLVER_BODY_ID);
771  }
773  // if no table entry yet,
774  if ( solverBodyId == INVALID_SOLVER_BODY_ID )
775  {
776  // create a table entry for this body
777  btRigidBody* rb = btRigidBody::upcast( &body );
778  solverBodyId = m_tmpSolverBodyPool.size();
779  btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
780  initSolverBody( &solverBody, &body, timeStep );
782  }
783  }
784  else
785  {
786  // all fixed bodies (inf mass) get mapped to a single solver id
787  if ( m_fixedBodyId < 0 )
788  {
790  btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
791  initSolverBody( &fixedBody, 0, timeStep );
792  }
793  solverBodyId = m_fixedBodyId;
794  }
795  btAssert( solverBodyId < m_tmpSolverBodyPool.size() );
796  return solverBodyId;
797 #else // BT_THREADSAFE
798 
799  int solverBodyIdA = -1;
800 
801  if (body.getCompanionId() >= 0)
802  {
803  //body has already been converted
804  solverBodyIdA = body.getCompanionId();
805  btAssert(solverBodyIdA < m_tmpSolverBodyPool.size());
806  } else
807  {
808  btRigidBody* rb = btRigidBody::upcast(&body);
809  //convert both active and kinematic objects (for their velocity)
810  if (rb && (rb->getInvMass() || rb->isKinematicObject()))
811  {
812  solverBodyIdA = m_tmpSolverBodyPool.size();
813  btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
814  initSolverBody(&solverBody,&body,timeStep);
815  body.setCompanionId(solverBodyIdA);
816  } else
817  {
818 
819  if (m_fixedBodyId<0)
820  {
822  btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
823  initSolverBody(&fixedBody,0,timeStep);
824  }
825  return m_fixedBodyId;
826 // return 0;//assume first one is a fixed solver body
827  }
828  }
829 
830  return solverBodyIdA;
831 #endif // BT_THREADSAFE
832 
833 }
834 #include <stdio.h>
835 
836 
838  int solverBodyIdA, int solverBodyIdB,
839  btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
840  btScalar& relaxation,
841  const btVector3& rel_pos1, const btVector3& rel_pos2)
842 {
843 
844  // const btVector3& pos1 = cp.getPositionWorldOnA();
845  // const btVector3& pos2 = cp.getPositionWorldOnB();
846 
847  btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA];
848  btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB];
849 
850  btRigidBody* rb0 = bodyA->m_originalBody;
851  btRigidBody* rb1 = bodyB->m_originalBody;
852 
853 // btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
854 // btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
855  //rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
856  //rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
857 
858  relaxation = infoGlobal.m_sor;
859  btScalar invTimeStep = btScalar(1)/infoGlobal.m_timeStep;
860 
861  //cfm = 1 / ( dt * kp + kd )
862  //erp = dt * kp / ( dt * kp + kd )
863 
864  btScalar cfm = infoGlobal.m_globalCfm;
865  btScalar erp = infoGlobal.m_erp2;
866 
868  {
870  cfm = cp.m_contactCFM;
872  erp = cp.m_contactERP;
873  } else
874  {
876  {
878  if (denom < SIMD_EPSILON)
879  {
880  denom = SIMD_EPSILON;
881  }
882  cfm = btScalar(1) / denom;
883  erp = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1) / denom;
884  }
885  }
886 
887  cfm *= invTimeStep;
888 
889 
890  btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
891  solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
892  btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);
893  solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
894 
895  {
896 #ifdef COMPUTE_IMPULSE_DENOM
897  btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
898  btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
899 #else
900  btVector3 vec;
901  btScalar denom0 = 0.f;
902  btScalar denom1 = 0.f;
903  if (rb0)
904  {
905  vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
906  denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
907  }
908  if (rb1)
909  {
910  vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
911  denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
912  }
913 #endif //COMPUTE_IMPULSE_DENOM
914 
915  btScalar denom = relaxation/(denom0+denom1+cfm);
916  solverConstraint.m_jacDiagABInv = denom;
917  }
918 
919  if (rb0)
920  {
921  solverConstraint.m_contactNormal1 = cp.m_normalWorldOnB;
922  solverConstraint.m_relpos1CrossNormal = torqueAxis0;
923  } else
924  {
925  solverConstraint.m_contactNormal1.setZero();
926  solverConstraint.m_relpos1CrossNormal.setZero();
927  }
928  if (rb1)
929  {
930  solverConstraint.m_contactNormal2 = -cp.m_normalWorldOnB;
931  solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
932  }else
933  {
934  solverConstraint.m_contactNormal2.setZero();
935  solverConstraint.m_relpos2CrossNormal.setZero();
936  }
937 
938  btScalar restitution = 0.f;
939  btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop;
940 
941  {
942  btVector3 vel1,vel2;
943 
944  vel1 = rb0? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
945  vel2 = rb1? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
946 
947  // btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
948  btVector3 vel = vel1 - vel2;
949  btScalar rel_vel = cp.m_normalWorldOnB.dot(vel);
950 
951 
952 
953  solverConstraint.m_friction = cp.m_combinedFriction;
954 
955 
956  restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution, infoGlobal.m_restitutionVelocityThreshold);
957  if (restitution <= btScalar(0.))
958  {
959  restitution = 0.f;
960  };
961  }
962 
963 
965  if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
966  {
967  solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
968  if (rb0)
969  bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
970  if (rb1)
971  bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
972  } else
973  {
974  solverConstraint.m_appliedImpulse = 0.f;
975  }
976 
977  solverConstraint.m_appliedPushImpulse = 0.f;
978 
979  {
980 
981  btVector3 externalForceImpulseA = bodyA->m_originalBody ? bodyA->m_externalForceImpulse: btVector3(0,0,0);
982  btVector3 externalTorqueImpulseA = bodyA->m_originalBody ? bodyA->m_externalTorqueImpulse: btVector3(0,0,0);
983  btVector3 externalForceImpulseB = bodyB->m_originalBody ? bodyB->m_externalForceImpulse: btVector3(0,0,0);
984  btVector3 externalTorqueImpulseB = bodyB->m_originalBody ?bodyB->m_externalTorqueImpulse : btVector3(0,0,0);
985 
986 
987  btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(bodyA->m_linearVelocity+externalForceImpulseA)
988  + solverConstraint.m_relpos1CrossNormal.dot(bodyA->m_angularVelocity+externalTorqueImpulseA);
989  btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(bodyB->m_linearVelocity+externalForceImpulseB)
990  + solverConstraint.m_relpos2CrossNormal.dot(bodyB->m_angularVelocity+externalTorqueImpulseB);
991  btScalar rel_vel = vel1Dotn+vel2Dotn;
992 
993  btScalar positionalError = 0.f;
994  btScalar velocityError = restitution - rel_vel;// * damping;
995 
996 
997 
998  if (penetration>0)
999  {
1000  positionalError = 0;
1001 
1002  velocityError -= penetration *invTimeStep;
1003  } else
1004  {
1005  positionalError = -penetration * erp*invTimeStep;
1006 
1007  }
1008 
1009  btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
1010  btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
1011 
1012  if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
1013  {
1014  //combine position and velocity into rhs
1015  solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;//-solverConstraint.m_contactNormal1.dot(bodyA->m_externalForce*bodyA->m_invMass-bodyB->m_externalForce/bodyB->m_invMass)*solverConstraint.m_jacDiagABInv;
1016  solverConstraint.m_rhsPenetration = 0.f;
1017 
1018  } else
1019  {
1020  //split position and velocity into rhs and m_rhsPenetration
1021  solverConstraint.m_rhs = velocityImpulse;
1022  solverConstraint.m_rhsPenetration = penetrationImpulse;
1023  }
1024  solverConstraint.m_cfm = cfm*solverConstraint.m_jacDiagABInv;
1025  solverConstraint.m_lowerLimit = 0;
1026  solverConstraint.m_upperLimit = 1e10f;
1027  }
1028 
1029 
1030 
1031 
1032 }
1033 
1034 
1035 
1037  int solverBodyIdA, int solverBodyIdB,
1038  btManifoldPoint& cp, const btContactSolverInfo& infoGlobal)
1039 {
1040 
1041  btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA];
1042  btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB];
1043 
1044  btRigidBody* rb0 = bodyA->m_originalBody;
1045  btRigidBody* rb1 = bodyB->m_originalBody;
1046 
1047  {
1048  btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
1049  if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
1050  {
1051  frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
1052  if (rb0)
1053  bodyA->internalApplyImpulse(frictionConstraint1.m_contactNormal1*rb0->getInvMass()*rb0->getLinearFactor(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
1054  if (rb1)
1055  bodyB->internalApplyImpulse(-frictionConstraint1.m_contactNormal2*rb1->getInvMass()*rb1->getLinearFactor(),-frictionConstraint1.m_angularComponentB,-(btScalar)frictionConstraint1.m_appliedImpulse);
1056  } else
1057  {
1058  frictionConstraint1.m_appliedImpulse = 0.f;
1059  }
1060  }
1061 
1063  {
1064  btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
1065  if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
1066  {
1067  frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
1068  if (rb0)
1069  bodyA->internalApplyImpulse(frictionConstraint2.m_contactNormal1*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
1070  if (rb1)
1071  bodyB->internalApplyImpulse(-frictionConstraint2.m_contactNormal2*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-(btScalar)frictionConstraint2.m_appliedImpulse);
1072  } else
1073  {
1074  frictionConstraint2.m_appliedImpulse = 0.f;
1075  }
1076  }
1077 }
1078 
1079 
1080 
1081 
1083 {
1084  btCollisionObject* colObj0=0,*colObj1=0;
1085 
1086  colObj0 = (btCollisionObject*)manifold->getBody0();
1087  colObj1 = (btCollisionObject*)manifold->getBody1();
1088 
1089  int solverBodyIdA = getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep);
1090  int solverBodyIdB = getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep);
1091 
1092 // btRigidBody* bodyA = btRigidBody::upcast(colObj0);
1093 // btRigidBody* bodyB = btRigidBody::upcast(colObj1);
1094 
1095  btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverBodyIdA];
1096  btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverBodyIdB];
1097 
1098 
1099 
1101  if (!solverBodyA || (solverBodyA->m_invMass.fuzzyZero() && (!solverBodyB || solverBodyB->m_invMass.fuzzyZero())))
1102  return;
1103 
1104  int rollingFriction=1;
1105  for (int j=0;j<manifold->getNumContacts();j++)
1106  {
1107 
1108  btManifoldPoint& cp = manifold->getContactPoint(j);
1109 
1110  if (cp.getDistance() <= manifold->getContactProcessingThreshold())
1111  {
1112  btVector3 rel_pos1;
1113  btVector3 rel_pos2;
1114  btScalar relaxation;
1115 
1116 
1117  int frictionIndex = m_tmpSolverContactConstraintPool.size();
1119  solverConstraint.m_solverBodyIdA = solverBodyIdA;
1120  solverConstraint.m_solverBodyIdB = solverBodyIdB;
1121 
1122  solverConstraint.m_originalContactPoint = &cp;
1123 
1124  const btVector3& pos1 = cp.getPositionWorldOnA();
1125  const btVector3& pos2 = cp.getPositionWorldOnB();
1126 
1127  rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
1128  rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
1129 
1130  btVector3 vel1;
1131  btVector3 vel2;
1132 
1133  solverBodyA->getVelocityInLocalPointNoDelta(rel_pos1,vel1);
1134  solverBodyB->getVelocityInLocalPointNoDelta(rel_pos2,vel2 );
1135 
1136  btVector3 vel = vel1 - vel2;
1137  btScalar rel_vel = cp.m_normalWorldOnB.dot(vel);
1138 
1139  setupContactConstraint(solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, relaxation, rel_pos1, rel_pos2);
1140 
1141 
1142 
1143 
1145 
1147 
1148  if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0))
1149  {
1150 
1151  {
1152  addTorsionalFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,cp.m_combinedSpinningFriction, rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
1153  btVector3 axis0,axis1;
1154  btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1);
1155  axis0.normalize();
1156  axis1.normalize();
1157 
1162  if (axis0.length()>0.001)
1163  addTorsionalFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,
1164  cp.m_combinedRollingFriction, rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
1165  if (axis1.length()>0.001)
1166  addTorsionalFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,
1167  cp.m_combinedRollingFriction, rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
1168 
1169  }
1170  }
1171 
1176 
1187 
1189  {
1190  cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
1191  btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
1193  {
1194  cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel);
1197  addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,infoGlobal);
1198 
1200  {
1205  addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, infoGlobal);
1206  }
1207 
1208  } else
1209  {
1211 
1214  addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, infoGlobal);
1215 
1217  {
1220  addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, infoGlobal);
1221  }
1222 
1223 
1225  {
1227  }
1228  }
1229 
1230  } else
1231  {
1232  addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM);
1233 
1235  addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM);
1236 
1237  }
1238  setFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
1239 
1240 
1241 
1242 
1243  }
1244  }
1245 }
1246 
1248 {
1249  int i;
1250  btPersistentManifold* manifold = 0;
1251 // btCollisionObject* colObj0=0,*colObj1=0;
1252 
1253 
1254  for (i=0;i<numManifolds;i++)
1255  {
1256  manifold = manifoldPtr[i];
1257  convertContact(manifold,infoGlobal);
1258  }
1259 }
1260 
1261 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
1262 {
1263  m_fixedBodyId = -1;
1264  BT_PROFILE("solveGroupCacheFriendlySetup");
1265  (void)debugDrawer;
1266 
1267  // if solver mode has changed,
1268  if ( infoGlobal.m_solverMode != m_cachedSolverMode )
1269  {
1270  // update solver functions to use SIMD or non-SIMD
1271  bool useSimd = !!( infoGlobal.m_solverMode & SOLVER_SIMD );
1272  setupSolverFunctions( useSimd );
1273  m_cachedSolverMode = infoGlobal.m_solverMode;
1274  }
1276 
1277 #ifdef BT_ADDITIONAL_DEBUG
1278  //make sure that dynamic bodies exist for all (enabled) constraints
1279  for (int i=0;i<numConstraints;i++)
1280  {
1281  btTypedConstraint* constraint = constraints[i];
1282  if (constraint->isEnabled())
1283  {
1284  if (!constraint->getRigidBodyA().isStaticOrKinematicObject())
1285  {
1286  bool found=false;
1287  for (int b=0;b<numBodies;b++)
1288  {
1289 
1290  if (&constraint->getRigidBodyA()==bodies[b])
1291  {
1292  found = true;
1293  break;
1294  }
1295  }
1296  btAssert(found);
1297  }
1298  if (!constraint->getRigidBodyB().isStaticOrKinematicObject())
1299  {
1300  bool found=false;
1301  for (int b=0;b<numBodies;b++)
1302  {
1303  if (&constraint->getRigidBodyB()==bodies[b])
1304  {
1305  found = true;
1306  break;
1307  }
1308  }
1309  btAssert(found);
1310  }
1311  }
1312  }
1313  //make sure that dynamic bodies exist for all contact manifolds
1314  for (int i=0;i<numManifolds;i++)
1315  {
1316  if (!manifoldPtr[i]->getBody0()->isStaticOrKinematicObject())
1317  {
1318  bool found=false;
1319  for (int b=0;b<numBodies;b++)
1320  {
1321 
1322  if (manifoldPtr[i]->getBody0()==bodies[b])
1323  {
1324  found = true;
1325  break;
1326  }
1327  }
1328  btAssert(found);
1329  }
1330  if (!manifoldPtr[i]->getBody1()->isStaticOrKinematicObject())
1331  {
1332  bool found=false;
1333  for (int b=0;b<numBodies;b++)
1334  {
1335  if (manifoldPtr[i]->getBody1()==bodies[b])
1336  {
1337  found = true;
1338  break;
1339  }
1340  }
1341  btAssert(found);
1342  }
1343  }
1344 #endif //BT_ADDITIONAL_DEBUG
1345 
1346 
1347  for (int i = 0; i < numBodies; i++)
1348  {
1349  bodies[i]->setCompanionId(-1);
1350  }
1351 #if BT_THREADSAFE
1353 #endif // BT_THREADSAFE
1354 
1355  m_tmpSolverBodyPool.reserve(numBodies+1);
1357 
1358  //btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
1359  //initSolverBody(&fixedBody,0);
1360 
1361  //convert all bodies
1362 
1363 
1364  for (int i=0;i<numBodies;i++)
1365  {
1366  int bodyId = getOrInitSolverBody(*bodies[i],infoGlobal.m_timeStep);
1367 
1368  btRigidBody* body = btRigidBody::upcast(bodies[i]);
1369  if (body && body->getInvMass())
1370  {
1371  btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
1372  btVector3 gyroForce (0,0,0);
1374  {
1375  gyroForce = body->computeGyroscopicForceExplicit(infoGlobal.m_maxGyroscopicForce);
1376  solverBody.m_externalTorqueImpulse -= gyroForce*body->getInvInertiaTensorWorld()*infoGlobal.m_timeStep;
1377  }
1379  {
1380  gyroForce = body->computeGyroscopicImpulseImplicit_World(infoGlobal.m_timeStep);
1381  solverBody.m_externalTorqueImpulse += gyroForce;
1382  }
1384  {
1385  gyroForce = body->computeGyroscopicImpulseImplicit_Body(infoGlobal.m_timeStep);
1386  solverBody.m_externalTorqueImpulse += gyroForce;
1387 
1388  }
1389 
1390 
1391  }
1392  }
1393 
1394  if (1)
1395  {
1396  int j;
1397  for (j=0;j<numConstraints;j++)
1398  {
1399  btTypedConstraint* constraint = constraints[j];
1400  constraint->buildJacobian();
1401  constraint->internalSetAppliedImpulse(0.0f);
1402  }
1403  }
1404 
1405  //btRigidBody* rb0=0,*rb1=0;
1406 
1407  //if (1)
1408  {
1409  {
1410 
1411  int totalNumRows = 0;
1412  int i;
1413 
1415  //calculate the total number of contraint rows
1416  for (i=0;i<numConstraints;i++)
1417  {
1419  btJointFeedback* fb = constraints[i]->getJointFeedback();
1420  if (fb)
1421  {
1426  }
1427 
1428  if (constraints[i]->isEnabled())
1429  {
1430  }
1431  if (constraints[i]->isEnabled())
1432  {
1433  constraints[i]->getInfo1(&info1);
1434  } else
1435  {
1436  info1.m_numConstraintRows = 0;
1437  info1.nub = 0;
1438  }
1439  totalNumRows += info1.m_numConstraintRows;
1440  }
1442 
1443 
1445  int currentRow = 0;
1446 
1447  for (i=0;i<numConstraints;i++)
1448  {
1450 
1451  if (info1.m_numConstraintRows)
1452  {
1453  btAssert(currentRow<totalNumRows);
1454 
1455  btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];
1456  btTypedConstraint* constraint = constraints[i];
1457  btRigidBody& rbA = constraint->getRigidBodyA();
1458  btRigidBody& rbB = constraint->getRigidBodyB();
1459 
1460  int solverBodyIdA = getOrInitSolverBody(rbA,infoGlobal.m_timeStep);
1461  int solverBodyIdB = getOrInitSolverBody(rbB,infoGlobal.m_timeStep);
1462 
1463  btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
1464  btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
1465 
1466 
1467 
1468 
1469  int overrideNumSolverIterations = constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations;
1470  if (overrideNumSolverIterations>m_maxOverrideNumSolverIterations)
1471  m_maxOverrideNumSolverIterations = overrideNumSolverIterations;
1472 
1473 
1474  int j;
1475  for ( j=0;j<info1.m_numConstraintRows;j++)
1476  {
1477  memset(&currentConstraintRow[j],0,sizeof(btSolverConstraint));
1478  currentConstraintRow[j].m_lowerLimit = -SIMD_INFINITY;
1479  currentConstraintRow[j].m_upperLimit = SIMD_INFINITY;
1480  currentConstraintRow[j].m_appliedImpulse = 0.f;
1481  currentConstraintRow[j].m_appliedPushImpulse = 0.f;
1482  currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA;
1483  currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB;
1484  currentConstraintRow[j].m_overrideNumSolverIterations = overrideNumSolverIterations;
1485  }
1486 
1487  bodyAPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
1488  bodyAPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
1489  bodyAPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f);
1490  bodyAPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
1491  bodyBPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
1492  bodyBPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
1493  bodyBPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f);
1494  bodyBPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
1495 
1496 
1498  info2.fps = 1.f/infoGlobal.m_timeStep;
1499  info2.erp = infoGlobal.m_erp;
1500  info2.m_J1linearAxis = currentConstraintRow->m_contactNormal1;
1501  info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal;
1502  info2.m_J2linearAxis = currentConstraintRow->m_contactNormal2;
1503  info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal;
1504  info2.rowskip = sizeof(btSolverConstraint)/sizeof(btScalar);//check this
1506  btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint));
1507  info2.m_constraintError = &currentConstraintRow->m_rhs;
1508  currentConstraintRow->m_cfm = infoGlobal.m_globalCfm;
1509  info2.m_damping = infoGlobal.m_damping;
1510  info2.cfm = &currentConstraintRow->m_cfm;
1511  info2.m_lowerLimit = &currentConstraintRow->m_lowerLimit;
1512  info2.m_upperLimit = &currentConstraintRow->m_upperLimit;
1513  info2.m_numIterations = infoGlobal.m_numIterations;
1514  constraints[i]->getInfo2(&info2);
1515 
1517  for ( j=0;j<info1.m_numConstraintRows;j++)
1518  {
1519  btSolverConstraint& solverConstraint = currentConstraintRow[j];
1520 
1521  if (solverConstraint.m_upperLimit>=constraints[i]->getBreakingImpulseThreshold())
1522  {
1523  solverConstraint.m_upperLimit = constraints[i]->getBreakingImpulseThreshold();
1524  }
1525 
1526  if (solverConstraint.m_lowerLimit<=-constraints[i]->getBreakingImpulseThreshold())
1527  {
1528  solverConstraint.m_lowerLimit = -constraints[i]->getBreakingImpulseThreshold();
1529  }
1530 
1531  solverConstraint.m_originalContactPoint = constraint;
1532 
1533  {
1534  const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
1535  solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld()*ftorqueAxis1*constraint->getRigidBodyA().getAngularFactor();
1536  }
1537  {
1538  const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal;
1539  solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld()*ftorqueAxis2*constraint->getRigidBodyB().getAngularFactor();
1540  }
1541 
1542  {
1543  btVector3 iMJlA = solverConstraint.m_contactNormal1*rbA.getInvMass();
1544  btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal;
1545  btVector3 iMJlB = solverConstraint.m_contactNormal2*rbB.getInvMass();//sign of normal?
1546  btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal;
1547 
1548  btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal1);
1549  sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
1550  sum += iMJlB.dot(solverConstraint.m_contactNormal2);
1551  sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
1552  btScalar fsum = btFabs(sum);
1553  btAssert(fsum > SIMD_EPSILON);
1554  btScalar sorRelaxation = 1.f;//todo: get from globalInfo?
1555  solverConstraint.m_jacDiagABInv = fsum>SIMD_EPSILON?sorRelaxation/sum : 0.f;
1556  }
1557 
1558 
1559 
1560  {
1561  btScalar rel_vel;
1562  btVector3 externalForceImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalForceImpulse : btVector3(0,0,0);
1563  btVector3 externalTorqueImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalTorqueImpulse : btVector3(0,0,0);
1564 
1565  btVector3 externalForceImpulseB = bodyBPtr->m_originalBody ? bodyBPtr->m_externalForceImpulse : btVector3(0,0,0);
1566  btVector3 externalTorqueImpulseB = bodyBPtr->m_originalBody ?bodyBPtr->m_externalTorqueImpulse : btVector3(0,0,0);
1567 
1568  btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(rbA.getLinearVelocity()+externalForceImpulseA)
1569  + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity()+externalTorqueImpulseA);
1570 
1571  btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(rbB.getLinearVelocity()+externalForceImpulseB)
1572  + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity()+externalTorqueImpulseB);
1573 
1574  rel_vel = vel1Dotn+vel2Dotn;
1575  btScalar restitution = 0.f;
1576  btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2
1577  btScalar velocityError = restitution - rel_vel * info2.m_damping;
1578  btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
1579  btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
1580  solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
1581  solverConstraint.m_appliedImpulse = 0.f;
1582 
1583 
1584  }
1585  }
1586  }
1587  currentRow+=m_tmpConstraintSizesPool[i].m_numConstraintRows;
1588  }
1589  }
1590 
1591  convertContacts(manifoldPtr,numManifolds,infoGlobal);
1592 
1593  }
1594 
1595 // btContactSolverInfo info = infoGlobal;
1596 
1597 
1598  int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
1599  int numConstraintPool = m_tmpSolverContactConstraintPool.size();
1600  int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
1601 
1605  m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool*2);
1606  else
1607  m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool);
1608 
1610  {
1611  int i;
1612  for (i=0;i<numNonContactPool;i++)
1613  {
1615  }
1616  for (i=0;i<numConstraintPool;i++)
1617  {
1618  m_orderTmpConstraintPool[i] = i;
1619  }
1620  for (i=0;i<numFrictionPool;i++)
1621  {
1623  }
1624  }
1625 
1626  return 0.f;
1627 
1628 }
1629 
1630 
1631 btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/)
1632 {
1633  btScalar leastSquaresResidual = 0.f;
1634 
1635  int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
1636  int numConstraintPool = m_tmpSolverContactConstraintPool.size();
1637  int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
1638 
1639  if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
1640  {
1641  if (1) // uncomment this for a bit less random ((iteration & 7) == 0)
1642  {
1643 
1644  for (int j=0; j<numNonContactPool; ++j) {
1645  int tmp = m_orderNonContactConstraintPool[j];
1646  int swapi = btRandInt2(j+1);
1648  m_orderNonContactConstraintPool[swapi] = tmp;
1649  }
1650 
1651  //contact/friction constraints are not solved more than
1652  if (iteration< infoGlobal.m_numIterations)
1653  {
1654  for (int j=0; j<numConstraintPool; ++j) {
1655  int tmp = m_orderTmpConstraintPool[j];
1656  int swapi = btRandInt2(j+1);
1658  m_orderTmpConstraintPool[swapi] = tmp;
1659  }
1660 
1661  for (int j=0; j<numFrictionPool; ++j) {
1662  int tmp = m_orderFrictionConstraintPool[j];
1663  int swapi = btRandInt2(j+1);
1665  m_orderFrictionConstraintPool[swapi] = tmp;
1666  }
1667  }
1668  }
1669  }
1670 
1672  for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
1673  {
1675  if (iteration < constraint.m_overrideNumSolverIterations)
1676  {
1678  leastSquaresResidual += residual*residual;
1679  }
1680  }
1681 
1682  if (iteration< infoGlobal.m_numIterations)
1683  {
1684  for (int j=0;j<numConstraints;j++)
1685  {
1686  if (constraints[j]->isEnabled())
1687  {
1688  int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep);
1689  int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
1690  btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
1691  btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
1692  constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
1693  }
1694  }
1695 
1698  {
1699  int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1700  int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)? 2 : 1;
1701 
1702  for (int c=0;c<numPoolConstraints;c++)
1703  {
1704  btScalar totalImpulse =0;
1705 
1706  {
1709  leastSquaresResidual += residual*residual;
1710 
1711  totalImpulse = solveManifold.m_appliedImpulse;
1712  }
1713  bool applyFriction = true;
1714  if (applyFriction)
1715  {
1716  {
1717 
1719 
1720  if (totalImpulse>btScalar(0))
1721  {
1722  solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
1723  solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
1724 
1726  leastSquaresResidual += residual*residual;
1727  }
1728  }
1729 
1731  {
1732 
1734 
1735  if (totalImpulse>btScalar(0))
1736  {
1737  solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
1738  solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
1739 
1741  leastSquaresResidual += residual*residual;
1742  }
1743  }
1744  }
1745  }
1746 
1747  }
1748  else//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
1749  {
1750  //solve the friction constraints after all contact constraints, don't interleave them
1751  int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1752  int j;
1753 
1754  for (j=0;j<numPoolConstraints;j++)
1755  {
1758  leastSquaresResidual += residual*residual;
1759  }
1760 
1761 
1762 
1764 
1765  int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
1766  for (j=0;j<numFrictionPoolConstraints;j++)
1767  {
1769  btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
1770 
1771  if (totalImpulse>btScalar(0))
1772  {
1773  solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
1774  solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
1775 
1777  leastSquaresResidual += residual*residual;
1778  }
1779  }
1780  }
1781 
1782 
1783  int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
1784  for (int j=0;j<numRollingFrictionPoolConstraints;j++)
1785  {
1786 
1788  btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
1789  if (totalImpulse>btScalar(0))
1790  {
1791  btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
1792  if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
1793  rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
1794 
1795  rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
1796  rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
1797 
1798  btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
1799  leastSquaresResidual += residual*residual;
1800  }
1801  }
1802 
1803 
1804  }
1805  return leastSquaresResidual;
1806 }
1807 
1808 
1809 void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
1810 {
1811  int iteration;
1812  if (infoGlobal.m_splitImpulse)
1813  {
1814  {
1815  for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
1816  {
1817  btScalar leastSquaresResidual =0.f;
1818  {
1819  int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1820  int j;
1821  for (j=0;j<numPoolConstraints;j++)
1822  {
1824 
1826  leastSquaresResidual += residual*residual;
1827  }
1828  }
1829  if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration>=(infoGlobal.m_numIterations-1))
1830  {
1831 #ifdef VERBOSE_RESIDUAL_PRINTF
1832  printf("residual = %f at iteration #%d\n",leastSquaresResidual,iteration);
1833 #endif
1834  break;
1835  }
1836  }
1837  }
1838  }
1839 }
1840 
1841 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
1842 {
1843  BT_PROFILE("solveGroupCacheFriendlyIterations");
1844 
1845  {
1847  solveGroupCacheFriendlySplitImpulseIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
1848 
1850 
1851  for ( int iteration = 0 ; iteration< maxIterations ; iteration++)
1852  //for ( int iteration = maxIterations-1 ; iteration >= 0;iteration--)
1853  {
1854  m_leastSquaresResidual = solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
1855 
1856  if (m_leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || (iteration>= (maxIterations-1)))
1857  {
1858 #ifdef VERBOSE_RESIDUAL_PRINTF
1859  printf("residual = %f at iteration #%d\n",m_leastSquaresResidual,iteration);
1860 #endif
1861  break;
1862  }
1863  }
1864 
1865  }
1866  return 0.f;
1867 }
1868 
1870 {
1871  int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1872  int i,j;
1873 
1874  if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
1875  {
1876  for (j=0;j<numPoolConstraints;j++)
1877  {
1878  const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j];
1880  btAssert(pt);
1881  pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
1882  // float f = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
1883  // printf("pt->m_appliedImpulseLateral1 = %f\n", f);
1885  //printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
1887  {
1889  }
1890  //do a callback here?
1891  }
1892  }
1893 
1894  numPoolConstraints = m_tmpSolverNonContactConstraintPool.size();
1895  for (j=0;j<numPoolConstraints;j++)
1896  {
1899  btJointFeedback* fb = constr->getJointFeedback();
1900  if (fb)
1901  {
1902  fb->m_appliedForceBodyA += solverConstr.m_contactNormal1*solverConstr.m_appliedImpulse*constr->getRigidBodyA().getLinearFactor()/infoGlobal.m_timeStep;
1903  fb->m_appliedForceBodyB += solverConstr.m_contactNormal2*solverConstr.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/infoGlobal.m_timeStep;
1904  fb->m_appliedTorqueBodyA += solverConstr.m_relpos1CrossNormal* constr->getRigidBodyA().getAngularFactor()*solverConstr.m_appliedImpulse/infoGlobal.m_timeStep;
1905  fb->m_appliedTorqueBodyB += solverConstr.m_relpos2CrossNormal* constr->getRigidBodyB().getAngularFactor()*solverConstr.m_appliedImpulse/infoGlobal.m_timeStep; /*RGM ???? */
1906 
1907  }
1908 
1909  constr->internalSetAppliedImpulse(solverConstr.m_appliedImpulse);
1910  if (btFabs(solverConstr.m_appliedImpulse)>=constr->getBreakingImpulseThreshold())
1911  {
1912  constr->setEnabled(false);
1913  }
1914  }
1915 
1916 
1917 
1918  for ( i=0;i<m_tmpSolverBodyPool.size();i++)
1919  {
1920  btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody;
1921  if (body)
1922  {
1923  if (infoGlobal.m_splitImpulse)
1924  m_tmpSolverBodyPool[i].writebackVelocityAndTransform(infoGlobal.m_timeStep, infoGlobal.m_splitImpulseTurnErp);
1925  else
1926  m_tmpSolverBodyPool[i].writebackVelocity();
1927 
1928  m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(
1929  m_tmpSolverBodyPool[i].m_linearVelocity+
1930  m_tmpSolverBodyPool[i].m_externalForceImpulse);
1931 
1932  m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(
1933  m_tmpSolverBodyPool[i].m_angularVelocity+
1934  m_tmpSolverBodyPool[i].m_externalTorqueImpulse);
1935 
1936  if (infoGlobal.m_splitImpulse)
1937  m_tmpSolverBodyPool[i].m_originalBody->setWorldTransform(m_tmpSolverBodyPool[i].m_worldTransform);
1938 
1939  m_tmpSolverBodyPool[i].m_originalBody->setCompanionId(-1);
1940  }
1941  }
1942 
1947 
1949  return 0.f;
1950 }
1951 
1952 
1953 
1955 btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btDispatcher* /*dispatcher*/)
1956 {
1957 
1958  BT_PROFILE("solveGroup");
1959  //you need to provide at least some bodies
1960 
1961  solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer);
1962 
1963  solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer);
1964 
1965  solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal);
1966 
1967  return 0.f;
1968 }
1969 
1971 {
1972  m_btSeed2 = 0;
1973 }
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:521
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
void setupFrictionConstraint(btSolverConstraint &solverConstraint, const btVector3 &normalAxis, int solverBodyIdA, int solverBodyIdB, btManifoldPoint &cp, const btVector3 &rel_pos1, const btVector3 &rel_pos2, btCollisionObject *colObj0, btCollisionObject *colObj1, btScalar relaxation, const btContactSolverInfo &infoGlobal, btScalar desiredVelocity=0., btScalar cfmSlip=0.)
virtual void getInfo1(btConstraintInfo1 *info)=0
internal method used by the constraint solver, don&#39;t use them directly
const btVector3 & getPositionWorldOnA() const
btScalar m_combinedContactStiffness1
btVector3 m_lateralFrictionDir1
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:652
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:1283
void resizeNoInitialize(int newsize)
resize changes the number of elements in the array.
btScalar btSqrt(btScalar y)
Definition: btScalar.h:444
btScalar m_appliedImpulse
void setIdentity()
Set this transformation to the identity.
Definition: btTransform.h:172
#define btAssert(x)
Definition: btScalar.h:131
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
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:309
bool fuzzyZero() const
Definition: btVector3.h:701
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:522
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:389
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:683
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)
int getWorldArrayIndex() const
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
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:215
btSimdScalar resolveSplitPenetrationImpulse(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
btScalar m_combinedContactDamping1
btSolverConstraint & addFrictionConstraint(const btVector3 &normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, btManifoldPoint &cp, const btVector3 &rel_pos1, const btVector3 &rel_pos2, btCollisionObject *colObj0, btCollisionObject *colObj1, btScalar relaxation, const btContactSolverInfo &infoGlobal, btScalar desiredVelocity=0., btScalar cfmSlip=0.)
btVector3 m_appliedTorqueBodyB
btSimdScalar m_appliedPushImpulse
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
Definition: btSolverBody.h:108
btAlignedObjectArray< btTypedConstraint::btConstraintInfo1 > m_tmpConstraintSizesPool
TypedConstraint is the baseclass for Bullet constraints and vehicles.
void resize(int newsize, const T &fillData=T())
btRigidBody * m_originalBody
Definition: btSolverBody.h:124
void internalApplyPushImpulse(const btVector3 &linearComponent, const btVector3 &angularComponent, btScalar impulseMagnitude)
Definition: btSolverBody.h:173
static btSimdScalar gResolveSplitPenetrationImpulse_scalar_reference(btSolverBody &body1, btSolverBody &body2, const btSolverConstraint &c)
btVector3 & internalGetDeltaLinearVelocity()
some internal methods, don&#39;t use them
Definition: btSolverBody.h:208
const btCollisionObject * getBody1() const
btScalar restitutionCurve(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold)
void setEnabled(bool enabled)
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
btScalar 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)
static btSimdScalar gResolveSplitPenetrationImpulse_sse2(btSolverBody &body1, btSolverBody &body2, const btSolverConstraint &c)
btVector3 & internalGetPushVelocity()
Definition: btSolverBody.h:233
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:878
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:75
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:292
btScalar m_combinedSpinningFriction
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:263
btAlignedObjectArray< int > m_kinematicBodyUniqueIdToSolverBodyTable
static int uniqueId
Definition: btRigidBody.cpp:27
btScalar btFabs(btScalar x)
Definition: btScalar.h:475
btSimdScalar(* btSingleConstraintRowSolver)(btSolverBody &, btSolverBody &, const btSolverConstraint &)
btTransform m_worldTransform
Definition: btSolverBody.h:111