fMatrix3.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  * The University of Tokyo
00008  */
00016 #ifndef __F_MATRIX3_H__
00017 #define __F_MATRIX3_H__
00018 
00019 #include "hrpModelExportDef.h"
00020 #include <dims_common.h>
00021 #include "fMatrix.h"
00022 
00023 class fVec3;
00024 
00029 class HRPBASE_EXPORT fMat33
00030 {
00031         friend class fVec3;
00032 public:
00034 
00037         fMat33() {
00038                 m11 = 1.0, m12 = 0.0, m13 = 0.0;
00039                 m21 = 0.0, m22 = 1.0, m23 = 0.0;
00040                 m31 = 0.0, m32 = 0.0, m33 = 1.0;
00041                 n_row = 3;
00042                 n_col = 3;
00043                 temp = 0.0;
00044         }
00046 
00049         fMat33(double* ini) {
00050                 m11 = ini[0], m12 = ini[1], m13 = ini[2];
00051                 m21 = ini[3], m22 = ini[4], m23 = ini[5];
00052                 m31 = ini[6], m32 = ini[7], m33 = ini[8];
00053                 n_row = 3;
00054                 n_col = 3;
00055                 temp = 0.0;
00056         }
00058 
00061         fMat33(double ini) {
00062                 m11 = m22 = m33 = ini;
00063                 m12 = m13 = 0.0;
00064                 m21 = m23 = 0.0;
00065                 m31 = m32 = 0.0;
00066                 n_row = 3;
00067                 n_col = 3;
00068                 temp = 0.0;
00069         }
00071 
00074         fMat33(double _m11, double _m12, double _m13,
00075                    double _m21, double _m22, double _m23,
00076                    double _m31, double _m32, double _m33) {
00077                 m11 = _m11;  m12 = _m12;  m13 = _m13;
00078                 m21 = _m21;  m22 = _m22;  m23 = _m23;
00079                 m31 = _m31;  m32 = _m32;  m33 = _m33;
00080                 n_row = 3;
00081                 n_col = 3;
00082                 temp = 0.0;
00083         }
00085 
00088         fMat33(double d1, double d2, double d3) {
00089                 m11 = d1;  m12 = 0.0; m13 = 0.0;
00090                 m21 = 0.0; m22 = d2;  m23 = 0.0;
00091                 m31 = 0.0; m32 = 0.0; m33 = d3;
00092                 n_row = 3;
00093                 n_col = 3;
00094                 temp = 0.0;
00095         }
00097 
00100         fMat33(const fMat33& ini) {
00101                 m11 = ini.m11;
00102                 m12 = ini.m12;
00103                 m13 = ini.m13;
00104                 m21 = ini.m21;
00105                 m22 = ini.m22;
00106                 m23 = ini.m23;
00107                 m31 = ini.m31;
00108                 m32 = ini.m32;
00109                 m33 = ini.m33;
00110                 n_row = 3;
00111                 n_col = 3;
00112                 temp = 0.0;
00113         }
00114 
00116         ~fMat33() {
00117         }
00118 
00120         double& operator () (int i, int j);
00122         double operator () (int i, int j) const;
00123 
00125         double operator[] (int i) const {
00126                 return *(&m11 + i);
00127         }
00129         fMat33 operator = (const fMat33& mat);
00131         void operator = (double d);
00132 
00136         friend fMat33 operator - (const fMat33& mat);
00137         void operator += (const fMat33& mat);
00138         void operator -= (const fMat33& mat);
00139         void operator *= (double d);
00140         void operator /= (double d);
00141 
00142         friend fMat33 operator + (const fMat33& mat1, const fMat33& mat2);
00143         friend fMat33 operator - (const fMat33& mat1, const fMat33& mat2);
00144         friend fMat33 operator * (double d, const fMat33& mat);
00145         friend fMat33 operator * (const fMat33& m1, const fMat33& m2);
00146         friend fVec3 operator * (const fMat33& m, const fVec3& v);
00147         friend fMat33 operator * (const fMat33& mat, double d);
00148         friend fMat33 operator / (const fMat33& mat, double d);
00149 
00151         friend HRPBASE_EXPORT ostream& operator << (ostream& ost, const fMat33& mat);
00152 
00154         operator double *() {
00155                 return &m11;
00156         }
00158         double* data() {
00159                 return &m11;
00160         }
00162         friend fMat33 tran(const fMat33& m);
00164         void tran(const fMat33&);
00166         void cross(const fVec3& p);        
00167         void diag(double, double, double); 
00168         void identity();                   
00169         void zero();                       
00170 
00171         void set(const fMat33& mat);
00173         void set(const class fEulerPara& ep);
00175         void neg(const fMat33& mat);
00176         void add(const fMat33& mat1, const fMat33& mat2);
00177         void add(const fMat33& mat);
00178         void sub(const fMat33& mat1, const fMat33& mat2);
00179         void mul(const fMat33& mat1, const fMat33& mat2);
00180         void mul(double d, const fMat33& mat);
00181         void mul(const fMat33& mat, double d);
00182         void div(const fMat33& mat, double d);
00183         void mul(const fVec3& v1, const fVec3& v2);  // v1 * v2^T
00185         void rot2mat(const fVec3&, double);
00186         void mat2rot(fVec3&, double&) const;
00187 
00189         void ea2mat_xyz(const fVec3& ea);  
00190         void ea2mat_xzy(const fVec3& ea);  
00191         void ea2mat_zyx(const fVec3& ea);  
00192         void ea2mat_yzx(const fVec3& ea);  
00193 
00194 protected:
00195         double m11, m21, m31;
00196         double m12, m22, m32;
00197         double m13, m23, m33;
00198         double temp;
00199         int n_row, n_col;
00200 };
00201 
00206 class HRPBASE_EXPORT fVec3
00207 {
00208         friend class fMat33;
00209 public:
00210         fVec3() {
00211                 m1 = m2 = m3 = 0.0;
00212                 n_row = 3;
00213                 n_col = 1;
00214                 temp = 0.0;
00215         }
00216         fVec3(double* ini) {
00217                 m1 = ini[0];
00218                 m2 = ini[1];
00219                 m3 = ini[2];
00220                 n_row = 3;
00221                 n_col = 1;
00222                 temp = 0.0;
00223         }
00224         fVec3(double ini) {
00225                 m1 = m2 = m3 = ini;
00226                 n_row = 3;
00227                 n_col = 1;
00228                 temp = 0.0;
00229         }
00230         fVec3(const fVec3& ini) {
00231                 m1 = ini.m1;
00232                 m2 = ini.m2;
00233                 m3 = ini.m3;
00234                 n_row = 3;
00235                 n_col = 1;
00236                 temp = 0.0;
00237         }
00238         fVec3(double _m1, double _m2, double _m3) {
00239                 m1 = _m1;
00240                 m2 = _m2;
00241                 m3 = _m3;
00242                 n_row = 3;
00243                 n_col = 1;
00244                 temp = 0.0;
00245         }
00246         ~fVec3() {
00247         }
00248 
00250         double& operator () (int i);
00251         double operator () (int i) const;
00252         double operator [] (int i) const {
00253                 return *(&m1 + i);
00254         }
00256         void operator = (double d);
00257         fVec3 operator = (const fVec3& vec);
00259         double* data() {
00260                 return &m1;
00261         }
00262         operator double *() {
00263                 return &m1;
00264         }
00266         friend fVec3 operator - (const fVec3& vec);
00267         void operator += (const fVec3& vec);
00268         void operator -= (const fVec3& vec);
00269         void operator *= (double d);
00270         void operator /= (double d);
00271         friend fVec3 operator + (const fVec3& vec1, const fVec3& vec2);
00272         friend fVec3 operator - (const fVec3& vec1, const fVec3& vec2);
00273         friend fVec3 operator / (const fVec3& vec, double d);
00274         friend fVec3 operator * (double d, const fVec3& vec1);
00275         friend fVec3 operator * (const fVec3& vec1, double d);
00276         friend double operator * (const fVec3& vec1, const fVec3& vec2);
00278         void cross(const fVec3& vec1, const fVec3& vec2);
00279         friend fVec3 operator & (const fVec3& vec1, const fVec3& vec2);
00281         friend HRPBASE_EXPORT ostream& operator << (ostream& ost, const fVec3& mat);
00283         void zero() {
00284                 m1 = m2 = m3 = 0.0;
00285         }
00287         friend double dist(const fVec3& p1, const fVec3& p2) {
00288                 fVec3 pp;
00289                 pp.sub(p2, p1);
00290                 return pp.length();
00291         }
00293         friend double length(const fVec3& v) {
00294                 return sqrt(v*v);
00295         }
00296         double length() const {
00297                 return sqrt((*this) * (*this));
00298         }
00300         friend fVec3 unit(const fVec3& v) {
00301                 fVec3 ret;
00302                 double len = v.length();
00303                 if(len > TINY) ret = v / len;
00304                 else ret.zero();
00305                 return ret;
00306         }
00308         void unit() {
00309                 double len = length();
00310                 if(len > TINY) (*this) /= len;
00311                 else zero();
00312         }
00314         void set(double* v){
00315                 m1=v[0];
00316                 m2=v[1];
00317                 m3=v[2];
00318         }
00319         void set(double d1, double d2, double d3) {
00320                 m1 = d1;
00321                 m2 = d2;
00322                 m3 = d3;
00323         }
00325         void set(const fVec3& vec);
00326         void neg(const fVec3& vec);
00327         void add(const fVec3& vec1, const fVec3& vec2);
00328         void add(const fVec3& vec);
00329         void sub(const fVec3& vec1, const fVec3& vec2);
00330         void div(const fVec3& vec, double d);
00331         void mul(const fVec3& vec, double d);
00332         void mul(double d, const fVec3& vec);
00333         void mul(const fMat33& mat, const fVec3& vec);
00335         void mul(const fVec3& vec, const fMat33& mat);
00336 
00338         void rotation(const fMat33& ref, const fMat33& tgt);
00339 
00341         void mat2ea_xyz(const fMat33& mat);
00342         void mat2ea_xyz(const fMat33& mat, const fVec3& ea_ref);
00343         void mat2ea_xzy(const fMat33& mat);
00344         void mat2ea_zyx(const fMat33& mat);
00345         void mat2ea_yzx(const fMat33& mat);
00346 
00348 
00353         void epdot2angvel(const fEulerPara& epara, const fEulerPara& edot);
00354 
00356 
00362         void epddot2angacc(const fEulerPara& _e, const fEulerPara& _de, const fEulerPara& _dde);
00363 protected:
00364         double m1, m2, m3;
00365         double temp;
00366         int n_row, n_col;
00367 };
00368 
00369 #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