Bugfix; rotation limits for < 3 DOF bones were using wrong reference

rotation, causing incorrect limits if there was already a pose transform.

diff --git a/intern/iksolver/intern/IK_QSegment.cpp b/intern/iksolver/intern/IK_QSegment.cpp
index 8d6655b153183dc0ba9fa433b324a0d0edf026c1..310f37084992b545419c9ce8531125632a2391ac 100644 (file)
--- 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 c6367dc4e036ef16d607374caa69eb74a1c2311e..d6082e205aed7c57bcd7685d5fe1b74de1d60bbb 100644 (file)
--- 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;