fmatvec  0.0.0
rotation_matrix.h
1#ifndef _FMATVEC_ROTATION_MATRIX_H_
2#define _FMATVEC_ROTATION_MATRIX_H_
3
4#include <fmatvec/fmatvec.h>
5
6namespace fmatvec {
7
8using std::sin;
9using std::cos;
10using std::asin;
11using std::atan2;
12using std::min;
13using std::max;
14
15template<class AT>
16Matrix<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
26template<class AT>
27Matrix<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
37template<class AT>
38Matrix<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
48template<class AT>
49Matrix<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
63template<class AT>
64Matrix<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
78template<class AT>
79Vector<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
94template<class AT>
95Vector<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
fmatvec
Namespace fmatvec.
Definition: _memory.cc:28