Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022 #include "jntspaceinertiamatrix.hpp"
00023
00024 namespace KDL
00025 {
00026 using namespace Eigen;
00027
00028 JntSpaceInertiaMatrix::JntSpaceInertiaMatrix()
00029 {
00030 }
00031
00032 JntSpaceInertiaMatrix::JntSpaceInertiaMatrix(int _size):
00033 data(_size, _size)
00034 {
00035 data.setZero();
00036 }
00037
00038
00039 JntSpaceInertiaMatrix::JntSpaceInertiaMatrix(const JntSpaceInertiaMatrix& arg):
00040 data(arg.data)
00041 {
00042 }
00043
00044 JntSpaceInertiaMatrix& JntSpaceInertiaMatrix::operator = (const JntSpaceInertiaMatrix& arg)
00045 {
00046 data=arg.data;
00047 return *this;
00048 }
00049
00050
00051 JntSpaceInertiaMatrix::~JntSpaceInertiaMatrix()
00052 {
00053 }
00054
00055 void JntSpaceInertiaMatrix::resize(unsigned int newSize)
00056 {
00057 data.resize(newSize,newSize);
00058 }
00059
00060 double JntSpaceInertiaMatrix::operator()(unsigned int i,unsigned int j)const
00061 {
00062 return data(i, j);
00063 }
00064
00065 double& JntSpaceInertiaMatrix::operator()(unsigned int i,unsigned int j)
00066 {
00067 return data(i, j);
00068 }
00069
00070 unsigned int JntSpaceInertiaMatrix::rows()const
00071 {
00072 return data.rows();
00073 }
00074
00075 unsigned int JntSpaceInertiaMatrix::columns()const
00076 {
00077 return data.cols();
00078 }
00079
00080
00081 void Add(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2,JntSpaceInertiaMatrix& dest)
00082 {
00083 dest.data=src1.data+src2.data;
00084 }
00085
00086 void Subtract(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2,JntSpaceInertiaMatrix& dest)
00087 {
00088 dest.data=src1.data-src2.data;
00089 }
00090
00091 void Multiply(const JntSpaceInertiaMatrix& src,const double& factor,JntSpaceInertiaMatrix& dest)
00092 {
00093 dest.data=factor*src.data;
00094 }
00095
00096 void Divide(const JntSpaceInertiaMatrix& src,const double& factor,JntSpaceInertiaMatrix& dest)
00097 {
00098 dest.data=src.data/factor;
00099 }
00100
00101 void Multiply(const JntSpaceInertiaMatrix& src, const JntArray& vec, JntArray& dest)
00102 {
00103 dest.data=src.data.lazyProduct(vec.data);
00104 }
00105
00106 void SetToZero(JntSpaceInertiaMatrix& mat)
00107 {
00108 mat.data.setZero();
00109 }
00110
00111 bool Equal(const JntSpaceInertiaMatrix& src1, const JntSpaceInertiaMatrix& src2,double eps)
00112 {
00113 if(src1.rows()!=src2.rows()||src1.columns()!=src2.columns())
00114 return false;
00115 return src1.data.isApprox(src2.data,eps);
00116 }
00117
00118 bool operator==(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2){return Equal(src1,src2);};
00119
00120
00121 }
00122
00123