tmaq
/
tornavis
1
0
Fork 0
Code Issues 2 Pull Requests 29 Packages Projects 24 Releases Wiki Activity

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.

This commit is contained in:
Benoit Bolsee 2009-09-27 16:20:42 +00:00
parent 2fef3dbaa3
commit 0cbc87b428
4 changed files with 46 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
intern/itasc/WDLSSolver.cpp
View File

@ -24,12 +24,22 @@ bool WDLSSolver::init(unsigned int nq, unsigned int nc, const std::vector<bool>&
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;

3
intern/itasc/WDLSSolver.hpp
View File

@ -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();

26
intern/itasc/WSDLSSolver.cpp
View File

@ -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;

3
intern/itasc/WSDLSSolver.hpp
View File

@ -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<bool> m_ytask;
e_scalar m_qmax;
unsigned int m_ns, m_nc, m_nq;
bool m_transpose;
public:
WSDLSSolver();
virtual ~WSDLSSolver();