/// <summary>
/// Finds a path from own position to given position in maze.
/// </summary>
/// <returns>The path.</returns>
/// <param name="map">Map.</param>
/// <param name="start">Start.</param>
/// <param name="end">End.</param>
/// <param name="solution">Solution.</param>
/// <param name="allowDiagonals">If set to <c>true</c> allow diagonals.</param>
public AStarSearch.SearchState FindPath(Vec3di end, List <Vec3di> solution, bool allowDiagonals)
{
solution.Clear();
AStarSearch astarsearch = new AStarSearch(map, ownPosition, end, allowDiagonals);
AStarSearch.SearchState SearchState;
do
{
SearchState = astarsearch.SearchStep();
}while(SearchState == AStarSearch.SearchState.SEARCHING);
if (SearchState == AStarSearch.SearchState.SUCCEEDED)
{
GridPosition mapPos = astarsearch.GetSolutionStart();
while (mapPos != null)
{
solution.Add(mapPos.Position);
mapPos = astarsearch.GetSolutionNext();
}
;
// Once you're done with the solution you can free the nodes up
astarsearch.Cleanup();
}
else if (SearchState == AStarSearch.SearchState.FAILED)
{
//Zentronic.Debug.LogError("search terminated. Goal state not found!");
}
return(SearchState);
}
public AStarSearch(Grid3di map, Vec3di start, Vec3di goal, bool AllowDiagonal)
{
searchState = SearchState.NOT_INITIALISED;
curSolutionNode = null;
cancelRequested = false;
SetStartAndGoalStates(map, start, goal);
allowDiagonal = AllowDiagonal;
}
public Cell(Vec3di position)
{
this.position = position;
this.left = true;
this.right = true;
this.upper = true;
this.lower = true;
}
public int GetAt(Vec3di pos)
{
if (pos.x < 0 || pos.x >= _width || pos.y < 0 || pos.y >= _height || pos.z < 0 || pos.z >= _depth)
{
return(_borderCode);
}
return(_data[pos.x + pos.y * _width + pos.z * _width * _height]);
}
private List <Cell> FindIntactNeighbours(Vec3di pos)
{
List <Cell> neighbours = new List <Cell>();
// test left neighbour
if (pos.x > 0)
{
Cell leftNeighbour = GetAt(pos.x - 1, pos.y);
if (leftNeighbour.IsIntact())
{
neighbours.Add(leftNeighbour);
}
}
// test right neighbour
if (pos.x < (w - 1))
{
Cell rightNeighbour = GetAt(pos.x + 1, pos.y);
if (rightNeighbour.IsIntact())
{
neighbours.Add(rightNeighbour);
}
}
// above neighbour
if (pos.y > 0)
{
Cell upperNeighbour = GetAt(pos.x, pos.y - 1);
if (upperNeighbour.IsIntact())
{
neighbours.Add(upperNeighbour);
}
}
// below neighbour
if (pos.y < (h - 1))
{
Cell lowerNeighbour = GetAt(pos.x, pos.y + 1);
if (lowerNeighbour.IsIntact())
{
neighbours.Add(lowerNeighbour);
}
}
return(neighbours);
}
// Set Start and goal states
void SetStartAndGoalStates(Grid3di map, Vec3di start, Vec3di goal)
{
cancelRequested = false;
startNode = new SearchNode(new GridPosition(map, start.x, start.y, start.z));
endNode = new SearchNode(new GridPosition(map, goal.x, goal.y, goal.z));
searchState = SearchState.SEARCHING;
// Initialise the AStar specific parts of the Start Node
// The user only needs fill out the state information
startNode.g = 0;
startNode.h = startNode.position.EstimatedDistanceToNode(endNode.position);
startNode.f = startNode.g + startNode.h;
startNode.prevNode = null;
openNodes.Clear();
// Push the start node on the Open list
openNodes.Add(startNode); // heap now unsorted
// Initialise counter for search steps
searchSteps = 0;
}
private Cell FindRandomIntactNeighbour(Vec3di pos, float p1, float p2, float p3, float p4)
{
Cell leftNeighbour = null;
Cell rightNeighbour = null;
Cell upperNeighbour = null;
Cell lowerNeighbour = null;
List <Cell> neighbours = new List <Cell>();
// test left neighbour
if (pos.x > 0)
{
leftNeighbour = GetAt(pos.x - 1, pos.y);
if (!leftNeighbour.IsIntact())
{
leftNeighbour = null;
}
}
// test right neighbour
if (pos.x < (w - 1))
{
rightNeighbour = GetAt(pos.x + 1, pos.y);
if (!rightNeighbour.IsIntact())
{
rightNeighbour = null;
}
}
// above neighbour
if (pos.y > 0)
{
upperNeighbour = GetAt(pos.x, pos.y - 1);
if (!upperNeighbour.IsIntact())
{
upperNeighbour = null;
}
}
// below neighbour
if (pos.y < (h - 1))
{
lowerNeighbour = GetAt(pos.x, pos.y + 1);
if (!lowerNeighbour.IsIntact())
{
lowerNeighbour = null;
}
}
// draw random between 0 and 1
float r = UnityEngine.Random.insideUnitCircle.magnitude;
if (r < p1 && leftNeighbour != null)
{
return(leftNeighbour);
}
else if (r < p2 && rightNeighbour != null)
{
return(rightNeighbour);
}
else if (r < p3 && upperNeighbour != null)
{
return(upperNeighbour);
}
else if (r < p4 && lowerNeighbour != null)
{
return(lowerNeighbour);
}
// we could not deliver a prefered direction, so just return a random neighbour
if (leftNeighbour != null)
{
neighbours.Add(leftNeighbour);
}
if (rightNeighbour != null)
{
neighbours.Add(rightNeighbour);
}
if (upperNeighbour != null)
{
neighbours.Add(upperNeighbour);
}
if (lowerNeighbour != null)
{
neighbours.Add(lowerNeighbour);
}
if (neighbours.Count > 0)
{
return(neighbours[UnityEngine.Random.Range(0, neighbours.Count)]);
}
return(null);
}
public Cell GetAt(Vec3di p)
{
return(arr[p.x + p.y * w]);
}
public bool CompareXY(Vec3di pos)
{
return(x == pos.x && y == pos.y);
}
public bool Compare(Vec3di pos)
{
return(x == pos.x && y == pos.y && z == pos.z);
}
public Vec3di(Vec3di P)
{
x = P.x;
y = P.y;
z = P.z;
}
// This generates the successors to the given Node. It uses a helper function called
// AddSuccessor to give the successors to the AStar class. The A* specific initialisation
// is done for each node internally, so here you just set the state information that
// is specific to the application
public void FindSuccessors(AStarSearch curSearch, GridPosition parent_node, bool allowDiagonal)
{
Vec3di pos_parent = new Vec3di(-1, -1, -1);
if (parent_node != null)
{
pos_parent = parent_node.pos;
}
Vec3di PosUpLeft = new Vec3di(pos.x - 1, pos.y + 1, pos.z);
Vec3di PosUpRight = new Vec3di(pos.x + 1, pos.y + 1, pos.z);
Vec3di PosDownLeft = new Vec3di(pos.x - 1, pos.y - 1, pos.z);
Vec3di PosDownRight = new Vec3di(pos.x + 1, pos.y - 1, pos.z);
Vec3di PosUp = new Vec3di(pos.x, pos.y + 1, pos.z);
Vec3di PosLeft = new Vec3di(pos.x - 1, pos.y, pos.z);
Vec3di PosRight = new Vec3di(pos.x + 1, pos.y, pos.z);
Vec3di PosDown = new Vec3di(pos.x, pos.y - 1, pos.z);
Vec3di PosAbove = new Vec3di(pos.x, pos.y, pos.z + 1);
Vec3di PosBelow = new Vec3di(pos.x, pos.y, pos.z - 1);
if (allowDiagonal)
{
if ((grid.GetAt(PosUpLeft) != grid.BorderCode) && PosUpLeft.Compare(pos_parent) == false)
{
// we have pos UpLeft, now check if at lest Up or left are maze
if (grid.GetAt(PosLeft) != grid.BorderCode || grid.GetAt(PosUp) != grid.BorderCode)
{
curSearch.AddSuccessor(new GridPosition(grid, PosUpLeft));
}
}
if ((grid.GetAt(PosUpRight) != grid.BorderCode) && PosUpRight.Compare(pos_parent) == false)
{
// we have pos UpLeft, now check if at lest Up or left are maze
if (grid.GetAt(PosRight) != grid.BorderCode || grid.GetAt(PosUp) != grid.BorderCode)
{
curSearch.AddSuccessor(new GridPosition(grid, PosUpRight));
}
}
if ((grid.GetAt(PosDownLeft) != grid.BorderCode) && PosDownLeft.Compare(pos_parent) == false)
{
// we have pos UpLeft, now check if at lest Up or left are maze
if (grid.GetAt(PosLeft) != grid.BorderCode || grid.GetAt(PosDown) != grid.BorderCode)
{
curSearch.AddSuccessor(new GridPosition(grid, PosDownLeft));
}
}
if ((grid.GetAt(PosDownRight) != grid.BorderCode) && PosDownRight.Compare(pos_parent) == false)
{
// we have pos UpLeft, now check if at lest Up or left are maze
if (grid.GetAt(PosRight) != grid.BorderCode || grid.GetAt(PosDown) != grid.BorderCode)
{
curSearch.AddSuccessor(new GridPosition(grid, PosDownRight));
}
}
}
// push each possible move except allowing the search to go backwards
if ((grid.GetAt(PosLeft) != grid.BorderCode) && PosLeft.Compare(pos_parent) == false)
{
curSearch.AddSuccessor(new GridPosition(grid, PosLeft));
}
if ((grid.GetAt(PosDown) != grid.BorderCode) && PosDown.Compare(pos_parent) == false)
{
curSearch.AddSuccessor(new GridPosition(grid, PosDown));
}
if ((grid.GetAt(PosRight) != grid.BorderCode) && PosRight.Compare(pos_parent) == false)
{
curSearch.AddSuccessor(new GridPosition(grid, PosRight));
}
if ((grid.GetAt(PosUp) != grid.BorderCode) && PosUp.Compare(pos_parent) == false)
{
curSearch.AddSuccessor(new GridPosition(grid, PosUp));
}
if ((grid.GetAt(PosAbove) != grid.BorderCode) && PosAbove.Compare(pos_parent) == false)
{
curSearch.AddSuccessor(new GridPosition(grid, PosAbove));
}
if ((grid.GetAt(PosBelow) != grid.BorderCode) && PosBelow.Compare(pos_parent) == false)
{
curSearch.AddSuccessor(new GridPosition(grid, PosBelow));
}
}
public GridPosition(Grid3di Map, Vec3di p)
{
grid = Map;
pos = p;
}
public GridPosition(Grid3di Map, int px, int py, int pz)
{
grid = Map;
pos = new Vec3di(px, py, pz);
}
public GridPosition(Grid3di Map)
{
grid = Map;
pos = new Vec3di();
}