public IList <AStarNode> Expand(AStarNode state)
{
AddInitial(state); // dodaj startowy element do listy zamkniętych(odwiedzonych)
AStarNode tmp;
while (!IsGoal(ClosedList.Last()))
{
OpenList.OrderBy(p => p.F); //lista otwartych posortowana wg najmniejszej wagi współczynnika F
tmp = OpenList.First(); //weź pierwszy element z otwartej listy(najlepiej prosperujący)
RemoveFirst(OpenList); //przenieś do zamkniętej
tmp.Opened = false;
tmp.Closed = true;
AddToClosed(tmp);
ManageNeighbours(ClosedList.Last()); //dodaj sąsiadów do otwartej listy lub zaktualizuj wsp F i rodzica
}
AStarNode parent = Goal; //uzupełnij listę ze ścieżką poprzez wybranie elementu docelowego i rekursywnie dodaj wszystkie elementy połączone
while (parent != null)
{
StackTrace.Insert(0, parent);
parent = parent.Parent;
}
return(StackTrace);
}
public void PathFinding(GameTime gameTime)
{
while (!Contains(arrival, ClosedList) && OpenedList.Count != 0)
{
current = new Node();
current = FindMinPoids(OpenedList);
OpenedList.Remove(current);
if (current.Position.X >= 0 && current.Position.X + monster.TextureSize.X <= mapData.MapWidth * 32 && current.Position.Y >= 0 && current.Position.Y + monster.TextureSize.Y <= 32 * mapData.MapHeight)
{
ClosedList.Add(current);
FindNode(current);
}
}
if (Contains(arrival, ClosedList))
{
list = new List <Vector2>();
test = new Node();
test = ClosedList.Find(node => node.Id == arrival.Id);
list.Add(test.Position);
while (test.Parent != null)
{
test = test.Parent;
list.Add(test.Position);
}
Mouvement(gameTime);
}
}
public void Reset()
{
for (int i = 0; i < NodeCost.Length; i++)
{
NodeCost[i] = 0;
ParentNode[i] = -1;
}
OpenList.Clear();
ClosedList.Clear();
}
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);
}
private void AddInitial(AStarNode initialState)
{
InitialState = initialState;
InitialState.Closed = true;
foreach (var item in InitialState.Neighbours) //dodaj wszystkich sąsiadów do listy otwartej
{
item.Opened = true;
item.Parent = InitialState;
item.ComputeF(InitialState.Position, Goal.Position);
AddToOpen(item);
}
ClosedList.Add(InitialState); //dodaj do listy zamkniętej
}
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));
}
/// <summary>
/// 重置
/// </summary>
public void ReSet()
{
if (StartCell != null)
{
MapBmpFillRectangle(StartCell, this._whiteSmoke);
StartCell = null;
}
if (GoalCell != null)
{
MapBmpFillRectangle(GoalCell, this._whiteSmoke);
GoalCell = null;
}
MapBmpFillRectangles(StoneCells, this._whiteSmoke);
StoneCells.Clear();
MapBmpFillRectangles(ClosedList, this._whiteSmoke);
ClosedList.Clear();
MapBmpFillRectangles(OpenList, this._whiteSmoke);
OpenList.Clear();
}
/// <summary>
/// Generic search algorithm
/// </summary>
private static SearchResult GenericSearch(StateBase initialState, StateBase goalState, string algName, OpenListComparator comparator, CostFunc cost, IHeuristic heuristic, int? depthLimit = null)
{
var openList = new OpenList(comparator);
var closedList = new ClosedList();
var cache = new HeuristicCache(goalState, heuristic);
var nodesGenerated = 0;
var nodesPrevGenerated = 0;
// add initial node to open list
openList.Push(new SearchNode(cost, initialState, null, null, cache.Evaluate));
while (true)
{
// if nothing on open list, fail
if (openList.Count == 0)
{
return new SearchResult(null, nodesGenerated, nodesPrevGenerated, openList.Count, closedList.Count, algName, heuristic);
}
// get next node to expand
var node = openList.Pop();
closedList.Push(node);
// if at goal state, success
if (node.State.Equals(goalState))
{
return new SearchResult(node, nodesGenerated, nodesPrevGenerated, openList.Count, closedList.Count, algName, heuristic);
}
// if at depth limit, don't generate successors
if (depthLimit != null && node.Depth == depthLimit)
{
continue;
}
var daughters = node.Successors(cost, cache.Evaluate);
foreach (var daughter in daughters)
{
nodesGenerated++;
// if this state is already in open list, replace old node with new node if g-hat is better
var foundInOpen = openList.Find(daughter.State);
if (foundInOpen != null)
{
nodesPrevGenerated++;
if (daughter.Ghat < foundInOpen.Ghat)
{
openList.Replace(foundInOpen, daughter);
}
}
else
{
// else if this state is already in closed list, move from closed to open if g-hat is better
var foundInClosed = closedList.Find(daughter.State);
if (foundInClosed != null)
{
nodesPrevGenerated++;
if (daughter.Ghat < foundInClosed.Ghat)
{
openList.Push(daughter);
closedList.Remove(foundInClosed);
}
}
else
{
// else didn't find in open or closed, add to open
openList.Push(daughter);
}
}
}
}
}
/// <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;
}
}
}
/// <summary>
/// Generic search algorithm
/// </summary>
private static SearchResult GenericSearch(StateBase initialState, StateBase goalState, string algName, OpenListComparator comparator, CostFunc cost, IHeuristic heuristic, int?depthLimit = null)
{
var openList = new OpenList(comparator);
var closedList = new ClosedList();
var cache = new HeuristicCache(goalState, heuristic);
var nodesGenerated = 0;
var nodesPrevGenerated = 0;
// add initial node to open list
openList.Push(new SearchNode(cost, initialState, null, null, cache.Evaluate));
while (true)
{
// if nothing on open list, fail
if (openList.Count == 0)
{
return(new SearchResult(null, nodesGenerated, nodesPrevGenerated, openList.Count, closedList.Count, algName, heuristic));
}
// get next node to expand
var node = openList.Pop();
closedList.Push(node);
// if at goal state, success
if (node.State.Equals(goalState))
{
return(new SearchResult(node, nodesGenerated, nodesPrevGenerated, openList.Count, closedList.Count, algName, heuristic));
}
// if at depth limit, don't generate successors
if (depthLimit != null && node.Depth == depthLimit)
{
continue;
}
var daughters = node.Successors(cost, cache.Evaluate);
foreach (var daughter in daughters)
{
nodesGenerated++;
// if this state is already in open list, replace old node with new node if g-hat is better
var foundInOpen = openList.Find(daughter.State);
if (foundInOpen != null)
{
nodesPrevGenerated++;
if (daughter.Ghat < foundInOpen.Ghat)
{
openList.Replace(foundInOpen, daughter);
}
}
else
{
// else if this state is already in closed list, move from closed to open if g-hat is better
var foundInClosed = closedList.Find(daughter.State);
if (foundInClosed != null)
{
nodesPrevGenerated++;
if (daughter.Ghat < foundInClosed.Ghat)
{
openList.Push(daughter);
closedList.Remove(foundInClosed);
}
}
else
{
// else didn't find in open or closed, add to open
openList.Push(daughter);
}
}
}
}
}
public void doPathFinding()
{
Node firstNode = new Node(m_current, null, m_end, m_start);
finalPath = new EnemyPathing();
OpenList openedList = new OpenList();
openedList.insertToOpenList(firstNode);
ClosedList closedList = new ClosedList();
SearchLvl search_lvl = new SearchLvl();
while (openedList.isEmpty() == false)
{
Node secondNodes = openedList.get0();
Node secondNode = new Node(secondNodes.getPosition(), secondNodes.returnPrev(), m_end, m_start);
openedList.remove0Fromlist();
closedList.insertToClosedList(secondNode);
if (Physics2D.OverlapBox(new Vector2(secondNode.getPosition().x, secondNode.getPosition().y), new Vector2(m_halfWidth / 2, m_halfWidth / 2), 0, targetMask))
{
results = secondNode;
break;
}
List <Node.Position> adjacentPositions = search_lvl.getAdjacentNodes(secondNode.getPosition().x, secondNode.getPosition().y, tileWid);
for (int i = 0; i < adjacentPositions.Count; i++)
{
if (closedList.isInClosedList(adjacentPositions[i]))
{
continue;
}
int inter = openedList.findFromOpenList(secondNode.getPosition()); //returns -1 if there was no match, iterator cant be negative anyway
Node previousNode = new Node(adjacentPositions[i], secondNode, m_end, m_start);
if (inter != -1)
{
//has been found in open list
if (openedList.returnAt(inter).getDistanceFromStart() > previousNode.getDistanceFromStart()) //the new distance is smaller
{
openedList.returnAt(inter).setPrev(secondNode);
//setting the previous node that is found at the index
}
}
else
{
//lineDrawer(previousNode.getPosition().x, previousNode.getPosition().y);
//This method just creates a box, used for debugging purposes only!
openedList.insertToOpenList(previousNode);
}
}
}
if (results != null)
{
while (results != null)
{
if (results.returnPrev() == null)
{
finalPath.insertToPath(results);
results = null;
donePathFind = true;
}
else
{
//Debug.DrawLine(new Vector2(results.getPosition().x, results.getPosition().y), new Vector2(results.returnPrev().getPosition().x, results.returnPrev().getPosition().y), Color.red, 1, false);
finalPath.insertToPath(results);
//draw a line to prev
results = results.returnPrev();
}
}
finalPath.Reversed();
}
else
{
donePathFind = true;
finalPath = null;
//NOT FOUND
}
}
public override ArrayList FindPaths(int StartPos, int InEndPos, int InID)
{
ArrayList Paths = new ArrayList();
if (!IsValidPosIndex(StartPos) || !IsValidPosIndex(InEndPos))
{
return(Paths);
}
CTimeCheck.Start(InID);
EndPos = InEndPos;
ClosedList.Clear();
OpenList.Clear();
foreach (Tile tile in Tiles)
{
if (tile.bBlock)
{
ClosedList.Add(tile.Index);
}
tile.Reset();
}
ClosedList.Add(StartPos);
int FindNextPath = StartPos;
int path_make_start_time = Environment.TickCount;
while (true)
{
//if (100 <= Environment.TickCount - path_make_start_time)
// return Paths;
ArrayList MakedOpenList = MakeOpenList(FindNextPath);
if (OpenList.Count == 0)
{
CDebugLog.Log(ELogType.AStar, "OpenList.Count == 0");
}
//FindNextPath = FindNextPathIndex(ref MakedOpenList);
////FindNextPath = FindNextPathIndexFromOpenList();
//if (-1 == FindNextPath)
//{
FindNextPath = FindNextPathIndexFromOpenList();
//}
if (-1 == FindNextPath)
{
CTimeCheck.End(ELogType.AStar, "Failed FindPaths");
return(Paths);
}
if (FindNextPath == EndPos)
{
break;
}
OpenList.Remove(FindNextPath);
ClosedList.Add(FindNextPath);
}
int path_index = EndPos;
Paths.Add(EndPos);
while (true)
{
if (path_index == StartPos)
{
break;
}
Paths.Add(((Tile)Tiles[path_index]).ParentTile);
path_index = ((Tile)Tiles[path_index]).ParentTile;
}
Paths.Reverse();
CTimeCheck.End(ELogType.AStar, "Success FindPaths");
return(Paths);
}
private void AddToClosed(AStarNode s)
{
ClosedList.Add(s);
}
private bool IsInClosed(Point cellPt)
{
return(ClosedList.FindIndex(record => record.Location.Equals(cellPt)) != -1);
}
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);
}
}
}
/**
* Test if this unit canMove to a given position
*
* @param pos
* The position we want to know if the unit can move to.
*
* @return
* true if it can move there. false if it cannot.
*/
public bool CanMove(Vector3 pos)
{
return(ClosedList.ContainsKey(pos));
}
