public void SolveMaze(bool visualizeSolver)
{
while (OpenSet.Count != 0)
{
int winnerIndex = CalculateWinnerIndex();
Node current = OpenSet[winnerIndex];
OpenSet.Remove(current);
ClosedSet.Add(current);
current.State = NodeState.Closed;
var neighbors = current.Neighbors;
CalculateNeighbors(current, neighbors);
// Calculates the current path
CalculatePath(current);
// We reached the end if this evaluates to true
if (current == _finishNode)
{
MazeSolved = true;
ReportProgress();
return;
}
ReportProgress();
if (visualizeSolver)
{
Thread.Sleep(10);
}
}
}
//This function is the main difference between A* and Theta* algorithms
private void UpdateNode(Node Current, Node Neighbour)
{
//If there is LoS, ignore current and use the path from its parent to the neighbour node
if (GridManager.Instance.LineOfSight(Current.Parent, Neighbour))
{
//Make sure this pathway is cheaper before updating it
if (Current.Parent.GScore + GridManager.FindHeuristic(Current.Parent, Neighbour) < Neighbour.GScore)
{
Neighbour.GScore = Current.Parent.GScore + GridManager.FindHeuristic(Current.Parent, Neighbour);
Neighbour.Parent = Current;
Neighbour.ToggleParentIndicator(true);
Neighbour.PointIndicator(Neighbour.GetDirection(Current));
if (OpenSet.Contains(Neighbour))
{
OpenSet.Remove(Neighbour);
}
OpenSet.Add(Neighbour);
}
}
else
{
if (Current.GScore + GridManager.FindHeuristic(Current, Neighbour) < Neighbour.GScore)
{
Neighbour.GScore = Current.GScore + GridManager.FindHeuristic(Current, Neighbour);
Neighbour.Parent = Current;
if (OpenSet.Contains(Neighbour))
{
OpenSet.Remove(Neighbour);
}
}
}
}
private void PerformStep()
{
if (openSet.Count == 0)
{
SetStateToFinished();
return;
}
currentNode = openSet.Pop();
if (IsPathFound(currentNode))
{
HandleFoundPath(currentNode);
SetStateToFinished();
return;
}
List <Node> successors = GetSuccessors(currentNode, DiagonalsEnabled);
foreach (Node successor in successors)
{
successor.MovementCost = currentNode.MovementCost + MovementCost(successor, currentNode);
successor.Parent = currentNode;
openSet.Add(successor);
}
openSet.Remove(currentNode);
closedSet.Add(currentNode);
}
public bool IsFinished()
{
if (Queue.TryDequeue(out var item))
{
var path = item.Value;
var coords = path.PathStep.Coords;
OpenSet.Remove(coords);
if (!ClosedSet.Contains(coords))
{
PathfinderExtensions.TraceFindPathDequeue(GetType().Name, coords, path, item.Key, 0);
if (item.Key < BestSoFar)
{
Partner.Heuristic(coords).IfHasValueDo(heuristic => {
if (path.TotalCost + Partner.FrontierMinimum() - heuristic < BestSoFar)
{
Hexside.ForEach(hexside => ExpandHex(path, hexside));
}
});
}
ClosedSet.Add(coords);
}
return(!Queue.Any());
}
return(true);
}
private void ExpandHex(IDirectedPath path, Hexside hexside)
{
var here = path.PathStep.Coords;
var there = here.GetNeighbour(hexside);
if (!ClosedSet.Contains(there))
{
TryStepCost(here, hexside).IfHasValueDo(cost => {
if (path.TotalCost + cost < BestSoFar || !OpenSet.ContainsKey(there))
{
Heuristic(there).IfHasValueDo(heuristic => {
var key = path.TotalCost + cost + heuristic;
var newPath = path.AddStep(there, HexsideDirection(hexside), cost);
PathfinderExtensions.TraceFindPathEnqueue(there, key, 0);
if (!OpenSet.TryGetValue(there, out var oldPath))
{
OpenSet.Add(there, newPath);
Queue.Enqueue(key, newPath);
}
else if (newPath.TotalCost < oldPath.TotalCost)
{
OpenSet.Remove(there);
OpenSet.Add(there, newPath);
Queue.Enqueue(key, newPath);
}
SetBestSoFar(newPath, PartnerPath(there));
});
}
});
}
}
internal bool IsFinished()
{
HexKeyValuePair <int, IDirectedPath> item;
if (Queue.TryDequeue(out item))
{
var path = item.Value;
var coords = path.PathStep.Hex.Coords;
OpenSet.Remove(coords);
if (!ClosedSet.Contains(coords))
{
TraceFindPathDequeue(GetType().Name, coords, path.TotalCost, path.HexsideExit, item.Key, 0);
if (item.Key < BestSoFar &&
path.TotalCost + Partner.FrontierMinimum() - Partner.Heuristic(coords) < BestSoFar
)
{
HexsideExtensions.HexsideList.ForEach(hexside => ExpandHex(path, hexside));
}
ClosedSet.Add(coords);
}
return(!Queue.Any());
}
return(true);
}
private void ExpandHex(IDirectedPath path, Hexside hexside)
{
var here = path.PathStep.Hex;
var there = Board[here.Coords.GetNeighbour(hexside)];
if (there != null && !ClosedSet.Contains(there.Coords))
{
var cost = StepCost(here, hexside, there);
if ((cost > 0) &&
(path.TotalCost + cost < BestSoFar || !OpenSet.ContainsKey(there.Coords))
)
{
var key = path.TotalCost + cost + Heuristic(there.Coords);
var newPath = path.AddStep(there, HexsideDirection(hexside), cost);
TraceFindPathEnqueue(there.Coords, key, 0);
IDirectedPath oldPath;
if (!OpenSet.TryGetValue(there.Coords, out oldPath))
{
OpenSet.Add(there.Coords, newPath);
Queue.Enqueue(key, newPath);
}
else if (newPath.TotalCost < oldPath.TotalCost)
{
OpenSet.Remove(there.Coords);
OpenSet.Add(there.Coords, newPath);
Queue.Enqueue(key, newPath);
}
SetBestSoFar(newPath, GetPartnerPath(there.Coords));
}
}
}
public override void IterateOpenSet()
{
OpenSetIterations++;
//Path doesnt exist if the openset runs out
if (OpenSet.Count <= 0)
{
Log.Print("no path found");
GridManager.Instance.HideAllParentIndicators();
FindingPathway = false;
return;
}
//Take the front node from the open set
Node Current = OpenSet[0];
OpenSet.Remove(Current);
Current.Closed = true;
//Pathway is completed once the end node has been reached
if (Current == PathEnd)
{
Log.Print("Pathway found after " + OpenSetIterations + " iterations.");
FindingPathway = false;
GridManager.Instance.HideAllParentIndicators();
foreach (Node Step in GridManager.Instance.GetCompletedPathway(PathStart, PathEnd))
{
Step.SetType(NodeType.Pathway);
}
return;
}
//Process all the nodes neighbours
foreach (Node Neighbour in GridManager.Instance.GetTraversableNeighbours(Current))
{
//Skip neighbours already searched
if (Neighbour.Closed || Neighbour.Opened)
{
continue;
}
//Move this neighbour onto the open list, open it and update its parent
OpenSet.Add(Neighbour);
Neighbour.Opened = true;
Neighbour.Parent = Current;
Neighbour.ToggleParentIndicator(true);
Neighbour.PointIndicator(Neighbour.GetDirection(Current));
}
}
public override void IterateOpenSet()
{
//Track how many iterations have taken place to find this pathway so far
OpenSetIterations++;
//If the open set is empty, then no pathway was able to be found
if (OpenSet.Count <= 0)
{
//Print a failure message and reset the grid
Log.Print("Unable to find a valid pathway using A* algorithm.");
FindingPathway = false;
GridManager.Instance.HideAllParentIndicators();
return;
}
//Get the cheapest node currently being stored in the open set
Node Current = GridManager.Instance.FindCheapestNode(OpenSet);
OpenSet.Remove(Current);
//When Current matches the end node, the pathway is ready to be reconstructed
if (Current == PathEnd)
{
//Announce the pathway has been found and how long it took to find
Log.Print("A* pathfinding completed after " + OpenSetIterations + " iterations.");
//Hide all parent indicators
GridManager.Instance.HideAllParentIndicators();
//Grab and display the final pathway
List <Node> Pathway = GridManager.Instance.GetCompletedPathway(PathStart, PathEnd);
foreach (Node PathStep in Pathway)
{
PathStep.SetType(NodeType.Pathway);
}
//Finalize the process
FindingPathway = false;
return;
}
//Remove the current node from the open set, then iterate over its neighbours
OpenSet.Remove(Current);
foreach (Node Neighbour in GridManager.Instance.GetTraversableNeighbours(Current))
{
if (Current.GScore < Neighbour.GScore)
{
//Update this as the preferred way to travel
Neighbour.Parent = Current;
Neighbour.ToggleParentIndicator(true);
Neighbour.PointIndicator(Neighbour.GetDirection(Current));
//Update neighbours score values
Neighbour.GScore = Current.GScore;
Neighbour.FScore = Neighbour.GScore + GridManager.FindHeuristic(Neighbour, PathEnd);
//Add neighbour to open set if its not there already
if (!OpenSet.Contains(Neighbour))
{
OpenSet.Add(Neighbour);
}
}
}
}
/// <summary>
/// 路径规划
/// </summary>
/// <param name="snId">起始点编号</param>
/// <param name="gnId">目标点编号</param>
/// <returns>规划所得路径的节点列表</returns>
public StoreHeap PathPanning(int snId, int gnId)
{
if (snId != gnId)
{
Node startNode = NodeList.FindNode(snId); //起始点
Node goalNode = NodeList.FindNode(gnId); //目标点
// 设置起始点和目标点的累积代价为0
startNode.CurrentCost = goalNode.CurrentCost = 0;
foreach (Node node1 in NodeList)
{
node1.NGoal = goalNode;
}
// 添加起始点到开放集合
OpenSet.Add(startNode);
DataTable dt = Graph.DS.Tables["NodeInfo"];
while (true)
{
// 当前点为开放集合中启发代价最小的点
Node currentNode = OpenSet.GetMinCostNode();
// 如果当前点为目标点，则循环结束
if (currentNode.Index == goalNode.Index)
{
// 设置目标点的父节点为关闭集合中的最后一个点
goalNode.NParent = ClosedSet.Pop();
break;
}
// 将当前点从开放集合中删去并添加到关闭集合
OpenSet.Remove(currentNode);
ClosedSet.Add(currentNode);
for (int i = 0; i < Graph.Nodes; i++)
{
if (Graph.Matrix[currentNode.Index - 1, i] != 0)
{
Node node = NodeList.FindNode((int)dt.Rows[i]["id"]);
node.CurrentCost = currentNode.CurrentCost + Graph.Matrix[currentNode.Index - 1, i];
// 如果节点存在于关闭集合，则跳过后续步骤，直接下一个循环
if (ClosedSet.IsExist(node))
{
continue;
}
// 如果节点不在开放集合，则将节点加入到开放集合
else if (!OpenSet.IsExist(node))
{
node.NParent = currentNode;
OpenSet.Add(node);
}
else
{
// 如果当前点到查询到的点的代价比现在的大，则更新节点的父节点
if (OpenSet.FindNode(node.Index).CurrentCost >= node.CurrentCost)
{
node.NParent = currentNode;
OpenSet.ReplaceNode(node);
}
}
}
}
}
StoreHeap finalPath = new StoreHeap(); //最终路径的节点列表
Node recallnode = goalNode;
finalPath.Add(recallnode);
// 只要当前点还存在父节点，则将其都加入到最终路径的节点列表
while (recallnode.HasParent())
{
recallnode = recallnode.NParent;
finalPath.Add(recallnode);
}
finalPath.Reverse();
return(finalPath);
}
// 如果起始点和目标点为同一点，则返回一个包含起始点的节点列表
else
{
StoreHeap result = new StoreHeap();
result.Add(NodeList.FindNode(snId));
return(result);
}
}
public override void IterateOpenSet()
{
//Track how many times the open set has been iterated over
OpenSetIterations++;
//If the open set is empty, then no pathway could be found
if (OpenSet.Count <= 0)
{
//Print a failure message and reset the grid
Log.Print("Unable to find a valid pathway using Theta* algorithm, after a total " + OpenSetIterations + " iterations.");
FindingPathway = false;
GridManager.Instance.HideAllParentIndicators();
return;
}
//Grab the node from open set with the lowest FScore value, move it from the OpenSet to the ClosedSet
Node Current = GridManager.Instance.FindCheapestNode(OpenSet);
OpenSet.Remove(Current);
ClosedSet.Add(Current);
//If the Current node is the EndNode, then the pathway has been found
if (Current == PathEnd)
{
//Announce the pathway has been found
Log.Print("Theta* pathfinding complete after " + OpenSetIterations + " iterations.");
//Hide all nodes parent indicators
GridManager.Instance.HideAllParentIndicators();
//Get the completed pathway
List <Node> FinalPathway = GridManager.Instance.GetCompletedPathway(PathStart, PathEnd);
//Change all pathway node types so the completed pathway is displayed
foreach (Node PathStep in FinalPathway)
{
PathStep.SetType(NodeType.Pathway);
}
//Finalize the pathfinding process
FindingPathway = false;
return;
}
//Go through all the current nodes neighbours
foreach (Node Neighbour in GridManager.Instance.GetTraversableNeighbours(Current))
{
//Ignore neighbours in the closed list
if (ClosedSet.Contains(Neighbour))
{
continue;
}
if (!OpenSet.Contains(Neighbour))
{
Neighbour.GScore = Mathf.Infinity;
Neighbour.Parent = null;
}
UpdateNode(Current, Neighbour);
}
}
private bool CalculateNextState()
{
if (OpenSet.Count > 0)
{
Square current = OpenSet[0];
// choose a square with a lowest F-cost
// H-cost - distance from a current node to end node +
// G-cost - distance from starting node to current node
// if F-cost is the same, then we choose the one with the lowest H-cost
for (int i = 1; i < OpenSet.Count; i++)
{
if (OpenSet[i].FCost < current.FCost || OpenSet[i].FCost == current.FCost && OpenSet[i].HCost < current.HCost)
{
current = OpenSet[i];
}
}
// remove current from open set
// add it to closed set, saying that it's
// no longer going to be operated on
OpenSet.Remove(current);
ClosedSet.Add(current);
// if we reached the end, escape
if (current == End)
{
RetracePath(Start, End);
return(false);
}
//RetracePath(Start, current);
// get all the neighbors of the current node
foreach (Square neighbor in GetNeighbors(current))
{
// skipping neighbors that are already in closed set
// and are obstacles
if (ClosedSet.Contains(neighbor) || neighbor.Obstacle)
{
continue;
}
// G-cost of a neighbor
int costToNeighbor = current.GCost + Heuristics(current, neighbor);
// if open set doesn't contain neighbor
// then the value if neighbor.GCost is going to be 0
// we will need to update neighbor's cost if we meet it second time
if (costToNeighbor < neighbor.GCost || !OpenSet.Contains(neighbor))
{
neighbor.GCost = costToNeighbor;
neighbor.HCost = Heuristics(neighbor, End);
// remember previous node of neighbor, so that
// we can retrace the path
neighbor.Previous = current;
if (!OpenSet.Contains(neighbor))
{
OpenSet.Add(neighbor);
}
}
}
return(true);
}
return(false);
}
public override SchedulareState Execute(Schedulare schedulare, ShiftsContainer shiftsContainer)
{
InitParams(schedulare, shiftsContainer);
var schedulareState = GetSchedulareState(schedulare.DeepClone(), shiftsContainer, TreeRoot);
OpenSet.Add(schedulareState);
ExecuteStopwatch.Start();
UpdateThreshold(schedulareState);
while (!OpenSet.IsNullOrEmpty())
{
var currState = GetCurrentState(OpenSet);
UpdateCurrentBestSolution(currState);
OpenSet.Remove(currState);
CloseSet.Add(currState);
var currNode = currState.Node;
PrintDebugData(shiftsContainer, currState);
if (IsGoal())
{
UpdateCurrentBestSolution(currState);
break;
}
// if the current node is full schedulare but it is not goal yet
// remove the node from open list and look for another solutions
if (IsSchedulareFull(currNode.Value, shiftsContainer))
{
UpdateCurrentBestSolution(currState);
OpenSet.Remove(currState);
CloseSet.Add(currState);
continue;
}
// create and add child nodes
#region build new nodes
foreach (var emp in shiftsContainer.EmployeeConstraints.Select(x => x.Name))
{
var newNodeSchedulare = currNode.Value.DeepClone();
var currShiftToAssin = GetIncompleteShift(newNodeSchedulare, shiftsContainer);
// modify schdulare
currShiftToAssin.Workers.Add(new Worker()
{
Name = emp
});
// add new node to the tree - currNode
var childNode = currNode.AddChild(newNodeSchedulare);
// get new state
var newNodeState = GetSchedulareState(newNodeSchedulare, shiftsContainer, childNode);
// add new state to openSet
OpenSet.Add(newNodeState);
}
#endregion
}
PrintDebugData(shiftsContainer, CurrentBestSolution);
var ret = CurrentBestSolution;
CurrentBestSolution = null;
IsFinished = false;
ExecuteStopwatch.Reset();
return(ret);
}
public override void IterateOpenSet()
{
//Track how many iterations have taken place to find this pathway
OpenSetIterations++;
//Once the openset has been exhausted its time to complete the pathway
if (OpenSet.Count <= 0)
{
//Get the completed pathway
List <Node> Pathway = GridManager.Instance.GetCompletedPathway(PathStart, PathEnd);
//If the path is empty, then no pathway was able to be found between the target nodes
if (Pathway == null)
{
//Print a failure message and reset the grid
Log.Print("Unable to find a valid pathway using Dijkstras algorithm.");
FindingPathway = false;
GridManager.Instance.HideAllParentIndicators();
return;
}
//Announce the pathway has been found
Log.Print("Dijkstras pathfinding completed after " + OpenSetIterations + " iterations.");
//Hide all the neighbour indicators
GridManager.Instance.HideAllParentIndicators();
//Change the type of all nodes in the pathway to display it in the game
foreach (Node PathStep in Pathway)
{
PathStep.SetType(NodeType.Pathway);
}
//Complete the process
FindingPathway = false;
return;
}
//Find the new current node, then iterate through all its neighbours
Node Current = GridManager.Instance.FindCheapestNode(OpenSet);
OpenSet.Remove(Current);
foreach (Node Neighbour in GridManager.Instance.GetTraversableNeighbours(Current))
{
//Ignore nodes not listed in the open set
if (!OpenSet.Contains(Neighbour))
{
continue;
}
//check if its cheaper to travel over this neighbour
float NeighbourCost = Current.FScore + GridManager.FindHeuristic(Current, Neighbour);
if (NeighbourCost < Neighbour.FScore)
{
//update this neighbour as the best way to travel
Neighbour.FScore = NeighbourCost;
Neighbour.Parent = Current;
Neighbour.ToggleParentIndicator(true);
Neighbour.PointIndicator(Neighbour.GetDirection(Current));
}
}
}
