/// <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(); }