public OPNode GetClosestNode(Vector3 pos) { float shortestDistance = Mathf.Infinity; OPNode node = null; for (int i = 0; i < map.nodes.Length; i++) { OPNode n = map.nodes[i]; if (n == null) { continue; } float currentDistance = Vector3.Distance(pos, n.position); if (currentDistance < 0.5) { node = n; break; } else if (currentDistance < shortestDistance) { shortestDistance = currentDistance; node = n; } } return(node); }
//private int count = 0; public OPGridMap(Vector3 start, Vector3 size, float gridSpacing, LayerMask layerMask) { List <OPNode> tempList = new List <OPNode>(); spacing = gridSpacing; int x; int z; // Raycast from every point in a horizontal grid for (x = 0; x < size.x; x++) { for (z = 0; z < size.z; z++) { Vector3 from = new Vector3(start.x + (x * spacing), start.y + (size.y * spacing), start.z + (z * spacing)); RaycastHit[] hits = RaycastContinuous(from, layerMask); // Add all hits to the list for (int r = 0; r < hits.Length; r++) { Vector3 p = hits[r].point; OPNode n = new OPNode(p.x, p.y, p.z); tempList.Add(n); } } } nodes = tempList.ToArray(); FindNeighbors(); }
public OPNode ( Vector3 v ) { position.x = v.x; position.y = v.y; position.z = v.z; estimatedTotalCost = 0.0f; costSoFar = 1.0f; parent = null; }
public OPNode ( float x, float y, float z ) { position.x = x; position.y = y; position.z = z; estimatedTotalCost = 0.0f; costSoFar = 1.0f; parent = null; }
public OPNode(Vector3 v) { position.x = v.x; position.y = v.y; position.z = v.z; estimatedTotalCost = 0.0f; costSoFar = 1.0f; parent = null; }
public OPNode(float x, float y, float z) { position.x = x; position.y = y; position.z = z; estimatedTotalCost = 0.0f; costSoFar = 1.0f; parent = null; }
private void MakeNeighbors ( OPNode a, OPNode b ) { if ( !a.neighbors.Contains ( b ) ) { a.neighbors.Add ( b ); } if ( !b.neighbors.Contains ( a ) ) { b.neighbors.Add ( a ); } }
void OnDrawGizmos() { if (map == null) { return; } if (map.nodes == null) { return; } Gizmos.color = Color.white; Gizmos.DrawWireCube(bounds.center, bounds.size); for (int i = 0; i < map.nodes.Length; i++) { OPNode n = map.nodes[i]; if (n == null) { continue; } Gizmos.color = new Color(0f, 0.8f, 1f, 1f); if (n.parent != null) { Gizmos.color = Color.red; } if (n.active) { Gizmos.color = Color.green; } if (n.neighbors.Count < 1) { Gizmos.color = Color.red; } Gizmos.DrawCube(n.position, new Vector3(0.25f, 0.25f, 0.25f)); Gizmos.color = Color.green; for (int o = 0; o < n.neighbors.Count; o++) { OPNode nb = n.neighbors[o]; if (n.active && nb.active) { Gizmos.DrawLine(n.position, nb.position); } } Gizmos.color = Color.white; } }
private void MakeNeighbors(OPNode a, OPNode b) { if (!a.neighbors.Contains(b)) { a.neighbors.Add(b); } if (!b.neighbors.Contains(a)) { b.neighbors.Add(a); } }
/*void OnDrawGizmos() { Mesh mesh = this.GetComponent<MeshFilter>().sharedMesh; List<Triangle> triangleList = new List<Triangle>(); List<OPNode> allNodes = new List<OPNode>(); int i = 0; int nb = 0; // Create triangles for (i = 0; i < mesh.triangles.Length; i += 3) { Triangle triangle = new Triangle( mesh.triangles[i], mesh.triangles[i + 1], mesh.triangles[i + 2] ); triangleList.Add(triangle); // Create median node OPNode mn = new OPNode(); mn.position = this.transform.TransformPoint(triangle.GetMedianPoint(mesh)); // Add median node to list allNodes.Add(mn); } Triangle[] triangleArray = triangleList.ToArray(); Vector3[] vertices = mesh.vertices; foreach(var no in allNodes) { Gizmos.DrawCube(no.position, new Vector3(0.25f, 0.25f, 0.25f)); } for (i = 0; i < triangleArray.Length; i++) { var n = triangleArray[i]; Gizmos.color = Color.white; Gizmos.DrawLine(vertices[n.indices[0]], vertices[n.indices[1]]); Gizmos.DrawLine(vertices[n.indices[1]], vertices[n.indices[2]]); Gizmos.DrawLine(vertices[n.indices[2]], vertices[n.indices[0]]); Gizmos.color = Color.white; } }*/ private void MakeNeighbors(OPNode a, OPNode b) { if (a == b) return; if (!a.neighbors.Contains(b)) a.neighbors.Add(b); if (!b.neighbors.Contains(a)) { b.neighbors.Add(a); } }
public OPNode Pop() { if (nodes.Count == 0) { return(null); } OPNode mn = (OPNode)nodes[0]; nodes.RemoveAt(0); return(mn); }
public IEnumerator FindPath(Vector3 start, Vector3 goal, List <OPNode> list) { if (!searching) { searching = true; OPNode here = GetClosestNode(start); OPNode there = GetClosestNode(goal); yield return(StartCoroutine(astar.Search(here, there, map, heuristic, list, maxCycles))); map.Reset(); searching = false; } }
// Locate neighbouring nodes private void FindNeighbors() { for (int o = 0; o < nodes.Length; o++) { OPNode thisNode = nodes[o]; for (int i = 0; i < nodes.Length; i++) { OPNode thatNode = nodes[i]; if ((thisNode.position - thatNode.position).sqrMagnitude <= spacing * 2.1) { thisNode.neighbors.Add(thatNode); } } } }
public int CompareTo(System.Object obj) { OPNode mn = (OPNode)obj; if (this.estimatedTotalCost < mn.estimatedTotalCost) { return(-1); } else if (this.estimatedTotalCost > mn.estimatedTotalCost) { return(1); } else { return(0); } }
public OPNode[] GetNodes() { Mesh mesh = this.GetComponent<MeshFilter>().sharedMesh; List<Triangle> triangleList = new List<Triangle>(); List<OPNode> allNodes = new List<OPNode>(); int i = 0; int nb = 0; // Create triangles for (i = 0; i < mesh.triangles.Length; i += 3) { Triangle triangle = new Triangle( mesh.triangles[i], mesh.triangles[i + 1], mesh.triangles[i + 2] ); triangleList.Add(triangle); // Create median node OPNode mn = new OPNode(); mn.position = this.transform.TransformPoint(triangle.GetMedianPoint(mesh)); // Add median node to list allNodes.Add(mn); } Triangle[] triangleArray = triangleList.ToArray(); Vector3[] vertices = mesh.vertices; // Connect median nodes for (i = 0; i < triangleArray.Length; i++) { var gn = triangleArray[i].GetNeighbors(triangleArray, vertices); for (nb = 0; nb < gn.Count; nb++) { MakeNeighbors(allNodes[i], allNodes[gn[nb]]); } } // Return return allNodes.ToArray(); }
public void SetGoal(Vector3 v, bool persist) { if (goal == v) { return; } var gNode = scanner.GetClosestNode(v); if (gNode == goalNode) { return; } goal = v; goalNode = gNode; UpdatePosition(persist); }
// Helper void used to build path for AStar search private void GetPath(OPNode node, List <OPNode> list) { // Traverse the path from goal to start int counter = 0; while (node != null) { if (counter > 100) { Debug.LogError("OpenPath | Screech! Failsafe engaged."); return; } list.Add(node); node = node.parent; counter++; } // Reverse it list.Reverse(); }
public OPNode[] GetNodes () { Mesh mesh = this.GetComponent<MeshFilter>().sharedMesh; List< Triangle > triangleList = new List< Triangle > (); List< OPNode > allNodes = new List< OPNode > (); int i = 0; int nb = 0; // Create triangles for ( i = 0; i < mesh.triangles.Length; i += 3 ) { Triangle triangle = new Triangle ( mesh.triangles [ i ], mesh.triangles [ i + 1 ], mesh.triangles [ i + 2 ] ); triangleList.Add ( triangle ); // Create median node OPNode mn = new OPNode (); mn.position = this.transform.TransformPoint ( triangle.GetMedianPoint ( mesh ) ); // Add median node to list allNodes.Add ( mn ); } Triangle[] triangleArray = triangleList.ToArray(); Vector3[] vertices = mesh.vertices; // Connect median nodes for ( i = 0; i < triangleArray.Length; i++ ) { for ( nb = 0; nb < triangleArray[i].GetNeighbors ( triangleArray, vertices ).Count; nb++ ) { MakeNeighbors ( allNodes [ i ], allNodes [ nb ] ); } } // Return return allNodes.ToArray(); }
//private int count = 0; public OPGridMap ( Vector3 start, Vector3 size, float gridSpacing, LayerMask layerMask ) { List< OPNode > tempList = new List< OPNode >(); spacing = gridSpacing; int x; int z; // Raycast from every point in a horizontal grid for ( x = 0; x < size.x; x++ ) { for ( z = 0; z < size.z; z++ ) { Vector3 from = new Vector3 ( start.x + (x*spacing), start.y + (size.y*spacing), start.z + (z*spacing) ); RaycastHit[] hits = RaycastContinuous ( from, layerMask ); // Add all hits to the list for ( int r = 0; r < hits.Length; r++ ) { Vector3 p = hits[r].point; OPNode n = new OPNode ( p.x, p.y, p.z ); tempList.Add ( n ); } } } nodes = tempList.ToArray(); FindNeighbors (); }
public List<OPNode> GetNeighbors ( OPNode node ) { return node.neighbors; }
public OPNode() { estimatedTotalCost = 0.0f; costSoFar = 1.0f; parent = null; }
public int Push ( OPNode node ) { nodes.Add ( node ); nodes.Sort(); return nodes.Count; }
// Find a path and return a list of each step public IEnumerator Search ( OPNode start, OPNode goal, OPMap map, float heuristicWeight, List< OPNode > list, int maxCycles ) { Debug.Log(start.ToString() + ", " + goal.ToString()); if ( start != null && goal != null ) { // Add the starting node to the open list openList = new OPPriorityQueue (); openList.Push ( start ); start.costSoFar = 0; start.estimatedTotalCost = HeuristicEstimate ( start, goal, heuristicWeight ); closedList = new OPPriorityQueue (); OPNode currentNode = null; int cycles = 0; // While the open list is not empty while ( openList.GetLength() != 0 ) { // Current node = node from the open list with the lowest cost currentNode = openList.Front(); if ( currentNode == goal ) { break; } // Examine each node adjacent to the current node List <OPNode> neighbors = map.GetNeighbors ( currentNode ); for ( int nIndex = 0; nIndex != neighbors.Count; nIndex++ ) { // Get the cost estimate for the end node OPNode endNode = (OPNode) neighbors[nIndex]; float incrementalCost = GetCost ( currentNode, endNode ); float endNodeCost = currentNode.costSoFar + incrementalCost; // If the node is closed we may have to skip or remove it from the closed list. if ( closedList.Contains ( endNode ) ) { // If we didn't find a shorter route, skip. if ( endNode.costSoFar <= endNodeCost ) { continue; } // Otherwise remove it from the closed list closedList.Remove( endNode ); // Skip if the node is open and we haven't found a better route } else if ( openList.Contains ( endNode ) ) { // If our route is no better, then skip if(endNode.costSoFar <= endNodeCost ) { continue; } } float endNodeHeuristic = HeuristicEstimate ( endNode, goal, heuristicWeight ); // We are here if we need to update the node // Update the cost and estimate endNode.costSoFar = endNodeCost; endNode.parent = currentNode; endNode.estimatedTotalCost = endNodeCost + endNodeHeuristic; // And add it to the open list if ( !openList.Contains ( endNode ) ) { openList.Push(endNode); } } // We've finished looking at the neighbors for the current node, so add it to the closed list and remove it from the open list closedList.Push ( currentNode ); openList.Remove ( currentNode ); if ( cycles > maxCycles ) { cycles = 0; yield return null; } else { cycles++; } } if ( currentNode.Equals ( goal ) ) { // Path complete GetPath ( currentNode, list ); } } }
// Helper void used to build path for AStar search private void GetPath ( OPNode node, List< OPNode > list ) { // Traverse the path from goal to start int counter = 0; while ( node != null ) { if ( counter > 100 ) { Debug.LogError ( "OpenPath | Screech! Failsafe engaged." ); return; } list.Add ( node ); node = node.parent; counter++; } // Reverse it list.Reverse(); }
public int Push(OPNode node) { nodes.Add(node); nodes.Sort(); return(nodes.Count); }
public int GetIndex(OPNode node) { return(System.Array.IndexOf(nodes, node)); }
private float GetCost(OPNode node0, OPNode node1) { return((node0.position - node1.position).magnitude); }
// Find a path and return a list of each step public IEnumerator Search(OPNode start, OPNode goal, OPMap map, float heuristicWeight, List <OPNode> list, int maxCycles) { Debug.Log(start.position + ", " + goal.position); if (start != null && goal != null) { // Add the starting node to the open list openList = new OPPriorityQueue(); openList.Push(start); start.costSoFar = 0; start.estimatedTotalCost = HeuristicEstimate(start, goal, heuristicWeight); closedList = new OPPriorityQueue(); OPNode currentNode = null; int cycles = 0; // While the open list is not empty while (openList.GetLength() != 0) { // Current node = node from the open list with the lowest cost currentNode = openList.Front(); if (currentNode == goal) { break; } // Examine each node adjacent to the current node List <OPNode> neighbors = map.GetNeighbors(currentNode); for (int nIndex = 0; nIndex != neighbors.Count; nIndex++) { // Get the cost estimate for the end node OPNode endNode = (OPNode)neighbors[nIndex]; float incrementalCost = GetCost(currentNode, endNode); float endNodeCost = currentNode.costSoFar + incrementalCost; // If the node is closed we may have to skip or remove it from the closed list. if (closedList.Contains(endNode)) { // If we didn't find a shorter route, skip. if (endNode.costSoFar <= endNodeCost) { continue; } // Otherwise remove it from the closed list closedList.Remove(endNode); // Skip if the node is open and we haven't found a better route } else if (openList.Contains(endNode)) { // If our route is no better, then skip if (endNode.costSoFar <= endNodeCost) { continue; } } float endNodeHeuristic = HeuristicEstimate(endNode, goal, heuristicWeight); // We are here if we need to update the node // Update the cost and estimate endNode.costSoFar = endNodeCost; endNode.parent = currentNode; endNode.estimatedTotalCost = endNodeCost + endNodeHeuristic; // And add it to the open list if (!openList.Contains(endNode)) { openList.Push(endNode); } } // We've finished looking at the neighbors for the current node, so add it to the closed list and remove it from the open list closedList.Push(currentNode); openList.Remove(currentNode); if (cycles > maxCycles) { cycles = 0; yield return(null); } else { cycles++; } } if (currentNode.Equals(goal)) { // Path complete GetPath(currentNode, list); } } }
private float HeuristicEstimate(OPNode currNode, OPNode goal, float heuristicWeight) { return((currNode.position - goal.position).magnitude * heuristicWeight); }
public bool Contains(OPNode node) { return(nodes.Contains(node)); }
public void Remove(OPNode node) { nodes.Remove(node); nodes.Sort(); }
public int GetIndex ( OPNode node ) { return System.Array.IndexOf ( nodes, node ); }
public void SetMap ( OPNode[] nodes ) { map = new OPMap (); map.nodes = nodes; }
private float HeuristicEstimate ( OPNode currNode, OPNode goal, float heuristicWeight ) { return ( currNode.position - goal.position ).magnitude * heuristicWeight; }
public void Remove ( OPNode node ) { nodes.Remove ( node ); nodes.Sort(); }
public List <OPNode> GetNeighbors(OPNode node) { return(node.neighbors); }
public bool Contains ( OPNode node ) { return nodes.Contains ( node ); }
private float GetCost ( OPNode node0, OPNode node1 ) { return ( node0.position - node1.position ).magnitude; }
public OPNode () { estimatedTotalCost = 0.0f; costSoFar = 1.0f; parent = null; }