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
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 void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFriendly(
302 btSolverBody& body1,
303 btSolverBody& body2,
304 const btSolverConstraint& c)
305 {
306 if (c.m_rhsPenetration)
307 {
308 gNumSplitImpulseRecoveries++;
309 btScalar deltaImpulse = c.m_rhsPenetration-btScalar(c.m_appliedPushImpulse)*c.m_cfm;
310 const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetPushVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetTurnVelocity());
311 const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(body2.internalGetPushVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetTurnVelocity());
312
313 deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
314 deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv;
315 const btScalar sum = btScalar(c.m_appliedPushImpulse) + deltaImpulse;
316 if (sum < c.m_lowerLimit)
317 {
318 deltaImpulse = c.m_lowerLimit-c.m_appliedPushImpulse;
319 c.m_appliedPushImpulse = c.m_lowerLimit;
320 }
321 else
322 {
323 c.m_appliedPushImpulse = sum;
324 }
325 body1.internalApplyPushImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
326 body2.internalApplyPushImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
327 }
328 }
329
330 void btSequentialImpulseConstraintSolver::resolveSplitPenetrationSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
331 {
332 #ifdef USE_SIMD
333 if (!c.m_rhsPenetration)
334 return;
335
336 gNumSplitImpulseRecoveries++;
337
338 __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedPushImpulse);
339 __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
340 __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
341 __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)));
342 __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128,body1.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetTurnVelocity().mVec128));
343 __m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128,body2.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetTurnVelocity().mVec128));
344 deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
345 deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
346 btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
347 btSimdScalar resultLowerLess,resultUpperLess;
348 resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
349 resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
350 __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
351 deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
352 c.m_appliedPushImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
353 __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128,body1.internalGetInvMass().mVec128);
354 __m128 linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128,body2.internalGetInvMass().mVec128);
355 __m128 impulseMagnitude = deltaImpulse;
356 body1.internalGetPushVelocity().mVec128 = _mm_add_ps(body1.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
357 body1.internalGetTurnVelocity().mVec128 = _mm_add_ps(body1.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
358 body2.internalGetPushVelocity().mVec128 = _mm_add_ps(body2.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
359 body2.internalGetTurnVelocity().mVec128 = _mm_add_ps(body2.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
360 #else
361 resolveSplitPenetrationImpulseCacheFriendly(body1,body2,c);
362 #endif
363 }
364
365
366 btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
367 : m_resolveSingleConstraintRowGeneric(gResolveSingleConstraintRowGeneric_scalar_reference),
368 m_resolveSingleConstraintRowLowerLimit(gResolveSingleConstraintRowLowerLimit_scalar_reference),
369 m_btSeed2(0)
370 {
371
372 #ifdef USE_SIMD
373 m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_sse2;
374 m_resolveSingleConstraintRowLowerLimit=gResolveSingleConstraintRowLowerLimit_sse2;
375 #endif //USE_SIMD
376
377 #ifdef BT_ALLOW_SSE4
378 int cpuFeatures = btCpuFeatureUtility::getCpuFeatures();
379 if ((cpuFeatures & btCpuFeatureUtility::CPU_FEATURE_FMA3) && (cpuFeatures & btCpuFeatureUtility::CPU_FEATURE_SSE4_1))
380 {
381 m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_sse4_1_fma3;
382 m_resolveSingleConstraintRowLowerLimit = gResolveSingleConstraintRowLowerLimit_sse4_1_fma3;
383 }
384 #endif//BT_ALLOW_SSE4
385
386 }
387
388 btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
389 {
390 }
391
392 btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getScalarConstraintRowSolverGeneric()
393 {
394 return gResolveSingleConstraintRowGeneric_scalar_reference;
395 }
396
397 btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getScalarConstraintRowSolverLowerLimit()
398 {
399 return gResolveSingleConstraintRowLowerLimit_scalar_reference;
400 }
401
402
403 #ifdef USE_SIMD
404 btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE2ConstraintRowSolverGeneric()
405 {
406 return gResolveSingleConstraintRowGeneric_sse2;
407 }
408 btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE2ConstraintRowSolverLowerLimit()
409 {
410 return gResolveSingleConstraintRowLowerLimit_sse2;
411 }
412 #ifdef BT_ALLOW_SSE4
413 btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE4_1ConstraintRowSolverGeneric()
414 {
415 return gResolveSingleConstraintRowGeneric_sse4_1_fma3;
416 }
417 btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE4_1ConstraintRowSolverLowerLimit()
418 {
419 return gResolveSingleConstraintRowLowerLimit_sse4_1_fma3;
420 }
421 #endif //BT_ALLOW_SSE4
422 #endif //USE_SIMD
423
424 unsigned long btSequentialImpulseConstraintSolver::btRand2()
425 {
426 m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff;
427 return m_btSeed2;
428 }
429
430
431
432 //See ODE: adam's all-int straightforward(?) dRandInt (0..n-1)
433 int btSequentialImpulseConstraintSolver::btRandInt2 (int n)
434 {
435 // seems good; xor-fold and modulus
436 const unsigned long un = static_cast<unsigned long>(n);
437 unsigned long r = btRand2();
438
439 // note: probably more aggressive than it needs to be -- might be
440 // able to get away without one or two of the innermost branches.
441 if (un <= 0x00010000UL) {
442 r ^= (r >> 16);
443 if (un <= 0x00000100UL) {
444 r ^= (r >> 8);
445 if (un <= 0x00000010UL) {
446 r ^= (r >> 4);
447 if (un <= 0x00000004UL) {
448 r ^= (r >> 2);
449 if (un <= 0x00000002UL) {
450 r ^= (r >> 1);
451 }
452 }
453 }
454 }
455 }
456
457 return (int) (r % un);
458 }
459
460
461
462 void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject, btScalar timeStep)
463 {
464
465 btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0;
466
467 solverBody->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
468 solverBody->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
469 solverBody->internalGetPushVelocity().setValue(0.f,0.f,0.f);
470 solverBody->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
471
472 if (rb)
473 {
474 solverBody->m_worldTransform = rb->getWorldTransform();
475 solverBody->internalSetInvMass(btVector3(rb->getInvMass(),rb->getInvMass(),rb->getInvMass())*rb->getLinearFactor());
476 solverBody->m_originalBody = rb;
477 solverBody->m_angularFactor = rb->getAngularFactor();
478 solverBody->m_linearFactor = rb->getLinearFactor();
479 solverBody->m_linearVelocity = rb->getLinearVelocity();
480 solverBody->m_angularVelocity = rb->getAngularVelocity();
481 solverBody->m_externalForceImpulse = rb->getTotalForce()*rb->getInvMass()*timeStep;
482 solverBody->m_externalTorqueImpulse = rb->getTotalTorque()*rb->getInvInertiaTensorWorld()*timeStep ;
483
484 } else
485 {
486 solverBody->m_worldTransform.setIdentity();
487 solverBody->internalSetInvMass(btVector3(0,0,0));
488 solverBody->m_originalBody = 0;
489 solverBody->m_angularFactor.setValue(1,1,1);
490 solverBody->m_linearFactor.setValue(1,1,1);
491 solverBody->m_linearVelocity.setValue(0,0,0);
492 solverBody->m_angularVelocity.setValue(0,0,0);
493 solverBody->m_externalForceImpulse.setValue(0,0,0);
494 solverBody->m_externalTorqueImpulse.setValue(0,0,0);
495 }
496
497
498 }
499
500
501
502
503
504
505 btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution)
506 {
507 btScalar rest = restitution * -rel_vel;
508 return rest;
509 }
510
511
512
513 void btSequentialImpulseConstraintSolver::applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode)
514 {
515
516
517 if (colObj && colObj->hasAnisotropicFriction(frictionMode))
518 {
519 // transform to local coordinates
520 btVector3 loc_lateral = frictionDirection * colObj->getWorldTransform().getBasis();
521 const btVector3& friction_scaling = colObj->getAnisotropicFriction();
522 //apply anisotropic friction
523 loc_lateral *= friction_scaling;
524 // ... and transform it back to global coordinates
525 frictionDirection = colObj->getWorldTransform().getBasis() * loc_lateral;
526 }
527
528 }
529
530
531
532
533 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)
534 {
535
536
537 btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
538 btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
539
540 btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody;
541 btRigidBody* body1 = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody;
542
543 solverConstraint.m_solverBodyIdA = solverBodyIdA;
544 solverConstraint.m_solverBodyIdB = solverBodyIdB;
545
546 solverConstraint.m_friction = cp.m_combinedFriction;
547 solverConstraint.m_originalContactPoint = 0;
548
549 solverConstraint.m_appliedImpulse = 0.f;
550 solverConstraint.m_appliedPushImpulse = 0.f;
551
552 if (body0)
553 {
554 solverConstraint.m_contactNormal1 = normalAxis;
555 btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal1);
556 solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
557 solverConstraint.m_angularComponentA = body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor();
558 }else
559 {
560 solverConstraint.m_contactNormal1.setZero();
561 solverConstraint.m_relpos1CrossNormal.setZero();
562 solverConstraint.m_angularComponentA .setZero();
563 }
564
565 if (body1)
566 {
567 solverConstraint.m_contactNormal2 = -normalAxis;
568 btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal2);
569 solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
570 solverConstraint.m_angularComponentB = body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor();
571 } else
572 {
573 solverConstraint.m_contactNormal2.setZero();
574 solverConstraint.m_relpos2CrossNormal.setZero();
575 solverConstraint.m_angularComponentB.setZero();
576 }
577
578 {
579 btVector3 vec;
580 btScalar denom0 = 0.f;
581 btScalar denom1 = 0.f;
582 if (body0)
583 {
584 vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
585 denom0 = body0->getInvMass() + normalAxis.dot(vec);
586 }
587 if (body1)
588 {
589 vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
590 denom1 = body1->getInvMass() + normalAxis.dot(vec);
591 }
592 btScalar denom = relaxation/(denom0+denom1);
593 solverConstraint.m_jacDiagABInv = denom;
594 }
595
596 {
597
598
599 btScalar rel_vel;
600 btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity+solverBodyA.m_externalForceImpulse:btVector3(0,0,0))
601 + solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0));
602 btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0))
603 + solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0));
604
605 rel_vel = vel1Dotn+vel2Dotn;
606
607 // btScalar positionalError = 0.f;
608
609 btScalar velocityError = desiredVelocity - rel_vel;
610 btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
611 solverConstraint.m_rhs = velocityImpulse;
612 solverConstraint.m_rhsPenetration = 0.f;
613 solverConstraint.m_cfm = cfmSlip;
614 solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
615 solverConstraint.m_upperLimit = solverConstraint.m_friction;
616
617 }
618 }
619
620 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)
621 {
622 btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing();
623 solverConstraint.m_frictionIndex = frictionIndex;
624 setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2,
625 colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
626 return solverConstraint;
627 }
628
629
630 void btSequentialImpulseConstraintSolver::setupTorsionalFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis1,int solverBodyIdA,int solverBodyIdB,
631 btManifoldPoint& cp,btScalar combinedTorsionalFriction, const btVector3& rel_pos1,const btVector3& rel_pos2,
632 btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation,
633 btScalar desiredVelocity, btScalar cfmSlip)
634
635 {
636 btVector3 normalAxis(0,0,0);
637
638
639 solverConstraint.m_contactNormal1 = normalAxis;
640 solverConstraint.m_contactNormal2 = -normalAxis;
641 btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
642 btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
643
644 btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody;
645 btRigidBody* body1 = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody;
646
647 solverConstraint.m_solverBodyIdA = solverBodyIdA;
648 solverConstraint.m_solverBodyIdB = solverBodyIdB;
649
650 solverConstraint.m_friction = combinedTorsionalFriction;
651 solverConstraint.m_originalContactPoint = 0;
652
653 solverConstraint.m_appliedImpulse = 0.f;
654 solverConstraint.m_appliedPushImpulse = 0.f;
655
656 {
657 btVector3 ftorqueAxis1 = -normalAxis1;
658 solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
659 solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor() : btVector3(0,0,0);
660 }
661 {
662 btVector3 ftorqueAxis1 = normalAxis1;
663 solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
664 solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor() : btVector3(0,0,0);
665 }
666
667
668 {
669 btVector3 iMJaA = body0?body0->getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal:btVector3(0,0,0);
670 btVector3 iMJaB = body1?body1->getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal:btVector3(0,0,0);
671 btScalar sum = 0;
672 sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
673 sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
674 solverConstraint.m_jacDiagABInv = btScalar(1.)/sum;
675 }
676
677 {
678
679
680 btScalar rel_vel;
681 btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity+solverBodyA.m_externalForceImpulse:btVector3(0,0,0))
682 + solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0));
683 btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0))
684 + solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0));
685
686 rel_vel = vel1Dotn+vel2Dotn;
687
688 // btScalar positionalError = 0.f;
689
690 btSimdScalar velocityError = desiredVelocity - rel_vel;
691 btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
692 solverConstraint.m_rhs = velocityImpulse;
693 solverConstraint.m_cfm = cfmSlip;
694 solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
695 solverConstraint.m_upperLimit = solverConstraint.m_friction;
696
697 }
698 }
699
700
701
702
703
704
705
706
707 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)
708 {
709 btSolverConstraint& solverConstraint = m_tmpSolverContactRollingFrictionConstraintPool.expandNonInitializing();
710 solverConstraint.m_frictionIndex = frictionIndex;
711 setupTorsionalFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, combinedTorsionalFriction,rel_pos1, rel_pos2,
712 colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
713 return solverConstraint;
714 }
715
716
717 int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body,btScalar timeStep)
718 {
719
720 int solverBodyIdA = -1;
721
722 if (body.getCompanionId() >= 0)
723 {
724 //body has already been converted
725 solverBodyIdA = body.getCompanionId();
726 btAssert(solverBodyIdA < m_tmpSolverBodyPool.size());
727 } else
728 {
729 btRigidBody* rb = btRigidBody::upcast(&body);
730 //convert both active and kinematic objects (for their velocity)
731 if (rb && (rb->getInvMass() || rb->isKinematicObject()))
732 {
733 solverBodyIdA = m_tmpSolverBodyPool.size();
734 btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
735 initSolverBody(&solverBody,&body,timeStep);
736 body.setCompanionId(solverBodyIdA);
737 } else
738 {
739
740 if (m_fixedBodyId<0)
741 {
742 m_fixedBodyId = m_tmpSolverBodyPool.size();
743 btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
744 initSolverBody(&fixedBody,0,timeStep);
745 }
746 return m_fixedBodyId;
747 // return 0;//assume first one is a fixed solver body
748 }
749 }
750
751 return solverBodyIdA;
752
753 }
754 #include <stdio.h>
755
756
757 void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstraint& solverConstraint,
758 int solverBodyIdA, int solverBodyIdB,
759 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
760 btScalar& relaxation,
761 const btVector3& rel_pos1, const btVector3& rel_pos2)
762 {
763
764 // const btVector3& pos1 = cp.getPositionWorldOnA();
765 // const btVector3& pos2 = cp.getPositionWorldOnB();
766
767 btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA];
768 btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB];
769
770 btRigidBody* rb0 = bodyA->m_originalBody;
771 btRigidBody* rb1 = bodyB->m_originalBody;
772
773 // btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
774 // btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
775 //rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
776 //rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
777
778 relaxation = infoGlobal.m_sor;
779 btScalar invTimeStep = btScalar(1)/infoGlobal.m_timeStep;
780
781 //cfm = 1 / ( dt * kp + kd )
782 //erp = dt * kp / ( dt * kp + kd )
783
784 btScalar cfm = infoGlobal.m_globalCfm;
785 btScalar erp = infoGlobal.m_erp2;
786
787 if ((cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM) || (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP))
788 {
789 if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM)
790 cfm = cp.m_contactCFM;
791 if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP)
792 erp = cp.m_contactERP;
793 } else
794 {
795 if (cp.m_contactPointFlags & BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING)
796 {
797 btScalar denom = ( infoGlobal.m_timeStep * cp.m_combinedContactStiffness1 + cp.m_combinedContactDamping1 );
798 if (denom < SIMD_EPSILON)
799 {
800 denom = SIMD_EPSILON;
801 }
802 cfm = btScalar(1) / denom;
803 erp = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1) / denom;
804 }
805 }
806
807 cfm *= invTimeStep;
808
809
810 btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
811 solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
812 btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);
813 solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
814
815 {
816 #ifdef COMPUTE_IMPULSE_DENOM
817 btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
818 btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
819 #else
820 btVector3 vec;
821 btScalar denom0 = 0.f;
822 btScalar denom1 = 0.f;
823 if (rb0)
824 {
825 vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
826 denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
827 }
828 if (rb1)
829 {
830 vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
831 denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
832 }
833 #endif //COMPUTE_IMPULSE_DENOM
834
835 btScalar denom = relaxation/(denom0+denom1+cfm);
836 solverConstraint.m_jacDiagABInv = denom;
837 }
838
839 if (rb0)
840 {
841 solverConstraint.m_contactNormal1 = cp.m_normalWorldOnB;
842 solverConstraint.m_relpos1CrossNormal = torqueAxis0;
843 } else
844 {
845 solverConstraint.m_contactNormal1.setZero();
846 solverConstraint.m_relpos1CrossNormal.setZero();
847 }
848 if (rb1)
849 {
850 solverConstraint.m_contactNormal2 = -cp.m_normalWorldOnB;
851 solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
852 }else
853 {
854 solverConstraint.m_contactNormal2.setZero();
855 solverConstraint.m_relpos2CrossNormal.setZero();
856 }
857
858 btScalar restitution = 0.f;
859 btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop;
860
861 {
862 btVector3 vel1,vel2;
863
864 vel1 = rb0? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
865 vel2 = rb1? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
866
867 // btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
868 btVector3 vel = vel1 - vel2;
869 btScalar rel_vel = cp.m_normalWorldOnB.dot(vel);
870
871
872
873 solverConstraint.m_friction = cp.m_combinedFriction;
874
875
876 restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution);
877 if (restitution <= btScalar(0.))
878 {
879 restitution = 0.f;
880 };
881 }
882
883
885 if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
886 {
887 solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
888 if (rb0)
889 bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
890 if (rb1)
891 bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
892 } else
893 {
894 solverConstraint.m_appliedImpulse = 0.f;
895 }
896
897 solverConstraint.m_appliedPushImpulse = 0.f;
898
899 {
900
901 btVector3 externalForceImpulseA = bodyA->m_originalBody ? bodyA->m_externalForceImpulse: btVector3(0,0,0);
902 btVector3 externalTorqueImpulseA = bodyA->m_originalBody ? bodyA->m_externalTorqueImpulse: btVector3(0,0,0);
903 btVector3 externalForceImpulseB = bodyB->m_originalBody ? bodyB->m_externalForceImpulse: btVector3(0,0,0);
904 btVector3 externalTorqueImpulseB = bodyB->m_originalBody ?bodyB->m_externalTorqueImpulse : btVector3(0,0,0);
905
906
907 btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(bodyA->m_linearVelocity+externalForceImpulseA)
908 + solverConstraint.m_relpos1CrossNormal.dot(bodyA->m_angularVelocity+externalTorqueImpulseA);
909 btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(bodyB->m_linearVelocity+externalForceImpulseB)
910 + solverConstraint.m_relpos2CrossNormal.dot(bodyB->m_angularVelocity+externalTorqueImpulseB);
911 btScalar rel_vel = vel1Dotn+vel2Dotn;
912
913 btScalar positionalError = 0.f;
914 btScalar velocityError = restitution - rel_vel;// * damping;
915
916
917
918 if (penetration>0)
919 {
920 positionalError = 0;
921
922 velocityError -= penetration *invTimeStep;
923 } else
924 {
925 positionalError = -penetration * erp*invTimeStep;
926
927 }
928
929 btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
930 btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
931
932 if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
933 {
934 //combine position and velocity into rhs
935 solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;//-solverConstraint.m_contactNormal1.dot(bodyA->m_externalForce*bodyA->m_invMass-bodyB->m_externalForce/bodyB->m_invMass)*solverConstraint.m_jacDiagABInv;
936 solverConstraint.m_rhsPenetration = 0.f;
937
938 } else
939 {
940 //split position and velocity into rhs and m_rhsPenetration
941 solverConstraint.m_rhs = velocityImpulse;
942 solverConstraint.m_rhsPenetration = penetrationImpulse;
943 }
944 solverConstraint.m_cfm = cfm*solverConstraint.m_jacDiagABInv;
945 solverConstraint.m_lowerLimit = 0;
946 solverConstraint.m_upperLimit = 1e10f;
947 }
948
949
950
951
952 }
953
954
955
956 void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolverConstraint& solverConstraint,
957 int solverBodyIdA, int solverBodyIdB,
958 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal)
959 {
960
961 btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA];
962 btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB];
963
964 btRigidBody* rb0 = bodyA->m_originalBody;
965 btRigidBody* rb1 = bodyB->m_originalBody;
966
967 {
968 btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
969 if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
970 {
971 frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
972 if (rb0)
973 bodyA->internalApplyImpulse(frictionConstraint1.m_contactNormal1*rb0->getInvMass()*rb0->getLinearFactor(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
974 if (rb1)
975 bodyB->internalApplyImpulse(-frictionConstraint1.m_contactNormal2*rb1->getInvMass()*rb1->getLinearFactor(),-frictionConstraint1.m_angularComponentB,-(btScalar)frictionConstraint1.m_appliedImpulse);
976 } else
977 {
978 frictionConstraint1.m_appliedImpulse = 0.f;
979 }
980 }
981
982 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
983 {
984 btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
985 if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
986 {
987 frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
988 if (rb0)
989 bodyA->internalApplyImpulse(frictionConstraint2.m_contactNormal1*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
990 if (rb1)
991 bodyB->internalApplyImpulse(-frictionConstraint2.m_contactNormal2*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-(btScalar)frictionConstraint2.m_appliedImpulse);
992 } else
993 {
994 frictionConstraint2.m_appliedImpulse = 0.f;
995 }
996 }
997 }
998
999
1000
1001
1002 void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal)
1003 {
1004 btCollisionObject* colObj0=0,*colObj1=0;
1005
1006 colObj0 = (btCollisionObject*)manifold->getBody0();
1007 colObj1 = (btCollisionObject*)manifold->getBody1();
1008
1009 int solverBodyIdA = getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep);
1010 int solverBodyIdB = getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep);
1011
1012 // btRigidBody* bodyA = btRigidBody::upcast(colObj0);
1013 // btRigidBody* bodyB = btRigidBody::upcast(colObj1);
1014
1015 btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverBodyIdA];
1016 btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverBodyIdB];
1017
1018
1019
1021 if (!solverBodyA || (solverBodyA->m_invMass.fuzzyZero() && (!solverBodyB || solverBodyB->m_invMass.fuzzyZero())))
1022 return;
1023
1024 int rollingFriction=1;
1025 for (int j=0;j<manifold->getNumContacts();j++)
1026 {
1027
1028 btManifoldPoint& cp = manifold->getContactPoint(j);
1029
1030 if (cp.getDistance() <= manifold->getContactProcessingThreshold())
1031 {
1032 btVector3 rel_pos1;
1033 btVector3 rel_pos2;
1034 btScalar relaxation;
1035
1036
1037 int frictionIndex = m_tmpSolverContactConstraintPool.size();
1038 btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing();
1039 btRigidBody* rb0 = btRigidBody::upcast(colObj0);
1040 btRigidBody* rb1 = btRigidBody::upcast(colObj1);
1041 solverConstraint.m_solverBodyIdA = solverBodyIdA;
1042 solverConstraint.m_solverBodyIdB = solverBodyIdB;
1043
1044 solverConstraint.m_originalContactPoint = &cp;
1045
1046 const btVector3& pos1 = cp.getPositionWorldOnA();
1047 const btVector3& pos2 = cp.getPositionWorldOnB();
1048
1049 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
1050 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
1051
1052 btVector3 vel1;// = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
1053 btVector3 vel2;// = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
1054
1055 solverBodyA->getVelocityInLocalPointNoDelta(rel_pos1,vel1);
1056 solverBodyB->getVelocityInLocalPointNoDelta(rel_pos2,vel2 );
1057
1058 btVector3 vel = vel1 - vel2;
1059 btScalar rel_vel = cp.m_normalWorldOnB.dot(vel);
1060
1061 setupContactConstraint(solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, relaxation, rel_pos1, rel_pos2);
1062
1063
1064
1065 // const btVector3& pos1 = cp.getPositionWorldOnA();
1066 // const btVector3& pos2 = cp.getPositionWorldOnB();
1067
1069
1070 solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
1071
1072 if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0))
1073 {
1074
1075 {
1076 addTorsionalFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,cp.m_combinedSpinningFriction, rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
1077 btVector3 axis0,axis1;
1078 btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1);
1079 axis0.normalize();
1080 axis1.normalize();
1081
1082 applyAnisotropicFriction(colObj0,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
1083 applyAnisotropicFriction(colObj1,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
1084 applyAnisotropicFriction(colObj0,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
1085 applyAnisotropicFriction(colObj1,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
1086 if (axis0.length()>0.001)
1087 addTorsionalFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,
1088 cp.m_combinedRollingFriction, rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
1089 if (axis1.length()>0.001)
1090 addTorsionalFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,
1091 cp.m_combinedRollingFriction, rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
1092
1093 }
1094 }
1095
1100
1111 if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !(cp.m_contactPointFlags&BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED))
1112 {
1113 cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
1114 btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
1115 if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
1116 {
1117 cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel);
1118 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
1119 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
1120 addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
1121
1122 if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
1123 {
1124 cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
1125 cp.m_lateralFrictionDir2.normalize();//??
1126 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
1127 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
1128 addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
1129 }
1130
1131 } else
1132 {
1133 btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
1134
1135 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
1136 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
1137 addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
1138
1139 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
1140 {
1141 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
1142 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
1143 addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
1144 }
1145
1146
1147 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
1148 {
1149 cp.m_contactPointFlags|=BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED;
1150 }
1151 }
1152
1153 } else
1154 {
1155 addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_frictionCFM);
1156
1157 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
1158 addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_frictionCFM);
1159
1160 }
1161 setFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
1162
1163
1164
1165
1166 }
1167 }
1168 }
1169
1170 void btSequentialImpulseConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal)
1171 {
1172 int i;
1173 btPersistentManifold* manifold = 0;
1174 // btCollisionObject* colObj0=0,*colObj1=0;
1175
1176
1177 for (i=0;i<numManifolds;i++)
1178 {
1179 manifold = manifoldPtr[i];
1180 convertContact(manifold,infoGlobal);
1181 }
1182 }
1183
1184 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
1185 {
1186 m_fixedBodyId = -1;
1187 BT_PROFILE("solveGroupCacheFriendlySetup");
1188 (void)debugDrawer;
1189
1190 m_maxOverrideNumSolverIterations = 0;
1191
1192 #ifdef BT_ADDITIONAL_DEBUG
1193 //make sure that dynamic bodies exist for all (enabled) constraints
1194 for (int i=0;i<numConstraints;i++)
1195 {
1196 btTypedConstraint* constraint = constraints[i];
1197 if (constraint->isEnabled())
1198 {
1199 if (!constraint->getRigidBodyA().isStaticOrKinematicObject())
1200 {
1201 bool found=false;
1202 for (int b=0;b<numBodies;b++)
1203 {
1204
1205 if (&constraint->getRigidBodyA()==bodies[b])
1206 {
1207 found = true;
1208 break;
1209 }
1210 }
1211 btAssert(found);
1212 }
1213 if (!constraint->getRigidBodyB().isStaticOrKinematicObject())
1214 {
1215 bool found=false;
1216 for (int b=0;b<numBodies;b++)
1217 {
1218 if (&constraint->getRigidBodyB()==bodies[b])
1219 {
1220 found = true;
1221 break;
1222 }
1223 }
1224 btAssert(found);
1225 }
1226 }
1227 }
1228 //make sure that dynamic bodies exist for all contact manifolds
1229 for (int i=0;i<numManifolds;i++)
1230 {
1231 if (!manifoldPtr[i]->getBody0()->isStaticOrKinematicObject())
1232 {
1233 bool found=false;
1234 for (int b=0;b<numBodies;b++)
1235 {
1236
1237 if (manifoldPtr[i]->getBody0()==bodies[b])
1238 {
1239 found = true;
1240 break;
1241 }
1242 }
1243 btAssert(found);
1244 }
1245 if (!manifoldPtr[i]->getBody1()->isStaticOrKinematicObject())
1246 {
1247 bool found=false;
1248 for (int b=0;b<numBodies;b++)
1249 {
1250 if (manifoldPtr[i]->getBody1()==bodies[b])
1251 {
1252 found = true;
1253 break;
1254 }
1255 }
1256 btAssert(found);
1257 }
1258 }
1259 #endif //BT_ADDITIONAL_DEBUG
1260
1261
1262 for (int i = 0; i < numBodies; i++)
1263 {
1264 bodies[i]->setCompanionId(-1);
1265 }
1266
1267
1268 m_tmpSolverBodyPool.reserve(numBodies+1);
1269 m_tmpSolverBodyPool.resize(0);
1270
1271 //btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
1272 //initSolverBody(&fixedBody,0);
1273
1274 //convert all bodies
1275
1276
1277 for (int i=0;i<numBodies;i++)
1278 {
1279 int bodyId = getOrInitSolverBody(*bodies[i],infoGlobal.m_timeStep);
1280
1281 btRigidBody* body = btRigidBody::upcast(bodies[i]);
1282 if (body && body->getInvMass())
1283 {
1284 btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
1285 btVector3 gyroForce (0,0,0);
1286 if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT)
1287 {
1288 gyroForce = body->computeGyroscopicForceExplicit(infoGlobal.m_maxGyroscopicForce);
1289 solverBody.m_externalTorqueImpulse -= gyroForce*body->getInvInertiaTensorWorld()*infoGlobal.m_timeStep;
1290 }
1291 if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD)
1292 {
1293 gyroForce = body->computeGyroscopicImpulseImplicit_World(infoGlobal.m_timeStep);
1294 solverBody.m_externalTorqueImpulse += gyroForce;
1295 }
1296 if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY)
1297 {
1298 gyroForce = body->computeGyroscopicImpulseImplicit_Body(infoGlobal.m_timeStep);
1299 solverBody.m_externalTorqueImpulse += gyroForce;
1300
1301 }
1302
1303
1304 }
1305 }
1306
1307 if (1)
1308 {
1309 int j;
1310 for (j=0;j<numConstraints;j++)
1311 {
1312 btTypedConstraint* constraint = constraints[j];
1313 constraint->buildJacobian();
1314 constraint->internalSetAppliedImpulse(0.0f);
1315 }
1316 }
1317
1318 //btRigidBody* rb0=0,*rb1=0;
1319
1320 //if (1)
1321 {
1322 {
1323
1324 int totalNumRows = 0;
1325 int i;
1326
1327 m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints);
1328 //calculate the total number of contraint rows
1329 for (i=0;i<numConstraints;i++)
1330 {
1331 btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
1332 btJointFeedback* fb = constraints[i]->getJointFeedback();
1333 if (fb)
1334 {
1335 fb->m_appliedForceBodyA.setZero();
1336 fb->m_appliedTorqueBodyA.setZero();
1337 fb->m_appliedForceBodyB.setZero();
1338 fb->m_appliedTorqueBodyB.setZero();
1339 }
1340
1341 if (constraints[i]->isEnabled())
1342 {
1343 }
1344 if (constraints[i]->isEnabled())
1345 {
1346 constraints[i]->getInfo1(&info1);
1347 } else
1348 {
1349 info1.m_numConstraintRows = 0;
1350 info1.nub = 0;
1351 }
1352 totalNumRows += info1.m_numConstraintRows;
1353 }
1354 m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows);
1355
1356
1358 int currentRow = 0;
1359
1360 for (i=0;i<numConstraints;i++)
1361 {
1362 const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
1363
1364 if (info1.m_numConstraintRows)
1365 {
1366 btAssert(currentRow<totalNumRows);
1367
1368 btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];
1369 btTypedConstraint* constraint = constraints[i];
1370 btRigidBody& rbA = constraint->getRigidBodyA();
1371 btRigidBody& rbB = constraint->getRigidBodyB();
1372
1373 int solverBodyIdA = getOrInitSolverBody(rbA,infoGlobal.m_timeStep);
1374 int solverBodyIdB = getOrInitSolverBody(rbB,infoGlobal.m_timeStep);
1375
1376 btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
1377 btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
1378
1379
1380
1381
1382 int overrideNumSolverIterations = constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations;
1383 if (overrideNumSolverIterations>m_maxOverrideNumSolverIterations)
1384 m_maxOverrideNumSolverIterations = overrideNumSolverIterations;
1385
1386
1387 int j;
1388 for ( j=0;j<info1.m_numConstraintRows;j++)
1389 {
1390 memset(&currentConstraintRow[j],0,sizeof(btSolverConstraint));
1391 currentConstraintRow[j].m_lowerLimit = -SIMD_INFINITY;
1392 currentConstraintRow[j].m_upperLimit = SIMD_INFINITY;
1393 currentConstraintRow[j].m_appliedImpulse = 0.f;
1394 currentConstraintRow[j].m_appliedPushImpulse = 0.f;
1395 currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA;
1396 currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB;
1397 currentConstraintRow[j].m_overrideNumSolverIterations = overrideNumSolverIterations;
1398 }
1399
1400 bodyAPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
1401 bodyAPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
1402 bodyAPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f);
1403 bodyAPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
1404 bodyBPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
1405 bodyBPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
1406 bodyBPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f);
1407 bodyBPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
1408
1409
1410 btTypedConstraint::btConstraintInfo2 info2;
1411 info2.fps = 1.f/infoGlobal.m_timeStep;
1412 info2.erp = infoGlobal.m_erp;
1413 info2.m_J1linearAxis = currentConstraintRow->m_contactNormal1;
1414 info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal;
1415 info2.m_J2linearAxis = currentConstraintRow->m_contactNormal2;
1416 info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal;
1417 info2.rowskip = sizeof(btSolverConstraint)/sizeof(btScalar);//check this
1419 btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint));
1420 info2.m_constraintError = &currentConstraintRow->m_rhs;
1421 currentConstraintRow->m_cfm = infoGlobal.m_globalCfm;
1422 info2.m_damping = infoGlobal.m_damping;
1423 info2.cfm = &currentConstraintRow->m_cfm;
1424 info2.m_lowerLimit = &currentConstraintRow->m_lowerLimit;
1425 info2.m_upperLimit = &currentConstraintRow->m_upperLimit;
1426 info2.m_numIterations = infoGlobal.m_numIterations;
1427 constraints[i]->getInfo2(&info2);
1428
1430 for ( j=0;j<info1.m_numConstraintRows;j++)
1431 {
1432 btSolverConstraint& solverConstraint = currentConstraintRow[j];
1433
1434 if (solverConstraint.m_upperLimit>=constraints[i]->getBreakingImpulseThreshold())
1435 {
1436 solverConstraint.m_upperLimit = constraints[i]->getBreakingImpulseThreshold();
1437 }
1438
1439 if (solverConstraint.m_lowerLimit<=-constraints[i]->getBreakingImpulseThreshold())
1440 {
1441 solverConstraint.m_lowerLimit = -constraints[i]->getBreakingImpulseThreshold();
1442 }
1443
1444 solverConstraint.m_originalContactPoint = constraint;
1445
1446 {
1447 const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
1448 solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld()*ftorqueAxis1*constraint->getRigidBodyA().getAngularFactor();
1449 }
1450 {
1451 const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal;
1452 solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld()*ftorqueAxis2*constraint->getRigidBodyB().getAngularFactor();
1453 }
1454
1455 {
1456 btVector3 iMJlA = solverConstraint.m_contactNormal1*rbA.getInvMass();
1457 btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal;
1458 btVector3 iMJlB = solverConstraint.m_contactNormal2*rbB.getInvMass();//sign of normal?
1459 btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal;
1460
1461 btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal1);
1462 sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
1463 sum += iMJlB.dot(solverConstraint.m_contactNormal2);
1464 sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
1465 btScalar fsum = btFabs(sum);
1466 btAssert(fsum > SIMD_EPSILON);
1467 solverConstraint.m_jacDiagABInv = fsum>SIMD_EPSILON?btScalar(1.)/sum : 0.f;
1468 }
1469
1470
1471
1472 {
1473 btScalar rel_vel;
1474 btVector3 externalForceImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalForceImpulse : btVector3(0,0,0);
1475 btVector3 externalTorqueImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalTorqueImpulse : btVector3(0,0,0);
1476
1477 btVector3 externalForceImpulseB = bodyBPtr->m_originalBody ? bodyBPtr->m_externalForceImpulse : btVector3(0,0,0);
1478 btVector3 externalTorqueImpulseB = bodyBPtr->m_originalBody ?bodyBPtr->m_externalTorqueImpulse : btVector3(0,0,0);
1479
1480 btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(rbA.getLinearVelocity()+externalForceImpulseA)
1481 + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity()+externalTorqueImpulseA);
1482
1483 btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(rbB.getLinearVelocity()+externalForceImpulseB)
1484 + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity()+externalTorqueImpulseB);
1485
1486 rel_vel = vel1Dotn+vel2Dotn;
1487 btScalar restitution = 0.f;
1488 btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2
1489 btScalar velocityError = restitution - rel_vel * info2.m_damping;
1490 btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
1491 btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
1492 solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
1493 solverConstraint.m_appliedImpulse = 0.f;
1494
1495
1496 }
1497 }
1498 }
1499 currentRow+=m_tmpConstraintSizesPool[i].m_numConstraintRows;
1500 }
1501 }
1502
1503 convertContacts(manifoldPtr,numManifolds,infoGlobal);
1504
1505 }
1506
1507 // btContactSolverInfo info = infoGlobal;
1508
1509
1510 int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
1511 int numConstraintPool = m_tmpSolverContactConstraintPool.size();
1512 int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
1513
1515 m_orderNonContactConstraintPool.resizeNoInitialize(numNonContactPool);
1516 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
1517 m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool*2);
1518 else
1519 m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool);
1520
1521 m_orderFrictionConstraintPool.resizeNoInitialize(numFrictionPool);
1522 {
1523 int i;
1524 for (i=0;i<numNonContactPool;i++)
1525 {
1526 m_orderNonContactConstraintPool[i] = i;
1527 }
1528 for (i=0;i<numConstraintPool;i++)
1529 {
1530 m_orderTmpConstraintPool[i] = i;
1531 }
1532 for (i=0;i<numFrictionPool;i++)
1533 {
1534 m_orderFrictionConstraintPool[i] = i;
1535 }
1536 }
1537
1538 return 0.f;
1539
1540 }
1541
1542
1543 btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/)
1544 {
1545
1546 int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
1547 int numConstraintPool = m_tmpSolverContactConstraintPool.size();
1548 int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
1549
1550 if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
1551 {
1552 if (1) // uncomment this for a bit less random ((iteration & 7) == 0)
1553 {
1554
1555 for (int j=0; j<numNonContactPool; ++j) {
1556 int tmp = m_orderNonContactConstraintPool[j];
1557 int swapi = btRandInt2(j+1);
1558 m_orderNonContactConstraintPool[j] = m_orderNonContactConstraintPool[swapi];
1559 m_orderNonContactConstraintPool[swapi] = tmp;
1560 }
1561
1562 //contact/friction constraints are not solved more than
1563 if (iteration< infoGlobal.m_numIterations)
1564 {
1565 for (int j=0; j<numConstraintPool; ++j) {
1566 int tmp = m_orderTmpConstraintPool[j];
1567 int swapi = btRandInt2(j+1);
1568 m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
1569 m_orderTmpConstraintPool[swapi] = tmp;
1570 }
1571
1572 for (int j=0; j<numFrictionPool; ++j) {
1573 int tmp = m_orderFrictionConstraintPool[j];
1574 int swapi = btRandInt2(j+1);
1575 m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
1576 m_orderFrictionConstraintPool[swapi] = tmp;
1577 }
1578 }
1579 }
1580 }
1581
1582 if (infoGlobal.m_solverMode & SOLVER_SIMD)
1583 {
1585 for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
1586 {
1587 btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
1588 if (iteration < constraint.m_overrideNumSolverIterations)
1589 resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
1590 }
1591
1592 if (iteration< infoGlobal.m_numIterations)
1593 {
1594 for (int j=0;j<numConstraints;j++)
1595 {
1596 if (constraints[j]->isEnabled())
1597 {
1598 int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep);
1599 int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
1600 btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
1601 btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
1602 constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
1603 }
1604 }
1605
1607 if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS)
1608 {
1609 int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1610 int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)? 2 : 1;
1611
1612 for (int c=0;c<numPoolConstraints;c++)
1613 {
1614 btScalar totalImpulse =0;
1615
1616 {
1617 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]];
1618 resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
1619 totalImpulse = solveManifold.m_appliedImpulse;
1620 }
1621 bool applyFriction = true;
1622 if (applyFriction)
1623 {
1624 {
1625
1626 btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier]];
1627
1628 if (totalImpulse>btScalar(0))
1629 {
1630 solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
1631 solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
1632
1633 resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
1634 }
1635 }
1636
1637 if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)
1638 {
1639
1640 btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier+1]];
1641
1642 if (totalImpulse>btScalar(0))
1643 {
1644 solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
1645 solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
1646
1647 resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
1648 }
1649 }
1650 }
1651 }
1652
1653 }
1654 else//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
1655 {
1656 //solve the friction constraints after all contact constraints, don't interleave them
1657 int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1658 int j;
1659
1660 for (j=0;j<numPoolConstraints;j++)
1661 {
1662 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
1663 resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
1664
1665 }
1666
1667
1668
1670
1671 int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
1672 for (j=0;j<numFrictionPoolConstraints;j++)
1673 {
1674 btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
1675 btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
1676
1677 if (totalImpulse>btScalar(0))
1678 {
1679 solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
1680 solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
1681
1682 resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
1683 }
1684 }
1685
1686
1687 int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
1688 for (j=0;j<numRollingFrictionPoolConstraints;j++)
1689 {
1690
1691 btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
1692 btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
1693 if (totalImpulse>btScalar(0))
1694 {
1695 btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
1696 if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
1697 rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
1698
1699 rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
1700 rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
1701
1702 resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
1703 }
1704 }
1705
1706
1707 }
1708 }
1709 } else
1710 {
1711 //non-SIMD version
1713 for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
1714 {
1715 btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
1716 if (iteration < constraint.m_overrideNumSolverIterations)
1717 resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
1718 }
1719
1720 if (iteration< infoGlobal.m_numIterations)
1721 {
1722 for (int j=0;j<numConstraints;j++)
1723 {
1724 if (constraints[j]->isEnabled())
1725 {
1726 int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep);
1727 int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
1728 btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
1729 btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
1730 constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
1731 }
1732 }
1734 int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1735 for (int j=0;j<numPoolConstraints;j++)
1736 {
1737 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
1738 resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
1739 }
1741 int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
1742 for (int 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 resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
1753 }
1754 }
1755
1756 int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
1757 for (int j=0;j<numRollingFrictionPoolConstraints;j++)
1758 {
1759 btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
1760 btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
1761 if (totalImpulse>btScalar(0))
1762 {
1763 btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
1764 if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
1765 rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
1766
1767 rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
1768 rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
1769
1770 resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
1771 }
1772 }
1773 }
1774 }
1775 return 0.f;
1776 }
1777
1778
1779 void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
1780 {
1781 int iteration;
1782 if (infoGlobal.m_splitImpulse)
1783 {
1784 if (infoGlobal.m_solverMode & SOLVER_SIMD)
1785 {
1786 for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
1787 {
1788 {
1789 int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1790 int j;
1791 for (j=0;j<numPoolConstraints;j++)
1792 {
1793 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
1794
1795 resolveSplitPenetrationSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
1796 }
1797 }
1798 }
1799 }
1800 else
1801 {
1802 for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
1803 {
1804 {
1805 int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1806 int j;
1807 for (j=0;j<numPoolConstraints;j++)
1808 {
1809 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
1810
1811 resolveSplitPenetrationImpulseCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
1812 }
1813 }
1814 }
1815 }
1816 }
1817 }
1818
1819 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
1820 {
1821 BT_PROFILE("solveGroupCacheFriendlyIterations");
1822
1823 {
1825 solveGroupCacheFriendlySplitImpulseIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
1826
1827 int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations;
1828
1829 for ( int iteration = 0 ; iteration< maxIterations ; iteration++)
1830 //for ( int iteration = maxIterations-1 ; iteration >= 0;iteration--)
1831 {
1832 solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
1833 }
1834
1835 }
1836 return 0.f;
1837 }
1838
1839 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal)
1840 {
1841 int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1842 int i,j;
1843
1844 if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
1845 {
1846 for (j=0;j<numPoolConstraints;j++)
1847 {
1848 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j];
1849 btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
1850 btAssert(pt);
1851 pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
1852 // float f = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
1853 // printf("pt->m_appliedImpulseLateral1 = %f\n", f);
1854 pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
1855 //printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
1856 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
1857 {
1858 pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
1859 }
1860 //do a callback here?
1861 }
1862 }
1863
1864 numPoolConstraints = m_tmpSolverNonContactConstraintPool.size();
1865 for (j=0;j<numPoolConstraints;j++)
1866 {
1867 const btSolverConstraint& solverConstr = m_tmpSolverNonContactConstraintPool[j];
1868 btTypedConstraint* constr = (btTypedConstraint*)solverConstr.m_originalContactPoint;
1869 btJointFeedback* fb = constr->getJointFeedback();
1870 if (fb)
1871 {
1872 fb->m_appliedForceBodyA += solverConstr.m_contactNormal1*solverConstr.m_appliedImpulse*constr->getRigidBodyA().getLinearFactor()/infoGlobal.m_timeStep;
1873 fb->m_appliedForceBodyB += solverConstr.m_contactNormal2*solverConstr.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/infoGlobal.m_timeStep;
1874 fb->m_appliedTorqueBodyA += solverConstr.m_relpos1CrossNormal* constr->getRigidBodyA().getAngularFactor()*solverConstr.m_appliedImpulse/infoGlobal.m_timeStep;
1875 fb->m_appliedTorqueBodyB += solverConstr.m_relpos2CrossNormal* constr->getRigidBodyB().getAngularFactor()*solverConstr.m_appliedImpulse/infoGlobal.m_timeStep; /*RGM ???? */
1876
1877 }
1878
1879 constr->internalSetAppliedImpulse(solverConstr.m_appliedImpulse);
1880 if (btFabs(solverConstr.m_appliedImpulse)>=constr->getBreakingImpulseThreshold())
1881 {
1882 constr->setEnabled(false);
1883 }
1884 }
1885
1886
1887
1888 for ( i=0;i<m_tmpSolverBodyPool.size();i++)
1889 {
1890 btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody;
1891 if (body)
1892 {
1893 if (infoGlobal.m_splitImpulse)
1894 m_tmpSolverBodyPool[i].writebackVelocityAndTransform(infoGlobal.m_timeStep, infoGlobal.m_splitImpulseTurnErp);
1895 else
1896 m_tmpSolverBodyPool[i].writebackVelocity();
1897
1898 m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(
1899 m_tmpSolverBodyPool[i].m_linearVelocity+
1900 m_tmpSolverBodyPool[i].m_externalForceImpulse);
1901
1902 m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(
1903 m_tmpSolverBodyPool[i].m_angularVelocity+
1904 m_tmpSolverBodyPool[i].m_externalTorqueImpulse);
1905
1906 if (infoGlobal.m_splitImpulse)
1907 m_tmpSolverBodyPool[i].m_originalBody->setWorldTransform(m_tmpSolverBodyPool[i].m_worldTransform);
1908
1909 m_tmpSolverBodyPool[i].m_originalBody->setCompanionId(-1);
1910 }
1911 }
1912
1913 m_tmpSolverContactConstraintPool.resizeNoInitialize(0);
1914 m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0);
1915 m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize(0);
1916 m_tmpSolverContactRollingFrictionConstraintPool.resizeNoInitialize(0);
1917
1918 m_tmpSolverBodyPool.resizeNoInitialize(0);
1919 return 0.f;
1920 }
1921
1922
1923
1925 btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btDispatcher* /*dispatcher*/)
1926 {
1927
1928 BT_PROFILE("solveGroup");
1929 //you need to provide at least some bodies
1930
1931 solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer);
1932
1933 solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer);
1934
1935 solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal);
1936
1937 return 0.f;
1938 }
1939
1940 void btSequentialImpulseConstraintSolver::reset()
1941 {
1942 m_btSeed2 = 0;
1943 }
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:54
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:388
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:1839
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:1925
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:197
btSequentialImpulseConstraintSolver::getSSE2ConstraintRowSolverLowerLimit
btSingleConstraintRowSolver getSSE2ConstraintRowSolverLowerLimit()
btSequentialImpulseConstraintSolver::m_resolveSingleConstraintRowGeneric
btSingleConstraintRowSolver m_resolveSingleConstraintRowGeneric
Definition: btSequentialImpulseConstraintSolver.h:49
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:1543
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:392
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:397
btRigidBody.h
btSequentialImpulseConstraintSolver::initSolverBody
void initSolverBody(btSolverBody *solverBody, btCollisionObject *collisionObject, btScalar timeStep)
Definition: btSequentialImpulseConstraintSolver.cpp:462
btTypedConstraint::getBreakingImpulseThreshold
btScalar getBreakingImpulseThreshold() const
Definition: btTypedConstraint.h:197
btCollisionObject::isStaticOrKinematicObject
bool isStaticOrKinematicObject() const
Definition: btCollisionObject.h:202
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:1819
btRigidBody::getAngularFactor
const btVector3 & getAngularFactor() const
Definition: btRigidBody.h:504
btSequentialImpulseConstraintSolver::restitutionCurve
btScalar restitutionCurve(btScalar rel_vel, btScalar restitution)
Definition: btSequentialImpulseConstraintSolver.cpp:505
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:1779
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:157
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:717
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:424
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:630
SIMD_INFINITY
#define SIMD_INFINITY
Definition: btScalar.h:496
btCollisionObject::getWorldTransform
btTransform & getWorldTransform()
Definition: btCollisionObject.h:367
btManifoldPoint::m_normalWorldOnB
btVector3 m_normalWorldOnB
Definition: btManifoldPoint.h:99
btContactSolverInfoData::m_damping
btScalar m_damping
Definition: btContactSolverInfo.h:40
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:956
btCollisionObject::getAnisotropicFriction
const btVector3 & getAnisotropicFriction() const
Definition: btCollisionObject.h:167
btManifoldPoint::getPositionWorldOnB
const btVector3 & getPositionWorldOnB() const
Definition: btManifoldPoint.h:159
btSequentialImpulseConstraintSolver::btRandInt2
int btRandInt2(int n)
Definition: btSequentialImpulseConstraintSolver.cpp:433
btSequentialImpulseConstraintSolver::convertContacts
virtual void convertContacts(btPersistentManifold **manifoldPtr, int numManifolds, const btContactSolverInfo &infoGlobal)
Definition: btSequentialImpulseConstraintSolver.cpp:1170
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:453
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:620
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:1002
btRigidBody::getVelocityInLocalPoint
btVector3 getVelocityInLocalPoint(const btVector3 &rel_pos) const
Definition: btRigidBody.h:382
BT_PROFILE
#define BT_PROFILE(name)
Definition: btQuickprof.h:203
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:533
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
btSequentialImpulseConstraintSolver::resolveSplitPenetrationSIMD
void resolveSplitPenetrationSIMD(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
Definition: btSequentialImpulseConstraintSolver.cpp:330
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:1184
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:707
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:177
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:757
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:1940
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:75
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
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:69
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:50
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:513
btCollisionObject::getCompanionId
int getCompanionId() const
Definition: btCollisionObject.h:448
btManifoldPoint::getDistance
btScalar getDistance() const
Definition: btManifoldPoint.h:145
btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver
btSequentialImpulseConstraintSolver()
Definition: btSequentialImpulseConstraintSolver.cpp:366
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
btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFriendly
void resolveSplitPenetrationImpulseCacheFriendly(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
Definition: btSequentialImpulseConstraintSolver.cpp:301
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
btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION
Definition: btCollisionObject.h:158
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