dali-physics/third-party/bullet3/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

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