diff --git a/Detour/Include/DetourNavMesh.h b/Detour/Include/DetourNavMesh.h index c862df1..0970d7e 100644 --- a/Detour/Include/DetourNavMesh.h +++ b/Detour/Include/DetourNavMesh.h @@ -28,6 +28,8 @@ static const int DT_VERTS_PER_POLYGON = 6; static const int DT_NAVMESH_MAGIC = 'DNAV'; static const int DT_NAVMESH_VERSION = 2; +static const unsigned char DT_POLY_OFFMESH_LINK = 1; + // Structure describing the navigation polygon data. struct dtPoly { @@ -36,7 +38,7 @@ struct dtPoly unsigned short links; // Base index to header 'links' array. unsigned char nlinks; // Number of links for unsigned char nv; // Number of vertices. - unsigned char flags; // Flags (not used). + unsigned char flags; // Flags. }; // Stucture describing polygon detail triangles. @@ -64,6 +66,13 @@ struct dtBVNode int i; // Index to item or if negative, escape index. }; +struct dtOffMeshLink +{ + dtPolyRef ref[2]; // Endpoint polys. + unsigned short p; // Poly Id + unsigned char side; // TODO +}; + struct dtMeshHeader { int magic; // Magic number, used to identify the data. @@ -76,6 +85,7 @@ struct dtMeshHeader int ndverts; // Number of detail vertices. int ndtris; // Number of detail triangles. int nbvtree; // Number of BVtree nodes. + int nomlinks; // Number of Off-Mesh links. float bmin[3], bmax[3]; // Bounding box of the tile. float bvquant; // BVtree quantization factor (world to bvnode coords) dtPoly* polys; // Pointer to the polygons (will be updated when tile is added). @@ -85,6 +95,7 @@ struct dtMeshHeader float* dverts; // Pointer to detail vertices (will be updated when tile added). unsigned char* dtris; // Pointer to detail triangles (will be updated when tile added). dtBVNode* bvtree; // Pointer to BVtree nodes (will be updated when tile added). + dtOffMeshLink* omlinks; // Pointer to Off-Mesh links. (will be updated when tile added). }; struct dtMeshTile diff --git a/Detour/Include/DetourNavMeshBuilder.h b/Detour/Include/DetourNavMeshBuilder.h index 921026b..d6e2517 100644 --- a/Detour/Include/DetourNavMeshBuilder.h +++ b/Detour/Include/DetourNavMeshBuilder.h @@ -23,6 +23,7 @@ bool dtCreateNavMeshData(const unsigned short* verts, const int nverts, const unsigned short* polys, const int npolys, const int nvp, const unsigned short* dmeshes, const float* dverts, const int ndverts, const unsigned char* dtris, const int ndtris, + const float* omverts, const int nomlinks, const float* bmin, const float* bmax, float cs, float ch, int tileSize, int walkableClimb, unsigned char** outData, int* outDataSize); diff --git a/Detour/Source/DetourNavMesh.cpp b/Detour/Source/DetourNavMesh.cpp index 217d0ae..f3573a0 100644 --- a/Detour/Source/DetourNavMesh.cpp +++ b/Detour/Source/DetourNavMesh.cpp @@ -354,28 +354,113 @@ void dtNavMesh::buildIntLinks(dtMeshTile* tile) dtPolyRef base = getTileId(tile); dtLink* pool = h->links; int nlinks = 0; + + unsigned int salt, it, ip, tileIdx; + dtDecodePolyId(base, salt, tileIdx, ip); + + // Find Off-mesh link end points. + for (int i = 0; i < h->nomlinks; ++i) + { + dtOffMeshLink* link = &h->omlinks[i]; + dtPoly* poly = &h->polys[link->p]; + + link->ref[0] = 0; + link->ref[1] = 0; + + const float ext[3] = { 0.1f, 0.3f, 0.1f }; //m_portalHeight }; // TODO: save agent size with mesh. + for (int j = 0; j < 2; ++j) + { + const float* v = &h->verts[poly->v[j]*3]; + dtPolyRef ref = findNearestPoly(v, ext); + // Make sure the location is on current mesh. + // TODO: Handle cross tile links. + dtDecodePolyId(ref, salt, it, ip); + if (it != tileIdx) + continue; + + link->ref[j] = ref; + } + } + for (int i = 0; i < h->npolys; ++i) { dtPoly* poly = &h->polys[i]; poly->links = nlinks; poly->nlinks = 0; - for (int j = 0; j < poly->nv; ++j) + + if (poly->flags & DT_POLY_OFFMESH_LINK) { - // Skip hard and non-internal edges. - if (poly->n[j] == 0 || (poly->n[j] & EXT_LINK)) continue; - - if (nlinks < h->maxlinks) + // Find Off-Mesh link and fill in information. + dtOffMeshLink* omlink = 0; + for (int j = 0; j < h->nomlinks; ++j) { - dtLink* link = &pool[nlinks++]; - link->ref = base | (unsigned int)(poly->n[j]-1); - link->p = (unsigned short)i; - link->e = (unsigned char)j; - link->side = 0xff; - link->bmin = link->bmax = 0; - poly->nlinks++; + if ((int)h->omlinks[j].p == i) + { + omlink = &h->omlinks[j]; + break; + } + } + if (!omlink) + continue; + for (int j = 0; j < 2; ++j) + { + if (nlinks < h->maxlinks) + { + dtLink* link = &pool[nlinks++]; + link->ref = omlink->ref[j]; + link->p = (unsigned short)i; + link->e = (unsigned char)j; + link->side = 0xff; + link->bmin = link->bmax = 0; + poly->nlinks++; + } + } + } + else + { + // Polygon edges. + for (int j = 0; j < poly->nv; ++j) + { + // Skip hard and non-internal edges. + if (poly->n[j] == 0 || (poly->n[j] & EXT_LINK)) continue; + + if (nlinks < h->maxlinks) + { + dtLink* link = &pool[nlinks++]; + link->ref = base | (unsigned int)(poly->n[j]-1); + link->p = (unsigned short)i; + link->e = (unsigned char)j; + link->side = 0xff; + link->bmin = link->bmax = 0; + poly->nlinks++; + } + } + + // Off-Mesh link targets. + dtPolyRef curRef = base | (unsigned int)i; + for (int j = 0; j < h->nomlinks; ++j) + { + const dtOffMeshLink* omlink = &h->omlinks[j]; + for (int k = 0; k < 2; ++k) + { + if (omlink->ref[k] == curRef) + { + if (nlinks < h->maxlinks) + { + dtLink* link = &pool[nlinks++]; + link->ref = base | (dtPolyRef)omlink->p; + link->p = (unsigned short)i; + link->e = 0; + link->side = 0xff; + link->bmin = link->bmax = 0; + poly->nlinks++; + } + } + } } } } + h->nlinks = nlinks; } @@ -423,6 +508,7 @@ bool dtNavMesh::addTileAt(int x, int y, unsigned char* data, int dataSize, bool const int detailVertsSize = align4(sizeof(float)*3*header->ndverts); const int detailTrisSize = align4(sizeof(unsigned char)*4*header->ndtris); const int bvtreeSize = header->nbvtree ? align4(sizeof(dtBVNode)*header->npolys*2) : 0; + const int offMeshLinksSize = align4(sizeof(dtOffMeshLink)*header->nomlinks); unsigned char* d = data + headerSize; header->verts = (float*)d; d += vertsSize; @@ -431,6 +517,7 @@ bool dtNavMesh::addTileAt(int x, int y, unsigned char* data, int dataSize, bool header->dmeshes = (dtPolyDetail*)d; d += detailMeshesSize; header->dverts = (float*)d; d += detailVertsSize; header->dtris = (unsigned char*)d; d += detailTrisSize; + header->omlinks = (dtOffMeshLink*)d; d += offMeshLinksSize; header->bvtree = header->nbvtree ? (dtBVNode*)d : 0; d += bvtreeSize; // Init tile. @@ -1224,43 +1311,69 @@ bool dtNavMesh::getPortalPoints(dtPolyRef from, dtPolyRef to, float* left, float for (int i = 0; i < fromPoly->nlinks; ++i) { const dtLink* link = &fromHeader->links[fromPoly->links+i]; - if (link->ref == to) + if (link->ref != to) + continue; + + if (fromPoly->flags & DT_POLY_OFFMESH_LINK) { - // Find portal vertices. - const int v0 = fromPoly->v[link->e]; - const int v1 = fromPoly->v[(link->e+1) % fromPoly->nv]; - vcopy(left, &fromHeader->verts[v0*3]); - vcopy(right, &fromHeader->verts[v1*3]); - // If the link is at tile boundary, clamp the vertices to - // the link width. - if (link->side == 0 || link->side == 2) - { - // Unpack portal limits. - const float smin = min(left[2],right[2]); - const float smax = max(left[2],right[2]); - const float s = (smax-smin) / 255.0f; - const float lmin = smin + link->bmin*s; - const float lmax = smin + link->bmax*s; - left[2] = max(left[2],lmin); - left[2] = min(left[2],lmax); - right[2] = max(right[2],lmin); - right[2] = min(right[2],lmax); - } - else if (link->side == 1 || link->side == 3) - { - // Unpack portal limits. - const float smin = min(left[0],right[0]); - const float smax = max(left[0],right[0]); - const float s = (smax-smin) / 255.0f; - const float lmin = smin + link->bmin*s; - const float lmax = smin + link->bmax*s; - left[0] = max(left[0],lmin); - left[0] = min(left[0],lmax); - right[0] = max(right[0],lmin); - right[0] = min(right[0],lmax); - } +// const int v = fromPoly->v[link->e]; + const int v = fromHeader->links[fromPoly->links+0].ref == to ? 0 : 1; + vcopy(left, &fromHeader->verts[fromPoly->v[v]*3]); + vcopy(right, &fromHeader->verts[fromPoly->v[v]*3]); return true; } + else + { + dtDecodePolyId(to, salt, it, ip); + if (it >= (unsigned int)m_maxTiles) return false; + if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return false; + if (ip >= (unsigned int)m_tiles[it].header->npolys) return false; + const dtMeshHeader* toHeader = m_tiles[it].header; + const dtPoly* toPoly = &toHeader->polys[ip]; + + if (toPoly->flags & DT_POLY_OFFMESH_LINK) + { + const int v = toHeader->links[toPoly->links+0].ref == from ? 0 : 1; + vcopy(left, &toHeader->verts[toPoly->v[v]*3]); + vcopy(right, &toHeader->verts[toPoly->v[v]*3]); + return true; + } + } + + // Find portal vertices. + const int v0 = fromPoly->v[link->e]; + const int v1 = fromPoly->v[(link->e+1) % fromPoly->nv]; + vcopy(left, &fromHeader->verts[v0*3]); + vcopy(right, &fromHeader->verts[v1*3]); + // If the link is at tile boundary, clamp the vertices to + // the link width. + if (link->side == 0 || link->side == 2) + { + // Unpack portal limits. + const float smin = min(left[2],right[2]); + const float smax = max(left[2],right[2]); + const float s = (smax-smin) / 255.0f; + const float lmin = smin + link->bmin*s; + const float lmax = smin + link->bmax*s; + left[2] = max(left[2],lmin); + left[2] = min(left[2],lmax); + right[2] = max(right[2],lmin); + right[2] = min(right[2],lmax); + } + else if (link->side == 1 || link->side == 3) + { + // Unpack portal limits. + const float smin = min(left[0],right[0]); + const float smax = max(left[0],right[0]); + const float s = (smax-smin) / 255.0f; + const float lmin = smin + link->bmin*s; + const float lmax = smin + link->bmax*s; + left[0] = max(left[0],lmin); + left[0] = min(left[0],lmax); + right[0] = max(right[0],lmin); + right[0] = min(right[0],lmax); + } + return true; } return false; } diff --git a/Detour/Source/DetourNavMeshBuilder.cpp b/Detour/Source/DetourNavMeshBuilder.cpp index 58288c7..7999baa 100644 --- a/Detour/Source/DetourNavMeshBuilder.cpp +++ b/Detour/Source/DetourNavMeshBuilder.cpp @@ -209,11 +209,139 @@ static int createBVTree(const unsigned short* verts, const int nverts, return curNode; } +/* +static int queryPolygons(dtMeshHeader* header, + const float* qmin, const float* qmax, + unsigned short* polys, const int maxPolys) +{ + const dtBVNode* node = &header->bvtree[0]; + const dtBVNode* end = &header->bvtree[header->nbvtree]; + + // Calculate quantized box + unsigned short bmin[3], bmax[3]; + // Clamp query box to world box. + float minx = clamp(qmin[0], header->bmin[0], header->bmax[0]) - header->bmin[0]; + float miny = clamp(qmin[1], header->bmin[1], header->bmax[1]) - header->bmin[1]; + float minz = clamp(qmin[2], header->bmin[2], header->bmax[2]) - header->bmin[2]; + float maxx = clamp(qmax[0], header->bmin[0], header->bmax[0]) - header->bmin[0]; + float maxy = clamp(qmax[1], header->bmin[1], header->bmax[1]) - header->bmin[1]; + float maxz = clamp(qmax[2], header->bmin[2], header->bmax[2]) - header->bmin[2]; + // Quantize + bmin[0] = (unsigned short)(header->bvquant * minx) & 0xfffe; + bmin[1] = (unsigned short)(header->bvquant * miny) & 0xfffe; + bmin[2] = (unsigned short)(header->bvquant * minz) & 0xfffe; + bmax[0] = (unsigned short)(header->bvquant * maxx + 1) | 1; + bmax[1] = (unsigned short)(header->bvquant * maxy + 1) | 1; + bmax[2] = (unsigned short)(header->bvquant * maxz + 1) | 1; + + // Traverse tree + dtPolyRef base = getTileId(tile); + int n = 0; + while (node < end) + { + bool overlap = checkOverlapBox(bmin, bmax, node->bmin, node->bmax); + bool isLeafNode = node->i >= 0; + + if (isLeafNode && overlap) + { + if (n < maxPolys) + polys[n++] = base | (dtPolyRef)node->i; + } + + if (overlap || isLeafNode) + node++; + else + { + const int escapeIndex = -node->i; + node += escapeIndex; + } + } + + return n; +} +dtMeshHeader* header +{ + bool dtNavMesh::closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest) const + { + unsigned int salt, it, ip; + dtDecodePolyId(ref, salt, it, ip); + if (it >= (unsigned int)m_maxTiles) return false; + if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return false; + const dtMeshHeader* header = m_tiles[it].header; + + if (ip >= (unsigned int)header->npolys) return false; + const dtPoly* poly = &header->polys[ip]; + + float closestDistSqr = FLT_MAX; + const dtPolyDetail* pd = &header->dmeshes[ip]; + + for (int j = 0; j < pd->ntris; ++j) + { + const unsigned char* t = &header->dtris[(pd->tbase+j)*4]; + const float* v[3]; + for (int k = 0; k < 3; ++k) + { + if (t[k] < poly->nv) + v[k] = &header->verts[poly->v[t[k]]*3]; + else + v[k] = &header->dverts[(pd->vbase+(t[k]-poly->nv))*3]; + } + float pt[3]; + closestPtPointTriangle(pt, pos, v[0], v[1], v[2]); + float d = vdistSqr(pos, pt); + if (d < closestDistSqr) + { + vcopy(closest, pt); + closestDistSqr = d; + } + } + + return true; + } +} + +unsigned short findNearestPoly(dtMeshHeader* header, const float* center, const float* extents) +{ + // Get nearby polygons from proximity grid. + float bmin[3], bmax[3]; + bmin[0] = center[0] - extents[0]; + bmin[1] = center[1] - extents[1]; + bmin[2] = center[2] - extents[2]; + bmax[0] = center[0] + extents[0]; + bmax[1] = center[1] + extents[1]; + bmax[2] = center[2] + extents[2]; + unsigned short polys[128]; + int npolys = queryPolygons(header, bmin, bmax, polys, 128); + + // Find nearest polygon amongst the nearby polygons. + unsigned short nearest = 0xffff; + float nearestDistanceSqr = FLT_MAX; + for (int i = 0; i < npolys; ++i) + { + dtPolyRef ref = polys[i]; + float closest[3]; + + if (!closestPointOnPoly(ref, center, closest)) + continue; + + float d = vdistSqr(center, closest); + if (d < nearestDistanceSqr) + { + nearestDistanceSqr = d; + nearest = ref; + } + } + + return nearest; +} +*/ + bool dtCreateNavMeshData(const unsigned short* verts, const int nverts, const unsigned short* polys, const int npolys, const int nvp, const unsigned short* dmeshes, const float* dverts, const int ndverts, const unsigned char* dtris, const int ndtris, + const float* omverts, const int nomlinks, const float* bmin, const float* bmax, float cs, float ch, int tileSize, int walkableClimb, unsigned char** outData, int* outDataSize) { @@ -229,6 +357,10 @@ bool dtCreateNavMeshData(const unsigned short* verts, const int nverts, if (!dmeshes || !dverts || ! dtris) return false; + // Off-mesh links are stored as polygons, adjust values. + const int totpolys = npolys + nomlinks; + const int totverts = nverts + nomlinks*2; + // Find portal edges which are at tile borders. int nedges = 0; int nportals = 0; @@ -259,7 +391,7 @@ bool dtCreateNavMeshData(const unsigned short* verts, const int nverts, } } - const int maxLinks = nedges + nportals*2; + const int maxLinks = nedges + nportals*2 + nomlinks*4; // Find unique detail vertices. @@ -283,16 +415,19 @@ bool dtCreateNavMeshData(const unsigned short* verts, const int nverts, // Calculate data size const int headerSize = align4(sizeof(dtMeshHeader)); - const int vertsSize = align4(sizeof(float)*3*nverts); - const int polysSize = align4(sizeof(dtPoly)*npolys); + const int vertsSize = align4(sizeof(float)*3*totverts); + const int polysSize = align4(sizeof(dtPoly)*totpolys); const int linksSize = align4(sizeof(dtLink)*maxLinks); const int detailMeshesSize = align4(sizeof(dtPolyDetail)*npolys); const int detailVertsSize = align4(sizeof(float)*3*uniqueDetailVerts); const int detailTrisSize = align4(sizeof(unsigned char)*4*ndtris); const int bvtreeSize = align4(sizeof(dtBVNode)*npolys*2); + const int offMeshLinksSize = align4(sizeof(dtOffMeshLink)*nomlinks); const int dataSize = headerSize + vertsSize + polysSize + linksSize + - detailMeshesSize + detailVertsSize + detailTrisSize + bvtreeSize; + detailMeshesSize + detailVertsSize + detailTrisSize + + bvtreeSize + offMeshLinksSize; + unsigned char* data = new unsigned char[dataSize]; if (!data) return false; @@ -306,14 +441,15 @@ bool dtCreateNavMeshData(const unsigned short* verts, const int nverts, dtPolyDetail* navDMeshes = (dtPolyDetail*)d; d += detailMeshesSize; float* navDVerts = (float*)d; d += detailVertsSize; unsigned char* navDTris = (unsigned char*)d; d += detailTrisSize; + dtOffMeshLink* offMeshLinks = (dtOffMeshLink*)d; d += offMeshLinksSize; dtBVNode* navBvtree = (dtBVNode*)d; d += bvtreeSize; // Store header header->magic = DT_NAVMESH_MAGIC; header->version = DT_NAVMESH_VERSION; - header->npolys = npolys; - header->nverts = nverts; + header->npolys = totpolys; + header->nverts = totverts; header->maxlinks = maxLinks; header->bmin[0] = bmin[0]; header->bmin[1] = bmin[1]; @@ -325,8 +461,11 @@ bool dtCreateNavMeshData(const unsigned short* verts, const int nverts, header->ndverts = uniqueDetailVerts; header->ndtris = ndtris; header->bvquant = 1.0f/cs; + header->nomlinks = nomlinks; + header->nbvtree = npolys*2; // Store vertices + // Mesh vertices for (int i = 0; i < nverts; ++i) { const unsigned short* iv = &verts[i*3]; @@ -335,8 +474,17 @@ bool dtCreateNavMeshData(const unsigned short* verts, const int nverts, v[1] = bmin[1] + iv[1] * ch; v[2] = bmin[2] + iv[2] * cs; } + // Off-mesh link vertices. + for (int i = 0; i < nomlinks; ++i) + { + const float* linkv = &omverts[i*2*3]; + float* v = &navVerts[(nverts+i*2)*3]; + vcopy(&v[0], &linkv[0]); + vcopy(&v[3], &linkv[3]); + } // Store polygons + // Mesh polys const unsigned short* src = polys; for (int i = 0; i < npolys; ++i) { @@ -351,7 +499,17 @@ bool dtCreateNavMeshData(const unsigned short* verts, const int nverts, } src += nvp*2; } - + // Off-mesh link vertices. + const int omvbase = nverts; // first off-mesh link vertex. + for (int i = 0; i < nomlinks; ++i) + { + dtPoly* p = &navPolys[npolys+i]; + p->nv = 2; + p->v[0] = (unsigned short)(omvbase + i*2+0); + p->v[1] = (unsigned short)(omvbase + i*2+1); + p->flags = DT_POLY_OFFMESH_LINK; // Off-mesh link poly. + } + // Store portal edges. if (tileSize > 0) { @@ -404,8 +562,14 @@ bool dtCreateNavMeshData(const unsigned short* verts, const int nverts, // Store and create BVtree. // TODO: take detail mesh into account! use byte per bbox extent? - header->nbvtree = createBVTree(verts, nverts, polys, npolys, nvp, - cs, ch, npolys*2, navBvtree); + createBVTree(verts, nverts, polys, npolys, nvp, cs, ch, npolys*2, navBvtree); + + // Store Off-Mesh links. + for (int i = 0; i < nomlinks; ++i) + { + dtOffMeshLink* link = &offMeshLinks[i]; + link->p = npolys + i; + } *outData = data; *outDataSize = dataSize;