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