diff options
Diffstat (limited to 'extern/recastnavigation/Detour/Source/DetourTileNavMesh.cpp')
| -rw-r--r-- | extern/recastnavigation/Detour/Source/DetourTileNavMesh.cpp | 1428 |
1 files changed, 0 insertions, 1428 deletions
diff --git a/extern/recastnavigation/Detour/Source/DetourTileNavMesh.cpp b/extern/recastnavigation/Detour/Source/DetourTileNavMesh.cpp deleted file mode 100644 index 0813c7755cc..00000000000 --- a/extern/recastnavigation/Detour/Source/DetourTileNavMesh.cpp +++ /dev/null @@ -1,1428 +0,0 @@ -// -// Copyright (c) 2009 Mikko Mononen memon@inside.org -// -// This software is provided 'as-is', without any express or implied -// warranty. In no event will the authors be held liable for any damages -// arising from the use of this software. -// Permission is granted to anyone to use this software for any purpose, -// including commercial applications, and to alter it and redistribute it -// freely, subject to the following restrictions: -// 1. The origin of this software must not be misrepresented; you must not -// claim that you wrote the original software. If you use this software -// in a product, an acknowledgment in the product documentation would be -// appreciated but is not required. -// 2. Altered source versions must be plainly marked as such, and must not be -// misrepresented as being the original software. -// 3. This notice may not be removed or altered from any source distribution. -// - -#include <math.h> -#include <float.h> -#include <string.h> -#include <stdio.h> -#include "DetourTileNavMesh.h" -#include "DetourNode.h" -#include "DetourCommon.h" - - -inline int opposite(int side) { return (side+2) & 0x3; } - -inline bool overlapBoxes(const float* amin, const float* amax, - const float* bmin, const float* bmax) -{ - bool overlap = true; - overlap = (amin[0] > bmax[0] || amax[0] < bmin[0]) ? false : overlap; - overlap = (amin[1] > bmax[1] || amax[1] < bmin[1]) ? false : overlap; - overlap = (amin[2] > bmax[2] || amax[2] < bmin[2]) ? false : overlap; - return overlap; -} - -inline bool overlapRects(const float* amin, const float* amax, - const float* bmin, const float* bmax) -{ - bool overlap = true; - overlap = (amin[0] > bmax[0] || amax[0] < bmin[0]) ? false : overlap; - overlap = (amin[1] > bmax[1] || amax[1] < bmin[1]) ? false : overlap; - return overlap; -} - -static void calcRect(const float* va, const float* vb, - float* bmin, float* bmax, - int side, float padx, float pady) -{ - if ((side&1) == 0) - { - bmin[0] = min(va[2],vb[2]) + padx; - bmin[1] = min(va[1],vb[1]) - pady; - bmax[0] = max(va[2],vb[2]) - padx; - bmax[1] = max(va[1],vb[1]) + pady; - } - else - { - bmin[0] = min(va[0],vb[0]) + padx; - bmin[1] = min(va[1],vb[1]) - pady; - bmax[0] = max(va[0],vb[0]) - padx; - bmax[1] = max(va[1],vb[1]) + pady; - } -} - -inline int computeTileHash(int x, int y) -{ - const unsigned int h1 = 0x8da6b343; // Large multiplicative constants; - const unsigned int h2 = 0xd8163841; // here arbitrarily chosen primes - unsigned int n = h1 * x + h2 * y; - return (int)(n & (DT_TILE_LOOKUP_SIZE-1)); -} - -////////////////////////////////////////////////////////////////////////////////////////// -dtTiledNavMesh::dtTiledNavMesh() : - m_tileSize(0), - m_portalHeight(0), - m_nextFree(0), - m_tmpLinks(0), - m_ntmpLinks(0), - m_nodePool(0), - m_openList(0) -{ -} - -dtTiledNavMesh::~dtTiledNavMesh() -{ - for (int i = 0; i < DT_MAX_TILES; ++i) - { - if (m_tiles[i].data && m_tiles[i].dataSize < 0) - { - delete [] m_tiles[i].data; - m_tiles[i].data = 0; - m_tiles[i].dataSize = 0; - } - } - delete [] m_tmpLinks; - delete m_nodePool; - delete m_openList; -} - -bool dtTiledNavMesh::init(const float* orig, float tileSize, float portalHeight) -{ - vcopy(m_orig, orig); - m_tileSize = tileSize; - m_portalHeight = portalHeight; - - // Init tiles - memset(m_tiles, 0, sizeof(dtTile)*DT_MAX_TILES); - memset(m_posLookup, 0, sizeof(dtTile*)*DT_TILE_LOOKUP_SIZE); - m_nextFree = 0; - for (int i = DT_MAX_TILES-1; i >= 0; --i) - { - m_tiles[i].next = m_nextFree; - m_nextFree = &m_tiles[i]; - } - - if (!m_nodePool) - { - m_nodePool = new dtNodePool(2048, 256); - if (!m_nodePool) - return false; - } - - if (!m_openList) - { - m_openList = new dtNodeQueue(2048); - if (!m_openList) - return false; - } - - return true; -} - -////////////////////////////////////////////////////////////////////////////////////////// -int dtTiledNavMesh::findConnectingPolys(const float* va, const float* vb, - dtTile* tile, int side, - dtTilePolyRef* con, float* conarea, int maxcon) -{ - if (!tile) return 0; - dtTileHeader* h = tile->header; - - float amin[2], amax[2]; - calcRect(va,vb, amin,amax, side, 0.01f, m_portalHeight); - - // Remove links pointing to 'side' and compact the links array. - float bmin[2], bmax[2]; - unsigned short m = 0x8000 | (unsigned short)side; - int n = 0; - - dtTilePolyRef base = getTileId(tile); - - for (int i = 0; i < h->npolys; ++i) - { - dtTilePoly* poly = &h->polys[i]; - for (int j = 0; j < poly->nv; ++j) - { - // Skip edges which do not point to the right side. - if (poly->n[j] != m) continue; - // Check if the segments touch. - const float* vc = &h->verts[poly->v[j]*3]; - const float* vd = &h->verts[poly->v[(j+1) % (int)poly->nv]*3]; - calcRect(vc,vd, bmin,bmax, side, 0.01f, m_portalHeight); - if (!overlapRects(amin,amax, bmin,bmax)) continue; - // Add return value. - if (n < maxcon) - { - conarea[n*2+0] = max(amin[0], bmin[0]); - conarea[n*2+1] = min(amax[0], bmax[0]); - con[n] = base | (unsigned int)i; - n++; - } - break; - } - } - return n; -} - -void dtTiledNavMesh::removeExtLinks(dtTile* tile, int side) -{ - if (!tile) return; - dtTileHeader* h = tile->header; - - // Remove links pointing to 'side' and compact the links array. - dtTileLink* pool = m_tmpLinks; - int nlinks = 0; - for (int i = 0; i < h->npolys; ++i) - { - dtTilePoly* poly = &h->polys[i]; - int plinks = nlinks; - int nplinks = 0; - for (int j = 0; j < poly->nlinks; ++j) - { - dtTileLink* link = &h->links[poly->links+j]; - if ((int)link->side != side) - { - if (nlinks < h->maxlinks) - { - dtTileLink* dst = &pool[nlinks++]; - memcpy(dst, link, sizeof(dtTileLink)); - nplinks++; - } - } - } - poly->links = plinks; - poly->nlinks = nplinks; - } - h->nlinks = nlinks; - if (h->nlinks) - memcpy(h->links, m_tmpLinks, sizeof(dtTileLink)*nlinks); -} - -void dtTiledNavMesh::buildExtLinks(dtTile* tile, dtTile* target, int side) -{ - if (!tile) return; - dtTileHeader* h = tile->header; - - // Remove links pointing to 'side' and compact the links array. - dtTileLink* pool = m_tmpLinks; - int nlinks = 0; - for (int i = 0; i < h->npolys; ++i) - { - dtTilePoly* poly = &h->polys[i]; - int plinks = nlinks; - int nplinks = 0; - // Copy internal and other external links. - for (int j = 0; j < poly->nlinks; ++j) - { - dtTileLink* link = &h->links[poly->links+j]; - if ((int)link->side != side) - { - if (nlinks < h->maxlinks) - { - dtTileLink* dst = &pool[nlinks++]; - memcpy(dst, link, sizeof(dtTileLink)); - nplinks++; - } - } - } - // Create new links. - unsigned short m = 0x8000 | (unsigned short)side; - for (int j = 0; j < poly->nv; ++j) - { - // Skip edges which do not point to the right side. - if (poly->n[j] != m) continue; - - // Create new links - const float* va = &h->verts[poly->v[j]*3]; - const float* vb = &h->verts[poly->v[(j+1)%(int)poly->nv]*3]; - dtTilePolyRef nei[4]; - float neia[4*2]; - int nnei = findConnectingPolys(va,vb, target, opposite(side), nei,neia,4); - for (int k = 0; k < nnei; ++k) - { - if (nlinks < h->maxlinks) - { - dtTileLink* link = &pool[nlinks++]; - link->ref = nei[k]; - link->p = (unsigned short)i; - link->e = (unsigned char)j; - link->side = (unsigned char)side; - - // Compress portal limits to a byte value. - if (side == 0 || side == 2) - { - const float lmin = min(va[2], vb[2]); - const float lmax = max(va[2], vb[2]); - link->bmin = (unsigned char)(clamp((neia[k*2+0]-lmin)/(lmax-lmin), 0.0f, 1.0f)*255.0f); - link->bmax = (unsigned char)(clamp((neia[k*2+1]-lmin)/(lmax-lmin), 0.0f, 1.0f)*255.0f); - } - else - { - const float lmin = min(va[0], vb[0]); - const float lmax = max(va[0], vb[0]); - link->bmin = (unsigned char)(clamp((neia[k*2+0]-lmin)/(lmax-lmin), 0.0f, 1.0f)*255.0f); - link->bmax = (unsigned char)(clamp((neia[k*2+1]-lmin)/(lmax-lmin), 0.0f, 1.0f)*255.0f); - } - nplinks++; - } - } - } - - poly->links = plinks; - poly->nlinks = nplinks; - } - h->nlinks = nlinks; - if (h->nlinks) - memcpy(h->links, m_tmpLinks, sizeof(dtTileLink)*nlinks); -} - -void dtTiledNavMesh::buildIntLinks(dtTile* tile) -{ - if (!tile) return; - dtTileHeader* h = tile->header; - - dtTilePolyRef base = getTileId(tile); - dtTileLink* pool = h->links; - int nlinks = 0; - for (int i = 0; i < h->npolys; ++i) - { - dtTilePoly* poly = &h->polys[i]; - poly->links = nlinks; - poly->nlinks = 0; - for (int j = 0; j < poly->nv; ++j) - { - // Skip hard and non-internal edges. - if (poly->n[j] == 0 || (poly->n[j] & 0x8000)) continue; - - if (nlinks < h->maxlinks) - { - dtTileLink* 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++; - } - } - } - h->nlinks = nlinks; -} - -bool dtTiledNavMesh::addTileAt(int x, int y, unsigned char* data, int dataSize, bool ownsData) -{ - if (getTileAt(x,y)) - return false; - // Make sure there is enough space for new tile. - if (!m_nextFree) - return false; - // Make sure the data is in right format. - dtTileHeader* header = (dtTileHeader*)data; - if (header->magic != DT_TILE_NAVMESH_MAGIC) - return false; - if (header->version != DT_TILE_NAVMESH_VERSION) - return false; - - // Make sure the tmp link array is large enough. - if (header->maxlinks > m_ntmpLinks) - { - m_ntmpLinks = header->maxlinks; - delete [] m_tmpLinks; - m_tmpLinks = 0; - m_tmpLinks = new dtTileLink[m_ntmpLinks]; - } - if (!m_tmpLinks) - return false; - - // Allocate a tile. - dtTile* tile = m_nextFree; - m_nextFree = tile->next; - tile->next = 0; - - // Insert tile into the position lut. - int h = computeTileHash(x,y); - tile->next = m_posLookup[h]; - m_posLookup[h] = tile; - - // Patch header pointers. - const int headerSize = sizeof(dtTileHeader); - const int vertsSize = sizeof(float)*3*header->nverts; - const int polysSize = sizeof(dtTilePoly)*header->npolys; - const int linksSize = sizeof(dtTileLink)*(header->maxlinks); - const int detailMeshesSize = sizeof(dtTilePolyDetail)*header->ndmeshes; - const int detailVertsSize = sizeof(float)*3*header->ndverts; - const int detailTrisSize = sizeof(unsigned char)*4*header->ndtris; - - unsigned char* d = data + headerSize; - header->verts = (float*)d; d += vertsSize; - header->polys = (dtTilePoly*)d; d += polysSize; - header->links = (dtTileLink*)d; d += linksSize; - header->dmeshes = (dtTilePolyDetail*)d; d += detailMeshesSize; - header->dverts = (float*)d; d += detailVertsSize; - header->dtris = (unsigned char*)d; d += detailTrisSize; - - // Init tile. - tile->header = header; - tile->x = x; - tile->y = y; - tile->data = data; - tile->dataSize = dataSize; - tile->ownsData = ownsData; - - buildIntLinks(tile); - - // Create connections connections. - for (int i = 0; i < 4; ++i) - { - dtTile* nei = getNeighbourTileAt(x,y,i); - if (nei) - { - buildExtLinks(tile, nei, i); - buildExtLinks(nei, tile, opposite(i)); - } - } - - return true; -} - -dtTile* dtTiledNavMesh::getTileAt(int x, int y) -{ - // Find tile based on hash. - int h = computeTileHash(x,y); - dtTile* tile = m_posLookup[h]; - while (tile) - { - if (tile->x == x && tile->y == y) - return tile; - tile = tile->next; - } - return 0; -} - -dtTile* dtTiledNavMesh::getTile(int i) -{ - return &m_tiles[i]; -} - -const dtTile* dtTiledNavMesh::getTile(int i) const -{ - return &m_tiles[i]; -} - -dtTile* dtTiledNavMesh::getNeighbourTileAt(int x, int y, int side) -{ - switch (side) - { - case 0: x++; break; - case 1: y++; break; - case 2: x--; break; - case 3: y--; break; - }; - return getTileAt(x,y); -} - -bool dtTiledNavMesh::removeTileAt(int x, int y, unsigned char** data, int* dataSize) -{ - // Remove tile from hash lookup. - int h = computeTileHash(x,y); - dtTile* prev = 0; - dtTile* tile = m_posLookup[h]; - while (tile) - { - if (tile->x == x && tile->y == y) - { - if (prev) - prev->next = tile->next; - else - m_posLookup[h] = tile->next; - break; - } - prev = tile; - tile = tile->next; - } - if (!tile) - return false; - - // Remove connections to neighbour tiles. - for (int i = 0; i < 4; ++i) - { - dtTile* nei = getNeighbourTileAt(x,y,i); - if (!nei) continue; - removeExtLinks(nei, opposite(i)); - } - - - // Reset tile. - if (tile->ownsData) - { - // Owns data - delete [] tile->data; - tile->data = 0; - tile->dataSize = 0; - if (data) *data = 0; - if (dataSize) *dataSize = 0; - } - else - { - if (data) *data = tile->data; - if (dataSize) *dataSize = tile->dataSize; - } - tile->header = 0; - tile->x = tile->y = 0; - tile->salt++; - - // Add to free list. - tile->next = m_nextFree; - m_nextFree = tile; - - return true; -} - - - -bool dtTiledNavMesh::closestPointToPoly(dtTilePolyRef ref, const float* pos, float* closest) const -{ - unsigned int salt, it, ip; - dtDecodeTileId(ref, salt, it, ip); - if (it >= DT_MAX_TILES) return false; - if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return false; - const dtTileHeader* header = m_tiles[it].header; - - if (ip >= (unsigned int)header->npolys) return false; - const dtTilePoly* poly = &header->polys[ip]; - - float closestDistSqr = FLT_MAX; - const dtTilePolyDetail* 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; -} - -bool dtTiledNavMesh::getPolyHeight(dtTilePolyRef ref, const float* pos, float* height) const -{ - unsigned int salt, it, ip; - dtDecodeTileId(ref, salt, it, ip); - if (it >= DT_MAX_TILES) return false; - if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return false; - const dtTileHeader* header = m_tiles[it].header; - - if (ip >= (unsigned int)header->npolys) return false; - const dtTilePoly* poly = &header->polys[ip]; - - const dtTilePolyDetail* 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 h; - if (closestHeightPointTriangle(pos, v[0], v[1], v[2], h)) - { - if (height) - *height = h; - return true; - } - } - - return false; -} - - -dtTilePolyRef dtTiledNavMesh::findNearestPoly(const float* center, const float* extents) -{ - // Get nearby polygons from proximity grid. - dtTilePolyRef polys[128]; - int npolys = queryPolygons(center, extents, polys, 128); - - // Find nearest polygon amongst the nearby polygons. - dtTilePolyRef nearest = 0; - float nearestDistanceSqr = FLT_MAX; - for (int i = 0; i < npolys; ++i) - { - dtTilePolyRef ref = polys[i]; - float closest[3]; - if (!closestPointToPoly(ref, center, closest)) - continue; - float d = vdistSqr(center, closest); - if (d < nearestDistanceSqr) - { - nearestDistanceSqr = d; - nearest = ref; - } - } - - return nearest; -} - -dtTilePolyRef dtTiledNavMesh::getTileId(dtTile* tile) -{ - if (!tile) return 0; - const unsigned int it = tile - m_tiles; - return dtEncodeTileId(tile->salt, it, 0); -} - -int dtTiledNavMesh::queryTilePolygons(dtTile* tile, - const float* qmin, const float* qmax, - dtTilePolyRef* polys, const int maxPolys) -{ - float bmin[3], bmax[3]; - const dtTileHeader* header = tile->header; - int n = 0; - dtTilePolyRef base = getTileId(tile); - for (int i = 0; i < header->npolys; ++i) - { - // Calc polygon bounds. - dtTilePoly* p = &header->polys[i]; - const float* v = &header->verts[p->v[0]*3]; - vcopy(bmin, v); - vcopy(bmax, v); - for (int j = 1; j < p->nv; ++j) - { - v = &header->verts[p->v[j]*3]; - vmin(bmin, v); - vmax(bmax, v); - } - if (overlapBoxes(qmin,qmax, bmin,bmax)) - { - if (n < maxPolys) - polys[n++] = base | (dtTilePolyRef)i; - } - } - return n; -} - -int dtTiledNavMesh::queryPolygons(const float* center, const float* extents, - dtTilePolyRef* polys, const int maxPolys) -{ - 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]; - - // Find tiles the query touches. - const int minx = (int)floorf((bmin[0]-m_orig[0]) / m_tileSize); - const int maxx = (int)ceilf((bmax[0]-m_orig[0]) / m_tileSize); - - const int miny = (int)floorf((bmin[2]-m_orig[2]) / m_tileSize); - const int maxy = (int)ceilf((bmax[2]-m_orig[2]) / m_tileSize); - - int n = 0; - for (int y = miny; y < maxy; ++y) - { - for (int x = minx; x < maxx; ++x) - { - dtTile* tile = getTileAt(x,y); - if (!tile) continue; - n += queryTilePolygons(tile, bmin, bmax, polys+n, maxPolys-n); - if (n >= maxPolys) return n; - } - } - - return n; -} - -int dtTiledNavMesh::findPath(dtTilePolyRef startRef, dtTilePolyRef endRef, - const float* startPos, const float* endPos, - dtTilePolyRef* path, const int maxPathSize) -{ - if (!startRef || !endRef) - return 0; - - if (!maxPathSize) - return 0; - - if (!getPolyByRef(startRef) || !getPolyByRef(endRef)) - return 0; - - if (startRef == endRef) - { - path[0] = startRef; - return 1; - } - - if (!m_nodePool || !m_openList) - return 0; - - m_nodePool->clear(); - m_openList->clear(); - - static const float H_SCALE = 1.1f; // Heuristic scale. - - dtNode* startNode = m_nodePool->getNode(startRef); - startNode->pidx = 0; - startNode->cost = 0; - startNode->total = vdist(startPos, endPos) * H_SCALE; - startNode->id = startRef; - startNode->flags = DT_NODE_OPEN; - m_openList->push(startNode); - - dtNode* lastBestNode = startNode; - float lastBestNodeCost = startNode->total; - while (!m_openList->empty()) - { - dtNode* bestNode = m_openList->pop(); - - if (bestNode->id == endRef) - { - lastBestNode = bestNode; - break; - } - - // Get poly and tile. - unsigned int salt, it, ip; - dtDecodeTileId(bestNode->id, salt, it, ip); - // The API input has been cheked already, skip checking internal data. - const dtTileHeader* header = m_tiles[it].header; - const dtTilePoly* poly = &header->polys[ip]; - - for (int i = 0; i < poly->nlinks; ++i) - { - dtTilePolyRef neighbour = header->links[poly->links+i].ref; - if (neighbour) - { - // Skip parent node. - if (bestNode->pidx && m_nodePool->getNodeAtIdx(bestNode->pidx)->id == neighbour) - continue; - - dtNode* parent = bestNode; - dtNode newNode; - newNode.pidx = m_nodePool->getNodeIdx(parent); - newNode.id = neighbour; - - // Calculate cost. - float p0[3], p1[3]; - if (!parent->pidx) - vcopy(p0, startPos); - else - getEdgeMidPoint(m_nodePool->getNodeAtIdx(parent->pidx)->id, parent->id, p0); - getEdgeMidPoint(parent->id, newNode.id, p1); - newNode.cost = parent->cost + vdist(p0,p1); - // Special case for last node. - if (newNode.id == endRef) - newNode.cost += vdist(p1, endPos); - - // Heuristic - const float h = vdist(p1,endPos)*H_SCALE; - newNode.total = newNode.cost + h; - - dtNode* actualNode = m_nodePool->getNode(newNode.id); - if (!actualNode) - continue; - - if (!((actualNode->flags & DT_NODE_OPEN) && newNode.total > actualNode->total) && - !((actualNode->flags & DT_NODE_CLOSED) && newNode.total > actualNode->total)) - { - actualNode->flags &= DT_NODE_CLOSED; - actualNode->pidx = newNode.pidx; - actualNode->cost = newNode.cost; - actualNode->total = newNode.total; - - if (h < lastBestNodeCost) - { - lastBestNodeCost = h; - lastBestNode = actualNode; - } - - if (actualNode->flags & DT_NODE_OPEN) - { - m_openList->modify(actualNode); - } - else - { - actualNode->flags |= DT_NODE_OPEN; - m_openList->push(actualNode); - } - } - } - } - bestNode->flags |= DT_NODE_CLOSED; - } - - // Reverse the path. - dtNode* prev = 0; - dtNode* node = lastBestNode; - do - { - dtNode* next = m_nodePool->getNodeAtIdx(node->pidx); - node->pidx = m_nodePool->getNodeIdx(prev); - prev = node; - node = next; - } - while (node); - - // Store path - node = prev; - int n = 0; - do - { - path[n++] = node->id; - node = m_nodePool->getNodeAtIdx(node->pidx); - } - while (node && n < maxPathSize); - - return n; -} - -int dtTiledNavMesh::findStraightPath(const float* startPos, const float* endPos, - const dtTilePolyRef* path, const int pathSize, - float* straightPath, const int maxStraightPathSize) -{ - if (!maxStraightPathSize) - return 0; - - if (!path[0]) - return 0; - - int straightPathSize = 0; - - float closestStartPos[3]; - if (!closestPointToPoly(path[0], startPos, closestStartPos)) - return 0; - - // Add start point. - vcopy(&straightPath[straightPathSize*3], closestStartPos); - straightPathSize++; - if (straightPathSize >= maxStraightPathSize) - return straightPathSize; - - float closestEndPos[3]; - if (!closestPointToPoly(path[pathSize-1], endPos, closestEndPos)) - return 0; - - float portalApex[3], portalLeft[3], portalRight[3]; - - if (pathSize > 1) - { - vcopy(portalApex, closestStartPos); - vcopy(portalLeft, portalApex); - vcopy(portalRight, portalApex); - int apexIndex = 0; - int leftIndex = 0; - int rightIndex = 0; - - for (int i = 0; i < pathSize; ++i) - { - float left[3], right[3]; - if (i < pathSize-1) - { - // Next portal. - if (!getPortalPoints(path[i], path[i+1], left, right)) - { - if (!closestPointToPoly(path[i], endPos, closestEndPos)) - return 0; - vcopy(&straightPath[straightPathSize*3], closestEndPos); - straightPathSize++; - return straightPathSize; - } - } - else - { - // End of the path. - vcopy(left, closestEndPos); - vcopy(right, closestEndPos); - } - - // Right vertex. - if (vequal(portalApex, portalRight)) - { - vcopy(portalRight, right); - rightIndex = i; - } - else - { - if (triArea2D(portalApex, portalRight, right) <= 0.0f) - { - if (triArea2D(portalApex, portalLeft, right) > 0.0f) - { - vcopy(portalRight, right); - rightIndex = i; - } - else - { - vcopy(portalApex, portalLeft); - apexIndex = leftIndex; - - if (!vequal(&straightPath[(straightPathSize-1)*3], portalApex)) - { - vcopy(&straightPath[straightPathSize*3], portalApex); - straightPathSize++; - if (straightPathSize >= maxStraightPathSize) - return straightPathSize; - } - - vcopy(portalLeft, portalApex); - vcopy(portalRight, portalApex); - leftIndex = apexIndex; - rightIndex = apexIndex; - - // Restart - i = apexIndex; - - continue; - } - } - } - - // Left vertex. - if (vequal(portalApex, portalLeft)) - { - vcopy(portalLeft, left); - leftIndex = i; - } - else - { - if (triArea2D(portalApex, portalLeft, left) >= 0.0f) - { - if (triArea2D(portalApex, portalRight, left) < 0.0f) - { - vcopy(portalLeft, left); - leftIndex = i; - } - else - { - vcopy(portalApex, portalRight); - apexIndex = rightIndex; - - if (!vequal(&straightPath[(straightPathSize-1)*3], portalApex)) - { - vcopy(&straightPath[straightPathSize*3], portalApex); - straightPathSize++; - if (straightPathSize >= maxStraightPathSize) - return straightPathSize; - } - - vcopy(portalLeft, portalApex); - vcopy(portalRight, portalApex); - leftIndex = apexIndex; - rightIndex = apexIndex; - - // Restart - i = apexIndex; - - continue; - } - } - } - } - } - - // Add end point. - vcopy(&straightPath[straightPathSize*3], closestEndPos); - straightPathSize++; - - return straightPathSize; -} - -// Returns portal points between two polygons. -bool dtTiledNavMesh::getPortalPoints(dtTilePolyRef from, dtTilePolyRef to, float* left, float* right) const -{ - unsigned int salt, it, ip; - dtDecodeTileId(from, salt, it, ip); - if (it >= DT_MAX_TILES) 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 dtTileHeader* fromHeader = m_tiles[it].header; - const dtTilePoly* fromPoly = &fromHeader->polys[ip]; - - for (int i = 0; i < fromPoly->nlinks; ++i) - { - const dtTileLink* link = &fromHeader->links[fromPoly->links+i]; - if (link->ref == to) - { - // 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; -} - -// Returns edge mid point between two polygons. -bool dtTiledNavMesh::getEdgeMidPoint(dtTilePolyRef from, dtTilePolyRef to, float* mid) const -{ - float left[3], right[3]; - if (!getPortalPoints(from, to, left,right)) return false; - mid[0] = (left[0]+right[0])*0.5f; - mid[1] = (left[1]+right[1])*0.5f; - mid[2] = (left[2]+right[2])*0.5f; - return true; -} - -int dtTiledNavMesh::raycast(dtTilePolyRef centerRef, const float* startPos, const float* endPos, - float& t, dtTilePolyRef* path, const int pathSize) -{ - t = 0; - - if (!centerRef || !getPolyByRef(centerRef)) - return 0; - - dtTilePolyRef curRef = centerRef; - float verts[DT_TILE_VERTS_PER_POLYGON*3]; - int n = 0; - - while (curRef) - { - // Cast ray against current polygon. - - // The API input has been cheked already, skip checking internal data. - unsigned int salt, it, ip; - dtDecodeTileId(curRef, salt, it, ip); - const dtTileHeader* header = m_tiles[it].header; - const dtTilePoly* poly = &header->polys[ip]; - - // Collect vertices. - int nv = 0; - for (int i = 0; i < (int)poly->nv; ++i) - { - vcopy(&verts[nv*3], &header->verts[poly->v[i]*3]); - nv++; - } - if (nv < 3) - { - // Hit bad polygon, report hit. - return n; - } - - float tmin, tmax; - int segMin, segMax; - if (!intersectSegmentPoly2D(startPos, endPos, verts, nv, tmin, tmax, segMin, segMax)) - { - // Could not hit the polygon, keep the old t and report hit. - return n; - } - // Keep track of furthest t so far. - if (tmax > t) - t = tmax; - - if (n < pathSize) - path[n++] = curRef; - - // Follow neighbours. - dtTilePolyRef nextRef = 0; - for (int i = 0; i < poly->nlinks; ++i) - { - const dtTileLink* link = &header->links[poly->links+i]; - if ((int)link->e == segMax) - { - // If the link is internal, just return the ref. - if (link->side == 0xff) - { - nextRef = link->ref; - break; - } - - // If the link is at tile boundary, - const int v0 = poly->v[link->e]; - const int v1 = poly->v[(link->e+1) % poly->nv]; - const float* left = &header->verts[v0*3]; - const float* right = &header->verts[v1*3]; - - // Check that the intersection lies inside the link portal. - if (link->side == 0 || link->side == 2) - { - // Calculate link size. - 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; - // Find Z intersection. - float z = startPos[2] + (endPos[2]-startPos[2])*tmax; - if (z >= lmin && z <= lmax) - { - nextRef = link->ref; - break; - } - } - else if (link->side == 1 || link->side == 3) - { - // Calculate link size. - 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; - // Find X intersection. - float x = startPos[0] + (endPos[0]-startPos[0])*tmax; - if (x >= lmin && x <= lmax) - { - nextRef = link->ref; - break; - } - } - } - } - - if (!nextRef) - { - // No neighbour, we hit a wall. - return n; - } - - // No hit, advance to neighbour polygon. - curRef = nextRef; - } - - return n; -} - -int dtTiledNavMesh::findPolysAround(dtTilePolyRef centerRef, const float* centerPos, float radius, - dtTilePolyRef* resultRef, dtTilePolyRef* resultParent, float* resultCost, - const int maxResult) -{ - if (!centerRef) return 0; - if (!getPolyByRef(centerRef)) return 0; - if (!m_nodePool || !m_openList) return 0; - - m_nodePool->clear(); - m_openList->clear(); - - dtNode* startNode = m_nodePool->getNode(centerRef); - startNode->pidx = 0; - startNode->cost = 0; - startNode->total = 0; - startNode->id = centerRef; - startNode->flags = DT_NODE_OPEN; - m_openList->push(startNode); - - int n = 0; - if (n < maxResult) - { - if (resultRef) - resultRef[n] = startNode->id; - if (resultParent) - resultParent[n] = 0; - if (resultCost) - resultCost[n] = 0; - ++n; - } - - const float radiusSqr = sqr(radius); - - while (!m_openList->empty()) - { - dtNode* bestNode = m_openList->pop(); - - // Get poly and tile. - unsigned int salt, it, ip; - dtDecodeTileId(bestNode->id, salt, it, ip); - // The API input has been cheked already, skip checking internal data. - const dtTileHeader* header = m_tiles[it].header; - const dtTilePoly* poly = &header->polys[ip]; - - for (int i = 0; i < poly->nlinks; ++i) - { - const dtTileLink* link = &header->links[poly->links+i]; - dtTilePolyRef neighbour = link->ref; - if (neighbour) - { - // Skip parent node. - if (bestNode->pidx && m_nodePool->getNodeAtIdx(bestNode->pidx)->id == neighbour) - continue; - - // Calc distance to the edge. - const float* va = &header->verts[poly->v[link->e]*3]; - const float* vb = &header->verts[poly->v[(link->e+1)%poly->nv]*3]; - float tseg; - float distSqr = distancePtSegSqr2D(centerPos, va, vb, tseg); - - // If the circle is not touching the next polygon, skip it. - if (distSqr > radiusSqr) - continue; - - dtNode* parent = bestNode; - dtNode newNode; - newNode.pidx = m_nodePool->getNodeIdx(parent); - newNode.id = neighbour; - - // Cost - float p0[3], p1[3]; - if (!parent->pidx) - vcopy(p0, centerPos); - else - getEdgeMidPoint(m_nodePool->getNodeAtIdx(parent->pidx)->id, parent->id, p0); - getEdgeMidPoint(parent->id, newNode.id, p1); - newNode.total = parent->total + vdist(p0,p1); - - dtNode* actualNode = m_nodePool->getNode(newNode.id); - if (!actualNode) - continue; - - if (!((actualNode->flags & DT_NODE_OPEN) && newNode.total > actualNode->total) && - !((actualNode->flags & DT_NODE_CLOSED) && newNode.total > actualNode->total)) - { - actualNode->flags &= ~DT_NODE_CLOSED; - actualNode->pidx = newNode.pidx; - actualNode->total = newNode.total; - - if (actualNode->flags & DT_NODE_OPEN) - { - m_openList->modify(actualNode); - } - else - { - if (n < maxResult) - { - if (resultRef) - resultRef[n] = actualNode->id; - if (resultParent) - resultParent[n] = m_nodePool->getNodeAtIdx(actualNode->pidx)->id; - if (resultCost) - resultCost[n] = actualNode->total; - ++n; - } - actualNode->flags = DT_NODE_OPEN; - m_openList->push(actualNode); - } - } - } - } - } - - return n; -} - -float dtTiledNavMesh::findDistanceToWall(dtTilePolyRef centerRef, const float* centerPos, float maxRadius, - float* hitPos, float* hitNormal) -{ - if (!centerRef) return 0; - if (!getPolyByRef(centerRef)) return 0; - if (!m_nodePool || !m_openList) return 0; - - m_nodePool->clear(); - m_openList->clear(); - - dtNode* startNode = m_nodePool->getNode(centerRef); - startNode->pidx = 0; - startNode->cost = 0; - startNode->total = 0; - startNode->id = centerRef; - startNode->flags = DT_NODE_OPEN; - m_openList->push(startNode); - - float radiusSqr = sqr(maxRadius); - - while (!m_openList->empty()) - { - dtNode* bestNode = m_openList->pop(); - - // Get poly and tile. - unsigned int salt, it, ip; - dtDecodeTileId(bestNode->id, salt, it, ip); - // The API input has been cheked already, skip checking internal data. - const dtTileHeader* header = m_tiles[it].header; - const dtTilePoly* poly = &header->polys[ip]; - - // Hit test walls. - for (int i = 0, j = (int)poly->nv-1; i < (int)poly->nv; j = i++) - { - // Skip non-solid edges. - if (poly->n[j] & 0x8000) - { - // Tile border. - bool solid = true; - for (int i = 0; i < poly->nlinks; ++i) - { - const dtTileLink* link = &header->links[poly->links+i]; - if (link->e == j && link->ref != 0) - { - solid = false; - break; - } - } - if (!solid) continue; - } - else if (poly->n[j]) - { - // Internal edge - continue; - } - - // Calc distance to the edge. - const float* vj = &header->verts[poly->v[j]*3]; - const float* vi = &header->verts[poly->v[i]*3]; - float tseg; - float distSqr = distancePtSegSqr2D(centerPos, vj, vi, tseg); - - // Edge is too far, skip. - if (distSqr > radiusSqr) - continue; - - // Hit wall, update radius. - radiusSqr = distSqr; - // Calculate hit pos. - hitPos[0] = vj[0] + (vi[0] - vj[0])*tseg; - hitPos[1] = vj[1] + (vi[1] - vj[1])*tseg; - hitPos[2] = vj[2] + (vi[2] - vj[2])*tseg; - } - - for (int i = 0; i < poly->nlinks; ++i) - { - const dtTileLink* link = &header->links[poly->links+i]; - dtTilePolyRef neighbour = link->ref; - if (neighbour) - { - // Skip parent node. - if (bestNode->pidx && m_nodePool->getNodeAtIdx(bestNode->pidx)->id == neighbour) - continue; - - // Calc distance to the edge. - const float* va = &header->verts[poly->v[link->e]*3]; - const float* vb = &header->verts[poly->v[(link->e+1)%poly->nv]*3]; - float tseg; - float distSqr = distancePtSegSqr2D(centerPos, va, vb, tseg); - - // If the circle is not touching the next polygon, skip it. - if (distSqr > radiusSqr) - continue; - - dtNode* parent = bestNode; - dtNode newNode; - newNode.pidx = m_nodePool->getNodeIdx(parent); - newNode.id = neighbour; - - float p0[3], p1[3]; - if (!parent->pidx) - vcopy(p0, centerPos); - else - getEdgeMidPoint(m_nodePool->getNodeAtIdx(parent->pidx)->id, parent->id, p0); - getEdgeMidPoint(parent->id, newNode.id, p1); - newNode.total = parent->total + vdist(p0,p1); - - dtNode* actualNode = m_nodePool->getNode(newNode.id); - if (!actualNode) - continue; - - if (!((actualNode->flags & DT_NODE_OPEN) && newNode.total > actualNode->total) && - !((actualNode->flags & DT_NODE_CLOSED) && newNode.total > actualNode->total)) - { - actualNode->flags &= ~DT_NODE_CLOSED; - actualNode->pidx = newNode.pidx; - actualNode->total = newNode.total; - - if (actualNode->flags & DT_NODE_OPEN) - { - m_openList->modify(actualNode); - } - else - { - actualNode->flags = DT_NODE_OPEN; - m_openList->push(actualNode); - } - } - } - } - } - - // Calc hit normal. - vsub(hitNormal, centerPos, hitPos); - vnormalize(hitNormal); - - return sqrtf(radiusSqr); -} - -const dtTilePoly* dtTiledNavMesh::getPolyByRef(dtTilePolyRef ref) const -{ - unsigned int salt, it, ip; - dtDecodeTileId(ref, salt, it, ip); - if (it >= DT_MAX_TILES) return 0; - if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return 0; - if (ip >= (unsigned int)m_tiles[it].header->npolys) return 0; - return &m_tiles[it].header->polys[ip]; -} - -const float* dtTiledNavMesh::getPolyVertsByRef(dtTilePolyRef ref) const -{ - unsigned int salt, it, ip; - dtDecodeTileId(ref, salt, it, ip); - if (it >= DT_MAX_TILES) return 0; - if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return 0; - if (ip >= (unsigned int)m_tiles[it].header->npolys) return 0; - return m_tiles[it].header->verts; -} - -const dtTileLink* dtTiledNavMesh::getPolyLinksByRef(dtTilePolyRef ref) const -{ - unsigned int salt, it, ip; - dtDecodeTileId(ref, salt, it, ip); - if (it >= DT_MAX_TILES) return 0; - if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return 0; - if (ip >= (unsigned int)m_tiles[it].header->npolys) return 0; - return m_tiles[it].header->links; -} |