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
43 ///This is the scalar reference implementation of solving a single constraint row, the innerloop of the Projected Gauss Seidel/Sequential Impulse constraint solver
44 ///Below are optional SSE2 and SSE4/FMA3 versions. We assume most hardware has SSE2. For SSE4/FMA3 we perform a CPU feature check.
gResolveSingleConstraintRowGeneric_scalar_reference(btSolverBody & body1,btSolverBody & body2,const btSolverConstraint & c)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
gResolveSingleConstraintRowLowerLimit_scalar_reference(btSolverBody & body1,btSolverBody & body2,const btSolverConstraint & c)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))
btSimdDot3(__m128 vec0,__m128 vec1)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
gResolveSingleConstraintRowGeneric_sse2(btSolverBody & body1,btSolverBody & body2,const btSolverConstraint & c)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
gResolveSingleConstraintRowGeneric_sse4_1_fma3(btSolverBody & body1,btSolverBody & body2,const btSolverConstraint & c)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
gResolveSingleConstraintRowLowerLimit_sse2(btSolverBody & body1,btSolverBody & body2,const btSolverConstraint & c)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
gResolveSingleConstraintRowLowerLimit_sse4_1_fma3(btSolverBody & body1,btSolverBody & body2,const btSolverConstraint & c)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
resolveSingleConstraintRowGenericSIMD(btSolverBody & body1,btSolverBody & body2,const btSolverConstraint & c)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
resolveSingleConstraintRowGeneric(btSolverBody & body1,btSolverBody & body2,const btSolverConstraint & c)280 btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
281 {
282 return gResolveSingleConstraintRowGeneric_scalar_reference(body1, body2, c);
283 }
284
resolveSingleConstraintRowLowerLimitSIMD(btSolverBody & body1,btSolverBody & body2,const btSolverConstraint & c)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
resolveSingleConstraintRowLowerLimit(btSolverBody & body1,btSolverBody & body2,const btSolverConstraint & c)295 btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
296 {
297 return gResolveSingleConstraintRowLowerLimit_scalar_reference(body1,body2,c);
298 }
299
300
resolveSplitPenetrationImpulseCacheFriendly(btSolverBody & body1,btSolverBody & body2,const btSolverConstraint & c)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
resolveSplitPenetrationSIMD(btSolverBody & body1,btSolverBody & body2,const btSolverConstraint & c)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
btSequentialImpulseConstraintSolver()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
~btSequentialImpulseConstraintSolver()388 btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
389 {
390 }
391
getScalarConstraintRowSolverGeneric()392 btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getScalarConstraintRowSolverGeneric()
393 {
394 return gResolveSingleConstraintRowGeneric_scalar_reference;
395 }
396
getScalarConstraintRowSolverLowerLimit()397 btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getScalarConstraintRowSolverLowerLimit()
398 {
399 return gResolveSingleConstraintRowLowerLimit_scalar_reference;
400 }
401
402
403 #ifdef USE_SIMD
getSSE2ConstraintRowSolverGeneric()404 btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE2ConstraintRowSolverGeneric()
405 {
406 return gResolveSingleConstraintRowGeneric_sse2;
407 }
getSSE2ConstraintRowSolverLowerLimit()408 btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE2ConstraintRowSolverLowerLimit()
409 {
410 return gResolveSingleConstraintRowLowerLimit_sse2;
411 }
412 #ifdef BT_ALLOW_SSE4
getSSE4_1ConstraintRowSolverGeneric()413 btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE4_1ConstraintRowSolverGeneric()
414 {
415 return gResolveSingleConstraintRowGeneric_sse4_1_fma3;
416 }
getSSE4_1ConstraintRowSolverLowerLimit()417 btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE4_1ConstraintRowSolverLowerLimit()
418 {
419 return gResolveSingleConstraintRowLowerLimit_sse4_1_fma3;
420 }
421 #endif //BT_ALLOW_SSE4
422 #endif //USE_SIMD
423
btRand2()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)
btRandInt2(int n)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
initSolverBody(btSolverBody * solverBody,btCollisionObject * collisionObject,btScalar timeStep)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
restitutionCurve(btScalar rel_vel,btScalar restitution)505 btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution)
506 {
507 btScalar rest = restitution * -rel_vel;
508 return rest;
509 }
510
511
512
applyAnisotropicFriction(btCollisionObject * colObj,btVector3 & frictionDirection,int frictionMode)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
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)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
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)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
setupRollingFrictionConstraint(btSolverConstraint & solverConstraint,const btVector3 & normalAxis1,int solverBodyIdA,int solverBodyIdB,btManifoldPoint & cp,const btVector3 & rel_pos1,const btVector3 & rel_pos2,btCollisionObject * colObj0,btCollisionObject * colObj1,btScalar relaxation,btScalar desiredVelocity,btScalar cfmSlip)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
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)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
getOrInitSolverBody(btCollisionObject & body,btScalar timeStep)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
setupContactConstraint(btSolverConstraint & solverConstraint,int solverBodyIdA,int solverBodyIdB,btManifoldPoint & cp,const btContactSolverInfo & infoGlobal,btScalar & relaxation,const btVector3 & rel_pos1,const btVector3 & rel_pos2)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
854 ///warm starting (or zero if disabled)
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
setFrictionConstraintImpulse(btSolverConstraint & solverConstraint,int solverBodyIdA,int solverBodyIdB,btManifoldPoint & cp,const btContactSolverInfo & infoGlobal)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
convertContact(btPersistentManifold * manifold,const btContactSolverInfo & infoGlobal)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
994 ///avoid collision response between two static objects
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
1042 /////setup the friction constraints
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
1082 ///Bullet has several options to set the friction directions
1083 ///By default, each contact has only a single friction direction that is recomputed automatically very frame
1084 ///based on the relative linear velocity.
1085 ///If the relative velocity it zero, it will automatically compute a friction direction.
1086
1087 ///You can also enable two friction directions, using the SOLVER_USE_2_FRICTION_DIRECTIONS.
1088 ///In that case, the second friction direction will be orthogonal to both contact normal and first friction direction.
1089 ///
1090 ///If you choose SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION, then the friction will be independent from the relative projected velocity.
1091 ///
1092 ///The user can manually override the friction directions for certain contacts using a contact callback,
1093 ///and set the cp.m_lateralFrictionInitialized to true
1094 ///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2)
1095 ///this will give a conveyor belt effect
1096 ///
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
convertContacts(btPersistentManifold ** manifoldPtr,int numManifolds,const btContactSolverInfo & infoGlobal)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
solveGroupCacheFriendlySetup(btCollisionObject ** bodies,int numBodies,btPersistentManifold ** manifoldPtr,int numManifolds,btTypedConstraint ** constraints,int numConstraints,const btContactSolverInfo & infoGlobal,btIDebugDraw * debugDrawer)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
1343 ///setup the btSolverConstraints
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(¤tConstraintRow[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
1404 ///the size of btSolverConstraint needs be a multiple of btScalar
1405 btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint));
1406 info2.m_constraintError = ¤tConstraintRow->m_rhs;
1407 currentConstraintRow->m_cfm = infoGlobal.m_globalCfm;
1408 info2.m_damping = infoGlobal.m_damping;
1409 info2.cfm = ¤tConstraintRow->m_cfm;
1410 info2.m_lowerLimit = ¤tConstraintRow->m_lowerLimit;
1411 info2.m_upperLimit = ¤tConstraintRow->m_upperLimit;
1412 info2.m_numIterations = infoGlobal.m_numIterations;
1413 constraints[i]->getInfo2(&info2);
1414
1415 ///finalize the constraint setup
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
1500 ///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
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
solveSingleIteration(int iteration,btCollisionObject **,int,btPersistentManifold **,int,btTypedConstraint ** constraints,int numConstraints,const btContactSolverInfo & infoGlobal,btIDebugDraw *)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 {
1570 ///solve all joint constraints, using SIMD, if available
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
1592 ///solve all contact constraints using SIMD, if available
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
1655 ///solve all friction constraints, using SIMD, if available
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
1698 ///solve all joint constraints
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 }
1719 ///solve all contact constraints
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 }
1726 ///solve all friction constraints
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
solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject ** bodies,int numBodies,btPersistentManifold ** manifoldPtr,int numManifolds,btTypedConstraint ** constraints,int numConstraints,const btContactSolverInfo & infoGlobal,btIDebugDraw * debugDrawer)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
solveGroupCacheFriendlyIterations(btCollisionObject ** bodies,int numBodies,btPersistentManifold ** manifoldPtr,int numManifolds,btTypedConstraint ** constraints,int numConstraints,const btContactSolverInfo & infoGlobal,btIDebugDraw * debugDrawer)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 {
1810 ///this is a special step to resolve penetrations (just for contacts)
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
solveGroupCacheFriendlyFinish(btCollisionObject ** bodies,int numBodies,const btContactSolverInfo & infoGlobal)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
1910 /// btSequentialImpulseConstraintSolver Sequentially applies impulses
solveGroup(btCollisionObject ** bodies,int numBodies,btPersistentManifold ** manifoldPtr,int numManifolds,btTypedConstraint ** constraints,int numConstraints,const btContactSolverInfo & infoGlobal,btIDebugDraw * debugDrawer,btDispatcher *)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
reset()1926 void btSequentialImpulseConstraintSolver::reset()
1927 {
1928 m_btSeed2 = 0;
1929 }
1930