RotationMatrix.cpp

#include "RotationMatrix.h"


#include "AxisVector.h"


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


/// [ Constructor ]
RotationMatrix::RotationMatrix() :
    Matrix( DefaultRow, DefaultColumn )
{
    // 単位行列に初期化する
    UnitMatrix();

    Debug::Assert( IsOrthogonalMatrix(), "直交行列である" );
}

/// ロール・ピッチ・ヨー角から生成する [ Constructor ]
RotationMatrix::RotationMatrix( double roll, double pitch, double yaw ) :
    Matrix( DefaultRow, DefaultColumn )
{
    Debug::Assert( Angle::IsExpressedByRadian( roll ), "Radianで表されている" );
    Debug::Assert( Angle::IsExpressedByRadian( pitch ), "Radianで表されている" );
    Debug::Assert( Angle::IsExpressedByRadian( yaw ), "Radianで表されている" );


    // ロール・ピッチ・ヨー角 それぞれのサインを求める
    double sinRoll = Math::Sin( roll );
    double sinPitch = Math::Sin( pitch );
    double sinYaw = Math::Sin( yaw );

    // ... コサインを求める
    double cosRoll = Math::Cos( roll );
    double cosPitch = Math::Cos( pitch );
    double cosYaw = Math::Cos( yaw );


    this[ 1 ][ 1 ] = cosYaw * cosPitch;
    this[ 2 ][ 1 ] = sinYaw * cosPitch;
    this[ 3 ][ 1 ] = -sinPitch;

    this[ 1 ][ 2 ] = ( -sinYaw * cosRoll ) + ( cosYaw * sinPitch * sinRoll );
    this[ 2 ][ 2 ] = ( cosYaw * cosRoll ) + ( sinYaw * sinPitch * sinRoll );
    this[ 3 ][ 2 ] = cosPitch * sinRoll;

    this[ 1 ][ 3 ] = ( sinYaw * sinRoll ) + ( cosYaw * sinPitch * cosRoll );
    this[ 2 ][ 3 ] = ( -cosYaw * sinRoll ) + ( sinYaw * sinPitch * cosRoll );
    this[ 3 ][ 3 ] = cosPitch * cosRoll;


    Debug::Assert( IsOrthogonalMatrix(), "直交行列である" );

// [ Reference ]「ヒューマノイドロボット」梶田秀司(オーム社) P25
// [ Reference ]「ここが知りたいロボット創造設計」米田完(講談社) P67
}

/// 行列から生成する [ Constructor ]
RotationMatrix::RotationMatrix( Matrix^ matrix ) :
    Matrix( matrix )
{
    Debug::Assert( IsCorrectSize( matrix ), "行列の大きさは正しい" );
    Debug::Assert( IsOrthogonalMatrix(), "直交行列である" );
}

/// XMLから生成する [ Constructor ]
RotationMatrix::RotationMatrix( System::Xml::XmlElement^ element ) :
    Matrix( DefaultRow, DefaultColumn )
{
    // 単位行列に初期化する
    UnitMatrix();


    // 要素を取得する
    Xml::XmlElement^ node = element[ "rotationMatrix" ];

    if( node != nullptr )
    {
        // 取得できたならば、属性から設定する
        this[ 1 ][ 1 ] = Double::Parse( node->GetAttribute( "c11" ) );
        this[ 1 ][ 2 ] = Double::Parse( node->GetAttribute( "c12" ) );
        this[ 1 ][ 3 ] = Double::Parse( node->GetAttribute( "c13" ) );

        this[ 2 ][ 1 ] = Double::Parse( node->GetAttribute( "c21" ) );
        this[ 2 ][ 2 ] = Double::Parse( node->GetAttribute( "c22" ) );
        this[ 2 ][ 3 ] = Double::Parse( node->GetAttribute( "c23" ) );

        this[ 3 ][ 1 ] = Double::Parse( node->GetAttribute( "c31" ) );
        this[ 3 ][ 2 ] = Double::Parse( node->GetAttribute( "c32" ) );
        this[ 3 ][ 3 ] = Double::Parse( node->GetAttribute( "c33" ) );
    }
    Debug::Assert( IsOrthogonalMatrix(), "直交行列である" );
}


