btSolve2LinearConstraint.cpp

Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose, 
00008 including commercial applications, and to alter it and redistribute it freely, 
00009 subject to the following restrictions:
00010 
00011 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.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
00014 */
00015 
00016 
00017 
00018 #include "btSolve2LinearConstraint.h"
00019 
00020 #include "BulletDynamics/Dynamics/btRigidBody.h"
00021 #include "LinearMath/btVector3.h"
00022 #include "btJacobianEntry.h"
00023 
00024 
00025 void btSolve2LinearConstraint::resolveUnilateralPairConstraint(
00026                                                                                                    btRigidBody* body1,
00027                 btRigidBody* body2,
00028 
00029                                                 const btMatrix3x3& world2A,
00030                                                 const btMatrix3x3& world2B,
00031                                                 
00032                                                 const btVector3& invInertiaADiag,
00033                                                 const btScalar invMassA,
00034                                                 const btVector3& linvelA,const btVector3& angvelA,
00035                                                 const btVector3& rel_posA1,
00036                                                 const btVector3& invInertiaBDiag,
00037                                                 const btScalar invMassB,
00038                                                 const btVector3& linvelB,const btVector3& angvelB,
00039                                                 const btVector3& rel_posA2,
00040 
00041                                           btScalar depthA, const btVector3& normalA, 
00042                                           const btVector3& rel_posB1,const btVector3& rel_posB2,
00043                                           btScalar depthB, const btVector3& normalB, 
00044                                           btScalar& imp0,btScalar& imp1)
00045 {
00046         (void)linvelA;
00047         (void)linvelB;
00048         (void)angvelB;
00049         (void)angvelA;
00050 
00051 
00052 
00053         imp0 = btScalar(0.);
00054         imp1 = btScalar(0.);
00055 
00056         btScalar len = btFabs(normalA.length()) - btScalar(1.);
00057         if (btFabs(len) >= SIMD_EPSILON)
00058                 return;
00059 
00060         btAssert(len < SIMD_EPSILON);
00061 
00062 
00063         //this jacobian entry could be re-used for all iterations
00064         btJacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA,
00065                 invInertiaBDiag,invMassB);
00066         btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA,
00067                 invInertiaBDiag,invMassB);
00068         
00069         //const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
00070         //const btScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
00071 
00072         const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1)-body2->getVelocityInLocalPoint(rel_posA1));
00073         const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1)-body2->getVelocityInLocalPoint(rel_posB1));
00074 
00075 //      btScalar penetrationImpulse = (depth*contactTau*timeCorrection)  * massTerm;//jacDiagABInv
00076         btScalar massTerm = btScalar(1.) / (invMassA + invMassB);
00077 
00078 
00079         // calculate rhs (or error) terms
00080         const btScalar dv0 = depthA  * m_tau * massTerm - vel0 * m_damping;
00081         const btScalar dv1 = depthB  * m_tau * massTerm - vel1 * m_damping;
00082 
00083 
00084         // dC/dv * dv = -C
00085         
00086         // jacobian * impulse = -error
00087         //
00088 
00089         //impulse = jacobianInverse * -error
00090 
00091         // inverting 2x2 symmetric system (offdiagonal are equal!)
00092         // 
00093 
00094 
00095         btScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB);
00096         btScalar        invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag );
00097         
00098         //imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
00099         //imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
00100 
00101         imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
00102         imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
00103 
00104         //[a b]                                                           [d -c]
00105         //[c d] inverse = (1 / determinant) * [-b a] where determinant is (ad - bc)
00106 
00107         //[jA nD] * [imp0] = [dv0]
00108         //[nD jB]   [imp1]   [dv1]
00109 
00110 }
00111 
00112 
00113 
00114 void btSolve2LinearConstraint::resolveBilateralPairConstraint(
00115                                                 btRigidBody* body1,
00116                                                 btRigidBody* body2,
00117                                                 const btMatrix3x3& world2A,
00118                                                 const btMatrix3x3& world2B,
00119                                                 
00120                                                 const btVector3& invInertiaADiag,
00121                                                 const btScalar invMassA,
00122                                                 const btVector3& linvelA,const btVector3& angvelA,
00123                                                 const btVector3& rel_posA1,
00124                                                 const btVector3& invInertiaBDiag,
00125                                                 const btScalar invMassB,
00126                                                 const btVector3& linvelB,const btVector3& angvelB,
00127                                                 const btVector3& rel_posA2,
00128 
00129                                           btScalar depthA, const btVector3& normalA, 
00130                                           const btVector3& rel_posB1,const btVector3& rel_posB2,
00131                                           btScalar depthB, const btVector3& normalB, 
00132                                           btScalar& imp0,btScalar& imp1)
00133 {
00134 
00135         (void)linvelA;
00136         (void)linvelB;
00137         (void)angvelA;
00138         (void)angvelB;
00139 
00140 
00141 
00142         imp0 = btScalar(0.);
00143         imp1 = btScalar(0.);
00144 
00145         btScalar len = btFabs(normalA.length()) - btScalar(1.);
00146         if (btFabs(len) >= SIMD_EPSILON)
00147                 return;
00148 
00149         btAssert(len < SIMD_EPSILON);
00150 
00151 
00152         //this jacobian entry could be re-used for all iterations
00153         btJacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA,
00154                 invInertiaBDiag,invMassB);
00155         btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA,
00156                 invInertiaBDiag,invMassB);
00157         
00158         //const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
00159         //const btScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
00160 
00161         const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1)-body2->getVelocityInLocalPoint(rel_posA1));
00162         const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1)-body2->getVelocityInLocalPoint(rel_posB1));
00163 
00164         // calculate rhs (or error) terms
00165         const btScalar dv0 = depthA  * m_tau - vel0 * m_damping;
00166         const btScalar dv1 = depthB  * m_tau - vel1 * m_damping;
00167 
00168         // dC/dv * dv = -C
00169         
00170         // jacobian * impulse = -error
00171         //
00172 
00173         //impulse = jacobianInverse * -error
00174 
00175         // inverting 2x2 symmetric system (offdiagonal are equal!)
00176         // 
00177 
00178 
00179         btScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB);
00180         btScalar        invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag );
00181         
00182         //imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
00183         //imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
00184 
00185         imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
00186         imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
00187 
00188         //[a b]                                                           [d -c]
00189         //[c d] inverse = (1 / determinant) * [-b a] where determinant is (ad - bc)
00190 
00191         //[jA nD] * [imp0] = [dv0]
00192         //[nD jB]   [imp1]   [dv1]
00193 
00194         if ( imp0 > btScalar(0.0))
00195         {
00196                 if ( imp1 > btScalar(0.0) )
00197                 {
00198                         //both positive
00199                 }
00200                 else
00201                 {
00202                         imp1 = btScalar(0.);
00203 
00204                         // now imp0>0 imp1<0
00205                         imp0 = dv0 / jacA.getDiagonal();
00206                         if ( imp0 > btScalar(0.0) )
00207                         {
00208                         } else
00209                         {
00210                                 imp0 = btScalar(0.);
00211                         }
00212                 }
00213         }
00214         else
00215         {
00216                 imp0 = btScalar(0.);
00217 
00218                 imp1 = dv1 / jacB.getDiagonal();
00219                 if ( imp1 <= btScalar(0.0) )
00220                 {
00221                         imp1 = btScalar(0.);
00222                         // now imp0>0 imp1<0
00223                         imp0 = dv0 / jacA.getDiagonal();
00224                         if ( imp0 > btScalar(0.0) )
00225                         {
00226                         } else
00227                         {
00228                                 imp0 = btScalar(0.);
00229                         }
00230                 } else
00231                 {
00232                 }
00233         }
00234 }
00235 
00236 
00237 /*
00238 void btSolve2LinearConstraint::resolveAngularConstraint(        const btMatrix3x3& invInertiaAWS,
00239                                                                                         const btScalar invMassA,
00240                                                                                         const btVector3& linvelA,const btVector3& angvelA,
00241                                                                                         const btVector3& rel_posA1,
00242                                                                                         const btMatrix3x3& invInertiaBWS,
00243                                                                                         const btScalar invMassB,
00244                                                                                         const btVector3& linvelB,const btVector3& angvelB,
00245                                                                                         const btVector3& rel_posA2,
00246 
00247                                                                                         btScalar depthA, const btVector3& normalA, 
00248                                                                                         const btVector3& rel_posB1,const btVector3& rel_posB2,
00249                                                                                         btScalar depthB, const btVector3& normalB, 
00250                                                                                         btScalar& imp0,btScalar& imp1)
00251 {
00252 
00253 }
00254 */
00255