00001
00002
00003
00004
00005
00006
00007
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);
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