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; }