EulerTransformationMatrices.hh
Go to the documentation of this file.
1 //------------------------------------------------------------------------
2 //
3 // Copyright (C) 2010 Manuel Wopfner
4 //
5 // wopfner@hs-ulm.de
6 //
7 // Christian Schlegel (schlegel@hs-ulm.de)
8 // University of Applied Sciences
9 // Prittwitzstr. 10
10 // 89075 Ulm (Germany)
11 //
12 // This file contains functions which creates the corresponding
13 // rotation matrixes for different euler angles
14 //
15 // This library is free software; you can redistribute it and/or
16 // modify it under the terms of the GNU Lesser General Public
17 // License as published by the Free Software Foundation; either
18 // version 2.1 of the License, or (at your option) any later version.
19 //
20 // This library is distributed in the hope that it will be useful,
21 // but WITHOUT ANY WARRANTY; without even the implied warranty of
22 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
23 // Lesser General Public License for more details.
24 //
25 // You should have received a copy of the GNU Lesser General Public
26 // License along with this library; if not, write to the Free Software
27 // Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
28 //--------------------------------------------------------------------------
29 
30 
31 #ifndef EULER_TANSFORMATION_MATRICES
32 #define EULER_TANSFORMATION_MATRICES
33 
34 #include <armadillo>
35 
36 class EulerTransformationMatrices {
37 public:
38 
44  static void create_zxz_matrix(double phi, double theta, double psi, arma::mat& matrix) {
45  double c_phi = cos(phi);
46  double s_phi = sin(phi);
47  double c_theta = cos(theta);
48  double s_theta = sin(theta);
49  double c_psi = cos(psi);
50  double s_psi = sin(psi);
51 
52  // If matrix is to small resize it
53  if (matrix.n_cols < 3 || matrix.n_rows < 3)
54  matrix.set_size(3, 3);
55 
56  matrix(0, 0) = c_phi * c_psi - s_phi * c_theta * s_psi;
57  matrix(0, 1) = -c_phi * s_psi - s_phi * c_theta * c_psi;
58  matrix(0, 2) = s_phi * s_theta;
59 
60  matrix(1, 0) = s_phi * c_psi + c_phi * c_theta * s_psi;
61  matrix(1, 1) = -s_phi * s_psi + c_phi * c_theta * c_psi;
62  matrix(1, 2) = -c_phi * s_theta;
63 
64  matrix(2, 0) = s_theta * s_psi;
65  matrix(2, 1) = s_theta * c_psi;
66  matrix(2, 2) = c_theta;
67 
68  }
69 
75  static void create_zxz_matrix(double x, double y, double z, double phi, double theta, double psi, arma::mat& matrix) {
76  double c_phi = cos(phi);
77  double s_phi = sin(phi);
78  double c_theta = cos(theta);
79  double s_theta = sin(theta);
80  double c_psi = cos(psi);
81  double s_psi = sin(psi);
82 
83  // If matrix is to small resize it
84  if (matrix.n_cols < 4 || matrix.n_rows < 4) {
85  matrix.set_size(4, 4);
86  matrix.zeros();
87  }
88 
89  matrix(0, 0) = c_phi * c_psi - s_phi * c_theta * s_psi;
90  matrix(0, 1) = -c_phi * s_psi - s_phi * c_theta * c_psi;
91  matrix(0, 2) = s_phi * s_theta;
92  matrix(0, 3) = x;
93 
94  matrix(1, 0) = s_phi * c_psi + c_phi * c_theta * s_psi;
95  matrix(1, 1) = -s_phi * s_psi + c_phi * c_theta * c_psi;
96  matrix(1, 2) = -c_phi * s_theta;
97  matrix(1, 3) = y;
98 
99  matrix(2, 0) = s_theta * s_psi;
100  matrix(2, 1) = s_theta * c_psi;
101  matrix(2, 2) = c_theta;
102  matrix(2, 3) = z;
103 
104  matrix(3, 3) = 1;
105 
106  }
107 
113  static void create_zyx_matrix(double phi, double theta, double psi, arma::mat& matrix) {
114  double c_phi = cos(phi);
115  double s_phi = sin(phi);
116  double c_theta = cos(theta);
117  double s_theta = sin(theta);
118  double c_psi = cos(psi);
119  double s_psi = sin(psi);
120 
121  // If matrix is to small resize it
122  if (matrix.n_cols < 3 || matrix.n_rows < 3)
123  matrix.set_size(3, 3);
124 
125  matrix(0, 0) = c_theta * c_phi;
126  matrix(0, 1) = -c_psi * s_phi + s_psi * s_theta * c_phi;
127  matrix(0, 2) = s_psi * s_phi + c_psi * s_theta * c_phi;
128 
129  matrix(1, 0) = c_theta * s_phi;
130  matrix(1, 1) = c_psi * c_phi + s_psi * s_theta * s_phi;
131  matrix(1, 2) = -s_psi * c_phi + c_psi * s_theta * s_phi;
132 
133  matrix(2, 0) = -s_theta;
134  matrix(2, 1) = s_psi * c_theta;
135  matrix(2, 2) = c_psi * c_theta;
136  }
137 
143  static void create_zyx_matrix(double x, double y, double z, double phi, double theta, double psi, arma::mat& matrix) {
144  double c_phi = cos(phi);
145  double s_phi = sin(phi);
146  double c_theta = cos(theta);
147  double s_theta = sin(theta);
148  double c_psi = cos(psi);
149  double s_psi = sin(psi);
150 
151  // If matrix is to small resize it
152  if (matrix.n_cols < 4 || matrix.n_rows < 4) {
153  matrix.set_size(4, 4);
154  matrix.zeros();
155  }
156  matrix(0, 0) = c_theta * c_phi;
157  matrix(0, 1) = -c_psi * s_phi + s_psi * s_theta * c_phi;
158  matrix(0, 2) = s_psi * s_phi + c_psi * s_theta * c_phi;
159  matrix(0, 3) = x;
160 
161  matrix(1, 0) = c_theta * s_phi;
162  matrix(1, 1) = c_psi * c_phi + s_psi * s_theta * s_phi;
163  matrix(1, 2) = -s_psi * c_phi + c_psi * s_theta * s_phi;
164  matrix(1, 3) = y;
165 
166  matrix(2, 0) = -s_theta;
167  matrix(2, 1) = s_psi * c_theta;
168  matrix(2, 2) = c_psi * c_theta;
169  matrix(2, 3) = z;
170 
171  matrix(3, 3) = 1;
172  }
173 
174  static void zxz_to_zyx_angles(const double in_a, const double in_e, const double in_r, double& out_a, double& out_e, double& out_r) {
175  // ZXZ rotation matrix
176  arma::mat r(3, 3);
177  EulerTransformationMatrices::create_zxz_matrix(in_a, in_e, in_r, r);
178 
179  // elevation is in the range [-pi/2, pi/2 ], so it is enough to calculate:
180  out_e = atan2(-r(2, 0), hypot(r(0, 0), r(1, 0)));
181 
182  // roll:
183  if (fabs(r(2, 1)) + fabs(r(2, 2)) < 10 * std::numeric_limits<double>::epsilon()) {
184  out_r = 0.0;
185  if (out_e > 0)
186  out_a = -atan2(-r(1, 2), r(0, 2));
187  else
188  out_a = atan2(-r(1, 2), -r(0, 2));
189  } else {
190  out_r = atan2(r(2, 1), r(2, 2));
191  out_a = atan2(r(1, 0), r(0, 0));
192  }
193  }
194 
195  static void zyx_to_zxz_angles(const double in_a, const double in_e, const double in_r, double& out_a, double& out_e, double& out_r) {
196  // ZYX rotation matrix
197  arma::mat r(3, 3);
198  EulerTransformationMatrices::create_zyx_matrix(in_a, in_e, in_r, r);
199 
200  out_e = acos(r(2, 2));
201 
202  if (fabs(sin(out_e)) > 1E-8) {
203  out_r = atan2(r(2, 0) / sin(out_e), r(2, 1) / sin(out_e));
204  out_a = -atan2(r(0, 2) / sin(out_e), r(1, 2) / sin(out_e)) + M_PI;
205  } else {
206  out_a = 0;
207  out_r = atan2(r(1, 0), r(1, 1)) + M_PI;
208  }
209  }
210 
211  static void zyx_from_matrix(const arma::mat& matrix, double& out_x, double& out_y, double& out_z, double& out_azimuth, double& out_elevation, double& out_roll) {
212  double azimuth, elevation, roll;
213 
214  // elevation is in the range [-pi/2, pi/2 ], so it is enough to calculate:
215  elevation = atan2(-matrix(2, 0), hypot(matrix(0, 0), matrix(1, 0)));
216 
217  // roll:
218  if ((fabs(matrix(2, 1)) + fabs(matrix(2, 2))) < 10 * std::numeric_limits<double>::epsilon()) {
219  roll = 0.0;
220  if (elevation > 0)
221  azimuth = -atan2(-matrix(1, 2), matrix(0, 2));
222  else
223  azimuth = atan2(-matrix(1, 2), -matrix(0, 2));
224  } else {
225  roll = atan2(matrix(2, 1), matrix(2, 2));
226  azimuth = atan2(matrix(1, 0), matrix(0, 0));
227  }
228 
229  out_x = matrix(0, 3);
230  out_y = matrix(1, 3);
231  out_z = matrix(2, 3);
232  out_azimuth = azimuth;
233  out_elevation = elevation;
234  out_roll = roll;
235  }
236 };
237 
238 #endif
EulerTransformationMatrices::create_zyx_matrix
static void create_zyx_matrix(double phi, double theta, double psi, arma::mat &matrix)
Definition: EulerTransformationMatrices.hh:113
EulerTransformationMatrices::create_zxz_matrix
static void create_zxz_matrix(double x, double y, double z, double phi, double theta, double psi, arma::mat &matrix)
Definition: EulerTransformationMatrices.hh:75
EulerTransformationMatrices::create_zyx_matrix
static void create_zyx_matrix(double x, double y, double z, double phi, double theta, double psi, arma::mat &matrix)
Definition: EulerTransformationMatrices.hh:143
EulerTransformationMatrices::zyx_to_zxz_angles
static void zyx_to_zxz_angles(const double in_a, const double in_e, const double in_r, double &out_a, double &out_e, double &out_r)
Definition: EulerTransformationMatrices.hh:195
EulerTransformationMatrices::zyx_from_matrix
static void zyx_from_matrix(const arma::mat &matrix, double &out_x, double &out_y, double &out_z, double &out_azimuth, double &out_elevation, double &out_roll)
Definition: EulerTransformationMatrices.hh:211
EulerTransformationMatrices::create_zxz_matrix
static void create_zxz_matrix(double phi, double theta, double psi, arma::mat &matrix)
Definition: EulerTransformationMatrices.hh:44
EulerTransformationMatrices::zxz_to_zyx_angles
static void zxz_to_zyx_angles(const double in_a, const double in_e, const double in_r, double &out_a, double &out_e, double &out_r)
Definition: EulerTransformationMatrices.hh:174
M_PI
#define M_PI

katana
Author(s): Martin Günther
autogenerated on Fri Jun 7 2019 22:06:58