Eigen3d.h
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
00003  * All rights reserved. This program is made available under the terms of the
00004  * Eclipse Public License v1.0 which accompanies this distribution, and is
00005  * available at http://www.eclipse.org/legal/epl-v10.html
00006  * Contributors:
00007  * National Institute of Advanced Industrial Science and Technology (AIST)
00008  * General Robotix Inc. 
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


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:53