Blender V2.61 - r43446

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