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