diff options
| author | sucofog <4pdcvicente@gmail.com> | 2017-11-14 15:14:43 +0100 |
|---|---|---|
| committer | Yehonal <yehonal.azeroth@gmail.com> | 2017-11-14 15:14:43 +0100 |
| commit | e772b08c6808bae29db96ed2f51ee5b34d768da4 (patch) | |
| tree | 7b85a379832ca68ba3d2a0d18e6e16e8caa7a179 /modules | |
| parent | d98ba9cdaa849b9af0fd701ea487f1d3127f07ef (diff) | |
Update Vmaps | Mmaps | Recastnav and fixed FleeingMovement
- Fixes getHeight collision (Map height is now calculated properly core-side, extraction of Maps, Vmaps is required)
- Fixes invisible walls causing LoS errores and wrong pathing in some zones.
- Mmaps update, padding is used, now to ensure proper binary-identical mmtiles
- Updated Recastnav to work properly with new updates
- Updated Area Storage
- Implement Map out of Bound (players will pop on closest graveyard if out of bounds)
- FleeingMovementGenerator updated, LoS calc to not go out of bounds or in/under textured when
fleeing
- Added command .mmap, port from TC (info about mmaps)
Diffstat (limited to 'modules')
41 files changed, 4390 insertions, 2757 deletions
diff --git a/modules/acore/extractors/map_extractor/CMakeLists.txt b/modules/acore/extractors/map_extractor/CMakeLists.txt index 872c9f5192..020f8a02e9 100644 --- a/modules/acore/extractors/map_extractor/CMakeLists.txt +++ b/modules/acore/extractors/map_extractor/CMakeLists.txt @@ -12,8 +12,10 @@ file(GLOB_RECURSE sources *.cpp *.h) set(include_Dirs + ${CMAKE_SOURCE_DIR}/modules/worldengine/nucleus/src/Utilities ${CMAKE_SOURCE_DIR}/modules/worldengine/nucleus/src ${CMAKE_SOURCE_DIR}/modules/worldengine/deps/libmpq + ${CMAKE_SOURCE_DIR}/modules/worldengine/deps/g3dlite/include ${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/loadlib ) diff --git a/modules/acore/extractors/map_extractor/System.cpp b/modules/acore/extractors/map_extractor/System.cpp index defab209d7..a22abb3ded 100644 --- a/modules/acore/extractors/map_extractor/System.cpp +++ b/modules/acore/extractors/map_extractor/System.cpp @@ -20,9 +20,12 @@ #include "dbcfile.h" #include "mpq_libmpq04.h" +#include "StringFormat.h" #include "adt.h" #include "wdt.h" + +#include "G3D/Plane.h" #include <fcntl.h> #if defined( __GNUC__ ) @@ -49,11 +52,10 @@ typedef struct } map_id; map_id *map_ids; -uint16 *areas; uint16 *LiqType; -char output_path[128] = "."; -char input_path[128] = "."; -uint32 maxAreaId = 0; +#define MAX_PATH_LENGTH 128 +char output_path[MAX_PATH_LENGTH] = "."; +char input_path[MAX_PATH_LENGTH] = "."; // ************************************************** // Extractor options @@ -244,30 +246,6 @@ uint32 ReadMapDBC() return map_count; } -void ReadAreaTableDBC() -{ - printf("Read AreaTable.dbc file..."); - DBCFile dbc("DBFilesClient\\AreaTable.dbc"); - - if(!dbc.open()) - { - printf("Fatal error: Invalid AreaTable.dbc file format!\n"); - exit(1); - } - - size_t area_count = dbc.getRecordCount(); - size_t maxid = dbc.getMaxId(); - areas = new uint16[maxid + 1]; - memset(areas, 0xff, (maxid + 1) * sizeof(uint16)); - - for(uint32 x = 0; x < area_count; ++x) - areas[dbc.getRecord(x).getUInt(0)] = dbc.getRecord(x).getUInt(3); - - maxAreaId = dbc.getMaxId(); - - printf("Done! (%u areas loaded)\n", (uint32)area_count); -} - void ReadLiquidTypeTableDBC() { printf("Read LiquidType.dbc file..."); @@ -295,7 +273,7 @@ void ReadLiquidTypeTableDBC() // Map file format data static char const* MAP_MAGIC = "MAPS"; -static char const* MAP_VERSION_MAGIC = "v1.3"; +static char const* MAP_VERSION_MAGIC = "v1.8"; static char const* MAP_AREA_MAGIC = "AREA"; static char const* MAP_HEIGHT_MAGIC = "MHGT"; static char const* MAP_LIQUID_MAGIC = "MLIQ"; @@ -324,9 +302,10 @@ struct map_areaHeader uint16 gridArea; }; -#define MAP_HEIGHT_NO_HEIGHT 0x0001 -#define MAP_HEIGHT_AS_INT16 0x0002 -#define MAP_HEIGHT_AS_INT8 0x0004 +#define MAP_HEIGHT_NO_HEIGHT 0x0001 +#define MAP_HEIGHT_AS_INT16 0x0002 +#define MAP_HEIGHT_AS_INT8 0x0004 +#define MAP_HEIGHT_HAS_FLIGHT_BOUNDS 0x0008 struct map_heightHeader { @@ -371,7 +350,7 @@ float selectUInt16StepStore(float maxDiff) return 65535 / maxDiff; } // Temporary grid data store -uint16 area_flags[ADT_CELLS_PER_GRID][ADT_CELLS_PER_GRID]; +uint16 area_ids[ADT_CELLS_PER_GRID][ADT_CELLS_PER_GRID]; float V8[ADT_GRID_SIZE][ADT_GRID_SIZE]; float V9[ADT_GRID_SIZE+1][ADT_GRID_SIZE+1]; @@ -385,17 +364,20 @@ uint8 liquid_flags[ADT_CELLS_PER_GRID][ADT_CELLS_PER_GRID]; bool liquid_show[ADT_GRID_SIZE][ADT_GRID_SIZE]; float liquid_height[ADT_GRID_SIZE+1][ADT_GRID_SIZE+1]; -bool ConvertADT(char *filename, char *filename2, int /*cell_y*/, int /*cell_x*/, uint32 build) +int16 flight_box_max[3][3]; +int16 flight_box_min[3][3]; + +bool ConvertADT(std::string const& inputPath, std::string const& outputPath, int /*cell_y*/, int /*cell_x*/, uint32 build) { ADT_file adt; - if (!adt.loadFile(filename)) + if (!adt.loadFile(inputPath)) return false; adt_MCIN *cells = adt.a_grid->getMCIN(); if (!cells) { - printf("Can't find cells in '%s'\n", filename); + printf("Can't find cells in '%s'\n", inputPath.c_str()); return false; } @@ -405,39 +387,25 @@ bool ConvertADT(char *filename, char *filename2, int /*cell_y*/, int /*cell_x*/, // Prepare map header map_fileheader map; - map.mapMagic = *(uint32 const*)MAP_MAGIC; - map.versionMagic = *(uint32 const*)MAP_VERSION_MAGIC; + map.mapMagic = *reinterpret_cast<uint32 const*>(MAP_MAGIC); + map.versionMagic = *reinterpret_cast<uint32 const*>(MAP_VERSION_MAGIC); map.buildMagic = build; // Get area flags data - for (int i=0;i<ADT_CELLS_PER_GRID;i++) - { - for(int j=0;j<ADT_CELLS_PER_GRID;j++) - { - adt_MCNK * cell = cells->getMCNK(i,j); - uint32 areaid = cell->areaid; - if(areaid && areaid <= maxAreaId) - { - if(areas[areaid] != 0xffff) - { - area_flags[i][j] = areas[areaid]; - continue; - } - printf("File: %s\nCan't find area flag for areaid %u [%d, %d].\n", filename, areaid, cell->ix, cell->iy); - } - area_flags[i][j] = 0xffff; - } - } + for (int i = 0; i < ADT_CELLS_PER_GRID; i++) + for (int j = 0; j < ADT_CELLS_PER_GRID; j++) + area_ids[i][j] = cells->getMCNK(i, j)->areaid; + //============================================ // Try pack area data //============================================ bool fullAreaData = false; - uint32 areaflag = area_flags[0][0]; - for (int y=0;y<ADT_CELLS_PER_GRID;y++) + uint32 areaId = area_ids[0][0]; + for (int y = 0; y < ADT_CELLS_PER_GRID; ++y) { - for(int x=0;x<ADT_CELLS_PER_GRID;x++) + for (int x = 0; x < ADT_CELLS_PER_GRID; ++x) { - if(area_flags[y][x]!=areaflag) + if (area_ids[y][x] != areaId) { fullAreaData = true; break; @@ -449,27 +417,27 @@ bool ConvertADT(char *filename, char *filename2, int /*cell_y*/, int /*cell_x*/, map.areaMapSize = sizeof(map_areaHeader); map_areaHeader areaHeader; - areaHeader.fourcc = *(uint32 const*)MAP_AREA_MAGIC; + areaHeader.fourcc = *reinterpret_cast<uint32 const*>(MAP_AREA_MAGIC); areaHeader.flags = 0; if (fullAreaData) { areaHeader.gridArea = 0; - map.areaMapSize+=sizeof(area_flags); + map.areaMapSize += sizeof(area_ids); } else { areaHeader.flags |= MAP_AREA_NO_AREA; - areaHeader.gridArea = (uint16)areaflag; + areaHeader.gridArea = static_cast<uint16>(areaId); } // // Get Height map from grid // - for (int i=0;i<ADT_CELLS_PER_GRID;i++) + for (int i = 0; i<ADT_CELLS_PER_GRID; i++) { - for(int j=0;j<ADT_CELLS_PER_GRID;j++) + for (int j = 0; j<ADT_CELLS_PER_GRID; j++) { - adt_MCNK * cell = cells->getMCNK(i,j); + adt_MCNK * cell = cells->getMCNK(i, j); if (!cell) continue; // Height values for triangles stored in order: @@ -489,22 +457,22 @@ bool ConvertADT(char *filename, char *filename2, int /*cell_y*/, int /*cell_x*/, // . . . . . . . . // Set map height as grid height - for (int y=0; y <= ADT_CELL_SIZE; y++) + for (int y = 0; y <= ADT_CELL_SIZE; y++) { int cy = i*ADT_CELL_SIZE + y; - for (int x=0; x <= ADT_CELL_SIZE; x++) + for (int x = 0; x <= ADT_CELL_SIZE; x++) { int cx = j*ADT_CELL_SIZE + x; - V9[cy][cx]=cell->ypos; + V9[cy][cx] = cell->ypos; } } - for (int y=0; y < ADT_CELL_SIZE; y++) + for (int y = 0; y < ADT_CELL_SIZE; y++) { int cy = i*ADT_CELL_SIZE + y; - for (int x=0; x < ADT_CELL_SIZE; x++) + for (int x = 0; x < ADT_CELL_SIZE; x++) { int cx = j*ADT_CELL_SIZE + x; - V8[cy][cx]=cell->ypos; + V8[cy][cx] = cell->ypos; } } // Get custom height @@ -512,23 +480,23 @@ bool ConvertADT(char *filename, char *filename2, int /*cell_y*/, int /*cell_x*/, if (!v) continue; // get V9 height map - for (int y=0; y <= ADT_CELL_SIZE; y++) + for (int y = 0; y <= ADT_CELL_SIZE; y++) { int cy = i*ADT_CELL_SIZE + y; - for (int x=0; x <= ADT_CELL_SIZE; x++) + for (int x = 0; x <= ADT_CELL_SIZE; x++) { int cx = j*ADT_CELL_SIZE + x; - V9[cy][cx]+=v->height_map[y*(ADT_CELL_SIZE*2+1)+x]; + V9[cy][cx] += v->height_map[y*(ADT_CELL_SIZE * 2 + 1) + x]; } } // get V8 height map - for (int y=0; y < ADT_CELL_SIZE; y++) + for (int y = 0; y < ADT_CELL_SIZE; y++) { int cy = i*ADT_CELL_SIZE + y; - for (int x=0; x < ADT_CELL_SIZE; x++) + for (int x = 0; x < ADT_CELL_SIZE; x++) { int cx = j*ADT_CELL_SIZE + x; - V8[cy][cx]+=v->height_map[y*(ADT_CELL_SIZE*2+1)+ADT_CELL_SIZE+1+x]; + V8[cy][cx] += v->height_map[y*(ADT_CELL_SIZE * 2 + 1) + ADT_CELL_SIZE + 1 + x]; } } } @@ -538,18 +506,18 @@ bool ConvertADT(char *filename, char *filename2, int /*cell_y*/, int /*cell_x*/, //============================================ float maxHeight = -20000; float minHeight = 20000; - for (int y=0; y<ADT_GRID_SIZE; y++) + for (int y = 0; y<ADT_GRID_SIZE; y++) { - for(int x=0;x<ADT_GRID_SIZE;x++) + for (int x = 0; x<ADT_GRID_SIZE; x++) { float h = V8[y][x]; if (maxHeight < h) maxHeight = h; if (minHeight > h) minHeight = h; } } - for (int y=0; y<=ADT_GRID_SIZE; y++) + for (int y = 0; y <= ADT_GRID_SIZE; y++) { - for(int x=0;x<=ADT_GRID_SIZE;x++) + for(int x = 0; x<= ADT_GRID_SIZE; x++) { float h = V9[y][x]; if (maxHeight < h) maxHeight = h; @@ -560,12 +528,12 @@ bool ConvertADT(char *filename, char *filename2, int /*cell_y*/, int /*cell_x*/, // Check for allow limit minimum height (not store height in deep ochean - allow save some memory) if (CONF_allow_height_limit && minHeight < CONF_use_minHeight) { - for (int y=0; y<ADT_GRID_SIZE; y++) - for(int x=0;x<ADT_GRID_SIZE;x++) + for (int y = 0; y<ADT_GRID_SIZE; y++) + for (int x = 0; x<ADT_GRID_SIZE; x++) if (V8[y][x] < CONF_use_minHeight) V8[y][x] = CONF_use_minHeight; - for (int y=0; y<=ADT_GRID_SIZE; y++) - for(int x=0;x<=ADT_GRID_SIZE;x++) + for (int y = 0; y <= ADT_GRID_SIZE; y++) + for (int x = 0; x <= ADT_GRID_SIZE; x++) if (V9[y][x] < CONF_use_minHeight) V9[y][x] = CONF_use_minHeight; if (minHeight < CONF_use_minHeight) @@ -574,11 +542,19 @@ bool ConvertADT(char *filename, char *filename2, int /*cell_y*/, int /*cell_x*/, maxHeight = CONF_use_minHeight; } + bool hasFlightBox = false; + if (adt_MFBO* mfbo = adt.a_grid->getMFBO()) + { + memcpy(flight_box_max, &mfbo->max, sizeof(flight_box_max)); + memcpy(flight_box_min, &mfbo->min, sizeof(flight_box_min)); + hasFlightBox = true; + } + map.heightMapOffset = map.areaMapOffset + map.areaMapSize; map.heightMapSize = sizeof(map_heightHeader); map_heightHeader heightHeader; - heightHeader.fourcc = *(uint32 const*)MAP_HEIGHT_MAGIC; + heightHeader.fourcc = *reinterpret_cast<uint32 const*>(MAP_HEIGHT_MAGIC); heightHeader.flags = 0; heightHeader.gridHeight = minHeight; heightHeader.gridMaxHeight = maxHeight; @@ -589,6 +565,12 @@ bool ConvertADT(char *filename, char *filename2, int /*cell_y*/, int /*cell_x*/, // Not need store if flat surface if (CONF_allow_float_to_int && (maxHeight - minHeight) < CONF_flat_height_delta_limit) heightHeader.flags |= MAP_HEIGHT_NO_HEIGHT; + + if (hasFlightBox) + { + heightHeader.flags |= MAP_HEIGHT_HAS_FLIGHT_BOUNDS; + map.heightMapSize += sizeof(flight_box_max) + sizeof(flight_box_min); + } // Try store as packed in uint16 or uint8 values if (!(heightHeader.flags & MAP_HEIGHT_NO_HEIGHT)) @@ -600,12 +582,12 @@ bool ConvertADT(char *filename, char *filename2, int /*cell_y*/, int /*cell_x*/, float diff = maxHeight - minHeight; if (diff < CONF_float_to_int8_limit) // As uint8 (max accuracy = CONF_float_to_int8_limit/256) { - heightHeader.flags|=MAP_HEIGHT_AS_INT8; + heightHeader.flags |= MAP_HEIGHT_AS_INT8; step = selectUInt8StepStore(diff); } else if (diff<CONF_float_to_int16_limit) // As uint16 (max accuracy = CONF_float_to_int16_limit/65536) { - heightHeader.flags|=MAP_HEIGHT_AS_INT16; + heightHeader.flags |= MAP_HEIGHT_AS_INT16; step = selectUInt16StepStore(diff); } } @@ -613,32 +595,32 @@ bool ConvertADT(char *filename, char *filename2, int /*cell_y*/, int /*cell_x*/, // Pack it to int values if need if (heightHeader.flags&MAP_HEIGHT_AS_INT8) { - for (int y=0; y<ADT_GRID_SIZE; y++) - for(int x=0;x<ADT_GRID_SIZE;x++) + for (int y = 0; y<ADT_GRID_SIZE; y++) + for (int x = 0; x<ADT_GRID_SIZE; x++) uint8_V8[y][x] = uint8((V8[y][x] - minHeight) * step + 0.5f); - for (int y=0; y<=ADT_GRID_SIZE; y++) - for(int x=0;x<=ADT_GRID_SIZE;x++) + for (int y = 0; y <= ADT_GRID_SIZE; y++) + for (int x = 0; x <= ADT_GRID_SIZE; x++) uint8_V9[y][x] = uint8((V9[y][x] - minHeight) * step + 0.5f); - map.heightMapSize+= sizeof(uint8_V9) + sizeof(uint8_V8); + map.heightMapSize += sizeof(uint8_V9) + sizeof(uint8_V8); } else if (heightHeader.flags&MAP_HEIGHT_AS_INT16) { - for (int y=0; y<ADT_GRID_SIZE; y++) - for(int x=0;x<ADT_GRID_SIZE;x++) + for (int y = 0; y<ADT_GRID_SIZE; y++) + for (int x = 0; x<ADT_GRID_SIZE; x++) uint16_V8[y][x] = uint16((V8[y][x] - minHeight) * step + 0.5f); - for (int y=0; y<=ADT_GRID_SIZE; y++) - for(int x=0;x<=ADT_GRID_SIZE;x++) + for (int y = 0; y <= ADT_GRID_SIZE; y++) + for (int x = 0; x <= ADT_GRID_SIZE; x++) uint16_V9[y][x] = uint16((V9[y][x] - minHeight) * step + 0.5f); - map.heightMapSize+= sizeof(uint16_V9) + sizeof(uint16_V8); + map.heightMapSize += sizeof(uint16_V9) + sizeof(uint16_V8); } else - map.heightMapSize+= sizeof(V9) + sizeof(V8); + map.heightMapSize += sizeof(V9) + sizeof(V8); } // Get from MCLQ chunk (old) for (int i = 0; i < ADT_CELLS_PER_GRID; i++) { - for(int j = 0; j < ADT_CELLS_PER_GRID; j++) + for (int j = 0; j < ADT_CELLS_PER_GRID; j++) { adt_MCNK *cell = cells->getMCNK(i, j); if (!cell) @@ -658,7 +640,7 @@ bool ConvertADT(char *filename, char *filename2, int /*cell_y*/, int /*cell_x*/, if (liquid->flags[y][x] != 0x0F) { liquid_show[cy][cx] = true; - if (liquid->flags[y][x] & (1<<7)) + if (liquid->flags[y][x] & (1 << 7)) liquid_flags[i][j] |= MAP_LIQUID_TYPE_DARK_WATER; ++count; } @@ -666,17 +648,17 @@ bool ConvertADT(char *filename, char *filename2, int /*cell_y*/, int /*cell_x*/, } uint32 c_flag = cell->flags; - if (c_flag & (1<<2)) + if (c_flag & (1 << 2)) { liquid_entry[i][j] = 1; liquid_flags[i][j] |= MAP_LIQUID_TYPE_WATER; // water } - if (c_flag & (1<<3)) + if (c_flag & (1 << 3)) { liquid_entry[i][j] = 2; liquid_flags[i][j] |= MAP_LIQUID_TYPE_OCEAN; // ocean } - if (c_flag & (1<<4)) + if (c_flag & (1 << 4)) { liquid_entry[i][j] = 3; liquid_flags[i][j] |= MAP_LIQUID_TYPE_MAGMA; // magma/slime @@ -734,7 +716,7 @@ bool ConvertADT(char *filename, char *filename2, int /*cell_y*/, int /*cell_x*/, case LIQUID_TYPE_MAGMA: liquid_flags[i][j] |= MAP_LIQUID_TYPE_MAGMA; break; case LIQUID_TYPE_SLIME: liquid_flags[i][j] |= MAP_LIQUID_TYPE_SLIME; break; default: - printf("\nCan't find Liquid type %u for map %s\nchunk %d,%d\n", h->liquidType, filename, i, j); + printf("\nCan't find Liquid type %u for map %s\nchunk %d,%d\n", h->liquidType, inputPath.c_str(), i, j); break; } // Dark water detect @@ -877,17 +859,17 @@ bool ConvertADT(char *filename, char *filename2, int /*cell_y*/, int /*cell_x*/, map.holesSize = 0; // Ok all data prepared - store it - FILE* output = fopen(filename2, "wb"); + FILE* output = fopen(outputPath.c_str(), "wb"); if (!output) { - printf("Can't create the output file '%s'\n", filename2); + printf("Can't create the output file '%s'\n", outputPath.c_str()); return false; } fwrite(&map, sizeof(map), 1, output); // Store area data fwrite(&areaHeader, sizeof(areaHeader), 1, output); if (!(areaHeader.flags&MAP_AREA_NO_AREA)) - fwrite(area_flags, sizeof(area_flags), 1, output); + fwrite(area_ids, sizeof(area_ids), 1, output); // Store height data fwrite(&heightHeader, sizeof(heightHeader), 1, output); @@ -910,6 +892,12 @@ bool ConvertADT(char *filename, char *filename2, int /*cell_y*/, int /*cell_x*/, } } + if (heightHeader.flags & MAP_HEIGHT_HAS_FLIGHT_BOUNDS) + { + fwrite(flight_box_max, sizeof(flight_box_max), 1, output); + fwrite(flight_box_min, sizeof(flight_box_min), 1, output); + } + // Store liquid data if need if (map.liquidMapOffset) { @@ -937,15 +925,14 @@ bool ConvertADT(char *filename, char *filename2, int /*cell_y*/, int /*cell_x*/, void ExtractMapsFromMpq(uint32 build) { - char mpq_filename[1024]; - char output_filename[1024]; - char mpq_map_name[1024]; + std::string mpqFileName; + std::string outputFileName; + std::string mpqMapName; printf("Extracting maps...\n"); uint32 map_count = ReadMapDBC(); - ReadAreaTableDBC(); ReadLiquidTypeTableDBC(); std::string path = output_path; @@ -957,9 +944,9 @@ void ExtractMapsFromMpq(uint32 build) { printf("Extract %s (%d/%u) \n", map_ids[z].name, z+1, map_count); // Loadup map grid data - sprintf(mpq_map_name, "World\\Maps\\%s\\%s.wdt", map_ids[z].name, map_ids[z].name); + mpqMapName = Trinity::StringFormat("World\\Maps\\%s\\%s.wdt", map_ids[z].name, map_ids[z].name); WDT_file wdt; - if (!wdt.loadFile(mpq_map_name, false)) + if (!wdt.loadFile(mpqMapName, false)) { // printf("Error loading %s map wdt data\n", map_ids[z].name); continue; @@ -971,17 +958,16 @@ void ExtractMapsFromMpq(uint32 build) { if (!wdt.main->adt_list[y][x].exist) continue; - sprintf(mpq_filename, "World\\Maps\\%s\\%s_%u_%u.adt", map_ids[z].name, map_ids[z].name, x, y); - sprintf(output_filename, "%s/maps/%03u%02u%02u.map", output_path, map_ids[z].id, y, x); - ConvertADT(mpq_filename, output_filename, y, x, build); + mpqFileName = Trinity::StringFormat("World\\Maps\\%s\\%s_%u_%u.adt", map_ids[z].name, map_ids[z].name, x, y); + outputFileName = Trinity::StringFormat("%s/maps/%03u%02u%02u.map", output_path, map_ids[z].id, y, x); + ConvertADT(mpqFileName, outputFileName, y, x, build); } // draw progress bar printf("Processing........................%d%%\r", (100 * (y+1)) / WDT_MAP_SIZE); } } printf("\n"); - delete [] areas; - delete [] map_ids; + delete[] map_ids; } bool ExtractFile( char const* mpq_name, std::string const& filename ) diff --git a/modules/acore/extractors/map_extractor/adt.cpp b/modules/acore/extractors/map_extractor/adt.cpp index 574167c216..257d0171aa 100644 --- a/modules/acore/extractors/map_extractor/adt.cpp +++ b/modules/acore/extractors/map_extractor/adt.cpp @@ -9,15 +9,16 @@ #include "adt.h" // Helper -int holetab_h[4] = {0x1111, 0x2222, 0x4444, 0x8888}; -int holetab_v[4] = {0x000F, 0x00F0, 0x0F00, 0xF000}; +int holetab_h[4] = { 0x1111, 0x2222, 0x4444, 0x8888 }; +int holetab_v[4] = { 0x000F, 0x00F0, 0x0F00, 0xF000 }; -u_map_fcc MHDRMagic = { {'R','D','H','M'} }; -u_map_fcc MCINMagic = { {'N','I','C','M'} }; -u_map_fcc MH2OMagic = { {'O','2','H','M'} }; -u_map_fcc MCNKMagic = { {'K','N','C','M'} }; -u_map_fcc MCVTMagic = { {'T','V','C','M'} }; -u_map_fcc MCLQMagic = { {'Q','L','C','M'} }; +u_map_fcc MHDRMagic = { { 'R','D','H','M' } }; +u_map_fcc MCINMagic = { { 'N','I','C','M' } }; +u_map_fcc MH2OMagic = { { 'O','2','H','M' } }; +u_map_fcc MCNKMagic = { { 'K','N','C','M' } }; +u_map_fcc MCVTMagic = { { 'T','V','C','M' } }; +u_map_fcc MCLQMagic = { { 'Q','L','C','M' } }; +u_map_fcc MFBOMagic = { { 'O','B','F','M' } }; bool isHole(int holes, int i, int j) { @@ -69,7 +70,7 @@ bool adt_MHDR::prepareLoadedData() if (fcc != MHDRMagic.fcc) return false; - if (size!=sizeof(adt_MHDR)-8) + if (size != sizeof(adt_MHDR) - 8) return false; // Check and prepare MCIN @@ -80,6 +81,9 @@ bool adt_MHDR::prepareLoadedData() if (offsMH2O && !getMH2O()->prepareLoadedData()) return false; + if (offsMFBO && flags & 1 && !getMFBO()->prepareLoadedData()) + return false; + return true; } @@ -142,3 +146,8 @@ bool adt_MCLQ::prepareLoadedData() return true; } + +bool adt_MFBO::prepareLoadedData() +{ + return fcc == MFBOMagic.fcc; +} diff --git a/modules/acore/extractors/map_extractor/adt.h b/modules/acore/extractors/map_extractor/adt.h index 71e83cd999..94b830c4ef 100644 --- a/modules/acore/extractors/map_extractor/adt.h +++ b/modules/acore/extractors/map_extractor/adt.h @@ -248,6 +248,27 @@ public: }; +// Adt file min/max height chunk +// +class adt_MFBO +{ + union + { + uint32 fcc; + char fcc_txt[4]; + }; +public: + uint32 size; + struct plane + { + int16 coords[9]; + }; + plane max; + plane min; + + bool prepareLoadedData(); +}; + // // Adt file header chunk // @@ -260,12 +281,12 @@ class adt_MHDR public: uint32 size; - uint32 pad; + uint32 flags; uint32 offsMCIN; // MCIN - uint32 offsTex; // MTEX - uint32 offsModels; // MMDX - uint32 offsModelsIds; // MMID - uint32 offsMapObejcts; // MWMO + uint32 offsTex; // MTEX + uint32 offsModels; // MMDX + uint32 offsModelsIds; // MMID + uint32 offsMapObejcts; // MWMO uint32 offsMapObejctsIds; // MWID uint32 offsDoodsDef; // MDDF uint32 offsObjectsDef; // MODF @@ -278,9 +299,22 @@ public: uint32 data5; public: bool prepareLoadedData(); - adt_MCIN *getMCIN(){ return (adt_MCIN *)((uint8 *)&pad+offsMCIN);} - adt_MH2O *getMH2O(){ return offsMH2O ? (adt_MH2O *)((uint8 *)&pad+offsMH2O) : 0;} - + adt_MCIN* getMCIN() + { + return reinterpret_cast<adt_MCIN*>(reinterpret_cast<uint8*>(&flags) + offsMCIN); + } + adt_MH2O* getMH2O() + { + if (offsMH2O) + return reinterpret_cast<adt_MH2O*>(reinterpret_cast<uint8*>(&flags) + offsMH2O); + return nullptr; + } + adt_MFBO* getMFBO() + { + if (flags & 1 && offsMFBO) + return reinterpret_cast<adt_MFBO*>(reinterpret_cast<uint8*>(&flags) + offsMFBO); + return nullptr; + } }; class ADT_file : public FileLoader{ diff --git a/modules/acore/extractors/map_extractor/loadlib.cpp b/modules/acore/extractors/map_extractor/loadlib.cpp index 2a3b0482f5..1219ef78b5 100644 --- a/modules/acore/extractors/map_extractor/loadlib.cpp +++ b/modules/acore/extractors/map_extractor/loadlib.cpp @@ -26,14 +26,14 @@ FileLoader::~FileLoader() free(); } -bool FileLoader::loadFile(char *filename, bool log) +bool FileLoader::loadFile(std::string const& fileName, bool log) { free(); - MPQFile mf(filename); + MPQFile mf(fileName.c_str()); if(mf.isEof()) { if (log) - printf("No such file %s\n", filename); + printf("No such file %s\n", fileName.c_str()); return false; } @@ -45,7 +45,7 @@ bool FileLoader::loadFile(char *filename, bool log) if (prepareLoadedData()) return true; - printf("Error loading %s", filename); + printf("Error loading %s", fileName.c_str()); mf.close(); free(); return false; diff --git a/modules/acore/extractors/map_extractor/loadlib/loadlib.h b/modules/acore/extractors/map_extractor/loadlib/loadlib.h index a2169b6b4b..e500f25e6e 100644 --- a/modules/acore/extractors/map_extractor/loadlib/loadlib.h +++ b/modules/acore/extractors/map_extractor/loadlib/loadlib.h @@ -7,6 +7,8 @@ #ifndef LOAD_LIB_H #define LOAD_LIB_H +#include <string> + #ifdef _WIN32 typedef __int64 int64; typedef __int32 int32; @@ -65,7 +67,7 @@ public: file_MVER *version; FileLoader(); ~FileLoader(); - bool loadFile(char *filename, bool log = true); + bool loadFile(std::string const& filename, bool log = true); virtual void free(); }; #endif diff --git a/modules/acore/extractors/mesh_extractor/Utils.h b/modules/acore/extractors/mesh_extractor/Utils.h index bf98e65f01..5f054a556d 100644 --- a/modules/acore/extractors/mesh_extractor/Utils.h +++ b/modules/acore/extractors/mesh_extractor/Utils.h @@ -341,7 +341,7 @@ public: }; #define MMAP_MAGIC 0x4d4d4150 // 'MMAP' -#define MMAP_VERSION 3 +#define MMAP_VERSION 6 struct MmapTileHeader { diff --git a/modules/acore/extractors/mmaps_generator/MapBuilder.cpp b/modules/acore/extractors/mmaps_generator/MapBuilder.cpp index e5edd4a611..41cf8b5cae 100644 --- a/modules/acore/extractors/mmaps_generator/MapBuilder.cpp +++ b/modules/acore/extractors/mmaps_generator/MapBuilder.cpp @@ -24,7 +24,7 @@ namespace DisableMgr } #define MMAP_MAGIC 0x4d4d4150 // 'MMAP' -#define MMAP_VERSION 3 +#define MMAP_VERSION 6 struct MmapTileHeader { @@ -32,12 +32,22 @@ struct MmapTileHeader uint32 dtVersion; uint32 mmapVersion; uint32 size; - bool usesLiquids : 1; + char usesLiquids; + char padding[3]; MmapTileHeader() : mmapMagic(MMAP_MAGIC), dtVersion(DT_NAVMESH_VERSION), - mmapVersion(MMAP_VERSION), size(0), usesLiquids(true) {} + mmapVersion(MMAP_VERSION), size(0), usesLiquids(true), padding() {} }; +// All padding fields must be handled and initialized to ensure mmaps_generator will produce binary-identical *.mmtile files +static_assert(sizeof(MmapTileHeader) == 20, "MmapTileHeader size is not correct, adjust the padding field size"); +static_assert(sizeof(MmapTileHeader) == (sizeof(MmapTileHeader::mmapMagic) + + sizeof(MmapTileHeader::dtVersion) + + sizeof(MmapTileHeader::mmapVersion) + + sizeof(MmapTileHeader::size) + + sizeof(MmapTileHeader::usesLiquids) + + sizeof(MmapTileHeader::padding)), "MmapTileHeader has uninitialized padding fields"); + namespace MMAP { MapBuilder::MapBuilder(float maxWalkableAngle, bool skipLiquid, @@ -57,6 +67,10 @@ namespace MMAP m_rcContext = new rcContext(false); + // percentageDone - Initializing + m_totalTiles = 0; + m_totalTilesBuilt = 0; + discoverTiles(); } @@ -65,8 +79,8 @@ namespace MMAP { for (TileList::iterator it = m_tiles.begin(); it != m_tiles.end(); ++it) { - (*it).second->clear(); - delete (*it).second; + (*it).m_tiles->clear(); + delete (*it).m_tiles; } delete m_terrainBuilder; @@ -85,9 +99,9 @@ namespace MMAP for (uint32 i = 0; i < files.size(); ++i) { mapID = uint32(atoi(files[i].substr(0,3).c_str())); - if (m_tiles.find(mapID) == m_tiles.end()) + if (std::find(m_tiles.begin(), m_tiles.end(), mapID) == m_tiles.end()) { - m_tiles.insert(std::pair<uint32, std::set<uint32>*>(mapID, new std::set<uint32>)); + m_tiles.emplace_back(MapTiles(mapID, new std::set<uint32>)); count++; } } @@ -97,8 +111,11 @@ namespace MMAP for (uint32 i = 0; i < files.size(); ++i) { mapID = uint32(atoi(files[i].substr(0,3).c_str())); - m_tiles.insert(std::pair<uint32, std::set<uint32>*>(mapID, new std::set<uint32>)); - count++; + if (std::find(m_tiles.begin(), m_tiles.end(), mapID) == m_tiles.end()) + { + m_tiles.emplace_back(MapTiles(mapID, new std::set<uint32>)); + count++; + } } printf("found %u.\n", count); @@ -106,8 +123,8 @@ namespace MMAP printf("Discovering tiles... "); for (TileList::iterator itr = m_tiles.begin(); itr != m_tiles.end(); ++itr) { - std::set<uint32>* tiles = (*itr).second; - mapID = (*itr).first; + std::set<uint32>* tiles = (*itr).m_tiles; + mapID = (*itr).m_mapId; sprintf(filter, "%03u*.vmtile", mapID); files.clear(); @@ -136,17 +153,20 @@ namespace MMAP } } printf("found %u.\n\n", count); + + // percentageDone - total tiles to process + m_totalTiles = count; } /**************************************************************************/ std::set<uint32>* MapBuilder::getTileList(uint32 mapID) { - TileList::iterator itr = m_tiles.find(mapID); + TileList::iterator itr = std::find(m_tiles.begin(), m_tiles.end(), mapID); if (itr != m_tiles.end()) - return (*itr).second; + return (*itr).m_tiles; std::set<uint32>* tiles = new std::set<uint32>(); - m_tiles.insert(std::pair<uint32, std::set<uint32>*>(mapID, tiles)); + m_tiles.emplace_back(MapTiles(mapID, tiles)); return tiles; } @@ -157,9 +177,14 @@ namespace MMAP BuilderThreadPool* pool = threads > 0 ? new BuilderThreadPool() : NULL; + m_tiles.sort([](MapTiles a, MapTiles b) + { + return a.m_tiles->size() > b.m_tiles->size(); + }); + for (TileList::iterator it = m_tiles.begin(); it != m_tiles.end(); ++it) { - uint32 mapID = it->first; + uint32 mapID = it->m_mapId; if (!shouldSkipMap(mapID)) { if (threads > 0) @@ -382,7 +407,8 @@ namespace MMAP /**************************************************************************/ void MapBuilder::buildTile(uint32 mapID, uint32 tileX, uint32 tileY, dtNavMesh* navMesh) { - printf("[Map %03i] Building tile [%02u,%02u]\n", mapID, tileX, tileY); + // percentageDone - added, now it will show addional reference percentage done of the overall process + printf("%u%% [Map %03i] Building tile [%02u,%02u]\n", percentageDone(m_totalTiles, m_totalTilesBuilt), mapID, tileX, tileY); MeshData meshData; @@ -416,6 +442,9 @@ namespace MMAP // build navmesh tile buildMoveMapTile(mapID, tileX, tileY, meshData, bmin, bmax, navMesh); + + // percentageDone - increment tiles built + m_totalTilesBuilt++; } /**************************************************************************/ @@ -970,5 +999,13 @@ namespace MMAP return true; } + + /**************************************************************************/ + uint32 MapBuilder::percentageDone(uint32 totalTiles, uint32 totalTilesBuilt) + { + if (totalTiles) + return totalTilesBuilt * 100 / totalTiles; + return 0; + } } diff --git a/modules/acore/extractors/mmaps_generator/MapBuilder.h b/modules/acore/extractors/mmaps_generator/MapBuilder.h index c99b8da0ea..4aefaede0b 100644 --- a/modules/acore/extractors/mmaps_generator/MapBuilder.h +++ b/modules/acore/extractors/mmaps_generator/MapBuilder.h @@ -9,7 +9,9 @@ #include <vector> #include <set> +#include <atomic> #include <map> +#include <list> #include "TerrainBuilder.h" #include "IntermediateValues.h" @@ -27,7 +29,24 @@ using namespace VMAP; namespace MMAP { - typedef std::map<uint32, std::set<uint32>*> TileList; + struct MapTiles + { + MapTiles() : m_mapId(uint32(-1)), m_tiles(NULL) {} + + MapTiles(uint32 id, std::set<uint32>* tiles) : m_mapId(id), m_tiles(tiles) {} + ~MapTiles() {} + + uint32 m_mapId; + std::set<uint32>* m_tiles; + + bool operator==(uint32 id) + { + return m_mapId == id; + } + }; + + typedef std::list<MapTiles> TileList; + struct Tile { Tile() : chf(NULL), solid(NULL), cset(NULL), pmesh(NULL), dmesh(NULL) {} @@ -96,6 +115,8 @@ namespace MMAP bool shouldSkipMap(uint32 mapID); bool isTransportMap(uint32 mapID); bool shouldSkipTile(uint32 mapID, uint32 tileX, uint32 tileY); + // percentageDone - method to calculate percentage + uint32 percentageDone(uint32 totalTiles, uint32 totalTilesDone); TerrainBuilder* m_terrainBuilder; TileList m_tiles; @@ -109,6 +130,9 @@ namespace MMAP float m_maxWalkableAngle; bool m_bigBaseUnit; + // percentageDone - variables to calculate percentage + uint32 m_totalTiles; + std::atomic<uint32> m_totalTilesBuilt; // build performance - not really used for now rcContext* m_rcContext; diff --git a/modules/acore/extractors/mmaps_generator/PathCommon.h b/modules/acore/extractors/mmaps_generator/PathCommon.h index 477f974021..586eeb7b52 100644 --- a/modules/acore/extractors/mmaps_generator/PathCommon.h +++ b/modules/acore/extractors/mmaps_generator/PathCommon.h @@ -9,9 +9,8 @@ #include <string> #include <vector> -#include <ace/OS_NS_sys_time.h> -#include "Define.h" +#include "Common.h" #ifndef _WIN32 #include <stddef.h> @@ -125,26 +124,6 @@ namespace MMAP return LISTFILE_OK; } - - inline uint32 getMSTime() - { - static const ACE_Time_Value ApplicationStartTime = ACE_OS::gettimeofday(); - return (ACE_OS::gettimeofday() - ApplicationStartTime).msec(); - } - - inline uint32 getMSTimeDiff(uint32 oldMSTime, uint32 newMSTime) - { - // getMSTime() have limited data range and this is case when it overflow in this tick - if (oldMSTime > newMSTime) - return (0xFFFFFFFF - oldMSTime) + newMSTime; - else - return newMSTime - oldMSTime; - } - - inline uint32 GetMSTimeDiffToNow(uint32 oldMSTime) - { - return getMSTimeDiff(oldMSTime, getMSTime()); - } } #endif diff --git a/modules/acore/extractors/mmaps_generator/PathGenerator.cpp b/modules/acore/extractors/mmaps_generator/PathGenerator.cpp index 16201eba1a..be02297d90 100644 --- a/modules/acore/extractors/mmaps_generator/PathGenerator.cpp +++ b/modules/acore/extractors/mmaps_generator/PathGenerator.cpp @@ -6,6 +6,7 @@ #include "PathCommon.h" #include "MapBuilder.h" +#include "Timer.h" using namespace MMAP; diff --git a/modules/acore/extractors/mmaps_generator/TerrainBuilder.cpp b/modules/acore/extractors/mmaps_generator/TerrainBuilder.cpp index 260bb6da7e..47fe43b6c2 100644 --- a/modules/acore/extractors/mmaps_generator/TerrainBuilder.cpp +++ b/modules/acore/extractors/mmaps_generator/TerrainBuilder.cpp @@ -70,7 +70,7 @@ struct map_liquidHeader namespace MMAP { - char const* MAP_VERSION_MAGIC = "v1.3"; + char const* MAP_VERSION_MAGIC = "v1.8"; TerrainBuilder::TerrainBuilder(bool skipLiquid) : m_skipLiquid (skipLiquid){ } TerrainBuilder::~TerrainBuilder() { } diff --git a/modules/acore/extractors/vmap4_extractor/loadlib/loadlib.h b/modules/acore/extractors/vmap4_extractor/loadlib/loadlib.h index e55b4a7c56..989f665934 100644 --- a/modules/acore/extractors/vmap4_extractor/loadlib/loadlib.h +++ b/modules/acore/extractors/vmap4_extractor/loadlib/loadlib.h @@ -7,6 +7,8 @@ #ifndef LOAD_LIB_H #define LOAD_LIB_H +#include <string> + #ifdef WIN32 typedef __int64 int64; typedef __int32 int32; @@ -59,7 +61,7 @@ public: file_MVER *version; FileLoader(); ~FileLoader(); - bool loadFile(char *filename, bool log = true); + bool loadFile(std::string const& filename, bool log = true); virtual void free(); }; #endif diff --git a/modules/acore/extractors/vmap4_extractor/model.cpp b/modules/acore/extractors/vmap4_extractor/model.cpp index 28c1bad31b..07f1f3ec16 100644 --- a/modules/acore/extractors/vmap4_extractor/model.cpp +++ b/modules/acore/extractors/vmap4_extractor/model.cpp @@ -84,17 +84,31 @@ bool Model::ConvertToVMAPModel(const char * outfilename) wsize = sizeof(uint32) + sizeof(unsigned short) * nIndexes; fwrite(&wsize, sizeof(int), 1, output); fwrite(&nIndexes, sizeof(uint32), 1, output); - if (nIndexes >0) + if (nIndexes > 0) + { + for (uint32 i = 0; i < nIndexes; ++i) + { + if ((i % 3) - 1 == 0 && i + 1 < nIndexes) + { + uint16 tmp = indices[i]; + indices[i] = indices[i + 1]; + indices[i + 1] = tmp; + } + } fwrite(indices, sizeof(unsigned short), nIndexes, output); - + } fwrite("VERT", 4, 1, output); wsize = sizeof(int) + sizeof(float) * 3 * nVertices; fwrite(&wsize, sizeof(int), 1, output); fwrite(&nVertices, sizeof(int), 1, output); if (nVertices >0) { - for(uint32 vpos=0; vpos <nVertices; ++vpos) - std::swap(vertices[vpos].y, vertices[vpos].z); + for (uint32 vpos = 0; vpos < nVertices; ++vpos) + { + float tmp = vertices[vpos].y; + vertices[vpos].y = -vertices[vpos].z; + vertices[vpos].z = tmp; + } fwrite(vertices, sizeof(float)*3, nVertices, output); } @@ -169,7 +183,6 @@ ModelInstance::ModelInstance(MPQFile& f, char const* ModelInstName, uint32 mapID int realy1 = (int) ((float) pos.z / 533.333333f); int realx2 = (int) ((float) pos.x / 533.333333f); int realy2 = (int) ((float) pos.z / 533.333333f); - fprintf(pDirfile,"%s/%s %f,%f,%f_%f,%f,%f %f %d %d %d,%d %d\n", MapName, ModelInstName, diff --git a/modules/worldengine/deps/CMakeLists.txt b/modules/worldengine/deps/CMakeLists.txt index c64a15ca1c..30ce4fe1fc 100644 --- a/modules/worldengine/deps/CMakeLists.txt +++ b/modules/worldengine/deps/CMakeLists.txt @@ -20,6 +20,10 @@ if(CMAKE_SYSTEM_NAME MATCHES "Linux") if(SERVERS AND NOT NOJEM) add_subdirectory(jemalloc) endif() + if(TOOLS) + add_subdirectory(bzip2) + endif() + add_subdirectory(zlib) endif() if(CMAKE_SYSTEM_NAME MATCHES "Windows") diff --git a/modules/worldengine/deps/recastnavigation/Detour/CMakeLists.txt b/modules/worldengine/deps/recastnavigation/Detour/CMakeLists.txt index a0028f9c98..12be71d32b 100644 --- a/modules/worldengine/deps/recastnavigation/Detour/CMakeLists.txt +++ b/modules/worldengine/deps/recastnavigation/Detour/CMakeLists.txt @@ -1,4 +1,4 @@ -# Copyright (C) +# Copyright (C) 2008-2016 TrinityCore <http://www.trinitycore.org/> # # This file is free software; as a special exception the author gives # unlimited permission to copy and/or distribute it, with or without @@ -9,20 +9,25 @@ # implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. set(Detour_STAT_SRCS - DetourAlloc.cpp - DetourCommon.cpp - DetourNavMesh.cpp - DetourNavMeshBuilder.cpp - DetourNavMeshQuery.cpp - DetourNode.cpp + Source/DetourAlloc.cpp + Source/DetourCommon.cpp + Source/DetourNavMesh.cpp + Source/DetourNavMeshBuilder.cpp + Source/DetourNavMeshQuery.cpp + Source/DetourNode.cpp ) -if(WIN32) - include_directories( - ${CMAKE_SOURCE_DIR}/modules/worldengine/deps/zlib - ) -endif() - add_library(Detour STATIC ${Detour_STAT_SRCS}) -target_link_libraries(Detour ${ZLIB_LIBRARIES}) +target_include_directories(Detour + PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/Include) + +target_link_libraries(Detour + PUBLIC + zlib) + +set_target_properties(Detour + PROPERTIES + FOLDER + "dep") diff --git a/modules/worldengine/deps/recastnavigation/Detour/DetourAlloc.h b/modules/worldengine/deps/recastnavigation/Detour/DetourAlloc.h deleted file mode 100644 index 8693475419..0000000000 --- a/modules/worldengine/deps/recastnavigation/Detour/DetourAlloc.h +++ /dev/null @@ -1,36 +0,0 @@ -// -// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org -// -// This software is provided 'as-is', without any express or implied -// warranty. In no event will the authors be held liable for any damages -// arising from the use of this software. -// Permission is granted to anyone to use this software for any purpose, -// including commercial applications, and to alter it and redistribute it -// freely, subject to the following restrictions: -// 1. The origin of this software must not be misrepresented; you must not -// claim that you wrote the original software. If you use this software -// in a product, an acknowledgment in the product documentation would be -// appreciated but is not required. -// 2. Altered source versions must be plainly marked as such, and must not be -// misrepresented as being the original software. -// 3. This notice may not be removed or altered from any source distribution. -// - -#ifndef DETOURALLOCATOR_H -#define DETOURALLOCATOR_H - -enum dtAllocHint -{ - DT_ALLOC_PERM, // Memory persist after a function call. - DT_ALLOC_TEMP // Memory used temporarily within a function. -}; - -typedef void* (dtAllocFunc)(int size, dtAllocHint hint); -typedef void (dtFreeFunc)(void* ptr); - -void dtAllocSetCustom(dtAllocFunc *allocFunc, dtFreeFunc *freeFunc); - -void* dtAlloc(int size, dtAllocHint hint); -void dtFree(void* ptr); - -#endif diff --git a/modules/worldengine/deps/recastnavigation/Detour/DetourCommon.h b/modules/worldengine/deps/recastnavigation/Detour/DetourCommon.h deleted file mode 100644 index 3cee3f6351..0000000000 --- a/modules/worldengine/deps/recastnavigation/Detour/DetourCommon.h +++ /dev/null @@ -1,248 +0,0 @@ -// -// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org -// -// This software is provided 'as-is', without any express or implied -// warranty. In no event will the authors be held liable for any damages -// arising from the use of this software. -// Permission is granted to anyone to use this software for any purpose, -// including commercial applications, and to alter it and redistribute it -// freely, subject to the following restrictions: -// 1. The origin of this software must not be misrepresented; you must not -// claim that you wrote the original software. If you use this software -// in a product, an acknowledgment in the product documentation would be -// appreciated but is not required. -// 2. Altered source versions must be plainly marked as such, and must not be -// misrepresented as being the original software. -// 3. This notice may not be removed or altered from any source distribution. -// - -#ifndef DETOURCOMMON_H -#define DETOURCOMMON_H - -template<class T> inline void dtSwap(T& a, T& b) { T t = a; a = b; b = t; } -template<class T> inline T dtMin(T a, T b) { return a < b ? a : b; } -template<class T> inline T dtMax(T a, T b) { return a > b ? a : b; } -template<class T> inline T dtAbs(T a) { return a < 0 ? -a : a; } -template<class T> inline T dtSqr(T a) { return a*a; } -template<class T> inline T dtClamp(T v, T mn, T mx) { return v < mn ? mn : (v > mx ? mx : v); } - -float dtSqrt(float x); - -inline void dtVcross(float* dest, const float* v1, const float* v2) -{ - dest[0] = v1[1]*v2[2] - v1[2]*v2[1]; - dest[1] = v1[2]*v2[0] - v1[0]*v2[2]; - dest[2] = v1[0]*v2[1] - v1[1]*v2[0]; -} - -inline float dtVdot(const float* v1, const float* v2) -{ - return v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2]; -} - -inline void dtVmad(float* dest, const float* v1, const float* v2, const float s) -{ - dest[0] = v1[0]+v2[0]*s; - dest[1] = v1[1]+v2[1]*s; - dest[2] = v1[2]+v2[2]*s; -} - -inline void dtVlerp(float* dest, const float* v1, const float* v2, const float t) -{ - dest[0] = v1[0]+(v2[0]-v1[0])*t; - dest[1] = v1[1]+(v2[1]-v1[1])*t; - dest[2] = v1[2]+(v2[2]-v1[2])*t; -} - -inline void dtVadd(float* dest, const float* v1, const float* v2) -{ - dest[0] = v1[0]+v2[0]; - dest[1] = v1[1]+v2[1]; - dest[2] = v1[2]+v2[2]; -} - -inline void dtVsub(float* dest, const float* v1, const float* v2) -{ - dest[0] = v1[0]-v2[0]; - dest[1] = v1[1]-v2[1]; - dest[2] = v1[2]-v2[2]; -} - -inline void dtVscale(float* dest, const float* v, const float t) -{ - dest[0] = v[0]*t; - dest[1] = v[1]*t; - dest[2] = v[2]*t; -} - -inline void dtVmin(float* mn, const float* v) -{ - mn[0] = dtMin(mn[0], v[0]); - mn[1] = dtMin(mn[1], v[1]); - mn[2] = dtMin(mn[2], v[2]); -} - -inline void dtVmax(float* mx, const float* v) -{ - mx[0] = dtMax(mx[0], v[0]); - mx[1] = dtMax(mx[1], v[1]); - mx[2] = dtMax(mx[2], v[2]); -} - -inline void dtVset(float* dest, const float x, const float y, const float z) -{ - dest[0] = x; dest[1] = y; dest[2] = z; -} - -inline void dtVcopy(float* dest, const float* a) -{ - dest[0] = a[0]; - dest[1] = a[1]; - dest[2] = a[2]; -} - -inline float dtVlen(const float* v) -{ - return dtSqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]); -} - -inline float dtVlenSqr(const float* v) -{ - return v[0]*v[0] + v[1]*v[1] + v[2]*v[2]; -} - -inline float dtVdist(const float* v1, const float* v2) -{ - const float dx = v2[0] - v1[0]; - const float dy = v2[1] - v1[1]; - const float dz = v2[2] - v1[2]; - return dtSqrt(dx*dx + dy*dy + dz*dz); -} - -inline float dtVdistSqr(const float* v1, const float* v2) -{ - const float dx = v2[0] - v1[0]; - const float dy = v2[1] - v1[1]; - const float dz = v2[2] - v1[2]; - return dx*dx + dy*dy + dz*dz; -} - -inline float dtVdist2D(const float* v1, const float* v2) -{ - const float dx = v2[0] - v1[0]; - const float dz = v2[2] - v1[2]; - return dtSqrt(dx*dx + dz*dz); -} - -inline float dtVdist2DSqr(const float* v1, const float* v2) -{ - const float dx = v2[0] - v1[0]; - const float dz = v2[2] - v1[2]; - return dx*dx + dz*dz; -} - -inline void dtVnormalize(float* v) -{ - float d = 1.0f / dtSqrt(dtSqr(v[0]) + dtSqr(v[1]) + dtSqr(v[2])); - v[0] *= d; - v[1] *= d; - v[2] *= d; -} - -inline bool dtVequal(const float* p0, const float* p1) -{ - static const float thr = dtSqr(1.0f/16384.0f); - const float d = dtVdistSqr(p0, p1); - return d < thr; -} - -inline unsigned int dtNextPow2(unsigned int v) -{ - v--; - v |= v >> 1; - v |= v >> 2; - v |= v >> 4; - v |= v >> 8; - v |= v >> 16; - v++; - return v; -} - -inline unsigned int dtIlog2(unsigned int v) -{ - unsigned int r; - unsigned int shift; - r = (v > 0xffff) << 4; v >>= r; - shift = (v > 0xff) << 3; v >>= shift; r |= shift; - shift = (v > 0xf) << 2; v >>= shift; r |= shift; - shift = (v > 0x3) << 1; v >>= shift; r |= shift; - r |= (v >> 1); - return r; -} - -inline int dtAlign4(int x) { return (x+3) & ~3; } - -inline int dtOppositeTile(int side) { return (side+4) & 0x7; } - -inline float dtVdot2D(const float* u, const float* v) -{ - return u[0]*v[0] + u[2]*v[2]; -} - -inline float dtVperp2D(const float* u, const float* v) -{ - return u[2]*v[0] - u[0]*v[2]; -} - -inline float dtTriArea2D(const float* a, const float* b, const float* c) -{ - const float abx = b[0] - a[0]; - const float abz = b[2] - a[2]; - const float acx = c[0] - a[0]; - const float acz = c[2] - a[2]; - return acx*abz - abx*acz; -} - -inline bool dtOverlapQuantBounds(const unsigned short amin[3], const unsigned short amax[3], - const unsigned short bmin[3], const unsigned short bmax[3]) -{ - bool overlap = true; - overlap = (amin[0] > bmax[0] || amax[0] < bmin[0]) ? false : overlap; - overlap = (amin[1] > bmax[1] || amax[1] < bmin[1]) ? false : overlap; - overlap = (amin[2] > bmax[2] || amax[2] < bmin[2]) ? false : overlap; - return overlap; -} - -inline bool dtOverlapBounds(const float* amin, const float* amax, - const float* bmin, const float* bmax) -{ - bool overlap = true; - overlap = (amin[0] > bmax[0] || amax[0] < bmin[0]) ? false : overlap; - overlap = (amin[1] > bmax[1] || amax[1] < bmin[1]) ? false : overlap; - overlap = (amin[2] > bmax[2] || amax[2] < bmin[2]) ? false : overlap; - return overlap; -} - -void dtClosestPtPointTriangle(float* closest, const float* p, - const float* a, const float* b, const float* c); - -bool dtClosestHeightPointTriangle(const float* p, const float* a, const float* b, const float* c, float& h); - -bool dtIntersectSegmentPoly2D(const float* p0, const float* p1, - const float* verts, int nverts, - float& tmin, float& tmax, - int& segMin, int& segMax); - -bool dtPointInPolygon(const float* pt, const float* verts, const int nverts); - -bool dtDistancePtPolyEdgesSqr(const float* pt, const float* verts, const int nverts, - float* ed, float* et); - -float dtDistancePtSegSqr2D(const float* pt, const float* p, const float* q, float& t); - -void dtCalcPolyCenter(float* tc, const unsigned short* idx, int nidx, const float* verts); - -bool dtOverlapPolyPoly2D(const float* polya, const int npolya, - const float* polyb, const int npolyb); - -#endif // DETOURCOMMON_H diff --git a/modules/worldengine/deps/recastnavigation/Detour/DetourNavMesh.h b/modules/worldengine/deps/recastnavigation/Detour/DetourNavMesh.h deleted file mode 100644 index 6f2db04004..0000000000 --- a/modules/worldengine/deps/recastnavigation/Detour/DetourNavMesh.h +++ /dev/null @@ -1,428 +0,0 @@ -// -// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org -// -// This software is provided 'as-is', without any express or implied -// warranty. In no event will the authors be held liable for any damages -// arising from the use of this software. -// Permission is granted to anyone to use this software for any purpose, -// including commercial applications, and to alter it and redistribute it -// freely, subject to the following restrictions: -// 1. The origin of this software must not be misrepresented; you must not -// claim that you wrote the original software. If you use this software -// in a product, an acknowledgment in the product documentation would be -// appreciated but is not required. -// 2. Altered source versions must be plainly marked as such, and must not be -// misrepresented as being the original software. -// 3. This notice may not be removed or altered from any source distribution. -// - -#ifndef DETOURNAVMESH_H -#define DETOURNAVMESH_H - -#include "DetourAlloc.h" - -#ifdef WIN32 - typedef unsigned __int64 uint64; -#else -#include <stdint.h> -#ifndef uint64_t -#ifdef __linux__ -#include <linux/types.h> -#endif -#endif - typedef uint64_t uint64; -#endif - -// Note: If you want to use 64-bit refs, change the types of both dtPolyRef & dtTileRef. -// It is also recommended to change dtHashRef() to proper 64-bit hash too. - -// Reference to navigation polygon. -typedef uint64 dtPolyRef; - -// Reference to navigation mesh tile. -typedef uint64 dtTileRef; - -// Maximum number of vertices per navigation polygon. -static const int DT_VERTS_PER_POLYGON = 6; - -static const int DT_NAVMESH_MAGIC = 'D'<<24 | 'N'<<16 | 'A'<<8 | 'V'; //'DNAV'; -static const int DT_NAVMESH_VERSION = 6; - -static const int DT_NAVMESH_STATE_MAGIC = 'D'<<24 | 'N'<<16 | 'M'<<8 | 'S'; //'DNMS'; -static const int DT_NAVMESH_STATE_VERSION = 1; - -static const unsigned short DT_EXT_LINK = 0x8000; -static const unsigned int DT_NULL_LINK = 0xffffffff; -static const unsigned int DT_OFFMESH_CON_BIDIR = 1; - -static const int DT_MAX_AREAS = 64; - -static const int STATIC_SALT_BITS = 12; -static const int STATIC_TILE_BITS = 21; -static const int STATIC_POLY_BITS = 31; -// we cannot have over 31 bits for either tile nor poly -// without changing polyCount to use 64bits too. - -// Flags for addTile -enum dtTileFlags -{ - DT_TILE_FREE_DATA = 0x01, // Navmesh owns the tile memory and should free it. -}; - -// Flags returned by findStraightPath(). -enum dtStraightPathFlags -{ - DT_STRAIGHTPATH_START = 0x01, // The vertex is the start position. - DT_STRAIGHTPATH_END = 0x02, // The vertex is the end position. - DT_STRAIGHTPATH_OFFMESH_CONNECTION = 0x04, // The vertex is start of an off-mesh link. -}; - -// Flags describing polygon properties. -enum dtPolyTypes -{ - DT_POLYTYPE_GROUND = 0, // Regular ground polygons. - DT_POLYTYPE_OFFMESH_CONNECTION = 1, // Off-mesh connections. -}; - -enum dtStatus -{ - DT_FAILURE = 0, // Operation failed. - DT_FAILURE_DATA_MAGIC, - DT_FAILURE_DATA_VERSION, - DT_FAILURE_OUT_OF_MEMORY, - DT_SUCCESS, // Operation succeed. - DT_IN_PROGRESS, // Operation still in progress. -}; - - -// Structure describing the navigation polygon data. -struct dtPoly -{ - unsigned int firstLink; // Index to first link in linked list. - unsigned short verts[DT_VERTS_PER_POLYGON]; // Indices to vertices of the poly. - unsigned short neis[DT_VERTS_PER_POLYGON]; // Refs to neighbours of the poly. - unsigned short flags; // Flags (see dtPolyFlags). - unsigned char vertCount; // Number of vertices. - unsigned char areaAndtype; // Bit packed: Area ID of the polygon, and Polygon type, see dtPolyTypes.. - inline void setArea(unsigned char a) { areaAndtype = (areaAndtype & 0xc0) | (a & 0x3f); } - inline void setType(unsigned char t) { areaAndtype = (areaAndtype & 0x3f) | (t << 6); } - inline unsigned char getArea() const { return areaAndtype & 0x3f; } - inline unsigned char getType() const { return areaAndtype >> 6; } -}; - -// Stucture describing polygon detail triangles. -struct dtPolyDetail -{ - unsigned int vertBase; // Offset to detail vertex array. - unsigned int triBase; // Offset to detail triangle array. - unsigned char vertCount; // Number of vertices in the detail mesh. - unsigned char triCount; // Number of triangles. -}; - -// Stucture describing a link to another polygon. -struct dtLink -{ - dtPolyRef ref; // Neighbour reference. - unsigned int next; // Index to next link. - unsigned char edge; // Index to polygon edge which owns this link. - unsigned char side; // If boundary link, defines on which side the link is. - unsigned char bmin, bmax; // If boundary link, defines the sub edge area. -}; - -struct dtBVNode -{ - unsigned short bmin[3], bmax[3]; // BVnode bounds - int i; // Index to item or if negative, escape index. -}; - -struct dtOffMeshConnection -{ - float pos[6]; // Both end point locations. - float rad; // Link connection radius. - unsigned short poly; // Poly Id - unsigned char flags; // Link flags - unsigned char side; // End point side. - unsigned int userId; // User ID to identify this connection. -}; - -struct dtMeshHeader -{ - int magic; // Magic number, used to identify the data. - int version; // Data version number. - int x, y; // Location of the time on the grid. - unsigned int userId; // User ID of the tile. - int polyCount; // Number of polygons in the tile. - int vertCount; // Number of vertices in the tile. - int maxLinkCount; // Number of allocated links. - int detailMeshCount; // Number of detail meshes. - int detailVertCount; // Number of detail vertices. - int detailTriCount; // Number of detail triangles. - int bvNodeCount; // Number of BVtree nodes. - int offMeshConCount; // Number of Off-Mesh links. - int offMeshBase; // Index to first polygon which is Off-Mesh link. - float walkableHeight; // Height of the agent. - float walkableRadius; // Radius of the agent - float walkableClimb; // Max climb height of the agent. - float bmin[3], bmax[3]; // Bounding box of the tile. - float bvQuantFactor; // BVtree quantization factor (world to bvnode coords) -}; - -struct dtMeshTile -{ - unsigned int salt; // Counter describing modifications to the tile. - - unsigned int linksFreeList; // Index to next free link. - dtMeshHeader* header; // Pointer to tile header. - dtPoly* polys; // Pointer to the polygons (will be updated when tile is added). - float* verts; // Pointer to the vertices (will be updated when tile added). - dtLink* links; // Pointer to the links (will be updated when tile added). - dtPolyDetail* detailMeshes; // Pointer to detail meshes (will be updated when tile added). - float* detailVerts; // Pointer to detail vertices (will be updated when tile added). - unsigned char* detailTris; // Pointer to detail triangles (will be updated when tile added). - dtBVNode* bvTree; // Pointer to BVtree nodes (will be updated when tile added). - dtOffMeshConnection* offMeshCons; // Pointer to Off-Mesh links. (will be updated when tile added). - - unsigned char* data; // Pointer to tile data. - int dataSize; // Size of the tile data. - int flags; // Tile flags, see dtTileFlags. - dtMeshTile* next; // Next free tile or, next tile in spatial grid. -}; - -struct dtNavMeshParams -{ - float orig[3]; // Origin of the nav mesh tile space. - float tileWidth, tileHeight; // Width and height of each tile. - int maxTiles; // Maximum number of tiles the navmesh can contain. - int maxPolys; // Maximum number of polygons each tile can contain. -}; - - -class dtNavMesh -{ -public: - dtNavMesh(); - ~dtNavMesh(); - - // Initializes the nav mesh for tiled use. - // Params: - // params - (in) navmesh initialization params, see dtNavMeshParams. - // Returns: True if succeed, else false. - dtStatus init(const dtNavMeshParams* params); - - // Initializes the nav mesh for single tile use. - // Params: - // data - (in) Data of the new tile mesh. - // dataSize - (in) Data size of the new tile mesh. - // flags - (in) Tile flags, see dtTileFlags. - // Returns: True if succeed, else false. - dtStatus init(unsigned char* data, const int dataSize, const int flags); - - // Returns pointer to navmesh initialization params. - const dtNavMeshParams* getParams() const; - - // Adds new tile into the navmesh. - // The add will fail if the data is in wrong format, - // there is not enough tiles left, or if there is a tile already at the location. - // Params: - // data - (in) Data of the new tile mesh. - // dataSize - (in) Data size of the new tile mesh. - // flags - (in) Tile flags, see dtTileFlags. - // lastRef - (in,optional) Last tile ref, the tile will be restored so that - // the reference (as well as poly references) will be the same. Default: 0. - // result - (out,optional) tile ref if the tile was succesfully added. - dtStatus addTile(unsigned char* data, int dataSize, int flags, dtTileRef lastRef, dtTileRef* result); - - // Removes specified tile. - // Params: - // ref - (in) Reference to the tile to remove. - // data - (out) Data associated with deleted tile. - // dataSize - (out) Size of the data associated with deleted tile. - dtStatus removeTile(dtTileRef ref, unsigned char** data, int* dataSize); - - // Calculates tile location based in input world position. - // Params: - // pos - (in) world position of the query. - // tx - (out) tile x location. - // ty - (out) tile y location. - void calcTileLoc(const float* pos, int* tx, int* ty) const; - - // Returns pointer to tile at specified location. - // Params: - // x,y - (in) Location of the tile to get. - // Returns: pointer to tile if tile exists or 0 tile does not exists. - const dtMeshTile* getTileAt(int x, int y) const; - - // Returns reference to tile at specified location. - // Params: - // x,y - (in) Location of the tile to get. - // Returns: reference to tile if tile exists or 0 tile does not exists. - dtTileRef getTileRefAt(int x, int y) const; - - // Returns tile references of a tile based on tile pointer. - dtTileRef getTileRef(const dtMeshTile* tile) const; - - // Returns tile based on references. - const dtMeshTile* getTileByRef(dtTileRef ref) const; - - // Returns max number of tiles. - int getMaxTiles() const; - - // Returns pointer to tile in the tile array. - // Params: - // i - (in) Index to the tile to retrieve, max index is getMaxTiles()-1. - // Returns: Pointer to specified tile. - const dtMeshTile* getTile(int i) const; - - // Returns pointer to tile and polygon pointed by the polygon reference. - // Params: - // ref - (in) reference to a polygon. - // tile - (out) pointer to the tile containing the polygon. - // poly - (out) pointer to the polygon. - dtStatus getTileAndPolyByRef(const dtPolyRef ref, const dtMeshTile** tile, const dtPoly** poly) const; - - // Returns pointer to tile and polygon pointed by the polygon reference. - // Note: this function does not check if 'ref' s valid, and is thus faster. Use only with valid refs! - // Params: - // ref - (in) reference to a polygon. - // tile - (out) pointer to the tile containing the polygon. - // poly - (out) pointer to the polygon. - void getTileAndPolyByRefUnsafe(const dtPolyRef ref, const dtMeshTile** tile, const dtPoly** poly) const; - - // Returns true if polygon reference points to valid data. - bool isValidPolyRef(dtPolyRef ref) const; - - // Returns base poly id for specified tile, polygon refs can be deducted from this. - dtPolyRef getPolyRefBase(const dtMeshTile* tile) const; - - // Returns start and end location of an off-mesh link polygon. - // Params: - // prevRef - (in) ref to the polygon before the link (used to select direction). - // polyRef - (in) ref to the off-mesh link polygon. - // startPos[3] - (out) start point of the link. - // endPos[3] - (out) end point of the link. - // Returns: true if link is found. - dtStatus getOffMeshConnectionPolyEndPoints(dtPolyRef prevRef, dtPolyRef polyRef, float* startPos, float* endPos) const; - - // Returns pointer to off-mesh connection based on polyref, or null if ref not valid. - const dtOffMeshConnection* getOffMeshConnectionByRef(dtPolyRef ref) const; - - // Sets polygon flags. - dtStatus setPolyFlags(dtPolyRef ref, unsigned short flags); - - // Return polygon flags. - dtStatus getPolyFlags(dtPolyRef ref, unsigned short* resultFlags) const; - - // Set polygon type. - dtStatus setPolyArea(dtPolyRef ref, unsigned char area); - - // Return polygon area type. - dtStatus getPolyArea(dtPolyRef ref, unsigned char* resultArea) const; - - - // Returns number of bytes required to store tile state. - int getTileStateSize(const dtMeshTile* tile) const; - - // Stores tile state to buffer. - dtStatus storeTileState(const dtMeshTile* tile, unsigned char* data, const int maxDataSize) const; - - // Restores tile state. - dtStatus restoreTileState(dtMeshTile* tile, const unsigned char* data, const int maxDataSize); - - - // Encodes a tile id. - inline dtPolyRef encodePolyId(unsigned int salt, unsigned int it, unsigned int ip) const - { - return ((dtPolyRef)salt << (m_polyBits+m_tileBits)) | ((dtPolyRef)it << m_polyBits) | (dtPolyRef)ip; - } - - // Decodes a tile id. - inline void decodePolyId(dtPolyRef ref, unsigned int& salt, unsigned int& it, unsigned int& ip) const - { - const dtPolyRef saltMask = ((dtPolyRef)1<<m_saltBits)-1; - const dtPolyRef tileMask = ((dtPolyRef)1<<m_tileBits)-1; - const dtPolyRef polyMask = ((dtPolyRef)1<<m_polyBits)-1; - salt = (unsigned int)((ref >> (m_polyBits+m_tileBits)) & saltMask); - it = (unsigned int)((ref >> m_polyBits) & tileMask); - ip = (unsigned int)(ref & polyMask); - } - - // Decodes a tile salt. - inline unsigned int decodePolyIdSalt(dtPolyRef ref) const - { - const dtPolyRef saltMask = ((dtPolyRef)1<<m_saltBits)-1; - return (unsigned int)((ref >> (m_polyBits+m_tileBits)) & saltMask); - } - - // Decodes a tile id. - inline unsigned int decodePolyIdTile(dtPolyRef ref) const - { - const dtPolyRef tileMask = ((dtPolyRef)1<<m_tileBits)-1; - return (unsigned int)((ref >> m_polyBits) & tileMask); - } - - // Decodes a poly id. - inline unsigned int decodePolyIdPoly(dtPolyRef ref) const - { - const dtPolyRef polyMask = ((dtPolyRef)1<<m_polyBits)-1; - return (unsigned int)(ref & polyMask); - } - -private: - - // Returns pointer to tile in the tile array. - dtMeshTile* getTile(int i); - - // Returns neighbour tile based on side. - dtMeshTile* getNeighbourTileAt(int x, int y, int side) const; - // Returns all polygons in neighbour tile based on portal defined by the segment. - int findConnectingPolys(const float* va, const float* vb, - const dtMeshTile* tile, int side, - dtPolyRef* con, float* conarea, int maxcon) const; - - // Builds internal polygons links for a tile. - void connectIntLinks(dtMeshTile* tile); - // Builds internal polygons links for a tile. - void connectIntOffMeshLinks(dtMeshTile* tile); - - // Builds external polygon links for a tile. - void connectExtLinks(dtMeshTile* tile, dtMeshTile* target, int side); - // Builds external polygon links for a tile. - void connectExtOffMeshLinks(dtMeshTile* tile, dtMeshTile* target, int side); - - // Removes external links at specified side. - void unconnectExtLinks(dtMeshTile* tile, int side); - - - // TODO: These methods are duplicates from dtNavMeshQuery, but are needed for off-mesh connection finding. - - // Queries polygons within a tile. - int queryPolygonsInTile(const dtMeshTile* tile, const float* qmin, const float* qmax, - dtPolyRef* polys, const int maxPolys) const; - // Find nearest polygon within a tile. - dtPolyRef findNearestPolyInTile(const dtMeshTile* tile, const float* center, - const float* extents, float* nearestPt) const; - // Returns closest point on polygon. - dtStatus closestPointOnPolyInTile(const dtMeshTile* tile, unsigned int ip, - const float* pos, float* closest) const; - - dtNavMeshParams m_params; // Current initialization params. TODO: do not store this info twice. - float m_orig[3]; // Origin of the tile (0,0) - float m_tileWidth, m_tileHeight; // Dimensions of each tile. - int m_maxTiles; // Max number of tiles. - int m_tileLutSize; // Tile hash lookup size (must be pot). - int m_tileLutMask; // Tile hash lookup mask. - - dtMeshTile** m_posLookup; // Tile hash lookup. - dtMeshTile* m_nextFree; // Freelist of tiles. - dtMeshTile* m_tiles; // List of tiles. - - unsigned int m_saltBits; // Number of salt bits in the tile ID. - unsigned int m_tileBits; // Number of tile bits in the tile ID. - unsigned int m_polyBits; // Number of poly bits in the tile ID. -}; - -// Helper function to allocate navmesh class using Detour allocator. -dtNavMesh* dtAllocNavMesh(); -void dtFreeNavMesh(dtNavMesh* navmesh); - -#endif // DETOURNAVMESH_H diff --git a/modules/worldengine/deps/recastnavigation/Detour/DetourNavMeshBuilder.h b/modules/worldengine/deps/recastnavigation/Detour/DetourNavMeshBuilder.h deleted file mode 100644 index aa802d71cb..0000000000 --- a/modules/worldengine/deps/recastnavigation/Detour/DetourNavMeshBuilder.h +++ /dev/null @@ -1,79 +0,0 @@ -// -// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org -// -// This software is provided 'as-is', without any express or implied -// warranty. In no event will the authors be held liable for any damages -// arising from the use of this software. -// Permission is granted to anyone to use this software for any purpose, -// including commercial applications, and to alter it and redistribute it -// freely, subject to the following restrictions: -// 1. The origin of this software must not be misrepresented; you must not -// claim that you wrote the original software. If you use this software -// in a product, an acknowledgment in the product documentation would be -// appreciated but is not required. -// 2. Altered source versions must be plainly marked as such, and must not be -// misrepresented as being the original software. -// 3. This notice may not be removed or altered from any source distribution. -// - -#ifndef DETOURNAVMESHBUILDER_H -#define DETOURNAVMESHBUILDER_H - -#include "DetourAlloc.h" - - -// The units of the parameters are specified in parenthesis as follows: -// (vx) voxels, (wu) world units -struct dtNavMeshCreateParams -{ - // Navmesh vertices. - const unsigned short* verts; // Array of vertices, each vertex has 3 components. (vx). - int vertCount; // Vertex count - // Navmesh polygons - const unsigned short* polys; // Array of polygons, uses same format as rcPolyMesh. - const unsigned short* polyFlags; // Array of flags per polygon. - const unsigned char* polyAreas; // Array of area ids per polygon. - int polyCount; // Number of polygons - int nvp; // Number of verts per polygon. - // Navmesh Detail - const unsigned int* detailMeshes; // Detail meshes, uses same format as rcPolyMeshDetail. - const float* detailVerts; // Detail mesh vertices, uses same format as rcPolyMeshDetail (wu). - int detailVertsCount; // Total number of detail vertices - const unsigned char* detailTris; // Array of detail tris per detail mesh. - int detailTriCount; // Total number of detail triangles. - // Off-Mesh Connections. - const float* offMeshConVerts; // Off-mesh connection vertices (wu). - const float* offMeshConRad; // Off-mesh connection radii (wu). - const unsigned short* offMeshConFlags; // Off-mesh connection flags. - const unsigned char* offMeshConAreas; // Off-mesh connection area ids. - const unsigned char* offMeshConDir; // Off-mesh connection direction flags (1 = bidir, 0 = oneway). - const unsigned int* offMeshConUserID; // Off-mesh connection user id (optional). - int offMeshConCount; // Number of off-mesh connections - // Tile location - unsigned int userId; // User ID bound to the tile. - int tileX, tileY; // Tile location (tile coords). - float bmin[3], bmax[3]; // Tile bounds (wu). - // Settings - float walkableHeight; // Agent height (wu). - float walkableRadius; // Agent radius (wu). - float walkableClimb; // Agent max climb (wu). - float cs; // Cell size (xz) (wu). - float ch; // Cell height (y) (wu). - int tileSize; // Tile size (width & height) (vx). - int tileLayer; - bool buildBvTree; -}; - -// Build navmesh data from given input data. -bool dtCreateNavMeshData(dtNavMeshCreateParams* params, unsigned char** outData, int* outDataSize); - -// Swaps endianess of navmesh header. -bool dtNavMeshHeaderSwapEndian(unsigned char* data, const int dataSize); - -// Swaps endianess of the navmesh data. This function assumes that the header is in correct -// endianess already. Call dtNavMeshHeaderSwapEndian() first on the data if the data is -// assumed to be in wrong endianess to start with. If converting from native endianess to foreign, -// call dtNavMeshHeaderSwapEndian() after the data has been swapped. -bool dtNavMeshDataSwapEndian(unsigned char* data, const int dataSize); - -#endif // DETOURNAVMESHBUILDER_H diff --git a/modules/worldengine/deps/recastnavigation/Detour/DetourNavMeshQuery.h b/modules/worldengine/deps/recastnavigation/Detour/DetourNavMeshQuery.h deleted file mode 100644 index f5046d8329..0000000000 --- a/modules/worldengine/deps/recastnavigation/Detour/DetourNavMeshQuery.h +++ /dev/null @@ -1,407 +0,0 @@ -// -// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org -// -// This software is provided 'as-is', without any express or implied -// warranty. In no event will the authors be held liable for any damages -// arising from the use of this software. -// Permission is granted to anyone to use this software for any purpose, -// including commercial applications, and to alter it and redistribute it -// freely, subject to the following restrictions: -// 1. The origin of this software must not be misrepresented; you must not -// claim that you wrote the original software. If you use this software -// in a product, an acknowledgment in the product documentation would be -// appreciated but is not required. -// 2. Altered source versions must be plainly marked as such, and must not be -// misrepresented as being the original software. -// 3. This notice may not be removed or altered from any source distribution. -// - -#ifndef DETOURNAVMESHQUERY_H -#define DETOURNAVMESHQUERY_H - -#include "DetourNavMesh.h" - - -// Define DT_VIRTUAL_QUERYFILTER if you wish to derive a custom filter from dtQueryFilter. -// On certain platforms indirect or virtual function call is expensive. The default -// setting is to use non-virtual functions, the actualy implementations of the functions -// are declared as inline for maximum speed. - -//#define DT_VIRTUAL_QUERYFILTER 1 - -// Class for polygon filtering and cost calculation during query operations. -// - It is possible to derive a custom query filter from dtQueryFilter by overriding -// the virtual functions passFilter() and getCost(). -// - Both functions should be as fast as possible. Use cached local copy of data -// instead of accessing your own objects where possible. -// - You do not need to adhere to the flags and cost logic provided by the default -// implementation. -// - In order for the A* to work properly, the cost should be proportional to -// the travel distance. Using cost modifier less than 1.0 is likely to lead -// to problems during pathfinding. -class dtQueryFilter -{ - float m_areaCost[DT_MAX_AREAS]; // Array storing cost per area type, used by default implementation. - unsigned short m_includeFlags; // Include poly flags, used by default implementation. - unsigned short m_excludeFlags; // Exclude poly flags, used by default implementation. - -public: - dtQueryFilter(); - - // Returns true if the polygon is can visited. - // Params: - // ref - (in) reference to the polygon test. - // tile - (in) pointer to the tile of the polygon test. - // poly - (in) pointer to the polygon test. -#ifdef DT_VIRTUAL_QUERYFILTER - virtual bool passFilter(const dtPolyRef ref, - const dtMeshTile* tile, - const dtPoly* poly) const; -#else - bool passFilter(const dtPolyRef ref, - const dtMeshTile* tile, - const dtPoly* poly) const; -#endif - - // Returns cost to travel from 'pa' to 'pb'.' - // The segment is fully contained inside 'cur'. - // 'pa' lies on the edge between 'prev' and 'cur', - // 'pb' lies on the edge between 'cur' and 'next'. - // Params: - // pa - (in) segment start position. - // pb - (in) segment end position. - // prevRef, prevTile, prevPoly - (in) data describing the previous polygon, can be null. - // curRef, curTile, curPoly - (in) data describing the current polygon. - // nextRef, nextTile, nextPoly - (in) data describing the next polygon, can be null. -#ifdef DT_VIRTUAL_QUERYFILTER - virtual float getCost(const float* pa, const float* pb, - const dtPolyRef prevRef, const dtMeshTile* prevTile, const dtPoly* prevPoly, - const dtPolyRef curRef, const dtMeshTile* curTile, const dtPoly* curPoly, - const dtPolyRef nextRef, const dtMeshTile* nextTile, const dtPoly* nextPoly) const; -#else - float getCost(const float* pa, const float* pb, - const dtPolyRef prevRef, const dtMeshTile* prevTile, const dtPoly* prevPoly, - const dtPolyRef curRef, const dtMeshTile* curTile, const dtPoly* curPoly, - const dtPolyRef nextRef, const dtMeshTile* nextTile, const dtPoly* nextPoly) const; -#endif - - // Getters and setters for the default implementation data. - inline float getAreaCost(const int i) const { return m_areaCost[i]; } - inline void setAreaCost(const int i, const float cost) { m_areaCost[i] = cost; } - - inline unsigned short getIncludeFlags() const { return m_includeFlags; } - inline void setIncludeFlags(const unsigned short flags) { m_includeFlags = flags; } - - inline unsigned short getExcludeFlags() const { return m_excludeFlags; } - inline void setExcludeFlags(const unsigned short flags) { m_excludeFlags = flags; } -}; - -class dtNavMeshQuery -{ -public: - dtNavMeshQuery(); - ~dtNavMeshQuery(); - - // Initializes the nav mesh query. - // Params: - // nav - (in) pointer to navigation mesh data. - // maxNodes - (in) Maximum number of search nodes to use (max 65536). - // Returns: True if succeed, else false. - dtStatus init(const dtNavMesh* nav, const int maxNodes); - - // Finds the nearest navigation polygon around the center location. - // Params: - // center[3] - (in) The center of the search box. - // extents[3] - (in) The extents of the search box. - // filter - (in) path polygon filter. - // nearestRef - (out) Reference to the nearest polygon. - // nearestPt[3] - (out, opt) The nearest point on found polygon, null if not needed. - // Returns: Reference identifier for the polygon, or 0 if no polygons found. - dtStatus findNearestPoly(const float* center, const float* extents, - const dtQueryFilter* filter, - dtPolyRef* nearestRef, float* nearestPt) const; - - // Returns polygons which overlap the query box. - // Params: - // center[3] - (in) the center of the search box. - // extents[3] - (in) the extents of the search box. - // filter - (in) path polygon filter. - // polys - (out) array holding the search result. - // polyCount - (out) Number of polygons in search result array. - // maxPolys - (in) The max number of polygons the polys array can hold. - dtStatus queryPolygons(const float* center, const float* extents, - const dtQueryFilter* filter, - dtPolyRef* polys, int* polyCount, const int maxPolys) const; - - // Finds path from start polygon to end polygon. - // If target polygon canno be reached through the navigation graph, - // the last node on the array is nearest node to the end polygon. - // Start end end positions are needed to calculate more accurate - // traversal cost at start end end polygons. - // Params: - // startRef - (in) ref to path start polygon. - // endRef - (in) ref to path end polygon. - // startPos[3] - (in) Path start location. - // endPos[3] - (in) Path end location. - // filter - (in) path polygon filter. - // path - (out) array holding the search result. - // pathCount - (out) Number of polygons in search result array. - // maxPath - (in) The max number of polygons the path array can hold. Must be at least 1. - dtStatus findPath(dtPolyRef startRef, dtPolyRef endRef, - const float* startPos, const float* endPos, - const dtQueryFilter* filter, - dtPolyRef* path, int* pathCount, const int maxPath) const; - - // Intializes sliced path find query. - // Note 1: calling any other dtNavMeshQuery method before calling findPathEnd() - // may results in corrupted data! - // Note 2: The pointer to filter is store, and used in subsequent - // calls to updateSlicedFindPath(). - // Params: - // startRef - (in) ref to path start polygon. - // endRef - (in) ref to path end polygon. - // startPos[3] - (in) Path start location. - // endPos[3] - (in) Path end location. - // filter - (in) path polygon filter. - dtStatus initSlicedFindPath(dtPolyRef startRef, dtPolyRef endRef, - const float* startPos, const float* endPos, - const dtQueryFilter* filter); - - // Updates sliced path find query. - // Params: - // maxIter - (in) max number of iterations to update. - // Returns: Path query state. - dtStatus updateSlicedFindPath(const int maxIter); - - // Finalizes sliced path find query and returns found path. - // path - (out) array holding the search result. - // pathCount - (out) Number of polygons in search result array. - // maxPath - (in) The max number of polygons the path array can hold. - dtStatus finalizeSlicedFindPath(dtPolyRef* path, int* pathCount, const int maxPath); - - // Finalizes partial sliced path find query and returns path to the furthest - // polygon on the existing path that was visited during the search. - // existing - (out) Array of polygons in the existing path. - // existingSize - (out) Number of polygons in existing path array. - // path - (out) array holding the search result. - // pathCount - (out) Number of polygons in search result array. - // maxPath - (in) The max number of polygons the path array can hold. - dtStatus finalizeSlicedFindPathPartial(const dtPolyRef* existing, const int existingSize, - dtPolyRef* path, int* pathCount, const int maxPath); - - // Finds a straight path from start to end locations within the corridor - // described by the path polygons. - // Start and end locations will be clamped on the corridor. - // The returned polygon references are point to polygon which was entered when - // a path point was added. For the end point, zero will be returned. This allows - // to match for example off-mesh link points to their representative polygons. - // Params: - // startPos[3] - (in) Path start location. - // endPo[3] - (in) Path end location. - // path - (in) Array of connected polygons describing the corridor. - // pathSize - (in) Number of polygons in path array. - // straightPath - (out) Points describing the straight path. - // straightPathFlags - (out, opt) Flags describing each point type, see dtStraightPathFlags. - // straightPathRefs - (out, opt) References to polygons at point locations. - // straightPathCount - (out) Number of points in the path. - // maxStraightPath - (in) The max number of points the straight path array can hold. Must be at least 1. - dtStatus findStraightPath(const float* startPos, const float* endPos, - const dtPolyRef* path, const int pathSize, - float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs, - int* straightPathCount, const int maxStraightPath) const; - - // Moves from startPos to endPos constrained to the navmesh. - // If the endPos is reachable, the resultPos will be endPos, - // or else the resultPos will be the nearest point in navmesh. - // Note: The resulting point is not projected to the ground, use getPolyHeight() to get height. - // Note: The algorithm is optimized for small delta movement and small number of polygons. - // Params: - // startRef - (in) ref to the polygon where startPos lies. - // startPos[3] - (in) start position of the mover. - // endPos[3] - (in) desired end position of the mover. - // filter - (in) path polygon filter. - // resultPos[3] - (out) new position of the mover. - // visited - (out) array of visited polygons. - // visitedCount - (out) Number of entries in the visited array. - // maxVisitedSize - (in) max number of polygons in the visited array. - dtStatus moveAlongSurface(dtPolyRef startRef, const float* startPos, const float* endPos, - const dtQueryFilter* filter, - float* resultPos, dtPolyRef* visited, int* visitedCount, const int maxVisitedSize) const; - - // Casts 'walkability' ray along the navmesh surface from startPos towards the endPos. - // Params: - // startRef - (in) ref to the polygon where the start lies. - // startPos[3] - (in) start position of the query. - // endPos[3] - (in) end position of the query. - // t - (out) hit parameter along the segment, FLT_MAX if no hit. - // hitNormal[3] - (out) normal of the nearest hit. - // filter - (in) path polygon filter. - // path - (out,opt) visited path polygons. - // pathCount - (out,opt) Number of polygons visited. - // maxPath - (in) max number of polygons in the path array. - dtStatus raycast(dtPolyRef startRef, const float* startPos, const float* endPos, - const dtQueryFilter* filter, - float* t, float* hitNormal, dtPolyRef* path, int* pathCount, const int maxPath) const; - - // Returns distance to nearest wall from the specified location. - // Params: - // startRef - (in) ref to the polygon where the center lies. - // centerPos[3] - (in) center if the query circle. - // maxRadius - (in) max search radius. - // filter - (in) path polygon filter. - // hitDist - (out) distance to nearest wall from the test location. - // hitPos[3] - (out) location of the nearest hit. - // hitNormal[3] - (out) normal of the nearest hit. - dtStatus findDistanceToWall(dtPolyRef startRef, const float* centerPos, const float maxRadius, - const dtQueryFilter* filter, - float* hitDist, float* hitPos, float* hitNormal) const; - - // Finds polygons found along the navigation graph which touch the specified circle. - // Params: - // startRef - (in) ref to the polygon where the search starts. - // centerPos[3] - (in) center if the query circle. - // radius - (in) radius of the query circle. - // filter - (in) path polygon filter. - // resultRef - (out, opt) refs to the polygons touched by the circle. - // resultParent - (out, opt) parent of each result polygon. - // resultCost - (out, opt) search cost at each result polygon. - // resultCount - (out, opt) Number of results. - // maxResult - (int) maximum capacity of search results. - dtStatus findPolysAroundCircle(dtPolyRef startRef, const float* centerPos, const float radius, - const dtQueryFilter* filter, - dtPolyRef* resultRef, dtPolyRef* resultParent, float* resultCost, - int* resultCount, const int maxResult) const; - - // Finds polygons found along the navigation graph which touch the convex polygon shape. - // Params: - // startRef - (in) ref to the polygon where the search starts. - // verts[3*n] - (in) vertices describing convex polygon shape (CCW). - // nverts - (in) number of vertices in the polygon. - // filter - (in) path polygon filter. - // resultRef - (out, opt) refs to the polygons touched by the circle. - // resultParent - (out, opt) parent of each result polygon. - // resultCost - (out, opt) search cost at each result polygon. - // resultCount - (out) number of results. - // maxResult - (int) maximum capacity of search results. - dtStatus findPolysAroundShape(dtPolyRef startRef, const float* verts, const int nverts, - const dtQueryFilter* filter, - dtPolyRef* resultRef, dtPolyRef* resultParent, float* resultCost, - int* resultCount, const int maxResult) const; - - // Finds non-overlapping local neighbourhood around center location. - // Note: The algorithm is optimized for small query radius and small number of polygons. - // Params: - // startRef - (in) ref to the polygon where the search starts. - // centerPos[3] - (in) center if the query circle. - // radius - (in) radius of the query circle. - // filter - (in) path polygon filter. - // resultRef - (out) refs to the polygons touched by the circle. - // resultParent - (out, opt) parent of each result polygon. - // resultCount - (out) number of results. - // maxResult - (int) maximum capacity of search results. - dtStatus findLocalNeighbourhood(dtPolyRef startRef, const float* centerPos, const float radius, - const dtQueryFilter* filter, - dtPolyRef* resultRef, dtPolyRef* resultParent, - int* resultCount, const int maxResult) const; - - // Returns wall segments of specified polygon. - // Params: - // ref - (in) ref to the polygon. - // filter - (in) path polygon filter. - // segments[6*maxSegments] - (out) wall segments (2 endpoints per segment). - // segmentCount - (out) number of wall segments. - // maxSegments - (in) max number of segments that can be stored in 'segments'. - dtStatus getPolyWallSegments(dtPolyRef ref, const dtQueryFilter* filter, - float* segments, int* segmentCount, const int maxSegments) const; - - // Returns closest point on navigation polygon. - // Uses detail polygons to find the closest point to the navigation polygon surface. - // Params: - // ref - (in) ref to the polygon. - // pos[3] - (in) the point to check. - // closest[3] - (out) closest point. - // Returns: true if closest point found. - dtStatus closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest) const; - - // Returns closest point on navigation polygon boundary. - // Uses the navigation polygon boundary to snap the point to poly boundary - // if it is outside the polygon. Much faster than closestPointToPoly. Does not affect height. - // Params: - // ref - (in) ref to the polygon. - // pos[3] - (in) the point to check. - // closest[3] - (out) closest point. - // Returns: true if closest point found. - dtStatus closestPointOnPolyBoundary(dtPolyRef ref, const float* pos, float* closest) const; - - // Returns start and end location of an off-mesh link polygon. - // Params: - // prevRef - (in) ref to the polygon before the link (used to select direction). - // polyRef - (in) ref to the off-mesh link polygon. - // startPos[3] - (out) start point of the link. - // endPos[3] - (out) end point of the link. - // Returns: true if link is found. - dtStatus getOffMeshConnectionPolyEndPoints(dtPolyRef prevRef, dtPolyRef polyRef, float* startPos, float* endPos) const; - - // Returns height of the polygon at specified location. - // Params: - // ref - (in) ref to the polygon. - // pos[3] - (in) the point where to locate the height. - // height - (out) height at the location. - // Returns: true if over polygon. - dtStatus getPolyHeight(dtPolyRef ref, const float* pos, float* height) const; - - // Returns true if poly reference ins in closed list. - bool isInClosedList(dtPolyRef ref) const; - - class dtNodePool* getNodePool() const { return m_nodePool; } - -private: - - // Returns neighbour tile based on side. - dtMeshTile* getNeighbourTileAt(int x, int y, int side) const; - - // Queries polygons within a tile. - int queryPolygonsInTile(const dtMeshTile* tile, const float* qmin, const float* qmax, const dtQueryFilter* filter, - dtPolyRef* polys, const int maxPolys) const; - // Find nearest polygon within a tile. - dtPolyRef findNearestPolyInTile(const dtMeshTile* tile, const float* center, const float* extents, - const dtQueryFilter* filter, float* nearestPt) const; - // Returns closest point on polygon. - dtStatus closestPointOnPolyInTile(const dtMeshTile* tile, const dtPoly* poly, const float* pos, float* closest) const; - - // Returns portal points between two polygons. - dtStatus getPortalPoints(dtPolyRef from, dtPolyRef to, float* left, float* right, - unsigned char& fromType, unsigned char& toType) const; - dtStatus getPortalPoints(dtPolyRef from, const dtPoly* fromPoly, const dtMeshTile* fromTile, - dtPolyRef to, const dtPoly* toPoly, const dtMeshTile* toTile, - float* left, float* right) const; - - // Returns edge mid point between two polygons. - dtStatus getEdgeMidPoint(dtPolyRef from, dtPolyRef to, float* mid) const; - dtStatus getEdgeMidPoint(dtPolyRef from, const dtPoly* fromPoly, const dtMeshTile* fromTile, - dtPolyRef to, const dtPoly* toPoly, const dtMeshTile* toTile, - float* mid) const; - - const dtNavMesh* m_nav; // Pointer to navmesh data. - - struct dtQueryData - { - dtStatus status; - struct dtNode* lastBestNode; - float lastBestNodeCost; - dtPolyRef startRef, endRef; - float startPos[3], endPos[3]; - const dtQueryFilter* filter; - }; - dtQueryData m_query; // Sliced query state. - - class dtNodePool* m_tinyNodePool; // Pointer to small node pool. - class dtNodePool* m_nodePool; // Pointer to node pool. - class dtNodeQueue* m_openList; // Pointer to open list queue. -}; - -// Helper function to allocate navmesh query class using Detour allocator. -dtNavMeshQuery* dtAllocNavMeshQuery(); -void dtFreeNavMeshQuery(dtNavMeshQuery* query); - -#endif // DETOURNAVMESHQUERY_H diff --git a/modules/worldengine/deps/recastnavigation/Detour/DetourObstacleAvoidance.cpp b/modules/worldengine/deps/recastnavigation/Detour/DetourObstacleAvoidance.cpp deleted file mode 100644 index a255c9b3fd..0000000000 --- a/modules/worldengine/deps/recastnavigation/Detour/DetourObstacleAvoidance.cpp +++ /dev/null @@ -1,532 +0,0 @@ -// -// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org -// -// This software is provided 'as-is', without any express or implied -// warranty. In no event will the authors be held liable for any damages -// arising from the use of this software. -// Permission is granted to anyone to use this software for any purpose, -// including commercial applications, and to alter it and redistribute it -// freely, subject to the following restrictions: -// 1. The origin of this software must not be misrepresented; you must not -// claim that you wrote the original software. If you use this software -// in a product, an acknowledgment in the product documentation would be -// appreciated but is not required. -// 2. Altered source versions must be plainly marked as such, and must not be -// misrepresented as being the original software. -// 3. This notice may not be removed or altered from any source distribution. -// - -#include "DetourObstacleAvoidance.h" -#include "DetourCommon.h" -#include "DetourAlloc.h" -#include "DetourAssert.h" -#include <string.h> -#include <math.h> -#include <float.h> -#include <new> - - -static int sweepCircleCircle(const float* c0, const float r0, const float* v, - const float* c1, const float r1, - float& tmin, float& tmax) -{ - static const float EPS = 0.0001f; - float s[3]; - dtVsub(s,c1,c0); - float r = r0+r1; - float c = dtVdot2D(s,s) - r*r; - float a = dtVdot2D(v,v); - if (a < EPS) return 0; // not moving - - // Overlap, calc time to exit. - float b = dtVdot2D(v,s); - float d = b*b - a*c; - if (d < 0.0f) return 0; // no intersection. - a = 1.0f / a; - const float rd = dtSqrt(d); - tmin = (b - rd) * a; - tmax = (b + rd) * a; - return 1; -} - -static int isectRaySeg(const float* ap, const float* u, - const float* bp, const float* bq, - float& t) -{ - float v[3], w[3]; - dtVsub(v,bq,bp); - dtVsub(w,ap,bp); - float d = dtVperp2D(u,v); - if (fabsf(d) < 1e-6f) return 0; - d = 1.0f/d; - t = dtVperp2D(v,w) * d; - if (t < 0 || t > 1) return 0; - float s = dtVperp2D(u,w) * d; - if (s < 0 || s > 1) return 0; - return 1; -} - - - -dtObstacleAvoidanceDebugData* dtAllocObstacleAvoidanceDebugData() -{ - void* mem = dtAlloc(sizeof(dtObstacleAvoidanceDebugData), DT_ALLOC_PERM); - if (!mem) return 0; - return new(mem) dtObstacleAvoidanceDebugData; -} - -void dtFreeObstacleAvoidanceDebugData(dtObstacleAvoidanceDebugData* ptr) -{ - if (!ptr) return; - ptr->~dtObstacleAvoidanceDebugData(); - dtFree(ptr); -} - - -dtObstacleAvoidanceDebugData::dtObstacleAvoidanceDebugData() : - m_nsamples(0), - m_maxSamples(0), - m_vel(0), - m_ssize(0), - m_pen(0), - m_vpen(0), - m_vcpen(0), - m_spen(0), - m_tpen(0) -{ -} - -dtObstacleAvoidanceDebugData::~dtObstacleAvoidanceDebugData() -{ - dtFree(m_vel); - dtFree(m_ssize); - dtFree(m_pen); - dtFree(m_vpen); - dtFree(m_vcpen); - dtFree(m_spen); - dtFree(m_tpen); -} - -bool dtObstacleAvoidanceDebugData::init(const int maxSamples) -{ - dtAssert(maxSamples); - m_maxSamples = maxSamples; - - m_vel = (float*)dtAlloc(sizeof(float)*3*m_maxSamples, DT_ALLOC_PERM); - if (!m_vel) - return false; - m_pen = (float*)dtAlloc(sizeof(float)*m_maxSamples, DT_ALLOC_PERM); - if (!m_pen) - return false; - m_ssize = (float*)dtAlloc(sizeof(float)*m_maxSamples, DT_ALLOC_PERM); - if (!m_ssize) - return false; - m_vpen = (float*)dtAlloc(sizeof(float)*m_maxSamples, DT_ALLOC_PERM); - if (!m_vpen) - return false; - m_vcpen = (float*)dtAlloc(sizeof(float)*m_maxSamples, DT_ALLOC_PERM); - if (!m_vcpen) - return false; - m_spen = (float*)dtAlloc(sizeof(float)*m_maxSamples, DT_ALLOC_PERM); - if (!m_spen) - return false; - m_tpen = (float*)dtAlloc(sizeof(float)*m_maxSamples, DT_ALLOC_PERM); - if (!m_tpen) - return false; - - return true; -} - -void dtObstacleAvoidanceDebugData::reset() -{ - m_nsamples = 0; -} - -void dtObstacleAvoidanceDebugData::addSample(const float* vel, const float ssize, const float pen, - const float vpen, const float vcpen, const float spen, const float tpen) -{ - if (m_nsamples >= m_maxSamples) - return; - dtAssert(m_vel); - dtAssert(m_ssize); - dtAssert(m_pen); - dtAssert(m_vpen); - dtAssert(m_vcpen); - dtAssert(m_spen); - dtAssert(m_tpen); - dtVcopy(&m_vel[m_nsamples*3], vel); - m_ssize[m_nsamples] = ssize; - m_pen[m_nsamples] = pen; - m_vpen[m_nsamples] = vpen; - m_vcpen[m_nsamples] = vcpen; - m_spen[m_nsamples] = spen; - m_tpen[m_nsamples] = tpen; - m_nsamples++; -} - -static void normalizeArray(float* arr, const int n) -{ - // Normalize penaly range. - float minPen = FLT_MAX; - float maxPen = -FLT_MAX; - for (int i = 0; i < n; ++i) - { - minPen = dtMin(minPen, arr[i]); - maxPen = dtMax(maxPen, arr[i]); - } - const float penRange = maxPen-minPen; - const float s = penRange > 0.001f ? (1.0f / penRange) : 1; - for (int i = 0; i < n; ++i) - arr[i] = dtClamp((arr[i]-minPen)*s, 0.0f, 1.0f); -} - -void dtObstacleAvoidanceDebugData::normalizeSamples() -{ - normalizeArray(m_pen, m_nsamples); - normalizeArray(m_vpen, m_nsamples); - normalizeArray(m_vcpen, m_nsamples); - normalizeArray(m_spen, m_nsamples); - normalizeArray(m_tpen, m_nsamples); -} - - -dtObstacleAvoidanceQuery* dtAllocObstacleAvoidanceQuery() -{ - void* mem = dtAlloc(sizeof(dtObstacleAvoidanceQuery), DT_ALLOC_PERM); - if (!mem) return 0; - return new(mem) dtObstacleAvoidanceQuery; -} - -void dtFreeObstacleAvoidanceQuery(dtObstacleAvoidanceQuery* ptr) -{ - if (!ptr) return; - ptr->~dtObstacleAvoidanceQuery(); - dtFree(ptr); -} - - -dtObstacleAvoidanceQuery::dtObstacleAvoidanceQuery() : - m_velBias(0.0f), - m_weightDesVel(0.0f), - m_weightCurVel(0.0f), - m_weightSide(0.0f), - m_weightToi(0.0f), - m_horizTime(0.0f), - m_maxCircles(0), - m_circles(0), - m_ncircles(0), - m_maxSegments(0), - m_segments(0), - m_nsegments(0) -{ -} - -dtObstacleAvoidanceQuery::~dtObstacleAvoidanceQuery() -{ - dtFree(m_circles); - dtFree(m_segments); -} - -bool dtObstacleAvoidanceQuery::init(const int maxCircles, const int maxSegments) -{ - m_maxCircles = maxCircles; - m_ncircles = 0; - m_circles = (dtObstacleCircle*)dtAlloc(sizeof(dtObstacleCircle)*m_maxCircles, DT_ALLOC_PERM); - if (!m_circles) - return false; - memset(m_circles, 0, sizeof(dtObstacleCircle)*m_maxCircles); - - m_maxSegments = maxSegments; - m_nsegments = 0; - m_segments = (dtObstacleSegment*)dtAlloc(sizeof(dtObstacleSegment)*m_maxSegments, DT_ALLOC_PERM); - if (!m_segments) - return false; - memset(m_segments, 0, sizeof(dtObstacleSegment)*m_maxSegments); - - return true; -} - -void dtObstacleAvoidanceQuery::reset() -{ - m_ncircles = 0; - m_nsegments = 0; -} - -void dtObstacleAvoidanceQuery::addCircle(const float* pos, const float rad, - const float* vel, const float* dvel) -{ - if (m_ncircles >= m_maxCircles) - return; - - dtObstacleCircle* cir = &m_circles[m_ncircles++]; - dtVcopy(cir->p, pos); - cir->rad = rad; - dtVcopy(cir->vel, vel); - dtVcopy(cir->dvel, dvel); -} - -void dtObstacleAvoidanceQuery::addSegment(const float* p, const float* q) -{ - if (m_nsegments > m_maxSegments) - return; - - dtObstacleSegment* seg = &m_segments[m_nsegments++]; - dtVcopy(seg->p, p); - dtVcopy(seg->q, q); -} - -void dtObstacleAvoidanceQuery::prepare(const float* pos, const float* dvel) -{ - // Prepare obstacles - for (int i = 0; i < m_ncircles; ++i) - { - dtObstacleCircle* cir = &m_circles[i]; - - // Side - const float* pa = pos; - const float* pb = cir->p; - - const float orig[3] = {0,0}; - float dv[3]; - dtVsub(cir->dp,pb,pa); - dtVnormalize(cir->dp); - dtVsub(dv, cir->dvel, dvel); - - const float a = dtTriArea2D(orig, cir->dp,dv); - if (a < 0.01f) - { - cir->np[0] = -cir->dp[2]; - cir->np[2] = cir->dp[0]; - } - else - { - cir->np[0] = cir->dp[2]; - cir->np[2] = -cir->dp[0]; - } - } - - for (int i = 0; i < m_nsegments; ++i) - { - dtObstacleSegment* seg = &m_segments[i]; - - // Precalc if the agent is really close to the segment. - const float r = 0.01f; - float t; - seg->touch = dtDistancePtSegSqr2D(pos, seg->p, seg->q, t) < dtSqr(r); - } -} - -float dtObstacleAvoidanceQuery::processSample(const float* vcand, const float cs, - const float* pos, const float rad, - const float vmax, const float* vel, const float* dvel, - dtObstacleAvoidanceDebugData* debug) -{ - // Find min time of impact and exit amongst all obstacles. - float tmin = m_horizTime; - float side = 0; - int nside = 0; - - for (int i = 0; i < m_ncircles; ++i) - { - const dtObstacleCircle* cir = &m_circles[i]; - - // RVO - float vab[3]; - dtVscale(vab, vcand, 2); - dtVsub(vab, vab, vel); - dtVsub(vab, vab, cir->vel); - - // Side - side += dtClamp(dtMin(dtVdot2D(cir->dp,vab)*0.5f+0.5f, dtVdot2D(cir->np,vab)*2), 0.0f, 1.0f); - nside++; - - float htmin = 0, htmax = 0; - if (!sweepCircleCircle(pos,rad, vab, cir->p,cir->rad, htmin, htmax)) - continue; - - // Handle overlapping obstacles. - if (htmin < 0.0f && htmax > 0.0f) - { - // Avoid more when overlapped. - htmin = -htmin * 0.5f; - } - - if (htmin >= 0.0f) - { - // The closest obstacle is somewhere ahead of us, keep track of nearest obstacle. - if (htmin < tmin) - tmin = htmin; - } - } - - for (int i = 0; i < m_nsegments; ++i) - { - const dtObstacleSegment* seg = &m_segments[i]; - float htmin = 0; - - if (seg->touch) - { - // Special case when the agent is very close to the segment. - float sdir[3], snorm[3]; - dtVsub(sdir, seg->q, seg->p); - snorm[0] = -sdir[2]; - snorm[2] = sdir[0]; - // If the velocity is pointing towards the segment, no collision. - if (dtVdot2D(snorm, vcand) < 0.0f) - continue; - // Else immediate collision. - htmin = 0.0f; - } - else - { - if (!isectRaySeg(pos, vcand, seg->p, seg->q, htmin)) - continue; - } - - // Avoid less when facing walls. - htmin *= 2.0f; - - // The closest obstacle is somewhere ahead of us, keep track of nearest obstacle. - if (htmin < tmin) - tmin = htmin; - } - - // Normalize side bias, to prevent it dominating too much. - if (nside) - side /= nside; - - const float ivmax = 1.0f / vmax; - const float vpen = m_weightDesVel * (dtVdist2D(vcand, dvel) * ivmax); - const float vcpen = m_weightCurVel * (dtVdist2D(vcand, vel) * ivmax); - const float spen = m_weightSide * side; - const float tpen = m_weightToi * (1.0f/(0.1f+tmin / m_horizTime)); - - const float penalty = vpen + vcpen + spen + tpen; - - // Store different penalties for debug viewing - if (debug) - debug->addSample(vcand, cs, penalty, vpen, vcpen, spen, tpen); - - return penalty; -} - -void dtObstacleAvoidanceQuery::sampleVelocityGrid(const float* pos, const float rad, const float vmax, - const float* vel, const float* dvel, - float* nvel, const int gsize, - dtObstacleAvoidanceDebugData* debug) -{ - prepare(pos, dvel); - - dtVset(nvel, 0,0,0); - - if (debug) - debug->reset(); - - const float cvx = dvel[0] * m_velBias; - const float cvz = dvel[2] * m_velBias; - const float cs = vmax * 2 * (1 - m_velBias) / (float)(gsize-1); - const float half = (gsize-1)*cs*0.5f; - - float minPenalty = FLT_MAX; - - for (int y = 0; y < gsize; ++y) - { - for (int x = 0; x < gsize; ++x) - { - float vcand[3]; - vcand[0] = cvx + x*cs - half; - vcand[1] = 0; - vcand[2] = cvz + y*cs - half; - - if (dtSqr(vcand[0])+dtSqr(vcand[2]) > dtSqr(vmax+cs/2)) continue; - - const float penalty = processSample(vcand, cs, pos,rad,vmax,vel,dvel, debug); - if (penalty < minPenalty) - { - minPenalty = penalty; - dtVcopy(nvel, vcand); - } - } - } -} - - -static const float DT_PI = 3.14159265f; - -void dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const float rad, const float vmax, - const float* vel, const float* dvel, float* nvel, - const int ndivs, const int nrings, const int depth, - dtObstacleAvoidanceDebugData* debug) -{ - prepare(pos, dvel); - - dtVset(nvel, 0,0,0); - - if (debug) - debug->reset(); - - // Build sampling pattern aligned to desired velocity. - static const int MAX_PATTERN_DIVS = 32; - static const int MAX_PATTERN_RINGS = 4; - float pat[(MAX_PATTERN_DIVS*MAX_PATTERN_RINGS+1)*2]; - int npat = 0; - - const int nd = dtClamp(ndivs, 1, MAX_PATTERN_DIVS); - const int nr = dtClamp(nrings, 1, MAX_PATTERN_RINGS); - const float da = (1.0f/nd) * DT_PI*2; - const float dang = atan2f(dvel[2], dvel[0]); - - // Always add sample at zero - pat[npat*2+0] = 0; - pat[npat*2+1] = 0; - npat++; - - for (int j = 0; j < nr; ++j) - { - const float rad = (float)(nr-j)/(float)nr; - float a = dang + (j&1)*0.5f*da; - for (int i = 0; i < nd; ++i) - { - pat[npat*2+0] = cosf(a)*rad; - pat[npat*2+1] = sinf(a)*rad; - npat++; - a += da; - } - } - - // Start sampling. - float cr = vmax * (1.0f-m_velBias); - float res[3]; - dtVset(res, dvel[0] * m_velBias, 0, dvel[2] * m_velBias); - - for (int k = 0; k < depth; ++k) - { - float minPenalty = FLT_MAX; - float bvel[3]; - dtVset(bvel, 0,0,0); - - for (int i = 0; i < npat; ++i) - { - float vcand[3]; - vcand[0] = res[0] + pat[i*2+0]*cr; - vcand[1] = 0; - vcand[2] = res[2] + pat[i*2+1]*cr; - - if (dtSqr(vcand[0])+dtSqr(vcand[2]) > dtSqr(vmax+0.001f)) continue; - - const float penalty = processSample(vcand,cr/10, pos,rad,vmax,vel,dvel, debug); - if (penalty < minPenalty) - { - minPenalty = penalty; - dtVcopy(bvel, vcand); - } - } - - dtVcopy(res, bvel); - - cr *= 0.5f; - } - - dtVcopy(nvel, res); -} - diff --git a/modules/worldengine/deps/recastnavigation/Detour/DetourObstacleAvoidance.h b/modules/worldengine/deps/recastnavigation/Detour/DetourObstacleAvoidance.h deleted file mode 100644 index 4a7187a799..0000000000 --- a/modules/worldengine/deps/recastnavigation/Detour/DetourObstacleAvoidance.h +++ /dev/null @@ -1,148 +0,0 @@ -// -// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org -// -// This software is provided 'as-is', without any express or implied -// warranty. In no event will the authors be held liable for any damages -// arising from the use of this software. -// Permission is granted to anyone to use this software for any purpose, -// including commercial applications, and to alter it and redistribute it -// freely, subject to the following restrictions: -// 1. The origin of this software must not be misrepresented; you must not -// claim that you wrote the original software. If you use this software -// in a product, an acknowledgment in the product documentation would be -// appreciated but is not required. -// 2. Altered source versions must be plainly marked as such, and must not be -// misrepresented as being the original software. -// 3. This notice may not be removed or altered from any source distribution. -// - -#ifndef DETOUROBSTACLEAVOIDANCE_H -#define DETOUROBSTACLEAVOIDANCE_H - -struct dtObstacleCircle -{ - float p[3]; // Position of the obstacle - float vel[3]; // Velocity of the obstacle - float dvel[3]; // Velocity of the obstacle - float rad; // Radius of the obstacle - float dp[3], np[3]; // Use for side selection during sampling. -}; - -struct dtObstacleSegment -{ - float p[3], q[3]; // End points of the obstacle segment - bool touch; -}; - -static const int RVO_SAMPLE_RAD = 15; -static const int MAX_RVO_SAMPLES = (RVO_SAMPLE_RAD*2+1)*(RVO_SAMPLE_RAD*2+1) + 100; - -class dtObstacleAvoidanceDebugData -{ -public: - dtObstacleAvoidanceDebugData(); - ~dtObstacleAvoidanceDebugData(); - - bool init(const int maxSamples); - void reset(); - void addSample(const float* vel, const float ssize, const float pen, - const float vpen, const float vcpen, const float spen, const float tpen); - - void normalizeSamples(); - - inline int getSampleCount() const { return m_nsamples; } - inline const float* getSampleVelocity(const int i) const { return &m_vel[i*3]; } - inline float getSampleSize(const int i) const { return m_ssize[i]; } - inline float getSamplePenalty(const int i) const { return m_pen[i]; } - inline float getSampleDesiredVelocityPenalty(const int i) const { return m_vpen[i]; } - inline float getSampleCurrentVelocityPenalty(const int i) const { return m_vcpen[i]; } - inline float getSamplePreferredSidePenalty(const int i) const { return m_spen[i]; } - inline float getSampleCollisionTimePenalty(const int i) const { return m_tpen[i]; } - -private: - int m_nsamples; - int m_maxSamples; - float* m_vel; - float* m_ssize; - float* m_pen; - float* m_vpen; - float* m_vcpen; - float* m_spen; - float* m_tpen; -}; - -dtObstacleAvoidanceDebugData* dtAllocObstacleAvoidanceDebugData(); -void dtFreeObstacleAvoidanceDebugData(dtObstacleAvoidanceDebugData* ptr); - - -class dtObstacleAvoidanceQuery -{ -public: - dtObstacleAvoidanceQuery(); - ~dtObstacleAvoidanceQuery(); - - bool init(const int maxCircles, const int maxSegments); - - void reset(); - - void addCircle(const float* pos, const float rad, - const float* vel, const float* dvel); - - void addSegment(const float* p, const float* q); - - inline void setVelocitySelectionBias(float v) { m_velBias = v; } - inline void setDesiredVelocityWeight(float w) { m_weightDesVel = w; } - inline void setCurrentVelocityWeight(float w) { m_weightCurVel = w; } - inline void setPreferredSideWeight(float w) { m_weightSide = w; } - inline void setCollisionTimeWeight(float w) { m_weightToi = w; } - inline void setTimeHorizon(float t) { m_horizTime = t; } - - void sampleVelocityGrid(const float* pos, const float rad, const float vmax, - const float* vel, const float* dvel, float* nvel, - const int gsize, - dtObstacleAvoidanceDebugData* debug = 0); - - void sampleVelocityAdaptive(const float* pos, const float rad, const float vmax, - const float* vel, const float* dvel, float* nvel, - const int ndivs, const int nrings, const int depth, - dtObstacleAvoidanceDebugData* debug = 0); - - inline int getObstacleCircleCount() const { return m_ncircles; } - const dtObstacleCircle* getObstacleCircle(const int i) { return &m_circles[i]; } - - inline int getObstacleSegmentCount() const { return m_nsegments; } - const dtObstacleSegment* getObstacleSegment(const int i) { return &m_segments[i]; } - -private: - - void prepare(const float* pos, const float* dvel); - - float processSample(const float* vcand, const float cs, - const float* pos, const float rad, - const float vmax, const float* vel, const float* dvel, - dtObstacleAvoidanceDebugData* debug); - - dtObstacleCircle* insertCircle(const float dist); - dtObstacleSegment* insertSegment(const float dist); - - float m_velBias; - float m_weightDesVel; - float m_weightCurVel; - float m_weightSide; - float m_weightToi; - float m_horizTime; - - int m_maxCircles; - dtObstacleCircle* m_circles; - int m_ncircles; - - int m_maxSegments; - dtObstacleSegment* m_segments; - int m_nsegments; -}; - -dtObstacleAvoidanceQuery* dtAllocObstacleAvoidanceQuery(); -void dtFreeObstacleAvoidanceQuery(dtObstacleAvoidanceQuery* ptr); - - -#endif // DETOUROBSTACLEAVOIDANCE_H
\ No newline at end of file diff --git a/modules/worldengine/deps/recastnavigation/Detour/Include/DetourAlloc.h b/modules/worldengine/deps/recastnavigation/Detour/Include/DetourAlloc.h new file mode 100644 index 0000000000..e814b62a71 --- /dev/null +++ b/modules/worldengine/deps/recastnavigation/Detour/Include/DetourAlloc.h @@ -0,0 +1,59 @@ +// +// 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 + +/// 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)(int 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(int 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/modules/worldengine/deps/recastnavigation/Detour/DetourAssert.h b/modules/worldengine/deps/recastnavigation/Detour/Include/DetourAssert.h index 3cf652288f..3cf652288f 100644 --- a/modules/worldengine/deps/recastnavigation/Detour/DetourAssert.h +++ b/modules/worldengine/deps/recastnavigation/Detour/Include/DetourAssert.h diff --git a/modules/worldengine/deps/recastnavigation/Detour/Include/DetourCommon.h b/modules/worldengine/deps/recastnavigation/Detour/Include/DetourCommon.h new file mode 100644 index 0000000000..2e354bd391 --- /dev/null +++ b/modules/worldengine/deps/recastnavigation/Detour/Include/DetourCommon.h @@ -0,0 +1,517 @@ +// +// 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" + +/** +@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. +/// @param [in] _ Unused parameter +template<class T> void dtIgnoreUnused(const T&) { } + +/// Swaps the values of the two parameters. +/// @param[in,out] a Value A +/// @param[in,out] b Value B +template<class T> 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<class T> 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<class T> 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<class T> 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<class T> 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<class T> 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; +} + +/// 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 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); + +/// @} + +#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.<br/> +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: +<ul> +<li>The vertices are wrapped in the normal Detour wrap direction.</li> +<li>The triangle's 3D face normal is in the general up direction.</li> +</ul> +A negative return value indicates: +<ul> +<li>The vertices are reverse wrapped. (Wrapped opposite the normal Detour wrap direction.)</li> +<li>The triangle's 3D face normal is in the general down direction.</li> +</ul> +*/ diff --git a/modules/worldengine/deps/recastnavigation/Detour/Include/DetourMath.h b/modules/worldengine/deps/recastnavigation/Detour/Include/DetourMath.h new file mode 100644 index 0000000000..0fff83af99 --- /dev/null +++ b/modules/worldengine/deps/recastnavigation/Detour/Include/DetourMath.h @@ -0,0 +1,19 @@ +/** +@defgroup detour Detour +Members in this module are wrappers around the standard math library +*/ + +#ifndef DETOURMATH_H +#define DETOURMATH_H + +#include <math.h> + +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); } + +#endif diff --git a/modules/worldengine/deps/recastnavigation/Detour/Include/DetourNavMesh.h b/modules/worldengine/deps/recastnavigation/Detour/Include/DetourNavMesh.h new file mode 100644 index 0000000000..3d170005eb --- /dev/null +++ b/modules/worldengine/deps/recastnavigation/Detour/Include/DetourNavMesh.h @@ -0,0 +1,708 @@ +// +// 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" + + +// Edited by TC +#if defined(WIN32) && !defined(__MINGW32__) +/// Do not rename back to uint64. Otherwise mac complains about typedef redefinition +typedef unsigned __int64 uint64_d; +#else +#include <stdint.h> +#ifndef uint64_t +#ifdef __linux__ +#include <linux/types.h> +#endif +#endif +/// Do not rename back to uint64. Otherwise mac complains about typedef redefinition +typedef uint64_t uint64_d; +#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. + +// Edited by TC +// We cannot have over 31 bits for either tile nor poly +// without changing polyCount to use 64bits too. +/// A handle to a polygon within a navigation mesh tile. +/// @ingroup detour +typedef uint64_d dtPolyRef; // Edited by TC + +/// A handle to a tile within a navigation mesh. +/// @ingroup detour +typedef uint64_d dtTileRef; // Edited by TC + +/// 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; + +static const int STATIC_SALT_BITS = 12; +static const int STATIC_TILE_BITS = 21; +static const int STATIC_POLY_BITS = 31; +// we cannot have over 31 bits for either tile nor poly +// without changing polyCount to use 64bits too. + +/// 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::findPath +enum dtFindPathOptions +{ + DT_FINDPATH_LOW_QUALITY_FAR = 0x01, ///< [provisional] trade quality for performance far from the origin. The idea is that by then a new query will be issued + 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 +}; + + +/// 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 polyogn 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 acess this value. + unsigned char areaAndtype; + + /// Sets the user defined area id. [Limit: < #DT_MAX_AREAS] + inline void setArea(unsigned char a) { areaAndtype = (areaAndtype & 0xc0) | (a & 0x3f); } + + /// Sets the 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. [Size: dtMeshHeader::vertCount] + dtLink* links; ///< The tile links. [Size: dtMeshHeader::maxLinkCount] + dtPolyDetail* detailMeshes; ///< The tile's detail sub-meshes. [Size: dtMeshHeader::detailMeshCount] + + /// The detail mesh's unique vertices. [(x, y, z) * dtMeshHeader::detailVertCount] + float* detailVerts; + + /// The detail mesh's triangles. [(vertA, vertB, vertC) * dtMeshHeader::detailTriCount] + 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. +}; + +/// 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. + int maxPolys; ///< The maximum number of polygons each tile can contain. +}; + +/// 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 + { + return ((dtPolyRef)salt << (m_polyBits+m_tileBits)) | ((dtPolyRef)it << m_polyBits) | (dtPolyRef)ip; + } + + /// 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 + { + const dtPolyRef saltMask = ((dtPolyRef)1<<m_saltBits)-1; + const dtPolyRef tileMask = ((dtPolyRef)1<<m_tileBits)-1; + const dtPolyRef polyMask = ((dtPolyRef)1<<m_polyBits)-1; + salt = (unsigned int)((ref >> (m_polyBits+m_tileBits)) & saltMask); + it = (unsigned int)((ref >> m_polyBits) & tileMask); + ip = (unsigned int)(ref & polyMask); + } + + /// 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 + { + const dtPolyRef saltMask = ((dtPolyRef)1<<m_saltBits)-1; + return (unsigned int)((ref >> (m_polyBits+m_tileBits)) & saltMask); + } + + /// 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 + { + const dtPolyRef tileMask = ((dtPolyRef)1<<m_tileBits)-1; + return (unsigned int)((ref >> m_polyBits) & tileMask); + } + + /// 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 + { + const dtPolyRef polyMask = ((dtPolyRef)1<<m_polyBits)-1; + return (unsigned int)(ref & polyMask); + } + + /// @} + +private: + + /// Returns pointer to tile in the tile array. + dtMeshTile* getTile(int i); + + /// Returns neighbour tile based on side. + int getTilesAt(const int x, const int y, + dtMeshTile** tiles, const int maxTiles) const; + + /// Returns neighbour tile based on side. + int getNeighbourTilesAt(const int x, const int y, const int side, + dtMeshTile** tiles, const int maxTiles) const; + + /// Returns all polygons in neighbour tile based on portal defined by the segment. + int findConnectingPolys(const float* va, const float* vb, + const dtMeshTile* tile, int side, + dtPolyRef* con, float* conarea, int maxcon) const; + + /// Builds internal polygons links for a tile. + void connectIntLinks(dtMeshTile* tile); + /// Builds internal polygons links for a tile. + void baseOffMeshLinks(dtMeshTile* tile); + + /// Builds external polygon links for a tile. + void connectExtLinks(dtMeshTile* tile, dtMeshTile* target, int side); + /// Builds external polygon links for a tile. + void connectExtOffMeshLinks(dtMeshTile* tile, dtMeshTile* target, int side); + + /// Removes external links at specified side. + void unconnectExtLinks(dtMeshTile* tile, dtMeshTile* target); + + + // TODO: These methods are duplicates from dtNavMeshQuery, but are needed for off-mesh connection finding. + + /// Queries polygons within a tile. + int queryPolygonsInTile(const dtMeshTile* tile, const float* qmin, const float* qmax, + dtPolyRef* polys, const int maxPolys) const; + /// Find nearest polygon within a tile. + dtPolyRef findNearestPolyInTile(const dtMeshTile* tile, const float* center, + const float* extents, float* nearestPt) const; + /// Returns closest point on polygon. + void closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const; + + dtNavMeshParams m_params; ///< Current initialization params. TODO: do not store this info twice. + float m_orig[3]; ///< Origin of the tile (0,0) + float m_tileWidth, m_tileHeight; ///< Dimensions of each tile. + int m_maxTiles; ///< Max number of tiles. + int m_tileLutSize; ///< Tile hash lookup size (must be pot). + int m_tileLutMask; ///< Tile hash lookup mask. + + dtMeshTile** m_posLookup; ///< Tile hash lookup. + dtMeshTile* m_nextFree; ///< Freelist of tiles. + dtMeshTile* m_tiles; ///< List of tiles. + + unsigned int m_saltBits; ///< Number of salt bits in the tile ID. + unsigned int m_tileBits; ///< Number of tile bits in the tile ID. + unsigned int m_polyBits; ///< Number of poly bits in the tile ID. +}; + +/// Allocates a navigation mesh object using the Detour allocator. +/// @return A navigation mesh that is ready for initialization, or null on failure. +/// @ingroup detour +dtNavMesh* dtAllocNavMesh(); + +/// Frees the specified navigation mesh object using the Detour allocator. +/// @param[in] navmesh A navigation mesh allocated using #dtAllocNavMesh +/// @ingroup detour +void dtFreeNavMesh(dtNavMesh* navmesh); + +#endif // DETOURNAVMESH_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. + +/** +@typedef dtPolyRef +@par +Polygon references are subject to the same invalidate/preserve/restore +rules that apply to #dtTileRef's. If the #dtTileRef for the polygon's +tile changes, the polygon reference becomes invalid. +Changing a polygon's flags, area id, etc. does not impact its polygon +reference. +@typedef dtTileRef +@par +The following changes will invalidate a tile reference: +- The referenced tile has been removed from the navigation mesh. +- The navigation mesh has been initialized using a different set + of #dtNavMeshParams. +A tile reference is preserved/restored if the tile is added to a navigation +mesh initialized with the original #dtNavMeshParams and is added at the +original reference location. (E.g. The lastRef parameter is used with +dtNavMesh::addTile.) +Basically, if the storage structure of a tile changes, its associated +tile reference changes. +@var unsigned short dtPoly::neis[DT_VERTS_PER_POLYGON] +@par +Each entry represents data for the edge starting at the vertex of the same index. +E.g. The entry at index n represents the edge data for vertex[n] to vertex[n+1]. +A value of zero indicates the edge has no polygon connection. (It makes up the +border of the navigation mesh.) +The information can be extracted as follows: +@code +neighborRef = neis[n] & 0xff; // Get the neighbor polygon reference. +if (neis[n] & #DT_EX_LINK) +{ + // The edge is an external (portal) edge. +} +@endcode +@var float dtMeshHeader::bvQuantFactor +@par +This value is used for converting between world and bounding volume coordinates. +For example: +@code +const float cs = 1.0f / tile->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/modules/worldengine/deps/recastnavigation/Detour/Include/DetourNavMeshBuilder.h b/modules/worldengine/deps/recastnavigation/Detour/Include/DetourNavMeshBuilder.h new file mode 100644 index 0000000000..b3fdce094c --- /dev/null +++ b/modules/worldengine/deps/recastnavigation/Detour/Include/DetourNavMeshBuilder.h @@ -0,0 +1,141 @@ +// +// 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.<br/> + /// #DT_OFFMESH_CON_BIDIR = Bidirectional travel. + const unsigned char* offMeshConDir; + /// The user defined ids of the off-mesh connection. [Size: #offMeshConCount] + const unsigned int* offMeshConUserID; + /// The number of off-mesh connections. [Limit: >= 0] + int offMeshConCount; + + /// @} + /// @name Tile Attributes + /// @note The tile grid/layer data can be left at zero if the destination is a single tile mesh. + /// @{ + + unsigned int userId; ///< The user defined id of the tile. + int tileX; ///< The tile's x-grid location within the multi-tile destination mesh. (Along the x-axis.) + int tileY; ///< The tile's y-grid location within the multi-tile desitation mesh. (Along the z-axis.) + int tileLayer; ///< The tile's layer within the layered destination mesh. [Limit: >= 0] (Along the y-axis.) + float bmin[3]; ///< The minimum bounds of the tile. [(x, y, z)] [Unit: wu] + float bmax[3]; ///< The maximum bounds of the tile. [(x, y, z)] [Unit: wu] + + /// @} + /// @name General Configuration Attributes + /// @{ + + float walkableHeight; ///< The agent height. [Unit: wu] + float walkableRadius; ///< The agent radius. [Unit: wu] + float walkableClimb; ///< The agent maximum traversable ledge. (Up/Down) [Unit: wu] + float cs; ///< The xz-plane cell size of the polygon mesh. [Limit: > 0] [Unit: wu] + float ch; ///< The y-axis cell height of the polygon mesh. [Limit: > 0] [Unit: wu] + + /// True if a bounding volume tree should be built for the tile. + /// @note The BVTree is not normally needed for layered navigation meshes. + bool buildBvTree; + + /// @} +}; + +/// Builds navigation mesh tile data from the provided tile creation data. +/// @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 <tt>init()</tt> function or the dtNavMesh::addTile() +function. +@see dtCreateNavMeshData +*/ diff --git a/modules/worldengine/deps/recastnavigation/Detour/Include/DetourNavMeshQuery.h b/modules/worldengine/deps/recastnavigation/Detour/Include/DetourNavMeshQuery.h new file mode 100644 index 0000000000..c7b360dcdc --- /dev/null +++ b/modules/worldengine/deps/recastnavigation/Detour/Include/DetourNavMeshQuery.h @@ -0,0 +1,536 @@ +// +// 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 visted. (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]; + + /// 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 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 <= 65536] + /// @returns The status flags for the query. + dtStatus init(const dtNavMesh* nav, const int maxNodes); + + /// @name Standard Pathfinding Functions + // /@{ + + /// Finds a path from the start polygon to the end polygon. + /// @param[in] startRef The refrence id of the start polygon. + /// @param[in] endRef The reference id of the end polygon. + /// @param[in] startPos A position within the start polygon. [(x, y, z)] + /// @param[in] endPos A position within the end polygon. [(x, y, z)] + /// @param[in] filter The polygon filter to apply to the query. + /// @param[out] path An ordered list of polygon references representing the path. (Start to end.) + /// [(polyRef) * @p pathCount] + /// @param[out] pathCount The number of polygons returned in the @p path array. + /// @param[in] maxPath The maximum number of polygons the @p path array can hold. [Limit: >= 1] + dtStatus findPath(dtPolyRef startRef, dtPolyRef endRef, + const float* startPos, const float* endPos, + const dtQueryFilter* filter, + dtPolyRef* path, int* pathCount, const int maxPath) const; + + /// Finds the straight path from the start to the end position within the polygon corridor. + /// @param[in] startPos Path start position. [(x, y, z)] + /// @param[in] endPos Path end position. [(x, y, z)] + /// @param[in] path An array of polygon references that represent the path corridor. + /// @param[in] pathSize The number of polygons in the @p path array. + /// @param[out] straightPath Points describing the straight path. [(x, y, z) * @p straightPathCount]. + /// @param[out] straightPathFlags Flags describing each point. (See: #dtStraightPathFlags) [opt] + /// @param[out] straightPathRefs The reference id of the polygon that is being entered at each point. [opt] + /// @param[out] straightPathCount The number of points in the straight path. + /// @param[in] maxStraightPath The maximum number of points the straight path arrays can hold. [Limit: > 0] + /// @param[in] options Query options. (see: #dtStraightPathOptions) + /// @returns The status flags for the query. + dtStatus findStraightPath(const float* startPos, const float* endPos, + const dtPolyRef* path, const int pathSize, + float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs, + int* straightPathCount, const int maxStraightPath, const int options = 0) const; + + ///@} + /// @name Sliced Pathfinding Functions + /// Common use case: + /// -# Call initSlicedFindPath() to initialize the sliced path query. + /// -# Call updateSlicedFindPath() until it returns complete. + /// -# Call finalizeSlicedFindPath() to get the path. + ///@{ + + /// Intializes a sliced path query. + /// @param[in] startRef The refrence id of the start polygon. + /// @param[in] endRef The reference id of the end polygon. + /// @param[in] startPos A position within the start polygon. [(x, y, z)] + /// @param[in] endPos A position within the end polygon. [(x, y, z)] + /// @param[in] filter The polygon filter to apply to the query. + /// @param[in] options query options (see: #dtFindPathOptions) + /// @returns The status flags for the query. + dtStatus initSlicedFindPath(dtPolyRef startRef, dtPolyRef endRef, + const float* startPos, const float* endPos, + const dtQueryFilter* filter, const unsigned int options = 0); + + /// Updates an in-progress sliced path query. + /// @param[in] maxIter The maximum number of iterations to perform. + /// @param[out] doneIters The actual number of iterations completed. [opt] + /// @returns The status flags for the query. + dtStatus updateSlicedFindPath(const int maxIter, int* doneIters); + + /// Finalizes and returns the results of a sliced path query. + /// @param[out] path An ordered list of polygon references representing the path. (Start to end.) + /// [(polyRef) * @p pathCount] + /// @param[out] pathCount The number of polygons returned in the @p path array. + /// @param[in] maxPath The max number of polygons the path array can hold. [Limit: >= 1] + /// @returns The status flags for the query. + dtStatus finalizeSlicedFindPath(dtPolyRef* path, int* pathCount, const int maxPath); + + /// Finalizes and returns the results of an incomplete sliced path query, returning the path to the furthest + /// polygon on the existing path that was visited during the search. + /// @param[in] existing An array of polygon references for the existing path. + /// @param[in] existingSize The number of polygon in the @p existing array. + /// @param[out] path An ordered list of polygon references representing the path. (Start to end.) + /// [(polyRef) * @p pathCount] + /// @param[out] pathCount The number of polygons returned in the @p path array. + /// @param[in] maxPath The max number of polygons the @p path array can hold. [Limit: >= 1] + /// @returns The status flags for the query. + dtStatus finalizeSlicedFindPathPartial(const dtPolyRef* existing, const int existingSize, + dtPolyRef* path, int* pathCount, const int maxPath); + + ///@} + /// @name Dijkstra Search Functions + /// @{ + + /// Finds the polygons along the navigation graph that touch the specified circle. + /// @param[in] startRef The reference id of the polygon where the search starts. + /// @param[in] centerPos The center of the search circle. [(x, y, z)] + /// @param[in] radius The radius of the search circle. + /// @param[in] filter The polygon filter to apply to the query. + /// @param[out] resultRef The reference ids of the polygons touched by the circle. [opt] + /// @param[out] resultParent The reference ids of the parent polygons for each result. + /// Zero if a result polygon has no parent. [opt] + /// @param[out] resultCost The search cost from @p centerPos to the polygon. [opt] + /// @param[out] resultCount The number of polygons found. [opt] + /// @param[in] maxResult The maximum number of polygons the result arrays can hold. + /// @returns The status flags for the query. + dtStatus findPolysAroundCircle(dtPolyRef startRef, const float* centerPos, const float radius, + const dtQueryFilter* filter, + dtPolyRef* resultRef, dtPolyRef* resultParent, float* resultCost, + int* resultCount, const int maxResult) const; + + /// Finds the polygons along the naviation graph that touch the specified convex polygon. + /// @param[in] startRef The reference id of the polygon where the search starts. + /// @param[in] verts The vertices describing the convex polygon. (CCW) + /// [(x, y, z) * @p nverts] + /// @param[in] nverts The number of vertices in the polygon. + /// @param[in] filter The polygon filter to apply to the query. + /// @param[out] resultRef The reference ids of the polygons touched by the search polygon. [opt] + /// @param[out] resultParent The reference ids of the parent polygons for each result. Zero if a + /// result polygon has no parent. [opt] + /// @param[out] resultCost The search cost from the centroid point to the polygon. [opt] + /// @param[out] resultCount The number of polygons found. + /// @param[in] maxResult The maximum number of polygons the result arrays can hold. + /// @returns The status flags for the query. + dtStatus findPolysAroundShape(dtPolyRef startRef, const float* verts, const int nverts, + const dtQueryFilter* filter, + dtPolyRef* resultRef, dtPolyRef* resultParent, float* resultCost, + int* resultCount, const int maxResult) const; + + /// @} + /// @name Local Query Functions + ///@{ + + /// Finds the polygon nearest to the specified center point. + /// @param[in] center The center of the search box. [(x, y, z)] + /// @param[in] extents The search distance along each axis. [(x, y, z)] + /// @param[in] filter The polygon filter to apply to the query. + /// @param[out] nearestRef The reference id of the nearest polygon. + /// @param[out] nearestPt The nearest point on the polygon. [opt] [(x, y, z)] + /// @returns The status flags for the query. + dtStatus findNearestPoly(const float* center, const float* extents, + const dtQueryFilter* filter, + dtPolyRef* nearestRef, float* nearestPt) const; + + /// Finds polygons that overlap the search box. + /// @param[in] center The center of the search box. [(x, y, z)] + /// @param[in] extents 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* extents, + const dtQueryFilter* filter, + dtPolyRef* polys, int* polyCount, const int maxPolys) const; + + /// Finds the non-overlapping navigation polygons in the local neighbourhood around the center position. + /// @param[in] startRef The reference id of the polygon where the search starts. + /// @param[in] centerPos The center of the query circle. [(x, y, z)] + /// @param[in] radius The radius of the query circle. + /// @param[in] filter The polygon filter to apply to the query. + /// @param[out] resultRef The reference ids of the polygons touched by the circle. + /// @param[out] resultParent The reference ids of the parent polygons for each result. + /// Zero if a result polygon has no parent. [opt] + /// @param[out] resultCount The number of polygons found. + /// @param[in] maxResult The maximum number of polygons the result arrays can hold. + /// @returns The status flags for the query. + dtStatus findLocalNeighbourhood(dtPolyRef startRef, const float* centerPos, const float radius, + const dtQueryFilter* filter, + dtPolyRef* resultRef, dtPolyRef* resultParent, + int* resultCount, const int maxResult) const; + + /// Moves from the start to the end position constrained to the navigation mesh. + /// @param[in] startRef The reference id of the start polygon. + /// @param[in] startPos A position of the mover within the start polygon. [(x, y, x)] + /// @param[in] endPos The desired end position of the mover. [(x, y, z)] + /// @param[in] filter The polygon filter to apply to the query. + /// @param[out] resultPos The result position of the mover. [(x, y, z)] + /// @param[out] visited The reference ids of the polygons visited during the move. + /// @param[out] visitedCount The number of polygons visited during the move. + /// @param[in] maxVisitedSize The maximum number of polygons the @p visited array can hold. + /// @returns The status flags for the query. + dtStatus moveAlongSurface(dtPolyRef startRef, const float* startPos, const float* endPos, + const dtQueryFilter* filter, + float* resultPos, dtPolyRef* visited, int* visitedCount, const int maxVisitedSize) const; + + /// Casts a 'walkability' ray along the surface of the navigation mesh from + /// the start position toward the end position. + /// @note A wrapper around raycast(..., RaycastHit*). Retained for backward compatibility. + /// @param[in] startRef The reference id of the start polygon. + /// @param[in] startPos A position within the start polygon representing + /// the start of the ray. [(x, y, z)] + /// @param[in] endPos The position to cast the ray toward. [(x, y, z)] + /// @param[out] t The hit parameter. (FLT_MAX if no wall hit.) + /// @param[out] hitNormal The normal of the nearest wall hit. [(x, y, z)] + /// @param[in] filter The polygon filter to apply to the query. + /// @param[out] path The reference ids of the visited polygons. [opt] + /// @param[out] pathCount The number of visited polygons. [opt] + /// @param[in] maxPath The maximum number of polygons the @p path array can hold. + /// @returns The status flags for the query. + dtStatus raycast(dtPolyRef startRef, const float* startPos, const float* endPos, + const dtQueryFilter* filter, + float* t, float* hitNormal, dtPolyRef* path, int* pathCount, const int maxPath) const; + + /// Casts a 'walkability' ray along the surface of the navigation mesh from + /// the start position toward the end position. + /// @param[in] startRef The reference id of the start polygon. + /// @param[in] startPos A position within the start polygon representing + /// the start of the ray. [(x, y, z)] + /// @param[in] endPos The position to cast the ray toward. [(x, y, z)] + /// @param[in] filter The polygon filter to apply to the query. + /// @param[in] flags govern how the raycast behaves. See dtRaycastOptions + /// @param[out] hit Pointer to a raycast hit structure which will be filled by the results. + /// @param[in] prevRef parent of start ref. Used during for cost calculation [opt] + /// @returns The status flags for the query. + dtStatus raycast(dtPolyRef startRef, const float* startPos, const float* endPos, + const dtQueryFilter* filter, const unsigned int options, + dtRaycastHit* hit, dtPolyRef prevRef = 0) const; + + + /// Finds the distance from the specified position to the nearest polygon wall. + /// @param[in] startRef The reference id of the polygon containing @p centerPos. + /// @param[in] centerPos The center of the search circle. [(x, y, z)] + /// @param[in] maxRadius The radius of the search circle. + /// @param[in] filter The polygon filter to apply to the query. + /// @param[out] hitDist The distance to the nearest wall from @p centerPos. + /// @param[out] hitPos The nearest position on the wall that was hit. [(x, y, z)] + /// @param[out] hitNormal The normalized ray formed from the wall point to the + /// source point. [(x, y, z)] + /// @returns The status flags for the query. + dtStatus findDistanceToWall(dtPolyRef startRef, const float* centerPos, const float maxRadius, + const dtQueryFilter* filter, + float* hitDist, float* hitPos, float* hitNormal) const; + + /// Returns the segments for the specified polygon, optionally including portals. + /// @param[in] ref The reference id of the polygon. + /// @param[in] filter The polygon filter to apply to the query. + /// @param[out] segmentVerts The segments. [(ax, ay, az, bx, by, bz) * segmentCount] + /// @param[out] segmentRefs The reference ids of each segment's neighbor polygon. + /// Or zero if the segment is a wall. [opt] [(parentRef) * @p segmentCount] + /// @param[out] segmentCount The number of segments returned. + /// @param[in] maxSegments The maximum number of segments the result arrays can hold. + /// @returns The status flags for the query. + dtStatus getPolyWallSegments(dtPolyRef ref, const dtQueryFilter* filter, + float* segmentVerts, dtPolyRef* segmentRefs, int* segmentCount, + const int maxSegments) const; + + /// Returns random location on navmesh. + /// Polygons are chosen weighted by area. The search runs in linear related to number of polygon. + /// @param[in] filter The polygon filter to apply to the query. + /// @param[in] frand Function returning a random number [0..1). + /// @param[out] randomRef The reference id of the random location. + /// @param[out] randomPt The random location. + /// @returns The status flags for the query. + dtStatus findRandomPoint(const dtQueryFilter* filter, float (*frand)(), + dtPolyRef* randomRef, float* randomPt) const; + + /// Returns random location on navmesh within the reach of specified location. + /// Polygons are chosen weighted by area. The search runs in linear related to number of polygon. + /// The location is not exactly constrained by the circle, but it limits the visited polygons. + /// @param[in] startRef The reference id of the polygon where the search starts. + /// @param[in] centerPos The center of the search circle. [(x, y, z)] + /// @param[in] filter The polygon filter to apply to the query. + /// @param[in] frand Function returning a random number [0..1). + /// @param[out] randomRef The reference id of the random location. + /// @param[out] randomPt The random location. [(x, y, z)] + /// @returns The status flags for the query. + dtStatus findRandomPointAroundCircle(dtPolyRef startRef, const float* centerPos, const float maxRadius, + const dtQueryFilter* filter, float (*frand)(), + dtPolyRef* randomRef, float* randomPt) const; + + /// Finds the closest point on the specified polygon. + /// @param[in] ref The reference id of the polygon. + /// @param[in] pos The position to check. [(x, y, z)] + /// @param[out] closest The closest point on the polygon. [(x, y, z)] + /// @param[out] posOverPoly True of the position is over the polygon. + /// @returns The status flags for the query. + dtStatus closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const; + + /// Returns a point on the boundary closest to the source point if the source point is outside the + /// polygon's xz-bounds. + /// @param[in] ref The reference id to the polygon. + /// @param[in] pos The position to check. [(x, y, z)] + /// @param[out] closest The closest point. [(x, y, z)] + /// @returns The status flags for the query. + dtStatus closestPointOnPolyBoundary(dtPolyRef ref, const float* pos, float* closest) const; + + /// Gets the height of the polygon at the provided position using the height detail. (Most accurate.) + /// @param[in] ref The reference id of the polygon. + /// @param[in] pos A position within the xz-bounds of the polygon. [(x, y, z)] + /// @param[out] height The height at the surface of the polygon. + /// @returns The status flags for the query. + dtStatus getPolyHeight(dtPolyRef ref, const float* pos, float* height) const; + + /// @} + /// @name Miscellaneous Functions + /// @{ + + /// Returns true if the polygon reference is valid and passes the filter restrictions. + /// @param[in] ref The polygon reference to check. + /// @param[in] filter The filter to apply. + bool isValidPolyRef(dtPolyRef ref, const dtQueryFilter* filter) const; + + /// Returns true if the polygon reference is in the closed list. + /// @param[in] ref The reference id of the polygon to check. + /// @returns True if the polygon is in closed list. + bool isInClosedList(dtPolyRef ref) const; + + /// Gets the node pool. + /// @returns The node pool. + class dtNodePool* getNodePool() const { return m_nodePool; } + + /// Gets the navigation mesh the query object is using. + /// @return The navigation mesh the query object is using. + const dtNavMesh* getAttachedNavMesh() const { return m_nav; } + + /// @} + +private: + + /// Returns neighbour tile based on side. + dtMeshTile* getNeighbourTileAt(int x, int y, int side) const; + + /// Queries polygons within a tile. + int queryPolygonsInTile(const dtMeshTile* tile, const float* qmin, const float* qmax, const dtQueryFilter* filter, + dtPolyRef* polys, const int maxPolys) const; + + /// 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; + + 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/modules/worldengine/deps/recastnavigation/Detour/DetourNode.h b/modules/worldengine/deps/recastnavigation/Detour/Include/DetourNode.h index e46254fb50..6fefdc8e0f 100644 --- a/modules/worldengine/deps/recastnavigation/Detour/DetourNode.h +++ b/modules/worldengine/deps/recastnavigation/Detour/Include/DetourNode.h @@ -25,22 +25,28 @@ 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 = ~0; +static const dtNodeIndex DT_NULL_IDX = (dtNodeIndex)~0; struct dtNode { - float pos[3]; // Position of the node. - float cost; // Cost from previous node to current node. - float total; // Cost up to the node. - unsigned int pidx : 30; // Index to parent node. - unsigned int flags : 2; // Node flags 0/open/closed. - dtPolyRef id; // Polygon ref the node corresponds to. + 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 : 24; ///< Index to parent node. + unsigned int state : 2; ///< 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 = 4; // number of extra states per node. See dtNode::state + + + class dtNodePool { public: @@ -48,8 +54,12 @@ public: ~dtNodePool(); inline void operator=(const dtNodePool&) {} void clear(); - dtNode* getNode(dtPolyRef id); - dtNode* findNode(dtPolyRef id); + + // 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 { @@ -82,6 +92,7 @@ public: 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: @@ -156,4 +167,4 @@ private: }; -#endif // DETOURNODE_H
\ No newline at end of file +#endif // DETOURNODE_H diff --git a/modules/worldengine/deps/recastnavigation/Detour/Include/DetourStatus.h b/modules/worldengine/deps/recastnavigation/Detour/Include/DetourStatus.h new file mode 100644 index 0000000000..af822c4a92 --- /dev/null +++ b/modules/worldengine/deps/recastnavigation/Detour/Include/DetourStatus.h @@ -0,0 +1,64 @@ +// +// 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. + + +// 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/modules/worldengine/deps/recastnavigation/Detour/DetourAlloc.cpp b/modules/worldengine/deps/recastnavigation/Detour/Source/DetourAlloc.cpp index 5f671df5bd..5f671df5bd 100644 --- a/modules/worldengine/deps/recastnavigation/Detour/DetourAlloc.cpp +++ b/modules/worldengine/deps/recastnavigation/Detour/Source/DetourAlloc.cpp diff --git a/modules/worldengine/deps/recastnavigation/Detour/DetourCommon.cpp b/modules/worldengine/deps/recastnavigation/Detour/Source/DetourCommon.cpp index c0b973e4a7..2373c19181 100644 --- a/modules/worldengine/deps/recastnavigation/Detour/DetourCommon.cpp +++ b/modules/worldengine/deps/recastnavigation/Detour/Source/DetourCommon.cpp @@ -16,16 +16,11 @@ // 3. This notice may not be removed or altered from any source distribution. // -#include <math.h> #include "DetourCommon.h" +#include "DetourMath.h" ////////////////////////////////////////////////////////////////////////////////////////// -float dtSqrt(float x) -{ - return sqrtf(x); -} - void dtClosestPtPointTriangle(float* closest, const float* p, const float* a, const float* b, const float* c) { @@ -238,6 +233,9 @@ bool dtClosestHeightPointTriangle(const float* p, const float* a, const float* b 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? @@ -291,6 +289,9 @@ inline bool overlapRange(const float amin, const float amax, 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) { @@ -327,3 +328,60 @@ bool dtOverlapPolyPoly2D(const float* polya, const int npolya, 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 = 0.0f; + int tri = 0; + 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/modules/worldengine/deps/recastnavigation/Detour/DetourNavMesh.cpp b/modules/worldengine/deps/recastnavigation/Detour/Source/DetourNavMesh.cpp index 95af28797d..1814f75553 100644 --- a/modules/worldengine/deps/recastnavigation/Detour/DetourNavMesh.cpp +++ b/modules/worldengine/deps/recastnavigation/Detour/Source/DetourNavMesh.cpp @@ -16,13 +16,13 @@ // 3. This notice may not be removed or altered from any source distribution. // -#include <math.h> #include <float.h> #include <string.h> #include <stdio.h> #include "DetourNavMesh.h" #include "DetourNode.h" #include "DetourCommon.h" +#include "DetourMath.h" #include "DetourAlloc.h" #include "DetourAssert.h" #include <new> @@ -64,6 +64,15 @@ inline bool overlapSlabs(const float* amin, const float* amax, 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) @@ -133,6 +142,10 @@ dtNavMesh* dtAllocNavMesh() 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; @@ -141,6 +154,30 @@ void dtFreeNavMesh(dtNavMesh* 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), @@ -190,10 +227,10 @@ dtStatus dtNavMesh::init(const dtNavMeshParams* params) m_tiles = (dtMeshTile*)dtAlloc(sizeof(dtMeshTile)*m_maxTiles, DT_ALLOC_PERM); if (!m_tiles) - return DT_FAILURE; + return DT_FAILURE | DT_OUT_OF_MEMORY; m_posLookup = (dtMeshTile**)dtAlloc(sizeof(dtMeshTile*)*m_tileLutSize, DT_ALLOC_PERM); if (!m_posLookup) - return DT_FAILURE; + 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; @@ -204,13 +241,11 @@ dtStatus dtNavMesh::init(const dtNavMeshParams* params) m_nextFree = &m_tiles[i]; } - // Init ID generator values. - m_tileBits = STATIC_TILE_BITS; //dtIlog2(dtNextPow2((unsigned int)params->maxTiles)); - m_polyBits = STATIC_POLY_BITS; //dtIlog2(dtNextPow2((unsigned int)params->maxPolys)); - m_saltBits = STATIC_SALT_BITS; //sizeof(dtPolyRef)*8 - m_tileBits - m_polyBits; - //if (m_saltBits < SALT_MIN_BITS) - //return DT_FAILURE; - + // Edited by TC + m_tileBits = STATIC_TILE_BITS; + m_polyBits = STATIC_POLY_BITS; + m_saltBits = STATIC_SALT_BITS; + return DT_SUCCESS; } @@ -219,9 +254,9 @@ dtStatus dtNavMesh::init(unsigned char* data, const int dataSize, const int flag // Make sure the data is in right format. dtMeshHeader* header = (dtMeshHeader*)data; if (header->magic != DT_NAVMESH_MAGIC) - return DT_FAILURE; + return DT_FAILURE | DT_WRONG_MAGIC; if (header->version != DT_NAVMESH_VERSION) - return DT_FAILURE; + return DT_FAILURE | DT_WRONG_VERSION; dtNavMeshParams params; dtVcopy(params.orig, header->bmin); @@ -230,13 +265,17 @@ dtStatus dtNavMesh::init(unsigned char* data, const int dataSize, const int flag params.maxTiles = 1; params.maxPolys = header->polyCount; - dtStatus res = init(¶ms); - if (res != DT_SUCCESS) - return res; + 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; @@ -251,6 +290,7 @@ int dtNavMesh::findConnectingPolys(const float* va, const float* vb, 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]; @@ -267,11 +307,18 @@ int dtNavMesh::findConnectingPolys(const float* va, const float* vb, { // Skip edges which do not point to the right side. if (poly->neis[j] != m) continue; - // Check if the segments touch. + const float* vc = &tile->verts[poly->verts[j]*3]; const float* vd = &tile->verts[poly->verts[(j+1) % nv]*3]; + 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. @@ -288,9 +335,11 @@ int dtNavMesh::findConnectingPolys(const float* va, const float* vb, return n; } -void dtNavMesh::unconnectExtLinks(dtMeshTile* tile, int side) +void dtNavMesh::unconnectExtLinks(dtMeshTile* tile, dtMeshTile* target) { - if (!tile) return; + if (!tile || !target) return; + + const unsigned int targetNum = decodePolyIdTile(getTileRef(target)); for (int i = 0; i < tile->header->polyCount; ++i) { @@ -299,7 +348,8 @@ void dtNavMesh::unconnectExtLinks(dtMeshTile* tile, int side) unsigned int pj = DT_NULL_LINK; while (j != DT_NULL_LINK) { - if (tile->links[j].side == side) + if (tile->links[j].side != 0xff && + decodePolyIdTile(tile->links[j].ref) == targetNum) { // Revove link. unsigned int nj = tile->links[j].next; @@ -330,19 +380,25 @@ void dtNavMesh::connectExtLinks(dtMeshTile* tile, dtMeshTile* target, int side) dtPoly* poly = &tile->polys[i]; // Create new links. - unsigned short m = DT_EXT_LINK | (unsigned short)side; +// unsigned short m = DT_EXT_LINK | (unsigned short)side; + const int nv = poly->vertCount; for (int j = 0; j < nv; ++j) { - // Skip edges which do not point to the right side. - if (poly->neis[j] != m) continue; + // 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(side), nei,neia,4); + int nnei = findConnectingPolys(va,vb, target, dtOppositeTile(dir), nei,neia,4); for (int k = 0; k < nnei; ++k) { unsigned int idx = allocLink(tile); @@ -351,13 +407,13 @@ void dtNavMesh::connectExtLinks(dtMeshTile* tile, dtMeshTile* target, int side) dtLink* link = &tile->links[idx]; link->ref = nei[k]; link->edge = (unsigned char)j; - link->side = (unsigned char)side; + link->side = (unsigned char)dir; link->next = poly->firstLink; poly->firstLink = idx; // Compress portal limits to a byte value. - if (side == 0 || side == 4) + 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]); @@ -366,7 +422,7 @@ void dtNavMesh::connectExtLinks(dtMeshTile* tile, dtMeshTile* target, int side) link->bmin = (unsigned char)(dtClamp(tmin, 0.0f, 1.0f)*255.0f); link->bmax = (unsigned char)(dtClamp(tmax, 0.0f, 1.0f)*255.0f); } - else if (side == 2 || side == 6) + 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]); @@ -387,15 +443,18 @@ void dtNavMesh::connectExtOffMeshLinks(dtMeshTile* tile, dtMeshTile* target, int // Connect off-mesh links. // We are interested on links which land from target tile to this tile. - const unsigned char oppositeSide = (unsigned char)dtOppositeTile(side); + 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 ext[3] = { targetCon->rad, target->header->walkableClimb, targetCon->rad }; @@ -403,7 +462,8 @@ void dtNavMesh::connectExtOffMeshLinks(dtMeshTile* tile, dtMeshTile* target, int const float* p = &targetCon->pos[3]; float nearestPt[3]; dtPolyRef ref = findNearestPolyInTile(tile, p, ext, nearestPt); - if (!ref) continue; + 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; @@ -428,19 +488,19 @@ void dtNavMesh::connectExtOffMeshLinks(dtMeshTile* tile, dtMeshTile* target, int // Link target poly to off-mesh connection. if (targetCon->flags & DT_OFFMESH_CON_BIDIR) { - unsigned int idx = allocLink(tile); - if (idx != DT_NULL_LINK) + 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[idx]; + dtLink* link = &tile->links[tidx]; link->ref = getPolyRefBase(target) | (dtPolyRef)(targetCon->poly); link->edge = 0xff; - link->side = (unsigned char)side; + link->side = (unsigned char)(side == -1 ? 0xff : side); link->bmin = link->bmax = 0; // Add to linked list. link->next = landPoly->firstLink; - landPoly->firstLink = idx; + landPoly->firstLink = tidx; } } } @@ -484,13 +544,13 @@ void dtNavMesh::connectIntLinks(dtMeshTile* tile) } } -void dtNavMesh::connectIntOffMeshLinks(dtMeshTile* tile) +void dtNavMesh::baseOffMeshLinks(dtMeshTile* tile) { if (!tile) return; dtPolyRef base = getPolyRefBase(tile); - // Find Off-mesh connection end points. + // Base off-mesh connection start points. for (int i = 0; i < tile->header->offMeshConCount; ++i) { dtOffMeshConnection* con = &tile->offMeshCons[i]; @@ -498,72 +558,109 @@ void dtNavMesh::connectIntOffMeshLinks(dtMeshTile* tile) const float ext[3] = { con->rad, tile->header->walkableClimb, con->rad }; - for (int j = 0; j < 2; ++j) - { - unsigned char side = j == 0 ? 0xff : con->side; + // Find polygon to connect to. + const float* p = &con->pos[0]; // First vertex + float nearestPt[3]; + dtPolyRef ref = findNearestPolyInTile(tile, p, ext, nearestPt); + if (!ref) continue; + // findNearestPoly may return too optimistic results, further check to make sure. + if (dtSqr(nearestPt[0]-p[0])+dtSqr(nearestPt[2]-p[2]) > dtSqr(con->rad)) + continue; + // Make sure the location is on current mesh. + float* v = &tile->verts[poly->verts[0]*3]; + dtVcopy(v, nearestPt); - if (side == 0xff) - { - // Find polygon to connect to. - const float* p = &con->pos[j*3]; - float nearestPt[3]; - dtPolyRef ref = findNearestPolyInTile(tile, p, ext, nearestPt); - if (!ref) continue; - // findNearestPoly may return too optimistic results, further check to make sure. - if (dtSqr(nearestPt[0]-p[0])+dtSqr(nearestPt[2]-p[2]) > dtSqr(con->rad)) - continue; - // Make sure the location is on current mesh. - float* v = &tile->verts[poly->verts[j]*3]; - dtVcopy(v, nearestPt); - - // Link off-mesh connection to target poly. - unsigned int idx = allocLink(tile); - if (idx != DT_NULL_LINK) - { - dtLink* link = &tile->links[idx]; - link->ref = ref; - link->edge = (unsigned char)j; - link->side = 0xff; - link->bmin = link->bmax = 0; - // Add to linked list. - link->next = poly->firstLink; - poly->firstLink = idx; - } + // 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, - // Destination end-point only if it is bidirectional link. - if (j == 0 || (j == 1 && (con->flags & DT_OFFMESH_CON_BIDIR))) - { - // Link target poly to off-mesh connection. - unsigned int idx = allocLink(tile); - if (idx != DT_NULL_LINK) - { - const unsigned short landPolyIdx = (unsigned short)decodePolyIdPoly(ref); - dtPoly* landPoly = &tile->polys[landPolyIdx]; - dtLink* link = &tile->links[idx]; - link->ref = base | (dtPolyRef)(con->poly); - link->edge = 0xff; - link->side = 0xff; - link->bmin = link->bmax = 0; - // Add to linked list. - link->next = landPoly->firstLink; - landPoly->firstLink = idx; - } - } - - } + // 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; } } } -dtStatus dtNavMesh::closestPointOnPolyInTile(const dtMeshTile* tile, unsigned int ip, - const float* pos, float* closest) const +void dtNavMesh::closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const { - const dtPoly* poly = &tile->polys[ip]; + const dtMeshTile* tile = 0; + const dtPoly* poly = 0; + getTileAndPolyByRefUnsafe(ref, &tile, &poly); - float closestDistSqr = FLT_MAX; + // 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]; + const float d0 = dtVdist(pos, v0); + const float d1 = dtVdist(pos, v1); + const float u = d0 / (d0+d1); + dtVlerp(closest, v0, v1, u); + if (posOverPoly) + *posOverPoly = false; + return; + } + + const unsigned int ip = (unsigned int)(poly - tile->polys); const dtPolyDetail* pd = &tile->detailMeshes[ip]; + // Clamp point to be inside the polygon. + float verts[DT_VERTS_PER_POLYGON*3]; + float edged[DT_VERTS_PER_POLYGON]; + float edget[DT_VERTS_PER_POLYGON]; + const int nv = poly->vertCount; + for (int i = 0; i < nv; ++i) + dtVcopy(&verts[i*3], &tile->verts[poly->verts[i]*3]); + + dtVcopy(closest, pos); + if (!dtDistancePtPolyEdgesSqr(pos, verts, nv, edged, edget)) + { + // Point is outside the polygon, dtClamp to nearest edge. + float dmin = FLT_MAX; + int imin = -1; + for (int i = 0; i < nv; ++i) + { + if (edged[i] < dmin) + { + dmin = edged[i]; + imin = i; + } + } + const float* va = &verts[imin*3]; + const float* vb = &verts[((imin+1)%nv)*3]; + dtVlerp(closest, va, vb, edget[imin]); + + if (posOverPoly) + *posOverPoly = false; + } + else + { + if (posOverPoly) + *posOverPoly = true; + } + + // Find height at the location. for (int j = 0; j < pd->triCount; ++j) { const unsigned char* t = &tile->detailTris[(pd->triBase+j)*4]; @@ -575,17 +672,13 @@ dtStatus dtNavMesh::closestPointOnPolyInTile(const dtMeshTile* tile, unsigned in else v[k] = &tile->detailVerts[(pd->vertBase+(t[k]-poly->vertCount))*3]; } - float pt[3]; - dtClosestPtPointTriangle(pt, pos, v[0], v[1], v[2]); - float d = dtVdistSqr(pos, pt); - if (d < closestDistSqr) + float h; + if (dtClosestHeightPointTriangle(pos, v[0], v[1], v[2], h)) { - dtVcopy(closest, pt); - closestDistSqr = d; + closest[1] = h; + break; } } - - return DT_SUCCESS; } dtPolyRef dtNavMesh::findNearestPolyInTile(const dtMeshTile* tile, @@ -607,13 +700,27 @@ dtPolyRef dtNavMesh::findNearestPolyInTile(const dtMeshTile* tile, { dtPolyRef ref = polys[i]; float closestPtPoly[3]; - if (closestPointOnPolyInTile(tile, decodePolyIdPoly(ref), center, closestPtPoly) != DT_SUCCESS) - continue; - float d = dtVdistSqr(center, closestPtPoly); + 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) { - if (nearestPt) - dtVcopy(nearestPt, closestPtPoly); + dtVcopy(nearestPt, closestPtPoly); nearestDistanceSqr = d; nearest = ref; } @@ -706,18 +813,29 @@ int dtNavMesh::queryPolygonsInTile(const dtMeshTile* tile, const float* qmin, co } } +/// @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. +/// +/// @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_DATA_MAGIC; + return DT_FAILURE | DT_WRONG_MAGIC; if (header->version != DT_NAVMESH_VERSION) - return DT_FAILURE_DATA_VERSION; + return DT_FAILURE | DT_WRONG_VERSION; // Make sure the location is free. - if (getTileAt(header->x, header->y)) + if (getTileAt(header->x, header->y, header->layer)) return DT_FAILURE; // Allocate a tile. @@ -736,7 +854,7 @@ dtStatus dtNavMesh::addTile(unsigned char* data, int dataSize, int flags, // Try to relocate the tile to specific index with same salt. int tileIndex = (int)decodePolyIdTile((dtPolyRef)lastRef); if (tileIndex >= m_maxTiles) - return DT_FAILURE_OUT_OF_MEMORY; + 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; @@ -748,7 +866,7 @@ dtStatus dtNavMesh::addTile(unsigned char* data, int dataSize, int flags, } // Could not find the correct location. if (tile != target) - return DT_FAILURE_OUT_OF_MEMORY; + return DT_FAILURE | DT_OUT_OF_MEMORY; // Remove from freelist if (!prev) m_nextFree = tile->next; @@ -761,7 +879,7 @@ dtStatus dtNavMesh::addTile(unsigned char* data, int dataSize, int flags, // Make sure we could allocate a tile. if (!tile) - return DT_FAILURE_OUT_OF_MEMORY; + return DT_FAILURE | DT_OUT_OF_MEMORY; // Insert tile into the position lut. int h = computeTileHash(header->x, header->y, m_tileLutMask); @@ -789,6 +907,10 @@ dtStatus dtNavMesh::addTile(unsigned char* data, int dataSize, int flags, tile->bvTree = (dtBVNode*)d; d += bvtreeSize; tile->offMeshCons = (dtOffMeshConnection*)d; 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; @@ -802,18 +924,36 @@ dtStatus dtNavMesh::addTile(unsigned char* data, int dataSize, int flags, tile->flags = flags; connectIntLinks(tile); - connectIntOffMeshLinks(tile); + baseOffMeshLinks(tile); - // Create connections connections. + // 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) + { + 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) { - dtMeshTile* nei = getNeighbourTileAt(header->x, header->y, i); - if (nei) + nneis = getNeighbourTilesAt(header->x, header->y, i, neis, MAX_NEIS); + for (int j = 0; j < nneis; ++j) { - connectExtLinks(tile, nei, i); - connectExtLinks(nei, tile, dtOppositeTile(i)); - connectExtOffMeshLinks(tile, nei, i); - connectExtOffMeshLinks(nei, tile, dtOppositeTile(i)); + connectExtLinks(tile, neis[j], i); + connectExtLinks(neis[j], tile, dtOppositeTile(i)); + connectExtOffMeshLinks(tile, neis[j], i); + connectExtOffMeshLinks(neis[j], tile, dtOppositeTile(i)); } } @@ -823,55 +963,106 @@ dtStatus dtNavMesh::addTile(unsigned char* data, int dataSize, int flags, return DT_SUCCESS; } -const dtMeshTile* dtNavMesh::getTileAt(int x, int y) const +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) + if (tile->header && + tile->header->x == x && + tile->header->y == y && + tile->header->layer == layer) + { return tile; + } tile = tile->next; } return 0; } -dtMeshTile* dtNavMesh::getNeighbourTileAt(int x, int y, int side) const +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: x++; break; - case 1: x++; y++; break; - case 2: y++; break; - case 3: x--; y++; break; - case 4: x--; break; - case 5: x--; y--; break; - case 6: y--; break; - case 7: x++; y--; break; + 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) - return tile; + if (tile->header && + tile->header->x == x && + tile->header->y == y) + { + if (n < maxTiles) + tiles[n++] = tile; + } tile = tile->next; } - return 0; + + 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(int x, int y) const + +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) + if (tile->header && + tile->header->x == x && + tile->header->y == y && + tile->header->layer == layer) + { return getTileRef(tile); + } tile = tile->next; } return 0; @@ -914,16 +1105,22 @@ void dtNavMesh::calcTileLoc(const float* pos, int* tx, int* ty) const 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; - if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return DT_FAILURE; - if (ip >= (unsigned int)m_tiles[it].header->polyCount) return DT_FAILURE; + 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; @@ -934,6 +1131,7 @@ void dtNavMesh::getTileAndPolyByRefUnsafe(const dtPolyRef ref, const dtMeshTile* 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; @@ -942,17 +1140,23 @@ bool dtNavMesh::isValidPolyRef(dtPolyRef ref) const 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; + 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; + return DT_FAILURE | DT_INVALID_PARAM; dtMeshTile* tile = &m_tiles[tileIndex]; if (tile->salt != tileSalt) - return DT_FAILURE; + return DT_FAILURE | DT_INVALID_PARAM; // Remove tile from hash lookup. int h = computeTileHash(tile->header->x,tile->header->y,m_tileLutMask); @@ -973,14 +1177,27 @@ dtStatus dtNavMesh::removeTile(dtTileRef ref, unsigned char** data, int* dataSiz } // Remove connections to neighbour tiles. - for (int i = 0; i < 8; ++i) + // 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(tile->header->x, tile->header->y, neis, MAX_NEIS); + for (int j = 0; j < nneis; ++j) { - dtMeshTile* nei = getNeighbourTileAt(tile->header->x,tile->header->y,i); - if (!nei) continue; - unconnectExtLinks(nei, dtOppositeTile(i)); + if (neis[j] == tile) continue; + unconnectExtLinks(neis[j], tile); } - + // Connect with 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) + unconnectExtLinks(neis[j], tile); + } + // Reset tile. if (tile->flags & DT_TILE_FREE_DATA) { @@ -1028,6 +1245,20 @@ dtTileRef dtNavMesh::getTileRef(const dtMeshTile* tile) const 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; @@ -1048,6 +1279,7 @@ struct dtPolyState unsigned char area; // Area ID of the polygon. }; +/// @see #storeTileState int dtNavMesh::getTileStateSize(const dtMeshTile* tile) const { if (!tile) return 0; @@ -1056,12 +1288,17 @@ int dtNavMesh::getTileStateSize(const dtMeshTile* tile) const 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; + return DT_FAILURE | DT_BUFFER_TOO_SMALL; dtTileState* tileState = (dtTileState*)data; data += dtAlign4(sizeof(dtTileState)); dtPolyState* polyStates = (dtPolyState*)data; data += dtAlign4(sizeof(dtPolyState) * tile->header->polyCount); @@ -1083,23 +1320,28 @@ dtStatus dtNavMesh::storeTileState(const dtMeshTile* tile, unsigned char* data, 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; + return DT_FAILURE | DT_INVALID_PARAM; const dtTileState* tileState = (const dtTileState*)data; data += dtAlign4(sizeof(dtTileState)); const dtPolyState* polyStates = (const dtPolyState*)data; data += dtAlign4(sizeof(dtPolyState) * tile->header->polyCount); // Check that the restore is possible. if (tileState->magic != DT_NAVMESH_STATE_MAGIC) - return DT_FAILURE_DATA_MAGIC; + return DT_FAILURE | DT_WRONG_MAGIC; if (tileState->version != DT_NAVMESH_STATE_VERSION) - return DT_FAILURE_DATA_VERSION; + return DT_FAILURE | DT_WRONG_VERSION; if (tileState->ref != getTileRef(tile)) - return DT_FAILURE; + return DT_FAILURE | DT_INVALID_PARAM; // Restore per poly state. for (int i = 0; i < tile->header->polyCount; ++i) @@ -1113,17 +1355,26 @@ dtStatus dtNavMesh::restoreTileState(dtMeshTile* tile, const unsigned char* data return DT_SUCCESS; } -// Returns start and end location of an off-mesh link polygon. +/// @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; - if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return DT_FAILURE; + 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; + 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. @@ -1158,6 +1409,9 @@ const dtOffMeshConnection* dtNavMesh::getOffMeshConnectionByRef(dtPolyRef ref) c { 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; @@ -1178,12 +1432,13 @@ const dtOffMeshConnection* dtNavMesh::getOffMeshConnectionByRef(dtPolyRef ref) c 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; - if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return DT_FAILURE; + 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; + if (ip >= (unsigned int)tile->header->polyCount) return DT_FAILURE | DT_INVALID_PARAM; dtPoly* poly = &tile->polys[ip]; // Change flags. @@ -1194,12 +1449,13 @@ dtStatus dtNavMesh::setPolyFlags(dtPolyRef ref, unsigned short flags) 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; - if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return DT_FAILURE; + 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; + if (ip >= (unsigned int)tile->header->polyCount) return DT_FAILURE | DT_INVALID_PARAM; const dtPoly* poly = &tile->polys[ip]; *resultFlags = poly->flags; @@ -1209,12 +1465,13 @@ dtStatus dtNavMesh::getPolyFlags(dtPolyRef ref, unsigned short* resultFlags) con 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; - if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return DT_FAILURE; + 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; + if (ip >= (unsigned int)tile->header->polyCount) return DT_FAILURE | DT_INVALID_PARAM; dtPoly* poly = &tile->polys[ip]; poly->setArea(area); @@ -1224,16 +1481,16 @@ dtStatus dtNavMesh::setPolyArea(dtPolyRef ref, unsigned char area) 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; - if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return DT_FAILURE; + 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; + 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/modules/worldengine/deps/recastnavigation/Detour/DetourNavMeshBuilder.cpp b/modules/worldengine/deps/recastnavigation/Detour/Source/DetourNavMeshBuilder.cpp index 68f3d730da..1bf271bed7 100644 --- a/modules/worldengine/deps/recastnavigation/Detour/DetourNavMeshBuilder.cpp +++ b/modules/worldengine/deps/recastnavigation/Detour/Source/DetourNavMeshBuilder.cpp @@ -16,12 +16,13 @@ // 3. This notice may not be removed or altered from any source distribution. // -#include <math.h> #include <stdio.h> #include <stdlib.h> #include <string.h> +#include <float.h> #include "DetourNavMesh.h" #include "DetourCommon.h" +#include "DetourMath.h" #include "DetourNavMeshBuilder.h" #include "DetourAlloc.h" #include "DetourAssert.h" @@ -201,8 +202,8 @@ static int createBVTree(const unsigned short* verts, const int /*nverts*/, if (z > it.bmax[2]) it.bmax[2] = z; } // Remap y - it.bmin[1] = (unsigned short)floorf((float)it.bmin[1]*ch/cs); - it.bmax[1] = (unsigned short)ceilf((float)it.bmax[1]*ch/cs); + it.bmin[1] = (unsigned short)dtMathFloorf((float)it.bmin[1]*ch/cs); + it.bmax[1] = (unsigned short)dtMathCeilf((float)it.bmax[1]*ch/cs); } int curNode = 0; @@ -237,11 +238,19 @@ static unsigned char classifyOffMeshPoint(const float* pt, const float* bmin, co 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) @@ -252,8 +261,6 @@ bool dtCreateNavMeshData(dtNavMeshCreateParams* params, unsigned char** outData, return false; if (!params->polyCount || !params->polys) return false; - //if (!params->detailMeshes || !params->detailVerts || !params->detailTris) - // return false; const int nvp = params->nvp; @@ -269,10 +276,50 @@ bool dtCreateNavMeshData(dtNavMeshCreateParams* params, unsigned char** outData, 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) { - offMeshConClass[i*2+0] = classifyOffMeshPoint(¶ms->offMeshConVerts[(i*2+0)*3], params->bmin, params->bmax); - offMeshConClass[i*2+1] = classifyOffMeshPoint(¶ms->offMeshConVerts[(i*2+1)*3], params->bmin, params->bmax); + 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) @@ -298,23 +345,13 @@ bool dtCreateNavMeshData(dtNavMeshCreateParams* params, unsigned char** outData, for (int j = 0; j < nvp; ++j) { if (p[j] == MESH_NULL_IDX) break; - int nj = j+1; - if (nj >= nvp || p[nj] == MESH_NULL_IDX) nj = 0; - const unsigned short* va = ¶ms->verts[p[j]*3]; - const unsigned short* vb = ¶ms->verts[p[nj]*3]; - edgeCount++; - if (params->tileSize > 0) + if (p[nvp+j] & 0x8000) { - if (va[0] == params->tileSize && vb[0] == params->tileSize) - portalCount++; // x+ - else if (va[2] == params->tileSize && vb[2] == params->tileSize) - portalCount++; // z+ - else if (va[0] == 0 && vb[0] == 0) - portalCount++; // x- - else if (va[2] == 0 && vb[2] == 0) - portalCount++; // z- + unsigned short dir = p[nvp+j] & 0xf; + if (dir != 0xf) + portalCount++; } } } @@ -368,7 +405,7 @@ bool dtCreateNavMeshData(dtNavMeshCreateParams* params, unsigned char** outData, const int detailMeshesSize = dtAlign4(sizeof(dtPolyDetail)*params->polyCount); const int detailVertsSize = dtAlign4(sizeof(float)*3*uniqueDetailVertCount); const int detailTrisSize = dtAlign4(sizeof(unsigned char)*4*detailTriCount); - const int bvTreeSize = dtAlign4(sizeof(dtBVNode)*params->polyCount*2); + const int bvTreeSize = params->buildBvTree ? dtAlign4(sizeof(dtBVNode)*params->polyCount*2) : 0; const int offMeshConsSize = dtAlign4(sizeof(dtOffMeshConnection)*storedOffMeshConCount); const int dataSize = headerSize + vertsSize + polysSize + linksSize + @@ -400,6 +437,7 @@ bool dtCreateNavMeshData(dtNavMeshCreateParams* params, unsigned char** outData, 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; @@ -415,7 +453,7 @@ bool dtCreateNavMeshData(dtNavMeshCreateParams* params, unsigned char** outData, header->walkableRadius = params->walkableRadius; header->walkableClimb = params->walkableClimb; header->offMeshConCount = storedOffMeshConCount; - header->bvNodeCount = params->polyCount*2; + header->bvNodeCount = params->buildBvTree ? params->polyCount*2 : 0; const int offMeshVertsBase = params->vertCount; const int offMeshPolyBase = params->polyCount; @@ -459,7 +497,27 @@ bool dtCreateNavMeshData(dtNavMeshCreateParams* params, unsigned char** outData, { if (src[j] == MESH_NULL_IDX) break; p->verts[j] = src[j]; - p->neis[j] = (src[nvp+j]+1) & 0xffff; + 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; @@ -481,32 +539,6 @@ bool dtCreateNavMeshData(dtNavMeshCreateParams* params, unsigned char** outData, n++; } } - - // Store portal edges. - if (params->tileSize > 0) - { - for (int i = 0; i < params->polyCount; ++i) - { - dtPoly* poly = &navPolys[i]; - for (int j = 0; j < poly->vertCount; ++j) - { - int nj = j+1; - if (nj >= poly->vertCount) nj = 0; - - const unsigned short* va = ¶ms->verts[poly->verts[j]*3]; - const unsigned short* vb = ¶ms->verts[poly->verts[nj]*3]; - - if (va[0] == params->tileSize && vb[0] == params->tileSize) // x+ - poly->neis[j] = DT_EXT_LINK | 0; - else if (va[2] == params->tileSize && vb[2] == params->tileSize) // z+ - poly->neis[j] = DT_EXT_LINK | 2; - else if (va[0] == 0 && vb[0] == 0) // x- - poly->neis[j] = DT_EXT_LINK | 4; - else if (va[2] == 0 && vb[2] == 0) // z- - poly->neis[j] = DT_EXT_LINK | 6; - } - } - } // Store detail meshes and vertices. // The nav polygon vertices are stored as the first vertices on each mesh. @@ -564,8 +596,11 @@ bool dtCreateNavMeshData(dtNavMeshCreateParams* params, unsigned char** outData, // Store and create BVtree. // TODO: take detail mesh into account! use byte per bbox extent? - createBVTree(params->verts, params->vertCount, params->polys, params->polyCount, - nvp, params->cs, params->ch, params->polyCount*2, navBvtree); + if (params->buildBvTree) + { + createBVTree(params->verts, params->vertCount, params->polys, params->polyCount, + nvp, params->cs, params->ch, params->polyCount*2, navBvtree); + } // Store Off-Mesh connections. n = 0; @@ -597,51 +632,14 @@ bool dtCreateNavMeshData(dtNavMeshCreateParams* params, unsigned char** outData, return true; } -inline void swapByte(unsigned char* a, unsigned char* b) -{ - unsigned char tmp = *a; - *a = *b; - *b = tmp; -} - -inline void swapEndian(unsigned short* v) -{ - unsigned char* x = (unsigned char*)v; - swapByte(x+0, x+1); -} - -inline void swapEndian(short* v) -{ - unsigned char* x = (unsigned char*)v; - swapByte(x+0, x+1); -} - -inline void swapEndian(unsigned int* v) -{ - unsigned char* x = (unsigned char*)v; - swapByte(x+0, x+3); swapByte(x+1, x+2); -} - -inline void swapEndian(int* v) -{ - unsigned char* x = (unsigned char*)v; - swapByte(x+0, x+3); swapByte(x+1, x+2); -} - -inline void swapEndian(float* v) -{ - unsigned char* x = (unsigned char*)v; - swapByte(x+0, x+3); swapByte(x+1, x+2); -} - bool dtNavMeshHeaderSwapEndian(unsigned char* data, const int /*dataSize*/) { dtMeshHeader* header = (dtMeshHeader*)data; int swappedMagic = DT_NAVMESH_MAGIC; int swappedVersion = DT_NAVMESH_VERSION; - swapEndian(&swappedMagic); - swapEndian(&swappedVersion); + dtSwapEndian(&swappedMagic); + dtSwapEndian(&swappedVersion); if ((header->magic != DT_NAVMESH_MAGIC || header->version != DT_NAVMESH_VERSION) && (header->magic != swappedMagic || header->version != swappedVersion)) @@ -649,36 +647,43 @@ bool dtNavMeshHeaderSwapEndian(unsigned char* data, const int /*dataSize*/) return false; } - swapEndian(&header->magic); - swapEndian(&header->version); - swapEndian(&header->x); - swapEndian(&header->y); - swapEndian(&header->userId); - swapEndian(&header->polyCount); - swapEndian(&header->vertCount); - swapEndian(&header->maxLinkCount); - swapEndian(&header->detailMeshCount); - swapEndian(&header->detailVertCount); - swapEndian(&header->detailTriCount); - swapEndian(&header->bvNodeCount); - swapEndian(&header->offMeshConCount); - swapEndian(&header->offMeshBase); - swapEndian(&header->walkableHeight); - swapEndian(&header->walkableRadius); - swapEndian(&header->walkableClimb); - swapEndian(&header->bmin[0]); - swapEndian(&header->bmin[1]); - swapEndian(&header->bmin[2]); - swapEndian(&header->bmax[0]); - swapEndian(&header->bmax[1]); - swapEndian(&header->bmax[2]); - swapEndian(&header->bvQuantFactor); + 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. @@ -712,7 +717,7 @@ bool dtNavMeshDataSwapEndian(unsigned char* data, const int /*dataSize*/) // Vertices for (int i = 0; i < header->vertCount*3; ++i) { - swapEndian(&verts[i]); + dtSwapEndian(&verts[i]); } // Polys @@ -722,10 +727,10 @@ bool dtNavMeshDataSwapEndian(unsigned char* data, const int /*dataSize*/) // poly->firstLink is update when tile is added, no need to swap. for (int j = 0; j < DT_VERTS_PER_POLYGON; ++j) { - swapEndian(&p->verts[j]); - swapEndian(&p->neis[j]); + dtSwapEndian(&p->verts[j]); + dtSwapEndian(&p->neis[j]); } - swapEndian(&p->flags); + dtSwapEndian(&p->flags); } // Links are rebuild when tile is added, no need to swap. @@ -734,14 +739,14 @@ bool dtNavMeshDataSwapEndian(unsigned char* data, const int /*dataSize*/) for (int i = 0; i < header->detailMeshCount; ++i) { dtPolyDetail* pd = &detailMeshes[i]; - swapEndian(&pd->vertBase); - swapEndian(&pd->triBase); + dtSwapEndian(&pd->vertBase); + dtSwapEndian(&pd->triBase); } // Detail verts for (int i = 0; i < header->detailVertCount*3; ++i) { - swapEndian(&detailVerts[i]); + dtSwapEndian(&detailVerts[i]); } // BV-tree @@ -750,10 +755,10 @@ bool dtNavMeshDataSwapEndian(unsigned char* data, const int /*dataSize*/) dtBVNode* node = &bvTree[i]; for (int j = 0; j < 3; ++j) { - swapEndian(&node->bmin[j]); - swapEndian(&node->bmax[j]); + dtSwapEndian(&node->bmin[j]); + dtSwapEndian(&node->bmax[j]); } - swapEndian(&node->i); + dtSwapEndian(&node->i); } // Off-mesh Connections. @@ -761,9 +766,9 @@ bool dtNavMeshDataSwapEndian(unsigned char* data, const int /*dataSize*/) { dtOffMeshConnection* con = &offMeshCons[i]; for (int j = 0; j < 6; ++j) - swapEndian(&con->pos[j]); - swapEndian(&con->rad); - swapEndian(&con->poly); + dtSwapEndian(&con->pos[j]); + dtSwapEndian(&con->rad); + dtSwapEndian(&con->poly); } return true; diff --git a/modules/worldengine/deps/recastnavigation/Detour/DetourNavMeshQuery.cpp b/modules/worldengine/deps/recastnavigation/Detour/Source/DetourNavMeshQuery.cpp index 33ee87fb95..fbf3724e85 100644 --- a/modules/worldengine/deps/recastnavigation/Detour/DetourNavMeshQuery.cpp +++ b/modules/worldengine/deps/recastnavigation/Detour/Source/DetourNavMeshQuery.cpp @@ -16,17 +16,49 @@ // 3. This notice may not be removed or altered from any source distribution. // -#include <math.h> #include <float.h> #include <string.h> #include "DetourNavMeshQuery.h" #include "DetourNavMesh.h" #include "DetourNode.h" #include "DetourCommon.h" +#include "DetourMath.h" #include "DetourAlloc.h" #include "DetourAssert.h" #include <new> +/// @class dtQueryFilter +/// +/// <b>The Default Implementation</b> +/// +/// 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. +/// +/// <b>Custom Implementations</b> +/// +/// 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), @@ -68,6 +100,7 @@ inline float dtQueryFilter::getCost(const float* pa, const float* pb, } #endif +// Edited by TC static const float H_SCALE = 2.0f; // Search heuristic scale. @@ -86,6 +119,23 @@ void dtFreeNavMeshQuery(dtNavMeshQuery* navmesh) } ////////////////////////////////////////////////////////////////////////////////////////// + +/// @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), @@ -108,6 +158,12 @@ dtNavMeshQuery::~dtNavMeshQuery() 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) { m_nav = nav; @@ -122,7 +178,7 @@ dtStatus dtNavMeshQuery::init(const dtNavMesh* nav, const int maxNodes) } m_nodePool = new (dtAlloc(sizeof(dtNodePool), DT_ALLOC_PERM)) dtNodePool(maxNodes, dtNextPow2(maxNodes/4)); if (!m_nodePool) - return DT_FAILURE_OUT_OF_MEMORY; + return DT_FAILURE | DT_OUT_OF_MEMORY; } else { @@ -133,7 +189,7 @@ dtStatus dtNavMeshQuery::init(const dtNavMesh* nav, const int maxNodes) { m_tinyNodePool = new (dtAlloc(sizeof(dtNodePool), DT_ALLOC_PERM)) dtNodePool(64, 32); if (!m_tinyNodePool) - return DT_FAILURE_OUT_OF_MEMORY; + return DT_FAILURE | DT_OUT_OF_MEMORY; } else { @@ -151,7 +207,7 @@ dtStatus dtNavMeshQuery::init(const dtNavMesh* nav, const int maxNodes) } m_openList = new (dtAlloc(sizeof(dtNodeQueue), DT_ALLOC_PERM)) dtNodeQueue(maxNodes); if (!m_openList) - return DT_FAILURE_OUT_OF_MEMORY; + return DT_FAILURE | DT_OUT_OF_MEMORY; } else { @@ -161,33 +217,318 @@ dtStatus dtNavMeshQuery::init(const dtNavMesh* nav, const int maxNodes) return DT_SUCCESS; } -////////////////////////////////////////////////////////////////////////////////////////// -dtStatus dtNavMeshQuery::closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest) const +dtStatus dtNavMeshQuery::findRandomPoint(const dtQueryFilter* filter, float (*frand)(), + dtPolyRef* randomRef, float* randomPt) const { dtAssert(m_nav); + + // Randomly pick one tile. Assume that all tiles cover roughly the same area. const dtMeshTile* tile = 0; + float tsum = 0.0f; + for (int i = 0; i < m_nav->getMaxTiles(); i++) + { + const dtMeshTile* t = m_nav->getTile(i); + if (!t || !t->header) continue; + + // Choose random tile using reservoi sampling. + const float area = 1.0f; // Could be tile area too. + tsum += area; + const float u = frand(); + if (u*tsum <= area) + tile = t; + } + if (!tile) + return DT_FAILURE; + + // Randomly pick one polygon weighted by polygon area. const dtPoly* poly = 0; - if (m_nav->getTileAndPolyByRef(ref, &tile, &poly) != DT_SUCCESS) + dtPolyRef polyRef = 0; + const dtPolyRef base = m_nav->getPolyRefBase(tile); + + float areaSum = 0.0f; + for (int i = 0; i < tile->header->polyCount; ++i) + { + const dtPoly* p = &tile->polys[i]; + // Do not return off-mesh connection polygons. + if (p->getType() != DT_POLYTYPE_GROUND) + continue; + // Must pass filter + const dtPolyRef ref = base | (dtPolyRef)i; + if (!filter->passFilter(ref, tile, p)) + continue; + + // Calc area of the polygon. + float polyArea = 0.0f; + for (int j = 2; j < p->vertCount; ++j) + { + const float* va = &tile->verts[p->verts[0]*3]; + const float* vb = &tile->verts[p->verts[j-1]*3]; + const float* vc = &tile->verts[p->verts[j]*3]; + polyArea += dtTriArea2D(va,vb,vc); + } + + // Choose random polygon weighted by area, using reservoi sampling. + areaSum += polyArea; + const float u = frand(); + if (u*areaSum <= polyArea) + { + poly = p; + polyRef = ref; + } + } + + if (!poly) return DT_FAILURE; - if (!tile) return DT_FAILURE; - if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) - return DT_FAILURE; + // Randomly pick point on polygon. + const float* v = &tile->verts[poly->verts[0]*3]; + float verts[3*DT_VERTS_PER_POLYGON]; + float areas[DT_VERTS_PER_POLYGON]; + dtVcopy(&verts[0*3],v); + for (int j = 1; j < poly->vertCount; ++j) + { + v = &tile->verts[poly->verts[j]*3]; + dtVcopy(&verts[j*3],v); + } + + const float s = frand(); + const float t = frand(); + + float pt[3]; + dtRandomPointInConvexPoly(verts, poly->vertCount, areas, s, t, pt); + + float h = 0.0f; + dtStatus status = getPolyHeight(polyRef, pt, &h); + if (dtStatusFailed(status)) + return status; + pt[1] = h; + + dtVcopy(randomPt, pt); + *randomRef = polyRef; + + return DT_SUCCESS; +} + +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 (!startRef || !m_nav->isValidPolyRef(startRef)) + 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; - if (closestPointOnPolyInTile(tile, poly, pos, closest) != DT_SUCCESS) + while (!m_openList->empty()) + { + dtNode* bestNode = m_openList->pop(); + bestNode->flags &= ~DT_NODE_OPEN; + bestNode->flags |= DT_NODE_CLOSED; + + // Get poly and tile. + // The API input has been cheked already, skip checking internal data. + const dtPolyRef bestRef = bestNode->id; + const dtMeshTile* bestTile = 0; + const dtPoly* bestPoly = 0; + m_nav->getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly); + + // Place random locations on on ground. + if (bestPoly->getType() == DT_POLYTYPE_GROUND) + { + // Calc area of the polygon. + float polyArea = 0.0f; + for (int j = 2; j < bestPoly->vertCount; ++j) + { + const float* va = &bestTile->verts[bestPoly->verts[0]*3]; + const float* vb = &bestTile->verts[bestPoly->verts[j-1]*3]; + const float* vc = &bestTile->verts[bestPoly->verts[j]*3]; + polyArea += dtTriArea2D(va,vb,vc); + } + // Choose random polygon weighted by area, using reservoi sampling. + areaSum += polyArea; + const float u = frand(); + if (u*areaSum <= polyArea) + { + randomTile = bestTile; + randomPoly = bestPoly; + randomPolyRef = bestRef; + } + } + + + // Get parent poly and tile. + dtPolyRef parentRef = 0; + const dtMeshTile* parentTile = 0; + const dtPoly* parentPoly = 0; + if (bestNode->pidx) + parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id; + if (parentRef) + m_nav->getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly); + + for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next) + { + const dtLink* link = &bestTile->links[i]; + dtPolyRef neighbourRef = link->ref; + // Skip invalid neighbours and do not follow back to parent. + if (!neighbourRef || neighbourRef == parentRef) + continue; + + // Expand to neighbour + const dtMeshTile* neighbourTile = 0; + const dtPoly* neighbourPoly = 0; + m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly); + + // Do not advance if the polygon is excluded by the filter. + if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly)) + continue; + + // Find edge and calc distance to the edge. + float va[3], vb[3]; + if (!getPortalPoints(bestRef, bestPoly, bestTile, neighbourRef, neighbourPoly, neighbourTile, va, vb)) + continue; + + // If the circle is not touching the next polygon, skip it. + float tseg; + float distSqr = dtDistancePtSegSqr2D(centerPos, va, vb, tseg); + if (distSqr > radiusSqr) + continue; + + dtNode* neighbourNode = m_nodePool->getNode(neighbourRef); + if (!neighbourNode) + { + status |= DT_OUT_OF_NODES; + continue; + } + + if (neighbourNode->flags & DT_NODE_CLOSED) + continue; + + // Cost + if (neighbourNode->flags == 0) + dtVlerp(neighbourNode->pos, va, vb, 0.5f); + + const float total = bestNode->total + dtVdist(bestNode->pos, neighbourNode->pos); + + // The node is already in open list and the new result is worse, skip. + if ((neighbourNode->flags & DT_NODE_OPEN) && total >= neighbourNode->total) + continue; + + neighbourNode->id = neighbourRef; + neighbourNode->flags = (neighbourNode->flags & ~DT_NODE_CLOSED); + neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode); + neighbourNode->total = total; + + if (neighbourNode->flags & DT_NODE_OPEN) + { + m_openList->modify(neighbourNode); + } + else + { + neighbourNode->flags = DT_NODE_OPEN; + m_openList->push(neighbourNode); + } + } + } + + if (!randomPoly) return DT_FAILURE; + + // Randomly pick point on polygon. + const float* v = &randomTile->verts[randomPoly->verts[0]*3]; + float verts[3*DT_VERTS_PER_POLYGON]; + float areas[DT_VERTS_PER_POLYGON]; + dtVcopy(&verts[0*3],v); + for (int j = 1; j < randomPoly->vertCount; ++j) + { + v = &randomTile->verts[randomPoly->verts[j]*3]; + dtVcopy(&verts[j*3],v); + } + + const float s = frand(); + const float t = frand(); + + float pt[3]; + dtRandomPointInConvexPoly(verts, randomPoly->vertCount, areas, s, t, pt); + + float h = 0.0f; + dtStatus stat = getPolyHeight(randomPolyRef, pt, &h); + if (dtStatusFailed(status)) + return stat; + pt[1] = h; + + dtVcopy(randomPt, pt); + *randomRef = randomPolyRef; + return DT_SUCCESS; } -dtStatus dtNavMeshQuery::closestPointOnPolyInTile(const dtMeshTile* tile, const dtPoly* poly, - const float* pos, float* closest) const + +////////////////////////////////////////////////////////////////////////////////////////// + +/// @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); + const dtMeshTile* tile = 0; + const dtPoly* poly = 0; + if (dtStatusFailed(m_nav->getTileAndPolyByRef(ref, &tile, &poly))) + return DT_FAILURE | DT_INVALID_PARAM; + if (!tile) + return DT_FAILURE | DT_INVALID_PARAM; + + // 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]; + const float d0 = dtVdist(pos, v0); + const float d1 = dtVdist(pos, v1); + const float u = d0 / (d0+d1); + dtVlerp(closest, v0, v1, u); + if (posOverPoly) + *posOverPoly = false; + return DT_SUCCESS; + } + const unsigned int ip = (unsigned int)(poly - tile->polys); const dtPolyDetail* pd = &tile->detailMeshes[ip]; - // TODO: The commented out version finds 'cylinder distance' instead of 'sphere distance' to the navmesh. - // Test and enable. -/* // Clamp point to be inside the polygon. float verts[DT_VERTS_PER_POLYGON*3]; float edged[DT_VERTS_PER_POLYGON]; @@ -213,6 +554,14 @@ dtStatus dtNavMeshQuery::closestPointOnPolyInTile(const dtMeshTile* tile, const const float* va = &verts[imin*3]; const float* vb = &verts[((imin+1)%nv)*3]; dtVlerp(closest, va, vb, edget[imin]); + + if (posOverPoly) + *posOverPoly = false; + } + else + { + if (posOverPoly) + *posOverPoly = true; } // Find height at the location. @@ -234,42 +583,29 @@ dtStatus dtNavMeshQuery::closestPointOnPolyInTile(const dtMeshTile* tile, const break; } } -*/ - float closestDistSqr = FLT_MAX; - for (int j = 0; j < pd->triCount; ++j) - { - const unsigned char* t = &tile->detailTris[(pd->triBase+j)*4]; - const float* v[3]; - for (int k = 0; k < 3; ++k) - { - if (t[k] < poly->vertCount) - v[k] = &tile->verts[poly->verts[t[k]]*3]; - else - v[k] = &tile->detailVerts[(pd->vertBase+(t[k]-poly->vertCount))*3]; - } - - float pt[3]; - dtClosestPtPointTriangle(pt, pos, v[0], v[1], v[2]); - float d = dtVdistSqr(pos, pt); - - if (d < closestDistSqr) - { - dtVcopy(closest, pt); - closestDistSqr = d; - } - } return DT_SUCCESS; } +/// @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 (m_nav->getTileAndPolyByRef(ref, &tile, &poly) != DT_SUCCESS) - return DT_FAILURE; + if (dtStatusFailed(m_nav->getTileAndPolyByRef(ref, &tile, &poly))) + return DT_FAILURE | DT_INVALID_PARAM; // Collect vertices. float verts[DT_VERTS_PER_POLYGON*3]; @@ -309,22 +645,26 @@ dtStatus dtNavMeshQuery::closestPointOnPolyBoundary(dtPolyRef ref, const float* return DT_SUCCESS; } - +/// @par +/// +/// Will return #DT_FAILURE 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 (m_nav->getTileAndPolyByRef(ref, &tile, &poly) != DT_SUCCESS) - return DT_FAILURE; + if (dtStatusFailed(m_nav->getTileAndPolyByRef(ref, &tile, &poly))) + return DT_FAILURE | DT_INVALID_PARAM; if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) { const float* v0 = &tile->verts[poly->verts[0]*3]; const float* v1 = &tile->verts[poly->verts[1]*3]; - const float d0 = dtVdist(pos, v0); - const float d1 = dtVdist(pos, v1); + const float d0 = dtVdist2D(pos, v0); + const float d1 = dtVdist2D(pos, v1); const float u = d0 / (d0+d1); if (height) *height = v0[1] + (v1[1] - v0[1]) * u; @@ -355,9 +695,18 @@ dtStatus dtNavMeshQuery::getPolyHeight(dtPolyRef ref, const float* pos, float* h } } - return DT_FAILURE; + return DT_FAILURE | DT_INVALID_PARAM; } +/// @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. +/// +/// @warning This function is not suitable for large area searches. If the search +/// extents overlaps more than MAX_SEARCH (128) polygons it may return an invalid result. +/// dtStatus dtNavMeshQuery::findNearestPoly(const float* center, const float* extents, const dtQueryFilter* filter, dtPolyRef* nearestRef, float* nearestPt) const @@ -367,10 +716,11 @@ dtStatus dtNavMeshQuery::findNearestPoly(const float* center, const float* exten *nearestRef = 0; // Get nearby polygons from proximity grid. - dtPolyRef polys[128]; + const int MAX_SEARCH = 128; + dtPolyRef polys[MAX_SEARCH]; int polyCount = 0; - if (queryPolygons(center, extents, filter, polys, &polyCount, 128) != DT_SUCCESS) - return DT_FAILURE; + if (dtStatusFailed(queryPolygons(center, extents, filter, polys, &polyCount, MAX_SEARCH))) + return DT_FAILURE | DT_INVALID_PARAM; // Find nearest polygon amongst the nearby polygons. dtPolyRef nearest = 0; @@ -379,9 +729,27 @@ dtStatus dtNavMeshQuery::findNearestPoly(const float* center, const float* exten { dtPolyRef ref = polys[i]; float closestPtPoly[3]; - if (closestPointOnPoly(ref, center, closestPtPoly) != DT_SUCCESS) - continue; - float d = dtVdistSqr(center, closestPtPoly); + float diff[3]; + bool posOverPoly = false; + float d = 0; + 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) + { + const dtMeshTile* tile = 0; + const dtPoly* poly = 0; + m_nav->getTileAndPolyByRefUnsafe(polys[i], &tile, &poly); + d = dtAbs(diff[1]) - tile->header->walkableClimb; + d = d > 0 ? d*d : 0; + } + else + { + d = dtVlenSqr(diff); + } + if (d < nearestDistanceSqr) { if (nearestPt) @@ -397,43 +765,6 @@ dtStatus dtNavMeshQuery::findNearestPoly(const float* center, const float* exten return DT_SUCCESS; } -dtPolyRef dtNavMeshQuery::findNearestPolyInTile(const dtMeshTile* tile, const float* center, const float* extents, - const dtQueryFilter* filter, float* nearestPt) const -{ - dtAssert(m_nav); - - float bmin[3], bmax[3]; - dtVsub(bmin, center, extents); - dtVadd(bmax, center, extents); - - // Get nearby polygons from proximity grid. - dtPolyRef polys[128]; - int polyCount = queryPolygonsInTile(tile, bmin, bmax, filter, polys, 128); - - // Find nearest polygon amongst the nearby polygons. - dtPolyRef nearest = 0; - float nearestDistanceSqr = FLT_MAX; - for (int i = 0; i < polyCount; ++i) - { - dtPolyRef ref = polys[i]; - const dtPoly* poly = &tile->polys[m_nav->decodePolyIdPoly(ref)]; - float closestPtPoly[3]; - if (closestPointOnPolyInTile(tile, poly, center, closestPtPoly) != DT_SUCCESS) - continue; - - float d = dtVdistSqr(center, closestPtPoly); - if (d < nearestDistanceSqr) - { - if (nearestPt) - dtVcopy(nearestPt, closestPtPoly); - nearestDistanceSqr = d; - nearest = ref; - } - } - - return nearest; -} - int dtNavMeshQuery::queryPolygonsInTile(const dtMeshTile* tile, const float* qmin, const float* qmax, const dtQueryFilter* filter, dtPolyRef* polys, const int maxPolys) const @@ -501,12 +832,15 @@ int dtNavMeshQuery::queryPolygonsInTile(const dtMeshTile* tile, const float* qmi const dtPolyRef base = m_nav->getPolyRefBase(tile); for (int i = 0; i < tile->header->polyCount; ++i) { - // Calc polygon bounds. - dtPoly* p = &tile->polys[i]; + const 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); @@ -518,18 +852,23 @@ int dtNavMeshQuery::queryPolygonsInTile(const dtMeshTile* tile, const float* qmi } if (dtOverlapBounds(qmin,qmax, bmin,bmax)) { - const dtPolyRef ref = base | (dtPolyRef)i; - if (filter->passFilter(ref, tile, p)) - { - if (n < maxPolys) - polys[n++] = ref; - } + if (n < maxPolys) + polys[n++] = ref; } } return n; } } +/// @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* extents, const dtQueryFilter* filter, dtPolyRef* polys, int* polyCount, const int maxPolys) const @@ -545,27 +884,23 @@ dtStatus dtNavMeshQuery::queryPolygons(const float* center, const float* extents m_nav->calcTileLoc(bmin, &minx, &miny); m_nav->calcTileLoc(bmax, &maxx, &maxy); + static const int MAX_NEIS = 32; + const dtMeshTile* neis[MAX_NEIS]; + int n = 0; - - /// pussywizard: additional checks as in PathGenerator::HaveTile - if (minx < 0) minx = 0; if (miny < 0) miny = 0; // min can be negative because we subtract extents (few lines above) - if (maxx < 0 || maxy < 0 || maxx-minx >= 64 /*MAX_NUMBER_OF_GRIDS*/ || maxy-miny >= 64) // max should never be negative - { - *polyCount = n; - return DT_SUCCESS; - } - for (int y = miny; y <= maxy; ++y) { for (int x = minx; x <= maxx; ++x) { - const dtMeshTile* tile = m_nav->getTileAt(x,y); - if (!tile) continue; - n += queryPolygonsInTile(tile, bmin, bmax, filter, polys+n, maxPolys-n); - if (n >= maxPolys) + const int nneis = m_nav->getTilesAt(x,y,neis,MAX_NEIS); + for (int j = 0; j < nneis; ++j) { - *polyCount = n; - return DT_SUCCESS; + n += queryPolygonsInTile(neis[j], bmin, bmax, filter, polys+n, maxPolys-n); + if (n >= maxPolys) + { + *polyCount = n; + return DT_SUCCESS | DT_BUFFER_TOO_SMALL; + } } } } @@ -574,6 +909,17 @@ dtStatus dtNavMeshQuery::queryPolygons(const float* center, const float* extents 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, @@ -586,14 +932,14 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef, *pathCount = 0; if (!startRef || !endRef) - return DT_FAILURE; + return DT_FAILURE | DT_INVALID_PARAM; if (!maxPath) - return DT_FAILURE; + return DT_FAILURE | DT_INVALID_PARAM; // Validate input if (!m_nav->isValidPolyRef(startRef) || !m_nav->isValidPolyRef(endRef)) - return DT_FAILURE; + return DT_FAILURE | DT_INVALID_PARAM; if (startRef == endRef) { @@ -617,6 +963,8 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef, dtNode* lastBestNode = startNode; float lastBestNodeCost = startNode->total; + dtStatus status = DT_SUCCESS; + while (!m_openList->empty()) { // Remove node from open list and put it in closed list. @@ -664,9 +1012,18 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef, if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly)) continue; - dtNode* neighbourNode = m_nodePool->getNode(neighbourRef); + // 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) + { + status |= DT_OUT_OF_NODES; continue; + } // If the node is visited the first time, calculate node position. if (neighbourNode->flags == 0) @@ -719,7 +1076,7 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef, // Add or update the node. neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode); neighbourNode->id = neighbourRef; - neighbourNode->flags &= ~DT_NODE_CLOSED; + neighbourNode->flags = (neighbourNode->flags & ~DT_NODE_CLOSED); neighbourNode->cost = cost; neighbourNode->total = total; @@ -744,6 +1101,9 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef, } } + if (lastBestNode->id != endRef) + status |= DT_PARTIAL_RESULT; + // Reverse the path. dtNode* prev = 0; dtNode* node = lastBestNode; @@ -762,18 +1122,31 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef, do { path[n++] = node->id; + if (n >= maxPath) + { + status |= DT_BUFFER_TOO_SMALL; + break; + } node = m_nodePool->getNodeAtIdx(node->pidx); } - while (node && n < maxPath); + while (node); *pathCount = n; - return DT_SUCCESS; + return status; } +/// @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 dtQueryFilter* filter, const unsigned int options) { dtAssert(m_nav); dtAssert(m_nodePool); @@ -787,13 +1160,25 @@ dtStatus dtNavMeshQuery::initSlicedFindPath(dtPolyRef startRef, dtPolyRef endRef dtVcopy(m_query.startPos, startPos); dtVcopy(m_query.endPos, endPos); m_query.filter = filter; + m_query.options = options; + m_query.raycastLimitSqr = FLT_MAX; if (!startRef || !endRef) - return DT_FAILURE; + return DT_FAILURE | DT_INVALID_PARAM; // Validate input if (!m_nav->isValidPolyRef(startRef) || !m_nav->isValidPolyRef(endRef)) - return DT_FAILURE; + 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) { @@ -820,9 +1205,9 @@ dtStatus dtNavMeshQuery::initSlicedFindPath(dtPolyRef startRef, dtPolyRef endRef return m_query.status; } -dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter) +dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters) { - if (m_query.status!= DT_IN_PROGRESS) + if (!dtStatusInProgress(m_query.status)) return m_query.status; // Make sure the request is still valid. @@ -831,6 +1216,9 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter) m_query.status = DT_FAILURE; return DT_FAILURE; } + + dtRaycastHit rayHit; + rayHit.maxPath = 0; int iter = 0; while (iter < maxIter && !m_openList->empty()) @@ -846,7 +1234,10 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter) if (bestNode->id == m_query.endRef) { m_query.lastBestNode = bestNode; - m_query.status = DT_SUCCESS; + const dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK; + m_query.status = DT_SUCCESS | details; + if (doneIters) + *doneIters = iter; return m_query.status; } @@ -855,28 +1246,47 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter) const dtPolyRef bestRef = bestNode->id; const dtMeshTile* bestTile = 0; const dtPoly* bestPoly = 0; - if (m_nav->getTileAndPolyByRef(bestRef, &bestTile, &bestPoly) != DT_SUCCESS) + 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 poly and tile. - dtPolyRef parentRef = 0; + // 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) - parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id; + { + parentNode = m_nodePool->getNodeAtIdx(bestNode->pidx); + parentRef = parentNode->id; + if (parentNode->pidx) + grandpaRef = m_nodePool->getNodeAtIdx(parentNode->pidx)->id; + } if (parentRef) { - if (m_nav->getTileAndPolyByRef(parentRef, &parentTile, &parentPoly) != DT_SUCCESS) + 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) { @@ -895,10 +1305,18 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter) if (!m_query.filter->passFilter(neighbourRef, neighbourTile, neighbourPoly)) continue; - dtNode* neighbourNode = m_nodePool->getNode(neighbourRef); + // 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) { @@ -911,30 +1329,44 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter) float cost = 0; float heuristic = 0; - // Special case for last node. - if (neighbourRef == m_query.endRef) + // raycast parent + bool foundShortCut = false; + rayHit.pathCost = rayHit.t = 0; + if (tryLOS) { - // Cost + 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); + 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 = bestNode->cost + curCost + endCost; + cost = cost + endCost; heuristic = 0; } else { - // Cost - const float curCost = m_query.filter->getCost(bestNode->pos, neighbourNode->pos, - parentRef, parentTile, parentPoly, - bestRef, bestTile, bestPoly, - neighbourRef, neighbourTile, neighbourPoly); - cost = bestNode->cost + curCost; heuristic = dtVdist(neighbourNode->pos, m_query.endPos)*H_SCALE; } @@ -948,11 +1380,13 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter) continue; // Add or update the node. - neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode); + neighbourNode->pidx = foundShortCut ? bestNode->pidx : m_nodePool->getNodeIdx(bestNode); neighbourNode->id = neighbourRef; - neighbourNode->flags &= ~DT_NODE_CLOSED; + 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) { @@ -977,7 +1411,13 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter) // Exhausted all nodes, but could not find path. if (m_openList->empty()) - m_query.status = DT_SUCCESS; + { + const dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK; + m_query.status = DT_SUCCESS | details; + } + + if (doneIters) + *doneIters = iter; return m_query.status; } @@ -986,7 +1426,7 @@ dtStatus dtNavMeshQuery::finalizeSlicedFindPath(dtPolyRef* path, int* pathCount, { *pathCount = 0; - if (m_query.status != DT_SUCCESS) + if (dtStatusFailed(m_query.status)) { // Reset query. memset(&m_query, 0, sizeof(dtQueryData)); @@ -1004,13 +1444,21 @@ dtStatus dtNavMeshQuery::finalizeSlicedFindPath(dtPolyRef* path, int* pathCount, { // 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); @@ -1019,18 +1467,43 @@ dtStatus dtNavMeshQuery::finalizeSlicedFindPath(dtPolyRef* path, int* pathCount, node = prev; do { - path[n++] = node->id; - node = m_nodePool->getNodeAtIdx(node->pidx); + 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 && n < maxPath); + 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; + return DT_SUCCESS | details; } dtStatus dtNavMeshQuery::finalizeSlicedFindPathPartial(const dtPolyRef* existing, const int existingSize, @@ -1043,7 +1516,7 @@ dtStatus dtNavMeshQuery::finalizeSlicedFindPathPartial(const dtPolyRef* existing return DT_FAILURE; } - if (m_query.status != DT_SUCCESS && m_query.status != DT_IN_PROGRESS) + if (dtStatusFailed(m_query.status)) { // Reset query. memset(&m_query, 0, sizeof(dtQueryData)); @@ -1064,22 +1537,28 @@ dtStatus dtNavMeshQuery::finalizeSlicedFindPathPartial(const dtPolyRef* existing dtNode* node = 0; for (int i = existingSize-1; i >= 0; --i) { - node = m_nodePool->findNode(existing[i]); + m_nodePool->findNodes(existing[i], &node, 1); if (node) break; } if (!node) { - return DT_FAILURE; + 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); @@ -1088,59 +1567,175 @@ dtStatus dtNavMeshQuery::finalizeSlicedFindPathPartial(const dtPolyRef* existing node = prev; do { - path[n++] = node->id; - node = m_nodePool->getNodeAtIdx(node->pidx); + 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 && n < maxPath); + 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; + 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 reached end of path or there is no space to append more vertices, return. + if (flags == DT_STRAIGHTPATH_END || (*straightPathCount) >= maxStraightPath) + { + return DT_SUCCESS | (((*straightPathCount) >= maxStraightPath) ? DT_BUFFER_TOO_SMALL : 0); + } + } + 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* straightPathCount, const int maxStraightPath, const int options) const { dtAssert(m_nav); *straightPathCount = 0; if (!maxStraightPath) - return DT_FAILURE; + return DT_FAILURE | DT_INVALID_PARAM; if (!path[0]) - return DT_FAILURE; + return DT_FAILURE | DT_INVALID_PARAM; - int n = 0; + dtStatus stat = 0; // TODO: Should this be callers responsibility? float closestStartPos[3]; - if (closestPointOnPolyBoundary(path[0], startPos, closestStartPos) != DT_SUCCESS) - return DT_FAILURE; + 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. - dtVcopy(&straightPath[n*3], closestStartPos); - if (straightPathFlags) - straightPathFlags[n] = DT_STRAIGHTPATH_START; - if (straightPathRefs) - straightPathRefs[n] = path[0]; - n++; - if (n >= maxStraightPath) - { - *straightPathCount = n; - return DT_SUCCESS; - } - - float closestEndPos[3]; - if (closestPointOnPolyBoundary(path[pathSize-1], endPos, closestEndPos) != DT_SUCCESS) - return DT_FAILURE; + stat = appendVertex(closestStartPos, DT_STRAIGHTPATH_START, path[0], + straightPath, straightPathFlags, straightPathRefs, + straightPathCount, maxStraightPath); + if (stat != DT_IN_PROGRESS) + return stat; if (pathSize > 1) { @@ -1166,19 +1761,30 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en if (i+1 < pathSize) { // Next portal. - if (getPortalPoints(path[i], path[i+1], left, right, fromType, toType) != DT_SUCCESS) + if (dtStatusFailed(getPortalPoints(path[i], path[i+1], left, right, fromType, toType))) { - if (closestPointOnPolyBoundary(path[i], endPos, closestEndPos) != DT_SUCCESS) - return DT_FAILURE; + // 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. - dtVcopy(&straightPath[n*3], closestEndPos); - if (straightPathFlags) - straightPathFlags[n] = 0; - if (straightPathRefs) - straightPathRefs[n] = path[i]; - n++; + 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)) + { + stat = appendPortals(apexIndex, i, closestEndPos, path, + straightPath, straightPathFlags, straightPathRefs, + straightPathCount, maxStraightPath, options); + } + + stat = appendVertex(closestEndPos, 0, path[i], + straightPath, straightPathFlags, straightPathRefs, + straightPathCount, maxStraightPath); - return DT_SUCCESS; + return DT_SUCCESS | DT_PARTIAL_RESULT | ((*straightPathCount >= maxStraightPath) ? DT_BUFFER_TOO_SMALL : 0); } // If starting really close the portal, advance. @@ -1210,6 +1816,16 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en } 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; @@ -1220,30 +1836,12 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en flags = DT_STRAIGHTPATH_OFFMESH_CONNECTION; dtPolyRef ref = leftPolyRef; - if (!dtVequal(&straightPath[(n-1)*3], portalApex)) - { - // Append new vertex. - dtVcopy(&straightPath[n*3], portalApex); - if (straightPathFlags) - straightPathFlags[n] = flags; - if (straightPathRefs) - straightPathRefs[n] = ref; - n++; - // If reached end of path or there is no space to append more vertices, return. - if (flags == DT_STRAIGHTPATH_END || n >= maxStraightPath) - { - *straightPathCount = n; - return DT_SUCCESS; - } - } - else - { - // The vertices are equal, update flags and poly. - if (straightPathFlags) - straightPathFlags[n-1] = flags; - if (straightPathRefs) - straightPathRefs[n-1] = ref; - } + // 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); @@ -1269,6 +1867,16 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en } 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; @@ -1278,31 +1886,13 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en else if (rightPolyType == DT_POLYTYPE_OFFMESH_CONNECTION) flags = DT_STRAIGHTPATH_OFFMESH_CONNECTION; dtPolyRef ref = rightPolyRef; - - if (!dtVequal(&straightPath[(n-1)*3], portalApex)) - { - // Append new vertex. - dtVcopy(&straightPath[n*3], portalApex); - if (straightPathFlags) - straightPathFlags[n] = flags; - if (straightPathRefs) - straightPathRefs[n] = ref; - n++; - // If reached end of path or there is no space to append more vertices, return. - if (flags == DT_STRAIGHTPATH_END || n >= maxStraightPath) - { - *straightPathCount = n; - return DT_SUCCESS; - } - } - else - { - // The vertices are equal, update flags and poly. - if (straightPathFlags) - straightPathFlags[n-1] = flags; - if (straightPathRefs) - straightPathRefs[n-1] = ref; - } + + // 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); @@ -1316,27 +1906,45 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en } } } + + // 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; + } } + + stat = appendVertex(closestEndPos, DT_STRAIGHTPATH_END, 0, + straightPath, straightPathFlags, straightPathRefs, + straightPathCount, maxStraightPath); - // If the point already exists, remove it and add reappend the actual end location. - if (n > 0 && dtVequal(&straightPath[(n-1)*3], closestEndPos)) - n--; - - // Add end point. - if (n < maxStraightPath) - { - dtVcopy(&straightPath[n*3], closestEndPos); - if (straightPathFlags) - straightPathFlags[n] = DT_STRAIGHTPATH_END; - if (straightPathRefs) - straightPathRefs[n] = 0; - n++; - } - - *straightPathCount = n; - return DT_SUCCESS; + 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 @@ -1347,8 +1955,12 @@ dtStatus dtNavMeshQuery::moveAlongSurface(dtPolyRef startRef, const float* start *visitedCount = 0; // Validate input - if (!startRef) return DT_FAILURE; - if (!m_nav->isValidPolyRef(startRef)) return DT_FAILURE; + if (!startRef) + return DT_FAILURE | DT_INVALID_PARAM; + if (!m_nav->isValidPolyRef(startRef)) + return DT_FAILURE | DT_INVALID_PARAM; + + dtStatus status = DT_SUCCESS; static const int MAX_STACK = 48; dtNode* stack[MAX_STACK]; @@ -1513,16 +2125,21 @@ dtStatus dtNavMeshQuery::moveAlongSurface(dtPolyRef startRef, const float* start do { visited[n++] = node->id; + if (n >= maxVisitedSize) + { + status |= DT_BUFFER_TOO_SMALL; + break; + } node = m_tinyNodePool->getNodeAtIdx(node->pidx); } - while (node && n < maxVisitedSize); + while (node); } dtVcopy(resultPos, bestPos); *visitedCount = n; - return DT_SUCCESS; + return status; } @@ -1533,14 +2150,14 @@ dtStatus dtNavMeshQuery::getPortalPoints(dtPolyRef from, dtPolyRef to, float* le const dtMeshTile* fromTile = 0; const dtPoly* fromPoly = 0; - if (m_nav->getTileAndPolyByRef(from, &fromTile, &fromPoly) != DT_SUCCESS) - return DT_FAILURE; + 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 (m_nav->getTileAndPolyByRef(to, &toTile, &toPoly) != DT_SUCCESS) - return DT_FAILURE; + 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); @@ -1562,7 +2179,7 @@ dtStatus dtNavMeshQuery::getPortalPoints(dtPolyRef from, const dtPoly* fromPoly, } } if (!link) - return DT_FAILURE; + return DT_FAILURE | DT_INVALID_PARAM; // Handle off-mesh connections. if (fromPoly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) @@ -1578,7 +2195,7 @@ dtStatus dtNavMeshQuery::getPortalPoints(dtPolyRef from, const dtPoly* fromPoly, return DT_SUCCESS; } } - return DT_FAILURE; + return DT_FAILURE | DT_INVALID_PARAM; } if (toPoly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) @@ -1593,7 +2210,7 @@ dtStatus dtNavMeshQuery::getPortalPoints(dtPolyRef from, const dtPoly* fromPoly, return DT_SUCCESS; } } - return DT_FAILURE; + return DT_FAILURE | DT_INVALID_PARAM; } // Find portal vertices. @@ -1625,7 +2242,8 @@ dtStatus dtNavMeshQuery::getEdgeMidPoint(dtPolyRef from, dtPolyRef to, float* mi { float left[3], right[3]; unsigned char fromType, toType; - if (!getPortalPoints(from, to, left,right, fromType, toType)) return DT_FAILURE; + 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; @@ -1637,81 +2255,196 @@ dtStatus dtNavMeshQuery::getEdgeMidPoint(dtPolyRef from, const dtPoly* fromPoly, float* mid) const { float left[3], right[3]; - if (getPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, left, right) != DT_SUCCESS) - return DT_FAILURE; + 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. +/// +/// <b>Using the Hit Parameter (t)</b> +/// +/// 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 +/// +/// <b>Use Case Restriction</b> +/// +/// 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 { - dtAssert(m_nav); + dtRaycastHit hit; + hit.path = path; + hit.maxPath = maxPath; + + dtStatus status = raycast(startRef, startPos, endPos, filter, 0, &hit); - *t = 0; + *t = hit.t; + if (hitNormal) + dtVcopy(hitNormal, hit.hitNormal); if (pathCount) - *pathCount = 0; + *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. +/// +/// <b>Using the Hit Parameter t of RaycastHit</b> +/// +/// 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 +/// +/// <b>Use Case Restriction</b> +/// +/// 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); + hit->t = 0; + hit->pathCount = 0; + hit->pathCost = 0; + // Validate input if (!startRef || !m_nav->isValidPolyRef(startRef)) - return DT_FAILURE; + return DT_FAILURE | DT_INVALID_PARAM; + if (prevRef && !m_nav->isValidPolyRef(prevRef)) + return DT_FAILURE | DT_INVALID_PARAM; - dtPolyRef curRef = startRef; - float verts[DT_VERTS_PER_POLYGON*3]; + float dir[3], curPos[3], lastPos[3]; + float verts[DT_VERTS_PER_POLYGON*3+3]; int n = 0; - - hitNormal[0] = 0; - hitNormal[1] = 0; - hitNormal[2] = 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, nextRef; + + // The API input has been checked already, skip checking internal data. + nextRef = 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. - // The API input has been cheked already, skip checking internal data. - const dtMeshTile* tile = 0; - const dtPoly* poly = 0; - m_nav->getTileAndPolyByRefUnsafe(curRef, &tile, &poly); - // Collect vertices. int nv = 0; for (int i = 0; i < (int)poly->vertCount; ++i) { dtVcopy(&verts[nv*3], &tile->verts[poly->verts[i]*3]); nv++; - } + } float tmin, tmax; int segMin, segMax; if (!dtIntersectSegmentPoly2D(startPos, endPos, verts, nv, tmin, tmax, segMin, segMax)) { // Could not hit the polygon, keep the old t and report hit. - if (pathCount) - *pathCount = n; - return DT_SUCCESS; + hit->pathCount = n; + return status; } // Keep track of furthest t so far. - if (tmax > *t) - *t = tmax; + if (tmax > hit->t) + hit->t = tmax; // Store visited polygons. - if (n < maxPath) - path[n++] = curRef; - + if (n < hit->maxPath) + hit->path[n++] = curRef; + else + status |= DT_BUFFER_TOO_SMALL; + // Ray end is completely inside the polygon. if (segMax == -1) { - *t = FLT_MAX; - if (pathCount) - *pathCount = n; - return DT_SUCCESS; + 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; + nextRef = 0; for (unsigned int i = poly->firstLink; i != DT_NULL_LINK; i = tile->links[i].next) { @@ -1722,8 +2455,8 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons continue; // Get pointer to the next polygon. - const dtMeshTile* nextTile = 0; - const dtPoly* nextPoly = 0; + nextTile = 0; + nextPoly = 0; m_nav->getTileAndPolyByRefUnsafe(link->ref, &nextTile, &nextPoly); // Skip off-mesh connections. @@ -1791,6 +2524,24 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons } } + // 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. @@ -1802,26 +2553,58 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons const float* vb = &verts[b*3]; const float dx = vb[0] - va[0]; const float dz = vb[2] - va[2]; - hitNormal[0] = dz; - hitNormal[1] = 0; - hitNormal[2] = -dx; - dtVnormalize(hitNormal); - - if (pathCount) - *pathCount = n; - return DT_SUCCESS; + 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; } - if (pathCount) - *pathCount = n; + hit->pathCount = n; - return DT_SUCCESS; + 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, @@ -1834,8 +2617,8 @@ dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float* *resultCount = 0; // Validate input - if (!startRef) return DT_FAILURE; - if (!m_nav->isValidPolyRef(startRef)) return DT_FAILURE; + if (!startRef || !m_nav->isValidPolyRef(startRef)) + return DT_FAILURE | DT_INVALID_PARAM; m_nodePool->clear(); m_openList->clear(); @@ -1849,6 +2632,8 @@ dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float* startNode->flags = DT_NODE_OPEN; m_openList->push(startNode); + dtStatus status = DT_SUCCESS; + int n = 0; if (n < maxResult) { @@ -1860,6 +2645,10 @@ dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float* resultCost[n] = 0; ++n; } + else + { + status |= DT_BUFFER_TOO_SMALL; + } const float radiusSqr = dtSqr(radius); @@ -1915,7 +2704,10 @@ dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float* dtNode* neighbourNode = m_nodePool->getNode(neighbourRef); if (!neighbourNode) + { + status |= DT_OUT_OF_NODES; continue; + } if (neighbourNode->flags & DT_NODE_CLOSED) continue; @@ -1931,7 +2723,7 @@ dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float* continue; neighbourNode->id = neighbourRef; - neighbourNode->flags &= ~DT_NODE_CLOSED; + neighbourNode->flags = (neighbourNode->flags & ~DT_NODE_CLOSED); neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode); neighbourNode->total = total; @@ -1951,6 +2743,10 @@ dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float* resultCost[n] = neighbourNode->total; ++n; } + else + { + status |= DT_BUFFER_TOO_SMALL; + } neighbourNode->flags = DT_NODE_OPEN; m_openList->push(neighbourNode); } @@ -1959,9 +2755,31 @@ dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float* *resultCount = n; - return DT_SUCCESS; + 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, @@ -1974,8 +2792,8 @@ dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* v *resultCount = 0; // Validate input - if (!startRef) return DT_FAILURE; - if (!m_nav->isValidPolyRef(startRef)) return DT_FAILURE; + if (!startRef || !m_nav->isValidPolyRef(startRef)) + return DT_FAILURE | DT_INVALID_PARAM; m_nodePool->clear(); m_openList->clear(); @@ -1994,6 +2812,8 @@ dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* v startNode->flags = DT_NODE_OPEN; m_openList->push(startNode); + dtStatus status = DT_SUCCESS; + int n = 0; if (n < maxResult) { @@ -2005,6 +2825,10 @@ dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* v resultCost[n] = 0; ++n; } + else + { + status |= DT_BUFFER_TOO_SMALL; + } while (!m_openList->empty()) { @@ -2060,7 +2884,10 @@ dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* v dtNode* neighbourNode = m_nodePool->getNode(neighbourRef); if (!neighbourNode) + { + status |= DT_OUT_OF_NODES; continue; + } if (neighbourNode->flags & DT_NODE_CLOSED) continue; @@ -2076,7 +2903,7 @@ dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* v continue; neighbourNode->id = neighbourRef; - neighbourNode->flags &= ~DT_NODE_CLOSED; + neighbourNode->flags = (neighbourNode->flags & ~DT_NODE_CLOSED); neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode); neighbourNode->total = total; @@ -2096,6 +2923,10 @@ dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* v resultCost[n] = neighbourNode->total; ++n; } + else + { + status |= DT_BUFFER_TOO_SMALL; + } neighbourNode->flags = DT_NODE_OPEN; m_openList->push(neighbourNode); } @@ -2104,9 +2935,31 @@ dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* v *resultCount = n; - return DT_SUCCESS; + return status; } +/// @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, @@ -2118,8 +2971,8 @@ dtStatus dtNavMeshQuery::findLocalNeighbourhood(dtPolyRef startRef, const float* *resultCount = 0; // Validate input - if (!startRef) return DT_FAILURE; - if (!m_nav->isValidPolyRef(startRef)) return DT_FAILURE; + if (!startRef || !m_nav->isValidPolyRef(startRef)) + return DT_FAILURE | DT_INVALID_PARAM; static const int MAX_STACK = 48; dtNode* stack[MAX_STACK]; @@ -2138,6 +2991,8 @@ dtStatus dtNavMeshQuery::findLocalNeighbourhood(dtPolyRef startRef, const float* float pa[DT_VERTS_PER_POLYGON*3]; float pb[DT_VERTS_PER_POLYGON*3]; + dtStatus status = DT_SUCCESS; + int n = 0; if (n < maxResult) { @@ -2146,6 +3001,10 @@ dtStatus dtNavMeshQuery::findLocalNeighbourhood(dtPolyRef startRef, const float* resultParent[n] = 0; ++n; } + else + { + status |= DT_BUFFER_TOO_SMALL; + } while (nstack) { @@ -2259,6 +3118,10 @@ dtStatus dtNavMeshQuery::findLocalNeighbourhood(dtPolyRef startRef, const float* resultParent[n] = curRef; ++n; } + else + { + status |= DT_BUFFER_TOO_SMALL; + } if (nstack < MAX_STACK) { @@ -2269,17 +3132,18 @@ dtStatus dtNavMeshQuery::findLocalNeighbourhood(dtPolyRef startRef, const float* *resultCount = n; - return DT_SUCCESS; + 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 short tmin, const short tmax, const dtPolyRef ref) { if (nints+1 > maxInts) return; // Find insertion point. @@ -2294,13 +3158,26 @@ static void insertInterval(dtSegInterval* ints, int& nints, const int maxInts, 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* segments, int* segmentCount, const int maxSegments) const + float* segmentVerts, dtPolyRef* segmentRefs, int* segmentCount, + const int maxSegments) const { dtAssert(m_nav); @@ -2308,14 +3185,18 @@ dtStatus dtNavMeshQuery::getPolyWallSegments(dtPolyRef ref, const dtQueryFilter* const dtMeshTile* tile = 0; const dtPoly* poly = 0; - if (m_nav->getTileAndPolyByRef(ref, &tile, &poly) != DT_SUCCESS) - return DT_FAILURE; + if (dtStatusFailed(m_nav->getTileAndPolyByRef(ref, &tile, &poly))) + 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. @@ -2335,54 +3216,95 @@ dtStatus dtNavMeshQuery::getPolyWallSegments(dtPolyRef ref, const dtQueryFilter* m_nav->getTileAndPolyByRefUnsafe(link->ref, &neiTile, &neiPoly); if (filter->passFilter(link->ref, neiTile, neiPoly)) { - insertInterval(ints, nints, MAX_INTERVAL, link->bmin, link->bmax); + insertInterval(ints, nints, MAX_INTERVAL, link->bmin, link->bmax, link->ref); } } } } } - else if (poly->neis[j]) + else { // Internal edge - const unsigned int idx = (unsigned int)(poly->neis[j]-1); - const dtPolyRef ref = m_nav->getPolyRefBase(tile) | idx; - if (filter->passFilter(ref, tile, &tile->polys[idx])) + 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); - insertInterval(ints, nints, MAX_INTERVAL, 255, 256); + insertInterval(ints, nints, MAX_INTERVAL, -1, 0, 0); + insertInterval(ints, nints, MAX_INTERVAL, 255, 256, 0); - // Store segment. + // 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) { - // Find the space inbetween the opening areas. - const int imin = ints[k-1].tmax; - const int imax = ints[k].tmin; - if (imin == imax) continue; - if (imin == 0 && imax == 255) + // 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 = &segments[n*6]; + 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++; - dtVcopy(seg+0, vj); - dtVcopy(seg+3, vi); + } + else + { + status |= DT_BUFFER_TOO_SMALL; } } - else + + // 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 = &segments[n*6]; - n++; + 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; } } } @@ -2390,9 +3312,19 @@ dtStatus dtNavMeshQuery::getPolyWallSegments(dtPolyRef ref, const dtQueryFilter* *segmentCount = n; - return DT_SUCCESS; + 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 @@ -2402,8 +3334,8 @@ dtStatus dtNavMeshQuery::findDistanceToWall(dtPolyRef startRef, const float* cen dtAssert(m_openList); // Validate input - if (!startRef) return DT_FAILURE; - if (!m_nav->isValidPolyRef(startRef)) return DT_FAILURE; + if (!startRef || !m_nav->isValidPolyRef(startRef)) + return DT_FAILURE | DT_INVALID_PARAM; m_nodePool->clear(); m_openList->clear(); @@ -2419,6 +3351,8 @@ dtStatus dtNavMeshQuery::findDistanceToWall(dtPolyRef startRef, const float* cen float radiusSqr = dtSqr(maxRadius); + dtStatus status = DT_SUCCESS; + while (!m_openList->empty()) { dtNode* bestNode = m_openList->pop(); @@ -2526,7 +3460,10 @@ dtStatus dtNavMeshQuery::findDistanceToWall(dtPolyRef startRef, const float* cen dtNode* neighbourNode = m_nodePool->getNode(neighbourRef); if (!neighbourNode) + { + status |= DT_OUT_OF_NODES; continue; + } if (neighbourNode->flags & DT_NODE_CLOSED) continue; @@ -2545,7 +3482,7 @@ dtStatus dtNavMeshQuery::findDistanceToWall(dtPolyRef startRef, const float* cen continue; neighbourNode->id = neighbourRef; - neighbourNode->flags &= ~DT_NODE_CLOSED; + neighbourNode->flags = (neighbourNode->flags & ~DT_NODE_CLOSED); neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode); neighbourNode->total = total; @@ -2567,12 +3504,40 @@ dtStatus dtNavMeshQuery::findDistanceToWall(dtPolyRef startRef, const float* cen *hitDist = sqrtf(radiusSqr); - return DT_SUCCESS; + 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; - const dtNode* node = m_nodePool->findNode(ref); - return node && node->flags & DT_NODE_CLOSED; + + dtNode* nodes[DT_MAX_STATES_PER_NODE]; + int n= m_nodePool->findNodes(ref, nodes, DT_MAX_STATES_PER_NODE); + + for (int i=0; i<n; i++) + { + if (nodes[i]->flags & DT_NODE_CLOSED) + return true; + } + + return false; } diff --git a/modules/worldengine/deps/recastnavigation/Detour/DetourNode.cpp b/modules/worldengine/deps/recastnavigation/Detour/Source/DetourNode.cpp index f7811e3450..1d1897708f 100644 --- a/modules/worldengine/deps/recastnavigation/Detour/DetourNode.cpp +++ b/modules/worldengine/deps/recastnavigation/Detour/Source/DetourNode.cpp @@ -24,6 +24,7 @@ inline unsigned int dtHashRef(dtPolyRef a) { + // Edited by TC a = (~a) + (a << 18); a = a ^ (a >> 31); a = a * 21; @@ -70,27 +71,46 @@ void dtNodePool::clear() m_nodeCount = 0; } -dtNode* dtNodePool::findNode(dtPolyRef id) +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) +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) + if (m_nodes[i].id == id && m_nodes[i].state == state) return &m_nodes[i]; i = m_next[i]; } @@ -107,6 +127,7 @@ dtNode* dtNodePool::getNode(dtPolyRef id) node->cost = 0; node->total = 0; node->id = id; + node->state = state; node->flags = 0; m_next[i] = m_first[bucket]; diff --git a/modules/worldengine/lib-collision/src/CMakeLists.txt b/modules/worldengine/lib-collision/src/CMakeLists.txt index 012c68bd52..57db910a2c 100644 --- a/modules/worldengine/lib-collision/src/CMakeLists.txt +++ b/modules/worldengine/lib-collision/src/CMakeLists.txt @@ -33,7 +33,7 @@ set(collision_STAT_SRCS include_directories( ${CMAKE_BINARY_DIR} ${CMAKE_SOURCE_DIR}/modules/worldengine/deps/g3dlite/include - ${CMAKE_SOURCE_DIR}/modules/worldengine/deps/recastnavigation/Detour + ${CMAKE_SOURCE_DIR}/modules/worldengine/deps/recastnavigation/Detour/Include ${CMAKE_SOURCE_DIR}/modules/worldengine/nucleus/src ${CMAKE_SOURCE_DIR}/modules/worldengine/nucleus/src/Configuration ${CMAKE_SOURCE_DIR}/modules/worldengine/nucleus/src/Debugging diff --git a/modules/worldengine/lib-collision/src/Maps/TileAssembler.h b/modules/worldengine/lib-collision/src/Maps/TileAssembler.h index be11c256b5..d93662d7fc 100644 --- a/modules/worldengine/lib-collision/src/Maps/TileAssembler.h +++ b/modules/worldengine/lib-collision/src/Maps/TileAssembler.h @@ -34,7 +34,7 @@ namespace VMAP float iScale; void init() { - iRotation = G3D::Matrix3::fromEulerAnglesZYX(G3D::pi()*iDir.y/180.f, G3D::pi()*iDir.x/180.f, G3D::pi()*iDir.z/180.f); + iRotation = G3D::Matrix3::fromEulerAnglesZYX(G3D::pif()*iDir.y/180.f, G3D::pif()*iDir.x/180.f, G3D::pif()*iDir.z/180.f); } G3D::Vector3 transform(const G3D::Vector3& pIn) const; void moveToBasePos(const G3D::Vector3& pBasePos) { iPos -= pBasePos; } diff --git a/modules/worldengine/nucleus/src/Utilities/StringFormat.h b/modules/worldengine/nucleus/src/Utilities/StringFormat.h new file mode 100644 index 0000000000..9e7bd1b61c --- /dev/null +++ b/modules/worldengine/nucleus/src/Utilities/StringFormat.h @@ -0,0 +1,52 @@ +/* + * Copyright (C) 2008-2016 TrinityCore <http://www.trinitycore.org/> + * Copyright (C) 2005-2009 MaNGOS <http://getmangos.com/> + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation; either version 2 of the License, or (at your + * option) any later version. + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see <http://www.gnu.org/licenses/>. + */ + +#ifndef __STRING_FORMAT_H__ +#define __STRING_FORMAT_H__ + +#include <memory> +#include <iostream> +#include <string> +#include <cstdio> + +namespace Trinity +{ + /// Default TC string format function. + template<typename... Args> + inline std::string StringFormat(const std::string& format, Args const&... args) + { + size_t size = std::snprintf(nullptr, 0, format.c_str(), args ...) + 1; // Extra space for '\0' + unique_ptr<char[]> buf(new char[size]); + std::snprintf(buf.get(), size, format.c_str(), args ...); + return std::string(buf.get(), buf.get() + size - 1); // We don + } + + /// Returns true if the given char pointer is null. + inline bool IsFormatEmptyOrNull(const char* fmt) + { + return fmt == nullptr; + } + + /// Returns true if the given std::string is empty. + inline bool IsFormatEmptyOrNull(std::string const& fmt) + { + return fmt.empty(); + } +} + +#endif |