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