dali-physics/third-party/bullet3/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp

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