├── NF.AI.PathFinding
    ├── .gitattributes
    ├── src
    │   ├── NF.Collections.Generic
    │   │   ├── PriorityQueue.cs
    │   │   └── PriorityQueueBinaryHeap.cs
    │   ├── NF.AI.PathFinding.Common
    │   │   ├── ExInt2.cs
    │   │   ├── EDirFlags.cs
    │   │   ├── AStarNode.cs
    │   │   ├── Bresenham.cs
    │   │   ├── BresenHamPathSmoother.cs
    │   │   └── DirFlags.cs
    │   ├── NF.AI.PathFinding.csproj
    │   ├── NF.AI.PathFinding.JPSPlus
    │   │   ├── JPSPlusNode.cs
    │   │   ├── JPSPlusMapBakerBlock.cs
    │   │   ├── JPSPlusBakedMap.cs
    │   │   ├── JPSPlusRunner.cs
    │   │   ├── JPSPlus.cs
    │   │   └── JPSPlusMapBaker.cs
    │   ├── NF.Mathematics
    │   │   └── Int2.cs
    │   ├── NF.AI.PathFinding.AStar
    │   │   └── AStar.cs
    │   ├── NF.AI.PathFinding.JPSOrthogonal
    │   │   └── JPSOrthogonal.cs
    │   └── NF.AI.PathFinding.JPS
    │   │   └── JPS.cs
    ├── playground
    │   ├── NF.AI.PathFinding.Playground.csproj
    │   ├── DrawableNode.cs
    │   ├── Line.cs
    │   ├── Board.cs
    │   └── Program.cs
    ├── .gitignore
    ├── test
    │   ├── NF.AI.PathFinding.Tests.csproj
    │   ├── PriorityQueueBinaryHeapTest.cs
    │   └── JPSTest.cs
    ├── NF.AI.PathFinding.sln
    └── .editorconfig
├── README.md
└── LICENSE.md


/NF.AI.PathFinding/.gitattributes:
--------------------------------------------------------------------------------
1 | *.cs eol=lf
2 | *.razor eol=lf
3 | 


