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 {
982 btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
983
984 frictionConstraint1.m_appliedImpulse = 0.f;
985 }
986
987 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
988 {
989 btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex + 1];
990
991 frictionConstraint2.m_appliedImpulse = 0.f;
992 }
993 }
994
995 void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold, const btContactSolverInfo& infoGlobal)
996 {
997 btCollisionObject *colObj0 = 0, *colObj1 = 0;
998
999 colObj0 = (btCollisionObject*)manifold->getBody0();
1000 colObj1 = (btCollisionObject*)manifold->getBody1();
1001
1002 int solverBodyIdA = getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep);
1003 int solverBodyIdB = getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep);
1004
1005 // btRigidBody* bodyA = btRigidBody::upcast(colObj0);
1006 // btRigidBody* bodyB = btRigidBody::upcast(colObj1);
1007
1008 btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverBodyIdA];
1009 btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverBodyIdB];
1010
1012 if (!solverBodyA || (solverBodyA->m_invMass.fuzzyZero() && (!solverBodyB || solverBodyB->m_invMass.fuzzyZero())))
1013 return;
1014
1015 int rollingFriction = 1;
1016 for (int j = 0; j < manifold->getNumContacts(); j++)
1017 {
1018 btManifoldPoint& cp = manifold->getContactPoint(j);
1019
1020 if (cp.getDistance() <= manifold->getContactProcessingThreshold())
1021 {
1022 btVector3 rel_pos1;
1023 btVector3 rel_pos2;
1024 btScalar relaxation;
1025
1026 int frictionIndex = m_tmpSolverContactConstraintPool.size();
1027 btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing();
1028 solverConstraint.m_solverBodyIdA = solverBodyIdA;
1029 solverConstraint.m_solverBodyIdB = solverBodyIdB;
1030
1031 solverConstraint.m_originalContactPoint = &cp;
1032
1033 const btVector3& pos1 = cp.getPositionWorldOnA();
1034 const btVector3& pos2 = cp.getPositionWorldOnB();
1035
1036 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
1037 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
1038
1039 btVector3 vel1;
1040 btVector3 vel2;
1041
1042 solverBodyA->getVelocityInLocalPointNoDelta(rel_pos1, vel1);
1043 solverBodyB->getVelocityInLocalPointNoDelta(rel_pos2, vel2);
1044
1045 btVector3 vel = vel1 - vel2;
1046 btScalar rel_vel = cp.m_normalWorldOnB.dot(vel);
1047
1048 setupContactConstraint(solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, relaxation, rel_pos1, rel_pos2);
1049
1051
1052 solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
1053
1054 if ((cp.m_combinedRollingFriction > 0.f) && (rollingFriction > 0))
1055 {
1056 {
1057 addTorsionalFrictionConstraint(cp.m_normalWorldOnB, solverBodyIdA, solverBodyIdB, frictionIndex, cp, cp.m_combinedSpinningFriction, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
1058 btVector3 axis0, axis1;
1059 btPlaneSpace1(cp.m_normalWorldOnB, axis0, axis1);
1060 axis0.normalize();
1061 axis1.normalize();
1062
1063 applyAnisotropicFriction(colObj0, axis0, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
1064 applyAnisotropicFriction(colObj1, axis0, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
1065 applyAnisotropicFriction(colObj0, axis1, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
1066 applyAnisotropicFriction(colObj1, axis1, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
1067 if (axis0.length() > 0.001)
1068 addTorsionalFrictionConstraint(axis0, solverBodyIdA, solverBodyIdB, frictionIndex, cp,
1069 cp.m_combinedRollingFriction, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
1070 if (axis1.length() > 0.001)
1071 addTorsionalFrictionConstraint(axis1, solverBodyIdA, solverBodyIdB, frictionIndex, cp,
1072 cp.m_combinedRollingFriction, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
1073 }
1074 }
1075
1080
1091
1092 if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !(cp.m_contactPointFlags & BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED))
1093 {
1094 cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
1095 btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
1096 if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
1097 {
1098 cp.m_lateralFrictionDir1 *= 1.f / btSqrt(lat_rel_vel);
1099 applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION);
1100 applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION);
1101 addFrictionConstraint(cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
1102
1103 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
1104 {
1105 cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
1106 cp.m_lateralFrictionDir2.normalize(); //??
1107 applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
1108 applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
1109 addFrictionConstraint(cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
1110 }
1111 }
1112 else
1113 {
1114 btPlaneSpace1(cp.m_normalWorldOnB, cp.m_lateralFrictionDir1, cp.m_lateralFrictionDir2);
1115
1116 applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION);
1117 applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION);
1118 addFrictionConstraint(cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
1119
1120 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
1121 {
1122 applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
1123 applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
1124 addFrictionConstraint(cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
1125 }
1126
1127 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
1128 {
1129 cp.m_contactPointFlags |= BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED;
1130 }
1131 }
1132 }
1133 else
1134 {
1135 addFrictionConstraint(cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM);
1136
1137 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
1138 addFrictionConstraint(cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM);
1139 }
1140 setFrictionConstraintImpulse(solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
1141 }
1142 }
1143 }
1144
1145 void btSequentialImpulseConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal)
1146 {
1147 int i;
1148 btPersistentManifold* manifold = 0;
1149 // btCollisionObject* colObj0=0,*colObj1=0;
1150
1151 for (i = 0; i < numManifolds; i++)
1152 {
1153 manifold = manifoldPtr[i];
1154 convertContact(manifold, infoGlobal);
1155 }
1156 }
1157
1158 void btSequentialImpulseConstraintSolver::convertJoint(btSolverConstraint* currentConstraintRow,
1159 btTypedConstraint* constraint,
1160 const btTypedConstraint::btConstraintInfo1& info1,
1161 int solverBodyIdA,
1162 int solverBodyIdB,
1163 const btContactSolverInfo& infoGlobal)
1164 {
1165 const btRigidBody& rbA = constraint->getRigidBodyA();
1166 const btRigidBody& rbB = constraint->getRigidBodyB();
1167
1168 const btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
1169 const btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
1170
1171 int overrideNumSolverIterations = constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations;
1172 if (overrideNumSolverIterations > m_maxOverrideNumSolverIterations)
1173 m_maxOverrideNumSolverIterations = overrideNumSolverIterations;
1174
1175 for (int j = 0; j < info1.m_numConstraintRows; j++)
1176 {
1177 memset(&currentConstraintRow[j], 0, sizeof(btSolverConstraint));
1178 currentConstraintRow[j].m_lowerLimit = -SIMD_INFINITY;
1179 currentConstraintRow[j].m_upperLimit = SIMD_INFINITY;
1180 currentConstraintRow[j].m_appliedImpulse = 0.f;
1181 currentConstraintRow[j].m_appliedPushImpulse = 0.f;
1182 currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA;
1183 currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB;
1184 currentConstraintRow[j].m_overrideNumSolverIterations = overrideNumSolverIterations;
1185 }
1186
1187 // these vectors are already cleared in initSolverBody, no need to redundantly clear again
1188 btAssert(bodyAPtr->getDeltaLinearVelocity().isZero());
1189 btAssert(bodyAPtr->getDeltaAngularVelocity().isZero());
1190 btAssert(bodyAPtr->getPushVelocity().isZero());
1191 btAssert(bodyAPtr->getTurnVelocity().isZero());
1192 btAssert(bodyBPtr->getDeltaLinearVelocity().isZero());
1193 btAssert(bodyBPtr->getDeltaAngularVelocity().isZero());
1194 btAssert(bodyBPtr->getPushVelocity().isZero());
1195 btAssert(bodyBPtr->getTurnVelocity().isZero());
1196 //bodyAPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
1197 //bodyAPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
1198 //bodyAPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f);
1199 //bodyAPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
1200 //bodyBPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
1201 //bodyBPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
1202 //bodyBPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f);
1203 //bodyBPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
1204
1205 btTypedConstraint::btConstraintInfo2 info2;
1206 info2.fps = 1.f / infoGlobal.m_timeStep;
1207 info2.erp = infoGlobal.m_erp;
1208 info2.m_J1linearAxis = currentConstraintRow->m_contactNormal1;
1209 info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal;
1210 info2.m_J2linearAxis = currentConstraintRow->m_contactNormal2;
1211 info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal;
1212 info2.rowskip = sizeof(btSolverConstraint) / sizeof(btScalar); //check this
1214 btAssert(info2.rowskip * sizeof(btScalar) == sizeof(btSolverConstraint));
1215 info2.m_constraintError = &currentConstraintRow->m_rhs;
1216 currentConstraintRow->m_cfm = infoGlobal.m_globalCfm;
1217 info2.m_damping = infoGlobal.m_damping;
1218 info2.cfm = &currentConstraintRow->m_cfm;
1219 info2.m_lowerLimit = &currentConstraintRow->m_lowerLimit;
1220 info2.m_upperLimit = &currentConstraintRow->m_upperLimit;
1221 info2.m_numIterations = infoGlobal.m_numIterations;
1222 constraint->getInfo2(&info2);
1223
1225 for (int j = 0; j < info1.m_numConstraintRows; j++)
1226 {
1227 btSolverConstraint& solverConstraint = currentConstraintRow[j];
1228
1229 if (solverConstraint.m_upperLimit >= constraint->getBreakingImpulseThreshold())
1230 {
1231 solverConstraint.m_upperLimit = constraint->getBreakingImpulseThreshold();
1232 }
1233
1234 if (solverConstraint.m_lowerLimit <= -constraint->getBreakingImpulseThreshold())
1235 {
1236 solverConstraint.m_lowerLimit = -constraint->getBreakingImpulseThreshold();
1237 }
1238
1239 solverConstraint.m_originalContactPoint = constraint;
1240
1241 {
1242 const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
1243 solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld() * ftorqueAxis1 * constraint->getRigidBodyA().getAngularFactor();
1244 }
1245 {
1246 const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal;
1247 solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld() * ftorqueAxis2 * constraint->getRigidBodyB().getAngularFactor();
1248 }
1249
1250 {
1251 btVector3 iMJlA = solverConstraint.m_contactNormal1 * rbA.getInvMass();
1252 btVector3 iMJaA = rbA.getInvInertiaTensorWorld() * solverConstraint.m_relpos1CrossNormal;
1253 btVector3 iMJlB = solverConstraint.m_contactNormal2 * rbB.getInvMass(); //sign of normal?
1254 btVector3 iMJaB = rbB.getInvInertiaTensorWorld() * solverConstraint.m_relpos2CrossNormal;
1255
1256 btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal1);
1257 sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
1258 sum += iMJlB.dot(solverConstraint.m_contactNormal2);
1259 sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
1260 btScalar fsum = btFabs(sum);
1261 btAssert(fsum > SIMD_EPSILON);
1262 btScalar sorRelaxation = 1.f; //todo: get from globalInfo?
1263 solverConstraint.m_jacDiagABInv = fsum > SIMD_EPSILON ? sorRelaxation / sum : 0.f;
1264 }
1265
1266 {
1267 btScalar rel_vel;
1268 btVector3 externalForceImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalForceImpulse : btVector3(0, 0, 0);
1269 btVector3 externalTorqueImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalTorqueImpulse : btVector3(0, 0, 0);
1270
1271 btVector3 externalForceImpulseB = bodyBPtr->m_originalBody ? bodyBPtr->m_externalForceImpulse : btVector3(0, 0, 0);
1272 btVector3 externalTorqueImpulseB = bodyBPtr->m_originalBody ? bodyBPtr->m_externalTorqueImpulse : btVector3(0, 0, 0);
1273
1274 btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(rbA.getLinearVelocity() + externalForceImpulseA) + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity() + externalTorqueImpulseA);
1275
1276 btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(rbB.getLinearVelocity() + externalForceImpulseB) + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity() + externalTorqueImpulseB);
1277
1278 rel_vel = vel1Dotn + vel2Dotn;
1279 btScalar restitution = 0.f;
1280 btScalar positionalError = solverConstraint.m_rhs; //already filled in by getConstraintInfo2
1281 btScalar velocityError = restitution - rel_vel * info2.m_damping;
1282 btScalar penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv;
1283 btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
1284 solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
1285 solverConstraint.m_appliedImpulse = 0.f;
1286 }
1287 }
1288 }
1289
1290 void btSequentialImpulseConstraintSolver::convertJoints(btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal)
1291 {
1292 BT_PROFILE("convertJoints");
1293 for (int j = 0; j < numConstraints; j++)
1294 {
1295 btTypedConstraint* constraint = constraints[j];
1296 constraint->buildJacobian();
1297 constraint->internalSetAppliedImpulse(0.0f);
1298 }
1299
1300 int totalNumRows = 0;
1301
1302 m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints);
1303 //calculate the total number of contraint rows
1304 for (int i = 0; i < numConstraints; i++)
1305 {
1306 btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
1307 btJointFeedback* fb = constraints[i]->getJointFeedback();
1308 if (fb)
1309 {
1310 fb->m_appliedForceBodyA.setZero();
1311 fb->m_appliedTorqueBodyA.setZero();
1312 fb->m_appliedForceBodyB.setZero();
1313 fb->m_appliedTorqueBodyB.setZero();
1314 }
1315
1316 if (constraints[i]->isEnabled())
1317 {
1318 constraints[i]->getInfo1(&info1);
1319 }
1320 else
1321 {
1322 info1.m_numConstraintRows = 0;
1323 info1.nub = 0;
1324 }
1325 totalNumRows += info1.m_numConstraintRows;
1326 }
1327 m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows);
1328
1330 int currentRow = 0;
1331
1332 for (int i = 0; i < numConstraints; i++)
1333 {
1334 const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
1335
1336 if (info1.m_numConstraintRows)
1337 {
1338 btAssert(currentRow < totalNumRows);
1339
1340 btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];
1341 btTypedConstraint* constraint = constraints[i];
1342 btRigidBody& rbA = constraint->getRigidBodyA();
1343 btRigidBody& rbB = constraint->getRigidBodyB();
1344
1345 int solverBodyIdA = getOrInitSolverBody(rbA, infoGlobal.m_timeStep);
1346 int solverBodyIdB = getOrInitSolverBody(rbB, infoGlobal.m_timeStep);
1347
1348 convertJoint(currentConstraintRow, constraint, info1, solverBodyIdA, solverBodyIdB, infoGlobal);
1349 }
1350 currentRow += info1.m_numConstraintRows;
1351 }
1352 }
1353
1354 void btSequentialImpulseConstraintSolver::convertBodies(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
1355 {
1356 BT_PROFILE("convertBodies");
1357 for (int i = 0; i < numBodies; i++)
1358 {
1359 bodies[i]->setCompanionId(-1);
1360 }
1361 #if BT_THREADSAFE
1362 m_kinematicBodyUniqueIdToSolverBodyTable.resize(0);
1363 #endif // BT_THREADSAFE
1364
1365 m_tmpSolverBodyPool.reserve(numBodies + 1);
1366 m_tmpSolverBodyPool.resize(0);
1367
1368 //btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
1369 //initSolverBody(&fixedBody,0);
1370
1371 for (int i = 0; i < numBodies; i++)
1372 {
1373 int bodyId = getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep);
1374
1375 btRigidBody* body = btRigidBody::upcast(bodies[i]);
1376 if (body && body->getInvMass())
1377 {
1378 btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
1379 btVector3 gyroForce(0, 0, 0);
1380 if (body->getFlags() & BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT)
1381 {
1382 gyroForce = body->computeGyroscopicForceExplicit(infoGlobal.m_maxGyroscopicForce);
1383 solverBody.m_externalTorqueImpulse -= gyroForce * body->getInvInertiaTensorWorld() * infoGlobal.m_timeStep;
1384 }
1385 if (body->getFlags() & BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD)
1386 {
1387 gyroForce = body->computeGyroscopicImpulseImplicit_World(infoGlobal.m_timeStep);
1388 solverBody.m_externalTorqueImpulse += gyroForce;
1389 }
1390 if (body->getFlags() & BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY)
1391 {
1392 gyroForce = body->computeGyroscopicImpulseImplicit_Body(infoGlobal.m_timeStep);
1393 solverBody.m_externalTorqueImpulse += gyroForce;
1394 }
1395 }
1396 }
1397 }
1398
1399 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
1400 {
1401 m_fixedBodyId = -1;
1402 BT_PROFILE("solveGroupCacheFriendlySetup");
1403 (void)debugDrawer;
1404
1405 // if solver mode has changed,
1406 if (infoGlobal.m_solverMode != m_cachedSolverMode)
1407 {
1408 // update solver functions to use SIMD or non-SIMD
1409 bool useSimd = !!(infoGlobal.m_solverMode & SOLVER_SIMD);
1410 setupSolverFunctions(useSimd);
1411 m_cachedSolverMode = infoGlobal.m_solverMode;
1412 }
1413 m_maxOverrideNumSolverIterations = 0;
1414
1415 #ifdef BT_ADDITIONAL_DEBUG
1416 //make sure that dynamic bodies exist for all (enabled) constraints
1417 for (int i = 0; i < numConstraints; i++)
1418 {
1419 btTypedConstraint* constraint = constraints[i];
1420 if (constraint->isEnabled())
1421 {
1422 if (!constraint->getRigidBodyA().isStaticOrKinematicObject())
1423 {
1424 bool found = false;
1425 for (int b = 0; b < numBodies; b++)
1426 {
1427 if (&constraint->getRigidBodyA() == bodies[b])
1428 {
1429 found = true;
1430 break;
1431 }
1432 }
1433 btAssert(found);
1434 }
1435 if (!constraint->getRigidBodyB().isStaticOrKinematicObject())
1436 {
1437 bool found = false;
1438 for (int b = 0; b < numBodies; b++)
1439 {
1440 if (&constraint->getRigidBodyB() == bodies[b])
1441 {
1442 found = true;
1443 break;
1444 }
1445 }
1446 btAssert(found);
1447 }
1448 }
1449 }
1450 //make sure that dynamic bodies exist for all contact manifolds
1451 for (int i = 0; i < numManifolds; i++)
1452 {
1453 if (!manifoldPtr[i]->getBody0()->isStaticOrKinematicObject())
1454 {
1455 bool found = false;
1456 for (int b = 0; b < numBodies; b++)
1457 {
1458 if (manifoldPtr[i]->getBody0() == bodies[b])
1459 {
1460 found = true;
1461 break;
1462 }
1463 }
1464 btAssert(found);
1465 }
1466 if (!manifoldPtr[i]->getBody1()->isStaticOrKinematicObject())
1467 {
1468 bool found = false;
1469 for (int b = 0; b < numBodies; b++)
1470 {
1471 if (manifoldPtr[i]->getBody1() == bodies[b])
1472 {
1473 found = true;
1474 break;
1475 }
1476 }
1477 btAssert(found);
1478 }
1479 }
1480 #endif //BT_ADDITIONAL_DEBUG
1481
1482 //convert all bodies
1483 convertBodies(bodies, numBodies, infoGlobal);
1484
1485 convertJoints(constraints, numConstraints, infoGlobal);
1486
1487 convertContacts(manifoldPtr, numManifolds, infoGlobal);
1488
1489 // btContactSolverInfo info = infoGlobal;
1490
1491 int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
1492 int numConstraintPool = m_tmpSolverContactConstraintPool.size();
1493 int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
1494
1496 m_orderNonContactConstraintPool.resizeNoInitialize(numNonContactPool);
1497 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
1498 m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool * 2);
1499 else
1500 m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool);
1501
1502 m_orderFrictionConstraintPool.resizeNoInitialize(numFrictionPool);
1503 {
1504 int i;
1505 for (i = 0; i < numNonContactPool; i++)
1506 {
1507 m_orderNonContactConstraintPool[i] = i;
1508 }
1509 for (i = 0; i < numConstraintPool; i++)
1510 {
1511 m_orderTmpConstraintPool[i] = i;
1512 }
1513 for (i = 0; i < numFrictionPool; i++)
1514 {
1515 m_orderFrictionConstraintPool[i] = i;
1516 }
1517 }
1518
1519 return 0.f;
1520 }
1521
1522 btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */, int /*numBodies*/, btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* /*debugDrawer*/)
1523 {
1524 BT_PROFILE("solveSingleIteration");
1525 btScalar leastSquaresResidual = 0.f;
1526
1527 int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
1528 int numConstraintPool = m_tmpSolverContactConstraintPool.size();
1529 int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
1530
1531 if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
1532 {
1533 if (1) // uncomment this for a bit less random ((iteration & 7) == 0)
1534 {
1535 for (int j = 0; j < numNonContactPool; ++j)
1536 {
1537 int tmp = m_orderNonContactConstraintPool[j];
1538 int swapi = btRandInt2(j + 1);
1539 m_orderNonContactConstraintPool[j] = m_orderNonContactConstraintPool[swapi];
1540 m_orderNonContactConstraintPool[swapi] = tmp;
1541 }
1542
1543 //contact/friction constraints are not solved more than
1544 if (iteration < infoGlobal.m_numIterations)
1545 {
1546 for (int j = 0; j < numConstraintPool; ++j)
1547 {
1548 int tmp = m_orderTmpConstraintPool[j];
1549 int swapi = btRandInt2(j + 1);
1550 m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
1551 m_orderTmpConstraintPool[swapi] = tmp;
1552 }
1553
1554 for (int j = 0; j < numFrictionPool; ++j)
1555 {
1556 int tmp = m_orderFrictionConstraintPool[j];
1557 int swapi = btRandInt2(j + 1);
1558 m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
1559 m_orderFrictionConstraintPool[swapi] = tmp;
1560 }
1561 }
1562 }
1563 }
1564
1566 for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++)
1567 {
1568 btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
1569 if (iteration < constraint.m_overrideNumSolverIterations)
1570 {
1571 btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA], m_tmpSolverBodyPool[constraint.m_solverBodyIdB], constraint);
1572 leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
1573 }
1574 }
1575
1576 if (iteration < infoGlobal.m_numIterations)
1577 {
1578 for (int j = 0; j < numConstraints; j++)
1579 {
1580 if (constraints[j]->isEnabled())
1581 {
1582 int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(), infoGlobal.m_timeStep);
1583 int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(), infoGlobal.m_timeStep);
1584 btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
1585 btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
1586 constraints[j]->solveConstraintObsolete(bodyA, bodyB, infoGlobal.m_timeStep);
1587 }
1588 }
1589
1591 if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS)
1592 {
1593 int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1594 int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) ? 2 : 1;
1595
1596 for (int c = 0; c < numPoolConstraints; c++)
1597 {
1598 btScalar totalImpulse = 0;
1599
1600 {
1601 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]];
1602 btScalar residual = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
1603 leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
1604
1605 totalImpulse = solveManifold.m_appliedImpulse;
1606 }
1607 bool applyFriction = true;
1608 if (applyFriction)
1609 {
1610 {
1611 btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c * multiplier]];
1612
1613 if (totalImpulse > btScalar(0))
1614 {
1615 solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
1616 solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
1617
1618 btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
1619 leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
1620 }
1621 }
1622
1623 if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)
1624 {
1625 btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c * multiplier + 1]];
1626
1627 if (totalImpulse > btScalar(0))
1628 {
1629 solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
1630 solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
1631
1632 btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
1633 leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
1634 }
1635 }
1636 }
1637 }
1638 }
1639 else //SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
1640 {
1641 //solve the friction constraints after all contact constraints, don't interleave them
1642 int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1643 int j;
1644
1645 for (j = 0; j < numPoolConstraints; j++)
1646 {
1647 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
1648 btScalar residual = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
1649 leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
1650 }
1651
1653
1654 int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
1655 for (j = 0; j < numFrictionPoolConstraints; j++)
1656 {
1657 btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
1658 btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
1659
1660 if (totalImpulse > btScalar(0))
1661 {
1662 solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
1663 solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
1664
1665 btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
1666 leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
1667 }
1668 }
1669 }
1670
1671 int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
1672 for (int j = 0; j < numRollingFrictionPoolConstraints; j++)
1673 {
1674 btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
1675 btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
1676 if (totalImpulse > btScalar(0))
1677 {
1678 btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction * totalImpulse;
1679 if (rollingFrictionMagnitude > rollingFrictionConstraint.m_friction)
1680 rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
1681
1682 rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
1683 rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
1684
1685 btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA], m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB], rollingFrictionConstraint);
1686 leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
1687 }
1688 }
1689 }
1690 return leastSquaresResidual;
1691 }
1692
1693 void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
1694 {
1695 BT_PROFILE("solveGroupCacheFriendlySplitImpulseIterations");
1696 int iteration;
1697 if (infoGlobal.m_splitImpulse)
1698 {
1699 {
1700 for (iteration = 0; iteration < infoGlobal.m_numIterations; iteration++)
1701 {
1702 btScalar leastSquaresResidual = 0.f;
1703 {
1704 int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1705 int j;
1706 for (j = 0; j < numPoolConstraints; j++)
1707 {
1708 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
1709
1710 btScalar residual = resolveSplitPenetrationImpulse(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
1711 leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
1712 }
1713 }
1714 if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration >= (infoGlobal.m_numIterations - 1))
1715 {
1716 #ifdef VERBOSE_RESIDUAL_PRINTF
1717 printf("residual = %f at iteration #%d\n", leastSquaresResidual, iteration);
1718 #endif
1719 break;
1720 }
1721 }
1722 }
1723 }
1724 }
1725
1726 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
1727 {
1728 BT_PROFILE("solveGroupCacheFriendlyIterations");
1729
1730 {
1732 solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
1733
1734 int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations;
1735
1736 for (int iteration = 0; iteration < maxIterations; iteration++)
1737 //for ( int iteration = maxIterations-1 ; iteration >= 0;iteration--)
1738 {
1739 m_leastSquaresResidual = solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
1740
1741 if (m_leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1)))
1742 {
1743 #ifdef VERBOSE_RESIDUAL_PRINTF
1744 printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration);
1745 #endif
1746 m_analyticsData.m_numSolverCalls++;
1747 m_analyticsData.m_numIterationsUsed = iteration+1;
1748 m_analyticsData.m_islandId = -2;
1749 if (numBodies>0)
1750 m_analyticsData.m_islandId = bodies[0]->getCompanionId();
1751 m_analyticsData.m_numBodies = numBodies;
1752 m_analyticsData.m_numContactManifolds = numManifolds;
1753 m_analyticsData.m_remainingLeastSquaresResidual = m_leastSquaresResidual;
1754 break;
1755 }
1756 }
1757 }
1758 return 0.f;
1759 }
1760
1761 void btSequentialImpulseConstraintSolver::writeBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal)
1762 {
1763 for (int j = iBegin; j < iEnd; j++)
1764 {
1765 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j];
1766 btManifoldPoint* pt = (btManifoldPoint*)solveManifold.m_originalContactPoint;
1767 btAssert(pt);
1768 pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
1769 // float f = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
1770 // printf("pt->m_appliedImpulseLateral1 = %f\n", f);
1771 pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
1772 //printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
1773 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
1774 {
1775 pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex + 1].m_appliedImpulse;
1776 }
1777 //do a callback here?
1778 }
1779 }
1780
1781 void btSequentialImpulseConstraintSolver::writeBackJoints(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal)
1782 {
1783 for (int j = iBegin; j < iEnd; j++)
1784 {
1785 const btSolverConstraint& solverConstr = m_tmpSolverNonContactConstraintPool[j];
1786 btTypedConstraint* constr = (btTypedConstraint*)solverConstr.m_originalContactPoint;
1787 btJointFeedback* fb = constr->getJointFeedback();
1788 if (fb)
1789 {
1790 fb->m_appliedForceBodyA += solverConstr.m_contactNormal1 * solverConstr.m_appliedImpulse * constr->getRigidBodyA().getLinearFactor() / infoGlobal.m_timeStep;
1791 fb->m_appliedForceBodyB += solverConstr.m_contactNormal2 * solverConstr.m_appliedImpulse * constr->getRigidBodyB().getLinearFactor() / infoGlobal.m_timeStep;
1792 fb->m_appliedTorqueBodyA += solverConstr.m_relpos1CrossNormal * constr->getRigidBodyA().getAngularFactor() * solverConstr.m_appliedImpulse / infoGlobal.m_timeStep;
1793 fb->m_appliedTorqueBodyB += solverConstr.m_relpos2CrossNormal * constr->getRigidBodyB().getAngularFactor() * solverConstr.m_appliedImpulse / infoGlobal.m_timeStep; /*RGM ???? */
1794 }
1795
1796 constr->internalSetAppliedImpulse(solverConstr.m_appliedImpulse);
1797 if (btFabs(solverConstr.m_appliedImpulse) >= constr->getBreakingImpulseThreshold())
1798 {
1799 constr->setEnabled(false);
1800 }
1801 }
1802 }
1803
1804 void btSequentialImpulseConstraintSolver::writeBackBodies(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal)
1805 {
1806 for (int i = iBegin; i < iEnd; i++)
1807 {
1808 btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody;
1809 if (body)
1810 {
1811 if (infoGlobal.m_splitImpulse)
1812 m_tmpSolverBodyPool[i].writebackVelocityAndTransform(infoGlobal.m_timeStep, infoGlobal.m_splitImpulseTurnErp);
1813 else
1814 m_tmpSolverBodyPool[i].writebackVelocity();
1815
1816 m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(
1817 m_tmpSolverBodyPool[i].m_linearVelocity +
1818 m_tmpSolverBodyPool[i].m_externalForceImpulse);
1819
1820 m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(
1821 m_tmpSolverBodyPool[i].m_angularVelocity +
1822 m_tmpSolverBodyPool[i].m_externalTorqueImpulse);
1823
1824 if (infoGlobal.m_splitImpulse)
1825 m_tmpSolverBodyPool[i].m_originalBody->setWorldTransform(m_tmpSolverBodyPool[i].m_worldTransform);
1826
1827 m_tmpSolverBodyPool[i].m_originalBody->setCompanionId(-1);
1828 }
1829 }
1830 }
1831
1832 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
1833 {
1834 BT_PROFILE("solveGroupCacheFriendlyFinish");
1835
1836 if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
1837 {
1838 writeBackContacts(0, m_tmpSolverContactConstraintPool.size(), infoGlobal);
1839 }
1840
1841 writeBackJoints(0, m_tmpSolverNonContactConstraintPool.size(), infoGlobal);
1842 writeBackBodies(0, m_tmpSolverBodyPool.size(), infoGlobal);
1843
1844 m_tmpSolverContactConstraintPool.resizeNoInitialize(0);
1845 m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0);
1846 m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize(0);
1847 m_tmpSolverContactRollingFrictionConstraintPool.resizeNoInitialize(0);
1848
1849 m_tmpSolverBodyPool.resizeNoInitialize(0);
1850 return 0.f;
1851 }
1852
1854 btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer, btDispatcher* /*dispatcher*/)
1855 {
1856 BT_PROFILE("solveGroup");
1857 //you need to provide at least some bodies
1858
1859 solveGroupCacheFriendlySetup(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
1860
1861 solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
1862
1863 solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal);
1864
1865 return 0.f;
1866 }
1867
1868 void btSequentialImpulseConstraintSolver::reset()
1869 {
1870 m_btSeed2 = 0;
1871 }
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:543
btSequentialImpulseConstraintSolver::m_tmpSolverContactFrictionConstraintPool
btConstraintArray m_tmpSolverContactFrictionConstraintPool
Definition: btSequentialImpulseConstraintSolver.h:59
btTypedConstraint::internalSetAppliedImpulse
void internalSetAppliedImpulse(btScalar appliedImpulse)
internal method used by the constraint solver, don't use them directly
Definition: btTypedConstraint.h:181
btSolverConstraint::m_lowerLimit
btScalar m_lowerLimit
Definition: btSolverConstraint.h:51
btTypedConstraint::btConstraintInfo2::m_damping
btScalar m_damping
Definition: btTypedConstraint.h:147
btSequentialImpulseConstraintSolver::writeBackContacts
void writeBackContacts(int iBegin, int iEnd, const btContactSolverInfo &infoGlobal)
Definition: btSequentialImpulseConstraintSolver.cpp:1761
btSolverBody
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
Definition: btSolverBody.h:105
btTypedConstraint
TypedConstraint is the baseclass for Bullet constraints and vehicles.
Definition: btTypedConstraint.h:76
btContactSolverInfoData::m_linearSlop
btScalar m_linearSlop
Definition: btContactSolverInfo.h:57
btSolverConstraint::m_relpos2CrossNormal
btVector3 m_relpos2CrossNormal
Definition: btSolverConstraint.h:37
btCollisionObject
btCollisionObject can be used to manage collision detection objects.
Definition: btCollisionObject.h:50
btSolverBody::m_originalBody
btRigidBody * m_originalBody
Definition: btSolverBody.h:120
btRigidBody
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:60
btManifoldPoint::m_lateralFrictionDir2
btVector3 m_lateralFrictionDir2
Definition: btManifoldPoint.h:140
SOLVER_SIMD
@ SOLVER_SIMD
Definition: btContactSolverInfo.h:30
btManifoldPoint::m_contactMotion2
btScalar m_contactMotion2
Definition: btManifoldPoint.h:123
btRigidBody::getTotalTorque
const btVector3 & getTotalTorque() const
Definition: btRigidBody.h:284
btSolverBody::internalSetInvMass
void internalSetInvMass(const btVector3 &invMass)
Definition: btSolverBody.h:217
btSolverBody::m_linearVelocity
btVector3 m_linearVelocity
Definition: btSolverBody.h:115
btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD
btScalar resolveSingleConstraintRowGenericSIMD(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
Definition: btSequentialImpulseConstraintSolver.cpp:261
btContactSolverInfoData::m_splitImpulsePenetrationThreshold
btScalar m_splitImpulsePenetrationThreshold
Definition: btContactSolverInfo.h:55
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:1854
btCpuFeatureUtility::CPU_FEATURE_FMA3
@ CPU_FEATURE_FMA3
Definition: btCpuFeatureUtility.h:29
BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT
@ BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT
BT_ENABLE_GYROPSCOPIC_FORCE flags is enabled by default in Bullet 2.83 and onwards.
Definition: btRigidBody.h:45
dot
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:888
btPlaneSpace1
void btPlaneSpace1(const T &n, T &p, T &q)
Definition: btVector3.h:1251
btSolverConstraint::m_friction
btScalar m_friction
Definition: btSolverConstraint.h:46
btManifoldPoint::m_contactCFM
btScalar m_contactCFM
Definition: btManifoldPoint.h:126
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
btSequentialImpulseConstraintSolver::getSSE4_1ConstraintRowSolverLowerLimit
btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverLowerLimit()
btSolverConstraint::m_appliedPushImpulse
btSimdScalar m_appliedPushImpulse
Definition: btSolverConstraint.h:43
btVector3::length
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:257
btManifoldPoint::m_contactERP
btScalar m_contactERP
Definition: btManifoldPoint.h:131
btSolverBody::internalGetInvMass
const btVector3 & internalGetInvMass() const
Definition: btSolverBody.h:212
btManifoldPoint::m_contactPointFlags
int m_contactPointFlags
Definition: btManifoldPoint.h:116
BT_CONTACT_FLAG_HAS_CONTACT_CFM
@ BT_CONTACT_FLAG_HAS_CONTACT_CFM
Definition: btManifoldPoint.h:43
btContactSolverInfo
Definition: btContactSolverInfo.h:73
btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulse
btScalar resolveSplitPenetrationImpulse(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
Definition: btSequentialImpulseConstraintSolver.h:139
maxIterations
int maxIterations
Definition: btQuantizedBvh.cpp:349
btVector3::setValue
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:640
btSolverConstraint
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
Definition: btSolverConstraint.h:31
btManifoldPoint::getPositionWorldOnA
const btVector3 & getPositionWorldOnA() const
Definition: btManifoldPoint.h:151
btTypedConstraint::getInfo1
virtual void getInfo1(btConstraintInfo1 *info)=0
internal method used by the constraint solver, don't use them directly
btRigidBody::getAngularVelocity
const btVector3 & getAngularVelocity() const
Definition: btRigidBody.h:402
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:1726
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric
btScalar resolveSingleConstraintRowGeneric(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
Definition: btSequentialImpulseConstraintSolver.cpp:267
btTypedConstraint::getRigidBodyA
const btRigidBody & getRigidBodyA() const
Definition: btTypedConstraint.h:214
btSolverBody::getPushVelocity
const btVector3 & getPushVelocity() const
Definition: btSolverBody.h:184
btDispatcher
The btDispatcher interface class can be used in combination with broadphase to dispatch calculations ...
Definition: btDispatcher.h:77
btSolverBody::internalGetTurnVelocity
btVector3 & internalGetTurnVelocity()
Definition: btSolverBody.h:227
SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION
@ SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION
Definition: btContactSolverInfo.h:28
btSequentialImpulseConstraintSolver::m_orderFrictionConstraintPool
btAlignedObjectArray< int > m_orderFrictionConstraintPool
Definition: btSequentialImpulseConstraintSolver.h:64
btTypedConstraint::getBreakingImpulseThreshold
btScalar getBreakingImpulseThreshold() const
Definition: btTypedConstraint.h:191
gResolveSplitPenetrationImpulse_scalar_reference
static btScalar gResolveSplitPenetrationImpulse_scalar_reference(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &c)
Definition: btSequentialImpulseConstraintSolver.cpp:282
btRigidBody::computeGyroscopicForceExplicit
btVector3 computeGyroscopicForceExplicit(btScalar maxGyroscopicForce) const
explicit version is best avoided, it gains energy
Definition: btRigidBody.cpp:279
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
btManifoldPoint::m_normalWorldOnB
btVector3 m_normalWorldOnB
Definition: btManifoldPoint.h:100
btVector3::cross
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:380
btSequentialImpulseConstraintSolver::restitutionCurve
btScalar restitutionCurve(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold)
Definition: btSequentialImpulseConstraintSolver.cpp:492
btPersistentManifold::getBody0
const btCollisionObject * getBody0() const
Definition: btPersistentManifold.h:105
btSingleConstraintRowSolver
btScalar(* btSingleConstraintRowSolver)(btSolverBody &, btSolverBody &, const btSolverConstraint &)
Definition: btSequentialImpulseConstraintSolver.h:30
btSequentialImpulseConstraintSolver::m_btSeed2
unsigned long m_btSeed2
m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
Definition: btSequentialImpulseConstraintSolver.h:108
btSequentialImpulseConstraintSolver::applyAnisotropicFriction
static void applyAnisotropicFriction(btCollisionObject *colObj, btVector3 &frictionDirection, int frictionMode)
Definition: btSequentialImpulseConstraintSolver.cpp:502
btTypedConstraint::btConstraintInfo2::m_J2angularAxis
btScalar * m_J2angularAxis
Definition: btTypedConstraint.h:130
btSolverConstraint::m_contactNormal2
btVector3 m_contactNormal2
Definition: btSolverConstraint.h:38
btContactSolverInfoData::m_splitImpulseTurnErp
btScalar m_splitImpulseTurnErp
Definition: btContactSolverInfo.h:56
SOLVER_USE_WARMSTARTING
@ SOLVER_USE_WARMSTARTING
Definition: btContactSolverInfo.h:25
btCpuFeatureUtility::CPU_FEATURE_SSE4_1
@ CPU_FEATURE_SSE4_1
Definition: btCpuFeatureUtility.h:30
btTypedConstraint::getJointFeedback
const btJointFeedback * getJointFeedback() const
Definition: btTypedConstraint.h:267
btSolverBody::m_externalForceImpulse
btVector3 m_externalForceImpulse
Definition: btSolverBody.h:117
btSequentialImpulseConstraintSolver::writeBackJoints
void writeBackJoints(int iBegin, int iEnd, const btContactSolverInfo &infoGlobal)
Definition: btSequentialImpulseConstraintSolver.cpp:1781
btSolverConstraint::m_cfm
btScalar m_cfm
Definition: btSolverConstraint.h:49
BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD
@ BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD
Definition: btRigidBody.h:46
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:229
btPersistentManifold::getNumContacts
int getNumContacts() const
Definition: btPersistentManifold.h:120
btContactSolverInfoData::m_sor
btScalar m_sor
Definition: btContactSolverInfo.h:46
btContactSolverInfoData::m_damping
btScalar m_damping
Definition: btContactSolverInfo.h:40
btContactSolverInfoData::m_maxGyroscopicForce
btScalar m_maxGyroscopicForce
Definition: btContactSolverInfo.h:63
btRigidBody.h
btSolverBody::m_linearFactor
btVector3 m_linearFactor
Definition: btSolverBody.h:111
btSequentialImpulseConstraintSolver::m_tmpSolverContactRollingFrictionConstraintPool
btConstraintArray m_tmpSolverContactRollingFrictionConstraintPool
Definition: btSequentialImpulseConstraintSolver.h:60
btRigidBody::computeGyroscopicImpulseImplicit_World
btVector3 computeGyroscopicImpulseImplicit_World(btScalar dt) const
perform implicit force computation in world space
Definition: btRigidBody.cpp:332
btSequentialImpulseConstraintSolver::m_kinematicBodyUniqueIdToSolverBodyTable
btAlignedObjectArray< int > m_kinematicBodyUniqueIdToSolverBodyTable
Definition: btSequentialImpulseConstraintSolver.h:75
btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit
btScalar resolveSingleConstraintRowLowerLimit(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
Definition: btSequentialImpulseConstraintSolver.cpp:277
BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY
@ BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY
Definition: btRigidBody.h:47
btJointFeedback
Definition: btTypedConstraint.h:65
btTypedConstraint::btConstraintInfo2::m_J2linearAxis
btScalar * m_J2linearAxis
Definition: btTypedConstraint.h:130
btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION
@ CF_ANISOTROPIC_ROLLING_FRICTION
Definition: btCollisionObject.h:160
btSolverConstraint::m_rhsPenetration
btScalar m_rhsPenetration
Definition: btSolverConstraint.h:53
btContactSolverInfoData::m_warmstartingFactor
btScalar m_warmstartingFactor
Definition: btContactSolverInfo.h:58
btContactSolverInfoData::m_frictionERP
btScalar m_frictionERP
Definition: btContactSolverInfo.h:51
btSolverBody::internalApplyImpulse
void internalApplyImpulse(const btVector3 &linearComponent, const btVector3 &angularComponent, const btScalar impulseMagnitude)
Definition: btSolverBody.h:243
btVector3::setZero
void setZero()
Definition: btVector3.h:671
btManifoldPoint::getDistance
btScalar getDistance() const
Definition: btManifoldPoint.h:142
btSolverConstraint::m_originalContactPoint
void * m_originalContactPoint
Definition: btSolverConstraint.h:55
btCollisionObject::isKinematicObject
bool isKinematicObject() const
Definition: btCollisionObject.h:200
btCpuFeatureUtility.h
btTypedConstraint::btConstraintInfo1::m_numConstraintRows
int m_numConstraintRows
Definition: btTypedConstraint.h:115
btSequentialImpulseConstraintSolver::reset
virtual void reset()
clear internal cached data and reset random seed
Definition: btSequentialImpulseConstraintSolver.cpp:1868
btSolverBody::getTurnVelocity
const btVector3 & getTurnVelocity() const
Definition: btSolverBody.h:189
btManifoldPoint::m_combinedRestitution
btScalar m_combinedRestitution
Definition: btManifoldPoint.h:106
btSolverBody::m_angularFactor
btVector3 m_angularFactor
Definition: btSolverBody.h:110
btSequentialImpulseConstraintSolver::m_resolveSingleConstraintRowGeneric
btSingleConstraintRowSolver m_resolveSingleConstraintRowGeneric
Definition: btSequentialImpulseConstraintSolver.h:77
btSequentialImpulseConstraintSolver::writeBackBodies
void writeBackBodies(int iBegin, int iEnd, const btContactSolverInfo &infoGlobal)
Definition: btSequentialImpulseConstraintSolver.cpp:1804
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:1522
btSolverConstraint::m_angularComponentB
btVector3 m_angularComponentB
Definition: btSolverConstraint.h:41
btSolverConstraint::m_contactNormal1
btVector3 m_contactNormal1
Definition: btSolverConstraint.h:35
btTransform::setIdentity
void setIdentity()
Set this transformation to the identity.
Definition: btTransform.h:166
btMax
const T & btMax(const T &a, const T &b)
Definition: btMinMax.h:27
BT_CONTACT_FLAG_FRICTION_ANCHOR
@ BT_CONTACT_FLAG_FRICTION_ANCHOR
Definition: btManifoldPoint.h:46
btTypedConstraint::getRigidBodyB
const btRigidBody & getRigidBodyB() const
Definition: btTypedConstraint.h:218
btSolverBody::m_angularVelocity
btVector3 m_angularVelocity
Definition: btSolverBody.h:116
btCollisionObject::isStaticOrKinematicObject
bool isStaticOrKinematicObject() const
Definition: btCollisionObject.h:205
btSolverAnalyticsData::m_islandId
int m_islandId
Definition: btSequentialImpulseConstraintSolver.h:41
btCollisionObject::getWorldTransform
btTransform & getWorldTransform()
Definition: btCollisionObject.h:367
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:1693
btSolverAnalyticsData::m_remainingLeastSquaresResidual
double m_remainingLeastSquaresResidual
Definition: btSequentialImpulseConstraintSolver.h:46
btJointFeedback::m_appliedForceBodyB
btVector3 m_appliedForceBodyB
Definition: btTypedConstraint.h:69
btSolverBody::m_invMass
btVector3 m_invMass
Definition: btSolverBody.h:112
btVector3::isZero
bool isZero() const
Definition: btVector3.h:683
btAssert
#define btAssert(x)
Definition: btScalar.h:153
btSequentialImpulseConstraintSolver::getSSE4_1ConstraintRowSolverGeneric
btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverGeneric()
btTypedConstraint::btConstraintInfo2::erp
btScalar erp
Definition: btTypedConstraint.h:124
btSolverAnalyticsData::m_numIterationsUsed
int m_numIterationsUsed
Definition: btSequentialImpulseConstraintSolver.h:45
btRigidBody::getLinearFactor
const btVector3 & getLinearFactor() const
Definition: btRigidBody.h:254
btFabs
btScalar btFabs(btScalar x)
Definition: btScalar.h:497
btJointFeedback::m_appliedForceBodyA
btVector3 m_appliedForceBodyA
Definition: btTypedConstraint.h:67
btSequentialImpulseConstraintSolver::convertContacts
virtual void convertContacts(btPersistentManifold **manifoldPtr, int numManifolds, const btContactSolverInfo &infoGlobal)
Definition: btSequentialImpulseConstraintSolver.cpp:1145
btManifoldPoint::m_combinedContactDamping1
btScalar m_combinedContactDamping1
Definition: btManifoldPoint.h:132
btSimdScalar
#define btSimdScalar
Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later,...
Definition: btSolverBody.h:99
btTypedConstraint::isEnabled
bool isEnabled() const
Definition: btTypedConstraint.h:201
btIDebugDraw
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations.
Definition: btIDebugDraw.h:27
btManifoldPoint
ManifoldContactPoint collects and maintains persistent contactpoints.
Definition: btManifoldPoint.h:52
btManifoldPoint::m_contactMotion1
btScalar m_contactMotion1
Definition: btManifoldPoint.h:122
btManifoldPoint::m_combinedSpinningFriction
btScalar m_combinedSpinningFriction
Definition: btManifoldPoint.h:105
btSequentialImpulseConstraintSolver::getScalarConstraintRowSolverGeneric
btSingleConstraintRowSolver getScalarConstraintRowSolverGeneric()
Various implementations of solving a single constraint row using a generic equality constraint,...
Definition: btSequentialImpulseConstraintSolver.cpp:387
btTypedConstraint.h
btMinMax.h
btAlignedObjectArray::resize
void resize(int newsize, const T &fillData=T())
Definition: btAlignedObjectArray.h:203
btTransform::getBasis
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:108
btContactSolverInfoData::m_timeStep
btScalar m_timeStep
Definition: btContactSolverInfo.h:42
btPersistentManifold::getContactProcessingThreshold
btScalar getContactProcessingThreshold() const
Definition: btPersistentManifold.h:145
btTypedConstraint::getOverrideNumSolverIterations
int getOverrideNumSolverIterations() const
Definition: btTypedConstraint.h:150
btRigidBody::getAngularFactor
const btVector3 & getAngularFactor() const
Definition: btRigidBody.h:532
btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject **bodies, int numBodies, const btContactSolverInfo &infoGlobal)
Definition: btSequentialImpulseConstraintSolver.cpp:1832
btSequentialImpulseConstraintSolver::getSSE2ConstraintRowSolverGeneric
btSingleConstraintRowSolver getSSE2ConstraintRowSolverGeneric()
btSolverBody::internalApplyPushImpulse
void internalApplyPushImpulse(const btVector3 &linearComponent, const btVector3 &angularComponent, btScalar impulseMagnitude)
Definition: btSolverBody.h:165
btSequentialImpulseConstraintSolver::getScalarConstraintRowSolverLowerLimit
btSingleConstraintRowSolver getScalarConstraintRowSolverLowerLimit()
Various implementations of solving a single constraint row using an inequality (lower limit) constrai...
Definition: btSequentialImpulseConstraintSolver.cpp:392
btCollisionObject::getAnisotropicFriction
const btVector3 & getAnisotropicFriction() const
Definition: btCollisionObject.h:169
btSequentialImpulseConstraintSolver::m_resolveSingleConstraintRowLowerLimit
btSingleConstraintRowSolver m_resolveSingleConstraintRowLowerLimit
Definition: btSequentialImpulseConstraintSolver.h:78
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
btSolverConstraint::m_rhs
btScalar m_rhs
Definition: btSolverConstraint.h:48
gResolveSingleConstraintRowLowerLimit_scalar_reference
static btScalar gResolveSingleConstraintRowLowerLimit_scalar_reference(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &c)
Definition: btSequentialImpulseConstraintSolver.cpp:77
btSolverBody::internalGetDeltaAngularVelocity
btVector3 & internalGetDeltaAngularVelocity()
Definition: btSolverBody.h:202
btTypedConstraint::btConstraintInfo2::m_lowerLimit
btScalar * m_lowerLimit
Definition: btTypedConstraint.h:141
btTypedConstraint::btConstraintInfo2::m_constraintError
btScalar * m_constraintError
Definition: btTypedConstraint.h:138
btVector3::fuzzyZero
bool fuzzyZero() const
Definition: btVector3.h:688
btManifoldPoint::m_frictionCFM
btScalar m_frictionCFM
Definition: btManifoldPoint.h:135
btRigidBody::getInvMass
btScalar getInvMass() const
Definition: btRigidBody.h:263
BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING
@ BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING
Definition: btManifoldPoint.h:45
btSequentialImpulseConstraintSolver::m_orderTmpConstraintPool
btAlignedObjectArray< int > m_orderTmpConstraintPool
Definition: btSequentialImpulseConstraintSolver.h:62
btCollisionObject::setCompanionId
void setCompanionId(int id)
Definition: btCollisionObject.h:451
btSolverConstraint::m_jacDiagABInv
btScalar m_jacDiagABInv
Definition: btSolverConstraint.h:47
btSequentialImpulseConstraintSolver::m_analyticsData
btSolverAnalyticsData m_analyticsData
Definition: btSequentialImpulseConstraintSolver.h:212
btContactSolverInfoData::m_splitImpulse
int m_splitImpulse
Definition: btContactSolverInfo.h:54
SIMD_INFINITY
#define SIMD_INFINITY
Definition: btScalar.h:544
btSolverBody::getVelocityInLocalPointNoDelta
void getVelocityInLocalPointNoDelta(const btVector3 &rel_pos, btVector3 &velocity) const
Definition: btSolverBody.h:131
btSolverBody::internalGetPushVelocity
btVector3 & internalGetPushVelocity()
Definition: btSolverBody.h:222
btSolverBody::internalGetDeltaLinearVelocity
btVector3 & internalGetDeltaLinearVelocity()
some internal methods, don't use them
Definition: btSolverBody.h:197
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:82
btManifoldPoint::getPositionWorldOnB
const btVector3 & getPositionWorldOnB() const
Definition: btManifoldPoint.h:157
btContactSolverInfoData::m_globalCfm
btScalar m_globalCfm
Definition: btContactSolverInfo.h:50
btTypedConstraint::btConstraintInfo1
Definition: btTypedConstraint.h:114
btSolverAnalyticsData::m_numBodies
int m_numBodies
Definition: btSequentialImpulseConstraintSolver.h:42
btSequentialImpulseConstraintSolver::m_maxOverrideNumSolverIterations
int m_maxOverrideNumSolverIterations
Definition: btSequentialImpulseConstraintSolver.h:66
btSequentialImpulseConstraintSolver::convertJoint
void convertJoint(btSolverConstraint *currentConstraintRow, btTypedConstraint *constraint, const btTypedConstraint::btConstraintInfo1 &info1, int solverBodyIdA, int solverBodyIdB, const btContactSolverInfo &infoGlobal)
Definition: btSequentialImpulseConstraintSolver.cpp:1158
btManifoldPoint::m_combinedRollingFriction
btScalar m_combinedRollingFriction
Definition: btManifoldPoint.h:104
btSequentialImpulseConstraintSolver::getSSE2ConstraintRowSolverLowerLimit
btSingleConstraintRowSolver getSSE2ConstraintRowSolverLowerLimit()
btContactSolverInfoData::m_solverMode
int m_solverMode
Definition: btContactSolverInfo.h:60
btPersistentManifold
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
Definition: btPersistentManifold.h:65
btTransform::getOrigin
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:113
btRigidBody::getLinearVelocity
const btVector3 & getLinearVelocity() const
Definition: btRigidBody.h:398
btRigidBody::getVelocityInLocalPoint
btVector3 getVelocityInLocalPoint(const btVector3 &rel_pos) const
Definition: btRigidBody.h:419
btSequentialImpulseConstraintSolver::convertContact
void convertContact(btPersistentManifold *manifold, const btContactSolverInfo &infoGlobal)
Definition: btSequentialImpulseConstraintSolver.cpp:995
btRigidBody::getFlags
int getFlags() const
Definition: btRigidBody.h:561
btSolverConstraint::m_upperLimit
btScalar m_upperLimit
Definition: btSolverConstraint.h:52
btContactSolverInfoData::m_leastSquaresResidualThreshold
btScalar m_leastSquaresResidualThreshold
Definition: btContactSolverInfo.h:65
BT_CONTACT_FLAG_HAS_CONTACT_ERP
@ BT_CONTACT_FLAG_HAS_CONTACT_ERP
Definition: btManifoldPoint.h:44
btSequentialImpulseConstraintSolver::m_fixedBodyId
int m_fixedBodyId
Definition: btSequentialImpulseConstraintSolver.h:67
btCollisionObject::getWorldArrayIndex
int getWorldArrayIndex() const
Definition: btCollisionObject.h:456
btTypedConstraint::btConstraintInfo2::rowskip
int rowskip
Definition: btTypedConstraint.h:133
btSolverConstraint::m_overrideNumSolverIterations
int m_overrideNumSolverIterations
Definition: btSolverConstraint.h:60
btManifoldPoint::m_appliedImpulseLateral1
btScalar m_appliedImpulseLateral1
Definition: btManifoldPoint.h:120
btCollisionObject::CO_FEATHERSTONE_LINK
@ CO_FEATHERSTONE_LINK
Definition: btCollisionObject.h:153
btTypedConstraint::btConstraintInfo1::nub
int nub
Definition: btTypedConstraint.h:115
btContactSolverInfoData::m_erp
btScalar m_erp
Definition: btContactSolverInfo.h:47
btSequentialImpulseConstraintSolver::convertJoints
virtual void convertJoints(btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal)
Definition: btSequentialImpulseConstraintSolver.cpp:1290
btSequentialImpulseConstraintSolver::convertBodies
virtual void convertBodies(btCollisionObject **bodies, int numBodies, const btContactSolverInfo &infoGlobal)
Definition: btSequentialImpulseConstraintSolver.cpp:1354
btSolverConstraint::m_solverBodyIdA
int m_solverBodyIdA
Definition: btSolverConstraint.h:62
btSolverAnalyticsData::m_numContactManifolds
int m_numContactManifolds
Definition: btSequentialImpulseConstraintSolver.h:43
btSequentialImpulseConstraintSolver::m_tmpSolverContactConstraintPool
btConstraintArray m_tmpSolverContactConstraintPool
Definition: btSequentialImpulseConstraintSolver.h:57
btSolverBody::m_externalTorqueImpulse
btVector3 m_externalTorqueImpulse
Definition: btSolverBody.h:118
btSequentialImpulseConstraintSolver.h
btPersistentManifold.h
btTypedConstraint::solveConstraintObsolete
virtual void solveConstraintObsolete(btSolverBody &, btSolverBody &, btScalar)
internal method used by the constraint solver, don't use them directly
Definition: btTypedConstraint.h:212
btJointFeedback::m_appliedTorqueBodyB
btVector3 m_appliedTorqueBodyB
Definition: btTypedConstraint.h:70
btAlignedObjectArray::expand
T & expand(const T &fillValue=T())
Definition: btAlignedObjectArray.h:242
btSequentialImpulseConstraintSolver::m_tmpConstraintSizesPool
btAlignedObjectArray< btTypedConstraint::btConstraintInfo1 > m_tmpConstraintSizesPool
Definition: btSequentialImpulseConstraintSolver.h:65
btSequentialImpulseConstraintSolver::m_orderNonContactConstraintPool
btAlignedObjectArray< int > m_orderNonContactConstraintPool
Definition: btSequentialImpulseConstraintSolver.h:63
btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD
btScalar resolveSingleConstraintRowLowerLimitSIMD(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
Definition: btSequentialImpulseConstraintSolver.cpp:272
btSequentialImpulseConstraintSolver::m_cachedSolverMode
int m_cachedSolverMode
Definition: btSequentialImpulseConstraintSolver.h:80
btSequentialImpulseConstraintSolver::m_tmpSolverBodyPool
btAlignedObjectArray< btSolverBody > m_tmpSolverBodyPool
Definition: btSequentialImpulseConstraintSolver.h:56
btIDebugDraw.h
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:1399
btSequentialImpulseConstraintSolver::m_resolveSplitPenetrationImpulse
btSingleConstraintRowSolver m_resolveSplitPenetrationImpulse
Definition: btSequentialImpulseConstraintSolver.h:79
btTypedConstraint::btConstraintInfo2::cfm
btScalar * cfm
Definition: btTypedConstraint.h:138
btAlignedObjectArray::reserve
void reserve(int _Count)
Definition: btAlignedObjectArray.h:280
btTypedConstraint::btConstraintInfo2::m_numIterations
int m_numIterations
Definition: btTypedConstraint.h:144
btSolverConstraint::m_solverBodyIdB
int m_solverBodyIdB
Definition: btSolverConstraint.h:63
btQuickprof.h
btCollisionObject::getInternalType
int getInternalType() const
reserved for Bullet internal usage
Definition: btCollisionObject.h:362
btSolverAnalyticsData::m_numSolverCalls
int m_numSolverCalls
Definition: btSequentialImpulseConstraintSolver.h:44
btSolverBody::getDeltaLinearVelocity
const btVector3 & getDeltaLinearVelocity() const
Definition: btSolverBody.h:174
btCpuFeatureUtility::getCpuFeatures
static int getCpuFeatures()
Definition: btCpuFeatureUtility.h:34
btTypedConstraint::setEnabled
void setEnabled(bool enabled)
Definition: btTypedConstraint.h:206
btContactSolverInfoData::m_restitutionVelocityThreshold
btScalar m_restitutionVelocityThreshold
Definition: btContactSolverInfo.h:66
btSolverConstraint::m_relpos1CrossNormal
btVector3 m_relpos1CrossNormal
Definition: btSolverConstraint.h:34
btTypedConstraint::btConstraintInfo2
Definition: btTypedConstraint.h:121
SOLVER_ENABLE_FRICTION_DIRECTION_CACHING
@ SOLVER_ENABLE_FRICTION_DIRECTION_CACHING
Definition: btContactSolverInfo.h:27
btSequentialImpulseConstraintSolver::m_tmpSolverNonContactConstraintPool
btConstraintArray m_tmpSolverNonContactConstraintPool
Definition: btSequentialImpulseConstraintSolver.h:58
btRigidBody::getInvInertiaTensorWorld
const btMatrix3x3 & getInvInertiaTensorWorld() const
Definition: btRigidBody.h:265
btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver
virtual ~btSequentialImpulseConstraintSolver()
Definition: btSequentialImpulseConstraintSolver.cpp:383
btJointFeedback::m_appliedTorqueBodyA
btVector3 m_appliedTorqueBodyA
Definition: btTypedConstraint.h:68
btPersistentManifold::getContactPoint
const btManifoldPoint & getContactPoint(int index) const
Definition: btPersistentManifold.h:130
btTypedConstraint::btConstraintInfo2::fps
btScalar fps
Definition: btTypedConstraint.h:124
SOLVER_USE_2_FRICTION_DIRECTIONS
@ SOLVER_USE_2_FRICTION_DIRECTIONS
Definition: btContactSolverInfo.h:26
btAlignedObjectArray.h
btManifoldPoint::m_lateralFrictionDir1
btVector3 m_lateralFrictionDir1
Definition: btManifoldPoint.h:139
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
btAlignedObjectArray::resizeNoInitialize
void resizeNoInitialize(int newsize)
resize changes the number of elements in the array.
Definition: btAlignedObjectArray.h:194
btCollisionObject::hasAnisotropicFriction
bool hasAnisotropicFriction(int frictionMode=CF_ANISOTROPIC_FRICTION) const
Definition: btCollisionObject.h:179
btPersistentManifold::getBody1
const btCollisionObject * getBody1() const
Definition: btPersistentManifold.h:106
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
btSequentialImpulseConstraintSolver::m_leastSquaresResidual
btScalar m_leastSquaresResidual
Definition: btSequentialImpulseConstraintSolver.h:83
btManifoldPoint::m_appliedImpulse
btScalar m_appliedImpulse
Definition: btManifoldPoint.h:118
SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
@ SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
Definition: btContactSolverInfo.h:31
btRigidBody::getTotalForce
const btVector3 & getTotalForce() const
Definition: btRigidBody.h:279
btTypedConstraint::btConstraintInfo2::m_J1angularAxis
btScalar * m_J1angularAxis
Definition: btTypedConstraint.h:130
btSolverConstraint::m_appliedImpulse
btSimdScalar m_appliedImpulse
Definition: btSolverConstraint.h:44
btSequentialImpulseConstraintSolver::btRand2
unsigned long btRand2()
Definition: btSequentialImpulseConstraintSolver.cpp:418
gNumSplitImpulseRecoveries
int gNumSplitImpulseRecoveries
Definition: btSequentialImpulseConstraintSolver.cpp:38
btSequentialImpulseConstraintSolver::getOrInitSolverBody
int getOrInitSolverBody(btCollisionObject &body, btScalar timeStep)
Definition: btSequentialImpulseConstraintSolver.cpp:689
btSequentialImpulseConstraintSolver::initSolverBody
void initSolverBody(btSolverBody *solverBody, btCollisionObject *collisionObject, btScalar timeStep)
Definition: btSequentialImpulseConstraintSolver.cpp:457
btTypedConstraint::btConstraintInfo2::m_upperLimit
btScalar * m_upperLimit
Definition: btTypedConstraint.h:141
btRigidBody::computeImpulseDenominator
btScalar computeImpulseDenominator(const btVector3 &pos, const btVector3 &normal) const
Definition: btRigidBody.h:435
btSqrt
btScalar btSqrt(btScalar y)
Definition: btScalar.h:466
btAlignedObjectArray::expandNonInitializing
T & expandNonInitializing()
Definition: btAlignedObjectArray.h:230
btCollisionObject::CF_ANISOTROPIC_FRICTION
@ CF_ANISOTROPIC_FRICTION
Definition: btCollisionObject.h:159
btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse
void setFrictionConstraintImpulse(btSolverConstraint &solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint &cp, const btContactSolverInfo &infoGlobal)
Definition: btSequentialImpulseConstraintSolver.cpp:977
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
btContactSolverInfoData::m_numIterations
int m_numIterations
Definition: btContactSolverInfo.h:44
SOLVER_RANDMIZE_ORDER
@ SOLVER_RANDMIZE_ORDER
Definition: btContactSolverInfo.h:23
btVector3::normalize
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:303
btStackAlloc.h
uniqueId
static int uniqueId
Definition: btRigidBody.cpp:27
gResolveSplitPenetrationImpulse_sse2
static btScalar gResolveSplitPenetrationImpulse_sse2(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &c)
Definition: btSequentialImpulseConstraintSolver.cpp:314
BT_PROFILE
#define BT_PROFILE(name)
Definition: btQuickprof.h:198
btSolverConstraint::m_frictionIndex
int m_frictionIndex
Definition: btSolverConstraint.h:61
btTypedConstraint::btConstraintInfo2::m_J1linearAxis
btScalar * m_J1linearAxis
Definition: btTypedConstraint.h:130
btSolverBody::m_worldTransform
btTransform m_worldTransform
Definition: btSolverBody.h:107
btContactSolverInfoData::m_erp2
btScalar m_erp2
Definition: btContactSolverInfo.h:48
sum
static T sum(const btAlignedObjectArray< T > &items)
Definition: btSoftBodyHelpers.cpp:94
btManifoldPoint::m_appliedImpulseLateral2
btScalar m_appliedImpulseLateral2
Definition: btManifoldPoint.h:121
btRigidBody::upcast
static const btRigidBody * upcast(const btCollisionObject *colObj)
to keep collision detection and dynamics separate we don't store a rigidbody pointer but a rigidbody ...
Definition: btRigidBody.h:189
btSequentialImpulseConstraintSolver::btRandInt2
int btRandInt2(int n)
Definition: btSequentialImpulseConstraintSolver.cpp:425
btManifoldPoint::m_combinedFriction
btScalar m_combinedFriction
Definition: btManifoldPoint.h:103
BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED
@ BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED
Definition: btManifoldPoint.h:42
btAlignedObjectArray::size
int size() const
return the number of elements in the array
Definition: btAlignedObjectArray.h:142
btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver
btSequentialImpulseConstraintSolver()
Definition: btSequentialImpulseConstraintSolver.cpp:351
btSolverBody::getDeltaAngularVelocity
const btVector3 & getDeltaAngularVelocity() const
Definition: btSolverBody.h:179
btManifoldPoint::m_combinedContactStiffness1
btScalar m_combinedContactStiffness1
Definition: btManifoldPoint.h:127
btTypedConstraint::getInfo2
virtual void getInfo2(btConstraintInfo2 *info)=0
internal method used by the constraint solver, don't use them directly
btVector3::length2
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:251
btSolverConstraint::m_angularComponentA
btVector3 m_angularComponentA
Definition: btSolverConstraint.h:40
btRigidBody::computeGyroscopicImpulseImplicit_Body
btVector3 computeGyroscopicImpulseImplicit_Body(btScalar step) const
perform implicit force computation in body space (inertial frame)
Definition: btRigidBody.cpp:293
btTypedConstraint::buildJacobian
virtual void buildJacobian()
internal method used by the constraint solver, don't use them directly
Definition: btTypedConstraint.h:163
btCollisionObject::getCompanionId
int getCompanionId() const
Definition: btCollisionObject.h:446
btSequentialImpulseConstraintSolver::setupSolverFunctions
void setupSolverFunctions(bool useSimd)
Definition: btSequentialImpulseConstraintSolver.cpp:358