rotationalinertia.cpp
Go to the documentation of this file.00001 // Copyright (C) 2009 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> 00002 00003 // Version: 1.0 00004 // Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> 00005 // Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> 00006 // URL: http://www.orocos.org/kdl 00007 00008 // This library is free software; you can redistribute it and/or 00009 // modify it under the terms of the GNU Lesser General Public 00010 // License as published by the Free Software Foundation; either 00011 // version 2.1 of the License, or (at your option) any later version. 00012 00013 // This library is distributed in the hope that it will be useful, 00014 // but WITHOUT ANY WARRANTY; without even the implied warranty of 00015 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 00016 // Lesser General Public License for more details. 00017 00018 // You should have received a copy of the GNU Lesser General Public 00019 // License along with this library; if not, write to the Free Software 00020 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 00021 00022 #include "rotationalinertia.hpp" 00023 #include <Eigen/Core> 00024 using namespace Eigen; 00025 00026 namespace KDL 00027 { 00028 RotationalInertia::RotationalInertia(double Ixx,double Iyy,double Izz,double Ixy,double Ixz,double Iyz) 00029 { 00030 data[0]=Ixx; 00031 data[1]=data[3]=Ixy; 00032 data[2]=data[6]=Ixz; 00033 data[4]=Iyy; 00034 data[5]=data[7]=Iyz; 00035 data[8]=Izz; 00036 00037 } 00038 00039 RotationalInertia::~RotationalInertia() 00040 { 00041 } 00042 00043 Vector RotationalInertia::operator*(const Vector& omega) const { 00044 // Complexity : 9M+6A 00045 Vector result; 00046 Map<Vector3d>(result.data)=Map<const Matrix3d>(this->data)*Map<const Vector3d>(omega.data); 00047 return result; 00048 } 00049 00050 RotationalInertia operator*(double a, const RotationalInertia& I){ 00051 RotationalInertia result; 00052 Map<Matrix3d>(result.data)=a*Map<const Matrix3d>(I.data); 00053 return result; 00054 } 00055 00056 RotationalInertia operator+(const RotationalInertia& Ia, const RotationalInertia& Ib){ 00057 RotationalInertia result; 00058 Map<Matrix3d>(result.data)=Map<const Matrix3d>(Ia.data)+Map<const Matrix3d>(Ib.data); 00059 return result; 00060 } 00061 } 00062