Geometry.cpp
#include "Geometry.h"
using namespace Robotics;
using namespace System;
using namespace System::Diagnostics;
/// グラハムスキャン (凸包を求める)
Points^ Geometry::GrahamScan( Points^ vertices )
{
int vertexSum = vertices->Length; // 頂点数
Debug::Assert( 3 <= vertexSum, "頂点数は３以上である (さもなければ多角形となり得ない)" );
Geometry^ geometry = gcnew Geometry();
// 演算用に 点の配列を生成する
Points^ result = gcnew Points( vertexSum + 1 );
// ... 配列の前を空けるようにコピーする
Array::Copy(
vertices, // コピー元
0, // コピーを開始するインデックス
result, // コピー先
1, // 格納を開始するインデックス
vertexSum // コピーする要素の数
);
// Y方向に最小、X方向に最大となる点のインデックスを取得する
int pointInCorner = geometry->GetIndexOfPointInCorner( result );
// ... その点と 最初の点を交換する
Point::Swap( result[ pointInCorner ], result[ 1 ] );
// 角度係数を格納する配列を生成する
array< double >^ comparisonOfAngle = gcnew array< double >( vertexSum + 1 );
for( int i=1; i<=vertexSum; i++ )
{
// 角度の間の大小関係の判定を行う係数を取得する
comparisonOfAngle[ i ] = geometry->GetCoefficientComparisonOfAngle( result[ 1 ], result[ i ] );
}
// ... 求めた角度係数に従い 頂点の配列をソートする
geometry->QuickSort(
1, // 区間の左端
vertexSum, // 区間の右端
comparisonOfAngle, // ソートの基準となるもの
result // ソートするもの
);
Debug::Assert( result[ 0 ] == Point( 0.0, 0.0 ), "要素ゼロの頂点は処理されていない" );
// 最初の頂点に 最後の頂点を代入する
result[ 0 ] = result[ vertexSum ];
int count = 2; // 凸包をなしている(処理が終わっている)頂点数
/// @note 基準となる頂点は、最初から処理が終わっているとみなす
// 凸包を形成する
for( int i=3; i<=vertexSum; i++ )
{
// ３点のなす角度が負になるまでくり返して、処理すべき頂点を探索する
while( -1 != geometry->WhetherAngleIsPlusOrMinus(
result[ count + 0 ],
result[ count - 1 ],
result[ i ]
) )
{
count--;
if( count == 0 )
{
// 探索を中断する
break;
/// @note
/// エラー回避のために考案した方法であり、
/// この処理には問題があると思われる。
}
}
count++;
// ２点を交換する
Point::Swap( result[ count ], result[ i ] );
}
// 配列の大きさを、頂点数と同じにする
Array::Resize( result, count );
return result;
// [ Reference ]「Ｃで書くアルゴリズム」疋田輝雄(サイエンス社) P135
}
/// 隅にある点のインデックスを取得する (Y方向に最小、X方向に最大の位置の点)
int Geometry::GetIndexOfPointInCorner( Points^ point )
{
/// @note 要素０は特別な意味を持つため、比較の対象としない
int result = 1;
// Y方向に最小となる点を探索する (最初の点同士を比較する必要はないので、その次の要素から始めている)
for( int i=result + 1; i < point->Length; i++ )
{
if( point[ i ].Y < point[ result ].Y )
{
result = i;
}
}
// ... その点とY座標が一致する点において X方向に最大となる点を探索する
for( int i=1; i < point->Length; i++ )
{
if( ( Math::Abs( point[ i ].Y - point[ result ].Y ) < Double::Epsilon )
&& ( point[ result ].X < point[ i ].X ) )
{
result = i;
}
}
return result;
// [ Reference ]「Ｃで書くアルゴリズム」疋田輝雄(サイエンス社) P135
}
/// 角度の間の大小関係の判定を行う 係数を取得する
double Geometry::GetCoefficientComparisonOfAngle( Point point1, Point point2 )
{
// ２点間の距離を求める
Point distance = point2 - point1;
double result = 0.0;
if( Double::Epsilon <= Math::Abs( distance.X )
|| Double::Epsilon <= Math::Abs( distance.Y ) )
{
// X方向の距離またはY方向の距離が ゼロではない
result = distance.Y / ( Math::Abs( distance.X ) + Math::Abs( distance.Y ) );
}
if( distance.X < 0.0 )
{
// X方向の距離が負数となっている (ここの定数の意味は不明)
result = 2.0 - result;
}
else if( distance.Y < 0.0 )
{
// Y方向の距離が負数となっている (ここの定数の意味は不明)
result = result + 4.0;
}
result *= 90.0;
/// @note この計算の意味は不明
// 係数の範囲を確認する (テキストで示されている範囲。根拠は不明)
Debug::Assert( 0.0 <= result && result <= 360.0, "結果は範囲内にある" );
return result;
// [ Reference ]「Ｃで書くアルゴリズム」疋田輝雄(サイエンス社) P132
}
/// ３点のなす角度は正か負か？
int Geometry::WhetherAngleIsPlusOrMinus( Point middle, Point left, Point right )
{
// 両端の点の距離を求める
Point d1 = right - left;
// 左端と中央の点との距離を求める
Point d2 = middle - left;
double area1 = d1.X * d2.Y;
double area2 = d1.Y * d2.X;
if( area1 > area2 )
{
return +1;
}
if( area1 < area2 )
{
return -1;
}
if( ( d1.X * d2.X < 0.0 ) || ( d1.Y * d2.Y < 0.0 ) )
{
return -1;
}
if( ( ( d1.X * d1.X ) + ( d1.Y * d1.Y ) ) < ( ( d2.X * d2.X ) + ( d2.Y * d2.Y ) ) )
{
return +1;
}
// 基本となる点は ２点の直線上にある
return 0;
// [ Reference ]「Ｃで書くアルゴリズム」疋田輝雄(サイエンス社) P127
}
/// 点と線分の距離を求める
double Geometry::CalculateDistanceOfPointAndSegment( Point point, Segment segment )
{
if( segment.IsSamePoint() )
{
// 線分が点(両端が同一点)ならば それと点との距離を返す
return Point::CalculateDistance( segment.Point1, point );
}
Point dPoint = segment.Point2 - segment.Point1;
Point origin = segment.Point1 - point;
double a = ( dPoint.X * dPoint.X ) + ( dPoint.Y * dPoint.Y );
double b = ( dPoint.X * origin.X ) + ( dPoint.Y * origin.Y );
// 媒介変数を求める
double parameter = -( b / a );
/// @note
/// 媒介変数を０から１の範囲に押さえ込むことで、垂線の足が線分の範囲外になるときに
/// それを線分の端に合わせることができる。
if( parameter < 0.0 ) parameter = 0.0;
if( parameter > 1.0 ) parameter = 1.0;
// 垂線の足を求める
Point perpendicular = segment.Point1 + ( parameter * dPoint );
// ... それと点との距離を求める
return Point::CalculateDistance( perpendicular, point );
// [ Reference ] ToyWiki - 線分と点の距離 ( http://toycode.com/toywiki/pages/_E7B79AE58886E381A8E782B9E381AEE8B79DE99BA2_.html )
}
/// 多角形の内部か？
bool Geometry::IsInsideOfPolygon( Points^ vertices, Point target )
{
int vertexSum = vertices->Length; // 頂点数
Debug::Assert( 3 <= vertexSum, "頂点数は３以上である (さもなければ多角形となり得ない)" );
// 演算用に 点の配列を生成する
Points^ points = gcnew Points( vertexSum + 2 );
// ... 配列の前後を空けるようにコピーする
Array::Copy(
vertices, // コピー元
0, // コピーを開始するインデックス
points, // コピー先
1, // 格納を開始するインデックス
vertexSum // コピーする要素の数
);
// 最初に 最後の頂点を格納する
points[ 0 ] = vertices[ vertexSum - 1 ];
// 最後に 最初の頂点を格納する
points[ vertexSum + 1 ] = vertices[ 0 ];
int crossingCount = 0; // 交差数
Point point = points[ 0 ]; // 点
// 幾何学クラスを生成する
Geometry^ geometry = gcnew Geometry();
// 対象とする点から X軸と平行に正の方向へ向かう 半直線を定義する
Segment halfLine( target, Point( Double::MaxValue, target.Y ) );
for( int i=1; i<=vertexSum; i++ )
{
// 多角形の頂点上に 大きさゼロの線分を定義する
Segment segment( points[ i ], points[ i ] );
if( !geometry->IsSegmentIntersect( segment, halfLine ) )
{
// それが半直線と交差しないならば、線分の一端を定義しなおす
segment.Point2 = point;
// ... 点を 対象としている頂点に定義しなおす
point = points[ i ];
if( geometry->IsSegmentIntersect( segment, halfLine ) )
{
// 半直線と交差するならば、交差数をカウントする
crossingCount++;
}
}
}
// 奇数個の点が交差するならば 点が内部にあると判断する
return( crossingCount & 1 );
// [ Reference ]「Ｃで書くアルゴリズム」疋田輝雄(サイエンス社) P129
}
/// 線分は交差するか？
bool Geometry::IsSegmentIntersect( Segment segment1, Segment segment2 )
{
int case1 = WhetherAngleIsPlusOrMinus( segment1.Point1, segment1.Point2, segment2.Point1 )
* WhetherAngleIsPlusOrMinus( segment1.Point1, segment1.Point2, segment2.Point2 );
int case2 = WhetherAngleIsPlusOrMinus( segment2.Point1, segment2.Point2, segment1.Point1 )
* WhetherAngleIsPlusOrMinus( segment2.Point1, segment2.Point2, segment1.Point2 );
return ( case1 <= 0 ) && ( case2 <= 0 );
// [ Reference ]「Ｃで書くアルゴリズム」疋田輝雄(サイエンス社) P128
}
/// クイックソートをする
void Geometry::QuickSort( int leftEdge, int rightEdge, array< double >^ reference, Points^% points )
{
Debug::Assert( reference->Length == points->Length, "参照値と点の数は一致している" );
// 基準値(区間の中央の値) を求める
const double standardValue = reference[ ( leftEdge + rightEdge ) / 2 ];
int left = leftEdge;
int right = rightEdge;
while( left <= right )
{
// 区間の左端から 基準値より大きなものを見つける
while( reference[ left ] < standardValue )
{
left++;
}
// 区間の右端から 基準値より小さなものを見つける
while( reference[ right ] > standardValue )
{
right--;
}
if( left <= right )
{
// ２点を交換する
Point::Swap( points[ left ], points[ right ] );
// ... ソートの参照値も交換する
Swap( reference[ left ], reference[ right ] );
left++;
right--;
}
}
// 区間の左端に達するまで 処理をくり返す
if( leftEdge < right )
{
QuickSort( leftEdge, right, reference, points );
}
// 区間の右端に達するまで 処理をくり返
if( left < rightEdge )
{
QuickSort( left, rightEdge, reference, points );
}
// [ Reference ]「Ｃで書くアルゴリズム」疋田輝雄(サイエンス社) P135
}
/// ２つの値を交換する
void Geometry::Swap( double% left, double% right )
{
double swap = left;
left = right;
right = swap;
}