/// <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); }
/// <summary> /// Resolves set running out of nodes, either changing NodeDistance, moving the ship, or failing pathfinding. /// </summary> /// <param name="pnSet">The set that is out of nodes.</param> private void OutOfNodes(FindingSet pnSet) { if (pnSet.NodeDistance > FindingSet.MinNodeDistance) { pnSet.ChangeNodeDistance(true, m_canChangeCourse); return; } if (pnSet != m_forward) { Log.DebugLog("Failed set: " + ReportRelativePosition(pnSet.m_startPosition), Logger.severity.DEBUG); return; } if (MoveToArbitrary()) { return; } // with line, failing is "normal" Log.DebugLog("Pathfinding failed", m_canChangeCourse ? Logger.severity.WARNING : Logger.severity.DEBUG); Logger.DebugNotify("Pathfinding failed", 10000, m_canChangeCourse ? Logger.severity.WARNING : Logger.severity.DEBUG); //#if PROFILE // LogStats(); //#endif PathfindingFailed(); return; }
/// <summary> /// Called when a blue sky node is reached. Tests if the ship can reach a target blue sky or target start. /// </summary> /// <param name="pnSet">The active set.</param> /// <param name="currentNode">The blue sky node.</param> /// <param name="currentNodeWorld">World Position of the current node.</param> /// <returns>True iff opposite start or target blue sky is reached, in which case, this method will have invoked BuildPath.</returns> private bool BlueSkyReached(FindingSet pnSet, PathNode currentNode, ref Vector3D currentNodeWorld) { //Logger.DebugNotify(SetName(pnSet) + " Blue Sky"); Log.DebugLog(SetName(pnSet) + " Blue sky node: " + ReportRelativePosition(currentNode.Position)); PathTester.TestInput input; Vector3D.Subtract(ref currentNodeWorld, ref m_currentPosition, out input.Offset); foreach (PathNodeSet target in pnSet.m_targets) { Vector3D disp; Vector3D.Subtract(ref target.m_startPosition, ref currentNode.Position, out disp); input.Direction = disp; input.Length = input.Direction.Normalize(); float proximity; if (CanTravelSegment(ref input, out proximity)) { Log.DebugLog(SetName(pnSet) + " Blue sky to opposite start"); if (pnSet == m_forward) { BuildPath(currentNode.Key, target, target.m_startPosition.GetHash()); } else { BuildPath(target.m_startPosition.GetHash(), pnSet, currentNode.Key); } return(true); } foreach (Vector3D targetBlueSky in target.BlueSkyNodes) { input.Direction = Vector3D.Subtract(targetBlueSky, currentNode.Position); input.Length = input.Direction.Normalize(); if (CanTravelSegment(ref input, out proximity)) { Log.DebugLog(SetName(pnSet) + " Blue sky path"); if (pnSet == m_forward) { BuildPath(currentNode.Key, target, targetBlueSky.GetHash()); } else { BuildPath(targetBlueSky.GetHash(), pnSet, currentNode.Key); } return(true); } } } pnSet.m_blueSkyNodes.Add(currentNode.Position); #if SHOW_REACHED ShowPosition(currentNode, "Blue Sky " + SetName(pnSet)); #endif return(false); }
/// <summary> /// Move to arbitrary node, this can resolve circular obstructions. /// </summary> /// <returns>True iff a path has been built to an arbitrary node.</returns> private bool MoveToArbitrary() { if (m_forward.m_openNodes.Count != 0 && m_forward.m_reachedNodes.Count * m_forward.NodeDistance < 1000) { // try to get more forward nodes before picking one return(false); } PathNode arbitraryNode = default(PathNode); foreach (KeyValuePair <long, PathNode> pair in m_forward.m_reachedNodes) { PathNode node = pair.Value; if (arbitraryNode.DistToCur < 100f) { if (node.DistToCur < arbitraryNode.DistToCur) { Log.DebugLog("Node does not get closer to 100 m from current. node.DistToCur: " + node.DistToCur + ", arbitraryNode.DistToCur: " + arbitraryNode.DistToCur); continue; } } else if (node.DistToCur < 100f) { Log.DebugLog("Node is not far enough: " + node.DistToCur); continue; } else if (FindingSet.MinPathDistance(ref m_forward.m_referencePosition, ref arbitraryNode.Position) < FindingSet.MinPathDistance(ref m_forward.m_referencePosition, ref node.Position)) { Log.DebugLog("Node is further from destination"); continue; } arbitraryNode = node; } Log.DebugLog("Chosen node: " + ReportRelativePosition(arbitraryNode.Position) + ", dist to cur: " + arbitraryNode.DistToCur + ", min path: " + FindingSet.MinPathDistance(ref m_forward.m_referencePosition, ref arbitraryNode.Position)); if (arbitraryNode.DistToCur > 10f) { Logger.DebugNotify("Building path to arbitrary node", level: Logger.severity.DEBUG); BuildPath(arbitraryNode.Position.GetHash()); return(true); } return(false); }
/// <summary> /// Remove all sets from m_backwardList. /// </summary> private void ClearBackwards() { if (m_backwardList == null) { return; } for (int i = 0; i < m_backwardList.Length; i++) { FindingSet back = (FindingSet)m_backwardList[i]; if (back != null) { back.Clear(); ResourcePool.Return(back); m_backwardList[i] = null; } } }
/// <summary> /// Called when a node is reached. Tests against targets and creates new nodes. /// </summary> /// <param name="pnSet">The active set.</param> /// <param name="currentNode">The node that was reached.</param> /// <param name="input">Param for CanTravelSegment, Offset and Direction should be correct.</param> /// <param name="proximity">Result from CanTravelSegment, how close the ship would come to an entity when traveling to this node from its parent.</param> private void ReachedNode(FindingSet pnSet, ref PathNode currentNode, ref PathTester.TestInput input, float proximity) { // impose a penalty for going near entities, this does not affect this node but will affect its children // this really helps prevent pathfinder getting stuck but the penalty might be too high float penalty = 10f * (100f - MathHelper.Clamp(proximity, 0f, 100f)); Log.DebugLog(SetName(pnSet) + " Reached node: " + ReportRelativePosition(currentNode.Position) + " from " + ReportRelativePosition(pnSet.m_reachedNodes[currentNode.ParentKey].Position) + ", reached: " + pnSet.m_reachedNodes.Count + ", open: " + pnSet.m_openNodes.Count + ", proximity: " + proximity + ", penalty: " + penalty); currentNode.DistToCur += penalty; long cNodePosHash = currentNode.Key; pnSet.m_reachedNodes.Add(cNodePosHash, currentNode); if (!m_canChangeCourse) { // test from current node position to destination Log.DebugLog(SetName(pnSet) + " Running backwards search", Logger.severity.ERROR, condition: pnSet != m_forward); Vector3D.Subtract(ref m_currentPosition, ref pnSet.m_referencePosition, out input.Offset); input.Length = (float)Vector3D.Distance(currentNode.Position, pnSet.m_referencePosition); if (CanTravelSegment(ref input, out proximity)) { Log.DebugLog(SetName(pnSet) + " Reached destination from node: " + ReportRelativePosition(currentNode.Position)); Log.DebugLog("Backwards start is not reference position", Logger.severity.ERROR, condition: m_backwardList[0].m_startPosition != pnSet.m_referencePosition); BuildPath(cNodePosHash, m_backwardList[0], pnSet.m_referencePosition.GetHash()); return; } #if SHOW_REACHED ShowPosition(currentNode, SetName(pnSet)); #endif return; } foreach (PathNodeSet target in pnSet.m_targets) { if (target.HasReached(cNodePosHash)) { Log.DebugLog(SetName(pnSet) + " Opposite set has same position"); if (pnSet == m_forward) { BuildPath(cNodePosHash, target, cNodePosHash); } else { BuildPath(cNodePosHash, pnSet, cNodePosHash); } return; } } Vector3D obstructPosition = m_obstructingEntity.GetPosition(); Vector3D currentNodeWorld; Vector3D.Add(ref obstructPosition, ref currentNode.Position, out currentNodeWorld); BoundingSphereD sphere = new BoundingSphereD() { Center = currentNodeWorld, Radius = m_autopilotShipBoundingRadius + 100f }; m_entitiesRepulse.Clear(); // use repulse list as prune/avoid is needed for CanTravelSegment MyGamePruningStructure.GetAllTopMostEntitiesInSphere(ref sphere, m_entitiesRepulse); if (m_entitiesRepulse.Count == 0 && BlueSkyReached(pnSet, currentNode, ref currentNodeWorld)) { return; } #if SHOW_REACHED ShowPosition(currentNode, SetName(pnSet)); #endif if (pnSet.NodeDistance < FindingSet.DefaultNodeDistance && pnSet.m_reachedNodes.Count % 10 == 0 && pnSet.m_openNodes.Count > 100) { Log.DebugLog("Reached: " + pnSet.m_reachedNodes.Count + ", trying with higher node distance"); pnSet.ChangeNodeDistance(false, m_canChangeCourse); return; } pnSet.CreatePathNode(ref currentNode, m_canChangeCourse); }
/// <summary> /// Continues pathfinding. /// </summary> /// <param name="pnSet">The active set.</param> private void ContinuePathfinding(FindingSet pnSet) { //PathNodeSet pnSet = isForwardSet ? m_forward : m_backward; Log.DebugLog(SetName(pnSet) + " m_obstructingEntity == null", Logger.severity.ERROR, condition: m_obstructingEntity.Entity == null); if (pnSet.m_openNodes.Count == 0) { OutOfNodes(pnSet); return; } PathNode currentNode = pnSet.m_openNodes.RemoveMin(); if (currentNode.DistToCur == 0f) { Log.DebugLog(SetName(pnSet) + " first node: " + ReportRelativePosition(currentNode.Position)); pnSet.CreatePathNode(ref currentNode, m_canChangeCourse); return; } if (pnSet.HasReached(currentNode.Position.GetHash())) { //Log.DebugLog("Already reached: " + ReportRelativePosition(currentNode.Position), secondaryState: SetName(direction)); return; } FillEntitiesLists(); Vector3 repulsion; CalcRepulsion(false, out repulsion); Log.DebugLog(SetName(pnSet) + " Calculated repulsion for some reason: " + repulsion, Logger.severity.WARNING, condition: repulsion != Vector3.Zero); PathNode parent; if (!pnSet.m_reachedNodes.TryGetValue(currentNode.ParentKey, out parent)) { Log.DebugLog("Failed to get parent", Logger.severity.ERROR); return; } Vector3D worldParent; Vector3D obstructPosition = m_obstructingEntity.GetPosition(); Vector3D.Add(ref obstructPosition, ref parent.Position, out worldParent); PathTester.TestInput input; Vector3D.Subtract(ref worldParent, ref m_currentPosition, out input.Offset); input.Direction = currentNode.DirectionFromParent; input.Length = currentNode.DistToCur - parent.DistToCur; float proximity; if (!CanTravelSegment(ref input, out proximity)) { #if PROFILE pnSet.m_unreachableNodes++; #endif //Log.DebugLog("Not reachable: " + ReportRelativePosition(currentNode.Position), secondaryState: SetName(pnSet)); return; } ReachedNode(pnSet, ref currentNode, ref input, proximity); }
/// <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(); }