diff options
Diffstat (limited to 'extern/bullet/BulletDynamics/ConstraintSolver/JacobianEntry.h')
-rw-r--r-- | extern/bullet/BulletDynamics/ConstraintSolver/JacobianEntry.h | 66 |
1 files changed, 33 insertions, 33 deletions
diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/JacobianEntry.h b/extern/bullet/BulletDynamics/ConstraintSolver/JacobianEntry.h index ebf58c34485..3a89f2ad1fd 100644 --- a/extern/bullet/BulletDynamics/ConstraintSolver/JacobianEntry.h +++ b/extern/bullet/BulletDynamics/ConstraintSolver/JacobianEntry.h @@ -16,7 +16,7 @@ //notes: -// Another memory optimization would be to store m_MbJ in the remaining 3 w components +// Another memory optimization would be to store m_1MinvJt in the remaining 3 w components // which makes the JacobianEntry memory layout 16 bytes // if you only are interested in angular part, just feed massInvA and massInvB zero @@ -32,59 +32,59 @@ public: const SimdMatrix3x3& world2A, const SimdMatrix3x3& world2B, const SimdVector3& rel_pos1,const SimdVector3& rel_pos2, - const SimdVector3& normal, + const SimdVector3& jointAxis, const SimdVector3& inertiaInvA, const SimdScalar massInvA, const SimdVector3& inertiaInvB, const SimdScalar massInvB) - :m_normalAxis(normal) + :m_jointAxis(jointAxis) { - m_aJ = world2A*(rel_pos1.cross(normal)); - m_bJ = world2B*(rel_pos2.cross(normal)); - m_MaJ = inertiaInvA * m_aJ; - m_MbJ = inertiaInvB * m_bJ; - m_jacDiagAB = massInvA + m_MaJ.dot(m_aJ) + massInvB + m_MbJ.dot(m_bJ); + m_aJ = world2A*(rel_pos1.cross(m_jointAxis)); + m_bJ = world2B*(rel_pos2.cross(m_jointAxis)); + m_0MinvJt = inertiaInvA * m_aJ; + m_1MinvJt = inertiaInvB * m_bJ; + m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ); } //angular constraint between two different rigidbodies - JacobianEntry(const SimdVector3& normal, + JacobianEntry(const SimdVector3& jointAxis, const SimdMatrix3x3& world2A, const SimdMatrix3x3& world2B, const SimdVector3& inertiaInvA, const SimdVector3& inertiaInvB) - :m_normalAxis(normal) + :m_jointAxis(m_jointAxis) { - m_aJ= world2A*normal; - m_bJ = world2B*-normal; - m_MaJ = inertiaInvA * m_aJ; - m_MbJ = inertiaInvB * m_bJ; - m_jacDiagAB = m_MaJ.dot(m_aJ) + m_MbJ.dot(m_bJ); + m_aJ= world2A*m_jointAxis; + m_bJ = world2B*-m_jointAxis; + m_0MinvJt = inertiaInvA * m_aJ; + m_1MinvJt = inertiaInvB * m_bJ; + m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ); } //constraint on one rigidbody JacobianEntry( const SimdMatrix3x3& world2A, const SimdVector3& rel_pos1,const SimdVector3& rel_pos2, - const SimdVector3& normal, + const SimdVector3& jointAxis, const SimdVector3& inertiaInvA, const SimdScalar massInvA) - :m_normalAxis(normal) + :m_jointAxis(jointAxis) { - m_aJ= world2A*(rel_pos1.cross(normal)); - m_bJ = world2A*(rel_pos2.cross(normal)); - m_MaJ = inertiaInvA * m_aJ; - m_MbJ = SimdVector3(0.f,0.f,0.f); - m_jacDiagAB = massInvA + m_MaJ.dot(m_aJ); + m_aJ= world2A*(rel_pos1.cross(m_jointAxis)); + m_bJ = world2A*(rel_pos2.cross(m_jointAxis)); + m_0MinvJt = inertiaInvA * m_aJ; + m_1MinvJt = SimdVector3(0.f,0.f,0.f); + m_Adiag = massInvA + m_0MinvJt.dot(m_aJ); } - SimdScalar getDiagonal() const { return m_jacDiagAB; } + SimdScalar getDiagonal() const { return m_Adiag; } // for two constraints on the same rigidbody (for example vehicle friction) SimdScalar getNonDiagonal(const JacobianEntry& jacB, const SimdScalar massInvA) const { const JacobianEntry& jacA = *this; - SimdScalar lin = massInvA * jacA.m_normalAxis.dot(jacB.m_normalAxis); - SimdScalar ang = jacA.m_MaJ.dot(jacB.m_aJ); + SimdScalar lin = massInvA * jacA.m_jointAxis.dot(jacB.m_jointAxis); + SimdScalar ang = jacA.m_0MinvJt.dot(jacB.m_aJ); return lin + ang; } @@ -94,9 +94,9 @@ public: SimdScalar getNonDiagonal(const JacobianEntry& jacB,const SimdScalar massInvA,const SimdScalar massInvB) const { const JacobianEntry& jacA = *this; - SimdVector3 lin = jacA.m_normalAxis * jacB.m_normalAxis; - SimdVector3 ang0 = jacA.m_MaJ * jacB.m_aJ; - SimdVector3 ang1 = jacA.m_MbJ * jacB.m_bJ; + SimdVector3 lin = jacA.m_jointAxis* jacB.m_jointAxis; + SimdVector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ; + SimdVector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ; SimdVector3 lin0 = massInvA * lin ; SimdVector3 lin1 = massInvB * lin; SimdVector3 sum = ang0+ang1+lin0+lin1; @@ -108,7 +108,7 @@ public: SimdVector3 linrel = linvelA - linvelB; SimdVector3 angvela = angvelA * m_aJ; SimdVector3 angvelb = angvelB * m_bJ; - linrel *= m_normalAxis; + linrel *= m_jointAxis; angvela += angvelb; angvela += linrel; SimdScalar rel_vel2 = angvela[0]+angvela[1]+angvela[2]; @@ -116,13 +116,13 @@ public: } //private: - SimdVector3 m_normalAxis; + SimdVector3 m_jointAxis; SimdVector3 m_aJ; SimdVector3 m_bJ; - SimdVector3 m_MaJ; - SimdVector3 m_MbJ; + SimdVector3 m_0MinvJt; + SimdVector3 m_1MinvJt; //Optimization: can be stored in the w/last component of one of the vectors - SimdScalar m_jacDiagAB; + SimdScalar m_Adiag; }; |