fMatrix3.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
3  * All rights reserved. This program is made available under the terms of the
4  * Eclipse Public License v1.0 which accompanies this distribution, and is
5  * available at http://www.eclipse.org/legal/epl-v10.html
6  * Contributors:
7  * The University of Tokyo
8  */
16 #ifndef __F_MATRIX3_H__
17 #define __F_MATRIX3_H__
18 
19 #include "hrpModelExportDef.h"
20 #include <dims_common.h>
21 #include "fMatrix.h"
22 
23 class fVec3;
24 
30 {
31  friend class fVec3;
32 public:
34 
37  fMat33() {
38  m11 = 1.0, m12 = 0.0, m13 = 0.0;
39  m21 = 0.0, m22 = 1.0, m23 = 0.0;
40  m31 = 0.0, m32 = 0.0, m33 = 1.0;
41  n_row = 3;
42  n_col = 3;
43  temp = 0.0;
44  }
46 
49  fMat33(double* ini) {
50  m11 = ini[0], m12 = ini[1], m13 = ini[2];
51  m21 = ini[3], m22 = ini[4], m23 = ini[5];
52  m31 = ini[6], m32 = ini[7], m33 = ini[8];
53  n_row = 3;
54  n_col = 3;
55  temp = 0.0;
56  }
58 
61  fMat33(double ini) {
62  m11 = m22 = m33 = ini;
63  m12 = m13 = 0.0;
64  m21 = m23 = 0.0;
65  m31 = m32 = 0.0;
66  n_row = 3;
67  n_col = 3;
68  temp = 0.0;
69  }
71 
74  fMat33(double _m11, double _m12, double _m13,
75  double _m21, double _m22, double _m23,
76  double _m31, double _m32, double _m33) {
77  m11 = _m11; m12 = _m12; m13 = _m13;
78  m21 = _m21; m22 = _m22; m23 = _m23;
79  m31 = _m31; m32 = _m32; m33 = _m33;
80  n_row = 3;
81  n_col = 3;
82  temp = 0.0;
83  }
85 
88  fMat33(double d1, double d2, double d3) {
89  m11 = d1; m12 = 0.0; m13 = 0.0;
90  m21 = 0.0; m22 = d2; m23 = 0.0;
91  m31 = 0.0; m32 = 0.0; m33 = d3;
92  n_row = 3;
93  n_col = 3;
94  temp = 0.0;
95  }
97 
100  fMat33(const fMat33& ini) {
101  m11 = ini.m11;
102  m12 = ini.m12;
103  m13 = ini.m13;
104  m21 = ini.m21;
105  m22 = ini.m22;
106  m23 = ini.m23;
107  m31 = ini.m31;
108  m32 = ini.m32;
109  m33 = ini.m33;
110  n_row = 3;
111  n_col = 3;
112  temp = 0.0;
113  }
114 
117  }
118 
120  double& operator () (int i, int j);
122  double operator () (int i, int j) const;
123 
125  double operator[] (int i) const {
126  return *(&m11 + i);
127  }
129  fMat33 operator = (const fMat33& mat);
131  void operator = (double d);
132 
136  friend fMat33 operator - (const fMat33& mat);
137  void operator += (const fMat33& mat);
138  void operator -= (const fMat33& mat);
139  void operator *= (double d);
140  void operator /= (double d);
141 
142  friend fMat33 operator + (const fMat33& mat1, const fMat33& mat2);
143  friend fMat33 operator - (const fMat33& mat1, const fMat33& mat2);
144  friend fMat33 operator * (double d, const fMat33& mat);
145  friend fMat33 operator * (const fMat33& m1, const fMat33& m2);
146  friend fVec3 operator * (const fMat33& m, const fVec3& v);
147  friend fMat33 operator * (const fMat33& mat, double d);
148  friend fMat33 operator / (const fMat33& mat, double d);
149 
151  friend HRPBASE_EXPORT ostream& operator << (ostream& ost, const fMat33& mat);
152 
154  operator double *() {
155  return &m11;
156  }
158  double* data() {
159  return &m11;
160  }
162  friend fMat33 tran(const fMat33& m);
164  void tran(const fMat33&);
166  void cross(const fVec3& p);
167  void diag(double, double, double);
168  void identity();
169  void zero();
170  void set(const fMat33& mat);
173  void set(const class fEulerPara& ep);
175  void neg(const fMat33& mat);
176  void add(const fMat33& mat1, const fMat33& mat2);
177  void add(const fMat33& mat);
178  void sub(const fMat33& mat1, const fMat33& mat2);
179  void mul(const fMat33& mat1, const fMat33& mat2);
180  void mul(double d, const fMat33& mat);
181  void mul(const fMat33& mat, double d);
182  void div(const fMat33& mat, double d);
183  void mul(const fVec3& v1, const fVec3& v2); // v1 * v2^T
185  void rot2mat(const fVec3&, double);
186  void mat2rot(fVec3&, double&) const;
187 
189  void ea2mat_xyz(const fVec3& ea);
190  void ea2mat_xzy(const fVec3& ea);
191  void ea2mat_zyx(const fVec3& ea);
192  void ea2mat_yzx(const fVec3& ea);
193 
194 protected:
195  double m11, m21, m31;
196  double m12, m22, m32;
197  double m13, m23, m33;
198  double temp;
199  int n_row, n_col;
200 };
201 
207 {
208  friend class fMat33;
209 public:
210  fVec3() {
211  m1 = m2 = m3 = 0.0;
212  n_row = 3;
213  n_col = 1;
214  temp = 0.0;
215  }
216  fVec3(double* ini) {
217  m1 = ini[0];
218  m2 = ini[1];
219  m3 = ini[2];
220  n_row = 3;
221  n_col = 1;
222  temp = 0.0;
223  }
224  fVec3(double ini) {
225  m1 = m2 = m3 = ini;
226  n_row = 3;
227  n_col = 1;
228  temp = 0.0;
229  }
230  fVec3(const fVec3& ini) {
231  m1 = ini.m1;
232  m2 = ini.m2;
233  m3 = ini.m3;
234  n_row = 3;
235  n_col = 1;
236  temp = 0.0;
237  }
238  fVec3(double _m1, double _m2, double _m3) {
239  m1 = _m1;
240  m2 = _m2;
241  m3 = _m3;
242  n_row = 3;
243  n_col = 1;
244  temp = 0.0;
245  }
246  ~fVec3() {
247  }
248 
250  double& operator () (int i);
251  double operator () (int i) const;
252  double operator [] (int i) const {
253  return *(&m1 + i);
254  }
256  void operator = (double d);
257  fVec3 operator = (const fVec3& vec);
259  double* data() {
260  return &m1;
261  }
262  operator double *() {
263  return &m1;
264  }
266  friend fVec3 operator - (const fVec3& vec);
267  void operator += (const fVec3& vec);
268  void operator -= (const fVec3& vec);
269  void operator *= (double d);
270  void operator /= (double d);
271  friend fVec3 operator + (const fVec3& vec1, const fVec3& vec2);
272  friend fVec3 operator - (const fVec3& vec1, const fVec3& vec2);
273  friend fVec3 operator / (const fVec3& vec, double d);
274  friend fVec3 operator * (double d, const fVec3& vec1);
275  friend fVec3 operator * (const fVec3& vec1, double d);
276  friend double operator * (const fVec3& vec1, const fVec3& vec2);
278  void cross(const fVec3& vec1, const fVec3& vec2);
279  friend fVec3 operator & (const fVec3& vec1, const fVec3& vec2);
281  friend HRPBASE_EXPORT ostream& operator << (ostream& ost, const fVec3& mat);
283  void zero() {
284  m1 = m2 = m3 = 0.0;
285  }
287  friend double dist(const fVec3& p1, const fVec3& p2) {
288  fVec3 pp;
289  pp.sub(p2, p1);
290  return pp.length();
291  }
293  friend double length(const fVec3& v) {
294  return sqrt(v*v);
295  }
296  double length() const {
297  return sqrt((*this) * (*this));
298  }
300  friend fVec3 unit(const fVec3& v) {
301  fVec3 ret;
302  double len = v.length();
303  if(len > TINY) ret = v / len;
304  else ret.zero();
305  return ret;
306  }
308  void unit() {
309  double len = length();
310  if(len > TINY) (*this) /= len;
311  else zero();
312  }
314  void set(double* v){
315  m1=v[0];
316  m2=v[1];
317  m3=v[2];
318  }
319  void set(double d1, double d2, double d3) {
320  m1 = d1;
321  m2 = d2;
322  m3 = d3;
323  }
325  void set(const fVec3& vec);
326  void neg(const fVec3& vec);
327  void add(const fVec3& vec1, const fVec3& vec2);
328  void add(const fVec3& vec);
329  void sub(const fVec3& vec1, const fVec3& vec2);
330  void div(const fVec3& vec, double d);
331  void mul(const fVec3& vec, double d);
332  void mul(double d, const fVec3& vec);
333  void mul(const fMat33& mat, const fVec3& vec);
335  void mul(const fVec3& vec, const fMat33& mat);
336 
338  void rotation(const fMat33& ref, const fMat33& tgt);
339 
341  void mat2ea_xyz(const fMat33& mat);
342  void mat2ea_xyz(const fMat33& mat, const fVec3& ea_ref);
343  void mat2ea_xzy(const fMat33& mat);
344  void mat2ea_zyx(const fMat33& mat);
345  void mat2ea_yzx(const fMat33& mat);
346 
348 
353  void epdot2angvel(const fEulerPara& epara, const fEulerPara& edot);
354 
356 
362  void epddot2angacc(const fEulerPara& _e, const fEulerPara& _de, const fEulerPara& _dde);
363 protected:
364  double m1, m2, m3;
365  double temp;
366  int n_row, n_col;
367 };
368 
369 #endif
fVec3(double *ini)
Definition: fMatrix3.h:216
int n_col
Definition: fMatrix3.h:199
double operator[](int i) const
Definition: fMatrix3.h:252
void operator/=(double d)
Definition: fMatrix3.cpp:450
void operator-=(const fMat33 &mat)
Definition: fMatrix3.cpp:430
double & operator()(int i, int j)
The reference to the (i, j)-th element.
Definition: fMatrix3.cpp:517
3x3 matrix class.
Definition: fMatrix3.h:29
#define HRPBASE_EXPORT
friend fVec3 unit(const fVec3 &v)
Returns the unit vector with the same direction (with length check)
Definition: fMatrix3.h:300
void operator*=(double d)
Definition: fMatrix3.cpp:443
void div(const fMat33 &mat, double d)
Definition: fMatrix3.cpp:746
void mul(fVec4 &_vec, double d)
Definition: fMatrix4.cpp:162
fEulerPara operator*(double d, const fEulerPara &ep)
Definition: fEulerPara.cpp:261
void zero()
Creates a zero vector.
Definition: fMatrix3.h:283
void mul(const fMat33 &mat1, const fMat33 &mat2)
Definition: fMatrix3.cpp:694
void zero()
Definition: fMatrix3.cpp:236
void operator/=(double d)
Definition: fMatrix3.cpp:812
fVec3 epdot2angvel(const fEulerPara &_epara, const fEulerPara &_edot)
Definition: fEulerPara.cpp:278
void add(const fVec4 &vec1, const fVec4 &vec2)
Definition: fMatrix4.cpp:156
double * data()
Pointer to the first element.
Definition: fMatrix3.h:158
double length() const
Definition: fMatrix3.h:296
fVec3 epddot2angacc(const fEulerPara &_e, const fEulerPara &_de, const fEulerPara &_dde)
Definition: fEulerPara.cpp:287
RTC::ReturnCode_t ret(RTC::Local::ReturnCode_t r)
double m13
Definition: fMatrix3.h:197
int n_row
Definition: fMatrix3.h:199
double m11
Definition: fMatrix3.h:195
fVec3(double ini)
Definition: fMatrix3.h:224
fVec3()
Definition: fMatrix3.h:210
png_bytep png_bytep png_size_t length
Definition: png.h:1541
double m33
Definition: fMatrix3.h:197
png_uint_32 i
Definition: png.h:2735
fVec3(double _m1, double _m2, double _m3)
Definition: fMatrix3.h:238
double m32
Definition: fMatrix3.h:196
double & operator()(int i)
Access the i-th element.
Definition: fMatrix3.cpp:541
void operator*=(double d)
Definition: fMatrix3.cpp:805
#define TINY
Definition: dims_common.h:25
fMat33(double *ini)
Constructor with initial values as array.
Definition: fMatrix3.h:49
fMat operator+(const fMat &mat1, const fMat &mat2)
Definition: fMatrix.cpp:1187
void neg(const fMat33 &mat)
Functions for basic operations.
Definition: fMatrix3.cpp:648
Euler parameter class.
Definition: fEulerPara.h:28
int n_col
Definition: fMatrix3.h:366
double m23
Definition: fMatrix3.h:197
Generic matrix/vector classes.
double m3
Definition: fMatrix3.h:364
int n_row
Definition: fMatrix3.h:366
double m31
Definition: fMatrix3.h:195
void operator-=(const fVec3 &vec)
Definition: fMatrix3.cpp:798
def j(str, encoding="cp932")
void unit()
Converts to unit vector.
Definition: fMatrix3.h:308
double operator[](int i) const
Value of the i-th element in the array.
Definition: fMatrix3.h:125
double m2
Definition: fMatrix3.h:364
void operator=(double d)
Assignment operators.
Definition: fMatrix3.cpp:762
~fVec3()
Definition: fMatrix3.h:246
void sub(const fVec4 &vec1, const fVec4 &vec2)
Definition: fMatrix4.cpp:150
std::ostream & operator<<(std::ostream &ost, const Point &p)
Definition: DistFuncs.cpp:3
fMat33(double ini)
Constructor with the same initial diagonal value.
Definition: fMatrix3.h:61
void operator+=(const fMat33 &mat)
Definition: fMatrix3.cpp:417
~fMat33()
Destructor.
Definition: fMatrix3.h:116
fVec3(const fVec3 &ini)
Definition: fMatrix3.h:230
double temp
Definition: fMatrix3.h:198
friend double dist(const fVec3 &p1, const fVec3 &p2)
Returns the distance between two points.
Definition: fMatrix3.h:287
double m22
Definition: fMatrix3.h:196
Vector3 cross(const Vector3 &v1, const Vector3 &v2)
Definition: Tvmet2Eigen.h:15
fMat tran(const fMat &mat)
Definition: fMatrix.cpp:1013
void sub(const fMat33 &mat1, const fMat33 &mat2)
Definition: fMatrix3.cpp:681
fMat33()
Default constructor.
Definition: fMatrix3.h:37
double m21
Definition: fMatrix3.h:195
void sub(const fVec3 &vec1, const fVec3 &vec2)
Definition: fMatrix3.cpp:902
fMat33(double d1, double d2, double d3)
Constructor with diagonal values.
Definition: fMatrix3.h:88
fVec3 operator&(const fVec3 &vec1, const fVec3 &vec2)
Definition: fMatrix3.cpp:773
friend double length(const fVec3 &v)
Returns the length of a vector.
Definition: fMatrix3.h:293
double m1
Definition: fMatrix3.h:364
void add(const fMat33 &mat1, const fMat33 &mat2)
Definition: fMatrix3.cpp:655
fMat33 operator=(const fMat33 &mat)
Assignment operator.
Definition: fMatrix3.cpp:380
fMat operator/(const fMat &mat, double d)
Definition: fMatrix.cpp:1435
3-element vector class.
Definition: fMatrix3.h:206
double * data()
Pointer to the first element.
Definition: fMatrix3.h:259
void operator+=(const fVec3 &vec)
Definition: fMatrix3.cpp:791
double temp
Definition: fMatrix3.h:365
fMat33(double _m11, double _m12, double _m13, double _m21, double _m22, double _m23, double _m31, double _m32, double _m33)
Constructor with all values.
Definition: fMatrix3.h:74
fEulerPara operator-(const fEulerPara &_ep)
Definition: fEulerPara.cpp:310
double m12
Definition: fMatrix3.h:196
fMat33(const fMat33 &ini)
Copy constructor.
Definition: fMatrix3.h:100


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sat May 8 2021 02:42:37