NavMesher.cpp

#include "NavMesher.h"

NavMesher::NavMesher(LPDIRECT3DDEVICE9 device)
{
    m_d3d9device = device;
    m_d3d9device->AddRef();
    m_buildOpts.m_cellSize             = 0.1f;//0.3f;
    m_buildOpts.m_cellHeight           = 0.1f;//0.2f;
    m_buildOpts.m_agentHeight          = 2.0f;
    m_buildOpts.m_agentRadius          = 0.4f;
    m_buildOpts.m_agentMaxClimb        = 0.2f;//0.9f;
    m_buildOpts.m_agentMaxSlope        = 45.0f;
    m_buildOpts.m_regionMinSize        = 8;
    m_buildOpts.m_regionMergeSize      = 20;
    m_buildOpts.m_monotonePartitioning = false;
    m_buildOpts.m_edgeMaxLen           = 12.0f;
    m_buildOpts.m_edgeMaxError         = 1.3f;
    m_buildOpts.m_vertsPerPoly         = 6.0f;
    m_buildOpts.m_detailSampleDist     = 6.0f;
    m_buildOpts.m_detailSampleMaxError = 1.0f;
}

NavMesher::~NavMesher()
{
    m_dxDebugMesh->Release();
    dtFreeNavMesh(m_navMesh);
    dtFreeNavMeshQuery(m_navQuery);
    m_d3d9device->Release();
}

