public SeekerData(SeekerData original, Connection connection, float gScore, float hScore)
{
m_Path = new List<Connection> (original.Path);
m_Path.Add (connection);
m_GScore = original.GScore + gScore;
m_HScore = original.HScore + hScore;
}
public SeekerData(SeekerData original, Connection connection, float gScore, float hScore)
{
m_Path = new List <Connection> (original.Path);
m_Path.Add(connection);
m_GScore = original.GScore + gScore;
m_HScore = original.HScore + hScore;
}
internal Path(Vector3 startPosition, Vector3 endPosition, SeekerData data, Navigator owner)
{
m_StartPosition = startPosition;
m_EndPosition = endPosition;
m_Segments = new List <Connection> (data.Path);
m_Owner = owner;
Optimize();
}
public int CompareTo(object other)
{
SeekerData seeker = other as SeekerData;
if (seeker == null)
{
throw new System.ApplicationException("Invalid SeekerData provided for comparison");
}
return(FScore.CompareTo(seeker.FScore));
}
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
}
}
}
}
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
}
}
}
}
