From 678ca8e25196f80d9be03e07bb85108ee5a76800 Mon Sep 17 00:00:00 2001 From: Christopher Date: Sat, 18 Jul 2020 01:07:59 +0200 Subject: [PATCH] add projekt files --- .gitignore | 290 ++++++++++++++++++ prebuild.xml | 36 +++ src/A/Collections/MinHeap.cs | 128 ++++++++ src/A/Graphs/Edge.cs | 37 +++ src/A/Graphs/IEdge.cs | 13 + src/A/Graphs/INode.cs | 12 + src/A/Graphs/Node.cs | 43 +++ src/A/Grids/Grid.cs | 234 ++++++++++++++ src/A/Paths/Path.cs | 28 ++ src/A/Paths/PathFinder.cs | 124 ++++++++ src/A/Paths/PathFinderNode.cs | 25 ++ src/A/Paths/PathReconstructor.cs | 37 +++ src/A/Paths/PathType.cs | 8 + src/A/Primitives/Distance.cs | 73 +++++ src/A/Primitives/Duration.cs | 52 ++++ src/A/Primitives/GridPosition.cs | 32 ++ src/A/Primitives/GridSize.cs | 30 ++ src/A/Primitives/Position.cs | 35 +++ src/A/Primitives/Size.cs | 30 ++ src/A/Primitives/Velocity.cs | 57 ++++ src/BasicPathFinding.cs | 507 +++++++++++++++++++++++++++++++ src/Environment.cs | 44 +++ src/PathNode.cs | 34 +++ src/ScriptRequestData.cs | 38 +++ 24 files changed, 1947 insertions(+) create mode 100644 .gitignore create mode 100644 prebuild.xml create mode 100644 src/A/Collections/MinHeap.cs create mode 100644 src/A/Graphs/Edge.cs create mode 100644 src/A/Graphs/IEdge.cs create mode 100644 src/A/Graphs/INode.cs create mode 100644 src/A/Graphs/Node.cs create mode 100644 src/A/Grids/Grid.cs create mode 100644 src/A/Paths/Path.cs create mode 100644 src/A/Paths/PathFinder.cs create mode 100644 src/A/Paths/PathFinderNode.cs create mode 100644 src/A/Paths/PathReconstructor.cs create mode 100644 src/A/Paths/PathType.cs create mode 100644 src/A/Primitives/Distance.cs create mode 100644 src/A/Primitives/Duration.cs create mode 100644 src/A/Primitives/GridPosition.cs create mode 100644 src/A/Primitives/GridSize.cs create mode 100644 src/A/Primitives/Position.cs create mode 100644 src/A/Primitives/Size.cs create mode 100644 src/A/Primitives/Velocity.cs create mode 100644 src/BasicPathFinding.cs create mode 100644 src/Environment.cs create mode 100644 src/PathNode.cs create mode 100644 src/ScriptRequestData.cs diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..c27e9fd --- /dev/null +++ b/.gitignore @@ -0,0 +1,290 @@ +## 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/master/VisualStudio.gitignore + +# User-specific files +*.suo +*.user +*.userosscache +*.sln.docstates +*.csproj.user +*.csproj +*.dll.build + +# User-specific files (MonoDevelop/Xamarin Studio) +*.userprefs + +# Build results +[Dd]ebug/ +[Dd]ebugPublic/ +[Rr]elease/ +[Rr]eleases/ +x64/ +x86/ +bld/ +[Bb]in/ +[Oo]bj/ +[Ll]og/ + +# Visual Studio 2015 cache/options directory +.vs/ +# Uncomment if you have tasks that create the project's static files in wwwroot +#wwwroot/ + +# MSTest test Results +[Tt]est[Rr]esult*/ +[Bb]uild[Ll]og.* + +# NUNIT +*.VisualState.xml +TestResult.xml + +# Build Results of an ATL Project +[Dd]ebugPS/ +[Rr]eleasePS/ +dlldata.c + +# .NET Core +project.lock.json +project.fragment.lock.json +artifacts/ +**/Properties/launchSettings.json + +*_i.c +*_p.c +*_i.h +*.ilk +*.meta +*.obj +*.pch +*.pdb +*.pgc +*.pgd +*.rsp +*.sbr +*.tlb +*.tli +*.tlh +*.tmp +*.tmp_proj +*.log +*.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 + +# TFS 2012 Local Workspace +$tf/ + +# Guidance Automation Toolkit +*.gpState + +# ReSharper is a .NET coding add-in +_ReSharper*/ +*.[Rr]e[Ss]harper +*.DotSettings.user + +# JustCode is a .NET coding add-in +.JustCode + +# TeamCity is a build add-in +_TeamCity* + +# DotCover is a Code Coverage Tool +*.dotCover + +# 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 +# TODO: 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 +# The packages folder can be ignored because of Package Restore +**/packages/* +# except build/, which is used as an MSBuild target. +!**/packages/build/ +# Uncomment if necessary however generally it will be regenerated when needed +#!**/packages/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 + +# 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 + +# 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 + +# SQL Server files +*.mdf +*.ldf + +# Business Intelligence projects +*.rdl.data +*.bim.layout +*.bim_*.settings + +# Microsoft Fakes +FakesAssemblies/ + +# GhostDoc plugin setting file +*.GhostDoc.xml + +# Node.js Tools for Visual Studio +.ntvs_analysis.dat +node_modules/ + +# Typescript v1 declaration files +typings/ + +# 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 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/ + +# JetBrains Rider +.idea/ +*.sln.iml + +# CodeRush +.cr/ + +# Python Tools for Visual Studio (PTVS) +__pycache__/ +*.pyc + +# Cake - Uncomment if you are using it +# tools/** +# !tools/packages.config + +# Telerik's JustMock configuration file +*.jmconfig + +# BizTalk build output +*.btp.cs +*.btm.cs +*.odx.cs +*.xsd.cs \ No newline at end of file diff --git a/prebuild.xml b/prebuild.xml new file mode 100644 index 0000000..5cfeaba --- /dev/null +++ b/prebuild.xml @@ -0,0 +1,36 @@ + + + + ../../../bin + + + + + ../../../bin + + + + ../../../bin + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/A/Collections/MinHeap.cs b/src/A/Collections/MinHeap.cs new file mode 100644 index 0000000..161ac12 --- /dev/null +++ b/src/A/Collections/MinHeap.cs @@ -0,0 +1,128 @@ +using System; +using System.Collections.Generic; + +namespace Roy_T.AStar.Collections +{ + // C# Adaptation of a min heap built for C++ by Robin Thomas + // Original source code at: https://github.com/robin-thomas/min-heap + + internal sealed class MinHeap + where T : IComparable + { + private readonly List Items; + + public MinHeap() + { + this.Items = new List(); + } + + public int Count => this.Items.Count; + + public T Peek() => this.Items[0]; + + public void Insert(T item) + { + this.Items.Add(item); + this.SortItem(item); + } + + public T Extract() + { + var node = this.Items[0]; + + this.ReplaceFirstItemWithLastItem(); + this.Heapify(0); + + return node; + } + + public void Remove(T item) + { + if (this.Count < 2) + { + this.Clear(); + } + else + { + var index = this.Items.IndexOf(item); + if (index >= 0) + { + this.Items[index] = this.Items[this.Items.Count - 1]; + this.Items.RemoveAt(this.Items.Count - 1); + + this.Heapify(0); + } + } + } + + public void Clear() => this.Items.Clear(); + + private void ReplaceFirstItemWithLastItem() + { + this.Items[0] = this.Items[this.Items.Count - 1]; + this.Items.RemoveAt(this.Items.Count - 1); + } + + private void SortItem(T item) + { + var index = this.Items.Count - 1; + + while (HasParent(index)) + { + var parentIndex = GetParentIndex(index); + if (ItemAIsSmallerThanItemB(item, this.Items[parentIndex])) + { + this.Items[index] = this.Items[parentIndex]; + index = parentIndex; + } + else + { + break; + } + } + + this.Items[index] = item; + } + + private void Heapify(int startIndex) + { + var bestIndex = startIndex; + + if (this.HasLeftChild(startIndex)) + { + var leftChildIndex = GetLeftChildIndex(startIndex); + if (ItemAIsSmallerThanItemB(this.Items[leftChildIndex], this.Items[bestIndex])) + { + bestIndex = leftChildIndex; + } + } + + if (this.HasRightChild(startIndex)) + { + var rightChildIndex = GetRightChildIndex(startIndex); + if (ItemAIsSmallerThanItemB(this.Items[rightChildIndex], this.Items[bestIndex])) + { + bestIndex = rightChildIndex; + } + } + + if (bestIndex != startIndex) + { + var temp = this.Items[bestIndex]; + this.Items[bestIndex] = this.Items[startIndex]; + this.Items[startIndex] = temp; + this.Heapify(bestIndex); + } + } + + private static bool ItemAIsSmallerThanItemB(T a, T b) => a.CompareTo(b) < 0; + + private static bool HasParent(int index) => index > 0; + private bool HasLeftChild(int index) => GetLeftChildIndex(index) < this.Items.Count; + private bool HasRightChild(int index) => GetRightChildIndex(index) < this.Items.Count; + + private static int GetParentIndex(int i) => (i - 1) / 2; + private static int GetLeftChildIndex(int i) => (2 * i) + 1; + private static int GetRightChildIndex(int i) => (2 * i) + 2; + } +} diff --git a/src/A/Graphs/Edge.cs b/src/A/Graphs/Edge.cs new file mode 100644 index 0000000..5ad6db7 --- /dev/null +++ b/src/A/Graphs/Edge.cs @@ -0,0 +1,37 @@ +using Roy_T.AStar.Primitives; + +namespace Roy_T.AStar.Graphs +{ + public sealed class Edge : IEdge + { + private Velocity traversalVelocity; + + public Edge(INode start, INode end, Velocity traversalVelocity) + { + this.Start = start; + this.End = end; + + this.Distance = Distance.BeweenPositions(start.Position, end.Position); + this.TraversalVelocity = traversalVelocity; + } + + public Velocity TraversalVelocity + { + get => this.traversalVelocity; + set + { + this.traversalVelocity = value; + this.TraversalDuration = this.Distance / value; + } + } + + public Duration TraversalDuration { get; private set; } + + public Distance Distance { get; } + + public INode Start { get; } + public INode End { get; } + + public override string ToString() => $"{this.Start} -> {this.End} @ {this.TraversalVelocity}"; + } +} diff --git a/src/A/Graphs/IEdge.cs b/src/A/Graphs/IEdge.cs new file mode 100644 index 0000000..f1f3837 --- /dev/null +++ b/src/A/Graphs/IEdge.cs @@ -0,0 +1,13 @@ +using Roy_T.AStar.Primitives; + +namespace Roy_T.AStar.Graphs +{ + public interface IEdge + { + Velocity TraversalVelocity { get; set; } + Duration TraversalDuration { get; } + Distance Distance { get; } + INode Start { get; } + INode End { get; } + } +} \ No newline at end of file diff --git a/src/A/Graphs/INode.cs b/src/A/Graphs/INode.cs new file mode 100644 index 0000000..4d33621 --- /dev/null +++ b/src/A/Graphs/INode.cs @@ -0,0 +1,12 @@ +using System.Collections.Generic; +using Roy_T.AStar.Primitives; + +namespace Roy_T.AStar.Graphs +{ + public interface INode + { + Position Position { get; } + IList Incoming { get; } + IList Outgoing { get; } + } +} diff --git a/src/A/Graphs/Node.cs b/src/A/Graphs/Node.cs new file mode 100644 index 0000000..11ba14e --- /dev/null +++ b/src/A/Graphs/Node.cs @@ -0,0 +1,43 @@ +using System.Collections.Generic; +using Roy_T.AStar.Primitives; + +namespace Roy_T.AStar.Graphs +{ + public sealed class Node : INode + { + public Node(Position position) + { + this.Incoming = new List(0); + this.Outgoing = new List(0); + + this.Position = position; + } + + public IList Incoming { get; } + public IList Outgoing { get; } + + public Position Position { get; } + + internal void Connect(INode node, Velocity traversalVelocity) + { + var edge = new Edge(this, node, traversalVelocity); + this.Outgoing.Add(edge); + node.Incoming.Add(edge); + } + + internal void Disconnect(INode node) + { + for (var i = this.Outgoing.Count - 1; i >= 0; i--) + { + var edge = this.Outgoing[i]; + if (edge.End == node) + { + this.Outgoing.Remove(edge); + node.Incoming.Remove(edge); + } + } + } + + public override string ToString() => this.Position.ToString(); + } +} diff --git a/src/A/Grids/Grid.cs b/src/A/Grids/Grid.cs new file mode 100644 index 0000000..c6600ff --- /dev/null +++ b/src/A/Grids/Grid.cs @@ -0,0 +1,234 @@ +using System; +using System.Collections.Generic; +using Roy_T.AStar.Graphs; +using Roy_T.AStar.Primitives; + +namespace Roy_T.AStar.Grids +{ + public sealed class Grid + { + private readonly Node[,] Nodes; + + public static Grid CreateGridWithLateralConnections(GridSize gridSize, Size cellSize, Velocity traversalVelocity) + { + CheckArguments(gridSize, cellSize, traversalVelocity); + + var grid = new Grid(gridSize, cellSize); + + grid.CreateLateralConnections(traversalVelocity); + + return grid; + } + + public static Grid CreateGridWithDiagonalConnections(GridSize gridSize, Size cellSize, Velocity traversalVelocity) + { + CheckArguments(gridSize, cellSize, traversalVelocity); + + var grid = new Grid(gridSize, cellSize); + + grid.CreateDiagonalConnections(traversalVelocity); + + return grid; + } + + public static Grid CreateGridWithLateralAndDiagonalConnections(GridSize gridSize, Size cellSize, Velocity traversalVelocity) + { + CheckArguments(gridSize, cellSize, traversalVelocity); + + var grid = new Grid(gridSize, cellSize); + + grid.CreateDiagonalConnections(traversalVelocity); + grid.CreateLateralConnections(traversalVelocity); + + return grid; + } + + private static void CheckArguments(GridSize gridSize, Size cellSize, Velocity defaultSpeed) + { + if (gridSize.Columns < 1) + { + throw new ArgumentOutOfRangeException( + nameof(gridSize), $"Argument {nameof(gridSize.Columns)} is {gridSize.Columns} but should be >= 1"); + } + + if (gridSize.Rows < 1) + { + throw new ArgumentOutOfRangeException( + nameof(gridSize), $"Argument {nameof(gridSize.Rows)} is {gridSize.Rows} but should be >= 1"); + } + + if (cellSize.Width <= Distance.Zero) + { + throw new ArgumentOutOfRangeException( + nameof(cellSize), $"Argument {nameof(cellSize.Width)} is {cellSize.Width} but should be > {Distance.Zero}"); + } + + if (cellSize.Height <= Distance.Zero) + { + throw new ArgumentOutOfRangeException( + nameof(cellSize), $"Argument {nameof(cellSize.Height)} is {cellSize.Height} but should be > {Distance.Zero}"); + } + + if (defaultSpeed.MetersPerSecond <= 0.0f) + { + throw new ArgumentOutOfRangeException( + nameof(defaultSpeed), $"Argument {nameof(defaultSpeed)} is {defaultSpeed} but should be > 0.0 m/s"); + } + } + + private Grid(GridSize gridSize, Size cellSize) + { + this.GridSize = gridSize; + this.Nodes = new Node[gridSize.Columns, gridSize.Rows]; + + this.CreateNodes(cellSize); + } + + private void CreateNodes(Size cellSize) + { + for (var x = 0; x < this.Columns; x++) + { + for (var y = 0; y < this.Rows; y++) + { + this.Nodes[x, y] = new Node(Position.FromOffset(cellSize.Width * x, cellSize.Height * y)); + } + } + } + + private void CreateLateralConnections(Velocity defaultSpeed) + { + for (var x = 0; x < this.Columns; x++) + { + for (var y = 0; y < this.Rows; y++) + { + var node = this.Nodes[x, y]; + + if (x < this.Columns - 1) + { + var eastNode = this.Nodes[x + 1, y]; + node.Connect(eastNode, defaultSpeed); + eastNode.Connect(node, defaultSpeed); + } + + if (y < this.Rows - 1) + { + var southNode = this.Nodes[x, y + 1]; + node.Connect(southNode, defaultSpeed); + southNode.Connect(node, defaultSpeed); + } + } + } + } + + private void CreateDiagonalConnections(Velocity defaultSpeed) + { + for (var x = 0; x < this.Columns; x++) + { + for (var y = 0; y < this.Rows; y++) + { + var node = this.Nodes[x, y]; + + if (x < this.Columns - 1 && y < this.Rows - 1) + { + var southEastNode = this.Nodes[x + 1, y + 1]; + node.Connect(southEastNode, defaultSpeed); + southEastNode.Connect(node, defaultSpeed); + } + + if (x > 0 && y < this.Rows - 1) + { + var southWestNode = this.Nodes[x - 1, y + 1]; + node.Connect(southWestNode, defaultSpeed); + southWestNode.Connect(node, defaultSpeed); + } + } + } + } + + public GridSize GridSize { get; } + + public int Columns => this.GridSize.Columns; + + public int Rows => this.GridSize.Rows; + + public INode GetNode(GridPosition position) => this.Nodes[position.X, position.Y]; + + public IReadOnlyList GetAllNodes() + { + var list = new List(this.Columns * this.Rows); + + for (var x = 0; x < this.Columns; x++) + { + for (var y = 0; y < this.Rows; y++) + { + list.Add(this.Nodes[x, y]); + } + } + + return list; + } + + public void DisconnectNode(GridPosition position) + { + var node = this.Nodes[position.X, position.Y]; + + foreach (var outgoingEdge in node.Outgoing) + { + var opposite = outgoingEdge.End; + opposite.Incoming.Remove(outgoingEdge); + } + + node.Outgoing.Clear(); + + foreach (var incomingEdge in node.Incoming) + { + var opposite = incomingEdge.Start; + opposite.Outgoing.Remove(incomingEdge); + } + + node.Incoming.Clear(); + } + + public void RemoveDiagonalConnectionsIntersectingWithNode(GridPosition position) + { + var left = new GridPosition(position.X - 1, position.Y); + var top = new GridPosition(position.X, position.Y - 1); + var right = new GridPosition(position.X + 1, position.Y); + var bottom = new GridPosition(position.X, position.Y + 1); + + if (this.IsInsideGrid(left) && this.IsInsideGrid(top)) + { + this.RemoveEdge(left, top); + this.RemoveEdge(top, left); + } + + if (this.IsInsideGrid(top) && this.IsInsideGrid(right)) + { + this.RemoveEdge(top, right); + this.RemoveEdge(right, top); + } + + if (this.IsInsideGrid(right) && this.IsInsideGrid(bottom)) + { + this.RemoveEdge(right, bottom); + this.RemoveEdge(bottom, right); + } + + if (this.IsInsideGrid(bottom) && this.IsInsideGrid(left)) + { + this.RemoveEdge(bottom, left); + this.RemoveEdge(left, bottom); + } + } + + public void RemoveEdge(GridPosition from, GridPosition to) + { + var fromNode = this.Nodes[from.X, from.Y]; + var toNode = this.Nodes[to.X, to.Y]; + + fromNode.Disconnect(toNode); + } + + private bool IsInsideGrid(GridPosition position) => position.X >= 0 && position.X < this.Columns && position.Y >= 0 && position.Y < this.Rows; + } +} diff --git a/src/A/Paths/Path.cs b/src/A/Paths/Path.cs new file mode 100644 index 0000000..c5917cf --- /dev/null +++ b/src/A/Paths/Path.cs @@ -0,0 +1,28 @@ +using System.Collections.Generic; +using Roy_T.AStar.Graphs; +using Roy_T.AStar.Primitives; + +namespace Roy_T.AStar.Paths +{ + public sealed class Path + { + public Path(PathType type, IReadOnlyList edges) + { + this.Type = type; + this.Edges = edges; + + for (var i = 0; i < this.Edges.Count; i++) + { + this.Duration += this.Edges[i].TraversalDuration; + this.Distance += this.Edges[i].Distance; + } + } + + public PathType Type { get; } + + public Duration Duration { get; } + + public IReadOnlyList Edges { get; } + public Distance Distance { get; } + } +} diff --git a/src/A/Paths/PathFinder.cs b/src/A/Paths/PathFinder.cs new file mode 100644 index 0000000..27d5e53 --- /dev/null +++ b/src/A/Paths/PathFinder.cs @@ -0,0 +1,124 @@ +using System.Collections.Generic; +using System.Linq; +using Roy_T.AStar.Collections; +using Roy_T.AStar.Graphs; +using Roy_T.AStar.Grids; +using Roy_T.AStar.Primitives; + +namespace Roy_T.AStar.Paths +{ + public sealed class PathFinder + { + private readonly MinHeap Interesting; + private readonly Dictionary Nodes; + private readonly PathReconstructor PathReconstructor; + + private PathFinderNode NodeClosestToGoal; + + public PathFinder() + { + this.Interesting = new MinHeap(); + this.Nodes = new Dictionary(); + this.PathReconstructor = new PathReconstructor(); + } + + public Path FindPath(GridPosition start, GridPosition end, Grid grid) + { + var startNode = grid.GetNode(start); + var endNode = grid.GetNode(end); + + var maximumVelocity = grid.GetAllNodes().SelectMany(n => n.Outgoing).Select(e => e.TraversalVelocity).Max(); + + return this.FindPath(startNode, endNode, maximumVelocity); + } + + public Path FindPath(GridPosition start, GridPosition end, Grid grid, Velocity maximumVelocity) + { + var startNode = grid.GetNode(start); + var endNode = grid.GetNode(end); + + return this.FindPath(startNode, endNode, maximumVelocity); + } + + public Path FindPath(INode start, INode goal, Velocity maximumVelocity) + { + this.ResetState(); + this.AddFirstNode(start, goal, maximumVelocity); + + while (this.Interesting.Count > 0) + { + var current = this.Interesting.Extract(); + if (GoalReached(goal, current)) + { + return this.PathReconstructor.ConstructPathTo(current.Node, goal); + } + + this.UpdateNodeClosestToGoal(current); + + foreach (var edge in current.Node.Outgoing) + { + var oppositeNode = edge.End; + var costSoFar = current.DurationSoFar + edge.TraversalDuration; + + if (this.Nodes.TryGetValue(oppositeNode, out var node)) + { + this.UpdateExistingNode(goal, maximumVelocity, current, edge, oppositeNode, costSoFar, node); + } + else + { + this.InsertNode(oppositeNode, edge, goal, costSoFar, maximumVelocity); + } + } + } + + return this.PathReconstructor.ConstructPathTo(this.NodeClosestToGoal.Node, goal); + } + + private void ResetState() + { + this.Interesting.Clear(); + this.Nodes.Clear(); + this.PathReconstructor.Clear(); + this.NodeClosestToGoal = null; + } + + private void AddFirstNode(INode start, INode goal, Velocity maximumVelocity) + { + var head = new PathFinderNode(start, Duration.Zero, ExpectedDuration(start, goal, maximumVelocity)); + this.Interesting.Insert(head); + this.Nodes.Add(head.Node, head); + this.NodeClosestToGoal = head; + } + + private static bool GoalReached(INode goal, PathFinderNode current) => current.Node == goal; + + private void UpdateNodeClosestToGoal(PathFinderNode current) + { + if (current.ExpectedRemainingTime < this.NodeClosestToGoal.ExpectedRemainingTime) + { + this.NodeClosestToGoal = current; + } + } + + private void UpdateExistingNode(INode goal, Velocity maximumVelocity, PathFinderNode current, IEdge edge, INode oppositeNode, Duration costSoFar, PathFinderNode node) + { + if (node.DurationSoFar > costSoFar) + { + this.Interesting.Remove(node); + this.InsertNode(oppositeNode, edge, goal, costSoFar, maximumVelocity); + } + } + + private void InsertNode(INode current, IEdge via, INode goal, Duration costSoFar, Velocity maximumVelocity) + { + this.PathReconstructor.SetCameFrom(current, via); + + var node = new PathFinderNode(current, costSoFar, ExpectedDuration(current, goal, maximumVelocity)); + this.Interesting.Insert(node); + this.Nodes[current] = node; + } + + public static Duration ExpectedDuration(INode a, INode b, Velocity maximumVelocity) + => Distance.BeweenPositions(a.Position, b.Position) / maximumVelocity; + } +} diff --git a/src/A/Paths/PathFinderNode.cs b/src/A/Paths/PathFinderNode.cs new file mode 100644 index 0000000..370dfdc --- /dev/null +++ b/src/A/Paths/PathFinderNode.cs @@ -0,0 +1,25 @@ +using System; +using Roy_T.AStar.Graphs; +using Roy_T.AStar.Primitives; + +namespace Roy_T.AStar.Paths +{ + internal sealed class PathFinderNode : IComparable + { + public PathFinderNode(INode node, Duration durationSoFar, Duration expectedRemainingTime) + { + this.Node = node; + this.DurationSoFar = durationSoFar; + this.ExpectedRemainingTime = expectedRemainingTime; + this.ExpectedTotalTime = this.DurationSoFar + this.ExpectedRemainingTime; + } + + public INode Node { get; } + public Duration DurationSoFar { get; } + public Duration ExpectedRemainingTime { get; } + public Duration ExpectedTotalTime { get; } + + public int CompareTo(PathFinderNode other) => this.ExpectedTotalTime.CompareTo(other.ExpectedTotalTime); + public override string ToString() => $"📍{{{this.Node.Position.X}, {this.Node.Position.Y}}}, ⏱~{this.ExpectedTotalTime}"; + } +} diff --git a/src/A/Paths/PathReconstructor.cs b/src/A/Paths/PathReconstructor.cs new file mode 100644 index 0000000..400e2cf --- /dev/null +++ b/src/A/Paths/PathReconstructor.cs @@ -0,0 +1,37 @@ +using System.Collections.Generic; +using Roy_T.AStar.Graphs; + +namespace Roy_T.AStar.Paths +{ + internal sealed class PathReconstructor + { + private readonly Dictionary CameFrom; + + public PathReconstructor() + { + this.CameFrom = new Dictionary(); + } + + public void SetCameFrom(INode node, IEdge via) + => this.CameFrom[node] = via; + + public Path ConstructPathTo(INode node, INode goal) + { + var current = node; + var edges = new List(); + + while (this.CameFrom.TryGetValue(current, out var via)) + { + edges.Add(via); + current = via.Start; + } + + edges.Reverse(); + + var type = node == goal ? PathType.Complete : PathType.ClosestApproach; + return new Path(type, edges); + } + + public void Clear() => this.CameFrom.Clear(); + } +} diff --git a/src/A/Paths/PathType.cs b/src/A/Paths/PathType.cs new file mode 100644 index 0000000..8f1e891 --- /dev/null +++ b/src/A/Paths/PathType.cs @@ -0,0 +1,8 @@ +namespace Roy_T.AStar.Paths +{ + public enum PathType + { + Complete, + ClosestApproach + } +} diff --git a/src/A/Primitives/Distance.cs b/src/A/Primitives/Distance.cs new file mode 100644 index 0000000..25fbb4d --- /dev/null +++ b/src/A/Primitives/Distance.cs @@ -0,0 +1,73 @@ +using System; + +namespace Roy_T.AStar.Primitives +{ + public struct Distance : IComparable, IEquatable + { + public static Distance Zero => new Distance(0); + + private Distance(float meters) + { + this.Meters = meters; + } + + public float Meters { get; } + + public static Distance FromMeters(float meters) => new Distance(meters); + + public static Distance BeweenPositions(Position a, Position b) + { + var sX = a.X; + var sY = a.Y; + var eX = b.X; + var eY = b.Y; + + var d0 = (eX - sX) * (eX - sX); + var d1 = (eY - sY) * (eY - sY); + + return FromMeters((float)Math.Sqrt(d0 + d1)); + } + + public static Distance operator +(Distance a, Distance b) + => new Distance(a.Meters + b.Meters); + + public static Distance operator -(Distance a, Distance b) + => new Distance(a.Meters - b.Meters); + + public static Distance operator *(Distance a, float b) + => new Distance(a.Meters * b); + + public static Distance operator /(Distance a, float b) + => new Distance(a.Meters / b); + + public static bool operator >(Distance a, Distance b) + => a.Meters > b.Meters; + + public static bool operator <(Distance a, Distance b) + => a.Meters < b.Meters; + + public static bool operator >=(Distance a, Distance b) + => a.Meters >= b.Meters; + + public static bool operator <=(Distance a, Distance b) + => a.Meters <= b.Meters; + + public static bool operator ==(Distance a, Distance b) + => a.Equals(b); + + public static bool operator !=(Distance a, Distance b) + => !a.Equals(b); + + public static Duration operator /(Distance distance, Velocity velocity) + => Duration.FromSeconds(distance.Meters / velocity.MetersPerSecond); + + public override string ToString() => $"{this.Meters:F2}m"; + + public override bool Equals(object obj) => obj is Distance distance && this.Equals(distance); + public bool Equals(Distance other) => this.Meters == other.Meters; + + public int CompareTo(Distance other) => this.Meters.CompareTo(other.Meters); + + public override int GetHashCode() => -1609761766 + this.Meters.GetHashCode(); + } +} diff --git a/src/A/Primitives/Duration.cs b/src/A/Primitives/Duration.cs new file mode 100644 index 0000000..fa381e3 --- /dev/null +++ b/src/A/Primitives/Duration.cs @@ -0,0 +1,52 @@ +using System; + +namespace Roy_T.AStar.Primitives +{ + public struct Duration : IComparable, IEquatable + { + public static Duration Zero => new Duration(0); + + private Duration(float seconds) + { + this.Seconds = seconds; + } + + public float Seconds { get; } + + public static Duration FromSeconds(float seconds) => new Duration(seconds); + + public static Duration operator +(Duration a, Duration b) + => new Duration(a.Seconds + b.Seconds); + + public static Duration operator -(Duration a, Duration b) + => new Duration(a.Seconds - b.Seconds); + + public static bool operator >(Duration a, Duration b) + => a.Seconds > b.Seconds; + + public static bool operator <(Duration a, Duration b) + => a.Seconds < b.Seconds; + + public static bool operator >=(Duration a, Duration b) + => a.Seconds >= b.Seconds; + + public static bool operator <=(Duration a, Duration b) + => a.Seconds <= b.Seconds; + + public static bool operator ==(Duration a, Duration b) + => a.Equals(b); + + public static bool operator !=(Duration a, Duration b) + => !a.Equals(b); + + public override string ToString() => $"{this.Seconds:F2}s"; + + public override bool Equals(object obj) => obj is Duration duration && this.Equals(duration); + + public bool Equals(Duration other) => this.Seconds == other.Seconds; + + public int CompareTo(Duration other) => this.Seconds.CompareTo(other.Seconds); + + public override int GetHashCode() => -1609761766 + this.Seconds.GetHashCode(); + } +} diff --git a/src/A/Primitives/GridPosition.cs b/src/A/Primitives/GridPosition.cs new file mode 100644 index 0000000..956964a --- /dev/null +++ b/src/A/Primitives/GridPosition.cs @@ -0,0 +1,32 @@ +using System; + +namespace Roy_T.AStar.Primitives +{ + public struct GridPosition : IEquatable + { + public static GridPosition Zero => new GridPosition(0, 0); + + public GridPosition(int x, int y) + { + this.X = x; + this.Y = y; + } + + public int X { get; } + public int Y { get; } + + public static bool operator ==(GridPosition a, GridPosition b) + => a.Equals(b); + + public static bool operator !=(GridPosition a, GridPosition b) + => !a.Equals(b); + + public override string ToString() => $"({this.X}, {this.Y})"; + + public override bool Equals(object obj) => obj is GridPosition GridPosition && this.Equals(GridPosition); + + public bool Equals(GridPosition other) => this.X == other.X && this.Y == other.Y; + + public override int GetHashCode() => -1609761766 + this.X + this.Y; + } +} diff --git a/src/A/Primitives/GridSize.cs b/src/A/Primitives/GridSize.cs new file mode 100644 index 0000000..6d0c056 --- /dev/null +++ b/src/A/Primitives/GridSize.cs @@ -0,0 +1,30 @@ +using System; + +namespace Roy_T.AStar.Primitives +{ + public struct GridSize : IEquatable + { + public GridSize(int columns, int rows) + { + this.Columns = columns; + this.Rows = rows; + } + + public int Columns { get; } + public int Rows { get; } + + public static bool operator ==(GridSize a, GridSize b) + => a.Equals(b); + + public static bool operator !=(GridSize a, GridSize b) + => !a.Equals(b); + + public override string ToString() => $"(columns: {this.Columns}, rows: {this.Rows})"; + + public override bool Equals(object obj) => obj is GridSize GridSize && this.Equals(GridSize); + + public bool Equals(GridSize other) => this.Columns == other.Columns && this.Rows == other.Rows; + + public override int GetHashCode() => -1609761766 + this.Columns.GetHashCode() + this.Rows.GetHashCode(); + } +} diff --git a/src/A/Primitives/Position.cs b/src/A/Primitives/Position.cs new file mode 100644 index 0000000..51387c8 --- /dev/null +++ b/src/A/Primitives/Position.cs @@ -0,0 +1,35 @@ +using System; + +namespace Roy_T.AStar.Primitives +{ + public struct Position : IEquatable + { + public static Position Zero => new Position(0, 0); + + public Position(float x, float y) + { + this.X = x; + this.Y = y; + } + + public static Position FromOffset(Distance xDistanceFromOrigin, Distance yDistanceFromOrigin) + => new Position(xDistanceFromOrigin.Meters, yDistanceFromOrigin.Meters); + + public float X { get; } + public float Y { get; } + + public static bool operator ==(Position a, Position b) + => a.Equals(b); + + public static bool operator !=(Position a, Position b) + => !a.Equals(b); + + public override string ToString() => $"({this.X:F2}, {this.Y:F2})"; + + public override bool Equals(object obj) => obj is Position position && this.Equals(position); + + public bool Equals(Position other) => this.X == other.X && this.Y == other.Y; + + public override int GetHashCode() => -1609761766 + this.X.GetHashCode() + this.Y.GetHashCode(); + } +} diff --git a/src/A/Primitives/Size.cs b/src/A/Primitives/Size.cs new file mode 100644 index 0000000..2304f36 --- /dev/null +++ b/src/A/Primitives/Size.cs @@ -0,0 +1,30 @@ +using System; + +namespace Roy_T.AStar.Primitives +{ + public struct Size : IEquatable + { + public Size(Distance width, Distance height) + { + this.Width = width; + this.Height = height; + } + + public Distance Width { get; } + public Distance Height { get; } + + public static bool operator ==(Size a, Size b) + => a.Equals(b); + + public static bool operator !=(Size a, Size b) + => !a.Equals(b); + + public override string ToString() => $"(width: {this.Width}, height: {this.Height})"; + + public override bool Equals(object obj) => obj is Size Size && this.Equals(Size); + + public bool Equals(Size other) => this.Width == other.Width && this.Height == other.Height; + + public override int GetHashCode() => -1609761766 + this.Width.GetHashCode() + this.Height.GetHashCode(); + } +} diff --git a/src/A/Primitives/Velocity.cs b/src/A/Primitives/Velocity.cs new file mode 100644 index 0000000..2ee7122 --- /dev/null +++ b/src/A/Primitives/Velocity.cs @@ -0,0 +1,57 @@ +using System; + +namespace Roy_T.AStar.Primitives +{ + public struct Velocity : IComparable, IEquatable + { + private Velocity(float metersPerSecond) + { + this.MetersPerSecond = metersPerSecond; + } + + public float MetersPerSecond { get; } + + public float KilometersPerHour => this.MetersPerSecond * 3.6f; + + + public static Velocity FromMetersPerSecond(float metersPerSecond) + => new Velocity(metersPerSecond); + + public static Velocity FromKilometersPerHour(float kilometersPerHour) + => new Velocity(kilometersPerHour / 3.6f); + + public static Velocity operator +(Velocity a, Velocity b) + => new Velocity(a.MetersPerSecond + b.MetersPerSecond); + + public static Velocity operator -(Velocity a, Velocity b) + => new Velocity(a.MetersPerSecond - b.MetersPerSecond); + + public static bool operator >(Velocity a, Velocity b) + => a.MetersPerSecond > b.MetersPerSecond; + + public static bool operator <(Velocity a, Velocity b) + => a.MetersPerSecond < b.MetersPerSecond; + + public static bool operator >=(Velocity a, Velocity b) + => a.MetersPerSecond >= b.MetersPerSecond; + + public static bool operator <=(Velocity a, Velocity b) + => a.MetersPerSecond <= b.MetersPerSecond; + + public static bool operator ==(Velocity a, Velocity b) + => a.Equals(b); + + public static bool operator !=(Velocity a, Velocity b) + => !a.Equals(b); + + public override string ToString() => $"{this.MetersPerSecond:F2} m/s"; + + public override bool Equals(object obj) => obj is Velocity velocity && this.MetersPerSecond == velocity.MetersPerSecond; + + public bool Equals(Velocity other) => this.MetersPerSecond == other.MetersPerSecond; + + public int CompareTo(Velocity other) => this.MetersPerSecond.CompareTo(other.MetersPerSecond); + + public override int GetHashCode() => -1419927970 + this.MetersPerSecond.GetHashCode(); + } +} diff --git a/src/BasicPathFinding.cs b/src/BasicPathFinding.cs new file mode 100644 index 0000000..aae4df2 --- /dev/null +++ b/src/BasicPathFinding.cs @@ -0,0 +1,507 @@ +using log4net; +using Mono.Addins; +using Nini.Config; +using OpenMetaverse; +using OpenSim.Region.CoreModules.World.LegacyMap; +using OpenSim.Region.Framework.Interfaces; +using OpenSim.Region.Framework.Scenes; +using OpenSim.Region.ScriptEngine.Interfaces; +using OpenSim.Region.ScriptEngine.Shared.Api; +using Roy_T.AStar.Graphs; +using Roy_T.AStar.Grids; +using Roy_T.AStar.Paths; +using Roy_T.AStar.Primitives; +using System; +using System.Collections.Generic; +using System.Drawing; +using System.Drawing.Imaging; +using System.Reflection; +using System.Threading; + +using LSL_Float = OpenSim.Region.ScriptEngine.Shared.LSL_Types.LSLFloat; +using LSL_Integer = OpenSim.Region.ScriptEngine.Shared.LSL_Types.LSLInteger; +using LSL_Key = OpenSim.Region.ScriptEngine.Shared.LSL_Types.LSLString; +using LSL_List = OpenSim.Region.ScriptEngine.Shared.LSL_Types.list; +using LSL_Rotation = OpenSim.Region.ScriptEngine.Shared.LSL_Types.Quaternion; +using LSL_String = OpenSim.Region.ScriptEngine.Shared.LSL_Types.LSLString; +using LSL_Vector = OpenSim.Region.ScriptEngine.Shared.LSL_Types.Vector3; + +[assembly: Addin("BasicPathFindingModule", "0.1")] +[assembly: AddinDependency("OpenSim.Region.Framework", OpenSim.VersionInfo.VersionNumber)] +namespace OpenSim.Modules.PathFinding +{ + [Extension(Path = "/OpenSim/RegionModules", NodeName = "RegionModule", Id = "BasicPathFindingModule")] + + public class BasicPathFindingModule : INonSharedRegionModule + { + #region Region Module + private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); + + private Scene m_scene = null; + private IConfig m_config = null; + private bool m_enabled = true; + private IScriptModuleComms m_scriptModule; + + private List m_environments = new List(); + + public string Name + { + get { return "BasicPathFindingModule"; } + } + + public Type ReplaceableInterface + { + get { return null; } + } + + public void AddRegion(Scene scene) + { + + } + + public void Close() + { + + } + + public void Initialise(IConfigSource source) + { + try + { + m_config = source.Configs["XEngine"]; + + if (m_config != null) + { + m_enabled = m_config.GetBoolean("EnablePathFinding", m_enabled); + } + else + { + m_log.Error("[" + Name + "]: Cant find config."); + } + } + catch (Exception e) + { + m_log.ErrorFormat("[" + Name + "]: initialization error: {0}", e.Message); + return; + } + + if (m_enabled) + { + m_log.Info("[" + Name + "]: module is enabled"); + } + else + { + m_log.Info("[" + Name + "]: module is disabled"); + } + } + + public void RegionLoaded(Scene scene) + { + if (m_enabled) + { + m_log.Info("[" + Name + "]: Load region " + scene.Name); + + m_scene = scene; + m_scriptModule = m_scene.RequestModuleInterface(); + if (m_scriptModule == null) + { + m_log.ErrorFormat("[" + Name + "]: Failed to load IScriptModuleComms!"); + m_enabled = false; + return; + } + + try + { + m_scriptModule.RegisterScriptInvocation(this, "osGeneratePathEnv"); + m_scriptModule.RegisterScriptInvocation(this, "osKeepAlivePathEnv"); + m_scriptModule.RegisterScriptInvocation(this, "osSetPathPositionData"); + m_scriptModule.RegisterScriptInvocation(this, "osSetPathLineData"); + m_scriptModule.RegisterScriptInvocation(this, "osGeneratePath"); + + m_scriptModule.RegisterScriptInvocation(this, "osGetSearchableObjectList"); + + m_scriptModule.RegisterScriptInvocation(this, "osGenerateDebugImage"); + + m_scriptModule.RegisterConstant("PATH_ENV_SUCCESSFUL", 19850); + m_scriptModule.RegisterConstant("PATH_ENV_ERR_NOT_FOUND", 19851); + m_scriptModule.RegisterConstant("PATH_ENV_ERR_OUT_OF_RANGE", 19852); + m_scriptModule.RegisterConstant("PATH_ENV_ERR_NOT_IN_LINE", 19853); + m_scriptModule.RegisterConstant("PATH_ENV_ERR_START_OR_END_UNKNOWN", 19854); + m_scriptModule.RegisterConstant("PATH_ENV_ERR_TARGET_NOT_REACHABLE", 19855); + m_scriptModule.RegisterConstant("PATH_ENV_ERR_UNKNOWN", 19860); + } + catch (Exception e) + { + m_log.WarnFormat("[" + Name + "]: script method registration failed; {0}", e.Message); + m_enabled = false; + } + + m_log.Info("[" + Name + "]: Region loading done!"); + } + } + + public void RemoveRegion(Scene scene) + { + + } + + #endregion + + #region Asyn Funktions + + private void generatePathEnvironment(ScriptRequestData requestData) + { + lock(m_environments) + { + try + { + UUID _envID = UUID.Random(); + Environment _newEnv = new Environment(_envID.ToString(), (int)m_scene.RegionInfo.RegionSizeX); + + m_environments.Add(_newEnv); + + m_scriptModule.DispatchReply(requestData.ScriptID, 19850, _envID.ToString(), requestData.RequestID.ToString()); + } + catch (Exception _error) + { + m_scriptModule.DispatchReply(requestData.ScriptID, 19860, _error.Message, requestData.RequestID.ToString()); + } + } + } + + private void keepAlivePathEnv(ScriptRequestData requestData) + { + lock (m_environments) + { + try + { + Environment _env = m_environments.Find(X => X.ID == requestData.EnvironmentID); + + if (_env != null) + { + _env.LastTimeUsed = (Int32)(DateTime.UtcNow.Subtract(new DateTime(1970, 1, 1))).TotalSeconds; + m_scriptModule.DispatchReply(requestData.ScriptID, 19850, "", requestData.RequestID.ToString()); + return; + } + + m_scriptModule.DispatchReply(requestData.ScriptID, 19851, "", requestData.RequestID.ToString()); + } + catch (Exception _error) + { + m_scriptModule.DispatchReply(requestData.ScriptID, 19860, _error.Message, requestData.RequestID.ToString()); + } + } + } + + private void setPositionData(ScriptRequestData requestData, Vector3 position, int walkable, int isTarget, int isStart) + { + lock(m_environments) + { + try + { + Environment _env = m_environments.Find(X => X.ID == requestData.EnvironmentID); + + if (_env != null) + { + PathNode _node = _env.Nodes.Find(X => X.PositionX == (int)position.X && X.PositionY == (int)position.Y); + + if (_node == null) + { + _node = new PathNode((int)position.X, (int)position.Y, false); + _env.Nodes.Add(_node); + } + + if (walkable == 1) + _node.Walkable = true; + + if (walkable == 0) + _node.Walkable = false; + + if (isTarget == 1) + _env.Target = _node; + + if (isStart == 1) + _env.Start = _node; + + return; + } + + m_scriptModule.DispatchReply(requestData.ScriptID, 19851, "", requestData.RequestID.ToString()); + } + catch (Exception _error) + { + m_scriptModule.DispatchReply(requestData.ScriptID, 19860, _error.Message, requestData.RequestID.ToString()); + } + } + } + + private void setLineData(ScriptRequestData requestData, Vector3 start, Vector3 target, int walkable) + { + lock(m_environments) + { + try + { + Environment _env = m_environments.Find(X => X.ID == requestData.EnvironmentID); + + if (_env != null) + { + if ((int)start.X == (int)target.X || (int)start.Y == (int)target.Y) + { + if ((int)start.X == (int)target.X && (int)start.Y == (int)target.Y) + { + m_scriptModule.DispatchReply(requestData.ScriptID, 19850, "", requestData.RequestID.ToString()); + return; + } + + Vector3 _PointA = new Vector3(0, 0, 0); + Vector3 _PointB = new Vector3(0, 0, 0); + + if ((int)start.X != (int)target.X) + { + if ((int)start.X < (int)target.X) + { + _PointA = start; + _PointB = target; + } + + if ((int)start.X > (int)target.X) + { + _PointA = target; + _PointB = start; + } + + while ((int)_PointA.X <= (int)_PointB.X) + { + setPositionData(requestData, _PointA, walkable, 0, 0); + _PointA.X = (int)_PointA.X + 1; + } + } + + if ((int)start.Y != (int)target.Y) + { + if ((int)start.Y < (int)target.Y) + { + _PointA = start; + _PointB = target; + } + + if ((int)start.Y > (int)target.Y) + { + _PointA = target; + _PointB = start; + } + + while ((int)_PointA.Y <= (int)_PointB.Y) + { + setPositionData(requestData, _PointA, walkable, 0, 0); + _PointA.Y = (int)_PointA.Y + 1; + } + } + + m_scriptModule.DispatchReply(requestData.ScriptID, 19850, "", requestData.RequestID.ToString()); + return; + } + else + { + m_scriptModule.DispatchReply(requestData.ScriptID, 19853, "", requestData.RequestID.ToString()); + } + } + else + { + m_scriptModule.DispatchReply(requestData.ScriptID, 19851, "", requestData.RequestID.ToString()); + } + } + catch (Exception _error) + { + m_scriptModule.DispatchReply(requestData.ScriptID, 19860, _error.Message, requestData.RequestID.ToString()); + } + } + } + + private void generatePath(ScriptRequestData requestData) + { + try + { + lock (m_environments) + { + Environment _env = m_environments.Find(X => X.ID == requestData.EnvironmentID); + + if (_env.Start != null && _env.Target != null) + { + if (_env.Start.PositionX == _env.Target.PositionX && _env.Start.PositionY == _env.Target.PositionY) + m_scriptModule.DispatchReply(requestData.ScriptID, 19850, "", requestData.RequestID.ToString()); + + GridSize _pathFindingGridSize = new GridSize(_env.Size, _env.Size); + Roy_T.AStar.Primitives.Size _pathFindingCellSize = new Roy_T.AStar.Primitives.Size(Distance.FromMeters(1), Distance.FromMeters(1)); + Velocity _pathFindingVelocity = Velocity.FromKilometersPerHour(11.5f); + + Grid _pathFindingGrid = Grid.CreateGridWithLateralAndDiagonalConnections(_pathFindingGridSize, _pathFindingCellSize, _pathFindingVelocity); + + foreach (INode _thisNode in _pathFindingGrid.GetAllNodes()) + { + PathNode _node = _env.Nodes.Find(X => X.PositionX == (int)_thisNode.Position.X && X.PositionY == (int)_thisNode.Position.Y); + + if (_node == null) + { + _pathFindingGrid.DisconnectNode(new GridPosition((int)_thisNode.Position.X, (int)_thisNode.Position.Y)); + _pathFindingGrid.RemoveDiagonalConnectionsIntersectingWithNode(new GridPosition((int)_thisNode.Position.X, (int)_thisNode.Position.Y)); + } + } + + PathFinder pathFinder = new PathFinder(); + Path _pathFindingPath = pathFinder.FindPath(new GridPosition(_env.Start.PositionX, _env.Start.PositionY), new GridPosition(_env.Target.PositionX, _env.Target.PositionY), _pathFindingGrid); + + String _pathString = ""; + int lastX = 0; + int lastY = 0; + + foreach (var _thisEdge in _pathFindingPath.Edges) + { + if (lastX != (int)_thisEdge.End.Position.X && lastY != (int)_thisEdge.End.Position.Y) + { + _pathString += "<" + _thisEdge.End.Position.X + ", " + _thisEdge.End.Position.Y + ", 0>;"; + + lastX = (int)_thisEdge.End.Position.X; + lastY = (int)_thisEdge.End.Position.Y; + } + } + + _pathString += "<" + _pathFindingPath.Edges[_pathFindingPath.Edges.Count - 1].End.Position.X + ", " + _pathFindingPath.Edges[_pathFindingPath.Edges.Count - 1].End.Position.Y + ", 0>;"; + + + if (_pathFindingPath.Type == PathType.Complete) + { + m_scriptModule.DispatchReply(requestData.ScriptID, 19850, _pathString, requestData.RequestID.ToString()); + } + else + { + m_scriptModule.DispatchReply(requestData.ScriptID, 19855, _pathString, requestData.RequestID.ToString()); + } + } + else + { + m_scriptModule.DispatchReply(requestData.ScriptID, 19854, "", requestData.RequestID.ToString()); + } + } + }catch(Exception _error) + { + m_scriptModule.DispatchReply(requestData.ScriptID, 19860, _error.Message, requestData.RequestID.ToString()); + } + } + + private void generateDebugImage(ScriptRequestData requestData) + { + lock(m_environments) + { + try + { + Environment _env = m_environments.Find(X => X.ID == requestData.EnvironmentID); + + if (_env != null) + { + Bitmap _bitmap = new Bitmap(_env.Size, _env.Size); + + foreach (PathNode thisNode in _env.Nodes) + { + if (thisNode.Walkable) + _bitmap.SetPixel(thisNode.PositionX, thisNode.PositionY, Color.Green); + + if (!thisNode.Walkable) + _bitmap.SetPixel(thisNode.PositionX, thisNode.PositionY, Color.Black); + } + + _bitmap.Save(requestData.EnvironmentID + ".png"); + m_scriptModule.DispatchReply(requestData.ScriptID, 19850, requestData.EnvironmentID + ".png", requestData.RequestID.ToString()); + return; + } + + m_scriptModule.DispatchReply(requestData.ScriptID, 19851, "", requestData.RequestID.ToString()); + } + catch (Exception _error) + { + m_scriptModule.DispatchReply(requestData.ScriptID, 19860, _error.Message, requestData.RequestID.ToString()); + } + } + } + + #endregion + + #region Script Funktions + + [ScriptInvocation] + public string osGeneratePathEnv(UUID hostID, UUID scriptID) + { + UUID requestKey = UUID.Random(); + + SceneObjectGroup _host = m_scene.GetSceneObjectGroup(hostID); + (new Thread(delegate () { generatePathEnvironment(new ScriptRequestData(hostID, scriptID, null, requestKey)); })).Start(); + + return requestKey.ToString(); + } + + [ScriptInvocation] + public string osKeepAlivePathEnv(UUID hostID, UUID scriptID, String environmentID) + { + UUID requestKey = UUID.Random(); + + SceneObjectGroup _host = m_scene.GetSceneObjectGroup(hostID); + (new Thread(delegate () { keepAlivePathEnv(new ScriptRequestData(hostID, scriptID, environmentID, requestKey)); })).Start(); + + return requestKey.ToString(); + } + + [ScriptInvocation] + public void osSetPathPositionData(UUID hostID, UUID scriptID, String environmentID, Vector3 position, int walkable, int isTarget, int isStart) + { + SceneObjectGroup _host = m_scene.GetSceneObjectGroup(hostID); + (new Thread(delegate () { setPositionData(new ScriptRequestData(hostID, scriptID, environmentID), position, walkable, isTarget, isStart); })).Start(); + } + + [ScriptInvocation] + public void osSetPathLineData(UUID hostID, UUID scriptID, String environmentID, Vector3 start, Vector3 target, int walkable) + { + SceneObjectGroup _host = m_scene.GetSceneObjectGroup(hostID); + (new Thread(delegate () { setLineData(new ScriptRequestData(hostID, scriptID, environmentID), start, target, walkable); })).Start(); + } + + [ScriptInvocation] + public string osGeneratePath(UUID hostID, UUID scriptID, String environmentID) + { + UUID requestKey = UUID.Random(); + + SceneObjectGroup _host = m_scene.GetSceneObjectGroup(hostID); + (new Thread(delegate () { generatePath(new ScriptRequestData(hostID, scriptID, environmentID, requestKey)); })).Start(); + + return requestKey.ToString(); + } + + [ScriptInvocation] + public string osGenerateDebugImage(UUID hostID, UUID scriptID, String environmentID) + { + UUID requestKey = UUID.Random(); + + SceneObjectGroup _host = m_scene.GetSceneObjectGroup(hostID); + (new Thread(delegate () { generateDebugImage(new ScriptRequestData(hostID, scriptID, environmentID, requestKey)); })).Start(); + + return requestKey.ToString(); + } + + [ScriptInvocation] + public object[] osGetSearchableObjectList(UUID hostID, UUID scriptID, String searchString) + { + List returnList = new List(); + + foreach (SceneObjectGroup thisGroup in m_scene.GetSceneObjectGroups()) + { + if(thisGroup.Name == searchString) + returnList.Add(thisGroup.UUID); + } + + return returnList.ToArray(); + } + + + #endregion + + } +} diff --git a/src/Environment.cs b/src/Environment.cs new file mode 100644 index 0000000..a012df7 --- /dev/null +++ b/src/Environment.cs @@ -0,0 +1,44 @@ +using OpenMetaverse; +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; + +namespace OpenSim.Modules.PathFinding +{ + class Environment + { + public String ID = null; + + public int Size = 0; + public int LastTimeUsed = 0; + + public PathNode Start = null; + public PathNode Target = null; + + private List m_nodes = new List(); + public List Nodes + { + get + { + LastTimeUsed = (Int32)(DateTime.UtcNow.Subtract(new DateTime(1970, 1, 1))).TotalSeconds; + return m_nodes; + } + set + { + LastTimeUsed = (Int32)(DateTime.UtcNow.Subtract(new DateTime(1970, 1, 1))).TotalSeconds; + m_nodes = value; + } + } + + public Environment(String envID, int size) + { + ID = envID; + + Size = size; + + LastTimeUsed = (Int32)(DateTime.UtcNow.Subtract(new DateTime(1970, 1, 1))).TotalSeconds; + } + } +} diff --git a/src/PathNode.cs b/src/PathNode.cs new file mode 100644 index 0000000..3e2c224 --- /dev/null +++ b/src/PathNode.cs @@ -0,0 +1,34 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; + +namespace OpenSim.Modules.PathFinding +{ + class PathNode + { + public int PositionX = 0; + public int PositionY = 0; + + public bool Walkable = false; + + public int f_cost = 99999; + + public PathNode Parent = null; + + public PathNode(int positionX, int positionY) + { + PositionX = positionX; + PositionY = positionY; + } + + public PathNode(int positionX, int positionY, bool isWalkable) + { + PositionX = positionX; + PositionY = positionY; + + Walkable = isWalkable; + } + } +} diff --git a/src/ScriptRequestData.cs b/src/ScriptRequestData.cs new file mode 100644 index 0000000..3595fd3 --- /dev/null +++ b/src/ScriptRequestData.cs @@ -0,0 +1,38 @@ +using OpenMetaverse; +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; + +namespace OpenSim.Modules.PathFinding +{ + public class ScriptRequestData + { + public UUID HostID = UUID.Zero; + public UUID ScriptID = UUID.Zero; + public String EnvironmentID = null; + public UUID RequestID = UUID.Zero; + + public ScriptRequestData(UUID host, UUID script) + { + HostID = host; + ScriptID = script; + } + + public ScriptRequestData(UUID host, UUID script, String envID) + { + HostID = host; + ScriptID = script; + EnvironmentID = envID; + } + + public ScriptRequestData(UUID host, UUID script, String envID, UUID requestID) + { + HostID = host; + ScriptID = script; + EnvironmentID = envID; + RequestID = requestID; + } + } +}