/// 積 * [ Operator Overloading ]
RotationMatrix^ RotationMatrix::operator*( RotationMatrix^ left, RotationMatrix^ right )
{
    RotationMatrix^ result = gcnew RotationMatrix( left );
    result *= right;

    return result;
}

/// 積 * [ Operator Overloading ]
RotationMatrix^ RotationMatrix::operator*( RotationMatrix^ rotationMatrix, Matrix^ matrix )
{
    Debug::Assert( IsCorrectSize( matrix ), "行列の大きさは正しい" );

    RotationMatrix^ result = gcnew RotationMatrix( rotationMatrix );
    result *= matrix;

    return result;
}

/// 積 * [ Operator Overloading ]
RotationMatrix^ RotationMatrix::operator*( Matrix^ matrix, RotationMatrix^ rotationMatrix )
{
    Debug::Assert( IsCorrectSize( matrix ), "行列の大きさは正しい" );

    RotationMatrix^ result = gcnew RotationMatrix( matrix );
    result *= rotationMatrix;

    return result;
}


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


    // ロール角、ピッチ角、ヨー角それぞれについて、精度を指定して比較する
    return ( this->Roll.Equals( obj->Roll, precision ) )
        && ( this->Pitch.Equals( obj->Pitch, precision ) )
        && ( this->Yaw.Equals( obj->Yaw, precision ) );
}


/// 指数関数を返す (ロドリゲスの公式 / 角速度ベクトルによって生じる回転行列を求める)
RotationMatrix^ RotationMatrix::MatrixExponential( Vector^ axisVector, Angle angle )
{
    // ひずみ対称行列(交代行列)を求める
    Matrix^ skewSymmetricMatrix = axisVector->SkewSymmetricMatrix();

    // ... それの２乗を求める
    Matrix^ squareSkewSymmetricMatrix = skewSymmetricMatrix * skewSymmetricMatrix;

    // ... そして、それぞれの行列をスカラー倍する
    skewSymmetricMatrix *= Math::Sin( angle );
    squareSkewSymmetricMatrix *= 1.0 - Math::Cos( angle );


    RotationMatrix^ result = gcnew RotationMatrix();
    Debug::Assert( result->IsUnitMatrix(), "回転行列は単位行列で初期化されている" );

    // ... 先に求めた２つの行列を加算する
    result += skewSymmetricMatrix + squareSkewSymmetricMatrix;

    return result;

// [ Reference ]「ヒューマノイドロボット」梶田秀司(オーム社) P34
}


/// 行列対数を返す
Matrix^ RotationMatrix::LogMatrix()
{
    // 角速度ベクトルを求める
    Vector^ angularVelocity = AngularVelocityVector();

    // ... それのひずみ対称行列を返す
    return angularVelocity->SkewSymmetricMatrix();


// [ Reference ]「ヒューマノイドロボット」梶田秀司(オーム社) P35
}

/// 角速度ベクトルを返す (１秒間で 対応する回転行列に到達する値)
Vector^ RotationMatrix::AngularVelocityVector()
{
    if( IsUnitMatrix() )
    {
        // 単位行列ならば 零ベクトルを返す
        Vector^ result = gcnew Vector( 3 );
        Debug::Assert( result->IsZeroVector(), "零ベクトルで初期化されている" );

        return result;
    }


    double parameterOfArcCosine
        = ( this[ 1 ][ 1 ] + this[ 2 ][ 2 ] + this[ 3 ][ 3 ] - 1.0 ) / 2.0;

    Debug::Assert(
        ( -1.0 <= parameterOfArcCosine ) &&
        ( +1.0 >= parameterOfArcCosine ),
        "逆余弦の引数は正当である"
    );
    double theta = Math::Acos( parameterOfArcCosine );
    double scalar = theta / ( 2.0 * Math::Sin( theta ) );


    Vector^ result = gcnew Vector( 3 );
    result[ 1 ] = scalar * ( this[ 3 ][ 2 ] - this[ 2 ][ 3 ] );
    result[ 2 ] = scalar * ( this[ 1 ][ 3 ] - this[ 3 ][ 1 ] );
    result[ 3 ] = scalar * ( this[ 2 ][ 1 ] - this[ 1 ][ 2 ] );

    return result;


// [ Reference ]「ヒューマノイドロボット」梶田秀司(オーム社) P35
}


