matrix.h

Go to the documentation of this file.

#ifndef RB_MATRIX_H_
#define RB_MATRIX_H_
#include "unit.h"

#include <Eigen/Dense>
#include <Eigen/Geometry>

namespace rb    
{
namespace math  
{
typedef Eigen::MatrixXd MatrixX;        
typedef Eigen::Matrix3d Matrix3;        
typedef Eigen::Matrix4d Matrix4;        

typedef Eigen::VectorXd VectorX;        
typedef Eigen::Vector3d Vector3;        
typedef Eigen::Vector4d Vector4;        

typedef Eigen::AngleAxisd AngleAxis;    

template <typename T, size_t R, size_t C>
  using Array = Eigen::Array<T, R, C>;

typedef Eigen::Array<double, 6, 1> Array6;

inline Matrix4 homoTrans(const double& A, const double& alpha, const double& D, const double theta)
{
  double ct = cos(DEG2RAD * theta);
  double st = sin(DEG2RAD * theta);
  double ca = cos(DEG2RAD * alpha);
  double sa = sin(DEG2RAD * alpha);

  Matrix4 T;
  T << ct,   -st,   0,     A,
    st*ca, ct*ca, -sa, -sa*D,
    st*sa, ct*sa,  ca,  ca*D,
        0,     0,   0,     1;
  return T;
}

}   // namespace math
}   // namespace rb

#endif // RB_MATRIX_H_