Alex Pirciu
a
Embed:
(wiki syntax)
Show/hide line numbers
linalg.h
00001 #ifndef LINALG_H 00002 #define LINALG_H 00003 00004 // #include <mbed.h> 00005 #include <stdint.h> 00006 #include <array> 00007 00008 namespace linalg 00009 { 00010 template <class T, uint32_t M, uint32_t N> 00011 class CMatrix 00012 { 00013 public: 00014 using CThisType = CMatrix<T,M,N>; 00015 using CTransposeType = CMatrix<T,N,M>; 00016 using CContainerType = std::array<std::array<T,N>,M>; 00017 using CDataType =T; 00018 00019 template <uint32_t P> 00020 using CLeftMultipliableType = CMatrix<T,P,M>; 00021 00022 template <uint32_t P> 00023 using CRightMultipliableType = CMatrix<T,N,P>; 00024 00025 template <uint32_t P> 00026 using CLeftMultiplicationResultType = CMatrix<T,P,N>; 00027 00028 template <uint32_t P> 00029 using CRightMultiplicationResultType = CMatrix<T,M,P>; 00030 00031 friend class CMatrix<T,N,M>; 00032 00033 CMatrix() : m_data() {} 00034 CMatrix(const CThisType& f_matrix) : m_data(f_matrix.m_data) {} 00035 CMatrix(const CThisType&& f_matrix) : m_data(f_matrix.m_data) {} 00036 CMatrix(const CContainerType& f_data) : m_data(f_data) {} 00037 CMatrix(const CContainerType&& f_data) : m_data(f_data) {} 00038 00039 CThisType& operator=(const CThisType& f_matrix) 00040 { 00041 for (uint32_t l_row = 0; l_row < M; ++l_row) 00042 { 00043 for (uint32_t l_col = 0; l_col < N; ++l_col) 00044 { 00045 this->m_data[l_row][l_col] = f_matrix.m_data[l_row][l_col]; 00046 } 00047 } 00048 return *this; 00049 } 00050 CThisType& operator=(const CThisType&& f_matrix) 00051 { 00052 for (uint32_t l_row = 0; l_row < M; ++l_row) 00053 { 00054 for (uint32_t l_col = 0; l_col < N; ++l_col) 00055 { 00056 this->m_data[l_row][l_col] = f_matrix.m_data[l_row][l_col]; 00057 } 00058 } 00059 return *this; 00060 } 00061 00062 std::array<T,N>& operator[](uint32_t f_row) 00063 { 00064 return m_data[f_row]; 00065 } 00066 00067 CDataType& operator()(uint32_t f_row, uint32_t f_col) 00068 { 00069 return m_data[f_row][f_col]; 00070 } 00071 00072 const std::array<T,N>& operator[](uint32_t f_row) const 00073 { 00074 return m_data[f_row]; 00075 } 00076 00077 const CDataType& operator()(uint32_t f_row, uint32_t f_col) const 00078 { 00079 return m_data[f_row][f_col]; 00080 } 00081 00082 // template<> 00083 // CMatrix<T,1,1>::operator CDataType() 00084 // { 00085 // return m_data[0][0]; 00086 // } 00087 00088 CThisType& operator+() {return *this;} 00089 CThisType operator-() 00090 { 00091 CThisType l_matrix; 00092 for (uint32_t l_row = 0; l_row < M; ++l_row) 00093 { 00094 for (uint32_t l_col = 0; l_col < N; ++l_col) 00095 { 00096 l_matrix.m_data[l_row][l_col] = -this->m_data[l_row][l_col]; 00097 } 00098 } 00099 return l_matrix; 00100 } 00101 CThisType operator+(const CThisType& f_matrix) 00102 { 00103 CThisType l_matrix; 00104 for (uint32_t l_row = 0; l_row < M; ++l_row) 00105 { 00106 for (uint32_t l_col = 0; l_col < N; ++l_col) 00107 { 00108 l_matrix.m_data[l_row][l_col] = this->m_data[l_row][l_col] + f_matrix.m_data[l_row][l_col]; 00109 } 00110 } 00111 return l_matrix; 00112 } 00113 CThisType operator-(const CThisType& f_matrix) 00114 { 00115 CThisType l_matrix; 00116 for (uint32_t l_row = 0; l_row < M; ++l_row) 00117 { 00118 for (uint32_t l_col = 0; l_col < N; ++l_col) 00119 { 00120 l_matrix.m_data[l_row][l_col] = this->m_data[l_row][l_col] - f_matrix.m_data[l_row][l_col]; 00121 } 00122 } 00123 return l_matrix; 00124 } 00125 CThisType& operator+=(const CThisType& f_matrix) 00126 { 00127 for (uint32_t l_row = 0; l_row < M; ++l_row) 00128 { 00129 for (uint32_t l_col = 0; l_col < N; ++l_col) 00130 { 00131 this->m_data[l_row][l_col] += f_matrix.m_data[l_row][l_col]; 00132 } 00133 } 00134 return *this; 00135 } 00136 CThisType& operator-=(const CThisType& f_matrix) 00137 { 00138 for (uint32_t l_row = 0; l_row < M; ++l_row) 00139 { 00140 for (uint32_t l_col = 0; l_col < N; ++l_col) 00141 { 00142 this->m_data[l_row][l_col] -= f_matrix.m_data[l_row][l_col]; 00143 } 00144 } 00145 return *this; 00146 } 00147 00148 CThisType operator+(const CDataType& f_val) 00149 { 00150 CThisType l_matrix; 00151 for (uint32_t l_row = 0; l_row < M; ++l_row) 00152 { 00153 for (uint32_t l_col = 0; l_col < N; ++l_col) 00154 { 00155 l_matrix.m_data[l_row][l_col] = this->m_data[l_row][l_col] + f_val; 00156 } 00157 } 00158 return l_matrix; 00159 } 00160 CThisType operator-(const CDataType& f_val) 00161 { 00162 CThisType l_matrix; 00163 for (uint32_t l_row = 0; l_row < M; ++l_row) 00164 { 00165 for (uint32_t l_col = 0; l_col < N; ++l_col) 00166 { 00167 l_matrix.m_data[l_row][l_col] = this->m_data[l_row][l_col] - f_val; 00168 } 00169 } 00170 return l_matrix; 00171 } 00172 CThisType& operator+=(const CDataType& f_val) 00173 { 00174 for (uint32_t l_row = 0; l_row < M; ++l_row) 00175 { 00176 for (uint32_t l_col = 0; l_col < N; ++l_col) 00177 { 00178 this->m_data[l_row][l_col] += f_val; 00179 } 00180 } 00181 return *this; 00182 } 00183 CThisType& operator-=(const CDataType& f_val) 00184 { 00185 for (uint32_t l_row = 0; l_row < M; ++l_row) 00186 { 00187 for (uint32_t l_col = 0; l_col < N; ++l_col) 00188 { 00189 this->m_data[l_row][l_col] -= f_val; 00190 } 00191 } 00192 return *this; 00193 } 00194 CThisType& operator*=(const CDataType& f_val) 00195 { 00196 for (uint32_t l_row = 0; l_row < M; ++l_row) 00197 { 00198 for (uint32_t l_col = 0; l_col < N; ++l_col) 00199 { 00200 this->m_data[l_row][l_col] *= f_val; 00201 } 00202 } 00203 return *this; 00204 } 00205 CThisType& operator*=(const CThisType& f_val) 00206 { 00207 CThisType& l_thisRef(*this); 00208 l_thisRef = l_thisRef * f_val; 00209 return l_thisRef; 00210 } 00211 CThisType& operator/=(const CDataType& f_val) 00212 { 00213 for (uint32_t l_row = 0; l_row < M; ++l_row) 00214 { 00215 for (uint32_t l_col = 0; l_col < N; ++l_col) 00216 { 00217 this->m_data[l_row][l_col] /= f_val; 00218 } 00219 } 00220 return *this; 00221 } 00222 CThisType operator*(const CDataType& f_val) 00223 { 00224 CThisType l_matrix; 00225 for (uint32_t l_row = 0; l_row < M; ++l_row) 00226 { 00227 for (uint32_t l_col = 0; l_col < N; ++l_col) 00228 { 00229 l_matrix.m_data[l_row][l_col] = this->m_data[l_row][l_col] * f_val; 00230 } 00231 } 00232 return l_matrix; 00233 } 00234 template <uint32_t P> 00235 CRightMultiplicationResultType<P> operator*(const CRightMultipliableType<P>& f_matrix) 00236 { 00237 CRightMultiplicationResultType<P> l_matrix; 00238 for (uint32_t l_row = 0; l_row < M; ++l_row) 00239 { 00240 for (uint32_t l_col = 0; l_col < P; ++l_col) 00241 { 00242 l_matrix[l_row][l_col] = 0; 00243 for (uint32_t l_idx = 0; l_idx < N; ++l_idx) 00244 { 00245 l_matrix[l_row][l_col] += this->m_data[l_row][l_idx] * f_matrix[l_idx][l_col]; 00246 } 00247 } 00248 } 00249 return l_matrix; 00250 } 00251 CThisType inv(); 00252 00253 CTransposeType transpose() 00254 { 00255 CTransposeType l_trsp; 00256 00257 for (uint32_t l_row = 0; l_row < M; ++l_row) 00258 { 00259 for (uint32_t l_col = 0; l_col < N; ++l_col) 00260 { 00261 l_trsp.m_data[l_col][l_row] = this->m_data[l_row][l_col]; 00262 } 00263 } 00264 00265 return l_trsp; 00266 } 00267 00268 template <uint32_t P> 00269 CRightMultiplicationResultType<P> solve(const CRightMultipliableType<P>& f_B) 00270 { 00271 return CLUDecomposition(*this).solve(); 00272 } 00273 00274 static CThisType zeros() 00275 { 00276 CThisType l_res; 00277 00278 for (uint32_t l_row = 0; l_row < M; ++l_row) 00279 { 00280 for (uint32_t l_col = 0; l_col < N; ++l_col) 00281 { 00282 l_res[l_row][l_col] = 0; 00283 } 00284 } 00285 00286 return l_res; 00287 } 00288 00289 static CThisType ones() 00290 { 00291 CThisType l_res; 00292 00293 for (uint32_t l_row = 0; l_row < M; ++l_row) 00294 { 00295 for (uint32_t l_col = 0; l_col < N; ++l_col) 00296 { 00297 l_res[l_row][l_col] = 1; 00298 } 00299 } 00300 00301 return l_res; 00302 } 00303 00304 static CThisType eye() 00305 { 00306 CThisType l_res(CThisType::zeros()); 00307 uint32_t l_minDim = std::min(M,N); 00308 00309 for (uint32_t l_row = 0; l_row < l_minDim; ++l_row) 00310 { 00311 l_res[l_row][l_row] = 1; 00312 } 00313 00314 return l_res; 00315 } 00316 00317 protected: 00318 std::array<std::array<T,N>,M> m_data; 00319 }; 00320 00321 template<class T, uint32_t N> 00322 class CLUDecomposition 00323 { 00324 public: 00325 using CThisType = CLUDecomposition<T,N>; 00326 using COriginalType = CMatrix<T,N,N>; 00327 using CDataType =T; 00328 00329 template <uint32_t P> 00330 using CLeftMultipliableType = CMatrix<T,P,N>; 00331 00332 template <uint32_t P> 00333 using CRightMultipliableType = CMatrix<T,N,P>; 00334 00335 template <uint32_t P> 00336 using CLeftMultiplicationResultType = CMatrix<T,P,N>; 00337 00338 template <uint32_t P> 00339 using CRightMultiplicationResultType = CMatrix<T,N,P>; 00340 00341 CLUDecomposition(const CThisType& f_decomposition) : m_L(f_decomposition.m_L), m_U(f_decomposition.m_U), m_P(f_decomposition.m_P) {} 00342 CLUDecomposition(const CThisType&& f_decomposition) : m_L(f_decomposition.m_L), m_U(f_decomposition.m_U), m_P(f_decomposition.m_P) {} 00343 CLUDecomposition(const COriginalType& f_matrix) : m_L(), m_U(), m_P() {decompose(f_matrix);} 00344 CLUDecomposition(const COriginalType&& f_matrix) : m_L(), m_U(), m_P() {decompose(f_matrix);} 00345 00346 operator COriginalType() 00347 { 00348 return m_L * m_U; 00349 } 00350 00351 COriginalType inv() 00352 { 00353 return triUInv() * triLInv(); 00354 } 00355 00356 COriginalType triLInv() 00357 { 00358 COriginalType l_invL(m_L); 00359 00360 for (uint32_t l_jdx = 0; l_jdx < N; ++l_jdx) 00361 { 00362 l_invL[l_jdx][l_jdx] = 1/l_invL[l_jdx][l_jdx]; 00363 00364 for (uint32_t l_idx = l_jdx+1; l_idx < N; ++l_idx) 00365 { 00366 l_invL[l_idx][l_jdx] = 0; 00367 00368 for (uint32_t l_kdx = l_jdx; l_kdx < l_idx; ++l_kdx) 00369 { 00370 l_invL[l_idx][l_jdx] -= m_L[l_idx][l_kdx]*l_invL[l_kdx][l_jdx]; 00371 } 00372 00373 l_invL[l_idx][l_jdx] /= l_invL[l_idx][l_idx]; 00374 } 00375 } 00376 00377 return l_invL; 00378 } 00379 00380 COriginalType triUInv() 00381 { 00382 COriginalType l_invU(m_U); 00383 00384 for (int32_t l_jdx = (N-1); l_jdx >= 0; --l_jdx) 00385 { 00386 l_invU[l_jdx][l_jdx] = 1/l_invU[l_jdx][l_jdx]; 00387 00388 for (int32_t l_idx = (l_jdx-1); l_idx >= 0; --l_idx) 00389 { 00390 l_invU[l_idx][l_jdx] = 0; 00391 00392 for (int32_t l_kdx = (l_idx+1); l_kdx < (l_jdx+1); ++l_kdx) 00393 { 00394 l_invU[l_idx][l_jdx] -= m_U[l_idx][l_kdx]*l_invU[l_kdx][l_jdx]; 00395 } 00396 00397 l_invU[l_idx][l_jdx] /= l_invU[l_idx][l_idx]; 00398 } 00399 } 00400 00401 return l_invU; 00402 } 00403 00404 template <uint32_t P> 00405 CRightMultiplicationResultType<P> solve(const CRightMultipliableType<P>& f_B) 00406 { 00407 CRightMultipliableType<P> l_B; 00408 00409 for (uint32_t l_idx = 0; l_idx < N; ++l_idx) 00410 { 00411 for (uint32_t l_jdx = 0; l_jdx < P; ++l_jdx) 00412 { 00413 l_B[l_idx][l_jdx] = f_B[m_P[l_idx]][l_jdx]; 00414 } 00415 } 00416 00417 return sTriL(sTriU(l_B)); 00418 } 00419 00420 template <uint32_t P> 00421 CRightMultiplicationResultType<P> sTriU(const CRightMultipliableType<P>& f_B) 00422 { 00423 return triUInv()*f_B; 00424 } 00425 00426 template <uint32_t P> 00427 CRightMultiplicationResultType<P> sTriL(const CRightMultipliableType<P>& f_B) 00428 { 00429 // return triLInv()*f_B; 00430 } 00431 00432 00433 // private: 00434 void decompose(const CMatrix<T,N,N>& f_matrix) 00435 { 00436 m_U = f_matrix; 00437 for (uint32_t l_kdx = 0; l_kdx < (N-1); ++l_kdx) 00438 { 00439 m_L[l_kdx][l_kdx] = 1; 00440 m_P[l_kdx] = l_kdx; 00441 00442 for(uint32_t l_idx = l_kdx+1; l_idx < N; ++l_idx) 00443 { 00444 m_L[l_idx][l_kdx] = m_U[l_idx][l_kdx]/m_U[l_kdx][l_kdx]; 00445 m_U[l_idx][l_kdx] = 0; 00446 for(uint32_t l_jdx = l_kdx+1; l_jdx < N; ++l_jdx) 00447 { 00448 m_U[l_idx][l_jdx] = m_U[l_idx][l_jdx] - m_L[l_idx][l_kdx]*m_U[l_kdx][l_jdx]; 00449 } 00450 } 00451 } 00452 m_L[N-1][N-1] = 1; 00453 m_P[N-1] = N-1; 00454 } 00455 CMatrix<T,N,N> m_L; 00456 CMatrix<T,N,N> m_U; 00457 std::array<uint32_t,N> m_P; 00458 }; 00459 00460 template <class T, uint32_t N> 00461 using CColVector = CMatrix<T,N,1>; 00462 00463 template <class T, uint32_t N> 00464 using CVector = CColVector<T,N>; 00465 00466 template <class T, uint32_t N> 00467 using CRowVector = CMatrix<T,1,N>; 00468 }; 00469 00470 #include "linalg.inl" 00471 00472 #endif // LINALG_H 00473
Generated on Tue Jul 12 2022 22:40:50 by
1.7.2