EulerTransformationMatrices.hh
Go to the documentation of this file.
00001 //------------------------------------------------------------------------
00002 //
00003 //  Copyright (C) 2010 Manuel Wopfner
00004 //
00005 //        wopfner@hs-ulm.de
00006 //
00007 //        Christian Schlegel (schlegel@hs-ulm.de)
00008 //        University of Applied Sciences
00009 //        Prittwitzstr. 10
00010 //        89075 Ulm (Germany)
00011 //
00012 //  This file contains functions which creates the corresponding
00013 //  rotation matrixes for different euler angles
00014 //
00015 //  This library is free software; you can redistribute it and/or
00016 //  modify it under the terms of the GNU Lesser General Public
00017 //  License as published by the Free Software Foundation; either
00018 //  version 2.1 of the License, or (at your option) any later version.
00019 //
00020 //  This library is distributed in the hope that it will be useful,
00021 //  but WITHOUT ANY WARRANTY; without even the implied warranty of
00022 //  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00023 //  Lesser General Public License for more details.
00024 //
00025 //  You should have received a copy of the GNU Lesser General Public
00026 //  License along with this library; if not, write to the Free Software
00027 //  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00028 //--------------------------------------------------------------------------
00029 
00030 
00031 #ifndef EULER_TANSFORMATION_MATRICES
00032 #define EULER_TANSFORMATION_MATRICES
00033 
00034 #include <armadillo>
00035 
00036 class EulerTransformationMatrices {
00037 public:
00038 
00044         static void create_zxz_matrix(double phi, double theta, double psi, arma::mat& matrix) {
00045                 double c_phi = cos(phi);
00046                 double s_phi = sin(phi);
00047                 double c_theta = cos(theta);
00048                 double s_theta = sin(theta);
00049                 double c_psi = cos(psi);
00050                 double s_psi = sin(psi);
00051 
00052                 // If matrix is to small resize it
00053                 if (matrix.n_cols < 3 || matrix.n_rows < 3)
00054                         matrix.set_size(3, 3);
00055 
00056                 matrix(0, 0) = c_phi * c_psi - s_phi * c_theta * s_psi;
00057                 matrix(0, 1) = -c_phi * s_psi - s_phi * c_theta * c_psi;
00058                 matrix(0, 2) = s_phi * s_theta;
00059 
00060                 matrix(1, 0) = s_phi * c_psi + c_phi * c_theta * s_psi;
00061                 matrix(1, 1) = -s_phi * s_psi + c_phi * c_theta * c_psi;
00062                 matrix(1, 2) = -c_phi * s_theta;
00063 
00064                 matrix(2, 0) = s_theta * s_psi;
00065                 matrix(2, 1) = s_theta * c_psi;
00066                 matrix(2, 2) = c_theta;
00067 
00068         }
00069 
00075         static void create_zxz_matrix(double x, double y, double z, double phi, double theta, double psi, arma::mat& matrix) {
00076                 double c_phi = cos(phi);
00077                 double s_phi = sin(phi);
00078                 double c_theta = cos(theta);
00079                 double s_theta = sin(theta);
00080                 double c_psi = cos(psi);
00081                 double s_psi = sin(psi);
00082 
00083                 // If matrix is to small resize it
00084                 if (matrix.n_cols < 4 || matrix.n_rows < 4) {
00085                         matrix.set_size(4, 4);
00086                         matrix.zeros();
00087                 }
00088 
00089                 matrix(0, 0) = c_phi * c_psi - s_phi * c_theta * s_psi;
00090                 matrix(0, 1) = -c_phi * s_psi - s_phi * c_theta * c_psi;
00091                 matrix(0, 2) = s_phi * s_theta;
00092                 matrix(0, 3) = x;
00093 
00094                 matrix(1, 0) = s_phi * c_psi + c_phi * c_theta * s_psi;
00095                 matrix(1, 1) = -s_phi * s_psi + c_phi * c_theta * c_psi;
00096                 matrix(1, 2) = -c_phi * s_theta;
00097                 matrix(1, 3) = y;
00098 
00099                 matrix(2, 0) = s_theta * s_psi;
00100                 matrix(2, 1) = s_theta * c_psi;
00101                 matrix(2, 2) = c_theta;
00102                 matrix(2, 3) = z;
00103 
00104                 matrix(3, 3) = 1;
00105 
00106         }
00107 
00113         static void create_zyx_matrix(double phi, double theta, double psi, arma::mat& matrix) {
00114                 double c_phi = cos(phi);
00115                 double s_phi = sin(phi);
00116                 double c_theta = cos(theta);
00117                 double s_theta = sin(theta);
00118                 double c_psi = cos(psi);
00119                 double s_psi = sin(psi);
00120 
00121                 // If matrix is to small resize it
00122                 if (matrix.n_cols < 3 || matrix.n_rows < 3)
00123                         matrix.set_size(3, 3);
00124 
00125                 matrix(0, 0) = c_theta * c_phi;
00126                 matrix(0, 1) = -c_psi * s_phi + s_psi * s_theta * c_phi;
00127                 matrix(0, 2) = s_psi * s_phi + c_psi * s_theta * c_phi;
00128 
00129                 matrix(1, 0) = c_theta * s_phi;
00130                 matrix(1, 1) = c_psi * c_phi + s_psi * s_theta * s_phi;
00131                 matrix(1, 2) = -s_psi * c_phi + c_psi * s_theta * s_phi;
00132 
00133                 matrix(2, 0) = -s_theta;
00134                 matrix(2, 1) = s_psi * c_theta;
00135                 matrix(2, 2) = c_psi * c_theta;
00136         }
00137 
00143         static void create_zyx_matrix(double x, double y, double z, double phi, double theta, double psi, arma::mat& matrix) {
00144                 double c_phi = cos(phi);
00145                 double s_phi = sin(phi);
00146                 double c_theta = cos(theta);
00147                 double s_theta = sin(theta);
00148                 double c_psi = cos(psi);
00149                 double s_psi = sin(psi);
00150 
00151                 // If matrix is to small resize it
00152                 if (matrix.n_cols < 4 || matrix.n_rows < 4) {
00153                         matrix.set_size(4, 4);
00154                         matrix.zeros();
00155                 }
00156                 matrix(0, 0) = c_theta * c_phi;
00157                 matrix(0, 1) = -c_psi * s_phi + s_psi * s_theta * c_phi;
00158                 matrix(0, 2) = s_psi * s_phi + c_psi * s_theta * c_phi;
00159                 matrix(0, 3) = x;
00160 
00161                 matrix(1, 0) = c_theta * s_phi;
00162                 matrix(1, 1) = c_psi * c_phi + s_psi * s_theta * s_phi;
00163                 matrix(1, 2) = -s_psi * c_phi + c_psi * s_theta * s_phi;
00164                 matrix(1, 3) = y;
00165 
00166                 matrix(2, 0) = -s_theta;
00167                 matrix(2, 1) = s_psi * c_theta;
00168                 matrix(2, 2) = c_psi * c_theta;
00169                 matrix(2, 3) = z;
00170 
00171                 matrix(3, 3) = 1;
00172         }
00173 
00174     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) {
00175         // ZXZ rotation matrix
00176                 arma::mat r(3, 3);
00177                 EulerTransformationMatrices::create_zxz_matrix(in_a, in_e, in_r, r);
00178 
00179                 // elevation is in the range [-pi/2, pi/2 ], so it is enough to calculate:
00180                 out_e = atan2(-r(2, 0), hypot(r(0, 0), r(1, 0)));
00181 
00182                 // roll:
00183                 if (fabs(r(2, 1)) + fabs(r(2, 2)) < 10 * std::numeric_limits<double>::epsilon()) {
00184                         out_r = 0.0;
00185                         if (out_e > 0)
00186                                 out_a = -atan2(-r(1, 2), r(0, 2));
00187                         else
00188                                 out_a = atan2(-r(1, 2), -r(0, 2));
00189                 } else {
00190                         out_r = atan2(r(2, 1), r(2, 2));
00191                         out_a = atan2(r(1, 0), r(0, 0));
00192                 }
00193     }
00194 
00195     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) {
00196         // ZYX rotation matrix
00197                 arma::mat r(3, 3);
00198                 EulerTransformationMatrices::create_zyx_matrix(in_a, in_e, in_r, r);
00199 
00200                 out_e = acos(r(2, 2));
00201 
00202                 if (fabs(sin(out_e)) > 1E-8) {
00203                         out_r = atan2(r(2, 0) / sin(out_e), r(2, 1) / sin(out_e));
00204                         out_a = -atan2(r(0, 2) / sin(out_e), r(1, 2) / sin(out_e)) + M_PI;
00205                 } else {
00206                         out_a = 0;
00207                         out_r = atan2(r(1, 0), r(1, 1)) + M_PI;
00208                 }
00209     }
00210 
00211     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) {
00212         double azimuth, elevation, roll;
00213 
00214                 // elevation is in the range [-pi/2, pi/2 ], so it is enough to calculate:
00215                 elevation = atan2(-matrix(2, 0), hypot(matrix(0, 0), matrix(1, 0)));
00216 
00217                 // roll:
00218                 if ((fabs(matrix(2, 1)) + fabs(matrix(2, 2))) < 10 * std::numeric_limits<double>::epsilon()) {
00219                         roll = 0.0;
00220                         if (elevation > 0)
00221                                 azimuth = -atan2(-matrix(1, 2), matrix(0, 2));
00222                         else
00223                                 azimuth = atan2(-matrix(1, 2), -matrix(0, 2));
00224                 } else {
00225                         roll = atan2(matrix(2, 1), matrix(2, 2));
00226                         azimuth = atan2(matrix(1, 0), matrix(0, 0));
00227                 }
00228 
00229                 out_x = matrix(0, 3);
00230                 out_y = matrix(1, 3);
00231                 out_z = matrix(2, 3);
00232                 out_azimuth = azimuth;
00233                 out_elevation = elevation;
00234                 out_roll = roll;
00235     }
00236 };
00237 
00238 #endif


katana
Author(s): Martin Günther
autogenerated on Mon Oct 6 2014 10:46:04