private bool SuitablePositionFound() { for (int i = 0; i < findPosTries; i++) { float newX = Random.Range(-0.5f * _grid.GridWorldSize.x, +0.5f * _grid.GridWorldSize.x); float newY = Random.Range(-0.5f * _grid.GridWorldSize.y, +0.5f * _grid.GridWorldSize.y); _newWorldPos = new Vector3(newX, 1, newY); if (_grid.NodeFromWorldInput(_newWorldPos).Walkable&& !ObstacleInArea(_newWorldPos)) { return(true); } } return(false); }
IEnumerator FindPath(Vector3 startPos, Vector3 targetPos) { Vector3[] waypoints = new Vector3[0]; bool pathSuccess = false; Node startNode = _grid.NodeFromWorldInput(startPos); Node targetNode = _grid.NodeFromWorldInput(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) { pathSuccess = true; break; } foreach (Node neighbour in _grid.GetNeighbours(currentNode)) { if (!neighbour.Walkable || closedSet.Contains(neighbour)) { continue; } int newMoveCostToNeighbour = currentNode.GCost + GetDistance(currentNode, neighbour); if (newMoveCostToNeighbour < neighbour.GCost || !openSet.Contains(neighbour)) { neighbour.GCost = newMoveCostToNeighbour; neighbour.HCost = GetDistance(neighbour, targetNode); neighbour.Parent = currentNode; if (!openSet.Contains(neighbour)) { openSet.Add(neighbour); } else { openSet.UpdateItem(neighbour); } } } } } yield return(null); if (pathSuccess) { waypoints = RetracePath(startNode, targetNode); } _requestManager.FinishedProcessingPath(waypoints, pathSuccess); }