diff options
Diffstat (limited to 'servers/physics/joints/jacobian_entry_sw.h')
| -rw-r--r-- | servers/physics/joints/jacobian_entry_sw.h | 123 |
1 files changed, 55 insertions, 68 deletions
diff --git a/servers/physics/joints/jacobian_entry_sw.h b/servers/physics/joints/jacobian_entry_sw.h index cd85162ba5..b0b31ed797 100644 --- a/servers/physics/joints/jacobian_entry_sw.h +++ b/servers/physics/joints/jacobian_entry_sw.h @@ -53,22 +53,21 @@ subject to the following restrictions: class JacobianEntrySW { public: - JacobianEntrySW() {}; + JacobianEntrySW(){}; //constraint between two different rigidbodies JacobianEntrySW( - const Basis& world2A, - const Basis& 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) - { + const Basis &world2A, + const Basis &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_0MinvJt = inertiaInvA * m_aJ; m_1MinvJt = inertiaInvB * m_bJ; m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ); @@ -76,104 +75,92 @@ public: } //angular constraint between two different rigidbodies - JacobianEntrySW(const Vector3& jointAxis, - const Basis& world2A, - const Basis& world2B, - const Vector3& inertiaInvA, - const Vector3& inertiaInvB) - :m_linearJointAxis(Vector3(real_t(0.),real_t(0.),real_t(0.))) - { - m_aJ= world2A.xform(jointAxis); + JacobianEntrySW(const Vector3 &jointAxis, + const Basis &world2A, + const Basis &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_0MinvJt = inertiaInvA * m_aJ; m_1MinvJt = inertiaInvB * m_bJ; - m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(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; + 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); + 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 Basis& 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)); + const Basis &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_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; } + 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 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; + 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 lin0 = massInvA * lin; Vector3 lin1 = massInvB * lin; - Vector3 sum = ang0+ang1+lin0+lin1; - return sum[0]+sum[1]+sum[2]; + 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) - { + 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; + 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]; + real_t rel_vel2 = angvela[0] + angvela[1] + angvela[2]; return rel_vel2 + CMP_EPSILON; } -//private: + //private: - Vector3 m_linearJointAxis; - Vector3 m_aJ; - Vector3 m_bJ; - Vector3 m_0MinvJt; - Vector3 m_1MinvJt; + 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; - + real_t m_Adiag; }; - #endif // JACOBIAN_ENTRY_SW_H |