diff --git a/CMakeLists.txt b/CMakeLists.txt index 74e21673d..3615733e9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -52,10 +52,12 @@ include(simpleini) include(discord_game_sdk) include(gwtoolboxdll_plugins) include(nativefiledialog) +include(fastlz) add_subdirectory(GWToolboxdll) add_subdirectory(Core) add_subdirectory(RestClient) add_subdirectory(GWToolbox) +add_subdirectory(Dependencies/recastnavigation) set_property(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} PROPERTY VS_STARTUP_PROJECT GWToolbox) diff --git a/Dependencies/fastlz/LICENSE.MIT b/Dependencies/fastlz/LICENSE.MIT new file mode 100644 index 000000000..4938b4d05 --- /dev/null +++ b/Dependencies/fastlz/LICENSE.MIT @@ -0,0 +1,21 @@ +FastLZ - Byte-aligned LZ77 compression library +Copyright (C) 2005-2020 Ariya Hidayat + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. + diff --git a/Dependencies/fastlz/fastlz.cpp b/Dependencies/fastlz/fastlz.cpp new file mode 100644 index 000000000..35b68a5ba --- /dev/null +++ b/Dependencies/fastlz/fastlz.cpp @@ -0,0 +1,508 @@ +/* + FastLZ - Byte-aligned LZ77 compression library + Copyright (C) 2005-2020 Ariya Hidayat + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +#include "fastlz.h" + +#include + +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wimplicit-fallthrough" + +/* + * Give hints to the compiler for branch prediction optimization. + */ +#if defined(__clang__) || (defined(__GNUC__) && (__GNUC__ > 2)) +#define FASTLZ_LIKELY(c) (__builtin_expect(!!(c), 1)) +#define FASTLZ_UNLIKELY(c) (__builtin_expect(!!(c), 0)) +#else +#define FASTLZ_LIKELY(c) (c) +#define FASTLZ_UNLIKELY(c) (c) +#endif + +/* + * Specialize custom 64-bit implementation for speed improvements. + */ +#if defined(__x86_64__) || defined(_M_X64) || defined(__aarch64__) +#define FLZ_ARCH64 +#endif + +/* + * Workaround for DJGPP to find uint8_t, uint16_t, etc. + */ +#if defined(__MSDOS__) && defined(__GNUC__) +#include +#endif + +#if defined(FASTLZ_USE_MEMMOVE) && (FASTLZ_USE_MEMMOVE == 0) + +static void fastlz_memmove(uint8_t* dest, const uint8_t* src, uint32_t count) { + do { + *dest++ = *src++; + } while (--count); +} + +static void fastlz_memcpy(uint8_t* dest, const uint8_t* src, uint32_t count) { + return fastlz_memmove(dest, src, count); +} + +#else + +#include + +static void fastlz_memmove(uint8_t* dest, const uint8_t* src, uint32_t count) { + if ((count > 4) && (dest >= src + count)) { + memmove(dest, src, count); + } else { + switch (count) { + default: + do { + *dest++ = *src++; + } while (--count); + break; + case 3: + *dest++ = *src++; + case 2: + *dest++ = *src++; + case 1: + *dest++ = *src++; + case 0: + break; + } + } +} + +static void fastlz_memcpy(uint8_t* dest, const uint8_t* src, uint32_t count) { memcpy(dest, src, count); } + +#endif + +#if defined(FLZ_ARCH64) + +static uint32_t flz_readu32(const void* ptr) { return *(const uint32_t*)ptr; } + +static uint32_t flz_cmp(const uint8_t* p, const uint8_t* q, const uint8_t* r) { + const uint8_t* start = p; + + if (flz_readu32(p) == flz_readu32(q)) { + p += 4; + q += 4; + } + while (q < r) + if (*p++ != *q++) break; + return p - start; +} + +#endif /* FLZ_ARCH64 */ + +#if !defined(FLZ_ARCH64) + +static uint32_t flz_readu32(const void* ptr) { + const uint8_t* p = (const uint8_t*)ptr; + return (p[3] << 24) | (p[2] << 16) | (p[1] << 8) | p[0]; +} + +static uint32_t flz_cmp(const uint8_t* p, const uint8_t* q, const uint8_t* r) { + const uint8_t* start = p; + while (q < r) + if (*p++ != *q++) break; + return p - start; +} + +#endif /* !FLZ_ARCH64 */ + +#define MAX_COPY 32 +#define MAX_LEN 264 /* 256 + 8 */ +#define MAX_L1_DISTANCE 8192 +#define MAX_L2_DISTANCE 8191 +#define MAX_FARDISTANCE (65535 + MAX_L2_DISTANCE - 1) + +#define HASH_LOG 13 +#define HASH_SIZE (1 << HASH_LOG) +#define HASH_MASK (HASH_SIZE - 1) + +static uint16_t flz_hash(uint32_t v) { + uint32_t h = (v * 2654435769LL) >> (32 - HASH_LOG); + return h & HASH_MASK; +} + +/* special case of memcpy: at most MAX_COPY bytes */ +static void flz_smallcopy(uint8_t* dest, const uint8_t* src, uint32_t count) { +#if defined(FLZ_ARCH64) + if (count >= 4) { + const uint32_t* p = (const uint32_t*)src; + uint32_t* q = (uint32_t*)dest; + while (count > 4) { + *q++ = *p++; + count -= 4; + dest += 4; + src += 4; + } + } +#endif + fastlz_memcpy(dest, src, count); +} + +/* special case of memcpy: exactly MAX_COPY bytes */ +static void flz_maxcopy(void* dest, const void* src) { +#if defined(FLZ_ARCH64) + const uint32_t* p = (const uint32_t*)src; + uint32_t* q = (uint32_t*)dest; + *q++ = *p++; + *q++ = *p++; + *q++ = *p++; + *q++ = *p++; + *q++ = *p++; + *q++ = *p++; + *q++ = *p++; + *q++ = *p++; +#else + fastlz_memcpy((uint8_t*)dest, (uint8_t*)src, MAX_COPY); +#endif +} + +static uint8_t* flz_literals(uint32_t runs, const uint8_t* src, uint8_t* dest) { + while (runs >= MAX_COPY) { + *dest++ = MAX_COPY - 1; + flz_maxcopy(dest, src); + src += MAX_COPY; + dest += MAX_COPY; + runs -= MAX_COPY; + } + if (runs > 0) { + *dest++ = runs - 1; + flz_smallcopy(dest, src, runs); + dest += runs; + } + return dest; +} + +static uint8_t* flz1_match(uint32_t len, uint32_t distance, uint8_t* op) { + --distance; + if (FASTLZ_UNLIKELY(len > MAX_LEN - 2)) + while (len > MAX_LEN - 2) { + *op++ = (7 << 5) + (distance >> 8); + *op++ = MAX_LEN - 2 - 7 - 2; + *op++ = (distance & 255); + len -= MAX_LEN - 2; + } + if (len < 7) { + *op++ = (len << 5) + (distance >> 8); + *op++ = (distance & 255); + } else { + *op++ = (7 << 5) + (distance >> 8); + *op++ = len - 7; + *op++ = (distance & 255); + } + return op; +} + +#define FASTLZ_BOUND_CHECK(cond) \ + if (FASTLZ_UNLIKELY(!(cond))) return 0; + +int fastlz1_compress(const void* input, int length, void* output) { + const uint8_t* ip = (const uint8_t*)input; + const uint8_t* ip_start = ip; + const uint8_t* ip_bound = ip + length - 4; /* because readU32 */ + const uint8_t* ip_limit = ip + length - 12 - 1; + uint8_t* op = (uint8_t*)output; + + uint32_t htab[HASH_SIZE]; + uint32_t seq, hash; + + /* initializes hash table */ + for (hash = 0; hash < HASH_SIZE; ++hash) htab[hash] = 0; + + /* we start with literal copy */ + const uint8_t* anchor = ip; + ip += 2; + + /* main loop */ + while (FASTLZ_LIKELY(ip < ip_limit)) { + const uint8_t* ref; + uint32_t distance, cmp; + + /* find potential match */ + do { + seq = flz_readu32(ip) & 0xffffff; + hash = flz_hash(seq); + ref = ip_start + htab[hash]; + htab[hash] = ip - ip_start; + distance = ip - ref; + cmp = FASTLZ_LIKELY(distance < MAX_L1_DISTANCE) ? flz_readu32(ref) & 0xffffff : 0x1000000; + if (FASTLZ_UNLIKELY(ip >= ip_limit)) break; + ++ip; + } while (seq != cmp); + + if (FASTLZ_UNLIKELY(ip >= ip_limit)) break; + --ip; + + if (FASTLZ_LIKELY(ip > anchor)) { + op = flz_literals(ip - anchor, anchor, op); + } + + uint32_t len = flz_cmp(ref + 3, ip + 3, ip_bound); + op = flz1_match(len, distance, op); + + /* update the hash at match boundary */ + ip += len; + seq = flz_readu32(ip); + hash = flz_hash(seq & 0xffffff); + htab[hash] = ip++ - ip_start; + seq >>= 8; + hash = flz_hash(seq); + htab[hash] = ip++ - ip_start; + + anchor = ip; + } + + uint32_t copy = (uint8_t*)input + length - anchor; + op = flz_literals(copy, anchor, op); + + return op - (uint8_t*)output; +} + +int fastlz1_decompress(const void* input, int length, void* output, int maxout) { + const uint8_t* ip = (const uint8_t*)input; + const uint8_t* ip_limit = ip + length; + const uint8_t* ip_bound = ip_limit - 2; + uint8_t* op = (uint8_t*)output; + uint8_t* op_limit = op + maxout; + uint32_t ctrl = (*ip++) & 31; + + while (1) { + if (ctrl >= 32) { + uint32_t len = (ctrl >> 5) - 1; + uint32_t ofs = (ctrl & 31) << 8; + const uint8_t* ref = op - ofs - 1; + if (len == 7 - 1) { + FASTLZ_BOUND_CHECK(ip <= ip_bound); + len += *ip++; + } + ref -= *ip++; + len += 3; + FASTLZ_BOUND_CHECK(op + len <= op_limit); + FASTLZ_BOUND_CHECK(ref >= (uint8_t*)output); + fastlz_memmove(op, ref, len); + op += len; + } else { + ctrl++; + FASTLZ_BOUND_CHECK(op + ctrl <= op_limit); + FASTLZ_BOUND_CHECK(ip + ctrl <= ip_limit); + fastlz_memcpy(op, ip, ctrl); + ip += ctrl; + op += ctrl; + } + + if (FASTLZ_UNLIKELY(ip > ip_bound)) break; + ctrl = *ip++; + } + + return op - (uint8_t*)output; +} + +static uint8_t* flz2_match(uint32_t len, uint32_t distance, uint8_t* op) { + --distance; + if (distance < MAX_L2_DISTANCE) { + if (len < 7) { + *op++ = (len << 5) + (distance >> 8); + *op++ = (distance & 255); + } else { + *op++ = (7 << 5) + (distance >> 8); + for (len -= 7; len >= 255; len -= 255) *op++ = 255; + *op++ = len; + *op++ = (distance & 255); + } + } else { + /* far away, but not yet in the another galaxy... */ + if (len < 7) { + distance -= MAX_L2_DISTANCE; + *op++ = (len << 5) + 31; + *op++ = 255; + *op++ = distance >> 8; + *op++ = distance & 255; + } else { + distance -= MAX_L2_DISTANCE; + *op++ = (7 << 5) + 31; + for (len -= 7; len >= 255; len -= 255) *op++ = 255; + *op++ = len; + *op++ = 255; + *op++ = distance >> 8; + *op++ = distance & 255; + } + } + return op; +} + +int fastlz2_compress(const void* input, int length, void* output) { + const uint8_t* ip = (const uint8_t*)input; + const uint8_t* ip_start = ip; + const uint8_t* ip_bound = ip + length - 4; /* because readU32 */ + const uint8_t* ip_limit = ip + length - 12 - 1; + uint8_t* op = (uint8_t*)output; + + uint32_t htab[HASH_SIZE]; + uint32_t seq, hash; + + /* initializes hash table */ + for (hash = 0; hash < HASH_SIZE; ++hash) htab[hash] = 0; + + /* we start with literal copy */ + const uint8_t* anchor = ip; + ip += 2; + + /* main loop */ + while (FASTLZ_LIKELY(ip < ip_limit)) { + const uint8_t* ref; + uint32_t distance, cmp; + + /* find potential match */ + do { + seq = flz_readu32(ip) & 0xffffff; + hash = flz_hash(seq); + ref = ip_start + htab[hash]; + htab[hash] = ip - ip_start; + distance = ip - ref; + cmp = FASTLZ_LIKELY(distance < MAX_FARDISTANCE) ? flz_readu32(ref) & 0xffffff : 0x1000000; + if (FASTLZ_UNLIKELY(ip >= ip_limit)) break; + ++ip; + } while (seq != cmp); + + if (FASTLZ_UNLIKELY(ip >= ip_limit)) break; + + --ip; + + /* far, needs at least 5-byte match */ + if (distance >= MAX_L2_DISTANCE) { + if (ref[3] != ip[3] || ref[4] != ip[4]) { + ++ip; + continue; + } + } + + if (FASTLZ_LIKELY(ip > anchor)) { + op = flz_literals(ip - anchor, anchor, op); + } + + uint32_t len = flz_cmp(ref + 3, ip + 3, ip_bound); + op = flz2_match(len, distance, op); + + /* update the hash at match boundary */ + ip += len; + seq = flz_readu32(ip); + hash = flz_hash(seq & 0xffffff); + htab[hash] = ip++ - ip_start; + seq >>= 8; + hash = flz_hash(seq); + htab[hash] = ip++ - ip_start; + + anchor = ip; + } + + uint32_t copy = (uint8_t*)input + length - anchor; + op = flz_literals(copy, anchor, op); + + /* marker for fastlz2 */ + *(uint8_t*)output |= (1 << 5); + + return op - (uint8_t*)output; +} + +int fastlz2_decompress(const void* input, int length, void* output, int maxout) { + const uint8_t* ip = (const uint8_t*)input; + const uint8_t* ip_limit = ip + length; + const uint8_t* ip_bound = ip_limit - 2; + uint8_t* op = (uint8_t*)output; + uint8_t* op_limit = op + maxout; + uint32_t ctrl = (*ip++) & 31; + + while (1) { + if (ctrl >= 32) { + uint32_t len = (ctrl >> 5) - 1; + uint32_t ofs = (ctrl & 31) << 8; + const uint8_t* ref = op - ofs - 1; + + uint8_t code; + if (len == 7 - 1) do { + FASTLZ_BOUND_CHECK(ip <= ip_bound); + code = *ip++; + len += code; + } while (code == 255); + code = *ip++; + ref -= code; + len += 3; + + /* match from 16-bit distance */ + if (FASTLZ_UNLIKELY(code == 255)) + if (FASTLZ_LIKELY(ofs == (31 << 8))) { + FASTLZ_BOUND_CHECK(ip < ip_bound); + ofs = (*ip++) << 8; + ofs += *ip++; + ref = op - ofs - MAX_L2_DISTANCE - 1; + } + + FASTLZ_BOUND_CHECK(op + len <= op_limit); + FASTLZ_BOUND_CHECK(ref >= (uint8_t*)output); + fastlz_memmove(op, ref, len); + op += len; + } else { + ctrl++; + FASTLZ_BOUND_CHECK(op + ctrl <= op_limit); + FASTLZ_BOUND_CHECK(ip + ctrl <= ip_limit); + fastlz_memcpy(op, ip, ctrl); + ip += ctrl; + op += ctrl; + } + + if (FASTLZ_UNLIKELY(ip >= ip_limit)) break; + ctrl = *ip++; + } + + return op - (uint8_t*)output; +} + +int fastlz_compress(const void* input, int length, void* output) { + /* for short block, choose fastlz1 */ + if (length < 65536) return fastlz1_compress(input, length, output); + + /* else... */ + return fastlz2_compress(input, length, output); +} + +int fastlz_decompress(const void* input, int length, void* output, int maxout) { + /* magic identifier for compression level */ + int level = ((*(const uint8_t*)input) >> 5) + 1; + + if (level == 1) return fastlz1_decompress(input, length, output, maxout); + if (level == 2) return fastlz2_decompress(input, length, output, maxout); + + /* unknown level, trigger error */ + return 0; +} + +int fastlz_compress_level(int level, const void* input, int length, void* output) { + if (level == 1) return fastlz1_compress(input, length, output); + if (level == 2) return fastlz2_compress(input, length, output); + + return 0; +} + +#pragma GCC diagnostic pop diff --git a/Dependencies/fastlz/fastlz.h b/Dependencies/fastlz/fastlz.h new file mode 100644 index 000000000..9172d74de --- /dev/null +++ b/Dependencies/fastlz/fastlz.h @@ -0,0 +1,97 @@ +/* + FastLZ - Byte-aligned LZ77 compression library + Copyright (C) 2005-2020 Ariya Hidayat + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +#ifndef FASTLZ_H +#define FASTLZ_H + +#define FASTLZ_VERSION 0x000500 + +#define FASTLZ_VERSION_MAJOR 0 +#define FASTLZ_VERSION_MINOR 5 +#define FASTLZ_VERSION_REVISION 0 + +#define FASTLZ_VERSION_STRING "0.5.0" + +#if defined(__cplusplus) +extern "C" { +#endif + +/** + Compress a block of data in the input buffer and returns the size of + compressed block. The size of input buffer is specified by length. The + minimum input buffer size is 16. + + The output buffer must be at least 5% larger than the input buffer + and can not be smaller than 66 bytes. + + If the input is not compressible, the return value might be larger than + length (input buffer size). + + The input buffer and the output buffer can not overlap. + + Compression level can be specified in parameter level. At the moment, + only level 1 and level 2 are supported. + Level 1 is the fastest compression and generally useful for short data. + Level 2 is slightly slower but it gives better compression ratio. + + Note that the compressed data, regardless of the level, can always be + decompressed using the function fastlz_decompress below. +*/ + +int fastlz_compress_level(int level, const void* input, int length, void* output); + +/** + Decompress a block of compressed data and returns the size of the + decompressed block. If error occurs, e.g. the compressed data is + corrupted or the output buffer is not large enough, then 0 (zero) + will be returned instead. + + The input buffer and the output buffer can not overlap. + + Decompression is memory safe and guaranteed not to write the output buffer + more than what is specified in maxout. + + Note that the decompression will always work, regardless of the + compression level specified in fastlz_compress_level above (when + producing the compressed block). + */ + +int fastlz_decompress(const void* input, int length, void* output, int maxout); + +/** + DEPRECATED. + + This is similar to fastlz_compress_level above, but with the level + automatically chosen. + + This function is deprecated and it will be completely removed in some future + version. +*/ + +int fastlz_compress(const void* input, int length, void* output); + +#if defined(__cplusplus) +} +#endif + +#endif /* FASTLZ_H */ diff --git a/Dependencies/recastnavigation/CMakeLists.txt b/Dependencies/recastnavigation/CMakeLists.txt new file mode 100644 index 000000000..dce93e77f --- /dev/null +++ b/Dependencies/recastnavigation/CMakeLists.txt @@ -0,0 +1,96 @@ +cmake_minimum_required(VERSION 3.1) + +project(RecastNavigation) + +# lib versions +SET(SOVERSION 1) +set(LIB_VERSION 1.6.0) +string(REPLACE "." "," LIB_VERSION_NUM "${LIB_VERSION}.0") + +set_property(GLOBAL PROPERTY CXX_STANDARD 98) + +option(RECASTNAVIGATION_DEMO "Build demo" OFF) +option(RECASTNAVIGATION_TESTS "Build tests" OFF) +option(RECASTNAVIGATION_EXAMPLES "Build examples" OFF) +option(RECASTNAVIGATION_DT_POLYREF64 "Use 64bit polyrefs instead of 32bit for Detour" OFF) +option(RECASTNAVIGATION_DT_VIRTUAL_QUERYFILTER "Use dynamic dispatch for dtQueryFilter in Detour to allow for custom filters" OFF) + +if(MSVC AND BUILD_SHARED_LIBS) + set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) +endif() + +include(CMakePackageConfigHelpers) +include(GNUInstallDirs) + +configure_file( + "${RecastNavigation_SOURCE_DIR}/version.h.in" + "${RecastNavigation_BINARY_DIR}/version.h") +install(FILES "${RecastNavigation_BINARY_DIR}/version.h" DESTINATION + ${CMAKE_INSTALL_INCLUDEDIR}/recastnavigation) + +# Needed for recastnavigation.pc.in +set(prefix ${CMAKE_INSTALL_PREFIX}) +set(exec_prefix "\${prefix}") +set(libdir "\${exec_prefix}/${CMAKE_INSTALL_LIBDIR}") +set(bindir "\${exec_prefix}/${CMAKE_INSTALL_BINDIR}") +set(includedir "\${prefix}/${CMAKE_INSTALL_INCLUDEDIR}") +set(PACKAGE_VERSION "${LIB_VERSION}") +if(RECASTNAVIGATION_DT_POLYREF64) + set(PKG_CONFIG_CFLAGS "${PKG_CONFIG_CFLAGS} -DDT_POLYREF64") +endif() +if(RECASTNAVIGATION_DT_VIRTUAL_QUERYFILTER) + set(PKG_CONFIG_CFLAGS "${PKG_CONFIG_CFLAGS} -DDT_VIRTUAL_QUERYFILTER") +endif() +configure_file( + "${RecastNavigation_SOURCE_DIR}/recastnavigation.pc.in" + "${RecastNavigation_BINARY_DIR}/recastnavigation.pc" + @ONLY) +install(FILES "${RecastNavigation_BINARY_DIR}/recastnavigation.pc" + DESTINATION "${CMAKE_INSTALL_LIBDIR}/pkgconfig") + +add_subdirectory(DebugUtils) +add_subdirectory(Detour) +add_subdirectory(DetourCrowd) +add_subdirectory(DetourTileCache) +add_subdirectory(Recast) + +configure_package_config_file( + ${PROJECT_SOURCE_DIR}/recastnavigation-config.cmake.in + recastnavigation-config.cmake + INSTALL_DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/recastnavigation +) + +write_basic_package_version_file( + recastnavigation-config-version.cmake + VERSION ${LIB_VERSION} + COMPATIBILITY AnyNewerVersion +) + +export( + EXPORT recastnavigation-targets + NAMESPACE RecastNavigation:: + FILE recastnavigation-targets.cmake +) + +install( + EXPORT recastnavigation-targets + NAMESPACE RecastNavigation:: + FILE recastnavigation-targets.cmake + DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/recastnavigation +) + +install( + FILES + ${PROJECT_BINARY_DIR}/recastnavigation-config.cmake + ${PROJECT_BINARY_DIR}/recastnavigation-config-version.cmake + DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/recastnavigation +) + +if (RECASTNAVIGATION_DEMO) + add_subdirectory(RecastDemo) +endif () + +if (RECASTNAVIGATION_TESTS) + enable_testing() + add_subdirectory(Tests) +endif () diff --git a/Dependencies/recastnavigation/DebugUtils/CMakeLists.txt b/Dependencies/recastnavigation/DebugUtils/CMakeLists.txt new file mode 100644 index 000000000..a3cd7995d --- /dev/null +++ b/Dependencies/recastnavigation/DebugUtils/CMakeLists.txt @@ -0,0 +1,39 @@ +file(GLOB SOURCES Source/*.cpp) +add_library(DebugUtils ${SOURCES}) + +add_library(RecastNavigation::DebugUtils ALIAS DebugUtils) +set_target_properties(DebugUtils PROPERTIES DEBUG_POSTFIX -d) + +set(DebugUtils_INCLUDE_DIR "${CMAKE_CURRENT_SOURCE_DIR}/Include") + +target_include_directories(DebugUtils PUBLIC + "$" +) + +target_link_libraries(DebugUtils + Recast + Detour + DetourTileCache +) + +set_target_properties(DebugUtils PROPERTIES + SOVERSION ${SOVERSION} + VERSION ${LIB_VERSION} + COMPILE_PDB_OUTPUT_DIRECTORY . + COMPILE_PDB_NAME "DebugUtils-d" + ) + +install(TARGETS DebugUtils + EXPORT recastnavigation-targets + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} COMPONENT library + INCLUDES DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} ${CMAKE_INSTALL_INCLUDEDIR}/recastnavigation + ) + +file(GLOB INCLUDES Include/*.h) +install(FILES ${INCLUDES} DESTINATION + ${CMAKE_INSTALL_INCLUDEDIR}/recastnavigation) +if(MSVC) + install(FILES "$/DebugUtils-d.pdb" CONFIGURATIONS "Debug" DESTINATION "lib" OPTIONAL) +endif() diff --git a/Dependencies/recastnavigation/DebugUtils/Include/DebugDraw.h b/Dependencies/recastnavigation/DebugUtils/Include/DebugDraw.h new file mode 100644 index 000000000..8ebe9eb24 --- /dev/null +++ b/Dependencies/recastnavigation/DebugUtils/Include/DebugDraw.h @@ -0,0 +1,225 @@ +// +// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#ifndef DEBUGDRAW_H +#define DEBUGDRAW_H + +// Some math headers don't have PI defined. +static const float DU_PI = 3.14159265f; + +enum duDebugDrawPrimitives +{ + DU_DRAW_POINTS, + DU_DRAW_LINES, + DU_DRAW_TRIS, + DU_DRAW_QUADS +}; + +/// Abstract debug draw interface. +struct duDebugDraw +{ + virtual ~duDebugDraw() = 0; + + virtual void depthMask(bool state) = 0; + + virtual void texture(bool state) = 0; + + /// Begin drawing primitives. + /// @param prim [in] primitive type to draw, one of rcDebugDrawPrimitives. + /// @param size [in] size of a primitive, applies to point size and line width only. + virtual void begin(duDebugDrawPrimitives prim, float size = 1.0f) = 0; + + /// Submit a vertex + /// @param pos [in] position of the verts. + /// @param color [in] color of the verts. + virtual void vertex(const float* pos, unsigned int color) = 0; + + /// Submit a vertex + /// @param x,y,z [in] position of the verts. + /// @param color [in] color of the verts. + virtual void vertex(const float x, const float y, const float z, unsigned int color) = 0; + + /// Submit a vertex + /// @param pos [in] position of the verts. + /// @param color [in] color of the verts. + /// @param uv [in] the uv coordinates of the verts. + virtual void vertex(const float* pos, unsigned int color, const float* uv) = 0; + + /// Submit a vertex + /// @param x,y,z [in] position of the verts. + /// @param color [in] color of the verts. + /// @param u,v [in] the uv coordinates of the verts. + virtual void vertex(const float x, const float y, const float z, unsigned int color, const float u, const float v) = 0; + + /// End drawing primitives. + virtual void end() = 0; + + /// Compute a color for given area. + virtual unsigned int areaToCol(unsigned int area); +}; + +inline unsigned int duRGBA(int r, int g, int b, int a) +{ + return ((unsigned int)r) | ((unsigned int)g << 8) | ((unsigned int)b << 16) | ((unsigned int)a << 24); +} + +inline unsigned int duRGBAf(float fr, float fg, float fb, float fa) +{ + unsigned char r = (unsigned char)(fr*255.0f); + unsigned char g = (unsigned char)(fg*255.0f); + unsigned char b = (unsigned char)(fb*255.0f); + unsigned char a = (unsigned char)(fa*255.0f); + return duRGBA(r,g,b,a); +} + +unsigned int duIntToCol(int i, int a); +void duIntToCol(int i, float* col); + +inline unsigned int duMultCol(const unsigned int col, const unsigned int d) +{ + const unsigned int r = col & 0xff; + const unsigned int g = (col >> 8) & 0xff; + const unsigned int b = (col >> 16) & 0xff; + const unsigned int a = (col >> 24) & 0xff; + return duRGBA((r*d) >> 8, (g*d) >> 8, (b*d) >> 8, a); +} + +inline unsigned int duDarkenCol(unsigned int col) +{ + return ((col >> 1) & 0x007f7f7f) | (col & 0xff000000); +} + +inline unsigned int duLerpCol(unsigned int ca, unsigned int cb, unsigned int u) +{ + const unsigned int ra = ca & 0xff; + const unsigned int ga = (ca >> 8) & 0xff; + const unsigned int ba = (ca >> 16) & 0xff; + const unsigned int aa = (ca >> 24) & 0xff; + const unsigned int rb = cb & 0xff; + const unsigned int gb = (cb >> 8) & 0xff; + const unsigned int bb = (cb >> 16) & 0xff; + const unsigned int ab = (cb >> 24) & 0xff; + + unsigned int r = (ra*(255-u) + rb*u)/255; + unsigned int g = (ga*(255-u) + gb*u)/255; + unsigned int b = (ba*(255-u) + bb*u)/255; + unsigned int a = (aa*(255-u) + ab*u)/255; + return duRGBA(r,g,b,a); +} + +inline unsigned int duTransCol(unsigned int c, unsigned int a) +{ + return (a<<24) | (c & 0x00ffffff); +} + + +void duCalcBoxColors(unsigned int* colors, unsigned int colTop, unsigned int colSide); + +void duDebugDrawCylinderWire(struct duDebugDraw* dd, float minx, float miny, float minz, + float maxx, float maxy, float maxz, unsigned int col, const float lineWidth); + +void duDebugDrawBoxWire(struct duDebugDraw* dd, float minx, float miny, float minz, + float maxx, float maxy, float maxz, unsigned int col, const float lineWidth); + +void duDebugDrawArc(struct duDebugDraw* dd, const float x0, const float y0, const float z0, + const float x1, const float y1, const float z1, const float h, + const float as0, const float as1, unsigned int col, const float lineWidth); + +void duDebugDrawArrow(struct duDebugDraw* dd, const float x0, const float y0, const float z0, + const float x1, const float y1, const float z1, + const float as0, const float as1, unsigned int col, const float lineWidth); + +void duDebugDrawCircle(struct duDebugDraw* dd, const float x, const float y, const float z, + const float r, unsigned int col, const float lineWidth); + +void duDebugDrawCross(struct duDebugDraw* dd, const float x, const float y, const float z, + const float size, unsigned int col, const float lineWidth); + +void duDebugDrawBox(struct duDebugDraw* dd, float minx, float miny, float minz, + float maxx, float maxy, float maxz, const unsigned int* fcol); + +void duDebugDrawCylinder(struct duDebugDraw* dd, float minx, float miny, float minz, + float maxx, float maxy, float maxz, unsigned int col); + +void duDebugDrawGridXZ(struct duDebugDraw* dd, const float ox, const float oy, const float oz, + const int w, const int h, const float size, + const unsigned int col, const float lineWidth); + + +// Versions without begin/end, can be used to draw multiple primitives. +void duAppendCylinderWire(struct duDebugDraw* dd, float minx, float miny, float minz, + float maxx, float maxy, float maxz, unsigned int col); + +void duAppendBoxWire(struct duDebugDraw* dd, float minx, float miny, float minz, + float maxx, float maxy, float maxz, unsigned int col); + +void duAppendBoxPoints(struct duDebugDraw* dd, float minx, float miny, float minz, + float maxx, float maxy, float maxz, unsigned int col); + +void duAppendArc(struct duDebugDraw* dd, const float x0, const float y0, const float z0, + const float x1, const float y1, const float z1, const float h, + const float as0, const float as1, unsigned int col); + +void duAppendArrow(struct duDebugDraw* dd, const float x0, const float y0, const float z0, + const float x1, const float y1, const float z1, + const float as0, const float as1, unsigned int col); + +void duAppendCircle(struct duDebugDraw* dd, const float x, const float y, const float z, + const float r, unsigned int col); + +void duAppendCross(struct duDebugDraw* dd, const float x, const float y, const float z, + const float size, unsigned int col); + +void duAppendBox(struct duDebugDraw* dd, float minx, float miny, float minz, + float maxx, float maxy, float maxz, const unsigned int* fcol); + +void duAppendCylinder(struct duDebugDraw* dd, float minx, float miny, float minz, + float maxx, float maxy, float maxz, unsigned int col); + + +class duDisplayList : public duDebugDraw +{ + float* m_pos; + unsigned int* m_color; + int m_size; + int m_cap; + + duDebugDrawPrimitives m_prim; + float m_primSize; + bool m_depthMask; + + void resize(int cap); + +public: + duDisplayList(int cap = 512); + virtual ~duDisplayList(); + virtual void depthMask(bool state); + virtual void begin(duDebugDrawPrimitives prim, float size = 1.0f); + virtual void vertex(const float x, const float y, const float z, unsigned int color); + virtual void vertex(const float* pos, unsigned int color); + virtual void end(); + void clear(); + void draw(struct duDebugDraw* dd); +private: + // Explicitly disabled copy constructor and copy assignment operator. + duDisplayList(const duDisplayList&); + duDisplayList& operator=(const duDisplayList&); +}; + + +#endif // DEBUGDRAW_H diff --git a/Dependencies/recastnavigation/DebugUtils/Include/DetourDebugDraw.h b/Dependencies/recastnavigation/DebugUtils/Include/DetourDebugDraw.h new file mode 100644 index 000000000..d1a808f1d --- /dev/null +++ b/Dependencies/recastnavigation/DebugUtils/Include/DetourDebugDraw.h @@ -0,0 +1,48 @@ +// +// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#ifndef DETOURDEBUGDRAW_H +#define DETOURDEBUGDRAW_H + +#include "DetourNavMesh.h" +#include "DetourNavMeshQuery.h" +#include "DetourTileCacheBuilder.h" + +enum DrawNavMeshFlags +{ + DU_DRAWNAVMESH_OFFMESHCONS = 0x01, + DU_DRAWNAVMESH_CLOSEDLIST = 0x02, + DU_DRAWNAVMESH_COLOR_TILES = 0x04 +}; + +void duDebugDrawNavMesh(struct duDebugDraw* dd, const dtNavMesh& mesh, unsigned char flags); +void duDebugDrawNavMeshWithClosedList(struct duDebugDraw* dd, const dtNavMesh& mesh, const dtNavMeshQuery& query, unsigned char flags); +void duDebugDrawNavMeshNodes(struct duDebugDraw* dd, const dtNavMeshQuery& query); +void duDebugDrawNavMeshBVTree(struct duDebugDraw* dd, const dtNavMesh& mesh); +void duDebugDrawNavMeshPortals(struct duDebugDraw* dd, const dtNavMesh& mesh); +void duDebugDrawNavMeshPolysWithFlags(struct duDebugDraw* dd, const dtNavMesh& mesh, const unsigned short polyFlags, const unsigned int col); +void duDebugDrawNavMeshPoly(struct duDebugDraw* dd, const dtNavMesh& mesh, dtPolyRef ref, const unsigned int col); + +void duDebugDrawTileCacheLayerAreas(struct duDebugDraw* dd, const dtTileCacheLayer& layer, const float cs, const float ch); +void duDebugDrawTileCacheLayerRegions(struct duDebugDraw* dd, const dtTileCacheLayer& layer, const float cs, const float ch); +void duDebugDrawTileCacheContours(duDebugDraw* dd, const struct dtTileCacheContourSet& lcset, + const float* orig, const float cs, const float ch); +void duDebugDrawTileCachePolyMesh(duDebugDraw* dd, const struct dtTileCachePolyMesh& lmesh, + const float* orig, const float cs, const float ch); + +#endif // DETOURDEBUGDRAW_H diff --git a/Dependencies/recastnavigation/DebugUtils/Include/RecastDebugDraw.h b/Dependencies/recastnavigation/DebugUtils/Include/RecastDebugDraw.h new file mode 100644 index 000000000..6a55fa647 --- /dev/null +++ b/Dependencies/recastnavigation/DebugUtils/Include/RecastDebugDraw.h @@ -0,0 +1,42 @@ +// +// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#ifndef RECAST_DEBUGDRAW_H +#define RECAST_DEBUGDRAW_H + +void duDebugDrawTriMesh(struct duDebugDraw* dd, const float* verts, int nverts, const int* tris, const float* normals, int ntris, const unsigned char* flags, const float texScale); +void duDebugDrawTriMeshSlope(struct duDebugDraw* dd, const float* verts, int nverts, const int* tris, const float* normals, int ntris, const float walkableSlopeAngle, const float texScale); + +void duDebugDrawHeightfieldSolid(struct duDebugDraw* dd, const struct rcHeightfield& hf); +void duDebugDrawHeightfieldWalkable(struct duDebugDraw* dd, const struct rcHeightfield& hf); + +void duDebugDrawCompactHeightfieldSolid(struct duDebugDraw* dd, const struct rcCompactHeightfield& chf); +void duDebugDrawCompactHeightfieldRegions(struct duDebugDraw* dd, const struct rcCompactHeightfield& chf); +void duDebugDrawCompactHeightfieldDistance(struct duDebugDraw* dd, const struct rcCompactHeightfield& chf); + +void duDebugDrawHeightfieldLayer(duDebugDraw* dd, const struct rcHeightfieldLayer& layer, const int idx); +void duDebugDrawHeightfieldLayers(duDebugDraw* dd, const struct rcHeightfieldLayerSet& lset); +void duDebugDrawHeightfieldLayersRegions(duDebugDraw* dd, const struct rcHeightfieldLayerSet& lset); + +void duDebugDrawRegionConnections(struct duDebugDraw* dd, const struct rcContourSet& cset, const float alpha = 1.0f); +void duDebugDrawRawContours(struct duDebugDraw* dd, const struct rcContourSet& cset, const float alpha = 1.0f); +void duDebugDrawContours(struct duDebugDraw* dd, const struct rcContourSet& cset, const float alpha = 1.0f); +void duDebugDrawPolyMesh(struct duDebugDraw* dd, const struct rcPolyMesh& mesh); +void duDebugDrawPolyMeshDetail(struct duDebugDraw* dd, const struct rcPolyMeshDetail& dmesh); + +#endif // RECAST_DEBUGDRAW_H diff --git a/Dependencies/recastnavigation/DebugUtils/Include/RecastDump.h b/Dependencies/recastnavigation/DebugUtils/Include/RecastDump.h new file mode 100644 index 000000000..18cc891a5 --- /dev/null +++ b/Dependencies/recastnavigation/DebugUtils/Include/RecastDump.h @@ -0,0 +1,42 @@ +// +// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#ifndef RECAST_DUMP_H +#define RECAST_DUMP_H + +struct duFileIO +{ + virtual ~duFileIO(); + virtual bool isWriting() const = 0; + virtual bool isReading() const = 0; + virtual bool write(const void* ptr, const size_t size) = 0; + virtual bool read(void* ptr, const size_t size) = 0; +}; + +bool duDumpPolyMeshToObj(struct rcPolyMesh& pmesh, duFileIO* io); +bool duDumpPolyMeshDetailToObj(struct rcPolyMeshDetail& dmesh, duFileIO* io); + +bool duDumpContourSet(struct rcContourSet& cset, duFileIO* io); +bool duReadContourSet(struct rcContourSet& cset, duFileIO* io); + +bool duDumpCompactHeightfield(struct rcCompactHeightfield& chf, duFileIO* io); +bool duReadCompactHeightfield(struct rcCompactHeightfield& chf, duFileIO* io); + +void duLogBuildTimes(rcContext& ctx, const int totalTileUsec); + +#endif // RECAST_DUMP_H diff --git a/Dependencies/recastnavigation/DebugUtils/Source/DebugDraw.cpp b/Dependencies/recastnavigation/DebugUtils/Source/DebugDraw.cpp new file mode 100644 index 000000000..655695d1c --- /dev/null +++ b/Dependencies/recastnavigation/DebugUtils/Source/DebugDraw.cpp @@ -0,0 +1,611 @@ +// +// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#include +#include "DebugDraw.h" +#include "DetourMath.h" +#include "DetourNavMesh.h" + + +duDebugDraw::~duDebugDraw() +{ + // Empty +} + +unsigned int duDebugDraw::areaToCol(unsigned int area) +{ + if (area == 0) + { + // Treat zero area type as default. + return duRGBA(0, 192, 255, 255); + } + else + { + return duIntToCol(area, 255); + } +} + +inline int bit(int a, int b) +{ + return (a & (1 << b)) >> b; +} + +unsigned int duIntToCol(int i, int a) +{ + int r = bit(i, 1) + bit(i, 3) * 2 + 1; + int g = bit(i, 2) + bit(i, 4) * 2 + 1; + int b = bit(i, 0) + bit(i, 5) * 2 + 1; + return duRGBA(r*63,g*63,b*63,a); +} + +void duIntToCol(int i, float* col) +{ + int r = bit(i, 0) + bit(i, 3) * 2 + 1; + int g = bit(i, 1) + bit(i, 4) * 2 + 1; + int b = bit(i, 2) + bit(i, 5) * 2 + 1; + col[0] = 1 - r*63.0f/255.0f; + col[1] = 1 - g*63.0f/255.0f; + col[2] = 1 - b*63.0f/255.0f; +} + +void duCalcBoxColors(unsigned int* colors, unsigned int colTop, unsigned int colSide) +{ + if (!colors) return; + + colors[0] = duMultCol(colTop, 250); + colors[1] = duMultCol(colSide, 140); + colors[2] = duMultCol(colSide, 165); + colors[3] = duMultCol(colSide, 217); + colors[4] = duMultCol(colSide, 165); + colors[5] = duMultCol(colSide, 217); +} + +void duDebugDrawCylinderWire(struct duDebugDraw* dd, float minx, float miny, float minz, + float maxx, float maxy, float maxz, unsigned int col, const float lineWidth) +{ + if (!dd) return; + + dd->begin(DU_DRAW_LINES, lineWidth); + duAppendCylinderWire(dd, minx,miny,minz, maxx,maxy,maxz, col); + dd->end(); +} + +void duDebugDrawBoxWire(struct duDebugDraw* dd, float minx, float miny, float minz, + float maxx, float maxy, float maxz, unsigned int col, const float lineWidth) +{ + if (!dd) return; + + dd->begin(DU_DRAW_LINES, lineWidth); + duAppendBoxWire(dd, minx,miny,minz, maxx,maxy,maxz, col); + dd->end(); +} + +void duDebugDrawArc(struct duDebugDraw* dd, const float x0, const float y0, const float z0, + const float x1, const float y1, const float z1, const float h, + const float as0, const float as1, unsigned int col, const float lineWidth) +{ + if (!dd) return; + + dd->begin(DU_DRAW_LINES, lineWidth); + duAppendArc(dd, x0,y0,z0, x1,y1,z1, h, as0, as1, col); + dd->end(); +} + +void duDebugDrawArrow(struct duDebugDraw* dd, const float x0, const float y0, const float z0, + const float x1, const float y1, const float z1, + const float as0, const float as1, unsigned int col, const float lineWidth) +{ + if (!dd) return; + + dd->begin(DU_DRAW_LINES, lineWidth); + duAppendArrow(dd, x0,y0,z0, x1,y1,z1, as0, as1, col); + dd->end(); +} + +void duDebugDrawCircle(struct duDebugDraw* dd, const float x, const float y, const float z, + const float r, unsigned int col, const float lineWidth) +{ + if (!dd) return; + + dd->begin(DU_DRAW_LINES, lineWidth); + duAppendCircle(dd, x,y,z, r, col); + dd->end(); +} + +void duDebugDrawCross(struct duDebugDraw* dd, const float x, const float y, const float z, + const float size, unsigned int col, const float lineWidth) +{ + if (!dd) return; + + dd->begin(DU_DRAW_LINES, lineWidth); + duAppendCross(dd, x,y,z, size, col); + dd->end(); +} + +void duDebugDrawBox(struct duDebugDraw* dd, float minx, float miny, float minz, + float maxx, float maxy, float maxz, const unsigned int* fcol) +{ + if (!dd) return; + + dd->begin(DU_DRAW_QUADS); + duAppendBox(dd, minx,miny,minz, maxx,maxy,maxz, fcol); + dd->end(); +} + +void duDebugDrawCylinder(struct duDebugDraw* dd, float minx, float miny, float minz, + float maxx, float maxy, float maxz, unsigned int col) +{ + if (!dd) return; + + dd->begin(DU_DRAW_TRIS); + duAppendCylinder(dd, minx,miny,minz, maxx,maxy,maxz, col); + dd->end(); +} + +void duDebugDrawGridXZ(struct duDebugDraw* dd, const float ox, const float oy, const float oz, + const int w, const int h, const float size, + const unsigned int col, const float lineWidth) +{ + if (!dd) return; + + dd->begin(DU_DRAW_LINES, lineWidth); + for (int i = 0; i <= h; ++i) + { + dd->vertex(ox,oy,oz+i*size, col); + dd->vertex(ox+w*size,oy,oz+i*size, col); + } + for (int i = 0; i <= w; ++i) + { + dd->vertex(ox+i*size,oy,oz, col); + dd->vertex(ox+i*size,oy,oz+h*size, col); + } + dd->end(); +} + + +void duAppendCylinderWire(struct duDebugDraw* dd, float minx, float miny, float minz, + float maxx, float maxy, float maxz, unsigned int col) +{ + if (!dd) return; + + static const int NUM_SEG = 16; + static float dir[NUM_SEG*2]; + static bool init = false; + if (!init) + { + init = true; + for (int i = 0; i < NUM_SEG; ++i) + { + const float a = (float)i/(float)NUM_SEG*DU_PI*2; + dir[i*2] = dtMathCosf(a); + dir[i*2+1] = dtMathSinf(a); + } + } + + const float cx = (maxx + minx)/2; + const float cz = (maxz + minz)/2; + const float rx = (maxx - minx)/2; + const float rz = (maxz - minz)/2; + + for (int i = 0, j = NUM_SEG-1; i < NUM_SEG; j = i++) + { + dd->vertex(cx+dir[j*2+0]*rx, miny, cz+dir[j*2+1]*rz, col); + dd->vertex(cx+dir[i*2+0]*rx, miny, cz+dir[i*2+1]*rz, col); + dd->vertex(cx+dir[j*2+0]*rx, maxy, cz+dir[j*2+1]*rz, col); + dd->vertex(cx+dir[i*2+0]*rx, maxy, cz+dir[i*2+1]*rz, col); + } + for (int i = 0; i < NUM_SEG; i += NUM_SEG/4) + { + dd->vertex(cx+dir[i*2+0]*rx, miny, cz+dir[i*2+1]*rz, col); + dd->vertex(cx+dir[i*2+0]*rx, maxy, cz+dir[i*2+1]*rz, col); + } +} + +void duAppendBoxWire(struct duDebugDraw* dd, float minx, float miny, float minz, + float maxx, float maxy, float maxz, unsigned int col) +{ + if (!dd) return; + // Top + dd->vertex(minx, miny, minz, col); + dd->vertex(maxx, miny, minz, col); + dd->vertex(maxx, miny, minz, col); + dd->vertex(maxx, miny, maxz, col); + dd->vertex(maxx, miny, maxz, col); + dd->vertex(minx, miny, maxz, col); + dd->vertex(minx, miny, maxz, col); + dd->vertex(minx, miny, minz, col); + + // bottom + dd->vertex(minx, maxy, minz, col); + dd->vertex(maxx, maxy, minz, col); + dd->vertex(maxx, maxy, minz, col); + dd->vertex(maxx, maxy, maxz, col); + dd->vertex(maxx, maxy, maxz, col); + dd->vertex(minx, maxy, maxz, col); + dd->vertex(minx, maxy, maxz, col); + dd->vertex(minx, maxy, minz, col); + + // Sides + dd->vertex(minx, miny, minz, col); + dd->vertex(minx, maxy, minz, col); + dd->vertex(maxx, miny, minz, col); + dd->vertex(maxx, maxy, minz, col); + dd->vertex(maxx, miny, maxz, col); + dd->vertex(maxx, maxy, maxz, col); + dd->vertex(minx, miny, maxz, col); + dd->vertex(minx, maxy, maxz, col); +} + +void duAppendBoxPoints(struct duDebugDraw* dd, float minx, float miny, float minz, + float maxx, float maxy, float maxz, unsigned int col) +{ + if (!dd) return; + // Top + dd->vertex(minx, miny, minz, col); + dd->vertex(maxx, miny, minz, col); + dd->vertex(maxx, miny, minz, col); + dd->vertex(maxx, miny, maxz, col); + dd->vertex(maxx, miny, maxz, col); + dd->vertex(minx, miny, maxz, col); + dd->vertex(minx, miny, maxz, col); + dd->vertex(minx, miny, minz, col); + + // bottom + dd->vertex(minx, maxy, minz, col); + dd->vertex(maxx, maxy, minz, col); + dd->vertex(maxx, maxy, minz, col); + dd->vertex(maxx, maxy, maxz, col); + dd->vertex(maxx, maxy, maxz, col); + dd->vertex(minx, maxy, maxz, col); + dd->vertex(minx, maxy, maxz, col); + dd->vertex(minx, maxy, minz, col); +} + +void duAppendBox(struct duDebugDraw* dd, float minx, float miny, float minz, + float maxx, float maxy, float maxz, const unsigned int* fcol) +{ + if (!dd) return; + const float verts[8*3] = + { + minx, miny, minz, + maxx, miny, minz, + maxx, miny, maxz, + minx, miny, maxz, + minx, maxy, minz, + maxx, maxy, minz, + maxx, maxy, maxz, + minx, maxy, maxz, + }; + static const unsigned char inds[6*4] = + { + 7, 6, 5, 4, + 0, 1, 2, 3, + 1, 5, 6, 2, + 3, 7, 4, 0, + 2, 6, 7, 3, + 0, 4, 5, 1, + }; + + const unsigned char* in = inds; + for (int i = 0; i < 6; ++i) + { + dd->vertex(&verts[*in*3], fcol[i]); in++; + dd->vertex(&verts[*in*3], fcol[i]); in++; + dd->vertex(&verts[*in*3], fcol[i]); in++; + dd->vertex(&verts[*in*3], fcol[i]); in++; + } +} + +void duAppendCylinder(struct duDebugDraw* dd, float minx, float miny, float minz, + float maxx, float maxy, float maxz, unsigned int col) +{ + if (!dd) return; + + static const int NUM_SEG = 16; + static float dir[NUM_SEG*2]; + static bool init = false; + if (!init) + { + init = true; + for (int i = 0; i < NUM_SEG; ++i) + { + const float a = (float)i/(float)NUM_SEG*DU_PI*2; + dir[i*2] = cosf(a); + dir[i*2+1] = sinf(a); + } + } + + unsigned int col2 = duMultCol(col, 160); + + const float cx = (maxx + minx)/2; + const float cz = (maxz + minz)/2; + const float rx = (maxx - minx)/2; + const float rz = (maxz - minz)/2; + + for (int i = 2; i < NUM_SEG; ++i) + { + const int a = 0, b = i-1, c = i; + dd->vertex(cx+dir[a*2+0]*rx, miny, cz+dir[a*2+1]*rz, col2); + dd->vertex(cx+dir[b*2+0]*rx, miny, cz+dir[b*2+1]*rz, col2); + dd->vertex(cx+dir[c*2+0]*rx, miny, cz+dir[c*2+1]*rz, col2); + } + for (int i = 2; i < NUM_SEG; ++i) + { + const int a = 0, b = i, c = i-1; + dd->vertex(cx+dir[a*2+0]*rx, maxy, cz+dir[a*2+1]*rz, col); + dd->vertex(cx+dir[b*2+0]*rx, maxy, cz+dir[b*2+1]*rz, col); + dd->vertex(cx+dir[c*2+0]*rx, maxy, cz+dir[c*2+1]*rz, col); + } + for (int i = 0, j = NUM_SEG-1; i < NUM_SEG; j = i++) + { + dd->vertex(cx+dir[i*2+0]*rx, miny, cz+dir[i*2+1]*rz, col2); + dd->vertex(cx+dir[j*2+0]*rx, miny, cz+dir[j*2+1]*rz, col2); + dd->vertex(cx+dir[j*2+0]*rx, maxy, cz+dir[j*2+1]*rz, col); + + dd->vertex(cx+dir[i*2+0]*rx, miny, cz+dir[i*2+1]*rz, col2); + dd->vertex(cx+dir[j*2+0]*rx, maxy, cz+dir[j*2+1]*rz, col); + dd->vertex(cx+dir[i*2+0]*rx, maxy, cz+dir[i*2+1]*rz, col); + } +} + + +inline void evalArc(const float x0, const float y0, const float z0, + const float dx, const float dy, const float dz, + const float h, const float u, float* res) +{ + res[0] = x0 + dx * u; + res[1] = y0 + dy * u + h * (1-(u*2-1)*(u*2-1)); + res[2] = z0 + dz * u; +} + + +inline void vcross(float* dest, const float* v1, const float* v2) +{ + dest[0] = v1[1]*v2[2] - v1[2]*v2[1]; + dest[1] = v1[2]*v2[0] - v1[0]*v2[2]; + dest[2] = v1[0]*v2[1] - v1[1]*v2[0]; +} + +inline void vnormalize(float* v) +{ + float d = 1.0f / sqrtf(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]); + v[0] *= d; + v[1] *= d; + v[2] *= d; +} + +inline void vsub(float* dest, const float* v1, const float* v2) +{ + dest[0] = v1[0]-v2[0]; + dest[1] = v1[1]-v2[1]; + dest[2] = v1[2]-v2[2]; +} + +inline float vdistSqr(const float* v1, const float* v2) +{ + const float x = v1[0]-v2[0]; + const float y = v1[1]-v2[1]; + const float z = v1[2]-v2[2]; + return x*x + y*y + z*z; +} + + +void appendArrowHead(struct duDebugDraw* dd, const float* p, const float* q, + const float s, unsigned int col) +{ + const float eps = 0.001f; + if (!dd) return; + if (vdistSqr(p,q) < eps*eps) return; + float ax[3], ay[3] = {0,1,0}, az[3]; + vsub(az, q, p); + vnormalize(az); + vcross(ax, ay, az); + vcross(ay, az, ax); + vnormalize(ay); + + dd->vertex(p, col); +// dd->vertex(p[0]+az[0]*s+ay[0]*s/2, p[1]+az[1]*s+ay[1]*s/2, p[2]+az[2]*s+ay[2]*s/2, col); + dd->vertex(p[0]+az[0]*s+ax[0]*s/3, p[1]+az[1]*s+ax[1]*s/3, p[2]+az[2]*s+ax[2]*s/3, col); + + dd->vertex(p, col); +// dd->vertex(p[0]+az[0]*s-ay[0]*s/2, p[1]+az[1]*s-ay[1]*s/2, p[2]+az[2]*s-ay[2]*s/2, col); + dd->vertex(p[0]+az[0]*s-ax[0]*s/3, p[1]+az[1]*s-ax[1]*s/3, p[2]+az[2]*s-ax[2]*s/3, col); + +} + +void duAppendArc(struct duDebugDraw* dd, const float x0, const float y0, const float z0, + const float x1, const float y1, const float z1, const float h, + const float as0, const float as1, unsigned int col) +{ + if (!dd) return; + static const int NUM_ARC_PTS = 8; + static const float PAD = 0.05f; + static const float ARC_PTS_SCALE = (1.0f-PAD*2) / (float)NUM_ARC_PTS; + const float dx = x1 - x0; + const float dy = y1 - y0; + const float dz = z1 - z0; + const float len = sqrtf(dx*dx + dy*dy + dz*dz); + float prev[3]; + evalArc(x0,y0,z0, dx,dy,dz, len*h, PAD, prev); + for (int i = 1; i <= NUM_ARC_PTS; ++i) + { + const float u = PAD + i * ARC_PTS_SCALE; + float pt[3]; + evalArc(x0,y0,z0, dx,dy,dz, len*h, u, pt); + dd->vertex(prev[0],prev[1],prev[2], col); + dd->vertex(pt[0],pt[1],pt[2], col); + prev[0] = pt[0]; prev[1] = pt[1]; prev[2] = pt[2]; + } + + // End arrows + if (as0 > 0.001f) + { + float p[3], q[3]; + evalArc(x0,y0,z0, dx,dy,dz, len*h, PAD, p); + evalArc(x0,y0,z0, dx,dy,dz, len*h, PAD+0.05f, q); + appendArrowHead(dd, p, q, as0, col); + } + + if (as1 > 0.001f) + { + float p[3], q[3]; + evalArc(x0,y0,z0, dx,dy,dz, len*h, 1-PAD, p); + evalArc(x0,y0,z0, dx,dy,dz, len*h, 1-(PAD+0.05f), q); + appendArrowHead(dd, p, q, as1, col); + } +} + +void duAppendArrow(struct duDebugDraw* dd, const float x0, const float y0, const float z0, + const float x1, const float y1, const float z1, + const float as0, const float as1, unsigned int col) +{ + if (!dd) return; + + dd->vertex(x0,y0,z0, col); + dd->vertex(x1,y1,z1, col); + + // End arrows + const float p[3] = {x0,y0,z0}, q[3] = {x1,y1,z1}; + if (as0 > 0.001f) + appendArrowHead(dd, p, q, as0, col); + if (as1 > 0.001f) + appendArrowHead(dd, q, p, as1, col); +} + +void duAppendCircle(struct duDebugDraw* dd, const float x, const float y, const float z, + const float r, unsigned int col) +{ + if (!dd) return; + static const int NUM_SEG = 40; + static float dir[40*2]; + static bool init = false; + if (!init) + { + init = true; + for (int i = 0; i < NUM_SEG; ++i) + { + const float a = (float)i/(float)NUM_SEG*DU_PI*2; + dir[i*2] = cosf(a); + dir[i*2+1] = sinf(a); + } + } + + for (int i = 0, j = NUM_SEG-1; i < NUM_SEG; j = i++) + { + dd->vertex(x+dir[j*2+0]*r, y, z+dir[j*2+1]*r, col); + dd->vertex(x+dir[i*2+0]*r, y, z+dir[i*2+1]*r, col); + } +} + +void duAppendCross(struct duDebugDraw* dd, const float x, const float y, const float z, + const float s, unsigned int col) +{ + if (!dd) return; + dd->vertex(x-s,y,z, col); + dd->vertex(x+s,y,z, col); + dd->vertex(x,y-s,z, col); + dd->vertex(x,y+s,z, col); + dd->vertex(x,y,z-s, col); + dd->vertex(x,y,z+s, col); +} + +duDisplayList::duDisplayList(int cap) : + m_pos(0), + m_color(0), + m_size(0), + m_cap(0), + m_prim(DU_DRAW_LINES), + m_primSize(1.0f), + m_depthMask(true) +{ + if (cap < 8) + cap = 8; + resize(cap); +} + +duDisplayList::~duDisplayList() +{ + delete [] m_pos; + delete [] m_color; +} + +void duDisplayList::resize(int cap) +{ + float* newPos = new float[cap*3]; + if (m_size) + memcpy(newPos, m_pos, sizeof(float)*3*m_size); + delete [] m_pos; + m_pos = newPos; + + unsigned int* newColor = new unsigned int[cap]; + if (m_size) + memcpy(newColor, m_color, sizeof(unsigned int)*m_size); + delete [] m_color; + m_color = newColor; + + m_cap = cap; +} + +void duDisplayList::clear() +{ + m_size = 0; +} + +void duDisplayList::depthMask(bool state) +{ + m_depthMask = state; +} + +void duDisplayList::begin(duDebugDrawPrimitives prim, float size) +{ + clear(); + m_prim = prim; + m_primSize = size; +} + +void duDisplayList::vertex(const float x, const float y, const float z, unsigned int color) +{ + if (m_size+1 >= m_cap) + resize(m_cap*2); + float* p = &m_pos[m_size*3]; + p[0] = x; + p[1] = y; + p[2] = z; + m_color[m_size] = color; + m_size++; +} + +void duDisplayList::vertex(const float* pos, unsigned int color) +{ + vertex(pos[0],pos[1],pos[2],color); +} + +void duDisplayList::end() +{ +} + +void duDisplayList::draw(struct duDebugDraw* dd) +{ + if (!dd) return; + if (!m_size) return; + dd->depthMask(m_depthMask); + dd->begin(m_prim, m_primSize); + for (int i = 0; i < m_size; ++i) + dd->vertex(&m_pos[i*3], m_color[i]); + dd->end(); +} diff --git a/Dependencies/recastnavigation/DebugUtils/Source/DetourDebugDraw.cpp b/Dependencies/recastnavigation/DebugUtils/Source/DetourDebugDraw.cpp new file mode 100644 index 000000000..4ca0581c7 --- /dev/null +++ b/Dependencies/recastnavigation/DebugUtils/Source/DetourDebugDraw.cpp @@ -0,0 +1,864 @@ +// +// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#include "DebugDraw.h" +#include "DetourDebugDraw.h" +#include "DetourNavMesh.h" +#include "DetourCommon.h" +#include "DetourNode.h" + + +static float distancePtLine2d(const float* pt, const float* p, const float* q) +{ + 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; + float t = pqx*dx + pqz*dz; + if (d != 0) t /= d; + dx = p[0] + t*pqx - pt[0]; + dz = p[2] + t*pqz - pt[2]; + return dx*dx + dz*dz; +} + +static void drawPolyBoundaries(duDebugDraw* dd, const dtMeshTile* tile, + const unsigned int col, const float linew, + bool inner) +{ + static const float thr = 0.01f*0.01f; + + dd->begin(DU_DRAW_LINES, linew); + + for (int i = 0; i < tile->header->polyCount; ++i) + { + const dtPoly* p = &tile->polys[i]; + + if (p->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) continue; + + const dtPolyDetail* pd = &tile->detailMeshes[i]; + + for (int j = 0, nj = (int)p->vertCount; j < nj; ++j) + { + unsigned int c = col; + if (inner) + { + if (p->neis[j] == 0) continue; + if (p->neis[j] & DT_EXT_LINK) + { + bool con = false; + for (unsigned int k = p->firstLink; k != DT_NULL_LINK; k = tile->links[k].next) + { + if (tile->links[k].edge == j) + { + con = true; + break; + } + } + if (con) + c = duRGBA(255,255,255,48); + else + c = duRGBA(0,0,0,48); + } + else + c = duRGBA(0,48,64,32); + } + else + { + if (p->neis[j] != 0) continue; + } + + const float* v0 = &tile->verts[p->verts[j]*3]; + const float* v1 = &tile->verts[p->verts[(j+1) % nj]*3]; + + // Draw detail mesh edges which align with the actual poly edge. + // This is really slow. + for (int k = 0; k < pd->triCount; ++k) + { + const unsigned char* t = &tile->detailTris[(pd->triBase+k)*4]; + const float* tv[3]; + for (int m = 0; m < 3; ++m) + { + if (t[m] < p->vertCount) + tv[m] = &tile->verts[p->verts[t[m]]*3]; + else + tv[m] = &tile->detailVerts[(pd->vertBase+(t[m]-p->vertCount))*3]; + } + for (int m = 0, n = 2; m < 3; n=m++) + { + if ((dtGetDetailTriEdgeFlags(t[3], n) & DT_DETAIL_EDGE_BOUNDARY) == 0) + continue; + + if (distancePtLine2d(tv[n],v0,v1) < thr && + distancePtLine2d(tv[m],v0,v1) < thr) + { + dd->vertex(tv[n], c); + dd->vertex(tv[m], c); + } + } + } + } + } + dd->end(); +} + +static void drawMeshTile(duDebugDraw* dd, const dtNavMesh& mesh, const dtNavMeshQuery* query, + const dtMeshTile* tile, unsigned char flags) +{ + dtPolyRef base = mesh.getPolyRefBase(tile); + + int tileNum = mesh.decodePolyIdTile(base); + const unsigned int tileColor = duIntToCol(tileNum, 128); + + dd->depthMask(false); + + dd->begin(DU_DRAW_TRIS); + for (int i = 0; i < tile->header->polyCount; ++i) + { + const dtPoly* p = &tile->polys[i]; + if (p->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) // Skip off-mesh links. + continue; + + const dtPolyDetail* pd = &tile->detailMeshes[i]; + + unsigned int col; + if (query && query->isInClosedList(base | (dtPolyRef)i)) + col = duRGBA(255,196,0,64); + else + { + if (flags & DU_DRAWNAVMESH_COLOR_TILES) + col = tileColor; + else + col = duTransCol(dd->areaToCol(p->getArea()), 64); + } + + for (int j = 0; j < pd->triCount; ++j) + { + const unsigned char* t = &tile->detailTris[(pd->triBase+j)*4]; + for (int k = 0; k < 3; ++k) + { + if (t[k] < p->vertCount) + dd->vertex(&tile->verts[p->verts[t[k]]*3], col); + else + dd->vertex(&tile->detailVerts[(pd->vertBase+t[k]-p->vertCount)*3], col); + } + } + } + dd->end(); + + // Draw inter poly boundaries + drawPolyBoundaries(dd, tile, duRGBA(0,48,64,32), 1.5f, true); + + // Draw outer poly boundaries + drawPolyBoundaries(dd, tile, duRGBA(0,48,64,220), 2.5f, false); + + if (flags & DU_DRAWNAVMESH_OFFMESHCONS) + { + dd->begin(DU_DRAW_LINES, 2.0f); + for (int i = 0; i < tile->header->polyCount; ++i) + { + const dtPoly* p = &tile->polys[i]; + if (p->getType() != DT_POLYTYPE_OFFMESH_CONNECTION) // Skip regular polys. + continue; + + unsigned int col, col2; + if (query && query->isInClosedList(base | (dtPolyRef)i)) + col = duRGBA(255,196,0,220); + else + col = duDarkenCol(duTransCol(dd->areaToCol(p->getArea()), 220)); + + const dtOffMeshConnection* con = &tile->offMeshCons[i - tile->header->offMeshBase]; + const float* va = &tile->verts[p->verts[0]*3]; + const float* vb = &tile->verts[p->verts[1]*3]; + + // Check to see if start and end end-points have links. + bool startSet = false; + bool endSet = false; + for (unsigned int k = p->firstLink; k != DT_NULL_LINK; k = tile->links[k].next) + { + if (tile->links[k].edge == 0) + startSet = true; + if (tile->links[k].edge == 1) + endSet = true; + } + + // End points and their on-mesh locations. + dd->vertex(va[0],va[1],va[2], col); + dd->vertex(con->pos[0],con->pos[1],con->pos[2], col); + col2 = startSet ? col : duRGBA(220,32,16,196); + duAppendCircle(dd, con->pos[0],con->pos[1]+0.1f,con->pos[2], con->rad, col2); + + dd->vertex(vb[0],vb[1],vb[2], col); + dd->vertex(con->pos[3],con->pos[4],con->pos[5], col); + col2 = endSet ? col : duRGBA(220,32,16,196); + duAppendCircle(dd, con->pos[3],con->pos[4]+0.1f,con->pos[5], con->rad, col2); + + // End point vertices. + dd->vertex(con->pos[0],con->pos[1],con->pos[2], duRGBA(0,48,64,196)); + dd->vertex(con->pos[0],con->pos[1]+0.2f,con->pos[2], duRGBA(0,48,64,196)); + + dd->vertex(con->pos[3],con->pos[4],con->pos[5], duRGBA(0,48,64,196)); + dd->vertex(con->pos[3],con->pos[4]+0.2f,con->pos[5], duRGBA(0,48,64,196)); + + // Connection arc. + duAppendArc(dd, con->pos[0],con->pos[1],con->pos[2], con->pos[3],con->pos[4],con->pos[5], 0.25f, + (con->flags & 1) ? 0.6f : 0, 0.6f, col); + } + dd->end(); + } + + const unsigned int vcol = duRGBA(0,0,0,196); + dd->begin(DU_DRAW_POINTS, 3.0f); + for (int i = 0; i < tile->header->vertCount; ++i) + { + const float* v = &tile->verts[i*3]; + dd->vertex(v[0], v[1], v[2], vcol); + } + dd->end(); + + dd->depthMask(true); +} + +void duDebugDrawNavMesh(duDebugDraw* dd, const dtNavMesh& mesh, unsigned char flags) +{ + if (!dd) return; + + for (int i = 0; i < mesh.getMaxTiles(); ++i) + { + const dtMeshTile* tile = mesh.getTile(i); + if (!tile->header) continue; + drawMeshTile(dd, mesh, 0, tile, flags); + } +} + +void duDebugDrawNavMeshWithClosedList(struct duDebugDraw* dd, const dtNavMesh& mesh, const dtNavMeshQuery& query, unsigned char flags) +{ + if (!dd) return; + + const dtNavMeshQuery* q = (flags & DU_DRAWNAVMESH_CLOSEDLIST) ? &query : 0; + + for (int i = 0; i < mesh.getMaxTiles(); ++i) + { + const dtMeshTile* tile = mesh.getTile(i); + if (!tile->header) continue; + drawMeshTile(dd, mesh, q, tile, flags); + } +} + +void duDebugDrawNavMeshNodes(struct duDebugDraw* dd, const dtNavMeshQuery& query) +{ + if (!dd) return; + + const dtNodePool* pool = query.getNodePool(); + if (pool) + { + const float off = 0.5f; + dd->begin(DU_DRAW_POINTS, 4.0f); + for (int i = 0; i < pool->getHashSize(); ++i) + { + for (dtNodeIndex j = pool->getFirst(i); j != DT_NULL_IDX; j = pool->getNext(j)) + { + const dtNode* node = pool->getNodeAtIdx(j+1); + if (!node) continue; + dd->vertex(node->pos[0],node->pos[1]+off,node->pos[2], duRGBA(255,192,0,255)); + } + } + dd->end(); + + dd->begin(DU_DRAW_LINES, 2.0f); + for (int i = 0; i < pool->getHashSize(); ++i) + { + for (dtNodeIndex j = pool->getFirst(i); j != DT_NULL_IDX; j = pool->getNext(j)) + { + const dtNode* node = pool->getNodeAtIdx(j+1); + if (!node) continue; + if (!node->pidx) continue; + const dtNode* parent = pool->getNodeAtIdx(node->pidx); + if (!parent) continue; + dd->vertex(node->pos[0],node->pos[1]+off,node->pos[2], duRGBA(255,192,0,128)); + dd->vertex(parent->pos[0],parent->pos[1]+off,parent->pos[2], duRGBA(255,192,0,128)); + } + } + dd->end(); + } +} + + +static void drawMeshTileBVTree(duDebugDraw* dd, const dtMeshTile* tile) +{ + // Draw BV nodes. + const float cs = 1.0f / tile->header->bvQuantFactor; + dd->begin(DU_DRAW_LINES, 1.0f); + for (int i = 0; i < tile->header->bvNodeCount; ++i) + { + const dtBVNode* n = &tile->bvTree[i]; + if (n->i < 0) // Leaf indices are positive. + continue; + duAppendBoxWire(dd, tile->header->bmin[0] + n->bmin[0]*cs, + tile->header->bmin[1] + n->bmin[1]*cs, + tile->header->bmin[2] + n->bmin[2]*cs, + tile->header->bmin[0] + n->bmax[0]*cs, + tile->header->bmin[1] + n->bmax[1]*cs, + tile->header->bmin[2] + n->bmax[2]*cs, + duRGBA(255,255,255,128)); + } + dd->end(); +} + +void duDebugDrawNavMeshBVTree(duDebugDraw* dd, const dtNavMesh& mesh) +{ + if (!dd) return; + + for (int i = 0; i < mesh.getMaxTiles(); ++i) + { + const dtMeshTile* tile = mesh.getTile(i); + if (!tile->header) continue; + drawMeshTileBVTree(dd, tile); + } +} + +static void drawMeshTilePortal(duDebugDraw* dd, const dtMeshTile* tile) +{ + // Draw portals + const float padx = 0.04f; + const float pady = tile->header->walkableClimb; + + dd->begin(DU_DRAW_LINES, 2.0f); + + for (int side = 0; side < 8; ++side) + { + unsigned short m = DT_EXT_LINK | (unsigned short)side; + + for (int i = 0; i < tile->header->polyCount; ++i) + { + dtPoly* poly = &tile->polys[i]; + + // Create new links. + const int nv = poly->vertCount; + for (int j = 0; j < nv; ++j) + { + // Skip edges which do not point to the right side. + if (poly->neis[j] != m) + continue; + + // Create new links + const float* va = &tile->verts[poly->verts[j]*3]; + const float* vb = &tile->verts[poly->verts[(j+1) % nv]*3]; + + if (side == 0 || side == 4) + { + unsigned int col = side == 0 ? duRGBA(128,0,0,128) : duRGBA(128,0,128,128); + + const float x = va[0] + ((side == 0) ? -padx : padx); + + dd->vertex(x,va[1]-pady,va[2], col); + dd->vertex(x,va[1]+pady,va[2], col); + + dd->vertex(x,va[1]+pady,va[2], col); + dd->vertex(x,vb[1]+pady,vb[2], col); + + dd->vertex(x,vb[1]+pady,vb[2], col); + dd->vertex(x,vb[1]-pady,vb[2], col); + + dd->vertex(x,vb[1]-pady,vb[2], col); + dd->vertex(x,va[1]-pady,va[2], col); + } + else if (side == 2 || side == 6) + { + unsigned int col = side == 2 ? duRGBA(0,128,0,128) : duRGBA(0,128,128,128); + + const float z = va[2] + ((side == 2) ? -padx : padx); + + dd->vertex(va[0],va[1]-pady,z, col); + dd->vertex(va[0],va[1]+pady,z, col); + + dd->vertex(va[0],va[1]+pady,z, col); + dd->vertex(vb[0],vb[1]+pady,z, col); + + dd->vertex(vb[0],vb[1]+pady,z, col); + dd->vertex(vb[0],vb[1]-pady,z, col); + + dd->vertex(vb[0],vb[1]-pady,z, col); + dd->vertex(va[0],va[1]-pady,z, col); + } + + } + } + } + + dd->end(); +} + +void duDebugDrawNavMeshPortals(duDebugDraw* dd, const dtNavMesh& mesh) +{ + if (!dd) return; + + for (int i = 0; i < mesh.getMaxTiles(); ++i) + { + const dtMeshTile* tile = mesh.getTile(i); + if (!tile->header) continue; + drawMeshTilePortal(dd, tile); + } +} + +void duDebugDrawNavMeshPolysWithFlags(struct duDebugDraw* dd, const dtNavMesh& mesh, + const unsigned short polyFlags, const unsigned int col) +{ + if (!dd) return; + + for (int i = 0; i < mesh.getMaxTiles(); ++i) + { + const dtMeshTile* tile = mesh.getTile(i); + if (!tile->header) continue; + dtPolyRef base = mesh.getPolyRefBase(tile); + + for (int j = 0; j < tile->header->polyCount; ++j) + { + const dtPoly* p = &tile->polys[j]; + if ((p->flags & polyFlags) == 0) continue; + duDebugDrawNavMeshPoly(dd, mesh, base|(dtPolyRef)j, col); + } + } +} + +void duDebugDrawNavMeshPoly(duDebugDraw* dd, const dtNavMesh& mesh, dtPolyRef ref, const unsigned int col) +{ + if (!dd) return; + + const dtMeshTile* tile = 0; + const dtPoly* poly = 0; + if (dtStatusFailed(mesh.getTileAndPolyByRef(ref, &tile, &poly))) + return; + + dd->depthMask(false); + + const unsigned int c = duTransCol(col, 64); + const unsigned int ip = (unsigned int)(poly - tile->polys); + + if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) + { + dtOffMeshConnection* con = &tile->offMeshCons[ip - tile->header->offMeshBase]; + + dd->begin(DU_DRAW_LINES, 2.0f); + + // Connection arc. + duAppendArc(dd, con->pos[0],con->pos[1],con->pos[2], con->pos[3],con->pos[4],con->pos[5], 0.25f, + (con->flags & 1) ? 0.6f : 0.0f, 0.6f, c); + + dd->end(); + } + else + { + const dtPolyDetail* pd = &tile->detailMeshes[ip]; + + dd->begin(DU_DRAW_TRIS); + for (int i = 0; i < pd->triCount; ++i) + { + const unsigned char* t = &tile->detailTris[(pd->triBase+i)*4]; + for (int j = 0; j < 3; ++j) + { + if (t[j] < poly->vertCount) + dd->vertex(&tile->verts[poly->verts[t[j]]*3], c); + else + dd->vertex(&tile->detailVerts[(pd->vertBase+t[j]-poly->vertCount)*3], c); + } + } + dd->end(); + } + + dd->depthMask(true); + +} + +static void debugDrawTileCachePortals(struct duDebugDraw* dd, const dtTileCacheLayer& layer, const float cs, const float ch) +{ + const int w = (int)layer.header->width; + const int h = (int)layer.header->height; + const float* bmin = layer.header->bmin; + + // Portals + unsigned int pcol = duRGBA(255,255,255,255); + + const int segs[4*4] = {0,0,0,1, 0,1,1,1, 1,1,1,0, 1,0,0,0}; + + // Layer portals + dd->begin(DU_DRAW_LINES, 2.0f); + for (int y = 0; y < h; ++y) + { + for (int x = 0; x < w; ++x) + { + const int idx = x+y*w; + const int lh = (int)layer.heights[idx]; + if (lh == 0xff) continue; + + for (int dir = 0; dir < 4; ++dir) + { + if (layer.cons[idx] & (1<<(dir+4))) + { + const int* seg = &segs[dir*4]; + const float ax = bmin[0] + (x+seg[0])*cs; + const float ay = bmin[1] + (lh+2)*ch; + const float az = bmin[2] + (y+seg[1])*cs; + const float bx = bmin[0] + (x+seg[2])*cs; + const float by = bmin[1] + (lh+2)*ch; + const float bz = bmin[2] + (y+seg[3])*cs; + dd->vertex(ax, ay, az, pcol); + dd->vertex(bx, by, bz, pcol); + } + } + } + } + dd->end(); +} + +void duDebugDrawTileCacheLayerAreas(struct duDebugDraw* dd, const dtTileCacheLayer& layer, const float cs, const float ch) +{ + const int w = (int)layer.header->width; + const int h = (int)layer.header->height; + const float* bmin = layer.header->bmin; + const float* bmax = layer.header->bmax; + const int idx = layer.header->tlayer; + + unsigned int color = duIntToCol(idx+1, 255); + + // Layer bounds + float lbmin[3], lbmax[3]; + lbmin[0] = bmin[0] + layer.header->minx*cs; + lbmin[1] = bmin[1]; + lbmin[2] = bmin[2] + layer.header->miny*cs; + lbmax[0] = bmin[0] + (layer.header->maxx+1)*cs; + lbmax[1] = bmax[1]; + lbmax[2] = bmin[2] + (layer.header->maxy+1)*cs; + duDebugDrawBoxWire(dd, lbmin[0],lbmin[1],lbmin[2], lbmax[0],lbmax[1],lbmax[2], duTransCol(color,128), 2.0f); + + // Layer height + dd->begin(DU_DRAW_QUADS); + for (int y = 0; y < h; ++y) + { + for (int x = 0; x < w; ++x) + { + const int lidx = x+y*w; + const int lh = (int)layer.heights[lidx]; + if (lh == 0xff) continue; + + const unsigned char area = layer.areas[lidx]; + unsigned int col; + if (area == 63) + col = duLerpCol(color, duRGBA(0,192,255,64), 32); + else if (area == 0) + col = duLerpCol(color, duRGBA(0,0,0,64), 32); + else + col = duLerpCol(color, dd->areaToCol(area), 32); + + const float fx = bmin[0] + x*cs; + const float fy = bmin[1] + (lh+1)*ch; + const float fz = bmin[2] + y*cs; + + dd->vertex(fx, fy, fz, col); + dd->vertex(fx, fy, fz+cs, col); + dd->vertex(fx+cs, fy, fz+cs, col); + dd->vertex(fx+cs, fy, fz, col); + } + } + dd->end(); + + debugDrawTileCachePortals(dd, layer, cs, ch); +} + +void duDebugDrawTileCacheLayerRegions(struct duDebugDraw* dd, const dtTileCacheLayer& layer, const float cs, const float ch) +{ + const int w = (int)layer.header->width; + const int h = (int)layer.header->height; + const float* bmin = layer.header->bmin; + const float* bmax = layer.header->bmax; + const int idx = layer.header->tlayer; + + unsigned int color = duIntToCol(idx+1, 255); + + // Layer bounds + float lbmin[3], lbmax[3]; + lbmin[0] = bmin[0] + layer.header->minx*cs; + lbmin[1] = bmin[1]; + lbmin[2] = bmin[2] + layer.header->miny*cs; + lbmax[0] = bmin[0] + (layer.header->maxx+1)*cs; + lbmax[1] = bmax[1]; + lbmax[2] = bmin[2] + (layer.header->maxy+1)*cs; + duDebugDrawBoxWire(dd, lbmin[0],lbmin[1],lbmin[2], lbmax[0],lbmax[1],lbmax[2], duTransCol(color,128), 2.0f); + + // Layer height + dd->begin(DU_DRAW_QUADS); + for (int y = 0; y < h; ++y) + { + for (int x = 0; x < w; ++x) + { + const int lidx = x+y*w; + const int lh = (int)layer.heights[lidx]; + if (lh == 0xff) continue; + const unsigned char reg = layer.regs[lidx]; + + unsigned int col = duLerpCol(color, duIntToCol(reg, 255), 192); + + const float fx = bmin[0] + x*cs; + const float fy = bmin[1] + (lh+1)*ch; + const float fz = bmin[2] + y*cs; + + dd->vertex(fx, fy, fz, col); + dd->vertex(fx, fy, fz+cs, col); + dd->vertex(fx+cs, fy, fz+cs, col); + dd->vertex(fx+cs, fy, fz, col); + } + } + dd->end(); + + debugDrawTileCachePortals(dd, layer, cs, ch); +} + + + + +/*struct dtTileCacheContour +{ + int nverts; + unsigned char* verts; + unsigned char reg; + unsigned char area; +}; + +struct dtTileCacheContourSet +{ + int nconts; + dtTileCacheContour* conts; +};*/ + +void duDebugDrawTileCacheContours(duDebugDraw* dd, const struct dtTileCacheContourSet& lcset, + const float* orig, const float cs, const float ch) +{ + if (!dd) return; + + const unsigned char a = 255;// (unsigned char)(alpha*255.0f); + + const int offs[2*4] = {-1,0, 0,1, 1,0, 0,-1}; + + dd->begin(DU_DRAW_LINES, 2.0f); + + for (int i = 0; i < lcset.nconts; ++i) + { + const dtTileCacheContour& c = lcset.conts[i]; + unsigned int color = 0; + + color = duIntToCol(i, a); + + for (int j = 0; j < c.nverts; ++j) + { + const int k = (j+1) % c.nverts; + const unsigned char* va = &c.verts[j*4]; + const unsigned char* vb = &c.verts[k*4]; + const float ax = orig[0] + va[0]*cs; + const float ay = orig[1] + (va[1]+1+(i&1))*ch; + const float az = orig[2] + va[2]*cs; + const float bx = orig[0] + vb[0]*cs; + const float by = orig[1] + (vb[1]+1+(i&1))*ch; + const float bz = orig[2] + vb[2]*cs; + unsigned int col = color; + if ((va[3] & 0xf) != 0xf) + { + // Portal segment + col = duRGBA(255,255,255,128); + int d = va[3] & 0xf; + + const float cx = (ax+bx)*0.5f; + const float cy = (ay+by)*0.5f; + const float cz = (az+bz)*0.5f; + + const float dx = cx + offs[d*2+0]*2*cs; + const float dy = cy; + const float dz = cz + offs[d*2+1]*2*cs; + + dd->vertex(cx,cy,cz,duRGBA(255,0,0,255)); + dd->vertex(dx,dy,dz,duRGBA(255,0,0,255)); + } + + duAppendArrow(dd, ax,ay,az, bx,by,bz, 0.0f, cs*0.5f, col); + } + } + dd->end(); + + dd->begin(DU_DRAW_POINTS, 4.0f); + + for (int i = 0; i < lcset.nconts; ++i) + { + const dtTileCacheContour& c = lcset.conts[i]; + unsigned int color = 0; + + for (int j = 0; j < c.nverts; ++j) + { + const unsigned char* va = &c.verts[j*4]; + + color = duDarkenCol(duIntToCol(i, a)); + if (va[3] & 0x80) + { + // Border vertex + color = duRGBA(255,0,0,255); + } + + float fx = orig[0] + va[0]*cs; + float fy = orig[1] + (va[1]+1+(i&1))*ch; + float fz = orig[2] + va[2]*cs; + dd->vertex(fx,fy,fz, color); + } + } + dd->end(); +} + +void duDebugDrawTileCachePolyMesh(duDebugDraw* dd, const struct dtTileCachePolyMesh& lmesh, + const float* orig, const float cs, const float ch) +{ + if (!dd) return; + + const int nvp = lmesh.nvp; + + const int offs[2*4] = {-1,0, 0,1, 1,0, 0,-1}; + + dd->begin(DU_DRAW_TRIS); + + for (int i = 0; i < lmesh.npolys; ++i) + { + const unsigned short* p = &lmesh.polys[i*nvp*2]; + const unsigned char area = lmesh.areas[i]; + + unsigned int color; + if (area == DT_TILECACHE_WALKABLE_AREA) + color = duRGBA(0,192,255,64); + else if (area == DT_TILECACHE_NULL_AREA) + color = duRGBA(0,0,0,64); + else + color = dd->areaToCol(area); + + unsigned short vi[3]; + for (int j = 2; j < nvp; ++j) + { + if (p[j] == DT_TILECACHE_NULL_IDX) break; + vi[0] = p[0]; + vi[1] = p[j-1]; + vi[2] = p[j]; + for (int k = 0; k < 3; ++k) + { + const unsigned short* v = &lmesh.verts[vi[k]*3]; + const float x = orig[0] + v[0]*cs; + const float y = orig[1] + (v[1]+1)*ch; + const float z = orig[2] + v[2]*cs; + dd->vertex(x,y,z, color); + } + } + } + dd->end(); + + // Draw neighbours edges + const unsigned int coln = duRGBA(0,48,64,32); + dd->begin(DU_DRAW_LINES, 1.5f); + for (int i = 0; i < lmesh.npolys; ++i) + { + const unsigned short* p = &lmesh.polys[i*nvp*2]; + for (int j = 0; j < nvp; ++j) + { + if (p[j] == DT_TILECACHE_NULL_IDX) break; + if (p[nvp+j] & 0x8000) continue; + const int nj = (j+1 >= nvp || p[j+1] == DT_TILECACHE_NULL_IDX) ? 0 : j+1; + int vi[2] = {p[j], p[nj]}; + + for (int k = 0; k < 2; ++k) + { + const unsigned short* v = &lmesh.verts[vi[k]*3]; + const float x = orig[0] + v[0]*cs; + const float y = orig[1] + (v[1]+1)*ch + 0.1f; + const float z = orig[2] + v[2]*cs; + dd->vertex(x, y, z, coln); + } + } + } + dd->end(); + + // Draw boundary edges + const unsigned int colb = duRGBA(0,48,64,220); + dd->begin(DU_DRAW_LINES, 2.5f); + for (int i = 0; i < lmesh.npolys; ++i) + { + const unsigned short* p = &lmesh.polys[i*nvp*2]; + for (int j = 0; j < nvp; ++j) + { + if (p[j] == DT_TILECACHE_NULL_IDX) break; + if ((p[nvp+j] & 0x8000) == 0) continue; + const int nj = (j+1 >= nvp || p[j+1] == DT_TILECACHE_NULL_IDX) ? 0 : j+1; + int vi[2] = {p[j], p[nj]}; + + unsigned int col = colb; + if ((p[nvp+j] & 0xf) != 0xf) + { + const unsigned short* va = &lmesh.verts[vi[0]*3]; + const unsigned short* vb = &lmesh.verts[vi[1]*3]; + + const float ax = orig[0] + va[0]*cs; + const float ay = orig[1] + (va[1]+1+(i&1))*ch; + const float az = orig[2] + va[2]*cs; + const float bx = orig[0] + vb[0]*cs; + const float by = orig[1] + (vb[1]+1+(i&1))*ch; + const float bz = orig[2] + vb[2]*cs; + + const float cx = (ax+bx)*0.5f; + const float cy = (ay+by)*0.5f; + const float cz = (az+bz)*0.5f; + + int d = p[nvp+j] & 0xf; + + const float dx = cx + offs[d*2+0]*2*cs; + const float dy = cy; + const float dz = cz + offs[d*2+1]*2*cs; + + dd->vertex(cx,cy,cz,duRGBA(255,0,0,255)); + dd->vertex(dx,dy,dz,duRGBA(255,0,0,255)); + + col = duRGBA(255,255,255,128); + } + + for (int k = 0; k < 2; ++k) + { + const unsigned short* v = &lmesh.verts[vi[k]*3]; + const float x = orig[0] + v[0]*cs; + const float y = orig[1] + (v[1]+1)*ch + 0.1f; + const float z = orig[2] + v[2]*cs; + dd->vertex(x, y, z, col); + } + } + } + dd->end(); + + dd->begin(DU_DRAW_POINTS, 3.0f); + const unsigned int colv = duRGBA(0,0,0,220); + for (int i = 0; i < lmesh.nverts; ++i) + { + const unsigned short* v = &lmesh.verts[i*3]; + const float x = orig[0] + v[0]*cs; + const float y = orig[1] + (v[1]+1)*ch + 0.1f; + const float z = orig[2] + v[2]*cs; + dd->vertex(x,y,z, colv); + } + dd->end(); +} + + + diff --git a/Dependencies/recastnavigation/DebugUtils/Source/RecastDebugDraw.cpp b/Dependencies/recastnavigation/DebugUtils/Source/RecastDebugDraw.cpp new file mode 100644 index 000000000..466c5b17b --- /dev/null +++ b/Dependencies/recastnavigation/DebugUtils/Source/RecastDebugDraw.cpp @@ -0,0 +1,1063 @@ +// +// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#include +#include "DebugDraw.h" +#include "RecastDebugDraw.h" +#include "Recast.h" + +void duDebugDrawTriMesh(duDebugDraw* dd, const float* verts, int /*nverts*/, + const int* tris, const float* normals, int ntris, + const unsigned char* flags, const float texScale) +{ + if (!dd) return; + if (!verts) return; + if (!tris) return; + if (!normals) return; + + float uva[2]; + float uvb[2]; + float uvc[2]; + + const unsigned int unwalkable = duRGBA(192,128,0,255); + + dd->texture(true); + + dd->begin(DU_DRAW_TRIS); + for (int i = 0; i < ntris*3; i += 3) + { + const float* norm = &normals[i]; + unsigned int color; + unsigned char a = (unsigned char)(220*(2+norm[0]+norm[1])/4); + if (flags && !flags[i/3]) + color = duLerpCol(duRGBA(a,a,a,255), unwalkable, 64); + else + color = duRGBA(a,a,a,255); + + const float* va = &verts[tris[i+0]*3]; + const float* vb = &verts[tris[i+1]*3]; + const float* vc = &verts[tris[i+2]*3]; + + int ax = 0, ay = 0; + if (rcAbs(norm[1]) > rcAbs(norm[ax])) + ax = 1; + if (rcAbs(norm[2]) > rcAbs(norm[ax])) + ax = 2; + ax = (1<vertex(va, color, uva); + dd->vertex(vb, color, uvb); + dd->vertex(vc, color, uvc); + } + dd->end(); + dd->texture(false); +} + +void duDebugDrawTriMeshSlope(duDebugDraw* dd, const float* verts, int /*nverts*/, + const int* tris, const float* normals, int ntris, + const float walkableSlopeAngle, const float texScale) +{ + if (!dd) return; + if (!verts) return; + if (!tris) return; + if (!normals) return; + + const float walkableThr = cosf(walkableSlopeAngle/180.0f*DU_PI); + + float uva[2]; + float uvb[2]; + float uvc[2]; + + dd->texture(true); + + const unsigned int unwalkable = duRGBA(192,128,0,255); + + dd->begin(DU_DRAW_TRIS); + for (int i = 0; i < ntris*3; i += 3) + { + const float* norm = &normals[i]; + unsigned int color; + unsigned char a = (unsigned char)(220*(2+norm[0]+norm[1])/4); + if (norm[1] < walkableThr) + color = duLerpCol(duRGBA(a,a,a,255), unwalkable, 64); + else + color = duRGBA(a,a,a,255); + + const float* va = &verts[tris[i+0]*3]; + const float* vb = &verts[tris[i+1]*3]; + const float* vc = &verts[tris[i+2]*3]; + + int ax = 0, ay = 0; + if (rcAbs(norm[1]) > rcAbs(norm[ax])) + ax = 1; + if (rcAbs(norm[2]) > rcAbs(norm[ax])) + ax = 2; + ax = (1<vertex(va, color, uva); + dd->vertex(vb, color, uvb); + dd->vertex(vc, color, uvc); + } + dd->end(); + + dd->texture(false); +} + +void duDebugDrawHeightfieldSolid(duDebugDraw* dd, const rcHeightfield& hf) +{ + if (!dd) return; + + const float* orig = hf.bmin; + const float cs = hf.cs; + const float ch = hf.ch; + + const int w = hf.width; + const int h = hf.height; + + unsigned int fcol[6]; + duCalcBoxColors(fcol, duRGBA(255,255,255,255), duRGBA(255,255,255,255)); + + dd->begin(DU_DRAW_QUADS); + + for (int y = 0; y < h; ++y) + { + for (int x = 0; x < w; ++x) + { + float fx = orig[0] + x*cs; + float fz = orig[2] + y*cs; + const rcSpan* s = hf.spans[x + y*w]; + while (s) + { + duAppendBox(dd, fx, orig[1]+s->smin*ch, fz, fx+cs, orig[1] + s->smax*ch, fz+cs, fcol); + s = s->next; + } + } + } + dd->end(); +} + +void duDebugDrawHeightfieldWalkable(duDebugDraw* dd, const rcHeightfield& hf) +{ + if (!dd) return; + + const float* orig = hf.bmin; + const float cs = hf.cs; + const float ch = hf.ch; + + const int w = hf.width; + const int h = hf.height; + + unsigned int fcol[6]; + duCalcBoxColors(fcol, duRGBA(255,255,255,255), duRGBA(217,217,217,255)); + + dd->begin(DU_DRAW_QUADS); + + for (int y = 0; y < h; ++y) + { + for (int x = 0; x < w; ++x) + { + float fx = orig[0] + x*cs; + float fz = orig[2] + y*cs; + const rcSpan* s = hf.spans[x + y*w]; + while (s) + { + if (s->area == RC_WALKABLE_AREA) + fcol[0] = duRGBA(64,128,160,255); + else if (s->area == RC_NULL_AREA) + fcol[0] = duRGBA(64,64,64,255); + else + fcol[0] = duMultCol(dd->areaToCol(s->area), 200); + + duAppendBox(dd, fx, orig[1]+s->smin*ch, fz, fx+cs, orig[1] + s->smax*ch, fz+cs, fcol); + s = s->next; + } + } + } + + dd->end(); +} + +void duDebugDrawCompactHeightfieldSolid(duDebugDraw* dd, const rcCompactHeightfield& chf) +{ + if (!dd) return; + + const float cs = chf.cs; + const float ch = chf.ch; + + dd->begin(DU_DRAW_QUADS); + + for (int y = 0; y < chf.height; ++y) + { + for (int x = 0; x < chf.width; ++x) + { + const float fx = chf.bmin[0] + x*cs; + const float fz = chf.bmin[2] + y*cs; + const rcCompactCell& c = chf.cells[x+y*chf.width]; + + for (unsigned i = c.index, ni = c.index+c.count; i < ni; ++i) + { + const rcCompactSpan& s = chf.spans[i]; + + const unsigned char area = chf.areas[i]; + unsigned int color; + if (area == RC_WALKABLE_AREA) + color = duRGBA(0,192,255,64); + else if (area == RC_NULL_AREA) + color = duRGBA(0,0,0,64); + else + color = dd->areaToCol(area); + + const float fy = chf.bmin[1] + (s.y+1)*ch; + dd->vertex(fx, fy, fz, color); + dd->vertex(fx, fy, fz+cs, color); + dd->vertex(fx+cs, fy, fz+cs, color); + dd->vertex(fx+cs, fy, fz, color); + } + } + } + dd->end(); +} + +void duDebugDrawCompactHeightfieldRegions(duDebugDraw* dd, const rcCompactHeightfield& chf) +{ + if (!dd) return; + + const float cs = chf.cs; + const float ch = chf.ch; + + dd->begin(DU_DRAW_QUADS); + + for (int y = 0; y < chf.height; ++y) + { + for (int x = 0; x < chf.width; ++x) + { + const float fx = chf.bmin[0] + x*cs; + const float fz = chf.bmin[2] + y*cs; + const rcCompactCell& c = chf.cells[x+y*chf.width]; + + for (unsigned i = c.index, ni = c.index+c.count; i < ni; ++i) + { + const rcCompactSpan& s = chf.spans[i]; + const float fy = chf.bmin[1] + (s.y)*ch; + unsigned int color; + if (s.reg) + color = duIntToCol(s.reg, 192); + else + color = duRGBA(0,0,0,64); + + dd->vertex(fx, fy, fz, color); + dd->vertex(fx, fy, fz+cs, color); + dd->vertex(fx+cs, fy, fz+cs, color); + dd->vertex(fx+cs, fy, fz, color); + } + } + } + + dd->end(); +} + + +void duDebugDrawCompactHeightfieldDistance(duDebugDraw* dd, const rcCompactHeightfield& chf) +{ + if (!dd) return; + if (!chf.dist) return; + + const float cs = chf.cs; + const float ch = chf.ch; + + float maxd = chf.maxDistance; + if (maxd < 1.0f) maxd = 1; + const float dscale = 255.0f / maxd; + + dd->begin(DU_DRAW_QUADS); + + for (int y = 0; y < chf.height; ++y) + { + for (int x = 0; x < chf.width; ++x) + { + const float fx = chf.bmin[0] + x*cs; + const float fz = chf.bmin[2] + y*cs; + const rcCompactCell& c = chf.cells[x+y*chf.width]; + + for (unsigned i = c.index, ni = c.index+c.count; i < ni; ++i) + { + const rcCompactSpan& s = chf.spans[i]; + const float fy = chf.bmin[1] + (s.y+1)*ch; + const unsigned char cd = (unsigned char)(chf.dist[i] * dscale); + const unsigned int color = duRGBA(cd,cd,cd,255); + dd->vertex(fx, fy, fz, color); + dd->vertex(fx, fy, fz+cs, color); + dd->vertex(fx+cs, fy, fz+cs, color); + dd->vertex(fx+cs, fy, fz, color); + } + } + } + dd->end(); +} + +static void drawLayerPortals(duDebugDraw* dd, const rcHeightfieldLayer* layer) +{ + const float cs = layer->cs; + const float ch = layer->ch; + const int w = layer->width; + const int h = layer->height; + + unsigned int pcol = duRGBA(255,255,255,255); + + const int segs[4*4] = {0,0,0,1, 0,1,1,1, 1,1,1,0, 1,0,0,0}; + + // Layer portals + dd->begin(DU_DRAW_LINES, 2.0f); + for (int y = 0; y < h; ++y) + { + for (int x = 0; x < w; ++x) + { + const int idx = x+y*w; + const int lh = (int)layer->heights[idx]; + if (lh == 255) continue; + + for (int dir = 0; dir < 4; ++dir) + { + if (layer->cons[idx] & (1<<(dir+4))) + { + const int* seg = &segs[dir*4]; + const float ax = layer->bmin[0] + (x+seg[0])*cs; + const float ay = layer->bmin[1] + (lh+2)*ch; + const float az = layer->bmin[2] + (y+seg[1])*cs; + const float bx = layer->bmin[0] + (x+seg[2])*cs; + const float by = layer->bmin[1] + (lh+2)*ch; + const float bz = layer->bmin[2] + (y+seg[3])*cs; + dd->vertex(ax, ay, az, pcol); + dd->vertex(bx, by, bz, pcol); + } + } + } + } + dd->end(); +} + +void duDebugDrawHeightfieldLayer(duDebugDraw* dd, const struct rcHeightfieldLayer& layer, const int idx) +{ + const float cs = layer.cs; + const float ch = layer.ch; + const int w = layer.width; + const int h = layer.height; + + unsigned int color = duIntToCol(idx+1, 255); + + // Layer bounds + float bmin[3], bmax[3]; + bmin[0] = layer.bmin[0] + layer.minx*cs; + bmin[1] = layer.bmin[1]; + bmin[2] = layer.bmin[2] + layer.miny*cs; + bmax[0] = layer.bmin[0] + (layer.maxx+1)*cs; + bmax[1] = layer.bmax[1]; + bmax[2] = layer.bmin[2] + (layer.maxy+1)*cs; + duDebugDrawBoxWire(dd, bmin[0],bmin[1],bmin[2], bmax[0],bmax[1],bmax[2], duTransCol(color,128), 2.0f); + + // Layer height + dd->begin(DU_DRAW_QUADS); + for (int y = 0; y < h; ++y) + { + for (int x = 0; x < w; ++x) + { + const int lidx = x+y*w; + const int lh = (int)layer.heights[lidx]; + if (h == 0xff) continue; + const unsigned char area = layer.areas[lidx]; + + unsigned int col; + if (area == RC_WALKABLE_AREA) + col = duLerpCol(color, duRGBA(0,192,255,64), 32); + else if (area == RC_NULL_AREA) + col = duLerpCol(color, duRGBA(0,0,0,64), 32); + else + col = duLerpCol(color, dd->areaToCol(area), 32); + + const float fx = layer.bmin[0] + x*cs; + const float fy = layer.bmin[1] + (lh+1)*ch; + const float fz = layer.bmin[2] + y*cs; + + dd->vertex(fx, fy, fz, col); + dd->vertex(fx, fy, fz+cs, col); + dd->vertex(fx+cs, fy, fz+cs, col); + dd->vertex(fx+cs, fy, fz, col); + } + } + dd->end(); + + // Portals + drawLayerPortals(dd, &layer); +} + +void duDebugDrawHeightfieldLayers(duDebugDraw* dd, const struct rcHeightfieldLayerSet& lset) +{ + if (!dd) return; + for (int i = 0; i < lset.nlayers; ++i) + duDebugDrawHeightfieldLayer(dd, lset.layers[i], i); +} + +/* +void duDebugDrawLayerContours(duDebugDraw* dd, const struct rcLayerContourSet& lcset) +{ + if (!dd) return; + + const float* orig = lcset.bmin; + const float cs = lcset.cs; + const float ch = lcset.ch; + + const unsigned char a = 255;// (unsigned char)(alpha*255.0f); + + const int offs[2*4] = {-1,0, 0,1, 1,0, 0,-1}; + + dd->begin(DU_DRAW_LINES, 2.0f); + + for (int i = 0; i < lcset.nconts; ++i) + { + const rcLayerContour& c = lcset.conts[i]; + unsigned int color = 0; + + color = duIntToCol(i, a); + + for (int j = 0; j < c.nverts; ++j) + { + const int k = (j+1) % c.nverts; + const unsigned char* va = &c.verts[j*4]; + const unsigned char* vb = &c.verts[k*4]; + const float ax = orig[0] + va[0]*cs; + const float ay = orig[1] + (va[1]+1+(i&1))*ch; + const float az = orig[2] + va[2]*cs; + const float bx = orig[0] + vb[0]*cs; + const float by = orig[1] + (vb[1]+1+(i&1))*ch; + const float bz = orig[2] + vb[2]*cs; + unsigned int col = color; + if ((va[3] & 0xf) != 0xf) + { + col = duRGBA(255,255,255,128); + int d = va[3] & 0xf; + + const float cx = (ax+bx)*0.5f; + const float cy = (ay+by)*0.5f; + const float cz = (az+bz)*0.5f; + + const float dx = cx + offs[d*2+0]*2*cs; + const float dy = cy; + const float dz = cz + offs[d*2+1]*2*cs; + + dd->vertex(cx,cy,cz,duRGBA(255,0,0,255)); + dd->vertex(dx,dy,dz,duRGBA(255,0,0,255)); + } + + duAppendArrow(dd, ax,ay,az, bx,by,bz, 0.0f, cs*0.5f, col); + } + } + dd->end(); + + dd->begin(DU_DRAW_POINTS, 4.0f); + + for (int i = 0; i < lcset.nconts; ++i) + { + const rcLayerContour& c = lcset.conts[i]; + unsigned int color = 0; + + for (int j = 0; j < c.nverts; ++j) + { + const unsigned char* va = &c.verts[j*4]; + + color = duDarkenCol(duIntToCol(i, a)); + if (va[3] & 0x80) + color = duRGBA(255,0,0,255); + + float fx = orig[0] + va[0]*cs; + float fy = orig[1] + (va[1]+1+(i&1))*ch; + float fz = orig[2] + va[2]*cs; + dd->vertex(fx,fy,fz, color); + } + } + dd->end(); +} + +void duDebugDrawLayerPolyMesh(duDebugDraw* dd, const struct rcLayerPolyMesh& lmesh) +{ + if (!dd) return; + + const int nvp = lmesh.nvp; + const float cs = lmesh.cs; + const float ch = lmesh.ch; + const float* orig = lmesh.bmin; + + const int offs[2*4] = {-1,0, 0,1, 1,0, 0,-1}; + + dd->begin(DU_DRAW_TRIS); + + for (int i = 0; i < lmesh.npolys; ++i) + { + const unsigned short* p = &lmesh.polys[i*nvp*2]; + + unsigned int color; + if (lmesh.areas[i] == RC_WALKABLE_AREA) + color = duRGBA(0,192,255,64); + else if (lmesh.areas[i] == RC_NULL_AREA) + color = duRGBA(0,0,0,64); + else + color = duIntToCol(lmesh.areas[i], 255); + + unsigned short vi[3]; + for (int j = 2; j < nvp; ++j) + { + if (p[j] == RC_MESH_NULL_IDX) break; + vi[0] = p[0]; + vi[1] = p[j-1]; + vi[2] = p[j]; + for (int k = 0; k < 3; ++k) + { + const unsigned short* v = &lmesh.verts[vi[k]*3]; + const float x = orig[0] + v[0]*cs; + const float y = orig[1] + (v[1]+1)*ch; + const float z = orig[2] + v[2]*cs; + dd->vertex(x,y,z, color); + } + } + } + dd->end(); + + // Draw neighbours edges + const unsigned int coln = duRGBA(0,48,64,32); + dd->begin(DU_DRAW_LINES, 1.5f); + for (int i = 0; i < lmesh.npolys; ++i) + { + const unsigned short* p = &lmesh.polys[i*nvp*2]; + for (int j = 0; j < nvp; ++j) + { + if (p[j] == RC_MESH_NULL_IDX) break; + if (p[nvp+j] & 0x8000) continue; + const int nj = (j+1 >= nvp || p[j+1] == RC_MESH_NULL_IDX) ? 0 : j+1; + int vi[2] = {p[j], p[nj]}; + + for (int k = 0; k < 2; ++k) + { + const unsigned short* v = &lmesh.verts[vi[k]*3]; + const float x = orig[0] + v[0]*cs; + const float y = orig[1] + (v[1]+1)*ch + 0.1f; + const float z = orig[2] + v[2]*cs; + dd->vertex(x, y, z, coln); + } + } + } + dd->end(); + + // Draw boundary edges + const unsigned int colb = duRGBA(0,48,64,220); + dd->begin(DU_DRAW_LINES, 2.5f); + for (int i = 0; i < lmesh.npolys; ++i) + { + const unsigned short* p = &lmesh.polys[i*nvp*2]; + for (int j = 0; j < nvp; ++j) + { + if (p[j] == RC_MESH_NULL_IDX) break; + if ((p[nvp+j] & 0x8000) == 0) continue; + const int nj = (j+1 >= nvp || p[j+1] == RC_MESH_NULL_IDX) ? 0 : j+1; + int vi[2] = {p[j], p[nj]}; + + unsigned int col = colb; + if ((p[nvp+j] & 0xf) != 0xf) + { + const unsigned short* va = &lmesh.verts[vi[0]*3]; + const unsigned short* vb = &lmesh.verts[vi[1]*3]; + + const float ax = orig[0] + va[0]*cs; + const float ay = orig[1] + (va[1]+1+(i&1))*ch; + const float az = orig[2] + va[2]*cs; + const float bx = orig[0] + vb[0]*cs; + const float by = orig[1] + (vb[1]+1+(i&1))*ch; + const float bz = orig[2] + vb[2]*cs; + + const float cx = (ax+bx)*0.5f; + const float cy = (ay+by)*0.5f; + const float cz = (az+bz)*0.5f; + + int d = p[nvp+j] & 0xf; + + const float dx = cx + offs[d*2+0]*2*cs; + const float dy = cy; + const float dz = cz + offs[d*2+1]*2*cs; + + dd->vertex(cx,cy,cz,duRGBA(255,0,0,255)); + dd->vertex(dx,dy,dz,duRGBA(255,0,0,255)); + + col = duRGBA(255,255,255,128); + } + + for (int k = 0; k < 2; ++k) + { + const unsigned short* v = &lmesh.verts[vi[k]*3]; + const float x = orig[0] + v[0]*cs; + const float y = orig[1] + (v[1]+1)*ch + 0.1f; + const float z = orig[2] + v[2]*cs; + dd->vertex(x, y, z, col); + } + } + } + dd->end(); + + dd->begin(DU_DRAW_POINTS, 3.0f); + const unsigned int colv = duRGBA(0,0,0,220); + for (int i = 0; i < lmesh.nverts; ++i) + { + const unsigned short* v = &lmesh.verts[i*3]; + const float x = orig[0] + v[0]*cs; + const float y = orig[1] + (v[1]+1)*ch + 0.1f; + const float z = orig[2] + v[2]*cs; + dd->vertex(x,y,z, colv); + } + dd->end(); +} +*/ + +static void getContourCenter(const rcContour* cont, const float* orig, float cs, float ch, float* center) +{ + center[0] = 0; + center[1] = 0; + center[2] = 0; + if (!cont->nverts) + return; + for (int i = 0; i < cont->nverts; ++i) + { + const int* v = &cont->verts[i*4]; + center[0] += (float)v[0]; + center[1] += (float)v[1]; + center[2] += (float)v[2]; + } + const float s = 1.0f / cont->nverts; + center[0] *= s * cs; + center[1] *= s * ch; + center[2] *= s * cs; + center[0] += orig[0]; + center[1] += orig[1] + 4*ch; + center[2] += orig[2]; +} + +static const rcContour* findContourFromSet(const rcContourSet& cset, unsigned short reg) +{ + for (int i = 0; i < cset.nconts; ++i) + { + if (cset.conts[i].reg == reg) + return &cset.conts[i]; + } + return 0; +} + +void duDebugDrawRegionConnections(duDebugDraw* dd, const rcContourSet& cset, const float alpha) +{ + if (!dd) return; + + const float* orig = cset.bmin; + const float cs = cset.cs; + const float ch = cset.ch; + + // Draw centers + float pos[3], pos2[3]; + + unsigned int color = duRGBA(0,0,0,196); + + dd->begin(DU_DRAW_LINES, 2.0f); + + for (int i = 0; i < cset.nconts; ++i) + { + const rcContour* cont = &cset.conts[i]; + getContourCenter(cont, orig, cs, ch, pos); + for (int j = 0; j < cont->nverts; ++j) + { + const int* v = &cont->verts[j*4]; + if (v[3] == 0 || (unsigned short)v[3] < cont->reg) continue; + const rcContour* cont2 = findContourFromSet(cset, (unsigned short)v[3]); + if (cont2) + { + getContourCenter(cont2, orig, cs, ch, pos2); + duAppendArc(dd, pos[0],pos[1],pos[2], pos2[0],pos2[1],pos2[2], 0.25f, 0.6f, 0.6f, color); + } + } + } + + dd->end(); + + unsigned char a = (unsigned char)(alpha * 255.0f); + + dd->begin(DU_DRAW_POINTS, 7.0f); + + for (int i = 0; i < cset.nconts; ++i) + { + const rcContour* cont = &cset.conts[i]; + unsigned int col = duDarkenCol(duIntToCol(cont->reg,a)); + getContourCenter(cont, orig, cs, ch, pos); + dd->vertex(pos, col); + } + dd->end(); +} + +void duDebugDrawRawContours(duDebugDraw* dd, const rcContourSet& cset, const float alpha) +{ + if (!dd) return; + + const float* orig = cset.bmin; + const float cs = cset.cs; + const float ch = cset.ch; + + const unsigned char a = (unsigned char)(alpha*255.0f); + + dd->begin(DU_DRAW_LINES, 2.0f); + + for (int i = 0; i < cset.nconts; ++i) + { + const rcContour& c = cset.conts[i]; + unsigned int color = duIntToCol(c.reg, a); + + for (int j = 0; j < c.nrverts; ++j) + { + const int* v = &c.rverts[j*4]; + float fx = orig[0] + v[0]*cs; + float fy = orig[1] + (v[1]+1+(i&1))*ch; + float fz = orig[2] + v[2]*cs; + dd->vertex(fx,fy,fz,color); + if (j > 0) + dd->vertex(fx,fy,fz,color); + } + // Loop last segment. + const int* v = &c.rverts[0]; + float fx = orig[0] + v[0]*cs; + float fy = orig[1] + (v[1]+1+(i&1))*ch; + float fz = orig[2] + v[2]*cs; + dd->vertex(fx,fy,fz,color); + } + dd->end(); + + dd->begin(DU_DRAW_POINTS, 2.0f); + + for (int i = 0; i < cset.nconts; ++i) + { + const rcContour& c = cset.conts[i]; + unsigned int color = duDarkenCol(duIntToCol(c.reg, a)); + + for (int j = 0; j < c.nrverts; ++j) + { + const int* v = &c.rverts[j*4]; + float off = 0; + unsigned int colv = color; + if (v[3] & RC_BORDER_VERTEX) + { + colv = duRGBA(255,255,255,a); + off = ch*2; + } + + float fx = orig[0] + v[0]*cs; + float fy = orig[1] + (v[1]+1+(i&1))*ch + off; + float fz = orig[2] + v[2]*cs; + dd->vertex(fx,fy,fz, colv); + } + } + dd->end(); +} + +void duDebugDrawContours(duDebugDraw* dd, const rcContourSet& cset, const float alpha) +{ + if (!dd) return; + + const float* orig = cset.bmin; + const float cs = cset.cs; + const float ch = cset.ch; + + const unsigned char a = (unsigned char)(alpha*255.0f); + + dd->begin(DU_DRAW_LINES, 2.5f); + + for (int i = 0; i < cset.nconts; ++i) + { + const rcContour& c = cset.conts[i]; + if (!c.nverts) + continue; + const unsigned int color = duIntToCol(c.reg, a); + const unsigned int bcolor = duLerpCol(color,duRGBA(255,255,255,a),128); + for (int j = 0, k = c.nverts-1; j < c.nverts; k=j++) + { + const int* va = &c.verts[k*4]; + const int* vb = &c.verts[j*4]; + unsigned int col = (va[3] & RC_AREA_BORDER) ? bcolor : color; + float fx,fy,fz; + fx = orig[0] + va[0]*cs; + fy = orig[1] + (va[1]+1+(i&1))*ch; + fz = orig[2] + va[2]*cs; + dd->vertex(fx,fy,fz, col); + fx = orig[0] + vb[0]*cs; + fy = orig[1] + (vb[1]+1+(i&1))*ch; + fz = orig[2] + vb[2]*cs; + dd->vertex(fx,fy,fz, col); + } + } + dd->end(); + + dd->begin(DU_DRAW_POINTS, 3.0f); + + for (int i = 0; i < cset.nconts; ++i) + { + const rcContour& c = cset.conts[i]; + unsigned int color = duDarkenCol(duIntToCol(c.reg, a)); + for (int j = 0; j < c.nverts; ++j) + { + const int* v = &c.verts[j*4]; + float off = 0; + unsigned int colv = color; + if (v[3] & RC_BORDER_VERTEX) + { + colv = duRGBA(255,255,255,a); + off = ch*2; + } + + float fx = orig[0] + v[0]*cs; + float fy = orig[1] + (v[1]+1+(i&1))*ch + off; + float fz = orig[2] + v[2]*cs; + dd->vertex(fx,fy,fz, colv); + } + } + dd->end(); +} + +void duDebugDrawPolyMesh(duDebugDraw* dd, const struct rcPolyMesh& mesh) +{ + if (!dd) return; + + const int nvp = mesh.nvp; + const float cs = mesh.cs; + const float ch = mesh.ch; + const float* orig = mesh.bmin; + + dd->begin(DU_DRAW_TRIS); + + for (int i = 0; i < mesh.npolys; ++i) + { + const unsigned short* p = &mesh.polys[i*nvp*2]; + const unsigned char area = mesh.areas[i]; + + unsigned int color; + if (area == RC_WALKABLE_AREA) + color = duRGBA(0,192,255,64); + else if (area == RC_NULL_AREA) + color = duRGBA(0,0,0,64); + else + color = dd->areaToCol(area); + + unsigned short vi[3]; + for (int j = 2; j < nvp; ++j) + { + if (p[j] == RC_MESH_NULL_IDX) break; + vi[0] = p[0]; + vi[1] = p[j-1]; + vi[2] = p[j]; + for (int k = 0; k < 3; ++k) + { + const unsigned short* v = &mesh.verts[vi[k]*3]; + const float x = orig[0] + v[0]*cs; + const float y = orig[1] + (v[1]+1)*ch; + const float z = orig[2] + v[2]*cs; + dd->vertex(x,y,z, color); + } + } + } + dd->end(); + + // Draw neighbours edges + const unsigned int coln = duRGBA(0,48,64,32); + dd->begin(DU_DRAW_LINES, 1.5f); + for (int i = 0; i < mesh.npolys; ++i) + { + const unsigned short* p = &mesh.polys[i*nvp*2]; + for (int j = 0; j < nvp; ++j) + { + if (p[j] == RC_MESH_NULL_IDX) break; + if (p[nvp+j] & 0x8000) continue; + const int nj = (j+1 >= nvp || p[j+1] == RC_MESH_NULL_IDX) ? 0 : j+1; + const int vi[2] = {p[j], p[nj]}; + + for (int k = 0; k < 2; ++k) + { + const unsigned short* v = &mesh.verts[vi[k]*3]; + const float x = orig[0] + v[0]*cs; + const float y = orig[1] + (v[1]+1)*ch + 0.1f; + const float z = orig[2] + v[2]*cs; + dd->vertex(x, y, z, coln); + } + } + } + dd->end(); + + // Draw boundary edges + const unsigned int colb = duRGBA(0,48,64,220); + dd->begin(DU_DRAW_LINES, 2.5f); + for (int i = 0; i < mesh.npolys; ++i) + { + const unsigned short* p = &mesh.polys[i*nvp*2]; + for (int j = 0; j < nvp; ++j) + { + if (p[j] == RC_MESH_NULL_IDX) break; + if ((p[nvp+j] & 0x8000) == 0) continue; + const int nj = (j+1 >= nvp || p[j+1] == RC_MESH_NULL_IDX) ? 0 : j+1; + const int vi[2] = {p[j], p[nj]}; + + unsigned int col = colb; + if ((p[nvp+j] & 0xf) != 0xf) + col = duRGBA(255,255,255,128); + for (int k = 0; k < 2; ++k) + { + const unsigned short* v = &mesh.verts[vi[k]*3]; + const float x = orig[0] + v[0]*cs; + const float y = orig[1] + (v[1]+1)*ch + 0.1f; + const float z = orig[2] + v[2]*cs; + dd->vertex(x, y, z, col); + } + } + } + dd->end(); + + dd->begin(DU_DRAW_POINTS, 3.0f); + const unsigned int colv = duRGBA(0,0,0,220); + for (int i = 0; i < mesh.nverts; ++i) + { + const unsigned short* v = &mesh.verts[i*3]; + const float x = orig[0] + v[0]*cs; + const float y = orig[1] + (v[1]+1)*ch + 0.1f; + const float z = orig[2] + v[2]*cs; + dd->vertex(x,y,z, colv); + } + dd->end(); +} + +void duDebugDrawPolyMeshDetail(duDebugDraw* dd, const struct rcPolyMeshDetail& dmesh) +{ + if (!dd) return; + + dd->begin(DU_DRAW_TRIS); + + for (int i = 0; i < dmesh.nmeshes; ++i) + { + const unsigned int* m = &dmesh.meshes[i*4]; + const unsigned int bverts = m[0]; + const unsigned int btris = m[2]; + const int ntris = (int)m[3]; + const float* verts = &dmesh.verts[bverts*3]; + const unsigned char* tris = &dmesh.tris[btris*4]; + + unsigned int color = duIntToCol(i, 192); + + for (int j = 0; j < ntris; ++j) + { + dd->vertex(&verts[tris[j*4+0]*3], color); + dd->vertex(&verts[tris[j*4+1]*3], color); + dd->vertex(&verts[tris[j*4+2]*3], color); + } + } + dd->end(); + + // Internal edges. + dd->begin(DU_DRAW_LINES, 1.0f); + const unsigned int coli = duRGBA(0,0,0,64); + for (int i = 0; i < dmesh.nmeshes; ++i) + { + const unsigned int* m = &dmesh.meshes[i*4]; + const unsigned int bverts = m[0]; + const unsigned int btris = m[2]; + const int ntris = (int)m[3]; + const float* verts = &dmesh.verts[bverts*3]; + const unsigned char* tris = &dmesh.tris[btris*4]; + + for (int j = 0; j < ntris; ++j) + { + const unsigned char* t = &tris[j*4]; + for (int k = 0, kp = 2; k < 3; kp=k++) + { + unsigned char ef = (t[3] >> (kp*2)) & 0x3; + if (ef == 0) + { + // Internal edge + if (t[kp] < t[k]) + { + dd->vertex(&verts[t[kp]*3], coli); + dd->vertex(&verts[t[k]*3], coli); + } + } + } + } + } + dd->end(); + + // External edges. + dd->begin(DU_DRAW_LINES, 2.0f); + const unsigned int cole = duRGBA(0,0,0,64); + for (int i = 0; i < dmesh.nmeshes; ++i) + { + const unsigned int* m = &dmesh.meshes[i*4]; + const unsigned int bverts = m[0]; + const unsigned int btris = m[2]; + const int ntris = (int)m[3]; + const float* verts = &dmesh.verts[bverts*3]; + const unsigned char* tris = &dmesh.tris[btris*4]; + + for (int j = 0; j < ntris; ++j) + { + const unsigned char* t = &tris[j*4]; + for (int k = 0, kp = 2; k < 3; kp=k++) + { + unsigned char ef = (t[3] >> (kp*2)) & 0x3; + if (ef != 0) + { + // Ext edge + dd->vertex(&verts[t[kp]*3], cole); + dd->vertex(&verts[t[k]*3], cole); + } + } + } + } + dd->end(); + + dd->begin(DU_DRAW_POINTS, 3.0f); + const unsigned int colv = duRGBA(0,0,0,64); + for (int i = 0; i < dmesh.nmeshes; ++i) + { + const unsigned int* m = &dmesh.meshes[i*4]; + const unsigned int bverts = m[0]; + const int nverts = (int)m[1]; + const float* verts = &dmesh.verts[bverts*3]; + for (int j = 0; j < nverts; ++j) + dd->vertex(&verts[j*3], colv); + } + dd->end(); +} diff --git a/Dependencies/recastnavigation/DebugUtils/Source/RecastDump.cpp b/Dependencies/recastnavigation/DebugUtils/Source/RecastDump.cpp new file mode 100644 index 000000000..698809557 --- /dev/null +++ b/Dependencies/recastnavigation/DebugUtils/Source/RecastDump.cpp @@ -0,0 +1,449 @@ +// +// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#include +#include +#include +#include +#include "Recast.h" +#include "RecastAlloc.h" +#include "RecastDump.h" + +duFileIO::~duFileIO() +{ + // Defined out of line to fix the weak v-tables warning +} + +static void ioprintf(duFileIO* io, const char* format, ...) +{ + char line[256]; + va_list ap; + va_start(ap, format); + const int n = vsnprintf(line, sizeof(line), format, ap); + va_end(ap); + if (n > 0) + io->write(line, sizeof(char)*n); +} + +bool duDumpPolyMeshToObj(rcPolyMesh& pmesh, duFileIO* io) +{ + if (!io) + { + printf("duDumpPolyMeshToObj: input IO is null.\n"); + return false; + } + if (!io->isWriting()) + { + printf("duDumpPolyMeshToObj: input IO not writing.\n"); + return false; + } + + const int nvp = pmesh.nvp; + const float cs = pmesh.cs; + const float ch = pmesh.ch; + const float* orig = pmesh.bmin; + + ioprintf(io, "# Recast Navmesh\n"); + ioprintf(io, "o NavMesh\n"); + + ioprintf(io, "\n"); + + for (int i = 0; i < pmesh.nverts; ++i) + { + const unsigned short* v = &pmesh.verts[i*3]; + const float x = orig[0] + v[0]*cs; + const float y = orig[1] + (v[1]+1)*ch + 0.1f; + const float z = orig[2] + v[2]*cs; + ioprintf(io, "v %f %f %f\n", x,y,z); + } + + ioprintf(io, "\n"); + + for (int i = 0; i < pmesh.npolys; ++i) + { + const unsigned short* p = &pmesh.polys[i*nvp*2]; + for (int j = 2; j < nvp; ++j) + { + if (p[j] == RC_MESH_NULL_IDX) break; + ioprintf(io, "f %d %d %d\n", p[0]+1, p[j-1]+1, p[j]+1); + } + } + + return true; +} + +bool duDumpPolyMeshDetailToObj(rcPolyMeshDetail& dmesh, duFileIO* io) +{ + if (!io) + { + printf("duDumpPolyMeshDetailToObj: input IO is null.\n"); + return false; + } + if (!io->isWriting()) + { + printf("duDumpPolyMeshDetailToObj: input IO not writing.\n"); + return false; + } + + ioprintf(io, "# Recast Navmesh\n"); + ioprintf(io, "o NavMesh\n"); + + ioprintf(io, "\n"); + + for (int i = 0; i < dmesh.nverts; ++i) + { + const float* v = &dmesh.verts[i*3]; + ioprintf(io, "v %f %f %f\n", v[0],v[1],v[2]); + } + + ioprintf(io, "\n"); + + for (int i = 0; i < dmesh.nmeshes; ++i) + { + const unsigned int* m = &dmesh.meshes[i*4]; + const unsigned int bverts = m[0]; + const unsigned int btris = m[2]; + const unsigned int ntris = m[3]; + const unsigned char* tris = &dmesh.tris[btris*4]; + for (unsigned int j = 0; j < ntris; ++j) + { + ioprintf(io, "f %d %d %d\n", + (int)(bverts+tris[j*4+0])+1, + (int)(bverts+tris[j*4+1])+1, + (int)(bverts+tris[j*4+2])+1); + } + } + + return true; +} + +static const int CSET_MAGIC = ('c' << 24) | ('s' << 16) | ('e' << 8) | 't'; +static const int CSET_VERSION = 2; + +bool duDumpContourSet(struct rcContourSet& cset, duFileIO* io) +{ + if (!io) + { + printf("duDumpContourSet: input IO is null.\n"); + return false; + } + if (!io->isWriting()) + { + printf("duDumpContourSet: input IO not writing.\n"); + return false; + } + + io->write(&CSET_MAGIC, sizeof(CSET_MAGIC)); + io->write(&CSET_VERSION, sizeof(CSET_VERSION)); + + io->write(&cset.nconts, sizeof(cset.nconts)); + + io->write(cset.bmin, sizeof(cset.bmin)); + io->write(cset.bmax, sizeof(cset.bmax)); + + io->write(&cset.cs, sizeof(cset.cs)); + io->write(&cset.ch, sizeof(cset.ch)); + + io->write(&cset.width, sizeof(cset.width)); + io->write(&cset.height, sizeof(cset.height)); + io->write(&cset.borderSize, sizeof(cset.borderSize)); + + for (int i = 0; i < cset.nconts; ++i) + { + const rcContour& cont = cset.conts[i]; + io->write(&cont.nverts, sizeof(cont.nverts)); + io->write(&cont.nrverts, sizeof(cont.nrverts)); + io->write(&cont.reg, sizeof(cont.reg)); + io->write(&cont.area, sizeof(cont.area)); + io->write(cont.verts, sizeof(int)*4*cont.nverts); + io->write(cont.rverts, sizeof(int)*4*cont.nrverts); + } + + return true; +} + +bool duReadContourSet(struct rcContourSet& cset, duFileIO* io) +{ + if (!io) + { + printf("duReadContourSet: input IO is null.\n"); + return false; + } + if (!io->isReading()) + { + printf("duReadContourSet: input IO not reading.\n"); + return false; + } + + int magic = 0; + int version = 0; + + io->read(&magic, sizeof(magic)); + io->read(&version, sizeof(version)); + + if (magic != CSET_MAGIC) + { + printf("duReadContourSet: Bad voodoo.\n"); + return false; + } + if (version != CSET_VERSION) + { + printf("duReadContourSet: Bad version.\n"); + return false; + } + + io->read(&cset.nconts, sizeof(cset.nconts)); + + cset.conts = (rcContour*)rcAlloc(sizeof(rcContour)*cset.nconts, RC_ALLOC_PERM); + if (!cset.conts) + { + printf("duReadContourSet: Could not alloc contours (%d)\n", cset.nconts); + return false; + } + memset(cset.conts, 0, sizeof(rcContour)*cset.nconts); + + io->read(cset.bmin, sizeof(cset.bmin)); + io->read(cset.bmax, sizeof(cset.bmax)); + + io->read(&cset.cs, sizeof(cset.cs)); + io->read(&cset.ch, sizeof(cset.ch)); + + io->read(&cset.width, sizeof(cset.width)); + io->read(&cset.height, sizeof(cset.height)); + io->read(&cset.borderSize, sizeof(cset.borderSize)); + + for (int i = 0; i < cset.nconts; ++i) + { + rcContour& cont = cset.conts[i]; + io->read(&cont.nverts, sizeof(cont.nverts)); + io->read(&cont.nrverts, sizeof(cont.nrverts)); + io->read(&cont.reg, sizeof(cont.reg)); + io->read(&cont.area, sizeof(cont.area)); + + cont.verts = (int*)rcAlloc(sizeof(int)*4*cont.nverts, RC_ALLOC_PERM); + if (!cont.verts) + { + printf("duReadContourSet: Could not alloc contour verts (%d)\n", cont.nverts); + return false; + } + cont.rverts = (int*)rcAlloc(sizeof(int)*4*cont.nrverts, RC_ALLOC_PERM); + if (!cont.rverts) + { + printf("duReadContourSet: Could not alloc contour rverts (%d)\n", cont.nrverts); + return false; + } + + io->read(cont.verts, sizeof(int)*4*cont.nverts); + io->read(cont.rverts, sizeof(int)*4*cont.nrverts); + } + + return true; +} + + +static const int CHF_MAGIC = ('r' << 24) | ('c' << 16) | ('h' << 8) | 'f'; +static const int CHF_VERSION = 3; + +bool duDumpCompactHeightfield(struct rcCompactHeightfield& chf, duFileIO* io) +{ + if (!io) + { + printf("duDumpCompactHeightfield: input IO is null.\n"); + return false; + } + if (!io->isWriting()) + { + printf("duDumpCompactHeightfield: input IO not writing.\n"); + return false; + } + + io->write(&CHF_MAGIC, sizeof(CHF_MAGIC)); + io->write(&CHF_VERSION, sizeof(CHF_VERSION)); + + io->write(&chf.width, sizeof(chf.width)); + io->write(&chf.height, sizeof(chf.height)); + io->write(&chf.spanCount, sizeof(chf.spanCount)); + + io->write(&chf.walkableHeight, sizeof(chf.walkableHeight)); + io->write(&chf.walkableClimb, sizeof(chf.walkableClimb)); + io->write(&chf.borderSize, sizeof(chf.borderSize)); + + io->write(&chf.maxDistance, sizeof(chf.maxDistance)); + io->write(&chf.maxRegions, sizeof(chf.maxRegions)); + + io->write(chf.bmin, sizeof(chf.bmin)); + io->write(chf.bmax, sizeof(chf.bmax)); + + io->write(&chf.cs, sizeof(chf.cs)); + io->write(&chf.ch, sizeof(chf.ch)); + + int tmp = 0; + if (chf.cells) tmp |= 1; + if (chf.spans) tmp |= 2; + if (chf.dist) tmp |= 4; + if (chf.areas) tmp |= 8; + + io->write(&tmp, sizeof(tmp)); + + if (chf.cells) + io->write(chf.cells, sizeof(rcCompactCell)*chf.width*chf.height); + if (chf.spans) + io->write(chf.spans, sizeof(rcCompactSpan)*chf.spanCount); + if (chf.dist) + io->write(chf.dist, sizeof(unsigned short)*chf.spanCount); + if (chf.areas) + io->write(chf.areas, sizeof(unsigned char)*chf.spanCount); + + return true; +} + +bool duReadCompactHeightfield(struct rcCompactHeightfield& chf, duFileIO* io) +{ + if (!io) + { + printf("duReadCompactHeightfield: input IO is null.\n"); + return false; + } + if (!io->isReading()) + { + printf("duReadCompactHeightfield: input IO not reading.\n"); + return false; + } + + int magic = 0; + int version = 0; + + io->read(&magic, sizeof(magic)); + io->read(&version, sizeof(version)); + + if (magic != CHF_MAGIC) + { + printf("duReadCompactHeightfield: Bad voodoo.\n"); + return false; + } + if (version != CHF_VERSION) + { + printf("duReadCompactHeightfield: Bad version.\n"); + return false; + } + + io->read(&chf.width, sizeof(chf.width)); + io->read(&chf.height, sizeof(chf.height)); + io->read(&chf.spanCount, sizeof(chf.spanCount)); + + io->read(&chf.walkableHeight, sizeof(chf.walkableHeight)); + io->read(&chf.walkableClimb, sizeof(chf.walkableClimb)); + io->read(&chf.borderSize, sizeof(chf.borderSize)); + + io->read(&chf.maxDistance, sizeof(chf.maxDistance)); + io->read(&chf.maxRegions, sizeof(chf.maxRegions)); + + io->read(chf.bmin, sizeof(chf.bmin)); + io->read(chf.bmax, sizeof(chf.bmax)); + + io->read(&chf.cs, sizeof(chf.cs)); + io->read(&chf.ch, sizeof(chf.ch)); + + int tmp = 0; + io->read(&tmp, sizeof(tmp)); + + if (tmp & 1) + { + chf.cells = (rcCompactCell*)rcAlloc(sizeof(rcCompactCell)*chf.width*chf.height, RC_ALLOC_PERM); + if (!chf.cells) + { + printf("duReadCompactHeightfield: Could not alloc cells (%d)\n", chf.width*chf.height); + return false; + } + io->read(chf.cells, sizeof(rcCompactCell)*chf.width*chf.height); + } + if (tmp & 2) + { + chf.spans = (rcCompactSpan*)rcAlloc(sizeof(rcCompactSpan)*chf.spanCount, RC_ALLOC_PERM); + if (!chf.spans) + { + printf("duReadCompactHeightfield: Could not alloc spans (%d)\n", chf.spanCount); + return false; + } + io->read(chf.spans, sizeof(rcCompactSpan)*chf.spanCount); + } + if (tmp & 4) + { + chf.dist = (unsigned short*)rcAlloc(sizeof(unsigned short)*chf.spanCount, RC_ALLOC_PERM); + if (!chf.dist) + { + printf("duReadCompactHeightfield: Could not alloc dist (%d)\n", chf.spanCount); + return false; + } + io->read(chf.dist, sizeof(unsigned short)*chf.spanCount); + } + if (tmp & 8) + { + chf.areas = (unsigned char*)rcAlloc(sizeof(unsigned char)*chf.spanCount, RC_ALLOC_PERM); + if (!chf.areas) + { + printf("duReadCompactHeightfield: Could not alloc areas (%d)\n", chf.spanCount); + return false; + } + io->read(chf.areas, sizeof(unsigned char)*chf.spanCount); + } + + return true; +} + + +static void logLine(rcContext& ctx, rcTimerLabel label, const char* name, const float pc) +{ + const int t = ctx.getAccumulatedTime(label); + if (t < 0) return; + ctx.log(RC_LOG_PROGRESS, "%s:\t%.2fms\t(%.1f%%)", name, t/1000.0f, t*pc); +} + +void duLogBuildTimes(rcContext& ctx, const int totalTimeUsec) +{ + const float pc = 100.0f / totalTimeUsec; + + ctx.log(RC_LOG_PROGRESS, "Build Times"); + logLine(ctx, RC_TIMER_RASTERIZE_TRIANGLES, "- Rasterize", pc); + logLine(ctx, RC_TIMER_BUILD_COMPACTHEIGHTFIELD, "- Build Compact", pc); + logLine(ctx, RC_TIMER_FILTER_BORDER, "- Filter Border", pc); + logLine(ctx, RC_TIMER_FILTER_WALKABLE, "- Filter Walkable", pc); + logLine(ctx, RC_TIMER_ERODE_AREA, "- Erode Area", pc); + logLine(ctx, RC_TIMER_MEDIAN_AREA, "- Median Area", pc); + logLine(ctx, RC_TIMER_MARK_BOX_AREA, "- Mark Box Area", pc); + logLine(ctx, RC_TIMER_MARK_CONVEXPOLY_AREA, "- Mark Convex Area", pc); + logLine(ctx, RC_TIMER_MARK_CYLINDER_AREA, "- Mark Cylinder Area", pc); + logLine(ctx, RC_TIMER_BUILD_DISTANCEFIELD, "- Build Distance Field", pc); + logLine(ctx, RC_TIMER_BUILD_DISTANCEFIELD_DIST, " - Distance", pc); + logLine(ctx, RC_TIMER_BUILD_DISTANCEFIELD_BLUR, " - Blur", pc); + logLine(ctx, RC_TIMER_BUILD_REGIONS, "- Build Regions", pc); + logLine(ctx, RC_TIMER_BUILD_REGIONS_WATERSHED, " - Watershed", pc); + logLine(ctx, RC_TIMER_BUILD_REGIONS_EXPAND, " - Expand", pc); + logLine(ctx, RC_TIMER_BUILD_REGIONS_FLOOD, " - Find Basins", pc); + logLine(ctx, RC_TIMER_BUILD_REGIONS_FILTER, " - Filter", pc); + logLine(ctx, RC_TIMER_BUILD_LAYERS, "- Build Layers", pc); + logLine(ctx, RC_TIMER_BUILD_CONTOURS, "- Build Contours", pc); + logLine(ctx, RC_TIMER_BUILD_CONTOURS_TRACE, " - Trace", pc); + logLine(ctx, RC_TIMER_BUILD_CONTOURS_SIMPLIFY, " - Simplify", pc); + logLine(ctx, RC_TIMER_BUILD_POLYMESH, "- Build Polymesh", pc); + logLine(ctx, RC_TIMER_BUILD_POLYMESHDETAIL, "- Build Polymesh Detail", pc); + logLine(ctx, RC_TIMER_MERGE_POLYMESH, "- Merge Polymeshes", pc); + logLine(ctx, RC_TIMER_MERGE_POLYMESHDETAIL, "- Merge Polymesh Details", pc); + ctx.log(RC_LOG_PROGRESS, "=== TOTAL:\t%.2fms", totalTimeUsec/1000.0f); +} + diff --git a/Dependencies/recastnavigation/Detour/CMakeLists.txt b/Dependencies/recastnavigation/Detour/CMakeLists.txt new file mode 100644 index 000000000..c8eb858f5 --- /dev/null +++ b/Dependencies/recastnavigation/Detour/CMakeLists.txt @@ -0,0 +1,40 @@ +file(GLOB SOURCES Source/*.cpp) +add_library(Detour ${SOURCES}) + +add_library(RecastNavigation::Detour ALIAS Detour) +set_target_properties(Detour PROPERTIES DEBUG_POSTFIX -d) + +set(Detour_INCLUDE_DIR "${CMAKE_CURRENT_SOURCE_DIR}/Include") + +if(RECASTNAVIGATION_DT_POLYREF64) + target_compile_definitions(Detour PUBLIC DT_POLYREF64) +endif() +if(RECASTNAVIGATION_DT_VIRTUAL_QUERYFILTER) + target_compile_definitions(Detour PUBLIC DT_VIRTUAL_QUERYFILTER) +endif() + +target_include_directories(Detour PUBLIC + "$" +) + +set_target_properties(Detour PROPERTIES + SOVERSION ${SOVERSION} + VERSION ${LIB_VERSION} + COMPILE_PDB_OUTPUT_DIRECTORY . + COMPILE_PDB_NAME "Detour-d" + ) + +install(TARGETS Detour + EXPORT recastnavigation-targets + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} COMPONENT library + INCLUDES DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} ${CMAKE_INSTALL_INCLUDEDIR}/recastnavigation + ) + +file(GLOB INCLUDES Include/*.h) +install(FILES ${INCLUDES} DESTINATION + ${CMAKE_INSTALL_INCLUDEDIR}/recastnavigation) +if(MSVC) + install(FILES "$/Detour-d.pdb" CONFIGURATIONS "Debug" DESTINATION "lib" OPTIONAL) +endif() diff --git a/Dependencies/recastnavigation/Detour/Include/DetourAlloc.h b/Dependencies/recastnavigation/Detour/Include/DetourAlloc.h new file mode 100644 index 000000000..f87b454ac --- /dev/null +++ b/Dependencies/recastnavigation/Detour/Include/DetourAlloc.h @@ -0,0 +1,61 @@ +// +// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#ifndef DETOURALLOCATOR_H +#define DETOURALLOCATOR_H + +#include + +/// Provides hint values to the memory allocator on how long the +/// 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. +}; + +/// A memory allocation function. +// @param[in] size The size, in bytes of memory, to allocate. +// @param[in] rcAllocHint A hint to the allocator on how long the memory is expected to be in use. +// @return A pointer to the beginning of the allocated memory block, or null if the allocation failed. +/// @see dtAllocSetCustom +typedef void* (dtAllocFunc)(size_t size, dtAllocHint hint); + +/// A memory deallocation function. +/// @param[in] ptr A pointer to a memory block previously allocated using #dtAllocFunc. +/// @see dtAllocSetCustom +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); + +/// Allocates a memory block. +/// @param[in] size The size, in bytes of memory, to allocate. +/// @param[in] hint A hint to the allocator on how long the memory is expected to be in use. +/// @return A pointer to the beginning of the allocated memory block, or null if the allocation failed. +/// @see dtFree +void* dtAlloc(size_t size, dtAllocHint hint); + +/// Deallocates a memory block. +/// @param[in] ptr A pointer to a memory block previously allocated using #dtAlloc. +/// @see dtAlloc +void dtFree(void* ptr); + +#endif diff --git a/Dependencies/recastnavigation/Detour/Include/DetourAssert.h b/Dependencies/recastnavigation/Detour/Include/DetourAssert.h new file mode 100644 index 000000000..ea58dd9a0 --- /dev/null +++ b/Dependencies/recastnavigation/Detour/Include/DetourAssert.h @@ -0,0 +1,56 @@ +// +// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#ifndef DETOURASSERT_H +#define DETOURASSERT_H + +// Note: This header file's only purpose is to include define assert. +// Feel free to change the file and include your own implementation instead. + +#ifdef RC_DISABLE_ASSERTS + +// 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 + +/// An assertion failure function. +// @param[in] expression asserted expression. +// @param[in] file Filename of the failed assertion. +// @param[in] line Line number of the failed assertion. +/// @see dtAssertFailSetCustom +typedef void (dtAssertFailFunc)(const char* expression, const char* file, int line); + +/// 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); + +/// Gets the base custom assertion failure function to be used by Detour. +dtAssertFailFunc* dtAssertFailGetCustom(); + +# include +# define dtAssert(expression) \ + { \ + dtAssertFailFunc* failFunc = dtAssertFailGetCustom(); \ + if(failFunc == NULL) { assert(expression); } \ + else if(!(expression)) { (*failFunc)(#expression, __FILE__, __LINE__); } \ + } + +#endif + +#endif // DETOURASSERT_H diff --git a/Dependencies/recastnavigation/Detour/Include/DetourCommon.h b/Dependencies/recastnavigation/Detour/Include/DetourCommon.h new file mode 100644 index 000000000..d54bc7edc --- /dev/null +++ b/Dependencies/recastnavigation/Detour/Include/DetourCommon.h @@ -0,0 +1,571 @@ +// +// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#ifndef DETOURCOMMON_H +#define DETOURCOMMON_H + +#include "DetourMath.h" +#include + +/** +@defgroup detour Detour + +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 +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. +template void dtIgnoreUnused(const T&) { } + +/// Swaps the values of the two parameters. +/// @param[in,out] a Value A +/// @param[in,out] b Value B +template inline void dtSwap(T& a, T& b) { T t = a; a = b; b = t; } + +/// Returns the minimum of two values. +/// @param[in] a Value A +/// @param[in] b Value B +/// @return The minimum of the two values. +template inline T dtMin(T a, T b) { return a < b ? a : b; } + +/// Returns the maximum of two values. +/// @param[in] a Value A +/// @param[in] b Value B +/// @return The maximum of the two values. +template inline T dtMax(T a, T b) { return a > b ? a : b; } + +/// Returns the absolute value. +/// @param[in] a The value. +/// @return The absolute value of the specified value. +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; } + +/// Clamps the value to the specified range. +/// @param[in] v The value to clamp. +/// @param[in] mn The minimum permitted return value. +/// @param[in] mx The maximum permitted return value. +/// @return The value, clamped to the specified range. +template inline T dtClamp(T v, T mn, T mx) { return v < mn ? mn : (v > mx ? mx : 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]; +} + +/// Derives the dot product of two vectors. (@p v1 . @p v2) +/// @param[in] v1 A Vector [(x, y, z)] +/// @param[in] v2 A vector [(x, y, z)] +/// @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]; +} + +/// Performs a scaled vector addition. (@p v1 + (@p v2 * @p s)) +/// @param[out] dest The result vector. [(x, y, z)] +/// @param[in] v1 The base vector. [(x, y, z)] +/// @param[in] v2 The vector to scale and add to @p v1. [(x, y, z)] +/// @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; +} + +/// Performs a linear interpolation between two vectors. (@p v1 toward @p v2) +/// @param[out] dest The result vector. [(x, y, x)] +/// @param[in] v1 The starting vector. +/// @param[in] v2 The destination vector. +/// @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; +} + +/// Performs a vector addition. (@p v1 + @p v2) +/// @param[out] dest The result vector. [(x, y, z)] +/// @param[in] v1 The base vector. [(x, y, z)] +/// @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]; +} + +/// Performs a vector subtraction. (@p v1 - @p v2) +/// @param[out] dest The result vector. [(x, y, z)] +/// @param[in] v1 The base vector. [(x, y, z)] +/// @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]; +} + +/// Scales the vector by the specified value. (@p v * @p t) +/// @param[out] dest The result vector. [(x, y, z)] +/// @param[in] v The vector to scale. [(x, y, z)] +/// @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; +} + +/// Selects the minimum value of each element from the specified vectors. +/// @param[in,out] mn A vector. (Will be updated with the result.) [(x, y, z)] +/// @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]); +} + +/// Selects the maximum value of each element from the specified vectors. +/// @param[in,out] mx A vector. (Will be updated with the result.) [(x, y, z)] +/// @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]); +} + +/// Sets the vector elements to the specified values. +/// @param[out] dest The result vector. [(x, y, z)] +/// @param[in] x The x-value of the vector. +/// @param[in] y The y-value of the vector. +/// @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; +} + +/// Performs a vector copy. +/// @param[out] dest The result. [(x, y, 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]; +} + +/// Derives the scalar length of the vector. +/// @param[in] v The vector. [(x, y, z)] +/// @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]); +} + +/// Derives the square of the scalar length of the vector. (len * len) +/// @param[in] v The vector. [(x, y, z)] +/// @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]; +} + +/// Returns the distance between two points. +/// @param[in] v1 A point. [(x, y, z)] +/// @param[in] v2 A point. [(x, y, z)] +/// @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); +} + +/// Returns the square of the distance between two points. +/// @param[in] v1 A point. [(x, y, z)] +/// @param[in] v2 A point. [(x, y, z)] +/// @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; +} + +/// Derives the distance between the specified points on the xz-plane. +/// @param[in] v1 A point. [(x, y, z)] +/// @param[in] v2 A point. [(x, y, z)] +/// @return The distance between the point on the xz-plane. +/// +/// 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); +} + +/// Derives the square of the distance between the specified points on the xz-plane. +/// @param[in] v1 A point. [(x, y, z)] +/// @param[in] v2 A point. [(x, y, z)] +/// @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; +} + +/// 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; +} + +/// Performs a 'sloppy' colocation check of the specified points. +/// @param[in] p0 A point. [(x, y, z)] +/// @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 +/// 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; +} + +/// Checks that the specified vector's components are all finite. +/// @param[in] v A point. [(x, y, z)] +/// @return True if all of the point's components are finite, i.e. not NaN +/// or any of the infinities. +inline bool dtVisfinite(const float* v) +{ + bool result = + dtMathIsfinite(v[0]) && + dtMathIsfinite(v[1]) && + dtMathIsfinite(v[2]); + + 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; +} + +/// Derives the dot product of two vectors on the xz-plane. (@p u . @p v) +/// @param[in] u A vector [(x, y, z)] +/// @param[in] v A vector [(x, y, z)] +/// @return The dot product on the xz-plane. +/// +/// 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]; +} + +/// 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 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]; +} + +/// @} +/// @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)] +/// @param[in] c Vertex C. [(x, y, z)] +/// @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; +} + +/// Determines if two axis-aligned bounding boxes overlap. +/// @param[in] amin Minimum bounds of box A. [(x, y, z)] +/// @param[in] amax Maximum bounds of box A. [(x, y, z)] +/// @param[in] bmin Minimum bounds of box B. [(x, y, z)] +/// @param[in] bmax Maximum bounds of box B. [(x, y, z)] +/// @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]) +{ + 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. +/// @param[in] amin Minimum bounds of box A. [(x, y, z)] +/// @param[in] amax Maximum bounds of box A. [(x, y, z)] +/// @param[in] bmin Minimum bounds of box B. [(x, y, z)] +/// @param[in] bmax Maximum bounds of box B. [(x, y, z)] +/// @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) +{ + 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[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); + +/// 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)] +/// @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)] +/// @param[out] h The resulting height. +bool dtClosestHeightPointTriangle(const float* p, const float* a, const float* b, const float* c, float& h); + +bool dtIntersectSegmentPoly2D(const float* p0, const float* p1, + const float* verts, int nverts, + float& tmin, float& tmax, + int& segMin, int& segMax); + +bool dtIntersectSegSeg2D(const float* ap, const float* aq, + 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)] +/// @param[in] verts The polygon vertices. [(x, y, z) * @p nverts] +/// @param[in] nverts The number of vertices. [Limit: >= 3] +/// @return True if the point is inside the polygon. +bool dtPointInPolygon(const float* pt, const float* verts, const int nverts); + +bool dtDistancePtPolyEdgesSqr(const float* pt, const float* verts, const int nverts, + float* ed, float* et); + +float dtDistancePtSegSqr2D(const float* pt, const float* p, const float* q, float& t); + +/// Derives the centroid of a convex polygon. +/// @param[out] tc The centroid of the polgyon. [(x, y, z)] +/// @param[in] idx The polygon indices. [(vertIndex) * @p nidx] +/// @param[in] nidx The number of indices in the polygon. [Limit: >= 3] +/// @param[in] verts The polygon vertices. [(x, y, z) * vertCount] +void dtCalcPolyCenter(float* tc, const unsigned short* idx, int nidx, const float* verts); + +/// Determines if the two convex polygons overlap on the xz-plane. +/// @param[in] polya Polygon A vertices. [(x, y, z) * @p npolya] +/// @param[in] npolya The number of vertices in polygon A. +/// @param[in] polyb Polygon B vertices. [(x, y, z) * @p npolyb] +/// @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); + +/// @} +/// @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; +} + +inline unsigned int dtIlog2(unsigned int v) +{ + unsigned int r; + unsigned int shift; + r = (v > 0xffff) << 4; v >>= r; + shift = (v > 0xff) << 3; v >>= shift; r |= shift; + shift = (v > 0xf) << 2; v >>= shift; r |= shift; + shift = (v > 0x3) << 1; v >>= shift; r |= shift; + r |= (v >> 1); + return r; +} + +inline int dtAlign4(int x) { return (x+3) & ~3; } + +inline int dtOppositeTile(int side) { return (side+4) & 0x7; } + +inline void dtSwapByte(unsigned char* a, unsigned char* b) +{ + 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); +} + +inline void dtSwapEndian(short* v) +{ + 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); +} + +inline void dtSwapEndian(int* v) +{ + 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); +} + +void dtRandomPointInConvexPoly(const float* pts, const int npts, float* areas, + 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; +} + +template +TypeToRetrieveAs* dtGetThenAdvanceBufferPointer(unsigned char*& buffer, const size_t distanceToAdvance) +{ + TypeToRetrieveAs* returnPointer = reinterpret_cast(buffer); + buffer += distanceToAdvance; + return returnPointer; +} + + +/// @} + +#endif // DETOURCOMMON_H + +/////////////////////////////////////////////////////////////////////////// + +// This section contains detailed documentation for members that don't have +// a source file. It reduces clutter in the main section of the header. + +/** + +@fn float dtTriArea2D(const float* a, const float* b, const float* c) +@par + +The vertices are projected onto the xz-plane, so the y-values are ignored. + +This is a low cost function than can be used for various purposes. Its main purpose +is for point/line relationship testing. + +In all cases: A value of zero indicates that all vertices are collinear or represent the same point. +(On the xz-plane.) + +When used for point/line relationship tests, AB usually represents a line against which +the C point is to be tested. In this case: + +A positive value indicates that point C is to the left of line AB, looking from A toward B.
+A negative value indicates that point C is to the right of lineAB, looking from A toward B. + +When used for evaluating a triangle: + +The absolute value of the return value is two times the area of the triangle when it is +projected onto the xz-plane. + +A positive return value indicates: + +
    +
  • The vertices are wrapped in the normal Detour wrap direction.
    • +
  • The triangle's 3D face normal is in the general up direction.
    • +
