2004-06-03 20:38:14 +02:00
|
|
|
#include "precompiled.h"
|
|
|
|
|
2004-05-29 05:32:33 +02:00
|
|
|
#include "PathfindSparse.h"
|
|
|
|
|
|
|
|
sparsePathTree::sparsePathTree( const CVector2D& _from, const CVector2D& _to, HEntity _entity, CBoundingObject* _destinationCollisionObject )
|
|
|
|
{
|
|
|
|
from = _from; to = _to;
|
|
|
|
|
|
|
|
entity = _entity; destinationCollisionObject = _destinationCollisionObject;
|
|
|
|
leftPre = NULL; leftPost = NULL;
|
|
|
|
rightPre = NULL; rightPost = NULL;
|
|
|
|
type = SPF_OPEN_UNVISITED;
|
|
|
|
leftImpossible = false; rightImpossible = false;
|
2004-06-03 04:20:48 +02:00
|
|
|
nextSubtree = 0;
|
2004-05-29 05:32:33 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
sparsePathTree::~sparsePathTree()
|
|
|
|
{
|
|
|
|
if( leftPre ) delete( leftPre );
|
|
|
|
if( leftPost ) delete( leftPost );
|
|
|
|
if( rightPre ) delete( rightPre );
|
|
|
|
if( rightPost ) delete( rightPost );
|
|
|
|
}
|
|
|
|
|
|
|
|
bool sparsePathTree::slice()
|
|
|
|
{
|
|
|
|
if( type == SPF_OPEN_UNVISITED )
|
|
|
|
{
|
|
|
|
rayIntersectionResults r;
|
|
|
|
|
|
|
|
CVector2D forward = to - from;
|
|
|
|
float len = forward.length();
|
2004-06-03 04:20:48 +02:00
|
|
|
|
|
|
|
assert( len != 0.0f );
|
|
|
|
|
2004-05-29 05:32:33 +02:00
|
|
|
forward /= len;
|
2004-06-03 04:20:48 +02:00
|
|
|
CVector2D v_right = CVector2D( forward.y, -forward.x );
|
2004-05-29 05:32:33 +02:00
|
|
|
|
2004-06-03 04:20:48 +02:00
|
|
|
if( !getRayIntersection( from, forward, v_right, len, entity->m_bounds->m_radius * 1.1f, destinationCollisionObject, &r ) )
|
2004-05-29 05:32:33 +02:00
|
|
|
{
|
|
|
|
type = SPF_CLOSED_DIRECT;
|
|
|
|
return( true );
|
|
|
|
}
|
|
|
|
|
2004-06-03 04:20:48 +02:00
|
|
|
float turningRadius = ( entity->m_bounds->m_radius + r.boundingObject->m_radius ) * 1.1f;
|
2004-05-29 05:32:33 +02:00
|
|
|
|
2004-06-11 00:24:03 +02:00
|
|
|
if( entity->m_turningRadius > turningRadius ) turningRadius = entity->m_turningRadius;
|
2004-05-29 05:32:33 +02:00
|
|
|
|
|
|
|
// Too close, an impossible turn
|
2004-06-03 04:20:48 +02:00
|
|
|
if( r.distance < turningRadius )
|
2004-05-29 05:32:33 +02:00
|
|
|
{
|
2004-06-03 04:20:48 +02:00
|
|
|
// Too close to make a proper turn; try dodging immediately a long way to the left or right.
|
|
|
|
left = from - v_right * r.boundingObject->m_radius * 2.5f;
|
|
|
|
right = from + v_right * r.boundingObject->m_radius * 2.5f;
|
2004-05-29 05:32:33 +02:00
|
|
|
}
|
2004-06-03 04:20:48 +02:00
|
|
|
else if( r.distance > ( len - turningRadius ) )
|
|
|
|
{
|
|
|
|
// Again, too close to avoid it properly. Try approaching the goal from the left or right.
|
|
|
|
left = to - v_right * r.boundingObject->m_radius * 2.5f;
|
|
|
|
right = to + v_right * r.boundingObject->m_radius * 2.5f;
|
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
|
|
|
// Dodge to the left or right of the obstacle.
|
|
|
|
// A distance of offsetDistance is sufficient to guarantee we'll make the turn.
|
2004-05-29 05:32:33 +02:00
|
|
|
|
2004-06-03 04:20:48 +02:00
|
|
|
CVector2D delta = r.position - from;
|
|
|
|
float length = delta.length();
|
2004-05-29 05:32:33 +02:00
|
|
|
|
2004-06-03 04:20:48 +02:00
|
|
|
float offsetDistance = ( turningRadius * length / sqrt( length * length - turningRadius * turningRadius ) );
|
|
|
|
left = r.position - v_right * offsetDistance;
|
|
|
|
right = r.position + v_right * offsetDistance;
|
|
|
|
}
|
2004-05-29 05:32:33 +02:00
|
|
|
favourLeft = false;
|
|
|
|
if( r.closestApproach < 0 )
|
|
|
|
favourLeft = true;
|
2004-06-03 04:20:48 +02:00
|
|
|
|
2004-05-29 05:32:33 +02:00
|
|
|
// First we path to the left...
|
2004-06-03 04:20:48 +02:00
|
|
|
|
2004-05-29 05:32:33 +02:00
|
|
|
leftPre = new sparsePathTree( from, left, entity, destinationCollisionObject );
|
|
|
|
leftPost = new sparsePathTree( left, to, entity, destinationCollisionObject );
|
|
|
|
|
|
|
|
// Then we path to the right...
|
|
|
|
|
|
|
|
rightPre = new sparsePathTree( from, right, entity, destinationCollisionObject );
|
|
|
|
rightPost = new sparsePathTree( right, to, entity, destinationCollisionObject );
|
|
|
|
|
|
|
|
// If anybody reaches this point and is thinking:
|
|
|
|
//
|
|
|
|
// "Let's Do The Time-Warp Agaaaain!"
|
|
|
|
//
|
|
|
|
// Let me know.
|
|
|
|
|
|
|
|
type = SPF_OPEN_PROCESSING;
|
|
|
|
|
|
|
|
return( true );
|
|
|
|
}
|
|
|
|
else /* type == SPF_OPEN_PROCESSING */
|
|
|
|
{
|
|
|
|
bool done = false;
|
2004-06-03 04:20:48 +02:00
|
|
|
while( !done )
|
2004-05-29 05:32:33 +02:00
|
|
|
{
|
2004-06-03 04:20:48 +02:00
|
|
|
if( subtrees[nextSubtree]->type & SPF_OPEN )
|
|
|
|
if( subtrees[nextSubtree]->slice() )
|
|
|
|
done = true;
|
|
|
|
nextSubtree++;
|
|
|
|
nextSubtree %= 4;
|
2004-05-29 05:32:33 +02:00
|
|
|
}
|
2004-06-03 04:20:48 +02:00
|
|
|
if( ( leftPre->type == SPF_IMPOSSIBLE ) || ( leftPost->type == SPF_IMPOSSIBLE ) )
|
|
|
|
leftImpossible = true;
|
|
|
|
if( ( rightPre->type == SPF_IMPOSSIBLE ) || ( rightPost->type == SPF_IMPOSSIBLE ) )
|
|
|
|
rightImpossible = true;
|
2004-05-29 05:32:33 +02:00
|
|
|
if( leftImpossible && rightImpossible )
|
|
|
|
{
|
|
|
|
type = SPF_IMPOSSIBLE;
|
|
|
|
return( done );
|
|
|
|
}
|
|
|
|
if( ( ( leftPre->type & SPF_SOLVED ) && ( leftPost->type & SPF_SOLVED ) ) ||
|
|
|
|
( ( rightPre->type & SPF_SOLVED ) && ( rightPost->type & SPF_SOLVED ) ) )
|
|
|
|
{
|
|
|
|
type = SPF_CLOSED_WAYPOINTED;
|
|
|
|
return( done );
|
|
|
|
}
|
|
|
|
return( done );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
void sparsePathTree::pushResults( std::vector<CVector2D>& nodelist )
|
|
|
|
{
|
|
|
|
assert( type & SPF_SOLVED );
|
|
|
|
|
|
|
|
if( type == SPF_CLOSED_DIRECT )
|
|
|
|
{
|
|
|
|
nodelist.push_back( to );
|
|
|
|
}
|
|
|
|
else /* type == SPF_CLOSED_WAYPOINTED */
|
|
|
|
{
|
|
|
|
leftImpossible = !( ( leftPre->type & SPF_SOLVED ) && ( leftPost->type & SPF_SOLVED ) );
|
|
|
|
rightImpossible = !( ( rightPre->type & SPF_SOLVED ) && ( rightPost->type & SPF_SOLVED ) );
|
|
|
|
|
|
|
|
if( !leftImpossible && ( favourLeft || rightImpossible ) )
|
|
|
|
{
|
|
|
|
leftPost->pushResults( nodelist );
|
|
|
|
leftPre->pushResults( nodelist );
|
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
|
|
|
assert( !rightImpossible );
|
|
|
|
rightPost->pushResults( nodelist );
|
|
|
|
rightPre->pushResults( nodelist );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
void nodeSmooth( HEntity entity, std::vector<CVector2D>& nodelist )
|
|
|
|
{
|
|
|
|
std::vector<CVector2D>::iterator it;
|
|
|
|
CVector2D next = nodelist.front();
|
|
|
|
|
|
|
|
CEntityOrder node;
|
2004-06-03 04:20:48 +02:00
|
|
|
node.m_type = CEntityOrder::ORDER_GOTO_SMOOTHED;
|
2004-05-29 05:32:33 +02:00
|
|
|
node.m_data[0].location = next;
|
|
|
|
|
|
|
|
entity->m_orderQueue.push_front( node );
|
|
|
|
|
|
|
|
for( it = nodelist.begin() + 1; it != nodelist.end(); it++ )
|
|
|
|
{
|
|
|
|
if( ( it + 1 ) == nodelist.end() ) break;
|
|
|
|
CVector2D current = *it;
|
|
|
|
CVector2D previous = *( it + 1 );
|
|
|
|
CVector2D u = current - previous;
|
|
|
|
CVector2D v = next - current;
|
|
|
|
u = u.normalize();
|
|
|
|
v = v.normalize();
|
|
|
|
CVector2D ubar = u.beta();
|
|
|
|
CVector2D vbar = v.beta();
|
|
|
|
float alpha = entity->m_turningRadius * ( ubar - vbar ).length() / ( u + v ).length();
|
2004-06-03 04:20:48 +02:00
|
|
|
node.m_data[0].location = current - u * alpha;
|
|
|
|
entity->m_orderQueue.push_front( node );
|
2004-05-29 05:32:33 +02:00
|
|
|
next = current;
|
|
|
|
}
|
2004-06-03 04:20:48 +02:00
|
|
|
|
|
|
|
// If we try to apply turning constraints to getting onto this path, there's a reasonable
|
|
|
|
// risk the entity will deviate so far from the first path segment that the path becomes
|
|
|
|
// unwalkable for it.
|
|
|
|
entity->m_orderQueue.front().m_type = CEntityOrder::ORDER_GOTO_NOPATHING;
|
2004-05-29 05:32:33 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
void pathSparse( HEntity entity, CVector2D destination )
|
|
|
|
{
|
|
|
|
std::vector<CVector2D> pathnodes;
|
2004-06-03 04:20:48 +02:00
|
|
|
CVector2D source( entity->m_position.X, entity->m_position.Z );
|
|
|
|
sparsePathTree sparseEngine( source, destination, entity, getContainingObject( destination ) );
|
2004-05-29 05:32:33 +02:00
|
|
|
while( sparseEngine.type & sparsePathTree::SPF_OPEN ) sparseEngine.slice();
|
|
|
|
|
2004-06-03 04:20:48 +02:00
|
|
|
assert( sparseEngine.type & sparsePathTree::SPF_SOLVED ); // Shouldn't be any impossible cases yet.
|
|
|
|
|
2004-05-29 05:32:33 +02:00
|
|
|
if( sparseEngine.type & sparsePathTree::SPF_SOLVED )
|
|
|
|
{
|
|
|
|
sparseEngine.pushResults( pathnodes );
|
2004-06-03 04:20:48 +02:00
|
|
|
pathnodes.push_back( source );
|
2004-05-29 05:32:33 +02:00
|
|
|
nodeSmooth( entity, pathnodes );
|
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
|
|
|
// Try a straight line. All we can do, really.
|
|
|
|
CEntityOrder direct;
|
|
|
|
direct.m_type = CEntityOrder::ORDER_GOTO_NOPATHING;
|
|
|
|
direct.m_data[0].location = destination;
|
|
|
|
entity->m_orderQueue.push_front( direct );
|
|
|
|
}
|
2004-06-02 18:11:32 +02:00
|
|
|
}
|