npc_navmesh.cpp

#include <math.h>
#include <DetourNavMesh.h>
#include <DetourNavMeshBuilder.h>
#include <Recast.h>
#include "NPC_local.h"

extern "C" {
#include "g_local.h"
#include "qcommon/qfiles.h"
}

levelNavMeshData navMeshData;

int CountIndices ( const dsurface_t *surfaces, int numSurfaces )
{
    int count = 0;
    for ( int i = 0; i < numSurfaces; i++, surfaces++ )
    {
        if ( surfaces->surfaceType != MST_PLANAR && surfaces->surfaceType != MST_TRIANGLE_SOUP )
        {
            continue;
        }
        
        count += surfaces->numIndexes;
    }
    
    return count;
}

bool IsSolid ( const int shaderNum, const dshader_t *shaders )
{
    int contentsFlags = shaders[shaderNum].contentFlags;

    return (contentsFlags & (CONTENTS_FOG | CONTENTS_LAVA | CONTENTS_WATER)) == 0;
}

int LoadTriangles ( const int *indexes, int* tris, const dsurface_t *surfaces, const int numSurfaces, const dshader_t *shaders )
{
    int t = 0;
    int v = 0;
    int numTris = 0;
    for ( int i = 0; i < numSurfaces; i++, surfaces++ )
    {
        if ( surfaces->surfaceType != MST_PLANAR && surfaces->surfaceType != MST_TRIANGLE_SOUP )
        {
            continue;
        }
        
        for ( int j = surfaces->firstIndex, k = 0; k < surfaces->numIndexes; j++, k++ )
        {
            tris[t++] = v + indexes[j];
            numTris++;
        }
        
        v += surfaces->numVerts;
    }

    return numTris;
}

int LoadVertices ( const drawVert_t *vertices, float* verts, const dsurface_t *surfaces, const int numSurfaces, const dshader_t *shaders )
{
    int v = 0;
    int numverts = 0;
    for ( int i = 0; i < numSurfaces; i++, surfaces++ )
    {
        // will handle patches later...
        if ( surfaces->surfaceType != MST_PLANAR && surfaces->surfaceType != MST_TRIANGLE_SOUP )
        {
            continue;
        }
        
        for ( int j = surfaces->firstVert, k = 0; k < surfaces->numVerts; j++, k++ )
        {
            verts[v++] = -vertices[j].xyz[0];
            verts[v++] = vertices[j].xyz[2];
            verts[v++] = -vertices[j].xyz[1];

            numverts += 3;
        }
    }

    return numverts;
}

void LoadModels ( const dheader_t *header, const char *buffer, vec3_t mapmins, vec3_t mapmaxs, float*& verts, int& numverts, int*& tris, int& numtris )
{
    const dmodel_t *models      = (const dmodel_t *)(buffer + header->lumps[LUMP_MODELS].fileofs);
    int numModels               = header->lumps[LUMP_MODELS].filelen / sizeof (dmodel_t);

    const int *indexes          = (const int *)(buffer + header->lumps[LUMP_DRAWINDEXES].fileofs);
    const dsurface_t *surfaces  = (const dsurface_t *)(buffer + header->lumps[LUMP_SURFACES].fileofs);
    const dshader_t *shaders    = (const dshader_t *)(buffer + header->lumps[LUMP_SHADERS].fileofs);
    int numSurfaces             = header->lumps[LUMP_SURFACES].filelen / sizeof (dsurface_t);

    for ( int i = 0; i < numModels; i++, models )
    {

        //models->
    }

    // Load indices
    numtris = CountIndices (surfaces, numSurfaces);
    tris = new int[numtris];
    numtris /= 3;
    
    LoadTriangles (indexes, tris, surfaces, numSurfaces, shaders);
    
    // Load vertices
    const drawVert_t *vertices = (const drawVert_t *)(buffer + header->lumps[LUMP_DRAWVERTS].fileofs);
    numverts = header->lumps[LUMP_DRAWVERTS].filelen / sizeof (drawVert_t);
    
    verts = new float[3 * numverts];
    LoadVertices (vertices, verts, surfaces, numSurfaces, shaders);

    // Get map bounds. First model is always the entire map
    mapmins[0] = -models[0].maxs[0];
    mapmins[1] = models[0].mins[2];
    mapmins[2] = -models[0].maxs[1];
    
    mapmaxs[0] = -models[0].mins[0];
    mapmaxs[1] = models[0].maxs[2];
    mapmaxs[2] = -models[0].mins[1];
}

