size_t NeedleCollisionHash::hash(const DblVec& x) {
DblVec extended_x = x;
for (int i = 0; i < helper->pis.size(); ++i) {
for (int j = 0; j < helper->pis[i]->local_configs.size(); ++j) {
DblVec state = toDblVec(logDown(helper->pis[i]->local_configs[j]->pose));
extended_x.insert(extended_x.end(), state.begin(), state.end());
}
}
return boost::hash_range(extended_x.begin(), extended_x.end());
}
inline double vecMax(const DblVec& v) {
return *std::max_element(v.begin(), v.end());
}
/*!
* A const function that for every value in a vector calculates the matrix
* exponential of the matrix multiplied with that value
* The exponential is calculated by finding the eigenvalues and eigenvectors
* of the matrix, exponentiating the eigenvalues. The eigenvalues is stored in
* a matrix V, eigenvectors is stored in a matrix A, inv(A) is calculated.
* The product A*V*inv(A) is returned.
* @param s A vector with values to be multiplied with the matrix before
* the exponent is calculated.
* @return A vector with the exponential of the matrix multiplied with every
* value in s
*/
MatVec Matrix::expm(const DblVec &s) const {
// Can only calculate eigenvalues and vectors of square matrices
if (get_rows() != get_cols())
throw std::out_of_range("Matrix needs to be square");
int size = get_rows();
DblVec eg_val_real(size, 0); // Real part of eigenvalues
DblVec eg_val_im(size, 0); // Imaginary part of eigenvalues
// should be zero
double dummy[1];
int dummy_size = 1;
double dummy_one = 1;
int info[1];
char n = 'N'; // Do not want to use this argument
char v = 'V'; // Want to use this argument
double workspace_size[1];
int w_query = -1;
// Need to make a copy of the data in Q to send into dgeev_ because
// the data sent in is overwritten
int data_size = get_rows()*get_cols();
DblVec data(m_data);
// Matrix for the eigenvectors
Matrix t_mat = Matrix(size, size);
//workspace-query
// SUBROUTINE DGEEV( JOBVL, JOBVR, N, A, LDA, WR, WI, VL, LDVL, VR,
// LDVR, WORK, LWORK, INFO )
dgeev_(&n, &v, &size, &data[0], &size, &eg_val_real[0], &eg_val_im[0], dummy,
&dummy_size, &t_mat.m_data[0], &size, workspace_size, &w_query, info);
DblVec workspace_vec(static_cast<int>(workspace_size[0]), 0);
int w_size = static_cast<int>(workspace_size[0]);
// Real calculation of eigenvalues and eigenvectors for Q
dgeev_(&n, &v, &size, &data[0], &size, &eg_val_real[0], &eg_val_im[0], dummy,
&dummy_size, &t_mat.m_data[0], &size, &workspace_vec[0], &w_size, info);
// Calculating inverse of matrix with eigenvectors
Matrix t_mat_inv(t_mat);
int ipiv[size];
// LU factorization, t_mat_inv.m_data is overwritten with the LU factorization
dgetrf_(&size, &size, &t_mat_inv.m_data[0], &size, ipiv, info);
//workspace-query, nothing happens with t_mat_inv.m_data
dgetri_(&size, &t_mat_inv.m_data[0], &size, ipiv, workspace_size, &w_query, info);
double workspace_vec2[static_cast<int>(workspace_size[0])];
w_size = static_cast<int>(workspace_size[0]);
// Inverse calculation from LU values, the inverse is stored in t_mat_inv.m_data
dgetri_(&size, &t_mat_inv.m_data[0], &size, ipiv, workspace_vec2, &w_size, info);
MatVec result;
result.reserve(s.size());
// e^(this) = T*D*T^-1
// T = matrix with eigenvectors (t_mat), D = matrix with exponentiated eigenvalues
// Calculate for every value in incoming vector s
DblVec eg_val_exp;
eg_val_exp.reserve(size);
for (DblVec::const_iterator it=s.begin(); it != s.end(); it++){
for (int i=0; i<size; i++)
eg_val_exp.push_back(exp(eg_val_real[i]*(*it)));
Matrix left = Matrix::mult(t_mat, Matrix(eg_val_exp));
Matrix res = Matrix::mult( left, t_mat_inv);
result.push_back(res);
eg_val_exp.clear();
}
return result;
}