private static void GetIntersectingFace(MatrixD matrix, Vector3D hitPosLocal, ICollection <int> impactFaces)
{
var boxMax = matrix.Backward + matrix.Right + matrix.Up;
var boxMin = -boxMax;
var box = new BoundingBoxD(boxMin, boxMax);
var maxWidth = box.Max.LengthSquared();
var testLine = new LineD(Vector3D.Zero, Vector3D.Normalize(hitPosLocal) * maxWidth); //This is to ensure we intersect the box
LineD testIntersection;
box.Intersect(ref testLine, out testIntersection);
var intersection = testIntersection.To;
var projFront = VectorProjection(intersection, matrix.Forward);
if (projFront.LengthSquared() >= 0.65 * matrix.Forward.LengthSquared()) //if within the side thickness
{
impactFaces.Add(intersection.Dot(matrix.Forward) > 0 ? 5 : 4);
}
var projLeft = VectorProjection(intersection, matrix.Left);
if (projLeft.LengthSquared() >= 0.65 * matrix.Left.LengthSquared()) //if within the side thickness
{
impactFaces.Add(intersection.Dot(matrix.Left) > 0 ? 1 : 0);
}
var projUp = VectorProjection(intersection, matrix.Up);
if (projUp.LengthSquared() >= 0.65 * matrix.Up.LengthSquared()) //if within the side thickness
{
impactFaces.Add(intersection.Dot(matrix.Up) > 0 ? 2 : 3);
}
}
internal static bool FaceIntersected(MatrixD shieldShape, MatrixD gridLocalMatrix, Vector3D worldPos, Vector3I faces)
{
var referenceLocalPosition = gridLocalMatrix.Translation;
var worldDirection = worldPos - referenceLocalPosition;
var localPosition = Vector3D.TransformNormal(worldDirection, MatrixD.Transpose(gridLocalMatrix));
var impactTransNorm = localPosition - shieldShape.Translation;
var boxMax = shieldShape.Backward + shieldShape.Right + shieldShape.Up;
var boxMin = -boxMax;
var box = new BoundingBoxD(boxMin, boxMax);
var maxWidth = box.Max.LengthSquared();
Vector3D norm;
Vector3D.Normalize(ref impactTransNorm, out norm);
var testLine = new LineD(Vector3D.Zero, norm * maxWidth); //This is to ensure we intersect the box
LineD testIntersection;
box.Intersect(ref testLine, out testIntersection);
var intersection = testIntersection.To;
var projFront = VectorProjection(intersection, shieldShape.Forward);
if (projFront.LengthSquared() >= 0.8 * shieldShape.Forward.LengthSquared()) //if within the side thickness
{
var face = intersection.Dot(shieldShape.Forward) > 0 ? 5 : 4;
Log.Line($"FaceIntersected (4-5): {face} - {faces}");
if (faces.Z == 2 || faces.Z != 0 && face == 5 && faces.Z == -1 || face == 4 && faces.Z == 1)
{
return(true);
}
}
var projLeft = VectorProjection(intersection, shieldShape.Left);
if (projLeft.LengthSquared() >= 0.8 * shieldShape.Left.LengthSquared()) //if within the side thickness
{
var face = intersection.Dot(shieldShape.Left) > 0 ? 1 : 0;
Log.Line($"FaceIntersected (1-0): {face} - {faces}");
if (faces.X == 2 || faces.X != 0 && face == 1 && faces.X == -1 || face == 0 && faces.X == 1)
{
return(true);
}
}
var projUp = VectorProjection(intersection, shieldShape.Up);
if (projUp.LengthSquared() >= 0.8 * shieldShape.Up.LengthSquared()) //if within the side thickness
{
var face = intersection.Dot(shieldShape.Up) > 0 ? 2 : 3;
Log.Line($"FaceIntersected(2-3): {face} - {faces}");
if (faces.Y == 2 || faces.Y != 0 && face == 2 && faces.Y == -1 || face == 3 && faces.Y == 1)
{
return(true);
}
}
return(false);
}
public override bool IsOverlapOverThreshold(BoundingBoxD worldAabb, float thresholdPercentage)
{
//Debug.Assert(
// worldAabb.Size.X > MyVoxelConstants.VOXEL_SIZE_IN_METRES &&
// worldAabb.Size.Y > MyVoxelConstants.VOXEL_SIZE_IN_METRES &&
// worldAabb.Size.Z > MyVoxelConstants.VOXEL_SIZE_IN_METRES,
// "One of the sides of queried AABB is too small compared to voxel size. Results will be unreliable.");
Vector3I minCorner, maxCorner;
MyVoxelCoordSystems.WorldPositionToVoxelCoord(PositionLeftBottomCorner, ref worldAabb.Min, out minCorner);
MyVoxelCoordSystems.WorldPositionToVoxelCoord(PositionLeftBottomCorner, ref worldAabb.Max, out maxCorner);
minCorner += StorageMin;
maxCorner += StorageMin;
Storage.ClampVoxelCoord(ref minCorner);
Storage.ClampVoxelCoord(ref maxCorner);
m_storageCache.Resize(minCorner, maxCorner);
Storage.ReadRange(m_storageCache, MyStorageDataTypeFlags.Content, 0, ref minCorner, ref maxCorner);
BoundingBoxD voxelBox;
//MyRenderProxy.DebugDrawAABB(worldAabb, Color.White, 1f, 1f, true);
var invFullVoxel = 1.0 / (double)MyVoxelConstants.VOXEL_CONTENT_FULL_FLOAT;
var voxelVolume = 1.0 / (double)MyVoxelConstants.VOXEL_VOLUME_IN_METERS;
double overlapContentVolume = 0.0;
var queryVolume = worldAabb.Volume;
//using (var batch = MyRenderProxy.DebugDrawBatchAABB(Matrix.Identity, new Color(Color.Green, 0.1f), true, true))
{
Vector3I coord, cache;
for (coord.Z = minCorner.Z, cache.Z = 0; coord.Z <= maxCorner.Z; coord.Z++, cache.Z++)
{
for (coord.Y = minCorner.Y, cache.Y = 0; coord.Y <= maxCorner.Y; coord.Y++, cache.Y++)
{
for (coord.X = minCorner.X, cache.X = 0; coord.X <= maxCorner.X; coord.X++, cache.X++)
{
MyVoxelCoordSystems.VoxelCoordToWorldAABB(PositionLeftBottomCorner, ref coord, out voxelBox);
if (worldAabb.Intersects(voxelBox))
{
var contentVolume = m_storageCache.Content(ref cache) * invFullVoxel * voxelVolume;
var overlapVolume = worldAabb.Intersect(voxelBox).Volume;
overlapContentVolume += contentVolume * overlapVolume;
//batch.Add(ref voxelBox);
}
}
}
}
}
var overlapVolumePercentage = overlapContentVolume / queryVolume;
//MyRenderProxy.DebugDrawText3D(worldAabb.Center, overlapVolumePercentage.ToString("0.000"), Color.White, 1f, false);
return(overlapVolumePercentage >= thresholdPercentage);
}
public void SubtractBB(ref BoundingBoxD bb)
{
if (m_bBox.Intersects(ref bb) == false)
{
return;
}
BoundingBoxD intersected = m_bBox.Intersect(bb);
m_sampler.Subtract(ref m_bBox, ref intersected);
}
private void GetIntersectingFace(MatrixD matrix, Vector3D hitPosLocal, ICollection <Session.ShieldSides> impactFaces)
{
if (Shield?.ShellActive == null)
{
return;
}
var boxMax = matrix.Backward + matrix.Right + matrix.Up;
var boxMin = -boxMax;
var box = new BoundingBoxD(boxMin, boxMax);
var maxWidth = box.Max.LengthSquared();
var testLine = new LineD(Vector3D.Zero, Vector3D.Normalize(hitPosLocal) * maxWidth); //This is to ensure we intersect the box
LineD testIntersection;
box.Intersect(ref testLine, out testIntersection);
var intersection = testIntersection.To;
var projFront = VectorProjection(intersection, matrix.Forward);
if (projFront.LengthSquared() >= 0.65 * matrix.Forward.LengthSquared()) //if within the side thickness
{
var dot = intersection.Dot(matrix.Forward);
//var face = intersection.Dot(matrix.Forward) > 0 ? Shield.RealSideStates[(Session.ShieldSides)0].Side : Shield.RealSideStates[(Session.ShieldSides)1].Side;
var face = dot > 0 ? Session.ShieldSides.Forward : Session.ShieldSides.Backward;
impactFaces.Add(face);
}
var projLeft = VectorProjection(intersection, matrix.Left);
if (projLeft.LengthSquared() >= 0.65 * matrix.Left.LengthSquared()) //if within the side thickness
{
var dot = intersection.Dot(matrix.Left);
//var face = intersection.Dot(matrix.Left) > 0 ? Shield.RealSideStates[(Session.ShieldSides)3].Side : Shield.RealSideStates[(Session.ShieldSides)2].Side;
var face = dot > 0 ? Session.ShieldSides.Left : Session.ShieldSides.Right;
impactFaces.Add(face);
}
var projUp = VectorProjection(intersection, matrix.Up);
if (projUp.LengthSquared() >= 0.65 * matrix.Up.LengthSquared()) //if within the side thickness
{
var dot = intersection.Dot(matrix.Up);
//var face = intersection.Dot(matrix.Up) > 0 ? Shield.RealSideStates[Session.ShieldSides.Up].Side : Shield.RealSideStates[Session.ShieldSides.Down].Side;
var face = dot > 0 ? Session.ShieldSides.Up : Session.ShieldSides.Down;
impactFaces.Add(face);
}
}
public static double GetIntersectingSurfaceArea(MatrixD matrix, Vector3D hitPosLocal)
{
var surfaceArea = -1d;
var boxMax = matrix.Backward + matrix.Right + matrix.Up;
var boxMin = -boxMax;
var box = new BoundingBoxD(boxMin, boxMax);
var maxWidth = box.Max.LengthSquared();
var testLine = new LineD(Vector3D.Zero, Vector3D.Normalize(hitPosLocal) * maxWidth);
LineD testIntersection;
box.Intersect(ref testLine, out testIntersection);
var intersection = testIntersection.To;
var epsilon = 1e-6;
var projFront = VectorProjection(intersection, matrix.Forward);
if (Math.Abs(projFront.LengthSquared() - matrix.Forward.LengthSquared()) < epsilon)
{
var a = Vector3D.Distance(matrix.Left, matrix.Right);
var b = Vector3D.Distance(matrix.Up, matrix.Down);
surfaceArea = a * b;
}
var projLeft = VectorProjection(intersection, matrix.Left);
if (Math.Abs(projLeft.LengthSquared() - matrix.Left.LengthSquared()) < epsilon)
{
var a = Vector3D.Distance(matrix.Forward, matrix.Backward);
var b = Vector3D.Distance(matrix.Up, matrix.Down);
surfaceArea = a * b;
}
var projUp = VectorProjection(intersection, matrix.Up);
if (Math.Abs(projUp.LengthSquared() - matrix.Up.LengthSquared()) < epsilon)
{
var a = Vector3D.Distance(matrix.Forward, matrix.Backward);
var b = Vector3D.Distance(matrix.Left, matrix.Right);
surfaceArea = a * b;
}
return(surfaceArea);
}
public float GetVoxelContentInBoundingBox_Obsolete(BoundingBoxD worldAabb, out float cellCount)
{
MyPrecalcComponent.AssertUpdateThread();
cellCount = 0;
float result = 0;
Vector3I minCorner, maxCorner;
MyVoxelCoordSystems.WorldPositionToVoxelCoord(PositionLeftBottomCorner, ref worldAabb.Min, out minCorner);
MyVoxelCoordSystems.WorldPositionToVoxelCoord(PositionLeftBottomCorner, ref worldAabb.Max, out maxCorner);
minCorner += StorageMin;
maxCorner += StorageMin;
Storage.ClampVoxelCoord(ref minCorner);
Storage.ClampVoxelCoord(ref maxCorner);
m_tempStorage.Resize(minCorner, maxCorner);
Storage.ReadRange(m_tempStorage, MyStorageDataTypeFlags.Content, 0, ref minCorner, ref maxCorner);
BoundingBoxD voxelBox;
Vector3I coord, cache;
for (coord.Z = minCorner.Z, cache.Z = 0; coord.Z <= maxCorner.Z; coord.Z++, cache.Z++)
{
for (coord.Y = minCorner.Y, cache.Y = 0; coord.Y <= maxCorner.Y; coord.Y++, cache.Y++)
{
for (coord.X = minCorner.X, cache.X = 0; coord.X <= maxCorner.X; coord.X++, cache.X++)
{
MyVoxelCoordSystems.VoxelCoordToWorldAABB(PositionLeftBottomCorner - StorageMin * MyVoxelConstants.VOXEL_SIZE_IN_METRES, ref coord, out voxelBox);
if (worldAabb.Intersects(voxelBox))
{
float content = m_tempStorage.Content(ref cache) / MyVoxelConstants.VOXEL_CONTENT_FULL_FLOAT;
float containPercent = (float)(worldAabb.Intersect(voxelBox).Volume / MyVoxelConstants.VOXEL_VOLUME_IN_METERS);
result += content * containPercent;
cellCount += containPercent;
}
}
}
}
return(result);
}
private void UpdateSlimBlock()
{
var grid = _root as IMyCubeGrid;
_block = grid?.FirstBlock(_hit.Position,
_hit.Position + _maxPrediction * _ray.Direction);
if (_block == null)
{
return;
}
Vector3 halfExtents;
_block.ComputeScaledHalfExtents(out halfExtents);
Vector3D center;
_block.ComputeScaledCenter(out center);
var bb = new BoundingBoxD(center - halfExtents, center + halfExtents);
LineD l;
var m = grid.WorldMatrixNormalizedInv;
l.From = Vector3D.Transform(_ray.From, ref m);
l.To = Vector3D.Transform(_ray.To, ref m);
l.Direction = Vector3D.TransformNormal(_ray.Direction, ref m);
l.Length = _ray.Length;
double t1, t2;
if (bb.Intersect(ref l, out t1, out t2))
{
_hitPosition = _ray.From + _ray.Direction * t1;
}
else
{
_hitPosition = _ray.From + (Vector3.Transform(center, grid.WorldMatrix) - _ray.From).Dot(_ray.Direction) * _ray.Direction;
}
}
public float GetVoxelContentInBoundingBox_Obsolete(BoundingBoxD worldAabb, out float cellCount)
{
MyPrecalcComponent.AssertUpdateThread();
cellCount = 0;
float result = 0;
Vector3I minCorner, maxCorner;
MyVoxelCoordSystems.WorldPositionToVoxelCoord(PositionLeftBottomCorner, ref worldAabb.Min, out minCorner);
MyVoxelCoordSystems.WorldPositionToVoxelCoord(PositionLeftBottomCorner, ref worldAabb.Max, out maxCorner);
minCorner += StorageMin;
maxCorner += StorageMin;
Storage.ClampVoxelCoord(ref minCorner);
Storage.ClampVoxelCoord(ref maxCorner);
m_storageCache.Resize(minCorner, maxCorner);
Storage.ReadRange(m_storageCache, MyStorageDataTypeFlags.Content, 0, ref minCorner, ref maxCorner);
BoundingBoxD voxelBox;
Vector3I coord, cache;
for (coord.Z = minCorner.Z, cache.Z = 0; coord.Z <= maxCorner.Z; coord.Z++, cache.Z++)
{
for (coord.Y = minCorner.Y, cache.Y = 0; coord.Y <= maxCorner.Y; coord.Y++, cache.Y++)
{
for (coord.X = minCorner.X, cache.X = 0; coord.X <= maxCorner.X; coord.X++, cache.X++)
{
MyVoxelCoordSystems.VoxelCoordToWorldAABB(PositionLeftBottomCorner - StorageMin * MyVoxelConstants.VOXEL_SIZE_IN_METRES, ref coord, out voxelBox);
if (worldAabb.Intersects(voxelBox))
{
float content = m_storageCache.Content(ref cache) / MyVoxelConstants.VOXEL_CONTENT_FULL_FLOAT;
float containPercent = (float)(worldAabb.Intersect(voxelBox).Volume / MyVoxelConstants.VOXEL_VOLUME_IN_METERS);
result += content * containPercent;
cellCount += containPercent;
}
}
}
}
return result;
}
/// <summary>
/// Returns true if the given small block connects to large one.
/// </summary>
/// <param name="smallBlock">small block</param>
/// <param name="smallBlockWorldAabb">small block world AABB</param>
/// <param name="largeBlock">large block</param>
/// <param name="largeBlockWorldAabb">large block wotld AABB</param>
/// <returns>true when connected</returns>
private bool SmallBlockConnectsToLarge(MySlimBlock smallBlock, ref BoundingBoxD smallBlockWorldAabb, MySlimBlock largeBlock, ref BoundingBoxD largeBlockWorldAabb)
{
Debug.Assert(smallBlock.BlockDefinition.CubeSize == MyCubeSize.Small);
Debug.Assert(largeBlock.BlockDefinition.CubeSize == MyCubeSize.Large);
Debug.Assert(!(smallBlock.FatBlock is MyCompoundCubeBlock));
Debug.Assert(!(largeBlock.FatBlock is MyCompoundCubeBlock));
BoundingBoxD smallBlockWorldAabbReduced = smallBlockWorldAabb;
smallBlockWorldAabbReduced.Inflate(-smallBlock.CubeGrid.GridSize / 4);
// Small block aabb penetrates large block aabb (large timbers).
bool penetratesAabbs = largeBlockWorldAabb.Contains(smallBlockWorldAabbReduced) == ContainmentType.Intersects;
if (!penetratesAabbs)
{
Vector3D centerToCenter = smallBlockWorldAabb.Center - largeBlockWorldAabb.Center;
Vector3I addDir = Base6Directions.GetIntVector(Base6Directions.GetClosestDirection(centerToCenter));
// Check small grid mount points
Quaternion smallBlockRotation;
smallBlock.Orientation.GetQuaternion(out smallBlockRotation);
smallBlockRotation = Quaternion.CreateFromRotationMatrix(smallBlock.CubeGrid.WorldMatrix) * smallBlockRotation;
if (!MyCubeGrid.CheckConnectivitySmallBlockToLargeGrid(largeBlock.CubeGrid, smallBlock.BlockDefinition, ref smallBlockRotation, ref addDir))
{
return(false);
}
}
BoundingBoxD smallBlockWorldAabbInflated = smallBlockWorldAabb;
smallBlockWorldAabbInflated.Inflate(2 * smallBlock.CubeGrid.GridSize / 3);
// Trim small block aabb with large block aabb.
BoundingBoxD intersectedBox = smallBlockWorldAabbInflated.Intersect(largeBlockWorldAabb);
Vector3D intersectedBoxCenter = intersectedBox.Center;
HkShape shape = new HkBoxShape((Vector3)intersectedBox.HalfExtents);
Quaternion largeRotation;
largeBlock.Orientation.GetQuaternion(out largeRotation);
largeRotation = Quaternion.CreateFromRotationMatrix(largeBlock.CubeGrid.WorldMatrix) * largeRotation;
Vector3D largeTranslation;
largeBlock.ComputeWorldCenter(out largeTranslation);
bool result = false;
try
{
if (largeBlock.FatBlock != null)
{
MyModel model = largeBlock.FatBlock.Model;
if (model != null)
{
HkShape[] shapes = model.HavokCollisionShapes;
if (shapes == null || shapes.Length == 0)
{
return(false);
}
for (int i = 0; i < shapes.Length; ++i)
{
result = MyPhysics.IsPenetratingShapeShape(shape, ref intersectedBoxCenter, ref Quaternion.Identity, shapes[i], ref largeTranslation, ref largeRotation);
if (result)
{
break;
}
}
}
else
{
HkShape shapeLarge = new HkBoxShape(largeBlock.BlockDefinition.Size * largeBlock.CubeGrid.GridSize / 2);
result = MyPhysics.IsPenetratingShapeShape(shape, ref intersectedBoxCenter, ref Quaternion.Identity, shapeLarge, ref largeTranslation, ref largeRotation);
shapeLarge.RemoveReference();
}
}
else
{
HkShape shapeLarge = new HkBoxShape(largeBlock.BlockDefinition.Size * largeBlock.CubeGrid.GridSize / 2);
result = MyPhysics.IsPenetratingShapeShape(shape, ref intersectedBoxCenter, ref Quaternion.Identity, shapeLarge, ref largeTranslation, ref largeRotation);
shapeLarge.RemoveReference();
}
}
finally
{
shape.RemoveReference();
}
return(result);
}
public override bool IsOverlapOverThreshold(BoundingBoxD worldAabb, float thresholdPercentage)
{
//Debug.Assert(
// worldAabb.Size.X > MyVoxelConstants.VOXEL_SIZE_IN_METRES &&
// worldAabb.Size.Y > MyVoxelConstants.VOXEL_SIZE_IN_METRES &&
// worldAabb.Size.Z > MyVoxelConstants.VOXEL_SIZE_IN_METRES,
// "One of the sides of queried AABB is too small compared to voxel size. Results will be unreliable.");
Vector3I minCorner, maxCorner;
MyVoxelCoordSystems.WorldPositionToVoxelCoord(PositionLeftBottomCorner, ref worldAabb.Min, out minCorner);
MyVoxelCoordSystems.WorldPositionToVoxelCoord(PositionLeftBottomCorner, ref worldAabb.Max, out maxCorner);
minCorner += StorageMin;
maxCorner += StorageMin;
Storage.ClampVoxelCoord(ref minCorner);
Storage.ClampVoxelCoord(ref maxCorner);
m_storageCache.Resize(minCorner, maxCorner);
Storage.ReadRange(m_storageCache, MyStorageDataTypeFlags.Content, 0, ref minCorner, ref maxCorner);
BoundingBoxD voxelBox;
//MyRenderProxy.DebugDrawAABB(worldAabb, Color.White, 1f, 1f, true);
var invFullVoxel = 1.0 / (double)MyVoxelConstants.VOXEL_CONTENT_FULL_FLOAT;
var voxelVolume = 1.0 / (double)MyVoxelConstants.VOXEL_VOLUME_IN_METERS;
double overlapContentVolume = 0.0;
var queryVolume = worldAabb.Volume;
//using (var batch = MyRenderProxy.DebugDrawBatchAABB(Matrix.Identity, new Color(Color.Green, 0.1f), true, true))
{
Vector3I coord, cache;
for (coord.Z = minCorner.Z, cache.Z = 0; coord.Z <= maxCorner.Z; coord.Z++, cache.Z++)
{
for (coord.Y = minCorner.Y, cache.Y = 0; coord.Y <= maxCorner.Y; coord.Y++, cache.Y++)
{
for (coord.X = minCorner.X, cache.X = 0; coord.X <= maxCorner.X; coord.X++, cache.X++)
{
MyVoxelCoordSystems.VoxelCoordToWorldAABB(PositionLeftBottomCorner, ref coord, out voxelBox);
if (worldAabb.Intersects(voxelBox))
{
var contentVolume = m_storageCache.Content(ref cache) * invFullVoxel * voxelVolume;
var overlapVolume = worldAabb.Intersect(voxelBox).Volume;
overlapContentVolume += contentVolume * overlapVolume;
//batch.Add(ref voxelBox);
}
}
}
}
}
var overlapVolumePercentage = overlapContentVolume / queryVolume;
//MyRenderProxy.DebugDrawText3D(worldAabb.Center, overlapVolumePercentage.ToString("0.000"), Color.White, 1f, false);
return overlapVolumePercentage >= thresholdPercentage;
}
private void SplitArea(BoundingBoxD box1, BoundingBoxD box2, List<BoundingBoxD> output)
{
var intersectValue = box1.Intersect(box2);
double x1 = Math.Min(intersectValue.Min.X, box2.Min.X);
double x2 = intersectValue.Min.X;
double x3 = intersectValue.Max.X;
double x4 = Math.Max(intersectValue.Max.X, box2.Max.X);
double z1 = Math.Min(intersectValue.Min.Z, box2.Min.Z);
double z2 = intersectValue.Min.Z;
double z3 = intersectValue.Max.Z;
double z4 = Math.Max(intersectValue.Max.Z, box2.Max.Z);
bool sameXMin = x1 == x2;
bool sameXMax = x3 == x4;
bool sameZMin = z1 == z2;
bool sameZMax = z3 == z4;
double minY = DEBUG_BOX_Y_MIN_POS;
double maxY = DEBUG_BOX_Y_MAX_POS;
if (sameXMin && sameXMax)
{
if (sameZMin && !sameZMax)
{
output.Add(new BoundingBoxD(new Vector3D(x1, minY, z3), new Vector3D(x4, maxY, z4)));
}
else if (!sameZMin && sameZMax)
{
output.Add(new BoundingBoxD(new Vector3D(x1, minY, z1), new Vector3D(x4, maxY, z2)));
}
else
{
output.Add(new BoundingBoxD(new Vector3D(x1, minY, z1), new Vector3D(x4, maxY, z2)));
output.Add(new BoundingBoxD(new Vector3D(x1, minY, z3), new Vector3D(x4, maxY, z4)));
}
}
else if (sameZMin && sameZMax)
{
if (sameXMin && !sameXMax)
{
output.Add(new BoundingBoxD(new Vector3D(x3, minY, z1), new Vector3D(x4, maxY, z4)));
}
else if (!sameXMin && sameXMax)
{
output.Add(new BoundingBoxD(new Vector3D(x1, minY, z1), new Vector3D(x2, maxY, z2)));
}
else
{
output.Add(new BoundingBoxD(new Vector3D(x1, minY, z1), new Vector3D(x2, maxY, z4)));
output.Add(new BoundingBoxD(new Vector3D(x3, minY, z1), new Vector3D(x4, maxY, z4)));
}
}
else
{
if (sameXMin)
{
if (sameZMin)
{
output.Add(new BoundingBoxD(new Vector3D(x3, minY, z1), new Vector3D(x4, maxY, z3)));
output.Add(new BoundingBoxD(new Vector3D(x1, minY, z3), new Vector3D(x4, maxY, z4)));
}
else if (sameZMax)
{
output.Add(new BoundingBoxD(new Vector3D(x1, minY, z1), new Vector3D(x4, maxY, z2)));
output.Add(new BoundingBoxD(new Vector3D(x3, minY, z2), new Vector3D(x4, maxY, z4)));
}
else
{
output.Add(new BoundingBoxD(new Vector3D(x1, minY, z1), new Vector3D(x4, maxY, z2)));
output.Add(new BoundingBoxD(new Vector3D(x3, minY, z2), new Vector3D(x4, maxY, z3)));
output.Add(new BoundingBoxD(new Vector3D(x1, minY, z3), new Vector3D(x4, maxY, z4)));
}
}
else if (sameXMax)
{
if (sameZMin)
{
output.Add(new BoundingBoxD(new Vector3D(x1, minY, z1), new Vector3D(x2, maxY, z3)));
output.Add(new BoundingBoxD(new Vector3D(x1, minY, z3), new Vector3D(x4, maxY, z4)));
}
else if (sameZMax)
{
output.Add(new BoundingBoxD(new Vector3D(x1, minY, z1), new Vector3D(x4, maxY, z2)));
output.Add(new BoundingBoxD(new Vector3D(x1, minY, z2), new Vector3D(x4, maxY, z4)));
}
else
{
output.Add(new BoundingBoxD(new Vector3D(x1, minY, z1), new Vector3D(x4, maxY, z2)));
output.Add(new BoundingBoxD(new Vector3D(x1, minY, z2), new Vector3D(x3, maxY, z3)));
output.Add(new BoundingBoxD(new Vector3D(x1, minY, z3), new Vector3D(x4, maxY, z4)));
}
}
else if (sameZMin)
{
output.Add(new BoundingBoxD(new Vector3D(x1, minY, z1), new Vector3D(x2, maxY, z4)));
output.Add(new BoundingBoxD(new Vector3D(x2, minY, z3), new Vector3D(x3, maxY, z4)));
output.Add(new BoundingBoxD(new Vector3D(x3, minY, z1), new Vector3D(x4, maxY, z4)));
}
else if (sameZMax)
{
output.Add(new BoundingBoxD(new Vector3D(x1, minY, z1), new Vector3D(x2, maxY, z4)));
output.Add(new BoundingBoxD(new Vector3D(x2, minY, z1), new Vector3D(x3, maxY, z3)));
output.Add(new BoundingBoxD(new Vector3D(x3, minY, z1), new Vector3D(x4, maxY, z4)));
}
else
{
MyDebug.AssertDebug(false);
}
}
}
private bool SmallBlockConnectsToLarge(MySlimBlock smallBlock, ref BoundingBoxD smallBlockWorldAabb, MySlimBlock largeBlock, ref BoundingBoxD largeBlockWorldAabb)
{
Quaternion quaternion;
Vector3D vectord2;
BoundingBoxD box = smallBlockWorldAabb;
box.Inflate((double)(-smallBlock.CubeGrid.GridSize / 4f));
if (!largeBlockWorldAabb.Intersects(box))
{
Quaternion quaternion2;
Vector3I addNormal = this.GetSmallBlockAddDirection(ref smallBlockWorldAabb, ref box, ref largeBlockWorldAabb);
smallBlock.Orientation.GetQuaternion(out quaternion2);
quaternion2 = Quaternion.CreateFromRotationMatrix(smallBlock.CubeGrid.WorldMatrix) * quaternion2;
if (!MyCubeGrid.CheckConnectivitySmallBlockToLargeGrid(largeBlock.CubeGrid, smallBlock.BlockDefinition, ref quaternion2, ref addNormal))
{
return(false);
}
}
BoundingBoxD xd2 = smallBlockWorldAabb;
xd2.Inflate((double)((2f * smallBlock.CubeGrid.GridSize) / 3f));
BoundingBoxD xd3 = xd2.Intersect(largeBlockWorldAabb);
Vector3D center = xd3.Center;
HkShape shape = (HkShape) new HkBoxShape((Vector3)xd3.HalfExtents);
largeBlock.Orientation.GetQuaternion(out quaternion);
quaternion = Quaternion.CreateFromRotationMatrix(largeBlock.CubeGrid.WorldMatrix) * quaternion;
largeBlock.ComputeWorldCenter(out vectord2);
bool flag = false;
try
{
if (largeBlock.FatBlock == null)
{
HkShape shape3 = (HkShape) new HkBoxShape((largeBlock.BlockDefinition.Size * largeBlock.CubeGrid.GridSize) / 2f);
flag = MyPhysics.IsPenetratingShapeShape(shape, ref center, ref Quaternion.Identity, shape3, ref vectord2, ref quaternion);
shape3.RemoveReference();
}
else
{
MyModel model = largeBlock.FatBlock.Model;
if ((model == null) || (model.HavokCollisionShapes == null))
{
HkShape shape2 = (HkShape) new HkBoxShape((largeBlock.BlockDefinition.Size * largeBlock.CubeGrid.GridSize) / 2f);
flag = MyPhysics.IsPenetratingShapeShape(shape, ref center, ref Quaternion.Identity, shape2, ref vectord2, ref quaternion);
shape2.RemoveReference();
}
else
{
HkShape[] havokCollisionShapes = model.HavokCollisionShapes;
for (int i = 0; i < havokCollisionShapes.Length; i++)
{
flag = MyPhysics.IsPenetratingShapeShape(shape, ref center, ref Quaternion.Identity, havokCollisionShapes[i], ref vectord2, ref quaternion);
if (flag)
{
break;
}
}
}
}
}
finally
{
shape.RemoveReference();
}
return(flag);
}
// To help debug even when using stackalloc
public unsafe bool IntersectInternalWStack(ref LineD line, MyCellCoord * stack, int stackSize) {
#else
int stackSize = MySparseOctree.EstimateStackSize(m_treeHeight);
MyCellCoord* stack = stackalloc MyCellCoord[stackSize];
#endif
var finalB = BoundingBoxD.CreateInvalid();
LineD result;
result.From = line.To;
result.To = line.From;
MyCellCoord data = new MyCellCoord(m_treeHeight + LeafLodCount, ref Vector3I.Zero);
int stackIdx = 0;
stack[stackIdx++] = data;
MyCellCoord cell = new MyCellCoord();
BoundingBoxI bb = new BoundingBoxI(new Vector3I(Vector3.Min(line.From, line.To)), new Vector3I(Vector3.Ceiling(Vector3.Max(line.From, line.To))));
double startOfft, endOfft;
while (stackIdx > 0)
{
Debug.Assert(stackIdx <= stackSize);
data = stack[--stackIdx];
cell.Lod = Math.Max(data.Lod - LeafLodCount, 0);
cell.CoordInLod = data.CoordInLod;
int lodDiff;
IMyOctreeLeafNode leaf;
if (m_contentLeaves.TryGetValue(cell.PackId64(), out leaf))
{
lodDiff = data.Lod;
var rangeMinInDataLod = bb.Min >> lodDiff;
var rangeMaxInDataLod = bb.Max >> lodDiff;
if (data.CoordInLod.IsInsideInclusive(ref rangeMinInDataLod, ref rangeMaxInDataLod))
{
var start = rangeMinInDataLod << data.Lod;
var end = (rangeMaxInDataLod) << data.Lod;
var leafBounds = new BoundingBoxD(start, end);
LineD ll;
leafBounds.Intersect(ref line, out ll);
if (leaf.Intersect(ref ll, out startOfft, out endOfft))
{
if (finalB.Contains(ll.From) == ContainmentType.Disjoint)
{
result.From = ll.From;
finalB.Include(ll.From);
}
if (finalB.Contains(ll.To) == ContainmentType.Disjoint)
{
result.To = ll.To;
finalB.Include(ll.To);
}
}
}
continue;
}
cell.Lod -= 1;
lodDiff = data.Lod - 1;
var node = m_contentNodes[cell.PackId64()];
var min = bb.Min >> lodDiff;
var max = bb.Max >> lodDiff;
var nodePositionInChild = data.CoordInLod << 1;
min -= nodePositionInChild;
max -= nodePositionInChild;
for (int i = 0; i < MyOctreeNode.CHILD_COUNT; ++i)
{
Vector3I childPosRelative;
ComputeChildCoord(i, out childPosRelative);
if (!childPosRelative.IsInsideInclusive(ref min, ref max))
continue;
BoundingBoxD nodeBounds;
nodeBounds.Min = childPosRelative + cell.CoordInLod << lodDiff;
nodeBounds.Max = nodeBounds.Min + (1 << data.Lod);
if (finalB.Contains(nodeBounds) == ContainmentType.Contains) continue;
if (!nodeBounds.Intersects(ref line))
continue;
if (node.HasChild(i))
{
Debug.Assert(stackIdx < stackSize);
stack[stackIdx++] = new MyCellCoord(data.Lod - 1, nodePositionInChild + childPosRelative);
}
}
}
if (!finalB.Valid) return false;
line.To = result.To;
line.From = result.From;
line.Length = (result.To - result.From).Length();
return true;
}