diff --git a/DebugUtils/Source/DetourDebugDraw.cpp b/DebugUtils/Source/DetourDebugDraw.cpp index adc5d0cf0..d9b778327 100755 --- a/DebugUtils/Source/DetourDebugDraw.cpp +++ b/DebugUtils/Source/DetourDebugDraw.cpp @@ -181,7 +181,7 @@ static void drawMeshTile(duDebugDraw* dd, const dtNavMesh& mesh, const dtNavMesh if (p->getType() != DT_POLYTYPE_OFFMESH_CONNECTION) // Skip regular polys. continue; - unsigned int col; + unsigned int col, col2; if (query && query->isInClosedList(base | (dtPolyRef)i)) col = duRGBA(255,196,0,220); else @@ -202,19 +202,16 @@ static void drawMeshTile(duDebugDraw* dd, const dtNavMesh& mesh, const dtNavMesh endSet = true; } - // End points and their on-mesh locations. - if (startSet) - { - dd->vertex(va[0],va[1],va[2], col); - dd->vertex(con->pos[0],con->pos[1],con->pos[2], col); - duAppendCircle(dd, con->pos[0],con->pos[1]+0.1f,con->pos[2], con->rad, duRGBA(0,48,64,196)); - } - if (endSet) - { - dd->vertex(vb[0],vb[1],vb[2], col); - dd->vertex(con->pos[3],con->pos[4],con->pos[5], col); - duAppendCircle(dd, con->pos[3],con->pos[4]+0.1f,con->pos[5], con->rad, duRGBA(0,48,64,196)); - } + // End points and their on-mesh locations. + dd->vertex(va[0],va[1],va[2], col); + dd->vertex(con->pos[0],con->pos[1],con->pos[2], col); + col2 = startSet ? col : duRGBA(220,32,16,196); + duAppendCircle(dd, con->pos[0],con->pos[1]+0.1f,con->pos[2], con->rad, col2); + + dd->vertex(vb[0],vb[1],vb[2], col); + dd->vertex(con->pos[3],con->pos[4],con->pos[5], col); + col2 = endSet ? col : duRGBA(220,32,16,196); + duAppendCircle(dd, con->pos[3],con->pos[4]+0.1f,con->pos[5], con->rad, col2); // End point vertices. dd->vertex(con->pos[0],con->pos[1],con->pos[2], duRGBA(0,48,64,196)); diff --git a/Detour/Include/DetourNavMesh.h b/Detour/Include/DetourNavMesh.h index e445fbdb5..d4fbe9674 100644 --- a/Detour/Include/DetourNavMesh.h +++ b/Detour/Include/DetourNavMesh.h @@ -535,7 +535,7 @@ class dtNavMesh /// Builds internal polygons links for a tile. void connectIntLinks(dtMeshTile* tile); /// Builds internal polygons links for a tile. - void connectIntOffMeshLinks(dtMeshTile* tile); + void baseOffMeshLinks(dtMeshTile* tile); /// Builds external polygon links for a tile. void connectExtLinks(dtMeshTile* tile, dtMeshTile* target, int side); diff --git a/Detour/Source/DetourNavMesh.cpp b/Detour/Source/DetourNavMesh.cpp index a77b80dd0..a4bd38c9a 100644 --- a/Detour/Source/DetourNavMesh.cpp +++ b/Detour/Source/DetourNavMesh.cpp @@ -453,15 +453,18 @@ void dtNavMesh::connectExtOffMeshLinks(dtMeshTile* tile, dtMeshTile* target, int // Connect off-mesh links. // We are interested on links which land from target tile to this tile. - const unsigned char oppositeSide = (unsigned char)dtOppositeTile(side); + const unsigned char oppositeSide = (side == -1) ? 0xff : (unsigned char)dtOppositeTile(side); for (int i = 0; i < target->header->offMeshConCount; ++i) { dtOffMeshConnection* targetCon = &target->offMeshCons[i]; if (targetCon->side != oppositeSide) continue; - + dtPoly* targetPoly = &target->polys[targetCon->poly]; + // Skip off-mesh connections which start location could not be connected at all. + if (targetPoly->firstLink == DT_NULL_LINK) + continue; const float ext[3] = { targetCon->rad, target->header->walkableClimb, targetCon->rad }; @@ -469,7 +472,8 @@ void dtNavMesh::connectExtOffMeshLinks(dtMeshTile* tile, dtMeshTile* target, int const float* p = &targetCon->pos[3]; float nearestPt[3]; dtPolyRef ref = findNearestPolyInTile(tile, p, ext, nearestPt); - if (!ref) continue; + if (!ref) + continue; // findNearestPoly may return too optimistic results, further check to make sure. if (dtSqr(nearestPt[0]-p[0])+dtSqr(nearestPt[2]-p[2]) > dtSqr(targetCon->rad)) continue; @@ -502,7 +506,7 @@ void dtNavMesh::connectExtOffMeshLinks(dtMeshTile* tile, dtMeshTile* target, int dtLink* link = &tile->links[tidx]; link->ref = getPolyRefBase(target) | (dtPolyRef)(targetCon->poly); link->edge = 0xff; - link->side = (unsigned char)side; + link->side = (unsigned char)(side == -1 ? 0xff : side); link->bmin = link->bmax = 0; // Add to linked list. link->next = landPoly->firstLink; @@ -550,13 +554,13 @@ void dtNavMesh::connectIntLinks(dtMeshTile* tile) } } -void dtNavMesh::connectIntOffMeshLinks(dtMeshTile* tile) +void dtNavMesh::baseOffMeshLinks(dtMeshTile* tile) { if (!tile) return; dtPolyRef base = getPolyRefBase(tile); - // Find Off-mesh connection end points. + // Base off-mesh connection start points. for (int i = 0; i < tile->header->offMeshConCount; ++i) { dtOffMeshConnection* con = &tile->offMeshCons[i]; @@ -564,60 +568,46 @@ void dtNavMesh::connectIntOffMeshLinks(dtMeshTile* tile) const float ext[3] = { con->rad, tile->header->walkableClimb, con->rad }; - for (int j = 0; j < 2; ++j) - { - unsigned char side = j == 0 ? 0xff : con->side; + // Find polygon to connect to. + const float* p = &con->pos[0]; // First vertex + float nearestPt[3]; + dtPolyRef ref = findNearestPolyInTile(tile, p, ext, nearestPt); + if (!ref) continue; + // findNearestPoly may return too optimistic results, further check to make sure. + if (dtSqr(nearestPt[0]-p[0])+dtSqr(nearestPt[2]-p[2]) > dtSqr(con->rad)) + continue; + // Make sure the location is on current mesh. + float* v = &tile->verts[poly->verts[0]*3]; + dtVcopy(v, nearestPt); - if (side == 0xff) - { - // Find polygon to connect to. - const float* p = &con->pos[j*3]; - float nearestPt[3]; - dtPolyRef ref = findNearestPolyInTile(tile, p, ext, nearestPt); - if (!ref) continue; - // findNearestPoly may return too optimistic results, further check to make sure. - if (dtSqr(nearestPt[0]-p[0])+dtSqr(nearestPt[2]-p[2]) > dtSqr(con->rad)) - continue; - // Make sure the location is on current mesh. - float* v = &tile->verts[poly->verts[j]*3]; - dtVcopy(v, nearestPt); - - // Link off-mesh connection to target poly. - unsigned int idx = allocLink(tile); - if (idx != DT_NULL_LINK) - { - dtLink* link = &tile->links[idx]; - link->ref = ref; - link->edge = (unsigned char)j; - link->side = 0xff; - link->bmin = link->bmax = 0; - // Add to linked list. - link->next = poly->firstLink; - poly->firstLink = idx; - } + // Link off-mesh connection to target poly. + unsigned int idx = allocLink(tile); + if (idx != DT_NULL_LINK) + { + dtLink* link = &tile->links[idx]; + link->ref = ref; + link->edge = (unsigned char)0; + link->side = 0xff; + link->bmin = link->bmax = 0; + // Add to linked list. + link->next = poly->firstLink; + poly->firstLink = idx; + } - // Start end-point is always connect back to off-mesh connection, - // Destination end-point only if it is bidirectional link. - if (j == 0 || (j == 1 && (con->flags & DT_OFFMESH_CON_BIDIR))) - { - // Link target poly to off-mesh connection. - unsigned int tidx = allocLink(tile); - if (tidx != DT_NULL_LINK) - { - const unsigned short landPolyIdx = (unsigned short)decodePolyIdPoly(ref); - dtPoly* landPoly = &tile->polys[landPolyIdx]; - dtLink* link = &tile->links[tidx]; - link->ref = base | (dtPolyRef)(con->poly); - link->edge = 0xff; - link->side = 0xff; - link->bmin = link->bmax = 0; - // Add to linked list. - link->next = landPoly->firstLink; - landPoly->firstLink = tidx; - } - } - - } + // Start end-point is always connect back to off-mesh connection. + unsigned int tidx = allocLink(tile); + if (tidx != DT_NULL_LINK) + { + const unsigned short landPolyIdx = (unsigned short)decodePolyIdPoly(ref); + dtPoly* landPoly = &tile->polys[landPolyIdx]; + dtLink* link = &tile->links[tidx]; + link->ref = base | (dtPolyRef)(con->poly); + link->edge = 0xff; + link->side = 0xff; + link->bmin = link->bmax = 0; + // Add to linked list. + link->next = landPoly->firstLink; + landPoly->firstLink = tidx; } } } @@ -880,7 +870,7 @@ dtStatus dtNavMesh::addTile(unsigned char* data, int dataSize, int flags, tile->flags = flags; connectIntLinks(tile); - connectIntOffMeshLinks(tile); + baseOffMeshLinks(tile); // Create connections with neighbour tiles. static const int MAX_NEIS = 32; @@ -891,9 +881,11 @@ dtStatus dtNavMesh::addTile(unsigned char* data, int dataSize, int flags, nneis = getTilesAt(header->x, header->y, neis, MAX_NEIS); for (int j = 0; j < nneis; ++j) { - if (neis[j] == tile) continue; - connectExtLinks(tile, neis[j], -1); - connectExtLinks(neis[j], tile, -1); + if (neis[j] != tile) + { + connectExtLinks(tile, neis[j], -1); + connectExtLinks(neis[j], tile, -1); + } connectExtOffMeshLinks(tile, neis[j], -1); connectExtOffMeshLinks(neis[j], tile, -1); } diff --git a/Detour/Source/DetourNavMeshBuilder.cpp b/Detour/Source/DetourNavMeshBuilder.cpp index d5d5bd840..9d8471b96 100644 --- a/Detour/Source/DetourNavMeshBuilder.cpp +++ b/Detour/Source/DetourNavMeshBuilder.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include "DetourNavMesh.h" #include "DetourCommon.h" #include "DetourNavMeshBuilder.h" @@ -237,6 +238,7 @@ static unsigned char classifyOffMeshPoint(const float* pt, const float* bmin, co case ZM: return 6; case XP|ZM: return 7; }; + return 0xff; } @@ -274,10 +276,50 @@ bool dtCreateNavMeshData(dtNavMeshCreateParams* params, unsigned char** outData, if (!offMeshConClass) return false; + // Find tight heigh bounds, used for culling out off-mesh start locations. + float hmin = FLT_MAX; + float hmax = -FLT_MAX; + + if (params->detailVerts && params->detailVertsCount) + { + for (int i = 0; i < params->detailVertsCount; ++i) + { + const float h = params->detailVerts[i*3+1]; + hmin = dtMin(hmin,h); + hmax = dtMax(hmax,h); + } + } + else + { + for (int i = 0; i < params->vertCount; ++i) + { + const unsigned short* iv = ¶ms->verts[i*3]; + const float h = params->bmin[1] + iv[1] * params->ch; + hmin = dtMin(hmin,h); + hmax = dtMax(hmax,h); + } + } + hmin -= params->walkableClimb; + hmax += params->walkableClimb; + float bmin[3], bmax[3]; + dtVcopy(bmin, params->bmin); + dtVcopy(bmax, params->bmax); + bmin[1] = hmin; + bmax[1] = hmax; + for (int i = 0; i < params->offMeshConCount; ++i) { - offMeshConClass[i*2+0] = classifyOffMeshPoint(¶ms->offMeshConVerts[(i*2+0)*3], params->bmin, params->bmax); - offMeshConClass[i*2+1] = classifyOffMeshPoint(¶ms->offMeshConVerts[(i*2+1)*3], params->bmin, params->bmax); + const float* p0 = ¶ms->offMeshConVerts[(i*2+0)*3]; + const float* p1 = ¶ms->offMeshConVerts[(i*2+1)*3]; + offMeshConClass[i*2+0] = classifyOffMeshPoint(p0, bmin, bmax); + offMeshConClass[i*2+1] = classifyOffMeshPoint(p1, bmin, bmax); + + // Zero out off-mesh start positions which are not even potentially touching the mesh. + if (offMeshConClass[i*2+0] == 0xff) + { + if (p0[1] < bmin[1] || p0[1] > bmax[1]) + offMeshConClass[i*2+0] = 0; + } // Cound how many links should be allocated for off-mesh connections. if (offMeshConClass[i*2+0] == 0xff) diff --git a/Detour/Source/DetourNavMeshQuery.cpp b/Detour/Source/DetourNavMeshQuery.cpp index 87b6169ed..0eb001146 100644 --- a/Detour/Source/DetourNavMeshQuery.cpp +++ b/Detour/Source/DetourNavMeshQuery.cpp @@ -519,6 +519,18 @@ dtStatus dtNavMeshQuery::closestPointOnPoly(dtPolyRef ref, const float* pos, flo void dtNavMeshQuery::closestPointOnPolyInTile(const dtMeshTile* tile, const dtPoly* poly, const float* pos, float* closest) const { + // Off-mesh connections don't have detail polygons. + if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) + { + const float* v0 = &tile->verts[poly->verts[0]*3]; + const float* v1 = &tile->verts[poly->verts[1]*3]; + const float d0 = dtVdist(pos, v0); + const float d1 = dtVdist(pos, v1); + const float u = d0 / (d0+d1); + dtVlerp(closest, v0, v1, u); + return; + } + const unsigned int ip = (unsigned int)(poly - tile->polys); const dtPolyDetail* pd = &tile->detailMeshes[ip]; diff --git a/RecastDemo/Source/NavMeshTesterTool.cpp b/RecastDemo/Source/NavMeshTesterTool.cpp index 639f552cc..163f7ee63 100644 --- a/RecastDemo/Source/NavMeshTesterTool.cpp +++ b/RecastDemo/Source/NavMeshTesterTool.cpp @@ -1053,7 +1053,7 @@ void NavMeshTesterTool::handleRender() if (m_straightPathFlags[i] & DT_STRAIGHTPATH_OFFMESH_CONNECTION) col = offMeshCol; else - col = pathCol; + col = spathCol; dd.vertex(m_straightPath[i*3], m_straightPath[i*3+1]+0.4f, m_straightPath[i*3+2], col); dd.vertex(m_straightPath[(i+1)*3], m_straightPath[(i+1)*3+1]+0.4f, m_straightPath[(i+1)*3+2], col); @@ -1070,7 +1070,7 @@ void NavMeshTesterTool::handleRender() else if (m_straightPathFlags[i] & DT_STRAIGHTPATH_OFFMESH_CONNECTION) col = offMeshCol; else - col = pathCol; + col = spathCol; dd.vertex(m_straightPath[i*3], m_straightPath[i*3+1]+0.4f, m_straightPath[i*3+2], spathCol); } dd.end();