From 275c214195c9209a3d3caa0081c42fe05f6e0f92 Mon Sep 17 00:00:00 2001 From: Brecht Van Lommel Date: Sun, 28 Aug 2005 13:06:41 +0000 Subject: IK rotation limits fixes: - Z-axis rotation limits were not working, was using wrong flag. - Don't allow min limit to go over max, or vice versa. - Fix for jacobian getting overwritten with IK clamping. --- intern/iksolver/intern/IK_QJacobian.cpp | 15 +++++++-------- intern/iksolver/intern/IK_QJacobian.h | 2 +- 2 files changed, 8 insertions(+), 9 deletions(-) (limited to 'intern/iksolver') diff --git a/intern/iksolver/intern/IK_QJacobian.cpp b/intern/iksolver/intern/IK_QJacobian.cpp index cfa764454d4..3eb79c89e4d 100644 --- a/intern/iksolver/intern/IK_QJacobian.cpp +++ b/intern/iksolver/intern/IK_QJacobian.cpp @@ -72,6 +72,8 @@ void IK_QJacobian::ArmMatrices(int dof, int task_size, int tasks) if (task_size >= dof) { m_transpose = false; + m_jacobian_tmp.newsize(task_size, dof); + m_svd_u.newsize(task_size, dof); m_svd_v.newsize(dof, dof); m_svd_w.newsize(dof); @@ -87,7 +89,7 @@ void IK_QJacobian::ArmMatrices(int dof, int task_size, int tasks) // as the original, and often allows using smaller matrices. m_transpose = true; - m_jacobian_t.newsize(dof, task_size); + m_jacobian_tmp.newsize(dof, task_size); m_svd_u.newsize(task_size, task_size); m_svd_v.newsize(dof, task_size); @@ -106,8 +108,6 @@ void IK_QJacobian::SetBetas(int id, int, const MT_Vector3& v) m_beta[id] = v.x(); m_beta[id+1] = v.y(); m_beta[id+2] = v.z(); - - //printf("#: %f %f %f\n", v.x(), v.y(), v.z()); } void IK_QJacobian::SetDerivatives(int id, int dof_id, const MT_Vector3& v) @@ -115,8 +115,6 @@ void IK_QJacobian::SetDerivatives(int id, int dof_id, const MT_Vector3& v) 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]; - - //printf("%d: %f %f %f\n", dof_id, v.x(), v.y(), v.z()); } void IK_QJacobian::Invert() @@ -124,13 +122,14 @@ void IK_QJacobian::Invert() if (m_transpose) { // SVD will decompose Jt into V*W*Ut with U,V orthogonal and W diagonal, // so J = U*W*Vt and Jinv = V*Winv*Ut - TNT::transpose(m_jacobian, m_jacobian_t); - TNT::SVD(m_jacobian_t, m_svd_v, m_svd_w, m_svd_u, m_work1, m_work2); + TNT::transpose(m_jacobian, m_jacobian_tmp); + TNT::SVD(m_jacobian_tmp, m_svd_v, m_svd_w, m_svd_u, m_work1, m_work2); } else { // SVD will decompose J into U*W*Vt with U,V orthogonal and W diagonal, // so Jinv = V*Winv*Ut - TNT::SVD(m_jacobian, m_svd_u, m_svd_w, m_svd_v, m_work1, m_work2); + m_jacobian_tmp = m_jacobian; + TNT::SVD(m_jacobian_tmp, m_svd_u, m_svd_w, m_svd_v, m_work1, m_work2); } if (m_sdls) diff --git a/intern/iksolver/intern/IK_QJacobian.h b/intern/iksolver/intern/IK_QJacobian.h index a16d7a7a941..b80db1d8f53 100644 --- a/intern/iksolver/intern/IK_QJacobian.h +++ b/intern/iksolver/intern/IK_QJacobian.h @@ -79,7 +79,7 @@ private: bool m_transpose; // the jacobian matrix and it's null space projector - TMatrix m_jacobian, m_jacobian_t; + TMatrix m_jacobian, m_jacobian_tmp; TMatrix m_null; /// the vector of intermediate betas -- cgit v1.2.3