move utils

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<long> path)
+        {
+            // Find steer target.
+            Result<List<StraightPathItem>> result = navQuery.findStraightPath(startPos, endPos, path, MAX_STEER_POINTS, 0);
+            if (result.failed())
+            {
+                return null;
+            }
+
+            List<StraightPathItem> 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<long> fixupCorridor(List<long> path, List<long> 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<long> fixupPath = new List<long>();
+            // 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<long> fixupShortcuts(List<long> path, NavMeshQuery navQuery)
+        {
+            if (path.Count < 3)
+            {
+                return path;
+            }
+
+            // Get connected polygons
+            List<long> neis = new List<long>();
+
+            Result<Tuple<MeshTile, Poly>> 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<long> shortcut = new List<long>();
+                shortcut.Add(path[0]);
+                shortcut.AddRange(path.GetRange(cut, path.Count - cut));
+                return shortcut;
+            }
+
+            return path;
+        }
+    }
+}

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;
+        }
+    }
+}

src/DotRecast.Recast/DotRecast.Recast.csproj

@@ -5,7 +5,7 @@
     </PropertyGroup>
 
     <ItemGroup>
-        <ProjectReference Include="..\DotRecast.Core\DotRecast.Core.csproj"/>
+        <ProjectReference Include="..\DotRecast.Core\DotRecast.Core.csproj" />
     </ItemGroup>
 
 </Project>

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;
+        }
+    }
+}