Implementation of A*

Graph/Graph.csproj.user

@@ -0,0 +1,4 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Project ToolsVersion="Current" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
+  <PropertyGroup />
+</Project>

Graph/Node.cs

@@ -1,16 +1,22 @@
 namespace Graph
 {
-    public struct Node
+    public class Node
     {
         public float lat { get; }
         public float lon { get; }
         public List<Edge> edges { get; }
 
+        public Node previousNode { get; set; }
+        public double goalDistance { get; set; }
+
         public Node(float lat, float lon)
         {
             this.lat = lat;
             this.lon = lon;
             this.edges = new List<Edge>();
-        }
+            this.previousNode = nullnode;
+            this.goalDistance = uint.MaxValue;
+        }
+        public static Node nullnode = new Node(float.NaN, float.NaN);
     }
 }

Graph/Utils.cs

@@ -0,0 +1,35 @@
+namespace Graph{
+
+    public struct Utils
+    {
+        public static double DistanceBetweenNodes(Node n1, Node n2)
+        {
+            return DistanceBetweenCoordinates(n1.lat, n1.lon, n2.lat, n2.lon);
+        }
+
+        public static double DistanceBetweenCoordinates(float lat1, float lon1, float lat2, float lon2)
+        {
+            const int earthRadius = 6371;
+            double differenceLat = DegreesToRadians(lat2 - lat1);
+            double differenceLon = DegreesToRadians(lon2 - lon1);
+
+            double lat1Rads = DegreesToRadians(lat1);
+            double lat2Rads = DegreesToRadians(lat2);
+
+            double a = Math.Sin(differenceLat / 2) * Math.Sin(differenceLat / 2) + Math.Sin(differenceLon / 2) * Math.Sin(differenceLon / 2) * Math.Cos(lat1Rads) * Math.Cos(lat2Rads);
+            double c = 2 * Math.Atan2(Math.Sqrt(a), Math.Sqrt(1 - a));
+
+            return earthRadius * c;
+        }
+
+        private static double DegreesToRadians(double deg)
+        {
+            return deg * Math.PI / 180.0;
+        }
+
+        private static double RadiansToDegrees(double rad)
+        {
+            return rad * 180.0 / Math.PI;
+        }
+    }
+} 

astar/Astar.cs

@@ -10,8 +10,83 @@ namespace astar
         public Astar()
         {
             this.logger = new Logger(LogType.Console, loglevel.DEBUG);
-            Dictionary<UInt64, Node> nodes = Importer.Import(logger);
+            Dictionary<UInt64, Node> nodes = Importer.Import(ref logger);
+            Random r = new Random();
+            List<Node> path = new List<Node>();
+            while(path.Count < 1)
+            {
+                Node n1 = nodes[nodes.Keys.ElementAt(r.Next(0, nodes.Count - 1))];
+                Node n2 = nodes[nodes.Keys.ElementAt(r.Next(0, nodes.Count - 1))];
+                logger.Log(loglevel.INFO, "From {0} - {1} to {2} - {3}", n1.lat, n1.lon, n2.lat, n2.lon);
+                path = FindPath(n1, n2, ref this.logger);
+            }
 
+            logger.Log(loglevel.INFO, "Path found");
+            foreach (Node n in path)
+                logger.Log(loglevel.INFO, "{0} {1} Distance left {2}", n.lat, n.lon, n.goalDistance);
+        }
+
+        private static void Reset(ref Dictionary<ulong, Node> nodes)
+        {
+            foreach(Node n in nodes.Values)
+            {
+                n.previousNode = Node.nullnode;
+                n.goalDistance = double.MaxValue;
+            }
+        }
+
+        private static List<Node> FindPath(Node start, Node goal, ref Logger logger)
+        {
+            List<Node> toVisit = new List<Node>();
+            toVisit.Add(start);
+            Node currentNode = start;
+            while(currentNode != goal && toVisit.Count > 0)
+            {
+                logger.Log(loglevel.VERBOSE, "toVisit-length: {0} distance: {1}", toVisit.Count, currentNode.goalDistance);
+                foreach (Edge e in currentNode.edges)
+                {
+                    if(e.neighbor.previousNode == Node.nullnode)
+                    {
+                        toVisit.Add(e.neighbor);
+                        e.neighbor.goalDistance = Utils.DistanceBetweenNodes(e.neighbor, goal);
+                        e.neighbor.previousNode = currentNode;
+                    }
+                }
+                toVisit.Remove(currentNode);
+                toVisit.Sort(CompareDistanceToGoal);
+                currentNode = toVisit.First();
+            }
+
+            List<Node> path = new List<Node>();
+
+            if (currentNode != goal)
+            {
+                logger.Log(loglevel.INFO, "No path between {0} - {1} and {2} - {3}", start.lat, start.lon, goal.lat, goal.lon);
+                return path;
+            }
+
+            path.Add(goal);
+            while(currentNode != start)
+            {
+                path.Add(currentNode.previousNode);
+                currentNode = currentNode.previousNode;
+            }
+            path.Reverse();
+            return path;
+        }
+        
+        private static int CompareDistanceToGoal(Node n1, Node n2)
+        {
+            if (n1 == null || n2 == null)
+                return 0;
+            else
+            {
+                if (n1.goalDistance < n2.goalDistance)
+                    return 1;
+                else if (n1.goalDistance > n2.goalDistance)
+                    return -1;
+                else return 0;
+            }
         }
     }
 }