bool NavMesher::buildNavMesh(LPD3DXMESH fromMesh)
{
    std::vector<float> vVerts;
    std::vector<int> vTris;
    extractMeshData(fromMesh, vVerts, vTris);

    float* verts = &vVerts[0];
    int* tris    = &vTris[0];
    int nverts   = vVerts.size() / 3;
    int ntris    = vTris.size() / 3;

    rcCalcBounds(verts, nverts, m_boundsMin, m_boundsMax);

    //
    // Step 1. Initialize build config.
    //
    rcConfig cfg;
    memset(&cfg, 0, sizeof(cfg));
    cfg.cs                      = m_buildOpts.m_cellSize;
    cfg.ch                      = m_buildOpts.m_cellHeight;
    cfg.walkableSlopeAngle      = m_buildOpts.m_agentMaxSlope;
    cfg.walkableHeight          = (int)ceilf(m_buildOpts.m_agentHeight / cfg.ch);
    cfg.walkableClimb           = (int)floorf(m_buildOpts.m_agentMaxClimb / cfg.ch);
    cfg.walkableRadius          = (int)ceilf(m_buildOpts.m_agentRadius / cfg.cs);
    cfg.maxEdgeLen              = (int)(m_buildOpts.m_edgeMaxLen / m_buildOpts.m_cellSize);
    cfg.maxSimplificationError  = m_buildOpts.m_edgeMaxError;
    cfg.minRegionArea           = (int)rcSqr(m_buildOpts.m_regionMinSize);      // Note: area = size*size
    cfg.mergeRegionArea         = (int)rcSqr(m_buildOpts.m_regionMergeSize);  // Note: area = size*size
    cfg.maxVertsPerPoly         = (int)m_buildOpts.m_vertsPerPoly;
    cfg.detailSampleDist        = m_buildOpts.m_detailSampleDist < 0.9f ? 0 : m_buildOpts.m_cellSize * m_buildOpts.m_detailSampleDist;
    cfg.detailSampleMaxError    = m_buildOpts.m_cellHeight * m_buildOpts.m_detailSampleMaxError;

    // Set the area where the navigation will be build.
    // Here the bounds of the input mesh are used, but the
    // area could be specified by an user defined box, etc.
    rcVcopy(cfg.bmin, m_boundsMin);
    rcVcopy(cfg.bmax, m_boundsMax);
    rcCalcGridSize(cfg.bmin, cfg.bmax, cfg.cs, &cfg.width, &cfg.height);

    rcContext* ctx = new rcContext(); // set your own "context", this does nothing
    ctx->resetTimers();
    ctx->startTimer(RC_TIMER_TOTAL);

    //
    // Step 2. Rasterize input polygon soup.
    //
    rcHeightfield* pHeightfield = rcAllocHeightfield();
    if(!pHeightfield)
    {
        MessageBox(0,"buildNavigation: Out of memory 'solid'.",0,0);
        return false;
    }
    if (!rcCreateHeightfield(ctx, *pHeightfield, cfg.width, cfg.height, cfg.bmin, cfg.bmax, cfg.cs, cfg.ch))
    {
        MessageBox(0,"buildNavigation: Could not create solid heightfield.",0,0);
        return false;
    }

    // Allocate array that can hold triangle area types.
    // If you have multiple meshes you need to process, allocate
    // and array which can hold the max number of triangles you need to process.
    //unsigned char* pTriAreas = new unsigned char[ntris];
    unsigned char* pTriAreas = new unsigned char[ntris];
    if(!pTriAreas)
    {
        MessageBox(0,"buildNavigation: Out of memory 'pTriAreas' (%d).", 0,0);
        return false;
    }

    // Find triangles which are walkable based on their slope and rasterize them.
    // If your input data is multiple meshes, you can transform them here, calculate
    // the are type for each of the meshes and rasterize them.
    memset(pTriAreas, 0, ntris);
    rcMarkWalkableTriangles(ctx, cfg.walkableSlopeAngle, verts, nverts, tris, ntris, pTriAreas);
    rcRasterizeTriangles(ctx, verts, nverts, tris, pTriAreas, ntris, *pHeightfield, cfg.walkableClimb);

    delete [] pTriAreas;
    pTriAreas = nullptr;

    //
    // Step 3. Filter walkables surfaces.
    //
    // Once all geoemtry is rasterized, we do initial pass of filtering to
    // remove unwanted overhangs caused by the conservative rasterization
    // as well as filter spans where the character cannot possibly stand.
    rcFilterLowHangingWalkableObstacles(ctx, cfg.walkableClimb, *pHeightfield);
    rcFilterLedgeSpans(ctx, cfg.walkableHeight, cfg.walkableClimb, *pHeightfield);
    rcFilterWalkableLowHeightSpans(ctx, cfg.walkableHeight, *pHeightfield);

    //
    // Step 4. Partition walkable surface to simple regions.
    //
    // Compact the heightfield so that it is faster to handle from now on.
    // This will result more cache coherent data as well as the neighbours
    // between walkable cells will be calculated.
    rcCompactHeightfield* pCmpHeightfield = rcAllocCompactHeightfield();
    if(!pCmpHeightfield)
    {
        MessageBox(0,"buildNavigation: Out of memory 'chf'.",0,0);
        return false;
    }
    if (!rcBuildCompactHeightfield(ctx, cfg.walkableHeight, cfg.walkableClimb, *pHeightfield, *pCmpHeightfield))
    {
        MessageBox(0,"buildNavigation: Could not build compact data.",0,0);
        return false;
    }

    rcFreeHeightField(pHeightfield);
    pHeightfield = nullptr;

    // Erode the walkable area by agent radius.
    if(!rcErodeWalkableArea(ctx, cfg.walkableRadius, *pCmpHeightfield))
    {
        MessageBox(0,"buildNavigation: Could not erode.",0,0);
        return false;
    }

    // Prepare for region partitioning, by calculating distance field along the walkable surface.
    if (!rcBuildDistanceField(ctx, *pCmpHeightfield))
    {
        MessageBox(0, "buildNavigation: Could not build distance field.",0,0);
        return false;
    }

    // Partition the walkable surface into simple regions without holes.
    if (!rcBuildRegions(ctx, *pCmpHeightfield, 0, cfg.minRegionArea, cfg.mergeRegionArea))
    {
        MessageBox(0, "buildNavigation: Could not build regions.",0,0);
        return false;
    }

    //
    // Step 5. Trace and simplify region contours.
    //
    // Create contours.
    rcContourSet* pContourSet = rcAllocContourSet();
    if(!pContourSet)
    {
        MessageBox(0,"buildNavigation: Out of memory 'cset'.",0,0);
        return false;
    }
    if(!rcBuildContours(ctx, *pCmpHeightfield, cfg.maxSimplificationError, cfg.maxEdgeLen, *pContourSet))
    {
        MessageBox(0,"buildNavigation: Could not create contours.",0,0);
        return false;
    }

    //
    // Step 6. Build polygons mesh from contours.
    //
    // Build polygon navmesh from the contours.
    rcPolyMesh* pPolyMesh = rcAllocPolyMesh();
    if(!pPolyMesh)
    {
        MessageBox(0,"buildNavigation: Out of memory 'pmesh'.",0,0);
        return false;
    }
    if(!rcBuildPolyMesh(ctx, *pContourSet, cfg.maxVertsPerPoly, *pPolyMesh))
    {
        MessageBox(0,"buildNavigation: Could not triangulate contours.",0,0);
        return false;
    }

    //
    // Step 7. Create detail mesh which allows to access approximate height on each polygon.
    //
    rcPolyMeshDetail* pDetailMesh = rcAllocPolyMeshDetail();
    if(!pDetailMesh)
    {
        MessageBox(0,"buildNavigation: Out of memory 'pmdtl'.",0,0);
        return false;
    }

    if(!rcBuildPolyMeshDetail(ctx, *pPolyMesh, *pCmpHeightfield, cfg.detailSampleDist, cfg.detailSampleMaxError, *pDetailMesh))
    {
        MessageBox(0,"buildNavigation: Could not build detail mesh.",0,0);
        return false;
    }

    rcFreeCompactHeightfield(pCmpHeightfield);
    pCmpHeightfield = nullptr;
    rcFreeContourSet(pContourSet);
    pContourSet = nullptr;

    // At this point the navigation mesh data is ready, you can access it from m_pmesh.
    // See duDebugDrawPolyMesh or dtCreateNavMeshData as examples how to access the data.
    //
    // (Optional) Step 8. Create Detour data from Recast poly mesh.
    //
    // The GUI may allow more max points per polygon than Detour can handle.
    // Only build the detour navmesh if we do not exceed the limit.
    if(cfg.maxVertsPerPoly <= DT_VERTS_PER_POLYGON)
    {
        // Update poly flags from areas.
        for(int i = 0; i < pPolyMesh->npolys; ++i)
        {
            if(pPolyMesh->areas[i] == RC_WALKABLE_AREA)
                pPolyMesh->areas[i] = SAMPLE_POLYAREA_GROUND;

            if(pPolyMesh->areas[i] == SAMPLE_POLYAREA_GROUND ||
                pPolyMesh->areas[i] == SAMPLE_POLYAREA_GRASS ||
                pPolyMesh->areas[i] == SAMPLE_POLYAREA_ROAD)
            {
                pPolyMesh->flags[i] = SAMPLE_POLYFLAGS_WALK;
            }
            else if(pPolyMesh->areas[i] == SAMPLE_POLYAREA_WATER)
            {
                pPolyMesh->flags[i] = SAMPLE_POLYFLAGS_SWIM;
            }
            else if(pPolyMesh->areas[i] == SAMPLE_POLYAREA_DOOR)
            {
                pPolyMesh->flags[i] = SAMPLE_POLYFLAGS_WALK | SAMPLE_POLYFLAGS_DOOR;
            }
        }

        dtNavMeshCreateParams params;
        memset(&params, 0, sizeof(params));
        params.verts            = pPolyMesh->verts;
        params.vertCount        = pPolyMesh->nverts;
        params.polys            = pPolyMesh->polys;
        params.polyAreas        = pPolyMesh->areas;
        params.polyFlags        = pPolyMesh->flags;
        params.polyCount        = pPolyMesh->npolys;
        params.nvp              = pPolyMesh->nvp;
        params.detailMeshes     = pDetailMesh->meshes;
        params.detailVerts      = pDetailMesh->verts;
        params.detailVertsCount = pDetailMesh->nverts;
        params.detailTris       = pDetailMesh->tris;
        params.detailTriCount   = pDetailMesh->ntris;

        params.offMeshConAreas  = 0;
        params.offMeshConCount  = 0;
        params.offMeshConDir    = 0;
        params.offMeshConFlags  = 0;
        params.offMeshConRad    = 0;
        params.offMeshConUserID = 0;
        params.offMeshConVerts  = 0;

        params.walkableHeight   = m_buildOpts.m_agentHeight;
        params.walkableRadius   = m_buildOpts.m_agentRadius;
        params.walkableClimb    = m_buildOpts.m_agentMaxClimb;
        rcVcopy(params.bmin, pPolyMesh->bmin);
        rcVcopy(params.bmax, pPolyMesh->bmax);
        params.cs = cfg.cs;
        params.ch = cfg.ch;
        params.buildBvTree = true;

        unsigned char* navData = 0;
        int navDataSize = 0;
        if(!dtCreateNavMeshData(&params, &navData, &navDataSize))
        {
            MessageBox(0,"Could not build Detour navmesh.",0,0);
            return false;
        }

        m_navMesh = dtAllocNavMesh();
        if(!m_navMesh)
        {
            dtFree(navData);
            MessageBox(0, "Could not create Detour navmesh",0,0);
            return false;
        }

        dtStatus status;

        status = m_navMesh->init(navData, navDataSize, DT_TILE_FREE_DATA);
        if (dtStatusFailed(status))
        {
            dtFree(navData);
            MessageBox(0,"Could not init Detour navmesh",0,0);
            return false;
        }

        m_navQuery = dtAllocNavMeshQuery();
        status = m_navQuery->init(m_navMesh, 2048);
        if(dtStatusFailed(status))
        {
            MessageBox(0,"Could not init Detour navmesh query",0,0);
            return false;
        }
    }

    ctx->stopTimer(RC_TIMER_TOTAL);

    buildDebugMesh(pDetailMesh);

    rcFreePolyMesh(pPolyMesh);
    rcFreePolyMeshDetail(pDetailMesh);
    delete ctx;
    return true;
}

