public override void OnInspectorGUI() { DrawDefaultInspector(); _surface = target as NavSurface; if (GUILayout.Button("Data Records")) { Undo.RecordObject(_surface, "Data Records"); _surface.DataRecords(); EditorUtility.SetDirty(_surface); } if (GUILayout.Button("Creating Edge")) { Undo.RecordObject(_surface, "Creating Edge"); _surface.CreatingEdge(); EditorUtility.SetDirty(_surface); } if (GUILayout.Button("Algorithm Dijkstra")) { Undo.RecordObject(_surface, "Algorithm Dijkstra"); _surface.AlgorithmDijkstra(); EditorUtility.SetDirty(_surface); } if (GUILayout.Button("Check")) { Undo.RecordObject(_surface, "Check"); _surface.chekc(); EditorUtility.SetDirty(_surface); } }
public NodeRecord(NavSurface n, Connection connect, float cost) { node = n; fromNodeInPath = null; connection = connect; costSoFar = cost; }
void Start() { foreach (Vector3Int pos in tilemap.cellBounds.allPositionsWithin) { NavSurface nav = ScriptableObject.CreateInstance <NavSurface>(); Vector3Int localPlace = new Vector3Int(pos.x, pos.y, pos.z); if (tilemap.HasTile(localPlace)) { SmartTile tile = tilemap.GetTile <SmartTile>(pos); nav.isNavigable = tile.nav.isNavigable; nav.cost = tile.nav.cost; nav.isCover = tile.nav.isCover; nav.coverLocations = new List <Vector3Int>(); nav.coverValue = tile.nav.coverValue; navDict.Add(pos, nav); } } foreach (Vector3Int pos in tilemap.cellBounds.allPositionsWithin) { NavSurface nav; navDict.TryGetValue(pos, out nav); if (nav.isCover) { Debug.Log("Is Cover."); for (int i = -1; i <= 1; i++) { if (i == 0) { continue; } Vector3Int positionX = new Vector3Int(pos.x + i, pos.y, pos.z); Vector3Int positionY = new Vector3Int(pos.x, pos.y + i, pos.z); if (HasSmartTile(tilemap, positionX)) { if (tilemap.GetTile <SmartTile>(positionX).nav.isNavigable) { nav.coverLocations.Add(positionX); Debug.Log("Cover Added: " + positionX); } } if (HasSmartTile(tilemap, positionY)) { if (tilemap.GetTile <SmartTile>(positionY).nav.isNavigable) { nav.coverLocations.Add(positionY); Debug.Log("Cover Added: " + positionY); } } } navDict.Remove(pos); navDict.Add(pos, nav); } } Debug.Log("Done defining nav locations."); }
public void GetPath(Vector3 from, Vector3 to, NavSurface targetSurface, PathCallback callback) { StopAllCoroutines(); if (untilNextSearch) { _final_path = null; } _surface = targetSurface; bool[] nodes = targetSurface.Nodes; Vector2 size = targetSurface.Size; Vector2 start = targetSurface.ClosestNodeToPoint(from); Vector2 end = targetSurface.ClosestNodeToPoint(to); _target_node = end; if (!targetSurface.NodesInConectedZones(start, end)) { callback(null); Debug.Log("Cannot path between unconected zones."); return; } if (g == null) { g = new float[(int)(size.x * size.y)]; h = new float[(int)(size.x * size.y)]; f = new float[(int)(size.x * size.y)]; cameFrom = new Vector2[(int)(size.x * size.y)]; openList = new List <Vector2>(); closedList = new List <Vector2>(); } else if (g.Length < size.x * size.y) { g = new float[(int)(size.x * size.y)]; h = new float[(int)(size.x * size.y)]; f = new float[(int)(size.x * size.y)]; cameFrom = new Vector2[(int)(size.x * size.y)]; } openList.Clear(); closedList.Clear(); for (int i = 0; i < g.Length; i++) { g[i] = float.MaxValue; h[i] = float.MaxValue; f[i] = float.MaxValue; cameFrom[i] = Vector2.down; } StartCoroutine(AStar(start, end, size, nodes, targetSurface, callback)); }
private void Update() { // DRAW stuff if (drawConnections) { foreach (NavSurface node in graph.nodes) { if (node.isNavigable && node.connections != null) { foreach (Connection edge in node.connections) { Debug.DrawLine(graph.grid.GetCellCenterWorld(node.gridPos), graph.grid.GetCellCenterWorld(edge.toNode.gridPos), Color.red); } } } } if (drawPath) { if (path != null) { foreach (Connection connection in path.connections) { Debug.DrawLine(graph.grid.GetCellCenterWorld(connection.fromNode.gridPos), graph.grid.GetCellCenterWorld(connection.toNode.gridPos), Color.blue); } } } // INTERACTABLE stuff if (Input.GetMouseButtonDown(0)) { Ray ray = Camera.main.ScreenPointToRay(Input.mousePosition); Vector3 worldPoint = ray.GetPoint(-ray.origin.z / ray.direction.z); Vector3Int position = graph.grid.WorldToCell(worldPoint); startNode = graph.nodes.Find(x => x.gridPos == position); } if (Input.GetMouseButtonDown(1)) { Ray ray = Camera.main.ScreenPointToRay(Input.mousePosition); Vector3 worldPoint = ray.GetPoint(-ray.origin.z / ray.direction.z); Vector3Int position = graph.grid.WorldToCell(worldPoint); goalNode = graph.nodes.Find(x => x.gridPos == position); } }
void Update() { if (Input.GetButtonDown("Fire2")) { RaycastHit hit; Ray ray = Camera.main.ScreenPointToRay(Input.mousePosition); if (Physics.Raycast(ray, out hit)) { NavSurface target = hit.transform.GetComponent <NavSurface>(); if (!target) { return; } _motor.Stop(); PathFinder.Instance.GetPath(transform.position, hit.point, target, HandleReturnedPath); } } }
public override void OnInspectorGUI() { DrawDefaultInspector(); NavSurface nav_surface = (NavSurface)target; GUILayout.BeginHorizontal(); if (GUILayout.Button("Bake Navigation Nodes")) { nav_surface.BakeNodes(); } if (GUILayout.Button("Clear Navigation Nodes")) { nav_surface.ClearNodes(); } GUILayout.EndHorizontal(); }
Vector3[] ReconstructPath(Vector2 current, int width, NavSurface surface) { // total_path:= [current] List <Vector3> finalPath = new List <Vector3>(); while (cameFrom[Index(current, width)] != Vector2.down) { finalPath.Add(surface.NodeWorldPos(current)); current = cameFrom[Index(current, width)]; } // Path is now from end to start so finalPath.Reverse(); if (showFinalPath) { _final_path = finalPath; } // return final_path return(finalPath.ToArray()); }
public void CalculateGraph() { lastUpdate = System.DateTime.Now.ToString(); if (nodes == null) { nodes = new List <NavSurface>(); } // Currently the graph is cleared on every update // Is tied directly into the loop, hence having to loop through all tiles everytime we update // #TODO Need to have it so such that only the edited nodes need to clear if (nodes != null) { if (nodes.Count > 0) { foreach (NavSurface node in nodes) { ScriptableObject.DestroyImmediate(node); } nodes.Clear(); } } foreach (Vector3Int pos in tilemap.cellBounds.allPositionsWithin) { // Spawn a new node for each tile position Vector3Int localPlace = new Vector3Int(pos.x, pos.y, pos.z); if (tilemap.HasTile(localPlace)) { NavSurface nav = ScriptableObject.CreateInstance <NavSurface>(); SmartTile tile = tilemap.GetTile <SmartTile>(pos); nav.gridPos = pos; nav.isNavigable = tile.nav.isNavigable; nav.cost = tile.nav.cost; nav.isCover = tile.nav.isCover; nav.coverLocations = new List <Vector3Int>(); nav.coverValue = tile.nav.coverValue; // Add the new node to the nodes list nodes.Add(nav); } } // Calculating connections for all nodes foreach (NavSurface node in nodes) { if (node.isNavigable) { for (int i = -1; i <= 1; i++) { for (int j = -1; j <= 1; j++) { if (i == 0 && j == 0) { continue; } int isDiagonal = Mathf.Abs(i) * Mathf.Abs(j); Vector3Int neighbourPos = new Vector3Int(node.gridPos.x + i, node.gridPos.y + j, node.gridPos.z); if (HasSmartTile(tilemap, neighbourPos)) { //int index = nodeIndex.FindLastIndex(pos); NavSurface neighbourNode = nodes.Find(x => x.gridPos == neighbourPos); if (neighbourNode.isNavigable) { Connection connection = ScriptableObject.CreateInstance <Connection>(); connection.fromNode = node; connection.toNode = neighbourNode; if (isDiagonal == 1) { connection.cost = connection.toNode.cost * 2; } else { connection.cost = connection.toNode.cost; } node.connections.Add(connection); } } } } } } }
public Path PathfindDijkstra(Graph graph, NavSurface startNode, NavSurface endNode) { // Init start node NodeRecord startRecord = new NodeRecord(startNode, null, 0.0f); // Init open and close lists List<NodeRecord> openNodes = new List<NodeRecord>(); List<NodeRecord> closedNodes = new List<NodeRecord>(); // Add startRecord to open list openNodes.Add(startRecord); NodeRecord current = new NodeRecord(null, null, 0.0f); while (openNodes.Count > 0) { // Find smallest element in the open list current = FindSmallestElement(openNodes); // Could not find any node to work with if (current.node == null) break; // If current node is goal node, terminate if (current.node == endNode) break; // Main Search Loop ~~ NavSurface nextNode; float nextNodeCost = 0.0f; foreach (Connection connection in current.node.connections) { // Update total cost estimate nextNode = connection.toNode; nextNodeCost += connection.cost; // Skip to next node if this one is already closed if (closedNodes.Exists(x => x.node == nextNode)) continue; NodeRecord recordedNode; bool nodeAlreadyExists = false; // Check if we already have the node with a worse cost if (openNodes.Exists(x => x.node == nextNode)) { nodeAlreadyExists = true; recordedNode = openNodes.Find(x => x.node == nextNode); if (recordedNode.costSoFar <= nextNodeCost) continue; } else { // Record the node otherwise recordedNode = new NodeRecord { node = nextNode }; } recordedNode.costSoFar = nextNodeCost; recordedNode.connection = connection; recordedNode.fromNodeInPath = current; // Add if node doest exist, is already up to date otherwise if (nodeAlreadyExists == false) openNodes.Add(recordedNode); } // Remove the current node from open list and add to closed openNodes.Remove(current); closedNodes.Add(current); } if (current.node != endNode) return null; // Create a path list and return Path path = new Path(); while (current.node != startNode) { path.connections.Add(current.connection); current = current.fromNodeInPath; } path.connections.Reverse(); return path; }
IEnumerator AStar(Vector2 start, Vector2 end, Vector2 size, bool[] nodes, NavSurface surface, PathCallback callback) { bool pathFound = false; // Initially, only the start node is known. openList.Add(start); // The cost of going from start to start is zero. g[Index(start, size.x)] = 0; // For the first node, the final and heuristic cost is the same. h[Index(start, size.x)] = Vector2.Distance(start, end); f[Index(start, size.x)] = h[Index(start, size.x)]; // while openSet is not empty while (openList.Count > 0) { // current:= the node in openSet having the lowest fScore[] value Vector2 current = GetBestNode(openList, (int)size.x); // if we found the path if (current == end) { // return reconstructed path callback(ReconstructPath(current, (int)size.x, surface)); if (showFinalPath) { openList.Clear(); closedList.Clear(); } pathFound = true; break; } // remove the current node from the open list openList.Remove(current); // and add it to the closed list closedList.Add(current); // for each neighbor of current for (int i = (int)Mathf.Max(current.x - 1, 0); i <= (int)Mathf.Min(current.x + 1, size.x - 1); i++) { for (int j = (int)Mathf.Max(current.y - 1, 0); j <= (int)Mathf.Min(current.y + 1, size.y - 1); j++) { Vector2 neighbor = new Vector2(i, j); // Allthough current is in the closed list and would be ignored this is cheaper for the CPU if (neighbor == current) { continue; } // if neighbor in closedSet if (!nodes[Index(neighbor, size.x)] || closedList.Contains(neighbor)) { continue; } // if neighbor not in openList: Discover a new node. Add it to open list if (!openList.Contains(neighbor)) { openList.Add(neighbor); h[Index(neighbor, size.x)] = Vector2.Distance(neighbor, end); } // The distance from start to a neighbor // the "dist_between" function may vary as per the solution requirements. float newGCost = g[Index(current, size.x)] + Vector2.Distance(current, neighbor); // if gCost is less then the current gCost keep the new else continue if (newGCost > g[Index(neighbor, size.x)]) { continue; } // // This path is the best until now. Record it! cameFrom[Index(neighbor, size.x)] = current; g[Index(neighbor, size.x)] = newGCost; // update fCost for neighbor f[Index(neighbor, size.x)] = g[Index(neighbor, size.x)] + h[Index(neighbor, size.x)]; } } if (visualizeAlgorithm) // If we are visualizing the algorithm // wait for (1/steps per second) seconds { yield return(new WaitForSeconds(1f / stepsPerSecond)); } } if (!pathFound) { callback(null); } yield return(null); }