add 3 party A* libery

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<T>
+        where T : IComparable<T>
+    {
+        private readonly List<T> Items;
+
+        public MinHeap()
+        {
+            this.Items = new List<T>();
+        }
+
+        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;
+    }
+}

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}";
+    }
+}

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; }
+    }
+}

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<IEdge> Incoming { get; }
+        IList<IEdge> Outgoing { get; }
+    }
+}

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<IEdge>(0);
+            this.Outgoing = new List<IEdge>(0);
+
+            this.Position = position;
+        }
+
+        public IList<IEdge> Incoming { get; }
+        public IList<IEdge> 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();
+    }
+}

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<INode> GetAllNodes()
+        {
+            var list = new List<INode>(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;
+    }
+}

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<IEdge> 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<IEdge> Edges { get; }
+        public Distance Distance { get; }
+    }
+}

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<PathFinderNode> Interesting;
+        private readonly Dictionary<INode, PathFinderNode> Nodes;
+        private readonly PathReconstructor PathReconstructor;
+
+        private PathFinderNode NodeClosestToGoal;
+
+        public PathFinder()
+        {
+            this.Interesting = new MinHeap<PathFinderNode>();
+            this.Nodes = new Dictionary<INode, PathFinderNode>();
+            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;
+    }
+}

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<PathFinderNode>
+    {
+        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}";
+    }
+}

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<INode, IEdge> CameFrom;
+
+        public PathReconstructor()
+        {
+            this.CameFrom = new Dictionary<INode, IEdge>();
+        }
+
+        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<IEdge>();
+
+            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();
+    }
+}

src/A/Paths/PathType.cs

@@ -0,0 +1,8 @@
+namespace Roy_T.AStar.Paths
+{
+    public enum PathType
+    {
+        Complete,
+        ClosestApproach
+    }
+}

src/A/Primitives/Distance.cs

@@ -0,0 +1,73 @@
+using System;
+
+namespace Roy_T.AStar.Primitives
+{
+    public struct Distance : IComparable<Distance>, IEquatable<Distance>
+    {
+        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();
+    }
+}

src/A/Primitives/Duration.cs

@@ -0,0 +1,52 @@
+using System;
+
+namespace Roy_T.AStar.Primitives
+{
+    public struct Duration : IComparable<Duration>, IEquatable<Duration>
+    {
+        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();
+    }
+}

src/A/Primitives/GridPosition.cs

@@ -0,0 +1,32 @@
+using System;
+
+namespace Roy_T.AStar.Primitives
+{
+    public struct GridPosition : IEquatable<GridPosition>
+    {
+        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;
+    }
+}

src/A/Primitives/GridSize.cs

@@ -0,0 +1,30 @@
+using System;
+
+namespace Roy_T.AStar.Primitives
+{
+    public struct GridSize : IEquatable<GridSize>
+    {
+        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();
+    }
+}

src/A/Primitives/Position.cs

@@ -0,0 +1,35 @@
+using System;
+
+namespace Roy_T.AStar.Primitives
+{
+    public struct Position : IEquatable<Position>
+    {
+        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();
+    }
+}

src/A/Primitives/Size.cs

@@ -0,0 +1,30 @@
+using System;
+
+namespace Roy_T.AStar.Primitives
+{
+    public struct Size : IEquatable<Size>
+    {
+        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();
+    }
+}

src/A/Primitives/Velocity.cs

@@ -0,0 +1,57 @@
+using System;
+
+namespace Roy_T.AStar.Primitives
+{
+    public struct Velocity : IComparable<Velocity>, IEquatable<Velocity>
+    {
+        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();
+    }
+}

src/BasicPathFinding.cs

@@ -7,6 +7,9 @@ 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.Grids;
+using Roy_T.AStar.Paths;
+using Roy_T.AStar.Primitives;
 using System;
 using System.Collections.Generic;
 using System.Drawing;
@@ -122,6 +125,8 @@ namespace OpenSim.Modules.PathFinding
                     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_CONNECTED", 19855);
                     m_scriptModule.RegisterConstant("PATH_ENV_ERR_UNKNOWN", 19860);
                 }
                 catch (Exception e)
@@ -212,7 +217,32 @@ namespace OpenSim.Modules.PathFinding
             }
         }
 
-        private void setPositionData(ScriptRequestData requestData, Vector3 position, int walkable, int isTarget)
+        private void removeAllStarts(ScriptRequestData requestData)
+        {
+            lock (m_environments)
+            {
+                try
+                {
+                    Environment _env = m_environments.Find(X => X.ID == requestData.EnvironmentID);
+
+                    if (_env != null)
+                    {
+                        List<PathNode> _nodes = _env.Nodes.FindAll(X => X.Start == true);
+
+                        foreach (PathNode thisNode in _nodes)
+                        {
+                            thisNode.Start = false;
+                        }
+                    }
+                }
+                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)
             {
@@ -225,11 +255,14 @@ namespace OpenSim.Modules.PathFinding
                         if (isTarget == 1)
                             removeAllTargets(requestData);
 
+                        if (isStart == 1)
+                            removeAllStarts(requestData);
+
                         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, false);
+                            _node = new PathNode((int)position.X, (int)position.Y, false, false, false);
                             _env.Nodes.Add(_node);
                         }
 
