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