mirror of
https://github.com/SapphireServer/Sapphire.git
synced 2025-04-25 14:07:46 +00:00
Add path follow code from RecastDemo
This commit is contained in:
goaaats
parent
b200204ee3
commit
ba40d49a68
2 changed files with 324 additions and 3 deletions
|
|
@ -10,13 +10,19 @@
|
|||
|
||||
#include "../Territory/Zone.h"
|
||||
#include <Logging/Logger.h>
|
||||
#include <DetourCommon.h>
|
||||
#include <recastnavigation/Recast/Include/Recast.h>
|
||||
|
||||
Sapphire::NaviProvider::NaviProvider( Sapphire::ZonePtr pZone, Sapphire::FrameworkPtr pFw ) :
|
||||
m_pFw( pFw ),
|
||||
m_pZone( pZone ),
|
||||
m_naviMesh( nullptr ),
|
||||
m_naviMeshQuery( nullptr )
|
||||
{
|
||||
{
|
||||
// Set defaults
|
||||
m_polyFindRange[0] = 2;
|
||||
m_polyFindRange[1] = 4;
|
||||
m_polyFindRange[2] = 2;
|
||||
}
|
||||
|
||||
void Sapphire::NaviProvider::init()
|
||||
|
|
@ -51,11 +57,319 @@ bool Sapphire::NaviProvider::HasNaviMesh() const
|
|||
|
||||
void Sapphire::NaviProvider::InitQuery()
|
||||
{
|
||||
if(m_naviMeshQuery != nullptr)
|
||||
if( m_naviMeshQuery != nullptr )
|
||||
dtFreeNavMeshQuery( m_naviMeshQuery );
|
||||
|
||||
m_naviMeshQuery = dtAllocNavMeshQuery();
|
||||
m_naviMeshQuery->init( m_naviMesh, 2048 );
|
||||
}
|
||||
|
||||
static int fixupCorridor( dtPolyRef* path, const int npath, const int maxPath,
|
||||
const dtPolyRef* visited, const int nvisited )
|
||||
{
|
||||
int furthestPath = -1;
|
||||
int furthestVisited = -1;
|
||||
|
||||
// Find furthest common polygon.
|
||||
for( int i = npath - 1; i >= 0; --i )
|
||||
{
|
||||
bool found = false;
|
||||
for( int j = nvisited - 1; j >= 0; --j )
|
||||
{
|
||||
if( path[i] == visited[j] )
|
||||
{
|
||||
furthestPath = i;
|
||||
furthestVisited = j;
|
||||
found = true;
|
||||
}
|
||||
}
|
||||
if( found )
|
||||
break;
|
||||
}
|
||||
|
||||
// If no intersection found just return current path.
|
||||
if( furthestPath == -1 || furthestVisited == -1 )
|
||||
return npath;
|
||||
|
||||
// Concatenate paths.
|
||||
|
||||
// Adjust beginning of the buffer to include the visited.
|
||||
const int req = nvisited - furthestVisited;
|
||||
const int orig = rcMin( furthestPath + 1, npath );
|
||||
int size = rcMax( 0, npath - orig );
|
||||
if( req + size > maxPath )
|
||||
size = maxPath - req;
|
||||
if( size )
|
||||
memmove( path + req, path + orig, size * sizeof( dtPolyRef ) );
|
||||
|
||||
// Store visited
|
||||
for( int i = 0; i < req; ++i )
|
||||
path[i] = visited[( nvisited - 1 ) - i];
|
||||
|
||||
return req + size;
|
||||
}
|
||||
|
||||
static int fixupShortcuts( dtPolyRef* path, int npath, dtNavMeshQuery* navQuery )
|
||||
{
|
||||
if( npath < 3 )
|
||||
return npath;
|
||||
|
||||
// Get connected polygons
|
||||
static const int maxNeis = 16;
|
||||
dtPolyRef neis[maxNeis];
|
||||
int nneis = 0;
|
||||
|
||||
const dtMeshTile* tile = 0;
|
||||
const dtPoly* poly = 0;
|
||||
if( dtStatusFailed( navQuery->getAttachedNavMesh()->getTileAndPolyByRef( path[0], &tile, &poly ) ) )
|
||||
return npath;
|
||||
|
||||
for( unsigned int k = poly->firstLink; k != DT_NULL_LINK; k = tile->links[k].next )
|
||||
{
|
||||
const dtLink* link = &tile->links[k];
|
||||
if( link->ref != 0 )
|
||||
{
|
||||
if( nneis < maxNeis )
|
||||
neis[nneis++] = link->ref;
|
||||
}
|
||||
}
|
||||
|
||||
// If any of the neighbour polygons is within the next few polygons
|
||||
// in the path, short cut to that polygon directly.
|
||||
static const int maxLookAhead = 6;
|
||||
int cut = 0;
|
||||
for( int i = dtMin( maxLookAhead, npath ) - 1; i > 1 && cut == 0; i-- ) {
|
||||
for( int j = 0; j < nneis; j++ )
|
||||
{
|
||||
if( path[i] == neis[j] ) {
|
||||
cut = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
if( cut > 1 )
|
||||
{
|
||||
int offset = cut - 1;
|
||||
npath -= offset;
|
||||
for( int i = 1; i < npath; i++ )
|
||||
path[i] = path[i + offset];
|
||||
}
|
||||
|
||||
return npath;
|
||||
}
|
||||
|
||||
inline bool inRange( const float* v1, const float* v2, const float r, const float h )
|
||||
{
|
||||
const float dx = v2[0] - v1[0];
|
||||
const float dy = v2[1] - v1[1];
|
||||
const float dz = v2[2] - v1[2];
|
||||
return ( dx*dx + dz * dz ) < r*r && fabsf( dy ) < h;
|
||||
}
|
||||
|
||||
static bool getSteerTarget( dtNavMeshQuery* navQuery, const float* startPos, const float* endPos,
|
||||
const float minTargetDist,
|
||||
const dtPolyRef* path, const int pathSize,
|
||||
float* steerPos, unsigned char& steerPosFlag, dtPolyRef& steerPosRef,
|
||||
float* outPoints = 0, int* outPointCount = 0 )
|
||||
{
|
||||
// Find steer target.
|
||||
static const int MAX_STEER_POINTS = 3;
|
||||
float steerPath[MAX_STEER_POINTS * 3];
|
||||
unsigned char steerPathFlags[MAX_STEER_POINTS];
|
||||
dtPolyRef steerPathPolys[MAX_STEER_POINTS];
|
||||
int nsteerPath = 0;
|
||||
navQuery->findStraightPath( startPos, endPos, path, pathSize,
|
||||
steerPath, steerPathFlags, steerPathPolys, &nsteerPath, MAX_STEER_POINTS );
|
||||
if( !nsteerPath )
|
||||
return false;
|
||||
|
||||
if( outPoints && outPointCount )
|
||||
{
|
||||
*outPointCount = nsteerPath;
|
||||
for( int i = 0; i < nsteerPath; ++i )
|
||||
dtVcopy( &outPoints[i * 3], &steerPath[i * 3] );
|
||||
}
|
||||
|
||||
|
||||
// Find vertex far enough to steer to.
|
||||
int ns = 0;
|
||||
while( ns < nsteerPath )
|
||||
{
|
||||
// Stop at Off-Mesh link or when point is further than slop away.
|
||||
if( ( steerPathFlags[ns] & DT_STRAIGHTPATH_OFFMESH_CONNECTION ) ||
|
||||
!inRange( &steerPath[ns * 3], startPos, minTargetDist, 1000.0f ) )
|
||||
break;
|
||||
ns++;
|
||||
}
|
||||
// Failed to find good point to steer to.
|
||||
if( ns >= nsteerPath )
|
||||
return false;
|
||||
|
||||
dtVcopy( steerPos, &steerPath[ns * 3] );
|
||||
steerPos[1] = startPos[1];
|
||||
steerPosFlag = steerPathFlags[ns];
|
||||
steerPosRef = steerPathPolys[ns];
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
std::vector< Sapphire::Common::FFXIVARR_POSITION3 > Sapphire::NaviProvider::PathFindFollow( Common::FFXIVARR_POSITION3 startPos, Common::FFXIVARR_POSITION3 endPos )
|
||||
{
|
||||
if( !m_naviMesh || !m_naviMeshQuery )
|
||||
throw std::exception( "No navimesh loaded" );
|
||||
|
||||
dtPolyRef startRef, endRef = 0;
|
||||
|
||||
float start[3] = { startPos.x, startPos.y, startPos.z };
|
||||
float end[3] = { startPos.x, startPos.y, startPos.z };
|
||||
|
||||
dtQueryFilter filter;
|
||||
filter.setAreaCost( 0, 0 );
|
||||
|
||||
m_naviMeshQuery->findNearestPoly( start, m_polyFindRange, &filter, &startRef, 0 );
|
||||
m_naviMeshQuery->findNearestPoly( end, m_polyFindRange, &filter, &endRef, 0 );
|
||||
|
||||
// Couldn't find any close polys to navigate from
|
||||
if( !startRef || !endRef )
|
||||
return {};
|
||||
|
||||
auto pathFindStatus = DT_FAILURE;
|
||||
|
||||
auto pathIterNum = 0;
|
||||
dtPolyRef polys[MAX_POLYS];
|
||||
int numPolys = 0;
|
||||
|
||||
m_naviMeshQuery->findPath( startRef, endRef, start, end, &filter, polys, &numPolys, MAX_POLYS );
|
||||
|
||||
// Check if we got polys back for navigation
|
||||
if( numPolys )
|
||||
{
|
||||
// Iterate over the path to find smooth path on the detail mesh surface.
|
||||
dtPolyRef polys[MAX_POLYS];
|
||||
memcpy( polys, polys, sizeof( dtPolyRef )*numPolys );
|
||||
int npolys = numPolys;
|
||||
|
||||
float iterPos[3], targetPos[3];
|
||||
m_naviMeshQuery->closestPointOnPoly( startRef, start, iterPos, 0 );
|
||||
m_naviMeshQuery->closestPointOnPoly( polys[npolys - 1], end, targetPos, 0 );
|
||||
|
||||
static const float STEP_SIZE = 0.5f;
|
||||
static const float SLOP = 0.01f;
|
||||
|
||||
int numSmoothPath = 0;
|
||||
float smoothPath[MAX_SMOOTH * 3];
|
||||
|
||||
dtVcopy( &smoothPath[numSmoothPath * 3], iterPos );
|
||||
numSmoothPath++;
|
||||
|
||||
// Move towards target a small advancement at a time until target reached or
|
||||
// when ran out of memory to store the path.
|
||||
while( npolys && numSmoothPath < MAX_SMOOTH )
|
||||
{
|
||||
// Find location to steer towards.
|
||||
float steerPos[3];
|
||||
unsigned char steerPosFlag;
|
||||
dtPolyRef steerPosRef;
|
||||
|
||||
if( !getSteerTarget( m_naviMeshQuery, iterPos, targetPos, SLOP,
|
||||
polys, npolys, steerPos, steerPosFlag, steerPosRef ) )
|
||||
break;
|
||||
|
||||
bool endOfPath = ( steerPosFlag & DT_STRAIGHTPATH_END ) ? true : false;
|
||||
bool offMeshConnection = ( steerPosFlag & DT_STRAIGHTPATH_OFFMESH_CONNECTION ) ? true : false;
|
||||
|
||||
// Find movement delta.
|
||||
float delta[3], len;
|
||||
dtVsub( delta, steerPos, iterPos );
|
||||
len = dtMathSqrtf( dtVdot( delta, delta ) );
|
||||
// If the steer target is end of path or off-mesh link, do not move past the location.
|
||||
if( ( endOfPath || offMeshConnection ) && len < STEP_SIZE )
|
||||
len = 1;
|
||||
else
|
||||
len = STEP_SIZE / len;
|
||||
float moveTgt[3];
|
||||
dtVmad( moveTgt, iterPos, delta, len );
|
||||
|
||||
// Move
|
||||
float result[3];
|
||||
dtPolyRef visited[16];
|
||||
int nvisited = 0;
|
||||
m_naviMeshQuery->moveAlongSurface( polys[0], iterPos, moveTgt, &filter,
|
||||
result, visited, &nvisited, 16 );
|
||||
|
||||
npolys = fixupCorridor( polys, npolys, MAX_POLYS, visited, nvisited );
|
||||
npolys = fixupShortcuts( polys, npolys, m_naviMeshQuery );
|
||||
|
||||
float h = 0;
|
||||
m_naviMeshQuery->getPolyHeight( polys[0], result, &h );
|
||||
result[1] = h;
|
||||
dtVcopy( iterPos, result );
|
||||
|
||||
// Handle end of path and off-mesh links when close enough.
|
||||
if( endOfPath && inRange( iterPos, steerPos, SLOP, 1.0f ) )
|
||||
{
|
||||
// Reached end of path.
|
||||
dtVcopy( iterPos, targetPos );
|
||||
if( numSmoothPath < MAX_SMOOTH )
|
||||
{
|
||||
dtVcopy( &smoothPath[numSmoothPath * 3], iterPos );
|
||||
numSmoothPath++;
|
||||
}
|
||||
break;
|
||||
}
|
||||
else if( offMeshConnection && inRange( iterPos, steerPos, SLOP, 1.0f ) )
|
||||
{
|
||||
// Reached off-mesh connection.
|
||||
float startPos[3], endPos[3];
|
||||
|
||||
// Advance the path up to and over the off-mesh connection.
|
||||
dtPolyRef prevRef = 0, polyRef = polys[0];
|
||||
int npos = 0;
|
||||
while( npos < npolys && polyRef != steerPosRef )
|
||||
{
|
||||
prevRef = polyRef;
|
||||
polyRef = polys[npos];
|
||||
npos++;
|
||||
}
|
||||
for( int i = npos; i < npolys; ++i )
|
||||
polys[i - npos] = polys[i];
|
||||
npolys -= npos;
|
||||
|
||||
// Handle the connection.
|
||||
dtStatus status = m_naviMesh->getOffMeshConnectionPolyEndPoints( prevRef, polyRef, startPos, endPos );
|
||||
if( dtStatusSucceed( status ) )
|
||||
{
|
||||
if( numSmoothPath < MAX_SMOOTH )
|
||||
{
|
||||
dtVcopy( &smoothPath[numSmoothPath * 3], startPos );
|
||||
numSmoothPath++;
|
||||
// Hack to make the dotted path not visible during off-mesh connection.
|
||||
if( numSmoothPath & 1 )
|
||||
{
|
||||
dtVcopy( &smoothPath[numSmoothPath * 3], startPos );
|
||||
numSmoothPath++;
|
||||
}
|
||||
}
|
||||
// Move position at the other side of the off-mesh link.
|
||||
dtVcopy( iterPos, endPos );
|
||||
float eh = 0.0f;
|
||||
m_naviMeshQuery->getPolyHeight( polys[0], iterPos, &eh );
|
||||
iterPos[1] = eh;
|
||||
}
|
||||
}
|
||||
|
||||
// Store results.
|
||||
if( numSmoothPath < MAX_SMOOTH )
|
||||
{
|
||||
dtVcopy( &smoothPath[numSmoothPath * 3], iterPos );
|
||||
numSmoothPath++;
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
return {};
|
||||
}
|
||||
}
|
||||
|
||||
void Sapphire::NaviProvider::LoadMesh( std::string path )
|
||||
|
|
@ -130,6 +444,6 @@ void Sapphire::NaviProvider::LoadMesh( std::string path )
|
|||
|
||||
m_naviMesh->addTile( data, tileHeader.dataSize, DT_TILE_FREE_DATA, tileHeader.tileRef, 0 );
|
||||
}
|
||||
|
||||
|
||||
fclose( fp );
|
||||
}
|
||||
|
|
@ -29,6 +29,9 @@ namespace Sapphire
|
|||
int dataSize;
|
||||
};
|
||||
|
||||
static const int MAX_POLYS = 256;
|
||||
static const int MAX_SMOOTH = 2048;
|
||||
|
||||
public:
|
||||
NaviProvider( const ZonePtr pZone, Sapphire::FrameworkPtr pFw );
|
||||
|
||||
|
|
@ -36,6 +39,8 @@ namespace Sapphire
|
|||
void LoadMesh( std::string path );
|
||||
void InitQuery();
|
||||
|
||||
std::vector< Sapphire::Common::FFXIVARR_POSITION3 > PathFindFollow(Common::FFXIVARR_POSITION3 startPos, Common::FFXIVARR_POSITION3 endPos);
|
||||
|
||||
bool HasNaviMesh() const;
|
||||
|
||||
protected:
|
||||
|
|
@ -44,6 +49,8 @@ namespace Sapphire
|
|||
|
||||
dtNavMesh* m_naviMesh;
|
||||
dtNavMeshQuery* m_naviMeshQuery;
|
||||
|
||||
float m_polyFindRange[3];
|
||||
};
|
||||
|
||||
}
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue