Vector.cpp

#include "Vector.h"


#include "Matrix.h"


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


/// 指定された次元で生成し 零ベクトルで初期化する [ Constructor ]
Vector::Vector( int dimension )
{
    // 領域を確保する
    SecureDomain( dimension );

    Debug::Assert( Dimension == dimension, "次元は指定値に設定されている" );

#ifdef _DEBUG
    for( int i=0; i<Dimension; i++ )
    {
        Debug::Assert( m_components[ i ] == 0.0, "零ベクトルに初期化されている" );
    }

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

#endif
}

/// 指定された次元で生成し 引数の配列で初期化する [ Constructor ]
Vector::Vector( const double* val, int dimension )
{
    // 領域を確保する
    SecureDomain( dimension );

    for( int i=0; i<Dimension; i++ )
    {
        m_components[ i ] = val[ i ];
    }

    Debug::Assert( Dimension == dimension, "次元は指定値に設定されている" );
}

/// コピー元と同次元で生成し それの成分で初期化する [ Copy constructor ]
Vector::Vector( Vector^ other )
{
    // 領域を確保する
    SecureDomain( other->Dimension );

    for( int i=0; i<Dimension; i++ )
    {
        m_components[ i ] = other->m_components[ i ];
    }

    Debug::Assert( Dimension == other->Dimension, "次元はコピー元と同じに設定されている" );
}


/// 指定された次元で領域を確保する
void Vector::SecureDomain( int dimension )
{
    Debug::Assert( 1 <= dimension, "次元は自然数である" );

    // メモリ領域を確保する
    m_components = gcnew array< double >( dimension );
}


/// Indexer [ Property ]
double Vector::default::get( int index )
{
    Debug::Assert( IsRangeOfDimension( index ), "添え字は次元の範囲内である" );

    // 添え字が１からとなるように、補正して指定する
    return m_components[ index - 1 ];
}

/// Indexer [ Property ]
void Vector::default::set( int index, double value )
{
    Debug::Assert( IsRangeOfDimension( index ), "添え字は次元の範囲内である" );

    // 添え字１からとなるように、補正して指定する
    m_components[ index - 1 ] = value;
}


/// 次元を設定する [ Property ]
void Vector::Dimension::set( int value )
{
    Debug::Assert( value != Dimension, "次元は変更される" );


    // 領域を解放する
    delete[] m_components;

    // ... 指定された次元で領域を確保する
    SecureDomain( value );

    Debug::Assert( Dimension == value, "次元は指定値に設定されている" );
}


/// 逆ベクトルを返す [ Operator Overloading ]
Vector^ Vector::operator-()
{
    return gcnew Vector( -1.0 * this );
}


/// 和 += [ Operator Overloading ]
Vector^ Vector::operator+=( Vector^ obj )
{
    Debug::Assert( Dimension == obj->Dimension, "引数のベクトルと次元は等しい" );


    for( int i=0; i<Dimension; i++ )
    {
        m_components[ i ] += obj->m_components[ i ];
    }
    return this;
}

/// 差 -= [ Operator Overloading ]
Vector^ Vector::operator-=( Vector^ obj )
{
    Debug::Assert( Dimension == obj->Dimension, "引数のベクトルと次元は等しい" );


    for( int i=0; i<Dimension; i++ )
    {
        m_components[ i ] -= obj->m_components[ i ];
    }
    return this;
}

/// 積 *= [ Operator Overloading ]
Vector^ Vector::operator*=( double val )
{
    for( int i=0; i<Dimension; i++ )
    {
        m_components[ i ] *= val;
    }
    return this;
}

/// 商 /= [ Operator Overloading ]
Vector^ Vector::operator/=( double val )
{
    Debug::Assert( Double::Epsilon <= Math::Abs( val ), "引数はゼロではない (ゼロ除算の防止)" );


    for( int i=0; i<Dimension; i++ )
    {
        m_components[ i ] /= val;
    }
    return this;
}

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

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

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

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

    Debug::Assert( Dimension == obj->Dimension, "引数のベクトルと次元は等しい" );


    for( int i=0; i<Dimension; i++ )
    {
        // 各成分の差の絶対値を求める
        double remainder = Math::Abs( m_components[ i ] - obj->m_components[ i ] );

        // ... それを精度(誤差の有効範囲)と比較する
        if( precision < remainder )
        {
            return false;
        }
    }
    return true;
}

/// ベクトルの和 [ Operator Overloading ]
Vector^ Vector::operator+( Vector^ left, Vector^ right )
{
    Vector^ result = gcnew Vector( left );
    result += right;

    return result;
}

/// ベクトルの差 [ Operator Overloading ]
Vector^ Vector::operator-( Vector^ left, Vector^ right )
{
    Vector^ result = gcnew Vector( left );
    result -= right;

    return result;
}

/// スカラー倍 (実数 x ベクトル) [ Operator Overloading ]
Vector^ Vector::operator*( double scalar, Vector^ obj )
{
    Vector^ result = gcnew Vector( obj );
    result *= scalar;

    return result;
}

/// スカラー倍 (ベクトル x 実数) [ Operator Overloading ]
Vector^ Vector::operator*( Vector^ obj, double scalar )
{
    // 引数の順を入れ替えて、オーバーロードしている同一関数を呼び出す
    return scalar * obj;
}

/// スカラー商 (ベクトル / 実数) [ Operator Overloading ]
Vector^ Vector::operator/( Vector^ obj, double scalar )
{
    Vector^ result = gcnew Vector( obj );
    result /= scalar;

    return result;
}


