747 lines
22 KiB
C++
747 lines
22 KiB
C++
// X2DPathFinder.h
|
|
//
|
|
// by Testors , 2005/08/01
|
|
|
|
#pragma once
|
|
|
|
|
|
#define WIN32_LEAN_AND_MEAN
|
|
#include <windows.h>
|
|
|
|
#include <vector>
|
|
|
|
#include "X2DPolygon.h"
|
|
#include "X2DLeakyPolygon.h"
|
|
#include "../toolkit/XCyclicIndex.h"
|
|
#include "../toolkit/XDijkstra.h"
|
|
|
|
|
|
const int MAX_POLYGON_POINT = 300; //150 정도 되면, 제대로 검사 안됨
|
|
|
|
|
|
namespace X2D
|
|
{
|
|
|
|
template< typename T >
|
|
struct DeadEndChecker
|
|
{
|
|
static bool IsDeadEnd( const T& polygon, size_t index ) { return false; }
|
|
};
|
|
|
|
template< typename T >
|
|
struct DeadEndChecker< X2D::LeakyPolygon< T > >
|
|
{
|
|
static bool IsDeadEnd( const X2D::LeakyPolygon< T >& polygon, size_t index )
|
|
{
|
|
return polygon.IsDeadEnd( index );
|
|
}
|
|
};
|
|
|
|
template< typename T, typename PT >
|
|
struct PathFinder
|
|
{
|
|
public:
|
|
|
|
typedef typename NakedType< PT >::result NakedPT;
|
|
typedef std::vector< Point<T> > PATH;
|
|
typedef DeadEndChecker< NakedPT > CheckDeadEnd;
|
|
|
|
template< typename PI >
|
|
static void Find( PI begin ,PI end, const X2D::Point<T> & start, const X2D::Point<T> & goal, PATH & vPath, const T & max_length = 0 )
|
|
{
|
|
_Find( begin, end, start, goal, vPath, max_length );
|
|
}
|
|
|
|
private:
|
|
|
|
static inline T GetFastPathLength( const std::vector< Point<T> > & vPath )
|
|
{
|
|
T total_length = 0;
|
|
for( size_t i = 0; i+1 < vPath.size(); ++i )
|
|
{
|
|
total_length += vPath[i].GetDistance( vPath[i+1] );
|
|
}
|
|
return total_length;
|
|
}
|
|
|
|
static inline bool IS_INSIDE( const bool & bIsClockWise, const Line<T> & prev_line, const Line<T> & line, const Point<T> & pt )
|
|
{
|
|
assert( prev_line.end == line.begin );
|
|
|
|
bool bIsLeftSide = false;
|
|
|
|
CCW_RESULT result_p = CheckClockWise( prev_line.begin, prev_line.end, line.end );
|
|
CCW_RESULT result_1 = CheckClockWise( prev_line.begin, prev_line.end, pt );
|
|
CCW_RESULT result_2 = CheckClockWise( line.end, line.begin, pt );
|
|
|
|
if( bIsClockWise )
|
|
{
|
|
if( result_p == X2D::PARALLELISM )
|
|
{
|
|
return ( result_1 == X2D::CLOCK_WISE && result_2 == X2D::COUNTER_CLOCK_WISE );
|
|
}
|
|
else if( result_p == X2D::CLOCK_WISE )
|
|
{
|
|
return ( result_1 == X2D::CLOCK_WISE && result_2 == X2D::COUNTER_CLOCK_WISE );
|
|
}
|
|
else if( result_p == X2D::COUNTER_CLOCK_WISE )
|
|
{
|
|
if( result_1 == X2D::CLOCK_WISE && result_2 == X2D::CLOCK_WISE ) return true;
|
|
else if( result_1 == X2D::PARALLELISM && result_2 == X2D::COUNTER_CLOCK_WISE ) return true;
|
|
else if( result_2 == X2D::PARALLELISM && result_1 == X2D::CLOCK_WISE ) return true;
|
|
else if( result_1 == X2D::CLOCK_WISE && result_2 == X2D::COUNTER_CLOCK_WISE ) return true;
|
|
else if( result_1 == X2D::COUNTER_CLOCK_WISE && result_2 == X2D::COUNTER_CLOCK_WISE ) return true;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if( result_p == X2D::PARALLELISM )
|
|
{
|
|
return ( result_1 == X2D::COUNTER_CLOCK_WISE && result_2 == X2D::CLOCK_WISE );
|
|
}
|
|
else if( result_p == X2D::COUNTER_CLOCK_WISE )
|
|
{
|
|
return ( result_1 == X2D::COUNTER_CLOCK_WISE && result_2 == X2D::CLOCK_WISE );
|
|
}
|
|
else if( result_p == X2D::CLOCK_WISE )
|
|
{
|
|
if( result_1 == X2D::CLOCK_WISE && result_2 == X2D::CLOCK_WISE ) return true;
|
|
else if( result_1 == X2D::PARALLELISM && result_2 == X2D::CLOCK_WISE ) return true;
|
|
else if( result_2 == X2D::PARALLELISM && result_1 == X2D::COUNTER_CLOCK_WISE ) return true;
|
|
else if( result_1 == X2D::COUNTER_CLOCK_WISE && result_2 == X2D::CLOCK_WISE ) return true;
|
|
else if( result_1 == X2D::COUNTER_CLOCK_WISE && result_2 == X2D::COUNTER_CLOCK_WISE ) return true;
|
|
}
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
static inline bool LOOSE_COLLISION( const bool & bIsClockWise, const Line<T> & prev_line, const Line<T> & line, const Line<T> & target )
|
|
{
|
|
return IS_INSIDE( bIsClockWise, prev_line, line, target.begin ) || IS_INSIDE( bIsClockWise, prev_line, line, target.end );
|
|
}
|
|
|
|
// 길찾기를 위해 필요한 느슨한 라인-폴리곤 충돌검사.
|
|
// 만약 라인이 폴리곤을 구성하는 라인과 겹친다면 false 를 리턴한다.
|
|
static inline bool LOOSE_COLLISION( const NakedPT & p, const Line<T> & line )
|
|
{
|
|
// TODO : 바운딩 박스 체크
|
|
if( !p.GetBoundingBox().IsCollision( line ) ) return false;
|
|
|
|
// 라인이 폴리곤을 구성하는 라인중 하나와 동일하다면 false
|
|
//if( p.Has( line ) ) return false;
|
|
|
|
size_t nTouchCnt = 0;
|
|
|
|
Line<T>::INTERSECT_RESULT result = Line<T>::SEPARATE;
|
|
Line<T>::INTERSECT_RESULT first_result = Line<T>::SEPARATE;
|
|
Line<T>::INTERSECT_RESULT prev_result = Line<T>::SEPARATE;
|
|
for( size_t idx = 0; idx < p.Size(); ++idx )
|
|
{
|
|
Line<T> current_line = p.GetSegment( idx );
|
|
result = current_line.IntersectCCW( line );
|
|
|
|
// 라인이 교차하면 충돌이다.
|
|
if( result == Line<T>::INTERSECT ) return true;
|
|
|
|
if( result == Line<T>::TOUCH )
|
|
{
|
|
if( idx == 0 ) first_result = result;
|
|
else
|
|
{
|
|
if( prev_result == Line<T>::TOUCH )
|
|
{
|
|
if( LOOSE_COLLISION( p.IsClockWise(), p.GetSegment( idx - 1 ), current_line, line ) )
|
|
return true;
|
|
}
|
|
}
|
|
|
|
nTouchCnt++;
|
|
}
|
|
prev_result = result;
|
|
}
|
|
|
|
if( result == Line<T>::TOUCH && first_result == Line<T>::TOUCH )
|
|
{
|
|
if( LOOSE_COLLISION( p.IsClockWise(), Line< T >( p.GetPoint( p.Size()-1 ), p.GetPoint( 0 ) ), p.GetSegment( 0 ), line ) )
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
template< typename PI >
|
|
static inline bool LOOSE_COLLISION( PI begin ,PI end, const Line<T> & line )
|
|
{
|
|
for( ; begin != end; ++begin )
|
|
{
|
|
if( LOOSE_COLLISION( *(*begin), line ) ) return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
template< typename PI >
|
|
static inline bool INCLUDE( PI begin ,PI end, const Point<T> & pt )
|
|
{
|
|
for( ; begin != end; ++begin )
|
|
{
|
|
if( X2D::INCLUDE( *begin, pt ) ) return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
static inline void NORMALIZE( const Polygon<T> & p, std::vector< Point<T> > & vResult )
|
|
{
|
|
size_t z = vResult.size();
|
|
|
|
if( z < 3 ) return;
|
|
|
|
size_t nStartIdx = 1;
|
|
if( p.Has( vResult.front() ) ) nStartIdx = 0;
|
|
|
|
for( size_t i = 1; i < z-1; ++i )
|
|
{
|
|
for( size_t x = 0; x < z - i - 2; ++x )
|
|
{
|
|
size_t t = z - 1 - x;
|
|
|
|
if( p.Has( Line<T>( vResult[i], vResult[t] ) ) ) continue;
|
|
|
|
if( LOOSE_COLLISION( p, Line<T>( vResult[i], vResult[t] ) ) ) continue;
|
|
|
|
vResult.erase( vResult.begin()+i+1, vResult.begin()+t );
|
|
|
|
NORMALIZE( p, vResult );
|
|
return;
|
|
}
|
|
}
|
|
}
|
|
|
|
template< typename PI >
|
|
static inline void NORMALIZE( PI begin ,PI end, std::vector< Point<T> > & vResult )
|
|
{
|
|
size_t z = vResult.size();
|
|
|
|
if( z < 3 ) return;
|
|
|
|
for( size_t i = 0; i < z-1; ++i )
|
|
{
|
|
for( size_t x = 0; x < z - i - 2; ++x )
|
|
{
|
|
size_t t = z - 1 - x;
|
|
|
|
//if( p.Has( Line<T>( vResult[i], vResult[t] ) ) ) continue;
|
|
|
|
if( LOOSE_COLLISION( begin, end, Line<T>( vResult[i], vResult[t] ) ) ) continue;
|
|
|
|
vResult.erase( vResult.begin()+i+1, vResult.begin()+t );
|
|
|
|
NORMALIZE( begin, end, vResult );
|
|
return;
|
|
}
|
|
}
|
|
}
|
|
|
|
static void getPath( const NakedPT & pl, const Point<T> & start, const Point<T> & end, PATH * pvPath )
|
|
{
|
|
PATH vGreenPath, vLeftPath, vRightPath;
|
|
std::vector< Point<T> > vGreenNodeList;
|
|
|
|
// bool bRtn = LOOSE_COLLISION( pl, Line<T>( Point<int>( 1250, 349 ), Point<int>( 775, 508 ) ) );
|
|
|
|
// bool bStartNodeIsPartOfPolygon = pl.Has( start );
|
|
|
|
pvPath->reserve( pl.Size() + 2 );
|
|
|
|
// 각 점들이 시작점과 끝점에서 직접 도달 가능한지 검사해 놓는다.
|
|
enum {
|
|
START = 0x0001 << 0,
|
|
END = 0x0001 << 1,
|
|
DEADEND = 0x0001 << 2,
|
|
};
|
|
std::vector< char > vVisibleProperty( pl.Size(), 0 );
|
|
size_t nStartIndex = std::numeric_limits< size_t >::max();
|
|
T nStartLength = std::numeric_limits< T >::max();
|
|
|
|
// 퍼포먼스를 위해서.. 임시로 삽입. 정밀하게 하고싶으면 nAdd 를 1로 세팅할것.
|
|
//size_t nAdd = ( pl.Size()/10 + 1 );
|
|
size_t nAdd = 1; // 1.11.1 실내 던전 길찾기 오류 수정
|
|
|
|
CCW_RESULT nResult = pl.IsClockWise() ? CLOCK_WISE : COUNTER_CLOCK_WISE;
|
|
for( size_t i = 0; i < pl.Size(); i += nAdd )
|
|
{
|
|
X2D::Point< T > pt = pl.GetPoint(i);
|
|
|
|
if( CheckDeadEnd::IsDeadEnd( pl, i ) )
|
|
vVisibleProperty[ i ] |= DEADEND;
|
|
|
|
|
|
// { 일종의 은면제거랄까.. -_-
|
|
bool bNeedStartCheck = true;
|
|
bool bNeedEndCheck = true;
|
|
|
|
X2D::Point< T > prev_pt = pl.GetPrevPoint( i );
|
|
X2D::Point< T > next_pt = pl.GetNextPoint( i );
|
|
|
|
CCW_RESULT nResult1 = X2D::CheckClockWise( start, pt, prev_pt );
|
|
CCW_RESULT nResult2 = X2D::CheckClockWise( start, pt, next_pt );
|
|
CCW_RESULT nResult3 = X2D::CheckClockWise( end, pt, prev_pt );
|
|
CCW_RESULT nResult4 = X2D::CheckClockWise( end, pt, next_pt );
|
|
|
|
CCW_RESULT nResultPl = X2D::CheckClockWise( prev_pt, pt, next_pt );
|
|
if( nResultPl == nResult )
|
|
{
|
|
if( nResult1 != PARALLELISM && nResult2 != PARALLELISM && nResult1 != nResult2 && nResult != nResult1 ) bNeedStartCheck = false;
|
|
if( nResult3 != PARALLELISM && nResult4 != PARALLELISM && nResult3 != nResult4 && nResult != nResult3 ) bNeedEndCheck = false;
|
|
}
|
|
else
|
|
{
|
|
if( nResult1 == PARALLELISM || nResult2 == PARALLELISM || nResult1 == nResult2 || nResultPl == nResult1 )
|
|
bNeedStartCheck = false;
|
|
if( nResult3 == PARALLELISM || nResult4 == PARALLELISM || nResult3 == nResult4 || nResultPl == nResult3 )
|
|
bNeedEndCheck = false;
|
|
}
|
|
// }
|
|
|
|
if( pl.GetPoint(i) == start )
|
|
{
|
|
vVisibleProperty[i] |= START;
|
|
}
|
|
if( pl.GetPoint(i) == end )
|
|
{
|
|
vVisibleProperty[i] |= END;
|
|
}
|
|
if( bNeedStartCheck && !LOOSE_COLLISION( pl, Line<T>( pt, start ) ) )
|
|
{
|
|
vVisibleProperty[i] |= START;
|
|
}
|
|
if( bNeedEndCheck && !LOOSE_COLLISION( pl, Line<T>( pt, end ) ) )
|
|
{
|
|
vVisibleProperty[i] |= END;
|
|
}
|
|
|
|
if( vVisibleProperty[i] & START )
|
|
{
|
|
// { 한 점만 거쳐서 바로 도달이 가능하다면 그 길을 찾아 놓는다.
|
|
if( vVisibleProperty[i] & END )
|
|
{
|
|
vGreenNodeList.push_back( pt );
|
|
}
|
|
// }
|
|
else
|
|
{
|
|
T length = start.GetDistance( pt );
|
|
if( length < nStartLength )
|
|
{
|
|
nStartLength = length;
|
|
nStartIndex = i;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// { 한번에 거쳐가는 길 중 가장 빠른것을 추려낸다.
|
|
if( !vGreenNodeList.empty() )
|
|
{
|
|
T green_length = std::numeric_limits< T >::max();
|
|
size_t idx = 0;
|
|
for( size_t i = 0; i < vGreenNodeList.size(); ++i )
|
|
{
|
|
T current_path_length = vGreenNodeList[i].GetDistance( start ) + vGreenNodeList[i].GetDistance( end );
|
|
if( current_path_length < green_length )
|
|
{
|
|
green_length = current_path_length;
|
|
idx = i;
|
|
}
|
|
}
|
|
vGreenPath.reserve( 3 );
|
|
vGreenPath.push_back( start );
|
|
vGreenPath.push_back( vGreenNodeList[idx] );
|
|
vGreenPath.push_back( end );
|
|
}
|
|
// }
|
|
|
|
if( nStartIndex == std::numeric_limits< size_t >::max() )
|
|
{
|
|
//assert( 0 );
|
|
return;
|
|
}
|
|
|
|
vLeftPath.push_back( start );
|
|
vRightPath.push_back( start );
|
|
|
|
bool bLeftEnd = false;
|
|
bool bRightEnd = false;
|
|
|
|
T nRightLength = 0;
|
|
T nLeftLength = 0;
|
|
|
|
xmod::cyclic_index< int > left_idx( 0, int( pl.Size() ) ), next_left_idx( 0, int( pl.Size() ) );
|
|
xmod::cyclic_index< int > right_idx( 0, int( pl.Size() ) ), next_right_idx( 0, int( pl.Size() ) );
|
|
left_idx = int( nStartIndex );
|
|
right_idx = int( nStartIndex );
|
|
|
|
for( size_t i = 0; i < pl.Size() ; ++i, ++left_idx, --right_idx )
|
|
{
|
|
if( bRightEnd && bLeftEnd ) break;
|
|
|
|
// { 왼쪽으로 비비고~
|
|
while( !bLeftEnd && vLeftPath.size() < MAX_POLYGON_POINT )
|
|
{
|
|
next_left_idx = left_idx + 1;
|
|
|
|
vLeftPath.push_back( pl.GetPoint(left_idx) );
|
|
nLeftLength += vLeftPath[ vLeftPath.size() - 2 ].GetDistance( vLeftPath[ vLeftPath.size() - 1 ] );
|
|
|
|
if( vVisibleProperty[left_idx] & END )
|
|
{
|
|
vLeftPath.push_back( end );
|
|
nLeftLength += vLeftPath[ vLeftPath.size() - 2 ].GetDistance( vLeftPath[ vLeftPath.size() - 1 ] );
|
|
bLeftEnd = true;
|
|
break;
|
|
}
|
|
|
|
if( vVisibleProperty[left_idx] & START )
|
|
{
|
|
nLeftLength = 0;
|
|
vLeftPath.erase( vLeftPath.begin() + 1, vLeftPath.end() );
|
|
vLeftPath.push_back( pl.GetPoint(left_idx) );
|
|
nLeftLength += vLeftPath[ vLeftPath.size() - 2 ].GetDistance( vLeftPath[ vLeftPath.size() - 1 ] );
|
|
}
|
|
|
|
if( bRightEnd && !vRightPath.empty() )
|
|
{
|
|
if( nLeftLength > nRightLength * 2 )
|
|
{
|
|
vLeftPath.clear();
|
|
bLeftEnd = true;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if( ( vVisibleProperty[ left_idx ] & DEADEND ) && ( vVisibleProperty[ next_left_idx ] & DEADEND ) )
|
|
{
|
|
vLeftPath.clear();
|
|
bLeftEnd = true;
|
|
break;
|
|
}
|
|
|
|
break;
|
|
}
|
|
// }
|
|
|
|
// { 오른쪽으로 비비고~
|
|
while( !bRightEnd && vRightPath.size() < MAX_POLYGON_POINT )
|
|
{
|
|
next_right_idx = right_idx - 1;
|
|
|
|
vRightPath.push_back( pl.GetPoint(right_idx) );
|
|
nRightLength += vRightPath[ vRightPath.size() - 2 ].GetDistance( vRightPath[ vRightPath.size() - 1 ] );
|
|
|
|
if( vVisibleProperty[right_idx] & END )
|
|
{
|
|
vRightPath.push_back( end );
|
|
nRightLength += vRightPath[ vRightPath.size() - 2 ].GetDistance( vRightPath[ vRightPath.size() - 1 ] );
|
|
bRightEnd = true;
|
|
break;
|
|
}
|
|
|
|
if( vVisibleProperty[right_idx] & START )
|
|
{
|
|
nRightLength = 0;
|
|
vRightPath.erase( vRightPath.begin() + 1, vRightPath.end() );
|
|
vRightPath.push_back( pl.GetPoint(right_idx) );
|
|
nRightLength += vRightPath[ vRightPath.size() - 2 ].GetDistance( vRightPath[ vRightPath.size() - 1 ] );
|
|
}
|
|
|
|
if( bLeftEnd && !vLeftPath.empty() )
|
|
{
|
|
if( nRightLength > nLeftLength * 2 )
|
|
{
|
|
vRightPath.clear();
|
|
bRightEnd = true;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if( ( vVisibleProperty[ right_idx ] & DEADEND ) && ( vVisibleProperty[ next_right_idx ] & DEADEND ) )
|
|
{
|
|
// we couldn't found a valid path while traveling to the right
|
|
vRightPath.clear();
|
|
bRightEnd = true;
|
|
break;
|
|
}
|
|
|
|
break;
|
|
}
|
|
// }
|
|
}
|
|
|
|
/*
|
|
NORMALIZE( pl, vLeftPath ); // 임시로 막음. 켜면 품질이좋지만 더 느려짐
|
|
NORMALIZE( pl, vRightPath ); // 임시로 막음. 켜면 품질이좋지만 더 느려짐
|
|
*/
|
|
|
|
/*
|
|
if( !vLeftPath.empty() )
|
|
{
|
|
vLeftPath.push_back( end );
|
|
//NORMALIZE( pl, vLeftPath ); // 임시로 막음. 켜면 품질이좋지만 더 느려짐
|
|
} else vLeftPath.clear();
|
|
|
|
if( !vRightPath.empty() )
|
|
{
|
|
vRightPath.push_back( end );
|
|
//NORMALIZE( pl, vRightPath ); //임시로 막음. 켜면 품질이좋지만 더 느려짐
|
|
} else vRightPath.clear();
|
|
*/
|
|
|
|
if( vGreenPath.empty() && vLeftPath.empty() && vRightPath.empty() )
|
|
return;
|
|
|
|
// 성능문제가 일어날것 같으면 길찾기 포기
|
|
if( vLeftPath.size() >= MAX_POLYGON_POINT )
|
|
{
|
|
//_oprint( "길찾기 에러 : vLeftPath.size() >= MAX_POLYGON_POINT\n" );
|
|
vLeftPath.clear();
|
|
}
|
|
if( vRightPath.size() >= MAX_POLYGON_POINT )
|
|
{
|
|
//_oprint( "길찾기 에러 : vRightPath.clear() >= MAX_POLYGON_POINT\n" );
|
|
vRightPath.clear();
|
|
}
|
|
|
|
dijkstra( pl, vLeftPath, vRightPath, vGreenPath, start, end, *pvPath );
|
|
}
|
|
|
|
struct GRAPH
|
|
{
|
|
GRAPH( size_t nNodeCount )
|
|
{
|
|
m_pGraph = new T[ nNodeCount*nNodeCount ];
|
|
m_nNodeCount = nNodeCount;
|
|
for( size_t idx = 0; idx < nNodeCount*nNodeCount; ++idx ) m_pGraph[idx] = std::numeric_limits<T>::max();
|
|
for( size_t idx = 0; idx < nNodeCount; ++idx ) SetCost( idx, idx, 0 );
|
|
}
|
|
~GRAPH() { delete [] m_pGraph; }
|
|
|
|
T GetCost( size_t from, size_t to ) const { return m_pGraph[ to * m_nNodeCount + from ]; }
|
|
void SetCost( size_t from, size_t to, const T & v ) { assert( to * m_nNodeCount + from < m_nNodeCount*m_nNodeCount ); m_pGraph[ to * m_nNodeCount + from ] = v; }
|
|
bool IsReachable( size_t from, size_t to ) const { return GetCost( from, to ) != std::numeric_limits< T >::max(); }
|
|
|
|
T* m_pGraph;
|
|
size_t m_nNodeCount;
|
|
};
|
|
|
|
static void dijkstra( const NakedPT & pl, const PATH & vLeftPath, const PATH & vRightPath, const PATH & vGreenPath, const Point<T> & start, const Point<T> & end, PATH & vResult )
|
|
{
|
|
std::vector< Point<T> > vPointList;
|
|
size_t node_count;
|
|
|
|
// { 노드 구성
|
|
vPointList.reserve( vLeftPath.size() + vRightPath.size() + vGreenPath.size() );
|
|
vPointList.push_back( start );
|
|
if( !vLeftPath.empty() ) { vPointList.insert( vPointList.end(), vLeftPath.begin()+1, vLeftPath.end()-1 ); }
|
|
if( !vRightPath.empty() ) { vPointList.insert( vPointList.end(), vRightPath.begin()+1, vRightPath.end()-1 ); }
|
|
if( !vGreenPath.empty() ) { vPointList.insert( vPointList.end(), vGreenPath.begin()+1, vGreenPath.end()-1 ); }
|
|
vPointList.push_back( end );
|
|
node_count = vPointList.size();
|
|
// }
|
|
|
|
// 시작점, Left List, Right List, Green, end
|
|
|
|
GRAPH graph( node_count+2 );
|
|
|
|
size_t idx;
|
|
size_t nAdd = 1;
|
|
if( !vLeftPath.empty() )
|
|
{
|
|
graph.SetCost( 0, 1, vLeftPath[0].GetDistance( vLeftPath[1] ) );
|
|
for( idx = 0; idx+3 < vLeftPath.size(); ++idx )
|
|
{
|
|
graph.SetCost( nAdd+idx, nAdd+idx+1, vLeftPath[idx+1].GetDistance( vLeftPath[idx+1+1] ) );
|
|
}
|
|
graph.SetCost( nAdd+idx, node_count-1, vLeftPath[vLeftPath.size()-2].GetDistance( vLeftPath[vLeftPath.size()-1] ) );
|
|
nAdd += ( vLeftPath.size() - 2 );
|
|
}
|
|
if( !vRightPath.empty() )
|
|
{
|
|
graph.SetCost( 0, nAdd, vRightPath[0].GetDistance( vRightPath[1] ) );
|
|
for( idx = 0; idx+3 < vRightPath.size(); ++idx )
|
|
{
|
|
graph.SetCost( nAdd+idx, nAdd+idx+1, vRightPath[idx+1].GetDistance( vRightPath[idx+1+1] ) );
|
|
}
|
|
graph.SetCost( nAdd+idx, node_count-1, vRightPath[vRightPath.size()-2].GetDistance( vRightPath[vRightPath.size()-1] ) );
|
|
nAdd += ( vRightPath.size() - 2 );
|
|
}
|
|
|
|
// 왼쪽에서 오른쪽으로 가는 경우
|
|
for( size_t li = 1; li+1 < vLeftPath.size(); ++li )
|
|
{
|
|
for( size_t ri = 1; ri+1 < vRightPath.size(); ++ri )
|
|
{
|
|
size_t from = li;
|
|
size_t tt = vRightPath.size() - ri - 1;
|
|
size_t to = vLeftPath.size() + tt - 2;
|
|
|
|
if( !LOOSE_COLLISION( pl, Line<T>( vPointList[from], vPointList[to] ) ) )
|
|
{
|
|
graph.SetCost( from, to, vPointList[from].GetDistance( vPointList[to] ) );
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
// 오른쪽에서 왼쪽으로 가는경우
|
|
for( size_t ri = 1; ri+1 < vRightPath.size(); ++ri )
|
|
{
|
|
for( size_t li = 1; li+1 < vLeftPath.size(); ++li )
|
|
{
|
|
size_t from = vLeftPath.size() + ri - 2;
|
|
size_t tt = vLeftPath.size() - li - 1;
|
|
size_t to = tt;
|
|
|
|
if( !LOOSE_COLLISION( pl, Line<T>( vPointList[from], vPointList[to] ) ) )
|
|
{
|
|
graph.SetCost( from, to, vPointList[from].GetDistance( vPointList[to] ) );
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
if( !vGreenPath.empty() )
|
|
{
|
|
graph.SetCost( 0, node_count-2, vGreenPath[0].GetDistance( vGreenPath[1] ) );
|
|
graph.SetCost( node_count-2, node_count-1, vGreenPath[1].GetDistance( vGreenPath[2] ) );
|
|
}
|
|
|
|
// 찾자~
|
|
std::vector< size_t > vNodeIndex;
|
|
T length = XDijkstra< T, GRAPH >().Find( graph, vPointList.size(), 0, vPointList.size() - 1, &vNodeIndex );
|
|
|
|
vResult.reserve( vNodeIndex.size() );
|
|
for( std::vector< size_t >::iterator it = vNodeIndex.begin(); it != vNodeIndex.end(); ++it ) vResult.push_back( vPointList[ *it ] );
|
|
}
|
|
|
|
|
|
|
|
struct PolygonDistanceLess
|
|
{
|
|
template< typename PI >
|
|
bool operator()( const PI & lh, const PI & rh )
|
|
{
|
|
T dist_lh = pt.GetAlternativeDistance( (*lh).GetCenter() );
|
|
T dist_rh = pt.GetAlternativeDistance( (*rh).GetCenter() );
|
|
return dist_lh < dist_rh;
|
|
}
|
|
|
|
X2D::Point<T> pt;
|
|
};
|
|
|
|
template< typename PI >
|
|
static void _Find( PI begin ,PI end, const X2D::Point<T> & start, const X2D::Point<T> & goal, PATH & vPath, const T & max_length )
|
|
{
|
|
std::vector< PT > vPolygonList( begin, end );
|
|
PolygonDistanceLess less;
|
|
less.pt = start;
|
|
std::sort( vPolygonList.begin(), vPolygonList.end(), less );
|
|
|
|
for( std::vector< PT >::iterator it = vPolygonList.begin(); it != vPolygonList.end(); ++it )
|
|
{
|
|
if( X2D::INCLUDE( *it, start ) ) return;
|
|
if( X2D::INCLUDE( *it, goal ) ) return;
|
|
}
|
|
|
|
vPath.erase( vPath.begin(), vPath.end() );
|
|
vPath.push_back( start );
|
|
vPath.push_back( goal );
|
|
|
|
PATH vNewPath;
|
|
|
|
if( false )
|
|
{
|
|
getPath( **begin, start, goal, &vPath );
|
|
}
|
|
else
|
|
{
|
|
for( size_t idx = 0; idx + 1 < vPath.size(); )
|
|
{
|
|
size_t next_idx = idx + 1;
|
|
int nAdd = 1;
|
|
|
|
for( PI it = vPolygonList.begin(); it != vPolygonList.end(); ++it )
|
|
{
|
|
if( COLLISION( *(*it), start ) ) assert( 0 );
|
|
if( COLLISION( *(*it), goal ) ) assert( 0 );
|
|
|
|
if( !LOOSE_COLLISION( *(*it), Line<T>( *(vPath.begin() + idx), *(vPath.begin() + next_idx) ) ) ) continue;
|
|
|
|
vNewPath.reserve( (*it)->Size() );
|
|
vNewPath.erase( vNewPath.begin(), vNewPath.end() );
|
|
|
|
getPath( *(*it), *(vPath.begin() + idx), *(vPath.begin() + next_idx), &vNewPath );
|
|
|
|
// debug
|
|
/*
|
|
for( size_t pi = 0; pi < vPath.size() && !vNewPath.empty(); ++pi )
|
|
{
|
|
for( size_t pi2 = 1; pi2 + 1 < vNewPath.size(); ++pi2 )
|
|
{
|
|
if( vPath[pi] == vNewPath[pi2] )
|
|
{
|
|
extern bool g_bMouseMove;
|
|
g_bMouseMove=false;
|
|
vNewPath.clear();
|
|
break;
|
|
}
|
|
}
|
|
}*/
|
|
|
|
if( vNewPath.size() > 2 )
|
|
{
|
|
if( std::find( vPath.begin(), vPath.end(), vNewPath[1] ) != vPath.end() )
|
|
{
|
|
vPath.clear();
|
|
return;
|
|
}
|
|
vPath.insert( vPath.begin()+idx+1, vNewPath.begin() +1 , vNewPath.end() - 1 );
|
|
nAdd = 0;
|
|
}
|
|
else if( vNewPath.size() == 0 )
|
|
{
|
|
//vPath.erase( vPath.begin(), vPath.end() );
|
|
vPath.clear();
|
|
return;
|
|
}
|
|
|
|
if( max_length )
|
|
{
|
|
T total_length = 0;
|
|
for( size_t i = 0; i+1 < vPath.size(); ++i )
|
|
{
|
|
total_length += vPath[i].GetDistance( vPath[i+1] );
|
|
if( total_length > max_length || vPath[i] == vPath[i+1] )
|
|
{
|
|
vPath.clear();
|
|
return;
|
|
}
|
|
}
|
|
}
|
|
|
|
break;
|
|
}
|
|
idx += nAdd;
|
|
}
|
|
}
|
|
|
|
NORMALIZE( begin, end, vPath );
|
|
|
|
//return vPath;
|
|
}
|
|
};
|
|
|
|
|
|
}; // namespace X2D
|