/// <summary>
/// Test a path between current position and destination.
/// </summary>
private void TestPath(Vector3D destination, MyEntity destEntity, byte runId, bool isAlternate, bool tryAlternates, bool slowDown = false)
{
if (runId != m_runId)
{
m_logger.debugLog("destination changed, abort", "TestPath()", Logger.severity.DEBUG);
return;
}
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;
}
try
{
if (m_grid != m_navBlock.Grid)
{
m_logger.debugLog("Grid has changed, from " + m_grid.DisplayName + " to " + m_navBlock.Grid.DisplayName + ", nav block: " + m_navBlock.Block.getBestName(), "TestPath()", Logger.severity.WARNING);
return;
}
m_logger.debugLog("Running, (destination:" + destination + ", destEntity: " + destEntity + ", runId :" + runId
+ ", isAlternate: " + isAlternate + ", tryAlternates: " + tryAlternates + ", slowDown: " + slowDown + ")", "TestPath()");
MyEntity obstructing;
Vector3? pointOfObstruction;
if ((isAlternate || tryAlternates) && !m_pathChecker.GravityTest(new LineSegmentD(m_navBlock.WorldPosition, destination), m_destination, out obstructing, out pointOfObstruction))
{
m_pathState = PathState.Searching;
m_logger.debugLog("blocked by gravity at " + pointOfObstruction, "TestGravity()", Logger.severity.DEBUG);
if (tryAlternates)
{
Vector3D point = pointOfObstruction.Value;
Vector3 direction = m_navBlock.WorldPosition - obstructing.GetCentre(); direction.Normalize();
float distance = Vector3.Distance(m_navBlock.WorldPosition, pointOfObstruction.Value);
float minDist = m_navSet.Settings_Current.DestinationRadius;
if (minDist < 1000f)
{
minDist = 1500f;
m_navSet.Settings_Current.DestinationRadius = 1000f;
}
else
minDist *= 1.5f;
m_pathHigh.Clear();
FindAlternate_AroundObstruction(pointOfObstruction.Value, direction, distance, minDist * minDist, runId);
m_pathLow.Enqueue(() => m_pathState = PathState.Path_Blocked);
}
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;
}
if (m_pathChecker.TestSlow(out obstructing, 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()", isAlternate ? Logger.severity.TRACE : Logger.severity.DEBUG);
m_logger.debugLog(obstructing is IMyCubeBlock, "grid: " + obstructing.GetTopMostParent().DisplayName, "TestPath()", isAlternate ? Logger.severity.TRACE : Logger.severity.DEBUG);
if (tryAlternates)
TryAlternates(runId, pointOfObstruction.Value, obstructing);
}
finally
{
lock_testPath.ReleaseExclusive();
RunItem();
}
}
/// <summary>
/// Test a path between current position and destination.
/// </summary>
private void TestPath(Vector3D destination, MyEntity ignoreEntity, byte runId, bool isAlternate, bool tryAlternates, bool slowDown = false)
{
m_logger.debugLog(m_navBlock == null, "m_navBlock == null", Logger.severity.FATAL);
if (runId != m_runId)
{
m_logger.debugLog("destination changed, abort", Logger.severity.DEBUG);
return;
}
if (!lock_testPath.TryAcquireExclusive())
{
m_logger.debugLog("Already running, requeue (destination:" + destination + ", ignoreEntity: " + ignoreEntity.getBestName() + ", runId :" + runId
+ ", isAlternate: " + isAlternate + ", tryAlternates: " + tryAlternates + ", slowDown: " + slowDown + ")");
LockedQueue<Action> queue = isAlternate ? m_pathLow : m_pathHigh;
queue.Enqueue(() => TestPath(destination, ignoreEntity, runId, isAlternate, tryAlternates));
return;
}
try
{
if (m_grid != m_navBlock.Grid)
{
m_logger.debugLog("Grid has changed, from " + m_grid.getBestName() + " to " + m_navBlock.Grid.getBestName() + ", nav block: " + m_navBlock.Block.getBestName(), Logger.severity.WARNING);
return;
}
m_logger.debugLog("Running, (destination:" + destination + ", ignoreEntity: " + ignoreEntity.getBestName() + ", runId :" + runId
+ ", isAlternate: " + isAlternate + ", tryAlternates: " + tryAlternates + ", slowDown: " + slowDown + ")");
MyEntity obstructing;
Vector3? pointOfObstruction;
if (!isAlternate && !m_ignoreAsteroid)
{
if (slowDown)
{
if ((m_planetCheckSpeed.CurrentState & PlanetChecker.State.Blocked) != 0)
{
float speed = Vector3.Distance(m_planetCheckSpeed.ObstructionPoint, m_navBlock.WorldPosition) * 0.1f;
if (speed < 1f)
speed = 1f;
m_navSet.Settings_Task_NavWay.SpeedTarget = speed;
m_logger.debugLog("Path blocked by planet, set SpeedTarget to " + speed + ", obstructed by planet", Logger.severity.TRACE);
return;
}
}
else
{
if ((m_planetCheckDest.CurrentState & PlanetChecker.State.Blocked) != 0 &&
// planet checker is using an older displacement so verify that obstruction is closer than destination
Vector3D.DistanceSquared(m_navBlock.WorldPosition, m_planetCheckDest.ObstructionPoint) < Vector3D.DistanceSquared(m_navBlock.WorldPosition, destination))
{
m_logger.debugLog("path blocked by planet, to destination: " + (destination - m_navBlock.WorldPosition) + ", to obstruction: " + (m_planetCheckDest.ObstructionPoint - m_navBlock.WorldPosition));
if (m_pathState < PathState.Searching)
m_pathState = PathState.Searching;
obstructing = m_planetCheckDest.ClosestPlanet;
Vector3 direction = Vector3.Normalize(m_navBlock.WorldPosition - obstructing.GetCentre());
pointOfObstruction = m_planetCheckDest.ObstructionPoint + direction * 1e3f;
float distance = Vector3.Distance(m_navBlock.WorldPosition, pointOfObstruction.Value);
MoveObstruction = obstructing;
m_pathHigh.Clear();
ClearAltPath();
if ((m_planetCheckDest.CurrentState & PlanetChecker.State.BlockedPath) != 0)
FindAlternate_AroundObstruction(pointOfObstruction.Value - m_navBlock.WorldPosition, new Vector3[] { direction }, 1e4f, runId);
else // blocked by gravity
FindAlternate_AroundObstruction(pointOfObstruction.Value - m_navBlock.WorldPosition, new Vector3[] { direction }, 1e6f, runId);
m_pathLow.Enqueue(() => {
if (m_altPath_AlternatesFound != 0)
SetWaypoint();
RunItem();
});
m_pathLow.Enqueue(() => m_pathState = PathState.Path_Blocked);
return;
}
}
}
// for alternates, check that it can be better than current value
if (isAlternate)
{
float distToWaypointSquared = (float)Vector3D.DistanceSquared(m_navBlock.WorldPosition, destination);
if (distToWaypointSquared * WaypointDistanceBias * WaypointDistanceBias > m_altPath_PathValue * m_altPath_PathValue)
{
m_logger.debugLog("no point in checking alternate path, bias is too high", Logger.severity.TRACE);
m_logger.debugLog(m_altPath_AlternatesFound == 0, "no alternate, yet path value is set", Logger.severity.ERROR);
IncrementAlternatesFound();
return;
}
}
if (m_pathChecker.TestFast(m_navBlock, destination, m_ignoreAsteroid, ignoreEntity, m_landing))
{
m_logger.debugLog("path is clear (fast)", Logger.severity.TRACE);
PathClear(ref destination, runId, isAlternate, slowDown);
return;
}
if (m_pathChecker.TestSlow(out obstructing, out pointOfObstruction))
{
m_logger.debugLog("path is clear (slow)", Logger.severity.TRACE);
PathClear(ref destination, runId, isAlternate, slowDown);
return;
}
if (runId != m_runId)
{
m_logger.debugLog("destination changed, abort", Logger.severity.DEBUG);
return;
}
if (slowDown)
{
float speed = Vector3.Distance(pointOfObstruction.Value, m_navBlock.WorldPosition) * 0.1f;
if (speed < 1f)
speed = 1f;
IMyEntity destEntity = m_navSet.Settings_Current.DestinationEntity;
if (destEntity != null)
destEntity = destEntity.GetTopMostParent();
if (obstructing.GetTopMostParent() == destEntity)
{
m_navSet.Settings_Task_NavWay.SpeedMaxRelative = speed;
m_logger.debugLog("Set SpeedMaxRelative to " + speed + ", obstructing: " + obstructing.getBestName() + ", DestinationEntity: " + m_navSet.Settings_Current.DestinationEntity, Logger.severity.TRACE);
}
else
{
m_navSet.Settings_Task_NavWay.SpeedTarget = speed;
m_logger.debugLog("Set SpeedTarget to " + speed + ", obstructing: " + obstructing.getBestName() + ", DestinationEntity: " + m_navSet.Settings_Current.DestinationEntity, Logger.severity.TRACE);
}
return;
}
if (m_pathState < PathState.Searching)
m_pathState = PathState.Searching;
m_logger.debugLog("path is blocked by " + obstructing.getBestName() + " at " + pointOfObstruction + ", ignoreEntity: " + ignoreEntity.getBestName(), isAlternate ? Logger.severity.TRACE : Logger.severity.DEBUG);
m_logger.debugLog(obstructing is IMyCubeBlock, "grid: " + obstructing.GetTopMostParent().DisplayName, isAlternate ? Logger.severity.TRACE : Logger.severity.DEBUG);
if (isAlternate && m_altPath_AlternatesFound != 0)
IncrementAlternatesFound();
if (tryAlternates)
{
//float angle = m_navSet.Settings_Current.DistanceAngle;
//if (angle > 0.1f && CanRotate)
//{
// m_logger.debugLog("wait for rotation", "TestPath()");
// return;
//}
if (m_navSet.Settings_Task_NavEngage.NavigatorMover != m_navSet.Settings_Current.NavigatorMover)
{
m_logger.debugLog("obstructed while flying to a waypoint, throwing it out and starting over", Logger.severity.DEBUG);
m_navSet.OnTaskComplete_NavWay();
return;
}
ClearAltPath();
MoveObstruction = obstructing;
TryAlternates(runId, pointOfObstruction.Value, obstructing);
}
}
finally
{
lock_testPath.ReleaseExclusive();
RunItem();
}
}