IEnumerator FindPath(Vector3 startPos, Vector3 targetPos)
{
Vector3[] wayPoints = new Vector3[0];
bool pathSuccess = false;
Node startNode = grid.GetNodeFromWorld(startPos);
Node targetNode = grid.GetNodeFromWorld(targetPos);
//Debug.Log(startNode.walkable);
if (true) //startNode.walkable targetNode.walkable
{
Heap <Node> openSet = new Heap <Node>(grid.MaxSize);
HashSet <Node> closedSet = new HashSet <Node>();
openSet.Add(startNode);
while (openSet.Count > 0)
{
Node currentNode = openSet.RemoveFirst();
//for (int i = 1; i < openSet.Count; i++) {
// if (currentNode.fCost > openSet[i].fCost || (currentNode.fCost == openSet[i].fCost &&
// currentNode.hCost > openSet[i].hCost))
// {
// currentNode = openSet[i];
// }
//}
//openSet.Remove(currentNode);
closedSet.Add(currentNode);
if (currentNode == targetNode)
{
//Debug.Log("find");
pathSuccess = true;
break;
}
foreach (Node neighbor in grid.GetNeighbors(currentNode))
{
if (!neighbor.walkable || closedSet.Contains(neighbor))
{
continue;
}
int newCostToNeighbor = currentNode.gCost + GetDistance(currentNode, neighbor) + neighbor.movementPenalty;
if (newCostToNeighbor < neighbor.gCost || !openSet.Contains(neighbor))
{
neighbor.gCost = newCostToNeighbor;
neighbor.hCost = GetDistance(neighbor, targetNode);
neighbor.parent = currentNode;
if (!openSet.Contains(neighbor))
{
openSet.Add(neighbor);
}
else
{
openSet.UpdateItem(neighbor);
}
}
}
}
}
yield return(null);
if (pathSuccess)
{
wayPoints = RetracePath(startNode, targetNode);
}
pathRequestManager.FinishProcessingPath(wayPoints, pathSuccess);
}
IEnumerator FindPathHeap(Vector3 startPos, Vector3 targetPos)
{
Stopwatch sw = new Stopwatch();
sw.Start();
Vector3[] waypoints = new Vector3[0];
bool pathSuccess = false;
Node startNode = grid.NodeFromWorldPoint(startPos);
Node targetNode = grid.NodeFromWorldPoint(targetPos);
if (startNode.walkable && targetNode.walkable)
{
Heap <Node> openSet = new Heap <Node>(grid.MaxSize);
HashSet <Node> closedSet = new HashSet <Node>();
openSet.Add(startNode);
while (openSet.Count > 0)
{
Node current = openSet.Remove();
closedSet.Add(current);
//if we found the target Node exit from the loop
if (current == targetNode)
{
sw.Stop();
print("Path found: " + sw.ElapsedMilliseconds + "ms. ");
pathSuccess = true;
break;
}
foreach (Node neighbour in grid.GetNeighbours(current))
{
if (!neighbour.walkable || closedSet.Contains(neighbour))
{
continue;
}
int costToNeighbour = current.gCost + GetDistance(current, neighbour);
if (costToNeighbour < neighbour.gCost || !openSet.Contains(neighbour))
{
neighbour.gCost = costToNeighbour;
neighbour.hCost = GetDistance(neighbour, targetNode);
neighbour.parent = current;
if (!openSet.Contains(neighbour))
{
openSet.Add(neighbour);
}
else
{
openSet.UpdateItem(neighbour);
}
}
}
}
yield return(null);
if (pathSuccess)
{
waypoints = RetracePath(startNode, targetNode);
}
requestManager.FinishProcessingPath(waypoints, pathSuccess);
}
}
//-------------------------------------------------------
IEnumerator FindPath(Vector3 startPos, Vector3 targetPos, bool isFollowing)
{
Stopwatch sw = new Stopwatch();
sw.Start();
bool replanReturn = false;
if (!isFollowing)
{
if (init(startPos, targetPos))
{
replanReturn = replan();
}
}
else
{
if (updatedNodes.Count > 0 || updatedObservedNodes.Count > 0 || updatedNodesTrigger.Count > 0)
{
updateKM();
if (updatedNodesTrigger.Count > 0)
{
for (int i = 0; i < updatedNodesTrigger.Count; i++)
{
Node nodeHit = updatedNodesTrigger[i];
update(nodeHit, double.PositiveInfinity);
}
updatedNodesTrigger.Clear();
}
if (updatedNodes.Count > 0)
{
for (int i = 0; i < updatedNodes.Count; i++)
{
Node nodeHit = updatedNodes[i];
update(nodeHit, double.PositiveInfinity);
}
updatedNodes.Clear();
}
if (updatedObservedNodes.Count > 0)
{
for (int i = 0; i < updatedObservedNodes.Count; i++)
{
Node nodeHit = updatedObservedNodes[i];
update(nodeHit, nodeHit.movementPenalty);
}
updatedObservedNodes.Clear();
}
replanReturn = replan();
}
else
{
replanReturn = true;
}
}
yield return(null);
if (replanReturn)
{
if (path.Count > 0)
{
path.RemoveAt(0);
nextMove = path[0].worldPosition;
startNode = path[0];
//path.RemoveAt(0);
}
requestManager.FinishProcessingPath(nextMove, replanReturn, (startNode.eq(targetNode)));
}
}
IEnumerator FindPath(Vector3 startPos, Vector3 targetPos, NodeGrid grid)
{
Vector3[] waypoints = new Vector3[0];
bool pathSucces = false;
if (grid != null)
{
Node startNode = grid.GetNodeFromWorldPoint(startPos);
Node targetNode = grid.GetNodeFromWorldPoint(targetPos);
if (/*startNode.walkable && */ targetNode.walkable)
{
NodeHeap openSet = new NodeHeap(grid.MaxSize);
HashSet <Node> closedSet = new HashSet <Node>();
openSet.Add(startNode);
while (openSet.Count > 0)
{
Node currentNode = openSet.RemoveFirst();
closedSet.Add(currentNode);
if (currentNode == targetNode)
{
pathSucces = true;
break;
}
foreach (Node neighbour in grid.GetNeighbours(currentNode))
{
if (!neighbour.walkable || closedSet.Contains(neighbour))
{
continue;
}
int newCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour);
if (newCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour))
{
neighbour.gCost = newCostToNeighbour;
neighbour.hCost = GetDistance(neighbour, targetNode);
neighbour.parent = currentNode;
if (!openSet.Contains(neighbour))
{
openSet.Add(neighbour);
}
else
{
openSet.UpdateItem(neighbour);
}
}
}
}
}
yield return(null);
if (pathSucces)
{
waypoints = RetracePath(startNode, targetNode, grid);
pathSucces = (waypoints.Length > 0);
}
requestManager.FinishProcessingPath(waypoints, pathSucces);
}
}
public void EndFindPath()
{
StopAllCoroutines();
requestManager.FinishProcessingPath(new Vector2[0], false);
}
IEnumerator FindPath(Vector3 startPos, Vector3 targetPos)
{
Stopwatch sw = new Stopwatch();
sw.Start();
Vector3[] waypoints = new Vector3[0];
bool pathSuccess = false;
Node startNode = grid.NodeFromWorldPoint(startPos);
Node targetNode = grid.NodeFromWorldPoint(targetPos);
if (startNode.walkable && targetNode.walkable)
{
Heap <Node> openSet = new Heap <Node>(grid.MaxSize);
HashSet <Node> closedSet = new HashSet <Node>();
openSet.Add(startNode);
while (openSet.Count > 0)
{
Node currentNode = openSet.RemoveFirst();
closedSet.Add(currentNode);
if (currentNode == targetNode)
{
sw.Stop();
pathSuccess = true;
break;
}
foreach (Node neighbour in grid.GetNeighbours(currentNode))
{
if (Physics.CheckSphere(neighbour.worldPos, grid.nodeRadius * gm.nodeRadiusMultiplyer, gm.inmateMask) && !gm.yardOver)
{
neighbour.movementPenalty = 50;
}
else
{
foreach (Node neighbour2 in grid.GetNeighbours(neighbour))
{
if (neighbour2.movementPenalty > 25)
{
neighbour.movementPenalty = 25;
}
else
{
neighbour.movementPenalty = 0;
}
}
}
if (!neighbour.walkable || closedSet.Contains(neighbour))
{
continue;
}
//the value for the new movement cost travelling to one of the neighbour nodes
int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour) + neighbour.movementPenalty;
//Check if the new path to neighbour is shorter or if the neighbour is not in the open set
if (newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour))
{
//Set fCost of neighbour
neighbour.gCost = newMovementCostToNeighbour;
neighbour.hCost = GetDistance(neighbour, targetNode);
//set parent of neighbour to the node
neighbour.parent = currentNode;
if (!openSet.Contains(neighbour))
{
openSet.Add(neighbour);
}
else
{
openSet.UpdateItem(neighbour);
}
neighbour.movementPenalty = 0;
}
}
}
}
yield return(null);
if (pathSuccess)
{
waypoints = RetracePath(startNode, targetNode);
}
requestManager.FinishProcessingPath(waypoints, pathSuccess);
}
private IEnumerator FindPath(Vector3 startPos, Vector3 targetPos)
{
Stopwatch sw = new Stopwatch();
sw.Start();
Vector3[] waypoints = new Vector3[0];
bool pathSuccess = false;
PathNode startNode = pathGrid.NodeFromWorldPoint(startPos);
PathNode targetNode = pathGrid.NodeFromWorldPoint(targetPos);
if (startNode.Walkable && targetNode.Walkable)
{
PathHeap <PathNode> openSet = new PathHeap <PathNode>(pathGrid.MaxSize);
HashSet <PathNode> closedSet = new HashSet <PathNode>();
openSet.Add(startNode);
while (openSet.Count > 0)
{
PathNode currentNode = openSet.RemoveFirstItem();
closedSet.Add(currentNode);
if (currentNode == targetNode)
{
sw.Stop();
print("Path found: " + sw.ElapsedMilliseconds + " ms");
pathSuccess = true;
//RetracePath(startNode, targetNode);
//return;
break;
}
foreach (PathNode neighbour in pathGrid.GetNeighbour(currentNode))
{
if (neighbour == null)
{
continue;
}
if (!neighbour.Walkable || closedSet.Contains(neighbour))
{
continue;
}
int newMovementCostToNeighbour = currentNode.GCost + GetDistance(currentNode, neighbour) + neighbour.Hindrance;
if (newMovementCostToNeighbour < neighbour.GCost || openSet.Contains(neighbour) == false)
{
neighbour.GCost = newMovementCostToNeighbour;
neighbour.HCost = GetDistance(neighbour, targetNode);
neighbour.Parent = currentNode;
if (openSet.Contains(neighbour) == false)
{
openSet.Add(neighbour);
}
else
{
openSet.UpdateItem(neighbour);
}
}
}
}
// List<PathNode> tempList = openSet;
// HashSet<PathNode> tempList2 = closedSet;
}
yield return(null);
if (pathSuccess)
{
waypoints = RetracePath(startNode, targetNode);
}
requestManager.FinishProcessingPath(waypoints, pathSuccess);
}
IEnumerator FindPath(Vector3 startPos, Vector3 targetPos)
{
Vector3[] waypoints = new Vector3[0];
bool pathSuccess = false;
Node startNode = grid.NodeFromWorldPoint(startPos);
Node targetNode = grid.NodeFromWorldPoint(targetPos);
startNode.parent = startNode;
Debug.Log("" + startNode.walkable + targetNode.walkable + targetNode.worldPosition);
if (startNode.walkable && targetNode.walkable)
{
Heap <Node> openHeap = new Heap <Node>(grid.MaxSize);
HashSet <Node> closedSet = new HashSet <Node>();
openHeap.Add(startNode);
while (openHeap.Count > 0)
{
Node currentNode = openHeap.RemoveFirst();
closedSet.Add(currentNode);
if (currentNode == targetNode)
{
pathSuccess = true;
break;
}
foreach (Node neighbour in grid.GetNeighbours(currentNode))
{
if (!neighbour.walkable || closedSet.Contains(neighbour))
{
continue;
}
int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour) + neighbour.movementPenalty;
if (newMovementCostToNeighbour < neighbour.gCost || !openHeap.Contains(neighbour))
{
neighbour.gCost = newMovementCostToNeighbour;
neighbour.hCost = GetDistance(neighbour, targetNode);
neighbour.parent = currentNode;
if (!openHeap.Contains(neighbour))
{
openHeap.Add(neighbour);
}
else
{
openHeap.UpdateItem(neighbour);
}
}
}
}
}
yield return(null);
if (pathSuccess)
{
waypoints = RetracePath(startNode, targetNode);
}
requestManager.FinishProcessingPath(waypoints, pathSuccess);
}