+ +A negative return value indicates: + +
    +
  • The vertices are reverse wrapped. (Wrapped opposite the normal Detour wrap direction.)
    • +
  • The triangle's 3D face normal is in the general down direction.
    • +
+ +*/ diff --git a/Dependencies/recastnavigation/Detour/Include/DetourMath.h b/Dependencies/recastnavigation/Detour/Include/DetourMath.h new file mode 100644 index 000000000..5528ead74 --- /dev/null +++ b/Dependencies/recastnavigation/Detour/Include/DetourMath.h @@ -0,0 +1,21 @@ +/** +@defgroup detour Detour + +Members in this module are wrappers around the standard math library +*/ + +#ifndef DETOURMATH_H +#define DETOURMATH_H + +#include + +inline float dtMathFabsf(float x) { return fabsf(x); } +inline float dtMathSqrtf(float x) { return sqrtf(x); } +inline float dtMathFloorf(float x) { return floorf(x); } +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 isfinite(x); } + +#endif diff --git a/Dependencies/recastnavigation/Detour/Include/DetourNavMesh.h b/Dependencies/recastnavigation/Detour/Include/DetourNavMesh.h new file mode 100644 index 000000000..3c3118756 --- /dev/null +++ b/Dependencies/recastnavigation/Detour/Include/DetourNavMesh.h @@ -0,0 +1,784 @@ +// +// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#ifndef DETOURNAVMESH_H +#define DETOURNAVMESH_H + +#include "DetourAlloc.h" +#include "DetourStatus.h" + +// Undefine (or define in a build config) 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 + +#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 +#include +#endif + +// Note: If you want to use 64-bit refs, change the types of both dtPolyRef & dtTileRef. +// It is also recommended that you change dtHashRef() to a proper 64-bit hash. + +/// A handle to a polygon within a navigation mesh tile. +/// @ingroup detour +#ifdef DT_POLYREF64 +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 + +/// A handle to a tile within a navigation mesh. +/// @ingroup detour +#ifdef DT_POLYREF64 +typedef uint64_t dtTileRef; +#else +typedef unsigned int dtTileRef; +#endif + +/// The maximum number of vertices per navigation polygon. +/// @ingroup detour +static const int DT_VERTS_PER_POLYGON = 6; + +/// @{ +/// @name Tile Serialization Constants +/// These constants are used to detect whether a navigation tile's data +/// and state format is compatible with the current build. +/// + +/// A magic number used to detect compatibility of navigation tile data. +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'; + +/// A version number used to detect compatibility of navigation tile states. +static const int DT_NAVMESH_STATE_VERSION = 1; + +/// @} + +/// A flag that indicates that an entity links to an external entity. +/// (E.g. A polygon edge is a portal that links to another polygon.) +static const unsigned short DT_EXT_LINK = 0x8000; + +/// A value that indicates the entity does not link to anything. +static const unsigned int DT_NULL_LINK = 0xffffffff; + +/// A flag that indicates that an off-mesh connection can be traversed in both directions. (Is bidirectional.) +static const unsigned int DT_OFFMESH_CON_BIDIR = 1; + +/// The maximum number of user defined area ids. +/// @ingroup detour +static const int DT_MAX_AREAS = 64; + +/// Tile flags used for various functions and fields. +/// 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 +}; + +/// 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. +}; + +/// 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. +}; + + +/// Options for dtNavMeshQuery::initSlicedFindPath and updateSlicedFindPath +enum dtFindPathOptions +{ + 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 +}; + +enum dtDetailTriEdgeFlags +{ + 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; + +/// 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 +}; + + +/// 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; + + /// 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]; + + /// The user defined polygon flags. + unsigned short flags; + + /// 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 access 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 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 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. +}; + +/// Defines a link between polygons. +/// @note This structure is rarely if ever used by the end user. +/// @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. +}; + +/// Bounding volume node. +/// @note This structure is rarely if ever used by the end user. +/// @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.) +}; + +/// 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 radius of the endpoints. [Limit: >= 0] + float rad; + + /// 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; + + /// End point side. + unsigned char side; + + /// 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; +}; + +/// 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. [(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&); +}; + +/// Get flags for edge in detail triangle. +/// @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; +} + +/// Configuration parameters used to define multi-tile navigation meshes. +/// The values are used to allocate space during the initialization of a navigation mesh. +/// @see dtNavMesh::init() +/// @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. 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. +/// @ingroup detour +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 + { +#ifdef DT_POLYREF64 + 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; +#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 + { +#ifdef DT_POLYREF64 + 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_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 + { +#ifdef DT_POLYREF64 + const dtPolyRef saltMask = ((dtPolyRef)1<> (DT_POLY_BITS+DT_TILE_BITS)) & saltMask); +#else + 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 + { +#ifdef DT_POLYREF64 + const dtPolyRef tileMask = ((dtPolyRef)1<> DT_POLY_BITS) & tileMask); +#else + 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 + { +#ifdef DT_POLYREF64 + const dtPolyRef polyMask = ((dtPolyRef)1<header->bvQuantFactor; +const dtBVNode* n = &tile->bvTree[i]; +if (n->i >= 0) +{ + // This is a leaf node. + float worldMinX = tile->header->bmin[0] + n->bmin[0]*cs; + float worldMinY = tile->header->bmin[0] + n->bmin[1]*cs; + // Etc... +} +@endcode + +@struct dtMeshTile +@par + +Tiles generally only exist within the context of a dtNavMesh object. + +Some tile content is optional. For example, a tile may not contain any +off-mesh connections. In this case the associated pointer will be null. + +If a detail mesh exists it will share vertices with the base polygon mesh. +Only the vertices unique to the detail mesh will be stored in #detailVerts. + +@warning Tiles returned by a dtNavMesh object are not guarenteed to be populated. +For example: The tile at a location might not have been loaded yet, or may have been removed. +In this case, pointers will be null. So if in doubt, check the polygon count in the +tile's header to determine if a tile has polygons defined. + +@var float dtOffMeshConnection::pos[6] +@par + +For a properly built navigation mesh, vertex A will always be within the bounds of the mesh. +Vertex B is not required to be within the bounds of the mesh. + +*/ diff --git a/Dependencies/recastnavigation/Detour/Include/DetourNavMeshBuilder.h b/Dependencies/recastnavigation/Detour/Include/DetourNavMeshBuilder.h new file mode 100644 index 000000000..f99be5d21 --- /dev/null +++ b/Dependencies/recastnavigation/Detour/Include/DetourNavMeshBuilder.h @@ -0,0 +1,149 @@ +// +// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#ifndef DETOURNAVMESHBUILDER_H +#define DETOURNAVMESHBUILDER_H + +#include "DetourAlloc.h" + +/// Represents the source data used to build an navigation mesh tile. +/// @ingroup detour +struct dtNavMeshCreateParams +{ + + /// @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 destination 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. +/// @ingroup detour +/// @param[in] params Tile creation data. +/// @param[out] outData The resulting tile data. +/// @param[out] outDataSize The size of the tile data array. +/// @return True if the tile data was successfully created. +bool dtCreateNavMeshData(dtNavMeshCreateParams* params, unsigned char** outData, int* outDataSize); + +/// Swaps the endianess of the tile data's header (#dtMeshHeader). +/// @param[in,out] data The tile data array. +/// @param[in] dataSize The size of the data array. +bool dtNavMeshHeaderSwapEndian(unsigned char* data, const int dataSize); + +/// Swaps endianess of the tile data. +/// @param[in,out] data The tile data array. +/// @param[in] dataSize The size of the data array. +bool dtNavMeshDataSwapEndian(unsigned char* data, const int dataSize); + +#endif // DETOURNAVMESHBUILDER_H + +// This section contains detailed documentation for members that don't have +// a source file. It reduces clutter in the main section of the header. + +/** + +@struct dtNavMeshCreateParams +@par + +This structure is used to marshal data between the Recast mesh generation pipeline and Detour navigation components. + +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 +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 +to a navigation mesh using either the dtNavMesh single tile init() function or the dtNavMesh::addTile() +function. + +@see dtCreateNavMeshData + +*/ + diff --git a/Dependencies/recastnavigation/Detour/Include/DetourNavMeshQuery.h b/Dependencies/recastnavigation/Detour/Include/DetourNavMeshQuery.h new file mode 100644 index 000000000..dd8150c29 --- /dev/null +++ b/Dependencies/recastnavigation/Detour/Include/DetourNavMeshQuery.h @@ -0,0 +1,590 @@ +// +// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#ifndef DETOURNAVMESHQUERY_H +#define DETOURNAVMESHQUERY_H + +#include "DetourNavMesh.h" +#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. + +//#define DT_VIRTUAL_QUERYFILTER 1 + +/// Defines polygon filtering and traversal costs for navigation mesh query operations. +/// @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 visited. (Used by default implementation.) + +public: + dtQueryFilter(); + +#ifdef DT_VIRTUAL_QUERYFILTER + 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. +#ifdef DT_VIRTUAL_QUERYFILTER + virtual bool passFilter(const dtPolyRef ref, + const dtMeshTile* tile, + const dtPoly* poly) const; +#else + bool passFilter(const dtPolyRef ref, + const dtMeshTile* tile, + const dtPoly* poly) const; +#endif + + /// Returns cost to 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; +#else + float getCost(const float* pa, const float* pb, + const dtPolyRef prevRef, const dtMeshTile* prevTile, const dtPoly* prevPoly, + const dtPolyRef curRef, const dtMeshTile* curTile, const dtPoly* curPoly, + const dtPolyRef nextRef, const dtMeshTile* nextTile, const dtPoly* nextPoly) const; +#endif + + /// @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; } + + ///@} + +}; + +/// 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. +/// Used by dtNavMeshQuery::queryPolygons. +/// @ingroup detour +class dtPolyQuery +{ +public: + 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; +}; + +/// Provides the ability to perform pathfinding related queries against +/// a navigation mesh. +/// @ingroup detour +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 reference 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. + ///@{ + + /// Initializes a sliced path query. + /// @param[in] startRef The reference 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. +}; + +/// Allocates a query object using the Detour allocator. +/// @return An allocated query object, or null on failure. +/// @ingroup detour +dtNavMeshQuery* dtAllocNavMeshQuery(); + +/// Frees the specified query object using the Detour allocator. +/// @param[in] query A query object allocated using #dtAllocNavMeshQuery +/// @ingroup detour +void dtFreeNavMeshQuery(dtNavMeshQuery* query); + +#endif // DETOURNAVMESHQUERY_H diff --git a/Dependencies/recastnavigation/Detour/Include/DetourNode.h b/Dependencies/recastnavigation/Detour/Include/DetourNode.h new file mode 100644 index 000000000..8918d4686 --- /dev/null +++ b/Dependencies/recastnavigation/Detour/Include/DetourNode.h @@ -0,0 +1,168 @@ +// +// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#ifndef DETOURNODE_H +#define DETOURNODE_H + +#include "DetourNavMesh.h" + +enum dtNodeFlags +{ + DT_NODE_OPEN = 0x01, + DT_NODE_CLOSED = 0x02, + DT_NODE_PARENT_DETACHED = 0x04 // parent of the node is not adjacent. Found using raycast. +}; + +typedef unsigned short dtNodeIndex; +static const dtNodeIndex DT_NULL_IDX = (dtNodeIndex)~0; + +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. +}; + +static const int DT_MAX_STATES_PER_NODE = 1 << DT_NODE_STATE_BITS; // number of extra states per node. See dtNode::state + +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; } + +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; +}; + +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; } + +private: + // 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; +}; + + +#endif // DETOURNODE_H diff --git a/Dependencies/recastnavigation/Detour/Include/DetourStatus.h b/Dependencies/recastnavigation/Detour/Include/DetourStatus.h new file mode 100644 index 000000000..8e1bb44b9 --- /dev/null +++ b/Dependencies/recastnavigation/Detour/Include/DetourStatus.h @@ -0,0 +1,65 @@ +// +// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#ifndef DETOURSTATUS_H +#define DETOURSTATUS_H + +typedef unsigned int dtStatus; + +// High level status. +static const unsigned int DT_FAILURE = 1u << 31; // Operation failed. +static const unsigned int DT_SUCCESS = 1u << 30; // Operation succeed. +static const unsigned int DT_IN_PROGRESS = 1u << 29; // Operation still in progress. + +// Detail information for status. +static const unsigned int DT_STATUS_DETAIL_MASK = 0x0ffffff; +static const unsigned int DT_WRONG_MAGIC = 1 << 0; // Input data is not recognized. +static const unsigned int DT_WRONG_VERSION = 1 << 1; // Input data is in wrong version. +static const unsigned int DT_OUT_OF_MEMORY = 1 << 2; // Operation ran out of memory. +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_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; +} + +// Returns true of status is failure. +inline bool dtStatusFailed(dtStatus status) +{ + return (status & DT_FAILURE) != 0; +} + +// Returns true of status is in progress. +inline bool dtStatusInProgress(dtStatus status) +{ + 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; +} + +#endif // DETOURSTATUS_H diff --git a/Dependencies/recastnavigation/Detour/Source/DetourAlloc.cpp b/Dependencies/recastnavigation/Detour/Source/DetourAlloc.cpp new file mode 100644 index 000000000..d9ad1fc01 --- /dev/null +++ b/Dependencies/recastnavigation/Detour/Source/DetourAlloc.cpp @@ -0,0 +1,50 @@ +// +// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#include +#include "DetourAlloc.h" + +static void *dtAllocDefault(size_t size, dtAllocHint) +{ + return malloc(size); +} + +static void dtFreeDefault(void *ptr) +{ + free(ptr); +} + +static dtAllocFunc* sAllocFunc = dtAllocDefault; +static dtFreeFunc* sFreeFunc = dtFreeDefault; + +void dtAllocSetCustom(dtAllocFunc *allocFunc, dtFreeFunc *freeFunc) +{ + sAllocFunc = allocFunc ? allocFunc : dtAllocDefault; + sFreeFunc = freeFunc ? freeFunc : dtFreeDefault; +} + +void* dtAlloc(size_t size, dtAllocHint hint) +{ + return sAllocFunc(size, hint); +} + +void dtFree(void* ptr) +{ + if (ptr) + sFreeFunc(ptr); +} diff --git a/Dependencies/recastnavigation/Detour/Source/DetourAssert.cpp b/Dependencies/recastnavigation/Detour/Source/DetourAssert.cpp new file mode 100644 index 000000000..a9727a319 --- /dev/null +++ b/Dependencies/recastnavigation/Detour/Source/DetourAssert.cpp @@ -0,0 +1,35 @@ +// +// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#include "DetourAssert.h" + +#ifndef RC_DISABLE_ASSERTS + +static dtAssertFailFunc* sAssertFailFunc = 0; + +void dtAssertFailSetCustom(dtAssertFailFunc *assertFailFunc) +{ + sAssertFailFunc = assertFailFunc; +} + +dtAssertFailFunc* dtAssertFailGetCustom() +{ + return sAssertFailFunc; +} + +#endif diff --git a/Dependencies/recastnavigation/Detour/Source/DetourCommon.cpp b/Dependencies/recastnavigation/Detour/Source/DetourCommon.cpp new file mode 100644 index 000000000..b8457828e --- /dev/null +++ b/Dependencies/recastnavigation/Detour/Source/DetourCommon.cpp @@ -0,0 +1,387 @@ +// +// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#include "DetourCommon.h" +#include "DetourMath.h" + +////////////////////////////////////////////////////////////////////////////////////////// + +void dtClosestPtPointTriangle(float* closest, const float* p, + const float* a, const float* b, const float* c) +{ + // Check if P in vertex region outside A + float ab[3], ac[3], ap[3]; + dtVsub(ab, b, a); + dtVsub(ac, c, a); + dtVsub(ap, p, a); + float d1 = dtVdot(ab, ap); + float d2 = dtVdot(ac, ap); + if (d1 <= 0.0f && d2 <= 0.0f) + { + // barycentric coordinates (1,0,0) + dtVcopy(closest, a); + return; + } + + // Check if P in vertex region outside B + float bp[3]; + dtVsub(bp, p, b); + float d3 = dtVdot(ab, bp); + float d4 = dtVdot(ac, bp); + if (d3 >= 0.0f && d4 <= d3) + { + // barycentric coordinates (0,1,0) + dtVcopy(closest, b); + return; + } + + // Check if P in edge region of AB, if so return projection of P onto AB + float vc = d1*d4 - d3*d2; + if (vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f) + { + // barycentric coordinates (1-v,v,0) + float v = d1 / (d1 - d3); + closest[0] = a[0] + v * ab[0]; + closest[1] = a[1] + v * ab[1]; + closest[2] = a[2] + v * ab[2]; + return; + } + + // Check if P in vertex region outside C + float cp[3]; + dtVsub(cp, p, c); + float d5 = dtVdot(ab, cp); + float d6 = dtVdot(ac, cp); + if (d6 >= 0.0f && d5 <= d6) + { + // barycentric coordinates (0,0,1) + dtVcopy(closest, c); + return; + } + + // Check if P in edge region of AC, if so return projection of P onto AC + float vb = d5*d2 - d1*d6; + if (vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f) + { + // barycentric coordinates (1-w,0,w) + float w = d2 / (d2 - d6); + closest[0] = a[0] + w * ac[0]; + closest[1] = a[1] + w * ac[1]; + closest[2] = a[2] + w * ac[2]; + return; + } + + // Check if P in edge region of BC, if so return projection of P onto BC + float va = d3*d6 - d5*d4; + if (va <= 0.0f && (d4 - d3) >= 0.0f && (d5 - d6) >= 0.0f) + { + // barycentric coordinates (0,1-w,w) + float w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); + closest[0] = b[0] + w * (c[0] - b[0]); + closest[1] = b[1] + w * (c[1] - b[1]); + closest[2] = b[2] + w * (c[2] - b[2]); + return; + } + + // P inside face region. Compute Q through its barycentric coordinates (u,v,w) + float denom = 1.0f / (va + vb + vc); + float v = vb * denom; + float w = vc * denom; + closest[0] = a[0] + ab[0] * v + ac[0] * w; + closest[1] = a[1] + ab[1] * v + ac[1] * w; + closest[2] = a[2] + ab[2] * v + ac[2] * w; +} + +bool dtIntersectSegmentPoly2D(const float* p0, const float* p1, + const float* verts, int nverts, + float& tmin, float& tmax, + int& segMin, int& segMax) +{ + static const float EPS = 0.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; +} + +void dtCalcPolyCenter(float* tc, const unsigned short* idx, int nidx, const float* verts) +{ + tc[0] = 0.0f; + tc[1] = 0.0f; + tc[2] = 0.0f; + for (int j = 0; j < nidx; ++j) + { + const float* v = &verts[idx[j]*3]; + tc[0] += v[0]; + tc[1] += v[1]; + tc[2] += v[2]; + } + const float s = 1.0f / nidx; + tc[0] *= s; + tc[1] *= s; + tc[2] *= s; +} + +bool dtClosestHeightPointTriangle(const float* p, const float* a, const float* b, const float* c, float& h) +{ + 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 +/// +/// 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; +} + +bool dtDistancePtPolyEdgesSqr(const float* pt, const float* verts, const int nverts, + float* ed, float* et) +{ + // TODO: Replace pnpoly with triArea2D tests? + int i, j; + bool c = false; + for (i = 0, j = nverts-1; i < nverts; j = i++) + { + const float* vi = &verts[i*3]; + const float* vj = &verts[j*3]; + if (((vi[2] > pt[2]) != (vj[2] > pt[2])) && + (pt[0] < (vj[0]-vi[0]) * (pt[2]-vi[2]) / (vj[2]-vi[2]) + vi[0]) ) + c = !c; + ed[j] = dtDistancePtSegSqr2D(pt, vj, vi, et[j]); + } + return c; +} + +static void projectPoly(const float* axis, const float* poly, const int npoly, + float& rmin, float& rmax) +{ + rmin = rmax = dtVdot2D(axis, &poly[0]); + for (int i = 1; i < npoly; ++i) + { + const float d = dtVdot2D(axis, &poly[i*3]); + rmin = dtMin(rmin, d); + rmax = dtMax(rmax, d); + } +} + +inline bool overlapRange(const float amin, const float amax, + const float bmin, const float bmax, + const float eps) +{ + return ((amin+eps) > bmax || (amax-eps) < bmin) ? false : true; +} + +/// @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 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) +{ + // 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]; } + +bool dtIntersectSegSeg2D(const float* ap, const float* aq, + 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; +} + diff --git a/Dependencies/recastnavigation/Detour/Source/DetourNavMesh.cpp b/Dependencies/recastnavigation/Detour/Source/DetourNavMesh.cpp new file mode 100644 index 000000000..2240526eb --- /dev/null +++ b/Dependencies/recastnavigation/Detour/Source/DetourNavMesh.cpp @@ -0,0 +1,1591 @@ +// +// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#include +#include +#include +#include "DetourNavMesh.h" +#include "DetourNode.h" +#include "DetourCommon.h" +#include "DetourMath.h" +#include "DetourAlloc.h" +#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) +{ + // 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; +} + +static void calcSlabEndPoints(const float* va, const float* vb, float* bmin, float* bmax, const int side) +{ + if (side == 0 || side == 4) + { + if (va[2] < vb[2]) + { + bmin[0] = va[2]; + bmin[1] = va[1]; + bmax[0] = vb[2]; + bmax[1] = vb[1]; + } + else + { + bmin[0] = vb[2]; + bmin[1] = vb[1]; + bmax[0] = va[2]; + bmax[1] = va[1]; + } + } + else if (side == 2 || side == 6) + { + if (va[0] < vb[0]) + { + bmin[0] = va[0]; + bmin[1] = va[1]; + bmax[0] = vb[0]; + bmax[1] = vb[1]; + } + else + { + bmin[0] = vb[0]; + bmin[1] = vb[1]; + bmax[0] = va[0]; + bmax[1] = va[1]; + } + } +} + +inline int computeTileHash(int x, int y, const int mask) +{ + const unsigned int h1 = 0x8da6b343; // Large multiplicative constants; + const unsigned int h2 = 0xd8163841; // here arbitrarily chosen primes + unsigned int n = h1 * x + h2 * y; + return (int)(n & mask); +} + +inline unsigned int allocLink(dtMeshTile* tile) +{ + if (tile->linksFreeList == DT_NULL_LINK) + return DT_NULL_LINK; + unsigned int link = tile->linksFreeList; + tile->linksFreeList = tile->links[link].next; + return link; +} + +inline void freeLink(dtMeshTile* tile, unsigned int link) +{ + tile->links[link].next = tile->linksFreeList; + tile->linksFreeList = link; +} + + +dtNavMesh* dtAllocNavMesh() +{ + void* mem = dtAlloc(sizeof(dtNavMesh), DT_ALLOC_PERM); + if (!mem) return 0; + return new(mem) dtNavMesh; +} + +/// @par +/// +/// This function will only free the memory for tiles with the #DT_TILE_FREE_DATA +/// flag set. +void dtFreeNavMesh(dtNavMesh* navmesh) +{ + if (!navmesh) return; + navmesh->~dtNavMesh(); + dtFree(navmesh); +} + +////////////////////////////////////////////////////////////////////////////////////////// + +/** +@class dtNavMesh + +The navigation mesh consists of one or more tiles defining three primary types of structural data: + +A polygon mesh which defines most of the navigation graph. (See rcPolyMesh for its structure.) +A detail mesh used for determining surface height on the polygon mesh. (See rcPolyMeshDetail for its structure.) +Off-mesh connections, which define custom point-to-point edges within the navigation graph. + +The general build process is as follows: + +-# Create rcPolyMesh and rcPolyMeshDetail data using the Recast build pipeline. +-# Optionally, create off-mesh connection data. +-# Combine the source data into a dtNavMeshCreateParams structure. +-# Create a tile data array using dtCreateNavMeshData(). +-# Allocate at dtNavMesh object and initialize it. (For single tile navigation meshes, + the tile data is loaded during this step.) +-# For multi-tile navigation meshes, load the tile data using dtNavMesh::addTile(). + +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 + to have only a single tile. +- 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) +{ +#ifndef DT_POLYREF64 + 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; +} + +dtNavMesh::~dtNavMesh() +{ + for (int i = 0; i < m_maxTiles; ++i) + { + if (m_tiles[i].flags & DT_TILE_FREE_DATA) + { + dtFree(m_tiles[i].data); + m_tiles[i].data = 0; + m_tiles[i].dataSize = 0; + } + } + dtFree(m_posLookup); + dtFree(m_tiles); +} + +dtStatus dtNavMesh::init(const dtNavMeshParams* params) +{ + memcpy(&m_params, params, sizeof(dtNavMeshParams)); + dtVcopy(m_orig, params->orig); + m_tileWidth = params->tileWidth; + m_tileHeight = params->tileHeight; + + // Init tiles + m_maxTiles = params->maxTiles; + m_tileLutSize = dtNextPow2(params->maxTiles/4); + if (!m_tileLutSize) m_tileLutSize = 1; + m_tileLutMask = m_tileLutSize-1; + + m_tiles = (dtMeshTile*)dtAlloc(sizeof(dtMeshTile)*m_maxTiles, DT_ALLOC_PERM); + if (!m_tiles) + return DT_FAILURE | 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); + + if (m_saltBits < 10) + return DT_FAILURE | DT_INVALID_PARAM; +#endif + + 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); +} + +/// @par +/// +/// @note The parameters are created automatically when the single tile +/// initialization is performed. +const dtNavMeshParams* dtNavMesh::getParams() const +{ + return &m_params; +} + +////////////////////////////////////////////////////////////////////////////////////////// +int dtNavMesh::findConnectingPolys(const float* va, const float* vb, + const dtMeshTile* tile, int side, + dtPolyRef* con, float* conarea, int maxcon) const +{ + if (!tile) return 0; + + float amin[2], amax[2]; + calcSlabEndPoints(va, vb, amin, amax, side); + 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; + } + } + } +} + +void dtNavMesh::connectExtLinks(dtMeshTile* tile, dtMeshTile* target, int side) +{ + if (!tile) return; + + // Connect border links. + for (int i = 0; i < tile->header->polyCount; ++i) + { + dtPoly* poly = &tile->polys[i]; + + // Create new links. +// unsigned short m = DT_EXT_LINK | (unsigned short)side; + + const int nv = poly->vertCount; + for (int j = 0; j < nv; ++j) + { + // Skip 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; + } + } + } + +} + +void dtNavMesh::connectIntLinks(dtMeshTile* tile) +{ + if (!tile) return; + + dtPolyRef base = getPolyRefBase(tile); + + for (int i = 0; i < tile->header->polyCount; ++i) + { + dtPoly* poly = &tile->polys[i]; + poly->firstLink = DT_NULL_LINK; + + if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) + continue; + + // Build edge links backwards so that the links will be + // in the linked list from lowest index to highest. + for (int j = poly->vertCount-1; j >= 0; --j) + { + // Skip hard and non-internal edges. + if (poly->neis[j] == 0 || (poly->neis[j] & DT_EXT_LINK)) continue; + + unsigned int idx = allocLink(tile); + if (idx != DT_NULL_LINK) + { + dtLink* link = &tile->links[idx]; + link->ref = base | (dtPolyRef)(poly->neis[j]-1); + link->edge = (unsigned char)j; + link->side = 0xff; + link->bmin = link->bmax = 0; + // Add to linked list. + link->next = poly->firstLink; + poly->firstLink = idx; + } + } + } +} + +void dtNavMesh::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; + } + } +} + +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); + } +} + +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; +} + +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); +} + +dtPolyRef dtNavMesh::findNearestPolyInTile(const dtMeshTile* tile, + 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; +} + +int dtNavMesh::queryPolygonsInTile(const dtMeshTile* tile, const float* qmin, const float* qmax, + dtPolyRef* polys, const int maxPolys) const +{ + if (tile->bvTree) + { + const dtBVNode* node = &tile->bvTree[0]; + const dtBVNode* end = &tile->bvTree[tile->header->bvNodeCount]; + const float* tbmin = tile->header->bmin; + const float* tbmax = tile->header->bmax; + const float qfac = tile->header->bvQuantFactor; + + // Calculate quantized box + unsigned short bmin[3], bmax[3]; + // dtClamp query box to world box. + float minx = dtClamp(qmin[0], tbmin[0], tbmax[0]) - tbmin[0]; + float miny = dtClamp(qmin[1], tbmin[1], tbmax[1]) - tbmin[1]; + float minz = dtClamp(qmin[2], tbmin[2], tbmax[2]) - tbmin[2]; + float maxx = dtClamp(qmax[0], tbmin[0], tbmax[0]) - tbmin[0]; + float maxy = dtClamp(qmax[1], tbmin[1], tbmax[1]) - tbmin[1]; + float maxz = dtClamp(qmax[2], tbmin[2], tbmax[2]) - tbmin[2]; + // Quantize + bmin[0] = (unsigned short)(qfac * minx) & 0xfffe; + bmin[1] = (unsigned short)(qfac * miny) & 0xfffe; + bmin[2] = (unsigned short)(qfac * minz) & 0xfffe; + bmax[0] = (unsigned short)(qfac * maxx + 1) | 1; + bmax[1] = (unsigned short)(qfac * maxy + 1) | 1; + bmax[2] = (unsigned short)(qfac * maxz + 1) | 1; + + // Traverse tree + dtPolyRef base = getPolyRefBase(tile); + int n = 0; + while (node < end) + { + const bool overlap = dtOverlapQuantBounds(bmin, bmax, node->bmin, node->bmax); + const bool isLeafNode = node->i >= 0; + + if (isLeafNode && overlap) + { + if (n < maxPolys) + polys[n++] = base | (dtPolyRef)node->i; + } + + if (overlap || isLeafNode) + node++; + else + { + const int escapeIndex = -node->i; + node += escapeIndex; + } + } + + return n; + } + else + { + float bmin[3], bmax[3]; + int n = 0; + dtPolyRef base = getPolyRefBase(tile); + for (int i = 0; i < tile->header->polyCount; ++i) + { + dtPoly* p = &tile->polys[i]; + // Do not return off-mesh connection polygons. + if (p->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) + continue; + // Calc polygon bounds. + const float* v = &tile->verts[p->verts[0]*3]; + dtVcopy(bmin, v); + dtVcopy(bmax, v); + for (int j = 1; j < p->vertCount; ++j) + { + v = &tile->verts[p->verts[j]*3]; + dtVmin(bmin, v); + dtVmax(bmax, v); + } + if (dtOverlapBounds(qmin,qmax, bmin,bmax)) + { + if (n < maxPolys) + polys[n++] = base | (dtPolyRef)i; + } + } + return n; + } +} + +/// @par +/// +/// The add operation will fail if the data is in the wrong format, the allocated tile +/// space is full, or there is a tile already at the specified reference. +/// +/// 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 +/// removed. +/// +/// The nav mesh assumes exclusive access to the data passed and will make +/// changes to the dynamic portion of the data. For that reason the data +/// should not be reused in other nav meshes until the tile has been successfully +/// removed from this nav mesh. +/// +/// @see dtCreateNavMeshData, #removeTile +dtStatus dtNavMesh::addTile(unsigned char* data, int dataSize, int flags, + dtTileRef lastRef, dtTileRef* result) +{ + // Make sure the data is in right format. + dtMeshHeader* header = (dtMeshHeader*)data; + if (header->magic != DT_NAVMESH_MAGIC) + return DT_FAILURE | 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; +} + +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 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; +} + +/// @par +/// +/// This function will not fail if the tiles array is too small to hold the +/// 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; +} + + +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; +} + +const dtMeshTile* dtNavMesh::getTileByRef(dtTileRef ref) const +{ + if (!ref) + return 0; + unsigned int tileIndex = decodePolyIdTile((dtPolyRef)ref); + unsigned int tileSalt = decodePolyIdSalt((dtPolyRef)ref); + if ((int)tileIndex >= m_maxTiles) + return 0; + const dtMeshTile* tile = &m_tiles[tileIndex]; + if (tile->salt != tileSalt) + return 0; + return tile; +} + +int dtNavMesh::getMaxTiles() const +{ + return m_maxTiles; +} + +dtMeshTile* dtNavMesh::getTile(int i) +{ + return &m_tiles[i]; +} + +const dtMeshTile* dtNavMesh::getTile(int i) const +{ + return &m_tiles[i]; +} + +void dtNavMesh::calcTileLoc(const float* pos, int* tx, int* ty) const +{ + *tx = (int)floorf((pos[0]-m_orig[0]) / m_tileWidth); + *ty = (int)floorf((pos[2]-m_orig[2]) / m_tileHeight); +} + +dtStatus dtNavMesh::getTileAndPolyByRef(const dtPolyRef ref, const dtMeshTile** tile, const dtPoly** poly) const +{ + 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 +/// +/// @warning Only use this function if it is known that the provided polygon +/// reference is valid. This function is faster than #getTileAndPolyByRef, but +/// 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]; +} + +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; +} + +/// @par +/// +/// This function returns the data for the tile so that, if desired, +/// it can be added back to the navigation mesh at a later point. +/// +/// @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. +#ifdef DT_POLYREF64 + tile->salt = (tile->salt+1) & ((1<salt = (tile->salt+1) & ((1<salt == 0) + tile->salt++; + + // Add to free list. + tile->next = m_nextFree; + m_nextFree = tile; + + return DT_SUCCESS; +} + +dtTileRef dtNavMesh::getTileRef(const dtMeshTile* tile) const +{ + if (!tile) return 0; + const unsigned int it = (unsigned int)(tile - m_tiles); + return (dtTileRef)encodePolyId(tile->salt, it, 0); +} + +/// @par +/// +/// Example use case: +/// @code +/// +/// const dtPolyRef base = navmesh->getPolyRefBase(tile); +/// for (int i = 0; i < tile->header->polyCount; ++i) +/// { +/// 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); +} + +struct dtTileState +{ + int magic; // Magic number, used to identify the data. + int version; // Data version number. + dtTileRef ref; // Tile ref at the time of storing the data. +}; + +struct dtPolyState +{ + unsigned short flags; // Flags (see dtPolyFlags). + unsigned char area; // Area ID of the polygon. +}; + +/// @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; +} + +/// @par +/// +/// Tile state includes non-structural data such as polygon flags, area ids, etc. +/// @note The state data is only valid until the tile reference changes. +/// @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; +} + +/// @par +/// +/// Tile state includes non-structural data such as polygon flags, area ids, etc. +/// @note This function does not impact the tile's #dtTileRef and #dtPolyRef's. +/// @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; +} + +/// @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 +/// 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; +} + + +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]; +} + + +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; +} + +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; +} + +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; +} + +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; +} + diff --git a/Dependencies/recastnavigation/Detour/Source/DetourNavMeshBuilder.cpp b/Dependencies/recastnavigation/Detour/Source/DetourNavMeshBuilder.cpp new file mode 100644 index 000000000..2c433705e --- /dev/null +++ b/Dependencies/recastnavigation/Detour/Source/DetourNavMeshBuilder.cpp @@ -0,0 +1,802 @@ +// +// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#include +#include +#include +#include +#include "DetourNavMesh.h" +#include "DetourCommon.h" +#include "DetourMath.h" +#include "DetourNavMeshBuilder.h" +#include "DetourAlloc.h" +#include "DetourAssert.h" + +static unsigned short MESH_NULL_IDX = 0xffff; + + +struct BVItem +{ + unsigned short bmin[3]; + unsigned short bmax[3]; + int i; +}; + +static int compareItemX(const void* va, const void* vb) +{ + const BVItem* a = (const BVItem*)va; + const BVItem* b = (const BVItem*)vb; + if (a->bmin[0] < b->bmin[0]) + return -1; + if (a->bmin[0] > b->bmin[0]) + return 1; + return 0; +} + +static int compareItemY(const void* va, const void* vb) +{ + const BVItem* a = (const BVItem*)va; + const BVItem* b = (const BVItem*)vb; + if (a->bmin[1] < b->bmin[1]) + return -1; + if (a->bmin[1] > b->bmin[1]) + return 1; + return 0; +} + +static int compareItemZ(const void* va, const void* vb) +{ + const BVItem* a = (const BVItem*)va; + const BVItem* b = (const BVItem*)vb; + if (a->bmin[2] < b->bmin[2]) + return -1; + if (a->bmin[2] > b->bmin[2]) + return 1; + return 0; +} + +static void calcExtends(BVItem* items, const int /*nitems*/, const int imin, const int imax, + unsigned short* bmin, unsigned short* bmax) +{ + bmin[0] = items[imin].bmin[0]; + bmin[1] = items[imin].bmin[1]; + bmin[2] = items[imin].bmin[2]; + + bmax[0] = items[imin].bmax[0]; + bmax[1] = items[imin].bmax[1]; + bmax[2] = items[imin].bmax[2]; + + for (int i = imin+1; i < imax; ++i) + { + const BVItem& it = items[i]; + if (it.bmin[0] < bmin[0]) bmin[0] = it.bmin[0]; + if (it.bmin[1] < bmin[1]) bmin[1] = it.bmin[1]; + if (it.bmin[2] < bmin[2]) bmin[2] = it.bmin[2]; + + if (it.bmax[0] > bmax[0]) bmax[0] = it.bmax[0]; + if (it.bmax[1] > bmax[1]) bmax[1] = it.bmax[1]; + if (it.bmax[2] > bmax[2]) bmax[2] = it.bmax[2]; + } +} + +inline int longestAxis(unsigned short x, unsigned short y, unsigned short z) +{ + int axis = 0; + unsigned short maxVal = x; + if (y > maxVal) + { + axis = 1; + maxVal = y; + } + if (z > maxVal) + { + axis = 2; + } + return axis; +} + +static void subdivide(BVItem* items, int nitems, int imin, int imax, int& curNode, dtBVNode* nodes) +{ + int inum = imax - imin; + int icur = curNode; + + dtBVNode& node = nodes[curNode++]; + + if (inum == 1) + { + // Leaf + node.bmin[0] = items[imin].bmin[0]; + node.bmin[1] = items[imin].bmin[1]; + node.bmin[2] = items[imin].bmin[2]; + + node.bmax[0] = items[imin].bmax[0]; + node.bmax[1] = items[imin].bmax[1]; + node.bmax[2] = items[imin].bmax[2]; + + node.i = items[imin].i; + } + else + { + // Split + calcExtends(items, nitems, imin, imax, node.bmin, node.bmax); + + int axis = longestAxis(node.bmax[0] - node.bmin[0], + node.bmax[1] - node.bmin[1], + node.bmax[2] - node.bmin[2]); + + if (axis == 0) + { + // Sort along x-axis + qsort(items+imin, inum, sizeof(BVItem), compareItemX); + } + else if (axis == 1) + { + // Sort along y-axis + qsort(items+imin, inum, sizeof(BVItem), compareItemY); + } + else + { + // Sort along z-axis + qsort(items+imin, inum, sizeof(BVItem), compareItemZ); + } + + int isplit = imin+inum/2; + + // Left + subdivide(items, nitems, imin, isplit, curNode, nodes); + // Right + subdivide(items, nitems, isplit, imax, curNode, nodes); + + int iescape = curNode - icur; + // Negative index means escape. + node.i = -iescape; + } +} + +static int createBVTree(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; +} + +static unsigned char classifyOffMeshPoint(const float* pt, const float* bmin, const float* bmax) +{ + static const unsigned char XP = 1<<0; + static const unsigned char ZP = 1<<1; + static const unsigned char XM = 1<<2; + static const unsigned char ZM = 1<<3; + + unsigned char outcode = 0; + outcode |= (pt[0] >= bmax[0]) ? XP : 0; + outcode |= (pt[2] >= bmax[2]) ? ZP : 0; + outcode |= (pt[0] < bmin[0]) ? XM : 0; + outcode |= (pt[2] < bmin[2]) ? ZM : 0; + + switch (outcode) + { + case XP: return 0; + case XP|ZP: return 1; + case ZP: return 2; + case XM|ZP: return 3; + case XM: return 4; + case XM|ZM: return 5; + case ZM: return 6; + case XP|ZM: return 7; + }; + + return 0xff; +} + +// TODO: Better error handling. + +/// @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. +/// +/// @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 connections 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; +} + +/// @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 +/// 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; +} diff --git a/Dependencies/recastnavigation/Detour/Source/DetourNavMeshQuery.cpp b/Dependencies/recastnavigation/Detour/Source/DetourNavMeshQuery.cpp new file mode 100644 index 000000000..9105b3498 --- /dev/null +++ b/Dependencies/recastnavigation/Detour/Source/DetourNavMeshQuery.cpp @@ -0,0 +1,3690 @@ +// +// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#include +#include +#include "DetourNavMeshQuery.h" +#include "DetourNavMesh.h" +#include "DetourNode.h" +#include "DetourCommon.h" +#include "DetourMath.h" +#include "DetourAlloc.h" +#include "DetourAssert.h" +#include + +/// @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 +/// 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 +/// your own objects where possible. +/// +/// 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 +/// to lead to problems during pathfinding. +/// +/// @see dtNavMeshQuery + +dtQueryFilter::dtQueryFilter() : + m_includeFlags(0xffff), + m_excludeFlags(0) +{ + for (int i = 0; i < DT_MAX_AREAS; ++i) + m_areaCost[i] = 1.0f; +} + +#ifdef DT_VIRTUAL_QUERYFILTER +bool dtQueryFilter::passFilter(const dtPolyRef /*ref*/, + const dtMeshTile* /*tile*/, + const dtPoly* poly) const +{ + return (poly->flags & m_includeFlags) != 0 && (poly->flags & m_excludeFlags) == 0; +} + +float dtQueryFilter::getCost(const float* pa, const float* pb, + const dtPolyRef /*prevRef*/, const dtMeshTile* /*prevTile*/, const dtPoly* /*prevPoly*/, + const dtPolyRef /*curRef*/, const dtMeshTile* /*curTile*/, const dtPoly* curPoly, + const dtPolyRef /*nextRef*/, const dtMeshTile* /*nextTile*/, const dtPoly* /*nextPoly*/) const +{ + return dtVdist(pa, pb) * m_areaCost[curPoly->getArea()]; +} +#else +inline bool dtQueryFilter::passFilter(const dtPolyRef /*ref*/, + const dtMeshTile* /*tile*/, + const dtPoly* poly) const +{ + return (poly->flags & m_includeFlags) != 0 && (poly->flags & m_excludeFlags) == 0; +} + +inline float dtQueryFilter::getCost(const float* pa, const float* pb, + const dtPolyRef /*prevRef*/, const dtMeshTile* /*prevTile*/, const dtPoly* /*prevPoly*/, + const dtPolyRef /*curRef*/, const dtMeshTile* /*curTile*/, const dtPoly* curPoly, + const dtPolyRef /*nextRef*/, const dtMeshTile* /*nextTile*/, const dtPoly* /*nextPoly*/) const +{ + return dtVdist(pa, pb) * m_areaCost[curPoly->getArea()]; +} +#endif + +static const float H_SCALE = 0.999f; // Search heuristic scale. + + +dtNavMeshQuery* dtAllocNavMeshQuery() +{ + void* mem = dtAlloc(sizeof(dtNavMeshQuery), DT_ALLOC_PERM); + if (!mem) return 0; + return new(mem) dtNavMeshQuery; +} + +void dtFreeNavMeshQuery(dtNavMeshQuery* navmesh) +{ + if (!navmesh) return; + navmesh->~dtNavMeshQuery(); + dtFree(navmesh); +} + +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 +/// 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 +/// 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) +{ + 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); +} + +/// @par +/// +/// Must be the first function called after construction, before other +/// functions are used. +/// +/// 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; +} + +dtStatus dtNavMeshQuery::findRandomPoint(const dtQueryFilter* filter, float (*frand)(), + 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 reservoir 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 reservoir 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 +{ + 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 checked 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 reservoir 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 +/// +/// Uses the detail polygons to find the surface height. (Most accurate.) +/// +/// @p pos does not have to be within the bounds of the polygon or navigation mesh. +/// +/// See closestPointOnPolyBoundary() for a limited but faster option. +/// +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; +} + +/// @par +/// +/// Much faster than closestPointOnPoly(). +/// +/// 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; +} + +/// @par +/// +/// 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; +} + +class dtFindNearestPolyQuery : public dtPolyQuery +{ + 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(), 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; + } + } + } +}; + +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 +/// @p nearestRef before using @p nearestPt. +/// +dtStatus dtNavMeshQuery::findNearestPoly(const float* center, const float* halfExtents, + const dtQueryFilter* filter, + dtPolyRef* nearestRef, float* nearestPt) const +{ + return findNearestPoly(center, halfExtents, filter, nearestRef, nearestPt, NULL); +} + +// 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 +{ + 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; + +public: + 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; + } +}; + +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 +/// 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 +{ + if (!polys || !polyCount || maxPolys < 0) + return DT_FAILURE | DT_INVALID_PARAM; + + dtCollectPolysQuery collector(polys, maxPolys); + + 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; +} + +/// @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 +/// passed to this function. The dtPolyQuery::process function is invoked multiple +/// times until all overlapping polygons have been processed. +/// +dtStatus dtNavMeshQuery::queryPolygons(const float* center, const float* halfExtents, + 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; +} + +/// @par +/// +/// 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 +/// far as possible from the start polygon toward the end polygon. +/// +/// 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 +{ + 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 checked 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 checked 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; +} + + +/// @par +/// +/// @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) +{ + 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 checked 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 checked 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; +} + +dtStatus dtNavMeshQuery::finalizeSlicedFindPathPartial(const dtPolyRef* existing, const int existingSize, + 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; +} + + +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 +{ + 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 +{ + 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 +/// 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 +/// 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 +/// 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 +{ + 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 +/// incomplete path. +/// +/// @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 +/// 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 +/// 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 +/// 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 +{ + 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 checked 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; +} + + +dtStatus dtNavMeshQuery::getPortalPoints(dtPolyRef from, dtPolyRef to, float* left, float* right, + unsigned char& fromType, unsigned char& toType) const +{ + dtAssert(m_nav); + + const dtMeshTile* fromTile = 0; + const dtPoly* fromPoly = 0; + if (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 +{ + // 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; +} + +dtStatus dtNavMeshQuery::getEdgeMidPoint(dtPolyRef from, const dtPoly* fromPoly, const dtMeshTile* fromTile, + dtPolyRef to, const dtPoly* toPoly, const dtMeshTile* toTile, + float* mid) const +{ + float left[3], right[3]; + if (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 +/// 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 +/// 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 +/// was hit and the value of @p hitNormal is undefined. +/// +/// If 0 < t < 1.0 then the following applies: +/// +/// @code +/// distanceToHitBorder = distanceToEndPosition * t +/// hitPoint = startPos + (endPos - startPos) * t +/// @endcode +/// +/// Use Case Restriction +/// +/// 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 +/// position is on the balcony. +/// +/// 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 +{ + 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 +/// 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 +/// 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 +/// was hit and the value of @p hitNormal is undefined. +/// +/// If 0 < t < 1.0 then the following applies: +/// +/// @code +/// distanceToHitBorder = distanceToEndPosition * t +/// hitPoint = startPos + (endPos - startPos) * t +/// @endcode +/// +/// Use Case Restriction +/// +/// 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 +/// position is on the balcony. +/// +/// 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 +{ + 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 +/// +/// At least one result array must be provided. +/// +/// 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. +/// 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 +/// 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 +/// 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 +/// 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 +/// effect intersection tests. +/// +/// 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 +{ + 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 checked 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 +/// 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 +/// calculations. +/// +/// 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 +/// 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 +{ + 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 checked 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; + + *pathCount = 0; + + 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); +} + +/// @par +/// +/// 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 +/// the start polygon. +/// +/// 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 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 +/// effect intersection tests. +/// +/// 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 +{ + 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 checked 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; +}; + +static void insertInterval(dtSegInterval* ints, int& nints, const int maxInts, + 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++; +} + +/// @par +/// +/// 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 +/// 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 +/// 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 +{ + 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 +/// 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 +{ + 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 checked 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; +} + +/// @par +/// +/// 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; iflags & DT_NODE_CLOSED) + return true; + } + + return false; +} diff --git a/Dependencies/recastnavigation/Detour/Source/DetourNode.cpp b/Dependencies/recastnavigation/Detour/Source/DetourNode.cpp new file mode 100644 index 000000000..48abbba6b --- /dev/null +++ b/Dependencies/recastnavigation/Detour/Source/DetourNode.cpp @@ -0,0 +1,200 @@ +// +// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#include "DetourNode.h" +#include "DetourAlloc.h" +#include "DetourAssert.h" +#include "DetourCommon.h" +#include + +#ifdef DT_POLYREF64 +// 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; +} +#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; +} +#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) +{ + 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); + + dtAssert(m_nodes); + dtAssert(m_next); + dtAssert(m_first); + + memset(m_first, 0xff, sizeof(dtNodeIndex)*m_hashSize); + memset(m_next, 0xff, sizeof(dtNodeIndex)*m_maxNodes); +} + +dtNodePool::~dtNodePool() +{ + dtFree(m_nodes); + dtFree(m_next); + dtFree(m_first); +} + +void dtNodePool::clear() +{ + memset(m_first, 0xff, sizeof(dtNodeIndex)*m_hashSize); + m_nodeCount = 0; +} + +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; +} + +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; +} + +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; +} + + +////////////////////////////////////////////////////////////////////////////////////////// +dtNodeQueue::dtNodeQueue(int n) : + m_heap(0), + m_capacity(n), + m_size(0) +{ + dtAssert(m_capacity > 0); + + m_heap = (dtNode**)dtAlloc(sizeof(dtNode*)*(m_capacity+1), DT_ALLOC_PERM); + dtAssert(m_heap); +} + +dtNodeQueue::~dtNodeQueue() +{ + dtFree(m_heap); +} + +void dtNodeQueue::bubbleUp(int i, dtNode* node) +{ + int parent = (i-1)/2; + // note: (index > 0) means there is a parent + while ((i > 0) && (m_heap[parent]->total > node->total)) + { + m_heap[i] = m_heap[parent]; + i = parent; + parent = (i-1)/2; + } + m_heap[i] = node; +} + +void dtNodeQueue::trickleDown(int i, dtNode* node) +{ + int child = (i*2)+1; + while (child < m_size) + { + if (((child+1) < m_size) && + (m_heap[child]->total > m_heap[child+1]->total)) + { + child++; + } + m_heap[i] = m_heap[child]; + i = child; + child = (i*2)+1; + } + bubbleUp(i, node); +} diff --git a/Dependencies/recastnavigation/DetourCrowd/CMakeLists.txt b/Dependencies/recastnavigation/DetourCrowd/CMakeLists.txt new file mode 100644 index 000000000..214cb4ced --- /dev/null +++ b/Dependencies/recastnavigation/DetourCrowd/CMakeLists.txt @@ -0,0 +1,37 @@ +file(GLOB SOURCES Source/*.cpp) +add_library(DetourCrowd ${SOURCES}) + +add_library(RecastNavigation::DetourCrowd ALIAS DetourCrowd) +set_target_properties(DetourCrowd PROPERTIES DEBUG_POSTFIX -d) + +set(DetourCrowd_INCLUDE_DIR "${CMAKE_CURRENT_SOURCE_DIR}/Include") + +target_include_directories(DetourCrowd PUBLIC + "$" +) + +target_link_libraries(DetourCrowd + Detour +) + +set_target_properties(DetourCrowd PROPERTIES + SOVERSION ${SOVERSION} + VERSION ${LIB_VERSION} + COMPILE_PDB_OUTPUT_DIRECTORY . + COMPILE_PDB_NAME "DetourCrowd-d" + ) + +install(TARGETS DetourCrowd + EXPORT recastnavigation-targets + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} COMPONENT library + INCLUDES DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} ${CMAKE_INSTALL_INCLUDEDIR}/recastnavigation + ) + +file(GLOB INCLUDES Include/*.h) +install(FILES ${INCLUDES} DESTINATION + ${CMAKE_INSTALL_INCLUDEDIR}/recastnavigation) +if(MSVC) + install(FILES "$/DetourCrowd-d.pdb" CONFIGURATIONS "Debug" DESTINATION "lib" OPTIONAL) +endif() diff --git a/Dependencies/recastnavigation/DetourCrowd/Include/DetourCrowd.h b/Dependencies/recastnavigation/DetourCrowd/Include/DetourCrowd.h new file mode 100644 index 000000000..6360dfb3e --- /dev/null +++ b/Dependencies/recastnavigation/DetourCrowd/Include/DetourCrowd.h @@ -0,0 +1,460 @@ +// +// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#ifndef DETOURCROWD_H +#define DETOURCROWD_H + +#include "DetourNavMeshQuery.h" +#include "DetourObstacleAvoidance.h" +#include "DetourLocalBoundary.h" +#include "DetourPathCorridor.h" +#include "DetourProximityGrid.h" +#include "DetourPathQueue.h" + +/// The maximum number of neighbors that a crowd agent can take into account +/// for steering decisions. +/// @ingroup crowd +static const int DT_CROWDAGENT_MAX_NEIGHBOURS = 6; + +/// The maximum number of corners a crowd agent will look ahead in the path. +/// This value is used for sizing the crowd agent corner buffers. +/// Due to the behavior of the crowd manager, the actual number of useful +/// corners will be one less than this number. +/// @ingroup crowd +static const int DT_CROWDAGENT_MAX_CORNERS = 4; + +/// The maximum number of crowd avoidance configurations supported by the +/// crowd manager. +/// @ingroup crowd +/// @see dtObstacleAvoidanceParams, dtCrowd::setObstacleAvoidanceParams(), dtCrowd::getObstacleAvoidanceParams(), +/// dtCrowdAgentParams::obstacleAvoidanceType +static const int DT_CROWD_MAX_OBSTAVOIDANCE_PARAMS = 8; + +/// The maximum number of query filter types supported by the crowd manager. +/// @ingroup crowd +/// @see dtQueryFilter, dtCrowd::getFilter() dtCrowd::getEditableFilter(), +/// dtCrowdAgentParams::queryFilterType +static const int DT_CROWD_MAX_QUERY_FILTER_TYPE = 16; + +/// Provides neighbor data for agents managed by the crowd. +/// @ingroup crowd +/// @see dtCrowdAgent::neis, dtCrowd +struct dtCrowdNeighbour +{ + int idx; ///< The index of the neighbor in the crowd. + float dist; ///< The distance between the current agent and the neighbor. +}; + +/// The type of navigation mesh polygon the agent is currently traversing. +/// @ingroup crowd +enum CrowdAgentState +{ + DT_CROWDAGENT_STATE_INVALID, ///< The agent is not in a valid state. + DT_CROWDAGENT_STATE_WALKING, ///< The agent is traversing a normal navigation mesh polygon. + DT_CROWDAGENT_STATE_OFFMESH ///< The agent is traversing an off-mesh connection. +}; + +/// Configuration parameters for a crowd agent. +/// @ingroup crowd +struct dtCrowdAgentParams +{ + float radius; ///< Agent radius. [Limit: >= 0] + float height; ///< Agent height. [Limit: > 0] + float maxAcceleration; ///< Maximum allowed acceleration. [Limit: >= 0] + float maxSpeed; ///< Maximum allowed speed. [Limit: >= 0] + + /// Defines how close a collision element must be before it is considered for steering behaviors. [Limits: > 0] + float collisionQueryRange; + + float pathOptimizationRange; ///< The path visibility optimization range. [Limit: > 0] + + /// How aggresive the agent manager should be at avoiding collisions with this agent. [Limit: >= 0] + float separationWeight; + + /// Flags that impact steering behavior. (See: #UpdateFlags) + unsigned char updateFlags; + + /// The index of the avoidance configuration to use for the agent. + /// [Limits: 0 <= value <= #DT_CROWD_MAX_OBSTAVOIDANCE_PARAMS] + unsigned char obstacleAvoidanceType; + + /// The index of the query filter used by this agent. + unsigned char queryFilterType; + + /// User defined data attached to the agent. + void* userData; +}; + +enum MoveRequestState +{ + DT_CROWDAGENT_TARGET_NONE = 0, + DT_CROWDAGENT_TARGET_FAILED, + DT_CROWDAGENT_TARGET_VALID, + DT_CROWDAGENT_TARGET_REQUESTING, + DT_CROWDAGENT_TARGET_WAITING_FOR_QUEUE, + DT_CROWDAGENT_TARGET_WAITING_FOR_PATH, + DT_CROWDAGENT_TARGET_VELOCITY +}; + +/// Represents an agent managed by a #dtCrowd object. +/// @ingroup crowd +struct dtCrowdAgent +{ + /// True if the agent is active, false if the agent is in an unused slot in the agent pool. + bool active; + + /// The type of mesh polygon the agent is traversing. (See: #CrowdAgentState) + unsigned char state; + + /// True if the agent has valid path (targetState == DT_CROWDAGENT_TARGET_VALID) and the path does not lead to the requested position, else false. + bool partial; + + /// The path corridor the agent is using. + dtPathCorridor corridor; + + /// The local boundary data for the agent. + dtLocalBoundary boundary; + + /// Time since the agent's path corridor was optimized. + float topologyOptTime; + + /// The known neighbors of the agent. + dtCrowdNeighbour neis[DT_CROWDAGENT_MAX_NEIGHBOURS]; + + /// The number of neighbors. + int nneis; + + /// The desired speed. + float desiredSpeed; + + float npos[3]; ///< The current agent position. [(x, y, z)] + float disp[3]; ///< A temporary value used to accumulate agent displacement during iterative collision resolution. [(x, y, z)] + float dvel[3]; ///< The desired velocity of the agent. Based on the current path, calculated from scratch each frame. [(x, y, z)] + float nvel[3]; ///< The desired velocity adjusted by obstacle avoidance, calculated from scratch each frame. [(x, y, z)] + float vel[3]; ///< The actual velocity of the agent. The change from nvel -> vel is constrained by max acceleration. [(x, y, z)] + + /// The agent's configuration parameters. + dtCrowdAgentParams params; + + /// The local path corridor corners for the agent. (Staight path.) [(x, y, z) * #ncorners] + float cornerVerts[DT_CROWDAGENT_MAX_CORNERS*3]; + + /// The local path corridor corner flags. (See: #dtStraightPathFlags) [(flags) * #ncorners] + unsigned char cornerFlags[DT_CROWDAGENT_MAX_CORNERS]; + + /// The reference id of the polygon being entered at the corner. [(polyRef) * #ncorners] + dtPolyRef cornerPolys[DT_CROWDAGENT_MAX_CORNERS]; + + /// The number of corners. + int ncorners; + + unsigned char targetState; ///< State of the movement request. + dtPolyRef targetRef; ///< Target polyref of the movement request. + float targetPos[3]; ///< Target position of the movement request (or velocity in case of DT_CROWDAGENT_TARGET_VELOCITY). + dtPathQueueRef targetPathqRef; ///< Path finder ref. + bool targetReplan; ///< Flag indicating that the current path is being replanned. + float targetReplanTime; ///