dali-physics/third-party/bullet3/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
   4
   5 This software is provided 'as-is', without any express or implied warranty.
   6 In no event will the authors be held liable for any damages arising from the use of this software.
   7 Permission is granted to anyone to use this software for any purpose,
   8 including commercial applications, and to alter it and redistribute it freely,
   9 subject to the following restrictions:
  10
  11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
  12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  13 3. This notice may not be removed or altered from any source distribution.
  14 */
  15
  16 #include "btMultiBodyConstraintSolver.h"
  17 #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
  18 #include "btMultiBodyLinkCollider.h"
  19
  20 #include "BulletDynamics/ConstraintSolver/btSolverBody.h"
  21 #include "btMultiBodyConstraint.h"
  22 #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
  23
  24 #include "LinearMath/btQuickprof.h"
  25 #include "BulletDynamics/Featherstone/btMultiBodySolverConstraint.h"
  26 #include "LinearMath/btScalar.h"
  27
  28 btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
  29 {
  30         btScalar leastSquaredResidual = btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
  31
  32         //solve featherstone non-contact constraints
  33         btScalar nonContactResidual = 0;
  34         //printf("m_multiBodyNonContactConstraints = %d\n",m_multiBodyNonContactConstraints.size());
  35         for (int i = 0; i < infoGlobal.m_numNonContactInnerIterations; ++i)
  36         {
  37                 // reset the nonContactResdual to 0 at start of each inner iteration
  38                 nonContactResidual = 0;
  39                 for (int j = 0; j < m_multiBodyNonContactConstraints.size(); j++)
  40                 {
  41                         int index = iteration & 1 ? j : m_multiBodyNonContactConstraints.size() - 1 - j;
  42
  43                         btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[index];
  44
  45                         btScalar residual = resolveSingleConstraintRowGeneric(constraint);
  46                         nonContactResidual = btMax(nonContactResidual, residual * residual);
  47
  48                         if (constraint.m_multiBodyA)
  49                                 constraint.m_multiBodyA->setPosUpdated(false);
  50                         if (constraint.m_multiBodyB)
  51                                 constraint.m_multiBodyB->setPosUpdated(false);
  52                 }
  53         }
  54         leastSquaredResidual = btMax(leastSquaredResidual, nonContactResidual);
  55
  56         //solve featherstone normal contact
  57         for (int j0 = 0; j0 < m_multiBodyNormalContactConstraints.size(); j0++)
  58         {
  59                 int index = j0;  //iteration&1? j0 : m_multiBodyNormalContactConstraints.size()-1-j0;
  60
  61                 btMultiBodySolverConstraint& constraint = m_multiBodyNormalContactConstraints[index];
  62                 btScalar residual = 0.f;
  63
  64                 if (iteration < infoGlobal.m_numIterations)
  65                 {
  66                         residual = resolveSingleConstraintRowGeneric(constraint);
  67                 }
  68
  69                 leastSquaredResidual = btMax(leastSquaredResidual, residual * residual);
  70
  71                 if (constraint.m_multiBodyA)
  72                         constraint.m_multiBodyA->setPosUpdated(false);
  73                 if (constraint.m_multiBodyB)
  74                         constraint.m_multiBodyB->setPosUpdated(false);
  75         }
  76
  77         //solve featherstone frictional contact
  78         if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS && ((infoGlobal.m_solverMode & SOLVER_DISABLE_IMPLICIT_CONE_FRICTION) == 0))
  79         {
  80                 for (int j1 = 0; j1 < this->m_multiBodySpinningFrictionContactConstraints.size(); j1++)
  81                 {
  82                         if (iteration < infoGlobal.m_numIterations)
  83                         {
  84                                 int index = j1;
  85
  86                                 btMultiBodySolverConstraint& frictionConstraint = m_multiBodySpinningFrictionContactConstraints[index];
  87                                 btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
  88                                 //adjust friction limits here
  89                                 if (totalImpulse > btScalar(0))
  90                                 {
  91                                         frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction * totalImpulse);
  92                                         frictionConstraint.m_upperLimit = frictionConstraint.m_friction * totalImpulse;
  93                                         btScalar residual = resolveSingleConstraintRowGeneric(frictionConstraint);
  94                                         leastSquaredResidual = btMax(leastSquaredResidual, residual * residual);
  95
  96                                         if (frictionConstraint.m_multiBodyA)
  97                                                 frictionConstraint.m_multiBodyA->setPosUpdated(false);
  98                                         if (frictionConstraint.m_multiBodyB)
  99                                                 frictionConstraint.m_multiBodyB->setPosUpdated(false);
 100                                 }
 101                         }
 102                 }
 103
 104                 for (int j1 = 0; j1 < this->m_multiBodyTorsionalFrictionContactConstraints.size(); j1++)
 105                 {
 106                         if (iteration < infoGlobal.m_numIterations)
 107                         {
 108                                 int index = j1;  //iteration&1? j1 : m_multiBodyTorsionalFrictionContactConstraints.size()-1-j1;
 109
 110                                 btMultiBodySolverConstraint& frictionConstraint = m_multiBodyTorsionalFrictionContactConstraints[index];
 111                                 btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
 112                                 j1++;
 113                                 int index2 = j1;
 114                                 btMultiBodySolverConstraint& frictionConstraintB = m_multiBodyTorsionalFrictionContactConstraints[index2];
 115                                 //adjust friction limits here
 116                                 if (totalImpulse > btScalar(0) && frictionConstraint.m_frictionIndex == frictionConstraintB.m_frictionIndex)
 117                                 {
 118                                         frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction * totalImpulse);
 119                                         frictionConstraint.m_upperLimit = frictionConstraint.m_friction * totalImpulse;
 120                                         frictionConstraintB.m_lowerLimit = -(frictionConstraintB.m_friction * totalImpulse);
 121                                         frictionConstraintB.m_upperLimit = frictionConstraintB.m_friction * totalImpulse;
 122
 123                                         btScalar residual = resolveConeFrictionConstraintRows(frictionConstraint, frictionConstraintB);
 124                                         leastSquaredResidual = btMax(leastSquaredResidual, residual * residual);
 125
 126                                         if (frictionConstraint.m_multiBodyA)
 127                                                 frictionConstraint.m_multiBodyA->setPosUpdated(false);
 128                                         if (frictionConstraint.m_multiBodyB)
 129                                                 frictionConstraint.m_multiBodyB->setPosUpdated(false);
 130
 131                                         if (frictionConstraintB.m_multiBodyA)
 132                                                 frictionConstraintB.m_multiBodyA->setPosUpdated(false);
 133                                         if (frictionConstraintB.m_multiBodyB)
 134                                                 frictionConstraintB.m_multiBodyB->setPosUpdated(false);
 135                                 }
 136                         }
 137                 }
 138
 139                 for (int j1 = 0; j1 < this->m_multiBodyFrictionContactConstraints.size(); j1++)
 140                 {
 141                         if (iteration < infoGlobal.m_numIterations)
 142                         {
 143                                 int index = j1;  //iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1;
 144                                 btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[index];
 145
 146                                 btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
 147                                 j1++;
 148                                 int index2 = j1;  //iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1;
 149                                 btMultiBodySolverConstraint& frictionConstraintB = m_multiBodyFrictionContactConstraints[index2];
 150                                 btAssert(frictionConstraint.m_frictionIndex == frictionConstraintB.m_frictionIndex);
 151
 152                                 if (frictionConstraint.m_frictionIndex == frictionConstraintB.m_frictionIndex)
 153                                 {
 154                                         frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction * totalImpulse);
 155                                         frictionConstraint.m_upperLimit = frictionConstraint.m_friction * totalImpulse;
 156                                         frictionConstraintB.m_lowerLimit = -(frictionConstraintB.m_friction * totalImpulse);
 157                                         frictionConstraintB.m_upperLimit = frictionConstraintB.m_friction * totalImpulse;
 158                                         btScalar residual = resolveConeFrictionConstraintRows(frictionConstraint, frictionConstraintB);
 159                                         leastSquaredResidual = btMax(leastSquaredResidual, residual * residual);
 160
 161                                         if (frictionConstraintB.m_multiBodyA)
 162                                                 frictionConstraintB.m_multiBodyA->setPosUpdated(false);
 163                                         if (frictionConstraintB.m_multiBodyB)
 164                                                 frictionConstraintB.m_multiBodyB->setPosUpdated(false);
 165
 166                                         if (frictionConstraint.m_multiBodyA)
 167                                                 frictionConstraint.m_multiBodyA->setPosUpdated(false);
 168                                         if (frictionConstraint.m_multiBodyB)
 169                                                 frictionConstraint.m_multiBodyB->setPosUpdated(false);
 170                                 }
 171                         }
 172                 }
 173         }
 174         else
 175         {
 176                 for (int j1 = 0; j1 < this->m_multiBodyFrictionContactConstraints.size(); j1++)
 177                 {
 178                         if (iteration < infoGlobal.m_numIterations)
 179                         {
 180                                 int index = j1;  //iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1;
 181
 182                                 btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[index];
 183                                 btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
 184                                 //adjust friction limits here
 185                                 if (totalImpulse > btScalar(0))
 186                                 {
 187                                         frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction * totalImpulse);
 188                                         frictionConstraint.m_upperLimit = frictionConstraint.m_friction * totalImpulse;
 189                                         btScalar residual = resolveSingleConstraintRowGeneric(frictionConstraint);
 190                                         leastSquaredResidual = btMax(leastSquaredResidual, residual * residual);
 191
 192                                         if (frictionConstraint.m_multiBodyA)
 193                                                 frictionConstraint.m_multiBodyA->setPosUpdated(false);
 194                                         if (frictionConstraint.m_multiBodyB)
 195                                                 frictionConstraint.m_multiBodyB->setPosUpdated(false);
 196                                 }
 197                         }
 198                 }
 199         }
 200         return leastSquaredResidual;
 201 }
 202
 203 btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
 204 {
 205         m_multiBodyNonContactConstraints.resize(0);
 206         m_multiBodyNormalContactConstraints.resize(0);
 207         m_multiBodyFrictionContactConstraints.resize(0);
 208         m_multiBodyTorsionalFrictionContactConstraints.resize(0);
 209         m_multiBodySpinningFrictionContactConstraints.resize(0);
 210
 211         m_data.m_jacobians.resize(0);
 212         m_data.m_deltaVelocitiesUnitImpulse.resize(0);
 213         m_data.m_deltaVelocities.resize(0);
 214
 215         for (int i = 0; i < numBodies; i++)
 216         {
 217                 const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(bodies[i]);
 218                 if (fcA)
 219                 {
 220                         fcA->m_multiBody->setCompanionId(-1);
 221                 }
 222         }
 223
 224         btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
 225
 226         return val;
 227 }
 228
 229 void btMultiBodyConstraintSolver::applyDeltaVee(btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof)
 230 {
 231         for (int i = 0; i < ndof; ++i)
 232                 m_data.m_deltaVelocities[velocityIndex + i] += delta_vee[i] * impulse;
 233 }
 234
 235 btScalar btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c)
 236 {
 237         btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse) * c.m_cfm;
 238         btScalar deltaVelADotn = 0;
 239         btScalar deltaVelBDotn = 0;
 240         btSolverBody* bodyA = 0;
 241         btSolverBody* bodyB = 0;
 242         int ndofA = 0;
 243         int ndofB = 0;
 244
 245         if (c.m_multiBodyA)
 246         {
 247                 ndofA = c.m_multiBodyA->getNumDofs() + 6;
 248                 for (int i = 0; i < ndofA; ++i)
 249                         deltaVelADotn += m_data.m_jacobians[c.m_jacAindex + i] * m_data.m_deltaVelocities[c.m_deltaVelAindex + i];
 250         }
 251         else if (c.m_solverBodyIdA >= 0)
 252         {
 253                 bodyA = &m_tmpSolverBodyPool[c.m_solverBodyIdA];
 254                 deltaVelADotn += c.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity());
 255         }
 256
 257         if (c.m_multiBodyB)
 258         {
 259                 ndofB = c.m_multiBodyB->getNumDofs() + 6;
 260                 for (int i = 0; i < ndofB; ++i)
 261                         deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex + i] * m_data.m_deltaVelocities[c.m_deltaVelBindex + i];
 262         }
 263         else if (c.m_solverBodyIdB >= 0)
 264         {
 265                 bodyB = &m_tmpSolverBodyPool[c.m_solverBodyIdB];
 266                 deltaVelBDotn += c.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity());
 267         }
 268
 269         deltaImpulse -= deltaVelADotn * c.m_jacDiagABInv;  //m_jacDiagABInv = 1./denom
 270         deltaImpulse -= deltaVelBDotn * c.m_jacDiagABInv;
 271         const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
 272
 273         if (sum < c.m_lowerLimit)
 274         {
 275                 deltaImpulse = c.m_lowerLimit - c.m_appliedImpulse;
 276                 c.m_appliedImpulse = c.m_lowerLimit;
 277         }
 278         else if (sum > c.m_upperLimit)
 279         {
 280                 deltaImpulse = c.m_upperLimit - c.m_appliedImpulse;
 281                 c.m_appliedImpulse = c.m_upperLimit;
 282         }
 283         else
 284         {
 285                 c.m_appliedImpulse = sum;
 286         }
 287
 288         if (c.m_multiBodyA)
 289         {
 290                 applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse, c.m_deltaVelAindex, ndofA);
 291 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
 292                 //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
 293                 //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
 294                 c.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse);
 295 #endif  //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
 296         }
 297         else if (c.m_solverBodyIdA >= 0)
 298         {
 299                 bodyA->internalApplyImpulse(c.m_contactNormal1 * bodyA->internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
 300         }
 301         if (c.m_multiBodyB)
 302         {
 303                 applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], deltaImpulse, c.m_deltaVelBindex, ndofB);
 304 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
 305                 //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
 306                 //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
 307                 c.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], deltaImpulse);
 308 #endif  //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
 309         }
 310         else if (c.m_solverBodyIdB >= 0)
 311         {
 312                 bodyB->internalApplyImpulse(c.m_contactNormal2 * bodyB->internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
 313         }
 314         btScalar deltaVel = deltaImpulse / c.m_jacDiagABInv;
 315         return deltaVel;
 316 }
 317
 318 btScalar btMultiBodyConstraintSolver::resolveConeFrictionConstraintRows(const btMultiBodySolverConstraint& cA1, const btMultiBodySolverConstraint& cB)
 319 {
 320         int ndofA = 0;
 321         int ndofB = 0;
 322         btSolverBody* bodyA = 0;
 323         btSolverBody* bodyB = 0;
 324         btScalar deltaImpulseB = 0.f;
 325         btScalar sumB = 0.f;
 326         {
 327                 deltaImpulseB = cB.m_rhs - btScalar(cB.m_appliedImpulse) * cB.m_cfm;
 328                 btScalar deltaVelADotn = 0;
 329                 btScalar deltaVelBDotn = 0;
 330                 if (cB.m_multiBodyA)
 331                 {
 332                         ndofA = cB.m_multiBodyA->getNumDofs() + 6;
 333                         for (int i = 0; i < ndofA; ++i)
 334                                 deltaVelADotn += m_data.m_jacobians[cB.m_jacAindex + i] * m_data.m_deltaVelocities[cB.m_deltaVelAindex + i];
 335                 }
 336                 else if (cB.m_solverBodyIdA >= 0)
 337                 {
 338                         bodyA = &m_tmpSolverBodyPool[cB.m_solverBodyIdA];
 339                         deltaVelADotn += cB.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + cB.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity());
 340                 }
 341
 342                 if (cB.m_multiBodyB)
 343                 {
 344                         ndofB = cB.m_multiBodyB->getNumDofs() + 6;
 345                         for (int i = 0; i < ndofB; ++i)
 346                                 deltaVelBDotn += m_data.m_jacobians[cB.m_jacBindex + i] * m_data.m_deltaVelocities[cB.m_deltaVelBindex + i];
 347                 }
 348                 else if (cB.m_solverBodyIdB >= 0)
 349                 {
 350                         bodyB = &m_tmpSolverBodyPool[cB.m_solverBodyIdB];
 351                         deltaVelBDotn += cB.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + cB.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity());
 352                 }
 353
 354                 deltaImpulseB -= deltaVelADotn * cB.m_jacDiagABInv;  //m_jacDiagABInv = 1./denom
 355                 deltaImpulseB -= deltaVelBDotn * cB.m_jacDiagABInv;
 356                 sumB = btScalar(cB.m_appliedImpulse) + deltaImpulseB;
 357         }
 358
 359         btScalar deltaImpulseA = 0.f;
 360         btScalar sumA = 0.f;
 361         const btMultiBodySolverConstraint& cA = cA1;
 362         {
 363                 {
 364                         deltaImpulseA = cA.m_rhs - btScalar(cA.m_appliedImpulse) * cA.m_cfm;
 365                         btScalar deltaVelADotn = 0;
 366                         btScalar deltaVelBDotn = 0;
 367                         if (cA.m_multiBodyA)
 368                         {
 369                                 ndofA = cA.m_multiBodyA->getNumDofs() + 6;
 370                                 for (int i = 0; i < ndofA; ++i)
 371                                         deltaVelADotn += m_data.m_jacobians[cA.m_jacAindex + i] * m_data.m_deltaVelocities[cA.m_deltaVelAindex + i];
 372                         }
 373                         else if (cA.m_solverBodyIdA >= 0)
 374                         {
 375                                 bodyA = &m_tmpSolverBodyPool[cA.m_solverBodyIdA];
 376                                 deltaVelADotn += cA.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + cA.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity());
 377                         }
 378
 379                         if (cA.m_multiBodyB)
 380                         {
 381                                 ndofB = cA.m_multiBodyB->getNumDofs() + 6;
 382                                 for (int i = 0; i < ndofB; ++i)
 383                                         deltaVelBDotn += m_data.m_jacobians[cA.m_jacBindex + i] * m_data.m_deltaVelocities[cA.m_deltaVelBindex + i];
 384                         }
 385                         else if (cA.m_solverBodyIdB >= 0)
 386                         {
 387                                 bodyB = &m_tmpSolverBodyPool[cA.m_solverBodyIdB];
 388                                 deltaVelBDotn += cA.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + cA.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity());
 389                         }
 390
 391                         deltaImpulseA -= deltaVelADotn * cA.m_jacDiagABInv;  //m_jacDiagABInv = 1./denom
 392                         deltaImpulseA -= deltaVelBDotn * cA.m_jacDiagABInv;
 393                         sumA = btScalar(cA.m_appliedImpulse) + deltaImpulseA;
 394                 }
 395         }
 396
 397         if (sumA * sumA + sumB * sumB >= cA.m_lowerLimit * cB.m_lowerLimit)
 398         {
 399                 btScalar angle = btAtan2(sumA, sumB);
 400                 btScalar sumAclipped = btFabs(cA.m_lowerLimit * btSin(angle));
 401                 btScalar sumBclipped = btFabs(cB.m_lowerLimit * btCos(angle));
 402
 403                 if (sumA < -sumAclipped)
 404                 {
 405                         deltaImpulseA = -sumAclipped - cA.m_appliedImpulse;
 406                         cA.m_appliedImpulse = -sumAclipped;
 407                 }
 408                 else if (sumA > sumAclipped)
 409                 {
 410                         deltaImpulseA = sumAclipped - cA.m_appliedImpulse;
 411                         cA.m_appliedImpulse = sumAclipped;
 412                 }
 413                 else
 414                 {
 415                         cA.m_appliedImpulse = sumA;
 416                 }
 417
 418                 if (sumB < -sumBclipped)
 419                 {
 420                         deltaImpulseB = -sumBclipped - cB.m_appliedImpulse;
 421                         cB.m_appliedImpulse = -sumBclipped;
 422                 }
 423                 else if (sumB > sumBclipped)
 424                 {
 425                         deltaImpulseB = sumBclipped - cB.m_appliedImpulse;
 426                         cB.m_appliedImpulse = sumBclipped;
 427                 }
 428                 else
 429                 {
 430                         cB.m_appliedImpulse = sumB;
 431                 }
 432                 //deltaImpulseA = sumAclipped-cA.m_appliedImpulse;
 433                 //cA.m_appliedImpulse = sumAclipped;
 434                 //deltaImpulseB = sumBclipped-cB.m_appliedImpulse;
 435                 //cB.m_appliedImpulse = sumBclipped;
 436         }
 437         else
 438         {
 439                 cA.m_appliedImpulse = sumA;
 440                 cB.m_appliedImpulse = sumB;
 441         }
 442
 443         if (cA.m_multiBodyA)
 444         {
 445                 applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacAindex], deltaImpulseA, cA.m_deltaVelAindex, ndofA);
 446 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
 447                 //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
 448                 //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
 449                 cA.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacAindex], deltaImpulseA);
 450 #endif  //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
 451         }
 452         else if (cA.m_solverBodyIdA >= 0)
 453         {
 454                 bodyA->internalApplyImpulse(cA.m_contactNormal1 * bodyA->internalGetInvMass(), cA.m_angularComponentA, deltaImpulseA);
 455         }
 456         if (cA.m_multiBodyB)
 457         {
 458                 applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacBindex], deltaImpulseA, cA.m_deltaVelBindex, ndofB);
 459 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
 460                 //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
 461                 //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
 462                 cA.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacBindex], deltaImpulseA);
 463 #endif  //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
 464         }
 465         else if (cA.m_solverBodyIdB >= 0)
 466         {
 467                 bodyB->internalApplyImpulse(cA.m_contactNormal2 * bodyB->internalGetInvMass(), cA.m_angularComponentB, deltaImpulseA);
 468         }
 469
 470         if (cB.m_multiBodyA)
 471         {
 472                 applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacAindex], deltaImpulseB, cB.m_deltaVelAindex, ndofA);
 473 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
 474                 //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
 475                 //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
 476                 cB.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacAindex], deltaImpulseB);
 477 #endif  //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
 478         }
 479         else if (cB.m_solverBodyIdA >= 0)
 480         {
 481                 bodyA->internalApplyImpulse(cB.m_contactNormal1 * bodyA->internalGetInvMass(), cB.m_angularComponentA, deltaImpulseB);
 482         }
 483         if (cB.m_multiBodyB)
 484         {
 485                 applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacBindex], deltaImpulseB, cB.m_deltaVelBindex, ndofB);
 486 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
 487                 //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
 488                 //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
 489                 cB.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacBindex], deltaImpulseB);
 490 #endif  //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
 491         }
 492         else if (cB.m_solverBodyIdB >= 0)
 493         {
 494                 bodyB->internalApplyImpulse(cB.m_contactNormal2 * bodyB->internalGetInvMass(), cB.m_angularComponentB, deltaImpulseB);
 495         }
 496
 497         btScalar deltaVel = deltaImpulseA / cA.m_jacDiagABInv + deltaImpulseB / cB.m_jacDiagABInv;
 498         return deltaVel;
 499 }
 500
 501 void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, const btVector3& contactNormal, const btScalar& appliedImpulse, btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, btScalar& relaxation, bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
 502 {
 503         BT_PROFILE("setupMultiBodyContactConstraint");
 504         btVector3 rel_pos1;
 505         btVector3 rel_pos2;
 506
 507         btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
 508         btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
 509
 510         const btVector3& pos1 = cp.getPositionWorldOnA();
 511         const btVector3& pos2 = cp.getPositionWorldOnB();
 512
 513         btSolverBody* bodyA = multiBodyA ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA];
 514         btSolverBody* bodyB = multiBodyB ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB];
 515
 516         btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
 517         btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
 518
 519         if (bodyA)
 520                 rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
 521         if (bodyB)
 522                 rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
 523
 524         relaxation = infoGlobal.m_sor;
 525
 526         btScalar invTimeStep = btScalar(1) / infoGlobal.m_timeStep;
 527
 528         //cfm = 1 /       ( dt * kp + kd )
 529         //erp = dt * kp / ( dt * kp + kd )
 530
 531         btScalar cfm;
 532         btScalar erp;
 533         if (isFriction)
 534         {
 535                 cfm = infoGlobal.m_frictionCFM;
 536                 erp = infoGlobal.m_frictionERP;
 537         }
 538         else
 539         {
 540                 cfm = infoGlobal.m_globalCfm;
 541                 erp = infoGlobal.m_erp2;
 542
 543                 if ((cp.m_contactPointFlags & BT_CONTACT_FLAG_HAS_CONTACT_CFM) || (cp.m_contactPointFlags & BT_CONTACT_FLAG_HAS_CONTACT_ERP))
 544                 {
 545                         if (cp.m_contactPointFlags & BT_CONTACT_FLAG_HAS_CONTACT_CFM)
 546                                 cfm = cp.m_contactCFM;
 547                         if (cp.m_contactPointFlags & BT_CONTACT_FLAG_HAS_CONTACT_ERP)
 548                                 erp = cp.m_contactERP;
 549                 }
 550                 else
 551                 {
 552                         if (cp.m_contactPointFlags & BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING)
 553                         {
 554                                 btScalar denom = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1 + cp.m_combinedContactDamping1);
 555                                 if (denom < SIMD_EPSILON)
 556                                 {
 557                                         denom = SIMD_EPSILON;
 558                                 }
 559                                 cfm = btScalar(1) / denom;
 560                                 erp = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1) / denom;
 561                         }
 562                 }
 563         }
 564
 565         cfm *= invTimeStep;
 566
 567         if (multiBodyA)
 568         {
 569                 if (solverConstraint.m_linkA < 0)
 570                 {
 571                         rel_pos1 = pos1 - multiBodyA->getBasePos();
 572                 }
 573                 else
 574                 {
 575                         rel_pos1 = pos1 - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
 576                 }
 577                 const int ndofA = multiBodyA->getNumDofs() + 6;
 578
 579                 solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
 580
 581                 if (solverConstraint.m_deltaVelAindex < 0)
 582                 {
 583                         solverConstraint.m_deltaVelAindex = m_data.m_deltaVelocities.size();
 584                         multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
 585                         m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size() + ndofA);
 586                 }
 587                 else
 588                 {
 589                         btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex + ndofA);
 590                 }
 591
 592                 solverConstraint.m_jacAindex = m_data.m_jacobians.size();
 593                 m_data.m_jacobians.resize(m_data.m_jacobians.size() + ndofA);
 594                 m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size() + ndofA);
 595                 btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
 596
 597                 btScalar* jac1 = &m_data.m_jacobians[solverConstraint.m_jacAindex];
 598                 multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
 599                 btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
 600                 multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex], delta, m_data.scratch_r, m_data.scratch_v);
 601
 602                 btVector3 torqueAxis0 = rel_pos1.cross(contactNormal);
 603                 solverConstraint.m_relpos1CrossNormal = torqueAxis0;
 604                 solverConstraint.m_contactNormal1 = contactNormal;
 605         }
 606         else
 607         {
 608                 btVector3 torqueAxis0 = rel_pos1.cross(contactNormal);
 609                 solverConstraint.m_relpos1CrossNormal = torqueAxis0;
 610                 solverConstraint.m_contactNormal1 = contactNormal;
 611                 solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld() * torqueAxis0 * rb0->getAngularFactor() : btVector3(0, 0, 0);
 612         }
 613
 614         if (multiBodyB)
 615         {
 616                 if (solverConstraint.m_linkB < 0)
 617                 {
 618                         rel_pos2 = pos2 - multiBodyB->getBasePos();
 619                 }
 620                 else
 621                 {
 622                         rel_pos2 = pos2 - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
 623                 }
 624
 625                 const int ndofB = multiBodyB->getNumDofs() + 6;
 626
 627                 solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
 628                 if (solverConstraint.m_deltaVelBindex < 0)
 629                 {
 630                         solverConstraint.m_deltaVelBindex = m_data.m_deltaVelocities.size();
 631                         multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
 632                         m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size() + ndofB);
 633                 }
 634
 635                 solverConstraint.m_jacBindex = m_data.m_jacobians.size();
 636
 637                 m_data.m_jacobians.resize(m_data.m_jacobians.size() + ndofB);
 638                 m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size() + ndofB);
 639                 btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
 640
 641                 multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -contactNormal, &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
 642                 multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex], &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v);
 643
 644                 btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);
 645                 solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
 646                 solverConstraint.m_contactNormal2 = -contactNormal;
 647         }
 648         else
 649         {
 650                 btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);
 651                 solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
 652                 solverConstraint.m_contactNormal2 = -contactNormal;
 653
 654                 solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld() * -torqueAxis1 * rb1->getAngularFactor() : btVector3(0, 0, 0);
 655         }
 656
 657         {
 658                 btVector3 vec;
 659                 btScalar denom0 = 0.f;
 660                 btScalar denom1 = 0.f;
 661                 btScalar* jacB = 0;
 662                 btScalar* jacA = 0;
 663                 btScalar* lambdaA = 0;
 664                 btScalar* lambdaB = 0;
 665                 int ndofA = 0;
 666                 if (multiBodyA)
 667                 {
 668                         ndofA = multiBodyA->getNumDofs() + 6;
 669                         jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
 670                         lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
 671                         for (int i = 0; i < ndofA; ++i)
 672                         {
 673                                 btScalar j = jacA[i];
 674                                 btScalar l = lambdaA[i];
 675                                 denom0 += j * l;
 676                         }
 677                 }
 678                 else
 679                 {
 680                         if (rb0)
 681                         {
 682                                 vec = (solverConstraint.m_angularComponentA).cross(rel_pos1);
 683                                 denom0 = rb0->getInvMass() + contactNormal.dot(vec);
 684                         }
 685                 }
 686                 if (multiBodyB)
 687                 {
 688                         const int ndofB = multiBodyB->getNumDofs() + 6;
 689                         jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
 690                         lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
 691                         for (int i = 0; i < ndofB; ++i)
 692                         {
 693                                 btScalar j = jacB[i];
 694                                 btScalar l = lambdaB[i];
 695                                 denom1 += j * l;
 696                         }
 697                 }
 698                 else
 699                 {
 700                         if (rb1)
 701                         {
 702                                 vec = (-solverConstraint.m_angularComponentB).cross(rel_pos2);
 703                                 denom1 = rb1->getInvMass() + contactNormal.dot(vec);
 704                         }
 705                 }
 706
 707                 btScalar d = denom0 + denom1 + cfm;
 708                 if (d > SIMD_EPSILON)
 709                 {
 710                         solverConstraint.m_jacDiagABInv = relaxation / (d);
 711                 }
 712                 else
 713                 {
 714                         //disable the constraint row to handle singularity/redundant constraint
 715                         solverConstraint.m_jacDiagABInv = 0.f;
 716                 }
 717         }
 718
 719         //compute rhs and remaining solverConstraint fields
 720
 721         btScalar restitution = 0.f;
 722         btScalar distance = 0;
 723         if (!isFriction)
 724         {
 725                 distance = cp.getDistance() + infoGlobal.m_linearSlop;
 726         }
 727         else
 728         {
 729                 if (cp.m_contactPointFlags & BT_CONTACT_FLAG_FRICTION_ANCHOR)
 730                 {
 731                         distance = (cp.getPositionWorldOnA() - cp.getPositionWorldOnB()).dot(contactNormal);
 732                 }
 733         }
 734
 735         btScalar rel_vel = 0.f;
 736         int ndofA = 0;
 737         int ndofB = 0;
 738         {
 739                 btVector3 vel1, vel2;
 740                 if (multiBodyA)
 741                 {
 742                         ndofA = multiBodyA->getNumDofs() + 6;
 743                         btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
 744                         for (int i = 0; i < ndofA; ++i)
 745                                 rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
 746                 }
 747                 else
 748                 {
 749                         if (rb0)
 750                         {
 751                                 rel_vel += (rb0->getVelocityInLocalPoint(rel_pos1) +
 752                                                         (rb0->getTotalTorque() * rb0->getInvInertiaTensorWorld() * infoGlobal.m_timeStep).cross(rel_pos1) +
 753                                                         rb0->getTotalForce() * rb0->getInvMass() * infoGlobal.m_timeStep)
 754                                                            .dot(solverConstraint.m_contactNormal1);
 755                         }
 756                 }
 757                 if (multiBodyB)
 758                 {
 759                         ndofB = multiBodyB->getNumDofs() + 6;
 760                         btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
 761                         for (int i = 0; i < ndofB; ++i)
 762                                 rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
 763                 }
 764                 else
 765                 {
 766                         if (rb1)
 767                         {
 768                                 rel_vel += (rb1->getVelocityInLocalPoint(rel_pos2) +
 769                                                         (rb1->getTotalTorque() * rb1->getInvInertiaTensorWorld() * infoGlobal.m_timeStep).cross(rel_pos2) +
 770                                                         rb1->getTotalForce() * rb1->getInvMass() * infoGlobal.m_timeStep)
 771                                                            .dot(solverConstraint.m_contactNormal2);
 772                         }
 773                 }
 774
 775                 solverConstraint.m_friction = cp.m_combinedFriction;
 776
 777                 if (!isFriction)
 778                 {
 779                         restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution, infoGlobal.m_restitutionVelocityThreshold);
 780                         if (restitution <= btScalar(0.))
 781                         {
 782                                 restitution = 0.f;
 783                         }
 784                 }
 785         }
 786
 787         {
 788                 btScalar positionalError = 0.f;
 789                 btScalar velocityError = restitution - rel_vel;  // * damping;  //note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction
 790                 if (isFriction)
 791                 {
 792                         positionalError = -distance * erp / infoGlobal.m_timeStep;
 793                 }
 794                 else
 795                 {
 796                         if (distance > 0)
 797                         {
 798                                 positionalError = 0;
 799                                 velocityError -= distance / infoGlobal.m_timeStep;
 800                         }
 801                         else
 802                         {
 803                                 positionalError = -distance * erp / infoGlobal.m_timeStep;
 804                         }
 805                 }
 806
 807                 btScalar penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv;
 808                 btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
 809
 810                 if (!isFriction)
 811                 {
 812                         //      if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
 813                         {
 814                                 //combine position and velocity into rhs
 815                                 solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
 816                                 solverConstraint.m_rhsPenetration = 0.f;
 817                         }
 818                         /*else
 819                         {
 820                                 //split position and velocity into rhs and m_rhsPenetration
 821                                 solverConstraint.m_rhs = velocityImpulse;
 822                                 solverConstraint.m_rhsPenetration = penetrationImpulse;
 823                         }
 824                         */
 825                         solverConstraint.m_lowerLimit = 0;
 826                         solverConstraint.m_upperLimit = 1e10f;
 827                 }
 828                 else
 829                 {
 830                         solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
 831                         solverConstraint.m_rhsPenetration = 0.f;
 832                         solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
 833                         solverConstraint.m_upperLimit = solverConstraint.m_friction;
 834                 }
 835
 836                 solverConstraint.m_cfm = cfm * solverConstraint.m_jacDiagABInv;
 837         }
 838
 839         if (infoGlobal.m_solverMode & SOLVER_USE_ARTICULATED_WARMSTARTING)
 840         {
 841                 if (btFabs(cp.m_prevRHS) > 1e-5 && cp.m_prevRHS < 2* solverConstraint.m_rhs && solverConstraint.m_rhs < 2*cp.m_prevRHS)
 842                 {
 843                         solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse / cp.m_prevRHS * solverConstraint.m_rhs * infoGlobal.m_articulatedWarmstartingFactor;
 844                         if (solverConstraint.m_appliedImpulse < 0)
 845                                 solverConstraint.m_appliedImpulse = 0;
 846                 }
 847                 else
 848                 {
 849                         solverConstraint.m_appliedImpulse = 0.f;
 850                 }
 851
 852                 if (solverConstraint.m_appliedImpulse)
 853                 {
 854                         if (multiBodyA)
 855                         {
 856                                 btScalar impulse = solverConstraint.m_appliedImpulse;
 857                                 btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
 858                                 multiBodyA->applyDeltaVeeMultiDof2(deltaV, impulse);
 859
 860                                 applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelAindex, ndofA);
 861                         }
 862                         else
 863                         {
 864                                 if (rb0)
 865                                         bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1 * bodyA->internalGetInvMass() * rb0->getLinearFactor(), solverConstraint.m_angularComponentA, solverConstraint.m_appliedImpulse);
 866                         }
 867                         if (multiBodyB)
 868                         {
 869                                 btScalar impulse = solverConstraint.m_appliedImpulse;
 870                                 btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
 871                                 multiBodyB->applyDeltaVeeMultiDof2(deltaV, impulse);
 872                                 applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelBindex, ndofB);
 873                         }
 874                         else
 875                         {
 876                                 if (rb1)
 877                                         bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2 * bodyB->internalGetInvMass() * rb1->getLinearFactor(), -solverConstraint.m_angularComponentB, -(btScalar)solverConstraint.m_appliedImpulse);
 878                         }
 879                 }
 880         }
 881         else
 882         {
 883                 solverConstraint.m_appliedImpulse = 0.f;
 884         solverConstraint.m_appliedPushImpulse = 0.f;
 885         }
 886 }
 887
 888 void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMultiBodySolverConstraint& solverConstraint,
 889                                                                                                                                                         const btVector3& constraintNormal,
 890                                                                                                                                                         btManifoldPoint& cp,
 891                                                                                                                                                         btScalar combinedTorsionalFriction,
 892                                                                                                                                                         const btContactSolverInfo& infoGlobal,
 893                                                                                                                                                         btScalar& relaxation,
 894                                                                                                                                                         bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
 895 {
 896         BT_PROFILE("setupMultiBodyRollingFrictionConstraint");
 897         btVector3 rel_pos1;
 898         btVector3 rel_pos2;
 899
 900         btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
 901         btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
 902
 903         const btVector3& pos1 = cp.getPositionWorldOnA();
 904         const btVector3& pos2 = cp.getPositionWorldOnB();
 905
 906         btSolverBody* bodyA = multiBodyA ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA];
 907         btSolverBody* bodyB = multiBodyB ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB];
 908
 909         btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
 910         btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
 911
 912         if (bodyA)
 913                 rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
 914         if (bodyB)
 915                 rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
 916
 917         relaxation = infoGlobal.m_sor;
 918
 919         // btScalar invTimeStep = btScalar(1)/infoGlobal.m_timeStep;
 920
 921         if (multiBodyA)
 922         {
 923                 if (solverConstraint.m_linkA < 0)
 924                 {
 925                         rel_pos1 = pos1 - multiBodyA->getBasePos();
 926                 }
 927                 else
 928                 {
 929                         rel_pos1 = pos1 - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
 930                 }
 931                 const int ndofA = multiBodyA->getNumDofs() + 6;
 932
 933                 solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
 934
 935                 if (solverConstraint.m_deltaVelAindex < 0)
 936                 {
 937                         solverConstraint.m_deltaVelAindex = m_data.m_deltaVelocities.size();
 938                         multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
 939                         m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size() + ndofA);
 940                 }
 941                 else
 942                 {
 943                         btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex + ndofA);
 944                 }
 945
 946                 solverConstraint.m_jacAindex = m_data.m_jacobians.size();
 947                 m_data.m_jacobians.resize(m_data.m_jacobians.size() + ndofA);
 948                 m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size() + ndofA);
 949                 btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
 950
 951                 btScalar* jac1 = &m_data.m_jacobians[solverConstraint.m_jacAindex];
 952                 multiBodyA->fillConstraintJacobianMultiDof(solverConstraint.m_linkA, cp.getPositionWorldOnA(), constraintNormal, btVector3(0, 0, 0), jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
 953                 btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
 954                 multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex], delta, m_data.scratch_r, m_data.scratch_v);
 955
 956                 btVector3 torqueAxis0 = constraintNormal;
 957                 solverConstraint.m_relpos1CrossNormal = torqueAxis0;
 958                 solverConstraint.m_contactNormal1 = btVector3(0, 0, 0);
 959         }
 960         else
 961         {
 962                 btVector3 torqueAxis0 = constraintNormal;
 963                 solverConstraint.m_relpos1CrossNormal = torqueAxis0;
 964                 solverConstraint.m_contactNormal1 = btVector3(0, 0, 0);
 965                 solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld() * torqueAxis0 * rb0->getAngularFactor() : btVector3(0, 0, 0);
 966         }
 967
 968         if (multiBodyB)
 969         {
 970                 if (solverConstraint.m_linkB < 0)
 971                 {
 972                         rel_pos2 = pos2 - multiBodyB->getBasePos();
 973                 }
 974                 else
 975                 {
 976                         rel_pos2 = pos2 - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
 977                 }
 978
 979                 const int ndofB = multiBodyB->getNumDofs() + 6;
 980
 981                 solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
 982                 if (solverConstraint.m_deltaVelBindex < 0)
 983                 {
 984                         solverConstraint.m_deltaVelBindex = m_data.m_deltaVelocities.size();
 985                         multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
 986                         m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size() + ndofB);
 987                 }
 988
 989                 solverConstraint.m_jacBindex = m_data.m_jacobians.size();
 990
 991                 m_data.m_jacobians.resize(m_data.m_jacobians.size() + ndofB);
 992                 m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size() + ndofB);
 993                 btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
 994
 995                 multiBodyB->fillConstraintJacobianMultiDof(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -constraintNormal, btVector3(0, 0, 0), &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
 996                 multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex], &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v);
 997
 998                 btVector3 torqueAxis1 = -constraintNormal;
 999                 solverConstraint.m_relpos2CrossNormal = torqueAxis1;