bool NavMesher::extractMeshData(LPD3DXMESH dxMesh, std::vector<float>& verts, std::vector<int>& tris)
{
    D3DVERTEXELEMENT9 decl[MAXD3DDECLLENGTH];
    dxMesh->GetDeclaration(decl);

    WORD offsetToPosition = 0;

    for (UINT i = 0; i < MAXD3DDECLLENGTH; ++i)
    {
        if (D3DDECLUSAGE_POSITION == decl[i].Usage)
        {
            offsetToPosition = decl[i].Offset;
        }
    }

    DWORD opt        = dxMesh->GetOptions();
    DWORD indStride  = (opt & D3DXMESH_32BIT) ? sizeof(DWORD) : sizeof(WORD);
    DWORD numInd     = dxMesh->GetNumFaces() * 3;
    DWORD vertStride = dxMesh->GetNumBytesPerVertex();
    DWORD numVerts   = dxMesh->GetNumVertices();

    BYTE* pInd;
    BYTE* pVert;
    dxMesh->LockVertexBuffer(0, (LPVOID*)&pVert);
    dxMesh->LockIndexBuffer(0, (LPVOID*)&pInd);

    for (int i = 0; i < int(numVerts); ++i)
    {
        D3DXVECTOR3 pos = *(D3DXVECTOR3*)(&pVert[vertStride * i + offsetToPosition]);
        verts.push_back(pos.x);
        verts.push_back(pos.y);
        verts.push_back(pos.z);
    }

    for (int i = 0; i < int(numInd); i += 3)
    {
        int inx0, inx1, inx2;
        if (sizeof(DWORD) == indStride)
        {
            inx0 = *(int*)(&pInd[indStride * (i + 0)]);
            inx1 = *(int*)(&pInd[indStride * (i + 1)]);
            inx2 = *(int*)(&pInd[indStride * (i + 2)]);
        }
        else
        {
            inx0 = *(WORD*)(&pInd[indStride * (i + 0)]);
            inx1 = *(WORD*)(&pInd[indStride * (i + 1)]);
            inx2 = *(WORD*)(&pInd[indStride * (i + 2)]);
        }
        tris.push_back(inx0);
        tris.push_back(inx1);
        tris.push_back(inx2);
    }

    dxMesh->UnlockVertexBuffer();
    dxMesh->UnlockIndexBuffer();

    return true;
}

