From c921ee25cc71decc843b230fb28a7fb195fb6786 Mon Sep 17 00:00:00 2001 From: Brecht Van Lommel Date: Mon, 29 Aug 2005 12:06:23 +0000 Subject: Bugfix; rotation limits for < 3 DOF bones were using wrong reference rotation, causing incorrect limits if there was already a pose transform. --- intern/iksolver/intern/IK_QSegment.cpp | 57 ++++++++++++++++++++++++++++++---- intern/iksolver/intern/IK_QSegment.h | 6 ++++ 2 files changed, 57 insertions(+), 6 deletions(-) (limited to 'intern/iksolver') diff --git a/intern/iksolver/intern/IK_QSegment.cpp b/intern/iksolver/intern/IK_QSegment.cpp index 8d6655b1531..310f3708499 100644 --- a/intern/iksolver/intern/IK_QSegment.cpp +++ b/intern/iksolver/intern/IK_QSegment.cpp @@ -55,6 +55,22 @@ static MT_Matrix3x3 RotationMatrix(MT_Scalar angle, int axis) return RotationMatrix(sin(angle), cos(angle), axis); } + +static MT_Scalar EulerAngleFromMatrix(const MT_Matrix3x3& R, int axis) +{ + MT_Scalar t = sqrt(R[0][0]*R[0][0] + R[0][1]*R[0][1]); + + if (t > 16.0*MT_EPSILON) { + if (axis == 0) return atan2(R[1][2], R[2][2]); + else if(axis == 1) return atan2(-R[0][2], t); + else return atan2(R[0][1], R[0][0]); + } else { + if (axis == 0) return atan2(-R[2][1], R[1][1]); + else if(axis == 1) return atan2(-R[0][2], t); + else return 0.0f; + } +} + static MT_Scalar safe_acos(MT_Scalar f) { if (f <= -1.0f) @@ -173,10 +189,11 @@ void IK_QSegment::SetTransform( m_start = start; m_rest_basis = rest_basis; - m_basis = basis; - m_translation = MT_Vector3(0, length, 0); - m_orig_basis = m_basis; + m_orig_basis = basis; + SetBasis(basis); + + m_translation = MT_Vector3(0, length, 0); m_orig_translation = m_translation; } @@ -485,6 +502,18 @@ IK_QRevoluteSegment::IK_QRevoluteSegment(int axis) { } +void IK_QRevoluteSegment::SetBasis(const MT_Matrix3x3& basis) +{ + if (m_axis == 1) { + m_angle = ComputeTwist(basis); + m_basis = ComputeTwistMatrix(m_angle); + } + else { + m_angle = EulerAngleFromMatrix(basis, m_axis); + m_basis = RotationMatrix(m_angle, m_axis); + } +} + MT_Vector3 IK_QRevoluteSegment::Axis(int) const { return m_global_transform.getBasis().getColumn(m_axis); @@ -524,8 +553,7 @@ void IK_QRevoluteSegment::Lock(int, IK_QJacobian& jacobian, MT_Vector3& delta) void IK_QRevoluteSegment::UpdateAngleApply() { m_angle = m_new_angle; - - m_basis = m_orig_basis*RotationMatrix(m_angle, m_axis); + m_basis = RotationMatrix(m_angle, m_axis); } void IK_QRevoluteSegment::SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax) @@ -556,6 +584,12 @@ IK_QSwingSegment::IK_QSwingSegment() { } +void IK_QSwingSegment::SetBasis(const MT_Matrix3x3& basis) +{ + m_basis = basis; + RemoveTwist(m_basis); +} + MT_Vector3 IK_QSwingSegment::Axis(int dof) const { return m_global_transform.getBasis().getColumn((dof==0)? 0: 2); @@ -736,6 +770,17 @@ IK_QElbowSegment::IK_QElbowSegment(int axis) { } +void IK_QElbowSegment::SetBasis(const MT_Matrix3x3& basis) +{ + m_basis = basis; + + m_twist = ComputeTwist(m_basis); + RemoveTwist(m_basis); + m_angle = EulerAngleFromMatrix(basis, m_axis); + + m_basis = RotationMatrix(m_angle, m_axis)*ComputeTwistMatrix(m_twist); +} + MT_Vector3 IK_QElbowSegment::Axis(int dof) const { if (dof == 0) { @@ -818,7 +863,7 @@ void IK_QElbowSegment::UpdateAngleApply() MT_Matrix3x3 A = RotationMatrix(m_angle, m_axis); MT_Matrix3x3 T = RotationMatrix(m_sin_twist, m_cos_twist, 1); - m_basis = m_orig_basis*A*T; + m_basis = A*T; } void IK_QElbowSegment::SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax) diff --git a/intern/iksolver/intern/IK_QSegment.h b/intern/iksolver/intern/IK_QSegment.h index c6367dc4e03..d6082e205ae 100644 --- a/intern/iksolver/intern/IK_QSegment.h +++ b/intern/iksolver/intern/IK_QSegment.h @@ -157,6 +157,8 @@ public: // set joint weights (per axis) virtual void SetWeight(int, MT_Scalar) {}; + virtual void SetBasis(const MT_Matrix3x3& basis) { m_basis = basis; } + protected: // num_DoF: number of degrees of freedom @@ -228,6 +230,7 @@ public: void UpdateAngleApply() {} MT_Vector3 Axis(int) const { return MT_Vector3(0, 0, 0); } + void SetBasis(const MT_Matrix3x3&) { m_basis.setIdentity(); } }; class IK_QRevoluteSegment : public IK_QSegment @@ -244,6 +247,7 @@ public: void SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax); void SetWeight(int axis, MT_Scalar weight); + void SetBasis(const MT_Matrix3x3& basis); private: int m_axis; @@ -266,6 +270,7 @@ public: void SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax); void SetWeight(int axis, MT_Scalar weight); + void SetBasis(const MT_Matrix3x3& basis); private: MT_Matrix3x3 m_new_basis; @@ -288,6 +293,7 @@ public: void SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax); void SetWeight(int axis, MT_Scalar weight); + void SetBasis(const MT_Matrix3x3& basis); private: int m_axis; -- cgit v1.2.3