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