/// <summary>
/// Tests if a line can be traveled from input.Offset to input.Offset + input.Direction * input.Length
/// </summary>
/// <param name="input">Param for PathTester</param>
/// <param name="proximity">How close the ship would come to an obstruction.</param>
/// <returns>True iff the segment can be traveled by the ship.</returns>
private bool CanTravelSegment(ref PathTester.TestInput input, out float proximity)
{
Profiler.StartProfileBlock();
Log.DebugLog(input.ToString());
MyCubeBlock ignoreBlock = NavSet.Settings_Current.DestinationEntity as MyCubeBlock;
proximity = float.MaxValue;
if (m_checkVoxel)
{
//Log.DebugLog("raycasting voxels");
PathTester.VoxelTestResult result;
if (m_tester.RayCastIntersectsVoxel(ref input, out result))
{
//Log.DebugLog("Obstructed by voxel " + hitVoxel + " at " + hitPosition, Logger.severity.DEBUG, condition: hitVoxel != m_obstructingEntity.Entity);
Profiler.EndProfileBlock();
return(false);
}
if (result.Proximity < proximity)
{
proximity = result.Proximity;
}
}
//Log.TraceLog("checking " + m_entitiesPruneAvoid.Count + " entites - voxels");
if (m_entitiesPruneAvoid.Count != 0)
{
for (int i = m_entitiesPruneAvoid.Count - 1; i >= 0; i--)
{
MyEntity entity = m_entitiesPruneAvoid[i];
if (entity is MyVoxelBase)
{
// already checked
continue;
}
//Log.DebugLog("checking: " + entity.nameWithId());
PathTester.GridTestResult result;
if (m_tester.ObstructedBy(entity, ignoreBlock, ref input, out result))
{
//Log.DebugLog("Obstructed by " + entity.nameWithId() + "." + obstructBlock, Logger.severity.DEBUG, condition: entity != m_obstructingEntity.Entity);
Profiler.EndProfileBlock();
return(false);
}
if (result.Proximity < proximity)
{
proximity = result.Proximity;
}
}
}
//Log.DebugLog("No obstruction. Start: " + (m_currentPosition + offset) + ", finish: " + (m_currentPosition + offset + (line.To - line.From)));
Profiler.EndProfileBlock();
return(true);
}
/// <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);
}
