public Pathfinder(IMyCubeGrid grid)
{
grid.throwIfNull_argument("grid");
CubeGrid = grid;
myPathChecker = new PathChecker(grid);
myLogger = new Logger("Pathfinder", () => grid.DisplayName);
myOutput = new PathfinderOutput(myPathChecker, PathfinderOutput.Result.Incomplete);
}
public Pathfinder(IMyCubeGrid grid, AllNavigationSettings navSet, Mover mover)
{
grid.throwIfNull_argument("grid");
m_grid = grid;
m_logger = new Logger(() => grid.DisplayName, () => m_pathState.ToString(), () => m_rotateState.ToString());
m_pathChecker = new PathChecker(grid);
m_rotateChecker = new RotateChecker(grid);
m_navSet = navSet;
m_mover = mover;
m_planetCheckDest = new PlanetChecker(grid);
m_planetCheckSpeed = new PlanetChecker(grid);
m_logger.debugLog("Initialized, grid: " + grid.DisplayName);
}
//public readonly bool AllowsMovement;
public PathfinderOutput(PathChecker getClosestFrom, Result PathfinderResult, IMyEntity Obstruction = null, Vector3D Waypoint = new Vector3D())
{
this.PathfinderResult = PathfinderResult;
this.Obstruction = Obstruction;
this.Waypoint = Waypoint;
switch (PathfinderResult)
{
case Result.Incomplete:
case Result.Path_Clear:
case Result.Alternate_Path:
//AllowsMovement = true;
//this.DistanceToClosest = getClosestFrom.ClosestEntity();
this.lazy_DistanceToClosest = new Lazy <double>(() => { return(getClosestFrom.ClosestEntity()); });
break;
case Result.Searching_Alt:
case Result.No_Way_Forward:
//AllowsMovement = false;
//this.DistanceToClosest = PathChecker.NearbyRange;
this.lazy_DistanceToClosest = new Lazy <double>(() => { return(PathChecker.NearbyRange); });
break;
}
}
public Pathfinder(IMyCubeGrid grid, AllNavigationSettings navSet, Mover mover)
{
grid.throwIfNull_argument("grid");
m_grid = grid;
m_logger = new Logger("Pathfinder", () => grid.DisplayName, () => m_pathState.ToString(), () => m_rotateState.ToString());
m_pathChecker = new PathChecker(grid);
m_rotateChecker = new RotateChecker(grid);
m_navSet = navSet;
m_mover = mover;
m_logger.debugLog("Initialized, grid: " + grid.DisplayName, "Pathfinder()");
}
/// <summary>
/// Test if it is safe for the grid to rotate.
/// </summary>
/// <param name="axis">Normalized axis of rotation in world space.</param>
/// <param name="ignoreAsteroids"></param>
/// <returns>True iff the path is clear.</returns>
public bool TestRotate(Vector3 axis, bool ignoreAsteroids, out IMyEntity obstruction)
{
Vector3 centreOfMass = m_grid.Physics.CenterOfMassWorld;
float longestDim = m_grid.GetLongestDim();
// calculate height
Matrix toMyLocal = m_grid.WorldMatrixNormalizedInv;
Vector3 myLocalCoM = Vector3.Transform(centreOfMass, toMyLocal);
Vector3 myLocalAxis = Vector3.Transform(axis, toMyLocal.GetOrientation());
Vector3 myLocalCentre = m_grid.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 = m_grid.LocalAABB.Intersects(upper);
if (!upperBound.HasValue)
{
m_logger.alwaysLog("Math fail, upperBound does not have a value", Logger.severity.FATAL);
}
Ray lower = new Ray(myLocalCentre - myLocalAxis * longestDim * 2f, myLocalAxis);
float?lowerBound = m_grid.LocalAABB.Intersects(lower);
if (!lowerBound.HasValue)
{
m_logger.alwaysLog("Math fail, lowerBound does not have a value", Logger.severity.FATAL);
}
m_logger.debugLog("LocalAABB: " + m_grid.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;
m_cells.ForEach(cell => {
Vector3 rejection = Vector3.Reject(cell * m_grid.GridSize, myLocalAxis);
float cellDistSquared = Vector3.DistanceSquared(myLocalCoM, rejection);
if (cellDistSquared > furthest)
{
furthest = cellDistSquared;
}
});
float length = (float)Math.Sqrt(furthest) + m_grid.GridSize * 0.5f;
m_logger.debugLog("height: " + height + ", length: " + length);
BoundingSphereD surroundingSphere = new BoundingSphereD(centreOfMass, Math.Max(length, height));
m_obstructions.Clear();
MyGamePruningStructure.GetAllTopMostEntitiesInSphere(ref surroundingSphere, m_obstructions);
LineSegment axisSegment = new LineSegment();
ClosestPlanet = MyPlanetExtensions.GetClosestPlanet(centreOfMass);
MyAPIGateway.Utilities.TryInvokeOnGameThread(TestPlanet);
foreach (MyEntity entity in m_obstructions)
{
if (PathChecker.collect_Entity(m_grid, entity))
{
if (entity is IMyVoxelBase)
{
if (ignoreAsteroids)
{
continue;
}
IMyVoxelMap voxel = entity as IMyVoxelMap;
if (voxel != null)
{
if (voxel.GetIntersectionWithSphere(ref surroundingSphere))
{
m_logger.debugLog("Too close to " + voxel.getBestName() + ", CoM: " + centreOfMass.ToGpsTag("Centre of Mass") + ", required distance: " + surroundingSphere.Radius);
obstruction = voxel;
return(false);
}
continue;
}
if (PlanetState != Pathfinder.PathState.No_Obstruction)
{
m_logger.debugLog("planet blocking");
obstruction = 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;
bool found = false;
GridCellCache.GetCellCache(grid).ForEach(cell => {
if (axisSegment.PointInCylinder(length, cell * grid.GridSize))
{
found = true;
return(true);
}
return(false);
});
if (found)
{
obstruction = grid;
return(false);
}
continue;
}
m_logger.debugLog("No tests for object: " + entity.getBestName(), Logger.severity.INFO);
obstruction = entity;
return(false);
}
}
obstruction = null;
return(true);
}