Link.cpp
#include "StdAfx.h"
#include "Link.h"
#include "ConnectionRelation.h"
#include "Joint.h"
#include "Shape.h"
using namespace Core;
using namespace Robotics;
using namespace Microsoft;
using namespace System;
using namespace System::Diagnostics;
// DirectXのライブラリによる const/volatile 修飾子の使用の警告を抑制する
#pragma warning( disable : 4400 )
/// XMLから生成する [ Constructor ]
Link::Link( Core::Joint^ joint, System::Xml::XmlElement^ element ) :
m_joint( joint )
{
m_posture = gcnew RotationMatrix();
m_centerOfGravity = gcnew PositionVector();
m_shape = Core::Shape::Create( element ); // 形状
Name = element->GetAttribute( "name" ); // 名称
// 関節の 絶対位置の変更イベントに登録する
m_joint->AbsolutePositionChanged += gcnew System::EventHandler( this, &Link::JointPositionChanged );
}
/// [ Finalizer ]
Link::!Link()
{
delete m_figure;
}
/// 名称を取得する [ Property ]
System::String^ Link::Name::get()
{
Debug::Assert( m_joint->Name == m_shape->Name, "関節と形状の名称は等しい" );
// 関節の名称を返す
return m_joint->Name;
}
/// 名称を設定する [ Property ]
void Link::Name::set( System::String^ value )
{
Debug::Assert( m_joint != nullptr, "関節は生成されている" );
m_joint->Name = value; // 関節の名称
m_shape->Name = value; // 形状の名称
}
/// 絶対位置を取得する [ Property ]
Robotics::PositionVector^ Link::AbsolutePosition::get()
{
/// @note
/// これが返す位置ベクトルに変更を加えても、この属性は変更されない。
// 関節の絶対位置を取得する
PositionVector^ result = gcnew PositionVector( m_joint->AbsolutePosition );
// ... 形状の相対位置を加算する
return result + ( Posture * m_shape->RelativePosition );
}
/// 絶対位置を設定する [ Property ]
void Link::AbsolutePosition::set( Robotics::PositionVector^ value )
{
/// @note
/// 絶対位置は姿勢の影響を受けるため、それも同時に設定する場合は
/// 姿勢を先に設定すること。
Debug::Assert( !IsStandard(), "基準リンクの絶対位置は変更されない" );
Debug::Assert( -GroundHeight <= value->Z, "位置は地面より上に設定される" );
// 形状の相対位置を減算する
value -= Posture->TransposedMatrix() * m_shape->RelativePosition;
// ... 関節の絶対位置を設定する
m_joint->AbsolutePosition = value;
}
/// 姿勢を設定する [ Property ]
void Link::Posture::set( Robotics::RotationMatrix^ value )
{
Debug::Assert( !IsStandard(), "基準リンクの姿勢は変更されない" );
if( m_posture != value )
{
// 現在の値と異なるならば、設定する
m_posture = value;
// ... 姿勢の変更イベントを発生する
PostureChanged( this, nullptr );
// 絶対位置も変更されるので、それの変更イベントを発生する
AbsolutePositionChanged( this, nullptr );
}
}
/// 軸方向(ワールド座標系) を取得する [ Property ]
AxisVector^ Link::AxialDirection::get()
{
// 関節の軸方向(ローカル座標系)を ワールド座標系に変換する
return m_posture * m_joint->AxialDirection;
// [ Reference ]「ヒューマノイドロボット」梶田秀司(オーム社) P61
}
/// 質量を設定する [ Property ]
void Link::Mass::set( double value )
{
m_mass = value;
// 構造変化イベントを発生する
OnMechanicsChanged( this, nullptr );
}
/// 重心を設定する [ Property ]
void Link::CenterOfGravity::set( Robotics::PositionVector^ value )
{
m_centerOfGravity = value;
// 位置ベクトルの 値の変更イベントに登録する
m_centerOfGravity->ValueChanged += gcnew System::EventHandler( this, &Link::OnMechanicsChanged );
/// @note
/// 位置ベクトルのプロパティに直接 設定された場合にも、
/// このクラスから変更イベントを発生できるようにしている。
}
/// 構造変化イベントを発生する
void Link::OnMechanicsChanged( System::Object^, System::EventArgs^ e )
{
MechanicsChanged( this, e );
}
/// 形状の頂点の絶対位置
PositionVectors^ Link::GetAbsolutePositionOfVertices()
{
PositionVectors^ result = m_shape->GetAbsolutePositionOfVertices(
m_joint->AbsolutePosition,
m_posture
);
return result;
}
/// 形状の最下部を取得する
Robotics::PositionVector^ Link::GetBottomOfShape()
{
PositionVector^ result;
double bottom = Double::MaxValue; // 最下部
// 全ての形状の頂点の絶対位置について調べる
for each( PositionVector^ vertex in GetAbsolutePositionOfVertices() )
{
if( vertex->Z < bottom )
{
// 最下部を更新する
bottom = vertex->Z;
// 結果を更新する
result = vertex;
}
}
Debug::Assert( bottom != Double::MaxValue, "最下部は更新される" );
return result;
}
/// 位置と姿勢を設定する
void Link::SetPositionAndPosture( Robotics::PositionVector^ position, Robotics::RotationMatrix^ posture )
{
Debug::Assert( !IsStandard(), "基準リンクの 位置と姿勢は変更されない" );
if( ( position != AbsolutePosition ) || ( posture != m_posture ) )
{
// 位置と姿勢の変更イベントを発生する
PositionAndPostureChanged( this, gcnew PositionAndPostureChangeEventArgs( position, posture ) );
}
}
/// 位置と姿勢を 強制的に設定する
void Link::SetPositionAndPostureForcibly( Robotics::PositionVector^ position, Robotics::RotationMatrix^ posture )
{
// 基準リンクを解除する
m_standard = nullptr;
/// @note
/// 基準リンクの 位置と姿勢は変更できないため、
/// 先にそれを解除する必要がある。
Posture = posture; // 姿勢
AbsolutePosition = position; // 絶対位置
/// @note
/// 位置より先に姿勢を設定しなければならない。
/// (詳細は 絶対位置プロパティを参照せよ)
Debug::Assert( IsSupportingPhase(), "支持相に設定されている" );
// 基準リンクにする
m_standard = this;
/// @note
/// 基準リンクの変更イベントを発生させないために、
/// ここではプロパティを用いずに設定している。
}
/// ローカル座標系における 重心ベクトルを取得する
Robotics::PositionVector^ Link::GetCenterOfGravityInLocalPosition()
{
return( m_posture * m_centerOfGravity );
}
/// ワールド座標系における 重心ベクトルを取得する
Robotics::PositionVector^ Link::GetCenterOfGravityInWorldPosition()
{
// ローカル座標系における重心ベクトルを取得する
PositionVector^ localPosition = GetCenterOfGravityInLocalPosition();
// ... ワールド座標系における重心ベクトルを求める
return m_mass * ( m_joint->AbsolutePosition + localPosition );
}
/// 胴体を設定する [ Property ]
void Link::Body::set( Link^ value )
{
Debug::WriteLine( "胴体を設定する : " + value->Name );
Debug::Assert( value != nullptr, "有効なインスタンスである" );
Debug::Assert( value->Connection->Upper == nullptr, "接続元を持たない" );
// 胴体を設定する
m_body = value;
}
/// 基準リンクを設定する [ Property ]
void Link::Standard::set( Link^ value )
{
Debug::Assert( value != nullptr, "基準リンクは解除されない" );
Debug::Assert( value->IsSupportingPhase(), "支持相である" );
if( m_standard != value )
{
// 現在の値と異なるならば、設定する
m_standard = value;
// ... 基準リンクの変更イベントを発生する
StandardChanged( value, nullptr );
Debug::WriteLine( "基準リンクを設定した : " + value->Name );
}
}
/// 支持相か？
bool Link::IsSupportingPhase()
{
if( GroundHeight < Math::Abs( AbsolutePosition->Z ) )
{
// 地面に接していなければ、支持相ではない
return false;
}
// 姿勢の誤差[ deg ]
double errorOfPosture = Angle::CalculateRadian( 0.1 );
if( !Posture->IsHorizontal( errorOfPosture ) )
{
// 水平でなければ、支持相ではない
return false;
}
return true;
}
/// 胴体まで 自身を経路に追加する (接続元を 胴体まで呼び出す)
void Link::AddOwnToRouteToBody( LinkList^ route )
{
Debug::Assert( route != nullptr, "格納先の経路は生成されている" );
Debug::Assert( Body != nullptr, "胴体は設定されている" );
if( !IsBody() )
{
// 胴体でなければ、自身を経路に追加する
route->Add( this );
Link^ upper = m_connectionRelation->Upper; // 接続元
Debug::Assert( upper != nullptr, "接続元は存在する" );
// ... 接続元の同一関数を呼び出す
upper->AddOwnToRouteToBody( route );
}
// [ Reference ]「ヒューマノイドロボット」梶田秀司(オーム社) P64
}
/// 末端まで 自身を経路に追加する (対等な接続と接続先を 末端まで呼び出す)
void Link::AddOwnToRouteToEnd( LinkList^ route )
{
Debug::Assert( route != nullptr, "格納先の経路は生成されている" );
// 自身を経路に追加する
route->Add( this );
if( ( m_connectionRelation->Equal != nullptr ) && ( route->Count != 1 ) )
{
// 経路の最初でなく 対等な接続が存在するならば、それを呼び出す
m_connectionRelation->Equal->AddOwnToRouteToEnd( route );
}
if( m_connectionRelation->Lower != nullptr )
{
// 接続先が存在するならば、それを呼び出す
m_connectionRelation->Lower->AddOwnToRouteToEnd( route );
}
}
/// 関節の位置が変更された
void Link::JointPositionChanged( System::Object^, System::EventArgs^ e )
{
Debug::Assert( !IsStandard(), "基準リンクの関節の位置は変更されない" );
// 自身を発生元として、イベントを発生する
AbsolutePositionChanged( this, e );
/// @note
/// リンクの絶対位置は、関節の絶対位置から算出している。
/// よって、関節の位置が変更された場合には、リンクも変更されたものとして
/// それを通知する必要がある。
}
/// トルクを求める
double Link::CalculateTorque( Robotics::PositionVector^ force, Robotics::PositionVector^ pointOfAction )
{
// 作用点と関節位置との変位量を求める
PositionVector^ displacement = ( pointOfAction - m_joint->AbsolutePosition );
// ... ワールド座標系での軸方向とその変位量との ベクトルの外積から モーメント・アームの大きさを求める
Vector^ momentArm = Vector::CrossProduct( this->AxialDirection, displacement );
// ... モーメント・アームの大きさと力の内積から トルクを求める
return Vector::InnerProduct( momentArm, force );
// [ Reference ]「ロボット制御基礎論」吉川恒夫(コロナ社) P65
// [ Reference ]「ヒューマノイドロボット」梶田秀司(オーム社) P61
}
/// 図形を生成する
void Link::CreateFigure( Microsoft::DirectX::Direct3D::Device^ device )
{
if( !IsBody() )
{
// 図形の 頂点バッファを生成する
m_figure = gcnew DirectX::Direct3D::VertexBuffer(
DirectX::Direct3D::CustomVertex::PositionColored::typeid, // 頂点データを定義した構造体
2, // 頂点数
device, // デバイス
DirectX::Direct3D::Usage::Dynamic | // 使用法
DirectX::Direct3D::Usage::WriteOnly,
DirectX::Direct3D::CustomVertex::PositionColored::Format, // 頂点フォーマット
DirectX::Direct3D::Pool::Default
);
// 頂点の生成イベントに登録する
m_figure->Created += gcnew System::EventHandler( this, &Link::SetFigure );
// ... そのイベントを呼び出して 設定させる
SetFigure( m_figure, nullptr );
// 関節の図形を生成する
m_joint->CreateFigure( device );
}
// 形状の図形を生成する
m_shape->CreateFigure( device );
}
/// 図形を設定する
void Link::SetFigure( System::Object^ sender, System::EventArgs^ )
{
// 引数から目的とする型を取得する
DirectX::Direct3D::VertexBuffer^ vertexBuffer = safe_cast< DirectX::Direct3D::VertexBuffer^ >( sender );
PositionVector^ position = m_joint->RelativePosition; // 関節の相対位置
// 頂点を格納する配列を生成する
array< DirectX::Direct3D::CustomVertex::PositionColored >^ vertices
= gcnew array< DirectX::Direct3D::CustomVertex::PositionColored >( 2 );
// ... 頂点の位置を設定する
vertices[ 0 ].Position = DirectX::Vector3( 0.0f, 0.0f, 0.0f );
vertices[ 1 ].Position = DirectX::Vector3(
safe_cast< float >( -position->X ),
safe_cast< float >( -position->Y ),
safe_cast< float >( -position->Z )
);
int color = Drawing::Color::White.ToArgb();
// ... 頂点の色を設定する
vertices[ 0 ].Color = color;
vertices[ 1 ].Color = color;
// 頂点データの範囲をロックして、頂点バッファ メモリへのリファレンスを取得する
vertexBuffer->SetData( vertices, 0, DirectX::Direct3D::LockFlags::None );
// [ Reference ]「Managed DirectX9」Tom Miller(Sams) P40
}
/// 機構モデルを描画する
void Link::RenderMechanismModel( Microsoft::DirectX::Direct3D::Device^ device )
{
if( !IsBody() )
{
// ライトを無効にする
device->RenderState->Lighting = false;
// 図形の頂点バッファを デバイスのデータ ストリームにバインドする
device->SetStreamSource( 0, m_figure, 0 );
device->VertexFormat = DirectX::Direct3D::CustomVertex::PositionColored::Format; // 頂点フォーマット
// ... レンダリングする
device->DrawPrimitives(
DirectX::Direct3D::PrimitiveType::LineList,
0, // 頂点の読み出し開始位置
1 // プリミティブの数
);
// 関節を描画する
m_joint->Render( device );
}
/// @note 胴体ならば、胴体の形状を描画する。
/// @note 末端ならば、形状を描画する。
// 基準リンクかどうかで、形状の色を設定する
m_shape->Color = ( IsStandard() )?
Drawing::Color::Gray :
Drawing::Color::White;
if( !IsBody() )
{
// 軸回りに回転する
m_joint->RotateAroundAxis( device );
}
// ... シンプルな形状を描画する
m_shape->RenderSimpleFigure( device );
}
/// リアルなモデルを描画する
void Link::RenderRealModel( Microsoft::DirectX::Direct3D::Device^ device )
{
if( !IsBody() )
{
// 軸回りに回転する
m_joint->RotateAroundAxis( device );
}
// ... リアルな形状を描画する
m_shape->RenderRealFigure( device );
}