Blender  V3.3
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 #ifdef BT_DEBUG
18 # define BT_ADDITIONAL_DEBUG
19 #endif
20 
21 //It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms.
22 
25 
28 
29 //#include "btJacobianEntry.h"
30 #include "LinearMath/btMinMax.h"
32 #include <new>
34 #include "LinearMath/btQuickprof.h"
35 //#include "btSolverBody.h"
36 //#include "btSolverConstraint.h"
38 #include <string.h> //for memset
39 
41 
43 
44 //#define VERBOSE_RESIDUAL_PRINTF 1
48 {
49  btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse) * c.m_cfm;
50  const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(bodyA.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(bodyA.internalGetDeltaAngularVelocity());
51  const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(bodyB.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(bodyB.internalGetDeltaAngularVelocity());
52 
53  // const btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn;
54  deltaImpulse -= deltaVel1Dotn * c.m_jacDiagABInv;
55  deltaImpulse -= deltaVel2Dotn * c.m_jacDiagABInv;
56 
57  const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
58  if (sum < c.m_lowerLimit)
59  {
60  deltaImpulse = c.m_lowerLimit - c.m_appliedImpulse;
61  c.m_appliedImpulse = c.m_lowerLimit;
62  }
63  else if (sum > c.m_upperLimit)
64  {
65  deltaImpulse = c.m_upperLimit - c.m_appliedImpulse;
66  c.m_appliedImpulse = c.m_upperLimit;
67  }
68  else
69  {
70  c.m_appliedImpulse = sum;
71  }
72 
73  bodyA.internalApplyImpulse(c.m_contactNormal1 * bodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
74  bodyB.internalApplyImpulse(c.m_contactNormal2 * bodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
75 
76  return deltaImpulse * (1. / c.m_jacDiagABInv);
77 }
78 
80 {
81  btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse) * c.m_cfm;
82  const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(bodyA.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(bodyA.internalGetDeltaAngularVelocity());
83  const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(bodyB.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(bodyB.internalGetDeltaAngularVelocity());
84 
85  deltaImpulse -= deltaVel1Dotn * c.m_jacDiagABInv;
86  deltaImpulse -= deltaVel2Dotn * c.m_jacDiagABInv;
87  const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
88  if (sum < c.m_lowerLimit)
89  {
90  deltaImpulse = c.m_lowerLimit - c.m_appliedImpulse;
91  c.m_appliedImpulse = c.m_lowerLimit;
92  }
93  else
94  {
95  c.m_appliedImpulse = sum;
96  }
97  bodyA.internalApplyImpulse(c.m_contactNormal1 * bodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
98  bodyB.internalApplyImpulse(c.m_contactNormal2 * bodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
99 
100  return deltaImpulse * (1. / c.m_jacDiagABInv);
101 }
102 
103 #ifdef USE_SIMD
104 #include <emmintrin.h>
105 
106 #define btVecSplat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e, e, e, e))
107 static inline __m128 btSimdDot3(__m128 vec0, __m128 vec1)
108 {
109  __m128 result = _mm_mul_ps(vec0, vec1);
110  return _mm_add_ps(btVecSplat(result, 0), _mm_add_ps(btVecSplat(result, 1), btVecSplat(result, 2)));
111 }
112 
113 #if defined(BT_ALLOW_SSE4)
114 #include <intrin.h>
115 
116 #define USE_FMA 1
117 #define USE_FMA3_INSTEAD_FMA4 1
118 #define USE_SSE4_DOT 1
119 
120 #define SSE4_DP(a, b) _mm_dp_ps(a, b, 0x7f)
121 #define SSE4_DP_FP(a, b) _mm_cvtss_f32(_mm_dp_ps(a, b, 0x7f))
122 
123 #if USE_SSE4_DOT
124 #define DOT_PRODUCT(a, b) SSE4_DP(a, b)
125 #else
126 #define DOT_PRODUCT(a, b) btSimdDot3(a, b)
127 #endif
128 
129 #if USE_FMA
130 #if USE_FMA3_INSTEAD_FMA4
131 // a*b + c
132 #define FMADD(a, b, c) _mm_fmadd_ps(a, b, c)
133 // -(a*b) + c
134 #define FMNADD(a, b, c) _mm_fnmadd_ps(a, b, c)
135 #else // USE_FMA3
136 // a*b + c
137 #define FMADD(a, b, c) _mm_macc_ps(a, b, c)
138 // -(a*b) + c
139 #define FMNADD(a, b, c) _mm_nmacc_ps(a, b, c)
140 #endif
141 #else // USE_FMA
142 // c + a*b
143 #define FMADD(a, b, c) _mm_add_ps(c, _mm_mul_ps(a, b))
144 // c - a*b
145 #define FMNADD(a, b, c) _mm_sub_ps(c, _mm_mul_ps(a, b))
146 #endif
147 #endif
148 
149 // Project Gauss Seidel or the equivalent Sequential Impulse
150 static btScalar gResolveSingleConstraintRowGeneric_sse2(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c)
151 {
152  __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
153  __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
154  __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
155  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)));
156  __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128, bodyA.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128, bodyA.internalGetDeltaAngularVelocity().mVec128));
157  __m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128, bodyB.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128, bodyB.internalGetDeltaAngularVelocity().mVec128));
158  deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel1Dotn, _mm_set1_ps(c.m_jacDiagABInv)));
159  deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel2Dotn, _mm_set1_ps(c.m_jacDiagABInv)));
160  btSimdScalar sum = _mm_add_ps(cpAppliedImp, deltaImpulse);
161  btSimdScalar resultLowerLess, resultUpperLess;
162  resultLowerLess = _mm_cmplt_ps(sum, lowerLimit1);
163  resultUpperLess = _mm_cmplt_ps(sum, upperLimit1);
164  __m128 lowMinApplied = _mm_sub_ps(lowerLimit1, cpAppliedImp);
165  deltaImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse));
166  c.m_appliedImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum));
167  __m128 upperMinApplied = _mm_sub_ps(upperLimit1, cpAppliedImp);
168  deltaImpulse = _mm_or_ps(_mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied));
169  c.m_appliedImpulse = _mm_or_ps(_mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1));
170  __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128);
171  __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal2).mVec128, bodyB.internalGetInvMass().mVec128);
172  __m128 impulseMagnitude = deltaImpulse;
173  bodyA.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(bodyA.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude));
174  bodyA.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(bodyA.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude));
175  bodyB.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(bodyB.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude));
176  bodyB.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(bodyB.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude));
177  return deltaImpulse.m_floats[0] / c.m_jacDiagABInv;
178 }
179 
180 // Enhanced version of gResolveSingleConstraintRowGeneric_sse2 with SSE4.1 and FMA3
181 static btScalar gResolveSingleConstraintRowGeneric_sse4_1_fma3(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c)
182 {
183 #if defined(BT_ALLOW_SSE4)
184  __m128 tmp = _mm_set_ps1(c.m_jacDiagABInv);
185  __m128 deltaImpulse = _mm_set_ps1(c.m_rhs - btScalar(c.m_appliedImpulse) * c.m_cfm);
186  const __m128 lowerLimit = _mm_set_ps1(c.m_lowerLimit);
187  const __m128 upperLimit = _mm_set_ps1(c.m_upperLimit);
188  const __m128 deltaVel1Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal1.mVec128, bodyA.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos1CrossNormal.mVec128, bodyA.internalGetDeltaAngularVelocity().mVec128));
189  const __m128 deltaVel2Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal2.mVec128, bodyB.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos2CrossNormal.mVec128, bodyB.internalGetDeltaAngularVelocity().mVec128));
190  deltaImpulse = FMNADD(deltaVel1Dotn, tmp, deltaImpulse);
191  deltaImpulse = FMNADD(deltaVel2Dotn, tmp, deltaImpulse);
192  tmp = _mm_add_ps(c.m_appliedImpulse, deltaImpulse); // sum
193  const __m128 maskLower = _mm_cmpgt_ps(tmp, lowerLimit);
194  const __m128 maskUpper = _mm_cmpgt_ps(upperLimit, tmp);
195  deltaImpulse = _mm_blendv_ps(_mm_sub_ps(lowerLimit, c.m_appliedImpulse), _mm_blendv_ps(_mm_sub_ps(upperLimit, c.m_appliedImpulse), deltaImpulse, maskUpper), maskLower);
196  c.m_appliedImpulse = _mm_blendv_ps(lowerLimit, _mm_blendv_ps(upperLimit, tmp, maskUpper), maskLower);
197  bodyA.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128), deltaImpulse, bodyA.internalGetDeltaLinearVelocity().mVec128);
198  bodyA.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentA.mVec128, deltaImpulse, bodyA.internalGetDeltaAngularVelocity().mVec128);
199  bodyB.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal2.mVec128, bodyB.internalGetInvMass().mVec128), deltaImpulse, bodyB.internalGetDeltaLinearVelocity().mVec128);
200  bodyB.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentB.mVec128, deltaImpulse, bodyB.internalGetDeltaAngularVelocity().mVec128);
201  btSimdScalar deltaImp = deltaImpulse;
202  return deltaImp.m_floats[0] * (1. / c.m_jacDiagABInv);
203 #else
204  return gResolveSingleConstraintRowGeneric_sse2(bodyA, bodyB, c);
205 #endif
206 }
207 
208 static btScalar gResolveSingleConstraintRowLowerLimit_sse2(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c)
209 {
210  __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
211  __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
212  __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
213  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)));
214  __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128, bodyA.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128, bodyA.internalGetDeltaAngularVelocity().mVec128));
215  __m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128, bodyB.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128, bodyB.internalGetDeltaAngularVelocity().mVec128));
216  deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel1Dotn, _mm_set1_ps(c.m_jacDiagABInv)));
217  deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel2Dotn, _mm_set1_ps(c.m_jacDiagABInv)));
218  btSimdScalar sum = _mm_add_ps(cpAppliedImp, deltaImpulse);
219  btSimdScalar resultLowerLess, resultUpperLess;
220  resultLowerLess = _mm_cmplt_ps(sum, lowerLimit1);
221  resultUpperLess = _mm_cmplt_ps(sum, upperLimit1);
222  __m128 lowMinApplied = _mm_sub_ps(lowerLimit1, cpAppliedImp);
223  deltaImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse));
224  c.m_appliedImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum));
225  __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128);
226  __m128 linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128, bodyB.internalGetInvMass().mVec128);
227  __m128 impulseMagnitude = deltaImpulse;
228  bodyA.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(bodyA.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude));
229  bodyA.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(bodyA.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude));
230  bodyB.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(bodyB.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude));
231  bodyB.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(bodyB.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude));
232  return deltaImpulse.m_floats[0] / c.m_jacDiagABInv;
233 }
234 
235 // Enhanced version of gResolveSingleConstraintRowGeneric_sse2 with SSE4.1 and FMA3
236 static btScalar gResolveSingleConstraintRowLowerLimit_sse4_1_fma3(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c)
237 {
238 #ifdef BT_ALLOW_SSE4
239  __m128 tmp = _mm_set_ps1(c.m_jacDiagABInv);
240  __m128 deltaImpulse = _mm_set_ps1(c.m_rhs - btScalar(c.m_appliedImpulse) * c.m_cfm);
241  const __m128 lowerLimit = _mm_set_ps1(c.m_lowerLimit);
242  const __m128 deltaVel1Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal1.mVec128, bodyA.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos1CrossNormal.mVec128, bodyA.internalGetDeltaAngularVelocity().mVec128));
243  const __m128 deltaVel2Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal2.mVec128, bodyB.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos2CrossNormal.mVec128, bodyB.internalGetDeltaAngularVelocity().mVec128));
244  deltaImpulse = FMNADD(deltaVel1Dotn, tmp, deltaImpulse);
245  deltaImpulse = FMNADD(deltaVel2Dotn, tmp, deltaImpulse);
246  tmp = _mm_add_ps(c.m_appliedImpulse, deltaImpulse);
247  const __m128 mask = _mm_cmpgt_ps(tmp, lowerLimit);
248  deltaImpulse = _mm_blendv_ps(_mm_sub_ps(lowerLimit, c.m_appliedImpulse), deltaImpulse, mask);
249  c.m_appliedImpulse = _mm_blendv_ps(lowerLimit, tmp, mask);
250  bodyA.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128), deltaImpulse, bodyA.internalGetDeltaLinearVelocity().mVec128);
251  bodyA.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentA.mVec128, deltaImpulse, bodyA.internalGetDeltaAngularVelocity().mVec128);
252  bodyB.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal2.mVec128, bodyB.internalGetInvMass().mVec128), deltaImpulse, bodyB.internalGetDeltaLinearVelocity().mVec128);
253  bodyB.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentB.mVec128, deltaImpulse, bodyB.internalGetDeltaAngularVelocity().mVec128);
254  btSimdScalar deltaImp = deltaImpulse;
255  return deltaImp.m_floats[0] * (1. / c.m_jacDiagABInv);
256 #else
257  return gResolveSingleConstraintRowLowerLimit_sse2(bodyA, bodyB, c);
258 #endif //BT_ALLOW_SSE4
259 }
260 
261 #endif //USE_SIMD
262 
264 {
265  return m_resolveSingleConstraintRowGeneric(bodyA, bodyB, c);
266 }
267 
268 // Project Gauss Seidel or the equivalent Sequential Impulse
270 {
271  return m_resolveSingleConstraintRowGeneric(bodyA, bodyB, c);
272 }
273 
275 {
276  return m_resolveSingleConstraintRowLowerLimit(bodyA, bodyB, c);
277 }
278 
280 {
281  return m_resolveSingleConstraintRowLowerLimit(bodyA, bodyB, c);
282 }
283 
285  btSolverBody& bodyA,
286  btSolverBody& bodyB,
287  const btSolverConstraint& c)
288 {
289  btScalar deltaImpulse = 0.f;
290 
291  if (c.m_rhsPenetration)
292  {
294  deltaImpulse = c.m_rhsPenetration - btScalar(c.m_appliedPushImpulse) * c.m_cfm;
295  const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(bodyA.internalGetPushVelocity()) + c.m_relpos1CrossNormal.dot(bodyA.internalGetTurnVelocity());
296  const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(bodyB.internalGetPushVelocity()) + c.m_relpos2CrossNormal.dot(bodyB.internalGetTurnVelocity());
297 
298  deltaImpulse -= deltaVel1Dotn * c.m_jacDiagABInv;
299  deltaImpulse -= deltaVel2Dotn * c.m_jacDiagABInv;
300  const btScalar sum = btScalar(c.m_appliedPushImpulse) + deltaImpulse;
301  if (sum < c.m_lowerLimit)
302  {
303  deltaImpulse = c.m_lowerLimit - c.m_appliedPushImpulse;
304  c.m_appliedPushImpulse = c.m_lowerLimit;
305  }
306  else
307  {
308  c.m_appliedPushImpulse = sum;
309  }
310  bodyA.internalApplyPushImpulse(c.m_contactNormal1 * bodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
311  bodyB.internalApplyPushImpulse(c.m_contactNormal2 * bodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
312  }
313  return deltaImpulse * (1. / c.m_jacDiagABInv);
314 }
315 
317 {
318 #ifdef USE_SIMD
319  if (!c.m_rhsPenetration)
320  return 0.f;
321 
323 
324  __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedPushImpulse);
325  __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
326  __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
327  __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)));
328  __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128, bodyA.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128, bodyA.internalGetTurnVelocity().mVec128));
329  __m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128, bodyB.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128, bodyB.internalGetTurnVelocity().mVec128));
330  deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel1Dotn, _mm_set1_ps(c.m_jacDiagABInv)));
331  deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel2Dotn, _mm_set1_ps(c.m_jacDiagABInv)));
332  btSimdScalar sum = _mm_add_ps(cpAppliedImp, deltaImpulse);
333  btSimdScalar resultLowerLess, resultUpperLess;
334  resultLowerLess = _mm_cmplt_ps(sum, lowerLimit1);
335  resultUpperLess = _mm_cmplt_ps(sum, upperLimit1);
336  __m128 lowMinApplied = _mm_sub_ps(lowerLimit1, cpAppliedImp);
337  deltaImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse));
338  c.m_appliedPushImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum));
339  __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128);
340  __m128 linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128, bodyB.internalGetInvMass().mVec128);
341  __m128 impulseMagnitude = deltaImpulse;
342  bodyA.internalGetPushVelocity().mVec128 = _mm_add_ps(bodyA.internalGetPushVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude));
343  bodyA.internalGetTurnVelocity().mVec128 = _mm_add_ps(bodyA.internalGetTurnVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude));
344  bodyB.internalGetPushVelocity().mVec128 = _mm_add_ps(bodyB.internalGetPushVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude));
345  bodyB.internalGetTurnVelocity().mVec128 = _mm_add_ps(bodyB.internalGetTurnVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude));
346  btSimdScalar deltaImp = deltaImpulse;
347  return deltaImp.m_floats[0] * (1. / c.m_jacDiagABInv);
348 #else
350 #endif
351 }
352 
354 {
355  m_btSeed2 = 0;
356  m_cachedSolverMode = 0;
357  setupSolverFunctions(false);
358 }
359 
361 {
365 
366  if (useSimd)
367  {
368 #ifdef USE_SIMD
369  m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_sse2;
370  m_resolveSingleConstraintRowLowerLimit = gResolveSingleConstraintRowLowerLimit_sse2;
372 
373 #ifdef BT_ALLOW_SSE4
374  int cpuFeatures = btCpuFeatureUtility::getCpuFeatures();
376  {
377  m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_sse4_1_fma3;
378  m_resolveSingleConstraintRowLowerLimit = gResolveSingleConstraintRowLowerLimit_sse4_1_fma3;
379  }
380 #endif //BT_ALLOW_SSE4
381 #endif //USE_SIMD
382  }
383 }
384 
386 {
387 }
388 
390 {
392 }
393 
395 {
397 }
398 
399 #ifdef USE_SIMD
401 {
402  return gResolveSingleConstraintRowGeneric_sse2;
403 }
405 {
406  return gResolveSingleConstraintRowLowerLimit_sse2;
407 }
408 #ifdef BT_ALLOW_SSE4
410 {
411  return gResolveSingleConstraintRowGeneric_sse4_1_fma3;
412 }
414 {
415  return gResolveSingleConstraintRowLowerLimit_sse4_1_fma3;
416 }
417 #endif //BT_ALLOW_SSE4
418 #endif //USE_SIMD
419 
421 {
422  m_btSeed2 = (1664525L * m_btSeed2 + 1013904223L) & 0xffffffff;
423  return m_btSeed2;
424 }
425 
426 //See ODE: adam's all-int straightforward(?) dRandInt (0..n-1)
428 {
429  // seems good; xor-fold and modulus
430  const unsigned long un = static_cast<unsigned long>(n);
431  unsigned long r = btRand2();
432 
433  // note: probably more aggressive than it needs to be -- might be
434  // able to get away without one or two of the innermost branches.
435  if (un <= 0x00010000UL)
436  {
437  r ^= (r >> 16);
438  if (un <= 0x00000100UL)
439  {
440  r ^= (r >> 8);
441  if (un <= 0x00000010UL)
442  {
443  r ^= (r >> 4);
444  if (un <= 0x00000004UL)
445  {
446  r ^= (r >> 2);
447  if (un <= 0x00000002UL)
448  {
449  r ^= (r >> 1);
450  }
451  }
452  }
453  }
454  }
455 
456  return (int)(r % un);
457 }
458 
460 {
461  btRigidBody* rb = collisionObject ? btRigidBody::upcast(collisionObject) : 0;
462 
463  solverBody->internalGetDeltaLinearVelocity().setValue(0.f, 0.f, 0.f);
464  solverBody->internalGetDeltaAngularVelocity().setValue(0.f, 0.f, 0.f);
465  solverBody->internalGetPushVelocity().setValue(0.f, 0.f, 0.f);
466  solverBody->internalGetTurnVelocity().setValue(0.f, 0.f, 0.f);
467 
468  if (rb)
469  {
470  solverBody->m_worldTransform = rb->getWorldTransform();
471  solverBody->internalSetInvMass(btVector3(rb->getInvMass(), rb->getInvMass(), rb->getInvMass()) * rb->getLinearFactor());
472  solverBody->m_originalBody = rb;
473  solverBody->m_angularFactor = rb->getAngularFactor();
474  solverBody->m_linearFactor = rb->getLinearFactor();
475  solverBody->m_linearVelocity = rb->getLinearVelocity();
476  solverBody->m_angularVelocity = rb->getAngularVelocity();
477  solverBody->m_externalForceImpulse = rb->getTotalForce() * rb->getInvMass() * timeStep;
478  solverBody->m_externalTorqueImpulse = rb->getTotalTorque() * rb->getInvInertiaTensorWorld() * timeStep;
479  }
480  else
481  {
482  solverBody->m_worldTransform.setIdentity();
483  solverBody->internalSetInvMass(btVector3(0, 0, 0));
484  solverBody->m_originalBody = 0;
485  solverBody->m_angularFactor.setValue(1, 1, 1);
486  solverBody->m_linearFactor.setValue(1, 1, 1);
487  solverBody->m_linearVelocity.setValue(0, 0, 0);
488  solverBody->m_angularVelocity.setValue(0, 0, 0);
489  solverBody->m_externalForceImpulse.setValue(0, 0, 0);
490  solverBody->m_externalTorqueImpulse.setValue(0, 0, 0);
491  }
492  }
493 
495 {
496  //printf("rel_vel =%f\n", rel_vel);
497  if (btFabs(rel_vel) < velocityThreshold)
498  return 0.;
499 
500  btScalar rest = restitution * -rel_vel;
501  return rest;
502 }
503 
505 {
506  if (colObj && colObj->hasAnisotropicFriction(frictionMode))
507  {
508  // transform to local coordinates
509  btVector3 loc_lateral = frictionDirection * colObj->getWorldTransform().getBasis();
510  const btVector3& friction_scaling = colObj->getAnisotropicFriction();
511  //apply anisotropic friction
512  loc_lateral *= friction_scaling;
513  // ... and transform it back to global coordinates
514  frictionDirection = colObj->getWorldTransform().getBasis() * loc_lateral;
515  }
516 }
517 
518 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)
519 {
520  btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
521  btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
522 
523  btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody;
524  btRigidBody* bodyA = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody;
525 
526  solverConstraint.m_solverBodyIdA = solverBodyIdA;
527  solverConstraint.m_solverBodyIdB = solverBodyIdB;
528 
529  solverConstraint.m_friction = cp.m_combinedFriction;
530  solverConstraint.m_originalContactPoint = 0;
531 
532  solverConstraint.m_appliedImpulse = 0.f;
533  solverConstraint.m_appliedPushImpulse = 0.f;
534 
535  if (body0)
536  {
537  solverConstraint.m_contactNormal1 = normalAxis;
538  btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal1);
539  solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
540  solverConstraint.m_angularComponentA = body0->getInvInertiaTensorWorld() * ftorqueAxis1 * body0->getAngularFactor();
541  }
542  else
543  {
544  solverConstraint.m_contactNormal1.setZero();
545  solverConstraint.m_relpos1CrossNormal.setZero();
546  solverConstraint.m_angularComponentA.setZero();
547  }
548 
549  if (bodyA)
550  {
551  solverConstraint.m_contactNormal2 = -normalAxis;
552  btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal2);
553  solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
554  solverConstraint.m_angularComponentB = bodyA->getInvInertiaTensorWorld() * ftorqueAxis1 * bodyA->getAngularFactor();
555  }
556  else
557  {
558  solverConstraint.m_contactNormal2.setZero();
559  solverConstraint.m_relpos2CrossNormal.setZero();
560  solverConstraint.m_angularComponentB.setZero();
561  }
562 
563  {
564  btVector3 vec;
565  btScalar denom0 = 0.f;
566  btScalar denom1 = 0.f;
567  if (body0)
568  {
569  vec = (solverConstraint.m_angularComponentA).cross(rel_pos1);
570  denom0 = body0->getInvMass() + normalAxis.dot(vec);
571  }
572  if (bodyA)
573  {
574  vec = (-solverConstraint.m_angularComponentB).cross(rel_pos2);
575  denom1 = bodyA->getInvMass() + normalAxis.dot(vec);
576  }
577  btScalar denom = relaxation / (denom0 + denom1);
578  solverConstraint.m_jacDiagABInv = denom;
579  }
580 
581  {
582  btScalar rel_vel;
583  btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0 ? solverBodyA.m_linearVelocity + solverBodyA.m_externalForceImpulse : btVector3(0, 0, 0)) + solverConstraint.m_relpos1CrossNormal.dot(body0 ? solverBodyA.m_angularVelocity : btVector3(0, 0, 0));
584  btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(bodyA ? solverBodyB.m_linearVelocity + solverBodyB.m_externalForceImpulse : btVector3(0, 0, 0)) + solverConstraint.m_relpos2CrossNormal.dot(bodyA ? solverBodyB.m_angularVelocity : btVector3(0, 0, 0));
585 
586  rel_vel = vel1Dotn + vel2Dotn;
587 
588  // btScalar positionalError = 0.f;
589 
590  btScalar velocityError = desiredVelocity - rel_vel;
591  btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
592 
593  btScalar penetrationImpulse = btScalar(0);
594 
596  {
597  btScalar distance = (cp.getPositionWorldOnA() - cp.getPositionWorldOnB()).dot(normalAxis);
599  penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv;
600  }
601 
602  solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
603  solverConstraint.m_rhsPenetration = 0.f;
604  solverConstraint.m_cfm = cfmSlip;
605  solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
606  solverConstraint.m_upperLimit = solverConstraint.m_friction;
607  }
608 }
609 
610 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)
611 {
613  solverConstraint.m_frictionIndex = frictionIndex;
614  setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2,
615  colObj0, colObj1, relaxation, infoGlobal, desiredVelocity, cfmSlip);
616  return solverConstraint;
617 }
618 
619 void btSequentialImpulseConstraintSolver::setupTorsionalFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis1, int solverBodyIdA, int solverBodyIdB,
620  btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2,
621  btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation,
622  btScalar desiredVelocity, btScalar cfmSlip)
623 
624 {
625  btVector3 normalAxis(0, 0, 0);
626 
627  solverConstraint.m_contactNormal1 = normalAxis;
628  solverConstraint.m_contactNormal2 = -normalAxis;
629  btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
630  btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
631 
632  btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody;
633  btRigidBody* bodyA = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody;
634 
635  solverConstraint.m_solverBodyIdA = solverBodyIdA;
636  solverConstraint.m_solverBodyIdB = solverBodyIdB;
637 
638  solverConstraint.m_friction = combinedTorsionalFriction;
639  solverConstraint.m_originalContactPoint = 0;
640 
641  solverConstraint.m_appliedImpulse = 0.f;
642  solverConstraint.m_appliedPushImpulse = 0.f;
643 
644  {
645  btVector3 ftorqueAxis1 = -normalAxis1;
646  solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
647  solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld() * ftorqueAxis1 * body0->getAngularFactor() : btVector3(0, 0, 0);
648  }
649  {
650  btVector3 ftorqueAxis1 = normalAxis1;
651  solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
652  solverConstraint.m_angularComponentB = bodyA ? bodyA->getInvInertiaTensorWorld() * ftorqueAxis1 * bodyA->getAngularFactor() : btVector3(0, 0, 0);
653  }
654 
655  {
656  btVector3 iMJaA = body0 ? body0->getInvInertiaTensorWorld() * solverConstraint.m_relpos1CrossNormal : btVector3(0, 0, 0);
657  btVector3 iMJaB = bodyA ? bodyA->getInvInertiaTensorWorld() * solverConstraint.m_relpos2CrossNormal : btVector3(0, 0, 0);
658  btScalar sum = 0;
659  sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
660  sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
661  solverConstraint.m_jacDiagABInv = btScalar(1.) / sum;
662  }
663 
664  {
665  btScalar rel_vel;
666  btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0 ? solverBodyA.m_linearVelocity + solverBodyA.m_externalForceImpulse : btVector3(0, 0, 0)) + solverConstraint.m_relpos1CrossNormal.dot(body0 ? solverBodyA.m_angularVelocity : btVector3(0, 0, 0));
667  btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(bodyA ? solverBodyB.m_linearVelocity + solverBodyB.m_externalForceImpulse : btVector3(0, 0, 0)) + solverConstraint.m_relpos2CrossNormal.dot(bodyA ? solverBodyB.m_angularVelocity : btVector3(0, 0, 0));
668 
669  rel_vel = vel1Dotn + vel2Dotn;
670 
671  // btScalar positionalError = 0.f;
672 
673  btSimdScalar velocityError = desiredVelocity - rel_vel;
674  btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
675  solverConstraint.m_rhs = velocityImpulse;
676  solverConstraint.m_cfm = cfmSlip;
677  solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
678  solverConstraint.m_upperLimit = solverConstraint.m_friction;
679  }
680 }
681 
682 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)
683 {
685  solverConstraint.m_frictionIndex = frictionIndex;
686  setupTorsionalFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, combinedTorsionalFriction, rel_pos1, rel_pos2,
687  colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
688  return solverConstraint;
689 }
690 
692 {
693 #if BT_THREADSAFE
694  int solverBodyId = -1;
695  const bool isRigidBodyType = btRigidBody::upcast(&body) != NULL;
696  const bool isStaticOrKinematic = body.isStaticOrKinematicObject();
697  const bool isKinematic = body.isKinematicObject();
698  if (isRigidBodyType && !isStaticOrKinematic)
699  {
700  // dynamic body
701  // Dynamic bodies can only be in one island, so it's safe to write to the companionId
702  solverBodyId = body.getCompanionId();
703  if (solverBodyId < 0)
704  {
705  solverBodyId = m_tmpSolverBodyPool.size();
706  btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
707  initSolverBody(&solverBody, &body, timeStep);
708  body.setCompanionId(solverBodyId);
709  }
710  }
711  else if (isRigidBodyType && isKinematic)
712  {
713  //
714  // NOTE: must test for kinematic before static because some kinematic objects also
715  // identify as "static"
716  //
717  // Kinematic bodies can be in multiple islands at once, so it is a
718  // race condition to write to them, so we use an alternate method
719  // to record the solverBodyId
720  int uniqueId = body.getWorldArrayIndex();
721  const int INVALID_SOLVER_BODY_ID = -1;
723  {
724  m_kinematicBodyUniqueIdToSolverBodyTable.resize(uniqueId + 1, INVALID_SOLVER_BODY_ID);
725  }
727  // if no table entry yet,
728  if (solverBodyId == INVALID_SOLVER_BODY_ID)
729  {
730  // create a table entry for this body
731  solverBodyId = m_tmpSolverBodyPool.size();
732  btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
733  initSolverBody(&solverBody, &body, timeStep);
735  }
736  }
737  else
738  {
739  bool isMultiBodyType = (body.getInternalType() & btCollisionObject::CO_FEATHERSTONE_LINK);
740  // Incorrectly set collision object flags can degrade performance in various ways.
741  if (!isMultiBodyType)
742  {
743  btAssert(body.isStaticOrKinematicObject());
744  }
745  //it could be a multibody link collider
746  // all fixed bodies (inf mass) get mapped to a single solver id
747  if (m_fixedBodyId < 0)
748  {
749  m_fixedBodyId = m_tmpSolverBodyPool.size();
750  btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
751  initSolverBody(&fixedBody, 0, timeStep);
752  }
753  solverBodyId = m_fixedBodyId;
754  }
755  btAssert(solverBodyId >= 0 && solverBodyId < m_tmpSolverBodyPool.size());
756  return solverBodyId;
757 #else // BT_THREADSAFE
758 
759  int solverBodyIdA = -1;
760 
761  if (body.getCompanionId() >= 0)
762  {
763  //body has already been converted
764  solverBodyIdA = body.getCompanionId();
765  btAssert(solverBodyIdA < m_tmpSolverBodyPool.size());
766  }
767  else
768  {
769  btRigidBody* rb = btRigidBody::upcast(&body);
770  //convert both active and kinematic objects (for their velocity)
771  if (rb && (rb->getInvMass() || rb->isKinematicObject()))
772  {
773  solverBodyIdA = m_tmpSolverBodyPool.size();
774  btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
775  initSolverBody(&solverBody, &body, timeStep);
776  body.setCompanionId(solverBodyIdA);
777  }
778  else
779  {
780  if (m_fixedBodyId < 0)
781  {
782  m_fixedBodyId = m_tmpSolverBodyPool.size();
783  btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
784  initSolverBody(&fixedBody, 0, timeStep);
785  }
786  return m_fixedBodyId;
787  // return 0;//assume first one is a fixed solver body
788  }
789  }
790 
791  return solverBodyIdA;
792 #endif // BT_THREADSAFE
793 }
794 #include <stdio.h>
795 
797  int solverBodyIdA, int solverBodyIdB,
799  btScalar& relaxation,
800  const btVector3& rel_pos1, const btVector3& rel_pos2)
801 {
802  // const btVector3& pos1 = cp.getPositionWorldOnA();
803  // const btVector3& pos2 = cp.getPositionWorldOnB();
804 
805  btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA];
806  btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB];
807 
808  btRigidBody* rb0 = bodyA->m_originalBody;
809  btRigidBody* rb1 = bodyB->m_originalBody;
810 
811  // btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
812  // btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
813  //rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
814  //rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
815 
816  relaxation = infoGlobal.m_sor;
817  btScalar invTimeStep = btScalar(1) / infoGlobal.m_timeStep;
818 
819  //cfm = 1 / ( dt * kp + kd )
820  //erp = dt * kp / ( dt * kp + kd )
821 
823  btScalar erp = infoGlobal.m_erp2;
824 
826  {
828  cfm = cp.m_contactCFM;
830  erp = cp.m_contactERP;
831  }
832  else
833  {
835  {
837  if (denom < SIMD_EPSILON)
838  {
839  denom = SIMD_EPSILON;
840  }
841  cfm = btScalar(1) / denom;
843  }
844  }
845 
846  cfm *= invTimeStep;
847 
848  btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
849  solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld() * torqueAxis0 * rb0->getAngularFactor() : btVector3(0, 0, 0);
850  btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);
851  solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld() * -torqueAxis1 * rb1->getAngularFactor() : btVector3(0, 0, 0);
852 
853  {
854 #ifdef COMPUTE_IMPULSE_DENOM
855  btScalar denom0 = rb0->computeImpulseDenominator(pos1, cp.m_normalWorldOnB);
856  btScalar denom1 = rb1->computeImpulseDenominator(pos2, cp.m_normalWorldOnB);
857 #else
858  btVector3 vec;
859  btScalar denom0 = 0.f;
860  btScalar denom1 = 0.f;
861  if (rb0)
862  {
863  vec = (solverConstraint.m_angularComponentA).cross(rel_pos1);
864  denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
865  }
866  if (rb1)
867  {
868  vec = (-solverConstraint.m_angularComponentB).cross(rel_pos2);
869  denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
870  }
871 #endif //COMPUTE_IMPULSE_DENOM
872 
873  btScalar denom = relaxation / (denom0 + denom1 + cfm);
874  solverConstraint.m_jacDiagABInv = denom;
875  }
876 
877  if (rb0)
878  {
879  solverConstraint.m_contactNormal1 = cp.m_normalWorldOnB;
880  solverConstraint.m_relpos1CrossNormal = torqueAxis0;
881  }
882  else
883  {
884  solverConstraint.m_contactNormal1.setZero();
885  solverConstraint.m_relpos1CrossNormal.setZero();
886  }
887  if (rb1)
888  {
889  solverConstraint.m_contactNormal2 = -cp.m_normalWorldOnB;
890  solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
891  }
892  else
893  {
894  solverConstraint.m_contactNormal2.setZero();
895  solverConstraint.m_relpos2CrossNormal.setZero();
896  }
897 
898  btScalar restitution = 0.f;
899  btScalar penetration = cp.getDistance() + infoGlobal.m_linearSlop;
900 
901  {
902  btVector3 vel1, vel2;
903 
904  vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0, 0, 0);
905  vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0, 0, 0);
906 
907  // btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
908  btVector3 vel = vel1 - vel2;
909  btScalar rel_vel = cp.m_normalWorldOnB.dot(vel);
910 
911  solverConstraint.m_friction = cp.m_combinedFriction;
912 
914  if (restitution <= btScalar(0.))
915  {
916  restitution = 0.f;
917  };
918  }
919 
922  {
923  solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
924  if (rb0)
925  bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1 * bodyA->internalGetInvMass(), solverConstraint.m_angularComponentA, solverConstraint.m_appliedImpulse);
926  if (rb1)
927  bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2 * bodyB->internalGetInvMass(), -solverConstraint.m_angularComponentB, -(btScalar)solverConstraint.m_appliedImpulse);
928  }
929  else
930  {
931  solverConstraint.m_appliedImpulse = 0.f;
932  }
933 
934  solverConstraint.m_appliedPushImpulse = 0.f;
935 
936  {
937  btVector3 externalForceImpulseA = bodyA->m_originalBody ? bodyA->m_externalForceImpulse : btVector3(0, 0, 0);
938  btVector3 externalTorqueImpulseA = bodyA->m_originalBody ? bodyA->m_externalTorqueImpulse : btVector3(0, 0, 0);
939  btVector3 externalForceImpulseB = bodyB->m_originalBody ? bodyB->m_externalForceImpulse : btVector3(0, 0, 0);
940  btVector3 externalTorqueImpulseB = bodyB->m_originalBody ? bodyB->m_externalTorqueImpulse : btVector3(0, 0, 0);
941 
942  btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(bodyA->m_linearVelocity + externalForceImpulseA) + solverConstraint.m_relpos1CrossNormal.dot(bodyA->m_angularVelocity + externalTorqueImpulseA);
943  btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(bodyB->m_linearVelocity + externalForceImpulseB) + solverConstraint.m_relpos2CrossNormal.dot(bodyB->m_angularVelocity + externalTorqueImpulseB);
944  btScalar rel_vel = vel1Dotn + vel2Dotn;
945 
946  btScalar positionalError = 0.f;
947  btScalar velocityError = restitution - rel_vel; // * damping;
948 
949  if (penetration > 0)
950  {
951  positionalError = 0;
952 
953  velocityError -= penetration * invTimeStep;
954  }
955  else
956  {
957  positionalError = -penetration * erp * invTimeStep;
958  }
959 
960  btScalar penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv;
961  btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
962 
964  {
965  //combine position and velocity into rhs
966  solverConstraint.m_rhs = penetrationImpulse + velocityImpulse; //-solverConstraint.m_contactNormal1.dot(bodyA->m_externalForce*bodyA->m_invMass-bodyB->m_externalForce/bodyB->m_invMass)*solverConstraint.m_jacDiagABInv;
967  solverConstraint.m_rhsPenetration = 0.f;
968  }
969  else
970  {
971  //split position and velocity into rhs and m_rhsPenetration
972  solverConstraint.m_rhs = velocityImpulse;
973  solverConstraint.m_rhsPenetration = penetrationImpulse;
974  }
975  solverConstraint.m_cfm = cfm * solverConstraint.m_jacDiagABInv;
976  solverConstraint.m_lowerLimit = 0;
977  solverConstraint.m_upperLimit = 1e10f;
978  }
979 }
980 
982  int solverBodyIdA, int solverBodyIdB,
984 {
985  {
986  btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
987 
988  frictionConstraint1.m_appliedImpulse = 0.f;
989  }
990 
992  {
993  btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex + 1];
994 
995  frictionConstraint2.m_appliedImpulse = 0.f;
996  }
997 }
998 
1000 {
1001  btCollisionObject *colObj0 = 0, *colObj1 = 0;
1002 
1003  colObj0 = (btCollisionObject*)manifold->getBody0();
1004  colObj1 = (btCollisionObject*)manifold->getBody1();
1005 
1006  int solverBodyIdA = getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep);
1007  int solverBodyIdB = getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep);
1008 
1009  // btRigidBody* bodyA = btRigidBody::upcast(colObj0);
1010  // btRigidBody* bodyB = btRigidBody::upcast(colObj1);
1011 
1012  btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverBodyIdA];
1013  btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverBodyIdB];
1014 
1016  if (!solverBodyA || (solverBodyA->m_invMass.fuzzyZero() && (!solverBodyB || solverBodyB->m_invMass.fuzzyZero())))
1017  return;
1018 
1019  int rollingFriction = 1;
1020  for (int j = 0; j < manifold->getNumContacts(); j++)
1021  {
1022  btManifoldPoint& cp = manifold->getContactPoint(j);
1023 
1024  if (cp.getDistance() <= manifold->getContactProcessingThreshold())
1025  {
1026  btVector3 rel_pos1;
1027  btVector3 rel_pos2;
1028  btScalar relaxation;
1029 
1030  int frictionIndex = m_tmpSolverContactConstraintPool.size();
1032  solverConstraint.m_solverBodyIdA = solverBodyIdA;
1033  solverConstraint.m_solverBodyIdB = solverBodyIdB;
1034 
1035  solverConstraint.m_originalContactPoint = &cp;
1036 
1037  const btVector3& pos1 = cp.getPositionWorldOnA();
1038  const btVector3& pos2 = cp.getPositionWorldOnB();
1039 
1040  rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
1041  rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
1042 
1043  btVector3 vel1;
1044  btVector3 vel2;
1045 
1046  solverBodyA->getVelocityInLocalPointNoDelta(rel_pos1, vel1);
1047  solverBodyB->getVelocityInLocalPointNoDelta(rel_pos2, vel2);
1048 
1049  btVector3 vel = vel1 - vel2;
1050  btScalar rel_vel = cp.m_normalWorldOnB.dot(vel);
1051 
1052  setupContactConstraint(solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, relaxation, rel_pos1, rel_pos2);
1053 
1055 
1056  solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
1057 
1058  if ((cp.m_combinedRollingFriction > 0.f) && (rollingFriction > 0))
1059  {
1060  {
1061  addTorsionalFrictionConstraint(cp.m_normalWorldOnB, solverBodyIdA, solverBodyIdB, frictionIndex, cp, cp.m_combinedSpinningFriction, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
1062  btVector3 axis0, axis1;
1063  btPlaneSpace1(cp.m_normalWorldOnB, axis0, axis1);
1064  axis0.normalize();
1065  axis1.normalize();
1066 
1071  if (axis0.length() > 0.001)
1072  addTorsionalFrictionConstraint(axis0, solverBodyIdA, solverBodyIdB, frictionIndex, cp,
1073  cp.m_combinedRollingFriction, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
1074  if (axis1.length() > 0.001)
1075  addTorsionalFrictionConstraint(axis1, solverBodyIdA, solverBodyIdB, frictionIndex, cp,
1076  cp.m_combinedRollingFriction, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
1077  }
1078  }
1079 
1084 
1095 
1097  {
1098  cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
1099  btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
1101  {
1102  cp.m_lateralFrictionDir1 *= 1.f / btSqrt(lat_rel_vel);
1105  addFrictionConstraint(cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
1106 
1108  {
1110  cp.m_lateralFrictionDir2.normalize(); //??
1113  addFrictionConstraint(cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
1114  }
1115  }
1116  else
1117  {
1119 
1122  addFrictionConstraint(cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
1123 
1125  {
1128  addFrictionConstraint(cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
1129  }
1130 
1132  {
1134  }
1135  }
1136  }
1137  else
1138  {
1139  addFrictionConstraint(cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM);
1140 
1142  addFrictionConstraint(cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM);
1143  }
1144  setFrictionConstraintImpulse(solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
1145  }
1146  }
1147  }
1148 
1150 {
1151  int i;
1152  btPersistentManifold* manifold = 0;
1153  // btCollisionObject* colObj0=0,*colObj1=0;
1154 
1155  for (i = 0; i < numManifolds; i++)
1156  {
1157  manifold = manifoldPtr[i];
1158  convertContact(manifold, infoGlobal);
1159  }
1160 }
1161 
1163  btTypedConstraint* constraint,
1164  const btTypedConstraint::btConstraintInfo1& info1,
1165  int solverBodyIdA,
1166  int solverBodyIdB,
1168 {
1169  const btRigidBody& rbA = constraint->getRigidBodyA();
1170  const btRigidBody& rbB = constraint->getRigidBodyB();
1171 
1172  const btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
1173  const btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
1174 
1175  int overrideNumSolverIterations = constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations;
1176  if (overrideNumSolverIterations > m_maxOverrideNumSolverIterations)
1177  m_maxOverrideNumSolverIterations = overrideNumSolverIterations;
1178 
1179  for (int j = 0; j < info1.m_numConstraintRows; j++)
1180  {
1181  memset(&currentConstraintRow[j], 0, sizeof(btSolverConstraint));
1182  currentConstraintRow[j].m_lowerLimit = -SIMD_INFINITY;
1183  currentConstraintRow[j].m_upperLimit = SIMD_INFINITY;
1184  currentConstraintRow[j].m_appliedImpulse = 0.f;
1185  currentConstraintRow[j].m_appliedPushImpulse = 0.f;
1186  currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA;
1187  currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB;
1188  currentConstraintRow[j].m_overrideNumSolverIterations = overrideNumSolverIterations;
1189  }
1190 
1191  // these vectors are already cleared in initSolverBody, no need to redundantly clear again
1192  btAssert(bodyAPtr->getDeltaLinearVelocity().isZero());
1193  btAssert(bodyAPtr->getDeltaAngularVelocity().isZero());
1194  btAssert(bodyAPtr->getPushVelocity().isZero());
1195  btAssert(bodyAPtr->getTurnVelocity().isZero());
1196  btAssert(bodyBPtr->getDeltaLinearVelocity().isZero());
1197  btAssert(bodyBPtr->getDeltaAngularVelocity().isZero());
1198  btAssert(bodyBPtr->getPushVelocity().isZero());
1199  btAssert(bodyBPtr->getTurnVelocity().isZero());
1200  //bodyAPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
1201  //bodyAPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
1202  //bodyAPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f);
1203  //bodyAPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
1204  //bodyBPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
1205  //bodyBPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
1206  //bodyBPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f);
1207  //bodyBPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
1208 
1209  btTypedConstraint::btConstraintInfo2 info2;
1210  info2.fps = 1.f / infoGlobal.m_timeStep;
1211  info2.erp = infoGlobal.m_erp;
1212  info2.m_J1linearAxis = currentConstraintRow->m_contactNormal1;
1213  info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal;
1214  info2.m_J2linearAxis = currentConstraintRow->m_contactNormal2;
1215  info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal;
1216  info2.rowskip = sizeof(btSolverConstraint) / sizeof(btScalar); //check this
1218  btAssert(info2.rowskip * sizeof(btScalar) == sizeof(btSolverConstraint));
1219  info2.m_constraintError = &currentConstraintRow->m_rhs;
1220  currentConstraintRow->m_cfm = infoGlobal.m_globalCfm;
1221  info2.m_damping = infoGlobal.m_damping;
1222  info2.cfm = &currentConstraintRow->m_cfm;
1223  info2.m_lowerLimit = &currentConstraintRow->m_lowerLimit;
1224  info2.m_upperLimit = &currentConstraintRow->m_upperLimit;
1225  info2.m_numIterations = infoGlobal.m_numIterations;
1226  constraint->getInfo2(&info2);
1227 
1229  for (int j = 0; j < info1.m_numConstraintRows; j++)
1230  {
1231  btSolverConstraint& solverConstraint = currentConstraintRow[j];
1232 
1233  if (solverConstraint.m_upperLimit >= constraint->getBreakingImpulseThreshold())
1234  {
1235  solverConstraint.m_upperLimit = constraint->getBreakingImpulseThreshold();
1236  }
1237 
1238  if (solverConstraint.m_lowerLimit <= -constraint->getBreakingImpulseThreshold())
1239  {
1240  solverConstraint.m_lowerLimit = -constraint->getBreakingImpulseThreshold();
1241  }
1242 
1243  solverConstraint.m_originalContactPoint = constraint;
1244 
1245  {
1246  const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
1247  solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld() * ftorqueAxis1 * constraint->getRigidBodyA().getAngularFactor();
1248  }
1249  {
1250  const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal;
1251  solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld() * ftorqueAxis2 * constraint->getRigidBodyB().getAngularFactor();
1252  }
1253 
1254  {
1255  btVector3 iMJlA = solverConstraint.m_contactNormal1 * rbA.getInvMass();
1256  btVector3 iMJaA = rbA.getInvInertiaTensorWorld() * solverConstraint.m_relpos1CrossNormal;
1257  btVector3 iMJlB = solverConstraint.m_contactNormal2 * rbB.getInvMass(); //sign of normal?
1258  btVector3 iMJaB = rbB.getInvInertiaTensorWorld() * solverConstraint.m_relpos2CrossNormal;
1259 
1260  btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal1);
1261  sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
1262  sum += iMJlB.dot(solverConstraint.m_contactNormal2);
1263  sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
1264  btScalar fsum = btFabs(sum);
1265  btAssert(fsum > SIMD_EPSILON);
1266  btScalar sorRelaxation = 1.f; //todo: get from globalInfo?
1267  solverConstraint.m_jacDiagABInv = fsum > SIMD_EPSILON ? sorRelaxation / sum : 0.f;
1268  }
1269 
1270  {
1271  btScalar rel_vel;
1272  btVector3 externalForceImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalForceImpulse : btVector3(0, 0, 0);
1273  btVector3 externalTorqueImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalTorqueImpulse : btVector3(0, 0, 0);
1274 
1275  btVector3 externalForceImpulseB = bodyBPtr->m_originalBody ? bodyBPtr->m_externalForceImpulse : btVector3(0, 0, 0);
1276  btVector3 externalTorqueImpulseB = bodyBPtr->m_originalBody ? bodyBPtr->m_externalTorqueImpulse : btVector3(0, 0, 0);
1277 
1278  btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(rbA.getLinearVelocity() + externalForceImpulseA) + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity() + externalTorqueImpulseA);
1279 
1280  btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(rbB.getLinearVelocity() + externalForceImpulseB) + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity() + externalTorqueImpulseB);
1281 
1282  rel_vel = vel1Dotn + vel2Dotn;
1283  btScalar restitution = 0.f;
1284  btScalar positionalError = solverConstraint.m_rhs; //already filled in by getConstraintInfo2
1285  btScalar velocityError = restitution - rel_vel * info2.m_damping;
1286  btScalar penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv;
1287  btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
1288  solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
1289  solverConstraint.m_appliedImpulse = 0.f;
1290  }
1291  }
1292 }
1293 
1295 {
1296  BT_PROFILE("convertJoints");
1297  for (int j = 0; j < numConstraints; j++)
1298  {
1299  btTypedConstraint* constraint = constraints[j];
1300  constraint->buildJacobian();
1301  constraint->internalSetAppliedImpulse(0.0f);
1302  }
1303 
1304  int totalNumRows = 0;
1305 
1306  m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints);
1307  //calculate the total number of contraint rows
1308  for (int i = 0; i < numConstraints; i++)
1309  {
1310  btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
1311  btJointFeedback* fb = constraints[i]->getJointFeedback();
1312  if (fb)
1313  {
1314  fb->m_appliedForceBodyA.setZero();
1315  fb->m_appliedTorqueBodyA.setZero();
1316  fb->m_appliedForceBodyB.setZero();
1317  fb->m_appliedTorqueBodyB.setZero();
1318  }
1319 
1320  if (constraints[i]->isEnabled())
1321  {
1322  constraints[i]->getInfo1(&info1);
1323  }
1324  else
1325  {
1326  info1.m_numConstraintRows = 0;
1327  info1.nub = 0;
1328  }
1329  totalNumRows += info1.m_numConstraintRows;
1330  }
1332 
1334  int currentRow = 0;
1335 
1336  for (int i = 0; i < numConstraints; i++)
1337  {
1338  const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
1339 
1340  if (info1.m_numConstraintRows)
1341  {
1342  btAssert(currentRow < totalNumRows);
1343 
1344  btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];
1345  btTypedConstraint* constraint = constraints[i];
1346  btRigidBody& rbA = constraint->getRigidBodyA();
1347  btRigidBody& rbB = constraint->getRigidBodyB();
1348 
1349  int solverBodyIdA = getOrInitSolverBody(rbA, infoGlobal.m_timeStep);
1350  int solverBodyIdB = getOrInitSolverBody(rbB, infoGlobal.m_timeStep);
1351 
1352  convertJoint(currentConstraintRow, constraint, info1, solverBodyIdA, solverBodyIdB, infoGlobal);
1353  }
1354  currentRow += info1.m_numConstraintRows;
1355  }
1356 }
1357 
1359 {
1360  BT_PROFILE("convertBodies");
1361  for (int i = 0; i < numBodies; i++)
1362  {
1363  bodies[i]->setCompanionId(-1);
1364  }
1365 #if BT_THREADSAFE
1367 #endif // BT_THREADSAFE
1368 
1369  m_tmpSolverBodyPool.reserve(numBodies + 1);
1370  m_tmpSolverBodyPool.resize(0);
1371 
1372  //btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
1373  //initSolverBody(&fixedBody,0);
1374 
1375  for (int i = 0; i < numBodies; i++)
1376  {
1377  int bodyId = getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep);
1378 
1379  btRigidBody* body = btRigidBody::upcast(bodies[i]);
1380  if (body && body->getInvMass())
1381  {
1382  btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
1383  btVector3 gyroForce(0, 0, 0);
1385  {
1387  solverBody.m_externalTorqueImpulse -= gyroForce * body->getInvInertiaTensorWorld() * infoGlobal.m_timeStep;
1388  }
1390  {
1392  solverBody.m_externalTorqueImpulse += gyroForce;
1393  }
1395  {
1397  solverBody.m_externalTorqueImpulse += gyroForce;
1398  }
1399  }
1400  }
1401 }
1402 
1404 {
1405  m_fixedBodyId = -1;
1406  BT_PROFILE("solveGroupCacheFriendlySetup");
1407  (void)debugDrawer;
1408 
1409  // if solver mode has changed,
1411  {
1412  // update solver functions to use SIMD or non-SIMD
1413  bool useSimd = !!(infoGlobal.m_solverMode & SOLVER_SIMD);
1414  setupSolverFunctions(useSimd);
1416  }
1418 
1419 #ifdef BT_ADDITIONAL_DEBUG
1420  //make sure that dynamic bodies exist for all (enabled) constraints
1421  for (int i = 0; i < numConstraints; i++)
1422  {
1423  btTypedConstraint* constraint = constraints[i];
1424  if (constraint->isEnabled())
1425  {
1426  if (!constraint->getRigidBodyA().isStaticOrKinematicObject())
1427  {
1428  bool found = false;
1429  for (int b = 0; b < numBodies; b++)
1430  {
1431  if (&constraint->getRigidBodyA() == bodies[b])
1432  {
1433  found = true;
1434  break;
1435  }
1436  }
1437  btAssert(found);
1438  }
1439  if (!constraint->getRigidBodyB().isStaticOrKinematicObject())
1440  {
1441  bool found = false;
1442  for (int b = 0; b < numBodies; b++)
1443  {
1444  if (&constraint->getRigidBodyB() == bodies[b])
1445  {
1446  found = true;
1447  break;
1448  }
1449  }
1450  btAssert(found);
1451  }
1452  }
1453  }
1454  //make sure that dynamic bodies exist for all contact manifolds
1455  for (int i = 0; i < numManifolds; i++)
1456  {
1458  {
1459  bool found = false;
1460  for (int b = 0; b < numBodies; b++)
1461  {
1462  if (manifoldPtr[i]->getBody0() == bodies[b])
1463  {
1464  found = true;
1465  break;
1466  }
1467  }
1468  btAssert(found);
1469  }
1471  {
1472  bool found = false;
1473  for (int b = 0; b < numBodies; b++)
1474  {
1475  if (manifoldPtr[i]->getBody1() == bodies[b])
1476  {
1477  found = true;
1478  break;
1479  }
1480  }
1481  btAssert(found);
1482  }
1483  }
1484 #endif //BT_ADDITIONAL_DEBUG
1485 
1486  //convert all bodies
1488 
1490 
1492 
1493  // btContactSolverInfo info = infoGlobal;
1494 
1495  int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
1496  int numConstraintPool = m_tmpSolverContactConstraintPool.size();
1497  int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
1498 
1502  m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool * 2);
1503  else
1504  m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool);
1505 
1507  {
1508  int i;
1509  for (i = 0; i < numNonContactPool; i++)
1510  {
1512  }
1513  for (i = 0; i < numConstraintPool; i++)
1514  {
1515  m_orderTmpConstraintPool[i] = i;
1516  }
1517  for (i = 0; i < numFrictionPool; i++)
1518  {
1520  }
1521  }
1522 
1523  return 0.f;
1524 }
1525 
1526 btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */, int /*numBodies*/, btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* /*debugDrawer*/)
1527 {
1528  BT_PROFILE("solveSingleIteration");
1529  btScalar leastSquaresResidual = 0.f;
1530 
1531  int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
1532  int numConstraintPool = m_tmpSolverContactConstraintPool.size();
1533  int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
1534 
1536  {
1537  if (1) // uncomment this for a bit less random ((iteration & 7) == 0)
1538  {
1539  for (int j = 0; j < numNonContactPool; ++j)
1540  {
1541  int tmp = m_orderNonContactConstraintPool[j];
1542  int swapi = btRandInt2(j + 1);
1544  m_orderNonContactConstraintPool[swapi] = tmp;
1545  }
1546 
1547  //contact/friction constraints are not solved more than
1548  if (iteration < infoGlobal.m_numIterations)
1549  {
1550  for (int j = 0; j < numConstraintPool; ++j)
1551  {
1552  int tmp = m_orderTmpConstraintPool[j];
1553  int swapi = btRandInt2(j + 1);
1555  m_orderTmpConstraintPool[swapi] = tmp;
1556  }
1557 
1558  for (int j = 0; j < numFrictionPool; ++j)
1559  {
1560  int tmp = m_orderFrictionConstraintPool[j];
1561  int swapi = btRandInt2(j + 1);
1563  m_orderFrictionConstraintPool[swapi] = tmp;
1564  }
1565  }
1566  }
1567  }
1568 
1570  for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++)
1571  {
1573  if (iteration < constraint.m_overrideNumSolverIterations)
1574  {
1575  btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA], m_tmpSolverBodyPool[constraint.m_solverBodyIdB], constraint);
1576  leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
1577  }
1578  }
1579 
1580  if (iteration < infoGlobal.m_numIterations)
1581  {
1582  for (int j = 0; j < numConstraints; j++)
1583  {
1584  if (constraints[j]->isEnabled())
1585  {
1588  btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
1589  btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
1590  constraints[j]->solveConstraintObsolete(bodyA, bodyB, infoGlobal.m_timeStep);
1591  }
1592  }
1593 
1596  {
1597  int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1598  int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) ? 2 : 1;
1599 
1600  for (int c = 0; c < numPoolConstraints; c++)
1601  {
1602  btScalar totalImpulse = 0;
1603 
1604  {
1606  btScalar residual = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
1607  leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
1608 
1609  totalImpulse = solveManifold.m_appliedImpulse;
1610  }
1611  bool applyFriction = true;
1612  if (applyFriction)
1613  {
1614  {
1616 
1617  if (totalImpulse > btScalar(0))
1618  {
1619  solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
1620  solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
1621 
1622  btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
1623  leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
1624  }
1625  }
1626 
1628  {
1630 
1631  if (totalImpulse > btScalar(0))
1632  {
1633  solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
1634  solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
1635 
1636  btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
1637  leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
1638  }
1639  }
1640  }
1641  }
1642  }
1643  else //SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
1644  {
1645  //solve the friction constraints after all contact constraints, don't interleave them
1646  int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1647  int j;
1648 
1649  for (j = 0; j < numPoolConstraints; j++)
1650  {
1652  btScalar residual = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
1653  leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
1654  }
1655 
1657 
1658  int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
1659  for (j = 0; j < numFrictionPoolConstraints; j++)
1660  {
1662  btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
1663 
1664  if (totalImpulse > btScalar(0))
1665  {
1666  solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
1667  solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
1668 
1669  btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
1670  leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
1671  }
1672  }
1673  }
1674 
1675  int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
1676  for (int j = 0; j < numRollingFrictionPoolConstraints; j++)
1677  {
1679  btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
1680  if (totalImpulse > btScalar(0))
1681  {
1682  btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction * totalImpulse;
1683  if (rollingFrictionMagnitude > rollingFrictionConstraint.m_friction)
1684  rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
1685 
1686  rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
1687  rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
1688 
1689  btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA], m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB], rollingFrictionConstraint);
1690  leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
1691  }
1692  }
1693  }
1694  return leastSquaresResidual;
1695 }
1696 
1698 {
1699  BT_PROFILE("solveGroupCacheFriendlySplitImpulseIterations");
1700  int iteration;
1702  {
1703  {
1704  for (iteration = 0; iteration < infoGlobal.m_numIterations; iteration++)
1705  {
1706  btScalar leastSquaresResidual = 0.f;
1707  {
1708  int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1709  int j;
1710  for (j = 0; j < numPoolConstraints; j++)
1711  {
1713 
1714  btScalar residual = resolveSplitPenetrationImpulse(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
1715  leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
1716  }
1717  }
1718  if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration >= (infoGlobal.m_numIterations - 1))
1719  {
1720 #ifdef VERBOSE_RESIDUAL_PRINTF
1721  printf("residual = %f at iteration #%d\n", leastSquaresResidual, iteration);
1722 #endif
1723  break;
1724  }
1725  }
1726  }
1727  }
1728 }
1729 
1731 {
1732  BT_PROFILE("solveGroupCacheFriendlyIterations");
1733 
1734  {
1737 
1739 
1740  for (int iteration = 0; iteration < maxIterations; iteration++)
1741  //for ( int iteration = maxIterations-1 ; iteration >= 0;iteration--)
1742  {
1744 
1745  if (m_leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1)))
1746  {
1747 #ifdef VERBOSE_RESIDUAL_PRINTF
1748  printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration);
1749 #endif
1751  m_analyticsData.m_numIterationsUsed = iteration+1;
1753  if (numBodies>0)
1754  m_analyticsData.m_islandId = bodies[0]->getCompanionId();
1758  break;
1759  }
1760  }
1761  }
1762  return 0.f;
1763 }
1764 
1766 {
1767  for (int j = iBegin; j < iEnd; j++)
1768  {
1769  const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j];
1770  btManifoldPoint* pt = (btManifoldPoint*)solveManifold.m_originalContactPoint;
1771  btAssert(pt);
1772  pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
1773  // float f = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
1774  // printf("pt->m_appliedImpulseLateral1 = %f\n", f);
1775  pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
1776  //printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
1778  {
1779  pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex + 1].m_appliedImpulse;
1780  }
1781  //do a callback here?
1782  }
1783 }
1784 
1786 {
1787  for (int j = iBegin; j < iEnd; j++)
1788  {
1790  btTypedConstraint* constr = (btTypedConstraint*)solverConstr.m_originalContactPoint;
1791  btJointFeedback* fb = constr->getJointFeedback();
1792  if (fb)
1793  {
1794  fb->m_appliedForceBodyA += solverConstr.m_contactNormal1 * solverConstr.m_appliedImpulse * constr->getRigidBodyA().getLinearFactor() / infoGlobal.m_timeStep;
1795  fb->m_appliedForceBodyB += solverConstr.m_contactNormal2 * solverConstr.m_appliedImpulse * constr->getRigidBodyB().getLinearFactor() / infoGlobal.m_timeStep;
1796  fb->m_appliedTorqueBodyA += solverConstr.m_relpos1CrossNormal * constr->getRigidBodyA().getAngularFactor() * solverConstr.m_appliedImpulse / infoGlobal.m_timeStep;
1797  fb->m_appliedTorqueBodyB += solverConstr.m_relpos2CrossNormal * constr->getRigidBodyB().getAngularFactor() * solverConstr.m_appliedImpulse / infoGlobal.m_timeStep; /*RGM ???? */
1798  }
1799 
1800  constr->internalSetAppliedImpulse(solverConstr.m_appliedImpulse);
1801  if (btFabs(solverConstr.m_appliedImpulse) >= constr->getBreakingImpulseThreshold())
1802  {
1803  constr->setEnabled(false);
1804  }
1805  }
1806 }
1807 
1809 {
1810  for (int i = iBegin; i < iEnd; i++)
1811  {
1812  btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody;
1813  if (body)
1814  {
1816  m_tmpSolverBodyPool[i].writebackVelocityAndTransform(infoGlobal.m_timeStep, infoGlobal.m_splitImpulseTurnErp);
1817  else
1818  m_tmpSolverBodyPool[i].writebackVelocity();
1819 
1820  m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(
1821  m_tmpSolverBodyPool[i].m_linearVelocity +
1822  m_tmpSolverBodyPool[i].m_externalForceImpulse);
1823 
1824  m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(
1825  m_tmpSolverBodyPool[i].m_angularVelocity +
1826  m_tmpSolverBodyPool[i].m_externalTorqueImpulse);
1827 
1829  m_tmpSolverBodyPool[i].m_originalBody->setWorldTransform(m_tmpSolverBodyPool[i].m_worldTransform);
1830 
1831  m_tmpSolverBodyPool[i].m_originalBody->setCompanionId(-1);
1832  }
1833  }
1834 }
1835 
1837 {
1838  BT_PROFILE("solveGroupCacheFriendlyFinish");
1839 
1841  {
1843  }
1844 
1846  writeBackBodies(0, m_tmpSolverBodyPool.size(), infoGlobal);
1847 
1852 
1853  m_tmpSolverBodyPool.resizeNoInitialize(0);
1854  return 0.f;
1855 }
1856 
1859 {
1860  BT_PROFILE("solveGroup");
1861  //you need to provide at least some bodies
1862 
1864 
1866 
1868 
1869  return 0.f;
1870 }
1871 
1873 {
1874  m_btSeed2 = 0;
1875 }
_GL_VOID GLfloat value _GL_VOID_RET _GL_VOID const GLuint GLboolean *residences _GL_BOOL_RET _GL_VOID GLsizei GLfloat GLfloat GLfloat GLfloat const GLubyte *bitmap _GL_VOID_RET _GL_VOID GLenum const void *lists _GL_VOID_RET _GL_VOID const GLdouble *equation _GL_VOID_RET _GL_VOID GLdouble GLdouble blue _GL_VOID_RET _GL_VOID GLfloat GLfloat blue _GL_VOID_RET _GL_VOID GLint GLint blue _GL_VOID_RET _GL_VOID GLshort GLshort blue _GL_VOID_RET _GL_VOID GLubyte GLubyte blue _GL_VOID_RET _GL_VOID GLuint GLuint blue _GL_VOID_RET _GL_VOID GLushort GLushort blue _GL_VOID_RET _GL_VOID GLbyte GLbyte GLbyte alpha _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble alpha _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat alpha _GL_VOID_RET _GL_VOID GLint GLint GLint alpha _GL_VOID_RET _GL_VOID GLshort GLshort GLshort alpha _GL_VOID_RET _GL_VOID GLubyte GLubyte GLubyte alpha _GL_VOID_RET _GL_VOID GLuint GLuint GLuint alpha _GL_VOID_RET _GL_VOID GLushort GLushort GLushort alpha _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLint GLsizei GLsizei GLenum type _GL_VOID_RET _GL_VOID GLsizei GLenum GLenum const void *pixels _GL_VOID_RET _GL_VOID const void *pointer _GL_VOID_RET _GL_VOID GLdouble v _GL_VOID_RET _GL_VOID GLfloat v _GL_VOID_RET _GL_VOID GLint GLint i2 _GL_VOID_RET _GL_VOID GLint j _GL_VOID_RET _GL_VOID GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble GLdouble GLdouble zFar _GL_VOID_RET _GL_UINT GLdouble *equation _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLenum GLfloat *v _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLfloat *values _GL_VOID_RET _GL_VOID GLushort *values _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLenum GLdouble *params _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_BOOL GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLushort pattern _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble u2 _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLdouble GLdouble v2 _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLdouble GLdouble nz _GL_VOID_RET _GL_VOID GLfloat GLfloat nz _GL_VOID_RET _GL_VOID GLint GLint nz _GL_VOID_RET _GL_VOID GLshort GLshort nz _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const GLfloat *values _GL_VOID_RET _GL_VOID GLsizei const GLushort *values _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID const GLuint const GLclampf *priorities _GL_VOID_RET _GL_VOID GLdouble y _GL_VOID_RET _GL_VOID GLfloat y _GL_VOID_RET _GL_VOID GLint y _GL_VOID_RET _GL_VOID GLshort y _GL_VOID_RET _GL_VOID GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLfloat GLfloat z _GL_VOID_RET _GL_VOID GLint GLint z _GL_VOID_RET _GL_VOID GLshort GLshort z _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble w _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat w _GL_VOID_RET _GL_VOID GLint GLint GLint w _GL_VOID_RET _GL_VOID GLshort GLshort GLshort w _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble y2 _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat y2 _GL_VOID_RET _GL_VOID GLint GLint GLint y2 _GL_VOID_RET _GL_VOID GLshort GLshort GLshort y2 _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLuint *buffer _GL_VOID_RET _GL_VOID GLdouble t _GL_VOID_RET _GL_VOID GLfloat t _GL_VOID_RET _GL_VOID GLint t _GL_VOID_RET _GL_VOID GLshort t _GL_VOID_RET _GL_VOID GLdouble GLdouble r _GL_VOID_RET _GL_VOID GLfloat GLfloat r _GL_VOID_RET _GL_VOID GLint GLint r _GL_VOID_RET _GL_VOID GLshort GLshort r _GL_VOID_RET _GL_VOID GLdouble GLdouble r
SIMD_FORCE_INLINE bool isStaticOrKinematicObject() const
btCollisionObject
@ CO_FEATHERSTONE_LINK
@ CF_ANISOTROPIC_FRICTION
@ CF_ANISOTROPIC_ROLLING_FRICTION
const btRigidBody & getRigidBodyA() const
const btRigidBody & getRigidBodyB() const
@ SOLVER_SIMD
@ SOLVER_ENABLE_FRICTION_DIRECTION_CACHING
@ SOLVER_USE_WARMSTARTING
@ SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
@ SOLVER_RANDMIZE_ORDER
@ SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION
@ SOLVER_USE_2_FRICTION_DIRECTIONS
btFixedConstraint btRigidBody & rbB
@ BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED
@ BT_CONTACT_FLAG_HAS_CONTACT_ERP
@ BT_CONTACT_FLAG_HAS_CONTACT_CFM
@ BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING
@ BT_CONTACT_FLAG_FRICTION_ANCHOR
SIMD_FORCE_INLINE const T & btMax(const T &a, const T &b)
Definition: btMinMax.h:27
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject **bodies, int numBodies, const btContactSolverInfo &infoGlobal)
virtual btScalar solveSingleIteration(int iteration, btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
SIMD_FORCE_INLINE const btCollisionObject * getBody1() const
SIMD_FORCE_INLINE const btCollisionObject * getBody0() const
btPersistentManifold()
#define BT_PROFILE(name)
Definition: btQuickprof.h:198
static int uniqueId
Definition: btRigidBody.cpp:27
@ BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY
Definition: btRigidBody.h:47
@ BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT
Definition: btRigidBody.h:45
@ BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD
Definition: btRigidBody.h:46
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
SIMD_FORCE_INLINE btScalar btFabs(btScalar x)
Definition: btScalar.h:497
SIMD_FORCE_INLINE btScalar btSqrt(btScalar y)
Definition: btScalar.h:466
#define SIMD_INFINITY
Definition: btScalar.h:544
#define SIMD_EPSILON
Definition: btScalar.h:543
#define btAssert(x)
Definition: btScalar.h:295
btSequentialImpulseConstraintSolverMt int btPersistentManifold int btTypedConstraint ** constraints
btSequentialImpulseConstraintSolverMt int btPersistentManifold ** manifoldPtr
btSequentialImpulseConstraintSolverMt int btPersistentManifold int btTypedConstraint int numConstraints
btSequentialImpulseConstraintSolverMt int numBodies
btSequentialImpulseConstraintSolverMt int btPersistentManifold int btTypedConstraint int const btContactSolverInfo & infoGlobal
btSequentialImpulseConstraintSolverMt int btPersistentManifold int numManifolds
static btScalar gResolveSingleConstraintRowGeneric_scalar_reference(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &c)
static btScalar gResolveSingleConstraintRowLowerLimit_scalar_reference(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &c)
static btScalar gResolveSplitPenetrationImpulse_sse2(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &c)
static btScalar gResolveSplitPenetrationImpulse_scalar_reference(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &c)
btSingleConstraintRowSolver getScalarConstraintRowSolverLowerLimit()
Various implementations of solving a single constraint row using an inequality (lower limit) constrai...
void initSolverBody(btSolverBody *solverBody, btCollisionObject *collisionObject, btScalar timeStep)
virtual void convertJoints(btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal)
btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverGeneric()
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.)
void writeBackJoints(int iBegin, int iEnd, const btContactSolverInfo &infoGlobal)
btConstraintArray m_tmpSolverContactRollingFrictionConstraintPool
btAlignedObjectArray< int > m_orderTmpConstraintPool
btScalar(* btSingleConstraintRowSolver)(btSolverBody &, btSolverBody &, const btSolverConstraint &)
btAlignedObjectArray< int > m_orderNonContactConstraintPool
int m_maxOverrideNumSolverIterations
virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
virtual void convertContacts(btPersistentManifold **manifoldPtr, int numManifolds, const btContactSolverInfo &infoGlobal)
virtual ~btSequentialImpulseConstraintSolver()
void convertJoint(btSolverConstraint *currentConstraintRow, btTypedConstraint *constraint, const btTypedConstraint::btConstraintInfo1 &info1, int solverBodyIdA, int solverBodyIdB, const btContactSolverInfo &infoGlobal)
btAlignedObjectArray< int > m_kinematicBodyUniqueIdToSolverBodyTable
virtual btScalar solveGroup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifold, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &info, btIDebugDraw *debugDrawer, btDispatcher *dispatcher)
btSolverAnalyticsData m_analyticsData
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)
btScalar m_leastSquaresResidual
btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverLowerLimit()
btSingleConstraintRowSolver m_resolveSplitPenetrationImpulse
btSingleConstraintRowSolver getSSE2ConstraintRowSolverLowerLimit()
btScalar resolveSingleConstraintRowLowerLimit(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
void setupSolverFunctions(bool useSimd)
unsigned long m_btSeed2
m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
static void applyAnisotropicFriction(btCollisionObject *colObj, btVector3 &frictionDirection, int frictionMode)
btSingleConstraintRowSolver m_resolveSingleConstraintRowGeneric
btScalar resolveSingleConstraintRowGeneric(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
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.)
void convertContact(btPersistentManifold *manifold, const btContactSolverInfo &infoGlobal)
btScalar resolveSplitPenetrationImpulse(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
btSingleConstraintRowSolver getScalarConstraintRowSolverGeneric()
Various implementations of solving a single constraint row using a generic equality constraint,...
btSingleConstraintRowSolver getSSE2ConstraintRowSolverGeneric()
btAlignedObjectArray< int > m_orderFrictionConstraintPool
unsigned long btRand2()
void writeBackBodies(int iBegin, int iEnd, const btContactSolverInfo &infoGlobal)
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.)
btConstraintArray m_tmpSolverContactFrictionConstraintPool
btConstraintArray m_tmpSolverContactConstraintPool
btSingleConstraintRowSolver m_resolveSingleConstraintRowLowerLimit
virtual void convertBodies(btCollisionObject **bodies, int numBodies, const btContactSolverInfo &infoGlobal)
void setFrictionConstraintImpulse(btSolverConstraint &solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint &cp, const btContactSolverInfo &infoGlobal)
btScalar resolveSingleConstraintRowLowerLimitSIMD(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
int getOrInitSolverBody(btCollisionObject &body, btScalar timeStep)
btScalar restitutionCurve(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold)
btScalar resolveSingleConstraintRowGenericSIMD(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
void writeBackContacts(int iBegin, int iEnd, const btContactSolverInfo &infoGlobal)
btConstraintArray m_tmpSolverNonContactConstraintPool
void setupContactConstraint(btSolverConstraint &solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint &cp, const btContactSolverInfo &infoGlobal, btScalar &relaxation, const btVector3 &rel_pos1, const btVector3 &rel_pos2)
btAlignedObjectArray< btTypedConstraint::btConstraintInfo1 > m_tmpConstraintSizesPool
static T sum(const btAlignedObjectArray< T > &items)
btSolverBody
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
Definition: btSolverBody.h:105
btVector3 m_externalTorqueImpulse
Definition: btSolverBody.h:118
btVector3 m_externalForceImpulse
Definition: btSolverBody.h:117
btTransform m_worldTransform
Definition: btSolverBody.h:107
btVector3 m_angularVelocity
Definition: btSolverBody.h:116
#define btSimdScalar
Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later,...
Definition: btSolverBody.h:99
btVector3 m_linearVelocity
Definition: btSolverBody.h:115
btSolverConstraint
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
bool isEnabled() const
btJointFeedback
btTypedConstraint(btTypedConstraintType type, btRigidBody &rbA)
SIMD_FORCE_INLINE void btPlaneSpace1(const T &n, T &p, T &q)
Definition: btVector3.h:1251
btVector3
btVector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-by...
Definition: btVector3.h:82
SIMD_FORCE_INLINE void resizeNoInitialize(int newsize)
SIMD_FORCE_INLINE int size() const
return the number of elements in the array
SIMD_FORCE_INLINE void resize(int newsize, const T &fillData=T())
SIMD_FORCE_INLINE T & expandNonInitializing()
btScalar m_frictionCFM
btScalar m_combinedSpinningFriction
btScalar m_combinedRollingFriction
btScalar getDistance() const
btScalar m_combinedContactStiffness1
const btVector3 & getPositionWorldOnB() const
btScalar m_combinedRestitution
btVector3 m_lateralFrictionDir2
btScalar m_combinedContactDamping1
btScalar m_appliedImpulseLateral2
btScalar m_appliedImpulse
btScalar m_appliedImpulseLateral1
const btVector3 & getPositionWorldOnA() const
btVector3 m_normalWorldOnB
btScalar m_combinedFriction
btScalar m_contactMotion2
btVector3 m_lateralFrictionDir1
btScalar m_contactMotion1
const btVector3 & getAngularVelocity() const
Definition: btRigidBody.h:437
btVector3 getVelocityInLocalPoint(const btVector3 &rel_pos) const
Definition: btRigidBody.h:460
int getFlags() const
Definition: btRigidBody.h:608
const btVector3 & getLinearFactor() const
Definition: btRigidBody.h:254
btScalar getInvMass() const
Definition: btRigidBody.h:263
static const btRigidBody * upcast(const btCollisionObject *colObj)
Definition: btRigidBody.h:189
const btVector3 & getTotalTorque() const
Definition: btRigidBody.h:284
btVector3 computeGyroscopicImpulseImplicit_World(btScalar dt) const
perform implicit force computation in world space
btVector3 m_angularFactor
Definition: btRigidBody.h:98
btVector3 computeGyroscopicImpulseImplicit_Body(btScalar step) const
perform implicit force computation in body space (inertial frame)
SIMD_FORCE_INLINE btScalar computeImpulseDenominator(const btVector3 &pos, const btVector3 &normal) const
Definition: btRigidBody.h:482
const btVector3 & getLinearVelocity() const
Definition: btRigidBody.h:433
const btVector3 & getTotalForce() const
Definition: btRigidBody.h:279
const btMatrix3x3 & getInvInertiaTensorWorld() const
Definition: btRigidBody.h:265
void setLinearVelocity(const btVector3 &lin_vel)
Definition: btRigidBody.h:442
const btVector3 & getAngularFactor() const
Definition: btRigidBody.h:579
btVector3 computeGyroscopicForceExplicit(btScalar maxGyroscopicForce) const
explicit version is best avoided, it gains energy
SyclQueue void void size_t num_bytes void
BLI_INLINE float fb(float length, float L)
ccl_gpu_kernel_postfix ccl_global float int int int int float bool reset
clear internal cached data and reset random seed
ccl_device_inline float4 mask(const int4 &mask, const float4 &a)
Definition: math_float4.h:513
static unsigned c
Definition: RandGen.cpp:83
T dot(const vec_base< T, Size > &a, const vec_base< T, Size > &b)
vec_base< T, 3 > cross(const vec_base< T, 3 > &a, const vec_base< T, 3 > &b)
T distance(const T &a, const T &b)
static const pxr::TfToken b("b", pxr::TfToken::Immortal)