Back to index

supertuxkart  0.5+dfsg1
btJacobianEntry.h
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 #ifndef JACOBIAN_ENTRY_H
00017 #define JACOBIAN_ENTRY_H
00018 
00019 #include "LinearMath/btVector3.h"
00020 #include "BulletDynamics/Dynamics/btRigidBody.h"
00021 
00022 
00023 //notes:
00024 // Another memory optimization would be to store m_1MinvJt in the remaining 3 w components
00025 // which makes the btJacobianEntry memory layout 16 bytes
00026 // if you only are interested in angular part, just feed massInvA and massInvB zero
00027 
00031 class btJacobianEntry
00032 {
00033 public:
00034        btJacobianEntry() {};
00035        //constraint between two different rigidbodies
00036        btJacobianEntry(
00037               const btMatrix3x3& world2A,
00038               const btMatrix3x3& world2B,
00039               const btVector3& rel_pos1,const btVector3& rel_pos2,
00040               const btVector3& jointAxis,
00041               const btVector3& inertiaInvA, 
00042               const btScalar massInvA,
00043               const btVector3& inertiaInvB,
00044               const btScalar massInvB)
00045               :m_linearJointAxis(jointAxis)
00046        {
00047               m_aJ = world2A*(rel_pos1.cross(m_linearJointAxis));
00048               m_bJ = world2B*(rel_pos2.cross(-m_linearJointAxis));
00049               m_0MinvJt     = inertiaInvA * m_aJ;
00050               m_1MinvJt = inertiaInvB * m_bJ;
00051               m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ);
00052 
00053               btAssert(m_Adiag > btScalar(0.0));
00054        }
00055 
00056        //angular constraint between two different rigidbodies
00057        btJacobianEntry(const btVector3& jointAxis,
00058               const btMatrix3x3& world2A,
00059               const btMatrix3x3& world2B,
00060               const btVector3& inertiaInvA,
00061               const btVector3& inertiaInvB)
00062               :m_linearJointAxis(btVector3(btScalar(0.),btScalar(0.),btScalar(0.)))
00063        {
00064               m_aJ= world2A*jointAxis;
00065               m_bJ = world2B*-jointAxis;
00066               m_0MinvJt     = inertiaInvA * m_aJ;
00067               m_1MinvJt = inertiaInvB * m_bJ;
00068               m_Adiag =  m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
00069 
00070               btAssert(m_Adiag > btScalar(0.0));
00071        }
00072 
00073        //angular constraint between two different rigidbodies
00074        btJacobianEntry(const btVector3& axisInA,
00075               const btVector3& axisInB,
00076               const btVector3& inertiaInvA,
00077               const btVector3& inertiaInvB)
00078               : m_linearJointAxis(btVector3(btScalar(0.),btScalar(0.),btScalar(0.)))
00079               , m_aJ(axisInA)
00080               , m_bJ(-axisInB)
00081        {
00082               m_0MinvJt     = inertiaInvA * m_aJ;
00083               m_1MinvJt = inertiaInvB * m_bJ;
00084               m_Adiag =  m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
00085 
00086               btAssert(m_Adiag > btScalar(0.0));
00087        }
00088 
00089        //constraint on one rigidbody
00090        btJacobianEntry(
00091               const btMatrix3x3& world2A,
00092               const btVector3& rel_pos1,const btVector3& rel_pos2,
00093               const btVector3& jointAxis,
00094               const btVector3& inertiaInvA, 
00095               const btScalar massInvA)
00096               :m_linearJointAxis(jointAxis)
00097        {
00098               m_aJ= world2A*(rel_pos1.cross(jointAxis));
00099               m_bJ = world2A*(rel_pos2.cross(-jointAxis));
00100               m_0MinvJt     = inertiaInvA * m_aJ;
00101               m_1MinvJt = btVector3(btScalar(0.),btScalar(0.),btScalar(0.));
00102               m_Adiag = massInvA + m_0MinvJt.dot(m_aJ);
00103 
00104               btAssert(m_Adiag > btScalar(0.0));
00105        }
00106 
00107        btScalar      getDiagonal() const { return m_Adiag; }
00108 
00109        // for two constraints on the same rigidbody (for example vehicle friction)
00110        btScalar      getNonDiagonal(const btJacobianEntry& jacB, const btScalar massInvA) const
00111        {
00112               const btJacobianEntry& jacA = *this;
00113               btScalar lin = massInvA * jacA.m_linearJointAxis.dot(jacB.m_linearJointAxis);
00114               btScalar ang = jacA.m_0MinvJt.dot(jacB.m_aJ);
00115               return lin + ang;
00116        }
00117 
00118        
00119 
00120        // for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies)
00121        btScalar      getNonDiagonal(const btJacobianEntry& jacB,const btScalar massInvA,const btScalar massInvB) const
00122        {
00123               const btJacobianEntry& jacA = *this;
00124               btVector3 lin = jacA.m_linearJointAxis * jacB.m_linearJointAxis;
00125               btVector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ;
00126               btVector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ;
00127               btVector3 lin0 = massInvA * lin ;
00128               btVector3 lin1 = massInvB * lin;
00129               btVector3 sum = ang0+ang1+lin0+lin1;
00130               return sum[0]+sum[1]+sum[2];
00131        }
00132 
00133        btScalar getRelativeVelocity(const btVector3& linvelA,const btVector3& angvelA,const btVector3& linvelB,const btVector3& angvelB)
00134        {
00135               btVector3 linrel = linvelA - linvelB;
00136               btVector3 angvela  = angvelA * m_aJ;
00137               btVector3 angvelb  = angvelB * m_bJ;
00138               linrel *= m_linearJointAxis;
00139               angvela += angvelb;
00140               angvela += linrel;
00141               btScalar rel_vel2 = angvela[0]+angvela[1]+angvela[2];
00142               return rel_vel2 + SIMD_EPSILON;
00143        }
00144 //private:
00145 
00146        btVector3     m_linearJointAxis;
00147        btVector3     m_aJ;
00148        btVector3     m_bJ;
00149        btVector3     m_0MinvJt;
00150        btVector3     m_1MinvJt;
00151        //Optimization: can be stored in the w/last component of one of the vectors
00152        btScalar      m_Adiag;
00153 
00154 };
00155 
00156 #endif //JACOBIAN_ENTRY_H