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);
}
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 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 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 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 Prestep(ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref Contact1PrestepData prestep, float dt, float inverseDt,
out Projection projection)
{
Vector3Wide.Subtract(prestep.OffsetA0, prestep.OffsetB, out var contactOffsetB);
Prestep(ref inertiaA, ref inertiaB,
ref prestep.OffsetA0, ref contactOffsetB, ref prestep.Normal, ref prestep.PenetrationDepth0, ref prestep.SpringSettings, ref prestep.MaximumRecoveryVelocity,
dt, inverseDt, out projection);
}
public static void ApplyImpulse(ref Vector3Wide angularJacobianA, ref BodyInertias inertiaA, ref BodyInertias inertiaB,
ref Vector <float> correctiveImpulse, ref BodyVelocities wsvA, ref BodyVelocities wsvB)
{
Vector3Wide.Scale(ref angularJacobianA, ref correctiveImpulse, out var worldCorrectiveImpulseA);
Triangular3x3Wide.TransformBySymmetricWithoutOverlap(ref worldCorrectiveImpulseA, ref inertiaA.InverseInertiaTensor, out var worldCorrectiveVelocityA);
Triangular3x3Wide.TransformBySymmetricWithoutOverlap(ref worldCorrectiveImpulseA, ref inertiaB.InverseInertiaTensor, out var worldCorrectiveVelocityB);
Vector3Wide.Add(ref wsvA.AngularVelocity, ref worldCorrectiveVelocityA, out wsvA.AngularVelocity);
Vector3Wide.Subtract(ref wsvB.AngularVelocity, ref worldCorrectiveVelocityB, out wsvB.AngularVelocity);
}
public static void ComputeVolume(ref BoundingBoxWide boxes, out Vector <float> volumes)
{
volumes = new Vector <float>();
Vector3Wide span;
Vector3Wide.Subtract(ref boxes.Max, ref boxes.Min, out span);
volumes = span.X * span.Y * span.Z;
}
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);
}
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 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);
}
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 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 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 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 static void Prestep(ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref Vector3Wide normal, ref Contact3PrestepData prestep, float dt, float inverseDt,
out Projection projection)
{
Vector3Wide.CrossWithoutOverlap(prestep.OffsetA0, normal, out projection.Penetration0.AngularA);
Vector3Wide.Subtract(prestep.OffsetA0, prestep.OffsetB, out var offsetB0);
Vector3Wide.CrossWithoutOverlap(normal, offsetB0, out projection.Penetration0.AngularB);
Vector3Wide.CrossWithoutOverlap(prestep.OffsetA1, normal, out projection.Penetration1.AngularA);
Vector3Wide.Subtract(prestep.OffsetA1, prestep.OffsetB, out var offsetB1);
Vector3Wide.CrossWithoutOverlap(normal, offsetB1, out projection.Penetration1.AngularB);
Vector3Wide.CrossWithoutOverlap(prestep.OffsetA2, normal, out projection.Penetration2.AngularA);
Vector3Wide.Subtract(prestep.OffsetA2, prestep.OffsetB, out var offsetB2);
Vector3Wide.CrossWithoutOverlap(normal, offsetB2, out projection.Penetration2.AngularB);
//effective mass
Symmetric3x3Wide.VectorSandwich(projection.Penetration0.AngularA, inertiaA.InverseInertiaTensor, out var angularA0);
Symmetric3x3Wide.VectorSandwich(projection.Penetration0.AngularB, inertiaB.InverseInertiaTensor, out var angularB0);
Symmetric3x3Wide.VectorSandwich(projection.Penetration1.AngularA, inertiaA.InverseInertiaTensor, out var angularA1);
Symmetric3x3Wide.VectorSandwich(projection.Penetration1.AngularB, inertiaB.InverseInertiaTensor, out var angularB1);
Symmetric3x3Wide.VectorSandwich(projection.Penetration2.AngularA, inertiaA.InverseInertiaTensor, out var angularA2);
Symmetric3x3Wide.VectorSandwich(projection.Penetration2.AngularB, inertiaB.InverseInertiaTensor, out var angularB2);
//Linear effective mass contribution notes:
//1) The J * M^-1 * JT can be reordered to J * JT * M^-1 for the linear components, since M^-1 is a scalar and dot(n * scalar, n) = dot(n, n) * scalar.
//2) dot(normal, normal) == 1, so the contribution from each body is just its inverse mass.
SpringSettingsWide.ComputeSpringiness(ref prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale);
var linear = inertiaA.InverseMass + inertiaB.InverseMass;
//Note that we don't precompute the JT * effectiveMass term. Since the jacobians are shared, we have to do that multiply anyway.
projection.Penetration0.EffectiveMass = effectiveMassCFMScale / (linear + angularA0 + angularB0);
projection.Penetration1.EffectiveMass = effectiveMassCFMScale / (linear + angularA1 + angularB1);
projection.Penetration2.EffectiveMass = effectiveMassCFMScale / (linear + angularA2 + angularB2);
//If depth is negative, the bias velocity will permit motion up until the depth hits zero. This works because positionErrorToVelocity * dt will always be <=1.
var inverseDtVector = new Vector <float>(inverseDt);
projection.Penetration0.BiasVelocity = Vector.Min(prestep.PenetrationDepth0 * inverseDtVector, Vector.Min(prestep.PenetrationDepth0 * positionErrorToVelocity, prestep.MaximumRecoveryVelocity));
projection.Penetration1.BiasVelocity = Vector.Min(prestep.PenetrationDepth1 * inverseDtVector, Vector.Min(prestep.PenetrationDepth1 * positionErrorToVelocity, prestep.MaximumRecoveryVelocity));
projection.Penetration2.BiasVelocity = Vector.Min(prestep.PenetrationDepth2 * inverseDtVector, Vector.Min(prestep.PenetrationDepth2 * positionErrorToVelocity, prestep.MaximumRecoveryVelocity));
}
public void Test(ref SphereWide a, ref TriangleWide b, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB,
out Vector <int> intersected, out Vector <float> distance, out Vector3Wide closestA, out Vector3Wide normal)
{
//Note that we're borrowing a lot here from the SphereTriangleCollisionTask. Could share more if you find yourself needing to change things dramatically.
//Main difficulty in fully sharing is that sweep tests do not honor one sidedness, so some of the conditions change.
//Work in the local space of the triangle, since it's quicker to transform the sphere position than the vertices of the triangle.
Matrix3x3Wide.CreateFromQuaternion(orientationB, out var rB);
Matrix3x3Wide.TransformByTransposedWithoutOverlap(offsetB, rB, out var localOffsetB);
Vector3Wide.Subtract(b.B, b.A, out var ab);
Vector3Wide.Subtract(b.C, b.A, out var ac);
//localOffsetA = -localOffsetB, so pa = triangle.A + localOffsetB.
Vector3Wide.Add(b.A, localOffsetB, out var pa);
Vector3Wide.CrossWithoutOverlap(ab, ac, out var localTriangleNormal);
Vector3Wide.Dot(localTriangleNormal, pa, out var paN);
//EdgeAB plane test: (pa x ab) * (ab x ac) >= 0
//EdgeAC plane test: (ac x pa) * (ab x ac) >= 0
//Note that these are scaled versions of the barycentric coordinates.
//To normalize them such that the weights of a point within the triangle equal 1, we just need to divide by dot(ab x ac, ab x ac).
//In other words, to test the third edge plane, we can ensure that the unnormalized weights are both positive and sum to a value less than dot(ab x ac, ab x ac).
//If a point is outside of an edge plane, we know that it's not in the face region or any other edge region. It could, however, be in an adjacent vertex region.
//Vertex cases can be handled by clamping an edge case.
//Further, note that for any query location, it is sufficient to only test one edge even if the point is outside two edge planes. If it's outside two edge planes,
//that just means it's going to be on the shared vertex, so a clamped edge test captures the correct closest point.
//So, at each edge, if the point is outside the plane, cache the edge. The last edge registering an outside result will be tested.
//(pa x ab) * (ab x ac) = (pa * ab) * (ab * ac) - (pa * ac) * (ab * ab)
//(ac x pa) * (ab x ac) = (ac * ab) * (pa * ac) - (ac * ac) * (pa * ab)
//(ab x ac) * (ab x ac) = (ab * ab) * (ac * ac) - (ab * ac) * (ab * ac)
Vector3Wide.Dot(pa, ab, out var abpa);
Vector3Wide.Dot(ab, ac, out var abac);
Vector3Wide.Dot(ac, pa, out var acpa);
Vector3Wide.Dot(ac, ac, out var acac);
Vector3Wide.Dot(ab, ab, out var abab);
var edgePlaneTestAB = abpa * abac - acpa * abab;
var edgePlaneTestAC = abac * acpa - acac * abpa;
var triangleNormalLengthSquared = abab * acac - abac * abac;
var edgePlaneTestBC = triangleNormalLengthSquared - edgePlaneTestAB - edgePlaneTestAC;
var outsideAB = Vector.LessThan(edgePlaneTestAB, Vector <float> .Zero);
var outsideAC = Vector.LessThan(edgePlaneTestAC, Vector <float> .Zero);
var outsideBC = Vector.LessThan(edgePlaneTestBC, Vector <float> .Zero);
var outsideAnyEdge = Vector.BitwiseOr(outsideAB, Vector.BitwiseOr(outsideAC, outsideBC));
Vector3Wide localClosestOnTriangle;
var negativeOne = new Vector <int>(-1);
if (Vector.EqualsAny(outsideAnyEdge, negativeOne))
{
//At least one lane detected a point outside of the triangle. Choose one edge which is outside as the representative.
Vector3Wide.ConditionalSelect(outsideAC, ac, ab, out var edgeDirection);
Vector3Wide.Subtract(b.C, b.B, out var bc);
Vector3Wide.ConditionalSelect(outsideBC, bc, edgeDirection, out edgeDirection);
Vector3Wide.ConditionalSelect(outsideBC, b.B, b.A, out var edgeStart);
Vector3Wide.Add(localOffsetB, edgeStart, out var negativeEdgeStartToP);
//This does some partially redundant work if the edge is AB or AC, but given that we didn't have bcbc or bcpb, it's fine.
Vector3Wide.Dot(negativeEdgeStartToP, edgeDirection, out var negativeOffsetDotEdge);
Vector3Wide.Dot(edgeDirection, edgeDirection, out var edgeDotEdge);
var edgeScale = Vector.Max(Vector <float> .Zero, Vector.Min(Vector <float> .One, -negativeOffsetDotEdge / edgeDotEdge));
Vector3Wide.Scale(edgeDirection, edgeScale, out var pointOnEdge);
Vector3Wide.Add(edgeStart, pointOnEdge, out pointOnEdge);
Vector3Wide.ConditionalSelect(outsideAnyEdge, pointOnEdge, localClosestOnTriangle, out localClosestOnTriangle);
}
if (Vector.EqualsAny(outsideAnyEdge, Vector <int> .Zero))
{
//p + N * (pa * N) / ||N||^2 = N * (pa * N) / ||N||^2 - (-p)
var nScale = paN / triangleNormalLengthSquared;
Vector3Wide.Scale(localTriangleNormal, nScale, out var offsetToPlane);
Vector3Wide.Subtract(offsetToPlane, localOffsetB, out var pointOnFace);
Vector3Wide.ConditionalSelect(outsideAnyEdge, localClosestOnTriangle, pointOnFace, out localClosestOnTriangle);
}
//normal = normalize(localOffsetA - localClosestOnTriangle) = (localOffsetB + localClosestOnTriangle) / (-||localOffsetB + localClosestOnTriangle||)
Vector3Wide.Add(localOffsetB, localClosestOnTriangle, out var localNormal);
Vector3Wide.Length(localNormal, out var localNormalLength);
Vector3Wide.Scale(localNormal, new Vector <float>(-1f) / localNormalLength, out localNormal);
Matrix3x3Wide.TransformWithoutOverlap(localNormal, rB, out normal);
Vector3Wide.Scale(normal, -a.Radius, out closestA);
distance = localNormalLength - a.Radius;
intersected = Vector.LessThanOrEqual(distance, Vector <float> .Zero);
}
public static void Prestep(ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref Vector3Wide normal, ref Contact2PrestepData prestep, float dt, float inverseDt,
out Projection projection)
{
//We directly take the prestep data here since the jacobians and error don't undergo any processing.
//The contact penetration constraint takes the form:
//dot(positionA + offsetA, N) >= dot(positionB + offsetB, N)
//Or:
//dot(positionA + offsetA, N) - dot(positionB + offsetB, N) >= 0
//dot(positionA + offsetA - positionB - offsetB, N) >= 0
//where positionA and positionB are the center of mass positions of the bodies offsetA and offsetB are world space offsets from the center of mass to the contact,
//and N is a unit length vector calibrated to point from B to A. (The normal pointing direction is important; it changes the sign.)
//In practice, we'll use the collision detection system's penetration depth instead of trying to recompute the error here.
//So, treating the normal as constant, the velocity constraint is:
//dot(d/dt(positionA + offsetA - positionB - offsetB), N) >= 0
//dot(linearVelocityA + d/dt(offsetA) - linearVelocityB - d/dt(offsetB)), N) >= 0
//The velocity of the offsets are defined by the angular velocity.
//dot(linearVelocityA + angularVelocityA x offsetA - linearVelocityB - angularVelocityB x offsetB), N) >= 0
//dot(linearVelocityA, N) + dot(angularVelocityA x offsetA, N) - dot(linearVelocityB, N) - dot(angularVelocityB x offsetB), N) >= 0
//Use the properties of the scalar triple product:
//dot(linearVelocityA, N) + dot(offsetA x N, angularVelocityA) - dot(linearVelocityB, N) - dot(offsetB x N, angularVelocityB) >= 0
//Bake in the negations:
//dot(linearVelocityA, N) + dot(offsetA x N, angularVelocityA) + dot(linearVelocityB, -N) + dot(-offsetB x N, angularVelocityB) >= 0
//A x B = -B x A:
//dot(linearVelocityA, N) + dot(offsetA x N, angularVelocityA) + dot(linearVelocityB, -N) + dot(N x offsetB, angularVelocityB) >= 0
//And there you go, the jacobians!
//linearA: N
//angularA: offsetA x N
//linearB: -N
//angularB: N x offsetB
//Note that we leave the penetration depth as is, even when it's negative. Speculative contacts!
Vector3Wide.CrossWithoutOverlap(prestep.OffsetA0, normal, out projection.Penetration0.AngularA);
Vector3Wide.Subtract(prestep.OffsetA0, prestep.OffsetB, out var offsetB0);
Vector3Wide.CrossWithoutOverlap(normal, offsetB0, out projection.Penetration0.AngularB);
Vector3Wide.CrossWithoutOverlap(prestep.OffsetA1, normal, out projection.Penetration1.AngularA);
Vector3Wide.Subtract(prestep.OffsetA1, prestep.OffsetB, out var offsetB1);
Vector3Wide.CrossWithoutOverlap(normal, offsetB1, out projection.Penetration1.AngularB);
//effective mass
Symmetric3x3Wide.VectorSandwich(projection.Penetration0.AngularA, inertiaA.InverseInertiaTensor, out var angularA0);
Symmetric3x3Wide.VectorSandwich(projection.Penetration0.AngularB, inertiaB.InverseInertiaTensor, out var angularB0);
Symmetric3x3Wide.VectorSandwich(projection.Penetration1.AngularA, inertiaA.InverseInertiaTensor, out var angularA1);
Symmetric3x3Wide.VectorSandwich(projection.Penetration1.AngularB, inertiaB.InverseInertiaTensor, out var angularB1);
//Linear effective mass contribution notes:
//1) The J * M^-1 * JT can be reordered to J * JT * M^-1 for the linear components, since M^-1 is a scalar and dot(n * scalar, n) = dot(n, n) * scalar.
//2) dot(normal, normal) == 1, so the contribution from each body is just its inverse mass.
SpringSettingsWide.ComputeSpringiness(ref prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale);
var linear = inertiaA.InverseMass + inertiaB.InverseMass;
//Note that we don't precompute the JT * effectiveMass term. Since the jacobians are shared, we have to do that multiply anyway.
projection.Penetration0.EffectiveMass = effectiveMassCFMScale / (linear + angularA0 + angularB0);
projection.Penetration1.EffectiveMass = effectiveMassCFMScale / (linear + angularA1 + angularB1);
//If depth is negative, the bias velocity will permit motion up until the depth hits zero. This works because positionErrorToVelocity * dt will always be <=1.
var inverseDtVector = new Vector <float>(inverseDt);
projection.Penetration0.BiasVelocity = Vector.Min(prestep.PenetrationDepth0 * inverseDtVector, Vector.Min(prestep.PenetrationDepth0 * positionErrorToVelocity, prestep.MaximumRecoveryVelocity));
projection.Penetration1.BiasVelocity = Vector.Min(prestep.PenetrationDepth1 * inverseDtVector, Vector.Min(prestep.PenetrationDepth1 * positionErrorToVelocity, prestep.MaximumRecoveryVelocity));
}
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 ExpandBoundingBoxes(ref Vector3Wide min, ref Vector3Wide max, ref BodyVelocities velocities, float dt,
ref Vector <float> maximumRadius, ref Vector <float> maximumAngularExpansion, ref Vector <float> maximumExpansion)
{
/*
* If an object sitting on a plane had a raw (unexpanded) AABB that is just barely above the plane, no contacts would be generated.
* If the velocity of the object would shove it down into the plane in the next frame, then it would generate contacts in the next frame- and,
* potentially, this cycle would repeat and cause jitter.
*
* To solve this, there are a couple of options:
* 1) Introduce an 'allowed penetration' so that objects can overlap a little bit. This tends to confuse people a little bit when they notice it,
* and in some circumstances objects can be seen settling into the allowed penetration slowly. It looks a bit odd.
* 2) Make contact constraints fight to maintain zero penetration depth, but expand the bounding box with velocity and allow contacts to be generated speculatively-
* contacts with negative penetration depth.
*
#2 is a form of continuous collision detection, but it's handy for general contact stability too.
* In this version of the engine, all objects generate speculative contacts by default, though only within a per-collidable-tuned 'speculative margin'.
* It's kind of like BEPUphysics v1's AllowedPenetration, except inverted. Speculative contacts that fall within the speculative margin-
* that is, those with negative depth of a magnitude less than the margin- are kept.
*
* So, a user could choose to have a very large speculative margin, and the speculative contact generation would provide a form of continuous collision detection.
* The main purpose, though, is just contact stability. With this in isolation, there's no strong reason to expand the bounding box more than the speculative margin.
* This is the 'discrete' mode.
*
* However, consider what would happen if an object A with high velocity and this 'discrete' mode was headed towards an object B in a 'continuous' mode.
* Object B only expands its bounding box by its own velocity, and object A doesn't expand beyond its speculative margin. The collision between A and B could easily be missed.
* To account for this, there is an intermediate mode- 'passive'- where the bounding box is allowed to expand beyond the margin,
* but no further continuous collision detection is performed.
*
* The fully continuous modes fully expand the bounding boxes. Notably, the inner sphere continuous collision detection mode could get by with less,
* but it would be pretty confusing to have the same kind of missed collision possibility if the other object in the pair was a substepping object.
* Two different inner sphere modes could be offered, but I'm unsure about the usefulness versus the complexity.
*
* (Note that there ARE situations where a bounding box which contains the full unconstrained motion will fail to capture constrained motion.
* Consider object A flying at high speed to impact the stationary object B, which sits next to another stationary object C.
* Object B's bounding box doesn't overlap with object C's bounding box- they're both stationary, so there's no velocity expansion. But during one frame,
* object A slams into B, and object B's velocity during that frame now forces it to tunnel all the way through C unimpeded, because no contacts were generated between B and C.
* There are ways to address this- all of which are a bit expensive- but CCD as implemented is not a hard guarantee.
* It's a 'best effort' that compromises with performance. Later on, if it's really necessary, we could consider harder guarantees with higher costs, but...
* given that no one seemed to have much of an issue with v1's rather limited CCD, it'll probably be fine.)
*
* So, how is the velocity expansion calculated?
* There's two parts, linear and angular.
*
* Linear is pretty simple- expand the bounding box in the direction of linear displacement (linearVelocity * dt).
*/
Vector <float> vectorDt = new Vector <float>(dt);
Vector3Wide.Scale(ref velocities.LinearVelocity, ref vectorDt, out var linearDisplacement);
var zero = Vector <float> .Zero;
Vector3Wide.Min(ref zero, ref linearDisplacement, out var minDisplacement);
Vector3Wide.Max(ref zero, ref linearDisplacement, out var maxDisplacement);
/*
* Angular requires a bit more care. Since the goal is to create a tight bound, simply using a v = w * r approximation isn't ideal. A slightly tighter can be found:
* 1) The maximum displacement along ANY axis during an intermediate time is equal to the distance from a starting position at MaximumRadius
* to the position of that point at the intermediate time.
* 2) The expansion cannot exceed the maximum radius, so angular deltas greater than pi/3 do not need to be considered.
* (An expansion equal to the maximum radius would result in an equilateral triangle, which has an angle of 60 degrees in each corner.)
* Larger values can simply be clamped.
* 3) The largest displacement along any axis, at any time, is the distance from the starting position to the position at dt. Note that this only holds because of the clamp:
* if the angle was allowed to wrap around, it the distance would start to go down again.
* 4) position(time) = {radius * sin(angular speed * time), radius * cos(angular speed * time)}
* 5) largest expansion required = ||position(dt) - position(0)|| = sqrt(2 * radius^2 * (1 - cos(dt * w)))
* 6) Don't have any true SIMD sin function, but we can approximate it using a taylor series, like: cos(x) = 1 - x^2 / 2! + x^4 / 4! - x^6 / 6!
* 7) Note that the cosine approximation should stop at a degree where it is smaller than the true value of cosine for the interval 0 to pi/3: this guarantees that the distance,
* which is larger when the cosine is smaller, is conservative and fully bounds the angular motion.
*
* Why do this extra work?
* 1) The bounding box calculation phase, as a part of the pose integration phase, tends to be severely memory bound.
* Spending a little of ALU time to get a smaller bounding box isn't a big concern, even though it includes a couple of sqrts.
* An extra few dozen ALU cycles is unlikely to meaningfully change the execution time.
* 2) Shrinking the bounding box reduces the number of collision pairs. Collision pairs are expensive- many times more expensive than the cost of shrinking the bounding box.
*/
Vector3Wide.Length(ref velocities.AngularVelocity, out var angularVelocityMagnitude);
var a = Vector.Min(angularVelocityMagnitude * vectorDt, new Vector <float>(MathHelper.Pi / 3f));
var a2 = a * a;
var a4 = a2 * a2;
var a6 = a4 * a2;
var cosAngleMinusOne = a2 * new Vector <float>(-1f / 2f) + a4 * new Vector <float>(1f / 24f) - a6 * new Vector <float>(1f / 720f);
//Note that it's impossible for angular motion to cause an increase in bounding box size beyond (maximumRadius-minimumRadius) on any given axis.
//That value, or a conservative approximation, is stored as the maximum angular expansion.
var angularExpansion = Vector.Min(maximumAngularExpansion,
Vector.SquareRoot(new Vector <float>(-2f) * maximumRadius * maximumRadius * cosAngleMinusOne));
Vector3Wide.Subtract(ref minDisplacement, ref angularExpansion, out minDisplacement);
Vector3Wide.Add(ref maxDisplacement, ref angularExpansion, out maxDisplacement);
//The maximum expansion passed into this function is the speculative margin for discrete mode collidables, and ~infinity for passive or continuous ones.
var negativeMaximum = -maximumExpansion;
Vector3Wide.Max(ref negativeMaximum, ref minDisplacement, out minDisplacement);
Vector3Wide.Min(ref maximumExpansion, ref maxDisplacement, out maxDisplacement);
Vector3Wide.Add(ref min, ref minDisplacement, out min);
Vector3Wide.Add(ref max, ref maxDisplacement, out max);
//Note that this is an area that will need to adapt to changes to the body pose representation. If you use a 32 bit fixed point position or a 64 bit double position,
//the bounding box calculation needs to be aware. (In the more extreme cases, so does the broad phase. The amount of conservative padding needed for a 32 bit bounding box
//with 64 bit positions is silly.)
}
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 CapsuleWide b, ref Vector <float> speculativeMargin,
ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount,
out Convex2ContactManifoldWide manifold)
{
//Compute the closest points between the two line segments. No clamping to begin with.
//We want to minimize distance = ||(a + da * ta) - (b + db * tb)||.
//Taking the derivative with respect to ta and doing some algebra (taking into account ||da|| == ||db|| == 1) to solve for ta yields:
//ta = (da * (b - a) + (db * (a - b)) * (da * db)) / (1 - ((da * db) * (da * db))
QuaternionWide.TransformUnitXY(orientationA, out var xa, out var da);
QuaternionWide.TransformUnitY(orientationB, out var db);
Vector3Wide.Dot(da, offsetB, out var daOffsetB);
Vector3Wide.Dot(db, offsetB, out var dbOffsetB);
Vector3Wide.Dot(da, db, out var dadb);
//Note potential division by zero when the axes are parallel. Arbitrarily clamp; near zero values will instead produce extreme values which get clamped to reasonable results.
var ta = (daOffsetB - dbOffsetB * dadb) / Vector.Max(new Vector <float>(1e-15f), Vector <float> .One - dadb * dadb);
//tb = ta * (da * db) - db * (b - a)
var tb = ta * dadb - dbOffsetB;
//We cannot simply clamp the ta and tb values to the capsule line segments. Instead, project each line segment onto the other line segment, clamping against the target's interval.
//That new clamped projected interval is the valid solution space on that line segment. We can clamp the t value by that interval to get the correctly bounded solution.
//The projected intervals are:
//B onto A: +-BHalfLength * (da * db) + da * offsetB
//A onto B: +-AHalfLength * (da * db) - db * offsetB
var absdadb = Vector.Abs(dadb);
var bOntoAOffset = b.HalfLength * absdadb;
var aOntoBOffset = a.HalfLength * absdadb;
var aMin = Vector.Max(-a.HalfLength, Vector.Min(a.HalfLength, daOffsetB - bOntoAOffset));
var aMax = Vector.Min(a.HalfLength, Vector.Max(-a.HalfLength, daOffsetB + bOntoAOffset));
var bMin = Vector.Max(-b.HalfLength, Vector.Min(b.HalfLength, -aOntoBOffset - dbOffsetB));
var bMax = Vector.Min(b.HalfLength, Vector.Max(-b.HalfLength, aOntoBOffset - dbOffsetB));
ta = Vector.Min(Vector.Max(ta, aMin), aMax);
tb = Vector.Min(Vector.Max(tb, bMin), bMax);
Vector3Wide.Scale(da, ta, out var closestPointOnA);
Vector3Wide.Scale(db, tb, out var closestPointOnB);
Vector3Wide.Add(closestPointOnB, offsetB, out closestPointOnB);
//Note that normals are calibrated to point from B to A by convention.
Vector3Wide.Subtract(closestPointOnA, closestPointOnB, out manifold.Normal);
Vector3Wide.Length(manifold.Normal, out var distance);
var inverseDistance = Vector <float> .One / distance;
Vector3Wide.Scale(manifold.Normal, inverseDistance, out manifold.Normal);
//In the event that the line segments are touching, the normal doesn't exist and we need an alternative. Any direction along the local horizontal (XZ) plane of either capsule
//is valid. (Normals along the local Y axes are not guaranteed to be as quick of a path to separation due to nonzero line length.)
var normalIsValid = Vector.GreaterThan(distance, new Vector <float>(1e-7f));
Vector3Wide.ConditionalSelect(normalIsValid, manifold.Normal, xa, out manifold.Normal);
//In the event that the two capsule axes are coplanar, we accept the whole interval as a source of contact.
//As the axes drift away from coplanarity, the accepted interval rapidly narrows to zero length, centered on ta and tb.
//We rate the degree of coplanarity based on the angle between the capsule axis and the plane defined by the opposing segment and contact normal:
//sin(angle) = dot(da, (db x normal)/||db x normal||)
//Finally, note that we are dealing with extremely small angles, and for small angles sin(angle) ~= angle,
//and also that fade behavior is completely arbitrary, so we can directly use squared angle without any concern.
//angle^2 ~= dot(da, (db x normal))^2 / ||db x normal||^2
//Note that if ||db x normal|| is zero, then any da should be accepted as being coplanar because there is no restriction. ConditionalSelect away the discontinuity.
Vector3Wide.CrossWithoutOverlap(db, manifold.Normal, out var planeNormal);
Vector3Wide.LengthSquared(planeNormal, out var planeNormalLengthSquared);
Vector3Wide.Dot(da, planeNormal, out var numeratorUnsquared);
var squaredAngle = Vector.ConditionalSelect(Vector.LessThan(planeNormalLengthSquared, new Vector <float>(1e-10f)), Vector <float> .Zero, numeratorUnsquared * numeratorUnsquared / planeNormalLengthSquared);
//Convert the squared angle to a lerp parameter. For squared angle from 0 to lowerThreshold, we should use the full interval (1). From lowerThreshold to upperThreshold, lerp to 0.
const float lowerThresholdAngle = 0.01f;
const float upperThresholdAngle = 0.05f;
const float lowerThreshold = lowerThresholdAngle * lowerThresholdAngle;
const float upperThreshold = upperThresholdAngle * upperThresholdAngle;
var intervalWeight = Vector.Max(Vector <float> .Zero, Vector.Min(Vector <float> .One, (new Vector <float>(upperThreshold) - squaredAngle) * new Vector <float>(1f / (upperThreshold - lowerThreshold))));
//If the line segments intersect, even if they're coplanar, we would ideally stick to using a single point. Would be easy enough,
//but we don't bother because it's such a weird and extremely temporary corner case. Not really worth handling.
var weightedTa = ta - ta * intervalWeight;
aMin = intervalWeight * aMin + weightedTa;
aMax = intervalWeight * aMax + weightedTa;
Vector3Wide.Scale(da, aMin, out manifold.OffsetA0);
Vector3Wide.Scale(da, aMax, out manifold.OffsetA1);
//In the coplanar case, there are two points. We need a method of computing depth which gives a reasonable result to the second contact.
//Note that one of the two contacts should end up with a distance equal to the previously computed segment distance, so we're doing some redundant work here.
//It's just easier to do that extra work than it would be to track which endpoint contributed the lower distance.
//Unproject the final interval endpoints from a back onto b.
//dot(offsetB + db * tb0, da) = ta0
//tb0 = (ta0 - daOffsetB) / dadb
//distance0 = dot(a0 - (offsetB + tb0 * db), normal)
//distance1 = dot(a1 - (offsetB + tb1 * db), normal)
Vector3Wide.Dot(db, manifold.Normal, out var dbNormal);
Vector3Wide.Subtract(manifold.OffsetA0, offsetB, out var offsetB0);
Vector3Wide.Subtract(manifold.OffsetA1, offsetB, out var offsetB1);
//Note potential division by zero. In that case, treat both projected points as the closest point. (Handled by the conditional select that chooses the previously computed distance.)
var inverseDadb = Vector <float> .One / dadb;
var projectedTb0 = Vector.Max(bMin, Vector.Min(bMax, (aMin - daOffsetB) * inverseDadb));
var projectedTb1 = Vector.Max(bMin, Vector.Min(bMax, (aMax - daOffsetB) * inverseDadb));
Vector3Wide.Dot(offsetB0, manifold.Normal, out var b0Normal);
Vector3Wide.Dot(offsetB1, manifold.Normal, out var b1Normal);
var capsulesArePerpendicular = Vector.LessThan(Vector.Abs(dadb), new Vector <float>(1e-7f));
var distance0 = Vector.ConditionalSelect(capsulesArePerpendicular, distance, b0Normal - dbNormal * projectedTb0);
var distance1 = Vector.ConditionalSelect(capsulesArePerpendicular, distance, b1Normal - dbNormal * projectedTb1);
var combinedRadius = a.Radius + b.Radius;
manifold.Depth0 = combinedRadius - distance0;
manifold.Depth1 = combinedRadius - distance1;
//Apply the normal offset to the contact positions.
var negativeOffsetFromA0 = manifold.Depth0 * 0.5f - a.Radius;
var negativeOffsetFromA1 = manifold.Depth1 * 0.5f - a.Radius;
Vector3Wide.Scale(manifold.Normal, negativeOffsetFromA0, out var normalPush0);
Vector3Wide.Scale(manifold.Normal, negativeOffsetFromA1, out var normalPush1);
Vector3Wide.Add(manifold.OffsetA0, normalPush0, out manifold.OffsetA0);
Vector3Wide.Add(manifold.OffsetA1, normalPush1, out manifold.OffsetA1);
manifold.FeatureId0 = Vector <int> .Zero;
manifold.FeatureId1 = Vector <int> .One;
var minimumAcceptedDepth = -speculativeMargin;
manifold.Contact0Exists = Vector.GreaterThanOrEqual(manifold.Depth0, minimumAcceptedDepth);
manifold.Contact1Exists = Vector.BitwiseAnd(
Vector.GreaterThanOrEqual(manifold.Depth1, minimumAcceptedDepth),
Vector.GreaterThan(aMax - aMin, new Vector <float>(1e-7f) * a.HalfLength));
//TODO: Since we added in the complexity of 2 contact support, this is probably large enough to benefit from working in the local space of one of the capsules.
//Worth looking into later.
}
public 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 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 void GetPoseOffset(out Vector3Wide offsetB)
{
Vector3Wide.Subtract(ref PositionB, ref PositionA, out offsetB);
}