Matrix.cpp

#include "Matrix.h"


#include "Vector.h"


using namespace Robotics;
using namespace System;
using namespace System::Diagnostics;


/// 指定された大きさで生成し 零行列で初期化する [ Constructor ]
Matrix::Matrix( int row, int column )
{
    // 領域を確保する
    SecureDomain( row );

    for( int m=0; m<row; m++ )
    {
        // ... 空のベクトルを生成する
        m_rowVectors[ m ] = gcnew Vector( column );
    }

#ifdef _DEBUG
    for( int m=0; m<Row; m++ )
    {
        Debug::Assert(
            m_rowVectors[ m ]->IsZeroVector(),
            "零行列に初期化されている"
        );
    }

    /// @note
    /// 零行列の判定を行う関数は、比較用に零行列を作成するとき
    /// このコンストラクタを呼び出す。よって、ここでその関数を呼び出すと
    /// 再帰してしまう。

#endif
}

/// コピー元と同じ大きさで生成し それの行ベクトルで初期化する [ Copy constructor ]
Matrix::Matrix( Matrix^ other )
{
    // 領域を確保する
    SecureDomain( other->Row );

    // 列数(行ベクトルの次元)を設定する
    for( int m=0; m < other->Row; m++ )
    {
        // ... 行ベクトルのコピーで生成する
        m_rowVectors[ m ] = gcnew Vector( other->m_rowVectors[ m ] );
    }
}


/// 指定された行数で領域を確保する
void Matrix::SecureDomain( int row )
{
    /// @note
    /// ここでは、行ベクトルの領域を確保するだけである。
    /// この後に行ベクトルのインスタンスを生成する必要がある。


    Debug::Assert( 1 <= row , "行数は１以上である" );


    // メモリ領域を行ベクトルとして確保する
    m_rowVectors = gcnew array< Vector^ >( row );


    /// @note
    /// C++ の文法上、配列の初期化でデフォルトコンストラクタ以外を
    /// 呼び出すことはできないため、行と列を分けて初期化している。
}

/// 行列の大きさを設定する
void Matrix::SetSize( int row, int column )
{
    Debug::Assert( ( row != Row ) || ( column != Column ), "行数または列数は変更される" );


    // 領域を解放する
    delete m_rowVectors;


    // ... 領域を確保する
    SecureDomain( row );

    for( int m=0; m<row; m++ )
    {
        // ... 空のベクトルを生成する
        m_rowVectors[ m ] = gcnew Vector( column );
    }
}


/// Indexer [ Property ]
Vector^ Matrix::default::get( int index )
{
    Debug::Assert( ( 0 <= index ) && ( index <= Row ), "添え字は行数の範囲内である" );

    // 添え字を１からとするために、補正して指定する
    return m_rowVectors[ index - 1 ];
}

/// 列数を取得する [ Property ]
int Matrix::Column::get()
{
    // 行ベクトルの次元数を取得する
    return m_rowVectors[ 0 ]->Dimension;
}

/// 和 += [ Operator Overloading ]
Matrix^ Matrix::operator+=( Matrix^ obj )
{
    Debug::Assert( IsEqualSize( obj ), "引数の行列と大きさは等しい" );


    for( int m=0; m<Row; m++ )
    {
        m_rowVectors[ m ] += obj->m_rowVectors[ m ];
    }
    return this;
}

/// 差 -= [ Operator Overloading ]
Matrix^ Matrix::operator-=( Matrix^ obj )
{
    Debug::Assert( IsEqualSize( obj ), "引数の行列と大きさは等しい" );


    for( int m=0; m<Row; m++ )
    {
        m_rowVectors[ m ] -= obj->m_rowVectors[ m ];
    }
    return this;
}

/// スカラー積 *= [ Operator Overloading ]
Matrix^ Matrix::operator*=( double val )
{
    for(int m=0; m<Row; m++ )
    {
        m_rowVectors[ m ] *= val;
    }
    return this;
}

/// 行列積 *= [ Operator Overloading ]
Matrix^ Matrix::operator*=( Matrix^ obj )
{
    // 行列積 (operator*)を呼び出す
    Matrix^ result = this * obj;

    for( int m=0; m<Row; m++ )
    {
        // 行ベクトルを、インスタンスとしてコピーする
        m_rowVectors[ m ] = gcnew Vector( result->m_rowVectors[ m ] );
    }

    return this;
}

