Tvmet2Eigen.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_TVMET2EIGEN_H_INCLUDED
00012 #define HRPUTIL_TVMET2EIGEN_H_INCLUDED
00013 
00014 namespace hrp{
00015     inline Vector3 cross(const Vector3& v1, const Vector3& v2){
00016         return v1.cross(v2);
00017     }
00018     inline Matrix33 trans(const Matrix33& m) { return m.transpose(); }
00019     inline double dot(const Vector3& v1, const Vector3& v2) {
00020         return v1.dot(v2);
00021     }
00022     inline double norm2(const Vector3& v) { return v.norm(); }
00023     inline Vector3 nomralize(const Vector3& v) { return v.normalized(); } 
00024     inline Matrix33 VVt_prod(const Vector3& a, const Vector3& b){
00025         Matrix33 m;
00026         m << a(0) * b(0), a(0) * b(1), a(0) * b(2),
00027             a(1) * b(0), a(1) * b(1), a(1) * b(2),
00028             a(2) * b(0), a(2) * b(1), a(2) * b(2);
00029         return m;
00030     }
00031 
00032     inline void calcInverse(Matrix33& inv, const Matrix33& m){
00033         bool invertible;
00034         m.computeInverseWithCheck(inv, invertible);
00035         if(!invertible){
00036             throw std::string("Inverse matrix cannot be calculated.");
00037         }
00038     }
00039 
00040     inline Matrix33 inverse(const Matrix33& m){
00041         Matrix33 inv;
00042         calcInverse(inv, m);
00043         return inv;
00044     }
00045     
00046     HRP_UTIL_EXPORT inline Matrix33 inverse33(const Matrix33& m) { return inverse(m); }
00047 };
00048 
00049 #endif


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Apr 11 2019 03:30:19