/// <summary>
/// Test if it is safe for the grid to rotate.
/// </summary>
/// <param name="axis">Normalized axis of rotation in world space.</param>
/// <returns>True iff the path is clear.</returns>
private bool in_TestRotate(Vector3 axis)
{
IMyCubeGrid myGrid = m_block.CubeGrid;
Vector3 centreOfMass = myGrid.Physics.CenterOfMassWorld;
float longestDim = myGrid.GetLongestDim();
// calculate height
Matrix toMyLocal = myGrid.WorldMatrixNormalizedInv;
Vector3 myLocalCoM = Vector3.Transform(centreOfMass, toMyLocal);
Vector3 myLocalAxis = Vector3.Transform(axis, toMyLocal.GetOrientation());
Vector3 myLocalCentre = myGrid.LocalAABB.Center; // CoM may not be on ship (it now considers mass from attached grids)
Ray upper = new Ray(myLocalCentre + myLocalAxis * longestDim * 2f, -myLocalAxis);
float? upperBound = myGrid.LocalAABB.Intersects(upper);
if (!upperBound.HasValue)
{
Log.AlwaysLog("Math fail, upperBound does not have a value", Logger.severity.FATAL);
}
Ray lower = new Ray(myLocalCentre - myLocalAxis * longestDim * 2f, myLocalAxis);
float?lowerBound = myGrid.LocalAABB.Intersects(lower);
if (!lowerBound.HasValue)
{
Log.AlwaysLog("Math fail, lowerBound does not have a value", Logger.severity.FATAL);
}
//Log.DebugLog("LocalAABB: " + myGrid.LocalAABB + ", centre: " + myLocalCentre + ", axis: " + myLocalAxis + ", longest dimension: " + longestDim + ", upper ray: " + upper + ", lower ray: " + lower);
float height = longestDim * 4f - upperBound.Value - lowerBound.Value;
float furthest = 0f;
foreach (IMyCubeGrid grid in AttachedGrid.AttachedGrids(myGrid, Attached.AttachedGrid.AttachmentKind.Physics, true))
{
CubeGridCache cache = CubeGridCache.GetFor(grid);
if (cache == null)
{
return(false);
}
foreach (Vector3I cell in cache.OccupiedCells())
{
Vector3 rejection = Vector3.Reject(cell * myGrid.GridSize, myLocalAxis);
float cellDistSquared = Vector3.DistanceSquared(myLocalCoM, rejection);
if (cellDistSquared > furthest)
{
furthest = cellDistSquared;
}
}
}
float length = (float)Math.Sqrt(furthest) + myGrid.GridSize;
//Log.DebugLog("height: " + height + ", length: " + length);
BoundingSphereD surroundingSphere = new BoundingSphereD(centreOfMass, Math.Max(length, height) * MathHelper.Sqrt2);
m_obstructions.Clear();
MyGamePruningStructure.GetAllTopMostEntitiesInSphere(ref surroundingSphere, m_obstructions);
LineSegment axisSegment = new LineSegment();
m_closestPlanet = null;
foreach (MyEntity entity in m_collector.Invoke(m_obstructions))
{
if (entity is IMyVoxelBase)
{
IMyVoxelMap voxel = entity as IMyVoxelMap;
if (voxel != null)
{
if (voxel.GetIntersectionWithSphere(ref surroundingSphere))
{
Log.DebugLog("Too close to " + voxel.getBestName() + ", CoM: " + centreOfMass.ToGpsTag("Centre of Mass") + ", required distance: " + surroundingSphere.Radius);
ObstructingEntity = voxel;
return(false);
}
continue;
}
if (m_closestPlanet == null)
{
MyPlanet planet = entity as MyPlanet;
if (planet == null)
{
continue;
}
double distToPlanetSq = Vector3D.DistanceSquared(centreOfMass, planet.PositionComp.GetPosition());
if (distToPlanetSq < planet.MaximumRadius * planet.MaximumRadius)
{
m_closestPlanet = planet;
if (m_planetObstruction)
{
Log.DebugLog("planet blocking");
ObstructingEntity = m_closestPlanet;
return(false);
}
}
}
continue;
}
IMyCubeGrid grid = entity as IMyCubeGrid;
if (grid != null)
{
Matrix toLocal = grid.WorldMatrixNormalizedInv;
Vector3 localAxis = Vector3.Transform(axis, toLocal.GetOrientation());
Vector3 localCentre = Vector3.Transform(centreOfMass, toLocal);
axisSegment.From = localCentre - localAxis * height;
axisSegment.To = localCentre + localAxis * height;
CubeGridCache cache = CubeGridCache.GetFor(grid);
if (cache == null)
{
return(false);
}
foreach (Vector3I cell in cache.OccupiedCells())
{
if (axisSegment.PointInCylinder(length, cell * grid.GridSize))
{
Log.DebugLog("axis segment: " + axisSegment.From + " to " + axisSegment.To + ", radius: " + length + ", hit " + grid.nameWithId() + " at " + cell);
ObstructingEntity = grid;
return(false);
}
}
continue;
}
Log.DebugLog("No tests for object: " + entity.getBestName(), Logger.severity.INFO);
ObstructingEntity = entity;
return(false);
}
MyAPIGateway.Utilities.TryInvokeOnGameThread(TestPlanet);
ObstructingEntity = null;
return(true);
}
/// <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);
}