Motion.cpp
#include "StdAfx.h"
#include "Motion.h"
#include "Pose.h"
#include "Model.h"
#include "Link.h"
using namespace Core;
using namespace Robotics;
using namespace System;
using namespace System::Diagnostics;
/// [ Constructor ]
Motion::Motion( Model^ model ) :
m_model( model ),
m_name( String::Empty )
{
Debug::Assert( model != nullptr, "有効なモデルである" );
// モデルの状態を初期化する
model->InitializeState();
// イベントに登録する
model->StateChanged += gcnew System::EventHandler( this, &Motion::ModelStateChanged ); // モデルの状態変化
Link::MechanicsChanged += gcnew System::EventHandler( this, &Motion::LinkStructureChanged );// リンクの構造変化
Link::StandardChanged += gcnew System::EventHandler( this, &Motion::StandardLinkChanged ); // 基準リンクの変更
// ポーズを初期化する
InitializePose();
}
/// ポーズを初期化する
void Motion::InitializePose()
{
Debug::WriteLine( "ポーズを初期化する" );
m_poses = gcnew Poses();
m_typicalPoses = gcnew Poses();
// 初期状態のポーズを生成する
Pose^ initialPose = gcnew Pose( 0.0, m_model );
// ... それを追加する
m_poses->Add( initialPose );
m_typicalPoses->Add( initialPose );
}
/// 現在のポーズのインデックスを設定する [ Property ]
void Motion::CurrentPoseIndex::set( int value )
{
m_currentPoseIndex = value;
// イベントハンドラを生成する
System::EventHandler^ modelEventHandler = gcnew System::EventHandler( this, &Motion::ModelStateChanged );
System::EventHandler^ linkEventHandler = gcnew System::EventHandler( this, &Motion::StandardLinkChanged );
// ... そのイベントから呼び出されないように、イベントを削除する
m_model->StateChanged -= modelEventHandler;
Link::StandardChanged -= linkEventHandler;
// モデルに 新しいポーズをとらせる
m_model->DoPose( CurrentPose );
// ... 削除したイベントに登録する
m_model->StateChanged += modelEventHandler;
Link::StandardChanged += linkEventHandler;
// 変更イベントを発生する
Changed( this, nullptr );
}
/// 特徴ポーズに追加する
void Motion::AddToTypicalPose( Pose^ pose )
{
/// @note 汎用性を持たせるには、「ポーズの挿入」メソッドにする。
Pose^ lastPose = m_poses[ PoseCount - 1 ]; // 最後のポーズ
double additionalTime = pose->Time - lastPose->Time; // 追加時間
int additionalNumber = safe_cast< int >( additionalTime / UnitTime ); // 追加数
// 追加時間だけ ポーズを追加する
for( int i = 0; i < additionalNumber; i++ )
{
if( i == additionalNumber - 1 )
{
// 最後ならば、渡されたポーズ自身を追加する
m_poses->Add( pose );
/// @note
/// インスタンスを検索する都合上、コピーではなく
/// それ自身を追加する必要がある。
}
else
{
// 追加するポーズのコピーを生成する
Pose^ newPose = gcnew Pose( PoseCount * UnitTime, lastPose );
// ... それをポーズに追加する
m_poses->Add( newPose );
/// @note
/// ポーズの時間は 相対的な基準から求めてはならない。
/// さもなければ、追加するごとに誤差が生じることになる。
}
}
// 特徴ポーズに追加する
m_typicalPoses->Add( pose );
}
/// ポーズのインデックスを取得する
int Motion::GetPoseIndex( Pose^ pose )
{
int result = m_poses->IndexOf( pose );
Debug::Assert( result != -1, "ポーズのインデックスは取得できる" );
return result;
}
/// 指定されたポーズの直前にある 特徴ポーズを取得する
Pose^ Motion::GetTypicalPoseJustBefore( Pose^ pose )
{
Debug::Assert( 0.0 < pose->Time, "指定されたポーズは 時間の最初ではない" );
do
{
// ポーズのインデックスを取得する
int poseIndex = m_poses->IndexOf( pose );
Debug::Assert( poseIndex != -1, "指定のポーズは、フィールドのポーズに含まれる" );
// ... それの前のポーズを取得する
pose = m_poses[ poseIndex - 1 ];
// ... それが特徴ポーズに含まれなければ 処理をくり返す
} while( !m_typicalPoses->Contains( pose ) );
return pose;
}
/// 指定されたポーズの直後にある 特徴ポーズを取得する
Pose^ Motion::GetTypicalPoseJustAfter( Pose^ pose )
{
Debug::Assert( pose->Time < PoseCount * UnitTime, "指定されたポーズは 時間の最後ではない" );
do
{
// ポーズのインデックスを取得する
int poseIndex = m_poses->IndexOf( pose );
Debug::Assert( poseIndex != -1, "指定のポーズは、フィールドのポーズに含まれる" );
// ... それの次のポーズを取得する
pose = m_poses[ poseIndex + 1 ];
// ... それが特徴ポーズに含まれなければ 処理をくり返す
} while( !m_typicalPoses->Contains( pose ) );
return pose;
}
/// 特徴ポーズのインデックスを取得する
int Motion::GetTypicalPoseIndex( Pose^ pose )
{
// 指定されたポーズを表す 特徴ポーズのインデックスを取得する
int result = m_typicalPoses->IndexOf( pose );
if( result == -1 )
{
// それが特徴ポーズに含まれていなければ、直後にある特徴ポーズを取得する
Pose^ typicalPose = GetTypicalPoseJustAfter( CurrentPose );
// ... それのインデックスを取得する
result = m_typicalPoses->IndexOf( typicalPose );
// ... その位置に 指定のポーズを追加する
m_typicalPoses->Insert( result, pose );
Debug::WriteLine( "特徴ポーズを追加した : Time = " + m_typicalPoses[ result ]->Time );
}
return result;
}
/// モデルの状態が変化した
void Motion::ModelStateChanged( System::Object^, System::EventArgs^ )
{
// 現在のポーズに モデルの状態を保存する
CurrentPose->SetModelState( m_model );
/// @todo
/// モデルに ポーズのフィールドへ参照させるようにする。
/// そうすれば、モデルが状態変化するたびに、設定し直す必要がなくなる。
// 現在のポーズを表す 特徴ポーズのインデックスを取得する
int typicalPoseIndex = GetTypicalPoseIndex( CurrentPose );
if( typicalPoseIndex != 0 )
{
// 最初の特徴ポーズでなければ、直前の特徴ポーズを取得する
Pose^ pose = m_typicalPoses[ typicalPoseIndex - 1 ];
// ... それと現在のポーズとの間を補完する
ComplementBetweenPoses( pose, CurrentPose );
}
if( typicalPoseIndex != ( m_typicalPoses->Count - 1 ) )
{
// 最後の特徴ポーズでなければ、直後の特徴ポーズを取得する
Pose^ pose = m_typicalPoses[ typicalPoseIndex + 1 ];
// ... 現在のポーズとそれの間を補完する
ComplementBetweenPoses( CurrentPose, pose );
}
/// @note
/// ポーズ間の補完時にモデルが変更されるため、それを元に戻す必要がある。
// モデルに 現在のポーズをとらせる
CurrentPose->PosedModel( m_model );
// 変更イベントを発生する
Changed( this, nullptr );
}
/// 基準リンクが変更された
void Motion::StandardLinkChanged( System::Object^ sender, System::EventArgs^ )
{
// 引数から目的とする型を取得する
Link^ link = safe_cast< Link^ >( sender );
// 現在のポーズを表す 特徴ポーズのインデックスを取得する
int typicalPoseIndex = GetTypicalPoseIndex( CurrentPose );
if( typicalPoseIndex == ( m_typicalPoses->Count - 1 ) )
{
// 最後の特徴ポーズならば、現在のポーズだけを設定する
CurrentPose->StandardLink = link;
}
else
{
// 直後の特徴ポーズを取得する
Pose^ pose = m_typicalPoses[ typicalPoseIndex + 1 ];
// ... そのポーズのインデックスを取得する
int poseIndex = m_poses->IndexOf( pose );
// ... 現在からそのポーズの直前までの 基準リンクを設定する
for( int i = m_currentPoseIndex; i < poseIndex; i++ )
{
m_poses[ i ]->StandardLink = link;
}
}
}
/// ポーズ間を補完する
void Motion::ComplementBetweenPoses( Pose^ before, Pose^ after )
{
Debug::Assert(
m_typicalPoses->Contains( before ) &&
m_typicalPoses->Contains( after ),
"補完の基準となるポーズは 特徴ポーズである"
);
int beforePoseIndex = m_poses->IndexOf( before ); // 前のポーズのインデックス
int afterPoseIndex = m_poses->IndexOf( after ); // 後のポーズのインデックス
Debug::Assert( beforePoseIndex != -1, "指定されたポーズは、フィールドのポーズに含まれる" );
Debug::Assert( afterPoseIndex != -1, "指定されたポーズは、フィールドのポーズに含まれる" );
Debug::Assert( beforePoseIndex < afterPoseIndex, "ポーズの前後の関係は正しい" );
// 前後のポーズ間にあるポーズを補完する
for( int i = beforePoseIndex + 1; i < afterPoseIndex; i++ )
{
Pose^ pose = m_poses[ i ]; // 対象となるポーズ
Debug::Assert( !m_typicalPoses->Contains( pose ), "特徴ポーズは補完されない" );
// 関節の角度を補完する
pose->ComplementAngleOfJoints( before, after );
// モデルに 補完されたポーズをとらせる
pose->PosedModel( m_model );
// ... そのモデルの状態を ポーズに設定する
pose->SetModelState( m_model );
}
}
/// 角速度を取得する
AngularVelocities^ Motion::GetAngularVelocities( int poseIndex )
{
Debug::Assert( poseIndex < PoseCount, "存在するポーズである" );
// 直前のポーズ番号を取得する (ゼロ未満にならないように補正する)
int comparison = Math::Max( poseIndex - 1, 0 );
// ... 指定されたポーズの 関節の角度を取得する
Angles^ angles = m_poses[ poseIndex ]->AngleOfJoints;
// ... 比較対象のポーズの 関節の角度を取得する
Angles^ beforeAngles = m_poses[ comparison ]->AngleOfJoints;
AngularVelocities^ result = gcnew AngularVelocities( angles->Length );
for( int i = 0; i < result->Length; i++ )
{
// 角度差から 角速度を生成する
result[ i ] = AngularVelocity( angles[ i ], beforeAngles[ i ], UnitTime );
}
return result;
}
/// リンクの変位を取得する
Robotics::PositionVector^ Motion::GetDisplacementOfLink( int poseIndex, int linkIndex )
{
Debug::Assert( poseIndex < PoseCount, "存在するポーズである" );
// 直前のポーズ番号を取得する (ゼロ未満にならないように補正する)
int comparison = Math::Max( poseIndex - 1, 0 );
// ... 指定されたポーズの リンクの位置を取得する
PositionVector^ positon = m_poses[ poseIndex ]->PositionOfLinks[ linkIndex ];
// ... 比較対象のポーズの リンクの位置を取得する
PositionVector^ beforePositon = m_poses[ comparison ]->PositionOfLinks[ linkIndex ];
// 比較対象の位置との差を返す
return( positon - beforePositon );
}
/// リンクの速度を取得する
Robotics::PositionVector^ Motion::GetVelocityOfLink( int poseIndex, int linkIndex )
{
// リンクの変位を取得する
PositionVector^ displacement = GetDisplacementOfLink( poseIndex, linkIndex );
// ... それを単位時間で除して速度を求める
return( displacement / UnitTime );
}
/// リンクの構造が変化した
void Motion::LinkStructureChanged( System::Object^, System::EventArgs^ )
{
// 全てのポーズにおいて 再計算する
for( int i = 0; i < PoseCount; i++ )
{
// モデルに ポーズをとらせる
m_poses[ i ]->PosedModel( m_model );
// ... そのモデルの状態を ポーズに設定する
m_poses[ i ]->SetModelState( m_model );
}
// モデルに 現在のポーズをとらせる
CurrentPose->PosedModel( m_model );
// 変更イベントを発生する
Changed( this, nullptr );
}
/// 設定を書き込む
System::Xml::XmlElement^ Motion::SaveSettings( System::Xml::XmlDocument^ document )
{
Debug::Assert( m_name != String::Empty, "名称は設定されている" );
// ルートを生成する
Xml::XmlElement^ root = document->CreateElement( "motion" );
for each( Pose^ pose in m_typicalPoses )
{
// 特徴ポーズをルートに追加する
root->AppendChild( pose->SaveSettings( document ) );
}
Debug::WriteLine( "モーションを書き込んだ : " + m_name );
return root;
}
/// 設定を読み込む
void Motion::LoadSettings( System::Xml::XmlDocument^ document )
{
// ポーズを削除する
m_poses->Clear();
// 特徴ポーズを削除する
m_typicalPoses->Clear();
m_name = IO::Path::GetFileNameWithoutExtension( document->BaseURI ); // 名称
// ルートを読み込む
Xml::XmlElement^ root = document[ "motion" ];
Pose^ beforePose = nullptr;
for each( Xml::XmlElement^ node in root->ChildNodes )
{
// ポーズをXMLから生成する
Pose^ pose = gcnew Pose( node, m_model );
if( beforePose == nullptr )
{
// 最初ならば、そのポーズ自身を追加する
m_poses->Add( pose );
// 特徴ポーズに追加する
m_typicalPoses->Add( pose );
}
else
{
// 特徴ポーズに追加する
AddToTypicalPose( pose );
// ポーズ間を補完する
ComplementBetweenPoses( beforePose, pose );
}
// ポーズを保存する
beforePose = pose;
}
Debug::WriteLine( "モーションを読み込んだ : " + m_name );
}