Fix #27891: IK stretch gives inaccurate results. Tweaked translation segment
authorBrecht Van Lommel <brechtvanlommel@pandora.be>
Fri, 8 Jul 2011 12:18:54 +0000 (12:18 +0000)
committerBrecht Van Lommel <brechtvanlommel@pandora.be>
Fri, 8 Jul 2011 12:18:54 +0000 (12:18 +0000)
convergence weight a bit to match angles better at typical scales.

intern/iksolver/intern/IK_QJacobian.cpp
intern/iksolver/intern/IK_QJacobian.h
intern/iksolver/intern/IK_QTask.cpp

index 6cc73e9c8085444182c39872e445a03d9a8f85d9..5a216309ceeedd304418375dd9dd13ec839d91b1 100644 (file)
@@ -59,6 +59,7 @@ void IK_QJacobian::ArmMatrices(int dof, int task_size)
 
        m_d_theta.newsize(dof);
        m_d_theta_tmp.newsize(dof);
+       m_d_norm_weight.newsize(dof);
 
        m_norm.newsize(dof);
        m_norm = 0.0;
@@ -111,11 +112,13 @@ void IK_QJacobian::SetBetas(int id, int, const MT_Vector3& v)
        m_beta[id+2] = v.z();
 }
 
-void IK_QJacobian::SetDerivatives(int id, int dof_id, const MT_Vector3& v)
+void IK_QJacobian::SetDerivatives(int id, int dof_id, const MT_Vector3& v, MT_Scalar norm_weight)
 {
        m_jacobian[id][dof_id] = v.x()*m_weight_sqrt[dof_id];
        m_jacobian[id+1][dof_id] = v.y()*m_weight_sqrt[dof_id];
        m_jacobian[id+2][dof_id] = v.z()*m_weight_sqrt[dof_id];
+
+       m_d_norm_weight[dof_id] = norm_weight;
 }
 
 void IK_QJacobian::Invert()
@@ -429,7 +432,7 @@ MT_Scalar IK_QJacobian::AngleUpdateNorm() const
        MT_Scalar mx = 0.0, dtheta_abs;
 
        for (i = 0; i < m_d_theta.size(); i++) {
-               dtheta_abs = MT_abs(m_d_theta[i]);
+               dtheta_abs = MT_abs(m_d_theta[i]*m_d_norm_weight[i]);
                if (dtheta_abs > mx)
                        mx = dtheta_abs;
        }
index 438b9a02c6796597d4d12d1aaf6d86b37c564323..ecfaee5ff972bf1616bafec8542908513744e521 100644 (file)
@@ -56,7 +56,7 @@ public:
 
        // Iteratively called
        void SetBetas(int id, int size, const MT_Vector3& v);
-       void SetDerivatives(int id, int dof_id, const MT_Vector3& v);
+       void SetDerivatives(int id, int dof_id, const MT_Vector3& v, MT_Scalar norm_weight);
 
        void Invert();
 
@@ -89,6 +89,7 @@ private:
 
        /// the vector of computed angle changes
        TVector m_d_theta;
+       TVector m_d_norm_weight;
 
        /// space required for SVD computation
 
index 32ecb833899c07545710722d83567866c4ec26ae..32143518533f61d4533780709b2991971d014b07 100644 (file)
@@ -95,10 +95,10 @@ void IK_QPositionTask::ComputeJacobian(IK_QJacobian& jacobian)
                        MT_Vector3 axis = seg->Axis(i)*m_weight;
 
                        if (seg->Translational())
-                               jacobian.SetDerivatives(m_id, seg->DoFId()+i, axis);
+                               jacobian.SetDerivatives(m_id, seg->DoFId()+i, axis, 1e2);
                        else {
                                MT_Vector3 pa = p.cross(axis);
-                               jacobian.SetDerivatives(m_id, seg->DoFId()+i, pa);
+                               jacobian.SetDerivatives(m_id, seg->DoFId()+i, pa, 1e0);
                        }
                }
        }
@@ -147,10 +147,10 @@ void IK_QOrientationTask::ComputeJacobian(IK_QJacobian& jacobian)
                for (i = 0; i < seg->NumberOfDoF(); i++) {
 
                        if (seg->Translational())
-                               jacobian.SetDerivatives(m_id, seg->DoFId()+i, MT_Vector3(0, 0, 0));
+                               jacobian.SetDerivatives(m_id, seg->DoFId()+i, MT_Vector3(0, 0, 0), 1e2);
                        else {
                                MT_Vector3 axis = seg->Axis(i)*m_weight;
-                               jacobian.SetDerivatives(m_id, seg->DoFId()+i, axis);
+                               jacobian.SetDerivatives(m_id, seg->DoFId()+i, axis, 1e0);
                        }
                }
 }
@@ -202,10 +202,10 @@ void IK_QCenterOfMassTask::JacobianSegment(IK_QJacobian& jacobian, MT_Vector3& c
                axis *= /*segment->Mass()**/m_total_mass_inv;
                
                if (segment->Translational())
-                       jacobian.SetDerivatives(m_id, segment->DoFId()+i, axis);
+                       jacobian.SetDerivatives(m_id, segment->DoFId()+i, axis, 1e2);
                else {
                        MT_Vector3 pa = axis.cross(p);
-                       jacobian.SetDerivatives(m_id, segment->DoFId()+i, pa);
+                       jacobian.SetDerivatives(m_id, segment->DoFId()+i, pa, 1e0);
                }
        }