bool NavMesher::buildDebugMesh(rcPolyMeshDetail* dmesh)
{
    if(dmesh->nmeshes == 0)
    {
        MessageBox(0,"getMeshDataFromPolyMeshDetail(): dmesh->nmeshes == 0\n", 0,0);
        return false;
    }

    D3DVERTEXELEMENT9 vertexElements[] =
    {
        {0, 0,  D3DDECLTYPE_FLOAT3,  D3DDECLMETHOD_DEFAULT, D3DDECLUSAGE_POSITION, 0},
        {0, 12, D3DDECLTYPE_FLOAT3,  D3DDECLMETHOD_DEFAULT, D3DDECLUSAGE_NORMAL,   0},
        {0, 24, D3DDECLTYPE_FLOAT2,  D3DDECLMETHOD_DEFAULT, D3DDECLUSAGE_TEXCOORD, 0},
        D3DDECL_END()
    };

    if(FAILED(D3DXCreateMesh(dmesh->ntris, dmesh->nverts, D3DXMESH_MANAGED, vertexElements, d3d9device, &m_dxDebugMesh)))
    {
        MessageBox(0, "Failed to create dx mesh!", 0, 0);
        return false;
    }

    struct VERTEX
    {
        D3DXVECTOR3 p;
        D3DXVECTOR3 n;
        D3DXVECTOR2 t;
    };
    WORD* pInd;
    VERTEX* pVert;

    m_dxDebugMesh->LockVertexBuffer(0, (LPVOID*)&pVert);
    m_dxDebugMesh->LockIndexBuffer(0, (LPVOID*)&pInd);

    const D3DXVECTOR3* verts = (const D3DXVECTOR3*)dmesh->verts;
    for(int i = 0; i < dmesh->nverts; ++i)
    {
        pVert[i].p = verts[i];
        pVert[i].n = D3DXVECTOR3(0.f,0.0f,0.0f); // temp
        pVert[i].t = D3DXVECTOR2(0.0f,0.0f); // temp
    }

    // does not work like this!
    /*const unsigned char* tris = dmesh->tris;
    for(int i = 0; i < dmesh->ntris * 3; ++i)
    {
        pInd[i] = tris[i];
    }*/

    // this is piece of code for indices found on Irrlicht forum
    int cnt = 0;
    int tri_offset = 0;
    int old_nverts = 0;
    for (int p = 0; p < dmesh->nmeshes; ++p)
    {
        unsigned int* m = &(dmesh->meshes[p * 4]);
        const unsigned short bverts = m[0]; // `b' means "beginning of"!!
        const unsigned short nverts = m[1];
        const unsigned short btris  = m[2];
        const unsigned short ntris  = m[3];
        const unsigned char* tris   = &(dmesh->tris[btris * 4]);

        tri_offset += old_nverts;

        for(int n = 0; n < ntris; ++n)
        {
            for(int k = 0; k < 3; ++k)
            {
                int tri = tris[n * 4 + k] + tri_offset;
                pInd[cnt] = tri;
                cnt++;
            }
        }
        old_nverts = nverts;
    }

    m_dxDebugMesh->UnlockVertexBuffer();
    m_dxDebugMesh->UnlockIndexBuffer();

    return true;
}

