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 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
}
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));
}
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);
}
/// <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;
}
}
}
private void AddToClosed(AStarNode s)
{
ClosedList.Add(s);
}