diff options
author | Benoit Bolsee <benoit.bolsee@online.be> | 2009-09-27 20:20:42 +0400 |
---|---|---|
committer | Benoit Bolsee <benoit.bolsee@online.be> | 2009-09-27 20:20:42 +0400 |
commit | 0cbc87b42816711469cb63825ee826a837cc364a (patch) | |
tree | c345b917fa3072008399b4b3d583f63b2465fed1 /intern/itasc/WSDLSSolver.cpp | |
parent | 2fef3dbaa3512f6561155402c46bf3ff8c955980 (diff) |
Speed optimization in itasc when with armature with many bones and few targets. Thanks to Brecht who pointed out a simple but efficient optimization in SVD decomposition.
Diffstat (limited to 'intern/itasc/WSDLSSolver.cpp')
-rw-r--r-- | intern/itasc/WSDLSSolver.cpp | 26 |
1 files changed, 21 insertions, 5 deletions
diff --git a/intern/itasc/WSDLSSolver.cpp b/intern/itasc/WSDLSSolver.cpp index 971fb7f482e..9f7ebed960a 100644 --- a/intern/itasc/WSDLSSolver.cpp +++ b/intern/itasc/WSDLSSolver.cpp @@ -31,13 +31,23 @@ bool WSDLSSolver::init(unsigned int _nq, unsigned int _nc, const std::vector<boo m_ns = std::min(m_nc,m_nq); m_AWq = e_zero_matrix(m_nc,m_nq); m_WyAWq = e_zero_matrix(m_nc,m_nq); - m_U = e_zero_matrix(m_nc,m_nq); + m_WyAWqt = e_zero_matrix(m_nq,m_nc); m_S = e_zero_vector(std::max(m_nc,m_nq)); - m_temp = e_zero_vector(m_nq); - m_V = e_zero_matrix(m_nq,m_nq); - m_WqV = e_zero_matrix(m_nq,m_nq); m_Wy_ydot = e_zero_vector(m_nc); m_ytask = gc; + if (m_nq > m_nc) { + m_transpose = true; + m_temp = e_zero_vector(m_nc); + m_U = e_zero_matrix(m_nc,m_nc); + m_V = e_zero_matrix(m_nq,m_nc); + m_WqV = e_zero_matrix(m_nq,m_nc); + } else { + m_transpose = false; + m_temp = e_zero_vector(m_nq); + m_U = e_zero_matrix(m_nc,m_nq); + m_V = e_zero_matrix(m_nq,m_nq); + m_WqV = e_zero_matrix(m_nq,m_nq); + } return true; } @@ -52,7 +62,13 @@ bool WSDLSSolver::solve(const e_matrix& A, const e_vector& Wy, const e_vector& y m_WyAWq.row(i) = Wy(i)*m_AWq.row(i); // Compute the SVD of the weighted jacobian - int ret = KDL::svd_eigen_HH(m_WyAWq,m_U,m_S,m_V,m_temp); + int ret; + if (m_transpose) { + m_WyAWqt = m_WyAWq.transpose(); + ret = KDL::svd_eigen_HH(m_WyAWqt,m_V,m_S,m_U,m_temp); + } else { + ret = KDL::svd_eigen_HH(m_WyAWq,m_U,m_S,m_V,m_temp); + } if(ret<0) return false; |