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