├── .gitattributes ├── .gitignore ├── BenchmarkHistory.md ├── LICENSE ├── README.md ├── Roy-T.AStar.Benchmark ├── Benchmarks.cs ├── GridBuilder.cs ├── Program.cs └── Roy-T.AStar.Benchmark.csproj ├── Roy-T.AStar.Tests ├── Collections │ └── MinHeapTests.cs ├── GridSerializationTests.cs ├── PathFinderTests.cs └── Roy-T.AStar.Tests.csproj ├── Roy-T.AStar.Viewer ├── App.xaml ├── App.xaml.cs ├── Connections.cs ├── EdgeSpeedColorConverter.cs ├── GraphDataTemplateSelector.cs ├── MainWindow.xaml ├── MainWindow.xaml.cs ├── MainWindowViewModel.cs ├── Model │ ├── EdgeModel.cs │ ├── ModelBuilder.cs │ ├── NodeModel.cs │ ├── NodeState.cs │ └── PathEdgeModel.cs ├── NodeStateColorConverter.cs ├── Roy-T.AStar.Viewer.csproj └── Settings.cs ├── Roy-T.AStar.sln ├── Roy-T.AStar ├── AssemblyInfo.cs ├── Collections │ └── MinHeap.cs ├── Graphs │ ├── Edge.cs │ ├── IEdge.cs │ ├── INode.cs │ └── Node.cs ├── Grids │ └── Grid.cs ├── Paths │ ├── Path.cs │ ├── PathFinder.cs │ ├── PathFinderNode.cs │ ├── PathReconstructor.cs │ └── PathType.cs ├── Primitives │ ├── Distance.cs │ ├── Duration.cs │ ├── GridPosition.cs │ ├── GridSize.cs │ ├── Position.cs │ ├── Size.cs │ └── Velocity.cs ├── Roy-T.AStar.csproj └── Serialization │ ├── EdgeDto.cs │ ├── GridDto.cs │ ├── GridPositionDto.cs │ ├── GridSerializer.cs │ ├── NodeDto.cs │ ├── PositionDto.cs │ └── VelocityDto.cs └── viewer.png /.gitattributes: -------------------------------------------------------------------------------- 1 | ############################################################################### 2 | # Set default behavior to automatically normalize line endings. 3 | ############################################################################### 4 | * text=auto 5 | 6 | ############################################################################### 7 | # Set default behavior for command prompt diff. 8 | # 9 | # This is need for earlier builds of msysgit that does not have it on by 10 | # default for csharp files. 11 | # Note: This is only used by command line 12 | ############################################################################### 13 | #*.cs diff=csharp 14 | 15 | ############################################################################### 16 | # Set the merge driver for project and solution files 17 | # 18 | # Merging from the command prompt will add diff markers to the files if there 19 | # are conflicts (Merging from VS is not affected by the settings below, in VS 20 | # the diff markers are never inserted). Diff markers may cause the following 21 | # file extensions to fail to load in VS. An alternative would be to treat 22 | # these files as binary and thus will always conflict and require user 23 | # intervention with every merge. To do so, just uncomment the entries below 24 | ############################################################################### 25 | #*.sln merge=binary 26 | #*.csproj merge=binary 27 | #*.vbproj merge=binary 28 | #*.vcxproj merge=binary 29 | #*.vcproj merge=binary 30 | #*.dbproj merge=binary 31 | #*.fsproj merge=binary 32 | #*.lsproj merge=binary 33 | #*.wixproj merge=binary 34 | #*.modelproj merge=binary 35 | #*.sqlproj merge=binary 36 | #*.wwaproj merge=binary 37 | 38 | ############################################################################### 39 | # behavior for image files 40 | # 41 | # image files are treated as binary by default. 42 | ############################################################################### 43 | #*.jpg binary 44 | #*.png binary 45 | #*.gif binary 46 | 47 | ############################################################################### 48 | # diff behavior for common document formats 49 | # 50 | # Convert binary document formats to text before diffing them. This feature 51 | # is only available from the command line. Turn it on by uncommenting the 52 | # entries below. 53 | ############################################################################### 54 | #*.doc diff=astextplain 55 | #*.DOC diff=astextplain 56 | #*.docx diff=astextplain 57 | #*.DOCX diff=astextplain 58 | #*.dot diff=astextplain 59 | #*.DOT diff=astextplain 60 | #*.pdf diff=astextplain 61 | #*.PDF diff=astextplain 62 | #*.rtf diff=astextplain 63 | #*.RTF diff=astextplain 64 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | ## Ignore Visual Studio temporary files, build results, and 2 | ## files generated by popular Visual Studio add-ons. 3 | 4 | # Benchmark .Net files 5 | [Bb]enchmarkDotNet.Artifacts/ 6 | 7 | # User-specific files 8 | *.suo 9 | *.user 10 | *.userosscache 11 | *.sln.docstates 12 | 13 | # User-specific files (MonoDevelop/Xamarin Studio) 14 | *.userprefs 15 | 16 | # Build results 17 | [Dd]ebug/ 18 | [Dd]ebugPublic/ 19 | [Rr]elease/ 20 | [Rr]eleases/ 21 | x64/ 22 | x86/ 23 | bld/ 24 | [Bb]in/ 25 | [Oo]bj/ 26 | [Ll]og/ 27 | 28 | # Visual Studio 2015 cache/options directory 29 | .vs/ 30 | # Uncomment if you have tasks that create the project's static files in wwwroot 31 | #wwwroot/ 32 | 33 | # MSTest test Results 34 | [Tt]est[Rr]esult*/ 35 | [Bb]uild[Ll]og.* 36 | 37 | # NUNIT 38 | *.VisualState.xml 39 | TestResult.xml 40 | 41 | # Build Results of an ATL Project 42 | [Dd]ebugPS/ 43 | [Rr]eleasePS/ 44 | dlldata.c 45 | 46 | # DNX 47 | project.lock.json 48 | project.fragment.lock.json 49 | artifacts/ 50 | 51 | *_i.c 52 | *_p.c 53 | *_i.h 54 | *.ilk 55 | *.meta 56 | *.obj 57 | *.pch 58 | *.pdb 59 | *.pgc 60 | *.pgd 61 | *.rsp 62 | *.sbr 63 | *.tlb 64 | *.tli 65 | *.tlh 66 | *.tmp 67 | *.tmp_proj 68 | *.log 69 | *.vspscc 70 | *.vssscc 71 | .builds 72 | *.pidb 73 | *.svclog 74 | *.scc 75 | 76 | # Chutzpah Test files 77 | _Chutzpah* 78 | 79 | # Visual C++ cache files 80 | ipch/ 81 | *.aps 82 | *.ncb 83 | *.opendb 84 | *.opensdf 85 | *.sdf 86 | *.cachefile 87 | *.VC.db 88 | *.VC.VC.opendb 89 | 90 | # Visual Studio profiler 91 | *.psess 92 | *.vsp 93 | *.vspx 94 | *.sap 95 | 96 | # TFS 2012 Local Workspace 97 | $tf/ 98 | 99 | # Guidance Automation Toolkit 100 | *.gpState 101 | 102 | # ReSharper is a .NET coding add-in 103 | _ReSharper*/ 104 | *.[Rr]e[Ss]harper 105 | *.DotSettings.user 106 | 107 | # JustCode is a .NET coding add-in 108 | .JustCode 109 | 110 | # TeamCity is a build add-in 111 | _TeamCity* 112 | 113 | # DotCover is a Code Coverage Tool 114 | *.dotCover 115 | 116 | # NCrunch 117 | _NCrunch_* 118 | .*crunch*.local.xml 119 | nCrunchTemp_* 120 | 121 | # MightyMoose 122 | *.mm.* 123 | AutoTest.Net/ 124 | 125 | # Web workbench (sass) 126 | .sass-cache/ 127 | 128 | # Installshield output folder 129 | [Ee]xpress/ 130 | 131 | # DocProject is a documentation generator add-in 132 | DocProject/buildhelp/ 133 | DocProject/Help/*.HxT 134 | DocProject/Help/*.HxC 135 | DocProject/Help/*.hhc 136 | DocProject/Help/*.hhk 137 | DocProject/Help/*.hhp 138 | DocProject/Help/Html2 139 | DocProject/Help/html 140 | 141 | # Click-Once directory 142 | publish/ 143 | 144 | # Publish Web Output 145 | *.[Pp]ublish.xml 146 | *.azurePubxml 147 | # TODO: Comment the next line if you want to checkin your web deploy settings 148 | # but database connection strings (with potential passwords) will be unencrypted 149 | #*.pubxml 150 | *.publishproj 151 | 152 | # Microsoft Azure Web App publish settings. Comment the next line if you want to 153 | # checkin your Azure Web App publish settings, but sensitive information contained 154 | # in these scripts will be unencrypted 155 | PublishScripts/ 156 | 157 | # NuGet Packages 158 | *.nupkg 159 | # The packages folder can be ignored because of Package Restore 160 | **/packages/* 161 | # except build/, which is used as an MSBuild target. 162 | !**/packages/build/ 163 | # Uncomment if necessary however generally it will be regenerated when needed 164 | #!**/packages/repositories.config 165 | # NuGet v3's project.json files produces more ignoreable files 166 | *.nuget.props 167 | *.nuget.targets 168 | 169 | # Microsoft Azure Build Output 170 | csx/ 171 | *.build.csdef 172 | 173 | # Microsoft Azure Emulator 174 | ecf/ 175 | rcf/ 176 | 177 | # Windows Store app package directories and files 178 | AppPackages/ 179 | BundleArtifacts/ 180 | Package.StoreAssociation.xml 181 | _pkginfo.txt 182 | 183 | # Visual Studio cache files 184 | # files ending in .cache can be ignored 185 | *.[Cc]ache 186 | # but keep track of directories ending in .cache 187 | !*.[Cc]ache/ 188 | 189 | # Others 190 | ClientBin/ 191 | ~$* 192 | *~ 193 | *.dbmdl 194 | *.dbproj.schemaview 195 | *.jfm 196 | *.pfx 197 | *.publishsettings 198 | node_modules/ 199 | orleans.codegen.cs 200 | 201 | # Since there are multiple workflows, uncomment next line to ignore bower_components 202 | # (https://github.com/github/gitignore/pull/1529#issuecomment-104372622) 203 | #bower_components/ 204 | 205 | # RIA/Silverlight projects 206 | Generated_Code/ 207 | 208 | # Backup & report files from converting an old project file 209 | # to a newer Visual Studio version. Backup files are not needed, 210 | # because we have git ;-) 211 | _UpgradeReport_Files/ 212 | Backup*/ 213 | UpgradeLog*.XML 214 | UpgradeLog*.htm 215 | 216 | # SQL Server files 217 | *.mdf 218 | *.ldf 219 | 220 | # Business Intelligence projects 221 | *.rdl.data 222 | *.bim.layout 223 | *.bim_*.settings 224 | 225 | # Microsoft Fakes 226 | FakesAssemblies/ 227 | 228 | # GhostDoc plugin setting file 229 | *.GhostDoc.xml 230 | 231 | # Node.js Tools for Visual Studio 232 | .ntvs_analysis.dat 233 | 234 | # Visual Studio 6 build log 235 | *.plg 236 | 237 | # Visual Studio 6 workspace options file 238 | *.opt 239 | 240 | # Visual Studio LightSwitch build output 241 | **/*.HTMLClient/GeneratedArtifacts 242 | **/*.DesktopClient/GeneratedArtifacts 243 | **/*.DesktopClient/ModelManifest.xml 244 | **/*.Server/GeneratedArtifacts 245 | **/*.Server/ModelManifest.xml 246 | _Pvt_Extensions 247 | 248 | # Paket dependency manager 249 | .paket/paket.exe 250 | paket-files/ 251 | 252 | # FAKE - F# Make 253 | .fake/ 254 | 255 | # JetBrains Rider 256 | .idea/ 257 | *.sln.iml 258 | 259 | # CodeRush 260 | .cr/ 261 | 262 | # Python Tools for Visual Studio (PTVS) 263 | __pycache__/ 264 | *.pyc -------------------------------------------------------------------------------- /BenchmarkHistory.md: -------------------------------------------------------------------------------- 1 | # Benchmarks overview 2 | For all benchmarks the graph is layed out in a grid like pattern and contains 10,000 nodes and 39,204 outgoing edges. All benchmarks try to find a path from the top-left node to the bottom-right node. 3 | 4 | ## GridBench 5 | All edges have the same traversal velocity. The A* algorithm will guess 100% right all the time. This benchmark is useful because it shows the absolute best case scenario. Note that this benchmark is so fast that the measuring error is usually several times greater than the mean, so it is hard to say how fast it really ran. 6 | 7 | ## GridWithHoleBench 8 | All edges have the same traversal velocity. All nodes, on a diagonal from the top right to the bottom left, have had their incoming edges removed. Except for the node next to the center node of which all edges remain intact. 9 | 10 | This benchmark is designed to see how fast the algorithm can find the node to pass through. It is useful because it shows that the heuristic searches through the best candidate nodes first. This should be a very fast benchmark. 11 | 12 | ## GridWithRandomHoles 13 | All edges have the same traversal velocity, pseudo-randomly 50% of the nodes have been disconnected. 14 | 15 | This benchmark is useful because it shows you how the A* algorithm will behave in a realistic setting, in a sparsely connected graph. The heuristic guides the search, but is not always right. 16 | 17 | ## GridWithRandomLimitsBench 18 | All edges have pseudo random traversal velocities between 80 and 100km/h. This benchmark was designed so that the A* heuristic has more trouble figuring out what the best path is. The general direction will be correct, but there will be a lot of small detours in the best path. 19 | 20 | This benchmark is useful because it shows how you how the A* algorithm will behave in a realistic setting, in a well connected graph. 21 | 22 | ## GridWithUnreachableTargetBench 23 | Disconnects the left part of the graph from the right part of the graph. Forcing the A* algorithm to inspect a lot of edges, before it can conclude that the target is unreachable. 24 | 25 | This benchmark is useful because it shows the worst-case performance of the A* algorithm. Because it has to search through all reachable nodes before it can definitely conclude that the target is unreachable. 26 | 27 | ## GridWithGradientBench 28 | Edges in the top left of the grid have the highest traversal velocity, while edges in the bottom right have the lowest traversal velocity. This means that the A* algorithm continously guesses wrong and has to search through almost the entire grid before finding the answer. This should be considered an adversial/torture benchmarks and an absolute worst case scenario. It is useful because it benchmarks the speed of our algorithm, without the heuristic getting in the way. 29 | 30 | This benchmark is compararable to the `Gradient100X100` benchmark from older versions. 31 | 32 | # Benchmarks 33 | _From newest to oldest_ 34 | 35 | ## 2020-02-05 Remove superseded nodes during search 36 | _git hash `302e2685743700d48d63f08c13df6794aed0e936`_ 37 | BenchmarkDotNet=v0.12.0, OS=Windows 10.0.17763.973 (1809/October2018Update/Redstone5) 38 | Intel Core i9-9900K CPU 3.60GHz (Coffee Lake), 1 CPU, 16 logical and 8 physical cores 39 | .NET Core SDK=3.0.100 40 | - [Host] : .NET Core 3.0.0 (CoreCLR 4.700.19.46205, CoreFX 4.700.19.46214), X64 RyuJIT 41 | - DefaultJob : .NET Core 3.0.0 (CoreCLR 4.700.19.46205, CoreFX 4.700.19.46214), X64 RyuJIT 42 | 43 | | Method | Mean | Error | StdDev | 44 | |------------------------------- |----------------:|-------------:|-------------:| 45 | | GridBench | 94,338.7 ns | 529.27 ns | 469.18 ns | 46 | | GridWithHoleBench | 120.2 ns | 0.88 ns | 0.82 ns | 47 | | GridWithRandomHolesBench | 119,319.2 ns | 474.85 ns | 420.94 ns | 48 | | GridWithRandomLimitsBench | 6,799,357.4 ns | 30,951.38 ns | 28,951.94 ns | 49 | | GridWithUnreachableTargetBench | 4,658,572.3 ns | 29,277.32 ns | 27,386.02 ns | 50 | | GridWithGradientBench | 10,435,749.8 ns | 91,339.85 ns | 85,439.36 ns | 51 | 52 | 53 | ## 2020-02-04 Precalculate travel duration over edges 54 | _git hash `973d6b86f5adaecf22e0db4401ee878817ab1b6c`_ 55 | BenchmarkDotNet=v0.12.0, OS=Windows 10.0.17763.973 (1809/October2018Update/Redstone5) 56 | Intel Core i9-9900K CPU 3.60GHz (Coffee Lake), 1 CPU, 16 logical and 8 physical cores 57 | .NET Core SDK=3.0.100 58 | - [Host] : .NET Core 3.0.0 (CoreCLR 4.700.19.46205, CoreFX 4.700.19.46214), X64 RyuJIT 59 | - DefaultJob : .NET Core 3.0.0 (CoreCLR 4.700.19.46205, CoreFX 4.700.19.46214), X64 RyuJIT 60 | 61 | | Method | Mean | Error | StdDev | 62 | |------------------------------- |----------------:|--------------:|--------------:| 63 | | GridBench | 83,950.3 ns | 283.08 ns | 264.79 ns | 64 | | GridWithHoleBench | 138.4 ns | 1.21 ns | 1.07 ns | 65 | | GridWithRandomHolesBench | 104,597.0 ns | 342.93 ns | 320.77 ns | 66 | | GridWithRandomLimitsBench | 6,582,456.5 ns | 46,309.74 ns | 43,318.16 ns | 67 | | GridWithUnreachableTargetBench | 7,065,479.0 ns | 60,852.18 ns | 50,814.33 ns | 68 | | GridWithGradientBench | 13,863,809.2 ns | 142,322.59 ns | 133,128.63 ns | 69 | 70 | 71 | ## 2020-01-26 Moving from a linked list to a binary min heap 72 | _git hash `a58f52404bb77a5a836768488809eb9c8b6f4ad0`_ 73 | 74 | BenchmarkDotNet=v0.12.0, OS=Windows 10.0.17763.973 (1809/October2018Update/Redstone5) 75 | Intel Core i9-9900K CPU 3.60GHz (Coffee Lake), 1 CPU, 16 logical and 8 physical cores 76 | .NET Core SDK=3.0.100 77 | - [Host] : .NET Core 3.0.0 (CoreCLR 4.700.19.46205, CoreFX 4.700.19.46214), X64 RyuJIT 78 | - DefaultJob : .NET Core 3.0.0 (CoreCLR 4.700.19.46205, CoreFX 4.700.19.46214), X64 RyuJIT 79 | 80 | | Method | Mean | Error | StdDev | 81 | |------------------------------- |----------------:|--------------:|--------------:| 82 | | GridBench | 95,021.7 ns | 350.61 ns | 327.96 ns | 83 | | GridWithHoleBench | 129.7 ns | 0.73 ns | 0.68 ns | 84 | | GridWithRandomHolesBench | 118,499.2 ns | 576.81 ns | 450.33 ns | 85 | | GridWithRandomLimitsBench | 7,289,230.7 ns | 25,773.80 ns | 24,108.83 ns | 86 | | GridWithUnreachableTargetBench | 8,004,423.0 ns | 55,038.19 ns | 51,482.75 ns | 87 | | GridWithGradientBench | 15,575,041.1 ns | 169,958.60 ns | 158,979.38 ns | 88 | 89 | 90 | ## 2020-01-23 Re-implementation using a graph 91 | _git hash `bbadc1325c942b9f2175b4d045cc5254c2cb04e6`_ 92 | 93 | BenchmarkDotNet=v0.12.0, OS=Windows 10.0.17763.973 (1809/October2018Update/Redstone5) 94 | Intel Core i9-9900K CPU 3.60GHz (Coffee Lake), 1 CPU, 16 logical and 8 physical cores 95 | .NET Core SDK=3.0.100 96 | - [Host] : .NET Core 3.0.0 (CoreCLR 4.700.19.46205, CoreFX 4.700.19.46214), X64 RyuJIT 97 | - DefaultJob : .NET Core 3.0.0 (CoreCLR 4.700.19.46205, CoreFX 4.700.19.46214), X64 RyuJIT 98 | 99 | | Method | Mean | Error | StdDev | 100 | |------------------------------- |----------------:|--------------:|--------------:| 101 | | GridBench | 326,209.3 ns | 1,109.91 ns | 1,038.21 ns | 102 | | GridWithHoleBench | 100.7 ns | 0.15 ns | 0.12 ns | 103 | | GridWithRandomHolesBench | 278,367.3 ns | 899.29 ns | 797.20 ns | 104 | | GridWithRandomLimitsBench | 22,880,403.8 ns | 97,026.26 ns | 86,011.25 ns | 105 | | GridWithUnreachableTargetBench | 24,213,165.6 ns | 242,178.42 ns | 226,533.85 ns | 106 | | GridWithGradientBench | 31,464,366.2 ns | 115,321.25 ns | 107,871.57 ns | 107 | 108 | # Benchmarks scores for older versions 109 | _Note: Gradient100X100 is approximately similar to the GridWithGradientBench benchmark in newer versions_ 110 | 111 | ## 2020-01-23 112 | _git hash `eaedfb12d9918977a8a3cde49a461e932f1a4e2b`_ 113 | 114 | BenchmarkDotNet=v0.12.0, OS=Windows 10.0.17763.973 (1809/October2018Update/Redstone5) 115 | Intel Core i9-9900K CPU 3.60GHz (Coffee Lake), 1 CPU, 16 logical and 8 physical cores 116 | .NET Core SDK=3.0.100 117 | - [Host] : .NET Core 3.0.0 (CoreCLR 4.700.19.46205, CoreFX 4.700.19.46214), X64 RyuJIT 118 | - DefaultJob : .NET Core 3.0.0 (CoreCLR 4.700.19.46205, CoreFX 4.700.19.46214), X64 RyuJIT 119 | 120 | | Method | Mean | Error | StdDev | 121 | |---------------- |---------:|---------:|---------:| 122 | | Gradient100X100 | 21.51 ms | 0.128 ms | 0.119 ms | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2017 R. A. Triesscheijn 2 | 3 | Permission is hereby granted, free of charge, to any person obtaining a copy 4 | of this software and associated documentation files (the "Software"), to deal 5 | in the Software without restriction, including without limitation the rights 6 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | copies of the Software, and to permit persons to whom the Software is 8 | furnished to do so, subject to the following conditions: 9 | 10 | The above copyright notice and this permission notice shall be included in all 11 | copies or substantial portions of the Software. 12 | 13 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 19 | SOFTWARE. -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Roy-T.AStar 2 | A fast 2D path finding library based on the A* algorithm. Works with both grids and graphs. Supports any .NET variant that supports .NETStandard 2.0 or higher. This library has no external dependencies. The library is licensed under the MIT license, see the `LICENSE` file for more details. 3 | 4 | A* is a greedy, graph based, path finding algorithm. It works by using a heuristic to guide the traveral along the graph. In this library we use the Euclidian distance heuristic. For a comprehensive overview of how the A* algorithm works I recommend this interactive [article](https://www.redblobgames.com/pathfinding/a-star/introduction.html) by Red Blob Games. 5 | 6 | ## Installation 7 | Add this library to your project using [NuGet](https://www.nuget.org/packages/RoyT.AStar/): 8 | 9 | ``` 10 | Install-Package RoyT.AStar 11 | ``` 12 | 13 | ## Usage Example 14 | ### Grids 15 | ```csharp 16 | using Roy_T.AStar.Grids; 17 | using Roy_T.AStar.Primitives; 18 | using Roy_T.AStar.Paths; 19 | 20 | // .... 21 | 22 | var gridSize = new GridSize(columns: 10, rows: 10); 23 | var cellSize = new Size(Distance.FromMeters(10), Distance.FromMeters(10)); 24 | var traversalVelocity = Velocity.FromKilometersPerHour(100); 25 | 26 | // Create a new grid, each cell is laterally connected (like how a rook moves over a chess board, other options are available) 27 | // each cell is 10x10 meters large. The connection between cells can be traversed at 100KM/h. 28 | var grid = Grid.CreateGridWithLateralConnections(gridSize, cellSize, traversalVelocity); 29 | 30 | var pathFinder = new PathFinder(); 31 | var path = pathFinder.FindPath(new GridPosition(0, 0), new GridPosition(9, 9), grid); 32 | 33 | Console.WriteLine($"type: {path.Type}, distance: {path.Distance}, duration {path.Duration}"); 34 | // prints: "type: Complete, distance: 180.00m, duration 6.48s" 35 | 36 | // Use path.Edges to get the actual path 37 | yourClass.TraversePath(path.Edges); 38 | 39 | ``` 40 | 41 | ### Graphs 42 | ```csharp 43 | using Roy_T.AStar.Graphs; 44 | using Roy_T.AStar.Primitives; 45 | using Roy_T.AStar.Paths; 46 | 47 | // The agent drives a car that can go at most 140KM/h 48 | var maxAgentSeed = Velocity.FromKilometersPerHour(140); 49 | 50 | // Create directed graph with node a and b, and a one-way direction from a to b 51 | var nodeA = new Node(Position.Zero); 52 | var nodeB = new Node(new Position(10, 10)); 53 | 54 | // On this road there is a speed limit of 100KM/h 55 | var speedLimit = Velocity.FromKilometersPerHour(100); 56 | nodeA.Connect(nodeB, speedLimit); 57 | 58 | var pathFinder = new PathFinder(); 59 | var path = pathFinder.FindPath(nodeA, nodeB, maximumVelocity: maxAgentSpeed); 60 | 61 | Console.WriteLine($"type: {path.Type}, distance: {path.Distance}, duration {path.Duration}"); 62 | // prints: "type: Complete, distance: 14.14m, duration 0.51s" 63 | 64 | // Use path.Edges to get the actual path 65 | yourClass.TraversePath(path.Edges); 66 | ``` 67 | 68 | ### Incomplete paths 69 | ```csharp 70 | // Create a graph with two nodes, but no connection between both nodes 71 | var nodeA = new Node(Position.Zero); 72 | var nodeB = new Node(new Position(10, 10)); 73 | 74 | var pathFinder = new PathFinder(); 75 | var path = pathFinder.FindPath(nodeA, nodeB, maximumVelocity: Velocity.FromKilometersPerHour(100)); 76 | 77 | Console.WriteLine($"type: {path.Type}, distance: {path.Distance}, duration {path.Duration}"); 78 | // prints: "type: ClosestApproach, distance: 0.00m, duration 0.00s" 79 | ``` 80 | 81 | ## Details 82 | This library uses a graph for all the underlying path finding. But for convenience there is also a grid class. Using this grid class you will never know that you are dealing with graphs, unless if you want too of course ;). 83 | 84 | The goal of this library is to make the path finding extremely fast. Even for huge graphs, with 10.000 nodes and 40.000 edges, the algorithm will find a path in 10 miliseconds. For more details please check the [BenchmarkHistory.md](BenchmarkHistory.md) file. 85 | 86 | This library is so fast because of the underlying data models we used. Especially the `MinHeap` data structure makes sure that we can efficiently look up the best candidates to advance the path. Another advantage is that most of the calculations (like costs of edges) are precomputed when building the graph. Which saves time when searching for a path. 87 | 88 | ## Viewer 89 | This code repository contains a WPF application which you can use to visualize the pathfinding algorithm. Right click nodes to remove them, it will automatically update the best path. You can also use the options in the graph menu to create different types of grids/graphs, and to randomize the traversal velocity of the edges. Remember: A* will always find the fastest path, not the shortest path! 90 | 91 | ![The viewer](viewer.png?raw=true "The viewer") 92 | 93 | 94 | ## Advanced techniques/Migrating from older versions 95 | Previous versions, before 3.0, had a few features that are no longer available in 3.0. However you can mimic most of these features in a more efficient way, using the new graph-first representation. 96 | 97 | ### Corner cutting 98 | If you disconnect a node from a grid at grid position (1,1), using `grid.DisconnectNode` you can also remove the diagonal connections from (0, 1) to (1, 0), (1, 0) to (2, 1), (2, 1) to (1, 2), (1, 2), to (0, 1) using the `grid.RemoveDiagonalConnectionsIntersectingWithNode` method. This mimics the behavior of preventing corner cutting, which was available in the path finder settings in previous versions, but is more efficient. 99 | 100 | ### Movement patterns 101 | If you have a grid you can mimic certain movement patterns. For example creating a grid using `Grids.CreateGridWithLateralConnections` will give you a grid where every agent can move only up/down and left/right between cells (like a rook in chess). You can also use `Grids.CreateGridWithDiagonalConnections` (your agent can move diagonally, like a bishop) or `Grid.CreateGridWithLateralAndDiagonalConnections` (your agent cann move both diagonally and laterally, like a queen). This method mimics movement patterns, but is more efficient. 102 | 103 | ### Different agent sizes 104 | In a previous version (which was only available on GitHub, not on Nuget). You can define different agent shapes and sizes. Unfortunately this slowed down the path finding algorithm considerably. Consider having different graphs for different sized agents, where you manually block off corners where they can't fit. If you really want to support different agent shapes in one grid I recommend using a different algorithm. For example the Explicit Corridor Map Model ([ECCM](https://www.staff.science.uu.nl/~gerae101/UU_crowd_simulation_publications_ecm.html)). 105 | -------------------------------------------------------------------------------- /Roy-T.AStar.Benchmark/Benchmarks.cs: -------------------------------------------------------------------------------- 1 | using BenchmarkDotNet.Attributes; 2 | using Roy_T.AStar.Grids; 3 | using Roy_T.AStar.Paths; 4 | using Roy_T.AStar.Primitives; 5 | 6 | namespace Roy_T.AStar.Benchmark 7 | { 8 | /// 9 | /// For more thorough explanation, and benchmark history, see BenchmarkHistory.md 10 | /// 11 | public class AStarBenchmark 12 | { 13 | private static readonly Velocity MaxSpeed = Velocity.FromKilometersPerHour(100); 14 | 15 | private readonly PathFinder PathFinder; 16 | 17 | private readonly Grid Grid; 18 | private readonly Grid GridWithGradient; 19 | private readonly Grid GridWithHole; 20 | private readonly Grid GridWithRandomLimits; 21 | private readonly Grid GridWithRandomHoles; 22 | private readonly Grid GridWithUnreachableTarget; 23 | 24 | public AStarBenchmark() 25 | { 26 | this.PathFinder = new PathFinder(); 27 | 28 | var gridSize = new GridSize(100, 100); 29 | var cellSize = new Size(Distance.FromMeters(1), Distance.FromMeters(1)); 30 | 31 | this.Grid = Grid.CreateGridWithLateralAndDiagonalConnections(gridSize, cellSize, MaxSpeed); 32 | 33 | this.GridWithGradient = Grid.CreateGridWithLateralAndDiagonalConnections(gridSize, cellSize, MaxSpeed); 34 | GridBuilder.SetGradientLimits(this.GridWithGradient); 35 | 36 | this.GridWithHole = Grid.CreateGridWithLateralAndDiagonalConnections(gridSize, cellSize, MaxSpeed); 37 | GridBuilder.DisconnectDiagonallyExceptForOneNode(this.GridWithHole); 38 | 39 | this.GridWithRandomLimits = Grid.CreateGridWithLateralAndDiagonalConnections(gridSize, cellSize, MaxSpeed); 40 | GridBuilder.SetRandomTraversalVelocities(this.GridWithRandomLimits); 41 | 42 | this.GridWithRandomHoles = Grid.CreateGridWithLateralAndDiagonalConnections(gridSize, cellSize, MaxSpeed); 43 | GridBuilder.DisconnectRandomNodes(this.GridWithRandomHoles); 44 | 45 | this.GridWithUnreachableTarget = Grid.CreateGridWithLateralAndDiagonalConnections(gridSize, cellSize, MaxSpeed); 46 | GridBuilder.DisconnectRightHalf(this.GridWithUnreachableTarget); 47 | } 48 | 49 | [Benchmark] 50 | public void GridBench() 51 | { 52 | this.PathFinder.FindPath( 53 | this.Grid.GetNode(GridPosition.Zero), 54 | this.Grid.GetNode(new GridPosition(this.Grid.Columns - 1, this.Grid.Rows - 1)), 55 | MaxSpeed); 56 | } 57 | 58 | [Benchmark] 59 | public void GridWithHoleBench() 60 | { 61 | this.PathFinder.FindPath( 62 | this.GridWithHole.GetNode(GridPosition.Zero), 63 | this.GridWithHole.GetNode(new GridPosition(this.GridWithHole.Columns - 1, this.GridWithHole.Rows - 1)), 64 | MaxSpeed); 65 | } 66 | 67 | [Benchmark] 68 | public void GridWithRandomHolesBench() 69 | { 70 | this.PathFinder.FindPath( 71 | this.GridWithRandomHoles.GetNode(GridPosition.Zero), 72 | this.GridWithRandomHoles.GetNode(new GridPosition(this.GridWithRandomHoles.Columns - 1, this.GridWithRandomHoles.Rows - 1)), 73 | MaxSpeed); 74 | } 75 | 76 | [Benchmark] 77 | public void GridWithRandomLimitsBench() 78 | { 79 | this.PathFinder.FindPath( 80 | this.GridWithRandomLimits.GetNode(GridPosition.Zero), 81 | this.GridWithRandomLimits.GetNode(new GridPosition(this.GridWithRandomLimits.Columns - 1, this.GridWithRandomLimits.Rows - 1)), 82 | MaxSpeed); 83 | } 84 | 85 | [Benchmark] 86 | public void GridWithUnreachableTargetBench() 87 | { 88 | this.PathFinder.FindPath( 89 | this.GridWithUnreachableTarget.GetNode(GridPosition.Zero), 90 | this.GridWithUnreachableTarget.GetNode(new GridPosition(this.GridWithUnreachableTarget.Columns - 1, this.GridWithUnreachableTarget.Rows - 1)), 91 | MaxSpeed); 92 | } 93 | 94 | [Benchmark] 95 | public void GridWithGradientBench() 96 | { 97 | var maxSpeed = Velocity.FromKilometersPerHour((this.GridWithGradient.Rows * this.GridWithGradient.Columns) + 1); 98 | this.PathFinder.FindPath( 99 | this.GridWithGradient.GetNode(GridPosition.Zero), 100 | this.GridWithGradient.GetNode(new GridPosition(this.GridWithGradient.Columns - 1, this.GridWithGradient.Rows - 1)), 101 | maxSpeed); 102 | } 103 | } 104 | } 105 | -------------------------------------------------------------------------------- /Roy-T.AStar.Benchmark/GridBuilder.cs: -------------------------------------------------------------------------------- 1 | using Roy_T.AStar.Grids; 2 | using Roy_T.AStar.Primitives; 3 | 4 | namespace Roy_T.AStar.Benchmark 5 | { 6 | public static class GridBuilder 7 | { 8 | private static readonly float[] RandomNumbers = new float[] 9 | { 10 | 23, 24, 95, 72, 34, 63, 46, 43, 57, 61, 11 | 12, 77, 30, 25, 49, 83, 54, 64, 42, 4, 12 | 14, 43, 61, 81, 44, 51, 5, 62, 84, 60, 13 | 42, 35, 90, 32, 7, 78, 58, 77, 67, 12, 14 | 65, 47, 11, 66, 37, 12, 27, 61, 73, 42, 15 | 51, 58, 27, 42, 42, 41, 43, 76, 72, 86, 16 | 49, 74, 96, 20, 50, 13, 85, 71, 51, 48, 17 | 13, 15, 35, 47, 87, 100, 53, 1, 9, 41, 18 | 1, 28, 59, 15, 38, 70, 92, 41, 84, 87, 19 | 6, 81, 80, 70, 1, 64, 94 20 | }; 21 | 22 | /// 23 | /// Disconnects the left part of the right part of the given graph. 24 | /// 25 | public static void DisconnectRightHalf(Grid grid) 26 | { 27 | for (var y = 0; y < grid.Rows; y++) 28 | { 29 | grid.DisconnectNode(new GridPosition(grid.Columns / 2, y)); 30 | } 31 | } 32 | 33 | 34 | /// 35 | /// Pseudo randomly disconnects roughly 50% of the nodes. Does not disconnect nodes 36 | /// in the top left and bottom right, corners. 37 | /// 38 | public static void DisconnectRandomNodes(Grid grid) 39 | { 40 | var z = 0; 41 | for (var y = 2; y < grid.Rows - 2; y++) 42 | { 43 | for (var x = 2; x < grid.Columns - 2; x++) 44 | { 45 | var rand = RandomNumbers[z]; 46 | if (rand < 50) 47 | { 48 | grid.DisconnectNode(new GridPosition(x, y)); 49 | } 50 | z = (z + 1) % RandomNumbers.Length; 51 | } 52 | } 53 | } 54 | 55 | /// 56 | /// Disconnects the top left part of the bottom right part of the given graph, 57 | /// except for in a single point near the center. 58 | /// 59 | public static void DisconnectDiagonallyExceptForOneNode(Grid grid) 60 | { 61 | for (var i = grid.Rows - 1; i >= 0; i--) 62 | { 63 | if (i != (grid.Rows / 2) - 1) 64 | { 65 | grid.DisconnectNode(new GridPosition(i, i)); 66 | } 67 | } 68 | } 69 | 70 | /// 71 | /// Makes edges in the top-left of the graph faster to traverse, while 72 | /// making the edges in the bottom right of the graph slower to traverse. 73 | /// 74 | public static void SetGradientLimits(Grid grid) 75 | { 76 | var speedLimit = (grid.Rows * grid.Columns) + 1; 77 | for (var y = 0; y < grid.Rows; y++) 78 | { 79 | for (var x = 0; x < grid.Columns; x++) 80 | { 81 | var node = grid.GetNode(new GridPosition(x, y)); 82 | foreach (var edge in node.Incoming) 83 | { 84 | edge.TraversalVelocity = Velocity.FromKilometersPerHour(speedLimit); 85 | } 86 | 87 | speedLimit -= 1; 88 | } 89 | } 90 | } 91 | 92 | /// 93 | /// Pseudo randomly assigns traversal velocities in [80..100km/h] to edges. 94 | /// 95 | public static void SetRandomTraversalVelocities(Grid grid) 96 | { 97 | var z = 0; 98 | for (var y = 0; y < grid.Rows; y++) 99 | { 100 | for (var x = 0; x < grid.Columns; x++) 101 | { 102 | var node = grid.GetNode(new GridPosition(x, y)); 103 | foreach (var edge in node.Incoming) 104 | { 105 | var speed = (RandomNumbers[z] / 100.0f * 20) + 80; 106 | edge.TraversalVelocity = Velocity.FromKilometersPerHour(speed); 107 | z = (z + 1) % RandomNumbers.Length; 108 | } 109 | } 110 | } 111 | } 112 | } 113 | } 114 | -------------------------------------------------------------------------------- /Roy-T.AStar.Benchmark/Program.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using BenchmarkDotNet.Running; 3 | 4 | namespace Roy_T.AStar.Benchmark 5 | { 6 | public class Program 7 | { 8 | static void Main(string[] _) 9 | { 10 | BenchmarkRunner.Run(); 11 | Console.ReadLine(); 12 | } 13 | } 14 | } 15 | -------------------------------------------------------------------------------- /Roy-T.AStar.Benchmark/Roy-T.AStar.Benchmark.csproj: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Exe 5 | net8.0 6 | Roy_T.AStar.Benchmark 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /Roy-T.AStar.Tests/Collections/MinHeapTests.cs: -------------------------------------------------------------------------------- 1 | using NUnit.Framework; 2 | using Roy_T.AStar.Collections; 3 | 4 | namespace Roy_T.AStar.Tests.Collections 5 | { 6 | public sealed class MinHeapTest 7 | { 8 | [Test] 9 | public void ShouldSort__ReserveSortedInput() 10 | { 11 | var heap = new MinHeap(); 12 | heap.Insert(5); 13 | heap.Insert(4); 14 | heap.Insert(3); 15 | heap.Insert(2); 16 | heap.Insert(1); 17 | 18 | Assert.That(heap.Peek(), Is.EqualTo(1)); 19 | Assert.That(heap.Extract(), Is.EqualTo(1)); 20 | 21 | Assert.That(heap.Peek(), Is.EqualTo(2)); 22 | Assert.That(heap.Extract(), Is.EqualTo(2)); 23 | 24 | Assert.That(heap.Peek(), Is.EqualTo(3)); 25 | Assert.That(heap.Extract(), Is.EqualTo(3)); 26 | 27 | Assert.That(heap.Peek(), Is.EqualTo(4)); 28 | Assert.That(heap.Extract(), Is.EqualTo(4)); 29 | 30 | Assert.That(heap.Peek(), Is.EqualTo(5)); 31 | Assert.That(heap.Extract(), Is.EqualTo(5)); 32 | } 33 | 34 | [Test] 35 | public void ShouldSort__SortedInput() 36 | { 37 | var heap = new MinHeap(); 38 | heap.Insert(1); 39 | heap.Insert(2); 40 | heap.Insert(3); 41 | heap.Insert(4); 42 | heap.Insert(5); 43 | 44 | Assert.That(heap.Peek(), Is.EqualTo(1)); 45 | Assert.That(heap.Extract(), Is.EqualTo(1)); 46 | 47 | Assert.That(heap.Peek(), Is.EqualTo(2)); 48 | Assert.That(heap.Extract(), Is.EqualTo(2)); 49 | 50 | Assert.That(heap.Peek(), Is.EqualTo(3)); 51 | Assert.That(heap.Extract(), Is.EqualTo(3)); 52 | 53 | Assert.That(heap.Peek(), Is.EqualTo(4)); 54 | Assert.That(heap.Extract(), Is.EqualTo(4)); 55 | 56 | Assert.That(heap.Peek(), Is.EqualTo(5)); 57 | Assert.That(heap.Extract(), Is.EqualTo(5)); 58 | } 59 | 60 | [Test] 61 | public void ShouldSort__UnsortedInput() 62 | { 63 | var heap = new MinHeap(); 64 | heap.Insert(3); 65 | heap.Insert(2); 66 | heap.Insert(1); 67 | heap.Insert(5); 68 | heap.Insert(4); 69 | 70 | Assert.That(heap.Peek(), Is.EqualTo(1)); 71 | Assert.That(heap.Extract(), Is.EqualTo(1)); 72 | 73 | Assert.That(heap.Peek(), Is.EqualTo(2)); 74 | Assert.That(heap.Extract(), Is.EqualTo(2)); 75 | 76 | Assert.That(heap.Peek(), Is.EqualTo(3)); 77 | Assert.That(heap.Extract(), Is.EqualTo(3)); 78 | 79 | Assert.That(heap.Peek(), Is.EqualTo(4)); 80 | Assert.That(heap.Extract(), Is.EqualTo(4)); 81 | 82 | Assert.That(heap.Peek(), Is.EqualTo(5)); 83 | Assert.That(heap.Extract(), Is.EqualTo(5)); 84 | } 85 | 86 | [Test] 87 | public void ShouldSort__AfterRemoving() 88 | { 89 | var heap = new MinHeap(); 90 | heap.Insert(1); 91 | heap.Insert(2); 92 | heap.Insert(3); 93 | heap.Insert(4); 94 | heap.Insert(5); 95 | 96 | heap.Remove(4); 97 | 98 | Assert.That(heap.Extract(), Is.EqualTo(1)); 99 | Assert.That(heap.Extract(), Is.EqualTo(2)); 100 | Assert.That(heap.Extract(), Is.EqualTo(3)); 101 | Assert.That(heap.Extract(), Is.EqualTo(5)); 102 | } 103 | } 104 | } 105 | -------------------------------------------------------------------------------- /Roy-T.AStar.Tests/GridSerializationTests.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using System.Collections.Generic; 3 | using System.Linq; 4 | using System.Text; 5 | using NUnit.Framework; 6 | using Roy_T.AStar.Graphs; 7 | using Roy_T.AStar.Grids; 8 | using Roy_T.AStar.Paths; 9 | using Roy_T.AStar.Primitives; 10 | using Roy_T.AStar.Serialization; 11 | 12 | namespace Roy_T.AStar.Tests 13 | { 14 | public sealed class GridSerializationTests 15 | { 16 | [Test] 17 | public void GraphIsEqualAfterSerializeAndDeSerialize() 18 | { 19 | var grid = Grid.CreateGridWithLateralConnections(new GridSize(2, 4), 20 | new Size(Distance.FromMeters(2.0f), Distance.FromMeters(1.0f)), Velocity.FromKilometersPerHour(3)); 21 | 22 | var stringGrid = GridSerializer.SerializeGrid(grid); 23 | var deserializedGrid = GridSerializer.DeSerializeGrid(stringGrid); 24 | 25 | Assert.AreEqual(grid.Rows, deserializedGrid.Rows); 26 | Assert.AreEqual(grid.Columns, deserializedGrid.Columns); 27 | for (int i = 0; i < grid.Columns; i++) 28 | { 29 | for (int j = 0; j < grid.Rows; j++) 30 | { 31 | var gridPosition = new GridPosition(i, j); 32 | var originalNode = grid.GetNode(gridPosition); 33 | var deserializedNode = deserializedGrid.GetNode(gridPosition); 34 | Assert.AreEqual(originalNode.Position, deserializedNode.Position); 35 | Assert.AreEqual(originalNode.Outgoing.Count, deserializedNode.Outgoing.Count); 36 | Assert.AreEqual(originalNode.Incoming.Count, deserializedNode.Incoming.Count); 37 | foreach (var edge in originalNode.Outgoing) 38 | { 39 | var matchingEdge = deserializedNode.Outgoing.Single(o => 40 | o.Start.Position.Equals(edge.Start.Position) && o.End.Position.Equals(edge.End.Position)); 41 | Assert.AreEqual(edge.Distance, matchingEdge.Distance); 42 | Assert.AreEqual(edge.TraversalDuration, matchingEdge.TraversalDuration); 43 | Assert.AreEqual(edge.TraversalVelocity, matchingEdge.TraversalVelocity); 44 | } 45 | 46 | foreach (var edge in originalNode.Incoming) 47 | { 48 | var matchingEdge = deserializedNode.Incoming.Single(o => 49 | o.Start.Position.Equals(edge.Start.Position) && o.End.Position.Equals(edge.End.Position)); 50 | Assert.AreEqual(edge.Distance, matchingEdge.Distance); 51 | Assert.AreEqual(edge.TraversalDuration, matchingEdge.TraversalDuration); 52 | Assert.AreEqual(edge.TraversalVelocity, matchingEdge.TraversalVelocity); 53 | } 54 | } 55 | } 56 | } 57 | } 58 | } -------------------------------------------------------------------------------- /Roy-T.AStar.Tests/PathFinderTests.cs: -------------------------------------------------------------------------------- 1 | using NUnit.Framework; 2 | using Roy_T.AStar.Graphs; 3 | using Roy_T.AStar.Grids; 4 | using Roy_T.AStar.Paths; 5 | using Roy_T.AStar.Primitives; 6 | 7 | namespace Roy_T.AStar.Tests 8 | { 9 | public sealed class PathFinderTests 10 | { 11 | private readonly PathFinder PathFinder; 12 | 13 | public PathFinderTests() 14 | { 15 | this.PathFinder = new PathFinder(); 16 | } 17 | 18 | 19 | [Test] 20 | public void Issue50() 21 | { 22 | var columns = 21; 23 | var rows = 21; 24 | var start = new GridPosition(0, 20); 25 | var end = new GridPosition(16, 2); 26 | 27 | var grid = Grid.CreateGridWithLateralAndDiagonalConnections(new GridSize(columns, rows), new Size(Distance.FromMeters(1), Distance.FromMeters(1)), Velocity.FromMetersPerSecond(1.0f)); 28 | 29 | grid.DisconnectNode(new GridPosition(7, 3)); 30 | grid.DisconnectNode(new GridPosition(8, 3)); 31 | grid.DisconnectNode(new GridPosition(9, 3)); 32 | grid.DisconnectNode(new GridPosition(10, 3)); 33 | grid.DisconnectNode(new GridPosition(11, 3)); 34 | grid.DisconnectNode(new GridPosition(12, 3)); 35 | grid.DisconnectNode(new GridPosition(13, 3)); 36 | grid.DisconnectNode(new GridPosition(14, 3)); 37 | 38 | grid.DisconnectNode(new GridPosition(14, 4)); 39 | grid.DisconnectNode(new GridPosition(14, 5)); 40 | grid.DisconnectNode(new GridPosition(14, 6)); 41 | grid.DisconnectNode(new GridPosition(14, 7)); 42 | grid.DisconnectNode(new GridPosition(14, 8)); 43 | grid.DisconnectNode(new GridPosition(14, 9)); 44 | grid.DisconnectNode(new GridPosition(14, 10)); 45 | grid.DisconnectNode(new GridPosition(14, 11)); 46 | grid.DisconnectNode(new GridPosition(14, 12)); 47 | 48 | var pathA = this.PathFinder.FindPath(start, end, grid); 49 | Assert.That(pathA.Edges.Count < 32); 50 | 51 | var pathB = this.PathFinder.FindPath(end, start, grid); 52 | Assert.That(pathB.Edges.Count < 32); 53 | } 54 | 55 | [Test] 56 | public void ShouldFindPath__StartNodeIsEndNode() 57 | { 58 | var node = new Node(Position.Zero); 59 | var path = this.PathFinder.FindPath(node, node, Velocity.FromMetersPerSecond(1)); 60 | 61 | Assert.That(path.Type, Is.EqualTo(PathType.Complete)); 62 | Assert.That(path.Edges.Count, Is.EqualTo(0)); 63 | Assert.That(path.Distance, Is.EqualTo(Distance.FromMeters(0))); 64 | Assert.That(path.Duration, Is.EqualTo(Duration.Zero)); 65 | } 66 | 67 | [Test] 68 | public void ShouldFindPath_AcyclicGraph() 69 | { 70 | var nodeA = new Node(new Position(0, 0)); 71 | var nodeB = new Node(new Position(10, 0)); 72 | var nodeC = new Node(new Position(20, 0)); 73 | 74 | nodeA.Connect(nodeB, Velocity.FromMetersPerSecond(1)); 75 | nodeB.Connect(nodeC, Velocity.FromMetersPerSecond(1)); 76 | 77 | var path = this.PathFinder.FindPath(nodeA, nodeC, Velocity.FromMetersPerSecond(1)); 78 | 79 | Assert.That(path.Type, Is.EqualTo(PathType.Complete)); 80 | Assert.That(path.Edges.Count, Is.EqualTo(2)); 81 | Assert.That(path.Distance, Is.EqualTo(Distance.FromMeters(20))); 82 | Assert.That(path.Duration, Is.EqualTo(Duration.FromSeconds(20))); 83 | } 84 | 85 | [Test] 86 | public void ShouldFindPath_CyclicGraph() 87 | { 88 | var nodeA = new Node(new Position(0, 0)); 89 | var nodeB = new Node(new Position(10, 0)); 90 | var nodeC = new Node(new Position(20, 0)); 91 | 92 | nodeA.Connect(nodeB, Velocity.FromMetersPerSecond(1)); 93 | nodeB.Connect(nodeC, Velocity.FromMetersPerSecond(1)); 94 | 95 | nodeB.Connect(nodeA, Velocity.FromMetersPerSecond(1)); 96 | nodeC.Connect(nodeB, Velocity.FromMetersPerSecond(1)); 97 | 98 | var path = this.PathFinder.FindPath(nodeA, nodeC, Velocity.FromMetersPerSecond(1)); 99 | 100 | Assert.That(path.Type, Is.EqualTo(PathType.Complete)); 101 | Assert.That(path.Edges.Count, Is.EqualTo(2)); 102 | Assert.That(path.Distance, Is.EqualTo(Distance.FromMeters(20))); 103 | Assert.That(path.Duration, Is.EqualTo(Duration.FromSeconds(20))); 104 | } 105 | 106 | [Test] 107 | public void ShouldFindPath_GraphWithDeadEnds() 108 | { 109 | var nodeCenter = new Node(new Position(10, 10)); 110 | var nodeLeft = new Node(new Position(0, 10)); 111 | var nodeRight = new Node(new Position(20, 10)); 112 | var nodeAbove = new Node(new Position(10, 0)); 113 | var nodeBelow = new Node(new Position(10, 20)); 114 | 115 | nodeCenter.Connect(nodeLeft, Velocity.FromMetersPerSecond(1)); 116 | nodeLeft.Connect(nodeCenter, Velocity.FromMetersPerSecond(1)); 117 | 118 | nodeCenter.Connect(nodeRight, Velocity.FromMetersPerSecond(1)); 119 | nodeRight.Connect(nodeCenter, Velocity.FromMetersPerSecond(1)); 120 | 121 | nodeCenter.Connect(nodeAbove, Velocity.FromMetersPerSecond(1)); 122 | nodeAbove.Connect(nodeCenter, Velocity.FromMetersPerSecond(1)); 123 | 124 | nodeCenter.Connect(nodeBelow, Velocity.FromMetersPerSecond(1)); 125 | nodeBelow.Connect(nodeCenter, Velocity.FromMetersPerSecond(1)); 126 | 127 | var path = this.PathFinder.FindPath(nodeLeft, nodeBelow, Velocity.FromMetersPerSecond(1)); 128 | 129 | Assert.That(path.Type, Is.EqualTo(PathType.Complete)); 130 | Assert.That(path.Edges.Count, Is.EqualTo(2)); 131 | Assert.That(path.Distance, Is.EqualTo(Distance.FromMeters(20))); 132 | Assert.That(path.Duration, Is.EqualTo(Duration.FromSeconds(20))); 133 | } 134 | } 135 | } 136 | -------------------------------------------------------------------------------- /Roy-T.AStar.Tests/Roy-T.AStar.Tests.csproj: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | net8.0 5 | Roy_T.AStar.Tests 6 | 7 | false 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /Roy-T.AStar.Viewer/App.xaml: -------------------------------------------------------------------------------- 1 |  6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /Roy-T.AStar.Viewer/App.xaml.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using System.Collections.Generic; 3 | using System.Configuration; 4 | using System.Data; 5 | using System.Linq; 6 | using System.Threading.Tasks; 7 | using System.Windows; 8 | 9 | namespace Roy_T.AStar.Viewer 10 | { 11 | /// 12 | /// Interaction logic for App.xaml 13 | /// 14 | public partial class App : Application 15 | { 16 | } 17 | } 18 | -------------------------------------------------------------------------------- /Roy-T.AStar.Viewer/Connections.cs: -------------------------------------------------------------------------------- 1 | namespace Roy_T.AStar.Viewer 2 | { 3 | internal enum Connections 4 | { 5 | Lateral, 6 | Diagonal, 7 | LateralAndDiagonal 8 | } 9 | } 10 | -------------------------------------------------------------------------------- /Roy-T.AStar.Viewer/EdgeSpeedColorConverter.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using System.Globalization; 3 | using System.Windows.Data; 4 | using System.Windows.Media; 5 | using Roy_T.AStar.Primitives; 6 | 7 | namespace Roy_T.AStar.Viewer 8 | { 9 | internal sealed class EdgeSpeedColorConverter : IValueConverter 10 | { 11 | public object Convert(object value, Type targetType, object parameter, CultureInfo culture) 12 | { 13 | if (value is Velocity velocity) 14 | { 15 | var range = Settings.MaxSpeed.MetersPerSecond - Settings.MinSpeed.MetersPerSecond; 16 | var lerp = (velocity.MetersPerSecond - Settings.MinSpeed.MetersPerSecond) / range; 17 | var invLerp = 1.0f - lerp; 18 | 19 | var color = Color.FromScRgb(1.0f, invLerp, lerp, 0.0f); 20 | return new SolidColorBrush(color); 21 | } 22 | 23 | return Brushes.Red; 24 | } 25 | 26 | public object ConvertBack(object value, Type targetType, object parameter, CultureInfo culture) 27 | => throw new NotImplementedException(); 28 | } 29 | } 30 | -------------------------------------------------------------------------------- /Roy-T.AStar.Viewer/GraphDataTemplateSelector.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using System.Windows; 3 | using System.Windows.Controls; 4 | using Roy_T.AStar.Viewer.Model; 5 | 6 | namespace Roy_T.AStar.Viewer 7 | { 8 | internal sealed class GraphDataTemplateSelector : DataTemplateSelector 9 | { 10 | public override DataTemplate SelectTemplate(object item, DependencyObject container) 11 | { 12 | var element = container as FrameworkElement; 13 | 14 | if (item is EdgeModel) 15 | { 16 | return element.FindResource("EdgeDataTemplate") as DataTemplate; 17 | } 18 | 19 | if (item is PathEdgeModel) 20 | { 21 | return element.FindResource("PathEdgeDataTemplate") as DataTemplate; 22 | } 23 | 24 | if (item is NodeModel) 25 | { 26 | return element.FindResource("NodeDataTemplate") as DataTemplate; 27 | } 28 | 29 | throw new NotSupportedException(); 30 | } 31 | } 32 | } 33 | -------------------------------------------------------------------------------- /Roy-T.AStar.Viewer/MainWindow.xaml: -------------------------------------------------------------------------------- 1 |  12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | -------------------------------------------------------------------------------- /Roy-T.AStar.Viewer/MainWindow.xaml.cs: -------------------------------------------------------------------------------- 1 | using System.Windows; 2 | 3 | namespace Roy_T.AStar.Viewer 4 | { 5 | /// 6 | /// Interaction logic for MainWindow.xaml 7 | /// 8 | public partial class MainWindow : Window 9 | { 10 | public MainWindow() 11 | { 12 | InitializeComponent(); 13 | this.Loaded += this.MainWindow_Loaded; 14 | } 15 | 16 | private void MainWindow_Loaded(object sender, RoutedEventArgs e) 17 | => this.DataContext = new MainWindowViewModel(); 18 | } 19 | } 20 | -------------------------------------------------------------------------------- /Roy-T.AStar.Viewer/MainWindowViewModel.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using System.Collections.Generic; 3 | using System.Collections.ObjectModel; 4 | using System.Diagnostics; 5 | using System.IO; 6 | using System.Linq; 7 | using System.Reactive.Linq; 8 | using System.Windows; 9 | using DynamicData; 10 | using Microsoft.Win32; 11 | using ReactiveUI; 12 | using Roy_T.AStar.Grids; 13 | using Roy_T.AStar.Paths; 14 | using Roy_T.AStar.Primitives; 15 | using Roy_T.AStar.Serialization; 16 | using Roy_T.AStar.Viewer.Model; 17 | 18 | namespace Roy_T.AStar.Viewer 19 | { 20 | internal sealed class MainWindowViewModel : ReactiveObject 21 | { 22 | private readonly Random Random; 23 | private readonly PathFinder PathFinder; 24 | 25 | private NodeModel startNode; 26 | private NodeModel endNode; 27 | private Grid grid; 28 | private string outcome; 29 | 30 | public MainWindowViewModel() 31 | { 32 | this.PathFinder = new PathFinder(); 33 | 34 | this.Models = new ObservableCollection(); 35 | 36 | this.Random = new Random(); 37 | this.outcome = string.Empty; 38 | 39 | this.ExitCommand = ReactiveCommand.Create(() => 40 | { 41 | Application.Current.Shutdown(); 42 | }); 43 | 44 | this.OpenGitHubCommand = ReactiveCommand.Create(() => 45 | { 46 | var psi = new ProcessStartInfo 47 | { 48 | FileName = "cmd", 49 | WindowStyle = ProcessWindowStyle.Hidden, 50 | UseShellExecute = false, 51 | CreateNoWindow = true, 52 | Arguments = $"/c start http://github.com/roy-t/AStar" 53 | }; 54 | Process.Start(psi); 55 | }); 56 | 57 | this.ResetCommand = ReactiveCommand.Create(() => this.CreateNodes(Connections.LateralAndDiagonal)); 58 | this.LateralCommand = ReactiveCommand.Create(() => this.CreateNodes(Connections.Lateral)); 59 | this.DiagonalCommand = ReactiveCommand.Create(() => this.CreateNodes(Connections.Diagonal)); 60 | this.RandomizeCommand = ReactiveCommand.Create(() => this.SetSpeedLimits(() => 61 | { 62 | var value = this.Random.Next((int)Settings.MinSpeed.MetersPerSecond, (int)Settings.MaxSpeed.MetersPerSecond + 1); 63 | return Velocity.FromMetersPerSecond(value); 64 | })); 65 | 66 | this.MaxCommand = ReactiveCommand.Create(() => this.SetSpeedLimits(() => Settings.MaxSpeed)); 67 | this.MinCommand = ReactiveCommand.Create(() => this.SetSpeedLimits(() => Settings.MinSpeed)); 68 | 69 | this.SaveGridCommand = ReactiveCommand.Create(this.SaveGrid); 70 | this.OpenGridCommand = ReactiveCommand.Create(this.OpenGrid); 71 | 72 | this.CreateNodes(Connections.LateralAndDiagonal); 73 | } 74 | 75 | public string Outcome 76 | { 77 | get => this.outcome; 78 | set => this.RaiseAndSetIfChanged(ref this.outcome, value); 79 | } 80 | 81 | public ObservableCollection Models { get; } 82 | 83 | public IReactiveCommand ExitCommand { get; } 84 | public IReactiveCommand OpenGitHubCommand { get; } 85 | 86 | public IReactiveCommand ResetCommand { get; } 87 | 88 | public IReactiveCommand LateralCommand { get; } 89 | 90 | public IReactiveCommand DiagonalCommand { get; } 91 | public IReactiveCommand RandomizeCommand { get; } 92 | public IReactiveCommand MaxCommand { get; } 93 | public IReactiveCommand MinCommand { get; } 94 | 95 | public IReactiveCommand SaveGridCommand { get; } 96 | 97 | public IReactiveCommand OpenGridCommand { get; } 98 | 99 | private void CreateNodes(Connections connections) 100 | { 101 | this.Clear(); 102 | this.grid = CreateGrid(connections); 103 | var models = ModelBuilder.BuildModel(this.grid, n => this.EditNode(n), n => this.RemoveNode(n)); 104 | this.Models.AddRange(models); 105 | 106 | this.startNode = this.Models.OfType().FirstOrDefault(); 107 | this.startNode.NodeState = NodeState.Start; 108 | 109 | this.endNode = this.Models.OfType().LastOrDefault(); 110 | this.endNode.NodeState = NodeState.End; 111 | 112 | this.CalculatePath(); 113 | } 114 | 115 | private void SaveGrid() 116 | { 117 | SaveFileDialog saveFileDialog = new SaveFileDialog(); 118 | saveFileDialog.Filter = "XML file (*.xml)|*.xml"; 119 | if (saveFileDialog.ShowDialog() == true) 120 | File.WriteAllText(saveFileDialog.FileName, GridSerializer.SerializeGrid(this.grid)); 121 | } 122 | 123 | private void OpenGrid() 124 | { 125 | OpenFileDialog openFileDialog = new OpenFileDialog(); 126 | openFileDialog.Filter = "XML file (*.xml)|*.xml"; 127 | if (openFileDialog.ShowDialog() == true) 128 | { 129 | this.Clear(); 130 | this.grid = GridSerializer.DeSerializeGrid(File.ReadAllText(openFileDialog.FileName)); 131 | 132 | var models = ModelBuilder.BuildModel(this.grid, n => this.EditNode(n), n => this.RemoveNode(n)); 133 | this.Models.AddRange(models); 134 | 135 | this.startNode = this.Models.OfType().FirstOrDefault(); 136 | this.startNode.NodeState = NodeState.Start; 137 | 138 | this.endNode = this.Models.OfType().LastOrDefault(); 139 | this.endNode.NodeState = NodeState.End; 140 | 141 | this.CalculatePath(); 142 | } 143 | } 144 | 145 | private static Grid CreateGrid(Connections connections) 146 | { 147 | var gridSize = new GridSize(columns: 14, rows: 7); 148 | var cellSize = new Primitives.Size(Distance.FromMeters(100), Distance.FromMeters(100)); 149 | 150 | return connections switch 151 | { 152 | Connections.Lateral => Grid.CreateGridWithLateralConnections(gridSize, cellSize, Settings.MaxSpeed), 153 | Connections.Diagonal => Grid.CreateGridWithDiagonalConnections(gridSize, cellSize, Settings.MaxSpeed), 154 | Connections.LateralAndDiagonal => Grid.CreateGridWithLateralAndDiagonalConnections(gridSize, cellSize, Settings.MaxSpeed), 155 | _ => throw new ArgumentOutOfRangeException(nameof(connections), $"Invalid connection type {connections}") 156 | }; 157 | } 158 | 159 | private void Clear() 160 | { 161 | this.startNode = null; 162 | this.endNode = null; 163 | this.outcome = string.Empty; 164 | this.Models.Clear(); 165 | } 166 | 167 | private void SetSpeedLimits(Func speedLimitFunc) 168 | { 169 | foreach (var edge in this.Models.OfType()) 170 | { 171 | edge.Velocity = speedLimitFunc(); 172 | } 173 | 174 | this.CalculatePath(); 175 | } 176 | 177 | private void EditNode(NodeModel model) 178 | { 179 | switch (model.NodeState) 180 | { 181 | case NodeState.Start: 182 | model.NodeState = NodeState.End; 183 | if (this.endNode != null) 184 | { 185 | this.endNode.NodeState = NodeState.None; 186 | } 187 | this.startNode = null; 188 | this.endNode = model; 189 | break; 190 | case NodeState.End: 191 | this.endNode = null; 192 | model.NodeState = NodeState.None; 193 | break; 194 | case NodeState.None: 195 | model.NodeState = NodeState.Start; 196 | if (this.startNode != null) 197 | { 198 | this.startNode.NodeState = NodeState.None; 199 | } 200 | this.startNode = model; 201 | break; 202 | } 203 | 204 | this.CalculatePath(); 205 | } 206 | 207 | private void RemoveNode(NodeModel model) 208 | { 209 | this.grid.DisconnectNode(model.GridPosition); 210 | this.grid.RemoveDiagonalConnectionsIntersectingWithNode(model.GridPosition); 211 | 212 | this.Models.Clear(); 213 | this.Models.AddRange(ModelBuilder.BuildModel(this.grid, n => this.EditNode(n), n => this.RemoveNode(n))); 214 | 215 | if (this.startNode != null) 216 | { 217 | this.startNode = this.Models.OfType().FirstOrDefault(n => n.GridPosition == this.startNode.GridPosition); 218 | } 219 | 220 | if (this.startNode != null) 221 | { 222 | this.startNode.NodeState = NodeState.Start; 223 | } 224 | 225 | if (this.endNode != null) 226 | { 227 | this.endNode = this.Models.OfType().FirstOrDefault(n => n.GridPosition == this.endNode.GridPosition); 228 | } 229 | 230 | if (this.endNode != null) 231 | { 232 | this.endNode.NodeState = NodeState.End; 233 | } 234 | 235 | this.CalculatePath(); 236 | } 237 | 238 | private void CalculatePath() 239 | { 240 | if (this.startNode != null && this.endNode != null) 241 | { 242 | var path = this.PathFinder.FindPath(this.startNode.Node, this.endNode.Node, Settings.MaxSpeed); 243 | var averageSpeed = Velocity.FromMetersPerSecond(path.Distance.Meters / path.Duration.Seconds); 244 | this.Outcome = $"Found path, type: {path.Type}, distance {path.Distance}, average speed {averageSpeed}, expected duration {path.Duration}"; 245 | 246 | this.ClearPath(); 247 | 248 | var toAdd = new List(); 249 | 250 | foreach (var edge in path.Edges) 251 | { 252 | var edgeModel = new PathEdgeModel(edge.Start.Position.X, edge.Start.Position.Y, edge.End.Position.X, edge.End.Position.Y); 253 | toAdd.Add(edgeModel); 254 | } 255 | 256 | this.Models.AddRange(toAdd); 257 | } 258 | else 259 | { 260 | this.ClearPath(); 261 | } 262 | } 263 | 264 | private void ClearPath() 265 | { 266 | var toRemove = new List(); 267 | foreach (var edge in this.Models.OfType()) 268 | { 269 | toRemove.Add(edge); 270 | } 271 | 272 | this.Models.RemoveMany(toRemove); 273 | } 274 | } 275 | } 276 | -------------------------------------------------------------------------------- /Roy-T.AStar.Viewer/Model/EdgeModel.cs: -------------------------------------------------------------------------------- 1 | using ReactiveUI; 2 | using Roy_T.AStar.Graphs; 3 | using Roy_T.AStar.Primitives; 4 | 5 | namespace Roy_T.AStar.Viewer.Model 6 | { 7 | internal class EdgeModel : ReactiveObject 8 | { 9 | public EdgeModel(IEdge edge) 10 | { 11 | this.Edge = edge; 12 | } 13 | 14 | public Velocity Velocity 15 | { 16 | get => this.Edge.TraversalVelocity; 17 | set 18 | { 19 | this.Edge.TraversalVelocity = value; 20 | this.RaisePropertyChanged(nameof(this.Velocity)); 21 | } 22 | } 23 | 24 | public IEdge Edge { get; } 25 | 26 | public float X1 => this.Edge.Start.Position.X; 27 | public float Y1 => this.Edge.Start.Position.Y; 28 | public float X2 => this.Edge.End.Position.X; 29 | public float Y2 => this.Edge.End.Position.Y; 30 | 31 | public float Z => 1; 32 | 33 | // To prevent binding errors (or complicated content presenter logic) we also define an X and Y component on edges 34 | public float X => 0; 35 | public float Y => 0; 36 | } 37 | } 38 | -------------------------------------------------------------------------------- /Roy-T.AStar.Viewer/Model/ModelBuilder.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using System.Collections.Generic; 3 | using ReactiveUI; 4 | using Roy_T.AStar.Grids; 5 | using Roy_T.AStar.Primitives; 6 | 7 | namespace Roy_T.AStar.Viewer.Model 8 | { 9 | internal sealed class ModelBuilder 10 | { 11 | public static IEnumerable BuildModel(Grid grid, Action leftClick, Action rightClick) 12 | { 13 | var models = new List(); 14 | 15 | for (var x = 0; x < grid.Columns; x++) 16 | { 17 | for (var y = 0; y < grid.Rows; y++) 18 | { 19 | var gridPosition = new GridPosition(x, y); 20 | 21 | var node = grid.GetNode(gridPosition); 22 | 23 | if (node.Outgoing.Count > 0) 24 | { 25 | 26 | var nodeModel = new NodeModel(node, gridPosition); 27 | nodeModel.LeftClickCommand = ReactiveCommand.Create(() => leftClick(nodeModel)); 28 | nodeModel.RightClickCommand = ReactiveCommand.Create(() => rightClick(nodeModel)); 29 | 30 | models.Add(nodeModel); 31 | 32 | foreach (var edge in node.Outgoing) 33 | { 34 | var edgeModel = new EdgeModel(edge); 35 | models.Add(edgeModel); 36 | } 37 | } 38 | } 39 | } 40 | 41 | return models; 42 | } 43 | } 44 | } 45 | -------------------------------------------------------------------------------- /Roy-T.AStar.Viewer/Model/NodeModel.cs: -------------------------------------------------------------------------------- 1 | using ReactiveUI; 2 | using Roy_T.AStar.Graphs; 3 | using Roy_T.AStar.Primitives; 4 | 5 | namespace Roy_T.AStar.Viewer.Model 6 | { 7 | internal sealed class NodeModel : ReactiveObject 8 | { 9 | private NodeState nodeState; 10 | 11 | public NodeModel(INode node, GridPosition gridPosition) 12 | { 13 | this.Node = node; 14 | this.GridPosition = gridPosition; 15 | this.nodeState = NodeState.None; 16 | } 17 | 18 | public INode Node { get; } 19 | public GridPosition GridPosition { get; } 20 | 21 | public float X => this.Node.Position.X; 22 | public float Y => this.Node.Position.Y; 23 | 24 | public NodeState NodeState 25 | { 26 | get => this.nodeState; 27 | set => this.RaiseAndSetIfChanged(ref this.nodeState, value); 28 | } 29 | 30 | public float Z => 2; 31 | 32 | public IReactiveCommand LeftClickCommand { get; set; } 33 | public IReactiveCommand RightClickCommand { get; set; } 34 | } 35 | } 36 | -------------------------------------------------------------------------------- /Roy-T.AStar.Viewer/Model/NodeState.cs: -------------------------------------------------------------------------------- 1 | namespace Roy_T.AStar.Viewer.Model 2 | { 3 | public enum NodeState 4 | { 5 | Start, 6 | End, 7 | None 8 | } 9 | } 10 | -------------------------------------------------------------------------------- /Roy-T.AStar.Viewer/Model/PathEdgeModel.cs: -------------------------------------------------------------------------------- 1 | using ReactiveUI; 2 | 3 | namespace Roy_T.AStar.Viewer.Model 4 | { 5 | internal sealed class PathEdgeModel : ReactiveObject 6 | { 7 | public PathEdgeModel(float x1, float y1, float x2, float y2) 8 | { 9 | this.X1 = x1; 10 | this.Y1 = y1; 11 | this.X2 = x2; 12 | this.Y2 = y2; 13 | } 14 | 15 | public float X1 { get; } 16 | public float Y1 { get; } 17 | public float X2 { get; } 18 | public float Y2 { get; } 19 | 20 | public float Z => 0; 21 | 22 | // To prevent binding errors (or complicated content presenter logic) we also define an X and Y component on edges 23 | public float X => 0; 24 | public float Y => 0; 25 | } 26 | } 27 | -------------------------------------------------------------------------------- /Roy-T.AStar.Viewer/NodeStateColorConverter.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using System.Globalization; 3 | using System.Windows.Data; 4 | using System.Windows.Media; 5 | using Roy_T.AStar.Viewer.Model; 6 | 7 | namespace Roy_T.AStar.Viewer 8 | { 9 | internal sealed class NodeStateColorConverter : IValueConverter 10 | { 11 | public object Convert(object value, Type targetType, object parameter, CultureInfo culture) 12 | { 13 | if (value is NodeState cellState) 14 | { 15 | switch (cellState) 16 | { 17 | case NodeState.None: 18 | return Brushes.Black; 19 | case NodeState.Start: 20 | return Brushes.LightGreen; 21 | case NodeState.End: 22 | return Brushes.DarkGreen; 23 | } 24 | } 25 | 26 | return Brushes.Red; 27 | } 28 | 29 | public object ConvertBack(object value, Type targetType, object parameter, CultureInfo culture) 30 | => throw new NotImplementedException(); 31 | } 32 | } 33 | -------------------------------------------------------------------------------- /Roy-T.AStar.Viewer/Roy-T.AStar.Viewer.csproj: -------------------------------------------------------------------------------- 1 |  2 | 3 | 4 | WinExe 5 | net8.0-windows7.0 6 | Roy_T.AStar.Viewer 7 | true 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /Roy-T.AStar.Viewer/Settings.cs: -------------------------------------------------------------------------------- 1 | using Roy_T.AStar.Primitives; 2 | 3 | namespace Roy_T.AStar.Viewer 4 | { 5 | public static class Settings 6 | { 7 | public static readonly Velocity MaxSpeed = Velocity.FromKilometersPerHour(100); 8 | public static readonly Velocity MinSpeed = Velocity.FromKilometersPerHour(10); 9 | } 10 | } 11 | -------------------------------------------------------------------------------- /Roy-T.AStar.sln: -------------------------------------------------------------------------------- 1 |  2 | Microsoft Visual Studio Solution File, Format Version 12.00 3 | # Visual Studio Version 16 4 | VisualStudioVersion = 16.0.29519.87 5 | MinimumVisualStudioVersion = 10.0.40219.1 6 | Project("{2150E333-8FDC-42A3-9474-1A3956D46DE8}") = "Solution Items", "Solution Items", "{7484E05D-3E03-498B-AA98-A49AE2D9D488}" 7 | ProjectSection(SolutionItems) = preProject 8 | BenchmarkHistory.md = BenchmarkHistory.md 9 | LICENSE = LICENSE 10 | README.md = README.md 11 | EndProjectSection 12 | EndProject 13 | Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "Roy-T.AStar", "Roy-T.AStar\Roy-T.AStar.csproj", "{6BAF6D44-A0FA-409F-813E-914AD3C57680}" 14 | EndProject 15 | Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "Roy-T.AStar.Tests", "Roy-T.AStar.Tests\Roy-T.AStar.Tests.csproj", "{0591D506-5AEC-4643-8D25-ED72DFDD4B40}" 16 | EndProject 17 | Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "Roy-T.AStar.Benchmark", "Roy-T.AStar.Benchmark\Roy-T.AStar.Benchmark.csproj", "{20C054FD-8967-484A-824E-BE761478E451}" 18 | EndProject 19 | Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "Roy-T.AStar.Viewer", "Roy-T.AStar.Viewer\Roy-T.AStar.Viewer.csproj", "{ECE64819-6423-4187-BC52-A02992ED1945}" 20 | EndProject 21 | Global 22 | GlobalSection(SolutionConfigurationPlatforms) = preSolution 23 | Debug|Any CPU = Debug|Any CPU 24 | Release|Any CPU = Release|Any CPU 25 | EndGlobalSection 26 | GlobalSection(ProjectConfigurationPlatforms) = postSolution 27 | {6BAF6D44-A0FA-409F-813E-914AD3C57680}.Debug|Any CPU.ActiveCfg = Debug|Any CPU 28 | {6BAF6D44-A0FA-409F-813E-914AD3C57680}.Debug|Any CPU.Build.0 = Debug|Any CPU 29 | {6BAF6D44-A0FA-409F-813E-914AD3C57680}.Release|Any CPU.ActiveCfg = Release|Any CPU 30 | {6BAF6D44-A0FA-409F-813E-914AD3C57680}.Release|Any CPU.Build.0 = Release|Any CPU 31 | {0591D506-5AEC-4643-8D25-ED72DFDD4B40}.Debug|Any CPU.ActiveCfg = Debug|Any CPU 32 | {0591D506-5AEC-4643-8D25-ED72DFDD4B40}.Debug|Any CPU.Build.0 = Debug|Any CPU 33 | {0591D506-5AEC-4643-8D25-ED72DFDD4B40}.Release|Any CPU.ActiveCfg = Release|Any CPU 34 | {0591D506-5AEC-4643-8D25-ED72DFDD4B40}.Release|Any CPU.Build.0 = Release|Any CPU 35 | {20C054FD-8967-484A-824E-BE761478E451}.Debug|Any CPU.ActiveCfg = Debug|Any CPU 36 | {20C054FD-8967-484A-824E-BE761478E451}.Debug|Any CPU.Build.0 = Debug|Any CPU 37 | {20C054FD-8967-484A-824E-BE761478E451}.Release|Any CPU.ActiveCfg = Release|Any CPU 38 | {20C054FD-8967-484A-824E-BE761478E451}.Release|Any CPU.Build.0 = Release|Any CPU 39 | {ECE64819-6423-4187-BC52-A02992ED1945}.Debug|Any CPU.ActiveCfg = Debug|Any CPU 40 | {ECE64819-6423-4187-BC52-A02992ED1945}.Debug|Any CPU.Build.0 = Debug|Any CPU 41 | {ECE64819-6423-4187-BC52-A02992ED1945}.Release|Any CPU.ActiveCfg = Release|Any CPU 42 | {ECE64819-6423-4187-BC52-A02992ED1945}.Release|Any CPU.Build.0 = Release|Any CPU 43 | EndGlobalSection 44 | GlobalSection(SolutionProperties) = preSolution 45 | HideSolutionNode = FALSE 46 | EndGlobalSection 47 | GlobalSection(ExtensibilityGlobals) = postSolution 48 | SolutionGuid = {0444D9DA-E5C8-4826-BE0F-4EB151D23592} 49 | EndGlobalSection 50 | EndGlobal 51 | -------------------------------------------------------------------------------- /Roy-T.AStar/AssemblyInfo.cs: -------------------------------------------------------------------------------- 1 | using System.Runtime.CompilerServices; 2 | [assembly: InternalsVisibleTo("Roy-T.AStar.Tests")] 3 | -------------------------------------------------------------------------------- /Roy-T.AStar/Collections/MinHeap.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using System.Collections.Generic; 3 | 4 | namespace Roy_T.AStar.Collections 5 | { 6 | // C# Adaptation of a min heap built for C++ by Robin Thomas 7 | // Original source code at: https://github.com/robin-thomas/min-heap 8 | 9 | internal sealed class MinHeap 10 | where T : IComparable 11 | { 12 | private readonly List Items; 13 | 14 | public MinHeap() 15 | { 16 | this.Items = new List(); 17 | } 18 | 19 | public int Count => this.Items.Count; 20 | 21 | public T Peek() => this.Items[0]; 22 | 23 | public void Insert(T item) 24 | { 25 | this.Items.Add(item); 26 | this.SortItem(item); 27 | } 28 | 29 | public T Extract() 30 | { 31 | var node = this.Items[0]; 32 | 33 | this.ReplaceFirstItemWithLastItem(); 34 | this.Heapify(0); 35 | 36 | return node; 37 | } 38 | 39 | public void Remove(T item) 40 | { 41 | if (this.Count < 2) 42 | { 43 | this.Clear(); 44 | } 45 | else 46 | { 47 | var index = this.Items.IndexOf(item); 48 | if (index >= 0) 49 | { 50 | this.Items[index] = this.Items[this.Items.Count - 1]; 51 | this.Items.RemoveAt(this.Items.Count - 1); 52 | 53 | this.Heapify(0); 54 | } 55 | } 56 | } 57 | 58 | public void Clear() => this.Items.Clear(); 59 | 60 | private void ReplaceFirstItemWithLastItem() 61 | { 62 | this.Items[0] = this.Items[this.Items.Count - 1]; 63 | this.Items.RemoveAt(this.Items.Count - 1); 64 | } 65 | 66 | private void SortItem(T item) 67 | { 68 | var index = this.Items.Count - 1; 69 | 70 | while (HasParent(index)) 71 | { 72 | var parentIndex = GetParentIndex(index); 73 | if (ItemAIsSmallerThanItemB(item, this.Items[parentIndex])) 74 | { 75 | this.Items[index] = this.Items[parentIndex]; 76 | index = parentIndex; 77 | } 78 | else 79 | { 80 | break; 81 | } 82 | } 83 | 84 | this.Items[index] = item; 85 | } 86 | 87 | private void Heapify(int startIndex) 88 | { 89 | var bestIndex = startIndex; 90 | 91 | if (this.HasLeftChild(startIndex)) 92 | { 93 | var leftChildIndex = GetLeftChildIndex(startIndex); 94 | if (ItemAIsSmallerThanItemB(this.Items[leftChildIndex], this.Items[bestIndex])) 95 | { 96 | bestIndex = leftChildIndex; 97 | } 98 | } 99 | 100 | if (this.HasRightChild(startIndex)) 101 | { 102 | var rightChildIndex = GetRightChildIndex(startIndex); 103 | if (ItemAIsSmallerThanItemB(this.Items[rightChildIndex], this.Items[bestIndex])) 104 | { 105 | bestIndex = rightChildIndex; 106 | } 107 | } 108 | 109 | if (bestIndex != startIndex) 110 | { 111 | var temp = this.Items[bestIndex]; 112 | this.Items[bestIndex] = this.Items[startIndex]; 113 | this.Items[startIndex] = temp; 114 | this.Heapify(bestIndex); 115 | } 116 | } 117 | 118 | private static bool ItemAIsSmallerThanItemB(T a, T b) => a.CompareTo(b) < 0; 119 | 120 | private static bool HasParent(int index) => index > 0; 121 | private bool HasLeftChild(int index) => GetLeftChildIndex(index) < this.Items.Count; 122 | private bool HasRightChild(int index) => GetRightChildIndex(index) < this.Items.Count; 123 | 124 | private static int GetParentIndex(int i) => (i - 1) / 2; 125 | private static int GetLeftChildIndex(int i) => (2 * i) + 1; 126 | private static int GetRightChildIndex(int i) => (2 * i) + 2; 127 | } 128 | } 129 | -------------------------------------------------------------------------------- /Roy-T.AStar/Graphs/Edge.cs: -------------------------------------------------------------------------------- 1 | using Roy_T.AStar.Primitives; 2 | 3 | namespace Roy_T.AStar.Graphs 4 | { 5 | public sealed class Edge : IEdge 6 | { 7 | private Velocity traversalVelocity; 8 | 9 | public Edge(INode start, INode end, Velocity traversalVelocity) 10 | { 11 | this.Start = start; 12 | this.End = end; 13 | 14 | this.Distance = Distance.BeweenPositions(start.Position, end.Position); 15 | this.TraversalVelocity = traversalVelocity; 16 | } 17 | 18 | public Velocity TraversalVelocity 19 | { 20 | get => this.traversalVelocity; 21 | set 22 | { 23 | this.traversalVelocity = value; 24 | this.TraversalDuration = this.Distance / value; 25 | } 26 | } 27 | 28 | public Duration TraversalDuration { get; private set; } 29 | 30 | public Distance Distance { get; } 31 | 32 | public INode Start { get; } 33 | public INode End { get; } 34 | 35 | public override string ToString() => $"{this.Start} -> {this.End} @ {this.TraversalVelocity}"; 36 | } 37 | } 38 | -------------------------------------------------------------------------------- /Roy-T.AStar/Graphs/IEdge.cs: -------------------------------------------------------------------------------- 1 | using Roy_T.AStar.Primitives; 2 | 3 | namespace Roy_T.AStar.Graphs 4 | { 5 | public interface IEdge 6 | { 7 | Velocity TraversalVelocity { get; set; } 8 | Duration TraversalDuration { get; } 9 | Distance Distance { get; } 10 | INode Start { get; } 11 | INode End { get; } 12 | } 13 | } -------------------------------------------------------------------------------- /Roy-T.AStar/Graphs/INode.cs: -------------------------------------------------------------------------------- 1 | using System.Collections.Generic; 2 | using Roy_T.AStar.Primitives; 3 | 4 | namespace Roy_T.AStar.Graphs 5 | { 6 | public interface INode 7 | { 8 | Position Position { get; } 9 | IList Incoming { get; } 10 | IList Outgoing { get; } 11 | } 12 | } 13 | -------------------------------------------------------------------------------- /Roy-T.AStar/Graphs/Node.cs: -------------------------------------------------------------------------------- 1 | using System.Collections.Generic; 2 | using Roy_T.AStar.Primitives; 3 | 4 | namespace Roy_T.AStar.Graphs 5 | { 6 | public sealed class Node : INode 7 | { 8 | public Node(Position position) 9 | { 10 | this.Incoming = new List(0); 11 | this.Outgoing = new List(0); 12 | 13 | this.Position = position; 14 | } 15 | 16 | public IList Incoming { get; } 17 | public IList Outgoing { get; } 18 | 19 | public Position Position { get; } 20 | 21 | public void Connect(INode node, Velocity traversalVelocity) 22 | { 23 | var edge = new Edge(this, node, traversalVelocity); 24 | this.Outgoing.Add(edge); 25 | node.Incoming.Add(edge); 26 | } 27 | 28 | public void Disconnect(INode node) 29 | { 30 | for (var i = this.Outgoing.Count - 1; i >= 0; i--) 31 | { 32 | var edge = this.Outgoing[i]; 33 | if (edge.End == node) 34 | { 35 | this.Outgoing.Remove(edge); 36 | node.Incoming.Remove(edge); 37 | } 38 | } 39 | } 40 | 41 | public override string ToString() => this.Position.ToString(); 42 | } 43 | } 44 | -------------------------------------------------------------------------------- /Roy-T.AStar/Grids/Grid.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using System.Collections.Generic; 3 | using Roy_T.AStar.Graphs; 4 | using Roy_T.AStar.Primitives; 5 | 6 | namespace Roy_T.AStar.Grids 7 | { 8 | public sealed class Grid 9 | { 10 | private readonly Node[,] Nodes; 11 | 12 | public static Grid CreateGridWithLateralConnections(GridSize gridSize, Size cellSize, Velocity traversalVelocity) 13 | { 14 | CheckArguments(gridSize, cellSize, traversalVelocity); 15 | 16 | var grid = new Grid(gridSize, cellSize); 17 | 18 | grid.CreateLateralConnections(traversalVelocity); 19 | 20 | return grid; 21 | } 22 | 23 | public static Grid CreateGridWithDiagonalConnections(GridSize gridSize, Size cellSize, Velocity traversalVelocity) 24 | { 25 | CheckArguments(gridSize, cellSize, traversalVelocity); 26 | 27 | var grid = new Grid(gridSize, cellSize); 28 | 29 | grid.CreateDiagonalConnections(traversalVelocity); 30 | 31 | return grid; 32 | } 33 | 34 | public static Grid CreateGridWithLateralAndDiagonalConnections(GridSize gridSize, Size cellSize, Velocity traversalVelocity) 35 | { 36 | CheckArguments(gridSize, cellSize, traversalVelocity); 37 | 38 | var grid = new Grid(gridSize, cellSize); 39 | 40 | grid.CreateDiagonalConnections(traversalVelocity); 41 | grid.CreateLateralConnections(traversalVelocity); 42 | 43 | return grid; 44 | } 45 | 46 | public static Grid CreateGridFrom2DArrayOfNodes(Node[,] nodes) 47 | { 48 | return new Grid(nodes); 49 | } 50 | 51 | private static void CheckGridSize(GridSize gridSize) 52 | { 53 | if (gridSize.Columns < 1) 54 | { 55 | throw new ArgumentOutOfRangeException( 56 | nameof(gridSize), $"Argument {nameof(gridSize.Columns)} is {gridSize.Columns} but should be >= 1"); 57 | } 58 | 59 | if (gridSize.Rows < 1) 60 | { 61 | throw new ArgumentOutOfRangeException( 62 | nameof(gridSize), $"Argument {nameof(gridSize.Rows)} is {gridSize.Rows} but should be >= 1"); 63 | } 64 | } 65 | 66 | private static void CheckArguments(GridSize gridSize, Size cellSize, Velocity defaultSpeed) 67 | { 68 | CheckGridSize(gridSize); 69 | 70 | 71 | if (cellSize.Width <= Distance.Zero) 72 | { 73 | throw new ArgumentOutOfRangeException( 74 | nameof(cellSize), $"Argument {nameof(cellSize.Width)} is {cellSize.Width} but should be > {Distance.Zero}"); 75 | } 76 | 77 | if (cellSize.Height <= Distance.Zero) 78 | { 79 | throw new ArgumentOutOfRangeException( 80 | nameof(cellSize), $"Argument {nameof(cellSize.Height)} is {cellSize.Height} but should be > {Distance.Zero}"); 81 | } 82 | 83 | if (defaultSpeed.MetersPerSecond <= 0.0f) 84 | { 85 | throw new ArgumentOutOfRangeException( 86 | nameof(defaultSpeed), $"Argument {nameof(defaultSpeed)} is {defaultSpeed} but should be > 0.0 m/s"); 87 | } 88 | } 89 | 90 | private Grid(Node[,] nodes) 91 | { 92 | this.GridSize = new GridSize(nodes.GetLength(0), nodes.GetLength(1)); 93 | CheckGridSize(this.GridSize); 94 | this.Nodes = nodes; 95 | } 96 | 97 | private Grid(GridSize gridSize, Size cellSize) 98 | { 99 | this.GridSize = gridSize; 100 | this.Nodes = new Node[gridSize.Columns, gridSize.Rows]; 101 | 102 | this.CreateNodes(cellSize); 103 | } 104 | 105 | private void CreateNodes(Size cellSize) 106 | { 107 | for (var x = 0; x < this.Columns; x++) 108 | { 109 | for (var y = 0; y < this.Rows; y++) 110 | { 111 | this.Nodes[x, y] = new Node(Position.FromOffset(cellSize.Width * x, cellSize.Height * y)); 112 | } 113 | } 114 | } 115 | 116 | private void CreateLateralConnections(Velocity defaultSpeed) 117 | { 118 | for (var x = 0; x < this.Columns; x++) 119 | { 120 | for (var y = 0; y < this.Rows; y++) 121 | { 122 | var node = this.Nodes[x, y]; 123 | 124 | if (x < this.Columns - 1) 125 | { 126 | var eastNode = this.Nodes[x + 1, y]; 127 | node.Connect(eastNode, defaultSpeed); 128 | eastNode.Connect(node, defaultSpeed); 129 | } 130 | 131 | if (y < this.Rows - 1) 132 | { 133 | var southNode = this.Nodes[x, y + 1]; 134 | node.Connect(southNode, defaultSpeed); 135 | southNode.Connect(node, defaultSpeed); 136 | } 137 | } 138 | } 139 | } 140 | 141 | private void CreateDiagonalConnections(Velocity defaultSpeed) 142 | { 143 | for (var x = 0; x < this.Columns; x++) 144 | { 145 | for (var y = 0; y < this.Rows; y++) 146 | { 147 | var node = this.Nodes[x, y]; 148 | 149 | if (x < this.Columns - 1 && y < this.Rows - 1) 150 | { 151 | var southEastNode = this.Nodes[x + 1, y + 1]; 152 | node.Connect(southEastNode, defaultSpeed); 153 | southEastNode.Connect(node, defaultSpeed); 154 | } 155 | 156 | if (x > 0 && y < this.Rows - 1) 157 | { 158 | var southWestNode = this.Nodes[x - 1, y + 1]; 159 | node.Connect(southWestNode, defaultSpeed); 160 | southWestNode.Connect(node, defaultSpeed); 161 | } 162 | } 163 | } 164 | } 165 | 166 | public GridSize GridSize { get; } 167 | 168 | public int Columns => this.GridSize.Columns; 169 | 170 | public int Rows => this.GridSize.Rows; 171 | 172 | public INode GetNode(GridPosition position) => this.Nodes[position.X, position.Y]; 173 | 174 | public IReadOnlyList GetAllNodes() 175 | { 176 | var list = new List(this.Columns * this.Rows); 177 | 178 | for (var x = 0; x < this.Columns; x++) 179 | { 180 | for (var y = 0; y < this.Rows; y++) 181 | { 182 | list.Add(this.Nodes[x, y]); 183 | } 184 | } 185 | 186 | return list; 187 | } 188 | 189 | public void DisconnectNode(GridPosition position) 190 | { 191 | var node = this.Nodes[position.X, position.Y]; 192 | 193 | foreach (var outgoingEdge in node.Outgoing) 194 | { 195 | var opposite = outgoingEdge.End; 196 | opposite.Incoming.Remove(outgoingEdge); 197 | } 198 | 199 | node.Outgoing.Clear(); 200 | 201 | foreach (var incomingEdge in node.Incoming) 202 | { 203 | var opposite = incomingEdge.Start; 204 | opposite.Outgoing.Remove(incomingEdge); 205 | } 206 | 207 | node.Incoming.Clear(); 208 | } 209 | 210 | public void RemoveDiagonalConnectionsIntersectingWithNode(GridPosition position) 211 | { 212 | var left = new GridPosition(position.X - 1, position.Y); 213 | var top = new GridPosition(position.X, position.Y - 1); 214 | var right = new GridPosition(position.X + 1, position.Y); 215 | var bottom = new GridPosition(position.X, position.Y + 1); 216 | 217 | if (this.IsInsideGrid(left) && this.IsInsideGrid(top)) 218 | { 219 | this.RemoveEdge(left, top); 220 | this.RemoveEdge(top, left); 221 | } 222 | 223 | if (this.IsInsideGrid(top) && this.IsInsideGrid(right)) 224 | { 225 | this.RemoveEdge(top, right); 226 | this.RemoveEdge(right, top); 227 | } 228 | 229 | if (this.IsInsideGrid(right) && this.IsInsideGrid(bottom)) 230 | { 231 | this.RemoveEdge(right, bottom); 232 | this.RemoveEdge(bottom, right); 233 | } 234 | 235 | if (this.IsInsideGrid(bottom) && this.IsInsideGrid(left)) 236 | { 237 | this.RemoveEdge(bottom, left); 238 | this.RemoveEdge(left, bottom); 239 | } 240 | } 241 | 242 | public void RemoveEdge(GridPosition from, GridPosition to) 243 | { 244 | var fromNode = this.Nodes[from.X, from.Y]; 245 | var toNode = this.Nodes[to.X, to.Y]; 246 | 247 | fromNode.Disconnect(toNode); 248 | } 249 | 250 | public void AddEdge(GridPosition from, GridPosition to, Velocity traversalVelocity) 251 | { 252 | var fromNode = this.Nodes[from.X, from.Y]; 253 | var toNode = this.Nodes[to.X, to.Y]; 254 | 255 | fromNode.Connect(toNode, traversalVelocity); 256 | } 257 | 258 | private bool IsInsideGrid(GridPosition position) => position.X >= 0 && position.X < this.Columns && position.Y >= 0 && position.Y < this.Rows; 259 | } 260 | } 261 | -------------------------------------------------------------------------------- /Roy-T.AStar/Paths/Path.cs: -------------------------------------------------------------------------------- 1 | using System.Collections.Generic; 2 | using Roy_T.AStar.Graphs; 3 | using Roy_T.AStar.Primitives; 4 | 5 | namespace Roy_T.AStar.Paths 6 | { 7 | public sealed class Path 8 | { 9 | public Path(PathType type, IReadOnlyList edges) 10 | { 11 | this.Type = type; 12 | this.Edges = edges; 13 | 14 | for (var i = 0; i < this.Edges.Count; i++) 15 | { 16 | this.Duration += this.Edges[i].TraversalDuration; 17 | this.Distance += this.Edges[i].Distance; 18 | } 19 | } 20 | 21 | public PathType Type { get; } 22 | 23 | public Duration Duration { get; } 24 | 25 | public IReadOnlyList Edges { get; } 26 | public Distance Distance { get; } 27 | } 28 | } 29 | -------------------------------------------------------------------------------- /Roy-T.AStar/Paths/PathFinder.cs: -------------------------------------------------------------------------------- 1 | using System.Collections.Generic; 2 | using System.Linq; 3 | using Roy_T.AStar.Collections; 4 | using Roy_T.AStar.Graphs; 5 | using Roy_T.AStar.Grids; 6 | using Roy_T.AStar.Primitives; 7 | 8 | namespace Roy_T.AStar.Paths 9 | { 10 | public sealed class PathFinder 11 | { 12 | private readonly MinHeap Interesting; 13 | private readonly Dictionary Nodes; 14 | private readonly PathReconstructor PathReconstructor; 15 | 16 | private PathFinderNode NodeClosestToGoal; 17 | 18 | public PathFinder() 19 | { 20 | this.Interesting = new MinHeap(); 21 | this.Nodes = new Dictionary(); 22 | this.PathReconstructor = new PathReconstructor(); 23 | } 24 | 25 | public Path FindPath(GridPosition start, GridPosition end, Grid grid) 26 | { 27 | var startNode = grid.GetNode(start); 28 | var endNode = grid.GetNode(end); 29 | 30 | var maximumVelocity = grid.GetAllNodes().SelectMany(n => n.Outgoing).Select(e => e.TraversalVelocity).Max(); 31 | 32 | return this.FindPath(startNode, endNode, maximumVelocity); 33 | } 34 | 35 | public Path FindPath(GridPosition start, GridPosition end, Grid grid, Velocity maximumVelocity) 36 | { 37 | var startNode = grid.GetNode(start); 38 | var endNode = grid.GetNode(end); 39 | 40 | return this.FindPath(startNode, endNode, maximumVelocity); 41 | } 42 | 43 | public Path FindPath(INode start, INode goal, Velocity maximumVelocity) 44 | { 45 | this.ResetState(); 46 | this.AddFirstNode(start, goal, maximumVelocity); 47 | 48 | while (this.Interesting.Count > 0) 49 | { 50 | var current = this.Interesting.Extract(); 51 | if (GoalReached(goal, current)) 52 | { 53 | return this.PathReconstructor.ConstructPathTo(current.Node, goal); 54 | } 55 | 56 | this.UpdateNodeClosestToGoal(current); 57 | 58 | foreach (var edge in current.Node.Outgoing) 59 | { 60 | var oppositeNode = edge.End; 61 | var costSoFar = current.DurationSoFar + edge.TraversalDuration; 62 | 63 | if (this.Nodes.TryGetValue(oppositeNode, out var node)) 64 | { 65 | this.UpdateExistingNode(goal, maximumVelocity, current, edge, oppositeNode, costSoFar, node); 66 | } 67 | else 68 | { 69 | this.InsertNode(oppositeNode, edge, goal, costSoFar, maximumVelocity); 70 | } 71 | } 72 | } 73 | 74 | return this.PathReconstructor.ConstructPathTo(this.NodeClosestToGoal.Node, goal); 75 | } 76 | 77 | private void ResetState() 78 | { 79 | this.Interesting.Clear(); 80 | this.Nodes.Clear(); 81 | this.PathReconstructor.Clear(); 82 | this.NodeClosestToGoal = null; 83 | } 84 | 85 | private void AddFirstNode(INode start, INode goal, Velocity maximumVelocity) 86 | { 87 | var head = new PathFinderNode(start, Duration.Zero, ExpectedDuration(start, goal, maximumVelocity)); 88 | this.Interesting.Insert(head); 89 | this.Nodes.Add(head.Node, head); 90 | this.NodeClosestToGoal = head; 91 | } 92 | 93 | private static bool GoalReached(INode goal, PathFinderNode current) => current.Node == goal; 94 | 95 | private void UpdateNodeClosestToGoal(PathFinderNode current) 96 | { 97 | if (current.ExpectedRemainingTime < this.NodeClosestToGoal.ExpectedRemainingTime) 98 | { 99 | this.NodeClosestToGoal = current; 100 | } 101 | } 102 | 103 | private void UpdateExistingNode(INode goal, Velocity maximumVelocity, PathFinderNode current, IEdge edge, INode oppositeNode, Duration costSoFar, PathFinderNode node) 104 | { 105 | if (node.DurationSoFar > costSoFar) 106 | { 107 | this.Interesting.Remove(node); 108 | this.InsertNode(oppositeNode, edge, goal, costSoFar, maximumVelocity); 109 | } 110 | } 111 | 112 | private void InsertNode(INode current, IEdge via, INode goal, Duration costSoFar, Velocity maximumVelocity) 113 | { 114 | this.PathReconstructor.SetCameFrom(current, via); 115 | 116 | var node = new PathFinderNode(current, costSoFar, ExpectedDuration(current, goal, maximumVelocity)); 117 | this.Interesting.Insert(node); 118 | this.Nodes[current] = node; 119 | } 120 | 121 | public static Duration ExpectedDuration(INode a, INode b, Velocity maximumVelocity) 122 | => Distance.BeweenPositions(a.Position, b.Position) / maximumVelocity; 123 | } 124 | } 125 | -------------------------------------------------------------------------------- /Roy-T.AStar/Paths/PathFinderNode.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using Roy_T.AStar.Graphs; 3 | using Roy_T.AStar.Primitives; 4 | 5 | namespace Roy_T.AStar.Paths 6 | { 7 | internal sealed class PathFinderNode : IComparable 8 | { 9 | public PathFinderNode(INode node, Duration durationSoFar, Duration expectedRemainingTime) 10 | { 11 | this.Node = node; 12 | this.DurationSoFar = durationSoFar; 13 | this.ExpectedRemainingTime = expectedRemainingTime; 14 | this.ExpectedTotalTime = this.DurationSoFar + this.ExpectedRemainingTime; 15 | } 16 | 17 | public INode Node { get; } 18 | public Duration DurationSoFar { get; } 19 | public Duration ExpectedRemainingTime { get; } 20 | public Duration ExpectedTotalTime { get; } 21 | 22 | public int CompareTo(PathFinderNode other) => this.ExpectedTotalTime.CompareTo(other.ExpectedTotalTime); 23 | public override string ToString() => $"📍{{{this.Node.Position.X}, {this.Node.Position.Y}}}, ⏱~{this.ExpectedTotalTime}"; 24 | } 25 | } 26 | -------------------------------------------------------------------------------- /Roy-T.AStar/Paths/PathReconstructor.cs: -------------------------------------------------------------------------------- 1 | using System.Collections.Generic; 2 | using Roy_T.AStar.Graphs; 3 | 4 | namespace Roy_T.AStar.Paths 5 | { 6 | internal sealed class PathReconstructor 7 | { 8 | private readonly Dictionary CameFrom; 9 | 10 | public PathReconstructor() 11 | { 12 | this.CameFrom = new Dictionary(); 13 | } 14 | 15 | public void SetCameFrom(INode node, IEdge via) 16 | => this.CameFrom[node] = via; 17 | 18 | public Path ConstructPathTo(INode node, INode goal) 19 | { 20 | var current = node; 21 | var edges = new List(); 22 | 23 | while (this.CameFrom.TryGetValue(current, out var via)) 24 | { 25 | edges.Add(via); 26 | current = via.Start; 27 | } 28 | 29 | edges.Reverse(); 30 | 31 | var type = node == goal ? PathType.Complete : PathType.ClosestApproach; 32 | return new Path(type, edges); 33 | } 34 | 35 | public void Clear() => this.CameFrom.Clear(); 36 | } 37 | } 38 | -------------------------------------------------------------------------------- /Roy-T.AStar/Paths/PathType.cs: -------------------------------------------------------------------------------- 1 | namespace Roy_T.AStar.Paths 2 | { 3 | public enum PathType 4 | { 5 | Complete, 6 | ClosestApproach 7 | } 8 | } 9 | -------------------------------------------------------------------------------- /Roy-T.AStar/Primitives/Distance.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | 3 | namespace Roy_T.AStar.Primitives 4 | { 5 | public struct Distance : IComparable, IEquatable 6 | { 7 | public static Distance Zero => new Distance(0); 8 | 9 | private Distance(float meters) 10 | { 11 | this.Meters = meters; 12 | } 13 | 14 | public float Meters { get; } 15 | 16 | public static Distance FromMeters(float meters) => new Distance(meters); 17 | 18 | public static Distance BeweenPositions(Position a, Position b) 19 | { 20 | var sX = a.X; 21 | var sY = a.Y; 22 | var eX = b.X; 23 | var eY = b.Y; 24 | 25 | var d0 = (eX - sX) * (eX - sX); 26 | var d1 = (eY - sY) * (eY - sY); 27 | 28 | return FromMeters((float)Math.Sqrt(d0 + d1)); 29 | } 30 | 31 | public static Distance operator +(Distance a, Distance b) 32 | => new Distance(a.Meters + b.Meters); 33 | 34 | public static Distance operator -(Distance a, Distance b) 35 | => new Distance(a.Meters - b.Meters); 36 | 37 | public static Distance operator *(Distance a, float b) 38 | => new Distance(a.Meters * b); 39 | 40 | public static Distance operator /(Distance a, float b) 41 | => new Distance(a.Meters / b); 42 | 43 | public static bool operator >(Distance a, Distance b) 44 | => a.Meters > b.Meters; 45 | 46 | public static bool operator <(Distance a, Distance b) 47 | => a.Meters < b.Meters; 48 | 49 | public static bool operator >=(Distance a, Distance b) 50 | => a.Meters >= b.Meters; 51 | 52 | public static bool operator <=(Distance a, Distance b) 53 | => a.Meters <= b.Meters; 54 | 55 | public static bool operator ==(Distance a, Distance b) 56 | => a.Equals(b); 57 | 58 | public static bool operator !=(Distance a, Distance b) 59 | => !a.Equals(b); 60 | 61 | public static Duration operator /(Distance distance, Velocity velocity) 62 | => Duration.FromSeconds(distance.Meters / velocity.MetersPerSecond); 63 | 64 | public override string ToString() => $"{this.Meters:F2}m"; 65 | 66 | public override bool Equals(object obj) => obj is Distance distance && this.Equals(distance); 67 | public bool Equals(Distance other) => this.Meters == other.Meters; 68 | 69 | public int CompareTo(Distance other) => this.Meters.CompareTo(other.Meters); 70 | 71 | public override int GetHashCode() => -1609761766 + this.Meters.GetHashCode(); 72 | } 73 | } 74 | -------------------------------------------------------------------------------- /Roy-T.AStar/Primitives/Duration.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | 3 | namespace Roy_T.AStar.Primitives 4 | { 5 | public struct Duration : IComparable, IEquatable 6 | { 7 | public static Duration Zero => new Duration(0); 8 | 9 | private Duration(float seconds) 10 | { 11 | this.Seconds = seconds; 12 | } 13 | 14 | public float Seconds { get; } 15 | 16 | public static Duration FromSeconds(float seconds) => new Duration(seconds); 17 | 18 | public static Duration operator +(Duration a, Duration b) 19 | => new Duration(a.Seconds + b.Seconds); 20 | 21 | public static Duration operator -(Duration a, Duration b) 22 | => new Duration(a.Seconds - b.Seconds); 23 | 24 | public static bool operator >(Duration a, Duration b) 25 | => a.Seconds > b.Seconds; 26 | 27 | public static bool operator <(Duration a, Duration b) 28 | => a.Seconds < b.Seconds; 29 | 30 | public static bool operator >=(Duration a, Duration b) 31 | => a.Seconds >= b.Seconds; 32 | 33 | public static bool operator <=(Duration a, Duration b) 34 | => a.Seconds <= b.Seconds; 35 | 36 | public static bool operator ==(Duration a, Duration b) 37 | => a.Equals(b); 38 | 39 | public static bool operator !=(Duration a, Duration b) 40 | => !a.Equals(b); 41 | 42 | public override string ToString() => $"{this.Seconds:F2}s"; 43 | 44 | public override bool Equals(object obj) => obj is Duration duration && this.Equals(duration); 45 | 46 | public bool Equals(Duration other) => this.Seconds == other.Seconds; 47 | 48 | public int CompareTo(Duration other) => this.Seconds.CompareTo(other.Seconds); 49 | 50 | public override int GetHashCode() => -1609761766 + this.Seconds.GetHashCode(); 51 | } 52 | } 53 | -------------------------------------------------------------------------------- /Roy-T.AStar/Primitives/GridPosition.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | 3 | namespace Roy_T.AStar.Primitives 4 | { 5 | public struct GridPosition : IEquatable 6 | { 7 | public static GridPosition Zero => new GridPosition(0, 0); 8 | 9 | public GridPosition(int x, int y) 10 | { 11 | this.X = x; 12 | this.Y = y; 13 | } 14 | 15 | public int X { get; } 16 | public int Y { get; } 17 | 18 | public static bool operator ==(GridPosition a, GridPosition b) 19 | => a.Equals(b); 20 | 21 | public static bool operator !=(GridPosition a, GridPosition b) 22 | => !a.Equals(b); 23 | 24 | public override string ToString() => $"({this.X}, {this.Y})"; 25 | 26 | public override bool Equals(object obj) => obj is GridPosition GridPosition && this.Equals(GridPosition); 27 | 28 | public bool Equals(GridPosition other) => this.X == other.X && this.Y == other.Y; 29 | 30 | public override int GetHashCode() => -1609761766 + this.X + this.Y; 31 | } 32 | } 33 | -------------------------------------------------------------------------------- /Roy-T.AStar/Primitives/GridSize.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | 3 | namespace Roy_T.AStar.Primitives 4 | { 5 | public struct GridSize : IEquatable 6 | { 7 | public GridSize(int columns, int rows) 8 | { 9 | this.Columns = columns; 10 | this.Rows = rows; 11 | } 12 | 13 | public int Columns { get; } 14 | public int Rows { get; } 15 | 16 | public static bool operator ==(GridSize a, GridSize b) 17 | => a.Equals(b); 18 | 19 | public static bool operator !=(GridSize a, GridSize b) 20 | => !a.Equals(b); 21 | 22 | public override string ToString() => $"(columns: {this.Columns}, rows: {this.Rows})"; 23 | 24 | public override bool Equals(object obj) => obj is GridSize GridSize && this.Equals(GridSize); 25 | 26 | public bool Equals(GridSize other) => this.Columns == other.Columns && this.Rows == other.Rows; 27 | 28 | public override int GetHashCode() => -1609761766 + this.Columns.GetHashCode() + this.Rows.GetHashCode(); 29 | } 30 | } 31 | -------------------------------------------------------------------------------- /Roy-T.AStar/Primitives/Position.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | 3 | namespace Roy_T.AStar.Primitives 4 | { 5 | public struct Position : IEquatable 6 | { 7 | public static Position Zero => new Position(0, 0); 8 | 9 | public Position(float x, float y) 10 | { 11 | this.X = x; 12 | this.Y = y; 13 | } 14 | 15 | public static Position FromOffset(Distance xDistanceFromOrigin, Distance yDistanceFromOrigin) 16 | => new Position(xDistanceFromOrigin.Meters, yDistanceFromOrigin.Meters); 17 | 18 | public float X { get; } 19 | public float Y { get; } 20 | 21 | public static bool operator ==(Position a, Position b) 22 | => a.Equals(b); 23 | 24 | public static bool operator !=(Position a, Position b) 25 | => !a.Equals(b); 26 | 27 | public override string ToString() => $"({this.X:F2}, {this.Y:F2})"; 28 | 29 | public override bool Equals(object obj) => obj is Position position && this.Equals(position); 30 | 31 | public bool Equals(Position other) => this.X == other.X && this.Y == other.Y; 32 | 33 | public override int GetHashCode() => -1609761766 + this.X.GetHashCode() + this.Y.GetHashCode(); 34 | } 35 | } 36 | -------------------------------------------------------------------------------- /Roy-T.AStar/Primitives/Size.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | 3 | namespace Roy_T.AStar.Primitives 4 | { 5 | public struct Size : IEquatable 6 | { 7 | public Size(Distance width, Distance height) 8 | { 9 | this.Width = width; 10 | this.Height = height; 11 | } 12 | 13 | public Distance Width { get; } 14 | public Distance Height { get; } 15 | 16 | public static bool operator ==(Size a, Size b) 17 | => a.Equals(b); 18 | 19 | public static bool operator !=(Size a, Size b) 20 | => !a.Equals(b); 21 | 22 | public override string ToString() => $"(width: {this.Width}, height: {this.Height})"; 23 | 24 | public override bool Equals(object obj) => obj is Size Size && this.Equals(Size); 25 | 26 | public bool Equals(Size other) => this.Width == other.Width && this.Height == other.Height; 27 | 28 | public override int GetHashCode() => -1609761766 + this.Width.GetHashCode() + this.Height.GetHashCode(); 29 | } 30 | } 31 | -------------------------------------------------------------------------------- /Roy-T.AStar/Primitives/Velocity.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | 3 | namespace Roy_T.AStar.Primitives 4 | { 5 | public struct Velocity : IComparable, IEquatable 6 | { 7 | private Velocity(float metersPerSecond) 8 | { 9 | this.MetersPerSecond = metersPerSecond; 10 | } 11 | 12 | public float MetersPerSecond { get; } 13 | 14 | public float KilometersPerHour => this.MetersPerSecond * 3.6f; 15 | 16 | 17 | public static Velocity FromMetersPerSecond(float metersPerSecond) 18 | => new Velocity(metersPerSecond); 19 | 20 | public static Velocity FromKilometersPerHour(float kilometersPerHour) 21 | => new Velocity(kilometersPerHour / 3.6f); 22 | 23 | public static Velocity operator +(Velocity a, Velocity b) 24 | => new Velocity(a.MetersPerSecond + b.MetersPerSecond); 25 | 26 | public static Velocity operator -(Velocity a, Velocity b) 27 | => new Velocity(a.MetersPerSecond - b.MetersPerSecond); 28 | 29 | public static bool operator >(Velocity a, Velocity b) 30 | => a.MetersPerSecond > b.MetersPerSecond; 31 | 32 | public static bool operator <(Velocity a, Velocity b) 33 | => a.MetersPerSecond < b.MetersPerSecond; 34 | 35 | public static bool operator >=(Velocity a, Velocity b) 36 | => a.MetersPerSecond >= b.MetersPerSecond; 37 | 38 | public static bool operator <=(Velocity a, Velocity b) 39 | => a.MetersPerSecond <= b.MetersPerSecond; 40 | 41 | public static bool operator ==(Velocity a, Velocity b) 42 | => a.Equals(b); 43 | 44 | public static bool operator !=(Velocity a, Velocity b) 45 | => !a.Equals(b); 46 | 47 | public override string ToString() => $"{this.MetersPerSecond:F2} m/s"; 48 | 49 | public override bool Equals(object obj) => obj is Velocity velocity && this.MetersPerSecond == velocity.MetersPerSecond; 50 | 51 | public bool Equals(Velocity other) => this.MetersPerSecond == other.MetersPerSecond; 52 | 53 | public int CompareTo(Velocity other) => this.MetersPerSecond.CompareTo(other.MetersPerSecond); 54 | 55 | public override int GetHashCode() => -1419927970 + this.MetersPerSecond.GetHashCode(); 56 | } 57 | } 58 | -------------------------------------------------------------------------------- /Roy-T.AStar/Roy-T.AStar.csproj: -------------------------------------------------------------------------------- 1 |  2 | 3 | 4 | netstandard2.0 5 | Roy_T.AStar 6 | RoyT.AStar 7 | 3.0.2 8 | Roy Triesscheijn 9 | 10 | A fast 2D path finding library based on the A* algorithm. Works with both grids and graphs. Supports .NETStandard 2.0 and higher. This library has no external dependencies. 11 | LICENSE 12 | https://github.com/roy-t/AStar/ 13 | https://github.com/roy-t/AStar/ 14 | true 15 | 3.0.2.0 16 | 3.0.2.0 17 | 18 | 19 | 20 | 21 | True 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /Roy-T.AStar/Serialization/EdgeDto.cs: -------------------------------------------------------------------------------- 1 | namespace Roy_T.AStar.Serialization 2 | { 3 | public class EdgeDto 4 | { 5 | public VelocityDto TraversalVelocity { get; set; } 6 | 7 | public GridPositionDto Start { get; set; } 8 | 9 | public GridPositionDto End { get; set; } 10 | } 11 | } 12 | -------------------------------------------------------------------------------- /Roy-T.AStar/Serialization/GridDto.cs: -------------------------------------------------------------------------------- 1 | namespace Roy_T.AStar.Serialization 2 | { 3 | public class GridDto 4 | { 5 | public NodeDto[][] Nodes { get; set; } 6 | } 7 | } 8 | -------------------------------------------------------------------------------- /Roy-T.AStar/Serialization/GridPositionDto.cs: -------------------------------------------------------------------------------- 1 | namespace Roy_T.AStar.Serialization 2 | { 3 | public class GridPositionDto 4 | { 5 | public int X { get; set; } 6 | public int Y { get; set; } 7 | } 8 | } 9 | -------------------------------------------------------------------------------- /Roy-T.AStar/Serialization/GridSerializer.cs: -------------------------------------------------------------------------------- 1 | using Roy_T.AStar.Graphs; 2 | using Roy_T.AStar.Grids; 3 | using Roy_T.AStar.Primitives; 4 | using System.Collections.Generic; 5 | using System.IO; 6 | using System.Linq; 7 | using System.Xml.Serialization; 8 | 9 | namespace Roy_T.AStar.Serialization 10 | { 11 | public static class GridSerializer 12 | { 13 | public static string SerializeGrid(Grid grid) 14 | { 15 | var gridDto = grid.ToDto(); 16 | XmlSerializer xmlSerializer = new XmlSerializer(gridDto.GetType()); 17 | 18 | using (StringWriter textWriter = new StringWriter()) 19 | { 20 | xmlSerializer.Serialize(textWriter, gridDto); 21 | return textWriter.ToString(); 22 | } 23 | } 24 | 25 | public static Grid DeSerializeGrid(string gridString) 26 | { 27 | XmlSerializer xmlSerializer = new XmlSerializer(typeof(GridDto)); 28 | using (StringReader textReader = new StringReader(gridString)) 29 | { 30 | var gridDto = (GridDto)xmlSerializer.Deserialize(textReader); 31 | Node[,] nodes = new Node[gridDto.Nodes.Length, gridDto.Nodes[0].Length]; 32 | for (int i = 0; i < gridDto.Nodes.Length; i++) 33 | { 34 | for (int j = 0; j < gridDto.Nodes[0].Length; j++) 35 | { 36 | var nodeDto = gridDto.Nodes[i][j]; 37 | var node = new Node(nodeDto.Position.FromDto()); 38 | nodes[i, j] = node; 39 | } 40 | } 41 | 42 | for (int i = 0; i < gridDto.Nodes.Length; i++) 43 | { 44 | for (int j = 0; j < gridDto.Nodes[0].Length; j++) 45 | { 46 | var nodeDto = gridDto.Nodes[i][j]; 47 | var node = nodes[i, j]; 48 | foreach (var outGoingEdge in nodeDto.OutGoingEdges) 49 | { 50 | var toNode = nodes[outGoingEdge.End.X, outGoingEdge.End.Y]; 51 | node.Connect(toNode, outGoingEdge.TraversalVelocity.FromDto()); 52 | } 53 | } 54 | } 55 | 56 | return Grid.CreateGridFrom2DArrayOfNodes(nodes); 57 | } 58 | } 59 | 60 | private static GridDto ToDto(this Grid grid) 61 | { 62 | var nodeToGridPositionDict = new Dictionary(); 63 | NodeDto[][] nodes = new NodeDto[grid.Columns][]; 64 | for (int i = 0; i < grid.Columns; i++) 65 | { 66 | for (int j = 0; j < grid.Rows; j++) 67 | { 68 | var gridPosition = new GridPosition(i, j); 69 | nodeToGridPositionDict[grid.GetNode(gridPosition)] = gridPosition; 70 | } 71 | } 72 | 73 | for (int i = 0; i < grid.Columns; i++) 74 | { 75 | nodes[i] = new NodeDto[grid.Rows]; 76 | for (int j = 0; j < grid.Rows; j++) 77 | { 78 | nodes[i][j] = grid.GetNode(new GridPosition(i, j)).ToDto(nodeToGridPositionDict); 79 | } 80 | } 81 | 82 | return new GridDto 83 | { 84 | Nodes = nodes 85 | }; 86 | } 87 | 88 | private static NodeDto ToDto(this INode node, Dictionary nodeToGridPositionDict) 89 | { 90 | return new NodeDto 91 | { 92 | Position = node.Position.ToDto(), 93 | GridPosition = nodeToGridPositionDict[node].ToDto(), 94 | OutGoingEdges = node.Outgoing.ToDto(nodeToGridPositionDict), 95 | IncomingEdges = node.Incoming.ToDto(nodeToGridPositionDict), 96 | }; 97 | } 98 | 99 | private static List ToDto(this IList edge, Dictionary nodeToGridPositionDict) 100 | { 101 | return edge.Select(e => e.ToDto(nodeToGridPositionDict)).ToList(); 102 | } 103 | 104 | private static EdgeDto ToDto(this IEdge edge, Dictionary nodeToGridPositionDict) 105 | { 106 | return new EdgeDto 107 | { 108 | TraversalVelocity = edge.TraversalVelocity.ToDto(), 109 | Start = nodeToGridPositionDict[edge.Start].ToDto(), 110 | End = nodeToGridPositionDict[edge.End].ToDto() 111 | }; 112 | } 113 | 114 | private static VelocityDto ToDto(this Velocity velocity) 115 | { 116 | return new VelocityDto 117 | { 118 | MetersPerSecond = velocity.MetersPerSecond 119 | }; 120 | } 121 | 122 | private static Velocity FromDto(this VelocityDto velocity) 123 | { 124 | return Velocity.FromMetersPerSecond(velocity.MetersPerSecond); 125 | } 126 | 127 | private static PositionDto ToDto(this Position position) 128 | { 129 | return new PositionDto 130 | { 131 | X = position.X, 132 | Y = position.Y 133 | }; 134 | } 135 | 136 | private static Position FromDto(this PositionDto position) 137 | { 138 | return new Position(position.X, position.Y); 139 | } 140 | 141 | private static GridPositionDto ToDto(this GridPosition position) 142 | { 143 | return new GridPositionDto 144 | { 145 | X = position.X, 146 | Y = position.Y 147 | }; 148 | } 149 | 150 | private static GridPosition FromDto(this GridPositionDto position) 151 | { 152 | return new GridPosition(position.X, position.Y); 153 | } 154 | } 155 | } -------------------------------------------------------------------------------- /Roy-T.AStar/Serialization/NodeDto.cs: -------------------------------------------------------------------------------- 1 | using System.Collections.Generic; 2 | 3 | namespace Roy_T.AStar.Serialization 4 | { 5 | public class NodeDto 6 | { 7 | public PositionDto Position { get; set; } 8 | public GridPositionDto GridPosition { get; set; } 9 | public List IncomingEdges { get; set; } 10 | public List OutGoingEdges { get; set; } 11 | } 12 | } 13 | -------------------------------------------------------------------------------- /Roy-T.AStar/Serialization/PositionDto.cs: -------------------------------------------------------------------------------- 1 | namespace Roy_T.AStar.Serialization 2 | { 3 | public class PositionDto 4 | { 5 | public float X { get; set; } 6 | public float Y { get; set; } 7 | } 8 | } 9 | -------------------------------------------------------------------------------- /Roy-T.AStar/Serialization/VelocityDto.cs: -------------------------------------------------------------------------------- 1 | namespace Roy_T.AStar.Serialization 2 | { 3 | public class VelocityDto 4 | { 5 | public float MetersPerSecond { get; set; } 6 | } 7 | } 8 | -------------------------------------------------------------------------------- /viewer.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/roy-t/AStar/3d57efaa83c8cd217e4973fd4738c57e2f694bd2/viewer.png --------------------------------------------------------------------------------