public unsafe void Test(ref BoxWide a, ref ConvexHullWide b, ref Vector <float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, out Convex4ContactManifoldWide manifold) { Matrix3x3Wide.CreateFromQuaternion(orientationA, out var boxOrientation); Matrix3x3Wide.CreateFromQuaternion(orientationB, out var hullOrientation); Matrix3x3Wide.MultiplyByTransposeWithoutOverlap(boxOrientation, hullOrientation, out var hullLocalBoxOrientation); Matrix3x3Wide.TransformByTransposedWithoutOverlap(offsetB, hullOrientation, out var localOffsetB); Vector3Wide.Negate(localOffsetB, out var localOffsetA); Vector3Wide.Length(localOffsetA, out var centerDistance); Vector3Wide.Scale(localOffsetA, Vector <float> .One / centerDistance, out var initialNormal); var useInitialFallback = Vector.LessThan(centerDistance, new Vector <float>(1e-8f)); initialNormal.X = Vector.ConditionalSelect(useInitialFallback, Vector <float> .Zero, initialNormal.X); initialNormal.Y = Vector.ConditionalSelect(useInitialFallback, Vector <float> .One, initialNormal.Y); initialNormal.Z = Vector.ConditionalSelect(useInitialFallback, Vector <float> .Zero, initialNormal.Z); var hullSupportFinder = default(ConvexHullSupportFinder); var boxSupportFinder = default(BoxSupportFinder); ManifoldCandidateHelper.CreateInactiveMask(pairCount, out var inactiveLanes); b.EstimateEpsilonScale(inactiveLanes, out var hullEpsilonScale); var epsilonScale = Vector.Min(Vector.Max(a.HalfWidth, Vector.Max(a.HalfHeight, a.HalfLength)), hullEpsilonScale); var depthThreshold = -speculativeMargin; DepthRefiner <ConvexHull, ConvexHullWide, ConvexHullSupportFinder, PhysicsBox, BoxWide, BoxSupportFinder> .FindMinimumDepth( b, a, localOffsetA, hullLocalBoxOrientation, ref hullSupportFinder, ref boxSupportFinder, initialNormal, inactiveLanes, 1e-5f *epsilonScale, depthThreshold, out var depth, out var localNormal, out var closestOnHull); inactiveLanes = Vector.BitwiseOr(inactiveLanes, Vector.LessThan(depth, depthThreshold)); //Not every lane will generate contacts. Rather than requiring every lane to carefully clear all contactExists states, just clear them up front. manifold = default; manifold.Contact0Exists = default; manifold.Contact1Exists = default; manifold.Contact2Exists = default; manifold.Contact3Exists = default; if (Vector.LessThanAll(inactiveLanes, Vector <int> .Zero)) { //No contacts generated. return; } //Identify the box face. Matrix3x3Wide.TransformByTransposedWithoutOverlap(localNormal, hullLocalBoxOrientation, out var localNormalInA); Vector3Wide.Abs(localNormalInA, out var absLocalNormalInA); var useX = Vector.BitwiseAnd(Vector.GreaterThan(absLocalNormalInA.X, absLocalNormalInA.Y), Vector.GreaterThan(absLocalNormalInA.X, absLocalNormalInA.Z)); var useY = Vector.AndNot(Vector.GreaterThan(absLocalNormalInA.Y, absLocalNormalInA.Z), useX); Vector3Wide.ConditionalSelect(useX, hullLocalBoxOrientation.X, hullLocalBoxOrientation.Z, out var boxFaceNormal); Vector3Wide.ConditionalSelect(useY, hullLocalBoxOrientation.Y, boxFaceNormal, out boxFaceNormal); Vector3Wide.ConditionalSelect(useX, hullLocalBoxOrientation.Y, hullLocalBoxOrientation.X, out var boxFaceX); Vector3Wide.ConditionalSelect(useY, hullLocalBoxOrientation.Z, boxFaceX, out boxFaceX); Vector3Wide.ConditionalSelect(useX, hullLocalBoxOrientation.Z, hullLocalBoxOrientation.Y, out var boxFaceY); Vector3Wide.ConditionalSelect(useY, hullLocalBoxOrientation.X, boxFaceY, out boxFaceY); var negateFace = Vector.ConditionalSelect(useX, Vector.GreaterThan(localNormalInA.X, Vector <float> .Zero), Vector.ConditionalSelect(useY, Vector.GreaterThan(localNormalInA.Y, Vector <float> .Zero), Vector.GreaterThan(localNormalInA.Z, Vector <float> .Zero))); Vector3Wide.ConditionallyNegate(negateFace, ref boxFaceNormal); //Winding is important; flip the face bases if necessary. Vector3Wide.ConditionallyNegate(Vector.OnesComplement(negateFace), ref boxFaceX); var boxFaceHalfWidth = Vector.ConditionalSelect(useX, a.HalfHeight, Vector.ConditionalSelect(useY, a.HalfLength, a.HalfWidth)); var boxFaceHalfHeight = Vector.ConditionalSelect(useX, a.HalfLength, Vector.ConditionalSelect(useY, a.HalfWidth, a.HalfHeight)); var boxFaceNormalOffset = Vector.ConditionalSelect(useX, a.HalfWidth, Vector.ConditionalSelect(useY, a.HalfHeight, a.HalfLength)); Vector3Wide.Scale(boxFaceNormal, boxFaceNormalOffset, out var boxFaceCenterOffset); Vector3Wide.Add(boxFaceCenterOffset, localOffsetA, out var boxFaceCenter); Vector3Wide.Scale(boxFaceX, boxFaceHalfWidth, out var boxFaceXOffset); Vector3Wide.Scale(boxFaceY, boxFaceHalfHeight, out var boxFaceYOffset); Vector3Wide.Subtract(boxFaceCenter, boxFaceXOffset, out var v0); Vector3Wide.Add(boxFaceCenter, boxFaceXOffset, out var v1); Vector3Wide.Subtract(v0, boxFaceYOffset, out var v00); Vector3Wide.Add(v0, boxFaceYOffset, out var v01); Vector3Wide.Subtract(v1, boxFaceYOffset, out var v10); Vector3Wide.Add(v1, boxFaceYOffset, out var v11); //To find the contact manifold, we'll clip the box edges against the hull face as usual, but we're dealing with potentially //distinct convex hulls. Rather than vectorizing over the different hulls, we vectorize within each hull. Helpers.FillVectorWithLaneIndices(out var slotOffsetIndices); var boundingPlaneEpsilon = 1e-3f * epsilonScale; //There can be no more than 8 contacts (provided there are no numerical errors); 2 per box edge. var candidates = stackalloc ManifoldCandidateScalar[8]; for (int slotIndex = 0; slotIndex < pairCount; ++slotIndex) { if (inactiveLanes[slotIndex] < 0) { continue; } ref var hull = ref b.Hulls[slotIndex]; ConvexHullTestHelper.PickRepresentativeFace(ref hull, slotIndex, ref localNormal, closestOnHull, slotOffsetIndices, ref boundingPlaneEpsilon, out var slotFaceNormal, out var slotLocalNormal, out var bestFaceIndex); //Test each face edge plane against the box face. //Note that we do not use the faceNormal x edgeOffset edge plane, but rather edgeOffset x localNormal. //The faces are wound counterclockwise in right handed coordinates. //X is 00->10; Y is 10->11; Z is 11->01; W is 01->00. ref var v00Slot = ref GatherScatter.GetOffsetInstance(ref v00, slotIndex);