src/sensor/rotation_vector/fusion_utils/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 TYPE_ROW_COL matrix<TYPE, ROW, COL>::matrix(void)
  23 {
  24         for (int i = 0; i < ROW; i++)
  25                 for (int j = 0; j < COL; j++)
  26                         m_mat[i][j] = 0;
  27 }
  28
  29 TYPE_ROW_COL matrix<TYPE, ROW, COL>::matrix(const matrix<TYPE, ROW, COL>& m)
  30 {
  31         for (int p = 0; p < ROW; p++)
  32                 for (int q = 0; q < COL; q++)
  33                         m_mat[p][q] = m.m_mat[p][q];
  34 }
  35
  36 TYPE_ROW_COL matrix<TYPE, ROW, COL>::matrix(TYPE mat_data[ROW][COL])
  37 {
  38         for (int i = 0; i < ROW; i++)
  39                 for (int j = 0; j < COL; j++)
  40                         m_mat[i][j] = mat_data[i][j];
  41 }
  42
  43 TYPE_ROW_COL matrix<TYPE, ROW, COL>::~matrix()
  44 {
  45 }
  46
  47 TYPE_ROW_COL matrix<TYPE, ROW, COL> matrix<TYPE, ROW, COL>::operator =(const matrix<TYPE, ROW, COL>& m)
  48 {
  49         if (this == &m)
  50         {
  51                 return *this;
  52         }
  53
  54         for (int i = 0; i < ROW; i++)
  55                 for (int j = 0; j < COL; j++)
  56                         m_mat[i][j] = m.m_mat[i][j];
  57
  58         return *this;
  59 }
  60
  61 T_R_C ostream& operator <<(ostream& dout, matrix<T, R, C>& m)
  62 {
  63         for (int i = 0; i < R; i++)
  64         {
  65                 for (int j = 0; j < C; j++)
  66                 {
  67                         dout << m.m_mat[i][j] << "\t";
  68                 }
  69                 dout << endl;
  70         }
  71         return dout;
  72 }
  73
  74 T_R_C matrix<T, R, C> operator +(const matrix<T, R, C> m1, const matrix<T, R, C> m2)
  75 {
  76         matrix<T, R, C> m3;
  77
  78         for (int i = 0; i < R; i++)
  79                 for (int j = 0; j < C; j++)
  80                         m3.m_mat[i][j] = m1.m_mat[i][j] + m2.m_mat[i][j];
  81
  82         return m3;
  83 }
  84
  85 T_R_C matrix<T, R, C> operator +(const matrix<T, R, C> m, const T val)
  86 {
  87         matrix<T, R, C> m1;
  88
  89         for (int i = 0; i < R; i++)
  90                 for (int j = 0; j < C; j++)
  91                         m1.m_mat[i][j] = m.m_mat[i][j] + val;
  92
  93         return m1;
  94 }
  95
  96 T_R_C matrix<T, R, C> operator -(const matrix<T, R, C> m1, const matrix<T, R, C> m2)
  97 {
  98         matrix<T, R, C> m3;
  99
 100         for (int i = 0; i < R; i++)
 101                 for (int j = 0; j < C; j++)
 102                         m3.m_mat[i][j] = m1.m_mat[i][j] - m2.m_mat[i][j];
 103
 104         return m3;
 105 }
 106
 107 T_R_C matrix<T, R, C> operator -(const matrix<T, R, C> m, const T val)
 108 {
 109         matrix<T, R, C> m1;
 110
 111         for (int i = 0; i < R; i++)
 112                 for (int j = 0; j < C; j++)
 113                         m1.m_mat[i][j] = m.m_mat[i][j] - val;
 114
 115         return m1;
 116 }
 117
 118 T_R_C_C2 matrix<T, R, C2> operator *(const matrix<T, R, C> m1, const matrix<T, C, C2> m2)
 119 {
 120         matrix<T, R, C2> m3;
 121
 122         for (int i = 0; i < R; i++)
 123         {
 124                 for (int j = 0; j < C2; j++)
 125                 {
 126                         m3.m_mat[i][j] = 0;
 127                         for (int k = 0; k < C; k++)
 128                                 m3.m_mat[i][j] += m1.m_mat[i][k] * m2.m_mat[k][j];
 129                 }
 130         }
 131
 132         return m3;
 133 }
 134
 135 T_R_C matrix<T, R, C> operator *(const matrix<T, R, C> m, const T val)
 136 {
 137         matrix<T, R, C> m1;
 138
 139         for (int i = 0; i < R; i++)
 140                 for (int j = 0; j < C; j++)
 141                         m1.m_mat[i][j] = m.m_mat[i][j] * val;
 142
 143         return m1;
 144 }
 145
 146 T_R_C matrix<T, R, C> operator /(const matrix<T, R, C> m1, const T val)
 147 {
 148         matrix<T, R, C> m3;
 149
 150         for (int i = 0; i < R; i++)
 151                 for (int j = 0; j < C; j++)
 152                         m3.m_mat[i][j] = m1.m_mat[i][j] / val;
 153
 154         return m3;
 155 }
 156
 157 T_R1_C1_R2_C2 bool operator ==(const matrix<T, R1, C1> m1, const matrix<T, R2, C2> m2)
 158 {
 159         if ((R1 == R2) && (C1 == C2)) {
 160                 for (int i = 0; i < R1; i++)
 161                         for (int j = 0; j < C2; j++)
 162                                 if (m1.m_mat[i][j] != m2.m_mat[i][j])
 163                                         return false;
 164                 return true;
 165         }
 166
 167         return false;
 168 }
 169
 170 T_R1_C1_R2_C2 bool operator !=(const matrix<T, R1, C1> m1, const matrix<T, R2, C2> m2)
 171 {
 172         return (!(m1 == m2));
 173 }
 174
 175 T_R_C matrix<T, R, C> tran(const matrix<T, R, C> m)
 176 {
 177         matrix<T, R, C> m1;
 178
 179         for (int i = 0; i < R; i++)
 180                 for (int j = 0; j < C; j++)
 181                         m1.m_mat[j][i] = m.m_mat[i][j];
 182
 183         return m1;
 184 }
 185
 186 #endif //_MATRIX_H_