/// 行列の和 [ Operator Overloading ]
Matrix^ Matrix::operator+( Matrix^ left, Matrix^ right )
{
    Matrix^ result = gcnew Matrix( left );
    result += right;

    return result;
}

/// 行列の差 [ Operator Overloading ]
Matrix^ Matrix::operator-( Matrix^ left, Matrix^ right )
{
    Matrix^ result = gcnew Matrix( left );
    result -= right;

    return result;
}


/// 行列とベクトルの積 [ Operator Overloading ]
Vector^ Matrix::operator*( Matrix^ matrix, Vector^ vector )
{
    /// @note
    /// ここでは、行ベクトルを列ベクトルと見なして計算している


    Debug::Assert(
        matrix->Column == vector->Dimension,
        "行列の列数とベクトルの次数は等しい"
    );


    Vector^ result = gcnew Vector( matrix->Row );

    for( int m=1; m<=matrix->Row; m++ )
    {
        // 行ごとに、ベクトルの成分との積の総和を求める
        double sum = 0.0;

        for( int n=1; n<=matrix->Column; n++ )
        {
            sum += matrix[ m ][ n ] * vector[ n ];
        }
        result[ m ] = sum;
    }

    // 誤差成分を除去する
    result->RemoveErrorComponent();

    return result;
}

/// ベクトルと行列の積 [ Operator Overloading ]
Vector^ Matrix::operator*( Vector^ vector, Matrix^ matrix )
{
    Debug::Assert(
        vector->Dimension == matrix->Column,
        "ベクトルの次数と行列の列数は等しい"
    );


    Vector^ result = gcnew Vector( matrix->Column );

    for( int n=1; n<=matrix->Column; n++ )
    {
        // 列ごとに、ベクトルの成分との積の総和を求める
        double sum = 0.0;

        for( int m=1; m<=matrix->Row; m++ )
        {
            sum += vector[ m ] * matrix[ m ][ n ];
        }
        result[ n ] = sum;
    }

    // 誤差成分を除去する
    result->RemoveErrorComponent();

    return result;
}

/// 行列積 [ Operator Overloading ]
Matrix^ Matrix::operator*( Matrix^ left, Matrix^ right )
{
    Debug::Assert(
        left->Column == right->Row,
        "行列left の列数と行列right の行数は等しい"
    );

    Matrix^ result = gcnew Matrix( left->Row, right->Column );

    // 行ベクトル x 行列
    for( int m=1; m <= left->Row; m++ )
    {
        for( int r=1; r <= right->Column; r++ )
        {
            double sum = 0.0;
            for( int n=1; n <= left->Column; n++ )
            {
                sum += left[ m ][ n ] * right[ n ][ r ];
            }
            result[ m ][ r ] = sum;
        }
    }

    // 誤差成分を除去する
    result->RemoveErrorComponent();

    return result;
}


/// 等価演算子 == [ Operator Overloading ]
bool Matrix::operator==( Matrix^ left, Matrix^ right )
{
    // 精度を 計算機イプシロンとして比較する
    return left->Equals( right, Double::Epsilon );
}

/// 不等価演算子 != [ Operator Overloading ]
bool Matrix::operator!=( Matrix^ left, Matrix^ right )
{
    // 値の等価の否定を返す
    return !left->Equals( right, Double::Epsilon );
}

/// 値の等価を確認する
bool Matrix::Equals( Matrix^ obj )
{
    // 精度を 計算機イプシロンとして比較する
    return Equals( obj, Double::Epsilon );
}

/// 値の等価を確認する
bool Matrix::Equals( Matrix^ obj, double precision )
{
    // NULLとは等価ではない
    if( safe_cast< System::Object^ >( obj ) == nullptr ) return false;


    Debug::Assert( IsEqualSize( obj ), "引数の行列と大きさは等しい" );


    for( int m=0; m<Row; m++ )
    {
        // 行ベクトルごとに、精度を指定して比較する
        if( !m_rowVectors[ m ]->Equals( obj->m_rowVectors[ m ], precision ) )
        {
            return false;
        }
    }
    return true;
}


/// 零行列にする
void Matrix::ZeroMatrix()
{
    // 全ての行ベクトルを零ベクトルにする
    for( int m=0; m<Row; m++ )
    {
        m_rowVectors[ m ]->ZeroVector();
    }
}

