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