public void VerifyBidirectional()
{
Func <PointInt, IEnumerable <PointInt> > getNeighbors = p => new[]
{
new PointInt(p.X - 1, p.Y + 0), // L
new PointInt(p.X + 1, p.Y + 0), // R
new PointInt(p.X + 0, p.Y - 1), // B
new PointInt(p.X + 0, p.Y + 1), // T
};
Func <PointInt, PointInt, double> getScoreBetween = (p1, p2) =>
{
// Manhatten Distance
return(Math.Abs(p1.X - p2.X) + Math.Abs(p1.Y - p2.Y));
};
var rand = new Random(42);
var start = new PointInt(0, 0);
var destination = new PointInt(400, 400);
Func <PointInt, bool, double> getHeuristicScore = (p, backward) =>
{
var dx = backward ? (p.X - start.X) : (p.X - destination.X);
var dy = backward ? (p.Y - start.Y) : (p.Y - destination.Y);
return(Math.Sqrt(dx * dx + dy * dy));
};
_output.WriteLine("Going from {0} to {1}", start, destination);
var sw = Stopwatch.StartNew();
double distance, distanceControl; bool success;
var results = AStarUtilities.BidirectionalFindMinimalPath(start, destination, getNeighbors, getScoreBetween, getHeuristicScore, out distance, out success);
_output.WriteLine("Done in {0}s.", sw.Elapsed.TotalSeconds);
_output.WriteLine("Expansions: {0}", AStarUtilities.LastExpansionCount);
_output.WriteLine("Result Count: {0}", results.Count);
_output.WriteLine("Distance: {0}", distance);
Assert.True(success);
Assert.Equal(start, results.First());
Assert.Equal(destination, results.Last());
sw.Restart();
var resultsControl = AStarUtilities.FindMinimalPath(start, destination, getNeighbors, getScoreBetween, p => getHeuristicScore(p, false), out distanceControl, out success);
_output.WriteLine("Control Done in {0}s.", sw.Elapsed.TotalSeconds);
_output.WriteLine("Expansions: {0}", AStarUtilities.LastExpansionCount);
_output.WriteLine("Result Count: {0}", resultsControl.Count);
_output.WriteLine("Distance: {0}", distanceControl);
Assert.True(success);
Assert.Equal(start, resultsControl.First());
Assert.Equal(destination, resultsControl.Last());
Assert.Equal(distanceControl, distance);
}
