489 lines
12 KiB
C++
489 lines
12 KiB
C++
//========= Copyright Valve Corporation, All rights reserved. ============//
|
|
//
|
|
// Purpose:
|
|
//
|
|
// $NoKeywords: $
|
|
//
|
|
//=============================================================================//
|
|
// nav_node.cpp
|
|
// AI Navigation Nodes
|
|
// Author: Michael S. Booth (mike@turtlerockstudios.com), January 2003
|
|
|
|
#include "cbase.h"
|
|
#include "nav_node.h"
|
|
#include "nav_colors.h"
|
|
#include "nav_mesh.h"
|
|
#include "tier1/utlhash.h"
|
|
#include "tier1/generichash.h"
|
|
|
|
// NOTE: This has to be the last file included!
|
|
#include "tier0/memdbgon.h"
|
|
|
|
|
|
NavDirType Opposite[ NUM_DIRECTIONS ] = { SOUTH, WEST, NORTH, EAST };
|
|
|
|
CNavNode *CNavNode::m_list = NULL;
|
|
unsigned int CNavNode::m_listLength = 0;
|
|
unsigned int CNavNode::m_nextID = 1;
|
|
|
|
extern Vector NavTraceMins;
|
|
extern Vector NavTraceMaxs;
|
|
|
|
//--------------------------------------------------------------------------------------------------------------
|
|
// Node hash
|
|
|
|
class CNodeHashFuncs
|
|
{
|
|
public:
|
|
CNodeHashFuncs( int ) {}
|
|
|
|
bool operator()( const CNavNode *pLhs, const CNavNode *pRhs ) const
|
|
{
|
|
return pRhs->GetPosition()->AsVector2D() == pLhs->GetPosition()->AsVector2D();
|
|
}
|
|
|
|
unsigned int operator()( const CNavNode *pItem ) const
|
|
{
|
|
return Hash8( &pItem->GetPosition()->AsVector2D() );
|
|
}
|
|
};
|
|
|
|
CUtlHash<CNavNode *, CNodeHashFuncs, CNodeHashFuncs> *g_pNavNodeHash;
|
|
|
|
|
|
//--------------------------------------------------------------------------------------------------------------
|
|
/**
|
|
* Constructor
|
|
*/
|
|
CNavNode::CNavNode( const Vector &pos, const Vector &normal, CNavNode *parent, bool isOnDisplacement )
|
|
{
|
|
m_pos = pos;
|
|
m_normal = normal;
|
|
|
|
m_id = m_nextID++;
|
|
|
|
int i;
|
|
for( i=0; i<NUM_DIRECTIONS; ++i )
|
|
{
|
|
m_to[ i ] = NULL;
|
|
m_obstacleHeight[ i ] = 0;
|
|
m_obstacleStartDist[ i ] = 0;
|
|
m_obstacleEndDist[ i ] = 0;
|
|
}
|
|
|
|
for ( i=0; i<NUM_CORNERS; ++i )
|
|
{
|
|
m_crouch[ i ] = false;
|
|
m_isBlocked[ i ] = false;
|
|
}
|
|
|
|
m_visited = 0;
|
|
m_parent = parent;
|
|
|
|
m_next = m_list;
|
|
m_list = this;
|
|
m_listLength++;
|
|
|
|
m_isCovered = false;
|
|
m_area = NULL;
|
|
|
|
m_attributeFlags = 0;
|
|
|
|
m_isOnDisplacement = isOnDisplacement;
|
|
|
|
if ( !g_pNavNodeHash )
|
|
{
|
|
g_pNavNodeHash = new CUtlHash<CNavNode *, CNodeHashFuncs, CNodeHashFuncs>( 16*1024 );
|
|
}
|
|
|
|
bool bDidInsert;
|
|
UtlHashHandle_t hHash = g_pNavNodeHash->Insert( this, &bDidInsert );
|
|
if ( !bDidInsert )
|
|
{
|
|
CNavNode *pExistingNode = g_pNavNodeHash->Element( hHash );
|
|
m_nextAtXY = pExistingNode;
|
|
g_pNavNodeHash->Element( hHash ) = this;
|
|
}
|
|
else
|
|
{
|
|
m_nextAtXY = NULL;
|
|
}
|
|
}
|
|
|
|
CNavNode::~CNavNode()
|
|
{
|
|
}
|
|
|
|
|
|
//--------------------------------------------------------------------------------------------------------------
|
|
void CNavNode::CleanupGeneration()
|
|
{
|
|
delete g_pNavNodeHash;
|
|
g_pNavNodeHash = NULL;
|
|
|
|
CNavNode *node, *next;
|
|
for( node = CNavNode::m_list; node; node = next )
|
|
{
|
|
next = node->m_next;
|
|
delete node;
|
|
}
|
|
CNavNode::m_list = NULL;
|
|
CNavNode::m_listLength = 0;
|
|
CNavNode::m_nextID = 1;
|
|
}
|
|
|
|
//--------------------------------------------------------------------------------------------------------------
|
|
#if DEBUG_NAV_NODES
|
|
ConVar nav_show_nodes( "nav_show_nodes", "0", FCVAR_CHEAT );
|
|
ConVar nav_show_node_id( "nav_show_node_id", "0", FCVAR_CHEAT );
|
|
ConVar nav_test_node( "nav_test_node", "0", FCVAR_CHEAT );
|
|
ConVar nav_test_node_crouch( "nav_test_node_crouch", "0", FCVAR_CHEAT );
|
|
ConVar nav_test_node_crouch_dir( "nav_test_node_crouch_dir", "4", FCVAR_CHEAT );
|
|
ConVar nav_show_node_grid( "nav_show_node_grid", "0", FCVAR_CHEAT );
|
|
#endif // DEBUG_NAV_NODES
|
|
|
|
|
|
//--------------------------------------------------------------------------------------------------------------
|
|
void CNavNode::Draw( void )
|
|
{
|
|
#if DEBUG_NAV_NODES
|
|
|
|
if ( !nav_show_nodes.GetBool() )
|
|
return;
|
|
|
|
int r = 0, g = 0, b = 0;
|
|
|
|
if ( m_isCovered )
|
|
{
|
|
if ( GetAttributes() & NAV_MESH_CROUCH )
|
|
{
|
|
b = 255;
|
|
}
|
|
else
|
|
{
|
|
r = 255;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if ( GetAttributes() & NAV_MESH_CROUCH )
|
|
{
|
|
b = 255;
|
|
}
|
|
g = 255;
|
|
}
|
|
|
|
NDebugOverlay::Cross3D( m_pos, 2, r, g, b, true, 0.1f );
|
|
|
|
if ( (!m_isCovered && nav_show_node_id.GetBool()) || (m_isCovered && nav_show_node_id.GetInt() < 0) )
|
|
{
|
|
char text[16];
|
|
Q_snprintf( text, sizeof( text ), "%d", m_id );
|
|
NDebugOverlay::Text( m_pos, text, true, 0.1f );
|
|
}
|
|
|
|
if ( (unsigned int)(nav_test_node.GetInt()) == m_id )
|
|
{
|
|
TheNavMesh->TestArea( this, 1, 1 );
|
|
nav_test_node.SetValue( 0 );
|
|
}
|
|
|
|
if ( (unsigned int)(nav_test_node_crouch.GetInt()) == m_id )
|
|
{
|
|
CheckCrouch();
|
|
nav_test_node_crouch.SetValue( 0 );
|
|
}
|
|
|
|
if ( GetAttributes() & NAV_MESH_CROUCH )
|
|
{
|
|
int i;
|
|
for( i=0; i<NUM_CORNERS; i++ )
|
|
{
|
|
if ( m_isBlocked[i] || m_crouch[i] )
|
|
{
|
|
Vector2D dir;
|
|
CornerToVector2D( (NavCornerType)i, &dir );
|
|
|
|
const float scale = 3.0f;
|
|
Vector scaled( dir.x * scale, dir.y * scale, 0 );
|
|
|
|
if ( m_isBlocked[i] )
|
|
{
|
|
NDebugOverlay::HorzArrow( m_pos, m_pos + scaled, 0.5, 255, 0, 0, 255, true, 0.1f );
|
|
}
|
|
else
|
|
{
|
|
NDebugOverlay::HorzArrow( m_pos, m_pos + scaled, 0.5, 0, 0, 255, 255, true, 0.1f );
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if ( nav_show_node_grid.GetBool() )
|
|
{
|
|
for ( int i = NORTH; i < NUM_DIRECTIONS; i++ )
|
|
{
|
|
CNavNode *nodeNext = GetConnectedNode( (NavDirType) i );
|
|
if ( nodeNext )
|
|
{
|
|
NDebugOverlay::Line( *GetPosition(), *nodeNext->GetPosition(), 255, 255, 0, false, 0.1f );
|
|
|
|
float obstacleHeight = m_obstacleHeight[i];
|
|
if ( obstacleHeight > 0 )
|
|
{
|
|
float z = GetPosition()->z + obstacleHeight;
|
|
Vector from = *GetPosition();
|
|
Vector to = from;
|
|
AddDirectionVector( &to, (NavDirType) i, m_obstacleStartDist[i] );
|
|
NDebugOverlay::Line( from, to, 255, 0, 255, false, 0.1f );
|
|
from = to;
|
|
to.z = z;
|
|
NDebugOverlay::Line( from, to, 255, 0, 255, false, 0.1f );
|
|
from = to;
|
|
to = *GetPosition();
|
|
to.z = z;
|
|
AddDirectionVector( &to, (NavDirType) i, m_obstacleEndDist[i] );
|
|
NDebugOverlay::Line( from, to, 255, 0, 255, false, 0.1f );
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
#endif // DEBUG_NAV_NODES
|
|
}
|
|
|
|
|
|
//--------------------------------------------------------------------------------------------------------
|
|
// return ground height above node in given corner direction (NUM_CORNERS for highest in any direction)
|
|
float CNavNode::GetGroundHeightAboveNode( NavCornerType cornerType ) const
|
|
{
|
|
if ( cornerType >= 0 && cornerType < NUM_CORNERS )
|
|
return m_groundHeightAboveNode[ cornerType ];
|
|
|
|
float blockedHeight = 0.0f;
|
|
for ( int i=0; i<NUM_CORNERS; ++i )
|
|
{
|
|
blockedHeight = MAX( blockedHeight, m_groundHeightAboveNode[i] );
|
|
}
|
|
|
|
return blockedHeight;
|
|
}
|
|
|
|
|
|
//--------------------------------------------------------------------------------------------------------------
|
|
/**
|
|
* Look up to JumpCrouchHeight in the air to see if we can fit a whole HumanHeight box
|
|
*/
|
|
bool CNavNode::TestForCrouchArea( NavCornerType cornerNum, const Vector& mins, const Vector& maxs, float *groundHeightAboveNode )
|
|
{
|
|
CTraceFilterWalkableEntities filter( NULL, COLLISION_GROUP_PLAYER_MOVEMENT, WALK_THRU_EVERYTHING );
|
|
trace_t tr;
|
|
|
|
Vector start( m_pos );
|
|
Vector end( start );
|
|
end.z += JumpCrouchHeight;
|
|
UTIL_TraceHull( start, end, NavTraceMins, NavTraceMaxs, MASK_NPCSOLID_BRUSHONLY, &filter, &tr );
|
|
|
|
float maxHeight = tr.endpos.z - start.z;
|
|
|
|
Vector realMaxs( maxs );
|
|
|
|
for ( float height = 0; height <= maxHeight; height += 1.0f )
|
|
{
|
|
start = m_pos;
|
|
start.z += height;
|
|
|
|
realMaxs.z = HumanCrouchHeight;
|
|
UTIL_TraceHull( start, start, mins, realMaxs, MASK_NPCSOLID_BRUSHONLY, &filter, &tr );
|
|
if ( !tr.startsolid )
|
|
{
|
|
*groundHeightAboveNode = start.z - m_pos.z;
|
|
|
|
// We found a crouch-sized space. See if we can stand up.
|
|
realMaxs.z = HumanHeight;
|
|
UTIL_TraceHull( start, start, mins, realMaxs, MASK_NPCSOLID_BRUSHONLY, &filter, &tr );
|
|
if ( !tr.startsolid )
|
|
{
|
|
// We found a crouch-sized space. See if we can stand up.
|
|
#if DEBUG_NAV_NODES
|
|
if ( (unsigned int)(nav_test_node_crouch.GetInt()) == GetID() )
|
|
{
|
|
NDebugOverlay::Box( start, mins, maxs, 0, 255, 255, 100, 100 );
|
|
}
|
|
#endif // DEBUG_NAV_NODES
|
|
return true;
|
|
}
|
|
#if DEBUG_NAV_NODES
|
|
if ( (unsigned int)(nav_test_node_crouch.GetInt()) == GetID() )
|
|
{
|
|
NDebugOverlay::Box( start, mins, maxs, 255, 0, 0, 100, 100 );
|
|
}
|
|
#endif // DEBUG_NAV_NODES
|
|
|
|
return false;
|
|
}
|
|
}
|
|
|
|
*groundHeightAboveNode = JumpCrouchHeight;
|
|
m_isBlocked[ cornerNum ] = true;
|
|
return false;
|
|
}
|
|
|
|
|
|
//--------------------------------------------------------------------------------------------------------------
|
|
void CNavNode::CheckCrouch( void )
|
|
{
|
|
// For each direction, trace upwards from our best ground height to VEC_HULL_MAX.z to see if we have standing room.
|
|
for ( int i=0; i<NUM_CORNERS; ++i )
|
|
{
|
|
#if DEBUG_NAV_NODES
|
|
if ( nav_test_node_crouch_dir.GetInt() != NUM_CORNERS && i != nav_test_node_crouch_dir.GetInt() )
|
|
continue;
|
|
#endif // DEBUG_NAV_NODES
|
|
|
|
NavCornerType corner = (NavCornerType)i;
|
|
Vector2D cornerVec;
|
|
CornerToVector2D( corner, &cornerVec );
|
|
|
|
// Build a mins/maxs pair for the HumanWidth x HalfHumanWidth box facing the appropriate direction
|
|
Vector mins( 0, 0, 0 );
|
|
Vector maxs( 0, 0, 0 );
|
|
if ( cornerVec.x < 0 )
|
|
{
|
|
mins.x = -HalfHumanWidth;
|
|
}
|
|
else if ( cornerVec.x > 0 )
|
|
{
|
|
maxs.x = HalfHumanWidth;
|
|
}
|
|
if ( cornerVec.y < 0 )
|
|
{
|
|
mins.y = -HalfHumanWidth;
|
|
}
|
|
else if ( cornerVec.y > 0 )
|
|
{
|
|
maxs.y = HalfHumanWidth;
|
|
}
|
|
maxs.z = HumanHeight;
|
|
|
|
// now make sure that mins is smaller than maxs
|
|
for ( int j=0; j<3; ++j )
|
|
{
|
|
if ( mins[j] > maxs[j] )
|
|
{
|
|
float tmp = mins[j];
|
|
mins[j] = maxs[j];
|
|
maxs[j] = tmp;
|
|
}
|
|
}
|
|
|
|
if ( !TestForCrouchArea( corner, mins, maxs, &m_groundHeightAboveNode[i] ) )
|
|
{
|
|
SetAttributes( NAV_MESH_CROUCH );
|
|
m_crouch[corner] = true;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
//--------------------------------------------------------------------------------------------------------------
|
|
/**
|
|
* Create a connection FROM this node TO the given node, in the given direction
|
|
*/
|
|
void CNavNode::ConnectTo( CNavNode *node, NavDirType dir, float obstacleHeight, float obstacleStartDist, float obstacleEndDist )
|
|
{
|
|
Assert( obstacleStartDist >= 0 && obstacleStartDist <= GenerationStepSize );
|
|
Assert( obstacleEndDist >= 0 && obstacleStartDist <= GenerationStepSize );
|
|
Assert( obstacleStartDist < obstacleEndDist );
|
|
|
|
m_to[ dir ] = node;
|
|
m_obstacleHeight[ dir ] = obstacleHeight;
|
|
m_obstacleStartDist[ dir ] = obstacleStartDist;
|
|
m_obstacleEndDist[ dir ] = obstacleEndDist;
|
|
}
|
|
|
|
//--------------------------------------------------------------------------------------------------------------
|
|
/**
|
|
* Return node at given position.
|
|
* @todo Need a hash table to make this lookup fast
|
|
*/
|
|
CNavNode *CNavNode::GetNode( const Vector &pos )
|
|
{
|
|
const float tolerance = 0.45f * GenerationStepSize; // 1.0f
|
|
CNavNode *pNode = NULL;
|
|
if ( g_pNavNodeHash )
|
|
{
|
|
static CNavNode lookup;
|
|
lookup.m_pos = pos;
|
|
UtlHashHandle_t hNode = g_pNavNodeHash->Find( &lookup );
|
|
|
|
if ( hNode != g_pNavNodeHash->InvalidHandle() )
|
|
{
|
|
for( pNode = g_pNavNodeHash->Element( hNode ); pNode; pNode = pNode->m_nextAtXY )
|
|
{
|
|
float dz = fabs( pNode->m_pos.z - pos.z );
|
|
|
|
if (dz < tolerance)
|
|
{
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
#ifdef DEBUG_NODE_HASH
|
|
CNavNode *pTestNode = NULL;
|
|
for( CNavNode *node = m_list; node; node = node->m_next )
|
|
{
|
|
float dx = fabs( node->m_pos.x - pos.x );
|
|
float dy = fabs( node->m_pos.y - pos.y );
|
|
float dz = fabs( node->m_pos.z - pos.z );
|
|
|
|
if (dx < tolerance && dy < tolerance && dz < tolerance)
|
|
{
|
|
pTestNode = node;
|
|
break;
|
|
}
|
|
}
|
|
AssertFatal( pTestNode == pNode );
|
|
#endif
|
|
|
|
return pNode;
|
|
}
|
|
|
|
//--------------------------------------------------------------------------------------------------------------
|
|
/**
|
|
* Return true if this node is bidirectionally linked to
|
|
* another node in the given direction
|
|
*/
|
|
BOOL CNavNode::IsBiLinked( NavDirType dir ) const
|
|
{
|
|
if (m_to[ dir ] && m_to[ dir ]->m_to[ Opposite[dir] ] == this)
|
|
{
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
//--------------------------------------------------------------------------------------------------------------
|
|
/**
|
|
* Return true if this node is the NW corner of a quad of nodes
|
|
* that are all bidirectionally linked.
|
|
*/
|
|
BOOL CNavNode::IsClosedCell( void ) const
|
|
{
|
|
if (IsBiLinked( SOUTH ) &&
|
|
IsBiLinked( EAST ) &&
|
|
m_to[ EAST ]->IsBiLinked( SOUTH ) &&
|
|
m_to[ SOUTH ]->IsBiLinked( EAST ) &&
|
|
m_to[ EAST ]->m_to[ SOUTH ] == m_to[ SOUTH ]->m_to[ EAST ])
|
|
{
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|