static Vector3D GridToWorld(Vector3I gridVector, IMyCubeGrid grid)
{
//convert a grid vector for the specified grid into a world direction vector
return(Vector3D.Subtract(
grid.GridIntegerToWorld(gridVector),
grid.GridIntegerToWorld(Vector3I.Zero)));
}
MatrixD GetGrid2WorldTransform(IMyCubeGrid grid)
{
Vector3D origin = grid.GridIntegerToWorld(new Vector3I(0, 0, 0));
Vector3D plusY = grid.GridIntegerToWorld(new Vector3I(0, 1, 0)) - origin;
Vector3D plusZ = grid.GridIntegerToWorld(new Vector3I(0, 0, 1)) - origin;
return(MatrixD.CreateScale(grid.GridSize) * MatrixD.CreateWorld(origin, -plusZ, plusY));
}
/// <summary>
/// Finds a random block facing the sky. Raycast is slightly randomized to simulate wind blowing rain
/// </summary>
/// <param name="grid"></param>
/// <param name="blocks"></param>
/// <param name="gravityDirection"></param>
/// <param name="physicsCast"></param>
/// <returns></returns>
public static IMySlimBlock GetRandomSkyFacingBlock(IMyCubeGrid grid, List <IMySlimBlock> blocks, Vector3D gravityDirection, out LineD line, bool physicsCast = false)
{
Vector3D posInt = grid.GridIntegerToWorld(blocks.GetRandomItemFromList().Position) + -gravityDirection * 2;
Vector3D posExt = posInt + gravityDirection * Math.Max(25, grid.WorldVolume.Radius * 2);
posExt = RandomPositionFromPoint(ref posExt, grid.WorldVolume.Radius);
line = new LineD();
Vector3I?blockPos = grid.RayCastBlocks(posExt, posInt);
if (!blockPos.HasValue)
{
return(null);
}
IMySlimBlock block = grid.GetCubeBlock(blockPos.Value);
if (block == null)
{
return(null);
}
if (physicsCast)
{
var hits = new List <IHitInfo>();
MyAPIGateway.Physics.CastRay(posExt, grid.GridIntegerToWorld(blockPos.Value), hits);
foreach (IHitInfo hit in hits)
{
if (hit?.HitEntity?.GetTopMostParent() == null)
{
continue;
}
if (hit.HitEntity.GetTopMostParent().EntityId != grid.EntityId)
{
return(null);
}
}
}
if (Debug)
{
Lines.Add(new LineD {
From = posExt, To = grid.GridIntegerToWorld(blockPos.Value)
});
}
line = new LineD {
From = posExt, To = grid.GridIntegerToWorld(blockPos.Value)
};
return(block);
}
public void Draw()
{
if (status != Status.DRAW || selectedGrid == null || selectedGrid.Closed || lines.Count == 0)
{
return;
}
var camMatrix = MyAPIGateway.Session.Camera.WorldMatrix;
var camPos = camMatrix.Translation;
var camFw = camMatrix.Forward;
var alpha = (drawTicks < DRAW_FADE_OUT_TICKS ? (DRAW_TRANSPARENCY * ((float)drawTicks / (float)DRAW_FADE_OUT_TICKS)) : DRAW_TRANSPARENCY);
var particleColor = COLOR_PARTICLES * alpha;
// start dot
var point = selectedGrid.GridIntegerToWorld(lines[lines.Count - 1].Start);
if (IsVisibleFast(ref camPos, ref camFw, ref point))
{
var startPos = camPos + ((point - camPos) * DRAW_DEPTH);
MyTransparentGeometry.AddPointBillboard(MATERIAL_DOT, Color.Green * alpha, startPos, DRAW_POINT_SIZE * DRAW_DEPTH_F, 0);
}
var pd = particleDataGridSize[(int)selectedGrid.GridSizeEnum]; // particle settings for the grid cell size
// spawning particles
if (!stopSpawning && ++delayParticles > pd.SpawnDelay)
{
delayParticles = 0;
particles.Add(new Particle(selectedGrid, lines));
}
// draw/move particles
for (int i = particles.Count - 1; i >= 0; --i)
{
if (!particles[i].Draw(selectedGrid, lines, ref particleColor, pd, ref camPos, ref camFw))
{
stopSpawning = true;
}
}
// end dot
point = selectedGrid.GridIntegerToWorld(lines[0].End);
if (IsVisibleFast(ref camPos, ref camFw, ref point))
{
var endPos = camPos + ((point - camPos) * DRAW_DEPTH);
MyTransparentGeometry.AddPointBillboard(MATERIAL_DOT, Color.Red * alpha, endPos, DRAW_POINT_SIZE * DRAW_DEPTH_F, 0);
}
}
private bool entityIntersectsWithSphere(IMyEntity entity, BoundingSphereD sphere)
{
IMyCubeGrid grid = entity as IMyCubeGrid;
if (grid == null)
{
log(myLogger, "Asteroid: " + entity.getBestName() + ", " + !ignoreAsteroids, "entityIntersectsWithSphere()", Logger.severity.DEBUG);
return(!ignoreAsteroids);
//return entity.GetIntersectionWithSphere(ref sphere); // not at all reliable
}
expensiveTest = true;
log(myLogger, "using grid test", "entityIntersectsWithSphere()", Logger.severity.DEBUG);
List <IMySlimBlock> blocksInGrid = new List <IMySlimBlock>();
grid.GetBlocks(blocksInGrid);
BoundingSphere sphereF = sphere;
DateTime beforeBlockCheck = DateTime.UtcNow;
foreach (IMySlimBlock block in blocksInGrid)
{
blocksChecked++;
Vector3 worldPos = grid.GridIntegerToWorld(block.Position);
if (sphereF.Contains(worldPos) != ContainmentType.Disjoint)
{
//log(myLogger, "took " + (DateTime.UtcNow - beforeBlockCheck).TotalMilliseconds + " to blockCheck, result=true");
return(true);
}
}
//log(myLogger, "took " + (DateTime.UtcNow - beforeBlockCheck).TotalMilliseconds + " to blockCheck, result=false");
return(false); // nothing found*/
}
static public OrientedBoundingBoxD GetBoundingBox(List <IMyCubeBlock> cubeList)
{
Vector3D min = new Vector3D(int.MaxValue, int.MaxValue, int.MaxValue);
Vector3D max = new Vector3D(int.MinValue, int.MinValue, int.MinValue);
Vector3D pos = new Vector3D(0, 0, 0);
IMyCubeGrid parent = (IMyCubeGrid)cubeList[0].Parent;
foreach (IMyCubeBlock block in cubeList)
{
min.X = Math.Min(min.X, block.Position.X);
min.Y = Math.Min(min.Y, block.Position.Y);
min.Z = Math.Min(min.Z, block.Position.Z);
max.X = Math.Max(max.X, block.Position.X);
max.Y = Math.Max(max.Y, block.Position.Y);
max.Z = Math.Max(max.Z, block.Position.Z);
pos += parent.GridIntegerToWorld(block.Position);
}
pos = pos / cubeList.Count;
// scale box to GridSize
var size = max - min;
double smallestNotZero = Math.Max(Math.Min(size.X, size.Y), Math.Min(Math.Max(size.X, size.Y), size.Z));
Vector3D rawSize = new Vector3D(size.X * 2.5f, size.Y * 2.5f, size.Z * 2.5f);
Vector3D expandedSize = new Vector3D(Math.Max(size.X, smallestNotZero) * 2.5f, Math.Max(size.Y, smallestNotZero) * 2.5f, Math.Max(size.Z, smallestNotZero) * 2.5f);
BoundingBoxD bb = new BoundingBoxD(new Vector3D(0, 0, 0), expandedSize).Translate(pos - (expandedSize / 2));
return(new OrientedBoundingBoxD(bb.Center, bb.HalfExtents, Quaternion.CreateFromRotationMatrix(parent.WorldMatrix.GetOrientation())));
}
public Particle(IMyCubeGrid grid, List <Line> lines)
{
var l = lines[lines.Count - 1];
position = grid.GridIntegerToWorld(l.Start);
walk = lines.Count - 1;
}
/// <summary>
/// Rejection test for intersection with the profiled grid.
/// </summary>
/// <param name="grid">Grid whose cells will be rejected and compared to the profiled grid's rejections.</param>
/// <returns>True iff there is a collision</returns>
public bool rejectionIntersects(IMyCubeGrid grid, IMyCubeBlock ignore, out MyEntity entity, out Vector3?pointOfObstruction)
{
m_logger.debugLog("m_grid == null", Logger.severity.FATAL, condition: m_grid == null);
//m_logger.debugLog("testing grid: " + grid.getBestName(), "rejectionIntersects()");
GridCellCache gridCache = GridCellCache.GetCellCache(grid);
MatrixD toLocal = m_grid.WorldMatrixNormalizedInv;
Line pathLine = Path.get_Line();
float minDist = m_grid.GridSize + grid.GridSize;
//if (!m_landing)
// minDist += NotLandingBuffer;
float pathRadius = Path.Radius + minDist;
float minDistSquared = minDist * minDist;
MyEntity entity_in = null;
Vector3? pointOfObstruction_in = null;
using (m_lock_rejcectionCells.AcquireSharedUsing())
gridCache.ForEach(cell => {
Vector3 world = grid.GridIntegerToWorld(cell);
//m_logger.debugLog("checking position: " + world, "rejectionIntersects()");
if (pathLine.PointInCylinder(pathRadius, world))
{
//m_logger.debugLog("point in cylinder: " + world, "rejectionIntersects()");
Vector3 local = Vector3.Transform(world, toLocal);
if (rejectionIntersects(local, minDistSquared))
{
entity_in = grid.GetCubeBlock(cell).FatBlock as MyEntity ?? grid as MyEntity;
if (ignore != null && entity_in == ignore)
{
return(false);
}
pointOfObstruction_in = pathLine.ClosestPoint(world);
return(true);
}
}
return(false);
});
if (pointOfObstruction_in.HasValue)
{
entity = entity_in;
pointOfObstruction = pointOfObstruction_in;
return(true);
}
entity = null;
pointOfObstruction = null;
return(false);
}
public MatrixD GetBlockWorldMatrix()
{
var position = grid.GridIntegerToWorld(Block.Position);
Matrix blockMatrix;
Block.Orientation.GetMatrix(out blockMatrix);
var transform = blockMatrix * grid.WorldMatrix;
transform.Translation = position;
return(transform);
}
private void TestPath()
{
using (m_lock.AcquireExclusiveUsing())
{
if (m_grid.MarkedForClose)
{
CurrentState = State.BlockedPath;
m_cells.Clear();
return;
}
if (CurrentState == State.None)
{
m_cells.Clear();
return;
}
//MyGameTimer timer = new MyGameTimer();
if (m_cells.Count != 0)
{
Vector3D worldPos = m_grid.GridIntegerToWorld(m_cells.Dequeue());
LineD worldLine = new LineD(worldPos, worldPos + m_displacement);
//var createLine = timer.Elapsed;
//timer = new MyGameTimer();
Vector3D?contact;
if (RayCast.RayCastVoxel(gravcomp, worldLine, out contact))
{
//var intersect = timer.Elapsed;
m_logger.debugLog("Intersected line: " + worldLine.From + " to " + worldLine.To + ", at " + contact, Logger.severity.DEBUG);
//m_logger.debugLog("Intersected line: " + worldLine.From + " to " + worldLine.To + ", at " + contact + ", createLine: " + createLine.ToPrettySeconds() + ", intersect: " + intersect.ToPrettySeconds(), "TestPath()", Logger.severity.DEBUG);
ObstructionPoint = contact.Value;
CurrentState = State.BlockedPath;
m_cells.Clear();
return;
}
//else
//{
// var intersect = timer.Elapsed;
// m_logger.debugLog("no intersection with line: " + worldLine.From + " to " + worldLine.To + ", createLine: " + createLine.ToPrettySeconds() + ", intersect: " + intersect.ToPrettySeconds(), "TestPath()", Logger.severity.TRACE);
//}
DoTests.Enqueue(TestPath);
}
else
{
m_logger.debugLog("finished, clear", Logger.severity.DEBUG);
CurrentState = State.Clear;
}
}
}
/// <summary>
/// Returns true if it should spawn more particles, false otherwise (reached the end and looping back)
/// </summary>
public bool Draw(IMyCubeGrid grid, List <Line> lines, ref Color color, ParticleData pd, ref Vector3D camPos, ref Vector3D camFw)
{
var i = (walk < 0 ? 0 : (int)Math.Floor(walk));
var l = lines[i];
var lineStart = grid.GridIntegerToWorld(l.Start);
var lineEnd = grid.GridIntegerToWorld(l.End);
if (IsVisibleFast(ref camPos, ref camFw, ref lineStart) || IsVisibleFast(ref camPos, ref camFw, ref lineEnd))
{
var lineDir = (lineEnd - lineStart);
var fraction = (1 - (walk - i));
var targetPosition = lineStart + lineDir * fraction;
position = Vector3D.Lerp(position, targetPosition, pd.LerpPos);
var drawPosition = camPos + ((position - camPos) * DRAW_DEPTH);
if (walk < 0)
{
MyTransparentGeometry.AddPointBillboard(BuildInfo.instance.leakInfo.MATERIAL_DOT, color * (1f - Math.Abs(walk)), drawPosition, pd.Size * DRAW_DEPTH_F, 0);
}
else
{
MyTransparentGeometry.AddPointBillboard(BuildInfo.instance.leakInfo.MATERIAL_DOT, color, drawPosition, pd.Size * DRAW_DEPTH_F, 0);
}
}
walk -= pd.WalkSpeed; // walk on the lines
if (walk < -1) // go back to the start and tell the component to stop spawning new ones
{
l = lines[lines.Count - 1];
position = grid.GridIntegerToWorld(l.Start);
walk = lines.Count - 1;
return(false);
}
return(true);
}
/// <summary>
/// Gets a block on the exterior hull of a ship
/// </summary>
/// <param name="grid"></param>
/// <param name="blocks"></param>
/// <returns></returns>
public static IMySlimBlock GetRandomExteriorBlock(IMyCubeGrid grid, List <IMySlimBlock> blocks)
{
Vector3D posInt = grid.GridIntegerToWorld(blocks.GetRandomItemFromList().Position);
Vector3D posExt = RandomPositionFromPoint(ref posInt, grid.WorldAABB.HalfExtents.Length());
if (Debug)
{
Lines.Add(new LineD {
From = posExt, To = posInt
});
}
Vector3I?blockPos = grid.RayCastBlocks(posExt, posInt);
return(blockPos.HasValue ? grid.GetCubeBlock(blockPos.Value) : null);
}
/// <summary>
/// gets the relative world position
/// </summary>
public Vector3 getWorld()
{
if (value__world != null)
{
return((Vector3)value__world);
}
if (value__worldAbsolute != null)
{
// create from world absolute
value__world = (Vector3)value__worldAbsolute - cubeGrid.GetPosition();
}
// create from grid
Vector3I gridInt = Vector3I.Round(getLocal() * precisionMultiplier / cubeGrid.GridSize);
value__world = (cubeGrid.GridIntegerToWorld(gridInt)) / precisionMultiplier;
return((Vector3)value__world);
}
/// <param name="offenders">entities to test</param>
/// <param name="myGridShape">iff null, skip rejection test</param>
private IMyEntity TestEntities(ICollection <IMyEntity> offenders, Capsule path, GridShapeProfiler myGridShape, out Vector3?pointOfObstruction, IMyCubeGrid GridDestination)
{
foreach (IMyEntity entity in offenders)
{
CheckInterrupt();
myLogger.debugLog("testing offender: " + entity.getBestName() + " at " + entity.GetPosition(), "TestEntities()");
IMyCubeGrid asGrid = entity as IMyCubeGrid;
if (asGrid != null)
{
if (asGrid == GridDestination)
{
myLogger.debugLog("grid is destination: " + asGrid.DisplayName, "TestEntities()");
continue;
}
if (!path.IntersectsAABB(entity))
{
myLogger.debugLog("no AABB intersection: " + asGrid.DisplayName, "TestEntities()");
continue;
}
myLogger.debugLog("searching blocks of " + entity.getBestName(), "TestEntities()");
uint cellCount = 0, cellRejectedCount = 0;
// foreach block
float GridSize = asGrid.GridSize;
List <IMySlimBlock> allSlims = new List <IMySlimBlock>();
asGrid.GetBlocks_Safe(allSlims);
foreach (IMySlimBlock slim in allSlims)
{
bool blockIntersects = false;
Vector3 cellPosWorld = new Vector3();
//myLogger.debugLog("slim = " + slim.getBestName() + ", fat = " + slim.FatBlock + ", cell = " + slim.Position, "TestEntities()");
//if (slim.FatBlock != null)
//{
// myLogger.debugLog("fatblock min = " + slim.FatBlock.Min + ", fatblock max = " + slim.FatBlock.Max, "TestEntities()");
// //myLogger.debugLog("fatblock AABB min = " + slim.FatBlock.LocalAABB.Min + ", fatblock AABB max = " + slim.FatBlock.LocalAABB.Max, "TestEntities()");
//}
slim.ForEachCell((cell) => {
CheckInterrupt();
cellPosWorld = asGrid.GridIntegerToWorld(cell);
cellCount++;
//myLogger.debugLog("slim = " + slim.getBestName() + ", cell = " + cell + ", world position = " + cellPosWorld, "TestEntities()");
// intersects capsule
if (!path.Intersects(cellPosWorld, GridSize))
{
return(false);
}
// rejection
cellRejectedCount++;
if (myGridShape == null || myGridShape.rejectionIntersects(RelativeVector3F.createFromWorldAbsolute(cellPosWorld, myCubeGrid), myCubeGrid.GridSize))
{
myLogger.debugLog("obstructing grid = " + asGrid.DisplayName + ", cell = " + cellPosWorld + ", block = " + slim.getBestName(), "TestEntities()", Logger.severity.DEBUG);
blockIntersects = true;
return(true);
}
//myLogger.debugLog("rejection: no collision, cell = " + cellPosWorld + ", block = " + slim.getBestName(), "TestEntities()", Logger.severity.DEBUG);
return(false);
});
if (blockIntersects)
{
//myLogger.debugLog("closest point on line: {" + path.get_Line().From + ", " + path.get_Line().To + "} to " + cellPosWorld + " is " + path.get_Line().ClosestPoint(cellPosWorld), "TestEntities()");
pointOfObstruction = path.get_Line().ClosestPoint(cellPosWorld);
return(entity);
}
}
myLogger.debugLog("no obstruction for grid " + asGrid.DisplayName + ", tested " + cellCount + " against capsule and " + cellRejectedCount + " against rejection", "TestPath()");
continue;
}
// not a grid
if (IgnoreAsteroids && entity is IMyVoxelMap)
{
myLogger.debugLog("Ignoring asteroid: " + entity.getBestName(), "TestEntities()");
continue;
}
myLogger.debugLog("not a grid, testing bounds", "TestEntities()");
if (!path.IntersectsAABB(entity))
{
continue;
}
if (!path.IntersectsVolume(entity))
{
continue;
}
myLogger.debugLog("no more tests for non-grids are implemented", "TestEntities()", Logger.severity.DEBUG);
//myLogger.debugLog("closest point on line: {" + path.get_Line().From + ", " + path.get_Line().To + "} to " + entity.GetCentre() + " is " + path.get_Line().ClosestPoint(entity.GetCentre()), "TestEntities()");
pointOfObstruction = path.get_Line().ClosestPoint(entity.GetCentre());
return(entity);
}
myLogger.debugLog("no obstruction was found", "TestPath()", Logger.severity.DEBUG);
pointOfObstruction = null;
return(null);
}
public static ConcurrentBag <MyGroups <MyCubeGrid, MyGridPhysicalGroupData> .Group> FindLookAtGridGroup(IMyCharacter controlledEntity)
{
const float range = 5000;
Matrix worldMatrix;
Vector3D startPosition;
Vector3D endPosition;
worldMatrix = controlledEntity.GetHeadMatrix(true, true, false); // dead center of player cross hairs, or the direction the player is looking with ALT.
startPosition = worldMatrix.Translation + worldMatrix.Forward * 0.5f;
endPosition = worldMatrix.Translation + worldMatrix.Forward * (range + 0.5f);
var list = new Dictionary <MyGroups <MyCubeGrid, MyGridPhysicalGroupData> .Group, double>();
var ray = new RayD(startPosition, worldMatrix.Forward);
foreach (var group in MyCubeGridGroups.Static.Physical.Groups)
{
foreach (MyGroups <MyCubeGrid, MyGridPhysicalGroupData> .Node groupNodes in group.Nodes)
{
IMyCubeGrid cubeGrid = groupNodes.NodeData;
if (cubeGrid == null)
{
continue;
}
if (cubeGrid.Physics == null)
{
continue;
}
// check if the ray comes anywhere near the Grid before continuing.
if (!ray.Intersects(cubeGrid.WorldAABB).HasValue)
{
continue;
}
Vector3I?hit = cubeGrid.RayCastBlocks(startPosition, endPosition);
if (!hit.HasValue)
{
continue;
}
double distance = (startPosition - cubeGrid.GridIntegerToWorld(hit.Value)).Length();
if (list.TryGetValue(group, out double oldDistance))
{
if (distance < oldDistance)
{
list.Remove(group);
list.Add(group, distance);
}
}
else
{
list.Add(group, distance);
}
}
}
var bag = new ConcurrentBag <MyGroups <MyCubeGrid, MyGridPhysicalGroupData> .Group>();
if (list.Count == 0)
{
return(bag);
}
// find the closest Entity.
var item = list.OrderBy(f => f.Value).First();
bag.Add(item.Key);
return(bag);
}
public PositionWorld ToWorld(IMyCubeGrid grid)
{
return grid.GridIntegerToWorld(vector);
}
//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);
}
public PositionWorld ToWorld(IMyCubeGrid grid)
{
return(grid.GridIntegerToWorld(vector));
}
/// <summary>
/// Rejection test for intersection with the profiled grid.
/// </summary>
/// <param name="grid">Grid whose cells will be rejected and compared to the profiled grid's rejections.</param>
/// <returns>True iff there is a collision</returns>
public bool rejectionIntersects(IMyCubeGrid grid, IMyCubeBlock ignore, out MyEntity entity, out Vector3? pointOfObstruction)
{
m_logger.debugLog(m_grid == null, "m_grid == null", Logger.severity.FATAL);
//m_logger.debugLog("testing grid: " + grid.getBestName(), "rejectionIntersects()");
GridCellCache gridCache = GridCellCache.GetCellCache(grid);
MatrixD toLocal = m_grid.WorldMatrixNormalizedInv;
Line pathLine = Path.get_Line();
float minDist = m_grid.GridSize + grid.GridSize;
//if (!m_landing)
// minDist += NotLandingBuffer;
float pathRadius = Path.Radius + minDist;
float minDistSquared = minDist * minDist;
MyEntity entity_in = null;
Vector3? pointOfObstruction_in = null;
using (m_lock_rejcectionCells.AcquireSharedUsing())
gridCache.ForEach(cell => {
Vector3 world = grid.GridIntegerToWorld(cell);
//m_logger.debugLog("checking position: " + world, "rejectionIntersects()");
if (pathLine.PointInCylinder(pathRadius, world))
{
//m_logger.debugLog("point in cylinder: " + world, "rejectionIntersects()");
Vector3 local = Vector3.Transform(world, toLocal);
if (rejectionIntersects(local, minDistSquared))
{
entity_in = grid.GetCubeBlock(cell).FatBlock as MyEntity ?? grid as MyEntity;
if (ignore != null && entity_in == ignore)
return false;
pointOfObstruction_in = pathLine.ClosestPoint(world);
return true;
}
}
return false;
});
if (pointOfObstruction_in.HasValue)
{
entity = entity_in;
pointOfObstruction = pointOfObstruction_in;
return true;
}
entity = null;
pointOfObstruction = null;
return false;
}
public override void Move()
{
Log.DebugLog("m_gridFinder == null", Logger.severity.FATAL, condition: m_gridFinder == null);
Log.DebugLog("m_navSet == null", Logger.severity.FATAL, condition: m_navSet == null);
Log.DebugLog("m_mover == null", Logger.severity.FATAL, condition: m_mover == null);
Log.DebugLog("m_navBlock == null", Logger.severity.FATAL, condition: m_navBlock == null);
m_gridFinder.Update();
if (m_gridFinder.Grid == null)
{
Log.DebugLog("searching");
m_mover.StopMove();
// only timeout if (Grid == null), ship could simply be waiting its turn
if (Globals.ElapsedTime > m_searchTimeoutAt)
{
Log.DebugLog("Search timed out", Logger.severity.INFO);
m_navSet.OnTaskComplete(m_settingLevel);
UnreserveTarget();
m_mover.StopMove();
m_mover.StopRotate();
return;
}
if (m_landingState > LandingState.Approach)
{
Log.DebugLog("Decreasing landing state from " + m_landingState + " to " + LandingState.Approach, Logger.severity.DEBUG);
m_landingState = LandingState.Approach;
}
return;
}
else
{
m_searchTimeoutAt = Globals.ElapsedTime + SearchTimeout;
if (!m_gridFinder.Grid.isRecent())
{
m_pathfinder.MoveTo(m_gridFinder.Grid);
return;
}
if (m_gridFinder.Block != null)
{
m_destination = Destination.FromWorld(m_gridFinder.Block, m_navSet.Settings_Current.DestinationOffset.ToWorld(m_gridFinder.Block));
}
else
{
IMyCubeGrid grid = (IMyCubeGrid)m_gridFinder.Grid.Entity;
CubeGridCache cache = CubeGridCache.GetFor(grid);
if (cache == null)
{
return;
}
m_previousCell = cache.GetClosestOccupiedCell(m_navBlock.WorldPosition, m_previousCell);
m_destination = Destination.FromWorld(grid, grid.GridIntegerToWorld(m_previousCell));
}
if (m_landingState > LandingState.Approach)
{
Move_Land();
return;
}
if (m_landingState == LandingState.None && m_navSet.Settings_Current.Stay_In_Formation)
{
Log.DebugLog("Maintaining offset position from target", condition: m_navSet.DistanceLessThanDestRadius());
m_pathfinder.MoveTo(destinations: m_destination);
return;
}
if (m_navSet.DistanceLessThanDestRadius())
{
if (m_landingState == LandingState.None)
{
if (m_targetBlock != null && m_targetBlock.Forward.HasValue ? m_navSet.DirectionMatched() : m_navBlock.Physics.AngularVelocity.LengthSquared() < 0.01f)
{
Log.DebugLog("Arrived at target", Logger.severity.INFO);
m_navSet.OnTaskComplete(m_settingLevel);
UnreserveTarget();
m_mover.StopRotate();
}
m_mover.StopMove();
return;
}
Move_Land();
return;
}
m_pathfinder.MoveTo(destinations: m_destination);
}
}
/// <summary>
/// <para>Test for entities intersecting a line segment.</para>
/// </summary>
/// <param name="world">Line to test for entities</param>
/// <param name="pointOfIntersection">the point of contact between Line and intersecting entity</param>
/// <param name="collect">Ignore entity if function returns false. Not tested against IMyCubeBlock</param>
/// <param name="collect_CubeBlock">If null, do not ignore any IMyCubeBlock. Ignore IMyCubeBlock if function returns false</param>
/// <param name="sort">entities will be sorted according to results of this function</param>
/// <returns>An entity intersecting the Line</returns>
/// <remarks>
/// <para>For IMyCubeGrid, if a cell the line passes through is occupied, the cell is considered to be the point of intersection</para>
/// <para>For entities that are not IMyCubeGrid, Line will be tested against IMyEntity.WorldVolume</para>
/// </remarks>
public static IMyEntity EntityOnLine(LineD world, out Vector3D?pointOfIntersection, Func <IMyEntity, bool> collect, Func <IMyCubeBlock, bool> collect_CubeBlock = null, Func <IMyEntity, double> sort = null)
{
if (collect == null)
{
throw new ArgumentNullException("collect");
}
// Get entities in AABB
Vector3D[] points = { world.From, world.To };
BoundingBoxD AABB = BoundingBoxD.CreateFromPoints(points);
ICollection <IMyEntity> entitiesInAABB = MyAPIGateway.Entities.GetEntitiesInAABB_Safe(ref AABB);
RayD worldRay = new RayD(world.From, world.Direction);
if (sort != null)
{
SortedDictionary <double, IMyEntity> sortedEntities = new SortedDictionary <double, IMyEntity>();
foreach (IMyEntity entity in entitiesInAABB)
{
// filter entities
if (entity is IMyCubeBlock || !collect(entity))
{
continue;
}
sortedEntities.Add(sort(entity), entity);
}
entitiesInAABB = sortedEntities.Values;
}
foreach (IMyEntity entity in entitiesInAABB)
{
if (sort == null)
{
// filter entities
if (entity is IMyCubeBlock || !collect(entity))
{
continue;
}
}
// ray cast
IMyCubeGrid entityAsGrid = entity as IMyCubeGrid;
if (entityAsGrid != null)
{
// TODO: test GetLineIntersectionExact...
List <Vector3I> rayCastCells = new List <Vector3I>();
entityAsGrid.RayCastCells(world.From, world.To, rayCastCells); // I do not know if rayCastCells will be sorted
foreach (Vector3I cell in rayCastCells)
{
IMySlimBlock block = entityAsGrid.GetCubeBlock(cell);
if (block == null)
{
continue;
}
IMyCubeBlock FatBlock = block.FatBlock;
if (FatBlock == null)
{
pointOfIntersection = entityAsGrid.GridIntegerToWorld(cell);
return(entityAsGrid);
}
else
if (collect_CubeBlock == null || collect_CubeBlock(FatBlock))
{
pointOfIntersection = entityAsGrid.GridIntegerToWorld(cell);
return(entityAsGrid);
}
}
}
else
{
double tMin, tMax;
if (entity.WorldVolume.IntersectRaySphere(worldRay, out tMin, out tMax))
{
pointOfIntersection = tMin * worldRay.Direction + worldRay.Position;
return(entity);
}
}
}
pointOfIntersection = null;
return(null);
}