src/sensor_fusion/standalone/util/vector.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 (_VECTOR_H) && defined (_MATRIX_H)
  21
  22 template <typename TYPE>
  23 vector<TYPE>::vector(void)
  24 {
  25         m_vec = NULL;
  26 }
  27
  28 template <typename TYPE>
  29 vector<TYPE>::vector(const int size)
  30 {
  31         m_size = size;
  32         m_vec = NULL;
  33         m_vec = new TYPE [m_size]();
  34 }
  35
  36 template <typename TYPE>
  37 vector<TYPE>::vector(const int size, TYPE *vec_data)
  38 {
  39         m_size = size;
  40         m_vec = NULL;
  41         m_vec = new TYPE [m_size];
  42
  43         for (int j = 0; j < m_size; j++)
  44                 m_vec[j] = *vec_data++;
  45 }
  46
  47 template <typename TYPE>
  48 vector<TYPE>::vector(const vector<TYPE>& v)
  49 {
  50         m_size = v.m_size;
  51         m_vec = NULL;
  52         m_vec = new TYPE [m_size];
  53
  54         for (int q = 0; q < m_size; q++)
  55                 m_vec[q] = v.m_vec[q];
  56 }
  57
  58 template <typename TYPE>
  59 vector<TYPE>::~vector()
  60 {
  61         if (m_vec != NULL)
  62                 delete[] m_vec;
  63 }
  64
  65 template <typename TYPE>
  66 vector<TYPE> vector<TYPE>::operator =(const vector<TYPE>& v)
  67 {
  68         if (this == &v)
  69         {
  70                 return *this;
  71         }
  72
  73         if (m_vec == NULL)
  74         {
  75                 m_size = v.m_size;
  76                 m_vec = new TYPE [m_size];
  77         }
  78         else
  79         {
  80                 if (m_size != v.m_size)
  81                 {
  82                         delete[] m_vec;
  83
  84                         m_size = v.m_size;
  85                         m_vec = new TYPE [m_size];
  86                 }
  87         }
  88
  89
  90         for (int q = 0; q < m_size; q++)
  91                 m_vec[q] = v.m_vec[q];
  92
  93
  94         return *this;
  95 }
  96
  97 template <typename TYPE>
  98 ostream& operator <<(ostream& dout, vector<TYPE>& v)
  99 {
 100         for (int j = 0; j < v.m_size; j++)
 101         {
 102                 dout << v.m_vec[j] << "\t";
 103         }
 104
 105         dout << endl;
 106
 107         return dout;
 108 }
 109
 110 template <typename T>
 111 vector<T> operator +(const vector<T> v1, const vector<T> v2)
 112 {
 113         assert(v1.m_size == v2.m_size);
 114
 115         vector<T> v3(v1.m_size);
 116
 117         for (int j = 0; j < v1.m_size; j++)
 118                 v3.m_vec[j] = v1.m_vec[j] + v2.m_vec[j];
 119
 120         return v3;
 121 }
 122
 123 template <typename T>
 124 vector<T> operator +(const vector<T> v, const T val)
 125 {
 126         vector<T> v1(v.m_size);
 127
 128         for (int j = 0; j < v.m_size; j++)
 129                 v1.m_vec[j] = v.m_vec[j] + val;
 130
 131         return v1;
 132 }
 133
 134 template <typename T>
 135 vector<T> operator -(const vector<T> v1, const vector<T> v2)
 136 {
 137         assert(v1.m_size == v2.m_size);
 138
 139         vector<T> v3(v1.m_size);
 140
 141         for (int j = 0; j < v1.m_size; j++)
 142                 v3.m_vec[j] = v1.m_vec[j] - v2.m_vec[j];
 143
 144         return v3;
 145 }
 146
 147 template <typename T>
 148 vector<T> operator -(const vector<T> v, const T val)
 149 {
 150         vector<T> v1(v.m_size);
 151
 152         for (int j = 0; j < v.m_size; j++)
 153                 v1.m_vec[j] = v.m_vec[j] - val;
 154
 155         return v1;
 156 }
 157
 158 template <typename T>
 159 matrix<T> operator *(const matrix<T> m, const vector<T> v)
 160 {
 161         assert(m.m_rows == v.m_size);
 162         assert(m.m_cols == 1);
 163
 164         matrix<T> m1(m.m_rows, v.m_size);
 165
 166         for (int i = 0; i < m1.m_rows; i++)
 167         {
 168                 for (int j = 0; j < m1.m_cols; j++)
 169                 {
 170                         m1.m_mat[i][j] = m.m_mat[i][0] * v.m_vec[j];
 171                 }
 172         }
 173
 174         return m1;
 175 }
 176
 177 template <typename T>
 178 vector<T> operator *(const vector<T> v, const matrix<T> m)
 179 {
 180         assert(m.m_rows == v.m_size);
 181         assert(m.m_cols != 1);
 182
 183         vector<T> v1(m.m_cols);
 184
 185         for (int j = 0; j < m.m_cols; j++)
 186         {
 187                 v1.m_vec[j] = 0;
 188                 for (int k = 0; k < m.m_rows; k++)
 189                         v1.m_vec[j] += v.m_vec[k] * m.m_mat[k][j];
 190         }
 191
 192         return v1;
 193 }
 194
 195 template <typename T>
 196 vector<T> operator *(const vector<T> v, const T val)
 197 {
 198         vector<T> v1(v.m_size);
 199
 200         for (int j = 0; j < v.m_size; j++)
 201                 v1.m_vec[j] = v.m_vec[j] * val;
 202
 203         return v1;
 204 }
 205
 206 template <typename T>
 207 vector<T> operator /(const vector<T> v, const T val)
 208 {
 209         vector<T> v1(v.m_size);
 210
 211         for (int j = 0; j < v.m_size; j++)
 212                 v1.m_vec[j] = v.m_vec[j] / val;
 213
 214         return v1;
 215 }
 216
 217 template <typename T>
 218 bool operator ==(const vector<T> v1, const vector<T> v2)
 219 {
 220         if (v1.m_size == v2.m_size)
 221         {
 222                 for (int i = 0; i < v1.m_size; i++)
 223                         if (v1.m_vec[i] != v2.m_vec[i])
 224                                 return false;
 225         }
 226         else
 227                 return false;
 228
 229         return true;
 230 }
 231
 232 template <typename T>
 233 bool operator !=(const vector<T> v1, const vector<T> v2)
 234 {
 235         return (!(v1 == v2));
 236 }
 237
 238 template <typename T>
 239 matrix<T> transpose(const vector<T> v)
 240 {
 241         matrix<T> m(v.m_size, 1);
 242
 243         for (int i = 0; i < v.m_size; i++)
 244                 m.m_mat[i][0] = v.m_vec[i];
 245
 246         return m;
 247 }
 248
 249 template <typename T>
 250 vector<T> transpose(const matrix<T> m)
 251 {
 252         vector<T> v(m.m_rows);
 253
 254         for (int i = 0; i < m.m_rows; i++)
 255                 v.m_vec[i] = m.m_mat[i][0];
 256
 257         return v;
 258 }
 259
 260 template <typename T>
 261 T mul(const vector<T> v, const matrix<T> m)
 262 {
 263         assert(m.m_rows == v.m_size);
 264         assert(m.m_cols == 1);
 265
 266         T result = (T) 0;
 267
 268         for (int k = 0; k < v.m_size; k++)
 269                 result += v.m_vec[k] * m.m_mat[k][0];
 270
 271         return result;
 272 }
 273
 274
 275 template <typename T>
 276 void insert_end(vector<T>& v, T val)
 277 {
 278         for (int i = 0; i < (v.m_size - 1); i++)
 279                 v.m_vec[i] = v.m_vec[i+1];
 280
 281         v.m_vec[v.m_size-1] = val;
 282 }
 283
 284 template <typename T>
 285 vector<T> cross(const vector<T> v1, const vector<T> v2)
 286 {
 287         vector<T> v3(v1.m_size);
 288
 289         v3.m_vec[0] = ((v1.m_vec[1] * v2.m_vec[2]) - (v1.m_vec[2] * v2.m_vec[1]));
 290         v3.m_vec[1] = ((v1.m_vec[2] * v2.m_vec[0]) - (v1.m_vec[0] * v2.m_vec[2]));
 291         v3.m_vec[2] = ((v1.m_vec[0] * v2.m_vec[1]) - (v1.m_vec[1] * v2.m_vec[0]));
 292
 293         return v3;
 294 }
 295
 296 template <typename T>
 297 bool is_initialized(const vector<T> v)
 298 {
 299         vector<T> v1(v.m_size);
 300         bool retval;
 301
 302         retval = (v == v1) ? false : true;
 303
 304         return retval;
 305 }
 306
 307 template <typename T>
 308 T var(const vector<T> v)
 309 {
 310         T val = 0;
 311         T mean, var, diff;
 312
 313         for (int i = 0; i < v.m_size; i++)
 314                 val += v.m_vec[i];
 315
 316         mean = val / v.m_size;
 317
 318         val = 0;
 319         for (int i = 0; i < v.m_size; i++)
 320         {
 321                 diff = (v.m_vec[i] - mean);
 322                 val += diff * diff;
 323         }
 324
 325         var = val / (v.m_size - 1);
 326
 327         return var;
 328 }
 329 #endif
 330