From fff856332bf1d5a99f7c7722e18f5a54e3a6b71b Mon Sep 17 00:00:00 2001 From: faramir118 Date: Tue, 30 Mar 2010 16:01:59 -0500 Subject: [PATCH] mmap - optimizations + merge with mangos master --- src/game/Chat.cpp | 1 + src/game/Chat.h | 1 + src/game/DestinationHolder.h | 1 + src/game/DestinationHolderImp.h | 7 + src/game/HomeMovementGenerator.cpp | 72 +- src/game/HomeMovementGenerator.h | 3 +- src/game/Map.cpp | 381 ++++ src/game/Map.h | 23 + src/game/MotionMaster.cpp | 5 + src/game/TargetedMovementGenerator.cpp | 61 +- src/game/TargetedMovementGenerator.h | 7 +- src/game/debugcmds.cpp | 60 +- src/shared/pathfinding/ChunkyTriMesh.cpp | 244 +++ src/shared/pathfinding/ChunkyTriMesh.h | 49 + src/shared/pathfinding/Detour/DetourCommon.cpp | 250 +++ src/shared/pathfinding/Detour/DetourCommon.h | 204 ++ src/shared/pathfinding/Detour/DetourNavMesh.cpp | 2285 ++++++++++++++++++++ src/shared/pathfinding/Detour/DetourNavMesh.h | 497 +++++ .../pathfinding/Detour/DetourNavMeshBuilder.cpp | 705 ++++++ .../pathfinding/Detour/DetourNavMeshBuilder.h | 68 + src/shared/pathfinding/Detour/DetourNode.cpp | 142 ++ src/shared/pathfinding/Detour/DetourNode.h | 149 ++ src/shared/pathfinding/Detour/Makefile.am | 38 + src/shared/pathfinding/InputGeom.cpp | 364 ++++ src/shared/pathfinding/InputGeom.h | 91 + src/shared/pathfinding/Makefile.am | 36 + src/shared/pathfinding/MeshLoaderObj.cpp | 229 ++ src/shared/pathfinding/MeshLoaderObj.h | 51 + src/shared/pathfinding/Recast/Makefile.am | 43 + src/shared/pathfinding/Recast/Recast.cpp | 279 +++ src/shared/pathfinding/Recast/Recast.h | 627 ++++++ src/shared/pathfinding/Recast/RecastArea.cpp | 326 +++ src/shared/pathfinding/Recast/RecastContour.cpp | 772 +++++++ src/shared/pathfinding/Recast/RecastFilter.cpp | 187 ++ src/shared/pathfinding/Recast/RecastLog.cpp | 77 + src/shared/pathfinding/Recast/RecastLog.h | 81 + src/shared/pathfinding/Recast/RecastMesh.cpp | 1234 +++++++++++ src/shared/pathfinding/Recast/RecastMeshDetail.cpp | 1223 +++++++++++ .../pathfinding/Recast/RecastRasterization.cpp | 356 +++ src/shared/pathfinding/Recast/RecastRegion.cpp | 1288 +++++++++++ src/shared/pathfinding/Recast/RecastTimer.cpp | 43 + src/shared/pathfinding/Recast/RecastTimer.h | 31 + win/VC90/shared.vcproj | 182 ++- 43 files changed, 12731 insertions(+), 42 deletions(-) create mode 100644 src/shared/pathfinding/ChunkyTriMesh.cpp create mode 100644 src/shared/pathfinding/ChunkyTriMesh.h create mode 100644 src/shared/pathfinding/Detour/DetourCommon.cpp create mode 100644 src/shared/pathfinding/Detour/DetourCommon.h create mode 100644 src/shared/pathfinding/Detour/DetourNavMesh.cpp create mode 100644 src/shared/pathfinding/Detour/DetourNavMesh.h create mode 100644 src/shared/pathfinding/Detour/DetourNavMeshBuilder.cpp create mode 100644 src/shared/pathfinding/Detour/DetourNavMeshBuilder.h create mode 100644 src/shared/pathfinding/Detour/DetourNode.cpp create mode 100644 src/shared/pathfinding/Detour/DetourNode.h create mode 100644 src/shared/pathfinding/Detour/Makefile.am create mode 100644 src/shared/pathfinding/InputGeom.cpp create mode 100644 src/shared/pathfinding/InputGeom.h create mode 100644 src/shared/pathfinding/Makefile.am create mode 100644 src/shared/pathfinding/MeshLoaderObj.cpp create mode 100644 src/shared/pathfinding/MeshLoaderObj.h create mode 100644 src/shared/pathfinding/Recast/Makefile.am create mode 100644 src/shared/pathfinding/Recast/Recast.cpp create mode 100644 src/shared/pathfinding/Recast/Recast.h create mode 100644 src/shared/pathfinding/Recast/RecastArea.cpp create mode 100644 src/shared/pathfinding/Recast/RecastContour.cpp create mode 100644 src/shared/pathfinding/Recast/RecastFilter.cpp create mode 100644 src/shared/pathfinding/Recast/RecastLog.cpp create mode 100644 src/shared/pathfinding/Recast/RecastLog.h create mode 100644 src/shared/pathfinding/Recast/RecastMesh.cpp create mode 100644 src/shared/pathfinding/Recast/RecastMeshDetail.cpp create mode 100644 src/shared/pathfinding/Recast/RecastRasterization.cpp create mode 100644 src/shared/pathfinding/Recast/RecastRegion.cpp create mode 100644 src/shared/pathfinding/Recast/RecastTimer.cpp create mode 100644 src/shared/pathfinding/Recast/RecastTimer.h diff --git a/src/game/Chat.cpp b/src/game/Chat.cpp index d004e6c..e4e03b8 100644 --- a/src/game/Chat.cpp +++ b/src/game/Chat.cpp @@ -171,6 +171,7 @@ ChatCommand * ChatHandler::getCommandTable() { "spawnvehicle", SEC_ADMINISTRATOR, false, &ChatHandler::HandleDebugSpawnVehicle, "", NULL }, { "uws", SEC_ADMINISTRATOR, false, &ChatHandler::HandleDebugUpdateWorldStateCommand, "", NULL }, { "update", SEC_ADMINISTRATOR, false, &ChatHandler::HandleDebugUpdateCommand, "", NULL }, + { "movemap", SEC_ADMINISTRATOR, false, &ChatHandler::HandleDebugMoveMapCommand, "", NULL }, { NULL, 0, false, NULL, "", NULL } }; diff --git a/src/game/Chat.h b/src/game/Chat.h index da0563e..d4dc571 100644 --- a/src/game/Chat.h +++ b/src/game/Chat.h @@ -146,6 +146,7 @@ class ChatHandler bool HandleDebugSpellCheckCommand(const char* args); bool HandleDebugUpdateCommand(const char* args); bool HandleDebugUpdateWorldStateCommand(const char* args); + bool HandleDebugMoveMapCommand(const char* args); bool HandleDebugPlayCinematicCommand(const char* args); bool HandleDebugPlayMovieCommand(const char* args); diff --git a/src/game/DestinationHolder.h b/src/game/DestinationHolder.h index 2c8e8a1..231cfe5 100644 --- a/src/game/DestinationHolder.h +++ b/src/game/DestinationHolder.h @@ -55,6 +55,7 @@ class MANGOS_DLL_DECL DestinationHolder void GetLocationNow(const Map * map, float &x, float &y, float &z, bool is3D = false) const; void GetLocationNowNoMicroMovement(float &x, float &y, float &z) const; // For use without micro movement float GetDistance3dFromDestSq(const WorldObject &obj) const; + float GetDistance3dFromDestSq(float x, float y, float z) const; private: void _findOffSetPoint(float x1, float y1, float x2, float y2, float offset, float &x, float &y); diff --git a/src/game/DestinationHolderImp.h b/src/game/DestinationHolderImp.h index b493294..5b70385 100644 --- a/src/game/DestinationHolderImp.h +++ b/src/game/DestinationHolderImp.h @@ -201,6 +201,13 @@ DestinationHolder::GetDistance3dFromDestSq(const WorldObject &obj) co template float +DestinationHolder::GetDistance3dFromDestSq(float x, float y, float z) const +{ + return (i_destX-x)*(i_destX-x)+(i_destY-y)*(i_destY-y)+(i_destZ-z)*(i_destZ-z); +} + +template +float DestinationHolder::GetDestinationDiff(float x, float y, float z) const { return sqrt(((x-i_destX)*(x-i_destX)) + ((y-i_destY)*(y-i_destY)) + ((z-i_destZ)*(z-i_destZ))); diff --git a/src/game/HomeMovementGenerator.cpp b/src/game/HomeMovementGenerator.cpp index 055951e..f929cc5 100644 --- a/src/game/HomeMovementGenerator.cpp +++ b/src/game/HomeMovementGenerator.cpp @@ -49,8 +49,39 @@ HomeMovementGenerator::_setTargetLocation(Creature & owner) owner.GetRespawnCoord(x, y, z); CreatureTraveller traveller(owner); + float myx,myy,myz; + owner.GetPosition(myx,myy,myz); - uint32 travel_time = i_destinationHolder.SetDestination(traveller, x, y, z); + if(i_path.Length) + { + // path exists, just need to update it + i_path.Start.x = myx; + i_path.Start.y = myy; + i_path.Start.z = myz; + i_path.End.x = x; + i_path.End.y = y; + i_path.End.z = z; + sLog.outError("Home: Need to update path."); + owner.GetMap()->UpdatePath(&i_path); + } + else + { + sLog.outError("Home: Need to get path."); + // path doesn't exist, create one + i_path = owner.GetMap()->GetPath(myx,myy,myz,x,y,z); + } + + uint32 travel_time; + if(i_path.Length) + { + sLog.outError("Home: got path!\n"); + travel_time = i_destinationHolder.SetDestination(traveller, i_path.NextDestination.x,i_path.NextDestination.y,i_path.NextDestination.z); + } + else + { + sLog.outError("Home: Didn't get path.\n"); + travel_time = i_destinationHolder.SetDestination(traveller, x, y, z); + } modifyTravelTime(travel_time); owner.clearUnitState(UNIT_STAT_ALL_STATE); } @@ -67,6 +98,45 @@ HomeMovementGenerator::Update(Creature &owner, const uint32& time_diff if (time_diff > i_travel_timer) { + // When reaching end position check if this is really the spawnpoint, if not continue moving. + + // other candidates for home locataion: + // owner.GetMotionMaster()->top()->GetResetPosition(owner,x,y,z) + // owner.GetCombatStartPosition(x, y, z) + float x, y, z; + owner.GetRespawnCoord(x, y, z); + + float myx,myy,myz; + owner.GetPosition(myx,myy,myz); // should switch to i_destinationHolder.GetLocationNow() ? + + if (x != myx || y != myy || z != myz) + { + if(i_path.Length) + { + // path exists, just need to update it + i_path.Start.x = myx; + i_path.Start.y = myy; + i_path.Start.z = myz; + i_path.End.x = x; + i_path.End.y = y; + i_path.End.z = z; + owner.GetMap()->UpdatePath(&i_path); + } + else + // path doesn't exist, create one + i_path = owner.GetMap()->GetPath(myx,myy,myz,x,y,z); + + uint32 travel_time; + if(i_path.Length) + // if path is good, go to NextDestination + travel_time = i_destinationHolder.SetDestination(traveller, i_path.NextDestination.x,i_path.NextDestination.y,i_path.NextDestination.z); + else + // path still bad, cheat and run through walls + travel_time = i_destinationHolder.SetDestination(traveller, x, y, z); + + modifyTravelTime(travel_time); + return true; + } owner.AddSplineFlag(SPLINEFLAG_WALKMODE); // restore orientation of not moving creature at returning to home diff --git a/src/game/HomeMovementGenerator.h b/src/game/HomeMovementGenerator.h index d28949a..250517f 100644 --- a/src/game/HomeMovementGenerator.h +++ b/src/game/HomeMovementGenerator.h @@ -34,7 +34,7 @@ class MANGOS_DLL_SPEC HomeMovementGenerator { public: - HomeMovementGenerator() {} + HomeMovementGenerator() : i_path() {} ~HomeMovementGenerator() {} void Initialize(Creature &); @@ -51,5 +51,6 @@ class MANGOS_DLL_SPEC HomeMovementGenerator DestinationHolder< Traveller > i_destinationHolder; uint32 i_travel_timer; + PathInfo i_path; }; #endif diff --git a/src/game/Map.cpp b/src/game/Map.cpp index d682a9b..8a12fba 100644 --- a/src/game/Map.cpp +++ b/src/game/Map.cpp @@ -40,6 +40,17 @@ #include "InstanceSaveMgr.h" #include "VMapFactory.h" #include "BattleGroundMgr.h" +//#include "OS_TLI.h" +#include "pathfinding/InputGeom.h" +#include "pathfinding/Recast/Recast.h" +#include "pathfinding/Detour/DetourNavMesh.h" +#include "pathfinding/Detour/DetourNavMeshBuilder.h" +// navmesh +//#include "ModelContainerView.h" +#include +#include + +#define MAX_CREATURE_ATTACK_RADIUS (45.0f * sWorld.getConfig(CONFIG_FLOAT_RATE_CREATURE_AGGRO)) GridState* si_GridStates[MAX_GRID_STATE]; @@ -118,6 +129,372 @@ bool Map::ExistVMap(uint32 mapid,int gx,int gy) return true; } +void Map::LoadNavMesh(int gx, int gy) +{ + if (m_navMesh[gx][gy]) + return; + // map file name + char *tmp=NULL; + int len = sWorld.GetDataPath().length()+strlen("mmaps/%03u%02u%02u.mmap")+1; + tmp = new char[len]; + snprintf(tmp, len, (char *)(sWorld.GetDataPath()+"mmaps/%03u%02u%02u.mmap").c_str(),i_id,gx,gy); + // woot no need for generation here: lets just load navmesh itself! + ifstream inf( tmp, ofstream::binary ); + if( inf ) + { + inf.seekg(0,std::ifstream::end); + int navDataSize = inf.tellg(); + inf.seekg(0); + //printf("DataSize is %i\n", navDataSize); + unsigned char *navData = new unsigned char[navDataSize]; + inf.read((char*) (navData),navDataSize); + inf.close(); + m_navMesh[gx][gy] = new dtNavMesh; + if (!m_navMesh) + { + delete [] navData; + return; + } + // printf("Created navMesh\n"); + if (!m_navMesh[gx][gy]->init(navData, navDataSize, true, 2048)) + { + delete [] navData; + return; + } + //sLog.outDetail("Loaded %s (%i kbyte)!", tmp, navDataSize); + //float startPos[3] = { -135.65, 60.41, -9514.61 }; + //float endPos[3] = { 114.97, 60.85, -9519.94 }; + //float mPolyPickingExtents[3] = { 2.00f, 4.00f, 2.00f }; + //dtQueryFilter* mPathFilter = new dtQueryFilter(); + //int gx = 32 - (-9514.61/533.333333f); + //int gy = 32 - (-135.65/533.333333f); + //dtNavMesh* myNavMesh = m_navMesh[gx][gy]; + //if (myNavMesh) + //{ + // sLog.outDetail("NAVMESH Found: [%i][%i]",gx,gy); + // sLog.outDetail("bmax: [%.2f][%.2f][%.2f]", myNavMesh->getTileAt(0,0)->header->bmax[0], myNavMesh->getTileAt(0,0)->header->bmax[1],myNavMesh->getTileAt(0,0)->header->bmax[2]); + // sLog.outDetail("bmin: [%.2f][%.2f][%.2f]", myNavMesh->getTileAt(0,0)->header->bmin[0],myNavMesh->getTileAt(0,0)->header->bmin[1],myNavMesh->getTileAt(0,0)->header->bmin[2]); + // sLog.outDetail("NodeCount: %i",myNavMesh->getTileAt(0,0)->header->bvNodeCount); + // sLog.outDetail("Trying to find Start and end Poligons for:"); + // sLog.outDetail("Start: [%.2f][%.2f][%.2f]", startPos[0],startPos[1],startPos[2]); + // sLog.outDetail("end: [%.2f][%.2f][%.2f]", endPos[0],endPos[1],endPos[2]); + // dtPolyRef mStartRef = myNavMesh->findNearestPoly(startPos,mPolyPickingExtents,mPathFilter,0); // this maybe should be saved on mob for later + // dtPolyRef mEndRef = myNavMesh->findNearestPoly(endPos,mPolyPickingExtents,mPathFilter,0); // saved on player? probably waste since player moves to much + // if (mStartRef != 0 && mEndRef != 0) + // { + // sLog.outDetail("Positions found NavMesh[%03u][%03u]: start [%03u], end [%03u]",gx,gy,mStartRef,mEndRef); + // } + //} + /* + sLog.outDetail("Performance Measurements:"); + struct timeval first, second, lapsed; + struct timezone tzp; + int pathes = 0; + gettimeofday (&first, &tzp); + float x,y,z; + float gx,gy,gz; + + for (ActiveNonPlayers::iterator it = m_activeNonPlayers.begin(); it != m_activeNonPlayers.end(); ++it) + { + (*it)->GetPosition(x,y,z); + ++it; + if (it != m_activeNonPlayers.end()) + { + (*it)->GetPosition(gx,gy,gz); + getNextPositionOnPathToLocation(x,y,z,gx,gy,gz); + pathes++; + } + } + gettimeofday (&second, &tzp); + if (first.tv_usec > second.tv_usec) + { + second.tv_usec += 1000000; + second.tv_sec--; + } + lapsed.tv_usec = second.tv_usec - first.tv_usec; + lapsed.tv_sec = second.tv_sec - first.tv_sec; + sLog.outDetail("Generated %i pathes in %i seconds %i ms", pathes,lapsed.tv_sec,lapsed.tv_usec); + */ + } + else + { + sLog.outError("No MoveMap for [%i][%i][%i]",i_id,gx,gy); + } + delete[] tmp; +} + +PathInfo Map::GetPath(const float startx, const float starty, const float startz, const float endx, const float endy, const float endz) +{ + sLog.outError("entered GetPath"); + PathInfo path = PathInfo(); + + float startPos[3] = {starty, startz, startx}; + float endPos[3] = {endy, endz, endx}; + float mPolyPickingExtents[3] = {2.0f, 4.0f, 2.0f}; + dtQueryFilter* mPathFilter = new dtQueryFilter(); + int gx = 32 - (startx/533.333333f); + int gy = 32 - (starty/533.333333f); + + dtNavMesh* navMesh = m_navMesh[gx][gy]; + + if(!navMesh) + { + sLog.outError("navmesh is bad"); + // pathfinding not possible + return path; + } + + dtPolyRef startPoly = navMesh->findNearestPoly(startPos, mPolyPickingExtents, mPathFilter, 0); + dtPolyRef endPoly = navMesh->findNearestPoly(endPos, mPolyPickingExtents, mPathFilter, 0); + + if(startPoly == 0 || endPoly == 0) + { + sLog.outError("start or end poly is bad"); + // one or both positions not in navmesh? + return path; + } + + dtPolyRef pathPolys[50]; + int pathPolyCount = navMesh->findPath(startPoly, endPoly, startPos, endPos, mPathFilter, pathPolys, 50); + + if(pathPolyCount == 0) + { + sLog.outError("no path existed, leaving GetPath"); + // no path exists + return path; + } + + //float actualpath[3*20]; + //unsigned char* flags = 0; + //dtPolyRef* polyrefs = 0; + + //int mNumPathPoints = m_navMesh[gx][gy]->findStraightPath(startPos, endPos,mPathResults, mNumPathResults, actualpath, flags, polyrefs, 20); + + //if (mNumPathPoints < 3) + // // no path exists + // return newPath; + + int i; + for(i = 0; i < pathPolyCount; ++i) + path.pathPolyRefs[i] = pathPolys[i]; // copy path polygons + path.Length = pathPolyCount; // set length + path.CurrentIndex = 0; + path.Start.x = startx; // store start location + path.Start.y = starty; + path.Start.z = startz; + path.End.x = endx; // store end location + path.End.y = endy; + path.End.z = endz; + path.navMesh = navMesh; // store the navmesh used to get this path info + + getNextPositionOnPathToLocation(&path); + + return path; +} + +void Map::UpdatePath(PathInfo* oldPath) +{ + sLog.outError("Entered UpdatePath"); + if(!oldPath) + { + sLog.outError("oldPath is null"); + // no way to build path without start/end info + return; + } + + int pathStartIndex, pathEndIndex, i, j; + dtPolyRef startPoly, endPoly; + + float startPos[3] = {oldPath->Start.y, oldPath->Start.z, oldPath->Start.x}; + float endPos[3] = {oldPath->End.y, oldPath->End.z, oldPath->End.x}; + float mPolyPickingExtents[3] = {2.0f, 4.0f, 2.0f}; + dtQueryFilter* mPathFilter = new dtQueryFilter(); + + dtNavMesh* navMesh = oldPath->navMesh; + + if(!navMesh) + { + int gx = 32 - (oldPath->Start.x/533.333333f); + int gy = 32 - (oldPath->Start.y/533.333333f); + + if(gx > 0 && gy > 0) + { + navMesh = m_navMesh[gx][gy]; + oldPath->navMesh = navMesh; + } + + if(!navMesh) + { + sLog.outError("navmesh is null!"); + // pathfinding not possible + return; + } + } + + startPoly = navMesh->findNearestPoly(startPos, mPolyPickingExtents, mPathFilter, 0); + endPoly = navMesh->findNearestPoly(endPos, mPolyPickingExtents, mPathFilter, 0); + + if(startPoly == 0 || endPoly == 0) + { + sLog.outError("didn't find the start or end poly"); + // one or both positions not in navmesh? + return; + } + + if(startPoly == endPoly) + { + sLog.outError("start and end poly are the same"); + // they're on the same polygon, path should just be a straight line + + oldPath->pathPolyRefs[0] = startPoly; + oldPath->Length = 1; + oldPath->NextDestination = oldPath->End; // end is next destination + return; + } + + if(!oldPath->pathPolyRefs) + { + // don't have a path yet (this shouldn't happen uner normal circumstances) + PathInfo newPath = GetPath(oldPath->Start.x, oldPath->Start.y, oldPath->Start.z, oldPath->End.x, oldPath->End.y, oldPath->End.z); + (*oldPath) = newPath; + getNextPositionOnPathToLocation(oldPath); + } + else + { + // we have a path, can analyse it to try and optimize it! + + // find if start node is on the existing path + for(pathStartIndex = 0; pathStartIndex < oldPath->Length; ++pathStartIndex) + if(oldPath->pathPolyRefs[pathStartIndex] == startPoly) + break; + + // find if end node is on the existing path + for(pathEndIndex = pathStartIndex; pathEndIndex < oldPath->Length; ++pathEndIndex) + if(oldPath->pathPolyRefs[pathEndIndex] == endPoly) + break; + + if(pathStartIndex == 0 && pathEndIndex == (oldPath->Length - 1)) + { + // old path hasn't changed, just need to update NextDestination + + if(oldPath->NextDestination.x != 0 && oldPath->NextDestination.y != 0 && oldPath->NextDestination.z != 0) + // NextDestination is (0,0,0), which is the default value + getNextPositionOnPathToLocation(oldPath); + // following should be handled by startPoly == endPoly condition above + //else if((oldPath->End is on endPoly) && (oldPath->NextDestination is on endPoly)) + // getNextPositionOnPathToLocation(oldPath) + } + else if(pathStartIndex < oldPath->Length && pathEndIndex < oldPath->Length) + { + // path data is valid, we just need to trim off the excess start and end portions + + oldPath->Length = pathEndIndex - pathStartIndex; // new path length + dtPolyRef newPolyRefs[50]; // allocate space for new path + for(i = pathStartIndex, j = 0; i < pathEndIndex; ++i, ++j) // copy shortened path data + newPolyRefs[j] = oldPath->pathPolyRefs[i]; + + for(i = 0; i < oldPath->Length; ++i) + oldPath->pathPolyRefs[i] = newPolyRefs[i]; // store new path + + getNextPositionOnPathToLocation(oldPath); // get NextDestination + } + else if(pathStartIndex < oldPath->Length && pathEndIndex >= oldPath->Length) + { + // start node is still on old path, but end node moved off of path + + // able to do some simple optimization + // more complicated checks might just waste time in the long run? + + // find out if endPoly is adjacent to the path + bool adjacent = false; + int i; + for(i = 0; i < DT_VERTS_PER_POLYGON; ++i) + if(endPoly == oldPath->navMesh->getPolyByRef(oldPath->pathPolyRefs[oldPath->Length - 1])->neis[i]) + { + adjacent = true; + break; + } + + if(adjacent && oldPath->Length < 50) + { + // endPoly is adjacent to the path, we can add it to the end of the path + oldPath->pathPolyRefs[oldPath->Length] = endPoly; + oldPath->Length++; + } + else + { + // can't optimize, just find brand new path + PathInfo newPath = GetPath(oldPath->Start.x, oldPath->Start.y, oldPath->Start.z, oldPath->End.x, oldPath->End.y, oldPath->End.z); + (*oldPath) = newPath; + } + + getNextPositionOnPathToLocation(oldPath); + } + else + { + // again, start or end poly might be adjacent to oldPath + // so we might be able to optimize a bit to avoid expensive pathfinding + + // old path is junk, need to redo it + PathInfo newPath = GetPath(oldPath->Start.x, oldPath->Start.y, oldPath->Start.z, oldPath->End.x, oldPath->End.y, oldPath->End.z); + (*oldPath) = newPath; + getNextPositionOnPathToLocation(oldPath); + } + } +} + +void Map::getNextPositionOnPathToLocation(PathInfo* path) +{ + float startPos[3] = {path->Start.y, path->Start.z, path->Start.x}; + float endPos[3] = {path->End.y, path->End.z, path->End.x}; + float* actualpath = new float[3*50]; + unsigned char* flags = 0; + dtPolyRef* polyrefs = 0; + + // get vertices of path + int mNumPathPoints = path->navMesh->findStraightPath(startPos, // start position + endPos, // end position + path->pathPolyRefs, // path polygons + path->Length, // path length + actualpath, // short path vertices (out) + flags, // search flags (optional) + polyrefs, // short path polygons (optional, out) + 50); // max path length + + Position pos = Position(); + + if (mNumPathPoints == 0) + { + sLog.outDebug("can't do path - findStraightPath returned 0"); + // only happens when path data is bad + path->Length = 0; + } + if (mNumPathPoints == 1) + { + // startPos == endPos ? + // or couldn't find path to endPos ? + // either way, there's a point and we can use it + pos.x = actualpath[2]; //0 3 y + pos.y = actualpath[0]; //1 4 z + pos.z = actualpath[1]; //2 5 x + } + else if (mNumPathPoints >= 2) + { + // mNumPathPoints == 2 : straightPos, endPos are colinear + // mNumPathPoints > 2 : multi-point path + + pos.x = actualpath[5]; //0 3 y + pos.y = actualpath[3]; //1 4 z + pos.z = actualpath[4]; //2 5 x + } + + // this is all we really wanted, the destination position + path->NextDestination = pos; + + // clean up + delete [] actualpath; +} + void Map::LoadVMap(int gx,int gy) { // x and y are swapped !! @@ -182,7 +559,10 @@ void Map::LoadMapAndVMap(int gx,int gy) { LoadMap(gx,gy); if(i_InstanceId == 0) + { LoadVMap(gx, gy); // Only load the data for the base map + LoadNavMesh(gx, gy); + } } void Map::InitStateMachine() @@ -214,6 +594,7 @@ Map::Map(uint32 id, time_t expiry, uint32 InstanceId, uint8 SpawnMode, Map* _par { //z code GridMaps[idx][j] =NULL; + m_navMesh[idx][j] = NULL; setNGrid(NULL, idx, j); } } diff --git a/src/game/Map.h b/src/game/Map.h index 30d34dd..6e7ed8b 100644 --- a/src/game/Map.h +++ b/src/game/Map.h @@ -34,6 +34,9 @@ #include "MapRefManager.h" #include "Utilities/TypeList.h" +#include "pathfinding/Detour/DetourNavMesh.h" +#include "Unit.h" + #include #include @@ -205,6 +208,19 @@ enum LevelRequirementVsMode LEVELREQUIREMENT_HEROIC = 70 }; +struct PathInfo +{ + PathInfo() : Length(0), CurrentIndex(-1), Start(), End(), NextDestination() {} + + dtPolyRef pathPolyRefs[50]; + int Length; // Length 0 == unreachable location + int CurrentIndex; // probably don't need this + Position Start; + Position End; + Position NextDestination; // this can end up being (0,0,0), which means no path + dtNavMesh* navMesh; +}; + #if defined( __GNUC__ ) #pragma pack() #else @@ -291,6 +307,10 @@ class MANGOS_DLL_SPEC Map : public GridRefManager, public MaNGOS::Obj float GetHeight(float x, float y, float z, bool pCheckVMap=true) const; bool IsInWater(float x, float y, float z) const; // does not use z pos. This is for future use + void getNextPositionOnPathToLocation(PathInfo* path); + PathInfo GetPath(const float startx, const float starty, const float startz, const float endx, const float endy, const float endz); + void UpdatePath(PathInfo* oldPath); + ZLiquidStatus getLiquidStatus(float x, float y, float z, uint8 ReqLiquidType, LiquidData *data = 0) const; uint16 GetAreaFlag(float x, float y, float z) const; @@ -422,6 +442,7 @@ class MANGOS_DLL_SPEC Map : public GridRefManager, public MaNGOS::Obj private: void LoadMapAndVMap(int gx, int gy); void LoadVMap(int gx, int gy); + void LoadNavMesh(int gx, int gy); void LoadMap(int gx,int gy, bool reload = false); GridMap *GetGrid(float x, float y); @@ -486,6 +507,8 @@ class MANGOS_DLL_SPEC Map : public GridRefManager, public MaNGOS::Obj //used for fast base_map (e.g. MapInstanced class object) search for //InstanceMaps and BattleGroundMaps... Map* m_parentMap; + + dtNavMesh *m_navMesh[MAX_NUMBER_OF_GRIDS][MAX_NUMBER_OF_GRIDS]; NGridType* i_grids[MAX_NUMBER_OF_GRIDS][MAX_NUMBER_OF_GRIDS]; GridMap *GridMaps[MAX_NUMBER_OF_GRIDS][MAX_NUMBER_OF_GRIDS]; diff --git a/src/game/MotionMaster.cpp b/src/game/MotionMaster.cpp index ce1008c..09a2a65 100644 --- a/src/game/MotionMaster.cpp +++ b/src/game/MotionMaster.cpp @@ -226,13 +226,16 @@ void MotionMaster::MoveRandom() void MotionMaster::MoveTargetedHome() { + sLog.outError("Entered MotionMaster::MoveTargetedHome()"); if(i_owner->hasUnitState(UNIT_STAT_FLEEING)) return; Clear(false); + sLog.outError("MotionMaster::MoveTargetedHome() : Cleared"); if(i_owner->GetTypeId()==TYPEID_UNIT && !((Creature*)i_owner)->GetCharmerOrOwnerGUID()) { + sLog.outError("%s targeted home", i_owner->GetName()); DEBUG_LOG("%s targeted home", i_owner->GetObjectGuid().GetString().c_str()); Mutate(new HomeMovementGenerator()); } @@ -240,11 +243,13 @@ MotionMaster::MoveTargetedHome() { if (Unit *target = ((Creature*)i_owner)->GetCharmerOrOwner()) { + sLog.outError("%s follow to %s", i_owner->GetName(), target->GetName()); DEBUG_LOG("%s follow to %s", i_owner->GetObjectGuid().GetString().c_str(), target->GetObjectGuid().GetString().c_str()); Mutate(new FollowMovementGenerator(*target,PET_FOLLOW_DIST,PET_FOLLOW_ANGLE)); } else { + sLog.outError("%s attempt but fail to follow owner", i_owner->GetName()); DEBUG_LOG("%s attempt but fail to follow owner", i_owner->GetObjectGuid().GetString().c_str()); } } diff --git a/src/game/TargetedMovementGenerator.cpp b/src/game/TargetedMovementGenerator.cpp index b59e6c8..bb4a31e 100644 --- a/src/game/TargetedMovementGenerator.cpp +++ b/src/game/TargetedMovementGenerator.cpp @@ -22,6 +22,7 @@ #include "Creature.h" #include "DestinationHolderImp.h" #include "World.h" +#include "Unit.h" #define SMALL_ALPHA 0.05f @@ -31,6 +32,7 @@ template void TargetedMovementGeneratorMedium::_setTargetLocation(T &owner) { + sLog.outError("Targeted: setTargetLocation for %s", owner.GetName()); if (!i_target.isValid() || !i_target->IsInWorld()) return; @@ -69,8 +71,44 @@ void TargetedMovementGeneratorMedium::_setTargetLocation(T &owner) if( i_destinationHolder.HasDestination() && i_destinationHolder.GetDestinationDiff(x,y,z) < bothObjectSize ) return; */ + if (!i_destinationHolder.HasArrived()) + return; + float myx,myy,myz; + //the Creature after us is actually in owner variable.. + owner.GetPosition(myx,myy,myz); + + if(i_path.Length) + { + // path exists, just need to update it + i_path.Start.x = myx; + i_path.Start.y = myy; + i_path.Start.z = myz; + i_path.End.x = x; + i_path.End.y = y; + i_path.End.z = z; + sLog.outError("Targeted: Need to update path."); + i_target->GetMap()->UpdatePath(&i_path); + } + else + { + sLog.outError("Targeted: Need to get path."); + // path doesn't exist, create one + i_path = i_target->GetMap()->GetPath(myx,myy,myz,x,y,z); + } + Traveller traveller(owner); - i_destinationHolder.SetDestination(traveller, x, y, z); + if(i_path.Length) + { + sLog.outError("Targeted: got path!\n"); + i_destinationHolder.SetDestination(traveller, i_path.NextDestination.x,i_path.NextDestination.y,i_path.NextDestination.z); + } + else + { + sLog.outError("Targeted: Didn't get path.\n"); + i_destinationHolder.SetDestination(traveller, x, y, z); + } + + //sLog.outString("Moving to x[%.2f] y[%.2f] z[%.2f]", i_path.NextDestination.x, i_path.NextDestination.y, i_path.NextDestination.z); D::_addUnitStateMove(owner); if (owner.GetTypeId() == TYPEID_UNIT && ((Creature*)&owner)->canFly()) @@ -106,6 +144,7 @@ void TargetedMovementGeneratorMedium template bool TargetedMovementGeneratorMedium::Update(T &owner, const uint32 & time_diff) { + sLog.outError("Targeted: Update for %s", owner.GetName()); if (!i_target.isValid() || !i_target->IsInWorld()) return false; @@ -161,20 +200,32 @@ bool TargetedMovementGeneratorMedium::Update(T &owner, const uint32 & time_ //More distance let have better performance, less distance let have more sensitive reaction at target move. // try to counter precision differences - if (i_destinationHolder.GetDistance3dFromDestSq(*i_target.getTarget()) >= dist * dist) + if ((i_path.Length && i_destinationHolder.GetDistance3dFromDestSq(i_path.End.x, i_path.End.y, i_path.End.z) >= dist * dist) + || (i_destinationHolder.GetDistance3dFromDestSq(*i_target.getTarget()) >= dist * dist)) { - owner.SetInFront(i_target.getTarget()); // Set new Angle For Map:: + // Set new Angle For Map:: + if(i_path.Length) + owner.SetOrientation(owner.GetAngle(i_path.NextDestination.x, i_path.NextDestination.y)); + else + owner.SetInFront(i_target.getTarget()); + _setTargetLocation(owner); //Calculate New Dest and Send data To Player } // Update the Angle of the target only for Map::, no need to send packet for player else if (!i_angle && !owner.HasInArc(0.01f, i_target.getTarget())) - owner.SetInFront(i_target.getTarget()); + if(i_path.Length) + owner.SetOrientation(owner.GetAngle(i_path.NextDestination.x, i_path.NextDestination.y)); + else + owner.SetInFront(i_target.getTarget()); if ((owner.IsStopped() && !i_destinationHolder.HasArrived()) || i_recalculateTravel) { i_recalculateTravel = false; //Angle update will take place into owner.StopMoving() - owner.SetInFront(i_target.getTarget()); + if(i_path.Length) + owner.SetOrientation(owner.GetAngle(i_path.NextDestination.x, i_path.NextDestination.y)); + else + owner.SetInFront(i_target.getTarget()); owner.StopMoving(); static_cast(this)->_reachTarget(owner); diff --git a/src/game/TargetedMovementGenerator.h b/src/game/TargetedMovementGenerator.h index a8d4462..c0f7c20 100644 --- a/src/game/TargetedMovementGenerator.h +++ b/src/game/TargetedMovementGenerator.h @@ -39,11 +39,11 @@ class MANGOS_DLL_SPEC TargetedMovementGeneratorMedium { protected: TargetedMovementGeneratorMedium() - : TargetedMovementGeneratorBase(), i_offset(0), i_angle(0), i_recalculateTravel(false) {} + : TargetedMovementGeneratorBase(), i_offset(0), i_angle(0), i_recalculateTravel(false), i_path() {} TargetedMovementGeneratorMedium(Unit &target) - : TargetedMovementGeneratorBase(target), i_offset(0), i_angle(0), i_recalculateTravel(false) {} + : TargetedMovementGeneratorBase(target), i_offset(0), i_angle(0), i_recalculateTravel(false), i_path() {} TargetedMovementGeneratorMedium(Unit &target, float offset, float angle) - : TargetedMovementGeneratorBase(target), i_offset(offset), i_angle(angle), i_recalculateTravel(false) {} + : TargetedMovementGeneratorBase(target), i_offset(offset), i_angle(angle), i_recalculateTravel(false), i_path() {} ~TargetedMovementGeneratorMedium() {} public: @@ -68,6 +68,7 @@ class MANGOS_DLL_SPEC TargetedMovementGeneratorMedium float i_angle; DestinationHolder< Traveller > i_destinationHolder; bool i_recalculateTravel; + PathInfo i_path; }; template diff --git a/src/game/debugcmds.cpp b/src/game/debugcmds.cpp index 74a76c6..482e06e 100644 --- a/src/game/debugcmds.cpp +++ b/src/game/debugcmds.cpp @@ -33,6 +33,10 @@ #include "ObjectGuid.h" #include "SpellMgr.h" +#include "CellImpl.h" +#include "GridNotifiers.h" +#include "GridNotifiersImpl.h" + bool ChatHandler::HandleDebugSendSpellFailCommand(const char* args) { if (!*args) @@ -212,7 +216,61 @@ bool ChatHandler::HandleDebugUpdateWorldStateCommand(const char* args) m_session->GetPlayer()->SendUpdateWorldState(world, state); return true; } - +bool ChatHandler::HandleDebugMoveMapCommand(const char* args) +{ + float range; + char* w = strtok((char*)args, " "); + if (!w) { + SendSysMessage("Syntax is .debug movemap range"); + return false; + } + range = atof(w); + PSendSysMessage("Generating MoveMap Path for all creatures around player In range:%.2f", range); + // Iterate for all slave masters + CellPair pair(MaNGOS::ComputeCellPair( m_session->GetPlayer()->GetPositionX(), m_session->GetPlayer()->GetPositionY()) ); + Cell cell(pair); + cell.data.Part.reserved = ALL_DISTRICT; + cell.SetNoCreate(); + + std::list creatureList; + + MaNGOS::AnyUnitInObjectRangeCheck go_check(m_session->GetPlayer(), range); // 25 yards check + MaNGOS::CreatureListSearcher go_search(m_session->GetPlayer(), creatureList, go_check); + TypeContainerVisitor, GridTypeMapContainer> go_visit(go_search); + + // Get Creatures + cell.Visit(pair, go_visit, *(m_session->GetPlayer()->GetMap()), *(m_session->GetPlayer()), range); + + if (!creatureList.empty()) + { + // Do what You want with these creatures + PSendSysMessage("Found %i Creatures.", creatureList.size()); + std::list::iterator aCreature = creatureList.begin(); + uint32 pathes = 0; + uint32 uStartTime = getMSTime(); + + float x,y,z; + float gx,gy,gz; + m_session->GetPlayer()->GetPosition(gx,gy,gz); + Map *theMap = m_session->GetPlayer()->GetMap(); + while (aCreature != creatureList.end()) { + (*aCreature)->GetPosition(x,y,z); + theMap->GetPath(x,y,z,gx,gy,gz); + ++pathes; + aCreature++; + } + + uint32 uPathLoadTime = getMSTimeDiff(uStartTime, getMSTime()); + sLog.outDetail("Generated %i pathes in %i seconds %i ms", pathes,(uPathLoadTime % 60000) / 1000, uPathLoadTime); + PSendSysMessage("Generated %i pathes in %i seconds %i ms", pathes,(uPathLoadTime % 60000) / 1000, uPathLoadTime); + return true; + } + else + { + SendSysMessage("No Creatures around player :("); + return false; + } +} bool ChatHandler::HandleDebugPlayCinematicCommand(const char* args) { // USAGE: .debug play cinematic #cinematicid diff --git a/src/shared/pathfinding/ChunkyTriMesh.cpp b/src/shared/pathfinding/ChunkyTriMesh.cpp new file mode 100644 index 0000000..68819b7 --- /dev/null +++ b/src/shared/pathfinding/ChunkyTriMesh.cpp @@ -0,0 +1,244 @@ +// +// Copyright (c) 2009 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 "ChunkyTriMesh.h" +#include +#include + +struct BoundsItem +{ + float bmin[2]; + float bmax[2]; + int i; +}; + +static int compareItemX(const void* va, const void* vb) +{ + const BoundsItem* a = (const BoundsItem*)va; + const BoundsItem* b = (const BoundsItem*)vb; + if (a->bmin[0] < b->bmin[0]) + return -1; + if (a->bmin[0] > b->bmin[0]) + return 1; + return 0; +} + +static int compareItemY(const void* va, const void* vb) +{ + const BoundsItem* a = (const BoundsItem*)va; + const BoundsItem* b = (const BoundsItem*)vb; + if (a->bmin[1] < b->bmin[1]) + return -1; + if (a->bmin[1] > b->bmin[1]) + return 1; + return 0; +} + +static void calcExtends(BoundsItem* items, int nitems, int imin, int imax, + float* bmin, float* bmax) +{ + bmin[0] = items[imin].bmin[0]; + bmin[1] = items[imin].bmin[1]; + + bmax[0] = items[imin].bmax[0]; + bmax[1] = items[imin].bmax[1]; + + for (int i = imin+1; i < imax; ++i) + { + const BoundsItem& it = items[i]; + if (it.bmin[0] < bmin[0]) bmin[0] = it.bmin[0]; + if (it.bmin[1] < bmin[1]) bmin[1] = it.bmin[1]; + + if (it.bmax[0] > bmax[0]) bmax[0] = it.bmax[0]; + if (it.bmax[1] > bmax[1]) bmax[1] = it.bmax[1]; + } +} + +inline int longestAxis(float x, float y) +{ + return y > x ? 1 : 0; +} + +static void subdivide(BoundsItem* items, int nitems, int imin, int imax, int trisPerChunk, + int& curNode, rcChunkyTriMeshNode* nodes, const int maxNodes, + int& curTri, int* outTris, const int* inTris) +{ + int inum = imax - imin; + int icur = curNode; + + if (curNode > maxNodes) + return; + + rcChunkyTriMeshNode& node = nodes[curNode++]; + + if (inum <= trisPerChunk) + { + // Leaf + calcExtends(items, nitems, imin, imax, node.bmin, node.bmax); + + // Copy triangles. + node.i = curTri; + node.n = inum; + + for (int i = imin; i < imax; ++i) + { + const int* src = &inTris[items[i].i*3]; + int* dst = &outTris[curTri*3]; + curTri++; + dst[0] = src[0]; + dst[1] = src[1]; + dst[2] = src[2]; + } + } + else + { + // Split + calcExtends(items, nitems, imin, imax, node.bmin, node.bmax); + + int axis = longestAxis(node.bmax[0] - node.bmin[0], + node.bmax[1] - node.bmin[1]); + + if (axis == 0) + { + // Sort along x-axis + qsort(items+imin, inum, sizeof(BoundsItem), compareItemX); + } + else if (axis == 1) + { + // Sort along y-axis + qsort(items+imin, inum, sizeof(BoundsItem), compareItemY); + } + + int isplit = imin+inum/2; + + // Left + subdivide(items, nitems, imin, isplit, trisPerChunk, curNode, nodes, maxNodes, curTri, outTris, inTris); + // Right + subdivide(items, nitems, isplit, imax, trisPerChunk, curNode, nodes, maxNodes, curTri, outTris, inTris); + + int iescape = curNode - icur; + // Negative index means escape. + node.i = -iescape; + } +} + +bool rcCreateChunkyTriMesh(const float* verts, const int* tris, int ntris, + int trisPerChunk, rcChunkyTriMesh* cm) +{ + int nchunks = (ntris + trisPerChunk-1) / trisPerChunk; + + cm->nodes = new rcChunkyTriMeshNode[nchunks*4]; + if (!cm->nodes) + return false; + + cm->tris = new int[ntris*3]; + if (!cm->tris) + return false; + + cm->ntris = ntris; + + // Build tree + BoundsItem* items = new BoundsItem[ntris]; + if (!items) + return false; + + for (int i = 0; i < ntris; i++) + { + const int* t = &tris[i*3]; + BoundsItem& it = items[i]; + it.i = i; + // Calc triangle XZ bounds. + it.bmin[0] = it.bmax[0] = verts[t[0]*3+0]; + it.bmin[1] = it.bmax[1] = verts[t[0]*3+2]; + for (int j = 1; j < 3; ++j) + { + const float* v = &verts[t[j]*3]; + if (v[0] < it.bmin[0]) it.bmin[0] = v[0]; + if (v[2] < it.bmin[1]) it.bmin[1] = v[2]; + + if (v[0] > it.bmax[0]) it.bmax[0] = v[0]; + if (v[2] > it.bmax[1]) it.bmax[1] = v[2]; + } + } + + int curTri = 0; + int curNode = 0; + subdivide(items, ntris, 0, ntris, trisPerChunk, curNode, cm->nodes, nchunks*4, curTri, cm->tris, tris); + + delete [] items; + + cm->nnodes = curNode; + + // Calc max tris per node. + cm->maxTrisPerChunk = 0; + for (int i = 0; i < cm->nnodes; ++i) + { + rcChunkyTriMeshNode& node = cm->nodes[i]; + const bool isLeaf = node.i >= 0; + if (!isLeaf) continue; + if (node.n > cm->maxTrisPerChunk) + cm->maxTrisPerChunk = node.n; + } + + return true; +} + + +inline bool checkOverlapRect(const float amin[2], const float amax[2], + const float bmin[2], const float bmax[2]) +{ + 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; + return overlap; +} + +int rcGetChunksInRect(const rcChunkyTriMesh* cm, + float bmin[2], float bmax[2], + int* ids, const int maxIds) +{ + // Traverse tree + int i = 0; + int n = 0; + while (i < cm->nnodes) + { + const rcChunkyTriMeshNode* node = &cm->nodes[i]; + const bool overlap = checkOverlapRect(bmin, bmax, node->bmin, node->bmax); + const bool isLeafNode = node->i >= 0; + + if (isLeafNode && overlap) + { + if (n < maxIds) + { + ids[n] = i; + n++; + } + } + + if (overlap || isLeafNode) + i++; + else + { + const int escapeIndex = -node->i; + i += escapeIndex; + } + } + + return n; +} + diff --git a/src/shared/pathfinding/ChunkyTriMesh.h b/src/shared/pathfinding/ChunkyTriMesh.h new file mode 100644 index 0000000..aca5ce4 --- /dev/null +++ b/src/shared/pathfinding/ChunkyTriMesh.h @@ -0,0 +1,49 @@ +// +// Copyright (c) 2009 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 CHUNKYTRIMESH_H +#define CHUNKYTRIMESH_H + +struct rcChunkyTriMeshNode +{ + float bmin[2], bmax[2]; + int i, n; +}; + +struct rcChunkyTriMesh +{ + inline rcChunkyTriMesh() : nodes(0), tris(0) {}; + inline ~rcChunkyTriMesh() { delete [] nodes; delete [] tris; } + + rcChunkyTriMeshNode* nodes; + int nnodes; + int* tris; + int ntris; + int maxTrisPerChunk; +}; + +// Creates partitioned triangle mesh (AABB tree), +// where each node contains at max trisPerChunk triangles. +bool rcCreateChunkyTriMesh(const float* verts, const int* tris, int ntris, + int trisPerChunk, rcChunkyTriMesh* cm); + +// Returns the chunk indices which touch the input rectable. +int rcGetChunksInRect(const rcChunkyTriMesh* cm, float bmin[2], float bmax[2], int* ids, const int maxIds); + + +#endif // CHUNKYTRIMESH_H diff --git a/src/shared/pathfinding/Detour/DetourCommon.cpp b/src/shared/pathfinding/Detour/DetourCommon.cpp new file mode 100644 index 0000000..f8a2fcd --- /dev/null +++ b/src/shared/pathfinding/Detour/DetourCommon.cpp @@ -0,0 +1,250 @@ +// +// Copyright (c) 2009 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#include +#include "DetourCommon.h" + +void closestPtPointTriangle(float* closest, const float* p, + const float* a, const float* b, const float* c) +{ + // Check if P in vertex region outside A + float ab[3], ac[3], ap[3]; + vsub(ab, b, a); + vsub(ac, c, a); + vsub(ap, p, a); + float d1 = vdot(ab, ap); + float d2 = vdot(ac, ap); + if (d1 <= 0.0f && d2 <= 0.0f) + { + // barycentric coordinates (1,0,0) + vcopy(closest, a); + return; + } + + // Check if P in vertex region outside B + float bp[3]; + vsub(bp, p, b); + float d3 = vdot(ab, bp); + float d4 = vdot(ac, bp); + if (d3 >= 0.0f && d4 <= d3) + { + // barycentric coordinates (0,1,0) + vcopy(closest, b); + return; + } + + // Check if P in edge region of AB, if so return projection of P onto AB + float vc = d1*d4 - d3*d2; + if (vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f) + { + // barycentric coordinates (1-v,v,0) + float v = d1 / (d1 - d3); + closest[0] = a[0] + v * ab[0]; + closest[1] = a[1] + v * ab[1]; + closest[2] = a[2] + v * ab[2]; + return; + } + + // Check if P in vertex region outside C + float cp[3]; + vsub(cp, p, c); + float d5 = vdot(ab, cp); + float d6 = vdot(ac, cp); + if (d6 >= 0.0f && d5 <= d6) + { + // barycentric coordinates (0,0,1) + vcopy(closest, c); + return; + } + + // Check if P in edge region of AC, if so return projection of P onto AC + float vb = d5*d2 - d1*d6; + if (vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f) + { + // barycentric coordinates (1-w,0,w) + float w = d2 / (d2 - d6); + closest[0] = a[0] + w * ac[0]; + closest[1] = a[1] + w * ac[1]; + closest[2] = a[2] + w * ac[2]; + return; + } + + // Check if P in edge region of BC, if so return projection of P onto BC + float va = d3*d6 - d5*d4; + if (va <= 0.0f && (d4 - d3) >= 0.0f && (d5 - d6) >= 0.0f) + { + // barycentric coordinates (0,1-w,w) + float w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); + closest[0] = b[0] + w * (c[0] - b[0]); + closest[1] = b[1] + w * (c[1] - b[1]); + closest[2] = b[2] + w * (c[2] - b[2]); + return; + } + + // P inside face region. Compute Q through its barycentric coordinates (u,v,w) + float denom = 1.0f / (va + vb + vc); + float v = vb * denom; + float w = vc * denom; + closest[0] = a[0] + ab[0] * v + ac[0] * w; + closest[1] = a[1] + ab[1] * v + ac[1] * w; + closest[2] = a[2] + ab[2] * v + ac[2] * w; +} + +bool intersectSegmentPoly2D(const float* p0, const float* p1, + const float* verts, int nverts, + float& tmin, float& tmax, + int& segMin, int& segMax) +{ + static const float EPS = 0.00000001f; + + tmin = 0; + tmax = 1; + segMin = -1; + segMax = -1; + + float dir[3]; + vsub(dir, p1, p0); + + for (int i = 0, j = nverts-1; i < nverts; j=i++) + { + float edge[3], diff[3]; + vsub(edge, &verts[i*3], &verts[j*3]); + vsub(diff, p0, &verts[j*3]); + float n = vperp2D(edge, diff); + float d = -vperp2D(edge, dir); + if (fabs(d) < EPS) + { + // S is nearly parallel to this edge + if (n < 0) + return false; + else + continue; + } + float t = n / d; + if (d < 0) + { + // segment S is entering across this edge + if (t > tmin) + { + tmin = t; + segMin = j; + // S enters after leaving polygon + if (tmin > tmax) + return false; + } + } + else + { + // segment S is leaving across this edge + if (t < tmax) + { + tmax = t; + segMax = j; + // S leaves before entering polygon + if (tmax < tmin) + return false; + } + } + } + + return true; +} + +float distancePtSegSqr2D(const float* pt, const float* p, const float* q, float& t) +{ + float pqx = q[0] - p[0]; + float pqz = q[2] - p[2]; + float dx = pt[0] - p[0]; + float dz = pt[2] - p[2]; + float d = pqx*pqx + pqz*pqz; + t = pqx*dx + pqz*dz; + if (d > 0) t /= d; + if (t < 0) t = 0; + else if (t > 1) t = 1; + dx = p[0] + t*pqx - pt[0]; + dz = p[2] + t*pqz - pt[2]; + return dx*dx + dz*dz; +} + +void calcPolyCenter(float* tc, const unsigned short* idx, int nidx, const float* verts) +{ + tc[0] = 0.0f; + tc[1] = 0.0f; + tc[2] = 0.0f; + for (int j = 0; j < nidx; ++j) + { + const float* v = &verts[idx[j]*3]; + tc[0] += v[0]; + tc[1] += v[1]; + tc[2] += v[2]; + } + const float s = 1.0f / nidx; + tc[0] *= s; + tc[1] *= s; + tc[2] *= s; +} + +bool closestHeightPointTriangle(const float* p, const float* a, const float* b, const float* c, float& h) +{ + float v0[3], v1[3], v2[3]; + vsub(v0, c,a); + vsub(v1, b,a); + vsub(v2, p,a); + + const float dot00 = vdot2D(v0, v0); + const float dot01 = vdot2D(v0, v1); + const float dot02 = vdot2D(v0, v2); + const float dot11 = vdot2D(v1, v1); + const float dot12 = vdot2D(v1, v2); + + // Compute barycentric coordinates + float invDenom = 1.0f / (dot00 * dot11 - dot01 * dot01); + float u = (dot11 * dot02 - dot01 * dot12) * invDenom; + float v = (dot00 * dot12 - dot01 * dot02) * invDenom; + + // The (sloppy) epsilon is needed to allow to get height of points which + // are interpolated along the edges of the triangles. + static const float EPS = 1e-4f; + + // If point lies inside the triangle, return interpolated ycoord. + if (u >= -EPS && v >= -EPS && (u+v) <= 1+EPS) + { + h = a[1] + v0[1]*u + v1[1]*v; + return true; + } + + return false; +} + +bool distancePtPolyEdgesSqr(const float* pt, const float* verts, const int nverts, + float* ed, float* et) +{ + // TODO: Replace pnpoly with triArea2D tests? + int i, j; + bool c = false; + for (i = 0, j = nverts-1; i < nverts; j = i++) + { + const float* vi = &verts[i*3]; + const float* vj = &verts[j*3]; + if (((vi[2] > pt[2]) != (vj[2] > pt[2])) && + (pt[0] < (vj[0]-vi[0]) * (pt[2]-vi[2]) / (vj[2]-vi[2]) + vi[0]) ) + c = !c; + ed[j] = distancePtSegSqr2D(pt, vj, vi, et[j]); + } + return c; +} diff --git a/src/shared/pathfinding/Detour/DetourCommon.h b/src/shared/pathfinding/Detour/DetourCommon.h new file mode 100644 index 0000000..7608fed --- /dev/null +++ b/src/shared/pathfinding/Detour/DetourCommon.h @@ -0,0 +1,204 @@ +// +// Copyright (c) 2009 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 inline void swap(T& a, T& b) { T t = a; a = b; b = t; } +template inline T min(T a, T b) { return a < b ? a : b; } +template inline T max(T a, T b) { return a > b ? a : b; } +template inline T abs(T a) { return a < 0 ? -a : a; } +template inline T sqr(T a) { return a*a; } +template inline T clamp(T v, T mn, T mx) { return v < mn ? mn : (v > mx ? mx : v); } + +inline void vcross(float* dest, const float* v1, const float* v2) +{ + dest[0] = v1[1]*v2[2] - v1[2]*v2[1]; + dest[1] = v1[2]*v2[0] - v1[0]*v2[2]; + dest[2] = v1[0]*v2[1] - v1[1]*v2[0]; +} + +inline float vdot(const float* v1, const float* v2) +{ + return v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2]; +} + +inline void vmad(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 vlerp(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 vadd(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 vsub(float* dest, const float* v1, const float* v2) +{ + dest[0] = v1[0]-v2[0]; + dest[1] = v1[1]-v2[1]; + dest[2] = v1[2]-v2[2]; +} + +inline void vmin(float* mn, const float* v) +{ + mn[0] = min(mn[0], v[0]); + mn[1] = min(mn[1], v[1]); + mn[2] = min(mn[2], v[2]); +} + +inline void vmax(float* mx, const float* v) +{ + mx[0] = max(mx[0], v[0]); + mx[1] = max(mx[1], v[1]); + mx[2] = max(mx[2], v[2]); +} + +inline void vcopy(float* dest, const float* a) +{ + dest[0] = a[0]; + dest[1] = a[1]; + dest[2] = a[2]; +} + +inline float vdist(const float* v1, const float* v2) +{ + float dx = v2[0] - v1[0]; + float dy = v2[1] - v1[1]; + float dz = v2[2] - v1[2]; + return sqrtf(dx*dx + dy*dy + dz*dz); +} + +inline float vdistSqr(const float* v1, const float* v2) +{ + float dx = v2[0] - v1[0]; + float dy = v2[1] - v1[1]; + float dz = v2[2] - v1[2]; + return dx*dx + dy*dy + dz*dz; +} + +inline void vnormalize(float* v) +{ + float d = 1.0f / sqrtf(sqr(v[0]) + sqr(v[1]) + sqr(v[2])); + v[0] *= d; + v[1] *= d; + v[2] *= d; +} + +inline bool vequal(const float* p0, const float* p1) +{ + static const float thr = sqr(1.0f/16384.0f); + const float d = vdistSqr(p0, p1); + return d < thr; +} + +inline unsigned int nextPow2(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 ilog2(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 align4(int x) { return (x+3) & ~3; } + +inline float vdot2D(const float* u, const float* v) +{ + return u[0]*v[0] + u[2]*v[2]; +} + +inline float vperp2D(const float* u, const float* v) +{ + return u[2]*v[0] - u[0]*v[2]; +} + +inline float triArea2D(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 checkOverlapBox(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 overlapBounds(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 closestPtPointTriangle(float* closest, const float* p, + const float* a, const float* b, const float* c); + +bool closestHeightPointTriangle(const float* p, const float* a, const float* b, const float* c, float& h); + +bool intersectSegmentPoly2D(const float* p0, const float* p1, + const float* verts, int nverts, + float& tmin, float& tmax, + int& segMin, int& segMax); + +bool distancePtPolyEdgesSqr(const float* pt, const float* verts, const int nverts, + float* ed, float* et); + +float distancePtSegSqr2D(const float* pt, const float* p, const float* q, float& t); + +void calcPolyCenter(float* tc, const unsigned short* idx, int nidx, const float* verts); + +#endif // DETOURCOMMON_H \ No newline at end of file diff --git a/src/shared/pathfinding/Detour/DetourNavMesh.cpp b/src/shared/pathfinding/Detour/DetourNavMesh.cpp new file mode 100644 index 0000000..9596f58 --- /dev/null +++ b/src/shared/pathfinding/Detour/DetourNavMesh.cpp @@ -0,0 +1,2285 @@ +// +// Copyright (c) 2009 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#include +#include +#include +#include +#include "DetourNavMesh.h" +#include "DetourNode.h" +#include "DetourCommon.h" + + +inline int opposite(int side) { return (side+4) & 0x7; } + +inline bool overlapBoxes(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; +} + +inline bool overlapRects(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; + return overlap; +} + +static void calcRect(const float* va, const float* vb, + float* bmin, float* bmax, + int side, float padx, float pady) +{ + if (side == 0 || side == 4) + { + bmin[0] = min(va[2],vb[2]) + padx; + bmin[1] = min(va[1],vb[1]) - pady; + bmax[0] = max(va[2],vb[2]) - padx; + bmax[1] = max(va[1],vb[1]) + pady; + } + else if (side == 2 || side == 6) + { + bmin[0] = min(va[0],vb[0]) + padx; + bmin[1] = min(va[1],vb[1]) - pady; + bmax[0] = max(va[0],vb[0]) - padx; + bmax[1] = max(va[1],vb[1]) + pady; + } +} + +inline int computeTileHash(int x, int y, const int mask) +{ + const unsigned int h1 = 0x8da6b343; // Large multiplicative constants; + const unsigned int h2 = 0xd8163841; // here arbitrarily chosen primes + unsigned int n = h1 * x + h2 * y; + return (int)(n & mask); +} + +inline unsigned int allocLink(dtMeshTile* tile) +{ + if (tile->linksFreeList == DT_NULL_LINK) + return DT_NULL_LINK; + unsigned int link = tile->linksFreeList; + tile->linksFreeList = tile->links[link].next; + return link; +} + +inline void freeLink(dtMeshTile* tile, unsigned int link) +{ + tile->links[link].next = tile->linksFreeList; + tile->linksFreeList = link; +} + + +inline bool passFilter(dtQueryFilter* filter, unsigned short flags) +{ + return (flags & filter->includeFlags) != 0 && (flags & filter->excludeFlags) == 0; +} + + + +////////////////////////////////////////////////////////////////////////////////////////// +dtNavMesh::dtNavMesh() : + m_tileWidth(0), + m_tileHeight(0), + m_maxTiles(0), + m_tileLutSize(0), + m_tileLutMask(0), + m_posLookup(0), + m_nextFree(0), + m_tiles(0), + m_saltBits(0), + m_tileBits(0), + m_polyBits(0), + m_nodePool(0), + m_openList(0) +{ + m_orig[0] = 0; + m_orig[1] = 0; + m_orig[2] = 0; + + for (int i = 0; i < DT_MAX_AREAS; ++i) + m_areaCost[i] = 1.0f; +} + +dtNavMesh::~dtNavMesh() +{ + for (int i = 0; i < m_maxTiles; ++i) + { + if (m_tiles[i].data && m_tiles[i].ownsData) + { + delete [] m_tiles[i].data; + m_tiles[i].data = 0; + m_tiles[i].dataSize = 0; + } + } + delete m_nodePool; + delete m_openList; + delete [] m_posLookup; + delete [] m_tiles; +} + +bool dtNavMesh::init(const float* orig, float tileWidth, float tileHeight, + int maxTiles, int maxPolys, int maxNodes) +{ + vcopy(m_orig, orig); + m_tileWidth = tileWidth; + m_tileHeight = tileHeight; + + // Init tiles + m_maxTiles = maxTiles; + m_tileLutSize = nextPow2(maxTiles/4); + if (!m_tileLutSize) m_tileLutSize = 1; + m_tileLutMask = m_tileLutSize-1; + + m_tiles = new dtMeshTile[m_maxTiles]; + if (!m_tiles) + return false; + m_posLookup = new dtMeshTile*[m_tileLutSize]; + if (!m_posLookup) + return false; + memset(m_tiles, 0, sizeof(dtMeshTile)*m_maxTiles); + memset(m_posLookup, 0, sizeof(dtMeshTile*)*m_tileLutSize); + m_nextFree = 0; + for (int i = m_maxTiles-1; i >= 0; --i) + { + m_tiles[i].next = m_nextFree; + m_nextFree = &m_tiles[i]; + } + + if (!m_nodePool) + { + m_nodePool = new dtNodePool(maxNodes, nextPow2(maxNodes/4)); + if (!m_nodePool) + return false; + } + + if (!m_openList) + { + m_openList = new dtNodeQueue(maxNodes); + if (!m_openList) + return false; + } + + // Init ID generator values. + m_tileBits = max((unsigned int)1,ilog2(nextPow2((unsigned int)maxTiles))); + m_polyBits = max((unsigned int)1,ilog2(nextPow2((unsigned int)maxPolys))); + m_saltBits = 32 - m_tileBits - m_polyBits; + if (m_saltBits < 10) + return false; + + return true; +} + +bool dtNavMesh::init(unsigned char* data, int dataSize, bool ownsData, int maxNodes) +{ + // Make sure the data is in right format. + dtMeshHeader* header = (dtMeshHeader*)data; + if (header->magic != DT_NAVMESH_MAGIC) + return false; + if (header->version != DT_NAVMESH_VERSION) + return false; + + const float w = header->bmax[0] - header->bmin[0]; + const float h = header->bmax[2] - header->bmin[2]; + if (!init(header->bmin, w, h, 1, header->polyCount, maxNodes)) + return false; + + return addTileAt(0,0, data, dataSize, ownsData); +} + +////////////////////////////////////////////////////////////////////////////////////////// +int dtNavMesh::findConnectingPolys(const float* va, const float* vb, + dtMeshTile* tile, int side, + dtPolyRef* con, float* conarea, int maxcon) +{ + if (!tile) return 0; + + float amin[2], amax[2]; + calcRect(va,vb, amin,amax, side, 0.01f, tile->header->walkableClimb); + + // Remove links pointing to 'side' and compact the links array. + float bmin[2], bmax[2]; + unsigned short m = DT_EXT_LINK | (unsigned short)side; + int n = 0; + + dtPolyRef base = getTileId(tile); + + for (int i = 0; i < tile->header->polyCount; ++i) + { + dtPoly* poly = &tile->polys[i]; + const int nv = poly->vertCount; + for (int j = 0; j < nv; ++j) + { + // Skip edges which do not point to the right side. + if (poly->neis[j] != m) continue; + // Check if the segments touch. + const float* vc = &tile->verts[poly->verts[j]*3]; + const float* vd = &tile->verts[poly->verts[(j+1) % nv]*3]; + calcRect(vc,vd, bmin,bmax, side, 0.01f, tile->header->walkableClimb); + if (!overlapRects(amin,amax, bmin,bmax)) continue; + // Add return value. + if (n < maxcon) + { + conarea[n*2+0] = max(amin[0], bmin[0]); + conarea[n*2+1] = min(amax[0], bmax[0]); + con[n] = base | (unsigned int)i; + n++; + } + break; + } + } + return n; +} + +void dtNavMesh::unconnectExtLinks(dtMeshTile* tile, int side) +{ + if (!tile) return; + + for (int i = 0; i < tile->header->polyCount; ++i) + { + dtPoly* poly = &tile->polys[i]; + unsigned int j = poly->firstLink; + unsigned int pj = DT_NULL_LINK; + while (j != DT_NULL_LINK) + { + if (tile->links[j].side == side) + { + // Revove link. + unsigned int nj = tile->links[j].next; + if (pj == DT_NULL_LINK) + poly->firstLink = nj; + else + tile->links[pj].next = nj; + freeLink(tile, j); + j = nj; + } + else + { + // Advance + pj = j; + j = tile->links[j].next; + } + } + } +} + +void dtNavMesh::connectExtLinks(dtMeshTile* tile, dtMeshTile* target, int side) +{ + if (!tile) return; + + // Connect border links. + for (int i = 0; i < tile->header->polyCount; ++i) + { + dtPoly* poly = &tile->polys[i]; + + // Create new links. + unsigned short m = DT_EXT_LINK | (unsigned short)side; + const int nv = poly->vertCount; + for (int j = 0; j < nv; ++j) + { + // Skip edges which do not point to the right side. + if (poly->neis[j] != m) continue; + + // Create new links + const float* va = &tile->verts[poly->verts[j]*3]; + const float* vb = &tile->verts[poly->verts[(j+1) % nv]*3]; + dtPolyRef nei[4]; + float neia[4*2]; + int nnei = findConnectingPolys(va,vb, target, opposite(side), nei,neia,4); + for (int k = 0; k < nnei; ++k) + { + unsigned int idx = allocLink(tile); + if (idx != DT_NULL_LINK) + { + dtLink* link = &tile->links[idx]; + link->ref = nei[k]; + link->edge = (unsigned char)j; + link->side = (unsigned char)side; + + link->next = poly->firstLink; + poly->firstLink = idx; + + // Compress portal limits to a byte value. + if (side == 0 || side == 4) + { + const float lmin = min(va[2], vb[2]); + const float lmax = max(va[2], vb[2]); + link->bmin = (unsigned char)(clamp((neia[k*2+0]-lmin)/(lmax-lmin), 0.0f, 1.0f)*255.0f); + link->bmax = (unsigned char)(clamp((neia[k*2+1]-lmin)/(lmax-lmin), 0.0f, 1.0f)*255.0f); + } + else if (side == 2 || side == 6) + { + const float lmin = min(va[0], vb[0]); + const float lmax = max(va[0], vb[0]); + link->bmin = (unsigned char)(clamp((neia[k*2+0]-lmin)/(lmax-lmin), 0.0f, 1.0f)*255.0f); + link->bmax = (unsigned char)(clamp((neia[k*2+1]-lmin)/(lmax-lmin), 0.0f, 1.0f)*255.0f); + } + } + } + } + } +} + +void dtNavMesh::connectExtOffMeshLinks(dtMeshTile* tile, dtMeshTile* target, int side) +{ + if (!tile) return; + + // Connect off-mesh links. + // We are interested on links which land from target tile to this tile. + const unsigned char oppositeSide = (unsigned char)opposite(side); + dtQueryFilter defaultFilter; + + for (int i = 0; i < target->header->offMeshConCount; ++i) + { + dtOffMeshConnection* targetCon = &target->offMeshCons[i]; + if (targetCon->side != oppositeSide) + continue; + + dtPoly* targetPoly = &target->polys[targetCon->poly]; + + const float ext[3] = { targetCon->rad, target->header->walkableClimb, targetCon->rad }; + + // Find polygon to connect to. + const float* p = &targetCon->pos[3]; + float nearestPt[3]; + dtPolyRef ref = findNearestPolyInTile(tile, p, ext, &defaultFilter, nearestPt); + if (!ref) continue; + // findNearestPoly may return too optimistic results, further check to make sure. + if (sqr(nearestPt[0]-p[0])+sqr(nearestPt[2]-p[2]) > sqr(targetCon->rad)) + continue; + // Make sure the location is on current mesh. + float* v = &target->verts[targetPoly->verts[1]*3]; + vcopy(v, nearestPt); + + // Link off-mesh connection to target poly. + unsigned int idx = allocLink(target); + if (idx != DT_NULL_LINK) + { + dtLink* link = &target->links[idx]; + link->ref = ref; + link->edge = (unsigned char)1; + link->side = oppositeSide; + link->bmin = link->bmax = 0; + // Add to linked list. + link->next = targetPoly->firstLink; + targetPoly->firstLink = idx; + } + + // Link target poly to off-mesh connection. + if (targetCon->flags & DT_OFFMESH_CON_BIDIR) + { + unsigned int idx = allocLink(tile); + if (idx != DT_NULL_LINK) + { + unsigned short landPolyIdx = decodePolyIdPoly(ref); + dtPoly* landPoly = &tile->polys[landPolyIdx]; + dtLink* link = &tile->links[idx]; + link->ref = getTileId(target) | (unsigned int)(targetCon->poly); + link->edge = 0; + link->side = side; + link->bmin = link->bmax = 0; + // Add to linked list. + link->next = landPoly->firstLink; + landPoly->firstLink = idx; + } + } + } + +} + +void dtNavMesh::connectIntLinks(dtMeshTile* tile) +{ + if (!tile) return; + + dtPolyRef base = getTileId(tile); + + for (int i = 0; i < tile->header->polyCount; ++i) + { + dtPoly* poly = &tile->polys[i]; + poly->firstLink = DT_NULL_LINK; + + if (poly->type == DT_POLYTYPE_OFFMESH_CONNECTION) + continue; + + // Build edge links backwards so that the links will be + // in the linked list from lowest index to highest. + for (int j = poly->vertCount-1; j >= 0; --j) + { + // Skip hard and non-internal edges. + if (poly->neis[j] == 0 || (poly->neis[j] & DT_EXT_LINK)) continue; + + unsigned int idx = allocLink(tile); + if (idx != DT_NULL_LINK) + { + dtLink* link = &tile->links[idx]; + link->ref = base | (unsigned int)(poly->neis[j]-1); + link->edge = (unsigned char)j; + link->side = 0xff; + link->bmin = link->bmax = 0; + // Add to linked list. + link->next = poly->firstLink; + poly->firstLink = idx; + } + } + } +} + +void dtNavMesh::connectIntOffMeshLinks(dtMeshTile* tile) +{ + if (!tile) return; + + dtPolyRef base = getTileId(tile); + + // Find Off-mesh connection end points. + for (int i = 0; i < tile->header->offMeshConCount; ++i) + { + dtOffMeshConnection* con = &tile->offMeshCons[i]; + dtPoly* poly = &tile->polys[con->poly]; + dtQueryFilter defaultFilter; + + const float ext[3] = { con->rad, tile->header->walkableClimb, con->rad }; + + for (int j = 0; j < 2; ++j) + { + unsigned char side = j == 0 ? 0xff : con->side; + + if (side == 0xff) + { + // Find polygon to connect to. + const float* p = &con->pos[j*3]; + float nearestPt[3]; + dtPolyRef ref = findNearestPolyInTile(tile, p, ext, &defaultFilter, nearestPt); + if (!ref) continue; + // findNearestPoly may return too optimistic results, further check to make sure. + if (sqr(nearestPt[0]-p[0])+sqr(nearestPt[2]-p[2]) > sqr(con->rad)) + continue; + // Make sure the location is on current mesh. + float* v = &tile->verts[poly->verts[j]*3]; + vcopy(v, nearestPt); + + // Link off-mesh connection to target poly. + unsigned int idx = allocLink(tile); + if (idx != DT_NULL_LINK) + { + dtLink* link = &tile->links[idx]; + link->ref = ref; + link->edge = (unsigned char)j; + link->side = 0xff; + link->bmin = link->bmax = 0; + // Add to linked list. + link->next = poly->firstLink; + poly->firstLink = idx; + } + + // Start end-point is always connect back to off-mesh connection, + // Destination end-point only if it is bidirectional link. + if (j == 0 || (j == 1 && (con->flags & DT_OFFMESH_CON_BIDIR))) + { + // Link target poly to off-mesh connection. + unsigned int idx = allocLink(tile); + if (idx != DT_NULL_LINK) + { + unsigned short landPolyIdx = decodePolyIdPoly(ref); + dtPoly* landPoly = &tile->polys[landPolyIdx]; + dtLink* link = &tile->links[idx]; + link->ref = base | (unsigned int)(con->poly); + link->edge = 0; + link->side = 0xff; + link->bmin = link->bmax = 0; + // Add to linked list. + link->next = landPoly->firstLink; + landPoly->firstLink = idx; + } + } + + } + } + } +} + +bool dtNavMesh::addTileAt(int x, int y, unsigned char* data, int dataSize, bool ownsData) +{ + if (getTileAt(x,y)) + return false; + // Make sure there is enough space for new tile. + if (!m_nextFree) + return false; + // Make sure the data is in right format. + dtMeshHeader* header = (dtMeshHeader*)data; + if (header->magic != DT_NAVMESH_MAGIC) + return false; + if (header->version != DT_NAVMESH_VERSION) + return false; + + // Allocate a tile. + dtMeshTile* tile = m_nextFree; + m_nextFree = tile->next; + tile->next = 0; + + // Insert tile into the position lut. + int h = computeTileHash(x,y,m_tileLutMask); + tile->next = m_posLookup[h]; + m_posLookup[h] = tile; + + // Patch header pointers. + const int headerSize = align4(sizeof(dtMeshHeader)); + const int vertsSize = align4(sizeof(float)*3*header->vertCount); + const int polysSize = align4(sizeof(dtPoly)*header->polyCount); + const int linksSize = align4(sizeof(dtLink)*(header->maxLinkCount)); + const int detailMeshesSize = align4(sizeof(dtPolyDetail)*header->detailMeshCount); + const int detailVertsSize = align4(sizeof(float)*3*header->detailVertCount); + const int detailTrisSize = align4(sizeof(unsigned char)*4*header->detailTriCount); + const int bvtreeSize = align4(sizeof(dtBVNode)*header->bvNodeCount); + const int offMeshLinksSize = align4(sizeof(dtOffMeshConnection)*header->offMeshConCount); + + unsigned char* d = data + headerSize; + tile->verts = (float*)d; d += vertsSize; + tile->polys = (dtPoly*)d; d += polysSize; + tile->links = (dtLink*)d; d += linksSize; + tile->detailMeshes = (dtPolyDetail*)d; d += detailMeshesSize; + tile->detailVerts = (float*)d; d += detailVertsSize; + tile->detailTris = (unsigned char*)d; d += detailTrisSize; + tile->bvTree = (dtBVNode*)d; d += bvtreeSize; + tile->offMeshCons = (dtOffMeshConnection*)d; d += offMeshLinksSize; + + // Build links freelist + tile->linksFreeList = 0; + tile->links[header->maxLinkCount-1].next = DT_NULL_LINK; + for (int i = 0; i < header->maxLinkCount-1; ++i) + tile->links[i].next = i+1; + + // Init tile. + tile->header = header; + tile->x = x; + tile->y = y; + tile->data = data; + tile->dataSize = dataSize; + tile->ownsData = ownsData; + + connectIntLinks(tile); + connectIntOffMeshLinks(tile); + + // Create connections connections. + for (int i = 0; i < 8; ++i) + { + dtMeshTile* nei = getNeighbourTileAt(x,y,i); + if (nei) + { + connectExtLinks(tile, nei, i); + connectExtLinks(nei, tile, opposite(i)); + connectExtOffMeshLinks(tile, nei, i); + connectExtOffMeshLinks(nei, tile, opposite(i)); + } + } + + return true; +} + +dtMeshTile* dtNavMesh::getTileAt(int x, int y) +{ + // Find tile based on hash. + int h = computeTileHash(x,y,m_tileLutMask); + dtMeshTile* tile = m_posLookup[h]; + while (tile) + { + if (tile->x == x && tile->y == y) + return tile; + tile = tile->next; + } + return 0; +} + +int dtNavMesh::getMaxTiles() const +{ + return m_maxTiles; +} + +dtMeshTile* dtNavMesh::getTile(int i) +{ + return &m_tiles[i]; +} + +const dtMeshTile* dtNavMesh::getTile(int i) const +{ + return &m_tiles[i]; +} + +const dtMeshTile* dtNavMesh::getTileByRef(dtPolyRef ref, int* polyIndex) const +{ + unsigned int salt, it, ip; + decodePolyId(ref, salt, it, ip); + if (it >= (unsigned int)m_maxTiles) return 0; + if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return 0; + if (ip >= (unsigned int)m_tiles[it].header->polyCount) return 0; + if (polyIndex) *polyIndex = (int)ip; + return &m_tiles[it]; +} + +dtMeshTile* dtNavMesh::getNeighbourTileAt(int x, int y, int side) +{ + 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; + }; + return getTileAt(x,y); +} + +bool dtNavMesh::removeTileAt(int x, int y, unsigned char** data, int* dataSize) +{ + // Remove tile from hash lookup. + int h = computeTileHash(x,y,m_tileLutMask); + dtMeshTile* prev = 0; + dtMeshTile* tile = m_posLookup[h]; + while (tile) + { + if (tile->x == x && tile->y == y) + { + if (prev) + prev->next = tile->next; + else + m_posLookup[h] = tile->next; + break; + } + prev = tile; + tile = tile->next; + } + if (!tile) + return false; + + // Remove connections to neighbour tiles. + for (int i = 0; i < 8; ++i) + { + dtMeshTile* nei = getNeighbourTileAt(x,y,i); + if (!nei) continue; + unconnectExtLinks(nei, opposite(i)); + } + + + // Reset tile. + if (tile->ownsData) + { + // Owns data + delete [] tile->data; + tile->data = 0; + tile->dataSize = 0; + if (data) *data = 0; + if (dataSize) *dataSize = 0; + } + else + { + if (data) *data = tile->data; + if (dataSize) *dataSize = tile->dataSize; + } + tile->header = 0; + tile->linksFreeList = 0; + tile->polys = 0; + tile->verts = 0; + tile->links = 0; + tile->detailMeshes = 0; + tile->detailVerts = 0; + tile->detailTris = 0; + tile->bvTree = 0; + tile->offMeshCons = 0; + + tile->x = tile->y = 0; + tile->salt++; + + // Add to free list. + tile->next = m_nextFree; + m_nextFree = tile; + + return true; +} + +dtPolyRef dtNavMesh::getTileId(const dtMeshTile* tile) const +{ + if (!tile) return 0; + const unsigned int it = tile - m_tiles; + return encodePolyId(tile->salt, it, 0); +} + +////////////////////////////////////////////////////////////////////////////////////////// +bool dtNavMesh::closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest) const +{ + unsigned int salt, it, ip; + decodePolyId(ref, salt, it, ip); + if (it >= (unsigned int)m_maxTiles) return false; + if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return false; + const dtMeshHeader* header = m_tiles[it].header; + if (ip >= (unsigned int)header->polyCount) return false; + + return closestPointOnPolyInTile(&m_tiles[it], ip, pos, closest); +} + +bool dtNavMesh::closestPointOnPolyInTile(const dtMeshTile* tile, unsigned int ip, const float* pos, float* closest) const +{ + const dtPoly* poly = &tile->polys[ip]; + + float closestDistSqr = FLT_MAX; + const dtPolyDetail* pd = &tile->detailMeshes[ip]; + + for (int j = 0; j < pd->triCount; ++j) + { + const unsigned char* t = &tile->detailTris[(pd->triBase+j)*4]; + const float* v[3]; + for (int k = 0; k < 3; ++k) + { + if (t[k] < poly->vertCount) + v[k] = &tile->verts[poly->verts[t[k]]*3]; + else + v[k] = &tile->detailVerts[(pd->vertBase+(t[k]-poly->vertCount))*3]; + } + float pt[3]; + closestPtPointTriangle(pt, pos, v[0], v[1], v[2]); + float d = vdistSqr(pos, pt); + if (d < closestDistSqr) + { + vcopy(closest, pt); + closestDistSqr = d; + } + } + + return true; +} + +bool dtNavMesh::closestPointOnPolyBoundary(dtPolyRef ref, const float* pos, float* closest) const +{ + unsigned int salt, it, ip; + decodePolyId(ref, salt, it, ip); + if (it >= (unsigned int)m_maxTiles) return false; + if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return false; + const dtMeshTile* tile = &m_tiles[it]; + + if (ip >= (unsigned int)tile->header->polyCount) return false; + const dtPoly* poly = &tile->polys[ip]; + + // Collect vertices. + float verts[DT_VERTS_PER_POLYGON*3]; + float edged[DT_VERTS_PER_POLYGON]; + float edget[DT_VERTS_PER_POLYGON]; + int nv = 0; + for (int i = 0; i < (int)poly->vertCount; ++i) + { + vcopy(&verts[nv*3], &tile->verts[poly->verts[i]*3]); + nv++; + } + + bool inside = distancePtPolyEdgesSqr(pos, verts, nv, edged, edget); + if (inside) + { + // Point is inside the polygon, return the point. + vcopy(closest, pos); + } + else + { + // Point is outside the polygon, clamp 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]; + vlerp(closest, va, vb, edget[imin]); + } + + return true; +} + +// Returns start and end location of an off-mesh link polygon. +bool dtNavMesh::getOffMeshConnectionPolyEndPoints(dtPolyRef prevRef, dtPolyRef polyRef, float* startPos, float* endPos) const +{ + unsigned int salt, it, ip; + + // Get current polygon + decodePolyId(polyRef, salt, it, ip); + if (it >= (unsigned int)m_maxTiles) return false; + if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return false; + const dtMeshTile* tile = &m_tiles[it]; + if (ip >= (unsigned int)tile->header->polyCount) return false; + const dtPoly* poly = &tile->polys[ip]; + + // Make sure that the current poly is indeed off-mesh link. + if (poly->type != DT_POLYTYPE_OFFMESH_CONNECTION) + return false; + + // Figure out which way to hand out the vertices. + int idx0 = 0, idx1 = 1; + + // Find link that points to first vertex. + for (unsigned int i = poly->firstLink; i != DT_NULL_LINK; i = tile->links[i].next) + { + if (tile->links[i].edge == 0) + { + if (tile->links[i].ref != prevRef) + { + idx0 = 1; + idx1 = 0; + } + break; + } + } + + vcopy(startPos, &tile->verts[poly->verts[idx0]*3]); + vcopy(endPos, &tile->verts[poly->verts[idx1]*3]); + + return true; +} + + +bool dtNavMesh::getPolyHeight(dtPolyRef ref, const float* pos, float* height) const +{ + unsigned int salt, it, ip; + decodePolyId(ref, salt, it, ip); + if (it >= (unsigned int)m_maxTiles) return false; + if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return false; + const dtMeshTile* tile = &m_tiles[it]; + + if (ip >= (unsigned int)tile->header->polyCount) return false; + const dtPoly* poly = &tile->polys[ip]; + + if (poly->type == 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 = vdist(pos, v0); + const float d1 = vdist(pos, v1); + const float u = d0 / (d0+d1); + if (height) + *height = v0[1] + (v1[1] - v0[1]) * u; + return true; + } + else + { + const dtPolyDetail* pd = &tile->detailMeshes[ip]; + for (int j = 0; j < pd->triCount; ++j) + { + const unsigned char* t = &tile->detailTris[(pd->triBase+j)*4]; + const float* v[3]; + for (int k = 0; k < 3; ++k) + { + if (t[k] < poly->vertCount) + v[k] = &tile->verts[poly->verts[t[k]]*3]; + else + v[k] = &tile->detailVerts[(pd->vertBase+(t[k]-poly->vertCount))*3]; + } + float h; + if (closestHeightPointTriangle(pos, v[0], v[1], v[2], h)) + { + if (height) + *height = h; + return true; + } + } + } + + return false; +} + +void dtNavMesh::setAreaCost(const int area, float cost) +{ + if (area >= 0 && area < DT_MAX_AREAS) + m_areaCost[area] = cost; +} + +float dtNavMesh::getAreaCost(const int area) const +{ + if (area >= 0 && area < DT_MAX_AREAS) + return m_areaCost[area]; + return -1; +} + +dtPolyRef dtNavMesh::findNearestPoly(const float* center, const float* extents, dtQueryFilter* filter, float* nearestPt) +{ + // Get nearby polygons from proximity grid. + dtPolyRef polys[128]; + int polyCount = queryPolygons(center, extents, 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]; + float closestPtPoly[3]; + if (!closestPointOnPoly(ref, center, closestPtPoly)) + continue; + float d = vdistSqr(center, closestPtPoly); + if (d < nearestDistanceSqr) + { + if (nearestPt) + vcopy(nearestPt, closestPtPoly); + nearestDistanceSqr = d; + nearest = ref; + } + } + + return nearest; +} + +dtPolyRef dtNavMesh::findNearestPolyInTile(dtMeshTile* tile, const float* center, const float* extents, + dtQueryFilter* filter, float* nearestPt) +{ + float bmin[3], bmax[3]; + vsub(bmin, center, extents); + vadd(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]; + float closestPtPoly[3]; + if (!closestPointOnPolyInTile(tile, decodePolyIdPoly(ref), center, closestPtPoly)) + continue; + float d = vdistSqr(center, closestPtPoly); + if (d < nearestDistanceSqr) + { + if (nearestPt) + vcopy(nearestPt, closestPtPoly); + nearestDistanceSqr = d; + nearest = ref; + } + } + + return nearest; +} + +int dtNavMesh::queryPolygonsInTile(dtMeshTile* tile, const float* qmin, const float* qmax, + dtQueryFilter* filter, + dtPolyRef* polys, const int maxPolys) +{ + if (tile->bvTree) + { + const dtBVNode* node = &tile->bvTree[0]; + const dtBVNode* end = &tile->bvTree[tile->header->bvNodeCount]; + const float* tbmin = tile->header->bmin; + const float* tbmax = tile->header->bmax; + const float qfac = tile->header->bvQuantFactor; + + // Calculate quantized box + unsigned short bmin[3], bmax[3]; + // Clamp query box to world box. + float minx = clamp(qmin[0], tbmin[0], tbmax[0]) - tbmin[0]; + float miny = clamp(qmin[1], tbmin[1], tbmax[1]) - tbmin[1]; + float minz = clamp(qmin[2], tbmin[2], tbmax[2]) - tbmin[2]; + float maxx = clamp(qmax[0], tbmin[0], tbmax[0]) - tbmin[0]; + float maxy = clamp(qmax[1], tbmin[1], tbmax[1]) - tbmin[1]; + float maxz = clamp(qmax[2], tbmin[2], tbmax[2]) - tbmin[2]; + // Quantize + bmin[0] = (unsigned short)(qfac * minx) & 0xfffe; + bmin[1] = (unsigned short)(qfac * miny) & 0xfffe; + bmin[2] = (unsigned short)(qfac * minz) & 0xfffe; + bmax[0] = (unsigned short)(qfac * maxx + 1) | 1; + bmax[1] = (unsigned short)(qfac * maxy + 1) | 1; + bmax[2] = (unsigned short)(qfac * maxz + 1) | 1; + + // Traverse tree + dtPolyRef base = getTileId(tile); + int n = 0; + while (node < end) + { + bool overlap = checkOverlapBox(bmin, bmax, node->bmin, node->bmax); + bool isLeafNode = node->i >= 0; + + if (isLeafNode && overlap) + { + if (passFilter(filter, tile->polys[node->i].flags)) + { + if (n < maxPolys) + polys[n++] = base | (dtPolyRef)node->i; + } + } + + if (overlap || isLeafNode) + node++; + else + { + const int escapeIndex = -node->i; + node += escapeIndex; + } + } + + return n; + } + else + { + float bmin[3], bmax[3]; + int n = 0; + dtPolyRef base = getTileId(tile); + for (int i = 0; i < tile->header->polyCount; ++i) + { + // Calc polygon bounds. + dtPoly* p = &tile->polys[i]; + const float* v = &tile->verts[p->verts[0]*3]; + vcopy(bmin, v); + vcopy(bmax, v); + for (int j = 1; j < p->vertCount; ++j) + { + v = &tile->verts[p->verts[j]*3]; + vmin(bmin, v); + vmax(bmax, v); + } + if (overlapBoxes(qmin,qmax, bmin,bmax)) + { + if (passFilter(filter, p->flags)) + { + if (n < maxPolys) + polys[n++] = base | (dtPolyRef)i; + } + } + } + return n; + } +} + +int dtNavMesh::queryPolygons(const float* center, const float* extents, dtQueryFilter* filter, + dtPolyRef* polys, const int maxPolys) +{ + float bmin[3], bmax[3]; + vsub(bmin, center, extents); + vadd(bmax, center, extents); + + // Find tiles the query touches. + const int minx = (int)((bmin[0]-m_orig[0]) / m_tileWidth); + const int maxx = (int)((bmax[0]-m_orig[0]) / m_tileWidth); + const int miny = (int)((bmin[2]-m_orig[2]) / m_tileHeight); + const int maxy = (int)((bmax[2]-m_orig[2]) / m_tileHeight); + + int n = 0; + for (int y = miny; y <= maxy; ++y) + { + for (int x = minx; x <= maxx; ++x) + { + dtMeshTile* tile = getTileAt(x,y); + if (!tile) continue; + n += queryPolygonsInTile(tile, bmin, bmax, filter, polys+n, maxPolys-n); + if (n >= maxPolys) return n; + } + } + + return n; +} + +int dtNavMesh::findPath(dtPolyRef startRef, dtPolyRef endRef, + const float* startPos, const float* endPos, + dtQueryFilter* filter, + dtPolyRef* path, const int maxPathSize) +{ + if (!startRef || !endRef) + return 0; + + if (!maxPathSize) + return 0; + + if (!getPolyByRef(startRef) || !getPolyByRef(endRef)) + return 0; + + if (startRef == endRef) + { + path[0] = startRef; + return 1; + } + + if (!m_nodePool || !m_openList) + return 0; + + m_nodePool->clear(); + m_openList->clear(); + + static const float H_SCALE = 0.999f; // Heuristic scale. + + dtNode* startNode = m_nodePool->getNode(startRef); + startNode->pidx = 0; + startNode->cost = 0; + startNode->total = vdist(startPos, endPos) * H_SCALE; + startNode->id = startRef; + startNode->flags = DT_NODE_OPEN; + m_openList->push(startNode); + + dtNode* lastBestNode = startNode; + float lastBestNodeCost = startNode->total; + + unsigned int it, ip; + + while (!m_openList->empty()) + { + dtNode* bestNode = m_openList->pop(); + // Remove node from open list and put it in closed list. + bestNode->flags &= ~DT_NODE_OPEN; + bestNode->flags |= DT_NODE_CLOSED; + + // Reached the goal, stop searching. + if (bestNode->id == endRef) + { + lastBestNode = bestNode; + break; + } + + float previousEdgeMidPoint[3]; + + // Get current poly and tile. + // The API input has been cheked already, skip checking internal data. + const dtPolyRef bestRef = bestNode->id; + it = decodePolyIdTile(bestRef); + ip = decodePolyIdPoly(bestRef); + const dtMeshTile* bestTile = &m_tiles[it]; + const dtPoly* bestPoly = &bestTile->polys[ip]; + + // 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) + { + it = decodePolyIdTile(parentRef); + ip = decodePolyIdPoly(parentRef); + parentTile = &m_tiles[it]; + parentPoly = &parentTile->polys[ip]; + + getEdgeMidPoint(parentRef, parentPoly, parentTile, + bestRef, bestPoly, bestTile, previousEdgeMidPoint); + } + else + { + vcopy(previousEdgeMidPoint, startPos); + } + + for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next) + { + dtPolyRef neighbourRef = bestTile->links[i].ref; + + // Skip invalid ids and do not expand back to where we came from. + if (!neighbourRef || neighbourRef == bestRef) + continue; + + // Get neighbour poly and tile. + // The API input has been cheked already, skip checking internal data. + it = decodePolyIdTile(neighbourRef); + ip = decodePolyIdPoly(neighbourRef); + const dtMeshTile* neighbourTile = &m_tiles[it]; + const dtPoly* neighbourPoly = &neighbourTile->polys[ip]; + + if (!passFilter(filter, neighbourPoly->flags)) + continue; + + dtNode newNode; + newNode.pidx = m_nodePool->getNodeIdx(bestNode); + newNode.id = neighbourRef; + + // Calculate cost. + float edgeMidPoint[3]; + + getEdgeMidPoint(bestRef, bestPoly, bestTile, + neighbourRef, neighbourPoly, neighbourTile, edgeMidPoint); + + // Special case for last node. + float h = 0; + if (neighbourRef == endRef) + { + // Cost + newNode.cost = bestNode->cost + + vdist(previousEdgeMidPoint,edgeMidPoint) * m_areaCost[bestPoly->area] + + vdist(edgeMidPoint, endPos) * m_areaCost[neighbourPoly->area]; + // Heuristic + h = 0; + } + else + { + // Cost + newNode.cost = bestNode->cost + + vdist(previousEdgeMidPoint,edgeMidPoint) * m_areaCost[bestPoly->area]; + // Heuristic + h = vdist(edgeMidPoint,endPos)*H_SCALE; + } + newNode.total = newNode.cost + h; + + dtNode* actualNode = m_nodePool->getNode(newNode.id); + if (!actualNode) + continue; + + // The node is already in open list and the new result is worse, skip. + if ((actualNode->flags & DT_NODE_OPEN) && newNode.total >= actualNode->total) + continue; + // The node is already visited and process, and the new result is worse, skip. + if ((actualNode->flags & DT_NODE_CLOSED) && newNode.total >= actualNode->total) + continue; + + // Add or update the node. + actualNode->flags &= ~DT_NODE_CLOSED; + actualNode->pidx = newNode.pidx; + actualNode->cost = newNode.cost; + actualNode->total = newNode.total; + + // Update nearest node to target so far. + if (h < lastBestNodeCost) + { + lastBestNodeCost = h; + lastBestNode = actualNode; + } + + if (actualNode->flags & DT_NODE_OPEN) + { + // Already in open, update node location. + m_openList->modify(actualNode); + } + else + { + // Put the node in open list. + actualNode->flags |= DT_NODE_OPEN; + m_openList->push(actualNode); + } + } + } + + // Reverse the path. + dtNode* prev = 0; + dtNode* node = lastBestNode; + do + { + dtNode* next = m_nodePool->getNodeAtIdx(node->pidx); + node->pidx = m_nodePool->getNodeIdx(prev); + prev = node; + node = next; + } + while (node); + + // Store path + node = prev; + int n = 0; + do + { + path[n++] = node->id; + node = m_nodePool->getNodeAtIdx(node->pidx); + } + while (node && n < maxPathSize); + + return n; +} + +int dtNavMesh::findStraightPath(const float* startPos, const float* endPos, + const dtPolyRef* path, const int pathSize, + float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs, + const int maxStraightPathSize) +{ + if (!maxStraightPathSize) + return 0; + + if (!path[0]) + return 0; + + int straightPathSize = 0; + + // TODO: Should this be callers responsibility? + float closestStartPos[3]; + if (!closestPointOnPolyBoundary(path[0], startPos, closestStartPos)) + return 0; + + // Add start point. + vcopy(&straightPath[straightPathSize*3], closestStartPos); + if (straightPathFlags) + straightPathFlags[straightPathSize] = DT_STRAIGHTPATH_START; + if (straightPathRefs) + straightPathRefs[straightPathSize] = path[0]; + straightPathSize++; + if (straightPathSize >= maxStraightPathSize) + return straightPathSize; + + float closestEndPos[3]; + if (!closestPointOnPolyBoundary(path[pathSize-1], endPos, closestEndPos)) + return 0; + + if (pathSize > 1) + { + float portalApex[3], portalLeft[3], portalRight[3]; + vcopy(portalApex, closestStartPos); + vcopy(portalLeft, portalApex); + vcopy(portalRight, portalApex); + int apexIndex = 0; + int leftIndex = 0; + int rightIndex = 0; + + unsigned char leftPolyType = 0; + unsigned char rightPolyType = 0; + + dtPolyRef leftPolyRef = path[0]; + dtPolyRef rightPolyRef = path[0]; + + for (int i = 0; i < pathSize; ++i) + { + float left[3], right[3]; + unsigned char fromType, toType; + + if (i+1 < pathSize) + { + // Next portal. + if (!getPortalPoints(path[i], path[i+1], left, right, fromType, toType)) + { + if (!closestPointOnPolyBoundary(path[i], endPos, closestEndPos)) + return 0; + + vcopy(&straightPath[straightPathSize*3], closestEndPos); + if (straightPathFlags) + straightPathFlags[straightPathSize] = 0; + if (straightPathRefs) + straightPathRefs[straightPathSize] = path[i]; + straightPathSize++; + + return straightPathSize; + } + + // If starting really close the portal, advance. + if (i == 0) + { + float t; + if (distancePtSegSqr2D(portalApex, left, right, t) < (0.001*0.001f)) + continue; + } + } + else + { + // End of the path. + vcopy(left, closestEndPos); + vcopy(right, closestEndPos); + + fromType = toType = DT_POLYTYPE_GROUND; + } + + // Right vertex. + if (vequal(portalApex, portalRight)) + { + vcopy(portalRight, right); + rightPolyRef = (i+1 < pathSize) ? path[i+1] : 0; + rightPolyType = toType; + rightIndex = i; + } + else + { + if (triArea2D(portalApex, portalRight, right) <= 0.0f) + { + if (triArea2D(portalApex, portalLeft, right) > 0.0f) + { + vcopy(portalRight, right); + rightPolyRef = (i+1 < pathSize) ? path[i+1] : 0; + rightPolyType = toType; + rightIndex = i; + } + else + { + vcopy(portalApex, portalLeft); + apexIndex = leftIndex; + + unsigned char flags = (leftPolyType == DT_POLYTYPE_OFFMESH_CONNECTION) ? DT_STRAIGHTPATH_OFFMESH_CONNECTION : 0; + dtPolyRef ref = leftPolyRef; + + if (!vequal(&straightPath[(straightPathSize-1)*3], portalApex)) + { + vcopy(&straightPath[straightPathSize*3], portalApex); + if (straightPathFlags) + straightPathFlags[straightPathSize] = flags; + if (straightPathRefs) + straightPathRefs[straightPathSize] = ref; + + straightPathSize++; + if (straightPathSize >= maxStraightPathSize) + return straightPathSize; + } + else + { + // The vertices are equal, update flags and poly. + if (straightPathFlags) + straightPathFlags[straightPathSize-1] = flags; + if (straightPathRefs) + straightPathRefs[straightPathSize-1] = ref; + } + + vcopy(portalLeft, portalApex); + vcopy(portalRight, portalApex); + leftIndex = apexIndex; + rightIndex = apexIndex; + + // Restart + i = apexIndex; + + continue; + } + } + } + + // Left vertex. + if (vequal(portalApex, portalLeft)) + { + vcopy(portalLeft, left); + leftPolyRef = (i+1 < pathSize) ? path[i+1] : 0; + leftPolyType = toType; + leftIndex = i; + } + else + { + if (triArea2D(portalApex, portalLeft, left) >= 0.0f) + { + if (triArea2D(portalApex, portalRight, left) < 0.0f) + { + vcopy(portalLeft, left); + leftPolyRef = (i+1 < pathSize) ? path[i+1] : 0; + leftPolyType = toType; + leftIndex = i; + } + else + { + vcopy(portalApex, portalRight); + apexIndex = rightIndex; + + unsigned char flags = (rightPolyType == DT_POLYTYPE_OFFMESH_CONNECTION) ? DT_STRAIGHTPATH_OFFMESH_CONNECTION : 0; + dtPolyRef ref = rightPolyRef; + + if (!vequal(&straightPath[(straightPathSize-1)*3], portalApex)) + { + vcopy(&straightPath[straightPathSize*3], portalApex); + if (straightPathFlags) + straightPathFlags[straightPathSize] = flags; + if (straightPathRefs) + straightPathRefs[straightPathSize] = ref; + + straightPathSize++; + if (straightPathSize >= maxStraightPathSize) + return straightPathSize; + } + else + { + // The vertices are equal, update flags and poly. + if (straightPathFlags) + straightPathFlags[straightPathSize-1] = flags; + if (straightPathRefs) + straightPathRefs[straightPathSize-1] = ref; + } + + vcopy(portalLeft, portalApex); + vcopy(portalRight, portalApex); + leftIndex = apexIndex; + rightIndex = apexIndex; + + // Restart + i = apexIndex; + + continue; + } + } + } + } + } + + // Add end point. + vcopy(&straightPath[straightPathSize*3], closestEndPos); + if (straightPathFlags) + straightPathFlags[straightPathSize] = DT_STRAIGHTPATH_END; + if (straightPathRefs) + straightPathRefs[straightPathSize] = 0; + + straightPathSize++; + + return straightPathSize; +} + +// Moves towards end position a long the path corridor. +// Returns: Index to the result path polygon. +int dtNavMesh::moveAlongPathCorridor(const float* startPos, const float* endPos, float* resultPos, + const dtPolyRef* path, const int pathSize) +{ + if (!pathSize) + return 0; + + float verts[DT_VERTS_PER_POLYGON*3]; + float edged[DT_VERTS_PER_POLYGON]; + float edget[DT_VERTS_PER_POLYGON]; + int n = 0; + + static const float SLOP = 0.01f; + + vcopy(resultPos, startPos); + + while (n < pathSize) + { + // Get current polygon and poly vertices. + unsigned int salt, it, ip; + decodePolyId(path[n], salt, it, ip); + if (it >= (unsigned int)m_maxTiles) return n; + if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return n; + if (ip >= (unsigned int)m_tiles[it].header->polyCount) return n; + const dtMeshTile* tile = &m_tiles[it]; + const dtPoly* poly = &tile->polys[ip]; + + // In case of Off-Mesh link, just snap to the end location and advance over it. + if (poly->type == DT_POLYTYPE_OFFMESH_CONNECTION) + { + if (n+1 < pathSize) + { + float left[3], right[3]; + unsigned char fromType, toType; + if (!getPortalPoints(path[n], path[n+1], left, right, fromType, toType)) + return n; + vcopy(resultPos, endPos); + } + return n+1; + } + + // Collect vertices. + int nv = 0; + for (int i = 0; i < (int)poly->vertCount; ++i) + { + vcopy(&verts[nv*3], &tile->verts[poly->verts[i]*3]); + nv++; + } + + bool inside = distancePtPolyEdgesSqr(endPos, verts, nv, edged, edget); + if (inside) + { + // The end point is inside the current polygon. + vcopy(resultPos, endPos); + return n; + } + + // Constraint the point on the polygon boundary. + // This results sliding movement. + 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]; + vlerp(resultPos, va, vb, edget[imin]); + + // Check to see if the point is on the portal edge to the next polygon. + if (n+1 >= pathSize) + return n; + // TODO: optimize + float left[3], right[3]; + unsigned char fromType, toType; + if (!getPortalPoints(path[n], path[n+1], left, right, fromType, toType)) + return n; + // If the clamped point is close to the next portal edge, advance to next poly. + float t; + float d = distancePtSegSqr2D(resultPos, left, right, t); + if (d > SLOP*SLOP) + return n; + // Advance to next polygon. + n++; + } + + return n; +} + +bool dtNavMesh::getPortalPoints(dtPolyRef from, dtPolyRef to, float* left, float* right, + unsigned char& fromType, unsigned char& toType) const +{ + unsigned int salt, it, ip; + decodePolyId(from, salt, it, ip); + if (it >= (unsigned int)m_maxTiles) return false; + if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return false; + const dtMeshTile* fromTile = &m_tiles[it]; + if (ip >= (unsigned int)fromTile->header->polyCount) return false; + const dtPoly* fromPoly = &fromTile->polys[ip]; + fromType = fromPoly->type; + + decodePolyId(to, salt, it, ip); + if (it >= (unsigned int)m_maxTiles) return false; + if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return false; + const dtMeshTile* toTile = &m_tiles[it]; + if (ip >= (unsigned int)toTile->header->polyCount) return false; + const dtPoly* toPoly = &toTile->polys[ip]; + toType = toPoly->type; + + return getPortalPoints(from, fromPoly, fromTile, + to, toPoly, toTile, + left, right); +} + +// Returns portal points between two polygons. +bool dtNavMesh::getPortalPoints(dtPolyRef from, const dtPoly* fromPoly, const dtMeshTile* fromTile, + dtPolyRef to, const dtPoly* toPoly, const dtMeshTile* toTile, + float* left, float* right) const +{ + // Find the link that points to the 'to' polygon. + const dtLink* link = 0; + for (unsigned int i = fromPoly->firstLink; i != DT_NULL_LINK; i = fromTile->links[i].next) + { + if (fromTile->links[i].ref == to) + { + link = &fromTile->links[i]; + break; + } + } + if (!link) + return false; + + // Handle off-mesh connections. + if (fromPoly->type == DT_POLYTYPE_OFFMESH_CONNECTION) + { + // Find link that points to first vertex. + for (unsigned int i = fromPoly->firstLink; i != DT_NULL_LINK; i = fromTile->links[i].next) + { + if (fromTile->links[i].ref == to) + { + const int v = fromTile->links[i].edge; + vcopy(left, &fromTile->verts[fromPoly->verts[v]*3]); + vcopy(right, &fromTile->verts[fromPoly->verts[v]*3]); + return true; + } + } + return false; + } + + if (toPoly->type == DT_POLYTYPE_OFFMESH_CONNECTION) + { + for (unsigned int i = toPoly->firstLink; i != DT_NULL_LINK; i = toTile->links[i].next) + { + if (toTile->links[i].ref == from) + { + const int v = toTile->links[i].edge; + vcopy(left, &toTile->verts[toPoly->verts[v]*3]); + vcopy(right, &toTile->verts[toPoly->verts[v]*3]); + return true; + } + } + return false; + } + + // Find portal vertices. + const int v0 = fromPoly->verts[link->edge]; + const int v1 = fromPoly->verts[(link->edge+1) % (int)fromPoly->vertCount]; + vcopy(left, &fromTile->verts[v0*3]); + vcopy(right, &fromTile->verts[v1*3]); + + // If the link is at tile boundary, clamp the vertices to + // the link width. + if (link->side == 0 || link->side == 4) + { + // Unpack portal limits. + const float smin = min(left[2],right[2]); + const float smax = max(left[2],right[2]); + const float s = (smax-smin) / 255.0f; + const float lmin = smin + link->bmin*s; + const float lmax = smin + link->bmax*s; + left[2] = max(left[2],lmin); + left[2] = min(left[2],lmax); + right[2] = max(right[2],lmin); + right[2] = min(right[2],lmax); + } + else if (link->side == 2 || link->side == 6) + { + // Unpack portal limits. + const float smin = min(left[0],right[0]); + const float smax = max(left[0],right[0]); + const float s = (smax-smin) / 255.0f; + const float lmin = smin + link->bmin*s; + const float lmax = smin + link->bmax*s; + left[0] = max(left[0],lmin); + left[0] = min(left[0],lmax); + right[0] = max(right[0],lmin); + right[0] = min(right[0],lmax); + } + + return true; +} + +// Returns edge mid point between two polygons. +bool dtNavMesh::getEdgeMidPoint(dtPolyRef from, dtPolyRef to, float* mid) const +{ + float left[3], right[3]; + unsigned char fromType, toType; + if (!getPortalPoints(from, to, left,right, fromType, toType)) return false; + 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 true; +} + +bool dtNavMesh::getEdgeMidPoint(dtPolyRef from, const dtPoly* fromPoly, const dtMeshTile* fromTile, + dtPolyRef to, const dtPoly* toPoly, const dtMeshTile* toTile, + float* mid) const +{ + float left[3], right[3]; + if (!getPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, left, right)) + return false; + 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 true; +} + +void dtNavMesh::setPolyFlags(dtPolyRef ref, unsigned short flags) +{ + unsigned int salt, it, ip; + decodePolyId(ref, salt, it, ip); + if (it >= (unsigned int)m_maxTiles) return; + if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return; + dtMeshTile* tile = &m_tiles[it]; + if (ip >= (unsigned int)tile->header->polyCount) return; + dtPoly* poly = &tile->polys[ip]; + // Change flags. + poly->flags = flags; +} + +unsigned short dtNavMesh::getPolyFlags(dtPolyRef ref) const +{ + unsigned int salt, it, ip; + decodePolyId(ref, salt, it, ip); + if (it >= (unsigned int)m_maxTiles) return 0; + if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return 0; + const dtMeshTile* tile = &m_tiles[it]; + if (ip >= (unsigned int)tile->header->polyCount) return 0; + const dtPoly* poly = &tile->polys[ip]; + return poly->flags; +} + +void dtNavMesh::setPolyArea(dtPolyRef ref, unsigned char area) +{ + unsigned int salt, it, ip; + decodePolyId(ref, salt, it, ip); + if (it >= (unsigned int)m_maxTiles) return; + if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return; + dtMeshTile* tile = &m_tiles[it]; + if (ip >= (unsigned int)tile->header->polyCount) return; + dtPoly* poly = &tile->polys[ip]; + poly->area = area; +} + +unsigned char dtNavMesh::getPolyArea(dtPolyRef ref) const +{ + unsigned int salt, it, ip; + decodePolyId(ref, salt, it, ip); + if (it >= (unsigned int)m_maxTiles) return 0; + if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return 0; + const dtMeshTile* tile = &m_tiles[it]; + if (ip >= (unsigned int)tile->header->polyCount) return 0; + const dtPoly* poly = &tile->polys[ip]; + return poly->area; +} + +int dtNavMesh::raycast(dtPolyRef centerRef, const float* startPos, const float* endPos, dtQueryFilter* filter, + float& t, float* hitNormal, dtPolyRef* path, const int pathSize) +{ + t = 0; + + if (!centerRef || !getPolyByRef(centerRef)) + return 0; + + dtPolyRef curRef = centerRef; + float verts[DT_VERTS_PER_POLYGON*3]; + int n = 0; + + hitNormal[0] = 0; + hitNormal[1] = 0; + hitNormal[2] = 0; + + while (curRef) + { + // Cast ray against current polygon. + + // The API input has been cheked already, skip checking internal data. + unsigned int it = decodePolyIdTile(curRef); + unsigned int ip = decodePolyIdPoly(curRef); + const dtMeshTile* tile = &m_tiles[it]; + const dtPoly* poly = &tile->polys[ip]; + + // Collect vertices. + int nv = 0; + for (int i = 0; i < (int)poly->vertCount; ++i) + { + vcopy(&verts[nv*3], &tile->verts[poly->verts[i]*3]); + nv++; + } + + float tmin, tmax; + int segMin, segMax; + if (!intersectSegmentPoly2D(startPos, endPos, verts, nv, tmin, tmax, segMin, segMax)) + { + // Could not hit the polygon, keep the old t and report hit. + return n; + } + // Keep track of furthest t so far. + if (tmax > t) + t = tmax; + + if (n < pathSize) + path[n++] = curRef; + + // Follow neighbours. + dtPolyRef nextRef = 0; + + for (unsigned int i = poly->firstLink; i != DT_NULL_LINK; i = tile->links[i].next) + { + const dtLink* link = &tile->links[i]; + if ((int)link->edge == segMax) + { + // If the link is internal, just return the ref. + if (link->side == 0xff) + { + nextRef = link->ref; + break; + } + + // If the link is at tile boundary, + const int v0 = poly->verts[link->edge]; + const int v1 = poly->verts[(link->edge+1) % poly->vertCount]; + const float* left = &tile->verts[v0*3]; + const float* right = &tile->verts[v1*3]; + + // Check that the intersection lies inside the link portal. + if (link->side == 0 || link->side == 4) + { + // Calculate link size. + const float smin = min(left[2],right[2]); + const float smax = max(left[2],right[2]); + const float s = (smax-smin) / 255.0f; + const float lmin = smin + link->bmin*s; + const float lmax = smin + link->bmax*s; + // Find Z intersection. + float z = startPos[2] + (endPos[2]-startPos[2])*tmax; + if (z >= lmin && z <= lmax) + { + nextRef = link->ref; + break; + } + } + else if (link->side == 2 || link->side == 6) + { + // Calculate link size. + const float smin = min(left[0],right[0]); + const float smax = max(left[0],right[0]); + const float s = (smax-smin) / 255.0f; + const float lmin = smin + link->bmin*s; + const float lmax = smin + link->bmax*s; + // Find X intersection. + float x = startPos[0] + (endPos[0]-startPos[0])*tmax; + if (x >= lmin && x <= lmax) + { + nextRef = link->ref; + break; + } + } + } + } + + if (!nextRef || !passFilter(filter, getPolyFlags(nextRef))) + { + // No neighbour, we hit a wall. + + // Calculate hit normal. + const int a = segMax; + const int b = segMax+1 < nv ? segMax+1 : 0; + const float* va = &verts[a*3]; + const float* vb = &verts[b*3]; + const float dx = vb[0] - va[0]; + const float dz = vb[2] - va[2]; + hitNormal[0] = dz; + hitNormal[1] = 0; + hitNormal[2] = -dx; + vnormalize(hitNormal); + + return n; + } + + // No hit, advance to neighbour polygon. + curRef = nextRef; + } + + return n; +} + +int dtNavMesh::findPolysAround(dtPolyRef centerRef, const float* centerPos, float radius, dtQueryFilter* filter, + dtPolyRef* resultRef, dtPolyRef* resultParent, float* resultCost, + const int maxResult) +{ + if (!centerRef) return 0; + if (!getPolyByRef(centerRef)) return 0; + if (!m_nodePool || !m_openList) return 0; + + m_nodePool->clear(); + m_openList->clear(); + + dtNode* startNode = m_nodePool->getNode(centerRef); + startNode->pidx = 0; + startNode->cost = 0; + startNode->total = 0; + startNode->id = centerRef; + startNode->flags = DT_NODE_OPEN; + m_openList->push(startNode); + + int n = 0; + if (n < maxResult) + { + if (resultRef) + resultRef[n] = startNode->id; + if (resultParent) + resultParent[n] = 0; + if (resultCost) + resultCost[n] = 0; + ++n; + } + + const float radiusSqr = sqr(radius); + + unsigned int it, ip; + + while (!m_openList->empty()) + { + dtNode* bestNode = m_openList->pop(); + + float previousEdgeMidPoint[3]; + + // Get poly and tile. + // The API input has been cheked already, skip checking internal data. + const dtPolyRef bestRef = bestNode->id; + it = decodePolyIdTile(bestRef); + ip = decodePolyIdPoly(bestRef); + const dtMeshTile* bestTile = &m_tiles[it]; + const dtPoly* bestPoly = &bestTile->polys[ip]; + + // 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) + { + it = decodePolyIdTile(parentRef); + ip = decodePolyIdPoly(parentRef); + parentTile = &m_tiles[it]; + parentPoly = &parentTile->polys[ip]; + + getEdgeMidPoint(parentRef, parentPoly, parentTile, + bestRef, bestPoly, bestTile, previousEdgeMidPoint); + } + else + { + vcopy(previousEdgeMidPoint, centerPos); + } + + 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; + + // Calc distance to the edge. + const float* va = &bestTile->verts[bestPoly->verts[link->edge]*3]; + const float* vb = &bestTile->verts[bestPoly->verts[(link->edge+1) % bestPoly->vertCount]*3]; + float tseg; + float distSqr = distancePtSegSqr2D(centerPos, va, vb, tseg); + + // If the circle is not touching the next polygon, skip it. + if (distSqr > radiusSqr) + continue; + + // Expand to neighbour + it = decodePolyIdTile(neighbourRef); + ip = decodePolyIdPoly(neighbourRef); + const dtMeshTile* neighbourTile = &m_tiles[it]; + const dtPoly* neighbourPoly = &neighbourTile->polys[ip]; + + if (!passFilter(filter, neighbourPoly->flags)) + continue; + + dtNode newNode; + newNode.pidx = m_nodePool->getNodeIdx(bestNode); + newNode.id = neighbourRef; + + // Cost + float edgeMidPoint[3]; + getEdgeMidPoint(bestRef, bestPoly, bestTile, + neighbourRef, neighbourPoly, neighbourTile, edgeMidPoint); + + newNode.total = bestNode->total + vdist(previousEdgeMidPoint, edgeMidPoint); + + dtNode* actualNode = m_nodePool->getNode(newNode.id); + if (!actualNode) + continue; + + if (!((actualNode->flags & DT_NODE_OPEN) && newNode.total > actualNode->total) && + !((actualNode->flags & DT_NODE_CLOSED) && newNode.total > actualNode->total)) + { + actualNode->flags &= ~DT_NODE_CLOSED; + actualNode->pidx = newNode.pidx; + actualNode->total = newNode.total; + + if (actualNode->flags & DT_NODE_OPEN) + { + m_openList->modify(actualNode); + } + else + { + if (n < maxResult) + { + if (resultRef) + resultRef[n] = actualNode->id; + if (resultParent) + resultParent[n] = m_nodePool->getNodeAtIdx(actualNode->pidx)->id; + if (resultCost) + resultCost[n] = actualNode->total; + ++n; + } + actualNode->flags = DT_NODE_OPEN; + m_openList->push(actualNode); + } + } + } + } + + return n; +} + +float dtNavMesh::findDistanceToWall(dtPolyRef centerRef, const float* centerPos, float maxRadius, dtQueryFilter* filter, + float* hitPos, float* hitNormal) +{ + if (!centerRef) return 0; + if (!getPolyByRef(centerRef)) return 0; + if (!m_nodePool || !m_openList) return 0; + + m_nodePool->clear(); + m_openList->clear(); + + dtNode* startNode = m_nodePool->getNode(centerRef); + startNode->pidx = 0; + startNode->cost = 0; + startNode->total = 0; + startNode->id = centerRef; + startNode->flags = DT_NODE_OPEN; + m_openList->push(startNode); + + float radiusSqr = sqr(maxRadius); + + unsigned int it, ip; + + while (!m_openList->empty()) + { + dtNode* bestNode = m_openList->pop(); + + float previousEdgeMidPoint[3]; + + // Get poly and tile. + // The API input has been cheked already, skip checking internal data. + const dtPolyRef bestRef = bestNode->id; + it = decodePolyIdTile(bestRef); + ip = decodePolyIdPoly(bestRef); + const dtMeshTile* bestTile = &m_tiles[it]; + const dtPoly* bestPoly = &bestTile->polys[ip]; + + // 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) + { + it = decodePolyIdTile(parentRef); + ip = decodePolyIdPoly(parentRef); + parentTile = &m_tiles[it]; + parentPoly = &parentTile->polys[ip]; + + getEdgeMidPoint(parentRef, parentPoly, parentTile, + bestRef, bestPoly, bestTile, previousEdgeMidPoint); + } + else + { + vcopy(previousEdgeMidPoint, centerPos); + } + + // Hit test walls. + for (int i = 0, j = (int)bestPoly->vertCount-1; i < (int)bestPoly->vertCount; j = i++) + { + // Skip non-solid edges. + if (bestPoly->neis[j] & DT_EXT_LINK) + { + // Tile border. + bool solid = true; + for (unsigned int k = bestPoly->firstLink; k != DT_NULL_LINK; k = bestTile->links[k].next) + { + const dtLink* link = &bestTile->links[k]; + if (link->edge == j) + { + if (link->ref != 0 && passFilter(filter, getPolyFlags(link->ref))) + solid = false; + break; + } + } + if (!solid) continue; + } + else if (bestPoly->neis[j] && passFilter(filter, bestTile->polys[bestPoly->neis[j]].flags)) + { + // Internal edge + continue; + } + + // Calc distance to the edge. + const float* vj = &bestTile->verts[bestPoly->verts[j]*3]; + const float* vi = &bestTile->verts[bestPoly->verts[i]*3]; + float tseg; + float distSqr = distancePtSegSqr2D(centerPos, vj, vi, tseg); + + // Edge is too far, skip. + if (distSqr > radiusSqr) + continue; + + // Hit wall, update radius. + radiusSqr = distSqr; + // Calculate hit pos. + hitPos[0] = vj[0] + (vi[0] - vj[0])*tseg; + hitPos[1] = vj[1] + (vi[1] - vj[1])*tseg; + hitPos[2] = vj[2] + (vi[2] - vj[2])*tseg; + } + + for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next) + { + const dtLink* link = &bestTile->links[i]; + dtPolyRef neighbourRef = link->ref; + // Skip invalid neighbours and do not follow back to parent. + if (!neighbourRef || neighbourRef == parentRef) + continue; + + // Calc distance to the edge. + const float* va = &bestTile->verts[bestPoly->verts[link->edge]*3]; + const float* vb = &bestTile->verts[bestPoly->verts[(link->edge+1) % bestPoly->vertCount]*3]; + float tseg; + float distSqr = distancePtSegSqr2D(centerPos, va, vb, tseg); + + // If the circle is not touching the next polygon, skip it. + if (distSqr > radiusSqr) + continue; + + // Expand to neighbour. + it = decodePolyIdTile(neighbourRef); + ip = decodePolyIdPoly(neighbourRef); + const dtMeshTile* neighbourTile = &m_tiles[it]; + const dtPoly* neighbourPoly = &neighbourTile->polys[ip]; + + if (!passFilter(filter, neighbourPoly->flags)) + continue; + + dtNode newNode; + newNode.pidx = m_nodePool->getNodeIdx(bestNode); + newNode.id = neighbourRef; + + // Cost + float edgeMidPoint[3]; + getEdgeMidPoint(bestRef, bestPoly, bestTile, + neighbourRef, neighbourPoly, neighbourTile, edgeMidPoint); + + newNode.total = bestNode->total + vdist(previousEdgeMidPoint, edgeMidPoint); + + dtNode* actualNode = m_nodePool->getNode(newNode.id); + if (!actualNode) + continue; + + if (!((actualNode->flags & DT_NODE_OPEN) && newNode.total > actualNode->total) && + !((actualNode->flags & DT_NODE_CLOSED) && newNode.total > actualNode->total)) + { + actualNode->flags &= ~DT_NODE_CLOSED; + actualNode->pidx = newNode.pidx; + actualNode->total = newNode.total; + + if (actualNode->flags & DT_NODE_OPEN) + { + m_openList->modify(actualNode); + } + else + { + actualNode->flags = DT_NODE_OPEN; + m_openList->push(actualNode); + } + } + } + } + + // Calc hit normal. + vsub(hitNormal, centerPos, hitPos); + vnormalize(hitNormal); + + return sqrtf(radiusSqr); +} + +const dtPoly* dtNavMesh::getPolyByRef(dtPolyRef ref) const +{ + unsigned int salt, it, ip; + decodePolyId(ref, salt, it, ip); + if (it >= (unsigned int)m_maxTiles) return 0; + if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return 0; + if (ip >= (unsigned int)m_tiles[it].header->polyCount) return 0; + return &m_tiles[it].polys[ip]; +} + +const float* dtNavMesh::getPolyVertsByRef(dtPolyRef ref) const +{ + unsigned int salt, it, ip; + decodePolyId(ref, salt, it, ip); + if (it >= (unsigned int)m_maxTiles) return 0; + if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return 0; + if (ip >= (unsigned int)m_tiles[it].header->polyCount) return 0; + return m_tiles[it].verts; +} + +const dtLink* dtNavMesh::getPolyLinksByRef(dtPolyRef ref) const +{ + unsigned int salt, it, ip; + decodePolyId(ref, salt, it, ip); + if (it >= (unsigned int)m_maxTiles) return 0; + if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return 0; + if (ip >= (unsigned int)m_tiles[it].header->polyCount) return 0; + return m_tiles[it].links; +} + +bool dtNavMesh::isInClosedList(dtPolyRef ref) const +{ + if (!m_nodePool) return false; + const dtNode* node = m_nodePool->findNode(ref); + return node && node->flags & DT_NODE_CLOSED; +} diff --git a/src/shared/pathfinding/Detour/DetourNavMesh.h b/src/shared/pathfinding/Detour/DetourNavMesh.h new file mode 100644 index 0000000..8dd638c --- /dev/null +++ b/src/shared/pathfinding/Detour/DetourNavMesh.h @@ -0,0 +1,497 @@ +// +// Copyright (c) 2009 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 + +// Reference to navigation polygon. +typedef unsigned int dtPolyRef; + +// 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 = 3; + +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; + +// 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. +}; + +struct dtQueryFilter +{ + dtQueryFilter() : includeFlags(0xffff), excludeFlags(0) {} + unsigned short includeFlags; // If any of the flags are set, the poly is included. + unsigned short excludeFlags; // If any of the flags are set, the poly is excluded. +}; + +// 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 area : 6; // Area ID of the polygon. + unsigned char type : 2; // Polygon type, see dtPolyTypes. +}; + +// Stucture describing polygon detail triangles. +struct dtPolyDetail +{ + unsigned short vertBase; // Offset to detail vertex array. + unsigned short vertCount; // Number of vertices in the detail mesh. + unsigned short triBase; // Offset to detail triangle array. + unsigned short 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. +}; + +struct dtMeshHeader +{ + int magic; // Magic number, used to identify the data. + int version; // Data version number. + 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. + int x,y; // Grid location of 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. + bool ownsData; // Flag indicating of the navmesh should release the data. + dtMeshTile* next; // Next free tile or, next tile in spatial grid. +}; + + +class dtNavMesh +{ +public: + dtNavMesh(); + ~dtNavMesh(); + + // Initializes the nav mesh for tiled use. + // Params: + // orig[3] - (in) origin of the nav mesh tile space. + // tileWidth - (in) width of each tile. + // tileHeight - (in) height of each tile. + // maxTiles - (in) maximum number of tiles the navmesh can contain*. + // maxPolys - (in) maximum number of polygons each tile can contain*. + // maxNodes - (in) maximum number of A* nodes to use*. + // *) Will be rounded to next power of two. + // Returns: True if succeed, else false. + bool init(const float* orig, float tileWidth, float tileHeight, + int maxTiles, int maxPolys, int maxNodes); + + // 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. + // ownsData - (in) Flag indicating if the navmesh should own and delete the data. + // maxNodes - (in) maximum number of A* nodes to use*. + // *) Will be rounded to next power of two. + // Returns: True if succeed, else false. + bool init(unsigned char* data, int dataSize, bool ownsData, int maxNodes); + + // 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: + // x,y - (in) Location of the new tile. + // data - (in) Data of the new tile mesh. + // dataSize - (in) Data size of the new tile mesh. + // ownsData - (in) Flag indicating if the navmesh should own and delete the data. + // Returns: True if tile was added, else false. + bool addTileAt(int x, int y, unsigned char* data, int dataSize, bool ownsData); + + // Removes tile at specified location. + // Params: + // x,y - (in) Location of the tile to remove. + // data - (out) Data associated with deleted tile. + // dataSize - (out) Size of the data associated with deleted tile. + // Returns: True if remove suceed, else false. + bool removeTileAt(int x, int y, unsigned char** data, int* dataSize); + + // 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. + dtMeshTile* getTileAt(int x, int y); + + // 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. + dtMeshTile* getTile(int i); + const dtMeshTile* getTile(int i) const; + + // Returns pointer to tile in the tile array. + // Params: + // ref - (in) reference to a polygon inside the tile. + // plyIndex - (out,optional) pointer to value where polygon index within the tile is stored. + // Returns: Pointer to specified tile. + const dtMeshTile* getTileByRef(dtPolyRef ref, int* polyIndex) const; + + // Returns base id for the tile. + dtPolyRef getTileId(const dtMeshTile* tile) const; + + // 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. + // 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. + dtPolyRef findNearestPoly(const float* center, const float* extents, dtQueryFilter* filter, float* nearestPt); + + // Returns polygons which touch 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. + // maxPolys - (in) The max number of polygons the polys array can hold. + // Returns: Number of polygons in search result array. + int queryPolygons(const float* center, const float* extents, dtQueryFilter* filter, + dtPolyRef* polys, const int maxPolys); + + // 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. + // maxPathSize - (in) The max number of polygons the path array can hold. + // Returns: Number of polygons in search result array. + int findPath(dtPolyRef startRef, dtPolyRef endRef, + const float* startPos, const float* endPos, + dtQueryFilter* filter, + dtPolyRef* path, const int maxPathSize); + + // 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. + // maxStraightPathSize - (in) The max number of points the straight path array can hold. + // Returns: Number of points in the path. + int findStraightPath(const float* startPos, const float* endPos, + const dtPolyRef* path, const int pathSize, + float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs, + const int maxStraightPathSize); + + // Moves towards end position a long the path corridor. + // The start location is assumed to be roughly at inside the first polygon on the path. + // The return value can be used to advance the path pointer along the path. + // Params: + // startPos[3] - (in) current position of the agent. + // endPos[3] - (in) new position of the agent. + // resultPos[3] - (out) new positio after the move, constrained to be inside the path polygons. + // path - (in) remainder of the path to follow. + // pathSize - (in) number of polygons on the path. + // Returns: Index to the path polygon where the result position lies. + int moveAlongPathCorridor(const float* startPos, const float* endPos, float* resultPos, + const dtPolyRef* path, const int pathSize); + + // Finds intersection againts walls starting from start pos. + // 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, 0 if no hit. + // hitNormal[3] - (out) normal of the nearest hit. + // filter - (in) path polygon filter. + // path - (out) visited path polygons. + // pathSize - (in) max number of polygons in the path array. + // Returns: Number of polygons visited or 0 if failed. + int raycast(dtPolyRef startRef, const float* startPos, const float* endPos, dtQueryFilter* filter, + float& t, float* hitNormal, dtPolyRef* path, const int pathSize); + + // Returns distance to nearest wall from the specified location. + // Params: + // centerRef - (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. + // hitPos[3] - (out) location of the nearest hit. + // hitNormal[3] - (out) normal of the nearest hit. + // Returns: Distance to nearest wall from the test location. + float findDistanceToWall(dtPolyRef centerRef, const float* centerPos, float maxRadius, + dtQueryFilter* filter, float* hitPos, float* hitNormal); + + // Finds polygons found along the navigation graph which touch the specified circle. + // Params: + // centerRef - (in) ref to the polygon where the center lies. + // 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. + // maxResult - (int) maximum capacity of search results. + // Returns: Number of results. + int findPolysAround(dtPolyRef centerRef, const float* centerPos, float radius, dtQueryFilter* filter, + dtPolyRef* resultRef, dtPolyRef* resultParent, float* resultCost, + const int maxResult); + + // 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. + bool 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. + bool 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. + bool 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. + bool getPolyHeight(dtPolyRef ref, const float* pos, float* height) const; + + // Sets the pathfinding cost of the specified area. + // Params: + // area - (in) area ID (0-63). + // cost - (int) travel cost of the area. + void setAreaCost(const int area, float cost); + + // Returns the pathfinding cost of the specified area. + // Params: + // area - (in) area ID (0-63). + float getAreaCost(const int area) const; + + // Sets polygon flags. + void setPolyFlags(dtPolyRef ref, unsigned short flags); + + // Return polygon flags. + unsigned short getPolyFlags(dtPolyRef ref) const; + + // Set polygon type. + void setPolyArea(dtPolyRef ref, unsigned char area); + + // Return polygon type. + unsigned char getPolyArea(dtPolyRef ref) const; + + // Returns pointer to a polygon based on ref. + const dtPoly* getPolyByRef(dtPolyRef ref) const; + + // Returns pointer to a polygon vertices based on ref. + const float* getPolyVertsByRef(dtPolyRef ref) const; + + // Returns pointer to a polygon link based on ref. + const dtLink* getPolyLinksByRef(dtPolyRef ref) const; + + // Returns true if poly reference ins in closed list. + bool isInClosedList(dtPolyRef ref) const; + + // Encodes a tile id. + inline dtPolyRef encodePolyId(unsigned int salt, unsigned int it, unsigned int ip) const + { + return (salt << (m_polyBits+m_tileBits)) | ((it+1) << m_polyBits) | ip; + } + + // Decodes a tile id. + inline void decodePolyId(dtPolyRef ref, unsigned int& salt, unsigned int& it, unsigned int& ip) const + { + salt = (ref >> (m_polyBits+m_tileBits)) & ((1<> m_polyBits) - 1) & ((1<> m_polyBits) - 1) & ((1< +#include +#include +#include +#include "DetourNavMesh.h" +#include "DetourCommon.h" +#include "DetourNavMeshBuilder.h" + +static unsigned short MESH_NULL_IDX = 0xffff; + + +struct BVItem +{ + unsigned short bmin[3]; + unsigned short bmax[3]; + int i; +}; + +static int compareItemX(const void* va, const void* vb) +{ + const BVItem* a = (const BVItem*)va; + const BVItem* b = (const BVItem*)vb; + if (a->bmin[0] < b->bmin[0]) + return -1; + if (a->bmin[0] > b->bmin[0]) + return 1; + return 0; +} + +static int compareItemY(const void* va, const void* vb) +{ + const BVItem* a = (const BVItem*)va; + const BVItem* b = (const BVItem*)vb; + if (a->bmin[1] < b->bmin[1]) + return -1; + if (a->bmin[1] > b->bmin[1]) + return 1; + return 0; +} + +static int compareItemZ(const void* va, const void* vb) +{ + const BVItem* a = (const BVItem*)va; + const BVItem* b = (const BVItem*)vb; + if (a->bmin[2] < b->bmin[2]) + return -1; + if (a->bmin[2] > b->bmin[2]) + return 1; + return 0; +} + +static void calcExtends(BVItem* items, int nitems, int imin, int imax, + unsigned short* bmin, unsigned short* bmax) +{ + bmin[0] = items[imin].bmin[0]; + bmin[1] = items[imin].bmin[1]; + bmin[2] = items[imin].bmin[2]; + + bmax[0] = items[imin].bmax[0]; + bmax[1] = items[imin].bmax[1]; + bmax[2] = items[imin].bmax[2]; + + for (int i = imin+1; i < imax; ++i) + { + const BVItem& it = items[i]; + if (it.bmin[0] < bmin[0]) bmin[0] = it.bmin[0]; + if (it.bmin[1] < bmin[1]) bmin[1] = it.bmin[1]; + if (it.bmin[2] < bmin[2]) bmin[2] = it.bmin[2]; + + if (it.bmax[0] > bmax[0]) bmax[0] = it.bmax[0]; + if (it.bmax[1] > bmax[1]) bmax[1] = it.bmax[1]; + if (it.bmax[2] > bmax[2]) bmax[2] = it.bmax[2]; + } +} + +inline int longestAxis(unsigned short x, unsigned short y, unsigned short z) +{ + int axis = 0; + unsigned short maxVal = x; + if (y > maxVal) + { + axis = 1; + maxVal = y; + } + if (z > maxVal) + { + axis = 2; + maxVal = z; + } + return axis; +} + +static void subdivide(BVItem* items, int nitems, int imin, int imax, int& curNode, dtBVNode* nodes) +{ + int inum = imax - imin; + int icur = curNode; + + dtBVNode& node = nodes[curNode++]; + + if (inum == 1) + { + // Leaf + node.bmin[0] = items[imin].bmin[0]; + node.bmin[1] = items[imin].bmin[1]; + node.bmin[2] = items[imin].bmin[2]; + + node.bmax[0] = items[imin].bmax[0]; + node.bmax[1] = items[imin].bmax[1]; + node.bmax[2] = items[imin].bmax[2]; + + node.i = items[imin].i; + } + else + { + // Split + calcExtends(items, nitems, imin, imax, node.bmin, node.bmax); + + int axis = longestAxis(node.bmax[0] - node.bmin[0], + node.bmax[1] - node.bmin[1], + node.bmax[2] - node.bmin[2]); + + if (axis == 0) + { + // Sort along x-axis + qsort(items+imin, inum, sizeof(BVItem), compareItemX); + } + else if (axis == 1) + { + // Sort along y-axis + qsort(items+imin, inum, sizeof(BVItem), compareItemY); + } + else + { + // Sort along z-axis + qsort(items+imin, inum, sizeof(BVItem), compareItemZ); + } + + int isplit = imin+inum/2; + + // Left + subdivide(items, nitems, imin, isplit, curNode, nodes); + // Right + subdivide(items, nitems, isplit, imax, curNode, nodes); + + int iescape = curNode - icur; + // Negative index means escape. + node.i = -iescape; + } +} + +static int createBVTree(const unsigned short* verts, const int nverts, + const unsigned short* polys, const int npolys, const int nvp, + float cs, float ch, + int nnodes, dtBVNode* nodes) +{ + // Build tree + BVItem* items = new BVItem[npolys]; + for (int i = 0; i < npolys; i++) + { + BVItem& it = items[i]; + it.i = i; + // Calc polygon bounds. + const unsigned short* p = &polys[i*nvp*2]; + it.bmin[0] = it.bmax[0] = verts[p[0]*3+0]; + it.bmin[1] = it.bmax[1] = verts[p[0]*3+1]; + it.bmin[2] = it.bmax[2] = verts[p[0]*3+2]; + + for (int j = 1; j < nvp; ++j) + { + if (p[j] == MESH_NULL_IDX) break; + unsigned short x = verts[p[j]*3+0]; + unsigned short y = verts[p[j]*3+1]; + unsigned short z = verts[p[j]*3+2]; + + if (x < it.bmin[0]) it.bmin[0] = x; + if (y < it.bmin[1]) it.bmin[1] = y; + if (z < it.bmin[2]) it.bmin[2] = z; + + if (x > it.bmax[0]) it.bmax[0] = x; + if (y > it.bmax[1]) it.bmax[1] = y; + if (z > it.bmax[2]) it.bmax[2] = z; + } + // Remap y + it.bmin[1] = (unsigned short)floorf((float)it.bmin[1]*ch/cs); + it.bmax[1] = (unsigned short)ceilf((float)it.bmax[1]*ch/cs); + } + + int curNode = 0; + subdivide(items, npolys, 0, npolys, curNode, nodes); + + delete [] items; + + return curNode; +} + +static unsigned char classifyOffMeshPoint(const float* pt, const float* bmin, const float* bmax) +{ + static const unsigned char XP = 1<<0; + static const unsigned char ZP = 1<<1; + static const unsigned char XM = 1<<2; + static const unsigned char ZM = 1<<3; + + unsigned char outcode = 0; + outcode |= (pt[0] >= bmax[0]) ? XP : 0; + outcode |= (pt[2] >= bmax[2]) ? ZP : 0; + outcode |= (pt[0] < bmin[0]) ? XM : 0; + outcode |= (pt[2] < bmin[2]) ? ZM : 0; + + switch (outcode) + { + case XP: return 0; + case XP|ZP: return 1; + case ZP: return 2; + case XM|ZP: return 3; + case XM: return 4; + case XM|ZM: return 5; + case ZM: return 6; + case XP|ZM: return 7; + }; + return 0xff; +} + +// TODO: Better error handling. + +bool dtCreateNavMeshData(dtNavMeshCreateParams* params, unsigned char** outData, int* outDataSize) +{ + if (params->nvp > DT_VERTS_PER_POLYGON) + return false; + if (params->vertCount >= 0xffff) + return false; + if (!params->vertCount || !params->verts) + return false; + if (!params->polyCount || !params->polys) + return false; + if (!params->detailMeshes || !params->detailVerts || !params->detailTris) + return false; + + const int nvp = params->nvp; + + // Classify off-mesh connection points. We store only the connections + // whose start point is inside the tile. + unsigned char* offMeshConClass = new unsigned char [params->offMeshConCount*2]; + if (!offMeshConClass) + return false; + + int storedOffMeshConCount = 0; + int offMeshConLinkCount = 0; + + for (int i = 0; i < params->offMeshConCount; ++i) + { + offMeshConClass[i*2+0] = classifyOffMeshPoint(¶ms->offMeshConVerts[(i*2+0)*3], params->bmin, params->bmax); + offMeshConClass[i*2+1] = classifyOffMeshPoint(¶ms->offMeshConVerts[(i*2+1)*3], params->bmin, params->bmax); + + // Cound how many links should be allocated for off-mesh connections. + if (offMeshConClass[i*2+0] == 0xff) + offMeshConLinkCount++; + if (offMeshConClass[i*2+1] == 0xff) + offMeshConLinkCount++; + + if (offMeshConClass[i*2+0] == 0xff) + storedOffMeshConCount++; + } + + // Off-mesh connectionss are stored as polygons, adjust values. + const int totPolyCount = params->polyCount + storedOffMeshConCount; + const int totVertCount = params->vertCount + storedOffMeshConCount*2; + + // Find portal edges which are at tile borders. + int edgeCount = 0; + int portalCount = 0; + for (int i = 0; i < params->polyCount; ++i) + { + const unsigned short* p = ¶ms->polys[i*2*nvp]; + for (int j = 0; j < nvp; ++j) + { + if (p[j] == MESH_NULL_IDX) break; + int nj = j+1; + if (nj >= nvp || p[nj] == MESH_NULL_IDX) nj = 0; + const unsigned short* va = ¶ms->verts[p[j]*3]; + const unsigned short* vb = ¶ms->verts[p[nj]*3]; + + edgeCount++; + + if (params->tileSize > 0) + { + if (va[0] == params->tileSize && vb[0] == params->tileSize) + portalCount++; // x+ + else if (va[2] == params->tileSize && vb[2] == params->tileSize) + portalCount++; // z+ + else if (va[0] == 0 && vb[0] == 0) + portalCount++; // x- + else if (va[2] == 0 && vb[2] == 0) + portalCount++; // z- + } + } + } + + const int maxLinkCount = edgeCount + portalCount*2 + offMeshConLinkCount*2; + + // Find unique detail vertices. + int uniqueDetailVertCount = 0; + for (int i = 0; i < params->polyCount; ++i) + { + const unsigned short* p = ¶ms->polys[i*nvp*2]; + int ndv = params->detailMeshes[i*4+1]; + int nv = 0; + for (int j = 0; j < nvp; ++j) + { + if (p[j] == MESH_NULL_IDX) break; + nv++; + } + ndv -= nv; + uniqueDetailVertCount += ndv; + } + + // Calculate data size + const int headerSize = align4(sizeof(dtMeshHeader)); + const int vertsSize = align4(sizeof(float)*3*totVertCount); + const int polysSize = align4(sizeof(dtPoly)*totPolyCount); + const int linksSize = align4(sizeof(dtLink)*maxLinkCount); + const int detailMeshesSize = align4(sizeof(dtPolyDetail)*params->polyCount); + const int detailVertsSize = align4(sizeof(float)*3*uniqueDetailVertCount); + const int detailTrisSize = align4(sizeof(unsigned char)*4*params->detailTriCount); + const int bvTreeSize = align4(sizeof(dtBVNode)*params->polyCount*2); + const int offMeshConsSize = align4(sizeof(dtOffMeshConnection)*storedOffMeshConCount); + + const int dataSize = headerSize + vertsSize + polysSize + linksSize + + detailMeshesSize + detailVertsSize + detailTrisSize + + bvTreeSize + offMeshConsSize; + + unsigned char* data = new unsigned char[dataSize]; + if (!data) + return false; + memset(data, 0, dataSize); + + unsigned char* d = data; + dtMeshHeader* header = (dtMeshHeader*)d; d += headerSize; + float* navVerts = (float*)d; d += vertsSize; + dtPoly* navPolys = (dtPoly*)d; d += polysSize; + d += linksSize; + dtPolyDetail* navDMeshes = (dtPolyDetail*)d; d += detailMeshesSize; + float* navDVerts = (float*)d; d += detailVertsSize; + unsigned char* navDTris = (unsigned char*)d; d += detailTrisSize; + dtBVNode* navBvtree = (dtBVNode*)d; d += bvTreeSize; + dtOffMeshConnection* offMeshCons = (dtOffMeshConnection*)d; d += offMeshConsSize; + + + // Store header + header->magic = DT_NAVMESH_MAGIC; + header->version = DT_NAVMESH_VERSION; + header->polyCount = totPolyCount; + header->vertCount = totVertCount; + header->maxLinkCount = maxLinkCount; + vcopy(header->bmin, params->bmin); + vcopy(header->bmax, params->bmax); + header->detailMeshCount = params->polyCount; + header->detailVertCount = uniqueDetailVertCount; + header->detailTriCount = params->detailTriCount; + header->bvQuantFactor = 1.0f / params->cs; + header->offMeshBase = params->polyCount; + header->walkableHeight = params->walkableHeight; + header->walkableRadius = params->walkableRadius; + header->walkableClimb = params->walkableClimb; + header->offMeshConCount = storedOffMeshConCount; + header->bvNodeCount = params->polyCount*2; + + const int offMeshVertsBase = params->vertCount; + const int offMeshPolyBase = params->polyCount; + + // Store vertices + // Mesh vertices + for (int i = 0; i < params->vertCount; ++i) + { + const unsigned short* iv = ¶ms->verts[i*3]; + float* v = &navVerts[i*3]; + v[0] = params->bmin[0] + iv[0] * params->cs; + v[1] = params->bmin[1] + iv[1] * params->ch; + v[2] = params->bmin[2] + iv[2] * params->cs; + } + // Off-mesh link vertices. + int n = 0; + for (int i = 0; i < params->offMeshConCount; ++i) + { + // Only store connections which start from this tile. + if (offMeshConClass[i*2+0] == 0xff) + { + const float* linkv = ¶ms->offMeshConVerts[i*2*3]; + float* v = &navVerts[(offMeshVertsBase + n*2)*3]; + vcopy(&v[0], &linkv[0]); + vcopy(&v[3], &linkv[3]); + n++; + } + } + + // Store polygons + // Mesh polys + const unsigned short* src = params->polys; + for (int i = 0; i < params->polyCount; ++i) + { + dtPoly* p = &navPolys[i]; + p->vertCount = 0; + p->flags = params->polyFlags[i]; + p->area = params->polyAreas[i]; + p->type = DT_POLYTYPE_GROUND; + for (int j = 0; j < nvp; ++j) + { + if (src[j] == MESH_NULL_IDX) break; + p->verts[j] = src[j]; + p->neis[j] = (src[nvp+j]+1) & 0xffff; + p->vertCount++; + } + src += nvp*2; + } + // Off-mesh connection vertices. + n = 0; + for (int i = 0; i < params->offMeshConCount; ++i) + { + // Only store connections which start from this tile. + if (offMeshConClass[i*2+0] == 0xff) + { + dtPoly* p = &navPolys[offMeshPolyBase+n]; + p->vertCount = 2; + p->verts[0] = (unsigned short)(offMeshVertsBase + n*2+0); + p->verts[1] = (unsigned short)(offMeshVertsBase + n*2+1); + p->flags = params->offMeshConFlags[i]; + p->area = params->offMeshConAreas[i]; + p->type = DT_POLYTYPE_OFFMESH_CONNECTION; + n++; + } + } + + // Store portal edges. + if (params->tileSize > 0) + { + for (int i = 0; i < params->polyCount; ++i) + { + dtPoly* poly = &navPolys[i]; + for (int j = 0; j < poly->vertCount; ++j) + { + int nj = j+1; + if (nj >= poly->vertCount) nj = 0; + + const unsigned short* va = ¶ms->verts[poly->verts[j]*3]; + const unsigned short* vb = ¶ms->verts[poly->verts[nj]*3]; + + if (va[0] == params->tileSize && vb[0] == params->tileSize) // x+ + poly->neis[j] = DT_EXT_LINK | 0; + else if (va[2] == params->tileSize && vb[2] == params->tileSize) // z+ + poly->neis[j] = DT_EXT_LINK | 2; + else if (va[0] == 0 && vb[0] == 0) // x- + poly->neis[j] = DT_EXT_LINK | 4; + else if (va[2] == 0 && vb[2] == 0) // z- + poly->neis[j] = DT_EXT_LINK | 6; + } + } + } + + // Store detail meshes and vertices. + // The nav polygon vertices are stored as the first vertices on each mesh. + // We compress the mesh data by skipping them and using the navmesh coordinates. + unsigned short vbase = 0; + for (int i = 0; i < params->polyCount; ++i) + { + dtPolyDetail& dtl = navDMeshes[i]; + const int vb = params->detailMeshes[i*4+0]; + const int ndv = params->detailMeshes[i*4+1]; + const int nv = navPolys[i].vertCount; + dtl.vertBase = vbase; + dtl.vertCount = ndv-nv; + dtl.triBase = params->detailMeshes[i*4+2]; + dtl.triCount = params->detailMeshes[i*4+3]; + // Copy vertices except the first 'nv' verts which are equal to nav poly verts. + if (ndv-nv) + { + memcpy(&navDVerts[vbase*3], ¶ms->detailVerts[(vb+nv)*3], sizeof(float)*3*(ndv-nv)); + vbase += ndv-nv; + } + } + // Store triangles. + memcpy(navDTris, params->detailTris, sizeof(unsigned char)*4*params->detailTriCount); + + // Store and create BVtree. + // TODO: take detail mesh into account! use byte per bbox extent? + createBVTree(params->verts, params->vertCount, params->polys, params->polyCount, + nvp, params->cs, params->ch, params->polyCount*2, navBvtree); + + // Store Off-Mesh connections. + n = 0; + for (int i = 0; i < params->offMeshConCount; ++i) + { + // Only store connections which start from this tile. + if (offMeshConClass[i*2+0] == 0xff) + { + dtOffMeshConnection* con = &offMeshCons[n]; + con->poly = offMeshPolyBase + n; + // Copy connection end-points. + const float* endPts = ¶ms->offMeshConVerts[i*2*3]; + vcopy(&con->pos[0], &endPts[0]); + vcopy(&con->pos[3], &endPts[3]); + con->rad = params->offMeshConRad[i]; + con->flags = params->offMeshConDir[i] ? DT_OFFMESH_CON_BIDIR : 0; + con->side = offMeshConClass[i*2+1]; + n++; + } + } + + delete [] offMeshConClass; + + *outData = data; + *outDataSize = dataSize; + + return true; +} + + + + +inline void swapByte(unsigned char* a, unsigned char* b) +{ + unsigned char tmp = *a; + *a = *b; + *b = tmp; +} + +inline void swapEndian(unsigned short* v) +{ + unsigned char* x = (unsigned char*)v; + swapByte(x+0, x+1); +} + +inline void swapEndian(short* v) +{ + unsigned char* x = (unsigned char*)v; + swapByte(x+0, x+1); +} + +inline void swapEndian(unsigned int* v) +{ + unsigned char* x = (unsigned char*)v; + swapByte(x+0, x+3); swapByte(x+1, x+2); +} + +inline void swapEndian(int* v) +{ + unsigned char* x = (unsigned char*)v; + swapByte(x+0, x+3); swapByte(x+1, x+2); +} + +inline void swapEndian(float* v) +{ + unsigned char* x = (unsigned char*)v; + swapByte(x+0, x+3); swapByte(x+1, x+2); +} + +bool dtNavMeshHeaderSwapEndian(unsigned char* data, const int dataSize) +{ + dtMeshHeader* header = (dtMeshHeader*)data; + + int swappedMagic = DT_NAVMESH_MAGIC; + int swappedVersion = DT_NAVMESH_VERSION; + swapEndian(&swappedMagic); + swapEndian(&swappedVersion); + + if ((header->magic != DT_NAVMESH_MAGIC || header->version != DT_NAVMESH_VERSION) && + (header->magic != swappedMagic || header->version != swappedVersion)) + { + return false; + } + + swapEndian(&header->magic); + swapEndian(&header->version); + swapEndian(&header->polyCount); + swapEndian(&header->vertCount); + swapEndian(&header->maxLinkCount); + swapEndian(&header->detailMeshCount); + swapEndian(&header->detailVertCount); + swapEndian(&header->detailTriCount); + swapEndian(&header->bvNodeCount); + swapEndian(&header->offMeshConCount); + swapEndian(&header->offMeshBase); + swapEndian(&header->walkableHeight); + swapEndian(&header->walkableRadius); + swapEndian(&header->walkableClimb); + swapEndian(&header->bmin[0]); + swapEndian(&header->bmin[1]); + swapEndian(&header->bmin[2]); + swapEndian(&header->bmax[0]); + swapEndian(&header->bmax[1]); + swapEndian(&header->bmax[2]); + swapEndian(&header->bvQuantFactor); + + // Freelist index and pointers are updated when tile is added, no need to swap. + + return true; +} + +bool dtNavMeshDataSwapEndian(unsigned char* data, const int dataSize) +{ + // Make sure the data is in right format. + dtMeshHeader* header = (dtMeshHeader*)data; + if (header->magic != DT_NAVMESH_MAGIC) + return false; + if (header->version != DT_NAVMESH_VERSION) + return false; + + // Patch header pointers. + const int headerSize = align4(sizeof(dtMeshHeader)); + const int vertsSize = align4(sizeof(float)*3*header->vertCount); + const int polysSize = align4(sizeof(dtPoly)*header->polyCount); + const int linksSize = align4(sizeof(dtLink)*(header->maxLinkCount)); + const int detailMeshesSize = align4(sizeof(dtPolyDetail)*header->detailMeshCount); + const int detailVertsSize = align4(sizeof(float)*3*header->detailVertCount); + const int detailTrisSize = align4(sizeof(unsigned char)*4*header->detailTriCount); + const int bvtreeSize = align4(sizeof(dtBVNode)*header->bvNodeCount); + const int offMeshLinksSize = align4(sizeof(dtOffMeshConnection)*header->offMeshConCount); + + unsigned char* d = data + headerSize; + float* verts = (float*)d; d += vertsSize; + dtPoly* polys = (dtPoly*)d; d += polysSize; + /*dtLink* links = (dtLink*)d;*/ d += linksSize; + dtPolyDetail* detailMeshes = (dtPolyDetail*)d; d += detailMeshesSize; + float* detailVerts = (float*)d; d += detailVertsSize; + /*unsigned char* detailTris = (unsigned char*)d;*/ d += detailTrisSize; + dtBVNode* bvTree = (dtBVNode*)d; d += bvtreeSize; + dtOffMeshConnection* offMeshCons = (dtOffMeshConnection*)d; d += offMeshLinksSize; + + // Vertices + for (int i = 0; i < header->vertCount*3; ++i) + { + swapEndian(&verts[i]); + } + + // Polys + for (int i = 0; i < header->polyCount; ++i) + { + dtPoly* p = &polys[i]; + // poly->firstLink is update when tile is added, no need to swap. + for (int j = 0; j < DT_VERTS_PER_POLYGON; ++j) + { + swapEndian(&p->verts[j]); + swapEndian(&p->neis[j]); + } + swapEndian(&p->flags); + } + + // Links are rebuild when tile is added, no need to swap. + + // Detail meshes + for (int i = 0; i < header->detailMeshCount; ++i) + { + dtPolyDetail* pd = &detailMeshes[i]; + swapEndian(&pd->vertBase); + swapEndian(&pd->vertCount); + swapEndian(&pd->triBase); + swapEndian(&pd->triCount); + } + + // Detail verts + for (int i = 0; i < header->detailVertCount*3; ++i) + { + swapEndian(&detailVerts[i]); + } + + // BV-tree + for (int i = 0; i < header->bvNodeCount; ++i) + { + dtBVNode* node = &bvTree[i]; + for (int j = 0; j < 3; ++j) + { + swapEndian(&node->bmin[j]); + swapEndian(&node->bmax[j]); + } + swapEndian(&node->i); + } + + // Off-mesh Connections. + for (int i = 0; i < header->offMeshConCount; ++i) + { + dtOffMeshConnection* con = &offMeshCons[i]; + for (int j = 0; j < 6; ++j) + swapEndian(&con->pos[j]); + swapEndian(&con->rad); + swapEndian(&con->poly); + } + + return true; +} diff --git a/src/shared/pathfinding/Detour/DetourNavMeshBuilder.h b/src/shared/pathfinding/Detour/DetourNavMeshBuilder.h new file mode 100644 index 0000000..cad4908 --- /dev/null +++ b/src/shared/pathfinding/Detour/DetourNavMeshBuilder.h @@ -0,0 +1,68 @@ +// +// Copyright (c) 2009 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 + +struct dtNavMeshCreateParams +{ + // Navmesh vertices. + const unsigned short* verts; + int vertCount; + // Navmesh polygons + const unsigned short* polys; + const unsigned short* polyFlags; + const unsigned char* polyAreas; + int polyCount; + int nvp; + // Navmesh Detail + const unsigned short* detailMeshes; + const float* detailVerts; + int detailVertsCount; + const unsigned char* detailTris; + int detailTriCount; + // Off-Mesh Connections. + const float* offMeshConVerts; + const float* offMeshConRad; + const unsigned short* offMeshConFlags; + const unsigned char* offMeshConAreas; + const unsigned char* offMeshConDir; + int offMeshConCount; + // Settings + float walkableHeight; + float walkableRadius; + float walkableClimb; + float bmin[3], bmax[3]; + float cs; + float ch; + int tileSize; +}; + +// 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/src/shared/pathfinding/Detour/DetourNode.cpp b/src/shared/pathfinding/Detour/DetourNode.cpp new file mode 100644 index 0000000..9ddb042 --- /dev/null +++ b/src/shared/pathfinding/Detour/DetourNode.cpp @@ -0,0 +1,142 @@ +// +// Copyright (c) 2009 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#include "DetourNode.h" +#include + +static const unsigned short DT_NULL_IDX = 0xffff; + +////////////////////////////////////////////////////////////////////////////////////////// +dtNodePool::dtNodePool(int maxNodes, int hashSize) : + + m_nodes(0), + m_first(0), + m_next(0), + m_maxNodes(maxNodes), + m_hashSize(hashSize), + m_nodeCount(0) +{ + m_nodes = new dtNode[m_maxNodes]; + m_next = new unsigned short[m_maxNodes]; + m_first = new unsigned short[hashSize]; + memset(m_first, 0xff, sizeof(unsigned short)*m_hashSize); + memset(m_next, 0xff, sizeof(unsigned short)*m_maxNodes); +} + +dtNodePool::~dtNodePool() +{ + delete [] m_nodes; + delete [] m_next; + delete [] m_first; +} + +void dtNodePool::clear() +{ + memset(m_first, 0xff, sizeof(unsigned short)*m_hashSize); + m_nodeCount = 0; +} + +const dtNode* dtNodePool::findNode(unsigned int id) const +{ + unsigned int bucket = hashint(id) & (m_hashSize-1); + unsigned short i = m_first[bucket]; + while (i != DT_NULL_IDX) + { + if (m_nodes[i].id == id) + return &m_nodes[i]; + i = m_next[i]; + } + return 0; +} + +dtNode* dtNodePool::getNode(unsigned int id) +{ + unsigned int bucket = hashint(id) & (m_hashSize-1); + unsigned short i = m_first[bucket]; + dtNode* node = 0; + while (i != DT_NULL_IDX) + { + if (m_nodes[i].id == id) + return &m_nodes[i]; + i = m_next[i]; + } + + if (m_nodeCount >= m_maxNodes) + return 0; + + i = (unsigned short)m_nodeCount; + m_nodeCount++; + + // Init node + node = &m_nodes[i]; + node->pidx = 0; + node->cost = 0; + node->total = 0; + node->id = id; + node->flags = 0; + + m_next[i] = m_first[bucket]; + m_first[bucket] = i; + + return node; +} + + +////////////////////////////////////////////////////////////////////////////////////////// +dtNodeQueue::dtNodeQueue(int n) : + m_heap(0), + m_capacity(n), + m_size(0) +{ + m_heap = new dtNode*[m_capacity+1]; +} + +dtNodeQueue::~dtNodeQueue() +{ + delete [] m_heap; +} + +void dtNodeQueue::bubbleUp(int i, dtNode* node) +{ + int parent = (i-1)/2; + // note: (index > 0) means there is a parent + while ((i > 0) && (m_heap[parent]->total > node->total)) + { + m_heap[i] = m_heap[parent]; + i = parent; + parent = (i-1)/2; + } + m_heap[i] = node; +} + +void dtNodeQueue::trickleDown(int i, dtNode* node) +{ + int child = (i*2)+1; + while (child < m_size) + { + if (((child+1) < m_size) && + (m_heap[child]->total > m_heap[child+1]->total)) + { + child++; + } + m_heap[i] = m_heap[child]; + i = child; + child = (i*2)+1; + } + bubbleUp(i, node); +} diff --git a/src/shared/pathfinding/Detour/DetourNode.h b/src/shared/pathfinding/Detour/DetourNode.h new file mode 100644 index 0000000..316d5bf --- /dev/null +++ b/src/shared/pathfinding/Detour/DetourNode.h @@ -0,0 +1,149 @@ +// +// Copyright (c) 2009 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#ifndef DETOURNODE_H +#define DETOURNODE_H + +enum dtNodeFlags +{ + DT_NODE_OPEN = 0x01, + DT_NODE_CLOSED = 0x02, +}; + +struct dtNode +{ + float cost; + float total; + unsigned int id; + unsigned int pidx : 30; + unsigned int flags : 2; +}; + +class dtNodePool +{ +public: + dtNodePool(int maxNodes, int hashSize); + ~dtNodePool(); + inline void operator=(const dtNodePool&) {} + void clear(); + dtNode* getNode(unsigned int id); + const dtNode* findNode(unsigned int id) const; + + inline unsigned int getNodeIdx(const dtNode* node) const + { + if (!node) return 0; + return (unsigned int)(node - m_nodes)+1; + } + + inline dtNode* getNodeAtIdx(unsigned int idx) + { + if (!idx) return 0; + return &m_nodes[idx-1]; + } + + inline int getMemUsed() const + { + return sizeof(*this) + + sizeof(dtNode)*m_maxNodes + + sizeof(unsigned short)*m_maxNodes + + sizeof(unsigned short)*m_hashSize; + } + +private: + inline unsigned int hashint(unsigned int a) const + { + a += ~(a<<15); + a ^= (a>>10); + a += (a<<3); + a ^= (a>>6); + a += ~(a<<11); + a ^= (a>>16); + return a; + } + + dtNode* m_nodes; + unsigned short* m_first; + unsigned short* m_next; + const int m_maxNodes; + const int m_hashSize; + int m_nodeCount; +}; + +class dtNodeQueue +{ +public: + dtNodeQueue(int n); + ~dtNodeQueue(); + inline void operator=(dtNodeQueue&) {} + + inline void clear() + { + m_size = 0; + } + + inline dtNode* top() + { + return m_heap[0]; + } + + inline dtNode* pop() + { + dtNode* result = m_heap[0]; + m_size--; + trickleDown(0, m_heap[m_size]); + return result; + } + + inline void push(dtNode* node) + { + m_size++; + bubbleUp(m_size-1, node); + } + + inline void modify(dtNode* node) + { + for (int i = 0; i < m_size; ++i) + { + if (m_heap[i] == node) + { + bubbleUp(i, node); + return; + } + } + } + + inline bool empty() const { return m_size == 0; } + + inline int getMemUsed() const + { + return sizeof(*this) + + sizeof(dtNode*)*(m_capacity+1); + } + + +private: + void bubbleUp(int i, dtNode* node); + void trickleDown(int i, dtNode* node); + + dtNode** m_heap; + const int m_capacity; + int m_size; +}; + + +#endif // DETOURNODE_H \ No newline at end of file diff --git a/src/shared/pathfinding/Detour/Makefile.am b/src/shared/pathfinding/Detour/Makefile.am new file mode 100644 index 0000000..d320560 --- /dev/null +++ b/src/shared/pathfinding/Detour/Makefile.am @@ -0,0 +1,38 @@ + Copyright (C) 2005-2010 MaNGOS +# +# 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, write to the Free Software +# Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + +## Process this file with automake to produce Makefile.in + +## Sub-directories to parse + +## CPP flags for includes, defines, etc. +AM_CPPFLAGS = $(MANGOS_INCLUDES) -I$(top_builddir)/src/shared -I$(srcdir) -I$(srcdir)/../../../dep/include -I$(srcdir)/../../framework -I$(srcdir)/../../shared -I$(srcdir)/../../../dep/include/g3dlite + +## Build MaNGOS shared library and its parts as convenience library. +# All libraries will be convenience libraries. Might be changed to shared +# later. +noinst_LIBRARIES = libmangosdetour.a + +libmangosdetour_a_SOURCES = \ + DetourCommon.cpp \ + DetourCommon.h \ + DetourNavMeshBuilder.cpp \ + DetourNavMeshBuilder.h \ + DetourNavMesh.cpp \ + DetourNavMesh.h \ + DetourNode.cpp \ + DetourNode.h + diff --git a/src/shared/pathfinding/InputGeom.cpp b/src/shared/pathfinding/InputGeom.cpp new file mode 100644 index 0000000..43dc399 --- /dev/null +++ b/src/shared/pathfinding/InputGeom.cpp @@ -0,0 +1,364 @@ +// +// Copyright (c) 2009 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. +// + +#define _USE_MATH_DEFINES +#include +#include +#include +#include +#include "Recast/Recast.h" +#include "Recast/RecastLog.h" +#include "InputGeom.h" +#include "ChunkyTriMesh.h" +#include "MeshLoaderObj.h" +#include "Detour/DetourNavMesh.h" + +static bool intersectSegmentTriangle(const float* sp, const float* sq, + const float* a, const float* b, const float* c, + float &t) +{ + float v, w; + float ab[3], ac[3], qp[3], ap[3], norm[3], e[3]; + vsub(ab, b, a); + vsub(ac, c, a); + vsub(qp, sp, sq); + + // Compute triangle normal. Can be precalculated or cached if + // intersecting multiple segments against the same triangle + vcross(norm, ab, ac); + + // Compute denominator d. If d <= 0, segment is parallel to or points + // away from triangle, so exit early + float d = vdot(qp, norm); + if (d <= 0.0f) return false; + + // Compute intersection t value of pq with plane of triangle. A ray + // intersects iff 0 <= t. Segment intersects iff 0 <= t <= 1. Delay + // dividing by d until intersection has been found to pierce triangle + vsub(ap, sp, a); + t = vdot(ap, norm); + if (t < 0.0f) return false; + if (t > d) return false; // For segment; exclude this code line for a ray test + + // Compute barycentric coordinate components and test if within bounds + vcross(e, qp, ap); + v = vdot(ac, e); + if (v < 0.0f || v > d) return false; + w = -vdot(ab, e); + if (w < 0.0f || v + w > d) return false; + + // Segment/ray intersects triangle. Perform delayed division + t /= d; + + return true; +} + +static char* parseRow(char* buf, char* bufEnd, char* row, int len) +{ + bool start = true; + bool done = false; + int n = 0; + while (!done && buf < bufEnd) + { + char c = *buf; + buf++; + // multirow + switch (c) + { + case '\n': + if (start) break; + done = true; + break; + case '\r': + break; + case '\t': + case ' ': + if (start) break; + default: + start = false; + row[n++] = c; + if (n >= len-1) + done = true; + break; + } + } + row[n] = '\0'; + return buf; +} + + + +InputGeom::InputGeom() : + m_chunkyMesh(0), + m_mesh(0), + m_offMeshConCount(0), + m_volumeCount(0) +{ +} + +InputGeom::~InputGeom() +{ + delete m_chunkyMesh; + delete m_mesh; +} + +bool InputGeom::loadMesh(const char* filepath) +{ + if (m_mesh) + { + delete m_chunkyMesh; + m_chunkyMesh = 0; + delete m_mesh; + m_mesh = 0; + } + m_offMeshConCount = 0; + m_volumeCount = 0; + + m_mesh = new rcMeshLoaderObj; + if (!m_mesh) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "loadMesh: Out of memory 'm_mesh'."); + return false; + } + if (!m_mesh->load(filepath)) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "buildTiledNavigation: Could not load '%s'", filepath); + return false; + } + + rcCalcBounds(m_mesh->getVerts(), m_mesh->getVertCount(), m_meshBMin, m_meshBMax); + + m_chunkyMesh = new rcChunkyTriMesh; + if (!m_chunkyMesh) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "buildTiledNavigation: Out of memory 'm_chunkyMesh'."); + return false; + } + if (!rcCreateChunkyTriMesh(m_mesh->getVerts(), m_mesh->getTris(), m_mesh->getTriCount(), 256, m_chunkyMesh)) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "buildTiledNavigation: Failed to build chunky mesh."); + return false; + } + + return true; +} + +bool InputGeom::load(const char* filePath) +{ + char* buf = 0; + FILE* fp = fopen(filePath, "rb"); + if (!fp) + return false; + fseek(fp, 0, SEEK_END); + int bufSize = ftell(fp); + fseek(fp, 0, SEEK_SET); + buf = new char[bufSize]; + if (!buf) + { + fclose(fp); + return false; + } + fread(buf, bufSize, 1, fp); + fclose(fp); + + m_offMeshConCount = 0; + m_volumeCount = 0; + delete m_mesh; + m_mesh = 0; + + char* src = buf; + char* srcEnd = buf + bufSize; + char row[512]; + while (src < srcEnd) + { + // Parse one row + row[0] = '\0'; + src = parseRow(src, srcEnd, row, sizeof(row)/sizeof(char)); + if (row[0] == 'f') + { + // File name. + const char* name = row+1; + // Skip white spaces + while (*name && isspace(*name)) + name++; + if (*name) + { + if (!loadMesh(name)) + { + delete [] buf; + return false; + } + } + } + else if (row[0] == 'c') + { + // Off-mesh connection + if (m_offMeshConCount < MAX_OFFMESH_CONNECTIONS) + { + float* v = &m_offMeshConVerts[m_offMeshConCount*3*2]; + int bidir, area = 0, flags = 0; + float rad; + sscanf(row+1, "%f %f %f %f %f %f %f %d %d %d", + &v[0], &v[1], &v[2], &v[3], &v[4], &v[5], &rad, &bidir, &area, &flags); + m_offMeshConRads[m_offMeshConCount] = rad; + m_offMeshConDirs[m_offMeshConCount] = bidir; + m_offMeshConAreas[m_offMeshConCount] = area; + m_offMeshConFlags[m_offMeshConCount] = flags; + m_offMeshConCount++; + } + } + else if (row[0] == 'v') + { + // Convex volumes + if (m_volumeCount < MAX_VOLUMES) + { + ConvexVolume* vol = &m_volumes[m_volumeCount++]; + sscanf(row+1, "%d %d %f %f", &vol->nverts, &vol->area, &vol->hmin, &vol->hmax); + for (int i = 0; i < vol->nverts; ++i) + { + row[0] = '\0'; + src = parseRow(src, srcEnd, row, sizeof(row)/sizeof(char)); + sscanf(row, "%f %f %f", &vol->verts[i*3+0], &vol->verts[i*3+1], &vol->verts[i*3+2]); + } + } + } + } + + delete [] buf; + + return true; +} + +bool InputGeom::save(const char* filepath) +{ + if (!m_mesh) return false; + + FILE* fp = fopen(filepath, "w"); + if (!fp) return false; + + // Store mesh filename. + fprintf(fp, "f %s\n", m_mesh->getFileName()); + + // Store off-mesh links. + for (int i = 0; i < m_offMeshConCount; ++i) + { + const float* v = &m_offMeshConVerts[i*3*2]; + const float rad = m_offMeshConRads[i]; + const int bidir = m_offMeshConDirs[i]; + const int area = m_offMeshConAreas[i]; + const int flags = m_offMeshConFlags[i]; + fprintf(fp, "c %f %f %f %f %f %f %f %d %d %d\n", + v[0], v[1], v[2], v[3], v[4], v[5], rad, bidir, area, flags); + } + + // Convex volumes + for (int i = 0; i < m_volumeCount; ++i) + { + ConvexVolume* vol = &m_volumes[i]; + fprintf(fp, "v %d %d %f %f\n", vol->nverts, vol->area, vol->hmin, vol->hmax); + for (int i = 0; i < vol->nverts; ++i) + fprintf(fp, "%f %f %f\n", vol->verts[i*3+0], vol->verts[i*3+1], vol->verts[i*3+2]); + } + + fclose(fp); + + return true; +} + +bool InputGeom::raycastMesh(float* src, float* dst, float& tmin) +{ + float dir[3]; + vsub(dir, dst, src); + + int nt = m_mesh->getTriCount(); + const float* verts = m_mesh->getVerts(); + const float* normals = m_mesh->getNormals(); + const int* tris = m_mesh->getTris(); + tmin = 1.0f; + bool hit = false; + + for (int i = 0; i < nt*3; i += 3) + { + const float* n = &normals[i]; + if (vdot(dir, n) > 0) + continue; + + float t = 1; + if (intersectSegmentTriangle(src, dst, + &verts[tris[i]*3], + &verts[tris[i+1]*3], + &verts[tris[i+2]*3], t)) + { + if (t < tmin) + tmin = t; + hit = true; + } + } + + return hit; +} + +void InputGeom::addOffMeshConnection(const float* spos, const float* epos, const float rad, + unsigned char bidir, unsigned char area, unsigned short flags) +{ + if (m_offMeshConCount >= MAX_OFFMESH_CONNECTIONS) return; + float* v = &m_offMeshConVerts[m_offMeshConCount*3*2]; + m_offMeshConRads[m_offMeshConCount] = rad; + m_offMeshConDirs[m_offMeshConCount] = bidir; + m_offMeshConAreas[m_offMeshConCount] = area; + m_offMeshConFlags[m_offMeshConCount] = flags; + vcopy(&v[0], spos); + vcopy(&v[3], epos); + m_offMeshConCount++; +} + +void InputGeom::deleteOffMeshConnection(int i) +{ + m_offMeshConCount--; + float* src = &m_offMeshConVerts[m_offMeshConCount*3*2]; + float* dst = &m_offMeshConVerts[i*3*2]; + vcopy(&dst[0], &src[0]); + vcopy(&dst[3], &src[3]); + m_offMeshConRads[i] = m_offMeshConRads[m_offMeshConCount]; + m_offMeshConDirs[i] = m_offMeshConDirs[m_offMeshConCount]; + m_offMeshConAreas[i] = m_offMeshConAreas[m_offMeshConCount]; + m_offMeshConFlags[i] = m_offMeshConFlags[m_offMeshConCount]; +} + +void InputGeom::addConvexVolume(const float* verts, const int nverts, + const float minh, const float maxh, unsigned char area) +{ + if (m_volumeCount >= MAX_VOLUMES) return; + ConvexVolume* vol = &m_volumes[m_volumeCount++]; + memset(vol, 0, sizeof(ConvexVolume)); + memcpy(vol->verts, verts, sizeof(float)*3*nverts); + vol->hmin = minh; + vol->hmax = maxh; + vol->nverts = nverts; + vol->area = area; +} + +void InputGeom::deleteConvexVolume(int i) +{ + m_volumeCount--; + m_volumes[i] = m_volumes[m_volumeCount]; +} diff --git a/src/shared/pathfinding/InputGeom.h b/src/shared/pathfinding/InputGeom.h new file mode 100644 index 0000000..0ad965e --- /dev/null +++ b/src/shared/pathfinding/InputGeom.h @@ -0,0 +1,91 @@ +// +// Copyright (c) 2009 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 INPUTGEOM_H +#define INPUTGEOM_H + +#include "ChunkyTriMesh.h" +#include "MeshLoaderObj.h" + +static const int MAX_CONVEXVOL_PTS = 12; +struct ConvexVolume +{ + float verts[MAX_CONVEXVOL_PTS*3]; + float hmin, hmax; + int nverts; + int area; +}; + +class InputGeom +{ + rcChunkyTriMesh* m_chunkyMesh; + rcMeshLoaderObj* m_mesh; + float m_meshBMin[3], m_meshBMax[3]; + + // Off-Mesh connections. + static const int MAX_OFFMESH_CONNECTIONS = 256; + float m_offMeshConVerts[MAX_OFFMESH_CONNECTIONS*3*2]; + float m_offMeshConRads[MAX_OFFMESH_CONNECTIONS]; + unsigned char m_offMeshConDirs[MAX_OFFMESH_CONNECTIONS]; + unsigned char m_offMeshConAreas[MAX_OFFMESH_CONNECTIONS]; + unsigned short m_offMeshConFlags[MAX_OFFMESH_CONNECTIONS]; + int m_offMeshConCount; + + // Convex Volumes. + static const int MAX_VOLUMES = 256; + ConvexVolume m_volumes[MAX_VOLUMES]; + int m_volumeCount; + +public: + InputGeom(); + ~InputGeom(); + + bool loadMesh(const char* filepath); + + bool load(const char* filepath); + bool save(const char* filepath); + + // Method to return static mesh data. + inline const rcMeshLoaderObj* getMesh() const { return m_mesh; } + inline const float* getMeshBoundsMin() const { return m_meshBMin; } + inline const float* getMeshBoundsMax() const { return m_meshBMax; } + inline const rcChunkyTriMesh* getChunkyMesh() const { return m_chunkyMesh; } + bool raycastMesh(float* src, float* dst, float& tmin); + + // Off-Mesh connections. + int getOffMeshConnectionCount() const { return m_offMeshConCount; } + const float* getOffMeshConnectionVerts() const { return m_offMeshConVerts; } + const float* getOffMeshConnectionRads() const { return m_offMeshConRads; } + const unsigned char* getOffMeshConnectionDirs() const { return m_offMeshConDirs; } + const unsigned char* getOffMeshConnectionAreas() const { return m_offMeshConAreas; } + const unsigned short* getOffMeshConnectionFlags() const { return m_offMeshConFlags; } + void addOffMeshConnection(const float* spos, const float* epos, const float rad, + unsigned char bidir, unsigned char area, unsigned short flags); + void deleteOffMeshConnection(int i); + void drawOffMeshConnections(struct duDebugDraw* dd, bool hilight = false); + + // Box Volumes. + int getConvexVolumeCount() const { return m_volumeCount; } + const ConvexVolume* getConvexVolumes() const { return m_volumes; } + void addConvexVolume(const float* verts, const int nverts, + const float minh, const float maxh, unsigned char area); + void deleteConvexVolume(int i); + void drawConvexVolumes(struct duDebugDraw* dd, bool hilight = false); +}; + +#endif // INPUTGEOM_H diff --git a/src/shared/pathfinding/Makefile.am b/src/shared/pathfinding/Makefile.am new file mode 100644 index 0000000..0bb1bc4 --- /dev/null +++ b/src/shared/pathfinding/Makefile.am @@ -0,0 +1,36 @@ + Copyright (C) 2005-2010 MaNGOS +# +# 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, write to the Free Software +# Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + +## Process this file with automake to produce Makefile.in + +## Sub-directories to parse +SUBDIRS = Recast Detour + +## CPP flags for includes, defines, etc. +AM_CPPFLAGS = $(MANGOS_INCLUDES) -I$(top_builddir)/src/shared -I$(srcdir) -I$(srcdir)/../../../dep/include -I$(srcdir)/../../framework -I$(srcdir)/../../shared -I$(srcdir)/../../../dep/include/g3dlite + +## Build MaNGOS shared library and its parts as convenience library. +# All libraries will be convenience libraries. Might be changed to shared +# later. +noinst_LIBRARIES = libmangospathfinding.a + +libmangospathfinding_a_SOURCES = \ + ChunkyTriMesh.h \ + ChunkyTriMesh.cpp \ + InputGeom.h \ + InputGeom.cpp \ + MeshLoaderObj.h \ + MeshLoaderObj.cpp diff --git a/src/shared/pathfinding/MeshLoaderObj.cpp b/src/shared/pathfinding/MeshLoaderObj.cpp new file mode 100644 index 0000000..2c87cba --- /dev/null +++ b/src/shared/pathfinding/MeshLoaderObj.cpp @@ -0,0 +1,229 @@ +// +// Copyright (c) 2009 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 "MeshLoaderObj.h" +#include +#include +#include +#define _USE_MATH_DEFINES +#include + +rcMeshLoaderObj::rcMeshLoaderObj() : + m_verts(0), + m_tris(0), + m_normals(0), + m_vertCount(0), + m_triCount(0) +{ +} + +rcMeshLoaderObj::~rcMeshLoaderObj() +{ + delete [] m_verts; + delete [] m_normals; + delete [] m_tris; +} + +void rcMeshLoaderObj::addVertex(float x, float y, float z, int& cap) +{ + if (m_vertCount+1 > cap) + { + cap = !cap ? 8 : cap*2; + float* nv = new float[cap*3]; + if (m_vertCount) + memcpy(nv, m_verts, m_vertCount*3*sizeof(float)); + delete [] m_verts; + m_verts = nv; + } + float* dst = &m_verts[m_vertCount*3]; + *dst++ = x; + *dst++ = y; + *dst++ = z; + m_vertCount++; +} + +void rcMeshLoaderObj::addTriangle(int a, int b, int c, int& cap) +{ + if (m_triCount+1 > cap) + { + cap = !cap ? 8 : cap*2; + int* nv = new int[cap*3]; + if (m_triCount) + memcpy(nv, m_tris, m_triCount*3*sizeof(int)); + delete [] m_tris; + m_tris = nv; + } + int* dst = &m_tris[m_triCount*3]; + *dst++ = a; + *dst++ = b; + *dst++ = c; + m_triCount++; +} + +static char* parseRow(char* buf, char* bufEnd, char* row, int len) +{ + bool cont = false; + bool start = true; + bool done = false; + int n = 0; + while (!done && buf < bufEnd) + { + char c = *buf; + buf++; + // multirow + switch (c) + { + case '\\': + cont = true; // multirow + break; + case '\n': + if (start) break; + done = true; + break; + case '\r': + break; + case '\t': + case ' ': + if (start) break; + default: + start = false; + cont = false; + row[n++] = c; + if (n >= len-1) + done = true; + break; + } + } + row[n] = '\0'; + return buf; +} + +static int parseFace(char* row, int* data, int n, int vcnt) +{ + int j = 0; + while (*row != '\0') + { + // Skip initial white space + while (*row != '\0' && (*row == ' ' || *row == '\t')) + row++; + char* s = row; + // Find vertex delimiter and terminated the string there for conversion. + while (*row != '\0' && *row != ' ' && *row != '\t') + { + if (*row == '/') *row = '\0'; + row++; + } + if (*s == '\0') + continue; + int vi = atoi(s); + data[j++] = vi < 0 ? vi+vcnt : vi-1; + if (j >= n) return j; + } + return j; +} + +bool rcMeshLoaderObj::load(const char* filename) +{ + char* buf = 0; + FILE* fp = fopen(filename, "rb"); + if (!fp) + return false; + fseek(fp, 0, SEEK_END); + int bufSize = ftell(fp); + fseek(fp, 0, SEEK_SET); + buf = new char[bufSize]; + if (!buf) + { + fclose(fp); + return false; + } + fread(buf, bufSize, 1, fp); + fclose(fp); + + char* src = buf; + char* srcEnd = buf + bufSize; + char row[512]; + int face[32]; + float x,y,z; + int nv; + int vcap = 0; + int tcap = 0; + + while (src < srcEnd) + { + // Parse one row + row[0] = '\0'; + src = parseRow(src, srcEnd, row, sizeof(row)/sizeof(char)); + // Skip comments + if (row[0] == '#') continue; + if (row[0] == 'v' && row[1] != 'n' && row[1] != 't') + { + // Vertex pos + sscanf(row+1, "%f %f %f", &x, &y, &z); + addVertex(x, y, z, vcap); + } + if (row[0] == 'f') + { + // Faces + nv = parseFace(row+1, face, 32, m_vertCount); + for (int i = 2; i < nv; ++i) + { + const int a = face[0]; + const int b = face[i-1]; + const int c = face[i]; + if (a < 0 || a >= m_vertCount || b < 0 || b >= m_vertCount || c < 0 || c >= m_vertCount) + continue; + addTriangle(a, b, c, tcap); + } + } + } + + delete [] buf; + + // Calculate normals. + m_normals = new float[m_triCount*3]; + for (int i = 0; i < m_triCount*3; i += 3) + { + const float* v0 = &m_verts[m_tris[i]*3]; + const float* v1 = &m_verts[m_tris[i+1]*3]; + const float* v2 = &m_verts[m_tris[i+2]*3]; + float e0[3], e1[3]; + for (int j = 0; j < 3; ++j) + { + e0[j] = v1[j] - v0[j]; + e1[j] = v2[j] - v0[j]; + } + float* n = &m_normals[i]; + n[0] = e0[1]*e1[2] - e0[2]*e1[1]; + n[1] = e0[2]*e1[0] - e0[0]*e1[2]; + n[2] = e0[0]*e1[1] - e0[1]*e1[0]; + float d = sqrtf(n[0]*n[0] + n[1]*n[1] + n[2]*n[2]); + if (d > 0) + { + d = 1.0f/d; + n[0] *= d; + n[1] *= d; + n[2] *= d; + } + } + + strncpy(m_filename, filename, sizeof(m_filename)); + m_filename[sizeof(m_filename)-1] = '\0'; + + return true; +} diff --git a/src/shared/pathfinding/MeshLoaderObj.h b/src/shared/pathfinding/MeshLoaderObj.h new file mode 100644 index 0000000..1da5491 --- /dev/null +++ b/src/shared/pathfinding/MeshLoaderObj.h @@ -0,0 +1,51 @@ +// +// Copyright (c) 2009 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 MESHLOADER_OBJ +#define MESHLOADER_OBJ + +class rcMeshLoaderObj +{ +public: + rcMeshLoaderObj(); + ~rcMeshLoaderObj(); + + bool load(const char* fileName); + + inline const float* getVerts() const { return m_verts; } + inline const float* getNormals() const { return m_normals; } + inline const int* getTris() const { return m_tris; } + inline int getVertCount() const { return m_vertCount; } + inline int getTriCount() const { return m_triCount; } + inline const char* getFileName() const { return m_filename; } + +private: + + void addVertex(float x, float y, float z, int& cap); + void addTriangle(int a, int b, int c, int& cap); + + char m_filename[260]; + + float* m_verts; + int* m_tris; + float* m_normals; + int m_vertCount; + int m_triCount; +}; + +#endif // MESHLOADER_OBJ diff --git a/src/shared/pathfinding/Recast/Makefile.am b/src/shared/pathfinding/Recast/Makefile.am new file mode 100644 index 0000000..52a8c5f --- /dev/null +++ b/src/shared/pathfinding/Recast/Makefile.am @@ -0,0 +1,43 @@ + Copyright (C) 2005-2010 MaNGOS +# +# 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, write to the Free Software +# Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + +## Process this file with automake to produce Makefile.in + +## Sub-directories to parse + +## CPP flags for includes, defines, etc. +AM_CPPFLAGS = $(MANGOS_INCLUDES) -I$(top_builddir)/src/shared -I$(srcdir) -I$(srcdir)/../../../dep/include -I$(srcdir)/../../framework -I$(srcdir)/../../shared -I$(srcdir)/../../../dep/include/g3dlite + +## Build MaNGOS shared library and its parts as convenience library. +# All libraries will be convenience libraries. Might be changed to shared +# later. +noinst_LIBRARIES = libmangosrecast.a + +libmangosrecast_a_SOURCES = \ + RecastArea.cpp \ + RecastContour.cpp \ + Recast.cpp \ + RecastFilter.cpp \ + Recast.h \ + RecastLog.cpp \ + RecastLog.h \ + RecastMesh.cpp \ + RecastMeshDetail.cpp \ + RecastRasterization.cpp \ + RecastRegion.cpp \ + RecastTimer.cpp \ + RecastTimer.h + diff --git a/src/shared/pathfinding/Recast/Recast.cpp b/src/shared/pathfinding/Recast/Recast.cpp new file mode 100644 index 0000000..27f59b8 --- /dev/null +++ b/src/shared/pathfinding/Recast/Recast.cpp @@ -0,0 +1,279 @@ +// +// Copyright (c) 2009 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 +#define _USE_MATH_DEFINES +#include +#include +#include +#include +#include "Recast.h" +#include "RecastLog.h" +#include "RecastTimer.h" + + +void rcIntArray::resize(int n) +{ + if (n > m_cap) + { + if (!m_cap) m_cap = 8; + while (m_cap < n) m_cap *= 2; + int* newData = new int[m_cap]; + if (m_size && newData) memcpy(newData, m_data, m_size*sizeof(int)); + delete [] m_data; + m_data = newData; + } + m_size = n; +} + +void rcCalcBounds(const float* verts, int nv, float* bmin, float* bmax) +{ + // Calculate bounding box. + vcopy(bmin, verts); + vcopy(bmax, verts); + for (int i = 1; i < nv; ++i) + { + const float* v = &verts[i*3]; + vmin(bmin, v); + vmax(bmax, v); + } +} + +void rcCalcGridSize(const float* bmin, const float* bmax, float cs, int* w, int* h) +{ + *w = (int)((bmax[0] - bmin[0])/cs+0.5f); + *h = (int)((bmax[2] - bmin[2])/cs+0.5f); +} + +bool rcCreateHeightfield(rcHeightfield& hf, int width, int height, + const float* bmin, const float* bmax, + float cs, float ch) +{ + hf.width = width; + hf.height = height; + hf.spans = new rcSpan*[hf.width*hf.height]; + vcopy(hf.bmin, bmin); + vcopy(hf.bmax, bmax); + hf.cs = cs; + hf.ch = ch; + if (!hf.spans) + return false; + memset(hf.spans, 0, sizeof(rcSpan*)*hf.width*hf.height); + return true; +} + +static void calcTriNormal(const float* v0, const float* v1, const float* v2, float* norm) +{ + float e0[3], e1[3]; + vsub(e0, v1, v0); + vsub(e1, v2, v0); + vcross(norm, e0, e1); + vnormalize(norm); +} + +void rcMarkWalkableTriangles(const float walkableSlopeAngle, + const float* verts, int nv, + const int* tris, int nt, + unsigned char* flags) +{ + const float walkableThr = cosf(walkableSlopeAngle/180.0f*(float)M_PI); + + float norm[3]; + + for (int i = 0; i < nt; ++i) + { + const int* tri = &tris[i*3]; + calcTriNormal(&verts[tri[0]*3], &verts[tri[1]*3], &verts[tri[2]*3], norm); + // Check if the face is walkable. + if (norm[1] > walkableThr) + flags[i] |= RC_WALKABLE; + } +} + +static int getSpanCount(unsigned char flags, rcHeightfield& hf) +{ + const int w = hf.width; + const int h = hf.height; + int spanCount = 0; + for (int y = 0; y < h; ++y) + { + for (int x = 0; x < w; ++x) + { + for (rcSpan* s = hf.spans[x + y*w]; s; s = s->next) + { + if (s->flags == flags) + spanCount++; + } + } + } + return spanCount; +} + +bool rcBuildCompactHeightfield(const int walkableHeight, const int walkableClimb, + unsigned char flags, rcHeightfield& hf, + rcCompactHeightfield& chf) +{ + rcTimeVal startTime = rcGetPerformanceTimer(); + + const int w = hf.width; + const int h = hf.height; + const int spanCount = getSpanCount(flags, hf); + + // Fill in header. + chf.width = w; + chf.height = h; + chf.spanCount = spanCount; + chf.walkableHeight = walkableHeight; + chf.walkableClimb = walkableClimb; + chf.maxRegions = 0; + vcopy(chf.bmin, hf.bmin); + vcopy(chf.bmax, hf.bmax); + chf.bmax[1] += walkableHeight*hf.ch; + chf.cs = hf.cs; + chf.ch = hf.ch; + chf.cells = new rcCompactCell[w*h]; + if (!chf.cells) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildCompactHeightfield: Out of memory 'chf.cells' (%d)", w*h); + return false; + } + memset(chf.cells, 0, sizeof(rcCompactCell)*w*h); + chf.spans = new rcCompactSpan[spanCount]; + if (!chf.spans) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildCompactHeightfield: Out of memory 'chf.spans' (%d)", spanCount); + return false; + } + memset(chf.spans, 0, sizeof(rcCompactSpan)*spanCount); + chf.areas = new unsigned char[spanCount]; + if (!chf.areas) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildCompactHeightfield: Out of memory 'chf.areas' (%d)", spanCount); + return false; + } + memset(chf.areas, RC_WALKABLE_AREA, sizeof(unsigned char)*spanCount); + + const int MAX_HEIGHT = 0xffff; + + // Fill in cells and spans. + int idx = 0; + for (int y = 0; y < h; ++y) + { + for (int x = 0; x < w; ++x) + { + const rcSpan* s = hf.spans[x + y*w]; + // If there are no spans at this cell, just leave the data to index=0, count=0. + if (!s) continue; + rcCompactCell& c = chf.cells[x+y*w]; + c.index = idx; + c.count = 0; + while (s) + { + if (s->flags == flags) + { + const int bot = (int)s->smax; + const int top = s->next ? (int)s->next->smin : MAX_HEIGHT; + chf.spans[idx].y = (unsigned short)rcClamp(bot, 0, 0xffff); + chf.spans[idx].h = (unsigned char)rcClamp(top - bot, 0, 0xff); + idx++; + c.count++; + } + s = s->next; + } + } + } + + // Find neighbour connections. + for (int y = 0; y < h; ++y) + { + for (int x = 0; x < w; ++x) + { + const rcCompactCell& c = chf.cells[x+y*w]; + for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i) + { + rcCompactSpan& s = chf.spans[i]; + + for (int dir = 0; dir < 4; ++dir) + { + rcSetCon(s, dir, RC_NOT_CONNECTED); + const int nx = x + rcGetDirOffsetX(dir); + const int ny = y + rcGetDirOffsetY(dir); + // First check that the neighbour cell is in bounds. + if (nx < 0 || ny < 0 || nx >= w || ny >= h) + continue; + + // Iterate over all neighbour spans and check if any of the is + // accessible from current cell. + const rcCompactCell& nc = chf.cells[nx+ny*w]; + for (int k = (int)nc.index, nk = (int)(nc.index+nc.count); k < nk; ++k) + { + const rcCompactSpan& ns = chf.spans[k]; + const int bot = rcMax(s.y, ns.y); + const int top = rcMin(s.y+s.h, ns.y+ns.h); + + // Check that the gap between the spans is walkable, + // and that the climb height between the gaps is not too high. + if ((top - bot) >= walkableHeight && rcAbs((int)ns.y - (int)s.y) <= walkableClimb) + { + // Mark direction as walkable. + rcSetCon(s, dir, k - (int)nc.index); + break; + } + } + + } + } + } + } + + rcTimeVal endTime = rcGetPerformanceTimer(); + + if (rcGetBuildTimes()) + rcGetBuildTimes()->buildCompact += rcGetDeltaTimeUsec(startTime, endTime); + + return true; +} + +/* +static int getHeightfieldMemoryUsage(const rcHeightfield& hf) +{ + int size = 0; + size += sizeof(hf); + size += hf.width * hf.height * sizeof(rcSpan*); + + rcSpanPool* pool = hf.pools; + while (pool) + { + size += (sizeof(rcSpanPool) - sizeof(rcSpan)) + sizeof(rcSpan)*RC_SPANS_PER_POOL; + pool = pool->next; + } + return size; +} + +static int getCompactHeightFieldMemoryusage(const rcCompactHeightfield& chf) +{ + int size = 0; + size += sizeof(rcCompactHeightfield); + size += sizeof(rcCompactSpan) * chf.spanCount; + size += sizeof(rcCompactCell) * chf.width * chf.height; + return size; +} +*/ diff --git a/src/shared/pathfinding/Recast/Recast.h b/src/shared/pathfinding/Recast/Recast.h new file mode 100644 index 0000000..f4d2d1e --- /dev/null +++ b/src/shared/pathfinding/Recast/Recast.h @@ -0,0 +1,627 @@ +// +// Copyright (c) 2009 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#ifndef RECAST_H +#define RECAST_H + +// The units of the parameters are specified in parenthesis as follows: +// (vx) voxels, (wu) world units +struct rcConfig +{ + int width, height; // Dimensions of the rasterized heighfield (vx) + int tileSize; // Width and Height of a tile (vx) + int borderSize; // Non-navigable Border around the heightfield (vx) + float cs, ch; // Grid cell size and height (wu) + float bmin[3], bmax[3]; // Grid bounds (wu) + float walkableSlopeAngle; // Maximum walkble slope angle in degrees. + int walkableHeight; // Minimum height where the agent can still walk (vx) + int walkableClimb; // Maximum height between grid cells the agent can climb (vx) + int walkableRadius; // Radius of the agent in cells (vx) + int maxEdgeLen; // Maximum contour edge length (vx) + float maxSimplificationError; // Maximum distance error from contour to cells (vx) + int minRegionSize; // Minimum regions size. Smaller regions will be deleted (vx) + int mergeRegionSize; // Minimum regions size. Smaller regions will be merged (vx) + int maxVertsPerPoly; // Max number of vertices per polygon + float detailSampleDist; // Detail mesh sample spacing. + float detailSampleMaxError; // Detail mesh simplification max sample error. +}; + +// Heightfield span. +struct rcSpan +{ + unsigned int smin : 15; // Span min height. + unsigned int smax : 15; // Span max height. + unsigned int flags : 2; // Span flags. + rcSpan* next; // Next span in column. +}; + +static const int RC_SPANS_PER_POOL = 2048; + +// Memory pool used for quick span allocation. +struct rcSpanPool +{ + rcSpanPool* next; // Pointer to next pool. + rcSpan items[1]; // Array of spans (size RC_SPANS_PER_POOL). +}; + +// Dynamic span-heightfield. +struct rcHeightfield +{ + inline rcHeightfield() : width(0), height(0), spans(0), pools(0), freelist(0) {} + inline ~rcHeightfield() + { + // Delete span array. + delete [] spans; + // Delete span pools. + while (pools) + { + rcSpanPool* next = pools->next; + delete [] reinterpret_cast(pools); + pools = next; + } + } + int width, height; // Dimension of the heightfield. + float bmin[3], bmax[3]; // Bounding box of the heightfield + float cs, ch; // Cell size and height. + rcSpan** spans; // Heightfield of spans (width*height). + rcSpanPool* pools; // Linked list of span pools. + rcSpan* freelist; // Pointer to next free span. +}; + +struct rcCompactCell +{ + unsigned int index : 24; // Index to first span in column. + unsigned int count : 8; // Number of spans in this column. +}; + +struct rcCompactSpan +{ + unsigned short y; // Bottom coordinate of the span. + unsigned short con; // Connections to neighbour cells. + unsigned char h; // Height of the span. +}; + +// Compact static heightfield. +struct rcCompactHeightfield +{ + inline rcCompactHeightfield() : + maxDistance(0), maxRegions(0), cells(0), + spans(0), dist(0), regs(0), areas(0) {} + inline ~rcCompactHeightfield() + { + delete [] cells; + delete [] spans; + delete [] dist; + delete [] regs; + delete [] areas; + } + int width, height; // Width and height of the heighfield. + int spanCount; // Number of spans in the heightfield. + int walkableHeight, walkableClimb; // Agent properties. + unsigned short maxDistance; // Maximum distance value stored in heightfield. + unsigned short maxRegions; // Maximum Region Id stored in heightfield. + float bmin[3], bmax[3]; // Bounding box of the heightfield. + float cs, ch; // Cell size and height. + rcCompactCell* cells; // Pointer to width*height cells. + rcCompactSpan* spans; // Pointer to spans. + unsigned short* dist; // Pointer to per span distance to border. + unsigned short* regs; // Pointer to per span region ID. + unsigned char* areas; // Pointer to per span area ID. +}; + +struct rcContour +{ + inline rcContour() : verts(0), nverts(0), rverts(0), nrverts(0) { } + inline ~rcContour() { delete [] verts; delete [] rverts; } + int* verts; // Vertex coordinates, each vertex contains 4 components. + int nverts; // Number of vertices. + int* rverts; // Raw vertex coordinates, each vertex contains 4 components. + int nrverts; // Number of raw vertices. + unsigned short reg; // Region ID of the contour. + unsigned char area; // Area ID of the contour. +}; + +struct rcContourSet +{ + inline rcContourSet() : conts(0), nconts(0) {} + inline ~rcContourSet() { delete [] conts; } + rcContour* conts; // Pointer to all contours. + int nconts; // Number of contours. + float bmin[3], bmax[3]; // Bounding box of the heightfield. + float cs, ch; // Cell size and height. +}; + +// Polymesh store a connected mesh of polygons. +// The polygons are store in an array where each polygons takes +// 'nvp*2' elements. The first 'nvp' elements are indices to vertices +// and the second 'nvp' elements are indices to neighbour polygons. +// If a polygona has less than 'bvp' vertices, the remaining indices +// are set to RC_MESH_NULL_IDX. If an polygon edge does not have a neighbour +// the neighbour index is set to RC_MESH_NULL_IDX. +// Vertices can be transformed into world space as follows: +// x = bmin[0] + verts[i*3+0]*cs; +// y = bmin[1] + verts[i*3+1]*ch; +// z = bmin[2] + verts[i*3+2]*cs; +struct rcPolyMesh +{ + inline rcPolyMesh() : verts(0), polys(0), regs(0), flags(0), areas(0), nverts(0), npolys(0), nvp(3) {} + + inline ~rcPolyMesh() { delete [] verts; delete [] polys; delete [] regs; delete [] flags; delete [] areas; } + + unsigned short* verts; // Vertices of the mesh, 3 elements per vertex. + unsigned short* polys; // Polygons of the mesh, nvp*2 elements per polygon. + unsigned short* regs; // Region ID of the polygons. + unsigned short* flags; // Per polygon flags. + unsigned char* areas; // Area ID of polygons. + int nverts; // Number of vertices. + int npolys; // Number of polygons. + int nvp; // Max number of vertices per polygon. + float bmin[3], bmax[3]; // Bounding box of the mesh. + float cs, ch; // Cell size and height. +}; + +// Detail mesh generated from a rcPolyMesh. +// Each submesh represents a polygon in the polymesh and they are stored in +// excatly same order. Each submesh is described as 4 values: +// base vertex, vertex count, base triangle, triangle count. That is, +// const unsigned char* t = &dtl.tris[(tbase+i)*3]; and +// const float* v = &dtl.verts[(vbase+t[j])*3]; +// If the input polygon has 'n' vertices, those vertices are first in the +// submesh vertex list. This allows to compres the mesh by not storing the +// first vertices and using the polymesh vertices instead. + +struct rcPolyMeshDetail +{ + inline rcPolyMeshDetail() : + meshes(0), verts(0), tris(0), + nmeshes(0), nverts(0), ntris(0) {} + inline ~rcPolyMeshDetail() + { + delete [] meshes; delete [] verts; delete [] tris; + } + + unsigned short* meshes; // Pointer to all mesh data. + float* verts; // Pointer to all vertex data. + unsigned char* tris; // Pointer to all triangle data. + int nmeshes; // Number of meshes. + int nverts; // Number of total vertices. + int ntris; // Number of triangles. +}; + + +// Simple dynamic array ints. +class rcIntArray +{ + int* m_data; + int m_size, m_cap; +public: + inline rcIntArray() : m_data(0), m_size(0), m_cap(0) {} + inline rcIntArray(int n) : m_data(0), m_size(0), m_cap(n) { m_data = new int[n]; } + inline ~rcIntArray() { delete [] m_data; } + void resize(int n); + inline void push(int item) { resize(m_size+1); m_data[m_size-1] = item; } + inline int pop() { if (m_size > 0) m_size--; return m_data[m_size]; } + inline const int& operator[](int i) const { return m_data[i]; } + inline int& operator[](int i) { return m_data[i]; } + inline int size() const { return m_size; } +}; + +// Simple helper class to delete array in scope +template class rcScopedDelete +{ + T* ptr; +public: + inline rcScopedDelete() : ptr(0) {} + inline rcScopedDelete(T* p) : ptr(p) {} + inline ~rcScopedDelete() { delete [] ptr; } + inline operator T*() { return ptr; } + inline T* operator=(T* p) { ptr = p; return ptr; } +}; + +enum rcSpanFlags +{ + RC_WALKABLE = 0x01, + RC_LEDGE = 0x02, +}; + +// If heightfield region ID has the following bit set, the region is on border area +// and excluded from many calculations. +static const unsigned short RC_BORDER_REG = 0x8000; + +// If contour region ID has the following bit set, the vertex will be later +// removed in order to match the segments and vertices at tile boundaries. +static const int RC_BORDER_VERTEX = 0x10000; + +static const int RC_AREA_BORDER = 0x20000; + +// Mask used with contours to extract region id. +static const int RC_CONTOUR_REG_MASK = 0xffff; + +// Null index which is used with meshes to mark unset or invalid indices. +static const unsigned short RC_MESH_NULL_IDX = 0xffff; + +// Area ID that is considered empty. +static const unsigned char RC_NULL_AREA = 0; + +// Area ID that is considered generally walkable. +static const unsigned char RC_WALKABLE_AREA = 255; + +// Value returned by rcGetCon() if the direction is not connected. +static const int RC_NOT_CONNECTED = 0xf; + +// Compact span neighbour helpers. +inline void rcSetCon(rcCompactSpan& s, int dir, int i) +{ + s.con &= ~(0xf << (dir*4)); + s.con |= (i&0xf) << (dir*4); +} + +inline int rcGetCon(const rcCompactSpan& s, int dir) +{ + return (s.con >> (dir*4)) & 0xf; +} + +inline int rcGetDirOffsetX(int dir) +{ + const int offset[4] = { -1, 0, 1, 0, }; + return offset[dir&0x03]; +} + +inline int rcGetDirOffsetY(int dir) +{ + const int offset[4] = { 0, 1, 0, -1 }; + return offset[dir&0x03]; +} + +// Common helper functions +template inline void rcSwap(T& a, T& b) { T t = a; a = b; b = t; } +template inline T rcMin(T a, T b) { return a < b ? a : b; } +template inline T rcMax(T a, T b) { return a > b ? a : b; } +template inline T rcAbs(T a) { return a < 0 ? -a : a; } +template inline T rcSqr(T a) { return a*a; } +template inline T rcClamp(T v, T mn, T mx) { return v < mn ? mn : (v > mx ? mx : v); } + +// Common vector helper functions. +inline void vcross(float* dest, const float* v1, const float* v2) +{ + dest[0] = v1[1]*v2[2] - v1[2]*v2[1]; + dest[1] = v1[2]*v2[0] - v1[0]*v2[2]; + dest[2] = v1[0]*v2[1] - v1[1]*v2[0]; +} + +inline float vdot(const float* v1, const float* v2) +{ + return v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2]; +} + +inline void vmad(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 vadd(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 vsub(float* dest, const float* v1, const float* v2) +{ + dest[0] = v1[0]-v2[0]; + dest[1] = v1[1]-v2[1]; + dest[2] = v1[2]-v2[2]; +} + +inline void vmin(float* mn, const float* v) +{ + mn[0] = rcMin(mn[0], v[0]); + mn[1] = rcMin(mn[1], v[1]); + mn[2] = rcMin(mn[2], v[2]); +} + +inline void vmax(float* mx, const float* v) +{ + mx[0] = rcMax(mx[0], v[0]); + mx[1] = rcMax(mx[1], v[1]); + mx[2] = rcMax(mx[2], v[2]); +} + +inline void vcopy(float* dest, const float* v) +{ + dest[0] = v[0]; + dest[1] = v[1]; + dest[2] = v[2]; +} + +inline float vdist(const float* v1, const float* v2) +{ + float dx = v2[0] - v1[0]; + float dy = v2[1] - v1[1]; + float dz = v2[2] - v1[2]; + return sqrtf(dx*dx + dy*dy + dz*dz); +} + +inline float vdistSqr(const float* v1, const float* v2) +{ + float dx = v2[0] - v1[0]; + float dy = v2[1] - v1[1]; + float dz = v2[2] - v1[2]; + return dx*dx + dy*dy + dz*dz; +} + +inline void vnormalize(float* v) +{ + float d = 1.0f / sqrtf(rcSqr(v[0]) + rcSqr(v[1]) + rcSqr(v[2])); + v[0] *= d; + v[1] *= d; + v[2] *= d; +} + +inline bool vequal(const float* p0, const float* p1) +{ + static const float thr = rcSqr(1.0f/16384.0f); + const float d = vdistSqr(p0, p1); + return d < thr; +} + + +// Calculated bounding box of array of vertices. +// Params: +// verts - (in) array of vertices +// nv - (in) vertex count +// bmin, bmax - (out) bounding box +void rcCalcBounds(const float* verts, int nv, float* bmin, float* bmax); + +// Calculates grid size based on bounding box and grid cell size. +// Params: +// bmin, bmax - (in) bounding box +// cs - (in) grid cell size +// w - (out) grid width +// h - (out) grid height +void rcCalcGridSize(const float* bmin, const float* bmax, float cs, int* w, int* h); + +// Creates and initializes new heightfield. +// Params: +// hf - (in/out) heightfield to initialize. +// width - (in) width of the heightfield. +// height - (in) height of the heightfield. +// bmin, bmax - (in) bounding box of the heightfield +// cs - (in) grid cell size +// ch - (in) grid cell height +bool rcCreateHeightfield(rcHeightfield& hf, int width, int height, + const float* bmin, const float* bmax, + float cs, float ch); + +// Sets the WALKABLE flag for every triangle whose slope is below +// the maximun walkable slope angle. +// Params: +// walkableSlopeAngle - (in) maximun slope angle in degrees. +// verts - (in) array of vertices +// nv - (in) vertex count +// tris - (in) array of triangle vertex indices +// nt - (in) triangle count +// flags - (out) array of triangle flags +void rcMarkWalkableTriangles(const float walkableSlopeAngle, + const float* verts, int nv, + const int* tris, int nt, + unsigned char* flags); + +// Adds span to heighfield. +// The span addition can set to favor flags. If the span is merged to +// another span and the new smax is within 'flagMergeThr' units away +// from the existing span the span flags are merged and stored. +// Params: +// solid - (in) heighfield where the spans is added to +// x,y - (in) location on the heighfield where the span is added +// smin,smax - (in) spans min/max height +// flags - (in) span flags (zero or WALKABLE) +// flagMergeThr - (in) merge threshold. +void rcAddSpan(rcHeightfield& solid, const int x, const int y, + const unsigned short smin, const unsigned short smax, + const unsigned short flags, const int flagMergeThr); + +// Rasterizes a triangle into heightfield spans. +// Params: +// v0,v1,v2 - (in) the vertices of the triangle. +// flags - (in) triangle flags (uses WALKABLE) +// solid - (in) heighfield where the triangle is rasterized +// flagMergeThr - (in) distance in voxel where walkable flag is favored over non-walkable. +void rcRasterizeTriangle(const float* v0, const float* v1, const float* v2, + unsigned char flags, rcHeightfield& solid, + const int flagMergeThr = 1); + +// Rasterizes indexed triangle mesh into heightfield spans. +// Params: +// verts - (in) array of vertices +// nv - (in) vertex count +// tris - (in) array of triangle vertex indices +// flags - (in) array of triangle flags (uses WALKABLE) +// nt - (in) triangle count +// solid - (in) heighfield where the triangles are rasterized +// flagMergeThr - (in) distance in voxel where walkable flag is favored over non-walkable. +void rcRasterizeTriangles(const float* verts, int nv, + const int* tris, const unsigned char* flags, int nt, + rcHeightfield& solid, const int flagMergeThr = 1); + +// Rasterizes indexed triangle mesh into heightfield spans. +// Params: +// verts - (in) array of vertices +// nv - (in) vertex count +// tris - (in) array of triangle vertex indices +// flags - (in) array of triangle flags (uses WALKABLE) +// nt - (in) triangle count +// solid - (in) heighfield where the triangles are rasterized +// flagMergeThr - (in) distance in voxel where walkable flag is favored over non-walkable. +void rcRasterizeTriangles(const float* verts, int nv, + const unsigned short* tris, const unsigned char* flags, int nt, + rcHeightfield& solid, const int flagMergeThr = 1); + +// Rasterizes the triangles into heightfield spans. +// Params: +// verts - (in) array of vertices +// flags - (in) array of triangle flags (uses WALKABLE) +// nt - (in) triangle count +// solid - (in) heighfield where the triangles are rasterized +void rcRasterizeTriangles(const float* verts, const unsigned char* flags, int nt, + rcHeightfield& solid, const int flagMergeThr = 1); + +// Marks non-walkable low obstacles as walkable if they are closer than walkableClimb +// from a walkable surface. Applying this filter allows to step over low hanging +// low obstacles. +// Params: +// walkableHeight - (in) minimum height where the agent can still walk +// solid - (in/out) heightfield describing the solid space +// TODO: Missuses ledge flag, must be called before rcFilterLedgeSpans! +void rcFilterLowHangingWalkableObstacles(const int walkableClimb, rcHeightfield& solid); + +// Removes WALKABLE flag from all spans that are at ledges. This filtering +// removes possible overestimation of the conservative voxelization so that +// the resulting mesh will not have regions hanging in air over ledges. +// Params: +// walkableHeight - (in) minimum height where the agent can still walk +// walkableClimb - (in) maximum height between grid cells the agent can climb +// solid - (in/out) heightfield describing the solid space +void rcFilterLedgeSpans(const int walkableHeight, + const int walkableClimb, + rcHeightfield& solid); + +// Removes WALKABLE flag from all spans which have smaller than +// 'walkableHeight' clearane above them. +// Params: +// walkableHeight - (in) minimum height where the agent can still walk +// solid - (in/out) heightfield describing the solid space +void rcFilterWalkableLowHeightSpans(int walkableHeight, + rcHeightfield& solid); + +// Builds compact representation of the heightfield. +// Params: +// walkableHeight - (in) minimum height where the agent can still walk +// walkableClimb - (in) maximum height between grid cells the agent can climb +// hf - (in) heightfield to be compacted +// chf - (out) compact heightfield representing the open space. +// Returns false if operation ran out of memory. +bool rcBuildCompactHeightfield(const int walkableHeight, const int walkableClimb, + unsigned char flags, + rcHeightfield& hf, + rcCompactHeightfield& chf); + +// Erodes specified area id and replaces the are with null. +// Params: +// areaId - (in) area to erode. +// radius - (in) radius of erosion (max 255). +// chf - (in/out) compact heightfield to erode. +bool rcErodeArea(unsigned char areaId, int radius, rcCompactHeightfield& chf); + +// Marks the area of the convex polygon into the area type of the compact heighfield. +// Params: +// bmin/bmax - (in) bounds of the axis aligned box. +// areaId - (in) area ID to mark. +// chf - (in/out) compact heightfield to mark. +void rcMarkBoxArea(const float* bmin, const float* bmax, unsigned char areaId, + rcCompactHeightfield& chf); + +// Marks the area of the convex polygon into the area type of the compact heighfield. +// Params: +// verts - (in) vertices of the convex polygon. +// nverts - (in) number of vertices in the polygon. +// hmin/hmax - (in) min and max height of the polygon. +// areaId - (in) area ID to mark. +// chf - (in/out) compact heightfield to mark. +void rcMarkConvexPolyArea(const float* verts, const int nverts, + const float hmin, const float hmax, unsigned char areaId, + rcCompactHeightfield& chf); + + +// Builds distance field and stores it into the combat heightfield. +// Params: +// chf - (in/out) compact heightfield representing the open space. +// Returns false if operation ran out of memory. +bool rcBuildDistanceField(rcCompactHeightfield& chf); + +// Divides the walkable heighfied into simple regions using watershed partitioning. +// Each region has only one contour and no overlaps. +// The regions are stored in the compact heightfield 'reg' field. +// The regions will be shrinked by the radius of the agent. +// The process sometimes creates small regions. The parameter +// 'minRegionSize' specifies the smallest allowed regions size. +// If the area of a regions is smaller than allowed, the regions is +// removed or merged to neighbour region. +// Params: +// chf - (in/out) compact heightfield representing the open space. +// minRegionSize - (in) the smallest allowed regions size. +// maxMergeRegionSize - (in) the largest allowed regions size which can be merged. +// Returns false if operation ran out of memory. +bool rcBuildRegions(rcCompactHeightfield& chf, + int borderSize, int minRegionSize, int mergeRegionSize); + +// Divides the walkable heighfied into simple regions using simple monotone partitioning. +// Each region has only one contour and no overlaps. +// The regions are stored in the compact heightfield 'reg' field. +// The regions will be shrinked by the radius of the agent. +// The process sometimes creates small regions. The parameter +// 'minRegionSize' specifies the smallest allowed regions size. +// If the area of a regions is smaller than allowed, the regions is +// removed or merged to neighbour region. +// Params: +// chf - (in/out) compact heightfield representing the open space. +// minRegionSize - (in) the smallest allowed regions size. +// maxMergeRegionSize - (in) the largest allowed regions size which can be merged. +// Returns false if operation ran out of memory. +bool rcBuildRegionsMonotone(rcCompactHeightfield& chf, + int borderSize, int minRegionSize, int mergeRegionSize); + +// Builds simplified contours from the regions outlines. +// Params: +// chf - (in) compact heightfield which has regions set. +// maxError - (in) maximum allowed distance between simplified countour and cells. +// maxEdgeLen - (in) maximum allowed contour edge length in cells. +// cset - (out) Resulting contour set. +// Returns false if operation ran out of memory. +bool rcBuildContours(rcCompactHeightfield& chf, + const float maxError, const int maxEdgeLen, + rcContourSet& cset); + +// Builds connected convex polygon mesh from contour polygons. +// Params: +// cset - (in) contour set. +// nvp - (in) maximum number of vertices per polygon. +// mesh - (out) poly mesh. +// Returns false if operation ran out of memory. +bool rcBuildPolyMesh(rcContourSet& cset, int nvp, rcPolyMesh& mesh); + +bool rcMergePolyMeshes(rcPolyMesh** meshes, const int nmeshes, rcPolyMesh& mesh); + +// Builds detail triangle mesh for each polygon in the poly mesh. +// Params: +// mesh - (in) poly mesh to detail. +// chf - (in) compacy height field, used to query height for new vertices. +// sampleDist - (in) spacing between height samples used to generate more detail into mesh. +// sampleMaxError - (in) maximum allowed distance between simplified detail mesh and height sample. +// pmdtl - (out) detail mesh. +// Returns false if operation ran out of memory. +bool rcBuildPolyMeshDetail(const rcPolyMesh& mesh, const rcCompactHeightfield& chf, + const float sampleDist, const float sampleMaxError, + rcPolyMeshDetail& dmesh); + +bool rcMergePolyMeshDetails(rcPolyMeshDetail** meshes, const int nmeshes, rcPolyMeshDetail& mesh); + + +#endif // RECAST_H diff --git a/src/shared/pathfinding/Recast/RecastArea.cpp b/src/shared/pathfinding/Recast/RecastArea.cpp new file mode 100644 index 0000000..cb5f2a8 --- /dev/null +++ b/src/shared/pathfinding/Recast/RecastArea.cpp @@ -0,0 +1,326 @@ +// +// Copyright (c) 2009 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 +#define _USE_MATH_DEFINES +#include +#include +#include +#include +#include "Recast.h" +#include "RecastLog.h" +#include "RecastTimer.h" + + +bool rcErodeArea(unsigned char areaId, int radius, rcCompactHeightfield& chf) +{ + const int w = chf.width; + const int h = chf.height; + + rcTimeVal startTime = rcGetPerformanceTimer(); + + unsigned char* dist = new unsigned char[chf.spanCount]; + if (!dist) + return false; + + // Init distance. + memset(dist, 0xff, sizeof(unsigned char)*chf.spanCount); + + // Mark boundary cells. + for (int y = 0; y < h; ++y) + { + for (int x = 0; x < w; ++x) + { + const rcCompactCell& c = chf.cells[x+y*w]; + for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i) + { + if (chf.areas[i] != RC_NULL_AREA) + { + const rcCompactSpan& s = chf.spans[i]; + int nc = 0; + for (int dir = 0; dir < 4; ++dir) + { + if (rcGetCon(s, dir) != 0xf) + { + const int ax = x + rcGetDirOffsetX(dir); + const int ay = y + rcGetDirOffsetY(dir); + const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, dir); + if (chf.areas[ai] == areaId) + nc++; + } + } + // At least one missing neighbour. + if (nc != 4) + dist[i] = 0; + } + } + } + } + + unsigned char nd; + + // Pass 1 + for (int y = 0; y < h; ++y) + { + for (int x = 0; x < w; ++x) + { + const rcCompactCell& c = chf.cells[x+y*w]; + for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i) + { + const rcCompactSpan& s = chf.spans[i]; + + if (rcGetCon(s, 0) != 0xf) + { + // (-1,0) + const int ax = x + rcGetDirOffsetX(0); + const int ay = y + rcGetDirOffsetY(0); + const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 0); + const rcCompactSpan& as = chf.spans[ai]; + nd = (unsigned char)rcMin((int)dist[ai]+2, 255); + if (nd < dist[i]) + dist[i] = nd; + + // (-1,-1) + if (rcGetCon(as, 3) != 0xf) + { + const int aax = ax + rcGetDirOffsetX(3); + const int aay = ay + rcGetDirOffsetY(3); + const int aai = (int)chf.cells[aax+aay*w].index + rcGetCon(as, 3); + nd = (unsigned char)rcMin((int)dist[aai]+3, 255); + if (nd < dist[i]) + dist[i] = nd; + } + } + if (rcGetCon(s, 3) != 0xf) + { + // (0,-1) + const int ax = x + rcGetDirOffsetX(3); + const int ay = y + rcGetDirOffsetY(3); + const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 3); + const rcCompactSpan& as = chf.spans[ai]; + nd = (unsigned char)rcMin((int)dist[ai]+2, 255); + if (nd < dist[i]) + dist[i] = nd; + + // (1,-1) + if (rcGetCon(as, 2) != 0xf) + { + const int aax = ax + rcGetDirOffsetX(2); + const int aay = ay + rcGetDirOffsetY(2); + const int aai = (int)chf.cells[aax+aay*w].index + rcGetCon(as, 2); + nd = (unsigned char)rcMin((int)dist[aai]+3, 255); + if (nd < dist[i]) + dist[i] = nd; + } + } + } + } + } + + // Pass 2 + for (int y = h-1; y >= 0; --y) + { + for (int x = w-1; x >= 0; --x) + { + const rcCompactCell& c = chf.cells[x+y*w]; + for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i) + { + const rcCompactSpan& s = chf.spans[i]; + + if (rcGetCon(s, 2) != 0xf) + { + // (1,0) + const int ax = x + rcGetDirOffsetX(2); + const int ay = y + rcGetDirOffsetY(2); + const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 2); + const rcCompactSpan& as = chf.spans[ai]; + nd = (unsigned char)rcMin((int)dist[ai]+2, 255); + if (nd < dist[i]) + dist[i] = nd; + + // (1,1) + if (rcGetCon(as, 1) != 0xf) + { + const int aax = ax + rcGetDirOffsetX(1); + const int aay = ay + rcGetDirOffsetY(1); + const int aai = (int)chf.cells[aax+aay*w].index + rcGetCon(as, 1); + nd = (unsigned char)rcMin((int)dist[aai]+3, 255); + if (nd < dist[i]) + dist[i] = nd; + } + } + if (rcGetCon(s, 1) != 0xf) + { + // (0,1) + const int ax = x + rcGetDirOffsetX(1); + const int ay = y + rcGetDirOffsetY(1); + const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 1); + const rcCompactSpan& as = chf.spans[ai]; + nd = (unsigned char)rcMin((int)dist[ai]+2, 255); + if (nd < dist[i]) + dist[i] = nd; + + // (-1,1) + if (rcGetCon(as, 0) != 0xf) + { + const int aax = ax + rcGetDirOffsetX(0); + const int aay = ay + rcGetDirOffsetY(0); + const int aai = (int)chf.cells[aax+aay*w].index + rcGetCon(as, 0); + nd = (unsigned char)rcMin((int)dist[aai]+3, 255); + if (nd < dist[i]) + dist[i] = nd; + } + } + } + } + } + + const unsigned char thr = (unsigned char)(radius*2); + for (int i = 0; i < chf.spanCount; ++i) + if (dist[i] < thr) + chf.areas[i] = 0; + + delete [] dist; + + rcTimeVal endTime = rcGetPerformanceTimer(); + + if (rcGetBuildTimes()) + { + rcGetBuildTimes()->erodeArea += rcGetDeltaTimeUsec(startTime, endTime); + } + + return true; +} + +void rcMarkBoxArea(const float* bmin, const float* bmax, unsigned char areaId, + rcCompactHeightfield& chf) +{ + int minx = (int)((bmin[0]-chf.bmin[0])/chf.cs); + int miny = (int)((bmin[1]-chf.bmin[1])/chf.ch); + int minz = (int)((bmin[2]-chf.bmin[2])/chf.cs); + int maxx = (int)((bmax[0]-chf.bmin[0])/chf.cs); + int maxy = (int)((bmax[1]-chf.bmin[1])/chf.ch); + int maxz = (int)((bmax[2]-chf.bmin[2])/chf.cs); + + if (maxx < 0) return; + if (minx >= chf.width) return; + if (maxz < 0) return; + if (minz >= chf.height) return; + + if (minx < 0) minx = 0; + if (maxx >= chf.width) maxx = chf.width-1; + if (minz < 0) minz = 0; + if (maxz >= chf.height) maxz = chf.height-1; + + for (int z = minz; z <= maxz; ++z) + { + for (int x = minx; x <= maxx; ++x) + { + const rcCompactCell& c = chf.cells[x+z*chf.width]; + for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i) + { + rcCompactSpan& s = chf.spans[i]; + if ((int)s.y >= miny && (int)s.y <= maxy) + { + if (areaId < chf.areas[i]) + chf.areas[i] = areaId; + } + } + } + } +} + + +static int pointInPoly(int nvert, const float* verts, const float* p) +{ + int i, j, c = 0; + for (i = 0, j = nvert-1; i < nvert; j = i++) + { + const float* vi = &verts[i*3]; + const float* vj = &verts[j*3]; + if (((vi[2] > p[2]) != (vj[2] > p[2])) && + (p[0] < (vj[0]-vi[0]) * (p[2]-vi[2]) / (vj[2]-vi[2]) + vi[0]) ) + c = !c; + } + return c; +} + +void rcMarkConvexPolyArea(const float* verts, const int nverts, + const float hmin, const float hmax, unsigned char areaId, + rcCompactHeightfield& chf) +{ + float bmin[3], bmax[3]; + vcopy(bmin, verts); + vcopy(bmax, verts); + for (int i = 1; i < nverts; ++i) + { + vmin(bmin, &verts[i*3]); + vmax(bmax, &verts[i*3]); + } + bmin[1] = hmin; + bmax[1] = hmax; + + int minx = (int)((bmin[0]-chf.bmin[0])/chf.cs); + int miny = (int)((bmin[1]-chf.bmin[1])/chf.ch); + int minz = (int)((bmin[2]-chf.bmin[2])/chf.cs); + int maxx = (int)((bmax[0]-chf.bmin[0])/chf.cs); + int maxy = (int)((bmax[1]-chf.bmin[1])/chf.ch); + int maxz = (int)((bmax[2]-chf.bmin[2])/chf.cs); + + if (maxx < 0) return; + if (minx >= chf.width) return; + if (maxz < 0) return; + if (minz >= chf.height) return; + + if (minx < 0) minx = 0; + if (maxx >= chf.width) maxx = chf.width-1; + if (minz < 0) minz = 0; + if (maxz >= chf.height) maxz = chf.height-1; + + + // TODO: Optimize. + for (int z = minz; z <= maxz; ++z) + { + for (int x = minx; x <= maxx; ++x) + { + const rcCompactCell& c = chf.cells[x+z*chf.width]; + for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i) + { + rcCompactSpan& s = chf.spans[i]; + if ((int)s.y >= miny && (int)s.y <= maxy) + { + if (areaId < chf.areas[i]) + { + float p[3]; + p[0] = chf.bmin[0] + (x+0.5f)*chf.cs; + p[1] = 0; + p[2] = chf.bmin[2] + (z+0.5f)*chf.cs; + + if (pointInPoly(nverts, verts, p)) + { + chf.areas[i] = areaId; + } + } + } + } + } + } + + +} + diff --git a/src/shared/pathfinding/Recast/RecastContour.cpp b/src/shared/pathfinding/Recast/RecastContour.cpp new file mode 100644 index 0000000..024eec4 --- /dev/null +++ b/src/shared/pathfinding/Recast/RecastContour.cpp @@ -0,0 +1,772 @@ +// +// Copyright (c) 2009 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. +// + +#define _USE_MATH_DEFINES +#include +#include +#include +#include "Recast.h" +#include "RecastLog.h" +#include "RecastTimer.h" + + +static int getCornerHeight(int x, int y, int i, int dir, + const rcCompactHeightfield& chf, + bool& isBorderVertex) +{ + const rcCompactSpan& s = chf.spans[i]; + int ch = (int)s.y; + int dirp = (dir+1) & 0x3; + + unsigned int regs[4] = {0,0,0,0}; + + // Combine region and area codes in order to prevent + // border vertices which are in between two areas to be removed. + regs[0] = chf.regs[i] | (chf.areas[i] << 16); + + if (rcGetCon(s, dir) != RC_NOT_CONNECTED) + { + const int ax = x + rcGetDirOffsetX(dir); + const int ay = y + rcGetDirOffsetY(dir); + const int ai = (int)chf.cells[ax+ay*chf.width].index + rcGetCon(s, dir); + const rcCompactSpan& as = chf.spans[ai]; + ch = rcMax(ch, (int)as.y); + regs[1] = chf.regs[ai] | (chf.areas[ai] << 16); + if (rcGetCon(as, dirp) != RC_NOT_CONNECTED) + { + const int ax2 = ax + rcGetDirOffsetX(dirp); + const int ay2 = ay + rcGetDirOffsetY(dirp); + const int ai2 = (int)chf.cells[ax2+ay2*chf.width].index + rcGetCon(as, dirp); + const rcCompactSpan& as2 = chf.spans[ai2]; + ch = rcMax(ch, (int)as2.y); + regs[2] = chf.regs[ai2] | (chf.areas[ai2] << 16); + } + } + if (rcGetCon(s, dirp) != RC_NOT_CONNECTED) + { + const int ax = x + rcGetDirOffsetX(dirp); + const int ay = y + rcGetDirOffsetY(dirp); + const int ai = (int)chf.cells[ax+ay*chf.width].index + rcGetCon(s, dirp); + const rcCompactSpan& as = chf.spans[ai]; + ch = rcMax(ch, (int)as.y); + regs[3] = chf.regs[ai] | (chf.areas[ai] << 16); + if (rcGetCon(as, dir) != RC_NOT_CONNECTED) + { + const int ax2 = ax + rcGetDirOffsetX(dir); + const int ay2 = ay + rcGetDirOffsetY(dir); + const int ai2 = (int)chf.cells[ax2+ay2*chf.width].index + rcGetCon(as, dir); + const rcCompactSpan& as2 = chf.spans[ai2]; + ch = rcMax(ch, (int)as2.y); + regs[2] = chf.regs[ai2] | (chf.areas[ai2] << 16); + } + } + + // Check if the vertex is special edge vertex, these vertices will be removed later. + for (int j = 0; j < 4; ++j) + { + const int a = j; + const int b = (j+1) & 0x3; + const int c = (j+2) & 0x3; + const int d = (j+3) & 0x3; + + // The vertex is a border vertex there are two same exterior cells in a row, + // followed by two interior cells and none of the regions are out of bounds. + const bool twoSameExts = (regs[a] & regs[b] & RC_BORDER_REG) != 0 && regs[a] == regs[b]; + const bool twoInts = ((regs[c] | regs[d]) & RC_BORDER_REG) == 0; + const bool intsSameArea = (regs[c]>>16) == (regs[d]>>16); + const bool noZeros = regs[a] != 0 && regs[b] != 0 && regs[c] != 0 && regs[d] != 0; + if (twoSameExts && twoInts && intsSameArea && noZeros) + { + isBorderVertex = true; + break; + } + } + + return ch; +} + +static void walkContour(int x, int y, int i, + rcCompactHeightfield& chf, + unsigned char* flags, rcIntArray& points) +{ + // Choose the first non-connected edge + unsigned char dir = 0; + while ((flags[i] & (1 << dir)) == 0) + dir++; + + unsigned char startDir = dir; + int starti = i; + + const unsigned char area = chf.areas[i]; + + int iter = 0; + while (++iter < 40000) + { + if (flags[i] & (1 << dir)) + { + // Choose the edge corner + bool isBorderVertex = false; + bool isAreaBorder = false; + int px = x; + int py = getCornerHeight(x, y, i, dir, chf, isBorderVertex); + int pz = y; + switch(dir) + { + case 0: pz++; break; + case 1: px++; pz++; break; + case 2: px++; break; + } + int r = 0; + const rcCompactSpan& s = chf.spans[i]; + if (rcGetCon(s, dir) != RC_NOT_CONNECTED) + { + const int ax = x + rcGetDirOffsetX(dir); + const int ay = y + rcGetDirOffsetY(dir); + const int ai = (int)chf.cells[ax+ay*chf.width].index + rcGetCon(s, dir); + r = (int)chf.regs[ai]; + if (area != chf.areas[ai]) + isAreaBorder = true; + } + if (isBorderVertex) + r |= RC_BORDER_VERTEX; + if (isAreaBorder) + r |= RC_AREA_BORDER; + points.push(px); + points.push(py); + points.push(pz); + points.push(r); + + flags[i] &= ~(1 << dir); // Remove visited edges + dir = (dir+1) & 0x3; // Rotate CW + } + else + { + int ni = -1; + const int nx = x + rcGetDirOffsetX(dir); + const int ny = y + rcGetDirOffsetY(dir); + const rcCompactSpan& s = chf.spans[i]; + if (rcGetCon(s, dir) != RC_NOT_CONNECTED) + { + const rcCompactCell& nc = chf.cells[nx+ny*chf.width]; + ni = (int)nc.index + rcGetCon(s, dir); + } + if (ni == -1) + { + // Should not happen. + return; + } + x = nx; + y = ny; + i = ni; + dir = (dir+3) & 0x3; // Rotate CCW + } + + if (starti == i && startDir == dir) + { + break; + } + } +} + +static float distancePtSeg(int x, int y, int z, + int px, int py, int pz, + int qx, int qy, int qz) +{ +/* float pqx = (float)(qx - px); + float pqy = (float)(qy - py); + float pqz = (float)(qz - pz); + float dx = (float)(x - px); + float dy = (float)(y - py); + float dz = (float)(z - pz); + float d = pqx*pqx + pqy*pqy + pqz*pqz; + float t = pqx*dx + pqy*dy + pqz*dz; + if (d > 0) + t /= d; + if (t < 0) + t = 0; + else if (t > 1) + t = 1; + + dx = px + t*pqx - x; + dy = py + t*pqy - y; + dz = pz + t*pqz - z; + + return dx*dx + dy*dy + dz*dz;*/ + + float pqx = (float)(qx - px); + float pqz = (float)(qz - pz); + float dx = (float)(x - px); + float dz = (float)(z - pz); + float d = pqx*pqx + pqz*pqz; + float t = pqx*dx + pqz*dz; + if (d > 0) + t /= d; + if (t < 0) + t = 0; + else if (t > 1) + t = 1; + + dx = px + t*pqx - x; + dz = pz + t*pqz - z; + + return dx*dx + dz*dz; +} + +static void simplifyContour(rcIntArray& points, rcIntArray& simplified, float maxError, int maxEdgeLen) +{ + // Add initial points. + bool noConnections = true; + for (int i = 0; i < points.size(); i += 4) + { + if ((points[i+3] & RC_CONTOUR_REG_MASK) != 0) + { + noConnections = false; + break; + } + } + + if (noConnections) + { + // If there is no connections at all, + // create some initial points for the simplification process. + // Find lower-left and upper-right vertices of the contour. + int llx = points[0]; + int lly = points[1]; + int llz = points[2]; + int lli = 0; + int urx = points[0]; + int ury = points[1]; + int urz = points[2]; + int uri = 0; + for (int i = 0; i < points.size(); i += 4) + { + int x = points[i+0]; + int y = points[i+1]; + int z = points[i+2]; + if (x < llx || (x == llx && z < llz)) + { + llx = x; + lly = y; + llz = z; + lli = i/4; + } + if (x > urx || (x == urx && z > urz)) + { + urx = x; + ury = y; + urz = z; + uri = i/4; + } + } + simplified.push(llx); + simplified.push(lly); + simplified.push(llz); + simplified.push(lli); + + simplified.push(urx); + simplified.push(ury); + simplified.push(urz); + simplified.push(uri); + } + else + { + // The contour has some portals to other regions. + // Add a new point to every location where the region changes. + for (int i = 0, ni = points.size()/4; i < ni; ++i) + { + int ii = (i+1) % ni; + const bool differentRegs = (points[i*4+3] & RC_CONTOUR_REG_MASK) != (points[ii*4+3] & RC_CONTOUR_REG_MASK); + const bool areaBorders = (points[i*4+3] & RC_AREA_BORDER) != (points[ii*4+3] & RC_AREA_BORDER); + if (differentRegs || areaBorders) + { + simplified.push(points[i*4+0]); + simplified.push(points[i*4+1]); + simplified.push(points[i*4+2]); + simplified.push(i); + } + } + } + + // Add points until all raw points are within + // error tolerance to the simplified shape. + const int pn = points.size()/4; + for (int i = 0; i < simplified.size()/4; ) + { + int ii = (i+1) % (simplified.size()/4); + + int ax = simplified[i*4+0]; + int ay = simplified[i*4+1]; + int az = simplified[i*4+2]; + int ai = simplified[i*4+3]; + + int bx = simplified[ii*4+0]; + int by = simplified[ii*4+1]; + int bz = simplified[ii*4+2]; + int bi = simplified[ii*4+3]; + + // Find maximum deviation from the segment. + float maxd = 0; + int maxi = -1; + int ci, cinc, endi; + + // Traverse the segment in lexilogical order so that the + // max deviation is calculated similarly when traversing + // opposite segments. + if (bx > ax || (bx == ax && bz > az)) + { + cinc = 1; + ci = (ai+cinc) % pn; + endi = bi; + } + else + { + cinc = pn-1; + ci = (bi+cinc) % pn; + endi = ai; + } + + // Tesselate only outer edges oredges between areas. + if ((points[ci*4+3] & RC_CONTOUR_REG_MASK) == 0 || + (points[ci*4+3] & RC_AREA_BORDER)) + { + while (ci != endi) + { + float d = distancePtSeg(points[ci*4+0], points[ci*4+1]/4, points[ci*4+2], + ax, ay/4, az, bx, by/4, bz); + if (d > maxd) + { + maxd = d; + maxi = ci; + } + ci = (ci+cinc) % pn; + } + } + + + // If the max deviation is larger than accepted error, + // add new point, else continue to next segment. + if (maxi != -1 && maxd > (maxError*maxError)) + { + // Add space for the new point. + simplified.resize(simplified.size()+4); + int n = simplified.size()/4; + for (int j = n-1; j > i; --j) + { + simplified[j*4+0] = simplified[(j-1)*4+0]; + simplified[j*4+1] = simplified[(j-1)*4+1]; + simplified[j*4+2] = simplified[(j-1)*4+2]; + simplified[j*4+3] = simplified[(j-1)*4+3]; + } + // Add the point. + simplified[(i+1)*4+0] = points[maxi*4+0]; + simplified[(i+1)*4+1] = points[maxi*4+1]; + simplified[(i+1)*4+2] = points[maxi*4+2]; + simplified[(i+1)*4+3] = maxi; + } + else + { + ++i; + } + } + + // Split too long edges. + if (maxEdgeLen > 0) + { + for (int i = 0; i < simplified.size()/4; ) + { + int ii = (i+1) % (simplified.size()/4); + + int ax = simplified[i*4+0]; + int az = simplified[i*4+2]; + int ai = simplified[i*4+3]; + + int bx = simplified[ii*4+0]; + int bz = simplified[ii*4+2]; + int bi = simplified[ii*4+3]; + + // Find maximum deviation from the segment. + int maxi = -1; + int ci = (ai+1) % pn; + + // Tesselate only outer edges. + if ((points[ci*4+3] & RC_CONTOUR_REG_MASK) == 0) + { + int dx = bx - ax; + int dz = bz - az; + if (dx*dx + dz*dz > maxEdgeLen*maxEdgeLen) + { + int n = bi < ai ? (bi+pn - ai) : (bi - ai); + maxi = (ai + n/2) % pn; + } + } + + // If the max deviation is larger than accepted error, + // add new point, else continue to next segment. + if (maxi != -1) + { + // Add space for the new point. + simplified.resize(simplified.size()+4); + int n = simplified.size()/4; + for (int j = n-1; j > i; --j) + { + simplified[j*4+0] = simplified[(j-1)*4+0]; + simplified[j*4+1] = simplified[(j-1)*4+1]; + simplified[j*4+2] = simplified[(j-1)*4+2]; + simplified[j*4+3] = simplified[(j-1)*4+3]; + } + // Add the point. + simplified[(i+1)*4+0] = points[maxi*4+0]; + simplified[(i+1)*4+1] = points[maxi*4+1]; + simplified[(i+1)*4+2] = points[maxi*4+2]; + simplified[(i+1)*4+3] = maxi; + } + else + { + ++i; + } + } + } + + for (int i = 0; i < simplified.size()/4; ++i) + { + // The edge vertex flag is take from the current raw point, + // and the neighbour region is take from the next raw point. + const int ai = (simplified[i*4+3]+1) % pn; + const int bi = simplified[i*4+3]; + simplified[i*4+3] = (points[ai*4+3] & RC_CONTOUR_REG_MASK) | (points[bi*4+3] & RC_BORDER_VERTEX); + } + +} + +static void removeDegenerateSegments(rcIntArray& simplified) +{ + // Remove adjacent vertices which are equal on xz-plane, + // or else the triangulator will get confused. + for (int i = 0; i < simplified.size()/4; ++i) + { + int ni = i+1; + if (ni >= (simplified.size()/4)) + ni = 0; + + if (simplified[i*4+0] == simplified[ni*4+0] && + simplified[i*4+2] == simplified[ni*4+2]) + { + // Degenerate segment, remove. + for (int j = i; j < simplified.size()/4-1; ++j) + { + simplified[j*4+0] = simplified[(j+1)*4+0]; + simplified[j*4+1] = simplified[(j+1)*4+1]; + simplified[j*4+2] = simplified[(j+1)*4+2]; + simplified[j*4+3] = simplified[(j+1)*4+3]; + } + simplified.resize(simplified.size()-4); + } + } +} + +static int calcAreaOfPolygon2D(const int* verts, const int nverts) +{ + int area = 0; + for (int i = 0, j = nverts-1; i < nverts; j=i++) + { + const int* vi = &verts[i*4]; + const int* vj = &verts[j*4]; + area += vi[0] * vj[2] - vj[0] * vi[2]; + } + return (area+1) / 2; +} + +static void getClosestIndices(const int* vertsa, const int nvertsa, + const int* vertsb, const int nvertsb, + int& ia, int& ib) +{ + int closestDist = 0xfffffff; + for (int i = 0; i < nvertsa; ++i) + { + const int* va = &vertsa[i*4]; + for (int j = 0; j < nvertsb; ++j) + { + const int* vb = &vertsb[j*4]; + const int dx = vb[0] - va[0]; + const int dz = vb[2] - va[2]; + const int d = dx*dx + dz*dz; + if (d < closestDist) + { + ia = i; + ib = j; + closestDist = d; + } + } + } +} + +static bool mergeContours(rcContour& ca, rcContour& cb, int ia, int ib) +{ + const int maxVerts = ca.nverts + cb.nverts + 2; + int* verts = new int[maxVerts*4]; + if (!verts) + return false; + + int nv = 0; + + // Copy contour A. + for (int i = 0; i <= ca.nverts; ++i) + { + int* dst = &verts[nv*4]; + const int* src = &ca.verts[((ia+i)%ca.nverts)*4]; + dst[0] = src[0]; + dst[1] = src[1]; + dst[2] = src[2]; + dst[3] = src[3]; + nv++; + } + + // Copy contour B + for (int i = 0; i <= cb.nverts; ++i) + { + int* dst = &verts[nv*4]; + const int* src = &cb.verts[((ib+i)%cb.nverts)*4]; + dst[0] = src[0]; + dst[1] = src[1]; + dst[2] = src[2]; + dst[3] = src[3]; + nv++; + } + + delete [] ca.verts; + ca.verts = verts; + ca.nverts = nv; + + delete [] cb.verts; + cb.verts = 0; + cb.nverts = 0; + + return true; +} + +bool rcBuildContours(rcCompactHeightfield& chf, + const float maxError, const int maxEdgeLen, + rcContourSet& cset) +{ + const int w = chf.width; + const int h = chf.height; + + rcTimeVal startTime = rcGetPerformanceTimer(); + + vcopy(cset.bmin, chf.bmin); + vcopy(cset.bmax, chf.bmax); + cset.cs = chf.cs; + cset.ch = chf.ch; + + int maxContours = rcMax((int)chf.maxRegions, 8); + cset.conts = new rcContour[maxContours]; + if (!cset.conts) + return false; + cset.nconts = 0; + + rcScopedDelete flags = new unsigned char[chf.spanCount]; + if (!flags) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildContours: Out of memory 'flags'."); + return false; + } + + rcTimeVal traceStartTime = rcGetPerformanceTimer(); + + + // Mark boundaries. + for (int y = 0; y < h; ++y) + { + for (int x = 0; x < w; ++x) + { + const rcCompactCell& c = chf.cells[x+y*w]; + for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i) + { + unsigned char res = 0; + const rcCompactSpan& s = chf.spans[i]; + if (!chf.regs[i] || (chf.regs[i] & RC_BORDER_REG)) + { + flags[i] = 0; + continue; + } + for (int dir = 0; dir < 4; ++dir) + { + unsigned short r = 0; + if (rcGetCon(s, dir) != RC_NOT_CONNECTED) + { + const int ax = x + rcGetDirOffsetX(dir); + const int ay = y + rcGetDirOffsetY(dir); + const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, dir); + r = chf.regs[ai]; + } + if (r == chf.regs[i]) + res |= (1 << dir); + } + flags[i] = res ^ 0xf; // Inverse, mark non connected edges. + } + } + } + + rcTimeVal traceEndTime = rcGetPerformanceTimer(); + + rcTimeVal simplifyStartTime = rcGetPerformanceTimer(); + + rcIntArray verts(256); + rcIntArray simplified(64); + + for (int y = 0; y < h; ++y) + { + for (int x = 0; x < w; ++x) + { + const rcCompactCell& c = chf.cells[x+y*w]; + for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i) + { + if (flags[i] == 0 || flags[i] == 0xf) + { + flags[i] = 0; + continue; + } + const unsigned short reg = chf.regs[i]; + if (!reg || (reg & RC_BORDER_REG)) + continue; + const unsigned char area = chf.areas[i]; + + verts.resize(0); + simplified.resize(0); + walkContour(x, y, i, chf, flags, verts); + simplifyContour(verts, simplified, maxError, maxEdgeLen); + removeDegenerateSegments(simplified); + + // Store region->contour remap info. + // Create contour. + if (simplified.size()/4 >= 3) + { + if (cset.nconts >= maxContours) + { + // Allocate more contours. + // This can happen when there are tiny holes in the heighfield. + const int oldMax = maxContours; + maxContours *= 2; + rcContour* newConts = new rcContour[maxContours]; + for (int j = 0; j < cset.nconts; ++j) + { + newConts[j] = cset.conts[j]; + // Reset source pointers to prevent data deletion. + cset.conts[j].verts = 0; + cset.conts[j].rverts = 0; + } + delete [] cset.conts; + cset.conts = newConts; + + if (rcGetLog()) + rcGetLog()->log(RC_LOG_WARNING, "rcBuildContours: Expanding max contours from %d to %d.", oldMax, maxContours); + } + + rcContour* cont = &cset.conts[cset.nconts++]; + + cont->nverts = simplified.size()/4; + cont->verts = new int[cont->nverts*4]; + memcpy(cont->verts, &simplified[0], sizeof(int)*cont->nverts*4); + + cont->nrverts = verts.size()/4; + cont->rverts = new int[cont->nrverts*4]; + memcpy(cont->rverts, &verts[0], sizeof(int)*cont->nrverts*4); + +/* cont->cx = cont->cy = cont->cz = 0; + for (int i = 0; i < cont->nverts; ++i) + { + cont->cx += cont->verts[i*4+0]; + cont->cy += cont->verts[i*4+1]; + cont->cz += cont->verts[i*4+2]; + } + cont->cx /= cont->nverts; + cont->cy /= cont->nverts; + cont->cz /= cont->nverts;*/ + + cont->reg = reg; + cont->area = area; + } + } + } + } + + // Check and merge droppings. + // Sometimes the previous algorithms can fail and create several countours + // per area. This pass will try to merge the holes into the main region. + for (int i = 0; i < cset.nconts; ++i) + { + rcContour& cont = cset.conts[i]; + // Check if the contour is would backwards. + if (calcAreaOfPolygon2D(cont.verts, cont.nverts) < 0) + { + // Find another contour which has the same region ID. + int mergeIdx = -1; + for (int j = 0; j < cset.nconts; ++j) + { + if (i == j) continue; + if (cset.conts[j].nverts && cset.conts[j].reg == cont.reg) + { + // Make sure the polygon is correctly oriented. + if (calcAreaOfPolygon2D(cset.conts[j].verts, cset.conts[j].nverts)) + { + mergeIdx = j; + break; + } + } + } + if (mergeIdx == -1) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_WARNING, "rcBuildContours: Could not find merge target for bad contour %d.", i); + } + else + { + rcContour& mcont = cset.conts[mergeIdx]; + // Merge by closest points. + int ia = 0, ib = 0; + getClosestIndices(mcont.verts, mcont.nverts, cont.verts, cont.nverts, ia, ib); + if (!mergeContours(mcont, cont, ia, ib)) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_WARNING, "rcBuildContours: Failed to merge contours %d and %d.", i, mergeIdx); + } + } + } + } + + rcTimeVal simplifyEndTime = rcGetPerformanceTimer(); + + rcTimeVal endTime = rcGetPerformanceTimer(); + +// if (rcGetLog()) +// { +// rcGetLog()->log(RC_LOG_PROGRESS, "Create contours: %.3f ms", rcGetDeltaTimeUsec(startTime, endTime)/1000.0f); +// rcGetLog()->log(RC_LOG_PROGRESS, " - boundary: %.3f ms", rcGetDeltaTimeUsec(boundaryStartTime, boundaryEndTime)/1000.0f); +// rcGetLog()->log(RC_LOG_PROGRESS, " - contour: %.3f ms", rcGetDeltaTimeUsec(contourStartTime, contourEndTime)/1000.0f); +// } + + if (rcGetBuildTimes()) + { + rcGetBuildTimes()->buildContours += rcGetDeltaTimeUsec(startTime, endTime); + rcGetBuildTimes()->buildContoursTrace += rcGetDeltaTimeUsec(traceStartTime, traceEndTime); + rcGetBuildTimes()->buildContoursSimplify += rcGetDeltaTimeUsec(simplifyStartTime, simplifyEndTime); + } + + return true; +} diff --git a/src/shared/pathfinding/Recast/RecastFilter.cpp b/src/shared/pathfinding/Recast/RecastFilter.cpp new file mode 100644 index 0000000..59110ce --- /dev/null +++ b/src/shared/pathfinding/Recast/RecastFilter.cpp @@ -0,0 +1,187 @@ +// +// Copyright (c) 2009 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. +// + +#define _USE_MATH_DEFINES +#include +#include +#include "Recast.h" +#include "RecastLog.h" +#include "RecastTimer.h" + + +// TODO: Missuses ledge flag, must be called before rcFilterLedgeSpans! +void rcFilterLowHangingWalkableObstacles(const int walkableClimb, rcHeightfield& solid) +{ + const int w = solid.width; + const int h = solid.height; + + for (int y = 0; y < h; ++y) + { + for (int x = 0; x < w; ++x) + { + rcSpan* ps = 0; + for (rcSpan* s = solid.spans[x + y*w]; s; ps = s, s = s->next) + { + const bool walkable = (s->flags & RC_WALKABLE) != 0; + const bool previousWalkable = ps && (ps->flags & RC_WALKABLE) != 0; + // If current span is not walkable, but there is walkable + // span just below it, mark the span above it walkable too. + // Missuse the edge flag so that walkable flag cannot propagate + // past multiple non-walkable objects. + if (!walkable && previousWalkable) + { + if (rcAbs((int)s->smax - (int)ps->smax) <= walkableClimb) + s->flags |= RC_LEDGE; + } + } + // Transfer "fake ledges" to walkables. + for (rcSpan* s = solid.spans[x + y*w]; s; ps = s, s = s->next) + { + if (s->flags & RC_LEDGE) + s->flags |= RC_WALKABLE; + s->flags &= ~RC_LEDGE; + } + } + } +} + +void rcFilterLedgeSpans(const int walkableHeight, + const int walkableClimb, + rcHeightfield& solid) +{ + rcTimeVal startTime = rcGetPerformanceTimer(); + + const int w = solid.width; + const int h = solid.height; + const int MAX_HEIGHT = 0xffff; + + // Mark border spans. + for (int y = 0; y < h; ++y) + { + for (int x = 0; x < w; ++x) + { + for (rcSpan* s = solid.spans[x + y*w]; s; s = s->next) + { + // Skip non walkable spans. + if ((s->flags & RC_WALKABLE) == 0) + continue; + + const int bot = (int)(s->smax); + const int top = s->next ? (int)(s->next->smin) : MAX_HEIGHT; + + // Find neighbours minimum height. + int minh = MAX_HEIGHT; + + // Min and max height of accessible neighbours. + int asmin = s->smax; + int asmax = s->smax; + + for (int dir = 0; dir < 4; ++dir) + { + int dx = x + rcGetDirOffsetX(dir); + int dy = y + rcGetDirOffsetY(dir); + // Skip neighbours which are out of bounds. + if (dx < 0 || dy < 0 || dx >= w || dy >= h) + { + minh = rcMin(minh, -walkableClimb - bot); + continue; + } + + // From minus infinity to the first span. + rcSpan* ns = solid.spans[dx + dy*w]; + int nbot = -walkableClimb; + int ntop = ns ? (int)ns->smin : MAX_HEIGHT; + // Skip neightbour if the gap between the spans is too small. + if (rcMin(top,ntop) - rcMax(bot,nbot) > walkableHeight) + minh = rcMin(minh, nbot - bot); + + // Rest of the spans. + for (ns = solid.spans[dx + dy*w]; ns; ns = ns->next) + { + nbot = (int)ns->smax; + ntop = ns->next ? (int)ns->next->smin : MAX_HEIGHT; + // Skip neightbour if the gap between the spans is too small. + if (rcMin(top,ntop) - rcMax(bot,nbot) > walkableHeight) + { + minh = rcMin(minh, nbot - bot); + + // Find min/max accessible neighbour height. + if (rcAbs(nbot - bot) <= walkableClimb) + { + if (nbot < asmin) asmin = nbot; + if (nbot > asmax) asmax = nbot; + } + + } + } + } + + // The current span is close to a ledge if the drop to any + // neighbour span is less than the walkableClimb. + if (minh < -walkableClimb) + s->flags |= RC_LEDGE; + + // If the difference between all neighbours is too large, + // we are at steep slope, mark the span as ledge. + if ((asmax - asmin) > walkableClimb) + { + s->flags |= RC_LEDGE; + } + } + } + } + + rcTimeVal endTime = rcGetPerformanceTimer(); +// if (rcGetLog()) +// rcGetLog()->log(RC_LOG_PROGRESS, "Filter border: %.3f ms", rcGetDeltaTimeUsec(startTime, endTime)/1000.0f); + if (rcGetBuildTimes()) + rcGetBuildTimes()->filterBorder += rcGetDeltaTimeUsec(startTime, endTime); +} + +void rcFilterWalkableLowHeightSpans(int walkableHeight, + rcHeightfield& solid) +{ + rcTimeVal startTime = rcGetPerformanceTimer(); + + const int w = solid.width; + const int h = solid.height; + const int MAX_HEIGHT = 0xffff; + + // Remove walkable flag from spans which do not have enough + // space above them for the agent to stand there. + for (int y = 0; y < h; ++y) + { + for (int x = 0; x < w; ++x) + { + for (rcSpan* s = solid.spans[x + y*w]; s; s = s->next) + { + const int bot = (int)(s->smax); + const int top = s->next ? (int)(s->next->smin) : MAX_HEIGHT; + if ((top - bot) <= walkableHeight) + s->flags &= ~RC_WALKABLE; + } + } + } + + rcTimeVal endTime = rcGetPerformanceTimer(); + +// if (rcGetLog()) +// rcGetLog()->log(RC_LOG_PROGRESS, "Filter walkable: %.3f ms", rcGetDeltaTimeUsec(startTime, endTime)/1000.0f); + if (rcGetBuildTimes()) + rcGetBuildTimes()->filterWalkable += rcGetDeltaTimeUsec(startTime, endTime); +} diff --git a/src/shared/pathfinding/Recast/RecastLog.cpp b/src/shared/pathfinding/Recast/RecastLog.cpp new file mode 100644 index 0000000..2786804 --- /dev/null +++ b/src/shared/pathfinding/Recast/RecastLog.cpp @@ -0,0 +1,77 @@ +// +// Copyright (c) 2009 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 "RecastLog.h" +#include +#include + +static rcLog* g_log = 0; +static rcBuildTimes* g_btimes = 0; + +rcLog::rcLog() : + m_messageCount(0), + m_textPoolSize(0) +{ +} + +rcLog::~rcLog() +{ + if (g_log == this) + g_log = 0; +} + +void rcLog::log(rcLogCategory category, const char* format, ...) +{ + if (m_messageCount >= MAX_MESSAGES) + return; + char* dst = &m_textPool[m_textPoolSize]; + int n = TEXT_POOL_SIZE - m_textPoolSize; + if (n < 2) + return; + // Store category + *dst = (char)category; + n--; + // Store message + va_list ap; + va_start(ap, format); + int ret = vsnprintf(dst+1, n-1, format, ap); + va_end(ap); + if (ret > 0) + m_textPoolSize += ret+2; + m_messages[m_messageCount++] = dst; +} + +void rcSetLog(rcLog* log) +{ + g_log = log; +} + +rcLog* rcGetLog() +{ + return g_log; +} + +void rcSetBuildTimes(rcBuildTimes* btimes) +{ + g_btimes = btimes; +} + +rcBuildTimes* rcGetBuildTimes() +{ + return g_btimes; +} diff --git a/src/shared/pathfinding/Recast/RecastLog.h b/src/shared/pathfinding/Recast/RecastLog.h new file mode 100644 index 0000000..d9cd4c3 --- /dev/null +++ b/src/shared/pathfinding/Recast/RecastLog.h @@ -0,0 +1,81 @@ +// +// Copyright (c) 2009 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#ifndef RECAST_LOG_H +#define RECAST_LOG_H + +enum rcLogCategory +{ + RC_LOG_PROGRESS = 1, + RC_LOG_WARNING, + RC_LOG_ERROR, +}; + +class rcLog +{ +public: + rcLog(); + ~rcLog(); + + void log(rcLogCategory category, const char* format, ...); + inline void clear() { m_messageCount = 0; m_textPoolSize = 0; } + inline int getMessageCount() const { return m_messageCount; } + inline char getMessageType(int i) const { return *m_messages[i]; } + inline const char* getMessageText(int i) const { return m_messages[i]+1; } + +private: + static const int MAX_MESSAGES = 1000; + const char* m_messages[MAX_MESSAGES]; + int m_messageCount; + static const int TEXT_POOL_SIZE = 8000; + char m_textPool[TEXT_POOL_SIZE]; + int m_textPoolSize; +}; + +struct rcBuildTimes +{ + int rasterizeTriangles; + int buildCompact; + int buildContours; + int buildContoursTrace; + int buildContoursSimplify; + int filterBorder; + int filterWalkable; + int filterMarkReachable; + int buildPolymesh; + int erodeArea; + int buildDistanceField; + int buildDistanceFieldDist; + int buildDistanceFieldBlur; + int buildRegions; + int buildRegionsReg; + int buildRegionsExp; + int buildRegionsFlood; + int buildRegionsFilter; + int buildDetailMesh; + int mergePolyMesh; + int mergePolyMeshDetail; +}; + +void rcSetLog(rcLog* log); +rcLog* rcGetLog(); + +void rcSetBuildTimes(rcBuildTimes* btimes); +rcBuildTimes* rcGetBuildTimes(); + +#endif // RECAST_LOG_H diff --git a/src/shared/pathfinding/Recast/RecastMesh.cpp b/src/shared/pathfinding/Recast/RecastMesh.cpp new file mode 100644 index 0000000..f364dba --- /dev/null +++ b/src/shared/pathfinding/Recast/RecastMesh.cpp @@ -0,0 +1,1234 @@ +// +// Copyright (c) 2009 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. +// + +#define _USE_MATH_DEFINES +#include +#include +#include +#include "Recast.h" +#include "RecastLog.h" +#include "RecastTimer.h" + + +struct rcEdge +{ + unsigned short vert[2]; + unsigned short polyEdge[2]; + unsigned short poly[2]; +}; + +static bool buildMeshAdjacency(unsigned short* polys, const int npolys, + const int nverts, const int vertsPerPoly) +{ + // Based on code by Eric Lengyel from: + // http://www.terathon.com/code/edges.php + + int maxEdgeCount = npolys*vertsPerPoly; + unsigned short* firstEdge = new unsigned short[nverts + maxEdgeCount]; + if (!firstEdge) + return false; + unsigned short* nextEdge = firstEdge + nverts; + int edgeCount = 0; + + rcEdge* edges = new rcEdge[maxEdgeCount]; + if (!edges) + return false; + + for (int i = 0; i < nverts; i++) + firstEdge[i] = RC_MESH_NULL_IDX; + + for (int i = 0; i < npolys; ++i) + { + unsigned short* t = &polys[i*vertsPerPoly*2]; + for (int j = 0; j < vertsPerPoly; ++j) + { + unsigned short v0 = t[j]; + unsigned short v1 = (j+1 >= vertsPerPoly || t[j+1] == RC_MESH_NULL_IDX) ? t[0] : t[j+1]; + if (v0 < v1) + { + rcEdge& edge = edges[edgeCount]; + edge.vert[0] = v0; + edge.vert[1] = v1; + edge.poly[0] = (unsigned short)i; + edge.polyEdge[0] = (unsigned short)j; + edge.poly[1] = (unsigned short)i; + edge.polyEdge[1] = 0; + // Insert edge + nextEdge[edgeCount] = firstEdge[v0]; + firstEdge[v0] = edgeCount; + edgeCount++; + } + } + } + + for (int i = 0; i < npolys; ++i) + { + unsigned short* t = &polys[i*vertsPerPoly*2]; + for (int j = 0; j < vertsPerPoly; ++j) + { + unsigned short v0 = t[j]; + unsigned short v1 = (j+1 >= vertsPerPoly || t[j+1] == RC_MESH_NULL_IDX) ? t[0] : t[j+1]; + if (v0 > v1) + { + for (unsigned short e = firstEdge[v1]; e != RC_MESH_NULL_IDX; e = nextEdge[e]) + { + rcEdge& edge = edges[e]; + if (edge.vert[1] == v0 && edge.poly[0] == edge.poly[1]) + { + edge.poly[1] = (unsigned short)i; + edge.polyEdge[1] = (unsigned short)j; + break; + } + } + } + } + } + + // Store adjacency + for (int i = 0; i < edgeCount; ++i) + { + const rcEdge& e = edges[i]; + if (e.poly[0] != e.poly[1]) + { + unsigned short* p0 = &polys[e.poly[0]*vertsPerPoly*2]; + unsigned short* p1 = &polys[e.poly[1]*vertsPerPoly*2]; + p0[vertsPerPoly + e.polyEdge[0]] = e.poly[1]; + p1[vertsPerPoly + e.polyEdge[1]] = e.poly[0]; + } + } + + delete [] firstEdge; + delete [] edges; + + return true; +} + + +static const int VERTEX_BUCKET_COUNT = (1<<12); + +inline int computeVertexHash(int x, int y, int z) +{ + const unsigned int h1 = 0x8da6b343; // Large multiplicative constants; + const unsigned int h2 = 0xd8163841; // here arbitrarily chosen primes + const unsigned int h3 = 0xcb1ab31f; + unsigned int n = h1 * x + h2 * y + h3 * z; + return (int)(n & (VERTEX_BUCKET_COUNT-1)); +} + +static int addVertex(unsigned short x, unsigned short y, unsigned short z, + unsigned short* verts, int* firstVert, int* nextVert, int& nv) +{ + int bucket = computeVertexHash(x, 0, z); + int i = firstVert[bucket]; + + while (i != -1) + { + const unsigned short* v = &verts[i*3]; + if (v[0] == x && (rcAbs(v[1] - y) <= 2) && v[2] == z) + return i; + i = nextVert[i]; // next + } + + // Could not find, create new. + i = nv; nv++; + unsigned short* v = &verts[i*3]; + v[0] = x; + v[1] = y; + v[2] = z; + nextVert[i] = firstVert[bucket]; + firstVert[bucket] = i; + + return i; +} + +inline int prev(int i, int n) { return i-1 >= 0 ? i-1 : n-1; } +inline int next(int i, int n) { return i+1 < n ? i+1 : 0; } + +inline int area2(const int* a, const int* b, const int* c) +{ + return (b[0] - a[0]) * (c[2] - a[2]) - (c[0] - a[0]) * (b[2] - a[2]); +} + +// Exclusive or: true iff exactly one argument is true. +// The arguments are negated to ensure that they are 0/1 +// values. Then the bitwise Xor operator may apply. +// (This idea is due to Michael Baldwin.) +inline bool xorb(bool x, bool y) +{ + return !x ^ !y; +} + +// Returns true iff c is strictly to the left of the directed +// line through a to b. +inline bool left(const int* a, const int* b, const int* c) +{ + return area2(a, b, c) < 0; +} + +inline bool leftOn(const int* a, const int* b, const int* c) +{ + return area2(a, b, c) <= 0; +} + +inline bool collinear(const int* a, const int* b, const int* c) +{ + return area2(a, b, c) == 0; +} + +// Returns true iff ab properly intersects cd: they share +// a point interior to both segments. The properness of the +// intersection is ensured by using strict leftness. +bool intersectProp(const int* a, const int* b, const int* c, const int* d) +{ + // Eliminate improper cases. + if (collinear(a,b,c) || collinear(a,b,d) || + collinear(c,d,a) || collinear(c,d,b)) + return false; + + return xorb(left(a,b,c), left(a,b,d)) && xorb(left(c,d,a), left(c,d,b)); +} + +// Returns T iff (a,b,c) are collinear and point c lies +// on the closed segement ab. +static bool between(const int* a, const int* b, const int* c) +{ + if (!collinear(a, b, c)) + return false; + // If ab not vertical, check betweenness on x; else on y. + if (a[0] != b[0]) + return ((a[0] <= c[0]) && (c[0] <= b[0])) || ((a[0] >= c[0]) && (c[0] >= b[0])); + else + return ((a[2] <= c[2]) && (c[2] <= b[2])) || ((a[2] >= c[2]) && (c[2] >= b[2])); +} + +// Returns true iff segments ab and cd intersect, properly or improperly. +static bool intersect(const int* a, const int* b, const int* c, const int* d) +{ + if (intersectProp(a, b, c, d)) + return true; + else if (between(a, b, c) || between(a, b, d) || + between(c, d, a) || between(c, d, b)) + return true; + else + return false; +} + +static bool vequal(const int* a, const int* b) +{ + return a[0] == b[0] && a[2] == b[2]; +} + +// Returns T iff (v_i, v_j) is a proper internal *or* external +// diagonal of P, *ignoring edges incident to v_i and v_j*. +static bool diagonalie(int i, int j, int n, const int* verts, int* indices) +{ + const int* d0 = &verts[(indices[i] & 0x0fffffff) * 4]; + const int* d1 = &verts[(indices[j] & 0x0fffffff) * 4]; + + // For each edge (k,k+1) of P + for (int k = 0; k < n; k++) + { + int k1 = next(k, n); + // Skip edges incident to i or j + if (!((k == i) || (k1 == i) || (k == j) || (k1 == j))) + { + const int* p0 = &verts[(indices[k] & 0x0fffffff) * 4]; + const int* p1 = &verts[(indices[k1] & 0x0fffffff) * 4]; + + if (vequal(d0, p0) || vequal(d1, p0) || vequal(d0, p1) || vequal(d1, p1)) + continue; + + if (intersect(d0, d1, p0, p1)) + return false; + } + } + return true; +} + +// Returns true iff the diagonal (i,j) is strictly internal to the +// polygon P in the neighborhood of the i endpoint. +static bool inCone(int i, int j, int n, const int* verts, int* indices) +{ + const int* pi = &verts[(indices[i] & 0x0fffffff) * 4]; + const int* pj = &verts[(indices[j] & 0x0fffffff) * 4]; + const int* pi1 = &verts[(indices[next(i, n)] & 0x0fffffff) * 4]; + const int* pin1 = &verts[(indices[prev(i, n)] & 0x0fffffff) * 4]; + + // If P[i] is a convex vertex [ i+1 left or on (i-1,i) ]. + if (leftOn(pin1, pi, pi1)) + return left(pi, pj, pin1) && left(pj, pi, pi1); + // Assume (i-1,i,i+1) not collinear. + // else P[i] is reflex. + return !(leftOn(pi, pj, pi1) && leftOn(pj, pi, pin1)); +} + +// Returns T iff (v_i, v_j) is a proper internal +// diagonal of P. +static bool diagonal(int i, int j, int n, const int* verts, int* indices) +{ + return inCone(i, j, n, verts, indices) && diagonalie(i, j, n, verts, indices); +} + +int triangulate(int n, const int* verts, int* indices, int* tris) +{ + int ntris = 0; + int* dst = tris; + + // The last bit of the index is used to indicate if the vertex can be removed. + for (int i = 0; i < n; i++) + { + int i1 = next(i, n); + int i2 = next(i1, n); + if (diagonal(i, i2, n, verts, indices)) + indices[i1] |= 0x80000000; + } + + while (n > 3) + { + int minLen = -1; + int mini = -1; + for (int i = 0; i < n; i++) + { + int i1 = next(i, n); + if (indices[i1] & 0x80000000) + { + const int* p0 = &verts[(indices[i] & 0x0fffffff) * 4]; + const int* p2 = &verts[(indices[next(i1, n)] & 0x0fffffff) * 4]; + + int dx = p2[0] - p0[0]; + int dy = p2[2] - p0[2]; + int len = dx*dx + dy*dy; + + if (minLen < 0 || len < minLen) + { + minLen = len; + mini = i; + } + } + } + + if (mini == -1) + { + // Should not happen. + if (rcGetLog()) + rcGetLog()->log(RC_LOG_WARNING, "triangulate: Failed to triangulate polygon."); +/* printf("mini == -1 ntris=%d n=%d\n", ntris, n); + for (int i = 0; i < n; i++) + { + printf("%d ", indices[i] & 0x0fffffff); + } + printf("\n");*/ + return -ntris; + } + + int i = mini; + int i1 = next(i, n); + int i2 = next(i1, n); + + *dst++ = indices[i] & 0x0fffffff; + *dst++ = indices[i1] & 0x0fffffff; + *dst++ = indices[i2] & 0x0fffffff; + ntris++; + + // Removes P[i1] by copying P[i+1]...P[n-1] left one index. + n--; + for (int k = i1; k < n; k++) + indices[k] = indices[k+1]; + + if (i1 >= n) i1 = 0; + i = prev(i1,n); + // Update diagonal flags. + if (diagonal(prev(i, n), i1, n, verts, indices)) + indices[i] |= 0x80000000; + else + indices[i] &= 0x0fffffff; + + if (diagonal(i, next(i1, n), n, verts, indices)) + indices[i1] |= 0x80000000; + else + indices[i1] &= 0x0fffffff; + } + + // Append the remaining triangle. + *dst++ = indices[0] & 0x0fffffff; + *dst++ = indices[1] & 0x0fffffff; + *dst++ = indices[2] & 0x0fffffff; + ntris++; + + return ntris; +} + +static int countPolyVerts(const unsigned short* p, const int nvp) +{ + for (int i = 0; i < nvp; ++i) + if (p[i] == RC_MESH_NULL_IDX) + return i; + return nvp; +} + +inline bool uleft(const unsigned short* a, const unsigned short* b, const unsigned short* c) +{ + return ((int)b[0] - (int)a[0]) * ((int)c[2] - (int)a[2]) - + ((int)c[0] - (int)a[0]) * ((int)b[2] - (int)a[2]) < 0; +} + +static int getPolyMergeValue(unsigned short* pa, unsigned short* pb, + const unsigned short* verts, int& ea, int& eb, + const int nvp) +{ + const int na = countPolyVerts(pa, nvp); + const int nb = countPolyVerts(pb, nvp); + + // If the merged polygon would be too big, do not merge. + if (na+nb-2 > nvp) + return -1; + + // Check if the polygons share an edge. + ea = -1; + eb = -1; + + for (int i = 0; i < na; ++i) + { + unsigned short va0 = pa[i]; + unsigned short va1 = pa[(i+1) % na]; + if (va0 > va1) + rcSwap(va0, va1); + for (int j = 0; j < nb; ++j) + { + unsigned short vb0 = pb[j]; + unsigned short vb1 = pb[(j+1) % nb]; + if (vb0 > vb1) + rcSwap(vb0, vb1); + if (va0 == vb0 && va1 == vb1) + { + ea = i; + eb = j; + break; + } + } + } + + // No common edge, cannot merge. + if (ea == -1 || eb == -1) + return -1; + + // Check to see if the merged polygon would be convex. + unsigned short va, vb, vc; + + va = pa[(ea+na-1) % na]; + vb = pa[ea]; + vc = pb[(eb+2) % nb]; + if (!uleft(&verts[va*3], &verts[vb*3], &verts[vc*3])) + return -1; + + va = pb[(eb+nb-1) % nb]; + vb = pb[eb]; + vc = pa[(ea+2) % na]; + if (!uleft(&verts[va*3], &verts[vb*3], &verts[vc*3])) + return -1; + + va = pa[ea]; + vb = pa[(ea+1)%na]; + + int dx = (int)verts[va*3+0] - (int)verts[vb*3+0]; + int dy = (int)verts[va*3+2] - (int)verts[vb*3+2]; + + return dx*dx + dy*dy; +} + +static void mergePolys(unsigned short* pa, unsigned short* pb, int ea, int eb, + unsigned short* tmp, const int nvp) +{ + const int na = countPolyVerts(pa, nvp); + const int nb = countPolyVerts(pb, nvp); + + // Merge polygons. + memset(tmp, 0xff, sizeof(unsigned short)*nvp); + int n = 0; + // Add pa + for (int i = 0; i < na-1; ++i) + tmp[n++] = pa[(ea+1+i) % na]; + // Add pb + for (int i = 0; i < nb-1; ++i) + tmp[n++] = pb[(eb+1+i) % nb]; + + memcpy(pa, tmp, sizeof(unsigned short)*nvp); +} + +static void pushFront(int v, int* arr, int& an) +{ + an++; + for (int i = an-1; i > 0; --i) arr[i] = arr[i-1]; + arr[0] = v; +} + +static void pushBack(int v, int* arr, int& an) +{ + arr[an] = v; + an++; +} + +static bool removeVertex(rcPolyMesh& mesh, const unsigned short rem, const int maxTris) +{ + const int nvp = mesh.nvp; + + // Count number of polygons to remove. + int nrem = 0; + for (int i = 0; i < mesh.npolys; ++i) + { + unsigned short* p = &mesh.polys[i*nvp*2]; + for (int j = 0; j < nvp; ++j) + if (p[j] == rem) { nrem++; break; } + } + + int nedges = 0; + rcScopedDelete edges = new int[nrem*nvp*4]; + if (!edges) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_WARNING, "removeVertex: Out of memory 'edges' (%d).", nrem*nvp*4); + return false; + } + + int nhole = 0; + rcScopedDelete hole = new int[nrem*nvp]; + if (!hole) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_WARNING, "removeVertex: Out of memory 'hole' (%d).", nrem*nvp); + return false; + } + + int nhreg = 0; + rcScopedDelete hreg = new int[nrem*nvp]; + if (!hreg) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_WARNING, "removeVertex: Out of memory 'hreg' (%d).", nrem*nvp); + return false; + } + + int nharea = 0; + rcScopedDelete harea = new int[nrem*nvp]; + if (!harea) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_WARNING, "removeVertex: Out of memory 'harea' (%d).", nrem*nvp); + return false; + } + + for (int i = 0; i < mesh.npolys; ++i) + { + unsigned short* p = &mesh.polys[i*nvp*2]; + const int nv = countPolyVerts(p, nvp); + bool hasRem = false; + for (int j = 0; j < nv; ++j) + if (p[j] == rem) hasRem = true; + if (hasRem) + { + // Collect edges which does not touch the removed vertex. + for (int j = 0, k = nv-1; j < nv; k = j++) + { + if (p[j] != rem && p[k] != rem) + { + int* e = &edges[nedges*4]; + e[0] = p[k]; + e[1] = p[j]; + e[2] = mesh.regs[i]; + e[3] = mesh.areas[i]; + nedges++; + } + } + // Remove the polygon. + unsigned short* p2 = &mesh.polys[(mesh.npolys-1)*nvp*2]; + memcpy(p,p2,sizeof(unsigned short)*nvp); + mesh.regs[i] = mesh.regs[mesh.npolys-1]; + mesh.areas[i] = mesh.areas[mesh.npolys-1]; + mesh.npolys--; + --i; + } + } + + // Remove vertex. + for (int i = (int)rem; i < mesh.nverts; ++i) + { + mesh.verts[i*3+0] = mesh.verts[(i+1)*3+0]; + mesh.verts[i*3+1] = mesh.verts[(i+1)*3+1]; + mesh.verts[i*3+2] = mesh.verts[(i+1)*3+2]; + } + mesh.nverts--; + + // Adjust indices to match the removed vertex layout. + for (int i = 0; i < mesh.npolys; ++i) + { + unsigned short* p = &mesh.polys[i*nvp*2]; + const int nv = countPolyVerts(p, nvp); + for (int j = 0; j < nv; ++j) + if (p[j] > rem) p[j]--; + } + for (int i = 0; i < nedges; ++i) + { + if (edges[i*4+0] > rem) edges[i*4+0]--; + if (edges[i*4+1] > rem) edges[i*4+1]--; + } + + if (nedges == 0) + return true; + + // Start with one vertex, keep appending connected + // segments to the start and end of the hole. + pushBack(edges[0], hole, nhole); + pushBack(edges[2], hreg, nhreg); + pushBack(edges[3], harea, nharea); + + while (nedges) + { + bool match = false; + + for (int i = 0; i < nedges; ++i) + { + const int ea = edges[i*4+0]; + const int eb = edges[i*4+1]; + const int r = edges[i*4+2]; + const int a = edges[i*4+3]; + bool add = false; + if (hole[0] == eb) + { + // The segment matches the beginning of the hole boundary. + pushFront(ea, hole, nhole); + pushFront(r, hreg, nhreg); + pushFront(a, harea, nharea); + add = true; + } + else if (hole[nhole-1] == ea) + { + // The segment matches the end of the hole boundary. + pushBack(eb, hole, nhole); + pushBack(r, hreg, nhreg); + pushBack(a, harea, nharea); + add = true; + } + if (add) + { + // The edge segment was added, remove it. + edges[i*4+0] = edges[(nedges-1)*4+0]; + edges[i*4+1] = edges[(nedges-1)*4+1]; + edges[i*4+2] = edges[(nedges-1)*4+2]; + edges[i*4+3] = edges[(nedges-1)*4+3]; + --nedges; + match = true; + --i; + } + } + + if (!match) + break; + } + + rcScopedDelete tris = new int[nhole*3]; + if (!tris) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_WARNING, "removeVertex: Out of memory 'tris' (%d).", nhole*3); + return false; + } + + rcScopedDelete tverts = new int[nhole*4]; + if (!tverts) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_WARNING, "removeVertex: Out of memory 'tverts' (%d).", nhole*4); + return false; + } + + rcScopedDelete thole = new int[nhole]; + if (!tverts) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_WARNING, "removeVertex: Out of memory 'thole' (%d).", nhole); + return false; + } + + // Generate temp vertex array for triangulation. + for (int i = 0; i < nhole; ++i) + { + const int pi = hole[i]; + tverts[i*4+0] = mesh.verts[pi*3+0]; + tverts[i*4+1] = mesh.verts[pi*3+1]; + tverts[i*4+2] = mesh.verts[pi*3+2]; + tverts[i*4+3] = 0; + thole[i] = i; + } + + // Triangulate the hole. + int ntris = triangulate(nhole, &tverts[0], &thole[0], tris); + if (ntris < 0) + { + ntris = -ntris; + if (rcGetLog()) + rcGetLog()->log(RC_LOG_WARNING, "removeVertex: triangulate() returned bad results."); + } + + // Merge the hole triangles back to polygons. + rcScopedDelete polys = new unsigned short[(ntris+1)*nvp]; + if (!polys) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "removeVertex: Out of memory 'polys' (%d).", (ntris+1)*nvp); + return false; + } + rcScopedDelete pregs = new unsigned short[ntris]; + if (!pregs) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "removeVertex: Out of memory 'pregs' (%d).", ntris); + return false; + } + rcScopedDelete pareas = new unsigned char[ntris]; + if (!pregs) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "removeVertex: Out of memory 'pareas' (%d).", ntris); + return false; + } + + unsigned short* tmpPoly = &polys[ntris*nvp]; + + // Build initial polygons. + int npolys = 0; + memset(polys, 0xff, ntris*nvp*sizeof(unsigned short)); + for (int j = 0; j < ntris; ++j) + { + int* t = &tris[j*3]; + if (t[0] != t[1] && t[0] != t[2] && t[1] != t[2]) + { + polys[npolys*nvp+0] = (unsigned short)hole[t[0]]; + polys[npolys*nvp+1] = (unsigned short)hole[t[1]]; + polys[npolys*nvp+2] = (unsigned short)hole[t[2]]; + pregs[npolys] = (unsigned short)hreg[t[0]]; + pareas[npolys] = (unsigned char)harea[t[0]]; + npolys++; + } + } + if (!npolys) + return true; + + // Merge polygons. + if (nvp > 3) + { + while (true) + { + // Find best polygons to merge. + int bestMergeVal = 0; + int bestPa = 0, bestPb = 0, bestEa = 0, bestEb = 0; + + for (int j = 0; j < npolys-1; ++j) + { + unsigned short* pj = &polys[j*nvp]; + for (int k = j+1; k < npolys; ++k) + { + unsigned short* pk = &polys[k*nvp]; + int ea, eb; + int v = getPolyMergeValue(pj, pk, mesh.verts, ea, eb, nvp); + if (v > bestMergeVal) + { + bestMergeVal = v; + bestPa = j; + bestPb = k; + bestEa = ea; + bestEb = eb; + } + } + } + + if (bestMergeVal > 0) + { + // Found best, merge. + unsigned short* pa = &polys[bestPa*nvp]; + unsigned short* pb = &polys[bestPb*nvp]; + mergePolys(pa, pb, bestEa, bestEb, tmpPoly, nvp); + memcpy(pb, &polys[(npolys-1)*nvp], sizeof(unsigned short)*nvp); + pregs[bestPb] = pregs[npolys-1]; + pareas[bestPb] = pareas[npolys-1]; + npolys--; + } + else + { + // Could not merge any polygons, stop. + break; + } + } + } + + // Store polygons. + for (int i = 0; i < npolys; ++i) + { + if (mesh.npolys >= maxTris) break; + unsigned short* p = &mesh.polys[mesh.npolys*nvp*2]; + memset(p,0xff,sizeof(unsigned short)*nvp*2); + for (int j = 0; j < nvp; ++j) + p[j] = polys[i*nvp+j]; + mesh.regs[mesh.npolys] = pregs[i]; + mesh.areas[mesh.npolys] = pareas[i]; + mesh.npolys++; + if (mesh.npolys > maxTris) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "removeVertex: Too many polygons %d (max:%d).", mesh.npolys, maxTris); + return false; + } + } + + return true; +} + + +bool rcBuildPolyMesh(rcContourSet& cset, int nvp, rcPolyMesh& mesh) +{ + rcTimeVal startTime = rcGetPerformanceTimer(); + + vcopy(mesh.bmin, cset.bmin); + vcopy(mesh.bmax, cset.bmax); + mesh.cs = cset.cs; + mesh.ch = cset.ch; + + int maxVertices = 0; + int maxTris = 0; + int maxVertsPerCont = 0; + for (int i = 0; i < cset.nconts; ++i) + { + // Skip null contours. + if (cset.conts[i].nverts < 3) continue; + maxVertices += cset.conts[i].nverts; + maxTris += cset.conts[i].nverts - 2; + maxVertsPerCont = rcMax(maxVertsPerCont, cset.conts[i].nverts); + } + + if (maxVertices >= 0xfffe) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMesh: Too many vertices %d.", maxVertices); + return false; + } + + rcScopedDelete vflags = new unsigned char[maxVertices]; + if (!vflags) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'mesh.verts' (%d).", maxVertices); + return false; + } + memset(vflags, 0, maxVertices); + + mesh.verts = new unsigned short[maxVertices*3]; + if (!mesh.verts) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'mesh.verts' (%d).", maxVertices); + return false; + } + mesh.polys = new unsigned short[maxTris*nvp*2*2]; + if (!mesh.polys) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'mesh.polys' (%d).", maxTris*nvp*2); + return false; + } + mesh.regs = new unsigned short[maxTris]; + if (!mesh.regs) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'mesh.regs' (%d).", maxTris); + return false; + } + mesh.areas = new unsigned char[maxTris]; + if (!mesh.areas) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'mesh.areas' (%d).", maxTris); + return false; + } + + mesh.nverts = 0; + mesh.npolys = 0; + mesh.nvp = nvp; + + memset(mesh.verts, 0, sizeof(unsigned short)*maxVertices*3); + memset(mesh.polys, 0xff, sizeof(unsigned short)*maxTris*nvp*2); + memset(mesh.regs, 0, sizeof(unsigned short)*maxTris); + memset(mesh.areas, 0, sizeof(unsigned char)*maxTris); + + rcScopedDelete nextVert = new int[maxVertices]; + if (!nextVert) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'nextVert' (%d).", maxVertices); + return false; + } + memset(nextVert, 0, sizeof(int)*maxVertices); + + rcScopedDelete firstVert = new int[VERTEX_BUCKET_COUNT]; + if (!firstVert) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'firstVert' (%d).", VERTEX_BUCKET_COUNT); + return false; + } + for (int i = 0; i < VERTEX_BUCKET_COUNT; ++i) + firstVert[i] = -1; + + rcScopedDelete indices = new int[maxVertsPerCont]; + if (!indices) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'indices' (%d).", maxVertsPerCont); + return false; + } + rcScopedDelete tris = new int[maxVertsPerCont*3]; + if (!tris) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'tris' (%d).", maxVertsPerCont*3); + return false; + } + rcScopedDelete polys = new unsigned short[(maxVertsPerCont+1)*nvp]; + if (!polys) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'polys' (%d).", maxVertsPerCont*nvp); + return false; + } + unsigned short* tmpPoly = &polys[maxVertsPerCont*nvp]; + + for (int i = 0; i < cset.nconts; ++i) + { + rcContour& cont = cset.conts[i]; + + // Skip null contours. + if (cont.nverts < 3) + continue; + + // Triangulate contour + for (int j = 0; j < cont.nverts; ++j) + indices[j] = j; + + int ntris = triangulate(cont.nverts, cont.verts, &indices[0], &tris[0]); + if (ntris <= 0) + { + // Bad triangulation, should not happen. +/* for (int k = 0; k < cont.nverts; ++k) + { + const int* v = &cont.verts[k*4]; + printf("\t\t%d,%d,%d,%d,\n", v[0], v[1], v[2], v[3]); + if (nBadPos < 100) + { + badPos[nBadPos*3+0] = v[0]; + badPos[nBadPos*3+1] = v[1]; + badPos[nBadPos*3+2] = v[2]; + nBadPos++; + } + }*/ + ntris = -ntris; + } + + // Add and merge vertices. + for (int j = 0; j < cont.nverts; ++j) + { + const int* v = &cont.verts[j*4]; + indices[j] = addVertex((unsigned short)v[0], (unsigned short)v[1], (unsigned short)v[2], + mesh.verts, firstVert, nextVert, mesh.nverts); + if (v[3] & RC_BORDER_VERTEX) + { + // This vertex should be removed. + vflags[indices[j]] = 1; + } + } + + // Build initial polygons. + int npolys = 0; + memset(polys, 0xff, maxVertsPerCont*nvp*sizeof(unsigned short)); + for (int j = 0; j < ntris; ++j) + { + int* t = &tris[j*3]; + if (t[0] != t[1] && t[0] != t[2] && t[1] != t[2]) + { + polys[npolys*nvp+0] = (unsigned short)indices[t[0]]; + polys[npolys*nvp+1] = (unsigned short)indices[t[1]]; + polys[npolys*nvp+2] = (unsigned short)indices[t[2]]; + npolys++; + } + } + if (!npolys) + continue; + + // Merge polygons. + if (nvp > 3) + { + while (true) + { + // Find best polygons to merge. + int bestMergeVal = 0; + int bestPa = 0, bestPb = 0, bestEa = 0, bestEb = 0; + + for (int j = 0; j < npolys-1; ++j) + { + unsigned short* pj = &polys[j*nvp]; + for (int k = j+1; k < npolys; ++k) + { + unsigned short* pk = &polys[k*nvp]; + int ea, eb; + int v = getPolyMergeValue(pj, pk, mesh.verts, ea, eb, nvp); + if (v > bestMergeVal) + { + bestMergeVal = v; + bestPa = j; + bestPb = k; + bestEa = ea; + bestEb = eb; + } + } + } + + if (bestMergeVal > 0) + { + // Found best, merge. + unsigned short* pa = &polys[bestPa*nvp]; + unsigned short* pb = &polys[bestPb*nvp]; + mergePolys(pa, pb, bestEa, bestEb, tmpPoly, nvp); + memcpy(pb, &polys[(npolys-1)*nvp], sizeof(unsigned short)*nvp); + npolys--; + } + else + { + // Could not merge any polygons, stop. + break; + } + } + } + + // Store polygons. + for (int j = 0; j < npolys; ++j) + { + unsigned short* p = &mesh.polys[mesh.npolys*nvp*2]; + unsigned short* q = &polys[j*nvp]; + for (int k = 0; k < nvp; ++k) + p[k] = q[k]; + mesh.regs[mesh.npolys] = cont.reg; + mesh.areas[mesh.npolys] = cont.area; + mesh.npolys++; + if (mesh.npolys > maxTris) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMesh: Too many polygons %d (max:%d).", mesh.npolys, maxTris); + return false; + } + } + } + + + // Remove edge vertices. + for (int i = 0; i < mesh.nverts; ++i) + { + if (vflags[i]) + { + if (!removeVertex(mesh, i, maxTris)) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMesh: Failed to remove edge vertex %d.", i); + return false; + } + // Note: mesh.nverts is already decremented inside removeVertex()! + for (int j = i; j < mesh.nverts; ++j) + vflags[j] = vflags[j+1]; + --i; + } + } + + // Calculate adjacency. + if (!buildMeshAdjacency(mesh.polys, mesh.npolys, mesh.nverts, nvp)) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMesh: Adjacency failed."); + return false; + } + + // Just allocate the mesh flags array. The user is resposible to fill it. + mesh.flags = new unsigned short[mesh.npolys]; + if (!mesh.flags) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'mesh.flags' (%d).", mesh.npolys); + return false; + } + memset(mesh.flags, 0, sizeof(unsigned short) * mesh.npolys); + + rcTimeVal endTime = rcGetPerformanceTimer(); + +// if (rcGetLog()) +// rcGetLog()->log(RC_LOG_PROGRESS, "Build polymesh: %.3f ms", rcGetDeltaTimeUsec(startTime, endTime)/1000.0f); + if (rcGetBuildTimes()) + rcGetBuildTimes()->buildPolymesh += rcGetDeltaTimeUsec(startTime, endTime); + + return true; +} + +bool rcMergePolyMeshes(rcPolyMesh** meshes, const int nmeshes, rcPolyMesh& mesh) +{ + if (!nmeshes || !meshes) + return true; + + rcTimeVal startTime = rcGetPerformanceTimer(); + + mesh.nvp = meshes[0]->nvp; + mesh.cs = meshes[0]->cs; + mesh.ch = meshes[0]->ch; + vcopy(mesh.bmin, meshes[0]->bmin); + vcopy(mesh.bmax, meshes[0]->bmax); + + int maxVerts = 0; + int maxPolys = 0; + int maxVertsPerMesh = 0; + for (int i = 0; i < nmeshes; ++i) + { + vmin(mesh.bmin, meshes[i]->bmin); + vmax(mesh.bmax, meshes[i]->bmax); + maxVertsPerMesh = rcMax(maxVertsPerMesh, meshes[i]->nverts); + maxVerts += meshes[i]->nverts; + maxPolys += meshes[i]->npolys; + } + + mesh.nverts = 0; + mesh.verts = new unsigned short[maxVerts*3]; + if (!mesh.verts) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcMergePolyMeshes: Out of memory 'mesh.verts' (%d).", maxVerts*3); + return false; + } + + mesh.npolys = 0; + mesh.polys = new unsigned short[maxPolys*2*mesh.nvp]; + if (!mesh.polys) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcMergePolyMeshes: Out of memory 'mesh.polys' (%d).", maxPolys*2*mesh.nvp); + return false; + } + memset(mesh.polys, 0xff, sizeof(unsigned short)*maxPolys*2*mesh.nvp); + + mesh.regs = new unsigned short[maxPolys]; + if (!mesh.regs) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcMergePolyMeshes: Out of memory 'mesh.regs' (%d).", maxPolys); + return false; + } + memset(mesh.regs, 0, sizeof(unsigned short)*maxPolys); + + mesh.areas = new unsigned char[maxPolys]; + if (!mesh.areas) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcMergePolyMeshes: Out of memory 'mesh.areas' (%d).", maxPolys); + return false; + } + memset(mesh.areas, 0, sizeof(unsigned char)*maxPolys); + + mesh.flags = new unsigned short[maxPolys]; + if (!mesh.flags) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcMergePolyMeshes: Out of memory 'mesh.flags' (%d).", maxPolys); + return false; + } + memset(mesh.flags, 0, sizeof(unsigned short)*maxPolys); + + rcScopedDelete nextVert = new int[maxVerts]; + if (!nextVert) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcMergePolyMeshes: Out of memory 'nextVert' (%d).", maxVerts); + return false; + } + memset(nextVert, 0, sizeof(int)*maxVerts); + + rcScopedDelete firstVert = new int[VERTEX_BUCKET_COUNT]; + if (!firstVert) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcMergePolyMeshes: Out of memory 'firstVert' (%d).", VERTEX_BUCKET_COUNT); + return false; + } + for (int i = 0; i < VERTEX_BUCKET_COUNT; ++i) + firstVert[i] = -1; + + rcScopedDelete vremap = new unsigned short[maxVertsPerMesh]; + if (!vremap) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcMergePolyMeshes: Out of memory 'vremap' (%d).", maxVertsPerMesh); + return false; + } + memset(nextVert, 0, sizeof(int)*maxVerts); + + for (int i = 0; i < nmeshes; ++i) + { + const rcPolyMesh* pmesh = meshes[i]; + + const unsigned short ox = (unsigned short)floorf((pmesh->bmin[0]-mesh.bmin[0])/mesh.cs+0.5f); + const unsigned short oz = (unsigned short)floorf((pmesh->bmin[2]-mesh.bmin[2])/mesh.cs+0.5f); + + for (int j = 0; j < pmesh->nverts; ++j) + { + unsigned short* v = &pmesh->verts[j*3]; + vremap[j] = addVertex(v[0]+ox, v[1], v[2]+oz, + mesh.verts, firstVert, nextVert, mesh.nverts); + } + + for (int j = 0; j < pmesh->npolys; ++j) + { + unsigned short* tgt = &mesh.polys[mesh.npolys*2*mesh.nvp]; + unsigned short* src = &pmesh->polys[j*2*mesh.nvp]; + mesh.regs[mesh.npolys] = pmesh->regs[j]; + mesh.areas[mesh.npolys] = pmesh->areas[j]; + mesh.flags[mesh.npolys] = pmesh->flags[j]; + mesh.npolys++; + for (int k = 0; k < mesh.nvp; ++k) + { + if (src[k] == RC_MESH_NULL_IDX) break; + tgt[k] = vremap[src[k]]; + } + } + } + + // Calculate adjacency. + if (!buildMeshAdjacency(mesh.polys, mesh.npolys, mesh.nverts, mesh.nvp)) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcMergePolyMeshes: Adjacency failed."); + return false; + } + + + rcTimeVal endTime = rcGetPerformanceTimer(); + + if (rcGetBuildTimes()) + rcGetBuildTimes()->mergePolyMesh += rcGetDeltaTimeUsec(startTime, endTime); + + return true; +} diff --git a/src/shared/pathfinding/Recast/RecastMeshDetail.cpp b/src/shared/pathfinding/Recast/RecastMeshDetail.cpp new file mode 100644 index 0000000..0eb3b55 --- /dev/null +++ b/src/shared/pathfinding/Recast/RecastMeshDetail.cpp @@ -0,0 +1,1223 @@ +// +// Copyright (c) 2009 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 +#define _USE_MATH_DEFINES +#include +#include +#include +#include +#include "Recast.h" +#include "RecastLog.h" +#include "RecastTimer.h" + + +static const unsigned RC_UNSET_HEIGHT = 0xffff; + +struct rcHeightPatch +{ + inline rcHeightPatch() : data(0) {} + inline ~rcHeightPatch() { delete [] data; } + unsigned short* data; + int xmin, ymin, width, height; +}; + + +inline float vdot2(const float* a, const float* b) +{ + return a[0]*b[0] + a[2]*b[2]; +} + +inline float vdistSq2(const float* p, const float* q) +{ + const float dx = q[0] - p[0]; + const float dy = q[2] - p[2]; + return dx*dx + dy*dy; +} + +inline float vdist2(const float* p, const float* q) +{ + return sqrtf(vdistSq2(p,q)); +} + +inline float vcross2(const float* p1, const float* p2, const float* p3) +{ + const float u1 = p2[0] - p1[0]; + const float v1 = p2[2] - p1[2]; + const float u2 = p3[0] - p1[0]; + const float v2 = p3[2] - p1[2]; + return u1 * v2 - v1 * u2; +} + +static bool circumCircle(const float* p1, const float* p2, const float* p3, + float* c, float& r) +{ + static const float EPS = 1e-6f; + + const float cp = vcross2(p1, p2, p3); + if (fabsf(cp) > EPS) + { + const float p1Sq = vdot2(p1,p1); + const float p2Sq = vdot2(p2,p2); + const float p3Sq = vdot2(p3,p3); + c[0] = (p1Sq*(p2[2]-p3[2]) + p2Sq*(p3[2]-p1[2]) + p3Sq*(p1[2]-p2[2])) / (2*cp); + c[2] = (p1Sq*(p3[0]-p2[0]) + p2Sq*(p1[0]-p3[0]) + p3Sq*(p2[0]-p1[0])) / (2*cp); + r = vdist2(c, p1); + return true; + } + + c[0] = p1[0]; + c[2] = p1[2]; + r = 0; + return false; +} + +static float distPtTri(const float* p, const float* a, const float* b, const float* c) +{ + float v0[3], v1[3], v2[3]; + vsub(v0, c,a); + vsub(v1, b,a); + vsub(v2, p,a); + + const float dot00 = vdot2(v0, v0); + const float dot01 = vdot2(v0, v1); + const float dot02 = vdot2(v0, v2); + const float dot11 = vdot2(v1, v1); + const float dot12 = vdot2(v1, v2); + + // Compute barycentric coordinates + float invDenom = 1.0f / (dot00 * dot11 - dot01 * dot01); + float u = (dot11 * dot02 - dot01 * dot12) * invDenom; + float v = (dot00 * dot12 - dot01 * dot02) * invDenom; + + // If point lies inside the triangle, return interpolated y-coord. + static const float EPS = 1e-4f; + if (u >= -EPS && v >= -EPS && (u+v) <= 1+EPS) + { + float y = a[1] + v0[1]*u + v1[1]*v; + return fabsf(y-p[1]); + } + return FLT_MAX; +} + +static float distancePtSeg(const float* pt, const float* p, const float* q) +{ + float pqx = q[0] - p[0]; + float pqy = q[1] - p[1]; + float pqz = q[2] - p[2]; + float dx = pt[0] - p[0]; + float dy = pt[1] - p[1]; + float dz = pt[2] - p[2]; + float d = pqx*pqx + pqy*pqy + pqz*pqz; + float t = pqx*dx + pqy*dy + pqz*dz; + if (d > 0) + t /= d; + if (t < 0) + t = 0; + else if (t > 1) + t = 1; + + dx = p[0] + t*pqx - pt[0]; + dy = p[1] + t*pqy - pt[1]; + dz = p[2] + t*pqz - pt[2]; + + return dx*dx + dy*dy + dz*dz; +} + +static float distancePtSeg2d(const float* pt, const float* p, const float* q) +{ + float pqx = q[0] - p[0]; + float pqz = q[2] - p[2]; + float dx = pt[0] - p[0]; + float dz = pt[2] - p[2]; + float d = pqx*pqx + pqz*pqz; + float t = pqx*dx + pqz*dz; + if (d > 0) + t /= d; + if (t < 0) + t = 0; + else if (t > 1) + t = 1; + + dx = p[0] + t*pqx - pt[0]; + dz = p[2] + t*pqz - pt[2]; + + return dx*dx + dz*dz; +} + +static float distToTriMesh(const float* p, const float* verts, int nverts, const int* tris, int ntris) +{ + float dmin = FLT_MAX; + for (int i = 0; i < ntris; ++i) + { + const float* va = &verts[tris[i*4+0]*3]; + const float* vb = &verts[tris[i*4+1]*3]; + const float* vc = &verts[tris[i*4+2]*3]; + float d = distPtTri(p, va,vb,vc); + if (d < dmin) + dmin = d; + } + if (dmin == FLT_MAX) return -1; + return dmin; +} + +static float distToPoly(int nvert, const float* verts, const float* p) +{ + + float dmin = FLT_MAX; + int i, j, c = 0; + for (i = 0, j = nvert-1; i < nvert; j = i++) + { + const float* vi = &verts[i*3]; + const float* vj = &verts[j*3]; + if (((vi[2] > p[2]) != (vj[2] > p[2])) && + (p[0] < (vj[0]-vi[0]) * (p[2]-vi[2]) / (vj[2]-vi[2]) + vi[0]) ) + c = !c; + dmin = rcMin(dmin, distancePtSeg2d(p, vj, vi)); + } + return c ? -dmin : dmin; +} + + +static unsigned short getHeight(const float fx, const float fy, const float fz, const float cs, const float ics, const float ch, const rcHeightPatch& hp) +{ + int ix = (int)floorf(fx*ics + 0.01f); + int iz = (int)floorf(fz*ics + 0.01f); + ix = rcClamp(ix-hp.xmin, 0, hp.width); + iz = rcClamp(iz-hp.ymin, 0, hp.height); + unsigned short h = hp.data[ix+iz*hp.width]; + if (h == RC_UNSET_HEIGHT) + { + // Special case when data might be bad. + // Find nearest neighbour pixel which has valid height. + const int off[8*2] = { -1,0, -1,-1, 0,-1, 1,-1, 1,0, 1,1, 0,1, -1,1}; + float dmin = FLT_MAX; + for (int i = 0; i < 8; ++i) + { + const int nx = ix+off[i*2+0]; + const int nz = iz+off[i*2+1]; + if (nx < 0 || nz < 0 || nx >= hp.width || nz >= hp.height) continue; + const unsigned short nh = hp.data[nx+nz*hp.width]; + if (nh == RC_UNSET_HEIGHT) continue; + + const float d = fabsf(nh*ch - fy); + if (d < dmin) + { + h = nh; + dmin = d; + } + +/* const float dx = (nx+0.5f)*cs - fx; + const float dz = (nz+0.5f)*cs - fz; + const float d = dx*dx+dz*dz; + if (d < dmin) + { + h = nh; + dmin = d; + } */ + } + } + return h; +} + + +enum EdgeValues +{ + UNDEF = -1, + HULL = -2, +}; + +static int findEdge(const int* edges, int nedges, int s, int t) +{ + for (int i = 0; i < nedges; i++) + { + const int* e = &edges[i*4]; + if ((e[0] == s && e[1] == t) || (e[0] == t && e[1] == s)) + return i; + } + return UNDEF; +} + +static int addEdge(int* edges, int& nedges, const int maxEdges, int s, int t, int l, int r) +{ + if (nedges >= maxEdges) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "addEdge: Too many edges (%d/%d).", nedges, maxEdges); + return UNDEF; + } + + // Add edge if not already in the triangulation. + int e = findEdge(edges, nedges, s, t); + if (e == UNDEF) + { + int* e = &edges[nedges*4]; + e[0] = s; + e[1] = t; + e[2] = l; + e[3] = r; + return nedges++; + } + else + { + return UNDEF; + } +} + +static void updateLeftFace(int* e, int s, int t, int f) +{ + if (e[0] == s && e[1] == t && e[2] == UNDEF) + e[2] = f; + else if (e[1] == s && e[0] == t && e[3] == UNDEF) + e[3] = f; +} + +static int overlapSegSeg2d(const float* a, const float* b, const float* c, const float* d) +{ + const float a1 = vcross2(a, b, d); + const float a2 = vcross2(a, b, c); + if (a1*a2 < 0.0f) + { + float a3 = vcross2(c, d, a); + float a4 = a3 + a2 - a1; + if (a3 * a4 < 0.0f) + return 1; + } + return 0; +} + +static bool overlapEdges(const float* pts, const int* edges, int nedges, int s1, int t1) +{ + for (int i = 0; i < nedges; ++i) + { + const int s0 = edges[i*4+0]; + const int t0 = edges[i*4+1]; + // Same or connected edges do not overlap. + if (s0 == s1 || s0 == t1 || t0 == s1 || t0 == t1) + continue; + if (overlapSegSeg2d(&pts[s0*3],&pts[t0*3], &pts[s1*3],&pts[t1*3])) + return true; + } + return false; +} + +static void completeFacet(const float* pts, int npts, int* edges, int& nedges, const int maxEdges, int& nfaces, int e) +{ + static const float EPS = 1e-5f; + + int* edge = &edges[e*4]; + + // Cache s and t. + int s,t; + if (edge[2] == UNDEF) + { + s = edge[0]; + t = edge[1]; + } + else if (edge[3] == UNDEF) + { + s = edge[1]; + t = edge[0]; + } + else + { + // Edge already completed. + return; + } + + // Find best point on left of edge. + int pt = npts; + float c[3] = {0,0,0}; + float r = -1; + for (int u = 0; u < npts; ++u) + { + if (u == s || u == t) continue; + if (vcross2(&pts[s*3], &pts[t*3], &pts[u*3]) > EPS) + { + if (r < 0) + { + // The circle is not updated yet, do it now. + pt = u; + circumCircle(&pts[s*3], &pts[t*3], &pts[u*3], c, r); + continue; + } + const float d = vdist2(c, &pts[u*3]); + const float tol = 0.001f; + if (d > r*(1+tol)) + { + // Outside current circumcircle, skip. + continue; + } + else if (d < r*(1-tol)) + { + // Inside safe circumcircle, update circle. + pt = u; + circumCircle(&pts[s*3], &pts[t*3], &pts[u*3], c, r); + } + else + { + // Inside epsilon circum circle, do extra tests to make sure the edge is valid. + // s-u and t-u cannot overlap with s-pt nor t-pt if they exists. + if (overlapEdges(pts, edges, nedges, s,u)) + continue; + if (overlapEdges(pts, edges, nedges, t,u)) + continue; + // Edge is valid. + pt = u; + circumCircle(&pts[s*3], &pts[t*3], &pts[u*3], c, r); + } + } + } + + // Add new triangle or update edge info if s-t is on hull. + if (pt < npts) + { + // Update face information of edge being completed. + updateLeftFace(&edges[e*4], s, t, nfaces); + + // Add new edge or update face info of old edge. + e = findEdge(edges, nedges, pt, s); + if (e == UNDEF) + addEdge(edges, nedges, maxEdges, pt, s, nfaces, UNDEF); + else + updateLeftFace(&edges[e*4], pt, s, nfaces); + + // Add new edge or update face info of old edge. + e = findEdge(edges, nedges, t, pt); + if (e == UNDEF) + addEdge(edges, nedges, maxEdges, t, pt, nfaces, UNDEF); + else + updateLeftFace(&edges[e*4], t, pt, nfaces); + + nfaces++; + } + else + { + updateLeftFace(&edges[e*4], s, t, HULL); + } +} + +static void delaunayHull(const int npts, const float* pts, + const int nhull, const int* hull, + rcIntArray& tris, rcIntArray& edges) +{ + int nfaces = 0; + int nedges = 0; + const int maxEdges = npts*10; + edges.resize(maxEdges*4); + + for (int i = 0, j = nhull-1; i < nhull; j=i++) + addEdge(&edges[0], nedges, maxEdges, hull[j],hull[i], HULL, UNDEF); + + int currentEdge = 0; + while (currentEdge < nedges) + { + if (edges[currentEdge*4+2] == UNDEF) + completeFacet(pts, npts, &edges[0], nedges, maxEdges, nfaces, currentEdge); + if (edges[currentEdge*4+3] == UNDEF) + completeFacet(pts, npts, &edges[0], nedges, maxEdges, nfaces, currentEdge); + currentEdge++; + } + + // Create tris + tris.resize(nfaces*4); + for (int i = 0; i < nfaces*4; ++i) + tris[i] = -1; + + for (int i = 0; i < nedges; ++i) + { + const int* e = &edges[i*4]; + if (e[3] >= 0) + { + // Left face + int* t = &tris[e[3]*4]; + if (t[0] == -1) + { + t[0] = e[0]; + t[1] = e[1]; + } + else if (t[0] == e[1]) + t[2] = e[0]; + else if (t[1] == e[0]) + t[2] = e[1]; + } + if (e[2] >= 0) + { + // Right + int* t = &tris[e[2]*4]; + if (t[0] == -1) + { + t[0] = e[1]; + t[1] = e[0]; + } + else if (t[0] == e[0]) + t[2] = e[1]; + else if (t[1] == e[1]) + t[2] = e[0]; + } + } + + for (int i = 0; i < tris.size()/4; ++i) + { + int* t = &tris[i*4]; + if (t[0] == -1 || t[1] == -1 || t[2] == -1) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_WARNING, "delaunayHull: Removing dangling face %d [%d,%d,%d].", i, t[0],t[1],t[2]); + t[0] = tris[tris.size()-4]; + t[1] = tris[tris.size()-3]; + t[2] = tris[tris.size()-2]; + t[3] = tris[tris.size()-1]; + tris.resize(tris.size()-4); + } + } + +} + + + +static bool buildPolyDetail(const float* in, const int nin, + const float sampleDist, const float sampleMaxError, + const rcCompactHeightfield& chf, const rcHeightPatch& hp, + float* verts, int& nverts, rcIntArray& tris, + rcIntArray& edges, rcIntArray& samples) +{ + static const int MAX_VERTS = 256; + static const int MAX_EDGE = 64; + float edge[(MAX_EDGE+1)*3]; + int hull[MAX_VERTS]; + int nhull = 0; + + nverts = 0; + + for (int i = 0; i < nin; ++i) + vcopy(&verts[i*3], &in[i*3]); + nverts = nin; + + const float cs = chf.cs; + const float ics = 1.0f/cs; + + // Tesselate outlines. + // This is done in separate pass in order to ensure + // seamless height values across the ply boundaries. + if (sampleDist > 0) + { + for (int i = 0, j = nin-1; i < nin; j=i++) + { + const float* vj = &in[j*3]; + const float* vi = &in[i*3]; + bool swapped = false; + // Make sure the segments are always handled in same order + // using lexological sort or else there will be seams. + if (fabsf(vj[0]-vi[0]) < 1e-6f) + { + if (vj[2] > vi[2]) + { + rcSwap(vj,vi); + swapped = true; + } + } + else + { + if (vj[0] > vi[0]) + { + rcSwap(vj,vi); + swapped = true; + } + } + // Create samples along the edge. + float dx = vi[0] - vj[0]; + float dy = vi[1] - vj[1]; + float dz = vi[2] - vj[2]; + float d = sqrtf(dx*dx + dz*dz); + int nn = 1 + (int)floorf(d/sampleDist); + if (nn > MAX_EDGE) nn = MAX_EDGE; + if (nverts+nn >= MAX_VERTS) + nn = MAX_VERTS-1-nverts; + for (int k = 0; k <= nn; ++k) + { + float u = (float)k/(float)nn; + float* pos = &edge[k*3]; + pos[0] = vj[0] + dx*u; + pos[1] = vj[1] + dy*u; + pos[2] = vj[2] + dz*u; + pos[1] = getHeight(pos[0],pos[1],pos[2], cs, ics, chf.ch, hp)*chf.ch; + } + // Simplify samples. + int idx[MAX_EDGE] = {0,nn}; + int nidx = 2; + for (int k = 0; k < nidx-1; ) + { + const int a = idx[k]; + const int b = idx[k+1]; + const float* va = &edge[a*3]; + const float* vb = &edge[b*3]; + // Find maximum deviation along the segment. + float maxd = 0; + int maxi = -1; + for (int m = a+1; m < b; ++m) + { + float d = distancePtSeg(&edge[m*3],va,vb); + if (d > maxd) + { + maxd = d; + maxi = m; + } + } + // If the max deviation is larger than accepted error, + // add new point, else continue to next segment. + if (maxi != -1 && maxd > rcSqr(sampleMaxError)) + { + for (int m = nidx; m > k; --m) + idx[m] = idx[m-1]; + idx[k+1] = maxi; + nidx++; + } + else + { + ++k; + } + } + + hull[nhull++] = j; + // Add new vertices. + if (swapped) + { + for (int k = nidx-2; k > 0; --k) + { + vcopy(&verts[nverts*3], &edge[idx[k]*3]); + hull[nhull++] = nverts; + nverts++; + } + } + else + { + for (int k = 1; k < nidx-1; ++k) + { + vcopy(&verts[nverts*3], &edge[idx[k]*3]); + hull[nhull++] = nverts; + nverts++; + } + } + } + } + + + // Tesselate the base mesh. + edges.resize(0); + tris.resize(0); + + delaunayHull(nverts, verts, nhull, hull, tris, edges); + + if (tris.size() == 0) + { + // Could not triangulate the poly, make sure there is some valid data there. + if (rcGetLog()) + rcGetLog()->log(RC_LOG_WARNING, "buildPolyDetail: Could not triangulate polygon, adding default data."); + for (int i = 2; i < nverts; ++i) + { + tris.push(0); + tris.push(i-1); + tris.push(i); + tris.push(0); + } + return true; + } + + if (sampleDist > 0) + { + // Create sample locations in a grid. + float bmin[3], bmax[3]; + vcopy(bmin, in); + vcopy(bmax, in); + for (int i = 1; i < nin; ++i) + { + vmin(bmin, &in[i*3]); + vmax(bmax, &in[i*3]); + } + int x0 = (int)floorf(bmin[0]/sampleDist); + int x1 = (int)ceilf(bmax[0]/sampleDist); + int z0 = (int)floorf(bmin[2]/sampleDist); + int z1 = (int)ceilf(bmax[2]/sampleDist); + samples.resize(0); + for (int z = z0; z < z1; ++z) + { + for (int x = x0; x < x1; ++x) + { + float pt[3]; + pt[0] = x*sampleDist; + pt[1] = (bmax[1]+bmin[1])*0.5f; + pt[2] = z*sampleDist; + // Make sure the samples are not too close to the edges. + if (distToPoly(nin,in,pt) > -sampleDist/2) continue; + samples.push(x); + samples.push(getHeight(pt[0], pt[1], pt[2], cs, ics, chf.ch, hp)); + samples.push(z); + } + } + + // Add the samples starting from the one that has the most + // error. The procedure stops when all samples are added + // or when the max error is within treshold. + const int nsamples = samples.size()/3; + for (int iter = 0; iter < nsamples; ++iter) + { + // Find sample with most error. + float bestpt[3] = {0,0,0}; + float bestd = 0; + for (int i = 0; i < nsamples; ++i) + { + float pt[3]; + pt[0] = samples[i*3+0]*sampleDist; + pt[1] = samples[i*3+1]*chf.ch; + pt[2] = samples[i*3+2]*sampleDist; + float d = distToTriMesh(pt, verts, nverts, &tris[0], tris.size()/4); + if (d < 0) continue; // did not hit the mesh. + if (d > bestd) + { + bestd = d; + vcopy(bestpt,pt); + } + } + // If the max error is within accepted threshold, stop tesselating. + if (bestd <= sampleMaxError) + break; + + // Add the new sample point. + vcopy(&verts[nverts*3],bestpt); + nverts++; + + // Create new triangulation. + // TODO: Incremental add instead of full rebuild. + edges.resize(0); + tris.resize(0); + delaunayHull(nverts, verts, nhull, hull, tris, edges); + + if (nverts >= MAX_VERTS) + break; + } + } + + return true; +} + +static void getHeightData(const rcCompactHeightfield& chf, + const unsigned short* poly, const int npoly, + const unsigned short* verts, + rcHeightPatch& hp, rcIntArray& stack) +{ + // Floodfill the heightfield to get 2D height data, + // starting at vertex locations as seeds. + + memset(hp.data, 0, sizeof(unsigned short)*hp.width*hp.height); + + stack.resize(0); + + static const int offset[9*2] = + { + 0,0, -1,-1, 0,-1, 1,-1, 1,0, 1,1, 0,1, -1,1, -1,0, + }; + + // Use poly vertices as seed points for the flood fill. + for (int j = 0; j < npoly; ++j) + { + int cx = 0, cz = 0, ci =-1; + int dmin = RC_UNSET_HEIGHT; + for (int k = 0; k < 9; ++k) + { + const int ax = (int)verts[poly[j]*3+0] + offset[k*2+0]; + const int ay = (int)verts[poly[j]*3+1]; + const int az = (int)verts[poly[j]*3+2] + offset[k*2+1]; + if (ax < hp.xmin || ax >= hp.xmin+hp.width || + az < hp.ymin || az >= hp.ymin+hp.height) + continue; + + const rcCompactCell& c = chf.cells[ax+az*chf.width]; + for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i) + { + const rcCompactSpan& s = chf.spans[i]; + int d = rcAbs(ay - (int)s.y); + if (d < dmin) + { + cx = ax; + cz = az; + ci = i; + dmin = d; + } + } + } + if (ci != -1) + { + stack.push(cx); + stack.push(cz); + stack.push(ci); + } + } + + // Find center of the polygon using flood fill. + int pcx = 0, pcy = 0, pcz = 0; + for (int j = 0; j < npoly; ++j) + { + pcx += (int)verts[poly[j]*3+0]; + pcy += (int)verts[poly[j]*3+1]; + pcz += (int)verts[poly[j]*3+2]; + } + pcx /= npoly; + pcy /= npoly; + pcz /= npoly; + + for (int i = 0; i < stack.size(); i += 3) + { + int cx = stack[i+0]; + int cy = stack[i+1]; + int idx = cx-hp.xmin+(cy-hp.ymin)*hp.width; + hp.data[idx] = 1; + } + + while (stack.size() > 0) + { + int ci = stack.pop(); + int cy = stack.pop(); + int cx = stack.pop(); + + // Check if close to center of the polygon. + if (rcAbs(cx-pcx) <= 1 && rcAbs(cy-pcz) <= 1) + { + stack.resize(0); + stack.push(cx); + stack.push(cy); + stack.push(ci); + break; + } + + const rcCompactSpan& cs = chf.spans[ci]; + + for (int dir = 0; dir < 4; ++dir) + { + if (rcGetCon(cs, dir) == RC_NOT_CONNECTED) continue; + + const int ax = cx + rcGetDirOffsetX(dir); + const int ay = cy + rcGetDirOffsetY(dir); + + if (ax < hp.xmin || ax >= (hp.xmin+hp.width) || + ay < hp.ymin || ay >= (hp.ymin+hp.height)) + continue; + + if (hp.data[ax-hp.xmin+(ay-hp.ymin)*hp.width] != 0) + continue; + + const int ai = (int)chf.cells[ax+ay*chf.width].index + rcGetCon(cs, dir); + + int idx = ax-hp.xmin+(ay-hp.ymin)*hp.width; + hp.data[idx] = 1; + + stack.push(ax); + stack.push(ay); + stack.push(ai); + } + } + + memset(hp.data, 0xff, sizeof(unsigned short)*hp.width*hp.height); + + // Mark start locations. + for (int i = 0; i < stack.size(); i += 3) + { + int cx = stack[i+0]; + int cy = stack[i+1]; + int ci = stack[i+2]; + int idx = cx-hp.xmin+(cy-hp.ymin)*hp.width; + const rcCompactSpan& cs = chf.spans[ci]; + hp.data[idx] = cs.y; + } + + while (stack.size() > 0) + { +/* int cx = stack[0]; + int cy = stack[1]; + int ci = stack[2]; + if (stack.size() >= 3) + memmove(&stack[0], &stack[3], sizeof(int)*(stack.size()-3)); + stack.resize(stack.size()-3);*/ + + int ci = stack.pop(); + int cy = stack.pop(); + int cx = stack.pop(); + + const rcCompactSpan& cs = chf.spans[ci]; + for (int dir = 0; dir < 4; ++dir) + { + if (rcGetCon(cs, dir) == RC_NOT_CONNECTED) continue; + + const int ax = cx + rcGetDirOffsetX(dir); + const int ay = cy + rcGetDirOffsetY(dir); + + if (ax < hp.xmin || ax >= (hp.xmin+hp.width) || + ay < hp.ymin || ay >= (hp.ymin+hp.height)) + continue; + + if (hp.data[ax-hp.xmin+(ay-hp.ymin)*hp.width] != RC_UNSET_HEIGHT) + continue; + + const int ai = (int)chf.cells[ax+ay*chf.width].index + rcGetCon(cs, dir); + + const rcCompactSpan& as = chf.spans[ai]; + int idx = ax-hp.xmin+(ay-hp.ymin)*hp.width; + hp.data[idx] = as.y; + + stack.push(ax); + stack.push(ay); + stack.push(ai); + } + } + +} + +static unsigned char getEdgeFlags(const float* va, const float* vb, + const float* vpoly, const int npoly) +{ + // Return true if edge (va,vb) is part of the polygon. + static const float thrSqr = rcSqr(0.001f); + for (int i = 0, j = npoly-1; i < npoly; j=i++) + { + if (distancePtSeg2d(va, &vpoly[j*3], &vpoly[i*3]) < thrSqr && + distancePtSeg2d(vb, &vpoly[j*3], &vpoly[i*3]) < thrSqr) + return 1; + } + return 0; +} + +static unsigned char getTriFlags(const float* va, const float* vb, const float* vc, + const float* vpoly, const int npoly) +{ + unsigned char flags = 0; + flags |= getEdgeFlags(va,vb,vpoly,npoly) << 0; + flags |= getEdgeFlags(vb,vc,vpoly,npoly) << 2; + flags |= getEdgeFlags(vc,va,vpoly,npoly) << 4; + return flags; +} + + + +bool rcBuildPolyMeshDetail(const rcPolyMesh& mesh, const rcCompactHeightfield& chf, + const float sampleDist, const float sampleMaxError, + rcPolyMeshDetail& dmesh) +{ + rcTimeVal startTime = rcGetPerformanceTimer(); + + if (mesh.nverts == 0 || mesh.npolys == 0) + return true; + + const int nvp = mesh.nvp; + const float cs = mesh.cs; + const float ch = mesh.ch; + const float* orig = mesh.bmin; + + rcIntArray edges(64); + rcIntArray tris(512); + rcIntArray stack(512); + rcIntArray samples(512); + float verts[256*3]; + rcHeightPatch hp; + int nPolyVerts = 0; + int maxhw = 0, maxhh = 0; + + rcScopedDelete bounds = new int[mesh.npolys*4]; + if (!bounds) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'bounds' (%d).", mesh.npolys*4); + return false; + } + rcScopedDelete poly = new float[nvp*3]; + if (!poly) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'poly' (%d).", nvp*3); + return false; + } + + // Find max size for a polygon area. + for (int i = 0; i < mesh.npolys; ++i) + { + const unsigned short* p = &mesh.polys[i*nvp*2]; + int& xmin = bounds[i*4+0]; + int& xmax = bounds[i*4+1]; + int& ymin = bounds[i*4+2]; + int& ymax = bounds[i*4+3]; + xmin = chf.width; + xmax = 0; + ymin = chf.height; + ymax = 0; + for (int j = 0; j < nvp; ++j) + { + if(p[j] == RC_MESH_NULL_IDX) break; + const unsigned short* v = &mesh.verts[p[j]*3]; + xmin = rcMin(xmin, (int)v[0]); + xmax = rcMax(xmax, (int)v[0]); + ymin = rcMin(ymin, (int)v[2]); + ymax = rcMax(ymax, (int)v[2]); + nPolyVerts++; + } + xmin = rcMax(0,xmin-1); + xmax = rcMin(chf.width,xmax+1); + ymin = rcMax(0,ymin-1); + ymax = rcMin(chf.height,ymax+1); + if (xmin >= xmax || ymin >= ymax) continue; + maxhw = rcMax(maxhw, xmax-xmin); + maxhh = rcMax(maxhh, ymax-ymin); + } + + hp.data = new unsigned short[maxhw*maxhh]; + if (!hp.data) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'hp.data' (%d).", maxhw*maxhh); + return false; + } + + dmesh.nmeshes = mesh.npolys; + dmesh.nverts = 0; + dmesh.ntris = 0; + dmesh.meshes = new unsigned short[dmesh.nmeshes*4]; + if (!dmesh.meshes) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'dmesh.meshes' (%d).", dmesh.nmeshes*4); + return false; + } + + int vcap = nPolyVerts+nPolyVerts/2; + int tcap = vcap*2; + + dmesh.nverts = 0; + dmesh.verts = new float[vcap*3]; + if (!dmesh.verts) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'dmesh.verts' (%d).", vcap*3); + return false; + } + dmesh.ntris = 0; + dmesh.tris = new unsigned char[tcap*4]; + if (!dmesh.tris) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'dmesh.tris' (%d).", tcap*4); + return false; + } + + for (int i = 0; i < mesh.npolys; ++i) + { + const unsigned short* p = &mesh.polys[i*nvp*2]; + + // Store polygon vertices for processing. + int npoly = 0; + for (int j = 0; j < nvp; ++j) + { + if(p[j] == RC_MESH_NULL_IDX) break; + const unsigned short* v = &mesh.verts[p[j]*3]; + poly[j*3+0] = v[0]*cs; + poly[j*3+1] = v[1]*ch; + poly[j*3+2] = v[2]*cs; + npoly++; + } + + // Get the height data from the area of the polygon. + hp.xmin = bounds[i*4+0]; + hp.ymin = bounds[i*4+2]; + hp.width = bounds[i*4+1]-bounds[i*4+0]; + hp.height = bounds[i*4+3]-bounds[i*4+2]; + getHeightData(chf, p, npoly, mesh.verts, hp, stack); + + // Build detail mesh. + int nverts = 0; + if (!buildPolyDetail(poly, npoly, + sampleDist, sampleMaxError, + chf, hp, verts, nverts, tris, + edges, samples)) + { + return false; + } + + // Move detail verts to world space. + for (int j = 0; j < nverts; ++j) + { + verts[j*3+0] += orig[0]; + verts[j*3+1] += orig[1] + chf.ch; // Is this offset necessary? + verts[j*3+2] += orig[2]; + } + // Offset poly too, will be used to flag checking. + for (int j = 0; j < npoly; ++j) + { + poly[j*3+0] += orig[0]; + poly[j*3+1] += orig[1]; + poly[j*3+2] += orig[2]; + } + + // Store detail submesh. + const int ntris = tris.size()/4; + + dmesh.meshes[i*4+0] = dmesh.nverts; + dmesh.meshes[i*4+1] = (unsigned short)nverts; + dmesh.meshes[i*4+2] = dmesh.ntris; + dmesh.meshes[i*4+3] = (unsigned short)ntris; + + // Store vertices, allocate more memory if necessary. + if (dmesh.nverts+nverts > vcap) + { + while (dmesh.nverts+nverts > vcap) + vcap += 256; + + float* newv = new float[vcap*3]; + if (!newv) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'newv' (%d).", vcap*3); + return false; + } + if (dmesh.nverts) + memcpy(newv, dmesh.verts, sizeof(float)*3*dmesh.nverts); + delete [] dmesh.verts; + dmesh.verts = newv; + } + for (int j = 0; j < nverts; ++j) + { + dmesh.verts[dmesh.nverts*3+0] = verts[j*3+0]; + dmesh.verts[dmesh.nverts*3+1] = verts[j*3+1]; + dmesh.verts[dmesh.nverts*3+2] = verts[j*3+2]; + dmesh.nverts++; + } + + // Store triangles, allocate more memory if necessary. + if (dmesh.ntris+ntris > tcap) + { + while (dmesh.ntris+ntris > tcap) + tcap += 256; + unsigned char* newt = new unsigned char[tcap*4]; + if (!newt) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'newt' (%d).", tcap*4); + return false; + } + if (dmesh.ntris) + memcpy(newt, dmesh.tris, sizeof(unsigned char)*4*dmesh.ntris); + delete [] dmesh.tris; + dmesh.tris = newt; + } + for (int j = 0; j < ntris; ++j) + { + const int* t = &tris[j*4]; + dmesh.tris[dmesh.ntris*4+0] = (unsigned char)t[0]; + dmesh.tris[dmesh.ntris*4+1] = (unsigned char)t[1]; + dmesh.tris[dmesh.ntris*4+2] = (unsigned char)t[2]; + dmesh.tris[dmesh.ntris*4+3] = getTriFlags(&verts[t[0]*3], &verts[t[1]*3], &verts[t[2]*3], poly, npoly); + dmesh.ntris++; + } + } + + rcTimeVal endTime = rcGetPerformanceTimer(); + + if (rcGetBuildTimes()) + rcGetBuildTimes()->buildDetailMesh += rcGetDeltaTimeUsec(startTime, endTime); + + return true; +} + +bool rcMergePolyMeshDetails(rcPolyMeshDetail** meshes, const int nmeshes, rcPolyMeshDetail& mesh) +{ + rcTimeVal startTime = rcGetPerformanceTimer(); + + int maxVerts = 0; + int maxTris = 0; + int maxMeshes = 0; + + for (int i = 0; i < nmeshes; ++i) + { + if (!meshes[i]) continue; + maxVerts += meshes[i]->nverts; + maxTris += meshes[i]->ntris; + maxMeshes += meshes[i]->nmeshes; + } + + mesh.nmeshes = 0; + mesh.meshes = new unsigned short[maxMeshes*4]; + if (!mesh.meshes) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'pmdtl.meshes' (%d).", maxMeshes*4); + return false; + } + + mesh.ntris = 0; + mesh.tris = new unsigned char[maxTris*4]; + if (!mesh.tris) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'dmesh.tris' (%d).", maxTris*4); + return false; + } + + mesh.nverts = 0; + mesh.verts = new float[maxVerts*3]; + if (!mesh.verts) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'dmesh.verts' (%d).", maxVerts*3); + return false; + } + + // Merge datas. + for (int i = 0; i < nmeshes; ++i) + { + rcPolyMeshDetail* dm = meshes[i]; + if (!dm) continue; + for (int j = 0; j < dm->nmeshes; ++j) + { + unsigned short* dst = &mesh.meshes[mesh.nmeshes*4]; + unsigned short* src = &dm->meshes[j*4]; + dst[0] = mesh.nverts+src[0]; + dst[1] = src[1]; + dst[2] = mesh.ntris+src[2]; + dst[3] = src[3]; + mesh.nmeshes++; + } + + for (int k = 0; k < dm->nverts; ++k) + { + vcopy(&mesh.verts[mesh.nverts*3], &dm->verts[k*3]); + mesh.nverts++; + } + for (int k = 0; k < dm->ntris; ++k) + { + mesh.tris[mesh.ntris*4+0] = dm->tris[k*4+0]; + mesh.tris[mesh.ntris*4+1] = dm->tris[k*4+1]; + mesh.tris[mesh.ntris*4+2] = dm->tris[k*4+2]; + mesh.tris[mesh.ntris*4+3] = dm->tris[k*4+3]; + mesh.ntris++; + } + } + + rcTimeVal endTime = rcGetPerformanceTimer(); + + if (rcGetBuildTimes()) + rcGetBuildTimes()->mergePolyMeshDetail += rcGetDeltaTimeUsec(startTime, endTime); + + return true; +} + diff --git a/src/shared/pathfinding/Recast/RecastRasterization.cpp b/src/shared/pathfinding/Recast/RecastRasterization.cpp new file mode 100644 index 0000000..6d8e087 --- /dev/null +++ b/src/shared/pathfinding/Recast/RecastRasterization.cpp @@ -0,0 +1,356 @@ +// +// Copyright (c) 2009 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. +// + +#define _USE_MATH_DEFINES +#include +#include +#include "Recast.h" +#include "RecastTimer.h" +#include "RecastLog.h" + +inline bool overlapBounds(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; +} + +inline bool overlapInterval(unsigned short amin, unsigned short amax, + unsigned short bmin, unsigned short bmax) +{ + if (amax < bmin) return false; + if (amin > bmax) return false; + return true; +} + + +static rcSpan* allocSpan(rcHeightfield& hf) +{ + // If running out of memory, allocate new page and update the freelist. + if (!hf.freelist || !hf.freelist->next) + { + // Create new page. + // Allocate memory for the new pool. + const int size = (sizeof(rcSpanPool)-sizeof(rcSpan)) + sizeof(rcSpan)*RC_SPANS_PER_POOL; + rcSpanPool* pool = reinterpret_cast(new unsigned char[size]); + if (!pool) return 0; + pool->next = 0; + // Add the pool into the list of pools. + pool->next = hf.pools; + hf.pools = pool; + // Add new items to the free list. + rcSpan* freelist = hf.freelist; + rcSpan* head = &pool->items[0]; + rcSpan* it = &pool->items[RC_SPANS_PER_POOL]; + do + { + --it; + it->next = freelist; + freelist = it; + } + while (it != head); + hf.freelist = it; + } + + // Pop item from in front of the free list. + rcSpan* it = hf.freelist; + hf.freelist = hf.freelist->next; + return it; +} + +static void freeSpan(rcHeightfield& hf, rcSpan* ptr) +{ + if (!ptr) return; + // Add the node in front of the free list. + ptr->next = hf.freelist; + hf.freelist = ptr; +} + +void rcAddSpan(rcHeightfield& hf, const int x, const int y, + const unsigned short smin, const unsigned short smax, + const unsigned short flags, const int flagMergeThr) +{ + int idx = x + y*hf.width; + + rcSpan* s = allocSpan(hf); + s->smin = smin; + s->smax = smax; + s->flags = flags; + s->next = 0; + + // Empty cell, add he first span. + if (!hf.spans[idx]) + { + hf.spans[idx] = s; + return; + } + rcSpan* prev = 0; + rcSpan* cur = hf.spans[idx]; + + // Insert and merge spans. + while (cur) + { + if (cur->smin > s->smax) + { + // Current span is further than the new span, break. + break; + } + else if (cur->smax < s->smin) + { + // Current span is before the new span advance. + prev = cur; + cur = cur->next; + } + else + { + // Merge spans. + if (cur->smin < s->smin) + s->smin = cur->smin; + if (cur->smax > s->smax) + s->smax = cur->smax; + + // Merge flags. + if (rcAbs((int)s->smax - (int)cur->smax) <= flagMergeThr) + s->flags |= cur->flags; + + // Remove current span. + rcSpan* next = cur->next; + freeSpan(hf, cur); + if (prev) + prev->next = next; + else + hf.spans[idx] = next; + cur = next; + } + } + + // Insert new span. + if (prev) + { + s->next = prev->next; + prev->next = s; + } + else + { + s->next = hf.spans[idx]; + hf.spans[idx] = s; + } +} + +static int clipPoly(const float* in, int n, float* out, float pnx, float pnz, float pd) +{ + float d[12]; + for (int i = 0; i < n; ++i) + d[i] = pnx*in[i*3+0] + pnz*in[i*3+2] + pd; + + int m = 0; + for (int i = 0, j = n-1; i < n; j=i, ++i) + { + bool ina = d[j] >= 0; + bool inb = d[i] >= 0; + if (ina != inb) + { + float s = d[j] / (d[j] - d[i]); + out[m*3+0] = in[j*3+0] + (in[i*3+0] - in[j*3+0])*s; + out[m*3+1] = in[j*3+1] + (in[i*3+1] - in[j*3+1])*s; + out[m*3+2] = in[j*3+2] + (in[i*3+2] - in[j*3+2])*s; + m++; + } + if (inb) + { + out[m*3+0] = in[i*3+0]; + out[m*3+1] = in[i*3+1]; + out[m*3+2] = in[i*3+2]; + m++; + } + } + return m; +} + +static void rasterizeTri(const float* v0, const float* v1, const float* v2, + unsigned char flags, rcHeightfield& hf, + const float* bmin, const float* bmax, + const float cs, const float ics, const float ich, + const int flagMergeThr) +{ + const int w = hf.width; + const int h = hf.height; + float tmin[3], tmax[3]; + const float by = bmax[1] - bmin[1]; + + // Calculate the bounding box of the triangle. + vcopy(tmin, v0); + vcopy(tmax, v0); + vmin(tmin, v1); + vmin(tmin, v2); + vmax(tmax, v1); + vmax(tmax, v2); + + // If the triangle does not touch the bbox of the heightfield, skip the triagle. + if (!overlapBounds(bmin, bmax, tmin, tmax)) + return; + + // Calculate the footpring of the triangle on the grid. + int x0 = (int)((tmin[0] - bmin[0])*ics); + int y0 = (int)((tmin[2] - bmin[2])*ics); + int x1 = (int)((tmax[0] - bmin[0])*ics); + int y1 = (int)((tmax[2] - bmin[2])*ics); + x0 = rcClamp(x0, 0, w-1); + y0 = rcClamp(y0, 0, h-1); + x1 = rcClamp(x1, 0, w-1); + y1 = rcClamp(y1, 0, h-1); + + // Clip the triangle into all grid cells it touches. + float in[7*3], out[7*3], inrow[7*3]; + + for (int y = y0; y <= y1; ++y) + { + // Clip polygon to row. + vcopy(&in[0], v0); + vcopy(&in[1*3], v1); + vcopy(&in[2*3], v2); + int nvrow = 3; + const float cz = bmin[2] + y*cs; + nvrow = clipPoly(in, nvrow, out, 0, 1, -cz); + if (nvrow < 3) continue; + nvrow = clipPoly(out, nvrow, inrow, 0, -1, cz+cs); + if (nvrow < 3) continue; + + for (int x = x0; x <= x1; ++x) + { + // Clip polygon to column. + int nv = nvrow; + const float cx = bmin[0] + x*cs; + nv = clipPoly(inrow, nv, out, 1, 0, -cx); + if (nv < 3) continue; + nv = clipPoly(out, nv, in, -1, 0, cx+cs); + if (nv < 3) continue; + + // Calculate min and max of the span. + float smin = in[1], smax = in[1]; + for (int i = 1; i < nv; ++i) + { + smin = rcMin(smin, in[i*3+1]); + smax = rcMax(smax, in[i*3+1]); + } + smin -= bmin[1]; + smax -= bmin[1]; + // Skip the span if it is outside the heightfield bbox + if (smax < 0.0f) continue; + if (smin > by) continue; + // Clamp the span to the heightfield bbox. + if (smin < 0.0f) smin = 0; + if (smax > by) smax = by; + + // Snap the span to the heightfield height grid. + unsigned short ismin = (unsigned short)rcClamp((int)floorf(smin * ich), 0, 0x7fff); + unsigned short ismax = (unsigned short)rcClamp((int)ceilf(smax * ich), 0, 0x7fff); + + rcAddSpan(hf, x, y, ismin, ismax, flags, flagMergeThr); + } + } +} + +void rcRasterizeTriangle(const float* v0, const float* v1, const float* v2, + unsigned char flags, rcHeightfield& solid, + const int flagMergeThr) +{ + rcTimeVal startTime = rcGetPerformanceTimer(); + + const float ics = 1.0f/solid.cs; + const float ich = 1.0f/solid.ch; + rasterizeTri(v0, v1, v2, flags, solid, solid.bmin, solid.bmax, solid.cs, ics, ich, flagMergeThr); + + rcTimeVal endTime = rcGetPerformanceTimer(); + + if (rcGetBuildTimes()) + rcGetBuildTimes()->rasterizeTriangles += rcGetDeltaTimeUsec(startTime, endTime); +} + +void rcRasterizeTriangles(const float* verts, int nv, + const int* tris, const unsigned char* flags, int nt, + rcHeightfield& solid, const int flagMergeThr) +{ + rcTimeVal startTime = rcGetPerformanceTimer(); + + const float ics = 1.0f/solid.cs; + const float ich = 1.0f/solid.ch; + // Rasterize triangles. + for (int i = 0; i < nt; ++i) + { + const float* v0 = &verts[tris[i*3+0]*3]; + const float* v1 = &verts[tris[i*3+1]*3]; + const float* v2 = &verts[tris[i*3+2]*3]; + // Rasterize. + rasterizeTri(v0, v1, v2, flags[i], solid, solid.bmin, solid.bmax, solid.cs, ics, ich, flagMergeThr); + } + + rcTimeVal endTime = rcGetPerformanceTimer(); + + if (rcGetBuildTimes()) + rcGetBuildTimes()->rasterizeTriangles += rcGetDeltaTimeUsec(startTime, endTime); +} + +void rcRasterizeTriangles(const float* verts, int nv, + const unsigned short* tris, const unsigned char* flags, int nt, + rcHeightfield& solid, const int flagMergeThr) +{ + rcTimeVal startTime = rcGetPerformanceTimer(); + + const float ics = 1.0f/solid.cs; + const float ich = 1.0f/solid.ch; + // Rasterize triangles. + for (int i = 0; i < nt; ++i) + { + const float* v0 = &verts[tris[i*3+0]*3]; + const float* v1 = &verts[tris[i*3+1]*3]; + const float* v2 = &verts[tris[i*3+2]*3]; + // Rasterize. + rasterizeTri(v0, v1, v2, flags[i], solid, solid.bmin, solid.bmax, solid.cs, ics, ich, flagMergeThr); + } + + rcTimeVal endTime = rcGetPerformanceTimer(); + + if (rcGetBuildTimes()) + rcGetBuildTimes()->rasterizeTriangles += rcGetDeltaTimeUsec(startTime, endTime); +} + +void rcRasterizeTriangles(const float* verts, const unsigned char* flags, int nt, + rcHeightfield& solid, const int flagMergeThr) +{ + rcTimeVal startTime = rcGetPerformanceTimer(); + + const float ics = 1.0f/solid.cs; + const float ich = 1.0f/solid.ch; + // Rasterize triangles. + for (int i = 0; i < nt; ++i) + { + const float* v0 = &verts[(i*3+0)*3]; + const float* v1 = &verts[(i*3+1)*3]; + const float* v2 = &verts[(i*3+2)*3]; + // Rasterize. + rasterizeTri(v0, v1, v2, flags[i], solid, solid.bmin, solid.bmax, solid.cs, ics, ich, flagMergeThr); + } + + rcTimeVal endTime = rcGetPerformanceTimer(); + + if (rcGetBuildTimes()) + rcGetBuildTimes()->rasterizeTriangles += rcGetDeltaTimeUsec(startTime, endTime); +} diff --git a/src/shared/pathfinding/Recast/RecastRegion.cpp b/src/shared/pathfinding/Recast/RecastRegion.cpp new file mode 100644 index 0000000..636e386 --- /dev/null +++ b/src/shared/pathfinding/Recast/RecastRegion.cpp @@ -0,0 +1,1288 @@ +// +// Copyright (c) 2009 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 +#define _USE_MATH_DEFINES +#include +#include +#include +#include +#include "Recast.h" +#include "RecastLog.h" +#include "RecastTimer.h" + + +static unsigned short* calculateDistanceField(rcCompactHeightfield& chf, + unsigned short* src, unsigned short* dst, + unsigned short& maxDist) +{ + const int w = chf.width; + const int h = chf.height; + + // Init distance and points. + for (int i = 0; i < chf.spanCount; ++i) + src[i] = 0xffff; + + // Mark boundary cells. + for (int y = 0; y < h; ++y) + { + for (int x = 0; x < w; ++x) + { + const rcCompactCell& c = chf.cells[x+y*w]; + for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i) + { + const rcCompactSpan& s = chf.spans[i]; + const unsigned char area = chf.areas[i]; + + int nc = 0; + for (int dir = 0; dir < 4; ++dir) + { + if (rcGetCon(s, dir) != RC_NOT_CONNECTED) + { + const int ax = x + rcGetDirOffsetX(dir); + const int ay = y + rcGetDirOffsetY(dir); + const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, dir); + if (area == chf.areas[ai]) + nc++; + } + } + if (nc != 4) + src[i] = 0; + } + } + } + + + // Pass 1 + for (int y = 0; y < h; ++y) + { + for (int x = 0; x < w; ++x) + { + const rcCompactCell& c = chf.cells[x+y*w]; + for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i) + { + const rcCompactSpan& s = chf.spans[i]; + + if (rcGetCon(s, 0) != RC_NOT_CONNECTED) + { + // (-1,0) + const int ax = x + rcGetDirOffsetX(0); + const int ay = y + rcGetDirOffsetY(0); + const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 0); + const rcCompactSpan& as = chf.spans[ai]; + if (src[ai]+2 < src[i]) + src[i] = src[ai]+2; + + // (-1,-1) + if (rcGetCon(as, 3) != RC_NOT_CONNECTED) + { + const int aax = ax + rcGetDirOffsetX(3); + const int aay = ay + rcGetDirOffsetY(3); + const int aai = (int)chf.cells[aax+aay*w].index + rcGetCon(as, 3); + if (src[aai]+3 < src[i]) + src[i] = src[aai]+3; + } + } + if (rcGetCon(s, 3) != RC_NOT_CONNECTED) + { + // (0,-1) + const int ax = x + rcGetDirOffsetX(3); + const int ay = y + rcGetDirOffsetY(3); + const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 3); + const rcCompactSpan& as = chf.spans[ai]; + if (src[ai]+2 < src[i]) + src[i] = src[ai]+2; + + // (1,-1) + if (rcGetCon(as, 2) != RC_NOT_CONNECTED) + { + const int aax = ax + rcGetDirOffsetX(2); + const int aay = ay + rcGetDirOffsetY(2); + const int aai = (int)chf.cells[aax+aay*w].index + rcGetCon(as, 2); + if (src[aai]+3 < src[i]) + src[i] = src[aai]+3; + } + } + } + } + } + + // Pass 2 + for (int y = h-1; y >= 0; --y) + { + for (int x = w-1; x >= 0; --x) + { + const rcCompactCell& c = chf.cells[x+y*w]; + for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i) + { + const rcCompactSpan& s = chf.spans[i]; + + if (rcGetCon(s, 2) != RC_NOT_CONNECTED) + { + // (1,0) + const int ax = x + rcGetDirOffsetX(2); + const int ay = y + rcGetDirOffsetY(2); + const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 2); + const rcCompactSpan& as = chf.spans[ai]; + if (src[ai]+2 < src[i]) + src[i] = src[ai]+2; + + // (1,1) + if (rcGetCon(as, 1) != RC_NOT_CONNECTED) + { + const int aax = ax + rcGetDirOffsetX(1); + const int aay = ay + rcGetDirOffsetY(1); + const int aai = (int)chf.cells[aax+aay*w].index + rcGetCon(as, 1); + if (src[aai]+3 < src[i]) + src[i] = src[aai]+3; + } + } + if (rcGetCon(s, 1) != RC_NOT_CONNECTED) + { + // (0,1) + const int ax = x + rcGetDirOffsetX(1); + const int ay = y + rcGetDirOffsetY(1); + const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 1); + const rcCompactSpan& as = chf.spans[ai]; + if (src[ai]+2 < src[i]) + src[i] = src[ai]+2; + + // (-1,1) + if (rcGetCon(as, 0) != RC_NOT_CONNECTED) + { + const int aax = ax + rcGetDirOffsetX(0); + const int aay = ay + rcGetDirOffsetY(0); + const int aai = (int)chf.cells[aax+aay*w].index + rcGetCon(as, 0); + if (src[aai]+3 < src[i]) + src[i] = src[aai]+3; + } + } + } + } + } + + maxDist = 0; + for (int i = 0; i < chf.spanCount; ++i) + maxDist = rcMax(src[i], maxDist); + + return src; + +} + +static unsigned short* boxBlur(rcCompactHeightfield& chf, int thr, + unsigned short* src, unsigned short* dst) +{ + const int w = chf.width; + const int h = chf.height; + + thr *= 2; + + for (int y = 0; y < h; ++y) + { + for (int x = 0; x < w; ++x) + { + const rcCompactCell& c = chf.cells[x+y*w]; + for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i) + { + const rcCompactSpan& s = chf.spans[i]; + int cd = (int)src[i]; + if (cd <= thr) + { + dst[i] = cd; + continue; + } + + int d = cd; + for (int dir = 0; dir < 4; ++dir) + { + if (rcGetCon(s, dir) != RC_NOT_CONNECTED) + { + const int ax = x + rcGetDirOffsetX(dir); + const int ay = y + rcGetDirOffsetY(dir); + const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, dir); + d += (int)src[ai]; + + const rcCompactSpan& as = chf.spans[ai]; + const int dir2 = (dir+1) & 0x3; + if (rcGetCon(as, dir2) != RC_NOT_CONNECTED) + { + const int ax2 = ax + rcGetDirOffsetX(dir2); + const int ay2 = ay + rcGetDirOffsetY(dir2); + const int ai2 = (int)chf.cells[ax2+ay2*w].index + rcGetCon(as, dir2); + d += (int)src[ai2]; + } + else + { + d += cd; + } + } + else + { + d += cd*2; + } + } + dst[i] = (unsigned short)((d+5)/9); + } + } + } + return dst; +} + + +static bool floodRegion(int x, int y, int i, + unsigned short level, unsigned short r, + rcCompactHeightfield& chf, + unsigned short* srcReg, unsigned short* srcDist, + rcIntArray& stack) +{ + const int w = chf.width; + + const unsigned char area = chf.areas[i]; + + // Flood fill mark region. + stack.resize(0); + stack.push((int)x); + stack.push((int)y); + stack.push((int)i); + srcReg[i] = r; + srcDist[i] = 0; + + unsigned short lev = level >= 2 ? level-2 : 0; + int count = 0; + + while (stack.size() > 0) + { + int ci = stack.pop(); + int cy = stack.pop(); + int cx = stack.pop(); + + const rcCompactSpan& cs = chf.spans[ci]; + + // Check if any of the neighbours already have a valid region set. + unsigned short ar = 0; + for (int dir = 0; dir < 4; ++dir) + { + // 8 connected + if (rcGetCon(cs, dir) != RC_NOT_CONNECTED) + { + const int ax = cx + rcGetDirOffsetX(dir); + const int ay = cy + rcGetDirOffsetY(dir); + const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(cs, dir); + if (chf.areas[ai] != area) + continue; + unsigned short nr = srcReg[ai]; + if (nr != 0 && nr != r) + ar = nr; + + const rcCompactSpan& as = chf.spans[ai]; + + const int dir2 = (dir+1) & 0x3; + if (rcGetCon(as, dir2) != RC_NOT_CONNECTED) + { + const int ax2 = ax + rcGetDirOffsetX(dir2); + const int ay2 = ay + rcGetDirOffsetY(dir2); + const int ai2 = (int)chf.cells[ax2+ay2*w].index + rcGetCon(as, dir2); + if (chf.areas[ai2] != area) + continue; + unsigned short nr = srcReg[ai2]; + if (nr != 0 && nr != r) + ar = nr; + } + } + } + if (ar != 0) + { + srcReg[ci] = 0; + continue; + } + count++; + + // Expand neighbours. + for (int dir = 0; dir < 4; ++dir) + { + if (rcGetCon(cs, dir) != RC_NOT_CONNECTED) + { + const int ax = cx + rcGetDirOffsetX(dir); + const int ay = cy + rcGetDirOffsetY(dir); + const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(cs, dir); + if (chf.areas[ai] != area) + continue; + if (chf.dist[ai] >= lev) + { + if (srcReg[ai] == 0) + { + srcReg[ai] = r; + srcDist[ai] = 0; + stack.push(ax); + stack.push(ay); + stack.push(ai); + } + } + } + } + } + + return count > 0; +} + +static unsigned short* expandRegions(int maxIter, unsigned short level, + rcCompactHeightfield& chf, + unsigned short* srcReg, unsigned short* srcDist, + unsigned short* dstReg, unsigned short* dstDist, + rcIntArray& stack) +{ + const int w = chf.width; + const int h = chf.height; + + // Find cells revealed by the raised level. + stack.resize(0); + for (int y = 0; y < h; ++y) + { + for (int x = 0; x < w; ++x) + { + const rcCompactCell& c = chf.cells[x+y*w]; + for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i) + { + if (chf.dist[i] >= level && srcReg[i] == 0 && chf.areas[i] != RC_NULL_AREA) + { + stack.push(x); + stack.push(y); + stack.push(i); + } + } + } + } + + int iter = 0; + while (stack.size() > 0) + { + int failed = 0; + + memcpy(dstReg, srcReg, sizeof(unsigned short)*chf.spanCount); + memcpy(dstDist, srcDist, sizeof(unsigned short)*chf.spanCount); + + for (int j = 0; j < stack.size(); j += 3) + { + int x = stack[j+0]; + int y = stack[j+1]; + int i = stack[j+2]; + if (i < 0) + { + failed++; + continue; + } + + unsigned short r = srcReg[i]; + unsigned short d2 = 0xffff; + const unsigned char area = chf.areas[i]; + const rcCompactSpan& s = chf.spans[i]; + for (int dir = 0; dir < 4; ++dir) + { + if (rcGetCon(s, dir) == RC_NOT_CONNECTED) continue; + const int ax = x + rcGetDirOffsetX(dir); + const int ay = y + rcGetDirOffsetY(dir); + const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, dir); + if (chf.areas[ai] != area) continue; + if (srcReg[ai] > 0 && (srcReg[ai] & RC_BORDER_REG) == 0) + { + if ((int)srcDist[ai]+2 < (int)d2) + { + r = srcReg[ai]; + d2 = srcDist[ai]+2; + } + } + } + if (r) + { + stack[j+2] = -1; // mark as used + dstReg[i] = r; + dstDist[i] = d2; + } + else + { + failed++; + } + } + + // rcSwap source and dest. + rcSwap(srcReg, dstReg); + rcSwap(srcDist, dstDist); + + if (failed*3 == stack.size()) + break; + + if (level > 0) + { + ++iter; + if (iter >= maxIter) + break; + } + } + + return srcReg; +} + + +struct rcRegion +{ + inline rcRegion() : count(0), id(0), area(0), remap(false) {} + + int count; + unsigned short id; + unsigned char area; + bool remap; + rcIntArray connections; + rcIntArray floors; +}; + +static void removeAdjacentNeighbours(rcRegion& reg) +{ + // Remove adjacent duplicates. + for (int i = 0; i < reg.connections.size() && reg.connections.size() > 1; ) + { + int ni = (i+1) % reg.connections.size(); + if (reg.connections[i] == reg.connections[ni]) + { + // Remove duplicate + for (int j = i; j < reg.connections.size()-1; ++j) + reg.connections[j] = reg.connections[j+1]; + reg.connections.pop(); + } + else + ++i; + } +} + +static void replaceNeighbour(rcRegion& reg, unsigned short oldId, unsigned short newId) +{ + bool neiChanged = false; + for (int i = 0; i < reg.connections.size(); ++i) + { + if (reg.connections[i] == oldId) + { + reg.connections[i] = newId; + neiChanged = true; + } + } + for (int i = 0; i < reg.floors.size(); ++i) + { + if (reg.floors[i] == oldId) + reg.floors[i] = newId; + } + if (neiChanged) + removeAdjacentNeighbours(reg); +} + +static bool canMergeWithRegion(const rcRegion& rega, const rcRegion& regb) +{ + if (rega.area != regb.area) + return false; + int n = 0; + for (int i = 0; i < rega.connections.size(); ++i) + { + if (rega.connections[i] == regb.id) + n++; + } + if (n > 1) + return false; + for (int i = 0; i < rega.floors.size(); ++i) + { + if (rega.floors[i] == regb.id) + return false; + } + return true; +} + +static void addUniqueFloorRegion(rcRegion& reg, unsigned short n) +{ + for (int i = 0; i < reg.floors.size(); ++i) + if (reg.floors[i] == n) + return; + reg.floors.push(n); +} + +static bool mergeRegions(rcRegion& rega, rcRegion& regb) +{ + unsigned short aid = rega.id; + unsigned short bid = regb.id; + + // Duplicate current neighbourhood. + rcIntArray acon; + acon.resize(rega.connections.size()); + for (int i = 0; i < rega.connections.size(); ++i) + acon[i] = rega.connections[i]; + rcIntArray& bcon = regb.connections; + + // Find insertion point on A. + int insa = -1; + for (int i = 0; i < acon.size(); ++i) + { + if (acon[i] == bid) + { + insa = i; + break; + } + } + if (insa == -1) + return false; + + // Find insertion point on B. + int insb = -1; + for (int i = 0; i < bcon.size(); ++i) + { + if (bcon[i] == aid) + { + insb = i; + break; + } + } + if (insb == -1) + return false; + + // Merge neighbours. + rega.connections.resize(0); + for (int i = 0, ni = acon.size(); i < ni-1; ++i) + rega.connections.push(acon[(insa+1+i) % ni]); + + for (int i = 0, ni = bcon.size(); i < ni-1; ++i) + rega.connections.push(bcon[(insb+1+i) % ni]); + + removeAdjacentNeighbours(rega); + + for (int j = 0; j < regb.floors.size(); ++j) + addUniqueFloorRegion(rega, regb.floors[j]); + rega.count += regb.count; + regb.count = 0; + regb.connections.resize(0); + + return true; +} + +static bool isRegionConnectedToBorder(const rcRegion& reg) +{ + // Region is connected to border if + // one of the neighbours is null id. + for (int i = 0; i < reg.connections.size(); ++i) + { + if (reg.connections[i] == 0) + return true; + } + return false; +} + +static bool isSolidEdge(rcCompactHeightfield& chf, unsigned short* srcReg, + int x, int y, int i, int dir) +{ + const rcCompactSpan& s = chf.spans[i]; + unsigned short r = 0; + if (rcGetCon(s, dir) != RC_NOT_CONNECTED) + { + const int ax = x + rcGetDirOffsetX(dir); + const int ay = y + rcGetDirOffsetY(dir); + const int ai = (int)chf.cells[ax+ay*chf.width].index + rcGetCon(s, dir); + r = srcReg[ai]; + } + if (r == srcReg[i]) + return false; + return true; +} + +static void walkContour(int x, int y, int i, int dir, + rcCompactHeightfield& chf, + unsigned short* srcReg, + rcIntArray& cont) +{ + int startDir = dir; + int starti = i; + + const rcCompactSpan& ss = chf.spans[i]; + unsigned short curReg = 0; + if (rcGetCon(ss, dir) != RC_NOT_CONNECTED) + { + const int ax = x + rcGetDirOffsetX(dir); + const int ay = y + rcGetDirOffsetY(dir); + const int ai = (int)chf.cells[ax+ay*chf.width].index + rcGetCon(ss, dir); + curReg = srcReg[ai]; + } + cont.push(curReg); + + int iter = 0; + while (++iter < 40000) + { + const rcCompactSpan& s = chf.spans[i]; + + if (isSolidEdge(chf, srcReg, x, y, i, dir)) + { + // Choose the edge corner + unsigned short r = 0; + if (rcGetCon(s, dir) != RC_NOT_CONNECTED) + { + const int ax = x + rcGetDirOffsetX(dir); + const int ay = y + rcGetDirOffsetY(dir); + const int ai = (int)chf.cells[ax+ay*chf.width].index + rcGetCon(s, dir); + r = srcReg[ai]; + } + if (r != curReg) + { + curReg = r; + cont.push(curReg); + } + + dir = (dir+1) & 0x3; // Rotate CW + } + else + { + int ni = -1; + const int nx = x + rcGetDirOffsetX(dir); + const int ny = y + rcGetDirOffsetY(dir); + if (rcGetCon(s, dir) != RC_NOT_CONNECTED) + { + const rcCompactCell& nc = chf.cells[nx+ny*chf.width]; + ni = (int)nc.index + rcGetCon(s, dir); + } + if (ni == -1) + { + // Should not happen. + return; + } + x = nx; + y = ny; + i = ni; + dir = (dir+3) & 0x3; // Rotate CCW + } + + if (starti == i && startDir == dir) + { + break; + } + } + + // Remove adjacent duplicates. + if (cont.size() > 1) + { + for (int i = 0; i < cont.size(); ) + { + int ni = (i+1) % cont.size(); + if (cont[i] == cont[ni]) + { + for (int j = i; j < cont.size()-1; ++j) + cont[j] = cont[j+1]; + cont.pop(); + } + else + ++i; + } + } +} + +static bool filterSmallRegions(int minRegionSize, int mergeRegionSize, + unsigned short& maxRegionId, + rcCompactHeightfield& chf, + unsigned short* srcReg) +{ + const int w = chf.width; + const int h = chf.height; + + int nreg = maxRegionId+1; + rcRegion* regions = new rcRegion[nreg]; + if (!regions) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "filterSmallRegions: Out of memory 'regions' (%d).", nreg); + return false; + } + + for (int i = 0; i < nreg; ++i) + regions[i].id = (unsigned short)i; + + // Find edge of a region and find connections around the contour. + for (int y = 0; y < h; ++y) + { + for (int x = 0; x < w; ++x) + { + const rcCompactCell& c = chf.cells[x+y*w]; + for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i) + { + unsigned short r = srcReg[i]; + if (r == 0 || r >= nreg) + continue; + + rcRegion& reg = regions[r]; + reg.count++; + + + // Update floors. + for (int j = (int)c.index; j < ni; ++j) + { + if (i == j) continue; + unsigned short floorId = srcReg[j]; + if (floorId == 0 || floorId >= nreg) + continue; + addUniqueFloorRegion(reg, floorId); + } + + // Have found contour + if (reg.connections.size() > 0) + continue; + + reg.area = chf.areas[i]; + + // Check if this cell is next to a border. + int ndir = -1; + for (int dir = 0; dir < 4; ++dir) + { + if (isSolidEdge(chf, srcReg, x, y, i, dir)) + { + ndir = dir; + break; + } + } + + if (ndir != -1) + { + // The cell is at border. + // Walk around the contour to find all the neighbours. + walkContour(x, y, i, ndir, chf, srcReg, reg.connections); + } + } + } + } + + // Remove too small unconnected regions. + for (int i = 0; i < nreg; ++i) + { + rcRegion& reg = regions[i]; + if (reg.id == 0 || (reg.id & RC_BORDER_REG)) + continue; + if (reg.count == 0) + continue; + + if (reg.connections.size() == 1 && reg.connections[0] == 0) + { + if (reg.count < minRegionSize) + { + // Non-connected small region, remove. + reg.count = 0; + reg.id = 0; + } + } + } + + + // Merge too small regions to neighbour regions. + int mergeCount = 0 ; + do + { + mergeCount = 0; + for (int i = 0; i < nreg; ++i) + { + rcRegion& reg = regions[i]; + if (reg.id == 0 || (reg.id & RC_BORDER_REG)) + continue; + if (reg.count == 0) + continue; + + // Check to see if the region should be merged. + if (reg.count > mergeRegionSize && isRegionConnectedToBorder(reg)) + continue; + + // Small region with more than 1 connection. + // Or region which is not connected to a border at all. + // Find smallest neighbour region that connects to this one. + int smallest = 0xfffffff; + unsigned short mergeId = reg.id; + for (int j = 0; j < reg.connections.size(); ++j) + { + if (reg.connections[j] & RC_BORDER_REG) continue; + rcRegion& mreg = regions[reg.connections[j]]; + if (mreg.id == 0 || (mreg.id & RC_BORDER_REG)) continue; + if (mreg.count < smallest && + canMergeWithRegion(reg, mreg) && + canMergeWithRegion(mreg, reg)) + { + smallest = mreg.count; + mergeId = mreg.id; + } + } + // Found new id. + if (mergeId != reg.id) + { + unsigned short oldId = reg.id; + rcRegion& target = regions[mergeId]; + + // Merge neighbours. + if (mergeRegions(target, reg)) + { + // Fixup regions pointing to current region. + for (int j = 0; j < nreg; ++j) + { + if (regions[j].id == 0 || (regions[j].id & RC_BORDER_REG)) continue; + // If another region was already merged into current region + // change the nid of the previous region too. + if (regions[j].id == oldId) + regions[j].id = mergeId; + // Replace the current region with the new one if the + // current regions is neighbour. + replaceNeighbour(regions[j], oldId, mergeId); + } + mergeCount++; + } + } + } + } + while (mergeCount > 0); + + // Compress region Ids. + for (int i = 0; i < nreg; ++i) + { + regions[i].remap = false; + if (regions[i].id == 0) continue; // Skip nil regions. + if (regions[i].id & RC_BORDER_REG) continue; // Skip external regions. + regions[i].remap = true; + } + + unsigned short regIdGen = 0; + for (int i = 0; i < nreg; ++i) + { + if (!regions[i].remap) + continue; + unsigned short oldId = regions[i].id; + unsigned short newId = ++regIdGen; + for (int j = i; j < nreg; ++j) + { + if (regions[j].id == oldId) + { + regions[j].id = newId; + regions[j].remap = false; + } + } + } + maxRegionId = regIdGen; + + // Remap regions. + for (int i = 0; i < chf.spanCount; ++i) + { + if ((srcReg[i] & RC_BORDER_REG) == 0) + srcReg[i] = regions[srcReg[i]].id; + } + + delete [] regions; + + return true; +} + + +bool rcBuildDistanceField(rcCompactHeightfield& chf) +{ + rcTimeVal startTime = rcGetPerformanceTimer(); + + if (chf.dist) + { + delete [] chf.dist; + chf.dist = 0; + } + + unsigned short* dist0 = new unsigned short[chf.spanCount]; + if (!dist0) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildDistanceField: Out of memory 'dist0' (%d).", chf.spanCount); + return false; + } + unsigned short* dist1 = new unsigned short[chf.spanCount]; + if (!dist1) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildDistanceField: Out of memory 'dist1' (%d).", chf.spanCount); + delete [] dist0; + return false; + } + + unsigned short* src = dist0; + unsigned short* dst = dist1; + + unsigned short maxDist = 0; + + rcTimeVal distStartTime = rcGetPerformanceTimer(); + + if (calculateDistanceField(chf, src, dst, maxDist) != src) + rcSwap(src, dst); + + chf.maxDistance = maxDist; + + rcTimeVal distEndTime = rcGetPerformanceTimer(); + + rcTimeVal blurStartTime = rcGetPerformanceTimer(); + + // Blur + if (boxBlur(chf, 1, src, dst) != src) + rcSwap(src, dst); + + // Store distance. + chf.dist = src; + + rcTimeVal blurEndTime = rcGetPerformanceTimer(); + + delete [] dst; + + rcTimeVal endTime = rcGetPerformanceTimer(); + +/* if (rcGetLog()) + { + rcGetLog()->log(RC_LOG_PROGRESS, "Build distance field: %.3f ms", rcGetDeltaTimeUsec(startTime, endTime)/1000.0f); + rcGetLog()->log(RC_LOG_PROGRESS, " - dist: %.3f ms", rcGetDeltaTimeUsec(distStartTime, distEndTime)/1000.0f); + rcGetLog()->log(RC_LOG_PROGRESS, " - blur: %.3f ms", rcGetDeltaTimeUsec(blurStartTime, blurEndTime)/1000.0f); + }*/ + if (rcGetBuildTimes()) + { + rcGetBuildTimes()->buildDistanceField += rcGetDeltaTimeUsec(startTime, endTime); + rcGetBuildTimes()->buildDistanceFieldDist += rcGetDeltaTimeUsec(distStartTime, distEndTime); + rcGetBuildTimes()->buildDistanceFieldBlur += rcGetDeltaTimeUsec(blurStartTime, blurEndTime); + } + + return true; +} + +static void paintRectRegion(int minx, int maxx, int miny, int maxy, + unsigned short regId, + rcCompactHeightfield& chf, unsigned short* srcReg) +{ + const int w = chf.width; + for (int y = miny; y < maxy; ++y) + { + for (int x = minx; x < maxx; ++x) + { + const rcCompactCell& c = chf.cells[x+y*w]; + for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i) + { + if (chf.areas[i] != RC_NULL_AREA) + srcReg[i] = regId; + } + } + } +} + + +static const unsigned short RC_NULL_NEI = 0xffff; + +struct rcSweepSpan +{ + unsigned short rid; // row id + unsigned short id; // region id + unsigned short ns; // number samples + unsigned short nei; // neighbour id +}; + +bool rcBuildRegionsMonotone(rcCompactHeightfield& chf, + int borderSize, int minRegionSize, int mergeRegionSize) +{ + rcTimeVal startTime = rcGetPerformanceTimer(); + + const int w = chf.width; + const int h = chf.height; + unsigned short id = 1; + + if (chf.regs) + { + delete [] chf.regs; + chf.regs = 0; + } + + rcScopedDelete srcReg = new unsigned short[chf.spanCount]; + if (!srcReg) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildRegionsMonotone: Out of memory 'src' (%d).", chf.spanCount); + return false; + } + memset(srcReg,0,sizeof(unsigned short)*chf.spanCount); + + rcScopedDelete sweeps = new rcSweepSpan[rcMax(chf.width,chf.height)]; + if (!sweeps) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildRegionsMonotone: Out of memory 'sweeps' (%d).", chf.width); + return false; + } + + + // Mark border regions. + if (borderSize) + { + paintRectRegion(0, borderSize, 0, h, id|RC_BORDER_REG, chf, srcReg); id++; + paintRectRegion(w-borderSize, w, 0, h, id|RC_BORDER_REG, chf, srcReg); id++; + paintRectRegion(0, w, 0, borderSize, id|RC_BORDER_REG, chf, srcReg); id++; + paintRectRegion(0, w, h-borderSize, h, id|RC_BORDER_REG, chf, srcReg); id++; + } + + rcIntArray prev(256); + + // Sweep one line at a time. + for (int y = borderSize; y < h-borderSize; ++y) + { + // Collect spans from this row. + prev.resize(id+1); + memset(&prev[0],0,sizeof(int)*id); + unsigned short rid = 1; + + for (int x = borderSize; x < w-borderSize; ++x) + { + const rcCompactCell& c = chf.cells[x+y*w]; + + for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i) + { + const rcCompactSpan& s = chf.spans[i]; + if (chf.areas[i] == RC_NULL_AREA) continue; + + // -x + unsigned short previd = 0; + if (rcGetCon(s, 0) != RC_NOT_CONNECTED) + { + const int ax = x + rcGetDirOffsetX(0); + const int ay = y + rcGetDirOffsetY(0); + const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 0); + if ((srcReg[ai] & RC_BORDER_REG) == 0 && chf.areas[i] == chf.areas[ai]) + previd = srcReg[ai]; + } + + if (!previd) + { + previd = rid++; + sweeps[previd].rid = previd; + sweeps[previd].ns = 0; + sweeps[previd].nei = 0; + } + + // -y + if (rcGetCon(s,3) != RC_NOT_CONNECTED) + { + const int ax = x + rcGetDirOffsetX(3); + const int ay = y + rcGetDirOffsetY(3); + const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 3); + if (srcReg[ai] && (srcReg[ai] & RC_BORDER_REG) == 0 && chf.areas[i] == chf.areas[ai]) + { + unsigned short nr = srcReg[ai]; + if (!sweeps[previd].nei || sweeps[previd].nei == nr) + { + sweeps[previd].nei = nr; + sweeps[previd].ns++; + prev[nr]++; + } + else + { + sweeps[previd].nei = RC_NULL_NEI; + } + } + } + + srcReg[i] = previd; + } + } + + // Create unique ID. + for (int i = 1; i < rid; ++i) + { + if (sweeps[i].nei != RC_NULL_NEI && sweeps[i].nei != 0 && + prev[sweeps[i].nei] == (int)sweeps[i].ns) + { + sweeps[i].id = sweeps[i].nei; + } + else + { + sweeps[i].id = id++; + } + } + + // Remap IDs + for (int x = borderSize; x < w-borderSize; ++x) + { + const rcCompactCell& c = chf.cells[x+y*w]; + + for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i) + { + if (srcReg[i] > 0 && srcReg[i] < rid) + srcReg[i] = sweeps[srcReg[i]].id; + } + } + } + + rcTimeVal filterStartTime = rcGetPerformanceTimer(); + + // Filter out small regions. + chf.maxRegions = id; + if (!filterSmallRegions(minRegionSize, mergeRegionSize, chf.maxRegions, chf, srcReg)) + return false; + + rcTimeVal filterEndTime = rcGetPerformanceTimer(); + + // Store the result out. + chf.regs = srcReg; + srcReg = 0; + + rcTimeVal endTime = rcGetPerformanceTimer(); + + if (rcGetBuildTimes()) + { + rcGetBuildTimes()->buildRegions += rcGetDeltaTimeUsec(startTime, endTime); + rcGetBuildTimes()->buildRegionsFilter += rcGetDeltaTimeUsec(filterStartTime, filterEndTime); + } + + return true; +} + +bool rcBuildRegions(rcCompactHeightfield& chf, + int borderSize, int minRegionSize, int mergeRegionSize) +{ + rcTimeVal startTime = rcGetPerformanceTimer(); + + const int w = chf.width; + const int h = chf.height; + + if (!chf.regs) + { + chf.regs = new unsigned short[chf.spanCount]; + if (!chf.regs) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildRegions: Out of memory 'chf.reg' (%d).", chf.spanCount); + return false; + } + } + + rcScopedDelete tmp = new unsigned short[chf.spanCount*4]; + if (!tmp) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildRegions: Out of memory 'tmp' (%d).", chf.spanCount*4); + return false; + } + + rcTimeVal regStartTime = rcGetPerformanceTimer(); + + rcIntArray stack(1024); + rcIntArray visited(1024); + + unsigned short* srcReg = tmp; + unsigned short* srcDist = tmp+chf.spanCount; + unsigned short* dstReg = tmp+chf.spanCount*2; + unsigned short* dstDist = tmp+chf.spanCount*3; + + memset(srcReg, 0, sizeof(unsigned short)*chf.spanCount); + memset(srcDist, 0, sizeof(unsigned short)*chf.spanCount); + + unsigned short regionId = 1; + unsigned short level = (chf.maxDistance+1) & ~1; + + // TODO: Figure better formula, expandIters defines how much the + // watershed "overflows" and simplifies the regions. Tying it to + // agent radius was usually good indication how greedy it could be. +// const int expandIters = 4 + walkableRadius * 2; + const int expandIters = 8; + + // Mark border regions. + paintRectRegion(0, borderSize, 0, h, regionId|RC_BORDER_REG, chf, srcReg); regionId++; + paintRectRegion(w-borderSize, w, 0, h, regionId|RC_BORDER_REG, chf, srcReg); regionId++; + paintRectRegion(0, w, 0, borderSize, regionId|RC_BORDER_REG, chf, srcReg); regionId++; + paintRectRegion(0, w, h-borderSize, h, regionId|RC_BORDER_REG, chf, srcReg); regionId++; + + rcTimeVal expTime = 0; + rcTimeVal floodTime = 0; + + while (level > 0) + { + level = level >= 2 ? level-2 : 0; + + rcTimeVal expStartTime = rcGetPerformanceTimer(); + + // Expand current regions until no empty connected cells found. + if (expandRegions(expandIters, level, chf, srcReg, srcDist, dstReg, dstDist, stack) != srcReg) + { + rcSwap(srcReg, dstReg); + rcSwap(srcDist, dstDist); + } + + expTime += rcGetPerformanceTimer() - expStartTime; + + rcTimeVal floodStartTime = rcGetPerformanceTimer(); + + // Mark new regions with IDs. + for (int y = 0; y < h; ++y) + { + for (int x = 0; x < w; ++x) + { + const rcCompactCell& c = chf.cells[x+y*w]; + for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i) + { + if (chf.dist[i] < level || srcReg[i] != 0 || chf.areas[i] == RC_NULL_AREA) + continue; + + if (floodRegion(x, y, i, level, regionId, chf, srcReg, srcDist, stack)) + regionId++; + } + } + } + + floodTime += rcGetPerformanceTimer() - floodStartTime; + + } + + // Expand current regions until no empty connected cells found. + if (expandRegions(expandIters*8, 0, chf, srcReg, srcDist, dstReg, dstDist, stack) != srcReg) + { + rcSwap(srcReg, dstReg); + rcSwap(srcDist, dstDist); + } + + rcTimeVal regEndTime = rcGetPerformanceTimer(); + + rcTimeVal filterStartTime = rcGetPerformanceTimer(); + + // Filter out small regions. + chf.maxRegions = regionId; + if (!filterSmallRegions(minRegionSize, mergeRegionSize, chf.maxRegions, chf, srcReg)) + return false; + + rcTimeVal filterEndTime = rcGetPerformanceTimer(); + + // Write the result out. + memcpy(chf.regs, srcReg, sizeof(unsigned short)*chf.spanCount); + + rcTimeVal endTime = rcGetPerformanceTimer(); + +/* if (rcGetLog()) + { + rcGetLog()->log(RC_LOG_PROGRESS, "Build regions: %.3f ms", rcGetDeltaTimeUsec(startTime, endTime)/1000.0f); + rcGetLog()->log(RC_LOG_PROGRESS, " - reg: %.3f ms", rcGetDeltaTimeUsec(regStartTime, regEndTime)/1000.0f); + rcGetLog()->log(RC_LOG_PROGRESS, " - exp: %.3f ms", rcGetDeltaTimeUsec(0, expTime)/1000.0f); + rcGetLog()->log(RC_LOG_PROGRESS, " - flood: %.3f ms", rcGetDeltaTimeUsec(0, floodTime)/1000.0f); + rcGetLog()->log(RC_LOG_PROGRESS, " - filter: %.3f ms", rcGetDeltaTimeUsec(filterStartTime, filterEndTime)/1000.0f); + } +*/ + if (rcGetBuildTimes()) + { + rcGetBuildTimes()->buildRegions += rcGetDeltaTimeUsec(startTime, endTime); + rcGetBuildTimes()->buildRegionsReg += rcGetDeltaTimeUsec(regStartTime, regEndTime); + rcGetBuildTimes()->buildRegionsExp += rcGetDeltaTimeUsec(0, expTime); + rcGetBuildTimes()->buildRegionsFlood += rcGetDeltaTimeUsec(0, floodTime); + rcGetBuildTimes()->buildRegionsFilter += rcGetDeltaTimeUsec(filterStartTime, filterEndTime); + } + + return true; +} + + diff --git a/src/shared/pathfinding/Recast/RecastTimer.cpp b/src/shared/pathfinding/Recast/RecastTimer.cpp new file mode 100644 index 0000000..a010bdc --- /dev/null +++ b/src/shared/pathfinding/Recast/RecastTimer.cpp @@ -0,0 +1,43 @@ +#include "RecastTimer.h" + +#if defined(WIN32) + +// Win32 +#include + +rcTimeVal rcGetPerformanceTimer() +{ + __int64 count; + QueryPerformanceCounter((LARGE_INTEGER*)&count); + return count; +} + +int rcGetDeltaTimeUsec(rcTimeVal start, rcTimeVal end) +{ + static __int64 freq = 0; + if (freq == 0) + QueryPerformanceFrequency((LARGE_INTEGER*)&freq); + __int64 elapsed = end - start; + return (int)(elapsed*1000000 / freq); +} + +#else + +// Linux, BSD, OSX + +#include +#include + +rcTimeVal rcGetPerformanceTimer() +{ + timeval now; + gettimeofday(&now, 0); + return (rcTimeVal)now.tv_sec*1000000L + (rcTimeVal)now.tv_usec; +} + +int rcGetDeltaTimeUsec(rcTimeVal start, rcTimeVal end) +{ + return (int)(end - start); +} + +#endif diff --git a/src/shared/pathfinding/Recast/RecastTimer.h b/src/shared/pathfinding/Recast/RecastTimer.h new file mode 100644 index 0000000..d4f21e5 --- /dev/null +++ b/src/shared/pathfinding/Recast/RecastTimer.h @@ -0,0 +1,31 @@ +// +// Copyright (c) 2009 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// +#ifndef RECAST_TIMER_H +#define RECAST_TIMER_H + +#ifdef __GNUC__ +#include +typedef int64_t rcTimeVal; +#else +typedef __int64 rcTimeVal; +#endif + +rcTimeVal rcGetPerformanceTimer(); +int rcGetDeltaTimeUsec(rcTimeVal start, rcTimeVal end); + +#endif // RECAST_TIMER_H diff --git a/win/VC90/shared.vcproj b/win/VC90/shared.vcproj index 91ae225..f15e1aa 100644 --- a/win/VC90/shared.vcproj +++ b/win/VC90/shared.vcproj @@ -101,7 +101,7 @@ /> + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -886,18 +1006,18 @@ /> @@ -971,7 +1091,7 @@ />