bullet: Update to current svn, r2636
[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 "btJacobianEntry.h"
26 #include "LinearMath/btMinMax.h"
27 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
28 #include <new>
29 #include "LinearMath/btStackAlloc.h"
30 #include "LinearMath/btQuickprof.h"
31 //#include "btSolverBody.h"
32 //#include "btSolverConstraint.h"
33 #include "LinearMath/btAlignedObjectArray.h"
34 #include <string.h> //for memset
35
36 int             gNumSplitImpulseRecoveries = 0;
37
38 #include "BulletDynamics/Dynamics/btRigidBody.h"
39
40 btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
41 :m_btSeed2(0)
42 {
43
44 }
45
46 btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
47 {
48 }
49
50 #ifdef USE_SIMD
51 #include <emmintrin.h>
52 #define btVecSplat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e,e,e,e))
53 static inline __m128 btSimdDot3( __m128 vec0, __m128 vec1 )
54 {
55         __m128 result = _mm_mul_ps( vec0, vec1);
56         return _mm_add_ps( btVecSplat( result, 0 ), _mm_add_ps( btVecSplat( result, 1 ), btVecSplat( result, 2 ) ) );
57 }
58 #endif//USE_SIMD
59
60 // Project Gauss Seidel or the equivalent Sequential Impulse
61 void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
62 {
63 #ifdef USE_SIMD
64         __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
65         __m128  lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
66         __m128  upperLimit1 = _mm_set1_ps(c.m_upperLimit);
67         __m128 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)));
68         __m128 deltaVel1Dotn    =       _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
69         __m128 deltaVel2Dotn    =       _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128,body2.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128));
70         deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
71         deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
72         btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
73         btSimdScalar resultLowerLess,resultUpperLess;
74         resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
75         resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
76         __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
77         deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
78         c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
79         __m128 upperMinApplied = _mm_sub_ps(upperLimit1,cpAppliedImp);
80         deltaImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied) );
81         c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1) );
82         __m128  linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128,body1.internalGetInvMass().mVec128);
83         __m128  linearComponentB = _mm_mul_ps((c.m_contactNormal2).mVec128,body2.internalGetInvMass().mVec128);
84         __m128 impulseMagnitude = deltaImpulse;
85         body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
86         body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
87         body2.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
88         body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
89 #else
90         resolveSingleConstraintRowGeneric(body1,body2,c);
91 #endif
92 }
93
94 // Project Gauss Seidel or the equivalent Sequential Impulse
95  void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
96 {
97         btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
98         const btScalar deltaVel1Dotn    =       c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity())  + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
99         const btScalar deltaVel2Dotn    =       c.m_contactNormal2.dot(body2.internalGetDeltaLinearVelocity())  + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
100
101 //      const btScalar delta_rel_vel    =       deltaVel1Dotn-deltaVel2Dotn;
102         deltaImpulse    -=      deltaVel1Dotn*c.m_jacDiagABInv;
103         deltaImpulse    -=      deltaVel2Dotn*c.m_jacDiagABInv;
104
105         const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
106         if (sum < c.m_lowerLimit)
107         {
108                 deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
109                 c.m_appliedImpulse = c.m_lowerLimit;
110         }
111         else if (sum > c.m_upperLimit) 
112         {
113                 deltaImpulse = c.m_upperLimit-c.m_appliedImpulse;
114                 c.m_appliedImpulse = c.m_upperLimit;
115         }
116         else
117         {
118                 c.m_appliedImpulse = sum;
119         }
120
121         body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
122         body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
123 }
124
125  void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
126 {
127 #ifdef USE_SIMD
128         __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
129         __m128  lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
130         __m128  upperLimit1 = _mm_set1_ps(c.m_upperLimit);
131         __m128 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)));
132         __m128 deltaVel1Dotn    =       _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
133         __m128 deltaVel2Dotn    =       _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128,body2.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128));
134         deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
135         deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
136         btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
137         btSimdScalar resultLowerLess,resultUpperLess;
138         resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
139         resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
140         __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
141         deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
142         c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
143         __m128  linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128,body1.internalGetInvMass().mVec128);
144         __m128  linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128,body2.internalGetInvMass().mVec128);
145         __m128 impulseMagnitude = deltaImpulse;
146         body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
147         body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
148         body2.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
149         body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
150 #else
151         resolveSingleConstraintRowLowerLimit(body1,body2,c);
152 #endif
153 }
154
155 // Project Gauss Seidel or the equivalent Sequential Impulse
156  void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
157 {
158         btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
159         const btScalar deltaVel1Dotn    =       c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
160         const btScalar deltaVel2Dotn    =       c.m_contactNormal2.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
161
162         deltaImpulse    -=      deltaVel1Dotn*c.m_jacDiagABInv;
163         deltaImpulse    -=      deltaVel2Dotn*c.m_jacDiagABInv;
164         const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
165         if (sum < c.m_lowerLimit)
166         {
167                 deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
168                 c.m_appliedImpulse = c.m_lowerLimit;
169         }
170         else
171         {
172                 c.m_appliedImpulse = sum;
173         }
174         body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
175         body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
176 }
177
178
179 void    btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFriendly(
180         btSolverBody& body1,
181         btSolverBody& body2,
182         const btSolverConstraint& c)
183 {
184                 if (c.m_rhsPenetration)
185         {
186                         gNumSplitImpulseRecoveries++;
187                         btScalar deltaImpulse = c.m_rhsPenetration-btScalar(c.m_appliedPushImpulse)*c.m_cfm;
188                         const btScalar deltaVel1Dotn    =       c.m_contactNormal1.dot(body1.internalGetPushVelocity())         + c.m_relpos1CrossNormal.dot(body1.internalGetTurnVelocity());
189                         const btScalar deltaVel2Dotn    =       c.m_contactNormal2.dot(body2.internalGetPushVelocity())         + c.m_relpos2CrossNormal.dot(body2.internalGetTurnVelocity());
190
191                         deltaImpulse    -=      deltaVel1Dotn*c.m_jacDiagABInv;
192                         deltaImpulse    -=      deltaVel2Dotn*c.m_jacDiagABInv;
193                         const btScalar sum = btScalar(c.m_appliedPushImpulse) + deltaImpulse;
194                         if (sum < c.m_lowerLimit)
195                         {
196                                 deltaImpulse = c.m_lowerLimit-c.m_appliedPushImpulse;
197                                 c.m_appliedPushImpulse = c.m_lowerLimit;
198                         }
199                         else
200                         {
201                                 c.m_appliedPushImpulse = sum;
202                         }
203                         body1.internalApplyPushImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
204                         body2.internalApplyPushImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
205         }
206 }
207
208  void btSequentialImpulseConstraintSolver::resolveSplitPenetrationSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
209 {
210 #ifdef USE_SIMD
211         if (!c.m_rhsPenetration)
212                 return;
213
214         gNumSplitImpulseRecoveries++;
215
216         __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedPushImpulse);
217         __m128  lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
218         __m128  upperLimit1 = _mm_set1_ps(c.m_upperLimit);
219         __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)));
220         __m128 deltaVel1Dotn    =       _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128,body1.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetTurnVelocity().mVec128));
221         __m128 deltaVel2Dotn    =       _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128,body2.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetTurnVelocity().mVec128));
222         deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
223         deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
224         btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
225         btSimdScalar resultLowerLess,resultUpperLess;
226         resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
227         resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
228         __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
229         deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
230         c.m_appliedPushImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
231         __m128  linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128,body1.internalGetInvMass().mVec128);
232         __m128  linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128,body2.internalGetInvMass().mVec128);
233         __m128 impulseMagnitude = deltaImpulse;
234         body1.internalGetPushVelocity().mVec128 = _mm_add_ps(body1.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
235         body1.internalGetTurnVelocity().mVec128 = _mm_add_ps(body1.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
236         body2.internalGetPushVelocity().mVec128 = _mm_add_ps(body2.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
237         body2.internalGetTurnVelocity().mVec128 = _mm_add_ps(body2.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
238 #else
239         resolveSplitPenetrationImpulseCacheFriendly(body1,body2,c);
240 #endif
241 }
242
243
244
245 unsigned long btSequentialImpulseConstraintSolver::btRand2()
246 {
247         m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff;
248         return m_btSeed2;
249 }
250
251
252
253 //See ODE: adam's all-int straightforward(?) dRandInt (0..n-1)
254 int btSequentialImpulseConstraintSolver::btRandInt2 (int n)
255 {
256         // seems good; xor-fold and modulus
257         const unsigned long un = static_cast<unsigned long>(n);
258         unsigned long r = btRand2();
259
260         // note: probably more aggressive than it needs to be -- might be
261         //       able to get away without one or two of the innermost branches.
262         if (un <= 0x00010000UL) {
263                 r ^= (r >> 16);
264                 if (un <= 0x00000100UL) {
265                         r ^= (r >> 8);
266                         if (un <= 0x00000010UL) {
267                                 r ^= (r >> 4);
268                                 if (un <= 0x00000004UL) {
269                                         r ^= (r >> 2);
270                                         if (un <= 0x00000002UL) {
271                                                 r ^= (r >> 1);
272                                         }
273                                 }
274                         }
275                 }
276         }
277
278         return (int) (r % un);
279 }
280
281
282
283 void    btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject)
284 {
285
286         btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0;
287
288         solverBody->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
289         solverBody->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
290         solverBody->internalGetPushVelocity().setValue(0.f,0.f,0.f);
291         solverBody->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
292
293         if (rb)
294         {
295                 solverBody->m_worldTransform = rb->getWorldTransform();
296                 solverBody->internalSetInvMass(btVector3(rb->getInvMass(),rb->getInvMass(),rb->getInvMass())*rb->getLinearFactor());
297                 solverBody->m_originalBody = rb;
298                 solverBody->m_angularFactor = rb->getAngularFactor();
299                 solverBody->m_linearFactor = rb->getLinearFactor();
300                 solverBody->m_linearVelocity = rb->getLinearVelocity();
301                 solverBody->m_angularVelocity = rb->getAngularVelocity();
302         } else
303         {
304                 solverBody->m_worldTransform.setIdentity();
305                 solverBody->internalSetInvMass(btVector3(0,0,0));
306                 solverBody->m_originalBody = 0;
307                 solverBody->m_angularFactor.setValue(1,1,1);
308                 solverBody->m_linearFactor.setValue(1,1,1);
309                 solverBody->m_linearVelocity.setValue(0,0,0);
310                 solverBody->m_angularVelocity.setValue(0,0,0);
311         }
312
313
314 }
315
316
317
318
319
320
321 btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution)
322 {
323         btScalar rest = restitution * -rel_vel;
324         return rest;
325 }
326
327
328
329 static void     applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode);
330 static void     applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode)
331 {
332         
333
334         if (colObj && colObj->hasAnisotropicFriction(frictionMode))
335         {
336                 // transform to local coordinates
337                 btVector3 loc_lateral = frictionDirection * colObj->getWorldTransform().getBasis();
338                 const btVector3& friction_scaling = colObj->getAnisotropicFriction();
339                 //apply anisotropic friction
340                 loc_lateral *= friction_scaling;
341                 // ... and transform it back to global coordinates
342                 frictionDirection = colObj->getWorldTransform().getBasis() * loc_lateral;
343         }
344
345 }
346
347
348
349
350 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)
351 {
352
353         
354         solverConstraint.m_contactNormal1 = normalAxis;
355         solverConstraint.m_contactNormal2 = -normalAxis;
356         btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
357         btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
358
359         btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody;
360         btRigidBody* body1 = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody;
361
362         solverConstraint.m_solverBodyIdA = solverBodyIdA;
363         solverConstraint.m_solverBodyIdB = solverBodyIdB;
364
365         solverConstraint.m_friction = cp.m_combinedFriction;
366         solverConstraint.m_originalContactPoint = 0;
367
368         solverConstraint.m_appliedImpulse = 0.f;
369         solverConstraint.m_appliedPushImpulse = 0.f;
370
371         {
372                 btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal1);
373                 solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
374                 solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor() : btVector3(0,0,0);
375         }
376         {
377                 btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal2);
378                 solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
379                 solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor() : btVector3(0,0,0);
380         }
381
382         {
383                 btVector3 vec;
384                 btScalar denom0 = 0.f;
385                 btScalar denom1 = 0.f;
386                 if (body0)
387                 {
388                         vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
389                         denom0 = body0->getInvMass() + normalAxis.dot(vec);
390                 }
391                 if (body1)
392                 {
393                         vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
394                         denom1 = body1->getInvMass() + normalAxis.dot(vec);
395                 }
396                 btScalar denom = relaxation/(denom0+denom1);
397                 solverConstraint.m_jacDiagABInv = denom;
398         }
399
400         {
401                 
402
403                 btScalar rel_vel;
404                 btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity:btVector3(0,0,0)) 
405                         + solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0));
406                 btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity:btVector3(0,0,0)) 
407                         + solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0));
408
409                 rel_vel = vel1Dotn+vel2Dotn;
410
411 //              btScalar positionalError = 0.f;
412
413                 btSimdScalar velocityError =  desiredVelocity - rel_vel;
414                 btSimdScalar    velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
415                 solverConstraint.m_rhs = velocityImpulse;
416                 solverConstraint.m_cfm = cfmSlip;
417                 solverConstraint.m_lowerLimit = 0;
418                 solverConstraint.m_upperLimit = 1e10f;
419                 
420         }
421 }
422
423 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)
424 {
425         btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing();
426         solverConstraint.m_frictionIndex = frictionIndex;
427         setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, 
428                                                         colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
429         return solverConstraint;
430 }
431
432
433 void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint(       btSolverConstraint& solverConstraint, const btVector3& normalAxis1,int solverBodyIdA,int  solverBodyIdB,
434                                                                         btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,
435                                                                         btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, 
436                                                                         btScalar desiredVelocity, btScalar cfmSlip)
437
438 {
439         btVector3 normalAxis(0,0,0);
440
441
442         solverConstraint.m_contactNormal1 = normalAxis;
443         solverConstraint.m_contactNormal2 = -normalAxis;
444         btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
445         btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
446
447         btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody;
448         btRigidBody* body1 = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody;
449
450         solverConstraint.m_solverBodyIdA = solverBodyIdA;
451         solverConstraint.m_solverBodyIdB = solverBodyIdB;
452
453         solverConstraint.m_friction = cp.m_combinedRollingFriction;
454         solverConstraint.m_originalContactPoint = 0;
455
456         solverConstraint.m_appliedImpulse = 0.f;
457         solverConstraint.m_appliedPushImpulse = 0.f;
458
459         {
460                 btVector3 ftorqueAxis1 = -normalAxis1;
461                 solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
462                 solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor() : btVector3(0,0,0);
463         }
464         {
465                 btVector3 ftorqueAxis1 = normalAxis1;
466                 solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
467                 solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor() : btVector3(0,0,0);
468         }
469
470
471         {
472                 btVector3 iMJaA = body0?body0->getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal:btVector3(0,0,0);
473                 btVector3 iMJaB = body1?body1->getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal:btVector3(0,0,0);
474                 btScalar sum = 0;
475                 sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
476                 sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
477                 solverConstraint.m_jacDiagABInv = btScalar(1.)/sum;
478         }
479
480         {
481                 
482
483                 btScalar rel_vel;
484                 btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity:btVector3(0,0,0)) 
485                         + solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0));
486                 btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity:btVector3(0,0,0)) 
487                         + solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0));
488
489                 rel_vel = vel1Dotn+vel2Dotn;
490
491 //              btScalar positionalError = 0.f;
492
493                 btSimdScalar velocityError =  desiredVelocity - rel_vel;
494                 btSimdScalar    velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
495                 solverConstraint.m_rhs = velocityImpulse;
496                 solverConstraint.m_cfm = cfmSlip;
497                 solverConstraint.m_lowerLimit = 0;
498                 solverConstraint.m_upperLimit = 1e10f;
499                 
500         }
501 }
502
503
504
505
506
507
508
509
510 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)
511 {
512         btSolverConstraint& solverConstraint = m_tmpSolverContactRollingFrictionConstraintPool.expandNonInitializing();
513         solverConstraint.m_frictionIndex = frictionIndex;
514         setupRollingFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, 
515                                                         colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
516         return solverConstraint;
517 }
518
519
520 int     btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body)
521 {
522
523         int solverBodyIdA = -1;
524
525         if (body.getCompanionId() >= 0)
526         {
527                 //body has already been converted
528                 solverBodyIdA = body.getCompanionId();
529         btAssert(solverBodyIdA < m_tmpSolverBodyPool.size());
530         } else
531         {
532                 btRigidBody* rb = btRigidBody::upcast(&body);
533                 //convert both active and kinematic objects (for their velocity)
534                 if (rb && (rb->getInvMass() || rb->isKinematicObject()))
535                 {
536                         solverBodyIdA = m_tmpSolverBodyPool.size();
537                         btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
538                         initSolverBody(&solverBody,&body);
539                         body.setCompanionId(solverBodyIdA);
540                 } else
541                 {
542                         return 0;//assume first one is a fixed solver body
543                 }
544         }
545
546         return solverBodyIdA;
547
548 }
549 #include <stdio.h>
550
551
552 void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstraint& solverConstraint, 
553                                                                                                                                  int solverBodyIdA, int solverBodyIdB,
554                                                                                                                                  btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
555                                                                                                                                  btVector3& vel, btScalar& rel_vel, btScalar& relaxation,
556                                                                                                                                  btVector3& rel_pos1, btVector3& rel_pos2)
557 {
558                         
559                         const btVector3& pos1 = cp.getPositionWorldOnA();
560                         const btVector3& pos2 = cp.getPositionWorldOnB();
561
562                         btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA];
563                         btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB];
564
565                         btRigidBody* rb0 = bodyA->m_originalBody;
566                         btRigidBody* rb1 = bodyB->m_originalBody;
567
568 //                      btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); 
569 //                      btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
570                         rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); 
571                         rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
572
573                         relaxation = 1.f;
574
575                         btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
576                         solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
577                         btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);            
578                         solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
579
580                                 {
581 #ifdef COMPUTE_IMPULSE_DENOM
582                                         btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
583                                         btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
584 #else                                                   
585                                         btVector3 vec;
586                                         btScalar denom0 = 0.f;
587                                         btScalar denom1 = 0.f;
588                                         if (rb0)
589                                         {
590                                                 vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
591                                                 denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
592                                         }
593                                         if (rb1)
594                                         {
595                                                 vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
596                                                 denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
597                                         }
598 #endif //COMPUTE_IMPULSE_DENOM          
599
600                                         btScalar denom = relaxation/(denom0+denom1);
601                                         solverConstraint.m_jacDiagABInv = denom;
602                                 }
603
604                                 solverConstraint.m_contactNormal1 = cp.m_normalWorldOnB;
605                                 solverConstraint.m_contactNormal2 = -cp.m_normalWorldOnB;
606                                 solverConstraint.m_relpos1CrossNormal = torqueAxis0;
607                                 solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
608
609                                 btScalar restitution = 0.f;
610                                 btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop;
611
612                                 {
613                                         btVector3 vel1,vel2;
614
615                                         vel1 = rb0? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
616                                         vel2 = rb1? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
617
618         //                      btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
619                                         vel  = vel1 - vel2;
620                                         rel_vel = cp.m_normalWorldOnB.dot(vel);
621
622                                         
623
624                                         solverConstraint.m_friction = cp.m_combinedFriction;
625
626                                 
627                                         restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution);
628                                         if (restitution <= btScalar(0.))
629                                         {
630                                                 restitution = 0.f;
631                                         };
632                                 }
633
634
635                                 ///warm starting (or zero if disabled)
636                                 if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
637                                 {
638                                         solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
639                                         if (rb0)
640                                                 bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
641                                         if (rb1)
642                                                 bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
643                                 } else
644                                 {
645                                         solverConstraint.m_appliedImpulse = 0.f;
646                                 }
647
648                                 solverConstraint.m_appliedPushImpulse = 0.f;
649
650                                 {
651                                         btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(rb0?bodyA->m_linearVelocity:btVector3(0,0,0)) 
652                                                 + solverConstraint.m_relpos1CrossNormal.dot(rb0?bodyA->m_angularVelocity:btVector3(0,0,0));
653                                         btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(rb1?bodyB->m_linearVelocity:btVector3(0,0,0)) 
654                                                 + solverConstraint.m_relpos2CrossNormal.dot(rb1?bodyB->m_angularVelocity:btVector3(0,0,0));
655                                         btScalar rel_vel = vel1Dotn+vel2Dotn;
656
657                                         btScalar positionalError = 0.f;
658                                         btScalar        velocityError = restitution - rel_vel;// * damping;
659                                         
660
661                                         btScalar erp = infoGlobal.m_erp2;
662                                         if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
663                                         {
664                                                 erp = infoGlobal.m_erp;
665                                         }
666
667                                         if (penetration>0)
668                                         {
669                                                 positionalError = 0;
670
671                                                 velocityError -= penetration / infoGlobal.m_timeStep;
672                                         } else
673                                         {
674                                                 positionalError = -penetration * erp/infoGlobal.m_timeStep;
675                                         }
676
677                                         btScalar  penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
678                                         btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
679
680                                         if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
681                                         {
682                                                 //combine position and velocity into rhs
683                                                 solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
684                                                 solverConstraint.m_rhsPenetration = 0.f;
685
686                                         } else
687                                         {
688                                                 //split position and velocity into rhs and m_rhsPenetration
689                                                 solverConstraint.m_rhs = velocityImpulse;
690                                                 solverConstraint.m_rhsPenetration = penetrationImpulse;
691                                         }
692                                         solverConstraint.m_cfm = 0.f;
693                                         solverConstraint.m_lowerLimit = 0;
694                                         solverConstraint.m_upperLimit = 1e10f;
695                                 }
696
697
698
699
700 }
701
702
703
704 void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, 
705                                                                                                                                                 int solverBodyIdA, int solverBodyIdB,
706                                                                                                                                  btManifoldPoint& cp, const btContactSolverInfo& infoGlobal)
707 {
708
709         btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA];
710         btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB];
711
712         btRigidBody* rb0 = bodyA->m_originalBody;
713         btRigidBody* rb1 = bodyB->m_originalBody;
714
715         {
716                 btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
717                 if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
718                 {
719                         frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
720                         if (rb0)
721                                 bodyA->internalApplyImpulse(frictionConstraint1.m_contactNormal1*rb0->getInvMass()*rb0->getLinearFactor(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
722                         if (rb1)
723                                 bodyB->internalApplyImpulse(-frictionConstraint1.m_contactNormal2*rb1->getInvMass()*rb1->getLinearFactor(),-frictionConstraint1.m_angularComponentB,-(btScalar)frictionConstraint1.m_appliedImpulse);
724                 } else
725                 {
726                         frictionConstraint1.m_appliedImpulse = 0.f;
727                 }
728         }
729
730         if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
731         {
732                 btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
733                 if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
734                 {
735                         frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2  * infoGlobal.m_warmstartingFactor;
736                         if (rb0)
737                                 bodyA->internalApplyImpulse(frictionConstraint2.m_contactNormal1*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
738                         if (rb1)
739                                 bodyB->internalApplyImpulse(-frictionConstraint2.m_contactNormal2*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-(btScalar)frictionConstraint2.m_appliedImpulse);
740                 } else
741                 {
742                         frictionConstraint2.m_appliedImpulse = 0.f;
743                 }
744         }
745 }
746
747
748
749
750 void    btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal)
751 {
752         btCollisionObject* colObj0=0,*colObj1=0;
753
754         colObj0 = (btCollisionObject*)manifold->getBody0();
755         colObj1 = (btCollisionObject*)manifold->getBody1();
756
757         int solverBodyIdA = getOrInitSolverBody(*colObj0);
758         int solverBodyIdB = getOrInitSolverBody(*colObj1);
759
760 //      btRigidBody* bodyA = btRigidBody::upcast(colObj0);
761 //      btRigidBody* bodyB = btRigidBody::upcast(colObj1);
762
763         btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverBodyIdA];
764         btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverBodyIdB];
765
766
767
768         ///avoid collision response between two static objects
769         if (!solverBodyA || (!solverBodyA->m_originalBody && (!solverBodyB || !solverBodyB->m_originalBody)))
770                 return;
771
772         int rollingFriction=1;
773         for (int j=0;j<manifold->getNumContacts();j++)
774         {
775
776                 btManifoldPoint& cp = manifold->getContactPoint(j);
777
778                 if (cp.getDistance() <= manifold->getContactProcessingThreshold())
779                 {
780                         btVector3 rel_pos1;
781                         btVector3 rel_pos2;
782                         btScalar relaxation;
783                         btScalar rel_vel;
784                         btVector3 vel;
785
786                         int frictionIndex = m_tmpSolverContactConstraintPool.size();
787                         btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing();
788 //                      btRigidBody* rb0 = btRigidBody::upcast(colObj0);
789 //                      btRigidBody* rb1 = btRigidBody::upcast(colObj1);
790                         solverConstraint.m_solverBodyIdA = solverBodyIdA;
791                         solverConstraint.m_solverBodyIdB = solverBodyIdB;
792
793                         solverConstraint.m_originalContactPoint = &cp;
794
795                         setupContactConstraint(solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, vel, rel_vel, relaxation, rel_pos1, rel_pos2);
796
797 //                      const btVector3& pos1 = cp.getPositionWorldOnA();
798 //                      const btVector3& pos2 = cp.getPositionWorldOnB();
799
800                         /////setup the friction constraints
801
802                         solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
803
804                         btVector3 angVelA,angVelB;
805                         solverBodyA->getAngularVelocity(angVelA);
806                         solverBodyB->getAngularVelocity(angVelB);                       
807                         btVector3 relAngVel = angVelB-angVelA;
808
809                         if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0))
810                         {
811                                 //only a single rollingFriction per manifold
812                                 rollingFriction--;
813                                 if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold)
814                                 {
815                                         relAngVel.normalize();
816                                         applyAnisotropicFriction(colObj0,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
817                                         applyAnisotropicFriction(colObj1,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
818                                         if (relAngVel.length()>0.001)
819                                                 addRollingFrictionConstraint(relAngVel,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
820
821                                 } else
822                                 {
823                                         addRollingFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
824                                         btVector3 axis0,axis1;
825                                         btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1);
826                                         applyAnisotropicFriction(colObj0,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
827                                         applyAnisotropicFriction(colObj1,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
828                                         applyAnisotropicFriction(colObj0,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
829                                         applyAnisotropicFriction(colObj1,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
830                                         if (axis0.length()>0.001)
831                                                 addRollingFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
832                                         if (axis1.length()>0.001)
833                                                 addRollingFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
834                 
835                                 }
836                         }
837
838                         ///Bullet has several options to set the friction directions
839                         ///By default, each contact has only a single friction direction that is recomputed automatically very frame 
840                         ///based on the relative linear velocity.
841                         ///If the relative velocity it zero, it will automatically compute a friction direction.
842                         
843                         ///You can also enable two friction directions, using the SOLVER_USE_2_FRICTION_DIRECTIONS.
844                         ///In that case, the second friction direction will be orthogonal to both contact normal and first friction direction.
845                         ///
846                         ///If you choose SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION, then the friction will be independent from the relative projected velocity.
847                         ///
848                         ///The user can manually override the friction directions for certain contacts using a contact callback, 
849                         ///and set the cp.m_lateralFrictionInitialized to true
850                         ///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2)
851                         ///this will give a conveyor belt effect
852                         ///
853                         if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
854                         {
855                                 cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
856                                 btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
857                                 if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
858                                 {
859                                         cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel);
860                                         if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
861                                         {
862                                                 cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
863                                                 cp.m_lateralFrictionDir2.normalize();//??
864                                                 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
865                                                 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
866                                                 addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
867
868                                         }
869
870                                         applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
871                                         applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
872                                         addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
873
874                                 } else
875                                 {
876                                         btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
877
878                                         if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
879                                         {
880                                                 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
881                                                 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
882                                                 addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
883                                         }
884
885                                         applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
886                                         applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
887                                         addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
888
889                                         if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
890                                         {
891                                                 cp.m_lateralFrictionInitialized = true;
892                                         }
893                                 }
894
895                         } else
896                         {
897                                 addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_contactCFM1);
898
899                                 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
900                                         addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2);
901
902                                 setFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
903                         }
904                 
905
906                         
907
908                 }
909         }
910 }
911
912 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
913 {
914         BT_PROFILE("solveGroupCacheFriendlySetup");
915         (void)stackAlloc;
916         (void)debugDrawer;
917
918         m_maxOverrideNumSolverIterations = 0;
919
920 #ifdef BT_ADDITIONAL_DEBUG
921          //make sure that dynamic bodies exist for all (enabled) constraints
922         for (int i=0;i<numConstraints;i++)
923         {
924                 btTypedConstraint* constraint = constraints[i];
925                 if (constraint->isEnabled())
926                 {
927                         if (!constraint->getRigidBodyA().isStaticOrKinematicObject())
928                         {
929                                 bool found=false;
930                                 for (int b=0;b<numBodies;b++)
931                                 {
932                 
933                                         if (&constraint->getRigidBodyA()==bodies[b])
934                                         {
935                                                 found = true;
936                                                 break;
937                                         }
938                                 }
939                                 btAssert(found);
940                         }
941                         if (!constraint->getRigidBodyB().isStaticOrKinematicObject())
942                         {
943                                 bool found=false;
944                                 for (int b=0;b<numBodies;b++)
945                                 {
946                                         if (&constraint->getRigidBodyB()==bodies[b])
947                                         {
948                                                 found = true;
949                                                 break;
950                                         }
951                                 }
952                                 btAssert(found);
953                         }
954                 }
955         }
956     //make sure that dynamic bodies exist for all contact manifolds
957     for (int i=0;i<numManifolds;i++)
958     {
959         if (!manifoldPtr[i]->getBody0()->isStaticOrKinematicObject())
960         {
961             bool found=false;
962             for (int b=0;b<numBodies;b++)
963             {
964                 
965                 if (manifoldPtr[i]->getBody0()==bodies[b])
966                 {
967                     found = true;
968                     break;
969                 }
970             }
971             btAssert(found);
972         }
973         if (!manifoldPtr[i]->getBody1()->isStaticOrKinematicObject())
974         {
975             bool found=false;
976             for (int b=0;b<numBodies;b++)
977             {
978                 if (manifoldPtr[i]->getBody1()==bodies[b])
979                 {
980                     found = true;
981                     break;
982                 }
983             }
984             btAssert(found);
985         }
986     }
987 #endif //BT_ADDITIONAL_DEBUG
988         
989         
990         for (int i = 0; i < numBodies; i++)
991         {
992                 bodies[i]->setCompanionId(-1);
993         }
994
995
996         m_tmpSolverBodyPool.reserve(numBodies+1);
997         m_tmpSolverBodyPool.resize(0);
998
999         btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
1000     initSolverBody(&fixedBody,0);
1001
1002         //convert all bodies
1003
1004         for (int i=0;i<numBodies;i++)
1005         {
1006                 int bodyId = getOrInitSolverBody(*bodies[i]);
1007                 btRigidBody* body = btRigidBody::upcast(bodies[i]);
1008                 if (body && body->getInvMass())
1009                 {
1010                         btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
1011                         btVector3 gyroForce (0,0,0);
1012                         if (body->getFlags()&BT_ENABLE_GYROPSCOPIC_FORCE)
1013                         {
1014                                 gyroForce = body->computeGyroscopicForce(infoGlobal.m_maxGyroscopicForce);
1015                         }
1016                         solverBody.m_linearVelocity += body->getTotalForce()*body->getInvMass()*infoGlobal.m_timeStep;
1017                         solverBody.m_angularVelocity += (body->getTotalTorque()-gyroForce)*body->getInvInertiaTensorWorld()*infoGlobal.m_timeStep;
1018                 }
1019         }
1020         
1021         if (1)
1022         {
1023                 int j;
1024                 for (j=0;j<numConstraints;j++)
1025                 {
1026                         btTypedConstraint* constraint = constraints[j];
1027                         constraint->buildJacobian();
1028                         constraint->internalSetAppliedImpulse(0.0f);
1029                 }
1030         }
1031
1032         //btRigidBody* rb0=0,*rb1=0;
1033
1034         //if (1)
1035         {
1036                 {
1037
1038                         int totalNumRows = 0;
1039                         int i;
1040                         
1041                         m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints);
1042                         //calculate the total number of contraint rows
1043                         for (i=0;i<numConstraints;i++)
1044                         {
1045                                 btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
1046                                 btJointFeedback* fb = constraints[i]->getJointFeedback();
1047                                 if (fb)
1048                                 {
1049                                         fb->m_appliedForceBodyA.setZero();
1050                                         fb->m_appliedTorqueBodyA.setZero();
1051                                         fb->m_appliedForceBodyB.setZero();
1052                                         fb->m_appliedTorqueBodyB.setZero();
1053                                 }
1054
1055                                 if (constraints[i]->isEnabled())
1056                                 {
1057                                 }
1058                                 if (constraints[i]->isEnabled())
1059                                 {
1060                                         constraints[i]->getInfo1(&info1);
1061                                 } else
1062                                 {
1063                                         info1.m_numConstraintRows = 0;
1064                                         info1.nub = 0;
1065                                 }
1066                                 totalNumRows += info1.m_numConstraintRows;
1067                         }
1068                         m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows);
1069
1070                         
1071                         ///setup the btSolverConstraints
1072                         int currentRow = 0;
1073
1074                         for (i=0;i<numConstraints;i++)
1075                         {
1076                                 const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
1077                                 
1078                                 if (info1.m_numConstraintRows)
1079                                 {
1080                                         btAssert(currentRow<totalNumRows);
1081
1082                                         btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];
1083                                         btTypedConstraint* constraint = constraints[i];
1084                                         btRigidBody& rbA = constraint->getRigidBodyA();
1085                                         btRigidBody& rbB = constraint->getRigidBodyB();
1086
1087                     int solverBodyIdA = getOrInitSolverBody(rbA);
1088                     int solverBodyIdB = getOrInitSolverBody(rbB);
1089
1090                     btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
1091                     btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
1092
1093
1094
1095
1096                                         int overrideNumSolverIterations = constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations;
1097                                         if (overrideNumSolverIterations>m_maxOverrideNumSolverIterations)
1098                                                 m_maxOverrideNumSolverIterations = overrideNumSolverIterations;
1099
1100
1101                                         int j;
1102                                         for ( j=0;j<info1.m_numConstraintRows;j++)
1103                                         {
1104                                                 memset(&currentConstraintRow[j],0,sizeof(btSolverConstraint));
1105                                                 currentConstraintRow[j].m_lowerLimit = -SIMD_INFINITY;
1106                                                 currentConstraintRow[j].m_upperLimit = SIMD_INFINITY;
1107                                                 currentConstraintRow[j].m_appliedImpulse = 0.f;
1108                                                 currentConstraintRow[j].m_appliedPushImpulse = 0.f;
1109                                                 currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA;
1110                                                 currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB;
1111                                                 currentConstraintRow[j].m_overrideNumSolverIterations = overrideNumSolverIterations;
1112                                         }
1113
1114                                         bodyAPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
1115                                         bodyAPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
1116                                         bodyAPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f);
1117                                         bodyAPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
1118                                         bodyBPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
1119                                         bodyBPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
1120                                         bodyBPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f);
1121                                         bodyBPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
1122
1123
1124                                         btTypedConstraint::btConstraintInfo2 info2;
1125                                         info2.fps = 1.f/infoGlobal.m_timeStep;
1126                                         info2.erp = infoGlobal.m_erp;
1127                                         info2.m_J1linearAxis = currentConstraintRow->m_contactNormal1;
1128                                         info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal;
1129                                         info2.m_J2linearAxis = currentConstraintRow->m_contactNormal2;
1130                                         info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal;
1131                                         info2.rowskip = sizeof(btSolverConstraint)/sizeof(btScalar);//check this
1132                                         ///the size of btSolverConstraint needs be a multiple of btScalar
1133                             btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint));
1134                                         info2.m_constraintError = &currentConstraintRow->m_rhs;
1135                                         currentConstraintRow->m_cfm = infoGlobal.m_globalCfm;
1136                                         info2.m_damping = infoGlobal.m_damping;
1137                                         info2.cfm = &currentConstraintRow->m_cfm;
1138                                         info2.m_lowerLimit = &currentConstraintRow->m_lowerLimit;
1139                                         info2.m_upperLimit = &currentConstraintRow->m_upperLimit;
1140                                         info2.m_numIterations = infoGlobal.m_numIterations;
1141                                         constraints[i]->getInfo2(&info2);
1142
1143                                         ///finalize the constraint setup
1144                                         for ( j=0;j<info1.m_numConstraintRows;j++)
1145                                         {
1146                                                 btSolverConstraint& solverConstraint = currentConstraintRow[j];
1147
1148                                                 if (solverConstraint.m_upperLimit>=constraints[i]->getBreakingImpulseThreshold())
1149                                                 {
1150                                                         solverConstraint.m_upperLimit = constraints[i]->getBreakingImpulseThreshold();
1151                                                 }
1152
1153                                                 if (solverConstraint.m_lowerLimit<=-constraints[i]->getBreakingImpulseThreshold())
1154                                                 {
1155                                                         solverConstraint.m_lowerLimit = -constraints[i]->getBreakingImpulseThreshold();
1156                                                 }
1157
1158                                                 solverConstraint.m_originalContactPoint = constraint;
1159
1160                                                 {
1161                                                         const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
1162                                                         solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld()*ftorqueAxis1*constraint->getRigidBodyA().getAngularFactor();
1163                                                 }
1164                                                 {
1165                                                         const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal;
1166                                                         solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld()*ftorqueAxis2*constraint->getRigidBodyB().getAngularFactor();
1167                                                 }
1168
1169                                                 {
1170                                                         btVector3 iMJlA = solverConstraint.m_contactNormal1*rbA.getInvMass();
1171                                                         btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal;
1172                                                         btVector3 iMJlB = solverConstraint.m_contactNormal2*rbB.getInvMass();//sign of normal?
1173                                                         btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal;
1174
1175                                                         btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal1);
1176                                                         sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
1177                                                         sum += iMJlB.dot(solverConstraint.m_contactNormal2);
1178                                                         sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
1179                                                         btScalar fsum = btFabs(sum);
1180                                                         btAssert(fsum > SIMD_EPSILON);
1181                                                         solverConstraint.m_jacDiagABInv = fsum>SIMD_EPSILON?btScalar(1.)/sum : 0.f;
1182                                                 }
1183
1184
1185                                                 ///fix rhs
1186                                                 ///todo: add force/torque accelerators
1187                                                 {
1188                                                         btScalar rel_vel;
1189                                                         btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(rbA.getLinearVelocity()) + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity());
1190                                                         btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(rbB.getLinearVelocity()) + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity());
1191
1192                                                         rel_vel = vel1Dotn+vel2Dotn;
1193
1194                                                         btScalar restitution = 0.f;
1195                                                         btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2
1196                                                         btScalar        velocityError = restitution - rel_vel * info2.m_damping;
1197                                                         btScalar        penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
1198                                                         btScalar        velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
1199                                                         solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
1200                                                         solverConstraint.m_appliedImpulse = 0.f;
1201
1202                                                 }
1203                                         }
1204                                 }
1205                                 currentRow+=m_tmpConstraintSizesPool[i].m_numConstraintRows;
1206                         }
1207                 }
1208
1209                 {
1210                         int i;
1211                         btPersistentManifold* manifold = 0;
1212 //                      btCollisionObject* colObj0=0,*colObj1=0;
1213
1214
1215                         for (i=0;i<numManifolds;i++)
1216                         {
1217                                 manifold = manifoldPtr[i];
1218                                 convertContact(manifold,infoGlobal);
1219                         }
1220                 }
1221         }
1222
1223 //      btContactSolverInfo info = infoGlobal;
1224
1225
1226         int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
1227         int numConstraintPool = m_tmpSolverContactConstraintPool.size();
1228         int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
1229
1230         ///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
1231         m_orderNonContactConstraintPool.resizeNoInitialize(numNonContactPool);
1232         if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
1233                 m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool*2);
1234         else
1235                 m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool);
1236
1237         m_orderFrictionConstraintPool.resizeNoInitialize(numFrictionPool);
1238         {
1239                 int i;
1240                 for (i=0;i<numNonContactPool;i++)
1241                 {
1242                         m_orderNonContactConstraintPool[i] = i;
1243                 }
1244                 for (i=0;i<numConstraintPool;i++)
1245                 {
1246                         m_orderTmpConstraintPool[i] = i;
1247                 }
1248                 for (i=0;i<numFrictionPool;i++)
1249                 {
1250                         m_orderFrictionConstraintPool[i] = i;
1251                 }
1252         }
1253
1254         return 0.f;
1255
1256 }
1257
1258
1259 btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/)
1260 {
1261
1262         int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
1263         int numConstraintPool = m_tmpSolverContactConstraintPool.size();
1264         int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
1265         
1266         if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
1267         {
1268                 if (1)                  // uncomment this for a bit less random ((iteration & 7) == 0)
1269                 {
1270
1271                         for (int j=0; j<numNonContactPool; ++j) {
1272                                 int tmp = m_orderNonContactConstraintPool[j];
1273                                 int swapi = btRandInt2(j+1);
1274                                 m_orderNonContactConstraintPool[j] = m_orderNonContactConstraintPool[swapi];
1275                                 m_orderNonContactConstraintPool[swapi] = tmp;
1276                         }
1277
1278                         //contact/friction constraints are not solved more than 
1279                         if (iteration< infoGlobal.m_numIterations)
1280                         {
1281                                 for (int j=0; j<numConstraintPool; ++j) {
1282                                         int tmp = m_orderTmpConstraintPool[j];
1283                                         int swapi = btRandInt2(j+1);
1284                                         m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
1285                                         m_orderTmpConstraintPool[swapi] = tmp;
1286                                 }
1287
1288                                 for (int j=0; j<numFrictionPool; ++j) {
1289                                         int tmp = m_orderFrictionConstraintPool[j];
1290                                         int swapi = btRandInt2(j+1);
1291                                         m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
1292                                         m_orderFrictionConstraintPool[swapi] = tmp;
1293                                 }
1294                         }
1295                 }
1296         }
1297
1298         if (infoGlobal.m_solverMode & SOLVER_SIMD)
1299         {
1300                 ///solve all joint constraints, using SIMD, if available
1301                 for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
1302                 {
1303                         btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
1304                         if (iteration < constraint.m_overrideNumSolverIterations)
1305                                 resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
1306                 }
1307
1308                 if (iteration< infoGlobal.m_numIterations)
1309                 {
1310                         for (int j=0;j<numConstraints;j++)
1311                         {
1312                 if (constraints[j]->isEnabled())
1313                 {
1314                     int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA());
1315                     int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB());
1316                     btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
1317                     btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
1318                     constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
1319                 }
1320                         }
1321
1322                         ///solve all contact constraints using SIMD, if available
1323                         if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS)
1324                         {
1325                                 int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1326                                 int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)? 2 : 1;
1327
1328                                 for (int c=0;c<numPoolConstraints;c++)
1329                                 {
1330                                         btScalar totalImpulse =0;
1331
1332                                         {
1333                                                 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]];
1334                                                 resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
1335                                                 totalImpulse = solveManifold.m_appliedImpulse;
1336                                         }
1337                                         bool applyFriction = true;
1338                                         if (applyFriction)
1339                                         {
1340                                                 {
1341
1342                                                         btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier]];
1343
1344                                                         if (totalImpulse>btScalar(0))
1345                                                         {
1346                                                                 solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
1347                                                                 solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
1348
1349                                                                 resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
1350                                                         }
1351                                                 }
1352
1353                                                 if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)
1354                                                 {
1355
1356                                                         btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier+1]];
1357                                 
1358                                                         if (totalImpulse>btScalar(0))
1359                                                         {
1360                                                                 solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
1361                                                                 solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
1362
1363                                                                 resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
1364                                                         }
1365                                                 }
1366                                         }
1367                                 }
1368
1369                         }
1370                         else//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
1371                         {
1372                                 //solve the friction constraints after all contact constraints, don't interleave them
1373                                 int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1374                                 int j;
1375
1376                                 for (j=0;j<numPoolConstraints;j++)
1377                                 {
1378                                         const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
1379                                         resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
1380
1381                                 }
1382                 
1383                                 
1384
1385                                 ///solve all friction constraints, using SIMD, if available
1386
1387                                 int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
1388                                 for (j=0;j<numFrictionPoolConstraints;j++)
1389                                 {
1390                                         btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
1391                                         btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
1392
1393                                         if (totalImpulse>btScalar(0))
1394                                         {
1395                                                 solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
1396                                                 solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
1397
1398                                                 resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
1399                                         }
1400                                 }
1401
1402                                 
1403                                 int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
1404                                 for (j=0;j<numRollingFrictionPoolConstraints;j++)
1405                                 {
1406
1407                                         btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
1408                                         btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
1409                                         if (totalImpulse>btScalar(0))
1410                                         {
1411                                                 btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
1412                                                 if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
1413                                                         rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
1414
1415                                                 rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
1416                                                 rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
1417
1418                                                 resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
1419                                         }
1420                                 }
1421                                 
1422
1423                         }                       
1424                 }
1425         } else
1426         {
1427                 //non-SIMD version
1428                 ///solve all joint constraints
1429                 for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
1430                 {
1431                         btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
1432                         if (iteration < constraint.m_overrideNumSolverIterations)
1433                                 resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
1434                 }
1435
1436                 if (iteration< infoGlobal.m_numIterations)
1437                 {
1438                         for (int j=0;j<numConstraints;j++)
1439                         {
1440                 if (constraints[j]->isEnabled())
1441                 {
1442                     int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA());
1443                     int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB());
1444                     btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
1445                     btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
1446                     constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
1447                 }
1448                         }
1449                         ///solve all contact constraints
1450                         int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1451                         for (int j=0;j<numPoolConstraints;j++)
1452                         {
1453                                 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
1454                                 resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
1455                         }
1456                         ///solve all friction constraints
1457                         int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
1458                         for (int j=0;j<numFrictionPoolConstraints;j++)
1459                         {
1460                                 btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
1461                                 btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
1462
1463                                 if (totalImpulse>btScalar(0))
1464                                 {
1465                                         solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
1466                                         solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
1467
1468                                         resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
1469                                 }
1470                         }
1471
1472                         int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
1473                         for (int j=0;j<numRollingFrictionPoolConstraints;j++)
1474                         {
1475                                 btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
1476                                 btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
1477                                 if (totalImpulse>btScalar(0))
1478                                 {
1479                                         btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
1480                                         if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
1481                                                 rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
1482
1483                                         rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
1484                                         rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
1485
1486                                         resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
1487                                 }
1488                         }
1489                 }
1490         }
1491         return 0.f;
1492 }
1493
1494
1495 void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
1496 {
1497         int iteration;
1498         if (infoGlobal.m_splitImpulse)
1499         {
1500                 if (infoGlobal.m_solverMode & SOLVER_SIMD)
1501                 {
1502                         for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
1503                         {
1504                                 {
1505                                         int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1506                                         int j;
1507                                         for (j=0;j<numPoolConstraints;j++)
1508                                         {
1509                                                 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
1510
1511                                                 resolveSplitPenetrationSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
1512                                         }
1513                                 }
1514                         }
1515                 }
1516                 else
1517                 {
1518                         for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
1519                         {
1520                                 {
1521                                         int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1522                                         int j;
1523                                         for (j=0;j<numPoolConstraints;j++)
1524                                         {
1525                                                 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
1526
1527                                                 resolveSplitPenetrationImpulseCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
1528                                         }
1529                                 }
1530                         }
1531                 }
1532         }
1533 }
1534
1535 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
1536 {
1537         BT_PROFILE("solveGroupCacheFriendlyIterations");
1538
1539         {
1540                 ///this is a special step to resolve penetrations (just for contacts)
1541                 solveGroupCacheFriendlySplitImpulseIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
1542
1543                 int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations;
1544
1545                 for ( int iteration = 0 ; iteration< maxIterations ; iteration++)
1546                 //for ( int iteration = maxIterations-1  ; iteration >= 0;iteration--)
1547                 {                       
1548                         solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
1549                 }
1550                 
1551         }
1552         return 0.f;
1553 }
1554
1555 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal)
1556 {
1557         int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1558         int i,j;
1559
1560         if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
1561         {
1562                 for (j=0;j<numPoolConstraints;j++)
1563                 {
1564                         const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j];
1565                         btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
1566                         btAssert(pt);
1567                         pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
1568                 //      float f = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
1569                         //      printf("pt->m_appliedImpulseLateral1 = %f\n", f);
1570                         pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
1571                         //printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
1572                         if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
1573                         {
1574                                 pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
1575                         }
1576                         //do a callback here?
1577                 }
1578         }
1579
1580         numPoolConstraints = m_tmpSolverNonContactConstraintPool.size();
1581         for (j=0;j<numPoolConstraints;j++)
1582         {
1583                 const btSolverConstraint& solverConstr = m_tmpSolverNonContactConstraintPool[j];
1584                 btTypedConstraint* constr = (btTypedConstraint*)solverConstr.m_originalContactPoint;
1585                 btJointFeedback* fb = constr->getJointFeedback();
1586                 if (fb)
1587                 {
1588                         fb->m_appliedForceBodyA += solverConstr.m_contactNormal1*solverConstr.m_appliedImpulse*constr->getRigidBodyA().getLinearFactor()/infoGlobal.m_timeStep;
1589                         fb->m_appliedForceBodyB += solverConstr.m_contactNormal2*solverConstr.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/infoGlobal.m_timeStep;
1590                         fb->m_appliedTorqueBodyA += solverConstr.m_relpos1CrossNormal* constr->getRigidBodyA().getAngularFactor()*solverConstr.m_appliedImpulse/infoGlobal.m_timeStep;
1591                         fb->m_appliedTorqueBodyB += solverConstr.m_relpos2CrossNormal* constr->getRigidBodyB().getAngularFactor()*solverConstr.m_appliedImpulse/infoGlobal.m_timeStep; /*RGM ???? */
1592                         
1593                 }
1594
1595                 constr->internalSetAppliedImpulse(solverConstr.m_appliedImpulse);
1596                 if (btFabs(solverConstr.m_appliedImpulse)>=constr->getBreakingImpulseThreshold())
1597                 {
1598                         constr->setEnabled(false);
1599                 }
1600         }
1601
1602
1603
1604         for ( i=0;i<m_tmpSolverBodyPool.size();i++)
1605         {
1606                 btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody;
1607                 if (body)
1608                 {
1609                         if (infoGlobal.m_splitImpulse)
1610                                 m_tmpSolverBodyPool[i].writebackVelocityAndTransform(infoGlobal.m_timeStep, infoGlobal.m_splitImpulseTurnErp);
1611                         else
1612                                 m_tmpSolverBodyPool[i].writebackVelocity();
1613
1614                         m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(m_tmpSolverBodyPool[i].m_linearVelocity);
1615                         m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(m_tmpSolverBodyPool[i].m_angularVelocity);
1616                         if (infoGlobal.m_splitImpulse)
1617                                 m_tmpSolverBodyPool[i].m_originalBody->setWorldTransform(m_tmpSolverBodyPool[i].m_worldTransform);
1618
1619                         m_tmpSolverBodyPool[i].m_originalBody->setCompanionId(-1);
1620                 }
1621         }
1622
1623         m_tmpSolverContactConstraintPool.resizeNoInitialize(0);
1624         m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0);
1625         m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize(0);
1626         m_tmpSolverContactRollingFrictionConstraintPool.resizeNoInitialize(0);
1627
1628         m_tmpSolverBodyPool.resizeNoInitialize(0);
1629         return 0.f;
1630 }
1631
1632
1633
1634 /// btSequentialImpulseConstraintSolver Sequentially applies impulses
1635 btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /*dispatcher*/)
1636 {
1637
1638         BT_PROFILE("solveGroup");
1639         //you need to provide at least some bodies
1640         
1641         solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
1642
1643         solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
1644
1645         solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal);
1646         
1647         return 0.f;
1648 }
1649
1650 void    btSequentialImpulseConstraintSolver::reset()
1651 {
1652         m_btSeed2 = 0;
1653 }
1654
1655