Bullet Collision Detection & Physics Library: btSequentialImpulseConstraintSolver.cpp Source File

Bullet Collision Detection & Physics Library

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