bool LoadMapGeometry ( const char *buffer, vec3_t mapmins, vec3_t mapmaxs, float*& verts, int& numverts, int*& tris, int& numtris )
{
    const dheader_t *header = (const dheader_t *)buffer;
    if ( header->ident != BSP_IDENT )
    {
        G_Printf ("Expected ident '%d', found %d.\n", BSP_IDENT, header->ident);
        return false;
    }
    
    if ( header->version != BSP_VERSION )
    {
        G_Printf ("Expected version '%d', found %d.\n", BSP_VERSION, header->version);
        return false;
    }
    
    // Load models
    LoadModels (header, buffer, mapmins, mapmaxs, verts, numverts,  tris, numtris);
    
    return true;
}

const int NAVMESH_DATA_VERSION = 1;
struct navMeshDataHeader_t
{
    int version;
    int filesize;
    int numTiles;
    
    dtNavMeshParams params;
};

struct navMeshDataTileHeader_t
{
    dtTileRef tileRef;
    int dataSize;
};

void CacheNavMeshData ( const char *mapname )
{
    navMeshDataHeader_t header;
    const dtNavMesh *mesh = navMeshData.navMesh;
    
    header.version = NAVMESH_DATA_VERSION;
	header.numTiles = 0;
    header.params = *mesh->getParams();
    header.filesize = sizeof (navMeshDataHeader_t);

	for (int i = 0; i < mesh->getMaxTiles(); ++i)
	{
		const dtMeshTile* tile = mesh->getTile(i);
		if (!tile || !tile->header || !tile->dataSize) continue;

		header.numTiles++;

        header.filesize += tile->dataSize + sizeof (navMeshDataTileHeader_t);
	}

    char *buffer = new char[header.filesize];

    int ptr = 0;
    memcpy (buffer, &header, sizeof (header));
    ptr += sizeof (header);

    for ( int i = 0; i < header.numTiles; i++ )
    {
        const dtMeshTile* tile = mesh->getTile(i);
		if (!tile || !tile->header || !tile->dataSize) continue;

        navMeshDataTileHeader_t tileHeader;
		tileHeader.tileRef = mesh->getTileRef(tile);
		tileHeader.dataSize = tile->dataSize;

        memcpy (buffer + ptr, &tileHeader, sizeof (tileHeader));
        ptr += sizeof (tileHeader);

        memcpy (buffer + ptr, tile->data, tileHeader.dataSize);
        ptr += tileHeader.dataSize;
    }

    if ( ptr != header.filesize )
    {
        G_Printf ("WE DUN GOOFED SIRZ\n");
    }
    else
    {
        fileHandle_t f;
        trap_FS_FOpenFile (va ("%s.jnd", mapname), &f, FS_WRITE);
        if ( !f )
        {
            G_Printf ("Failed to create cache file for navmesh.\n");
        }
        else
        {
            trap_FS_Write (buffer, header.filesize, f);
            trap_FS_FCloseFile (f);
            
            G_Printf ("Navmesh data cached (%d bytes written)...\n", header.filesize);
        }
    }
    
    delete[] buffer;
}

void LoadCachedNavMesh ( const char *mapname )
{
    char mappath[MAX_QPATH];
    Com_sprintf (mappath, sizeof (mappath), "maps/%s.bsp.jnd", mapname);

    fileHandle_t f = 0;
    int fileLength = trap_FS_FOpenFile (mappath, &f, FS_READ);
    if ( fileLength == -1 || f == 0 )
    {
        // No cache available
        return;
    }

    char *buffer = new char[fileLength + 1];

    trap_FS_Read (buffer, fileLength, f);
    trap_FS_FCloseFile (f);

    buffer[fileLength] = '\0';

    navMeshDataHeader_t *header = (navMeshDataHeader_t *)buffer;
    if ( header->version != NAVMESH_DATA_VERSION )
    {
        delete[] buffer;
        return;
    }

    if ( header->filesize != fileLength )
    {
        delete[] buffer;
        return;
    }

    navMeshData.navMesh = dtAllocNavMesh();
    if ( dtStatusFailed (navMeshData.navMesh->init (&header->params)) )
    {
        G_Printf ("Failed to create navmesh from cache file.\n");

        delete[] buffer;
        return;
    }

    int offset = sizeof (navMeshDataHeader_t);
    for ( int i = 0; i < header->numTiles; i++ )
    {
        navMeshDataTileHeader_t *tileHeader = (navMeshDataTileHeader_t *)((char *)buffer + offset);
        offset += tileHeader->dataSize + sizeof (navMeshDataTileHeader_t);

        unsigned char *tileData = (unsigned char *)dtAlloc (tileHeader->dataSize, DT_ALLOC_PERM);
        memcpy (tileData, (char *)(tileHeader + 1), tileHeader->dataSize);

        navMeshData.navMesh->addTile (tileData, tileHeader->dataSize, DT_TILE_FREE_DATA, tileHeader->tileRef, NULL);
    }

    delete[] buffer;

    navMeshData.navQuery = dtAllocNavMeshQuery();
    if ( dtStatusFailed (navMeshData.navQuery->init (navMeshData.navMesh, 2048)) )
    {
        G_Printf ("Failed to create navigation mesh query object.\n");
    }
    else
    {
        G_Printf ("Navigation mesh cache loaded.\n");
    }
}

