dali-physics/third-party/bullet3/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.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 "btSolve2LinearConstraint.h"
  17
  18 #include "BulletDynamics/Dynamics/btRigidBody.h"
  19 #include "LinearMath/btVector3.h"
  20 #include "btJacobianEntry.h"
  21
  22 void btSolve2LinearConstraint::resolveUnilateralPairConstraint(
  23         btRigidBody* body1,
  24         btRigidBody* body2,
  25
  26         const btMatrix3x3& world2A,
  27         const btMatrix3x3& world2B,
  28
  29         const btVector3& invInertiaADiag,
  30         const btScalar invMassA,
  31         const btVector3& linvelA, const btVector3& angvelA,
  32         const btVector3& rel_posA1,
  33         const btVector3& invInertiaBDiag,
  34         const btScalar invMassB,
  35         const btVector3& linvelB, const btVector3& angvelB,
  36         const btVector3& rel_posA2,
  37
  38         btScalar depthA, const btVector3& normalA,
  39         const btVector3& rel_posB1, const btVector3& rel_posB2,
  40         btScalar depthB, const btVector3& normalB,
  41         btScalar& imp0, btScalar& imp1)
  42 {
  43         (void)linvelA;
  44         (void)linvelB;
  45         (void)angvelB;
  46         (void)angvelA;
  47
  48         imp0 = btScalar(0.);
  49         imp1 = btScalar(0.);
  50
  51         btScalar len = btFabs(normalA.length()) - btScalar(1.);
  52         if (btFabs(len) >= SIMD_EPSILON)
  53                 return;
  54
  55         btAssert(len < SIMD_EPSILON);
  56
  57         //this jacobian entry could be re-used for all iterations
  58         btJacobianEntry jacA(world2A, world2B, rel_posA1, rel_posA2, normalA, invInertiaADiag, invMassA,
  59                                                  invInertiaBDiag, invMassB);
  60         btJacobianEntry jacB(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA,
  61                                                  invInertiaBDiag, invMassB);
  62
  63         //const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
  64         //const btScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
  65
  66         const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1) - body2->getVelocityInLocalPoint(rel_posA1));
  67         const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1) - body2->getVelocityInLocalPoint(rel_posB1));
  68
  69         //      btScalar penetrationImpulse = (depth*contactTau*timeCorrection)  * massTerm;//jacDiagABInv
  70         btScalar massTerm = btScalar(1.) / (invMassA + invMassB);
  71
  72         // calculate rhs (or error) terms
  73         const btScalar dv0 = depthA * m_tau * massTerm - vel0 * m_damping;
  74         const btScalar dv1 = depthB * m_tau * massTerm - vel1 * m_damping;
  75
  76         // dC/dv * dv = -C
  77
  78         // jacobian * impulse = -error
  79         //
  80
  81         //impulse = jacobianInverse * -error
  82
  83         // inverting 2x2 symmetric system (offdiagonal are equal!)
  84         //
  85
  86         btScalar nonDiag = jacA.getNonDiagonal(jacB, invMassA, invMassB);
  87         btScalar invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag);
  88
  89         //imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
  90         //imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
  91
  92         imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
  93         imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * -nonDiag * invDet;
  94
  95         //[a b]                                                           [d -c]
  96         //[c d] inverse = (1 / determinant) * [-b a] where determinant is (ad - bc)
  97
  98         //[jA nD] * [imp0] = [dv0]
  99         //[nD jB]   [imp1]   [dv1]
 100 }
 101
 102 void btSolve2LinearConstraint::resolveBilateralPairConstraint(
 103         btRigidBody* body1,
 104         btRigidBody* body2,
 105         const btMatrix3x3& world2A,
 106         const btMatrix3x3& world2B,
 107
 108         const btVector3& invInertiaADiag,
 109         const btScalar invMassA,
 110         const btVector3& linvelA, const btVector3& angvelA,
 111         const btVector3& rel_posA1,
 112         const btVector3& invInertiaBDiag,
 113         const btScalar invMassB,
 114         const btVector3& linvelB, const btVector3& angvelB,
 115         const btVector3& rel_posA2,
 116
 117         btScalar depthA, const btVector3& normalA,
 118         const btVector3& rel_posB1, const btVector3& rel_posB2,
 119         btScalar depthB, const btVector3& normalB,
 120         btScalar& imp0, btScalar& imp1)
 121 {
 122         (void)linvelA;
 123         (void)linvelB;
 124         (void)angvelA;
 125         (void)angvelB;
 126
 127         imp0 = btScalar(0.);
 128         imp1 = btScalar(0.);
 129
 130         btScalar len = btFabs(normalA.length()) - btScalar(1.);
 131         if (btFabs(len) >= SIMD_EPSILON)
 132                 return;
 133
 134         btAssert(len < SIMD_EPSILON);
 135
 136         //this jacobian entry could be re-used for all iterations
 137         btJacobianEntry jacA(world2A, world2B, rel_posA1, rel_posA2, normalA, invInertiaADiag, invMassA,
 138                                                  invInertiaBDiag, invMassB);
 139         btJacobianEntry jacB(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA,
 140                                                  invInertiaBDiag, invMassB);
 141
 142         //const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
 143         //const btScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
 144
 145         const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1) - body2->getVelocityInLocalPoint(rel_posA1));
 146         const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1) - body2->getVelocityInLocalPoint(rel_posB1));
 147
 148         // calculate rhs (or error) terms
 149         const btScalar dv0 = depthA * m_tau - vel0 * m_damping;
 150         const btScalar dv1 = depthB * m_tau - vel1 * m_damping;
 151
 152         // dC/dv * dv = -C
 153
 154         // jacobian * impulse = -error
 155         //
 156
 157         //impulse = jacobianInverse * -error
 158
 159         // inverting 2x2 symmetric system (offdiagonal are equal!)
 160         //
 161
 162         btScalar nonDiag = jacA.getNonDiagonal(jacB, invMassA, invMassB);
 163         btScalar invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag);
 164
 165         //imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
 166         //imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
 167
 168         imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
 169         imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * -nonDiag * invDet;
 170
 171         //[a b]                                                           [d -c]
 172         //[c d] inverse = (1 / determinant) * [-b a] where determinant is (ad - bc)
 173
 174         //[jA nD] * [imp0] = [dv0]
 175         //[nD jB]   [imp1]   [dv1]
 176
 177         if (imp0 > btScalar(0.0))
 178         {
 179                 if (imp1 > btScalar(0.0))
 180                 {
 181                         //both positive
 182                 }
 183                 else
 184                 {
 185                         imp1 = btScalar(0.);
 186
 187                         // now imp0>0 imp1<0
 188                         imp0 = dv0 / jacA.getDiagonal();
 189                         if (imp0 > btScalar(0.0))
 190                         {
 191                         }
 192                         else
 193                         {
 194                                 imp0 = btScalar(0.);
 195                         }
 196                 }
 197         }
 198         else
 199         {
 200                 imp0 = btScalar(0.);
 201
 202                 imp1 = dv1 / jacB.getDiagonal();
 203                 if (imp1 <= btScalar(0.0))
 204                 {
 205                         imp1 = btScalar(0.);
 206                         // now imp0>0 imp1<0
 207                         imp0 = dv0 / jacA.getDiagonal();
 208                         if (imp0 > btScalar(0.0))
 209                         {
 210                         }
 211                         else
 212                         {
 213                                 imp0 = btScalar(0.);
 214                         }
 215                 }
 216                 else
 217                 {
 218                 }
 219         }
 220 }
 221
 222 /*
 223 void btSolve2LinearConstraint::resolveAngularConstraint(        const btMatrix3x3& invInertiaAWS,
 224                                                                                         const btScalar invMassA,
 225                                                                                         const btVector3& linvelA,const btVector3& angvelA,
 226                                                                                         const btVector3& rel_posA1,
 227                                                                                         const btMatrix3x3& invInertiaBWS,
 228                                                                                         const btScalar invMassB,
 229                                                                                         const btVector3& linvelB,const btVector3& angvelB,
 230                                                                                         const btVector3& rel_posA2,
 231
 232                                                                                         btScalar depthA, const btVector3& normalA,
 233                                                                                         const btVector3& rel_posB1,const btVector3& rel_posB2,
 234                                                                                         btScalar depthB, const btVector3& normalB,
 235                                                                                         btScalar& imp0,btScalar& imp1)
 236 {
 237
 238 }
 239 */