SupportPolygon.cpp
#include "StdAfx.h"
#include "SupportPolygon.h"
#include "Link.h"
using namespace Core;
using namespace Robotics;
using namespace Microsoft;
using namespace System;
using namespace System::Diagnostics;
// DirectXのライブラリによる const/volatile 修飾子の使用の警告を抑制する
#pragma warning( disable : 4400 )
/// [ Constructor ]
SupportPolygon::SupportPolygon( System::Collections::Generic::List< Robotics::PositionVector^ >^ positionVectores )
{
int vertexSum = positionVectores->Count; // 頂点数
Debug::Assert( 3 <= vertexSum, "頂点数は３以上である (さもなければ多角形となり得ない)" );
// 頂点数分だけメモリを確保する
Points^ vertices = gcnew Points( vertexSum );
// ３次元の位置ベクトルから、XY平面上の位置に変換する
for( int i = 0; i < vertexSum; i++ )
{
Debug::Assert( positionVectores[ i ]->Z < Link::GroundHeight, "支持多角形となり得る頂点は接地している" );
/// @note
/// ここでは形状の頂点の位置を確認しているが、リンクが定義している
/// 「接地しているとみなす高さ」はリンクの位置であり、そこに相違がある。
vertices[ i ] = Point(
positionVectores[ i ]->X,
positionVectores[ i ]->Y
);
}
// 凸包を求める
m_vertices = Geometry::GrahamScan( vertices );
}
/// [ Copy constructor ]
SupportPolygon::SupportPolygon( SupportPolygon^ other )
{
// 頂点を 同じ数だけ生成する
m_vertices = gcnew Points( other->CountOfVertex );
// ... 全ての頂点をコピーする
other->m_vertices->CopyTo( m_vertices, 0 );
}
/// [ Finalizer ]
SupportPolygon::!SupportPolygon()
{
delete m_figure;
}
/// 生成する
SupportPolygon^ SupportPolygon::Create( Links^ links )
{
// 支持多角形を構成する頂点を格納するためのリストを生成する
Collections::Generic::List< PositionVector^ >^ composeVertex
= gcnew Collections::Generic::List< PositionVector^ >();
// 支持多角形を構成する頂点を集める
for each( Link^ link in links )
{
if( link->IsSupportingPhase() )
{
// 支持相ならば、形状の頂点を取得する
PositionVectors^ vertexsOfShape = link->GetAbsolutePositionOfVertices();
// ... それの下面だけを、構成する頂点に追加する
for( int i = 0; i < ( vertexsOfShape->Length / 2); i++ )
{
composeVertex->Add( vertexsOfShape[ i ] );
}
}
}
Debug::Assert( composeVertex->Count != 0, "支持相のリンクは存在する" );
// 生成して返す
return gcnew SupportPolygon( composeVertex );
}
/// 安定余裕を求める
double SupportPolygon::CalculateStabilityMargin( Robotics::PositionVector^ centerOfGravity )
{
// 重心投影点を求める
Point centerOfGravityProjection( centerOfGravity->X, centerOfGravity->Y );
double result = Double::MaxValue; // 安定余裕
for( int i = 0; i < CountOfVertex; i++ )
{
// 重心投影点と 支持多角形を構成する線分との距離を求める
double distance = Geometry::CalculateDistanceOfPointAndSegment(
centerOfGravityProjection, Segment[ i ]
);
if( distance < result )
{
// その距離が現在の値より小さければ これを更新する
result = distance;
}
}
Debug::Assert( 0.0 <= result, "点と線分との距離は 正数である" );
if( !Geometry::IsInsideOfPolygon( m_vertices, centerOfGravityProjection ) )
{
// 重心投影点が支持多角形の外部にあるならば、符合を反転する
result = -result;
}
return result;
}
/// 線分を取得する [ Property ]
Robotics::Segment SupportPolygon::Segment::get( int index )
{
Debug::Assert( index < CountOfVertex, "インデックスは 存在する頂点を示している" );
// 最後の頂点ならば最初の頂点を、さもなくば次の頂点を指定する
int nextIndex = ( index == ( CountOfVertex - 1 ) )? 0 : index + 1;
// ... ２つの頂点を結んだ線分を 生成して返す
return Robotics::Segment(
m_vertices[ index ],
m_vertices[ nextIndex ]
);
}
/// PointF構造体に キャストして返す
array< System::Drawing::PointF >^ SupportPolygon::ToPointF()
{
array< Drawing::PointF >^ result = gcnew array< Drawing::PointF >( CountOfVertex );
for( int i = 0; i < CountOfVertex; i++ )
{
result[ i ] = m_vertices[ i ].ToPointF();
}
return result;
}
/// 図形を生成する
void SupportPolygon::CreateFigure( Microsoft::DirectX::Direct3D::Device^ device )
{
// 図形の 頂点バッファを生成する
m_figure = gcnew DirectX::Direct3D::VertexBuffer(
DirectX::Direct3D::CustomVertex::PositionColored::typeid, // 頂点データを定義した構造体
CountOfVertex + 1, // 頂点数
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, &SupportPolygon::SetFigure );
// ... そのイベントを呼び出して 設定させる
SetFigure( m_figure, nullptr );
}
/// 図形を設定する
void SupportPolygon::SetFigure( System::Object^ sender, System::EventArgs^ )
{
// 引数から目的とする型を取得する
DirectX::Direct3D::VertexBuffer^ vertexBuffer
= safe_cast< DirectX::Direct3D::VertexBuffer^ >( sender );
// 頂点を格納する配列を生成する
array< DirectX::Direct3D::CustomVertex::PositionColored >^ vertices
= gcnew array< DirectX::Direct3D::CustomVertex::PositionColored >( CountOfVertex + 1 );
for( int i = 0; i < CountOfVertex; i++ )
{
vertices[ i ].Position = DirectX::Vector3( // 頂点の位置
safe_cast< float >( m_vertices[ i ].X ),
safe_cast< float >( m_vertices[ i ].Y ),
0.0f
);
vertices[ i ].Color = Drawing::Color::Orange.ToArgb(); // 頂点の色
}
// 最後の頂点を 最初の頂点に重ねて ループを閉じる
vertices[ CountOfVertex ].Position = vertices[ 0 ].Position;
vertices[ CountOfVertex ].Color = vertices[ 0 ].Color;
// 頂点データの範囲をロックして、頂点バッファ メモリへのリファレンスを取得する
vertexBuffer->SetData( vertices, 0, DirectX::Direct3D::LockFlags::None );
// [ Reference ]「Managed DirectX9」Tom Miller(Sams) P40
}
/// 描画する
void SupportPolygon::Render( Microsoft::DirectX::Direct3D::Device^ device )
{
Debug::Assert( m_figure != nullptr, "図形は生成されている" );
// ライトを無効にする
device->RenderState->Lighting = false;
// 変換行列を初期化する
device->Transform->World = DirectX::Matrix::Identity;
// 座標軸の頂点バッファを デバイスのデータ ストリームにバインドする
device->SetStreamSource( 0, m_figure, 0 );
device->VertexFormat = DirectX::Direct3D::CustomVertex::PositionColored::Format; // 頂点フォーマット
// ... レンダリングする
device->DrawPrimitives(
DirectX::Direct3D::PrimitiveType::LineStrip,
0, // 頂点の読み出し開始位置
CountOfVertex // プリミティブの数
);
}