Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016 #ifndef BT_JACOBIAN_ENTRY_H
00017 #define BT_JACOBIAN_ENTRY_H
00018
00019 #include "LinearMath/btMatrix3x3.h"
00020
00021
00022
00023
00024
00025
00026
00030 ATTRIBUTE_ALIGNED16(class) btJacobianEntry
00031 {
00032 public:
00033 btJacobianEntry() {};
00034
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
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
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
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
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
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
00144
00145 btVector3 m_linearJointAxis;
00146 btVector3 m_aJ;
00147 btVector3 m_bJ;
00148 btVector3 m_0MinvJt;
00149 btVector3 m_1MinvJt;
00150
00151 btScalar m_Adiag;
00152
00153 };
00154
00155 #endif //BT_JACOBIAN_ENTRY_H