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