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