void NavMesher::togglePolyActiveState(float* p)
{
    dtNavMesh* nav = m_navMesh;
    dtNavMeshQuery* navquery = m_navQuery;
    if (nav && navquery)
    {
        dtQueryFilter filter;
        float ext[3] = { 2.0f, 4.0f, 2.0f };
        float tgt[3];
        dtPolyRef ref;
        navquery->findNearestPoly(p, ext, &filter, &ref, tgt);
        if (ref)
        {
            unsigned short flags = 0;
            if (dtStatusSucceed(nav->getPolyFlags(ref, &flags)))
            {
                flags ^= SAMPLE_POLYFLAGS_DISABLED;
                nav->setPolyFlags(ref, flags);
            }
        }
    }
}

bool NavMesher::findPath(const D3DXVECTOR3& from, const D3DXVECTOR3& to, NavPath* path)
{
    if(nullptr == m_navQuery)
    {
        return false;
    }
    if(nullptr == m_navMesh)
    {
        return  false;
    }

    dtQueryFilter queryFilter;
    dtPolyRef startRef;
    dtPolyRef endRef;

    dtPolyRef returnedPath[MAX_PATH_NODES];
    float extents[3];
    extents[0] = 2.f;
    extents[1] = 4.f;
    extents[2] = 2.f;


    m_navQuery->findNearestPoly(from, extents, &queryFilter, &startRef, 0);
    if (startRef == 0)
    {
        return false;

    }
    m_navQuery->findNearestPoly(to, extents, &queryFilter, &endRef, 0);
    if (endRef == 0)
    {
        return false;

    }
    dtStatus findStatus = DT_FAILURE;
    int pathCount;

    findStatus = m_navQuery->findPath(startRef, endRef, from, to, &queryFilter, returnedPath, &pathCount, MAX_PATH_NODES);
    if (dtStatusFailed(findStatus))
    {
        return false;
    }

    if(pathCount > 0)
    {
        int numNodes = 0;
        findStatus = m_navQuery->findStraightPath(from, to, returnedPath, pathCount, path->pos, 0, 0, &numNodes, MAX_PATH_NODES);
        if(dtStatusFailed(findStatus))
        {
            return false;
        }
        path->count = numNodes;
    }
    return true;
}