fmatvec  0.0.0
 All Classes Namespaces Functions Variables Typedefs Enumerations Friends Pages
rotation_matrix.h
1 #ifndef _FMATVEC_ROTATION_MATRIX_H_
2 #define _FMATVEC_ROTATION_MATRIX_H_
3 
4 #include <fmatvec/fmatvec.h>
5 
6 namespace fmatvec {
7 
8 using std::sin;
9 using std::cos;
10 using std::asin;
11 using std::atan2;
12 using std::min;
13 using std::max;
14 
15 template<class AT>
16 Matrix<Rotation, Fixed<3>, Fixed<3>, AT> al2T(const AT& al) {
17  Matrix<Rotation, Fixed<3>, Fixed<3>, AT> T;
18  AT sa = sin(al);
19  AT ca = cos(al);
20  T(0,0)= 1.0; T(0,1)= 0.0; T(0,2)= 0.0;
21  T(1,0)= 0.0; T(1,1)= ca ; T(1,2)= -sa;
22  T(2,0)= 0.0; T(2,1)= sa ; T(2,2)= ca ;
23  return T;
24 }
25 
26 template<class AT>
27 Matrix<Rotation, Fixed<3>, Fixed<3>, AT> be2T(const AT& be) {
28  Matrix<Rotation, Fixed<3>, Fixed<3>, AT> T;
29  AT sb = sin(be);
30  AT cb = cos(be);
31  T(0,0)= cb ; T(0,1)= 0.0; T(0,2)= sb ;
32  T(1,0)= 0.0; T(1,1)= 1.0; T(1,2)= 0.0;
33  T(2,0)= -sb; T(2,1)= 0.0; T(2,2)= cb ;
34  return T;
35 }
36 
37 template<class AT>
38 Matrix<Rotation, Fixed<3>, Fixed<3>, AT> ga2T(const AT& ga) {
39  Matrix<Rotation, Fixed<3>, Fixed<3>, AT> T;
40  AT sg = sin(ga);
41  AT cg = cos(ga);
42  T(0,0)= cg ; T(0,1)= -sg; T(0,2)= 0.0;
43  T(1,0)= sg ; T(1,1)= cg ; T(1,2)= 0.0;
44  T(2,0)= 0.0; T(2,1)= 0.0; T(2,2)= 1.0;
45  return T;
46 }
47 
48 template<class AT>
49 Matrix<Rotation, Fixed<3>, Fixed<3>, AT> albega2T(const Vector<Fixed<3>, AT> &albega) {
50  Matrix<Rotation, Fixed<3>, Fixed<3>, AT> T;
51  AT sa = sin(albega(0));
52  AT sb = sin(albega(1));
53  AT sg = sin(albega(2));
54  AT ca = cos(albega(0));
55  AT cb = cos(albega(1));
56  AT cg = cos(albega(2));
57  T(0,0) = cb*cg ; T(0,1) = -sg*cb ; T(0,2) = sb ;
58  T(1,0) = sa*sb*cg + sg*ca; T(1,1) = -sa*sb*sg + ca*cg; T(1,2) = -sa*cb;
59  T(2,0) = sa*sg - sb*ca*cg; T(2,1) = sa*cg + sb*sg*ca ; T(2,2) = ca*cb ;
60  return T;
61 }
62 
63 template<class AT>
64 Matrix<Rotation, Fixed<3>, Fixed<3>, AT> begaal2T(const Vector<Fixed<3>, AT> &begaal) {
65  Matrix<Rotation, Fixed<3>, Fixed<3>, AT> T;
66  AT sb = sin(begaal(0));
67  AT sg = sin(begaal(1));
68  AT sa = sin(begaal(2));
69  AT cb = cos(begaal(0));
70  AT cg = cos(begaal(1));
71  AT ca = cos(begaal(2));
72  T(0,0) = cb*cg ; T(0,1) = sa*sb - sg*ca*cb; T(0,2) = sa*sg*cb + sb*ca ;
73  T(1,0) = sg ; T(1,1) = ca*cg ; T(1,2) = -sa*cg ;
74  T(2,0) = -sb*cg; T(2,1) = sa*cb + sb*sg*ca; T(2,2) = -sa*sb*sg + ca*cb;
75  return T;
76 }
77 
78 template<class AT>
79 Vector<Fixed<3>, AT> T2albega(const Matrix<Rotation, Fixed<3>, Fixed<3>, AT> &T) {
80  Vector<Fixed<3>, AT> albega(NONINIT);
81  assert(T(0,2)>=-1-1e-12 && T(0,2)<1+1e-12);
82  albega(1)= asin(max(min(T(0,2), 1.0), -1.0));
83  double nenner = cos(albega(1));
84  if (fabs(nenner)>1e-10) {
85  albega(0) = atan2(-T(1,2),T(2,2));
86  albega(2) = atan2(-T(0,1),T(0,0));
87  } else {
88  albega(0)=0;
89  albega(2)=atan2(T(1,0),T(1,1));
90  }
91  return albega;
92 }
93 
94 template<class AT>
95 Vector<Fixed<3>, AT> T2begaal(const Matrix<Rotation, Fixed<3>, Fixed<3>, AT> &T) {
96  Vector<Fixed<3>, AT> begaal(NONINIT);
97  assert(T(1,0)>=-1-1e-12 && T(1,0)<1+1e-12);
98  begaal(1)= asin(max(min(T(1,0), 1.0), -1.0));
99  double nenner = cos(begaal(1));
100  if (fabs(nenner)>1e-10) {
101  begaal(0) = atan2(-T(2,0),T(0,0));
102  begaal(2) = atan2(-T(1,2),T(1,1));
103  } else {
104  begaal(0)=0;
105  begaal(2)=atan2(T(2,1),T(2,2));
106  }
107  return begaal;
108 }
109 
110 }
111 
112 #endif