private void Optimize() { if (!m_Owner.takeShortcuts || m_Segments.Count < 2) { #if DEBUG_SEEKER Debug.Log("Path: Not optimizing. Too few segments or no shortcut taking."); #endif return; } #if DEBUG_SEEKER Debug.Log("Path: Optimizing start."); #endif for (int i = 1; i < m_Segments.Count - 1;) { if (m_Segments[i].From.Contains(m_StartPosition) || m_Owner.DirectPath(m_StartPosition, m_Segments[i].From.Position)) { #if DEBUG_SEEKER Debug.Log("Path: Cut to starting at " + m_Segments[i].From + "."); #endif m_Segments.RemoveRange(0, i); i = 1; } else { i++; } } #if DEBUG_SEEKER Debug.Log("Path: Optimizing end."); #endif for (int i = m_Segments.Count - 1; i > 0;) { if (m_Segments[i].To.Contains(m_EndPosition) || m_Owner.DirectPath(m_Segments[i].To.Position, m_EndPosition)) { #if DEBUG_SEEKER Debug.Log("Path: Cut to ending at " + m_Segments[i].To + "."); #endif m_Segments.RemoveRange(i, m_Segments.Count - i); i = m_Segments.Count - 1; } else { i--; } } }
/// Returns nearest node to a position for a given Navigator. If the Navigator has non-zero pathBlockingLayers /// specified, each potential node will be checked for accessibility with a sphere cast. public static Waypoint GetNearestNode(Vector3 position, Navigator navigator) { Waypoint nearest = null; foreach (Waypoint waypoint in Navigation.Waypoints) { if ( waypoint.Enabled && ( nearest == null || (nearest.Position - position).sqrMagnitude > (waypoint.Position - position).sqrMagnitude ) && ( (waypoint.Position - position).magnitude < waypoint.Radius || navigator == null || navigator.DirectPath(position, waypoint.Position) ) ) { nearest = waypoint; } } return(nearest); }
/// Returns nearest node to a position for a given Navigator. If the Navigator has non-zero pathBlockingLayers /// specified, each potential node will be checked for accessibility with a sphere cast. public static Waypoint GetNearestNode(Vector3 position, Navigator navigator) { Waypoint nearest = null; foreach (Waypoint waypoint in Navigation.Waypoints) { if ( waypoint.Enabled && ( nearest == null || (nearest.Position - position).sqrMagnitude > (waypoint.Position - position).sqrMagnitude ) && ( (waypoint.Position - position).magnitude < waypoint.Radius || navigator == null || navigator.DirectPath (position, waypoint.Position) ) ) { nearest = waypoint; } } return nearest; }
public IEnumerator Seek() { #if DEBUG_SEEKER Debug.Log("Seeker: Seek started."); #endif m_StartTime = Time.realtimeSinceStartup; m_Seeking = true; if (m_Owner.takeShortcuts && m_Owner.DirectPath(m_StartPosition, m_EndPosition)) // See if we can just go directly from start to end and early-out if so { #if DEBUG_SEEKER Debug.Log("Seeker: DirectPath. Early out."); #endif OnPathResult(new Path(m_StartPosition, m_EndPosition, m_Owner)); yield break; } Waypoint startNode = Navigation.GetNearestNode(m_StartPosition, m_Owner), endNode = Navigation.GetNearestNode(m_EndPosition, m_Owner); if (startNode == null || endNode == null) // Unable to find either a start or an end node { #if DEBUG_SEEKER Debug.Log(string.Format("Seeker: No start or end node found while trying to pathfind from {0} to {1}. Failure.", m_StartPosition, m_EndPosition)); #endif OnPathFailed(); yield break; } if (startNode == endNode) // If start and end node is the same, we can early out as well { #if DEBUG_SEEKER Debug.Log(string.Format("Seeker: Start and end node shared: {0}. Early out.", startNode)); #endif OnPathResult(new Path(m_StartPosition, m_EndPosition, startNode, m_Owner)); yield break; } Dictionary <Connection, SeekerData> openSet = new Dictionary <Connection, SeekerData> (); foreach (Connection connection in startNode.Connections) { if (!Valid(connection)) { #if DEBUG_SEEKER Debug.Log(string.Format("Seeker: Skipping invalid connection {0}.", connection)); #endif continue; } openSet[connection] = new SeekerData(connection, GScore(connection), HScore(connection)); #if DEBUG_SEEKER Debug.Log("Added " + connection + " to open set."); #endif } List <Connection> closedSet = new List <Connection> (); while (Application.isPlaying && m_Seeking) // Continue seeking until the end of time or we're instructed to stop { yield return(null); for (int i = 0; i < m_IterationCap; i++) // Run the maximum specified number of iterations before yielding the frame again { if (openSet.Count == 0) // No more avenues to investigate. Unable to find path. { #if DEBUG_SEEKER Debug.Log(string.Format("Seeker: Empty open set while trying to pathfind from {0} to {1}. Failure.", startNode, endNode)); #endif OnPathFailed(); yield break; } // Pick the cheapest option for investigation List <SeekerData> openSetValues = new List <SeekerData> (openSet.Values); openSetValues.Sort(); SeekerData currentPath = openSetValues[0]; if (currentPath.Destination == endNode) // Did find the path. If still valid, return it - otherwise start over { Path path = new Path(m_StartPosition, m_EndPosition, currentPath, m_Owner); if (path.Valid) { OnPathResult(path); yield break; } else { #if DEBUG Debug.Log("Seeker: Path invalidated in middle of search. Re-seeking."); #endif yield return(Seek()); yield break; } } // Update the open/closed sets openSet.Remove(currentPath.LastSegment); closedSet.Add(currentPath.LastSegment); foreach (Connection connection in currentPath.Options) // Add connections leading out from this connection to the open set { if (!Valid(connection)) // Ignore invalid options { #if DEBUG_SEEKER Debug.Log(string.Format("Seeker: Skipping invalid connection {0} in path {1}.", connection, currentPath)); #endif continue; } if (closedSet.Contains(connection)) // Ignore already investigated options { #if DEBUG_SEEKER Debug.Log(string.Format("Seeker: Skipping closed set connection {0} in path {1}.", connection, currentPath)); #endif continue; } if (openSet.ContainsKey(connection)) // Ignore options we're already looking into { #if DEBUG_SEEKER Debug.Log(string.Format("Seeker: Skipping open set connection {0} in path {1}.", connection, currentPath)); #endif continue; } // Add a new option - the current path with this connection at the end openSet[connection] = new SeekerData(currentPath, connection, GScore(connection), HScore(connection)); #if DEBUG_SEEKER Debug.Log("Added " + connection + " to open set."); #endif } } } }