private void FillAirDensity()
{
#if DEBUG_IN_SPACE
m_airDensity = 1f;
#else
Vector3D gridCentre = m_grid.GetCentre();
MyPlanet closestPlanet = MyPlanetExtensions.GetClosestPlanet(gridCentre);
if (closestPlanet == null)
{
m_airDensity = 0f;
}
else
{
m_airDensity = closestPlanet.GetAirDensity(gridCentre);
}
#endif
}
private void TestPlanet()
{
MyPlanet planet = m_closestPlanet;
if (planet == null)
{
return;
}
IMyCubeGrid grid = m_block.CubeGrid;
Vector3D myPos = grid.GetCentre();
Vector3D planetCentre = planet.GetCentre();
double distSqToPlanet = Vector3D.DistanceSquared(myPos, planetCentre);
if (distSqToPlanet > planet.MaximumRadius * planet.MaximumRadius)
{
Log.DebugLog("higher than planet maximum");
m_planetObstruction = false;
return;
}
Vector3D closestPoint = planet.GetClosestSurfacePointGlobal(ref myPos);
if (distSqToPlanet < Vector3D.DistanceSquared(closestPoint, planetCentre))
{
Log.DebugLog("below surface");
return;
}
float longest = grid.GetLongestDim();
if (Vector3D.DistanceSquared(myPos, closestPoint) < longest * longest)
{
Log.DebugLog("near surface");
m_planetObstruction = true;
return;
}
Log.DebugLog("clear");
m_planetObstruction = false;
return;
}
private void TestPlanet()
{
MyPlanet planet = ClosestPlanet;
if (planet == null)
{
return;
}
Vector3D myPos = m_grid.GetCentre();
Vector3D planetCentre = planet.GetCentre();
double distSqToPlanet = Vector3D.DistanceSquared(myPos, planetCentre);
if (distSqToPlanet > planet.MaximumRadius * planet.MaximumRadius)
{
m_logger.debugLog("higher than planet maximum");
PlanetState = Pathfinder.PathState.No_Obstruction;
return;
}
using (lock_closestPoint.AcquireExclusiveUsing())
m_closestPoint = planet.GetClosestSurfacePointGlobal(ref myPos);
if (distSqToPlanet < Vector3D.DistanceSquared(m_closestPoint, planetCentre))
{
m_logger.debugLog("below surface");
PlanetState = Pathfinder.PathState.Path_Blocked;
return;
}
float longest = m_grid.GetLongestDim();
if (Vector3D.DistanceSquared(myPos, m_closestPoint) < longest * longest)
{
m_logger.debugLog("near surface");
PlanetState = Pathfinder.PathState.Path_Blocked;
return;
}
m_logger.debugLog("clear");
PlanetState = Pathfinder.PathState.No_Obstruction;
return;
}
public static bool IsNearVoxel(IMyCubeGrid grid, double lengthMulti = 1d)
{
BoundingSphereD surround = new BoundingSphereD(grid.GetCentre(), grid.GetLongestDim() * lengthMulti);
List<MyVoxelBase> voxels = new List<MyVoxelBase>();
MyGamePruningStructure.GetAllVoxelMapsInSphere(ref surround, voxels);
if (voxels != null)
foreach (IMyVoxelBase vox in voxels)
{
if (vox is IMyVoxelMap)
{
if (vox.GetIntersectionWithSphere(ref surround))
return true;
}
else
{
MyPlanet planet = vox as MyPlanet;
if (planet != null && planet.Intersects(ref surround))
return true;
}
}
return false;
}
public static bool IsNearVoxel(IMyCubeGrid grid, double lengthMulti = 0.5d)
{
BoundingSphereD surround = new BoundingSphereD(grid.GetCentre(), grid.GetLongestDim() * lengthMulti);
List<MyVoxelBase> voxels = new List<MyVoxelBase>();
MyGamePruningStructure.GetAllVoxelMapsInSphere(ref surround, voxels);
if (voxels != null)
foreach (IMyVoxelMap vox in voxels)
if (vox.GetIntersectionWithSphere(ref surround))
return true;
return false;
}
/// <summary>
/// Starts checking path against the geometry of the closest planet.
/// </summary>
/// <param name="displacement">Destination - current postion</param>
private void Start(ref Vector3 displacement)
{
using (m_lock.AcquireExclusiveUsing())
{
if ((CurrentState & State.Running) != 0)
{
return;
}
CurrentState |= State.Running;
m_displacement = displacement;
m_cells.Clear();
Vector3D gridCentre = m_grid.GetCentre();
double distSq;
gravcomp = MyPlanetExtensions.GetClosestPlanet(gridCentre, out distSq);
if (gravcomp == null)
{
//m_logger.debugLog("No planets found", "Start()", Logger.severity.TRACE);
CurrentState = State.Clear;
return;
}
if (distSq > gravcomp.MaximumRadius * gravcomp.MaximumRadius)
{
//m_logger.debugLog("Outside maximum radius of closest planet", "Start()", Logger.severity.TRACE);
// gravity test
// TODO: it might be worthwhile to perform gravity test against multiple planets
Path.From = gridCentre;
Path.To = gridCentre + displacement;
Vector3D closestPoint = Path.ClosestPoint(gravcomp.WorldMatrix.Translation);
if (closestPoint != Path.From && closestPoint != Path.To)
{
//float gravityAtClose = ClosestPlanet.GetGravityMultiplier(closestPoint) - MinGravityAvoid;
float gravityAtClose = gravComp.GetGravityMultiplier(closestPoint) - MinGravityAvoid;
//if (gravityAtClose > 0f && gravityAtClose > ClosestPlanet.GetGravityMultiplier(Path.From) && gravityAtClose > ClosestPlanet.GetGravityMultiplier(Path.To))
if (gravityAtClose > 0f && gravityAtClose > gravComp.GetGravityMultiplier(Path.From) && gravityAtClose > gravComp.GetGravityMultiplier(Path.To))
{
ObstructionPoint = closestPoint;
CurrentState = State.BlockedGravity;
return;
}
}
CurrentState = State.Clear;
return;
}
Vector3 direction;
Vector3.Normalize(ref displacement, out direction);
direction = Vector3.Transform(direction, m_grid.WorldMatrixNormalizedInv.GetOrientation());
GridCellCache.GetCellCache(m_grid).ForEach(cell => {
Vector3I rejected;
Vector3.Reject(cell, direction).ApplyOperation(x => (int)Math.Round(x), out rejected);
if (m_cellsUnique.Add(rejected))
{
m_cells.Enqueue(rejected);
}
});
m_cellsUnique.Clear();
DoTests.Enqueue(TestPath);
}
}
private void CheckPath()
{
Vector3? pointOfObstruction = null;
Vector3D WayDest = Destination;
Vector3D NavBlockPosition = NavigationBlock.GetPosition();
DistanceToDest = (float)(Destination - NavBlockPosition).Length();
IMyEntity ObstructingEntity = null;
//if (CNS.landingState == NavSettings.LANDING.OFF
// && (!FlyTheLine || Waypoint == null)) // if flying a line and has a waypoint, do not reroute to destination
if (SpecialFyingInstructions == NavSettings.SpecialFlying.None || Waypoint == null)
{
myLogger.debugLog("testing path to destination", "CheckPath()");
ObstructingEntity = myPathChecker.TestPath(Destination, NavigationBlock, IgnoreAsteroids, out pointOfObstruction, DestGrid);
if (ObstructingEntity == null)
{
if (Waypoint == null)
{
myLogger.debugLog("Path to destination is clear", "CheckPath()", Logger.severity.DEBUG);
SetOutput(new PathfinderOutput(myPathChecker, PathfinderOutput.Result.Path_Clear));
}
else
{
myLogger.debugLog("Re-routing to destination, path is now clear", "CheckPath()", Logger.severity.DEBUG);
SetOutput(new PathfinderOutput(myPathChecker, PathfinderOutput.Result.Alternate_Path, null, Destination));
}
return;
}
CheckInterrupt();
myLogger.debugLog("Path to destination is obstructed by " + ObstructingEntity.getBestName(), "CheckPath()", Logger.severity.DEBUG);
}
if (Waypoint != null)
{
WayDest = (Vector3)Waypoint;
myLogger.debugLog("testing path to waypoint", "CheckPath()");
ObstructingEntity = myPathChecker.TestPath((Vector3D)Waypoint, NavigationBlock, IgnoreAsteroids, out pointOfObstruction, DestGrid);
if (ObstructingEntity == null)
{
myLogger.debugLog("Path to waypoint is clear", "CheckPath()", Logger.severity.DEBUG);
SetOutput(new PathfinderOutput(myPathChecker, PathfinderOutput.Result.Path_Clear));
return;
}
}
if (ObstructingEntity == null)
{
myLogger.debugLog("Neither Destination nor Waypoint were tested", "CheckPath()", Logger.severity.DEBUG);
return;
}
CheckInterrupt();
if (SpecialFyingInstructions != NavSettings.SpecialFlying.None)
{
//Vector3 direction = WayDest - NavBlockPosition;
if (CanMoveInDirection(NavBlockPosition, WayDest - NavBlockPosition, "forward"))
{
return;
}
//if (SpecialFyingInstructions == NavSettings.SpecialFlying.Line_Any && CanMoveInDirection(NavBlockPosition, NavBlockPosition - WayDest, "backward", false))
//{
// myLogger.debugLog("Changing special instructions to Line_SidelForward", "CheckPath()");
// CNS.SpecialFlyingInstructions = NavSettings.SpecialFlying.Line_SidelForward;
// return;
//}
myLogger.debugLog("NoAlternateRoute, Obstruction = " + ObstructingEntity.getBestName(), "CheckPath()", Logger.severity.DEBUG);
SetOutput(new PathfinderOutput(myPathChecker, PathfinderOutput.Result.No_Way_Forward, ObstructingEntity));
return;
}
SetOutput(new PathfinderOutput(myPathChecker, PathfinderOutput.Result.Searching_Alt, ObstructingEntity));
myLogger.debugLog("Path to Way/Dest is obstructed by " + ObstructingEntity.getBestName() + " at " + pointOfObstruction, "CheckPath()", Logger.severity.TRACE);
Vector3 lineToWayDest = WayDest - CubeGrid.GetCentre();
Vector3 newPath_v1, newPath_v2;
lineToWayDest.CalculatePerpendicularVector(out newPath_v1);
newPath_v2 = lineToWayDest.Cross(newPath_v1);
newPath_v1 = Vector3.Normalize(newPath_v1);
newPath_v2 = Vector3.Normalize(newPath_v2);
Vector3[] NewPathVectors = { newPath_v1, newPath_v2, Vector3.Negate(newPath_v1), Vector3.Negate(newPath_v2) };
for (int newPathDistance = 8; newPathDistance < 10000; newPathDistance *= 2)
{
myLogger.debugLog("newPathDistance = " + newPathDistance, "CheckPath()", Logger.severity.TRACE);
// check far enough & sort alternate paths
SortedDictionary <float, Vector3> Alternate_Path = new SortedDictionary <float, Vector3>();
foreach (Vector3 PathVector in NewPathVectors)
{
pointOfObstruction.throwIfNull_variable("pointOfObstruction");
Vector3 Alternate = (Vector3)pointOfObstruction + newPathDistance * PathVector;
myLogger.debugLog("Alternate = " + Alternate, "CheckPath()");
CNS.throwIfNull_variable("CNS");
if (!CNS.waypointFarEnough(Alternate))
{
myLogger.debugLog("waypoint is too close, throwing out: " + Alternate, "CheckPath()");
continue;
}
float distanceFromDest = Vector3.Distance(Destination, Alternate);
Alternate_Path.throwIfNull_variable("Alternate_Path");
while (Alternate_Path.ContainsKey(distanceFromDest))
{
distanceFromDest = distanceFromDest.IncrementSignificand();
}
Alternate_Path.Add(distanceFromDest, Alternate);
myLogger.debugLog("Added alt path: " + Alternate + " with distance of " + distanceFromDest, "CheckPath()");
}
foreach (Vector3 AlternatePoint in Alternate_Path.Values)
{
if (TestAltPath(AlternatePoint))
{
return;
}
}
}
// before going to No Way Forward, check if we can move around
// try moving forward
Vector3D GridCentre = CubeGrid.GetCentre();
Vector3 Forward = NavigationBlock.WorldMatrix.Forward;
if (CanMoveInDirection(GridCentre, Forward, "forward"))
{
return;
}
// try moving backward
Vector3 Backward = NavigationBlock.WorldMatrix.Backward;
if (CanMoveInDirection(GridCentre, Backward, "backward"))
{
return;
}
SetOutput(new PathfinderOutput(myPathChecker, PathfinderOutput.Result.No_Way_Forward, ObstructingEntity));
myLogger.debugLog("No Way Forward: " + ObstructingEntity.getBestName(), "CheckPath()", Logger.severity.INFO);
}