Skip to content

Commit

Permalink
adapt A*algo for #13, use manhatten distance for unittest to make it …
Browse files Browse the repository at this point in the history 
…compareable with SpatialAStar
  • Loading branch information
@mfe-
mfe- committed May 2, 2020
1 parent 2da981b commit 582e181
Show file tree
Hide file tree
Showing 4 changed files with 78 additions and 58 deletions.
59 changes: 41 additions & 18 deletions Algorithms.Graph.Test/Graph.Extensions.Test.cs
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Xml.Linq;
Expand Down Expand Up @@ -435,7 +436,7 @@ public void AStarYt2Example_should_find_shortes_path()
[InlineData(4, 4)]
[InlineData(512, 512)]
[InlineData(1024, 1024)]
public void AStar_should_find_in_grid_graph_from_start_to_last(int i, int j)
public void AStar_using_ManhattanDistance_visit_expected_amount_vertices(int i, int j)
{
DataStructures.Graph g;
Stopwatch stopwatch1 = new Stopwatch();
Expand All @@ -461,49 +462,71 @@ public void AStar_should_find_in_grid_graph_from_start_to_last(int i, int j)

IVertex VertexFactoryDouble(int x, int y)
{
//the starting point doesn't have any costs
double distance = 0;
if (!(x == 0 && y == 0))
{
distance = Math.Sqrt((Math.Pow(i - x, 2) + Math.Pow((double)(j - y), 2)));
}
return new DataStructures.Vertex(distance);
var vertex = new DataStructures.Vertex<Point>();
vertex.Value = new Point(x, y);
return vertex;
}

g = GraphExtensions.GenerateGridGraph(i, j, VertexFactoryDouble, (lastVertex) => goalToFind = lastVertex, actionOnRowCreated);
g = GraphExtensions.GenerateGridGraph(i, j, VertexFactoryDouble, (lastVertex) => goalToFind = lastVertex, actionOnRowCreated, 0.1);
IEnumerable<IVertex> result;
int completeAmountEdges = 2 * (i * j) - i - j;
int completeAmountVertices = i * j;

Point goalPoint = ((IVertex<Point>)goalToFind).Value;
Func<IVertex, double> funcManhattanDistanceHeuristic = new Func<IVertex, double>((vertex) =>
{
Point currentPoint = ((IVertex<Point>)vertex).Value;
return Math.Abs(currentPoint.X - goalPoint.X) + Math.Abs(currentPoint.Y - goalPoint.Y);

});
double SQRT_2 = Math.Sqrt(2);
Func<IVertex, IEdge, double> funcNeighborDistance = new Func<IVertex, IEdge, double>((current, e) =>
{
Point currentPoint = ((IVertex<Point>)current).Value;
Point neighbourPoint = ((IVertex<Point>)e.V).Value;

int diffX = Math.Abs(currentPoint.X - neighbourPoint.X);
int diffY = Math.Abs(currentPoint.Y - neighbourPoint.Y);

switch (diffX + diffY)
{
case 1: return 1;
case 2: return SQRT_2;
case 0: return 0;
default:
throw new ApplicationException();
}
});

IDictionary<Guid, IEdge> path;
stopwatch1.Start();
{
var path = g.Start.AStar(goalToFind);
path = g.Start.AStar(goalToFind, funcManhattanDistanceHeuristic);
result = path.ReconstructPath(goalToFind);
}
stopwatch1.Stop();

//check if we can get from the start to the goal using the result list
Assert.Equal(g.Start, result.Last());
double totalCosts = 0;

IVertex lastVisitedVertex = g.Start;
//revers list so we begin from start
var enumerator = result.Reverse().GetEnumerator();
//skip the first item in the list as we want to know to which vertex we should move from start
enumerator.MoveNext();
while (enumerator.MoveNext())
{
totalCosts = totalCosts + lastVisitedVertex.Weighted;
lastVisitedVertex = lastVisitedVertex.Edges.FirstOrDefault(a => a.V == enumerator.Current).V;
}
totalCosts = totalCosts + lastVisitedVertex.Weighted;
if (i == 4 && j == 4)
{
Assert.Equal(19, Convert.ToInt32(totalCosts));
}
//with the ManhattanDistanceHeuristic we only need to visit
int expectedVisitedVertices = (i+j)-1;
Assert.Equal(expectedVisitedVertices, path.Count);

//did we find the goal?
Assert.Equal(goalToFind, lastVisitedVertex);

_testOutputHelper.WriteLine($"Astar took for grid graph of {i}x{j} with Edges:{completeAmountEdges} Vertices:{completeAmountVertices} {stopwatch1.ElapsedMilliseconds}ms");
_testOutputHelper.WriteLine($"Astar result path contains {result.Count()} with costs of {totalCosts}");
_testOutputHelper.WriteLine($"Astar result path contains {path.Count()} and reconstructed path {result.Count()}");

}
//xml Deserializer creates readOnly list
Expand Down
72 changes: 34 additions & 38 deletions Algorithms.Graph/Graph.Extensions.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -203,7 +203,7 @@ public static IEnumerable<IVertex> BreadthFirstSearchQueue(this IVertex vertex,
/// <param name="amount_height_vertices">height of grid</param>
/// <param name="funFactory">The vertice factory to create vertices</param>
/// <returns>The created graph</returns>
public static DataStructures.Graph GenerateGridGraph(int amountWidthVertices, int amountHeightVertices, Func<int, int, IVertex> funFactory, Action<IVertex> onLastVertexCreatedAction = null, Action<IVertex[]> actionOnRowCreated = null)
public static DataStructures.Graph GenerateGridGraph(int amountWidthVertices, int amountHeightVertices, Func<int, int, IVertex> funFactory, Action<IVertex> onLastVertexCreatedAction = null, Action<IVertex[]> actionOnRowCreated = null, double edgeWeight = 1)
{
DataStructures.Graph g = new DataStructures.Graph();
IVertex[] lastVerticesRow = new IVertex[amountWidthVertices];
Expand All @@ -218,13 +218,13 @@ public static DataStructures.Graph GenerateGridGraph(int amountWidthVertices, in
//connect previous vertex on x axis
if (x - 1 >= 0)
{
currentVertex.AddEdge(lastVerticesRow[x - 1], x, true);
lastVerticesRow[x - 1].AddEdge(currentVertex, x, true);
currentVertex.AddEdge(lastVerticesRow[x - 1], edgeWeight, true);
lastVerticesRow[x - 1].AddEdge(currentVertex, edgeWeight, true);
}
//connect previous vertex on y axis
if (lastyVerticesRow[x] != null)
{
currentVertex.AddEdge(lastyVerticesRow[x], y, false);
currentVertex.AddEdge(lastyVerticesRow[x], edgeWeight, false);
}
if (y == amountHeightVertices - 1 && x == amountWidthVertices - 1)
{
Expand Down Expand Up @@ -269,48 +269,48 @@ public AEdge(IVertex current, IVertex predecessor, double weight, double f)
public double Weighted { get; set; }
public double F { get; set; }
}
public static IEnumerable<IVertex> ReconstructPath(this IDictionary<IVertex, IEdge> edge, IVertex goal)
public static IEnumerable<IVertex> ReconstructPath(this IDictionary<Guid, IEdge> edge, IVertex goal)
{
List<IVertex> vertices = new List<IVertex>();
IEdge current = edge[goal];
IEdge current = edge[goal.Guid];
while (current.U != null)
{
vertices.Add(current.V);
current = edge[current.U];
yield return current.V;
current = edge[current.U.Guid];
}
vertices.Add(current.V);
return vertices;
yield return current.V;
}
public static IDictionary<IVertex, IEdge> AStar(this IVertex start, IVertex goal, Func<IVertex, double> funcHeuristic = null, Func<IEdge, double> funcEdgeWeight = null)
public static IDictionary<Guid, IEdge> AStar(this IVertex start, IVertex goal, Func<IVertex, double> funcHeuristic = null, Func<IVertex, IEdge, double> funcEdgeWeight = null)
{
if (funcHeuristic == null)
{
funcHeuristic = (v) => v.Weighted;
}
if (funcEdgeWeight == null)
{
funcEdgeWeight = (e) => e.Weighted;
funcEdgeWeight = (current, e) => e.Weighted;
}
// The set of discovered nodes that may need to be (re-)expanded.
// Initially, only the start node is known.
// This is usually implemented as a min-heap or priority queue rather than a hash-set.
var openSet = new PriorityQueue<AEdge>(a => a.F);
var openOrderedSet = new PriorityQueue<AEdge>(a => a.F);
// caching list which keeps the key for the vertices of PriorityQueue
Dictionary<string, IComparable> verticeKeyForPriorityQueue = new Dictionary<string, IComparable>();
Dictionary<Guid, IComparable> openSet = new Dictionary<Guid, IComparable>();

openSet.Enqueue(new AEdge(start, null, 0, funcHeuristic(start)));
verticeKeyForPriorityQueue.Add(start.ToString(), funcHeuristic(start));
openOrderedSet.Enqueue(new AEdge(start, null, 0, funcHeuristic(start)));
openSet.Add(start.Guid, funcHeuristic(start));

var closeSet = new Dictionary<IVertex, IEdge>();
while (openSet.Any())
var closeSet = new Dictionary<Guid, IEdge>();
int counter = 0;
while (openOrderedSet.Any())
{
counter++;
//the node in openSet having the lowest fScore[] value
AEdge openVertex = openSet.Dequeue();
verticeKeyForPriorityQueue.Remove(openVertex.V.ToString());
AEdge openVertex = openOrderedSet.Dequeue();
openSet.Remove(openVertex.V.Guid);
IVertex currentNode = openVertex.V;

// Current node goes into the closed set
closeSet.Add(currentNode, openVertex);
closeSet.Add(currentNode.Guid, openVertex);

if (currentNode == goal)
{
Expand All @@ -320,22 +320,22 @@ public static IDictionary<IVertex, IEdge> AStar(this IVertex start, IVertex goal
// expand all neighbours
foreach (IEdge edge in currentNode.Edges)
{
if (closeSet.ContainsKey(edge.V))
if (closeSet.ContainsKey(edge.V.Guid))
continue;

// calculate g-value for new path:
// g-value of predecessor + costs/weight of current edge
double tentative_g = openVertex.Weighted + funcEdgeWeight(edge);
double tentative_g = openVertex.Weighted + funcEdgeWeight(currentNode, edge);
// if successor is alread on list
var sucessorvertex = new AEdge();
var cacheKey = edge.V.ToString();
bool exists = verticeKeyForPriorityQueue.ContainsKey(cacheKey);
IList<AEdge> edgeListNode = null;
var cacheKey = edge.V.Guid;
bool exists = openSet.ContainsKey(cacheKey);
ICollection<AEdge> edgeListNode = null;
double oldKey = -1;
if (exists)
{
edgeListNode = ((PriorityQueue<AEdge>.PriorityNode<AEdge>)
openSet.GetNode(verticeKeyForPriorityQueue[cacheKey])).Datas;
openOrderedSet.GetNode(openSet[cacheKey])).Datas;
sucessorvertex = edgeListNode.FirstOrDefault(aedge => aedge.V == edge.V);
oldKey = sucessorvertex.F;
}
Expand All @@ -348,20 +348,16 @@ public static IDictionary<IVertex, IEdge> AStar(this IVertex start, IVertex goal
edgeListNode.Remove(sucessorvertex);
if (edgeListNode.Count == 0)
{
openSet.Remove(oldKey);
openOrderedSet.Remove(oldKey);
}
verticeKeyForPriorityQueue.Remove(cacheKey);
openSet.Remove(cacheKey);
}
// the path to neighbor is better than any previous one or it wasnt added. Record it!
sucessorvertex = new AEdge();
sucessorvertex.V = edge.V;
sucessorvertex.U = currentNode;
sucessorvertex.Weighted = tentative_g;
// update f - value
sucessorvertex.F = tentative_g + funcHeuristic(edge.V);
sucessorvertex =
new AEdge(edge.V, currentNode, tentative_g, tentative_g + funcHeuristic(edge.V));

openSet.Enqueue(sucessorvertex);
verticeKeyForPriorityQueue.Add(cacheKey, sucessorvertex.F);
openOrderedSet.Enqueue(sucessorvertex);
openSet.Add(cacheKey, sucessorvertex.F);

}
}
Expand Down
4 changes: 2 additions & 2 deletions Algorithms/PriorityQueue{TData}.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,9 +18,9 @@ public class PriorityNode<TData1> : BNodeLeafe<TData1>
{
public PriorityNode(IComparable comparer, TData1 value) : base(comparer, value)
{
Datas = new List<TData1>();
Datas = new HashSet<TData1>();
}
public IList<TData1> Datas { get; }
public ICollection<TData1> Datas { get; }
}
private readonly Func<TData, IComparable> _funcKey;
public PriorityQueue(Func<TData, IComparable> funcKey) : base()
Expand Down
1 change: 1 addition & 0 deletions Readme.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,6 +50,7 @@ With the old version of the framework I mainly focused on visually representing

##### Some algo related links

- A*Star https://www.codeguru.com/csharp/csharp/cs_misc/designtechniques/article.php/c12527/AStar-A-Implementation-in-C-Path-Finding-PathFinder.htm
- The Travelling salesman problem is one of the most well know NP-hard problem. Concorde’s solver can be used to solve exactly or approximately even large instances. <http://www.math.uwaterloo.ca/tsp/index.html>
- PRIM <http://bioinfo.ict.ac.cn/~dbu/AlgorithmCourses/Lectures/Prim1957.pdf>

Expand Down

0 comments on commit 582e181

Please sign in to comment.