src/sensor_fusion/euler_angles.cpp

   1 /*
   2  * sensord
   3  *
   4  * Copyright (c) 2014 Samsung Electronics Co., Ltd.
   5  *
   6  * Licensed under the Apache License, Version 2.0 (the "License");
   7  * you may not use this file except in compliance with the License.
   8  * You may obtain a copy of the License at
   9  *
  10  * http://www.apache.org/licenses/LICENSE-2.0
  11  *
  12  * Unless required by applicable law or agreed to in writing, software
  13  * distributed under the License is distributed on an "AS IS" BASIS,
  14  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  15  * See the License for the specific language governing permissions and
  16  * limitations under the License.
  17  *
  18  */
  19 #if defined (_EULER_ANGLES_H) && defined (_VECTOR_H)
  20
  21 #include <math.h>
  22
  23 #define EULER_SIZE 3
  24 #define RAD2DEG 57.2957795
  25 #define DEG2RAD 0.0174532925
  26
  27 template <typename TYPE>
  28 euler_angles<TYPE>::euler_angles() : m_ang(EULER_SIZE)
  29 {
  30 }
  31
  32 template <typename TYPE>
  33 euler_angles<TYPE>::euler_angles(const TYPE roll, const TYPE pitch, const TYPE yaw)
  34 {
  35         TYPE euler_data[EULER_SIZE] = {roll, pitch, yaw};
  36
  37         vect<TYPE> v(EULER_SIZE, euler_data);
  38         m_ang = v;
  39 }
  40
  41 template <typename TYPE>
  42 euler_angles<TYPE>::euler_angles(const vect<TYPE> v)
  43 {
  44         m_ang = v;
  45 }
  46
  47 template <typename TYPE>
  48 euler_angles<TYPE>::euler_angles(const euler_angles<TYPE>& e)
  49 {
  50         m_ang = e.m_ang;
  51 }
  52
  53 template <typename TYPE>
  54 euler_angles<TYPE>::~euler_angles()
  55 {
  56
  57 }
  58
  59 template <typename TYPE>
  60 euler_angles<TYPE> euler_angles<TYPE>::operator =(const euler_angles<TYPE>& e)
  61 {
  62         m_ang = e.m_ang;
  63
  64         return *this;
  65 }
  66
  67 template <typename T>
  68 euler_angles<T> quat2euler(const quaternion<T> q)
  69 {
  70         T w, x, y, z;
  71         T R11, R21, R31, R32, R33;
  72         T phi, theta, psi;
  73
  74         w = q.m_quat.m_vec[0];
  75         x = q.m_quat.m_vec[1];
  76         y = q.m_quat.m_vec[2];
  77         z = q.m_quat.m_vec[3];
  78
  79         R11 = 2 * (w * w) - 1 + 2 * (x * x);
  80         R21 = 2 * ((x * y) - (w * z));
  81         R31 = 2 * ((x * z) + (w * y));
  82         R32 = 2 * ((y * z) - (w * x));
  83         R33 = 2 * (w * w) - 1 + 2 * (z * z);
  84
  85         phi = atan2(R32, R33);
  86         theta = -atan(R31 / sqrt(1 - (R31 * R31)));
  87         psi = atan2(R21, R11);
  88
  89         euler_angles<T> e(phi, theta, psi);
  90
  91         return e;
  92 }
  93
  94 template <typename T>
  95 euler_angles<T> rad2deg(const euler_angles<T> e)
  96 {
  97         return (e.m_ang * (T) RAD2DEG);
  98 }
  99
 100 template <typename T>
 101 euler_angles<T> deg2rad(const euler_angles<T> e)
 102 {
 103         return (e.m_ang * (T) DEG2RAD);
 104 }
 105
 106 #endif  //_EULER_ANGLES_H