Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #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(¶ms, 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(¶ms, &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;
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement