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 #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  btScalar deltaImpulse = 0.f;
307 
308  if (c.m_rhsPenetration)
309  {
311  deltaImpulse = c.m_rhsPenetration-btScalar(c.m_appliedPushImpulse)*c.m_cfm;
314 
315  deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
316  deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv;
317  const btScalar sum = btScalar(c.m_appliedPushImpulse) + deltaImpulse;
318  if (sum < c.m_lowerLimit)
319  {
320  deltaImpulse = c.m_lowerLimit-c.m_appliedPushImpulse;
322  }
323  else
324  {
326  }
329  }
330  return deltaImpulse;
331 }
332 
334 {
335 #ifdef USE_SIMD
336  if (!c.m_rhsPenetration)
337  return 0.f;
338 
340 
341  __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedPushImpulse);
342  __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
343  __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
344  __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)));
345  __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128,body1.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetTurnVelocity().mVec128));
346  __m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128,body2.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetTurnVelocity().mVec128));
347  deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
348  deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
349  btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
350  btSimdScalar resultLowerLess,resultUpperLess;
351  resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
352  resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
353  __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
354  deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
355  c.m_appliedPushImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
356  __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128,body1.internalGetInvMass().mVec128);
357  __m128 linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128,body2.internalGetInvMass().mVec128);
358  __m128 impulseMagnitude = deltaImpulse;
359  body1.internalGetPushVelocity().mVec128 = _mm_add_ps(body1.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
360  body1.internalGetTurnVelocity().mVec128 = _mm_add_ps(body1.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
361  body2.internalGetPushVelocity().mVec128 = _mm_add_ps(body2.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
362  body2.internalGetTurnVelocity().mVec128 = _mm_add_ps(body2.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
363  return deltaImpulse;
364 #else
365  return resolveSplitPenetrationImpulseCacheFriendly(body1,body2,c);
366 #endif
367 }
368 
369 
373  m_btSeed2(0)
374  {
375 
376 #ifdef USE_SIMD
377  m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_sse2;
378  m_resolveSingleConstraintRowLowerLimit=gResolveSingleConstraintRowLowerLimit_sse2;
379 #endif //USE_SIMD
380 
381 #ifdef BT_ALLOW_SSE4
382  int cpuFeatures = btCpuFeatureUtility::getCpuFeatures();
384  {
385  m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_sse4_1_fma3;
386  m_resolveSingleConstraintRowLowerLimit = gResolveSingleConstraintRowLowerLimit_sse4_1_fma3;
387  }
388 #endif//BT_ALLOW_SSE4
389 
390  }
391 
393  {
394  }
395 
397  {
399  }
400 
402  {
404  }
405 
406 
407 #ifdef USE_SIMD
409  {
410  return gResolveSingleConstraintRowGeneric_sse2;
411  }
413  {
414  return gResolveSingleConstraintRowLowerLimit_sse2;
415  }
416 #ifdef BT_ALLOW_SSE4
418  {
419  return gResolveSingleConstraintRowGeneric_sse4_1_fma3;
420  }
422  {
423  return gResolveSingleConstraintRowLowerLimit_sse4_1_fma3;
424  }
425 #endif //BT_ALLOW_SSE4
426 #endif //USE_SIMD
427 
429 {
430  m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff;
431  return m_btSeed2;
432 }
433 
434 
435 
436 //See ODE: adam's all-int straightforward(?) dRandInt (0..n-1)
438 {
439  // seems good; xor-fold and modulus
440  const unsigned long un = static_cast<unsigned long>(n);
441  unsigned long r = btRand2();
442 
443  // note: probably more aggressive than it needs to be -- might be
444  // able to get away without one or two of the innermost branches.
445  if (un <= 0x00010000UL) {
446  r ^= (r >> 16);
447  if (un <= 0x00000100UL) {
448  r ^= (r >> 8);
449  if (un <= 0x00000010UL) {
450  r ^= (r >> 4);
451  if (un <= 0x00000004UL) {
452  r ^= (r >> 2);
453  if (un <= 0x00000002UL) {
454  r ^= (r >> 1);
455  }
456  }
457  }
458  }
459  }
460 
461  return (int) (r % un);
462 }
463 
464 
465 
467 {
468 
469  btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0;
470 
471  solverBody->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
472  solverBody->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
473  solverBody->internalGetPushVelocity().setValue(0.f,0.f,0.f);
474  solverBody->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
475 
476  if (rb)
477  {
478  solverBody->m_worldTransform = rb->getWorldTransform();
479  solverBody->internalSetInvMass(btVector3(rb->getInvMass(),rb->getInvMass(),rb->getInvMass())*rb->getLinearFactor());
480  solverBody->m_originalBody = rb;
481  solverBody->m_angularFactor = rb->getAngularFactor();
482  solverBody->m_linearFactor = rb->getLinearFactor();
483  solverBody->m_linearVelocity = rb->getLinearVelocity();
484  solverBody->m_angularVelocity = rb->getAngularVelocity();
485  solverBody->m_externalForceImpulse = rb->getTotalForce()*rb->getInvMass()*timeStep;
486  solverBody->m_externalTorqueImpulse = rb->getTotalTorque()*rb->getInvInertiaTensorWorld()*timeStep ;
487 
488  } else
489  {
490  solverBody->m_worldTransform.setIdentity();
491  solverBody->internalSetInvMass(btVector3(0,0,0));
492  solverBody->m_originalBody = 0;
493  solverBody->m_angularFactor.setValue(1,1,1);
494  solverBody->m_linearFactor.setValue(1,1,1);
495  solverBody->m_linearVelocity.setValue(0,0,0);
496  solverBody->m_angularVelocity.setValue(0,0,0);
497  solverBody->m_externalForceImpulse.setValue(0,0,0);
498  solverBody->m_externalTorqueImpulse.setValue(0,0,0);
499  }
500 
501 
502 }
503 
504 
505 
506 
507 
508 
510 {
511  btScalar rest = restitution * -rel_vel;
512  return rest;
513 }
514 
515 
516 
518 {
519 
520 
521  if (colObj && colObj->hasAnisotropicFriction(frictionMode))
522  {
523  // transform to local coordinates
524  btVector3 loc_lateral = frictionDirection * colObj->getWorldTransform().getBasis();
525  const btVector3& friction_scaling = colObj->getAnisotropicFriction();
526  //apply anisotropic friction
527  loc_lateral *= friction_scaling;
528  // ... and transform it back to global coordinates
529  frictionDirection = colObj->getWorldTransform().getBasis() * loc_lateral;
530  }
531 
532 }
533 
534 
535 
536 
537 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)
538 {
539 
540 
541  btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
542  btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
543 
544  btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody;
545  btRigidBody* body1 = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody;
546 
547  solverConstraint.m_solverBodyIdA = solverBodyIdA;
548  solverConstraint.m_solverBodyIdB = solverBodyIdB;
549 
550  solverConstraint.m_friction = cp.m_combinedFriction;
551  solverConstraint.m_originalContactPoint = 0;
552 
553  solverConstraint.m_appliedImpulse = 0.f;
554  solverConstraint.m_appliedPushImpulse = 0.f;
555 
556  if (body0)
557  {
558  solverConstraint.m_contactNormal1 = normalAxis;
559  btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal1);
560  solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
561  solverConstraint.m_angularComponentA = body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor();
562  }else
563  {
564  solverConstraint.m_contactNormal1.setZero();
565  solverConstraint.m_relpos1CrossNormal.setZero();
566  solverConstraint.m_angularComponentA .setZero();
567  }
568 
569  if (body1)
570  {
571  solverConstraint.m_contactNormal2 = -normalAxis;
572  btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal2);
573  solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
574  solverConstraint.m_angularComponentB = body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor();
575  } else
576  {
577  solverConstraint.m_contactNormal2.setZero();
578  solverConstraint.m_relpos2CrossNormal.setZero();
579  solverConstraint.m_angularComponentB.setZero();
580  }
581 
582  {
583  btVector3 vec;
584  btScalar denom0 = 0.f;
585  btScalar denom1 = 0.f;
586  if (body0)
587  {
588  vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
589  denom0 = body0->getInvMass() + normalAxis.dot(vec);
590  }
591  if (body1)
592  {
593  vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
594  denom1 = body1->getInvMass() + normalAxis.dot(vec);
595  }
596  btScalar denom = relaxation/(denom0+denom1);
597  solverConstraint.m_jacDiagABInv = denom;
598  }
599 
600  {
601 
602 
603  btScalar rel_vel;
604  btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity+solverBodyA.m_externalForceImpulse:btVector3(0,0,0))
605  + solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0));
606  btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0))
607  + solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0));
608 
609  rel_vel = vel1Dotn+vel2Dotn;
610 
611 // btScalar positionalError = 0.f;
612 
613  btScalar velocityError = desiredVelocity - rel_vel;
614  btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
615  solverConstraint.m_rhs = velocityImpulse;
616  solverConstraint.m_rhsPenetration = 0.f;
617  solverConstraint.m_cfm = cfmSlip;
618  solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
619  solverConstraint.m_upperLimit = solverConstraint.m_friction;
620 
621  }
622 }
623 
624 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)
625 {
627  solverConstraint.m_frictionIndex = frictionIndex;
628  setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2,
629  colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
630  return solverConstraint;
631 }
632 
633 
634 void btSequentialImpulseConstraintSolver::setupTorsionalFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis1,int solverBodyIdA,int solverBodyIdB,
635  btManifoldPoint& cp,btScalar combinedTorsionalFriction, const btVector3& rel_pos1,const btVector3& rel_pos2,
636  btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation,
637  btScalar desiredVelocity, btScalar cfmSlip)
638 
639 {
640  btVector3 normalAxis(0,0,0);
641 
642 
643  solverConstraint.m_contactNormal1 = normalAxis;
644  solverConstraint.m_contactNormal2 = -normalAxis;
645  btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
646  btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
647 
648  btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody;
649  btRigidBody* body1 = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody;
650 
651  solverConstraint.m_solverBodyIdA = solverBodyIdA;
652  solverConstraint.m_solverBodyIdB = solverBodyIdB;
653 
654  solverConstraint.m_friction = combinedTorsionalFriction;
655  solverConstraint.m_originalContactPoint = 0;
656 
657  solverConstraint.m_appliedImpulse = 0.f;
658  solverConstraint.m_appliedPushImpulse = 0.f;
659 
660  {
661  btVector3 ftorqueAxis1 = -normalAxis1;
662  solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
663  solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor() : btVector3(0,0,0);
664  }
665  {
666  btVector3 ftorqueAxis1 = normalAxis1;
667  solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
668  solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor() : btVector3(0,0,0);
669  }
670 
671 
672  {
673  btVector3 iMJaA = body0?body0->getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal:btVector3(0,0,0);
674  btVector3 iMJaB = body1?body1->getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal:btVector3(0,0,0);
675  btScalar sum = 0;
676  sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
677  sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
678  solverConstraint.m_jacDiagABInv = btScalar(1.)/sum;
679  }
680 
681  {
682 
683 
684  btScalar rel_vel;
685  btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity+solverBodyA.m_externalForceImpulse:btVector3(0,0,0))
686  + solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0));
687  btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0))
688  + solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0));
689 
690  rel_vel = vel1Dotn+vel2Dotn;
691 
692 // btScalar positionalError = 0.f;
693 
694  btSimdScalar velocityError = desiredVelocity - rel_vel;
695  btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
696  solverConstraint.m_rhs = velocityImpulse;
697  solverConstraint.m_cfm = cfmSlip;
698  solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
699  solverConstraint.m_upperLimit = solverConstraint.m_friction;
700 
701  }
702 }
703 
704 
705 
706 
707 
708 
709 
710 
711 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)
712 {
714  solverConstraint.m_frictionIndex = frictionIndex;
715  setupTorsionalFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, combinedTorsionalFriction,rel_pos1, rel_pos2,
716  colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
717  return solverConstraint;
718 }
719 
720 
722 {
723 #if BT_THREADSAFE
724  int solverBodyId = -1;
725  if ( !body.isStaticOrKinematicObject() )
726  {
727  // dynamic body
728  // Dynamic bodies can only be in one island, so it's safe to write to the companionId
729  solverBodyId = body.getCompanionId();
730  if ( solverBodyId < 0 )
731  {
732  if ( btRigidBody* rb = btRigidBody::upcast( &body ) )
733  {
734  solverBodyId = m_tmpSolverBodyPool.size();
735  btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
736  initSolverBody( &solverBody, &body, timeStep );
737  body.setCompanionId( solverBodyId );
738  }
739  }
740  }
741  else if (body.isKinematicObject())
742  {
743  //
744  // NOTE: must test for kinematic before static because some kinematic objects also
745  // identify as "static"
746  //
747  // Kinematic bodies can be in multiple islands at once, so it is a
748  // race condition to write to them, so we use an alternate method
749  // to record the solverBodyId
750  int uniqueId = body.getWorldArrayIndex();
751  const int INVALID_SOLVER_BODY_ID = -1;
753  {
754  m_kinematicBodyUniqueIdToSolverBodyTable.resize(uniqueId + 1, INVALID_SOLVER_BODY_ID);
755  }
757  // if no table entry yet,
758  if ( solverBodyId == INVALID_SOLVER_BODY_ID )
759  {
760  // create a table entry for this body
761  btRigidBody* rb = btRigidBody::upcast( &body );
762  solverBodyId = m_tmpSolverBodyPool.size();
763  btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
764  initSolverBody( &solverBody, &body, timeStep );
766  }
767  }
768  else
769  {
770  // all fixed bodies (inf mass) get mapped to a single solver id
771  if ( m_fixedBodyId < 0 )
772  {
774  btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
775  initSolverBody( &fixedBody, 0, timeStep );
776  }
777  solverBodyId = m_fixedBodyId;
778  }
779  btAssert( solverBodyId < m_tmpSolverBodyPool.size() );
780  return solverBodyId;
781 #else // BT_THREADSAFE
782 
783  int solverBodyIdA = -1;
784 
785  if (body.getCompanionId() >= 0)
786  {
787  //body has already been converted
788  solverBodyIdA = body.getCompanionId();
789  btAssert(solverBodyIdA < m_tmpSolverBodyPool.size());
790  } else
791  {
792  btRigidBody* rb = btRigidBody::upcast(&body);
793  //convert both active and kinematic objects (for their velocity)
794  if (rb && (rb->getInvMass() || rb->isKinematicObject()))
795  {
796  solverBodyIdA = m_tmpSolverBodyPool.size();
797  btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
798  initSolverBody(&solverBody,&body,timeStep);
799  body.setCompanionId(solverBodyIdA);
800  } else
801  {
802 
803  if (m_fixedBodyId<0)
804  {
806  btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
807  initSolverBody(&fixedBody,0,timeStep);
808  }
809  return m_fixedBodyId;
810 // return 0;//assume first one is a fixed solver body
811  }
812  }
813 
814  return solverBodyIdA;
815 #endif // BT_THREADSAFE
816 
817 }
818 #include <stdio.h>
819 
820 
822  int solverBodyIdA, int solverBodyIdB,
823  btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
824  btScalar& relaxation,
825  const btVector3& rel_pos1, const btVector3& rel_pos2)
826 {
827 
828  // const btVector3& pos1 = cp.getPositionWorldOnA();
829  // const btVector3& pos2 = cp.getPositionWorldOnB();
830 
831  btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA];
832  btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB];
833 
834  btRigidBody* rb0 = bodyA->m_originalBody;
835  btRigidBody* rb1 = bodyB->m_originalBody;
836 
837 // btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
838 // btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
839  //rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
840  //rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
841 
842  relaxation = infoGlobal.m_sor;
843  btScalar invTimeStep = btScalar(1)/infoGlobal.m_timeStep;
844 
845  //cfm = 1 / ( dt * kp + kd )
846  //erp = dt * kp / ( dt * kp + kd )
847 
848  btScalar cfm = infoGlobal.m_globalCfm;
849  btScalar erp = infoGlobal.m_erp2;
850 
852  {
854  cfm = cp.m_contactCFM;
856  erp = cp.m_contactERP;
857  } else
858  {
860  {
862  if (denom < SIMD_EPSILON)
863  {
864  denom = SIMD_EPSILON;
865  }
866  cfm = btScalar(1) / denom;
867  erp = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1) / denom;
868  }
869  }
870 
871  cfm *= invTimeStep;
872 
873 
874  btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
875  solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
876  btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);
877  solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
878 
879  {
880 #ifdef COMPUTE_IMPULSE_DENOM
881  btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
882  btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
883 #else
884  btVector3 vec;
885  btScalar denom0 = 0.f;
886  btScalar denom1 = 0.f;
887  if (rb0)
888  {
889  vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
890  denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
891  }
892  if (rb1)
893  {
894  vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
895  denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
896  }
897 #endif //COMPUTE_IMPULSE_DENOM
898 
899  btScalar denom = relaxation/(denom0+denom1+cfm);
900  solverConstraint.m_jacDiagABInv = denom;
901  }
902 
903  if (rb0)
904  {
905  solverConstraint.m_contactNormal1 = cp.m_normalWorldOnB;
906  solverConstraint.m_relpos1CrossNormal = torqueAxis0;
907  } else
908  {
909  solverConstraint.m_contactNormal1.setZero();
910  solverConstraint.m_relpos1CrossNormal.setZero();
911  }
912  if (rb1)
913  {
914  solverConstraint.m_contactNormal2 = -cp.m_normalWorldOnB;
915  solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
916  }else
917  {
918  solverConstraint.m_contactNormal2.setZero();
919  solverConstraint.m_relpos2CrossNormal.setZero();
920  }
921 
922  btScalar restitution = 0.f;
923  btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop;
924 
925  {
926  btVector3 vel1,vel2;
927 
928  vel1 = rb0? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
929  vel2 = rb1? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
930 
931  // btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
932  btVector3 vel = vel1 - vel2;
933  btScalar rel_vel = cp.m_normalWorldOnB.dot(vel);
934 
935 
936 
937  solverConstraint.m_friction = cp.m_combinedFriction;
938 
939 
940  restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution);
941  if (restitution <= btScalar(0.))
942  {
943  restitution = 0.f;
944  };
945  }
946 
947 
949  if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
950  {
951  solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
952  if (rb0)
953  bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
954  if (rb1)
955  bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
956  } else
957  {
958  solverConstraint.m_appliedImpulse = 0.f;
959  }
960 
961  solverConstraint.m_appliedPushImpulse = 0.f;
962 
963  {
964 
965  btVector3 externalForceImpulseA = bodyA->m_originalBody ? bodyA->m_externalForceImpulse: btVector3(0,0,0);
966  btVector3 externalTorqueImpulseA = bodyA->m_originalBody ? bodyA->m_externalTorqueImpulse: btVector3(0,0,0);
967  btVector3 externalForceImpulseB = bodyB->m_originalBody ? bodyB->m_externalForceImpulse: btVector3(0,0,0);
968  btVector3 externalTorqueImpulseB = bodyB->m_originalBody ?bodyB->m_externalTorqueImpulse : btVector3(0,0,0);
969 
970 
971  btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(bodyA->m_linearVelocity+externalForceImpulseA)
972  + solverConstraint.m_relpos1CrossNormal.dot(bodyA->m_angularVelocity+externalTorqueImpulseA);
973  btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(bodyB->m_linearVelocity+externalForceImpulseB)
974  + solverConstraint.m_relpos2CrossNormal.dot(bodyB->m_angularVelocity+externalTorqueImpulseB);
975  btScalar rel_vel = vel1Dotn+vel2Dotn;
976 
977  btScalar positionalError = 0.f;
978  btScalar velocityError = restitution - rel_vel;// * damping;
979 
980 
981 
982  if (penetration>0)
983  {
984  positionalError = 0;
985 
986  velocityError -= penetration *invTimeStep;
987  } else
988  {
989  positionalError = -penetration * erp*invTimeStep;
990 
991  }
992 
993  btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
994  btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
995 
996  if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
997  {
998  //combine position and velocity into rhs
999  solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;//-solverConstraint.m_contactNormal1.dot(bodyA->m_externalForce*bodyA->m_invMass-bodyB->m_externalForce/bodyB->m_invMass)*solverConstraint.m_jacDiagABInv;
1000  solverConstraint.m_rhsPenetration = 0.f;
1001 
1002  } else
1003  {
1004  //split position and velocity into rhs and m_rhsPenetration
1005  solverConstraint.m_rhs = velocityImpulse;
1006  solverConstraint.m_rhsPenetration = penetrationImpulse;
1007  }
1008  solverConstraint.m_cfm = cfm*solverConstraint.m_jacDiagABInv;
1009  solverConstraint.m_lowerLimit = 0;
1010  solverConstraint.m_upperLimit = 1e10f;
1011  }
1012 
1013 
1014 
1015 
1016 }
1017 
1018 
1019 
1021  int solverBodyIdA, int solverBodyIdB,
1022  btManifoldPoint& cp, const btContactSolverInfo& infoGlobal)
1023 {
1024 
1025  btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA];
1026  btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB];
1027 
1028  btRigidBody* rb0 = bodyA->m_originalBody;
1029  btRigidBody* rb1 = bodyB->m_originalBody;
1030 
1031  {
1032  btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
1033  if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
1034  {
1035  frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
1036  if (rb0)
1037  bodyA->internalApplyImpulse(frictionConstraint1.m_contactNormal1*rb0->getInvMass()*rb0->getLinearFactor(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
1038  if (rb1)
1039  bodyB->internalApplyImpulse(-frictionConstraint1.m_contactNormal2*rb1->getInvMass()*rb1->getLinearFactor(),-frictionConstraint1.m_angularComponentB,-(btScalar)frictionConstraint1.m_appliedImpulse);
1040  } else
1041  {
1042  frictionConstraint1.m_appliedImpulse = 0.f;
1043  }
1044  }
1045 
1047  {
1048  btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
1049  if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
1050  {
1051  frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
1052  if (rb0)
1053  bodyA->internalApplyImpulse(frictionConstraint2.m_contactNormal1*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
1054  if (rb1)
1055  bodyB->internalApplyImpulse(-frictionConstraint2.m_contactNormal2*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-(btScalar)frictionConstraint2.m_appliedImpulse);
1056  } else
1057  {
1058  frictionConstraint2.m_appliedImpulse = 0.f;
1059  }
1060  }
1061 }
1062 
1063 
1064 
1065 
1067 {
1068  btCollisionObject* colObj0=0,*colObj1=0;
1069 
1070  colObj0 = (btCollisionObject*)manifold->getBody0();
1071  colObj1 = (btCollisionObject*)manifold->getBody1();
1072 
1073  int solverBodyIdA = getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep);
1074  int solverBodyIdB = getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep);
1075 
1076 // btRigidBody* bodyA = btRigidBody::upcast(colObj0);
1077 // btRigidBody* bodyB = btRigidBody::upcast(colObj1);
1078 
1079  btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverBodyIdA];
1080  btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverBodyIdB];
1081 
1082 
1083 
1085  if (!solverBodyA || (solverBodyA->m_invMass.fuzzyZero() && (!solverBodyB || solverBodyB->m_invMass.fuzzyZero())))
1086  return;
1087 
1088  int rollingFriction=1;
1089  for (int j=0;j<manifold->getNumContacts();j++)
1090  {
1091 
1092  btManifoldPoint& cp = manifold->getContactPoint(j);
1093 
1094  if (cp.getDistance() <= manifold->getContactProcessingThreshold())
1095  {
1096  btVector3 rel_pos1;
1097  btVector3 rel_pos2;
1098  btScalar relaxation;
1099 
1100 
1101  int frictionIndex = m_tmpSolverContactConstraintPool.size();
1103  solverConstraint.m_solverBodyIdA = solverBodyIdA;
1104  solverConstraint.m_solverBodyIdB = solverBodyIdB;
1105 
1106  solverConstraint.m_originalContactPoint = &cp;
1107 
1108  const btVector3& pos1 = cp.getPositionWorldOnA();
1109  const btVector3& pos2 = cp.getPositionWorldOnB();
1110 
1111  rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
1112  rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
1113 
1114  btVector3 vel1;
1115  btVector3 vel2;
1116 
1117  solverBodyA->getVelocityInLocalPointNoDelta(rel_pos1,vel1);
1118  solverBodyB->getVelocityInLocalPointNoDelta(rel_pos2,vel2 );
1119 
1120  btVector3 vel = vel1 - vel2;
1121  btScalar rel_vel = cp.m_normalWorldOnB.dot(vel);
1122 
1123  setupContactConstraint(solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, relaxation, rel_pos1, rel_pos2);
1124 
1125 
1126 
1127 
1129 
1131 
1132  if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0))
1133  {
1134 
1135  {
1136  addTorsionalFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,cp.m_combinedSpinningFriction, rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
1137  btVector3 axis0,axis1;
1138  btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1);
1139  axis0.normalize();
1140  axis1.normalize();
1141 
1146  if (axis0.length()>0.001)
1147  addTorsionalFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,
1148  cp.m_combinedRollingFriction, rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
1149  if (axis1.length()>0.001)
1150  addTorsionalFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,
1151  cp.m_combinedRollingFriction, rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
1152 
1153  }
1154  }
1155 
1160 
1172  {
1173  cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
1174  btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
1176  {
1177  cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel);
1180  addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
1181 
1183  {
1188  addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
1189  }
1190 
1191  } else
1192  {
1194 
1197  addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
1198 
1200  {
1203  addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
1204  }
1205 
1206 
1208  {
1210  }
1211  }
1212 
1213  } else
1214  {
1215  addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_frictionCFM);
1216 
1218  addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_frictionCFM);
1219 
1220  }
1221  setFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
1222 
1223 
1224 
1225 
1226  }
1227  }
1228 }
1229 
1231 {
1232  int i;
1233  btPersistentManifold* manifold = 0;
1234 // btCollisionObject* colObj0=0,*colObj1=0;
1235 
1236 
1237  for (i=0;i<numManifolds;i++)
1238  {
1239  manifold = manifoldPtr[i];
1240  convertContact(manifold,infoGlobal);
1241  }
1242 }
1243 
1244 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
1245 {
1246  m_fixedBodyId = -1;
1247  BT_PROFILE("solveGroupCacheFriendlySetup");
1248  (void)debugDrawer;
1249 
1251 
1252 #ifdef BT_ADDITIONAL_DEBUG
1253  //make sure that dynamic bodies exist for all (enabled) constraints
1254  for (int i=0;i<numConstraints;i++)
1255  {
1256  btTypedConstraint* constraint = constraints[i];
1257  if (constraint->isEnabled())
1258  {
1259  if (!constraint->getRigidBodyA().isStaticOrKinematicObject())
1260  {
1261  bool found=false;
1262  for (int b=0;b<numBodies;b++)
1263  {
1264 
1265  if (&constraint->getRigidBodyA()==bodies[b])
1266  {
1267  found = true;
1268  break;
1269  }
1270  }
1271  btAssert(found);
1272  }
1273  if (!constraint->getRigidBodyB().isStaticOrKinematicObject())
1274  {
1275  bool found=false;
1276  for (int b=0;b<numBodies;b++)
1277  {
1278  if (&constraint->getRigidBodyB()==bodies[b])
1279  {
1280  found = true;
1281  break;
1282  }
1283  }
1284  btAssert(found);
1285  }
1286  }
1287  }
1288  //make sure that dynamic bodies exist for all contact manifolds
1289  for (int i=0;i<numManifolds;i++)
1290  {
1291  if (!manifoldPtr[i]->getBody0()->isStaticOrKinematicObject())
1292  {
1293  bool found=false;
1294  for (int b=0;b<numBodies;b++)
1295  {
1296 
1297  if (manifoldPtr[i]->getBody0()==bodies[b])
1298  {
1299  found = true;
1300  break;
1301  }
1302  }
1303  btAssert(found);
1304  }
1305  if (!manifoldPtr[i]->getBody1()->isStaticOrKinematicObject())
1306  {
1307  bool found=false;
1308  for (int b=0;b<numBodies;b++)
1309  {
1310  if (manifoldPtr[i]->getBody1()==bodies[b])
1311  {
1312  found = true;
1313  break;
1314  }
1315  }
1316  btAssert(found);
1317  }
1318  }
1319 #endif //BT_ADDITIONAL_DEBUG
1320 
1321 
1322  for (int i = 0; i < numBodies; i++)
1323  {
1324  bodies[i]->setCompanionId(-1);
1325  }
1326 #if BT_THREADSAFE
1328 #endif // BT_THREADSAFE
1329 
1330  m_tmpSolverBodyPool.reserve(numBodies+1);
1332 
1333  //btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
1334  //initSolverBody(&fixedBody,0);
1335 
1336  //convert all bodies
1337 
1338 
1339  for (int i=0;i<numBodies;i++)
1340  {
1341  int bodyId = getOrInitSolverBody(*bodies[i],infoGlobal.m_timeStep);
1342 
1343  btRigidBody* body = btRigidBody::upcast(bodies[i]);
1344  if (body && body->getInvMass())
1345  {
1346  btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
1347  btVector3 gyroForce (0,0,0);
1349  {
1350  gyroForce = body->computeGyroscopicForceExplicit(infoGlobal.m_maxGyroscopicForce);
1351  solverBody.m_externalTorqueImpulse -= gyroForce*body->getInvInertiaTensorWorld()*infoGlobal.m_timeStep;
1352  }
1354  {
1355  gyroForce = body->computeGyroscopicImpulseImplicit_World(infoGlobal.m_timeStep);
1356  solverBody.m_externalTorqueImpulse += gyroForce;
1357  }
1359  {
1360  gyroForce = body->computeGyroscopicImpulseImplicit_Body(infoGlobal.m_timeStep);
1361  solverBody.m_externalTorqueImpulse += gyroForce;
1362 
1363  }
1364 
1365 
1366  }
1367  }
1368 
1369  if (1)
1370  {
1371  int j;
1372  for (j=0;j<numConstraints;j++)
1373  {
1374  btTypedConstraint* constraint = constraints[j];
1375  constraint->buildJacobian();
1376  constraint->internalSetAppliedImpulse(0.0f);
1377  }
1378  }
1379 
1380  //btRigidBody* rb0=0,*rb1=0;
1381 
1382  //if (1)
1383  {
1384  {
1385 
1386  int totalNumRows = 0;
1387  int i;
1388 
1390  //calculate the total number of contraint rows
1391  for (i=0;i<numConstraints;i++)
1392  {
1394  btJointFeedback* fb = constraints[i]->getJointFeedback();
1395  if (fb)
1396  {
1401  }
1402 
1403  if (constraints[i]->isEnabled())
1404  {
1405  }
1406  if (constraints[i]->isEnabled())
1407  {
1408  constraints[i]->getInfo1(&info1);
1409  } else
1410  {
1411  info1.m_numConstraintRows = 0;
1412  info1.nub = 0;
1413  }
1414  totalNumRows += info1.m_numConstraintRows;
1415  }
1417 
1418 
1420  int currentRow = 0;
1421 
1422  for (i=0;i<numConstraints;i++)
1423  {
1425 
1426  if (info1.m_numConstraintRows)
1427  {
1428  btAssert(currentRow<totalNumRows);
1429 
1430  btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];
1431  btTypedConstraint* constraint = constraints[i];
1432  btRigidBody& rbA = constraint->getRigidBodyA();
1433  btRigidBody& rbB = constraint->getRigidBodyB();
1434 
1435  int solverBodyIdA = getOrInitSolverBody(rbA,infoGlobal.m_timeStep);
1436  int solverBodyIdB = getOrInitSolverBody(rbB,infoGlobal.m_timeStep);
1437 
1438  btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
1439  btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
1440 
1441 
1442 
1443 
1444  int overrideNumSolverIterations = constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations;
1445  if (overrideNumSolverIterations>m_maxOverrideNumSolverIterations)
1446  m_maxOverrideNumSolverIterations = overrideNumSolverIterations;
1447 
1448 
1449  int j;
1450  for ( j=0;j<info1.m_numConstraintRows;j++)
1451  {
1452  memset(&currentConstraintRow[j],0,sizeof(btSolverConstraint));
1453  currentConstraintRow[j].m_lowerLimit = -SIMD_INFINITY;
1454  currentConstraintRow[j].m_upperLimit = SIMD_INFINITY;
1455  currentConstraintRow[j].m_appliedImpulse = 0.f;
1456  currentConstraintRow[j].m_appliedPushImpulse = 0.f;
1457  currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA;
1458  currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB;
1459  currentConstraintRow[j].m_overrideNumSolverIterations = overrideNumSolverIterations;
1460  }
1461 
1462  bodyAPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
1463  bodyAPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
1464  bodyAPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f);
1465  bodyAPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
1466  bodyBPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
1467  bodyBPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
1468  bodyBPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f);
1469  bodyBPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
1470 
1471 
1473  info2.fps = 1.f/infoGlobal.m_timeStep;
1474  info2.erp = infoGlobal.m_erp;
1475  info2.m_J1linearAxis = currentConstraintRow->m_contactNormal1;
1476  info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal;
1477  info2.m_J2linearAxis = currentConstraintRow->m_contactNormal2;
1478  info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal;
1479  info2.rowskip = sizeof(btSolverConstraint)/sizeof(btScalar);//check this
1481  btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint));
1482  info2.m_constraintError = &currentConstraintRow->m_rhs;
1483  currentConstraintRow->m_cfm = infoGlobal.m_globalCfm;
1484  info2.m_damping = infoGlobal.m_damping;
1485  info2.cfm = &currentConstraintRow->m_cfm;
1486  info2.m_lowerLimit = &currentConstraintRow->m_lowerLimit;
1487  info2.m_upperLimit = &currentConstraintRow->m_upperLimit;
1488  info2.m_numIterations = infoGlobal.m_numIterations;
1489  constraints[i]->getInfo2(&info2);
1490 
1492  for ( j=0;j<info1.m_numConstraintRows;j++)
1493  {
1494  btSolverConstraint& solverConstraint = currentConstraintRow[j];
1495 
1496  if (solverConstraint.m_upperLimit>=constraints[i]->getBreakingImpulseThreshold())
1497  {
1498  solverConstraint.m_upperLimit = constraints[i]->getBreakingImpulseThreshold();
1499  }
1500 
1501  if (solverConstraint.m_lowerLimit<=-constraints[i]->getBreakingImpulseThreshold())
1502  {
1503  solverConstraint.m_lowerLimit = -constraints[i]->getBreakingImpulseThreshold();
1504  }
1505 
1506  solverConstraint.m_originalContactPoint = constraint;
1507 
1508  {
1509  const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
1510  solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld()*ftorqueAxis1*constraint->getRigidBodyA().getAngularFactor();
1511  }
1512  {
1513  const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal;
1514  solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld()*ftorqueAxis2*constraint->getRigidBodyB().getAngularFactor();
1515  }
1516 
1517  {
1518  btVector3 iMJlA = solverConstraint.m_contactNormal1*rbA.getInvMass();
1519  btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal;
1520  btVector3 iMJlB = solverConstraint.m_contactNormal2*rbB.getInvMass();//sign of normal?
1521  btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal;
1522 
1523  btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal1);
1524  sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
1525  sum += iMJlB.dot(solverConstraint.m_contactNormal2);
1526  sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
1527  btScalar fsum = btFabs(sum);
1528  btAssert(fsum > SIMD_EPSILON);
1529  solverConstraint.m_jacDiagABInv = fsum>SIMD_EPSILON?btScalar(1.)/sum : 0.f;
1530  }
1531 
1532 
1533 
1534  {
1535  btScalar rel_vel;
1536  btVector3 externalForceImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalForceImpulse : btVector3(0,0,0);
1537  btVector3 externalTorqueImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalTorqueImpulse : btVector3(0,0,0);
1538 
1539  btVector3 externalForceImpulseB = bodyBPtr->m_originalBody ? bodyBPtr->m_externalForceImpulse : btVector3(0,0,0);
1540  btVector3 externalTorqueImpulseB = bodyBPtr->m_originalBody ?bodyBPtr->m_externalTorqueImpulse : btVector3(0,0,0);
1541 
1542  btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(rbA.getLinearVelocity()+externalForceImpulseA)
1543  + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity()+externalTorqueImpulseA);
1544 
1545  btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(rbB.getLinearVelocity()+externalForceImpulseB)
1546  + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity()+externalTorqueImpulseB);
1547 
1548  rel_vel = vel1Dotn+vel2Dotn;
1549  btScalar restitution = 0.f;
1550  btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2
1551  btScalar velocityError = restitution - rel_vel * info2.m_damping;
1552  btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
1553  btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
1554  solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
1555  solverConstraint.m_appliedImpulse = 0.f;
1556 
1557 
1558  }
1559  }
1560  }
1561  currentRow+=m_tmpConstraintSizesPool[i].m_numConstraintRows;
1562  }
1563  }
1564 
1565  convertContacts(manifoldPtr,numManifolds,infoGlobal);
1566 
1567  }
1568 
1569 // btContactSolverInfo info = infoGlobal;
1570 
1571 
1572  int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
1573  int numConstraintPool = m_tmpSolverContactConstraintPool.size();
1574  int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
1575 
1579  m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool*2);
1580  else
1581  m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool);
1582 
1584  {
1585  int i;
1586  for (i=0;i<numNonContactPool;i++)
1587  {
1589  }
1590  for (i=0;i<numConstraintPool;i++)
1591  {
1592  m_orderTmpConstraintPool[i] = i;
1593  }
1594  for (i=0;i<numFrictionPool;i++)
1595  {
1597  }
1598  }
1599 
1600  return 0.f;
1601 
1602 }
1603 
1604 
1605 btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/)
1606 {
1607  btScalar leastSquaresResidual = 0.f;
1608 
1609  int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
1610  int numConstraintPool = m_tmpSolverContactConstraintPool.size();
1611  int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
1612 
1613  if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
1614  {
1615  if (1) // uncomment this for a bit less random ((iteration & 7) == 0)
1616  {
1617 
1618  for (int j=0; j<numNonContactPool; ++j) {
1619  int tmp = m_orderNonContactConstraintPool[j];
1620  int swapi = btRandInt2(j+1);
1622  m_orderNonContactConstraintPool[swapi] = tmp;
1623  }
1624 
1625  //contact/friction constraints are not solved more than
1626  if (iteration< infoGlobal.m_numIterations)
1627  {
1628  for (int j=0; j<numConstraintPool; ++j) {
1629  int tmp = m_orderTmpConstraintPool[j];
1630  int swapi = btRandInt2(j+1);
1632  m_orderTmpConstraintPool[swapi] = tmp;
1633  }
1634 
1635  for (int j=0; j<numFrictionPool; ++j) {
1636  int tmp = m_orderFrictionConstraintPool[j];
1637  int swapi = btRandInt2(j+1);
1639  m_orderFrictionConstraintPool[swapi] = tmp;
1640  }
1641  }
1642  }
1643  }
1644 
1645  if (infoGlobal.m_solverMode & SOLVER_SIMD)
1646  {
1648  for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
1649  {
1651  if (iteration < constraint.m_overrideNumSolverIterations)
1652  {
1654  leastSquaresResidual += residual*residual;
1655  }
1656  }
1657 
1658  if (iteration< infoGlobal.m_numIterations)
1659  {
1660  for (int j=0;j<numConstraints;j++)
1661  {
1662  if (constraints[j]->isEnabled())
1663  {
1664  int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep);
1665  int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
1666  btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
1667  btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
1668  constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
1669  }
1670  }
1671 
1674  {
1675  int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1676  int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)? 2 : 1;
1677 
1678  for (int c=0;c<numPoolConstraints;c++)
1679  {
1680  btScalar totalImpulse =0;
1681 
1682  {
1685  leastSquaresResidual += residual*residual;
1686 
1687  totalImpulse = solveManifold.m_appliedImpulse;
1688  }
1689  bool applyFriction = true;
1690  if (applyFriction)
1691  {
1692  {
1693 
1695 
1696  if (totalImpulse>btScalar(0))
1697  {
1698  solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
1699  solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
1700 
1702  leastSquaresResidual += residual*residual;
1703  }
1704  }
1705 
1707  {
1708 
1710 
1711  if (totalImpulse>btScalar(0))
1712  {
1713  solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
1714  solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
1715 
1717  leastSquaresResidual += residual*residual;
1718  }
1719  }
1720  }
1721  }
1722 
1723  }
1724  else//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
1725  {
1726  //solve the friction constraints after all contact constraints, don't interleave them
1727  int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1728  int j;
1729 
1730  for (j=0;j<numPoolConstraints;j++)
1731  {
1734  leastSquaresResidual += residual*residual;
1735  }
1736 
1737 
1738 
1740 
1741  int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
1742  for (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  leastSquaresResidual += residual*residual;
1754  }
1755  }
1756 
1757 
1758  int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
1759  for (j=0;j<numRollingFrictionPoolConstraints;j++)
1760  {
1761 
1763  btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
1764  if (totalImpulse>btScalar(0))
1765  {
1766  btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
1767  if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
1768  rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
1769 
1770  rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
1771  rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
1772 
1773  btScalar residual = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
1774  leastSquaresResidual += residual*residual;
1775  }
1776  }
1777 
1778 
1779  }
1780  }
1781  } else
1782  {
1783  //non-SIMD version
1785  for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
1786  {
1788  if (iteration < constraint.m_overrideNumSolverIterations)
1789  {
1791  leastSquaresResidual += residual*residual;
1792  }
1793  }
1794 
1795  if (iteration< infoGlobal.m_numIterations)
1796  {
1797  for (int j=0;j<numConstraints;j++)
1798  {
1799  if (constraints[j]->isEnabled())
1800  {
1801  int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep);
1802  int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
1803  btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
1804  btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
1805  constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
1806  }
1807  }
1809  int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1810  for (int j=0;j<numPoolConstraints;j++)
1811  {
1814  leastSquaresResidual += residual*residual;
1815  }
1817  int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
1818  for (int j=0;j<numFrictionPoolConstraints;j++)
1819  {
1821  btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
1822 
1823  if (totalImpulse>btScalar(0))
1824  {
1825  solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
1826  solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
1827 
1829  leastSquaresResidual += residual*residual;
1830  }
1831  }
1832 
1833  int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
1834  for (int j=0;j<numRollingFrictionPoolConstraints;j++)
1835  {
1837  btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
1838  if (totalImpulse>btScalar(0))
1839  {
1840  btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
1841  if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
1842  rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
1843 
1844  rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
1845  rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
1846 
1847  btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
1848  leastSquaresResidual += residual*residual;
1849  }
1850  }
1851  }
1852  }
1853  return leastSquaresResidual;
1854 }
1855 
1856 
1857 void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
1858 {
1859  int iteration;
1860  if (infoGlobal.m_splitImpulse)
1861  {
1862  if (infoGlobal.m_solverMode & SOLVER_SIMD)
1863  {
1864  for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
1865  {
1866  btScalar leastSquaresResidual =0.f;
1867  {
1868  int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1869  int j;
1870  for (j=0;j<numPoolConstraints;j++)
1871  {
1873 
1874  btScalar residual = resolveSplitPenetrationSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
1875  leastSquaresResidual += residual*residual;
1876  }
1877  }
1878  if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration>=(infoGlobal.m_numIterations-1))
1879  {
1880 #ifdef VERBOSE_RESIDUAL_PRINTF
1881  printf("residual = %f at iteration #%d\n",leastSquaresResidual,iteration);
1882 #endif
1883  break;
1884  }
1885  }
1886  }
1887  else
1888  {
1889  for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
1890  {
1891  btScalar leastSquaresResidual = 0.f;
1892  {
1893  int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1894  int j;
1895  for (j=0;j<numPoolConstraints;j++)
1896  {
1898 
1900  leastSquaresResidual += residual*residual;
1901  }
1902  if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration>=(infoGlobal.m_numIterations-1))
1903  {
1904 #ifdef VERBOSE_RESIDUAL_PRINTF
1905  printf("residual = %f at iteration #%d\n",leastSquaresResidual,iteration);
1906 #endif
1907  break;
1908  }
1909  }
1910  }
1911  }
1912  }
1913 }
1914 
1915 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
1916 {
1917  BT_PROFILE("solveGroupCacheFriendlyIterations");
1918 
1919  {
1921  solveGroupCacheFriendlySplitImpulseIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
1922 
1924 
1925  for ( int iteration = 0 ; iteration< maxIterations ; iteration++)
1926  //for ( int iteration = maxIterations-1 ; iteration >= 0;iteration--)
1927  {
1928  m_leastSquaresResidual = solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
1929 
1930  if (m_leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || (iteration>= (maxIterations-1)))
1931  {
1932 #ifdef VERBOSE_RESIDUAL_PRINTF
1933  printf("residual = %f at iteration #%d\n",m_leastSquaresResidual,iteration);
1934 #endif
1935  break;
1936  }
1937  }
1938 
1939  }
1940  return 0.f;
1941 }
1942 
1944 {
1945  int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1946  int i,j;
1947 
1948  if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
1949  {
1950  for (j=0;j<numPoolConstraints;j++)
1951  {
1952  const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j];
1954  btAssert(pt);
1955  pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
1956  // float f = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
1957  // printf("pt->m_appliedImpulseLateral1 = %f\n", f);
1959  //printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
1961  {
1963  }
1964  //do a callback here?
1965  }
1966  }
1967 
1968  numPoolConstraints = m_tmpSolverNonContactConstraintPool.size();
1969  for (j=0;j<numPoolConstraints;j++)
1970  {
1973  btJointFeedback* fb = constr->getJointFeedback();
1974  if (fb)
1975  {
1976  fb->m_appliedForceBodyA += solverConstr.m_contactNormal1*solverConstr.m_appliedImpulse*constr->getRigidBodyA().getLinearFactor()/infoGlobal.m_timeStep;
1977  fb->m_appliedForceBodyB += solverConstr.m_contactNormal2*solverConstr.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/infoGlobal.m_timeStep;
1978  fb->m_appliedTorqueBodyA += solverConstr.m_relpos1CrossNormal* constr->getRigidBodyA().getAngularFactor()*solverConstr.m_appliedImpulse/infoGlobal.m_timeStep;
1979  fb->m_appliedTorqueBodyB += solverConstr.m_relpos2CrossNormal* constr->getRigidBodyB().getAngularFactor()*solverConstr.m_appliedImpulse/infoGlobal.m_timeStep; /*RGM ???? */
1980 
1981  }
1982 
1983  constr->internalSetAppliedImpulse(solverConstr.m_appliedImpulse);
1984  if (btFabs(solverConstr.m_appliedImpulse)>=constr->getBreakingImpulseThreshold())
1985  {
1986  constr->setEnabled(false);
1987  }
1988  }
1989 
1990 
1991 
1992  for ( i=0;i<m_tmpSolverBodyPool.size();i++)
1993  {
1994  btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody;
1995  if (body)
1996  {
1997  if (infoGlobal.m_splitImpulse)
1998  m_tmpSolverBodyPool[i].writebackVelocityAndTransform(infoGlobal.m_timeStep, infoGlobal.m_splitImpulseTurnErp);
1999  else
2000  m_tmpSolverBodyPool[i].writebackVelocity();
2001 
2002  m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(
2003  m_tmpSolverBodyPool[i].m_linearVelocity+
2004  m_tmpSolverBodyPool[i].m_externalForceImpulse);
2005 
2006  m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(
2007  m_tmpSolverBodyPool[i].m_angularVelocity+
2008  m_tmpSolverBodyPool[i].m_externalTorqueImpulse);
2009 
2010  if (infoGlobal.m_splitImpulse)
2011  m_tmpSolverBodyPool[i].m_originalBody->setWorldTransform(m_tmpSolverBodyPool[i].m_worldTransform);
2012 
2013  m_tmpSolverBodyPool[i].m_originalBody->setCompanionId(-1);
2014  }
2015  }
2016 
2021 
2023  return 0.f;
2024 }
2025 
2026 
2027 
2029 btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btDispatcher* /*dispatcher*/)
2030 {
2031 
2032  BT_PROFILE("solveGroup");
2033  //you need to provide at least some bodies
2034 
2035  solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer);
2036 
2037  solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer);
2038 
2039  solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal);
2040 
2041  return 0.f;
2042 }
2043 
2045 {
2046  m_btSeed2 = 0;
2047 }
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
btSimdScalar resolveSplitPenetrationSIMD(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
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)
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
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:213
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 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
btScalar resolveSplitPenetrationImpulseCacheFriendly(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
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:279
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:450
btSimdScalar(* btSingleConstraintRowSolver)(btSolverBody &, btSolverBody &, const btSolverConstraint &)
btTransform m_worldTransform
Definition: btSolverBody.h:111