1000                 solverConstraint.m_contactNormal2 = -btVector3(0, 0, 0);
1001         }
1002         else
1003         {
1004                 btVector3 torqueAxis1 = -constraintNormal;
1005                 solverConstraint.m_relpos2CrossNormal = torqueAxis1;
1006                 solverConstraint.m_contactNormal2 = -btVector3(0, 0, 0);
1007
1008                 solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld() * torqueAxis1 * rb1->getAngularFactor() : btVector3(0, 0, 0);
1009         }
1010
1011         {
1012                 btScalar denom0 = 0.f;
1013                 btScalar denom1 = 0.f;
1014                 btScalar* jacB = 0;
1015                 btScalar* jacA = 0;
1016                 btScalar* lambdaA = 0;
1017                 btScalar* lambdaB = 0;
1018                 int ndofA = 0;
1019                 if (multiBodyA)
1020                 {
1021                         ndofA = multiBodyA->getNumDofs() + 6;
1022                         jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
1023                         lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
1024                         for (int i = 0; i < ndofA; ++i)
1025                         {
1026                                 btScalar j = jacA[i];
1027                                 btScalar l = lambdaA[i];
1028                                 denom0 += j * l;
1029                         }
1030                 }
1031                 else
1032                 {
1033                         if (rb0)
1034                         {
1035                                 btVector3 iMJaA = rb0 ? rb0->getInvInertiaTensorWorld() * solverConstraint.m_relpos1CrossNormal : btVector3(0, 0, 0);
1036                                 denom0 = iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
1037                         }
1038                 }
1039                 if (multiBodyB)
1040                 {
1041                         const int ndofB = multiBodyB->getNumDofs() + 6;
1042                         jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
1043                         lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
1044                         for (int i = 0; i < ndofB; ++i)
1045                         {
1046                                 btScalar j = jacB[i];
1047                                 btScalar l = lambdaB[i];
1048                                 denom1 += j * l;
1049                         }
1050                 }
1051                 else
1052                 {
1053                         if (rb1)
1054                         {
1055                                 btVector3 iMJaB = rb1 ? rb1->getInvInertiaTensorWorld() * solverConstraint.m_relpos2CrossNormal : btVector3(0, 0, 0);
1056                                 denom1 = iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
1057                         }
1058                 }
1059
1060                 btScalar d = denom0 + denom1 + infoGlobal.m_globalCfm;
1061                 if (d > SIMD_EPSILON)
1062                 {
1063                         solverConstraint.m_jacDiagABInv = relaxation / (d);
1064                 }
1065                 else
1066                 {
1067                         //disable the constraint row to handle singularity/redundant constraint
1068                         solverConstraint.m_jacDiagABInv = 0.f;
1069                 }
1070         }
1071
1072         //compute rhs and remaining solverConstraint fields
1073
1074         btScalar restitution = 0.f;
1075         btScalar penetration = isFriction ? 0 : cp.getDistance();
1076
1077         btScalar rel_vel = 0.f;
1078         int ndofA = 0;
1079         int ndofB = 0;
1080         {
1081                 btVector3 vel1, vel2;
1082                 if (multiBodyA)
1083                 {
1084                         ndofA = multiBodyA->getNumDofs() + 6;
1085                         btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
1086                         for (int i = 0; i < ndofA; ++i)
1087                                 rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
1088                 }
1089                 else
1090                 {
1091                         if (rb0)
1092                         {
1093                                 btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA];
1094                                 rel_vel += solverConstraint.m_contactNormal1.dot(rb0 ? solverBodyA->m_linearVelocity + solverBodyA->m_externalForceImpulse : btVector3(0, 0, 0)) + solverConstraint.m_relpos1CrossNormal.dot(rb0 ? solverBodyA->m_angularVelocity : btVector3(0, 0, 0));
1095                         }
1096                 }
1097                 if (multiBodyB)
1098                 {
1099                         ndofB = multiBodyB->getNumDofs() + 6;
1100                         btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
1101                         for (int i = 0; i < ndofB; ++i)
1102                                 rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
1103                 }
1104                 else
1105                 {
1106                         if (rb1)
1107                         {
1108                                 btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB];
1109                                 rel_vel += solverConstraint.m_contactNormal2.dot(rb1 ? solverBodyB->m_linearVelocity + solverBodyB->m_externalForceImpulse : btVector3(0, 0, 0)) + solverConstraint.m_relpos2CrossNormal.dot(rb1 ? solverBodyB->m_angularVelocity : btVector3(0, 0, 0));
1110                         }
1111                 }
1112
1113                 solverConstraint.m_friction = combinedTorsionalFriction;
1114
1115                 if (!isFriction)
1116                 {
1117                         restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution, infoGlobal.m_restitutionVelocityThreshold);
1118                         if (restitution <= btScalar(0.))
1119                         {
1120                                 restitution = 0.f;
1121                         }
1122                 }
1123         }
1124
1125         solverConstraint.m_appliedImpulse = 0.f;
1126         solverConstraint.m_appliedPushImpulse = 0.f;
1127
1128         {
1129                 btScalar velocityError = 0 - rel_vel;  // * damping;    //note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction
1130
1131                 btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
1132
1133                 solverConstraint.m_rhs = velocityImpulse;
1134                 solverConstraint.m_rhsPenetration = 0.f;
1135                 solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
1136                 solverConstraint.m_upperLimit = solverConstraint.m_friction;
1137
1138                 solverConstraint.m_cfm = infoGlobal.m_globalCfm * solverConstraint.m_jacDiagABInv;
1139         }
1140 }
1141
1142 btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint(const btVector3& normalAxis, const btScalar& appliedImpulse, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
1143 {
1144         BT_PROFILE("addMultiBodyFrictionConstraint");
1145         btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing();
1146         solverConstraint.m_orgConstraint = 0;
1147         solverConstraint.m_orgDofIndex = -1;
1148
1149         solverConstraint.m_frictionIndex = frictionIndex;
1150         bool isFriction = true;
1151
1152         const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
1153         const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
1154
1155         btMultiBody* mbA = fcA ? fcA->m_multiBody : 0;
1156         btMultiBody* mbB = fcB ? fcB->m_multiBody : 0;
1157
1158         int solverBodyIdA = mbA ? -1 : getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep);
1159         int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep);
1160
1161         solverConstraint.m_solverBodyIdA = solverBodyIdA;
1162         solverConstraint.m_solverBodyIdB = solverBodyIdB;
1163         solverConstraint.m_multiBodyA = mbA;
1164         if (mbA)
1165                 solverConstraint.m_linkA = fcA->m_link;
1166
1167         solverConstraint.m_multiBodyB = mbB;
1168         if (mbB)
1169                 solverConstraint.m_linkB = fcB->m_link;
1170
1171         solverConstraint.m_originalContactPoint = &cp;
1172
1173         setupMultiBodyContactConstraint(solverConstraint, normalAxis, 0, cp, infoGlobal, relaxation, isFriction, desiredVelocity, cfmSlip);
1174         return solverConstraint;
1175 }
1176
1177 btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyTorsionalFrictionConstraint(const btVector3& normalAxis, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp,
1178                                                                                                                                                                                                   btScalar combinedTorsionalFriction,
1179                                                                                                                                                                                                   btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
1180 {
1181         BT_PROFILE("addMultiBodyRollingFrictionConstraint");
1182
1183         bool useTorsionalAndConeFriction = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS && ((infoGlobal.m_solverMode & SOLVER_DISABLE_IMPLICIT_CONE_FRICTION) == 0));
1184
1185         btMultiBodySolverConstraint& solverConstraint = useTorsionalAndConeFriction ? m_multiBodyTorsionalFrictionContactConstraints.expandNonInitializing() : m_multiBodyFrictionContactConstraints.expandNonInitializing();
1186         solverConstraint.m_orgConstraint = 0;
1187         solverConstraint.m_orgDofIndex = -1;
1188
1189         solverConstraint.m_frictionIndex = frictionIndex;
1190         bool isFriction = true;
1191
1192         const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
1193         const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
1194
1195         btMultiBody* mbA = fcA ? fcA->m_multiBody : 0;
1196         btMultiBody* mbB = fcB ? fcB->m_multiBody : 0;
1197
1198         int solverBodyIdA = mbA ? -1 : getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep);
1199         int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep);
1200
1201         solverConstraint.m_solverBodyIdA = solverBodyIdA;
1202         solverConstraint.m_solverBodyIdB = solverBodyIdB;
1203         solverConstraint.m_multiBodyA = mbA;
1204         if (mbA)
1205                 solverConstraint.m_linkA = fcA->m_link;
1206
1207         solverConstraint.m_multiBodyB = mbB;
1208         if (mbB)
1209                 solverConstraint.m_linkB = fcB->m_link;
1210
1211         solverConstraint.m_originalContactPoint = &cp;
1212
1213         setupMultiBodyTorsionalFrictionConstraint(solverConstraint, normalAxis, cp, combinedTorsionalFriction, infoGlobal, relaxation, isFriction, desiredVelocity, cfmSlip);
1214         return solverConstraint;
1215 }
1216
1217 btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodySpinningFrictionConstraint(const btVector3& normalAxis, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp,
1218         btScalar combinedTorsionalFriction,
1219         btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
1220 {
1221         BT_PROFILE("addMultiBodyRollingFrictionConstraint");
1222
1223         btMultiBodySolverConstraint& solverConstraint = m_multiBodySpinningFrictionContactConstraints.expandNonInitializing();
1224         solverConstraint.m_orgConstraint = 0;
1225         solverConstraint.m_orgDofIndex = -1;
1226
1227         solverConstraint.m_frictionIndex = frictionIndex;
1228         bool isFriction = true;
1229
1230         const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
1231         const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
1232
1233         btMultiBody* mbA = fcA ? fcA->m_multiBody : 0;
1234         btMultiBody* mbB = fcB ? fcB->m_multiBody : 0;
1235
1236         int solverBodyIdA = mbA ? -1 : getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep);
1237         int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep);
1238
1239         solverConstraint.m_solverBodyIdA = solverBodyIdA;
1240         solverConstraint.m_solverBodyIdB = solverBodyIdB;
1241         solverConstraint.m_multiBodyA = mbA;
1242         if (mbA)
1243                 solverConstraint.m_linkA = fcA->m_link;
1244
1245         solverConstraint.m_multiBodyB = mbB;
1246         if (mbB)
1247                 solverConstraint.m_linkB = fcB->m_link;
1248
1249         solverConstraint.m_originalContactPoint = &cp;
1250
1251         setupMultiBodyTorsionalFrictionConstraint(solverConstraint, normalAxis, cp, combinedTorsionalFriction, infoGlobal, relaxation, isFriction, desiredVelocity, cfmSlip);
1252         return solverConstraint;
1253 }
1254 void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* manifold, const btContactSolverInfo& infoGlobal)
1255 {
1256         const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
1257         const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
1258
1259         btMultiBody* mbA = fcA ? fcA->m_multiBody : 0;
1260         btMultiBody* mbB = fcB ? fcB->m_multiBody : 0;
1261
1262         btCollisionObject *colObj0 = 0, *colObj1 = 0;
1263
1264         colObj0 = (btCollisionObject*)manifold->getBody0();
1265         colObj1 = (btCollisionObject*)manifold->getBody1();
1266
1267         int solverBodyIdA = mbA ? -1 : getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep);
1268         int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep);
1269
1270         //      btSolverBody* solverBodyA = mbA ? 0 : &m_tmpSolverBodyPool[solverBodyIdA];
1271         //      btSolverBody* solverBodyB = mbB ? 0 : &m_tmpSolverBodyPool[solverBodyIdB];
1272
1273         ///avoid collision response between two static objects
1274         //      if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero())))
1275         //      return;
1276
1277         //only a single rollingFriction per manifold
1278         int rollingFriction = 4;
1279
1280         for (int j = 0; j < manifold->getNumContacts(); j++)
1281         {
1282                 btManifoldPoint& cp = manifold->getContactPoint(j);
1283
1284                 if (cp.getDistance() <= manifold->getContactProcessingThreshold())
1285                 {
1286                         btScalar relaxation;
1287
1288                         int frictionIndex = m_multiBodyNormalContactConstraints.size();
1289
1290                         btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints.expandNonInitializing();
1291
1292                         //              btRigidBody* rb0 = btRigidBody::upcast(colObj0);
1293                         //              btRigidBody* rb1 = btRigidBody::upcast(colObj1);
1294                         solverConstraint.m_orgConstraint = 0;
1295                         solverConstraint.m_orgDofIndex = -1;
1296                         solverConstraint.m_solverBodyIdA = solverBodyIdA;
1297                         solverConstraint.m_solverBodyIdB = solverBodyIdB;
1298                         solverConstraint.m_multiBodyA = mbA;
1299                         if (mbA)
1300                                 solverConstraint.m_linkA = fcA->m_link;
1301
1302                         solverConstraint.m_multiBodyB = mbB;
1303                         if (mbB)
1304                                 solverConstraint.m_linkB = fcB->m_link;
1305
1306                         solverConstraint.m_originalContactPoint = &cp;
1307
1308                         bool isFriction = false;
1309                         setupMultiBodyContactConstraint(solverConstraint, cp.m_normalWorldOnB, cp.m_appliedImpulse, cp, infoGlobal, relaxation, isFriction);
1310
1311                         //                      const btVector3& pos1 = cp.getPositionWorldOnA();
1312                         //                      const btVector3& pos2 = cp.getPositionWorldOnB();
1313
1314                         /////setup the friction constraints
1315 #define ENABLE_FRICTION
1316 #ifdef ENABLE_FRICTION
1317                         solverConstraint.m_frictionIndex = m_multiBodyFrictionContactConstraints.size();
1318
1319                         ///Bullet has several options to set the friction directions
1320                         ///By default, each contact has only a single friction direction that is recomputed automatically every frame
1321                         ///based on the relative linear velocity.
1322                         ///If the relative velocity is zero, it will automatically compute a friction direction.
1323
1324                         ///You can also enable two friction directions, using the SOLVER_USE_2_FRICTION_DIRECTIONS.
1325                         ///In that case, the second friction direction will be orthogonal to both contact normal and first friction direction.
1326                         ///
1327                         ///If you choose SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION, then the friction will be independent from the relative projected velocity.
1328                         ///
1329                         ///The user can manually override the friction directions for certain contacts using a contact callback,
1330                         ///and set the cp.m_lateralFrictionInitialized to true
1331                         ///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2)
1332                         ///this will give a conveyor belt effect
1333                         ///
1334
1335                         btPlaneSpace1(cp.m_normalWorldOnB, cp.m_lateralFrictionDir1, cp.m_lateralFrictionDir2);
1336                         cp.m_lateralFrictionDir1.normalize();
1337                         cp.m_lateralFrictionDir2.normalize();
1338
1339                         if (rollingFriction > 0)
1340                         {
1341                                 if (cp.m_combinedSpinningFriction > 0)
1342                                 {
1343                                         addMultiBodySpinningFrictionConstraint(cp.m_normalWorldOnB, manifold, frictionIndex, cp, cp.m_combinedSpinningFriction, colObj0, colObj1, relaxation, infoGlobal);
1344                                 }
1345                                 if (cp.m_combinedRollingFriction > 0)
1346                                 {
1347                                         applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
1348                                         applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
1349                                         applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
1350                                         applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
1351
1352                                         addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir1, manifold, frictionIndex, cp, cp.m_combinedRollingFriction, colObj0, colObj1, relaxation, infoGlobal);
1353                                         addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir2, manifold, frictionIndex, cp, cp.m_combinedRollingFriction, colObj0, colObj1, relaxation, infoGlobal);
1354                                 }
1355                                 rollingFriction--;
1356                         }
1357                         if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !(cp.m_contactPointFlags & BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED))
1358                         { /*
1359                                 cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
1360                                 btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
1361                                 if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
1362                                 {
1363                                         cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel);
1364                                         if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
1365                                         {
1366                                                 cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
1367                                                 cp.m_lateralFrictionDir2.normalize();//??
1368                                                 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
1369                                                 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
1370                                                 addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
1371
1372                                         }
1373
1374                                         applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
1375                                         applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
1376                                         addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
1377
1378                                 } else
1379                                 */
1380                                 {
1381                                         applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION);
1382                                         applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION);
1383                                         addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1, cp.m_appliedImpulseLateral1, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal);
1384
1385                                         if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
1386                                         {
1387                                                 applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
1388                                                 applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
1389                                           addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2, cp.m_appliedImpulseLateral2, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal);
1390                                         }
1391
1392                                         if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
1393                                         {
1394                                                 cp.m_contactPointFlags |= BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED;
1395                                         }
1396                                 }
1397                         }
1398                         else
1399                         {
1400                                 addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1, cp.m_appliedImpulseLateral1, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM);
1401
1402                                 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
1403                                         addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2, cp.m_appliedImpulseLateral2, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM);
1404                                 solverConstraint.m_appliedImpulse = 0.f;
1405                                 solverConstraint.m_appliedPushImpulse = 0.f;
1406       }
1407
1408 #endif  //ENABLE_FRICTION
1409                 }
1410                 else
1411                 {
1412                         // Reset quantities related to warmstart as 0.
1413                         cp.m_appliedImpulse = 0;
1414                         cp.m_prevRHS = 0;
1415                 }
1416         }
1417 }
1418
1419 void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal)
1420 {
1421         for (int i = 0; i < numManifolds; i++)
1422         {
1423                 btPersistentManifold* manifold = manifoldPtr[i];
1424                 const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
1425                 const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
1426                 if (!fcA && !fcB)
1427                 {
1428                         //the contact doesn't involve any Featherstone btMultiBody, so deal with the regular btRigidBody/btCollisionObject case
1429                         convertContact(manifold, infoGlobal);
1430                 }
1431                 else
1432                 {
1433                         convertMultiBodyContact(manifold, infoGlobal);
1434                 }
1435         }
1436
1437         //also convert the multibody constraints, if any
1438
1439         for (int i = 0; i < m_tmpNumMultiBodyConstraints; i++)
1440         {
1441                 btMultiBodyConstraint* c = m_tmpMultiBodyConstraints[i];
1442                 m_data.m_solverBodyPool = &m_tmpSolverBodyPool;
1443                 m_data.m_fixedBodyId = m_fixedBodyId;
1444
1445                 c->createConstraintRows(m_multiBodyNonContactConstraints, m_data, infoGlobal);
1446         }
1447
1448         // Warmstart for noncontact constraints
1449         if (infoGlobal.m_solverMode & SOLVER_USE_ARTICULATED_WARMSTARTING)
1450         {
1451                 for (int i = 0; i < m_multiBodyNonContactConstraints.size(); i++)
1452                 {
1453                         btMultiBodySolverConstraint& solverConstraint =
1454                                 m_multiBodyNonContactConstraints[i];
1455                         solverConstraint.m_appliedImpulse =
1456                                 solverConstraint.m_orgConstraint->getAppliedImpulse(solverConstraint.m_orgDofIndex) *
1457                                 infoGlobal.m_articulatedWarmstartingFactor;
1458
1459                         btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
1460                         btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
1461                         if (solverConstraint.m_appliedImpulse)
1462                         {
1463                                 if (multiBodyA)
1464                                 {
1465                                         int ndofA = multiBodyA->getNumDofs() + 6;
1466                                         btScalar* deltaV =
1467                                                 &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
1468                                         btScalar impulse = solverConstraint.m_appliedImpulse;
1469                                         multiBodyA->applyDeltaVeeMultiDof2(deltaV, impulse);
1470                                         applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelAindex, ndofA);
1471                                 }
1472                                 if (multiBodyB)
1473                                 {
1474                                         int ndofB = multiBodyB->getNumDofs() + 6;
1475                                         btScalar* deltaV =
1476                                                 &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
1477                                         btScalar impulse = solverConstraint.m_appliedImpulse;
1478                                         multiBodyB->applyDeltaVeeMultiDof2(deltaV, impulse);
1479                                         applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelBindex, ndofB);
1480                                 }
1481                         }
1482                 }
1483         }
1484         else
1485         {
1486                 for (int i = 0; i < m_multiBodyNonContactConstraints.size(); i++)
1487                 {
1488                         btMultiBodySolverConstraint& solverConstraint = m_multiBodyNonContactConstraints[i];
1489                         solverConstraint.m_appliedImpulse = 0;
1490                 }
1491         }
1492 }
1493
1494 btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher)
1495 {
1496         //printf("btMultiBodyConstraintSolver::solveGroup: numBodies=%d, numConstraints=%d\n", numBodies, numConstraints);
1497         return btSequentialImpulseConstraintSolver::solveGroup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer, dispatcher);
1498 }
1499
1500 #if 0
1501 static void applyJointFeedback(btMultiBodyJacobianData& data, const btMultiBodySolverConstraint& solverConstraint, int jacIndex, btMultiBody* mb, btScalar appliedImpulse)
1502 {
1503         if (appliedImpulse!=0 && mb->internalNeedsJointFeedback())
1504         {
1505                 //todo: get rid of those temporary memory allocations for the joint feedback
1506                 btAlignedObjectArray<btScalar> forceVector;
1507                 int numDofsPlusBase = 6+mb->getNumDofs();
1508                 forceVector.resize(numDofsPlusBase);
1509                 for (int i=0;i<numDofsPlusBase;i++)
1510                 {
1511                         forceVector[i] = data.m_jacobians[jacIndex+i]*appliedImpulse;
1512                 }
1513                 btAlignedObjectArray<btScalar> output;
1514                 output.resize(numDofsPlusBase);
1515                 bool applyJointFeedback = true;
1516                 mb->calcAccelerationDeltasMultiDof(&forceVector[0],&output[0],data.scratch_r,data.scratch_v,applyJointFeedback);
1517         }
1518 }
1519 #endif
1520
1521 void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint& c, btScalar deltaTime)
1522 {
1523 #if 1
1524
1525         //bod->addBaseForce(m_gravity * bod->getBaseMass());
1526         //bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
1527
1528         if (c.m_orgConstraint)
1529         {
1530                 c.m_orgConstraint->internalSetAppliedImpulse(c.m_orgDofIndex, c.m_appliedImpulse);
1531         }
1532
1533         if (c.m_multiBodyA)
1534         {
1535                 c.m_multiBodyA->setCompanionId(-1);
1536                 btVector3 force = c.m_contactNormal1 * (c.m_appliedImpulse / deltaTime);
1537                 btVector3 torque = c.m_relpos1CrossNormal * (c.m_appliedImpulse / deltaTime);
1538                 if (c.m_linkA < 0)
1539                 {
1540                         c.m_multiBodyA->addBaseConstraintForce(force);
1541                         c.m_multiBodyA->addBaseConstraintTorque(torque);
1542                 }
1543                 else
1544                 {
1545                         c.m_multiBodyA->addLinkConstraintForce(c.m_linkA, force);
1546                         //b3Printf("force = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
1547                         c.m_multiBodyA->addLinkConstraintTorque(c.m_linkA, torque);
1548                 }
1549         }
1550
1551         if (c.m_multiBodyB)
1552         {
1553                 {
1554                         c.m_multiBodyB->setCompanionId(-1);
1555                         btVector3 force = c.m_contactNormal2 * (c.m_appliedImpulse / deltaTime);
1556                         btVector3 torque = c.m_relpos2CrossNormal * (c.m_appliedImpulse / deltaTime);
1557                         if (c.m_linkB < 0)
1558                         {
1559                                 c.m_multiBodyB->addBaseConstraintForce(force);
1560                                 c.m_multiBodyB->addBaseConstraintTorque(torque);
1561                         }
1562                         else
1563                         {
1564                                 {
1565                                         c.m_multiBodyB->addLinkConstraintForce(c.m_linkB, force);
1566                                         //b3Printf("t = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
1567                                         c.m_multiBodyB->addLinkConstraintTorque(c.m_linkB, torque);
1568                                 }
1569                         }
1570                 }
1571         }
1572 #endif
1573
1574 #ifndef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
1575
1576         if (c.m_multiBodyA)
1577         {
1578                 c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], c.m_appliedImpulse);
1579         }
1580
1581         if (c.m_multiBodyB)
1582         {
1583                 c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], c.m_appliedImpulse);
1584         }
1585 #endif
1586 }
1587
1588 btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
1589 {
1590         BT_PROFILE("btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish");
1591         int numPoolConstraints = m_multiBodyNormalContactConstraints.size();
1592
1593         //write back the delta v to the multi bodies, either as applied impulse (direct velocity change)
1594         //or as applied force, so we can measure the joint reaction forces easier
1595         for (int i = 0; i < numPoolConstraints; i++)
1596         {
1597                 btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[i];
1598                 writeBackSolverBodyToMultiBody(solverConstraint, infoGlobal.m_timeStep);
1599
1600                 writeBackSolverBodyToMultiBody(m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex], infoGlobal.m_timeStep);
1601
1602                 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
1603                 {
1604                         writeBackSolverBodyToMultiBody(m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex + 1], infoGlobal.m_timeStep);
1605                 }
1606         }
1607
1608         for (int i = 0; i < m_multiBodyNonContactConstraints.size(); i++)
1609         {
1610                 btMultiBodySolverConstraint& solverConstraint = m_multiBodyNonContactConstraints[i];
1611                 writeBackSolverBodyToMultiBody(solverConstraint, infoGlobal.m_timeStep);
1612         }
1613
1614
1615         {
1616                 BT_PROFILE("warm starting write back");
1617                 for (int j = 0; j < numPoolConstraints; j++)
1618                 {
1619                         const btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[j];
1620                         btManifoldPoint* pt = (btManifoldPoint*)solverConstraint.m_originalContactPoint;
1621                         btAssert(pt);
1622                         pt->m_appliedImpulse = solverConstraint.m_appliedImpulse;
1623                   pt->m_prevRHS = solverConstraint.m_rhs;
1624                         pt->m_appliedImpulseLateral1 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse;
1625
1626                         //printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
1627                         if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
1628                         {
1629                                 pt->m_appliedImpulseLateral2 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex + 1].m_appliedImpulse;
1630                         } else
1631                         {
1632                                 pt->m_appliedImpulseLateral2 = 0;
1633                         }
1634                 }
1635         }
1636
1637 #if 0
1638         //multibody joint feedback
1639         {
1640                 BT_PROFILE("multi body joint feedback");
1641                 for (int j=0;j<numPoolConstraints;j++)
1642                 {
1643                         const btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[j];
1644
1645                         //apply the joint feedback into all links of the btMultiBody
1646                         //todo: double-check the signs of the applied impulse
1647
1648                         if(solverConstraint.m_multiBodyA && solverConstraint.m_multiBodyA->isMultiDof())
1649                         {
1650                                 applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacAindex,solverConstraint.m_multiBodyA, solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
1651                         }
1652                         if(solverConstraint.m_multiBodyB && solverConstraint.m_multiBodyB->isMultiDof())
1653                         {
1654                                 applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacBindex,solverConstraint.m_multiBodyB,solverConstraint.m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
1655                         }
1656 #if 0
1657                         if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA->isMultiDof())
1658                         {
1659                                 applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],
1660                                         m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_jacAindex,
1661                                         m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA,
1662                                         m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
1663
1664                         }
1665                         if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB->isMultiDof())
1666                         {
1667                                 applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],
1668                                         m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_jacBindex,
1669                                         m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB,
1670                                         m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
1671                         }
1672
1673                         if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
1674                         {
1675                                 if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA->isMultiDof())
1676                                 {
1677                                         applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],
1678                                                 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_jacAindex,
1679                                                 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA,
1680                                                 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
1681                                 }
1682
1683                                 if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB->isMultiDof())
1684                                 {
1685                                         applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],
1686                                                 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_jacBindex,
1687                                                 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB,
1688                                                 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
1689                                 }
1690                         }
1691 #endif
1692                 }
1693
1694                 for (int i=0;i<m_multiBodyNonContactConstraints.size();i++)
1695                 {
1696                         const btMultiBodySolverConstraint& solverConstraint = m_multiBodyNonContactConstraints[i];
1697                         if(solverConstraint.m_multiBodyA && solverConstraint.m_multiBodyA->isMultiDof())
1698                         {
1699                                 applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacAindex,solverConstraint.m_multiBodyA, solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
1700                         }
1701                         if(solverConstraint.m_multiBodyB && solverConstraint.m_multiBodyB->isMultiDof())
1702                         {
1703                                 applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacBindex,solverConstraint.m_multiBodyB,solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
1704                         }
1705                 }
1706         }
1707
1708         numPoolConstraints = m_multiBodyNonContactConstraints.size();
1709
1710 #if 0
1711         //@todo: m_originalContactPoint is not initialized for btMultiBodySolverConstraint
1712         for (int i=0;i<numPoolConstraints;i++)
1713         {
1714                 const btMultiBodySolverConstraint& c = m_multiBodyNonContactConstraints[i];
1715
1716                 btTypedConstraint* constr = (btTypedConstraint*)c.m_originalContactPoint;
1717                 btJointFeedback* fb = constr->getJointFeedback();
1718                 if (fb)
1719                 {
1720                         fb->m_appliedForceBodyA += c.m_contactNormal1*c.m_appliedImpulse*constr->getRigidBodyA().getLinearFactor()/infoGlobal.m_timeStep;
1721                         fb->m_appliedForceBodyB += c.m_contactNormal2*c.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/infoGlobal.m_timeStep;
1722                         fb->m_appliedTorqueBodyA += c.m_relpos1CrossNormal* constr->getRigidBodyA().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep;
1723                         fb->m_appliedTorqueBodyB += c.m_relpos2CrossNormal* constr->getRigidBodyB().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep; /*RGM ???? */
1724
1725                 }
1726
1727                 constr->internalSetAppliedImpulse(c.m_appliedImpulse);
1728                 if (btFabs(c.m_appliedImpulse)>=constr->getBreakingImpulseThreshold())
1729                 {
1730                         constr->setEnabled(false);
1731                 }
1732
1733         }
1734 #endif
1735 #endif
1736
1737         return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal);
1738 }
1739
1740 void btMultiBodyConstraintSolver::solveMultiBodyGroup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher)
1741 {
1742         //printf("solveMultiBodyGroup: numBodies=%d, numConstraints=%d, numManifolds=%d, numMultiBodyConstraints=%d\n", numBodies, numConstraints, numManifolds, numMultiBodyConstraints);
1743
1744         //printf("solveMultiBodyGroup start\n");
1745         m_tmpMultiBodyConstraints = multiBodyConstraints;
1746         m_tmpNumMultiBodyConstraints = numMultiBodyConstraints;
1747
1748         btSequentialImpulseConstraintSolver::solveGroup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer, dispatcher);
1749
1750         m_tmpMultiBodyConstraints = 0;
1751         m_tmpNumMultiBodyConstraints = 0;
1752 }