From 6a66ea9f6370e8341ce45b0cb67faf64b073d3fb Mon Sep 17 00:00:00 2001
From: C9Glax <gfreak70@gmail.com>
Date: Thu, 5 May 2022 16:23:00 +0200
Subject: [PATCH] intermediate

---
 Graph/Graph.csproj                 | 14 ---------
 OpenStreetMap Importer/Importer.cs | 46 ++++++++++++++++++++++++++----
 {Graph => astar}/Edge.cs           |  4 ++-
 {Graph => astar}/Node.cs           |  0
 4 files changed, 44 insertions(+), 20 deletions(-)
 delete mode 100644 Graph/Graph.csproj
 rename {Graph => astar}/Edge.cs (55%)
 rename {Graph => astar}/Node.cs (100%)

diff --git a/Graph/Graph.csproj b/Graph/Graph.csproj
deleted file mode 100644
index 921dbb1..0000000
--- a/Graph/Graph.csproj
+++ /dev/null
@@ -1,14 +0,0 @@
-<Project Sdk="Microsoft.NET.Sdk">
-
-  <PropertyGroup>
-    <OutputType>Exe</OutputType>
-    <TargetFramework>net6.0</TargetFramework>
-    <ImplicitUsings>enable</ImplicitUsings>
-    <Nullable>enable</Nullable>
-  </PropertyGroup>
-
-  <ItemGroup>
-    <ProjectReference Include="..\Logging\Logging.csproj" />
-  </ItemGroup>
-
-</Project>
diff --git a/OpenStreetMap Importer/Importer.cs b/OpenStreetMap Importer/Importer.cs
index 4f418a1..57393d1 100644
--- a/OpenStreetMap Importer/Importer.cs	
+++ b/OpenStreetMap Importer/Importer.cs	
@@ -35,7 +35,7 @@ namespace OpenStreetMap_Importer
                     if(currentNodeType == nodeType.WAY)
                     {
                         ImportWay(currentWay);
-                        logger.log(loglevel.INFO, "Way nodes: {0}", currentWay.nodes.Count);
+                        logger.log(loglevel.INFO, "Way nodes: {0} Weight: {1}", currentWay.nodes.Count);
                     }
                     currentNodeType = nodeType.WAY;
                     currentNode = nullNode;
@@ -95,18 +95,24 @@ namespace OpenStreetMap_Importer
             {
                 for(int index = 0; index < way.nodes.Count - 1; index++)
                 {
-                    way.nodes[index].edges.Add(new Edge(way.nodes[index + 1]));
+                    Node currentNode = way.nodes[index];
+                    Node neighborNode = way.nodes[index + 1];
+                    ushort weight = Convert.ToUInt16(DistanceBetweenNodes(currentNode, neighborNode));
+                    currentNode.edges.Add(new Edge(neighborNode, weight));
                     if (!way.oneway)
-                        way.nodes[index+1].edges.Add(new Edge(way.nodes[index]));
+                        neighborNode.edges.Add(new Edge(currentNode, weight));
                 }
             }
             else
             {
                 for (int index = way.nodes.Count-1; index > 1; index--)
                 {
-                    way.nodes[index].edges.Add(new Edge(way.nodes[index - 1]));
+                    Node currentNode = way.nodes[index];
+                    Node neighborNode = way.nodes[index - 1];
+                    ushort weight = Convert.ToUInt16(DistanceBetweenNodes(currentNode, neighborNode));
+                    currentNode.edges.Add(new Edge(neighborNode, weight));
                     if (!way.oneway)
-                        way.nodes[index - 1].edges.Add(new Edge(way.nodes[index]));
+                        neighborNode.edges.Add(new Edge(currentNode, weight));
                 }
             }
         }
@@ -127,5 +133,35 @@ namespace OpenStreetMap_Importer
                 this.direction = wayDirection.forward;
             }
         }
+
+        private static double DistanceBetweenNodes(Node n1, Node n2)
+        {
+            return DistanceBetweenCoordinates(n1.lat, n1.lon, n2.lat, n2.lon);
+        }
+
+        private 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;
+        }
     }
 }
diff --git a/Graph/Edge.cs b/astar/Edge.cs
similarity index 55%
rename from Graph/Edge.cs
rename to astar/Edge.cs
index dece1e0..6a84822 100644
--- a/Graph/Edge.cs
+++ b/astar/Edge.cs
@@ -3,9 +3,11 @@
     public struct Edge
     {
         public Node neighbor { get; }
-        public Edge(Node neighbor)
+        public ushort weight { get; }
+        public Edge(Node neighbor, ushort weight)
         {
             this.neighbor = neighbor;
+            this.weight = weight;
         }
     }
 }
diff --git a/Graph/Node.cs b/astar/Node.cs
similarity index 100%
rename from Graph/Node.cs
rename to astar/Node.cs