private Vector2Int FindDecrement(int[,] matrix, int x, int y, NeighbourMode neighbourMode)
{
int d = matrix[x, y];
Vector2Int[] steps = Steps.GetSteps(neighbourMode);
foreach (var step in steps)
{
int xNew = x + step.X;
int yNew = y + step.Y;
if (xNew < 0 || yNew < 0)
{
continue;
}
if (xNew >= matrix.GetLength(0) || yNew >= matrix.GetLength(1))
{
continue;
}
if (matrix[xNew, yNew] == d - 1)
{
return(new Vector2Int(xNew, yNew));
}
}
return(default(Vector2Int));
}
public void CalculatePaths(NeighbourMode neighbourMode, Action <object, int, int, int> onCellViewedCallback)
{
_pathMatrix = new IList <Vector2Int> [_transitionNodes.Count, _transitionNodes.Count];
AStarAlgorithm aStarAlgorithm = new AStarAlgorithm();
aStarAlgorithm.OnCellViewedEvent += onCellViewedCallback;
CoordinateTransformer transformer = new CoordinateTransformer(this, LeftBottom);
for (int i = 0; i < _transitionNodes.Count; i++)
{
for (int j = i + 1; j < _transitionNodes.Count; j++)
{
var nodeA = _transitionNodes[i];
var nodeB = _transitionNodes[j];
IList <Vector2Int> path =
aStarAlgorithm.GetPath(transformer, nodeA.Position - LeftBottom, nodeB.Position - LeftBottom,
neighbourMode);
_pathMatrix[i, j] = path;
_pathMatrix[j, i] = Utils.GetInvertedList(path);
if (path != null)
{
nodeA.SetWeight(nodeB, path.Count);
nodeB.SetWeight(nodeA, path.Count);
}
}
}
}
public HierarchicalMap GenerateMap(ICellMap cellMap, NeighbourMode neighbourMode, params int[] clusterSizes)
{
if (clusterSizes.Length == 1)
{
return(GenerateTwoLevelMap(cellMap, neighbourMode, clusterSizes[0]));
}
throw new NotImplementedException();
}
private HierarchicalMap GenerateTwoLevelMap(ICellMap cellMap, NeighbourMode neighbourMode,
int levelZeroClusterSize)
{
CellCluster[,] clusterMatrix = CreateClusters(cellMap, levelZeroClusterSize);
GeneratedClusters = clusterMatrix;
HierarchicalMap levelOneMap = new HierarchicalMap(levelZeroClusterSize, levelZeroClusterSize);
for (int i = 0; i < clusterMatrix.GetLength(0); i++)
{
for (int j = 0; j < clusterMatrix.GetLength(1); j++)
{
CellCluster currentCluster = clusterMatrix[i, j];
int prevI = i - 1;
int prevJ = j - 1;
int nextJ = j + 1;
if (prevI >= 0)
{
CellCluster neighbourCluster = clusterMatrix[prevI, j];
ProceedNeighbourClusters(currentCluster, neighbourCluster, levelOneMap);
}
if (prevJ >= 0)
{
CellCluster neighbourCluster = clusterMatrix[i, prevJ];
ProceedNeighbourClusters(currentCluster, neighbourCluster, levelOneMap);
}
if (prevJ >= 0 && prevI >= 0 && neighbourMode == NeighbourMode.SidesAndDiagonals)
{
CellCluster neighbourCluster = clusterMatrix[prevI, prevJ];
ProceedNeighbourClusters(currentCluster, neighbourCluster, levelOneMap);
}
if (nextJ < cellMap.Height && prevI >= 0 && neighbourMode == NeighbourMode.SidesAndDiagonals)
{
CellCluster neighbourCluster = clusterMatrix[prevI, nextJ];
ProceedNeighbourClusters(currentCluster, neighbourCluster, levelOneMap);
}
}
}
for (int i = 0; i < clusterMatrix.GetLength(0); i++)
{
for (int j = 0; j < clusterMatrix.GetLength(1); j++)
{
_currentCellCluster = clusterMatrix[i, j];
clusterMatrix[i, j].CalculatePaths(neighbourMode, OnCellClusterCellViewed);
}
}
levelOneMap.ZeroLevelClusters = clusterMatrix;
return(levelOneMap);
}
public static WeightedGraph <double> GetWeightedGraph(ICellMap map, NeighbourMode neighbourMode)
{
WeightedGraphNode <double>[,] nodes = new WeightedGraphNode <double> [map.Width, map.Height];
for (int x = 0; x < map.Width; x++)
{
for (int y = 0; y < map.Height; y++)
{
if (map.IsPassable(x, y))
{
WeightedGraphNode <double> connectionNode;
WeightedGraphNode <double> currentNode = new WeightedGraphNode <double>(Double.PositiveInfinity);
nodes[x, y] = currentNode;
currentNode.Position = new Vector2Int(x, y);
int prevX = x - 1;
int prevY = y - 1;
int nextY = y + 1;
if (prevX >= 0)
{
connectionNode = nodes[prevX, y];
ConnectNodes(currentNode, connectionNode, 1);
}
if (prevY >= 0)
{
connectionNode = nodes[x, prevY];
ConnectNodes(currentNode, connectionNode, 1);
}
if (prevX >= 0 && prevY >= 0 && neighbourMode == NeighbourMode.SidesAndDiagonals)
{
connectionNode = nodes[prevX, prevY];
ConnectNodes(currentNode, connectionNode, Sqrt2);
}
if (nextY < map.Height && prevX >= 0 && neighbourMode == NeighbourMode.SidesAndDiagonals)
{
connectionNode = nodes[prevX, nextY];
ConnectNodes(currentNode, connectionNode, Sqrt2);
}
}
else
{
nodes[x, y] = null;
}
}
}
WeightedGraph <double> weightedGraph = new WeightedGraph <double>(nodes, Double.PositiveInfinity);
return(weightedGraph);
;
}
public static GraphNode[,] GetGraph(ICellMap map, NeighbourMode neighbourMode)
{
GraphNode[,] nodes = new GraphNode[map.Width, map.Height];
for (int x = 0; x < map.Width; x++)
{
for (int y = 0; y < map.Height; y++)
{
if (map.IsPassable(x, y))
{
GraphNode connectionNode;
GraphNode currentNode = new GraphNode();
nodes[x, y] = currentNode;
currentNode.Position = new Vector2Int(x, y);
int prevX = x - 1;
int prevY = y - 1;
int nextY = y + 1;
if (prevX >= 0)
{
connectionNode = nodes[prevX, y];
ConnectNodes(currentNode, connectionNode);
}
if (prevY >= 0)
{
connectionNode = nodes[x, prevY];
ConnectNodes(currentNode, connectionNode);
}
if (prevX >= 0 && prevY >= 0 && neighbourMode == NeighbourMode.SidesAndDiagonals)
{
connectionNode = nodes[prevX, prevY];
ConnectNodes(currentNode, connectionNode);
}
if (nextY < map.Height && prevX >= 0 && neighbourMode == NeighbourMode.SidesAndDiagonals)
{
connectionNode = nodes[prevX, nextY];
ConnectNodes(currentNode, connectionNode);
}
}
else
{
nodes[x, y] = null;
}
}
}
return(nodes);
}
public List <Tile> GetNeighbours(Tile tile, NeighbourMode neighbourMode = NeighbourMode.Orthogonal)
{
if (!_neighboursRelations.ContainsKey(tile))
{
var x = tile.X;
var y = tile.Z;
var result = new List <Tile>();
if (x - 1 >= 0)
{
result.Add(Tiles[new Vector2Int(x - 1, y)]);
}
if (x + 1 < XResolution)
{
result.Add(Tiles[new Vector2Int(x + 1, y)]);
}
if (y - 1 >= 0)
{
result.Add(Tiles[new Vector2Int(x, y - 1)]);
}
if (y + 1 < ZResolution)
{
result.Add(Tiles[new Vector2Int(x, y + 1)]);
}
if (neighbourMode == NeighbourMode.All)
{
if (x - 1 >= 0 && y - 1 >= 0)
{
result.Add(Tiles[new Vector2Int(x - 1, y - 1)]);
}
if (x + 1 < XResolution && y + 1 < ZResolution)
{
result.Add(Tiles[new Vector2Int(x + 1, y + 1)]);
}
if (x + 1 < XResolution && y - 1 >= 0)
{
result.Add(Tiles[new Vector2Int(x + 1, y - 1)]);
}
if (x - 1 >= 0 && y + 1 < ZResolution)
{
result.Add(Tiles[new Vector2Int(x - 1, y + 1)]);
}
}
_neighboursRelations[tile] = result;
}
return(_neighboursRelations[tile]);;
}
public void ConnectNode(IWeightedGraphNode <double> node, NeighbourMode neighbourMode)
{
AStarAlgorithm aStarAlgorithm = new AStarAlgorithm();
CoordinateTransformer transformer = new CoordinateTransformer(this, LeftBottom);
foreach (var transitionNode in _transitionNodes)
{
IList <Vector2Int> path = aStarAlgorithm.GetPath(transformer, node.Position - LeftBottom,
transitionNode.Position - LeftBottom, neighbourMode);
if (path != null)
{
node.SetWeight(transitionNode, path.Count);
transitionNode.SetWeight(node, path.Count);
// Other mode will not be connected
}
}
}
public static Vector2Int[] GetSteps(NeighbourMode neighbourMode)
{
switch (neighbourMode)
{
case NeighbourMode.SideOnly:
return(new Vector2Int[] { new Vector2Int(-1, 0), new Vector2Int(1, 0), new Vector2Int(0, -1), new Vector2Int(0, 1) });
case NeighbourMode.SidesAndDiagonals:
Vector2Int[] result = new Vector2Int[]
{
new Vector2Int(-1, 0), new Vector2Int(1, 0), new Vector2Int(0, -1), new Vector2Int(0, 1),
new Vector2Int(-1, -1), new Vector2Int(1, 1), new Vector2Int(-1, 1), new Vector2Int(1, -1),
};
return(result);
default:
throw new ArgumentOutOfRangeException(nameof(neighbourMode), neighbourMode, null);
}
}
public IEnumerable <ITile> GetNeighboursOf(ITile tile, NeighbourMode neighbourMode)
{
var result = new List <ITile>();
IPoint tileLocation = GetLocationOf(tile);
if (tileLocation.X - 1 >= 0)
{
result.Add(this[tileLocation.X - 1, tileLocation.Y]);
}
if (tileLocation.X + 1 < Width)
{
result.Add(this[tileLocation.X + 1, tileLocation.Y]);
}
if (tileLocation.Y - 1 >= 0)
{
result.Add(this[tileLocation.X, tileLocation.Y - 1]);
}
if (tileLocation.Y + 1 < Height)
{
result.Add(this[tileLocation.X, tileLocation.Y + 1]);
}
if (neighbourMode == NeighbourMode.ByWallAndCorners)
{
if (tileLocation.X - 1 >= 0 && tileLocation.Y - 1 >= 0)
{
result.Add(this[tileLocation.X - 1, tileLocation.Y - 1]);
}
if (tileLocation.X + 1 < Width && tileLocation.Y + 1 < Height)
{
result.Add(this[tileLocation.X + 1, tileLocation.Y + 1]);
}
if (tileLocation.X - 1 >= 0 && tileLocation.Y + 1 < Height)
{
result.Add(this[tileLocation.X - 1, tileLocation.Y + 1]);
}
if (tileLocation.X + 1 < Width && tileLocation.Y - 1 >= 0)
{
result.Add(this[tileLocation.X + 1, tileLocation.Y - 1]);
}
}
return(result);
}
public void RecalculateObstacles(NeighbourMode neighbourMode = NeighbourMode.SidesAndDiagonals)
{
HierarchicalMapGenerator generator = new HierarchicalMapGenerator();
foreach (var removedObstacle in _removedObstacles)
{
_layeredCellMap.RemoveLayer(removedObstacle);
}
generator.UpdateGraph(_layeredCellMap, _removedObstacles, HierarchicalGraph, neighbourMode);
_removedObstacles.Clear();
foreach (var obstacle in _userObstacles)
{
_layeredCellMap.AddFragment(obstacle);
}
generator.UpdateGraph(_layeredCellMap, _userObstacles, HierarchicalGraph, neighbourMode);
}
public void RecalculateObstacles(NeighbourMode neighbourMode = NeighbourMode.SidesAndDiagonals)
{
throw new NotImplementedException();
}
public void UpdateGraph(ICellMap map, ICellFragment cellFragment, HierarchicalMap graph, NeighbourMode neighbourMode)
{
var clusterMatrix = graph.ZeroLevelClusters;
int clusterWidth = graph.ClusterDefaultWidth;
int clusterHeight = graph.ClusterDefaultHeight;
Vector2Int fragmentSize = new Vector2Int(cellFragment.Width, cellFragment.Height);
Vector2Int leftBottomClusterPosition = GetContainingClusterPosition(clusterMatrix, clusterWidth, clusterHeight, cellFragment.LeftBottom);
Vector2Int rightTopClusterPosition = GetContainingClusterPosition(clusterMatrix, clusterWidth, clusterHeight, cellFragment.LeftBottom + fragmentSize);
int iMin = leftBottomClusterPosition.X;
int iMax = rightTopClusterPosition.X;
int jMin = leftBottomClusterPosition.Y;
int jMax = rightTopClusterPosition.Y;
for (int i = iMin; i <= iMax; i++)
{
for (int j = jMin; j <= jMax; j++)
{
CellCluster currentCluster = clusterMatrix[i, j];
int prevI = i - 1;
int prevJ = j - 1;
int nextJ = j + 1;
if (prevI >= iMin)
{
CellCluster neighbourCluster = clusterMatrix[prevI, j];
RemoveTransitionNodes(currentCluster, neighbourCluster, graph);
ProceedNeighbourClusters(currentCluster, neighbourCluster, graph);
}
if (prevJ >= jMin)
{
CellCluster neighbourCluster = clusterMatrix[i, prevJ];
RemoveTransitionNodes(currentCluster, neighbourCluster, graph);
ProceedNeighbourClusters(currentCluster, neighbourCluster, graph);
}
}
}
for (int i = iMin; i <= iMax; i++)
{
for (int j = jMin; j <= jMax; j++)
{
CellCluster currentCluster = clusterMatrix[i, j];
currentCluster.CalculatePaths(neighbourMode, null);
}
}
}
public void UpdateGraph(ICellMap map, IEnumerable <ICellFragment> obstacles, HierarchicalMap graph, NeighbourMode neighbourMode)
{
foreach (var obstacle in obstacles)
{
UpdateGraph(map, obstacle, graph, neighbourMode);
}
}
public IList <Vector2Int> GetPath(ICellMap map, Vector2Int start, Vector2Int stop, NeighbourMode neighbourMode)
{
if (!_isInitialized)
{
Initialize(map);
}
CellCluster startContainer = HierarchicalMapGenerator.GetContainingCluster(HierarchicalGraph.ZeroLevelClusters, ClusterSizeZero, ClusterSizeZero, start);
CellCluster stopContainer = HierarchicalMapGenerator.GetContainingCluster(HierarchicalGraph.ZeroLevelClusters, ClusterSizeZero, ClusterSizeZero, stop);
HierarchicalGraphNode startNode = new HierarchicalGraphNode();
startNode.Position = start;
startContainer.ConnectNode(startNode, neighbourMode);
startNode.ParentCluster = startContainer;
HierarchicalGraphNode stopNode = new HierarchicalGraphNode();
stopNode.Position = stop;
stopContainer.ConnectNode(stopNode, neighbourMode);
stopNode.ParentCluster = stopContainer;
AStarAlgorithm aStarAlgorithm = new AStarAlgorithm();
_currentCellCluster = null;
aStarAlgorithm.OnCellViewedEvent += OnAStarCellViewed;
IList <IGraphNode> abstractPath = aStarAlgorithm.GetPath(null, startNode, stopNode);
List <Vector2Int> path = null;
if (abstractPath != null)
{
path = new List <Vector2Int>();
for (var i = 0; i < abstractPath.Count - 1; i++)
{
HierarchicalGraphNode nodeA = (HierarchicalGraphNode)abstractPath[i];
HierarchicalGraphNode nodeB = (HierarchicalGraphNode)abstractPath[i + 1];
if (nodeA.ParentCluster == nodeB.ParentCluster)
{
CoordinateTransformer transformer =
new CoordinateTransformer(nodeA.ParentCluster, nodeA.ParentCluster.LeftBottom);
_currentCellCluster = nodeA.ParentCluster;
Vector2Int clusterStart = nodeA.Position - transformer.Transform;
Vector2Int clusterStop = nodeB.Position - transformer.Transform;
IList <Vector2Int> realPath = aStarAlgorithm.GetPathSingleLayer(transformer, clusterStart, clusterStop, neighbourMode);
if (realPath == null)
{
var bitmap = CellMapToBitmap.GetBitmap(nodeA.ParentCluster, 16, clusterStart, clusterStop, null);
LogManager.Log($"Path in cluster not found. Start: {clusterStart}; Stop: {clusterStop}. Bitmap printed");
LogManager.Log(bitmap);
throw new InvalidOperationException();
}
TransformPath(realPath, transformer.Transform);
realPath = Utils.GetInvertedList(realPath);
path.AddRange(realPath);
}
}
}
DestroyConnections(startNode);
DestroyConnections(stopNode);
DestroyData(HierarchicalGraph.ZeroLevelClusters);
return(path);
}
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 void RecalculateObstacles(NeighbourMode neighbourMode = NeighbourMode.SidesAndDiagonals)
{
}
public IList <Vector2Int> GetSmoothedPath(ICellMap map, Vector2Int start, Vector2Int stop, NeighbourMode neighbourMode)
{
if (_layeredCellMap == null)
{
CreateLayeredMap(map);
}
map = _layeredCellMap;
var rawPath = GetPath(_layeredCellMap, start, stop, neighbourMode);
if (rawPath == null)
{
return(null);
}
PathSmoother smoother = new PathSmoother();
var path = smoother.GetSmoothedPath(map, rawPath);
return(path);
}
public IList <Vector2Int> GetPathSingleLayer(ICellMap mapBase, Vector2Int start, Vector2Int stop, NeighbourMode neighbourMode)
{
if (neighbourMode == NeighbourMode.SideOnly)
{
GraphNode[,] graphNodes = GraphGenerator.GetGraph(mapBase, neighbourMode);
IGraphNode[,] nodesInterface = new IGraphNode[mapBase.Width, mapBase.Height];
for (int i = 0; i < mapBase.Width; i++)
{
for (int j = 0; j < mapBase.Height; j++)
{
if (graphNodes[i, j] == null)
{
continue;
}
graphNodes[i, j].Data = new GraphData();
SetPosition(graphNodes[i, j], new Vector2Int(i, j));
nodesInterface[i, j] = graphNodes[i, j];
}
}
IGraphNode startNode = graphNodes[start.X, start.Y];
IGraphNode stopNode = graphNodes[stop.X, stop.Y];
Graph graph = new Graph(nodesInterface);
var nodePath = GetPath(graph, startNode, stopNode);
if (nodePath == null)
{
return(null);
}
List <Vector2Int> path = new List <Vector2Int>(nodePath.Count);
foreach (var node in nodePath)
{
path.Add(node.Position);
}
return(path);
}
else
{
IWeightedGraph <double> graph = GraphGenerator.GetWeightedGraph(mapBase, NeighbourMode.SidesAndDiagonals);
var nodes = graph.GetWeightedGraphNodes();
IWeightedGraphNode <double> startNode = null, stopNode = null;
foreach (var node in nodes)
{
if (node == null)
{
continue;
}
node.Data = new GraphData();
if (node.Position == start)
{
startNode = node;
}
if (node.Position == stop)
{
stopNode = node;
}
}
var nodePath = GetPath(graph, startNode, stopNode);
if (nodePath == null)
{
return(null);
}
List <Vector2Int> path = new List <Vector2Int>(nodePath.Count);
foreach (var node in nodePath)
{
path.Add(node.Position);
}
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);
}
public IList <Vector2Int> GetPath(ICellMap mapBase, Vector2Int start, Vector2Int stop, NeighbourMode neighbourMode)
{
if (_layeredCellMap == null)
{
_layeredCellMap = new LayeredCellMap(mapBase);
}
bool StartNodeFilter(IWeightedGraphNode <double> node)
{
return(node != null && node.Position == start);
}
bool StopNodeFilter(IWeightedGraphNode <double> node)
{
return(node != null && node.Position == stop);
}
IWeightedGraph <double> graph = GraphGenerator.GetWeightedGraph(_layeredCellMap, neighbourMode);
var nodes = graph.GetWeightedGraphNodes();
IWeightedGraphNode <double>[] keyNodes = Utils.GetItemsByFilters(nodes, StartNodeFilter, StopNodeFilter);
var startNode = keyNodes[0];
var stopNode = keyNodes[1];
foreach (var node in nodes)
{
if (node == null)
{
continue;
}
node.Data = new GraphData()
{
DistanceToStop = GetDistance(node.Position, stop)
};
}
var nodePath = GetPath(graph, startNode, stopNode);
if (nodePath == null)
{
return(null);
}
List <Vector2Int> path = new List <Vector2Int>(nodePath.Count);
foreach (var node in nodePath)
{
path.Add(node.Position);
}
return(path);
}
public void GetGraphTest()
{
int width = _rnd.Next(10, 20);
int height = _rnd.Next(10, 20);
NeighbourMode neighbourMode = NeighbourMode.SideOnly;
MemoryCellFragment cellFragment = new MemoryCellFragment(width, height);
FillRandom(cellFragment);
GraphNode[,] nodeMatrix = GraphGenerator.GetGraph(cellFragment, neighbourMode);
if (nodeMatrix.GetLength(0) != width)
{
Assert.Fail("Graph width is incorrect");
}
if (nodeMatrix.GetLength(1) != height)
{
Assert.Fail("Graph height is incorrect");
}
Vector2Int[] steps = Steps.GetSteps(neighbourMode);
for (int i = 0; i < width; i++)
{
for (int j = 0; j < height; j++)
{
GraphNode node = nodeMatrix[i, j];
bool cellPassable = cellFragment[i, j];
if (node == null)
{
if (cellPassable)
{
Assert.Fail($"Cell ({i}, {j}) is passable, but node does not exist");
}
continue;
}
int connectedCellsCount = 0;
foreach (var step in steps)
{
int xShift = i + step.X;
int yShift = j + step.Y;
bool stepInBounds = cellFragment.IsInBounds(xShift, yShift);
bool stepPassable = stepInBounds && cellFragment.IsPassable(xShift, yShift);
Debug.WriteLine($"Position: {node.Position}, Step: {step}, Neighbour: {node.Position + step}");
GraphNode connection = GetConnection(node, step);
if (stepPassable)
{
connectedCellsCount++;
if (connection == null)
{
Assert.Fail($"Node {node.Position} is not connected to position ({xShift}, {yShift}), that is passable");
}
}
else
{
if (connection != null)
{
Assert.Fail($"Node {node.Position} is connected to position ({xShift}, {yShift}), that is not passable");
}
}
}
if (node.Connections.Count != connectedCellsCount)
{
Assert.Fail("Number of connected nodes is incorrect");
}
}
}
}
