private static bool Intersects(MyEntity entity, ref LineD localLine, out MyIntersectionResultLineTriangleEx?result, IntersectionFlags flags = DefaultFlags)
{
Logger.TraceLog("Checking entity: " + entity.getBestName() + ", line from: " + localLine.From + ", to: " + localLine.To);
Logger.DebugLog("entity is a MyCubeGrid, please call the appropriate method", Logger.severity.FATAL, condition: entity is MyCubeGrid);
Matrix localMatrix = entity.PositionComp.LocalMatrix;
Matrix invLocal; Matrix.Invert(ref localMatrix, out invLocal);
MatrixD invLocalD = invLocal;
result = entity.ModelCollision.GetTrianglePruningStructure().GetIntersectionWithLine(entity, ref localLine, ref invLocalD, flags);
if (result.HasValue)
{
return(true);
}
if (entity.Subparts != null)
{
foreach (MyEntitySubpart part in entity.Subparts.Values)
{
if (Intersects(part, ref localLine, out result, flags))
{
return(true);
}
}
}
return(false);
}
protected void traceLog(string toLog, Logger.severity level = Logger.severity.TRACE, bool condition = true, [CallerFilePath] string filePath = null, [CallerMemberName] string member = null, [CallerLineNumber] int lineNumber = 0)
{
if (condition)
{
Logger.TraceLog(toLog, level, GetContext(), GetPrimary(), GetSecondary(), condition, filePath, member, lineNumber);
}
}
/// <summary>
/// Estimates the proximity between a given capsule and a voxel.
/// </summary>
/// <param name="capsule">The capsule to test for intersection, the points must be at least one metre apart.</param>
public static double Proximity(ref CapsuleD capsule, MyVoxelBase voxel, ref Vector3D hitPosition, double capsuleLength = -1d)
{
Logger.TraceLog(capsule.String());
if (capsuleLength < 0)
{
Vector3D.Distance(ref capsule.P0, ref capsule.P1, out capsuleLength);
}
double halfLength = capsuleLength * 0.5d;
Vector3D temp; Vector3D.Add(ref capsule.P0, ref capsule.P1, out temp);
Vector3D middle; Vector3D.Multiply(ref temp, 0.5d, out middle);
if (capsuleLength < 1d)
{
hitPosition = middle;
Logger.TraceLog("capsule length < 1: " + capsuleLength);
return(capsuleLength);
}
double radius = halfLength + capsule.Radius;
BoundingSphereD worldSphere = new BoundingSphereD()
{
Center = middle, Radius = radius
};
if (capsuleLength < Math.Max(capsule.Radius, 1f) * 8f)
{
if (!voxel.ContainsOrIntersects(ref worldSphere))
{
Logger.TraceLog("sphere does not intersect voxel: " + worldSphere + ", capsuleLength: " + capsuleLength);
return(capsuleLength);
}
}
else if (!voxel.PositionComp.WorldAABB.Intersects(ref worldSphere))
{
Logger.TraceLog("sphere does not intersect voxel AABB: " + worldSphere + ", capsuleLength: " + capsuleLength);
return(capsuleLength);
}
CapsuleD halfCapsule;
halfCapsule.P0 = capsule.P0;
halfCapsule.P1 = middle;
halfCapsule.Radius = capsule.Radius;
double prox = Proximity(ref halfCapsule, voxel, ref hitPosition, halfLength);
if (prox < 1f)
{
return(prox);
}
halfCapsule.P0 = middle;
halfCapsule.P1 = capsule.P1;
return(Math.Min(prox, Proximity(ref halfCapsule, voxel, ref hitPosition, halfLength)));
}
/// <summary>
/// Calculate the angular difference in X/Z plane from one vector to another.
/// </summary>
/// <param name="current">The vector that represents the current position.</param>
/// <param name="target">The vector that represents the target position.</param>
/// <param name="delta1">The change in angle from current to target, rotating the shorter distance.</param>
/// <param name="delta2">The change in angle from current to target, rotating the longer distance.</param>
private static void CalcDelta(Vector3 current, Vector3 target, out float delta1, out float delta2)
{
Logger.TraceLog(nameof(current) + " is " + current);
Logger.TraceLog(nameof(target) + " is " + target);
current.Y = 0f;
float temp = current.Normalize();
if (temp == 0f || !temp.IsValid())
{
delta1 = delta2 = temp;
return;
}
target.Y = 0f;
temp = target.Normalize();
if (temp == 0f || !temp.IsValid())
{
delta1 = delta2 = temp;
return;
}
//Debug.Assert(current.Y == 0f, nameof(current) + " does not have Y == 0");
//Debug.Assert(target.Y == 0f, nameof(target) + " does not have Y == 0");
//current.AssertNormalized(nameof(current));
//target.AssertNormalized(nameof(target));
Logger.TraceLog("normalized " + nameof(current) + " is " + current);
Logger.TraceLog("normalized " + nameof(target) + " is " + target);
float dot; Vector3.Dot(ref current, ref target, out dot);
if (dot > 0.99999f)
{
Logger.TraceLog("Already facing desired direction");
// already facing the desired direction
delta1 = delta2 = 0f;
return;
}
float angle = (float)Math.Acos(dot);
angle.AssertIsValid();
if (target.Z * current.X - target.X * current.Z > 0f) // Y component of cross product > 0
{
delta1 = angle;
delta2 = angle - MathHelper.TwoPi;
}
else
{
delta1 = -angle;
delta2 = MathHelper.TwoPi - angle;
}
Logger.TraceLog(nameof(delta1) + " is " + delta1);
Logger.TraceLog(nameof(delta2) + " is " + delta2);
return;
}
/// <summary>
/// Tests a line for intersection with models of blocks on the grid.
/// </summary>
public static bool Intersects(this MySlimBlock slim, ref LineD localLine, out MyIntersectionResultLineTriangleEx?result, IntersectionFlags flags = DefaultFlags)
{
Logger.TraceLog("Checking slim block: " + slim.getBestName() + ", line from: " + localLine.From + ", to: " + localLine.To);
if (slim.FatBlock != null)
{
return(Intersects(slim.FatBlock, ref localLine, out result));
}
MyCube cube;
if (!slim.CubeGrid.TryGetCube(slim.Position, out cube))
{
throw new Exception("Failed to get MyCube for " + slim.nameWithId());
}
foreach (MyCubePart part in cube.Parts)
{
Matrix localMatrix = part.InstanceData.LocalMatrix;
Matrix invLocal; Matrix.Invert(ref localMatrix, out invLocal);
MatrixD invLocalD = invLocal;
Logger.TraceLog("Checking part: " + part.Model.AssetName + ", line from " + localLine.From + " to " + localLine.To + " becomes " +
Vector3D.Transform(localLine.From, ref invLocalD) + " to " + Vector3D.Transform(localLine.To, ref invLocalD));
result = part.Model.GetTrianglePruningStructure().GetIntersectionWithLine(slim.CubeGrid, ref localLine, ref invLocalD, flags);
if (result.HasValue)
{
MyIntersectionResultLineTriangleEx value = result.Value;
Vector3.Transform(ref value.IntersectionPointInObjectSpace, ref localMatrix, out value.IntersectionPointInObjectSpace);
Matrix orientation = localMatrix.GetOrientation();
Vector3.Transform(ref value.NormalInObjectSpace, ref orientation, out value.NormalInObjectSpace);
result = value;
return(true);
}
}
result = null;
return(false);
}
/// <summary>
/// Tests a line for intersection with models of blocks on the grid.
/// </summary>
public static bool Intersects(this MyCubeBlock block, ref LineD localLine, out MyIntersectionResultLineTriangleEx?result, IntersectionFlags flags = DefaultFlags)
{
Logger.TraceLog("Checking cube block: " + block.getBestName() + ", line from: " + localLine.From + ", to: " + localLine.To);
MyCompoundCubeBlock compound = block as MyCompoundCubeBlock;
if (compound != null)
{
foreach (MySlimBlock subBlock in compound.GetBlocks())
{
if (Intersects(subBlock, ref localLine, out result))
{
return(true);
}
}
result = null;
return(false);
}
return(Intersects((MyEntity)block, ref localLine, out result, flags));
}
/// <summary>
/// Estimates the proximity between a given capsule and any voxel.
/// </summary>
/// <param name="capsule">The capsule to test for intersection, the points must be at least one metre apart.</param>
public static double ProximityToVoxel(ref CapsuleD capsule, out MyVoxelBase hitVoxel, out Vector3D hitPosition, bool checkPlanet, double capsuleLength = -1d)
{
Profiler.StartProfileBlock();
if (capsuleLength < 0)
{
Vector3D.Distance(ref capsule.P0, ref capsule.P1, out capsuleLength);
}
double halfLength = capsuleLength * 0.5d;
Vector3D temp; Vector3D.Add(ref capsule.P0, ref capsule.P1, out temp);
Vector3D middle; Vector3D.Multiply(ref temp, 0.5d, out middle);
double radius = halfLength + capsule.Radius;
BoundingSphereD worldSphere = new BoundingSphereD()
{
Center = middle, Radius = radius
};
List <MyVoxelBase> voxels = ResourcePool <List <MyVoxelBase> > .Get();
MyGamePruningStructure.GetAllVoxelMapsInSphere(ref worldSphere, voxels);
double closestProx = double.MaxValue;
hitPosition = Globals.Invalid;
foreach (MyVoxelBase voxel in voxels)
{
if (voxel is MyVoxelMap || voxel is MyPlanet && checkPlanet)
{
if (voxel is MyPlanet)
{
LineSegmentD line;
ResourcePool.Get(out line);
line.From = capsule.P0;
line.To = capsule.P1;
double minDistSq = line.DistanceSquared(voxel.GetCentre());
double maxPlanetRadius = ((MyPlanet)voxel).MaximumRadius;
ResourcePool.Return(line);
if (minDistSq > maxPlanetRadius * maxPlanetRadius)
{
Logger.DebugLog("Skip planet from: " + line.From + ", to: " + line.To + ", planet centre: " + voxel.GetCentre() + ", min dist sq to centre: " + minDistSq + ", max planet radius: " + maxPlanetRadius);
continue;
}
Logger.DebugLog("Check planet from: " + line.From + ", to: " + line.To + ", planet centre: " + voxel.GetCentre() + ", min dist sq to centre: " + minDistSq + ", max planet radius: " + maxPlanetRadius);
}
double proximity = Proximity(ref capsule, voxel, ref hitPosition, capsuleLength);
if (proximity < closestProx)
{
Logger.TraceLog("proximity is less than closest. proximity: " + proximity + ", closest: " + closestProx);
closestProx = proximity;
}
if (closestProx < 1d)
{
Logger.TraceLog("closest under 1: " + closestProx);
hitVoxel = voxel;
voxels.Clear();
ResourcePool.Return(voxels);
Profiler.EndProfileBlock();
return(closestProx);
}
}
}
voxels.Clear();
ResourcePool.Return(voxels);
hitVoxel = null;
Profiler.EndProfileBlock();
return(closestProx);
}
private static bool ArePerpendicular(IMyMotorStator statorOne, IMyMotorStator statorTwo)
{
Logger.TraceLog(nameof(statorOne) + ": " + statorOne.nameWithId() + ", Up: " + statorOne.WorldMatrix.Up + ", " + nameof(statorTwo) + ": " + statorTwo.nameWithId() + ", Up: " + statorTwo.WorldMatrix.Up + ", dot: " + statorOne.WorldMatrix.Up.Dot(statorTwo.WorldMatrix.Up));
return(Math.Abs(statorOne.WorldMatrix.Up.Dot(statorTwo.WorldMatrix.Up)) < 0.1f);
}
/// <summary>
/// Yields the first occupied cells encountered when raycasting a grid in a given base direction.
/// </summary>
/// <param name="grid">The grid to get blocks from.</param>
/// <param name="baseDirection">The direction of ray.</param>
public static IEnumerable <Vector3I> FirstBlocks(this IMyCubeGrid grid, Vector3I baseDirection)
{
Logger.DebugLog("baseDirection(" + baseDirection + ") has a magnitude", Logger.severity.FATAL, condition: baseDirection.RectangularLength() != 1);
BoundingBox localAABB = grid.LocalAABB;
Vector3I min = grid.Min, max = grid.Max;
// ???
//Vector3 minF; Vector3.Divide(ref localAABB.Min, grid.GridSize, out minF);
//Vector3 maxF; Vector3.Divide(ref localAABB.Max, grid.GridSize, out maxF);
//Vector3I min, max;
//Func<float, int> round = f => (int)Math.Round(f);
//minF.ApplyOperation(round, out min);
//maxF.ApplyOperation(round, out max);
Vector3I perp0, perp1;
perp0 = Base6Directions.GetIntVector(Base6Directions.GetPerpendicular(Base6Directions.GetDirection(baseDirection)));
Vector3I.Cross(ref baseDirection, ref perp0, out perp1);
int baseStart; Vector3I.Dot(ref baseDirection, ref min, out baseStart);
int baseEnd; Vector3I.Dot(ref baseDirection, ref max, out baseEnd);
if (baseStart > baseEnd)
{
int temp = baseStart;
baseStart = baseEnd;
baseEnd = temp;
}
bool incrementBase = baseStart <= baseEnd;
int perp0Min; Vector3I.Dot(ref perp0, ref min, out perp0Min);
int perp0Max; Vector3I.Dot(ref perp0, ref max, out perp0Max);
if (perp0Max < perp0Min)
{
int temp = perp0Max;
perp0Max = perp0Min;
perp0Min = temp;
}
int perp1Min; Vector3I.Dot(ref perp1, ref min, out perp1Min);
int perp1Max; Vector3I.Dot(ref perp1, ref max, out perp1Max);
if (perp1Max < perp1Min)
{
int temp = perp1Max;
perp1Max = perp1Min;
perp1Min = temp;
}
Logger.TraceLog("min: " + min + ", max: " + max, Logger.severity.DEBUG);
Logger.TraceLog("base: " + baseDirection + ", perp0: " + perp0 + ", perp1: " + perp1, Logger.severity.DEBUG);
Logger.TraceLog("base range: " + baseStart + ":" + baseEnd, Logger.severity.DEBUG);
Logger.TraceLog("perp0 range: " + perp0Min + ":" + perp0Max, Logger.severity.DEBUG);
Logger.TraceLog("perp1 range: " + perp1Min + ":" + perp1Max, Logger.severity.DEBUG);
for (int perp0Value = perp0Min; perp0Value <= perp0Max; perp0Value++)
{
for (int perp1Value = perp1Min; perp1Value <= perp1Max; perp1Value++)
{
int baseValue = baseStart;
while (true)
{
Vector3I cell = baseValue * baseDirection + perp0Value * perp0 + perp1Value * perp1;
if (grid.CubeExists(cell))
{
yield return(cell);
break;
}
if (baseValue == baseEnd)
{
break;
}
if (incrementBase)
{
baseValue++;
}
else
{
baseValue--;
}
}
}
}
yield break;
}