public bool FindTheShortesPath(Node Start, Node Target)
{
Node CurrentNode = Start;
OpenList.Add(CurrentNode);
for (; OpenList.Count > 0;)
{
CurrentNode = OpenList[0];
if (CurrentNode.Equals(Target))
{
VisitedNodes.Add(Target);
for (int Index = 0; Index < VisitedNodes.Count - 1; Index++)
{
ShortesPath.Add(InnerGraph.FindEdge(VisitedNodes[Index], VisitedNodes[Index + 1]));
}
return(true);
}
OpenList.Remove(CurrentNode);
ClosedList.Add(CurrentNode);
foreach (Node Inheritor in CurrentNode.Inheritors.Where(Node => Node != null && Node.Index != Node.Inheritors.Length - 1))
{
if (!ClosedList.Contains(Inheritor))
{
if (!OpenList.Contains(Inheritor))
{
Inheritor[Inheritor.Index] = CurrentNode;
Inheritor.HeuristicPathWeight = CalculateHeuristic(Inheritor, Target);
Inheritor.GainedPathWeight = InnerGraph.FindEdge(CurrentNode, Inheritor).Weight;
Inheritor.TotalPathWeight = Inheritor.GainedPathWeight + Inheritor.HeuristicPathWeight;
OpenList.Add(Inheritor);
OpenList = OpenList.OrderBy(Node => Node.TotalPathWeight).ToList <Node>();
}
}
}
VisitedNodes.Add(CurrentNode);
}
return(true);
}
bool PushOpenList(int StandardPos, int OpenPos)
{
if (!ClosedList.Contains(OpenPos))
{
if (!OpenList.Contains(OpenPos))
{
OpenList.Add(OpenPos);
((Tile)Tiles[OpenPos]).Goal = CalcuGoal(StandardPos, OpenPos);
((Tile)Tiles[OpenPos]).Heuristic = CalcuHeuristic(OpenPos, EndPos);
((Tile)Tiles[OpenPos]).Fitness = ((Tile)Tiles[OpenPos]).Goal + ((Tile)Tiles[OpenPos]).Heuristic;
((Tile)Tiles[OpenPos]).ParentTile = StandardPos;
return(true);
}
}
return(false);
}
int FindNextPathIndexFromOpenList()
{
int FindPath = -1;
float FindFitness = 999999999.0f;
foreach (int CurIndex in OpenList)
{
if (false == ClosedList.Contains(CurIndex))
{
if (((Tile)Tiles[CurIndex]).Fitness < FindFitness)
{
FindPath = CurIndex;
FindFitness = ((Tile)Tiles[CurIndex]).Fitness;
}
}
}
return(FindPath);
}
void OpenListAdd(int checkX, int checkY)
{
if (checkX >= BottomLeft.x && checkX < TopRight.x + 1 && checkY >= BottomLeft.y &&
checkY < TopRight.y + 1 && !NodeArray[checkX - BottomLeft.x, checkY - BottomLeft.y].isWall &&
!ClosedList.Contains(NodeArray[checkX - BottomLeft.x, checkY - BottomLeft.y]))
{
Node NeighborNode = NodeArray[checkX - BottomLeft.x, checkY - BottomLeft.y];
int MoveCost = CurNode.G + 10;
if (MoveCost < NeighborNode.G || !OpenList.Contains(NeighborNode))
{
NeighborNode.G = MoveCost;
NeighborNode.H = (Mathf.Abs(NeighborNode.x - TargetNode.x) + Mathf.Abs(NeighborNode.y - TargetNode.y)) * 10;
NeighborNode.ParentNode = CurNode;
OpenList.Add(NeighborNode);
}
}
}
public SearchResult <T> Search()
{
T state = Problem.InitialState;
HeuristicNode <T> node = new HeuristicNode <T>(state, 1);
if (Problem.GoalTest(state))
{
return(new SearchResult <T>(node));
}
PriorityQueue.Push(node);
OpenList.Add(state);
while (!PriorityQueue.IsEmpty)
{
node = PriorityQueue.Pop();
state = node.State;
ClosedList.Add(state);
foreach (IAction <T> action in Problem.Actions(state))
{
HeuristicNode <T> childNode = node.ChildNode(Problem, action, Heuristic);
T childState = childNode.State;
if (ClosedList.Contains(childState) || OpenList.Contains(childState))
{
if (PriorityQueue.Contains(childNode) && childNode.HeuristicCost > node.HeuristicCost)
{
PriorityQueue.Push(childNode);
OpenList.Add(childState);
}
}
if (!ClosedList.Contains(childState) && !OpenList.Contains(childState))
{
if (Problem.GoalTest(childState))
{
return(new SearchResult <T>(childNode));
}
PriorityQueue.Push(childNode);
OpenList.Add(childState);
}
}
}
return(new SearchResult <T>(null));
}
void OpenListAdd(int checkX, int checkY)
{
// 상하좌우 범위를 벗어나지 않고, 벽이 아니면서, 닫힌리스트에 없다면
if (checkX >= bottomLeft.x && checkX < topRight.x + 1 && checkY >= bottomLeft.y && checkY < topRight.y + 1 && !NodeArray[checkX - bottomLeft.x, checkY - bottomLeft.y].isWall && !ClosedList.Contains(NodeArray[checkX - bottomLeft.x, checkY - bottomLeft.y]))
{
// 대각선 허용시, 벽 사이로 통과 안됨
if (allowDiagonal)
{
if (NodeArray[CurNode.x - bottomLeft.x, checkY - bottomLeft.y].isWall && NodeArray[checkX - bottomLeft.x, CurNode.y - bottomLeft.y].isWall)
{
return;
}
}
// 코너를 가로질러 가지 않을시, 이동 중에 수직수평 장애물이 있으면 안됨
if (dontCrossCorner)
{
if (NodeArray[CurNode.x - bottomLeft.x, checkY - bottomLeft.y].isWall || NodeArray[checkX - bottomLeft.x, CurNode.y - bottomLeft.y].isWall)
{
return;
}
}
// 이웃노드에 넣고, 직선은 10, 대각선은 14비용
Node NeighborNode = NodeArray[checkX - bottomLeft.x, checkY - bottomLeft.y];
int MoveCost = CurNode.G + (CurNode.x - checkX == 0 || CurNode.y - checkY == 0 ? 10 : 14);
// 이동비용이 이웃노드G보다 작거나 또는 열린리스트에 이웃노드가 없다면 G, H, ParentNode를 설정 후 열린리스트에 추가
if (MoveCost < NeighborNode.G || !OpenList.Contains(NeighborNode))
{
NeighborNode.G = MoveCost;
NeighborNode.H = (Mathf.Abs(NeighborNode.x - TargetNode.x) + Mathf.Abs(NeighborNode.y - TargetNode.y)) * 10;
NeighborNode.ParentNode = CurNode;
OpenList.Add(NeighborNode);
}
}
}
/// <summary>
/// Finds the shortest path from every point in the PRM to the goal.
/// </summary>
/// <param name="points">A list of points in the PRM</param>
/// <param name="edges">A matrix of edges in the PRM</param>
/// <param name="goalID">The index of the goal point in the points array</param>
/// <param name="ct">A cancellation token to signal the task to abort early</param>
private void FindPaths(Vector3[] points, float[,] edges, int goalID, CancellationToken ct)
{
Stopwatch sw = Stopwatch.StartNew();
var len = points.Length;
// Standard Djikstra's algorithm
var openList = new PriorityQueue(len / 2);
openList.Add(new QueueNode(points[goalID], 0, null, goalID));
var closedList = new ClosedList(len);
while (!openList.IsEmpty())
{
// If the background thread is cancelled, abort operation.
if (ct.IsCancellationRequested)
{
throw new TaskCanceledException();
}
QueueNode current = openList.Pop();
closedList.Add(current);
for (int nextIdx = 0; nextIdx < len; nextIdx++)
{
if (nextIdx == current.ID)
{
continue;
}
var costCurrentToNext = edges[current.ID, nextIdx];
if (float.IsNegativeInfinity(costCurrentToNext))
{
continue;
}
if (closedList.Contains(nextIdx))
{
continue;
}
var totalCostToNext = current.Depth + costCurrentToNext;
if (openList.Contains(nextIdx))
{
openList.ReparentPathNode(nextIdx, current, costCurrentToNext);
}
else
{
openList.Add(new QueueNode(points[nextIdx], totalCostToNext, current, nextIdx));
}
}
}
sw.Stop();
Debug.Log("Found paths in " + sw.ElapsedMilliseconds + "ms");
Results = new PathNode[len];
foreach (var pnode in closedList.List)
{
for (int i = 0; i < len; i++)
{
if (pnode.ID != i)
{
continue;
}
Vector3 direction = pnode.Parent != null ? pnode.Parent.Position - pnode.Position: Vector3.zero;
Results[i] = new PathNode(pnode.Position, direction, pnode.Depth);
break;
}
}
}
