static void Select(ref Vector <float> distanceSquared, ref Vector3Wide localNormal, ref Vector <float> distanceSquaredCandidate, ref Vector3Wide localNormalCandidate)
{
var useCandidate = Vector.LessThan(distanceSquaredCandidate, distanceSquared);
distanceSquared = Vector.Min(distanceSquaredCandidate, distanceSquared);
Vector3Wide.ConditionalSelect(useCandidate, localNormalCandidate, localNormal, out localNormal);
}
public void Test(ref SphereWide a, ref CapsuleWide b, ref Vector <float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex1ContactManifoldWide manifold)
{
//The contact for a sphere-capsule pair is based on the closest point of the sphere center to the capsule internal line segment.
QuaternionWide.TransformUnitXY(orientationB, out var x, out var y);
Vector3Wide.Dot(y, offsetB, out var t);
t = Vector.Min(b.HalfLength, Vector.Max(-b.HalfLength, -t));
Vector3Wide.Scale(y, t, out var capsuleLocalClosestPointOnLineSegment);
Vector3Wide.Add(offsetB, capsuleLocalClosestPointOnLineSegment, out var sphereToInternalSegment);
Vector3Wide.Length(sphereToInternalSegment, out var internalDistance);
//Note that the normal points from B to A by convention. Here, the sphere is A, the capsule is B, so the normalization requires a negation.
var inverseDistance = new Vector <float>(-1f) / internalDistance;
Vector3Wide.Scale(sphereToInternalSegment, inverseDistance, out manifold.Normal);
var normalIsValid = Vector.GreaterThan(internalDistance, Vector <float> .Zero);
//If the center of the sphere is on the internal line segment, then choose a direction on the plane defined by the capsule's up vector.
//We computed one such candidate earlier. Note that we could usually get away with choosing a completely arbitrary direction, but
//going through the extra effort to compute a true local horizontal direction avoids some nasty corner case surprises if a user is trying
//to do something like manually resolving collisions or other query-based logic.
//A cheaper option would be to simply use the y axis as the normal. That's known to be suboptimal, but if we don't guarantee minimum penetration depth, that's totally fine.
//My guess is that computing x will be so cheap as to be irrelevant.
Vector3Wide.ConditionalSelect(normalIsValid, manifold.Normal, x, out manifold.Normal);
manifold.Depth = a.Radius + b.Radius - internalDistance;
manifold.FeatureId = Vector <int> .Zero;
//The contact position relative to object A (the sphere) is computed as the average of the extreme point along the normal toward the opposing shape on each shape, averaged.
//For capsule-sphere, this can be computed from the normal and depth.
var negativeOffsetFromSphere = manifold.Depth * 0.5f - a.Radius;
Vector3Wide.Scale(manifold.Normal, negativeOffsetFromSphere, out manifold.OffsetA);
manifold.ContactExists = Vector.GreaterThan(manifold.Depth, -speculativeMargin);
}
private static void Select(
ref Vector <float> depth, ref Vector3Wide localNormal,
ref Vector <float> depthCandidate, ref Vector3Wide localNormalCandidate)
{
var useCandidate = Vector.LessThan(depthCandidate, depth);
depth = Vector.Min(depth, depthCandidate);
Vector3Wide.ConditionalSelect(useCandidate, localNormalCandidate, localNormal, out localNormal);
}
public void ApplyFlipMask(ref Vector3Wide offsetB, ref Vector <int> flipMask)
{
Vector3Wide.Negate(ref Normal, out var flippedNormal);
Vector3Wide.Subtract(ref OffsetA0, ref offsetB, out var flippedContactPosition);
Vector3Wide.Negate(ref offsetB, out var flippedOffsetB);
Vector3Wide.ConditionalSelect(ref flipMask, ref flippedNormal, ref Normal, out Normal);
Vector3Wide.ConditionalSelect(ref flipMask, ref flippedContactPosition, ref OffsetA0, out OffsetA0);
Vector3Wide.ConditionalSelect(ref flipMask, ref flippedOffsetB, ref offsetB, out offsetB);
}
public void ComputeSupport(ref CapsuleWide shape, ref Matrix3x3Wide orientation, ref Vector3Wide direction, out Vector3Wide support)
{
Vector3Wide.Scale(orientation.Y, shape.HalfLength, out support);
Vector3Wide.Negate(support, out var negated);
Vector3Wide.Dot(orientation.Y, direction, out var dot);
var shouldNegate = Vector.LessThan(dot, Vector <float> .Zero);
Vector3Wide.ConditionalSelect(shouldNegate, negated, support, out support);
}
private static void SelectForEdge(
ref Vector <float> edgeDepth, ref Vector3Wide edgeLocalNormal, ref Vector <int> edgeDirectionIndex,
ref Vector <float> edgeDepthCandidate, ref Vector3Wide edgeLocalNormalCandidate, Vector <int> edgeDirectionIndexCandidate)
{
var useCandidate = Vector.LessThan(edgeDepthCandidate, edgeDepth);
edgeDepth = Vector.Min(edgeDepth, edgeDepthCandidate);
Vector3Wide.ConditionalSelect(useCandidate, edgeLocalNormalCandidate, edgeLocalNormal, out edgeLocalNormal);
edgeDirectionIndex = Vector.ConditionalSelect(useCandidate, edgeDirectionIndexCandidate, edgeDirectionIndex);
}
public void ComputeSupport(ref TriangleWide shape, ref Matrix3x3Wide orientation, ref Vector3Wide direction, out Vector3Wide support)
{
Matrix3x3Wide.TransformByTransposedWithoutOverlap(direction, orientation, out var localDirection);
Vector3Wide.Dot(shape.A, localDirection, out var a);
Vector3Wide.Dot(shape.B, localDirection, out var b);
Vector3Wide.Dot(shape.C, localDirection, out var c);
var max = Vector.Max(a, Vector.Max(b, c));
Vector3Wide.ConditionalSelect(Vector.Equals(max, a), shape.A, shape.B, out var localSupport);
Vector3Wide.ConditionalSelect(Vector.Equals(max, c), shape.C, localSupport, out localSupport);
Matrix3x3Wide.TransformWithoutOverlap(localSupport, orientation, out support);
}
public void Test(ref SphereWide a, ref BoxWide b, ref Vector <float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex1ContactManifoldWide manifold)
{
//Clamp the position of the sphere to the box.
Matrix3x3Wide.CreateFromQuaternion(orientationB, out var orientationMatrixB);
//Note that we're working with localOffsetB, which is the offset from A to B, even though conceptually we want to be operating on the offset from B to A.
//Those offsets differ only by their sign, so are equivalent due to the symmetry of the box. The negation is left implicit.
Matrix3x3Wide.TransformByTransposedWithoutOverlap(offsetB, orientationMatrixB, out var localOffsetB);
Vector3Wide clampedLocalOffsetB;
clampedLocalOffsetB.X = Vector.Min(Vector.Max(localOffsetB.X, -b.HalfWidth), b.HalfWidth);
clampedLocalOffsetB.Y = Vector.Min(Vector.Max(localOffsetB.Y, -b.HalfHeight), b.HalfHeight);
clampedLocalOffsetB.Z = Vector.Min(Vector.Max(localOffsetB.Z, -b.HalfLength), b.HalfLength);
//Implicit negation to make the normal point from B to A, following convention.
Vector3Wide.Subtract(clampedLocalOffsetB, localOffsetB, out var outsideNormal);
Vector3Wide.Length(outsideNormal, out var distance);
var inverseDistance = Vector <float> .One / distance;
Vector3Wide.Scale(outsideNormal, inverseDistance, out outsideNormal);
var outsideDepth = a.Radius - distance;
//If the sphere center is inside the box, then the shortest local axis to exit must be chosen.
var depthX = b.HalfWidth - Vector.Abs(localOffsetB.X);
var depthY = b.HalfHeight - Vector.Abs(localOffsetB.Y);
var depthZ = b.HalfLength - Vector.Abs(localOffsetB.Z);
var insideDepth = Vector.Min(depthX, Vector.Min(depthY, depthZ));
//Only one axis may have a nonzero component.
var useX = Vector.Equals(insideDepth, depthX);
var useY = Vector.AndNot(Vector.Equals(insideDepth, depthY), useX);
var useZ = Vector.OnesComplement(Vector.BitwiseOr(useX, useY));
Vector3Wide insideNormal;
//A faster sign test would be nice.
insideNormal.X = Vector.ConditionalSelect(useX, Vector.ConditionalSelect(Vector.LessThan(localOffsetB.X, Vector <float> .Zero), new Vector <float>(1f), new Vector <float>(-1f)), Vector <float> .Zero);
insideNormal.Y = Vector.ConditionalSelect(useY, Vector.ConditionalSelect(Vector.LessThan(localOffsetB.Y, Vector <float> .Zero), new Vector <float>(1f), new Vector <float>(-1f)), Vector <float> .Zero);
insideNormal.Z = Vector.ConditionalSelect(useZ, Vector.ConditionalSelect(Vector.LessThan(localOffsetB.Z, Vector <float> .Zero), new Vector <float>(1f), new Vector <float>(-1f)), Vector <float> .Zero);
insideDepth += a.Radius;
var useInside = Vector.Equals(distance, Vector <float> .Zero);
Vector3Wide.ConditionalSelect(useInside, insideNormal, outsideNormal, out var localNormal);
Matrix3x3Wide.TransformWithoutOverlap(localNormal, orientationMatrixB, out manifold.Normal);
manifold.Depth = Vector.ConditionalSelect(useInside, insideDepth, outsideDepth);
manifold.FeatureId = Vector <int> .Zero;
//The contact position relative to object A (the sphere) is computed as the average of the extreme point along the normal toward the opposing shape on each shape, averaged.
//For capsule-sphere, this can be computed from the normal and depth.
var negativeOffsetFromSphere = manifold.Depth * 0.5f - a.Radius;
Vector3Wide.Scale(manifold.Normal, negativeOffsetFromSphere, out manifold.OffsetA);
manifold.ContactExists = Vector.GreaterThan(manifold.Depth, -speculativeMargin);
}
public void ApplyFlipMask(ref Vector3Wide offsetB, ref Vector <int> flipMask)
{
Vector3Wide.Negate(ref Normal, out var flippedNormal);
Vector3Wide.Subtract(ref OffsetA0, ref offsetB, out var flippedA0);
Vector3Wide.Subtract(ref OffsetA1, ref offsetB, out var flippedA1);
Vector3Wide.Subtract(ref OffsetA2, ref offsetB, out var flippedA2);
Vector3Wide.Subtract(ref OffsetA3, ref offsetB, out var flippedA3);
Vector3Wide.Negate(ref offsetB, out var flippedOffsetB);
Vector3Wide.ConditionalSelect(ref flipMask, ref flippedNormal, ref Normal, out Normal);
Vector3Wide.ConditionalSelect(ref flipMask, ref flippedA0, ref OffsetA0, out OffsetA0);
Vector3Wide.ConditionalSelect(ref flipMask, ref flippedA1, ref OffsetA1, out OffsetA1);
Vector3Wide.ConditionalSelect(ref flipMask, ref flippedA2, ref OffsetA2, out OffsetA2);
Vector3Wide.ConditionalSelect(ref flipMask, ref flippedA3, ref OffsetA3, out OffsetA3);
Vector3Wide.ConditionalSelect(ref flipMask, ref flippedOffsetB, ref offsetB, out offsetB);
}
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 void Test(ref CapsuleWide a, ref BoxWide b, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB,
out Vector <int> intersected, out Vector <float> distance, out Vector3Wide closestA, out Vector3Wide normal)
{
//Bring the capsule into the box's local space.
Matrix3x3Wide.CreateFromQuaternion(orientationB, out var rB);
QuaternionWide.TransformUnitY(orientationA, out var capsuleAxis);
Matrix3x3Wide.TransformByTransposedWithoutOverlap(capsuleAxis, rB, out var localCapsuleAxis);
Matrix3x3Wide.TransformByTransposedWithoutOverlap(offsetB, rB, out var localOffsetB);
Vector3Wide.Negate(localOffsetB, out var localOffsetA);
Vector3Wide.Scale(localCapsuleAxis, a.HalfLength, out var endpointOffset);
Vector3Wide.Subtract(localOffsetA, endpointOffset, out var endpoint0);
TestEndpointNormal(ref localOffsetA, ref localCapsuleAxis, ref a.HalfLength, ref endpoint0, ref b, out var depth, out var localNormal);
Vector3Wide.Add(localOffsetA, endpointOffset, out var endpoint1);
TestEndpointNormal(ref localOffsetA, ref localCapsuleAxis, ref a.HalfLength, ref endpoint1, ref b, out var depthCandidate, out var localNormalCandidate);
Select(ref depth, ref localNormal, ref depthCandidate, ref localNormalCandidate);
//Note that we did not yet pick a closest point for endpoint cases. That's because each case only generates a normal and interval test, not a minimal distance test.
//The choice of which endpoint is actually closer is deferred until now.
Vector3Wide.Dot(localCapsuleAxis, localNormal, out var endpointChoiceDot);
Vector3Wide.ConditionalSelect(Vector.LessThan(endpointChoiceDot, Vector <float> .Zero), endpoint1, endpoint0, out var localClosest);
Vector3Wide edgeLocalNormal, edgeLocalNormalCandidate;
//Swizzle XYZ -> YZX
TestBoxEdge(ref localOffsetA.Y, ref localOffsetA.Z, ref localOffsetA.X,
ref localCapsuleAxis.Y, ref localCapsuleAxis.Z, ref localCapsuleAxis.X,
ref b.HalfHeight, ref b.HalfLength, ref b.HalfWidth,
out var edgeDepth, out edgeLocalNormal.Y, out edgeLocalNormal.Z, out edgeLocalNormal.X);
var edgeDirectionIndex = Vector <int> .Zero;
//Swizzle XYZ -> ZXY
TestBoxEdge(ref localOffsetA.Z, ref localOffsetA.X, ref localOffsetA.Y,
ref localCapsuleAxis.Z, ref localCapsuleAxis.X, ref localCapsuleAxis.Y,
ref b.HalfLength, ref b.HalfWidth, ref b.HalfHeight,
out var edgeDepthCandidate, out edgeLocalNormalCandidate.Z, out edgeLocalNormalCandidate.X, out edgeLocalNormalCandidate.Y);
SelectForEdge(ref edgeDepth, ref edgeLocalNormal, ref edgeDirectionIndex, ref edgeDepthCandidate, ref edgeLocalNormalCandidate, Vector <int> .One);
//Swizzle XYZ -> XYZ
TestBoxEdge(ref localOffsetA.X, ref localOffsetA.Y, ref localOffsetA.Z,
ref localCapsuleAxis.X, ref localCapsuleAxis.Y, ref localCapsuleAxis.Z,
ref b.HalfWidth, ref b.HalfHeight, ref b.HalfLength,
out edgeDepthCandidate, out edgeLocalNormalCandidate.X, out edgeLocalNormalCandidate.Y, out edgeLocalNormalCandidate.Z);
SelectForEdge(ref edgeDepth, ref edgeLocalNormal, ref edgeDirectionIndex, ref edgeDepthCandidate, ref edgeLocalNormalCandidate, new Vector <int>(2));
//We can skip the edge finalization if they aren't ever used.
if (Vector.LessThanAny(edgeDepth, depth))
{
GetEdgeClosestPoint(ref edgeLocalNormal, ref edgeDirectionIndex, ref b, ref localOffsetA, ref localCapsuleAxis, ref a.HalfLength, out var edgeLocalClosest);
Select(ref depth, ref localNormal, ref localClosest, ref edgeDepth, ref edgeLocalNormal, ref edgeLocalClosest);
}
TestVertexAxis(ref b, ref localOffsetA, ref localCapsuleAxis, ref a.HalfLength, out depthCandidate, out localNormalCandidate, out var localClosestCandidate);
Select(ref depth, ref localNormal, ref localClosest, ref depthCandidate, ref localNormalCandidate, ref localClosestCandidate);
//Transform normal and closest point back into world space.
Matrix3x3Wide.TransformWithoutOverlap(localNormal, rB, out normal);
Matrix3x3Wide.TransformWithoutOverlap(localClosest, rB, out closestA);
Vector3Wide.Add(closestA, offsetB, out closestA);
Vector3Wide.Scale(normal, a.Radius, out var closestOffset);
Vector3Wide.Subtract(closestA, closestOffset, out closestA);
distance = -depth - a.Radius;
intersected = Vector.LessThan(distance, Vector <float> .Zero);
}
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);
Vector <int> hullInsideAndBelowTriangle;
Vector3Wide.CrossWithoutOverlap(triangleAB, triangleNormal, out var edgePlaneAB);
Vector3Wide.CrossWithoutOverlap(triangleBC, triangleNormal, out var edgePlaneBC);
Vector3Wide.CrossWithoutOverlap(triangleCA, triangleNormal, out var edgePlaneCA);
if (Vector.LessThanAny(hullBelowPlane, Vector <int> .Zero))
{
//Is the hull position within the triangle bounds?
Vector3Wide.Dot(edgePlaneAB, triangleA, out var abPlaneTest);
Vector3Wide.Dot(edgePlaneBC, triangleB, out var bcPlaneTest);
Vector3Wide.Dot(edgePlaneCA, triangleC, out var caPlaneTest);
hullInsideAndBelowTriangle = Vector.BitwiseAnd(
Vector.BitwiseAnd(hullBelowPlane, Vector.LessThanOrEqual(abPlaneTest, Vector <float> .Zero)),
Vector.BitwiseAnd(Vector.LessThanOrEqual(bcPlaneTest, Vector <float> .Zero), Vector.LessThanOrEqual(caPlaneTest, Vector <float> .Zero)));
}
else
{
hullInsideAndBelowTriangle = Vector <int> .Zero;
}
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.
Vector <int> triangleNormalIsMinimal;
var hullSupportFinder = default(ConvexHullSupportFinder);
DepthRefiner.SimplexWithWitness simplex;
var triangleSupportFinder = default(PretransformedTriangleSupportFinder);
//Create a simplex entry for the direction from the hull center to triangle center.
DepthRefiner.FindSupport(b, triangle, localTriangleCenter, hullLocalTriangleOrientation, ref hullSupportFinder, ref triangleSupportFinder, initialNormal, inactiveLanes, out simplex.A.Support, out simplex.A.SupportOnA);
Vector3Wide.Dot(simplex.A.Support, initialNormal, out var depth);
simplex.A.Exists = Vector.OnesComplement(inactiveLanes);
//Create a simplex entry for the triangle face normal.
Vector3Wide.Negate(triangleNormal, out var negatedTriangleNormal);
hullSupportFinder.ComputeLocalSupport(b, negatedTriangleNormal, inactiveLanes, out var hullSupportAlongTriangleNormal);
simplex.B.SupportOnA = hullSupportAlongTriangleNormal;
Vector3Wide.Subtract(simplex.B.SupportOnA, localTriangleCenter, out simplex.B.Support);
Vector3Wide.Dot(simplex.B.Support, negatedTriangleNormal, out var triangleFaceDepth);
var useTriangleFace = Vector.LessThan(triangleFaceDepth, depth);
Vector3Wide.ConditionalSelect(useTriangleFace, negatedTriangleNormal, initialNormal, out initialNormal);
depth = Vector.ConditionalSelect(useTriangleFace, triangleFaceDepth, depth);
simplex.B.Exists = simplex.A.Exists;
simplex.C.Exists = default;
//Check if the extreme point on the hull is contained within the bounds of the triangle face. If it is, there is no need for a full depth refinement.
Vector3Wide.Subtract(triangleA, hullSupportAlongTriangleNormal, out var closestToA);
Vector3Wide.Subtract(triangleB, hullSupportAlongTriangleNormal, out var closestToB);
Vector3Wide.Subtract(triangleC, hullSupportAlongTriangleNormal, out var closestToC);
Vector3Wide.Dot(edgePlaneAB, closestToA, out var extremeABPlaneTest);
Vector3Wide.Dot(edgePlaneBC, closestToB, out var extremeBCPlaneTest);
Vector3Wide.Dot(edgePlaneCA, closestToC, out var extremeCAPlaneTest);
triangleNormalIsMinimal = Vector.BitwiseAnd(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;
if (Vector.EqualsAny(skipDepthRefine, Vector <int> .Zero))
{
DepthRefiner.FindMinimumDepth(
b, triangle, localTriangleCenter, hullLocalTriangleOrientation, ref hullSupportFinder, ref triangleSupportFinder, ref simplex, initialNormal, depth, skipDepthRefine, 1e-5f * epsilonScale, depthThreshold,
out var refinedDepth, out var refinedNormal, out var refinedClosestOnHull);
Vector3Wide.ConditionalSelect(skipDepthRefine, hullSupportAlongTriangleNormal, refinedClosestOnHull, out closestOnHull);
Vector3Wide.ConditionalSelect(skipDepthRefine, initialNormal, refinedNormal, out localNormal);
depth = Vector.ConditionalSelect(skipDepthRefine, depth, refinedDepth);
}
else
{
//No depth refine ran; the extreme point prepass did everything we needed. Just use the initial normal.
localNormal = initialNormal;
closestOnHull = hullSupportAlongTriangleNormal;
}
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 Test2(
ref CapsuleWide a, ref BoxWide b,
ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB,
out Convex2ContactManifoldWide manifold)
{
QuaternionWide.Conjugate(ref orientationB, out var toLocalB);
QuaternionWide.TransformWithoutOverlap(ref offsetB, ref toLocalB, out var localOffsetA);
Vector3Wide.Negate(ref localOffsetA);
QuaternionWide.ConcatenateWithoutOverlap(ref orientationA, ref toLocalB, out var boxLocalOrientationA);
QuaternionWide.TransformUnitY(ref boxLocalOrientationA, out var capsuleAxis);
//For each face of the box, clip the capsule axis against its bounding planes and then clamp the result to the box's extent along that axis.
//The face which has the largest clipped interval of the capsule line segment will be picked as the representative.
var positive = Vector <float> .One;
var negative = -positive;
var divisionGuard = new Vector <float>(1e-15f);
//Division by zero is hacked away by clamping the magnitude to some nonzero value and recovering the sign in the numerator.
//The resulting interval will be properly signed and enormous, so it serves the necessary purpose.
//Note the negation: t = dot(N, +-halfExtent - offsetA) / dot(N, capsuleAxis) = dot(N, offsetA +- halfExtent) / -dot(N, capsuleAxis)
var scaleX = Vector.ConditionalSelect(Vector.LessThan(capsuleAxis.X, Vector <float> .Zero), positive, negative) / Vector.Max(Vector.Abs(capsuleAxis.X), divisionGuard);
var scaleY = Vector.ConditionalSelect(Vector.LessThan(capsuleAxis.Y, Vector <float> .Zero), positive, negative) / Vector.Max(Vector.Abs(capsuleAxis.Y), divisionGuard);
var scaleZ = Vector.ConditionalSelect(Vector.LessThan(capsuleAxis.Z, Vector <float> .Zero), positive, negative) / Vector.Max(Vector.Abs(capsuleAxis.Z), divisionGuard);
Vector3Wide scaledExtents, scaledOffset;
scaledExtents.X = b.HalfWidth * Vector.Abs(scaleX);
scaledExtents.Y = b.HalfHeight * Vector.Abs(scaleY);
scaledExtents.Z = b.HalfLength * Vector.Abs(scaleZ);
scaledOffset.X = localOffsetA.X * scaleX;
scaledOffset.Y = localOffsetA.Y * scaleY;
scaledOffset.Z = localOffsetA.Z * scaleZ;
Vector3Wide.Subtract(ref scaledOffset, ref scaledExtents, out var tCandidateMin);
var negativeHalfLength = -a.HalfLength;
Vector3Wide.Max(ref negativeHalfLength, ref tCandidateMin, out tCandidateMin);
Vector3Wide.Add(ref scaledOffset, ref scaledExtents, out var tCandidateMax);
Vector3Wide.Min(ref a.HalfLength, ref tCandidateMax, out tCandidateMax);
//We now have clipped intervals for all faces. The faces along axis X, for example, use interval max(tMin.Y, tMin.Z) to min(tMax.Y, tMax.Z).
//Find the longest interval. Note that it's possible for the interval length to be negative when a line segment does not enter the face's voronoi region.
//It may be negative for all axes, even; that can happen when the line segment is outside all face voronoi regions. That's fine, though.
//It's simply a signal that the line segment is in one of the face-adjacent voronoi regions, and clamping the endpoints will provide the closest point on the box to the segment.
var intervalMaxX = Vector.Min(tCandidateMax.Y, tCandidateMax.Z);
var intervalMaxY = Vector.Min(tCandidateMax.X, tCandidateMax.Z);
var intervalMaxZ = Vector.Min(tCandidateMax.X, tCandidateMax.Y);
var intervalMinX = Vector.Max(tCandidateMin.Y, tCandidateMin.Z);
var intervalMinY = Vector.Max(tCandidateMin.X, tCandidateMin.Z);
var intervalMinZ = Vector.Max(tCandidateMin.X, tCandidateMin.Y);
var intervalLengthX = Vector.Max(Vector <float> .Zero, intervalMaxX - intervalMinX);
var intervalLengthY = Vector.Max(Vector <float> .Zero, intervalMaxY - intervalMinY);
var intervalLengthZ = Vector.Max(Vector <float> .Zero, intervalMaxZ - intervalMinZ);
var x2 = capsuleAxis.X * capsuleAxis.X;
var y2 = capsuleAxis.Y * capsuleAxis.Y;
var z2 = capsuleAxis.Z * capsuleAxis.Z;
var projectedLengthSquaredX = (y2 + z2) * intervalLengthX * intervalLengthX;
var projectedLengthSquaredY = (x2 + z2) * intervalLengthY * intervalLengthY;
var projectedLengthSquaredZ = (x2 + y2) * intervalLengthZ * intervalLengthZ;
var maxLengthSquared = Vector.Max(projectedLengthSquaredX, Vector.Max(projectedLengthSquaredY, projectedLengthSquaredZ));
var useX = Vector.Equals(projectedLengthSquaredX, maxLengthSquared);
var useY = Vector.Equals(projectedLengthSquaredY, maxLengthSquared);
var tMin = Vector.ConditionalSelect(useX, intervalMinX, Vector.ConditionalSelect(useY, intervalMinY, intervalMinZ));
var tMax = Vector.ConditionalSelect(useX, intervalMaxX, Vector.ConditionalSelect(useY, intervalMaxY, intervalMaxZ));
Vector3Wide.Scale(ref capsuleAxis, ref tMin, out var capsuleStart);
Vector3Wide.Scale(ref capsuleAxis, ref tMax, out var capsuleEnd);
Vector3Wide.Add(ref localOffsetA, ref capsuleStart, out capsuleStart);
Vector3Wide.Add(ref localOffsetA, ref capsuleEnd, out capsuleEnd);
//Clamp the start and end to the box. Note that we haven't kept track of which axis we're working on, so we just do clamping on all three axes. Two axes will be wasted,
//but it's easier to do this than to try to reconstruct which axis is the one we're supposed to work on.
Vector3Wide boxStart, boxEnd;
boxStart.X = Vector.Max(Vector.Min(capsuleStart.X, b.HalfWidth), -b.HalfWidth);
boxStart.Y = Vector.Max(Vector.Min(capsuleStart.Y, b.HalfHeight), -b.HalfHeight);
boxStart.Z = Vector.Max(Vector.Min(capsuleStart.Z, b.HalfLength), -b.HalfLength);
boxEnd.X = Vector.Max(Vector.Min(capsuleEnd.X, b.HalfWidth), -b.HalfWidth);
boxEnd.Y = Vector.Max(Vector.Min(capsuleEnd.Y, b.HalfHeight), -b.HalfHeight);
boxEnd.Z = Vector.Max(Vector.Min(capsuleEnd.Z, b.HalfLength), -b.HalfLength);
//Compute the closest point on the line segment to the clamped start and end.
Vector3Wide.Subtract(ref boxStart, ref localOffsetA, out var aToBoxStart);
Vector3Wide.Subtract(ref boxEnd, ref localOffsetA, out var aToBoxEnd);
Vector3Wide.Dot(ref capsuleAxis, ref aToBoxStart, out var dotStart);
Vector3Wide.Dot(ref capsuleAxis, ref aToBoxEnd, out var dotEnd);
dotStart = Vector.Max(Vector.Min(dotStart, a.HalfLength), negativeHalfLength);
dotEnd = Vector.Max(Vector.Min(dotEnd, a.HalfLength), negativeHalfLength);
//Now compute the distances. The normal will be defined by the shorter of the two offsets.
Vector3Wide.Scale(ref capsuleAxis, ref dotStart, out var closestFromBoxStart);
Vector3Wide.Scale(ref capsuleAxis, ref dotEnd, out var closestFromBoxEnd);
Vector3Wide.Add(ref closestFromBoxStart, ref localOffsetA, out closestFromBoxStart);
Vector3Wide.Add(ref closestFromBoxEnd, ref localOffsetA, out closestFromBoxEnd);
//Note that these offsets are pointing from the box to the capsule (B to A), matching contact normal convention.
Vector3Wide.Subtract(ref closestFromBoxStart, ref boxStart, out var startOffset);
Vector3Wide.Subtract(ref closestFromBoxEnd, ref boxEnd, out var endOffset);
Vector3Wide.LengthSquared(ref startOffset, out var startLengthSquared);
Vector3Wide.LengthSquared(ref endOffset, out var endLengthSquared);
var minLengthSquared = Vector.Min(startLengthSquared, endLengthSquared);
var useStart = Vector.Equals(minLengthSquared, startLengthSquared);
Vector3Wide.ConditionalSelect(ref useStart, ref startOffset, ref endOffset, out var localNormal);
var inverseLength = Vector <float> .One / Vector.SquareRoot(minLengthSquared);
Vector3Wide.Scale(ref localNormal, ref inverseLength, out localNormal);
//Note that we defer contact position offseting by the normal * depth until later when we know the normal isn't going to change.
Vector3Wide.Subtract(ref closestFromBoxStart, ref localOffsetA, out var localA0);
Vector3Wide.Subtract(ref closestFromBoxEnd, ref localOffsetA, out var localA1);
Vector3Wide.Dot(ref localNormal, ref startOffset, out var dot0);
Vector3Wide.Dot(ref localNormal, ref endOffset, out var dot1);
manifold.Depth0 = a.Radius - dot0;
manifold.Depth1 = a.Radius - dot1;
manifold.FeatureId0 = Vector <int> .Zero;
manifold.FeatureId1 = Vector <int> .One;
//Capsule-box has an extremely important property:
//Capsule internal line segments almost never intersect the box due to the capsule's radius.
//We can effectively split this implementation into two tests- exterior and interior.
//The interior test only needs to be run if one of the subpairs happens to be deeply intersecting, which should be ~never in practice.
//That's fortunate, because computing an accurate interior normal is painful!
var useInterior = Vector.LessThan(minLengthSquared, new Vector <float>(1e-10f));
if (Vector.EqualsAny(useInterior, new Vector <int>(-1)))
{
//There are six possible separating axes when the capsule's internal line segment intersects the box:
//box.X
//box.Y
//box.Z
//box.X x capsule.Y
//box.Y x capsule.Y
//box.Z x capsule.Y
//Choose the axis with the minimum penetration depth.
//By working in the box's local space, box.X/Y/Z become simply (1,0,0), (0,1,0), and (0,0,1). This cancels out a whole bunch of arithmetic.
//Note that these depths will not take into account the radius until the very end. We pretend that the capsule is simply a line until then, since the radius changes nothing.
var faceDepthX = b.HalfWidth - localOffsetA.X + Vector.Abs(capsuleAxis.X * a.HalfLength);
var faceDepthY = b.HalfHeight - localOffsetA.Y + Vector.Abs(capsuleAxis.Y * a.HalfLength);
var faceDepthZ = b.HalfLength - localOffsetA.Z + Vector.Abs(capsuleAxis.Z * a.HalfLength);
var faceDepth = Vector.Min(Vector.Min(faceDepthX, faceDepthY), faceDepthZ);
var useFaceX = Vector.Equals(faceDepth, faceDepthX);
var useFaceY = Vector.AndNot(Vector.Equals(faceDepth, faceDepthY), useFaceX);
var useFaceZ = Vector.OnesComplement(Vector.BitwiseOr(useFaceX, useFaceY));
//Note that signs are calibrated as a last step so that edge normals do not need their own calibration.
Vector3Wide faceNormal;
faceNormal.X = Vector.ConditionalSelect(useFaceX, Vector <float> .One, Vector <float> .Zero);
faceNormal.Y = Vector.ConditionalSelect(useFaceY, Vector <float> .One, Vector <float> .Zero);
faceNormal.Z = Vector.ConditionalSelect(useFaceZ, Vector <float> .One, Vector <float> .Zero);
//For each edge, given the zero components, the cross product boils down to creating a perpendicular 2d vector on the plane of the box axis from the cylinder axis.
var infiniteDepth = new Vector <float>(float.MaxValue);
var bestDepth = infiniteDepth;
var edgeXLength = Vector.SquareRoot(y2 + z2);
var edgeYLength = Vector.SquareRoot(x2 + z2);
var edgeZLength = Vector.SquareRoot(x2 + y2);
//depth = dot(N, halfExtents - offsetA), where N = (edgeAxis x capsuleAxis) / ||(edgeAxis x capsuleAxis)||, and both N and halfExtents have their signs calibrated.
//Using abs allows us to handle the sign calibration inline at the cost of splitting the dot product.
var inverseLengthX = Vector <float> .One / edgeXLength;
var inverseLengthY = Vector <float> .One / edgeYLength;
var inverseLengthZ = Vector <float> .One / edgeZLength;
var edgeXDepth = (Vector.Abs(b.HalfHeight * capsuleAxis.Z - b.HalfLength * capsuleAxis.Y) - Vector.Abs(offsetB.Y * capsuleAxis.Z - offsetB.Z * capsuleAxis.Y)) * inverseLengthX;
var edgeYDepth = (Vector.Abs(b.HalfWidth * capsuleAxis.Z - b.HalfLength * capsuleAxis.X) - Vector.Abs(offsetB.X * capsuleAxis.Z - offsetB.Z * capsuleAxis.X)) * inverseLengthY;
var edgeZDepth = (Vector.Abs(b.HalfWidth * capsuleAxis.Y - b.HalfHeight * capsuleAxis.X) - Vector.Abs(offsetB.X * capsuleAxis.Y - offsetB.Y * capsuleAxis.X)) * inverseLengthZ;
var epsilon = new Vector <float>(1e-9f);
//If the capsule internal segment and the edge are parallel, we ignore the edge by replacing its depth with ~infinity.
//Such parallel axes are guaranteed to do no better than one of the face directions since this path is only relevant during segment-box deep collisions.
edgeXDepth = Vector.ConditionalSelect(Vector.LessThan(edgeXLength, epsilon), infiniteDepth, edgeXDepth);
edgeYDepth = Vector.ConditionalSelect(Vector.LessThan(edgeYLength, epsilon), infiniteDepth, edgeYDepth);
edgeZDepth = Vector.ConditionalSelect(Vector.LessThan(edgeZLength, epsilon), infiniteDepth, edgeZDepth);
var edgeDepth = Vector.Min(Vector.Min(edgeXDepth, edgeYDepth), edgeZDepth);
var useEdgeX = Vector.Equals(edgeDepth, edgeXDepth);
var useEdgeY = Vector.Equals(edgeDepth, edgeYDepth);
Vector3Wide edgeNormal;
edgeNormal.X = Vector.ConditionalSelect(useEdgeX, Vector <float> .Zero, Vector.ConditionalSelect(useEdgeY, capsuleAxis.Z, capsuleAxis.Y));
edgeNormal.Y = Vector.ConditionalSelect(useEdgeX, capsuleAxis.Z, Vector.ConditionalSelect(useEdgeY, Vector <float> .Zero, -capsuleAxis.X));
edgeNormal.Z = Vector.ConditionalSelect(useEdgeX, -capsuleAxis.Y, Vector.ConditionalSelect(useEdgeY, -capsuleAxis.X, Vector <float> .Zero));
var useEdge = Vector.LessThan(edgeDepth, faceDepth);
Vector3Wide.ConditionalSelect(ref useEdge, ref edgeNormal, ref faceNormal, out var interiorNormal);
//Calibrate the normal such that it points from B to A.
Vector3Wide.Dot(ref interiorNormal, ref offsetB, out var dot);
var shouldNegateNormal = Vector.GreaterThan(dot, Vector <float> .Zero);
interiorNormal.X = Vector.ConditionalSelect(shouldNegateNormal, -interiorNormal.X, interiorNormal.X);
interiorNormal.Y = Vector.ConditionalSelect(shouldNegateNormal, -interiorNormal.Y, interiorNormal.Y);
interiorNormal.Z = Vector.ConditionalSelect(shouldNegateNormal, -interiorNormal.Z, interiorNormal.Z);
//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.
//Compute the interval of the box on the normal. Note that the normal is already calibrated to point from B to A (box to capsule).
var boxExtreme = Vector.Abs(localNormal.X * b.HalfWidth) + Vector.Abs(localNormal.Y * b.HalfHeight) + Vector.Abs(localNormal.Z * b.HalfLength);
Vector3Wide.Dot(ref localNormal, ref closestFromBoxStart, out dot0);
Vector3Wide.Dot(ref localNormal, ref closestFromBoxEnd, out dot1);
manifold.Depth0 = Vector.ConditionalSelect(useInterior, a.Radius + boxExtreme - dot0, manifold.Depth0);
manifold.Depth1 = Vector.ConditionalSelect(useInterior, a.Radius + boxExtreme - dot1, manifold.Depth1);
//The interior normal doesn't check all possible separating axes in the non-segment-intersecting case.
//It would be wrong to overwrite the initial test if the interior test was unnecessary.
Vector3Wide.ConditionalSelect(ref useInterior, ref interiorNormal, ref localNormal, out localNormal);
}
//Transform A0, A1, and the normal into world space.
Matrix3x3Wide.CreateFromQuaternion(ref orientationB, out var orientationMatrixB);
Matrix3x3Wide.TransformWithoutOverlap(ref localNormal, ref orientationMatrixB, out manifold.Normal);
Matrix3x3Wide.TransformWithoutOverlap(ref localA0, ref orientationMatrixB, out manifold.OffsetA0);
Matrix3x3Wide.TransformWithoutOverlap(ref localA1, ref 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(ref manifold.Normal, ref negativeOffsetFromA0, out var normalPush0);
Vector3Wide.Scale(ref manifold.Normal, ref negativeOffsetFromA1, out var normalPush1);
Vector3Wide.Add(ref manifold.OffsetA0, ref normalPush0, out manifold.OffsetA0);
Vector3Wide.Add(ref manifold.OffsetA1, ref normalPush1, out manifold.OffsetA1);
//Note that it is possible that the two contacts is redundant. Only keep them both if they're separated.
manifold.Count = Vector.ConditionalSelect(Vector.LessThan(Vector.Abs(dotStart - dotEnd), a.HalfLength * new Vector <float>(1e-7f)), Vector <int> .One, new Vector <int>(2));
}
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);
public static void ConditionalSelect(ref Vector <int> mask, ref BoundingBoxWide a, ref BoundingBoxWide b, out BoundingBoxWide result)
{
Vector3Wide.ConditionalSelect(ref mask, ref a.Min, ref b.Min, out result.Min);
Vector3Wide.ConditionalSelect(ref mask, ref a.Max, ref b.Max, out result.Max);
}
public void Test(ref SphereWide a, ref TriangleWide b, ref Vector <float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, 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.
//(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(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 void Test(
ref CapsuleWide a, ref CapsuleWide b, ref Vector <float> speculativeMargin,
ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount,
out Convex2ContactManifoldWide manifold)
{
//Compute the closest points between the two line segments. No clamping to begin with.
//We want to minimize distance = ||(a + da * ta) - (b + db * tb)||.
//Taking the derivative with respect to ta and doing some algebra (taking into account ||da|| == ||db|| == 1) to solve for ta yields:
//ta = (da * (b - a) + (db * (a - b)) * (da * db)) / (1 - ((da * db) * (da * db))
QuaternionWide.TransformUnitXY(orientationA, out var xa, out var da);
QuaternionWide.TransformUnitY(orientationB, out var db);
Vector3Wide.Dot(da, offsetB, out var daOffsetB);
Vector3Wide.Dot(db, offsetB, out var dbOffsetB);
Vector3Wide.Dot(da, db, out var dadb);
//Note potential division by zero when the axes are parallel. Arbitrarily clamp; near zero values will instead produce extreme values which get clamped to reasonable results.
var ta = (daOffsetB - dbOffsetB * dadb) / Vector.Max(new Vector <float>(1e-15f), Vector <float> .One - dadb * dadb);
//tb = ta * (da * db) - db * (b - a)
var tb = ta * dadb - dbOffsetB;
//We cannot simply clamp the ta and tb values to the capsule line segments. Instead, project each line segment onto the other line segment, clamping against the target's interval.
//That new clamped projected interval is the valid solution space on that line segment. We can clamp the t value by that interval to get the correctly bounded solution.
//The projected intervals are:
//B onto A: +-BHalfLength * (da * db) + da * offsetB
//A onto B: +-AHalfLength * (da * db) - db * offsetB
var absdadb = Vector.Abs(dadb);
var bOntoAOffset = b.HalfLength * absdadb;
var aOntoBOffset = a.HalfLength * absdadb;
var aMin = Vector.Max(-a.HalfLength, Vector.Min(a.HalfLength, daOffsetB - bOntoAOffset));
var aMax = Vector.Min(a.HalfLength, Vector.Max(-a.HalfLength, daOffsetB + bOntoAOffset));
var bMin = Vector.Max(-b.HalfLength, Vector.Min(b.HalfLength, -aOntoBOffset - dbOffsetB));
var bMax = Vector.Min(b.HalfLength, Vector.Max(-b.HalfLength, aOntoBOffset - dbOffsetB));
ta = Vector.Min(Vector.Max(ta, aMin), aMax);
tb = Vector.Min(Vector.Max(tb, bMin), bMax);
Vector3Wide.Scale(da, ta, out var closestPointOnA);
Vector3Wide.Scale(db, tb, out var closestPointOnB);
Vector3Wide.Add(closestPointOnB, offsetB, out closestPointOnB);
//Note that normals are calibrated to point from B to A by convention.
Vector3Wide.Subtract(closestPointOnA, closestPointOnB, out manifold.Normal);
Vector3Wide.Length(manifold.Normal, out var distance);
var inverseDistance = Vector <float> .One / distance;
Vector3Wide.Scale(manifold.Normal, inverseDistance, out manifold.Normal);
//In the event that the line segments are touching, the normal doesn't exist and we need an alternative. Any direction along the local horizontal (XZ) plane of either capsule
//is valid. (Normals along the local Y axes are not guaranteed to be as quick of a path to separation due to nonzero line length.)
var normalIsValid = Vector.GreaterThan(distance, new Vector <float>(1e-7f));
Vector3Wide.ConditionalSelect(normalIsValid, manifold.Normal, xa, out manifold.Normal);
//In the event that the two capsule axes are coplanar, we accept the whole interval as a source of contact.
//As the axes drift away from coplanarity, the accepted interval rapidly narrows to zero length, centered on ta and tb.
//We rate the degree of coplanarity based on the angle between the capsule axis and the plane defined by the opposing segment and contact normal:
//sin(angle) = dot(da, (db x normal)/||db x normal||)
//Finally, note that we are dealing with extremely small angles, and for small angles sin(angle) ~= angle,
//and also that fade behavior is completely arbitrary, so we can directly use squared angle without any concern.
//angle^2 ~= dot(da, (db x normal))^2 / ||db x normal||^2
//Note that if ||db x normal|| is zero, then any da should be accepted as being coplanar because there is no restriction. ConditionalSelect away the discontinuity.
Vector3Wide.CrossWithoutOverlap(db, manifold.Normal, out var planeNormal);
Vector3Wide.LengthSquared(planeNormal, out var planeNormalLengthSquared);
Vector3Wide.Dot(da, planeNormal, out var numeratorUnsquared);
var squaredAngle = Vector.ConditionalSelect(Vector.LessThan(planeNormalLengthSquared, new Vector <float>(1e-10f)), Vector <float> .Zero, numeratorUnsquared * numeratorUnsquared / planeNormalLengthSquared);
//Convert the squared angle to a lerp parameter. For squared angle from 0 to lowerThreshold, we should use the full interval (1). From lowerThreshold to upperThreshold, lerp to 0.
const float lowerThresholdAngle = 0.01f;
const float upperThresholdAngle = 0.05f;
const float lowerThreshold = lowerThresholdAngle * lowerThresholdAngle;
const float upperThreshold = upperThresholdAngle * upperThresholdAngle;
var intervalWeight = Vector.Max(Vector <float> .Zero, Vector.Min(Vector <float> .One, (new Vector <float>(upperThreshold) - squaredAngle) * new Vector <float>(1f / (upperThreshold - lowerThreshold))));
//If the line segments intersect, even if they're coplanar, we would ideally stick to using a single point. Would be easy enough,
//but we don't bother because it's such a weird and extremely temporary corner case. Not really worth handling.
var weightedTa = ta - ta * intervalWeight;
aMin = intervalWeight * aMin + weightedTa;
aMax = intervalWeight * aMax + weightedTa;
Vector3Wide.Scale(da, aMin, out manifold.OffsetA0);
Vector3Wide.Scale(da, aMax, out manifold.OffsetA1);
//In the coplanar case, there are two points. We need a method of computing depth which gives a reasonable result to the second contact.
//Note that one of the two contacts should end up with a distance equal to the previously computed segment distance, so we're doing some redundant work here.
//It's just easier to do that extra work than it would be to track which endpoint contributed the lower distance.
//Unproject the final interval endpoints from a back onto b.
//dot(offsetB + db * tb0, da) = ta0
//tb0 = (ta0 - daOffsetB) / dadb
//distance0 = dot(a0 - (offsetB + tb0 * db), normal)
//distance1 = dot(a1 - (offsetB + tb1 * db), normal)
Vector3Wide.Dot(db, manifold.Normal, out var dbNormal);
Vector3Wide.Subtract(manifold.OffsetA0, offsetB, out var offsetB0);
Vector3Wide.Subtract(manifold.OffsetA1, offsetB, out var offsetB1);
//Note potential division by zero. In that case, treat both projected points as the closest point. (Handled by the conditional select that chooses the previously computed distance.)
var inverseDadb = Vector <float> .One / dadb;
var projectedTb0 = Vector.Max(bMin, Vector.Min(bMax, (aMin - daOffsetB) * inverseDadb));
var projectedTb1 = Vector.Max(bMin, Vector.Min(bMax, (aMax - daOffsetB) * inverseDadb));
Vector3Wide.Dot(offsetB0, manifold.Normal, out var b0Normal);
Vector3Wide.Dot(offsetB1, manifold.Normal, out var b1Normal);
var capsulesArePerpendicular = Vector.LessThan(Vector.Abs(dadb), new Vector <float>(1e-7f));
var distance0 = Vector.ConditionalSelect(capsulesArePerpendicular, distance, b0Normal - dbNormal * projectedTb0);
var distance1 = Vector.ConditionalSelect(capsulesArePerpendicular, distance, b1Normal - dbNormal * projectedTb1);
var combinedRadius = a.Radius + b.Radius;
manifold.Depth0 = combinedRadius - distance0;
manifold.Depth1 = combinedRadius - distance1;
//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);
manifold.FeatureId0 = Vector <int> .Zero;
manifold.FeatureId1 = Vector <int> .One;
var minimumAcceptedDepth = -speculativeMargin;
manifold.Contact0Exists = Vector.GreaterThanOrEqual(manifold.Depth0, minimumAcceptedDepth);
manifold.Contact1Exists = Vector.BitwiseAnd(
Vector.GreaterThanOrEqual(manifold.Depth1, minimumAcceptedDepth),
Vector.GreaterThan(aMax - aMin, new Vector <float>(1e-7f) * a.HalfLength));
//TODO: Since we added in the complexity of 2 contact support, this is probably large enough to benefit from working in the local space of one of the capsules.
//Worth looking into later.
}
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);
}