internal static void Prepare(
ref CapsuleWide a, ref BoxWide b, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB,
out Vector3Wide localOffsetA, out Vector3Wide capsuleAxis, out Vector3Wide edgeCenters)
{
QuaternionWide.Conjugate(orientationB, out var toLocalB);
QuaternionWide.TransformWithoutOverlap(offsetB, toLocalB, out localOffsetA);
Vector3Wide.Negate(ref localOffsetA);
QuaternionWide.ConcatenateWithoutOverlap(orientationA, toLocalB, out var boxLocalOrientationA);
QuaternionWide.TransformUnitY(boxLocalOrientationA, out capsuleAxis);
//Get the closest point on the capsule segment to the box center to choose which edge to use.
//(Pointless to test the other 9; they're guaranteed to be further away.)
//closestPointOnCapsuleToBoxPosition = clamp((boxPosition - capsulePosition) * capsuleAxis, halfLength) * capsuleAxis + capsulePosition
//offsetFromBoxToCapsule = closestPointOnCapsuleToBoxPosition - boxPosition
//offsetFromBoxToCapsule = clamp(-localOffsetA * capsuleAxis, halfLength) * capsuleAxis + localOffsetA
//offsetFromBoxToCapsule = localOffsetA - clamp(localOffsetA * capsuleAxis, halfLength) * capsuleAxis
Vector3Wide.Dot(localOffsetA, capsuleAxis, out var dot);
var clampedDot = Vector.Min(a.HalfLength, Vector.Max(-a.HalfLength, dot));
Vector3Wide.Scale(capsuleAxis, clampedDot, out var offsetToCapsuleFromBox);
Vector3Wide.Subtract(localOffsetA, offsetToCapsuleFromBox, out offsetToCapsuleFromBox);
edgeCenters.X = Vector.ConditionalSelect(Vector.LessThan(offsetToCapsuleFromBox.X, Vector <float> .Zero), -b.HalfWidth, b.HalfWidth);
edgeCenters.Y = Vector.ConditionalSelect(Vector.LessThan(offsetToCapsuleFromBox.Y, Vector <float> .Zero), -b.HalfHeight, b.HalfHeight);
edgeCenters.Z = Vector.ConditionalSelect(Vector.LessThan(offsetToCapsuleFromBox.Z, Vector <float> .Zero), -b.HalfLength, b.HalfLength);
}
public unsafe void Test(ref ConvexHullWide 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 rA);
Matrix3x3Wide.CreateFromQuaternion(orientationB, out var rB);
Matrix3x3Wide.MultiplyByTransposeWithoutOverlap(rA, rB, out var bLocalOrientationA);
Matrix3x3Wide.TransformByTransposedWithoutOverlap(offsetB, rB, 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);
ManifoldCandidateHelper.CreateInactiveMask(pairCount, out var inactiveLanes);
a.EstimateEpsilonScale(inactiveLanes, out var aEpsilonScale);
b.EstimateEpsilonScale(inactiveLanes, out var bEpsilonScale);
var epsilonScale = Vector.Min(aEpsilonScale, bEpsilonScale);
var depthThreshold = -speculativeMargin;
DepthRefiner <ConvexHull, ConvexHullWide, ConvexHullSupportFinder, ConvexHull, ConvexHullWide, ConvexHullSupportFinder> .FindMinimumDepth(
b, a, localOffsetA, bLocalOrientationA, ref hullSupportFinder, ref hullSupportFinder, initialNormal, inactiveLanes, 1e-5f *epsilonScale, depthThreshold,
out var depth, out var localNormal, out var closestOnB);
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;
}
Matrix3x3Wide.TransformByTransposedWithoutOverlap(localNormal, bLocalOrientationA, out var localNormalInA);
Vector3Wide.Negate(localNormalInA, out var negatedLocalNormalInA);
Vector3Wide.Scale(localNormal, depth, out var negatedOffsetToClosestOnA);
Vector3Wide.Subtract(closestOnB, negatedOffsetToClosestOnA, out var closestOnA);
Vector3Wide.Subtract(closestOnA, localOffsetA, out var aToClosestOnA);
Matrix3x3Wide.TransformByTransposedWithoutOverlap(aToClosestOnA, bLocalOrientationA, out var closestOnAInA);
//To find the contact manifold, we'll clip the capsule axis against the 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;
for (int slotIndex = 0; slotIndex < pairCount; ++slotIndex)
{
if (inactiveLanes[slotIndex] < 0)
{
continue;
}
ref var aSlot = ref a.Hulls[slotIndex];
ref var bSlot = ref b.Hulls[slotIndex];
public void Test(Random random, int innerIterations)
{
RotationBetweenNormalsTestSIMD.GenerateRandomNormalizedVector(random, out var v1);
Vector3Wide.Negate(v1, out var v2);
for (int i = 0; i < innerIterations; ++i)
{
QuaternionWide.GetQuaternionBetweenNormalizedVectors(v1, v2, out var v1ToV2);
QuaternionWide.GetQuaternionBetweenNormalizedVectors(v2, v1, out var v2ToV1);
#if DEBUG
QuaternionWide.ConcatenateWithoutOverlap(v1ToV2, v2ToV1, out var concatenated);
QuaternionWide.TransformWithoutOverlap(v1, v1ToV2, out var v1TransformedToV2);
QuaternionWide.TransformWithoutOverlap(v2, v2ToV1, out var v2TransformedToV1);
QuaternionWide.TransformWithoutOverlap(v1, concatenated, out var v1TransformedToV1);
Vector3Wide.Subtract(v1TransformedToV2, v2, out var v1ToV2Error);
Vector3Wide.LengthSquared(v1ToV2Error, out var v1ToV2ErrorLength);
Vector3Wide.Subtract(v2TransformedToV1, v1, out var v2ToV1Error);
Vector3Wide.LengthSquared(v2ToV1Error, out var v2ToV1ErrorLength);
Vector3Wide.Subtract(v1TransformedToV1, v1, out var v1ToV1Error);
Vector3Wide.LengthSquared(v1ToV1Error, out var v1ToV1ErrorLength);
const float epsilon = 1e-6f;
Debug.Assert(
Vector.LessThanAll(v1ToV2ErrorLength, new Vector <float>(epsilon)) &&
Vector.LessThanAll(v2ToV1ErrorLength, new Vector <float>(epsilon)) &&
Vector.LessThanAll(v1ToV1ErrorLength, new Vector <float>(epsilon)));
#endif
}
}
public void Test(ref SphereWide a, ref ConvexHullWide b, ref Vector <float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex1ContactManifoldWide manifold)
{
Matrix3x3Wide.CreateFromQuaternion(orientationB, out var hullOrientation);
Matrix3x3Wide.TransformByTransposedWithoutOverlap(offsetB, hullOrientation, out var localOffsetB);
Vector3Wide.Negate(localOffsetB, out var localOffsetA);
Matrix3x3Wide.CreateIdentity(out var identity);
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 sphereSupportFinder = default(SphereSupportFinder);
ManifoldCandidateHelper.CreateInactiveMask(pairCount, out var inactiveLanes);
b.EstimateEpsilonScale(inactiveLanes, out var hullEpsilonScale);
var epsilonScale = Vector.Min(a.Radius, hullEpsilonScale);
DepthRefiner <ConvexHull, ConvexHullWide, ConvexHullSupportFinder, Sphere, SphereWide, SphereSupportFinder> .FindMinimumDepth(
b, a, localOffsetA, identity, ref hullSupportFinder, ref sphereSupportFinder, initialNormal, inactiveLanes, 1e-5f *epsilonScale, -speculativeMargin,
out var depth, out var localNormal, out var closestOnHull);
Matrix3x3Wide.TransformWithoutOverlap(closestOnHull, hullOrientation, out var hullToContact);
Matrix3x3Wide.TransformWithoutOverlap(localNormal, hullOrientation, out manifold.Normal);
Vector3Wide.Add(hullToContact, offsetB, out manifold.OffsetA);
manifold.FeatureId = Vector <int> .Zero;
manifold.Depth = depth;
manifold.ContactExists = Vector.GreaterThanOrEqual(manifold.Depth, -speculativeMargin);
}
public unsafe void FindLocalOverlaps(ref Buffer <BoundsTestedPair> pairs, int pairCount, BufferPool pool, Shapes shapes, float dt, out ConvexCompoundTaskOverlaps overlaps)
{
overlaps = new ConvexCompoundTaskOverlaps(pool, pairCount);
var pairsToTest = stackalloc PairsToTestForOverlap[pairCount];
Vector3Wide offsetB = default;
QuaternionWide orientationA = default;
QuaternionWide orientationB = default;
Vector3Wide relativeLinearVelocityA = default;
Vector3Wide angularVelocityA = default;
Vector3Wide angularVelocityB = default;
Vector <float> maximumAllowedExpansion = default;
TConvexWide convexWide = default;
for (int i = 0; i < pairCount; i += Vector <float> .Count)
{
var count = pairCount - i;
if (count > Vector <float> .Count)
{
count = Vector <float> .Count;
}
//Compute the local bounding boxes using wide operations for the expansion work.
//Doing quite a bit of gather work (and still quite a bit of scalar work). Very possible that a scalar path could win. TODO: test that.
for (int j = 0; j < count; ++j)
{
var pairIndex = i + j;
ref var pair = ref pairs[pairIndex];
pairsToTest[pairIndex].Container = pair.B;
Vector3Wide.WriteFirst(pair.OffsetB, ref GatherScatter.GetOffsetInstance(ref offsetB, j));
QuaternionWide.WriteFirst(pair.OrientationA, ref GatherScatter.GetOffsetInstance(ref orientationA, j));
QuaternionWide.WriteFirst(pair.OrientationB, ref GatherScatter.GetOffsetInstance(ref orientationB, j));
Vector3Wide.WriteFirst(pair.RelativeLinearVelocityA, ref GatherScatter.GetOffsetInstance(ref relativeLinearVelocityA, j));
Vector3Wide.WriteFirst(pair.AngularVelocityA, ref GatherScatter.GetOffsetInstance(ref angularVelocityA, j));
Vector3Wide.WriteFirst(pair.AngularVelocityB, ref GatherScatter.GetOffsetInstance(ref angularVelocityB, j));
Unsafe.Add(ref Unsafe.As <Vector <float>, float>(ref maximumAllowedExpansion), j) = pair.MaximumExpansion;
GatherScatter.GetOffsetInstance(ref convexWide, j).WriteFirst(ref Unsafe.AsRef <TConvex>(pair.A));
}
QuaternionWide.Conjugate(orientationB, out var inverseOrientationB);
QuaternionWide.TransformWithoutOverlap(offsetB, inverseOrientationB, out var localOffsetB);
QuaternionWide.ConcatenateWithoutOverlap(orientationA, inverseOrientationB, out var localOrientationA);
QuaternionWide.TransformWithoutOverlap(relativeLinearVelocityA, inverseOrientationB, out var localRelativeLinearVelocityA);
convexWide.GetBounds(ref localOrientationA, out var maximumRadius, out var maximumAngularExpansion, out var min, out var max);
Vector3Wide.Negate(localOffsetB, out var localPositionA);
BoundingBoxHelpers.ExpandLocalBoundingBoxes(ref min, ref max, Vector <float> .Zero, localPositionA, localRelativeLinearVelocityA, angularVelocityA, angularVelocityB, dt,
maximumRadius, maximumAngularExpansion, maximumAllowedExpansion);
for (int j = 0; j < count; ++j)
{
ref var pairToTest = ref pairsToTest[i + j];
Vector3Wide.ReadSlot(ref min, j, out pairToTest.Min);
Vector3Wide.ReadSlot(ref max, j, out pairToTest.Max);
}
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);
}
void SampleMinkowskiDifference(
ref TShapeWideA a, ref Matrix3x3Wide rA, ref TSupportFinderA supportFinderA,
ref TShapeWideB b, ref Matrix3x3Wide rB, ref TSupportFinderB supportFinderB, ref Vector3Wide offsetB, ref Vector3Wide direction,
out Vector3Wide supportA, out Vector3Wide support)
{
supportFinderA.ComputeSupport(ref a, ref rA, ref direction, out supportA);
Vector3Wide.Negate(direction, out var negatedDirection);
supportFinderB.ComputeSupport(ref b, ref rB, ref negatedDirection, out var supportB);
Vector3Wide.Add(supportB, offsetB, out supportB);
Vector3Wide.Subtract(supportA, supportB, out support);
}
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 void Test(
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);
//Get the capsule-axis-perpendicular offset from the box to the capsule and use it to choose which edges to test.
//(Pointless to test the other 9; they're guaranteed to be further away.)
Vector3Wide.Dot(ref localOffsetA, ref capsuleAxis, out var axisOffsetADot);
Vector3Wide.Scale(ref capsuleAxis, ref axisOffsetADot, out var toRemove);
Vector3Wide.Subtract(ref localOffsetA, ref toRemove, out var perpendicularOffset);
Vector3Wide edgeCenters;
edgeCenters.X = Vector.ConditionalSelect(Vector.LessThan(perpendicularOffset.X, Vector <float> .Zero), -b.HalfWidth, b.HalfWidth);
edgeCenters.Y = Vector.ConditionalSelect(Vector.LessThan(perpendicularOffset.Y, Vector <float> .Zero), -b.HalfHeight, b.HalfHeight);
edgeCenters.Z = Vector.ConditionalSelect(Vector.LessThan(perpendicularOffset.Z, Vector <float> .Zero), -b.HalfLength, b.HalfLength);
//Swizzle XYZ -> YZX
Vector3Wide localNormal;
TestBoxEdge(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 taMin, out var taMax, out var depth, out localNormal.Y, out localNormal.Z, out localNormal.X);
//Swizzle XYZ -> ZXY
TestBoxEdge(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 eytaMin, out var eytaMax, out var eyDepth, out var eynZ, out var eynX, out var eynY);
Select(ref depth, ref taMin, ref taMax, ref localNormal.X, ref localNormal.Y, ref localNormal.Z,
ref eyDepth, ref eytaMin, ref eytaMax, ref eynX, ref eynY, ref eynZ);
//Swizzle XYZ -> XYZ
TestBoxEdge(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 eztaMin, out var eztaMax, out var ezDepth, out var eznX, out var eznY, out var eznZ);
Select(ref depth, ref taMin, ref taMax, ref localNormal.X, ref localNormal.Y, ref localNormal.Z,
ref ezDepth, ref eztaMin, ref eztaMax, ref eznX, ref eznY, ref eznZ);
//For each face, clip the capsule axis against the face bounds. The closest offset between the clipped capsule axis endpoints and the box is the normal candidate.
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;
//Clip slightly beyond the actual face limit to avoid pointlessly cutting the interval for near-edge-parallel capsules.
var epsilonScale = new Vector <float>(1f + 1e-6f);
scaledExtents.X = epsilonScale * b.HalfWidth * Vector.Abs(scaleX);
scaledExtents.Y = epsilonScale * b.HalfHeight * Vector.Abs(scaleY);
scaledExtents.Z = epsilonScale * 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.Min(ref a.HalfLength, ref tCandidateMin, out tCandidateMin);
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);
Vector3Wide.Max(ref negativeHalfLength, ref tCandidateMax, out tCandidateMax);
var zero = Vector <float> .Zero;
//Face X
TestBoxFace(ref localOffsetA.X,
ref capsuleAxis.X, ref a.HalfLength,
ref tCandidateMin.Y, ref tCandidateMax.Y, ref tCandidateMin.Z, ref tCandidateMax.Z,
ref b.HalfWidth,
out var fxDepth, out var fxtaMin, out var fxtaMax, out var fxn);
Select(ref depth, ref taMin, ref taMax, ref localNormal.X, ref localNormal.Y, ref localNormal.Z,
ref fxDepth, ref fxtaMin, ref fxtaMax, ref fxn, ref zero, ref zero);
//Face Y
TestBoxFace(ref localOffsetA.Y,
ref capsuleAxis.Y, ref a.HalfLength,
ref tCandidateMin.X, ref tCandidateMax.X, ref tCandidateMin.Z, ref tCandidateMax.Z,
ref b.HalfHeight,
out var fyDepth, out var fytaMin, out var fytaMax, out var fyn);
Select(ref depth, ref taMin, ref taMax, ref localNormal.X, ref localNormal.Y, ref localNormal.Z,
ref fyDepth, ref fytaMin, ref fytaMax, ref zero, ref fyn, ref zero);
//Face Z
TestBoxFace(ref localOffsetA.Z,
ref capsuleAxis.Z, ref a.HalfLength,
ref tCandidateMin.X, ref tCandidateMax.X, ref tCandidateMin.Y, ref tCandidateMax.Y,
ref b.HalfLength,
out var fzDepth, out var fztaMin, out var fztaMax, out var fzn);
Select(ref depth, ref taMin, ref taMax, ref localNormal.X, ref localNormal.Y, ref localNormal.Z,
ref fzDepth, ref fztaMin, ref fztaMax, ref zero, ref zero, ref fzn);
//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).
//(This is partially redundant with the per-case calculations, but simply redoing some cheap ALU work is easier than trying to keep track of per-contact depths across all cases.)
Vector3Wide.Scale(ref capsuleAxis, ref taMin, out var localA0);
Vector3Wide.Scale(ref capsuleAxis, ref taMax, out var localA1);
Vector3Wide.Add(ref localOffsetA, ref localA0, out var bToA0);
Vector3Wide.Add(ref localOffsetA, ref localA1, out var bToA1);
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 bToA0, out var dot0);
Vector3Wide.Dot(ref localNormal, ref bToA1, out var dot1);
manifold.Depth0 = a.Radius + boxExtreme - dot0;
manifold.Depth1 = a.Radius + boxExtreme - dot1;
manifold.FeatureId0 = Vector <int> .Zero;
manifold.FeatureId1 = Vector <int> .One;
//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);
manifold.Count = Vector.ConditionalSelect(Vector.LessThan(taMax - taMin, new Vector <float>(1e-7f) * a.HalfLength), Vector <int> .One, new Vector <int>(2));
}
