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);
public unsafe void Test(
ref BoxWide a, ref BoxWide b, ref Vector <float> speculativeMargin,
ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount,
out Convex4ContactManifoldWide manifold)
{
Matrix3x3Wide.CreateFromQuaternion(orientationA, out var worldRA);
Matrix3x3Wide.CreateFromQuaternion(orientationB, out var worldRB);
Matrix3x3Wide.MultiplyByTransposeWithoutOverlap(worldRB, worldRA, out var rB);
Matrix3x3Wide.TransformByTransposedWithoutOverlap(offsetB, worldRA, out var localOffsetB);
Vector3Wide localNormal;
//b.X
TestEdgeEdge(
ref a.HalfWidth, ref a.HalfHeight, ref a.HalfLength,
ref b.HalfWidth, ref b.HalfHeight, ref b.HalfLength,
ref localOffsetB.X, ref localOffsetB.Y, ref localOffsetB.Z,
ref rB.X, ref rB.Y, ref rB.Z, ref rB.X,
out var depth, out localNormal.X, out localNormal.Y, out localNormal.Z);
//b.Y
TestEdgeEdge(
ref a.HalfWidth, ref a.HalfHeight, ref a.HalfLength,
ref b.HalfWidth, ref b.HalfHeight, ref b.HalfLength,
ref localOffsetB.X, ref localOffsetB.Y, ref localOffsetB.Z,
ref rB.X, ref rB.Y, ref rB.Z, ref rB.Y,
out var edgeYDepth, out var edgeYNX, out var edgeYNY, out var edgeYNZ);
Select(ref depth, ref localNormal,
ref edgeYDepth, ref edgeYNX, ref edgeYNY, ref edgeYNZ);
//b.Z
TestEdgeEdge(
ref a.HalfWidth, ref a.HalfHeight, ref a.HalfLength,
ref b.HalfWidth, ref b.HalfHeight, ref b.HalfLength,
ref localOffsetB.X, ref localOffsetB.Y, ref localOffsetB.Z,
ref rB.X, ref rB.Y, ref rB.Z, ref rB.Z,
out var edgeZDepth, out var edgeZNX, out var edgeZNY, out var edgeZNZ);
Select(ref depth, ref localNormal,
ref edgeZDepth, ref edgeZNX, ref edgeZNY, ref edgeZNZ);
//Test face normals of A. Working in local space of A means potential axes are just (1,0,0) etc.
Vector3Wide.Abs(rB.X, out var absRBX);
Vector3Wide.Abs(rB.Y, out var absRBY);
Vector3Wide.Abs(rB.Z, out var absRBZ);
var faceAXDepth = a.HalfWidth + b.HalfWidth * absRBX.X + b.HalfHeight * absRBY.X + b.HalfLength * absRBZ.X - Vector.Abs(localOffsetB.X);
var one = Vector <float> .One;
var zero = Vector <float> .Zero;
Select(ref depth, ref localNormal, ref faceAXDepth, ref one, ref zero, ref zero);
var faceAYDepth = a.HalfHeight + b.HalfWidth * absRBX.Y + b.HalfHeight * absRBY.Y + b.HalfLength * absRBZ.Y - Vector.Abs(localOffsetB.Y);
Select(ref depth, ref localNormal, ref faceAYDepth, ref zero, ref one, ref zero);
var faceAZDepth = a.HalfLength + b.HalfWidth * absRBX.Z + b.HalfHeight * absRBY.Z + b.HalfLength * absRBZ.Z - Vector.Abs(localOffsetB.Z);
Select(ref depth, ref localNormal, ref faceAZDepth, ref zero, ref zero, ref one);
//Test face normals of B. Rows of A->B rotation.
Matrix3x3Wide.TransformByTransposedWithoutOverlap(localOffsetB, rB, out var bLocalOffsetB);
var faceBXDepth = b.HalfWidth + a.HalfWidth * absRBX.X + a.HalfHeight * absRBX.Y + a.HalfLength * absRBX.Z - Vector.Abs(bLocalOffsetB.X);
Select(ref depth, ref localNormal, ref faceBXDepth, ref rB.X.X, ref rB.X.Y, ref rB.X.Z);
var faceBYDepth = b.HalfHeight + a.HalfWidth * absRBY.X + a.HalfHeight * absRBY.Y + a.HalfLength * absRBY.Z - Vector.Abs(bLocalOffsetB.Y);
Select(ref depth, ref localNormal, ref faceBYDepth, ref rB.Y.X, ref rB.Y.Y, ref rB.Y.Z);
var faceBZDepth = b.HalfLength + a.HalfWidth * absRBZ.X + a.HalfHeight * absRBZ.Y + a.HalfLength * absRBZ.Z - Vector.Abs(bLocalOffsetB.Z);
Select(ref depth, ref localNormal, ref faceBZDepth, ref rB.Z.X, ref rB.Z.Y, ref rB.Z.Z);
//Calibrate the normal to point from B to A, matching convention.
Vector3Wide.Dot(localNormal, localOffsetB, out var normalDotOffsetB);
var shouldNegateNormal = Vector.GreaterThan(normalDotOffsetB, Vector <float> .Zero);
localNormal.X = Vector.ConditionalSelect(shouldNegateNormal, -localNormal.X, localNormal.X);
localNormal.Y = Vector.ConditionalSelect(shouldNegateNormal, -localNormal.Y, localNormal.Y);
localNormal.Z = Vector.ConditionalSelect(shouldNegateNormal, -localNormal.Z, localNormal.Z);
Matrix3x3Wide.TransformWithoutOverlap(localNormal, worldRA, out manifold.Normal);
//Contact generation always assumes face-face clipping. Other forms of contact generation are just special cases of face-face, and since we pay
//for all code paths, there's no point in handling them separately.
//We just have to guarantee that the face chosen on each box is guaranteed to include the deepest feature along the contact normal.
//To do this, choose the face on each box associated with the maximum axis dot with the collision normal.
//We represent each face as a center position, its two tangent axes, and the length along those axes.
//Technically, we could leave A's tangents implicit by swizzling components, but that complicates things a little bit for not much gain.
//Since we're not taking advantage of the dimension reduction of working in A's local space from here on out, just use the world axes to avoid a final retransform.
Vector3Wide.Dot(manifold.Normal, worldRA.X, out var axDot);
Vector3Wide.Dot(manifold.Normal, worldRA.Y, out var ayDot);
Vector3Wide.Dot(manifold.Normal, worldRA.Z, out var azDot);
var absAXDot = Vector.Abs(axDot);
var absAYDot = Vector.Abs(ayDot);
var absAZDot = Vector.Abs(azDot);
var maxADot = Vector.Max(absAXDot, Vector.Max(absAYDot, absAZDot));
var useAX = Vector.Equals(maxADot, absAXDot);
var useAY = Vector.AndNot(Vector.Equals(maxADot, absAYDot), useAX);
Vector3Wide.ConditionalSelect(useAX, worldRA.X, worldRA.Z, out var normalA);
Vector3Wide.ConditionalSelect(useAY, worldRA.Y, normalA, out normalA);
Vector3Wide.ConditionalSelect(useAX, worldRA.Z, worldRA.Y, out var tangentAX);
Vector3Wide.ConditionalSelect(useAY, worldRA.X, tangentAX, out tangentAX);
Vector3Wide.ConditionalSelect(useAX, worldRA.Y, worldRA.X, out var tangentAY);
Vector3Wide.ConditionalSelect(useAY, worldRA.Z, tangentAY, out tangentAY);
var halfSpanAX = Vector.ConditionalSelect(useAX, a.HalfLength, Vector.ConditionalSelect(useAY, a.HalfWidth, a.HalfHeight));
var halfSpanAY = Vector.ConditionalSelect(useAX, a.HalfHeight, Vector.ConditionalSelect(useAY, a.HalfLength, a.HalfWidth));
var halfSpanAZ = Vector.ConditionalSelect(useAX, a.HalfWidth, Vector.ConditionalSelect(useAY, a.HalfHeight, a.HalfLength));
//We'll construct vertex feature ids from axis ids.
//Vertex ids will be constructed by setting or not setting the relevant bit for each axis.
var localXId = new Vector <int>(1);
var localYId = new Vector <int>(4);
var localZId = new Vector <int>(16);
var axisIdAX = Vector.ConditionalSelect(useAX, localZId, Vector.ConditionalSelect(useAY, localXId, localYId));
var axisIdAY = Vector.ConditionalSelect(useAX, localYId, Vector.ConditionalSelect(useAY, localZId, localXId));
var axisIdAZ = Vector.ConditionalSelect(useAX, localXId, Vector.ConditionalSelect(useAY, localYId, localZId));
Vector3Wide.Dot(manifold.Normal, worldRB.X, out var bxDot);
Vector3Wide.Dot(manifold.Normal, worldRB.Y, out var byDot);
Vector3Wide.Dot(manifold.Normal, worldRB.Z, out var bzDot);
var absBXDot = Vector.Abs(bxDot);
var absBYDot = Vector.Abs(byDot);
var absBZDot = Vector.Abs(bzDot);
var maxBDot = Vector.Max(absBXDot, Vector.Max(absBYDot, absBZDot));
var useBX = Vector.Equals(maxBDot, absBXDot);
var useBY = Vector.AndNot(Vector.Equals(maxBDot, absBYDot), useBX);
Vector3Wide.ConditionalSelect(useBX, worldRB.X, worldRB.Z, out var normalB);
Vector3Wide.ConditionalSelect(useBY, worldRB.Y, normalB, out normalB);
Vector3Wide.ConditionalSelect(useBX, worldRB.Z, worldRB.Y, out var tangentBX);
Vector3Wide.ConditionalSelect(useBY, worldRB.X, tangentBX, out tangentBX);
Vector3Wide.ConditionalSelect(useBX, worldRB.Y, worldRB.X, out var tangentBY);
Vector3Wide.ConditionalSelect(useBY, worldRB.Z, tangentBY, out tangentBY);
var halfSpanBX = Vector.ConditionalSelect(useBX, b.HalfLength, Vector.ConditionalSelect(useBY, b.HalfWidth, b.HalfHeight));
var halfSpanBY = Vector.ConditionalSelect(useBX, b.HalfHeight, Vector.ConditionalSelect(useBY, b.HalfLength, b.HalfWidth));
var halfSpanBZ = Vector.ConditionalSelect(useBX, b.HalfWidth, Vector.ConditionalSelect(useBY, b.HalfHeight, b.HalfLength));
//We'll construct edge feature ids from axis ids.
//Edge ids will be 6 bits total, representing 3 possible states (-1, 0, 1) for each of the 3 axes. Multiply the axis id by 1, 2, or 3 to get the edge id contribution for the axis.
var axisIdBX = Vector.ConditionalSelect(useBX, localZId, Vector.ConditionalSelect(useBY, localXId, localYId));
var axisIdBY = Vector.ConditionalSelect(useBX, localYId, Vector.ConditionalSelect(useBY, localZId, localXId));
var axisIdBZ = Vector.ConditionalSelect(useBX, localXId, Vector.ConditionalSelect(useBY, localYId, localZId));
//Calibrate normalB to face toward A, and normalA to face toward B.
Vector3Wide.Dot(normalA, manifold.Normal, out var calibrationDotA);
var shouldNegateNormalA = Vector.GreaterThan(calibrationDotA, Vector <float> .Zero);
normalA.X = Vector.ConditionalSelect(shouldNegateNormalA, -normalA.X, normalA.X);
normalA.Y = Vector.ConditionalSelect(shouldNegateNormalA, -normalA.Y, normalA.Y);
normalA.Z = Vector.ConditionalSelect(shouldNegateNormalA, -normalA.Z, normalA.Z);
Vector3Wide.Dot(normalB, manifold.Normal, out var calibrationDotB);
var shouldNegateNormalB = Vector.LessThan(calibrationDotB, Vector <float> .Zero);
normalB.X = Vector.ConditionalSelect(shouldNegateNormalB, -normalB.X, normalB.X);
normalB.Y = Vector.ConditionalSelect(shouldNegateNormalB, -normalB.Y, normalB.Y);
normalB.Z = Vector.ConditionalSelect(shouldNegateNormalB, -normalB.Z, normalB.Z);
//Clip edges of B against the face bounds of A to collect all edge-bound contacts.
Vector3Wide.Scale(normalB, halfSpanBZ, out var faceCenterB);
Vector3Wide.Add(faceCenterB, offsetB, out faceCenterB);
Vector3Wide.Scale(tangentBY, halfSpanBY, out var edgeOffsetBX);
Vector3Wide.Scale(tangentBX, halfSpanBX, out var edgeOffsetBY);
Vector3Wide.Dot(tangentAX, tangentBX, out var axbx);
Vector3Wide.Dot(tangentAY, tangentBX, out var aybx);
Vector3Wide.Dot(tangentAX, tangentBY, out var axby);
Vector3Wide.Dot(tangentAY, tangentBY, out var ayby);
GetEdgeVersusFaceBoundsIntervals(ref tangentAX, ref tangentAY, ref halfSpanAX, ref halfSpanAY, ref axbx, ref aybx, ref faceCenterB, ref halfSpanBX, ref edgeOffsetBX,
out var bX0Min, out var bX0Max, out var bX1Min, out var bX1Max);
GetEdgeVersusFaceBoundsIntervals(ref tangentAX, ref tangentAY, ref halfSpanAX, ref halfSpanAY, ref axby, ref ayby, ref faceCenterB, ref halfSpanBY, ref edgeOffsetBY,
out var bY0Min, out var bY0Max, out var bY1Min, out var bY1Max);
//Note that we only allocate up to 8 candidates. It is not possible for this process to generate more than 8 (unless there are numerical problems, which we guard against).
int byteCount = Unsafe.SizeOf <ManifoldCandidate>() * 8;
var buffer = stackalloc byte[byteCount];
ref var candidates = ref Unsafe.As <byte, ManifoldCandidate>(ref *buffer);
