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