void CreateNavMesh ( const char *mapname )
{
    rcHeightfield *heightField;
    unsigned char *navData;
    int navDataSize;
    
    fileHandle_t f = 0;
    int fileLength = trap_FS_FOpenFile (mapname, &f, FS_READ);
    if ( fileLength == -1 || !f )
    {
        G_Printf ("Unable to open '%s' to create the navigation mesh.\n", mapname);
        return;
    }
    
    char *buffer = new char[fileLength + 1];
    trap_FS_Read (buffer, fileLength, f);
    buffer[fileLength] = '\0';
    trap_FS_FCloseFile (f);
    
    vec3_t mapmins;
    vec3_t mapmaxs;
    float *verts = NULL;
    int numverts;
    int *tris = NULL;
    int numtris;
    unsigned char *triareas = NULL;
    rcContourSet *contours = NULL;
    rcCompactHeightfield *compHeightField = NULL;
    rcPolyMesh *polyMesh = NULL;
    rcPolyMeshDetail *detailedPolyMesh = NULL;
    rcConfig cfg = {};
    dtNavMeshCreateParams nvParams = {};

    rcContext context (false);
    
    if ( !LoadMapGeometry (buffer, mapmins, mapmaxs, verts, numverts, tris, numtris) )
    {
        G_Printf ("Unable to load map geometry from '%s'.\n", mapname);
        goto cleanup;
    }
    
    // 1. Create build config...
    VectorCopy (mapmaxs, cfg.bmax);
    VectorCopy (mapmins, cfg.bmin);
    cfg.ch = 3.0f;
    cfg.cs = 15.0f;
    cfg.walkableSlopeAngle = 45.0f; // worked out from MIN_WALK_NORMAL - i think it's correct? :x
    cfg.walkableHeight = 64 / cfg.ch;
    cfg.walkableClimb = STEPSIZE / cfg.ch;
    cfg.walkableRadius = 15 / cfg.cs;
    cfg.maxEdgeLen = 12 / cfg.cs;
    cfg.maxSimplificationError = 0.5f;
    cfg.minRegionArea = 40;
    cfg.mergeRegionArea = 400;
    cfg.maxVertsPerPoly = 6;
    cfg.tileSize = 1024;
    cfg.borderSize = 15;
    cfg.detailSampleDist = 6.0f * cfg.cs;
    cfg.detailSampleMaxError = 1.0f * cfg.ch;
    
    rcCalcGridSize (cfg.bmin, cfg.bmax, cfg.cs, &cfg.width, &cfg.height);
    
    // 2. Rasterize input polygon soup!
    heightField = rcAllocHeightfield();
    if ( !rcCreateHeightfield (&context, *heightField, cfg.width, cfg.height, cfg.bmin, cfg.bmax, cfg.cs, cfg.ch) )
    {
        G_Printf ("Failed to create heightfield for navigation mesh.\n");
        goto cleanup;
    }
    
    triareas = new unsigned char[numtris];
    memset (triareas, 0, numtris);
    rcMarkWalkableTriangles (&context, cfg.walkableSlopeAngle, verts, numverts, tris, numtris, triareas);
    rcRasterizeTriangles (&context, verts, numverts, tris, triareas, numtris, *heightField, cfg.walkableClimb);
    delete[] triareas;
    triareas = NULL;
    
    // 3. Filter walkable surfaces
    rcFilterLowHangingWalkableObstacles (&context, cfg.walkableClimb, *heightField);
    rcFilterLedgeSpans (&context, cfg.walkableHeight, cfg.walkableClimb, *heightField);
    rcFilterWalkableLowHeightSpans (&context, cfg.walkableHeight, *heightField);
    
    // 4. Partition walkable surface to simple regions
    compHeightField = rcAllocCompactHeightfield();
    if ( !rcBuildCompactHeightfield (&context, cfg.walkableHeight, cfg.walkableClimb, *heightField, *compHeightField) )
    {
        G_Printf ("Failed to create compact heightfield for navigation mesh.\n");
        goto cleanup;
    }
    
    if ( !rcErodeWalkableArea (&context, cfg.walkableRadius, *compHeightField) )
    {
        G_Printf ("Unable to erode walkable surfaces.\n");
        goto cleanup;
    }
    
    if ( !rcBuildDistanceField (&context, *compHeightField) )
    {
        G_Printf ("Failed to build distance field for navigation mesh.\n");
        goto cleanup;
    }
    
    if ( !rcBuildRegions (&context, *compHeightField, 0, cfg.minRegionArea, cfg.mergeRegionArea) )
    {
        G_Printf ("Failed to build regions for navigation mesh.\n");
        goto cleanup;
    }
    
    // 5. Create contours
    contours = rcAllocContourSet();
    if ( !rcBuildContours (&context, *compHeightField, cfg.maxSimplificationError, cfg.maxEdgeLen, *contours) )
    {
        G_Printf ("Failed to create contour set for navigation mesh.\n");
        goto cleanup;
    }
    
    // 6. Build polygons mesh from contours
    polyMesh = rcAllocPolyMesh();
    if ( !rcBuildPolyMesh (&context, *contours, cfg.maxVertsPerPoly, *polyMesh) )
    {
        G_Printf ("Failed to triangulate contours.\n");
        goto cleanup;
    }
    
    // 7. Create detail mesh
    detailedPolyMesh = rcAllocPolyMeshDetail();
    if ( !rcBuildPolyMeshDetail (&context, *polyMesh, *compHeightField, cfg.detailSampleDist, cfg.detailSampleMaxError, *detailedPolyMesh) )
    {
        G_Printf ("Failed to create detail mesh for navigation mesh.\n");
        goto cleanup;
    }

    for ( int i = 0; i < polyMesh->npolys; i++ )
    {
		polyMesh->flags[i] = 1;
    }
    
    // 8. Create navigation mesh query object
    nvParams.verts = polyMesh->verts;
    nvParams.vertCount = polyMesh->nverts;
    nvParams.polys = polyMesh->polys;
    nvParams.polyAreas = polyMesh->areas;
    nvParams.polyFlags = polyMesh->flags;
    nvParams.polyCount = polyMesh->npolys;
    nvParams.nvp = polyMesh->nvp;
    nvParams.detailMeshes = detailedPolyMesh->meshes;
    nvParams.detailVerts = detailedPolyMesh->verts;
    nvParams.detailVertsCount = detailedPolyMesh->nverts;
    nvParams.detailTris = detailedPolyMesh->tris;
    nvParams.detailTriCount = detailedPolyMesh->ntris;
    nvParams.walkableHeight = 64.0f;
    nvParams.walkableRadius = 23.0f;
    nvParams.walkableClimb = STEPSIZE;
    VectorCopy (polyMesh->bmin, nvParams.bmin);
    VectorCopy (polyMesh->bmax, nvParams.bmax);
    nvParams.cs = cfg.cs;
    nvParams.ch = cfg.ch;
    nvParams.buildBvTree = true;
    
    navData = NULL;
    navDataSize = 0;
    if ( !dtCreateNavMeshData (&nvParams, &navData, &navDataSize) )
    {
        G_Printf ("Failed to create navigation mesh data.\n");
        goto cleanup;
    }

    if ( navMeshData.navMesh != NULL )
    {
        dtFreeNavMeshQuery (navMeshData.navQuery);
        dtFreeNavMesh (navMeshData.navMesh);
    }

    navMeshData.navMesh = dtAllocNavMesh();
    if ( dtStatusFailed (navMeshData.navMesh->init (navData, navDataSize, DT_TILE_FREE_DATA)) )
    {
        G_Printf ("Failed to create navigation mesh.\n");
        goto cleanup;
    }

    // Cache the stuffffffff
    CacheNavMeshData (mapname);

    navMeshData.navQuery = dtAllocNavMeshQuery();
    if ( dtStatusFailed (navMeshData.navQuery->init (navMeshData.navMesh, 2048)) )
    {
        G_Printf ("Failed to create navigation mesh query object.\n");
        goto cleanup;
    }
    
cleanup:
    rcFreeHeightField (heightField);
    rcFreeCompactHeightfield (compHeightField);
    rcFreeContourSet (contours);
    rcFreePolyMesh (polyMesh);
    rcFreePolyMeshDetail (detailedPolyMesh);
    
    delete[] verts;
    delete[] tris;
    delete[] buffer;
}

extern "C"
{
    void Nav_Init ( const char *mapname )
    {
        level.navMeshData = &navMeshData;

        LoadCachedNavMesh (mapname);
    }

    void Nav_Shutdown ( void )
    {
        dtFreeNavMeshQuery (navMeshData.navQuery);
        navMeshData.navQuery = NULL;

        dtFreeNavMesh (navMeshData.navMesh);
        navMeshData.navMesh = NULL;
    }

    void Nav_CreateNavMesh ( const char* mapname )
    {
        trap_SendServerCommand (-1, "print \"Creating navigation mesh...this may take a while.\n\"");
        CreateNavMesh (mapname);
        trap_SendServerCommand (-1, "print \"Finished!\n\"");
    }
}