/// 内積を返す
double Vector::InnerProduct( Vector^ left, Vector^ right )
{
    Debug::Assert(
        left->Dimension == right->Dimension,
        "引数の２つのベクトルの次元は等しい"
    );

    double result = 0.0;

    // 全ての成分の積の和を求める
    for( int i=1; i <= left->Dimension; i++ )
    {
        result += left[ i ] * right[ i ];
    }
    return result;
}


/// 外積を返す
Vector^ Vector::CrossProduct( Vector^ left, Vector^ right )
{
    Debug::Assert(
        ( left->Dimension == 3 ) && ( right->Dimension == 3 ),
        "２つのベクトルは３次元である"
    );

    Vector^ result = gcnew Vector( 3 );

    result[ 1 ] = ( left[ 2 ] * right[ 3 ] ) - ( left[ 3 ] * right[ 2 ] );
    result[ 2 ] = ( left[ 3 ] * right[ 1 ] ) - ( left[ 1 ] * right[ 3 ] );
    result[ 3 ] = ( left[ 1 ] * right[ 2 ] ) - ( left[ 2 ] * right[ 1 ] );

    return result;


// [ Reference ]「ヒューマノイドロボット」梶田秀司(オーム社) P29
// [ Reference ]「ロボット制御基礎論」吉川恒夫(コロナ社) P52
}


/// ２つのベクトルを結合する
Vector^ Vector::Combine( Vector^ left, Vector^ right )
{
    // ２つのベクトルの次元の和を 生成するベクトルの次元とする
    int dimension = left->Dimension + right->Dimension;

    Vector^ result = gcnew Vector( dimension );


    int index = 1;

    // 左側のベクトルを代入する
    for( int i=1; i <= left->Dimension; i++ )
    {
        result[ index++ ] = left[ i ];
    }

    // 右側のベクトルを代入する
    for( int i=1; i <= right->Dimension; i++ )
    {
        result[ index++ ] = right[ i ];
    }
    return result;
}


/// 零ベクトルにする
void Vector::ZeroVector()
{
#ifdef _DEBUG
    int counter;

    for( counter=0; counter<Dimension; counter++ )
    {
        if( m_components[ counter ] != 0.0 ) break; }
//  } cope with NCover

    Debug::Assert(
        counter <= Dimension,
        "全ての成分はゼロではない(零ベクトルではない)。"
        "無駄な初期化が行われているので、それを修正する"
    );
#endif

    for( int i=0; i<Dimension; i++ )
    {
        m_components[ i ] = 0.0;
    }
}

/// ひずみ対称行列 (交代行列) を返す
Matrix^ Vector::SkewSymmetricMatrix()
{
    Debug::Assert( Dimension == 3, "３次元である" );

    Matrix^ result = gcnew Matrix( Dimension, Dimension );

    result[ 1 ][ 1 ] = 0.0;
    result[ 2 ][ 2 ] = 0.0;
    result[ 3 ][ 3 ] = 0.0;

    result[ 3 ][ 2 ] = this[ 1 ];
    result[ 1 ][ 3 ] = this[ 2 ];
    result[ 2 ][ 1 ] = this[ 3 ];

    result[ 2 ][ 3 ] = -this[ 1 ];
    result[ 3 ][ 1 ] = -this[ 2 ];
    result[ 1 ][ 2 ] = -this[ 3 ];

    return result;


// [ Reference ]「ヒューマノイドロボット」梶田秀司(オーム社) P32
// [ Reference ]「ロボット制御基礎論」吉川恒夫(コロナ社) P62
}


/// ノルム (ベクトル空間における長さ) を返す
double Vector::Norm()
{
    double result = 0.0;

    // 全ての成分の二乗和を求める
    for( int i=0; i<Dimension; i++ )
    {
        result += m_components[ i ] * m_components[ i ];
    }

    // ... それの平方根を返す
    return Math::Sqrt( result );


// [ Reference ]「工学を志す人の線形代数」長宗雄(東京教学社) P105
}


/// 単位ベクトルか？
bool Vector::IsUnitVector( double precision )
{
    Debug::Assert( Dimension <= 3, "３次元までしか適用できない" );

    double result = 0.0;

    // ２乗和を求める
    for( int i=0; i<Dimension; i++ )
    {
        result += m_components[ i ] * m_components[ i ];
    }

    // ... それの平方根の絶対値が、有効範囲(精度)内かどうかで判定する
    return Math::Abs( Math::Sqrt( result ) - 1.0 ) < precision;
}

/// 零ベクトルか？
bool Vector::IsZeroVector()
{
    // 比較用の零ベクトルを作成する (作成時に零ベクトルに初期化される)
    Vector^ zeroVector = gcnew Vector( Dimension );

    return ( this == zeroVector );
}


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

    // 絶対値が最大の成分を見つける
    for( int i=0; i<Dimension; i++ )
    {
        double compare = Math::Abs( m_components[ i ] );

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


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

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


/// 次元の範囲内か？
bool Vector::IsRangeOfDimension( int index )
{
    const int baseNumberOfSuffix = 1;   // 添え字の開始番号

    return ( index >= baseNumberOfSuffix )
        && ( index <= baseNumberOfSuffix + Dimension );
}


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

    // 全ての成分を 文字列として連結する
    for each( double value in m_components )
    {
        result += value.ToString( "0.0000000" ) + " ";
    }
    return result;
}

Copyright © 2006-2025 BABA. All Rights Reserved.