diff options
author | Yehonal <yehonal.azeroth@gmail.com> | 2017-12-21 11:26:43 +0100 |
---|---|---|
committer | Yehonal <yehonal.azeroth@gmail.com> | 2017-12-21 11:26:43 +0100 |
commit | 403ed2600f39c1105b6641086808a6cdafbca1c5 (patch) | |
tree | 0422298438422af22f3f0723dfa62f66e10510f1 /deps/recastnavigation/Detour | |
parent | acd60005e833efee3a10a2f1a4aee7ecced4cc97 (diff) | |
parent | a0d17509a2c0b06d9e74c01d3fe7dc4df2c5c9a7 (diff) |
Merge branch 'master' of https://github.com/azerothcore/azerothcore-wotlk into dir-restructure
Diffstat (limited to 'deps/recastnavigation/Detour')
-rw-r--r-- | deps/recastnavigation/Detour/CMakeLists.txt | 27 | ||||
-rw-r--r-- | deps/recastnavigation/Detour/DetourAlloc.cpp | 50 | ||||
-rw-r--r-- | deps/recastnavigation/Detour/DetourAlloc.h | 36 | ||||
-rw-r--r-- | deps/recastnavigation/Detour/DetourAssert.h | 33 | ||||
-rw-r--r-- | deps/recastnavigation/Detour/DetourCommon.cpp | 329 | ||||
-rw-r--r-- | deps/recastnavigation/Detour/DetourCommon.h | 248 | ||||
-rw-r--r-- | deps/recastnavigation/Detour/DetourNavMesh.cpp | 1239 | ||||
-rw-r--r-- | deps/recastnavigation/Detour/DetourNavMesh.h | 428 | ||||
-rw-r--r-- | deps/recastnavigation/Detour/DetourNavMeshBuilder.cpp | 770 | ||||
-rw-r--r-- | deps/recastnavigation/Detour/DetourNavMeshBuilder.h | 79 | ||||
-rw-r--r-- | deps/recastnavigation/Detour/DetourNavMeshQuery.cpp | 2578 | ||||
-rw-r--r-- | deps/recastnavigation/Detour/DetourNavMeshQuery.h | 407 | ||||
-rw-r--r-- | deps/recastnavigation/Detour/DetourNode.cpp | 164 | ||||
-rw-r--r-- | deps/recastnavigation/Detour/DetourNode.h | 159 | ||||
-rw-r--r-- | deps/recastnavigation/Detour/DetourObstacleAvoidance.cpp | 532 | ||||
-rw-r--r-- | deps/recastnavigation/Detour/DetourObstacleAvoidance.h | 148 |
16 files changed, 19 insertions, 7208 deletions
diff --git a/deps/recastnavigation/Detour/CMakeLists.txt b/deps/recastnavigation/Detour/CMakeLists.txt index 233d123434..be271211b3 100644 --- a/deps/recastnavigation/Detour/CMakeLists.txt +++ b/deps/recastnavigation/Detour/CMakeLists.txt @@ -1,4 +1,4 @@ -# Copyright (C) +# Copyright (C) 2008-2016 TrinityCore <http://www.trinitycore.org/> # # This file is free software; as a special exception the author gives # unlimited permission to copy and/or distribute it, with or without @@ -9,12 +9,12 @@ # implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. set(Detour_STAT_SRCS - DetourAlloc.cpp - DetourCommon.cpp - DetourNavMesh.cpp - DetourNavMeshBuilder.cpp - DetourNavMeshQuery.cpp - DetourNode.cpp + Source/DetourAlloc.cpp + Source/DetourCommon.cpp + Source/DetourNavMesh.cpp + Source/DetourNavMeshBuilder.cpp + Source/DetourNavMeshQuery.cpp + Source/DetourNode.cpp ) if(WIN32) @@ -25,4 +25,15 @@ endif() add_library(Detour STATIC ${Detour_STAT_SRCS}) -target_link_libraries(Detour ${ZLIB_LIBRARIES}) +target_include_directories(Detour + PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/Include) + +target_link_libraries(Detour + PUBLIC + zlib) + +set_target_properties(Detour + PROPERTIES + FOLDER + "dep") diff --git a/deps/recastnavigation/Detour/DetourAlloc.cpp b/deps/recastnavigation/Detour/DetourAlloc.cpp deleted file mode 100644 index 5f671df5bd..0000000000 --- a/deps/recastnavigation/Detour/DetourAlloc.cpp +++ /dev/null @@ -1,50 +0,0 @@ -// -// Copyright (c) 2009-2010 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 <stdlib.h> -#include "DetourAlloc.h" - -static void *dtAllocDefault(int size, dtAllocHint) -{ - return malloc(size); -} - -static void dtFreeDefault(void *ptr) -{ - free(ptr); -} - -static dtAllocFunc* sAllocFunc = dtAllocDefault; -static dtFreeFunc* sFreeFunc = dtFreeDefault; - -void dtAllocSetCustom(dtAllocFunc *allocFunc, dtFreeFunc *freeFunc) -{ - sAllocFunc = allocFunc ? allocFunc : dtAllocDefault; - sFreeFunc = freeFunc ? freeFunc : dtFreeDefault; -} - -void* dtAlloc(int size, dtAllocHint hint) -{ - return sAllocFunc(size, hint); -} - -void dtFree(void* ptr) -{ - if (ptr) - sFreeFunc(ptr); -} diff --git a/deps/recastnavigation/Detour/DetourAlloc.h b/deps/recastnavigation/Detour/DetourAlloc.h deleted file mode 100644 index 8693475419..0000000000 --- a/deps/recastnavigation/Detour/DetourAlloc.h +++ /dev/null @@ -1,36 +0,0 @@ -// -// Copyright (c) 2009-2010 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. -// - -#ifndef DETOURALLOCATOR_H -#define DETOURALLOCATOR_H - -enum dtAllocHint -{ - DT_ALLOC_PERM, // Memory persist after a function call. - DT_ALLOC_TEMP // Memory used temporarily within a function. -}; - -typedef void* (dtAllocFunc)(int size, dtAllocHint hint); -typedef void (dtFreeFunc)(void* ptr); - -void dtAllocSetCustom(dtAllocFunc *allocFunc, dtFreeFunc *freeFunc); - -void* dtAlloc(int size, dtAllocHint hint); -void dtFree(void* ptr); - -#endif diff --git a/deps/recastnavigation/Detour/DetourAssert.h b/deps/recastnavigation/Detour/DetourAssert.h deleted file mode 100644 index 3cf652288f..0000000000 --- a/deps/recastnavigation/Detour/DetourAssert.h +++ /dev/null @@ -1,33 +0,0 @@ -// -// Copyright (c) 2009-2010 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. -// - -#ifndef DETOURASSERT_H -#define DETOURASSERT_H - -// Note: This header file's only purpose is to include define assert. -// Feel free to change the file and include your own implementation instead. - -#ifdef NDEBUG -// From http://cnicholson.net/2009/02/stupid-c-tricks-adventures-in-assert/ -# define dtAssert(x) do { (void)sizeof(x); } while((void)(__LINE__==-1),false) -#else -# include <assert.h> -# define dtAssert assert -#endif - -#endif // DETOURASSERT_H diff --git a/deps/recastnavigation/Detour/DetourCommon.cpp b/deps/recastnavigation/Detour/DetourCommon.cpp deleted file mode 100644 index c0b973e4a7..0000000000 --- a/deps/recastnavigation/Detour/DetourCommon.cpp +++ /dev/null @@ -1,329 +0,0 @@ -// -// Copyright (c) 2009-2010 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 "DetourCommon.h" - -////////////////////////////////////////////////////////////////////////////////////////// - -float dtSqrt(float x) -{ - return sqrtf(x); -} - -void dtClosestPtPointTriangle(float* closest, const float* p, - const float* a, const float* b, const float* c) -{ - // Check if P in vertex region outside A - float ab[3], ac[3], ap[3]; - dtVsub(ab, b, a); - dtVsub(ac, c, a); - dtVsub(ap, p, a); - float d1 = dtVdot(ab, ap); - float d2 = dtVdot(ac, ap); - if (d1 <= 0.0f && d2 <= 0.0f) - { - // barycentric coordinates (1,0,0) - dtVcopy(closest, a); - return; - } - - // Check if P in vertex region outside B - float bp[3]; - dtVsub(bp, p, b); - float d3 = dtVdot(ab, bp); - float d4 = dtVdot(ac, bp); - if (d3 >= 0.0f && d4 <= d3) - { - // barycentric coordinates (0,1,0) - dtVcopy(closest, b); - return; - } - - // Check if P in edge region of AB, if so return projection of P onto AB - float vc = d1*d4 - d3*d2; - if (vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f) - { - // barycentric coordinates (1-v,v,0) - float v = d1 / (d1 - d3); - closest[0] = a[0] + v * ab[0]; - closest[1] = a[1] + v * ab[1]; - closest[2] = a[2] + v * ab[2]; - return; - } - - // Check if P in vertex region outside C - float cp[3]; - dtVsub(cp, p, c); - float d5 = dtVdot(ab, cp); - float d6 = dtVdot(ac, cp); - if (d6 >= 0.0f && d5 <= d6) - { - // barycentric coordinates (0,0,1) - dtVcopy(closest, c); - return; - } - - // Check if P in edge region of AC, if so return projection of P onto AC - float vb = d5*d2 - d1*d6; - if (vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f) - { - // barycentric coordinates (1-w,0,w) - float w = d2 / (d2 - d6); - closest[0] = a[0] + w * ac[0]; - closest[1] = a[1] + w * ac[1]; - closest[2] = a[2] + w * ac[2]; - return; - } - - // Check if P in edge region of BC, if so return projection of P onto BC - float va = d3*d6 - d5*d4; - if (va <= 0.0f && (d4 - d3) >= 0.0f && (d5 - d6) >= 0.0f) - { - // barycentric coordinates (0,1-w,w) - float w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); - closest[0] = b[0] + w * (c[0] - b[0]); - closest[1] = b[1] + w * (c[1] - b[1]); - closest[2] = b[2] + w * (c[2] - b[2]); - return; - } - - // P inside face region. Compute Q through its barycentric coordinates (u,v,w) - float denom = 1.0f / (va + vb + vc); - float v = vb * denom; - float w = vc * denom; - closest[0] = a[0] + ab[0] * v + ac[0] * w; - closest[1] = a[1] + ab[1] * v + ac[1] * w; - closest[2] = a[2] + ab[2] * v + ac[2] * w; -} - -bool dtIntersectSegmentPoly2D(const float* p0, const float* p1, - const float* verts, int nverts, - float& tmin, float& tmax, - int& segMin, int& segMax) -{ - static const float EPS = 0.00000001f; - - tmin = 0; - tmax = 1; - segMin = -1; - segMax = -1; - - float dir[3]; - dtVsub(dir, p1, p0); - - for (int i = 0, j = nverts-1; i < nverts; j=i++) - { - float edge[3], diff[3]; - dtVsub(edge, &verts[i*3], &verts[j*3]); - dtVsub(diff, p0, &verts[j*3]); - const float n = dtVperp2D(edge, diff); - const float d = dtVperp2D(dir, edge); - if (fabsf(d) < EPS) - { - // S is nearly parallel to this edge - if (n < 0) - return false; - else - continue; - } - const float t = n / d; - if (d < 0) - { - // segment S is entering across this edge - if (t > tmin) - { - tmin = t; - segMin = j; - // S enters after leaving polygon - if (tmin > tmax) - return false; - } - } - else - { - // segment S is leaving across this edge - if (t < tmax) - { - tmax = t; - segMax = j; - // S leaves before entering polygon - if (tmax < tmin) - return false; - } - } - } - - return true; -} - -float dtDistancePtSegSqr2D(const float* pt, const float* p, const float* q, float& t) -{ - float pqx = q[0] - p[0]; - float pqz = q[2] - p[2]; - float dx = pt[0] - p[0]; - float dz = pt[2] - p[2]; - float d = pqx*pqx + pqz*pqz; - t = pqx*dx + pqz*dz; - if (d > 0) t /= d; - if (t < 0) t = 0; - else if (t > 1) t = 1; - dx = p[0] + t*pqx - pt[0]; - dz = p[2] + t*pqz - pt[2]; - return dx*dx + dz*dz; -} - -void dtCalcPolyCenter(float* tc, const unsigned short* idx, int nidx, const float* verts) -{ - tc[0] = 0.0f; - tc[1] = 0.0f; - tc[2] = 0.0f; - for (int j = 0; j < nidx; ++j) - { - const float* v = &verts[idx[j]*3]; - tc[0] += v[0]; - tc[1] += v[1]; - tc[2] += v[2]; - } - const float s = 1.0f / nidx; - tc[0] *= s; - tc[1] *= s; - tc[2] *= s; -} - -bool dtClosestHeightPointTriangle(const float* p, const float* a, const float* b, const float* c, float& h) -{ - float v0[3], v1[3], v2[3]; - dtVsub(v0, c,a); - dtVsub(v1, b,a); - dtVsub(v2, p,a); - - const float dot00 = dtVdot2D(v0, v0); - const float dot01 = dtVdot2D(v0, v1); - const float dot02 = dtVdot2D(v0, v2); - const float dot11 = dtVdot2D(v1, v1); - const float dot12 = dtVdot2D(v1, v2); - - // Compute barycentric coordinates - const float invDenom = 1.0f / (dot00 * dot11 - dot01 * dot01); - const float u = (dot11 * dot02 - dot01 * dot12) * invDenom; - const float v = (dot00 * dot12 - dot01 * dot02) * invDenom; - - // The (sloppy) epsilon is needed to allow to get height of points which - // are interpolated along the edges of the triangles. - static const float EPS = 1e-4f; - - // If point lies inside the triangle, return interpolated ycoord. - if (u >= -EPS && v >= -EPS && (u+v) <= 1+EPS) - { - h = a[1] + v0[1]*u + v1[1]*v; - return true; - } - - return false; -} - -bool dtPointInPolygon(const float* pt, const float* verts, const int nverts) -{ - // TODO: Replace pnpoly with triArea2D tests? - int i, j; - bool c = false; - for (i = 0, j = nverts-1; i < nverts; j = i++) - { - const float* vi = &verts[i*3]; - const float* vj = &verts[j*3]; - if (((vi[2] > pt[2]) != (vj[2] > pt[2])) && - (pt[0] < (vj[0]-vi[0]) * (pt[2]-vi[2]) / (vj[2]-vi[2]) + vi[0]) ) - c = !c; - } - return c; -} - -bool dtDistancePtPolyEdgesSqr(const float* pt, const float* verts, const int nverts, - float* ed, float* et) -{ - // TODO: Replace pnpoly with triArea2D tests? - int i, j; - bool c = false; - for (i = 0, j = nverts-1; i < nverts; j = i++) - { - const float* vi = &verts[i*3]; - const float* vj = &verts[j*3]; - if (((vi[2] > pt[2]) != (vj[2] > pt[2])) && - (pt[0] < (vj[0]-vi[0]) * (pt[2]-vi[2]) / (vj[2]-vi[2]) + vi[0]) ) - c = !c; - ed[j] = dtDistancePtSegSqr2D(pt, vj, vi, et[j]); - } - return c; -} - -static void projectPoly(const float* axis, const float* poly, const int npoly, - float& rmin, float& rmax) -{ - rmin = rmax = dtVdot2D(axis, &poly[0]); - for (int i = 1; i < npoly; ++i) - { - const float d = dtVdot2D(axis, &poly[i*3]); - rmin = dtMin(rmin, d); - rmax = dtMax(rmax, d); - } -} - -inline bool overlapRange(const float amin, const float amax, - const float bmin, const float bmax, - const float eps) -{ - return ((amin+eps) > bmax || (amax-eps) < bmin) ? false : true; -} - -bool dtOverlapPolyPoly2D(const float* polya, const int npolya, - const float* polyb, const int npolyb) -{ - const float eps = 1e-4f; - - for (int i = 0, j = npolya-1; i < npolya; j=i++) - { - const float* va = &polya[j*3]; - const float* vb = &polya[i*3]; - const float n[3] = { vb[2]-va[2], 0, -(vb[0]-va[0]) }; - float amin,amax,bmin,bmax; - projectPoly(n, polya, npolya, amin,amax); - projectPoly(n, polyb, npolyb, bmin,bmax); - if (!overlapRange(amin,amax, bmin,bmax, eps)) - { - // Found separating axis - return false; - } - } - for (int i = 0, j = npolyb-1; i < npolyb; j=i++) - { - const float* va = &polyb[j*3]; - const float* vb = &polyb[i*3]; - const float n[3] = { vb[2]-va[2], 0, -(vb[0]-va[0]) }; - float amin,amax,bmin,bmax; - projectPoly(n, polya, npolya, amin,amax); - projectPoly(n, polyb, npolyb, bmin,bmax); - if (!overlapRange(amin,amax, bmin,bmax, eps)) - { - // Found separating axis - return false; - } - } - return true; -} - diff --git a/deps/recastnavigation/Detour/DetourCommon.h b/deps/recastnavigation/Detour/DetourCommon.h deleted file mode 100644 index 3cee3f6351..0000000000 --- a/deps/recastnavigation/Detour/DetourCommon.h +++ /dev/null @@ -1,248 +0,0 @@ -// -// Copyright (c) 2009-2010 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. -// - -#ifndef DETOURCOMMON_H -#define DETOURCOMMON_H - -template<class T> inline void dtSwap(T& a, T& b) { T t = a; a = b; b = t; } -template<class T> inline T dtMin(T a, T b) { return a < b ? a : b; } -template<class T> inline T dtMax(T a, T b) { return a > b ? a : b; } -template<class T> inline T dtAbs(T a) { return a < 0 ? -a : a; } -template<class T> inline T dtSqr(T a) { return a*a; } -template<class T> inline T dtClamp(T v, T mn, T mx) { return v < mn ? mn : (v > mx ? mx : v); } - -float dtSqrt(float x); - -inline void dtVcross(float* dest, const float* v1, const float* v2) -{ - dest[0] = v1[1]*v2[2] - v1[2]*v2[1]; - dest[1] = v1[2]*v2[0] - v1[0]*v2[2]; - dest[2] = v1[0]*v2[1] - v1[1]*v2[0]; -} - -inline float dtVdot(const float* v1, const float* v2) -{ - return v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2]; -} - -inline void dtVmad(float* dest, const float* v1, const float* v2, const float s) -{ - dest[0] = v1[0]+v2[0]*s; - dest[1] = v1[1]+v2[1]*s; - dest[2] = v1[2]+v2[2]*s; -} - -inline void dtVlerp(float* dest, const float* v1, const float* v2, const float t) -{ - dest[0] = v1[0]+(v2[0]-v1[0])*t; - dest[1] = v1[1]+(v2[1]-v1[1])*t; - dest[2] = v1[2]+(v2[2]-v1[2])*t; -} - -inline void dtVadd(float* dest, const float* v1, const float* v2) -{ - dest[0] = v1[0]+v2[0]; - dest[1] = v1[1]+v2[1]; - dest[2] = v1[2]+v2[2]; -} - -inline void dtVsub(float* dest, const float* v1, const float* v2) -{ - dest[0] = v1[0]-v2[0]; - dest[1] = v1[1]-v2[1]; - dest[2] = v1[2]-v2[2]; -} - -inline void dtVscale(float* dest, const float* v, const float t) -{ - dest[0] = v[0]*t; - dest[1] = v[1]*t; - dest[2] = v[2]*t; -} - -inline void dtVmin(float* mn, const float* v) -{ - mn[0] = dtMin(mn[0], v[0]); - mn[1] = dtMin(mn[1], v[1]); - mn[2] = dtMin(mn[2], v[2]); -} - -inline void dtVmax(float* mx, const float* v) -{ - mx[0] = dtMax(mx[0], v[0]); - mx[1] = dtMax(mx[1], v[1]); - mx[2] = dtMax(mx[2], v[2]); -} - -inline void dtVset(float* dest, const float x, const float y, const float z) -{ - dest[0] = x; dest[1] = y; dest[2] = z; -} - -inline void dtVcopy(float* dest, const float* a) -{ - dest[0] = a[0]; - dest[1] = a[1]; - dest[2] = a[2]; -} - -inline float dtVlen(const float* v) -{ - return dtSqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]); -} - -inline float dtVlenSqr(const float* v) -{ - return v[0]*v[0] + v[1]*v[1] + v[2]*v[2]; -} - -inline float dtVdist(const float* v1, const float* v2) -{ - const float dx = v2[0] - v1[0]; - const float dy = v2[1] - v1[1]; - const float dz = v2[2] - v1[2]; - return dtSqrt(dx*dx + dy*dy + dz*dz); -} - -inline float dtVdistSqr(const float* v1, const float* v2) -{ - const float dx = v2[0] - v1[0]; - const float dy = v2[1] - v1[1]; - const float dz = v2[2] - v1[2]; - return dx*dx + dy*dy + dz*dz; -} - -inline float dtVdist2D(const float* v1, const float* v2) -{ - const float dx = v2[0] - v1[0]; - const float dz = v2[2] - v1[2]; - return dtSqrt(dx*dx + dz*dz); -} - -inline float dtVdist2DSqr(const float* v1, const float* v2) -{ - const float dx = v2[0] - v1[0]; - const float dz = v2[2] - v1[2]; - return dx*dx + dz*dz; -} - -inline void dtVnormalize(float* v) -{ - float d = 1.0f / dtSqrt(dtSqr(v[0]) + dtSqr(v[1]) + dtSqr(v[2])); - v[0] *= d; - v[1] *= d; - v[2] *= d; -} - -inline bool dtVequal(const float* p0, const float* p1) -{ - static const float thr = dtSqr(1.0f/16384.0f); - const float d = dtVdistSqr(p0, p1); - return d < thr; -} - -inline unsigned int dtNextPow2(unsigned int v) -{ - v--; - v |= v >> 1; - v |= v >> 2; - v |= v >> 4; - v |= v >> 8; - v |= v >> 16; - v++; - return v; -} - -inline unsigned int dtIlog2(unsigned int v) -{ - unsigned int r; - unsigned int shift; - r = (v > 0xffff) << 4; v >>= r; - shift = (v > 0xff) << 3; v >>= shift; r |= shift; - shift = (v > 0xf) << 2; v >>= shift; r |= shift; - shift = (v > 0x3) << 1; v >>= shift; r |= shift; - r |= (v >> 1); - return r; -} - -inline int dtAlign4(int x) { return (x+3) & ~3; } - -inline int dtOppositeTile(int side) { return (side+4) & 0x7; } - -inline float dtVdot2D(const float* u, const float* v) -{ - return u[0]*v[0] + u[2]*v[2]; -} - -inline float dtVperp2D(const float* u, const float* v) -{ - return u[2]*v[0] - u[0]*v[2]; -} - -inline float dtTriArea2D(const float* a, const float* b, const float* c) -{ - const float abx = b[0] - a[0]; - const float abz = b[2] - a[2]; - const float acx = c[0] - a[0]; - const float acz = c[2] - a[2]; - return acx*abz - abx*acz; -} - -inline bool dtOverlapQuantBounds(const unsigned short amin[3], const unsigned short amax[3], - const unsigned short bmin[3], const unsigned short bmax[3]) -{ - 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 dtOverlapBounds(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; -} - -void dtClosestPtPointTriangle(float* closest, const float* p, - const float* a, const float* b, const float* c); - -bool dtClosestHeightPointTriangle(const float* p, const float* a, const float* b, const float* c, float& h); - -bool dtIntersectSegmentPoly2D(const float* p0, const float* p1, - const float* verts, int nverts, - float& tmin, float& tmax, - int& segMin, int& segMax); - -bool dtPointInPolygon(const float* pt, const float* verts, const int nverts); - -bool dtDistancePtPolyEdgesSqr(const float* pt, const float* verts, const int nverts, - float* ed, float* et); - -float dtDistancePtSegSqr2D(const float* pt, const float* p, const float* q, float& t); - -void dtCalcPolyCenter(float* tc, const unsigned short* idx, int nidx, const float* verts); - -bool dtOverlapPolyPoly2D(const float* polya, const int npolya, - const float* polyb, const int npolyb); - -#endif // DETOURCOMMON_H diff --git a/deps/recastnavigation/Detour/DetourNavMesh.cpp b/deps/recastnavigation/Detour/DetourNavMesh.cpp deleted file mode 100644 index 95af28797d..0000000000 --- a/deps/recastnavigation/Detour/DetourNavMesh.cpp +++ /dev/null @@ -1,1239 +0,0 @@ -// -// Copyright (c) 2009-2010 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 "DetourNavMesh.h" -#include "DetourNode.h" -#include "DetourCommon.h" -#include "DetourAlloc.h" -#include "DetourAssert.h" -#include <new> - - -inline bool overlapSlabs(const float* amin, const float* amax, - const float* bmin, const float* bmax, - const float px, const float py) -{ - // Check for horizontal overlap. - // The segment is shrunken a little so that slabs which touch - // at end points are not connected. - const float minx = dtMax(amin[0]+px,bmin[0]+px); - const float maxx = dtMin(amax[0]-px,bmax[0]-px); - if (minx > maxx) - return false; - - // Check vertical overlap. - const float ad = (amax[1]-amin[1]) / (amax[0]-amin[0]); - const float ak = amin[1] - ad*amin[0]; - const float bd = (bmax[1]-bmin[1]) / (bmax[0]-bmin[0]); - const float bk = bmin[1] - bd*bmin[0]; - const float aminy = ad*minx + ak; - const float amaxy = ad*maxx + ak; - const float bminy = bd*minx + bk; - const float bmaxy = bd*maxx + bk; - const float dmin = bminy - aminy; - const float dmax = bmaxy - amaxy; - - // Crossing segments always overlap. - if (dmin*dmax < 0) - return true; - - // Check for overlap at endpoints. - const float thr = dtSqr(py*2); - if (dmin*dmin <= thr || dmax*dmax <= thr) - return true; - - return false; -} - -static void calcSlabEndPoints(const float* va, const float* vb, float* bmin, float* bmax, const int side) -{ - if (side == 0 || side == 4) - { - if (va[2] < vb[2]) - { - bmin[0] = va[2]; - bmin[1] = va[1]; - bmax[0] = vb[2]; - bmax[1] = vb[1]; - } - else - { - bmin[0] = vb[2]; - bmin[1] = vb[1]; - bmax[0] = va[2]; - bmax[1] = va[1]; - } - } - else if (side == 2 || side == 6) - { - if (va[0] < vb[0]) - { - bmin[0] = va[0]; - bmin[1] = va[1]; - bmax[0] = vb[0]; - bmax[1] = vb[1]; - } - else - { - bmin[0] = vb[0]; - bmin[1] = vb[1]; - bmax[0] = va[0]; - bmax[1] = va[1]; - } - } -} - -inline int computeTileHash(int x, int y, const int mask) -{ - 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 & mask); -} - -inline unsigned int allocLink(dtMeshTile* tile) -{ - if (tile->linksFreeList == DT_NULL_LINK) - return DT_NULL_LINK; - unsigned int link = tile->linksFreeList; - tile->linksFreeList = tile->links[link].next; - return link; -} - -inline void freeLink(dtMeshTile* tile, unsigned int link) -{ - tile->links[link].next = tile->linksFreeList; - tile->linksFreeList = link; -} - - -dtNavMesh* dtAllocNavMesh() -{ - void* mem = dtAlloc(sizeof(dtNavMesh), DT_ALLOC_PERM); - if (!mem) return 0; - return new(mem) dtNavMesh; -} - -void dtFreeNavMesh(dtNavMesh* navmesh) -{ - if (!navmesh) return; - navmesh->~dtNavMesh(); - dtFree(navmesh); -} - -////////////////////////////////////////////////////////////////////////////////////////// -dtNavMesh::dtNavMesh() : - m_tileWidth(0), - m_tileHeight(0), - m_maxTiles(0), - m_tileLutSize(0), - m_tileLutMask(0), - m_posLookup(0), - m_nextFree(0), - m_tiles(0), - m_saltBits(0), - m_tileBits(0), - m_polyBits(0) -{ - memset(&m_params, 0, sizeof(dtNavMeshParams)); - m_orig[0] = 0; - m_orig[1] = 0; - m_orig[2] = 0; -} - -dtNavMesh::~dtNavMesh() -{ - for (int i = 0; i < m_maxTiles; ++i) - { - if (m_tiles[i].flags & DT_TILE_FREE_DATA) - { - dtFree(m_tiles[i].data); - m_tiles[i].data = 0; - m_tiles[i].dataSize = 0; - } - } - dtFree(m_posLookup); - dtFree(m_tiles); -} - -dtStatus dtNavMesh::init(const dtNavMeshParams* params) -{ - memcpy(&m_params, params, sizeof(dtNavMeshParams)); - dtVcopy(m_orig, params->orig); - m_tileWidth = params->tileWidth; - m_tileHeight = params->tileHeight; - - // Init tiles - m_maxTiles = params->maxTiles; - m_tileLutSize = dtNextPow2(params->maxTiles/4); - if (!m_tileLutSize) m_tileLutSize = 1; - m_tileLutMask = m_tileLutSize-1; - - m_tiles = (dtMeshTile*)dtAlloc(sizeof(dtMeshTile)*m_maxTiles, DT_ALLOC_PERM); - if (!m_tiles) - return DT_FAILURE; - m_posLookup = (dtMeshTile**)dtAlloc(sizeof(dtMeshTile*)*m_tileLutSize, DT_ALLOC_PERM); - if (!m_posLookup) - return DT_FAILURE; - memset(m_tiles, 0, sizeof(dtMeshTile)*m_maxTiles); - memset(m_posLookup, 0, sizeof(dtMeshTile*)*m_tileLutSize); - m_nextFree = 0; - for (int i = m_maxTiles-1; i >= 0; --i) - { - m_tiles[i].salt = 1; - m_tiles[i].next = m_nextFree; - m_nextFree = &m_tiles[i]; - } - - // Init ID generator values. - m_tileBits = STATIC_TILE_BITS; //dtIlog2(dtNextPow2((unsigned int)params->maxTiles)); - m_polyBits = STATIC_POLY_BITS; //dtIlog2(dtNextPow2((unsigned int)params->maxPolys)); - m_saltBits = STATIC_SALT_BITS; //sizeof(dtPolyRef)*8 - m_tileBits - m_polyBits; - //if (m_saltBits < SALT_MIN_BITS) - //return DT_FAILURE; - - return DT_SUCCESS; -} - -dtStatus dtNavMesh::init(unsigned char* data, const int dataSize, const int flags) -{ - // Make sure the data is in right format. - dtMeshHeader* header = (dtMeshHeader*)data; - if (header->magic != DT_NAVMESH_MAGIC) - return DT_FAILURE; - if (header->version != DT_NAVMESH_VERSION) - return DT_FAILURE; - - dtNavMeshParams params; - dtVcopy(params.orig, header->bmin); - params.tileWidth = header->bmax[0] - header->bmin[0]; - params.tileHeight = header->bmax[2] - header->bmin[2]; - params.maxTiles = 1; - params.maxPolys = header->polyCount; - - dtStatus res = init(¶ms); - if (res != DT_SUCCESS) - return res; - - return addTile(data, dataSize, flags, 0, 0); -} - -const dtNavMeshParams* dtNavMesh::getParams() const -{ - return &m_params; -} - -////////////////////////////////////////////////////////////////////////////////////////// -int dtNavMesh::findConnectingPolys(const float* va, const float* vb, - const dtMeshTile* tile, int side, - dtPolyRef* con, float* conarea, int maxcon) const -{ - if (!tile) return 0; - - float amin[2], amax[2]; - calcSlabEndPoints(va,vb, amin,amax, side); - - // Remove links pointing to 'side' and compact the links array. - float bmin[2], bmax[2]; - unsigned short m = DT_EXT_LINK | (unsigned short)side; - int n = 0; - - dtPolyRef base = getPolyRefBase(tile); - - for (int i = 0; i < tile->header->polyCount; ++i) - { - dtPoly* poly = &tile->polys[i]; - const int nv = poly->vertCount; - for (int j = 0; j < nv; ++j) - { - // Skip edges which do not point to the right side. - if (poly->neis[j] != m) continue; - // Check if the segments touch. - const float* vc = &tile->verts[poly->verts[j]*3]; - const float* vd = &tile->verts[poly->verts[(j+1) % nv]*3]; - calcSlabEndPoints(vc,vd, bmin,bmax, side); - - if (!overlapSlabs(amin,amax, bmin,bmax, 0.01f, tile->header->walkableClimb)) continue; - - // Add return value. - if (n < maxcon) - { - conarea[n*2+0] = dtMax(amin[0], bmin[0]); - conarea[n*2+1] = dtMin(amax[0], bmax[0]); - con[n] = base | (dtPolyRef)i; - n++; - } - break; - } - } - return n; -} - -void dtNavMesh::unconnectExtLinks(dtMeshTile* tile, int side) -{ - if (!tile) return; - - for (int i = 0; i < tile->header->polyCount; ++i) - { - dtPoly* poly = &tile->polys[i]; - unsigned int j = poly->firstLink; - unsigned int pj = DT_NULL_LINK; - while (j != DT_NULL_LINK) - { - if (tile->links[j].side == side) - { - // Revove link. - unsigned int nj = tile->links[j].next; - if (pj == DT_NULL_LINK) - poly->firstLink = nj; - else - tile->links[pj].next = nj; - freeLink(tile, j); - j = nj; - } - else - { - // Advance - pj = j; - j = tile->links[j].next; - } - } - } -} - -void dtNavMesh::connectExtLinks(dtMeshTile* tile, dtMeshTile* target, int side) -{ - if (!tile) return; - - // Connect border links. - for (int i = 0; i < tile->header->polyCount; ++i) - { - dtPoly* poly = &tile->polys[i]; - - // Create new links. - unsigned short m = DT_EXT_LINK | (unsigned short)side; - const int nv = poly->vertCount; - for (int j = 0; j < nv; ++j) - { - // Skip edges which do not point to the right side. - if (poly->neis[j] != m) continue; - - // Create new links - const float* va = &tile->verts[poly->verts[j]*3]; - const float* vb = &tile->verts[poly->verts[(j+1) % nv]*3]; - dtPolyRef nei[4]; - float neia[4*2]; - int nnei = findConnectingPolys(va,vb, target, dtOppositeTile(side), nei,neia,4); - for (int k = 0; k < nnei; ++k) - { - unsigned int idx = allocLink(tile); - if (idx != DT_NULL_LINK) - { - dtLink* link = &tile->links[idx]; - link->ref = nei[k]; - link->edge = (unsigned char)j; - link->side = (unsigned char)side; - - link->next = poly->firstLink; - poly->firstLink = idx; - - // Compress portal limits to a byte value. - if (side == 0 || side == 4) - { - float tmin = (neia[k*2+0]-va[2]) / (vb[2]-va[2]); - float tmax = (neia[k*2+1]-va[2]) / (vb[2]-va[2]); - if (tmin > tmax) - dtSwap(tmin,tmax); - link->bmin = (unsigned char)(dtClamp(tmin, 0.0f, 1.0f)*255.0f); - link->bmax = (unsigned char)(dtClamp(tmax, 0.0f, 1.0f)*255.0f); - } - else if (side == 2 || side == 6) - { - float tmin = (neia[k*2+0]-va[0]) / (vb[0]-va[0]); - float tmax = (neia[k*2+1]-va[0]) / (vb[0]-va[0]); - if (tmin > tmax) - dtSwap(tmin,tmax); - link->bmin = (unsigned char)(dtClamp(tmin, 0.0f, 1.0f)*255.0f); - link->bmax = (unsigned char)(dtClamp(tmax, 0.0f, 1.0f)*255.0f); - } - } - } - } - } -} - -void dtNavMesh::connectExtOffMeshLinks(dtMeshTile* tile, dtMeshTile* target, int side) -{ - if (!tile) return; - - // 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); - - 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]; - - const float ext[3] = { targetCon->rad, target->header->walkableClimb, targetCon->rad }; - - // Find polygon to connect to. - const float* p = &targetCon->pos[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(targetCon->rad)) - continue; - // Make sure the location is on current mesh. - float* v = &target->verts[targetPoly->verts[1]*3]; - dtVcopy(v, nearestPt); - - // Link off-mesh connection to target poly. - unsigned int idx = allocLink(target); - if (idx != DT_NULL_LINK) - { - dtLink* link = &target->links[idx]; - link->ref = ref; - link->edge = (unsigned char)1; - link->side = oppositeSide; - link->bmin = link->bmax = 0; - // Add to linked list. - link->next = targetPoly->firstLink; - targetPoly->firstLink = idx; - } - - // Link target poly to off-mesh connection. - if (targetCon->flags & DT_OFFMESH_CON_BIDIR) - { - unsigned int idx = allocLink(tile); - if (idx != DT_NULL_LINK) - { - const unsigned short landPolyIdx = (unsigned short)decodePolyIdPoly(ref); - dtPoly* landPoly = &tile->polys[landPolyIdx]; - dtLink* link = &tile->links[idx]; - link->ref = getPolyRefBase(target) | (dtPolyRef)(targetCon->poly); - link->edge = 0xff; - link->side = (unsigned char)side; - link->bmin = link->bmax = 0; - // Add to linked list. - link->next = landPoly->firstLink; - landPoly->firstLink = idx; - } - } - } - -} - -void dtNavMesh::connectIntLinks(dtMeshTile* tile) -{ - if (!tile) return; - - dtPolyRef base = getPolyRefBase(tile); - - for (int i = 0; i < tile->header->polyCount; ++i) - { - dtPoly* poly = &tile->polys[i]; - poly->firstLink = DT_NULL_LINK; - - if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) - continue; - - // Build edge links backwards so that the links will be - // in the linked list from lowest index to highest. - for (int j = poly->vertCount-1; j >= 0; --j) - { - // Skip hard and non-internal edges. - if (poly->neis[j] == 0 || (poly->neis[j] & DT_EXT_LINK)) continue; - - unsigned int idx = allocLink(tile); - if (idx != DT_NULL_LINK) - { - dtLink* link = &tile->links[idx]; - link->ref = base | (dtPolyRef)(poly->neis[j]-1); - link->edge = (unsigned char)j; - link->side = 0xff; - link->bmin = link->bmax = 0; - // Add to linked list. - link->next = poly->firstLink; - poly->firstLink = idx; - } - } - } -} - -void dtNavMesh::connectIntOffMeshLinks(dtMeshTile* tile) -{ - if (!tile) return; - - dtPolyRef base = getPolyRefBase(tile); - - // Find Off-mesh connection end points. - for (int i = 0; i < tile->header->offMeshConCount; ++i) - { - dtOffMeshConnection* con = &tile->offMeshCons[i]; - dtPoly* poly = &tile->polys[con->poly]; - - 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; - - 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; - } - - // 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 idx = allocLink(tile); - if (idx != DT_NULL_LINK) - { - const unsigned short landPolyIdx = (unsigned short)decodePolyIdPoly(ref); - dtPoly* landPoly = &tile->polys[landPolyIdx]; - dtLink* link = &tile->links[idx]; - 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 = idx; - } - } - - } - } - } -} - -dtStatus dtNavMesh::closestPointOnPolyInTile(const dtMeshTile* tile, unsigned int ip, - const float* pos, float* closest) const -{ - const dtPoly* poly = &tile->polys[ip]; - - float closestDistSqr = FLT_MAX; - const dtPolyDetail* pd = &tile->detailMeshes[ip]; - - for (int j = 0; j < pd->triCount; ++j) - { - const unsigned char* t = &tile->detailTris[(pd->triBase+j)*4]; - const float* v[3]; - for (int k = 0; k < 3; ++k) - { - if (t[k] < poly->vertCount) - v[k] = &tile->verts[poly->verts[t[k]]*3]; - else - v[k] = &tile->detailVerts[(pd->vertBase+(t[k]-poly->vertCount))*3]; - } - float pt[3]; - dtClosestPtPointTriangle(pt, pos, v[0], v[1], v[2]); - float d = dtVdistSqr(pos, pt); - if (d < closestDistSqr) - { - dtVcopy(closest, pt); - closestDistSqr = d; - } - } - - return DT_SUCCESS; -} - -dtPolyRef dtNavMesh::findNearestPolyInTile(const dtMeshTile* tile, - const float* center, const float* extents, - float* nearestPt) const -{ - float bmin[3], bmax[3]; - dtVsub(bmin, center, extents); - dtVadd(bmax, center, extents); - - // Get nearby polygons from proximity grid. - dtPolyRef polys[128]; - int polyCount = queryPolygonsInTile(tile, bmin, bmax, polys, 128); - - // Find nearest polygon amongst the nearby polygons. - dtPolyRef nearest = 0; - float nearestDistanceSqr = FLT_MAX; - for (int i = 0; i < polyCount; ++i) - { - dtPolyRef ref = polys[i]; - float closestPtPoly[3]; - if (closestPointOnPolyInTile(tile, decodePolyIdPoly(ref), center, closestPtPoly) != DT_SUCCESS) - continue; - float d = dtVdistSqr(center, closestPtPoly); - if (d < nearestDistanceSqr) - { - if (nearestPt) - dtVcopy(nearestPt, closestPtPoly); - nearestDistanceSqr = d; - nearest = ref; - } - } - - return nearest; -} - -int dtNavMesh::queryPolygonsInTile(const dtMeshTile* tile, const float* qmin, const float* qmax, - dtPolyRef* polys, const int maxPolys) const -{ - if (tile->bvTree) - { - const dtBVNode* node = &tile->bvTree[0]; - const dtBVNode* end = &tile->bvTree[tile->header->bvNodeCount]; - const float* tbmin = tile->header->bmin; - const float* tbmax = tile->header->bmax; - const float qfac = tile->header->bvQuantFactor; - - // Calculate quantized box - unsigned short bmin[3], bmax[3]; - // dtClamp query box to world box. - float minx = dtClamp(qmin[0], tbmin[0], tbmax[0]) - tbmin[0]; - float miny = dtClamp(qmin[1], tbmin[1], tbmax[1]) - tbmin[1]; - float minz = dtClamp(qmin[2], tbmin[2], tbmax[2]) - tbmin[2]; - float maxx = dtClamp(qmax[0], tbmin[0], tbmax[0]) - tbmin[0]; - float maxy = dtClamp(qmax[1], tbmin[1], tbmax[1]) - tbmin[1]; - float maxz = dtClamp(qmax[2], tbmin[2], tbmax[2]) - tbmin[2]; - // Quantize - bmin[0] = (unsigned short)(qfac * minx) & 0xfffe; - bmin[1] = (unsigned short)(qfac * miny) & 0xfffe; - bmin[2] = (unsigned short)(qfac * minz) & 0xfffe; - bmax[0] = (unsigned short)(qfac * maxx + 1) | 1; - bmax[1] = (unsigned short)(qfac * maxy + 1) | 1; - bmax[2] = (unsigned short)(qfac * maxz + 1) | 1; - - // Traverse tree - dtPolyRef base = getPolyRefBase(tile); - int n = 0; - while (node < end) - { - const bool overlap = dtOverlapQuantBounds(bmin, bmax, node->bmin, node->bmax); - const 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; - } - else - { - float bmin[3], bmax[3]; - int n = 0; - dtPolyRef base = getPolyRefBase(tile); - for (int i = 0; i < tile->header->polyCount; ++i) - { - dtPoly* p = &tile->polys[i]; - // Do not return off-mesh connection polygons. - if (p->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) - continue; - // Calc polygon bounds. - const float* v = &tile->verts[p->verts[0]*3]; - dtVcopy(bmin, v); - dtVcopy(bmax, v); - for (int j = 1; j < p->vertCount; ++j) - { - v = &tile->verts[p->verts[j]*3]; - dtVmin(bmin, v); - dtVmax(bmax, v); - } - if (dtOverlapBounds(qmin,qmax, bmin,bmax)) - { - if (n < maxPolys) - polys[n++] = base | (dtPolyRef)i; - } - } - return n; - } -} - -dtStatus dtNavMesh::addTile(unsigned char* data, int dataSize, int flags, - dtTileRef lastRef, dtTileRef* result) -{ - // Make sure the data is in right format. - dtMeshHeader* header = (dtMeshHeader*)data; - if (header->magic != DT_NAVMESH_MAGIC) - return DT_FAILURE_DATA_MAGIC; - if (header->version != DT_NAVMESH_VERSION) - return DT_FAILURE_DATA_VERSION; - - // Make sure the location is free. - if (getTileAt(header->x, header->y)) - return DT_FAILURE; - - // Allocate a tile. - dtMeshTile* tile = 0; - if (!lastRef) - { - if (m_nextFree) - { - tile = m_nextFree; - m_nextFree = tile->next; - tile->next = 0; - } - } - else - { - // Try to relocate the tile to specific index with same salt. - int tileIndex = (int)decodePolyIdTile((dtPolyRef)lastRef); - if (tileIndex >= m_maxTiles) - return DT_FAILURE_OUT_OF_MEMORY; - // Try to find the specific tile id from the free list. - dtMeshTile* target = &m_tiles[tileIndex]; - dtMeshTile* prev = 0; - tile = m_nextFree; - while (tile && tile != target) - { - prev = tile; - tile = tile->next; - } - // Could not find the correct location. - if (tile != target) - return DT_FAILURE_OUT_OF_MEMORY; - // Remove from freelist - if (!prev) - m_nextFree = tile->next; - else - prev->next = tile->next; - - // Restore salt. - tile->salt = decodePolyIdSalt((dtPolyRef)lastRef); - } - - // Make sure we could allocate a tile. - if (!tile) - return DT_FAILURE_OUT_OF_MEMORY; - - // Insert tile into the position lut. - int h = computeTileHash(header->x, header->y, m_tileLutMask); - tile->next = m_posLookup[h]; - m_posLookup[h] = tile; - - // Patch header pointers. - const int headerSize = dtAlign4(sizeof(dtMeshHeader)); - const int vertsSize = dtAlign4(sizeof(float)*3*header->vertCount); - const int polysSize = dtAlign4(sizeof(dtPoly)*header->polyCount); - const int linksSize = dtAlign4(sizeof(dtLink)*(header->maxLinkCount)); - const int detailMeshesSize = dtAlign4(sizeof(dtPolyDetail)*header->detailMeshCount); - const int detailVertsSize = dtAlign4(sizeof(float)*3*header->detailVertCount); - const int detailTrisSize = dtAlign4(sizeof(unsigned char)*4*header->detailTriCount); - const int bvtreeSize = dtAlign4(sizeof(dtBVNode)*header->bvNodeCount); - const int offMeshLinksSize = dtAlign4(sizeof(dtOffMeshConnection)*header->offMeshConCount); - - unsigned char* d = data + headerSize; - tile->verts = (float*)d; d += vertsSize; - tile->polys = (dtPoly*)d; d += polysSize; - tile->links = (dtLink*)d; d += linksSize; - tile->detailMeshes = (dtPolyDetail*)d; d += detailMeshesSize; - tile->detailVerts = (float*)d; d += detailVertsSize; - tile->detailTris = (unsigned char*)d; d += detailTrisSize; - tile->bvTree = (dtBVNode*)d; d += bvtreeSize; - tile->offMeshCons = (dtOffMeshConnection*)d; d += offMeshLinksSize; - - // Build links freelist - tile->linksFreeList = 0; - tile->links[header->maxLinkCount-1].next = DT_NULL_LINK; - for (int i = 0; i < header->maxLinkCount-1; ++i) - tile->links[i].next = i+1; - - // Init tile. - tile->header = header; - tile->data = data; - tile->dataSize = dataSize; - tile->flags = flags; - - connectIntLinks(tile); - connectIntOffMeshLinks(tile); - - // Create connections connections. - for (int i = 0; i < 8; ++i) - { - dtMeshTile* nei = getNeighbourTileAt(header->x, header->y, i); - if (nei) - { - connectExtLinks(tile, nei, i); - connectExtLinks(nei, tile, dtOppositeTile(i)); - connectExtOffMeshLinks(tile, nei, i); - connectExtOffMeshLinks(nei, tile, dtOppositeTile(i)); - } - } - - if (result) - *result = getTileRef(tile); - - return DT_SUCCESS; -} - -const dtMeshTile* dtNavMesh::getTileAt(int x, int y) const -{ - // Find tile based on hash. - int h = computeTileHash(x,y,m_tileLutMask); - dtMeshTile* tile = m_posLookup[h]; - while (tile) - { - if (tile->header && tile->header->x == x && tile->header->y == y) - return tile; - tile = tile->next; - } - return 0; -} - -dtMeshTile* dtNavMesh::getNeighbourTileAt(int x, int y, int side) const -{ - switch (side) - { - case 0: x++; break; - case 1: x++; y++; break; - case 2: y++; break; - case 3: x--; y++; break; - case 4: x--; break; - case 5: x--; y--; break; - case 6: y--; break; - case 7: x++; y--; break; - }; - - // Find tile based on hash. - int h = computeTileHash(x,y,m_tileLutMask); - dtMeshTile* tile = m_posLookup[h]; - while (tile) - { - if (tile->header && tile->header->x == x && tile->header->y == y) - return tile; - tile = tile->next; - } - return 0; -} - -dtTileRef dtNavMesh::getTileRefAt(int x, int y) const -{ - // Find tile based on hash. - int h = computeTileHash(x,y,m_tileLutMask); - dtMeshTile* tile = m_posLookup[h]; - while (tile) - { - if (tile->header && tile->header->x == x && tile->header->y == y) - return getTileRef(tile); - tile = tile->next; - } - return 0; -} - -const dtMeshTile* dtNavMesh::getTileByRef(dtTileRef ref) const -{ - if (!ref) - return 0; - unsigned int tileIndex = decodePolyIdTile((dtPolyRef)ref); - unsigned int tileSalt = decodePolyIdSalt((dtPolyRef)ref); - if ((int)tileIndex >= m_maxTiles) - return 0; - const dtMeshTile* tile = &m_tiles[tileIndex]; - if (tile->salt != tileSalt) - return 0; - return tile; -} - -int dtNavMesh::getMaxTiles() const -{ - return m_maxTiles; -} - -dtMeshTile* dtNavMesh::getTile(int i) -{ - return &m_tiles[i]; -} - -const dtMeshTile* dtNavMesh::getTile(int i) const -{ - return &m_tiles[i]; -} - -void dtNavMesh::calcTileLoc(const float* pos, int* tx, int* ty) const -{ - *tx = (int)floorf((pos[0]-m_orig[0]) / m_tileWidth); - *ty = (int)floorf((pos[2]-m_orig[2]) / m_tileHeight); -} - -dtStatus dtNavMesh::getTileAndPolyByRef(const dtPolyRef ref, const dtMeshTile** tile, const dtPoly** poly) const -{ - unsigned int salt, it, ip; - decodePolyId(ref, salt, it, ip); - if (it >= (unsigned int)m_maxTiles) return DT_FAILURE; - if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return DT_FAILURE; - if (ip >= (unsigned int)m_tiles[it].header->polyCount) return DT_FAILURE; - *tile = &m_tiles[it]; - *poly = &m_tiles[it].polys[ip]; - return DT_SUCCESS; -} - -void dtNavMesh::getTileAndPolyByRefUnsafe(const dtPolyRef ref, const dtMeshTile** tile, const dtPoly** poly) const -{ - unsigned int salt, it, ip; - decodePolyId(ref, salt, it, ip); - *tile = &m_tiles[it]; - *poly = &m_tiles[it].polys[ip]; -} - -bool dtNavMesh::isValidPolyRef(dtPolyRef ref) const -{ - unsigned int salt, it, ip; - decodePolyId(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; - if (ip >= (unsigned int)m_tiles[it].header->polyCount) return false; - return true; -} - -dtStatus dtNavMesh::removeTile(dtTileRef ref, unsigned char** data, int* dataSize) -{ - if (!ref) - return DT_FAILURE; - unsigned int tileIndex = decodePolyIdTile((dtPolyRef)ref); - unsigned int tileSalt = decodePolyIdSalt((dtPolyRef)ref); - if ((int)tileIndex >= m_maxTiles) - return DT_FAILURE; - dtMeshTile* tile = &m_tiles[tileIndex]; - if (tile->salt != tileSalt) - return DT_FAILURE; - - // Remove tile from hash lookup. - int h = computeTileHash(tile->header->x,tile->header->y,m_tileLutMask); - dtMeshTile* prev = 0; - dtMeshTile* cur = m_posLookup[h]; - while (cur) - { - if (cur == tile) - { - if (prev) - prev->next = cur->next; - else - m_posLookup[h] = cur->next; - break; - } - prev = cur; - cur = cur->next; - } - - // Remove connections to neighbour tiles. - for (int i = 0; i < 8; ++i) - { - dtMeshTile* nei = getNeighbourTileAt(tile->header->x,tile->header->y,i); - if (!nei) continue; - unconnectExtLinks(nei, dtOppositeTile(i)); - } - - - // Reset tile. - if (tile->flags & DT_TILE_FREE_DATA) - { - // Owns data - dtFree(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->flags = 0; - tile->linksFreeList = 0; - tile->polys = 0; - tile->verts = 0; - tile->links = 0; - tile->detailMeshes = 0; - tile->detailVerts = 0; - tile->detailTris = 0; - tile->bvTree = 0; - tile->offMeshCons = 0; - - // Update salt, salt should never be zero. - tile->salt = (tile->salt+1) & ((1<<m_saltBits)-1); - if (tile->salt == 0) - tile->salt++; - - // Add to free list. - tile->next = m_nextFree; - m_nextFree = tile; - - return DT_SUCCESS; -} - -dtTileRef dtNavMesh::getTileRef(const dtMeshTile* tile) const -{ - if (!tile) return 0; - const unsigned int it = (unsigned int)(tile - m_tiles); - return (dtTileRef)encodePolyId(tile->salt, it, 0); -} - -dtPolyRef dtNavMesh::getPolyRefBase(const dtMeshTile* tile) const -{ - if (!tile) return 0; - const unsigned int it = (unsigned int)(tile - m_tiles); - return encodePolyId(tile->salt, it, 0); -} - -struct dtTileState -{ - int magic; // Magic number, used to identify the data. - int version; // Data version number. - dtTileRef ref; // Tile ref at the time of storing the data. -}; - -struct dtPolyState -{ - unsigned short flags; // Flags (see dtPolyFlags). - unsigned char area; // Area ID of the polygon. -}; - -int dtNavMesh::getTileStateSize(const dtMeshTile* tile) const -{ - if (!tile) return 0; - const int headerSize = dtAlign4(sizeof(dtTileState)); - const int polyStateSize = dtAlign4(sizeof(dtPolyState) * tile->header->polyCount); - return headerSize + polyStateSize; -} - -dtStatus dtNavMesh::storeTileState(const dtMeshTile* tile, unsigned char* data, const int maxDataSize) const -{ - // Make sure there is enough space to store the state. - const int sizeReq = getTileStateSize(tile); - if (maxDataSize < sizeReq) - return DT_FAILURE; - - dtTileState* tileState = (dtTileState*)data; data += dtAlign4(sizeof(dtTileState)); - dtPolyState* polyStates = (dtPolyState*)data; data += dtAlign4(sizeof(dtPolyState) * tile->header->polyCount); - - // Store tile state. - tileState->magic = DT_NAVMESH_STATE_MAGIC; - tileState->version = DT_NAVMESH_STATE_VERSION; - tileState->ref = getTileRef(tile); - - // Store per poly state. - for (int i = 0; i < tile->header->polyCount; ++i) - { - const dtPoly* p = &tile->polys[i]; - dtPolyState* s = &polyStates[i]; - s->flags = p->flags; - s->area = p->getArea(); - } - - return DT_SUCCESS; -} - -dtStatus dtNavMesh::restoreTileState(dtMeshTile* tile, const unsigned char* data, const int maxDataSize) -{ - // Make sure there is enough space to store the state. - const int sizeReq = getTileStateSize(tile); - if (maxDataSize < sizeReq) - return DT_FAILURE; - - const dtTileState* tileState = (const dtTileState*)data; data += dtAlign4(sizeof(dtTileState)); - const dtPolyState* polyStates = (const dtPolyState*)data; data += dtAlign4(sizeof(dtPolyState) * tile->header->polyCount); - - // Check that the restore is possible. - if (tileState->magic != DT_NAVMESH_STATE_MAGIC) - return DT_FAILURE_DATA_MAGIC; - if (tileState->version != DT_NAVMESH_STATE_VERSION) - return DT_FAILURE_DATA_VERSION; - if (tileState->ref != getTileRef(tile)) - return DT_FAILURE; - - // Restore per poly state. - for (int i = 0; i < tile->header->polyCount; ++i) - { - dtPoly* p = &tile->polys[i]; - const dtPolyState* s = &polyStates[i]; - p->flags = s->flags; - p->setArea(s->area); - } - - return DT_SUCCESS; -} - -// Returns start and end location of an off-mesh link polygon. -dtStatus dtNavMesh::getOffMeshConnectionPolyEndPoints(dtPolyRef prevRef, dtPolyRef polyRef, float* startPos, float* endPos) const -{ - unsigned int salt, it, ip; - - // Get current polygon - decodePolyId(polyRef, salt, it, ip); - if (it >= (unsigned int)m_maxTiles) return DT_FAILURE; - if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return DT_FAILURE; - const dtMeshTile* tile = &m_tiles[it]; - if (ip >= (unsigned int)tile->header->polyCount) return DT_FAILURE; - const dtPoly* poly = &tile->polys[ip]; - - // Make sure that the current poly is indeed off-mesh link. - if (poly->getType() != DT_POLYTYPE_OFFMESH_CONNECTION) - return DT_FAILURE; - - // Figure out which way to hand out the vertices. - int idx0 = 0, idx1 = 1; - - // Find link that points to first vertex. - for (unsigned int i = poly->firstLink; i != DT_NULL_LINK; i = tile->links[i].next) - { - if (tile->links[i].edge == 0) - { - if (tile->links[i].ref != prevRef) - { - idx0 = 1; - idx1 = 0; - } - break; - } - } - - dtVcopy(startPos, &tile->verts[poly->verts[idx0]*3]); - dtVcopy(endPos, &tile->verts[poly->verts[idx1]*3]); - - return DT_SUCCESS; -} - - -const dtOffMeshConnection* dtNavMesh::getOffMeshConnectionByRef(dtPolyRef ref) const -{ - unsigned int salt, it, ip; - - // Get current polygon - decodePolyId(ref, salt, it, ip); - if (it >= (unsigned int)m_maxTiles) return 0; - if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return 0; - const dtMeshTile* tile = &m_tiles[it]; - if (ip >= (unsigned int)tile->header->polyCount) return 0; - const dtPoly* poly = &tile->polys[ip]; - - // Make sure that the current poly is indeed off-mesh link. - if (poly->getType() != DT_POLYTYPE_OFFMESH_CONNECTION) - return 0; - - const unsigned int idx = ip - tile->header->offMeshBase; - dtAssert(idx < (unsigned int)tile->header->offMeshConCount); - return &tile->offMeshCons[idx]; -} - - -dtStatus dtNavMesh::setPolyFlags(dtPolyRef ref, unsigned short flags) -{ - unsigned int salt, it, ip; - decodePolyId(ref, salt, it, ip); - if (it >= (unsigned int)m_maxTiles) return DT_FAILURE; - if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return DT_FAILURE; - dtMeshTile* tile = &m_tiles[it]; - if (ip >= (unsigned int)tile->header->polyCount) return DT_FAILURE; - dtPoly* poly = &tile->polys[ip]; - - // Change flags. - poly->flags = flags; - - return DT_SUCCESS; -} - -dtStatus dtNavMesh::getPolyFlags(dtPolyRef ref, unsigned short* resultFlags) const -{ - unsigned int salt, it, ip; - decodePolyId(ref, salt, it, ip); - if (it >= (unsigned int)m_maxTiles) return DT_FAILURE; - if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return DT_FAILURE; - const dtMeshTile* tile = &m_tiles[it]; - if (ip >= (unsigned int)tile->header->polyCount) return DT_FAILURE; - const dtPoly* poly = &tile->polys[ip]; - - *resultFlags = poly->flags; - - return DT_SUCCESS; -} - -dtStatus dtNavMesh::setPolyArea(dtPolyRef ref, unsigned char area) -{ - unsigned int salt, it, ip; - decodePolyId(ref, salt, it, ip); - if (it >= (unsigned int)m_maxTiles) return DT_FAILURE; - if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return DT_FAILURE; - dtMeshTile* tile = &m_tiles[it]; - if (ip >= (unsigned int)tile->header->polyCount) return DT_FAILURE; - dtPoly* poly = &tile->polys[ip]; - - poly->setArea(area); - - return DT_SUCCESS; -} - -dtStatus dtNavMesh::getPolyArea(dtPolyRef ref, unsigned char* resultArea) const -{ - unsigned int salt, it, ip; - decodePolyId(ref, salt, it, ip); - if (it >= (unsigned int)m_maxTiles) return DT_FAILURE; - if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return DT_FAILURE; - const dtMeshTile* tile = &m_tiles[it]; - if (ip >= (unsigned int)tile->header->polyCount) return DT_FAILURE; - const dtPoly* poly = &tile->polys[ip]; - - *resultArea = poly->getArea(); - - return DT_SUCCESS; -} - diff --git a/deps/recastnavigation/Detour/DetourNavMesh.h b/deps/recastnavigation/Detour/DetourNavMesh.h deleted file mode 100644 index 6f2db04004..0000000000 --- a/deps/recastnavigation/Detour/DetourNavMesh.h +++ /dev/null @@ -1,428 +0,0 @@ -// -// Copyright (c) 2009-2010 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. -// - -#ifndef DETOURNAVMESH_H -#define DETOURNAVMESH_H - -#include "DetourAlloc.h" - -#ifdef WIN32 - typedef unsigned __int64 uint64; -#else -#include <stdint.h> -#ifndef uint64_t -#ifdef __linux__ -#include <linux/types.h> -#endif -#endif - typedef uint64_t uint64; -#endif - -// Note: If you want to use 64-bit refs, change the types of both dtPolyRef & dtTileRef. -// It is also recommended to change dtHashRef() to proper 64-bit hash too. - -// Reference to navigation polygon. -typedef uint64 dtPolyRef; - -// Reference to navigation mesh tile. -typedef uint64 dtTileRef; - -// Maximum number of vertices per navigation polygon. -static const int DT_VERTS_PER_POLYGON = 6; - -static const int DT_NAVMESH_MAGIC = 'D'<<24 | 'N'<<16 | 'A'<<8 | 'V'; //'DNAV'; -static const int DT_NAVMESH_VERSION = 6; - -static const int DT_NAVMESH_STATE_MAGIC = 'D'<<24 | 'N'<<16 | 'M'<<8 | 'S'; //'DNMS'; -static const int DT_NAVMESH_STATE_VERSION = 1; - -static const unsigned short DT_EXT_LINK = 0x8000; -static const unsigned int DT_NULL_LINK = 0xffffffff; -static const unsigned int DT_OFFMESH_CON_BIDIR = 1; - -static const int DT_MAX_AREAS = 64; - -static const int STATIC_SALT_BITS = 12; -static const int STATIC_TILE_BITS = 21; -static const int STATIC_POLY_BITS = 31; -// we cannot have over 31 bits for either tile nor poly -// without changing polyCount to use 64bits too. - -// Flags for addTile -enum dtTileFlags -{ - DT_TILE_FREE_DATA = 0x01, // Navmesh owns the tile memory and should free it. -}; - -// Flags returned by findStraightPath(). -enum dtStraightPathFlags -{ - DT_STRAIGHTPATH_START = 0x01, // The vertex is the start position. - DT_STRAIGHTPATH_END = 0x02, // The vertex is the end position. - DT_STRAIGHTPATH_OFFMESH_CONNECTION = 0x04, // The vertex is start of an off-mesh link. -}; - -// Flags describing polygon properties. -enum dtPolyTypes -{ - DT_POLYTYPE_GROUND = 0, // Regular ground polygons. - DT_POLYTYPE_OFFMESH_CONNECTION = 1, // Off-mesh connections. -}; - -enum dtStatus -{ - DT_FAILURE = 0, // Operation failed. - DT_FAILURE_DATA_MAGIC, - DT_FAILURE_DATA_VERSION, - DT_FAILURE_OUT_OF_MEMORY, - DT_SUCCESS, // Operation succeed. - DT_IN_PROGRESS, // Operation still in progress. -}; - - -// Structure describing the navigation polygon data. -struct dtPoly -{ - unsigned int firstLink; // Index to first link in linked list. - unsigned short verts[DT_VERTS_PER_POLYGON]; // Indices to vertices of the poly. - unsigned short neis[DT_VERTS_PER_POLYGON]; // Refs to neighbours of the poly. - unsigned short flags; // Flags (see dtPolyFlags). - unsigned char vertCount; // Number of vertices. - unsigned char areaAndtype; // Bit packed: Area ID of the polygon, and Polygon type, see dtPolyTypes.. - inline void setArea(unsigned char a) { areaAndtype = (areaAndtype & 0xc0) | (a & 0x3f); } - inline void setType(unsigned char t) { areaAndtype = (areaAndtype & 0x3f) | (t << 6); } - inline unsigned char getArea() const { return areaAndtype & 0x3f; } - inline unsigned char getType() const { return areaAndtype >> 6; } -}; - -// Stucture describing polygon detail triangles. -struct dtPolyDetail -{ - unsigned int vertBase; // Offset to detail vertex array. - unsigned int triBase; // Offset to detail triangle array. - unsigned char vertCount; // Number of vertices in the detail mesh. - unsigned char triCount; // Number of triangles. -}; - -// Stucture describing a link to another polygon. -struct dtLink -{ - dtPolyRef ref; // Neighbour reference. - unsigned int next; // Index to next link. - unsigned char edge; // Index to polygon edge which owns this link. - unsigned char side; // If boundary link, defines on which side the link is. - unsigned char bmin, bmax; // If boundary link, defines the sub edge area. -}; - -struct dtBVNode -{ - unsigned short bmin[3], bmax[3]; // BVnode bounds - int i; // Index to item or if negative, escape index. -}; - -struct dtOffMeshConnection -{ - float pos[6]; // Both end point locations. - float rad; // Link connection radius. - unsigned short poly; // Poly Id - unsigned char flags; // Link flags - unsigned char side; // End point side. - unsigned int userId; // User ID to identify this connection. -}; - -struct dtMeshHeader -{ - int magic; // Magic number, used to identify the data. - int version; // Data version number. - int x, y; // Location of the time on the grid. - unsigned int userId; // User ID of the tile. - int polyCount; // Number of polygons in the tile. - int vertCount; // Number of vertices in the tile. - int maxLinkCount; // Number of allocated links. - int detailMeshCount; // Number of detail meshes. - int detailVertCount; // Number of detail vertices. - int detailTriCount; // Number of detail triangles. - int bvNodeCount; // Number of BVtree nodes. - int offMeshConCount; // Number of Off-Mesh links. - int offMeshBase; // Index to first polygon which is Off-Mesh link. - float walkableHeight; // Height of the agent. - float walkableRadius; // Radius of the agent - float walkableClimb; // Max climb height of the agent. - float bmin[3], bmax[3]; // Bounding box of the tile. - float bvQuantFactor; // BVtree quantization factor (world to bvnode coords) -}; - -struct dtMeshTile -{ - unsigned int salt; // Counter describing modifications to the tile. - - unsigned int linksFreeList; // Index to next free link. - dtMeshHeader* header; // Pointer to tile header. - dtPoly* polys; // Pointer to the polygons (will be updated when tile is added). - float* verts; // Pointer to the vertices (will be updated when tile added). - dtLink* links; // Pointer to the links (will be updated when tile added). - dtPolyDetail* detailMeshes; // Pointer to detail meshes (will be updated when tile added). - float* detailVerts; // Pointer to detail vertices (will be updated when tile added). - unsigned char* detailTris; // Pointer to detail triangles (will be updated when tile added). - dtBVNode* bvTree; // Pointer to BVtree nodes (will be updated when tile added). - dtOffMeshConnection* offMeshCons; // Pointer to Off-Mesh links. (will be updated when tile added). - - unsigned char* data; // Pointer to tile data. - int dataSize; // Size of the tile data. - int flags; // Tile flags, see dtTileFlags. - dtMeshTile* next; // Next free tile or, next tile in spatial grid. -}; - -struct dtNavMeshParams -{ - float orig[3]; // Origin of the nav mesh tile space. - float tileWidth, tileHeight; // Width and height of each tile. - int maxTiles; // Maximum number of tiles the navmesh can contain. - int maxPolys; // Maximum number of polygons each tile can contain. -}; - - -class dtNavMesh -{ -public: - dtNavMesh(); - ~dtNavMesh(); - - // Initializes the nav mesh for tiled use. - // Params: - // params - (in) navmesh initialization params, see dtNavMeshParams. - // Returns: True if succeed, else false. - dtStatus init(const dtNavMeshParams* params); - - // Initializes the nav mesh for single tile use. - // Params: - // data - (in) Data of the new tile mesh. - // dataSize - (in) Data size of the new tile mesh. - // flags - (in) Tile flags, see dtTileFlags. - // Returns: True if succeed, else false. - dtStatus init(unsigned char* data, const int dataSize, const int flags); - - // Returns pointer to navmesh initialization params. - const dtNavMeshParams* getParams() const; - - // Adds new tile into the navmesh. - // The add will fail if the data is in wrong format, - // there is not enough tiles left, or if there is a tile already at the location. - // Params: - // data - (in) Data of the new tile mesh. - // dataSize - (in) Data size of the new tile mesh. - // flags - (in) Tile flags, see dtTileFlags. - // lastRef - (in,optional) Last tile ref, the tile will be restored so that - // the reference (as well as poly references) will be the same. Default: 0. - // result - (out,optional) tile ref if the tile was succesfully added. - dtStatus addTile(unsigned char* data, int dataSize, int flags, dtTileRef lastRef, dtTileRef* result); - - // Removes specified tile. - // Params: - // ref - (in) Reference to the tile to remove. - // data - (out) Data associated with deleted tile. - // dataSize - (out) Size of the data associated with deleted tile. - dtStatus removeTile(dtTileRef ref, unsigned char** data, int* dataSize); - - // Calculates tile location based in input world position. - // Params: - // pos - (in) world position of the query. - // tx - (out) tile x location. - // ty - (out) tile y location. - void calcTileLoc(const float* pos, int* tx, int* ty) const; - - // Returns pointer to tile at specified location. - // Params: - // x,y - (in) Location of the tile to get. - // Returns: pointer to tile if tile exists or 0 tile does not exists. - const dtMeshTile* getTileAt(int x, int y) const; - - // Returns reference to tile at specified location. - // Params: - // x,y - (in) Location of the tile to get. - // Returns: reference to tile if tile exists or 0 tile does not exists. - dtTileRef getTileRefAt(int x, int y) const; - - // Returns tile references of a tile based on tile pointer. - dtTileRef getTileRef(const dtMeshTile* tile) const; - - // Returns tile based on references. - const dtMeshTile* getTileByRef(dtTileRef ref) const; - - // Returns max number of tiles. - int getMaxTiles() const; - - // Returns pointer to tile in the tile array. - // Params: - // i - (in) Index to the tile to retrieve, max index is getMaxTiles()-1. - // Returns: Pointer to specified tile. - const dtMeshTile* getTile(int i) const; - - // Returns pointer to tile and polygon pointed by the polygon reference. - // Params: - // ref - (in) reference to a polygon. - // tile - (out) pointer to the tile containing the polygon. - // poly - (out) pointer to the polygon. - dtStatus getTileAndPolyByRef(const dtPolyRef ref, const dtMeshTile** tile, const dtPoly** poly) const; - - // Returns pointer to tile and polygon pointed by the polygon reference. - // Note: this function does not check if 'ref' s valid, and is thus faster. Use only with valid refs! - // Params: - // ref - (in) reference to a polygon. - // tile - (out) pointer to the tile containing the polygon. - // poly - (out) pointer to the polygon. - void getTileAndPolyByRefUnsafe(const dtPolyRef ref, const dtMeshTile** tile, const dtPoly** poly) const; - - // Returns true if polygon reference points to valid data. - bool isValidPolyRef(dtPolyRef ref) const; - - // Returns base poly id for specified tile, polygon refs can be deducted from this. - dtPolyRef getPolyRefBase(const dtMeshTile* tile) const; - - // Returns start and end location of an off-mesh link polygon. - // Params: - // prevRef - (in) ref to the polygon before the link (used to select direction). - // polyRef - (in) ref to the off-mesh link polygon. - // startPos[3] - (out) start point of the link. - // endPos[3] - (out) end point of the link. - // Returns: true if link is found. - dtStatus getOffMeshConnectionPolyEndPoints(dtPolyRef prevRef, dtPolyRef polyRef, float* startPos, float* endPos) const; - - // Returns pointer to off-mesh connection based on polyref, or null if ref not valid. - const dtOffMeshConnection* getOffMeshConnectionByRef(dtPolyRef ref) const; - - // Sets polygon flags. - dtStatus setPolyFlags(dtPolyRef ref, unsigned short flags); - - // Return polygon flags. - dtStatus getPolyFlags(dtPolyRef ref, unsigned short* resultFlags) const; - - // Set polygon type. - dtStatus setPolyArea(dtPolyRef ref, unsigned char area); - - // Return polygon area type. - dtStatus getPolyArea(dtPolyRef ref, unsigned char* resultArea) const; - - - // Returns number of bytes required to store tile state. - int getTileStateSize(const dtMeshTile* tile) const; - - // Stores tile state to buffer. - dtStatus storeTileState(const dtMeshTile* tile, unsigned char* data, const int maxDataSize) const; - - // Restores tile state. - dtStatus restoreTileState(dtMeshTile* tile, const unsigned char* data, const int maxDataSize); - - - // Encodes a tile id. - inline dtPolyRef encodePolyId(unsigned int salt, unsigned int it, unsigned int ip) const - { - return ((dtPolyRef)salt << (m_polyBits+m_tileBits)) | ((dtPolyRef)it << m_polyBits) | (dtPolyRef)ip; - } - - // Decodes a tile id. - inline void decodePolyId(dtPolyRef ref, unsigned int& salt, unsigned int& it, unsigned int& ip) const - { - const dtPolyRef saltMask = ((dtPolyRef)1<<m_saltBits)-1; - const dtPolyRef tileMask = ((dtPolyRef)1<<m_tileBits)-1; - const dtPolyRef polyMask = ((dtPolyRef)1<<m_polyBits)-1; - salt = (unsigned int)((ref >> (m_polyBits+m_tileBits)) & saltMask); - it = (unsigned int)((ref >> m_polyBits) & tileMask); - ip = (unsigned int)(ref & polyMask); - } - - // Decodes a tile salt. - inline unsigned int decodePolyIdSalt(dtPolyRef ref) const - { - const dtPolyRef saltMask = ((dtPolyRef)1<<m_saltBits)-1; - return (unsigned int)((ref >> (m_polyBits+m_tileBits)) & saltMask); - } - - // Decodes a tile id. - inline unsigned int decodePolyIdTile(dtPolyRef ref) const - { - const dtPolyRef tileMask = ((dtPolyRef)1<<m_tileBits)-1; - return (unsigned int)((ref >> m_polyBits) & tileMask); - } - - // Decodes a poly id. - inline unsigned int decodePolyIdPoly(dtPolyRef ref) const - { - const dtPolyRef polyMask = ((dtPolyRef)1<<m_polyBits)-1; - return (unsigned int)(ref & polyMask); - } - -private: - - // Returns pointer to tile in the tile array. - dtMeshTile* getTile(int i); - - // Returns neighbour tile based on side. - dtMeshTile* getNeighbourTileAt(int x, int y, int side) const; - // Returns all polygons in neighbour tile based on portal defined by the segment. - int findConnectingPolys(const float* va, const float* vb, - const dtMeshTile* tile, int side, - dtPolyRef* con, float* conarea, int maxcon) const; - - // Builds internal polygons links for a tile. - void connectIntLinks(dtMeshTile* tile); - // Builds internal polygons links for a tile. - void connectIntOffMeshLinks(dtMeshTile* tile); - - // Builds external polygon links for a tile. - void connectExtLinks(dtMeshTile* tile, dtMeshTile* target, int side); - // Builds external polygon links for a tile. - void connectExtOffMeshLinks(dtMeshTile* tile, dtMeshTile* target, int side); - - // Removes external links at specified side. - void unconnectExtLinks(dtMeshTile* tile, int side); - - - // TODO: These methods are duplicates from dtNavMeshQuery, but are needed for off-mesh connection finding. - - // Queries polygons within a tile. - int queryPolygonsInTile(const dtMeshTile* tile, const float* qmin, const float* qmax, - dtPolyRef* polys, const int maxPolys) const; - // Find nearest polygon within a tile. - dtPolyRef findNearestPolyInTile(const dtMeshTile* tile, const float* center, - const float* extents, float* nearestPt) const; - // Returns closest point on polygon. - dtStatus closestPointOnPolyInTile(const dtMeshTile* tile, unsigned int ip, - const float* pos, float* closest) const; - - dtNavMeshParams m_params; // Current initialization params. TODO: do not store this info twice. - float m_orig[3]; // Origin of the tile (0,0) - float m_tileWidth, m_tileHeight; // Dimensions of each tile. - int m_maxTiles; // Max number of tiles. - int m_tileLutSize; // Tile hash lookup size (must be pot). - int m_tileLutMask; // Tile hash lookup mask. - - dtMeshTile** m_posLookup; // Tile hash lookup. - dtMeshTile* m_nextFree; // Freelist of tiles. - dtMeshTile* m_tiles; // List of tiles. - - unsigned int m_saltBits; // Number of salt bits in the tile ID. - unsigned int m_tileBits; // Number of tile bits in the tile ID. - unsigned int m_polyBits; // Number of poly bits in the tile ID. -}; - -// Helper function to allocate navmesh class using Detour allocator. -dtNavMesh* dtAllocNavMesh(); -void dtFreeNavMesh(dtNavMesh* navmesh); - -#endif // DETOURNAVMESH_H diff --git a/deps/recastnavigation/Detour/DetourNavMeshBuilder.cpp b/deps/recastnavigation/Detour/DetourNavMeshBuilder.cpp deleted file mode 100644 index 68f3d730da..0000000000 --- a/deps/recastnavigation/Detour/DetourNavMeshBuilder.cpp +++ /dev/null @@ -1,770 +0,0 @@ -// -// Copyright (c) 2009-2010 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 <stdio.h> -#include <stdlib.h> -#include <string.h> -#include "DetourNavMesh.h" -#include "DetourCommon.h" -#include "DetourNavMeshBuilder.h" -#include "DetourAlloc.h" -#include "DetourAssert.h" - -static unsigned short MESH_NULL_IDX = 0xffff; - - -struct BVItem -{ - unsigned short bmin[3]; - unsigned short bmax[3]; - int i; -}; - -static int compareItemX(const void* va, const void* vb) -{ - const BVItem* a = (const BVItem*)va; - const BVItem* b = (const BVItem*)vb; - if (a->bmin[0] < b->bmin[0]) - return -1; - if (a->bmin[0] > b->bmin[0]) - return 1; - return 0; -} - -static int compareItemY(const void* va, const void* vb) -{ - const BVItem* a = (const BVItem*)va; - const BVItem* b = (const BVItem*)vb; - if (a->bmin[1] < b->bmin[1]) - return -1; - if (a->bmin[1] > b->bmin[1]) - return 1; - return 0; -} - -static int compareItemZ(const void* va, const void* vb) -{ - const BVItem* a = (const BVItem*)va; - const BVItem* b = (const BVItem*)vb; - if (a->bmin[2] < b->bmin[2]) - return -1; - if (a->bmin[2] > b->bmin[2]) - return 1; - return 0; -} - -static void calcExtends(BVItem* items, const int /*nitems*/, const int imin, const int imax, - unsigned short* bmin, unsigned short* bmax) -{ - bmin[0] = items[imin].bmin[0]; - bmin[1] = items[imin].bmin[1]; - bmin[2] = items[imin].bmin[2]; - - bmax[0] = items[imin].bmax[0]; - bmax[1] = items[imin].bmax[1]; - bmax[2] = items[imin].bmax[2]; - - for (int i = imin+1; i < imax; ++i) - { - const BVItem& it = items[i]; - if (it.bmin[0] < bmin[0]) bmin[0] = it.bmin[0]; - if (it.bmin[1] < bmin[1]) bmin[1] = it.bmin[1]; - if (it.bmin[2] < bmin[2]) bmin[2] = it.bmin[2]; - - if (it.bmax[0] > bmax[0]) bmax[0] = it.bmax[0]; - if (it.bmax[1] > bmax[1]) bmax[1] = it.bmax[1]; - if (it.bmax[2] > bmax[2]) bmax[2] = it.bmax[2]; - } -} - -inline int longestAxis(unsigned short x, unsigned short y, unsigned short z) -{ - int axis = 0; - unsigned short maxVal = x; - if (y > maxVal) - { - axis = 1; - maxVal = y; - } - if (z > maxVal) - { - axis = 2; - maxVal = z; - } - return axis; -} - -static void subdivide(BVItem* items, int nitems, int imin, int imax, int& curNode, dtBVNode* nodes) -{ - int inum = imax - imin; - int icur = curNode; - - dtBVNode& node = nodes[curNode++]; - - if (inum == 1) - { - // Leaf - node.bmin[0] = items[imin].bmin[0]; - node.bmin[1] = items[imin].bmin[1]; - node.bmin[2] = items[imin].bmin[2]; - - node.bmax[0] = items[imin].bmax[0]; - node.bmax[1] = items[imin].bmax[1]; - node.bmax[2] = items[imin].bmax[2]; - - node.i = items[imin].i; - } - else - { - // Split - calcExtends(items, nitems, imin, imax, node.bmin, node.bmax); - - int axis = longestAxis(node.bmax[0] - node.bmin[0], - node.bmax[1] - node.bmin[1], - node.bmax[2] - node.bmin[2]); - - if (axis == 0) - { - // Sort along x-axis - qsort(items+imin, inum, sizeof(BVItem), compareItemX); - } - else if (axis == 1) - { - // Sort along y-axis - qsort(items+imin, inum, sizeof(BVItem), compareItemY); - } - else - { - // Sort along z-axis - qsort(items+imin, inum, sizeof(BVItem), compareItemZ); - } - - int isplit = imin+inum/2; - - // Left - subdivide(items, nitems, imin, isplit, curNode, nodes); - // Right - subdivide(items, nitems, isplit, imax, curNode, nodes); - - int iescape = curNode - icur; - // Negative index means escape. - node.i = -iescape; - } -} - -static int createBVTree(const unsigned short* verts, const int /*nverts*/, - const unsigned short* polys, const int npolys, const int nvp, - const float cs, const float ch, - const int /*nnodes*/, dtBVNode* nodes) -{ - // Build tree - BVItem* items = (BVItem*)dtAlloc(sizeof(BVItem)*npolys, DT_ALLOC_TEMP); - for (int i = 0; i < npolys; i++) - { - BVItem& it = items[i]; - it.i = i; - // Calc polygon bounds. - const unsigned short* p = &polys[i*nvp*2]; - it.bmin[0] = it.bmax[0] = verts[p[0]*3+0]; - it.bmin[1] = it.bmax[1] = verts[p[0]*3+1]; - it.bmin[2] = it.bmax[2] = verts[p[0]*3+2]; - - for (int j = 1; j < nvp; ++j) - { - if (p[j] == MESH_NULL_IDX) break; - unsigned short x = verts[p[j]*3+0]; - unsigned short y = verts[p[j]*3+1]; - unsigned short z = verts[p[j]*3+2]; - - if (x < it.bmin[0]) it.bmin[0] = x; - if (y < it.bmin[1]) it.bmin[1] = y; - if (z < it.bmin[2]) it.bmin[2] = z; - - if (x > it.bmax[0]) it.bmax[0] = x; - if (y > it.bmax[1]) it.bmax[1] = y; - if (z > it.bmax[2]) it.bmax[2] = z; - } - // Remap y - it.bmin[1] = (unsigned short)floorf((float)it.bmin[1]*ch/cs); - it.bmax[1] = (unsigned short)ceilf((float)it.bmax[1]*ch/cs); - } - - int curNode = 0; - subdivide(items, npolys, 0, npolys, curNode, nodes); - - dtFree(items); - - return curNode; -} - -static unsigned char classifyOffMeshPoint(const float* pt, const float* bmin, const float* bmax) -{ - static const unsigned char XP = 1<<0; - static const unsigned char ZP = 1<<1; - static const unsigned char XM = 1<<2; - static const unsigned char ZM = 1<<3; - - unsigned char outcode = 0; - outcode |= (pt[0] >= bmax[0]) ? XP : 0; - outcode |= (pt[2] >= bmax[2]) ? ZP : 0; - outcode |= (pt[0] < bmin[0]) ? XM : 0; - outcode |= (pt[2] < bmin[2]) ? ZM : 0; - - switch (outcode) - { - case XP: return 0; - case XP|ZP: return 1; - case ZP: return 2; - case XM|ZP: return 3; - case XM: return 4; - case XM|ZM: return 5; - case ZM: return 6; - case XP|ZM: return 7; - }; - return 0xff; -} - -// TODO: Better error handling. - -bool dtCreateNavMeshData(dtNavMeshCreateParams* params, unsigned char** outData, int* outDataSize) -{ - if (params->nvp > DT_VERTS_PER_POLYGON) - return false; - if (params->vertCount >= 0xffff) - return false; - if (!params->vertCount || !params->verts) - return false; - if (!params->polyCount || !params->polys) - return false; - //if (!params->detailMeshes || !params->detailVerts || !params->detailTris) - // return false; - - const int nvp = params->nvp; - - // Classify off-mesh connection points. We store only the connections - // whose start point is inside the tile. - unsigned char* offMeshConClass = 0; - int storedOffMeshConCount = 0; - int offMeshConLinkCount = 0; - - if (params->offMeshConCount > 0) - { - offMeshConClass = (unsigned char*)dtAlloc(sizeof(unsigned char)*params->offMeshConCount*2, DT_ALLOC_TEMP); - if (!offMeshConClass) - return false; - - 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); - - // Cound how many links should be allocated for off-mesh connections. - if (offMeshConClass[i*2+0] == 0xff) - offMeshConLinkCount++; - if (offMeshConClass[i*2+1] == 0xff) - offMeshConLinkCount++; - - if (offMeshConClass[i*2+0] == 0xff) - storedOffMeshConCount++; - } - } - - // Off-mesh connectionss are stored as polygons, adjust values. - const int totPolyCount = params->polyCount + storedOffMeshConCount; - const int totVertCount = params->vertCount + storedOffMeshConCount*2; - - // Find portal edges which are at tile borders. - int edgeCount = 0; - int portalCount = 0; - for (int i = 0; i < params->polyCount; ++i) - { - const unsigned short* p = ¶ms->polys[i*2*nvp]; - for (int j = 0; j < nvp; ++j) - { - if (p[j] == MESH_NULL_IDX) break; - int nj = j+1; - if (nj >= nvp || p[nj] == MESH_NULL_IDX) nj = 0; - const unsigned short* va = ¶ms->verts[p[j]*3]; - const unsigned short* vb = ¶ms->verts[p[nj]*3]; - - edgeCount++; - - if (params->tileSize > 0) - { - if (va[0] == params->tileSize && vb[0] == params->tileSize) - portalCount++; // x+ - else if (va[2] == params->tileSize && vb[2] == params->tileSize) - portalCount++; // z+ - else if (va[0] == 0 && vb[0] == 0) - portalCount++; // x- - else if (va[2] == 0 && vb[2] == 0) - portalCount++; // z- - } - } - } - - const int maxLinkCount = edgeCount + portalCount*2 + offMeshConLinkCount*2; - - // Find unique detail vertices. - int uniqueDetailVertCount = 0; - int detailTriCount = 0; - if (params->detailMeshes) - { - // Has detail mesh, count unique detail vertex count and use input detail tri count. - detailTriCount = params->detailTriCount; - for (int i = 0; i < params->polyCount; ++i) - { - const unsigned short* p = ¶ms->polys[i*nvp*2]; - int ndv = params->detailMeshes[i*4+1]; - int nv = 0; - for (int j = 0; j < nvp; ++j) - { - if (p[j] == MESH_NULL_IDX) break; - nv++; - } - ndv -= nv; - uniqueDetailVertCount += ndv; - } - } - else - { - // No input detail mesh, build detail mesh from nav polys. - uniqueDetailVertCount = 0; // No extra detail verts. - detailTriCount = 0; - for (int i = 0; i < params->polyCount; ++i) - { - const unsigned short* p = ¶ms->polys[i*nvp*2]; - int nv = 0; - for (int j = 0; j < nvp; ++j) - { - if (p[j] == MESH_NULL_IDX) break; - nv++; - } - detailTriCount += nv-2; - } - } - - // Calculate data size - const int headerSize = dtAlign4(sizeof(dtMeshHeader)); - const int vertsSize = dtAlign4(sizeof(float)*3*totVertCount); - const int polysSize = dtAlign4(sizeof(dtPoly)*totPolyCount); - const int linksSize = dtAlign4(sizeof(dtLink)*maxLinkCount); - const int detailMeshesSize = dtAlign4(sizeof(dtPolyDetail)*params->polyCount); - const int detailVertsSize = dtAlign4(sizeof(float)*3*uniqueDetailVertCount); - const int detailTrisSize = dtAlign4(sizeof(unsigned char)*4*detailTriCount); - const int bvTreeSize = dtAlign4(sizeof(dtBVNode)*params->polyCount*2); - const int offMeshConsSize = dtAlign4(sizeof(dtOffMeshConnection)*storedOffMeshConCount); - - const int dataSize = headerSize + vertsSize + polysSize + linksSize + - detailMeshesSize + detailVertsSize + detailTrisSize + - bvTreeSize + offMeshConsSize; - - unsigned char* data = (unsigned char*)dtAlloc(sizeof(unsigned char)*dataSize, DT_ALLOC_PERM); - if (!data) - { - dtFree(offMeshConClass); - return false; - } - memset(data, 0, dataSize); - - unsigned char* d = data; - dtMeshHeader* header = (dtMeshHeader*)d; d += headerSize; - float* navVerts = (float*)d; d += vertsSize; - dtPoly* navPolys = (dtPoly*)d; d += polysSize; - d += linksSize; - dtPolyDetail* navDMeshes = (dtPolyDetail*)d; d += detailMeshesSize; - float* navDVerts = (float*)d; d += detailVertsSize; - unsigned char* navDTris = (unsigned char*)d; d += detailTrisSize; - dtBVNode* navBvtree = (dtBVNode*)d; d += bvTreeSize; - dtOffMeshConnection* offMeshCons = (dtOffMeshConnection*)d; d += offMeshConsSize; - - - // Store header - header->magic = DT_NAVMESH_MAGIC; - header->version = DT_NAVMESH_VERSION; - header->x = params->tileX; - header->y = params->tileY; - header->userId = params->userId; - header->polyCount = totPolyCount; - header->vertCount = totVertCount; - header->maxLinkCount = maxLinkCount; - dtVcopy(header->bmin, params->bmin); - dtVcopy(header->bmax, params->bmax); - header->detailMeshCount = params->polyCount; - header->detailVertCount = uniqueDetailVertCount; - header->detailTriCount = detailTriCount; - header->bvQuantFactor = 1.0f / params->cs; - header->offMeshBase = params->polyCount; - header->walkableHeight = params->walkableHeight; - header->walkableRadius = params->walkableRadius; - header->walkableClimb = params->walkableClimb; - header->offMeshConCount = storedOffMeshConCount; - header->bvNodeCount = params->polyCount*2; - - const int offMeshVertsBase = params->vertCount; - const int offMeshPolyBase = params->polyCount; - - // Store vertices - // Mesh vertices - for (int i = 0; i < params->vertCount; ++i) - { - const unsigned short* iv = ¶ms->verts[i*3]; - float* v = &navVerts[i*3]; - v[0] = params->bmin[0] + iv[0] * params->cs; - v[1] = params->bmin[1] + iv[1] * params->ch; - v[2] = params->bmin[2] + iv[2] * params->cs; - } - // Off-mesh link vertices. - int n = 0; - for (int i = 0; i < params->offMeshConCount; ++i) - { - // Only store connections which start from this tile. - if (offMeshConClass[i*2+0] == 0xff) - { - const float* linkv = ¶ms->offMeshConVerts[i*2*3]; - float* v = &navVerts[(offMeshVertsBase + n*2)*3]; - dtVcopy(&v[0], &linkv[0]); - dtVcopy(&v[3], &linkv[3]); - n++; - } - } - - // Store polygons - // Mesh polys - const unsigned short* src = params->polys; - for (int i = 0; i < params->polyCount; ++i) - { - dtPoly* p = &navPolys[i]; - p->vertCount = 0; - p->flags = params->polyFlags[i]; - p->setArea(params->polyAreas[i]); - p->setType(DT_POLYTYPE_GROUND); - for (int j = 0; j < nvp; ++j) - { - if (src[j] == MESH_NULL_IDX) break; - p->verts[j] = src[j]; - p->neis[j] = (src[nvp+j]+1) & 0xffff; - p->vertCount++; - } - src += nvp*2; - } - // Off-mesh connection vertices. - n = 0; - for (int i = 0; i < params->offMeshConCount; ++i) - { - // Only store connections which start from this tile. - if (offMeshConClass[i*2+0] == 0xff) - { - dtPoly* p = &navPolys[offMeshPolyBase+n]; - p->vertCount = 2; - p->verts[0] = (unsigned short)(offMeshVertsBase + n*2+0); - p->verts[1] = (unsigned short)(offMeshVertsBase + n*2+1); - p->flags = params->offMeshConFlags[i]; - p->setArea(params->offMeshConAreas[i]); - p->setType(DT_POLYTYPE_OFFMESH_CONNECTION); - n++; - } - } - - // Store portal edges. - if (params->tileSize > 0) - { - for (int i = 0; i < params->polyCount; ++i) - { - dtPoly* poly = &navPolys[i]; - for (int j = 0; j < poly->vertCount; ++j) - { - int nj = j+1; - if (nj >= poly->vertCount) nj = 0; - - const unsigned short* va = ¶ms->verts[poly->verts[j]*3]; - const unsigned short* vb = ¶ms->verts[poly->verts[nj]*3]; - - if (va[0] == params->tileSize && vb[0] == params->tileSize) // x+ - poly->neis[j] = DT_EXT_LINK | 0; - else if (va[2] == params->tileSize && vb[2] == params->tileSize) // z+ - poly->neis[j] = DT_EXT_LINK | 2; - else if (va[0] == 0 && vb[0] == 0) // x- - poly->neis[j] = DT_EXT_LINK | 4; - else if (va[2] == 0 && vb[2] == 0) // z- - poly->neis[j] = DT_EXT_LINK | 6; - } - } - } - - // Store detail meshes and vertices. - // The nav polygon vertices are stored as the first vertices on each mesh. - // We compress the mesh data by skipping them and using the navmesh coordinates. - if (params->detailMeshes) - { - unsigned short vbase = 0; - for (int i = 0; i < params->polyCount; ++i) - { - dtPolyDetail& dtl = navDMeshes[i]; - const int vb = (int)params->detailMeshes[i*4+0]; - const int ndv = (int)params->detailMeshes[i*4+1]; - const int nv = navPolys[i].vertCount; - dtl.vertBase = (unsigned int)vbase; - dtl.vertCount = (unsigned char)(ndv-nv); - dtl.triBase = (unsigned int)params->detailMeshes[i*4+2]; - dtl.triCount = (unsigned char)params->detailMeshes[i*4+3]; - // Copy vertices except the first 'nv' verts which are equal to nav poly verts. - if (ndv-nv) - { - memcpy(&navDVerts[vbase*3], ¶ms->detailVerts[(vb+nv)*3], sizeof(float)*3*(ndv-nv)); - vbase += (unsigned short)(ndv-nv); - } - } - // Store triangles. - memcpy(navDTris, params->detailTris, sizeof(unsigned char)*4*params->detailTriCount); - } - else - { - // Create dummy detail mesh by triangulating polys. - int tbase = 0; - for (int i = 0; i < params->polyCount; ++i) - { - dtPolyDetail& dtl = navDMeshes[i]; - const int nv = navPolys[i].vertCount; - dtl.vertBase = 0; - dtl.vertCount = 0; - dtl.triBase = (unsigned int)tbase; - dtl.triCount = (unsigned char)(nv-2); - // Triangulate polygon (local indices). - for (int j = 2; j < nv; ++j) - { - unsigned char* t = &navDTris[tbase*4]; - t[0] = 0; - t[1] = (unsigned char)(j-1); - t[2] = (unsigned char)j; - // Bit for each edge that belongs to poly boundary. - t[3] = (1<<2); - if (j == 2) t[3] |= (1<<0); - if (j == nv-1) t[3] |= (1<<4); - tbase++; - } - } - } - - // Store and create BVtree. - // TODO: take detail mesh into account! use byte per bbox extent? - createBVTree(params->verts, params->vertCount, params->polys, params->polyCount, - nvp, params->cs, params->ch, params->polyCount*2, navBvtree); - - // Store Off-Mesh connections. - n = 0; - for (int i = 0; i < params->offMeshConCount; ++i) - { - // Only store connections which start from this tile. - if (offMeshConClass[i*2+0] == 0xff) - { - dtOffMeshConnection* con = &offMeshCons[n]; - con->poly = (unsigned short)(offMeshPolyBase + n); - // Copy connection end-points. - const float* endPts = ¶ms->offMeshConVerts[i*2*3]; - dtVcopy(&con->pos[0], &endPts[0]); - dtVcopy(&con->pos[3], &endPts[3]); - con->rad = params->offMeshConRad[i]; - con->flags = params->offMeshConDir[i] ? DT_OFFMESH_CON_BIDIR : 0; - con->side = offMeshConClass[i*2+1]; - if (params->offMeshConUserID) - con->userId = params->offMeshConUserID[i]; - n++; - } - } - - dtFree(offMeshConClass); - - *outData = data; - *outDataSize = dataSize; - - return true; -} - -inline void swapByte(unsigned char* a, unsigned char* b) -{ - unsigned char tmp = *a; - *a = *b; - *b = tmp; -} - -inline void swapEndian(unsigned short* v) -{ - unsigned char* x = (unsigned char*)v; - swapByte(x+0, x+1); -} - -inline void swapEndian(short* v) -{ - unsigned char* x = (unsigned char*)v; - swapByte(x+0, x+1); -} - -inline void swapEndian(unsigned int* v) -{ - unsigned char* x = (unsigned char*)v; - swapByte(x+0, x+3); swapByte(x+1, x+2); -} - -inline void swapEndian(int* v) -{ - unsigned char* x = (unsigned char*)v; - swapByte(x+0, x+3); swapByte(x+1, x+2); -} - -inline void swapEndian(float* v) -{ - unsigned char* x = (unsigned char*)v; - swapByte(x+0, x+3); swapByte(x+1, x+2); -} - -bool dtNavMeshHeaderSwapEndian(unsigned char* data, const int /*dataSize*/) -{ - dtMeshHeader* header = (dtMeshHeader*)data; - - int swappedMagic = DT_NAVMESH_MAGIC; - int swappedVersion = DT_NAVMESH_VERSION; - swapEndian(&swappedMagic); - swapEndian(&swappedVersion); - - if ((header->magic != DT_NAVMESH_MAGIC || header->version != DT_NAVMESH_VERSION) && - (header->magic != swappedMagic || header->version != swappedVersion)) - { - return false; - } - - swapEndian(&header->magic); - swapEndian(&header->version); - swapEndian(&header->x); - swapEndian(&header->y); - swapEndian(&header->userId); - swapEndian(&header->polyCount); - swapEndian(&header->vertCount); - swapEndian(&header->maxLinkCount); - swapEndian(&header->detailMeshCount); - swapEndian(&header->detailVertCount); - swapEndian(&header->detailTriCount); - swapEndian(&header->bvNodeCount); - swapEndian(&header->offMeshConCount); - swapEndian(&header->offMeshBase); - swapEndian(&header->walkableHeight); - swapEndian(&header->walkableRadius); - swapEndian(&header->walkableClimb); - swapEndian(&header->bmin[0]); - swapEndian(&header->bmin[1]); - swapEndian(&header->bmin[2]); - swapEndian(&header->bmax[0]); - swapEndian(&header->bmax[1]); - swapEndian(&header->bmax[2]); - swapEndian(&header->bvQuantFactor); - - // Freelist index and pointers are updated when tile is added, no need to swap. - - return true; -} - -bool dtNavMeshDataSwapEndian(unsigned char* data, const int /*dataSize*/) -{ - // Make sure the data is in right format. - dtMeshHeader* header = (dtMeshHeader*)data; - if (header->magic != DT_NAVMESH_MAGIC) - return false; - if (header->version != DT_NAVMESH_VERSION) - return false; - - // Patch header pointers. - const int headerSize = dtAlign4(sizeof(dtMeshHeader)); - const int vertsSize = dtAlign4(sizeof(float)*3*header->vertCount); - const int polysSize = dtAlign4(sizeof(dtPoly)*header->polyCount); - const int linksSize = dtAlign4(sizeof(dtLink)*(header->maxLinkCount)); - const int detailMeshesSize = dtAlign4(sizeof(dtPolyDetail)*header->detailMeshCount); - const int detailVertsSize = dtAlign4(sizeof(float)*3*header->detailVertCount); - const int detailTrisSize = dtAlign4(sizeof(unsigned char)*4*header->detailTriCount); - const int bvtreeSize = dtAlign4(sizeof(dtBVNode)*header->bvNodeCount); - const int offMeshLinksSize = dtAlign4(sizeof(dtOffMeshConnection)*header->offMeshConCount); - - unsigned char* d = data + headerSize; - float* verts = (float*)d; d += vertsSize; - dtPoly* polys = (dtPoly*)d; d += polysSize; - /*dtLink* links = (dtLink*)d;*/ d += linksSize; - dtPolyDetail* detailMeshes = (dtPolyDetail*)d; d += detailMeshesSize; - float* detailVerts = (float*)d; d += detailVertsSize; - /*unsigned char* detailTris = (unsigned char*)d;*/ d += detailTrisSize; - dtBVNode* bvTree = (dtBVNode*)d; d += bvtreeSize; - dtOffMeshConnection* offMeshCons = (dtOffMeshConnection*)d; d += offMeshLinksSize; - - // Vertices - for (int i = 0; i < header->vertCount*3; ++i) - { - swapEndian(&verts[i]); - } - - // Polys - for (int i = 0; i < header->polyCount; ++i) - { - dtPoly* p = &polys[i]; - // poly->firstLink is update when tile is added, no need to swap. - for (int j = 0; j < DT_VERTS_PER_POLYGON; ++j) - { - swapEndian(&p->verts[j]); - swapEndian(&p->neis[j]); - } - swapEndian(&p->flags); - } - - // Links are rebuild when tile is added, no need to swap. - - // Detail meshes - for (int i = 0; i < header->detailMeshCount; ++i) - { - dtPolyDetail* pd = &detailMeshes[i]; - swapEndian(&pd->vertBase); - swapEndian(&pd->triBase); - } - - // Detail verts - for (int i = 0; i < header->detailVertCount*3; ++i) - { - swapEndian(&detailVerts[i]); - } - - // BV-tree - for (int i = 0; i < header->bvNodeCount; ++i) - { - dtBVNode* node = &bvTree[i]; - for (int j = 0; j < 3; ++j) - { - swapEndian(&node->bmin[j]); - swapEndian(&node->bmax[j]); - } - swapEndian(&node->i); - } - - // Off-mesh Connections. - for (int i = 0; i < header->offMeshConCount; ++i) - { - dtOffMeshConnection* con = &offMeshCons[i]; - for (int j = 0; j < 6; ++j) - swapEndian(&con->pos[j]); - swapEndian(&con->rad); - swapEndian(&con->poly); - } - - return true; -} diff --git a/deps/recastnavigation/Detour/DetourNavMeshBuilder.h b/deps/recastnavigation/Detour/DetourNavMeshBuilder.h deleted file mode 100644 index aa802d71cb..0000000000 --- a/deps/recastnavigation/Detour/DetourNavMeshBuilder.h +++ /dev/null @@ -1,79 +0,0 @@ -// -// Copyright (c) 2009-2010 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. -// - -#ifndef DETOURNAVMESHBUILDER_H -#define DETOURNAVMESHBUILDER_H - -#include "DetourAlloc.h" - - -// The units of the parameters are specified in parenthesis as follows: -// (vx) voxels, (wu) world units -struct dtNavMeshCreateParams -{ - // Navmesh vertices. - const unsigned short* verts; // Array of vertices, each vertex has 3 components. (vx). - int vertCount; // Vertex count - // Navmesh polygons - const unsigned short* polys; // Array of polygons, uses same format as rcPolyMesh. - const unsigned short* polyFlags; // Array of flags per polygon. - const unsigned char* polyAreas; // Array of area ids per polygon. - int polyCount; // Number of polygons - int nvp; // Number of verts per polygon. - // Navmesh Detail - const unsigned int* detailMeshes; // Detail meshes, uses same format as rcPolyMeshDetail. - const float* detailVerts; // Detail mesh vertices, uses same format as rcPolyMeshDetail (wu). - int detailVertsCount; // Total number of detail vertices - const unsigned char* detailTris; // Array of detail tris per detail mesh. - int detailTriCount; // Total number of detail triangles. - // Off-Mesh Connections. - const float* offMeshConVerts; // Off-mesh connection vertices (wu). - const float* offMeshConRad; // Off-mesh connection radii (wu). - const unsigned short* offMeshConFlags; // Off-mesh connection flags. - const unsigned char* offMeshConAreas; // Off-mesh connection area ids. - const unsigned char* offMeshConDir; // Off-mesh connection direction flags (1 = bidir, 0 = oneway). - const unsigned int* offMeshConUserID; // Off-mesh connection user id (optional). - int offMeshConCount; // Number of off-mesh connections - // Tile location - unsigned int userId; // User ID bound to the tile. - int tileX, tileY; // Tile location (tile coords). - float bmin[3], bmax[3]; // Tile bounds (wu). - // Settings - float walkableHeight; // Agent height (wu). - float walkableRadius; // Agent radius (wu). - float walkableClimb; // Agent max climb (wu). - float cs; // Cell size (xz) (wu). - float ch; // Cell height (y) (wu). - int tileSize; // Tile size (width & height) (vx). - int tileLayer; - bool buildBvTree; -}; - -// Build navmesh data from given input data. -bool dtCreateNavMeshData(dtNavMeshCreateParams* params, unsigned char** outData, int* outDataSize); - -// Swaps endianess of navmesh header. -bool dtNavMeshHeaderSwapEndian(unsigned char* data, const int dataSize); - -// Swaps endianess of the navmesh data. This function assumes that the header is in correct -// endianess already. Call dtNavMeshHeaderSwapEndian() first on the data if the data is -// assumed to be in wrong endianess to start with. If converting from native endianess to foreign, -// call dtNavMeshHeaderSwapEndian() after the data has been swapped. -bool dtNavMeshDataSwapEndian(unsigned char* data, const int dataSize); - -#endif // DETOURNAVMESHBUILDER_H diff --git a/deps/recastnavigation/Detour/DetourNavMeshQuery.cpp b/deps/recastnavigation/Detour/DetourNavMeshQuery.cpp deleted file mode 100644 index 33ee87fb95..0000000000 --- a/deps/recastnavigation/Detour/DetourNavMeshQuery.cpp +++ /dev/null @@ -1,2578 +0,0 @@ -// -// Copyright (c) 2009-2010 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 "DetourNavMeshQuery.h" -#include "DetourNavMesh.h" -#include "DetourNode.h" -#include "DetourCommon.h" -#include "DetourAlloc.h" -#include "DetourAssert.h" -#include <new> - - -dtQueryFilter::dtQueryFilter() : - m_includeFlags(0xffff), - m_excludeFlags(0) -{ - for (int i = 0; i < DT_MAX_AREAS; ++i) - m_areaCost[i] = 1.0f; -} - -#ifdef DT_VIRTUAL_QUERYFILTER -bool dtQueryFilter::passFilter(const dtPolyRef /*ref*/, - const dtMeshTile* /*tile*/, - const dtPoly* poly) const -{ - return (poly->flags & m_includeFlags) != 0 && (poly->flags & m_excludeFlags) == 0; -} - -float dtQueryFilter::getCost(const float* pa, const float* pb, - const dtPolyRef /*prevRef*/, const dtMeshTile* /*prevTile*/, const dtPoly* /*prevPoly*/, - const dtPolyRef /*curRef*/, const dtMeshTile* /*curTile*/, const dtPoly* curPoly, - const dtPolyRef /*nextRef*/, const dtMeshTile* /*nextTile*/, const dtPoly* /*nextPoly*/) const -{ - return dtVdist(pa, pb) * m_areaCost[curPoly->getArea()]; -} -#else -inline bool dtQueryFilter::passFilter(const dtPolyRef /*ref*/, - const dtMeshTile* /*tile*/, - const dtPoly* poly) const -{ - return (poly->flags & m_includeFlags) != 0 && (poly->flags & m_excludeFlags) == 0; -} - -inline float dtQueryFilter::getCost(const float* pa, const float* pb, - const dtPolyRef /*prevRef*/, const dtMeshTile* /*prevTile*/, const dtPoly* /*prevPoly*/, - const dtPolyRef /*curRef*/, const dtMeshTile* /*curTile*/, const dtPoly* curPoly, - const dtPolyRef /*nextRef*/, const dtMeshTile* /*nextTile*/, const dtPoly* /*nextPoly*/) const -{ - return dtVdist(pa, pb) * m_areaCost[curPoly->getArea()]; -} -#endif - -static const float H_SCALE = 2.0f; // Search heuristic scale. - - -dtNavMeshQuery* dtAllocNavMeshQuery() -{ - void* mem = dtAlloc(sizeof(dtNavMeshQuery), DT_ALLOC_PERM); - if (!mem) return 0; - return new(mem) dtNavMeshQuery; -} - -void dtFreeNavMeshQuery(dtNavMeshQuery* navmesh) -{ - if (!navmesh) return; - navmesh->~dtNavMeshQuery(); - dtFree(navmesh); -} - -////////////////////////////////////////////////////////////////////////////////////////// -dtNavMeshQuery::dtNavMeshQuery() : - m_nav(0), - m_tinyNodePool(0), - m_nodePool(0), - m_openList(0) -{ - memset(&m_query, 0, sizeof(dtQueryData)); -} - -dtNavMeshQuery::~dtNavMeshQuery() -{ - if (m_tinyNodePool) - m_tinyNodePool->~dtNodePool(); - if (m_nodePool) - m_nodePool->~dtNodePool(); - if (m_openList) - m_openList->~dtNodeQueue(); - dtFree(m_tinyNodePool); - dtFree(m_nodePool); - dtFree(m_openList); -} - -dtStatus dtNavMeshQuery::init(const dtNavMesh* nav, const int maxNodes) -{ - m_nav = nav; - - if (!m_nodePool || m_nodePool->getMaxNodes() < maxNodes) - { - if (m_nodePool) - { - m_nodePool->~dtNodePool(); - dtFree(m_nodePool); - m_nodePool = 0; - } - m_nodePool = new (dtAlloc(sizeof(dtNodePool), DT_ALLOC_PERM)) dtNodePool(maxNodes, dtNextPow2(maxNodes/4)); - if (!m_nodePool) - return DT_FAILURE_OUT_OF_MEMORY; - } - else - { - m_nodePool->clear(); - } - - if (!m_tinyNodePool) - { - m_tinyNodePool = new (dtAlloc(sizeof(dtNodePool), DT_ALLOC_PERM)) dtNodePool(64, 32); - if (!m_tinyNodePool) - return DT_FAILURE_OUT_OF_MEMORY; - } - else - { - m_tinyNodePool->clear(); - } - - // TODO: check the open list size too. - if (!m_openList || m_openList->getCapacity() < maxNodes) - { - if (m_openList) - { - m_openList->~dtNodeQueue(); - dtFree(m_openList); - m_openList = 0; - } - m_openList = new (dtAlloc(sizeof(dtNodeQueue), DT_ALLOC_PERM)) dtNodeQueue(maxNodes); - if (!m_openList) - return DT_FAILURE_OUT_OF_MEMORY; - } - else - { - m_openList->clear(); - } - - return DT_SUCCESS; -} - -////////////////////////////////////////////////////////////////////////////////////////// -dtStatus dtNavMeshQuery::closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest) const -{ - dtAssert(m_nav); - const dtMeshTile* tile = 0; - const dtPoly* poly = 0; - if (m_nav->getTileAndPolyByRef(ref, &tile, &poly) != DT_SUCCESS) - return DT_FAILURE; - if (!tile) return DT_FAILURE; - - if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) - return DT_FAILURE; - - if (closestPointOnPolyInTile(tile, poly, pos, closest) != DT_SUCCESS) - return DT_FAILURE; - return DT_SUCCESS; -} - -dtStatus dtNavMeshQuery::closestPointOnPolyInTile(const dtMeshTile* tile, const dtPoly* poly, - const float* pos, float* closest) const -{ - const unsigned int ip = (unsigned int)(poly - tile->polys); - const dtPolyDetail* pd = &tile->detailMeshes[ip]; - - // TODO: The commented out version finds 'cylinder distance' instead of 'sphere distance' to the navmesh. - // Test and enable. -/* - // Clamp point to be inside the polygon. - float verts[DT_VERTS_PER_POLYGON*3]; - float edged[DT_VERTS_PER_POLYGON]; - float edget[DT_VERTS_PER_POLYGON]; - const int nv = poly->vertCount; - for (int i = 0; i < nv; ++i) - dtVcopy(&verts[i*3], &tile->verts[poly->verts[i]*3]); - - dtVcopy(closest, pos); - if (!dtDistancePtPolyEdgesSqr(pos, verts, nv, edged, edget)) - { - // Point is outside the polygon, dtClamp to nearest edge. - float dmin = FLT_MAX; - int imin = -1; - for (int i = 0; i < nv; ++i) - { - if (edged[i] < dmin) - { - dmin = edged[i]; - imin = i; - } - } - const float* va = &verts[imin*3]; - const float* vb = &verts[((imin+1)%nv)*3]; - dtVlerp(closest, va, vb, edget[imin]); - } - - // Find height at the location. - for (int j = 0; j < pd->triCount; ++j) - { - const unsigned char* t = &tile->detailTris[(pd->triBase+j)*4]; - const float* v[3]; - for (int k = 0; k < 3; ++k) - { - if (t[k] < poly->vertCount) - v[k] = &tile->verts[poly->verts[t[k]]*3]; - else - v[k] = &tile->detailVerts[(pd->vertBase+(t[k]-poly->vertCount))*3]; - } - float h; - if (dtClosestHeightPointTriangle(pos, v[0], v[1], v[2], h)) - { - closest[1] = h; - break; - } - } -*/ - float closestDistSqr = FLT_MAX; - for (int j = 0; j < pd->triCount; ++j) - { - const unsigned char* t = &tile->detailTris[(pd->triBase+j)*4]; - const float* v[3]; - for (int k = 0; k < 3; ++k) - { - if (t[k] < poly->vertCount) - v[k] = &tile->verts[poly->verts[t[k]]*3]; - else - v[k] = &tile->detailVerts[(pd->vertBase+(t[k]-poly->vertCount))*3]; - } - - float pt[3]; - dtClosestPtPointTriangle(pt, pos, v[0], v[1], v[2]); - float d = dtVdistSqr(pos, pt); - - if (d < closestDistSqr) - { - dtVcopy(closest, pt); - closestDistSqr = d; - } - } - - return DT_SUCCESS; -} - -dtStatus dtNavMeshQuery::closestPointOnPolyBoundary(dtPolyRef ref, const float* pos, float* closest) const -{ - dtAssert(m_nav); - - const dtMeshTile* tile = 0; - const dtPoly* poly = 0; - if (m_nav->getTileAndPolyByRef(ref, &tile, &poly) != DT_SUCCESS) - return DT_FAILURE; - - // Collect vertices. - float verts[DT_VERTS_PER_POLYGON*3]; - float edged[DT_VERTS_PER_POLYGON]; - float edget[DT_VERTS_PER_POLYGON]; - int nv = 0; - for (int i = 0; i < (int)poly->vertCount; ++i) - { - dtVcopy(&verts[nv*3], &tile->verts[poly->verts[i]*3]); - nv++; - } - - bool inside = dtDistancePtPolyEdgesSqr(pos, verts, nv, edged, edget); - if (inside) - { - // Point is inside the polygon, return the point. - dtVcopy(closest, pos); - } - else - { - // Point is outside the polygon, dtClamp to nearest edge. - float dmin = FLT_MAX; - int imin = -1; - for (int i = 0; i < nv; ++i) - { - if (edged[i] < dmin) - { - dmin = edged[i]; - imin = i; - } - } - const float* va = &verts[imin*3]; - const float* vb = &verts[((imin+1)%nv)*3]; - dtVlerp(closest, va, vb, edget[imin]); - } - - return DT_SUCCESS; -} - - -dtStatus dtNavMeshQuery::getPolyHeight(dtPolyRef ref, const float* pos, float* height) const -{ - dtAssert(m_nav); - - const dtMeshTile* tile = 0; - const dtPoly* poly = 0; - if (m_nav->getTileAndPolyByRef(ref, &tile, &poly) != DT_SUCCESS) - return DT_FAILURE; - - 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); - if (height) - *height = v0[1] + (v1[1] - v0[1]) * u; - return DT_SUCCESS; - } - else - { - const unsigned int ip = (unsigned int)(poly - tile->polys); - const dtPolyDetail* pd = &tile->detailMeshes[ip]; - for (int j = 0; j < pd->triCount; ++j) - { - const unsigned char* t = &tile->detailTris[(pd->triBase+j)*4]; - const float* v[3]; - for (int k = 0; k < 3; ++k) - { - if (t[k] < poly->vertCount) - v[k] = &tile->verts[poly->verts[t[k]]*3]; - else - v[k] = &tile->detailVerts[(pd->vertBase+(t[k]-poly->vertCount))*3]; - } - float h; - if (dtClosestHeightPointTriangle(pos, v[0], v[1], v[2], h)) - { - if (height) - *height = h; - return DT_SUCCESS; - } - } - } - - return DT_FAILURE; -} - -dtStatus dtNavMeshQuery::findNearestPoly(const float* center, const float* extents, - const dtQueryFilter* filter, - dtPolyRef* nearestRef, float* nearestPt) const -{ - dtAssert(m_nav); - - *nearestRef = 0; - - // Get nearby polygons from proximity grid. - dtPolyRef polys[128]; - int polyCount = 0; - if (queryPolygons(center, extents, filter, polys, &polyCount, 128) != DT_SUCCESS) - return DT_FAILURE; - - // Find nearest polygon amongst the nearby polygons. - dtPolyRef nearest = 0; - float nearestDistanceSqr = FLT_MAX; - for (int i = 0; i < polyCount; ++i) - { - dtPolyRef ref = polys[i]; - float closestPtPoly[3]; - if (closestPointOnPoly(ref, center, closestPtPoly) != DT_SUCCESS) - continue; - float d = dtVdistSqr(center, closestPtPoly); - if (d < nearestDistanceSqr) - { - if (nearestPt) - dtVcopy(nearestPt, closestPtPoly); - nearestDistanceSqr = d; - nearest = ref; - } - } - - if (nearestRef) - *nearestRef = nearest; - - return DT_SUCCESS; -} - -dtPolyRef dtNavMeshQuery::findNearestPolyInTile(const dtMeshTile* tile, const float* center, const float* extents, - const dtQueryFilter* filter, float* nearestPt) const -{ - dtAssert(m_nav); - - float bmin[3], bmax[3]; - dtVsub(bmin, center, extents); - dtVadd(bmax, center, extents); - - // Get nearby polygons from proximity grid. - dtPolyRef polys[128]; - int polyCount = queryPolygonsInTile(tile, bmin, bmax, filter, polys, 128); - - // Find nearest polygon amongst the nearby polygons. - dtPolyRef nearest = 0; - float nearestDistanceSqr = FLT_MAX; - for (int i = 0; i < polyCount; ++i) - { - dtPolyRef ref = polys[i]; - const dtPoly* poly = &tile->polys[m_nav->decodePolyIdPoly(ref)]; - float closestPtPoly[3]; - if (closestPointOnPolyInTile(tile, poly, center, closestPtPoly) != DT_SUCCESS) - continue; - - float d = dtVdistSqr(center, closestPtPoly); - if (d < nearestDistanceSqr) - { - if (nearestPt) - dtVcopy(nearestPt, closestPtPoly); - nearestDistanceSqr = d; - nearest = ref; - } - } - - return nearest; -} - -int dtNavMeshQuery::queryPolygonsInTile(const dtMeshTile* tile, const float* qmin, const float* qmax, - const dtQueryFilter* filter, - dtPolyRef* polys, const int maxPolys) const -{ - dtAssert(m_nav); - - if (tile->bvTree) - { - const dtBVNode* node = &tile->bvTree[0]; - const dtBVNode* end = &tile->bvTree[tile->header->bvNodeCount]; - const float* tbmin = tile->header->bmin; - const float* tbmax = tile->header->bmax; - const float qfac = tile->header->bvQuantFactor; - - // Calculate quantized box - unsigned short bmin[3], bmax[3]; - // dtClamp query box to world box. - float minx = dtClamp(qmin[0], tbmin[0], tbmax[0]) - tbmin[0]; - float miny = dtClamp(qmin[1], tbmin[1], tbmax[1]) - tbmin[1]; - float minz = dtClamp(qmin[2], tbmin[2], tbmax[2]) - tbmin[2]; - float maxx = dtClamp(qmax[0], tbmin[0], tbmax[0]) - tbmin[0]; - float maxy = dtClamp(qmax[1], tbmin[1], tbmax[1]) - tbmin[1]; - float maxz = dtClamp(qmax[2], tbmin[2], tbmax[2]) - tbmin[2]; - // Quantize - bmin[0] = (unsigned short)(qfac * minx) & 0xfffe; - bmin[1] = (unsigned short)(qfac * miny) & 0xfffe; - bmin[2] = (unsigned short)(qfac * minz) & 0xfffe; - bmax[0] = (unsigned short)(qfac * maxx + 1) | 1; - bmax[1] = (unsigned short)(qfac * maxy + 1) | 1; - bmax[2] = (unsigned short)(qfac * maxz + 1) | 1; - - // Traverse tree - const dtPolyRef base = m_nav->getPolyRefBase(tile); - int n = 0; - while (node < end) - { - const bool overlap = dtOverlapQuantBounds(bmin, bmax, node->bmin, node->bmax); - const bool isLeafNode = node->i >= 0; - - if (isLeafNode && overlap) - { - dtPolyRef ref = base | (dtPolyRef)node->i; - if (filter->passFilter(ref, tile, &tile->polys[node->i])) - { - if (n < maxPolys) - polys[n++] = ref; - } - } - - if (overlap || isLeafNode) - node++; - else - { - const int escapeIndex = -node->i; - node += escapeIndex; - } - } - - return n; - } - else - { - float bmin[3], bmax[3]; - int n = 0; - const dtPolyRef base = m_nav->getPolyRefBase(tile); - for (int i = 0; i < tile->header->polyCount; ++i) - { - // Calc polygon bounds. - dtPoly* p = &tile->polys[i]; - // Do not return off-mesh connection polygons. - if (p->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) - continue; - - const float* v = &tile->verts[p->verts[0]*3]; - dtVcopy(bmin, v); - dtVcopy(bmax, v); - for (int j = 1; j < p->vertCount; ++j) - { - v = &tile->verts[p->verts[j]*3]; - dtVmin(bmin, v); - dtVmax(bmax, v); - } - if (dtOverlapBounds(qmin,qmax, bmin,bmax)) - { - const dtPolyRef ref = base | (dtPolyRef)i; - if (filter->passFilter(ref, tile, p)) - { - if (n < maxPolys) - polys[n++] = ref; - } - } - } - return n; - } -} - -dtStatus dtNavMeshQuery::queryPolygons(const float* center, const float* extents, - const dtQueryFilter* filter, - dtPolyRef* polys, int* polyCount, const int maxPolys) const -{ - dtAssert(m_nav); - - float bmin[3], bmax[3]; - dtVsub(bmin, center, extents); - dtVadd(bmax, center, extents); - - // Find tiles the query touches. - int minx, miny, maxx, maxy; - m_nav->calcTileLoc(bmin, &minx, &miny); - m_nav->calcTileLoc(bmax, &maxx, &maxy); - - int n = 0; - - /// pussywizard: additional checks as in PathGenerator::HaveTile - if (minx < 0) minx = 0; if (miny < 0) miny = 0; // min can be negative because we subtract extents (few lines above) - if (maxx < 0 || maxy < 0 || maxx-minx >= 64 /*MAX_NUMBER_OF_GRIDS*/ || maxy-miny >= 64) // max should never be negative - { - *polyCount = n; - return DT_SUCCESS; - } - - for (int y = miny; y <= maxy; ++y) - { - for (int x = minx; x <= maxx; ++x) - { - const dtMeshTile* tile = m_nav->getTileAt(x,y); - if (!tile) continue; - n += queryPolygonsInTile(tile, bmin, bmax, filter, polys+n, maxPolys-n); - if (n >= maxPolys) - { - *polyCount = n; - return DT_SUCCESS; - } - } - } - *polyCount = n; - - return DT_SUCCESS; -} - -dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef, - const float* startPos, const float* endPos, - const dtQueryFilter* filter, - dtPolyRef* path, int* pathCount, const int maxPath) const -{ - dtAssert(m_nav); - dtAssert(m_nodePool); - dtAssert(m_openList); - - *pathCount = 0; - - if (!startRef || !endRef) - return DT_FAILURE; - - if (!maxPath) - return DT_FAILURE; - - // Validate input - if (!m_nav->isValidPolyRef(startRef) || !m_nav->isValidPolyRef(endRef)) - return DT_FAILURE; - - if (startRef == endRef) - { - path[0] = startRef; - *pathCount = 1; - return DT_SUCCESS; - } - - m_nodePool->clear(); - m_openList->clear(); - - dtNode* startNode = m_nodePool->getNode(startRef); - dtVcopy(startNode->pos, startPos); - startNode->pidx = 0; - startNode->cost = 0; - startNode->total = dtVdist(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()) - { - // Remove node from open list and put it in closed list. - dtNode* bestNode = m_openList->pop(); - bestNode->flags &= ~DT_NODE_OPEN; - bestNode->flags |= DT_NODE_CLOSED; - - // Reached the goal, stop searching. - if (bestNode->id == endRef) - { - lastBestNode = bestNode; - break; - } - - // Get current poly and tile. - // The API input has been cheked already, skip checking internal data. - const dtPolyRef bestRef = bestNode->id; - const dtMeshTile* bestTile = 0; - const dtPoly* bestPoly = 0; - m_nav->getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly); - - // Get parent poly and tile. - dtPolyRef parentRef = 0; - const dtMeshTile* parentTile = 0; - const dtPoly* parentPoly = 0; - if (bestNode->pidx) - parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id; - if (parentRef) - m_nav->getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly); - - for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next) - { - dtPolyRef neighbourRef = bestTile->links[i].ref; - - // Skip invalid ids and do not expand back to where we came from. - if (!neighbourRef || neighbourRef == parentRef) - continue; - - // Get neighbour poly and tile. - // The API input has been cheked already, skip checking internal data. - const dtMeshTile* neighbourTile = 0; - const dtPoly* neighbourPoly = 0; - m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly); - - if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly)) - continue; - - dtNode* neighbourNode = m_nodePool->getNode(neighbourRef); - if (!neighbourNode) - continue; - - // If the node is visited the first time, calculate node position. - if (neighbourNode->flags == 0) - { - getEdgeMidPoint(bestRef, bestPoly, bestTile, - neighbourRef, neighbourPoly, neighbourTile, - neighbourNode->pos); - } - - // Calculate cost and heuristic. - float cost = 0; - float heuristic = 0; - - // Special case for last node. - if (neighbourRef == endRef) - { - // Cost - const float curCost = filter->getCost(bestNode->pos, neighbourNode->pos, - parentRef, parentTile, parentPoly, - bestRef, bestTile, bestPoly, - neighbourRef, neighbourTile, neighbourPoly); - const float endCost = filter->getCost(neighbourNode->pos, endPos, - bestRef, bestTile, bestPoly, - neighbourRef, neighbourTile, neighbourPoly, - 0, 0, 0); - - cost = bestNode->cost + curCost + endCost; - heuristic = 0; - } - else - { - // Cost - const float curCost = filter->getCost(bestNode->pos, neighbourNode->pos, - parentRef, parentTile, parentPoly, - bestRef, bestTile, bestPoly, - neighbourRef, neighbourTile, neighbourPoly); - cost = bestNode->cost + curCost; - heuristic = dtVdist(neighbourNode->pos, endPos)*H_SCALE; - } - - const float total = cost + heuristic; - - // The node is already in open list and the new result is worse, skip. - if ((neighbourNode->flags & DT_NODE_OPEN) && total >= neighbourNode->total) - continue; - // The node is already visited and process, and the new result is worse, skip. - if ((neighbourNode->flags & DT_NODE_CLOSED) && total >= neighbourNode->total) - continue; - - // Add or update the node. - neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode); - neighbourNode->id = neighbourRef; - neighbourNode->flags &= ~DT_NODE_CLOSED; - neighbourNode->cost = cost; - neighbourNode->total = total; - - if (neighbourNode->flags & DT_NODE_OPEN) - { - // Already in open, update node location. - m_openList->modify(neighbourNode); - } - else - { - // Put the node in open list. - neighbourNode->flags |= DT_NODE_OPEN; - m_openList->push(neighbourNode); - } - - // Update nearest node to target so far. - if (heuristic < lastBestNodeCost) - { - lastBestNodeCost = heuristic; - lastBestNode = neighbourNode; - } - } - } - - // 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 < maxPath); - - *pathCount = n; - - return DT_SUCCESS; -} - -dtStatus dtNavMeshQuery::initSlicedFindPath(dtPolyRef startRef, dtPolyRef endRef, - const float* startPos, const float* endPos, - const dtQueryFilter* filter) -{ - dtAssert(m_nav); - dtAssert(m_nodePool); - dtAssert(m_openList); - - // Init path state. - memset(&m_query, 0, sizeof(dtQueryData)); - m_query.status = DT_FAILURE; - m_query.startRef = startRef; - m_query.endRef = endRef; - dtVcopy(m_query.startPos, startPos); - dtVcopy(m_query.endPos, endPos); - m_query.filter = filter; - - if (!startRef || !endRef) - return DT_FAILURE; - - // Validate input - if (!m_nav->isValidPolyRef(startRef) || !m_nav->isValidPolyRef(endRef)) - return DT_FAILURE; - - if (startRef == endRef) - { - m_query.status = DT_SUCCESS; - return DT_SUCCESS; - } - - m_nodePool->clear(); - m_openList->clear(); - - dtNode* startNode = m_nodePool->getNode(startRef); - dtVcopy(startNode->pos, startPos); - startNode->pidx = 0; - startNode->cost = 0; - startNode->total = dtVdist(startPos, endPos) * H_SCALE; - startNode->id = startRef; - startNode->flags = DT_NODE_OPEN; - m_openList->push(startNode); - - m_query.status = DT_IN_PROGRESS; - m_query.lastBestNode = startNode; - m_query.lastBestNodeCost = startNode->total; - - return m_query.status; -} - -dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter) -{ - if (m_query.status!= DT_IN_PROGRESS) - return m_query.status; - - // Make sure the request is still valid. - if (!m_nav->isValidPolyRef(m_query.startRef) || !m_nav->isValidPolyRef(m_query.endRef)) - { - m_query.status = DT_FAILURE; - return DT_FAILURE; - } - - int iter = 0; - while (iter < maxIter && !m_openList->empty()) - { - iter++; - - // Remove node from open list and put it in closed list. - dtNode* bestNode = m_openList->pop(); - bestNode->flags &= ~DT_NODE_OPEN; - bestNode->flags |= DT_NODE_CLOSED; - - // Reached the goal, stop searching. - if (bestNode->id == m_query.endRef) - { - m_query.lastBestNode = bestNode; - m_query.status = DT_SUCCESS; - return m_query.status; - } - - // Get current poly and tile. - // The API input has been cheked already, skip checking internal data. - const dtPolyRef bestRef = bestNode->id; - const dtMeshTile* bestTile = 0; - const dtPoly* bestPoly = 0; - if (m_nav->getTileAndPolyByRef(bestRef, &bestTile, &bestPoly) != DT_SUCCESS) - { - // The polygon has disappeared during the sliced query, fail. - m_query.status = DT_FAILURE; - return m_query.status; - } - - // Get parent poly and tile. - dtPolyRef parentRef = 0; - const dtMeshTile* parentTile = 0; - const dtPoly* parentPoly = 0; - if (bestNode->pidx) - parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id; - if (parentRef) - { - if (m_nav->getTileAndPolyByRef(parentRef, &parentTile, &parentPoly) != DT_SUCCESS) - { - // The polygon has disappeared during the sliced query, fail. - m_query.status = DT_FAILURE; - return m_query.status; - } - } - - for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next) - { - dtPolyRef neighbourRef = bestTile->links[i].ref; - - // Skip invalid ids and do not expand back to where we came from. - if (!neighbourRef || neighbourRef == parentRef) - continue; - - // Get neighbour poly and tile. - // The API input has been cheked already, skip checking internal data. - const dtMeshTile* neighbourTile = 0; - const dtPoly* neighbourPoly = 0; - m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly); - - if (!m_query.filter->passFilter(neighbourRef, neighbourTile, neighbourPoly)) - continue; - - dtNode* neighbourNode = m_nodePool->getNode(neighbourRef); - if (!neighbourNode) - continue; - - // If the node is visited the first time, calculate node position. - if (neighbourNode->flags == 0) - { - getEdgeMidPoint(bestRef, bestPoly, bestTile, - neighbourRef, neighbourPoly, neighbourTile, - neighbourNode->pos); - } - - // Calculate cost and heuristic. - float cost = 0; - float heuristic = 0; - - // Special case for last node. - if (neighbourRef == m_query.endRef) - { - // Cost - const float curCost = m_query.filter->getCost(bestNode->pos, neighbourNode->pos, - parentRef, parentTile, parentPoly, - bestRef, bestTile, bestPoly, - neighbourRef, neighbourTile, neighbourPoly); - const float endCost = m_query.filter->getCost(neighbourNode->pos, m_query.endPos, - bestRef, bestTile, bestPoly, - neighbourRef, neighbourTile, neighbourPoly, - 0, 0, 0); - - cost = bestNode->cost + curCost + endCost; - heuristic = 0; - } - else - { - // Cost - const float curCost = m_query.filter->getCost(bestNode->pos, neighbourNode->pos, - parentRef, parentTile, parentPoly, - bestRef, bestTile, bestPoly, - neighbourRef, neighbourTile, neighbourPoly); - cost = bestNode->cost + curCost; - heuristic = dtVdist(neighbourNode->pos, m_query.endPos)*H_SCALE; - } - - const float total = cost + heuristic; - - // The node is already in open list and the new result is worse, skip. - if ((neighbourNode->flags & DT_NODE_OPEN) && total >= neighbourNode->total) - continue; - // The node is already visited and process, and the new result is worse, skip. - if ((neighbourNode->flags & DT_NODE_CLOSED) && total >= neighbourNode->total) - continue; - - // Add or update the node. - neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode); - neighbourNode->id = neighbourRef; - neighbourNode->flags &= ~DT_NODE_CLOSED; - neighbourNode->cost = cost; - neighbourNode->total = total; - - if (neighbourNode->flags & DT_NODE_OPEN) - { - // Already in open, update node location. - m_openList->modify(neighbourNode); - } - else - { - // Put the node in open list. - neighbourNode->flags |= DT_NODE_OPEN; - m_openList->push(neighbourNode); - } - - // Update nearest node to target so far. - if (heuristic < m_query.lastBestNodeCost) - { - m_query.lastBestNodeCost = heuristic; - m_query.lastBestNode = neighbourNode; - } - } - } - - // Exhausted all nodes, but could not find path. - if (m_openList->empty()) - m_query.status = DT_SUCCESS; - - return m_query.status; -} - -dtStatus dtNavMeshQuery::finalizeSlicedFindPath(dtPolyRef* path, int* pathCount, const int maxPath) -{ - *pathCount = 0; - - if (m_query.status != DT_SUCCESS) - { - // Reset query. - memset(&m_query, 0, sizeof(dtQueryData)); - return DT_FAILURE; - } - - int n = 0; - - if (m_query.startRef == m_query.endRef) - { - // Special case: the search starts and ends at same poly. - path[n++] = m_query.startRef; - } - else - { - // Reverse the path. - dtAssert(m_query.lastBestNode); - dtNode* prev = 0; - dtNode* node = m_query.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; - do - { - path[n++] = node->id; - node = m_nodePool->getNodeAtIdx(node->pidx); - } - while (node && n < maxPath); - } - - // Reset query. - memset(&m_query, 0, sizeof(dtQueryData)); - - *pathCount = n; - - return DT_SUCCESS; -} - -dtStatus dtNavMeshQuery::finalizeSlicedFindPathPartial(const dtPolyRef* existing, const int existingSize, - dtPolyRef* path, int* pathCount, const int maxPath) -{ - *pathCount = 0; - - if (existingSize == 0) - { - return DT_FAILURE; - } - - if (m_query.status != DT_SUCCESS && m_query.status != DT_IN_PROGRESS) - { - // Reset query. - memset(&m_query, 0, sizeof(dtQueryData)); - return DT_FAILURE; - } - - int n = 0; - - if (m_query.startRef == m_query.endRef) - { - // Special case: the search starts and ends at same poly. - path[n++] = m_query.startRef; - } - else - { - // Find furthest existing node that was visited. - dtNode* prev = 0; - dtNode* node = 0; - for (int i = existingSize-1; i >= 0; --i) - { - node = m_nodePool->findNode(existing[i]); - if (node) - break; - } - - if (!node) - { - return DT_FAILURE; - } - - // Reverse the path. - do - { - dtNode* next = m_nodePool->getNodeAtIdx(node->pidx); - node->pidx = m_nodePool->getNodeIdx(prev); - prev = node; - node = next; - } - while (node); - - // Store path - node = prev; - do - { - path[n++] = node->id; - node = m_nodePool->getNodeAtIdx(node->pidx); - } - while (node && n < maxPath); - } - - // Reset query. - memset(&m_query, 0, sizeof(dtQueryData)); - - *pathCount = n; - - return DT_SUCCESS; -} - - -dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* endPos, - const dtPolyRef* path, const int pathSize, - float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs, - int* straightPathCount, const int maxStraightPath) const -{ - dtAssert(m_nav); - - *straightPathCount = 0; - - if (!maxStraightPath) - return DT_FAILURE; - - if (!path[0]) - return DT_FAILURE; - - int n = 0; - - // TODO: Should this be callers responsibility? - float closestStartPos[3]; - if (closestPointOnPolyBoundary(path[0], startPos, closestStartPos) != DT_SUCCESS) - return DT_FAILURE; - - // Add start point. - dtVcopy(&straightPath[n*3], closestStartPos); - if (straightPathFlags) - straightPathFlags[n] = DT_STRAIGHTPATH_START; - if (straightPathRefs) - straightPathRefs[n] = path[0]; - n++; - if (n >= maxStraightPath) - { - *straightPathCount = n; - return DT_SUCCESS; - } - - float closestEndPos[3]; - if (closestPointOnPolyBoundary(path[pathSize-1], endPos, closestEndPos) != DT_SUCCESS) - return DT_FAILURE; - - if (pathSize > 1) - { - float portalApex[3], portalLeft[3], portalRight[3]; - dtVcopy(portalApex, closestStartPos); - dtVcopy(portalLeft, portalApex); - dtVcopy(portalRight, portalApex); - int apexIndex = 0; - int leftIndex = 0; - int rightIndex = 0; - - unsigned char leftPolyType = 0; - unsigned char rightPolyType = 0; - - dtPolyRef leftPolyRef = path[0]; - dtPolyRef rightPolyRef = path[0]; - - for (int i = 0; i < pathSize; ++i) - { - float left[3], right[3]; - unsigned char fromType, toType; - - if (i+1 < pathSize) - { - // Next portal. - if (getPortalPoints(path[i], path[i+1], left, right, fromType, toType) != DT_SUCCESS) - { - if (closestPointOnPolyBoundary(path[i], endPos, closestEndPos) != DT_SUCCESS) - return DT_FAILURE; - - dtVcopy(&straightPath[n*3], closestEndPos); - if (straightPathFlags) - straightPathFlags[n] = 0; - if (straightPathRefs) - straightPathRefs[n] = path[i]; - n++; - - return DT_SUCCESS; - } - - // If starting really close the portal, advance. - if (i == 0) - { - float t; - if (dtDistancePtSegSqr2D(portalApex, left, right, t) < dtSqr(0.001f)) - continue; - } - } - else - { - // End of the path. - dtVcopy(left, closestEndPos); - dtVcopy(right, closestEndPos); - - fromType = toType = DT_POLYTYPE_GROUND; - } - - // Right vertex. - if (dtTriArea2D(portalApex, portalRight, right) <= 0.0f) - { - if (dtVequal(portalApex, portalRight) || dtTriArea2D(portalApex, portalLeft, right) > 0.0f) - { - dtVcopy(portalRight, right); - rightPolyRef = (i+1 < pathSize) ? path[i+1] : 0; - rightPolyType = toType; - rightIndex = i; - } - else - { - dtVcopy(portalApex, portalLeft); - apexIndex = leftIndex; - - unsigned char flags = 0; - if (!leftPolyRef) - flags = DT_STRAIGHTPATH_END; - else if (leftPolyType == DT_POLYTYPE_OFFMESH_CONNECTION) - flags = DT_STRAIGHTPATH_OFFMESH_CONNECTION; - dtPolyRef ref = leftPolyRef; - - if (!dtVequal(&straightPath[(n-1)*3], portalApex)) - { - // Append new vertex. - dtVcopy(&straightPath[n*3], portalApex); - if (straightPathFlags) - straightPathFlags[n] = flags; - if (straightPathRefs) - straightPathRefs[n] = ref; - n++; - // If reached end of path or there is no space to append more vertices, return. - if (flags == DT_STRAIGHTPATH_END || n >= maxStraightPath) - { - *straightPathCount = n; - return DT_SUCCESS; - } - } - else - { - // The vertices are equal, update flags and poly. - if (straightPathFlags) - straightPathFlags[n-1] = flags; - if (straightPathRefs) - straightPathRefs[n-1] = ref; - } - - dtVcopy(portalLeft, portalApex); - dtVcopy(portalRight, portalApex); - leftIndex = apexIndex; - rightIndex = apexIndex; - - // Restart - i = apexIndex; - - continue; - } - } - - // Left vertex. - if (dtTriArea2D(portalApex, portalLeft, left) >= 0.0f) - { - if (dtVequal(portalApex, portalLeft) || dtTriArea2D(portalApex, portalRight, left) < 0.0f) - { - dtVcopy(portalLeft, left); - leftPolyRef = (i+1 < pathSize) ? path[i+1] : 0; - leftPolyType = toType; - leftIndex = i; - } - else - { - dtVcopy(portalApex, portalRight); - apexIndex = rightIndex; - - unsigned char flags = 0; - if (!rightPolyRef) - flags = DT_STRAIGHTPATH_END; - else if (rightPolyType == DT_POLYTYPE_OFFMESH_CONNECTION) - flags = DT_STRAIGHTPATH_OFFMESH_CONNECTION; - dtPolyRef ref = rightPolyRef; - - if (!dtVequal(&straightPath[(n-1)*3], portalApex)) - { - // Append new vertex. - dtVcopy(&straightPath[n*3], portalApex); - if (straightPathFlags) - straightPathFlags[n] = flags; - if (straightPathRefs) - straightPathRefs[n] = ref; - n++; - // If reached end of path or there is no space to append more vertices, return. - if (flags == DT_STRAIGHTPATH_END || n >= maxStraightPath) - { - *straightPathCount = n; - return DT_SUCCESS; - } - } - else - { - // The vertices are equal, update flags and poly. - if (straightPathFlags) - straightPathFlags[n-1] = flags; - if (straightPathRefs) - straightPathRefs[n-1] = ref; - } - - dtVcopy(portalLeft, portalApex); - dtVcopy(portalRight, portalApex); - leftIndex = apexIndex; - rightIndex = apexIndex; - - // Restart - i = apexIndex; - - continue; - } - } - } - } - - // If the point already exists, remove it and add reappend the actual end location. - if (n > 0 && dtVequal(&straightPath[(n-1)*3], closestEndPos)) - n--; - - // Add end point. - if (n < maxStraightPath) - { - dtVcopy(&straightPath[n*3], closestEndPos); - if (straightPathFlags) - straightPathFlags[n] = DT_STRAIGHTPATH_END; - if (straightPathRefs) - straightPathRefs[n] = 0; - n++; - } - - *straightPathCount = n; - return DT_SUCCESS; -} - -dtStatus dtNavMeshQuery::moveAlongSurface(dtPolyRef startRef, const float* startPos, const float* endPos, - const dtQueryFilter* filter, - float* resultPos, dtPolyRef* visited, int* visitedCount, const int maxVisitedSize) const -{ - dtAssert(m_nav); - dtAssert(m_tinyNodePool); - - *visitedCount = 0; - - // Validate input - if (!startRef) return DT_FAILURE; - if (!m_nav->isValidPolyRef(startRef)) return DT_FAILURE; - - static const int MAX_STACK = 48; - dtNode* stack[MAX_STACK]; - int nstack = 0; - - m_tinyNodePool->clear(); - - dtNode* startNode = m_tinyNodePool->getNode(startRef); - startNode->pidx = 0; - startNode->cost = 0; - startNode->total = 0; - startNode->id = startRef; - startNode->flags = DT_NODE_CLOSED; - stack[nstack++] = startNode; - - float bestPos[3]; - float bestDist = FLT_MAX; - dtNode* bestNode = 0; - dtVcopy(bestPos, startPos); - - // Search constraints - float searchPos[3], searchRadSqr; - dtVlerp(searchPos, startPos, endPos, 0.5f); - searchRadSqr = dtSqr(dtVdist(startPos, endPos)/2.0f + 0.001f); - - float verts[DT_VERTS_PER_POLYGON*3]; - - while (nstack) - { - // Pop front. - dtNode* curNode = stack[0]; - for (int i = 0; i < nstack-1; ++i) - stack[i] = stack[i+1]; - nstack--; - - // Get poly and tile. - // The API input has been cheked already, skip checking internal data. - const dtPolyRef curRef = curNode->id; - const dtMeshTile* curTile = 0; - const dtPoly* curPoly = 0; - m_nav->getTileAndPolyByRefUnsafe(curRef, &curTile, &curPoly); - - // Collect vertices. - const int nverts = curPoly->vertCount; - for (int i = 0; i < nverts; ++i) - dtVcopy(&verts[i*3], &curTile->verts[curPoly->verts[i]*3]); - - // If target is inside the poly, stop search. - if (dtPointInPolygon(endPos, verts, nverts)) - { - bestNode = curNode; - dtVcopy(bestPos, endPos); - break; - } - - // Find wall edges and find nearest point inside the walls. - for (int i = 0, j = (int)curPoly->vertCount-1; i < (int)curPoly->vertCount; j = i++) - { - // Find links to neighbours. - static const int MAX_NEIS = 8; - int nneis = 0; - dtPolyRef neis[MAX_NEIS]; - - if (curPoly->neis[j] & DT_EXT_LINK) - { - // Tile border. - for (unsigned int k = curPoly->firstLink; k != DT_NULL_LINK; k = curTile->links[k].next) - { - const dtLink* link = &curTile->links[k]; - if (link->edge == j) - { - if (link->ref != 0) - { - const dtMeshTile* neiTile = 0; - const dtPoly* neiPoly = 0; - m_nav->getTileAndPolyByRefUnsafe(link->ref, &neiTile, &neiPoly); - if (filter->passFilter(link->ref, neiTile, neiPoly)) - { - if (nneis < MAX_NEIS) - neis[nneis++] = link->ref; - } - } - } - } - } - else if (curPoly->neis[j]) - { - const unsigned int idx = (unsigned int)(curPoly->neis[j]-1); - const dtPolyRef ref = m_nav->getPolyRefBase(curTile) | idx; - if (filter->passFilter(ref, curTile, &curTile->polys[idx])) - { - // Internal edge, encode id. - neis[nneis++] = ref; - } - } - - if (!nneis) - { - // Wall edge, calc distance. - const float* vj = &verts[j*3]; - const float* vi = &verts[i*3]; - float tseg; - const float distSqr = dtDistancePtSegSqr2D(endPos, vj, vi, tseg); - if (distSqr < bestDist) - { - // Update nearest distance. - dtVlerp(bestPos, vj,vi, tseg); - bestDist = distSqr; - bestNode = curNode; - } - } - else - { - for (int k = 0; k < nneis; ++k) - { - // Skip if no node can be allocated. - dtNode* neighbourNode = m_tinyNodePool->getNode(neis[k]); - if (!neighbourNode) - continue; - // Skip if already visited. - if (neighbourNode->flags & DT_NODE_CLOSED) - continue; - - // Skip the link if it is too far from search constraint. - // TODO: Maybe should use getPortalPoints(), but this one is way faster. - const float* vj = &verts[j*3]; - const float* vi = &verts[i*3]; - float tseg; - float distSqr = dtDistancePtSegSqr2D(searchPos, vj, vi, tseg); - if (distSqr > searchRadSqr) - continue; - - // Mark as the node as visited and push to queue. - if (nstack < MAX_STACK) - { - neighbourNode->pidx = m_tinyNodePool->getNodeIdx(curNode); - neighbourNode->flags |= DT_NODE_CLOSED; - stack[nstack++] = neighbourNode; - } - } - } - } - } - - int n = 0; - if (bestNode) - { - // Reverse the path. - dtNode* prev = 0; - dtNode* node = bestNode; - do - { - dtNode* next = m_tinyNodePool->getNodeAtIdx(node->pidx); - node->pidx = m_tinyNodePool->getNodeIdx(prev); - prev = node; - node = next; - } - while (node); - - // Store result - node = prev; - do - { - visited[n++] = node->id; - node = m_tinyNodePool->getNodeAtIdx(node->pidx); - } - while (node && n < maxVisitedSize); - } - - dtVcopy(resultPos, bestPos); - - *visitedCount = n; - - return DT_SUCCESS; -} - - -dtStatus dtNavMeshQuery::getPortalPoints(dtPolyRef from, dtPolyRef to, float* left, float* right, - unsigned char& fromType, unsigned char& toType) const -{ - dtAssert(m_nav); - - const dtMeshTile* fromTile = 0; - const dtPoly* fromPoly = 0; - if (m_nav->getTileAndPolyByRef(from, &fromTile, &fromPoly) != DT_SUCCESS) - return DT_FAILURE; - fromType = fromPoly->getType(); - - const dtMeshTile* toTile = 0; - const dtPoly* toPoly = 0; - if (m_nav->getTileAndPolyByRef(to, &toTile, &toPoly) != DT_SUCCESS) - return DT_FAILURE; - toType = toPoly->getType(); - - return getPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, left, right); -} - -// Returns portal points between two polygons. -dtStatus dtNavMeshQuery::getPortalPoints(dtPolyRef from, const dtPoly* fromPoly, const dtMeshTile* fromTile, - dtPolyRef to, const dtPoly* toPoly, const dtMeshTile* toTile, - float* left, float* right) const -{ - // Find the link that points to the 'to' polygon. - const dtLink* link = 0; - for (unsigned int i = fromPoly->firstLink; i != DT_NULL_LINK; i = fromTile->links[i].next) - { - if (fromTile->links[i].ref == to) - { - link = &fromTile->links[i]; - break; - } - } - if (!link) - return DT_FAILURE; - - // Handle off-mesh connections. - if (fromPoly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) - { - // Find link that points to first vertex. - for (unsigned int i = fromPoly->firstLink; i != DT_NULL_LINK; i = fromTile->links[i].next) - { - if (fromTile->links[i].ref == to) - { - const int v = fromTile->links[i].edge; - dtVcopy(left, &fromTile->verts[fromPoly->verts[v]*3]); - dtVcopy(right, &fromTile->verts[fromPoly->verts[v]*3]); - return DT_SUCCESS; - } - } - return DT_FAILURE; - } - - if (toPoly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) - { - for (unsigned int i = toPoly->firstLink; i != DT_NULL_LINK; i = toTile->links[i].next) - { - if (toTile->links[i].ref == from) - { - const int v = toTile->links[i].edge; - dtVcopy(left, &toTile->verts[toPoly->verts[v]*3]); - dtVcopy(right, &toTile->verts[toPoly->verts[v]*3]); - return DT_SUCCESS; - } - } - return DT_FAILURE; - } - - // Find portal vertices. - const int v0 = fromPoly->verts[link->edge]; - const int v1 = fromPoly->verts[(link->edge+1) % (int)fromPoly->vertCount]; - dtVcopy(left, &fromTile->verts[v0*3]); - dtVcopy(right, &fromTile->verts[v1*3]); - - // If the link is at tile boundary, dtClamp the vertices to - // the link width. - if (link->side != 0xff) - { - // Unpack portal limits. - if (link->bmin != 0 || link->bmax != 255) - { - const float s = 1.0f/255.0f; - const float tmin = link->bmin*s; - const float tmax = link->bmax*s; - dtVlerp(left, &fromTile->verts[v0*3], &fromTile->verts[v1*3], tmin); - dtVlerp(right, &fromTile->verts[v0*3], &fromTile->verts[v1*3], tmax); - } - } - - return DT_SUCCESS; -} - -// Returns edge mid point between two polygons. -dtStatus dtNavMeshQuery::getEdgeMidPoint(dtPolyRef from, dtPolyRef to, float* mid) const -{ - float left[3], right[3]; - unsigned char fromType, toType; - if (!getPortalPoints(from, to, left,right, fromType, toType)) return DT_FAILURE; - 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 DT_SUCCESS; -} - -dtStatus dtNavMeshQuery::getEdgeMidPoint(dtPolyRef from, const dtPoly* fromPoly, const dtMeshTile* fromTile, - dtPolyRef to, const dtPoly* toPoly, const dtMeshTile* toTile, - float* mid) const -{ - float left[3], right[3]; - if (getPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, left, right) != DT_SUCCESS) - return DT_FAILURE; - 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 DT_SUCCESS; -} - -dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, const float* endPos, - const dtQueryFilter* filter, - float* t, float* hitNormal, dtPolyRef* path, int* pathCount, const int maxPath) const -{ - dtAssert(m_nav); - - *t = 0; - if (pathCount) - *pathCount = 0; - - // Validate input - if (!startRef || !m_nav->isValidPolyRef(startRef)) - return DT_FAILURE; - - dtPolyRef curRef = startRef; - float verts[DT_VERTS_PER_POLYGON*3]; - int n = 0; - - hitNormal[0] = 0; - hitNormal[1] = 0; - hitNormal[2] = 0; - - while (curRef) - { - // Cast ray against current polygon. - - // The API input has been cheked already, skip checking internal data. - const dtMeshTile* tile = 0; - const dtPoly* poly = 0; - m_nav->getTileAndPolyByRefUnsafe(curRef, &tile, &poly); - - // Collect vertices. - int nv = 0; - for (int i = 0; i < (int)poly->vertCount; ++i) - { - dtVcopy(&verts[nv*3], &tile->verts[poly->verts[i]*3]); - nv++; - } - - float tmin, tmax; - int segMin, segMax; - if (!dtIntersectSegmentPoly2D(startPos, endPos, verts, nv, tmin, tmax, segMin, segMax)) - { - // Could not hit the polygon, keep the old t and report hit. - if (pathCount) - *pathCount = n; - return DT_SUCCESS; - } - // Keep track of furthest t so far. - if (tmax > *t) - *t = tmax; - - // Store visited polygons. - if (n < maxPath) - path[n++] = curRef; - - // Ray end is completely inside the polygon. - if (segMax == -1) - { - *t = FLT_MAX; - if (pathCount) - *pathCount = n; - return DT_SUCCESS; - } - - // Follow neighbours. - dtPolyRef nextRef = 0; - - for (unsigned int i = poly->firstLink; i != DT_NULL_LINK; i = tile->links[i].next) - { - const dtLink* link = &tile->links[i]; - - // Find link which contains this edge. - if ((int)link->edge != segMax) - continue; - - // Get pointer to the next polygon. - const dtMeshTile* nextTile = 0; - const dtPoly* nextPoly = 0; - m_nav->getTileAndPolyByRefUnsafe(link->ref, &nextTile, &nextPoly); - - // Skip off-mesh connections. - if (nextPoly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) - continue; - - // Skip links based on filter. - if (!filter->passFilter(link->ref, nextTile, nextPoly)) - continue; - - // If the link is internal, just return the ref. - if (link->side == 0xff) - { - nextRef = link->ref; - break; - } - - // If the link is at tile boundary, - - // Check if the link spans the whole edge, and accept. - if (link->bmin == 0 && link->bmax == 255) - { - nextRef = link->ref; - break; - } - - // Check for partial edge links. - const int v0 = poly->verts[link->edge]; - const int v1 = poly->verts[(link->edge+1) % poly->vertCount]; - const float* left = &tile->verts[v0*3]; - const float* right = &tile->verts[v1*3]; - - // Check that the intersection lies inside the link portal. - if (link->side == 0 || link->side == 4) - { - // Calculate link size. - const float s = 1.0f/255.0f; - float lmin = left[2] + (right[2] - left[2])*(link->bmin*s); - float lmax = left[2] + (right[2] - left[2])*(link->bmax*s); - if (lmin > lmax) dtSwap(lmin, lmax); - - // 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 == 2 || link->side == 6) - { - // Calculate link size. - const float s = 1.0f/255.0f; - float lmin = left[0] + (right[0] - left[0])*(link->bmin*s); - float lmax = left[0] + (right[0] - left[0])*(link->bmax*s); - if (lmin > lmax) dtSwap(lmin, lmax); - - // 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. - - // Calculate hit normal. - const int a = segMax; - const int b = segMax+1 < nv ? segMax+1 : 0; - const float* va = &verts[a*3]; - const float* vb = &verts[b*3]; - const float dx = vb[0] - va[0]; - const float dz = vb[2] - va[2]; - hitNormal[0] = dz; - hitNormal[1] = 0; - hitNormal[2] = -dx; - dtVnormalize(hitNormal); - - if (pathCount) - *pathCount = n; - return DT_SUCCESS; - } - - // No hit, advance to neighbour polygon. - curRef = nextRef; - } - - if (pathCount) - *pathCount = n; - - return DT_SUCCESS; -} - -dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float* centerPos, const float radius, - const dtQueryFilter* filter, - dtPolyRef* resultRef, dtPolyRef* resultParent, float* resultCost, - int* resultCount, const int maxResult) const -{ - dtAssert(m_nav); - dtAssert(m_nodePool); - dtAssert(m_openList); - - *resultCount = 0; - - // Validate input - if (!startRef) return DT_FAILURE; - if (!m_nav->isValidPolyRef(startRef)) return DT_FAILURE; - - m_nodePool->clear(); - m_openList->clear(); - - dtNode* startNode = m_nodePool->getNode(startRef); - dtVcopy(startNode->pos, centerPos); - startNode->pidx = 0; - startNode->cost = 0; - startNode->total = 0; - startNode->id = startRef; - 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 = dtSqr(radius); - - while (!m_openList->empty()) - { - dtNode* bestNode = m_openList->pop(); - bestNode->flags &= ~DT_NODE_OPEN; - bestNode->flags |= DT_NODE_CLOSED; - - // Get poly and tile. - // The API input has been cheked already, skip checking internal data. - const dtPolyRef bestRef = bestNode->id; - const dtMeshTile* bestTile = 0; - const dtPoly* bestPoly = 0; - m_nav->getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly); - - // Get parent poly and tile. - dtPolyRef parentRef = 0; - const dtMeshTile* parentTile = 0; - const dtPoly* parentPoly = 0; - if (bestNode->pidx) - parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id; - if (parentRef) - m_nav->getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly); - - for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next) - { - const dtLink* link = &bestTile->links[i]; - dtPolyRef neighbourRef = link->ref; - // Skip invalid neighbours and do not follow back to parent. - if (!neighbourRef || neighbourRef == parentRef) - continue; - - // Expand to neighbour - const dtMeshTile* neighbourTile = 0; - const dtPoly* neighbourPoly = 0; - m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly); - - // Do not advance if the polygon is excluded by the filter. - if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly)) - continue; - - // Find edge and calc distance to the edge. - float va[3], vb[3]; - if (!getPortalPoints(bestRef, bestPoly, bestTile, neighbourRef, neighbourPoly, neighbourTile, va, vb)) - continue; - - // If the circle is not touching the next polygon, skip it. - float tseg; - float distSqr = dtDistancePtSegSqr2D(centerPos, va, vb, tseg); - if (distSqr > radiusSqr) - continue; - - dtNode* neighbourNode = m_nodePool->getNode(neighbourRef); - if (!neighbourNode) - continue; - - if (neighbourNode->flags & DT_NODE_CLOSED) - continue; - - // Cost - if (neighbourNode->flags == 0) - dtVlerp(neighbourNode->pos, va, vb, 0.5f); - - const float total = bestNode->total + dtVdist(bestNode->pos, neighbourNode->pos); - - // The node is already in open list and the new result is worse, skip. - if ((neighbourNode->flags & DT_NODE_OPEN) && total >= neighbourNode->total) - continue; - - neighbourNode->id = neighbourRef; - neighbourNode->flags &= ~DT_NODE_CLOSED; - neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode); - neighbourNode->total = total; - - if (neighbourNode->flags & DT_NODE_OPEN) - { - m_openList->modify(neighbourNode); - } - else - { - if (n < maxResult) - { - if (resultRef) - resultRef[n] = neighbourNode->id; - if (resultParent) - resultParent[n] = m_nodePool->getNodeAtIdx(neighbourNode->pidx)->id; - if (resultCost) - resultCost[n] = neighbourNode->total; - ++n; - } - neighbourNode->flags = DT_NODE_OPEN; - m_openList->push(neighbourNode); - } - } - } - - *resultCount = n; - - return DT_SUCCESS; -} - -dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* verts, const int nverts, - const dtQueryFilter* filter, - dtPolyRef* resultRef, dtPolyRef* resultParent, float* resultCost, - int* resultCount, const int maxResult) const -{ - dtAssert(m_nav); - dtAssert(m_nodePool); - dtAssert(m_openList); - - *resultCount = 0; - - // Validate input - if (!startRef) return DT_FAILURE; - if (!m_nav->isValidPolyRef(startRef)) return DT_FAILURE; - - m_nodePool->clear(); - m_openList->clear(); - - float centerPos[3] = {0,0,0}; - for (int i = 0; i < nverts; ++i) - dtVadd(centerPos,centerPos,&verts[i*3]); - dtVscale(centerPos,centerPos,1.0f/nverts); - - dtNode* startNode = m_nodePool->getNode(startRef); - dtVcopy(startNode->pos, centerPos); - startNode->pidx = 0; - startNode->cost = 0; - startNode->total = 0; - startNode->id = startRef; - 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; - } - - while (!m_openList->empty()) - { - dtNode* bestNode = m_openList->pop(); - bestNode->flags &= ~DT_NODE_OPEN; - bestNode->flags |= DT_NODE_CLOSED; - - // Get poly and tile. - // The API input has been cheked already, skip checking internal data. - const dtPolyRef bestRef = bestNode->id; - const dtMeshTile* bestTile = 0; - const dtPoly* bestPoly = 0; - m_nav->getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly); - - // Get parent poly and tile. - dtPolyRef parentRef = 0; - const dtMeshTile* parentTile = 0; - const dtPoly* parentPoly = 0; - if (bestNode->pidx) - parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id; - if (parentRef) - m_nav->getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly); - - for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next) - { - const dtLink* link = &bestTile->links[i]; - dtPolyRef neighbourRef = link->ref; - // Skip invalid neighbours and do not follow back to parent. - if (!neighbourRef || neighbourRef == parentRef) - continue; - - // Expand to neighbour - const dtMeshTile* neighbourTile = 0; - const dtPoly* neighbourPoly = 0; - m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly); - - // Do not advance if the polygon is excluded by the filter. - if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly)) - continue; - - // Find edge and calc distance to the edge. - float va[3], vb[3]; - if (!getPortalPoints(bestRef, bestPoly, bestTile, neighbourRef, neighbourPoly, neighbourTile, va, vb)) - continue; - - // If the poly is not touching the edge to the next polygon, skip the connection it. - float tmin, tmax; - int segMin, segMax; - if (!dtIntersectSegmentPoly2D(va, vb, verts, nverts, tmin, tmax, segMin, segMax)) - continue; - if (tmin > 1.0f || tmax < 0.0f) - continue; - - dtNode* neighbourNode = m_nodePool->getNode(neighbourRef); - if (!neighbourNode) - continue; - - if (neighbourNode->flags & DT_NODE_CLOSED) - continue; - - // Cost - if (neighbourNode->flags == 0) - dtVlerp(neighbourNode->pos, va, vb, 0.5f); - - const float total = bestNode->total + dtVdist(bestNode->pos, neighbourNode->pos); - - // The node is already in open list and the new result is worse, skip. - if ((neighbourNode->flags & DT_NODE_OPEN) && total >= neighbourNode->total) - continue; - - neighbourNode->id = neighbourRef; - neighbourNode->flags &= ~DT_NODE_CLOSED; - neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode); - neighbourNode->total = total; - - if (neighbourNode->flags & DT_NODE_OPEN) - { - m_openList->modify(neighbourNode); - } - else - { - if (n < maxResult) - { - if (resultRef) - resultRef[n] = neighbourNode->id; - if (resultParent) - resultParent[n] = m_nodePool->getNodeAtIdx(neighbourNode->pidx)->id; - if (resultCost) - resultCost[n] = neighbourNode->total; - ++n; - } - neighbourNode->flags = DT_NODE_OPEN; - m_openList->push(neighbourNode); - } - } - } - - *resultCount = n; - - return DT_SUCCESS; -} - -dtStatus dtNavMeshQuery::findLocalNeighbourhood(dtPolyRef startRef, const float* centerPos, const float radius, - const dtQueryFilter* filter, - dtPolyRef* resultRef, dtPolyRef* resultParent, - int* resultCount, const int maxResult) const -{ - dtAssert(m_nav); - dtAssert(m_tinyNodePool); - - *resultCount = 0; - - // Validate input - if (!startRef) return DT_FAILURE; - if (!m_nav->isValidPolyRef(startRef)) return DT_FAILURE; - - static const int MAX_STACK = 48; - dtNode* stack[MAX_STACK]; - int nstack = 0; - - m_tinyNodePool->clear(); - - dtNode* startNode = m_tinyNodePool->getNode(startRef); - startNode->pidx = 0; - startNode->id = startRef; - startNode->flags = DT_NODE_CLOSED; - stack[nstack++] = startNode; - - const float radiusSqr = dtSqr(radius); - - float pa[DT_VERTS_PER_POLYGON*3]; - float pb[DT_VERTS_PER_POLYGON*3]; - - int n = 0; - if (n < maxResult) - { - resultRef[n] = startNode->id; - if (resultParent) - resultParent[n] = 0; - ++n; - } - - while (nstack) - { - // Pop front. - dtNode* curNode = stack[0]; - for (int i = 0; i < nstack-1; ++i) - stack[i] = stack[i+1]; - nstack--; - - // Get poly and tile. - // The API input has been cheked already, skip checking internal data. - const dtPolyRef curRef = curNode->id; - const dtMeshTile* curTile = 0; - const dtPoly* curPoly = 0; - m_nav->getTileAndPolyByRefUnsafe(curRef, &curTile, &curPoly); - - for (unsigned int i = curPoly->firstLink; i != DT_NULL_LINK; i = curTile->links[i].next) - { - const dtLink* link = &curTile->links[i]; - dtPolyRef neighbourRef = link->ref; - // Skip invalid neighbours. - if (!neighbourRef) - continue; - - // Skip if cannot alloca more nodes. - dtNode* neighbourNode = m_tinyNodePool->getNode(neighbourRef); - if (!neighbourNode) - continue; - // Skip visited. - if (neighbourNode->flags & DT_NODE_CLOSED) - continue; - - // Expand to neighbour - const dtMeshTile* neighbourTile = 0; - const dtPoly* neighbourPoly = 0; - m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly); - - // Skip off-mesh connections. - if (neighbourPoly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) - continue; - - // Do not advance if the polygon is excluded by the filter. - if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly)) - continue; - - // Find edge and calc distance to the edge. - float va[3], vb[3]; - if (!getPortalPoints(curRef, curPoly, curTile, neighbourRef, neighbourPoly, neighbourTile, va, vb)) - continue; - - // If the circle is not touching the next polygon, skip it. - float tseg; - float distSqr = dtDistancePtSegSqr2D(centerPos, va, vb, tseg); - if (distSqr > radiusSqr) - continue; - - // Mark node visited, this is done before the overlap test so that - // we will not visit the poly again if the test fails. - neighbourNode->flags |= DT_NODE_CLOSED; - neighbourNode->pidx = m_tinyNodePool->getNodeIdx(curNode); - - // Check that the polygon does not collide with existing polygons. - - // Collect vertices of the neighbour poly. - const int npa = neighbourPoly->vertCount; - for (int k = 0; k < npa; ++k) - dtVcopy(&pa[k*3], &neighbourTile->verts[neighbourPoly->verts[k]*3]); - - bool overlap = false; - for (int j = 0; j < n; ++j) - { - dtPolyRef pastRef = resultRef[j]; - - // Connected polys do not overlap. - bool connected = false; - for (unsigned int k = curPoly->firstLink; k != DT_NULL_LINK; k = curTile->links[k].next) - { - if (curTile->links[k].ref == pastRef) - { - connected = true; - break; - } - } - if (connected) - continue; - - // Potentially overlapping. - const dtMeshTile* pastTile = 0; - const dtPoly* pastPoly = 0; - m_nav->getTileAndPolyByRefUnsafe(pastRef, &pastTile, &pastPoly); - - // Get vertices and test overlap - const int npb = pastPoly->vertCount; - for (int k = 0; k < npb; ++k) - dtVcopy(&pb[k*3], &pastTile->verts[pastPoly->verts[k]*3]); - - if (dtOverlapPolyPoly2D(pa,npa, pb,npb)) - { - overlap = true; - break; - } - } - if (overlap) - continue; - - // This poly is fine, store and advance to the poly. - if (n < maxResult) - { - resultRef[n] = neighbourRef; - if (resultParent) - resultParent[n] = curRef; - ++n; - } - - if (nstack < MAX_STACK) - { - stack[nstack++] = neighbourNode; - } - } - } - - *resultCount = n; - - return DT_SUCCESS; -} - - -struct dtSegInterval -{ - short tmin, tmax; -}; - -static void insertInterval(dtSegInterval* ints, int& nints, const int maxInts, - const short tmin, const short tmax) -{ - if (nints+1 > maxInts) return; - // Find insertion point. - int idx = 0; - while (idx < nints) - { - if (tmax <= ints[idx].tmin) - break; - idx++; - } - // Move current results. - if (nints-idx) - memmove(ints+idx+1, ints+idx, sizeof(dtSegInterval)*(nints-idx)); - // Store - ints[idx].tmin = tmin; - ints[idx].tmax = tmax; - nints++; -} - -dtStatus dtNavMeshQuery::getPolyWallSegments(dtPolyRef ref, const dtQueryFilter* filter, - float* segments, int* segmentCount, const int maxSegments) const -{ - dtAssert(m_nav); - - *segmentCount = 0; - - const dtMeshTile* tile = 0; - const dtPoly* poly = 0; - if (m_nav->getTileAndPolyByRef(ref, &tile, &poly) != DT_SUCCESS) - return DT_FAILURE; - - int n = 0; - static const int MAX_INTERVAL = 16; - dtSegInterval ints[MAX_INTERVAL]; - int nints; - - for (int i = 0, j = (int)poly->vertCount-1; i < (int)poly->vertCount; j = i++) - { - // Skip non-solid edges. - nints = 0; - if (poly->neis[j] & DT_EXT_LINK) - { - // Tile border. - for (unsigned int k = poly->firstLink; k != DT_NULL_LINK; k = tile->links[k].next) - { - const dtLink* link = &tile->links[k]; - if (link->edge == j) - { - if (link->ref != 0) - { - const dtMeshTile* neiTile = 0; - const dtPoly* neiPoly = 0; - m_nav->getTileAndPolyByRefUnsafe(link->ref, &neiTile, &neiPoly); - if (filter->passFilter(link->ref, neiTile, neiPoly)) - { - insertInterval(ints, nints, MAX_INTERVAL, link->bmin, link->bmax); - } - } - } - } - } - else if (poly->neis[j]) - { - // Internal edge - const unsigned int idx = (unsigned int)(poly->neis[j]-1); - const dtPolyRef ref = m_nav->getPolyRefBase(tile) | idx; - if (filter->passFilter(ref, tile, &tile->polys[idx])) - continue; - } - - // Add sentinels - insertInterval(ints, nints, MAX_INTERVAL, -1, 0); - insertInterval(ints, nints, MAX_INTERVAL, 255, 256); - - // Store segment. - const float* vj = &tile->verts[poly->verts[j]*3]; - const float* vi = &tile->verts[poly->verts[i]*3]; - for (int k = 1; k < nints; ++k) - { - // Find the space inbetween the opening areas. - const int imin = ints[k-1].tmax; - const int imax = ints[k].tmin; - if (imin == imax) continue; - if (imin == 0 && imax == 255) - { - if (n < maxSegments) - { - float* seg = &segments[n*6]; - n++; - dtVcopy(seg+0, vj); - dtVcopy(seg+3, vi); - } - } - else - { - const float tmin = imin/255.0f; - const float tmax = imax/255.0f; - if (n < maxSegments) - { - float* seg = &segments[n*6]; - n++; - dtVlerp(seg+0, vj,vi, tmin); - dtVlerp(seg+3, vj,vi, tmax); - } - } - } - } - - *segmentCount = n; - - return DT_SUCCESS; -} - -dtStatus dtNavMeshQuery::findDistanceToWall(dtPolyRef startRef, const float* centerPos, const float maxRadius, - const dtQueryFilter* filter, - float* hitDist, float* hitPos, float* hitNormal) const -{ - dtAssert(m_nav); - dtAssert(m_nodePool); - dtAssert(m_openList); - - // Validate input - if (!startRef) return DT_FAILURE; - if (!m_nav->isValidPolyRef(startRef)) return DT_FAILURE; - - m_nodePool->clear(); - m_openList->clear(); - - dtNode* startNode = m_nodePool->getNode(startRef); - dtVcopy(startNode->pos, centerPos); - startNode->pidx = 0; - startNode->cost = 0; - startNode->total = 0; - startNode->id = startRef; - startNode->flags = DT_NODE_OPEN; - m_openList->push(startNode); - - float radiusSqr = dtSqr(maxRadius); - - while (!m_openList->empty()) - { - dtNode* bestNode = m_openList->pop(); - bestNode->flags &= ~DT_NODE_OPEN; - bestNode->flags |= DT_NODE_CLOSED; - - // Get poly and tile. - // The API input has been cheked already, skip checking internal data. - const dtPolyRef bestRef = bestNode->id; - const dtMeshTile* bestTile = 0; - const dtPoly* bestPoly = 0; - m_nav->getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly); - - // Get parent poly and tile. - dtPolyRef parentRef = 0; - const dtMeshTile* parentTile = 0; - const dtPoly* parentPoly = 0; - if (bestNode->pidx) - parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id; - if (parentRef) - m_nav->getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly); - - // Hit test walls. - for (int i = 0, j = (int)bestPoly->vertCount-1; i < (int)bestPoly->vertCount; j = i++) - { - // Skip non-solid edges. - if (bestPoly->neis[j] & DT_EXT_LINK) - { - // Tile border. - bool solid = true; - for (unsigned int k = bestPoly->firstLink; k != DT_NULL_LINK; k = bestTile->links[k].next) - { - const dtLink* link = &bestTile->links[k]; - if (link->edge == j) - { - if (link->ref != 0) - { - const dtMeshTile* neiTile = 0; - const dtPoly* neiPoly = 0; - m_nav->getTileAndPolyByRefUnsafe(link->ref, &neiTile, &neiPoly); - if (filter->passFilter(link->ref, neiTile, neiPoly)) - solid = false; - } - break; - } - } - if (!solid) continue; - } - else if (bestPoly->neis[j]) - { - // Internal edge - const unsigned int idx = (unsigned int)(bestPoly->neis[j]-1); - const dtPolyRef ref = m_nav->getPolyRefBase(bestTile) | idx; - if (filter->passFilter(ref, bestTile, &bestTile->polys[idx])) - continue; - } - - // Calc distance to the edge. - const float* vj = &bestTile->verts[bestPoly->verts[j]*3]; - const float* vi = &bestTile->verts[bestPoly->verts[i]*3]; - float tseg; - float distSqr = dtDistancePtSegSqr2D(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 (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next) - { - const dtLink* link = &bestTile->links[i]; - dtPolyRef neighbourRef = link->ref; - // Skip invalid neighbours and do not follow back to parent. - if (!neighbourRef || neighbourRef == parentRef) - continue; - - // Expand to neighbour. - const dtMeshTile* neighbourTile = 0; - const dtPoly* neighbourPoly = 0; - m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly); - - // Skip off-mesh connections. - if (neighbourPoly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) - continue; - - // Calc distance to the edge. - const float* va = &bestTile->verts[bestPoly->verts[link->edge]*3]; - const float* vb = &bestTile->verts[bestPoly->verts[(link->edge+1) % bestPoly->vertCount]*3]; - float tseg; - float distSqr = dtDistancePtSegSqr2D(centerPos, va, vb, tseg); - - // If the circle is not touching the next polygon, skip it. - if (distSqr > radiusSqr) - continue; - - if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly)) - continue; - - dtNode* neighbourNode = m_nodePool->getNode(neighbourRef); - if (!neighbourNode) - continue; - - if (neighbourNode->flags & DT_NODE_CLOSED) - continue; - - // Cost - if (neighbourNode->flags == 0) - { - getEdgeMidPoint(bestRef, bestPoly, bestTile, - neighbourRef, neighbourPoly, neighbourTile, neighbourNode->pos); - } - - const float total = bestNode->total + dtVdist(bestNode->pos, neighbourNode->pos); - - // The node is already in open list and the new result is worse, skip. - if ((neighbourNode->flags & DT_NODE_OPEN) && total >= neighbourNode->total) - continue; - - neighbourNode->id = neighbourRef; - neighbourNode->flags &= ~DT_NODE_CLOSED; - neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode); - neighbourNode->total = total; - - if (neighbourNode->flags & DT_NODE_OPEN) - { - m_openList->modify(neighbourNode); - } - else - { - neighbourNode->flags |= DT_NODE_OPEN; - m_openList->push(neighbourNode); - } - } - } - - // Calc hit normal. - dtVsub(hitNormal, centerPos, hitPos); - dtVnormalize(hitNormal); - - *hitDist = sqrtf(radiusSqr); - - return DT_SUCCESS; -} - -bool dtNavMeshQuery::isInClosedList(dtPolyRef ref) const -{ - if (!m_nodePool) return false; - const dtNode* node = m_nodePool->findNode(ref); - return node && node->flags & DT_NODE_CLOSED; -} diff --git a/deps/recastnavigation/Detour/DetourNavMeshQuery.h b/deps/recastnavigation/Detour/DetourNavMeshQuery.h deleted file mode 100644 index f5046d8329..0000000000 --- a/deps/recastnavigation/Detour/DetourNavMeshQuery.h +++ /dev/null @@ -1,407 +0,0 @@ -// -// Copyright (c) 2009-2010 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. -// - -#ifndef DETOURNAVMESHQUERY_H -#define DETOURNAVMESHQUERY_H - -#include "DetourNavMesh.h" - - -// Define DT_VIRTUAL_QUERYFILTER if you wish to derive a custom filter from dtQueryFilter. -// On certain platforms indirect or virtual function call is expensive. The default -// setting is to use non-virtual functions, the actualy implementations of the functions -// are declared as inline for maximum speed. - -//#define DT_VIRTUAL_QUERYFILTER 1 - -// Class for polygon filtering and cost calculation during query operations. -// - It is possible to derive a custom query filter from dtQueryFilter by overriding -// the virtual functions passFilter() and getCost(). -// - Both functions should be as fast as possible. Use cached local copy of data -// instead of accessing your own objects where possible. -// - You do not need to adhere to the flags and cost logic provided by the default -// implementation. -// - In order for the A* to work properly, the cost should be proportional to -// the travel distance. Using cost modifier less than 1.0 is likely to lead -// to problems during pathfinding. -class dtQueryFilter -{ - float m_areaCost[DT_MAX_AREAS]; // Array storing cost per area type, used by default implementation. - unsigned short m_includeFlags; // Include poly flags, used by default implementation. - unsigned short m_excludeFlags; // Exclude poly flags, used by default implementation. - -public: - dtQueryFilter(); - - // Returns true if the polygon is can visited. - // Params: - // ref - (in) reference to the polygon test. - // tile - (in) pointer to the tile of the polygon test. - // poly - (in) pointer to the polygon test. -#ifdef DT_VIRTUAL_QUERYFILTER - virtual bool passFilter(const dtPolyRef ref, - const dtMeshTile* tile, - const dtPoly* poly) const; -#else - bool passFilter(const dtPolyRef ref, - const dtMeshTile* tile, - const dtPoly* poly) const; -#endif - - // Returns cost to travel from 'pa' to 'pb'.' - // The segment is fully contained inside 'cur'. - // 'pa' lies on the edge between 'prev' and 'cur', - // 'pb' lies on the edge between 'cur' and 'next'. - // Params: - // pa - (in) segment start position. - // pb - (in) segment end position. - // prevRef, prevTile, prevPoly - (in) data describing the previous polygon, can be null. - // curRef, curTile, curPoly - (in) data describing the current polygon. - // nextRef, nextTile, nextPoly - (in) data describing the next polygon, can be null. -#ifdef DT_VIRTUAL_QUERYFILTER - virtual float getCost(const float* pa, const float* pb, - const dtPolyRef prevRef, const dtMeshTile* prevTile, const dtPoly* prevPoly, - const dtPolyRef curRef, const dtMeshTile* curTile, const dtPoly* curPoly, - const dtPolyRef nextRef, const dtMeshTile* nextTile, const dtPoly* nextPoly) const; -#else - float getCost(const float* pa, const float* pb, - const dtPolyRef prevRef, const dtMeshTile* prevTile, const dtPoly* prevPoly, - const dtPolyRef curRef, const dtMeshTile* curTile, const dtPoly* curPoly, - const dtPolyRef nextRef, const dtMeshTile* nextTile, const dtPoly* nextPoly) const; -#endif - - // Getters and setters for the default implementation data. - inline float getAreaCost(const int i) const { return m_areaCost[i]; } - inline void setAreaCost(const int i, const float cost) { m_areaCost[i] = cost; } - - inline unsigned short getIncludeFlags() const { return m_includeFlags; } - inline void setIncludeFlags(const unsigned short flags) { m_includeFlags = flags; } - - inline unsigned short getExcludeFlags() const { return m_excludeFlags; } - inline void setExcludeFlags(const unsigned short flags) { m_excludeFlags = flags; } -}; - -class dtNavMeshQuery -{ -public: - dtNavMeshQuery(); - ~dtNavMeshQuery(); - - // Initializes the nav mesh query. - // Params: - // nav - (in) pointer to navigation mesh data. - // maxNodes - (in) Maximum number of search nodes to use (max 65536). - // Returns: True if succeed, else false. - dtStatus init(const dtNavMesh* nav, const int maxNodes); - - // Finds the nearest navigation polygon around the center location. - // Params: - // center[3] - (in) The center of the search box. - // extents[3] - (in) The extents of the search box. - // filter - (in) path polygon filter. - // nearestRef - (out) Reference to the nearest polygon. - // nearestPt[3] - (out, opt) The nearest point on found polygon, null if not needed. - // Returns: Reference identifier for the polygon, or 0 if no polygons found. - dtStatus findNearestPoly(const float* center, const float* extents, - const dtQueryFilter* filter, - dtPolyRef* nearestRef, float* nearestPt) const; - - // Returns polygons which overlap the query box. - // Params: - // center[3] - (in) the center of the search box. - // extents[3] - (in) the extents of the search box. - // filter - (in) path polygon filter. - // polys - (out) array holding the search result. - // polyCount - (out) Number of polygons in search result array. - // maxPolys - (in) The max number of polygons the polys array can hold. - dtStatus queryPolygons(const float* center, const float* extents, - const dtQueryFilter* filter, - dtPolyRef* polys, int* polyCount, const int maxPolys) const; - - // Finds path from start polygon to end polygon. - // If target polygon canno be reached through the navigation graph, - // the last node on the array is nearest node to the end polygon. - // Start end end positions are needed to calculate more accurate - // traversal cost at start end end polygons. - // Params: - // startRef - (in) ref to path start polygon. - // endRef - (in) ref to path end polygon. - // startPos[3] - (in) Path start location. - // endPos[3] - (in) Path end location. - // filter - (in) path polygon filter. - // path - (out) array holding the search result. - // pathCount - (out) Number of polygons in search result array. - // maxPath - (in) The max number of polygons the path array can hold. Must be at least 1. - dtStatus findPath(dtPolyRef startRef, dtPolyRef endRef, - const float* startPos, const float* endPos, - const dtQueryFilter* filter, - dtPolyRef* path, int* pathCount, const int maxPath) const; - - // Intializes sliced path find query. - // Note 1: calling any other dtNavMeshQuery method before calling findPathEnd() - // may results in corrupted data! - // Note 2: The pointer to filter is store, and used in subsequent - // calls to updateSlicedFindPath(). - // Params: - // startRef - (in) ref to path start polygon. - // endRef - (in) ref to path end polygon. - // startPos[3] - (in) Path start location. - // endPos[3] - (in) Path end location. - // filter - (in) path polygon filter. - dtStatus initSlicedFindPath(dtPolyRef startRef, dtPolyRef endRef, - const float* startPos, const float* endPos, - const dtQueryFilter* filter); - - // Updates sliced path find query. - // Params: - // maxIter - (in) max number of iterations to update. - // Returns: Path query state. - dtStatus updateSlicedFindPath(const int maxIter); - - // Finalizes sliced path find query and returns found path. - // path - (out) array holding the search result. - // pathCount - (out) Number of polygons in search result array. - // maxPath - (in) The max number of polygons the path array can hold. - dtStatus finalizeSlicedFindPath(dtPolyRef* path, int* pathCount, const int maxPath); - - // Finalizes partial sliced path find query and returns path to the furthest - // polygon on the existing path that was visited during the search. - // existing - (out) Array of polygons in the existing path. - // existingSize - (out) Number of polygons in existing path array. - // path - (out) array holding the search result. - // pathCount - (out) Number of polygons in search result array. - // maxPath - (in) The max number of polygons the path array can hold. - dtStatus finalizeSlicedFindPathPartial(const dtPolyRef* existing, const int existingSize, - dtPolyRef* path, int* pathCount, const int maxPath); - - // Finds a straight path from start to end locations within the corridor - // described by the path polygons. - // Start and end locations will be clamped on the corridor. - // The returned polygon references are point to polygon which was entered when - // a path point was added. For the end point, zero will be returned. This allows - // to match for example off-mesh link points to their representative polygons. - // Params: - // startPos[3] - (in) Path start location. - // endPo[3] - (in) Path end location. - // path - (in) Array of connected polygons describing the corridor. - // pathSize - (in) Number of polygons in path array. - // straightPath - (out) Points describing the straight path. - // straightPathFlags - (out, opt) Flags describing each point type, see dtStraightPathFlags. - // straightPathRefs - (out, opt) References to polygons at point locations. - // straightPathCount - (out) Number of points in the path. - // maxStraightPath - (in) The max number of points the straight path array can hold. Must be at least 1. - dtStatus findStraightPath(const float* startPos, const float* endPos, - const dtPolyRef* path, const int pathSize, - float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs, - int* straightPathCount, const int maxStraightPath) const; - - // Moves from startPos to endPos constrained to the navmesh. - // If the endPos is reachable, the resultPos will be endPos, - // or else the resultPos will be the nearest point in navmesh. - // Note: The resulting point is not projected to the ground, use getPolyHeight() to get height. - // Note: The algorithm is optimized for small delta movement and small number of polygons. - // Params: - // startRef - (in) ref to the polygon where startPos lies. - // startPos[3] - (in) start position of the mover. - // endPos[3] - (in) desired end position of the mover. - // filter - (in) path polygon filter. - // resultPos[3] - (out) new position of the mover. - // visited - (out) array of visited polygons. - // visitedCount - (out) Number of entries in the visited array. - // maxVisitedSize - (in) max number of polygons in the visited array. - dtStatus moveAlongSurface(dtPolyRef startRef, const float* startPos, const float* endPos, - const dtQueryFilter* filter, - float* resultPos, dtPolyRef* visited, int* visitedCount, const int maxVisitedSize) const; - - // Casts 'walkability' ray along the navmesh surface from startPos towards the endPos. - // Params: - // startRef - (in) ref to the polygon where the start lies. - // startPos[3] - (in) start position of the query. - // endPos[3] - (in) end position of the query. - // t - (out) hit parameter along the segment, FLT_MAX if no hit. - // hitNormal[3] - (out) normal of the nearest hit. - // filter - (in) path polygon filter. - // path - (out,opt) visited path polygons. - // pathCount - (out,opt) Number of polygons visited. - // maxPath - (in) max number of polygons in the path array. - dtStatus raycast(dtPolyRef startRef, const float* startPos, const float* endPos, - const dtQueryFilter* filter, - float* t, float* hitNormal, dtPolyRef* path, int* pathCount, const int maxPath) const; - - // Returns distance to nearest wall from the specified location. - // Params: - // startRef - (in) ref to the polygon where the center lies. - // centerPos[3] - (in) center if the query circle. - // maxRadius - (in) max search radius. - // filter - (in) path polygon filter. - // hitDist - (out) distance to nearest wall from the test location. - // hitPos[3] - (out) location of the nearest hit. - // hitNormal[3] - (out) normal of the nearest hit. - dtStatus findDistanceToWall(dtPolyRef startRef, const float* centerPos, const float maxRadius, - const dtQueryFilter* filter, - float* hitDist, float* hitPos, float* hitNormal) const; - - // Finds polygons found along the navigation graph which touch the specified circle. - // Params: - // startRef - (in) ref to the polygon where the search starts. - // centerPos[3] - (in) center if the query circle. - // radius - (in) radius of the query circle. - // filter - (in) path polygon filter. - // resultRef - (out, opt) refs to the polygons touched by the circle. - // resultParent - (out, opt) parent of each result polygon. - // resultCost - (out, opt) search cost at each result polygon. - // resultCount - (out, opt) Number of results. - // maxResult - (int) maximum capacity of search results. - dtStatus findPolysAroundCircle(dtPolyRef startRef, const float* centerPos, const float radius, - const dtQueryFilter* filter, - dtPolyRef* resultRef, dtPolyRef* resultParent, float* resultCost, - int* resultCount, const int maxResult) const; - - // Finds polygons found along the navigation graph which touch the convex polygon shape. - // Params: - // startRef - (in) ref to the polygon where the search starts. - // verts[3*n] - (in) vertices describing convex polygon shape (CCW). - // nverts - (in) number of vertices in the polygon. - // filter - (in) path polygon filter. - // resultRef - (out, opt) refs to the polygons touched by the circle. - // resultParent - (out, opt) parent of each result polygon. - // resultCost - (out, opt) search cost at each result polygon. - // resultCount - (out) number of results. - // maxResult - (int) maximum capacity of search results. - dtStatus findPolysAroundShape(dtPolyRef startRef, const float* verts, const int nverts, - const dtQueryFilter* filter, - dtPolyRef* resultRef, dtPolyRef* resultParent, float* resultCost, - int* resultCount, const int maxResult) const; - - // Finds non-overlapping local neighbourhood around center location. - // Note: The algorithm is optimized for small query radius and small number of polygons. - // Params: - // startRef - (in) ref to the polygon where the search starts. - // centerPos[3] - (in) center if the query circle. - // radius - (in) radius of the query circle. - // filter - (in) path polygon filter. - // resultRef - (out) refs to the polygons touched by the circle. - // resultParent - (out, opt) parent of each result polygon. - // resultCount - (out) number of results. - // maxResult - (int) maximum capacity of search results. - dtStatus findLocalNeighbourhood(dtPolyRef startRef, const float* centerPos, const float radius, - const dtQueryFilter* filter, - dtPolyRef* resultRef, dtPolyRef* resultParent, - int* resultCount, const int maxResult) const; - - // Returns wall segments of specified polygon. - // Params: - // ref - (in) ref to the polygon. - // filter - (in) path polygon filter. - // segments[6*maxSegments] - (out) wall segments (2 endpoints per segment). - // segmentCount - (out) number of wall segments. - // maxSegments - (in) max number of segments that can be stored in 'segments'. - dtStatus getPolyWallSegments(dtPolyRef ref, const dtQueryFilter* filter, - float* segments, int* segmentCount, const int maxSegments) const; - - // Returns closest point on navigation polygon. - // Uses detail polygons to find the closest point to the navigation polygon surface. - // Params: - // ref - (in) ref to the polygon. - // pos[3] - (in) the point to check. - // closest[3] - (out) closest point. - // Returns: true if closest point found. - dtStatus closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest) const; - - // Returns closest point on navigation polygon boundary. - // Uses the navigation polygon boundary to snap the point to poly boundary - // if it is outside the polygon. Much faster than closestPointToPoly. Does not affect height. - // Params: - // ref - (in) ref to the polygon. - // pos[3] - (in) the point to check. - // closest[3] - (out) closest point. - // Returns: true if closest point found. - dtStatus closestPointOnPolyBoundary(dtPolyRef ref, const float* pos, float* closest) const; - - // Returns start and end location of an off-mesh link polygon. - // Params: - // prevRef - (in) ref to the polygon before the link (used to select direction). - // polyRef - (in) ref to the off-mesh link polygon. - // startPos[3] - (out) start point of the link. - // endPos[3] - (out) end point of the link. - // Returns: true if link is found. - dtStatus getOffMeshConnectionPolyEndPoints(dtPolyRef prevRef, dtPolyRef polyRef, float* startPos, float* endPos) const; - - // Returns height of the polygon at specified location. - // Params: - // ref - (in) ref to the polygon. - // pos[3] - (in) the point where to locate the height. - // height - (out) height at the location. - // Returns: true if over polygon. - dtStatus getPolyHeight(dtPolyRef ref, const float* pos, float* height) const; - - // Returns true if poly reference ins in closed list. - bool isInClosedList(dtPolyRef ref) const; - - class dtNodePool* getNodePool() const { return m_nodePool; } - -private: - - // Returns neighbour tile based on side. - dtMeshTile* getNeighbourTileAt(int x, int y, int side) const; - - // Queries polygons within a tile. - int queryPolygonsInTile(const dtMeshTile* tile, const float* qmin, const float* qmax, const dtQueryFilter* filter, - dtPolyRef* polys, const int maxPolys) const; - // Find nearest polygon within a tile. - dtPolyRef findNearestPolyInTile(const dtMeshTile* tile, const float* center, const float* extents, - const dtQueryFilter* filter, float* nearestPt) const; - // Returns closest point on polygon. - dtStatus closestPointOnPolyInTile(const dtMeshTile* tile, const dtPoly* poly, const float* pos, float* closest) const; - - // Returns portal points between two polygons. - dtStatus getPortalPoints(dtPolyRef from, dtPolyRef to, float* left, float* right, - unsigned char& fromType, unsigned char& toType) const; - dtStatus getPortalPoints(dtPolyRef from, const dtPoly* fromPoly, const dtMeshTile* fromTile, - dtPolyRef to, const dtPoly* toPoly, const dtMeshTile* toTile, - float* left, float* right) const; - - // Returns edge mid point between two polygons. - dtStatus getEdgeMidPoint(dtPolyRef from, dtPolyRef to, float* mid) const; - dtStatus getEdgeMidPoint(dtPolyRef from, const dtPoly* fromPoly, const dtMeshTile* fromTile, - dtPolyRef to, const dtPoly* toPoly, const dtMeshTile* toTile, - float* mid) const; - - const dtNavMesh* m_nav; // Pointer to navmesh data. - - struct dtQueryData - { - dtStatus status; - struct dtNode* lastBestNode; - float lastBestNodeCost; - dtPolyRef startRef, endRef; - float startPos[3], endPos[3]; - const dtQueryFilter* filter; - }; - dtQueryData m_query; // Sliced query state. - - class dtNodePool* m_tinyNodePool; // Pointer to small node pool. - class dtNodePool* m_nodePool; // Pointer to node pool. - class dtNodeQueue* m_openList; // Pointer to open list queue. -}; - -// Helper function to allocate navmesh query class using Detour allocator. -dtNavMeshQuery* dtAllocNavMeshQuery(); -void dtFreeNavMeshQuery(dtNavMeshQuery* query); - -#endif // DETOURNAVMESHQUERY_H diff --git a/deps/recastnavigation/Detour/DetourNode.cpp b/deps/recastnavigation/Detour/DetourNode.cpp deleted file mode 100644 index f7811e3450..0000000000 --- a/deps/recastnavigation/Detour/DetourNode.cpp +++ /dev/null @@ -1,164 +0,0 @@ -// -// Copyright (c) 2009-2010 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 "DetourNode.h" -#include "DetourAlloc.h" -#include "DetourAssert.h" -#include "DetourCommon.h" -#include <string.h> - -inline unsigned int dtHashRef(dtPolyRef a) -{ - a = (~a) + (a << 18); - a = a ^ (a >> 31); - a = a * 21; - a = a ^ (a >> 11); - a = a + (a << 6); - a = a ^ (a >> 22); - return (unsigned int)a; -} - -////////////////////////////////////////////////////////////////////////////////////////// -dtNodePool::dtNodePool(int maxNodes, int hashSize) : - m_nodes(0), - m_first(0), - m_next(0), - m_maxNodes(maxNodes), - m_hashSize(hashSize), - m_nodeCount(0) -{ - dtAssert(dtNextPow2(m_hashSize) == (unsigned int)m_hashSize); - dtAssert(m_maxNodes > 0); - - m_nodes = (dtNode*)dtAlloc(sizeof(dtNode)*m_maxNodes, DT_ALLOC_PERM); - m_next = (dtNodeIndex*)dtAlloc(sizeof(dtNodeIndex)*m_maxNodes, DT_ALLOC_PERM); - m_first = (dtNodeIndex*)dtAlloc(sizeof(dtNodeIndex)*hashSize, DT_ALLOC_PERM); - - dtAssert(m_nodes); - dtAssert(m_next); - dtAssert(m_first); - - memset(m_first, 0xff, sizeof(dtNodeIndex)*m_hashSize); - memset(m_next, 0xff, sizeof(dtNodeIndex)*m_maxNodes); -} - -dtNodePool::~dtNodePool() -{ - dtFree(m_nodes); - dtFree(m_next); - dtFree(m_first); -} - -void dtNodePool::clear() -{ - memset(m_first, 0xff, sizeof(dtNodeIndex)*m_hashSize); - m_nodeCount = 0; -} - -dtNode* dtNodePool::findNode(dtPolyRef id) -{ - unsigned int bucket = dtHashRef(id) & (m_hashSize-1); - dtNodeIndex i = m_first[bucket]; - while (i != DT_NULL_IDX) - { - if (m_nodes[i].id == id) - return &m_nodes[i]; - i = m_next[i]; - } - return 0; -} - -dtNode* dtNodePool::getNode(dtPolyRef id) -{ - unsigned int bucket = dtHashRef(id) & (m_hashSize-1); - dtNodeIndex i = m_first[bucket]; - dtNode* node = 0; - while (i != DT_NULL_IDX) - { - if (m_nodes[i].id == id) - return &m_nodes[i]; - i = m_next[i]; - } - - if (m_nodeCount >= m_maxNodes) - return 0; - - i = (dtNodeIndex)m_nodeCount; - m_nodeCount++; - - // Init node - node = &m_nodes[i]; - node->pidx = 0; - node->cost = 0; - node->total = 0; - node->id = id; - node->flags = 0; - - m_next[i] = m_first[bucket]; - m_first[bucket] = i; - - return node; -} - - -////////////////////////////////////////////////////////////////////////////////////////// -dtNodeQueue::dtNodeQueue(int n) : - m_heap(0), - m_capacity(n), - m_size(0) -{ - dtAssert(m_capacity > 0); - - m_heap = (dtNode**)dtAlloc(sizeof(dtNode*)*(m_capacity+1), DT_ALLOC_PERM); - dtAssert(m_heap); -} - -dtNodeQueue::~dtNodeQueue() -{ - dtFree(m_heap); -} - -void dtNodeQueue::bubbleUp(int i, dtNode* node) -{ - int parent = (i-1)/2; - // note: (index > 0) means there is a parent - while ((i > 0) && (m_heap[parent]->total > node->total)) - { - m_heap[i] = m_heap[parent]; - i = parent; - parent = (i-1)/2; - } - m_heap[i] = node; -} - -void dtNodeQueue::trickleDown(int i, dtNode* node) -{ - int child = (i*2)+1; - while (child < m_size) - { - if (((child+1) < m_size) && - (m_heap[child]->total > m_heap[child+1]->total)) - { - child++; - } - m_heap[i] = m_heap[child]; - i = child; - child = (i*2)+1; - } - bubbleUp(i, node); -} diff --git a/deps/recastnavigation/Detour/DetourNode.h b/deps/recastnavigation/Detour/DetourNode.h deleted file mode 100644 index e46254fb50..0000000000 --- a/deps/recastnavigation/Detour/DetourNode.h +++ /dev/null @@ -1,159 +0,0 @@ -// -// Copyright (c) 2009-2010 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. -// - -#ifndef DETOURNODE_H -#define DETOURNODE_H - -#include "DetourNavMesh.h" - -enum dtNodeFlags -{ - DT_NODE_OPEN = 0x01, - DT_NODE_CLOSED = 0x02, -}; - -typedef unsigned short dtNodeIndex; -static const dtNodeIndex DT_NULL_IDX = ~0; - -struct dtNode -{ - float pos[3]; // Position of the node. - float cost; // Cost from previous node to current node. - float total; // Cost up to the node. - unsigned int pidx : 30; // Index to parent node. - unsigned int flags : 2; // Node flags 0/open/closed. - dtPolyRef id; // Polygon ref the node corresponds to. -}; - - -class dtNodePool -{ -public: - dtNodePool(int maxNodes, int hashSize); - ~dtNodePool(); - inline void operator=(const dtNodePool&) {} - void clear(); - dtNode* getNode(dtPolyRef id); - dtNode* findNode(dtPolyRef id); - - inline unsigned int getNodeIdx(const dtNode* node) const - { - if (!node) return 0; - return (unsigned int)(node - m_nodes)+1; - } - - inline dtNode* getNodeAtIdx(unsigned int idx) - { - if (!idx) return 0; - return &m_nodes[idx-1]; - } - - inline const dtNode* getNodeAtIdx(unsigned int idx) const - { - if (!idx) return 0; - return &m_nodes[idx-1]; - } - - inline int getMemUsed() const - { - return sizeof(*this) + - sizeof(dtNode)*m_maxNodes + - sizeof(dtNodeIndex)*m_maxNodes + - sizeof(dtNodeIndex)*m_hashSize; - } - - inline int getMaxNodes() const { return m_maxNodes; } - - inline int getHashSize() const { return m_hashSize; } - inline dtNodeIndex getFirst(int bucket) const { return m_first[bucket]; } - inline dtNodeIndex getNext(int i) const { return m_next[i]; } - -private: - - dtNode* m_nodes; - dtNodeIndex* m_first; - dtNodeIndex* m_next; - const int m_maxNodes; - const int m_hashSize; - int m_nodeCount; -}; - -class dtNodeQueue -{ -public: - dtNodeQueue(int n); - ~dtNodeQueue(); - inline void operator=(dtNodeQueue&) {} - - inline void clear() - { - m_size = 0; - } - - inline dtNode* top() - { - return m_heap[0]; - } - - inline dtNode* pop() - { - dtNode* result = m_heap[0]; - m_size--; - trickleDown(0, m_heap[m_size]); - return result; - } - - inline void push(dtNode* node) - { - m_size++; - bubbleUp(m_size-1, node); - } - - inline void modify(dtNode* node) - { - for (int i = 0; i < m_size; ++i) - { - if (m_heap[i] == node) - { - bubbleUp(i, node); - return; - } - } - } - - inline bool empty() const { return m_size == 0; } - - inline int getMemUsed() const - { - return sizeof(*this) + - sizeof(dtNode*)*(m_capacity+1); - } - - inline int getCapacity() const { return m_capacity; } - -private: - void bubbleUp(int i, dtNode* node); - void trickleDown(int i, dtNode* node); - - dtNode** m_heap; - const int m_capacity; - int m_size; -}; - - -#endif // DETOURNODE_H
\ No newline at end of file diff --git a/deps/recastnavigation/Detour/DetourObstacleAvoidance.cpp b/deps/recastnavigation/Detour/DetourObstacleAvoidance.cpp deleted file mode 100644 index a255c9b3fd..0000000000 --- a/deps/recastnavigation/Detour/DetourObstacleAvoidance.cpp +++ /dev/null @@ -1,532 +0,0 @@ -// -// Copyright (c) 2009-2010 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 "DetourObstacleAvoidance.h" -#include "DetourCommon.h" -#include "DetourAlloc.h" -#include "DetourAssert.h" -#include <string.h> -#include <math.h> -#include <float.h> -#include <new> - - -static int sweepCircleCircle(const float* c0, const float r0, const float* v, - const float* c1, const float r1, - float& tmin, float& tmax) -{ - static const float EPS = 0.0001f; - float s[3]; - dtVsub(s,c1,c0); - float r = r0+r1; - float c = dtVdot2D(s,s) - r*r; - float a = dtVdot2D(v,v); - if (a < EPS) return 0; // not moving - - // Overlap, calc time to exit. - float b = dtVdot2D(v,s); - float d = b*b - a*c; - if (d < 0.0f) return 0; // no intersection. - a = 1.0f / a; - const float rd = dtSqrt(d); - tmin = (b - rd) * a; - tmax = (b + rd) * a; - return 1; -} - -static int isectRaySeg(const float* ap, const float* u, - const float* bp, const float* bq, - float& t) -{ - float v[3], w[3]; - dtVsub(v,bq,bp); - dtVsub(w,ap,bp); - float d = dtVperp2D(u,v); - if (fabsf(d) < 1e-6f) return 0; - d = 1.0f/d; - t = dtVperp2D(v,w) * d; - if (t < 0 || t > 1) return 0; - float s = dtVperp2D(u,w) * d; - if (s < 0 || s > 1) return 0; - return 1; -} - - - -dtObstacleAvoidanceDebugData* dtAllocObstacleAvoidanceDebugData() -{ - void* mem = dtAlloc(sizeof(dtObstacleAvoidanceDebugData), DT_ALLOC_PERM); - if (!mem) return 0; - return new(mem) dtObstacleAvoidanceDebugData; -} - -void dtFreeObstacleAvoidanceDebugData(dtObstacleAvoidanceDebugData* ptr) -{ - if (!ptr) return; - ptr->~dtObstacleAvoidanceDebugData(); - dtFree(ptr); -} - - -dtObstacleAvoidanceDebugData::dtObstacleAvoidanceDebugData() : - m_nsamples(0), - m_maxSamples(0), - m_vel(0), - m_ssize(0), - m_pen(0), - m_vpen(0), - m_vcpen(0), - m_spen(0), - m_tpen(0) -{ -} - -dtObstacleAvoidanceDebugData::~dtObstacleAvoidanceDebugData() -{ - dtFree(m_vel); - dtFree(m_ssize); - dtFree(m_pen); - dtFree(m_vpen); - dtFree(m_vcpen); - dtFree(m_spen); - dtFree(m_tpen); -} - -bool dtObstacleAvoidanceDebugData::init(const int maxSamples) -{ - dtAssert(maxSamples); - m_maxSamples = maxSamples; - - m_vel = (float*)dtAlloc(sizeof(float)*3*m_maxSamples, DT_ALLOC_PERM); - if (!m_vel) - return false; - m_pen = (float*)dtAlloc(sizeof(float)*m_maxSamples, DT_ALLOC_PERM); - if (!m_pen) - return false; - m_ssize = (float*)dtAlloc(sizeof(float)*m_maxSamples, DT_ALLOC_PERM); - if (!m_ssize) - return false; - m_vpen = (float*)dtAlloc(sizeof(float)*m_maxSamples, DT_ALLOC_PERM); - if (!m_vpen) - return false; - m_vcpen = (float*)dtAlloc(sizeof(float)*m_maxSamples, DT_ALLOC_PERM); - if (!m_vcpen) - return false; - m_spen = (float*)dtAlloc(sizeof(float)*m_maxSamples, DT_ALLOC_PERM); - if (!m_spen) - return false; - m_tpen = (float*)dtAlloc(sizeof(float)*m_maxSamples, DT_ALLOC_PERM); - if (!m_tpen) - return false; - - return true; -} - -void dtObstacleAvoidanceDebugData::reset() -{ - m_nsamples = 0; -} - -void dtObstacleAvoidanceDebugData::addSample(const float* vel, const float ssize, const float pen, - const float vpen, const float vcpen, const float spen, const float tpen) -{ - if (m_nsamples >= m_maxSamples) - return; - dtAssert(m_vel); - dtAssert(m_ssize); - dtAssert(m_pen); - dtAssert(m_vpen); - dtAssert(m_vcpen); - dtAssert(m_spen); - dtAssert(m_tpen); - dtVcopy(&m_vel[m_nsamples*3], vel); - m_ssize[m_nsamples] = ssize; - m_pen[m_nsamples] = pen; - m_vpen[m_nsamples] = vpen; - m_vcpen[m_nsamples] = vcpen; - m_spen[m_nsamples] = spen; - m_tpen[m_nsamples] = tpen; - m_nsamples++; -} - -static void normalizeArray(float* arr, const int n) -{ - // Normalize penaly range. - float minPen = FLT_MAX; - float maxPen = -FLT_MAX; - for (int i = 0; i < n; ++i) - { - minPen = dtMin(minPen, arr[i]); - maxPen = dtMax(maxPen, arr[i]); - } - const float penRange = maxPen-minPen; - const float s = penRange > 0.001f ? (1.0f / penRange) : 1; - for (int i = 0; i < n; ++i) - arr[i] = dtClamp((arr[i]-minPen)*s, 0.0f, 1.0f); -} - -void dtObstacleAvoidanceDebugData::normalizeSamples() -{ - normalizeArray(m_pen, m_nsamples); - normalizeArray(m_vpen, m_nsamples); - normalizeArray(m_vcpen, m_nsamples); - normalizeArray(m_spen, m_nsamples); - normalizeArray(m_tpen, m_nsamples); -} - - -dtObstacleAvoidanceQuery* dtAllocObstacleAvoidanceQuery() -{ - void* mem = dtAlloc(sizeof(dtObstacleAvoidanceQuery), DT_ALLOC_PERM); - if (!mem) return 0; - return new(mem) dtObstacleAvoidanceQuery; -} - -void dtFreeObstacleAvoidanceQuery(dtObstacleAvoidanceQuery* ptr) -{ - if (!ptr) return; - ptr->~dtObstacleAvoidanceQuery(); - dtFree(ptr); -} - - -dtObstacleAvoidanceQuery::dtObstacleAvoidanceQuery() : - m_velBias(0.0f), - m_weightDesVel(0.0f), - m_weightCurVel(0.0f), - m_weightSide(0.0f), - m_weightToi(0.0f), - m_horizTime(0.0f), - m_maxCircles(0), - m_circles(0), - m_ncircles(0), - m_maxSegments(0), - m_segments(0), - m_nsegments(0) -{ -} - -dtObstacleAvoidanceQuery::~dtObstacleAvoidanceQuery() -{ - dtFree(m_circles); - dtFree(m_segments); -} - -bool dtObstacleAvoidanceQuery::init(const int maxCircles, const int maxSegments) -{ - m_maxCircles = maxCircles; - m_ncircles = 0; - m_circles = (dtObstacleCircle*)dtAlloc(sizeof(dtObstacleCircle)*m_maxCircles, DT_ALLOC_PERM); - if (!m_circles) - return false; - memset(m_circles, 0, sizeof(dtObstacleCircle)*m_maxCircles); - - m_maxSegments = maxSegments; - m_nsegments = 0; - m_segments = (dtObstacleSegment*)dtAlloc(sizeof(dtObstacleSegment)*m_maxSegments, DT_ALLOC_PERM); - if (!m_segments) - return false; - memset(m_segments, 0, sizeof(dtObstacleSegment)*m_maxSegments); - - return true; -} - -void dtObstacleAvoidanceQuery::reset() -{ - m_ncircles = 0; - m_nsegments = 0; -} - -void dtObstacleAvoidanceQuery::addCircle(const float* pos, const float rad, - const float* vel, const float* dvel) -{ - if (m_ncircles >= m_maxCircles) - return; - - dtObstacleCircle* cir = &m_circles[m_ncircles++]; - dtVcopy(cir->p, pos); - cir->rad = rad; - dtVcopy(cir->vel, vel); - dtVcopy(cir->dvel, dvel); -} - -void dtObstacleAvoidanceQuery::addSegment(const float* p, const float* q) -{ - if (m_nsegments > m_maxSegments) - return; - - dtObstacleSegment* seg = &m_segments[m_nsegments++]; - dtVcopy(seg->p, p); - dtVcopy(seg->q, q); -} - -void dtObstacleAvoidanceQuery::prepare(const float* pos, const float* dvel) -{ - // Prepare obstacles - for (int i = 0; i < m_ncircles; ++i) - { - dtObstacleCircle* cir = &m_circles[i]; - - // Side - const float* pa = pos; - const float* pb = cir->p; - - const float orig[3] = {0,0}; - float dv[3]; - dtVsub(cir->dp,pb,pa); - dtVnormalize(cir->dp); - dtVsub(dv, cir->dvel, dvel); - - const float a = dtTriArea2D(orig, cir->dp,dv); - if (a < 0.01f) - { - cir->np[0] = -cir->dp[2]; - cir->np[2] = cir->dp[0]; - } - else - { - cir->np[0] = cir->dp[2]; - cir->np[2] = -cir->dp[0]; - } - } - - for (int i = 0; i < m_nsegments; ++i) - { - dtObstacleSegment* seg = &m_segments[i]; - - // Precalc if the agent is really close to the segment. - const float r = 0.01f; - float t; - seg->touch = dtDistancePtSegSqr2D(pos, seg->p, seg->q, t) < dtSqr(r); - } -} - -float dtObstacleAvoidanceQuery::processSample(const float* vcand, const float cs, - const float* pos, const float rad, - const float vmax, const float* vel, const float* dvel, - dtObstacleAvoidanceDebugData* debug) -{ - // Find min time of impact and exit amongst all obstacles. - float tmin = m_horizTime; - float side = 0; - int nside = 0; - - for (int i = 0; i < m_ncircles; ++i) - { - const dtObstacleCircle* cir = &m_circles[i]; - - // RVO - float vab[3]; - dtVscale(vab, vcand, 2); - dtVsub(vab, vab, vel); - dtVsub(vab, vab, cir->vel); - - // Side - side += dtClamp(dtMin(dtVdot2D(cir->dp,vab)*0.5f+0.5f, dtVdot2D(cir->np,vab)*2), 0.0f, 1.0f); - nside++; - - float htmin = 0, htmax = 0; - if (!sweepCircleCircle(pos,rad, vab, cir->p,cir->rad, htmin, htmax)) - continue; - - // Handle overlapping obstacles. - if (htmin < 0.0f && htmax > 0.0f) - { - // Avoid more when overlapped. - htmin = -htmin * 0.5f; - } - - if (htmin >= 0.0f) - { - // The closest obstacle is somewhere ahead of us, keep track of nearest obstacle. - if (htmin < tmin) - tmin = htmin; - } - } - - for (int i = 0; i < m_nsegments; ++i) - { - const dtObstacleSegment* seg = &m_segments[i]; - float htmin = 0; - - if (seg->touch) - { - // Special case when the agent is very close to the segment. - float sdir[3], snorm[3]; - dtVsub(sdir, seg->q, seg->p); - snorm[0] = -sdir[2]; - snorm[2] = sdir[0]; - // If the velocity is pointing towards the segment, no collision. - if (dtVdot2D(snorm, vcand) < 0.0f) - continue; - // Else immediate collision. - htmin = 0.0f; - } - else - { - if (!isectRaySeg(pos, vcand, seg->p, seg->q, htmin)) - continue; - } - - // Avoid less when facing walls. - htmin *= 2.0f; - - // The closest obstacle is somewhere ahead of us, keep track of nearest obstacle. - if (htmin < tmin) - tmin = htmin; - } - - // Normalize side bias, to prevent it dominating too much. - if (nside) - side /= nside; - - const float ivmax = 1.0f / vmax; - const float vpen = m_weightDesVel * (dtVdist2D(vcand, dvel) * ivmax); - const float vcpen = m_weightCurVel * (dtVdist2D(vcand, vel) * ivmax); - const float spen = m_weightSide * side; - const float tpen = m_weightToi * (1.0f/(0.1f+tmin / m_horizTime)); - - const float penalty = vpen + vcpen + spen + tpen; - - // Store different penalties for debug viewing - if (debug) - debug->addSample(vcand, cs, penalty, vpen, vcpen, spen, tpen); - - return penalty; -} - -void dtObstacleAvoidanceQuery::sampleVelocityGrid(const float* pos, const float rad, const float vmax, - const float* vel, const float* dvel, - float* nvel, const int gsize, - dtObstacleAvoidanceDebugData* debug) -{ - prepare(pos, dvel); - - dtVset(nvel, 0,0,0); - - if (debug) - debug->reset(); - - const float cvx = dvel[0] * m_velBias; - const float cvz = dvel[2] * m_velBias; - const float cs = vmax * 2 * (1 - m_velBias) / (float)(gsize-1); - const float half = (gsize-1)*cs*0.5f; - - float minPenalty = FLT_MAX; - - for (int y = 0; y < gsize; ++y) - { - for (int x = 0; x < gsize; ++x) - { - float vcand[3]; - vcand[0] = cvx + x*cs - half; - vcand[1] = 0; - vcand[2] = cvz + y*cs - half; - - if (dtSqr(vcand[0])+dtSqr(vcand[2]) > dtSqr(vmax+cs/2)) continue; - - const float penalty = processSample(vcand, cs, pos,rad,vmax,vel,dvel, debug); - if (penalty < minPenalty) - { - minPenalty = penalty; - dtVcopy(nvel, vcand); - } - } - } -} - - -static const float DT_PI = 3.14159265f; - -void dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const float rad, const float vmax, - const float* vel, const float* dvel, float* nvel, - const int ndivs, const int nrings, const int depth, - dtObstacleAvoidanceDebugData* debug) -{ - prepare(pos, dvel); - - dtVset(nvel, 0,0,0); - - if (debug) - debug->reset(); - - // Build sampling pattern aligned to desired velocity. - static const int MAX_PATTERN_DIVS = 32; - static const int MAX_PATTERN_RINGS = 4; - float pat[(MAX_PATTERN_DIVS*MAX_PATTERN_RINGS+1)*2]; - int npat = 0; - - const int nd = dtClamp(ndivs, 1, MAX_PATTERN_DIVS); - const int nr = dtClamp(nrings, 1, MAX_PATTERN_RINGS); - const float da = (1.0f/nd) * DT_PI*2; - const float dang = atan2f(dvel[2], dvel[0]); - - // Always add sample at zero - pat[npat*2+0] = 0; - pat[npat*2+1] = 0; - npat++; - - for (int j = 0; j < nr; ++j) - { - const float rad = (float)(nr-j)/(float)nr; - float a = dang + (j&1)*0.5f*da; - for (int i = 0; i < nd; ++i) - { - pat[npat*2+0] = cosf(a)*rad; - pat[npat*2+1] = sinf(a)*rad; - npat++; - a += da; - } - } - - // Start sampling. - float cr = vmax * (1.0f-m_velBias); - float res[3]; - dtVset(res, dvel[0] * m_velBias, 0, dvel[2] * m_velBias); - - for (int k = 0; k < depth; ++k) - { - float minPenalty = FLT_MAX; - float bvel[3]; - dtVset(bvel, 0,0,0); - - for (int i = 0; i < npat; ++i) - { - float vcand[3]; - vcand[0] = res[0] + pat[i*2+0]*cr; - vcand[1] = 0; - vcand[2] = res[2] + pat[i*2+1]*cr; - - if (dtSqr(vcand[0])+dtSqr(vcand[2]) > dtSqr(vmax+0.001f)) continue; - - const float penalty = processSample(vcand,cr/10, pos,rad,vmax,vel,dvel, debug); - if (penalty < minPenalty) - { - minPenalty = penalty; - dtVcopy(bvel, vcand); - } - } - - dtVcopy(res, bvel); - - cr *= 0.5f; - } - - dtVcopy(nvel, res); -} - diff --git a/deps/recastnavigation/Detour/DetourObstacleAvoidance.h b/deps/recastnavigation/Detour/DetourObstacleAvoidance.h deleted file mode 100644 index 4a7187a799..0000000000 --- a/deps/recastnavigation/Detour/DetourObstacleAvoidance.h +++ /dev/null @@ -1,148 +0,0 @@ -// -// Copyright (c) 2009-2010 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. -// - -#ifndef DETOUROBSTACLEAVOIDANCE_H -#define DETOUROBSTACLEAVOIDANCE_H - -struct dtObstacleCircle -{ - float p[3]; // Position of the obstacle - float vel[3]; // Velocity of the obstacle - float dvel[3]; // Velocity of the obstacle - float rad; // Radius of the obstacle - float dp[3], np[3]; // Use for side selection during sampling. -}; - -struct dtObstacleSegment -{ - float p[3], q[3]; // End points of the obstacle segment - bool touch; -}; - -static const int RVO_SAMPLE_RAD = 15; -static const int MAX_RVO_SAMPLES = (RVO_SAMPLE_RAD*2+1)*(RVO_SAMPLE_RAD*2+1) + 100; - -class dtObstacleAvoidanceDebugData -{ -public: - dtObstacleAvoidanceDebugData(); - ~dtObstacleAvoidanceDebugData(); - - bool init(const int maxSamples); - void reset(); - void addSample(const float* vel, const float ssize, const float pen, - const float vpen, const float vcpen, const float spen, const float tpen); - - void normalizeSamples(); - - inline int getSampleCount() const { return m_nsamples; } - inline const float* getSampleVelocity(const int i) const { return &m_vel[i*3]; } - inline float getSampleSize(const int i) const { return m_ssize[i]; } - inline float getSamplePenalty(const int i) const { return m_pen[i]; } - inline float getSampleDesiredVelocityPenalty(const int i) const { return m_vpen[i]; } - inline float getSampleCurrentVelocityPenalty(const int i) const { return m_vcpen[i]; } - inline float getSamplePreferredSidePenalty(const int i) const { return m_spen[i]; } - inline float getSampleCollisionTimePenalty(const int i) const { return m_tpen[i]; } - -private: - int m_nsamples; - int m_maxSamples; - float* m_vel; - float* m_ssize; - float* m_pen; - float* m_vpen; - float* m_vcpen; - float* m_spen; - float* m_tpen; -}; - -dtObstacleAvoidanceDebugData* dtAllocObstacleAvoidanceDebugData(); -void dtFreeObstacleAvoidanceDebugData(dtObstacleAvoidanceDebugData* ptr); - - -class dtObstacleAvoidanceQuery -{ -public: - dtObstacleAvoidanceQuery(); - ~dtObstacleAvoidanceQuery(); - - bool init(const int maxCircles, const int maxSegments); - - void reset(); - - void addCircle(const float* pos, const float rad, - const float* vel, const float* dvel); - - void addSegment(const float* p, const float* q); - - inline void setVelocitySelectionBias(float v) { m_velBias = v; } - inline void setDesiredVelocityWeight(float w) { m_weightDesVel = w; } - inline void setCurrentVelocityWeight(float w) { m_weightCurVel = w; } - inline void setPreferredSideWeight(float w) { m_weightSide = w; } - inline void setCollisionTimeWeight(float w) { m_weightToi = w; } - inline void setTimeHorizon(float t) { m_horizTime = t; } - - void sampleVelocityGrid(const float* pos, const float rad, const float vmax, - const float* vel, const float* dvel, float* nvel, - const int gsize, - dtObstacleAvoidanceDebugData* debug = 0); - - void sampleVelocityAdaptive(const float* pos, const float rad, const float vmax, - const float* vel, const float* dvel, float* nvel, - const int ndivs, const int nrings, const int depth, - dtObstacleAvoidanceDebugData* debug = 0); - - inline int getObstacleCircleCount() const { return m_ncircles; } - const dtObstacleCircle* getObstacleCircle(const int i) { return &m_circles[i]; } - - inline int getObstacleSegmentCount() const { return m_nsegments; } - const dtObstacleSegment* getObstacleSegment(const int i) { return &m_segments[i]; } - -private: - - void prepare(const float* pos, const float* dvel); - - float processSample(const float* vcand, const float cs, - const float* pos, const float rad, - const float vmax, const float* vel, const float* dvel, - dtObstacleAvoidanceDebugData* debug); - - dtObstacleCircle* insertCircle(const float dist); - dtObstacleSegment* insertSegment(const float dist); - - float m_velBias; - float m_weightDesVel; - float m_weightCurVel; - float m_weightSide; - float m_weightToi; - float m_horizTime; - - int m_maxCircles; - dtObstacleCircle* m_circles; - int m_ncircles; - - int m_maxSegments; - dtObstacleSegment* m_segments; - int m_nsegments; -}; - -dtObstacleAvoidanceQuery* dtAllocObstacleAvoidanceQuery(); -void dtFreeObstacleAvoidanceQuery(dtObstacleAvoidanceQuery* ptr); - - -#endif // DETOUROBSTACLEAVOIDANCE_H
\ No newline at end of file |