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