C++ DblVec::begin方法代码示例

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