// This function checks if the path has a small U-turn, that is, // a polygon further in the path is adjacent to the first polygon // in the path. If that happens, a shortcut is taken. // This can happen if the target (T) location is at tile boundary, // and we're (S) approaching it parallel to the tile edge. // The choice at the vertex can be arbitrary, // +---+---+ // |:::|:::| // +-S-+-T-+ // |:::| | <-- the step can end up in here, resulting U-turn path. // +---+---+ static int fixupShortcuts(dtPolyRef[] path, int npath, Detour.dtNavMeshQuery navQuery) { if (npath < 3) return npath; // Get connected polygons const int maxNeis = 16; dtPolyRef[] neis = new dtPolyRef[maxNeis]; int nneis = 0; Detour.dtMeshTile tile = null; Detour.dtPoly poly = null; if (Detour.dtStatusFailed(navQuery.getAttachedNavMesh().getTileAndPolyByRef(path[0], ref tile, ref poly))) return npath; for (uint k = poly.firstLink; k != Detour.DT_NULL_LINK; k = tile.links[k].next) { Detour.dtLink link = tile.links[k]; if (link.polyRef != 0) { if (nneis < maxNeis) neis[nneis++] = link.polyRef; } } // If any of the neighbour polygons is within the next few polygons // in the path, short cut to that polygon directly. const int maxLookAhead = 6; int cut = 0; for (int i = Math.Min(maxLookAhead, npath) - 1; i > 1 && cut == 0; i--) { for (int j = 0; j < nneis; j++) { if (path[i] == neis[j]) { cut = i; break; } } } if (cut > 1) { int offset = cut-1; npath -= offset; for (int i = 1; i < npath; i++) path[i] = path[i+offset]; } return npath; }
public static SmoothPath ComputeSmoothPath(Detour.dtNavMeshQuery navQuery, float[] startWorldPos, float[] endWorldPos) { SmoothPath smoothPath = new SmoothPath(); if (navQuery == null){ return smoothPath; } float[] extents = new float[3]; for (int i=0;i<3;++i){ extents[i] = 10.0f; } dtPolyRef startRef = 0; dtPolyRef endRef = 0; float[] startPt = new float[3]; float[] endPt = new float[3]; Detour.dtQueryFilter filter = new Detour.dtQueryFilter(); navQuery.findNearestPoly(startWorldPos, extents, filter, ref startRef, ref startPt); navQuery.findNearestPoly(endWorldPos, extents, filter, ref endRef, ref endPt); const int maxPath = SmoothPath.MAX_POLYS; dtPolyRef[] path = new dtPolyRef[maxPath]; int pathCount = -1; navQuery.findPath(startRef, endRef, startPt, endPt, filter, path, ref pathCount, maxPath ); smoothPath.m_nsmoothPath = 0; if (pathCount > 0) { // Iterate over the path to find smooth path on the detail mesh surface. dtPolyRef[] polys = new dtPolyRef[SmoothPath.MAX_POLYS]; for (int i=0;i<pathCount;++i){ polys[i] = path[i]; } int npolys = pathCount; float[] iterPos = new float[3]; float[] targetPos = new float[3]; bool posOverPoly_dummy = false; navQuery.closestPointOnPoly(startRef, startPt, iterPos, ref posOverPoly_dummy); navQuery.closestPointOnPoly(polys[npolys-1], endPt, targetPos, ref posOverPoly_dummy); const float STEP_SIZE = 0.5f; const float SLOP = 0.01f; smoothPath.m_nsmoothPath = 0; Detour.dtVcopy(smoothPath.m_smoothPath,smoothPath.m_nsmoothPath*3, iterPos, 0); smoothPath.m_nsmoothPath++; // Move towards target a small advancement at a time until target reached or // when ran out of memory to store the path. while (npolys != 0 && smoothPath.m_nsmoothPath < SmoothPath.MAX_SMOOTH) { // Find location to steer towards. float[] steerPos = new float[3]; byte steerPosFlag = 0; dtPolyRef steerPosRef = 0; if (!getSteerTarget(navQuery, iterPos, targetPos, SLOP, polys, npolys, steerPos, ref steerPosFlag, ref steerPosRef)) break; bool endOfPath = (steerPosFlag & (byte)Detour.dtStraightPathFlags.DT_STRAIGHTPATH_END) != 0 ? true : false; bool offMeshConnection = (steerPosFlag & (byte)Detour.dtStraightPathFlags.DT_STRAIGHTPATH_OFFMESH_CONNECTION) != 0 ? true : false; // Find movement delta. float[] delta = new float[3];//, len; float len = .0f; Detour.dtVsub(delta, steerPos, iterPos); len = (float)Mathf.Sqrt(Detour.dtVdot(delta,delta)); // If the steer target is end of path or off-mesh link, do not move past the location. if ((endOfPath || offMeshConnection) && len < STEP_SIZE) len = 1; else len = STEP_SIZE / len; float[] moveTgt = new float[3]; Detour.dtVmad(moveTgt, iterPos, delta, len); // Move float[] result = new float[3]; dtPolyRef[] visited = new dtPolyRef[16]; int nvisited = 0; navQuery.moveAlongSurface(polys[0], iterPos, moveTgt, filter, result, visited, ref nvisited, 16); npolys = fixupCorridor(polys, npolys, SmoothPath.MAX_POLYS, visited, nvisited); npolys = fixupShortcuts(polys, npolys, navQuery); float h = 0; dtStatus getHeightStatus = navQuery.getPolyHeight(polys[0], result, ref h); result[1] = h; if ((getHeightStatus & Detour.DT_FAILURE) != 0) { Debug.LogError("Failed to getPolyHeight " + polys[0] + " pos " + result[0] + " " + result[1] + " " + result[2] + " h " + h); } Detour.dtVcopy(iterPos, result); // Handle end of path and off-mesh links when close enough. if (endOfPath && inRange(iterPos, 0, steerPos, 0, SLOP, 1.0f)) { // Reached end of path. Detour.dtVcopy(iterPos, targetPos); if (smoothPath.m_nsmoothPath < SmoothPath.MAX_SMOOTH) { Detour.dtVcopy(smoothPath.m_smoothPath,smoothPath.m_nsmoothPath*3, iterPos, 0); smoothPath.m_nsmoothPath++; } break; } else if (offMeshConnection && inRange(iterPos, 0, steerPos, 0, SLOP, 1.0f)) { // Reached off-mesh connection. float[] startPos = new float[3];//, endPos[3]; float[] endPos = new float[3]; // Advance the path up to and over the off-mesh connection. dtPolyRef prevRef = 0, polyRef = polys[0]; int npos = 0; while (npos < npolys && polyRef != steerPosRef) { prevRef = polyRef; polyRef = polys[npos]; npos++; } for (int i = npos; i < npolys; ++i) polys[i-npos] = polys[i]; npolys -= npos; // Handle the connection. dtStatus status = navQuery.getAttachedNavMesh().getOffMeshConnectionPolyEndPoints(prevRef, polyRef, startPos, endPos); if (Detour.dtStatusSucceed(status)) { if (smoothPath.m_nsmoothPath < SmoothPath.MAX_SMOOTH) { Detour.dtVcopy(smoothPath.m_smoothPath,smoothPath.m_nsmoothPath*3, startPos, 0); smoothPath.m_nsmoothPath++; // Hack to make the dotted path not visible during off-mesh connection. if ((smoothPath.m_nsmoothPath & 1) != 0) { Detour.dtVcopy(smoothPath.m_smoothPath, smoothPath.m_nsmoothPath * 3, startPos, 0); smoothPath.m_nsmoothPath++; } } // Move position at the other side of the off-mesh link. Detour.dtVcopy(iterPos, endPos); float eh = 0.0f; navQuery.getPolyHeight(polys[0], iterPos, ref eh); iterPos[1] = eh; } } // Store results. if (smoothPath.m_nsmoothPath < SmoothPath.MAX_SMOOTH) { Detour.dtVcopy(smoothPath.m_smoothPath, smoothPath.m_nsmoothPath * 3, iterPos, 0); smoothPath.m_nsmoothPath++; } } } return smoothPath; }