From 763a464e20d51dda0cddd92ccbd70ae99f021bea Mon Sep 17 00:00:00 2001 From: Jannis Date: Tue, 9 Jan 2024 06:15:39 +0100 Subject: [PATCH] update recastnavigation to 1.6.0 --- AmeisenNavigation.Server/src/Main.hpp | 2 +- recastnavigation/Detour/Include/DetourAlloc.h | 6 +- .../Detour/Include/DetourAssert.h | 8 +- .../Detour/Include/DetourCommon.h | 266 +- recastnavigation/Detour/Include/DetourMath.h | 5 +- .../Detour/Include/DetourNavMesh.h | 931 ++- .../Detour/Include/DetourNavMeshBuilder.h | 153 +- .../Detour/Include/DetourNavMeshQuery.h | 988 +-- recastnavigation/Detour/Include/DetourNode.h | 233 +- .../Detour/Include/DetourStatus.h | 11 +- .../Detour/Source/DetourAlloc.cpp | 22 +- .../Detour/Source/DetourAssert.cpp | 8 +- .../Detour/Source/DetourCommon.cpp | 617 +- .../Detour/Source/DetourNavMesh.cpp | 2486 +++---- .../Detour/Source/DetourNavMeshBuilder.cpp | 1426 ++-- .../Detour/Source/DetourNavMeshQuery.cpp | 6429 +++++++++-------- recastnavigation/Detour/Source/DetourNode.cpp | 243 +- 17 files changed, 6962 insertions(+), 6872 deletions(-) diff --git a/AmeisenNavigation.Server/src/Main.hpp b/AmeisenNavigation.Server/src/Main.hpp index 0251fd8..bd5d276 100644 --- a/AmeisenNavigation.Server/src/Main.hpp +++ b/AmeisenNavigation.Server/src/Main.hpp @@ -11,7 +11,7 @@ #include #include -constexpr auto AMEISENNAV_VERSION = "1.8.1.0"; +constexpr auto AMEISENNAV_VERSION = "1.8.2.0"; constexpr auto VEC3_SIZE = sizeof(float) * 3; diff --git a/recastnavigation/Detour/Include/DetourAlloc.h b/recastnavigation/Detour/Include/DetourAlloc.h index 888eb21..f87b454 100644 --- a/recastnavigation/Detour/Include/DetourAlloc.h +++ b/recastnavigation/Detour/Include/DetourAlloc.h @@ -25,8 +25,8 @@ /// memory is expected to be used. enum dtAllocHint { - DT_ALLOC_PERM, ///< Memory persist after a function call. - DT_ALLOC_TEMP ///< Memory used temporarily within a function. + DT_ALLOC_PERM, ///< Memory persist after a function call. + DT_ALLOC_TEMP ///< Memory used temporarily within a function. }; /// A memory allocation function. @@ -44,7 +44,7 @@ typedef void (dtFreeFunc)(void* ptr); /// Sets the base custom allocation functions to be used by Detour. /// @param[in] allocFunc The memory allocation function to be used by #dtAlloc /// @param[in] freeFunc The memory de-allocation function to be used by #dtFree -void dtAllocSetCustom(dtAllocFunc* allocFunc, dtFreeFunc* freeFunc); +void dtAllocSetCustom(dtAllocFunc *allocFunc, dtFreeFunc *freeFunc); /// Allocates a memory block. /// @param[in] size The size, in bytes of memory, to allocate. diff --git a/recastnavigation/Detour/Include/DetourAssert.h b/recastnavigation/Detour/Include/DetourAssert.h index 6bed3f9..038d538 100644 --- a/recastnavigation/Detour/Include/DetourAssert.h +++ b/recastnavigation/Detour/Include/DetourAssert.h @@ -24,8 +24,8 @@ #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) +// From https://web.archive.org/web/20210117002833/http://cnicholson.net/2009/02/stupid-c-tricks-adventures-in-assert/ +# define dtAssert(x) do { (void)sizeof(x); } while((void)(__LINE__==-1),false) #else @@ -38,12 +38,12 @@ typedef void (dtAssertFailFunc)(const char* expression, const char* file, int li /// Sets the base custom assertion failure function to be used by Detour. /// @param[in] assertFailFunc The function to be invoked in case of failure of #dtAssert -void dtAssertFailSetCustom(dtAssertFailFunc* assertFailFunc); +void dtAssertFailSetCustom(dtAssertFailFunc *assertFailFunc); /// Gets the base custom assertion failure function to be used by Detour. dtAssertFailFunc* dtAssertFailGetCustom(); -# include +# include # define dtAssert(expression) \ { \ dtAssertFailFunc* failFunc = dtAssertFailGetCustom(); \ diff --git a/recastnavigation/Detour/Include/DetourCommon.h b/recastnavigation/Detour/Include/DetourCommon.h index 62c3a8c..d54bc7e 100644 --- a/recastnavigation/Detour/Include/DetourCommon.h +++ b/recastnavigation/Detour/Include/DetourCommon.h @@ -25,18 +25,18 @@ /** @defgroup detour Detour -Members in this module are used to create, manipulate, and query navigation +Members in this module are used to create, manipulate, and query navigation meshes. -@note This is a summary list of members. Use the index or search +@note This is a summary list of members. Use the index or search feature to find minor members. */ /// @name General helper functions /// @{ + /// Used to ignore a function parameter. VS complains about unused parameters /// and this silences the warning. -/// @param [in] _ Unused parameter template void dtIgnoreUnused(const T&) { } /// Swaps the values of the two parameters. @@ -64,7 +64,7 @@ template inline T dtAbs(T a) { return a < 0 ? -a : a; } /// Returns the square of the value. /// @param[in] a The value. /// @return The square of the value. -template inline T dtSqr(T a) { return a * a; } +template inline T dtSqr(T a) { return a*a; } /// Clamps the value to the specified range. /// @param[in] v The value to clamp. @@ -76,15 +76,16 @@ template inline T dtClamp(T v, T mn, T mx) { return v < mn ? mn : (v > /// @} /// @name Vector helper functions. /// @{ + /// Derives the cross product of two vectors. (@p v1 x @p v2) /// @param[out] dest The cross product. [(x, y, z)] /// @param[in] v1 A Vector [(x, y, z)] /// @param[in] v2 A vector [(x, y, z)] 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]; + 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]; } /// Derives the dot product of two vectors. (@p v1 . @p v2) @@ -93,7 +94,7 @@ inline void dtVcross(float* dest, const float* v1, const float* v2) /// @return The dot product. inline float dtVdot(const float* v1, const float* v2) { - return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2]; + return v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2]; } /// Performs a scaled vector addition. (@p v1 + (@p v2 * @p s)) @@ -103,9 +104,9 @@ inline float dtVdot(const float* v1, const float* v2) /// @param[in] s The amount to scale @p v2 by before adding to @p v1. 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; + dest[0] = v1[0]+v2[0]*s; + dest[1] = v1[1]+v2[1]*s; + dest[2] = v1[2]+v2[2]*s; } /// Performs a linear interpolation between two vectors. (@p v1 toward @p v2) @@ -115,9 +116,9 @@ inline void dtVmad(float* dest, const float* v1, const float* v2, const float s) /// @param[in] t The interpolation factor. [Limits: 0 <= value <= 1.0] 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; + 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; } /// Performs a vector addition. (@p v1 + @p v2) @@ -126,9 +127,9 @@ inline void dtVlerp(float* dest, const float* v1, const float* v2, const float t /// @param[in] v2 The vector to add to @p v1. [(x, y, z)] 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]; + dest[0] = v1[0]+v2[0]; + dest[1] = v1[1]+v2[1]; + dest[2] = v1[2]+v2[2]; } /// Performs a vector subtraction. (@p v1 - @p v2) @@ -137,9 +138,9 @@ inline void dtVadd(float* dest, const float* v1, const float* v2) /// @param[in] v2 The vector to subtract from @p v1. [(x, y, z)] 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]; + dest[0] = v1[0]-v2[0]; + dest[1] = v1[1]-v2[1]; + dest[2] = v1[2]-v2[2]; } /// Scales the vector by the specified value. (@p v * @p t) @@ -148,9 +149,9 @@ inline void dtVsub(float* dest, const float* v1, const float* v2) /// @param[in] t The scaling factor. 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; + dest[0] = v[0]*t; + dest[1] = v[1]*t; + dest[2] = v[2]*t; } /// Selects the minimum value of each element from the specified vectors. @@ -158,9 +159,9 @@ inline void dtVscale(float* dest, const float* v, const float t) /// @param[in] v A vector. [(x, y, z)] 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]); + mn[0] = dtMin(mn[0], v[0]); + mn[1] = dtMin(mn[1], v[1]); + mn[2] = dtMin(mn[2], v[2]); } /// Selects the maximum value of each element from the specified vectors. @@ -168,9 +169,9 @@ inline void dtVmin(float* mn, const float* v) /// @param[in] v A vector. [(x, y, z)] 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]); + mx[0] = dtMax(mx[0], v[0]); + mx[1] = dtMax(mx[1], v[1]); + mx[2] = dtMax(mx[2], v[2]); } /// Sets the vector elements to the specified values. @@ -180,7 +181,7 @@ inline void dtVmax(float* mx, const float* v) /// @param[in] z The z-value of the vector. inline void dtVset(float* dest, const float x, const float y, const float z) { - dest[0] = x; dest[1] = y; dest[2] = z; + dest[0] = x; dest[1] = y; dest[2] = z; } /// Performs a vector copy. @@ -188,9 +189,9 @@ inline void dtVset(float* dest, const float x, const float y, const float z) /// @param[in] a The vector to copy. [(x, y, z)] inline void dtVcopy(float* dest, const float* a) { - dest[0] = a[0]; - dest[1] = a[1]; - dest[2] = a[2]; + dest[0] = a[0]; + dest[1] = a[1]; + dest[2] = a[2]; } /// Derives the scalar length of the vector. @@ -198,7 +199,7 @@ inline void dtVcopy(float* dest, const float* a) /// @return The scalar length of the vector. inline float dtVlen(const float* v) { - return dtMathSqrtf(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]); + return dtMathSqrtf(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]); } /// Derives the square of the scalar length of the vector. (len * len) @@ -206,7 +207,7 @@ inline float dtVlen(const float* v) /// @return The square of the scalar length of the vector. inline float dtVlenSqr(const float* v) { - return v[0] * v[0] + v[1] * v[1] + v[2] * v[2]; + return v[0]*v[0] + v[1]*v[1] + v[2]*v[2]; } /// Returns the distance between two points. @@ -215,10 +216,10 @@ inline float dtVlenSqr(const float* v) /// @return The distance between the two points. 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 dtMathSqrtf(dx * dx + dy * dy + dz * dz); + const float dx = v2[0] - v1[0]; + const float dy = v2[1] - v1[1]; + const float dz = v2[2] - v1[2]; + return dtMathSqrtf(dx*dx + dy*dy + dz*dz); } /// Returns the square of the distance between two points. @@ -227,10 +228,10 @@ inline float dtVdist(const float* v1, const float* v2) /// @return The square of the distance between the two points. 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; + 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; } /// Derives the distance between the specified points on the xz-plane. @@ -241,9 +242,9 @@ inline float dtVdistSqr(const float* v1, const float* v2) /// The vectors are projected onto the xz-plane, so the y-values are ignored. inline float dtVdist2D(const float* v1, const float* v2) { - const float dx = v2[0] - v1[0]; - const float dz = v2[2] - v1[2]; - return dtMathSqrtf(dx * dx + dz * dz); + const float dx = v2[0] - v1[0]; + const float dz = v2[2] - v1[2]; + return dtMathSqrtf(dx*dx + dz*dz); } /// Derives the square of the distance between the specified points on the xz-plane. @@ -252,19 +253,19 @@ inline float dtVdist2D(const float* v1, const float* v2) /// @return The square of the distance between the point on the xz-plane. 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; + const float dx = v2[0] - v1[0]; + const float dz = v2[2] - v1[2]; + return dx*dx + dz*dz; } /// Normalizes the vector. /// @param[in,out] v The vector to normalize. [(x, y, z)] inline void dtVnormalize(float* v) { - float d = 1.0f / dtMathSqrtf(dtSqr(v[0]) + dtSqr(v[1]) + dtSqr(v[2])); - v[0] *= d; - v[1] *= d; - v[2] *= d; + float d = 1.0f / dtMathSqrtf(dtSqr(v[0]) + dtSqr(v[1]) + dtSqr(v[2])); + v[0] *= d; + v[1] *= d; + v[2] *= d; } /// Performs a 'sloppy' colocation check of the specified points. @@ -272,13 +273,13 @@ inline void dtVnormalize(float* v) /// @param[in] p1 A point. [(x, y, z)] /// @return True if the points are considered to be at the same location. /// -/// Basically, this function will return true if the specified points are +/// Basically, this function will return true if the specified points are /// close enough to eachother to be considered colocated. 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; + static const float thr = dtSqr(1.0f/16384.0f); + const float d = dtVdistSqr(p0, p1); + return d < thr; } /// Checks that the specified vector's components are all finite. @@ -287,20 +288,20 @@ inline bool dtVequal(const float* p0, const float* p1) /// or any of the infinities. inline bool dtVisfinite(const float* v) { - bool result = - dtMathIsfinite(v[0]) && - dtMathIsfinite(v[1]) && - dtMathIsfinite(v[2]); + bool result = + dtMathIsfinite(v[0]) && + dtMathIsfinite(v[1]) && + dtMathIsfinite(v[2]); - return result; + return result; } /// Checks that the specified vector's 2D components are finite. /// @param[in] v A point. [(x, y, z)] inline bool dtVisfinite2D(const float* v) { - bool result = dtMathIsfinite(v[0]) && dtMathIsfinite(v[2]); - return result; + bool result = dtMathIsfinite(v[0]) && dtMathIsfinite(v[2]); + return result; } /// Derives the dot product of two vectors on the xz-plane. (@p u . @p v) @@ -311,23 +312,24 @@ inline bool dtVisfinite2D(const float* v) /// The vectors are projected onto the xz-plane, so the y-values are ignored. inline float dtVdot2D(const float* u, const float* v) { - return u[0] * v[0] + u[2] * v[2]; + return u[0]*v[0] + u[2]*v[2]; } /// Derives the xz-plane 2D perp product of the two vectors. (uz*vx - ux*vz) /// @param[in] u The LHV vector [(x, y, z)] /// @param[in] v The RHV vector [(x, y, z)] -/// @return The dot product on the xz-plane. +/// @return The perp dot product on the xz-plane. /// /// The vectors are projected onto the xz-plane, so the y-values are ignored. inline float dtVperp2D(const float* u, const float* v) { - return u[2] * v[0] - u[0] * v[2]; + return u[2]*v[0] - u[0]*v[2]; } /// @} /// @name Computational geometry helper functions. /// @{ + /// Derives the signed xz-plane area of the triangle ABC, or the relationship of line AB to point C. /// @param[in] a Vertex A. [(x, y, z)] /// @param[in] b Vertex B. [(x, y, z)] @@ -335,11 +337,11 @@ inline float dtVperp2D(const float* u, const float* v) /// @return The signed xz-plane area of the triangle. 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; + 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; } /// Determines if two axis-aligned bounding boxes overlap. @@ -350,13 +352,13 @@ inline float dtTriArea2D(const float* a, const float* b, const float* c) /// @return True if the two AABB's overlap. /// @see dtOverlapBounds inline bool dtOverlapQuantBounds(const unsigned short amin[3], const unsigned short amax[3], - const unsigned short bmin[3], const unsigned short bmax[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; + 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; } /// Determines if two axis-aligned bounding boxes overlap. @@ -367,23 +369,23 @@ inline bool dtOverlapQuantBounds(const unsigned short amin[3], const unsigned sh /// @return True if the two AABB's overlap. /// @see dtOverlapQuantBounds inline bool dtOverlapBounds(const float* amin, const float* amax, - const float* bmin, const float* bmax) + 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; + 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; } /// Derives the closest point on a triangle from the specified reference point. -/// @param[out] closest The closest point on the triangle. +/// @param[out] closest The closest point on the triangle. /// @param[in] p The reference point from which to test. [(x, y, z)] /// @param[in] a Vertex A of triangle ABC. [(x, y, z)] /// @param[in] b Vertex B of triangle ABC. [(x, y, z)] /// @param[in] c Vertex C of triangle ABC. [(x, y, z)] void dtClosestPtPointTriangle(float* closest, const float* p, - const float* a, const float* b, const float* c); + const float* a, const float* b, const float* c); /// Derives the y-axis height of the closest point on the triangle from the specified reference point. /// @param[in] p The reference point from which to test. [(x, y, z)] @@ -394,13 +396,13 @@ void dtClosestPtPointTriangle(float* closest, const float* p, 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); + const float* verts, int nverts, + float& tmin, float& tmax, + int& segMin, int& segMax); bool dtIntersectSegSeg2D(const float* ap, const float* aq, - const float* bp, const float* bq, - float& s, float& t); + const float* bp, const float* bq, + float& s, float& t); /// Determines if the specified point is inside the convex polygon on the xz-plane. /// @param[in] pt The point to check. [(x, y, z)] @@ -410,7 +412,7 @@ bool dtIntersectSegSeg2D(const float* ap, const float* aq, 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* ed, float* et); float dtDistancePtSegSqr2D(const float* pt, const float* p, const float* q, float& t); @@ -428,95 +430,97 @@ void dtCalcPolyCenter(float* tc, const unsigned short* idx, int nidx, const floa /// @param[in] npolyb The number of vertices in polygon B. /// @return True if the two polygons overlap. bool dtOverlapPolyPoly2D(const float* polya, const int npolya, - const float* polyb, const int npolyb); + const float* polyb, const int npolyb); /// @} /// @name Miscellanious functions. /// @{ + 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; + 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; + 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 dtAlign4(int x) { return (x+3) & ~3; } -inline int dtOppositeTile(int side) { return (side + 4) & 0x7; } +inline int dtOppositeTile(int side) { return (side+4) & 0x7; } inline void dtSwapByte(unsigned char* a, unsigned char* b) { - unsigned char tmp = *a; - *a = *b; - *b = tmp; + unsigned char tmp = *a; + *a = *b; + *b = tmp; } inline void dtSwapEndian(unsigned short* v) { - unsigned char* x = (unsigned char*)v; - dtSwapByte(x + 0, x + 1); + unsigned char* x = (unsigned char*)v; + dtSwapByte(x+0, x+1); } inline void dtSwapEndian(short* v) { - unsigned char* x = (unsigned char*)v; - dtSwapByte(x + 0, x + 1); + unsigned char* x = (unsigned char*)v; + dtSwapByte(x+0, x+1); } inline void dtSwapEndian(unsigned int* v) { - unsigned char* x = (unsigned char*)v; - dtSwapByte(x + 0, x + 3); dtSwapByte(x + 1, x + 2); + unsigned char* x = (unsigned char*)v; + dtSwapByte(x+0, x+3); dtSwapByte(x+1, x+2); } inline void dtSwapEndian(int* v) { - unsigned char* x = (unsigned char*)v; - dtSwapByte(x + 0, x + 3); dtSwapByte(x + 1, x + 2); + unsigned char* x = (unsigned char*)v; + dtSwapByte(x+0, x+3); dtSwapByte(x+1, x+2); } inline void dtSwapEndian(float* v) { - unsigned char* x = (unsigned char*)v; - dtSwapByte(x + 0, x + 3); dtSwapByte(x + 1, x + 2); + unsigned char* x = (unsigned char*)v; + dtSwapByte(x+0, x+3); dtSwapByte(x+1, x+2); } void dtRandomPointInConvexPoly(const float* pts, const int npts, float* areas, - const float s, const float t, float* out); + const float s, const float t, float* out); template TypeToRetrieveAs* dtGetThenAdvanceBufferPointer(const unsigned char*& buffer, const size_t distanceToAdvance) { - TypeToRetrieveAs* returnPointer = reinterpret_cast(buffer); - buffer += distanceToAdvance; - return returnPointer; + TypeToRetrieveAs* returnPointer = reinterpret_cast(buffer); + buffer += distanceToAdvance; + return returnPointer; } template TypeToRetrieveAs* dtGetThenAdvanceBufferPointer(unsigned char*& buffer, const size_t distanceToAdvance) { - TypeToRetrieveAs* returnPointer = reinterpret_cast(buffer); - buffer += distanceToAdvance; - return returnPointer; + TypeToRetrieveAs* returnPointer = reinterpret_cast(buffer); + buffer += distanceToAdvance; + return returnPointer; } + /// @} #endif // DETOURCOMMON_H diff --git a/recastnavigation/Detour/Include/DetourMath.h b/recastnavigation/Detour/Include/DetourMath.h index 54af8af..5528ead 100644 --- a/recastnavigation/Detour/Include/DetourMath.h +++ b/recastnavigation/Detour/Include/DetourMath.h @@ -8,9 +8,6 @@ Members in this module are wrappers around the standard math library #define DETOURMATH_H #include -// This include is required because libstdc++ has problems with isfinite -// if cmath is included before math.h. -#include inline float dtMathFabsf(float x) { return fabsf(x); } inline float dtMathSqrtf(float x) { return sqrtf(x); } @@ -19,6 +16,6 @@ inline float dtMathCeilf(float x) { return ceilf(x); } inline float dtMathCosf(float x) { return cosf(x); } inline float dtMathSinf(float x) { return sinf(x); } inline float dtMathAtan2f(float y, float x) { return atan2f(y, x); } -inline bool dtMathIsfinite(float x) { return std::isfinite(x); } +inline bool dtMathIsfinite(float x) { return isfinite(x); } #endif diff --git a/recastnavigation/Detour/Include/DetourNavMesh.h b/recastnavigation/Detour/Include/DetourNavMesh.h index c71822f..4c1277d 100644 --- a/recastnavigation/Detour/Include/DetourNavMesh.h +++ b/recastnavigation/Detour/Include/DetourNavMesh.h @@ -25,25 +25,13 @@ // Undefine (or define in a build cofnig) the following line to use 64bit polyref. // Generally not needed, useful for very large worlds. // Note: tiles build using 32bit refs are not compatible with 64bit refs! -#define DT_POLYREF64 1 +//#define DT_POLYREF64 1 #ifdef DT_POLYREF64 // TODO: figure out a multiplatform version of uint64_t // - maybe: https://code.google.com/p/msinttypes/ // - or: http://www.azillionmonkeys.com/qed/pstdint.h -#if defined(WIN32) && !defined(__MINGW32__) -/// Do not rename back to uint64. Otherwise mac complains about typedef redefinition -typedef unsigned __int64 uint64_d; -#else #include -#ifndef uint64_t -#ifdef __linux__ -#include -#endif -#endif -/// Do not rename back to uint64. Otherwise mac complains about typedef redefinition -typedef uint64_t uint64_d; -#endif #endif // Note: If you want to use 64-bit refs, change the types of both dtPolyRef & dtTileRef. @@ -52,10 +40,10 @@ typedef uint64_t uint64_d; /// A handle to a polygon within a navigation mesh tile. /// @ingroup detour #ifdef DT_POLYREF64 -static const unsigned int DT_SALT_BITS = 12; -static const unsigned int DT_TILE_BITS = 21; -static const unsigned int DT_POLY_BITS = 31; -typedef uint64_d dtPolyRef; +static const unsigned int DT_SALT_BITS = 16; +static const unsigned int DT_TILE_BITS = 28; +static const unsigned int DT_POLY_BITS = 20; +typedef uint64_t dtPolyRef; #else typedef unsigned int dtPolyRef; #endif @@ -63,7 +51,7 @@ typedef unsigned int dtPolyRef; /// A handle to a tile within a navigation mesh. /// @ingroup detour #ifdef DT_POLYREF64 -typedef uint64_d dtTileRef; +typedef uint64_t dtTileRef; #else typedef unsigned int dtTileRef; #endif @@ -79,13 +67,13 @@ static const int DT_VERTS_PER_POLYGON = 6; /// /// A magic number used to detect compatibility of navigation tile data. -static const int DT_NAVMESH_MAGIC = 'D' << 24 | 'N' << 16 | 'A' << 8 | 'V'; +static const int DT_NAVMESH_MAGIC = 'D'<<24 | 'N'<<16 | 'A'<<8 | 'V'; /// A version number used to detect compatibility of navigation tile data. static const int DT_NAVMESH_VERSION = 7; /// A magic number used to detect the compatibility of navigation tile states. -static const int DT_NAVMESH_STATE_MAGIC = 'D' << 24 | 'N' << 16 | 'M' << 8 | 'S'; +static const int DT_NAVMESH_STATE_MAGIC = 'D'<<24 | 'N'<<16 | 'M'<<8 | 'S'; /// A version number used to detect compatibility of navigation tile states. static const int DT_NAVMESH_STATE_VERSION = 1; @@ -110,42 +98,44 @@ static const int DT_MAX_AREAS = 64; /// For an example, see dtNavMesh::addTile(). enum dtTileFlags { - /// The navigation mesh owns the tile memory and is responsible for freeing it. - DT_TILE_FREE_DATA = 0x01, + /// The navigation mesh owns the tile memory and is responsible for freeing it. + DT_TILE_FREE_DATA = 0x01 }; /// Vertex flags returned by dtNavMeshQuery::findStraightPath. enum dtStraightPathFlags { - DT_STRAIGHTPATH_START = 0x01, ///< The vertex is the start position in the path. - DT_STRAIGHTPATH_END = 0x02, ///< The vertex is the end position in the path. - DT_STRAIGHTPATH_OFFMESH_CONNECTION = 0x04, ///< The vertex is the start of an off-mesh connection. + DT_STRAIGHTPATH_START = 0x01, ///< The vertex is the start position in the path. + DT_STRAIGHTPATH_END = 0x02, ///< The vertex is the end position in the path. + DT_STRAIGHTPATH_OFFMESH_CONNECTION = 0x04 ///< The vertex is the start of an off-mesh connection. }; /// Options for dtNavMeshQuery::findStraightPath. enum dtStraightPathOptions { - DT_STRAIGHTPATH_AREA_CROSSINGS = 0x01, ///< Add a vertex at every polygon edge crossing where area changes. - DT_STRAIGHTPATH_ALL_CROSSINGS = 0x02, ///< Add a vertex at every polygon edge crossing. + DT_STRAIGHTPATH_AREA_CROSSINGS = 0x01, ///< Add a vertex at every polygon edge crossing where area changes. + DT_STRAIGHTPATH_ALL_CROSSINGS = 0x02 ///< Add a vertex at every polygon edge crossing. }; + /// Options for dtNavMeshQuery::initSlicedFindPath and updateSlicedFindPath enum dtFindPathOptions { - DT_FINDPATH_ANY_ANGLE = 0x02, ///< use raycasts during pathfind to "shortcut" (raycast still consider costs) + DT_FINDPATH_ANY_ANGLE = 0x02 ///< use raycasts during pathfind to "shortcut" (raycast still consider costs) }; /// Options for dtNavMeshQuery::raycast enum dtRaycastOptions { - DT_RAYCAST_USE_COSTS = 0x01, ///< Raycast should calculate movement cost along the ray and fill RaycastHit::cost + DT_RAYCAST_USE_COSTS = 0x01 ///< Raycast should calculate movement cost along the ray and fill RaycastHit::cost }; enum dtDetailTriEdgeFlags { - DT_DETAIL_EDGE_BOUNDARY = 0x01, ///< Detail triangle edge is part of the poly boundary + DT_DETAIL_EDGE_BOUNDARY = 0x01 ///< Detail triangle edge is part of the poly boundary }; + /// Limit raycasting during any angle pahfinding /// The limit is given as a multiple of the character radius static const float DT_RAY_CAST_LIMIT_PROPORTIONS = 50.0f; @@ -153,56 +143,57 @@ static const float DT_RAY_CAST_LIMIT_PROPORTIONS = 50.0f; /// Flags representing the type of a navigation mesh polygon. enum dtPolyTypes { - /// The polygon is a standard convex polygon that is part of the surface of the mesh. - DT_POLYTYPE_GROUND = 0, - /// The polygon is an off-mesh connection consisting of two vertices. - DT_POLYTYPE_OFFMESH_CONNECTION = 1, + /// The polygon is a standard convex polygon that is part of the surface of the mesh. + DT_POLYTYPE_GROUND = 0, + /// The polygon is an off-mesh connection consisting of two vertices. + DT_POLYTYPE_OFFMESH_CONNECTION = 1 }; + /// Defines a polygon within a dtMeshTile object. /// @ingroup detour struct dtPoly { - /// Index to first link in linked list. (Or #DT_NULL_LINK if there is no link.) - unsigned int firstLink; + /// Index to first link in linked list. (Or #DT_NULL_LINK if there is no link.) + unsigned int firstLink; - /// The indices of the polygon's vertices. - /// The actual vertices are located in dtMeshTile::verts. - unsigned short verts[DT_VERTS_PER_POLYGON]; + /// The indices of the polygon's vertices. + /// The actual vertices are located in dtMeshTile::verts. + unsigned short verts[DT_VERTS_PER_POLYGON]; - /// Packed data representing neighbor polygons references and flags for each edge. - unsigned short neis[DT_VERTS_PER_POLYGON]; + /// Packed data representing neighbor polygons references and flags for each edge. + unsigned short neis[DT_VERTS_PER_POLYGON]; - /// The user defined polygon flags. - unsigned short flags; + /// The user defined polygon flags. + unsigned short flags; - /// The number of vertices in the polygon. - unsigned char vertCount; + /// The number of vertices in the polygon. + unsigned char vertCount; - /// The bit packed area id and polygon type. - /// @note Use the structure's set and get methods to acess this value. - unsigned char areaAndtype; + /// The bit packed area id and polygon type. + /// @note Use the structure's set and get methods to acess this value. + unsigned char areaAndtype; - /// Sets the user defined area id. [Limit: < #DT_MAX_AREAS] - inline void setArea(unsigned char a) { areaAndtype = (areaAndtype & 0xc0) | (a & 0x3f); } + /// Sets the user defined area id. [Limit: < #DT_MAX_AREAS] + inline void setArea(unsigned char a) { areaAndtype = (areaAndtype & 0xc0) | (a & 0x3f); } - /// Sets the polygon type. (See: #dtPolyTypes.) - inline void setType(unsigned char t) { areaAndtype = (areaAndtype & 0x3f) | (t << 6); } + /// Sets the polygon type. (See: #dtPolyTypes.) + inline void setType(unsigned char t) { areaAndtype = (areaAndtype & 0x3f) | (t << 6); } - /// Gets the user defined area id. - inline unsigned char getArea() const { return areaAndtype & 0x3f; } + /// Gets the user defined area id. + inline unsigned char getArea() const { return areaAndtype & 0x3f; } - /// Gets the polygon type. (See: #dtPolyTypes) - inline unsigned char getType() const { return areaAndtype >> 6; } + /// Gets the polygon type. (See: #dtPolyTypes) + inline unsigned char getType() const { return areaAndtype >> 6; } }; /// Defines the location of detail sub-mesh data within a dtMeshTile. struct dtPolyDetail { - unsigned int vertBase; ///< The offset of the vertices in the dtMeshTile::detailVerts array. - unsigned int triBase; ///< The offset of the triangles in the dtMeshTile::detailTris array. - unsigned char vertCount; ///< The number of vertices in the sub-mesh. - unsigned char triCount; ///< The number of triangles in the sub-mesh. + unsigned int vertBase; ///< The offset of the vertices in the dtMeshTile::detailVerts array. + unsigned int triBase; ///< The offset of the triangles in the dtMeshTile::detailTris array. + unsigned char vertCount; ///< The number of vertices in the sub-mesh. + unsigned char triCount; ///< The number of triangles in the sub-mesh. }; /// Defines a link between polygons. @@ -210,12 +201,12 @@ struct dtPolyDetail /// @see dtMeshTile struct dtLink { - dtPolyRef ref; ///< Neighbour reference. (The neighbor that is linked to.) - unsigned int next; ///< Index of the next link. - unsigned char edge; ///< Index of the polygon edge that owns this link. - unsigned char side; ///< If a boundary link, defines on which side the link is. - unsigned char bmin; ///< If a boundary link, defines the minimum sub-edge area. - unsigned char bmax; ///< If a boundary link, defines the maximum sub-edge area. + dtPolyRef ref; ///< Neighbour reference. (The neighbor that is linked to.) + unsigned int next; ///< Index of the next link. + unsigned char edge; ///< Index of the polygon edge that owns this link. + unsigned char side; ///< If a boundary link, defines on which side the link is. + unsigned char bmin; ///< If a boundary link, defines the minimum sub-edge area. + unsigned char bmax; ///< If a boundary link, defines the maximum sub-edge area. }; /// Bounding volume node. @@ -223,110 +214,110 @@ struct dtLink /// @see dtMeshTile struct dtBVNode { - unsigned short bmin[3]; ///< Minimum bounds of the node's AABB. [(x, y, z)] - unsigned short bmax[3]; ///< Maximum bounds of the node's AABB. [(x, y, z)] - int i; ///< The node's index. (Negative for escape sequence.) + unsigned short bmin[3]; ///< Minimum bounds of the node's AABB. [(x, y, z)] + unsigned short bmax[3]; ///< Maximum bounds of the node's AABB. [(x, y, z)] + int i; ///< The node's index. (Negative for escape sequence.) }; /// Defines an navigation mesh off-mesh connection within a dtMeshTile object. /// An off-mesh connection is a user defined traversable connection made up to two vertices. struct dtOffMeshConnection { - /// The endpoints of the connection. [(ax, ay, az, bx, by, bz)] - float pos[6]; + /// The endpoints of the connection. [(ax, ay, az, bx, by, bz)] + float pos[6]; - /// The radius of the endpoints. [Limit: >= 0] - float rad; + /// The radius of the endpoints. [Limit: >= 0] + float rad; - /// The polygon reference of the connection within the tile. - unsigned short poly; + /// The polygon reference of the connection within the tile. + unsigned short poly; - /// Link flags. - /// @note These are not the connection's user defined flags. Those are assigned via the - /// connection's dtPoly definition. These are link flags used for internal purposes. - unsigned char flags; + /// Link flags. + /// @note These are not the connection's user defined flags. Those are assigned via the + /// connection's dtPoly definition. These are link flags used for internal purposes. + unsigned char flags; - /// End point side. - unsigned char side; + /// End point side. + unsigned char side; - /// The id of the offmesh connection. (User assigned when the navigation mesh is built.) - unsigned int userId; + /// The id of the offmesh connection. (User assigned when the navigation mesh is built.) + unsigned int userId; }; /// Provides high level information related to a dtMeshTile object. /// @ingroup detour struct dtMeshHeader { - int magic; ///< Tile magic number. (Used to identify the data format.) - int version; ///< Tile data format version number. - int x; ///< The x-position of the tile within the dtNavMesh tile grid. (x, y, layer) - int y; ///< The y-position of the tile within the dtNavMesh tile grid. (x, y, layer) - int layer; ///< The layer of the tile within the dtNavMesh tile grid. (x, y, layer) - unsigned int userId; ///< The user defined id of the tile. - int polyCount; ///< The number of polygons in the tile. - int vertCount; ///< The number of vertices in the tile. - int maxLinkCount; ///< The number of allocated links. - int detailMeshCount; ///< The number of sub-meshes in the detail mesh. - - /// The number of unique vertices in the detail mesh. (In addition to the polygon vertices.) - int detailVertCount; - - int detailTriCount; ///< The number of triangles in the detail mesh. - int bvNodeCount; ///< The number of bounding volume nodes. (Zero if bounding volumes are disabled.) - int offMeshConCount; ///< The number of off-mesh connections. - int offMeshBase; ///< The index of the first polygon which is an off-mesh connection. - float walkableHeight; ///< The height of the agents using the tile. - float walkableRadius; ///< The radius of the agents using the tile. - float walkableClimb; ///< The maximum climb height of the agents using the tile. - float bmin[3]; ///< The minimum bounds of the tile's AABB. [(x, y, z)] - float bmax[3]; ///< The maximum bounds of the tile's AABB. [(x, y, z)] - - /// The bounding volume quantization factor. - float bvQuantFactor; + int magic; ///< Tile magic number. (Used to identify the data format.) + int version; ///< Tile data format version number. + int x; ///< The x-position of the tile within the dtNavMesh tile grid. (x, y, layer) + int y; ///< The y-position of the tile within the dtNavMesh tile grid. (x, y, layer) + int layer; ///< The layer of the tile within the dtNavMesh tile grid. (x, y, layer) + unsigned int userId; ///< The user defined id of the tile. + int polyCount; ///< The number of polygons in the tile. + int vertCount; ///< The number of vertices in the tile. + int maxLinkCount; ///< The number of allocated links. + int detailMeshCount; ///< The number of sub-meshes in the detail mesh. + + /// The number of unique vertices in the detail mesh. (In addition to the polygon vertices.) + int detailVertCount; + + int detailTriCount; ///< The number of triangles in the detail mesh. + int bvNodeCount; ///< The number of bounding volume nodes. (Zero if bounding volumes are disabled.) + int offMeshConCount; ///< The number of off-mesh connections. + int offMeshBase; ///< The index of the first polygon which is an off-mesh connection. + float walkableHeight; ///< The height of the agents using the tile. + float walkableRadius; ///< The radius of the agents using the tile. + float walkableClimb; ///< The maximum climb height of the agents using the tile. + float bmin[3]; ///< The minimum bounds of the tile's AABB. [(x, y, z)] + float bmax[3]; ///< The maximum bounds of the tile's AABB. [(x, y, z)] + + /// The bounding volume quantization factor. + float bvQuantFactor; }; /// Defines a navigation mesh tile. /// @ingroup detour struct dtMeshTile { - unsigned int salt; ///< Counter describing modifications to the tile. - - unsigned int linksFreeList; ///< Index to the next free link. - dtMeshHeader* header; ///< The tile header. - dtPoly* polys; ///< The tile polygons. [Size: dtMeshHeader::polyCount] - float* verts; ///< The tile vertices. [Size: dtMeshHeader::vertCount] - dtLink* links; ///< The tile links. [Size: dtMeshHeader::maxLinkCount] - dtPolyDetail* detailMeshes; ///< The tile's detail sub-meshes. [Size: dtMeshHeader::detailMeshCount] - - /// The detail mesh's unique vertices. [(x, y, z) * dtMeshHeader::detailVertCount] - float* detailVerts; - - /// The detail mesh's triangles. [(vertA, vertB, vertC, triFlags) * dtMeshHeader::detailTriCount]. - /// See dtDetailTriEdgeFlags and dtGetDetailTriEdgeFlags. - unsigned char* detailTris; - - /// The tile bounding volume nodes. [Size: dtMeshHeader::bvNodeCount] - /// (Will be null if bounding volumes are disabled.) - dtBVNode* bvTree; - - dtOffMeshConnection* offMeshCons; ///< The tile off-mesh connections. [Size: dtMeshHeader::offMeshConCount] - - unsigned char* data; ///< The tile data. (Not directly accessed under normal situations.) - int dataSize; ///< Size of the tile data. - int flags; ///< Tile flags. (See: #dtTileFlags) - dtMeshTile* next; ///< The next free tile, or the next tile in the spatial grid. + unsigned int salt; ///< Counter describing modifications to the tile. + + unsigned int linksFreeList; ///< Index to the next free link. + dtMeshHeader* header; ///< The tile header. + dtPoly* polys; ///< The tile polygons. [Size: dtMeshHeader::polyCount] + float* verts; ///< The tile vertices. [(x, y, z) * dtMeshHeader::vertCount] + dtLink* links; ///< The tile links. [Size: dtMeshHeader::maxLinkCount] + dtPolyDetail* detailMeshes; ///< The tile's detail sub-meshes. [Size: dtMeshHeader::detailMeshCount] + + /// The detail mesh's unique vertices. [(x, y, z) * dtMeshHeader::detailVertCount] + float* detailVerts; + + /// The detail mesh's triangles. [(vertA, vertB, vertC, triFlags) * dtMeshHeader::detailTriCount]. + /// See dtDetailTriEdgeFlags and dtGetDetailTriEdgeFlags. + unsigned char* detailTris; + + /// The tile bounding volume nodes. [Size: dtMeshHeader::bvNodeCount] + /// (Will be null if bounding volumes are disabled.) + dtBVNode* bvTree; + + dtOffMeshConnection* offMeshCons; ///< The tile off-mesh connections. [Size: dtMeshHeader::offMeshConCount] + + unsigned char* data; ///< The tile data. (Not directly accessed under normal situations.) + int dataSize; ///< Size of the tile data. + int flags; ///< Tile flags. (See: #dtTileFlags) + dtMeshTile* next; ///< The next free tile, or the next tile in the spatial grid. private: - dtMeshTile(const dtMeshTile&); - dtMeshTile& operator=(const dtMeshTile&); + dtMeshTile(const dtMeshTile&); + dtMeshTile& operator=(const dtMeshTile&); }; /// Get flags for edge in detail triangle. -/// @param triFlags[in] The flags for the triangle (last component of detail vertices above). -/// @param edgeIndex[in] The index of the first vertex of the edge. For instance, if 0, +/// @param[in] triFlags The flags for the triangle (last component of detail vertices above). +/// @param[in] edgeIndex The index of the first vertex of the edge. For instance, if 0, /// returns flags for edge AB. inline int dtGetDetailTriEdgeFlags(unsigned char triFlags, int edgeIndex) { - return (triFlags >> (edgeIndex * 2)) & 0x3; + return (triFlags >> (edgeIndex * 2)) & 0x3; } /// Configuration parameters used to define multi-tile navigation meshes. @@ -335,11 +326,11 @@ inline int dtGetDetailTriEdgeFlags(unsigned char triFlags, int edgeIndex) /// @ingroup detour struct dtNavMeshParams { - float orig[3]; ///< The world space origin of the navigation mesh's tile space. [(x, y, z)] - float tileWidth; ///< The width of each tile. (Along the x-axis.) - float tileHeight; ///< The height of each tile. (Along the z-axis.) - int maxTiles; ///< The maximum number of tiles the navigation mesh can contain. - int maxPolys; ///< The maximum number of polygons each tile can contain. + float orig[3]; ///< The world space origin of the navigation mesh's tile space. [(x, y, z)] + float tileWidth; ///< The width of each tile. (Along the x-axis.) + float tileHeight; ///< The height of each tile. (Along the z-axis.) + int maxTiles; ///< The maximum number of tiles the navigation mesh can contain. This and maxPolys are used to calculate how many bits are needed to identify tiles and polygons uniquely. + int maxPolys; ///< The maximum number of polygons each tile can contain. This and maxTiles are used to calculate how many bits are needed to identify tiles and polygons uniquely. }; /// A navigation mesh based on tiles of convex polygons. @@ -347,341 +338,342 @@ struct dtNavMeshParams class dtNavMesh { public: - dtNavMesh(); - ~dtNavMesh(); - - /// @{ - /// @name Initialization and Tile Management - - /// Initializes the navigation mesh for tiled use. - /// @param[in] params Initialization parameters. - /// @return The status flags for the operation. - dtStatus init(const dtNavMeshParams* params); - - /// Initializes the navigation mesh for single tile use. - /// @param[in] data Data of the new tile. (See: #dtCreateNavMeshData) - /// @param[in] dataSize The data size of the new tile. - /// @param[in] flags The tile flags. (See: #dtTileFlags) - /// @return The status flags for the operation. - /// @see dtCreateNavMeshData - dtStatus init(unsigned char* data, const int dataSize, const int flags); - - /// The navigation mesh initialization params. - const dtNavMeshParams* getParams() const; - - /// Adds a tile to the navigation mesh. - /// @param[in] data Data for the new tile mesh. (See: #dtCreateNavMeshData) - /// @param[in] dataSize Data size of the new tile mesh. - /// @param[in] flags Tile flags. (See: #dtTileFlags) - /// @param[in] lastRef The desired reference for the tile. (When reloading a tile.) [opt] [Default: 0] - /// @param[out] result The tile reference. (If the tile was succesfully added.) [opt] - /// @return The status flags for the operation. - dtStatus addTile(unsigned char* data, int dataSize, int flags, dtTileRef lastRef, dtTileRef* result); - - /// Removes the specified tile from the navigation mesh. - /// @param[in] ref The reference of the tile to remove. - /// @param[out] data Data associated with deleted tile. - /// @param[out] dataSize Size of the data associated with deleted tile. - /// @return The status flags for the operation. - dtStatus removeTile(dtTileRef ref, unsigned char** data, int* dataSize); - - /// @} - - /// @{ - /// @name Query Functions - - /// Calculates the tile grid location for the specified world position. - /// @param[in] pos The world position for the query. [(x, y, z)] - /// @param[out] tx The tile's x-location. (x, y) - /// @param[out] ty The tile's y-location. (x, y) - void calcTileLoc(const float* pos, int* tx, int* ty) const; - - /// Gets the tile at the specified grid location. - /// @param[in] x The tile's x-location. (x, y, layer) - /// @param[in] y The tile's y-location. (x, y, layer) - /// @param[in] layer The tile's layer. (x, y, layer) - /// @return The tile, or null if the tile does not exist. - const dtMeshTile* getTileAt(const int x, const int y, const int layer) const; - - /// Gets all tiles at the specified grid location. (All layers.) - /// @param[in] x The tile's x-location. (x, y) - /// @param[in] y The tile's y-location. (x, y) - /// @param[out] tiles A pointer to an array of tiles that will hold the result. - /// @param[in] maxTiles The maximum tiles the tiles parameter can hold. - /// @return The number of tiles returned in the tiles array. - int getTilesAt(const int x, const int y, - dtMeshTile const** tiles, const int maxTiles) const; - - /// Gets the tile reference for the tile at specified grid location. - /// @param[in] x The tile's x-location. (x, y, layer) - /// @param[in] y The tile's y-location. (x, y, layer) - /// @param[in] layer The tile's layer. (x, y, layer) - /// @return The tile reference of the tile, or 0 if there is none. - dtTileRef getTileRefAt(int x, int y, int layer) const; - - /// Gets the tile reference for the specified tile. - /// @param[in] tile The tile. - /// @return The tile reference of the tile. - dtTileRef getTileRef(const dtMeshTile* tile) const; - - /// Gets the tile for the specified tile reference. - /// @param[in] ref The tile reference of the tile to retrieve. - /// @return The tile for the specified reference, or null if the - /// reference is invalid. - const dtMeshTile* getTileByRef(dtTileRef ref) const; - - /// The maximum number of tiles supported by the navigation mesh. - /// @return The maximum number of tiles supported by the navigation mesh. - int getMaxTiles() const; - - /// Gets the tile at the specified index. - /// @param[in] i The tile index. [Limit: 0 >= index < #getMaxTiles()] - /// @return The tile at the specified index. - const dtMeshTile* getTile(int i) const; - - /// Gets the tile and polygon for the specified polygon reference. - /// @param[in] ref The reference for the a polygon. - /// @param[out] tile The tile containing the polygon. - /// @param[out] poly The polygon. - /// @return The status flags for the operation. - dtStatus getTileAndPolyByRef(const dtPolyRef ref, const dtMeshTile** tile, const dtPoly** poly) const; - - /// Returns the tile and polygon for the specified polygon reference. - /// @param[in] ref A known valid reference for a polygon. - /// @param[out] tile The tile containing the polygon. - /// @param[out] poly The polygon. - void getTileAndPolyByRefUnsafe(const dtPolyRef ref, const dtMeshTile** tile, const dtPoly** poly) const; - - /// Checks the validity of a polygon reference. - /// @param[in] ref The polygon reference to check. - /// @return True if polygon reference is valid for the navigation mesh. - bool isValidPolyRef(dtPolyRef ref) const; - - /// Gets the polygon reference for the tile's base polygon. - /// @param[in] tile The tile. - /// @return The polygon reference for the base polygon in the specified tile. - dtPolyRef getPolyRefBase(const dtMeshTile* tile) const; - - /// Gets the endpoints for an off-mesh connection, ordered by "direction of travel". - /// @param[in] prevRef The reference of the polygon before the connection. - /// @param[in] polyRef The reference of the off-mesh connection polygon. - /// @param[out] startPos The start position of the off-mesh connection. [(x, y, z)] - /// @param[out] endPos The end position of the off-mesh connection. [(x, y, z)] - /// @return The status flags for the operation. - dtStatus getOffMeshConnectionPolyEndPoints(dtPolyRef prevRef, dtPolyRef polyRef, float* startPos, float* endPos) const; - - /// Gets the specified off-mesh connection. - /// @param[in] ref The polygon reference of the off-mesh connection. - /// @return The specified off-mesh connection, or null if the polygon reference is not valid. - const dtOffMeshConnection* getOffMeshConnectionByRef(dtPolyRef ref) const; - - /// @} - - /// @{ - /// @name State Management - /// These functions do not effect #dtTileRef or #dtPolyRef's. - - /// Sets the user defined flags for the specified polygon. - /// @param[in] ref The polygon reference. - /// @param[in] flags The new flags for the polygon. - /// @return The status flags for the operation. - dtStatus setPolyFlags(dtPolyRef ref, unsigned short flags); - - /// Gets the user defined flags for the specified polygon. - /// @param[in] ref The polygon reference. - /// @param[out] resultFlags The polygon flags. - /// @return The status flags for the operation. - dtStatus getPolyFlags(dtPolyRef ref, unsigned short* resultFlags) const; - - /// Sets the user defined area for the specified polygon. - /// @param[in] ref The polygon reference. - /// @param[in] area The new area id for the polygon. [Limit: < #DT_MAX_AREAS] - /// @return The status flags for the operation. - dtStatus setPolyArea(dtPolyRef ref, unsigned char area); - - /// Gets the user defined area for the specified polygon. - /// @param[in] ref The polygon reference. - /// @param[out] resultArea The area id for the polygon. - /// @return The status flags for the operation. - dtStatus getPolyArea(dtPolyRef ref, unsigned char* resultArea) const; - - /// Gets the size of the buffer required by #storeTileState to store the specified tile's state. - /// @param[in] tile The tile. - /// @return The size of the buffer required to store the state. - int getTileStateSize(const dtMeshTile* tile) const; - - /// Stores the non-structural state of the tile in the specified buffer. (Flags, area ids, etc.) - /// @param[in] tile The tile. - /// @param[out] data The buffer to store the tile's state in. - /// @param[in] maxDataSize The size of the data buffer. [Limit: >= #getTileStateSize] - /// @return The status flags for the operation. - dtStatus storeTileState(const dtMeshTile* tile, unsigned char* data, const int maxDataSize) const; - - /// Restores the state of the tile. - /// @param[in] tile The tile. - /// @param[in] data The new state. (Obtained from #storeTileState.) - /// @param[in] maxDataSize The size of the state within the data buffer. - /// @return The status flags for the operation. - dtStatus restoreTileState(dtMeshTile* tile, const unsigned char* data, const int maxDataSize); - - /// @} - - /// @{ - /// @name Encoding and Decoding - /// These functions are generally meant for internal use only. - - /// Derives a standard polygon reference. - /// @note This function is generally meant for internal use only. - /// @param[in] salt The tile's salt value. - /// @param[in] it The index of the tile. - /// @param[in] ip The index of the polygon within the tile. - inline dtPolyRef encodePolyId(unsigned int salt, unsigned int it, unsigned int ip) const - { + dtNavMesh(); + ~dtNavMesh(); + + /// @{ + /// @name Initialization and Tile Management + + /// Initializes the navigation mesh for tiled use. + /// @param[in] params Initialization parameters. + /// @return The status flags for the operation. + dtStatus init(const dtNavMeshParams* params); + + /// Initializes the navigation mesh for single tile use. + /// @param[in] data Data of the new tile. (See: #dtCreateNavMeshData) + /// @param[in] dataSize The data size of the new tile. + /// @param[in] flags The tile flags. (See: #dtTileFlags) + /// @return The status flags for the operation. + /// @see dtCreateNavMeshData + dtStatus init(unsigned char* data, const int dataSize, const int flags); + + /// The navigation mesh initialization params. + const dtNavMeshParams* getParams() const; + + /// Adds a tile to the navigation mesh. + /// @param[in] data Data for the new tile mesh. (See: #dtCreateNavMeshData) + /// @param[in] dataSize Data size of the new tile mesh. + /// @param[in] flags Tile flags. (See: #dtTileFlags) + /// @param[in] lastRef The desired reference for the tile. (When reloading a tile.) [opt] [Default: 0] + /// @param[out] result The tile reference. (If the tile was succesfully added.) [opt] + /// @return The status flags for the operation. + dtStatus addTile(unsigned char* data, int dataSize, int flags, dtTileRef lastRef, dtTileRef* result); + + /// Removes the specified tile from the navigation mesh. + /// @param[in] ref The reference of the tile to remove. + /// @param[out] data Data associated with deleted tile. + /// @param[out] dataSize Size of the data associated with deleted tile. + /// @return The status flags for the operation. + dtStatus removeTile(dtTileRef ref, unsigned char** data, int* dataSize); + + /// @} + + /// @{ + /// @name Query Functions + + /// Calculates the tile grid location for the specified world position. + /// @param[in] pos The world position for the query. [(x, y, z)] + /// @param[out] tx The tile's x-location. (x, y) + /// @param[out] ty The tile's y-location. (x, y) + void calcTileLoc(const float* pos, int* tx, int* ty) const; + + /// Gets the tile at the specified grid location. + /// @param[in] x The tile's x-location. (x, y, layer) + /// @param[in] y The tile's y-location. (x, y, layer) + /// @param[in] layer The tile's layer. (x, y, layer) + /// @return The tile, or null if the tile does not exist. + const dtMeshTile* getTileAt(const int x, const int y, const int layer) const; + + /// Gets all tiles at the specified grid location. (All layers.) + /// @param[in] x The tile's x-location. (x, y) + /// @param[in] y The tile's y-location. (x, y) + /// @param[out] tiles A pointer to an array of tiles that will hold the result. + /// @param[in] maxTiles The maximum tiles the tiles parameter can hold. + /// @return The number of tiles returned in the tiles array. + int getTilesAt(const int x, const int y, + dtMeshTile const** tiles, const int maxTiles) const; + + /// Gets the tile reference for the tile at specified grid location. + /// @param[in] x The tile's x-location. (x, y, layer) + /// @param[in] y The tile's y-location. (x, y, layer) + /// @param[in] layer The tile's layer. (x, y, layer) + /// @return The tile reference of the tile, or 0 if there is none. + dtTileRef getTileRefAt(int x, int y, int layer) const; + + /// Gets the tile reference for the specified tile. + /// @param[in] tile The tile. + /// @return The tile reference of the tile. + dtTileRef getTileRef(const dtMeshTile* tile) const; + + /// Gets the tile for the specified tile reference. + /// @param[in] ref The tile reference of the tile to retrieve. + /// @return The tile for the specified reference, or null if the + /// reference is invalid. + const dtMeshTile* getTileByRef(dtTileRef ref) const; + + /// The maximum number of tiles supported by the navigation mesh. + /// @return The maximum number of tiles supported by the navigation mesh. + int getMaxTiles() const; + + /// Gets the tile at the specified index. + /// @param[in] i The tile index. [Limit: 0 >= index < #getMaxTiles()] + /// @return The tile at the specified index. + const dtMeshTile* getTile(int i) const; + + /// Gets the tile and polygon for the specified polygon reference. + /// @param[in] ref The reference for the a polygon. + /// @param[out] tile The tile containing the polygon. + /// @param[out] poly The polygon. + /// @return The status flags for the operation. + dtStatus getTileAndPolyByRef(const dtPolyRef ref, const dtMeshTile** tile, const dtPoly** poly) const; + + /// Returns the tile and polygon for the specified polygon reference. + /// @param[in] ref A known valid reference for a polygon. + /// @param[out] tile The tile containing the polygon. + /// @param[out] poly The polygon. + void getTileAndPolyByRefUnsafe(const dtPolyRef ref, const dtMeshTile** tile, const dtPoly** poly) const; + + /// Checks the validity of a polygon reference. + /// @param[in] ref The polygon reference to check. + /// @return True if polygon reference is valid for the navigation mesh. + bool isValidPolyRef(dtPolyRef ref) const; + + /// Gets the polygon reference for the tile's base polygon. + /// @param[in] tile The tile. + /// @return The polygon reference for the base polygon in the specified tile. + dtPolyRef getPolyRefBase(const dtMeshTile* tile) const; + + /// Gets the endpoints for an off-mesh connection, ordered by "direction of travel". + /// @param[in] prevRef The reference of the polygon before the connection. + /// @param[in] polyRef The reference of the off-mesh connection polygon. + /// @param[out] startPos The start position of the off-mesh connection. [(x, y, z)] + /// @param[out] endPos The end position of the off-mesh connection. [(x, y, z)] + /// @return The status flags for the operation. + dtStatus getOffMeshConnectionPolyEndPoints(dtPolyRef prevRef, dtPolyRef polyRef, float* startPos, float* endPos) const; + + /// Gets the specified off-mesh connection. + /// @param[in] ref The polygon reference of the off-mesh connection. + /// @return The specified off-mesh connection, or null if the polygon reference is not valid. + const dtOffMeshConnection* getOffMeshConnectionByRef(dtPolyRef ref) const; + + /// @} + + /// @{ + /// @name State Management + /// These functions do not effect #dtTileRef or #dtPolyRef's. + + /// Sets the user defined flags for the specified polygon. + /// @param[in] ref The polygon reference. + /// @param[in] flags The new flags for the polygon. + /// @return The status flags for the operation. + dtStatus setPolyFlags(dtPolyRef ref, unsigned short flags); + + /// Gets the user defined flags for the specified polygon. + /// @param[in] ref The polygon reference. + /// @param[out] resultFlags The polygon flags. + /// @return The status flags for the operation. + dtStatus getPolyFlags(dtPolyRef ref, unsigned short* resultFlags) const; + + /// Sets the user defined area for the specified polygon. + /// @param[in] ref The polygon reference. + /// @param[in] area The new area id for the polygon. [Limit: < #DT_MAX_AREAS] + /// @return The status flags for the operation. + dtStatus setPolyArea(dtPolyRef ref, unsigned char area); + + /// Gets the user defined area for the specified polygon. + /// @param[in] ref The polygon reference. + /// @param[out] resultArea The area id for the polygon. + /// @return The status flags for the operation. + dtStatus getPolyArea(dtPolyRef ref, unsigned char* resultArea) const; + + /// Gets the size of the buffer required by #storeTileState to store the specified tile's state. + /// @param[in] tile The tile. + /// @return The size of the buffer required to store the state. + int getTileStateSize(const dtMeshTile* tile) const; + + /// Stores the non-structural state of the tile in the specified buffer. (Flags, area ids, etc.) + /// @param[in] tile The tile. + /// @param[out] data The buffer to store the tile's state in. + /// @param[in] maxDataSize The size of the data buffer. [Limit: >= #getTileStateSize] + /// @return The status flags for the operation. + dtStatus storeTileState(const dtMeshTile* tile, unsigned char* data, const int maxDataSize) const; + + /// Restores the state of the tile. + /// @param[in] tile The tile. + /// @param[in] data The new state. (Obtained from #storeTileState.) + /// @param[in] maxDataSize The size of the state within the data buffer. + /// @return The status flags for the operation. + dtStatus restoreTileState(dtMeshTile* tile, const unsigned char* data, const int maxDataSize); + + /// @} + + /// @{ + /// @name Encoding and Decoding + /// These functions are generally meant for internal use only. + + /// Derives a standard polygon reference. + /// @note This function is generally meant for internal use only. + /// @param[in] salt The tile's salt value. + /// @param[in] it The index of the tile. + /// @param[in] ip The index of the polygon within the tile. + inline dtPolyRef encodePolyId(unsigned int salt, unsigned int it, unsigned int ip) const + { #ifdef DT_POLYREF64 - return ((dtPolyRef)salt << (DT_POLY_BITS + DT_TILE_BITS)) | ((dtPolyRef)it << DT_POLY_BITS) | (dtPolyRef)ip; + return ((dtPolyRef)salt << (DT_POLY_BITS+DT_TILE_BITS)) | ((dtPolyRef)it << DT_POLY_BITS) | (dtPolyRef)ip; #else - return ((dtPolyRef)salt << (m_polyBits + m_tileBits)) | ((dtPolyRef)it << m_polyBits) | (dtPolyRef)ip; + return ((dtPolyRef)salt << (m_polyBits+m_tileBits)) | ((dtPolyRef)it << m_polyBits) | (dtPolyRef)ip; #endif - } - - /// Decodes a standard polygon reference. - /// @note This function is generally meant for internal use only. - /// @param[in] ref The polygon reference to decode. - /// @param[out] salt The tile's salt value. - /// @param[out] it The index of the tile. - /// @param[out] ip The index of the polygon within the tile. - /// @see #encodePolyId - inline void decodePolyId(dtPolyRef ref, unsigned int& salt, unsigned int& it, unsigned int& ip) const - { + } + + /// Decodes a standard polygon reference. + /// @note This function is generally meant for internal use only. + /// @param[in] ref The polygon reference to decode. + /// @param[out] salt The tile's salt value. + /// @param[out] it The index of the tile. + /// @param[out] ip The index of the polygon within the tile. + /// @see #encodePolyId + inline void decodePolyId(dtPolyRef ref, unsigned int& salt, unsigned int& it, unsigned int& ip) const + { #ifdef DT_POLYREF64 - const dtPolyRef saltMask = ((dtPolyRef)1 << DT_SALT_BITS) - 1; - const dtPolyRef tileMask = ((dtPolyRef)1 << DT_TILE_BITS) - 1; - const dtPolyRef polyMask = ((dtPolyRef)1 << DT_POLY_BITS) - 1; - salt = (unsigned int)((ref >> (DT_POLY_BITS + DT_TILE_BITS)) & saltMask); - it = (unsigned int)((ref >> DT_POLY_BITS) & tileMask); - ip = (unsigned int)(ref & polyMask); + const dtPolyRef saltMask = ((dtPolyRef)1<> (DT_POLY_BITS+DT_TILE_BITS)) & saltMask); + it = (unsigned int)((ref >> DT_POLY_BITS) & tileMask); + ip = (unsigned int)(ref & polyMask); #else - 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); + const dtPolyRef saltMask = ((dtPolyRef)1<> (m_polyBits+m_tileBits)) & saltMask); + it = (unsigned int)((ref >> m_polyBits) & tileMask); + ip = (unsigned int)(ref & polyMask); #endif - } - - /// Extracts a tile's salt value from the specified polygon reference. - /// @note This function is generally meant for internal use only. - /// @param[in] ref The polygon reference. - /// @see #encodePolyId - inline unsigned int decodePolyIdSalt(dtPolyRef ref) const - { + } + + /// Extracts a tile's salt value from the specified polygon reference. + /// @note This function is generally meant for internal use only. + /// @param[in] ref The polygon reference. + /// @see #encodePolyId + inline unsigned int decodePolyIdSalt(dtPolyRef ref) const + { #ifdef DT_POLYREF64 - const dtPolyRef saltMask = ((dtPolyRef)1 << DT_SALT_BITS) - 1; - return (unsigned int)((ref >> (DT_POLY_BITS + DT_TILE_BITS)) & saltMask); + const dtPolyRef saltMask = ((dtPolyRef)1<> (DT_POLY_BITS+DT_TILE_BITS)) & saltMask); #else - const dtPolyRef saltMask = ((dtPolyRef)1 << m_saltBits) - 1; - return (unsigned int)((ref >> (m_polyBits + m_tileBits)) & saltMask); + const dtPolyRef saltMask = ((dtPolyRef)1<> (m_polyBits+m_tileBits)) & saltMask); #endif - } - - /// Extracts the tile's index from the specified polygon reference. - /// @note This function is generally meant for internal use only. - /// @param[in] ref The polygon reference. - /// @see #encodePolyId - inline unsigned int decodePolyIdTile(dtPolyRef ref) const - { + } + + /// Extracts the tile's index from the specified polygon reference. + /// @note This function is generally meant for internal use only. + /// @param[in] ref The polygon reference. + /// @see #encodePolyId + inline unsigned int decodePolyIdTile(dtPolyRef ref) const + { #ifdef DT_POLYREF64 - const dtPolyRef tileMask = ((dtPolyRef)1 << DT_TILE_BITS) - 1; - return (unsigned int)((ref >> DT_POLY_BITS) & tileMask); + const dtPolyRef tileMask = ((dtPolyRef)1<> DT_POLY_BITS) & tileMask); #else - const dtPolyRef tileMask = ((dtPolyRef)1 << m_tileBits) - 1; - return (unsigned int)((ref >> m_polyBits) & tileMask); + const dtPolyRef tileMask = ((dtPolyRef)1<> m_polyBits) & tileMask); #endif - } - - /// Extracts the polygon's index (within its tile) from the specified polygon reference. - /// @note This function is generally meant for internal use only. - /// @param[in] ref The polygon reference. - /// @see #encodePolyId - inline unsigned int decodePolyIdPoly(dtPolyRef ref) const - { + } + + /// Extracts the polygon's index (within its tile) from the specified polygon reference. + /// @note This function is generally meant for internal use only. + /// @param[in] ref The polygon reference. + /// @see #encodePolyId + inline unsigned int decodePolyIdPoly(dtPolyRef ref) const + { #ifdef DT_POLYREF64 - const dtPolyRef polyMask = ((dtPolyRef)1 << DT_POLY_BITS) - 1; - return (unsigned int)(ref & polyMask); + const dtPolyRef polyMask = ((dtPolyRef)1<= 3] - const unsigned short* polys; ///< The polygon data. [Size: #polyCount * 2 * #nvp] - const unsigned short* polyFlags; ///< The user defined flags assigned to each polygon. [Size: #polyCount] - const unsigned char* polyAreas; ///< The user defined area ids assigned to each polygon. [Size: #polyCount] - int polyCount; ///< Number of polygons in the mesh. [Limit: >= 1] - int nvp; ///< Number maximum number of vertices per polygon. [Limit: >= 3] - - /// @} - /// @name Height Detail Attributes (Optional) - /// See #rcPolyMeshDetail for details related to these attributes. - /// @{ - const unsigned int* detailMeshes; ///< The height detail sub-mesh data. [Size: 4 * #polyCount] - const float* detailVerts; ///< The detail mesh vertices. [Size: 3 * #detailVertsCount] [Unit: wu] - int detailVertsCount; ///< The number of vertices in the detail mesh. - const unsigned char* detailTris; ///< The detail mesh triangles. [Size: 4 * #detailTriCount] - int detailTriCount; ///< The number of triangles in the detail mesh. - - /// @} - /// @name Off-Mesh Connections Attributes (Optional) - /// Used to define a custom point-to-point edge within the navigation graph, an - /// off-mesh connection is a user defined traversable connection made up to two vertices, - /// at least one of which resides within a navigation mesh polygon. - /// @{ - /// Off-mesh connection vertices. [(ax, ay, az, bx, by, bz) * #offMeshConCount] [Unit: wu] - const float* offMeshConVerts; - /// Off-mesh connection radii. [Size: #offMeshConCount] [Unit: wu] - const float* offMeshConRad; - /// User defined flags assigned to the off-mesh connections. [Size: #offMeshConCount] - const unsigned short* offMeshConFlags; - /// User defined area ids assigned to the off-mesh connections. [Size: #offMeshConCount] - const unsigned char* offMeshConAreas; - /// The permitted travel direction of the off-mesh connections. [Size: #offMeshConCount] - /// - /// 0 = Travel only from endpoint A to endpoint B.
- /// #DT_OFFMESH_CON_BIDIR = Bidirectional travel. - const unsigned char* offMeshConDir; - /// The user defined ids of the off-mesh connection. [Size: #offMeshConCount] - const unsigned int* offMeshConUserID; - /// The number of off-mesh connections. [Limit: >= 0] - int offMeshConCount; - - /// @} - /// @name Tile Attributes - /// @note The tile grid/layer data can be left at zero if the destination is a single tile mesh. - /// @{ - unsigned int userId; ///< The user defined id of the tile. - int tileX; ///< The tile's x-grid location within the multi-tile destination mesh. (Along the x-axis.) - int tileY; ///< The tile's y-grid location within the multi-tile desitation mesh. (Along the z-axis.) - int tileLayer; ///< The tile's layer within the layered destination mesh. [Limit: >= 0] (Along the y-axis.) - float bmin[3]; ///< The minimum bounds of the tile. [(x, y, z)] [Unit: wu] - float bmax[3]; ///< The maximum bounds of the tile. [(x, y, z)] [Unit: wu] - - /// @} - /// @name General Configuration Attributes - /// @{ - float walkableHeight; ///< The agent height. [Unit: wu] - float walkableRadius; ///< The agent radius. [Unit: wu] - float walkableClimb; ///< The agent maximum traversable ledge. (Up/Down) [Unit: wu] - float cs; ///< The xz-plane cell size of the polygon mesh. [Limit: > 0] [Unit: wu] - float ch; ///< The y-axis cell height of the polygon mesh. [Limit: > 0] [Unit: wu] - - /// True if a bounding volume tree should be built for the tile. - /// @note The BVTree is not normally needed for layered navigation meshes. - bool buildBvTree; - - /// @} + + /// @name Polygon Mesh Attributes + /// Used to create the base navigation graph. + /// See #rcPolyMesh for details related to these attributes. + /// @{ + + const unsigned short* verts; ///< The polygon mesh vertices. [(x, y, z) * #vertCount] [Unit: vx] + int vertCount; ///< The number vertices in the polygon mesh. [Limit: >= 3] + const unsigned short* polys; ///< The polygon data. [Size: #polyCount * 2 * #nvp] + const unsigned short* polyFlags; ///< The user defined flags assigned to each polygon. [Size: #polyCount] + const unsigned char* polyAreas; ///< The user defined area ids assigned to each polygon. [Size: #polyCount] + int polyCount; ///< Number of polygons in the mesh. [Limit: >= 1] + int nvp; ///< Number maximum number of vertices per polygon. [Limit: >= 3] + + /// @} + /// @name Height Detail Attributes (Optional) + /// See #rcPolyMeshDetail for details related to these attributes. + /// @{ + + const unsigned int* detailMeshes; ///< The height detail sub-mesh data. [Size: 4 * #polyCount] + const float* detailVerts; ///< The detail mesh vertices. [Size: 3 * #detailVertsCount] [Unit: wu] + int detailVertsCount; ///< The number of vertices in the detail mesh. + const unsigned char* detailTris; ///< The detail mesh triangles. [Size: 4 * #detailTriCount] + int detailTriCount; ///< The number of triangles in the detail mesh. + + /// @} + /// @name Off-Mesh Connections Attributes (Optional) + /// Used to define a custom point-to-point edge within the navigation graph, an + /// off-mesh connection is a user defined traversable connection made up to two vertices, + /// at least one of which resides within a navigation mesh polygon. + /// @{ + + /// Off-mesh connection vertices. [(ax, ay, az, bx, by, bz) * #offMeshConCount] [Unit: wu] + const float* offMeshConVerts; + /// Off-mesh connection radii. [Size: #offMeshConCount] [Unit: wu] + const float* offMeshConRad; + /// User defined flags assigned to the off-mesh connections. [Size: #offMeshConCount] + const unsigned short* offMeshConFlags; + /// User defined area ids assigned to the off-mesh connections. [Size: #offMeshConCount] + const unsigned char* offMeshConAreas; + /// The permitted travel direction of the off-mesh connections. [Size: #offMeshConCount] + /// + /// 0 = Travel only from endpoint A to endpoint B.
+ /// #DT_OFFMESH_CON_BIDIR = Bidirectional travel. + const unsigned char* offMeshConDir; + /// The user defined ids of the off-mesh connection. [Size: #offMeshConCount] + const unsigned int* offMeshConUserID; + /// The number of off-mesh connections. [Limit: >= 0] + int offMeshConCount; + + /// @} + /// @name Tile Attributes + /// @note The tile grid/layer data can be left at zero if the destination is a single tile mesh. + /// @{ + + unsigned int userId; ///< The user defined id of the tile. + int tileX; ///< The tile's x-grid location within the multi-tile destination mesh. (Along the x-axis.) + int tileY; ///< The tile's y-grid location within the multi-tile desitation mesh. (Along the z-axis.) + int tileLayer; ///< The tile's layer within the layered destination mesh. [Limit: >= 0] (Along the y-axis.) + float bmin[3]; ///< The minimum bounds of the tile. [(x, y, z)] [Unit: wu] + float bmax[3]; ///< The maximum bounds of the tile. [(x, y, z)] [Unit: wu] + + /// @} + /// @name General Configuration Attributes + /// @{ + + float walkableHeight; ///< The agent height. [Unit: wu] + float walkableRadius; ///< The agent radius. [Unit: wu] + float walkableClimb; ///< The agent maximum traversable ledge. (Up/Down) [Unit: wu] + float cs; ///< The xz-plane cell size of the polygon mesh. [Limit: > 0] [Unit: wu] + float ch; ///< The y-axis cell height of the polygon mesh. [Limit: > 0] [Unit: wu] + + /// True if a bounding volume tree should be built for the tile. + /// @note The BVTree is not normally needed for layered navigation meshes. + bool buildBvTree; + + /// @} }; /// Builds navigation mesh tile data from the provided tile creation data. @@ -130,13 +136,14 @@ This structure is used to marshal data between the Recast mesh generation pipeli See the rcPolyMesh and rcPolyMeshDetail documentation for detailed information related to mesh structure. -Units are usually in voxels (vx) or world units (wu). The units for voxels, grid size, and cell size +Units are usually in voxels (vx) or world units (wu). The units for voxels, grid size, and cell size are all based on the values of #cs and #ch. -The standard navigation mesh build process is to create tile data using dtCreateNavMeshData, then add the tile +The standard navigation mesh build process is to create tile data using dtCreateNavMeshData, then add the tile to a navigation mesh using either the dtNavMesh single tile init() function or the dtNavMesh::addTile() function. @see dtCreateNavMeshData */ + diff --git a/recastnavigation/Detour/Include/DetourNavMeshQuery.h b/recastnavigation/Detour/Include/DetourNavMeshQuery.h index 539a24b..101b3c8 100644 --- a/recastnavigation/Detour/Include/DetourNavMeshQuery.h +++ b/recastnavigation/Detour/Include/DetourNavMeshQuery.h @@ -22,10 +22,11 @@ #include "DetourNavMesh.h" #include "DetourStatus.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 actual implementations of the functions -// are declared as inline for maximum speed. +// are declared as inline for maximum speed. //#define DT_VIRTUAL_QUERYFILTER 1 @@ -33,114 +34,116 @@ /// @ingroup detour class dtQueryFilter { - float m_areaCost[DT_MAX_AREAS]; ///< Cost per area type. (Used by default implementation.) - unsigned short m_includeFlags; ///< Flags for polygons that can be visited. (Used by default implementation.) - unsigned short m_excludeFlags; ///< Flags for polygons that should not be visted. (Used by default implementation.) - + float m_areaCost[DT_MAX_AREAS]; ///< Cost per area type. (Used by default implementation.) + unsigned short m_includeFlags; ///< Flags for polygons that can be visited. (Used by default implementation.) + unsigned short m_excludeFlags; ///< Flags for polygons that should not be visted. (Used by default implementation.) + public: - dtQueryFilter(); - + dtQueryFilter(); + #ifdef DT_VIRTUAL_QUERYFILTER - virtual ~dtQueryFilter() { } + virtual ~dtQueryFilter() { } #endif - - /// Returns true if the polygon can be visited. (I.e. Is traversable.) - /// @param[in] ref The reference id of the polygon test. - /// @param[in] tile The tile containing the polygon. - /// @param[in] poly The polygon to test. + + /// Returns true if the polygon can be visited. (I.e. Is traversable.) + /// @param[in] ref The reference id of the polygon test. + /// @param[in] tile The tile containing the polygon. + /// @param[in] poly The polygon to test. #ifdef DT_VIRTUAL_QUERYFILTER - virtual bool passFilter(const dtPolyRef ref, - const dtMeshTile* tile, - const dtPoly* poly) const; + 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; + bool passFilter(const dtPolyRef ref, + const dtMeshTile* tile, + const dtPoly* poly) const; #endif - /// Returns cost to move from the beginning to the end of a line segment - /// that is fully contained within a polygon. - /// @param[in] pa The start position on the edge of the previous and current polygon. [(x, y, z)] - /// @param[in] pb The end position on the edge of the current and next polygon. [(x, y, z)] - /// @param[in] prevRef The reference id of the previous polygon. [opt] - /// @param[in] prevTile The tile containing the previous polygon. [opt] - /// @param[in] prevPoly The previous polygon. [opt] - /// @param[in] curRef The reference id of the current polygon. - /// @param[in] curTile The tile containing the current polygon. - /// @param[in] curPoly The current polygon. - /// @param[in] nextRef The refernece id of the next polygon. [opt] - /// @param[in] nextTile The tile containing the next polygon. [opt] - /// @param[in] nextPoly The next polygon. [opt] + /// Returns cost to move from the beginning to the end of a line segment + /// that is fully contained within a polygon. + /// @param[in] pa The start position on the edge of the previous and current polygon. [(x, y, z)] + /// @param[in] pb The end position on the edge of the current and next polygon. [(x, y, z)] + /// @param[in] prevRef The reference id of the previous polygon. [opt] + /// @param[in] prevTile The tile containing the previous polygon. [opt] + /// @param[in] prevPoly The previous polygon. [opt] + /// @param[in] curRef The reference id of the current polygon. + /// @param[in] curTile The tile containing the current polygon. + /// @param[in] curPoly The current polygon. + /// @param[in] nextRef The refernece id of the next polygon. [opt] + /// @param[in] nextTile The tile containing the next polygon. [opt] + /// @param[in] nextPoly The next polygon. [opt] #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; + 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; + 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 - /// @name Getters and setters for the default implementation data. - ///@{ - /// Returns the traversal cost of the area. - /// @param[in] i The id of the area. - /// @returns The traversal cost of the area. - inline float getAreaCost(const int i) const { return m_areaCost[i]; } - - /// Sets the traversal cost of the area. - /// @param[in] i The id of the area. - /// @param[in] cost The new cost of traversing the area. - inline void setAreaCost(const int i, const float cost) { m_areaCost[i] = cost; } - - /// Returns the include flags for the filter. - /// Any polygons that include one or more of these flags will be - /// included in the operation. - inline unsigned short getIncludeFlags() const { return m_includeFlags; } - - /// Sets the include flags for the filter. - /// @param[in] flags The new flags. - inline void setIncludeFlags(const unsigned short flags) { m_includeFlags = flags; } - - /// Returns the exclude flags for the filter. - /// Any polygons that include one ore more of these flags will be - /// excluded from the operation. - inline unsigned short getExcludeFlags() const { return m_excludeFlags; } - - /// Sets the exclude flags for the filter. - /// @param[in] flags The new flags. - inline void setExcludeFlags(const unsigned short flags) { m_excludeFlags = flags; } - - ///@} -}; + /// @name Getters and setters for the default implementation data. + ///@{ -/// Provides information about raycast hit -/// filled by dtNavMeshQuery::raycast -/// @ingroup detour -struct dtRaycastHit -{ - /// The hit parameter. (FLT_MAX if no wall hit.) - float t; + /// Returns the traversal cost of the area. + /// @param[in] i The id of the area. + /// @returns The traversal cost of the area. + inline float getAreaCost(const int i) const { return m_areaCost[i]; } - /// hitNormal The normal of the nearest wall hit. [(x, y, z)] - float hitNormal[3]; + /// Sets the traversal cost of the area. + /// @param[in] i The id of the area. + /// @param[in] cost The new cost of traversing the area. + inline void setAreaCost(const int i, const float cost) { m_areaCost[i] = cost; } - /// The index of the edge on the final polygon where the wall was hit. - int hitEdgeIndex; + /// Returns the include flags for the filter. + /// Any polygons that include one or more of these flags will be + /// included in the operation. + inline unsigned short getIncludeFlags() const { return m_includeFlags; } - /// Pointer to an array of reference ids of the visited polygons. [opt] - dtPolyRef* path; + /// Sets the include flags for the filter. + /// @param[in] flags The new flags. + inline void setIncludeFlags(const unsigned short flags) { m_includeFlags = flags; } - /// The number of visited polygons. [opt] - int pathCount; + /// Returns the exclude flags for the filter. + /// Any polygons that include one ore more of these flags will be + /// excluded from the operation. + inline unsigned short getExcludeFlags() const { return m_excludeFlags; } - /// The maximum number of polygons the @p path array can hold. - int maxPath; + /// Sets the exclude flags for the filter. + /// @param[in] flags The new flags. + inline void setExcludeFlags(const unsigned short flags) { m_excludeFlags = flags; } + + ///@} - /// The cost of the path until hit. - float pathCost; +}; + +/// Provides information about raycast hit +/// filled by dtNavMeshQuery::raycast +/// @ingroup detour +struct dtRaycastHit +{ + /// The hit parameter. (FLT_MAX if no wall hit.) + float t; + + /// hitNormal The normal of the nearest wall hit. [(x, y, z)] + float hitNormal[3]; + + /// The index of the edge on the final polygon where the wall was hit. + int hitEdgeIndex; + + /// Pointer to an array of reference ids of the visited polygons. [opt] + dtPolyRef* path; + + /// The number of visited polygons. [opt] + int pathCount; + + /// The maximum number of polygons the @p path array can hold. + int maxPath; + + /// The cost of the path until hit. + float pathCost; }; /// Provides custom polygon query behavior. @@ -149,11 +152,11 @@ struct dtRaycastHit class dtPolyQuery { public: - virtual ~dtPolyQuery() { } + virtual ~dtPolyQuery(); - /// Called for each batch of unique polygons touched by the search area in dtNavMeshQuery::queryPolygons. - /// This can be called multiple times for a single query. - virtual void process(const dtMeshTile* tile, dtPoly** polys, dtPolyRef* refs, int count) = 0; + /// Called for each batch of unique polygons touched by the search area in dtNavMeshQuery::queryPolygons. + /// This can be called multiple times for a single query. + virtual void process(const dtMeshTile* tile, dtPoly** polys, dtPolyRef* refs, int count) = 0; }; /// Provides the ability to perform pathfinding related queries against @@ -162,393 +165,416 @@ class dtPolyQuery class dtNavMeshQuery { public: - dtNavMeshQuery(); - ~dtNavMeshQuery(); - - /// Initializes the query object. - /// @param[in] nav Pointer to the dtNavMesh object to use for all queries. - /// @param[in] maxNodes Maximum number of search nodes. [Limits: 0 < value <= 65535] - /// @returns The status flags for the query. - dtStatus init(const dtNavMesh* nav, const int maxNodes); - - /// @name Standard Pathfinding Functions - // /@{ - /// Finds a path from the start polygon to the end polygon. - /// @param[in] startRef The refrence id of the start polygon. - /// @param[in] endRef The reference id of the end polygon. - /// @param[in] startPos A position within the start polygon. [(x, y, z)] - /// @param[in] endPos A position within the end polygon. [(x, y, z)] - /// @param[in] filter The polygon filter to apply to the query. - /// @param[out] path An ordered list of polygon references representing the path. (Start to end.) - /// [(polyRef) * @p pathCount] - /// @param[out] pathCount The number of polygons returned in the @p path array. - /// @param[in] maxPath The maximum number of polygons the @p path array can hold. [Limit: >= 1] - dtStatus findPath(dtPolyRef startRef, dtPolyRef endRef, - const float* startPos, const float* endPos, - const dtQueryFilter* filter, - dtPolyRef* path, int* pathCount, const int maxPath) const; - - /// Finds the straight path from the start to the end position within the polygon corridor. - /// @param[in] startPos Path start position. [(x, y, z)] - /// @param[in] endPos Path end position. [(x, y, z)] - /// @param[in] path An array of polygon references that represent the path corridor. - /// @param[in] pathSize The number of polygons in the @p path array. - /// @param[out] straightPath Points describing the straight path. [(x, y, z) * @p straightPathCount]. - /// @param[out] straightPathFlags Flags describing each point. (See: #dtStraightPathFlags) [opt] - /// @param[out] straightPathRefs The reference id of the polygon that is being entered at each point. [opt] - /// @param[out] straightPathCount The number of points in the straight path. - /// @param[in] maxStraightPath The maximum number of points the straight path arrays can hold. [Limit: > 0] - /// @param[in] options Query options. (see: #dtStraightPathOptions) - /// @returns The status flags for the query. - 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 int options = 0) const; - - ///@} - /// @name Sliced Pathfinding Functions - /// Common use case: - /// -# Call initSlicedFindPath() to initialize the sliced path query. - /// -# Call updateSlicedFindPath() until it returns complete. - /// -# Call finalizeSlicedFindPath() to get the path. - ///@{ - /// Intializes a sliced path query. - /// @param[in] startRef The refrence id of the start polygon. - /// @param[in] endRef The reference id of the end polygon. - /// @param[in] startPos A position within the start polygon. [(x, y, z)] - /// @param[in] endPos A position within the end polygon. [(x, y, z)] - /// @param[in] filter The polygon filter to apply to the query. - /// @param[in] options query options (see: #dtFindPathOptions) - /// @returns The status flags for the query. - dtStatus initSlicedFindPath(dtPolyRef startRef, dtPolyRef endRef, - const float* startPos, const float* endPos, - const dtQueryFilter* filter, const unsigned int options = 0); - - /// Updates an in-progress sliced path query. - /// @param[in] maxIter The maximum number of iterations to perform. - /// @param[out] doneIters The actual number of iterations completed. [opt] - /// @returns The status flags for the query. - dtStatus updateSlicedFindPath(const int maxIter, int* doneIters); - - /// Finalizes and returns the results of a sliced path query. - /// @param[out] path An ordered list of polygon references representing the path. (Start to end.) - /// [(polyRef) * @p pathCount] - /// @param[out] pathCount The number of polygons returned in the @p path array. - /// @param[in] maxPath The max number of polygons the path array can hold. [Limit: >= 1] - /// @returns The status flags for the query. - dtStatus finalizeSlicedFindPath(dtPolyRef* path, int* pathCount, const int maxPath); - - /// Finalizes and returns the results of an incomplete sliced path query, returning the path to the furthest - /// polygon on the existing path that was visited during the search. - /// @param[in] existing An array of polygon references for the existing path. - /// @param[in] existingSize The number of polygon in the @p existing array. - /// @param[out] path An ordered list of polygon references representing the path. (Start to end.) - /// [(polyRef) * @p pathCount] - /// @param[out] pathCount The number of polygons returned in the @p path array. - /// @param[in] maxPath The max number of polygons the @p path array can hold. [Limit: >= 1] - /// @returns The status flags for the query. - dtStatus finalizeSlicedFindPathPartial(const dtPolyRef* existing, const int existingSize, - dtPolyRef* path, int* pathCount, const int maxPath); - - ///@} - /// @name Dijkstra Search Functions - /// @{ - /// Finds the polygons along the navigation graph that touch the specified circle. - /// @param[in] startRef The reference id of the polygon where the search starts. - /// @param[in] centerPos The center of the search circle. [(x, y, z)] - /// @param[in] radius The radius of the search circle. - /// @param[in] filter The polygon filter to apply to the query. - /// @param[out] resultRef The reference ids of the polygons touched by the circle. [opt] - /// @param[out] resultParent The reference ids of the parent polygons for each result. - /// Zero if a result polygon has no parent. [opt] - /// @param[out] resultCost The search cost from @p centerPos to the polygon. [opt] - /// @param[out] resultCount The number of polygons found. [opt] - /// @param[in] maxResult The maximum number of polygons the result arrays can hold. - /// @returns The status flags for the query. - 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 the polygons along the naviation graph that touch the specified convex polygon. - /// @param[in] startRef The reference id of the polygon where the search starts. - /// @param[in] verts The vertices describing the convex polygon. (CCW) - /// [(x, y, z) * @p nverts] - /// @param[in] nverts The number of vertices in the polygon. - /// @param[in] filter The polygon filter to apply to the query. - /// @param[out] resultRef The reference ids of the polygons touched by the search polygon. [opt] - /// @param[out] resultParent The reference ids of the parent polygons for each result. Zero if a - /// result polygon has no parent. [opt] - /// @param[out] resultCost The search cost from the centroid point to the polygon. [opt] - /// @param[out] resultCount The number of polygons found. - /// @param[in] maxResult The maximum number of polygons the result arrays can hold. - /// @returns The status flags for the query. - 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; - - /// Gets a path from the explored nodes in the previous search. - /// @param[in] endRef The reference id of the end polygon. - /// @param[out] path An ordered list of polygon references representing the path. (Start to end.) - /// [(polyRef) * @p pathCount] - /// @param[out] pathCount The number of polygons returned in the @p path array. - /// @param[in] maxPath The maximum number of polygons the @p path array can hold. [Limit: >= 0] - /// @returns The status flags. Returns DT_FAILURE | DT_INVALID_PARAM if any parameter is wrong, or if - /// @p endRef was not explored in the previous search. Returns DT_SUCCESS | DT_BUFFER_TOO_SMALL - /// if @p path cannot contain the entire path. In this case it is filled to capacity with a partial path. - /// Otherwise returns DT_SUCCESS. - /// @remarks The result of this function depends on the state of the query object. For that reason it should only - /// be used immediately after one of the two Dijkstra searches, findPolysAroundCircle or findPolysAroundShape. - dtStatus getPathFromDijkstraSearch(dtPolyRef endRef, dtPolyRef* path, int* pathCount, int maxPath) const; - - /// @} - /// @name Local Query Functions - ///@{ - /// Finds the polygon nearest to the specified center point. - /// @param[in] center The center of the search box. [(x, y, z)] - /// @param[in] halfExtents The search distance along each axis. [(x, y, z)] - /// @param[in] filter The polygon filter to apply to the query. - /// @param[out] nearestRef The reference id of the nearest polygon. - /// @param[out] nearestPt The nearest point on the polygon. [opt] [(x, y, z)] - /// @returns The status flags for the query. - dtStatus findNearestPoly(const float* center, const float* halfExtents, - const dtQueryFilter* filter, - dtPolyRef* nearestRef, float* nearestPt) const; - - /// Finds polygons that overlap the search box. - /// @param[in] center The center of the search box. [(x, y, z)] - /// @param[in] halfExtents The search distance along each axis. [(x, y, z)] - /// @param[in] filter The polygon filter to apply to the query. - /// @param[out] polys The reference ids of the polygons that overlap the query box. - /// @param[out] polyCount The number of polygons in the search result. - /// @param[in] maxPolys The maximum number of polygons the search result can hold. - /// @returns The status flags for the query. - dtStatus queryPolygons(const float* center, const float* halfExtents, - const dtQueryFilter* filter, - dtPolyRef* polys, int* polyCount, const int maxPolys) const; - - /// Finds polygons that overlap the search box. - /// @param[in] center The center of the search box. [(x, y, z)] - /// @param[in] halfExtents The search distance along each axis. [(x, y, z)] - /// @param[in] filter The polygon filter to apply to the query. - /// @param[in] query The query. Polygons found will be batched together and passed to this query. - dtStatus queryPolygons(const float* center, const float* halfExtents, - const dtQueryFilter* filter, dtPolyQuery* query) const; - - /// Finds the non-overlapping navigation polygons in the local neighbourhood around the center position. - /// @param[in] startRef The reference id of the polygon where the search starts. - /// @param[in] centerPos The center of the query circle. [(x, y, z)] - /// @param[in] radius The radius of the query circle. - /// @param[in] filter The polygon filter to apply to the query. - /// @param[out] resultRef The reference ids of the polygons touched by the circle. - /// @param[out] resultParent The reference ids of the parent polygons for each result. - /// Zero if a result polygon has no parent. [opt] - /// @param[out] resultCount The number of polygons found. - /// @param[in] maxResult The maximum number of polygons the result arrays can hold. - /// @returns The status flags for the query. - dtStatus findLocalNeighbourhood(dtPolyRef startRef, const float* centerPos, const float radius, - const dtQueryFilter* filter, - dtPolyRef* resultRef, dtPolyRef* resultParent, - int* resultCount, const int maxResult) const; - - /// Moves from the start to the end position constrained to the navigation mesh. - /// @param[in] startRef The reference id of the start polygon. - /// @param[in] startPos A position of the mover within the start polygon. [(x, y, x)] - /// @param[in] endPos The desired end position of the mover. [(x, y, z)] - /// @param[in] filter The polygon filter to apply to the query. - /// @param[out] resultPos The result position of the mover. [(x, y, z)] - /// @param[out] visited The reference ids of the polygons visited during the move. - /// @param[out] visitedCount The number of polygons visited during the move. - /// @param[in] maxVisitedSize The maximum number of polygons the @p visited array can hold. - /// @returns The status flags for the query. - dtStatus moveAlongSurface(dtPolyRef startRef, const float* startPos, const float* endPos, - const dtQueryFilter* filter, - float* resultPos, dtPolyRef* visited, int* visitedCount, const int maxVisitedSize) const; - - /// Casts a 'walkability' ray along the surface of the navigation mesh from - /// the start position toward the end position. - /// @note A wrapper around raycast(..., RaycastHit*). Retained for backward compatibility. - /// @param[in] startRef The reference id of the start polygon. - /// @param[in] startPos A position within the start polygon representing - /// the start of the ray. [(x, y, z)] - /// @param[in] endPos The position to cast the ray toward. [(x, y, z)] - /// @param[out] t The hit parameter. (FLT_MAX if no wall hit.) - /// @param[out] hitNormal The normal of the nearest wall hit. [(x, y, z)] - /// @param[in] filter The polygon filter to apply to the query. - /// @param[out] path The reference ids of the visited polygons. [opt] - /// @param[out] pathCount The number of visited polygons. [opt] - /// @param[in] maxPath The maximum number of polygons the @p path array can hold. - /// @returns The status flags for the query. - 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; - - /// Casts a 'walkability' ray along the surface of the navigation mesh from - /// the start position toward the end position. - /// @param[in] startRef The reference id of the start polygon. - /// @param[in] startPos A position within the start polygon representing - /// the start of the ray. [(x, y, z)] - /// @param[in] endPos The position to cast the ray toward. [(x, y, z)] - /// @param[in] filter The polygon filter to apply to the query. - /// @param[in] flags govern how the raycast behaves. See dtRaycastOptions - /// @param[out] hit Pointer to a raycast hit structure which will be filled by the results. - /// @param[in] prevRef parent of start ref. Used during for cost calculation [opt] - /// @returns The status flags for the query. - dtStatus raycast(dtPolyRef startRef, const float* startPos, const float* endPos, - const dtQueryFilter* filter, const unsigned int options, - dtRaycastHit* hit, dtPolyRef prevRef = 0) const; - - /// Finds the distance from the specified position to the nearest polygon wall. - /// @param[in] startRef The reference id of the polygon containing @p centerPos. - /// @param[in] centerPos The center of the search circle. [(x, y, z)] - /// @param[in] maxRadius The radius of the search circle. - /// @param[in] filter The polygon filter to apply to the query. - /// @param[out] hitDist The distance to the nearest wall from @p centerPos. - /// @param[out] hitPos The nearest position on the wall that was hit. [(x, y, z)] - /// @param[out] hitNormal The normalized ray formed from the wall point to the - /// source point. [(x, y, z)] - /// @returns The status flags for the query. - dtStatus findDistanceToWall(dtPolyRef startRef, const float* centerPos, const float maxRadius, - const dtQueryFilter* filter, - float* hitDist, float* hitPos, float* hitNormal) const; - - /// Returns the segments for the specified polygon, optionally including portals. - /// @param[in] ref The reference id of the polygon. - /// @param[in] filter The polygon filter to apply to the query. - /// @param[out] segmentVerts The segments. [(ax, ay, az, bx, by, bz) * segmentCount] - /// @param[out] segmentRefs The reference ids of each segment's neighbor polygon. - /// Or zero if the segment is a wall. [opt] [(parentRef) * @p segmentCount] - /// @param[out] segmentCount The number of segments returned. - /// @param[in] maxSegments The maximum number of segments the result arrays can hold. - /// @returns The status flags for the query. - dtStatus getPolyWallSegments(dtPolyRef ref, const dtQueryFilter* filter, - float* segmentVerts, dtPolyRef* segmentRefs, int* segmentCount, - const int maxSegments) const; - - /// Returns random location on navmesh. - /// Polygons are chosen weighted by area. The search runs in linear related to number of polygon. - /// @param[in] filter The polygon filter to apply to the query. - /// @param[in] frand Function returning a random number [0..1). - /// @param[out] randomRef The reference id of the random location. - /// @param[out] randomPt The random location. - /// @returns The status flags for the query. - dtStatus findRandomPoint(const dtQueryFilter* filter, float (*frand)(), - dtPolyRef* randomRef, float* randomPt) const; - - /// Returns random location on navmesh within the reach of specified location. - /// Polygons are chosen weighted by area. The search runs in linear related to number of polygon. - /// The location is not exactly constrained by the circle, but it limits the visited polygons. - /// @param[in] startRef The reference id of the polygon where the search starts. - /// @param[in] centerPos The center of the search circle. [(x, y, z)] - /// @param[in] filter The polygon filter to apply to the query. - /// @param[in] frand Function returning a random number [0..1). - /// @param[out] randomRef The reference id of the random location. - /// @param[out] randomPt The random location. [(x, y, z)] - /// @returns The status flags for the query. - dtStatus findRandomPointAroundCircle(dtPolyRef startRef, const float* centerPos, const float maxRadius, - const dtQueryFilter* filter, float (*frand)(), - dtPolyRef* randomRef, float* randomPt) const; - - /// Finds the closest point on the specified polygon. - /// @param[in] ref The reference id of the polygon. - /// @param[in] pos The position to check. [(x, y, z)] - /// @param[out] closest The closest point on the polygon. [(x, y, z)] - /// @param[out] posOverPoly True of the position is over the polygon. - /// @returns The status flags for the query. - dtStatus closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const; - - /// Returns a point on the boundary closest to the source point if the source point is outside the - /// polygon's xz-bounds. - /// @param[in] ref The reference id to the polygon. - /// @param[in] pos The position to check. [(x, y, z)] - /// @param[out] closest The closest point. [(x, y, z)] - /// @returns The status flags for the query. - dtStatus closestPointOnPolyBoundary(dtPolyRef ref, const float* pos, float* closest) const; - - /// Gets the height of the polygon at the provided position using the height detail. (Most accurate.) - /// @param[in] ref The reference id of the polygon. - /// @param[in] pos A position within the xz-bounds of the polygon. [(x, y, z)] - /// @param[out] height The height at the surface of the polygon. - /// @returns The status flags for the query. - dtStatus getPolyHeight(dtPolyRef ref, const float* pos, float* height) const; - - /// @} - /// @name Miscellaneous Functions - /// @{ - /// Returns true if the polygon reference is valid and passes the filter restrictions. - /// @param[in] ref The polygon reference to check. - /// @param[in] filter The filter to apply. - bool isValidPolyRef(dtPolyRef ref, const dtQueryFilter* filter) const; - - /// Returns true if the polygon reference is in the closed list. - /// @param[in] ref The reference id of the polygon to check. - /// @returns True if the polygon is in closed list. - bool isInClosedList(dtPolyRef ref) const; - - /// Gets the node pool. - /// @returns The node pool. - class dtNodePool* getNodePool() const { return m_nodePool; } - - /// Gets the navigation mesh the query object is using. - /// @return The navigation mesh the query object is using. - const dtNavMesh* getAttachedNavMesh() const { return m_nav; } - - /// @} - + dtNavMeshQuery(); + ~dtNavMeshQuery(); + + /// Initializes the query object. + /// @param[in] nav Pointer to the dtNavMesh object to use for all queries. + /// @param[in] maxNodes Maximum number of search nodes. [Limits: 0 < value <= 65535] + /// @returns The status flags for the query. + dtStatus init(const dtNavMesh* nav, const int maxNodes); + + /// @name Standard Pathfinding Functions + /// @{ + + /// Finds a path from the start polygon to the end polygon. + /// @param[in] startRef The refrence id of the start polygon. + /// @param[in] endRef The reference id of the end polygon. + /// @param[in] startPos A position within the start polygon. [(x, y, z)] + /// @param[in] endPos A position within the end polygon. [(x, y, z)] + /// @param[in] filter The polygon filter to apply to the query. + /// @param[out] path An ordered list of polygon references representing the path. (Start to end.) + /// [(polyRef) * @p pathCount] + /// @param[out] pathCount The number of polygons returned in the @p path array. + /// @param[in] maxPath The maximum number of polygons the @p path array can hold. [Limit: >= 1] + dtStatus findPath(dtPolyRef startRef, dtPolyRef endRef, + const float* startPos, const float* endPos, + const dtQueryFilter* filter, + dtPolyRef* path, int* pathCount, const int maxPath) const; + + /// Finds the straight path from the start to the end position within the polygon corridor. + /// @param[in] startPos Path start position. [(x, y, z)] + /// @param[in] endPos Path end position. [(x, y, z)] + /// @param[in] path An array of polygon references that represent the path corridor. + /// @param[in] pathSize The number of polygons in the @p path array. + /// @param[out] straightPath Points describing the straight path. [(x, y, z) * @p straightPathCount]. + /// @param[out] straightPathFlags Flags describing each point. (See: #dtStraightPathFlags) [opt] + /// @param[out] straightPathRefs The reference id of the polygon that is being entered at each point. [opt] + /// @param[out] straightPathCount The number of points in the straight path. + /// @param[in] maxStraightPath The maximum number of points the straight path arrays can hold. [Limit: > 0] + /// @param[in] options Query options. (see: #dtStraightPathOptions) + /// @returns The status flags for the query. + 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 int options = 0) const; + + ///@} + /// @name Sliced Pathfinding Functions + /// Common use case: + /// -# Call initSlicedFindPath() to initialize the sliced path query. + /// -# Call updateSlicedFindPath() until it returns complete. + /// -# Call finalizeSlicedFindPath() to get the path. + ///@{ + + /// Intializes a sliced path query. + /// @param[in] startRef The refrence id of the start polygon. + /// @param[in] endRef The reference id of the end polygon. + /// @param[in] startPos A position within the start polygon. [(x, y, z)] + /// @param[in] endPos A position within the end polygon. [(x, y, z)] + /// @param[in] filter The polygon filter to apply to the query. + /// @param[in] options query options (see: #dtFindPathOptions) + /// @returns The status flags for the query. + dtStatus initSlicedFindPath(dtPolyRef startRef, dtPolyRef endRef, + const float* startPos, const float* endPos, + const dtQueryFilter* filter, const unsigned int options = 0); + + /// Updates an in-progress sliced path query. + /// @param[in] maxIter The maximum number of iterations to perform. + /// @param[out] doneIters The actual number of iterations completed. [opt] + /// @returns The status flags for the query. + dtStatus updateSlicedFindPath(const int maxIter, int* doneIters); + + /// Finalizes and returns the results of a sliced path query. + /// @param[out] path An ordered list of polygon references representing the path. (Start to end.) + /// [(polyRef) * @p pathCount] + /// @param[out] pathCount The number of polygons returned in the @p path array. + /// @param[in] maxPath The max number of polygons the path array can hold. [Limit: >= 1] + /// @returns The status flags for the query. + dtStatus finalizeSlicedFindPath(dtPolyRef* path, int* pathCount, const int maxPath); + + /// Finalizes and returns the results of an incomplete sliced path query, returning the path to the furthest + /// polygon on the existing path that was visited during the search. + /// @param[in] existing An array of polygon references for the existing path. + /// @param[in] existingSize The number of polygon in the @p existing array. + /// @param[out] path An ordered list of polygon references representing the path. (Start to end.) + /// [(polyRef) * @p pathCount] + /// @param[out] pathCount The number of polygons returned in the @p path array. + /// @param[in] maxPath The max number of polygons the @p path array can hold. [Limit: >= 1] + /// @returns The status flags for the query. + dtStatus finalizeSlicedFindPathPartial(const dtPolyRef* existing, const int existingSize, + dtPolyRef* path, int* pathCount, const int maxPath); + + ///@} + /// @name Dijkstra Search Functions + /// @{ + + /// Finds the polygons along the navigation graph that touch the specified circle. + /// @param[in] startRef The reference id of the polygon where the search starts. + /// @param[in] centerPos The center of the search circle. [(x, y, z)] + /// @param[in] radius The radius of the search circle. + /// @param[in] filter The polygon filter to apply to the query. + /// @param[out] resultRef The reference ids of the polygons touched by the circle. [opt] + /// @param[out] resultParent The reference ids of the parent polygons for each result. + /// Zero if a result polygon has no parent. [opt] + /// @param[out] resultCost The search cost from @p centerPos to the polygon. [opt] + /// @param[out] resultCount The number of polygons found. [opt] + /// @param[in] maxResult The maximum number of polygons the result arrays can hold. + /// @returns The status flags for the query. + 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 the polygons along the naviation graph that touch the specified convex polygon. + /// @param[in] startRef The reference id of the polygon where the search starts. + /// @param[in] verts The vertices describing the convex polygon. (CCW) + /// [(x, y, z) * @p nverts] + /// @param[in] nverts The number of vertices in the polygon. + /// @param[in] filter The polygon filter to apply to the query. + /// @param[out] resultRef The reference ids of the polygons touched by the search polygon. [opt] + /// @param[out] resultParent The reference ids of the parent polygons for each result. Zero if a + /// result polygon has no parent. [opt] + /// @param[out] resultCost The search cost from the centroid point to the polygon. [opt] + /// @param[out] resultCount The number of polygons found. + /// @param[in] maxResult The maximum number of polygons the result arrays can hold. + /// @returns The status flags for the query. + 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; + + /// Gets a path from the explored nodes in the previous search. + /// @param[in] endRef The reference id of the end polygon. + /// @param[out] path An ordered list of polygon references representing the path. (Start to end.) + /// [(polyRef) * @p pathCount] + /// @param[out] pathCount The number of polygons returned in the @p path array. + /// @param[in] maxPath The maximum number of polygons the @p path array can hold. [Limit: >= 0] + /// @returns The status flags. Returns DT_FAILURE | DT_INVALID_PARAM if any parameter is wrong, or if + /// @p endRef was not explored in the previous search. Returns DT_SUCCESS | DT_BUFFER_TOO_SMALL + /// if @p path cannot contain the entire path. In this case it is filled to capacity with a partial path. + /// Otherwise returns DT_SUCCESS. + /// @remarks The result of this function depends on the state of the query object. For that reason it should only + /// be used immediately after one of the two Dijkstra searches, findPolysAroundCircle or findPolysAroundShape. + dtStatus getPathFromDijkstraSearch(dtPolyRef endRef, dtPolyRef* path, int* pathCount, int maxPath) const; + + /// @} + /// @name Local Query Functions + ///@{ + + /// Finds the polygon nearest to the specified center point. + /// [opt] means the specified parameter can be a null pointer, in that case the output parameter will not be set. + /// + /// @param[in] center The center of the search box. [(x, y, z)] + /// @param[in] halfExtents The search distance along each axis. [(x, y, z)] + /// @param[in] filter The polygon filter to apply to the query. + /// @param[out] nearestRef The reference id of the nearest polygon. Will be set to 0 if no polygon is found. + /// @param[out] nearestPt The nearest point on the polygon. Unchanged if no polygon is found. [opt] [(x, y, z)] + /// @returns The status flags for the query. + dtStatus findNearestPoly(const float* center, const float* halfExtents, + const dtQueryFilter* filter, + dtPolyRef* nearestRef, float* nearestPt) const; + + /// Finds the polygon nearest to the specified center point. + /// [opt] means the specified parameter can be a null pointer, in that case the output parameter will not be set. + /// + /// @param[in] center The center of the search box. [(x, y, z)] + /// @param[in] halfExtents The search distance along each axis. [(x, y, z)] + /// @param[in] filter The polygon filter to apply to the query. + /// @param[out] nearestRef The reference id of the nearest polygon. Will be set to 0 if no polygon is found. + /// @param[out] nearestPt The nearest point on the polygon. Unchanged if no polygon is found. [opt] [(x, y, z)] + /// @param[out] isOverPoly Set to true if the point's X/Z coordinate lies inside the polygon, false otherwise. Unchanged if no polygon is found. [opt] + /// @returns The status flags for the query. + dtStatus findNearestPoly(const float* center, const float* halfExtents, + const dtQueryFilter* filter, + dtPolyRef* nearestRef, float* nearestPt, bool* isOverPoly) const; + + /// Finds polygons that overlap the search box. + /// @param[in] center The center of the search box. [(x, y, z)] + /// @param[in] halfExtents The search distance along each axis. [(x, y, z)] + /// @param[in] filter The polygon filter to apply to the query. + /// @param[out] polys The reference ids of the polygons that overlap the query box. + /// @param[out] polyCount The number of polygons in the search result. + /// @param[in] maxPolys The maximum number of polygons the search result can hold. + /// @returns The status flags for the query. + dtStatus queryPolygons(const float* center, const float* halfExtents, + const dtQueryFilter* filter, + dtPolyRef* polys, int* polyCount, const int maxPolys) const; + + /// Finds polygons that overlap the search box. + /// @param[in] center The center of the search box. [(x, y, z)] + /// @param[in] halfExtents The search distance along each axis. [(x, y, z)] + /// @param[in] filter The polygon filter to apply to the query. + /// @param[in] query The query. Polygons found will be batched together and passed to this query. + dtStatus queryPolygons(const float* center, const float* halfExtents, + const dtQueryFilter* filter, dtPolyQuery* query) const; + + /// Finds the non-overlapping navigation polygons in the local neighbourhood around the center position. + /// @param[in] startRef The reference id of the polygon where the search starts. + /// @param[in] centerPos The center of the query circle. [(x, y, z)] + /// @param[in] radius The radius of the query circle. + /// @param[in] filter The polygon filter to apply to the query. + /// @param[out] resultRef The reference ids of the polygons touched by the circle. + /// @param[out] resultParent The reference ids of the parent polygons for each result. + /// Zero if a result polygon has no parent. [opt] + /// @param[out] resultCount The number of polygons found. + /// @param[in] maxResult The maximum number of polygons the result arrays can hold. + /// @returns The status flags for the query. + dtStatus findLocalNeighbourhood(dtPolyRef startRef, const float* centerPos, const float radius, + const dtQueryFilter* filter, + dtPolyRef* resultRef, dtPolyRef* resultParent, + int* resultCount, const int maxResult) const; + + /// Moves from the start to the end position constrained to the navigation mesh. + /// @param[in] startRef The reference id of the start polygon. + /// @param[in] startPos A position of the mover within the start polygon. [(x, y, x)] + /// @param[in] endPos The desired end position of the mover. [(x, y, z)] + /// @param[in] filter The polygon filter to apply to the query. + /// @param[out] resultPos The result position of the mover. [(x, y, z)] + /// @param[out] visited The reference ids of the polygons visited during the move. + /// @param[out] visitedCount The number of polygons visited during the move. + /// @param[in] maxVisitedSize The maximum number of polygons the @p visited array can hold. + /// @returns The status flags for the query. + dtStatus moveAlongSurface(dtPolyRef startRef, const float* startPos, const float* endPos, + const dtQueryFilter* filter, + float* resultPos, dtPolyRef* visited, int* visitedCount, const int maxVisitedSize) const; + + /// Casts a 'walkability' ray along the surface of the navigation mesh from + /// the start position toward the end position. + /// @note A wrapper around raycast(..., RaycastHit*). Retained for backward compatibility. + /// @param[in] startRef The reference id of the start polygon. + /// @param[in] startPos A position within the start polygon representing + /// the start of the ray. [(x, y, z)] + /// @param[in] endPos The position to cast the ray toward. [(x, y, z)] + /// @param[in] filter The polygon filter to apply to the query. + /// @param[out] t The hit parameter. (FLT_MAX if no wall hit.) + /// @param[out] hitNormal The normal of the nearest wall hit. [(x, y, z)] + /// @param[out] path The reference ids of the visited polygons. [opt] + /// @param[out] pathCount The number of visited polygons. [opt] + /// @param[in] maxPath The maximum number of polygons the @p path array can hold. + /// @returns The status flags for the query. + 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; + + /// Casts a 'walkability' ray along the surface of the navigation mesh from + /// the start position toward the end position. + /// @param[in] startRef The reference id of the start polygon. + /// @param[in] startPos A position within the start polygon representing + /// the start of the ray. [(x, y, z)] + /// @param[in] endPos The position to cast the ray toward. [(x, y, z)] + /// @param[in] filter The polygon filter to apply to the query. + /// @param[in] options govern how the raycast behaves. See dtRaycastOptions + /// @param[out] hit Pointer to a raycast hit structure which will be filled by the results. + /// @param[in] prevRef parent of start ref. Used during for cost calculation [opt] + /// @returns The status flags for the query. + dtStatus raycast(dtPolyRef startRef, const float* startPos, const float* endPos, + const dtQueryFilter* filter, const unsigned int options, + dtRaycastHit* hit, dtPolyRef prevRef = 0) const; + + + /// Finds the distance from the specified position to the nearest polygon wall. + /// @param[in] startRef The reference id of the polygon containing @p centerPos. + /// @param[in] centerPos The center of the search circle. [(x, y, z)] + /// @param[in] maxRadius The radius of the search circle. + /// @param[in] filter The polygon filter to apply to the query. + /// @param[out] hitDist The distance to the nearest wall from @p centerPos. + /// @param[out] hitPos The nearest position on the wall that was hit. [(x, y, z)] + /// @param[out] hitNormal The normalized ray formed from the wall point to the + /// source point. [(x, y, z)] + /// @returns The status flags for the query. + dtStatus findDistanceToWall(dtPolyRef startRef, const float* centerPos, const float maxRadius, + const dtQueryFilter* filter, + float* hitDist, float* hitPos, float* hitNormal) const; + + /// Returns the segments for the specified polygon, optionally including portals. + /// @param[in] ref The reference id of the polygon. + /// @param[in] filter The polygon filter to apply to the query. + /// @param[out] segmentVerts The segments. [(ax, ay, az, bx, by, bz) * segmentCount] + /// @param[out] segmentRefs The reference ids of each segment's neighbor polygon. + /// Or zero if the segment is a wall. [opt] [(parentRef) * @p segmentCount] + /// @param[out] segmentCount The number of segments returned. + /// @param[in] maxSegments The maximum number of segments the result arrays can hold. + /// @returns The status flags for the query. + dtStatus getPolyWallSegments(dtPolyRef ref, const dtQueryFilter* filter, + float* segmentVerts, dtPolyRef* segmentRefs, int* segmentCount, + const int maxSegments) const; + + /// Returns random location on navmesh. + /// Polygons are chosen weighted by area. The search runs in linear related to number of polygon. + /// @param[in] filter The polygon filter to apply to the query. + /// @param[in] frand Function returning a random number [0..1). + /// @param[out] randomRef The reference id of the random location. + /// @param[out] randomPt The random location. + /// @returns The status flags for the query. + dtStatus findRandomPoint(const dtQueryFilter* filter, float (*frand)(), + dtPolyRef* randomRef, float* randomPt) const; + + /// Returns random location on navmesh within the reach of specified location. + /// Polygons are chosen weighted by area. The search runs in linear related to number of polygon. + /// The location is not exactly constrained by the circle, but it limits the visited polygons. + /// @param[in] startRef The reference id of the polygon where the search starts. + /// @param[in] centerPos The center of the search circle. [(x, y, z)] + /// @param[in] maxRadius The radius of the search circle. [Units: wu] + /// @param[in] filter The polygon filter to apply to the query. + /// @param[in] frand Function returning a random number [0..1). + /// @param[out] randomRef The reference id of the random location. + /// @param[out] randomPt The random location. [(x, y, z)] + /// @returns The status flags for the query. + dtStatus findRandomPointAroundCircle(dtPolyRef startRef, const float* centerPos, const float maxRadius, + const dtQueryFilter* filter, float (*frand)(), + dtPolyRef* randomRef, float* randomPt) const; + + /// Finds the closest point on the specified polygon. + /// @param[in] ref The reference id of the polygon. + /// @param[in] pos The position to check. [(x, y, z)] + /// @param[out] closest The closest point on the polygon. [(x, y, z)] + /// @param[out] posOverPoly True of the position is over the polygon. + /// @returns The status flags for the query. + dtStatus closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const; + + /// Returns a point on the boundary closest to the source point if the source point is outside the + /// polygon's xz-bounds. + /// @param[in] ref The reference id to the polygon. + /// @param[in] pos The position to check. [(x, y, z)] + /// @param[out] closest The closest point. [(x, y, z)] + /// @returns The status flags for the query. + dtStatus closestPointOnPolyBoundary(dtPolyRef ref, const float* pos, float* closest) const; + + /// Gets the height of the polygon at the provided position using the height detail. (Most accurate.) + /// @param[in] ref The reference id of the polygon. + /// @param[in] pos A position within the xz-bounds of the polygon. [(x, y, z)] + /// @param[out] height The height at the surface of the polygon. + /// @returns The status flags for the query. + dtStatus getPolyHeight(dtPolyRef ref, const float* pos, float* height) const; + + /// @} + /// @name Miscellaneous Functions + /// @{ + + /// Returns true if the polygon reference is valid and passes the filter restrictions. + /// @param[in] ref The polygon reference to check. + /// @param[in] filter The filter to apply. + bool isValidPolyRef(dtPolyRef ref, const dtQueryFilter* filter) const; + + /// Returns true if the polygon reference is in the closed list. + /// @param[in] ref The reference id of the polygon to check. + /// @returns True if the polygon is in closed list. + bool isInClosedList(dtPolyRef ref) const; + + /// Gets the node pool. + /// @returns The node pool. + class dtNodePool* getNodePool() const { return m_nodePool; } + + /// Gets the navigation mesh the query object is using. + /// @return The navigation mesh the query object is using. + const dtNavMesh* getAttachedNavMesh() const { return m_nav; } + + /// @} + private: - // Explicitly disabled copy constructor and copy assignment operator - dtNavMeshQuery(const dtNavMeshQuery&); - dtNavMeshQuery& operator=(const dtNavMeshQuery&); - - /// Queries polygons within a tile. - void queryPolygonsInTile(const dtMeshTile* tile, const float* qmin, const float* qmax, - const dtQueryFilter* filter, dtPolyQuery* query) 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; - - // Appends vertex to a straight path - dtStatus appendVertex(const float* pos, const unsigned char flags, const dtPolyRef ref, - float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs, - int* straightPathCount, const int maxStraightPath) const; - - // Appends intermediate portal points to a straight path. - dtStatus appendPortals(const int startIdx, const int endIdx, const float* endPos, const dtPolyRef* path, - float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs, - int* straightPathCount, const int maxStraightPath, const int options) const; - - // Gets the path leading to the specified end node. - dtStatus getPathToNode(struct dtNode* endNode, dtPolyRef* path, int* pathCount, int maxPath) 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; - unsigned int options; - float raycastLimitSqr; - }; - 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. + // Explicitly disabled copy constructor and copy assignment operator + dtNavMeshQuery(const dtNavMeshQuery&); + dtNavMeshQuery& operator=(const dtNavMeshQuery&); + + /// Queries polygons within a tile. + void queryPolygonsInTile(const dtMeshTile* tile, const float* qmin, const float* qmax, + const dtQueryFilter* filter, dtPolyQuery* query) 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; + + // Appends vertex to a straight path + dtStatus appendVertex(const float* pos, const unsigned char flags, const dtPolyRef ref, + float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs, + int* straightPathCount, const int maxStraightPath) const; + + // Appends intermediate portal points to a straight path. + dtStatus appendPortals(const int startIdx, const int endIdx, const float* endPos, const dtPolyRef* path, + float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs, + int* straightPathCount, const int maxStraightPath, const int options) const; + + // Gets the path leading to the specified end node. + dtStatus getPathToNode(struct dtNode* endNode, dtPolyRef* path, int* pathCount, int maxPath) 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; + unsigned int options; + float raycastLimitSqr; + }; + 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. }; /// Allocates a query object using the Detour allocator. diff --git a/recastnavigation/Detour/Include/DetourNode.h b/recastnavigation/Detour/Include/DetourNode.h index 171bd40..8918d46 100644 --- a/recastnavigation/Detour/Include/DetourNode.h +++ b/recastnavigation/Detour/Include/DetourNode.h @@ -23,9 +23,9 @@ enum dtNodeFlags { - DT_NODE_OPEN = 0x01, - DT_NODE_CLOSED = 0x02, - DT_NODE_PARENT_DETACHED = 0x04, // parent of the node is not adjacent. Found using raycast. + DT_NODE_OPEN = 0x01, + DT_NODE_CLOSED = 0x02, + DT_NODE_PARENT_DETACHED = 0x04 // parent of the node is not adjacent. Found using raycast. }; typedef unsigned short dtNodeIndex; @@ -35,13 +35,13 @@ static const int DT_NODE_PARENT_BITS = 24; static const int DT_NODE_STATE_BITS = 2; 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 : DT_NODE_PARENT_BITS; ///< Index to parent node. - unsigned int state : DT_NODE_STATE_BITS; ///< extra state information. A polyRef can have multiple nodes with different extra info. see DT_MAX_STATES_PER_NODE - unsigned int flags : 3; ///< Node flags. A combination of dtNodeFlags. - dtPolyRef id; ///< Polygon ref the node corresponds to. + 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 : DT_NODE_PARENT_BITS; ///< Index to parent node. + unsigned int state : DT_NODE_STATE_BITS; ///< extra state information. A polyRef can have multiple nodes with different extra info. see DT_MAX_STATES_PER_NODE + unsigned int flags : 3; ///< Node flags. A combination of dtNodeFlags. + dtPolyRef id; ///< Polygon ref the node corresponds to. }; static const int DT_MAX_STATES_PER_NODE = 1 << DT_NODE_STATE_BITS; // number of extra states per node. See dtNode::state @@ -49,119 +49,120 @@ static const int DT_MAX_STATES_PER_NODE = 1 << DT_NODE_STATE_BITS; // number of class dtNodePool { public: - dtNodePool(int maxNodes, int hashSize); - ~dtNodePool(); - void clear(); - - // Get a dtNode by ref and extra state information. If there is none then - allocate - // There can be more than one node for the same polyRef but with different extra state information - dtNode* getNode(dtPolyRef id, unsigned char state = 0); - dtNode* findNode(dtPolyRef id, unsigned char state); - unsigned int findNodes(dtPolyRef id, dtNode** nodes, const int maxNodes); - - 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]; } - inline int getNodeCount() const { return m_nodeCount; } - + dtNodePool(int maxNodes, int hashSize); + ~dtNodePool(); + void clear(); + + // Get a dtNode by ref and extra state information. If there is none then - allocate + // There can be more than one node for the same polyRef but with different extra state information + dtNode* getNode(dtPolyRef id, unsigned char state=0); + dtNode* findNode(dtPolyRef id, unsigned char state); + unsigned int findNodes(dtPolyRef id, dtNode** nodes, const int maxNodes); + + 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]; } + inline int getNodeCount() const { return m_nodeCount; } + private: - // Explicitly disabled copy constructor and copy assignment operator. - dtNodePool(const dtNodePool&); - dtNodePool& operator=(const dtNodePool&); - - dtNode* m_nodes; - dtNodeIndex* m_first; - dtNodeIndex* m_next; - const int m_maxNodes; - const int m_hashSize; - int m_nodeCount; + // Explicitly disabled copy constructor and copy assignment operator. + dtNodePool(const dtNodePool&); + dtNodePool& operator=(const dtNodePool&); + + 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 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; } - + dtNodeQueue(int n); + ~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: - // Explicitly disabled copy constructor and copy assignment operator. - dtNodeQueue(const dtNodeQueue&); - dtNodeQueue& operator=(const dtNodeQueue&); + // Explicitly disabled copy constructor and copy assignment operator. + dtNodeQueue(const dtNodeQueue&); + dtNodeQueue& operator=(const dtNodeQueue&); + + void bubbleUp(int i, dtNode* node); + void trickleDown(int i, dtNode* node); + + dtNode** m_heap; + const int m_capacity; + int m_size; +}; - 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 diff --git a/recastnavigation/Detour/Include/DetourStatus.h b/recastnavigation/Detour/Include/DetourStatus.h index 73a15cf..8e1bb44 100644 --- a/recastnavigation/Detour/Include/DetourStatus.h +++ b/recastnavigation/Detour/Include/DetourStatus.h @@ -34,31 +34,32 @@ static const unsigned int DT_OUT_OF_MEMORY = 1 << 2; // Operation ran out of mem static const unsigned int DT_INVALID_PARAM = 1 << 3; // An input parameter was invalid. static const unsigned int DT_BUFFER_TOO_SMALL = 1 << 4; // Result buffer for the query was too small to store all results. static const unsigned int DT_OUT_OF_NODES = 1 << 5; // Query ran out of nodes during search. -static const unsigned int DT_PARTIAL_RESULT = 1 << 6; // Query did not reach the end location, returning best guess. +static const unsigned int DT_PARTIAL_RESULT = 1 << 6; // Query did not reach the end location, returning best guess. static const unsigned int DT_ALREADY_OCCUPIED = 1 << 7; // A tile has already been assigned to the given x,y coordinate + // Returns true of status is success. inline bool dtStatusSucceed(dtStatus status) { - return (status & DT_SUCCESS) != 0; + return (status & DT_SUCCESS) != 0; } // Returns true of status is failure. inline bool dtStatusFailed(dtStatus status) { - return (status & DT_FAILURE) != 0; + return (status & DT_FAILURE) != 0; } // Returns true of status is in progress. inline bool dtStatusInProgress(dtStatus status) { - return (status & DT_IN_PROGRESS) != 0; + return (status & DT_IN_PROGRESS) != 0; } // Returns true if specific detail is set. inline bool dtStatusDetail(dtStatus status, unsigned int detail) { - return (status & detail) != 0; + return (status & detail) != 0; } #endif // DETOURSTATUS_H diff --git a/recastnavigation/Detour/Source/DetourAlloc.cpp b/recastnavigation/Detour/Source/DetourAlloc.cpp index 2b07195..d9ad1fc 100644 --- a/recastnavigation/Detour/Source/DetourAlloc.cpp +++ b/recastnavigation/Detour/Source/DetourAlloc.cpp @@ -19,32 +19,32 @@ #include #include "DetourAlloc.h" -static void* dtAllocDefault(size_t size, dtAllocHint) +static void *dtAllocDefault(size_t size, dtAllocHint) { - return malloc(size); + return malloc(size); } -static void dtFreeDefault(void* ptr) +static void dtFreeDefault(void *ptr) { - free(ptr); + free(ptr); } static dtAllocFunc* sAllocFunc = dtAllocDefault; static dtFreeFunc* sFreeFunc = dtFreeDefault; -void dtAllocSetCustom(dtAllocFunc* allocFunc, dtFreeFunc* freeFunc) +void dtAllocSetCustom(dtAllocFunc *allocFunc, dtFreeFunc *freeFunc) { - sAllocFunc = allocFunc ? allocFunc : dtAllocDefault; - sFreeFunc = freeFunc ? freeFunc : dtFreeDefault; + sAllocFunc = allocFunc ? allocFunc : dtAllocDefault; + sFreeFunc = freeFunc ? freeFunc : dtFreeDefault; } void* dtAlloc(size_t size, dtAllocHint hint) { - return sAllocFunc(size, hint); + return sAllocFunc(size, hint); } void dtFree(void* ptr) { - if (ptr) - sFreeFunc(ptr); -} \ No newline at end of file + if (ptr) + sFreeFunc(ptr); +} diff --git a/recastnavigation/Detour/Source/DetourAssert.cpp b/recastnavigation/Detour/Source/DetourAssert.cpp index b7fd1bb..5e019e0 100644 --- a/recastnavigation/Detour/Source/DetourAssert.cpp +++ b/recastnavigation/Detour/Source/DetourAssert.cpp @@ -22,14 +22,14 @@ static dtAssertFailFunc* sAssertFailFunc = 0; -void dtAssertFailSetCustom(dtAssertFailFunc* assertFailFunc) +void dtAssertFailSetCustom(dtAssertFailFunc *assertFailFunc) { - sAssertFailFunc = assertFailFunc; + sAssertFailFunc = assertFailFunc; } dtAssertFailFunc* dtAssertFailGetCustom() { - return sAssertFailFunc; + return sAssertFailFunc; } -#endif \ No newline at end of file +#endif diff --git a/recastnavigation/Detour/Source/DetourCommon.cpp b/recastnavigation/Detour/Source/DetourCommon.cpp index 730c5b8..b845782 100644 --- a/recastnavigation/Detour/Source/DetourCommon.cpp +++ b/recastnavigation/Detour/Source/DetourCommon.cpp @@ -22,214 +22,214 @@ ////////////////////////////////////////////////////////////////////////////////////////// void dtClosestPtPointTriangle(float* closest, const float* p, - const float* a, const float* b, const float* c) + 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; + // 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) + 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; + static const float EPS = 0.000001f; + + 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; + 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; + 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) { - const float EPS = 1e-6f; - float v0[3], v1[3], v2[3]; - - dtVsub(v0, c, a); - dtVsub(v1, b, a); - dtVsub(v2, p, a); - - // Compute scaled barycentric coordinates - float denom = v0[0] * v1[2] - v0[2] * v1[0]; - if (fabsf(denom) < EPS) - return false; - - float u = v1[2] * v2[0] - v1[0] * v2[2]; - float v = v0[0] * v2[2] - v0[2] * v2[0]; - - if (denom < 0) { - denom = -denom; - u = -u; - v = -v; - } - - // If point lies inside the triangle, return interpolated ycoord. - if (u >= 0.0f && v >= 0.0f && (u + v) <= denom) { - h = a[1] + (v0[1] * u + v1[1] * v) / denom; - return true; - } - return false; + const float EPS = 1e-6f; + float v0[3], v1[3], v2[3]; + + dtVsub(v0, c, a); + dtVsub(v1, b, a); + dtVsub(v2, p, a); + + // Compute scaled barycentric coordinates + float denom = v0[0] * v1[2] - v0[2] * v1[0]; + if (fabsf(denom) < EPS) + return false; + + float u = v1[2] * v2[0] - v1[0] * v2[2]; + float v = v0[0] * v2[2] - v0[2] * v2[0]; + + if (denom < 0) { + denom = -denom; + u = -u; + v = -v; + } + + // If point lies inside the triangle, return interpolated ycoord. + if (u >= 0.0f && v >= 0.0f && (u + v) <= denom) { + h = a[1] + (v0[1] * u + v1[1] * v) / denom; + return true; + } + return false; } /// @par @@ -237,150 +237,151 @@ bool dtClosestHeightPointTriangle(const float* p, const float* a, const float* b /// All points are projected onto the xz-plane, so the y-values are ignored. 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; + // 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) + 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; + // 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) + 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); - } + 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) + const float bmin, const float bmax, + const float eps) { - return ((amin + eps) > bmax || (amax - eps) < bmin) ? false : true; + return ((amin+eps) > bmax || (amax-eps) < bmin) ? false : true; } /// @par /// /// All vertices are projected onto the xz-plane, so the y-values are ignored. bool dtOverlapPolyPoly2D(const float* polya, const int npolya, - const float* polyb, const int npolyb) + 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; + 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; } // Returns a random point in a convex polygon. // Adapted from Graphics Gems article. void dtRandomPointInConvexPoly(const float* pts, const int npts, float* areas, - const float s, const float t, float* out) + const float s, const float t, float* out) { - // Calc triangle araes - float areasum = 0.0f; - for (int i = 2; i < npts; i++) { - areas[i] = dtTriArea2D(&pts[0], &pts[(i - 1) * 3], &pts[i * 3]); - areasum += dtMax(0.001f, areas[i]); - } - // Find sub triangle weighted by area. - const float thr = s * areasum; - float acc = 0.0f; - float u = 1.0f; - int tri = npts - 1; - for (int i = 2; i < npts; i++) { - const float dacc = areas[i]; - if (thr >= acc && thr < (acc + dacc)) - { - u = (thr - acc) / dacc; - tri = i; - break; - } - acc += dacc; - } - - float v = dtMathSqrtf(t); - - const float a = 1 - v; - const float b = (1 - u) * v; - const float c = u * v; - const float* pa = &pts[0]; - const float* pb = &pts[(tri - 1) * 3]; - const float* pc = &pts[tri * 3]; - - out[0] = a * pa[0] + b * pb[0] + c * pc[0]; - out[1] = a * pa[1] + b * pb[1] + c * pc[1]; - out[2] = a * pa[2] + b * pb[2] + c * pc[2]; + // Calc triangle araes + float areasum = 0.0f; + for (int i = 2; i < npts; i++) { + areas[i] = dtTriArea2D(&pts[0], &pts[(i-1)*3], &pts[i*3]); + areasum += dtMax(0.001f, areas[i]); + } + // Find sub triangle weighted by area. + const float thr = s*areasum; + float acc = 0.0f; + float u = 1.0f; + int tri = npts - 1; + for (int i = 2; i < npts; i++) { + const float dacc = areas[i]; + if (thr >= acc && thr < (acc+dacc)) + { + u = (thr - acc) / dacc; + tri = i; + break; + } + acc += dacc; + } + + float v = dtMathSqrtf(t); + + const float a = 1 - v; + const float b = (1 - u) * v; + const float c = u * v; + const float* pa = &pts[0]; + const float* pb = &pts[(tri-1)*3]; + const float* pc = &pts[tri*3]; + + out[0] = a*pa[0] + b*pb[0] + c*pc[0]; + out[1] = a*pa[1] + b*pb[1] + c*pc[1]; + out[2] = a*pa[2] + b*pb[2] + c*pc[2]; } -inline float vperpXZ(const float* a, const float* b) { return a[0] * b[2] - a[2] * b[0]; } +inline float vperpXZ(const float* a, const float* b) { return a[0]*b[2] - a[2]*b[0]; } bool dtIntersectSegSeg2D(const float* ap, const float* aq, - const float* bp, const float* bq, - float& s, float& t) + const float* bp, const float* bq, + float& s, float& t) { - float u[3], v[3], w[3]; - dtVsub(u, aq, ap); - dtVsub(v, bq, bp); - dtVsub(w, ap, bp); - float d = vperpXZ(u, v); - if (fabsf(d) < 1e-6f) return false; - s = vperpXZ(v, w) / d; - t = vperpXZ(u, w) / d; - return true; -} \ No newline at end of file + float u[3], v[3], w[3]; + dtVsub(u,aq,ap); + dtVsub(v,bq,bp); + dtVsub(w,ap,bp); + float d = vperpXZ(u,v); + if (fabsf(d) < 1e-6f) return false; + s = vperpXZ(v,w) / d; + t = vperpXZ(u,w) / d; + return true; +} + diff --git a/recastnavigation/Detour/Source/DetourNavMesh.cpp b/recastnavigation/Detour/Source/DetourNavMesh.cpp index cd606ad..2240526 100644 --- a/recastnavigation/Detour/Source/DetourNavMesh.cpp +++ b/recastnavigation/Detour/Source/DetourNavMesh.cpp @@ -27,117 +27,119 @@ #include "DetourAssert.h" #include + inline bool overlapSlabs(const float* amin, const float* amax, - const float* bmin, const float* bmax, - const float px, const float py) + 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; + // 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 float getSlabCoord(const float* va, const int side) { - if (side == 0 || side == 4) - return va[0]; - else if (side == 2 || side == 6) - return va[2]; - return 0; + if (side == 0 || side == 4) + return va[0]; + else if (side == 2 || side == 6) + return va[2]; + return 0; } 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]; - } - } + 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); + 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; + 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; + 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* mem = dtAlloc(sizeof(dtNavMesh), DT_ALLOC_PERM); + if (!mem) return 0; + return new(mem) dtNavMesh; } /// @par @@ -146,9 +148,9 @@ dtNavMesh* dtAllocNavMesh() /// flag set. void dtFreeNavMesh(dtNavMesh* navmesh) { - if (!navmesh) return; - navmesh->~dtNavMesh(); - dtFree(navmesh); + if (!navmesh) return; + navmesh->~dtNavMesh(); + dtFree(navmesh); } ////////////////////////////////////////////////////////////////////////////////////////// @@ -175,114 +177,114 @@ The general build process is as follows: Notes: - This class is usually used in conjunction with the dtNavMeshQuery class for pathfinding. -- Technically, all navigation meshes are tiled. A 'solo' mesh is simply a navigation mesh initialized +- Technically, all navigation meshes are tiled. A 'solo' mesh is simply a navigation mesh initialized to have only a single tile. -- This class does not implement any asynchronous methods. So the ::dtStatus result of all methods will +- This class does not implement any asynchronous methods. So the ::dtStatus result of all methods will always contain either a success or failure flag. @see dtNavMeshQuery, dtCreateNavMeshData, dtNavMeshCreateParams, #dtAllocNavMesh, #dtFreeNavMesh */ 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_tileWidth(0), + m_tileHeight(0), + m_maxTiles(0), + m_tileLutSize(0), + m_tileLutMask(0), + m_posLookup(0), + m_nextFree(0), + m_tiles(0) { #ifndef DT_POLYREF64 - m_saltBits = 0; - m_tileBits = 0; - m_polyBits = 0; + m_saltBits = 0; + m_tileBits = 0; + m_polyBits = 0; #endif - memset(&m_params, 0, sizeof(dtNavMeshParams)); - m_orig[0] = 0; - m_orig[1] = 0; - m_orig[2] = 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); + 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 | DT_OUT_OF_MEMORY; - m_posLookup = (dtMeshTile**)dtAlloc(sizeof(dtMeshTile*) * m_tileLutSize, DT_ALLOC_PERM); - if (!m_posLookup) - return DT_FAILURE | DT_OUT_OF_MEMORY; - 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. + 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 | DT_OUT_OF_MEMORY; + m_posLookup = (dtMeshTile**)dtAlloc(sizeof(dtMeshTile*)*m_tileLutSize, DT_ALLOC_PERM); + if (!m_posLookup) + return DT_FAILURE | DT_OUT_OF_MEMORY; + 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. #ifndef DT_POLYREF64 - m_tileBits = dtIlog2(dtNextPow2((unsigned int)params->maxTiles)); - m_polyBits = dtIlog2(dtNextPow2((unsigned int)params->maxPolys)); - // Only allow 31 salt bits, since the salt mask is calculated using 32bit uint and it will overflow. - m_saltBits = dtMin((unsigned int)31, 32 - m_tileBits - m_polyBits); + m_tileBits = dtIlog2(dtNextPow2((unsigned int)params->maxTiles)); + m_polyBits = dtIlog2(dtNextPow2((unsigned int)params->maxPolys)); + // Only allow 31 salt bits, since the salt mask is calculated using 32bit uint and it will overflow. + m_saltBits = dtMin((unsigned int)31, 32 - m_tileBits - m_polyBits); - if (m_saltBits < 10) - return DT_FAILURE | DT_INVALID_PARAM; + if (m_saltBits < 10) + return DT_FAILURE | DT_INVALID_PARAM; #endif - - return DT_SUCCESS; + + 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 | DT_WRONG_MAGIC; - if (header->version != DT_NAVMESH_VERSION) - return DT_FAILURE | DT_WRONG_VERSION; - - 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 status = init(¶ms); - if (dtStatusFailed(status)) - return status; - - return addTile(data, dataSize, flags, 0, 0); + // Make sure the data is in right format. + dtMeshHeader* header = (dtMeshHeader*)data; + if (header->magic != DT_NAVMESH_MAGIC) + return DT_FAILURE | DT_WRONG_MAGIC; + if (header->version != DT_NAVMESH_VERSION) + return DT_FAILURE | DT_WRONG_VERSION; + + 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 status = init(¶ms); + if (dtStatusFailed(status)) + return status; + + return addTile(data, dataSize, flags, 0, 0); } /// @par @@ -291,599 +293,600 @@ dtStatus dtNavMesh::init(unsigned char* data, const int dataSize, const int flag /// initialization is performed. const dtNavMeshParams* dtNavMesh::getParams() const { - return &m_params; + return &m_params; } ////////////////////////////////////////////////////////////////////////////////////////// int dtNavMesh::findConnectingPolys(const float* va, const float* vb, - const dtMeshTile* tile, int side, - dtPolyRef* con, float* conarea, int maxcon) const + 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); - const float apos = getSlabCoord(va, 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; - - const float* vc = &tile->verts[poly->verts[j] * 3]; - const float* vd = &tile->verts[poly->verts[(j + 1) % nv] * 3]; - const float bpos = getSlabCoord(vc, side); - - // Segments are not close enough. - if (dtAbs(apos - bpos) > 0.01f) - continue; - - // Check if the segments touch. - 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; + if (!tile) return 0; + + float amin[2], amax[2]; + calcSlabEndPoints(va, vb, amin, amax, side); + const float apos = getSlabCoord(va, 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; + + const float* vc = &tile->verts[poly->verts[j]*3]; + const float* vd = &tile->verts[poly->verts[(j+1) % nv]*3]; + const float bpos = getSlabCoord(vc, side); + + // Segments are not close enough. + if (dtAbs(apos-bpos) > 0.01f) + continue; + + // Check if the segments touch. + 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::unconnectLinks(dtMeshTile* tile, dtMeshTile* target) { - if (!tile || !target) return; - - const unsigned int targetNum = decodePolyIdTile(getTileRef(target)); - - 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 (decodePolyIdTile(tile->links[j].ref) == targetNum) - { - // Remove 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; - } - } - } + if (!tile || !target) return; + + const unsigned int targetNum = decodePolyIdTile(getTileRef(target)); + + 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 (decodePolyIdTile(tile->links[j].ref) == targetNum) + { + // Remove 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. + 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 non-portal edges. - if ((poly->neis[j] & DT_EXT_LINK) == 0) - continue; - - const int dir = (int)(poly->neis[j] & 0xff); - if (side != -1 && dir != side) - 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(dir), 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)dir; - - link->next = poly->firstLink; - poly->firstLink = idx; - - // Compress portal limits to a byte value. - if (dir == 0 || dir == 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 (dir == 2 || dir == 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); - } - } - } - } - } + + const int nv = poly->vertCount; + for (int j = 0; j < nv; ++j) + { + // Skip non-portal edges. + if ((poly->neis[j] & DT_EXT_LINK) == 0) + continue; + + const int dir = (int)(poly->neis[j] & 0xff); + if (side != -1 && dir != side) + 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(dir), 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)dir; + + link->next = poly->firstLink; + poly->firstLink = idx; + + // Compress portal limits to a byte value. + if (dir == 0 || dir == 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)roundf(dtClamp(tmin, 0.0f, 1.0f)*255.0f); + link->bmax = (unsigned char)roundf(dtClamp(tmax, 0.0f, 1.0f)*255.0f); + } + else if (dir == 2 || dir == 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)roundf(dtClamp(tmin, 0.0f, 1.0f)*255.0f); + link->bmax = (unsigned char)roundf(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 = (side == -1) ? 0xff : (unsigned char)dtOppositeTile(side); - - for (int i = 0; i < target->header->offMeshConCount; ++i) - { - dtOffMeshConnection* targetCon = &target->offMeshCons[i]; - if (targetCon->side != oppositeSide) - continue; - - dtPoly* targetPoly = &target->polys[targetCon->poly]; - // Skip off-mesh connections which start location could not be connected at all. - if (targetPoly->firstLink == DT_NULL_LINK) - continue; - - const float halfExtents[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, halfExtents, 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 tidx = allocLink(tile); - if (tidx != DT_NULL_LINK) - { - const unsigned short landPolyIdx = (unsigned short)decodePolyIdPoly(ref); - dtPoly* landPoly = &tile->polys[landPolyIdx]; - dtLink* link = &tile->links[tidx]; - link->ref = getPolyRefBase(target) | (dtPolyRef)(targetCon->poly); - link->edge = 0xff; - link->side = (unsigned char)(side == -1 ? 0xff : side); - link->bmin = link->bmax = 0; - // Add to linked list. - link->next = landPoly->firstLink; - landPoly->firstLink = tidx; - } - } - } + if (!tile) return; + + // Connect off-mesh links. + // We are interested on links which land from target tile to this tile. + const unsigned char oppositeSide = (side == -1) ? 0xff : (unsigned char)dtOppositeTile(side); + + for (int i = 0; i < target->header->offMeshConCount; ++i) + { + dtOffMeshConnection* targetCon = &target->offMeshCons[i]; + if (targetCon->side != oppositeSide) + continue; + + dtPoly* targetPoly = &target->polys[targetCon->poly]; + // Skip off-mesh connections which start location could not be connected at all. + if (targetPoly->firstLink == DT_NULL_LINK) + continue; + + const float halfExtents[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, halfExtents, 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 tidx = allocLink(tile); + if (tidx != DT_NULL_LINK) + { + const unsigned short landPolyIdx = (unsigned short)decodePolyIdPoly(ref); + dtPoly* landPoly = &tile->polys[landPolyIdx]; + dtLink* link = &tile->links[tidx]; + link->ref = getPolyRefBase(target) | (dtPolyRef)(targetCon->poly); + link->edge = 0xff; + link->side = (unsigned char)(side == -1 ? 0xff : side); + link->bmin = link->bmax = 0; + // Add to linked list. + link->next = landPoly->firstLink; + landPoly->firstLink = tidx; + } + } + } + } 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; - } - } - } + 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::baseOffMeshLinks(dtMeshTile* tile) { - if (!tile) return; - - dtPolyRef base = getPolyRefBase(tile); - - // Base off-mesh connection start points. - for (int i = 0; i < tile->header->offMeshConCount; ++i) - { - dtOffMeshConnection* con = &tile->offMeshCons[i]; - dtPoly* poly = &tile->polys[con->poly]; - - const float halfExtents[3] = { con->rad, tile->header->walkableClimb, con->rad }; - - // Find polygon to connect to. - const float* p = &con->pos[0]; // First vertex - float nearestPt[3]; - dtPolyRef ref = findNearestPolyInTile(tile, p, halfExtents, nearestPt); - if (!ref) continue; - // findNearestPoly may return too optimistic results, further check to make sure. - if (dtSqr(nearestPt[0] - p[0]) + dtSqr(nearestPt[2] - p[2]) > dtSqr(con->rad)) - continue; - // Make sure the location is on current mesh. - float* v = &tile->verts[poly->verts[0] * 3]; - dtVcopy(v, nearestPt); - - // Link off-mesh connection to target poly. - unsigned int idx = allocLink(tile); - if (idx != DT_NULL_LINK) - { - dtLink* link = &tile->links[idx]; - link->ref = ref; - link->edge = (unsigned char)0; - link->side = 0xff; - link->bmin = link->bmax = 0; - // Add to linked list. - link->next = poly->firstLink; - poly->firstLink = idx; - } - - // Start end-point is always connect back to off-mesh connection. - unsigned int tidx = allocLink(tile); - if (tidx != DT_NULL_LINK) - { - const unsigned short landPolyIdx = (unsigned short)decodePolyIdPoly(ref); - dtPoly* landPoly = &tile->polys[landPolyIdx]; - dtLink* link = &tile->links[tidx]; - link->ref = base | (dtPolyRef)(con->poly); - link->edge = 0xff; - link->side = 0xff; - link->bmin = link->bmax = 0; - // Add to linked list. - link->next = landPoly->firstLink; - landPoly->firstLink = tidx; - } - } + if (!tile) return; + + dtPolyRef base = getPolyRefBase(tile); + + // Base off-mesh connection start points. + for (int i = 0; i < tile->header->offMeshConCount; ++i) + { + dtOffMeshConnection* con = &tile->offMeshCons[i]; + dtPoly* poly = &tile->polys[con->poly]; + + const float halfExtents[3] = { con->rad, tile->header->walkableClimb, con->rad }; + + // Find polygon to connect to. + const float* p = &con->pos[0]; // First vertex + float nearestPt[3]; + dtPolyRef ref = findNearestPolyInTile(tile, p, halfExtents, nearestPt); + if (!ref) continue; + // findNearestPoly may return too optimistic results, further check to make sure. + if (dtSqr(nearestPt[0]-p[0])+dtSqr(nearestPt[2]-p[2]) > dtSqr(con->rad)) + continue; + // Make sure the location is on current mesh. + float* v = &tile->verts[poly->verts[0]*3]; + dtVcopy(v, nearestPt); + + // Link off-mesh connection to target poly. + unsigned int idx = allocLink(tile); + if (idx != DT_NULL_LINK) + { + dtLink* link = &tile->links[idx]; + link->ref = ref; + link->edge = (unsigned char)0; + link->side = 0xff; + link->bmin = link->bmax = 0; + // Add to linked list. + link->next = poly->firstLink; + poly->firstLink = idx; + } + + // Start end-point is always connect back to off-mesh connection. + unsigned int tidx = allocLink(tile); + if (tidx != DT_NULL_LINK) + { + const unsigned short landPolyIdx = (unsigned short)decodePolyIdPoly(ref); + dtPoly* landPoly = &tile->polys[landPolyIdx]; + dtLink* link = &tile->links[tidx]; + link->ref = base | (dtPolyRef)(con->poly); + link->edge = 0xff; + link->side = 0xff; + link->bmin = link->bmax = 0; + // Add to linked list. + link->next = landPoly->firstLink; + landPoly->firstLink = tidx; + } + } } namespace { - template - void closestPointOnDetailEdges(const dtMeshTile* tile, const dtPoly* poly, const float* pos, float* closest) - { - const unsigned int ip = (unsigned int)(poly - tile->polys); - const dtPolyDetail* pd = &tile->detailMeshes[ip]; - - float dmin = FLT_MAX; - float tmin = 0; - const float* pmin = 0; - const float* pmax = 0; - - for (int i = 0; i < pd->triCount; i++) - { - const unsigned char* tris = &tile->detailTris[(pd->triBase + i) * 4]; - const int ANY_BOUNDARY_EDGE = - (DT_DETAIL_EDGE_BOUNDARY << 0) | - (DT_DETAIL_EDGE_BOUNDARY << 2) | - (DT_DETAIL_EDGE_BOUNDARY << 4); - if (onlyBoundary && (tris[3] & ANY_BOUNDARY_EDGE) == 0) - continue; - - const float* v[3]; - for (int j = 0; j < 3; ++j) - { - if (tris[j] < poly->vertCount) - v[j] = &tile->verts[poly->verts[tris[j]] * 3]; - else - v[j] = &tile->detailVerts[(pd->vertBase + (tris[j] - poly->vertCount)) * 3]; - } - - for (int k = 0, j = 2; k < 3; j = k++) - { - if ((dtGetDetailTriEdgeFlags(tris[3], j) & DT_DETAIL_EDGE_BOUNDARY) == 0 && - (onlyBoundary || tris[j] < tris[k])) - { - // Only looking at boundary edges and this is internal, or - // this is an inner edge that we will see again or have already seen. - continue; - } - - float t; - float d = dtDistancePtSegSqr2D(pos, v[j], v[k], t); - if (d < dmin) - { - dmin = d; - tmin = t; - pmin = v[j]; - pmax = v[k]; - } - } - } - - dtVlerp(closest, pmin, pmax, tmin); - } + template + void closestPointOnDetailEdges(const dtMeshTile* tile, const dtPoly* poly, const float* pos, float* closest) + { + const unsigned int ip = (unsigned int)(poly - tile->polys); + const dtPolyDetail* pd = &tile->detailMeshes[ip]; + + float dmin = FLT_MAX; + float tmin = 0; + const float* pmin = 0; + const float* pmax = 0; + + for (int i = 0; i < pd->triCount; i++) + { + const unsigned char* tris = &tile->detailTris[(pd->triBase + i) * 4]; + const int ANY_BOUNDARY_EDGE = + (DT_DETAIL_EDGE_BOUNDARY << 0) | + (DT_DETAIL_EDGE_BOUNDARY << 2) | + (DT_DETAIL_EDGE_BOUNDARY << 4); + if (onlyBoundary && (tris[3] & ANY_BOUNDARY_EDGE) == 0) + continue; + + const float* v[3]; + for (int j = 0; j < 3; ++j) + { + if (tris[j] < poly->vertCount) + v[j] = &tile->verts[poly->verts[tris[j]] * 3]; + else + v[j] = &tile->detailVerts[(pd->vertBase + (tris[j] - poly->vertCount)) * 3]; + } + + for (int k = 0, j = 2; k < 3; j = k++) + { + if ((dtGetDetailTriEdgeFlags(tris[3], j) & DT_DETAIL_EDGE_BOUNDARY) == 0 && + (onlyBoundary || tris[j] < tris[k])) + { + // Only looking at boundary edges and this is internal, or + // this is an inner edge that we will see again or have already seen. + continue; + } + + float t; + float d = dtDistancePtSegSqr2D(pos, v[j], v[k], t); + if (d < dmin) + { + dmin = d; + tmin = t; + pmin = v[j]; + pmax = v[k]; + } + } + } + + dtVlerp(closest, pmin, pmax, tmin); + } } bool dtNavMesh::getPolyHeight(const dtMeshTile* tile, const dtPoly* poly, const float* pos, float* height) const { - // Off-mesh connections do not have detail polys and getting height - // over them does not make sense. - if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) - return false; - - const unsigned int ip = (unsigned int)(poly - tile->polys); - const dtPolyDetail* pd = &tile->detailMeshes[ip]; - - float verts[DT_VERTS_PER_POLYGON * 3]; - const int nv = poly->vertCount; - for (int i = 0; i < nv; ++i) - dtVcopy(&verts[i * 3], &tile->verts[poly->verts[i] * 3]); - - if (!dtPointInPolygon(pos, verts, nv)) - return false; - - if (!height) - return true; - - // 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)) - { - *height = h; - return true; - } - } - - // If all triangle checks failed above (can happen with degenerate triangles - // or larger floating point values) the point is on an edge, so just select - // closest. This should almost never happen so the extra iteration here is - // ok. - float closest[3]; - closestPointOnDetailEdges(tile, poly, pos, closest); - *height = closest[1]; - return true; + // Off-mesh connections do not have detail polys and getting height + // over them does not make sense. + if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) + return false; + + const unsigned int ip = (unsigned int)(poly - tile->polys); + const dtPolyDetail* pd = &tile->detailMeshes[ip]; + + float verts[DT_VERTS_PER_POLYGON*3]; + const int nv = poly->vertCount; + for (int i = 0; i < nv; ++i) + dtVcopy(&verts[i*3], &tile->verts[poly->verts[i]*3]); + + if (!dtPointInPolygon(pos, verts, nv)) + return false; + + if (!height) + return true; + + // 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)) + { + *height = h; + return true; + } + } + + // If all triangle checks failed above (can happen with degenerate triangles + // or larger floating point values) the point is on an edge, so just select + // closest. This should almost never happen so the extra iteration here is + // ok. + float closest[3]; + closestPointOnDetailEdges(tile, poly, pos, closest); + *height = closest[1]; + return true; } void dtNavMesh::closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const { - const dtMeshTile* tile = 0; - const dtPoly* poly = 0; - getTileAndPolyByRefUnsafe(ref, &tile, &poly); - - dtVcopy(closest, pos); - if (getPolyHeight(tile, poly, pos, &closest[1])) - { - if (posOverPoly) - *posOverPoly = true; - return; - } - - if (posOverPoly) - *posOverPoly = false; - - // Off-mesh connections don't have detail polygons. - if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) - { - const float* v0 = &tile->verts[poly->verts[0] * 3]; - const float* v1 = &tile->verts[poly->verts[1] * 3]; - float t; - dtDistancePtSegSqr2D(pos, v0, v1, t); - dtVlerp(closest, v0, v1, t); - return; - } - - // Outside poly that is not an offmesh connection. - closestPointOnDetailEdges(tile, poly, pos, closest); + const dtMeshTile* tile = 0; + const dtPoly* poly = 0; + getTileAndPolyByRefUnsafe(ref, &tile, &poly); + + dtVcopy(closest, pos); + if (getPolyHeight(tile, poly, pos, &closest[1])) + { + if (posOverPoly) + *posOverPoly = true; + return; + } + + if (posOverPoly) + *posOverPoly = false; + + // Off-mesh connections don't have detail polygons. + if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) + { + const float* v0 = &tile->verts[poly->verts[0]*3]; + const float* v1 = &tile->verts[poly->verts[1]*3]; + float t; + dtDistancePtSegSqr2D(pos, v0, v1, t); + dtVlerp(closest, v0, v1, t); + return; + } + + // Outside poly that is not an offmesh connection. + closestPointOnDetailEdges(tile, poly, pos, closest); } dtPolyRef dtNavMesh::findNearestPolyInTile(const dtMeshTile* tile, - const float* center, const float* halfExtents, - float* nearestPt) const + const float* center, const float* halfExtents, + float* nearestPt) const { - float bmin[3], bmax[3]; - dtVsub(bmin, center, halfExtents); - dtVadd(bmax, center, halfExtents); - - // 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]; - float diff[3]; - bool posOverPoly = false; - float d; - closestPointOnPoly(ref, center, closestPtPoly, &posOverPoly); - - // If a point is directly over a polygon and closer than - // climb height, favor that instead of straight line nearest point. - dtVsub(diff, center, closestPtPoly); - if (posOverPoly) - { - d = dtAbs(diff[1]) - tile->header->walkableClimb; - d = d > 0 ? d * d : 0; - } - else - { - d = dtVlenSqr(diff); - } - - if (d < nearestDistanceSqr) - { - dtVcopy(nearestPt, closestPtPoly); - nearestDistanceSqr = d; - nearest = ref; - } - } - - return nearest; + float bmin[3], bmax[3]; + dtVsub(bmin, center, halfExtents); + dtVadd(bmax, center, halfExtents); + + // 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]; + float diff[3]; + bool posOverPoly = false; + float d; + closestPointOnPoly(ref, center, closestPtPoly, &posOverPoly); + + // If a point is directly over a polygon and closer than + // climb height, favor that instead of straight line nearest point. + dtVsub(diff, center, closestPtPoly); + if (posOverPoly) + { + d = dtAbs(diff[1]) - tile->header->walkableClimb; + d = d > 0 ? d*d : 0; + } + else + { + d = dtVlenSqr(diff); + } + + if (d < nearestDistanceSqr) + { + 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 + 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; - } + 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; + } } /// @par @@ -893,7 +896,7 @@ int dtNavMesh::queryPolygonsInTile(const dtMeshTile* tile, const float* qmin, co /// /// The lastRef parameter is used to restore a tile with the same tile /// reference it had previously used. In this case the #dtPolyRef's for the -/// tile will be restored to the same values they were before the tile was +/// tile will be restored to the same values they were before the tile was /// removed. /// /// The nav mesh assumes exclusive access to the data passed and will make @@ -903,204 +906,211 @@ int dtNavMesh::queryPolygonsInTile(const dtMeshTile* tile, const float* qmin, co /// /// @see dtCreateNavMeshData, #removeTile dtStatus dtNavMesh::addTile(unsigned char* data, int dataSize, int flags, - dtTileRef lastRef, dtTileRef* result) + 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 | DT_WRONG_MAGIC; - if (header->version != DT_NAVMESH_VERSION) - return DT_FAILURE | DT_WRONG_VERSION; - - // Make sure the location is free. - if (getTileAt(header->x, header->y, header->layer)) - return DT_FAILURE | DT_ALREADY_OCCUPIED; - - // 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 | DT_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 | DT_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 | DT_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 = dtGetThenAdvanceBufferPointer(d, vertsSize); - tile->polys = dtGetThenAdvanceBufferPointer(d, polysSize); - tile->links = dtGetThenAdvanceBufferPointer(d, linksSize); - tile->detailMeshes = dtGetThenAdvanceBufferPointer(d, detailMeshesSize); - tile->detailVerts = dtGetThenAdvanceBufferPointer(d, detailVertsSize); - tile->detailTris = dtGetThenAdvanceBufferPointer(d, detailTrisSize); - tile->bvTree = dtGetThenAdvanceBufferPointer(d, bvtreeSize); - tile->offMeshCons = dtGetThenAdvanceBufferPointer(d, offMeshLinksSize); - - // If there are no items in the bvtree, reset the tree pointer. - if (!bvtreeSize) - tile->bvTree = 0; - - // 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); - - // Base off-mesh connections to their starting polygons and connect connections inside the tile. - baseOffMeshLinks(tile); - connectExtOffMeshLinks(tile, tile, -1); - - // Create connections with neighbour tiles. - static const int MAX_NEIS = 32; - dtMeshTile* neis[MAX_NEIS]; - int nneis; - - // Connect with layers in current tile. - nneis = getTilesAt(header->x, header->y, neis, MAX_NEIS); - for (int j = 0; j < nneis; ++j) - { - if (neis[j] == tile) - continue; - - connectExtLinks(tile, neis[j], -1); - connectExtLinks(neis[j], tile, -1); - connectExtOffMeshLinks(tile, neis[j], -1); - connectExtOffMeshLinks(neis[j], tile, -1); - } - - // Connect with neighbour tiles. - for (int i = 0; i < 8; ++i) - { - nneis = getNeighbourTilesAt(header->x, header->y, i, neis, MAX_NEIS); - for (int j = 0; j < nneis; ++j) - { - connectExtLinks(tile, neis[j], i); - connectExtLinks(neis[j], tile, dtOppositeTile(i)); - connectExtOffMeshLinks(tile, neis[j], i); - connectExtOffMeshLinks(neis[j], tile, dtOppositeTile(i)); - } - } - - if (result) - *result = getTileRef(tile); - - return DT_SUCCESS; + // Make sure the data is in right format. + dtMeshHeader* header = (dtMeshHeader*)data; + if (header->magic != DT_NAVMESH_MAGIC) + return DT_FAILURE | DT_WRONG_MAGIC; + if (header->version != DT_NAVMESH_VERSION) + return DT_FAILURE | DT_WRONG_VERSION; + +#ifndef DT_POLYREF64 + // Do not allow adding more polygons than specified in the NavMesh's maxPolys constraint. + // Otherwise, the poly ID cannot be represented with the given number of bits. + if (m_polyBits < dtIlog2(dtNextPow2((unsigned int)header->polyCount))) + return DT_FAILURE | DT_INVALID_PARAM; +#endif + + // Make sure the location is free. + if (getTileAt(header->x, header->y, header->layer)) + return DT_FAILURE | DT_ALREADY_OCCUPIED; + + // 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 | DT_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 | DT_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 | DT_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 = dtGetThenAdvanceBufferPointer(d, vertsSize); + tile->polys = dtGetThenAdvanceBufferPointer(d, polysSize); + tile->links = dtGetThenAdvanceBufferPointer(d, linksSize); + tile->detailMeshes = dtGetThenAdvanceBufferPointer(d, detailMeshesSize); + tile->detailVerts = dtGetThenAdvanceBufferPointer(d, detailVertsSize); + tile->detailTris = dtGetThenAdvanceBufferPointer(d, detailTrisSize); + tile->bvTree = dtGetThenAdvanceBufferPointer(d, bvtreeSize); + tile->offMeshCons = dtGetThenAdvanceBufferPointer(d, offMeshLinksSize); + + // If there are no items in the bvtree, reset the tree pointer. + if (!bvtreeSize) + tile->bvTree = 0; + + // 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); + + // Base off-mesh connections to their starting polygons and connect connections inside the tile. + baseOffMeshLinks(tile); + connectExtOffMeshLinks(tile, tile, -1); + + // Create connections with neighbour tiles. + static const int MAX_NEIS = 32; + dtMeshTile* neis[MAX_NEIS]; + int nneis; + + // Connect with layers in current tile. + nneis = getTilesAt(header->x, header->y, neis, MAX_NEIS); + for (int j = 0; j < nneis; ++j) + { + if (neis[j] == tile) + continue; + + connectExtLinks(tile, neis[j], -1); + connectExtLinks(neis[j], tile, -1); + connectExtOffMeshLinks(tile, neis[j], -1); + connectExtOffMeshLinks(neis[j], tile, -1); + } + + // Connect with neighbour tiles. + for (int i = 0; i < 8; ++i) + { + nneis = getNeighbourTilesAt(header->x, header->y, i, neis, MAX_NEIS); + for (int j = 0; j < nneis; ++j) + { + connectExtLinks(tile, neis[j], i); + connectExtLinks(neis[j], tile, dtOppositeTile(i)); + connectExtOffMeshLinks(tile, neis[j], i); + connectExtOffMeshLinks(neis[j], tile, dtOppositeTile(i)); + } + } + + if (result) + *result = getTileRef(tile); + + return DT_SUCCESS; } const dtMeshTile* dtNavMesh::getTileAt(const int x, const int y, const int layer) 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 && - tile->header->layer == layer) - { - return tile; - } - tile = tile->next; - } - return 0; + // 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 && + tile->header->layer == layer) + { + return tile; + } + tile = tile->next; + } + return 0; } int dtNavMesh::getNeighbourTilesAt(const int x, const int y, const int side, dtMeshTile** tiles, const int maxTiles) const { - int nx = x, ny = y; - switch (side) - { - case 0: nx++; break; - case 1: nx++; ny++; break; - case 2: ny++; break; - case 3: nx--; ny++; break; - case 4: nx--; break; - case 5: nx--; ny--; break; - case 6: ny--; break; - case 7: nx++; ny--; break; - }; - - return getTilesAt(nx, ny, tiles, maxTiles); + int nx = x, ny = y; + switch (side) + { + case 0: nx++; break; + case 1: nx++; ny++; break; + case 2: ny++; break; + case 3: nx--; ny++; break; + case 4: nx--; break; + case 5: nx--; ny--; break; + case 6: ny--; break; + case 7: nx++; ny--; break; + }; + + return getTilesAt(nx, ny, tiles, maxTiles); } int dtNavMesh::getTilesAt(const int x, const int y, dtMeshTile** tiles, const int maxTiles) const { - int n = 0; - - // 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) - { - if (n < maxTiles) - tiles[n++] = tile; - } - tile = tile->next; - } - - return n; + int n = 0; + + // 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) + { + if (n < maxTiles) + tiles[n++] = tile; + } + tile = tile->next; + } + + return n; } /// @par @@ -1109,91 +1119,92 @@ int dtNavMesh::getTilesAt(const int x, const int y, dtMeshTile** tiles, const in /// entire result set. It will simply fill the array to capacity. int dtNavMesh::getTilesAt(const int x, const int y, dtMeshTile const** tiles, const int maxTiles) const { - int n = 0; - - // 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) - { - if (n < maxTiles) - tiles[n++] = tile; - } - tile = tile->next; - } - - return n; + int n = 0; + + // 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) + { + if (n < maxTiles) + tiles[n++] = tile; + } + tile = tile->next; + } + + return n; } + dtTileRef dtNavMesh::getTileRefAt(const int x, const int y, const int layer) 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 && - tile->header->layer == layer) - { - return getTileRef(tile); - } - tile = tile->next; - } - return 0; + // 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 && + tile->header->layer == layer) + { + 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; + 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; + return m_maxTiles; } dtMeshTile* dtNavMesh::getTile(int i) { - return &m_tiles[i]; + return &m_tiles[i]; } const dtMeshTile* dtNavMesh::getTile(int i) const { - return &m_tiles[i]; + 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); + *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 { - if (!ref) return DT_FAILURE; - unsigned int salt, it, ip; - decodePolyId(ref, salt, it, ip); - if (it >= (unsigned int)m_maxTiles) return DT_FAILURE | DT_INVALID_PARAM; - if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return DT_FAILURE | DT_INVALID_PARAM; - if (ip >= (unsigned int)m_tiles[it].header->polyCount) return DT_FAILURE | DT_INVALID_PARAM; - *tile = &m_tiles[it]; - *poly = &m_tiles[it].polys[ip]; - return DT_SUCCESS; + if (!ref) return DT_FAILURE; + unsigned int salt, it, ip; + decodePolyId(ref, salt, it, ip); + if (it >= (unsigned int)m_maxTiles) return DT_FAILURE | DT_INVALID_PARAM; + if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return DT_FAILURE | DT_INVALID_PARAM; + if (ip >= (unsigned int)m_tiles[it].header->polyCount) return DT_FAILURE | DT_INVALID_PARAM; + *tile = &m_tiles[it]; + *poly = &m_tiles[it].polys[ip]; + return DT_SUCCESS; } /// @par @@ -1203,21 +1214,21 @@ dtStatus dtNavMesh::getTileAndPolyByRef(const dtPolyRef ref, const dtMeshTile** /// it does not validate the reference. 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]; + 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 { - if (!ref) return false; - 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; + if (!ref) return false; + 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; } /// @par @@ -1228,104 +1239,104 @@ bool dtNavMesh::isValidPolyRef(dtPolyRef ref) const /// @see #addTile dtStatus dtNavMesh::removeTile(dtTileRef ref, unsigned char** data, int* dataSize) { - if (!ref) - return DT_FAILURE | DT_INVALID_PARAM; - unsigned int tileIndex = decodePolyIdTile((dtPolyRef)ref); - unsigned int tileSalt = decodePolyIdSalt((dtPolyRef)ref); - if ((int)tileIndex >= m_maxTiles) - return DT_FAILURE | DT_INVALID_PARAM; - dtMeshTile* tile = &m_tiles[tileIndex]; - if (tile->salt != tileSalt) - return DT_FAILURE | DT_INVALID_PARAM; - - // 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. - static const int MAX_NEIS = 32; - dtMeshTile* neis[MAX_NEIS]; - int nneis; - - // Disconnect from other layers in current tile. - nneis = getTilesAt(tile->header->x, tile->header->y, neis, MAX_NEIS); - for (int j = 0; j < nneis; ++j) - { - if (neis[j] == tile) continue; - unconnectLinks(neis[j], tile); - } - - // Disconnect from neighbour tiles. - for (int i = 0; i < 8; ++i) - { - nneis = getNeighbourTilesAt(tile->header->x, tile->header->y, i, neis, MAX_NEIS); - for (int j = 0; j < nneis; ++j) - unconnectLinks(neis[j], tile); - } - - // 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. + if (!ref) + return DT_FAILURE | DT_INVALID_PARAM; + unsigned int tileIndex = decodePolyIdTile((dtPolyRef)ref); + unsigned int tileSalt = decodePolyIdSalt((dtPolyRef)ref); + if ((int)tileIndex >= m_maxTiles) + return DT_FAILURE | DT_INVALID_PARAM; + dtMeshTile* tile = &m_tiles[tileIndex]; + if (tile->salt != tileSalt) + return DT_FAILURE | DT_INVALID_PARAM; + + // 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. + static const int MAX_NEIS = 32; + dtMeshTile* neis[MAX_NEIS]; + int nneis; + + // Disconnect from other layers in current tile. + nneis = getTilesAt(tile->header->x, tile->header->y, neis, MAX_NEIS); + for (int j = 0; j < nneis; ++j) + { + if (neis[j] == tile) continue; + unconnectLinks(neis[j], tile); + } + + // Disconnect from neighbour tiles. + for (int i = 0; i < 8; ++i) + { + nneis = getNeighbourTilesAt(tile->header->x, tile->header->y, i, neis, MAX_NEIS); + for (int j = 0; j < nneis; ++j) + unconnectLinks(neis[j], tile); + } + + // 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. #ifdef DT_POLYREF64 - tile->salt = (tile->salt + 1) & ((1 << DT_SALT_BITS) - 1); + tile->salt = (tile->salt+1) & ((1<salt = (tile->salt + 1) & ((1 << m_saltBits) - 1); + tile->salt = (tile->salt+1) & ((1<salt == 0) - tile->salt++; + if (tile->salt == 0) + tile->salt++; - // Add to free list. - tile->next = m_nextFree; - m_nextFree = tile; + // Add to free list. + tile->next = m_nextFree; + m_nextFree = tile; - return DT_SUCCESS; + 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); + if (!tile) return 0; + const unsigned int it = (unsigned int)(tile - m_tiles); + return (dtTileRef)encodePolyId(tile->salt, it, 0); } /// @par @@ -1338,37 +1349,37 @@ dtTileRef dtNavMesh::getTileRef(const dtMeshTile* tile) const /// { /// const dtPoly* p = &tile->polys[i]; /// const dtPolyRef ref = base | (dtPolyRef)i; -/// +/// /// // Use the reference to access the polygon data. /// } /// @endcode 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); + 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. + 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. + unsigned short flags; // Flags (see dtPolyFlags). + unsigned char area; // Area ID of the polygon. }; /// @see #storeTileState 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; + if (!tile) return 0; + const int headerSize = dtAlign4(sizeof(dtTileState)); + const int polyStateSize = dtAlign4(sizeof(dtPolyState) * tile->header->polyCount); + return headerSize + polyStateSize; } /// @par @@ -1378,29 +1389,29 @@ int dtNavMesh::getTileStateSize(const dtMeshTile* tile) const /// @see #getTileStateSize, #restoreTileState 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 | DT_BUFFER_TOO_SMALL; - - dtTileState* tileState = dtGetThenAdvanceBufferPointer(data, dtAlign4(sizeof(dtTileState))); - dtPolyState* polyStates = dtGetThenAdvanceBufferPointer(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; + // Make sure there is enough space to store the state. + const int sizeReq = getTileStateSize(tile); + if (maxDataSize < sizeReq) + return DT_FAILURE | DT_BUFFER_TOO_SMALL; + + dtTileState* tileState = dtGetThenAdvanceBufferPointer(data, dtAlign4(sizeof(dtTileState))); + dtPolyState* polyStates = dtGetThenAdvanceBufferPointer(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; } /// @par @@ -1410,168 +1421,171 @@ dtStatus dtNavMesh::storeTileState(const dtMeshTile* tile, unsigned char* data, /// @see #storeTileState 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 | DT_INVALID_PARAM; - - const dtTileState* tileState = dtGetThenAdvanceBufferPointer(data, dtAlign4(sizeof(dtTileState))); - const dtPolyState* polyStates = dtGetThenAdvanceBufferPointer(data, dtAlign4(sizeof(dtPolyState) * tile->header->polyCount)); - - // Check that the restore is possible. - if (tileState->magic != DT_NAVMESH_STATE_MAGIC) - return DT_FAILURE | DT_WRONG_MAGIC; - if (tileState->version != DT_NAVMESH_STATE_VERSION) - return DT_FAILURE | DT_WRONG_VERSION; - if (tileState->ref != getTileRef(tile)) - return DT_FAILURE | DT_INVALID_PARAM; - - // 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; + // Make sure there is enough space to store the state. + const int sizeReq = getTileStateSize(tile); + if (maxDataSize < sizeReq) + return DT_FAILURE | DT_INVALID_PARAM; + + const dtTileState* tileState = dtGetThenAdvanceBufferPointer(data, dtAlign4(sizeof(dtTileState))); + const dtPolyState* polyStates = dtGetThenAdvanceBufferPointer(data, dtAlign4(sizeof(dtPolyState) * tile->header->polyCount)); + + // Check that the restore is possible. + if (tileState->magic != DT_NAVMESH_STATE_MAGIC) + return DT_FAILURE | DT_WRONG_MAGIC; + if (tileState->version != DT_NAVMESH_STATE_VERSION) + return DT_FAILURE | DT_WRONG_VERSION; + if (tileState->ref != getTileRef(tile)) + return DT_FAILURE | DT_INVALID_PARAM; + + // 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; } /// @par /// -/// Off-mesh connections are stored in the navigation mesh as special 2-vertex -/// polygons with a single edge. At least one of the vertices is expected to be -/// inside a normal polygon. So an off-mesh connection is "entered" from a -/// normal polygon at one of its endpoints. This is the polygon identified by +/// Off-mesh connections are stored in the navigation mesh as special 2-vertex +/// polygons with a single edge. At least one of the vertices is expected to be +/// inside a normal polygon. So an off-mesh connection is "entered" from a +/// normal polygon at one of its endpoints. This is the polygon identified by /// the prevRef parameter. dtStatus dtNavMesh::getOffMeshConnectionPolyEndPoints(dtPolyRef prevRef, dtPolyRef polyRef, float* startPos, float* endPos) const { - unsigned int salt, it, ip; - - if (!polyRef) - return DT_FAILURE; - - // Get current polygon - decodePolyId(polyRef, salt, it, ip); - if (it >= (unsigned int)m_maxTiles) return DT_FAILURE | DT_INVALID_PARAM; - if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return DT_FAILURE | DT_INVALID_PARAM; - const dtMeshTile* tile = &m_tiles[it]; - if (ip >= (unsigned int)tile->header->polyCount) return DT_FAILURE | DT_INVALID_PARAM; - 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; + unsigned int salt, it, ip; + + if (!polyRef) + return DT_FAILURE; + + // Get current polygon + decodePolyId(polyRef, salt, it, ip); + if (it >= (unsigned int)m_maxTiles) return DT_FAILURE | DT_INVALID_PARAM; + if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return DT_FAILURE | DT_INVALID_PARAM; + const dtMeshTile* tile = &m_tiles[it]; + if (ip >= (unsigned int)tile->header->polyCount) return DT_FAILURE | DT_INVALID_PARAM; + 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; - - if (!ref) - return 0; - - // 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]; + unsigned int salt, it, ip; + + if (!ref) + return 0; + + // 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) { - if (!ref) return DT_FAILURE; - unsigned int salt, it, ip; - decodePolyId(ref, salt, it, ip); - if (it >= (unsigned int)m_maxTiles) return DT_FAILURE | DT_INVALID_PARAM; - if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return DT_FAILURE | DT_INVALID_PARAM; - dtMeshTile* tile = &m_tiles[it]; - if (ip >= (unsigned int)tile->header->polyCount) return DT_FAILURE | DT_INVALID_PARAM; - dtPoly* poly = &tile->polys[ip]; - - // Change flags. - poly->flags = flags; - - return DT_SUCCESS; + if (!ref) return DT_FAILURE; + unsigned int salt, it, ip; + decodePolyId(ref, salt, it, ip); + if (it >= (unsigned int)m_maxTiles) return DT_FAILURE | DT_INVALID_PARAM; + if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return DT_FAILURE | DT_INVALID_PARAM; + dtMeshTile* tile = &m_tiles[it]; + if (ip >= (unsigned int)tile->header->polyCount) return DT_FAILURE | DT_INVALID_PARAM; + dtPoly* poly = &tile->polys[ip]; + + // Change flags. + poly->flags = flags; + + return DT_SUCCESS; } dtStatus dtNavMesh::getPolyFlags(dtPolyRef ref, unsigned short* resultFlags) const { - if (!ref) return DT_FAILURE; - unsigned int salt, it, ip; - decodePolyId(ref, salt, it, ip); - if (it >= (unsigned int)m_maxTiles) return DT_FAILURE | DT_INVALID_PARAM; - if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return DT_FAILURE | DT_INVALID_PARAM; - const dtMeshTile* tile = &m_tiles[it]; - if (ip >= (unsigned int)tile->header->polyCount) return DT_FAILURE | DT_INVALID_PARAM; - const dtPoly* poly = &tile->polys[ip]; - - *resultFlags = poly->flags; - - return DT_SUCCESS; + if (!ref) return DT_FAILURE; + unsigned int salt, it, ip; + decodePolyId(ref, salt, it, ip); + if (it >= (unsigned int)m_maxTiles) return DT_FAILURE | DT_INVALID_PARAM; + if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return DT_FAILURE | DT_INVALID_PARAM; + const dtMeshTile* tile = &m_tiles[it]; + if (ip >= (unsigned int)tile->header->polyCount) return DT_FAILURE | DT_INVALID_PARAM; + const dtPoly* poly = &tile->polys[ip]; + + *resultFlags = poly->flags; + + return DT_SUCCESS; } dtStatus dtNavMesh::setPolyArea(dtPolyRef ref, unsigned char area) { - if (!ref) return DT_FAILURE; - unsigned int salt, it, ip; - decodePolyId(ref, salt, it, ip); - if (it >= (unsigned int)m_maxTiles) return DT_FAILURE | DT_INVALID_PARAM; - if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return DT_FAILURE | DT_INVALID_PARAM; - dtMeshTile* tile = &m_tiles[it]; - if (ip >= (unsigned int)tile->header->polyCount) return DT_FAILURE | DT_INVALID_PARAM; - dtPoly* poly = &tile->polys[ip]; - - poly->setArea(area); - - return DT_SUCCESS; + if (!ref) return DT_FAILURE; + unsigned int salt, it, ip; + decodePolyId(ref, salt, it, ip); + if (it >= (unsigned int)m_maxTiles) return DT_FAILURE | DT_INVALID_PARAM; + if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return DT_FAILURE | DT_INVALID_PARAM; + dtMeshTile* tile = &m_tiles[it]; + if (ip >= (unsigned int)tile->header->polyCount) return DT_FAILURE | DT_INVALID_PARAM; + dtPoly* poly = &tile->polys[ip]; + + poly->setArea(area); + + return DT_SUCCESS; } dtStatus dtNavMesh::getPolyArea(dtPolyRef ref, unsigned char* resultArea) const { - if (!ref) return DT_FAILURE; - unsigned int salt, it, ip; - decodePolyId(ref, salt, it, ip); - if (it >= (unsigned int)m_maxTiles) return DT_FAILURE | DT_INVALID_PARAM; - if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return DT_FAILURE | DT_INVALID_PARAM; - const dtMeshTile* tile = &m_tiles[it]; - if (ip >= (unsigned int)tile->header->polyCount) return DT_FAILURE | DT_INVALID_PARAM; - const dtPoly* poly = &tile->polys[ip]; - - *resultArea = poly->getArea(); - - return DT_SUCCESS; -} \ No newline at end of file + if (!ref) return DT_FAILURE; + unsigned int salt, it, ip; + decodePolyId(ref, salt, it, ip); + if (it >= (unsigned int)m_maxTiles) return DT_FAILURE | DT_INVALID_PARAM; + if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return DT_FAILURE | DT_INVALID_PARAM; + const dtMeshTile* tile = &m_tiles[it]; + if (ip >= (unsigned int)tile->header->polyCount) return DT_FAILURE | DT_INVALID_PARAM; + const dtPoly* poly = &tile->polys[ip]; + + *resultArea = poly->getArea(); + + return DT_SUCCESS; +} + diff --git a/recastnavigation/Detour/Source/DetourNavMeshBuilder.cpp b/recastnavigation/Detour/Source/DetourNavMeshBuilder.cpp index e3f02f7..e93a976 100644 --- a/recastnavigation/Detour/Source/DetourNavMeshBuilder.cpp +++ b/recastnavigation/Detour/Source/DetourNavMeshBuilder.cpp @@ -29,248 +29,249 @@ static unsigned short MESH_NULL_IDX = 0xffff; + struct BVItem { - unsigned short bmin[3]; - unsigned short bmax[3]; - int i; + 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; + 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; + 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; + 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) + 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]; - } + 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; - } - return axis; + int axis = 0; + unsigned short maxVal = x; + if (y > maxVal) + { + axis = 1; + maxVal = y; + } + if (z > maxVal) + { + axis = 2; + } + 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; - } + 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(dtNavMeshCreateParams* params, dtBVNode* nodes, int /*nnodes*/) { - // Build tree - float quantFactor = 1 / params->cs; - BVItem* items = (BVItem*)dtAlloc(sizeof(BVItem) * params->polyCount, DT_ALLOC_TEMP); - for (int i = 0; i < params->polyCount; i++) - { - BVItem& it = items[i]; - it.i = i; - // Calc polygon bounds. Use detail meshes if available. - if (params->detailMeshes) - { - int vb = (int)params->detailMeshes[i * 4 + 0]; - int ndv = (int)params->detailMeshes[i * 4 + 1]; - float bmin[3]; - float bmax[3]; - - const float* dv = ¶ms->detailVerts[vb * 3]; - dtVcopy(bmin, dv); - dtVcopy(bmax, dv); - - for (int j = 1; j < ndv; j++) - { - dtVmin(bmin, &dv[j * 3]); - dtVmax(bmax, &dv[j * 3]); - } - - // BV-tree uses cs for all dimensions - it.bmin[0] = (unsigned short)dtClamp((int)((bmin[0] - params->bmin[0]) * quantFactor), 0, 0xffff); - it.bmin[1] = (unsigned short)dtClamp((int)((bmin[1] - params->bmin[1]) * quantFactor), 0, 0xffff); - it.bmin[2] = (unsigned short)dtClamp((int)((bmin[2] - params->bmin[2]) * quantFactor), 0, 0xffff); - - it.bmax[0] = (unsigned short)dtClamp((int)((bmax[0] - params->bmin[0]) * quantFactor), 0, 0xffff); - it.bmax[1] = (unsigned short)dtClamp((int)((bmax[1] - params->bmin[1]) * quantFactor), 0, 0xffff); - it.bmax[2] = (unsigned short)dtClamp((int)((bmax[2] - params->bmin[2]) * quantFactor), 0, 0xffff); - } - else - { - const unsigned short* p = ¶ms->polys[i * params->nvp * 2]; - it.bmin[0] = it.bmax[0] = params->verts[p[0] * 3 + 0]; - it.bmin[1] = it.bmax[1] = params->verts[p[0] * 3 + 1]; - it.bmin[2] = it.bmax[2] = params->verts[p[0] * 3 + 2]; - - for (int j = 1; j < params->nvp; ++j) - { - if (p[j] == MESH_NULL_IDX) break; - unsigned short x = params->verts[p[j] * 3 + 0]; - unsigned short y = params->verts[p[j] * 3 + 1]; - unsigned short z = params->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)dtMathFloorf((float)it.bmin[1] * params->ch / params->cs); - it.bmax[1] = (unsigned short)dtMathCeilf((float)it.bmax[1] * params->ch / params->cs); - } - } - - int curNode = 0; - subdivide(items, params->polyCount, 0, params->polyCount, curNode, nodes); - - dtFree(items); - - return curNode; + // Build tree + float quantFactor = 1 / params->cs; + BVItem* items = (BVItem*)dtAlloc(sizeof(BVItem)*params->polyCount, DT_ALLOC_TEMP); + for (int i = 0; i < params->polyCount; i++) + { + BVItem& it = items[i]; + it.i = i; + // Calc polygon bounds. Use detail meshes if available. + if (params->detailMeshes) + { + int vb = (int)params->detailMeshes[i*4+0]; + int ndv = (int)params->detailMeshes[i*4+1]; + float bmin[3]; + float bmax[3]; + + const float* dv = ¶ms->detailVerts[vb*3]; + dtVcopy(bmin, dv); + dtVcopy(bmax, dv); + + for (int j = 1; j < ndv; j++) + { + dtVmin(bmin, &dv[j * 3]); + dtVmax(bmax, &dv[j * 3]); + } + + // BV-tree uses cs for all dimensions + it.bmin[0] = (unsigned short)dtClamp((int)((bmin[0] - params->bmin[0])*quantFactor), 0, 0xffff); + it.bmin[1] = (unsigned short)dtClamp((int)((bmin[1] - params->bmin[1])*quantFactor), 0, 0xffff); + it.bmin[2] = (unsigned short)dtClamp((int)((bmin[2] - params->bmin[2])*quantFactor), 0, 0xffff); + + it.bmax[0] = (unsigned short)dtClamp((int)((bmax[0] - params->bmin[0])*quantFactor), 0, 0xffff); + it.bmax[1] = (unsigned short)dtClamp((int)((bmax[1] - params->bmin[1])*quantFactor), 0, 0xffff); + it.bmax[2] = (unsigned short)dtClamp((int)((bmax[2] - params->bmin[2])*quantFactor), 0, 0xffff); + } + else + { + const unsigned short* p = ¶ms->polys[i*params->nvp * 2]; + it.bmin[0] = it.bmax[0] = params->verts[p[0] * 3 + 0]; + it.bmin[1] = it.bmax[1] = params->verts[p[0] * 3 + 1]; + it.bmin[2] = it.bmax[2] = params->verts[p[0] * 3 + 2]; + + for (int j = 1; j < params->nvp; ++j) + { + if (p[j] == MESH_NULL_IDX) break; + unsigned short x = params->verts[p[j] * 3 + 0]; + unsigned short y = params->verts[p[j] * 3 + 1]; + unsigned short z = params->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)dtMathFloorf((float)it.bmin[1] * params->ch / params->cs); + it.bmax[1] = (unsigned short)dtMathCeilf((float)it.bmax[1] * params->ch / params->cs); + } + } + + int curNode = 0; + subdivide(items, params->polyCount, 0, params->polyCount, 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; + 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. /// @par -/// +/// /// The output data array is allocated using the detour allocator (dtAlloc()). The method /// used to free the memory will be determined by how the tile is added to the navigation /// mesh. @@ -278,523 +279,524 @@ static unsigned char classifyOffMeshPoint(const float* pt, const float* bmin, co /// @see dtNavMesh, dtNavMesh::addTile() 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; - - 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; - - // Find tight heigh bounds, used for culling out off-mesh start locations. - float hmin = FLT_MAX; - float hmax = -FLT_MAX; - - if (params->detailVerts && params->detailVertsCount) - { - for (int i = 0; i < params->detailVertsCount; ++i) - { - const float h = params->detailVerts[i * 3 + 1]; - hmin = dtMin(hmin, h); - hmax = dtMax(hmax, h); - } - } - else - { - for (int i = 0; i < params->vertCount; ++i) - { - const unsigned short* iv = ¶ms->verts[i * 3]; - const float h = params->bmin[1] + iv[1] * params->ch; - hmin = dtMin(hmin, h); - hmax = dtMax(hmax, h); - } - } - hmin -= params->walkableClimb; - hmax += params->walkableClimb; - float bmin[3], bmax[3]; - dtVcopy(bmin, params->bmin); - dtVcopy(bmax, params->bmax); - bmin[1] = hmin; - bmax[1] = hmax; - - for (int i = 0; i < params->offMeshConCount; ++i) - { - const float* p0 = ¶ms->offMeshConVerts[(i * 2 + 0) * 3]; - const float* p1 = ¶ms->offMeshConVerts[(i * 2 + 1) * 3]; - offMeshConClass[i * 2 + 0] = classifyOffMeshPoint(p0, bmin, bmax); - offMeshConClass[i * 2 + 1] = classifyOffMeshPoint(p1, bmin, bmax); - - // Zero out off-mesh start positions which are not even potentially touching the mesh. - if (offMeshConClass[i * 2 + 0] == 0xff) - { - if (p0[1] < bmin[1] || p0[1] > bmax[1]) - offMeshConClass[i * 2 + 0] = 0; - } - - // Cound how many links should be allocated for off-mesh connections. - if (offMeshConClass[i * 2 + 0] == 0xff) - offMeshConLinkCount++; - if (offMeshConClass[i * 2 + 1] == 0xff) - offMeshConLinkCount++; - - if (offMeshConClass[i * 2 + 0] == 0xff) - storedOffMeshConCount++; - } - } - - // Off-mesh connectionss are stored as polygons, adjust values. - const int totPolyCount = params->polyCount + storedOffMeshConCount; - const int totVertCount = params->vertCount + storedOffMeshConCount * 2; - - // Find portal edges which are at tile borders. - int edgeCount = 0; - int portalCount = 0; - for (int i = 0; i < params->polyCount; ++i) - { - const unsigned short* p = ¶ms->polys[i * 2 * nvp]; - for (int j = 0; j < nvp; ++j) - { - if (p[j] == MESH_NULL_IDX) break; - edgeCount++; - - if (p[nvp + j] & 0x8000) - { - unsigned short dir = p[nvp + j] & 0xf; - if (dir != 0xf) - portalCount++; - } - } - } - - const int maxLinkCount = edgeCount + portalCount * 2 + offMeshConLinkCount * 2; - - // Find unique detail vertices. - int uniqueDetailVertCount = 0; - int detailTriCount = 0; - if (params->detailMeshes) - { - // Has detail mesh, count unique detail vertex count and use input detail tri count. - detailTriCount = params->detailTriCount; - for (int i = 0; i < params->polyCount; ++i) - { - const unsigned short* p = ¶ms->polys[i * nvp * 2]; - int ndv = params->detailMeshes[i * 4 + 1]; - int nv = 0; - for (int j = 0; j < nvp; ++j) - { - if (p[j] == MESH_NULL_IDX) break; - nv++; - } - ndv -= nv; - uniqueDetailVertCount += ndv; - } - } - else - { - // No input detail mesh, build detail mesh from nav polys. - uniqueDetailVertCount = 0; // No extra detail verts. - detailTriCount = 0; - for (int i = 0; i < params->polyCount; ++i) - { - const unsigned short* p = ¶ms->polys[i * nvp * 2]; - int nv = 0; - for (int j = 0; j < nvp; ++j) - { - if (p[j] == MESH_NULL_IDX) break; - nv++; - } - detailTriCount += nv - 2; - } - } - - // Calculate data size - const int headerSize = dtAlign4(sizeof(dtMeshHeader)); - const int vertsSize = dtAlign4(sizeof(float) * 3 * totVertCount); - const int polysSize = dtAlign4(sizeof(dtPoly) * totPolyCount); - const int linksSize = dtAlign4(sizeof(dtLink) * maxLinkCount); - const int detailMeshesSize = dtAlign4(sizeof(dtPolyDetail) * params->polyCount); - const int detailVertsSize = dtAlign4(sizeof(float) * 3 * uniqueDetailVertCount); - const int detailTrisSize = dtAlign4(sizeof(unsigned char) * 4 * detailTriCount); - const int bvTreeSize = params->buildBvTree ? dtAlign4(sizeof(dtBVNode) * params->polyCount * 2) : 0; - 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 = dtGetThenAdvanceBufferPointer(d, headerSize); - float* navVerts = dtGetThenAdvanceBufferPointer(d, vertsSize); - dtPoly* navPolys = dtGetThenAdvanceBufferPointer(d, polysSize); - d += linksSize; // Ignore links; just leave enough space for them. They'll be created on load. - dtPolyDetail* navDMeshes = dtGetThenAdvanceBufferPointer(d, detailMeshesSize); - float* navDVerts = dtGetThenAdvanceBufferPointer(d, detailVertsSize); - unsigned char* navDTris = dtGetThenAdvanceBufferPointer(d, detailTrisSize); - dtBVNode* navBvtree = dtGetThenAdvanceBufferPointer(d, bvTreeSize); - dtOffMeshConnection* offMeshCons = dtGetThenAdvanceBufferPointer(d, offMeshConsSize); - - // Store header - header->magic = DT_NAVMESH_MAGIC; - header->version = DT_NAVMESH_VERSION; - header->x = params->tileX; - header->y = params->tileY; - header->layer = params->tileLayer; - 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->buildBvTree ? params->polyCount * 2 : 0; - - const int offMeshVertsBase = params->vertCount; - const int offMeshPolyBase = params->polyCount; - - // Store vertices - // Mesh vertices - for (int i = 0; i < params->vertCount; ++i) - { - const unsigned short* iv = ¶ms->verts[i * 3]; - float* v = &navVerts[i * 3]; - v[0] = params->bmin[0] + iv[0] * params->cs; - v[1] = params->bmin[1] + iv[1] * params->ch; - v[2] = params->bmin[2] + iv[2] * params->cs; - } - // Off-mesh link vertices. - int n = 0; - for (int i = 0; i < params->offMeshConCount; ++i) - { - // Only store connections which start from this tile. - if (offMeshConClass[i * 2 + 0] == 0xff) - { - const float* linkv = ¶ms->offMeshConVerts[i * 2 * 3]; - float* v = &navVerts[(offMeshVertsBase + n * 2) * 3]; - dtVcopy(&v[0], &linkv[0]); - dtVcopy(&v[3], &linkv[3]); - n++; - } - } - - // Store polygons - // Mesh polys - const unsigned short* src = params->polys; - for (int i = 0; i < params->polyCount; ++i) - { - dtPoly* p = &navPolys[i]; - p->vertCount = 0; - p->flags = params->polyFlags[i]; - p->setArea(params->polyAreas[i]); - p->setType(DT_POLYTYPE_GROUND); - for (int j = 0; j < nvp; ++j) - { - if (src[j] == MESH_NULL_IDX) break; - p->verts[j] = src[j]; - if (src[nvp + j] & 0x8000) - { - // Border or portal edge. - unsigned short dir = src[nvp + j] & 0xf; - if (dir == 0xf) // Border - p->neis[j] = 0; - else if (dir == 0) // Portal x- - p->neis[j] = DT_EXT_LINK | 4; - else if (dir == 1) // Portal z+ - p->neis[j] = DT_EXT_LINK | 2; - else if (dir == 2) // Portal x+ - p->neis[j] = DT_EXT_LINK | 0; - else if (dir == 3) // Portal z- - p->neis[j] = DT_EXT_LINK | 6; - } - else - { - // Normal connection - p->neis[j] = src[nvp + j] + 1; - } - - 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 detail meshes and vertices. - // The nav polygon vertices are stored as the first vertices on each mesh. - // We compress the mesh data by skipping them and using the navmesh coordinates. - if (params->detailMeshes) - { - unsigned short vbase = 0; - for (int i = 0; i < params->polyCount; ++i) - { - dtPolyDetail& dtl = navDMeshes[i]; - const int vb = (int)params->detailMeshes[i * 4 + 0]; - const int ndv = (int)params->detailMeshes[i * 4 + 1]; - const int nv = navPolys[i].vertCount; - dtl.vertBase = (unsigned int)vbase; - dtl.vertCount = (unsigned char)(ndv - nv); - dtl.triBase = (unsigned int)params->detailMeshes[i * 4 + 2]; - dtl.triCount = (unsigned char)params->detailMeshes[i * 4 + 3]; - // Copy vertices except the first 'nv' verts which are equal to nav poly verts. - if (ndv - nv) - { - memcpy(&navDVerts[vbase * 3], ¶ms->detailVerts[(vb + nv) * 3], sizeof(float) * 3 * (ndv - nv)); - vbase += (unsigned short)(ndv - nv); - } - } - // Store triangles. - memcpy(navDTris, params->detailTris, sizeof(unsigned char) * 4 * params->detailTriCount); - } - else - { - // Create dummy detail mesh by triangulating polys. - int tbase = 0; - for (int i = 0; i < params->polyCount; ++i) - { - dtPolyDetail& dtl = navDMeshes[i]; - const int nv = navPolys[i].vertCount; - dtl.vertBase = 0; - dtl.vertCount = 0; - dtl.triBase = (unsigned int)tbase; - dtl.triCount = (unsigned char)(nv - 2); - // Triangulate polygon (local indices). - for (int j = 2; j < nv; ++j) - { - unsigned char* t = &navDTris[tbase * 4]; - t[0] = 0; - t[1] = (unsigned char)(j - 1); - t[2] = (unsigned char)j; - // Bit for each edge that belongs to poly boundary. - t[3] = (1 << 2); - if (j == 2) t[3] |= (1 << 0); - if (j == nv - 1) t[3] |= (1 << 4); - tbase++; - } - } - } - - // Store and create BVtree. - if (params->buildBvTree) - { - createBVTree(params, navBvtree, 2 * params->polyCount); - } - - // Store Off-Mesh connections. - n = 0; - for (int i = 0; i < params->offMeshConCount; ++i) - { - // Only store connections which start from this tile. - if (offMeshConClass[i * 2 + 0] == 0xff) - { - dtOffMeshConnection* con = &offMeshCons[n]; - con->poly = (unsigned short)(offMeshPolyBase + n); - // Copy connection end-points. - const float* endPts = ¶ms->offMeshConVerts[i * 2 * 3]; - dtVcopy(&con->pos[0], &endPts[0]); - dtVcopy(&con->pos[3], &endPts[3]); - con->rad = params->offMeshConRad[i]; - con->flags = params->offMeshConDir[i] ? DT_OFFMESH_CON_BIDIR : 0; - con->side = offMeshConClass[i * 2 + 1]; - if (params->offMeshConUserID) - con->userId = params->offMeshConUserID[i]; - n++; - } - } - - dtFree(offMeshConClass); - - *outData = data; - *outDataSize = dataSize; - - return true; + 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; + + 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; + + // Find tight heigh bounds, used for culling out off-mesh start locations. + float hmin = FLT_MAX; + float hmax = -FLT_MAX; + + if (params->detailVerts && params->detailVertsCount) + { + for (int i = 0; i < params->detailVertsCount; ++i) + { + const float h = params->detailVerts[i*3+1]; + hmin = dtMin(hmin,h); + hmax = dtMax(hmax,h); + } + } + else + { + for (int i = 0; i < params->vertCount; ++i) + { + const unsigned short* iv = ¶ms->verts[i*3]; + const float h = params->bmin[1] + iv[1] * params->ch; + hmin = dtMin(hmin,h); + hmax = dtMax(hmax,h); + } + } + hmin -= params->walkableClimb; + hmax += params->walkableClimb; + float bmin[3], bmax[3]; + dtVcopy(bmin, params->bmin); + dtVcopy(bmax, params->bmax); + bmin[1] = hmin; + bmax[1] = hmax; + + for (int i = 0; i < params->offMeshConCount; ++i) + { + const float* p0 = ¶ms->offMeshConVerts[(i*2+0)*3]; + const float* p1 = ¶ms->offMeshConVerts[(i*2+1)*3]; + offMeshConClass[i*2+0] = classifyOffMeshPoint(p0, bmin, bmax); + offMeshConClass[i*2+1] = classifyOffMeshPoint(p1, bmin, bmax); + + // Zero out off-mesh start positions which are not even potentially touching the mesh. + if (offMeshConClass[i*2+0] == 0xff) + { + if (p0[1] < bmin[1] || p0[1] > bmax[1]) + offMeshConClass[i*2+0] = 0; + } + + // Cound how many links should be allocated for off-mesh connections. + if (offMeshConClass[i*2+0] == 0xff) + offMeshConLinkCount++; + if (offMeshConClass[i*2+1] == 0xff) + offMeshConLinkCount++; + + if (offMeshConClass[i*2+0] == 0xff) + storedOffMeshConCount++; + } + } + + // Off-mesh connectionss are stored as polygons, adjust values. + const int totPolyCount = params->polyCount + storedOffMeshConCount; + const int totVertCount = params->vertCount + storedOffMeshConCount*2; + + // Find portal edges which are at tile borders. + int edgeCount = 0; + int portalCount = 0; + for (int i = 0; i < params->polyCount; ++i) + { + const unsigned short* p = ¶ms->polys[i*2*nvp]; + for (int j = 0; j < nvp; ++j) + { + if (p[j] == MESH_NULL_IDX) break; + edgeCount++; + + if (p[nvp+j] & 0x8000) + { + unsigned short dir = p[nvp+j] & 0xf; + if (dir != 0xf) + portalCount++; + } + } + } + + const int maxLinkCount = edgeCount + portalCount*2 + offMeshConLinkCount*2; + + // Find unique detail vertices. + int uniqueDetailVertCount = 0; + int detailTriCount = 0; + if (params->detailMeshes) + { + // Has detail mesh, count unique detail vertex count and use input detail tri count. + detailTriCount = params->detailTriCount; + for (int i = 0; i < params->polyCount; ++i) + { + const unsigned short* p = ¶ms->polys[i*nvp*2]; + int ndv = params->detailMeshes[i*4+1]; + int nv = 0; + for (int j = 0; j < nvp; ++j) + { + if (p[j] == MESH_NULL_IDX) break; + nv++; + } + ndv -= nv; + uniqueDetailVertCount += ndv; + } + } + else + { + // No input detail mesh, build detail mesh from nav polys. + uniqueDetailVertCount = 0; // No extra detail verts. + detailTriCount = 0; + for (int i = 0; i < params->polyCount; ++i) + { + const unsigned short* p = ¶ms->polys[i*nvp*2]; + int nv = 0; + for (int j = 0; j < nvp; ++j) + { + if (p[j] == MESH_NULL_IDX) break; + nv++; + } + detailTriCount += nv-2; + } + } + + // Calculate data size + const int headerSize = dtAlign4(sizeof(dtMeshHeader)); + const int vertsSize = dtAlign4(sizeof(float)*3*totVertCount); + const int polysSize = dtAlign4(sizeof(dtPoly)*totPolyCount); + const int linksSize = dtAlign4(sizeof(dtLink)*maxLinkCount); + const int detailMeshesSize = dtAlign4(sizeof(dtPolyDetail)*params->polyCount); + const int detailVertsSize = dtAlign4(sizeof(float)*3*uniqueDetailVertCount); + const int detailTrisSize = dtAlign4(sizeof(unsigned char)*4*detailTriCount); + const int bvTreeSize = params->buildBvTree ? dtAlign4(sizeof(dtBVNode)*params->polyCount*2) : 0; + 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 = dtGetThenAdvanceBufferPointer(d, headerSize); + float* navVerts = dtGetThenAdvanceBufferPointer(d, vertsSize); + dtPoly* navPolys = dtGetThenAdvanceBufferPointer(d, polysSize); + d += linksSize; // Ignore links; just leave enough space for them. They'll be created on load. + dtPolyDetail* navDMeshes = dtGetThenAdvanceBufferPointer(d, detailMeshesSize); + float* navDVerts = dtGetThenAdvanceBufferPointer(d, detailVertsSize); + unsigned char* navDTris = dtGetThenAdvanceBufferPointer(d, detailTrisSize); + dtBVNode* navBvtree = dtGetThenAdvanceBufferPointer(d, bvTreeSize); + dtOffMeshConnection* offMeshCons = dtGetThenAdvanceBufferPointer(d, offMeshConsSize); + + + // Store header + header->magic = DT_NAVMESH_MAGIC; + header->version = DT_NAVMESH_VERSION; + header->x = params->tileX; + header->y = params->tileY; + header->layer = params->tileLayer; + 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->buildBvTree ? params->polyCount*2 : 0; + + const int offMeshVertsBase = params->vertCount; + const int offMeshPolyBase = params->polyCount; + + // Store vertices + // Mesh vertices + for (int i = 0; i < params->vertCount; ++i) + { + const unsigned short* iv = ¶ms->verts[i*3]; + float* v = &navVerts[i*3]; + v[0] = params->bmin[0] + iv[0] * params->cs; + v[1] = params->bmin[1] + iv[1] * params->ch; + v[2] = params->bmin[2] + iv[2] * params->cs; + } + // Off-mesh link vertices. + int n = 0; + for (int i = 0; i < params->offMeshConCount; ++i) + { + // Only store connections which start from this tile. + if (offMeshConClass[i*2+0] == 0xff) + { + const float* linkv = ¶ms->offMeshConVerts[i*2*3]; + float* v = &navVerts[(offMeshVertsBase + n*2)*3]; + dtVcopy(&v[0], &linkv[0]); + dtVcopy(&v[3], &linkv[3]); + n++; + } + } + + // Store polygons + // Mesh polys + const unsigned short* src = params->polys; + for (int i = 0; i < params->polyCount; ++i) + { + dtPoly* p = &navPolys[i]; + p->vertCount = 0; + p->flags = params->polyFlags[i]; + p->setArea(params->polyAreas[i]); + p->setType(DT_POLYTYPE_GROUND); + for (int j = 0; j < nvp; ++j) + { + if (src[j] == MESH_NULL_IDX) break; + p->verts[j] = src[j]; + if (src[nvp+j] & 0x8000) + { + // Border or portal edge. + unsigned short dir = src[nvp+j] & 0xf; + if (dir == 0xf) // Border + p->neis[j] = 0; + else if (dir == 0) // Portal x- + p->neis[j] = DT_EXT_LINK | 4; + else if (dir == 1) // Portal z+ + p->neis[j] = DT_EXT_LINK | 2; + else if (dir == 2) // Portal x+ + p->neis[j] = DT_EXT_LINK | 0; + else if (dir == 3) // Portal z- + p->neis[j] = DT_EXT_LINK | 6; + } + else + { + // Normal connection + p->neis[j] = src[nvp+j]+1; + } + + 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 detail meshes and vertices. + // The nav polygon vertices are stored as the first vertices on each mesh. + // We compress the mesh data by skipping them and using the navmesh coordinates. + if (params->detailMeshes) + { + unsigned short vbase = 0; + for (int i = 0; i < params->polyCount; ++i) + { + dtPolyDetail& dtl = navDMeshes[i]; + const int vb = (int)params->detailMeshes[i*4+0]; + const int ndv = (int)params->detailMeshes[i*4+1]; + const int nv = navPolys[i].vertCount; + dtl.vertBase = (unsigned int)vbase; + dtl.vertCount = (unsigned char)(ndv-nv); + dtl.triBase = (unsigned int)params->detailMeshes[i*4+2]; + dtl.triCount = (unsigned char)params->detailMeshes[i*4+3]; + // Copy vertices except the first 'nv' verts which are equal to nav poly verts. + if (ndv-nv) + { + memcpy(&navDVerts[vbase*3], ¶ms->detailVerts[(vb+nv)*3], sizeof(float)*3*(ndv-nv)); + vbase += (unsigned short)(ndv-nv); + } + } + // Store triangles. + memcpy(navDTris, params->detailTris, sizeof(unsigned char)*4*params->detailTriCount); + } + else + { + // Create dummy detail mesh by triangulating polys. + int tbase = 0; + for (int i = 0; i < params->polyCount; ++i) + { + dtPolyDetail& dtl = navDMeshes[i]; + const int nv = navPolys[i].vertCount; + dtl.vertBase = 0; + dtl.vertCount = 0; + dtl.triBase = (unsigned int)tbase; + dtl.triCount = (unsigned char)(nv-2); + // Triangulate polygon (local indices). + for (int j = 2; j < nv; ++j) + { + unsigned char* t = &navDTris[tbase*4]; + t[0] = 0; + t[1] = (unsigned char)(j-1); + t[2] = (unsigned char)j; + // Bit for each edge that belongs to poly boundary. + t[3] = (1<<2); + if (j == 2) t[3] |= (1<<0); + if (j == nv-1) t[3] |= (1<<4); + tbase++; + } + } + } + + // Store and create BVtree. + if (params->buildBvTree) + { + createBVTree(params, navBvtree, 2*params->polyCount); + } + + // Store Off-Mesh connections. + n = 0; + for (int i = 0; i < params->offMeshConCount; ++i) + { + // Only store connections which start from this tile. + if (offMeshConClass[i*2+0] == 0xff) + { + dtOffMeshConnection* con = &offMeshCons[n]; + con->poly = (unsigned short)(offMeshPolyBase + n); + // Copy connection end-points. + const float* endPts = ¶ms->offMeshConVerts[i*2*3]; + dtVcopy(&con->pos[0], &endPts[0]); + dtVcopy(&con->pos[3], &endPts[3]); + con->rad = params->offMeshConRad[i]; + con->flags = params->offMeshConDir[i] ? DT_OFFMESH_CON_BIDIR : 0; + con->side = offMeshConClass[i*2+1]; + if (params->offMeshConUserID) + con->userId = params->offMeshConUserID[i]; + n++; + } + } + + dtFree(offMeshConClass); + + *outData = data; + *outDataSize = dataSize; + + return true; } bool dtNavMeshHeaderSwapEndian(unsigned char* data, const int /*dataSize*/) { - dtMeshHeader* header = (dtMeshHeader*)data; - - int swappedMagic = DT_NAVMESH_MAGIC; - int swappedVersion = DT_NAVMESH_VERSION; - dtSwapEndian(&swappedMagic); - dtSwapEndian(&swappedVersion); - - if ((header->magic != DT_NAVMESH_MAGIC || header->version != DT_NAVMESH_VERSION) && - (header->magic != swappedMagic || header->version != swappedVersion)) - { - return false; - } - - dtSwapEndian(&header->magic); - dtSwapEndian(&header->version); - dtSwapEndian(&header->x); - dtSwapEndian(&header->y); - dtSwapEndian(&header->layer); - dtSwapEndian(&header->userId); - dtSwapEndian(&header->polyCount); - dtSwapEndian(&header->vertCount); - dtSwapEndian(&header->maxLinkCount); - dtSwapEndian(&header->detailMeshCount); - dtSwapEndian(&header->detailVertCount); - dtSwapEndian(&header->detailTriCount); - dtSwapEndian(&header->bvNodeCount); - dtSwapEndian(&header->offMeshConCount); - dtSwapEndian(&header->offMeshBase); - dtSwapEndian(&header->walkableHeight); - dtSwapEndian(&header->walkableRadius); - dtSwapEndian(&header->walkableClimb); - dtSwapEndian(&header->bmin[0]); - dtSwapEndian(&header->bmin[1]); - dtSwapEndian(&header->bmin[2]); - dtSwapEndian(&header->bmax[0]); - dtSwapEndian(&header->bmax[1]); - dtSwapEndian(&header->bmax[2]); - dtSwapEndian(&header->bvQuantFactor); - - // Freelist index and pointers are updated when tile is added, no need to swap. - - return true; + dtMeshHeader* header = (dtMeshHeader*)data; + + int swappedMagic = DT_NAVMESH_MAGIC; + int swappedVersion = DT_NAVMESH_VERSION; + dtSwapEndian(&swappedMagic); + dtSwapEndian(&swappedVersion); + + if ((header->magic != DT_NAVMESH_MAGIC || header->version != DT_NAVMESH_VERSION) && + (header->magic != swappedMagic || header->version != swappedVersion)) + { + return false; + } + + dtSwapEndian(&header->magic); + dtSwapEndian(&header->version); + dtSwapEndian(&header->x); + dtSwapEndian(&header->y); + dtSwapEndian(&header->layer); + dtSwapEndian(&header->userId); + dtSwapEndian(&header->polyCount); + dtSwapEndian(&header->vertCount); + dtSwapEndian(&header->maxLinkCount); + dtSwapEndian(&header->detailMeshCount); + dtSwapEndian(&header->detailVertCount); + dtSwapEndian(&header->detailTriCount); + dtSwapEndian(&header->bvNodeCount); + dtSwapEndian(&header->offMeshConCount); + dtSwapEndian(&header->offMeshBase); + dtSwapEndian(&header->walkableHeight); + dtSwapEndian(&header->walkableRadius); + dtSwapEndian(&header->walkableClimb); + dtSwapEndian(&header->bmin[0]); + dtSwapEndian(&header->bmin[1]); + dtSwapEndian(&header->bmin[2]); + dtSwapEndian(&header->bmax[0]); + dtSwapEndian(&header->bmax[1]); + dtSwapEndian(&header->bmax[2]); + dtSwapEndian(&header->bvQuantFactor); + + // Freelist index and pointers are updated when tile is added, no need to swap. + + return true; } /// @par /// -/// @warning This function assumes that the header is in the correct endianess already. -/// Call #dtNavMeshHeaderSwapEndian() first on the data if the data is expected to be in wrong endianess -/// to start with. Call #dtNavMeshHeaderSwapEndian() after the data has been swapped if converting from +/// @warning This function assumes that the header is in the correct endianess already. +/// Call #dtNavMeshHeaderSwapEndian() first on the data if the data is expected to be in wrong endianess +/// to start with. Call #dtNavMeshHeaderSwapEndian() after the data has been swapped if converting from /// native to foreign endianess. 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 = dtGetThenAdvanceBufferPointer(d, vertsSize); - dtPoly* polys = dtGetThenAdvanceBufferPointer(d, polysSize); - d += linksSize; // Ignore links; they technically should be endian-swapped but all their data is overwritten on load anyway. - //dtLink* links = dtGetThenAdvanceBufferPointer(d, linksSize); - dtPolyDetail* detailMeshes = dtGetThenAdvanceBufferPointer(d, detailMeshesSize); - float* detailVerts = dtGetThenAdvanceBufferPointer(d, detailVertsSize); - d += detailTrisSize; // Ignore detail tris; single bytes can't be endian-swapped. - //unsigned char* detailTris = dtGetThenAdvanceBufferPointer(d, detailTrisSize); - dtBVNode* bvTree = dtGetThenAdvanceBufferPointer(d, bvtreeSize); - dtOffMeshConnection* offMeshCons = dtGetThenAdvanceBufferPointer(d, offMeshLinksSize); - - // Vertices - for (int i = 0; i < header->vertCount * 3; ++i) - { - dtSwapEndian(&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) - { - dtSwapEndian(&p->verts[j]); - dtSwapEndian(&p->neis[j]); - } - dtSwapEndian(&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]; - dtSwapEndian(&pd->vertBase); - dtSwapEndian(&pd->triBase); - } - - // Detail verts - for (int i = 0; i < header->detailVertCount * 3; ++i) - { - dtSwapEndian(&detailVerts[i]); - } - - // BV-tree - for (int i = 0; i < header->bvNodeCount; ++i) - { - dtBVNode* node = &bvTree[i]; - for (int j = 0; j < 3; ++j) - { - dtSwapEndian(&node->bmin[j]); - dtSwapEndian(&node->bmax[j]); - } - dtSwapEndian(&node->i); - } - - // Off-mesh Connections. - for (int i = 0; i < header->offMeshConCount; ++i) - { - dtOffMeshConnection* con = &offMeshCons[i]; - for (int j = 0; j < 6; ++j) - dtSwapEndian(&con->pos[j]); - dtSwapEndian(&con->rad); - dtSwapEndian(&con->poly); - } - - return true; -} \ No newline at end of file + // 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 = dtGetThenAdvanceBufferPointer(d, vertsSize); + dtPoly* polys = dtGetThenAdvanceBufferPointer(d, polysSize); + d += linksSize; // Ignore links; they technically should be endian-swapped but all their data is overwritten on load anyway. + //dtLink* links = dtGetThenAdvanceBufferPointer(d, linksSize); + dtPolyDetail* detailMeshes = dtGetThenAdvanceBufferPointer(d, detailMeshesSize); + float* detailVerts = dtGetThenAdvanceBufferPointer(d, detailVertsSize); + d += detailTrisSize; // Ignore detail tris; single bytes can't be endian-swapped. + //unsigned char* detailTris = dtGetThenAdvanceBufferPointer(d, detailTrisSize); + dtBVNode* bvTree = dtGetThenAdvanceBufferPointer(d, bvtreeSize); + dtOffMeshConnection* offMeshCons = dtGetThenAdvanceBufferPointer(d, offMeshLinksSize); + + // Vertices + for (int i = 0; i < header->vertCount*3; ++i) + { + dtSwapEndian(&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) + { + dtSwapEndian(&p->verts[j]); + dtSwapEndian(&p->neis[j]); + } + dtSwapEndian(&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]; + dtSwapEndian(&pd->vertBase); + dtSwapEndian(&pd->triBase); + } + + // Detail verts + for (int i = 0; i < header->detailVertCount*3; ++i) + { + dtSwapEndian(&detailVerts[i]); + } + + // BV-tree + for (int i = 0; i < header->bvNodeCount; ++i) + { + dtBVNode* node = &bvTree[i]; + for (int j = 0; j < 3; ++j) + { + dtSwapEndian(&node->bmin[j]); + dtSwapEndian(&node->bmax[j]); + } + dtSwapEndian(&node->i); + } + + // Off-mesh Connections. + for (int i = 0; i < header->offMeshConCount; ++i) + { + dtOffMeshConnection* con = &offMeshCons[i]; + for (int j = 0; j < 6; ++j) + dtSwapEndian(&con->pos[j]); + dtSwapEndian(&con->rad); + dtSwapEndian(&con->poly); + } + + return true; +} diff --git a/recastnavigation/Detour/Source/DetourNavMeshQuery.cpp b/recastnavigation/Detour/Source/DetourNavMeshQuery.cpp index ef1c437..cebe26b 100644 --- a/recastnavigation/Detour/Source/DetourNavMeshQuery.cpp +++ b/recastnavigation/Detour/Source/DetourNavMeshQuery.cpp @@ -30,133 +30,139 @@ /// @class dtQueryFilter /// /// The Default Implementation -/// +/// /// At construction: All area costs default to 1.0. All flags are included /// and none are excluded. -/// +/// /// If a polygon has both an include and an exclude flag, it will be excluded. -/// -/// The way filtering works, a navigation mesh polygon must have at least one flag +/// +/// The way filtering works, a navigation mesh polygon must have at least one flag /// set to ever be considered by a query. So a polygon with no flags will never /// be considered. /// /// Setting the include flags to 0 will result in all polygons being excluded. /// /// Custom Implementations -/// +/// /// DT_VIRTUAL_QUERYFILTER must be defined in order to extend this class. -/// -/// Implement a custom query filter by overriding the virtual passFilter() -/// and getCost() functions. If this is done, both functions should be as -/// fast as possible. Use cached local copies of data rather than accessing +/// +/// Implement a custom query filter by overriding the virtual passFilter() +/// and getCost() functions. If this is done, both functions should be as +/// fast as possible. Use cached local copies of data rather than accessing /// your own objects where possible. -/// -/// Custom implementations do not need to adhere to the flags or cost logic -/// used by the default implementation. -/// +/// +/// Custom implementations do not need to adhere to the flags or cost logic +/// used by the default implementation. +/// /// In order for A* searches to work properly, the cost should be proportional to -/// the travel distance. Implementing a cost modifier less than 1.0 is likely +/// the travel distance. Implementing a cost modifier less than 1.0 is likely /// to lead to problems during pathfinding. /// /// @see dtNavMeshQuery dtQueryFilter::dtQueryFilter() : - m_includeFlags(0xffff), - m_excludeFlags(0) + m_includeFlags(0xffff), + m_excludeFlags(0) { - for (int i = 0; i < DT_MAX_AREAS; ++i) - m_areaCost[i] = 1.0f; + 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 + const dtMeshTile* /*tile*/, + const dtPoly* poly) const { - return (poly->flags & m_includeFlags) != 0 && (poly->flags & m_excludeFlags) == 0; + 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 + 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()]; + return dtVdist(pa, pb) * m_areaCost[curPoly->getArea()]; } #else inline bool dtQueryFilter::passFilter(const dtPolyRef /*ref*/, - const dtMeshTile* /*tile*/, - const dtPoly* poly) const + const dtMeshTile* /*tile*/, + const dtPoly* poly) const { - return (poly->flags & m_includeFlags) != 0 && (poly->flags & m_excludeFlags) == 0; + 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 + 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()]; + return dtVdist(pa, pb) * m_areaCost[curPoly->getArea()]; } -#endif - +#endif + static const float H_SCALE = 0.999f; // Search heuristic scale. + dtNavMeshQuery* dtAllocNavMeshQuery() { - void* mem = dtAlloc(sizeof(dtNavMeshQuery), DT_ALLOC_PERM); - if (!mem) return 0; - return new(mem) dtNavMeshQuery; + 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); + if (!navmesh) return; + navmesh->~dtNavMeshQuery(); + dtFree(navmesh); +} + +dtPolyQuery::~dtPolyQuery() +{ + // Defined out of line to fix the weak v-tables warning } ////////////////////////////////////////////////////////////////////////////////////////// /// @class dtNavMeshQuery /// -/// For methods that support undersized buffers, if the buffer is too small -/// to hold the entire result set the return status of the method will include +/// For methods that support undersized buffers, if the buffer is too small +/// to hold the entire result set the return status of the method will include /// the #DT_BUFFER_TOO_SMALL flag. /// /// Constant member functions can be used by multiple clients without side /// effects. (E.g. No change to the closed list. No impact on an in-progress /// sliced path query. Etc.) -/// -/// Walls and portals: A @e wall is a polygon segment that is +/// +/// Walls and portals: A @e wall is a polygon segment that is /// considered impassable. A @e portal is a passable segment between polygons. /// A portal may be treated as a wall based on the dtQueryFilter used for a query. /// /// @see dtNavMesh, dtQueryFilter, #dtAllocNavMeshQuery(), #dtAllocNavMeshQuery() dtNavMeshQuery::dtNavMeshQuery() : - m_nav(0), - m_tinyNodePool(0), - m_nodePool(0), - m_openList(0) + m_nav(0), + m_tinyNodePool(0), + m_nodePool(0), + m_openList(0) { - memset(&m_query, 0, sizeof(dtQueryData)); + 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); + 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); } -/// @par +/// @par /// /// Must be the first function called after construction, before other /// functions are used. @@ -164,340 +170,334 @@ dtNavMeshQuery::~dtNavMeshQuery() /// This function can be used multiple times. dtStatus dtNavMeshQuery::init(const dtNavMesh* nav, const int maxNodes) { - if (maxNodes > DT_NULL_IDX || maxNodes > (1 << DT_NODE_PARENT_BITS) - 1) - return DT_FAILURE | DT_INVALID_PARAM; - - 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 | DT_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 | DT_OUT_OF_MEMORY; - } - else - { - m_tinyNodePool->clear(); - } - - 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 | DT_OUT_OF_MEMORY; - } - else - { - m_openList->clear(); - } - - return DT_SUCCESS; + if (maxNodes > DT_NULL_IDX || maxNodes > (1 << DT_NODE_PARENT_BITS) - 1) + return DT_FAILURE | DT_INVALID_PARAM; + + 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 | DT_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 | DT_OUT_OF_MEMORY; + } + else + { + m_tinyNodePool->clear(); + } + + 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 | DT_OUT_OF_MEMORY; + } + else + { + m_openList->clear(); + } + + return DT_SUCCESS; } dtStatus dtNavMeshQuery::findRandomPoint(const dtQueryFilter* filter, float (*frand)(), - dtPolyRef* randomRef, float* randomPt) const + dtPolyRef* randomRef, float* randomPt) const { - dtAssert(m_nav); - - if (!filter || !frand || !randomRef || !randomPt) - return DT_FAILURE | DT_INVALID_PARAM; - - // Randomly pick one tile. Assume that all tiles cover roughly the same area. - const dtMeshTile* tile = 0; - float tsum = 0.0f; - for (int i = 0; i < m_nav->getMaxTiles(); i++) - { - const dtMeshTile* t = m_nav->getTile(i); - if (!t || !t->header) continue; - - // Choose random tile using reservoi sampling. - const float area = 1.0f; // Could be tile area too. - tsum += area; - const float u = frand(); - if (u * tsum <= area) - tile = t; - } - if (!tile) - return DT_FAILURE; - - // Randomly pick one polygon weighted by polygon area. - const dtPoly* poly = 0; - dtPolyRef polyRef = 0; - const dtPolyRef base = m_nav->getPolyRefBase(tile); - - float areaSum = 0.0f; - for (int i = 0; i < tile->header->polyCount; ++i) - { - const dtPoly* p = &tile->polys[i]; - // Do not return off-mesh connection polygons. - if (p->getType() != DT_POLYTYPE_GROUND) - continue; - // Must pass filter - const dtPolyRef ref = base | (dtPolyRef)i; - if (!filter->passFilter(ref, tile, p)) - continue; - - // Calc area of the polygon. - float polyArea = 0.0f; - for (int j = 2; j < p->vertCount; ++j) - { - const float* va = &tile->verts[p->verts[0] * 3]; - const float* vb = &tile->verts[p->verts[j - 1] * 3]; - const float* vc = &tile->verts[p->verts[j] * 3]; - polyArea += dtTriArea2D(va, vb, vc); - } - - // Choose random polygon weighted by area, using reservoi sampling. - areaSum += polyArea; - const float u = frand(); - if (u * areaSum <= polyArea) - { - poly = p; - polyRef = ref; - } - } - - if (!poly) - return DT_FAILURE; - - // Randomly pick point on polygon. - const float* v = &tile->verts[poly->verts[0] * 3]; - float verts[3 * DT_VERTS_PER_POLYGON]; - float areas[DT_VERTS_PER_POLYGON]; - dtVcopy(&verts[0 * 3], v); - for (int j = 1; j < poly->vertCount; ++j) - { - v = &tile->verts[poly->verts[j] * 3]; - dtVcopy(&verts[j * 3], v); - } - - const float s = frand(); - const float t = frand(); - - float pt[3]; - dtRandomPointInConvexPoly(verts, poly->vertCount, areas, s, t, pt); - - float h = 0.0f; - dtStatus status = getPolyHeight(polyRef, pt, &h); - if (dtStatusFailed(status)) - return status; - pt[1] = h; - - dtVcopy(randomPt, pt); - *randomRef = polyRef; - - return DT_SUCCESS; + dtAssert(m_nav); + + if (!filter || !frand || !randomRef || !randomPt) + return DT_FAILURE | DT_INVALID_PARAM; + + // Randomly pick one tile. Assume that all tiles cover roughly the same area. + const dtMeshTile* tile = 0; + float tsum = 0.0f; + for (int i = 0; i < m_nav->getMaxTiles(); i++) + { + const dtMeshTile* t = m_nav->getTile(i); + if (!t || !t->header) continue; + + // Choose random tile using reservoi sampling. + const float area = 1.0f; // Could be tile area too. + tsum += area; + const float u = frand(); + if (u*tsum <= area) + tile = t; + } + if (!tile) + return DT_FAILURE; + + // Randomly pick one polygon weighted by polygon area. + const dtPoly* poly = 0; + dtPolyRef polyRef = 0; + const dtPolyRef base = m_nav->getPolyRefBase(tile); + + float areaSum = 0.0f; + for (int i = 0; i < tile->header->polyCount; ++i) + { + const dtPoly* p = &tile->polys[i]; + // Do not return off-mesh connection polygons. + if (p->getType() != DT_POLYTYPE_GROUND) + continue; + // Must pass filter + const dtPolyRef ref = base | (dtPolyRef)i; + if (!filter->passFilter(ref, tile, p)) + continue; + + // Calc area of the polygon. + float polyArea = 0.0f; + for (int j = 2; j < p->vertCount; ++j) + { + const float* va = &tile->verts[p->verts[0]*3]; + const float* vb = &tile->verts[p->verts[j-1]*3]; + const float* vc = &tile->verts[p->verts[j]*3]; + polyArea += dtTriArea2D(va,vb,vc); + } + + // Choose random polygon weighted by area, using reservoi sampling. + areaSum += polyArea; + const float u = frand(); + if (u*areaSum <= polyArea) + { + poly = p; + polyRef = ref; + } + } + + if (!poly) + return DT_FAILURE; + + // Randomly pick point on polygon. + const float* v = &tile->verts[poly->verts[0]*3]; + float verts[3*DT_VERTS_PER_POLYGON]; + float areas[DT_VERTS_PER_POLYGON]; + dtVcopy(&verts[0*3],v); + for (int j = 1; j < poly->vertCount; ++j) + { + v = &tile->verts[poly->verts[j]*3]; + dtVcopy(&verts[j*3],v); + } + + const float s = frand(); + const float t = frand(); + + float pt[3]; + dtRandomPointInConvexPoly(verts, poly->vertCount, areas, s, t, pt); + + closestPointOnPoly(polyRef, pt, pt, NULL); + + dtVcopy(randomPt, pt); + *randomRef = polyRef; + + return DT_SUCCESS; } dtStatus dtNavMeshQuery::findRandomPointAroundCircle(dtPolyRef startRef, const float* centerPos, const float maxRadius, - const dtQueryFilter* filter, float (*frand)(), - dtPolyRef* randomRef, float* randomPt) const + const dtQueryFilter* filter, float (*frand)(), + dtPolyRef* randomRef, float* randomPt) const { - dtAssert(m_nav); - dtAssert(m_nodePool); - dtAssert(m_openList); - - // Validate input - if (!m_nav->isValidPolyRef(startRef) || - !centerPos || !dtVisfinite(centerPos) || - maxRadius < 0 || !dtMathIsfinite(maxRadius) || - !filter || !frand || !randomRef || !randomPt) - { - return DT_FAILURE | DT_INVALID_PARAM; - } - - const dtMeshTile* startTile = 0; - const dtPoly* startPoly = 0; - m_nav->getTileAndPolyByRefUnsafe(startRef, &startTile, &startPoly); - if (!filter->passFilter(startRef, startTile, startPoly)) - return DT_FAILURE | DT_INVALID_PARAM; - - 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); - - dtStatus status = DT_SUCCESS; - - const float radiusSqr = dtSqr(maxRadius); - float areaSum = 0.0f; - - const dtMeshTile* randomTile = 0; - const dtPoly* randomPoly = 0; - dtPolyRef randomPolyRef = 0; - - 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); - - // Place random locations on on ground. - if (bestPoly->getType() == DT_POLYTYPE_GROUND) - { - // Calc area of the polygon. - float polyArea = 0.0f; - for (int j = 2; j < bestPoly->vertCount; ++j) - { - const float* va = &bestTile->verts[bestPoly->verts[0] * 3]; - const float* vb = &bestTile->verts[bestPoly->verts[j - 1] * 3]; - const float* vc = &bestTile->verts[bestPoly->verts[j] * 3]; - polyArea += dtTriArea2D(va, vb, vc); - } - // Choose random polygon weighted by area, using reservoi sampling. - areaSum += polyArea; - const float u = frand(); - if (u * areaSum <= polyArea) - { - randomTile = bestTile; - randomPoly = bestPoly; - randomPolyRef = bestRef; - } - } - - // 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) - { - status |= DT_OUT_OF_NODES; - 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 = (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); - } - } - } - - if (!randomPoly) - return DT_FAILURE; - - // Randomly pick point on polygon. - const float* v = &randomTile->verts[randomPoly->verts[0] * 3]; - float verts[3 * DT_VERTS_PER_POLYGON]; - float areas[DT_VERTS_PER_POLYGON]; - dtVcopy(&verts[0 * 3], v); - for (int j = 1; j < randomPoly->vertCount; ++j) - { - v = &randomTile->verts[randomPoly->verts[j] * 3]; - dtVcopy(&verts[j * 3], v); - } - - const float s = frand(); - const float t = frand(); - - float pt[3]; - dtRandomPointInConvexPoly(verts, randomPoly->vertCount, areas, s, t, pt); - - float h = 0.0f; - dtStatus stat = getPolyHeight(randomPolyRef, pt, &h); - if (dtStatusFailed(status)) - return stat; - pt[1] = h; - - dtVcopy(randomPt, pt); - *randomRef = randomPolyRef; - - return DT_SUCCESS; + dtAssert(m_nav); + dtAssert(m_nodePool); + dtAssert(m_openList); + + // Validate input + if (!m_nav->isValidPolyRef(startRef) || + !centerPos || !dtVisfinite(centerPos) || + maxRadius < 0 || !dtMathIsfinite(maxRadius) || + !filter || !frand || !randomRef || !randomPt) + { + return DT_FAILURE | DT_INVALID_PARAM; + } + + const dtMeshTile* startTile = 0; + const dtPoly* startPoly = 0; + m_nav->getTileAndPolyByRefUnsafe(startRef, &startTile, &startPoly); + if (!filter->passFilter(startRef, startTile, startPoly)) + return DT_FAILURE | DT_INVALID_PARAM; + + 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); + + dtStatus status = DT_SUCCESS; + + const float radiusSqr = dtSqr(maxRadius); + float areaSum = 0.0f; + + const dtMeshTile* randomTile = 0; + const dtPoly* randomPoly = 0; + dtPolyRef randomPolyRef = 0; + + 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); + + // Place random locations on on ground. + if (bestPoly->getType() == DT_POLYTYPE_GROUND) + { + // Calc area of the polygon. + float polyArea = 0.0f; + for (int j = 2; j < bestPoly->vertCount; ++j) + { + const float* va = &bestTile->verts[bestPoly->verts[0]*3]; + const float* vb = &bestTile->verts[bestPoly->verts[j-1]*3]; + const float* vc = &bestTile->verts[bestPoly->verts[j]*3]; + polyArea += dtTriArea2D(va,vb,vc); + } + // Choose random polygon weighted by area, using reservoi sampling. + areaSum += polyArea; + const float u = frand(); + if (u*areaSum <= polyArea) + { + randomTile = bestTile; + randomPoly = bestPoly; + randomPolyRef = bestRef; + } + } + + + // 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) + { + status |= DT_OUT_OF_NODES; + 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 = (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); + } + } + } + + if (!randomPoly) + return DT_FAILURE; + + // Randomly pick point on polygon. + const float* v = &randomTile->verts[randomPoly->verts[0]*3]; + float verts[3*DT_VERTS_PER_POLYGON]; + float areas[DT_VERTS_PER_POLYGON]; + dtVcopy(&verts[0*3],v); + for (int j = 1; j < randomPoly->vertCount; ++j) + { + v = &randomTile->verts[randomPoly->verts[j]*3]; + dtVcopy(&verts[j*3],v); + } + + const float s = frand(); + const float t = frand(); + + float pt[3]; + dtRandomPointInConvexPoly(verts, randomPoly->vertCount, areas, s, t, pt); + + closestPointOnPoly(randomPolyRef, pt, pt, NULL); + + dtVcopy(randomPt, pt); + *randomRef = randomPolyRef; + + return status; } + ////////////////////////////////////////////////////////////////////////////////////////// /// @par @@ -510,380 +510,410 @@ dtStatus dtNavMeshQuery::findRandomPointAroundCircle(dtPolyRef startRef, const f /// dtStatus dtNavMeshQuery::closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const { - dtAssert(m_nav); - if (!m_nav->isValidPolyRef(ref) || - !pos || !dtVisfinite(pos) || - !closest) - { - return DT_FAILURE | DT_INVALID_PARAM; - } - - m_nav->closestPointOnPoly(ref, pos, closest, posOverPoly); - return DT_SUCCESS; + dtAssert(m_nav); + if (!m_nav->isValidPolyRef(ref) || + !pos || !dtVisfinite(pos) || + !closest) + { + return DT_FAILURE | DT_INVALID_PARAM; + } + + m_nav->closestPointOnPoly(ref, pos, closest, posOverPoly); + return DT_SUCCESS; } /// @par /// /// Much faster than closestPointOnPoly(). /// -/// If the provided position lies within the polygon's xz-bounds (above or below), +/// If the provided position lies within the polygon's xz-bounds (above or below), /// then @p pos and @p closest will be equal. /// /// The height of @p closest will be the polygon boundary. The height detail is not used. -/// +/// /// @p pos does not have to be within the bounds of the polybon or the navigation mesh. -/// +/// dtStatus dtNavMeshQuery::closestPointOnPolyBoundary(dtPolyRef ref, const float* pos, float* closest) const { - dtAssert(m_nav); - - const dtMeshTile* tile = 0; - const dtPoly* poly = 0; - if (dtStatusFailed(m_nav->getTileAndPolyByRef(ref, &tile, &poly))) - return DT_FAILURE | DT_INVALID_PARAM; - - if (!pos || !dtVisfinite(pos) || !closest) - return DT_FAILURE | DT_INVALID_PARAM; - - // 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 = edged[0]; - int imin = 0; - for (int i = 1; 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; + dtAssert(m_nav); + + const dtMeshTile* tile = 0; + const dtPoly* poly = 0; + if (dtStatusFailed(m_nav->getTileAndPolyByRef(ref, &tile, &poly))) + return DT_FAILURE | DT_INVALID_PARAM; + + if (!pos || !dtVisfinite(pos) || !closest) + return DT_FAILURE | DT_INVALID_PARAM; + + // 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 = edged[0]; + int imin = 0; + for (int i = 1; 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; } /// @par /// -/// Will return #DT_FAILURE | DT_INVALID_PARAM if the provided position is outside the xz-bounds +/// Will return #DT_FAILURE | DT_INVALID_PARAM if the provided position is outside the xz-bounds /// of the polygon. -/// +/// dtStatus dtNavMeshQuery::getPolyHeight(dtPolyRef ref, const float* pos, float* height) const { - dtAssert(m_nav); - - const dtMeshTile* tile = 0; - const dtPoly* poly = 0; - if (dtStatusFailed(m_nav->getTileAndPolyByRef(ref, &tile, &poly))) - return DT_FAILURE | DT_INVALID_PARAM; - - if (!pos || !dtVisfinite2D(pos)) - return DT_FAILURE | DT_INVALID_PARAM; - - // We used to return success for offmesh connections, but the - // getPolyHeight in DetourNavMesh does not do this, so special - // case it here. - 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]; - float t; - dtDistancePtSegSqr2D(pos, v0, v1, t); - if (height) - *height = v0[1] + (v1[1] - v0[1]) * t; - - return DT_SUCCESS; - } - - return m_nav->getPolyHeight(tile, poly, pos, height) - ? DT_SUCCESS - : DT_FAILURE | DT_INVALID_PARAM; + dtAssert(m_nav); + + const dtMeshTile* tile = 0; + const dtPoly* poly = 0; + if (dtStatusFailed(m_nav->getTileAndPolyByRef(ref, &tile, &poly))) + return DT_FAILURE | DT_INVALID_PARAM; + + if (!pos || !dtVisfinite2D(pos)) + return DT_FAILURE | DT_INVALID_PARAM; + + // We used to return success for offmesh connections, but the + // getPolyHeight in DetourNavMesh does not do this, so special + // case it here. + 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]; + float t; + dtDistancePtSegSqr2D(pos, v0, v1, t); + if (height) + *height = v0[1] + (v1[1] - v0[1])*t; + + return DT_SUCCESS; + } + + return m_nav->getPolyHeight(tile, poly, pos, height) + ? DT_SUCCESS + : DT_FAILURE | DT_INVALID_PARAM; } class dtFindNearestPolyQuery : public dtPolyQuery { - const dtNavMeshQuery* m_query; - const float* m_center; - float m_nearestDistanceSqr; - dtPolyRef m_nearestRef; - float m_nearestPoint[3]; + const dtNavMeshQuery* m_query; + const float* m_center; + float m_nearestDistanceSqr; + dtPolyRef m_nearestRef; + float m_nearestPoint[3]; + bool m_overPoly; public: - dtFindNearestPolyQuery(const dtNavMeshQuery* query, const float* center) - : m_query(query), m_center(center), m_nearestDistanceSqr(FLT_MAX), m_nearestRef(0), m_nearestPoint() - { - } - - dtPolyRef nearestRef() const { return m_nearestRef; } - const float* nearestPoint() const { return m_nearestPoint; } - - void process(const dtMeshTile* tile, dtPoly** polys, dtPolyRef* refs, int count) - { - dtIgnoreUnused(polys); - - for (int i = 0; i < count; ++i) - { - dtPolyRef ref = refs[i]; - float closestPtPoly[3]; - float diff[3]; - bool posOverPoly = false; - float d; - m_query->closestPointOnPoly(ref, m_center, closestPtPoly, &posOverPoly); - - // If a point is directly over a polygon and closer than - // climb height, favor that instead of straight line nearest point. - dtVsub(diff, m_center, closestPtPoly); - if (posOverPoly) - { - d = dtAbs(diff[1]) - tile->header->walkableClimb; - d = d > 0 ? d * d : 0; - } - else - { - d = dtVlenSqr(diff); - } - - if (d < m_nearestDistanceSqr) - { - dtVcopy(m_nearestPoint, closestPtPoly); - - m_nearestDistanceSqr = d; - m_nearestRef = ref; - } - } - } + dtFindNearestPolyQuery(const dtNavMeshQuery* query, const float* center) + : m_query(query), m_center(center), m_nearestDistanceSqr(FLT_MAX), m_nearestRef(0), m_nearestPoint(), m_overPoly(false) + { + } + + virtual ~dtFindNearestPolyQuery(); + + dtPolyRef nearestRef() const { return m_nearestRef; } + const float* nearestPoint() const { return m_nearestPoint; } + bool isOverPoly() const { return m_overPoly; } + + void process(const dtMeshTile* tile, dtPoly** polys, dtPolyRef* refs, int count) + { + dtIgnoreUnused(polys); + + for (int i = 0; i < count; ++i) + { + dtPolyRef ref = refs[i]; + float closestPtPoly[3]; + float diff[3]; + bool posOverPoly = false; + float d; + m_query->closestPointOnPoly(ref, m_center, closestPtPoly, &posOverPoly); + + // If a point is directly over a polygon and closer than + // climb height, favor that instead of straight line nearest point. + dtVsub(diff, m_center, closestPtPoly); + if (posOverPoly) + { + d = dtAbs(diff[1]) - tile->header->walkableClimb; + d = d > 0 ? d*d : 0; + } + else + { + d = dtVlenSqr(diff); + } + + if (d < m_nearestDistanceSqr) + { + dtVcopy(m_nearestPoint, closestPtPoly); + + m_nearestDistanceSqr = d; + m_nearestRef = ref; + m_overPoly = posOverPoly; + } + } + } }; -/// @par +dtFindNearestPolyQuery::~dtFindNearestPolyQuery() +{ + // Defined out of line to fix the weak v-tables warning +} + +/// @par /// -/// @note If the search box does not intersect any polygons the search will -/// return #DT_SUCCESS, but @p nearestRef will be zero. So if in doubt, check +/// @note If the search box does not intersect any polygons the search will +/// return #DT_SUCCESS, but @p nearestRef will be zero. So if in doubt, check /// @p nearestRef before using @p nearestPt. /// dtStatus dtNavMeshQuery::findNearestPoly(const float* center, const float* halfExtents, - const dtQueryFilter* filter, - dtPolyRef* nearestRef, float* nearestPt) const + const dtQueryFilter* filter, + dtPolyRef* nearestRef, float* nearestPt) const { - dtAssert(m_nav); - - if (!nearestRef) - return DT_FAILURE | DT_INVALID_PARAM; - - // queryPolygons below will check rest of params - - dtFindNearestPolyQuery query(this, center); - - dtStatus status = queryPolygons(center, halfExtents, filter, &query); - if (dtStatusFailed(status)) - return status; - - *nearestRef = query.nearestRef(); - // Only override nearestPt if we actually found a poly so the nearest point - // is valid. - if (nearestPt && *nearestRef) - dtVcopy(nearestPt, query.nearestPoint()); + return findNearestPoly(center, halfExtents, filter, nearestRef, nearestPt, NULL); +} - return DT_SUCCESS; +// If center and nearestPt point to an equal position, isOverPoly will be true; +// however there's also a special case of climb height inside the polygon (see dtFindNearestPolyQuery) +dtStatus dtNavMeshQuery::findNearestPoly(const float* center, const float* halfExtents, + const dtQueryFilter* filter, + dtPolyRef* nearestRef, float* nearestPt, bool* isOverPoly) const +{ + dtAssert(m_nav); + + if (!nearestRef) + return DT_FAILURE | DT_INVALID_PARAM; + + // queryPolygons below will check rest of params + + dtFindNearestPolyQuery query(this, center); + + dtStatus status = queryPolygons(center, halfExtents, filter, &query); + if (dtStatusFailed(status)) + return status; + + *nearestRef = query.nearestRef(); + // Only override nearestPt if we actually found a poly so the nearest point + // is valid. + if (nearestPt && *nearestRef) + { + dtVcopy(nearestPt, query.nearestPoint()); + if (isOverPoly) + *isOverPoly = query.isOverPoly(); + } + + return DT_SUCCESS; } void dtNavMeshQuery::queryPolygonsInTile(const dtMeshTile* tile, const float* qmin, const float* qmax, - const dtQueryFilter* filter, dtPolyQuery* query) const + const dtQueryFilter* filter, dtPolyQuery* query) const { - dtAssert(m_nav); - static const int batchSize = 32; - dtPolyRef polyRefs[batchSize]; - dtPoly* polys[batchSize]; - int n = 0; - - 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); - 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])) - { - polyRefs[n] = ref; - polys[n] = &tile->polys[node->i]; - - if (n == batchSize - 1) - { - query->process(tile, polys, polyRefs, batchSize); - n = 0; - } - else - { - n++; - } - } - } - - if (overlap || isLeafNode) - node++; - else - { - const int escapeIndex = -node->i; - node += escapeIndex; - } - } - } - else - { - float bmin[3], bmax[3]; - const dtPolyRef base = m_nav->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; - // Must pass filter - const dtPolyRef ref = base | (dtPolyRef)i; - if (!filter->passFilter(ref, tile, p)) - 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)) - { - polyRefs[n] = ref; - polys[n] = p; - - if (n == batchSize - 1) - { - query->process(tile, polys, polyRefs, batchSize); - n = 0; - } - else - { - n++; - } - } - } - } - - // Process the last polygons that didn't make a full batch. - if (n > 0) - query->process(tile, polys, polyRefs, n); + dtAssert(m_nav); + static const int batchSize = 32; + dtPolyRef polyRefs[batchSize]; + dtPoly* polys[batchSize]; + int n = 0; + + 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); + 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])) + { + polyRefs[n] = ref; + polys[n] = &tile->polys[node->i]; + + if (n == batchSize - 1) + { + query->process(tile, polys, polyRefs, batchSize); + n = 0; + } + else + { + n++; + } + } + } + + if (overlap || isLeafNode) + node++; + else + { + const int escapeIndex = -node->i; + node += escapeIndex; + } + } + } + else + { + float bmin[3], bmax[3]; + const dtPolyRef base = m_nav->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; + // Must pass filter + const dtPolyRef ref = base | (dtPolyRef)i; + if (!filter->passFilter(ref, tile, p)) + 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)) + { + polyRefs[n] = ref; + polys[n] = p; + + if (n == batchSize - 1) + { + query->process(tile, polys, polyRefs, batchSize); + n = 0; + } + else + { + n++; + } + } + } + } + + // Process the last polygons that didn't make a full batch. + if (n > 0) + query->process(tile, polys, polyRefs, n); } class dtCollectPolysQuery : public dtPolyQuery { - dtPolyRef* m_polys; - const int m_maxPolys; - int m_numCollected; - bool m_overflow; + dtPolyRef* m_polys; + const int m_maxPolys; + int m_numCollected; + bool m_overflow; public: - dtCollectPolysQuery(dtPolyRef* polys, const int maxPolys) - : m_polys(polys), m_maxPolys(maxPolys), m_numCollected(0), m_overflow(false) - { - } - - int numCollected() const { return m_numCollected; } - bool overflowed() const { return m_overflow; } - - void process(const dtMeshTile* tile, dtPoly** polys, dtPolyRef* refs, int count) - { - dtIgnoreUnused(tile); - dtIgnoreUnused(polys); - - int numLeft = m_maxPolys - m_numCollected; - int toCopy = count; - if (toCopy > numLeft) - { - m_overflow = true; - toCopy = numLeft; - } - - memcpy(m_polys + m_numCollected, refs, (size_t)toCopy * sizeof(dtPolyRef)); - m_numCollected += toCopy; - } + dtCollectPolysQuery(dtPolyRef* polys, const int maxPolys) + : m_polys(polys), m_maxPolys(maxPolys), m_numCollected(0), m_overflow(false) + { + } + + virtual ~dtCollectPolysQuery(); + + int numCollected() const { return m_numCollected; } + bool overflowed() const { return m_overflow; } + + void process(const dtMeshTile* tile, dtPoly** polys, dtPolyRef* refs, int count) + { + dtIgnoreUnused(tile); + dtIgnoreUnused(polys); + + int numLeft = m_maxPolys - m_numCollected; + int toCopy = count; + if (toCopy > numLeft) + { + m_overflow = true; + toCopy = numLeft; + } + + memcpy(m_polys + m_numCollected, refs, (size_t)toCopy * sizeof(dtPolyRef)); + m_numCollected += toCopy; + } }; -/// @par +dtCollectPolysQuery::~dtCollectPolysQuery() +{ + // Defined out of line to fix the weak v-tables warning +} + +/// @par /// /// If no polygons are found, the function will return #DT_SUCCESS with a /// @p polyCount of zero. /// -/// If @p polys is too small to hold the entire result set, then the array will -/// be filled to capacity. The method of choosing which polygons from the +/// If @p polys is too small to hold the entire result set, then the array will +/// be filled to capacity. The method of choosing which polygons from the /// full set are included in the partial result set is undefined. /// dtStatus dtNavMeshQuery::queryPolygons(const float* center, const float* halfExtents, - const dtQueryFilter* filter, - dtPolyRef* polys, int* polyCount, const int maxPolys) const + const dtQueryFilter* filter, + dtPolyRef* polys, int* polyCount, const int maxPolys) const { - if (!polys || !polyCount || maxPolys < 0) - return DT_FAILURE | DT_INVALID_PARAM; + if (!polys || !polyCount || maxPolys < 0) + return DT_FAILURE | DT_INVALID_PARAM; - dtCollectPolysQuery collector(polys, maxPolys); + dtCollectPolysQuery collector(polys, maxPolys); - dtStatus status = queryPolygons(center, halfExtents, filter, &collector); - if (dtStatusFailed(status)) - return status; + dtStatus status = queryPolygons(center, halfExtents, filter, &collector); + if (dtStatusFailed(status)) + return status; - *polyCount = collector.numCollected(); - return collector.overflowed() ? DT_SUCCESS | DT_BUFFER_TOO_SMALL : DT_SUCCESS; + *polyCount = collector.numCollected(); + return collector.overflowed() ? DT_SUCCESS | DT_BUFFER_TOO_SMALL : DT_SUCCESS; } -/// @par +/// @par /// /// The query will be invoked with batches of polygons. Polygons passed /// to the query have bounding boxes that overlap with the center and halfExtents @@ -891,42 +921,42 @@ dtStatus dtNavMeshQuery::queryPolygons(const float* center, const float* halfExt /// times until all overlapping polygons have been processed. /// dtStatus dtNavMeshQuery::queryPolygons(const float* center, const float* halfExtents, - const dtQueryFilter* filter, dtPolyQuery* query) const + const dtQueryFilter* filter, dtPolyQuery* query) const { - dtAssert(m_nav); - - if (!center || !dtVisfinite(center) || - !halfExtents || !dtVisfinite(halfExtents) || - !filter || !query) - { - return DT_FAILURE | DT_INVALID_PARAM; - } - - float bmin[3], bmax[3]; - dtVsub(bmin, center, halfExtents); - dtVadd(bmax, center, halfExtents); - - // Find tiles the query touches. - int minx, miny, maxx, maxy; - m_nav->calcTileLoc(bmin, &minx, &miny); - m_nav->calcTileLoc(bmax, &maxx, &maxy); - - static const int MAX_NEIS = 32; - const dtMeshTile* neis[MAX_NEIS]; - - for (int y = miny; y <= maxy; ++y) - { - for (int x = minx; x <= maxx; ++x) - { - const int nneis = m_nav->getTilesAt(x, y, neis, MAX_NEIS); - for (int j = 0; j < nneis; ++j) - { - queryPolygonsInTile(neis[j], bmin, bmax, filter, query); - } - } - } - - return DT_SUCCESS; + dtAssert(m_nav); + + if (!center || !dtVisfinite(center) || + !halfExtents || !dtVisfinite(halfExtents) || + !filter || !query) + { + return DT_FAILURE | DT_INVALID_PARAM; + } + + float bmin[3], bmax[3]; + dtVsub(bmin, center, halfExtents); + dtVadd(bmax, center, halfExtents); + + // Find tiles the query touches. + int minx, miny, maxx, maxy; + m_nav->calcTileLoc(bmin, &minx, &miny); + m_nav->calcTileLoc(bmax, &maxx, &maxy); + + static const int MAX_NEIS = 32; + const dtMeshTile* neis[MAX_NEIS]; + + for (int y = miny; y <= maxy; ++y) + { + for (int x = minx; x <= maxx; ++x) + { + const int nneis = m_nav->getTilesAt(x,y,neis,MAX_NEIS); + for (int j = 0; j < nneis; ++j) + { + queryPolygonsInTile(neis[j], bmin, bmax, filter, query); + } + } + } + + return DT_SUCCESS; } /// @par @@ -934,1412 +964,1423 @@ dtStatus dtNavMeshQuery::queryPolygons(const float* center, const float* halfExt /// If the end polygon cannot be reached through the navigation graph, /// the last polygon in the path will be the nearest the end polygon. /// -/// If the path array is to small to hold the full result, it will be filled as +/// If the path array is to small to hold the full result, it will be filled as /// far as possible from the start polygon toward the end polygon. /// -/// The start and end positions are used to calculate traversal costs. +/// The start and end positions are used to calculate traversal costs. /// (The y-values impact the result.) /// dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef, - const float* startPos, const float* endPos, - const dtQueryFilter* filter, - dtPolyRef* path, int* pathCount, const int maxPath) const + 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); - - if (!pathCount) - return DT_FAILURE | DT_INVALID_PARAM; - - *pathCount = 0; - - // Validate input - if (!m_nav->isValidPolyRef(startRef) || !m_nav->isValidPolyRef(endRef) || - !startPos || !dtVisfinite(startPos) || - !endPos || !dtVisfinite(endPos) || - !filter || !path || maxPath <= 0) - { - return DT_FAILURE | DT_INVALID_PARAM; - } - - 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; - - bool outOfNodes = false; - - 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; - - // deal explicitly with crossing tile boundaries - unsigned char crossSide = 0; - if (bestTile->links[i].side != 0xff) - crossSide = bestTile->links[i].side >> 1; - - // get the node - dtNode* neighbourNode = m_nodePool->getNode(neighbourRef, crossSide); - if (!neighbourNode) - { - outOfNodes = true; - 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 = (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; - } - } - } - - dtStatus status = getPathToNode(lastBestNode, path, pathCount, maxPath); - - if (lastBestNode->id != endRef) - status |= DT_PARTIAL_RESULT; - - if (outOfNodes) - status |= DT_OUT_OF_NODES; - - return status; + dtAssert(m_nav); + dtAssert(m_nodePool); + dtAssert(m_openList); + + if (!pathCount) + return DT_FAILURE | DT_INVALID_PARAM; + + *pathCount = 0; + + // Validate input + if (!m_nav->isValidPolyRef(startRef) || !m_nav->isValidPolyRef(endRef) || + !startPos || !dtVisfinite(startPos) || + !endPos || !dtVisfinite(endPos) || + !filter || !path || maxPath <= 0) + { + return DT_FAILURE | DT_INVALID_PARAM; + } + + 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; + + bool outOfNodes = false; + + 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; + + // deal explicitly with crossing tile boundaries + unsigned char crossSide = 0; + if (bestTile->links[i].side != 0xff) + crossSide = bestTile->links[i].side >> 1; + + // get the node + dtNode* neighbourNode = m_nodePool->getNode(neighbourRef, crossSide); + if (!neighbourNode) + { + outOfNodes = true; + 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 = (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; + } + } + } + + dtStatus status = getPathToNode(lastBestNode, path, pathCount, maxPath); + + if (lastBestNode->id != endRef) + status |= DT_PARTIAL_RESULT; + + if (outOfNodes) + status |= DT_OUT_OF_NODES; + + return status; } dtStatus dtNavMeshQuery::getPathToNode(dtNode* endNode, dtPolyRef* path, int* pathCount, int maxPath) const { - // Find the length of the entire path. - dtNode* curNode = endNode; - int length = 0; - do - { - length++; - curNode = m_nodePool->getNodeAtIdx(curNode->pidx); - } while (curNode); - - // If the path cannot be fully stored then advance to the last node we will be able to store. - curNode = endNode; - int writeCount; - for (writeCount = length; writeCount > maxPath; writeCount--) - { - dtAssert(curNode); - - curNode = m_nodePool->getNodeAtIdx(curNode->pidx); - } - - // Write path - for (int i = writeCount - 1; i >= 0; i--) - { - dtAssert(curNode); - - path[i] = curNode->id; - curNode = m_nodePool->getNodeAtIdx(curNode->pidx); - } - - dtAssert(!curNode); - - *pathCount = dtMin(length, maxPath); - - if (length > maxPath) - return DT_SUCCESS | DT_BUFFER_TOO_SMALL; - - return DT_SUCCESS; + // Find the length of the entire path. + dtNode* curNode = endNode; + int length = 0; + do + { + length++; + curNode = m_nodePool->getNodeAtIdx(curNode->pidx); + } while (curNode); + + // If the path cannot be fully stored then advance to the last node we will be able to store. + curNode = endNode; + int writeCount; + for (writeCount = length; writeCount > maxPath; writeCount--) + { + dtAssert(curNode); + + curNode = m_nodePool->getNodeAtIdx(curNode->pidx); + } + + // Write path + for (int i = writeCount - 1; i >= 0; i--) + { + dtAssert(curNode); + + path[i] = curNode->id; + curNode = m_nodePool->getNodeAtIdx(curNode->pidx); + } + + dtAssert(!curNode); + + *pathCount = dtMin(length, maxPath); + + if (length > maxPath) + return DT_SUCCESS | DT_BUFFER_TOO_SMALL; + + return DT_SUCCESS; } + /// @par /// -/// @warning Calling any non-slice methods before calling finalizeSlicedFindPath() +/// @warning Calling any non-slice methods before calling finalizeSlicedFindPath() /// or finalizeSlicedFindPathPartial() may result in corrupted data! /// /// The @p filter pointer is stored and used for the duration of the sliced /// path query. /// dtStatus dtNavMeshQuery::initSlicedFindPath(dtPolyRef startRef, dtPolyRef endRef, - const float* startPos, const float* endPos, - const dtQueryFilter* filter, const unsigned int options) + const float* startPos, const float* endPos, + const dtQueryFilter* filter, const unsigned int options) { - 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; - if (startPos) - dtVcopy(m_query.startPos, startPos); - if (endPos) - dtVcopy(m_query.endPos, endPos); - m_query.filter = filter; - m_query.options = options; - m_query.raycastLimitSqr = FLT_MAX; - - // Validate input - if (!m_nav->isValidPolyRef(startRef) || !m_nav->isValidPolyRef(endRef) || - !startPos || !dtVisfinite(startPos) || - !endPos || !dtVisfinite(endPos) || !filter) - { - return DT_FAILURE | DT_INVALID_PARAM; - } - - // trade quality with performance? - if (options & DT_FINDPATH_ANY_ANGLE) - { - // limiting to several times the character radius yields nice results. It is not sensitive - // so it is enough to compute it from the first tile. - const dtMeshTile* tile = m_nav->getTileByRef(startRef); - float agentRadius = tile->header->walkableRadius; - m_query.raycastLimitSqr = dtSqr(agentRadius * DT_RAY_CAST_LIMIT_PROPORTIONS); - } - - 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; + 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; + if (startPos) + dtVcopy(m_query.startPos, startPos); + if (endPos) + dtVcopy(m_query.endPos, endPos); + m_query.filter = filter; + m_query.options = options; + m_query.raycastLimitSqr = FLT_MAX; + + // Validate input + if (!m_nav->isValidPolyRef(startRef) || !m_nav->isValidPolyRef(endRef) || + !startPos || !dtVisfinite(startPos) || + !endPos || !dtVisfinite(endPos) || !filter) + { + return DT_FAILURE | DT_INVALID_PARAM; + } + + // trade quality with performance? + if (options & DT_FINDPATH_ANY_ANGLE) + { + // limiting to several times the character radius yields nice results. It is not sensitive + // so it is enough to compute it from the first tile. + const dtMeshTile* tile = m_nav->getTileByRef(startRef); + float agentRadius = tile->header->walkableRadius; + m_query.raycastLimitSqr = dtSqr(agentRadius * DT_RAY_CAST_LIMIT_PROPORTIONS); + } + + 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, int* doneIters) { - if (!dtStatusInProgress(m_query.status)) - 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; - } - - dtRaycastHit rayHit; - rayHit.maxPath = 0; - - 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; - const dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK; - m_query.status = DT_SUCCESS | details; - if (doneIters) - *doneIters = iter; - 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 (dtStatusFailed(m_nav->getTileAndPolyByRef(bestRef, &bestTile, &bestPoly))) - { - // The polygon has disappeared during the sliced query, fail. - m_query.status = DT_FAILURE; - if (doneIters) - *doneIters = iter; - return m_query.status; - } - - // Get parent and grand parent poly and tile. - dtPolyRef parentRef = 0, grandpaRef = 0; - const dtMeshTile* parentTile = 0; - const dtPoly* parentPoly = 0; - dtNode* parentNode = 0; - if (bestNode->pidx) - { - parentNode = m_nodePool->getNodeAtIdx(bestNode->pidx); - parentRef = parentNode->id; - if (parentNode->pidx) - grandpaRef = m_nodePool->getNodeAtIdx(parentNode->pidx)->id; - } - if (parentRef) - { - bool invalidParent = dtStatusFailed(m_nav->getTileAndPolyByRef(parentRef, &parentTile, &parentPoly)); - if (invalidParent || (grandpaRef && !m_nav->isValidPolyRef(grandpaRef))) - { - // The polygon has disappeared during the sliced query, fail. - m_query.status = DT_FAILURE; - if (doneIters) - *doneIters = iter; - return m_query.status; - } - } - - // decide whether to test raycast to previous nodes - bool tryLOS = false; - if (m_query.options & DT_FINDPATH_ANY_ANGLE) - { - if ((parentRef != 0) && (dtVdistSqr(parentNode->pos, bestNode->pos) < m_query.raycastLimitSqr)) - tryLOS = true; - } - - 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; - - // get the neighbor node - dtNode* neighbourNode = m_nodePool->getNode(neighbourRef, 0); - if (!neighbourNode) - { - m_query.status |= DT_OUT_OF_NODES; - continue; - } - - // do not expand to nodes that were already visited from the same parent - if (neighbourNode->pidx != 0 && neighbourNode->pidx == bestNode->pidx) - 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; - - // raycast parent - bool foundShortCut = false; - rayHit.pathCost = rayHit.t = 0; - if (tryLOS) - { - raycast(parentRef, parentNode->pos, neighbourNode->pos, m_query.filter, DT_RAYCAST_USE_COSTS, &rayHit, grandpaRef); - foundShortCut = rayHit.t >= 1.0f; - } - - // update move cost - if (foundShortCut) - { - // shortcut found using raycast. Using shorter cost instead - cost = parentNode->cost + rayHit.pathCost; - } - else - { - // No shortcut found. - const float curCost = m_query.filter->getCost(bestNode->pos, neighbourNode->pos, - parentRef, parentTile, parentPoly, - bestRef, bestTile, bestPoly, - neighbourRef, neighbourTile, neighbourPoly); - cost = bestNode->cost + curCost; - } - - // Special case for last node. - if (neighbourRef == m_query.endRef) - { - const float endCost = m_query.filter->getCost(neighbourNode->pos, m_query.endPos, - bestRef, bestTile, bestPoly, - neighbourRef, neighbourTile, neighbourPoly, - 0, 0, 0); - - cost = cost + endCost; - heuristic = 0; - } - else - { - 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 = foundShortCut ? bestNode->pidx : m_nodePool->getNodeIdx(bestNode); - neighbourNode->id = neighbourRef; - neighbourNode->flags = (neighbourNode->flags & ~(DT_NODE_CLOSED | DT_NODE_PARENT_DETACHED)); - neighbourNode->cost = cost; - neighbourNode->total = total; - if (foundShortCut) - neighbourNode->flags = (neighbourNode->flags | DT_NODE_PARENT_DETACHED); - - 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()) - { - const dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK; - m_query.status = DT_SUCCESS | details; - } - - if (doneIters) - *doneIters = iter; - - return m_query.status; + if (!dtStatusInProgress(m_query.status)) + 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; + } + + dtRaycastHit rayHit; + rayHit.maxPath = 0; + + 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; + const dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK; + m_query.status = DT_SUCCESS | details; + if (doneIters) + *doneIters = iter; + 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 (dtStatusFailed(m_nav->getTileAndPolyByRef(bestRef, &bestTile, &bestPoly))) + { + // The polygon has disappeared during the sliced query, fail. + m_query.status = DT_FAILURE; + if (doneIters) + *doneIters = iter; + return m_query.status; + } + + // Get parent and grand parent poly and tile. + dtPolyRef parentRef = 0, grandpaRef = 0; + const dtMeshTile* parentTile = 0; + const dtPoly* parentPoly = 0; + dtNode* parentNode = 0; + if (bestNode->pidx) + { + parentNode = m_nodePool->getNodeAtIdx(bestNode->pidx); + parentRef = parentNode->id; + if (parentNode->pidx) + grandpaRef = m_nodePool->getNodeAtIdx(parentNode->pidx)->id; + } + if (parentRef) + { + bool invalidParent = dtStatusFailed(m_nav->getTileAndPolyByRef(parentRef, &parentTile, &parentPoly)); + if (invalidParent || (grandpaRef && !m_nav->isValidPolyRef(grandpaRef)) ) + { + // The polygon has disappeared during the sliced query, fail. + m_query.status = DT_FAILURE; + if (doneIters) + *doneIters = iter; + return m_query.status; + } + } + + // decide whether to test raycast to previous nodes + bool tryLOS = false; + if (m_query.options & DT_FINDPATH_ANY_ANGLE) + { + if ((parentRef != 0) && (dtVdistSqr(parentNode->pos, bestNode->pos) < m_query.raycastLimitSqr)) + tryLOS = true; + } + + 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; + + // get the neighbor node + dtNode* neighbourNode = m_nodePool->getNode(neighbourRef, 0); + if (!neighbourNode) + { + m_query.status |= DT_OUT_OF_NODES; + continue; + } + + // do not expand to nodes that were already visited from the same parent + if (neighbourNode->pidx != 0 && neighbourNode->pidx == bestNode->pidx) + 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; + + // raycast parent + bool foundShortCut = false; + rayHit.pathCost = rayHit.t = 0; + if (tryLOS) + { + raycast(parentRef, parentNode->pos, neighbourNode->pos, m_query.filter, DT_RAYCAST_USE_COSTS, &rayHit, grandpaRef); + foundShortCut = rayHit.t >= 1.0f; + } + + // update move cost + if (foundShortCut) + { + // shortcut found using raycast. Using shorter cost instead + cost = parentNode->cost + rayHit.pathCost; + } + else + { + // No shortcut found. + const float curCost = m_query.filter->getCost(bestNode->pos, neighbourNode->pos, + parentRef, parentTile, parentPoly, + bestRef, bestTile, bestPoly, + neighbourRef, neighbourTile, neighbourPoly); + cost = bestNode->cost + curCost; + } + + // Special case for last node. + if (neighbourRef == m_query.endRef) + { + const float endCost = m_query.filter->getCost(neighbourNode->pos, m_query.endPos, + bestRef, bestTile, bestPoly, + neighbourRef, neighbourTile, neighbourPoly, + 0, 0, 0); + + cost = cost + endCost; + heuristic = 0; + } + else + { + 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 = foundShortCut ? bestNode->pidx : m_nodePool->getNodeIdx(bestNode); + neighbourNode->id = neighbourRef; + neighbourNode->flags = (neighbourNode->flags & ~(DT_NODE_CLOSED | DT_NODE_PARENT_DETACHED)); + neighbourNode->cost = cost; + neighbourNode->total = total; + if (foundShortCut) + neighbourNode->flags = (neighbourNode->flags | DT_NODE_PARENT_DETACHED); + + 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()) + { + const dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK; + m_query.status = DT_SUCCESS | details; + } + + if (doneIters) + *doneIters = iter; + + return m_query.status; } dtStatus dtNavMeshQuery::finalizeSlicedFindPath(dtPolyRef* path, int* pathCount, const int maxPath) { - if (!pathCount) - return DT_FAILURE | DT_INVALID_PARAM; - - *pathCount = 0; - - if (!path || maxPath <= 0) - return DT_FAILURE | DT_INVALID_PARAM; - - if (dtStatusFailed(m_query.status)) - { - // 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); - - if (m_query.lastBestNode->id != m_query.endRef) - m_query.status |= DT_PARTIAL_RESULT; - - dtNode* prev = 0; - dtNode* node = m_query.lastBestNode; - int prevRay = 0; - do - { - dtNode* next = m_nodePool->getNodeAtIdx(node->pidx); - node->pidx = m_nodePool->getNodeIdx(prev); - prev = node; - int nextRay = node->flags & DT_NODE_PARENT_DETACHED; // keep track of whether parent is not adjacent (i.e. due to raycast shortcut) - node->flags = (node->flags & ~DT_NODE_PARENT_DETACHED) | prevRay; // and store it in the reversed path's node - prevRay = nextRay; - node = next; - } while (node); - - // Store path - node = prev; - do - { - dtNode* next = m_nodePool->getNodeAtIdx(node->pidx); - dtStatus status = 0; - if (node->flags & DT_NODE_PARENT_DETACHED) - { - float t, normal[3]; - int m; - status = raycast(node->id, node->pos, next->pos, m_query.filter, &t, normal, path + n, &m, maxPath - n); - n += m; - // raycast ends on poly boundary and the path might include the next poly boundary. - if (path[n - 1] == next->id) - n--; // remove to avoid duplicates - } - else - { - path[n++] = node->id; - if (n >= maxPath) - status = DT_BUFFER_TOO_SMALL; - } - - if (status & DT_STATUS_DETAIL_MASK) - { - m_query.status |= status & DT_STATUS_DETAIL_MASK; - break; - } - node = next; - } while (node); - } - - const dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK; - - // Reset query. - memset(&m_query, 0, sizeof(dtQueryData)); - - *pathCount = n; - - return DT_SUCCESS | details; + if (!pathCount) + return DT_FAILURE | DT_INVALID_PARAM; + + *pathCount = 0; + + if (!path || maxPath <= 0) + return DT_FAILURE | DT_INVALID_PARAM; + + if (dtStatusFailed(m_query.status)) + { + // 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); + + if (m_query.lastBestNode->id != m_query.endRef) + m_query.status |= DT_PARTIAL_RESULT; + + dtNode* prev = 0; + dtNode* node = m_query.lastBestNode; + int prevRay = 0; + do + { + dtNode* next = m_nodePool->getNodeAtIdx(node->pidx); + node->pidx = m_nodePool->getNodeIdx(prev); + prev = node; + int nextRay = node->flags & DT_NODE_PARENT_DETACHED; // keep track of whether parent is not adjacent (i.e. due to raycast shortcut) + node->flags = (node->flags & ~DT_NODE_PARENT_DETACHED) | prevRay; // and store it in the reversed path's node + prevRay = nextRay; + node = next; + } + while (node); + + // Store path + node = prev; + do + { + dtNode* next = m_nodePool->getNodeAtIdx(node->pidx); + dtStatus status = 0; + if (node->flags & DT_NODE_PARENT_DETACHED) + { + float t, normal[3]; + int m; + status = raycast(node->id, node->pos, next->pos, m_query.filter, &t, normal, path+n, &m, maxPath-n); + n += m; + // raycast ends on poly boundary and the path might include the next poly boundary. + if (path[n-1] == next->id) + n--; // remove to avoid duplicates + } + else + { + path[n++] = node->id; + if (n >= maxPath) + status = DT_BUFFER_TOO_SMALL; + } + + if (status & DT_STATUS_DETAIL_MASK) + { + m_query.status |= status & DT_STATUS_DETAIL_MASK; + break; + } + node = next; + } + while (node); + } + + const dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK; + + // Reset query. + memset(&m_query, 0, sizeof(dtQueryData)); + + *pathCount = n; + + return DT_SUCCESS | details; } dtStatus dtNavMeshQuery::finalizeSlicedFindPathPartial(const dtPolyRef* existing, const int existingSize, - dtPolyRef* path, int* pathCount, const int maxPath) + dtPolyRef* path, int* pathCount, const int maxPath) { - if (!pathCount) - return DT_FAILURE | DT_INVALID_PARAM; - - *pathCount = 0; - - if (!existing || existingSize <= 0 || !path || !pathCount || maxPath <= 0) - return DT_FAILURE | DT_INVALID_PARAM; - - if (dtStatusFailed(m_query.status)) - { - // 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) - { - m_nodePool->findNodes(existing[i], &node, 1); - if (node) - break; - } - - if (!node) - { - m_query.status |= DT_PARTIAL_RESULT; - dtAssert(m_query.lastBestNode); - node = m_query.lastBestNode; - } - - // Reverse the path. - int prevRay = 0; - do - { - dtNode* next = m_nodePool->getNodeAtIdx(node->pidx); - node->pidx = m_nodePool->getNodeIdx(prev); - prev = node; - int nextRay = node->flags & DT_NODE_PARENT_DETACHED; // keep track of whether parent is not adjacent (i.e. due to raycast shortcut) - node->flags = (node->flags & ~DT_NODE_PARENT_DETACHED) | prevRay; // and store it in the reversed path's node - prevRay = nextRay; - node = next; - } while (node); - - // Store path - node = prev; - do - { - dtNode* next = m_nodePool->getNodeAtIdx(node->pidx); - dtStatus status = 0; - if (node->flags & DT_NODE_PARENT_DETACHED) - { - float t, normal[3]; - int m; - status = raycast(node->id, node->pos, next->pos, m_query.filter, &t, normal, path + n, &m, maxPath - n); - n += m; - // raycast ends on poly boundary and the path might include the next poly boundary. - if (path[n - 1] == next->id) - n--; // remove to avoid duplicates - } - else - { - path[n++] = node->id; - if (n >= maxPath) - status = DT_BUFFER_TOO_SMALL; - } - - if (status & DT_STATUS_DETAIL_MASK) - { - m_query.status |= status & DT_STATUS_DETAIL_MASK; - break; - } - node = next; - } while (node); - } - - const dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK; - - // Reset query. - memset(&m_query, 0, sizeof(dtQueryData)); - - *pathCount = n; - - return DT_SUCCESS | details; + if (!pathCount) + return DT_FAILURE | DT_INVALID_PARAM; + + *pathCount = 0; + + if (!existing || existingSize <= 0 || !path || !pathCount || maxPath <= 0) + return DT_FAILURE | DT_INVALID_PARAM; + + if (dtStatusFailed(m_query.status)) + { + // 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) + { + m_nodePool->findNodes(existing[i], &node, 1); + if (node) + break; + } + + if (!node) + { + m_query.status |= DT_PARTIAL_RESULT; + dtAssert(m_query.lastBestNode); + node = m_query.lastBestNode; + } + + // Reverse the path. + int prevRay = 0; + do + { + dtNode* next = m_nodePool->getNodeAtIdx(node->pidx); + node->pidx = m_nodePool->getNodeIdx(prev); + prev = node; + int nextRay = node->flags & DT_NODE_PARENT_DETACHED; // keep track of whether parent is not adjacent (i.e. due to raycast shortcut) + node->flags = (node->flags & ~DT_NODE_PARENT_DETACHED) | prevRay; // and store it in the reversed path's node + prevRay = nextRay; + node = next; + } + while (node); + + // Store path + node = prev; + do + { + dtNode* next = m_nodePool->getNodeAtIdx(node->pidx); + dtStatus status = 0; + if (node->flags & DT_NODE_PARENT_DETACHED) + { + float t, normal[3]; + int m; + status = raycast(node->id, node->pos, next->pos, m_query.filter, &t, normal, path+n, &m, maxPath-n); + n += m; + // raycast ends on poly boundary and the path might include the next poly boundary. + if (path[n-1] == next->id) + n--; // remove to avoid duplicates + } + else + { + path[n++] = node->id; + if (n >= maxPath) + status = DT_BUFFER_TOO_SMALL; + } + + if (status & DT_STATUS_DETAIL_MASK) + { + m_query.status |= status & DT_STATUS_DETAIL_MASK; + break; + } + node = next; + } + while (node); + } + + const dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK; + + // Reset query. + memset(&m_query, 0, sizeof(dtQueryData)); + + *pathCount = n; + + return DT_SUCCESS | details; } + dtStatus dtNavMeshQuery::appendVertex(const float* pos, const unsigned char flags, const dtPolyRef ref, - float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs, - int* straightPathCount, const int maxStraightPath) const + float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs, + int* straightPathCount, const int maxStraightPath) const { - if ((*straightPathCount) > 0 && dtVequal(&straightPath[((*straightPathCount) - 1) * 3], pos)) - { - // The vertices are equal, update flags and poly. - if (straightPathFlags) - straightPathFlags[(*straightPathCount) - 1] = flags; - if (straightPathRefs) - straightPathRefs[(*straightPathCount) - 1] = ref; - } - else - { - // Append new vertex. - dtVcopy(&straightPath[(*straightPathCount) * 3], pos); - if (straightPathFlags) - straightPathFlags[(*straightPathCount)] = flags; - if (straightPathRefs) - straightPathRefs[(*straightPathCount)] = ref; - (*straightPathCount)++; - - // If there is no space to append more vertices, return. - if ((*straightPathCount) >= maxStraightPath) - { - return DT_SUCCESS | DT_BUFFER_TOO_SMALL; - } - - // If reached end of path, return. - if (flags == DT_STRAIGHTPATH_END) - { - return DT_SUCCESS; - } - } - return DT_IN_PROGRESS; + if ((*straightPathCount) > 0 && dtVequal(&straightPath[((*straightPathCount)-1)*3], pos)) + { + // The vertices are equal, update flags and poly. + if (straightPathFlags) + straightPathFlags[(*straightPathCount)-1] = flags; + if (straightPathRefs) + straightPathRefs[(*straightPathCount)-1] = ref; + } + else + { + // Append new vertex. + dtVcopy(&straightPath[(*straightPathCount)*3], pos); + if (straightPathFlags) + straightPathFlags[(*straightPathCount)] = flags; + if (straightPathRefs) + straightPathRefs[(*straightPathCount)] = ref; + (*straightPathCount)++; + + // If there is no space to append more vertices, return. + if ((*straightPathCount) >= maxStraightPath) + { + return DT_SUCCESS | DT_BUFFER_TOO_SMALL; + } + + // If reached end of path, return. + if (flags == DT_STRAIGHTPATH_END) + { + return DT_SUCCESS; + } + } + return DT_IN_PROGRESS; } dtStatus dtNavMeshQuery::appendPortals(const int startIdx, const int endIdx, const float* endPos, const dtPolyRef* path, - float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs, - int* straightPathCount, const int maxStraightPath, const int options) const + float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs, + int* straightPathCount, const int maxStraightPath, const int options) const { - const float* startPos = &straightPath[(*straightPathCount - 1) * 3]; - // Append or update last vertex - dtStatus stat = 0; - for (int i = startIdx; i < endIdx; i++) - { - // Calculate portal - const dtPolyRef from = path[i]; - const dtMeshTile* fromTile = 0; - const dtPoly* fromPoly = 0; - if (dtStatusFailed(m_nav->getTileAndPolyByRef(from, &fromTile, &fromPoly))) - return DT_FAILURE | DT_INVALID_PARAM; - - const dtPolyRef to = path[i + 1]; - const dtMeshTile* toTile = 0; - const dtPoly* toPoly = 0; - if (dtStatusFailed(m_nav->getTileAndPolyByRef(to, &toTile, &toPoly))) - return DT_FAILURE | DT_INVALID_PARAM; - - float left[3], right[3]; - if (dtStatusFailed(getPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, left, right))) - break; - - if (options & DT_STRAIGHTPATH_AREA_CROSSINGS) - { - // Skip intersection if only area crossings are requested. - if (fromPoly->getArea() == toPoly->getArea()) - continue; - } - - // Append intersection - float s, t; - if (dtIntersectSegSeg2D(startPos, endPos, left, right, s, t)) - { - float pt[3]; - dtVlerp(pt, left, right, t); - - stat = appendVertex(pt, 0, path[i + 1], - straightPath, straightPathFlags, straightPathRefs, - straightPathCount, maxStraightPath); - if (stat != DT_IN_PROGRESS) - return stat; - } - } - return DT_IN_PROGRESS; + const float* startPos = &straightPath[(*straightPathCount-1)*3]; + // Append or update last vertex + dtStatus stat = 0; + for (int i = startIdx; i < endIdx; i++) + { + // Calculate portal + const dtPolyRef from = path[i]; + const dtMeshTile* fromTile = 0; + const dtPoly* fromPoly = 0; + if (dtStatusFailed(m_nav->getTileAndPolyByRef(from, &fromTile, &fromPoly))) + return DT_FAILURE | DT_INVALID_PARAM; + + const dtPolyRef to = path[i+1]; + const dtMeshTile* toTile = 0; + const dtPoly* toPoly = 0; + if (dtStatusFailed(m_nav->getTileAndPolyByRef(to, &toTile, &toPoly))) + return DT_FAILURE | DT_INVALID_PARAM; + + float left[3], right[3]; + if (dtStatusFailed(getPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, left, right))) + break; + + if (options & DT_STRAIGHTPATH_AREA_CROSSINGS) + { + // Skip intersection if only area crossings are requested. + if (fromPoly->getArea() == toPoly->getArea()) + continue; + } + + // Append intersection + float s,t; + if (dtIntersectSegSeg2D(startPos, endPos, left, right, s, t)) + { + float pt[3]; + dtVlerp(pt, left,right, t); + + stat = appendVertex(pt, 0, path[i+1], + straightPath, straightPathFlags, straightPathRefs, + straightPathCount, maxStraightPath); + if (stat != DT_IN_PROGRESS) + return stat; + } + } + return DT_IN_PROGRESS; } /// @par -/// +/// /// This method peforms what is often called 'string pulling'. /// -/// The start position is clamped to the first polygon in the path, and the -/// end position is clamped to the last. So the start and end positions should +/// The start position is clamped to the first polygon in the path, and the +/// end position is clamped to the last. So the start and end positions should /// normally be within or very near the first and last polygons respectively. /// -/// The returned polygon references represent the reference id of the polygon -/// that is entered at the associated path position. The reference id associated -/// with the end point will always be zero. This allows, for example, matching +/// The returned polygon references represent the reference id of the polygon +/// that is entered at the associated path position. The reference id associated +/// with the end point will always be zero. This allows, for example, matching /// off-mesh link points to their representative polygons. /// -/// If the provided result buffers are too small for the entire result set, -/// they will be filled as far as possible from the start toward the end +/// If the provided result buffers are too small for the entire result set, +/// they will be filled as far as possible from the start toward the end /// position. /// 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 int options) const + const dtPolyRef* path, const int pathSize, + float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs, + int* straightPathCount, const int maxStraightPath, const int options) const { - dtAssert(m_nav); - - if (!straightPathCount) - return DT_FAILURE | DT_INVALID_PARAM; - - *straightPathCount = 0; - - if (!startPos || !dtVisfinite(startPos) || - !endPos || !dtVisfinite(endPos) || - !path || pathSize <= 0 || !path[0] || - maxStraightPath <= 0) - { - return DT_FAILURE | DT_INVALID_PARAM; - } - - dtStatus stat = 0; - - // TODO: Should this be callers responsibility? - float closestStartPos[3]; - if (dtStatusFailed(closestPointOnPolyBoundary(path[0], startPos, closestStartPos))) - return DT_FAILURE | DT_INVALID_PARAM; - - float closestEndPos[3]; - if (dtStatusFailed(closestPointOnPolyBoundary(path[pathSize - 1], endPos, closestEndPos))) - return DT_FAILURE | DT_INVALID_PARAM; - - // Add start point. - stat = appendVertex(closestStartPos, DT_STRAIGHTPATH_START, path[0], - straightPath, straightPathFlags, straightPathRefs, - straightPathCount, maxStraightPath); - if (stat != DT_IN_PROGRESS) - return stat; - - 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 toType; - - if (i + 1 < pathSize) - { - unsigned char fromType; // fromType is ignored. - - // Next portal. - if (dtStatusFailed(getPortalPoints(path[i], path[i + 1], left, right, fromType, toType))) - { - // Failed to get portal points, in practice this means that path[i+1] is invalid polygon. - // Clamp the end point to path[i], and return the path so far. - - if (dtStatusFailed(closestPointOnPolyBoundary(path[i], endPos, closestEndPos))) - { - // This should only happen when the first polygon is invalid. - return DT_FAILURE | DT_INVALID_PARAM; - } - - // Apeend portals along the current straight path segment. - if (options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS)) - { - // Ignore status return value as we're just about to return anyway. - appendPortals(apexIndex, i, closestEndPos, path, - straightPath, straightPathFlags, straightPathRefs, - straightPathCount, maxStraightPath, options); - } - - // Ignore status return value as we're just about to return anyway. - appendVertex(closestEndPos, 0, path[i], - straightPath, straightPathFlags, straightPathRefs, - straightPathCount, maxStraightPath); - - return DT_SUCCESS | DT_PARTIAL_RESULT | ((*straightPathCount >= maxStraightPath) ? DT_BUFFER_TOO_SMALL : 0); - } - - // 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); - - 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 - { - // Append portals along the current straight path segment. - if (options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS)) - { - stat = appendPortals(apexIndex, leftIndex, portalLeft, path, - straightPath, straightPathFlags, straightPathRefs, - straightPathCount, maxStraightPath, options); - if (stat != DT_IN_PROGRESS) - return stat; - } - - 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; - - // Append or update vertex - stat = appendVertex(portalApex, flags, ref, - straightPath, straightPathFlags, straightPathRefs, - straightPathCount, maxStraightPath); - if (stat != DT_IN_PROGRESS) - return stat; - - 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 - { - // Append portals along the current straight path segment. - if (options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS)) - { - stat = appendPortals(apexIndex, rightIndex, portalRight, path, - straightPath, straightPathFlags, straightPathRefs, - straightPathCount, maxStraightPath, options); - if (stat != DT_IN_PROGRESS) - return stat; - } - - 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; - - // Append or update vertex - stat = appendVertex(portalApex, flags, ref, - straightPath, straightPathFlags, straightPathRefs, - straightPathCount, maxStraightPath); - if (stat != DT_IN_PROGRESS) - return stat; - - dtVcopy(portalLeft, portalApex); - dtVcopy(portalRight, portalApex); - leftIndex = apexIndex; - rightIndex = apexIndex; - - // Restart - i = apexIndex; - - continue; - } - } - } - - // Append portals along the current straight path segment. - if (options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS)) - { - stat = appendPortals(apexIndex, pathSize - 1, closestEndPos, path, - straightPath, straightPathFlags, straightPathRefs, - straightPathCount, maxStraightPath, options); - if (stat != DT_IN_PROGRESS) - return stat; - } - } - - // Ignore status return value as we're just about to return anyway. - appendVertex(closestEndPos, DT_STRAIGHTPATH_END, 0, - straightPath, straightPathFlags, straightPathRefs, - straightPathCount, maxStraightPath); - - return DT_SUCCESS | ((*straightPathCount >= maxStraightPath) ? DT_BUFFER_TOO_SMALL : 0); + dtAssert(m_nav); + + if (!straightPathCount) + return DT_FAILURE | DT_INVALID_PARAM; + + *straightPathCount = 0; + + if (!startPos || !dtVisfinite(startPos) || + !endPos || !dtVisfinite(endPos) || + !path || pathSize <= 0 || !path[0] || + maxStraightPath <= 0) + { + return DT_FAILURE | DT_INVALID_PARAM; + } + + dtStatus stat = 0; + + // TODO: Should this be callers responsibility? + float closestStartPos[3]; + if (dtStatusFailed(closestPointOnPolyBoundary(path[0], startPos, closestStartPos))) + return DT_FAILURE | DT_INVALID_PARAM; + + float closestEndPos[3]; + if (dtStatusFailed(closestPointOnPolyBoundary(path[pathSize-1], endPos, closestEndPos))) + return DT_FAILURE | DT_INVALID_PARAM; + + // Add start point. + stat = appendVertex(closestStartPos, DT_STRAIGHTPATH_START, path[0], + straightPath, straightPathFlags, straightPathRefs, + straightPathCount, maxStraightPath); + if (stat != DT_IN_PROGRESS) + return stat; + + 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 toType; + + if (i+1 < pathSize) + { + unsigned char fromType; // fromType is ignored. + + // Next portal. + if (dtStatusFailed(getPortalPoints(path[i], path[i+1], left, right, fromType, toType))) + { + // Failed to get portal points, in practice this means that path[i+1] is invalid polygon. + // Clamp the end point to path[i], and return the path so far. + + if (dtStatusFailed(closestPointOnPolyBoundary(path[i], endPos, closestEndPos))) + { + // This should only happen when the first polygon is invalid. + return DT_FAILURE | DT_INVALID_PARAM; + } + + // Apeend portals along the current straight path segment. + if (options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS)) + { + // Ignore status return value as we're just about to return anyway. + appendPortals(apexIndex, i, closestEndPos, path, + straightPath, straightPathFlags, straightPathRefs, + straightPathCount, maxStraightPath, options); + } + + // Ignore status return value as we're just about to return anyway. + appendVertex(closestEndPos, 0, path[i], + straightPath, straightPathFlags, straightPathRefs, + straightPathCount, maxStraightPath); + + return DT_SUCCESS | DT_PARTIAL_RESULT | ((*straightPathCount >= maxStraightPath) ? DT_BUFFER_TOO_SMALL : 0); + } + + // 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); + + 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 + { + // Append portals along the current straight path segment. + if (options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS)) + { + stat = appendPortals(apexIndex, leftIndex, portalLeft, path, + straightPath, straightPathFlags, straightPathRefs, + straightPathCount, maxStraightPath, options); + if (stat != DT_IN_PROGRESS) + return stat; + } + + 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; + + // Append or update vertex + stat = appendVertex(portalApex, flags, ref, + straightPath, straightPathFlags, straightPathRefs, + straightPathCount, maxStraightPath); + if (stat != DT_IN_PROGRESS) + return stat; + + 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 + { + // Append portals along the current straight path segment. + if (options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS)) + { + stat = appendPortals(apexIndex, rightIndex, portalRight, path, + straightPath, straightPathFlags, straightPathRefs, + straightPathCount, maxStraightPath, options); + if (stat != DT_IN_PROGRESS) + return stat; + } + + 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; + + // Append or update vertex + stat = appendVertex(portalApex, flags, ref, + straightPath, straightPathFlags, straightPathRefs, + straightPathCount, maxStraightPath); + if (stat != DT_IN_PROGRESS) + return stat; + + dtVcopy(portalLeft, portalApex); + dtVcopy(portalRight, portalApex); + leftIndex = apexIndex; + rightIndex = apexIndex; + + // Restart + i = apexIndex; + + continue; + } + } + } + + // Append portals along the current straight path segment. + if (options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS)) + { + stat = appendPortals(apexIndex, pathSize-1, closestEndPos, path, + straightPath, straightPathFlags, straightPathRefs, + straightPathCount, maxStraightPath, options); + if (stat != DT_IN_PROGRESS) + return stat; + } + } + + // Ignore status return value as we're just about to return anyway. + appendVertex(closestEndPos, DT_STRAIGHTPATH_END, 0, + straightPath, straightPathFlags, straightPathRefs, + straightPathCount, maxStraightPath); + + return DT_SUCCESS | ((*straightPathCount >= maxStraightPath) ? DT_BUFFER_TOO_SMALL : 0); } /// @par /// -/// This method is optimized for small delta movement and a small number of -/// polygons. If used for too great a distance, the result set will form an +/// This method is optimized for small delta movement and a small number of +/// polygons. If used for too great a distance, the result set will form an /// incomplete path. /// -/// @p resultPos will equal the @p endPos if the end is reached. +/// @p resultPos will equal the @p endPos if the end is reached. /// Otherwise the closest reachable position will be returned. -/// -/// @p resultPos is not projected onto the surface of the navigation +/// +/// @p resultPos is not projected onto the surface of the navigation /// mesh. Use #getPolyHeight if this is needed. /// -/// This method treats the end position in the same manner as -/// the #raycast method. (As a 2D point.) See that method's documentation +/// This method treats the end position in the same manner as +/// the #raycast method. (As a 2D point.) See that method's documentation /// for details. -/// -/// If the @p visited array is too small to hold the entire result set, it will -/// be filled as far as possible from the start position toward the end +/// +/// If the @p visited array is too small to hold the entire result set, it will +/// be filled as far as possible from the start position toward the end /// position. /// dtStatus dtNavMeshQuery::moveAlongSurface(dtPolyRef startRef, const float* startPos, const float* endPos, - const dtQueryFilter* filter, - float* resultPos, dtPolyRef* visited, int* visitedCount, const int maxVisitedSize) const + const dtQueryFilter* filter, + float* resultPos, dtPolyRef* visited, int* visitedCount, const int maxVisitedSize) const { - dtAssert(m_nav); - dtAssert(m_tinyNodePool); - - if (!visitedCount) - return DT_FAILURE | DT_INVALID_PARAM; - - *visitedCount = 0; - - if (!m_nav->isValidPolyRef(startRef) || - !startPos || !dtVisfinite(startPos) || - !endPos || !dtVisfinite(endPos) || - !filter || !resultPos || !visited || - maxVisitedSize <= 0) - { - return DT_FAILURE | DT_INVALID_PARAM; - } - - dtStatus status = DT_SUCCESS; - - 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) - { + dtAssert(m_nav); + dtAssert(m_tinyNodePool); + + if (!visitedCount) + return DT_FAILURE | DT_INVALID_PARAM; + + *visitedCount = 0; + + if (!m_nav->isValidPolyRef(startRef) || + !startPos || !dtVisfinite(startPos) || + !endPos || !dtVisfinite(endPos) || + !filter || !resultPos || !visited || + maxVisitedSize <= 0) + { + return DT_FAILURE | DT_INVALID_PARAM; + } + + dtStatus status = DT_SUCCESS; + + 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; - if (n >= maxVisitedSize) - { - status |= DT_BUFFER_TOO_SMALL; - break; - } - node = m_tinyNodePool->getNodeAtIdx(node->pidx); - } while (node); - } - - dtVcopy(resultPos, bestPos); - - *visitedCount = n; - - return status; + 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; + if (n >= maxVisitedSize) + { + status |= DT_BUFFER_TOO_SMALL; + break; + } + node = m_tinyNodePool->getNodeAtIdx(node->pidx); + } + while (node); + } + + dtVcopy(resultPos, bestPos); + + *visitedCount = n; + + return status; } + dtStatus dtNavMeshQuery::getPortalPoints(dtPolyRef from, dtPolyRef to, float* left, float* right, - unsigned char& fromType, unsigned char& toType) const + unsigned char& fromType, unsigned char& toType) const { - dtAssert(m_nav); - - const dtMeshTile* fromTile = 0; - const dtPoly* fromPoly = 0; - if (dtStatusFailed(m_nav->getTileAndPolyByRef(from, &fromTile, &fromPoly))) - return DT_FAILURE | DT_INVALID_PARAM; - fromType = fromPoly->getType(); - - const dtMeshTile* toTile = 0; - const dtPoly* toPoly = 0; - if (dtStatusFailed(m_nav->getTileAndPolyByRef(to, &toTile, &toPoly))) - return DT_FAILURE | DT_INVALID_PARAM; - toType = toPoly->getType(); - - return getPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, left, right); + dtAssert(m_nav); + + const dtMeshTile* fromTile = 0; + const dtPoly* fromPoly = 0; + if (dtStatusFailed(m_nav->getTileAndPolyByRef(from, &fromTile, &fromPoly))) + return DT_FAILURE | DT_INVALID_PARAM; + fromType = fromPoly->getType(); + + const dtMeshTile* toTile = 0; + const dtPoly* toPoly = 0; + if (dtStatusFailed(m_nav->getTileAndPolyByRef(to, &toTile, &toPoly))) + return DT_FAILURE | DT_INVALID_PARAM; + 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 + 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 | DT_INVALID_PARAM; - - // 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 | DT_INVALID_PARAM; - } - - 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 | DT_INVALID_PARAM; - } - - // 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; + // 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 | DT_INVALID_PARAM; + + // 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 | DT_INVALID_PARAM; + } + + 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 | DT_INVALID_PARAM; + } + + // 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 (dtStatusFailed(getPortalPoints(from, to, left, right, fromType, toType))) - return DT_FAILURE | DT_INVALID_PARAM; - 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; + float left[3], right[3]; + unsigned char fromType, toType; + if (dtStatusFailed(getPortalPoints(from, to, left,right, fromType, toType))) + return DT_FAILURE | DT_INVALID_PARAM; + 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 + dtPolyRef to, const dtPoly* toPoly, const dtMeshTile* toTile, + float* mid) const { - float left[3], right[3]; - if (dtStatusFailed(getPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, left, right))) - return DT_FAILURE | DT_INVALID_PARAM; - 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; + float left[3], right[3]; + if (dtStatusFailed(getPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, left, right))) + return DT_FAILURE | DT_INVALID_PARAM; + 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; } + + /// @par /// /// This method is meant to be used for quick, short distance checks. /// -/// If the path array is too small to hold the result, it will be filled as +/// If the path array is too small to hold the result, it will be filled as /// far as possible from the start postion toward the end position. /// /// Using the Hit Parameter (t) -/// -/// If the hit parameter is a very high value (FLT_MAX), then the ray has hit -/// the end position. In this case the path represents a valid corridor to the +/// +/// If the hit parameter is a very high value (FLT_MAX), then the ray has hit +/// the end position. In this case the path represents a valid corridor to the /// end position and the value of @p hitNormal is undefined. /// -/// If the hit parameter is zero, then the start position is on the wall that +/// If the hit parameter is zero, then the start position is on the wall that /// was hit and the value of @p hitNormal is undefined. /// /// If 0 < t < 1.0 then the following applies: @@ -2351,52 +2392,53 @@ dtStatus dtNavMeshQuery::getEdgeMidPoint(dtPolyRef from, const dtPoly* fromPoly, /// /// Use Case Restriction /// -/// The raycast ignores the y-value of the end position. (2D check.) This +/// The raycast ignores the y-value of the end position. (2D check.) This /// places significant limits on how it can be used. For example: /// -/// Consider a scene where there is a main floor with a second floor balcony -/// that hangs over the main floor. So the first floor mesh extends below the -/// balcony mesh. The start position is somewhere on the first floor. The end +/// Consider a scene where there is a main floor with a second floor balcony +/// that hangs over the main floor. So the first floor mesh extends below the +/// balcony mesh. The start position is somewhere on the first floor. The end /// position is on the balcony. /// -/// The raycast will search toward the end position along the first floor mesh. +/// The raycast will search toward the end position along the first floor mesh. /// If it reaches the end position's xz-coordinates it will indicate FLT_MAX /// (no wall hit), meaning it reached the end position. This is one example of why /// this method is meant for short distance checks. /// 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 + const dtQueryFilter* filter, + float* t, float* hitNormal, dtPolyRef* path, int* pathCount, const int maxPath) const { - dtRaycastHit hit; - hit.path = path; - hit.maxPath = maxPath; - - dtStatus status = raycast(startRef, startPos, endPos, filter, 0, &hit); - - *t = hit.t; - if (hitNormal) - dtVcopy(hitNormal, hit.hitNormal); - if (pathCount) - *pathCount = hit.pathCount; - - return status; + dtRaycastHit hit; + hit.path = path; + hit.maxPath = maxPath; + + dtStatus status = raycast(startRef, startPos, endPos, filter, 0, &hit); + + *t = hit.t; + if (hitNormal) + dtVcopy(hitNormal, hit.hitNormal); + if (pathCount) + *pathCount = hit.pathCount; + + return status; } + /// @par /// /// This method is meant to be used for quick, short distance checks. /// -/// If the path array is too small to hold the result, it will be filled as +/// If the path array is too small to hold the result, it will be filled as /// far as possible from the start postion toward the end position. /// /// Using the Hit Parameter t of RaycastHit -/// -/// If the hit parameter is a very high value (FLT_MAX), then the ray has hit -/// the end position. In this case the path represents a valid corridor to the +/// +/// If the hit parameter is a very high value (FLT_MAX), then the ray has hit +/// the end position. In this case the path represents a valid corridor to the /// end position and the value of @p hitNormal is undefined. /// -/// If the hit parameter is zero, then the start position is on the wall that +/// If the hit parameter is zero, then the start position is on the wall that /// was hit and the value of @p hitNormal is undefined. /// /// If 0 < t < 1.0 then the following applies: @@ -2408,242 +2450,242 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons /// /// Use Case Restriction /// -/// The raycast ignores the y-value of the end position. (2D check.) This +/// The raycast ignores the y-value of the end position. (2D check.) This /// places significant limits on how it can be used. For example: /// -/// Consider a scene where there is a main floor with a second floor balcony -/// that hangs over the main floor. So the first floor mesh extends below the -/// balcony mesh. The start position is somewhere on the first floor. The end +/// Consider a scene where there is a main floor with a second floor balcony +/// that hangs over the main floor. So the first floor mesh extends below the +/// balcony mesh. The start position is somewhere on the first floor. The end /// position is on the balcony. /// -/// The raycast will search toward the end position along the first floor mesh. +/// The raycast will search toward the end position along the first floor mesh. /// If it reaches the end position's xz-coordinates it will indicate FLT_MAX /// (no wall hit), meaning it reached the end position. This is one example of why /// this method is meant for short distance checks. /// dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, const float* endPos, - const dtQueryFilter* filter, const unsigned int options, - dtRaycastHit* hit, dtPolyRef prevRef) const + const dtQueryFilter* filter, const unsigned int options, + dtRaycastHit* hit, dtPolyRef prevRef) const { - dtAssert(m_nav); - - if (!hit) - return DT_FAILURE | DT_INVALID_PARAM; - - hit->t = 0; - hit->pathCount = 0; - hit->pathCost = 0; - - // Validate input - if (!m_nav->isValidPolyRef(startRef) || - !startPos || !dtVisfinite(startPos) || - !endPos || !dtVisfinite(endPos) || - !filter || - (prevRef && !m_nav->isValidPolyRef(prevRef))) - { - return DT_FAILURE | DT_INVALID_PARAM; - } - - float dir[3], curPos[3], lastPos[3]; - float verts[DT_VERTS_PER_POLYGON * 3 + 3]; - int n = 0; - - dtVcopy(curPos, startPos); - dtVsub(dir, endPos, startPos); - dtVset(hit->hitNormal, 0, 0, 0); - - dtStatus status = DT_SUCCESS; - - const dtMeshTile* prevTile, * tile, * nextTile; - const dtPoly* prevPoly, * poly, * nextPoly; - dtPolyRef curRef; - - // The API input has been checked already, skip checking internal data. - curRef = startRef; - tile = 0; - poly = 0; - m_nav->getTileAndPolyByRefUnsafe(curRef, &tile, &poly); - nextTile = prevTile = tile; - nextPoly = prevPoly = poly; - if (prevRef) - m_nav->getTileAndPolyByRefUnsafe(prevRef, &prevTile, &prevPoly); - - while (curRef) - { - // Cast ray against current polygon. - - // 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. - hit->pathCount = n; - return status; - } - - hit->hitEdgeIndex = segMax; - - // Keep track of furthest t so far. - if (tmax > hit->t) - hit->t = tmax; - - // Store visited polygons. - if (n < hit->maxPath) - hit->path[n++] = curRef; - else - status |= DT_BUFFER_TOO_SMALL; - - // Ray end is completely inside the polygon. - if (segMax == -1) - { - hit->t = FLT_MAX; - hit->pathCount = n; - - // add the cost - if (options & DT_RAYCAST_USE_COSTS) - hit->pathCost += filter->getCost(curPos, endPos, prevRef, prevTile, prevPoly, curRef, tile, poly, curRef, tile, poly); - return status; - } - - // 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. - nextTile = 0; - 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; - } - } - } - - // add the cost - if (options & DT_RAYCAST_USE_COSTS) - { - // compute the intersection point at the furthest end of the polygon - // and correct the height (since the raycast moves in 2d) - dtVcopy(lastPos, curPos); - dtVmad(curPos, startPos, dir, hit->t); - float* e1 = &verts[segMax * 3]; - float* e2 = &verts[((segMax + 1) % nv) * 3]; - float eDir[3], diff[3]; - dtVsub(eDir, e2, e1); - dtVsub(diff, curPos, e1); - float s = dtSqr(eDir[0]) > dtSqr(eDir[2]) ? diff[0] / eDir[0] : diff[2] / eDir[2]; - curPos[1] = e1[1] + eDir[1] * s; - - hit->pathCost += filter->getCost(lastPos, curPos, prevRef, prevTile, prevPoly, curRef, tile, poly, nextRef, nextTile, nextPoly); - } - - 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]; - hit->hitNormal[0] = dz; - hit->hitNormal[1] = 0; - hit->hitNormal[2] = -dx; - dtVnormalize(hit->hitNormal); - - hit->pathCount = n; - return status; - } - - // No hit, advance to neighbour polygon. - prevRef = curRef; - curRef = nextRef; - prevTile = tile; - tile = nextTile; - prevPoly = poly; - poly = nextPoly; - } - - hit->pathCount = n; - - return status; + dtAssert(m_nav); + + if (!hit) + return DT_FAILURE | DT_INVALID_PARAM; + + hit->t = 0; + hit->pathCount = 0; + hit->pathCost = 0; + + // Validate input + if (!m_nav->isValidPolyRef(startRef) || + !startPos || !dtVisfinite(startPos) || + !endPos || !dtVisfinite(endPos) || + !filter || + (prevRef && !m_nav->isValidPolyRef(prevRef))) + { + return DT_FAILURE | DT_INVALID_PARAM; + } + + float dir[3], curPos[3], lastPos[3]; + float verts[DT_VERTS_PER_POLYGON*3+3]; + int n = 0; + + dtVcopy(curPos, startPos); + dtVsub(dir, endPos, startPos); + dtVset(hit->hitNormal, 0, 0, 0); + + dtStatus status = DT_SUCCESS; + + const dtMeshTile* prevTile, *tile, *nextTile; + const dtPoly* prevPoly, *poly, *nextPoly; + dtPolyRef curRef; + + // The API input has been checked already, skip checking internal data. + curRef = startRef; + tile = 0; + poly = 0; + m_nav->getTileAndPolyByRefUnsafe(curRef, &tile, &poly); + nextTile = prevTile = tile; + nextPoly = prevPoly = poly; + if (prevRef) + m_nav->getTileAndPolyByRefUnsafe(prevRef, &prevTile, &prevPoly); + + while (curRef) + { + // Cast ray against current polygon. + + // 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. + hit->pathCount = n; + return status; + } + + hit->hitEdgeIndex = segMax; + + // Keep track of furthest t so far. + if (tmax > hit->t) + hit->t = tmax; + + // Store visited polygons. + if (n < hit->maxPath) + hit->path[n++] = curRef; + else + status |= DT_BUFFER_TOO_SMALL; + + // Ray end is completely inside the polygon. + if (segMax == -1) + { + hit->t = FLT_MAX; + hit->pathCount = n; + + // add the cost + if (options & DT_RAYCAST_USE_COSTS) + hit->pathCost += filter->getCost(curPos, endPos, prevRef, prevTile, prevPoly, curRef, tile, poly, curRef, tile, poly); + return status; + } + + // 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. + nextTile = 0; + 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; + } + } + } + + // add the cost + if (options & DT_RAYCAST_USE_COSTS) + { + // compute the intersection point at the furthest end of the polygon + // and correct the height (since the raycast moves in 2d) + dtVcopy(lastPos, curPos); + dtVmad(curPos, startPos, dir, hit->t); + float* e1 = &verts[segMax*3]; + float* e2 = &verts[((segMax+1)%nv)*3]; + float eDir[3], diff[3]; + dtVsub(eDir, e2, e1); + dtVsub(diff, curPos, e1); + float s = dtSqr(eDir[0]) > dtSqr(eDir[2]) ? diff[0] / eDir[0] : diff[2] / eDir[2]; + curPos[1] = e1[1] + eDir[1] * s; + + hit->pathCost += filter->getCost(lastPos, curPos, prevRef, prevTile, prevPoly, curRef, tile, poly, nextRef, nextTile, nextPoly); + } + + 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]; + hit->hitNormal[0] = dz; + hit->hitNormal[1] = 0; + hit->hitNormal[2] = -dx; + dtVnormalize(hit->hitNormal); + + hit->pathCount = n; + return status; + } + + // No hit, advance to neighbour polygon. + prevRef = curRef; + curRef = nextRef; + prevTile = tile; + tile = nextTile; + prevPoly = poly; + poly = nextPoly; + } + + hit->pathCount = n; + + return status; } /// @par @@ -2652,996 +2694,997 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons /// /// The order of the result set is from least to highest cost to reach the polygon. /// -/// A common use case for this method is to perform Dijkstra searches. +/// A common use case for this method is to perform Dijkstra searches. /// Candidate polygons are found by searching the graph beginning at the start polygon. /// -/// If a polygon is not found via the graph search, even if it intersects the +/// If a polygon is not found via the graph search, even if it intersects the /// search circle, it will not be included in the result set. For example: /// /// polyA is the start polygon. /// polyB shares an edge with polyA. (Is adjacent.) /// polyC shares an edge with polyB, but not with polyA -/// Even if the search circle overlaps polyC, it will not be included in the +/// Even if the search circle overlaps polyC, it will not be included in the /// result set unless polyB is also in the set. -/// -/// The value of the center point is used as the start position for cost -/// calculations. It is not projected onto the surface of the mesh, so its +/// +/// The value of the center point is used as the start position for cost +/// calculations. It is not projected onto the surface of the mesh, so its /// y-value will effect the costs. /// -/// Intersection tests occur in 2D. All polygons and the search circle are -/// projected onto the xz-plane. So the y-value of the center point does not +/// Intersection tests occur in 2D. All polygons and the search circle are +/// projected onto the xz-plane. So the y-value of the center point does not /// effect intersection tests. /// -/// If the result arrays are to small to hold the entire result set, they will be +/// If the result arrays are to small to hold the entire result set, they will be /// filled to capacity. -/// +/// 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 + const dtQueryFilter* filter, + dtPolyRef* resultRef, dtPolyRef* resultParent, float* resultCost, + int* resultCount, const int maxResult) const { - dtAssert(m_nav); - dtAssert(m_nodePool); - dtAssert(m_openList); - - if (!resultCount) - return DT_FAILURE | DT_INVALID_PARAM; - - *resultCount = 0; - - if (!m_nav->isValidPolyRef(startRef) || - !centerPos || !dtVisfinite(centerPos) || - radius < 0 || !dtMathIsfinite(radius) || - !filter || maxResult < 0) - { - return DT_FAILURE | DT_INVALID_PARAM; - } - - 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); - - dtStatus status = DT_SUCCESS; - - int n = 0; - - 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); - - if (n < maxResult) - { - if (resultRef) - resultRef[n] = bestRef; - if (resultParent) - resultParent[n] = parentRef; - if (resultCost) - resultCost[n] = bestNode->total; - ++n; - } - else - { - status |= DT_BUFFER_TOO_SMALL; - } - - 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) - { - status |= DT_OUT_OF_NODES; - continue; - } - - if (neighbourNode->flags & DT_NODE_CLOSED) - continue; - - // Cost - if (neighbourNode->flags == 0) - dtVlerp(neighbourNode->pos, va, vb, 0.5f); - - float cost = filter->getCost( - bestNode->pos, neighbourNode->pos, - parentRef, parentTile, parentPoly, - bestRef, bestTile, bestPoly, - neighbourRef, neighbourTile, neighbourPoly); - - const float total = bestNode->total + cost; - - // 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->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); - } - } - } - - *resultCount = n; - - return status; + dtAssert(m_nav); + dtAssert(m_nodePool); + dtAssert(m_openList); + + if (!resultCount) + return DT_FAILURE | DT_INVALID_PARAM; + + *resultCount = 0; + + if (!m_nav->isValidPolyRef(startRef) || + !centerPos || !dtVisfinite(centerPos) || + radius < 0 || !dtMathIsfinite(radius) || + !filter || maxResult < 0) + { + return DT_FAILURE | DT_INVALID_PARAM; + } + + 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); + + dtStatus status = DT_SUCCESS; + + int n = 0; + + 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); + + if (n < maxResult) + { + if (resultRef) + resultRef[n] = bestRef; + if (resultParent) + resultParent[n] = parentRef; + if (resultCost) + resultCost[n] = bestNode->total; + ++n; + } + else + { + status |= DT_BUFFER_TOO_SMALL; + } + + 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) + { + status |= DT_OUT_OF_NODES; + continue; + } + + if (neighbourNode->flags & DT_NODE_CLOSED) + continue; + + // Cost + if (neighbourNode->flags == 0) + dtVlerp(neighbourNode->pos, va, vb, 0.5f); + + float cost = filter->getCost( + bestNode->pos, neighbourNode->pos, + parentRef, parentTile, parentPoly, + bestRef, bestTile, bestPoly, + neighbourRef, neighbourTile, neighbourPoly); + + const float total = bestNode->total + cost; + + // 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->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); + } + } + } + + *resultCount = n; + + return status; } /// @par /// /// The order of the result set is from least to highest cost. -/// +/// /// At least one result array must be provided. /// -/// A common use case for this method is to perform Dijkstra searches. -/// Candidate polygons are found by searching the graph beginning at the start +/// A common use case for this method is to perform Dijkstra searches. +/// Candidate polygons are found by searching the graph beginning at the start /// polygon. -/// +/// /// The same intersection test restrictions that apply to findPolysAroundCircle() /// method apply to this method. -/// -/// The 3D centroid of the search polygon is used as the start position for cost +/// +/// The 3D centroid of the search polygon is used as the start position for cost /// calculations. -/// -/// Intersection tests occur in 2D. All polygons are projected onto the +/// +/// Intersection tests occur in 2D. All polygons are projected onto the /// xz-plane. So the y-values of the vertices do not effect intersection tests. -/// -/// If the result arrays are is too small to hold the entire result set, they will +/// +/// If the result arrays are is too small to hold the entire result set, they will /// be filled to capacity. /// 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 + const dtQueryFilter* filter, + dtPolyRef* resultRef, dtPolyRef* resultParent, float* resultCost, + int* resultCount, const int maxResult) const { - dtAssert(m_nav); - dtAssert(m_nodePool); - dtAssert(m_openList); - - if (!resultCount) - return DT_FAILURE | DT_INVALID_PARAM; - - *resultCount = 0; - - if (!m_nav->isValidPolyRef(startRef) || - !verts || nverts < 3 || - !filter || maxResult < 0) - { - return DT_FAILURE | DT_INVALID_PARAM; - } - - // Validate input - if (!startRef || !m_nav->isValidPolyRef(startRef)) - return DT_FAILURE | DT_INVALID_PARAM; - - 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); - - dtStatus status = DT_SUCCESS; - - int n = 0; - - 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); - - if (n < maxResult) - { - if (resultRef) - resultRef[n] = bestRef; - if (resultParent) - resultParent[n] = parentRef; - if (resultCost) - resultCost[n] = bestNode->total; - - ++n; - } - else - { - status |= DT_BUFFER_TOO_SMALL; - } - - 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) - { - status |= DT_OUT_OF_NODES; - continue; - } - - if (neighbourNode->flags & DT_NODE_CLOSED) - continue; - - // Cost - if (neighbourNode->flags == 0) - dtVlerp(neighbourNode->pos, va, vb, 0.5f); - - float cost = filter->getCost( - bestNode->pos, neighbourNode->pos, - parentRef, parentTile, parentPoly, - bestRef, bestTile, bestPoly, - neighbourRef, neighbourTile, neighbourPoly); - - const float total = bestNode->total + cost; - - // 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->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); - } - } - } - - *resultCount = n; - - return status; + dtAssert(m_nav); + dtAssert(m_nodePool); + dtAssert(m_openList); + + if (!resultCount) + return DT_FAILURE | DT_INVALID_PARAM; + + *resultCount = 0; + + if (!m_nav->isValidPolyRef(startRef) || + !verts || nverts < 3 || + !filter || maxResult < 0) + { + return DT_FAILURE | DT_INVALID_PARAM; + } + + // Validate input + if (!startRef || !m_nav->isValidPolyRef(startRef)) + return DT_FAILURE | DT_INVALID_PARAM; + + 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); + + dtStatus status = DT_SUCCESS; + + int n = 0; + + 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); + + if (n < maxResult) + { + if (resultRef) + resultRef[n] = bestRef; + if (resultParent) + resultParent[n] = parentRef; + if (resultCost) + resultCost[n] = bestNode->total; + + ++n; + } + else + { + status |= DT_BUFFER_TOO_SMALL; + } + + 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) + { + status |= DT_OUT_OF_NODES; + continue; + } + + if (neighbourNode->flags & DT_NODE_CLOSED) + continue; + + // Cost + if (neighbourNode->flags == 0) + dtVlerp(neighbourNode->pos, va, vb, 0.5f); + + float cost = filter->getCost( + bestNode->pos, neighbourNode->pos, + parentRef, parentTile, parentPoly, + bestRef, bestTile, bestPoly, + neighbourRef, neighbourTile, neighbourPoly); + + const float total = bestNode->total + cost; + + // 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->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); + } + } + } + + *resultCount = n; + + return status; } dtStatus dtNavMeshQuery::getPathFromDijkstraSearch(dtPolyRef endRef, dtPolyRef* path, int* pathCount, int maxPath) const { - if (!m_nav->isValidPolyRef(endRef) || !path || !pathCount || maxPath < 0) - return DT_FAILURE | DT_INVALID_PARAM; + if (!m_nav->isValidPolyRef(endRef) || !path || !pathCount || maxPath < 0) + return DT_FAILURE | DT_INVALID_PARAM; - *pathCount = 0; + *pathCount = 0; - dtNode* endNode; - if (m_nodePool->findNodes(endRef, &endNode, 1) != 1 || - (endNode->flags & DT_NODE_CLOSED) == 0) - return DT_FAILURE | DT_INVALID_PARAM; + dtNode* endNode; + if (m_nodePool->findNodes(endRef, &endNode, 1) != 1 || + (endNode->flags & DT_NODE_CLOSED) == 0) + return DT_FAILURE | DT_INVALID_PARAM; - return getPathToNode(endNode, path, pathCount, maxPath); + return getPathToNode(endNode, path, pathCount, maxPath); } /// @par /// -/// This method is optimized for a small search radius and small number of result +/// This method is optimized for a small search radius and small number of result /// polygons. /// -/// Candidate polygons are found by searching the navigation graph beginning at +/// Candidate polygons are found by searching the navigation graph beginning at /// the start polygon. /// -/// The same intersection test restrictions that apply to the findPolysAroundCircle +/// The same intersection test restrictions that apply to the findPolysAroundCircle /// mehtod applies to this method. /// -/// The value of the center point is used as the start point for cost calculations. -/// It is not projected onto the surface of the mesh, so its y-value will effect +/// The value of the center point is used as the start point for cost calculations. +/// It is not projected onto the surface of the mesh, so its y-value will effect /// the costs. -/// -/// Intersection tests occur in 2D. All polygons and the search circle are -/// projected onto the xz-plane. So the y-value of the center point does not +/// +/// Intersection tests occur in 2D. All polygons and the search circle are +/// projected onto the xz-plane. So the y-value of the center point does not /// effect intersection tests. -/// -/// If the result arrays are is too small to hold the entire result set, they will +/// +/// If the result arrays are is too small to hold the entire result set, they will /// be filled to capacity. -/// +/// dtStatus dtNavMeshQuery::findLocalNeighbourhood(dtPolyRef startRef, const float* centerPos, const float radius, - const dtQueryFilter* filter, - dtPolyRef* resultRef, dtPolyRef* resultParent, - int* resultCount, const int maxResult) const + const dtQueryFilter* filter, + dtPolyRef* resultRef, dtPolyRef* resultParent, + int* resultCount, const int maxResult) const { - dtAssert(m_nav); - dtAssert(m_tinyNodePool); - - if (!resultCount) - return DT_FAILURE | DT_INVALID_PARAM; - - *resultCount = 0; - - if (!m_nav->isValidPolyRef(startRef) || - !centerPos || !dtVisfinite(centerPos) || - radius < 0 || !dtMathIsfinite(radius) || - !filter || maxResult < 0) - { - return DT_FAILURE | DT_INVALID_PARAM; - } - - 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]; - - dtStatus status = DT_SUCCESS; - - int n = 0; - if (n < maxResult) - { - resultRef[n] = startNode->id; - if (resultParent) - resultParent[n] = 0; - ++n; - } - else - { - status |= DT_BUFFER_TOO_SMALL; - } - - 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; - } - else - { - status |= DT_BUFFER_TOO_SMALL; - } - - if (nstack < MAX_STACK) - { - stack[nstack++] = neighbourNode; - } - } - } - - *resultCount = n; - - return status; + dtAssert(m_nav); + dtAssert(m_tinyNodePool); + + if (!resultCount) + return DT_FAILURE | DT_INVALID_PARAM; + + *resultCount = 0; + + if (!m_nav->isValidPolyRef(startRef) || + !centerPos || !dtVisfinite(centerPos) || + radius < 0 || !dtMathIsfinite(radius) || + !filter || maxResult < 0) + { + return DT_FAILURE | DT_INVALID_PARAM; + } + + 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]; + + dtStatus status = DT_SUCCESS; + + int n = 0; + if (n < maxResult) + { + resultRef[n] = startNode->id; + if (resultParent) + resultParent[n] = 0; + ++n; + } + else + { + status |= DT_BUFFER_TOO_SMALL; + } + + 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; + } + else + { + status |= DT_BUFFER_TOO_SMALL; + } + + if (nstack < MAX_STACK) + { + stack[nstack++] = neighbourNode; + } + } + } + + *resultCount = n; + + return status; } + struct dtSegInterval { - dtPolyRef ref; - short tmin, tmax; + dtPolyRef ref; + short tmin, tmax; }; static void insertInterval(dtSegInterval* ints, int& nints, const int maxInts, - const short tmin, const short tmax, const dtPolyRef ref) + const short tmin, const short tmax, const dtPolyRef ref) { - 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].ref = ref; - ints[idx].tmin = tmin; - ints[idx].tmax = tmax; - nints++; + 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].ref = ref; + ints[idx].tmin = tmin; + ints[idx].tmax = tmax; + nints++; } /// @par /// -/// If the @p segmentRefs parameter is provided, then all polygon segments will be returned. +/// If the @p segmentRefs parameter is provided, then all polygon segments will be returned. /// Otherwise only the wall segments are returned. -/// -/// A segment that is normally a portal will be included in the result set as a +/// +/// A segment that is normally a portal will be included in the result set as a /// wall if the @p filter results in the neighbor polygon becoomming impassable. -/// -/// The @p segmentVerts and @p segmentRefs buffers should normally be sized for the +/// +/// The @p segmentVerts and @p segmentRefs buffers should normally be sized for the /// maximum segments per polygon of the source navigation mesh. -/// +/// dtStatus dtNavMeshQuery::getPolyWallSegments(dtPolyRef ref, const dtQueryFilter* filter, - float* segmentVerts, dtPolyRef* segmentRefs, int* segmentCount, - const int maxSegments) const + float* segmentVerts, dtPolyRef* segmentRefs, int* segmentCount, + const int maxSegments) const { - dtAssert(m_nav); - - if (!segmentCount) - return DT_FAILURE | DT_INVALID_PARAM; - - *segmentCount = 0; - - const dtMeshTile* tile = 0; - const dtPoly* poly = 0; - if (dtStatusFailed(m_nav->getTileAndPolyByRef(ref, &tile, &poly))) - return DT_FAILURE | DT_INVALID_PARAM; - - if (!filter || !segmentVerts || maxSegments < 0) - return DT_FAILURE | DT_INVALID_PARAM; - - int n = 0; - static const int MAX_INTERVAL = 16; - dtSegInterval ints[MAX_INTERVAL]; - int nints; - - const bool storePortals = segmentRefs != 0; - - dtStatus status = DT_SUCCESS; - - 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, link->ref); - } - } - } - } - } - else - { - // Internal edge - dtPolyRef neiRef = 0; - if (poly->neis[j]) - { - const unsigned int idx = (unsigned int)(poly->neis[j] - 1); - neiRef = m_nav->getPolyRefBase(tile) | idx; - if (!filter->passFilter(neiRef, tile, &tile->polys[idx])) - neiRef = 0; - } - - // If the edge leads to another polygon and portals are not stored, skip. - if (neiRef != 0 && !storePortals) - continue; - - if (n < maxSegments) - { - const float* vj = &tile->verts[poly->verts[j] * 3]; - const float* vi = &tile->verts[poly->verts[i] * 3]; - float* seg = &segmentVerts[n * 6]; - dtVcopy(seg + 0, vj); - dtVcopy(seg + 3, vi); - if (segmentRefs) - segmentRefs[n] = neiRef; - n++; - } - else - { - status |= DT_BUFFER_TOO_SMALL; - } - - continue; - } - - // Add sentinels - insertInterval(ints, nints, MAX_INTERVAL, -1, 0, 0); - insertInterval(ints, nints, MAX_INTERVAL, 255, 256, 0); - - // Store segments. - 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) - { - // Portal segment. - if (storePortals && ints[k].ref) - { - const float tmin = ints[k].tmin / 255.0f; - const float tmax = ints[k].tmax / 255.0f; - if (n < maxSegments) - { - float* seg = &segmentVerts[n * 6]; - dtVlerp(seg + 0, vj, vi, tmin); - dtVlerp(seg + 3, vj, vi, tmax); - if (segmentRefs) - segmentRefs[n] = ints[k].ref; - n++; - } - else - { - status |= DT_BUFFER_TOO_SMALL; - } - } - - // Wall segment. - const int imin = ints[k - 1].tmax; - const int imax = ints[k].tmin; - if (imin != imax) - { - const float tmin = imin / 255.0f; - const float tmax = imax / 255.0f; - if (n < maxSegments) - { - float* seg = &segmentVerts[n * 6]; - dtVlerp(seg + 0, vj, vi, tmin); - dtVlerp(seg + 3, vj, vi, tmax); - if (segmentRefs) - segmentRefs[n] = 0; - n++; - } - else - { - status |= DT_BUFFER_TOO_SMALL; - } - } - } - } - - *segmentCount = n; - - return status; + dtAssert(m_nav); + + if (!segmentCount) + return DT_FAILURE | DT_INVALID_PARAM; + + *segmentCount = 0; + + const dtMeshTile* tile = 0; + const dtPoly* poly = 0; + if (dtStatusFailed(m_nav->getTileAndPolyByRef(ref, &tile, &poly))) + return DT_FAILURE | DT_INVALID_PARAM; + + if (!filter || !segmentVerts || maxSegments < 0) + return DT_FAILURE | DT_INVALID_PARAM; + + int n = 0; + static const int MAX_INTERVAL = 16; + dtSegInterval ints[MAX_INTERVAL]; + int nints; + + const bool storePortals = segmentRefs != 0; + + dtStatus status = DT_SUCCESS; + + 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, link->ref); + } + } + } + } + } + else + { + // Internal edge + dtPolyRef neiRef = 0; + if (poly->neis[j]) + { + const unsigned int idx = (unsigned int)(poly->neis[j]-1); + neiRef = m_nav->getPolyRefBase(tile) | idx; + if (!filter->passFilter(neiRef, tile, &tile->polys[idx])) + neiRef = 0; + } + + // If the edge leads to another polygon and portals are not stored, skip. + if (neiRef != 0 && !storePortals) + continue; + + if (n < maxSegments) + { + const float* vj = &tile->verts[poly->verts[j]*3]; + const float* vi = &tile->verts[poly->verts[i]*3]; + float* seg = &segmentVerts[n*6]; + dtVcopy(seg+0, vj); + dtVcopy(seg+3, vi); + if (segmentRefs) + segmentRefs[n] = neiRef; + n++; + } + else + { + status |= DT_BUFFER_TOO_SMALL; + } + + continue; + } + + // Add sentinels + insertInterval(ints, nints, MAX_INTERVAL, -1, 0, 0); + insertInterval(ints, nints, MAX_INTERVAL, 255, 256, 0); + + // Store segments. + 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) + { + // Portal segment. + if (storePortals && ints[k].ref) + { + const float tmin = ints[k].tmin/255.0f; + const float tmax = ints[k].tmax/255.0f; + if (n < maxSegments) + { + float* seg = &segmentVerts[n*6]; + dtVlerp(seg+0, vj,vi, tmin); + dtVlerp(seg+3, vj,vi, tmax); + if (segmentRefs) + segmentRefs[n] = ints[k].ref; + n++; + } + else + { + status |= DT_BUFFER_TOO_SMALL; + } + } + + // Wall segment. + const int imin = ints[k-1].tmax; + const int imax = ints[k].tmin; + if (imin != imax) + { + const float tmin = imin/255.0f; + const float tmax = imax/255.0f; + if (n < maxSegments) + { + float* seg = &segmentVerts[n*6]; + dtVlerp(seg+0, vj,vi, tmin); + dtVlerp(seg+3, vj,vi, tmax); + if (segmentRefs) + segmentRefs[n] = 0; + n++; + } + else + { + status |= DT_BUFFER_TOO_SMALL; + } + } + } + } + + *segmentCount = n; + + return status; } /// @par /// /// @p hitPos is not adjusted using the height detail data. /// -/// @p hitDist will equal the search radius if there is no wall within the +/// @p hitDist will equal the search radius if there is no wall within the /// radius. In this case the values of @p hitPos and @p hitNormal are /// undefined. /// /// The normal will become unpredicable if @p hitDist is a very small number. /// dtStatus dtNavMeshQuery::findDistanceToWall(dtPolyRef startRef, const float* centerPos, const float maxRadius, - const dtQueryFilter* filter, - float* hitDist, float* hitPos, float* hitNormal) const + const dtQueryFilter* filter, + float* hitDist, float* hitPos, float* hitNormal) const { - dtAssert(m_nav); - dtAssert(m_nodePool); - dtAssert(m_openList); - - // Validate input - if (!m_nav->isValidPolyRef(startRef) || - !centerPos || !dtVisfinite(centerPos) || - maxRadius < 0 || !dtMathIsfinite(maxRadius) || - !filter || !hitDist || !hitPos || !hitNormal) - { - return DT_FAILURE | DT_INVALID_PARAM; - } - - 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); - - dtStatus status = DT_SUCCESS; - - 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) - { - status |= DT_OUT_OF_NODES; - 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 = (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 status; + dtAssert(m_nav); + dtAssert(m_nodePool); + dtAssert(m_openList); + + // Validate input + if (!m_nav->isValidPolyRef(startRef) || + !centerPos || !dtVisfinite(centerPos) || + maxRadius < 0 || !dtMathIsfinite(maxRadius) || + !filter || !hitDist || !hitPos || !hitNormal) + { + return DT_FAILURE | DT_INVALID_PARAM; + } + + 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); + + dtStatus status = DT_SUCCESS; + + 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) + { + status |= DT_OUT_OF_NODES; + 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 = (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 = dtMathSqrtf(radiusSqr); + + return status; } bool dtNavMeshQuery::isValidPolyRef(dtPolyRef ref, const dtQueryFilter* filter) const { - const dtMeshTile* tile = 0; - const dtPoly* poly = 0; - dtStatus status = m_nav->getTileAndPolyByRef(ref, &tile, &poly); - // If cannot get polygon, assume it does not exists and boundary is invalid. - if (dtStatusFailed(status)) - return false; - // If cannot pass filter, assume flags has changed and boundary is invalid. - if (!filter->passFilter(ref, tile, poly)) - return false; - return true; + const dtMeshTile* tile = 0; + const dtPoly* poly = 0; + dtStatus status = m_nav->getTileAndPolyByRef(ref, &tile, &poly); + // If cannot get polygon, assume it does not exists and boundary is invalid. + if (dtStatusFailed(status)) + return false; + // If cannot pass filter, assume flags has changed and boundary is invalid. + if (!filter->passFilter(ref, tile, poly)) + return false; + return true; } /// @par /// -/// The closed list is the list of polygons that were fully evaluated during +/// The closed list is the list of polygons that were fully evaluated during /// the last navigation graph search. (A* or Dijkstra) -/// +/// bool dtNavMeshQuery::isInClosedList(dtPolyRef ref) const { - if (!m_nodePool) return false; - - dtNode* nodes[DT_MAX_STATES_PER_NODE]; - int n = m_nodePool->findNodes(ref, nodes, DT_MAX_STATES_PER_NODE); - - for (int i = 0; i < n; i++) - { - if (nodes[i]->flags & DT_NODE_CLOSED) - return true; - } - - return false; -} \ No newline at end of file + if (!m_nodePool) return false; + + dtNode* nodes[DT_MAX_STATES_PER_NODE]; + int n= m_nodePool->findNodes(ref, nodes, DT_MAX_STATES_PER_NODE); + + for (int i=0; iflags & DT_NODE_CLOSED) + return true; + } + + return false; +} diff --git a/recastnavigation/Detour/Source/DetourNode.cpp b/recastnavigation/Detour/Source/DetourNode.cpp index 5d643cd..48abbba 100644 --- a/recastnavigation/Detour/Source/DetourNode.cpp +++ b/recastnavigation/Detour/Source/DetourNode.cpp @@ -26,174 +26,175 @@ // From Thomas Wang, https://gist.github.com/badboy/6267743 inline unsigned int dtHashRef(dtPolyRef a) { - a = (~a) + (a << 18); // a = (a << 18) - a - 1; - a = a ^ (a >> 31); - a = a * 21; // a = (a + (a << 2)) + (a << 4); - a = a ^ (a >> 11); - a = a + (a << 6); - a = a ^ (a >> 22); - return (unsigned int)a; + a = (~a) + (a << 18); // a = (a << 18) - a - 1; + a = a ^ (a >> 31); + a = a * 21; // a = (a + (a << 2)) + (a << 4); + a = a ^ (a >> 11); + a = a + (a << 6); + a = a ^ (a >> 22); + return (unsigned int)a; } #else inline unsigned int dtHashRef(dtPolyRef a) { - a += ~(a << 15); - a ^= (a >> 10); - a += (a << 3); - a ^= (a >> 6); - a += ~(a << 11); - a ^= (a >> 16); - return (unsigned int)a; + a += ~(a<<15); + a ^= (a>>10); + a += (a<<3); + a ^= (a>>6); + a += ~(a<<11); + a ^= (a>>16); + return (unsigned int)a; } #endif ////////////////////////////////////////////////////////////////////////////////////////// dtNodePool::dtNodePool(int maxNodes, int hashSize) : - m_nodes(0), - m_first(0), - m_next(0), - m_maxNodes(maxNodes), - m_hashSize(hashSize), - m_nodeCount(0) + 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); - // pidx is special as 0 means "none" and 1 is the first node. For that reason - // we have 1 fewer nodes available than the number of values it can contain. - dtAssert(m_maxNodes > 0 && m_maxNodes <= DT_NULL_IDX && m_maxNodes <= (1 << DT_NODE_PARENT_BITS) - 1); + dtAssert(dtNextPow2(m_hashSize) == (unsigned int)m_hashSize); + // pidx is special as 0 means "none" and 1 is the first node. For that reason + // we have 1 fewer nodes available than the number of values it can contain. + dtAssert(m_maxNodes > 0 && m_maxNodes <= DT_NULL_IDX && m_maxNodes <= (1 << DT_NODE_PARENT_BITS) - 1); - 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); + 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); + 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); + 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); + dtFree(m_nodes); + dtFree(m_next); + dtFree(m_first); } void dtNodePool::clear() { - memset(m_first, 0xff, sizeof(dtNodeIndex) * m_hashSize); - m_nodeCount = 0; + memset(m_first, 0xff, sizeof(dtNodeIndex)*m_hashSize); + m_nodeCount = 0; } unsigned int dtNodePool::findNodes(dtPolyRef id, dtNode** nodes, const int maxNodes) { - int n = 0; - unsigned int bucket = dtHashRef(id) & (m_hashSize - 1); - dtNodeIndex i = m_first[bucket]; - while (i != DT_NULL_IDX) - { - if (m_nodes[i].id == id) - { - if (n >= maxNodes) - return n; - nodes[n++] = &m_nodes[i]; - } - i = m_next[i]; - } - - return n; + int n = 0; + unsigned int bucket = dtHashRef(id) & (m_hashSize-1); + dtNodeIndex i = m_first[bucket]; + while (i != DT_NULL_IDX) + { + if (m_nodes[i].id == id) + { + if (n >= maxNodes) + return n; + nodes[n++] = &m_nodes[i]; + } + i = m_next[i]; + } + + return n; } dtNode* dtNodePool::findNode(dtPolyRef id, unsigned char state) { - unsigned int bucket = dtHashRef(id) & (m_hashSize - 1); - dtNodeIndex i = m_first[bucket]; - while (i != DT_NULL_IDX) - { - if (m_nodes[i].id == id && m_nodes[i].state == state) - return &m_nodes[i]; - i = m_next[i]; - } - return 0; + unsigned int bucket = dtHashRef(id) & (m_hashSize-1); + dtNodeIndex i = m_first[bucket]; + while (i != DT_NULL_IDX) + { + if (m_nodes[i].id == id && m_nodes[i].state == state) + return &m_nodes[i]; + i = m_next[i]; + } + return 0; } dtNode* dtNodePool::getNode(dtPolyRef id, unsigned char state) { - 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 && m_nodes[i].state == state) - 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->state = state; - node->flags = 0; - - m_next[i] = m_first[bucket]; - m_first[bucket] = i; - - return node; + 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 && m_nodes[i].state == state) + 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->state = state; + 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) + 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); + dtAssert(m_capacity > 0); + + m_heap = (dtNode**)dtAlloc(sizeof(dtNode*)*(m_capacity+1), DT_ALLOC_PERM); + dtAssert(m_heap); } dtNodeQueue::~dtNodeQueue() { - dtFree(m_heap); + 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; + 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); -} \ No newline at end of file + 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); +}