/// 単位行列にする
void Matrix::UnitMatrix()
{
    Debug::Assert( IsSquareMatrix(), "正方行列である" );

    for( int n=1; n<=Column; n++ )
    {
        for( int m=1; m<=Row; m++ )
        {
            // 対角成分ならば1.0 それ以外は0.0 とする
            this[ m ][ n ] = ( m == n )? 1.0 : 0.0;
        }
    }
}

/// 転置行列を返す
Matrix^ Matrix::TransposedMatrix()
{
    // 行数と列数を反転させた行列を作成する
    Matrix^ result = gcnew Matrix( Column, Row );

    for( int n=1; n<=Column; n++ )
    {
        for( int m=1; m<=Row; m++ )
        {
            // 行と列を反転させて代入する
            result[ n ][ m ] = this[ m ][ n ];
        }
    }
    return result;
}


/// 逆行列を求める
bool Matrix::InverseMatrix( Matrix^% result )
{
    Debug::Assert( IsSquareMatrix(), "正方行列である" );


    // 演算用の行列を生成する
    Matrix^ matrix = gcnew Matrix( this );

    array< int >^ informationToChangeRow;   // 行交換の情報

    // ＬＵ分解をする
    if( !matrix->LuDecomposition( informationToChangeRow ) )
    {
        // ＬＵ分解できないならば 処理を中止する
        return false;
    }


    // 自身と同じ大きさで 結果格納用の行列を生成する
    result = gcnew Matrix( Row, Column );

    for( int n=1; n<=Row; n++ )
    {
        // 最初の行から 末尾の行まで
        for( int m=1; m<=Row; m++ )
        {
            const int row = informationToChangeRow[ m ];

            double component = ( row == n )? 1.0 : 0.0;

            for( int i=1; i<m; i++ )
            {
                // 元の行列の列と結果となる行列の行の 成分の積を減ずる
                component -= matrix[ row ][ i ] * result[ i ][ n ];
            }
            result[ m ][ n ] = component;
        }

        // 末尾から 最初の行まで
        for( int m=Row; m>=1; m-- )
        {
            const int row = informationToChangeRow[ m ];

            double component = result[ m ][ n ];

            for( int i=m+1; i<=Row; i++ )
            {
                // 元の行列の列と結果となる行列の行の 成分の積を減ずる
                component -= matrix[ row ][ i ] * result[ i ][ n ];
            }
            result[ m ][ n ] = component / matrix[ row ][ m ];
        }
    }
    return true;


// [ Reference ]「Ｃ言語による最新アルゴリズム事典」奥村晴彦(技術評論社) P40
}


/// ＬＵ分解 (行列を下三角行列(Lower)と上三角行列(Upper) に分解する)
bool Matrix::LuDecomposition( array< int >^% informationToChangeRow )
{
    Debug::Assert( informationToChangeRow == nullptr, "結果を格納する引数は 生成されていないものである" );
    Debug::Assert( IsSquareMatrix(), "正方行列である" );


    // 行交換の情報を生成する
    informationToChangeRow = gcnew array< int >( Row + 1 );

    // ... それを初期化する
    for( int n=1; n<=Row; n++ )
    {
        informationToChangeRow[ n ] = n;
    }


    // 各行について 絶対値が最大の成分を取得する
    array< double >^ maxAbsolutes = GetComponentWithMaximumAbsoluteValue();

    for( int n=1; n<=Row; n++ )
    {
        if( maxAbsolutes[ n ] == 0.0 )
        {
            // 最大の成分がゼロの行(零ベクトル)を含むならば ＬＵ分解はできない
            return false;
        }
    }


    for( int n=1; n<=Row; n++ )
    {
        int maxRow = Row;   // 最大の行

        double max = Double::MinValue;

        for( int m=n; m<=Row; m++ )
        {
            // ある行より下において 絶対値の最大の逆数 x 絶対値 が最大の行を求める
            const int row = informationToChangeRow[ m ];

            double component = maxAbsolutes[ row ] * Math::Abs( this[ row ][ n ] );

            if( max < component )
            {
                max = component;
                maxRow = m;
            }
        }

        const int currentRow = informationToChangeRow[ maxRow ];    // 現在の行

        if( maxRow != n )
        {
            // 現在の行が最大でなければ 行番号を交換する
            informationToChangeRow[ maxRow ] = informationToChangeRow[ n ];
            informationToChangeRow[ n ] = currentRow;
        }

        const double diagonalComponent = this[ currentRow ][ n ];   // 対角成分

        if( Math::Abs( diagonalComponent ) < Double::Epsilon )
        {
            // 対角成分がゼロならば ＬＵ分解はできない
            return false;
        }

        // Gauss 消去法
        for( int m=n+1; m<=Row; m++ )
        {
            const int row = informationToChangeRow[ m ];

            this[ row ][ n ] /= diagonalComponent;

            for( maxRow=n+1; maxRow<=Row; maxRow++ )
            {
                this[ row ][ maxRow ] -= this[ row ][ n ] * this[ currentRow ][ maxRow ];
            }
        }
    }
    return true;


// [ Reference ]「Ｃ言語による最新アルゴリズム事典」奥村晴彦(技術評論社) P387
}

