//public bool GravityTest(LineSegmentD line, Vector3D finalDestination, out MyEntity blockingPath, out Vector3? pointOfObstruction)
//{
// if (Vector3.DistanceSquared(line.From, line.To) > 10000f)
// {
// BoundingBoxD box = line.BoundingBox;
// m_voxels.Clear();
// MyGamePruningStructure.GetAllVoxelMapsInBox(ref box, m_voxels);
// foreach (MyEntity entity in m_voxels)
// {
// MyPlanet planet = entity as MyPlanet;
// if (planet == null)
// continue;
// Vector3D planetCentre = planet.GetCentre();
// Vector3D closestPoint = line.ClosestPoint(planetCentre);
// if (!planet.IsPositionInGravityWell(closestPoint))
// continue;
// m_logger.debugLog("path: " + line.From + " to " + line.To + ", final: " + finalDestination + ", closest point: " + closestPoint + ", planet @ " + planetCentre + " : " + planet.getBestName(), "GravityTest()");
// if (closestPoint == line.From)
// {
// m_logger.debugLog("closest point is start", "GravityTest()");
// continue;
// }
// if (closestPoint == line.To)
// {
// if (line.To == finalDestination )
// {
// m_logger.debugLog("closest point is final", "GravityTest()");
// continue;
// }
// if (Vector3D.DistanceSquared(planetCentre, closestPoint) < Vector3D.DistanceSquared(planetCentre, finalDestination))
// {
// m_logger.debugLog("closest point is end, which is closer to planet than final dest is", "GravityTest()");
// blockingPath = entity;
// pointOfObstruction = closestPoint;
// return false;
// }
// m_logger.debugLog("closest point is end", "GravityTest()");
// continue;
// }
// //float startGravity = planet.GetWorldGravityGrid( line.From).LengthSquared();
// //float closestGravity = planet.GetWorldGravityGrid(closestPoint).LengthSquared();
// //double toDistSq = Vector3D.DistanceSquared(planetCentre, line.To);
// //if (closestPoint == line.To && (line.To == finalDestination || ))
// //{
// // m_logger.debugLog("path: " + line.From + " to " + line.To + ", closest point: " + closestPoint + ", planet @ " + planetCentre + " : " + planet.getBestName(), "GravityTest()");
// // m_logger.debugLog("closest point is end", "GravityTest()");
// // continue;
// //}
// //m_logger.debugLog("path: " + line.From + " to " + line.To + ", closest point: " + closestPoint + ", planet @ " + planetCentre + " : " + planet.getBestName(), "GravityTest()", Logger.severity.DEBUG);
// double closestDistSq = Vector3D.DistanceSquared(planetCentre, closestPoint) + 1f;
// if (closestDistSq < Vector3D.DistanceSquared(planetCentre, line.From) || closestDistSq < Vector3D.DistanceSquared(planetCentre, line.To))
// {
// m_logger.debugLog("path moves ship closer to planet. closestDistSq: " + closestDistSq + ", from dist sq: " + Vector3D.DistanceSquared(planetCentre, line.From) + ", to dist sq: " + Vector3D.DistanceSquared(planetCentre, line.To), "GravityTest()", Logger.severity.INFO);
// blockingPath = entity;
// pointOfObstruction = closestPoint;
// return false;
// }
// }
// }
// blockingPath = null;
// pointOfObstruction = null;
// return true;
//}
/// <summary>
/// How far long the line would the ship be able to travel? Uses a capsule derived from previously calculated path.
/// </summary>
/// <param name="canTravel">Line along which navigation block would travel</param>
/// <remarks>
/// Capsule only test because the ship will not be oriented correctly
/// </remarks>
/// <returns>distance from the destination that can be reached</returns>
public float distanceCanTravel(LineSegment canTravel)
{
BoundingBoxD atFrom = m_grid.WorldAABB.Translate(canTravel.From - m_grid.GetPosition());
BoundingBoxD atTo = m_grid.WorldAABB.Translate(canTravel.To - m_grid.GetPosition());
ICollection <MyEntity> offenders = EntitiesInLargeAABB(atFrom, atTo);
if (offenders.Count == 0)
{
m_logger.debugLog("AABB is empty");
return(0);
}
m_logger.debugLog("collected entities to test: " + offenders.Count);
IOrderedEnumerable <MyEntity> ordered = offenders.OrderBy(entity => Vector3D.Distance(canTravel.From, entity.GetCentre()));
Capsule path = new Capsule(canTravel.From, canTravel.To, m_path.Radius);
Vector3?pointOfObstruction = null;
foreach (MyEntity entity in ordered)
{
if (path.IntersectsAABB(entity))
{
MyVoxelBase voxel = entity as MyVoxelBase;
if (voxel != null)
{
if (!TestVoxel(voxel, path, out pointOfObstruction))
{
m_logger.debugLog("obstruction at " + pointOfObstruction + " distance from dest is " + Vector3.Distance(canTravel.To, (Vector3)pointOfObstruction));
return(Vector3.Distance(canTravel.To, (Vector3)pointOfObstruction));
}
}
IMyCubeGrid grid = entity as IMyCubeGrid;
if (grid != null)
{
float minDistSq = (m_grid.GridSize + grid.GridSize) * (m_grid.GridSize + grid.GridSize);
GridCellCache cache = GridCellCache.GetCellCache(grid);
cache.ForEach(cell => {
Vector3 cellPos = grid.GridIntegerToWorld(cell);
if (canTravel.PointInCylinder(minDistSq, ref cellPos))
{
pointOfObstruction = cellPos;
return(true);
}
return(false);
});
if (pointOfObstruction.HasValue)
{
return(Vector3.Distance(canTravel.To, (Vector3)pointOfObstruction));
}
}
m_logger.debugLog("not a grid, testing bounds");
if (!m_path.IntersectsVolume(entity))
{
continue;
}
m_logger.debugLog("no more tests for non-grids are implemented", Logger.severity.DEBUG);
pointOfObstruction = m_path.get_Line().ClosestPoint(entity.GetCentre());
return(Vector3.Distance(canTravel.To, (Vector3)pointOfObstruction));
}
}
// no obstruction
m_logger.debugLog("no obstruction");
return(0f);
}
/// <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);
}