PositionChangeOfStabilityPanel.cpp
#include "StdAfx.h"
#include "PositionChangeOfStabilityPanel.h"
#include "Motion.h"
#include "Pose.h"
#include "SupportPolygon.h"
using namespace Core;
using namespace Robotics;
using namespace System;
/// [ Constructor ]
PositionChangeOfStabilityPanel::PositionChangeOfStabilityPanel( void )
{
// コンポーネントを初期化する
InitializeComponent();
}
/// [ Destructor ]
PositionChangeOfStabilityPanel::~PositionChangeOfStabilityPanel()
{
if( components != nullptr )
{
delete components;
}
}
#pragma region Windows Form Designer generated code
/// <summary>
/// デザイナ サポートに必要なメソッドです。このメソッドの内容を
/// コード エディタで変更しないでください。
/// </summary>
void PositionChangeOfStabilityPanel::InitializeComponent(void)
{
this->m_panel = (gcnew System::Windows::Forms::Panel());
this->SuspendLayout();
//
// m_panel
//
this->m_panel->BackColor = System::Drawing::SystemColors::Window;
this->m_panel->Dock = System::Windows::Forms::DockStyle::Fill;
this->m_panel->Location = System::Drawing::Point(0, 0);
this->m_panel->Name = L"m_panel";
this->m_panel->Size = System::Drawing::Size(150, 150);
this->m_panel->TabIndex = 0;
this->m_panel->Paint += gcnew System::Windows::Forms::PaintEventHandler(this, &PositionChangeOfStabilityPanel::PanelPaint);
//
// PositionChangeOfStabilityPanel
//
this->AutoScaleDimensions = System::Drawing::SizeF(6, 12);
this->AutoScaleMode = System::Windows::Forms::AutoScaleMode::Font;
this->Controls->Add(this->m_panel);
this->Name = L"PositionChangeOfStabilityPanel";
this->ResumeLayout(false);
}
#pragma endregion
/// 初期化する
void PositionChangeOfStabilityPanel::Initialize( Motion^ motion )
{
// モーションの変更イベントに登録する
motion->Changed += gcnew System::EventHandler( this, &PositionChangeOfStabilityPanel::MotionChanged );
// モーションの変更イベントを呼び出し、フィールドを初期化する
MotionChanged( motion, nullptr );
}
/// サイズが変更された
void PositionChangeOfStabilityPanel::OnResize( System::EventArgs^ e )
{
// 基本クラスの同一関数を呼び出す
System::Windows::Forms::UserControl::OnResize( e );
// パネルを再描画する
m_panel->Invalidate();
}
/// モーションが変更された
void PositionChangeOfStabilityPanel::MotionChanged( System::Object^ sender, System::EventArgs^ )
{
// 引数から目的とする型を取得する
Motion^ motion = safe_cast< Motion^ >( sender );
// モーションのポーズ数から配列の大きさを決定する
m_centerOfGravity = gcnew array< Drawing::PointF >( motion->PoseCount );
m_supportPolygon = gcnew array< SupportPolygon^ >( motion->PoseCount );
for( int i = 0; i < motion->PoseCount; i++ )
{
m_supportPolygon[ i ] = motion[ i ]->SupportPolygon; // 支持多角形
PositionVector^ position = motion[ i ]->CenterOfGravity;
m_centerOfGravity[ i ] = Drawing::PointF( // 重心
safe_cast< float >( position->X ),
safe_cast< float >( position->Y )
);
}
// 現在のポーズのインデックスを取得する
m_currentPoseIndex = motion->CurrentPoseIndex;
// 現在の安定余裕を取得する
m_stabilityMargin = motion->CurrentPose->StabilityMargin;
if( m_stabilityMargin < 0.0 )
{
// 安定余裕が負の場合には、転倒イベントを発生する
Fall( this, nullptr );
}
// パネルを再描画する
m_panel->Invalidate();
}
/// パネルを再描画する
System::Void PositionChangeOfStabilityPanel::PanelPaint( System::Object^, System::Windows::Forms::PaintEventArgs^ e )
{
Drawing::Graphics^ graphics = e->Graphics; // グラフィックス オブジェクト
// 背景を描画する
DrawBackground( graphics );
// ... 座標系を設定する
SetCoordinate( graphics );
// 支持多角形の軌跡を描画する
DrawLocusSupportPolygon( graphics );
// 重心投影点の軌跡を描画する
DrawLocusOfCenterOfGravityProjection( graphics );
// 安定余裕を描画する
DrawStabilityMargin(
graphics,
m_centerOfGravity[ m_currentPoseIndex ],
m_stabilityMargin
);
}
/// 座標系を設定する
void PositionChangeOfStabilityPanel::SetCoordinate( System::Drawing::Graphics^ graphics )
{
graphics->PageScale = 500.0f; // 表示スケール
// スケールに応じた クライアント領域の大きさを取得する
System::Drawing::SizeF clientSize = graphics->VisibleClipBounds.Size;
// ... それを スケールを考慮して補正する
Drawing::SizeF origin( clientSize.Width / graphics->PageScale, clientSize.Height / graphics->PageScale );
// ... 座標系の原点を設定する
graphics->TranslateTransform( origin.Width / 2.0f, origin.Height / 2.0f );
// Y軸の正の方向が画面の上方となるように 上下を反転する
graphics->ScaleTransform( 1.0f, -1.0f );
}
/// 背景を描画する
void PositionChangeOfStabilityPanel::DrawBackground( System::Drawing::Graphics^ graphics )
{
// ペンを作成する
Drawing::Pen^ pen = gcnew Drawing::Pen( Drawing::Pens::Black->Color, 0.0f );
// ... 終端のキャップスタイルを カスタムにする
pen->EndCap = Drawing::Drawing2D::LineCap::Custom;
// ... それに、矢印型のラインキャップを設定する
pen->CustomEndCap = gcnew Drawing::Drawing2D::AdjustableArrowCap( 5.0f, 5.0f );
const float Margin = 10.0f;
const float LengthOfLine = 30.0f;
Drawing::PointF basePoint( Margin, graphics->VisibleClipBounds.Height - Margin );
// 矢印を描画する
graphics->DrawLine( pen, basePoint, Drawing::PointF( basePoint.X + LengthOfLine, basePoint.Y ) ); // X軸
graphics->DrawLine( pen, basePoint, Drawing::PointF( basePoint.X, basePoint.Y - LengthOfLine ) ); // Y軸
Drawing::Font^ font = this->Font; // フォント
// ブラシを生成する
Drawing::SolidBrush^ brush = gcnew Drawing::SolidBrush( this->ForeColor );
// フォントの大きさを取得する
Drawing::SizeF xSize = graphics->MeasureString( "X", font );
Drawing::SizeF ySize = graphics->MeasureString( "Y", font );
Drawing::PointF xPoint( // Xの表示位置
basePoint.X + LengthOfLine,
basePoint.Y - xSize.Height / 2.0f
);
Drawing::PointF yPoint( // Yの表示位置
basePoint.X - ySize.Width / 2.0f,
basePoint.Y - ySize.Height - LengthOfLine
);
// ... 文字を描画する
graphics->DrawString( "X", font, brush, xPoint );
graphics->DrawString( "Y", font, brush, yPoint );
}
/// 支持多角形を描画する
void PositionChangeOfStabilityPanel::DrawSupportPolygon( System::Drawing::Graphics^ graphics, SupportPolygon^ supportPolygon, System::Drawing::Color color )
{
// 支持多角形の頂点データを 描画メソッドに渡せる型に キャストする
array< Drawing::PointF >^ points = supportPolygon->ToPointF();
// ペンを作成する
Drawing::Pen^ pen = gcnew Drawing::Pen( color, 0.0f );
// ... ポリゴンを描画する
graphics->DrawPolygon( pen, points );
}
/// 支持多角形の軌跡を描画する
void PositionChangeOfStabilityPanel::DrawLocusSupportPolygon( System::Drawing::Graphics^ graphics )
{
for( int i = 0; i < m_supportPolygon->Length; i++ )
{
if( i != m_currentPoseIndex )
{
// 現在のポーズでなければ、支持多角形を描画する
DrawSupportPolygon( graphics, m_supportPolygon[ i ], Drawing::Color::LightGray );
}
}
// 現在のポーズの、支持多角形を描画する
DrawSupportPolygon( graphics, m_supportPolygon[ m_currentPoseIndex ], Drawing::Color::Orange );
}
/// 重心投影点を描画する
void PositionChangeOfStabilityPanel::DrawCenterOfGravityProjection( System::Drawing::Graphics^ graphics, System::Drawing::PointF point )
{
// ペンを作成する
Drawing::Pen^ pen = gcnew Drawing::Pen( Drawing::Pens::Black->Color, 0.0f );
// 表示スケールに応じて 記号の大きさを求める
float markSize = 20.0f / graphics->PageScale;
// 外接する四角形を生成する
Drawing::RectangleF rectangle(
point.X - ( markSize / 2.0f ),
point.Y - ( markSize / 2.0f ),
markSize,
markSize
);
// ベースとなる 白く塗りつぶした円を描画する
graphics->FillEllipse( Drawing::Brushes::White, rectangle );
// ... それを囲う 黒い円を描画する
graphics->DrawEllipse( pen, rectangle );
// 扇形を描く
FillPie( graphics, Drawing::Brushes::Black, rectangle, 0.0f, -90.0f );
FillPie( graphics, Drawing::Brushes::Black, rectangle, 180.0f, -90.0f );
}
/// 重心投影点の軌跡を描画する
void PositionChangeOfStabilityPanel::DrawLocusOfCenterOfGravityProjection( System::Drawing::Graphics^ graphics )
{
// ペンを作成する
Drawing::Pen^ pen = gcnew Drawing::Pen( Drawing::Pens::Black->Color );
// ... 表示スケールに応じて ペンの幅を設定する
pen->Width = 2.0f / graphics->PageScale;
// ... 連続した直線を描く
graphics->DrawLines( pen, m_centerOfGravity );
// 重心投影点を描画する
DrawCenterOfGravityProjection( graphics, m_centerOfGravity[ m_currentPoseIndex ] );
}
/// 安定余裕を描画する
void PositionChangeOfStabilityPanel::DrawStabilityMargin( System::Drawing::Graphics^ graphics, System::Drawing::PointF point, double stabilityMargin )
{
// 安定余裕の符号に応じて 色を設定する
System::Drawing::Pen^ color = ( 0.0 <= stabilityMargin )? Drawing::Pens::Blue : Drawing::Pens::Red;
// ... ペンを作成する
Drawing::Pen^ pen = gcnew Drawing::Pen( color->Color, 0.0f );
// 重心投影点から 安定余裕の位置まで矢印を描画する
}
/// 内部を塗りつぶした扇形を描く
void PositionChangeOfStabilityPanel::FillPie( System::Drawing::Graphics^ graphics, System::Drawing::Brush^ brush, System::Drawing::RectangleF rectangle, float startAngle, float sweepAngle )
{
graphics->FillPie(
brush,
rectangle.X,
rectangle.Y,
rectangle.Width,
rectangle.Height,
startAngle,
sweepAngle
);
/// @note
/// Drawing::Graphics::FillPie()には RectangleFを引数に取るバージョンがない。
}