src/fusion-sensor/rotation_vector/fusion_utils/sensor_data.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 (_SENSOR_DATA_H_) && defined (_VECTOR_H_)
  21
  22 #include "math.h"
  23
  24 template <typename TYPE>
  25 sensor_data<TYPE>::sensor_data() : m_data(), m_time_stamp(0)
  26 {
  27 }
  28
  29 template <typename TYPE>
  30 sensor_data<TYPE>::sensor_data(const TYPE x, const TYPE y,
  31                 const TYPE z, const unsigned long long time_stamp)
  32 {
  33         TYPE vec_data[SENSOR_DATA_SIZE] = {x, y, z};
  34
  35         vect<TYPE, SENSOR_DATA_SIZE> v(vec_data);
  36         m_data = v;
  37         m_time_stamp = time_stamp;
  38 }
  39
  40 template <typename TYPE>
  41 sensor_data<TYPE>::sensor_data(const vect<TYPE, SENSOR_DATA_SIZE> v,
  42                 const unsigned long long time_stamp)
  43 {
  44         m_data = v;
  45         m_time_stamp = time_stamp;
  46 }
  47
  48 template <typename TYPE>
  49 sensor_data<TYPE>::sensor_data(const sensor_data<TYPE>& s)
  50 {
  51         m_data = s.m_data;
  52         m_time_stamp = s.m_time_stamp;
  53 }
  54
  55 template <typename TYPE>
  56 sensor_data<TYPE>::~sensor_data()
  57 {
  58 }
  59
  60 template <typename TYPE>
  61 sensor_data<TYPE> sensor_data<TYPE>::operator =(const sensor_data<TYPE>& s)
  62 {
  63         m_data = s.m_data;
  64         m_time_stamp = s.m_time_stamp;
  65
  66         return *this;
  67 }
  68
  69 template <typename T>
  70 sensor_data<T> operator +(sensor_data<T> data1, sensor_data<T> data2)
  71 {
  72         sensor_data<T> result(data1.m_data + data2.m_data, 0);
  73         return result;
  74 }
  75
  76 template <typename T>
  77 void normalize(sensor_data<T>& data)
  78 {
  79         T x, y, z;
  80
  81         x = data.m_data.m_vec[0];
  82         y = data.m_data.m_vec[1];
  83         z = data.m_data.m_vec[2];
  84
  85         T val = sqrt(x*x + y*y + z*z);
  86
  87         data.m_data.m_vec[0] = x / val;
  88         data.m_data.m_vec[1] = y / val;
  89         data.m_data.m_vec[2] = z / val;
  90 }
  91
  92 template <typename T>
  93 sensor_data<T> scale_data(sensor_data<T> data, T scaling_factor)
  94 {
  95         T x, y, z;
  96
  97         x = data.m_data.m_vec[0] / scaling_factor;
  98         y = data.m_data.m_vec[1] / scaling_factor;
  99         z = data.m_data.m_vec[2] / scaling_factor;
 100
 101         sensor_data<T> s(x, y, z, data.m_time_stamp);
 102
 103         return s;
 104 }
 105
 106
 107 template<typename T>
 108 quaternion<T> sensor_data2quat(const sensor_data<T> data, const vect<T, REF_VEC_SIZE> ref_vec)
 109 {
 110         vect<T, REF_VEC_SIZE> axis;
 111         T angle;
 112
 113         axis = cross(data.m_data, ref_vec);
 114         angle = acos(dot(data.m_data, ref_vec));
 115
 116         return axis2quat(axis, angle);
 117 }
 118
 119 template<typename T>
 120 void pre_process_data(sensor_data<T> &data_out, const T *data_in, int sign, int scale)
 121 {
 122         data_out.m_data.m_vec[0] = sign * data_in[0] / scale;
 123         data_out.m_data.m_vec[1] = sign * data_in[1] / scale;
 124         data_out.m_data.m_vec[2] = sign * data_in[2] / scale;
 125 }
 126
 127 #endif /* _SENSOR_DATA_H_ */