fMatrix3.cpp
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 #include "fMatrix3.h"
17 #include "fEulerPara.h"
18 
19 /*
20  * Euler Parameters
21  */
22 // x-y-z (zyx Euler angles)
23 void fMat33::ea2mat_xyz(const fVec3& ea)
24 {
25  double ca = cos(ea.m1), sa = sin(ea.m1);
26  double cb = cos(ea.m2), sb = sin(ea.m2);
27  double cc = cos(ea.m3), sc = sin(ea.m3);
28  m11 = cb*cc;
29  m12 = sa*sb*cc - ca*sc;
30  m13 = ca*sb*cc + sa*sc;
31  m21 = cb*sc;
32  m22 = sa*sb*sc + ca*cc;
33  m23 = ca*sb*sc - sa*cc;
34  m31 = -sb;
35  m32 = sa*cb;
36  m33 = ca*cb;
37 }
38 
39 void fVec3::mat2ea_xyz(const fMat33& mat)
40 {
41  double cb = sqrt(mat.m32*mat.m32 + mat.m33*mat.m33);
42  m2 = atan2(-mat.m31, cb);
43  if(cb < TINY)
44  {
45  m1 = atan2(mat.m12, mat.m22);
46  m3 = 0.0;
47  }
48  else
49  {
50  m1 = atan2(mat.m32/cb, mat.m33/cb);
51  m3 = atan2(mat.m21/cb, mat.m11/cb);
52  }
53 }
54 
55 // x-z-y (yzx Euler angles)
56 void fMat33::ea2mat_xzy(const fVec3& ea)
57 {
58  double c1 = cos(ea.m1), s1 = sin(ea.m1);
59  double c2 = cos(ea.m2), s2 = sin(ea.m2);
60  double c3 = cos(ea.m3), s3 = sin(ea.m3);
61  m11 = c2*c3;
62  m12 = s1*s3 - c1*s2*c3;
63  m13 = c1*s3 + s1*s2*c3;
64  m21 = s2;
65  m22 = c1*c2;
66  m23 = -s1*c2;
67  m31 = -c2*s3;
68  m32 = s1*c3 + c1*s2*s3;
69  m33 = c1*c3 - s1*s2*s3;
70 }
71 
72 void fVec3::mat2ea_xzy(const fMat33& mat)
73 {
74  double c2 = sqrt(mat.m22*mat.m22 + mat.m23*mat.m23);
75  m2 = atan2(mat.m21, c2);
76  if(c2 < TINY)
77  {
78  m1 = atan2(mat.m32, mat.m33);
79  m3 = 0.0;
80  }
81  else
82  {
83  m1 = atan2(-mat.m23, mat.m22);
84  m3 = atan2(-mat.m31, mat.m11);
85  }
86 }
87 
88 // z-y-x (xyz Euler angles)
89 void fMat33::ea2mat_zyx(const fVec3& ea)
90 {
91  double c1 = cos(ea.m1), s1 = sin(ea.m1);
92  double c2 = cos(ea.m2), s2 = sin(ea.m2);
93  double c3 = cos(ea.m3), s3 = sin(ea.m3);
94  m11 = c1*c2;
95  m12 = -s1*c2;
96  m13 = s2;
97  m21 = s1*c3 + c1*s2*s3;
98  m22 = c1*c3 - s1*s2*s3;
99  m23 = -c2*s3;
100  m31 = s1*s3 - c1*s2*c3;
101  m32 = c1*s3 + s1*s2*s3;
102  m33 = c2*c3;
103 }
104 
105 void fVec3::mat2ea_zyx(const fMat33& mat)
106 {
107  double c2 = sqrt(mat.m11*mat.m11 + mat.m12*mat.m12);
108  m2 = atan2(mat.m13, c2);
109  if(c2 < TINY)
110  {
111  m1 = atan2(mat.m21, mat.m22);
112  m3 = 0.0;
113  }
114  else
115  {
116  m1 = atan2(-mat.m12, mat.m11);
117  m3 = atan2(-mat.m23, mat.m33);
118  }
119 }
120 
121 // y-z-x (xzy Euler angles)
122 void fMat33::ea2mat_yzx(const fVec3& ea)
123 {
124  double c1 = cos(ea.m1), s1 = sin(ea.m1);
125  double c2 = cos(ea.m2), s2 = sin(ea.m2);
126  double c3 = cos(ea.m3), s3 = sin(ea.m3);
127  m11 = c1*c2;
128  m12 = -s2;
129  m13 = s1*c2;
130  m21 = s1*s3 + c1*s2*c3;
131  m22 = c2*c3;
132  m23 = -c1*s3 + s1*s2*c3;
133  m31 = -s1*c3 + c1*s2*s3;
134  m32 = c2*s3;
135  m33 = c1*c3 + s1*s2*s3;
136 }
137 
138 void fVec3::mat2ea_yzx(const fMat33& mat)
139 {
140  double c2 = sqrt(mat.m11*mat.m11 + mat.m13*mat.m13);
141  m2 = atan2(-mat.m12, c2);
142  if(c2 < TINY)
143  {
144  m1 = atan2(mat.m23, mat.m33);
145  m3 = 0.0;
146  }
147  else
148  {
149  m1 = atan2(mat.m13, mat.m11);
150  m3 = atan2(mat.m32, mat.m22);
151  }
152 }
153 
154 void fVec3::mat2ea_xyz(const fMat33& mat, const fVec3& ea_ref)
155 {
156  mat2ea_xyz(mat);
157  if(fabs(m1-ea_ref(0)) > PI)
158  {
159  if(m1 < -PI*0.5 || ea_ref(0) > PI*0.5)
160  {
161  m1 += 2.0*PI;
162  }
163  else if(m1 > PI*0.5 || ea_ref(0) < -PI*0.5)
164  {
165  m1 -= 2.0*PI;
166  }
167  }
168  if(fabs(m3-ea_ref(2)) > PI)
169  {
170 // cout << m3 << ", " << ea_ref(2) << endl;
171  if(m3 < -PI*0.5 || ea_ref(2) > PI*0.5)
172  {
173 // cout << " " << m3 << "->" << flush;
174  m3 += 2.0*PI;
175 // cout << m3 << endl;
176  }
177  else if(m3 > PI*0.5 || ea_ref(2) < -PI*0.5)
178  {
179 // cout << " " << m3 << "->" << flush;
180  m3 -= 2.0*PI;
181 // cout << m3 << endl;
182  }
183  }
184 }
185 
186 void fVec3::epdot2angvel(const fEulerPara& _epara, const fEulerPara& _edot)
187 {
188  static fVec3 temp;
189  fEulerPara epara(_epara), edot(_edot);
190  fMat33 mat(epara(3), epara(2), -epara(1), -epara(2), epara(3), epara(0), epara(1), -epara(0), epara(3));
191  mul(mat, edot.Vec());
192  temp.mul(-edot(3), epara.Vec());
193  add(temp);
194  (*this) *= 2.0;
195 // ret = (- edot(3)*epara.Vec() + mat*edot.Vec()) * 2;
196 }
197 
198 void fVec3::epddot2angacc(const fEulerPara& _e, const fEulerPara& _de, const fEulerPara& _dde)
199 {
200  fEulerPara e(_e), de(_de), dde(_dde);
201  fMat33 mat1(e(3), e(2), -e(1), -e(2), e(3), e(0), e(1), -e(0), e(3));
202  fMat33 mat2(de(3), de(2), -de(1), -de(2), de(3), de(0), de(1), -de(0), de(3));
203  static fVec3 tmp;
204  mul(mat2, de.Vec());
205  tmp.mul(mat1, dde.Vec());
206  add(tmp);
207  tmp.mul(-de(3), de.Vec());
208  add(tmp);
209  tmp.mul(-dde(3), e.Vec());
210  add(tmp);
211  (*this) *= 2.0;
212 }
213 
214 /*
215  * set special values
216  */
217 void fMat33::cross(const fVec3& p)
218 {
219  m11 = 0; m12 = -p.m3; m13 = p.m2;
220  m21 = p.m3; m22 = 0; m23 = -p.m1;
221  m31 = -p.m2; m32 = p.m1; m33 = 0;
222 }
223 
224 void fMat33::diag(double v1, double v2, double v3)
225 {
226  m11 = v1; m12 = 0; m13 = 0;
227  m21 = 0; m22 = v2; m23 = 0;
228  m31 = 0; m32 = 0; m33 = v3;
229 }
231 {
232  m11 = 1; m12 = 0; m13 = 0;
233  m21 = 0; m22 = 1; m23 = 0;
234  m31 = 0; m32 = 0; m33 = 1;
235 }
237 {
238  m11 = 0; m12 = 0; m13 = 0;
239  m21 = 0; m22 = 0; m23 = 0;
240  m31 = 0; m32 = 0; m33 = 0;
241 }
242 
243 /*
244  * equivalent rotation
245  */
246 inline double sgn(double x)
247 {
248  if(x < 0.0) return -1.0;
249  else if(x > 0.0) return 1.0;
250  else return 0.0;
251 }
252 
253 void fMat33::mat2rot(fVec3& axis, double& angle) const
254 {
255  double e = ((*this)(2,1) - (*this)(1,2)) * ((*this)(2,1) - (*this)(1,2)) +
256  ((*this)(0,2) - (*this)(2,0)) * ((*this)(0,2) - (*this)(2,0)) +
257  ((*this)(1,0) - (*this)(0,1)) * ((*this)(1,0) - (*this)(0,1));
258  double c = 0.5 * ((*this)(0,0) + (*this)(1,1) + (*this)(2,2) - 1.0);
259 
260  double v = 1.0 - c;
261  if(v < TINY)
262  {
263  angle = 0.0;
264  axis(0) = 0.0;
265  axis(1) = 0.0;
266  axis(2) = 1.0;
267  }
268  else
269  {
270  double s = 0.5 * sqrt(e);
271  angle = atan2(s, c);
272 
273  if(c > -0.7071)
274  {
275  // when angle < 3 * pi / 4
276  if(fabs(s) < TINY)
277  {
278  axis(0) = 0.0;
279  axis(1) = 0.0;
280  axis(2) = 1.0;
281  }
282  else
283  {
284  axis(0) = 0.5 * ((*this)(2,1) - (*this)(1,2)) / s;
285  axis(1) = 0.5 * ((*this)(0,2) - (*this)(2,0)) / s;
286  axis(2) = 0.5 * ((*this)(1,0) - (*this)(0,1)) / s;
287  }
288  }
289  else
290  {
291  // when 3* pi /4 < angle
292  double rx = (*this)(0,0) - c;
293  double ry = (*this)(1,1) - c;
294  double rz = (*this)(2,2) - c;
295 
296  if(rx > ry)
297  {
298  if(rx > rz)
299  {
300  // rx is largest
301  if (fabs(s) < TINY)
302  {
303  axis(0) = sqrt(rx / v);
304  }
305  else
306  {
307  axis(0) = sgn((*this)(2,1) - (*this)(1,2)) * sqrt(rx / v);
308  }
309  axis(1) = ((*this)(1,0) + (*this)(0,1)) / (2 * axis(0) * v);
310  axis(2) = ((*this)(0,2) + (*this)(2,0)) / (2 * axis(0) * v);
311  }
312  else
313  {
314  // rz is largest
315  if (fabs(s) < TINY)
316  {
317  axis(2) = sqrt(rz / v);
318  }
319  else
320  {
321  axis(2) = sgn((*this)(1,0) - (*this)(0,1)) * sqrt(rz / v);
322  }
323  axis(0) = ((*this)(0,2) + (*this)(2,0)) / (2 * axis(2) * v);
324  axis(1) = ((*this)(1,2) + (*this)(2,1)) / (2 * axis(2) * v);
325  }
326  }
327  else
328  {
329  if(ry > rz)
330  {
331  // ry is largest
332  if (fabs(s) < TINY)
333  {
334  axis(1) = sqrt(ry / v);
335  }
336  else
337  {
338  axis(1) = sgn((*this)(0,2) - (*this)(2,0)) * sqrt(ry / v);
339  }
340  axis(0) = ((*this)(0,1) + (*this)(1,0)) / (2 * axis(1) * v);
341  axis(2) = ((*this)(1,2) + (*this)(2,1)) / (2 * axis(1) * v);
342  }
343  else
344  {
345  // rz is largest
346  if (fabs(s) < TINY)
347  {
348  axis(2) = sqrt(rz / v);
349  }
350  else
351  {
352  axis(2) = sgn((*this)(1,0) - (*this)(0,1)) * sqrt(rz / v);
353  }
354  axis(0) = ((*this)(0,2) + (*this)(2,0)) / (2 * axis(2) * v);
355  axis(1) = ((*this)(1,2) + (*this)(2,1)) / (2 * axis(2) * v);
356  }
357  }
358  }
359  }
360 }
361 
362 void fMat33::rot2mat(const fVec3& axis, double angle)
363 {
364  double x = axis.m1, y = axis.m2, z = axis.m3;
365  double sa = sin(angle), ca = cos(angle);
366  (*this)(0,0) = ca + (1-ca)*x*x;
367  (*this)(0,1) = (1-ca)*x*y - sa*z;
368  (*this)(0,2) = (1-ca)*x*z + sa*y;
369  (*this)(1,0) = (1-ca)*y*x + sa*z;
370  (*this)(1,1) = ca + (1-ca)*y*y;
371  (*this)(1,2) = (1-ca)*y*z - sa*x;
372  (*this)(2,0) = (1-ca)*z*x - sa*y;
373  (*this)(2,1) = (1-ca)*z*y + sa*x;
374  (*this)(2,2) = ca + (1-ca)*z*z;
375 }
376 
377 /*
378  * operators (matrix)
379  */
381 {
382  m11 = mat.m11;
383  m12 = mat.m12;
384  m13 = mat.m13;
385  m21 = mat.m21;
386  m22 = mat.m22;
387  m23 = mat.m23;
388  m31 = mat.m31;
389  m32 = mat.m32;
390  m33 = mat.m33;
391  return *this;
392 }
393 
394 void fMat33::operator = (double d)
395 {
396  m11 = m22 = m33 = d;
397  m12 = m13 = 0.0;
398  m21 = m23 = 0.0;
399  m31 = m32 = 0.0;
400 }
401 
403 {
404  fMat33 ret;
405  ret.m11 = - mat.m11;
406  ret.m12 = - mat.m12;
407  ret.m13 = - mat.m13;
408  ret.m21 = - mat.m21;
409  ret.m22 = - mat.m22;
410  ret.m23 = - mat.m23;
411  ret.m31 = - mat.m31;
412  ret.m32 = - mat.m32;
413  ret.m33 = - mat.m33;
414  return ret;
415 }
416 
417 void fMat33::operator += (const fMat33& mat)
418 {
419  m11 += mat.m11;
420  m12 += mat.m12;
421  m13 += mat.m13;
422  m21 += mat.m21;
423  m22 += mat.m22;
424  m23 += mat.m23;
425  m31 += mat.m31;
426  m32 += mat.m32;
427  m33 += mat.m33;
428 }
429 
430 void fMat33::operator -= (const fMat33& mat)
431 {
432  m11 -= mat.m11;
433  m12 -= mat.m12;
434  m13 -= mat.m13;
435  m21 -= mat.m21;
436  m22 -= mat.m22;
437  m23 -= mat.m23;
438  m31 -= mat.m31;
439  m32 -= mat.m32;
440  m33 -= mat.m33;
441 }
442 
443 void fMat33::operator *= (double d)
444 {
445  m11 *= d; m12 *= d; m13 *= d;
446  m21 *= d; m22 *= d; m23 *= d;
447  m31 *= d; m32 *= d; m33 *= d;
448 }
449 
450 void fMat33::operator /= (double d)
451 {
452  m11 /= d; m12 /= d; m13 /= d;
453  m21 /= d; m22 /= d; m23 /= d;
454  m31 /= d; m32 /= d; m33 /= d;
455 }
456 
457 fMat33 operator + (const fMat33& mat1, const fMat33& mat2)
458 {
459  fMat33 ret;
460  ret.m11 = mat1.m11 + mat2.m11;
461  ret.m12 = mat1.m12 + mat2.m12;
462  ret.m13 = mat1.m13 + mat2.m13;
463  ret.m21 = mat1.m21 + mat2.m21;
464  ret.m22 = mat1.m22 + mat2.m22;
465  ret.m23 = mat1.m23 + mat2.m23;
466  ret.m31 = mat1.m31 + mat2.m31;
467  ret.m32 = mat1.m32 + mat2.m32;
468  ret.m33 = mat1.m33 + mat2.m33;
469  return ret;
470 }
471 
472 fMat33 operator - (const fMat33& mat1, const fMat33& mat2)
473 {
474  fMat33 ret;
475  ret.m11 = mat1.m11 - mat2.m11;
476  ret.m12 = mat1.m12 - mat2.m12;
477  ret.m13 = mat1.m13 - mat2.m13;
478  ret.m21 = mat1.m21 - mat2.m21;
479  ret.m22 = mat1.m22 - mat2.m22;
480  ret.m23 = mat1.m23 - mat2.m23;
481  ret.m31 = mat1.m31 - mat2.m31;
482  ret.m32 = mat1.m32 - mat2.m32;
483  ret.m33 = mat1.m33 - mat2.m33;
484  return ret;
485 }
486 
487 fMat33 operator * (double d, const fMat33& mat)
488 {
489  fMat33 ret;
490  ret.m11 = d * mat.m11;
491  ret.m12 = d * mat.m12;
492  ret.m13 = d * mat.m13;
493  ret.m21 = d * mat.m21;
494  ret.m22 = d * mat.m22;
495  ret.m23 = d * mat.m23;
496  ret.m31 = d * mat.m31;
497  ret.m32 = d * mat.m32;
498  ret.m33 = d * mat.m33;
499  return ret;
500 }
501 
502 fMat33 operator * (const fMat33& mat1, const fMat33& mat2)
503 {
504  fMat33 ret;
505  ret.m11 = mat1.m11*mat2.m11 + mat1.m12*mat2.m21 + mat1.m13*mat2.m31;
506  ret.m21 = mat1.m21*mat2.m11 + mat1.m22*mat2.m21 + mat1.m23*mat2.m31;
507  ret.m31 = mat1.m31*mat2.m11 + mat1.m32*mat2.m21 + mat1.m33*mat2.m31;
508  ret.m12 = mat1.m11*mat2.m12 + mat1.m12*mat2.m22 + mat1.m13*mat2.m32;
509  ret.m22 = mat1.m21*mat2.m12 + mat1.m22*mat2.m22 + mat1.m23*mat2.m32;
510  ret.m32 = mat1.m31*mat2.m12 + mat1.m32*mat2.m22 + mat1.m33*mat2.m32;
511  ret.m13 = mat1.m11*mat2.m13 + mat1.m12*mat2.m23 + mat1.m13*mat2.m33;
512  ret.m23 = mat1.m21*mat2.m13 + mat1.m22*mat2.m23 + mat1.m23*mat2.m33;
513  ret.m33 = mat1.m31*mat2.m13 + mat1.m32*mat2.m23 + mat1.m33*mat2.m33;
514  return ret;
515 }
516 
517 double& fMat33::operator () (int i, int j)
518 {
519 #ifndef NDEBUG
520  if(i<0 || i>=3 || j<0 || j>=3)
521  {
522  cerr << "matrix size error at operator ()" << endl;
523  return temp;
524  }
525 #endif
526  return *(&m11 + i + j*3);
527 }
528 
529 double fMat33::operator () (int i, int j) const
530 {
531 #ifndef NDEBUG
532  if(i<0 || i>=3 || j<0 || j>=3)
533  {
534  cerr << "matrix size error at operator ()" << endl;
535  return temp;
536  }
537 #endif
538  return *(&m11 + i + j*3);
539 }
540 
541 double& fVec3::operator () (int i)
542 {
543 #ifndef NDEBUG
544  if(i<0 || i>=3)
545  {
546  cerr << "vector size error at operator ()" << endl;
547  return temp;
548  }
549 #endif
550  return *(&m1 + i);
551 }
552 
553 double fVec3::operator () (int i) const
554 {
555 #ifndef NDEBUG
556  if(i<0 || i>=3)
557  {
558  cerr << "vector size error at operator ()" << endl;
559  return temp;
560  }
561 #endif
562  return *(&m1 + i);
563 }
564 
565 fVec3 operator * (const fMat33& m, const fVec3& v)
566 {
567  fVec3 ret;
568  ret.data()[0] = m.m11*v[0] + m.m12*v[1] + m.m13*v[2];
569  ret.data()[1] = m.m21*v[0] + m.m22*v[1] + m.m23*v[2];
570  ret.data()[2] = m.m31*v[0] + m.m32*v[1] + m.m33*v[2];
571  return ret;
572 }
573 
574 fMat33 operator * (const fMat33& mat, double d)
575 {
576  fMat33 ret;
577  ret.m11 = mat.m11 * d;
578  ret.m12 = mat.m12 * d;
579  ret.m13 = mat.m13 * d;
580  ret.m21 = mat.m21 * d;
581  ret.m22 = mat.m22 * d;
582  ret.m23 = mat.m23 * d;
583  ret.m31 = mat.m31 * d;
584  ret.m32 = mat.m32 * d;
585  ret.m33 = mat.m33 * d;
586  return ret;
587 }
588 
589 fMat33 operator / (const fMat33& mat, double d)
590 {
591  fMat33 ret;
592  ret.m11 = mat.m11 / d;
593  ret.m12 = mat.m12 / d;
594  ret.m13 = mat.m13 / d;
595  ret.m21 = mat.m21 / d;
596  ret.m22 = mat.m22 / d;
597  ret.m23 = mat.m23 / d;
598  ret.m31 = mat.m31 / d;
599  ret.m32 = mat.m32 / d;
600  ret.m33 = mat.m33 / d;
601  return ret;
602 }
603 
604 /*
605  * functions (matrix)
606  */
607 fMat33 tran(const fMat33& mat)
608 {
609  fMat33 ret;
610  ret.m11 = mat.m11; ret.m12 = mat.m21; ret.m13 = mat.m31;
611  ret.m21 = mat.m12; ret.m22 = mat.m22; ret.m23 = mat.m32;
612  ret.m31 = mat.m13; ret.m32 = mat.m23; ret.m33 = mat.m33;
613  return ret;
614 }
615 
616 void fMat33::tran(const fMat33& mat)
617 {
618  m11 = mat.m11; m12 = mat.m21; m13 = mat.m31;
619  m21 = mat.m12; m22 = mat.m22; m23 = mat.m32;
620  m31 = mat.m13; m32 = mat.m23; m33 = mat.m33;
621 }
622 
623 void fMat33::set(const fMat33& mat)
624 {
625  m11 = mat.m11; m12 = mat.m12; m13 = mat.m13;
626  m21 = mat.m21; m22 = mat.m22; m23 = mat.m23;
627  m31 = mat.m31; m32 = mat.m32; m33 = mat.m33;
628 }
629 
630 void fMat33::set(const fEulerPara& ep)
631 {
632  fEulerPara epara(ep);
633  double ex = epara(0), ey = epara(1), ez = epara(2), e = epara(3);
634  double ee = e * e, exx = ex * ex, eyy = ey * ey, ezz = ez * ez;
635  double exy = ex * ey, eyz = ey * ez, ezx = ez * ex;
636  double eex = e * ex, eey = e * ey, eez = e * ez;
637  m11 = exx - eyy - ezz + ee;
638  m22 = - exx + eyy - ezz + ee;
639  m33 = - exx - eyy + ezz + ee;
640  m12 = 2.0 * (exy - eez);
641  m21 = 2.0 * (exy + eez);
642  m13 = 2.0 * (ezx + eey);
643  m31 = 2.0 * (ezx - eey);
644  m23 = 2.0 * (eyz - eex);
645  m32 = 2.0 * (eyz + eex);
646 }
647 
648 void fMat33::neg(const fMat33& mat)
649 {
650  m11 = -mat.m11; m12 = -mat.m12; m13 = -mat.m13;
651  m21 = -mat.m21; m22 = -mat.m22; m23 = -mat.m23;
652  m31 = -mat.m31; m32 = -mat.m32; m33 = -mat.m33;
653 }
654 
655 void fMat33::add(const fMat33& mat1, const fMat33& mat2)
656 {
657  m11 = mat1.m11 + mat2.m11;
658  m12 = mat1.m12 + mat2.m12;
659  m13 = mat1.m13 + mat2.m13;
660  m21 = mat1.m21 + mat2.m21;
661  m22 = mat1.m22 + mat2.m22;
662  m23 = mat1.m23 + mat2.m23;
663  m31 = mat1.m31 + mat2.m31;
664  m32 = mat1.m32 + mat2.m32;
665  m33 = mat1.m33 + mat2.m33;
666 }
667 
668 void fMat33::add(const fMat33& mat)
669 {
670  m11 += mat.m11;
671  m12 += mat.m12;
672  m13 += mat.m13;
673  m21 += mat.m21;
674  m22 += mat.m22;
675  m23 += mat.m23;
676  m31 += mat.m31;
677  m32 += mat.m32;
678  m33 += mat.m33;
679 }
680 
681 void fMat33::sub(const fMat33& mat1, const fMat33& mat2)
682 {
683  m11 = mat1.m11 - mat2.m11;
684  m12 = mat1.m12 - mat2.m12;
685  m13 = mat1.m13 - mat2.m13;
686  m21 = mat1.m21 - mat2.m21;
687  m22 = mat1.m22 - mat2.m22;
688  m23 = mat1.m23 - mat2.m23;
689  m31 = mat1.m31 - mat2.m31;
690  m32 = mat1.m32 - mat2.m32;
691  m33 = mat1.m33 - mat2.m33;
692 }
693 
694 void fMat33::mul(const fMat33& mat1, const fMat33& mat2)
695 {
696  m11 = mat1.m11*mat2.m11 + mat1.m12*mat2.m21 + mat1.m13*mat2.m31;
697  m21 = mat1.m21*mat2.m11 + mat1.m22*mat2.m21 + mat1.m23*mat2.m31;
698  m31 = mat1.m31*mat2.m11 + mat1.m32*mat2.m21 + mat1.m33*mat2.m31;
699  m12 = mat1.m11*mat2.m12 + mat1.m12*mat2.m22 + mat1.m13*mat2.m32;
700  m22 = mat1.m21*mat2.m12 + mat1.m22*mat2.m22 + mat1.m23*mat2.m32;
701  m32 = mat1.m31*mat2.m12 + mat1.m32*mat2.m22 + mat1.m33*mat2.m32;
702  m13 = mat1.m11*mat2.m13 + mat1.m12*mat2.m23 + mat1.m13*mat2.m33;
703  m23 = mat1.m21*mat2.m13 + mat1.m22*mat2.m23 + mat1.m23*mat2.m33;
704  m33 = mat1.m31*mat2.m13 + mat1.m32*mat2.m23 + mat1.m33*mat2.m33;
705 }
706 
707 void fMat33::mul(double d, const fMat33& mat)
708 {
709  m11 = d * mat.m11;
710  m12 = d * mat.m12;
711  m13 = d * mat.m13;
712  m21 = d * mat.m21;
713  m22 = d * mat.m22;
714  m23 = d * mat.m23;
715  m31 = d * mat.m31;
716  m32 = d * mat.m32;
717  m33 = d * mat.m33;
718 }
719 
720 void fMat33::mul(const fMat33& mat, double d)
721 {
722  m11 = d * mat.m11;
723  m12 = d * mat.m12;
724  m13 = d * mat.m13;
725  m21 = d * mat.m21;
726  m22 = d * mat.m22;
727  m23 = d * mat.m23;
728  m31 = d * mat.m31;
729  m32 = d * mat.m32;
730  m33 = d * mat.m33;
731 }
732 
733 void fMat33::mul(const fVec3& v1, const fVec3& v2)
734 {
735  m11 = v1.m1 * v2.m1;
736  m12 = v1.m1 * v2.m2;
737  m13 = v1.m1 * v2.m3;
738  m21 = v1.m2 * v2.m1;
739  m22 = v1.m2 * v2.m2;
740  m23 = v1.m2 * v2.m3;
741  m31 = v1.m3 * v2.m1;
742  m32 = v1.m3 * v2.m2;
743  m33 = v1.m3 * v2.m3;
744 }
745 
746 void fMat33::div(const fMat33& mat, double d)
747 {
748  m11 = mat.m11 / d;
749  m12 = mat.m12 / d;
750  m13 = mat.m13 / d;
751  m21 = mat.m21 / d;
752  m22 = mat.m22 / d;
753  m23 = mat.m23 / d;
754  m31 = mat.m31 / d;
755  m32 = mat.m32 / d;
756  m33 = mat.m33 / d;
757 }
758 
759 /*
760  * operators (vector)
761  */
762 void fVec3::operator = (double d)
763 {
764  m1 = m2 = m3 = d;
765 }
766 
768 {
769  m1 = vec.m1; m2 = vec.m2; m3 = vec.m3;
770  return *this;
771 }
772 
773 fVec3 operator & (const fVec3& vec1, const fVec3& vec2)
774 {
775  fVec3 ret;
776  ret.m1 = vec1.m2*vec2.m3 - vec1.m3*vec2.m2;
777  ret.m2 = vec1.m3*vec2.m1 - vec1.m1*vec2.m3;
778  ret.m3 = vec1.m1*vec2.m2 - vec1.m2*vec2.m1;
779  return ret;
780 }
781 
783 {
784  fVec3 ret;
785  ret.m1 = -vec.m1;
786  ret.m2 = -vec.m2;
787  ret.m3 = -vec.m3;
788  return ret;
789 }
790 
791 void fVec3::operator += (const fVec3& vec)
792 {
793  m1 += vec.m1;
794  m2 += vec.m2;
795  m3 += vec.m3;
796 }
797 
798 void fVec3::operator -= (const fVec3& vec)
799 {
800  m1 -= vec.m1;
801  m2 -= vec.m2;
802  m3 -= vec.m3;
803 }
804 
805 void fVec3::operator *= (double d)
806 {
807  m1 *= d;
808  m2 *= d;
809  m3 *= d;
810 }
811 
812 void fVec3::operator /= (double d)
813 {
814  m1 /= d;
815  m2 /= d;
816  m3 /= d;
817 }
818 
819 fVec3 operator - (const fVec3& vec1, const fVec3& vec2)
820 {
821  fVec3 ret;
822  ret.m1 = vec1.m1 - vec2.m1;
823  ret.m2 = vec1.m2 - vec2.m2;
824  ret.m3 = vec1.m3 - vec2.m3;
825  return ret;
826 }
827 
828 fVec3 operator + (const fVec3& vec1, const fVec3& vec2)
829 {
830  fVec3 ret;
831  ret.m1 = vec1.m1 + vec2.m1;
832  ret.m2 = vec1.m2 + vec2.m2;
833  ret.m3 = vec1.m3 + vec2.m3;
834  return ret;
835 }
836 
837 fVec3 operator / (const fVec3& vec, double d)
838 {
839  fVec3 ret;
840  ret.m1 = vec.m1 / d;
841  ret.m2 = vec.m2 / d;
842  ret.m3 = vec.m3 / d;
843  return ret;
844 }
845 
846 double operator * (const fVec3& vec1, const fVec3& vec2)
847 {
848  double ret = 0;
849  ret = vec1.m1*vec2.m1 + vec1.m2*vec2.m2 + vec1.m3*vec2.m3;
850  return ret;
851 }
852 
853 fVec3 operator * (const fVec3& vec1, double d)
854 {
855  fVec3 ret;
856  ret.m1 = vec1.m1 * d;
857  ret.m2 = vec1.m2 * d;
858  ret.m3 = vec1.m3 * d;
859  return ret;
860 }
861 
862 fVec3 operator * (double d, const fVec3& vec1)
863 {
864  fVec3 ret;
865  ret.m1 = vec1.m1 * d;
866  ret.m2 = vec1.m2 * d;
867  ret.m3 = vec1.m3 * d;
868  return ret;
869 }
870 
871 /*
872  * functions (vector)
873  */
874 void fVec3::set(const fVec3& vec)
875 {
876  m1 = vec.m1;
877  m2 = vec.m2;
878  m3 = vec.m3;
879 }
880 
881 void fVec3::neg(const fVec3& vec)
882 {
883  m1 = -vec.m1;
884  m2 = -vec.m2;
885  m3 = -vec.m3;
886 }
887 
888 void fVec3::add(const fVec3& vec1, const fVec3& vec2)
889 {
890  m1 = vec1.m1 + vec2.m1;
891  m2 = vec1.m2 + vec2.m2;
892  m3 = vec1.m3 + vec2.m3;
893 }
894 
895 void fVec3::add(const fVec3& vec)
896 {
897  m1 += vec.m1;
898  m2 += vec.m2;
899  m3 += vec.m3;
900 }
901 
902 void fVec3::sub(const fVec3& vec1, const fVec3& vec2)
903 {
904  m1 = vec1.m1 - vec2.m1;
905  m2 = vec1.m2 - vec2.m2;
906  m3 = vec1.m3 - vec2.m3;
907 }
908 
909 void fVec3::div(const fVec3& vec, double d)
910 {
911  m1 = vec.m1 / d;
912  m2 = vec.m2 / d;
913  m3 = vec.m3 / d;
914 }
915 
916 void fVec3::mul(const fVec3& vec, double d)
917 {
918  m1 = vec.m1 * d;
919  m2 = vec.m2 * d;
920  m3 = vec.m3 * d;
921 }
922 
923 void fVec3::mul(double d, const fVec3& vec)
924 {
925  m1 = vec.m1 * d;
926  m2 = vec.m2 * d;
927  m3 = vec.m3 * d;
928 }
929 
930 void fVec3::mul(const fMat33& mat, const fVec3& vec)
931 {
932  m1 = mat.m11*vec.m1 + mat.m12*vec.m2 + mat.m13*vec.m3;
933  m2 = mat.m21*vec.m1 + mat.m22*vec.m2 + mat.m23*vec.m3;
934  m3 = mat.m31*vec.m1 + mat.m32*vec.m2 + mat.m33*vec.m3;
935 }
936 
937 void fVec3::mul(const fVec3& vec, const fMat33& mat)
938 {
939  m1 = vec.m1*mat.m11 + vec.m2*mat.m21 + vec.m3*mat.m31;
940  m2 = vec.m1*mat.m12 + vec.m2*mat.m22 + vec.m3*mat.m32;
941  m3 = vec.m1*mat.m13 + vec.m2*mat.m23 + vec.m3*mat.m33;
942 }
943 
944 void fVec3::cross(const fVec3& vec1, const fVec3& vec2)
945 {
946  m1 = vec1.m2*vec2.m3 - vec1.m3*vec2.m2;
947  m2 = vec1.m3*vec2.m1 - vec1.m1*vec2.m3;
948  m3 = vec1.m1*vec2.m2 - vec1.m2*vec2.m1;
949 }
950 
951 /*
952  * rotation from target to ref
953  */
954 void fVec3::rotation(const fMat33& ref, const fMat33& target)
955 {
956  static fMat33 tmp;
957  static fVec3 v;
958  tmp.mul(ref, tran(target));
959  v.m1 = tmp.m32 - tmp.m23;
960  v.m2 = tmp.m13 - tmp.m31;
961  v.m3 = tmp.m21 - tmp.m12;
962  (*this) = 0.5 * v;
963 }
964 
965 /*
966  * stream output
967  */
968 ostream& operator << (ostream& ost, const fVec3& v)
969 {
970  ost << "(" << v.m1 << ", " << v.m2 << ", " << v.m3 << ")" << flush;
971  return ost;
972 }
973 
974 ostream& operator << (ostream& ost, const fMat33& m)
975 {
976  ost << "(" << m.m11 << ", " << m.m12 << ", " << m.m13 << "," << endl
977  << " " << m.m21 << ", " << m.m22 << ", " << m.m23 << "," << endl
978  << " " << m.m31 << ", " << m.m32 << ", " << m.m33 << ")" << flush;
979  return ost;
980 }
void rotation(const fMat33 &ref, const fMat33 &tgt)
Computes the rotation from tgt to ref.
Definition: fMatrix3.cpp:954
void operator/=(double d)
Definition: fMatrix3.cpp:450
friend fMat33 operator-(const fMat33 &mat)
Definition: fMatrix3.cpp:402
void operator-=(const fMat33 &mat)
Definition: fMatrix3.cpp:430
void add(const fVec3 &vec1, const fVec3 &vec2)
Definition: fMatrix3.cpp:888
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
int c
Definition: autoplay.py:16
void ea2mat_xyz(const fVec3 &ea)
Converts from Euler angles.
Definition: fMatrix3.cpp:23
void mat2ea_xyz(const fMat33 &mat)
Orientation matrix to Euler angles.
Definition: fMatrix3.cpp:39
void epdot2angvel(const fEulerPara &epara, const fEulerPara &edot)
Computes the angular velocity from the velocity of Euler parameters.
Definition: fMatrix3.cpp:186
fVec3 operator&(const fVec3 &vec1, const fVec3 &vec2)
Definition: fMatrix3.cpp:773
png_voidp s1
Definition: png.h:2110
void ea2mat_yzx(const fVec3 &ea)
Euler angles to matrix.
Definition: fMatrix3.cpp:122
void operator*=(double d)
Definition: fMatrix3.cpp:443
* x
Definition: IceUtils.h:98
void div(const fMat33 &mat, double d)
Definition: fMatrix3.cpp:746
fVec3 & Vec()
Definition: fMatrix4.h:118
void set(const fMat33 &mat)
Copies a matrix.
Definition: fMatrix3.cpp:623
void mul(const fMat33 &mat1, const fMat33 &mat2)
Definition: fMatrix3.cpp:694
friend fMat33 tran(const fMat33 &m)
Returns the transpose.
Definition: fMatrix3.cpp:607
void zero()
Definition: fMatrix3.cpp:236
void operator/=(double d)
Definition: fMatrix3.cpp:812
void cross(const fVec3 &vec1, const fVec3 &vec2)
Cross product.
Definition: fMatrix3.cpp:944
RTC::ReturnCode_t ret(RTC::Local::ReturnCode_t r)
double m13
Definition: fMatrix3.h:197
double m11
Definition: fMatrix3.h:195
void diag(double, double, double)
Diagonal matrix.
Definition: fMatrix3.cpp:224
Euler parameter representation of orientation.
void identity()
Identity matrix.
Definition: fMatrix3.cpp:230
void set(double *v)
Set element values from array or three values.
Definition: fMatrix3.h:314
double m33
Definition: fMatrix3.h:197
png_uint_32 i
Definition: png.h:2735
friend fMat33 operator*(double d, const fMat33 &mat)
Definition: fMatrix3.cpp:487
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
void neg(const fMat33 &mat)
Functions for basic operations.
Definition: fMatrix3.cpp:648
Euler parameter class.
Definition: fEulerPara.h:28
void rot2mat(const fVec3 &, double)
Converts from/to equivalent rotation axis and angle.
Definition: fMatrix3.cpp:362
double m23
Definition: fMatrix3.h:197
void ea2mat_xzy(const fVec3 &ea)
Euler angles to matrix.
Definition: fMatrix3.cpp:56
double m3
Definition: fMatrix3.h:364
void mat2ea_yzx(const fMat33 &mat)
Definition: fMatrix3.cpp:138
friend fMat33 operator+(const fMat33 &mat1, const fMat33 &mat2)
Definition: fMatrix3.cpp:457
double m31
Definition: fMatrix3.h:195
void epddot2angacc(const fEulerPara &_e, const fEulerPara &_de, const fEulerPara &_dde)
Computes angular acceleration from the acceleration of Euler parameters.
Definition: fMatrix3.cpp:198
void operator-=(const fVec3 &vec)
Definition: fMatrix3.cpp:798
friend fMat33 operator/(const fMat33 &mat, double d)
Definition: fMatrix3.cpp:589
void div(const fVec3 &vec, double d)
Definition: fMatrix3.cpp:909
double m2
Definition: fMatrix3.h:364
void operator=(double d)
Assignment operators.
Definition: fMatrix3.cpp:762
void mat2ea_zyx(const fMat33 &mat)
Definition: fMatrix3.cpp:105
void cross(const fVec3 &p)
Sets spectial matrices.
Definition: fMatrix3.cpp:217
#define PI
PI.
Definition: IceTypes.h:32
void neg(const fVec3 &vec)
Definition: fMatrix3.cpp:881
void mat2ea_xzy(const fMat33 &mat)
Definition: fMatrix3.cpp:72
void operator+=(const fMat33 &mat)
Definition: fMatrix3.cpp:417
3x3 matrix and 3-element vector classes.
double temp
Definition: fMatrix3.h:198
double m22
Definition: fMatrix3.h:196
png_voidp png_voidp s2
Definition: png.h:2110
void sub(const fMat33 &mat1, const fMat33 &mat2)
Definition: fMatrix3.cpp:681
double m21
Definition: fMatrix3.h:195
void sub(const fVec3 &vec1, const fVec3 &vec2)
Definition: fMatrix3.cpp:902
void mat2rot(fVec3 &, double &) const
Definition: fMatrix3.cpp:253
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
3-element vector class.
Definition: fMatrix3.h:206
double sgn(double x)
Definition: fMatrix3.cpp:246
double * data()
Pointer to the first element.
Definition: fMatrix3.h:259
void mul(const fVec3 &vec, double d)
Definition: fMatrix3.cpp:916
void operator+=(const fVec3 &vec)
Definition: fMatrix3.cpp:791
void ea2mat_zyx(const fVec3 &ea)
Euler angles to matrix.
Definition: fMatrix3.cpp:89
friend HRPBASE_EXPORT ostream & operator<<(ostream &ost, const fMat33 &mat)
Outputs the elements to a stream.
Definition: fMatrix3.cpp:974
* y
Definition: IceUtils.h:97
double m12
Definition: fMatrix3.h:196


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Sep 8 2022 02:24:03