== SoC Bullet - Bullet Upgrade to 2.76 ==
[blender.git] / extern / bullet2 / 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 //It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms.
18
19 #include "btSequentialImpulseConstraintSolver.h"
20 #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
21 #include "BulletDynamics/Dynamics/btRigidBody.h"
22 #include "btContactConstraint.h"
23 #include "btSolve2LinearConstraint.h"
24 #include "btContactSolverInfo.h"
25 #include "LinearMath/btIDebugDraw.h"
26 #include "btJacobianEntry.h"
27 #include "LinearMath/btMinMax.h"
28 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
29 #include <new>
30 #include "LinearMath/btStackAlloc.h"
31 #include "LinearMath/btQuickprof.h"
32 #include "btSolverBody.h"
33 #include "btSolverConstraint.h"
34 #include "LinearMath/btAlignedObjectArray.h"
35 #include <string.h> //for memset
36
37 int             gNumSplitImpulseRecoveries = 0;
38
39 btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
40 :m_btSeed2(0)
41 {
42
43 }
44
45 btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
46 {
47 }
48
49 #ifdef USE_SIMD
50 #include <emmintrin.h>
51 #define vec_splat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e,e,e,e))
52 static inline __m128 _vmathVfDot3( __m128 vec0, __m128 vec1 )
53 {
54         __m128 result = _mm_mul_ps( vec0, vec1);
55         return _mm_add_ps( vec_splat( result, 0 ), _mm_add_ps( vec_splat( result, 1 ), vec_splat( result, 2 ) ) );
56 }
57 #endif//USE_SIMD
58
59 // Project Gauss Seidel or the equivalent Sequential Impulse
60 void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c)
61 {
62 #ifdef USE_SIMD
63         __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
64         __m128  lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
65         __m128  upperLimit1 = _mm_set1_ps(c.m_upperLimit);
66         __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)));
67         __m128 deltaVel1Dotn    =       _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
68         __m128 deltaVel2Dotn    =       _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.internalGetDeltaLinearVelocity().mVec128));
69         deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
70         deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
71         btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
72         btSimdScalar resultLowerLess,resultUpperLess;
73         resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
74         resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
75         __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
76         deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
77         c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
78         __m128 upperMinApplied = _mm_sub_ps(upperLimit1,cpAppliedImp);
79         deltaImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied) );
80         c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1) );
81         __m128  linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128);
82         __m128  linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128);
83         __m128 impulseMagnitude = deltaImpulse;
84         body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
85         body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
86         body2.internalGetDeltaLinearVelocity().mVec128 = _mm_sub_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
87         body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
88 #else
89         resolveSingleConstraintRowGeneric(body1,body2,c);
90 #endif
91 }
92
93 // Project Gauss Seidel or the equivalent Sequential Impulse
94  void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c)
95 {
96         btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
97         const btScalar deltaVel1Dotn    =       c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity())   + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
98         const btScalar deltaVel2Dotn    =       -c.m_contactNormal.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
99
100 //      const btScalar delta_rel_vel    =       deltaVel1Dotn-deltaVel2Dotn;
101         deltaImpulse    -=      deltaVel1Dotn*c.m_jacDiagABInv;
102         deltaImpulse    -=      deltaVel2Dotn*c.m_jacDiagABInv;
103
104         const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
105         if (sum < c.m_lowerLimit)
106         {
107                 deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
108                 c.m_appliedImpulse = c.m_lowerLimit;
109         }
110         else if (sum > c.m_upperLimit) 
111         {
112                 deltaImpulse = c.m_upperLimit-c.m_appliedImpulse;
113                 c.m_appliedImpulse = c.m_upperLimit;
114         }
115         else
116         {
117                 c.m_appliedImpulse = sum;
118         }
119                 body1.internalApplyImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
120                 body2.internalApplyImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
121 }
122
123  void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c)
124 {
125 #ifdef USE_SIMD
126         __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
127         __m128  lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
128         __m128  upperLimit1 = _mm_set1_ps(c.m_upperLimit);
129         __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)));
130         __m128 deltaVel1Dotn    =       _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
131         __m128 deltaVel2Dotn    =       _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.internalGetDeltaLinearVelocity().mVec128));
132         deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
133         deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
134         btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
135         btSimdScalar resultLowerLess,resultUpperLess;
136         resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
137         resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
138         __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
139         deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
140         c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
141         __m128  linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128);
142         __m128  linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128);
143         __m128 impulseMagnitude = deltaImpulse;
144         body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
145         body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
146         body2.internalGetDeltaLinearVelocity().mVec128 = _mm_sub_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
147         body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
148 #else
149         resolveSingleConstraintRowLowerLimit(body1,body2,c);
150 #endif
151 }
152
153 // Project Gauss Seidel or the equivalent Sequential Impulse
154  void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c)
155 {
156         btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
157         const btScalar deltaVel1Dotn    =       c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity())   + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
158         const btScalar deltaVel2Dotn    =       -c.m_contactNormal.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
159
160         deltaImpulse    -=      deltaVel1Dotn*c.m_jacDiagABInv;
161         deltaImpulse    -=      deltaVel2Dotn*c.m_jacDiagABInv;
162         const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
163         if (sum < c.m_lowerLimit)
164         {
165                 deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
166                 c.m_appliedImpulse = c.m_lowerLimit;
167         }
168         else
169         {
170                 c.m_appliedImpulse = sum;
171         }
172         body1.internalApplyImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
173         body2.internalApplyImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
174 }
175
176
177 void    btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFriendly(
178         btRigidBody& body1,
179         btRigidBody& body2,
180         const btSolverConstraint& c)
181 {
182                 if (c.m_rhsPenetration)
183         {
184                         gNumSplitImpulseRecoveries++;
185                         btScalar deltaImpulse = c.m_rhsPenetration-btScalar(c.m_appliedPushImpulse)*c.m_cfm;
186                         const btScalar deltaVel1Dotn    =       c.m_contactNormal.dot(body1.internalGetPushVelocity())  + c.m_relpos1CrossNormal.dot(body1.internalGetTurnVelocity());
187                         const btScalar deltaVel2Dotn    =       -c.m_contactNormal.dot(body2.internalGetPushVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetTurnVelocity());
188
189                         deltaImpulse    -=      deltaVel1Dotn*c.m_jacDiagABInv;
190                         deltaImpulse    -=      deltaVel2Dotn*c.m_jacDiagABInv;
191                         const btScalar sum = btScalar(c.m_appliedPushImpulse) + deltaImpulse;
192                         if (sum < c.m_lowerLimit)
193                         {
194                                 deltaImpulse = c.m_lowerLimit-c.m_appliedPushImpulse;
195                                 c.m_appliedPushImpulse = c.m_lowerLimit;
196                         }
197                         else
198                         {
199                                 c.m_appliedPushImpulse = sum;
200                         }
201                         body1.internalApplyPushImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
202                         body2.internalApplyPushImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
203         }
204 }
205
206  void btSequentialImpulseConstraintSolver::resolveSplitPenetrationSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c)
207 {
208 #ifdef USE_SIMD
209         if (!c.m_rhsPenetration)
210                 return;
211
212         gNumSplitImpulseRecoveries++;
213
214         __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedPushImpulse);
215         __m128  lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
216         __m128  upperLimit1 = _mm_set1_ps(c.m_upperLimit);
217         __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)));
218         __m128 deltaVel1Dotn    =       _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.internalGetPushVelocity().mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetTurnVelocity().mVec128));
219         __m128 deltaVel2Dotn    =       _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetTurnVelocity().mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.internalGetPushVelocity().mVec128));
220         deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
221         deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
222         btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
223         btSimdScalar resultLowerLess,resultUpperLess;
224         resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
225         resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
226         __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
227         deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
228         c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
229         __m128  linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128);
230         __m128  linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128);
231         __m128 impulseMagnitude = deltaImpulse;
232         body1.internalGetPushVelocity().mVec128 = _mm_add_ps(body1.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
233         body1.internalGetTurnVelocity().mVec128 = _mm_add_ps(body1.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
234         body2.internalGetPushVelocity().mVec128 = _mm_sub_ps(body2.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
235         body2.internalGetTurnVelocity().mVec128 = _mm_add_ps(body2.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
236 #else
237         resolveSplitPenetrationImpulseCacheFriendly(body1,body2,c);
238 #endif
239 }
240
241
242
243 unsigned long btSequentialImpulseConstraintSolver::btRand2()
244 {
245         m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff;
246         return m_btSeed2;
247 }
248
249
250
251 //See ODE: adam's all-int straightforward(?) dRandInt (0..n-1)
252 int btSequentialImpulseConstraintSolver::btRandInt2 (int n)
253 {
254         // seems good; xor-fold and modulus
255         const unsigned long un = static_cast<unsigned long>(n);
256         unsigned long r = btRand2();
257
258         // note: probably more aggressive than it needs to be -- might be
259         //       able to get away without one or two of the innermost branches.
260         if (un <= 0x00010000UL) {
261                 r ^= (r >> 16);
262                 if (un <= 0x00000100UL) {
263                         r ^= (r >> 8);
264                         if (un <= 0x00000010UL) {
265                                 r ^= (r >> 4);
266                                 if (un <= 0x00000004UL) {
267                                         r ^= (r >> 2);
268                                         if (un <= 0x00000002UL) {
269                                                 r ^= (r >> 1);
270                                         }
271                                 }
272                         }
273                 }
274         }
275
276         return (int) (r % un);
277 }
278
279
280 #if 0
281 void    btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject)
282 {
283         btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0;
284
285         solverBody->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
286         solverBody->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
287         solverBody->internalGetPushVelocity().setValue(0.f,0.f,0.f);
288         solverBody->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
289
290         if (rb)
291         {
292                 solverBody->internalGetInvMass() = btVector3(rb->getInvMass(),rb->getInvMass(),rb->getInvMass())*rb->getLinearFactor();
293                 solverBody->m_originalBody = rb;
294                 solverBody->m_angularFactor = rb->getAngularFactor();
295         } else
296         {
297                 solverBody->internalGetInvMass().setValue(0,0,0);
298                 solverBody->m_originalBody = 0;
299                 solverBody->m_angularFactor.setValue(1,1,1);
300         }
301 }
302 #endif
303
304
305
306
307
308 btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution)
309 {
310         btScalar rest = restitution * -rel_vel;
311         return rest;
312 }
313
314
315
316 void    applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection);
317 void    applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection)
318 {
319         if (colObj && colObj->hasAnisotropicFriction())
320         {
321                 // transform to local coordinates
322                 btVector3 loc_lateral = frictionDirection * colObj->getWorldTransform().getBasis();
323                 const btVector3& friction_scaling = colObj->getAnisotropicFriction();
324                 //apply anisotropic friction
325                 loc_lateral *= friction_scaling;
326                 // ... and transform it back to global coordinates
327                 frictionDirection = colObj->getWorldTransform().getBasis() * loc_lateral;
328         }
329 }
330
331
332 void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyB,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
333 {
334
335
336         btRigidBody* body0=btRigidBody::upcast(colObj0);
337         btRigidBody* body1=btRigidBody::upcast(colObj1);
338
339         solverConstraint.m_contactNormal = normalAxis;
340
341         solverConstraint.m_solverBodyA = body0 ? body0 : &getFixedBody();
342         solverConstraint.m_solverBodyB = body1 ? body1 : &getFixedBody();
343
344         solverConstraint.m_friction = cp.m_combinedFriction;
345         solverConstraint.m_originalContactPoint = 0;
346
347         solverConstraint.m_appliedImpulse = 0.f;
348         solverConstraint.m_appliedPushImpulse = 0.f;
349
350         {
351                 btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal);
352                 solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
353                 solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor() : btVector3(0,0,0);
354         }
355         {
356                 btVector3 ftorqueAxis1 = rel_pos2.cross(-solverConstraint.m_contactNormal);
357                 solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
358                 solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor() : btVector3(0,0,0);
359         }
360
361 #ifdef COMPUTE_IMPULSE_DENOM
362         btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal);
363         btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal);
364 #else
365         btVector3 vec;
366         btScalar denom0 = 0.f;
367         btScalar denom1 = 0.f;
368         if (body0)
369         {
370                 vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
371                 denom0 = body0->getInvMass() + normalAxis.dot(vec);
372         }
373         if (body1)
374         {
375                 vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
376                 denom1 = body1->getInvMass() + normalAxis.dot(vec);
377         }
378
379
380 #endif //COMPUTE_IMPULSE_DENOM
381         btScalar denom = relaxation/(denom0+denom1);
382         solverConstraint.m_jacDiagABInv = denom;
383
384 #ifdef _USE_JACOBIAN
385         solverConstraint.m_jac =  btJacobianEntry (
386                 rel_pos1,rel_pos2,solverConstraint.m_contactNormal,
387                 body0->getInvInertiaDiagLocal(),
388                 body0->getInvMass(),
389                 body1->getInvInertiaDiagLocal(),
390                 body1->getInvMass());
391 #endif //_USE_JACOBIAN
392
393
394         {
395                 btScalar rel_vel;
396                 btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0?body0->getLinearVelocity():btVector3(0,0,0)) 
397                         + solverConstraint.m_relpos1CrossNormal.dot(body0?body0->getAngularVelocity():btVector3(0,0,0));
398                 btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(body1?body1->getLinearVelocity():btVector3(0,0,0)) 
399                         + solverConstraint.m_relpos2CrossNormal.dot(body1?body1->getAngularVelocity():btVector3(0,0,0));
400
401                 rel_vel = vel1Dotn+vel2Dotn;
402
403 //              btScalar positionalError = 0.f;
404
405                 btSimdScalar velocityError =  desiredVelocity - rel_vel;
406                 btSimdScalar    velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
407                 solverConstraint.m_rhs = velocityImpulse;
408                 solverConstraint.m_cfm = cfmSlip;
409                 solverConstraint.m_lowerLimit = 0;
410                 solverConstraint.m_upperLimit = 1e10f;
411         }
412 }
413
414
415
416 btSolverConstraint&     btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
417 {
418         btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing();
419         solverConstraint.m_frictionIndex = frictionIndex;
420         setupFrictionConstraint(solverConstraint, normalAxis, solverBodyA, solverBodyB, cp, rel_pos1, rel_pos2, 
421                                                         colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
422         return solverConstraint;
423 }
424
425 int     btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body)
426 {
427 #if 0
428         int solverBodyIdA = -1;
429
430         if (body.getCompanionId() >= 0)
431         {
432                 //body has already been converted
433                 solverBodyIdA = body.getCompanionId();
434         } else
435         {
436                 btRigidBody* rb = btRigidBody::upcast(&body);
437                 if (rb && rb->getInvMass())
438                 {
439                         solverBodyIdA = m_tmpSolverBodyPool.size();
440                         btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
441                         initSolverBody(&solverBody,&body);
442                         body.setCompanionId(solverBodyIdA);
443                 } else
444                 {
445                         return 0;//assume first one is a fixed solver body
446                 }
447         }
448         return solverBodyIdA;
449 #endif
450         return 0;
451 }
452 #include <stdio.h>
453
454
455 void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstraint& solverConstraint, 
456                                                                                                                                  btCollisionObject* colObj0, btCollisionObject* colObj1,
457                                                                                                                                  btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
458                                                                                                                                  btVector3& vel, btScalar& rel_vel, btScalar& relaxation,
459                                                                                                                                  btVector3& rel_pos1, btVector3& rel_pos2)
460 {
461                         btRigidBody* rb0 = btRigidBody::upcast(colObj0);
462                         btRigidBody* rb1 = btRigidBody::upcast(colObj1);
463
464                         const btVector3& pos1 = cp.getPositionWorldOnA();
465                         const btVector3& pos2 = cp.getPositionWorldOnB();
466
467 //                      btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); 
468 //                      btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
469                         rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); 
470                         rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
471
472                         relaxation = 1.f;
473
474                         btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
475                         solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
476                         btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);            
477                         solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
478
479                                 {
480 #ifdef COMPUTE_IMPULSE_DENOM
481                                         btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
482                                         btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
483 #else                                                   
484                                         btVector3 vec;
485                                         btScalar denom0 = 0.f;
486                                         btScalar denom1 = 0.f;
487                                         if (rb0)
488                                         {
489                                                 vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
490                                                 denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
491                                         }
492                                         if (rb1)
493                                         {
494                                                 vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
495                                                 denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
496                                         }
497 #endif //COMPUTE_IMPULSE_DENOM          
498
499                                         btScalar denom = relaxation/(denom0+denom1);
500                                         solverConstraint.m_jacDiagABInv = denom;
501                                 }
502
503                                 solverConstraint.m_contactNormal = cp.m_normalWorldOnB;
504                                 solverConstraint.m_relpos1CrossNormal = rel_pos1.cross(cp.m_normalWorldOnB);
505                                 solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(-cp.m_normalWorldOnB);
506
507
508
509
510                         btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
511                         btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
512                         vel  = vel1 - vel2;
513                         rel_vel = cp.m_normalWorldOnB.dot(vel);
514
515                                 btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop;
516
517
518                                 solverConstraint.m_friction = cp.m_combinedFriction;
519
520                                 btScalar restitution = 0.f;
521                                 
522                                 if (cp.m_lifeTime>infoGlobal.m_restingContactRestitutionThreshold)
523                                 {
524                                         restitution = 0.f;
525                                 } else
526                                 {
527                                         restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution);
528                                         if (restitution <= btScalar(0.))
529                                         {
530                                                 restitution = 0.f;
531                                         };
532                                 }
533
534
535                                 ///warm starting (or zero if disabled)
536                                 if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
537                                 {
538                                         solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
539                                         if (rb0)
540                                                 rb0->internalApplyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
541                                         if (rb1)
542                                                 rb1->internalApplyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-solverConstraint.m_appliedImpulse);
543                                 } else
544                                 {
545                                         solverConstraint.m_appliedImpulse = 0.f;
546                                 }
547
548                                 solverConstraint.m_appliedPushImpulse = 0.f;
549
550                                 {
551                                         btScalar rel_vel;
552                                         btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rb0?rb0->getLinearVelocity():btVector3(0,0,0)) 
553                                                 + solverConstraint.m_relpos1CrossNormal.dot(rb0?rb0->getAngularVelocity():btVector3(0,0,0));
554                                         btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rb1?rb1->getLinearVelocity():btVector3(0,0,0)) 
555                                                 + solverConstraint.m_relpos2CrossNormal.dot(rb1?rb1->getAngularVelocity():btVector3(0,0,0));
556
557                                         rel_vel = vel1Dotn+vel2Dotn;
558
559                                         btScalar positionalError = 0.f;
560                                         positionalError = -penetration * infoGlobal.m_erp/infoGlobal.m_timeStep;
561                                         btScalar        velocityError = restitution - rel_vel;// * damping;
562                                         btScalar  penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
563                                         btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
564                                         if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
565                                         {
566                                                 //combine position and velocity into rhs
567                                                 solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
568                                                 solverConstraint.m_rhsPenetration = 0.f;
569                                         } else
570                                         {
571                                                 //split position and velocity into rhs and m_rhsPenetration
572                                                 solverConstraint.m_rhs = velocityImpulse;
573                                                 solverConstraint.m_rhsPenetration = penetrationImpulse;
574                                         }
575                                         solverConstraint.m_cfm = 0.f;
576                                         solverConstraint.m_lowerLimit = 0;
577                                         solverConstraint.m_upperLimit = 1e10f;
578                                 }
579
580
581
582
583 }
584
585
586
587 void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, 
588                                                                                                                                                 btRigidBody* rb0, btRigidBody* rb1, 
589                                                                                                                                  btManifoldPoint& cp, const btContactSolverInfo& infoGlobal)
590 {
591                                         if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
592                                         {
593                                                 {
594                                                         btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
595                                                         if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
596                                                         {
597                                                                 frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
598                                                                 if (rb0)
599                                                                         rb0->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
600                                                                 if (rb1)
601                                                                         rb1->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse);
602                                                         } else
603                                                         {
604                                                                 frictionConstraint1.m_appliedImpulse = 0.f;
605                                                         }
606                                                 }
607
608                                                 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
609                                                 {
610                                                         btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
611                                                         if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
612                                                         {
613                                                                 frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
614                                                                 if (rb0)
615                                                                         rb0->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
616                                                                 if (rb1)
617                                                                         rb1->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse);
618                                                         } else
619                                                         {
620                                                                 frictionConstraint2.m_appliedImpulse = 0.f;
621                                                         }
622                                                 }
623                                         } else
624                                         {
625                                                 btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
626                                                 frictionConstraint1.m_appliedImpulse = 0.f;
627                                                 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
628                                                 {
629                                                         btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
630                                                         frictionConstraint2.m_appliedImpulse = 0.f;
631                                                 }
632                                         }
633 }
634
635
636
637
638 void    btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal)
639 {
640         btCollisionObject* colObj0=0,*colObj1=0;
641
642         colObj0 = (btCollisionObject*)manifold->getBody0();
643         colObj1 = (btCollisionObject*)manifold->getBody1();
644
645
646         btRigidBody* solverBodyA = btRigidBody::upcast(colObj0);
647         btRigidBody* solverBodyB = btRigidBody::upcast(colObj1);
648
649         ///avoid collision response between two static objects
650         if ((!solverBodyA || !solverBodyA->getInvMass()) && (!solverBodyB || !solverBodyB->getInvMass()))
651                 return;
652
653         for (int j=0;j<manifold->getNumContacts();j++)
654         {
655
656                 btManifoldPoint& cp = manifold->getContactPoint(j);
657
658                 if (cp.getDistance() <= manifold->getContactProcessingThreshold())
659                 {
660                         btVector3 rel_pos1;
661                         btVector3 rel_pos2;
662                         btScalar relaxation;
663                         btScalar rel_vel;
664                         btVector3 vel;
665
666                         int frictionIndex = m_tmpSolverContactConstraintPool.size();
667                         btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing();
668                         btRigidBody* rb0 = btRigidBody::upcast(colObj0);
669                         btRigidBody* rb1 = btRigidBody::upcast(colObj1);
670                         solverConstraint.m_solverBodyA = rb0? rb0 : &getFixedBody();
671                         solverConstraint.m_solverBodyB = rb1? rb1 : &getFixedBody();
672                         solverConstraint.m_originalContactPoint = &cp;
673
674                         setupContactConstraint(solverConstraint, colObj0, colObj1, cp, infoGlobal, vel, rel_vel, relaxation, rel_pos1, rel_pos2);
675
676 //                      const btVector3& pos1 = cp.getPositionWorldOnA();
677 //                      const btVector3& pos2 = cp.getPositionWorldOnB();
678
679                         /////setup the friction constraints
680
681                         solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
682
683                         if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
684                         {
685                                 cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
686                                 btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
687                                 if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
688                                 {
689                                         cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel);
690                                         if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
691                                         {
692                                                 cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
693                                                 cp.m_lateralFrictionDir2.normalize();//??
694                                                 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
695                                                 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
696                                                 addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
697                                         }
698
699                                         applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
700                                         applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
701                                         addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
702                                         cp.m_lateralFrictionInitialized = true;
703                                 } else
704                                 {
705                                         //re-calculate friction direction every frame, todo: check if this is really needed
706                                         btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
707                                         if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
708                                         {
709                                                 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
710                                                 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
711                                                 addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
712                                         }
713
714                                         applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
715                                         applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
716                                         addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
717
718                                         cp.m_lateralFrictionInitialized = true;
719                                 }
720
721                         } else
722                         {
723                                 addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_contactCFM1);
724                                 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
725                                         addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2);
726                         }
727                         
728                         setFrictionConstraintImpulse( solverConstraint, rb0, rb1, cp, infoGlobal);
729
730                 }
731         }
732 }
733
734
735 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** /*bodies */,int /*numBodies */,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
736 {
737         BT_PROFILE("solveGroupCacheFriendlySetup");
738         (void)stackAlloc;
739         (void)debugDrawer;
740
741
742         if (!(numConstraints + numManifolds))
743         {
744                 //              printf("empty\n");
745                 return 0.f;
746         }
747
748         if (1)
749         {
750                 int j;
751                 for (j=0;j<numConstraints;j++)
752                 {
753                         btTypedConstraint* constraint = constraints[j];
754                         constraint->buildJacobian();
755                 }
756         }
757         //btRigidBody* rb0=0,*rb1=0;
758
759         //if (1)
760         {
761                 {
762
763                         int totalNumRows = 0;
764                         int i;
765                         
766                         m_tmpConstraintSizesPool.resize(numConstraints);
767                         //calculate the total number of contraint rows
768                         for (i=0;i<numConstraints;i++)
769                         {
770                                 btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
771                                 constraints[i]->getInfo1(&info1);
772                                 totalNumRows += info1.m_numConstraintRows;
773                         }
774                         m_tmpSolverNonContactConstraintPool.resize(totalNumRows);
775
776                         
777                         ///setup the btSolverConstraints
778                         int currentRow = 0;
779
780                         for (i=0;i<numConstraints;i++)
781                         {
782                                 const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
783                                 
784                                 if (info1.m_numConstraintRows)
785                                 {
786                                         btAssert(currentRow<totalNumRows);
787
788                                         btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];
789                                         btTypedConstraint* constraint = constraints[i];
790
791
792
793                                         btRigidBody& rbA = constraint->getRigidBodyA();
794                                         btRigidBody& rbB = constraint->getRigidBodyB();
795
796                                         
797                                         int j;
798                                         for ( j=0;j<info1.m_numConstraintRows;j++)
799                                         {
800                                                 memset(&currentConstraintRow[j],0,sizeof(btSolverConstraint));
801                                                 currentConstraintRow[j].m_lowerLimit = -FLT_MAX;
802                                                 currentConstraintRow[j].m_upperLimit = FLT_MAX;
803                                                 currentConstraintRow[j].m_appliedImpulse = 0.f;
804                                                 currentConstraintRow[j].m_appliedPushImpulse = 0.f;
805                                                 currentConstraintRow[j].m_solverBodyA = &rbA;
806                                                 currentConstraintRow[j].m_solverBodyB = &rbB;
807                                         }
808
809                                         rbA.internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
810                                         rbA.internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
811                                         rbB.internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
812                                         rbB.internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
813
814
815
816                                         btTypedConstraint::btConstraintInfo2 info2;
817                                         info2.fps = 1.f/infoGlobal.m_timeStep;
818                                         info2.erp = infoGlobal.m_erp;
819                                         info2.m_J1linearAxis = currentConstraintRow->m_contactNormal;
820                                         info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal;
821                                         info2.m_J2linearAxis = 0;
822                                         info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal;
823                                         info2.rowskip = sizeof(btSolverConstraint)/sizeof(btScalar);//check this
824                                         ///the size of btSolverConstraint needs be a multiple of btScalar
825                                         btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint));
826                                         info2.m_constraintError = &currentConstraintRow->m_rhs;
827                                         currentConstraintRow->m_cfm = infoGlobal.m_globalCfm;
828                                         info2.cfm = &currentConstraintRow->m_cfm;
829                                         info2.m_lowerLimit = &currentConstraintRow->m_lowerLimit;
830                                         info2.m_upperLimit = &currentConstraintRow->m_upperLimit;
831                                         info2.m_numIterations = infoGlobal.m_numIterations;
832                                         constraints[i]->getInfo2(&info2);
833
834                                         ///finalize the constraint setup
835                                         for ( j=0;j<info1.m_numConstraintRows;j++)
836                                         {
837                                                 btSolverConstraint& solverConstraint = currentConstraintRow[j];
838                                                 solverConstraint.m_originalContactPoint = constraint;
839
840                                                 {
841                                                         const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
842                                                         solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld()*ftorqueAxis1*constraint->getRigidBodyA().getAngularFactor();
843                                                 }
844                                                 {
845                                                         const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal;
846                                                         solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld()*ftorqueAxis2*constraint->getRigidBodyB().getAngularFactor();
847                                                 }
848
849                                                 {
850                                                         btVector3 iMJlA = solverConstraint.m_contactNormal*rbA.getInvMass();
851                                                         btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal;
852                                                         btVector3 iMJlB = solverConstraint.m_contactNormal*rbB.getInvMass();//sign of normal?
853                                                         btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal;
854
855                                                         btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal);
856                                                         sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
857                                                         sum += iMJlB.dot(solverConstraint.m_contactNormal);
858                                                         sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
859
860                                                         solverConstraint.m_jacDiagABInv = btScalar(1.)/sum;
861                                                 }
862
863
864                                                 ///fix rhs
865                                                 ///todo: add force/torque accelerators
866                                                 {
867                                                         btScalar rel_vel;
868                                                         btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rbA.getLinearVelocity()) + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity());
869                                                         btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rbB.getLinearVelocity()) + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity());
870
871                                                         rel_vel = vel1Dotn+vel2Dotn;
872
873                                                         btScalar restitution = 0.f;
874                                                         btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2
875                                                         btScalar        velocityError = restitution - rel_vel;// * damping;
876                                                         btScalar        penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
877                                                         btScalar        velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
878                                                         solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
879                                                         solverConstraint.m_appliedImpulse = 0.f;
880
881                                                 }
882                                         }
883                                 }
884                                 currentRow+=m_tmpConstraintSizesPool[i].m_numConstraintRows;
885                         }
886                 }
887
888                 {
889                         int i;
890                         btPersistentManifold* manifold = 0;
891 //                      btCollisionObject* colObj0=0,*colObj1=0;
892
893
894                         for (i=0;i<numManifolds;i++)
895                         {
896                                 manifold = manifoldPtr[i];
897                                 convertContact(manifold,infoGlobal);
898                         }
899                 }
900         }
901
902         btContactSolverInfo info = infoGlobal;
903
904
905
906         int numConstraintPool = m_tmpSolverContactConstraintPool.size();
907         int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
908
909         ///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
910         m_orderTmpConstraintPool.resize(numConstraintPool);
911         m_orderFrictionConstraintPool.resize(numFrictionPool);
912         {
913                 int i;
914                 for (i=0;i<numConstraintPool;i++)
915                 {
916                         m_orderTmpConstraintPool[i] = i;
917                 }
918                 for (i=0;i<numFrictionPool;i++)
919                 {
920                         m_orderFrictionConstraintPool[i] = i;
921                 }
922         }
923
924         return 0.f;
925
926 }
927
928 btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/)
929 {
930
931         int numConstraintPool = m_tmpSolverContactConstraintPool.size();
932         int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
933
934         int j;
935
936         if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
937         {
938                 if ((iteration & 7) == 0) {
939                         for (j=0; j<numConstraintPool; ++j) {
940                                 int tmp = m_orderTmpConstraintPool[j];
941                                 int swapi = btRandInt2(j+1);
942                                 m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
943                                 m_orderTmpConstraintPool[swapi] = tmp;
944                         }
945
946                         for (j=0; j<numFrictionPool; ++j) {
947                                 int tmp = m_orderFrictionConstraintPool[j];
948                                 int swapi = btRandInt2(j+1);
949                                 m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
950                                 m_orderFrictionConstraintPool[swapi] = tmp;
951                         }
952                 }
953         }
954
955         if (infoGlobal.m_solverMode & SOLVER_SIMD)
956         {
957                 ///solve all joint constraints, using SIMD, if available
958                 for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
959                 {
960                         btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
961                         resolveSingleConstraintRowGenericSIMD(*constraint.m_solverBodyA,*constraint.m_solverBodyB,constraint);
962                 }
963
964                 for (j=0;j<numConstraints;j++)
965                 {
966                         constraints[j]->solveConstraintObsolete(constraints[j]->getRigidBodyA(),constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
967                 }
968
969                 ///solve all contact constraints using SIMD, if available
970                 int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
971                 for (j=0;j<numPoolConstraints;j++)
972                 {
973                         const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
974                         resolveSingleConstraintRowLowerLimitSIMD(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
975
976                 }
977                 ///solve all friction constraints, using SIMD, if available
978                 int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
979                 for (j=0;j<numFrictionPoolConstraints;j++)
980                 {
981                         btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
982                         btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
983
984                         if (totalImpulse>btScalar(0))
985                         {
986                                 solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
987                                 solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
988
989                                 resolveSingleConstraintRowGenericSIMD(*solveManifold.m_solverBodyA,     *solveManifold.m_solverBodyB,solveManifold);
990                         }
991                 }
992         } else
993         {
994
995                 ///solve all joint constraints
996                 for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
997                 {
998                         btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
999                         resolveSingleConstraintRowGeneric(*constraint.m_solverBodyA,*constraint.m_solverBodyB,constraint);
1000                 }
1001
1002                 for (j=0;j<numConstraints;j++)
1003                 {
1004                         constraints[j]->solveConstraintObsolete(constraints[j]->getRigidBodyA(),constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
1005                 }
1006                 ///solve all contact constraints
1007                 int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1008                 for (j=0;j<numPoolConstraints;j++)
1009                 {
1010                         const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
1011                         resolveSingleConstraintRowLowerLimit(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
1012                 }
1013                 ///solve all friction constraints
1014                 int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
1015                 for (j=0;j<numFrictionPoolConstraints;j++)
1016                 {
1017                         btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
1018                         btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
1019
1020                         if (totalImpulse>btScalar(0))
1021                         {
1022                                 solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
1023                                 solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
1024
1025                                 resolveSingleConstraintRowGeneric(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
1026                         }
1027                 }
1028         }
1029         return 0.f;
1030 }
1031
1032
1033 void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
1034 {
1035         int iteration;
1036         if (infoGlobal.m_splitImpulse)
1037         {
1038                 if (infoGlobal.m_solverMode & SOLVER_SIMD)
1039                 {
1040                         for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
1041                         {
1042                                 {
1043                                         int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1044                                         int j;
1045                                         for (j=0;j<numPoolConstraints;j++)
1046                                         {
1047                                                 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
1048
1049                                                 resolveSplitPenetrationSIMD(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
1050                                         }
1051                                 }
1052                         }
1053                 }
1054                 else
1055                 {
1056                         for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
1057                         {
1058                                 {
1059                                         int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1060                                         int j;
1061                                         for (j=0;j<numPoolConstraints;j++)
1062                                         {
1063                                                 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
1064
1065                                                 resolveSplitPenetrationImpulseCacheFriendly(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
1066                                         }
1067                                 }
1068                         }
1069                 }
1070         }
1071 }
1072
1073 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
1074 {
1075         BT_PROFILE("solveGroupCacheFriendlyIterations");
1076
1077         
1078         //should traverse the contacts random order...
1079         int iteration;
1080         {
1081                 for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
1082                 {                       
1083                         solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
1084                 }
1085                 
1086                 solveGroupCacheFriendlySplitImpulseIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
1087         }
1088         return 0.f;
1089 }
1090
1091 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies ,int numBodies,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** /*constraints*/,int /* numConstraints*/,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/)
1092 {
1093         int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1094         int i,j;
1095
1096         for (j=0;j<numPoolConstraints;j++)
1097         {
1098
1099                 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j];
1100                 btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
1101                 btAssert(pt);
1102                 pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
1103                 if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
1104                 {
1105                         pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
1106                         pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
1107                 }
1108
1109                 //do a callback here?
1110         }
1111
1112         numPoolConstraints = m_tmpSolverNonContactConstraintPool.size();
1113         for (j=0;j<numPoolConstraints;j++)
1114         {
1115                 const btSolverConstraint& solverConstr = m_tmpSolverNonContactConstraintPool[j];
1116                 btTypedConstraint* constr = (btTypedConstraint*)solverConstr.m_originalContactPoint;
1117                 btScalar sum = constr->internalGetAppliedImpulse();
1118                 sum += solverConstr.m_appliedImpulse;
1119                 constr->internalSetAppliedImpulse(sum);
1120         }
1121
1122
1123         if (infoGlobal.m_splitImpulse)
1124         {               
1125                 for ( i=0;i<numBodies;i++)
1126                 {
1127                         btRigidBody* body = btRigidBody::upcast(bodies[i]);
1128                         if (body)
1129                                 body->internalWritebackVelocity(infoGlobal.m_timeStep);
1130                 }
1131         } else
1132         {
1133                 for ( i=0;i<numBodies;i++)
1134                 {
1135                         btRigidBody* body = btRigidBody::upcast(bodies[i]);
1136                         if (body)
1137                                 body->internalWritebackVelocity();
1138                 }
1139         }
1140
1141
1142         m_tmpSolverContactConstraintPool.resize(0);
1143         m_tmpSolverNonContactConstraintPool.resize(0);
1144         m_tmpSolverContactFrictionConstraintPool.resize(0);
1145
1146         return 0.f;
1147 }
1148
1149
1150
1151 /// btSequentialImpulseConstraintSolver Sequentially applies impulses
1152 btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /*dispatcher*/)
1153 {
1154
1155         BT_PROFILE("solveGroup");
1156         //you need to provide at least some bodies
1157         btAssert(bodies);
1158         btAssert(numBodies);
1159
1160         solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
1161
1162         solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
1163
1164         solveGroupCacheFriendlyFinish(bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
1165         
1166         return 0.f;
1167 }
1168
1169 void    btSequentialImpulseConstraintSolver::reset()
1170 {
1171         m_btSeed2 = 0;
1172 }
1173
1174