src/sensor_fusion/rotation_matrix.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
  20 #if defined (_ROTATION_MATRIX_H) && defined (_MATRIX_H)
  21
  22 #define QUAT_LEN 4
  23 #define ROT_MAT_ROWS 3
  24 #define ROT_MAT_COLS 3
  25
  26 template <typename T> T get_sign(T val)
  27 {
  28         return (val >= (T) 0) ? (T) 1 : (T) -1;
  29 }
  30
  31 template <typename TYPE>
  32 rotation_matrix<TYPE>::rotation_matrix() : m_rot_mat(ROT_MAT_ROWS, ROT_MAT_COLS)
  33 {
  34
  35 }
  36
  37 template <typename TYPE>
  38 rotation_matrix<TYPE>::rotation_matrix(const matrix<TYPE> m)
  39 {
  40         m_rot_mat = m;
  41 }
  42
  43 template <typename TYPE>
  44 rotation_matrix<TYPE>::rotation_matrix(const int rows, const int cols, TYPE *mat_data)
  45 {
  46         matrix<TYPE> m(rows, cols, mat_data);
  47         m_rot_mat = m;
  48 }
  49
  50 template <typename TYPE>
  51 rotation_matrix<TYPE>::rotation_matrix(const rotation_matrix<TYPE>& rm)
  52 {
  53         m_rot_mat = rm.m_rot_mat;
  54 }
  55
  56 template <typename TYPE>
  57 rotation_matrix<TYPE>::~rotation_matrix()
  58 {
  59
  60 }
  61
  62 template <typename TYPE>
  63 rotation_matrix<TYPE> rotation_matrix<TYPE>::operator =(const rotation_matrix<TYPE>& rm)
  64 {
  65         m_rot_mat = rm.m_rot_mat;
  66
  67         return *this;
  68 }
  69
  70 template <typename T>
  71 rotation_matrix<T> quat2rot_mat(quaternion<T> q)
  72 {
  73         T w, x, y, z;
  74         T R[ROT_MAT_ROWS][ROT_MAT_COLS];
  75
  76         w = q.m_quat.m_vec[0];
  77         x = q.m_quat.m_vec[1];
  78         y = q.m_quat.m_vec[2];
  79         z = q.m_quat.m_vec[3];
  80
  81         R[0][0] = (2 * w * w) - 1 + (2 * x * x);
  82         R[0][1] = 2 * ((x * y) + (w * z));
  83         R[0][2] = 2 * ((x * z) - (w * y));
  84         R[1][0] = 2 * ((x * y) - (w * z));
  85         R[1][1] = (2 * w * w) - 1 + (2 * y * y);
  86         R[1][2] = 2 * ((y * z) + (w * x));
  87         R[2][0] = 2 * ((x * z) + (w * y));
  88         R[2][1] = 2 * ((y * z) - (w * x));
  89         R[2][2] = (2 * w * w) - 1 + (2 * z * z);
  90
  91         rotation_matrix<T> rm(ROT_MAT_ROWS, ROT_MAT_COLS, &R[0][0]);
  92
  93         return rm;
  94 }
  95
  96 template <typename T>
  97 quaternion<T> rot_mat2quat(rotation_matrix<T> rm)
  98 {
  99         T q0, q1, q2, q3;
 100
 101         T diag_sum = rm.m_rot_mat.m_mat[0][0] + rm.m_rot_mat.m_mat[1][1] + rm.m_rot_mat.m_mat[2][2];
 102
 103         if ( diag_sum > 0 )
 104         {
 105                 T val = (T) 0.5 / sqrt(diag_sum + (T) 1.0);
 106                 q0 = (T) 0.25 / val;
 107                 q1 = ( rm.m_rot_mat.m_mat[2][1] - rm.m_rot_mat.m_mat[1][2] ) * val;
 108                 q2 = ( rm.m_rot_mat.m_mat[0][2] - rm.m_rot_mat.m_mat[2][0] ) * val;
 109                 q3 = ( rm.m_rot_mat.m_mat[1][0] - rm.m_rot_mat.m_mat[0][1] ) * val;
 110         }
 111         else
 112         {
 113                 if ( rm.m_rot_mat.m_mat[0][0] > rm.m_rot_mat.m_mat[1][1] && rm.m_rot_mat.m_mat[0][0] > rm.m_rot_mat.m_mat[2][2] )
 114                 {
 115                         T val = (T) 2.0 * sqrt( 1.0 + rm.m_rot_mat.m_mat[0][0] - rm.m_rot_mat.m_mat[1][1] - rm.m_rot_mat.m_mat[2][2]);
 116                         q0 = (rm.m_rot_mat.m_mat[2][1] - rm.m_rot_mat.m_mat[1][2] ) / val;
 117                         q1 = (T) 0.25 * val;
 118                         q2 = (rm.m_rot_mat.m_mat[0][1] + rm.m_rot_mat.m_mat[1][0] ) / val;
 119                         q3 = (rm.m_rot_mat.m_mat[0][2] + rm.m_rot_mat.m_mat[2][0] ) / val;
 120                 }
 121                 else if (rm.m_rot_mat.m_mat[1][1] > rm.m_rot_mat.m_mat[2][2])
 122                 {
 123                         T val = (T) 2.0 * sqrt( 1.0 + rm.m_rot_mat.m_mat[1][1] - rm.m_rot_mat.m_mat[0][0] - rm.m_rot_mat.m_mat[2][2]);
 124                         q0 = (rm.m_rot_mat.m_mat[0][2] - rm.m_rot_mat.m_mat[2][0] ) / val;
 125                         q1 = (rm.m_rot_mat.m_mat[0][1] + rm.m_rot_mat.m_mat[1][0] ) / val;
 126                         q2 = (T) 0.25 * val;
 127                         q3 = (rm.m_rot_mat.m_mat[1][2] + rm.m_rot_mat.m_mat[2][1] ) / val;
 128                 }
 129                 else
 130                 {
 131                         T val = (T) 2.0 * sqrt( 1.0 + rm.m_rot_mat.m_mat[2][2] - rm.m_rot_mat.m_mat[0][0] - rm.m_rot_mat.m_mat[1][1] );
 132                         q0 = (rm.m_rot_mat.m_mat[1][0] - rm.m_rot_mat.m_mat[0][1] ) / val;
 133                         q1 = (rm.m_rot_mat.m_mat[0][2] + rm.m_rot_mat.m_mat[2][0] ) / val;
 134                         q2 = (rm.m_rot_mat.m_mat[1][2] + rm.m_rot_mat.m_mat[2][1] ) / val;
 135                         q3 = (T) 0.25 * val;
 136                 }
 137         }
 138
 139         quaternion<T> q(q0, q1, q2, q3);
 140
 141         q.quat_normalize();
 142
 143         return q;
 144 }
 145
 146 #endif /* _ROTATION_MATRIX_H */