void CreateGrid() { grid = new Node2D[gridSizeX, gridSizeY]; Vector3 worldBottomLeft = transform.position - Vector3.right * gridWorldSize.x / 2 - Vector3.up * gridWorldSize.y / 2; for (int x = 0; x < gridSizeX; x++) { for (int y = 0; y < gridSizeY; y++) { Vector3 worldPoint = worldBottomLeft + Vector3.right * (x * nodeDiameter + nodeRadius) + Vector3.up * (y * nodeDiameter + nodeRadius); //bool walkable = !(Physics2D.OverlapCircle(worldPoint, nodeRadius, unwalkableMask)); bool walkable = false; int movementPenalty = 0; RaycastHit2D hit = Physics2D.Raycast(worldPoint + Vector3.forward * 50, Vector3.back, 100, walkableMask); if (hit) { walkableRegionsDictionary.TryGetValue(hit.collider.gameObject.layer, out movementPenalty); walkable = true; } Ray ray = new Ray(worldPoint + Vector3.forward * 50, Vector3.back); RaycastHit hit3D; if (Physics.Raycast(ray, out hit3D, 100, walkableMask)) { walkableRegionsDictionary.TryGetValue(hit3D.collider.gameObject.layer, out movementPenalty); walkable = true; } if (!walkable) { movementPenalty += obstacleProximityPenalty; } grid[x, y] = new Node2D(walkable, worldPoint, x, y, movementPenalty); } } BlurPenaltyMap(3); }
public Node2D[] NodesFromWorldBound(Vector3 worldPosition, Vector2 worldBounds) { int nodesX = Mathf.RoundToInt(worldBounds.x / nodeDiameter); nodesX = (nodesX < 1) ? 1 : nodesX; int nodesY = Mathf.RoundToInt(worldBounds.y / nodeDiameter); nodesY = (nodesY < 1) ? 1 : nodesY; Vector3 lowerLeftPos = new Vector3(worldPosition.x - worldBounds.x / 2f, worldPosition.y - worldBounds.y / 2f); Node2D[] nodes = new Node2D[nodesX * nodesY]; int i = 0; for (int x = 0; x < nodesX; x++) { for (int y = 0; y < nodesY; y++) { nodes[i] = NodeFromWorldPoint(lowerLeftPos + Vector3.right * x * nodeDiameter + Vector3.up * y * nodeDiameter); i++; } } return(nodes); }
public void FindPath(PathRequest request, Action <PathResult> callback) { Stopwatch sw = new Stopwatch(); sw.Start(); Vector3[] waypoints = new Vector3[0]; bool pathSuccess = false; Node2D startNode = grid.NodeFromWorldPoint(request.pathStart); var endNodes = grid.NodesFromWorldBound(request.pathEnd, request.endBounds); UnityEngine.Debug.Log("nodes in end node: " + endNodes.Length); Node2D[] targetNode = endNodes.Where(n => n.walkable).ToArray(); UnityEngine.Debug.Log("usable target nodes: " + targetNode.Length); Node2D endNode = startNode; try { endNode = targetNode[0]; startNode.parent = startNode; if (startNode.walkable && targetNode != null && targetNode.Length > 0) { Heap <Node2D> openSet = new Heap <Node2D>(grid.MaxSize); HashSet <Node2D> closedSet = new HashSet <Node2D>(); openSet.Add(startNode); while (openSet.Count > 0) { Node2D currentNode = openSet.RemoveFirst(); closedSet.Add(currentNode); if (targetNode.Contains(currentNode)) { sw.Stop(); UnityEngine.Debug.Log("Path found: " + sw.ElapsedMilliseconds + " ms"); pathSuccess = true; endNode = currentNode; break; } foreach (Node2D 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 || !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); } } } } } } catch (Exception e) { UnityEngine.Debug.Log("No usable end nodes"); callback(new PathResult(waypoints, pathSuccess, request.callback)); } if (pathSuccess) { waypoints = RetracePath(startNode, endNode); pathSuccess = waypoints.Length > 0; } callback(new PathResult(waypoints, pathSuccess, request.callback)); }
int GetDistance(Node2D nodeA, Node2D[] nodesB) { return(nodesB.Min <Node2D>(o => GetDistance(nodeA, o))); }