00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011 #ifndef HRPUTIL_EIGEN3D_H_INCLUDED
00012 #define HRPUTIL_EIGEN3D_H_INCLUDED
00013
00014 #include "config.h"
00015 #include "EigenTypes.h"
00016
00017 namespace hrp
00018 {
00019 typedef Eigen::Vector3d Vector3Ref;
00020 inline Vector3Ref getVector3Ref(const double* data) {
00021 return Vector3Ref(data[0], data[1], data[2]);
00022 }
00023
00024 HRP_UTIL_EXPORT void calcRodrigues(Matrix33& out_R, const Vector3& axis, double q);
00025 HRP_UTIL_EXPORT void calcRotFromRpy(Matrix33& out_R, double r, double p, double y);
00026
00027 inline Matrix33 rodrigues(const Vector3& axis, double q){
00028 Matrix33 R;
00029 calcRodrigues(R, axis, q);
00030 return R;
00031 }
00032
00033 inline Matrix33 rotFromRpy(const Vector3& rpy){
00034 Matrix33 R;
00035 calcRotFromRpy(R, rpy[0], rpy[1], rpy[2]);
00036 return R;
00037 }
00038
00039 inline Matrix33 rotFromRpy(double r, double p, double y){
00040 Matrix33 R;
00041 calcRotFromRpy(R, r, p, y);
00042 return R;
00043 }
00044
00045 inline Matrix33 rotationX(double theta){
00046 Matrix33 R;
00047 double c = cos(theta);
00048 double s = sin(theta);
00049 R << 1.0, 0.0, 0.0,
00050 0.0, c, -s,
00051 0.0, s, c ;
00052 return R;
00053 }
00054
00055 inline Matrix33 rotationY(double theta){
00056 Matrix33 R;
00057 double c = cos(theta);
00058 double s = sin(theta);
00059 R << c, 0.0, s,
00060 0.0, 1.0, 0.0,
00061 -s, 0.0, c ;
00062 return R;
00063 }
00064
00065 inline Matrix33 rotationZ(double theta){
00066 Matrix33 R;
00067 double c = cos(theta);
00068 double s = sin(theta);
00069 R << c, -s, 0.0,
00070 s, c, 0.0,
00071 0.0, 0.0, 1.0;
00072 return R;
00073 }
00074
00075 HRP_UTIL_EXPORT Vector3 omegaFromRot(const Matrix33& r);
00076 HRP_UTIL_EXPORT Vector3 rpyFromRot(const Matrix33& m);
00077
00078 HRP_UTIL_EXPORT bool isOrthogonalMatrix(Matrix33& m);
00079
00080 inline Matrix33 hat(const Vector3& c) {
00081 Matrix33 m;
00082 m << 0.0, -c(2), c(1),
00083 c(2), 0.0, -c(0),
00084 -c(1), c(0), 0.0;
00085 return m;
00086 }
00087
00088 template<class M> inline void setMatrix33(const Matrix33& m33, M& m, size_t row = 0, size_t col = 0){
00089 m(row, col) = m33(0, 0); m(row, col+1) = m33(0, 1); m(row, col+2) = m33(0, 2);
00090 ++row;
00091 m(row, col) = m33(1, 0); m(row, col+1) = m33(1, 1); m(row, col+2) = m33(1, 2);
00092 ++row;
00093 m(row, col) = m33(2, 0); m(row, col+1) = m33(2, 1); m(row, col+2) = m33(2, 2);
00094 }
00095
00096 template<class M> inline void setTransMatrix33(const Matrix33& m33, M& m, size_t row = 0, size_t col = 0){
00097 m(row, col) = m33(0, 0); m(row, col+1) = m33(1, 0); m(row, col+2) = m33(2, 0);
00098 ++row;
00099 m(row, col) = m33(0, 1); m(row, col+1) = m33(1, 1); m(row, col+2) = m33(2, 1);
00100 ++row;
00101 m(row, col) = m33(0, 2); m(row, col+1) = m33(1, 2); m(row, col+2) = m33(2, 2);
00102 }
00103
00104 template<class Array> inline void setMatrix33ToRowMajorArray
00105 (const Matrix33& m33, Array& a, size_t top = 0) {
00106 a[top++] = m33(0, 0);
00107 a[top++] = m33(0, 1);
00108 a[top++] = m33(0, 2);
00109 a[top++] = m33(1, 0);
00110 a[top++] = m33(1, 1);
00111 a[top++] = m33(1, 2);
00112 a[top++] = m33(2, 0);
00113 a[top++] = m33(2, 1);
00114 a[top ] = m33(2, 2);
00115 }
00116
00117 template<class Array> inline void getMatrix33FromRowMajorArray
00118 (Matrix33& m33, const Array& a, size_t top = 0) {
00119 m33(0, 0) = a[top++];
00120 m33(0, 1) = a[top++];
00121 m33(0, 2) = a[top++];
00122 m33(1, 0) = a[top++];
00123 m33(1, 1) = a[top++];
00124 m33(1, 2) = a[top++];
00125 m33(2, 0) = a[top++];
00126 m33(2, 1) = a[top++];
00127 m33(2, 2) = a[top ];
00128 }
00129
00130 template<class V> inline void setVector3(const Vector3& v3, V& v, size_t top = 0){
00131 v[top++] = v3(0); v[top++] = v3(1); v[top] = v3(2);
00132 }
00133
00134 template<class V> inline void setVector3(const Vector3& v3, const V& v, size_t top = 0){
00135 v[top++] = v3(0); v[top++] = v3(1); v[top] = v3(2);
00136 }
00137
00138 template<class V> inline void getVector3(Vector3& v3, const V& v, size_t top = 0){
00139 v3(0) = v[top++]; v3(1) = v[top++]; v3(2) = v[top];
00140 }
00141
00142 template<class M> inline void setVector3(const Vector3& v3, M& m, size_t row, size_t col){
00143 m(row++, col) = v3(0); m(row++, col) = v3(1); m(row, col) = v3(2);
00144 }
00145
00146 template<class M> inline void getVector3(Vector3& v3, const M& m, size_t row, size_t col){
00147 v3(0) = m(row++, col);
00148 v3(1) = m(row++, col);
00149 v3(2) = m(row, col);
00150 }
00151
00152 };
00153
00154 #endif