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