Implementation of A*
This commit is contained in:
parent
0760803685
commit
f6d5fe5905
4
Graph/Graph.csproj.user
Normal file
4
Graph/Graph.csproj.user
Normal file
@ -0,0 +1,4 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<Project ToolsVersion="Current" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
|
||||
<PropertyGroup />
|
||||
</Project>
|
@ -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.edges = new List<Edge>();
|
||||
this.previousNode = nullnode;
|
||||
this.goalDistance = uint.MaxValue;
|
||||
}
|
||||
public static Node nullnode = new Node(float.NaN, float.NaN);
|
||||
}
|
||||
}
|
||||
|
35
Graph/Utils.cs
Normal file
35
Graph/Utils.cs
Normal file
@ -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;
|
||||
}
|
||||
}
|
||||
}
|
@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
Loading…
Reference in New Issue
Block a user