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