Back to index

supertuxkart  0.5+dfsg1
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