/// 絶対値が最大の成分を取得する
array< double >^ Matrix::GetComponentWithMaximumAbsoluteValue()
{
    array< double >^ result = gcnew array< double >( Row + 1 );


    // 各行について調べる
    for( int n=1; n<=Row; n++ )
    {
        double max = 0.0;

        // 絶対値が最大の成分を探索する
        for( int m=1; m<=Row; m++ )
        {
            const double absolute = Math::Abs( this[ n ][ m ] );

            if( max < absolute )
            {
                // 最大値を更新する
                max = absolute;
            }
        }

        // 絶対値が最大の成分の逆数を 結果に格納する(ゼロ除算しないようにしている)
        result[ n ] = ( max == 0.0 )? 0.0 : 1.0 / max;
    }
    return result;
}


/// 大きさは等しいか？
bool Matrix::IsEqualSize( Matrix^ obj )
{
    return ( Row == obj->Row ) && ( Column == obj->Column);
}

/// 正方行列か？
bool Matrix::IsSquareMatrix()
{
    return Row == Column;
}


/// 直交行列か？
bool Matrix::IsOrthogonalMatrix( double precision )
{
    Debug::Assert( IsSquareMatrix(), "正方行列である" );

    // 比較用の転置行列を作成する
    Matrix^ matrix = TransposedMatrix();

    // ... それとの行列積を求める
    matrix *= this;

    // ... それが単位行列となっているか？
    return matrix->IsUnitMatrix( precision );
}

/// 単位行列か？
bool Matrix::IsUnitMatrix( double precision )
{
    Debug::Assert( IsSquareMatrix(), "正方行列である" );

    // 比較用の行列を作成する
    Matrix^ compare = gcnew Matrix( Row, Column );

    // ... それを単位行列とする
    compare->UnitMatrix();

    // ... それと等しいか比較する
    return Equals( compare, precision );
}

/// 零行列か？
bool Matrix::IsZeroMatrix()
{
    // 比較用の零行列を作成する
    Matrix^ compare = gcnew Matrix( Row, Column );

#ifdef _DEBUG
    for( int m=1; m<=compare->Row; m++ )
    {
        Debug::Assert(
            compare[ m ]->IsZeroVector(),
            "零行列に初期化されている"
        );
    }
#endif

    return ( this == compare );
}


/// 誤差成分を除去する (微少成分の消去)
void Matrix::RemoveErrorComponent()
{
    double maxOfAbsoluteValue = 0.0;


    for( int m=1; m<=Row; m++ )
    {
        // 絶対値が最大の成分を見つける
        for( int n=1; n<Column; n++ )
        {
            double compare = Math::Abs( this[ m ][ n ] );

            if( maxOfAbsoluteValue < compare )
            {
                maxOfAbsoluteValue = compare;
            }
        }
    }


    if( Double::Epsilon < maxOfAbsoluteValue )
    {
        // 最大の成分が 計算機イプシロンより大きければ修正する
        for( int m=1; m<=Row; m++ )
        {
            for( int n=1; n<Column; n++ )
            {
                // 絶対値が最大の成分と比較して相対的に小さい値を ゼロとする
                if( ( Math::Abs( this[ m ][ n ] ) / maxOfAbsoluteValue ) < Tolerance )
                {
                    this[ m ][ n ] = 0.0;
                }
            }
        }
    }

// [ Reference ]「数値計算以前」Yamada.K ( http://www.asahi-net.or.jp/~uc3k-ymd/Lesson/Section03/section03_13.html )
}


/// インスタンスの説明を文字列で返す
System::String^ Matrix::ToString()
{
    String^ result;

    // 全てのベクトルを 文字列として連結する
    for each( Vector^ vector in m_rowVectors )
    {
        result += vector->ToString() + Environment::NewLine;
    }
    return result;
}

Copyright © 2006-2025 BABA. All Rights Reserved.