public IList <IGraphNode> GetPath(IGraph graph, IGraphNode startNode, IGraphNode stopNode)
{
List <IGraphNode> openNodes = new List <IGraphNode> {
startNode
};
while (openNodes.Count > 0)
{
openNodes.Sort(this);
IGraphNode currentNode = openNodes[0];
openNodes.RemoveAt(0);
if (currentNode == null || IsClosed(currentNode))
{
//Console.WriteLine("Processing closed node!");
continue;
}
Vector2Int position = currentNode.Position;
OnCellViewedEvent?.Invoke(this, position.X, position.Y, GetStepsToStart(currentNode));
SetClosed(currentNode);
if (currentNode == stopNode)
{
break;
}
foreach (var connected in currentNode.GetConnectedNodes())
{
if (!IsClosed(connected))
{
SetParentNode(connected, currentNode);
openNodes.Add(connected);
}
}
}
if (!GetData <GraphData>(startNode).IsClosed)
{
return(null);
}
List <IGraphNode> path = new List <IGraphNode>();
IGraphNode currentPathNode = stopNode;
path.Add(currentPathNode);
while (currentPathNode != startNode)
{
currentPathNode = GetData <GraphData>(currentPathNode).ParentNode;
path.Add(currentPathNode);
}
return(path);
}
private void OnAStarCellViewed(object sender, int x, int y, int d)
{
if (_currentCellCluster != null)
{
OnCellViewedEvent?.Invoke(this, x + _currentCellCluster.LeftBottom.X,
y + _currentCellCluster.LeftBottom.Y, d);
}
else
{
OnCellViewedEvent?.Invoke(this, x, y, d);
}
}
private void UpdateCellSurroundings(ICellMap map, int[,] matrix, int x, int y, int d, NeighbourMode neighbourMode)
{
foreach (var step in Steps.GetSteps(neighbourMode))
{
int xNew = x + step.X;
int yNew = y + step.Y;
if (!map.IsInBounds(xNew, yNew))
{
continue;
}
if (map.IsPassable(xNew, yNew) && matrix[xNew, yNew] == -1)
{
matrix[xNew, yNew] = d;
OnCellViewedEvent?.Invoke(this, xNew, yNew, d);
}
}
}
public IList <IGraphNode> GetPath(IGraph map, IGraphNode startNode, IGraphNode stopNode)
{
if (startNode == null || stopNode == null)
{
return(null);
}
List <IGraphNode> openNodes = new List <IGraphNode>();
bool isWeighted = startNode is IWeightedGraphNode <double>;
openNodes.Add(startNode);
while (openNodes.Count > 0)
{
//SortFValue(openNodes);
IGraphNode x = DequeueLast(openNodes);
Vector2Int position = GetPosition(x);
OnCellViewedEvent?.Invoke(this, position.X, position.Y, (int)GetGValue(x));
if (x == stopNode)
{
break;
}
CloseNode(x);
foreach (var y in x.GetConnectedNodes())
{
if (IsClosed(y))
{
continue;
}
int yIndex = openNodes.IndexOf(y);
if (yIndex == -1)
{
SetPathPredecessor(y, x);
double nodesDistance;
if (isWeighted)
{
nodesDistance = GetConnectionWeight((IWeightedGraphNode <double>)x, (IWeightedGraphNode <double>)y);
}
else
{
nodesDistance = GetConnectionWeight(x, y);
}
SetGValue(y, GetGValue(x) + nodesDistance);
double estimateDistance = GetEstimateDistance(y, stopNode);
SetHValue(y, estimateDistance);
Utils.InsertSortedDescending(openNodes, y, this);
}
else
{
double nodesDistance;
if (isWeighted)
{
nodesDistance = GetConnectionWeight((IWeightedGraphNode <double>)x, (IWeightedGraphNode <double>)y);
}
else
{
nodesDistance = GetConnectionWeight(x, y);
}
double xGValue = GetGValue(x) + nodesDistance;
double yGValue = GetGValue(y);
if (xGValue < yGValue)
{
SetGValue(y, xGValue);
SetPathPredecessor(y, x);
openNodes.RemoveAt(yIndex);
Utils.InsertSortedDescending(openNodes, y, this);
}
}
}
}
if (GetPathPredecessor(stopNode) == null)
{
return(null);
}
List <IGraphNode> path = new List <IGraphNode>();
path.Add(stopNode);
IGraphNode currentNode = stopNode;
while (true)
{
currentNode = GetPathPredecessor(currentNode);
if (currentNode == null)
{
break;
}
path.Add(currentNode);
}
return(path);
}
public IList <Vector2Int> GetPath(ICellMap map, Vector2Int start, Vector2Int stop, NeighbourMode neighbourMode)
{
WeightedGraph <double> weightedGraph = GraphGenerator.GetWeightedGraph(map, neighbourMode);
int GetNodeIndex(int x, int y) => y * map.Width + x;
Vector2Int GetNodePosition(int index)
{
int y = index / map.Width;
int x = index - map.Width * y;
return(new Vector2Int(x, y));
}
void SetArrayValue <T>(T[] array, int x, int y, T value) => array[GetNodeIndex(x, y)] = value;
int count = weightedGraph.Width * weightedGraph.Height;
double[] distances = new double[count];
bool[] used = new bool[count];
int[] prev = new int[count];
FillArray(distances, Int32.MaxValue);
FillArray(prev, -1);
FillArray(used, false); // Can be removed
SetArrayValue(distances, start.X, start.Y, 0);
double minDistance = 0;
int minVertex = GetNodeIndex(start.X, start.Y);
while (minDistance < Int32.MaxValue)
{
int i = minVertex;
used[i] = true;
for (int j = 0; j < count; j++)
{
double distanceI = distances[i];
double distanceJ = distances[j];
double weightIj = weightedGraph.GetWeight(i, j);
if (double.IsPositiveInfinity(weightIj))
{
continue;
}
double nDistance;
if (double.IsPositiveInfinity(distanceI))
{
nDistance = Int32.MaxValue;
}
else
{
nDistance = distanceI + weightIj;
}
if (nDistance < distanceJ)
{
distanceJ = nDistance;
distances[j] = distanceJ;
prev[j] = i;
Vector2Int nodePosition = GetNodePosition(j);
OnCellViewedEvent?.Invoke(this, nodePosition.X, nodePosition.Y, (int)distanceJ);
}
}
minDistance = Double.PositiveInfinity;
for (int j = 0; j < count; j++)
{
if (!used[j] && distances[j] < minDistance)
{
minDistance = distances[j];
minVertex = j;
}
}
}
List <Vector2Int> path = new List <Vector2Int>();
int pathIndex = GetNodeIndex(stop.X, stop.Y);
while (pathIndex != -1)
{
path.Add(GetNodePosition(pathIndex));
pathIndex = prev[pathIndex];
}
path.Reverse();
return(path);
}
public IList <Vector2Int> GetPath(ICellMap mapBase, Vector2Int start, Vector2Int stop, NeighbourMode neighbourMode)
{
if (_layeredCellMap == null)
{
_layeredCellMap = new LayeredCellMap(mapBase);
}
int[,] matrix = new int[_layeredCellMap.Width, _layeredCellMap.Height];
for (int i = 0; i < _layeredCellMap.Width; i++)
{
for (int j = 0; j < _layeredCellMap.Height; j++)
{
matrix[i, j] = -1;
}
}
matrix[start.X, start.Y] = 0;
OnCellViewedEvent?.Invoke(this, start.X, start.Y, 0);
int d = 0;
bool goFurther = true;
while (goFurther)
{
goFurther = false;
for (int i = 0; i < _layeredCellMap.Width; i++)
{
if (matrix[stop.X, stop.Y] != -1)
{
break;
}
for (int j = 0; j < _layeredCellMap.Height; j++)
{
if (matrix[stop.X, stop.Y] != -1)
{
break;
}
if (matrix[i, j] == d)
{
UpdateCellSurroundings(_layeredCellMap, matrix, i, j, d + 1, neighbourMode);
goFurther = true;
}
}
}
d++;
}
if (matrix[stop.X, stop.Y] == -1)
{
return(null);
}
List <Vector2Int> path = new List <Vector2Int>();
Vector2Int current = stop;
path.Add(current);
while (current != start)
{
current = FindDecrement(matrix, current.X, current.Y, neighbourMode);
path.Add(current);
}
path.Reverse();
return(path);
}
