private static void DrawGraphNode(Bitmap bitmap, IGraphNode node, int scale, List <IGraphNode> visitedList)
{
DrawScaledPixel(bitmap, Color.Blue, node.Position.X, node.Position.Y, scale);
visitedList.Add(node);
LogManager.Log($"Visited list size: {visitedList.Count}");
foreach (var connection in node.GetConnectedNodes())
{
if (!visitedList.Contains(connection))
{
DrawGraphNode(bitmap, connection, scale, visitedList);
}
}
}
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);
}
