/// <summary> /// Sphere test for simple entities, like floating objects, and for avoiding dangerous ships tools. /// </summary> /// <param name="entity">The entity to test.</param> /// <param name="input"><see cref="TestInput"/></param> /// <param name="result"><see cref="GridTestResult"/></param> /// <returns>True if the entity is obstructing the ship.</returns> private bool SphereTest(MyEntity entity, ref TestInput input, ref GridTestResult result) { Vector3D currentPosition = AutopilotGrid.GetCentre(); Vector3D rejectD = input.Direction; Vector3D disp; Vector3D.Multiply(ref rejectD, input.Length, out disp); Vector3D finalPosition; Vector3D.Add(ref currentPosition, ref disp, out finalPosition); float shipRadius = AutopilotGrid.PositionComp.LocalVolume.Radius; Vector3D obsPos = entity.GetCentre(); Vector3D offObsPos; Vector3D.Subtract(ref obsPos, ref input.Offset, out offObsPos); float obsRadius = entity.PositionComp.LocalVolume.Radius; if (entity is MyCubeBlock) { // it is an active tool so increase the radius obsRadius += 10f; } m_lineSegment.From = currentPosition; m_lineSegment.To = finalPosition; Vector3D closest; result.Distance = (float)m_lineSegment.ClosestPoint(ref offObsPos, out closest); double distance; Vector3D.Distance(ref offObsPos, ref closest, out distance); result.Proximity = (float)distance - (shipRadius + obsRadius); if (result.Proximity <= 0f && IsRejectionTowards(ref obsPos, ref currentPosition, ref rejectD)) { Logger.DebugLog("Rejection " + input.Direction + " hit " + entity.nameWithId()); result.ObstructingBlock = entity as MyCubeBlock; // it may not be a block return(true); } return(false); }
/// <summary> /// Tests if autopilot's ship would endanger a grid with a ship tool. /// </summary> /// <param name="grid">The grid that is potentially endangered.</param> /// <param name="input"><see cref="TestInput"/></param> /// <param name="result"><see cref="GridTestResult"/></param> /// True if the ship would endanger grid. private bool EndangerGrid(MyCubeGrid grid, ref TestInput input, ref GridTestResult result) { Vector3D currentPosition = AutopilotGrid.GetCentre(); Vector3D obstructPositon = grid.GetCentre(); Vector3D rejectD = input.Direction; Vector3D gridPosition = grid.GetCentre() - input.Offset; float gridRadius = grid.PositionComp.LocalVolume.Radius; Vector3 rejectDispF; Vector3.Multiply(ref input.Direction, input.Length, out rejectDispF); Vector3D rejectDisp = rejectDispF; IEnumerable <CubeGridCache> myCaches = AttachedGrid.AttachedGrids(AutopilotGrid, AttachedGrid.AttachmentKind.Terminal, true).Select(CubeGridCache.GetFor); foreach (CubeGridCache cache in myCaches) { if (cache == null) { Logger.DebugLog("Missing a cache", Logger.severity.DEBUG); return(true); } foreach (MyShipDrill drill in cache.BlocksOfType(typeof(MyObjectBuilder_Drill))) { if (drill.IsShooting && ToolObstructed(drill, ref gridPosition, gridRadius, ref rejectDisp, ref result) && IsRejectionTowards(ref obstructPositon, ref currentPosition, ref rejectD)) { return(true); } } foreach (MyShipGrinder grinder in cache.BlocksOfType(typeof(MyObjectBuilder_ShipGrinder))) { if (grinder.IsShooting && ToolObstructed(grinder, ref gridPosition, gridRadius, ref rejectDisp, ref result) && IsRejectionTowards(ref obstructPositon, ref currentPosition, ref rejectD)) { return(true); } } } return(false); }
/// <summary> /// Tests if a specified tool will endager a grid at gridPosition. /// </summary> /// <param name="tool">The hazardous tool.</param> /// <param name="gridPosition">The positon of the grid.</param> /// <param name="gridRadius">The radius around gridPosition which the tool must not intersect.</param> /// <param name="rejectDisp">The displacement of travel.</param> /// <param name="result"><see cref="GridTestResult"/></param> /// <returns>True if tool would endanger a grid at gridPosition.</returns> private bool ToolObstructed(MyCubeBlock tool, ref Vector3D gridPosition, float gridRadius, ref Vector3D rejectDisp, ref GridTestResult result) { m_lineSegment.From = tool.PositionComp.GetPosition(); m_lineSegment.To = m_lineSegment.From + rejectDisp; Vector3D closest; result.Distance = (float)m_lineSegment.ClosestPoint(ref gridPosition, out closest); double distance; Vector3D.Distance(ref gridPosition, ref closest, out distance); result.Proximity = (float)distance - (gridRadius + tool.PositionComp.LocalVolume.Radius + 5f); return(result.Proximity <= 0f); }
/// <summary> /// Tests a grid for obstructing the ship via vector rejection. /// </summary> /// <param name="oGrid">The grid that may obstruct this one.</param> /// <param name="ignoreBlock">Block to ignore, or null</param> /// <param name="input"><see cref="TestInput"/></param> /// <param name="result"><see cref="GridTestResult"/></param> /// <returns>True if oGrid is blocking the ship.</returns> private bool RejectionIntersects(MyCubeGrid oGrid, MyCubeBlock ignoreBlock, ref TestInput input, ref GridTestResult result) { //Logger.DebugLog("Rejection vector is not normalized, length squared: " + rejectionVector.LengthSquared(), Logger.severity.FATAL, condition: Math.Abs(rejectionVector.LengthSquared() - 1f) > 0.001f); //Logger.DebugLog("Testing for rejection intersection: " + oGrid.nameWithId() + ", starting from: " + (AutopilotGrid.GetCentre() + offset) + ", rejection vector: " + rejectionVector + ", distance: " + rejectionDistance + // ", final: " + (AutopilotGrid.GetCentre() + offset + rejectionVector * rejectionDistance)); //Logger.DebugLog("rejction distance < 0: " + rejectionDistance, Logger.severity.ERROR, condition: rejectionDistance < 0f); IEnumerable <CubeGridCache> myCaches = AttachedGrid.AttachedGrids(AutopilotGrid, AttachedGrid.AttachmentKind.Physics, true).Select(CubeGridCache.GetFor); Vector3D currentPosition = AutopilotGrid.GetCentre(); CubeGridCache oCache = CubeGridCache.GetFor(oGrid); if (oCache == null) { Logger.DebugLog("Failed to get cache for other grid", Logger.severity.DEBUG); return(false); } bool checkBlock = ignoreBlock != null && oGrid == ignoreBlock.CubeGrid; Vector3 v; input.Direction.CalculatePerpendicularVector(out v); Vector3 w; Vector3.Cross(ref input.Direction, ref v, out w); Matrix to3D = new Matrix(v.X, v.Y, v.Z, 0f, w.X, w.Y, w.Z, 0f, input.Direction.X, input.Direction.Y, input.Direction.Z, 0f, 0f, 0f, 0f, 1f); Matrix to2D; Matrix.Invert(ref to3D, out to2D); float roundTo; int minDistanceSquared; if (AutopilotGrid.GridSizeEnum == oGrid.GridSizeEnum) { roundTo = AutopilotGrid.GridSize; minDistanceSquared = 1; } else { roundTo = Math.Min(AutopilotGrid.GridSize, oGrid.GridSize); minDistanceSquared = (int)Math.Ceiling(Math.Max(AutopilotGrid.GridSize, oGrid.GridSize) / roundTo); minDistanceSquared *= minDistanceSquared; } int maxDistanceSquared = minDistanceSquared * 100; Profiler.StartProfileBlock("RejectionIntersects:Build ship"); Vector2IMatrix <bool> apShipRejections; ResourcePool.Get(out apShipRejections); MatrixD worldMatrix = AutopilotGrid.WorldMatrix; float gridSize = AutopilotGrid.GridSize; float minProjection = float.MaxValue, maxProjection = float.MinValue; // the permitted range when rejecting the other grids cells foreach (CubeGridCache cache in myCaches) { if (cache == null) { Logger.DebugLog("Missing a cache", Logger.severity.DEBUG); Profiler.EndProfileBlock(); return(false); } foreach (Vector3I cell in cache.OccupiedCells()) { Vector3 local = cell * gridSize; MyCubeBlock block = (MyCubeBlock)cache.CubeGrid.GetCubeBlock(cell)?.FatBlock; if (block != null && block.Subparts != null && block.Subparts.Count != 0 && !CellOccupiedByBlock(cell, block)) { continue; } Vector3D world; Vector3D.Transform(ref local, ref worldMatrix, out world); Vector3D relative; Vector3D.Subtract(ref world, ref currentPosition, out relative); Vector3 relativeF = relative; float projectionDistance; Vector3 rejection; VectorExtensions.RejectNormalized(ref relativeF, ref input.Direction, out projectionDistance, out rejection); if (projectionDistance < minProjection) { minProjection = projectionDistance; } else if (projectionDistance > maxProjection) { maxProjection = projectionDistance; } Vector3 planarComponents; Vector3.Transform(ref rejection, ref to2D, out planarComponents); //Logger.DebugLog("Math fail: rejection: " + rejection + ", planar components: " + planarComponents + "\nto3D: " + to3D, Logger.severity.WARNING, condition: planarComponents.Z > 0.001f || planarComponents.Z < -0.001f); Vector2 pc2 = new Vector2(planarComponents.X, planarComponents.Y); apShipRejections[ToCell(pc2, roundTo)] = true; //Logger.DebugLog("My rejection: " + rejection + ", planar: " + ToCell(pc2, roundTo)); } } Profiler.EndProfileBlock(); minProjection += StartRayCast; // allow autopilot to move away from a touching object //Logger.DebugLog("projection min: " + minProjection + ", max: " + maxProjection + ", max for other: " + (maxProjection + rejectionDistance)); maxProjection += input.Length; //Logger.DebugLog("checking other grid cells"); Profiler.StartProfileBlock("RejectionIntersects:other grid"); Vector2IMatrix <bool> otherGridRejections; ResourcePool.Get(out otherGridRejections); worldMatrix = oGrid.WorldMatrix; gridSize = oGrid.GridSize; foreach (Vector3I cell in oCache.OccupiedCells()) { //Logger.DebugLog("cell: " + cell); Vector3 local = cell * gridSize; Vector3D world; Vector3D.Transform(ref local, ref worldMatrix, out world); Vector3D offsetWorld; Vector3D.Subtract(ref world, ref input.Offset, out offsetWorld); Vector3D relative; Vector3D.Subtract(ref offsetWorld, ref currentPosition, out relative); Vector3 relativeF = relative; Vector3 rejection; VectorExtensions.RejectNormalized(ref relativeF, ref input.Direction, out result.Distance, out rejection); if (result.Distance < minProjection || result.Distance > maxProjection) { continue; } Vector3 planarComponents; Vector3.Transform(ref rejection, ref to2D, out planarComponents); //Logger.DebugLog("Math fail: rejection: " + rejection + ", planar components: " + planarComponents + "\nto3D: " + to3D, Logger.severity.WARNING, condition: planarComponents.Z > 0.001f || planarComponents.Z < -0.001f); Vector2 pc2 = new Vector2(planarComponents.X, planarComponents.Y); Vector2I cell2D = ToCell(pc2, roundTo); if (!otherGridRejections.Add(cell2D, true)) { //Logger.DebugLog("Already tested: " + cell2D); continue; } //Logger.DebugLog("Rejection: " + rejection + ", planar: " + cell2D); //Logger.DebugLog("testing range. x: " + (cell2D.X - steps) + " - " + (cell2D.X + steps)); ExpandingRings.Ring ring = default(ExpandingRings.Ring); for (int ringIndex = 0; ring.DistanceSquared <= maxDistanceSquared; ringIndex++) { ring = ExpandingRings.GetRing(ringIndex); for (int squareIndex = 0; squareIndex < ring.Squares.Length; squareIndex++) { if (apShipRejections.Contains(cell2D + ring.Squares[squareIndex])) { if (ring.DistanceSquared <= minDistanceSquared) { IMySlimBlock slim = oGrid.GetCubeBlock(cell); if (slim != null) { if (checkBlock && slim.FatBlock == ignoreBlock) { continue; } MyCubeBlock fat = (MyCubeBlock)slim.FatBlock; if (fat != null && fat.Subparts != null && fat.Subparts.Count != 0 && !CellOccupiedByBlock(cell, fat)) { continue; } result.ObstructingBlock = fat; } else { result.ObstructingBlock = null; } result.Proximity = 0f; Logger.DebugLog("Hit, projectionDistance: " + result.Distance + ", min: " + minProjection + ", max: " + maxProjection + ", ring: " + ringIndex + ", ring dist sq: " + ring.DistanceSquared + ", min dist sq: " + minDistanceSquared + ", max dist sq: " + maxDistanceSquared); Profiler.EndProfileBlock(); apShipRejections.Clear(); otherGridRejections.Clear(); ResourcePool.Return(apShipRejections); ResourcePool.Return(otherGridRejections); return(true); } else { maxDistanceSquared = ring.DistanceSquared; goto NextCell; } } } } NextCell :; } Profiler.EndProfileBlock(); apShipRejections.Clear(); otherGridRejections.Clear(); ResourcePool.Return(apShipRejections); ResourcePool.Return(otherGridRejections); result.Proximity = (float)Math.Sqrt(maxDistanceSquared); return(false); }
/// <summary> /// Tests if a section of space can be travelled without hitting the specified entity. /// </summary> /// <param name="entity">The potential obstruction</param> /// <param name="ignoreBlock">Null or the block autopilot is trying to connect with.</param> /// <param name="input"><see cref="TestInput"/></param> /// <param name="result"><see cref="GridTestResult"/></param> /// <returns>True if the specified entity obstructs the path.</returns> public bool ObstructedBy(MyEntity entity, MyCubeBlock ignoreBlock, ref TestInput input, out GridTestResult result) { //Logger.DebugLog("checking: " + entity.getBestName() + ", offset: " + offset + ", rejection vector: " + rejectionVector + ", rejection distance: " + rejectionDistance); #if DEBUG if (!input.Direction.IsValid() || Math.Abs(1f - input.Direction.LengthSquared()) > 0.01f) { throw new Exception("rejection vector is invalid. entity: " + entity.nameWithId() + ", input: " + input); } #endif result = GridTestResult.Default; result.Distance = input.Length; MyCubeGrid grid = entity as MyCubeGrid; if (grid != null) { // check for dangerous tools on grid CubeGridCache cache = CubeGridCache.GetFor(grid); if (cache == null) { return(false); } Profiler.StartProfileBlock("Checking Tools"); foreach (MyShipDrill drill in cache.BlocksOfType(typeof(MyObjectBuilder_Drill))) { if (drill.IsShooting) { if (SphereTest(drill, ref input, ref result)) { Profiler.EndProfileBlock(); return(true); } } } foreach (MyShipGrinder grinder in cache.BlocksOfType(typeof(MyObjectBuilder_ShipGrinder))) { if (grinder.IsShooting) { if (SphereTest(grinder, ref input, ref result)) { Profiler.EndProfileBlock(); return(true); } } } Profiler.EndProfileBlock(); if (ExtensionsRelations.canConsiderFriendly(Controller.CubeBlock, grid) && EndangerGrid(grid, ref input, ref result)) { Logger.DebugLog("Movement would endanger: " + grid.getBestName()); return(true); } Profiler.StartProfileBlock("RejectionIntersects"); if (RejectionIntersects(grid, ignoreBlock, ref input, ref result)) { Profiler.EndProfileBlock(); return(true); } Profiler.EndProfileBlock(); } else { return(SphereTest(entity, ref input, ref result)); } return(false); }