Merge branch 'blender-v2.81-release'
[blender.git] / extern / bullet2 / src / BulletDynamics / ConstraintSolver / btSequentialImpulseConstraintSolver.cpp
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.
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
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
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
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
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
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(&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
1404                                         ///the size of btSolverConstraint needs be a multiple of btScalar
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
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
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
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         {
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
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
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 }