public bool FindPath(Vector3 startPos, Vector3 targetPos, Orientation seekerOrientation, Orientation targetOrientation, bool additive = false)
{
Node startNode = grid.NodeFromWorldPoint(startPos);
Node targetNode = grid.NodeFromWorldPoint(targetPos);
Heap <Node> openSet = new Heap <Node>(grid.MaxSize);
HashSet <Node> closedSet = new HashSet <Node>();
openSet.Add(startNode);
if (targetOrientation != seekerOrientation)
{
for (int i = 0; i < grid.slopes.Length; i++)
{
Slope currentSlope = grid.slopes[i];
if (currentSlope.downOrientation == targetOrientation && currentSlope.faceOrientation == seekerOrientation ||
currentSlope.downOrientation == seekerOrientation && currentSlope.faceOrientation == targetOrientation)
{
if (FindPath(startPos, currentSlope.transform.position + grid.targetOrientation.Inverse().ToVector(), grid.targetOrientation, grid.targetOrientation))
{
print("found path!");
}
else
{
print("did not find path...");
}
}
}
return(false);
}
while (openSet.Count > 0)
{
Node currentNode = openSet.RemoveFirst();
closedSet.Add(currentNode);
if (currentNode == targetNode)
{
//print("path found");
RetracePath(startNode, targetNode);
return(true);
}
foreach (Node neighbour in grid.GetValidNeighbours(currentNode, grid.targetOrientation))
{
if (closedSet.Contains(neighbour))
{
continue;
}
if (!neighbour.walkable[(int)grid.targetOrientation])
{
continue;
}
int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour);
if (newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour))
{
neighbour.gCost = newMovementCostToNeighbour;
neighbour.hCost = GetDistance(neighbour, targetNode);
neighbour.parent = currentNode;
if (!openSet.Contains(neighbour))
{
openSet.Add(neighbour);
}
else
{
openSet.UpdateItem(neighbour);
}
}
}
}
return(false);
}
