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; }