private ShortestPathNode? findShortestPath(Vector start, IReadOnlyList<IGoal> wayPoints, double stepSize)
{
var open = new BinaryHeapOpenSet<GridKey, GridSearchNode>();
var closed = new ClosedSet<GridKey>();
open.Add(
new GridSearchNode(
key: new GridKey(start, wayPoints.Count),
start,
previous: null,
distanceFromStart: 0.0,
estimatedCost: 0.0,
wayPoints,
targetWayPoint: 0));
while (!open.IsEmpty)
{
var nodeToExpand = open.DequeueMostPromissing();
if (nodeToExpand.RemainingWayPoints.Count == 0)
{
var head = new ShortestPathNode(
wayPoints.Last().Position, // use the goal position directly
targetWayPoint: wayPoints.Count - 1); // the goal way point should be kept here
var backtrackingNode = nodeToExpand.Previous;
while (backtrackingNode != null)
{
var node = new ShortestPathNode(backtrackingNode.Position, backtrackingNode.TargetWayPoint);
node.CostToNext = TimeSpan.FromSeconds(Distance.Between(node.Position, head.Position) / maxSpeed);
node.Next = head;
head = node;
backtrackingNode = backtrackingNode.Previous;
}
return head;
}
closed.Add(nodeToExpand.Key);
for (var dx = -1; dx <= 1; dx++)
{
for (var dy = -1; dy <= 1; dy++)
{
if (dx == 0 && dy == 0) continue;
var nextPoint = new Vector(
nodeToExpand.Position.X + dx * stepSize,
nodeToExpand.Position.Y + dy * stepSize);
var reachedWayPoint = nodeToExpand.RemainingWayPoints[0].ReachedGoal(nextPoint);
var remainingWayPoints = reachedWayPoint
? nodeToExpand.RemainingWayPoints.Skip(1).ToList().AsReadOnly()
: nodeToExpand.RemainingWayPoints;
var targetWayPoint = wayPoints.Count - nodeToExpand.RemainingWayPoints.Count; // I want to keep the ID of the waypoint in the node which reaches the waypoint and only increase it for its childer
var key = new GridKey(nextPoint, remainingWayPoints.Count);
if (closed.Contains(key))
{
continue;
}
if (collisionDetector.IsCollision(nextPoint))
{
closed.Add(key);
continue;
}
var distance = nodeToExpand.DistanceFromStart + (nextPoint - nodeToExpand.Position).CalculateLength();
var node = new GridSearchNode(key, nextPoint, nodeToExpand, distance, distance + 0, remainingWayPoints, targetWayPoint);
if (open.Contains(node.Key))
{
if (node.DistanceFromStart < open.Get(node.Key).DistanceFromStart)
{
open.ReplaceExistingWithTheSameKey(node);
}
}
else
{
open.Add(node);
}
}
}
}
return null;
}
public void hValue_heuristicFor5x5B_Equals20()
{
GridSearchNode initialState = _basicWorld5b.GetInitialSearchNode <GridSearchNode>();
Assert.AreEqual(20, initialState.h);
}
public int CompareTo(GridSearchNode other)
{
var diff = EstimatedTotalCost - other.EstimatedTotalCost;
return diff < 0 ? -1 : (diff == 0 ? 0 : 1);
}
public void hValue_heuristicFor3x3_Equals8()
{
GridSearchNode initialState = _basicWorld3.GetInitialSearchNode <GridSearchNode>();
Assert.AreEqual(8, initialState.h);
}
public void hValue_heuristicFor4x4B_Equals15()
{
GridSearchNode initialState = _basicWorld4b.GetInitialSearchNode <GridSearchNode>();
Assert.AreEqual(15, initialState.h);
}
public void hValue_heuristicFor5x5E_Equals18()
{
GridSearchNode initialState = _basicWorld5e.GetInitialSearchNode <GridSearchNode>();
Assert.AreEqual(18, initialState.h);
}
public void hValue_heuristicFor2x2_Equals2()
{
GridSearchNode initialState = _basicWorld2.GetInitialSearchNode <GridSearchNode>();
Assert.AreEqual(2, initialState.h);
}
public void hValue_basicWorldSeparateBccV1_Equals20()
{
GridSearchNode initialState = _basicWorldSeparateBccV1.GetInitialSearchNode <GridSearchNode>();
Assert.AreEqual(14, initialState.h);
}
public void hValue_basicWorldSeparateBccV2_Equals31()
{
GridSearchNode initialState = _basicWorldSeparateBccV2.GetInitialSearchNode <GridSearchNode>();
Assert.AreEqual(31, initialState.h);
}
public void gValue_getFvalueForChildNode_Equals20()
{
GridSearchNode initialState = _basicWorld3.GetInitialSearchNode <GridSearchNode>();
Assert.AreEqual(20, initialState.h);
}
public int Calc_H(World w, GridSearchNode gridNode)
{
return(w.Width * w.Height);
}
public void fValue_getFvalueForNewNode_EqualsZero()
{
GridSearchNode parent = _basicWorld.GetInitialSearchNode <GridSearchNode>();
Assert.AreEqual(0, parent.g);
}
public void Constructor_CreatesState_Created()
{
GridSearchNode gn = new GridSearchNode(_basicWorld);
Assert.IsNotNull(gn);
}
public void hValue_heuristicFor5x5G_Equals33()
{
GridSearchNode initialState = _basicWorldSeparateBccV2.GetInitialSearchNode <GridSearchNode>();
Assert.AreEqual(33, initialState.h);
}
