commit 899244732379a7597260b776306bd7e584efa0fb Author: Stone_Red <56473591+Stone-Red-Code@users.noreply.github.com> Date: Fri Feb 14 01:17:55 2025 +0100 Initial commit diff --git a/.gitattributes b/.gitattributes new file mode 100644 index 0000000..dfe0770 --- /dev/null +++ b/.gitattributes @@ -0,0 +1,2 @@ +# Auto detect text files and perform LF normalization +* text=auto diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..8a30d25 --- /dev/null +++ b/.gitignore @@ -0,0 +1,398 @@ +## Ignore Visual Studio temporary files, build results, and +## files generated by popular Visual Studio add-ons. +## +## Get latest from https://github.com/github/gitignore/blob/main/VisualStudio.gitignore + +# User-specific files +*.rsuser +*.suo +*.user +*.userosscache +*.sln.docstates + +# User-specific files (MonoDevelop/Xamarin Studio) +*.userprefs + +# Mono auto generated files +mono_crash.* + +# Build results +[Dd]ebug/ +[Dd]ebugPublic/ +[Rr]elease/ +[Rr]eleases/ +x64/ +x86/ +[Ww][Ii][Nn]32/ +[Aa][Rr][Mm]/ +[Aa][Rr][Mm]64/ +bld/ +[Bb]in/ +[Oo]bj/ +[Ll]og/ +[Ll]ogs/ + +# Visual Studio 2015/2017 cache/options directory +.vs/ +# Uncomment if you have tasks that create the project's static files in wwwroot +#wwwroot/ + +# Visual Studio 2017 auto generated files +Generated\ Files/ + +# MSTest test Results +[Tt]est[Rr]esult*/ +[Bb]uild[Ll]og.* + +# NUnit +*.VisualState.xml +TestResult.xml +nunit-*.xml + +# Build Results of an ATL Project +[Dd]ebugPS/ +[Rr]eleasePS/ +dlldata.c + +# Benchmark Results +BenchmarkDotNet.Artifacts/ + +# .NET Core +project.lock.json +project.fragment.lock.json +artifacts/ + +# ASP.NET Scaffolding +ScaffoldingReadMe.txt + +# StyleCop +StyleCopReport.xml + +# Files built by Visual Studio +*_i.c +*_p.c +*_h.h +*.ilk +*.meta +*.obj +*.iobj +*.pch +*.pdb +*.ipdb +*.pgc +*.pgd +*.rsp +*.sbr +*.tlb +*.tli +*.tlh +*.tmp +*.tmp_proj +*_wpftmp.csproj +*.log +*.tlog +*.vspscc +*.vssscc +.builds +*.pidb +*.svclog +*.scc + +# Chutzpah Test files +_Chutzpah* + +# Visual C++ cache files +ipch/ +*.aps +*.ncb +*.opendb +*.opensdf +*.sdf +*.cachefile +*.VC.db +*.VC.VC.opendb + +# Visual Studio profiler +*.psess +*.vsp +*.vspx +*.sap + +# Visual Studio Trace Files +*.e2e + +# TFS 2012 Local Workspace +$tf/ + +# Guidance Automation Toolkit +*.gpState + +# ReSharper is a .NET coding add-in +_ReSharper*/ +*.[Rr]e[Ss]harper +*.DotSettings.user + +# TeamCity is a build add-in +_TeamCity* + +# DotCover is a Code Coverage Tool +*.dotCover + +# AxoCover is a Code Coverage Tool +.axoCover/* +!.axoCover/settings.json + +# Coverlet is a free, cross platform Code Coverage Tool +coverage*.json +coverage*.xml +coverage*.info + +# Visual Studio code coverage results +*.coverage +*.coveragexml + +# NCrunch +_NCrunch_* +.*crunch*.local.xml +nCrunchTemp_* + +# MightyMoose +*.mm.* +AutoTest.Net/ + +# Web workbench (sass) +.sass-cache/ + +# Installshield output folder +[Ee]xpress/ + +# DocProject is a documentation generator add-in +DocProject/buildhelp/ +DocProject/Help/*.HxT +DocProject/Help/*.HxC +DocProject/Help/*.hhc +DocProject/Help/*.hhk +DocProject/Help/*.hhp +DocProject/Help/Html2 +DocProject/Help/html + +# Click-Once directory +publish/ + +# Publish Web Output +*.[Pp]ublish.xml +*.azurePubxml +# Note: Comment the next line if you want to checkin your web deploy settings, +# but database connection strings (with potential passwords) will be unencrypted +*.pubxml +*.publishproj + +# Microsoft Azure Web App publish settings. Comment the next line if you want to +# checkin your Azure Web App publish settings, but sensitive information contained +# in these scripts will be unencrypted +PublishScripts/ + +# NuGet Packages +*.nupkg +# NuGet Symbol Packages +*.snupkg +# The packages folder can be ignored because of Package Restore +**/[Pp]ackages/* +# except build/, which is used as an MSBuild target. +!**/[Pp]ackages/build/ +# Uncomment if necessary however generally it will be regenerated when needed +#!**/[Pp]ackages/repositories.config +# NuGet v3's project.json files produces more ignorable files +*.nuget.props +*.nuget.targets + +# Microsoft Azure Build Output +csx/ +*.build.csdef + +# Microsoft Azure Emulator +ecf/ +rcf/ + +# Windows Store app package directories and files +AppPackages/ +BundleArtifacts/ +Package.StoreAssociation.xml +_pkginfo.txt +*.appx +*.appxbundle +*.appxupload + +# Visual Studio cache files +# files ending in .cache can be ignored +*.[Cc]ache +# but keep track of directories ending in .cache +!?*.[Cc]ache/ + +# Others +ClientBin/ +~$* +*~ +*.dbmdl +*.dbproj.schemaview +*.jfm +*.pfx +*.publishsettings +orleans.codegen.cs + +# Including strong name files can present a security risk +# (https://github.com/github/gitignore/pull/2483#issue-259490424) +#*.snk + +# Since there are multiple workflows, uncomment next line to ignore bower_components +# (https://github.com/github/gitignore/pull/1529#issuecomment-104372622) +#bower_components/ + +# RIA/Silverlight projects +Generated_Code/ + +# Backup & report files from converting an old project file +# to a newer Visual Studio version. Backup files are not needed, +# because we have git ;-) +_UpgradeReport_Files/ +Backup*/ +UpgradeLog*.XML +UpgradeLog*.htm +ServiceFabricBackup/ +*.rptproj.bak + +# SQL Server files +*.mdf +*.ldf +*.ndf + +# Business Intelligence projects +*.rdl.data +*.bim.layout +*.bim_*.settings +*.rptproj.rsuser +*- [Bb]ackup.rdl +*- [Bb]ackup ([0-9]).rdl +*- [Bb]ackup ([0-9][0-9]).rdl + +# Microsoft Fakes +FakesAssemblies/ + +# GhostDoc plugin setting file +*.GhostDoc.xml + +# Node.js Tools for Visual Studio +.ntvs_analysis.dat +node_modules/ + +# Visual Studio 6 build log +*.plg + +# Visual Studio 6 workspace options file +*.opt + +# Visual Studio 6 auto-generated workspace file (contains which files were open etc.) +*.vbw + +# Visual Studio 6 auto-generated project file (contains which files were open etc.) +*.vbp + +# Visual Studio 6 workspace and project file (working project files containing files to include in project) +*.dsw +*.dsp + +# Visual Studio 6 technical files +*.ncb +*.aps + +# Visual Studio LightSwitch build output +**/*.HTMLClient/GeneratedArtifacts +**/*.DesktopClient/GeneratedArtifacts +**/*.DesktopClient/ModelManifest.xml +**/*.Server/GeneratedArtifacts +**/*.Server/ModelManifest.xml +_Pvt_Extensions + +# Paket dependency manager +.paket/paket.exe +paket-files/ + +# FAKE - F# Make +.fake/ + +# CodeRush personal settings +.cr/personal + +# Python Tools for Visual Studio (PTVS) +__pycache__/ +*.pyc + +# Cake - Uncomment if you are using it +# tools/** +# !tools/packages.config + +# Tabs Studio +*.tss + +# Telerik's JustMock configuration file +*.jmconfig + +# BizTalk build output +*.btp.cs +*.btm.cs +*.odx.cs +*.xsd.cs + +# OpenCover UI analysis results +OpenCover/ + +# Azure Stream Analytics local run output +ASALocalRun/ + +# MSBuild Binary and Structured Log +*.binlog + +# NVidia Nsight GPU debugger configuration file +*.nvuser + +# MFractors (Xamarin productivity tool) working folder +.mfractor/ + +# Local History for Visual Studio +.localhistory/ + +# Visual Studio History (VSHistory) files +.vshistory/ + +# BeatPulse healthcheck temp database +healthchecksdb + +# Backup folder for Package Reference Convert tool in Visual Studio 2017 +MigrationBackup/ + +# Ionide (cross platform F# VS Code tools) working folder +.ionide/ + +# Fody - auto-generated XML schema +FodyWeavers.xsd + +# VS Code files for those working on multiple tools +.vscode/* +!.vscode/settings.json +!.vscode/tasks.json +!.vscode/launch.json +!.vscode/extensions.json +*.code-workspace + +# Local History for Visual Studio Code +.history/ + +# Windows Installer files from build outputs +*.cab +*.msi +*.msix +*.msm +*.msp + +# JetBrains Rider +*.sln.iml diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..ccfb086 --- /dev/null +++ b/LICENSE @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2025 Stone_Red + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/README.md b/README.md new file mode 100644 index 0000000..08c3d21 --- /dev/null +++ b/README.md @@ -0,0 +1,2 @@ +# LargeGridPathfinding + A zone-based pathfinding algorithm that efficiently handles concurrent navigation of numerous agents across large, grid-based maps diff --git a/src/LargeGridPathfinding.sln b/src/LargeGridPathfinding.sln new file mode 100644 index 0000000..d61d4f2 --- /dev/null +++ b/src/LargeGridPathfinding.sln @@ -0,0 +1,22 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Version 17 +VisualStudioVersion = 17.12.35707.178 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "LargeGridPathfinding", "LargeGridPathfinding\LargeGridPathfinding.csproj", "{4F58A5EA-B389-4AFE-979C-0F0B0AAAD86C}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Release|Any CPU = Release|Any CPU + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {4F58A5EA-B389-4AFE-979C-0F0B0AAAD86C}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {4F58A5EA-B389-4AFE-979C-0F0B0AAAD86C}.Debug|Any CPU.Build.0 = Debug|Any CPU + {4F58A5EA-B389-4AFE-979C-0F0B0AAAD86C}.Release|Any CPU.ActiveCfg = Release|Any CPU + {4F58A5EA-B389-4AFE-979C-0F0B0AAAD86C}.Release|Any CPU.Build.0 = Release|Any CPU + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/src/LargeGridPathfinding/.config/dotnet-tools.json b/src/LargeGridPathfinding/.config/dotnet-tools.json new file mode 100644 index 0000000..84ff03d --- /dev/null +++ b/src/LargeGridPathfinding/.config/dotnet-tools.json @@ -0,0 +1,36 @@ +{ + "version": 1, + "isRoot": true, + "tools": { + "dotnet-mgcb": { + "version": "3.8.2.1105", + "commands": [ + "mgcb" + ] + }, + "dotnet-mgcb-editor": { + "version": "3.8.2.1105", + "commands": [ + "mgcb-editor" + ] + }, + "dotnet-mgcb-editor-linux": { + "version": "3.8.2.1105", + "commands": [ + "mgcb-editor-linux" + ] + }, + "dotnet-mgcb-editor-windows": { + "version": "3.8.2.1105", + "commands": [ + "mgcb-editor-windows" + ] + }, + "dotnet-mgcb-editor-mac": { + "version": "3.8.2.1105", + "commands": [ + "mgcb-editor-mac" + ] + } + } +} \ No newline at end of file diff --git a/src/LargeGridPathfinding/Agent.cs b/src/LargeGridPathfinding/Agent.cs new file mode 100644 index 0000000..aa0b962 --- /dev/null +++ b/src/LargeGridPathfinding/Agent.cs @@ -0,0 +1,19 @@ +using Microsoft.Xna.Framework; + +using System; +using System.Collections.Generic; + +namespace LargeGridPathfinding; + +internal class Agent(Vector2 position) +{ + public Vector2 Position { get; set; } = position; + + public Point GridPosition => new Point((int)Math.Round(Position.X), (int)Math.Round(Position.Y)); + + public Vector2 NextPosition { get; set; } + + public Point? Destination { get; set; } + + public List? Path { get; set; } +} \ No newline at end of file diff --git a/src/LargeGridPathfinding/Content/Content.mgcb b/src/LargeGridPathfinding/Content/Content.mgcb new file mode 100644 index 0000000..9f2147c --- /dev/null +++ b/src/LargeGridPathfinding/Content/Content.mgcb @@ -0,0 +1,22 @@ + +#----------------------------- Global Properties ----------------------------# + +/outputDir:bin/$(Platform) +/intermediateDir:obj/$(Platform) +/platform:DesktopGL +/config: +/profile:Reach +/compress:False + +#-------------------------------- References --------------------------------# + + +#---------------------------------- Content ---------------------------------# + +#begin File1.spritefont +/importer:FontDescriptionImporter +/processor:FontDescriptionProcessor +/processorParam:PremultiplyAlpha=True +/processorParam:TextureFormat=Compressed +/build:File1.spritefont;ManoloMono.spritefont + diff --git a/src/LargeGridPathfinding/Content/File.spritefont b/src/LargeGridPathfinding/Content/File.spritefont new file mode 100644 index 0000000..bd33ecf --- /dev/null +++ b/src/LargeGridPathfinding/Content/File.spritefont @@ -0,0 +1,60 @@ + + + + + + + Arial + + + 12 + + + 0 + + + true + + + + + + + + + + + + ~ + + + + diff --git a/src/LargeGridPathfinding/Content/File1.spritefont b/src/LargeGridPathfinding/Content/File1.spritefont new file mode 100644 index 0000000..ae017fb --- /dev/null +++ b/src/LargeGridPathfinding/Content/File1.spritefont @@ -0,0 +1,60 @@ + + + + + + + ManoloMono + + + 12 + + + 0 + + + true + + + + + + + + + + + + ~ + + + + diff --git a/src/LargeGridPathfinding/Content/ManoloMono.ttf b/src/LargeGridPathfinding/Content/ManoloMono.ttf new file mode 100644 index 0000000..ade1523 Binary files /dev/null and b/src/LargeGridPathfinding/Content/ManoloMono.ttf differ diff --git a/src/LargeGridPathfinding/GridFiller.cs b/src/LargeGridPathfinding/GridFiller.cs new file mode 100644 index 0000000..06282df --- /dev/null +++ b/src/LargeGridPathfinding/GridFiller.cs @@ -0,0 +1,297 @@ +using Microsoft.Xna.Framework; + +using System; +using System.Collections.Concurrent; +using System.Collections.Generic; +using System.Diagnostics; +using System.Linq; +using System.Threading.Tasks; + +namespace LargeGridPathfinding; + +internal class GridFiller +{ + private int currentLabel = 1; + private int currentObstacleLabel = -1; + public int[,] Grid { get; } + public ConcurrentDictionary PlacedRectangles { get; } + public int Width { get; } + public int Height { get; } + + public GridFiller(int width, int height, List obstacles) + { + Width = width; + Height = height; + Grid = new int[height, width]; + PlacedRectangles = []; + + foreach (Rectangle rectangle in obstacles) + { + PlaceObstacle(rectangle); + } + } + + public void FillGrid(int? x1 = null, int? y1 = null, int? x2 = null, int? y2 = null, bool fillAll = false, IProgress? totalProgress = null, IProgress? calculatingCandidatesProgress = null, IProgress? placingCandidatesProgress = null) + { + x1 ??= 0; + y1 ??= 0; + x2 ??= Width; + y2 ??= Height; + + if (fillAll) + { + PlacedRectangles.Clear(); + for (int y = y1.Value; y < y2; y++) + { + for (int x = x1.Value; x < x2; x++) + { + if (Grid[y, x] > 0) + { + Grid[y, x] = 0; + } + } + } + } + + // Collect all valid rectangle placements + ConcurrentBag<(int x, int y, int w, int h)> candidates = []; + + int cells = Grid.GetLength(0) * Grid.GetLength(1); + int areaPlaced = 0; + int maxRectangleSize = Math.Max((Grid.GetLength(0) + Grid.GetLength(1)) / 20, 100); + int reportInterval = cells / 100; + + do + { + candidates.Clear(); + + Debug.WriteLine("Calculating candidates..."); + + // Calculate all possible rectangle placements + _ = Parallel.For(y1.Value, y2.Value, (y, loopState) => + { + for (int x = x1.Value; x < x2; x++) + { + if (Grid[y, x] == 0) + { + (int w, int h) = GetMaxRectangleSize(x, y, maxRectangleSize, maxRectangleSize); + + if (w > 0 && h > 0) + { + candidates.Add((x, y, w, h)); + } + + if (w >= maxRectangleSize && h >= maxRectangleSize) + { + loopState.Break(); + return; + } + + if (candidates.Count % reportInterval == 0) + { + calculatingCandidatesProgress?.Report((float)candidates.Count / (cells - areaPlaced)); + Debug.WriteLine($"Candidates Progress: {candidates.Count} / {cells - areaPlaced} ({(float)candidates.Count / (cells - areaPlaced):P2})"); + } + } + } + }); + + Debug.WriteLine($"Candidates: {candidates.Count}"); + Debug.WriteLine("Sorting candidates..."); + + // Sort by area (largest first) + List<(int x, int y, int w, int h)> sortedCandidates = [.. candidates]; + sortedCandidates.Sort((a, b) => (b.w * b.h).CompareTo(a.w * a.h)); + + Debug.WriteLine("Placing rectangles..."); + + int placed = 0; + int reportIntervalPlacing = Math.Max(sortedCandidates.Count / 10, 1); + + // Place rectangles, ensuring no overlap + foreach ((int x, int y, int w, int h) in sortedCandidates) + { + // Check if area is still free + if (IsAreaFree(x, y, w, h)) + { + Rectangle rectangle = new Rectangle(x, y, w, h); + PlaceRectangle(rectangle); + areaPlaced += w * h; + } + + placed++; + + if (placed % reportIntervalPlacing == 0) + { + totalProgress?.Report((float)areaPlaced / cells); + placingCandidatesProgress?.Report((float)placed / (sortedCandidates.Count + 1)); + Debug.WriteLine($"Placed Progress: {placed} / {sortedCandidates.Count} ({(float)placed / sortedCandidates.Count:P2})"); + } + } + + totalProgress?.Report((float)areaPlaced / cells); + } while (!candidates.IsEmpty); + + totalProgress?.Report(1); + calculatingCandidatesProgress?.Report(1); + placingCandidatesProgress?.Report(1); + } + + public void PlaceObstacle(Rectangle rectangle) + { + // Check area around the rectangle to update all adjacent rectangles + + int y = int.Clamp(rectangle.Y - 1, 0, Height); + int x = int.Clamp(rectangle.X - 1, 0, Width); + int h = int.Clamp(rectangle.Y + rectangle.Height + 2, 0, Height); + int w = int.Clamp(rectangle.X + rectangle.Width + 2, 0, Width); + + List removedRectangles = [new Rectangle(x, y, w - x, h - y)]; + + int obstacle = currentObstacleLabel--; + + for (int dy = y; dy < h; dy++) + { + for (int dx = x; dx < w; dx++) + { + if (Grid[dy, dx] > 0 && PlacedRectangles.ContainsKey(Grid[dy, dx])) + { + removedRectangles.Add(RemoveRectangle(Grid[dy, dx])); + } + + if (Grid[dy, dx] >= 0 && dy < rectangle.Y + rectangle.Height && dx < rectangle.X + rectangle.Width && dy >= rectangle.Y && dx >= rectangle.X) + { + Grid[dy, dx] = obstacle; + } + } + } + + if (removedRectangles.Count == 1) + { + return; + } + + int minStartX = removedRectangles.Min(r => r.X); + int minStartY = removedRectangles.Min(r => r.Y); + int maxEndX = removedRectangles.Max(r => r.X + r.Width); + int maxEndY = removedRectangles.Max(r => r.Y + r.Height); + + FillGrid(minStartX, minStartY, maxEndX, maxEndY); + + Debug.WriteLine("Placed obstacle"); + Debug.WriteLine($"Placed obstacle {rectangle.X}, {rectangle.Y}, {rectangle.Width}, {rectangle.Height}"); + Debug.WriteLine($"minX: {minStartX}, minY: {minStartY}, maxX: {maxEndX}, maxY: {maxEndY}"); + } + + public void RemoveObstacle(Rectangle rectangle) + { + // Check area around the rectangle to update all adjacent rectangles + + int y = int.Clamp(rectangle.Y - 1, 0, Height); + int x = int.Clamp(rectangle.X - 1, 0, Width); + int h = int.Clamp(rectangle.Y + rectangle.Height + 2, 0, Height); + int w = int.Clamp(rectangle.X + rectangle.Width + 2, 0, Width); + + List removedRectangles = [new Rectangle(x, y, w - x, h - y)]; + + for (int dy = y; dy < h; dy++) + { + for (int dx = x; dx < w; dx++) + { + if (Grid[dy, dx] > 0) + { + removedRectangles.Add(RemoveRectangle(Grid[dy, dx])); + } + + if (Grid[dy, dx] < 0 && dy < rectangle.Y + rectangle.Height && dx < rectangle.X + rectangle.Width && dy >= rectangle.Y && dx >= rectangle.X) + { + Grid[dy, dx] = 0; + } + } + } + + int minStartX = removedRectangles.Min(r => r.X); + int minStartY = removedRectangles.Min(r => r.Y); + int maxEndX = removedRectangles.Max(r => r.X + r.Width); + int maxEndY = removedRectangles.Max(r => r.Y + r.Height); + + FillGrid(minStartX, minStartY, maxEndX, maxEndY); + + Debug.WriteLine("Removed obstacle"); + Debug.WriteLine($"Placed obstacle {rectangle.X}, {rectangle.Y}, {rectangle.Width}, {rectangle.Height}"); + Debug.WriteLine($"minX: {minStartX}, minY: {minStartY}, maxX: {maxEndX}, maxY: {maxEndY}"); + } + + // Helper method to check if a rectangle can still be placed + private bool IsAreaFree(int x, int y, int w, int h) + { + for (int dy = 0; dy < h; dy++) + { + for (int dx = 0; dx < w; dx++) + { + if (Grid[y + dy, x + dx] != 0) // Not empty + { + return false; + } + } + } + return true; + } + + private (int, int) GetMaxRectangleSize(int startX, int startY, int limitW, int limitH) + { + int maxWidth = 0, maxHeight = 0; + + for (int x = startX; x < Width && maxWidth < limitW && Grid[startY, x] == 0; x++) + { + maxWidth++; + } + + for (int h = 1; h <= Math.Min(Height - startY, limitH); h++) + { + for (int x = startX; x < startX + maxWidth; x++) + { + if (Grid[startY + h - 1, x] != 0) + { + return (maxWidth, h - 1); + } + } + maxHeight = h; + } + + return (maxWidth, maxHeight); + } + + private void PlaceRectangle(Rectangle rectangle) + { + int label = currentLabel++; + + for (int dy = 0; dy < rectangle.Height; dy++) + { + for (int dx = 0; dx < rectangle.Width; dx++) + { + Grid[rectangle.Y + dy, rectangle.X + dx] = label; + } + } + + PlacedRectangles[label] = rectangle; + } + + private Rectangle RemoveRectangle(int label) + { + if (!PlacedRectangles.TryRemove(label, out Rectangle rectangle)) + { + return Rectangle.Empty; + } + + for (int dy = 0; dy < rectangle.Height; dy++) + { + for (int dx = 0; dx < rectangle.Width; dx++) + { + Grid[rectangle.Y + dy, rectangle.X + dx] = 0; + } + } + + return rectangle; + } +} \ No newline at end of file diff --git a/src/LargeGridPathfinding/Icon.bmp b/src/LargeGridPathfinding/Icon.bmp new file mode 100644 index 0000000..2b48165 Binary files /dev/null and b/src/LargeGridPathfinding/Icon.bmp differ diff --git a/src/LargeGridPathfinding/Icon.ico b/src/LargeGridPathfinding/Icon.ico new file mode 100644 index 0000000..7d9dec1 Binary files /dev/null and b/src/LargeGridPathfinding/Icon.ico differ diff --git a/src/LargeGridPathfinding/InputHelper.cs b/src/LargeGridPathfinding/InputHelper.cs new file mode 100644 index 0000000..4849499 --- /dev/null +++ b/src/LargeGridPathfinding/InputHelper.cs @@ -0,0 +1,31 @@ +using Microsoft.Xna.Framework; +using Microsoft.Xna.Framework.Input; + +using MonoGame.Extended.Input; + +namespace LargeGridPathfinding; + +internal static class InputHelper +{ + public static Vector2 GetMovementDirection(this KeyboardStateExtended keyboardState) + { + Vector2 movementDirection = Vector2.Zero; + if (keyboardState.IsKeyDown(Keys.S)) + { + movementDirection += Vector2.UnitY; + } + if (keyboardState.IsKeyDown(Keys.W)) + { + movementDirection -= Vector2.UnitY; + } + if (keyboardState.IsKeyDown(Keys.A)) + { + movementDirection -= Vector2.UnitX; + } + if (keyboardState.IsKeyDown(Keys.D)) + { + movementDirection += Vector2.UnitX; + } + return movementDirection; + } +} \ No newline at end of file diff --git a/src/LargeGridPathfinding/LargeGridPathfinding.csproj b/src/LargeGridPathfinding/LargeGridPathfinding.csproj new file mode 100644 index 0000000..3ba61c7 --- /dev/null +++ b/src/LargeGridPathfinding/LargeGridPathfinding.csproj @@ -0,0 +1,35 @@ + + + WinExe + net8.0 + Major + false + false + + + app.manifest + Icon.ico + enable + + + + + + + + Icon.ico + + + Icon.bmp + + + + + + + + + + + + \ No newline at end of file diff --git a/src/LargeGridPathfinding/LargeGridPathfindingGame.cs b/src/LargeGridPathfinding/LargeGridPathfindingGame.cs new file mode 100644 index 0000000..7a958f3 --- /dev/null +++ b/src/LargeGridPathfinding/LargeGridPathfindingGame.cs @@ -0,0 +1,652 @@ +using Microsoft.Xna.Framework; +using Microsoft.Xna.Framework.Graphics; +using Microsoft.Xna.Framework.Input; + +using MonoGame.Extended; +using MonoGame.Extended.Input; + +using System; +using System.Collections.Concurrent; +using System.Collections.Generic; +using System.Diagnostics; +using System.Linq; +using System.Threading; +using System.Threading.Tasks; + +namespace LargeGridPathfinding; + +public class LargeGridPathfindingGame : Game +{ + private readonly ConcurrentDictionary temporaryIndicators = []; + private readonly ProgressTracker progressTracker = new ProgressTracker(); + private readonly List agents = []; + private SpriteBatch spriteBatch = null!; + private SpriteBatch uiSpriteBatch = null!; + private SpriteFont uiFont = null!; + private OrthographicCamera camera = null!; + private GridFiller gridFiller = null!; + private Pathfinder pathfinder = null!; + private bool gridChanged; + private bool inputBlocked = true; + private bool showZones = true; + private bool showGrid = false; + private bool showPaths = false; + private Vector2? previousMousePosition; + + public LargeGridPathfindingGame() + { + _ = new GraphicsDeviceManager(this) + { + PreferredBackBufferWidth = 1920, + PreferredBackBufferHeight = 1080 + }; + + Content.RootDirectory = "Content"; + IsMouseVisible = true; + + Window.Title = "Large Grid Pathfinding"; + Window.AllowUserResizing = true; + } + + protected override void Initialize() + { + camera = new OrthographicCamera(GraphicsDevice) + { + MinimumZoom = 0.1f, + MaximumZoom = 2, + Zoom = 0.5f, + }; + + StartInitializationTask(); + + base.Initialize(); + } + + protected override void LoadContent() + { + spriteBatch = new SpriteBatch(GraphicsDevice); + uiSpriteBatch = new SpriteBatch(GraphicsDevice); + uiFont = Content.Load("ManoloMono"); + } + + protected override void Update(GameTime gameTime) + { + if (!IsActive) + { + return; + } + + if (pathfinder is not null) + { + // Update agent positions + foreach (Agent agent in agents) + { + if (agent.Path is null || agent.Path.Count < 2) + { + continue; + } + + if (Vector2.Distance(agent.Position, agent.NextPosition) < 0.1) + { + if (agent.NextPosition == agent.Path[^1]) + { + agent.Path = null; + agent.Destination = null; + } + else + { + agent.NextPosition = agent.Path[agent.Path.LastIndexOf(agent.NextPosition) + 1]; + } + } + else + { + float distance = Vector2.Distance(agent.Position, agent.NextPosition); + float lerpFactor = Math.Min(10 / distance * gameTime.GetElapsedSeconds(), 1); + + Vector2 nextPosition = Vector2.Lerp(agent.Position, agent.NextPosition, lerpFactor); + + int newX = (int)Math.Round(nextPosition.X); + int newY = (int)Math.Round(nextPosition.Y); + + int[,] grid = gridFiller.Grid; + + if (grid[newY, newX] < 0) + { + agent.Path = null; + continue; + } + + agent.Position = nextPosition; + } + } + } + + MouseExtended.Update(); + KeyboardExtended.Update(); + + MouseStateExtended mouseState = MouseExtended.GetState(); + KeyboardStateExtended keyboardState = KeyboardExtended.GetState(); + + float movementSpeed = (float)Math.Pow(200, 2 - camera.Zoom); + + movementSpeed = Math.Clamp(movementSpeed, 1000, 20000); + + camera.Move(keyboardState.GetMovementDirection() * movementSpeed * gameTime.GetElapsedSeconds()); + + if (mouseState.DeltaScrollWheelValue < 0) + { + camera.ZoomIn(0.1f); + } + else if (mouseState.DeltaScrollWheelValue > 0) + { + camera.ZoomOut(0.1f); + } + + if (inputBlocked) + { + return; + } + + // Toggle debug options + + if (keyboardState.WasKeyPressed(Keys.Z)) + { + showZones = !showZones; + } + + if (keyboardState.WasKeyPressed(Keys.G)) + { + showGrid = !showGrid; + } + + if (keyboardState.WasKeyPressed(Keys.P)) + { + showPaths = !showPaths; + } + + if (keyboardState.WasKeyPressed(Keys.F)) + { + Debug.WriteLine("Filling grid..."); + + _ = progressTracker.AddProgress("Filling grid", out IProgress fillingGridProgressReporter); + _ = progressTracker.AddProgress("Calculating candidates", out IProgress calculatingCandidatesProgressReporter); + _ = progressTracker.AddProgress("Placing zones", out IProgress placingCandidatesProgressReporter); + + bool showZonesBefore = showZones; + bool showGridBefore = showGrid; + bool showPathsBefore = showPaths; + showZones = true; + showGrid = false; + showPaths = false; + inputBlocked = true; + + _ = Task.Run(() => + { + gridFiller.FillGrid(fillAll: true, totalProgress: fillingGridProgressReporter, calculatingCandidatesProgress: calculatingCandidatesProgressReporter, placingCandidatesProgress: placingCandidatesProgressReporter); + gridChanged = true; + inputBlocked = false; + showZones = showZonesBefore; + showGrid = showGridBefore; + showPaths = showPathsBefore; + }); + } + + // Place or remove obstacles via mouse input + + if (mouseState.IsButtonDown(MouseButton.Left)) + { + Vector2 mousePosition = camera.ScreenToWorld(mouseState.Position.ToVector2()); + Vector2 gridPosition = new Vector2((int)mousePosition.X / 10, (int)mousePosition.Y / 10); + + if (gridPosition.X < 0 || gridPosition.X >= gridFiller.Width || gridPosition.Y < 0 || gridPosition.Y >= gridFiller.Height || gridFiller.Grid[(int)gridPosition.Y, (int)gridPosition.X] < 0 || temporaryIndicators.ContainsKey(gridPosition)) + { + return; + } + + Vector2? previousGridPosition = null; + if (previousMousePosition is not null) + { + previousGridPosition = new Vector2((int)previousMousePosition.Value.X / 10, (int)previousMousePosition.Value.Y / 10); + } + + // Place temporary indicators to show the obstacle is scheduled for placement + temporaryIndicators[gridPosition] = Color.Red; + + previousMousePosition = mousePosition; + + _ = Task.Run(() => + { + if (previousGridPosition is not null) + { + List interpolatedPositions = []; + + // Interpolate between previous and current mouse position to place obstacles in a straight line to prevent gaps + float distance = Vector2.Distance(previousGridPosition.Value, gridPosition); + float lerpSteps = distance * 10; + + for (float t = 0; t <= 1; t += 1 / lerpSteps) + { + Vector2 interpolatedPosition = Vector2.Lerp(previousGridPosition.Value, gridPosition, t); + interpolatedPosition.Round(); + interpolatedPositions.Add(interpolatedPosition); + temporaryIndicators[interpolatedPosition] = Color.Red; + } + + foreach (Vector2 interpolatedPosition in interpolatedPositions.Distinct()) + { + gridFiller.PlaceObstacle(new Rectangle((int)interpolatedPosition.X, (int)interpolatedPosition.Y, 1, 1)); + _ = temporaryIndicators.TryRemove(interpolatedPosition, out _); + } + } + + gridFiller.PlaceObstacle(new Rectangle((int)gridPosition.X, (int)gridPosition.Y, 1, 1)); + gridChanged = true; + _ = temporaryIndicators.TryRemove(gridPosition, out _); + }); + } + else if (mouseState.IsButtonDown(MouseButton.Right)) + { + Vector2 mousePosition = camera.ScreenToWorld(mouseState.Position.ToVector2()); + Vector2 gridPosition = new Vector2((int)mousePosition.X / 10, (int)mousePosition.Y / 10); + + if (gridPosition.X < 0 || gridPosition.X >= gridFiller.Width || gridPosition.Y < 0 || gridPosition.Y >= gridFiller.Height || gridFiller.Grid[(int)gridPosition.Y, (int)gridPosition.X] > 0 || temporaryIndicators.ContainsKey(gridPosition)) + { + return; + } + + Vector2? previousGridPosition = null; + if (previousMousePosition is not null) + { + previousGridPosition = new Vector2((int)previousMousePosition.Value.X / 10, (int)previousMousePosition.Value.Y / 10); + } + + // Place temporary indicators to show the obstacle is scheduled for removal + temporaryIndicators[gridPosition] = Color.Yellow; + + previousMousePosition = mousePosition; + + _ = Task.Run(() => + { + if (previousGridPosition is not null) + { + List interpolatedPositions = []; + + // Interpolate between previous and current mouse position to place obstacles in a straight line to prevent gaps + + float distance = Vector2.Distance(previousGridPosition.Value, gridPosition); + float lerpSteps = distance * 10; + + for (float t = 0; t <= 1; t += 1 / lerpSteps) + { + Vector2 interpolatedPosition = Vector2.Lerp(previousGridPosition.Value, gridPosition, t); + interpolatedPosition.Round(); + interpolatedPositions.Add(interpolatedPosition); + temporaryIndicators[interpolatedPosition] = Color.Yellow; + } + + foreach (Vector2 interpolatedPosition in interpolatedPositions.Distinct()) + { + gridFiller.RemoveObstacle(new Rectangle((int)interpolatedPosition.X, (int)interpolatedPosition.Y, 1, 1)); + _ = temporaryIndicators.TryRemove(interpolatedPosition, out _); + } + } + + gridFiller.RemoveObstacle(new Rectangle((int)gridPosition.X, (int)gridPosition.Y, 1, 1)); + gridChanged = true; + _ = temporaryIndicators.TryRemove(gridPosition, out _); + }); + } + else + { + previousMousePosition = null; + } + + base.Update(gameTime); + } + + protected override void Draw(GameTime gameTime) + { + if (gridFiller is null) + { + return; + } + + GraphicsDevice.Clear(Color.CornflowerBlue); + + Matrix transformMatrix = camera.GetViewMatrix(); + + int yStart = Math.Max(0, (int)(camera.BoundingRectangle.Top / 10) - 2); + int xStart = Math.Max(0, (int)(camera.BoundingRectangle.Left / 10) - 2); + int yEnd = Math.Min(gridFiller.Height, (int)(camera.BoundingRectangle.Bottom / 10) + 2); + int xEnd = Math.Min(gridFiller.Width, (int)(camera.BoundingRectangle.Right / 10) + 2); + + int[,] grid = gridFiller.Grid; + Rectangle cellRect = new Rectangle(0, 0, 10, 10); + + Color[] colorLookup = [ + Color.Blue, Color.Cyan, Color.Magenta, Color.Yellow, Color.Orange, + Color.DarkMagenta, Color.DarkCyan, Color.Tan, Color.RosyBrown, + Color.DarkKhaki, Color.DarkSalmon, Color.DarkSlateGray, + Color.DarkTurquoise, Color.DarkGoldenrod, Color.Aqua, Color.Aquamarine, + Color.Bisque, Color.DarkSlateBlue, Color.BlueViolet, Color.Brown, + Color.BurlyWood, Color.CadetBlue, Color.Chartreuse, Color.Chocolate, + Color.Coral, Color.CornflowerBlue, Color.Crimson, Color.DarkBlue + ]; + + // Draw grid cells + spriteBatch.Begin(SpriteSortMode.Deferred, samplerState: SamplerState.PointClamp, transformMatrix: transformMatrix); + + spriteBatch.FillRectangle(new Rectangle(0, 0, gridFiller.Width * 10, gridFiller.Height * 10), Color.White, layerDepth: 0.4f); + + if (showZones) + { + Dictionary rectangles = new Dictionary(gridFiller.PlacedRectangles); + + foreach ((int label, Rectangle rectangle) in rectangles) + { + Color color = colorLookup[label % 28]; + + spriteBatch.FillRectangle(new Rectangle(rectangle.X * 10, rectangle.Y * 10, rectangle.Width * 10, rectangle.Height * 10), color, layerDepth: 0.2f); + } + } + + for (int y = yStart; y < yEnd; y++) + { + for (int x = xStart; x < xEnd; x++) + { + cellRect.X = x * 10; + cellRect.Y = y * 10; + + int cellValue = grid[y, x]; + + if (cellValue < 0) + { + spriteBatch.FillRectangle(cellRect, Color.Red, layerDepth: 0.3f); + } + + if (temporaryIndicators.TryGetValue(new Vector2(x, y), out Color indicatorColor)) + { + spriteBatch.DrawCircle(cellRect.Center.ToVector2(), 5, 10, indicatorColor, 2f, layerDepth: 0.2f); + } + + if (showGrid) + { + spriteBatch.DrawRectangle(cellRect, Color.Black, layerDepth: 0.2f); + } + } + } + spriteBatch.End(); + + // Draw paths in a separate batch if needed + if (showPaths) + { + spriteBatch.Begin(SpriteSortMode.Deferred, samplerState: SamplerState.PointClamp, transformMatrix: transformMatrix); + foreach (List? path in agents.Select(a => a.Path)) + { + if (path is null) + { + continue; + } + + for (int i = 0; i < path.Count - 1; i++) + { + Vector2 start = (path[i] * 10) + new Vector2(5, 5); + Vector2 end = (path[i + 1] * 10) + new Vector2(5, 5); + + if ((start.X < camera.BoundingRectangle.Left - 10 && end.X < camera.BoundingRectangle.Left - 10) || + (start.X > camera.BoundingRectangle.Right + 10 && end.X > camera.BoundingRectangle.Right + 10) || + (start.Y < camera.BoundingRectangle.Top - 10 && end.Y < camera.BoundingRectangle.Top - 10) || + (start.Y > camera.BoundingRectangle.Bottom + 10 && end.Y > camera.BoundingRectangle.Bottom + 10)) + { + continue; + } + + spriteBatch.DrawLine(start, end, Color.Black, 2f, layerDepth: 0.1f); + spriteBatch.DrawCircle(start, 5, 10, i == 0 ? Color.Green : Color.Black, 2f, layerDepth: 0.1f); + spriteBatch.DrawCircle(end, 5, 10, i == path.Count - 2 ? Color.Red : Color.Black, 2f, layerDepth: 0.1f); + } + } + spriteBatch.End(); + } + + // Draw agents + spriteBatch.Begin(SpriteSortMode.Deferred, samplerState: SamplerState.PointClamp, transformMatrix: transformMatrix); + + foreach (Vector2 currentPosition in agents.Select(a => a.Position)) + { + if (currentPosition.X < xStart || currentPosition.X > xEnd || currentPosition.Y < yStart || currentPosition.Y > yEnd) + { + continue; + } + + spriteBatch.DrawCircle((currentPosition + new Vector2(0.5f, 0.5f)) * 10, 5, 10, Color.Blue, 2f, layerDepth: 0.1f); + } + + spriteBatch.End(); + + // Draw UI elements + uiSpriteBatch.Begin(SpriteSortMode.Deferred, samplerState: SamplerState.PointClamp); + + uiSpriteBatch.DrawString(uiFont, $"FPS: {1 / gameTime.GetElapsedSeconds():0}", new Vector2(10, 10), Color.Black); + uiSpriteBatch.DrawString(uiFont, $"Grid: {gridFiller.Width}x{gridFiller.Height}", new Vector2(10, 30), Color.Black); + uiSpriteBatch.DrawString(uiFont, $"Zones: {gridFiller.PlacedRectangles.Count}", new Vector2(10, 50), Color.Black); + uiSpriteBatch.DrawString(uiFont, $"Agents: {agents.Count}", new Vector2(10, 70), Color.Black); + + IReadOnlyList progresses = progressTracker.GetProgresses(); + + for (int i = 0; i < progresses.Count; i++) + { + ProgressTracker.ProgressData progressData = progresses[i]; + + string progress = progressData.Indeterminate ? "..." : progressData.Progress.ToString("P0"); + + uiSpriteBatch.DrawString(uiFont, $"{progressData.Name}: {progress}", new Vector2(10, 90 + (i * 20)), Color.Black); + } + + uiSpriteBatch.End(); + + base.Draw(gameTime); + } + + private void StartInitializationTask() + { + _ = Task.Run(() => + { + // Configuration options + int width = 1000; + int height = 1000; + int agentCount = 10000; + + bool pathRandomization = true; // Randomize path costs to prevent agents from following the same path + bool penalizeStretchedRectangles = true; // (EXPERIMENTAL) Penalize paths that go through stretched rectangles to prevent too many agents in a small area + + for (int i = 0; i < agentCount; i++) + { + int x = Random.Shared.Next(0, width - 1); + int y = Random.Shared.Next(0, height - 1); + + agents.Add(new(new Vector2(x, y))); + } + + List obstacles = []; + + Debug.WriteLine("Generating random obstacles..."); + + // Generate room/building like obstacles with walls and doors + + for (int i = 0; i < (width + height) / 2; i++) + { + int x = Random.Shared.Next(0, width - 1); + int y = Random.Shared.Next(0, height - 1); + int w = Random.Shared.Next(5, (width + height) / 100); + int h = Random.Shared.Next(5, (width + height) / 100); + + Rectangle wall1 = new(x, y, w, 1); // Top wall + Rectangle wall2 = new(x, y, 1, h); // Left wall + Rectangle wall3 = new(x + w - 1, y, 1, h); // Right wall + Rectangle wall4 = new(x, y + h - 1, w, 1); // Bottom wall + + int doorSide = Random.Shared.Next(4); // Choose a random side for the door + + if (doorSide == 0) // Bottom side + { + obstacles.Add(wall1); + obstacles.Add(wall2); + obstacles.Add(wall3); + + int doorX = x + Random.Shared.Next(1, w - 2); + Rectangle doorWall1 = new(x, y + h - 1, doorX - x, 1); + Rectangle doorWall2 = new(doorX + 2, y + h - 1, x + w - (doorX + 2), 1); + + obstacles.Add(doorWall1); + obstacles.Add(doorWall2); + } + else if (doorSide == 1) // Top side + { + obstacles.Add(wall2); + obstacles.Add(wall3); + obstacles.Add(wall4); + + int doorX = x + Random.Shared.Next(1, w - 2); + Rectangle doorWall1 = new(x, y, doorX - x, 1); + Rectangle doorWall2 = new(doorX + 2, y, x + w - (doorX + 2), 1); + + obstacles.Add(doorWall1); + obstacles.Add(doorWall2); + } + else if (doorSide == 2) // Left side + { + obstacles.Add(wall1); + obstacles.Add(wall3); + obstacles.Add(wall4); + + int doorY = y + Random.Shared.Next(1, h - 2); + Rectangle doorWall1 = new(x, y, 1, doorY - y); + Rectangle doorWall2 = new(x, doorY + 2, 1, y + h - (doorY + 2)); + + obstacles.Add(doorWall1); + obstacles.Add(doorWall2); + } + else if (doorSide == 3) // Right side + { + obstacles.Add(wall1); + obstacles.Add(wall2); + obstacles.Add(wall4); + + int doorY = y + Random.Shared.Next(1, h - 2); + Rectangle doorWall1 = new(x + w - 1, y, 1, doorY - y); + Rectangle doorWall2 = new(x + w - 1, doorY + 2, 1, y + h - (doorY + 2)); + + obstacles.Add(doorWall1); + obstacles.Add(doorWall2); + } + } + + Debug.WriteLine("Initializing grid..."); + + ProgressTracker.ProgressData progressData = progressTracker.AddProgress("Initializing grid", true, out _); + gridFiller = new GridFiller(width, height, obstacles); + progressTracker.RemoveProgress(progressData); + + Debug.WriteLine("Filling grid..."); + + _ = progressTracker.AddProgress("Filling grid", out IProgress fillingGridProgressReporter); + _ = progressTracker.AddProgress("Calculating candidates", out IProgress calculatingCandidatesProgressReporter); + _ = progressTracker.AddProgress("Placing zones", out IProgress placingCandidatesProgressReporter); + + gridFiller.FillGrid(totalProgress: fillingGridProgressReporter, calculatingCandidatesProgress: calculatingCandidatesProgressReporter, placingCandidatesProgress: placingCandidatesProgressReporter); + + pathfinder = new Pathfinder(gridFiller.PlacedRectangles, pathRandomization, penalizeStretchedRectangles); + gridChanged = true; + inputBlocked = false; + showZones = false; + showPaths = false; + + StartUpdatePathTask(); + }); + } + + // Handles pathfinding calculations in a separate task + private void StartUpdatePathTask() + { + _ = Task.Factory.StartNew(async () => + { + while (true) + { + try + { + if (gridChanged) + { + gridChanged = false; + ProgressTracker.ProgressData progressDataBuildGraph = progressTracker.AddProgress("Rebuilding graph", true, out _); + pathfinder.RebuildGraph(); + progressTracker.RemoveProgress(progressDataBuildGraph); + } + + Agent[] agentsRequirePath = agents.Where(a => a.Path is null).ToArray(); + + if (agentsRequirePath.Length != 0) + { + ProgressTracker.ProgressData progressDataPaths = progressTracker.AddProgress($"Calculating {agentsRequirePath.Length} paths", out IProgress progress); + + int pathsCalculated = 0; + int reportInterval = Math.Max(1, agentsRequirePath.Length / 10); + + // Calculate paths for agents that require a new path + _ = Parallel.For(0, agentsRequirePath.Length, i => + { + Agent agent = agentsRequirePath[i]; + + agent.Path = CalculatePath(agent.GridPosition, agent.Destination); + agent.Path ??= CalculatePath(); + agent.Position = agent.Path?[0] ?? agent.Position; + agent.NextPosition = agent.Path?[1] ?? agent.Position; + agent.Destination = agent.Path?.LastOrDefault().ToPoint(); + + int localPathsCalculated = Interlocked.Increment(ref pathsCalculated); + + if (localPathsCalculated % reportInterval == 0) + { + progress.Report((float)localPathsCalculated / agentsRequirePath.Length); + } + }); + + progressTracker.RemoveProgress(progressDataPaths); + } + + await Task.Delay(100); + } + catch (Exception ex) + { + Debug.WriteLine(ex); + } + } + }, TaskCreationOptions.LongRunning); + } + + private List? CalculatePath(Point? start = null, Point? goal = null) + { + int width = gridFiller.Width; + int height = gridFiller.Height; + int startX = Random.Shared.Next(0, width - 1); + int startY = Random.Shared.Next(0, height - 1); + int goalX = Random.Shared.Next(0, width - 1); + int goalY = Random.Shared.Next(0, height - 1); + + start ??= new Point(startX, startY); + goal ??= new Point(goalX, goalY); + + int[,] grid = gridFiller.Grid; + + List? path = pathfinder.FindPath(ref grid[start.Value.Y, start.Value.X], ref grid[goal.Value.Y, goal.Value.X]); + + if (path is not null) + { + // Add start and goal positions to the path because the pathfinder only returns transitions between rectangles + path.Insert(0, start.Value.ToVector2()); + path.Add(goal.Value.ToVector2()); + } + + return path; + } +} \ No newline at end of file diff --git a/src/LargeGridPathfinding/Pathfinder.cs b/src/LargeGridPathfinding/Pathfinder.cs new file mode 100644 index 0000000..786a42a --- /dev/null +++ b/src/LargeGridPathfinding/Pathfinder.cs @@ -0,0 +1,231 @@ +using Microsoft.Xna.Framework; + +using System; +using System.Collections.Concurrent; +using System.Collections.Generic; +using System.Diagnostics; +using System.Linq; + +namespace LargeGridPathfinding; + +internal class Pathfinder +{ + private readonly ConcurrentDictionary rectanglesSource; + private readonly bool pathRandomization; + private readonly bool penalizeStretchedRectangles; + private Dictionary rectangleMap; + private Dictionary> adjacencyList; + + public Pathfinder(ConcurrentDictionary rectangles, bool pathRandomization = false, bool penalizeStretchedRectangles = false) + { + rectanglesSource = rectangles; + this.pathRandomization = pathRandomization; + this.penalizeStretchedRectangles = penalizeStretchedRectangles; + rectangleMap = new Dictionary(rectangles); + adjacencyList = BuildGraph(); + } + + public List? FindPath(ref int start, ref int goal) + { + PriorityQueue<(int node, List path, int cost), int> queue = new PriorityQueue<(int node, List path, int cost), int>(); + HashSet visited = []; + queue.Enqueue((start, new List { start }, 0), 0); + + if (!rectangleMap.ContainsKey(start) || !rectangleMap.ContainsKey(goal)) + { + Debug.WriteLine($"{(rectangleMap.ContainsKey(start) ? "Goal" : "Start")} rectangle not found."); + return null; + } + + while (queue.Count > 0) + { + (int node, List path, int cost) = queue.Dequeue(); + + if (node == goal) + { + // Convert rectangle path to coordinate transitions + List coordinatePath = []; + + for (int i = 0; i < path.Count - 1; i++) + { + (Vector2 entry, Vector2 exit) = GetTransitionPoints(path[i], path[i + 1], i + 2 < path.Count ? path[i + 2] : null); + coordinatePath.Add(entry); + coordinatePath.Add(exit); + } + + return coordinatePath; + } + + if (visited.Contains(node)) + { + continue; + } + + _ = visited.Add(node); + + foreach (int neighbor in adjacencyList.TryGetValue(node, out List? neighbors) ? neighbors : []) + { + if (!visited.Contains(neighbor)) + { + List newPath = new List(path) { neighbor }; + + int newCost = cost + 1; + + if (penalizeStretchedRectangles) + { + newCost += Math.Max(rectangleMap[neighbor].Width, rectangleMap[neighbor].Height) / Math.Min(rectangleMap[neighbor].Width, rectangleMap[neighbor].Height); + } + + if (pathRandomization) + { + newCost += Random.Shared.Next(-1, 2); + } + + queue.Enqueue((neighbor, newPath, newCost), newCost); + } + } + } + + return null; + } + + public void RebuildGraph() + { + rectangleMap = new Dictionary(rectanglesSource); + adjacencyList.Clear(); + adjacencyList = BuildGraph(); + } + + private static bool AreRectanglesAdjacent(Rectangle r1, Rectangle r2) + { + bool adjacentHorizontally = (r1.X + r1.Width == r2.X || r2.X + r2.Width == r1.X) && r1.Y < r2.Y + r2.Height && r1.Y + r1.Height > r2.Y; + bool adjacentVertically = (r1.Y + r1.Height == r2.Y || r2.Y + r2.Height == r1.Y) && r1.X < r2.X + r2.Width && r1.X + r1.Width > r2.X; + return adjacentHorizontally || adjacentVertically; + } + + private Vector2 GetRectangleClosestEdge(Rectangle rect, Vector2 point) + { + if (pathRandomization) + { + float randomX = Random.Shared.Next(rect.Left, rect.Right); + float randomY = Random.Shared.Next(rect.Top, rect.Bottom); + return new Vector2(randomX, randomY); + } + + // Clamp the point to the closest edge of the rectangle + float closestX = Math.Clamp(point.X, rect.Left, rect.Right - 1); + float closestY = Math.Clamp(point.Y, rect.Top, rect.Bottom - 1); + + // If point is inside the rectangle, push it to the nearest edge + if (point.X >= rect.Left && point.X < rect.Right && + point.Y >= rect.Top && point.Y < rect.Bottom) + { + float leftDistance = Math.Abs(point.X - rect.Left); + float rightDistance = Math.Abs(point.X - (rect.Right - 1)); + float topDistance = Math.Abs(point.Y - rect.Top); + float bottomDistance = Math.Abs(point.Y - (rect.Bottom - 1)); + + float minDistance = Math.Min(Math.Min(leftDistance, rightDistance), Math.Min(topDistance, bottomDistance)); + + if (minDistance == leftDistance) + { + closestX = rect.Left; + } + else if (minDistance == rightDistance) + { + closestX = rect.Right - 1; + } + else if (minDistance == topDistance) + { + closestY = rect.Top; + } + else if (minDistance == bottomDistance) + { + closestY = rect.Bottom - 1; + } + } + + return new Vector2(closestX, closestY); + } + + // Build a graph where each rectangle is a node and each edge represents an adjacency + private Dictionary> BuildGraph() + { + Dictionary> graph = []; + + foreach (KeyValuePair rect1 in rectangleMap) + { + if (!graph.ContainsKey(rect1.Key)) + { + graph[rect1.Key] = []; + } + + foreach (KeyValuePair rect2 in rectangleMap) + { + if (rect1.Key != rect2.Key && AreRectanglesAdjacent(rect1.Value, rect2.Value)) + { + graph[rect1.Key].Add(rect2.Key); + } + } + } + + return graph; + } + + // Find the transition points between two rectangles + private (Vector2 entry, Vector2 exit) GetTransitionPoints(int from, int to, int? next = null) + { + Rectangle r1 = rectangleMap[from]; + Rectangle r2 = rectangleMap[to]; + + // Find all possible valid transition points + List<(Vector2 entry, Vector2 exit)> possibleTransitions = []; + + if (r1.Right == r2.Left) + { + for (int y = Math.Max(r1.Top, r2.Top); y < Math.Min(r1.Bottom, r2.Bottom); y++) + { + possibleTransitions.Add((new Vector2(r1.Right - 1, y), new Vector2(r2.Left, y))); + } + } + else if (r2.Right == r1.Left) + { + for (int y = Math.Max(r1.Top, r2.Top); y < Math.Min(r1.Bottom, r2.Bottom); y++) + { + possibleTransitions.Add((new Vector2(r1.Left, y), new Vector2(r2.Right - 1, y))); + } + } + else if (r1.Bottom == r2.Top) + { + for (int x = Math.Max(r1.Left, r2.Left); x < Math.Min(r1.Right, r2.Right); x++) + { + possibleTransitions.Add((new Vector2(x, r1.Bottom - 1), new Vector2(x, r2.Top))); + } + } + else if (r2.Bottom == r1.Top) + { + for (int x = Math.Max(r1.Left, r2.Left); x < Math.Min(r1.Right, r2.Right); x++) + { + possibleTransitions.Add((new Vector2(x, r1.Top), new Vector2(x, r2.Bottom - 1))); + } + } + + if (possibleTransitions.Count == 0) + { + return (new Vector2(-1, -1), new Vector2(-1, -1)); + } + + // If there's a next rectangle, optimize the transition to minimize the distance + if (next is not null && rectangleMap.TryGetValue(next.Value, out Rectangle r3)) + { + (Vector2 entry, Vector2 exit) bestTransition = possibleTransitions + .OrderBy(t => Vector2.DistanceSquared(t.exit, GetRectangleClosestEdge(r3, t.exit))) + .First(); + + return bestTransition; + } + + // Default: Pick the middle transition point + return possibleTransitions[possibleTransitions.Count / 2]; + } +} \ No newline at end of file diff --git a/src/LargeGridPathfinding/Program.cs b/src/LargeGridPathfinding/Program.cs new file mode 100644 index 0000000..c75454c --- /dev/null +++ b/src/LargeGridPathfinding/Program.cs @@ -0,0 +1,2 @@ +using LargeGridPathfinding.LargeGridPathfindingGame game = new LargeGridPathfinding.LargeGridPathfindingGame(); +game.Run(); \ No newline at end of file diff --git a/src/LargeGridPathfinding/ProgressTracker.cs b/src/LargeGridPathfinding/ProgressTracker.cs new file mode 100644 index 0000000..7875ba8 --- /dev/null +++ b/src/LargeGridPathfinding/ProgressTracker.cs @@ -0,0 +1,79 @@ +using System; +using System.Collections.Generic; + +namespace LargeGridPathfinding; + +internal class ProgressTracker +{ + private readonly List progresses = []; + + public ProgressData AddProgress(string name, out IProgress progress) + { + return AddProgress(name, false, false, out progress); + } + + public ProgressData AddProgress(string name, bool indeterminate, out IProgress progress) + { + return AddProgress(name, indeterminate, false, out progress); + } + + public ProgressData AddProgress(string name, bool indeterminate, bool disableAutoRemove, out IProgress progress) + { + CustomProgress newProgress = new CustomProgress(); + ProgressData progressData = new ProgressData(name, indeterminate, disableAutoRemove); + + newProgress.ProgressChanged += (_, progress) => Progress_ProgressChanged(progressData, progress); + + lock (progresses) + { + progresses.Add(progressData); + } + + progress = newProgress; + return progressData; + } + + public void RemoveProgress(ProgressData progressData) + { + lock (progresses) + { + if (!progressData.DoNotRemove) + { + _ = progresses.Remove(progressData); + } + } + } + + public IReadOnlyList GetProgresses() + { + return progresses; + } + + private void Progress_ProgressChanged(ProgressData progressData, float progress) + { + progressData.Progress = progress; + + if (progress >= 1) + { + RemoveProgress(progressData); + } + } + + public class ProgressData(string name, bool indeterminate, bool doNotRemove) + { + public string Name { get; } = name; + public float Progress { get; set; } = 0; + public bool Indeterminate { get; } = indeterminate; + public bool DoNotRemove { get; } = doNotRemove; + } + + public class CustomProgress() : IProgress + { + public EventHandler? ProgressChanged { get; set; } + + public void Report(T value) + { + ProgressChanged?.Invoke(this, value); + } + } +} \ No newline at end of file diff --git a/src/LargeGridPathfinding/app.manifest b/src/LargeGridPathfinding/app.manifest new file mode 100644 index 0000000..64a6e2a --- /dev/null +++ b/src/LargeGridPathfinding/app.manifest @@ -0,0 +1,43 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true/pm + permonitorv2,permonitor + + + +