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