npc_pathfinding.cpp

#include <DetourNavMeshQuery.h>
#include "NPC_local.h"

extern "C"
{
    #include "g_local.h"
}

bool FindNearestPoly ( const vec3_t position, const dtNavMesh *navMesh, const dtNavMeshQuery *query, dtPolyRef *nearestPoly )
{
    vec3_t recastStart;
    Q3PointToRecast (position, recastStart);

    vec3_t extents = { 64.0f, 64.0f, 128.0f };
    Q3PointToRecast (extents, extents);
    for ( int i = 0; i < 3; i++ ) extents[i] = abs (extents[i]);

    dtQueryFilter filter;
    dtStatus status = query->findNearestPoly (recastStart, extents, &filter, nearestPoly, NULL);
    if ( dtStatusFailed (status) )
    {
        return false;
    }

    return true;
}

extern "C" void G_TestLine (vec3_t start, vec3_t end, int color, int time);
bool CalculatePath ( const vec3_t start, const vec3_t end, const dtNavMesh *navMesh, const dtNavMeshQuery *query, vec3_t *path, int maxPathLength, int *pathLength )
{
    dtPolyRef startPoly, endPoly;
    if ( !FindNearestPoly (start, navMesh, query, &startPoly) )
    {
        return false;
    }

    if ( !FindNearestPoly (end, navMesh, query, &endPoly) )
    {
        return false;
    }

    vec3_t recastStart, recastEnd;
    Q3PointToRecast (start, recastStart);
    Q3PointToRecast (end, recastEnd);

    static const int MAX_PATH_POLYS = 1024;
    dtPolyRef pathPolys[MAX_PATH_POLYS];
    int pathCount;

    dtQueryFilter filter;
    if ( dtStatusFailed (query->findPath (startPoly, endPoly, &recastStart[0], &recastEnd[0], &filter, pathPolys, &pathCount, MAX_PATH_POLYS)) )
    {
        return false;
    }

    // Get best path from here...
    if ( dtStatusFailed (query->findStraightPath (&recastStart[0], &recastEnd[0], pathPolys, pathCount, &path[0][0], NULL, NULL, pathLength, maxPathLength)) )
    {
        return false;
    }

    for ( int i = 0; i < *pathLength; i++ )
    {
        RecastPointToQ3 (path[i], path[i]);
    }

    if ( *pathLength > 1 )
    {
        // Now draw the path
        // Draw first segment
        vec3_t p1, p2;
        VectorCopy (path[0], p1);
        VectorCopy (path[1], p2);

        p1[2] += 15.0f;
        p2[2] += 15.0f;
        G_TestLine (p1, p2, 0x0000ff, 15000);

        for ( int i = 2; i < *pathLength; i++ )
        {
            VectorCopy (path[i - 1], p1);
            VectorCopy (path[i]    , p2);

            p1[2] += 15.0f;
            p2[2] += 15.0f;
            G_TestLine (p1, p2, 0x0000ff, 15000);
        }
    }

    // Remove nodes which are too close
    while ( *pathLength > 1 && DistanceSquared (path[0], start) < 200.0f )
    {
        memmove (path, path + 1, sizeof (vec3_t) * (*pathLength - 1));
        (*pathLength)--;
    }

    return true;
}

extern "C" void Cmd_Nav_DebugPathfinding ( const gentity_t *ent )
{
    if ( !ent || !ent->client )
    {
        return;
    }

    if ( ent->client->sess.sessionTeam == TEAM_SPECTATOR )
    {
        PrintTo (ent->s.number, "Cannot use this command while spectating.\n");
        return;
    }

    static const float MAX_TRACE_DISTANCE = 8192.0f;
    vec3_t forward;
    vec3_t start, end;

    AngleVectors (ent->client->ps.viewangles, forward, NULL, NULL);
    VectorCopy (ent->client->ps.origin, start);
    start[2] += ent->client->ps.viewheight;

    VectorMA (start, MAX_TRACE_DISTANCE, forward, end);

    trace_t tr;

    trap_Trace (&tr, start, vec3_origin, vec3_origin, end, ent->s.number, MASK_PLAYERSOLID);
    if ( tr.fraction < 1.0f )
    {
        const gentity_t *npc = GetEntity (tr.entityNum);
        if ( npc->s.eType != ET_NPC )
        {
            PrintTo (ent->s.number, "No NPC found at crosshair.\n");
        }
        else
        {
            VectorCopy (ent->client->ps.origin, end);
            VectorCopy (npc->client->ps.origin, start);

            if ( CalculatePath (start, end, level.navMeshData->navMesh, level.navMeshData->navQuery, npc->npc->pathNodes, 100, &npc->npc->pathLength) )
            {
                PrintTo (ent->s.number, "Found route.\n");
                npc->npc->targetNodeIndex = 0;
            }
            else
            {
                PrintTo (ent->s.number, "Failed to find route.\n");
            }
        }
    }
}