/// X方向の軸ベクトルを取得する [ Property ]
AxisVector^ RotationMatrix::X::get()
{
    AxisVector^ result = gcnew AxisVector(
        this[ 1 ][ 1 ],
        this[ 2 ][ 1 ],
        this[ 3 ][ 1 ]
    );
    return result;
}

/// Y方向の軸ベクトルを取得する [ Property ]
AxisVector^ RotationMatrix::Y::get()
{
    AxisVector^ result = gcnew AxisVector(
        this[ 1 ][ 2 ],
        this[ 2 ][ 2 ],
        this[ 3 ][ 2 ]
    );
    return result;
}

/// Z方向の軸ベクトルを取得する [ Property ]
AxisVector^ RotationMatrix::Z::get()
{
    AxisVector^ result = gcnew AxisVector(
        this[ 1 ][ 3 ],
        this[ 2 ][ 3 ],
        this[ 3 ][ 3 ]
    );
    return result;
}



/// ロール角を取得する [ Property ]
Angle RotationMatrix::Roll::get()
{
    double y = this[ 3 ][ 2 ];
    double x = this[ 3 ][ 3 ];

    return Angle( Atan2( y, x ) );

// [ Reference ]「ここが知りたいロボット創造設計」米田完(講談社) P67
}

/// ピッチ角を取得する [ Property ]
Angle RotationMatrix::Pitch::get()
{
    double y = this[ 3 ][ 1 ];
    double x = Math::Sqrt(
        this[ 1 ][ 1 ] * this[ 1 ][ 1 ] +
        this[ 2 ][ 1 ] * this[ 2 ][ 1 ]
    );

    return Angle( Atan2( -y, x ) );

// [ Reference ]「ここが知りたいロボット創造設計」米田完(講談社) P67
}

/// ヨー角を取得する [ Property ]
Angle RotationMatrix::Yaw::get()
{
    double y = this[ 2 ][ 1 ];
    double x = this[ 1 ][ 1 ];

    return Angle( Atan2( y, x ) );

// [ Reference ]「ここが知りたいロボット創造設計」米田完(講談社) P67
}


/// 基準に近い ロール角を取得する
Angle RotationMatrix::GetRoll( Angle standard )
{
    double y = this[ 3 ][ 2 ];
    double x = this[ 3 ][ 3 ];


    // 符号を変えて、２つの解を求める
    Angle result1( Atan2( +y, +x ) );
    Angle result2( Atan2( -y, -x ) );

    // ... 基準に近い方を返す
    return standard.Near( result1, result2 );
}

/// 基準に近い ピッチ角を取得する
Angle RotationMatrix::GetPitch( Angle standard )
{
    double y = this[ 3 ][ 1 ];
    double x = Math::Sqrt(
        this[ 1 ][ 1 ] * this[ 1 ][ 1 ] +
        this[ 2 ][ 1 ] * this[ 2 ][ 1 ]
    );


    // 符号を変えて、２つの解を求める
    Angle result1( Atan2( -y, +x ) );
    Angle result2( Atan2( -y, -x ) );

    // ... 基準に近い方を返す
    return standard.Near( result1, result2 );
}

/// 基準に近い ヨー角を取得する
Angle RotationMatrix::GetYaw( Angle standard )
{
    double y = this[ 2 ][ 1 ];
    double x = this[ 1 ][ 1 ];


    // 符号を変えて、２つの解を求める
    Angle result1( Atan2( +y, +x ) );
    Angle result2( Atan2( -y, -x ) );

    // ... 基準に近い方を返す
    return standard.Near( result1, result2 );
}


/// 大きさは正しいか？
bool RotationMatrix::IsCorrectSize( Matrix^ obj )
{
    // 行数は既定の行数であり、列数は既定の列数か？
    return ( obj->Row == DefaultRow )
        && ( obj->Column == DefaultColumn );
}

/// 水平か？
bool RotationMatrix::IsHorizontal( double precision )
{
    // ロール角とピッチ角が ゼロであることを確認する
    return ( Math::Abs( Roll ) < precision )
        && ( Math::Abs( Pitch ) < precision );
}

/// 水平にする
void RotationMatrix::Horizontal()
{
    double sinYaw = Math::Sin( Yaw );
    double cosYaw = Math::Cos( Yaw );

    this[ 1 ][ 1 ] = cosYaw;
    this[ 2 ][ 1 ] = sinYaw;
    this[ 3 ][ 1 ] = 0.0;

    this[ 1 ][ 2 ] = -sinYaw;
    this[ 2 ][ 2 ] = cosYaw;
    this[ 3 ][ 2 ] = 0.0;

    this[ 1 ][ 3 ] = 0.0;
    this[ 2 ][ 3 ] = 0.0;
    this[ 3 ][ 3 ] = 1.0;


    Debug::Assert( IsHorizontal( Double::Epsilon ), "水平である" );
}


/// タンジェントが 2つの指定された数の商である角度を返す
double RotationMatrix::Atan2( double y, double x )
{
    if( x == 0 && y == 0 )
    {
        // 2つの引数がゼロの場合は、ゼロを返す
        return 0.0;
    }
    else
    {
        // Mathクラスの 同一関数を呼び出す
        return Math::Atan2( y, x );
    }


    /// @note
    /// 2つの引数がゼロの場合の戻り地について、C言語の標準ライブラリでは処理系定義とされている。
    /// しかし.NETの Mathクラスにおいては、戻り値が明記されていない。
    /// よって、ここで処理を明示的に行っている。
    /// http://msdn2.microsoft.com/ja-jp/library/system.math.atan2.aspx
}


/// インスタンスの説明を文字列で返す (MATLAB形式)
System::String^ RotationMatrix::ToString()
{
    return String::Format(
        "[ {0:0.0000000} {1:0.0000000} {2:0.0000000};" + Environment::NewLine +
        "  {3:0.0000000} {4:0.0000000} {5:0.0000000};" + Environment::NewLine +
        "  {6:0.0000000} {7:0.0000000} {8:0.0000000} ]",
        this[ 1 ][ 1 ], this[ 1 ][ 2 ], this[ 1 ][ 3 ],
        this[ 2 ][ 1 ], this[ 2 ][ 2 ], this[ 2 ][ 3 ],
        this[ 3 ][ 1 ], this[ 3 ][ 2 ], this[ 3 ][ 3 ]
    );
}

/// インスタンスの説明をロール・ピッチ・ヨー表現の文字列で返す
System::String^ RotationMatrix::ToStringByRollPitchYaw()
{
    return String::Format(
        "Roll:{0:0.0} Pitch:{1:0.0} Yaw:{2:0.0}",
        Roll.Degree, Pitch.Degree, Yaw.Degree
    );
}


/// インスタンスの説明をXMLで返す
System::Xml::XmlElement^ RotationMatrix::ToXml( System::Xml::XmlDocument^ document )
{
    // ルートを生成する
    Xml::XmlElement^ root = document->CreateElement( "rotationMatrix" );

    // ... フィールドを属性として設定する
    root->SetAttribute( "c11", this[ 1 ][ 1 ].ToString() );
    root->SetAttribute( "c12", this[ 1 ][ 2 ].ToString() );
    root->SetAttribute( "c13", this[ 1 ][ 3 ].ToString() );

    root->SetAttribute( "c21", this[ 2 ][ 1 ].ToString() );
    root->SetAttribute( "c22", this[ 2 ][ 2 ].ToString() );
    root->SetAttribute( "c23", this[ 2 ][ 3 ].ToString() );

    root->SetAttribute( "c31", this[ 3 ][ 1 ].ToString() );
    root->SetAttribute( "c32", this[ 3 ][ 2 ].ToString() );
    root->SetAttribute( "c33", this[ 3 ][ 3 ].ToString() );

    return root;
}

Copyright © 2006-2024 BABA. All Rights Reserved.