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

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
   4
   5 This software is provided 'as-is', without any express or implied warranty.
   6 In no event will the authors be held liable for any damages arising from the use of this software.
   7 Permission is granted to anyone to use this software for any purpose,
   8 including commercial applications, and to alter it and redistribute it freely,
   9 subject to the following restrictions:
  10
  11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
  12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  13 3. This notice may not be removed or altered from any source distribution.
  14 */
  15
  16 #include "btNNCGConstraintSolver.h"
  17
  18 btScalar btNNCGConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
  19 {
  20         btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
  21
  22         m_pNC.resizeNoInitialize(m_tmpSolverNonContactConstraintPool.size());
  23         m_pC.resizeNoInitialize(m_tmpSolverContactConstraintPool.size());
  24         m_pCF.resizeNoInitialize(m_tmpSolverContactFrictionConstraintPool.size());
  25         m_pCRF.resizeNoInitialize(m_tmpSolverContactRollingFrictionConstraintPool.size());
  26
  27         m_deltafNC.resizeNoInitialize(m_tmpSolverNonContactConstraintPool.size());
  28         m_deltafC.resizeNoInitialize(m_tmpSolverContactConstraintPool.size());
  29         m_deltafCF.resizeNoInitialize(m_tmpSolverContactFrictionConstraintPool.size());
  30         m_deltafCRF.resizeNoInitialize(m_tmpSolverContactRollingFrictionConstraintPool.size());
  31
  32         return val;
  33 }
  34
  35 btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */, int /*numBodies*/, btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* /*debugDrawer*/)
  36 {
  37         int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
  38         int numConstraintPool = m_tmpSolverContactConstraintPool.size();
  39         int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
  40
  41         if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
  42         {
  43                 if (1)  // uncomment this for a bit less random ((iteration & 7) == 0)
  44                 {
  45                         for (int j = 0; j < numNonContactPool; ++j)
  46                         {
  47                                 int tmp = m_orderNonContactConstraintPool[j];
  48                                 int swapi = btRandInt2(j + 1);
  49                                 m_orderNonContactConstraintPool[j] = m_orderNonContactConstraintPool[swapi];
  50                                 m_orderNonContactConstraintPool[swapi] = tmp;
  51                         }
  52
  53                         //contact/friction constraints are not solved more than
  54                         if (iteration < infoGlobal.m_numIterations)
  55                         {
  56                                 for (int j = 0; j < numConstraintPool; ++j)
  57                                 {
  58                                         int tmp = m_orderTmpConstraintPool[j];
  59                                         int swapi = btRandInt2(j + 1);
  60                                         m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
  61                                         m_orderTmpConstraintPool[swapi] = tmp;
  62                                 }
  63
  64                                 for (int j = 0; j < numFrictionPool; ++j)
  65                                 {
  66                                         int tmp = m_orderFrictionConstraintPool[j];
  67                                         int swapi = btRandInt2(j + 1);
  68                                         m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
  69                                         m_orderFrictionConstraintPool[swapi] = tmp;
  70                                 }
  71                         }
  72                 }
  73         }
  74
  75         btScalar deltaflengthsqr = 0;
  76         {
  77                 for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++)
  78                 {
  79                         btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
  80                         if (iteration < constraint.m_overrideNumSolverIterations)
  81                         {
  82                                 btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA], m_tmpSolverBodyPool[constraint.m_solverBodyIdB], constraint);
  83                                 m_deltafNC[j] = deltaf;
  84                                 deltaflengthsqr += deltaf * deltaf;
  85                         }
  86                 }
  87         }
  88
  89         if (m_onlyForNoneContact)
  90         {
  91                 if (iteration == 0)
  92                 {
  93                         for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++) m_pNC[j] = m_deltafNC[j];
  94                 }
  95                 else
  96                 {
  97                         // deltaflengthsqrprev can be 0 only if the solver solved the problem exactly in the previous iteration. In this case we should have quit, but mainly for debug reason with this 'hack' it is now allowed to continue the calculation
  98                         btScalar beta = m_deltafLengthSqrPrev > 0 ? deltaflengthsqr / m_deltafLengthSqrPrev : 2;
  99                         if (beta > 1)
 100                         {
 101                                 for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++) m_pNC[j] = 0;
 102                         }
 103                         else
 104                         {
 105                                 for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++)
 106                                 {
 107                                         btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
 108                                         if (iteration < constraint.m_overrideNumSolverIterations)
 109                                         {
 110                                                 btScalar additionaldeltaimpulse = beta * m_pNC[j];
 111                                                 constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
 112                                                 m_pNC[j] = beta * m_pNC[j] + m_deltafNC[j];
 113                                                 btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
 114                                                 btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
 115                                                 const btSolverConstraint& c = constraint;
 116                                                 body1.internalApplyImpulse(c.m_contactNormal1 * body1.internalGetInvMass(), c.m_angularComponentA, additionaldeltaimpulse);
 117                                                 body2.internalApplyImpulse(c.m_contactNormal2 * body2.internalGetInvMass(), c.m_angularComponentB, additionaldeltaimpulse);
 118                                         }
 119                                 }
 120                         }
 121                 }
 122                 m_deltafLengthSqrPrev = deltaflengthsqr;
 123         }
 124
 125         {
 126                 if (iteration < infoGlobal.m_numIterations)
 127                 {
 128                         for (int j = 0; j < numConstraints; j++)
 129                         {
 130                                 if (constraints[j]->isEnabled())
 131                                 {
 132                                         int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(), infoGlobal.m_timeStep);
 133                                         int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(), infoGlobal.m_timeStep);
 134                                         btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
 135                                         btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
 136                                         constraints[j]->solveConstraintObsolete(bodyA, bodyB, infoGlobal.m_timeStep);
 137                                 }
 138                         }
 139
 140                         ///solve all contact constraints
 141                         if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS)
 142                         {
 143                                 int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
 144                                 int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) ? 2 : 1;
 145
 146                                 for (int c = 0; c < numPoolConstraints; c++)
 147                                 {
 148                                         btScalar totalImpulse = 0;
 149
 150                                         {
 151                                                 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]];
 152                                                 btScalar deltaf = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
 153                                                 m_deltafC[c] = deltaf;
 154                                                 deltaflengthsqr += deltaf * deltaf;
 155                                                 totalImpulse = solveManifold.m_appliedImpulse;
 156                                         }
 157                                         bool applyFriction = true;
 158                                         if (applyFriction)
 159                                         {
 160                                                 {
 161                                                         btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c * multiplier]];
 162
 163                                                         if (totalImpulse > btScalar(0))
 164                                                         {
 165                                                                 solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
 166                                                                 solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
 167                                                                 btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
 168                                                                 m_deltafCF[c * multiplier] = deltaf;
 169                                                                 deltaflengthsqr += deltaf * deltaf;
 170                                                         }
 171                                                         else
 172                                                         {
 173                                                                 m_deltafCF[c * multiplier] = 0;
 174                                                         }
 175                                                 }
 176
 177                                                 if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)
 178                                                 {
 179                                                         btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c * multiplier + 1]];
 180
 181                                                         if (totalImpulse > btScalar(0))
 182                                                         {
 183                                                                 solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
 184                                                                 solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
 185                                                                 btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
 186                                                                 m_deltafCF[c * multiplier + 1] = deltaf;
 187                                                                 deltaflengthsqr += deltaf * deltaf;
 188                                                         }
 189                                                         else
 190                                                         {
 191                                                                 m_deltafCF[c * multiplier + 1] = 0;
 192                                                         }
 193                                                 }
 194                                         }
 195                                 }
 196                         }
 197                         else  //SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
 198                         {
 199                                 //solve the friction constraints after all contact constraints, don't interleave them
 200                                 int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
 201                                 int j;
 202
 203                                 for (j = 0; j < numPoolConstraints; j++)
 204                                 {
 205                                         const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
 206                                         btScalar deltaf = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
 207                                         m_deltafC[j] = deltaf;
 208                                         deltaflengthsqr += deltaf * deltaf;
 209                                 }
 210
 211                                 ///solve all friction constraints
 212
 213                                 int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
 214                                 for (j = 0; j < numFrictionPoolConstraints; j++)
 215                                 {
 216                                         btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
 217                                         btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
 218
 219                                         if (totalImpulse > btScalar(0))
 220                                         {
 221                                                 solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
 222                                                 solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
 223
 224                                                 btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
 225                                                 m_deltafCF[j] = deltaf;
 226                                                 deltaflengthsqr += deltaf * deltaf;
 227                                         }
 228                                         else
 229                                         {
 230                                                 m_deltafCF[j] = 0;
 231                                         }
 232                                 }
 233                         }
 234
 235                         {
 236                                 int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
 237                                 for (int j = 0; j < numRollingFrictionPoolConstraints; j++)
 238                                 {
 239                                         btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
 240                                         btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
 241                                         if (totalImpulse > btScalar(0))
 242                                         {
 243                                                 btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction * totalImpulse;
 244                                                 if (rollingFrictionMagnitude > rollingFrictionConstraint.m_friction)
 245                                                         rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
 246
 247                                                 rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
 248                                                 rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
 249
 250                                                 btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA], m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB], rollingFrictionConstraint);
 251                                                 m_deltafCRF[j] = deltaf;
 252                                                 deltaflengthsqr += deltaf * deltaf;
 253                                         }
 254                                         else
 255                                         {
 256                                                 m_deltafCRF[j] = 0;
 257                                         }
 258                                 }
 259                         }
 260                 }
 261         }
 262
 263         if (!m_onlyForNoneContact)
 264         {
 265                 if (iteration == 0)
 266                 {
 267                         for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++) m_pNC[j] = m_deltafNC[j];
 268                         for (int j = 0; j < m_tmpSolverContactConstraintPool.size(); j++) m_pC[j] = m_deltafC[j];
 269                         for (int j = 0; j < m_tmpSolverContactFrictionConstraintPool.size(); j++) m_pCF[j] = m_deltafCF[j];
 270                         for (int j = 0; j < m_tmpSolverContactRollingFrictionConstraintPool.size(); j++) m_pCRF[j] = m_deltafCRF[j];
 271                 }
 272                 else
 273                 {
 274                         // deltaflengthsqrprev can be 0 only if the solver solved the problem exactly in the previous iteration. In this case we should have quit, but mainly for debug reason with this 'hack' it is now allowed to continue the calculation
 275                         btScalar beta = m_deltafLengthSqrPrev > 0 ? deltaflengthsqr / m_deltafLengthSqrPrev : 2;
 276                         if (beta > 1)
 277                         {
 278                                 for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++) m_pNC[j] = 0;
 279                                 for (int j = 0; j < m_tmpSolverContactConstraintPool.size(); j++) m_pC[j] = 0;
 280                                 for (int j = 0; j < m_tmpSolverContactFrictionConstraintPool.size(); j++) m_pCF[j] = 0;
 281                                 for (int j = 0; j < m_tmpSolverContactRollingFrictionConstraintPool.size(); j++) m_pCRF[j] = 0;
 282                         }
 283                         else
 284                         {
 285                                 for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++)
 286                                 {
 287                                         btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
 288                                         if (iteration < constraint.m_overrideNumSolverIterations)
 289                                         {
 290                                                 btScalar additionaldeltaimpulse = beta * m_pNC[j];
 291                                                 constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
 292                                                 m_pNC[j] = beta * m_pNC[j] + m_deltafNC[j];
 293                                                 btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
 294                                                 btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
 295                                                 const btSolverConstraint& c = constraint;
 296                                                 body1.internalApplyImpulse(c.m_contactNormal1 * body1.internalGetInvMass(), c.m_angularComponentA, additionaldeltaimpulse);
 297                                                 body2.internalApplyImpulse(c.m_contactNormal2 * body2.internalGetInvMass(), c.m_angularComponentB, additionaldeltaimpulse);
 298                                         }
 299                                 }
 300                                 for (int j = 0; j < m_tmpSolverContactConstraintPool.size(); j++)
 301                                 {
 302                                         btSolverConstraint& constraint = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
 303                                         if (iteration < infoGlobal.m_numIterations)
 304                                         {
 305                                                 btScalar additionaldeltaimpulse = beta * m_pC[j];
 306                                                 constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
 307                                                 m_pC[j] = beta * m_pC[j] + m_deltafC[j];
 308                                                 btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
 309                                                 btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
 310                                                 const btSolverConstraint& c = constraint;
 311                                                 body1.internalApplyImpulse(c.m_contactNormal1 * body1.internalGetInvMass(), c.m_angularComponentA, additionaldeltaimpulse);
 312                                                 body2.internalApplyImpulse(c.m_contactNormal2 * body2.internalGetInvMass(), c.m_angularComponentB, additionaldeltaimpulse);
 313                                         }
 314                                 }
 315                                 for (int j = 0; j < m_tmpSolverContactFrictionConstraintPool.size(); j++)
 316                                 {
 317                                         btSolverConstraint& constraint = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
 318                                         if (iteration < infoGlobal.m_numIterations)
 319                                         {
 320                                                 btScalar additionaldeltaimpulse = beta * m_pCF[j];
 321                                                 constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
 322                                                 m_pCF[j] = beta * m_pCF[j] + m_deltafCF[j];
 323                                                 btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
 324                                                 btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
 325                                                 const btSolverConstraint& c = constraint;
 326                                                 body1.internalApplyImpulse(c.m_contactNormal1 * body1.internalGetInvMass(), c.m_angularComponentA, additionaldeltaimpulse);
 327                                                 body2.internalApplyImpulse(c.m_contactNormal2 * body2.internalGetInvMass(), c.m_angularComponentB, additionaldeltaimpulse);
 328                                         }
 329                                 }
 330                                 {
 331                                         for (int j = 0; j < m_tmpSolverContactRollingFrictionConstraintPool.size(); j++)
 332                                         {
 333                                                 btSolverConstraint& constraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
 334                                                 if (iteration < infoGlobal.m_numIterations)
 335                                                 {
 336                                                         btScalar additionaldeltaimpulse = beta * m_pCRF[j];
 337                                                         constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
 338                                                         m_pCRF[j] = beta * m_pCRF[j] + m_deltafCRF[j];
 339                                                         btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
 340                                                         btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
 341                                                         const btSolverConstraint& c = constraint;
 342                                                         body1.internalApplyImpulse(c.m_contactNormal1 * body1.internalGetInvMass(), c.m_angularComponentA, additionaldeltaimpulse);
 343                                                         body2.internalApplyImpulse(c.m_contactNormal2 * body2.internalGetInvMass(), c.m_angularComponentB, additionaldeltaimpulse);
 344                                                 }
 345                                         }
 346                                 }
 347                         }
 348                 }
 349                 m_deltafLengthSqrPrev = deltaflengthsqr;
 350         }
 351
 352         return deltaflengthsqr;
 353 }
 354
 355 btScalar btNNCGConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
 356 {
 357         m_pNC.resizeNoInitialize(0);
 358         m_pC.resizeNoInitialize(0);
 359         m_pCF.resizeNoInitialize(0);
 360         m_pCRF.resizeNoInitialize(0);
 361
 362         m_deltafNC.resizeNoInitialize(0);
 363         m_deltafC.resizeNoInitialize(0);
 364         m_deltafCF.resizeNoInitialize(0);
 365         m_deltafCRF.resizeNoInitialize(0);
 366
 367         return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal);
 368 }