A lite library for operations in linear algebra
Embed:
(wiki syntax)
Show/hide line numbers
MATRIX_PRIMITIVE.cpp
00001 #include "MATRIX_PRIMITIVE.h" 00002 00003 // 00004 // using namespace MATRIX_PRIMITIVE; 00005 // 00006 00007 // Namespace MATRIX_PRIMITIVE 00008 ////////////////////////// 00009 namespace MATRIX_PRIMITIVE 00010 { 00011 // Utilities 00012 /////////////////////////// 00013 void Mat_multiply_Vec(vector<float> &v_out, vector<vector<float> > &m_left, vector<float> &v_right){ // v_out = m_left*v_right 00014 vector<float>::iterator it_out; 00015 vector<float>::iterator it_m_row; 00016 vector<float>::iterator it_v; 00017 // 00018 it_out = v_out.begin(); 00019 for (size_t i = 0; i < m_left.size(); ++i){ 00020 *it_out = 0.0; 00021 it_m_row = m_left[i].begin(); 00022 it_v = v_right.begin(); 00023 for (size_t j = 0; j < m_left[i].size(); ++j){ 00024 // *it_out += m_left[i][j] * v_right[j]; 00025 if (*it_m_row != 0.0 && *it_v != 0.0){ 00026 (*it_out) += (*it_m_row) * (*it_v); 00027 }else{ 00028 // (*it_out) += 0.0 00029 } 00030 // (*it_out) += (*it_m_row) * (*it_v); 00031 // 00032 it_m_row++; 00033 it_v++; 00034 } 00035 it_out++; 00036 } 00037 } 00038 vector<float> Mat_multiply_Vec(vector<vector<float> > &m_left, vector<float> &v_right){ // v_out = m_left*v_right 00039 static vector<float> v_out; 00040 // Size check 00041 if (v_out.size() != m_left.size()){ 00042 v_out.resize(m_left.size()); 00043 } 00044 // Iterators 00045 vector<float>::iterator it_out; 00046 vector<float>::iterator it_m_row; 00047 vector<float>::iterator it_v; 00048 // 00049 it_out = v_out.begin(); 00050 for (size_t i = 0; i < m_left.size(); ++i){ 00051 *it_out = 0.0; 00052 it_m_row = m_left[i].begin(); 00053 it_v = v_right.begin(); 00054 for (size_t j = 0; j < m_left[i].size(); ++j){ 00055 // *it_out += m_left[i][j] * v_right[j]; 00056 if (*it_m_row != 0.0 && *it_v != 0.0){ 00057 (*it_out) += (*it_m_row) * (*it_v); 00058 }else{ 00059 // (*it_out) += 0.0 00060 } 00061 // (*it_out) += (*it_m_row) * (*it_v); 00062 // 00063 it_m_row++; 00064 it_v++; 00065 } 00066 it_out++; 00067 } 00068 return v_out; 00069 } 00070 00071 // New function for matrix multiply matrix 00072 vector<vector<float> > Mat_multiply_Mat(vector<vector<float> > &m_left, vector<vector<float> > &m_right){ // m_out = m_left*m_right 00073 static vector<vector<float> > m_out; 00074 00075 // mxn = (mxk)*(kxn) 00076 size_t M, K, N; 00077 // 00078 M = m_left.size(); 00079 K = m_left[0].size(); 00080 N = m_right[0].size(); 00081 00082 // Size check 00083 if (m_out.size() != M || m_out[0].size() != N ){ 00084 m_out.resize(M, vector<float>( N ) ); 00085 } 00086 00087 // Using indexing 00088 for (size_t i = 0; i < M; ++i){ // row in m_out 00089 for (size_t j = 0; j < N; ++j){ // column in m_out 00090 m_out[i][j] = 0.0; 00091 for (size_t k = 0; k < K; ++k){ 00092 if (m_left[i][k] != 0.0 && m_right[k][j] != 0.0) 00093 m_out[i][j] += m_left[i][k]*m_right[k][j]; 00094 } 00095 } 00096 } 00097 00098 return m_out; 00099 } 00100 00101 vector<float> Get_VectorPlus(const vector<float> &v_a, const vector<float> &v_b, bool is_minus) // v_a + (or -) v_b 00102 { 00103 static vector<float> v_c; 00104 // Size check 00105 if (v_c.size() != v_a.size()){ 00106 v_c.resize(v_a.size()); 00107 } 00108 // 00109 for (size_t i = 0; i < v_a.size(); ++i){ 00110 if (is_minus){ 00111 v_c[i] = v_a[i] - v_b[i]; 00112 }else{ 00113 v_c[i] = v_a[i] + v_b[i]; 00114 } 00115 } 00116 return v_c; 00117 } 00118 vector<float> Get_VectorScalarMultiply(const vector<float> &v_a, float scale) // scale*v_a 00119 { 00120 static vector<float> v_c; 00121 // Size check 00122 if (v_c.size() != v_a.size()){ 00123 v_c.resize(v_a.size()); 00124 } 00125 // for pure negative 00126 if (scale == -1.0){ 00127 for (size_t i = 0; i < v_a.size(); ++i){ 00128 v_c[i] = -v_a[i]; 00129 } 00130 return v_c; 00131 } 00132 // else 00133 for (size_t i = 0; i < v_a.size(); ++i){ 00134 v_c[i] = scale*v_a[i]; 00135 00136 } 00137 return v_c; 00138 } 00139 00140 // Important! 00141 // New function for scale-up a vector 00142 // v_a *= scale 00143 void Get_VectorScaleUp(vector<float> &v_a, float scale) // v_a *= scale 00144 { 00145 // for pure negative 00146 if (scale == -1.0){ 00147 for (size_t i = 0; i < v_a.size(); ++i){ 00148 v_a[i] = -v_a[i]; 00149 } 00150 // 00151 }else{ 00152 // else 00153 for (size_t i = 0; i < v_a.size(); ++i){ 00154 v_a[i] *= scale; 00155 } 00156 // 00157 } 00158 } 00159 00160 // Increment 00161 void Get_VectorIncrement(vector<float> &v_a, const vector<float> &v_b, bool is_minus){ // v_a += (or -=) v_b 00162 // Size check 00163 if (v_a.size() != v_b.size()){ 00164 v_a.resize(v_b.size()); 00165 } 00166 // 00167 if (is_minus){ // -= 00168 for (size_t i = 0; i < v_b.size(); ++i){ 00169 v_a[i] -= v_b[i]; 00170 } 00171 }else{ // += 00172 for (size_t i = 0; i < v_b.size(); ++i){ 00173 v_a[i] += v_b[i]; 00174 } 00175 } 00176 00177 } 00178 /////////////////////////// end Utilities 00179 00180 00181 // Matrix inversion, using Gauss method 00182 ///////////////////////////////////////// 00183 bool SolveSingularityOnDiag(vector<vector<float> > &M, vector<vector<float> > &M_inv, size_t i){ 00184 // Return value: is_singular 00185 //-----------------------------------// 00186 // true: the matrix is singular 00187 // false: the singularity is not yet been found or the matrix is invertible 00188 //-----------------------------------// 00189 00190 00191 // For ith-row 00192 //-----------------------------------// 00193 // If the ith element on diagonal is zero, find a row with it ith elemnt non-zero and add to the current row (ith row) 00194 // For both M and M_inv 00195 00196 size_t n = M.size(); 00197 // Search for other non-zero elements 00198 float max_absValue = 0.0; 00199 size_t idx_max = i; 00200 00201 // The following block of code is wrong. 00202 /* 00203 for (size_t j = 0; j < n; ++j){ 00204 if (j != i){ // Other than i 00205 float absValue = fabs(M[j][i]); 00206 if ( absValue > max_absValue){ 00207 max_absValue = absValue; 00208 idx_max = j; 00209 // 00210 // break; // Once found a non-zero element, break it (Not going to find the maximum one) 00211 // 00212 } 00213 } 00214 } 00215 */ 00216 00217 // It's sufficient and "should" only looking downward 00218 for (size_t j = (i+1); j < n; ++j){ 00219 // Below ith-row 00220 float absValue = fabs(M[j][i]); 00221 if ( absValue > max_absValue){ 00222 max_absValue = absValue; 00223 idx_max = j; 00224 // 00225 // break; // Once found a non-zero element, break it (Not going to find the maximum one) 00226 // 00227 } 00228 // 00229 } 00230 00231 // 00232 if (idx_max == i){ // The matrix is singular !! 00233 return true; // is_singular 00234 } 00235 00236 // Add that row to the current row 00237 Get_VectorIncrement(M[i], M[idx_max], false); // += 00238 Get_VectorIncrement(M_inv[i], M_inv[idx_max], false); // += 00239 // 00240 return false; // may be non-singular 00241 } 00242 vector<vector<float> > MatrixInversion(vector<vector<float> > M){ 00243 // 00244 size_t n = M.size(); // The size of the square matrix 00245 00246 // Check if M is a square matrix 00247 if (n != M[0].size()){ 00248 return M; 00249 } 00250 00251 // Output matrix 00252 vector<vector<float> > M_inv(n,vector<float>(n, 0.0)); 00253 00254 // Initialize the identity matrix M_inv 00255 for (size_t i = 0; i < n; ++i){ 00256 M_inv[i][i] = 1.0; 00257 } 00258 00259 // 00260 // cout << "M_inv:\n"; 00261 // printMatrix(M_inv); 00262 00263 00264 // A row vector 00265 vector<float> row_left; 00266 vector<float> row_right; 00267 float M_diag = 1.0; 00268 00269 /* 00270 // Check if each element on diagonal is not zero 00271 // If it is zero, find a row with a non-zero element on that column and add that row to the current row 00272 for (size_t i = 0; i < n; ++i){ 00273 M_diag = M[i][i]; 00274 if (M_diag == 0.0){ 00275 // Search for other non-zero elements 00276 // SolveSingularityOnDiag(M, M_inv, i); 00277 00278 // Search for other non-zero elements 00279 float max_absValue = 0.0; 00280 size_t idx_max = i; 00281 for (size_t j = 0; j < n; ++j){ 00282 if (j != i){ // Other that i 00283 float absValue = fabs(M[j][i]); 00284 if ( absValue > max_absValue){ 00285 max_absValue = absValue; 00286 idx_max = j; 00287 } 00288 } 00289 } 00290 // Add that row to the current row 00291 Get_VectorIncrement(M[i], M[idx_max], false); // += 00292 Get_VectorIncrement(M_inv[i], M_inv[idx_max], false); // += 00293 // 00294 00295 }else{ 00296 // Fine! Nothing to do. 00297 } 00298 } 00299 */ 00300 00301 // 00302 /* 00303 cout << "M:\n"; 00304 printMatrix(M); 00305 // 00306 cout << "M_inv:\n"; 00307 printMatrix(M_inv); 00308 */ 00309 // 00310 00311 /* 00312 // Scale each row vector for normalizing each elements on the diagonal to 1.0 00313 for (size_t i = 0; i < n; ++i){ 00314 float ratio; 00315 ratio = 1.0/M[i][i]; 00316 // 00317 Get_VectorScaleUp(M[i], ratio); 00318 Get_VectorScaleUp(M_inv[i], ratio); 00319 // 00320 // M[i] = Get_VectorScalarMultiply(M[i], ratio); 00321 // M_inv[i] = Get_VectorScalarMultiply(M_inv[i], ratio); 00322 // 00323 } 00324 */ 00325 00326 /* 00327 // 00328 cout << "M:\n"; 00329 printMatrix(M); 00330 // 00331 cout << "M_inv:\n"; 00332 printMatrix(M_inv); 00333 // 00334 */ 00335 00336 // 00337 // The flag for singularity 00338 bool is_singular = false; 00339 // 00340 00341 // Eliminate all the other elements except those on the diagonal 00342 for (size_t i = 0; i < n; ++i){ // For each element on diagonal 00343 float ratio; 00344 00345 // For solving the problem of that diagonal element is zero 00346 if (M[i][i] == 0.0){ 00347 is_singular |= SolveSingularityOnDiag(M, M_inv, i); 00348 } 00349 00350 // 00351 if (is_singular){ 00352 // The matrix is singular, 00353 // stop this meaningless process and return. 00354 // break; 00355 return M_inv; 00356 } 00357 00358 // Normalize the diagonal element in M to 1.0 00359 if (M[i][i] != 1.0){ 00360 ratio = 1.0/M[i][i]; 00361 Get_VectorScaleUp(M[i], ratio); 00362 Get_VectorScaleUp(M_inv[i], ratio); 00363 } 00364 // 00365 00366 // 00367 row_left = M[i]; 00368 row_right = M_inv[i]; 00369 // 00370 // M_diag = M[i][i]; // This has been normalized, which is 1.0 00371 // 00372 for (size_t j = 0; j < n; ++j){ // For the row other than than the current row 00373 if (j != i){ // Not going to do anything with that row itself 00374 // 00375 // ratio = M[j][i]/M_diag; 00376 ratio = M[j][i]; // Because M_diag = 1.0 00377 Get_VectorIncrement(M[j], Get_VectorScalarMultiply(row_left, ratio), true); // -= 00378 Get_VectorIncrement(M_inv[j], Get_VectorScalarMultiply(row_right, ratio), true); // -= 00379 // 00380 /* 00381 Get_VectorIncrement(M[j], Get_VectorScalarMultiply(M[i], ratio), true); // -= 00382 Get_VectorIncrement(M_inv[j], Get_VectorScalarMultiply(M_inv[i], ratio), true); // -= 00383 */ 00384 } 00385 } 00386 00387 /* 00388 // 00389 cout << "Diagonal element i = " << i << "\n"; 00390 // 00391 cout << "M:\n"; 00392 printMatrix(M); 00393 // 00394 cout << "M_inv:\n"; 00395 printMatrix(M_inv); 00396 */ 00397 } 00398 return M_inv; 00399 00400 } 00401 ///////////////////////////////////////// end Matrix inversion, using Gauss method 00402 00403 00404 } 00405 ////////////////////////// end Namespace MATRIX_PRIMITIVE
Generated on Fri Jul 15 2022 15:12:08 by
1.7.2