/// <summary>
/// Prepares the backwards sets for pathfinding.
/// </summary>
/// <param name="referencePosition">The position from which all pathfinding nodes for all sets will have a discrete distance.</param>
/// <param name="maxNodeDistance">The maximum distance between nodes.</param>
private void SetupBackward(out Vector3D referencePosition, int maxNodeDistance)
{
referencePosition = default(Vector3D);
CreateBackwards();
int index = 0;
foreach (Destination dest in m_destinations)
{
PathNodeSet pns = m_backwardList[index];
Vector3D startPosition;
GetFromObsPosition(dest, out startPosition);
if (index == 0)
{
referencePosition = startPosition;
}
pns.Setup(ref referencePosition, ref startPosition, m_canChangeCourse, maxNodeDistance);
index++;
}
if (referencePosition == default(Vector3D))
{
throw new Exception("no backwards set! destination count: " + m_destinations.Length);
}
}
/// <summary>
/// Compares this PathNodeSet to another for the purposes of scheduling.
/// </summary>
/// <param name="other">The object to compare this to.</param>
/// <returns>A negative integer, zero, or a positive integer, indicating scheduling priority.</returns>
public override int CompareTo(PathNodeSet other)
{
FindingSet otherFS = (FindingSet)other;
if (this.Failed)
{
return(1);
}
else if (otherFS.Failed)
{
return(-1);
}
int value = m_blueSkyNodes.Count - otherFS.m_blueSkyNodes.Count;
if (value != 0)
{
return(value);
}
value = m_reachedNodes.Count - otherFS.m_reachedNodes.Count;
if (value != 0)
{
return(value);
}
return(0);
}
private string SetName(PathNodeSet pnSet)
{
return(pnSet == m_forward ? "Forward" : "Backward");
}
/// <summary>
/// Upon completion of pathfinding, this method builds the path that autopilot will have to follow.
/// </summary>
/// <param name="forwardHash">Position hash of the forward node that was reached.</param>
/// <param name="backward">The backward set that was reached.</param>
/// <param name="backwardHash">Position hash of the backward node that was reached.</param>
private void BuildPath(long forwardHash, PathNodeSet backward = null, long backwardHash = 0L)
{
m_path.Clear();
#if SHOW_PATH
PurgeTempGPS();
#endif
Log.DebugLog("Obstruction: " + m_obstructingEntity.Entity.nameWithId() + ", match position: " + m_obstructingEntity.MatchPosition + ", position: " + m_obstructingEntity.GetPosition() + ", actual position: " + m_obstructingEntity.Entity.PositionComp.GetPosition());
PathNode node;
if (!m_forward.TryGetReached(forwardHash, out node))
{
Log.AlwaysLog("Parent hash " + forwardHash + " not found in forward set, failed to build path", Logger.severity.ERROR);
if (backward.HasReached(forwardHash))
{
Log.AlwaysLog("Backward set does contains hash", Logger.severity.DEBUG);
}
PathfindingFailed();
return;
}
while (node.DistToCur != 0f)
{
#if SHOW_PATH
ShowPosition(node, "Path");
#endif
m_path.AddFront(ref node.Position);
Log.DebugLog("Forward: " + ReportRelativePosition(node.Position));
if (!m_forward.TryGetReached(node.ParentKey, out node))
{
Log.AlwaysLog("Child hash " + forwardHash + " not found in forward set, failed to build path", Logger.severity.ERROR);
if (backward.HasReached(forwardHash))
{
Log.AlwaysLog("Backward set does contains hash", Logger.severity.DEBUG);
}
PathfindingFailed();
return;
}
}
m_path.AddFront(ref node.Position);
Log.DebugLog("Forward: " + ReportRelativePosition(node.Position));
if (backwardHash != 0L)
{
if (!backward.TryGetReached(backwardHash, out node))
{
Log.AlwaysLog("Parent hash " + backwardHash + " not found in backward set, failed to build path", Logger.severity.ERROR);
if (m_forward.HasReached(forwardHash))
{
Log.AlwaysLog("Forward set does contains hash", Logger.severity.DEBUG);
}
PathfindingFailed();
return;
}
if (forwardHash == backwardHash && node.ParentKey != 0L)
{
if (!backward.TryGetReached(node.ParentKey, out node))
{
Log.AlwaysLog("First child hash " + backwardHash + " not found in backward set, failed to build path", Logger.severity.ERROR);
if (m_forward.HasReached(forwardHash))
{
Log.AlwaysLog("Forward set does contains hash", Logger.severity.DEBUG);
}
PathfindingFailed();
return;
}
}
while (node.DistToCur != 0f)
{
#if SHOW_PATH
ShowPosition(node, "Path");
#endif
m_path.AddBack(ref node.Position);
Log.DebugLog("Backward: " + ReportRelativePosition(node.Position));
if (!backward.TryGetReached(node.ParentKey, out node))
{
Log.AlwaysLog("Child hash " + backwardHash + " not found in backward set, failed to build path", Logger.severity.ERROR);
if (m_forward.HasReached(forwardHash))
{
Log.AlwaysLog("Forward set does contains hash", Logger.severity.DEBUG);
}
PathfindingFailed();
return;
}
}
m_path.AddBack(ref node.Position);
Log.DebugLog("Backward: " + ReportRelativePosition(node.Position));
}
//#if PROFILE
// LogStats();
//#endif
foreach (Vector3D position in m_path.m_positions)
{
Log.DebugLog("Path: " + ReportRelativePosition(position));
}
if (backward != null)
{
for (int i = 0; i < m_backwardList.Length; i++)
{
if (m_backwardList[i] == backward)
{
m_pickedDestination = m_destinations[i];
Log.DebugLog("Picked destination: " + m_pickedDestination);
break;
}
}
}
CurrentState = State.FollowingPath;
Log.DebugLog("Built path", Logger.severity.INFO);
//Logger.DebugNotify("Finished Pathfinding", level: Logger.severity.INFO);
SetNextPathTarget();
}
/// <summary>
/// Main entry point while pathfinding.
/// </summary>
private void ContinuePathfinding()
{
if (m_waitUntil > Globals.UpdateCount)
{
m_holdPosition = true;
OnComplete();
return;
}
Log.DebugLog("m_runningLock == null", Logger.severity.FATAL, condition: m_runningLock == null);
if (!m_runningLock.TryAcquireExclusive())
{
return;
}
try
{
if (m_runHalt || m_runInterrupt)
{
return;
}
FillDestWorld();
if (!m_canChangeCourse)
{
ContinuePathfinding(m_forward);
}
else
{
Log.DebugLog("m_forward == null", Logger.severity.FATAL, condition: m_forward == null);
Log.DebugLog("m_backwardList == null", Logger.severity.FATAL, condition: m_backwardList == null);
PathNodeSet bestSet = null;
foreach (PathNodeSet backSet in m_backwardList)
{
if (bestSet == null || backSet.CompareTo(bestSet) < 0)
{
bestSet = backSet;
}
}
Log.DebugLog("bestSet == null", Logger.severity.FATAL, condition: bestSet == null);
FindingSet fs = (FindingSet)bestSet;
if (fs.Failed)
{
if (!MoveToArbitrary())
{
ContinuePathfinding(m_forward);
}
}
else
{
if (m_forward.CompareTo(bestSet) < 0)
{
fs = m_forward;
}
ContinuePathfinding(fs);
}
}
OnComplete();
}
catch
{
if (m_runHalt || m_runInterrupt)
{
Log.DebugLog("Exception due to halt/interrupt", Logger.severity.DEBUG);
return;
}
else
{
Log.AlwaysLog("Pathfinder crashed", Logger.severity.ERROR);
CurrentState = State.Crashed;
throw;
}
}
finally { m_runningLock.ReleaseExclusive(); }
PostRun();
}
