File content as of revision 3:40559ebef0f1:

#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 retMat(*this);

    for (int i = 0; i < row * col; i++) {
        retMat.components[i] = - this->components[i];
    }

    return retMat;
}

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;
}

float Matrix::det() const {
    if (row != col) AbortWithMsg("failed to calculate det : matrix is not square");
    
    Matrix temp(*this);
    int decSign = 1;

    for (int j = 0; j < col - 1; j++) {

        // 列内のみで最大の要素を探す
        int maxNo = j;
        for (int k = j; k < row; k++) {
            if (temp.components[maxNo * col + j] < temp.components[k * col + j]) maxNo = k;
        }
        if (maxNo != j) {
            temp.SwapRow(j + 1, maxNo + 1);
            decSign *= -1;
        }
        // 列内の最大要素が小さ過ぎる場合、行内の最大要素も探す
        if (fabs(temp.components[j * col + j]) < NEARLY_ZERO) {
            maxNo = j;
            for (int k = j; k < col; k++) {
                if (temp.components[j * col + maxNo] < temp.components[j * col + k])maxNo = k;
            }
            if (maxNo != j) {
                temp.SwapCol(j + 1, maxNo + 1);
                decSign *= -1;
            }

            // 列内、行内の最大要素を選んでも小さすぎる場合はエラー
            if (fabs(temp.components[j * col + j]) < NEARLY_ZERO) {
                if (row != col) AbortWithMsg("failed to calculate det : Division by Zero");
            }
        }

        float c1 = 1.0f / temp.components[j * col + j];

        for (int i = j + 1; i < row; i++) {
            float c2 = temp.components[i * col + j] * c1;
            for (int k = j; k < col; k++) {
                temp.components[i * col + k] = temp.components[i * col + k] - c2 * temp.components[j * col + k];
            }
        }
        
    }

    if (fabs(temp.components[(row - 1) * col + (col - 1)]) < NEARLY_ZERO) return 0.0f;

    float retVal = 1.0f;
    for (int i = 0; i < row; i++) {
        retVal *= temp.components[i * 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;
}

void Matrix::SwapCol(int colNo1, int colNo2) {
    if (colNo1 > col || colNo2 > col) AbortWithMsg("Index Out of Bounds Error !!");
    float temp = 0.0f;

    for (int i = 0; i < row; i++) {
        temp = components[i * col + colNo1];
        components[i * col + colNo1] = components[i * col + colNo2];
        components[i * col + colNo2] = temp;
    }
}