private void InititializeSearchNodes(BasicCube[,] map)
{
searchNodes = new SearchNode[searchMapWidth, searchMapHeight];
for (int x = 0 ; x < searchMapWidth; x++)
{
for (int y = 0; y < searchMapHeight; y++)
{
SearchNode node = new SearchNode();
node.position = new Point(x,y);
//logic here to detect if face is free
node.isOccupied = checkForObstacles(map[x, y]);
if (node.isOccupied == false)
{
node.neighbours = new SearchNode[4];
searchNodes[x, y] = node;
}
}
}
for (int x = 0; x < searchMapWidth; x++)
{
for (int y = 0; y < searchMapHeight; y++)
{
SearchNode node = searchNodes[x, y];
// if there is no node, or the node is occupied by an obstacle, continue
if (node == null || node.isOccupied == true)
{
continue;
}
// TODO maybe change this to make it work for x,y and z,
// or put that logic outside and work only in x and y here
Point[] neighbours = new Point[]
{
new Point(x, y-1),
new Point(x, y+1),
new Point(x-1, y),
new Point(x+1, y)
};
// loop through neighbours
for (int i = 0; i < neighbours.Length; i++)
{
Point position = neighbours[i];
// make sure neighbour is part of the level
if (position.X < 0 || position.X > searchMapWidth - 1 ||
position.Y < 0 || position.Y > searchMapHeight - 1)
{
continue;
}
SearchNode neighbour = searchNodes[position.X, position.Y];
if (neighbour == null || neighbour.isOccupied == true)
{
continue;
}
node.neighbours[i] = neighbour;
}
}
}
}
private List<Vector3> FindFinalPath(SearchNode startNode, SearchNode goalNode)
{
closedList.Add(goalNode);
SearchNode parentTile = goalNode.parent;
while (parentTile != startNode)
{
closedList.Add(parentTile);
parentTile = parentTile.parent;
}
List<Vector3> finalPath = new List<Vector3>();
//reverse Path + handle transform into world space
// todo maybe not here but outside?
for (int i = closedList.Count - 1; i >= 0; i--)
{
switch (currentOrientation)
{
case SpectrumEnums.Orientation.posX:
case SpectrumEnums.Orientation.negX:
finalPath.Add(new Vector3(startPosX, closedList[i].position.X * 2, closedList[i].position.Y * 2));
break;
case SpectrumEnums.Orientation.posY:
case SpectrumEnums.Orientation.negY:
finalPath.Add(new Vector3(closedList[i].position.X * 2, startPosY, closedList[i].position.Y * 2));
break;
case SpectrumEnums.Orientation.posZ:
case SpectrumEnums.Orientation.negZ:
finalPath.Add(new Vector3(closedList[i].position.X * 2, closedList[i].position.Y * 2, startPosZ));
break;
}
}
foreach (Vector3 path in finalPath)
{
Console.WriteLine(path.ToString());
}
ResetSearchNodes();
return finalPath;
}