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

Bullet Collision Detection & Physics Library

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