src/sensor_fusion/quaternion.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 (_QUATERNION_H_) && defined (_VECTOR_H_)
  21
  22 #include <math.h>
  23
  24 #define QUAT_SIZE 4
  25
  26 template <typename T> int sgn(T val) {
  27         if (val >= 0)
  28                 return 1;
  29         else
  30                 return -1;
  31 }
  32
  33 template <typename T> T mag(T val) {
  34         if (val < 0)
  35                 return val * (T)-1;
  36         else
  37                 return val;
  38 }
  39
  40 template <typename TYPE>
  41 quaternion<TYPE>::quaternion() : m_quat(QUAT_SIZE)
  42 {
  43 }
  44
  45 template <typename TYPE>
  46 quaternion<TYPE>::quaternion(const TYPE w, const TYPE x, const TYPE y, const TYPE z)
  47 {
  48         TYPE vec_data[QUAT_SIZE] = {w, x, y, z};
  49
  50         vect<TYPE> v(QUAT_SIZE, vec_data);
  51         m_quat = v;
  52 }
  53
  54 template <typename TYPE>
  55 quaternion<TYPE>::quaternion(const vect<TYPE> v)
  56 {
  57         m_quat = v;
  58 }
  59
  60 template <typename TYPE>
  61 quaternion<TYPE>::quaternion(const quaternion<TYPE>& q)
  62 {
  63         m_quat = q.m_quat;
  64 }
  65
  66 template <typename TYPE>
  67 quaternion<TYPE>::~quaternion()
  68 {
  69 }
  70
  71 template <typename TYPE>
  72 quaternion<TYPE> quaternion<TYPE>::operator =(const quaternion<TYPE>& q)
  73 {
  74         m_quat = q.m_quat;
  75
  76         return *this;
  77 }
  78
  79 template <typename TYPE>
  80 void quaternion<TYPE>::quat_normalize()
  81 {
  82         TYPE w, x, y, z;
  83         TYPE val;
  84
  85         w = m_quat.m_vec[0] * m_quat.m_vec[0];
  86         x = m_quat.m_vec[1] * m_quat.m_vec[1];
  87         y = m_quat.m_vec[2] * m_quat.m_vec[2];
  88         z = m_quat.m_vec[3] * m_quat.m_vec[3];
  89
  90         val = sqrt(w + x + y + z);
  91
  92         m_quat = m_quat / val;
  93 }
  94
  95 template <typename T>
  96 quaternion<T> operator *(const quaternion<T> q, const T val)
  97 {
  98         quaternion<T> res;
  99         res = q.m_quat * val;
 100         return (res);
 101 }
 102
 103 template <typename T>
 104 quaternion<T> operator *(const quaternion<T> q1, const quaternion<T> q2)
 105 {
 106         T w, x, y, z;
 107         T w1, x1, y1, z1;
 108         T w2, x2, y2, z2;
 109
 110         w1 = q1.m_quat.m_vec[0];
 111         x1 = q1.m_quat.m_vec[1];
 112         y1 = q1.m_quat.m_vec[2];
 113         z1 = q1.m_quat.m_vec[3];
 114
 115         w2 = q2.m_quat.m_vec[0];
 116         x2 = q2.m_quat.m_vec[1];
 117         y2 = q2.m_quat.m_vec[2];
 118         z2 = q2.m_quat.m_vec[3];
 119
 120         x = x1*w2 + y1*z2 - z1*y2 + w1*x2;
 121         y = -x1*z2 + y1*w2 + z1*x2 + w1*y2;
 122         z = x1*y2 - y1*x2 + z1*w2 + w1*z2;
 123         w = -x1*x2 - y1*y2 - z1*z2 + w1*w2;
 124
 125         quaternion<T> q(w, x, y, z);
 126
 127         return q;
 128 }
 129
 130 template <typename T>
 131 quaternion<T> operator +(const quaternion<T> q1, const quaternion<T> q2)
 132 {
 133         return (q1.m_quat + q2.m_quat);
 134 }
 135
 136
 137 template<typename T>
 138 quaternion<T> phase_correction(const quaternion<T> q1, const quaternion<T> q2)
 139 {
 140         T w, x, y, z;
 141         w = mag(q1.m_quat.m_vec[0]) * sgn(q2.m_quat.m_vec[0]);
 142         x = mag(q1.m_quat.m_vec[1]) * sgn(q2.m_quat.m_vec[1]);
 143         y = mag(q1.m_quat.m_vec[2]) * sgn(q2.m_quat.m_vec[2]);
 144         z = mag(q1.m_quat.m_vec[3]) * sgn(q2.m_quat.m_vec[3]);
 145
 146         quaternion<T> q(w, x, y, z);
 147
 148         return q;
 149 }
 150 #endif  //_QUATERNION_H_