diff options
Diffstat (limited to 'servers/physics/joints/jacobian_entry_sw.h')
| -rw-r--r-- | servers/physics/joints/jacobian_entry_sw.h | 146 |
1 files changed, 146 insertions, 0 deletions
diff --git a/servers/physics/joints/jacobian_entry_sw.h b/servers/physics/joints/jacobian_entry_sw.h new file mode 100644 index 0000000000..16fa034215 --- /dev/null +++ b/servers/physics/joints/jacobian_entry_sw.h @@ -0,0 +1,146 @@ +#ifndef JACOBIAN_ENTRY_SW_H +#define JACOBIAN_ENTRY_SW_H + +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "transform.h" + +class JacobianEntrySW { +public: + JacobianEntrySW() {}; + //constraint between two different rigidbodies + JacobianEntrySW( + const Matrix3& world2A, + const Matrix3& world2B, + const Vector3& rel_pos1,const Vector3& rel_pos2, + const Vector3& jointAxis, + const Vector3& inertiaInvA, + const real_t massInvA, + const Vector3& inertiaInvB, + const real_t massInvB) + :m_linearJointAxis(jointAxis) + { + m_aJ = world2A.xform(rel_pos1.cross(m_linearJointAxis)); + m_bJ = world2B.xform(rel_pos2.cross(-m_linearJointAxis)); + m_0MinvJt = inertiaInvA * m_aJ; + m_1MinvJt = inertiaInvB * m_bJ; + m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ); + + ERR_FAIL_COND(m_Adiag <= real_t(0.0)); + } + + //angular constraint between two different rigidbodies + JacobianEntrySW(const Vector3& jointAxis, + const Matrix3& world2A, + const Matrix3& world2B, + const Vector3& inertiaInvA, + const Vector3& inertiaInvB) + :m_linearJointAxis(Vector3(real_t(0.),real_t(0.),real_t(0.))) + { + m_aJ= world2A.xform(jointAxis); + m_bJ = world2B.xform(-jointAxis); + m_0MinvJt = inertiaInvA * m_aJ; + m_1MinvJt = inertiaInvB * m_bJ; + m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ); + + ERR_FAIL_COND(m_Adiag <= real_t(0.0)); + } + + //angular constraint between two different rigidbodies + JacobianEntrySW(const Vector3& axisInA, + const Vector3& axisInB, + const Vector3& inertiaInvA, + const Vector3& inertiaInvB) + : m_linearJointAxis(Vector3(real_t(0.),real_t(0.),real_t(0.))) + , m_aJ(axisInA) + , m_bJ(-axisInB) + { + m_0MinvJt = inertiaInvA * m_aJ; + m_1MinvJt = inertiaInvB * m_bJ; + m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ); + + ERR_FAIL_COND(m_Adiag <= real_t(0.0)); + } + + //constraint on one rigidbody + JacobianEntrySW( + const Matrix3& world2A, + const Vector3& rel_pos1,const Vector3& rel_pos2, + const Vector3& jointAxis, + const Vector3& inertiaInvA, + const real_t massInvA) + :m_linearJointAxis(jointAxis) + { + m_aJ= world2A.xform(rel_pos1.cross(jointAxis)); + m_bJ = world2A.xform(rel_pos2.cross(-jointAxis)); + m_0MinvJt = inertiaInvA * m_aJ; + m_1MinvJt = Vector3(real_t(0.),real_t(0.),real_t(0.)); + m_Adiag = massInvA + m_0MinvJt.dot(m_aJ); + + ERR_FAIL_COND(m_Adiag <= real_t(0.0)); + } + + real_t getDiagonal() const { return m_Adiag; } + + // for two constraints on the same rigidbody (for example vehicle friction) + real_t getNonDiagonal(const JacobianEntrySW& jacB, const real_t massInvA) const + { + const JacobianEntrySW& jacA = *this; + real_t lin = massInvA * jacA.m_linearJointAxis.dot(jacB.m_linearJointAxis); + real_t ang = jacA.m_0MinvJt.dot(jacB.m_aJ); + return lin + ang; + } + + + + // for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies) + real_t getNonDiagonal(const JacobianEntrySW& jacB,const real_t massInvA,const real_t massInvB) const + { + const JacobianEntrySW& jacA = *this; + Vector3 lin = jacA.m_linearJointAxis * jacB.m_linearJointAxis; + Vector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ; + Vector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ; + Vector3 lin0 = massInvA * lin ; + Vector3 lin1 = massInvB * lin; + Vector3 sum = ang0+ang1+lin0+lin1; + return sum[0]+sum[1]+sum[2]; + } + + real_t getRelativeVelocity(const Vector3& linvelA,const Vector3& angvelA,const Vector3& linvelB,const Vector3& angvelB) + { + Vector3 linrel = linvelA - linvelB; + Vector3 angvela = angvelA * m_aJ; + Vector3 angvelb = angvelB * m_bJ; + linrel *= m_linearJointAxis; + angvela += angvelb; + angvela += linrel; + real_t rel_vel2 = angvela[0]+angvela[1]+angvela[2]; + return rel_vel2 + CMP_EPSILON; + } +//private: + + Vector3 m_linearJointAxis; + Vector3 m_aJ; + Vector3 m_bJ; + Vector3 m_0MinvJt; + Vector3 m_1MinvJt; + //Optimization: can be stored in the w/last component of one of the vectors + real_t m_Adiag; + +}; + + +#endif // JACOBIAN_ENTRY_SW_H |