Diff: Matrix.cpp

Revision:

1:2eca9b376580

Child:

2:4a6b46653abf

diff -r 5e220b09d315 -r 2eca9b376580 Matrix.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Matrix.cpp	Thu Apr 16 08:51:04 2015 +0000
@@ -0,0 +1,298 @@
+#include "mbed.h"
+#include "myConstants.h"
+#include "Matrix.h"
+
+
+
+Matrix::Matrix(int row, int col) : row(row), col(col), components(0) {
+    components = new float[row*col];
+    if (!components) AbortWithMsg("Memory Allocation Error");
+    memset(components, 0, sizeof(float)*row*col);
+    if (row == col) {
+        for (int i = 0; i < row; i++) {
+            components[i * col + i] = 1.0f;
+        }
+    }
+}
+
+Matrix::Matrix(int row, int col, float* comps) : row(row), col(col), components(0) {
+    components = new float[row*col];
+    if (!components) AbortWithMsg("Memory Allocation Error");
+    memcpy(components, comps, sizeof(float)*row*col);
+}
+
+
+Matrix::~Matrix() {
+    delete[] components;
+}
+
+Matrix::Matrix(const Matrix& m) : row(m.row), col(m.col), components(0) {
+    components = new float[row*col];
+    if (!components) AbortWithMsg("Memory Allocation Error");
+    memcpy(components, m.GetpComponents(), sizeof(float)*row*col);
+}
+
+Matrix& Matrix::operator=(const Matrix& m) {
+    if (this == &m) return *this;
+    row = m.row;
+    col = m.col;
+    delete[] components;
+    components = new float[row*col];
+    if (!components) AbortWithMsg("Memory Allocation Error");
+    memcpy(components, m.GetpComponents(), sizeof(float)*row*col);
+
+    return *this;
+}
+
+Matrix& Matrix::operator+=(const Matrix& m) {
+    if (row != m.GetRow() || col != m.GetCol()) AbortWithMsg("Irregular Dimention");
+    
+    for (int i = 0; i < row; i++) {
+        for (int j = 0; j < col; j++) {
+            components[i * col + j] += m.components[i * col + j];
+        }
+    }
+
+    this->CleanUp();
+
+    return *this;
+}
+
+Matrix& Matrix::operator-=(const Matrix& m) {
+    if (row != m.GetRow() || col != m.GetCol()) AbortWithMsg("Irregular Dimention");
+
+    for (int i = 0; i < row; i++) {
+        for (int j = 0; j < col; j++) {
+            components[i * col + j] -= m.components[i * col + j];
+        }
+    }
+
+    this->CleanUp();
+
+    return *this;
+}
+
+Matrix& Matrix::operator*=(const Matrix& m) {
+    if (col != m.GetRow()) AbortWithMsg("Irregular Dimention");
+    Matrix temp = Matrix(*this);
+    
+    col = m.GetCol();
+    delete[] components;
+    components = new float[row*col];
+
+    for (int i = 0; i < row; i++) {
+        for (int j = 0; j < col; j++) {
+            components[i*col + j] = 0.0f;
+            for (int k = 0; k < m.GetRow(); k++) {
+                components[i * col + j] += temp.components[i * col + k] * m.components[k * col + j];
+            }
+        }
+    }
+
+    this->CleanUp();
+
+    return *this;
+}
+
+Matrix& Matrix::operator*=(float c) {
+    for (int i = 0; i < row; i++) {
+        for (int j = 0; j < col; j++) {
+            components[i*col + j] *= c;
+        }
+    }
+
+    return *this;
+}
+
+Matrix& Matrix::operator/=(float c) {
+    if (fabs(c) < NEARLY_ZERO) AbortWithMsg("Division by Zero");
+    for (int i = 0; i < row; i++) {
+        for (int j = 0; j < col; j++) {
+            components[i*col + j] /= c;
+        }
+    }
+
+    return *this;
+}
+
+void Matrix::SetComp(int rowNo, int colNo, float val) {
+    if (rowNo > row || colNo > col) AbortWithMsg("Index Out of Bounds Error");
+    components[(rowNo-1)*col + (colNo-1)] = val;
+}
+
+void Matrix::SetComps(float* pComps) {
+    memcpy(components, pComps, sizeof(float) * row * col);
+}
+
+float Matrix::Determinant() const{
+    if (row != col) AbortWithMsg("failed to calculate det. : matrix is not square");
+    int decSign = 0;
+    float retVal = 1.0f;
+
+    // 行列のLU分解
+    Matrix LU(this->LU_Decompose(&decSign));
+
+    for (int i = 0; i < LU.row; i++) {
+        retVal *= LU.components[i * LU.col + i];
+    }
+
+    return retVal*decSign;
+}
+
+Matrix Matrix::LU_Decompose(int* sign, Matrix* p) const{
+    if (row != col) AbortWithMsg("failed to LU decomposition: matrix is not square");
+    if (sign != 0) *sign = 1;
+    if (p != 0) {
+        if (p->row != row || p->row != p->col) AbortWithMsg("failed to LU decomposition: permitation matrix is incorrect");
+        // 置換行列は最初に単位行列にしておく
+        memset(p->components, 0, sizeof(float) * row * col);
+        for (int i = 0; i < row; i++) {
+            p->components[i * col + i] = 1.0f;
+        }
+    }
+    Matrix retVal(*this);
+
+    for (int d = 0; d < row - 1; d++) { // 1行1列ずつ分解を行う
+        // d列目の最大の要素を探索し、見つけた要素の行とd行目を交換する
+        int maxNo = d;
+        for (int i = d; i < row; i++) {
+            if (retVal.components[i * col + d] > retVal.components[maxNo * col + d]) maxNo = i;
+        }
+        if (maxNo != d) {
+            retVal.SwapRow(d + 1, maxNo + 1);
+            if (sign != 0) *sign *= -1;
+            if (p != 0) {
+                p->SwapRow(d + 1, maxNo + 1);
+            }
+        }
+        float c = retVal.components[d * col + d];
+        if (fabs(c) < NEARLY_ZERO) AbortWithMsg("failed to LU decomposition: Division by Zero");
+
+        // d行d列目以降の行列について計算
+        for (int i = d+1; i < row; i++) {
+            retVal.components[i * col + d] /= c;
+            for (int j = d+1; j < col; j++) {
+                retVal.components[i * col + j] -= retVal.components[d * col + j] * retVal.components[i * col + d];
+            }
+        }
+    }
+
+    retVal.CleanUp();
+
+    return retVal;
+}
+
+bool Matrix::Inverse(Matrix& invm) const{
+    if (row != col) AbortWithMsg("failed to get Inv. : matrix is not square");
+
+    Matrix P(*this);
+    Matrix LU(LU_Decompose(0, &P));
+
+    // 分解した行列の対角成分の積から行列式を求める
+    // det = 0　ならfalse
+    float det = 1.0f;
+    for (int i = 0; i < row; i++) {
+        det *= LU.components[i * col + i];
+    }
+    if (fabs(det) < NEARLY_ZERO) return false;
+
+    // U、Lそれぞれの逆行列を計算する
+    Matrix U_inv = Matrix(row, col);
+    Matrix L_inv = Matrix(row, col);
+
+    for (int j = 0; j < col; j++) {
+        for (int i = 0; i <= j; i++) {
+            int i_U = j - i;        // U行列の逆行列は対角成分から上へ向かって
+                                    // 左から順番に値を計算する
+
+            int j_L = col - 1 - j;  // L行列の逆行列は右から順番に
+            int i_L = j_L + i;      // 対角成分から下へ向かって計算する
+
+            if (i_U != j) { // 非対角成分
+                float temp_U = 0.0f;
+                float temp_L = 0.0f;
+
+                for (int k = 0; k < i; k++) {
+
+                    temp_U -= U_inv.components[(j - k) * col + j] * LU.components[i_U * col + (j - k)];
+                    
+                    if (k == 0) {
+                        temp_L -= LU.components[i_L * col + j_L];
+                    } else {
+                        temp_L -= L_inv.components[(j_L + k) * col + j_L] * LU.components[i_L * col + j_L + k];
+                    }
+                    
+                }
+
+                U_inv.components[i_U * col + j] = temp_U / LU.components[i_U * col + i_U];
+                L_inv.components[i_L * col + j_L] = temp_L;
+
+            } else {    // 対角成分
+                if (fabs(LU.components[i_U * col + i_U]) >= NEARLY_ZERO) {
+                    U_inv.components[i_U * col + i_U] = 1.0f / LU.components[i_U * col + i_U];
+                }
+            }
+        }
+    }
+
+    invm = U_inv * L_inv * P;
+
+    return true;
+}
+
+Matrix Matrix::Transpose() const{
+    if (row != col) AbortWithMsg("failed to get Trans. : matrix is not square");
+    Matrix retVal(*this);
+
+    for (int i = 0; i < row; i++) {
+        for (int j = i + 1; j < col; j++) {
+            float temp = retVal.components[i * col + j];
+            retVal.components[i * col + j] = retVal.components[j * col + i];
+            retVal.components[j * col + i] = temp;            
+        }
+    }
+
+    return retVal;
+}
+
+Matrix operator+(const Matrix& lhm, const Matrix& rhm) {
+    Matrix temp = Matrix(lhm);
+    temp += rhm;
+    return temp;
+}
+
+Matrix operator-(const Matrix& lhm, const Matrix& rhm) {
+    Matrix temp = Matrix(lhm);
+    temp -= rhm;
+    return temp;
+}
+
+Matrix operator*(const Matrix& lhm, const Matrix& rhm) {
+    Matrix temp = Matrix(lhm);
+    temp *= rhm;
+    return temp;
+}
+
+void Matrix::CleanUp() {
+    int num = row*col;
+    float maxComp = 0.0f;
+    for (int i = 0; i < num; i++) {
+        if (maxComp < fabs(components[i])) maxComp = fabs(components[i]);
+    }
+    if (maxComp > NEARLY_ZERO) {
+        for (int i = 0; i < num; i++) {
+            if (fabs(components[i]) / maxComp < ZERO_TOLERANCE) components[i] = 0.0f;
+        }
+    }
+}
+
+void Matrix::SwapRow(int rowNo1, int rowNo2) {
+    if (rowNo1 > row || rowNo2 > row) AbortWithMsg("Index Out of Bounds Error !!");
+    float* temp = new float[col];
+
+    memcpy(temp, components + (rowNo1 - 1) * col, sizeof(float) * col);
+    memcpy(components + (rowNo1 - 1) * col, components + (rowNo2 - 1) * col, sizeof(float) * col);
+    memcpy(components + (rowNo2 - 1) * col, temp, sizeof(float) * col);
+
+    delete[] temp;
+}
\ No newline at end of file