private void Run()
{
Voxel.FindFreeSpace(Deposit, MinRadius, out m_freePosition);
CapsuleD capsule = new CapsuleD(m_freePosition, Deposit, 1f);
Vector3D closest; CapsuleDExtensions.Intersects(ref capsule, Voxel, out closest);
NearSurface = Vector3D.DistanceSquared(Deposit, closest) < 100d;
Completed = true;
}
private void SetNextSurfacePoint()
{
CapsuleD surfaceFinder;
surfaceFinder.Radius = 1f;
float maxRingSize = m_grid.LocalVolume.Radius; maxRingSize *= maxRingSize;
bool overMax = false;
surfaceFinder.P0 = m_grid.GetCentre();
Vector3D targetWorld = m_target.WorldPosition();
for (int i = 0; i < 1000; i++)
{
ExpandingRings.Ring ring = ExpandingRings.GetRing(m_ringIndex);
if (m_squareIndex >= ring.Squares.Length)
{
ring = ExpandingRings.GetRing(++m_ringIndex);
m_squareIndex = 0;
}
Vector2I square = ring.Squares[m_squareIndex++];
Vector3 direct1; Vector3.Multiply(ref m_perp1, square.X, out direct1);
Vector3 direct2; Vector3.Multiply(ref m_perp2, square.Y, out direct2);
surfaceFinder.P1 = targetWorld + direct1 + direct2;
if (CapsuleDExtensions.Intersects(ref surfaceFinder, (MyVoxelBase)m_target.Entity, out m_surfacePoint))
{
Log.DebugLog("test from " + surfaceFinder.P0 + " to " + surfaceFinder.P1 + ", hit voxel at " + m_surfacePoint);
m_finalMine = Vector3D.DistanceSquared(m_surfacePoint, m_target.WorldPosition()) < 1d;
m_stage = Stage.Mine;
return;
}
Log.DebugLog("test from " + surfaceFinder.P0 + " to " + surfaceFinder.P1 + ", did not hit voxel. P1 constructed from " + targetWorld + ", " + direct1 + ", " + direct2);
if (ring.DistanceSquared > maxRingSize)
{
if (overMax)
{
Log.AlwaysLog("Infinite loop", Logger.severity.FATAL);
throw new Exception("Infinte loop");
}
overMax = true;
Log.DebugLog("Over max ring size, starting next level", Logger.severity.INFO);
m_squareIndex = 0;
m_ringIndex = 0;
}
}
Log.AlwaysLog("Infinite loop", Logger.severity.FATAL);
throw new Exception("Infinte loop");
}
protected void SetOutsideTarget(Vector3D direction)
{
PseudoBlock navBlock = m_navBlock;
MyVoxelBase voxel = (MyVoxelBase)m_target.Entity;
CapsuleD capsule;
Vector3D offset; Vector3D.Multiply(ref direction, voxel.PositionComp.LocalVolume.Radius * 2d, out offset);
capsule.P1 = navBlock.WorldPosition;
Vector3D.Add(ref capsule.P1, ref offset, out capsule.P0);
capsule.Radius = m_grid.LocalVolume.Radius * 4f;
Vector3D hitPos;
if (!CapsuleDExtensions.Intersects(ref capsule, voxel, out hitPos))
{
Log.DebugLog("capsule: " + capsule.String() + ", does not intersect voxel", Logger.severity.DEBUG);
hitPos = capsule.P0;
}
//Log.DebugLog((tunnel ? "Tunnel target: " : "Backout target: ") + hitPos, Logger.severity.DEBUG);
m_target.SetWorld(ref hitPos);
}
/// <summary>
/// Tests if the ship is obstructed by any voxel.
/// </summary>
/// <param name="input"><see cref="TestInput"/></param>
/// <param name="result"><see cref="VoxelTestResult"/></param>
/// <returns>True iff a voxel is obstructing the ship.</returns>
public bool RayCastIntersectsVoxel(ref TestInput input, out VoxelTestResult result)
{
Profiler.StartProfileBlock();
Logger.DebugLog("direction vector is invalid: " + input.Direction, Logger.severity.FATAL, condition: !input.Direction.IsValid() || Math.Abs(1f - input.Direction.LengthSquared()) > 0.01f);
Logger.TraceLog(input.ToString());
if (input.Length < 1f)
{
// need to skip as Proximity doesn't work with short capsules
// should be safe, as the ship got here somehow
Logger.TraceLog("Input length is small, no voxel test necessary");
result = VoxelTestResult.Default;
result.Distance = input.Length;
result.Proximity = 1f;
return(false);
}
Vector3D currentPosition = AutopilotGrid.GetCentre();
Vector3 startOffset; Vector3.Multiply(ref input.Direction, StartRayCast, out startOffset);
Vector3D startOffsetD = startOffset;
Vector3D totalOffset; Vector3D.Add(ref input.Offset, ref startOffsetD, out totalOffset);
CapsuleD capsule;
Vector3D.Add(ref currentPosition, ref totalOffset, out capsule.P0);
Vector3D capsuleDisp;
{
capsuleDisp.X = input.Direction.X * input.Length;
capsuleDisp.Y = input.Direction.Y * input.Length;
capsuleDisp.Z = input.Direction.Z * input.Length;
}
Vector3D.Add(ref capsule.P0, ref capsuleDisp, out capsule.P1);
capsule.Radius = AutopilotGrid.PositionComp.LocalVolume.Radius;
//Logger.DebugLog("current position: " + currentPosition + ", offset: " + offset + ", line: " + rayDirectionLength + ", start: " + capsule.P0 + ", end: " + capsule.P1);
result = VoxelTestResult.Default;
Vector3D hitPosition;
float proximity = (float)CapsuleDExtensions.ProximityToVoxel(ref capsule, out result.ObstructingVoxel, out hitPosition, true, input.Length);
result.Proximity = 10f * proximity; // lie because we have not done a proper test but could have a very nice result
if (proximity > 1f)
{
Logger.TraceLog("Large capsule DOES NOT intersect voxel: " + capsule.String() + ", proximity: " + proximity + "/" + result.Proximity);
result.Distance = input.Length;
Profiler.EndProfileBlock();
return(false);
}
Logger.TraceLog("Large capsule DOES intersect voxel: " + capsule.String() + ", proximity: " + proximity + "/" + result.Proximity);
IEnumerable <CubeGridCache> myCaches = AttachedGrid.AttachedGrids(AutopilotGrid, AttachedGrid.AttachmentKind.Physics, true).Select(CubeGridCache.GetFor);
Vector3 v; input.Direction.CalculatePerpendicularVector(out v);
Vector3 w; Vector3.Cross(ref input.Direction, ref v, out w);
Matrix to3D = new Matrix(v.X, v.Y, v.Z, 0f,
w.X, w.Y, w.Z, 0f,
input.Direction.X, input.Direction.Y, input.Direction.Z, 0f,
0f, 0f, 0f, 1f);
Matrix to2D; Matrix.Invert(ref to3D, out to2D);
Vector2IMatrix <Vector3D> apShipRejections;
ResourcePool.Get(out apShipRejections);
MatrixD worldMatrix = AutopilotGrid.WorldMatrix;
float gridSize = AutopilotGrid.GridSize;
foreach (CubeGridCache cache in myCaches)
{
if (cache == null)
{
Logger.DebugLog("Missing a cache", Logger.severity.DEBUG);
Profiler.EndProfileBlock();
return(false);
}
foreach (Vector3I cell in cache.OccupiedCells())
{
Vector3 local = cell * gridSize;
Vector3D world; Vector3D.Transform(ref local, ref worldMatrix, out world);
Vector3D offsetWorld; Vector3D.Add(ref world, ref totalOffset, out offsetWorld);
Vector3D relative; Vector3D.Subtract(ref offsetWorld, ref currentPosition, out relative);
Vector3 relativeF = relative;
Vector3 rejection; Vector3.Reject(ref relativeF, ref input.Direction, out rejection);
Vector3 planarComponents; Vector3.Transform(ref rejection, ref to2D, out planarComponents);
Logger.DebugLog("Math fail: rejection: " + rejection + ", planar components: " + planarComponents + "\nto3D: " + to3D, Logger.severity.FATAL, condition: planarComponents.Z > 0.001f || planarComponents.Z < -0.001f);
Vector2 pc2 = new Vector2(planarComponents.X, planarComponents.Y);
apShipRejections.Add(ToCell(pc2, gridSize), offsetWorld);
}
}
Vector2IMatrix <bool> testedRejections;
ResourcePool.Get(out testedRejections);
const int allowedEmpty = 2;
foreach (KeyValuePair <Vector2I, Vector3D> cell in apShipRejections.MiddleOut())
{
//Logger.DebugLog("Cell was not set: " + cell, Logger.severity.FATAL, condition: cell.Value == Vector3D.Zero);
if (!testedRejections.Add(cell.Key, true))
{
continue;
}
int ringIndex = 0;
m_insideRing.Clear();
int biggestRingSq = 0;
while (true)
{
int consecutiveEmpty = 0;
ExpandingRings.Ring ring = ExpandingRings.GetRing(ringIndex++);
foreach (Vector2I ringOffset in ring.Squares)
{
if (apShipRejections.Contains(ringOffset + cell.Key))
{
consecutiveEmpty = 0;
}
else
{
consecutiveEmpty++;
if (consecutiveEmpty > allowedEmpty)
{
goto GotRing;
}
}
}
m_insideRing.AddArray(ring.Squares);
biggestRingSq = ring.DistanceSquared;
}
GotRing:
foreach (Vector2I ringOffset in m_insideRing)
{
testedRejections.Add(ringOffset + cell.Key, true);
}
capsule.P0 = cell.Value;
Vector3D.Add(ref capsule.P0, ref capsuleDisp, out capsule.P1);
capsule.Radius = (1f + (float)Math.Sqrt(biggestRingSq)) * gridSize;
result.Proximity = (float)CapsuleDExtensions.ProximityToVoxel(ref capsule, out result.ObstructingVoxel, out hitPosition, true, input.Length);
if (result.Proximity <= 1f)
{
Logger.TraceLog("Block capsule does hit voxel: " + capsule.String() + ", proxmity: " + result.Proximity);
double distance; Vector3D.Distance(ref capsule.P0, ref hitPosition, out distance);
result.Distance = (float)distance;
apShipRejections.Clear();
testedRejections.Clear();
ResourcePool.Return(apShipRejections);
ResourcePool.Return(testedRejections);
Profiler.EndProfileBlock();
return(true);
}
}
Logger.TraceLog("Ship's path is clear from voxels, proximity: " + result.Proximity);
apShipRejections.Clear();
testedRejections.Clear();
ResourcePool.Return(apShipRejections);
ResourcePool.Return(testedRejections);
Profiler.EndProfileBlock();
return(false);
}
