Chun Feng Huang / MATRIX_LIB

A lite library for operations in linear algebra

MATRIX_PRIMITIVE.cpp@0:33b75d52d2a7, 2017-02-10 (annotated)

Committer:
benson516
Date:
Fri Feb 10 18:31:32 2017 +0000
Revision:
0:33b75d52d2a7
Speed up the library by using constant references

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benson516 0:33b75d52d2a7 1 #include "MATRIX_PRIMITIVE.h"
benson516 0:33b75d52d2a7 2
benson516 0:33b75d52d2a7 3 //
benson516 0:33b75d52d2a7 4 // using namespace MATRIX_PRIMITIVE;
benson516 0:33b75d52d2a7 5 //
benson516 0:33b75d52d2a7 6
benson516 0:33b75d52d2a7 7 // Namespace MATRIX_PRIMITIVE
benson516 0:33b75d52d2a7 8 //////////////////////////
benson516 0:33b75d52d2a7 9 namespace MATRIX_PRIMITIVE
benson516 0:33b75d52d2a7 10 {
benson516 0:33b75d52d2a7 11 // Utilities
benson516 0:33b75d52d2a7 12 ///////////////////////////
benson516 0:33b75d52d2a7 13 void Mat_multiply_Vec(vector<float> &v_out, vector<vector<float> > &m_left, vector<float> &v_right){ // v_out = m_left*v_right
benson516 0:33b75d52d2a7 14 vector<float>::iterator it_out;
benson516 0:33b75d52d2a7 15 vector<float>::iterator it_m_row;
benson516 0:33b75d52d2a7 16 vector<float>::iterator it_v;
benson516 0:33b75d52d2a7 17 //
benson516 0:33b75d52d2a7 18 it_out = v_out.begin();
benson516 0:33b75d52d2a7 19 for (size_t i = 0; i < m_left.size(); ++i){
benson516 0:33b75d52d2a7 20 *it_out = 0.0;
benson516 0:33b75d52d2a7 21 it_m_row = m_left[i].begin();
benson516 0:33b75d52d2a7 22 it_v = v_right.begin();
benson516 0:33b75d52d2a7 23 for (size_t j = 0; j < m_left[i].size(); ++j){
benson516 0:33b75d52d2a7 24 // *it_out += m_left[i][j] * v_right[j];
benson516 0:33b75d52d2a7 25 if (*it_m_row != 0.0 && *it_v != 0.0){
benson516 0:33b75d52d2a7 26 (*it_out) += (*it_m_row) * (*it_v);
benson516 0:33b75d52d2a7 27 }else{
benson516 0:33b75d52d2a7 28 // (*it_out) += 0.0
benson516 0:33b75d52d2a7 29 }
benson516 0:33b75d52d2a7 30 // (*it_out) += (*it_m_row) * (*it_v);
benson516 0:33b75d52d2a7 31 //
benson516 0:33b75d52d2a7 32 it_m_row++;
benson516 0:33b75d52d2a7 33 it_v++;
benson516 0:33b75d52d2a7 34 }
benson516 0:33b75d52d2a7 35 it_out++;
benson516 0:33b75d52d2a7 36 }
benson516 0:33b75d52d2a7 37 }
benson516 0:33b75d52d2a7 38 vector<float> Mat_multiply_Vec(vector<vector<float> > &m_left, vector<float> &v_right){ // v_out = m_left*v_right
benson516 0:33b75d52d2a7 39 static vector<float> v_out;
benson516 0:33b75d52d2a7 40 // Size check
benson516 0:33b75d52d2a7 41 if (v_out.size() != m_left.size()){
benson516 0:33b75d52d2a7 42 v_out.resize(m_left.size());
benson516 0:33b75d52d2a7 43 }
benson516 0:33b75d52d2a7 44 // Iterators
benson516 0:33b75d52d2a7 45 vector<float>::iterator it_out;
benson516 0:33b75d52d2a7 46 vector<float>::iterator it_m_row;
benson516 0:33b75d52d2a7 47 vector<float>::iterator it_v;
benson516 0:33b75d52d2a7 48 //
benson516 0:33b75d52d2a7 49 it_out = v_out.begin();
benson516 0:33b75d52d2a7 50 for (size_t i = 0; i < m_left.size(); ++i){
benson516 0:33b75d52d2a7 51 *it_out = 0.0;
benson516 0:33b75d52d2a7 52 it_m_row = m_left[i].begin();
benson516 0:33b75d52d2a7 53 it_v = v_right.begin();
benson516 0:33b75d52d2a7 54 for (size_t j = 0; j < m_left[i].size(); ++j){
benson516 0:33b75d52d2a7 55 // *it_out += m_left[i][j] * v_right[j];
benson516 0:33b75d52d2a7 56 if (*it_m_row != 0.0 && *it_v != 0.0){
benson516 0:33b75d52d2a7 57 (*it_out) += (*it_m_row) * (*it_v);
benson516 0:33b75d52d2a7 58 }else{
benson516 0:33b75d52d2a7 59 // (*it_out) += 0.0
benson516 0:33b75d52d2a7 60 }
benson516 0:33b75d52d2a7 61 // (*it_out) += (*it_m_row) * (*it_v);
benson516 0:33b75d52d2a7 62 //
benson516 0:33b75d52d2a7 63 it_m_row++;
benson516 0:33b75d52d2a7 64 it_v++;
benson516 0:33b75d52d2a7 65 }
benson516 0:33b75d52d2a7 66 it_out++;
benson516 0:33b75d52d2a7 67 }
benson516 0:33b75d52d2a7 68 return v_out;
benson516 0:33b75d52d2a7 69 }
benson516 0:33b75d52d2a7 70
benson516 0:33b75d52d2a7 71 // New function for matrix multiply matrix
benson516 0:33b75d52d2a7 72 vector<vector<float> > Mat_multiply_Mat(vector<vector<float> > &m_left, vector<vector<float> > &m_right){ // m_out = m_left*m_right
benson516 0:33b75d52d2a7 73 static vector<vector<float> > m_out;
benson516 0:33b75d52d2a7 74
benson516 0:33b75d52d2a7 75 // mxn = (mxk)*(kxn)
benson516 0:33b75d52d2a7 76 size_t M, K, N;
benson516 0:33b75d52d2a7 77 //
benson516 0:33b75d52d2a7 78 M = m_left.size();
benson516 0:33b75d52d2a7 79 K = m_left[0].size();
benson516 0:33b75d52d2a7 80 N = m_right[0].size();
benson516 0:33b75d52d2a7 81
benson516 0:33b75d52d2a7 82 // Size check
benson516 0:33b75d52d2a7 83 if (m_out.size() != M || m_out[0].size() != N ){
benson516 0:33b75d52d2a7 84 m_out.resize(M, vector<float>( N ) );
benson516 0:33b75d52d2a7 85 }
benson516 0:33b75d52d2a7 86
benson516 0:33b75d52d2a7 87 // Using indexing
benson516 0:33b75d52d2a7 88 for (size_t i = 0; i < M; ++i){ // row in m_out
benson516 0:33b75d52d2a7 89 for (size_t j = 0; j < N; ++j){ // column in m_out
benson516 0:33b75d52d2a7 90 m_out[i][j] = 0.0;
benson516 0:33b75d52d2a7 91 for (size_t k = 0; k < K; ++k){
benson516 0:33b75d52d2a7 92 if (m_left[i][k] != 0.0 && m_right[k][j] != 0.0)
benson516 0:33b75d52d2a7 93 m_out[i][j] += m_left[i][k]*m_right[k][j];
benson516 0:33b75d52d2a7 94 }
benson516 0:33b75d52d2a7 95 }
benson516 0:33b75d52d2a7 96 }
benson516 0:33b75d52d2a7 97
benson516 0:33b75d52d2a7 98 return m_out;
benson516 0:33b75d52d2a7 99 }
benson516 0:33b75d52d2a7 100
benson516 0:33b75d52d2a7 101 vector<float> Get_VectorPlus(const vector<float> &v_a, const vector<float> &v_b, bool is_minus) // v_a + (or -) v_b
benson516 0:33b75d52d2a7 102 {
benson516 0:33b75d52d2a7 103 static vector<float> v_c;
benson516 0:33b75d52d2a7 104 // Size check
benson516 0:33b75d52d2a7 105 if (v_c.size() != v_a.size()){
benson516 0:33b75d52d2a7 106 v_c.resize(v_a.size());
benson516 0:33b75d52d2a7 107 }
benson516 0:33b75d52d2a7 108 //
benson516 0:33b75d52d2a7 109 for (size_t i = 0; i < v_a.size(); ++i){
benson516 0:33b75d52d2a7 110 if (is_minus){
benson516 0:33b75d52d2a7 111 v_c[i] = v_a[i] - v_b[i];
benson516 0:33b75d52d2a7 112 }else{
benson516 0:33b75d52d2a7 113 v_c[i] = v_a[i] + v_b[i];
benson516 0:33b75d52d2a7 114 }
benson516 0:33b75d52d2a7 115 }
benson516 0:33b75d52d2a7 116 return v_c;
benson516 0:33b75d52d2a7 117 }
benson516 0:33b75d52d2a7 118 vector<float> Get_VectorScalarMultiply(const vector<float> &v_a, float scale) // scale*v_a
benson516 0:33b75d52d2a7 119 {
benson516 0:33b75d52d2a7 120 static vector<float> v_c;
benson516 0:33b75d52d2a7 121 // Size check
benson516 0:33b75d52d2a7 122 if (v_c.size() != v_a.size()){
benson516 0:33b75d52d2a7 123 v_c.resize(v_a.size());
benson516 0:33b75d52d2a7 124 }
benson516 0:33b75d52d2a7 125 // for pure negative
benson516 0:33b75d52d2a7 126 if (scale == -1.0){
benson516 0:33b75d52d2a7 127 for (size_t i = 0; i < v_a.size(); ++i){
benson516 0:33b75d52d2a7 128 v_c[i] = -v_a[i];
benson516 0:33b75d52d2a7 129 }
benson516 0:33b75d52d2a7 130 return v_c;
benson516 0:33b75d52d2a7 131 }
benson516 0:33b75d52d2a7 132 // else
benson516 0:33b75d52d2a7 133 for (size_t i = 0; i < v_a.size(); ++i){
benson516 0:33b75d52d2a7 134 v_c[i] = scale*v_a[i];
benson516 0:33b75d52d2a7 135
benson516 0:33b75d52d2a7 136 }
benson516 0:33b75d52d2a7 137 return v_c;
benson516 0:33b75d52d2a7 138 }
benson516 0:33b75d52d2a7 139
benson516 0:33b75d52d2a7 140 // Important!
benson516 0:33b75d52d2a7 141 // New function for scale-up a vector
benson516 0:33b75d52d2a7 142 // v_a *= scale
benson516 0:33b75d52d2a7 143 void Get_VectorScaleUp(vector<float> &v_a, float scale) // v_a *= scale
benson516 0:33b75d52d2a7 144 {
benson516 0:33b75d52d2a7 145 // for pure negative
benson516 0:33b75d52d2a7 146 if (scale == -1.0){
benson516 0:33b75d52d2a7 147 for (size_t i = 0; i < v_a.size(); ++i){
benson516 0:33b75d52d2a7 148 v_a[i] = -v_a[i];
benson516 0:33b75d52d2a7 149 }
benson516 0:33b75d52d2a7 150 //
benson516 0:33b75d52d2a7 151 }else{
benson516 0:33b75d52d2a7 152 // else
benson516 0:33b75d52d2a7 153 for (size_t i = 0; i < v_a.size(); ++i){
benson516 0:33b75d52d2a7 154 v_a[i] *= scale;
benson516 0:33b75d52d2a7 155 }
benson516 0:33b75d52d2a7 156 //
benson516 0:33b75d52d2a7 157 }
benson516 0:33b75d52d2a7 158 }
benson516 0:33b75d52d2a7 159
benson516 0:33b75d52d2a7 160 // Increment
benson516 0:33b75d52d2a7 161 void Get_VectorIncrement(vector<float> &v_a, const vector<float> &v_b, bool is_minus){ // v_a += (or -=) v_b
benson516 0:33b75d52d2a7 162 // Size check
benson516 0:33b75d52d2a7 163 if (v_a.size() != v_b.size()){
benson516 0:33b75d52d2a7 164 v_a.resize(v_b.size());
benson516 0:33b75d52d2a7 165 }
benson516 0:33b75d52d2a7 166 //
benson516 0:33b75d52d2a7 167 if (is_minus){ // -=
benson516 0:33b75d52d2a7 168 for (size_t i = 0; i < v_b.size(); ++i){
benson516 0:33b75d52d2a7 169 v_a[i] -= v_b[i];
benson516 0:33b75d52d2a7 170 }
benson516 0:33b75d52d2a7 171 }else{ // +=
benson516 0:33b75d52d2a7 172 for (size_t i = 0; i < v_b.size(); ++i){
benson516 0:33b75d52d2a7 173 v_a[i] += v_b[i];
benson516 0:33b75d52d2a7 174 }
benson516 0:33b75d52d2a7 175 }
benson516 0:33b75d52d2a7 176
benson516 0:33b75d52d2a7 177 }
benson516 0:33b75d52d2a7 178 /////////////////////////// end Utilities
benson516 0:33b75d52d2a7 179
benson516 0:33b75d52d2a7 180
benson516 0:33b75d52d2a7 181 // Matrix inversion, using Gauss method
benson516 0:33b75d52d2a7 182 /////////////////////////////////////////
benson516 0:33b75d52d2a7 183 bool SolveSingularityOnDiag(vector<vector<float> > &M, vector<vector<float> > &M_inv, size_t i){
benson516 0:33b75d52d2a7 184 // Return value: is_singular
benson516 0:33b75d52d2a7 185 //-----------------------------------//
benson516 0:33b75d52d2a7 186 // true: the matrix is singular
benson516 0:33b75d52d2a7 187 // false: the singularity is not yet been found or the matrix is invertible
benson516 0:33b75d52d2a7 188 //-----------------------------------//
benson516 0:33b75d52d2a7 189
benson516 0:33b75d52d2a7 190
benson516 0:33b75d52d2a7 191 // For ith-row
benson516 0:33b75d52d2a7 192 //-----------------------------------//
benson516 0:33b75d52d2a7 193 // 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)
benson516 0:33b75d52d2a7 194 // For both M and M_inv
benson516 0:33b75d52d2a7 195
benson516 0:33b75d52d2a7 196 size_t n = M.size();
benson516 0:33b75d52d2a7 197 // Search for other non-zero elements
benson516 0:33b75d52d2a7 198 float max_absValue = 0.0;
benson516 0:33b75d52d2a7 199 size_t idx_max = i;
benson516 0:33b75d52d2a7 200
benson516 0:33b75d52d2a7 201 // The following block of code is wrong.
benson516 0:33b75d52d2a7 202 /*
benson516 0:33b75d52d2a7 203 for (size_t j = 0; j < n; ++j){
benson516 0:33b75d52d2a7 204 if (j != i){ // Other than i
benson516 0:33b75d52d2a7 205 float absValue = fabs(M[j][i]);
benson516 0:33b75d52d2a7 206 if ( absValue > max_absValue){
benson516 0:33b75d52d2a7 207 max_absValue = absValue;
benson516 0:33b75d52d2a7 208 idx_max = j;
benson516 0:33b75d52d2a7 209 //
benson516 0:33b75d52d2a7 210 // break; // Once found a non-zero element, break it (Not going to find the maximum one)
benson516 0:33b75d52d2a7 211 //
benson516 0:33b75d52d2a7 212 }
benson516 0:33b75d52d2a7 213 }
benson516 0:33b75d52d2a7 214 }
benson516 0:33b75d52d2a7 215 */
benson516 0:33b75d52d2a7 216
benson516 0:33b75d52d2a7 217 // It's sufficient and "should" only looking downward
benson516 0:33b75d52d2a7 218 for (size_t j = (i+1); j < n; ++j){
benson516 0:33b75d52d2a7 219 // Below ith-row
benson516 0:33b75d52d2a7 220 float absValue = fabs(M[j][i]);
benson516 0:33b75d52d2a7 221 if ( absValue > max_absValue){
benson516 0:33b75d52d2a7 222 max_absValue = absValue;
benson516 0:33b75d52d2a7 223 idx_max = j;
benson516 0:33b75d52d2a7 224 //
benson516 0:33b75d52d2a7 225 // break; // Once found a non-zero element, break it (Not going to find the maximum one)
benson516 0:33b75d52d2a7 226 //
benson516 0:33b75d52d2a7 227 }
benson516 0:33b75d52d2a7 228 //
benson516 0:33b75d52d2a7 229 }
benson516 0:33b75d52d2a7 230
benson516 0:33b75d52d2a7 231 //
benson516 0:33b75d52d2a7 232 if (idx_max == i){ // The matrix is singular !!
benson516 0:33b75d52d2a7 233 return true; // is_singular
benson516 0:33b75d52d2a7 234 }
benson516 0:33b75d52d2a7 235
benson516 0:33b75d52d2a7 236 // Add that row to the current row
benson516 0:33b75d52d2a7 237 Get_VectorIncrement(M[i], M[idx_max], false); // +=
benson516 0:33b75d52d2a7 238 Get_VectorIncrement(M_inv[i], M_inv[idx_max], false); // +=
benson516 0:33b75d52d2a7 239 //
benson516 0:33b75d52d2a7 240 return false; // may be non-singular
benson516 0:33b75d52d2a7 241 }
benson516 0:33b75d52d2a7 242 vector<vector<float> > MatrixInversion(vector<vector<float> > M){
benson516 0:33b75d52d2a7 243 //
benson516 0:33b75d52d2a7 244 size_t n = M.size(); // The size of the square matrix
benson516 0:33b75d52d2a7 245
benson516 0:33b75d52d2a7 246 // Check if M is a square matrix
benson516 0:33b75d52d2a7 247 if (n != M[0].size()){
benson516 0:33b75d52d2a7 248 return M;
benson516 0:33b75d52d2a7 249 }
benson516 0:33b75d52d2a7 250
benson516 0:33b75d52d2a7 251 // Output matrix
benson516 0:33b75d52d2a7 252 vector<vector<float> > M_inv(n,vector<float>(n, 0.0));
benson516 0:33b75d52d2a7 253
benson516 0:33b75d52d2a7 254 // Initialize the identity matrix M_inv
benson516 0:33b75d52d2a7 255 for (size_t i = 0; i < n; ++i){
benson516 0:33b75d52d2a7 256 M_inv[i][i] = 1.0;
benson516 0:33b75d52d2a7 257 }
benson516 0:33b75d52d2a7 258
benson516 0:33b75d52d2a7 259 //
benson516 0:33b75d52d2a7 260 // cout << "M_inv:\n";
benson516 0:33b75d52d2a7 261 // printMatrix(M_inv);
benson516 0:33b75d52d2a7 262
benson516 0:33b75d52d2a7 263
benson516 0:33b75d52d2a7 264 // A row vector
benson516 0:33b75d52d2a7 265 vector<float> row_left;
benson516 0:33b75d52d2a7 266 vector<float> row_right;
benson516 0:33b75d52d2a7 267 float M_diag = 1.0;
benson516 0:33b75d52d2a7 268
benson516 0:33b75d52d2a7 269 /*
benson516 0:33b75d52d2a7 270 // Check if each element on diagonal is not zero
benson516 0:33b75d52d2a7 271 // If it is zero, find a row with a non-zero element on that column and add that row to the current row
benson516 0:33b75d52d2a7 272 for (size_t i = 0; i < n; ++i){
benson516 0:33b75d52d2a7 273 M_diag = M[i][i];
benson516 0:33b75d52d2a7 274 if (M_diag == 0.0){
benson516 0:33b75d52d2a7 275 // Search for other non-zero elements
benson516 0:33b75d52d2a7 276 // SolveSingularityOnDiag(M, M_inv, i);
benson516 0:33b75d52d2a7 277
benson516 0:33b75d52d2a7 278 // Search for other non-zero elements
benson516 0:33b75d52d2a7 279 float max_absValue = 0.0;
benson516 0:33b75d52d2a7 280 size_t idx_max = i;
benson516 0:33b75d52d2a7 281 for (size_t j = 0; j < n; ++j){
benson516 0:33b75d52d2a7 282 if (j != i){ // Other that i
benson516 0:33b75d52d2a7 283 float absValue = fabs(M[j][i]);
benson516 0:33b75d52d2a7 284 if ( absValue > max_absValue){
benson516 0:33b75d52d2a7 285 max_absValue = absValue;
benson516 0:33b75d52d2a7 286 idx_max = j;
benson516 0:33b75d52d2a7 287 }
benson516 0:33b75d52d2a7 288 }
benson516 0:33b75d52d2a7 289 }
benson516 0:33b75d52d2a7 290 // Add that row to the current row
benson516 0:33b75d52d2a7 291 Get_VectorIncrement(M[i], M[idx_max], false); // +=
benson516 0:33b75d52d2a7 292 Get_VectorIncrement(M_inv[i], M_inv[idx_max], false); // +=
benson516 0:33b75d52d2a7 293 //
benson516 0:33b75d52d2a7 294
benson516 0:33b75d52d2a7 295 }else{
benson516 0:33b75d52d2a7 296 // Fine! Nothing to do.
benson516 0:33b75d52d2a7 297 }
benson516 0:33b75d52d2a7 298 }
benson516 0:33b75d52d2a7 299 */
benson516 0:33b75d52d2a7 300
benson516 0:33b75d52d2a7 301 //
benson516 0:33b75d52d2a7 302 /*
benson516 0:33b75d52d2a7 303 cout << "M:\n";
benson516 0:33b75d52d2a7 304 printMatrix(M);
benson516 0:33b75d52d2a7 305 //
benson516 0:33b75d52d2a7 306 cout << "M_inv:\n";
benson516 0:33b75d52d2a7 307 printMatrix(M_inv);
benson516 0:33b75d52d2a7 308 */
benson516 0:33b75d52d2a7 309 //
benson516 0:33b75d52d2a7 310
benson516 0:33b75d52d2a7 311 /*
benson516 0:33b75d52d2a7 312 // Scale each row vector for normalizing each elements on the diagonal to 1.0
benson516 0:33b75d52d2a7 313 for (size_t i = 0; i < n; ++i){
benson516 0:33b75d52d2a7 314 float ratio;
benson516 0:33b75d52d2a7 315 ratio = 1.0/M[i][i];
benson516 0:33b75d52d2a7 316 //
benson516 0:33b75d52d2a7 317 Get_VectorScaleUp(M[i], ratio);
benson516 0:33b75d52d2a7 318 Get_VectorScaleUp(M_inv[i], ratio);
benson516 0:33b75d52d2a7 319 //
benson516 0:33b75d52d2a7 320 // M[i] = Get_VectorScalarMultiply(M[i], ratio);
benson516 0:33b75d52d2a7 321 // M_inv[i] = Get_VectorScalarMultiply(M_inv[i], ratio);
benson516 0:33b75d52d2a7 322 //
benson516 0:33b75d52d2a7 323 }
benson516 0:33b75d52d2a7 324 */
benson516 0:33b75d52d2a7 325
benson516 0:33b75d52d2a7 326 /*
benson516 0:33b75d52d2a7 327 //
benson516 0:33b75d52d2a7 328 cout << "M:\n";
benson516 0:33b75d52d2a7 329 printMatrix(M);
benson516 0:33b75d52d2a7 330 //
benson516 0:33b75d52d2a7 331 cout << "M_inv:\n";
benson516 0:33b75d52d2a7 332 printMatrix(M_inv);
benson516 0:33b75d52d2a7 333 //
benson516 0:33b75d52d2a7 334 */
benson516 0:33b75d52d2a7 335
benson516 0:33b75d52d2a7 336 //
benson516 0:33b75d52d2a7 337 // The flag for singularity
benson516 0:33b75d52d2a7 338 bool is_singular = false;
benson516 0:33b75d52d2a7 339 //
benson516 0:33b75d52d2a7 340
benson516 0:33b75d52d2a7 341 // Eliminate all the other elements except those on the diagonal
benson516 0:33b75d52d2a7 342 for (size_t i = 0; i < n; ++i){ // For each element on diagonal
benson516 0:33b75d52d2a7 343 float ratio;
benson516 0:33b75d52d2a7 344
benson516 0:33b75d52d2a7 345 // For solving the problem of that diagonal element is zero
benson516 0:33b75d52d2a7 346 if (M[i][i] == 0.0){
benson516 0:33b75d52d2a7 347 is_singular |= SolveSingularityOnDiag(M, M_inv, i);
benson516 0:33b75d52d2a7 348 }
benson516 0:33b75d52d2a7 349
benson516 0:33b75d52d2a7 350 //
benson516 0:33b75d52d2a7 351 if (is_singular){
benson516 0:33b75d52d2a7 352 // The matrix is singular,
benson516 0:33b75d52d2a7 353 // stop this meaningless process and return.
benson516 0:33b75d52d2a7 354 // break;
benson516 0:33b75d52d2a7 355 return M_inv;
benson516 0:33b75d52d2a7 356 }
benson516 0:33b75d52d2a7 357
benson516 0:33b75d52d2a7 358 // Normalize the diagonal element in M to 1.0
benson516 0:33b75d52d2a7 359 if (M[i][i] != 1.0){
benson516 0:33b75d52d2a7 360 ratio = 1.0/M[i][i];
benson516 0:33b75d52d2a7 361 Get_VectorScaleUp(M[i], ratio);
benson516 0:33b75d52d2a7 362 Get_VectorScaleUp(M_inv[i], ratio);
benson516 0:33b75d52d2a7 363 }
benson516 0:33b75d52d2a7 364 //
benson516 0:33b75d52d2a7 365
benson516 0:33b75d52d2a7 366 //
benson516 0:33b75d52d2a7 367 row_left = M[i];
benson516 0:33b75d52d2a7 368 row_right = M_inv[i];
benson516 0:33b75d52d2a7 369 //
benson516 0:33b75d52d2a7 370 // M_diag = M[i][i]; // This has been normalized, which is 1.0
benson516 0:33b75d52d2a7 371 //
benson516 0:33b75d52d2a7 372 for (size_t j = 0; j < n; ++j){ // For the row other than than the current row
benson516 0:33b75d52d2a7 373 if (j != i){ // Not going to do anything with that row itself
benson516 0:33b75d52d2a7 374 //
benson516 0:33b75d52d2a7 375 // ratio = M[j][i]/M_diag;
benson516 0:33b75d52d2a7 376 ratio = M[j][i]; // Because M_diag = 1.0
benson516 0:33b75d52d2a7 377 Get_VectorIncrement(M[j], Get_VectorScalarMultiply(row_left, ratio), true); // -=
benson516 0:33b75d52d2a7 378 Get_VectorIncrement(M_inv[j], Get_VectorScalarMultiply(row_right, ratio), true); // -=
benson516 0:33b75d52d2a7 379 //
benson516 0:33b75d52d2a7 380 /*
benson516 0:33b75d52d2a7 381 Get_VectorIncrement(M[j], Get_VectorScalarMultiply(M[i], ratio), true); // -=
benson516 0:33b75d52d2a7 382 Get_VectorIncrement(M_inv[j], Get_VectorScalarMultiply(M_inv[i], ratio), true); // -=
benson516 0:33b75d52d2a7 383 */
benson516 0:33b75d52d2a7 384 }
benson516 0:33b75d52d2a7 385 }
benson516 0:33b75d52d2a7 386
benson516 0:33b75d52d2a7 387 /*
benson516 0:33b75d52d2a7 388 //
benson516 0:33b75d52d2a7 389 cout << "Diagonal element i = " << i << "\n";
benson516 0:33b75d52d2a7 390 //
benson516 0:33b75d52d2a7 391 cout << "M:\n";
benson516 0:33b75d52d2a7 392 printMatrix(M);
benson516 0:33b75d52d2a7 393 //
benson516 0:33b75d52d2a7 394 cout << "M_inv:\n";
benson516 0:33b75d52d2a7 395 printMatrix(M_inv);
benson516 0:33b75d52d2a7 396 */
benson516 0:33b75d52d2a7 397 }
benson516 0:33b75d52d2a7 398 return M_inv;
benson516 0:33b75d52d2a7 399
benson516 0:33b75d52d2a7 400 }
benson516 0:33b75d52d2a7 401 ///////////////////////////////////////// end Matrix inversion, using Gauss method
benson516 0:33b75d52d2a7 402
benson516 0:33b75d52d2a7 403
benson516 0:33b75d52d2a7 404 }
benson516 0:33b75d52d2a7 405 ////////////////////////// end Namespace MATRIX_PRIMITIVE