/// <summary>
/// Test a path between current position and destination.
/// </summary>
private void TestPath(Vector3D destination, MyEntity destEntity, short runId, bool isAlternate, bool tryAlternates, bool slowDown = false)
{
if (!lock_testPath.TryAcquireExclusive())
{
m_logger.debugLog("Already running, requeue (destination:" + destination + ", destEntity: " + destEntity + ", runId :" + runId
+ ", isAlternate: " + isAlternate + ", tryAlternates: " + tryAlternates + ", slowDown: " + slowDown + ")", "TestPath()");
LockedQueue<Action> queue = isAlternate ? m_pathLow : m_pathHigh;
queue.Enqueue(() => TestPath(destination, destEntity, runId, isAlternate, tryAlternates));
return;
}
m_logger.debugLog("Running, (destination:" + destination + ", destEntity: " + destEntity + ", runId :" + runId
+ ", isAlternate: " + isAlternate + ", tryAlternates: " + tryAlternates + ", slowDown: " + slowDown + ")", "TestPath()");
try
{
if (!isAlternate && !tryAlternates)
m_logger.debugLog("velocity based test follows", "TestPath()", Logger.severity.INFO);
if (runId != m_runId)
{
m_logger.debugLog("destination changed, abort", "TestPath()", Logger.severity.DEBUG);
return;
}
if (m_pathChecker.TestFast(m_navBlock, destination, m_ignoreAsteroid, destEntity, m_landing))
{
m_logger.debugLog("path is clear (fast)", "TestPath()", Logger.severity.TRACE);
PathClear(ref destination, runId, isAlternate, slowDown);
return;
}
MyEntity obstructing;
IMyCubeGrid obsGrid;
Vector3? pointOfObstruction;
if (m_pathChecker.TestSlow(out obstructing, out obsGrid, out pointOfObstruction))
{
m_logger.debugLog("path is clear (slow)", "TestPath()", Logger.severity.TRACE);
PathClear(ref destination, runId, isAlternate, slowDown);
return;
}
if (runId != m_runId)
{
m_logger.debugLog("destination changed, abort", "TestPath()", Logger.severity.DEBUG);
return;
}
if (slowDown)
{
Vector3 displacement = pointOfObstruction.Value - m_navBlock.WorldPosition;
m_navSet.Settings_Task_NavWay.SpeedMaxRelative = Vector3.Distance(pointOfObstruction.Value, m_navBlock.WorldPosition) / LookAheadSpeed_Seconds;
m_logger.debugLog("Set MaxRelativeSpeed to " + m_navSet.Settings_Task_NavWay.SpeedMaxRelative, "TestPath()", Logger.severity.TRACE);
return;
}
m_pathState = PathState.Searching;
m_logger.debugLog("path is blocked by " + obstructing.getBestName() + " at " + pointOfObstruction + ", destEntity: " + destEntity.getBestName(), "TestPath()", Logger.severity.TRACE);
if (tryAlternates)
{
m_pathHigh.Clear();
Vector3 displacement = pointOfObstruction.Value - m_navBlock.WorldPosition;
if (m_canChangeCourse)
{
FindAlternate_AroundObstruction(displacement, pointOfObstruction.Value, obstructing.GetLinearVelocity(), runId);
FindAlternate_JustMove(runId);
}
FindAlternate_HalfwayObstruction(displacement, runId);
m_pathLow.Enqueue(() => m_pathState = PathState.Path_Blocked);
}
}
finally
{
lock_testPath.ReleaseExclusive();
RunItem();
}
}
