/// <summary>
/// Test the path for obstructions
/// </summary>
/// <exception cref="InterruptException">If interrupted</exception>
/// I considered keeping track of the closest entity, in the event there was no obstruction. This would have been, at best, unreliable due to initial AABB test.
public IMyEntity TestPath(Vector3D worldDestination, IMyCubeBlock navigationBlock, bool IgnoreAsteroids, out Vector3?pointOfObstruction, IMyCubeGrid DestGrid)
{
worldDestination.throwIfNull_argument("destination");
worldDestination.throwIfNull_argument("navigationBlock");
Interrupt = false;
this.NavigationBlock = navigationBlock;
this.IgnoreAsteroids = IgnoreAsteroids;
this.DestGrid = DestGrid;
myLogger.debugLog("Test path to (world absolute) " + worldDestination, "TestPath()");
//myLogger.debugLog("destination (local) = " + worldDestination.getLocal(), "TestPath()");
//myLogger.debugLog("destination (nav block) = " + worldDestination.getBlock(navigationBlock), "TestPath()");
Vector3D Displacement = worldDestination - navigationBlock.GetPosition();
myLogger.debugLog("Displacement = " + Displacement, "TestPath()");
// entities in large AABB
BoundingBoxD AtDest = myCubeGrid.WorldAABB.Translate(Displacement);
ICollection <IMyEntity> offenders = EntitiesInLargeAABB(myCubeGrid.WorldAABB, AtDest);
if (offenders.Count == 0)
{
myLogger.debugLog("AABB is empty", "TestPath()", Logger.severity.DEBUG);
pointOfObstruction = null;
return(null);
}
myLogger.debugLog("collected entities to test: " + offenders.Count, "TestPath()");
// sort offenders by distance
offenders = SortByDistance(offenders);
// set destination
GridShapeProfiler myGridShape = GridShapeProfiler.getFor(myCubeGrid);
//myLogger.debugLog("destination = " + worldDestination.getWorldAbsolute() + ", navigationBlock = " + navigationBlock.GetPosition(), "TestPath()");
myGridShape.SetDestination(RelativeVector3F.createFromWorldAbsolute(worldDestination, myCubeGrid), navigationBlock);
myPath = myGridShape.myPath;
myLogger.debugLog("got path from " + myPath.P0 + " to " + myPath.P1 + " with radius " + myPath.Radius, "TestPath()");
// test path
return(TestEntities(offenders, myPath, myGridShape, out pointOfObstruction, this.DestGrid));
}
/// <param name="offenders">entities to test</param>
/// <param name="myGridShape">iff null, skip rejection test</param>
private IMyEntity TestEntities(ICollection <IMyEntity> offenders, Capsule path, GridShapeProfiler myGridShape, out Vector3?pointOfObstruction, IMyCubeGrid GridDestination)
{
foreach (IMyEntity entity in offenders)
{
CheckInterrupt();
myLogger.debugLog("testing offender: " + entity.getBestName() + " at " + entity.GetPosition(), "TestEntities()");
IMyCubeGrid asGrid = entity as IMyCubeGrid;
if (asGrid != null)
{
if (asGrid == GridDestination)
{
myLogger.debugLog("grid is destination: " + asGrid.DisplayName, "TestEntities()");
continue;
}
if (!path.IntersectsAABB(entity))
{
myLogger.debugLog("no AABB intersection: " + asGrid.DisplayName, "TestEntities()");
continue;
}
myLogger.debugLog("searching blocks of " + entity.getBestName(), "TestEntities()");
uint cellCount = 0, cellRejectedCount = 0;
// foreach block
float GridSize = asGrid.GridSize;
List <IMySlimBlock> allSlims = new List <IMySlimBlock>();
asGrid.GetBlocks_Safe(allSlims);
foreach (IMySlimBlock slim in allSlims)
{
bool blockIntersects = false;
Vector3 cellPosWorld = new Vector3();
//myLogger.debugLog("slim = " + slim.getBestName() + ", fat = " + slim.FatBlock + ", cell = " + slim.Position, "TestEntities()");
//if (slim.FatBlock != null)
//{
// myLogger.debugLog("fatblock min = " + slim.FatBlock.Min + ", fatblock max = " + slim.FatBlock.Max, "TestEntities()");
// //myLogger.debugLog("fatblock AABB min = " + slim.FatBlock.LocalAABB.Min + ", fatblock AABB max = " + slim.FatBlock.LocalAABB.Max, "TestEntities()");
//}
slim.ForEachCell((cell) => {
CheckInterrupt();
cellPosWorld = asGrid.GridIntegerToWorld(cell);
cellCount++;
//myLogger.debugLog("slim = " + slim.getBestName() + ", cell = " + cell + ", world position = " + cellPosWorld, "TestEntities()");
// intersects capsule
if (!path.Intersects(cellPosWorld, GridSize))
{
return(false);
}
// rejection
cellRejectedCount++;
if (myGridShape == null || myGridShape.rejectionIntersects(RelativeVector3F.createFromWorldAbsolute(cellPosWorld, myCubeGrid), myCubeGrid.GridSize))
{
myLogger.debugLog("obstructing grid = " + asGrid.DisplayName + ", cell = " + cellPosWorld + ", block = " + slim.getBestName(), "TestEntities()", Logger.severity.DEBUG);
blockIntersects = true;
return(true);
}
//myLogger.debugLog("rejection: no collision, cell = " + cellPosWorld + ", block = " + slim.getBestName(), "TestEntities()", Logger.severity.DEBUG);
return(false);
});
if (blockIntersects)
{
//myLogger.debugLog("closest point on line: {" + path.get_Line().From + ", " + path.get_Line().To + "} to " + cellPosWorld + " is " + path.get_Line().ClosestPoint(cellPosWorld), "TestEntities()");
pointOfObstruction = path.get_Line().ClosestPoint(cellPosWorld);
return(entity);
}
}
myLogger.debugLog("no obstruction for grid " + asGrid.DisplayName + ", tested " + cellCount + " against capsule and " + cellRejectedCount + " against rejection", "TestPath()");
continue;
}
// not a grid
if (IgnoreAsteroids && entity is IMyVoxelMap)
{
myLogger.debugLog("Ignoring asteroid: " + entity.getBestName(), "TestEntities()");
continue;
}
myLogger.debugLog("not a grid, testing bounds", "TestEntities()");
if (!path.IntersectsAABB(entity))
{
continue;
}
if (!path.IntersectsVolume(entity))
{
continue;
}
myLogger.debugLog("no more tests for non-grids are implemented", "TestEntities()", Logger.severity.DEBUG);
//myLogger.debugLog("closest point on line: {" + path.get_Line().From + ", " + path.get_Line().To + "} to " + entity.GetCentre() + " is " + path.get_Line().ClosestPoint(entity.GetCentre()), "TestEntities()");
pointOfObstruction = path.get_Line().ClosestPoint(entity.GetCentre());
return(entity);
}
myLogger.debugLog("no obstruction was found", "TestPath()", Logger.severity.DEBUG);
pointOfObstruction = null;
return(null);
}
public PathChecker(IMyCubeGrid grid)
{
this.m_logger = new Logger(() => m_grid.DisplayName);
this.m_grid = grid;
this.m_profiler = new GridShapeProfiler();
}
