From 0cbc87b42816711469cb63825ee826a837cc364a Mon Sep 17 00:00:00 2001 From: Benoit Bolsee Date: Sun, 27 Sep 2009 16:20:42 +0000 Subject: 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. --- intern/itasc/WDLSSolver.cpp | 26 +++++++++++++++++++++----- intern/itasc/WDLSSolver.hpp | 3 ++- intern/itasc/WSDLSSolver.cpp | 26 +++++++++++++++++++++----- intern/itasc/WSDLSSolver.hpp | 3 ++- 4 files changed, 46 insertions(+), 12 deletions(-) (limited to 'intern/itasc') diff --git a/intern/itasc/WDLSSolver.cpp b/intern/itasc/WDLSSolver.cpp index e8bfc95e5dd..1d0efde54c9 100644 --- a/intern/itasc/WDLSSolver.cpp +++ b/intern/itasc/WDLSSolver.cpp @@ -24,12 +24,22 @@ bool WDLSSolver::init(unsigned int nq, unsigned int nc, const std::vector& m_ns = std::min(nc,nq); m_AWq = e_zero_matrix(nc,nq); m_WyAWq = e_zero_matrix(nc,nq); - m_U = e_zero_matrix(nc,nq); + m_WyAWqt = e_zero_matrix(nq,nc); m_S = e_zero_vector(std::max(nc,nq)); - m_temp = e_zero_vector(nq); - m_V = e_zero_matrix(nq,nq); - m_WqV = e_zero_matrix(nq,nq); m_Wy_ydot = e_zero_vector(nc); + if (nq > nc) { + m_transpose = true; + m_temp = e_zero_vector(nc); + m_U = e_zero_matrix(nc,nc); + m_V = e_zero_matrix(nq,nc); + m_WqV = e_zero_matrix(nq,nc); + } else { + m_transpose = false; + m_temp = e_zero_vector(nq); + m_U = e_zero_matrix(nc,nq); + m_V = e_zero_matrix(nq,nq); + m_WqV = e_zero_matrix(nq,nq); + } return true; } @@ -42,7 +52,13 @@ bool WDLSSolver::solve(const e_matrix& A, const e_vector& Wy, const e_vector& yd 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; diff --git a/intern/itasc/WDLSSolver.hpp b/intern/itasc/WDLSSolver.hpp index 4418e73675c..b56ad1ab2b8 100644 --- a/intern/itasc/WDLSSolver.hpp +++ b/intern/itasc/WDLSSolver.hpp @@ -14,12 +14,13 @@ namespace iTaSC { class WDLSSolver: public iTaSC::Solver { private: - e_matrix m_AWq,m_WyAWq,m_U,m_V,m_WqV; + e_matrix m_AWq,m_WyAWq,m_WyAWqt,m_U,m_V,m_WqV; e_vector m_S,m_temp,m_Wy_ydot; double m_lambda; double m_epsilon; double m_qmax; int m_ns; + bool m_transpose; public: WDLSSolver(); virtual ~WDLSSolver(); 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 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; diff --git a/intern/itasc/WSDLSSolver.hpp b/intern/itasc/WSDLSSolver.hpp index 1341cf2af66..0b17f26ef47 100644 --- a/intern/itasc/WSDLSSolver.hpp +++ b/intern/itasc/WSDLSSolver.hpp @@ -14,11 +14,12 @@ namespace iTaSC { class WSDLSSolver: public iTaSC::Solver { private: - e_matrix m_AWq,m_WyAWq,m_U,m_V,m_WqV; + e_matrix m_AWq,m_WyAWq,m_WyAWqt,m_U,m_V,m_WqV; e_vector m_S,m_temp,m_Wy_ydot; std::vector m_ytask; e_scalar m_qmax; unsigned int m_ns, m_nc, m_nq; + bool m_transpose; public: WSDLSSolver(); virtual ~WSDLSSolver(); -- cgit v1.2.3