--------------------------------------------------------------------------------
/NF.AI.PathFinding/src/NF.Collections.Generic/PriorityQueue.cs:
--------------------------------------------------------------------------------
1 | namespace NF.Collections.Generic
2 | {
3 |     // https://github.com/BlueRaja/High-Speed-Priority-Queue-for-C-Sharp 
4 |     public class PriorityQueue<T> : Priority_Queue.SimplePriorityQueue<T, long>
5 |     {
6 | 
7 |     }
8 | }
9 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # NF.AI.PathFinding
 2 | 
 3 | ## What's this?
 4 | 
 5 | Dotnet Implementation of path finding algorithms.
 6 | 
 7 | - [A*](https://en.wikipedia.org/wiki/A*_search_algorithm)
 8 | - [JPS(Jump Point Search)](https://en.wikipedia.org/wiki/Jump_point_search)
 9 | - JPS(with Orthogonal)
10 | - JPS+ : [demo](https://github.com/netpyoung/unity.playground.pathfinding)


--------------------------------------------------------------------------------
/NF.AI.PathFinding/playground/NF.AI.PathFinding.Playground.csproj:
--------------------------------------------------------------------------------
 1 | <Project Sdk="Microsoft.NET.Sdk">
 2 | 
 3 |   <PropertyGroup>
 4 |     <OutputType>Exe</OutputType>
 5 |     <TargetFramework>netcoreapp3.1</TargetFramework>
 6 |   </PropertyGroup>
 7 | 
 8 |   <ItemGroup>
 9 |     <ProjectReference Include="..\src\NF.AI.PathFinding.csproj" />
10 |   </ItemGroup>
11 | 
12 |   <ItemGroup>
13 |     <PackageReference Include="SFML.Net" Version="2.5.0" />
14 |   </ItemGroup>
15 | 
16 | </Project>
17 | 


--------------------------------------------------------------------------------
/NF.AI.PathFinding/src/NF.AI.PathFinding.Common/ExInt2.cs:
--------------------------------------------------------------------------------
 1 | using NF.Mathematics;
 2 | 
 3 | namespace NF.AI.PathFinding.Common
 4 | {
 5 |     public static class ExInt2
 6 |     {
 7 |         public static Int2 Foward(this Int2 x, EDirFlags dir)
 8 |         {
 9 |             return x + DirFlags.ToPos(dir);
10 |         }
11 | 
12 |         public static Int2 Backward(this Int2 x, EDirFlags dir)
13 |         {
14 |             return x - DirFlags.ToPos(dir);
15 |         }
16 |     }
17 | }
18 | 


--------------------------------------------------------------------------------
/NF.AI.PathFinding/src/NF.AI.PathFinding.Common/EDirFlags.cs:
--------------------------------------------------------------------------------
 1 | namespace NF.AI.PathFinding.Common
 2 | {
 3 |     public enum EDirFlags : int
 4 |     {
 5 |         NONE = 0,
 6 |         NORTH = 1,
 7 |         SOUTH = 2,
 8 |         EAST = 4,
 9 |         WEST = 8,
10 |         NORTHEAST = 16,
11 |         NORTHWEST = 32,
12 |         SOUTHEAST = 64,
13 |         SOUTHWEST = 128,
14 |         ALL = NORTH | SOUTH | EAST | WEST | NORTHEAST | NORTHWEST | SOUTHEAST | SOUTHWEST,
15 |     };
16 | }
17 | 


--------------------------------------------------------------------------------
/NF.AI.PathFinding/.gitignore:
--------------------------------------------------------------------------------
 1 | *.swp
 2 | *.*~
 3 | project.lock.json
 4 | .DS_Store
 5 | *.pyc
 6 | nupkg/
 7 | 
 8 | # Visual Studio Code
 9 | .vscode
10 | 
11 | # Rider
12 | .idea
13 | 
14 | # User-specific files
15 | *.suo
16 | *.user
17 | *.userosscache
18 | *.sln.docstates
19 | 
20 | # Build results
21 | [Dd]ebug/
22 | [Dd]ebugPublic/
23 | [Rr]elease/
24 | [Rr]eleases/
25 | x64/
26 | x86/
27 | build/
28 | bld/
29 | [Bb]in/
30 | [Oo]bj/
31 | [Oo]ut/
32 | msbuild.log
33 | msbuild.err
34 | msbuild.wrn
35 | 
36 | # Visual Studio 2015
37 | .vs/


--------------------------------------------------------------------------------
/NF.AI.PathFinding/src/NF.AI.PathFinding.csproj:
--------------------------------------------------------------------------------
 1 | <Project Sdk="Microsoft.NET.Sdk">
 2 | 
 3 |   <PropertyGroup>
 4 |     <TargetFramework>netstandard2.0</TargetFramework>
 5 |   </PropertyGroup>
 6 | 
 7 |   <ItemGroup>
 8 |     <AssemblyAttribute Include="System.Runtime.CompilerServices.InternalsVisibleTo">
 9 |       <_Parameter1>$(AssemblyName).Tests</_Parameter1>
10 |     </AssemblyAttribute>
11 |   </ItemGroup>
12 |   
13 |   <ItemGroup>
14 |     <PackageReference Include="OptimizedPriorityQueue" Version="4.2.0" />
15 |   </ItemGroup>
16 |   
17 | </Project>
18 | 


--------------------------------------------------------------------------------
/NF.AI.PathFinding/test/NF.AI.PathFinding.Tests.csproj:
--------------------------------------------------------------------------------
 1 | <Project Sdk="Microsoft.NET.Sdk">
 2 | 
 3 |   <PropertyGroup>
 4 |     <TargetFramework>netcoreapp3.1</TargetFramework>
 5 | 
 6 |     <IsPackable>false</IsPackable>
 7 |   </PropertyGroup>
 8 | 
 9 |   <ItemGroup>
10 |     <PackageReference Include="Microsoft.NET.Test.Sdk" Version="16.2.0" />
11 |     <PackageReference Include="xunit" Version="2.4.0" />
12 |     <PackageReference Include="xunit.runner.visualstudio" Version="2.4.0" />
13 |     <PackageReference Include="coverlet.collector" Version="1.0.1" />
14 |   </ItemGroup>
15 |   
16 |   <ItemGroup>
17 |     <ProjectReference Include="..\src\NF.AI.PathFinding.csproj" />
18 |   </ItemGroup>
19 | </Project>
20 | 


--------------------------------------------------------------------------------
/NF.AI.PathFinding/src/NF.AI.PathFinding.JPSPlus/JPSPlusNode.cs:
--------------------------------------------------------------------------------
 1 | using NF.AI.PathFinding.Common;
 2 | using NF.Mathematics;
 3 | 
 4 | namespace NF.AI.PathFinding.JPSPlus
 5 | {
 6 |     public class JPSPlusNode : AStarNode
 7 |     {
 8 |         private int[] mJumpDistances;
 9 | 
10 |         public JPSPlusNode(in Int2 p, int[] jumpDistances) : base(p)
11 |         {
12 |             mJumpDistances = jumpDistances;
13 |         }
14 | 
15 |         public int GetDistance(EDirFlags dir)
16 |         {
17 |             return mJumpDistances[DirFlags.ToArrayIndex(dir)];
18 |         }
19 | 
20 |         internal void Refresh(int[] jumpDistances)
21 |         {
22 |             mJumpDistances = jumpDistances;
23 |             Refresh();
24 |         }
25 |     }
26 | }
27 | 


--------------------------------------------------------------------------------
/NF.AI.PathFinding/test/PriorityQueueBinaryHeapTest.cs:
--------------------------------------------------------------------------------
 1 | using System.Collections.Generic;
 2 | 
 3 | using Xunit;
 4 | 
 5 | namespace NF.Collections.Generic.Test
 6 | {
 7 |     public class PriorityQueueBinaryHeapTest
 8 |     {
 9 |         [Fact]
10 |         public void Test1()
11 |         {
12 |             PriorityQueueBinaryHeap<int> q = new PriorityQueueBinaryHeap<int>();
13 |             q.Push(20);
14 |             q.Push(10);
15 |             q.Push(30);
16 |             q.Push(90);
17 |             q.Push(40);
18 | 
19 |             List<int> lst = new List<int>();
20 |             while (q.Count > 0)
21 |             {
22 |                 lst.Add(q.Pop());
23 |             }
24 |             Assert.Equal(lst, new List<int> { 90, 40, 30, 20, 10 });
25 |         }
26 |     }
27 | }
28 | 


--------------------------------------------------------------------------------
/NF.AI.PathFinding/src/NF.AI.PathFinding.Common/AStarNode.cs:
--------------------------------------------------------------------------------
 1 | using NF.Mathematics;
 2 | 
 3 | namespace NF.AI.PathFinding.Common
 4 | {
 5 |     public class AStarNode
 6 |     {
 7 |         public Int2 Position { get; private set; }
 8 |         public int G { get; internal set; } = 0;
 9 |         public int H { get; internal set; } = 0;
10 |         public long F => G + H;
11 |         public AStarNode Parent { get; internal set; }
12 | 
13 |         public AStarNode(in Int2 p)
14 |         {
15 |             G = 0;
16 |             H = 0;
17 |             Position = p;
18 |         }
19 | 
20 |         public AStarNode(int x, int y) :
21 |             this(new Int2 { X = x, Y = y })
22 |         {
23 |         }
24 | 
25 |         public void Refresh()
26 |         {
27 |             G = 0;
28 |             H = 0;
29 |             Parent = null;
30 |         }
31 |     }
32 | }
33 | 


--------------------------------------------------------------------------------
/NF.AI.PathFinding/src/NF.AI.PathFinding.JPSPlus/JPSPlusMapBakerBlock.cs:
--------------------------------------------------------------------------------
 1 | using NF.AI.PathFinding.Common;
 2 | using NF.Mathematics;
 3 | 
 4 | namespace NF.AI.PathFinding.JPSPlus
 5 | {
 6 |     public class JPSPlusMapBakerBlock
 7 |     {
 8 |         public readonly int[] JumpDistances = new int[8];
 9 |         public readonly Int2 Pos;
10 |         public EDirFlags JumpDirFlags = EDirFlags.NONE;
11 | 
12 |         public JPSPlusMapBakerBlock(in Int2 pos)
13 |         {
14 |             Pos = pos;
15 |         }
16 | 
17 |         public bool IsJumpable(EDirFlags dir)
18 |         {
19 |             return (JumpDirFlags & dir) == dir;
20 |         }
21 | 
22 |         public void SetDistance(EDirFlags dir, int distance)
23 |         {
24 |             JumpDistances[DirFlags.ToArrayIndex(dir)] = distance;
25 |         }
26 | 
27 |         public int GetDistance(EDirFlags dir)
28 |         {
29 |             return JumpDistances[DirFlags.ToArrayIndex(dir)];
30 |         }
31 |     }
32 | }
33 | 


--------------------------------------------------------------------------------
/NF.AI.PathFinding/src/NF.AI.PathFinding.JPSPlus/JPSPlusBakedMap.cs:
--------------------------------------------------------------------------------
 1 | using NF.Mathematics;
 2 | 
 3 | namespace NF.AI.PathFinding.JPSPlus
 4 | {
 5 |     public class JPSPlusBakedMap
 6 |     {
 7 |         public class JPSPlusBakedMapBlock
 8 |         {
 9 |             public readonly int[] JumpDistances; // for 8 direction distance;
10 |             public readonly Int2 Pos;
11 | 
12 |             public JPSPlusBakedMapBlock(in Int2 pos, int[] jumpDistances)
13 |             {
14 |                 JumpDistances = jumpDistances;
15 |                 Pos = pos;
16 |             }
17 |         }
18 | 
19 |         public readonly int[,] BlockLUT;
20 |         public readonly JPSPlusBakedMapBlock[] Blocks;
21 |         public int Width => BlockLUT.GetLength(1);
22 |         public int Height => BlockLUT.GetLength(0);
23 | 
24 |         public JPSPlusBakedMap(int[,] blockLUT, JPSPlusBakedMapBlock[] blocks)
25 |         {
26 |             BlockLUT = blockLUT;
27 |             Blocks = blocks;
28 |         }
29 |     }
30 | }
31 | 


--------------------------------------------------------------------------------
/NF.AI.PathFinding/playground/DrawableNode.cs:
--------------------------------------------------------------------------------
 1 | using SFML.Graphics;
 2 | using SFML.System;
 3 | 
 4 | namespace NF.AI.PathFinding.Playground
 5 | {
 6 |     internal class DrawableNode : Transformable, Drawable
 7 |     {
 8 |         private readonly int mNodeSize;
 9 |         private readonly RectangleShape mBack = new RectangleShape();
10 | 
11 |         public DrawableNode(int nodeSize)
12 |         {
13 |             mNodeSize = nodeSize;
14 |             Init(nodeSize);
15 |         }
16 | 
17 |         private void Init(int nodeSize)
18 |         {
19 |             mBack.Size = new Vector2f(nodeSize - 5, nodeSize - 5);
20 |             mBack.Position = new Vector2f(2.5f, 2.5f);
21 |         }
22 | 
23 |         public void Draw(RenderTarget target, RenderStates states)
24 |         {
25 |             states.Transform *= Transform;
26 |             target.Draw(mBack, states);
27 |         }
28 | 
29 |         public void SetFillColor(Color color)
30 |         {
31 |             mBack.FillColor = color;
32 |         }
33 |     }
34 | }
35 | 


--------------------------------------------------------------------------------
/LICENSE.md:
--------------------------------------------------------------------------------
 1 | MIT License
 2 | 
 3 | Copyright (c) 2020 Eunpyoung Kim
 4 | 
 5 | Permission is hereby granted, free of charge, to any person obtaining a copy
 6 | of this software and associated documentation files (the "Software"), to deal
 7 | in the Software without restriction, including without limitation the rights
 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 9 | copies of the Software, and to permit persons to whom the Software is
10 | furnished to do so, subject to the following conditions:
11 | 
12 | The above copyright notice and this permission notice shall be included in all
13 | copies or substantial portions of the Software.
14 | 
15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 | SOFTWARE.
22 | 


--------------------------------------------------------------------------------
/NF.AI.PathFinding/src/NF.AI.PathFinding.Common/Bresenham.cs:
--------------------------------------------------------------------------------
 1 | using NF.Mathematics;
 2 | 
 3 | using System;
 4 | 
 5 | namespace NF.AI.PathFinding.Common
 6 | {
 7 |     public class BresenHam
 8 |     {
 9 |         // ref: https://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm
10 |         private int mDx;
11 |         private int mSx;
12 |         private int mDy;
13 |         private int mSy;
14 |         private int mErr;
15 |         private Int2 mCurr;
16 |         private Int2 mDest;
17 | 
18 |         public void Init(in Int2 src, in Int2 dst)
19 |         {
20 |             mDx = Math.Abs(dst.X - src.X);
21 |             mDy = -Math.Abs(dst.Y - src.Y);
22 |             mSx = (src.X < dst.X) ? 1 : -1;
23 |             mSy = (src.Y < dst.Y) ? 1 : -1;
24 |             mErr = mDx + mDy;
25 |             mCurr = src;
26 |             mDest = dst;
27 |         }
28 | 
29 |         public bool TryGetNext(ref Int2 nextP)
30 |         {
31 |             if (mCurr == mDest)
32 |             {
33 |                 return false;
34 |             }
35 | 
36 |             int e2 = 2 * mErr;
37 | 
38 |             if (e2 >= mDy)
39 |             {
40 |                 mErr += mDy;
41 |                 mCurr.X += mSx;
42 |             }
43 |             if (e2 <= mDx)
44 |             {
45 |                 mErr += mDx;
46 |                 mCurr.Y += mSy;
47 |             }
48 | 
49 |             nextP = mCurr;
50 |             return true;
51 |         }
52 |     }
53 | }
54 | 


--------------------------------------------------------------------------------
/NF.AI.PathFinding/src/NF.AI.PathFinding.Common/BresenHamPathSmoother.cs:
--------------------------------------------------------------------------------
 1 | using NF.Mathematics;
 2 | 
 3 | using System.Collections.Generic;
 4 | 
 5 | namespace NF.AI.PathFinding.Common
 6 | {
 7 |     public class BresenHamPathSmoother
 8 |     {
 9 |         private readonly BresenHam mBresenHam = new BresenHam();
10 | 
11 |         public delegate bool DelIsWalkable(in Int2 p);
12 | 
13 |         public List<Int2> SmoothPath(List<Int2> path, DelIsWalkable fnIsWalkable)
14 |         {
15 |             List<Int2> ret = new List<Int2>();
16 |             if (path.Count < 2)
17 |             {
18 |                 return ret;
19 |             }
20 | 
21 |             Int2 bp = new Int2();
22 |             Int2 prevTargetP = new Int2();
23 | 
24 |             Int2 beginP = path[0];
25 |             Int2 targetP = path[1];
26 |             int index = 1;
27 |             mBresenHam.Init(beginP, targetP);
28 |             ret.Add(beginP);
29 | 
30 |             while (true)
31 |             {
32 |                 if (!mBresenHam.TryGetNext(ref bp))
33 |                 {
34 |                     prevTargetP = targetP;
35 |                     index++;
36 |                     if (index == path.Count)
37 |                     {
38 |                         ret.Add(targetP);
39 |                         break;
40 |                     }
41 |                     targetP = path[index];
42 |                     mBresenHam.Init(beginP, targetP);
43 |                 }
44 |                 else if (!fnIsWalkable(bp))
45 |                 {
46 |                     ret.Add(prevTargetP);
47 |                     beginP = prevTargetP;
48 |                     mBresenHam.Init(beginP, targetP);
49 |                 }
50 |             }
51 |             return ret;
52 |         }
53 |     }
54 | }
55 | 


--------------------------------------------------------------------------------
/NF.AI.PathFinding/src/NF.Collections.Generic/PriorityQueueBinaryHeap.cs:
--------------------------------------------------------------------------------
 1 | using System;
 2 | using System.Collections.Generic;
 3 | 
 4 | namespace NF.Collections.Generic
 5 | {
 6 |     public class PriorityQueueBinaryHeap<T> where T : IComparable<T>
 7 |     {
 8 |         public void Push(T t)
 9 |         {
10 |             mHeap.Add(t);
11 | 
12 |             int now = mHeap.Count - 1;
13 |             while (now > 0)
14 |             {
15 |                 int next = (now - 1) / 2;
16 |                 if (mHeap[now].CompareTo(mHeap[next]) < 0)
17 |                 {
18 |                     break;
19 |                 }
20 |                 (mHeap[next], mHeap[now]) = (mHeap[now], mHeap[next]);
21 |                 now = next;
22 |             }
23 |         }
24 | 
25 |         public T Pop()
26 |         {
27 |             T ret = mHeap[0];
28 |             int lastIndex = mHeap.Count - 1;
29 |             mHeap[0] = mHeap[lastIndex];
30 |             mHeap.RemoveAt(lastIndex);
31 |             lastIndex--;
32 | 
33 |             int now = 0;
34 |             while (true)
35 |             {
36 |                 int left = (2 * now) + 1;
37 |                 int right = (2 * now) + 2;
38 | 
39 |                 int next = now;
40 |                 if (left <= lastIndex && mHeap[next].CompareTo(mHeap[left]) < 0)
41 |                 {
42 |                     next = left;
43 |                 }
44 | 
45 |                 if (right <= lastIndex && mHeap[next].CompareTo(mHeap[right]) < 0)
46 |                 {
47 |                     next = right;
48 |                 }
49 | 
50 |                 if (next == now)
51 |                 {
52 |                     break;
53 |                 }
54 | 
55 |                 (mHeap[next], mHeap[now]) = (mHeap[now], mHeap[next]);
56 |                 now = next;
57 |             }
58 | 
59 |             return ret;
60 |         }
61 | 
62 |         public int Count => mHeap.Count;
63 | 
64 |         private readonly List<T> mHeap = new List<T>();
65 | 
66 |     }
67 | }
68 | 


--------------------------------------------------------------------------------
/NF.AI.PathFinding/playground/Line.cs:
--------------------------------------------------------------------------------
 1 | using SFML.Graphics;
 2 | using SFML.System;
 3 | 
 4 | using System;
 5 | 
 6 | namespace NF.AI.PathFinding.Playground
 7 | {
 8 |     internal class Line : Drawable
 9 |     {
10 |         public void Refresh(Vector2f p1, Vector2f p2)
11 |         {
12 |             P1 = p1;
13 |             P2 = p2;
14 |             SetTickness(Tickness);
15 |             SetColor(Color);
16 |         }
17 | 
18 |         public void SetTickness(float ticknessAmount)
19 |         {
20 |             Tickness = ticknessAmount;
21 |             Vector2f direction = P2 - P1;
22 |             Vector2f unitDirection = direction / (float)Math.Sqrt((direction.X * direction.X) + (direction.Y * direction.Y));
23 |             Vector2f unitPerpendicular = new Vector2f(-unitDirection.Y, unitDirection.X);
24 |             Vector2f offset = ticknessAmount / 2f * unitPerpendicular;
25 | 
26 |             mVertices[0].Position = P1 + offset;
27 |             mVertices[1].Position = P2 + offset;
28 |             mVertices[2].Position = P2 - offset;
29 |             mVertices[3].Position = P1 - offset;
30 |         }
31 | 
32 |         public void Draw(RenderTarget target, RenderStates states)
33 |         {
34 |             target.Draw(mVertices, 0, 4, PrimitiveType.Quads);
35 |         }
36 | 
37 |         public float Tickness { get; private set; }
38 |         public Vector2f P1 { get; private set; }
39 |         public Vector2f P2 { get; private set; }
40 |         public Color Color { get; private set; }
41 | 
42 |         private readonly Vertex[] mVertices = new Vertex[4];
43 | 
44 |         public void SetColor(Color color)
45 |         {
46 |             Color = color;
47 |             for (int i = 0; i < 4; ++i)
48 |             {
49 |                 mVertices[i].Color = color;
50 |             }
51 |         }
52 |         public Line(Vector2f p1, Vector2f p2)
53 |         {
54 |             P1 = p1;
55 |             P2 = p2;
56 |             Tickness = 5;
57 |             Color = Color.Black;
58 |             Refresh(p1, p2);
59 |         }
60 |     }
61 | }
62 | 


--------------------------------------------------------------------------------
/NF.AI.PathFinding/test/JPSTest.cs:
--------------------------------------------------------------------------------
 1 | using NF.AI.PathFinding.Common;
 2 | using NF.Mathematics;
 3 | 
 4 | using Xunit;
 5 | 
 6 | namespace NF.AI.PathFinding.JPS.Test
 7 | {
 8 |     public class JPSTest
 9 |     {
10 |         [Fact]
11 |         public void TestJumpRecursive()
12 |         {
13 |             // . f .
14 |             // . X j
15 |             // . . .
16 |             // p . g
17 | 
18 |             // p   : (0, 3)
19 |             // g   : (2, 3)
20 |             // dir : NORTHEAST
21 |             // j   : (2, 1)
22 | 
23 |             JPS jps = new JPS(new bool[5, 4] {
24 |                 { false, false, false, false },
25 |                 { false, true, false, false },
26 |                 { false, false, false, false },
27 |                 { false, false, false, false },
28 |                 { false, false, false, false },
29 |             });
30 | 
31 |             Int2 p = new Int2(0, 3);
32 |             Int2 g = new Int2(2, 3);
33 |             EDirFlags dir = EDirFlags.NORTHEAST;
34 | 
35 |             Int2 result = new Int2(0, 0);
36 |             Assert.True(jps.TryJump(p, dir, g, ref result));
37 |             Assert.Equal(new Int2(2, 1), result);
38 |         }
39 | 
40 |         [Fact]
41 |         public void TESTSuccesorsDir()
42 |         {
43 |             JPS jps = new JPS(new bool[5, 5] {
44 |                 { false, false, false, false,false },
45 |                 { false, false, false, false,false },
46 |                 { false, false, false, true,false },
47 |                 { false, false, false, false,false },
48 |                 { false, false, false, false,false },
49 |             });
50 | 
51 |             Int2 o = new Int2(2, 2);
52 | 
53 |             // . . F
54 |             // . o X
55 |             // . . P
56 |             EDirFlags forcedNeighbourDir = jps.ForcedNeighbourDir(o, EDirFlags.NORTHWEST);
57 |             Assert.Equal(EDirFlags.NORTHEAST, forcedNeighbourDir);
58 | 
59 |             // N N .
60 |             // N o X
61 |             // . . P
62 |             EDirFlags naturalNeighbours = JPS.NaturalNeighbours(EDirFlags.NORTHEAST);
63 |             Assert.Equal(EDirFlags.NORTHEAST | EDirFlags.NORTH | EDirFlags.EAST, naturalNeighbours);
64 | 
65 |             // S S S
66 |             // S o X
67 |             // . . P
68 |             EDirFlags succesorsDir = jps.SuccesorsDir(o, EDirFlags.NORTHWEST);
69 |             Assert.Equal(EDirFlags.NORTHWEST | EDirFlags.NORTH | EDirFlags.NORTHEAST | EDirFlags.WEST, succesorsDir);
70 |         }
71 |     }
72 | }
73 | 


--------------------------------------------------------------------------------
/NF.AI.PathFinding/src/NF.AI.PathFinding.Common/DirFlags.cs:
--------------------------------------------------------------------------------
 1 | using NF.Mathematics;
 2 | 
 3 | using System.Collections.Generic;
 4 | 
 5 | namespace NF.AI.PathFinding.Common
 6 | {
 7 |     public static class DirFlags
 8 |     {
 9 |         public static int ToArrayIndex(EDirFlags dir)
10 |         {
11 |             switch (dir)
12 |             {
13 |                 case EDirFlags.NORTHWEST:
14 |                     return 0;
15 |                 case EDirFlags.NORTH:
16 |                     return 1;
17 |                 case EDirFlags.NORTHEAST:
18 |                     return 2;
19 |                 case EDirFlags.WEST:
20 |                     return 3;
21 |                 case EDirFlags.EAST:
22 |                     return 4;
23 |                 case EDirFlags.SOUTHWEST:
24 |                     return 5;
25 |                 case EDirFlags.SOUTH:
26 |                     return 6;
27 |                 case EDirFlags.SOUTHEAST:
28 |                     return 7;
29 |                 default:
30 |                     return -1;
31 |             }
32 |         }
33 | 
34 |         public static bool IsStraight(EDirFlags dir)
35 |         {
36 |             return (dir & (EDirFlags.NORTH | EDirFlags.SOUTH | EDirFlags.EAST | EDirFlags.WEST)) != EDirFlags.NONE;
37 |         }
38 | 
39 |         public static bool IsDiagonal(EDirFlags dir)
40 |         {
41 |             return (dir & (EDirFlags.NORTHEAST | EDirFlags.NORTHWEST | EDirFlags.SOUTHEAST | EDirFlags.SOUTHWEST)) != EDirFlags.NONE;
42 |         }
43 | 
44 |         public static EDirFlags DiagonalToEastWest(EDirFlags dir)
45 |         {
46 |             if ((dir & (EDirFlags.NORTHEAST | EDirFlags.SOUTHEAST)) != EDirFlags.NONE)
47 |             {
48 |                 return EDirFlags.EAST;
49 |             }
50 | 
51 |             if ((dir & (EDirFlags.NORTHWEST | EDirFlags.SOUTHWEST)) != EDirFlags.NONE)
52 |             {
53 |                 return EDirFlags.WEST;
54 |             }
55 | 
56 |             return EDirFlags.NONE;
57 |         }
58 | 
59 |         public static EDirFlags DiagonalToNorthSouth(EDirFlags dir)
60 |         {
61 |             if ((dir & (EDirFlags.NORTHEAST | EDirFlags.NORTHWEST)) != EDirFlags.NONE)
62 |             {
63 |                 return EDirFlags.NORTH;
64 |             }
65 | 
66 |             if ((dir & (EDirFlags.SOUTHEAST | EDirFlags.SOUTHWEST)) != EDirFlags.NONE)
67 |             {
68 |                 return EDirFlags.SOUTH;
69 |             }
70 | 
71 |             return EDirFlags.NONE;
72 |         }
73 | 
74 |         private static readonly Dictionary<EDirFlags, Int2> DirToPos = new Dictionary<EDirFlags, Int2>()
75 |         {
76 |             { EDirFlags.NORTH,new Int2(0, -1) },
77 |             { EDirFlags.SOUTH,new Int2(0, 1) },
78 |             { EDirFlags.EAST,new Int2(1, 0) },
79 |             { EDirFlags.WEST,new Int2(-1, 0) },
80 |             { EDirFlags.NORTHEAST,new Int2(1, -1) },
81 |             { EDirFlags.NORTHWEST,new Int2(-1, -1) },
82 |             { EDirFlags.SOUTHEAST,new Int2(1, 1) },
83 |             { EDirFlags.SOUTHWEST,new Int2(-1, 1) },
84 |         };
85 | 
86 |         public static Int2 ToPos(EDirFlags dir)
87 |         {
88 |             return DirToPos[dir];
89 |         }
90 |     }
91 | }
92 | 


--------------------------------------------------------------------------------
/NF.AI.PathFinding/src/NF.AI.PathFinding.JPSPlus/JPSPlusRunner.cs:
--------------------------------------------------------------------------------
  1 | using NF.AI.PathFinding.Common;
  2 | using NF.Mathematics;
  3 | 
  4 | using System.Collections.Generic;
  5 | 
  6 | namespace NF.AI.PathFinding.JPSPlus
  7 | {
  8 |     public class JPSPlusRunner
  9 |     {
 10 |         private readonly JPSPlus mJpsPlus = new JPSPlus();
 11 |         private readonly JPSPlusMapBaker mBaker = new JPSPlusMapBaker();
 12 |         private bool[,] mWalls = null;
 13 | 
 14 |         public int Width { get; private set; }
 15 |         public int Height { get; private set; }
 16 | 
 17 |         public Int2 StartP => mStartP.Value;
 18 |         public Int2 GoalP => mGoalP.Value;
 19 | 
 20 |         private bool mIsWallChanged = true;
 21 |         private Int2? mStartP = null;
 22 |         private Int2? mGoalP = null;
 23 | 
 24 |         public JPSPlusRunner(int width, int height)
 25 |         {
 26 |             Init(new bool[height, width]);
 27 |         }
 28 | 
 29 |         public JPSPlusRunner(bool[,] walls)
 30 |         {
 31 |             Init(walls);
 32 |         }
 33 | 
 34 |         public void Init(bool[,] walls)
 35 |         {
 36 |             mWalls = walls;
 37 |             Width = walls.GetLength(1);
 38 |             Height = walls.GetLength(0);
 39 |         }
 40 | 
 41 |         public void ToggleWall(in Int2 p)
 42 |         {
 43 |             if (!IsInBoundary(p))
 44 |             {
 45 |                 return;
 46 |             }
 47 |             mWalls[p.Y, p.X] = !mWalls[p.Y, p.X];
 48 |             mIsWallChanged = true;
 49 |         }
 50 | 
 51 |         public void SetStart(in Int2 p)
 52 |         {
 53 |             if (!IsInBoundary(p))
 54 |             {
 55 |                 return;
 56 |             }
 57 |             mStartP = p;
 58 |         }
 59 | 
 60 |         public void SetGoal(in Int2 p)
 61 |         {
 62 |             if (!IsInBoundary(p))
 63 |             {
 64 |                 return;
 65 |             }
 66 |             mGoalP = p;
 67 |         }
 68 | 
 69 |         public bool IsWalkable(in Int2 p)
 70 |         {
 71 |             if (!IsInBoundary(p))
 72 |             {
 73 |                 return false;
 74 |             }
 75 |             return !mWalls[p.Y, p.X];
 76 |         }
 77 | 
 78 |         public bool StepAll(int stepCount = int.MaxValue)
 79 |         {
 80 |             if (mIsWallChanged)
 81 |             {
 82 |                 mBaker.Init(mWalls);
 83 |                 mJpsPlus.Init(mBaker.Bake());
 84 |                 mIsWallChanged = false;
 85 |             }
 86 |             _ = mJpsPlus.SetStart(mStartP.Value);
 87 |             _ = mJpsPlus.SetGoal(mGoalP.Value);
 88 |             return mJpsPlus.StepAll(stepCount);
 89 |         }
 90 | 
 91 |         public void SetWall(in Int2 p, bool isWall)
 92 |         {
 93 |             if (!IsInBoundary(p))
 94 |             {
 95 |                 return;
 96 |             }
 97 |             mWalls[p.Y, p.X] = isWall;
 98 |             mIsWallChanged = true;
 99 |         }
100 | 
101 |         public bool[,] GetWalls()
102 |         {
103 |             return mWalls;
104 |         }
105 | 
106 |         public IReadOnlyList<AStarNode> GetPaths()
107 |         {
108 |             return mJpsPlus.GetPaths();
109 |         }
110 | 
111 |         private bool IsInBoundary(in Int2 p)
112 |         {
113 |             return 0 <= p.X && p.X < Width && 0 <= p.Y && p.Y < Height;
114 |         }
115 | 
116 |     }
117 | }
118 | 


--------------------------------------------------------------------------------
/NF.AI.PathFinding/src/NF.Mathematics/Int2.cs:
--------------------------------------------------------------------------------
  1 | using System;
  2 | using System.Runtime.CompilerServices;
  3 | 
  4 | namespace NF.Mathematics
  5 | {
  6 |     [Serializable]
  7 |     public struct Int2 : IEquatable<Int2>, IFormattable
  8 |     {
  9 |         public int X;
 10 |         public int Y;
 11 | 
 12 |         public Int2(int x, int y)
 13 |         {
 14 |             X = x;
 15 |             Y = y;
 16 |         }
 17 | 
 18 |         [System.ComponentModel.EditorBrowsable(System.ComponentModel.EditorBrowsableState.Never)]
 19 |         public Int2 XY
 20 |         {
 21 |             [MethodImpl(MethodImplOptions.AggressiveInlining)]
 22 |             get
 23 |             {
 24 |                 return new Int2(X, Y);
 25 |             }
 26 | 
 27 |             [MethodImpl(MethodImplOptions.AggressiveInlining)]
 28 |             set
 29 |             {
 30 |                 X = value.X;
 31 |                 Y = value.Y;
 32 |             }
 33 |         }
 34 | 
 35 |         [System.ComponentModel.EditorBrowsable(System.ComponentModel.EditorBrowsableState.Never)]
 36 |         public Int2 YX
 37 |         {
 38 |             [MethodImpl(MethodImplOptions.AggressiveInlining)]
 39 |             get
 40 |             {
 41 |                 return new Int2(Y, X);
 42 |             }
 43 | 
 44 |             [MethodImpl(MethodImplOptions.AggressiveInlining)]
 45 |             set
 46 |             {
 47 |                 Y = value.X;
 48 |                 X = value.Y;
 49 |             }
 50 |         }
 51 | 
 52 | 
 53 |         [MethodImpl(MethodImplOptions.AggressiveInlining)]
 54 |         public static Int2 operator +(Int2 a, Int2 b)
 55 |         {
 56 |             return new Int2 { X = a.X + b.X, Y = a.Y + b.Y };
 57 |         }
 58 | 
 59 |         [MethodImpl(MethodImplOptions.AggressiveInlining)]
 60 |         public static Int2 operator -(Int2 a, Int2 b)
 61 |         {
 62 |             return new Int2 { X = a.X - b.X, Y = a.Y - b.Y };
 63 |         }
 64 | 
 65 |         [MethodImpl(MethodImplOptions.AggressiveInlining)]
 66 |         public static Int2 operator *(Int2 a, int val)
 67 |         {
 68 |             return new Int2 { X = a.X * val, Y = a.Y * val };
 69 |         }
 70 | 
 71 |         [MethodImpl(MethodImplOptions.AggressiveInlining)]
 72 |         public static Int2 operator /(Int2 a, int val)
 73 |         {
 74 |             return new Int2 { X = a.X / val, Y = a.Y / val };
 75 |         }
 76 | 
 77 |         [MethodImpl(MethodImplOptions.AggressiveInlining)]
 78 |         public static bool operator ==(Int2 a, Int2 b)
 79 |         {
 80 |             return a.X == b.X && a.Y == b.Y;
 81 |         }
 82 | 
 83 |         [MethodImpl(MethodImplOptions.AggressiveInlining)]
 84 |         public static bool operator !=(Int2 a, Int2 b)
 85 |         {
 86 |             return !(a == b);
 87 |         }
 88 | 
 89 |         [MethodImpl(MethodImplOptions.AggressiveInlining)]
 90 |         public override bool Equals(object obj)
 91 |         {
 92 |             return Equals((Int2)obj);
 93 |         }
 94 | 
 95 |         [MethodImpl(MethodImplOptions.AggressiveInlining)]
 96 |         public override int GetHashCode()
 97 |         {
 98 |             return base.GetHashCode();
 99 |         }
100 | 
101 |         [MethodImpl(MethodImplOptions.AggressiveInlining)]
102 |         public override string ToString()
103 |         {
104 |             return $"Int2({X}, {Y})";
105 |         }
106 | 
107 |         [MethodImpl(MethodImplOptions.AggressiveInlining)]
108 |         public bool Equals(Int2 other)
109 |         {
110 |             return this == other;
111 |         }
112 | 
113 |         [MethodImpl(MethodImplOptions.AggressiveInlining)]
114 |         public string ToString(string format, IFormatProvider formatProvider)
115 |         {
116 |             return $"Int2({X.ToString(format, formatProvider)}, {Y.ToString(format, formatProvider)})";
117 |         }
118 |     }
119 | }
120 | 


--------------------------------------------------------------------------------
/NF.AI.PathFinding/NF.AI.PathFinding.sln:
--------------------------------------------------------------------------------
 1 | 
 2 | Microsoft Visual Studio Solution File, Format Version 12.00
 3 | # Visual Studio Version 16
 4 | VisualStudioVersion = 16.0.29806.167
 5 | MinimumVisualStudioVersion = 15.0.26124.0
 6 | Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "NF.AI.PathFinding", "src\NF.AI.PathFinding.csproj", "{81343C5B-3DCC-4CEA-8A28-190F54700F27}"
 7 | EndProject
 8 | Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "NF.AI.PathFinding.Playground", "playground\NF.AI.PathFinding.Playground.csproj", "{623A3FF5-40EA-4A36-BA1E-EEC8559FE6F2}"
 9 | EndProject
10 | Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "NF.AI.PathFinding.Tests", "test\NF.AI.PathFinding.Tests.csproj", "{377EAE32-6C6B-488A-9D22-0014CC88ACB6}"
11 | EndProject
12 | Global
13 | 	GlobalSection(SolutionConfigurationPlatforms) = preSolution
14 | 		Debug|Any CPU = Debug|Any CPU
15 | 		Debug|x64 = Debug|x64
16 | 		Debug|x86 = Debug|x86
17 | 		Release|Any CPU = Release|Any CPU
18 | 		Release|x64 = Release|x64
19 | 		Release|x86 = Release|x86
20 | 	EndGlobalSection
21 | 	GlobalSection(ProjectConfigurationPlatforms) = postSolution
22 | 		{81343C5B-3DCC-4CEA-8A28-190F54700F27}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
23 | 		{81343C5B-3DCC-4CEA-8A28-190F54700F27}.Debug|Any CPU.Build.0 = Debug|Any CPU
24 | 		{81343C5B-3DCC-4CEA-8A28-190F54700F27}.Debug|x64.ActiveCfg = Debug|Any CPU
25 | 		{81343C5B-3DCC-4CEA-8A28-190F54700F27}.Debug|x64.Build.0 = Debug|Any CPU
26 | 		{81343C5B-3DCC-4CEA-8A28-190F54700F27}.Debug|x86.ActiveCfg = Debug|Any CPU
27 | 		{81343C5B-3DCC-4CEA-8A28-190F54700F27}.Debug|x86.Build.0 = Debug|Any CPU
28 | 		{81343C5B-3DCC-4CEA-8A28-190F54700F27}.Release|Any CPU.ActiveCfg = Release|Any CPU
29 | 		{81343C5B-3DCC-4CEA-8A28-190F54700F27}.Release|Any CPU.Build.0 = Release|Any CPU
30 | 		{81343C5B-3DCC-4CEA-8A28-190F54700F27}.Release|x64.ActiveCfg = Release|Any CPU
31 | 		{81343C5B-3DCC-4CEA-8A28-190F54700F27}.Release|x64.Build.0 = Release|Any CPU
32 | 		{81343C5B-3DCC-4CEA-8A28-190F54700F27}.Release|x86.ActiveCfg = Release|Any CPU
33 | 		{81343C5B-3DCC-4CEA-8A28-190F54700F27}.Release|x86.Build.0 = Release|Any CPU
34 | 		{623A3FF5-40EA-4A36-BA1E-EEC8559FE6F2}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
35 | 		{623A3FF5-40EA-4A36-BA1E-EEC8559FE6F2}.Debug|Any CPU.Build.0 = Debug|Any CPU
36 | 		{623A3FF5-40EA-4A36-BA1E-EEC8559FE6F2}.Debug|x64.ActiveCfg = Debug|Any CPU
37 | 		{623A3FF5-40EA-4A36-BA1E-EEC8559FE6F2}.Debug|x64.Build.0 = Debug|Any CPU
38 | 		{623A3FF5-40EA-4A36-BA1E-EEC8559FE6F2}.Debug|x86.ActiveCfg = Debug|Any CPU
39 | 		{623A3FF5-40EA-4A36-BA1E-EEC8559FE6F2}.Debug|x86.Build.0 = Debug|Any CPU
40 | 		{623A3FF5-40EA-4A36-BA1E-EEC8559FE6F2}.Release|Any CPU.ActiveCfg = Release|Any CPU
41 | 		{623A3FF5-40EA-4A36-BA1E-EEC8559FE6F2}.Release|Any CPU.Build.0 = Release|Any CPU
42 | 		{623A3FF5-40EA-4A36-BA1E-EEC8559FE6F2}.Release|x64.ActiveCfg = Release|Any CPU
43 | 		{623A3FF5-40EA-4A36-BA1E-EEC8559FE6F2}.Release|x64.Build.0 = Release|Any CPU
44 | 		{623A3FF5-40EA-4A36-BA1E-EEC8559FE6F2}.Release|x86.ActiveCfg = Release|Any CPU
45 | 		{623A3FF5-40EA-4A36-BA1E-EEC8559FE6F2}.Release|x86.Build.0 = Release|Any CPU
46 | 		{377EAE32-6C6B-488A-9D22-0014CC88ACB6}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
47 | 		{377EAE32-6C6B-488A-9D22-0014CC88ACB6}.Debug|Any CPU.Build.0 = Debug|Any CPU
48 | 		{377EAE32-6C6B-488A-9D22-0014CC88ACB6}.Debug|x64.ActiveCfg = Debug|Any CPU
49 | 		{377EAE32-6C6B-488A-9D22-0014CC88ACB6}.Debug|x64.Build.0 = Debug|Any CPU
50 | 		{377EAE32-6C6B-488A-9D22-0014CC88ACB6}.Debug|x86.ActiveCfg = Debug|Any CPU
51 | 		{377EAE32-6C6B-488A-9D22-0014CC88ACB6}.Debug|x86.Build.0 = Debug|Any CPU
52 | 		{377EAE32-6C6B-488A-9D22-0014CC88ACB6}.Release|Any CPU.ActiveCfg = Release|Any CPU
53 | 		{377EAE32-6C6B-488A-9D22-0014CC88ACB6}.Release|Any CPU.Build.0 = Release|Any CPU
54 | 		{377EAE32-6C6B-488A-9D22-0014CC88ACB6}.Release|x64.ActiveCfg = Release|Any CPU
55 | 		{377EAE32-6C6B-488A-9D22-0014CC88ACB6}.Release|x64.Build.0 = Release|Any CPU
56 | 		{377EAE32-6C6B-488A-9D22-0014CC88ACB6}.Release|x86.ActiveCfg = Release|Any CPU
57 | 		{377EAE32-6C6B-488A-9D22-0014CC88ACB6}.Release|x86.Build.0 = Release|Any CPU
58 | 	EndGlobalSection
59 | 	GlobalSection(SolutionProperties) = preSolution
60 | 		HideSolutionNode = FALSE
61 | 	EndGlobalSection
62 | 	GlobalSection(ExtensibilityGlobals) = postSolution
63 | 		SolutionGuid = {7FC1AD6A-479E-48E2-BF0A-EA2B2CA55888}
64 | 	EndGlobalSection
65 | EndGlobal
66 | 


--------------------------------------------------------------------------------
/NF.AI.PathFinding/playground/Board.cs:
--------------------------------------------------------------------------------
  1 | using NF.AI.PathFinding.Common;
  2 | using NF.Mathematics;
  3 | 
  4 | using SFML.Graphics;
  5 | using SFML.System;
  6 | 
  7 | using System.Collections.Generic;
  8 | using System.Linq;
  9 | 
 10 | namespace NF.AI.PathFinding.Playground
 11 | {
 12 |     internal class Board : Drawable
 13 |     {
 14 |         private readonly DrawableNode[,] mDrawbleNodes;
 15 |         private readonly BresenHamPathSmoother mPathSmoother = new BresenHamPathSmoother();
 16 | 
 17 |         //AStar.AStar mPathFinder;
 18 |         //JPS.JPS mPathFinder;
 19 |         //JPSOrthogonal.JPSOrthogonal mPathFinder;
 20 |         private readonly JPSPlus.JPSPlusRunner mPathFinder;
 21 |         private readonly List<Line> mGridLines = new List<Line>();
 22 |         private readonly List<Line> mPathLines = new List<Line>();
 23 |         private Vector2f mHalfNodeP;
 24 | 
 25 |         public int Width { get; }
 26 |         public int Height { get; }
 27 |         public int NodeSize { get; }
 28 |         public Int2 StartP => mPathFinder.StartP;
 29 |         public Int2 GoalP => mPathFinder.GoalP;
 30 | 
 31 |         public Board(int screenWidth, int screenHeight, int nodeSize)
 32 |         {
 33 |             //mPathFinder = new AStar.AStar(screenWidth / nodeSize, screenHeight / nodeSize);
 34 |             //mPathFinder = new JPS.JPS(screenWidth / nodeSize, screenHeight / nodeSize);
 35 |             //mPathFinder = new JPSOrthogonal.JPSOrthogonal(screenWidth / nodeSize, screenHeight / nodeSize);
 36 |             mPathFinder = new JPSPlus.JPSPlusRunner(screenWidth / nodeSize, screenHeight / nodeSize);
 37 | 
 38 |             Width = screenWidth / nodeSize;
 39 |             Height = screenHeight / nodeSize;
 40 |             NodeSize = nodeSize;
 41 |             mHalfNodeP = new Vector2f(NodeSize / 2, NodeSize / 2);
 42 | 
 43 |             for (int x = 0; x <= screenWidth; x += nodeSize)
 44 |             {
 45 |                 mGridLines.Add(new Line(new Vector2f(x, 0), new Vector2f(x, screenHeight)));
 46 |             }
 47 | 
 48 |             for (int y = 0; y <= screenHeight; y += nodeSize)
 49 |             {
 50 |                 mGridLines.Add(new Line(new Vector2f(0, y), new Vector2f(screenWidth, y)));
 51 |             }
 52 | 
 53 |             mDrawbleNodes = new DrawableNode[Height, Width];
 54 |             for (int y = 0; y < Height; ++y)
 55 |             {
 56 |                 for (int x = 0; x < Width; ++x)
 57 |                 {
 58 |                     mDrawbleNodes[y, x] = new DrawableNode(nodeSize)
 59 |                     {
 60 |                         Position = new Vector2f(x * nodeSize, y * nodeSize)
 61 |                     };
 62 |                 }
 63 |             }
 64 |         }
 65 | 
 66 |         public void Draw(RenderTarget target, RenderStates states)
 67 |         {
 68 |             foreach (Line line in mGridLines)
 69 |             {
 70 |                 target.Draw(line);
 71 |             }
 72 |             for (int y = 0; y < Height; ++y)
 73 |             {
 74 |                 for (int x = 0; x < Width; ++x)
 75 |                 {
 76 |                     target.Draw(mDrawbleNodes[y, x]);
 77 |                 }
 78 |             }
 79 |             foreach (Line line in mPathLines)
 80 |             {
 81 |                 target.Draw(line);
 82 |             }
 83 |         }
 84 | 
 85 |         public void ToggleWall(in Int2 mp)
 86 |         {
 87 |             mPathFinder.ToggleWall(mp);
 88 |         }
 89 | 
 90 |         public void SetStart(in Int2 p)
 91 |         {
 92 |             mPathFinder.SetStart(p);
 93 |         }
 94 | 
 95 |         public void SetGoal(in Int2 p)
 96 |         {
 97 |             mPathFinder.SetGoal(p);
 98 |         }
 99 | 
100 |         internal void StepAll()
101 |         {
102 |             _ = mPathFinder.StepAll();
103 |         }
104 | 
105 |         public bool IsWall(in Int2 p)
106 |         {
107 |             return !mPathFinder.IsWalkable(p);
108 |         }
109 | 
110 |         public void CreateWall(in Int2 p)
111 |         {
112 |             mPathFinder.SetWall(p, true);
113 |         }
114 | 
115 |         public void RemoveWall(in Int2 p)
116 |         {
117 |             mPathFinder.SetWall(p, false);
118 |         }
119 | 
120 |         internal void Update()
121 |         {
122 |             bool[,] walls = mPathFinder.GetWalls();
123 |             for (int y = 0; y < Height; ++y)
124 |             {
125 |                 for (int x = 0; x < Width; ++x)
126 |                 {
127 |                     if (walls[y, x])
128 |                     {
129 |                         mDrawbleNodes[y, x].SetFillColor(Color.Blue);
130 |                     }
131 |                     else
132 |                     {
133 |                         mDrawbleNodes[y, x].SetFillColor(Color.White);
134 |                     }
135 |                 }
136 |             }
137 | 
138 |             Int2 sp = mPathFinder.StartP;
139 |             mDrawbleNodes[sp.Y, sp.X].SetFillColor(Color.Yellow);
140 |             Int2 gp = mPathFinder.GoalP;
141 |             mDrawbleNodes[gp.Y, gp.X].SetFillColor(Color.Green);
142 |         }
143 | 
144 |         public void FindPath()
145 |         {
146 |             bool[,] walls = mPathFinder.GetWalls();
147 |             for (int y = 0; y < Height; ++y)
148 |             {
149 |                 for (int x = 0; x < Width; ++x)
150 |                 {
151 |                     if (walls[y, x])
152 |                     {
153 |                         mDrawbleNodes[y, x].SetFillColor(Color.Blue);
154 |                     }
155 |                     else
156 |                     {
157 |                         mDrawbleNodes[y, x].SetFillColor(Color.White);
158 |                     }
159 |                 }
160 |             }
161 | 
162 |             { // show path
163 |                 mPathLines.Clear();
164 |                 DrawableNode prevNode = null;
165 |                 List<Int2> pp1 = mPathFinder.GetPaths().Select(x =>
166 |                 {
167 |                     return x.Position;
168 |                 }).ToList();
169 |                 //var pp2 = mPathSmoother.SmoothPath(pp1, mPathFinder.IsWalkable);
170 |                 foreach (Int2 p in pp1)
171 |                 {
172 |                     DrawableNode pathNode = mDrawbleNodes[p.Y, p.X];
173 |                     pathNode.SetFillColor(Color.Cyan);
174 |                     if (prevNode != null)
175 |                     {
176 |                         Line line = new Line(pathNode.Position + mHalfNodeP, prevNode.Position + mHalfNodeP);
177 |                         line.SetColor(Color.Red);
178 |                         mPathLines.Add(line);
179 |                     }
180 |                     prevNode = pathNode;
181 |                 }
182 |             } // show path
183 | 
184 |             Int2 sp = mPathFinder.StartP;
185 |             mDrawbleNodes[sp.Y, sp.X].SetFillColor(Color.Yellow);
186 |             Int2 gp = mPathFinder.GoalP;
187 |             mDrawbleNodes[gp.Y, gp.X].SetFillColor(Color.Green);
188 |         }
189 |     }
190 | }
191 | 


--------------------------------------------------------------------------------
/NF.AI.PathFinding/src/NF.AI.PathFinding.AStar/AStar.cs:
--------------------------------------------------------------------------------
  1 | using NF.AI.PathFinding.Common;
  2 | using NF.Collections.Generic;
  3 | using NF.Mathematics;
  4 | 
  5 | using System;
  6 | using System.Collections.Generic;
  7 | 
  8 | namespace NF.AI.PathFinding.AStar
  9 | {
 10 |     public class AStar
 11 |     {
 12 |         // ============================
 13 |         // Private
 14 |         // ============================
 15 |         private AStarNode mStart = null;
 16 |         private AStarNode mGoal = null;
 17 |         private readonly AStarNode[,] mNodes;
 18 |         private readonly PriorityQueue<AStarNode> mOpenList = new PriorityQueue<AStarNode>();
 19 |         private readonly HashSet<AStarNode> mCloseList = new HashSet<AStarNode>();
 20 |         private readonly bool[,] mWalls;
 21 | 
 22 |         // ============================
 23 |         // Properties
 24 |         // ============================
 25 |         public int Width { get; private set; }
 26 |         public int Height { get; private set; }
 27 |         public Int2 StartP => mStart.Position;
 28 |         public Int2 GoalP => mGoal.Position;
 29 | 
 30 |         public AStar(int width, int height)
 31 |         {
 32 |             Width = width;
 33 |             Height = height;
 34 |             mNodes = new AStarNode[Height, Width];
 35 |             mWalls = new bool[Height, Width];
 36 |         }
 37 | 
 38 |         public AStar(bool[,] walls)
 39 |         {
 40 |             Height = walls.GetLength(0);
 41 |             Width = walls.GetLength(1);
 42 |             mNodes = new AStarNode[Height, Width];
 43 |             mWalls = walls;
 44 |         }
 45 | 
 46 |         // ============================
 47 |         // Public Methods
 48 |         // ============================
 49 | 
 50 |         public bool StepAll()
 51 |         {
 52 |             mOpenList.Clear();
 53 |             mCloseList.Clear();
 54 |             mOpenList.Enqueue(mStart, mStart.F);
 55 |             mGoal.Parent = null;
 56 |             return Step(int.MaxValue);
 57 |         }
 58 | 
 59 |         public bool Step(int stepCount)
 60 |         {
 61 |             for (int step = stepCount; step > 0; --step)
 62 |             {
 63 |                 if (mOpenList.Count == 0)
 64 |                 {
 65 |                     return false;
 66 |                 }
 67 | 
 68 |                 AStarNode curr = mOpenList.Dequeue();
 69 |                 if (curr == mGoal)
 70 |                 {
 71 |                     return true;
 72 |                 }
 73 | 
 74 |                 _ = mCloseList.Add(curr);
 75 | 
 76 |                 for (int i = 0b10000000; i > 0; i >>= 1)
 77 |                 {
 78 | 
 79 |                     EDirFlags dir = (EDirFlags)i;
 80 |                     Int2 dp = DirFlags.ToPos(dir);
 81 |                     AStarNode adjacent = GetNodeOrNull(curr.Position + dp);
 82 |                     if (adjacent == null)
 83 |                     {
 84 |                         continue;
 85 |                     }
 86 |                     if (IsWall(adjacent.Position))
 87 |                     {
 88 |                         continue;
 89 |                     }
 90 | 
 91 |                     if (DirFlags.IsDiagonal(dir))
 92 |                     { // for prevent corner cutting
 93 |                         if (IsWall(curr.Position + new Int2(dp.X, 0)) || IsWall(curr.Position + new Int2(0, dp.Y)))
 94 |                         {
 95 |                             continue;
 96 |                         }
 97 |                     }
 98 | 
 99 |                     if (mCloseList.Contains(adjacent))
100 |                     {
101 |                         continue;
102 |                     }
103 | 
104 |                     int nextG = G(curr, adjacent);
105 |                     if (!mOpenList.Contains(adjacent))
106 |                     {
107 |                         adjacent.Parent = curr;
108 |                         adjacent.G = nextG;
109 |                         adjacent.H = H(adjacent, mGoal);
110 |                         mOpenList.Enqueue(adjacent, adjacent.F);
111 |                     }
112 |                     else if (nextG < adjacent.G)
113 |                     {
114 |                         adjacent.Parent = curr;
115 |                         adjacent.G = nextG;
116 |                         adjacent.H = H(adjacent, mGoal);
117 |                         mOpenList.UpdatePriority(adjacent, adjacent.F);
118 |                     }
119 |                 }
120 |             }
121 |             return false;
122 |         }
123 | 
124 |         public bool SetStart(in Int2 pos)
125 |         {
126 |             if (!IsInBoundary(pos))
127 |             {
128 |                 return false;
129 |             }
130 | 
131 |             mStart = GetNodeOrNull(pos);
132 |             return true;
133 |         }
134 | 
135 |         public bool SetGoal(in Int2 pos)
136 |         {
137 |             if (!IsInBoundary(pos))
138 |             {
139 |                 return false;
140 |             }
141 | 
142 |             mGoal = GetNodeOrNull(pos);
143 |             return true;
144 |         }
145 | 
146 |         public void SetWall(in Int2 p, bool isWall)
147 |         {
148 |             if (!IsInBoundary(p))
149 |             {
150 |                 return;
151 |             }
152 |             mWalls[p.Y, p.X] = isWall;
153 |         }
154 | 
155 |         public bool ToggleWall(in Int2 pos)
156 |         {
157 |             if (!IsInBoundary(pos))
158 |             {
159 |                 return false;
160 |             }
161 |             mWalls[pos.Y, pos.X] = !mWalls[pos.Y, +pos.X];
162 |             return true;
163 |         }
164 | 
165 |         public List<AStarNode> GetPaths()
166 |         {
167 |             List<AStarNode> ret = new List<AStarNode>();
168 |             AStarNode node = mGoal;
169 |             while (node != null)
170 |             {
171 |                 ret.Add(node);
172 |                 node = node.Parent;
173 |             }
174 |             ret.Reverse();
175 |             return ret;
176 |         }
177 | 
178 |         public AStarNode GetStart()
179 |         {
180 |             return mStart;
181 |         }
182 | 
183 |         public AStarNode GetGoal()
184 |         {
185 |             return mGoal;
186 | 
187 |         }
188 | 
189 |         public PriorityQueue<AStarNode> GetOpenList()
190 |         {
191 |             return mOpenList;
192 |         }
193 | 
194 |         public HashSet<AStarNode> GetCloseList()
195 |         {
196 |             return mCloseList;
197 |         }
198 | 
199 |         public bool[,] GetWalls()
200 |         {
201 |             return mWalls;
202 |         }
203 | 
204 |         public bool IsWalkable(in Int2 p)
205 |         {
206 |             if (!IsInBoundary(p))
207 |             {
208 |                 return false;
209 |             }
210 |             return !IsWall(p);
211 |         }
212 | 
213 |         // ============================
214 |         // Private Methods
215 |         // ============================
216 |         private bool IsInBoundary(in Int2 pos)
217 |         {
218 |             return IsInBoundary(pos.X, pos.Y);
219 |         }
220 | 
221 |         private bool IsInBoundary(int x, int y)
222 |         {
223 |             return 0 <= x && x < Width && 0 <= y && y < Height;
224 |         }
225 | 
226 |         private AStarNode GetNodeOrNull(in Int2 pos)
227 |         {
228 |             int x = pos.X;
229 |             int y = pos.Y;
230 |             if (!IsInBoundary(x, y))
231 |             {
232 |                 return null;
233 |             }
234 |             AStarNode node = mNodes[y, x];
235 |             if (node != null)
236 |             {
237 |                 return node;
238 |             }
239 |             node = new AStarNode(x, y);
240 |             mNodes[y, x] = node;
241 |             return node;
242 |         }
243 | 
244 |         private bool IsWall(in Int2 pos)
245 |         {
246 |             return mWalls[pos.Y, pos.X];
247 |         }
248 | 
249 |         private static int G(AStarNode from, AStarNode adjacent)
250 |         {
251 |             // cost so far to reach n 
252 |             Int2 p = from.Position - adjacent.Position;
253 |             if (p.X == 0 || p.Y == 0)
254 |             {
255 |                 return from.G + 10;
256 |             }
257 |             else
258 |             {
259 |                 return from.G + 14;
260 |             }
261 |         }
262 | 
263 |         internal static int H(AStarNode n, AStarNode goal)
264 |         {
265 |             // calculate estimated cost
266 |             return Math.Abs(goal.Position.X - n.Position.X) + (Math.Abs(goal.Position.Y - n.Position.Y) * 10);
267 |         }
268 |     }
269 | }
270 | 


--------------------------------------------------------------------------------
/NF.AI.PathFinding/.editorconfig:
--------------------------------------------------------------------------------
  1 | root = true
  2 | 
  3 | [*]
  4 | end_of_line = lf
  5 | charset = utf-8
  6 | trim_trailing_whitespace = true
  7 | insert_final_newline = true
  8 | 
  9 | [*.razor]
 10 | end_of_line = crlf
 11 | indent_style = space
 12 | indent_size = 4
 13 | 
 14 | [*.{cs,csx}]
 15 | end_of_line = lf
 16 | indent_style = space
 17 | indent_size = 4
 18 | 
 19 | [*.{md,markdown}]
 20 | trim_trailing_whitespace = false
 21 | 
 22 | [*.{json}]
 23 | indent_style = space
 24 | indent_size = 2
 25 | 
 26 | [*.{vcxproj,vcxproj.filters,csproj,props,targets}]
 27 | indent_style = space
 28 | indent_size = 2
 29 | end_of_line = crlf
 30 | charset = utf-8-bom
 31 | trim_trailing_whitespace = true
 32 | insert_final_newline = false
 33 | 
 34 | [*.{sln,sln.template}]
 35 | indent_style = tab
 36 | indent_size = 4
 37 | end_of_line = crlf
 38 | trim_trailing_whitespace = true
 39 | insert_final_newline = false
 40 | 
 41 | 
 42 | # ====================================================================================================
 43 | [*.{cs,csx,razor}]
 44 | 
 45 | file_header_template = unset
 46 | dotnet_sort_system_directives_first = false
 47 | dotnet_separate_import_directive_groups = true
 48 | 
 49 | dotnet_code_quality_unused_parameters = all:error
 50 | dotnet_remove_unnecessary_suppression_exclusions = none:error
 51 | csharp_style_unused_value_expression_statement_preference = discard_variable:suggestion
 52 | csharp_style_unused_value_assignment_preference = discard_variable:suggestion
 53 | 
 54 | ##
 55 | dotnet_style_namespace_match_folder = true:suggestion
 56 | dotnet_style_qualification_for_field = false:suggestion
 57 | dotnet_style_qualification_for_property = false:suggestion
 58 | dotnet_style_qualification_for_method = false:suggestion
 59 | dotnet_style_qualification_for_event = false:suggestion
 60 | dotnet_style_predefined_type_for_locals_parameters_members = true:error
 61 | dotnet_style_predefined_type_for_member_access = true:error
 62 | dotnet_style_parentheses_in_arithmetic_binary_operators = always_for_clarity:error
 63 | dotnet_style_parentheses_in_relational_binary_operators = always_for_clarity:error
 64 | dotnet_style_parentheses_in_other_binary_operators = always_for_clarity:error
 65 | dotnet_style_parentheses_in_other_operators = always_for_clarity:error
 66 | dotnet_style_require_accessibility_modifiers = always:error
 67 | dotnet_style_coalesce_expression = false:error
 68 | dotnet_style_null_propagation = false:error
 69 | dotnet_style_prefer_conditional_expression_over_assignment = false:error
 70 | dotnet_style_prefer_conditional_expression_over_return = false:error
 71 | dotnet_style_prefer_simplified_boolean_expressions = true:error
 72 | dotnet_style_prefer_is_null_check_over_reference_equality_method = true:suggestion
 73 | dotnet_style_readonly_field = true:error
 74 | dotnet_style_object_initializer = true:error
 75 | dotnet_style_collection_initializer = true:error
 76 | dotnet_style_explicit_tuple_names = true:error
 77 | dotnet_style_prefer_inferred_tuple_names = true:error
 78 | dotnet_style_prefer_inferred_anonymous_type_member_names = true:error
 79 | dotnet_style_prefer_auto_properties = true:error
 80 | dotnet_style_prefer_compound_assignment = true:error
 81 | dotnet_style_prefer_simplified_interpolation = true:error
 82 | dotnet_style_prefer_simplified_boolean_expressions = true:error
 83 | 
 84 | ##
 85 | csharp_style_expression_bodied_properties = true:error
 86 | csharp_style_expression_bodied_indexers = true:error
 87 | csharp_style_expression_bodied_accessors = false:error
 88 | csharp_style_expression_bodied_methods = false:error
 89 | csharp_style_expression_bodied_constructors = false:error
 90 | csharp_style_expression_bodied_operators = false:error
 91 | csharp_style_expression_bodied_lambdas = false:error
 92 | csharp_style_expression_bodied_local_functions = false:error
 93 | 
 94 | ##
 95 | csharp_new_line_before_open_brace = all
 96 | csharp_new_line_before_else = true
 97 | csharp_new_line_before_catch = true
 98 | csharp_new_line_before_finally = true
 99 | csharp_new_line_before_members_in_object_initializers = true
100 | csharp_new_line_before_members_in_anonymous_types = true
101 | csharp_new_line_between_query_expression_clauses = true
102 | csharp_indent_switch_labels = true
103 | csharp_indent_case_contents = true
104 | csharp_indent_case_contents_when_block = false
105 | csharp_indent_labels = no_change
106 | csharp_indent_block_contents = true
107 | csharp_indent_braces = false
108 | csharp_space_after_cast = false
109 | csharp_space_after_keywords_in_control_flow_statements = true
110 | csharp_space_between_parentheses = false
111 | csharp_space_before_colon_in_inheritance_clause = true
112 | csharp_space_after_colon_in_inheritance_clause = true
113 | csharp_space_around_binary_operators = before_and_after
114 | csharp_space_between_method_declaration_parameter_list_parentheses = false
115 | csharp_space_between_method_declaration_empty_parameter_list_parentheses = false
116 | csharp_space_between_method_declaration_name_and_open_parenthesis = false
117 | csharp_space_between_method_call_parameter_list_parentheses = false
118 | csharp_space_between_method_call_empty_parameter_list_parentheses = false
119 | csharp_space_between_method_call_name_and_opening_parenthesis = false
120 | csharp_space_after_comma = true
121 | csharp_space_before_comma = false
122 | csharp_space_after_dot = false
123 | csharp_space_before_dot = false
124 | csharp_space_after_semicolon_in_for_statement = true
125 | csharp_space_before_semicolon_in_for_statement = false
126 | csharp_space_around_declaration_statements = false
127 | csharp_space_before_open_square_brackets = false
128 | csharp_space_between_empty_square_brackets = false
129 | csharp_space_between_square_brackets = false
130 | csharp_preserve_single_line_statements = false
131 | csharp_preserve_single_line_blocks = true
132 | csharp_style_namespace_declarations = block_scoped:error
133 | csharp_style_conditional_delegate_call = false:error
134 | csharp_prefer_simple_default_expression = false:error
135 | 
136 | ##
137 | csharp_style_var_for_built_in_types = false:error
138 | csharp_style_var_when_type_is_apparent = false:error
139 | csharp_style_var_elsewhere = false:error
140 | csharp_style_pattern_local_over_anonymous_function = false:error
141 | csharp_style_prefer_index_operator = false:error
142 | csharp_style_prefer_range_operator = false:error
143 | csharp_style_implicit_object_creation_when_type_is_apparent = false:error
144 | csharp_prefer_static_local_function = true:error
145 | csharp_using_directive_placement = outside_namespace:error
146 | csharp_prefer_simple_using_statement = false:error
147 | csharp_style_prefer_switch_expression = false:error
148 | 
149 | ## prefix/postfix
150 | dotnet_naming_symbols.interface_group.applicable_kinds = interface
151 | dotnet_naming_rule.interface_rule.symbols              = interface_group
152 | dotnet_naming_rule.interface_rule.style                = xx_style_interface
153 | dotnet_naming_rule.interface_rule.severity             = suggestion
154 | dotnet_naming_style.xx_style_interface.capitalization = pascal_case
155 | dotnet_naming_style.xx_style_interface.required_prefix = I
156 | 
157 | dotnet_naming_symbols.enum_group.applicable_kinds = enum
158 | dotnet_naming_rule.enum_rule.symbols              = enum_group
159 | dotnet_naming_rule.enum_rule.style                = xx_style_enum
160 | dotnet_naming_rule.enum_rule.severity             = error
161 | dotnet_naming_style.xx_style_enum.required_prefix = E_
162 | dotnet_naming_style.xx_style_enum.capitalization = all_upper
163 | 
164 | dotnet_naming_symbols.private_fields.applicable_kinds           = field
165 | dotnet_naming_symbols.private_fields.applicable_accessibilities = private
166 | dotnet_naming_rule.private_fields_underscored.symbols           = private_fields
167 | dotnet_naming_rule.private_fields_underscored.style             = xx_style_privatemember
168 | dotnet_naming_rule.private_fields_underscored.severity          = error
169 | dotnet_naming_style.xx_style_privatemember.required_prefix      = _
170 | dotnet_naming_style.xx_style_privatemember.capitalization       = camel_case
171 | 
172 | 
173 | ## dotnet_diagnostic
174 | dotnet_analyzer_diagnostic.severity = error
175 | 
176 | dotnet_diagnostic.CA1002.severity = none
177 | dotnet_diagnostic.CA1307.severity = none
178 | dotnet_diagnostic.CA1707.severity = none
179 | dotnet_diagnostic.CA1805.severity = none
180 | dotnet_diagnostic.CA2007.severity = none
181 | dotnet_diagnostic.CA2227.severity = none # for razor file
182 | 
183 | dotnet_diagnostic.IDE0010.severity = suggestion
184 | dotnet_diagnostic.IDE0035.severity = none
185 | dotnet_diagnostic.IDE0051.severity = none
186 | dotnet_diagnostic.IDE0052.severity = none
187 | dotnet_diagnostic.IDE0060.severity = none
188 | dotnet_diagnostic.IDE0130.severity = suggestion
189 | 
190 | dotnet_diagnostic.JSON002.severity = none
191 | 
192 | dotnet_diagnostic.IDE1006.severity = error
193 | 
194 | dotnet_diagnostic.RCS1188.serverity = none
195 | 
196 | dotnet_diagnostic.VSTHRD111.severity = none
197 | 
198 | 
199 | 


--------------------------------------------------------------------------------
/NF.AI.PathFinding/playground/Program.cs:
--------------------------------------------------------------------------------
  1 | using NF.Mathematics;
  2 | 
  3 | using SFML.Graphics;
  4 | using SFML.System;
  5 | using SFML.Window;
  6 | 
  7 | using System;
  8 | using System.Diagnostics;
  9 | 
 10 | namespace NF.AI.PathFinding.Playground
 11 | {
 12 |     public enum E_ClickState
 13 |     {
 14 |         None,
 15 |         MoveStartNode,
 16 |         MoveGoalNode,
 17 |         MakeWall,
 18 |         EraseWall,
 19 |     }
 20 | 
 21 |     internal class Program
 22 |     {
 23 |         private readonly uint WIDTH = 1000;
 24 |         private readonly uint HEIGHT = 1000;
 25 | 
 26 |         private int NodeSize { get; } = 50;
 27 | 
 28 |         private Board mBoard;
 29 |         private E_ClickState mState = E_ClickState.None;
 30 |         private Int2? mLatestP = null;
 31 | 
 32 |         private static void Main()
 33 |         {
 34 |             //Main5();
 35 |             Program program = new Program();
 36 |             program.Run();
 37 |         }
 38 | 
 39 |         private void Run()
 40 |         {
 41 |             RenderWindow window = new RenderWindow(new VideoMode(WIDTH, HEIGHT), "sfml");
 42 |             window.KeyPressed += KeyPressed;
 43 |             window.Closed += Closed;
 44 |             window.MouseButtonPressed += OnMouseButtonPressed;
 45 |             window.MouseMoved += OnMouseMoved;
 46 |             window.MouseButtonReleased += OnMouseButtonReleased;
 47 | 
 48 |             ResetBoard();
 49 | 
 50 |             while (window.IsOpen)
 51 |             {
 52 |                 window.DispatchEvents();
 53 |                 window.Clear(Color.Blue);
 54 |                 window.Draw(mBoard);
 55 |                 window.Display();
 56 |             }
 57 |         }
 58 | 
 59 |         private void ResetBoard()
 60 |         {
 61 |             mBoard = new Board((int)WIDTH, (int)HEIGHT, NodeSize);
 62 |             mBoard.SetStart(new Int2(5, 5));
 63 |             mBoard.SetGoal(new Int2(11, 5));
 64 |             mBoard.ToggleWall(new Int2(6, 5));
 65 | 
 66 |             //mBoard.SetStart(new Int2(2, 2));
 67 |             //mBoard.SetGoal(new Int2(7, 2));
 68 |             //mBoard.ToggleWall(new Int2(6, 2));
 69 | 
 70 |             mBoard.StepAll();
 71 |             mBoard.Update();
 72 |         }
 73 | 
 74 |         private Vector2i GetGridPosition(Window window, int nodeSize)
 75 |         {
 76 |             return Mouse.GetPosition(window) / nodeSize;
 77 |         }
 78 | 
 79 |         private void OnMouseButtonPressed(object sender, MouseButtonEventArgs e)
 80 |         {
 81 |             if (e.Button != Mouse.Button.Left)
 82 |             {
 83 |                 return;
 84 |             }
 85 | 
 86 |             Window window = (Window)sender;
 87 |             Vector2i mp = GetGridPosition(window, NodeSize);
 88 |             Int2 mmp = new Int2(mp.X, mp.Y);
 89 | 
 90 |             mLatestP = mmp;
 91 | 
 92 |             if (mmp == mBoard.StartP)
 93 |             {
 94 |                 mState = E_ClickState.MoveStartNode;
 95 |                 return;
 96 |             }
 97 | 
 98 |             if (mmp == mBoard.GoalP)
 99 |             {
100 |                 mState = E_ClickState.MoveGoalNode;
101 |                 return;
102 |             }
103 | 
104 |             if (mBoard.IsWall(mmp))
105 |             {
106 | 
107 |                 mState = E_ClickState.EraseWall;
108 |                 return;
109 |             }
110 | 
111 |             mState = E_ClickState.MakeWall;
112 |         }
113 | 
114 |         private void OnMouseMoved(object sender, MouseMoveEventArgs e)
115 |         {
116 |             if (!Mouse.IsButtonPressed(Mouse.Button.Left))
117 |             {
118 |                 return;
119 |             }
120 | 
121 |             if (mState == E_ClickState.None)
122 |             {
123 |                 return;
124 |             }
125 | 
126 |             Window window = (Window)sender;
127 |             Vector2i mp = GetGridPosition(window, NodeSize);
128 |             Int2 mmp = new Int2(mp.X, mp.Y);
129 |             switch (mState)
130 |             {
131 |                 case E_ClickState.MoveStartNode:
132 |                 {
133 |                     if (!mBoard.IsWall(mmp))
134 |                     {
135 |                         mBoard.SetStart(mmp);
136 |                     }
137 |                 }
138 |                 break;
139 |                 case E_ClickState.MoveGoalNode:
140 |                 {
141 |                     if (!mBoard.IsWall(mmp))
142 |                     {
143 |                         mBoard.SetGoal(mmp);
144 |                     }
145 |                 }
146 |                 break;
147 |                 case E_ClickState.MakeWall:
148 |                 {
149 |                     if (mmp != mBoard.StartP && mmp != mBoard.GoalP)
150 |                     {
151 |                         mBoard.CreateWall(mmp);
152 |                     }
153 |                 }
154 |                 break;
155 |                 case E_ClickState.EraseWall:
156 |                 {
157 |                     if (mmp != mBoard.StartP && mmp != mBoard.GoalP)
158 |                     {
159 |                         mBoard.RemoveWall(mmp);
160 |                     }
161 |                 }
162 |                 break;
163 |                 default:
164 |                     break;
165 |             }
166 |             mBoard.Update();
167 |         }
168 | 
169 |         private void OnMouseButtonReleased(object sender, MouseButtonEventArgs e)
170 |         {
171 |             if (e.Button != Mouse.Button.Left)
172 |             {
173 |                 return;
174 |             }
175 | 
176 |             if (mState == E_ClickState.EraseWall || mState == E_ClickState.MakeWall)
177 |             {
178 |                 Window window = (Window)sender;
179 |                 Vector2i mp = GetGridPosition(window, NodeSize);
180 |                 Int2 mmp = new Int2(mp.X, mp.Y);
181 |                 if (mLatestP == mmp)
182 |                 {
183 |                     if (mState == E_ClickState.EraseWall)
184 |                     {
185 |                         mBoard.RemoveWall(mmp);
186 |                     }
187 |                     else
188 |                     {
189 |                         mBoard.CreateWall(mmp);
190 |                     }
191 |                 }
192 |                 mBoard.Update();
193 |             }
194 |             mLatestP = null;
195 |             mState = E_ClickState.None;
196 |         }
197 | 
198 |         private void Closed(object sender, EventArgs e)
199 |         {
200 |             Window window = (Window)sender;
201 |             window.Close();
202 |         }
203 | 
204 |         private void KeyPressed(object sender, KeyEventArgs e)
205 |         {
206 |             Window window = (Window)sender;
207 |             switch (e.Code)
208 |             {
209 |                 case Keyboard.Key.Escape:
210 |                     window.Close();
211 |                     break;
212 |                 case Keyboard.Key.Space:
213 |                     FindPath();
214 |                     break;
215 |                 case Keyboard.Key.R:
216 |                     ResetBoard();
217 |                     break;
218 |                 default:
219 |                     break;
220 |             }
221 | 
222 |         }
223 | 
224 |         private void FindPath()
225 |         {
226 |             Stopwatch sw = Stopwatch.StartNew();
227 |             mBoard.StepAll();
228 |             sw.Stop();
229 |             Console.WriteLine($"Step Ticks : {1000.0 * sw.ElapsedTicks / Stopwatch.Frequency}");
230 | 
231 |             mBoard.FindPath();
232 |         }
233 | 
234 |         private static bool[,] GetWalls(string[] strs)
235 |         {
236 |             int height = strs.Length;
237 |             int width = strs[0].Length;
238 |             bool[,] walls = new bool[height, width];
239 |             for (int y = 0; y < height; ++y)
240 |             {
241 |                 for (int x = 0; x < width; ++x)
242 |                 {
243 |                     switch (strs[y][x])
244 |                     {
245 |                         case 'X':
246 |                             walls[y, x] = true;
247 |                             break;
248 |                     }
249 |                 }
250 |             }
251 |             return walls;
252 |         }
253 | 
254 |         private static void Main5()
255 |         {
256 |             bool[,] walls = GetWalls(new string[] {
257 |                  "..X...X..",
258 |                  "......X..",
259 |                  ".XX...XX.",
260 |                  "..X......",
261 |                  "..X...X..",
262 |             });
263 | 
264 |             JPSPlus.JPSPlusMapBaker jpspBaker = new JPSPlus.JPSPlusMapBaker(walls);
265 |             JPSPlus.JPSPlusBakedMap bakedMap = jpspBaker.Bake();
266 |             JPSPlus.JPSPlus jpsp = new JPSPlus.JPSPlus();
267 |             jpsp.Init(bakedMap);
268 | 
269 |             _ = jpsp.SetStart(new Int2(0, 4));
270 |             _ = jpsp.SetGoal(new Int2(7, 0));
271 | 
272 |             Stopwatch sw = Stopwatch.StartNew();
273 |             bool isOk = jpsp.StepAll();
274 |             sw.Stop();
275 |             Console.WriteLine($"JPSPlus Take MS: {sw.ElapsedMilliseconds}");
276 | 
277 |             foreach (Common.AStarNode path in jpsp.GetPaths())
278 |             {
279 |                 Console.WriteLine(path.Position);
280 |             }
281 |             Console.WriteLine(isOk);
282 |             Console.WriteLine("Hello World!");
283 |         }
284 | 
285 |         private static void Main2()
286 |         {
287 |             bool[,] walls = GetWalls(new string[] {
288 |                  "..X...X..",
289 |                  "......X..",
290 |                  ".XX...XX.",
291 |                  "..X......",
292 |                  "..X...X..",
293 |             });
294 |             JPS.JPS jps = new JPS.JPS(walls);
295 |             jps.SetStart(new Int2(0, 4));
296 |             jps.SetGoal(new Int2(7, 0));
297 | 
298 |             Stopwatch sw = Stopwatch.StartNew();
299 |             bool isOk = jps.StepAll();
300 |             sw.Stop();
301 |             Console.WriteLine($"JPS Take MS: {sw.ElapsedMilliseconds}");
302 | 
303 |             foreach (Common.AStarNode path in jps.GetPaths())
304 |             {
305 |                 Console.WriteLine(path.Position);
306 |             }
307 |             Console.WriteLine(isOk);
308 |             Console.WriteLine("Hello World!");
309 |         }
310 | 
311 |         private static void Main3()
312 |         {
313 |             bool[,] walls = GetWalls(new string[] {
314 |                  "..X...X..",
315 |                  "......X..",
316 |                  ".XX...XX.",
317 |                  "..X......",
318 |                  "..X...X..",
319 |             });
320 |             AStar.AStar astar = new AStar.AStar(walls);
321 |             _ = astar.SetStart(new Int2(0, 4));
322 |             _ = astar.SetGoal(new Int2(7, 0));
323 | 
324 |             Stopwatch sw = Stopwatch.StartNew();
325 |             bool isOk = astar.StepAll();
326 |             sw.Stop();
327 |             Console.WriteLine($"AStar Take MS: {sw.ElapsedMilliseconds}");
328 | 
329 |             foreach (Common.AStarNode path in astar.GetPaths())
330 |             {
331 |                 Console.WriteLine(path.Position);
332 |             }
333 |             Console.WriteLine(isOk);
334 |             Console.WriteLine("Hello World!");
335 |         }
336 | 
337 |         private static void Main4()
338 |         {
339 |             bool[,] walls = GetWalls(new string[] {
340 |                  "..X...X..",
341 |                  "......X..",
342 |                  ".XX...XX.",
343 |                  "..X......",
344 |                  "..X...X..",
345 |             });
346 |             JPSOrthogonal.JPSOrthogonal jpso = new JPSOrthogonal.JPSOrthogonal(walls);
347 |             _ = jpso.SetStart(new Int2(0, 4));
348 |             _ = jpso.SetGoal(new Int2(7, 0));
349 | 
350 |             Stopwatch sw = Stopwatch.StartNew();
351 |             bool isOk = jpso.StepAll();
352 |             sw.Stop();
353 |             Console.WriteLine($"JPSOrthogonal Take MS: {sw.ElapsedMilliseconds}");
354 | 
355 |             foreach (Common.AStarNode path in jpso.GetPaths())
356 |             {
357 |                 Console.WriteLine(path.Position);
358 |             }
359 |             Console.WriteLine(isOk);
360 |             Console.WriteLine("Hello World!");
361 |         }
362 |     }
363 | }
364 | 


--------------------------------------------------------------------------------
/NF.AI.PathFinding/src/NF.AI.PathFinding.JPSPlus/JPSPlus.cs:
--------------------------------------------------------------------------------
  1 | using NF.AI.PathFinding.Common;
  2 | using NF.Collections.Generic;
  3 | using NF.Mathematics;
  4 | 
  5 | using System;
  6 | using System.Collections.Generic;
  7 | using System.Linq;
  8 | using System.Runtime.CompilerServices;
  9 | 
 10 | namespace NF.AI.PathFinding.JPSPlus
 11 | {
 12 |     public class JPSPlus
 13 |     {
 14 |         // =======================
 15 |         // Members
 16 |         // =======================
 17 |         private JPSPlusNode mStart = null;
 18 |         private JPSPlusNode mGoal = null;
 19 |         private readonly Dictionary<Int2, JPSPlusNode> mCreatedNodes = new Dictionary<Int2, JPSPlusNode>();
 20 |         private readonly PriorityQueue<(JPSPlusNode Node, EDirFlags Dir)> mOpenList = new PriorityQueue<(JPSPlusNode Node, EDirFlags Dir)>();
 21 |         private readonly HashSet<JPSPlusNode> mCloseList = new HashSet<JPSPlusNode>();
 22 |         private JPSPlusBakedMap mBakedMap;
 23 | 
 24 |         public JPSPlus()
 25 |         {
 26 | 
 27 |         }
 28 | 
 29 |         public void Init(JPSPlusBakedMap bakedMap)
 30 |         {
 31 |             mBakedMap = bakedMap;
 32 |         }
 33 | 
 34 |         public bool StepAll(int stepCount = int.MaxValue)
 35 |         {
 36 |             mOpenList.Clear();
 37 |             mCloseList.Clear();
 38 | 
 39 |             foreach (JPSPlusNode node in mCreatedNodes.Values.ToList())
 40 |             {
 41 |                 int index = mBakedMap.BlockLUT[node.Position.Y, node.Position.X];
 42 |                 if (index < 0)
 43 |                 {
 44 |                     _ = mCreatedNodes.Remove(node.Position);
 45 |                 }
 46 |                 else
 47 |                 {
 48 |                     node.Refresh(mBakedMap.Blocks[index].JumpDistances);
 49 |                 }
 50 |             }
 51 |             mOpenList.Enqueue((mStart, EDirFlags.ALL), mStart.F);
 52 |             return Step(stepCount);
 53 |         }
 54 | 
 55 |         public bool Step(int stepCount)
 56 |         {
 57 |             int step = stepCount;
 58 |             while (true)
 59 |             {
 60 |                 if (step <= 0)
 61 |                 {
 62 |                     return false;
 63 |                 }
 64 |                 if (mOpenList.Count == 0)
 65 |                 {
 66 |                     return false;
 67 |                 }
 68 | 
 69 |                 (JPSPlusNode Node, EDirFlags fromDir) curr = mOpenList.Dequeue();
 70 |                 JPSPlusNode currNode = curr.Node;
 71 |                 EDirFlags fromDir = curr.fromDir;
 72 |                 _ = mCloseList.Add(currNode);
 73 | 
 74 |                 Int2 currPos = currNode.Position;
 75 |                 Int2 goalPos = mGoal.Position;
 76 | 
 77 |                 if (currPos == goalPos)
 78 |                 {
 79 |                     return true;
 80 |                 }
 81 | 
 82 |                 EDirFlags validDirs = ValidLookUPTable(fromDir);
 83 |                 for (int i = 0b10000000; i > 0; i >>= 1)
 84 |                 {
 85 |                     EDirFlags processDir = (EDirFlags)i;
 86 |                     if ((processDir & validDirs) == EDirFlags.NONE)
 87 |                     {
 88 |                         continue;
 89 |                     }
 90 | 
 91 |                     bool isDiagonalDir = DirFlags.IsDiagonal(processDir);
 92 |                     int dirDistance = currNode.GetDistance(processDir);
 93 |                     int lengthX = RowDiff(currNode, mGoal);
 94 |                     int lengthY = ColDiff(currNode, mGoal);
 95 | 
 96 | 
 97 |                     JPSPlusNode nextNode;
 98 |                     int nextG;
 99 |                     if (!isDiagonalDir
100 |                         && IsGoalInExactDirection(currPos, processDir, goalPos)
101 |                         && Math.Max(lengthX, lengthY) <= Math.Abs(dirDistance))
102 |                     {
103 |                         // 직선이동중
104 |                         // 골과 같은 방향
105 |                         // 골 노드거리 방향거리보다 같거나 작으면 그게 바로 골
106 |                         nextNode = mGoal;
107 |                         nextG = currNode.G + (Math.Max(lengthX, lengthY) * 10);
108 |                     }
109 |                     else if (isDiagonalDir
110 |                         && IsGoalInGeneralDirection(currPos, processDir, goalPos)
111 |                         && (lengthX <= Math.Abs(dirDistance) || lengthY <= Math.Abs(dirDistance))
112 |                         )
113 |                     {
114 |                         // Target Jump Point
115 |                         // 대각 이동중
116 |                         // 골과 일반적 방향
117 |                         int minDiff = Math.Min(lengthX, lengthY);
118 |                         nextNode = GetNode(currNode, minDiff, processDir);
119 |                         nextG = currNode.G + (Math.Max(lengthX, lengthY) * 14); // 대각길이 비용.
120 |                     }
121 |                     else if (dirDistance > 0)
122 |                     {
123 |                         // 점프가 가능하면 점프!
124 |                         nextNode = GetNode(currNode, processDir);
125 |                         if (isDiagonalDir)
126 |                         {
127 |                             nextG = currNode.G + (Math.Max(lengthX, lengthY) * 14);
128 |                         }
129 |                         else
130 |                         {
131 |                             nextG = currNode.G + (Math.Max(lengthX, lengthY) * 10);
132 |                         }
133 |                     }
134 |                     else
135 |                     {
136 |                         // 찾지못하면 다음 것으로.
137 |                         continue;
138 |                     }
139 | 
140 |                     (JPSPlusNode, EDirFlags) openJump = (nextNode, processDir);
141 | 
142 |                     if (!mOpenList.Contains(openJump) && !mCloseList.Contains(nextNode))
143 |                     {
144 |                         nextNode.Parent = currNode;
145 |                         nextNode.G = nextG;
146 |                         nextNode.H = H(nextNode, mGoal);
147 |                         mOpenList.Enqueue(openJump, nextNode.F);
148 |                     }
149 |                     else if (nextG < nextNode.G)
150 |                     {
151 |                         nextNode.Parent = currNode;
152 |                         nextNode.G = nextG;
153 |                         nextNode.H = H(nextNode, mGoal);
154 |                         mOpenList.UpdatePriority(openJump, nextNode.F);
155 |                     }
156 |                 }
157 |                 step--;
158 |             }
159 |         }
160 | 
161 |         public bool IsWalkable(in Int2 p)
162 |         {
163 |             if (mBakedMap == null)
164 |             {
165 |                 return false;
166 |             }
167 | 
168 |             if (!IsInBoundary(p))
169 |             {
170 |                 return false;
171 |             }
172 | 
173 |             return mBakedMap.BlockLUT[p.Y, p.X] >= 0;
174 |         }
175 | 
176 |         public bool SetStart(in Int2 p)
177 |         {
178 |             if (mBakedMap == null)
179 |             {
180 |                 return false;
181 |             }
182 | 
183 |             if (!IsInBoundary(p))
184 |             {
185 |                 return false;
186 |             }
187 | 
188 |             mStart = GetOrCreatedNode(p);
189 |             return true;
190 |         }
191 | 
192 |         public bool SetGoal(in Int2 p)
193 |         {
194 |             if (mBakedMap == null)
195 |             {
196 |                 return false;
197 |             }
198 | 
199 |             if (!IsInBoundary(p))
200 |             {
201 |                 return false;
202 |             }
203 | 
204 |             mGoal = GetOrCreatedNode(p);
205 |             return true;
206 |         }
207 | 
208 |         public JPSPlusNode GetJPSPlusNode(in Int2 p)
209 |         {
210 |             return new JPSPlusNode(p, mBakedMap.Blocks[mBakedMap.BlockLUT[p.Y, p.X]].JumpDistances);
211 |         }
212 | 
213 |         public IReadOnlyList<AStarNode> GetPaths()
214 |         {
215 |             List<AStarNode> ret = new List<AStarNode>();
216 |             AStarNode n = mGoal;
217 |             while (n != null)
218 |             {
219 |                 ret.Add(n);
220 |                 n = n.Parent;
221 |             }
222 |             ret.Reverse();
223 |             return ret;
224 |         }
225 | 
226 |         [MethodImpl(MethodImplOptions.AggressiveInlining)]
227 |         public JPSPlusNode GetStart()
228 |         {
229 |             return mStart;
230 |         }
231 | 
232 |         [MethodImpl(MethodImplOptions.AggressiveInlining)]
233 |         public JPSPlusNode GetGoal()
234 |         {
235 |             return mGoal;
236 |         }
237 | 
238 |         [MethodImpl(MethodImplOptions.AggressiveInlining)]
239 |         public PriorityQueue<(JPSPlusNode, EDirFlags)> GetOpenList()
240 |         {
241 |             return mOpenList;
242 |         }
243 | 
244 |         [MethodImpl(MethodImplOptions.AggressiveInlining)]
245 |         public HashSet<JPSPlusNode> GetCloseList()
246 |         {
247 |             return mCloseList;
248 |         }
249 | 
250 |         public int Width => mBakedMap.Width;
251 |         public int Height => mBakedMap.Height;
252 | 
253 |         // =======================
254 |         // Private Methods
255 |         // =======================
256 |         private JPSPlusNode GetOrCreatedNode(in Int2 p)
257 |         {
258 |             if (mCreatedNodes.TryGetValue(p, out JPSPlusNode createdNode))
259 |             {
260 |                 return createdNode;
261 |             }
262 |             JPSPlusNode newNode = GetJPSPlusNode(p);
263 |             mCreatedNodes.Add(p, newNode);
264 |             return newNode;
265 |         }
266 | 
267 |         private bool IsInBoundary(in Int2 p)
268 |         {
269 |             return 0 <= p.X && p.X < Width && 0 <= p.Y && p.Y < Height;
270 |         }
271 | 
272 |         #region plus
273 |         private EDirFlags ValidLookUPTable(EDirFlags dir)
274 |         {
275 |             switch (dir)
276 |             {
277 |                 // . N .
278 |                 // W . E
279 |                 // . S .
280 |                 case EDirFlags.NORTH:
281 |                     return EDirFlags.EAST | EDirFlags.NORTHEAST | EDirFlags.NORTH | EDirFlags.NORTHWEST | EDirFlags.WEST;
282 |                 case EDirFlags.WEST:
283 |                     return EDirFlags.NORTH | EDirFlags.NORTHWEST | EDirFlags.WEST | EDirFlags.SOUTHWEST | EDirFlags.SOUTH;
284 |                 case EDirFlags.EAST:
285 |                     return EDirFlags.SOUTH | EDirFlags.SOUTHEAST | EDirFlags.EAST | EDirFlags.NORTHEAST | EDirFlags.NORTH;
286 |                 case EDirFlags.SOUTH:
287 |                     return EDirFlags.WEST | EDirFlags.SOUTHWEST | EDirFlags.SOUTH | EDirFlags.SOUTHEAST | EDirFlags.EAST;
288 |                 case EDirFlags.NORTHWEST:
289 |                     return EDirFlags.NORTH | EDirFlags.NORTHWEST | EDirFlags.WEST;
290 |                 case EDirFlags.NORTHEAST:
291 |                     return EDirFlags.NORTH | EDirFlags.NORTHEAST | EDirFlags.EAST;
292 |                 case EDirFlags.SOUTHWEST:
293 |                     return EDirFlags.SOUTH | EDirFlags.SOUTHWEST | EDirFlags.WEST;
294 |                 case EDirFlags.SOUTHEAST:
295 |                     return EDirFlags.SOUTH | EDirFlags.SOUTHEAST | EDirFlags.EAST;
296 |                 default:
297 |                     return dir;
298 |             }
299 |         }
300 | 
301 |         private bool IsGoalInExactDirection(in Int2 curr, EDirFlags processDir, in Int2 goal)
302 |         {
303 |             int dx = goal.X - curr.X;
304 |             int dy = goal.Y - curr.Y;
305 | 
306 |             switch (processDir)
307 |             {
308 |                 case EDirFlags.NORTH:
309 |                     return dx == 0 && dy < 0;
310 |                 case EDirFlags.SOUTH:
311 |                     return dx == 0 && dy > 0;
312 |                 case EDirFlags.WEST:
313 |                     return dx < 0 && dy == 0;
314 |                 case EDirFlags.EAST:
315 |                     return dx > 0 && dy == 0;
316 |                 case EDirFlags.NORTHWEST:
317 |                     return dx < 0 && dy < 0 && (Math.Abs(dx) == Math.Abs(dy));
318 |                 case EDirFlags.NORTHEAST:
319 |                     return dx > 0 && dy < 0 && (Math.Abs(dx) == Math.Abs(dy));
320 |                 case EDirFlags.SOUTHWEST:
321 |                     return dx < 0 && dy > 0 && (Math.Abs(dx) == Math.Abs(dy));
322 |                 case EDirFlags.SOUTHEAST:
323 |                     return dx > 0 && dy > 0 && (Math.Abs(dx) == Math.Abs(dy));
324 |                 default:
325 |                     return false;
326 |             }
327 |         }
328 | 
329 |         private bool IsGoalInGeneralDirection(in Int2 curr, EDirFlags processDir, in Int2 goal)
330 |         {
331 |             int dx = goal.X - curr.X;
332 |             int dy = goal.Y - curr.Y;
333 | 
334 |             switch (processDir)
335 |             {
336 |                 case EDirFlags.NORTH:
337 |                     return dx == 0 && dy < 0;
338 |                 case EDirFlags.SOUTH:
339 |                     return dx == 0 && dy > 0;
340 |                 case EDirFlags.WEST:
341 |                     return dx < 0 && dy == 0;
342 |                 case EDirFlags.EAST:
343 |                     return dx > 0 && dy == 0;
344 |                 case EDirFlags.NORTHWEST:
345 |                     return dx < 0 && dy < 0;
346 |                 case EDirFlags.NORTHEAST:
347 |                     return dx > 0 && dy < 0;
348 |                 case EDirFlags.SOUTHWEST:
349 |                     return dx < 0 && dy > 0;
350 |                 case EDirFlags.SOUTHEAST:
351 |                     return dx > 0 && dy > 0;
352 |                 default:
353 |                     return false;
354 |             }
355 |         }
356 | 
357 |         private JPSPlusNode GetNode(JPSPlusNode node, EDirFlags dir)
358 |         {
359 |             return GetOrCreatedNode(node.Position + (DirFlags.ToPos(dir) * node.GetDistance(dir)));
360 |         }
361 | 
362 |         private JPSPlusNode GetNode(JPSPlusNode node, int dist, EDirFlags dir)
363 |         {
364 |             return GetOrCreatedNode(node.Position + (DirFlags.ToPos(dir) * dist));
365 |         }
366 | 
367 |         private int ColDiff(JPSPlusNode currNode, JPSPlusNode goalNode)
368 |         {
369 |             return Math.Abs(goalNode.Position.X - currNode.Position.X);
370 |         }
371 | 
372 |         private int RowDiff(JPSPlusNode currNode, JPSPlusNode goalNode)
373 |         {
374 |             return Math.Abs(goalNode.Position.Y - currNode.Position.Y);
375 |         }
376 |         #endregion plus
377 | 
378 |         // =========================================
379 |         // Statics
380 |         // =========================================
381 |         [MethodImpl(MethodImplOptions.AggressiveInlining)]
382 |         private static int H(JPSPlusNode n, JPSPlusNode goal)
383 |         {
384 |             // calculate estimated cost
385 |             return (Math.Abs(goal.Position.X - n.Position.X) + Math.Abs(goal.Position.Y - n.Position.Y)) * 10;
386 |         }
387 |     }
388 | }
389 | 


--------------------------------------------------------------------------------
/NF.AI.PathFinding/src/NF.AI.PathFinding.JPSOrthogonal/JPSOrthogonal.cs:
--------------------------------------------------------------------------------
  1 | using NF.AI.PathFinding.Common;
  2 | using NF.Collections.Generic;
  3 | using NF.Mathematics;
  4 | 
  5 | using System;
  6 | using System.Collections.Generic;
  7 | using System.Runtime.CompilerServices;
  8 | 
  9 | namespace NF.AI.PathFinding.JPSOrthogonal
 10 | {
 11 |     public class JPSOrthogonal
 12 |     {
 13 | 
 14 |         // =======================
 15 |         // Members
 16 |         // =======================
 17 |         private AStarNode mStart = null;
 18 |         private AStarNode mGoal = null;
 19 |         private readonly Dictionary<Int2, AStarNode> mCreateNodes = new Dictionary<Int2, AStarNode>();
 20 |         private readonly PriorityQueue<(AStarNode AStarNode, EDirFlags Dir)> mOpenList = new PriorityQueue<(AStarNode AStarNode, EDirFlags Dir)>();
 21 |         private readonly HashSet<AStarNode> mCloseList = new HashSet<AStarNode>();
 22 |         private bool[,] mWalls = null;
 23 | 
 24 |         public int Width { get; private set; }
 25 |         public int Height { get; private set; }
 26 | 
 27 |         public Int2 StartP => mStart.Position;
 28 |         public Int2 GoalP => mGoal.Position;
 29 | 
 30 |         public JPSOrthogonal()
 31 |         {
 32 | 
 33 |         }
 34 | 
 35 |         public JPSOrthogonal(int width, int height)
 36 |         {
 37 |             Init(new bool[height, width]);
 38 |         }
 39 |         public JPSOrthogonal(bool[,] walls)
 40 |         {
 41 |             Init(walls);
 42 |         }
 43 | 
 44 |         public void Init(bool[,] walls)
 45 |         {
 46 |             Width = walls.GetLength(1);
 47 |             Height = walls.GetLength(0);
 48 |             mWalls = walls;
 49 |         }
 50 | 
 51 |         public bool StepAll(int stepCount = int.MaxValue)
 52 |         {
 53 |             mOpenList.Clear();
 54 |             mCloseList.Clear();
 55 |             foreach (AStarNode AStarNode in mCreateNodes.Values)
 56 |             {
 57 |                 AStarNode.Refresh();
 58 |             }
 59 |             mOpenList.Enqueue((mStart, EDirFlags.ALL), mStart.F);
 60 |             return Step(stepCount);
 61 |         }
 62 | 
 63 |         public bool Step(int stepCount)
 64 |         {
 65 |             int step = stepCount;
 66 |             Int2 jumpPos = new Int2(0, 0);
 67 | 
 68 |             while (true)
 69 |             {
 70 |                 if (step <= 0)
 71 |                 {
 72 |                     return false;
 73 |                 }
 74 |                 if (mOpenList.Count == 0)
 75 |                 {
 76 |                     return false;
 77 |                 }
 78 |                 (AStarNode AStarNode, EDirFlags Dir) curr = mOpenList.Dequeue();
 79 |                 AStarNode currNode = curr.AStarNode;
 80 |                 EDirFlags currDir = curr.Dir;
 81 |                 _ = mCloseList.Add(currNode);
 82 | 
 83 |                 Int2 currPos = currNode.Position;
 84 |                 Int2 goalPos = mGoal.Position;
 85 |                 if (currPos == goalPos)
 86 |                 {
 87 |                     return true;
 88 |                 }
 89 | 
 90 |                 EDirFlags succesors = SuccesorsDir(currPos, currDir);
 91 |                 for (int i = 0b10000000; i > 0; i >>= 1)
 92 |                 {
 93 |                     EDirFlags succesorDir = (EDirFlags)i;
 94 |                     if ((succesorDir & succesors) == EDirFlags.NONE)
 95 |                     {
 96 |                         continue;
 97 |                     }
 98 | 
 99 |                     if (!TryJump(currPos, succesorDir, goalPos, ref jumpPos))
100 |                     {
101 |                         continue;
102 |                     }
103 | 
104 |                     AStarNode jumpNode = GetOrCreateNode(jumpPos);
105 |                     if (mCloseList.Contains(jumpNode))
106 |                     {
107 |                         continue;
108 |                     }
109 | 
110 |                     int jumpG = G(currNode, jumpNode);
111 |                     (AStarNode, EDirFlags) openJump = (jumpNode, succesorDir);
112 |                     if (!mOpenList.Contains(openJump))
113 |                     {
114 |                         jumpNode.Parent = currNode;
115 |                         jumpNode.G = jumpG;
116 |                         jumpNode.H = H(jumpNode, mGoal);
117 |                         mOpenList.Enqueue(openJump, jumpNode.F);
118 |                     }
119 |                     else if (jumpG < jumpNode.G)
120 |                     {
121 |                         jumpNode.Parent = currNode;
122 |                         jumpNode.G = jumpG;
123 |                         jumpNode.H = H(jumpNode, mGoal);
124 |                         mOpenList.UpdatePriority(openJump, jumpNode.F);
125 |                     }
126 |                 }
127 |                 step--;
128 |             }
129 |         }
130 |         public bool SetStart(in Int2 p)
131 |         {
132 |             if (!IsInBoundary(p))
133 |             {
134 |                 return false;
135 |             }
136 |             mStart = GetOrCreateNode(p);
137 |             return true;
138 |         }
139 | 
140 |         public bool SetGoal(in Int2 p)
141 |         {
142 |             if (!IsInBoundary(p))
143 |             {
144 |                 return false;
145 |             }
146 |             mGoal = GetOrCreateNode(p);
147 |             return true;
148 |         }
149 |         public void SetWall(in Int2 p, bool isWall)
150 |         {
151 |             if (!IsInBoundary(p))
152 |             {
153 |                 return;
154 |             }
155 |             mWalls[p.Y, p.X] = isWall;
156 |         }
157 | 
158 |         public bool ToggleWall(in Int2 p)
159 |         {
160 |             if (!IsInBoundary(p))
161 |             {
162 |                 return false;
163 |             }
164 |             mWalls[p.Y, p.X] = !mWalls[p.Y, p.X];
165 |             return true;
166 |         }
167 | 
168 |         public IReadOnlyList<AStarNode> GetPaths()
169 |         {
170 |             List<AStarNode> ret = new List<AStarNode>();
171 |             AStarNode n = mGoal;
172 |             while (n != null)
173 |             {
174 |                 ret.Add(n);
175 |                 n = n.Parent;
176 |             }
177 |             ret.Reverse();
178 |             return ret;
179 |         }
180 | 
181 |         [MethodImpl(MethodImplOptions.AggressiveInlining)]
182 |         public AStarNode GetStart()
183 |         {
184 |             return mStart;
185 |         }
186 | 
187 |         [MethodImpl(MethodImplOptions.AggressiveInlining)]
188 |         public AStarNode GetGoal()
189 |         {
190 |             return mGoal;
191 |         }
192 | 
193 |         [MethodImpl(MethodImplOptions.AggressiveInlining)]
194 |         public PriorityQueue<(AStarNode, EDirFlags)> GetOpenList()
195 |         {
196 |             return mOpenList;
197 |         }
198 | 
199 |         [MethodImpl(MethodImplOptions.AggressiveInlining)]
200 |         public HashSet<AStarNode> GetCloseList()
201 |         {
202 |             return mCloseList;
203 |         }
204 | 
205 |         [MethodImpl(MethodImplOptions.AggressiveInlining)]
206 |         public bool[,] GetWalls()
207 |         {
208 |             return mWalls;
209 |         }
210 | 
211 | 
212 |         // =======================
213 |         // Private Methods
214 |         // =======================
215 |         private AStarNode GetOrCreateNode(in Int2 p)
216 |         {
217 |             if (mCreateNodes.TryGetValue(p, out AStarNode node))
218 |             {
219 |                 return node;
220 |             }
221 |             AStarNode newNode = new AStarNode(p);
222 |             mCreateNodes.Add(p, newNode);
223 |             return newNode;
224 |         }
225 | 
226 |         public bool IsWalkable(in Int2 p)
227 |         {
228 |             if (!IsInBoundary(p))
229 |             {
230 |                 return false;
231 |             }
232 |             return !mWalls[p.Y, p.X];
233 |         }
234 | 
235 |         private bool IsInBoundary(in Int2 p)
236 |         {
237 |             return 0 <= p.X && p.X < Width && 0 <= p.Y && p.Y < Height;
238 |         }
239 | 
240 |         internal EDirFlags NeighbourDir(in Int2 pos)
241 |         {
242 |             EDirFlags ret = EDirFlags.NONE;
243 |             for (int i = 0b10000000; i > 0; i >>= 1)
244 |             {
245 |                 EDirFlags dir = (EDirFlags)i;
246 |                 if (!IsWalkable(pos.Foward(dir)))
247 |                 {
248 |                     continue;
249 |                 }
250 | 
251 |                 if (DirFlags.IsDiagonal(dir))
252 |                 {
253 |                     Int2 dp = DirFlags.ToPos(dir);
254 |                     if (!IsWalkable(new Int2(pos.X + dp.X, pos.Y)) ||
255 |                         !IsWalkable(new Int2(pos.X, pos.Y + dp.Y)))
256 |                     {
257 |                         continue;
258 |                     }
259 |                 }
260 |                 ret |= dir;
261 |             }
262 |             return ret;
263 |         }
264 | 
265 |         internal EDirFlags OrthogonalNeighbourDir(in Int2 pos)
266 |         {
267 |             EDirFlags ret = EDirFlags.NONE;
268 |             for (int i = 0b00001000; i > 0; i >>= 1)
269 |             {
270 |                 EDirFlags dir = (EDirFlags)i;
271 |                 if (!IsWalkable(pos.Foward(dir)))
272 |                 {
273 |                     continue;
274 |                 }
275 |                 ret |= dir;
276 |             }
277 |             return ret;
278 |         }
279 | 
280 |         internal EDirFlags OrthogonalForcedNeighbourDir(in Int2 n, EDirFlags dir)
281 |         {
282 |             if (dir == EDirFlags.ALL)
283 |             {
284 |                 return EDirFlags.ALL;
285 |             }
286 | 
287 |             EDirFlags ret = EDirFlags.NONE;
288 | 
289 |             Int2 next = new Int2(0, 0);
290 |             if (DirFlags.IsStraight(dir))
291 |             {
292 |                 next = n.Backward(dir);
293 |             }
294 | 
295 |             switch (dir)
296 |             {
297 |                 case EDirFlags.NORTH:
298 |                     // F . F
299 |                     // X N X
300 |                     // . P .
301 |                     if (!IsWalkable(next.Foward(EDirFlags.EAST)))
302 |                     {
303 |                         ret |= EDirFlags.EAST | EDirFlags.NORTHEAST;
304 |                     }
305 |                     if (!IsWalkable(next.Foward(EDirFlags.WEST)))
306 |                     {
307 |                         ret |= EDirFlags.WEST | EDirFlags.NORTHWEST;
308 |                     }
309 |                     break;
310 |                 case EDirFlags.SOUTH:
311 |                     // . P .
312 |                     // X N X
313 |                     // F . F
314 |                     if (!IsWalkable(next.Foward(EDirFlags.EAST)))
315 |                     {
316 |                         ret |= EDirFlags.EAST | EDirFlags.SOUTHEAST;
317 |                     }
318 |                     if (!IsWalkable(next.Foward(EDirFlags.WEST)))
319 |                     {
320 |                         ret |= EDirFlags.WEST | EDirFlags.SOUTHWEST;
321 |                     }
322 |                     break;
323 |                 case EDirFlags.EAST:
324 |                     // . X F
325 |                     // P N .
326 |                     // . X F
327 |                     if (!IsWalkable(next.Foward(EDirFlags.NORTH)))
328 |                     {
329 |                         ret |= EDirFlags.NORTH | EDirFlags.NORTHEAST;
330 |                     }
331 |                     if (!IsWalkable(next.Foward(EDirFlags.SOUTH)))
332 |                     {
333 |                         ret |= EDirFlags.SOUTH | EDirFlags.SOUTHEAST;
334 |                     }
335 |                     break;
336 |                 case EDirFlags.WEST:
337 |                     // F X .
338 |                     // . N P
339 |                     // F X .
340 |                     if (!IsWalkable(next.Foward(EDirFlags.NORTH)))
341 |                     {
342 |                         ret |= EDirFlags.NORTH | EDirFlags.NORTHWEST;
343 |                     }
344 |                     if (!IsWalkable(next.Foward(EDirFlags.SOUTH)))
345 |                     {
346 |                         ret |= EDirFlags.SOUTH | EDirFlags.SOUTHWEST;
347 |                     }
348 |                     break;
349 |                 case EDirFlags.NORTHWEST:
350 |                     break;
351 |                 case EDirFlags.NORTHEAST:
352 |                     break;
353 |                 case EDirFlags.SOUTHWEST:
354 |                     break;
355 |                 case EDirFlags.SOUTHEAST:
356 |                     break;
357 |                 case EDirFlags.ALL:
358 |                     ret |= EDirFlags.ALL;
359 |                     break;
360 |                 case EDirFlags.NONE:
361 |                 default:
362 |                     throw new ArgumentOutOfRangeException($"[ForcedNeighbourDir] invalid dir - {dir}");
363 |             }
364 |             return ret;
365 |         }
366 | 
367 |         [MethodImpl(MethodImplOptions.AggressiveInlining)]
368 |         internal EDirFlags SuccesorsDir(in Int2 pos, EDirFlags dir)
369 |         {
370 |             return NeighbourDir(pos) & (NaturalNeighbours(dir) | OrthogonalForcedNeighbourDir(pos, dir));
371 |         }
372 | 
373 |         internal bool TryJump(in Int2 p, EDirFlags dir, in Int2 goal, ref Int2 outJumped)
374 |         {
375 |             Int2 curr = p;
376 |             Int2 next = curr.Foward(dir);
377 | 
378 |             while (true)
379 |             {
380 |                 if (!IsWalkable(next))
381 |                 {
382 |                     return false;
383 |                 }
384 |                 if ((dir & NeighbourDir(curr)) == EDirFlags.NONE)
385 |                 {
386 |                     return false;
387 |                 }
388 |                 if (next == goal)
389 |                 {
390 |                     outJumped = next;
391 |                     return true;
392 |                 }
393 | 
394 |                 if ((OrthogonalForcedNeighbourDir(next, dir) & OrthogonalNeighbourDir(next)) != EDirFlags.NONE)
395 |                 {
396 |                     outJumped = next;
397 |                     return true;
398 |                 }
399 | 
400 |                 if (DirFlags.IsDiagonal(dir))
401 |                 {
402 |                     // TODO(pyoung): TOC 가능하게 수정 할 수 있나?
403 |                     // d1: EAST  | WEST
404 |                     if (TryJump(next, DirFlags.DiagonalToEastWest(dir), goal, ref outJumped))
405 |                     {
406 |                         outJumped = next;
407 |                         return true;
408 |                     }
409 |                     // d2: NORTH | SOUTH
410 |                     if (TryJump(next, DirFlags.DiagonalToNorthSouth(dir), goal, ref outJumped))
411 |                     {
412 |                         outJumped = next;
413 |                         return true;
414 |                     }
415 |                 }
416 |                 curr = next;
417 |                 next = next.Foward(dir);
418 |             }
419 |         }
420 | 
421 |         // =========================================
422 |         // Statics
423 |         // =========================================
424 |         private static EDirFlags NaturalNeighbours(EDirFlags dir)
425 |         {
426 | 
427 |             switch (dir)
428 |             {
429 |                 case EDirFlags.NORTHEAST:
430 |                     return EDirFlags.NORTHEAST | EDirFlags.NORTH | EDirFlags.EAST;
431 |                 case EDirFlags.NORTHWEST:
432 |                     return EDirFlags.NORTHWEST | EDirFlags.NORTH | EDirFlags.WEST;
433 |                 case EDirFlags.SOUTHEAST:
434 |                     return EDirFlags.SOUTHEAST | EDirFlags.SOUTH | EDirFlags.EAST;
435 |                 case EDirFlags.SOUTHWEST:
436 |                     return EDirFlags.SOUTHWEST | EDirFlags.SOUTH | EDirFlags.WEST;
437 |                 default:
438 |                     return dir;
439 |             }
440 |         }
441 | 
442 |         [MethodImpl(MethodImplOptions.AggressiveInlining)]
443 |         private static int G(AStarNode from, AStarNode adjacent)
444 |         {
445 |             // cost so far to reach n 
446 |             Int2 p = from.Position - adjacent.Position;
447 |             if (p.X == 0 || p.Y == 0)
448 |             {
449 |                 return from.G + (Math.Max(Math.Abs(p.X), Math.Abs(p.Y)) * 10);
450 |             }
451 |             else
452 |             {
453 |                 return from.G + (Math.Max(Math.Abs(p.X), Math.Abs(p.Y)) * 14);
454 |             }
455 |         }
456 | 
457 |         [MethodImpl(MethodImplOptions.AggressiveInlining)]
458 |         internal static int H(AStarNode n, AStarNode goal)
459 |         {
460 |             // calculate estimated cost
461 |             return (Math.Abs(goal.Position.X - n.Position.X) + Math.Abs(goal.Position.Y - n.Position.Y)) * 10;
462 |         }
463 |     }
464 | }
465 | 


--------------------------------------------------------------------------------
/NF.AI.PathFinding/src/NF.AI.PathFinding.JPS/JPS.cs:
--------------------------------------------------------------------------------
  1 | using NF.AI.PathFinding.Common;
  2 | using NF.Collections.Generic;
  3 | using NF.Mathematics;
  4 | 
  5 | using System;
  6 | using System.Collections.Generic;
  7 | using System.Runtime.CompilerServices;
  8 | 
  9 | namespace NF.AI.PathFinding.JPS
 10 | {
 11 |     public class JPS
 12 |     {
 13 |         // =======================
 14 |         // Members
 15 |         // =======================
 16 |         private AStarNode mStart = null;
 17 |         private AStarNode mGoal = null;
 18 |         private readonly Dictionary<Int2, AStarNode> mCreatedNodes = new Dictionary<Int2, AStarNode>();
 19 |         private readonly PriorityQueue<(AStarNode Node, EDirFlags Dir)> mOpenList = new PriorityQueue<(AStarNode Node, EDirFlags Dir)>();
 20 |         private readonly HashSet<AStarNode> mCloseList = new HashSet<AStarNode>();
 21 |         private bool[,] mWalls = null;
 22 | 
 23 |         public int Width { get; private set; }
 24 |         public int Height { get; private set; }
 25 |         public Int2 StartP => mStart.Position;
 26 |         public Int2 GoalP => mGoal.Position;
 27 | 
 28 |         public JPS()
 29 |         {
 30 | 
 31 |         }
 32 | 
 33 |         public JPS(int width, int height)
 34 |         {
 35 |             Init(new bool[height, width]);
 36 |         }
 37 |         public JPS(bool[,] walls)
 38 |         {
 39 |             Init(walls);
 40 |         }
 41 | 
 42 |         public void Init(bool[,] walls)
 43 |         {
 44 |             mWalls = walls;
 45 |             Width = walls.GetLength(1);
 46 |             Height = walls.GetLength(0);
 47 |         }
 48 | 
 49 |         public bool StepAll(int stepCount = int.MaxValue)
 50 |         {
 51 |             mOpenList.Clear();
 52 |             mCloseList.Clear();
 53 |             foreach (AStarNode node in mCreatedNodes.Values)
 54 |             {
 55 |                 node.Refresh();
 56 |             }
 57 |             mOpenList.Enqueue((mStart, EDirFlags.ALL), mStart.F);
 58 |             return Step(stepCount);
 59 |         }
 60 | 
 61 |         public bool Step(int stepCount)
 62 |         {
 63 |             int step = stepCount;
 64 |             Int2 jumpPos = new Int2(0, 0);
 65 | 
 66 |             while (true)
 67 |             {
 68 |                 if (step <= 0)
 69 |                 {
 70 |                     return false;
 71 |                 }
 72 |                 if (mOpenList.Count == 0)
 73 |                 {
 74 |                     return false;
 75 |                 }
 76 |                 (AStarNode Node, EDirFlags Dir) = mOpenList.Dequeue();
 77 |                 AStarNode currNode = Node;
 78 |                 EDirFlags currDir = Dir;
 79 |                 _ = mCloseList.Add(currNode);
 80 | 
 81 |                 Int2 currPos = currNode.Position;
 82 |                 Int2 goalPos = mGoal.Position;
 83 |                 if (currPos == goalPos)
 84 |                 {
 85 |                     return true;
 86 |                 }
 87 | 
 88 |                 EDirFlags succesors = SuccesorsDir(currPos, currDir);
 89 |                 for (int i = 0b10000000; i > 0; i >>= 1)
 90 |                 {
 91 |                     EDirFlags succesorDir = (EDirFlags)i;
 92 |                     if ((succesorDir & succesors) == EDirFlags.NONE)
 93 |                     {
 94 |                         continue;
 95 |                     }
 96 | 
 97 |                     if (!TryJump(currPos, succesorDir, goalPos, ref jumpPos))
 98 |                     {
 99 |                         continue;
100 |                     }
101 | 
102 |                     AStarNode jumpNode = GetOrCreateNode(jumpPos);
103 |                     if (mCloseList.Contains(jumpNode))
104 |                     {
105 |                         continue;
106 |                     }
107 | 
108 |                     int jumpG = G(currNode, jumpNode);
109 |                     (AStarNode, EDirFlags) openJump = (jumpNode, succesorDir);
110 |                     if (!mOpenList.Contains(openJump))
111 |                     {
112 |                         jumpNode.Parent = currNode;
113 |                         jumpNode.G = jumpG;
114 |                         jumpNode.H = H(jumpNode, mGoal);
115 |                         mOpenList.Enqueue(openJump, jumpNode.F);
116 |                     }
117 |                     else if (jumpG < jumpNode.G)
118 |                     {
119 |                         jumpNode.Parent = currNode;
120 |                         jumpNode.G = jumpG;
121 |                         jumpNode.H = H(jumpNode, mGoal);
122 |                         mOpenList.UpdatePriority(openJump, jumpNode.F);
123 |                     }
124 |                 }
125 |                 step--;
126 |             }
127 |         }
128 |         public void SetStart(in Int2 p)
129 |         {
130 |             if (!IsInBoundary(p))
131 |             {
132 |                 return;
133 |             }
134 |             mStart = GetOrCreateNode(p);
135 |         }
136 | 
137 |         public void SetGoal(in Int2 p)
138 |         {
139 |             if (!IsInBoundary(p))
140 |             {
141 |                 return;
142 |             }
143 |             mGoal = GetOrCreateNode(p);
144 |         }
145 | 
146 |         public void SetWall(in Int2 p, bool isWall)
147 |         {
148 |             if (!IsInBoundary(p))
149 |             {
150 |                 return;
151 |             }
152 |             mWalls[p.Y, p.X] = isWall;
153 |         }
154 | 
155 |         public void ToggleWall(in Int2 p)
156 |         {
157 |             if (!IsInBoundary(p))
158 |             {
159 |                 return;
160 |             }
161 |             mWalls[p.Y, p.X] = !mWalls[p.Y, p.X];
162 |         }
163 | 
164 |         public IReadOnlyList<AStarNode> GetPaths()
165 |         {
166 |             List<AStarNode> ret = new List<AStarNode>();
167 |             AStarNode n = mGoal;
168 |             while (n != null)
169 |             {
170 |                 ret.Add(n);
171 |                 n = n.Parent;
172 |             }
173 |             ret.Reverse();
174 |             return ret;
175 |         }
176 | 
177 |         [MethodImpl(MethodImplOptions.AggressiveInlining)]
178 |         public AStarNode GetStart()
179 |         {
180 |             return mStart;
181 |         }
182 | 
183 |         [MethodImpl(MethodImplOptions.AggressiveInlining)]
184 |         public AStarNode GetGoal()
185 |         {
186 |             return mGoal;
187 |         }
188 | 
189 |         [MethodImpl(MethodImplOptions.AggressiveInlining)]
190 |         public PriorityQueue<(AStarNode, EDirFlags)> GetOpenList()
191 |         {
192 |             return mOpenList;
193 |         }
194 | 
195 |         [MethodImpl(MethodImplOptions.AggressiveInlining)]
196 |         public HashSet<AStarNode> GetCloseList()
197 |         {
198 |             return mCloseList;
199 |         }
200 | 
201 |         [MethodImpl(MethodImplOptions.AggressiveInlining)]
202 |         public bool[,] GetWalls()
203 |         {
204 |             return mWalls;
205 |         }
206 | 
207 |         public bool IsWalkable(in Int2 p)
208 |         {
209 |             if (!IsInBoundary(p))
210 |             {
211 |                 return false;
212 |             }
213 |             return !mWalls[p.Y, p.X];
214 |         }
215 | 
216 |         // =======================
217 |         // Private Methods
218 |         // =======================
219 |         private AStarNode GetOrCreateNode(in Int2 p)
220 |         {
221 |             if (mCreatedNodes.TryGetValue(p, out AStarNode createdNode))
222 |             {
223 |                 return createdNode;
224 |             }
225 |             AStarNode newNode = new AStarNode(p);
226 |             mCreatedNodes.Add(p, newNode);
227 |             return newNode;
228 |         }
229 | 
230 |         [MethodImpl(MethodImplOptions.AggressiveInlining)]
231 |         private bool IsInBoundary(in Int2 p)
232 |         {
233 |             return 0 <= p.X && p.X < Width && 0 <= p.Y && p.Y < Height;
234 |         }
235 | 
236 |         internal EDirFlags NeighbourDir(in Int2 pos)
237 |         {
238 |             EDirFlags ret = EDirFlags.NONE;
239 |             for (int i = 0b10000000; i > 0; i >>= 1)
240 |             {
241 |                 EDirFlags dir = (EDirFlags)i;
242 |                 if (!IsWalkable(pos.Foward(dir)))
243 |                 {
244 |                     continue;
245 |                 }
246 | 
247 |                 if (DirFlags.IsDiagonal(dir))
248 |                 {
249 |                     Int2 dp = DirFlags.ToPos(dir);
250 |                     if (!IsWalkable(new Int2(pos.X + dp.X, pos.Y)) &&
251 |                         !IsWalkable(new Int2(pos.X, pos.Y + dp.Y)))
252 |                     {
253 |                         continue;
254 |                     }
255 |                 }
256 |                 ret |= dir;
257 |             }
258 |             return ret;
259 |         }
260 | 
261 |         internal EDirFlags ForcedNeighbourDir(in Int2 n, EDirFlags dir)
262 |         {
263 |             EDirFlags ret = EDirFlags.NONE;
264 |             switch (dir)
265 |             {
266 |                 case EDirFlags.NORTH:
267 |                     // F . F
268 |                     // X N X
269 |                     // . P .
270 |                     if (!IsWalkable(n.Foward(EDirFlags.EAST)))
271 |                     {
272 |                         ret |= EDirFlags.NORTHEAST;
273 |                     }
274 |                     if (!IsWalkable(n.Foward(EDirFlags.WEST)))
275 |                     {
276 |                         ret |= EDirFlags.NORTHWEST;
277 |                     }
278 |                     break;
279 |                 case EDirFlags.SOUTH:
280 |                     // . P .
281 |                     // X N X
282 |                     // F . F
283 |                     if (!IsWalkable(n.Foward(EDirFlags.EAST)))
284 |                     {
285 |                         ret |= EDirFlags.SOUTHEAST;
286 |                     }
287 |                     if (!IsWalkable(n.Foward(EDirFlags.WEST)))
288 |                     {
289 |                         ret |= EDirFlags.SOUTHWEST;
290 |                     }
291 |                     break;
292 |                 case EDirFlags.EAST:
293 |                     // . X F
294 |                     // P N .
295 |                     // . X F
296 |                     if (!IsWalkable(n.Foward(EDirFlags.NORTH)))
297 |                     {
298 |                         ret |= EDirFlags.NORTHEAST;
299 |                     }
300 |                     if (!IsWalkable(n.Foward(EDirFlags.SOUTH)))
301 |                     {
302 |                         ret |= EDirFlags.SOUTHEAST;
303 |                     }
304 |                     break;
305 |                 case EDirFlags.WEST:
306 |                     // F X .
307 |                     // . N P
308 |                     // F X .
309 |                     if (!IsWalkable(n.Foward(EDirFlags.NORTH)))
310 |                     {
311 |                         ret |= EDirFlags.NORTHWEST;
312 |                     }
313 |                     if (!IsWalkable(n.Foward(EDirFlags.SOUTH)))
314 |                     {
315 |                         ret |= EDirFlags.SOUTHWEST;
316 |                     }
317 |                     break;
318 |                 case EDirFlags.NORTHWEST:
319 |                     // . . F
320 |                     // . N X
321 |                     // F X P
322 |                     if (IsWalkable(n.Foward(EDirFlags.NORTH)) && !IsWalkable(n.Foward(EDirFlags.EAST)))
323 |                     {
324 |                         ret |= EDirFlags.NORTHEAST;
325 |                     }
326 |                     if (!IsWalkable(n.Foward(EDirFlags.SOUTH)) && IsWalkable(n.Foward(EDirFlags.WEST)))
327 |                     {
328 |                         ret |= EDirFlags.SOUTHWEST;
329 |                     }
330 |                     break;
331 |                 case EDirFlags.NORTHEAST:
332 |                     // F . .
333 |                     // X N .
334 |                     // P X F
335 |                     if (IsWalkable(n.Foward(EDirFlags.NORTH)) && !IsWalkable(n.Foward(EDirFlags.WEST)))
336 |                     {
337 |                         ret |= EDirFlags.NORTHWEST;
338 |                     }
339 |                     if (!IsWalkable(n.Foward(EDirFlags.SOUTH)) && IsWalkable(n.Foward(EDirFlags.EAST)))
340 |                     {
341 |                         ret |= EDirFlags.SOUTHEAST;
342 |                     }
343 |                     break;
344 |                 case EDirFlags.SOUTHWEST:
345 |                     // F X P
346 |                     // . N X
347 |                     // . . F
348 |                     if (IsWalkable(n.Foward(EDirFlags.SOUTH)) && !IsWalkable(n.Foward(EDirFlags.EAST)))
349 |                     {
350 |                         ret |= EDirFlags.SOUTHEAST;
351 |                     }
352 |                     if (!IsWalkable(n.Foward(EDirFlags.NORTH)) && IsWalkable(n.Foward(EDirFlags.WEST)))
353 |                     {
354 |                         ret |= EDirFlags.NORTHWEST;
355 |                     }
356 |                     break;
357 |                 case EDirFlags.SOUTHEAST:
358 |                     // P X F
359 |                     // X N .
360 |                     // F . .
361 |                     if (IsWalkable(n.Foward(EDirFlags.SOUTH)) && !IsWalkable(n.Foward(EDirFlags.WEST)))
362 |                     {
363 |                         ret |= EDirFlags.SOUTHWEST;
364 |                     }
365 |                     if (!IsWalkable(n.Foward(EDirFlags.NORTH)) && IsWalkable(n.Foward(EDirFlags.EAST)))
366 |                     {
367 |                         ret |= EDirFlags.NORTHEAST;
368 |                     }
369 |                     break;
370 |                 case EDirFlags.ALL:
371 |                     ret |= EDirFlags.ALL;
372 |                     break;
373 |                 case EDirFlags.NONE:
374 |                 default:
375 |                     throw new ArgumentOutOfRangeException($"[ForcedNeighbourDir] invalid dir - {dir}");
376 |             }
377 |             return ret;
378 |         }
379 | 
380 |         [MethodImpl(MethodImplOptions.AggressiveInlining)]
381 |         internal EDirFlags SuccesorsDir(in Int2 pos, EDirFlags dir)
382 |         {
383 |             return NeighbourDir(pos) & (NaturalNeighbours(dir) | ForcedNeighbourDir(pos, dir));
384 |         }
385 | 
386 |         internal bool TryJump(in Int2 p, EDirFlags dir, in Int2 goal, ref Int2 outJumped)
387 |         {
388 |             if (DirFlags.IsStraight(dir))
389 |             {
390 |                 return TryJumpStraight(p, dir, goal, ref outJumped);
391 |             }
392 |             else
393 |             {
394 |                 return TryJumpDiagonal(p, dir, goal, ref outJumped);
395 |             }
396 |         }
397 | 
398 |         internal bool TryJumpStraight(in Int2 p, EDirFlags dir, in Int2 goal, ref Int2 outJumped)
399 |         {
400 |             Int2 curr = p;
401 |             Int2 next = curr.Foward(dir);
402 | 
403 |             while (true)
404 |             {
405 |                 if (!IsWalkable(next))
406 |                 {
407 |                     return false;
408 |                 }
409 | 
410 |                 if ((dir & NeighbourDir(curr)) == EDirFlags.NONE)
411 |                 {
412 |                     return false;
413 |                 }
414 | 
415 |                 if (next == goal)
416 |                 {
417 |                     outJumped = next;
418 |                     return true;
419 |                 }
420 | 
421 |                 if ((ForcedNeighbourDir(next, dir) & NeighbourDir(next)) != EDirFlags.NONE)
422 |                 {
423 |                     outJumped = next;
424 |                     return true;
425 |                 }
426 | 
427 |                 curr = next;
428 |                 next = curr.Foward(dir);
429 |             }
430 |         }
431 | 
432 |         internal bool TryJumpDiagonal(in Int2 p, EDirFlags dir, in Int2 goal, ref Int2 outJumped)
433 |         {
434 |             Int2 curr = p;
435 |             Int2 next = curr.Foward(dir);
436 | 
437 |             while (true)
438 |             {
439 |                 if (!IsWalkable(next))
440 |                 {
441 |                     return false;
442 |                 }
443 | 
444 |                 if ((dir & NeighbourDir(curr)) == EDirFlags.NONE)
445 |                 {
446 |                     return false;
447 |                 }
448 | 
449 |                 if (next == goal)
450 |                 {
451 |                     outJumped = next;
452 |                     return true;
453 |                 }
454 | 
455 |                 if ((ForcedNeighbourDir(next, dir) & NeighbourDir(next)) != EDirFlags.NONE)
456 |                 {
457 |                     outJumped = next;
458 |                     return true;
459 |                 }
460 | 
461 |                 // d1: EAST  | WEST
462 |                 if (TryJumpStraight(next, DirFlags.DiagonalToEastWest(dir), goal, ref outJumped))
463 |                 {
464 |                     outJumped = next;
465 |                     return true;
466 |                 }
467 | 
468 |                 // d2: NORTH | SOUTH
469 |                 if (TryJumpStraight(next, DirFlags.DiagonalToNorthSouth(dir), goal, ref outJumped))
470 |                 {
471 |                     outJumped = next;
472 |                     return true;
473 |                 }
474 | 
475 |                 curr = next;
476 |                 next = curr.Foward(dir);
477 |             }
478 |         }
479 | 
480 |         // =========================================
481 |         // Statics
482 |         // =========================================
483 |         internal static EDirFlags NaturalNeighbours(EDirFlags dir)
484 |         {
485 | 
486 |             switch (dir)
487 |             {
488 |                 case EDirFlags.NORTHEAST:
489 |                     return EDirFlags.NORTHEAST | EDirFlags.NORTH | EDirFlags.EAST;
490 |                 case EDirFlags.NORTHWEST:
491 |                     return EDirFlags.NORTHWEST | EDirFlags.NORTH | EDirFlags.WEST;
492 |                 case EDirFlags.SOUTHEAST:
493 |                     return EDirFlags.SOUTHEAST | EDirFlags.SOUTH | EDirFlags.EAST;
494 |                 case EDirFlags.SOUTHWEST:
495 |                     return EDirFlags.SOUTHWEST | EDirFlags.SOUTH | EDirFlags.WEST;
496 |                 default:
497 |                     return dir;
498 |             }
499 |         }
500 | 
501 |         [MethodImpl(MethodImplOptions.AggressiveInlining)]
502 |         private static int G(AStarNode from, AStarNode adjacent)
503 |         {
504 |             // cost so far to reach n 
505 |             Int2 p = from.Position - adjacent.Position;
506 |             if (p.X == 0 || p.Y == 0)
507 |             {
508 |                 return from.G + (Math.Max(Math.Abs(p.X), Math.Abs(p.Y)) * 10);
509 |             }
510 |             else
511 |             {
512 |                 return from.G + (Math.Max(Math.Abs(p.X), Math.Abs(p.Y)) * 14);
513 |             }
514 |         }
515 | 
516 |         [MethodImpl(MethodImplOptions.AggressiveInlining)]
517 |         internal static int H(AStarNode n, AStarNode goal)
518 |         {
519 |             // calculate estimated cost
520 |             return (Math.Abs(goal.Position.X - n.Position.X) + Math.Abs(goal.Position.Y - n.Position.Y)) * 10;
521 |         }
522 |     }
523 | }
524 | 


--------------------------------------------------------------------------------
/NF.AI.PathFinding/src/NF.AI.PathFinding.JPSPlus/JPSPlusMapBaker.cs:
--------------------------------------------------------------------------------
  1 | using NF.AI.PathFinding.Common;
  2 | using NF.Mathematics;
  3 | 
  4 | using System;
  5 | using System.Collections.Generic;
  6 | using System.Linq;
  7 | 
  8 | namespace NF.AI.PathFinding.JPSPlus
  9 | {
 10 |     public class JPSPlusMapBaker
 11 |     {
 12 |         public int Width { get; private set; }
 13 |         public int Height { get; private set; }
 14 |         public int[,] BlockLUT;
 15 |         public JPSPlusMapBakerBlock[] Blocks;
 16 | 
 17 |         #region Public
 18 |         public JPSPlusMapBaker()
 19 |         {
 20 | 
 21 |         }
 22 |         public JPSPlusMapBaker(bool[,] walls)
 23 |         {
 24 |             Init(walls);
 25 |         }
 26 | 
 27 |         public void Init(bool[,] walls)
 28 |         {
 29 |             Width = walls.GetLength(1);
 30 |             Height = walls.GetLength(0);
 31 |             BlockLUT = new int[Height, Width];
 32 | 
 33 |             List<JPSPlusMapBakerBlock> blocks = new List<JPSPlusMapBakerBlock>();
 34 |             int index = 0;
 35 |             for (int y = 0; y < Height; ++y)
 36 |             {
 37 |                 for (int x = 0; x < Width; ++x)
 38 |                 {
 39 |                     if (walls[y, x])
 40 |                     {
 41 |                         BlockLUT[y, x] = -1;
 42 |                     }
 43 |                     else
 44 |                     {
 45 |                         BlockLUT[y, x] = index;
 46 |                         blocks.Add(new JPSPlusMapBakerBlock(new Int2(x, y)));
 47 |                         index++;
 48 |                     }
 49 |                 }
 50 |             }
 51 |             Blocks = blocks.ToArray();
 52 |         }
 53 | 
 54 |         public JPSPlusBakedMap Bake()
 55 |         {
 56 |             MarkPrimary();
 57 |             MarkStraight();
 58 |             MarkDiagonal();
 59 |             return new JPSPlusBakedMap(
 60 |                 BlockLUT,
 61 |                 Blocks.Select(x =>
 62 |                 {
 63 |                     return new JPSPlusBakedMap.JPSPlusBakedMapBlock(x.Pos, x.JumpDistances);
 64 |                 }).ToArray());
 65 |         }
 66 |         #endregion Public
 67 | 
 68 |         private void MarkPrimary()
 69 |         {
 70 |             for (int y = 0; y < Height; ++y)
 71 |             {
 72 |                 for (int x = 0; x < Width; ++x)
 73 |                 {
 74 |                     Int2 p = new Int2(x, y);
 75 | 
 76 |                     if (IsWalkable(p))
 77 |                     {
 78 |                         continue;
 79 |                     }
 80 | 
 81 |                     for (int d = 0b10000000; d > 0b00001111; d >>= 1)
 82 |                     {
 83 |                         EDirFlags dir = (EDirFlags)d;
 84 |                         Int2 primaryP = p.Foward(dir);
 85 |                         JPSPlusMapBakerBlock primaryB = GetBlockOrNull(primaryP);
 86 |                         if (primaryB == null)
 87 |                         {
 88 |                             continue;
 89 |                         }
 90 | 
 91 |                         switch (dir)
 92 |                         {
 93 |                             case EDirFlags.NORTHEAST:
 94 |                             {
 95 |                                 Int2 p1 = p.Foward(EDirFlags.NORTH);
 96 |                                 Int2 p2 = p.Foward(EDirFlags.EAST);
 97 |                                 if (IsWalkable(p1) && IsWalkable(p2))
 98 |                                 {
 99 |                                     primaryB.JumpDirFlags |= EDirFlags.SOUTH | EDirFlags.WEST;
100 |                                 }
101 |                                 break;
102 |                             }
103 |                             case EDirFlags.SOUTHEAST:
104 |                             {
105 |                                 Int2 p1 = p.Foward(EDirFlags.SOUTH);
106 |                                 Int2 p2 = p.Foward(EDirFlags.EAST);
107 |                                 if (IsWalkable(p1) && IsWalkable(p2))
108 |                                 {
109 |                                     primaryB.JumpDirFlags |= EDirFlags.NORTH | EDirFlags.WEST;
110 |                                 }
111 |                                 break;
112 |                             }
113 |                             case EDirFlags.NORTHWEST:
114 |                             {
115 |                                 Int2 p1 = p.Foward(EDirFlags.NORTH);
116 |                                 Int2 p2 = p.Foward(EDirFlags.WEST);
117 |                                 if (IsWalkable(p1) && IsWalkable(p2))
118 |                                 {
119 |                                     primaryB.JumpDirFlags |= EDirFlags.SOUTH | EDirFlags.EAST;
120 |                                 }
121 |                                 break;
122 |                             }
123 |                             case EDirFlags.SOUTHWEST:
124 |                             {
125 |                                 Int2 p1 = p.Foward(EDirFlags.SOUTH);
126 |                                 Int2 p2 = p.Foward(EDirFlags.WEST);
127 |                                 if (IsWalkable(p1) && IsWalkable(p2))
128 |                                 {
129 |                                     primaryB.JumpDirFlags |= EDirFlags.NORTH | EDirFlags.EAST;
130 |                                 }
131 |                                 break;
132 |                             }
133 |                             default:
134 |                                 throw new ArgumentException();
135 |                         }
136 |                     }
137 |                 }
138 |             }
139 |         }
140 | 
141 |         private void MarkStraight()
142 |         {
143 |             // . . .
144 |             // W . .
145 |             // . . .
146 |             for (int y = 0; y < Height; ++y)
147 |             { // WEST
148 |                 bool isJumpPointLastSeen = false;
149 |                 int distance = -1;
150 |                 for (int x = 0; x < Width; ++x)
151 |                 {
152 |                     Int2 p = new Int2(x, y);
153 |                     JPSPlusMapBakerBlock block = GetBlockOrNull(p);
154 |                     if (block == null)
155 |                     {
156 |                         distance = -1;
157 |                         isJumpPointLastSeen = false;
158 |                         continue;
159 |                     }
160 | 
161 |                     distance++;
162 |                     if (isJumpPointLastSeen)
163 |                     {
164 |                         block.SetDistance(EDirFlags.WEST, distance); // Straight Distance
165 |                     }
166 |                     else
167 |                     {
168 |                         block.SetDistance(EDirFlags.WEST, -distance); // Straight-Wall Distance
169 |                     }
170 | 
171 |                     if (block.IsJumpable(EDirFlags.EAST))
172 |                     {
173 |                         distance = 0;
174 |                         isJumpPointLastSeen = true;
175 |                     }
176 |                 }
177 |             } // WEST
178 | 
179 |             // . . .
180 |             // . . E
181 |             // . . .
182 |             for (int y = 0; y < Height; ++y)
183 |             { // EAST
184 |                 bool isJumpPointLastSeen = false;
185 |                 int distance = -1;
186 |                 for (int x = Width - 1; x >= 0; --x)
187 |                 {
188 |                     Int2 p = new Int2(x, y);
189 |                     JPSPlusMapBakerBlock block = GetBlockOrNull(p);
190 |                     if (block == null)
191 |                     {
192 |                         distance = -1;
193 |                         isJumpPointLastSeen = false;
194 |                         continue;
195 |                     }
196 | 
197 |                     distance++;
198 |                     if (isJumpPointLastSeen)
199 |                     {
200 |                         block.SetDistance(EDirFlags.EAST, distance); // Straight Distance
201 |                     }
202 |                     else
203 |                     {
204 |                         block.SetDistance(EDirFlags.EAST, -distance); // Straight-Wall Distance
205 |                     }
206 | 
207 |                     if (block.IsJumpable(EDirFlags.WEST))
208 |                     {
209 |                         distance = 0;
210 |                         isJumpPointLastSeen = true;
211 |                     }
212 |                 }
213 |             } // EAST
214 | 
215 |             // . . .
216 |             // . . .
217 |             // . S .
218 |             for (int x = 0; x < Width; ++x)
219 |             { // SOUTH
220 |                 bool isJumpPointLastSeen = false;
221 |                 int distance = -1;
222 |                 for (int y = Height - 1; y >= 0; --y)
223 |                 {
224 |                     Int2 p = new Int2(x, y);
225 |                     JPSPlusMapBakerBlock block = GetBlockOrNull(p);
226 |                     if (block == null)
227 |                     {
228 |                         distance = -1;
229 |                         isJumpPointLastSeen = false;
230 |                         continue;
231 |                     }
232 | 
233 |                     distance++;
234 |                     if (isJumpPointLastSeen)
235 |                     {
236 |                         block.SetDistance(EDirFlags.SOUTH, distance); // Straight Distance
237 |                     }
238 |                     else
239 |                     {
240 |                         block.SetDistance(EDirFlags.SOUTH, -distance); // Straight-Wall Distance
241 |                     }
242 | 
243 |                     if (block.IsJumpable(EDirFlags.NORTH))
244 |                     {
245 |                         distance = 0;
246 |                         isJumpPointLastSeen = true;
247 |                     }
248 |                 }
249 |             } // SOUTH
250 | 
251 |             // . N .
252 |             // . . .
253 |             // . . .
254 |             for (int x = 0; x < Width; ++x)
255 |             { // NORTH
256 |                 bool isJumpPointLastSeen = false;
257 |                 int distance = -1;
258 | 
259 |                 for (int y = 0; y < Height; ++y)
260 |                 {
261 |                     Int2 p = new Int2(x, y);
262 |                     JPSPlusMapBakerBlock block = GetBlockOrNull(p);
263 |                     if (block == null)
264 |                     {
265 |                         distance = -1;
266 |                         isJumpPointLastSeen = false;
267 |                         continue;
268 |                     }
269 | 
270 |                     distance++;
271 |                     if (isJumpPointLastSeen)
272 |                     {
273 |                         block.SetDistance(EDirFlags.NORTH, distance); // Straight Distance
274 |                     }
275 |                     else
276 |                     {
277 |                         block.SetDistance(EDirFlags.NORTH, -distance); // Straight-Wall Distance
278 |                     }
279 | 
280 |                     if (block.IsJumpable(EDirFlags.SOUTH))
281 |                     {
282 |                         distance = 0;
283 |                         isJumpPointLastSeen = true;
284 |                     }
285 |                 }
286 |             } // NORTH
287 |         }
288 | 
289 |         private void MarkDiagonal()
290 |         {
291 |             // * N .
292 |             // W . .
293 |             // . . .
294 |             for (int y = 0; y < Height; ++y)
295 |             { // NORTH & WEST
296 |                 for (int x = 0; x < Width; ++x)
297 |                 {
298 |                     Int2 p = new Int2(x, y);
299 |                     JPSPlusMapBakerBlock block = GetBlockOrNull(p);
300 |                     if (block == null)
301 |                     {
302 |                         continue;
303 |                     }
304 | 
305 |                     if (x == 0 || y == 0)
306 |                     {
307 |                         block.SetDistance(EDirFlags.NORTHWEST, 0); // Diagonal-Wall Distance
308 |                         continue;
309 |                     }
310 | 
311 |                     Int2 p1 = p.Foward(EDirFlags.NORTH);
312 |                     Int2 p2 = p.Foward(EDirFlags.NORTHWEST);
313 |                     Int2 p3 = p.Foward(EDirFlags.WEST);
314 |                     bool p1Walkable = IsWalkable(p1);
315 |                     bool p3Walkable = IsWalkable(p3);
316 | 
317 |                     if (!p1Walkable || !IsWalkable(p2) || !p3Walkable)
318 |                     {
319 |                         block.SetDistance(EDirFlags.NORTHWEST, 0); // Diagonal-Wall Distance
320 |                         continue;
321 |                     }
322 | 
323 |                     JPSPlusMapBakerBlock prevBlock = GetBlockOrNull(p2);
324 |                     if (p1Walkable && p3Walkable
325 |                         && (prevBlock.GetDistance(EDirFlags.NORTH) > 0 || prevBlock.GetDistance(EDirFlags.WEST) > 0))
326 |                     {
327 |                         block.SetDistance(EDirFlags.NORTHWEST, 1); // Initial Diagonal Distance
328 |                         continue;
329 |                     }
330 | 
331 |                     int distanceFromPrev = prevBlock.GetDistance(EDirFlags.NORTHWEST);
332 |                     if (distanceFromPrev > 0)
333 |                     {
334 |                         block.SetDistance(EDirFlags.NORTHWEST, distanceFromPrev + 1); // Diagonal Distance
335 |                     }
336 |                     else
337 |                     {
338 |                         block.SetDistance(EDirFlags.NORTHWEST, distanceFromPrev - 1); // Diagonal-Wall Distance
339 |                     }
340 |                 }
341 |             } // NORTH & WEST
342 | 
343 |             // . N *
344 |             // . . E
345 |             // . . .
346 |             for (int y = 0; y < Height; ++y)
347 |             { // NORTH & EAST
348 |                 for (int x = Width - 1; x >= 0; --x)
349 |                 {
350 |                     Int2 p = new Int2(x, y);
351 |                     JPSPlusMapBakerBlock block = GetBlockOrNull(p);
352 |                     if (block == null)
353 |                     {
354 |                         continue;
355 |                     }
356 | 
357 |                     if (x == Width - 1 || y == 0)
358 |                     {
359 |                         block.SetDistance(EDirFlags.NORTHEAST, 0); // Diagonal-Wall Distance
360 |                         continue;
361 |                     }
362 | 
363 |                     Int2 p1 = p.Foward(EDirFlags.NORTH);
364 |                     Int2 p2 = p.Foward(EDirFlags.NORTHEAST);
365 |                     Int2 p3 = p.Foward(EDirFlags.EAST);
366 |                     bool p1Walkable = IsWalkable(p1);
367 |                     bool p3Walkable = IsWalkable(p3);
368 | 
369 |                     if (!p1Walkable || !IsWalkable(p2) || !p3Walkable)
370 |                     {
371 |                         block.SetDistance(EDirFlags.NORTHEAST, 0); // Diagonal-Wall Distance
372 |                         continue;
373 |                     }
374 | 
375 |                     JPSPlusMapBakerBlock prevBlock = GetBlockOrNull(p2);
376 |                     if (p1Walkable && p3Walkable
377 |                         && (prevBlock.GetDistance(EDirFlags.NORTH) > 0 || prevBlock.GetDistance(EDirFlags.EAST) > 0))
378 |                     {
379 |                         block.SetDistance(EDirFlags.NORTHEAST, 1); // Initial Diagonal Distance
380 |                         continue;
381 |                     }
382 | 
383 |                     int distanceFromPrev = prevBlock.GetDistance(EDirFlags.NORTHEAST);
384 |                     if (distanceFromPrev > 0)
385 |                     {
386 |                         block.SetDistance(EDirFlags.NORTHEAST, distanceFromPrev + 1); // Diagonal Distance
387 |                     }
388 |                     else
389 |                     {
390 |                         block.SetDistance(EDirFlags.NORTHEAST, distanceFromPrev - 1); // Diagonal-Wall Distance
391 |                     }
392 |                 }
393 |             } // NORTH & EAST
394 | 
395 |             // . . .
396 |             // W . .
397 |             // * S .
398 |             for (int y = Height - 1; y >= 0; --y)
399 |             { // SOUTH & WEST
400 |                 for (int x = 0; x < Width; ++x)
401 |                 {
402 |                     Int2 p = new Int2(x, y);
403 |                     JPSPlusMapBakerBlock block = GetBlockOrNull(p);
404 |                     if (block == null)
405 |                     {
406 |                         continue;
407 |                     }
408 | 
409 |                     if (x == 0 || y == Height - 1)
410 |                     {
411 |                         block.SetDistance(EDirFlags.SOUTHWEST, 0); // Diagonal-Wall Distance
412 |                         continue;
413 |                     }
414 | 
415 |                     Int2 p1 = p.Foward(EDirFlags.SOUTH);
416 |                     Int2 p2 = p.Foward(EDirFlags.SOUTHWEST);
417 |                     Int2 p3 = p.Foward(EDirFlags.WEST);
418 |                     bool p1Walkable = IsWalkable(p1);
419 |                     bool p3Walkable = IsWalkable(p3);
420 | 
421 |                     if (!p1Walkable || !IsWalkable(p2) || !p3Walkable)
422 |                     {
423 |                         block.SetDistance(EDirFlags.SOUTHWEST, 0); // Diagonal-Wall Distance
424 |                         continue;
425 |                     }
426 | 
427 |                     JPSPlusMapBakerBlock prevBlock = GetBlockOrNull(p2);
428 |                     if (p1Walkable && p3Walkable
429 |                         && (prevBlock.GetDistance(EDirFlags.SOUTH) > 0 || prevBlock.GetDistance(EDirFlags.WEST) > 0))
430 |                     {
431 |                         block.SetDistance(EDirFlags.SOUTHWEST, 1); // Initial Diagonal Distance
432 |                         continue;
433 |                     }
434 | 
435 |                     int distanceFromPrev = prevBlock.GetDistance(EDirFlags.SOUTHWEST);
436 |                     if (distanceFromPrev > 0)
437 |                     {
438 |                         block.SetDistance(EDirFlags.SOUTHWEST, distanceFromPrev + 1); // Diagonal Distance
439 |                     }
440 |                     else
441 |                     {
442 |                         block.SetDistance(EDirFlags.SOUTHWEST, distanceFromPrev - 1); // Diagonal-Wall Distance
443 |                     }
444 |                 }
445 |             } // SOUTH & WEST
446 | 
447 |             // . . .
448 |             // . . E
449 |             // . S *
450 |             for (int y = Height - 1; y >= 0; --y)
451 |             { // SOUTH & EAST
452 |                 for (int x = Width - 1; x >= 0; --x)
453 |                 {
454 |                     Int2 p = new Int2(x, y);
455 |                     JPSPlusMapBakerBlock block = GetBlockOrNull(p);
456 |                     if (block == null)
457 |                     {
458 |                         continue;
459 |                     }
460 | 
461 |                     if (x == Width - 1 || y == Height - 1)
462 |                     {
463 |                         block.SetDistance(EDirFlags.SOUTHEAST, 0); // Diagonal-Wall Distance
464 |                         continue;
465 |                     }
466 | 
467 |                     Int2 p1 = p.Foward(EDirFlags.SOUTH);
468 |                     Int2 p2 = p.Foward(EDirFlags.SOUTHEAST);
469 |                     Int2 p3 = p.Foward(EDirFlags.EAST);
470 |                     bool p1Walkable = IsWalkable(p1);
471 |                     bool p3Walkable = IsWalkable(p3);
472 | 
473 |                     if (!p1Walkable || !IsWalkable(p2) || !p3Walkable)
474 |                     {
475 |                         block.SetDistance(EDirFlags.SOUTHEAST, 0); // Diagonal-Wall Distance
476 |                         continue;
477 |                     }
478 | 
479 |                     JPSPlusMapBakerBlock prevBlock = GetBlockOrNull(p2);
480 |                     if (p1Walkable && p3Walkable
481 |                         && (prevBlock.GetDistance(EDirFlags.SOUTH) > 0 || prevBlock.GetDistance(EDirFlags.EAST) > 0))
482 |                     {
483 |                         block.SetDistance(EDirFlags.SOUTHEAST, 1); // Initial Diagonal Distance
484 |                         continue;
485 |                     }
486 | 
487 |                     int distanceFromPrev = prevBlock.GetDistance(EDirFlags.SOUTHEAST);
488 |                     if (distanceFromPrev > 0)
489 |                     {
490 |                         block.SetDistance(EDirFlags.SOUTHEAST, distanceFromPrev + 1); // Diagonal Distance
491 |                     }
492 |                     else
493 |                     {
494 |                         block.SetDistance(EDirFlags.SOUTHEAST, distanceFromPrev - 1); // Diagonal-Wall Distance
495 |                     }
496 |                 }
497 |             } // SOUTH & EAST
498 |         }
499 | 
500 |         private bool IsWalkable(in Int2 p)
501 |         {
502 |             if (!IsInBoundary(p))
503 |             {
504 |                 return false;
505 |             }
506 |             return BlockLUT[p.Y, p.X] >= 0;
507 |         }
508 | 
509 |         private bool IsInBoundary(in Int2 p)
510 |         {
511 |             return 0 <= p.X && p.X < Width && 0 <= p.Y && p.Y < Height;
512 |         }
513 | 
514 |         private JPSPlusMapBakerBlock GetBlockOrNull(in Int2 p)
515 |         {
516 |             if (!IsInBoundary(p))
517 |             {
518 |                 return null;
519 |             }
520 | 
521 |             int index = BlockLUT[p.Y, p.X];
522 |             if (index < 0)
523 |             {
524 |                 return null;
525 |             }
526 |             return Blocks[index];
527 |         }
528 |     }
529 | }
530 | 


--------------------------------------------------------------------------------