@@ -245,6 +278,12 @@ namespace OpenSim.Modules.PathFinding
                         if (isTarget == 0)
                             _node.Target = false;
 
+                        if (isStart == 1)
+                            _node.Start = true;
+
+                        if (isStart == 0)
+                            _node.Start = false;
+
                         return;
                     }
 
@@ -294,7 +333,7 @@ namespace OpenSim.Modules.PathFinding
 
                                 while ((int)_PointA.X <= (int)_PointB.X)
                                 {
-                                    setPositionData(requestData, _PointA, walkable, 0);
+                                    setPositionData(requestData, _PointA, walkable, 0, 0);
                                     _PointA.X = (int)_PointA.X + 1;
                                 }
                             }
@@ -315,7 +354,7 @@ namespace OpenSim.Modules.PathFinding
 
                                 while ((int)_PointA.Y <= (int)_PointB.Y)
                                 {
-                                    setPositionData(requestData, _PointA, walkable, 0);
+                                    setPositionData(requestData, _PointA, walkable, 0, 0);
                                     _PointA.Y = (int)_PointA.Y + 1;
                                 }
                             }
@@ -340,9 +379,76 @@ namespace OpenSim.Modules.PathFinding
             }
         }
 
+        private PathNode getStartNode(ScriptRequestData requestData)
+        {
+            lock (m_environments)
+            {
+                Environment _env = m_environments.Find(X => X.ID == requestData.EnvironmentID);
+
+                if (_env != null)
+                {
+                    PathNode _startNode = _env.Nodes.Find(X => X.Start == true);
+                    return _startNode;
+                }
+            }
+
+            return null;
+        }
+
+        private PathNode getTargetNode(ScriptRequestData requestData)
+        {
+            lock (m_environments)
+            {
+                Environment _env = m_environments.Find(X => X.ID == requestData.EnvironmentID);
+
+                if (_env != null)
+                {
+                    PathNode _startNode = _env.Nodes.Find(X => X.Target == true);
+                    return _startNode;
+                }
+            }
+
+            return null;
+        }
+
         private void generatePath(ScriptRequestData requestData)
         {
+            lock (m_environments)
+            {
+                Environment _env = m_environments.Find(X => X.ID == requestData.EnvironmentID);
 
+                PathNode _startNode = getStartNode(requestData);
+                PathNode _targetNode = getTargetNode(requestData);
+
+                if(_startNode != null && _targetNode != null)
+                {
+                    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);
+                    PathFinder pathFinder = new PathFinder();
+                    Path _pathFindingPath = pathFinder.FindPath(new GridPosition(_startNode.PositionX, _startNode.PositionY), new GridPosition(_targetNode.PositionX, _targetNode.PositionY), _pathFindingGrid);
+
+                    if(_pathFindingPath.Type == PathType.Complete)
+                    {
+                        String _pathString = "";
+
+                        foreach(var _thisEdge in _pathFindingPath.Edges)
+                            _pathString += "<" + _thisEdge.End.Position.X + ", " + _thisEdge.End.Position.Y + ", 0>;";
+
+                        m_scriptModule.DispatchReply(requestData.ScriptID, 19850, _pathString, requestData.RequestID.ToString());
+                    }
+                    else
+                    {
+                        m_scriptModule.DispatchReply(requestData.ScriptID, 19855, "", requestData.RequestID.ToString());
+                    }
+                }
+                else
+                {
+                    m_scriptModule.DispatchReply(requestData.ScriptID, 19854, "", requestData.RequestID.ToString());
+                }
+            }
         }
 
         private void generateDebugImage(ScriptRequestData requestData)
@@ -410,10 +516,10 @@ namespace OpenSim.Modules.PathFinding
         }
 
         [ScriptInvocation]
-        public void osSetPathPositionData(UUID hostID, UUID scriptID, String environmentID, Vector3 position, int walkable, int isTarget)
+        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); })).Start();
+            (new Thread(delegate () { setPositionData(new ScriptRequestData(hostID, scriptID, environmentID), position, walkable, isTarget, isStart); })).Start();
         }
 
         [ScriptInvocation]

src/PathNode.cs

@@ -13,8 +13,9 @@ namespace OpenSim.Modules.PathFinding
 
         public bool Walkable = false;
         public bool Target = false;
+        public bool Start = false;
 
-        public int Coast = 0;
+        public int f_cost = 99999;
 
         public PathNode Parent = null;
 
@@ -32,13 +33,14 @@ namespace OpenSim.Modules.PathFinding
             Walkable = isWalkable;
         }
 
-        public PathNode(int positionX, int positionY, bool isWalkable, bool isTarget)
+        public PathNode(int positionX, int positionY, bool isWalkable, bool isTarget, bool isStart)
         {
             PositionX = positionX;
             PositionY = positionY;
 
             Walkable = isWalkable;
             Target = isTarget;
+            Start = isStart;
         }
     }
 }