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 static void ApplyImpulse(ref Vector3Wide angularJacobianA, ref BodyInertias inertiaA,
ref Vector <float> correctiveImpulse, ref BodyVelocities wsvA)
{
Vector3Wide.Scale(angularJacobianA, correctiveImpulse, out var worldCorrectiveImpulseA);
Symmetric3x3Wide.TransformWithoutOverlap(worldCorrectiveImpulseA, inertiaA.InverseInertiaTensor, out var worldCorrectiveVelocityA);
Vector3Wide.Add(wsvA.Angular, worldCorrectiveVelocityA, out wsvA.Angular);
}
public static void Test(
ref Vector <float> radiiA, ref Vector <float> radiiB,
ref Vector3Wide relativePositionB,
out Vector3Wide relativeContactPosition, out Vector3Wide contactNormal, out Vector <float> depth)
{
Vector3Wide.Length(ref relativePositionB, out var centerDistance);
//Note the negative 1. By convention, the normal points from B to A.
var inverseDistance = new Vector <float>(-1f) / centerDistance;
Vector3Wide.Scale(ref relativePositionB, ref inverseDistance, out contactNormal);
var normalIsValid = Vector.GreaterThan(centerDistance, Vector <float> .Zero);
//Arbitrarily choose the (0,1,0) if the two spheres are in the same position. Any unit length vector is equally valid.
contactNormal.X = Vector.ConditionalSelect(normalIsValid, contactNormal.X, Vector <float> .Zero);
contactNormal.Y = Vector.ConditionalSelect(normalIsValid, contactNormal.Y, Vector <float> .One);
contactNormal.Z = Vector.ConditionalSelect(normalIsValid, contactNormal.Z, Vector <float> .Zero);
depth = radiiA + radiiB - centerDistance;
//The contact position relative to object A is computed as the average of the extreme point along the normal toward the opposing sphere on each sphere, averaged.
//In other words:
//contactPosition = (positionA + (-normal) * radiusA + positionB + normal * radiusB) / 2
//Note the use of the negative normal for the contribution from sphere A- the normal is calibrated to point from B to A.
//Now, because both positions are relative to positionA:
//relativeContactPosition = (-normal * radiusA + relativePositionB + normal * radiusB) / 2
//relativeContactPosition = (relativePositionB + (normal * radiusB - normal * radiusA)) / 2
//relativeContactPosition = (relativePositionB + normal * (radiusB - radiusA)) / 2
var radiusDifference = radiiB - radiiA;
Vector3Wide.Scale(ref contactNormal, ref radiusDifference, out var offsetFromB);
Vector3Wide.Add(ref relativePositionB, ref offsetFromB, out relativeContactPosition);
var scale = new Vector <float>(0.5f);
Vector3Wide.Scale(ref relativeContactPosition, ref scale, out relativeContactPosition);
}
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 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 void Prestep(Bodies bodies, ref TwoBodyReferences bodyReferences, int count,
float dt, float inverseDt, ref Contact4PrestepData prestep, out Contact4Projection projection)
{
//Some speculative compression options not (yet) pursued:
//1) Store the surface basis in a compressed fashion. It could be stored within 32 bits by using standard compression schemes, but we lack the necessary
//instructions to properly SIMDify the decode operation (e.g. shift). Even with the potential savings of 3 floats (relative to uncompressed storage), it would be questionable.
//We could drop one of the four components of the quaternion and reconstruct it relatively easily- that would just require that the encoder ensures the W component is positive.
//It would require a square root, but it might still be a net win. On an IVB, sqrt has a 7 cycle throughput. 4 bytes saved * 4 lanes = 16 bytes, which takes
//about 16 / 5.5GBps = 2.9ns, where 5.5 is roughly the per-core bandwidth on a 3770K. 7 cycles is only 2ns at 3.5ghz.
//There are a couple of other instructions necessary to decode, but sqrt is by far the heaviest; it's likely a net win.
//Be careful about the execution order here. It should be aligned with the prestep data layout to ensure prefetching works well.
bodies.GatherInertia(ref bodyReferences, count, out projection.InertiaA, out projection.InertiaB);
Vector3Wide.Add(ref prestep.OffsetA0, ref prestep.OffsetA1, out var a01);
Vector3Wide.Add(ref prestep.OffsetA2, ref prestep.OffsetA3, out var a23);
Vector3Wide.Add(ref a01, ref a23, out var offsetToManifoldCenterA);
var scale = new Vector <float>(0.25f);
Vector3Wide.Scale(ref offsetToManifoldCenterA, ref scale, out offsetToManifoldCenterA);
Vector3Wide.Subtract(ref offsetToManifoldCenterA, ref prestep.OffsetB, out var offsetToManifoldCenterB);
projection.PremultipliedFrictionCoefficient = scale * prestep.FrictionCoefficient;
projection.Normal = prestep.Normal;
Helpers.BuildOrthnormalBasis(ref prestep.Normal, out var x, out var z);
TangentFriction.Prestep(ref x, ref z, ref offsetToManifoldCenterA, ref offsetToManifoldCenterB, ref projection.InertiaA, ref projection.InertiaB, out projection.Tangent);
PenetrationLimit4.Prestep(ref projection.InertiaA, ref projection.InertiaB, ref prestep.Normal, ref prestep, dt, inverseDt, out projection.Penetration);
//Just assume the lever arms for B are the same. It's a good guess. (The only reason we computed the offset B is because we didn't want to go into world space.)
Vector3Wide.Distance(ref prestep.OffsetA0, ref offsetToManifoldCenterA, out projection.LeverArm0);
Vector3Wide.Distance(ref prestep.OffsetA1, ref offsetToManifoldCenterA, out projection.LeverArm1);
Vector3Wide.Distance(ref prestep.OffsetA2, ref offsetToManifoldCenterA, out projection.LeverArm2);
Vector3Wide.Distance(ref prestep.OffsetA3, ref offsetToManifoldCenterA, out projection.LeverArm3);
TwistFriction.Prestep(ref projection.InertiaA, ref projection.InertiaB, ref prestep.Normal, out projection.Twist);
}
public void Test(ref SphereWide 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)
{
//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 localNormal);
Vector3Wide.Length(localNormal, out var innerDistance);
var inverseDistance = Vector <float> .One / innerDistance;
Vector3Wide.Scale(localNormal, inverseDistance, out localNormal);
Matrix3x3Wide.TransformWithoutOverlap(localNormal, orientationMatrixB, out normal);
var negativeRadius = -a.Radius;
Vector3Wide.Scale(normal, negativeRadius, out closestA);
distance = innerDistance - a.Radius;
intersected = Vector.LessThanOrEqual(distance, Vector <float> .Zero);
}
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);
}
static void TestEndpointNormal(ref Vector3Wide offsetA, ref Vector3Wide capsuleAxis, ref Vector <float> capsuleHalfLength, ref Vector3Wide endpoint,
ref BoxWide box, out Vector <float> depth, out Vector3Wide normal)
{
Vector3Wide clamped;
clamped.X = Vector.Min(box.HalfWidth, Vector.Max(-box.HalfWidth, endpoint.X));
clamped.Y = Vector.Min(box.HalfHeight, Vector.Max(-box.HalfHeight, endpoint.Y));
clamped.Z = Vector.Min(box.HalfLength, Vector.Max(-box.HalfLength, endpoint.Z));
Vector3Wide.Subtract(endpoint, clamped, out normal);
Vector3Wide.Length(normal, out var length);
var inverseLength = Vector <float> .One / length;
Vector3Wide.Scale(normal, inverseLength, out normal);
//The dot between the offset from B to A and the normal gives us the center offset.
//The dot between the capsule axis and normal gives us the (unscaled) extent of the capsule along the normal.
//The depth is (boxExtentAlongNormal + capsuleExtentAlongNormal) - separationAlongNormal.
Vector3Wide.Dot(offsetA, normal, out var baN);
Vector3Wide.Dot(capsuleAxis, normal, out var daN);
depth =
Vector.Abs(normal.X) * box.HalfWidth + Vector.Abs(normal.Y) * box.HalfHeight + Vector.Abs(normal.Z) * box.HalfLength +
Vector.Abs(daN * capsuleHalfLength) -
Vector.Abs(baN);
//If the endpoint doesn't generate a valid normal due to containment, ignore the depth result.
depth = Vector.ConditionalSelect(Vector.GreaterThan(length, new Vector <float>(1e-10f)), depth, new Vector <float>(float.MaxValue));
}
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);
}
public static void ApplyImpulse(ref Jacobians jacobians, ref BodyInertias inertiaA,
ref Vector2Wide correctiveImpulse, ref BodyVelocities wsvA)
{
Matrix2x3Wide.Transform(correctiveImpulse, jacobians.LinearA, out var linearImpulseA);
Matrix2x3Wide.Transform(correctiveImpulse, jacobians.AngularA, out var angularImpulseA);
BodyVelocities correctiveVelocityA;
Vector3Wide.Scale(linearImpulseA, inertiaA.InverseMass, out correctiveVelocityA.Linear);
Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseA, inertiaA.InverseInertiaTensor, out correctiveVelocityA.Angular);
Vector3Wide.Add(wsvA.Linear, correctiveVelocityA.Linear, out wsvA.Linear);
Vector3Wide.Add(wsvA.Angular, correctiveVelocityA.Angular, out wsvA.Angular);
}
static void GetEdgeClosestPoint(ref Vector3Wide normal, ref Vector <int> edgeDirectionIndex,
ref BoxWide box,
ref Vector3Wide offsetA, ref Vector3Wide capsuleAxis, ref Vector <float> capsuleHalfLength, out Vector3Wide closestPointFromEdge)
{
Vector3Wide.Dot(normal, offsetA, out var calibrationDot);
var flipNormal = Vector.LessThan(calibrationDot, Vector <float> .Zero);
normal.X = Vector.ConditionalSelect(flipNormal, -normal.X, normal.X);
normal.Y = Vector.ConditionalSelect(flipNormal, -normal.Y, normal.Y);
normal.Z = Vector.ConditionalSelect(flipNormal, -normal.Z, normal.Z);
Vector3Wide boxEdgeCenter;
//Note that this can result in a closest point in the middle of the box when the capsule is parallel to a face. That's fine.
boxEdgeCenter.X = Vector.ConditionalSelect(Vector.Equals(normal.X, Vector <float> .Zero), Vector <float> .Zero,
Vector.ConditionalSelect(Vector.GreaterThan(normal.X, Vector <float> .Zero), box.HalfWidth, -box.HalfWidth));
boxEdgeCenter.Y = Vector.ConditionalSelect(Vector.Equals(normal.Y, Vector <float> .Zero), Vector <float> .Zero,
Vector.ConditionalSelect(Vector.GreaterThan(normal.Y, Vector <float> .Zero), box.HalfHeight, -box.HalfHeight));
boxEdgeCenter.Z = Vector.ConditionalSelect(Vector.Equals(normal.Z, Vector <float> .Zero), Vector <float> .Zero,
Vector.ConditionalSelect(Vector.GreaterThan(normal.Z, Vector <float> .Zero), box.HalfLength, -box.HalfLength));
Vector3Wide boxEdgeDirection;
var useEdgeX = Vector.Equals(edgeDirectionIndex, Vector <int> .Zero);
var useEdgeY = Vector.Equals(edgeDirectionIndex, Vector <int> .One);
var useEdgeZ = Vector.Equals(edgeDirectionIndex, new Vector <int>(2));
boxEdgeDirection.X = Vector.ConditionalSelect(useEdgeX, Vector <float> .One, Vector <float> .Zero);
boxEdgeDirection.Y = Vector.ConditionalSelect(useEdgeY, Vector <float> .One, Vector <float> .Zero);
boxEdgeDirection.Z = Vector.ConditionalSelect(useEdgeZ, Vector <float> .One, Vector <float> .Zero);
//From CapsulePairCollisionTask, point of closest approach along the capsule axis, unbounded:
//ta = (da * (b - a) + (db * (a - b)) * (da * db)) / (1 - ((da * db) * (da * db))
//where da = capsuleAxis, db = boxEdgeDirection, a = offsetA, b = boxEdgeCenter
Vector3Wide.Subtract(boxEdgeCenter, offsetA, out var ab);
Vector3Wide.Dot(ab, capsuleAxis, out var abda);
Vector3Wide.Dot(ab, boxEdgeDirection, out var abdb);
Vector3Wide.Dot(capsuleAxis, boxEdgeDirection, out var dadb);
//Note division by zero guard.
var ta = (abda - abdb * dadb) / Vector.Max(new Vector <float>(1e-15f), (Vector <float> .One - dadb * dadb));
//In some cases, ta won't be in a useful location. Need to constrain it to the projection of the box edge onto the capsule edge.
//B onto A: +-BHalfExtent * (da * db) + da * ab
var bHalfExtent = Vector.ConditionalSelect(useEdgeX, box.HalfWidth, Vector.ConditionalSelect(useEdgeY, box.HalfHeight, box.HalfLength));
var bOntoAOffset = bHalfExtent * Vector.Abs(dadb);
var taMin = Vector.Max(-capsuleHalfLength, Vector.Min(capsuleHalfLength, abda - bOntoAOffset));
var taMax = Vector.Min(capsuleHalfLength, Vector.Max(-capsuleHalfLength, abda + bOntoAOffset));
ta = Vector.Min(Vector.Max(ta, taMin), taMax);
Vector3Wide.Scale(capsuleAxis, ta, out var offsetAlongCapsule);
Vector3Wide.Add(offsetA, offsetAlongCapsule, out closestPointFromEdge);
}
public static void ApplyImpulse(ref PenetrationLimitOneBodyProjection projection, ref BodyInertias inertiaA, ref Vector3Wide normal,
ref Vector <float> correctiveImpulse,
ref BodyVelocities wsvA)
{
var linearVelocityChangeA = correctiveImpulse * inertiaA.InverseMass;
Vector3Wide.Scale(normal, linearVelocityChangeA, out var correctiveVelocityALinearVelocity);
Vector3Wide.Scale(projection.AngularA, correctiveImpulse, out var correctiveAngularImpulseA);
Symmetric3x3Wide.TransformWithoutOverlap(correctiveAngularImpulseA, inertiaA.InverseInertiaTensor, out var correctiveVelocityAAngularVelocity);
Vector3Wide.Add(wsvA.Linear, correctiveVelocityALinearVelocity, out wsvA.Linear);
Vector3Wide.Add(wsvA.Angular, correctiveVelocityAAngularVelocity, out wsvA.Angular);
}
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 Test(ref SphereWide a, ref SphereWide b, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB,
out Vector <int> intersected, out Vector <float> distance, out Vector3Wide closestA, out Vector3Wide normal)
{
Vector3Wide.Length(offsetB, out var centerDistance);
//Note the negative 1. By convention, the normal points from B to A.
var inverseDistance = new Vector <float>(-1f) / centerDistance;
Vector3Wide.Scale(offsetB, inverseDistance, out normal);
distance = centerDistance - a.Radius - b.Radius;
var negativeRadiusA = -a.Radius;
Vector3Wide.Scale(normal, negativeRadiusA, out closestA);
intersected = Vector.LessThanOrEqual(distance, Vector <float> .Zero);
}
static void TestVertexAxis(ref BoxWide box, ref Vector3Wide offsetA, ref Vector3Wide capsuleAxis, ref Vector <float> capsuleHalfLength,
out Vector <float> depth, out Vector3Wide normal, out Vector3Wide closestA)
{
//The available feature pairs between the capsule axis and box are:
//Box edge - capsule endpoints: handled by capsule endpoint clamping
//Box edge - capsule axis: handled explicitly by the edge cases
//Box face - capsule endpoints: handled by capsule endpoint clamping
//Box face - capsule axis: redundant with edge-axis or face-endpoint
//Box vertex - capsule endpoints: handled by capsule endpoint clamping
//Box vertex - capsule axis: unhandled
//So here, we need to identify the maximum separating axis caused by vertices.
//We can safely ignore cases that are handled by the endpoint clamp, too.
//A brute force approach could test every vertex-axis offset as a normal, but more cleverness is possible.
//Note that this case requires that the capsule axis be in the voronoi region of a vertex. That is only possible if the offset from the box origin to the capsule axis
//supports an extreme point at the vertex.
//(That extreme point relationship may also be met in cases of intersection, but that's fine- this distance tester is not concerned with intersection beyond a boolean result.)
//closest point on axis to origin = offsetA - (offsetA * capsuleAxis) * capsuleAxis
Vector3Wide.Dot(offsetA, capsuleAxis, out var dot);
var clampedDot = Vector.Min(capsuleHalfLength, Vector.Max(-capsuleHalfLength, dot));
Vector3Wide.Scale(capsuleAxis, clampedDot, out var axisOffset);
Vector3Wide.Subtract(offsetA, axisOffset, out var closestOnAxis);
Vector3Wide vertex;
vertex.X = Vector.ConditionalSelect(Vector.LessThan(closestOnAxis.X, Vector <float> .Zero), -box.HalfWidth, box.HalfWidth);
vertex.Y = Vector.ConditionalSelect(Vector.LessThan(closestOnAxis.Y, Vector <float> .Zero), -box.HalfHeight, box.HalfHeight);
vertex.Z = Vector.ConditionalSelect(Vector.LessThan(closestOnAxis.Z, Vector <float> .Zero), -box.HalfLength, box.HalfLength);
//closest point on axis to vertex: ((vertex - offsetA) * capsuleAxis) * capsuleAxis + offsetA - vertex
Vector3Wide.Subtract(vertex, offsetA, out var capsuleCenterToVertex);
Vector3Wide.Dot(capsuleCenterToVertex, capsuleAxis, out var vertexDot);
Vector3Wide.Scale(capsuleAxis, vertexDot, out var vertexAxisOffset);
Vector3Wide.Add(vertexAxisOffset, offsetA, out closestA);
Vector3Wide.Subtract(closestA, vertex, out var vertexToClosestOnCapsule);
Vector3Wide.Length(vertexToClosestOnCapsule, out var length);
var inverseLength = Vector <float> .One / length;
Vector3Wide.Scale(vertexToClosestOnCapsule, inverseLength, out normal);
//The normal is perpendicular to the capsule axis by construction, so no need to include the capsule length extent.
depth = Vector.Abs(normal.X) * box.HalfWidth + Vector.Abs(normal.Y) * box.HalfHeight + Vector.Abs(normal.Z) * box.HalfLength -
Vector.Abs(offsetA.X * normal.X + offsetA.Y * normal.Y + offsetA.Z * normal.Z);
//Ignore degenerate cases. Worst outcome is that it reports intersection, which is pretty reasonable.
depth = Vector.ConditionalSelect(Vector.LessThan(length, new Vector <float>(1e-10f)), new Vector <float>(float.MaxValue), depth);
}
public void Test(ref CapsuleWide a, ref CapsuleWide b, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB,
out Vector <int> intersected, out Vector <float> distance, out Vector3Wide closestA, out Vector3Wide normal)
{
//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 closestA);
Vector3Wide.Scale(db, tb, out var closestB);
Vector3Wide.Add(closestB, offsetB, out closestB);
Vector3Wide.Subtract(closestA, closestB, out normal);
Vector3Wide.Length(normal, out distance);
var inverseDistance = Vector <float> .One / distance;
Vector3Wide.Scale(normal, inverseDistance, out normal);
Vector3Wide.Scale(normal, a.Radius, out var aOffset);
Vector3Wide.Subtract(closestA, aOffset, out closestA);
distance = distance - a.Radius - b.Radius;
intersected = Vector.LessThanOrEqual(distance, Vector <float> .Zero);
}
public static void ApplyImpulse(ref Jacobians jacobians, ref BodyInertias inertiaA, ref BodyInertias inertiaB,
ref Vector2Wide correctiveImpulse, ref BodyVelocities wsvA, ref BodyVelocities wsvB)
{
Matrix2x3Wide.Transform(correctiveImpulse, jacobians.LinearA, out var linearImpulseA);
Matrix2x3Wide.Transform(correctiveImpulse, jacobians.AngularA, out var angularImpulseA);
Matrix2x3Wide.Transform(correctiveImpulse, jacobians.AngularB, out var angularImpulseB);
BodyVelocities correctiveVelocityA, correctiveVelocityB;
Vector3Wide.Scale(linearImpulseA, inertiaA.InverseMass, out correctiveVelocityA.Linear);
Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseA, inertiaA.InverseInertiaTensor, out correctiveVelocityA.Angular);
Vector3Wide.Scale(linearImpulseA, inertiaB.InverseMass, out correctiveVelocityB.Linear);
Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseB, inertiaB.InverseInertiaTensor, out correctiveVelocityB.Angular);
Vector3Wide.Add(wsvA.Linear, correctiveVelocityA.Linear, out wsvA.Linear);
Vector3Wide.Add(wsvA.Angular, correctiveVelocityA.Angular, out wsvA.Angular);
Vector3Wide.Subtract(wsvB.Linear, correctiveVelocityB.Linear, out wsvB.Linear); //note subtract- we based it on the LinearA jacobian.
Vector3Wide.Add(wsvB.Angular, correctiveVelocityB.Angular, out wsvB.Angular);
}
public static void ComputeCorrectiveImpulse(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref Vector3Wide offsetA, ref Vector3Wide offsetB,
ref Vector3Wide biasVelocity, ref Symmetric3x3Wide effectiveMass, ref Vector <float> softnessImpulseScale, ref Vector3Wide accumulatedImpulse, out Vector3Wide correctiveImpulse)
{
//csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular);
//Note subtraction; jLinearB = -I.
Vector3Wide.Subtract(velocityA.Linear, velocityB.Linear, out var csv);
Vector3Wide.CrossWithoutOverlap(velocityA.Angular, offsetA, out var angularCSV);
Vector3Wide.Add(csv, angularCSV, out csv);
//Note reversed cross order; matches the jacobian -CrossMatrix(offsetB).
Vector3Wide.CrossWithoutOverlap(offsetB, velocityB.Angular, out angularCSV);
Vector3Wide.Add(csv, angularCSV, out csv);
Vector3Wide.Subtract(biasVelocity, csv, out csv);
Symmetric3x3Wide.TransformWithoutOverlap(csv, effectiveMass, out correctiveImpulse);
Vector3Wide.Scale(accumulatedImpulse, softnessImpulseScale, out var softness);
Vector3Wide.Subtract(correctiveImpulse, softness, out correctiveImpulse);
}
public static void ApplyImpulse(ref BodyVelocities velocityA, ref BodyVelocities velocityB,
ref Vector3Wide offsetA, ref Vector3Wide offsetB, ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref Vector3Wide constraintSpaceImpulse)
{
Vector3Wide.CrossWithoutOverlap(offsetA, constraintSpaceImpulse, out var wsi);
Symmetric3x3Wide.TransformWithoutOverlap(wsi, inertiaA.InverseInertiaTensor, out var change);
Vector3Wide.Add(velocityA.Angular, change, out velocityA.Angular);
Vector3Wide.Scale(constraintSpaceImpulse, inertiaA.InverseMass, out change);
Vector3Wide.Add(velocityA.Linear, change, out velocityA.Linear);
Vector3Wide.CrossWithoutOverlap(constraintSpaceImpulse, offsetB, out wsi); //note flip-negation
Symmetric3x3Wide.TransformWithoutOverlap(wsi, inertiaB.InverseInertiaTensor, out change);
Vector3Wide.Add(velocityB.Angular, change, out velocityB.Angular);
Vector3Wide.Scale(constraintSpaceImpulse, inertiaB.InverseMass, out change);
Vector3Wide.Subtract(velocityB.Linear, change, out velocityB.Linear); //note subtraction; the jacobian is -I
}
public void Test(ref CapsuleWide 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)
{
//While a capsule-triangle dedicated case would likely be ~20-40% faster than using GJK, it's not really worth the effort without some motivating use case.
//Instead, we'll just use GJK, but our capsule support function only considers the internal line segment (plus GJK's epsilon).
//So, here, we'll modify the distance and closestA to account for the disparity.
GJKTester tester = default;
tester.Test(ref a, ref b, ref offsetB, ref orientationA, ref orientationB, out intersected, out distance, out closestA, out normal);
//The containment epsilon expands the minkowski sum a little bit. Get rid of that so that the distance corresponds exactly to the capsule's surface.
//(We don't have this luxury for box-box, but we might as well handle it when we're able.)
var distanceChange = new Vector <float>(GJKTester.ContainmentEpsilonDefault) - a.Radius;
distance += distanceChange;
Vector3Wide.Scale(normal, distanceChange, out var closestPointOffset);
Vector3Wide.Add(closestA, closestPointOffset, out closestA);
intersected = Vector.BitwiseOr(intersected, Vector.LessThan(distance, Vector <float> .Zero));
}
public static void ApplyImpulse(ref Projection2Body1DOF data, ref Vector <float> correctiveImpulse,
ref BodyVelocities wsvA, ref BodyVelocities wsvB)
{
//Applying the impulse requires transforming the constraint space impulse into a world space velocity change.
//The first step is to transform into a world space impulse, which requires transforming by the transposed jacobian
//(transpose(jacobian) goes from world to constraint space, jacobian goes from constraint to world space).
//That world space impulse is then converted to a corrective velocity change by scaling the impulse by the inverse mass/inertia.
//As an optimization for constraints with smaller jacobians, the jacobian * (inertia or mass) transform is precomputed.
BodyVelocities correctiveVelocityA, correctiveVelocityB;
Vector3Wide.Scale(data.CSIToWSVLinearA, correctiveImpulse, out correctiveVelocityA.Linear);
Vector3Wide.Scale(data.CSIToWSVAngularA, correctiveImpulse, out correctiveVelocityA.Angular);
Vector3Wide.Scale(data.CSIToWSVLinearB, correctiveImpulse, out correctiveVelocityB.Linear);
Vector3Wide.Scale(data.CSIToWSVAngularB, correctiveImpulse, out correctiveVelocityB.Angular);
Vector3Wide.Add(correctiveVelocityA.Linear, wsvA.Linear, out wsvA.Linear);
Vector3Wide.Add(correctiveVelocityA.Angular, wsvA.Angular, out wsvA.Angular);
Vector3Wide.Add(correctiveVelocityB.Linear, wsvB.Linear, out wsvB.Linear);
Vector3Wide.Add(correctiveVelocityB.Angular, wsvB.Angular, out wsvB.Angular);
}
public void Test(ref SphereWide a, ref SphereWide b, ref Vector3Wide offsetB, out Convex1ContactManifoldWide manifold)
{
Vector3Wide.Length(ref offsetB, out var centerDistance);
//Note the negative 1. By convention, the normal points from B to A.
var inverseDistance = new Vector <float>(-1f) / centerDistance;
Vector3Wide.Scale(ref offsetB, ref inverseDistance, out manifold.Normal);
var normalIsValid = Vector.GreaterThan(centerDistance, Vector <float> .Zero);
//Arbitrarily choose the (0,1,0) if the two spheres are in the same position. Any unit length vector is equally valid.
manifold.Normal.X = Vector.ConditionalSelect(normalIsValid, manifold.Normal.X, Vector <float> .Zero);
manifold.Normal.Y = Vector.ConditionalSelect(normalIsValid, manifold.Normal.Y, Vector <float> .One);
manifold.Normal.Z = Vector.ConditionalSelect(normalIsValid, manifold.Normal.Z, Vector <float> .Zero);
manifold.Depth = a.Radius + b.Radius - centerDistance;
//The contact position relative to object A is computed as the average of the extreme point along the normal toward the opposing sphere on each sphere, averaged.
var negativeOffsetFromA = manifold.Depth * 0.5f - a.Radius;
Vector3Wide.Scale(ref manifold.Normal, ref negativeOffsetFromA, out manifold.OffsetA0);
}
public static void ApplyImpulse(ref PenetrationLimitProjection projection, ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref Vector3Wide normal,
ref Vector <float> correctiveImpulse,
ref BodyVelocities wsvA, ref BodyVelocities wsvB)
{
var linearVelocityChangeA = correctiveImpulse * inertiaA.InverseMass;
Vector3Wide.Scale(normal, linearVelocityChangeA, out var correctiveVelocityALinearVelocity);
Vector3Wide.Scale(projection.AngularA, correctiveImpulse, out var correctiveAngularImpulseA);
Symmetric3x3Wide.TransformWithoutOverlap(correctiveAngularImpulseA, inertiaA.InverseInertiaTensor, out var correctiveVelocityAAngularVelocity);
var linearVelocityChangeB = correctiveImpulse * inertiaB.InverseMass;
Vector3Wide.Scale(normal, linearVelocityChangeB, out var correctiveVelocityBLinearVelocity);
Vector3Wide.Scale(projection.AngularB, correctiveImpulse, out var correctiveAngularImpulseB);
Symmetric3x3Wide.TransformWithoutOverlap(correctiveAngularImpulseB, inertiaB.InverseInertiaTensor, out var correctiveVelocityBAngularVelocity);
Vector3Wide.Add(wsvA.Linear, correctiveVelocityALinearVelocity, out wsvA.Linear);
Vector3Wide.Add(wsvA.Angular, correctiveVelocityAAngularVelocity, out wsvA.Angular);
Vector3Wide.Subtract(wsvB.Linear, correctiveVelocityBLinearVelocity, out wsvB.Linear); //Note subtract; normal = -jacobianLinearB
Vector3Wide.Add(wsvB.Angular, correctiveVelocityBAngularVelocity, out wsvB.Angular);
}
public void Test(ref SphereWide a, ref CapsuleWide b, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB,
out Vector <int> intersected, out Vector <float> distance, out Vector3Wide closestA, out Vector3Wide normal)
{
//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 normal);
var surfaceOffset = -a.Radius;
Vector3Wide.Scale(normal, surfaceOffset, out closestA);
distance = internalDistance - a.Radius - b.Radius;
intersected = Vector.LessThanOrEqual(distance, Vector <float> .Zero);
}
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 static void Prestep(ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref TwoBody1DOFJacobians jacobians, ref SpringSettingsWide springSettings, ref Vector <float> maximumRecoveryVelocity,
ref Vector <float> positionError, float dt, float inverseDt, out Projection2Body1DOF projection)
{
//unsoftened effective mass = (J * M^-1 * JT)^-1
//where J is a constraintDOF x bodyCount*6 sized matrix, JT is its transpose, and for two bodies3D M^-1 is:
//[inverseMassA, 0, 0, 0]
//[0, inverseInertiaA, 0, 0]
//[0, 0, inverseMassB, 0]
//[0, 0, 0, inverseInertiaB]
//The entries of J match up to this convention, containing the linear and angular components of each body in sequence, so for a 2 body 1DOF constraint J would look like:
//[linearA 1x3, angularA 1x3, linearB 1x3, angularB 1x3]
//Note that it is a row vector by convention. When transforming velocities from world space into constraint space, it is assumed that the velocity vector is organized as a
//row vector matching up to the jacobian (that is, [linearA 1x3, angularA 1x3, linearB 1x3, angularB 1x3]), so for a 2 body 2 DOF constraint,
//worldVelocity * JT would be a [worldVelocity: 1x12] * [JT: 12x2], resulting in a 1x2 constraint space velocity row vector.
//Similarly, when going from constraint space impulse to world space impulse in the above example, we would do [csi: 1x2] * [J: 2x12] to get a 1x12 world impulse row vector.
//Note that the engine uses row vectors for all velocities and positions and so on. Rotation and inertia tensors are constructed for premultiplication.
//In other words, unlike many of the presentations in the space, we use v * JT and csi * J instead of J * v and JT * csi.
//There is no meaningful difference- the two conventions are just transpositions of each other.
//(If you want to know how this stuff works, go read the constraint related presentations: http://box2d.org/downloads/
//Be mindful of the difference in conventions. You'll see J * v instead of v * JT, for example. Everything is still fundamentally the same, though.)
//Due to the block structure of the mass matrix, we can handle each component separately and then sum the results.
//For this 1DOF constraint, the result is a simple scalar.
//Note that we store the intermediate results of J * M^-1 for use when projecting from constraint space impulses to world velocity changes.
//If we didn't store those intermediate values, we could just scale the dot product of jacobians.LinearA with itself to save 4 multiplies.
Vector3Wide.Scale(jacobians.LinearA, inertiaA.InverseMass, out projection.CSIToWSVLinearA);
Vector3Wide.Scale(jacobians.LinearB, inertiaB.InverseMass, out projection.CSIToWSVLinearB);
Vector3Wide.Dot(projection.CSIToWSVLinearA, jacobians.LinearA, out var linearA);
Vector3Wide.Dot(projection.CSIToWSVLinearB, jacobians.LinearB, out var linearB);
//The angular components are a little more involved; (J * I^-1) * JT is explicitly computed.
Symmetric3x3Wide.TransformWithoutOverlap(jacobians.AngularA, inertiaA.InverseInertiaTensor, out projection.CSIToWSVAngularA);
Symmetric3x3Wide.TransformWithoutOverlap(jacobians.AngularB, inertiaB.InverseInertiaTensor, out projection.CSIToWSVAngularB);
Vector3Wide.Dot(projection.CSIToWSVAngularA, jacobians.AngularA, out var angularA);
Vector3Wide.Dot(projection.CSIToWSVAngularB, jacobians.AngularB, out var angularB);
//Now for a digression!
//Softness is applied along the diagonal (which, for a 1DOF constraint, is just the only element).
//Check the the ODE reference for a bit more information: http://ode.org/ode-latest-userguide.html#sec_3_8_0
//And also see Erin Catto's Soft Constraints presentation for more details: http://box2d.org/files/GDC2011/GDC2011_Catto_Erin_Soft_Constraints.pdf)
//There are some very interesting tricks you can use here, though.
//Our core tuning variables are the damping ratio and natural frequency.
//Our runtime used variables are softness and an error reduction feedback scale..
//(For the following, I'll use the ODE terms CFM and ERP, constraint force mixing and error reduction parameter.)
//So first, we need to get from damping ratio and natural frequency to stiffness and damping spring constants.
//From there, we'll go to CFM/ERP.
//Then, we'll create an expression for a softened effective mass matrix (i.e. one that takes into account the CFM term),
//and an expression for the contraint force mixing term in the solve iteration.
//Finally, compute ERP.
//(And then some tricks.)
//1) Convert from damping ratio and natural frequency to stiffness and damping constants.
//The raw expressions are:
//stiffness = effectiveMass * naturalFrequency^2
//damping = effectiveMass * 2 * dampingRatio * naturalFrequency
//Rather than using any single object as the reference for the 'mass' term involved in this conversion, use the effective mass of the constraint.
//In other words, we're dynamically picking the spring constants necessary to achieve the desired behavior for the current constraint configuration.
//(See Erin Catto's presentation above for more details on this.)
//(Note that this is different from BEPUphysics v1. There, users configured stiffness and damping constants. That worked okay, but people often got confused about
//why constraints didn't behave the same when they changed masses. Usually it manifested as someone creating an incredibly high mass object relative to the default
//stiffness/damping, and they'd post on the forum wondering why constraints were so soft. Basically, the defaults were another sneaky tuning factor to get wrong.
//Since damping ratio and natural frequency define the behavior independent of the mass, this problem goes away- and it makes some other interesting things happen...)
//2) Convert from stiffness and damping constants to CFM and ERP.
//CFM = (stiffness * dt + damping)^-1
//ERP = (stiffness * dt) * (stiffness * dt + damping)^-1
//Or, to rephrase:
//ERP = (stiffness * dt) * CFM
//3) Use CFM and ERP to create a softened effective mass matrix and a force mixing term for the solve iterations.
//Start with a base definition which we won't be deriving, the velocity constraint itself (stated as an equality constraint here):
//This means 'world space velocity projected into constraint space should equal the velocity bias term combined with the constraint force mixing term'.
//(The velocity bias term will be computed later- it's the position error scaled by the error reduction parameter, ERP. Position error is used to create a velocity motor goal.)
//We're pulling back from the implementation of sequential impulses here, so rather than using the term 'accumulated impulse', we'll use 'lambda'
//(which happens to be consistent with the ODE documentation covering the same topic). Lambda is impulse that satisfies the constraint.
//wsv * JT = bias - lambda * CFM/dt
//This can be phrased as:
//currentVelocity = targetVelocity
//Or:
//goalVelocityChange = targetVelocity - currentVelocity
//lambda = goalVelocityChange * effectiveMass
//lambda = (targetVelocity - currentVelocity) * effectiveMass
//lambda = (bias - lambda * CFM/dt - currentVelocity) * effectiveMass
//Solving for lambda:
//lambda = (bias - currentVelocity) * effectiveMass - lambda * CFM/dt * effectiveMass
//lambda + lambda * CFM/dt * effectiveMass = (bias - currentVelocity) * effectiveMass
//(lambda + lambda * CFM/dt * effectiveMass) * effectiveMass^-1 = bias - currentVelocity
//lambda * effectiveMass^-1 + lambda * CFM/dt = bias - currentVelocity
//lambda * (effectiveMass^-1 + CFM/dt) = bias - currentVelocity
//lambda = (bias - currentVelocity) * (effectiveMass^-1 + CFM/dt)^-1
//lambda = (bias - wsv * JT) * (effectiveMass^-1 + CFM/dt)^-1
//In other words, we transform the velocity change (bias - wsv * JT) into the constraint-satisfying impulse, lambda, using a matrix (effectiveMass^-1 + CFM/dt)^-1.
//That matrix is the softened effective mass:
//softenedEffectiveMass = (effectiveMass^-1 + CFM/dt)^-1
//Here's where some trickiness occurs. (Be mindful of the distinction between the softened and unsoftened effective mass).
//Start by substituting CFM into the softened effective mass definition:
//CFM/dt = (stiffness * dt + damping)^-1 / dt = (dt * (stiffness * dt + damping))^-1 = (stiffness * dt^2 + damping*dt)^-1
//softenedEffectiveMass = (effectiveMass^-1 + (stiffness * dt^2 + damping * dt)^-1)^-1
//Now substitute the definitions of stiffness and damping, treating the scalar components as uniform scaling matrices of dimension equal to effectiveMass:
//softenedEffectiveMass = (effectiveMass^-1 + ((effectiveMass * naturalFrequency^2) * dt^2 + (effectiveMass * 2 * dampingRatio * naturalFrequency) * dt)^-1)^-1
//Combine the inner effectiveMass coefficients, given matrix multiplication distributes over addition:
//softenedEffectiveMass = (effectiveMass^-1 + (effectiveMass * (naturalFrequency^2 * dt^2) + effectiveMass * (2 * dampingRatio * naturalFrequency * dt))^-1)^-1
//softenedEffectiveMass = (effectiveMass^-1 + (effectiveMass * (naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt))^-1)^-1
//Apply the inner matrix inverse:
//softenedEffectiveMass = (effectiveMass^-1 + (naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1 * effectiveMass^-1)^-1
//Once again, combine coefficients of the inner effectiveMass^-1 terms:
//softenedEffectiveMass = ((1 + (naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1) * effectiveMass^-1)^-1
//Apply the inverse again:
//softenedEffectiveMass = effectiveMass * (1 + (naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1)^-1
//So, to put it another way- because CFM is based on the effective mass, applying it to the effective mass results in a simple downscale.
//What has been gained? Consider what happens in the solve iteration.
//We take the velocity error:
//velocityError = bias - accumulatedImpulse * CFM/dt - wsv * JT
//and convert it to a corrective impulse with the effective mass:
//impulse = (bias - accumulatedImpulse * CFM/dt - wsv * JT) * softenedEffectiveMass
//The effective mass distributes over the set:
//impulse = bias * softenedEffectiveMass - accumulatedImpulse * CFM/dt * softenedEffectiveMass - wsv * JT * softenedEffectiveMass
//Focus on the CFM term:
//-accumulatedImpulse * CFM/dt * softenedEffectiveMass
//What is CFM/dt * softenedEffectiveMass? Substitute.
//(stiffness * dt^2 + damping * dt)^-1 * softenedEffectiveMass
//((effectiveMass * naturalFrequency^2) * dt^2 + (effectiveMass * 2 * dampingRatio * naturalFrequency * dt))^-1 * softenedEffectiveMass
//Combine terms:
//(effectiveMass * (naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt))^-1 * softenedEffectiveMass
//Apply inverse:
//(naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1 * effectiveMass^-1 * softenedEffectiveMass
//Expand softened effective mass from earlier:
//(naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1 * effectiveMass^-1 * effectiveMass * (1 + (naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1)^-1
//Cancel effective masses: (!)
//(naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1 * (1 + (naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1)^-1
//Because CFM was created from effectiveMass, the CFM/dt * effectiveMass term is actually independent of the effectiveMass!
//The remaining expression is still a matrix, but fortunately it is a simple uniform scaling matrix that we can store and apply as a single scalar.
//4) How do you compute ERP?
//ERP = (stiffness * dt) * CFM
//ERP = (stiffness * dt) * (stiffness * dt + damping)^-1
//ERP = ((effectiveMass * naturalFrequency^2) * dt) * ((effectiveMass * naturalFrequency^2) * dt + (effectiveMass * 2 * dampingRatio * naturalFrequency))^-1
//Combine denominator terms:
//ERP = ((effectiveMass * naturalFrequency^2) * dt) * ((effectiveMass * (naturalFrequency^2 * dt + 2 * dampingRatio * naturalFrequency))^-1
//Apply denominator inverse:
//ERP = ((effectiveMass * naturalFrequency^2) * dt) * (naturalFrequency^2 * dt + 2 * dampingRatio * naturalFrequency)^-1 * effectiveMass^-1
//Uniform scaling matrices commute:
//ERP = (naturalFrequency^2 * dt) * effectiveMass * effectiveMass^-1 * (naturalFrequency^2 * dt + 2 * dampingRatio * naturalFrequency)^-1
//Cancellation!
//ERP = (naturalFrequency^2 * dt) * (naturalFrequency^2 * dt + 2 * dampingRatio * naturalFrequency)^-1
//ERP = (naturalFrequency * dt) * (naturalFrequency * dt + 2 * dampingRatio)^-1
//ERP is a simple scalar, independent of mass.
//5) So we can compute CFM, ERP, the softened effective mass matrix, and we have an interesting shortcut on the constraint force mixing term of the solve iterations.
//Is there anything more that can be done? You bet!
//Let's look at the post-distribution impulse computation again:
//impulse = bias * effectiveMass - accumulatedImpulse * CFM/dt * effectiveMass - wsv * JT * effectiveMass
//During the solve iterations, the only quantities that vary are the accumulated impulse and world space velocities. So the rest can be precomputed.
//bias * effectiveMass,
//CFM/dt * effectiveMass,
//JT * effectiveMass
//In other words, we bypass the intermediate velocity state and go directly from source velocities to an impulse.
//Note the sizes of the precomputed types above:
//bias * effective mass is the same size as bias (vector with dimension equal to constrained DOFs)
//CFM/dt * effectiveMass is a single scalar regardless of constrained DOFs,
//JT * effectiveMass is the same size as JT
//But note that we no longer need to load the effective mass! It is implicit.
//The resulting computation is:
//impulse = a - accumulatedImpulse * b - wsv * c
//two DOF-width adds (add/subtract), one DOF-width multiply, and a 1xDOF * DOFx12 jacobian-sized transform.
//Compare to;
//(bias - accumulatedImpulse * CFM/dt - wsv * JT) * effectiveMass
//two DOF-width adds (add/subtract), one DOF width multiply, a 1xDOF * DOFx12 jacobian-sized transform, and a 1xDOF * DOFxDOF transform.
//In other words, we shave off a whole 1xDOF * DOFxDOF transform per iteration.
//So, taken in isolation, this is a strict win both in terms of memory and the amount of computation.
//Unfortunately, it's not quite so simple- jacobians are ALSO used to transform the impulse into world space so that it can be used to change the body velocities.
//We still need to have those around. So while we no longer store the effective mass, our jacobian has sort of been duplicated.
//But wait, there's more!
//That process looks like:
//wsv += impulse * J * M^-1
//So while we need to store something here, we can take advantage of the fact that we aren't using the jacobian anywhere else (it's replaced by the JT * effectiveMass term above).
//Precompute J*M^-1, too.
//So you're still loading a jacobian-sized matrix, but you don't need to load M^-1! That saves you 14 scalars. (symmetric 3x3 + 1 + symmetric 3x3 + 1)
//That saves you the multiplication of (impulse * J) * M^-1, which is 6 multiplies and 6 dot products.
//Note that this optimization's value depends on the number of constrained DOFs.
//Net memory change, opt vs no opt, in scalars:
//1DOF: costs 1x12, saves 1x1 effective mass and the 14 scalar M^-1: -3
//2DOF: costs 2x12, saves 2x2 symmetric effective mass and the 14 scalar M^-1: 7
//3DOF: costs 3x12, saves 3x3 symmetric effective mass and the 14 scalar M^-1: 16
//4DOF: costs 4x12, saves 4x4 symmetric effective mass and the 14 scalar M^-1: 24
//5DOF: costs 5x12, saves 5x5 symmetric effective mass and the 14 scalar M^-1: 31
//6DOF: costs 6x12, saves 6x6 symmetric effective mass and the 14 scalar M^-1: 37
//Net compute savings, opt vs no opt:
//DOF savings = 1xDOF * DOFxDOF (DOF DOFdot products), 2 1x3 * scalar (6 multiplies), 2 1x3 * 3x3 (6 3dot products)
// = (DOF*DOF multiplies + DOF*(DOF-1) adds) + (6 multiplies) + (18 multiplies + 12 adds)
// = DOF*DOF + 24 multiplies, DOF*DOF-DOF + 12 adds
//1DOF: 25 multiplies, 12 adds
//2DOF: 28 multiplies, 14 adds
//3DOF: 33 multiplies, 18 adds
//4DOF: 40 multiplies, 24 adds
//5DOF: 49 multiplies, 32 adds
//6DOF: 60 multiplies, 42 adds
//So does our 'optimization' actually do anything useful?
//In 1 DOF constraints, it's often a win with no downsides.
//2+ are difficult to determine.
//This depends on heavily on the machine's SIMD width. You do every lane's ALU ops in parallel, but the loads are still fundamentally bound by memory bandwidth.
//The loads are coherent, at least- no gathers on this stuff. But I wouldn't be surprised if 3DOF+ constraints end up being faster *without* the pretransformations on wide SIMD.
//This is just something that will require case by case analysis. Constraints can have special structure which change the judgment.
//(Also, note that large DOF jacobians are often very sparse. Consider the jacobians used by a 6DOF weld joint. You could likely do special case optimizations to reduce the
//load further. It is unlikely that you could find a way to do the same to JT * effectiveMass. J * M^-1 might have some savings, though. But J*M^-1 isn't *sparser*
//than J by itself, so the space savings are limited. As long as you precompute, the above load requirement offset will persist.)
//Good news, though! There are a lot of constraints where this trick is applicable.
//We'll start with the unsoftened effective mass, constructed from the contributions computed above:
var effectiveMass = Vector <float> .One / (linearA + linearB + angularA + angularB);
SpringSettingsWide.ComputeSpringiness(springSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale);
var softenedEffectiveMass = effectiveMass * effectiveMassCFMScale;
//Note that we use a bit of a hack when computing the bias velocity- even if our damping ratio/natural frequency implies a strongly springy response
//that could cause a significant velocity overshoot, we apply an arbitrary clamping value to keep it reasonable.
//This is useful for a variety of inequality constraints (like contacts) because you don't always want them behaving as true springs.
var biasVelocity = Vector.Min(positionError * positionErrorToVelocity, maximumRecoveryVelocity);
projection.BiasImpulse = biasVelocity * softenedEffectiveMass;
//Precompute the wsv * (JT * softenedEffectiveMass) term.
//Note that we store it in a Vector3Wide as if it's a row vector, but this is really a column (because JT is a column vector).
//So we're really storing (JT * softenedEffectiveMass)T = softenedEffectiveMassT * J.
//Since this constraint is 1DOF, the softenedEffectiveMass is a scalar and the order doesn't matter.
//In the solve iterations, the WSVtoCSI term will be transposed during transformation,
//resulting in the proper wsv * (softenedEffectiveMassT * J)T = wsv * (JT * softenedEffectiveMass).
//You'll see this pattern repeated in higher DOF constraints. We explicitly compute softenedEffectiveMassT * J, and then apply the transpose in the solves.
//(Why? Because creating a Matrix3x2 and Matrix2x3 and 4x3 and 3x4 and 5x3 and 3x5 and so on just doubles the number of representations with little value.)
Vector3Wide.Scale(jacobians.LinearA, softenedEffectiveMass, out projection.WSVtoCSILinearA);
Vector3Wide.Scale(jacobians.AngularA, softenedEffectiveMass, out projection.WSVtoCSIAngularA);
Vector3Wide.Scale(jacobians.LinearB, softenedEffectiveMass, out projection.WSVtoCSILinearB);
Vector3Wide.Scale(jacobians.AngularB, softenedEffectiveMass, out projection.WSVtoCSIAngularB);
}
public void Test(
ref CapsuleWide a, ref BoxWide b, ref Vector <float> speculativeMargin,
ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount,
out Convex2ContactManifoldWide manifold)
{
Prepare(ref a, ref b, ref offsetB, ref orientationA, ref orientationB, out var localOffsetA, out var capsuleAxis, out var edgeCenters);
//Swizzle XYZ -> YZX
Vector3Wide localNormal;
TestAndRefineBoxEdge(ref localOffsetA.Y, ref localOffsetA.Z, ref localOffsetA.X,
ref capsuleAxis.Y, ref capsuleAxis.Z, ref capsuleAxis.X,
ref a.HalfLength,
ref edgeCenters.Y, ref edgeCenters.Z,
ref b.HalfHeight, ref b.HalfLength, ref b.HalfWidth,
out var ta, out var depth, out localNormal.Y, out localNormal.Z, out localNormal.X);
//Swizzle XYZ -> ZXY
TestAndRefineBoxEdge(ref localOffsetA.Z, ref localOffsetA.X, ref localOffsetA.Y,
ref capsuleAxis.Z, ref capsuleAxis.X, ref capsuleAxis.Y,
ref a.HalfLength,
ref edgeCenters.Z, ref edgeCenters.X,
ref b.HalfLength, ref b.HalfWidth, ref b.HalfHeight,
out var eyta, out var eyDepth, out var eynZ, out var eynX, out var eynY);
Select(ref depth, ref ta, ref localNormal.X, ref localNormal.Y, ref localNormal.Z,
ref eyDepth, ref eyta, ref eynX, ref eynY, ref eynZ);
//Swizzle XYZ -> XYZ
TestAndRefineBoxEdge(ref localOffsetA.X, ref localOffsetA.Y, ref localOffsetA.Z,
ref capsuleAxis.X, ref capsuleAxis.Y, ref capsuleAxis.Z,
ref a.HalfLength,
ref edgeCenters.X, ref edgeCenters.Y,
ref b.HalfWidth, ref b.HalfHeight, ref b.HalfLength,
out var ezta, out var ezDepth, out var eznX, out var eznY, out var eznZ);
Select(ref depth, ref ta, ref localNormal.X, ref localNormal.Y, ref localNormal.Z,
ref ezDepth, ref ezta, ref eznX, ref eznY, ref eznZ);
var zero = Vector <float> .Zero;
//Face X
TestBoxFace(ref localOffsetA.X,
ref capsuleAxis.X, ref a.HalfLength,
ref b.HalfWidth,
out var fxDepth, out var fxn);
Select(ref depth, ref localNormal.X, ref localNormal.Y, ref localNormal.Z,
ref fxDepth, ref fxn, ref zero, ref zero);
//Face Y
TestBoxFace(ref localOffsetA.Y,
ref capsuleAxis.Y, ref a.HalfLength,
ref b.HalfHeight,
out var fyDepth, out var fyn);
Select(ref depth, ref localNormal.X, ref localNormal.Y, ref localNormal.Z,
ref fyDepth, ref zero, ref fyn, ref zero);
//Face Z
TestBoxFace(ref localOffsetA.Z,
ref capsuleAxis.Z, ref a.HalfLength,
ref b.HalfLength,
out var fzDepth, out var fzn);
Select(ref depth, ref localNormal.X, ref localNormal.Y, ref localNormal.Z,
ref fzDepth, ref zero, ref zero, ref fzn);
//While the above chooses a minimal depth, edge-edge contact will frequently produce interval lengths of 0 and end up overwriting near-equivalent face intervals.
//One option would be to bias the comparison to accept face contacts more readily, but we can do something a little less fragile, and which also gives us a
//way to safely compute depth on a per contact basis:
//choose a representative box face based on the collision normal detected above, and compute the interval of intersection along the capsule axis of the box face projected onto the capsule axis.
//We'll compute this by unprojecting the capsule axis onto the box face plane along the local normal.
//Note that this interval always includes the closest point.
var xDot = localNormal.X * fxn;
var yDot = localNormal.Y * fyn;
var zDot = localNormal.Z * fzn;
var useX = Vector.GreaterThan(xDot, Vector.Max(yDot, zDot));
var useY = Vector.AndNot(Vector.GreaterThan(yDot, zDot), useX);
var useZ = Vector.AndNot(Vector.OnesComplement(useX), useY);
//Unproject the capsule center and capsule axis onto the representative face plane.
//unprojectedAxis = capsuleAxis - localNormal * dot(capsuleAxis, faceNormal) / dot(localNormal, faceNormal)
//unprojectedCenter = capsuleCenter - localNormal * dot(capsuleCenter - pointOnFace, faceNormal) / dot(localNormal, faceNormal)
var faceNormalDotLocalNormal = Vector.ConditionalSelect(useX, xDot, Vector.ConditionalSelect(useY, yDot, zDot));
var inverseFaceNormalDotLocalNormal = Vector <float> .One / Vector.Max(new Vector <float>(1e-15f), faceNormalDotLocalNormal);
var capsuleAxisDotFaceNormal = Vector.ConditionalSelect(useX, capsuleAxis.X * fxn, Vector.ConditionalSelect(useY, capsuleAxis.Y * fyn, capsuleAxis.Z * fzn));
var capsuleCenterDotFaceNormal = Vector.ConditionalSelect(useX, localOffsetA.X * fxn, Vector.ConditionalSelect(useY, localOffsetA.Y * fyn, localOffsetA.Z * fzn));
var facePlaneOffset = Vector.ConditionalSelect(useX, b.HalfWidth, Vector.ConditionalSelect(useY, b.HalfHeight, b.HalfLength));
var tAxis = capsuleAxisDotFaceNormal * inverseFaceNormalDotLocalNormal;
var tCenter = (capsuleCenterDotFaceNormal - facePlaneOffset) * inverseFaceNormalDotLocalNormal;
//Work in tangent space.
//Face X uses tangents Y and Z.
//Face Y uses tangents X and Z.
//Face Z uses tangents X and Y.
Vector3Wide.Scale(localNormal, tAxis, out var axisOffset);
Vector3Wide.Scale(localNormal, tCenter, out var centerOffset);
Vector3Wide.Subtract(capsuleAxis, axisOffset, out var unprojectedAxis);
Vector3Wide.Subtract(localOffsetA, centerOffset, out var unprojectedCenter);
Vector2Wide tangentSpaceCenter, tangentSpaceAxis;
tangentSpaceAxis.X = Vector.ConditionalSelect(useX, unprojectedAxis.Y, unprojectedAxis.X);
tangentSpaceAxis.Y = Vector.ConditionalSelect(useZ, unprojectedAxis.Y, unprojectedAxis.Z);
tangentSpaceCenter.X = Vector.ConditionalSelect(useX, unprojectedCenter.Y, unprojectedCenter.X);
tangentSpaceCenter.Y = Vector.ConditionalSelect(useZ, unprojectedCenter.Y, unprojectedCenter.Z);
//Slightly boost the size of the face to avoid minor numerical issues that could block coplanar contacts.
var epsilonScale = Vector.Min(Vector.Max(b.HalfWidth, Vector.Max(b.HalfHeight, b.HalfLength)), Vector.Max(a.HalfLength, a.Radius));
var epsilon = epsilonScale * 1e-3f;
var halfExtentX = epsilon + Vector.ConditionalSelect(useX, b.HalfHeight, b.HalfWidth);
var halfExtentY = epsilon + Vector.ConditionalSelect(useZ, b.HalfHeight, b.HalfLength);
//Compute interval bounded by edge normals pointing along tangentX.
//tX = -dot(tangentSpaceCenter +- halfExtentX, edgeNormal) / dot(unprojectedCapsuleAxis, edgeNormal)
var negativeOne = new Vector <float>(-1);
var inverseAxisX = negativeOne / tangentSpaceAxis.X;
var inverseAxisY = negativeOne / tangentSpaceAxis.Y;
var tX0 = (tangentSpaceCenter.X - halfExtentX) * inverseAxisX;
var tX1 = (tangentSpaceCenter.X + halfExtentX) * inverseAxisX;
var tY0 = (tangentSpaceCenter.Y - halfExtentY) * inverseAxisY;
var tY1 = (tangentSpaceCenter.Y + halfExtentY) * inverseAxisY;
var minX = Vector.Min(tX0, tX1);
var maxX = Vector.Max(tX0, tX1);
var minY = Vector.Min(tY0, tY1);
var maxY = Vector.Max(tY0, tY1);
//Protect against division by zero. If the unprojected capsule is within the slab, use an infinite interval. If it's outside and parallel, use an invalid interval.
var useFallbackX = Vector.LessThan(Vector.Abs(tangentSpaceAxis.X), new Vector <float>(1e-15f));
var useFallbackY = Vector.LessThan(Vector.Abs(tangentSpaceAxis.Y), new Vector <float>(1e-15f));
var centerContainedX = Vector.LessThanOrEqual(Vector.Abs(tangentSpaceCenter.X), halfExtentX);
var centerContainedY = Vector.LessThanOrEqual(Vector.Abs(tangentSpaceCenter.Y), halfExtentY);
var largeNegative = new Vector <float>(-float.MaxValue);
var largePositive = new Vector <float>(float.MaxValue);
minX = Vector.ConditionalSelect(useFallbackX, Vector.ConditionalSelect(centerContainedX, largeNegative, largePositive), minX);
maxX = Vector.ConditionalSelect(useFallbackX, Vector.ConditionalSelect(centerContainedX, largePositive, largeNegative), maxX);
minY = Vector.ConditionalSelect(useFallbackY, Vector.ConditionalSelect(centerContainedY, largeNegative, largePositive), minY);
maxY = Vector.ConditionalSelect(useFallbackY, Vector.ConditionalSelect(centerContainedY, largePositive, largeNegative), maxY);
var faceMin = Vector.Max(minX, minY);
var faceMax = Vector.Min(maxX, maxY);
//Clamp the resulting interval to the capsule axis.
var tMin = Vector.Max(Vector.Min(faceMin, a.HalfLength), -a.HalfLength);
var tMax = Vector.Max(Vector.Min(faceMax, a.HalfLength), -a.HalfLength);
var faceIntervalExists = Vector.GreaterThanOrEqual(faceMax, faceMin);
tMin = Vector.ConditionalSelect(faceIntervalExists, Vector.Min(tMin, ta), ta);
tMax = Vector.ConditionalSelect(faceIntervalExists, Vector.Max(tMax, ta), ta);
//Each contact may have its own depth.
//Imagine a face collision- if the capsule axis isn't fully parallel with the plane's surface, it would be strange to use the same depth for both contacts.
//We have two points on the capsule and box. We can reuse the unprojeection from earlier to compute the offset between them.
var separationMin = tCenter + tAxis * tMin;
var separationMax = tCenter + tAxis * tMax;
manifold.Depth0 = a.Radius - separationMin;
manifold.Depth1 = a.Radius - separationMax;
Vector3Wide.Scale(capsuleAxis, tMin, out var localA0);
Vector3Wide.Scale(capsuleAxis, tMax, out var localA1);
Vector3Wide.Add(localOffsetA, localA0, out var bToA0);
Vector3Wide.Add(localOffsetA, localA1, out var bToA1);
manifold.FeatureId0 = Vector <int> .Zero;
manifold.FeatureId1 = Vector <int> .One;
//Transform A0, A1, and the normal into world space.
Matrix3x3Wide.CreateFromQuaternion(orientationB, out var orientationMatrixB);
Matrix3x3Wide.TransformWithoutOverlap(localNormal, orientationMatrixB, out manifold.Normal);
Matrix3x3Wide.TransformWithoutOverlap(localA0, orientationMatrixB, out manifold.OffsetA0);
Matrix3x3Wide.TransformWithoutOverlap(localA1, orientationMatrixB, out manifold.OffsetA1);
//Apply the normal offset to the contact positions.
var negativeOffsetFromA0 = manifold.Depth0 * 0.5f - a.Radius;
var negativeOffsetFromA1 = manifold.Depth1 * 0.5f - a.Radius;
Vector3Wide.Scale(manifold.Normal, negativeOffsetFromA0, out var normalPush0);
Vector3Wide.Scale(manifold.Normal, negativeOffsetFromA1, out var normalPush1);
Vector3Wide.Add(manifold.OffsetA0, normalPush0, out manifold.OffsetA0);
Vector3Wide.Add(manifold.OffsetA1, normalPush1, out manifold.OffsetA1);
var minimumAcceptedDepth = -speculativeMargin;
manifold.Contact0Exists = Vector.GreaterThanOrEqual(manifold.Depth0, minimumAcceptedDepth);
manifold.Contact1Exists = Vector.BitwiseAnd(
Vector.GreaterThanOrEqual(manifold.Depth1, minimumAcceptedDepth),
Vector.GreaterThan(tMax - tMin, new Vector <float>(1e-7f) * a.HalfLength));
}
public unsafe void Test(ref 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);
}