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, Box, 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.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 void Test(ref SphereWide a, ref TriangleWide b, ref Vector <float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount,
out Convex1ContactManifoldWide manifold)
{
//Work in the local space of the triangle, since it's quicker to transform the sphere position than the vertices of the triangle.
Matrix3x3Wide.CreateFromQuaternion(orientationB, out var rB);
Matrix3x3Wide.TransformByTransposedWithoutOverlap(offsetB, rB, out var localOffsetB);
Vector3Wide.Subtract(b.B, b.A, out var ab);
Vector3Wide.Subtract(b.C, b.A, out var ac);
//localOffsetA = -localOffsetB, so pa = triangle.A + localOffsetB.
Vector3Wide.Add(b.A, localOffsetB, out var pa);
Vector3Wide.CrossWithoutOverlap(ab, ac, out var localTriangleNormal);
Vector3Wide.Dot(localTriangleNormal, pa, out var paN);
var collidingWithSolidSide = Vector.GreaterThan(paN, Vector <float> .Zero);
if (Vector.EqualsAll(collidingWithSolidSide, Vector <int> .Zero))
{
//No lanes can generate contacts due to the triangle's one sidedness.
manifold.ContactExists = Vector <int> .Zero;
return;
}
//EdgeAB plane test: (pa x ab) * (ab x ac) >= 0
//EdgeAC plane test: (ac x pa) * (ab x ac) >= 0
//Note that these are scaled versions of the barycentric coordinates.
//To normalize them such that the weights of a point within the triangle equal 1, we just need to divide by dot(ab x ac, ab x ac).
//In other words, to test the third edge plane, we can ensure that the unnormalized weights are both positive and sum to a value less than dot(ab x ac, ab x ac).
//If a point is outside of an edge plane, we know that it's not in the face region or any other edge region. It could, however, be in an adjacent vertex region.
//Vertex cases can be handled by clamping an edge case.
//Further, note that for any query location, it is sufficient to only test one edge even if the point is outside two edge planes. If it's outside two edge planes,
//that just means it's going to be on the shared vertex, so a clamped edge test captures the correct closest point.
//So, at each edge, if the point is outside the plane, cache the edge. The last edge registering an outside result will be tested.
Vector3Wide.CrossWithoutOverlap(pa, ab, out var paxab);
Vector3Wide.CrossWithoutOverlap(ac, pa, out var acxpa);
Vector3Wide.Dot(paxab, localTriangleNormal, out var edgePlaneTestAB);
Vector3Wide.Dot(acxpa, localTriangleNormal, out var edgePlaneTestAC);
Vector3Wide.Dot(localTriangleNormal, localTriangleNormal, out var triangleNormalLengthSquared);
var edgePlaneTestBC = triangleNormalLengthSquared - edgePlaneTestAB - edgePlaneTestAC;
var outsideAB = Vector.LessThan(edgePlaneTestAB, Vector <float> .Zero);
var outsideAC = Vector.LessThan(edgePlaneTestAC, Vector <float> .Zero);
var outsideBC = Vector.LessThan(edgePlaneTestBC, Vector <float> .Zero);
var outsideAnyEdge = Vector.BitwiseOr(outsideAB, Vector.BitwiseOr(outsideAC, outsideBC));
Vector3Wide localClosestOnTriangle;
var negativeOne = new Vector <int>(-1);
if (Vector.EqualsAny(Vector.BitwiseAnd(collidingWithSolidSide, outsideAnyEdge), negativeOne))
{
//At least one lane detected a point outside of the triangle. Choose one edge which is outside as the representative.
Vector3Wide.ConditionalSelect(outsideAC, ac, ab, out var edgeDirection);
Vector3Wide.Subtract(b.C, b.B, out var bc);
Vector3Wide.ConditionalSelect(outsideBC, bc, edgeDirection, out edgeDirection);
Vector3Wide.ConditionalSelect(outsideBC, b.B, b.A, out var edgeStart);
Vector3Wide.Add(localOffsetB, edgeStart, out var negativeEdgeStartToP);
//This does some partially redundant work if the edge is AB or AC, but given that we didn't have bcbc or bcpb, it's fine.
Vector3Wide.Dot(negativeEdgeStartToP, edgeDirection, out var negativeOffsetDotEdge);
Vector3Wide.Dot(edgeDirection, edgeDirection, out var edgeDotEdge);
var edgeScale = Vector.Max(Vector <float> .Zero, Vector.Min(Vector <float> .One, -negativeOffsetDotEdge / edgeDotEdge));
Vector3Wide.Scale(edgeDirection, edgeScale, out var pointOnEdge);
Vector3Wide.Add(edgeStart, pointOnEdge, out pointOnEdge);
Vector3Wide.ConditionalSelect(outsideAnyEdge, pointOnEdge, localClosestOnTriangle, out localClosestOnTriangle);
}
if (Vector.EqualsAny(Vector.AndNot(collidingWithSolidSide, outsideAnyEdge), negativeOne))
{
//p + N * (pa * N) / ||N||^2 = N * (pa * N) / ||N||^2 - (-p)
var nScale = paN / triangleNormalLengthSquared;
Vector3Wide.Scale(localTriangleNormal, nScale, out var offsetToPlane);
Vector3Wide.Subtract(offsetToPlane, localOffsetB, out var pointOnFace);
Vector3Wide.ConditionalSelect(outsideAnyEdge, localClosestOnTriangle, pointOnFace, out localClosestOnTriangle);
}
manifold.FeatureId = Vector.ConditionalSelect(outsideAnyEdge, Vector <int> .Zero, new Vector <int>(MeshReduction.FaceCollisionFlag));
//We'll be using the contact position to perform boundary smoothing; in order to find other triangles, the contact position has to be on the mesh surface.
Matrix3x3Wide.TransformWithoutOverlap(localClosestOnTriangle, rB, out manifold.OffsetA);
Vector3Wide.Add(manifold.OffsetA, offsetB, out manifold.OffsetA);
Vector3Wide.Length(manifold.OffsetA, out var distance);
//Note the normal is calibrated to point from B to A.
var normalScale = new Vector <float>(-1) / distance;
Vector3Wide.Scale(manifold.OffsetA, normalScale, out manifold.Normal);
manifold.Depth = a.Radius - distance;
//In the event that the sphere's center point is touching the triangle, the normal is undefined. In that case, the 'correct' normal would be the triangle's normal.
//However, given that this is a pretty rare degenerate case and that we already treat triangle backfaces as noncolliding, we'll treat zero distance as a backface non-collision.
manifold.ContactExists = Vector.BitwiseAnd(
Vector.GreaterThan(distance, Vector <float> .Zero),
Vector.BitwiseAnd(
Vector.GreaterThanOrEqual(paN, Vector <float> .Zero),
Vector.GreaterThanOrEqual(manifold.Depth, -speculativeMargin)));
}
public unsafe void Test(
ref BoxWide a, ref BoxWide b, ref Vector <float> speculativeMargin,
ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB,
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);
public void Test(ref CapsuleWide a, ref ConvexHullWide b, ref Vector <float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, out Convex2ContactManifoldWide manifold)
{
Matrix3x3Wide.CreateFromQuaternion(orientationA, out var capsuleOrientation);
Matrix3x3Wide.CreateFromQuaternion(orientationB, out var hullOrientation);
Matrix3x3Wide.MultiplyByTransposeWithoutOverlap(capsuleOrientation, hullOrientation, out var hullLocalCapsuleOrientation);
ref var localCapsuleAxis = ref hullLocalCapsuleOrientation.Y;
public void Do()
{
Matrix3x3Wide.MultiplyTransposedWithoutOverlap(ref rotation, ref symmetric, out var intermediateWide);
Matrix3x3Wide.MultiplyWithoutOverlap(ref intermediateWide, ref rotation, out result);
}
public void Do()
{
Matrix3x3Wide.Invert(ref symmetric, out result);
}
public void Do()
{
Matrix3x3Wide.CreateCrossProduct(ref v, out var skew);
Matrix3x3Wide.MultiplyTransposedWithoutOverlap(ref skew, ref symmetric, out var intermediate);
Matrix3x3Wide.MultiplyWithoutOverlap(ref intermediate, ref skew, out result);
}
public void Test(
ref CapsuleWide a, ref BoxWide b, ref Vector <float> speculativeMargin,
ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB,
out Convex2ContactManifoldWide manifold)
{
Prepare(ref a, ref b, ref offsetB, ref orientationA, ref orientationB, out var localOffsetA, out var capsuleAxis, out var edgeCenters);
//Swizzle XYZ -> YZX
Vector3Wide localNormal;
TestAndRefineBoxEdge(ref localOffsetA.Y, ref localOffsetA.Z, ref localOffsetA.X,
ref capsuleAxis.Y, ref capsuleAxis.Z, ref capsuleAxis.X,
ref a.HalfLength,
ref edgeCenters.Y, ref edgeCenters.Z,
ref b.HalfHeight, ref b.HalfLength, ref b.HalfWidth,
out var ta, out var depth, out localNormal.Y, out localNormal.Z, out localNormal.X);
//Swizzle XYZ -> ZXY
TestAndRefineBoxEdge(ref localOffsetA.Z, ref localOffsetA.X, ref localOffsetA.Y,
ref capsuleAxis.Z, ref capsuleAxis.X, ref capsuleAxis.Y,
ref a.HalfLength,
ref edgeCenters.Z, ref edgeCenters.X,
ref b.HalfLength, ref b.HalfWidth, ref b.HalfHeight,
out var eyta, out var eyDepth, out var eynZ, out var eynX, out var eynY);
Select(ref depth, ref ta, ref localNormal.X, ref localNormal.Y, ref localNormal.Z,
ref eyDepth, ref eyta, ref eynX, ref eynY, ref eynZ);
//Swizzle XYZ -> XYZ
TestAndRefineBoxEdge(ref localOffsetA.X, ref localOffsetA.Y, ref localOffsetA.Z,
ref capsuleAxis.X, ref capsuleAxis.Y, ref capsuleAxis.Z,
ref a.HalfLength,
ref edgeCenters.X, ref edgeCenters.Y,
ref b.HalfWidth, ref b.HalfHeight, ref b.HalfLength,
out var ezta, out var ezDepth, out var eznX, out var eznY, out var eznZ);
Select(ref depth, ref ta, ref localNormal.X, ref localNormal.Y, ref localNormal.Z,
ref ezDepth, ref ezta, ref eznX, ref eznY, ref eznZ);
var zero = Vector <float> .Zero;
//Face X
TestBoxFace(ref localOffsetA.X,
ref capsuleAxis.X, ref a.HalfLength,
ref b.HalfWidth,
out var fxDepth, out var fxn);
Select(ref depth, ref localNormal.X, ref localNormal.Y, ref localNormal.Z,
ref fxDepth, ref fxn, ref zero, ref zero);
//Face Y
TestBoxFace(ref localOffsetA.Y,
ref capsuleAxis.Y, ref a.HalfLength,
ref b.HalfHeight,
out var fyDepth, out var fyn);
Select(ref depth, ref localNormal.X, ref localNormal.Y, ref localNormal.Z,
ref fyDepth, ref zero, ref fyn, ref zero);
//Face Z
TestBoxFace(ref localOffsetA.Z,
ref capsuleAxis.Z, ref a.HalfLength,
ref b.HalfLength,
out var fzDepth, out var fzn);
Select(ref depth, ref localNormal.X, ref localNormal.Y, ref localNormal.Z,
ref fzDepth, ref zero, ref zero, ref fzn);
//While the above chooses a minimal depth, edge-edge contact will frequently produce interval lengths of 0 and end up overwriting near-equivalent face intervals.
//One option would be to bias the comparison to accept face contacts more readily, but we can do something a little less fragile, and which also gives us a
//way to safely compute depth on a per contact basis:
//choose a representative box face based on the collision normal detected above, and compute the interval of intersection along the capsule axis of the box face projected onto the capsule axis.
//We'll compute this by unprojecting the capsule axis onto the box face plane along the local normal.
//Note that this interval always includes the closest point.
var xDot = localNormal.X * fxn;
var yDot = localNormal.Y * fyn;
var zDot = localNormal.Z * fzn;
var useX = Vector.GreaterThan(xDot, Vector.Max(yDot, zDot));
var useY = Vector.AndNot(Vector.GreaterThan(yDot, zDot), useX);
var useZ = Vector.AndNot(Vector.OnesComplement(useX), useY);
//Unproject the capsule center and capsule axis onto the representative face plane.
//unprojectedAxis = capsuleAxis - localNormal * dot(capsuleAxis, faceNormal) / dot(localNormal, faceNormal)
//unprojectedCenter = capsuleCenter - localNormal * dot(capsuleCenter - pointOnFace, faceNormal) / dot(localNormal, faceNormal)
var faceNormalDotLocalNormal = Vector.ConditionalSelect(useX, xDot, Vector.ConditionalSelect(useY, yDot, zDot));
var inverseFaceNormalDotLocalNormal = Vector <float> .One / Vector.Max(new Vector <float>(1e-15f), faceNormalDotLocalNormal);
var capsuleAxisDotFaceNormal = Vector.ConditionalSelect(useX, capsuleAxis.X * fxn, Vector.ConditionalSelect(useY, capsuleAxis.Y * fyn, capsuleAxis.Z * fzn));
var capsuleCenterDotFaceNormal = Vector.ConditionalSelect(useX, localOffsetA.X * fxn, Vector.ConditionalSelect(useY, localOffsetA.Y * fyn, localOffsetA.Z * fzn));
var facePlaneOffset = Vector.ConditionalSelect(useX, b.HalfWidth, Vector.ConditionalSelect(useY, b.HalfHeight, b.HalfLength));
var tAxis = capsuleAxisDotFaceNormal * inverseFaceNormalDotLocalNormal;
var tCenter = (capsuleCenterDotFaceNormal - facePlaneOffset) * inverseFaceNormalDotLocalNormal;
//Work in tangent space.
//Face X uses tangents Y and Z.
//Face Y uses tangents X and Z.
//Face Z uses tangents X and Y.
Vector3Wide.Scale(localNormal, tAxis, out var axisOffset);
Vector3Wide.Scale(localNormal, tCenter, out var centerOffset);
Vector3Wide.Subtract(capsuleAxis, axisOffset, out var unprojectedAxis);
Vector3Wide.Subtract(localOffsetA, centerOffset, out var unprojectedCenter);
Vector2Wide tangentSpaceCenter, tangentSpaceAxis;
tangentSpaceAxis.X = Vector.ConditionalSelect(useX, unprojectedAxis.Y, unprojectedAxis.X);
tangentSpaceAxis.Y = Vector.ConditionalSelect(useZ, unprojectedAxis.Y, unprojectedAxis.Z);
tangentSpaceCenter.X = Vector.ConditionalSelect(useX, unprojectedCenter.Y, unprojectedCenter.X);
tangentSpaceCenter.Y = Vector.ConditionalSelect(useZ, unprojectedCenter.Y, unprojectedCenter.Z);
//Slightly boost the size of the face to avoid minor numerical issues that could block coplanar contacts.
var epsilonScale = Vector.Min(Vector.Max(b.HalfWidth, Vector.Max(b.HalfHeight, b.HalfLength)), Vector.Max(a.HalfLength, a.Radius));
var epsilon = epsilonScale * 1e-3f;
var halfExtentX = epsilon + Vector.ConditionalSelect(useX, b.HalfHeight, b.HalfWidth);
var halfExtentY = epsilon + Vector.ConditionalSelect(useZ, b.HalfHeight, b.HalfLength);
//Compute interval bounded by edge normals pointing along tangentX.
//tX = -dot(tangentSpaceCenter +- halfExtentX, edgeNormal) / dot(unprojectedCapsuleAxis, edgeNormal)
var negativeOne = new Vector <float>(-1);
var inverseAxisX = negativeOne / tangentSpaceAxis.X;
var inverseAxisY = negativeOne / tangentSpaceAxis.Y;
var tX0 = (tangentSpaceCenter.X - halfExtentX) * inverseAxisX;
var tX1 = (tangentSpaceCenter.X + halfExtentX) * inverseAxisX;
var tY0 = (tangentSpaceCenter.Y - halfExtentY) * inverseAxisY;
var tY1 = (tangentSpaceCenter.Y + halfExtentY) * inverseAxisY;
var minX = Vector.Min(tX0, tX1);
var maxX = Vector.Max(tX0, tX1);
var minY = Vector.Min(tY0, tY1);
var maxY = Vector.Max(tY0, tY1);
//Protect against division by zero. If the unprojected capsule is within the slab, use an infinite interval. If it's outside and parallel, use an invalid interval.
var useFallbackX = Vector.LessThan(Vector.Abs(tangentSpaceAxis.X), new Vector <float>(1e-15f));
var useFallbackY = Vector.LessThan(Vector.Abs(tangentSpaceAxis.Y), new Vector <float>(1e-15f));
var centerContainedX = Vector.LessThanOrEqual(Vector.Abs(tangentSpaceCenter.X), halfExtentX);
var centerContainedY = Vector.LessThanOrEqual(Vector.Abs(tangentSpaceCenter.Y), halfExtentY);
var largeNegative = new Vector <float>(-float.MaxValue);
var largePositive = new Vector <float>(float.MaxValue);
minX = Vector.ConditionalSelect(useFallbackX, Vector.ConditionalSelect(centerContainedX, largeNegative, largePositive), minX);
maxX = Vector.ConditionalSelect(useFallbackX, Vector.ConditionalSelect(centerContainedX, largePositive, largeNegative), maxX);
minY = Vector.ConditionalSelect(useFallbackY, Vector.ConditionalSelect(centerContainedY, largeNegative, largePositive), minY);
maxY = Vector.ConditionalSelect(useFallbackY, Vector.ConditionalSelect(centerContainedY, largePositive, largeNegative), maxY);
var faceMin = Vector.Max(minX, minY);
var faceMax = Vector.Min(maxX, maxY);
//Clamp the resulting interval to the capsule axis.
var tMin = Vector.Max(Vector.Min(faceMin, a.HalfLength), -a.HalfLength);
var tMax = Vector.Max(Vector.Min(faceMax, a.HalfLength), -a.HalfLength);
var faceIntervalExists = Vector.GreaterThanOrEqual(faceMax, faceMin);
tMin = Vector.ConditionalSelect(faceIntervalExists, Vector.Min(tMin, ta), ta);
tMax = Vector.ConditionalSelect(faceIntervalExists, Vector.Max(tMax, ta), ta);
//Each contact may have its own depth.
//Imagine a face collision- if the capsule axis isn't fully parallel with the plane's surface, it would be strange to use the same depth for both contacts.
//We have two points on the capsule and box. We can reuse the unprojeection from earlier to compute the offset between them.
var separationMin = tCenter + tAxis * tMin;
var separationMax = tCenter + tAxis * tMax;
manifold.Depth0 = a.Radius - separationMin;
manifold.Depth1 = a.Radius - separationMax;
Vector3Wide.Scale(capsuleAxis, tMin, out var localA0);
Vector3Wide.Scale(capsuleAxis, tMax, out var localA1);
Vector3Wide.Add(localOffsetA, localA0, out var bToA0);
Vector3Wide.Add(localOffsetA, localA1, out var bToA1);
manifold.FeatureId0 = Vector <int> .Zero;
manifold.FeatureId1 = Vector <int> .One;
//Transform A0, A1, and the normal into world space.
Matrix3x3Wide.CreateFromQuaternion(orientationB, out var orientationMatrixB);
Matrix3x3Wide.TransformWithoutOverlap(localNormal, orientationMatrixB, out manifold.Normal);
Matrix3x3Wide.TransformWithoutOverlap(localA0, orientationMatrixB, out manifold.OffsetA0);
Matrix3x3Wide.TransformWithoutOverlap(localA1, orientationMatrixB, out manifold.OffsetA1);
//Apply the normal offset to the contact positions.
var negativeOffsetFromA0 = manifold.Depth0 * 0.5f - a.Radius;
var negativeOffsetFromA1 = manifold.Depth1 * 0.5f - a.Radius;
Vector3Wide.Scale(manifold.Normal, negativeOffsetFromA0, out var normalPush0);
Vector3Wide.Scale(manifold.Normal, negativeOffsetFromA1, out var normalPush1);
Vector3Wide.Add(manifold.OffsetA0, normalPush0, out manifold.OffsetA0);
Vector3Wide.Add(manifold.OffsetA1, normalPush1, out manifold.OffsetA1);
var minimumAcceptedDepth = -speculativeMargin;
manifold.Contact0Exists = Vector.GreaterThanOrEqual(manifold.Depth0, minimumAcceptedDepth);
manifold.Contact1Exists = Vector.BitwiseAnd(
Vector.GreaterThanOrEqual(manifold.Depth1, minimumAcceptedDepth),
Vector.GreaterThan(tMax - tMin, new Vector <float>(1e-7f) * a.HalfLength));
}
public unsafe void Test(ref TriangleWide 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 triangleOrientation);
Matrix3x3Wide.CreateFromQuaternion(orientationB, out var hullOrientation);
Matrix3x3Wide.MultiplyByTransposeWithoutOverlap(triangleOrientation, hullOrientation, out var hullLocalTriangleOrientation);
Matrix3x3Wide.TransformByTransposedWithoutOverlap(offsetB, hullOrientation, out var localOffsetB);
Vector3Wide.Negate(localOffsetB, out var localOffsetA);
TriangleWide triangle;
Matrix3x3Wide.TransformWithoutOverlap(a.A, hullLocalTriangleOrientation, out triangle.A);
Matrix3x3Wide.TransformWithoutOverlap(a.B, hullLocalTriangleOrientation, out triangle.B);
Matrix3x3Wide.TransformWithoutOverlap(a.C, hullLocalTriangleOrientation, out triangle.C);
Vector3Wide.Add(triangle.A, triangle.B, out var centroid);
Vector3Wide.Add(triangle.C, centroid, out centroid);
Vector3Wide.Scale(centroid, new Vector<float>(1f / 3f), out centroid);
Vector3Wide.Subtract(triangle.A, centroid, out triangle.A);
Vector3Wide.Subtract(triangle.B, centroid, out triangle.B);
Vector3Wide.Subtract(triangle.C, centroid, out triangle.C);
Vector3Wide.Subtract(centroid, localOffsetB, out var localTriangleCenter);
Vector3Wide.Subtract(triangle.B, triangle.A, out var triangleAB);
Vector3Wide.Subtract(triangle.C, triangle.B, out var triangleBC);
Vector3Wide.Subtract(triangle.A, triangle.C, out var triangleCA);
//We'll be using B-local triangle vertices quite a bit, so cache them.
Vector3Wide.Add(triangle.A, localTriangleCenter, out var triangleA);
Vector3Wide.Add(triangle.B, localTriangleCenter, out var triangleB);
Vector3Wide.Add(triangle.C, localTriangleCenter, out var triangleC);
Vector3Wide.CrossWithoutOverlap(triangleAB, triangleCA, out var triangleNormal);
Vector3Wide.Length(triangleNormal, out var triangleNormalLength);
Vector3Wide.Scale(triangleNormal, Vector<float>.One / triangleNormalLength, out triangleNormal);
//Check if the hull's position is within the triangle and below the triangle plane. If so, we can ignore it.
Vector3Wide.Dot(triangleNormal, localTriangleCenter, out var hullToTriangleCenterDot);
var hullBelowPlane = Vector.GreaterThanOrEqual(hullToTriangleCenterDot, Vector<float>.Zero);
Vector3Wide.CrossWithoutOverlap(triangleAB, triangleNormal, out var edgePlaneAB);
Vector3Wide.CrossWithoutOverlap(triangleBC, triangleNormal, out var edgePlaneBC);
Vector3Wide.CrossWithoutOverlap(triangleCA, triangleNormal, out var edgePlaneCA);
Vector3Wide.Dot(edgePlaneAB, triangleA, out var abPlaneTest);
Vector3Wide.Dot(edgePlaneBC, triangleB, out var bcPlaneTest);
Vector3Wide.Dot(edgePlaneCA, triangleC, out var caPlaneTest);
var hullInsideTriangleEdgePlanes = Vector.BitwiseAnd(Vector.LessThanOrEqual(abPlaneTest, Vector<float>.Zero),
Vector.BitwiseAnd(Vector.LessThanOrEqual(bcPlaneTest, Vector<float>.Zero), Vector.LessThanOrEqual(caPlaneTest, Vector<float>.Zero)));
var hullInsideAndBelowTriangle = Vector.BitwiseAnd(hullBelowPlane, hullInsideTriangleEdgePlanes);
ManifoldCandidateHelper.CreateInactiveMask(pairCount, out var inactiveLanes);
a.EstimateEpsilonScale(out var triangleEpsilonScale);
b.EstimateEpsilonScale(inactiveLanes, out var hullEpsilonScale);
var epsilonScale = Vector.Min(triangleEpsilonScale, hullEpsilonScale);
inactiveLanes = Vector.BitwiseOr(inactiveLanes, Vector.LessThan(triangleNormalLength, epsilonScale * 1e-6f));
inactiveLanes = Vector.BitwiseOr(inactiveLanes, hullInsideAndBelowTriangle);
//Not every lane will generate contacts. Rather than requiring every lane to carefully clear all contactExists states, just clear them up front.
manifold.Contact0Exists = default;
manifold.Contact1Exists = default;
manifold.Contact2Exists = default;
manifold.Contact3Exists = default;
if (Vector.LessThanAll(inactiveLanes, Vector<int>.Zero))
{
//No contacts generated.
return;
}
//Note the use of the triangle center as the initial normal rather than the localOffsetA.
//Triangles are not guaranteed to be centered on their center of mass, and the DepthRefiner
//will converge to a depth which does not oppose the so-far best normal- which, on the early iterations,
//could be the initial normal.
Vector3Wide.Length(localTriangleCenter, out var centerDistance);
Vector3Wide.Scale(localTriangleCenter, Vector<float>.One / centerDistance, out var initialNormal);
var useInitialFallback = Vector.LessThan(centerDistance, new Vector<float>(1e-10f));
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);
//Check if the extreme point of the hull toward the triangle along its face normal lies inside the triangle.
//If it is, then there's no need for depth refinement.
var hullSupportFinder = default(ConvexHullSupportFinder);
var triangleSupportFinder = default(PretransformedTriangleSupportFinder);
//Sample the hull's extreme point along the triangle face normal- if it's contained within the triangle edge planes, we can avoid doing more expensive refinement.
Vector3Wide.Negate(triangleNormal, out var negatedTriangleNormal);
hullSupportFinder.ComputeLocalSupport(b, negatedTriangleNormal, inactiveLanes, out var hullSupportAlongNegatedTriangleNormal);
Vector3Wide.Subtract(hullSupportAlongNegatedTriangleNormal, localTriangleCenter, out var supportAlongNegatedTriangleNormal);
Vector3Wide.Dot(supportAlongNegatedTriangleNormal, negatedTriangleNormal, out var triangleFaceDepth);
Vector3Wide.Subtract(triangleA, hullSupportAlongNegatedTriangleNormal, out var closestToA);
Vector3Wide.Subtract(triangleB, hullSupportAlongNegatedTriangleNormal, out var closestToB);
Vector3Wide.Subtract(triangleC, hullSupportAlongNegatedTriangleNormal, out var closestToC);
Vector3Wide.Dot(edgePlaneAB, closestToA, out var extremeABPlaneTest);
Vector3Wide.Dot(edgePlaneBC, closestToB, out var extremeBCPlaneTest);
Vector3Wide.Dot(edgePlaneCA, closestToC, out var extremeCAPlaneTest);
//Note that the triangle face extreme point can only be trusted if the hull's center is above the triangle's surface *AND* contained within the edge normals.
//Merely being above the surface is insufficient- imagine a hull off to the side of the triangle, wedged beneath it.
var triangleNormalIsMinimal = Vector.BitwiseAnd(
Vector.BitwiseAnd(
Vector.AndNot(hullInsideTriangleEdgePlanes, hullBelowPlane),
Vector.LessThanOrEqual(extremeABPlaneTest, Vector<float>.Zero)),
Vector.BitwiseAnd(
Vector.LessThanOrEqual(extremeBCPlaneTest, Vector<float>.Zero),
Vector.LessThanOrEqual(extremeCAPlaneTest, Vector<float>.Zero)));
var depthThreshold = -speculativeMargin;
var skipDepthRefine = Vector.BitwiseOr(triangleNormalIsMinimal, inactiveLanes);
Vector3Wide localNormal, closestOnHull;
Vector<float> depth;
if (Vector.EqualsAny(skipDepthRefine, Vector<int>.Zero))
{
DepthRefiner.FindMinimumDepth(
b, triangle, localTriangleCenter, hullLocalTriangleOrientation, ref hullSupportFinder, ref triangleSupportFinder, initialNormal, skipDepthRefine, 1e-4f * epsilonScale, depthThreshold,
out var refinedDepth, out var refinedNormal, out var refinedClosestOnHull);
Vector3Wide.ConditionalSelect(skipDepthRefine, hullSupportAlongNegatedTriangleNormal, refinedClosestOnHull, out closestOnHull);
Vector3Wide.ConditionalSelect(skipDepthRefine, negatedTriangleNormal, refinedNormal, out localNormal);
depth = Vector.ConditionalSelect(skipDepthRefine, triangleFaceDepth, refinedDepth);
}
else
{
//No depth refine ran; the extreme point prepass did everything we needed. Just use the initial normal.
localNormal = negatedTriangleNormal;
closestOnHull = hullSupportAlongNegatedTriangleNormal;
depth = triangleFaceDepth;
}
Vector3Wide.Dot(triangleNormal, localNormal, out var triangleNormalDotLocalNormal);
inactiveLanes = Vector.BitwiseOr(inactiveLanes, Vector.BitwiseOr(Vector.GreaterThanOrEqual(triangleNormalDotLocalNormal, Vector<float>.Zero), Vector.LessThan(depth, depthThreshold)));
if (Vector.LessThanAll(inactiveLanes, Vector<int>.Zero))
{
//No contacts generated.
return;
}
//To find the contact manifold, we'll clip the triangle 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 6 contacts (provided there are no numerical errors); 2 per triangle edge.
var candidates = stackalloc ManifoldCandidateScalar[6];
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 triangle edge against the hull face.
//Note that we do not use the faceNormal x edgeOffset edge plane, but rather edgeOffset x localNormal.
//The faces are wound counterclockwise.
//Note that the triangle edges are packed into a Vector4. Historically, there were some minor codegen issues with Vector3.
//May not matter anymore, but it costs ~nothing to use a dead slot.
ref var aSlot = ref GatherScatter.GetOffsetInstance(ref triangleA, slotIndex);
public void Test(ref SphereWide a, ref TriangleWide b, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB,
out Vector <int> intersected, out Vector <float> distance, out Vector3Wide closestA, out Vector3Wide normal)
{
//Note that we're borrowing a lot here from the SphereTriangleCollisionTask. Could share more if you find yourself needing to change things dramatically.
//Main difficulty in fully sharing is that sweep tests do not honor one sidedness, so some of the conditions change.
//Work in the local space of the triangle, since it's quicker to transform the sphere position than the vertices of the triangle.
Matrix3x3Wide.CreateFromQuaternion(orientationB, out var rB);
Matrix3x3Wide.TransformByTransposedWithoutOverlap(offsetB, rB, out var localOffsetB);
Vector3Wide.Subtract(b.B, b.A, out var ab);
Vector3Wide.Subtract(b.C, b.A, out var ac);
//localOffsetA = -localOffsetB, so pa = triangle.A + localOffsetB.
Vector3Wide.Add(b.A, localOffsetB, out var pa);
Vector3Wide.CrossWithoutOverlap(ab, ac, out var localTriangleNormal);
Vector3Wide.Dot(localTriangleNormal, pa, out var paN);
//EdgeAB plane test: (pa x ab) * (ab x ac) >= 0
//EdgeAC plane test: (ac x pa) * (ab x ac) >= 0
//Note that these are scaled versions of the barycentric coordinates.
//To normalize them such that the weights of a point within the triangle equal 1, we just need to divide by dot(ab x ac, ab x ac).
//In other words, to test the third edge plane, we can ensure that the unnormalized weights are both positive and sum to a value less than dot(ab x ac, ab x ac).
//If a point is outside of an edge plane, we know that it's not in the face region or any other edge region. It could, however, be in an adjacent vertex region.
//Vertex cases can be handled by clamping an edge case.
//Further, note that for any query location, it is sufficient to only test one edge even if the point is outside two edge planes. If it's outside two edge planes,
//that just means it's going to be on the shared vertex, so a clamped edge test captures the correct closest point.
//So, at each edge, if the point is outside the plane, cache the edge. The last edge registering an outside result will be tested.
//(pa x ab) * (ab x ac) = (pa * ab) * (ab * ac) - (pa * ac) * (ab * ab)
//(ac x pa) * (ab x ac) = (ac * ab) * (pa * ac) - (ac * ac) * (pa * ab)
//(ab x ac) * (ab x ac) = (ab * ab) * (ac * ac) - (ab * ac) * (ab * ac)
Vector3Wide.Dot(pa, ab, out var abpa);
Vector3Wide.Dot(ab, ac, out var abac);
Vector3Wide.Dot(ac, pa, out var acpa);
Vector3Wide.Dot(ac, ac, out var acac);
Vector3Wide.Dot(ab, ab, out var abab);
var edgePlaneTestAB = abpa * abac - acpa * abab;
var edgePlaneTestAC = abac * acpa - acac * abpa;
var triangleNormalLengthSquared = abab * acac - abac * abac;
var edgePlaneTestBC = triangleNormalLengthSquared - edgePlaneTestAB - edgePlaneTestAC;
var outsideAB = Vector.LessThan(edgePlaneTestAB, Vector <float> .Zero);
var outsideAC = Vector.LessThan(edgePlaneTestAC, Vector <float> .Zero);
var outsideBC = Vector.LessThan(edgePlaneTestBC, Vector <float> .Zero);
var outsideAnyEdge = Vector.BitwiseOr(outsideAB, Vector.BitwiseOr(outsideAC, outsideBC));
Vector3Wide localClosestOnTriangle;
var negativeOne = new Vector <int>(-1);
if (Vector.EqualsAny(outsideAnyEdge, negativeOne))
{
//At least one lane detected a point outside of the triangle. Choose one edge which is outside as the representative.
Vector3Wide.ConditionalSelect(outsideAC, ac, ab, out var edgeDirection);
Vector3Wide.Subtract(b.C, b.B, out var bc);
Vector3Wide.ConditionalSelect(outsideBC, bc, edgeDirection, out edgeDirection);
Vector3Wide.ConditionalSelect(outsideBC, b.B, b.A, out var edgeStart);
Vector3Wide.Add(localOffsetB, edgeStart, out var negativeEdgeStartToP);
//This does some partially redundant work if the edge is AB or AC, but given that we didn't have bcbc or bcpb, it's fine.
Vector3Wide.Dot(negativeEdgeStartToP, edgeDirection, out var negativeOffsetDotEdge);
Vector3Wide.Dot(edgeDirection, edgeDirection, out var edgeDotEdge);
var edgeScale = Vector.Max(Vector <float> .Zero, Vector.Min(Vector <float> .One, -negativeOffsetDotEdge / edgeDotEdge));
Vector3Wide.Scale(edgeDirection, edgeScale, out var pointOnEdge);
Vector3Wide.Add(edgeStart, pointOnEdge, out pointOnEdge);
Vector3Wide.ConditionalSelect(outsideAnyEdge, pointOnEdge, localClosestOnTriangle, out localClosestOnTriangle);
}
if (Vector.EqualsAny(outsideAnyEdge, Vector <int> .Zero))
{
//p + N * (pa * N) / ||N||^2 = N * (pa * N) / ||N||^2 - (-p)
var nScale = paN / triangleNormalLengthSquared;
Vector3Wide.Scale(localTriangleNormal, nScale, out var offsetToPlane);
Vector3Wide.Subtract(offsetToPlane, localOffsetB, out var pointOnFace);
Vector3Wide.ConditionalSelect(outsideAnyEdge, localClosestOnTriangle, pointOnFace, out localClosestOnTriangle);
}
//normal = normalize(localOffsetA - localClosestOnTriangle) = (localOffsetB + localClosestOnTriangle) / (-||localOffsetB + localClosestOnTriangle||)
Vector3Wide.Add(localOffsetB, localClosestOnTriangle, out var localNormal);
Vector3Wide.Length(localNormal, out var localNormalLength);
Vector3Wide.Scale(localNormal, new Vector <float>(-1f) / localNormalLength, out localNormal);
Matrix3x3Wide.TransformWithoutOverlap(localNormal, rB, out normal);
Vector3Wide.Scale(normal, -a.Radius, out closestA);
distance = localNormalLength - a.Radius;
intersected = Vector.LessThanOrEqual(distance, Vector <float> .Zero);
}
public void Do()
{
Matrix3x3Wide.Transform(v, symmetric, out var intermediate);
Vector3Wide.Dot(v, intermediate, out result);
}