diff --git a/src/DotRecast.Detour/PathUtils.cs b/src/DotRecast.Detour/PathUtils.cs new file mode 100644 index 0000000..73112d3 --- /dev/null +++ b/src/DotRecast.Detour/PathUtils.cs @@ -0,0 +1,200 @@ +/* +Copyright (c) 2009-2010 Mikko Mononen memon@inside.org +recast4j copyright (c) 2015-2019 Piotr Piastucki piotr@jtilia.org +DotRecast Copyright (c) 2023 Choi Ikpil ikpil@naver.com + +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. +*/ + +using System; +using System.Collections.Generic; +using DotRecast.Core; + +namespace DotRecast.Detour +{ + public static class PathUtils + { + private const int MAX_STEER_POINTS = 3; + + + public static SteerTarget getSteerTarget(NavMeshQuery navQuery, Vector3f startPos, Vector3f endPos, + float minTargetDist, List path) + { + // Find steer target. + Result> result = navQuery.findStraightPath(startPos, endPos, path, MAX_STEER_POINTS, 0); + if (result.failed()) + { + return null; + } + + List straightPath = result.result; + float[] steerPoints = new float[straightPath.Count * 3]; + for (int i = 0; i < straightPath.Count; i++) + { + steerPoints[i * 3] = straightPath[i].getPos()[0]; + steerPoints[i * 3 + 1] = straightPath[i].getPos()[1]; + steerPoints[i * 3 + 2] = straightPath[i].getPos()[2]; + } + + // Find vertex far enough to steer to. + int ns = 0; + while (ns < straightPath.Count) + { + // Stop at Off-Mesh link or when point is further than slop away. + if (((straightPath[ns].getFlags() & NavMeshQuery.DT_STRAIGHTPATH_OFFMESH_CONNECTION) != 0) + || !inRange(straightPath[ns].getPos(), startPos, minTargetDist, 1000.0f)) + break; + ns++; + } + + // Failed to find good point to steer to. + if (ns >= straightPath.Count) + return null; + + Vector3f steerPos = Vector3f.Of( + straightPath[ns].getPos()[0], + startPos[1], + straightPath[ns].getPos()[2] + ); + int steerPosFlag = straightPath[ns].getFlags(); + long steerPosRef = straightPath[ns].getRef(); + + SteerTarget target = new SteerTarget(steerPos, steerPosFlag, steerPosRef, steerPoints); + return target; + } + + public static bool inRange(Vector3f v1, Vector3f v2, float r, float h) + { + float dx = v2[0] - v1[0]; + float dy = v2[1] - v1[1]; + float dz = v2[2] - v1[2]; + return (dx * dx + dz * dz) < r * r && Math.Abs(dy) < h; + } + + public static List fixupCorridor(List path, List visited) + { + int furthestPath = -1; + int furthestVisited = -1; + + // Find furthest common polygon. + for (int i = path.Count - 1; i >= 0; --i) + { + bool found = false; + for (int j = visited.Count - 1; j >= 0; --j) + { + if (path[i] == visited[j]) + { + furthestPath = i; + furthestVisited = j; + found = true; + } + } + + if (found) + break; + } + + // If no intersection found just return current path. + if (furthestPath == -1 || furthestVisited == -1) + return path; + + // Concatenate paths. + + // Adjust beginning of the buffer to include the visited. + int req = visited.Count - furthestVisited; + int orig = Math.Min(furthestPath + 1, path.Count); + int size = Math.Max(0, path.Count - orig); + List fixupPath = new List(); + // Store visited + for (int i = 0; i < req; ++i) + { + fixupPath.Add(visited[(visited.Count - 1) - i]); + } + + for (int i = 0; i < size; i++) + { + fixupPath.Add(path[orig + i]); + } + + return fixupPath; + } + + // 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. + // +---+---+ + public static List fixupShortcuts(List path, NavMeshQuery navQuery) + { + if (path.Count < 3) + { + return path; + } + + // Get connected polygons + List neis = new List(); + + Result> tileAndPoly = navQuery.getAttachedNavMesh().getTileAndPolyByRef(path[0]); + if (tileAndPoly.failed()) + { + return path; + } + + MeshTile tile = tileAndPoly.result.Item1; + Poly poly = tileAndPoly.result.Item2; + + for (int k = tile.polyLinks[poly.index]; k != NavMesh.DT_NULL_LINK; k = tile.links[k].next) + { + Link link = tile.links[k]; + if (link.refs != 0) + { + neis.Add(link.refs); + } + } + + // If any of the neighbour polygons is within the next few polygons + // in the path, short cut to that polygon directly. + int maxLookAhead = 6; + int cut = 0; + for (int i = Math.Min(maxLookAhead, path.Count) - 1; i > 1 && cut == 0; i--) + { + for (int j = 0; j < neis.Count; j++) + { + if (path[i] == neis[j]) + { + cut = i; + break; + } + } + } + + if (cut > 1) + { + List shortcut = new List(); + shortcut.Add(path[0]); + shortcut.AddRange(path.GetRange(cut, path.Count - cut)); + return shortcut; + } + + return path; + } + } +} \ No newline at end of file diff --git a/src/DotRecast.Detour/SteerTarget.cs b/src/DotRecast.Detour/SteerTarget.cs new file mode 100644 index 0000000..8d37fda --- /dev/null +++ b/src/DotRecast.Detour/SteerTarget.cs @@ -0,0 +1,20 @@ +using DotRecast.Core; + +namespace DotRecast.Detour +{ + public class SteerTarget + { + public readonly Vector3f steerPos; + public readonly int steerPosFlag; + public readonly long steerPosRef; + public readonly float[] steerPoints; + + public SteerTarget(Vector3f steerPos, int steerPosFlag, long steerPosRef, float[] steerPoints) + { + this.steerPos = steerPos; + this.steerPosFlag = steerPosFlag; + this.steerPosRef = steerPosRef; + this.steerPoints = steerPoints; + } + } +} \ No newline at end of file diff --git a/src/DotRecast.Recast/DotRecast.Recast.csproj b/src/DotRecast.Recast/DotRecast.Recast.csproj index f08ddff..b49b2e6 100644 --- a/src/DotRecast.Recast/DotRecast.Recast.csproj +++ b/src/DotRecast.Recast/DotRecast.Recast.csproj @@ -5,7 +5,7 @@ - + diff --git a/src/DotRecast.Recast/PolyUtils.cs b/src/DotRecast.Recast/PolyUtils.cs new file mode 100644 index 0000000..7a23065 --- /dev/null +++ b/src/DotRecast.Recast/PolyUtils.cs @@ -0,0 +1,129 @@ +/* +Copyright (c) 2009-2010 Mikko Mononen memon@inside.org +recast4j copyright (c) 2021 Piotr Piastucki piotr@jtilia.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. +*/ + +using System; +using DotRecast.Core; + +namespace DotRecast.Recast +{ + public static class PolyUtils + { + public static bool pointInPoly(float[] verts, Vector3f p) + { + int i, j; + bool c = false; + for (i = 0, j = verts.Length / 3 - 1; i < verts.Length / 3; j = i++) + { + Vector3f vi = Vector3f.Of(verts[i * 3], verts[i * 3 + 1], verts[i * 3 + 2]); + Vector3f vj = Vector3f.Of(verts[j * 3], verts[j * 3 + 1], verts[j * 3 + 2]); + 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; + } + + public static int offsetPoly(float[] verts, int nverts, float offset, float[] outVerts, int maxOutVerts) + { + float MITER_LIMIT = 1.20f; + + int n = 0; + + for (int i = 0; i < nverts; i++) + { + int a = (i + nverts - 1) % nverts; + int b = i; + int c = (i + 1) % nverts; + int va = a * 3; + int vb = b * 3; + int vc = c * 3; + float dx0 = verts[vb] - verts[va]; + float dy0 = verts[vb + 2] - verts[va + 2]; + float d0 = dx0 * dx0 + dy0 * dy0; + if (d0 > 1e-6f) + { + d0 = (float)(1.0f / Math.Sqrt(d0)); + dx0 *= d0; + dy0 *= d0; + } + + float dx1 = verts[vc] - verts[vb]; + float dy1 = verts[vc + 2] - verts[vb + 2]; + float d1 = dx1 * dx1 + dy1 * dy1; + if (d1 > 1e-6f) + { + d1 = (float)(1.0f / Math.Sqrt(d1)); + dx1 *= d1; + dy1 *= d1; + } + + float dlx0 = -dy0; + float dly0 = dx0; + float dlx1 = -dy1; + float dly1 = dx1; + float cross = dx1 * dy0 - dx0 * dy1; + float dmx = (dlx0 + dlx1) * 0.5f; + float dmy = (dly0 + dly1) * 0.5f; + float dmr2 = dmx * dmx + dmy * dmy; + bool bevel = dmr2 * MITER_LIMIT * MITER_LIMIT < 1.0f; + if (dmr2 > 1e-6f) + { + float scale = 1.0f / dmr2; + dmx *= scale; + dmy *= scale; + } + + if (bevel && cross < 0.0f) + { + if (n + 2 >= maxOutVerts) + { + return 0; + } + + float d = (1.0f - (dx0 * dx1 + dy0 * dy1)) * 0.5f; + outVerts[n * 3 + 0] = verts[vb] + (-dlx0 + dx0 * d) * offset; + outVerts[n * 3 + 1] = verts[vb + 1]; + outVerts[n * 3 + 2] = verts[vb + 2] + (-dly0 + dy0 * d) * offset; + n++; + outVerts[n * 3 + 0] = verts[vb] + (-dlx1 - dx1 * d) * offset; + outVerts[n * 3 + 1] = verts[vb + 1]; + outVerts[n * 3 + 2] = verts[vb + 2] + (-dly1 - dy1 * d) * offset; + n++; + } + else + { + if (n + 1 >= maxOutVerts) + { + return 0; + } + + outVerts[n * 3 + 0] = verts[vb] - dmx * offset; + outVerts[n * 3 + 1] = verts[vb + 1]; + outVerts[n * 3 + 2] = verts[vb + 2] - dmy * offset; + n++; + } + } + + return n; + } + } +} \ No newline at end of file