Matrix.cpp

00001 #include "myConstants.h"
00002 #include "Matrix.h"
00003 
00004 
00005 
00006 Matrix::Matrix(int row, int col) : row(row), col(col), components(0) {
00007     components = new float[row*col];
00008     if (!components) error("Memory Allocation Error");
00009     for(int i=0; i<row*col; i++) components[i] = 0.0f;
00010     if (row == col) {
00011         for (int i = 0; i < row; i++) {
00012             components[i * col + i] = 1.0f;
00013         }
00014     }
00015 }
00016 
00017 Matrix::Matrix(int row, int col, float* comps) : row(row), col(col), components(0) {
00018     components = new float[row*col];
00019     if (!components) error("Memory Allocation Error");
00020     memcpy(components, comps, sizeof(float)*row*col);
00021 }
00022 
00023 
00024 Matrix::~Matrix() {
00025     delete[] components;
00026 }
00027 
00028 Matrix::Matrix(const Matrix& m) : row(m.row), col(m.col), components(0) {
00029     components = new float[row*col];
00030     if (!components) error("Memory Allocation Error");
00031     memcpy(components, m.GetpComponents(), sizeof(float)*row*col);
00032 }
00033 
00034 Matrix Matrix::operator-() const{
00035     Matrix retMat(*this);
00036 
00037     for (int i = 0; i < row * col; i++) {
00038         retMat.components[i] = - this->components[i];
00039     }
00040 
00041     return retMat;
00042 }
00043 
00044 Matrix& Matrix::operator=(const Matrix& m) {
00045     if (this == &m) return *this;
00046     row = m.row;
00047     col = m.col;
00048     delete[] components;
00049     components = new float[row*col];
00050     if (!components) error("Memory Allocation Error");
00051     memcpy(components, m.GetpComponents(), sizeof(float)*row*col);
00052 
00053     return *this;
00054 }
00055 
00056 Matrix& Matrix::operator+=(const Matrix& m) {
00057     if (row != m.GetRow() || col != m.GetCol()) error("Irregular Dimention");
00058     
00059     for (int i = 0; i < row; i++) {
00060         for (int j = 0; j < col; j++) {
00061             components[i * col + j] += m.components[i * col + j];
00062         }
00063     }
00064 
00065     this->CleanUp();
00066 
00067     return *this;
00068 }
00069 
00070 Matrix& Matrix::operator-=(const Matrix& m) {
00071     if (row != m.GetRow() || col != m.GetCol()) error("Irregular Dimention");
00072 
00073     for (int i = 0; i < row; i++) {
00074         for (int j = 0; j < col; j++) {
00075             components[i * col + j] -= m.components[i * col + j];
00076         }
00077     }
00078 
00079     this->CleanUp();
00080 
00081     return *this;
00082 }
00083 /*
00084 Matrix& Matrix::operator*=(const Matrix& m) {
00085     if (col != m.GetRow()) error("Irregular Dimention");
00086     Matrix temp = Matrix(*this);
00087     
00088     col = m.GetCol();
00089     delete[] components;
00090     components = new float[row*col];
00091 
00092     for (int i = 0; i < row; i++) {
00093         for (int j = 0; j < col; j++) {
00094             components[i*col + j] = 0.0f;
00095             for (int k = 0; k < m.GetRow(); k++) {
00096                 components[i * col + j] += temp.components[i * col + k] * m.components[k * col + j];
00097             }
00098         }
00099     }
00100 
00101     this->CleanUp();
00102 
00103     return *this;
00104 }
00105 */
00106 
00107 Matrix& Matrix::operator*=(float c) {
00108     for (int i = 0; i < row; i++) {
00109         for (int j = 0; j < col; j++) {
00110             components[i*col + j] *= c;
00111         }
00112     }
00113 
00114     return *this;
00115 }
00116 
00117 Matrix& Matrix::operator/=(float c) {
00118     if (fabs(c) < NEARLY_ZERO) error("Division by Zero");
00119     for (int i = 0; i < row; i++) {
00120         for (int j = 0; j < col; j++) {
00121             components[i*col + j] /= c;
00122         }
00123     }
00124 
00125     return *this;
00126 }
00127 
00128 void Matrix::SetComp(int rowNo, int colNo, float val) {
00129     if (rowNo > row || colNo > col) error("Index Out of Bounds Error");
00130     components[(rowNo-1)*col + (colNo-1)] = val;
00131 }
00132 
00133 void Matrix::SetComps(float* pComps) {
00134     memcpy(components, pComps, sizeof(float) * row * col);
00135 }
00136 
00137 float Matrix::Determinant() const{
00138     if (row != col) error("failed to calculate det. : matrix is not square");
00139     int decSign = 0;
00140     float retVal = 1.0f;
00141 
00142     // 行列のLU分解
00143     Matrix LU(this->LU_Decompose(&decSign));
00144 
00145     for (int i = 0; i < LU.row; i++) {
00146         retVal *= LU.components[i * LU.col + i];
00147     }
00148 
00149     return retVal*decSign;
00150 }
00151 
00152 float Matrix::det() const {
00153     if (row != col) error("failed to calculate det : matrix is not square");
00154     
00155     Matrix temp(*this);
00156     int decSign = 1;
00157 
00158     for (int j = 0; j < col - 1; j++) {
00159 
00160         // 列内のみで最大の要素を探す
00161         int maxNo = j;
00162         for (int k = j; k < row; k++) {
00163             if (temp.components[maxNo * col + j] < temp.components[k * col + j]) maxNo = k;
00164         }
00165         if (maxNo != j) {
00166             temp.SwapRow(j + 1, maxNo + 1);
00167             decSign *= -1;
00168         }
00169         // 列内の最大要素が小さ過ぎる場合、行内の最大要素も探す
00170         if (fabs(temp.components[j * col + j]) < NEARLY_ZERO) {
00171             maxNo = j;
00172             for (int k = j; k < col; k++) {
00173                 if (temp.components[j * col + maxNo] < temp.components[j * col + k])maxNo = k;
00174             }
00175             if (maxNo != j) {
00176                 temp.SwapCol(j + 1, maxNo + 1);
00177                 decSign *= -1;
00178             }
00179 
00180             // 列内、行内の最大要素を選んでも小さすぎる場合はエラー
00181             if (fabs(temp.components[j * col + j]) < NEARLY_ZERO) {
00182                 if (row != col) error("failed to calculate det : Division by Zero");
00183             }
00184         }
00185 
00186         float c1 = 1.0f / temp.components[j * col + j];
00187 
00188         for (int i = j + 1; i < row; i++) {
00189             float c2 = temp.components[i * col + j] * c1;
00190             for (int k = j; k < col; k++) {
00191                 temp.components[i * col + k] = temp.components[i * col + k] - c2 * temp.components[j * col + k];
00192             }
00193         }
00194         
00195     }
00196 
00197     if (fabs(temp.components[(row - 1) * col + (col - 1)]) < NEARLY_ZERO) return 0.0f;
00198 
00199     float retVal = 1.0f;
00200     for (int i = 0; i < row; i++) {
00201         retVal *= temp.components[i * col + i];
00202     }
00203 
00204     return retVal * decSign;
00205 }
00206 
00207 Matrix Matrix::LU_Decompose(int* sign, Matrix* p) const{
00208     if (row != col) error("failed to LU decomposition: matrix is not square");
00209     if (sign != 0) *sign = 1;
00210     if (p != 0) {
00211         if (p->row != row || p->row != p->col) error("failed to LU decomposition: permitation matrix is incorrect");
00212         // 置換行列は最初に単位行列にしておく
00213         memset(p->components, 0, sizeof(float) * row * col);
00214         for (int i = 0; i < row; i++) {
00215             p->components[i * col + i] = 1.0f;
00216         }
00217     }
00218     Matrix retVal(*this);
00219 
00220     for (int d = 0; d < row - 1; d++) { // 1行1列ずつ分解を行う
00221         // d列目の最大の要素を探索し、見つけた要素の行とd行目を交換する
00222         int maxNo = d;
00223         for (int i = d; i < row; i++) {
00224             if (retVal.components[i * col + d] > retVal.components[maxNo * col + d]) maxNo = i;
00225         }
00226         if (maxNo != d) {
00227             retVal.SwapRow(d + 1, maxNo + 1);
00228             if (sign != 0) *sign *= -1;
00229             if (p != 0) {
00230                 p->SwapRow(d + 1, maxNo + 1);
00231             }
00232         }
00233         float c = retVal.components[d * col + d];
00234         if (fabs(c) < NEARLY_ZERO) error("failed to LU decomposition: Division by Zero");
00235 
00236         // d行d列目以降の行列について計算
00237         for (int i = d+1; i < row; i++) {
00238             retVal.components[i * col + d] /= c;
00239             for (int j = d+1; j < col; j++) {
00240                 retVal.components[i * col + j] -= retVal.components[d * col + j] * retVal.components[i * col + d];
00241             }
00242         }
00243     }
00244 
00245     retVal.CleanUp();
00246 
00247     return retVal;
00248 }
00249 
00250 float Matrix::Inverse(Matrix& invm) const{
00251     if (row != col) error("failed to get Inv. : matrix is not square");
00252 
00253     Matrix P(*this);
00254     Matrix LU(LU_Decompose(0, &P));
00255 
00256     // 分解した行列の対角成分の積から行列式を求める
00257     // det = 0　ならfalse
00258     float det = 1.0f;
00259     for (int i = 0; i < row; i++) {
00260         det *= LU.components[i * col + i];
00261     }
00262     if (fabs(det) < NEARLY_ZERO) {
00263         return fabs(det);
00264     }
00265 
00266     // U、Lそれぞれの逆行列を計算する
00267     Matrix U_inv = Matrix(row, col);
00268     Matrix L_inv = Matrix(row, col);
00269 
00270     for (int j = 0; j < col; j++) {
00271         for (int i = 0; i <= j; i++) {
00272             int i_U = j - i;        // U行列の逆行列は対角成分から上へ向かって
00273                                     // 左から順番に値を計算する
00274 
00275             int j_L = col - 1 - j;  // L行列の逆行列は右から順番に
00276             int i_L = j_L + i;      // 対角成分から下へ向かって計算する
00277 
00278             if (i_U != j) { // 非対角成分
00279                 float temp_U = 0.0f;
00280                 float temp_L = 0.0f;
00281 
00282                 for (int k = 0; k < i; k++) {
00283 
00284                     temp_U -= U_inv.components[(j - k) * col + j] * LU.components[i_U * col + (j - k)];
00285                     
00286                     if (k == 0) {
00287                         temp_L -= LU.components[i_L * col + j_L];
00288                     } else {
00289                         temp_L -= L_inv.components[(j_L + k) * col + j_L] * LU.components[i_L * col + j_L + k];
00290                     }
00291                     
00292                 }
00293 
00294                 U_inv.components[i_U * col + j] = temp_U / LU.components[i_U * col + i_U];
00295                 L_inv.components[i_L * col + j_L] = temp_L;
00296 
00297             } else {    // 対角成分
00298                 if (fabs(LU.components[i_U * col + i_U]) >= NEARLY_ZERO) {
00299                     U_inv.components[i_U * col + i_U] = 1.0f / LU.components[i_U * col + i_U];
00300                 }
00301             }
00302         }
00303     }
00304 
00305     invm = U_inv * L_inv * P;
00306 
00307     return -1.0f;
00308 }
00309 
00310 Matrix Matrix::Transpose() const{
00311     //if (row != col) error("failed to get Trans. : matrix is not square");
00312     Matrix retVal(col, row);
00313 
00314     for (int i = 0; i < row; i++) {
00315         for (int j = 0; j < col; j++) {
00316             retVal.components[j * row + i] = this->components[i * col + j];        
00317         }
00318     }
00319 
00320     return retVal;
00321 }
00322 
00323 Matrix operator+(const Matrix& lhm, const Matrix& rhm) {
00324     Matrix temp = Matrix(lhm);
00325     temp += rhm;
00326     return temp;
00327 }
00328 
00329 Matrix operator-(const Matrix& lhm, const Matrix& rhm) {
00330     Matrix temp = Matrix(lhm);
00331     temp -= rhm;
00332     return temp;
00333 }
00334 
00335 Matrix operator*(const Matrix& lhm, const Matrix& rhm) {
00336     if(lhm.GetCol() != rhm.GetRow()) error("Matrix product Error: Irregular Dimention.");
00337     int row = lhm.GetRow();
00338     int col = rhm.GetCol();
00339     int sum = lhm.GetCol();
00340     Matrix temp(row, col);
00341     
00342     for (int i = 1; i <= row; i++) {
00343         for (int j = 1; j <= col; j++) {
00344             float temp_c = 0.0f;
00345             for (int k = 1; k <= sum; k++) {
00346                 temp_c += lhm.GetComp(i, k) * rhm.GetComp(k, j);
00347             }
00348             temp.SetComp(i, j, temp_c);
00349         }
00350     }
00351     
00352     return temp;
00353 }
00354 
00355 void Matrix::CleanUp() {
00356     int num = row*col;
00357     float maxComp = 0.0f;
00358     for (int i = 0; i < num; i++) {
00359         if (maxComp < fabs(components[i])) maxComp = fabs(components[i]);
00360     }
00361     if (maxComp > NEARLY_ZERO) {
00362         for (int i = 0; i < num; i++) {
00363             if (fabs(components[i]) / maxComp < ZERO_TOLERANCE) components[i] = 0.0f;
00364         }
00365     }
00366 }
00367 
00368 void Matrix::SwapRow(int rowNo1, int rowNo2) {
00369     if (rowNo1 > row || rowNo2 > row) error("Index Out of Bounds Error !!");
00370     float* temp = new float[col];
00371 
00372     memcpy(temp, components + (rowNo1 - 1) * col, sizeof(float) * col);
00373     memcpy(components + (rowNo1 - 1) * col, components + (rowNo2 - 1) * col, sizeof(float) * col);
00374     memcpy(components + (rowNo2 - 1) * col, temp, sizeof(float) * col);
00375 
00376     delete[] temp;
00377 }
00378 
00379 void Matrix::SwapCol(int colNo1, int colNo2) {
00380     if (colNo1 > col || colNo2 > col) error("Index Out of Bounds Error !!");
00381     float temp = 0.0f;
00382 
00383     for (int i = 0; i < row; i++) {
00384         temp = components[i * col + colNo1];
00385         components[i * col + colNo1] = components[i * col + colNo2];
00386         components[i * col + colNo2] = temp;
00387     }
00388 }