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); }