summaryrefslogtreecommitdiff
path: root/deps/recastnavigation/Detour
diff options
context:
space:
mode:
authorYehonal <yehonal.azeroth@gmail.com>2017-12-21 11:26:43 +0100
committerYehonal <yehonal.azeroth@gmail.com>2017-12-21 11:26:43 +0100
commit403ed2600f39c1105b6641086808a6cdafbca1c5 (patch)
tree0422298438422af22f3f0723dfa62f66e10510f1 /deps/recastnavigation/Detour
parentacd60005e833efee3a10a2f1a4aee7ecced4cc97 (diff)
parenta0d17509a2c0b06d9e74c01d3fe7dc4df2c5c9a7 (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.txt27
-rw-r--r--deps/recastnavigation/Detour/DetourAlloc.cpp50
-rw-r--r--deps/recastnavigation/Detour/DetourAlloc.h36
-rw-r--r--deps/recastnavigation/Detour/DetourAssert.h33
-rw-r--r--deps/recastnavigation/Detour/DetourCommon.cpp329
-rw-r--r--deps/recastnavigation/Detour/DetourCommon.h248
-rw-r--r--deps/recastnavigation/Detour/DetourNavMesh.cpp1239
-rw-r--r--deps/recastnavigation/Detour/DetourNavMesh.h428
-rw-r--r--deps/recastnavigation/Detour/DetourNavMeshBuilder.cpp770
-rw-r--r--deps/recastnavigation/Detour/DetourNavMeshBuilder.h79
-rw-r--r--deps/recastnavigation/Detour/DetourNavMeshQuery.cpp2578
-rw-r--r--deps/recastnavigation/Detour/DetourNavMeshQuery.h407
-rw-r--r--deps/recastnavigation/Detour/DetourNode.cpp164
-rw-r--r--deps/recastnavigation/Detour/DetourNode.h159
-rw-r--r--deps/recastnavigation/Detour/DetourObstacleAvoidance.cpp532
-rw-r--r--deps/recastnavigation/Detour/DetourObstacleAvoidance.h148
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(&params);
- 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(&params->offMeshConVerts[(i*2+0)*3], params->bmin, params->bmax);
- offMeshConClass[i*2+1] = classifyOffMeshPoint(&params->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 = &params->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 = &params->verts[p[j]*3];
- const unsigned short* vb = &params->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 = &params->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 = &params->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 = &params->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 = &params->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 = &params->verts[poly->verts[j]*3];
- const unsigned short* vb = &params->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], &params->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 = &params->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