private void PerformFirstStep()
{
currentNode = StartNode;
currentNode.MovementCost = 0;
if (IsPathFound(currentNode))
{
HandleFoundPath(currentNode);
}
openSet.Add(currentNode);
}
//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 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));
});
}
});
}
}
protected void StartPath(IHex start)
{
var path = new DirectedPath(start);
OpenSet.Add(path.PathStep.Hex.Coords, path);
Queue.Enqueue(0, path);
}
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));
}
}
private void CalculateNeighbors(Node current, IReadOnlyList <Node> neighbors)
{
foreach (Node neighbor in neighbors)
{
if (ClosedSet.Contains(neighbor) || neighbor.IsWall)
{
continue;
}
int tempGScore = current.GScore + 1;
bool newPath = false;
if (OpenSet.Contains(neighbor))
{
if (tempGScore < neighbor.GScore)
{
neighbor.GScore = tempGScore;
newPath = true;
}
}
else
{
neighbor.GScore = tempGScore;
OpenSet.Add(neighbor);
neighbor.State = NodeState.Open;
newPath = true;
}
if (newPath)
{
neighbor.Hscore = Heuristic(neighbor, _finishNode);
neighbor.FScore = neighbor.GScore + neighbor.Hscore;
neighbor.Previous = current;
}
}
}
public override SchedulareState Execute(Schedulare schedulare, ShiftsContainer shiftsContainer)
{
InitParams(schedulare, shiftsContainer);
var schedulareState = GetSchedulareState(schedulare.DeepClone(), shiftsContainer, TreeRoot);
UpdateCurrentBestSolution(schedulareState);
OpenSet.Add(schedulareState);
ExecuteStopwatch.Start();
var shiftsList = GetShiftList(schedulareState.Node.Value);
while (!IsGoal())
{
var randShift = GetRandomShift(shiftsList);
// DEBUG
PrintDebugData(shiftsContainer, CurrentBestSolution);
ExplorationStopwatch.Reset();
ExplorationStopwatch.Start();
#region Exploration loop
while (ExplorationStopwatch.Elapsed.TotalSeconds < EXPLORATION_TIME_SECONDS)
{
// IsGoal
// OR break id exploration ended with no result
if (IsGoal() || OpenSet.Count.Equals(0))
{
break;
}
var currState = OpenSet.FindMin();
UpdateCurrentBestSolution(currState);
OpenSet.DeleteMin();
CloseSet.Add(currState.Node.Value);
for (int workerIndex = 0; workerIndex < randShift.Shift.Workers.Count; workerIndex++)
{
var currStateInOrderToReplaceEmp = currState.DeepClone();
var currStateNodeInOrderToReplaceEmp = currStateInOrderToReplaceEmp.Node;
RemoveEmpFromCurrShift(randShift, workerIndex, currStateInOrderToReplaceEmp);
// DEBUG
PrintDebugData(shiftsContainer, currStateInOrderToReplaceEmp);
#region build new nodes
foreach (var emp in shiftsContainer.EmployeeConstraints.Select(x => x.Name))
{
var newNodeSchedulare = currStateNodeInOrderToReplaceEmp.Value.DeepClone();
var currShiftToAssin = GetIncompleteShift(newNodeSchedulare, shiftsContainer);
// modify schdulare
currShiftToAssin.Workers.Remove(currShiftToAssin.Workers.FirstOrDefault(x => x.Name.IsNullOrEmpty()));
currShiftToAssin.Workers.Add(new Worker()
{
Name = emp
});
// validate if the new state in tabu list - is yes ignore it
if (CloseSet.Contains(newNodeSchedulare))
{
if (!DEBUG)
{
continue;
}
Console.WriteLine($"####### Tabu list filterd #######");
continue;
}
// add new node to the tree - currNode
var childNode = currStateNodeInOrderToReplaceEmp.AddChild(newNodeSchedulare);
// get new state
var newNodeState = GetSchedulareState(newNodeSchedulare, shiftsContainer, childNode);
// add new state to openSet
OpenSet.Add(newNodeState);
// DEBUG
PrintDebugData(shiftsContainer, newNodeState);
}
#endregion
}
}
ExplorationStopwatch.Stop();
#endregion
if (IsGoal())
{
break;
}
}
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 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 void SetStart(int x, int y)
{
Start = Squares[x, y];
OpenSet.Add(Start);
}
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 PathFinderResult FindPath()
{
GraphNodeHeuristic tempX, tempY;
bool updateDist;
int i, conn, distTemp;
var numberOfSteps = 0;
Distances[Graph.StartNode.NodeCode] = 0; // distance in firts node is zero
OpenSet.Add(new GraphNodeHeuristic(Graph.StartNode, 0, 0)); //O(log n)
while (OpenSet.Count > 0) //O(1)
{
numberOfSteps++;
tempX = OpenSet.ExtractMin(); // get first suitable key and remove it (O(log n))
if (tempX.GraphNode.Equals(Graph.EndNode))
{
return new PathFinderResult
{
Path = Path,
PathDirections = PathDirections,
Distances = Distances
}
}
;
ClosedSet[tempX.NodeCode] = true;
for (i = 0; i < 4; i++) // four directions
{
conn = tempX.NodeConnections[i];
if (conn == int.MaxValue || ClosedSet[conn])
{
continue; //skip value, no connections
}
distTemp = Distances[tempX.NodeCode] + Graph.GraphBody[conn].NodeValue;
tempY = new GraphNodeHeuristic(Graph.GraphBody[conn], distTemp,
HeuristicComparer.ComputeHeuristic(Graph.GraphBody[conn]));
if (!OpenSet.Contains(tempY)) //O(log 1)
{
OpenSet.Add(tempY); //O(log n)
updateDist = true;
}
else if (distTemp < Distances[tempY.NodeCode])
{
updateDist = true;
}
else
{
updateDist = false;
}
if (updateDist)
{
OpenSet.UpdateDistance(tempY); // O(log n)
Path[tempY.NodeCode] = tempX.GraphNode;
PathDirections[tempY.NodeCode] = GraphDirections.GetDirectionCharCode((byte)i);
Distances[tempY.NodeCode] = distTemp;
}
}
}
// no path found
return(new PathFinderResult
{
Path = null,
PathDirections = null,
Distances = null
});
}
}
