Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/trunk/src/external/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @ 7725

Last change on this file since 7725 was 5781, checked in by rgrieder, 15 years ago

Reverted trunk again. We might want to find a way to delete these revisions again (x3n's changes are still available as diff in the commit mails).

  • Property svn:eol-style set to native
File size: 39.8 KB
Line 
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. 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
37btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
38:m_btSeed2(0)
39{
40
41}
42
43btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
44{
45}
46
47#ifdef USE_SIMD
48#include <emmintrin.h>
49#define vec_splat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e,e,e,e))
50static inline __m128 _vmathVfDot3( __m128 vec0, __m128 vec1 )
51{
52        __m128 result = _mm_mul_ps( vec0, vec1);
53        return _mm_add_ps( vec_splat( result, 0 ), _mm_add_ps( vec_splat( result, 1 ), vec_splat( result, 2 ) ) );
54}
55#endif//USE_SIMD
56
57// Project Gauss Seidel or the equivalent Sequential Impulse
58void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
59{
60#ifdef USE_SIMD
61        __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
62        __m128  lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
63        __m128  upperLimit1 = _mm_set1_ps(c.m_upperLimit);
64        __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)));
65        __m128 deltaVel1Dotn    =       _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.m_deltaLinearVelocity.mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.m_deltaAngularVelocity.mVec128));
66        __m128 deltaVel2Dotn    =       _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.m_deltaAngularVelocity.mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.m_deltaLinearVelocity.mVec128));
67        deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
68        deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
69        btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
70        btSimdScalar resultLowerLess,resultUpperLess;
71        resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
72        resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
73        __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
74        deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
75        c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
76        __m128 upperMinApplied = _mm_sub_ps(upperLimit1,cpAppliedImp);
77        deltaImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied) );
78        c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1) );
79        __m128  linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,_mm_set1_ps(body1.m_invMass));
80        __m128  linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,_mm_set1_ps(body2.m_invMass));
81        __m128 impulseMagnitude = deltaImpulse;
82        body1.m_deltaLinearVelocity.mVec128 = _mm_add_ps(body1.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
83        body1.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body1.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
84        body2.m_deltaLinearVelocity.mVec128 = _mm_sub_ps(body2.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
85        body2.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body2.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
86#else
87        resolveSingleConstraintRowGeneric(body1,body2,c);
88#endif
89}
90
91// Project Gauss Seidel or the equivalent Sequential Impulse
92 void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
93{
94        btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
95        const btScalar deltaVel1Dotn    =       c.m_contactNormal.dot(body1.m_deltaLinearVelocity)      + c.m_relpos1CrossNormal.dot(body1.m_deltaAngularVelocity);
96        const btScalar deltaVel2Dotn    =       -c.m_contactNormal.dot(body2.m_deltaLinearVelocity) + c.m_relpos2CrossNormal.dot(body2.m_deltaAngularVelocity);
97
98        const btScalar delta_rel_vel    =       deltaVel1Dotn-deltaVel2Dotn;
99        deltaImpulse    -=      deltaVel1Dotn*c.m_jacDiagABInv;
100        deltaImpulse    -=      deltaVel2Dotn*c.m_jacDiagABInv;
101
102        const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
103        if (sum < c.m_lowerLimit)
104        {
105                deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
106                c.m_appliedImpulse = c.m_lowerLimit;
107        }
108        else if (sum > c.m_upperLimit) 
109        {
110                deltaImpulse = c.m_upperLimit-c.m_appliedImpulse;
111                c.m_appliedImpulse = c.m_upperLimit;
112        }
113        else
114        {
115                c.m_appliedImpulse = sum;
116        }
117        if (body1.m_invMass)
118                body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse);
119        if (body2.m_invMass)
120                body2.applyImpulse(-c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,deltaImpulse);
121}
122
123 void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& 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.m_deltaLinearVelocity.mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.m_deltaAngularVelocity.mVec128));
131        __m128 deltaVel2Dotn    =       _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.m_deltaAngularVelocity.mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.m_deltaLinearVelocity.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,_mm_set1_ps(body1.m_invMass));
142        __m128  linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,_mm_set1_ps(body2.m_invMass));
143        __m128 impulseMagnitude = deltaImpulse;
144        body1.m_deltaLinearVelocity.mVec128 = _mm_add_ps(body1.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
145        body1.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body1.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
146        body2.m_deltaLinearVelocity.mVec128 = _mm_sub_ps(body2.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
147        body2.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body2.m_deltaAngularVelocity.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(btSolverBody& body1,btSolverBody& 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.m_deltaLinearVelocity)      + c.m_relpos1CrossNormal.dot(body1.m_deltaAngularVelocity);
158        const btScalar deltaVel2Dotn    =       -c.m_contactNormal.dot(body2.m_deltaLinearVelocity) + c.m_relpos2CrossNormal.dot(body2.m_deltaAngularVelocity);
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        if (body1.m_invMass)
173                body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse);
174        if (body2.m_invMass)
175                body2.applyImpulse(-c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,deltaImpulse);
176}
177
178
179
180unsigned long btSequentialImpulseConstraintSolver::btRand2()
181{
182        m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff;
183        return m_btSeed2;
184}
185
186
187
188//See ODE: adam's all-int straightforward(?) dRandInt (0..n-1)
189int btSequentialImpulseConstraintSolver::btRandInt2 (int n)
190{
191        // seems good; xor-fold and modulus
192        const unsigned long un = static_cast<unsigned long>(n);
193        unsigned long r = btRand2();
194
195        // note: probably more aggressive than it needs to be -- might be
196        //       able to get away without one or two of the innermost branches.
197        if (un <= 0x00010000UL) {
198                r ^= (r >> 16);
199                if (un <= 0x00000100UL) {
200                        r ^= (r >> 8);
201                        if (un <= 0x00000010UL) {
202                                r ^= (r >> 4);
203                                if (un <= 0x00000004UL) {
204                                        r ^= (r >> 2);
205                                        if (un <= 0x00000002UL) {
206                                                r ^= (r >> 1);
207                                        }
208                                }
209                        }
210                }
211        }
212
213        return (int) (r % un);
214}
215
216
217
218void    btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject)
219{
220        btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0;
221
222        solverBody->m_deltaLinearVelocity.setValue(0.f,0.f,0.f);
223        solverBody->m_deltaAngularVelocity.setValue(0.f,0.f,0.f);
224
225        if (rb)
226        {
227                solverBody->m_invMass = rb->getInvMass();
228                solverBody->m_originalBody = rb;
229                solverBody->m_angularFactor = rb->getAngularFactor();
230        } else
231        {
232                solverBody->m_invMass = 0.f;
233                solverBody->m_originalBody = 0;
234                solverBody->m_angularFactor = 1.f;
235        }
236}
237
238
239int             gNumSplitImpulseRecoveries = 0;
240
241btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution)
242{
243        btScalar rest = restitution * -rel_vel;
244        return rest;
245}
246
247
248
249void    applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection);
250void    applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection)
251{
252        if (colObj && colObj->hasAnisotropicFriction())
253        {
254                // transform to local coordinates
255                btVector3 loc_lateral = frictionDirection * colObj->getWorldTransform().getBasis();
256                const btVector3& friction_scaling = colObj->getAnisotropicFriction();
257                //apply anisotropic friction
258                loc_lateral *= friction_scaling;
259                // ... and transform it back to global coordinates
260                frictionDirection = colObj->getWorldTransform().getBasis() * loc_lateral;
261        }
262}
263
264
265
266btSolverConstraint&     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)
267{
268
269
270        btRigidBody* body0=btRigidBody::upcast(colObj0);
271        btRigidBody* body1=btRigidBody::upcast(colObj1);
272
273        btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expand();
274        memset(&solverConstraint,0xff,sizeof(btSolverConstraint));
275        solverConstraint.m_contactNormal = normalAxis;
276
277        solverConstraint.m_solverBodyIdA = solverBodyIdA;
278        solverConstraint.m_solverBodyIdB = solverBodyIdB;
279        solverConstraint.m_frictionIndex = frictionIndex;
280
281        solverConstraint.m_friction = cp.m_combinedFriction;
282        solverConstraint.m_originalContactPoint = 0;
283
284        solverConstraint.m_appliedImpulse = 0.f;
285        //      solverConstraint.m_appliedPushImpulse = 0.f;
286
287        {
288                btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal);
289                solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
290                solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor() : btVector3(0,0,0);
291        }
292        {
293                btVector3 ftorqueAxis1 = rel_pos2.cross(-solverConstraint.m_contactNormal);
294                solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
295                solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor() : btVector3(0,0,0);
296        }
297
298#ifdef COMPUTE_IMPULSE_DENOM
299        btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal);
300        btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal);
301#else
302        btVector3 vec;
303        btScalar denom0 = 0.f;
304        btScalar denom1 = 0.f;
305        if (body0)
306        {
307                vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
308                denom0 = body0->getInvMass() + normalAxis.dot(vec);
309        }
310        if (body1)
311        {
312                vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
313                denom1 = body1->getInvMass() + normalAxis.dot(vec);
314        }
315
316
317#endif //COMPUTE_IMPULSE_DENOM
318        btScalar denom = relaxation/(denom0+denom1);
319        solverConstraint.m_jacDiagABInv = denom;
320
321#ifdef _USE_JACOBIAN
322        solverConstraint.m_jac =  btJacobianEntry (
323                rel_pos1,rel_pos2,solverConstraint.m_contactNormal,
324                body0->getInvInertiaDiagLocal(),
325                body0->getInvMass(),
326                body1->getInvInertiaDiagLocal(),
327                body1->getInvMass());
328#endif //_USE_JACOBIAN
329
330
331        {
332                btScalar rel_vel;
333                btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0?body0->getLinearVelocity():btVector3(0,0,0)) 
334                        + solverConstraint.m_relpos1CrossNormal.dot(body0?body0->getAngularVelocity():btVector3(0,0,0));
335                btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(body1?body1->getLinearVelocity():btVector3(0,0,0)) 
336                        + solverConstraint.m_relpos2CrossNormal.dot(body1?body1->getAngularVelocity():btVector3(0,0,0));
337
338                rel_vel = vel1Dotn+vel2Dotn;
339
340                btScalar positionalError = 0.f;
341
342                btSimdScalar velocityError =  - rel_vel;
343                btSimdScalar    velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
344                solverConstraint.m_rhs = velocityImpulse;
345                solverConstraint.m_cfm = 0.f;
346                solverConstraint.m_lowerLimit = 0;
347                solverConstraint.m_upperLimit = 1e10f;
348        }
349
350        return solverConstraint;
351}
352
353int     btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body)
354{
355        int solverBodyIdA = -1;
356
357        if (body.getCompanionId() >= 0)
358        {
359                //body has already been converted
360                solverBodyIdA = body.getCompanionId();
361        } else
362        {
363                btRigidBody* rb = btRigidBody::upcast(&body);
364                if (rb && rb->getInvMass())
365                {
366                        solverBodyIdA = m_tmpSolverBodyPool.size();
367                        btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
368                        initSolverBody(&solverBody,&body);
369                        body.setCompanionId(solverBodyIdA);
370                } else
371                {
372                        return 0;//assume first one is a fixed solver body
373                }
374        }
375        return solverBodyIdA;
376}
377#include <stdio.h>
378
379
380
381void    btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal)
382{
383        btCollisionObject* colObj0=0,*colObj1=0;
384
385        colObj0 = (btCollisionObject*)manifold->getBody0();
386        colObj1 = (btCollisionObject*)manifold->getBody1();
387
388        int solverBodyIdA=-1;
389        int solverBodyIdB=-1;
390
391        if (manifold->getNumContacts())
392        {
393                solverBodyIdA = getOrInitSolverBody(*colObj0);
394                solverBodyIdB = getOrInitSolverBody(*colObj1);
395        }
396
397        ///avoid collision response between two static objects
398        if (!solverBodyIdA && !solverBodyIdB)
399                return;
400
401        btVector3 rel_pos1;
402        btVector3 rel_pos2;
403        btScalar relaxation;
404
405        for (int j=0;j<manifold->getNumContacts();j++)
406        {
407
408                btManifoldPoint& cp = manifold->getContactPoint(j);
409
410                if (cp.getDistance() <= manifold->getContactProcessingThreshold())
411                {
412
413                        const btVector3& pos1 = cp.getPositionWorldOnA();
414                        const btVector3& pos2 = cp.getPositionWorldOnB();
415
416                        rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); 
417                        rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
418
419
420                        relaxation = 1.f;
421                        btScalar rel_vel;
422                        btVector3 vel;
423
424                        int frictionIndex = m_tmpSolverContactConstraintPool.size();
425
426                        {
427                                btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expand();
428                                btRigidBody* rb0 = btRigidBody::upcast(colObj0);
429                                btRigidBody* rb1 = btRigidBody::upcast(colObj1);
430
431                                solverConstraint.m_solverBodyIdA = solverBodyIdA;
432                                solverConstraint.m_solverBodyIdB = solverBodyIdB;
433
434                                solverConstraint.m_originalContactPoint = &cp;
435
436                                btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
437                                solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
438                                btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);           
439                                solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
440                                {
441#ifdef COMPUTE_IMPULSE_DENOM
442                                        btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
443                                        btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
444#else                                                   
445                                        btVector3 vec;
446                                        btScalar denom0 = 0.f;
447                                        btScalar denom1 = 0.f;
448                                        if (rb0)
449                                        {
450                                                vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
451                                                denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
452                                        }
453                                        if (rb1)
454                                        {
455                                                vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
456                                                denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
457                                        }
458#endif //COMPUTE_IMPULSE_DENOM         
459
460                                        btScalar denom = relaxation/(denom0+denom1);
461                                        solverConstraint.m_jacDiagABInv = denom;
462                                }
463
464                                solverConstraint.m_contactNormal = cp.m_normalWorldOnB;
465                                solverConstraint.m_relpos1CrossNormal = rel_pos1.cross(cp.m_normalWorldOnB);
466                                solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(-cp.m_normalWorldOnB);
467
468
469                                btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
470                                btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
471
472                                vel  = vel1 - vel2;
473
474                                rel_vel = cp.m_normalWorldOnB.dot(vel);
475
476                                btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop;
477
478
479                                solverConstraint.m_friction = cp.m_combinedFriction;
480
481                                btScalar restitution = 0.f;
482                               
483                                if (cp.m_lifeTime>infoGlobal.m_restingContactRestitutionThreshold)
484                                {
485                                        restitution = 0.f;
486                                } else
487                                {
488                                        restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution);
489                                        if (restitution <= btScalar(0.))
490                                        {
491                                                restitution = 0.f;
492                                        };
493                                }
494
495
496                                ///warm starting (or zero if disabled)
497                                if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
498                                {
499                                        solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
500                                        if (rb0)
501                                                m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
502                                        if (rb1)
503                                                m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass(),-solverConstraint.m_angularComponentB,-solverConstraint.m_appliedImpulse);
504                                } else
505                                {
506                                        solverConstraint.m_appliedImpulse = 0.f;
507                                }
508
509                                //                                                      solverConstraint.m_appliedPushImpulse = 0.f;
510
511                                {
512                                        btScalar rel_vel;
513                                        btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rb0?rb0->getLinearVelocity():btVector3(0,0,0)) 
514                                                + solverConstraint.m_relpos1CrossNormal.dot(rb0?rb0->getAngularVelocity():btVector3(0,0,0));
515                                        btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rb1?rb1->getLinearVelocity():btVector3(0,0,0)) 
516                                                + solverConstraint.m_relpos2CrossNormal.dot(rb1?rb1->getAngularVelocity():btVector3(0,0,0));
517
518                                        rel_vel = vel1Dotn+vel2Dotn;
519
520                                        btScalar positionalError = 0.f;
521                                        positionalError = -penetration * infoGlobal.m_erp/infoGlobal.m_timeStep;
522                                        btScalar        velocityError = restitution - rel_vel;// * damping;
523                                        btScalar  penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
524                                        btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
525                                        solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
526                                        solverConstraint.m_cfm = 0.f;
527                                        solverConstraint.m_lowerLimit = 0;
528                                        solverConstraint.m_upperLimit = 1e10f;
529                                }
530
531
532                                /////setup the friction constraints
533
534
535
536                                if (1)
537                                {
538                                        solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
539                                        if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
540                                        {
541                                                cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
542                                                btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
543                                                if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
544                                                {
545                                                        cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel);
546                                                        applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
547                                                        applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
548                                                        addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
549                                                        if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
550                                                        {
551                                                                cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
552                                                                cp.m_lateralFrictionDir2.normalize();//??
553                                                                applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
554                                                                applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
555                                                                addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
556                                                        }
557                                                        cp.m_lateralFrictionInitialized = true;
558                                                } else
559                                                {
560                                                        //re-calculate friction direction every frame, todo: check if this is really needed
561                                                        btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
562                                                        applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
563                                                        applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
564
565                                                        addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
566                                                        if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
567                                                        {
568                                                                applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
569                                                                applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
570                                                                addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
571                                                        }
572                                                        cp.m_lateralFrictionInitialized = true;
573                                                }
574
575                                        } else
576                                        {
577                                                addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
578                                                if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
579                                                        addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
580                                        }
581
582                                        if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
583                                        {
584                                                {
585                                                        btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
586                                                        if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
587                                                        {
588                                                                frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
589                                                                if (rb0)
590                                                                        m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
591                                                                if (rb1)
592                                                                        m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass(),-frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse);
593                                                        } else
594                                                        {
595                                                                frictionConstraint1.m_appliedImpulse = 0.f;
596                                                        }
597                                                }
598
599                                                if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
600                                                {
601                                                        btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
602                                                        if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
603                                                        {
604                                                                frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
605                                                                if (rb0)
606                                                                        m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
607                                                                if (rb1)
608                                                                        m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse);
609                                                        } else
610                                                        {
611                                                                frictionConstraint2.m_appliedImpulse = 0.f;
612                                                        }
613                                                }
614                                        } else
615                                        {
616                                                btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
617                                                frictionConstraint1.m_appliedImpulse = 0.f;
618                                                if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
619                                                {
620                                                        btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
621                                                        frictionConstraint2.m_appliedImpulse = 0.f;
622                                                }
623                                        }
624                                }
625                        }
626
627
628                }
629        }
630}
631
632
633btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** /*bodies */,int /*numBodies */,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
634{
635        BT_PROFILE("solveGroupCacheFriendlySetup");
636        (void)stackAlloc;
637        (void)debugDrawer;
638
639
640        if (!(numConstraints + numManifolds))
641        {
642                //              printf("empty\n");
643                return 0.f;
644        }
645
646        if (1)
647        {
648                int j;
649                for (j=0;j<numConstraints;j++)
650                {
651                        btTypedConstraint* constraint = constraints[j];
652                        constraint->buildJacobian();
653                }
654        }
655
656        btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
657        initSolverBody(&fixedBody,0);
658
659        //btRigidBody* rb0=0,*rb1=0;
660
661        //if (1)
662        {
663                {
664
665                        int totalNumRows = 0;
666                        int i;
667                        //calculate the total number of contraint rows
668                        for (i=0;i<numConstraints;i++)
669                        {
670
671                                btTypedConstraint::btConstraintInfo1 info1;
672                                constraints[i]->getInfo1(&info1);
673                                totalNumRows += info1.m_numConstraintRows;
674                        }
675                        m_tmpSolverNonContactConstraintPool.resize(totalNumRows);
676
677                        btTypedConstraint::btConstraintInfo1 info1;
678                        info1.m_numConstraintRows = 0;
679
680
681                        ///setup the btSolverConstraints
682                        int currentRow = 0;
683
684                        for (i=0;i<numConstraints;i++,currentRow+=info1.m_numConstraintRows)
685                        {
686                                constraints[i]->getInfo1(&info1);
687                                if (info1.m_numConstraintRows)
688                                {
689                                        btAssert(currentRow<totalNumRows);
690
691                                        btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];
692                                        btTypedConstraint* constraint = constraints[i];
693
694
695
696                                        btRigidBody& rbA = constraint->getRigidBodyA();
697                                        btRigidBody& rbB = constraint->getRigidBodyB();
698
699                                        int solverBodyIdA = getOrInitSolverBody(rbA);
700                                        int solverBodyIdB = getOrInitSolverBody(rbB);
701
702                                        btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
703                                        btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
704
705                                        int j;
706                                        for ( j=0;j<info1.m_numConstraintRows;j++)
707                                        {
708                                                memset(&currentConstraintRow[j],0,sizeof(btSolverConstraint));
709                                                currentConstraintRow[j].m_lowerLimit = -FLT_MAX;
710                                                currentConstraintRow[j].m_upperLimit = FLT_MAX;
711                                                currentConstraintRow[j].m_appliedImpulse = 0.f;
712                                                currentConstraintRow[j].m_appliedPushImpulse = 0.f;
713                                                currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA;
714                                                currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB;
715                                        }
716
717                                        bodyAPtr->m_deltaLinearVelocity.setValue(0.f,0.f,0.f);
718                                        bodyAPtr->m_deltaAngularVelocity.setValue(0.f,0.f,0.f);
719                                        bodyBPtr->m_deltaLinearVelocity.setValue(0.f,0.f,0.f);
720                                        bodyBPtr->m_deltaAngularVelocity.setValue(0.f,0.f,0.f);
721
722
723
724                                        btTypedConstraint::btConstraintInfo2 info2;
725                                        info2.fps = 1.f/infoGlobal.m_timeStep;
726                                        info2.erp = infoGlobal.m_erp;
727                                        info2.m_J1linearAxis = currentConstraintRow->m_contactNormal;
728                                        info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal;
729                                        info2.m_J2linearAxis = 0;
730                                        info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal;
731                                        info2.rowskip = sizeof(btSolverConstraint)/sizeof(btScalar);//check this
732                                        ///the size of btSolverConstraint needs be a multiple of btScalar
733                                        btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint));
734                                        info2.m_constraintError = &currentConstraintRow->m_rhs;
735                                        info2.cfm = &currentConstraintRow->m_cfm;
736                                        info2.m_lowerLimit = &currentConstraintRow->m_lowerLimit;
737                                        info2.m_upperLimit = &currentConstraintRow->m_upperLimit;
738                                        constraints[i]->getInfo2(&info2);
739
740                                        ///finalize the constraint setup
741                                        for ( j=0;j<info1.m_numConstraintRows;j++)
742                                        {
743                                                btSolverConstraint& solverConstraint = currentConstraintRow[j];
744
745                                                {
746                                                        const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
747                                                        solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld()*ftorqueAxis1*constraint->getRigidBodyA().getAngularFactor();
748                                                }
749                                                {
750                                                        const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal;
751                                                        solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld()*ftorqueAxis2*constraint->getRigidBodyB().getAngularFactor();
752                                                }
753
754                                                {
755                                                        btVector3 iMJlA = solverConstraint.m_contactNormal*rbA.getInvMass();
756                                                        btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal;
757                                                        btVector3 iMJlB = solverConstraint.m_contactNormal*rbB.getInvMass();//sign of normal?
758                                                        btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal;
759
760                                                        btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal);
761                                                        sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
762                                                        sum += iMJlB.dot(solverConstraint.m_contactNormal);
763                                                        sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
764
765                                                        solverConstraint.m_jacDiagABInv = btScalar(1.)/sum;
766                                                }
767
768
769                                                ///fix rhs
770                                                ///todo: add force/torque accelerators
771                                                {
772                                                        btScalar rel_vel;
773                                                        btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rbA.getLinearVelocity()) + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity());
774                                                        btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rbB.getLinearVelocity()) + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity());
775
776                                                        rel_vel = vel1Dotn+vel2Dotn;
777
778                                                        btScalar restitution = 0.f;
779                                                        btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2
780                                                        btScalar        velocityError = restitution - rel_vel;// * damping;
781                                                        btScalar        penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
782                                                        btScalar        velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
783                                                        solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
784                                                        solverConstraint.m_appliedImpulse = 0.f;
785
786                                                }
787                                        }
788                                }
789                        }
790                }
791
792                {
793                        int i;
794                        btPersistentManifold* manifold = 0;
795                        btCollisionObject* colObj0=0,*colObj1=0;
796
797
798                        for (i=0;i<numManifolds;i++)
799                        {
800                                manifold = manifoldPtr[i];
801                                convertContact(manifold,infoGlobal);
802                        }
803                }
804        }
805
806        btContactSolverInfo info = infoGlobal;
807
808
809
810        int numConstraintPool = m_tmpSolverContactConstraintPool.size();
811        int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
812
813        ///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
814        m_orderTmpConstraintPool.resize(numConstraintPool);
815        m_orderFrictionConstraintPool.resize(numFrictionPool);
816        {
817                int i;
818                for (i=0;i<numConstraintPool;i++)
819                {
820                        m_orderTmpConstraintPool[i] = i;
821                }
822                for (i=0;i<numFrictionPool;i++)
823                {
824                        m_orderFrictionConstraintPool[i] = i;
825                }
826        }
827
828        return 0.f;
829
830}
831
832btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/)
833{
834        BT_PROFILE("solveGroupCacheFriendlyIterations");
835
836        int numConstraintPool = m_tmpSolverContactConstraintPool.size();
837        int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
838
839        //should traverse the contacts random order...
840        int iteration;
841        {
842                for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
843                {                       
844
845                        int j;
846                        if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
847                        {
848                                if ((iteration & 7) == 0) {
849                                        for (j=0; j<numConstraintPool; ++j) {
850                                                int tmp = m_orderTmpConstraintPool[j];
851                                                int swapi = btRandInt2(j+1);
852                                                m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
853                                                m_orderTmpConstraintPool[swapi] = tmp;
854                                        }
855
856                                        for (j=0; j<numFrictionPool; ++j) {
857                                                int tmp = m_orderFrictionConstraintPool[j];
858                                                int swapi = btRandInt2(j+1);
859                                                m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
860                                                m_orderFrictionConstraintPool[swapi] = tmp;
861                                        }
862                                }
863                        }
864
865                        if (infoGlobal.m_solverMode & SOLVER_SIMD)
866                        {
867                                ///solve all joint constraints, using SIMD, if available
868                                for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
869                                {
870                                        btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
871                                        resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
872                                }
873
874                                for (j=0;j<numConstraints;j++)
875                                {
876                                        int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA());
877                                        int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB());
878                                        btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
879                                        btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
880                                        constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
881                                }
882
883                                ///solve all contact constraints using SIMD, if available
884                                int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
885                                for (j=0;j<numPoolConstraints;j++)
886                                {
887                                        const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
888                                        resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
889
890                                }
891                                ///solve all friction constraints, using SIMD, if available
892                                int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
893                                for (j=0;j<numFrictionPoolConstraints;j++)
894                                {
895                                        btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
896                                        btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
897
898                                        if (totalImpulse>btScalar(0))
899                                        {
900                                                solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
901                                                solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
902
903                                                resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],       m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
904                                        }
905                                }
906                        } else
907                        {
908
909                                ///solve all joint constraints
910                                for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
911                                {
912                                        btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
913                                        resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
914                                }
915
916                                for (j=0;j<numConstraints;j++)
917                                {
918                                        int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA());
919                                        int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB());
920                                        btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
921                                        btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
922
923                                        constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
924                                }
925
926                                ///solve all contact constraints
927                                int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
928                                for (j=0;j<numPoolConstraints;j++)
929                                {
930                                        const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
931                                        resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
932                                }
933                                ///solve all friction constraints
934                                int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
935                                for (j=0;j<numFrictionPoolConstraints;j++)
936                                {
937                                        btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
938                                        btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
939
940                                        if (totalImpulse>btScalar(0))
941                                        {
942                                                solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
943                                                solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
944
945                                                resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],                                                   m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
946                                        }
947                                }
948                        }
949
950
951
952                }
953        }
954        return 0.f;
955}
956
957
958
959/// btSequentialImpulseConstraintSolver Sequentially applies impulses
960btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /*dispatcher*/)
961{
962
963       
964
965        BT_PROFILE("solveGroup");
966        //we only implement SOLVER_CACHE_FRIENDLY now
967        //you need to provide at least some bodies
968        btAssert(bodies);
969        btAssert(numBodies);
970
971        int i;
972
973        solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
974        solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
975
976        int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
977        int j;
978
979        for (j=0;j<numPoolConstraints;j++)
980        {
981
982                const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j];
983                btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
984                btAssert(pt);
985                pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
986                if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
987                {
988                        pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
989                        pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
990                }
991
992                //do a callback here?
993        }
994
995        if (infoGlobal.m_splitImpulse)
996        {               
997                for ( i=0;i<m_tmpSolverBodyPool.size();i++)
998                {
999                        m_tmpSolverBodyPool[i].writebackVelocity(infoGlobal.m_timeStep);
1000                }
1001        } else
1002        {
1003                for ( i=0;i<m_tmpSolverBodyPool.size();i++)
1004                {
1005                        m_tmpSolverBodyPool[i].writebackVelocity();
1006                }
1007        }
1008
1009
1010        m_tmpSolverBodyPool.resize(0);
1011        m_tmpSolverContactConstraintPool.resize(0);
1012        m_tmpSolverNonContactConstraintPool.resize(0);
1013        m_tmpSolverContactFrictionConstraintPool.resize(0);
1014
1015        return 0.f;
1016}
1017
1018
1019
1020
1021
1022
1023
1024
1025
1026void    btSequentialImpulseConstraintSolver::reset()
1027{
1028        m_btSeed2 = 0;
1029}
1030
1031
Note: See TracBrowser for help on using the repository browser.