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

Bullet Collision Detection & Physics Library

- Main Page
- Namespaces
- Classes
- Files

- File List
- File Members

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