public bool TestSlow(out MyEntity blockingPath, out Vector3? pointOfObstruction)
{
m_logger.debugLog(m_offendingEntities == null, "m_offendingEntities == null, did you remember to call TestFast()?", "TestSlow()", Logger.severity.FATAL);
IMyCubeBlock ignoreBlock = m_ignoreEntity as IMyCubeBlock;
foreach (MyEntity entity in m_offendingEntities)
{
m_logger.debugLog("checking entity: " + entity.getBestName(), "TestSlow()");
IMyVoxelBase voxel = entity as IMyVoxelBase;
if (voxel != null)
{
if (m_ignoreAsteroid)
{
m_logger.debugLog("Ignoring asteroid: " + voxel.getBestName(), "TestSlow()");
continue;
}
Vector3[] intersection = new Vector3[2];
if (!m_path.IntersectsAABB(voxel, out intersection[0]))
{
m_logger.debugLog("path does not intersect AABB. " + voxel.getBestName(), "TestSlow()", Logger.severity.TRACE);
continue;
}
if (!m_path.get_Reverse().IntersectsAABB(voxel, out intersection[1]))
{
m_logger.debugLog("Reversed path does not intersect AABB, perhaps it moved? " + voxel.getBestName(), "TestSlow()", Logger.severity.WARNING);
continue;
}
Capsule testSection = new Capsule(intersection[0], intersection[1], m_path.Radius);
IMyVoxelMap asteroid = voxel as IMyVoxelMap;
if (asteroid != null)
{
if (testSection.Intersects(asteroid, out pointOfObstruction))
{
blockingPath = entity;
return false;
}
}
else
{
MyPlanet planet = voxel as MyPlanet;
if (planet != null && testSection.Intersects(planet, out pointOfObstruction))
{
blockingPath = entity;
return false;
}
}
m_logger.debugLog("Does not intersect path: " + voxel.getBestName(), "TestSlow()", Logger.severity.TRACE);
continue;
}
IMyCubeGrid grid = entity as IMyCubeGrid;
if (grid != null)
{
if (m_profiler.rejectionIntersects(grid, ignoreBlock, out blockingPath, out pointOfObstruction))
return false;
continue;
}
m_logger.debugLog("not a grid, testing bounds", "TestSlow()");
if (!m_path.IntersectsAABB(entity))
continue;
if (!m_path.IntersectsVolume(entity))
continue;
m_logger.debugLog("no more tests for non-grids are implemented", "TestSlow()", Logger.severity.DEBUG);
pointOfObstruction = m_path.get_Line().ClosestPoint(entity.GetCentre());
blockingPath = entity;
return false;
}
blockingPath = null;
pointOfObstruction = null;
return true;
}
/// <param name="centreDestination">where the centre of the grid will end up (local)</param>
private void createCapsule(Vector3 centreDestination, Vector3 localPosition)
{
float longestDistanceSquared = 0;
using (m_lock_rejcectionCells.AcquireSharedUsing())
foreach (Vector3 rejection in m_rejectionCells)
{
float distanceSquared = (rejection - m_centreRejection).LengthSquared();
if (distanceSquared > longestDistanceSquared)
longestDistanceSquared = distanceSquared;
}
Vector3 P0 = RelativePosition3F.FromLocal(m_grid, Centre).ToWorld();
//Vector3D P1;
//if (m_landing)
//{
Vector3 P1 = RelativePosition3F.FromLocal(m_grid, centreDestination).ToWorld();
//}
//else
//{
// //// extend capsule past destination by distance between remote and front of grid
// //Ray navTowardsDest = new Ray(localPosition, m_directNorm);
// //float tMin, tMax;
// //m_grid.LocalVolume.IntersectRaySphere(navTowardsDest, out tMin, out tMax);
// //P1 = RelativeVector3F.createFromLocal(centreDestination + tMax * m_directNorm, m_grid).getWorldAbsolute();
// // extend capsule by length of grid
// P1 = RelativePosition3F.FromLocal(m_grid, centreDestination + m_directNorm * m_grid.GetLongestDim()).ToWorld();
//}
float CapsuleRadius = (float)Math.Sqrt(longestDistanceSquared) + 3f * m_grid.GridSize;// +(m_landing ? 0f : NotLandingBuffer);
Path = new Capsule(P0, P1, CapsuleRadius);
m_logger.debugLog("Path capsule created from " + P0 + " to " + P1 + ", radius: " + CapsuleRadius);
}
/// <summary>
/// Test two capsules for intersection
/// </summary>
/// <param name="capsule">first capsule to test</param>
/// <param name="other">second capsule to test</param>
/// <param name="shortestDistanceSquared">distance squared between lines of capsules</param>
/// <returns>true if the capsules intersect (including boundary)</returns>
public static bool Intersects(this Capsule capsule, Capsule other, out float shortestDistanceSquared)
{
shortestDistanceSquared = Line.GetShortestDistanceSquared(capsule.get_Line(), other.get_Line());
float radiiSquared = capsule.Radius + other.Radius;
radiiSquared *= radiiSquared;
return shortestDistanceSquared <= radiiSquared;
}
/// <summary>
/// Tests for path intersection with voxel.
/// </summary>
/// <returns>True iff path is clear; voxel does not intersect path.</returns>
private bool TestVoxel(MyVoxelBase voxel, Capsule path, out Vector3? pointOfObstruction)
{
if (m_ignoreAsteroid)
{
m_logger.debugLog("Ignoring asteroid: " + voxel.getBestName());
pointOfObstruction = null;
return true;
}
Vector3[] intersection = new Vector3[2];
if (!path.IntersectsAABB(voxel, out intersection[0]))
{
m_logger.debugLog("path does not intersect AABB. " + voxel.getBestName(), Logger.severity.TRACE);
pointOfObstruction = null;
return true;
}
if (!path.get_Reverse().IntersectsAABB(voxel, out intersection[1]))
{
m_logger.debugLog("Reversed path does not intersect AABB, perhaps it moved? " + voxel.getBestName(), Logger.severity.WARNING);
pointOfObstruction = null;
return true;
}
Capsule testSection = new Capsule(intersection[0], intersection[1], path.Radius);
IMyVoxelMap asteroid = voxel as IMyVoxelMap;
if (asteroid != null)
{
if (testSection.Intersects(asteroid, out pointOfObstruction))
return false;
}
// planet test is done by PlanetChecker
m_logger.debugLog("Does not intersect path: " + voxel.getBestName(), Logger.severity.TRACE);
pointOfObstruction = null;
return true;
}
//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>
/// Gets a point outside of an asteroid.
/// </summary>
/// <param name="startPoint">Where to start the search from, must be inside WorldAABB, can be inside or outside asteroid.</param>
/// <param name="direction">Direction from outside asteroid towards inside asteroid</param>
/// <param name="buffer">Minimum distance between the voxel and exterior point</param>
private Vector3D GetExteriorPoint_Asteroid(Vector3D startPoint, Vector3 direction, float buffer)
{
IMyVoxelMap voxel = m_targetVoxel as IMyVoxelMap;
if (voxel == null)
{
m_logger.alwaysLog("m_targetVoxel is not IMyVoxelMap: " + m_targetVoxel.getBestName(), Logger.severity.FATAL);
throw new InvalidOperationException("m_targetVoxel is not IMyVoxelMap");
}
Vector3 v = direction * m_targetVoxel.LocalAABB.GetLongestDim();
Capsule surfaceFinder = new Capsule(startPoint - v, startPoint + v, buffer);
Vector3? obstruction;
if (surfaceFinder.Intersects(voxel, out obstruction))
return obstruction.Value;
else
{
m_logger.debugLog("Failed to intersect asteroid, using surfaceFinder.P0", Logger.severity.WARNING);
return surfaceFinder.P0;
}
}
