internal static void Prepare(
ref CapsuleWide a, ref BoxWide b, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB,
out Vector3Wide localOffsetA, out Vector3Wide capsuleAxis, out Vector3Wide edgeCenters)
{
QuaternionWide.Conjugate(orientationB, out var toLocalB);
QuaternionWide.TransformWithoutOverlap(offsetB, toLocalB, out localOffsetA);
Vector3Wide.Negate(ref localOffsetA);
QuaternionWide.ConcatenateWithoutOverlap(orientationA, toLocalB, out var boxLocalOrientationA);
QuaternionWide.TransformUnitY(boxLocalOrientationA, out capsuleAxis);
//Get the closest point on the capsule segment to the box center to choose which edge to use.
//(Pointless to test the other 9; they're guaranteed to be further away.)
//closestPointOnCapsuleToBoxPosition = clamp((boxPosition - capsulePosition) * capsuleAxis, halfLength) * capsuleAxis + capsulePosition
//offsetFromBoxToCapsule = closestPointOnCapsuleToBoxPosition - boxPosition
//offsetFromBoxToCapsule = clamp(-localOffsetA * capsuleAxis, halfLength) * capsuleAxis + localOffsetA
//offsetFromBoxToCapsule = localOffsetA - clamp(localOffsetA * capsuleAxis, halfLength) * capsuleAxis
Vector3Wide.Dot(localOffsetA, capsuleAxis, out var dot);
var clampedDot = Vector.Min(a.HalfLength, Vector.Max(-a.HalfLength, dot));
Vector3Wide.Scale(capsuleAxis, clampedDot, out var offsetToCapsuleFromBox);
Vector3Wide.Subtract(localOffsetA, offsetToCapsuleFromBox, out offsetToCapsuleFromBox);
edgeCenters.X = Vector.ConditionalSelect(Vector.LessThan(offsetToCapsuleFromBox.X, Vector <float> .Zero), -b.HalfWidth, b.HalfWidth);
edgeCenters.Y = Vector.ConditionalSelect(Vector.LessThan(offsetToCapsuleFromBox.Y, Vector <float> .Zero), -b.HalfHeight, b.HalfHeight);
edgeCenters.Z = Vector.ConditionalSelect(Vector.LessThan(offsetToCapsuleFromBox.Z, Vector <float> .Zero), -b.HalfLength, b.HalfLength);
}
public void Test(ref SphereWide a, ref CapsuleWide b, ref Vector <float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex1ContactManifoldWide manifold)
{
//The contact for a sphere-capsule pair is based on the closest point of the sphere center to the capsule internal line segment.
QuaternionWide.TransformUnitXY(orientationB, out var x, out var y);
Vector3Wide.Dot(y, offsetB, out var t);
t = Vector.Min(b.HalfLength, Vector.Max(-b.HalfLength, -t));
Vector3Wide.Scale(y, t, out var capsuleLocalClosestPointOnLineSegment);
Vector3Wide.Add(offsetB, capsuleLocalClosestPointOnLineSegment, out var sphereToInternalSegment);
Vector3Wide.Length(sphereToInternalSegment, out var internalDistance);
//Note that the normal points from B to A by convention. Here, the sphere is A, the capsule is B, so the normalization requires a negation.
var inverseDistance = new Vector <float>(-1f) / internalDistance;
Vector3Wide.Scale(sphereToInternalSegment, inverseDistance, out manifold.Normal);
var normalIsValid = Vector.GreaterThan(internalDistance, Vector <float> .Zero);
//If the center of the sphere is on the internal line segment, then choose a direction on the plane defined by the capsule's up vector.
//We computed one such candidate earlier. Note that we could usually get away with choosing a completely arbitrary direction, but
//going through the extra effort to compute a true local horizontal direction avoids some nasty corner case surprises if a user is trying
//to do something like manually resolving collisions or other query-based logic.
//A cheaper option would be to simply use the y axis as the normal. That's known to be suboptimal, but if we don't guarantee minimum penetration depth, that's totally fine.
//My guess is that computing x will be so cheap as to be irrelevant.
Vector3Wide.ConditionalSelect(normalIsValid, manifold.Normal, x, out manifold.Normal);
manifold.Depth = a.Radius + b.Radius - internalDistance;
manifold.FeatureId = Vector <int> .Zero;
//The contact position relative to object A (the sphere) is computed as the average of the extreme point along the normal toward the opposing shape on each shape, averaged.
//For capsule-sphere, this can be computed from the normal and depth.
var negativeOffsetFromSphere = manifold.Depth * 0.5f - a.Radius;
Vector3Wide.Scale(manifold.Normal, negativeOffsetFromSphere, out manifold.OffsetA);
manifold.ContactExists = Vector.GreaterThan(manifold.Depth, -speculativeMargin);
}
static void TestEndpointNormal(ref Vector3Wide offsetA, ref Vector3Wide capsuleAxis, ref Vector <float> capsuleHalfLength, ref Vector3Wide endpoint,
ref BoxWide box, out Vector <float> depth, out Vector3Wide normal)
{
Vector3Wide clamped;
clamped.X = Vector.Min(box.HalfWidth, Vector.Max(-box.HalfWidth, endpoint.X));
clamped.Y = Vector.Min(box.HalfHeight, Vector.Max(-box.HalfHeight, endpoint.Y));
clamped.Z = Vector.Min(box.HalfLength, Vector.Max(-box.HalfLength, endpoint.Z));
Vector3Wide.Subtract(endpoint, clamped, out normal);
Vector3Wide.Length(normal, out var length);
var inverseLength = Vector <float> .One / length;
Vector3Wide.Scale(normal, inverseLength, out normal);
//The dot between the offset from B to A and the normal gives us the center offset.
//The dot between the capsule axis and normal gives us the (unscaled) extent of the capsule along the normal.
//The depth is (boxExtentAlongNormal + capsuleExtentAlongNormal) - separationAlongNormal.
Vector3Wide.Dot(offsetA, normal, out var baN);
Vector3Wide.Dot(capsuleAxis, normal, out var daN);
depth =
Vector.Abs(normal.X) * box.HalfWidth + Vector.Abs(normal.Y) * box.HalfHeight + Vector.Abs(normal.Z) * box.HalfLength +
Vector.Abs(daN * capsuleHalfLength) -
Vector.Abs(baN);
//If the endpoint doesn't generate a valid normal due to containment, ignore the depth result.
depth = Vector.ConditionalSelect(Vector.GreaterThan(length, new Vector <float>(1e-10f)), depth, new Vector <float>(float.MaxValue));
}
public void ComputeSupport(ref CapsuleWide shape, ref Matrix3x3Wide orientation, ref Vector3Wide direction, out Vector3Wide support)
{
Vector3Wide.Scale(orientation.Y, shape.HalfLength, out support);
Vector3Wide.Negate(support, out var negated);
Vector3Wide.Dot(orientation.Y, direction, out var dot);
var shouldNegate = Vector.LessThan(dot, Vector <float> .Zero);
Vector3Wide.ConditionalSelect(shouldNegate, negated, support, out support);
}
public void ComputeSupport(ref TriangleWide shape, ref Matrix3x3Wide orientation, ref Vector3Wide direction, out Vector3Wide support)
{
Matrix3x3Wide.TransformByTransposedWithoutOverlap(direction, orientation, out var localDirection);
Vector3Wide.Dot(shape.A, localDirection, out var a);
Vector3Wide.Dot(shape.B, localDirection, out var b);
Vector3Wide.Dot(shape.C, localDirection, out var c);
var max = Vector.Max(a, Vector.Max(b, c));
Vector3Wide.ConditionalSelect(Vector.Equals(max, a), shape.A, shape.B, out var localSupport);
Vector3Wide.ConditionalSelect(Vector.Equals(max, c), shape.C, localSupport, out localSupport);
Matrix3x3Wide.TransformWithoutOverlap(localSupport, orientation, out support);
}
static void GetEdgeClosestPoint(ref Vector3Wide normal, ref Vector <int> edgeDirectionIndex,
ref BoxWide box,
ref Vector3Wide offsetA, ref Vector3Wide capsuleAxis, ref Vector <float> capsuleHalfLength, out Vector3Wide closestPointFromEdge)
{
Vector3Wide.Dot(normal, offsetA, out var calibrationDot);
var flipNormal = Vector.LessThan(calibrationDot, Vector <float> .Zero);
normal.X = Vector.ConditionalSelect(flipNormal, -normal.X, normal.X);
normal.Y = Vector.ConditionalSelect(flipNormal, -normal.Y, normal.Y);
normal.Z = Vector.ConditionalSelect(flipNormal, -normal.Z, normal.Z);
Vector3Wide boxEdgeCenter;
//Note that this can result in a closest point in the middle of the box when the capsule is parallel to a face. That's fine.
boxEdgeCenter.X = Vector.ConditionalSelect(Vector.Equals(normal.X, Vector <float> .Zero), Vector <float> .Zero,
Vector.ConditionalSelect(Vector.GreaterThan(normal.X, Vector <float> .Zero), box.HalfWidth, -box.HalfWidth));
boxEdgeCenter.Y = Vector.ConditionalSelect(Vector.Equals(normal.Y, Vector <float> .Zero), Vector <float> .Zero,
Vector.ConditionalSelect(Vector.GreaterThan(normal.Y, Vector <float> .Zero), box.HalfHeight, -box.HalfHeight));
boxEdgeCenter.Z = Vector.ConditionalSelect(Vector.Equals(normal.Z, Vector <float> .Zero), Vector <float> .Zero,
Vector.ConditionalSelect(Vector.GreaterThan(normal.Z, Vector <float> .Zero), box.HalfLength, -box.HalfLength));
Vector3Wide boxEdgeDirection;
var useEdgeX = Vector.Equals(edgeDirectionIndex, Vector <int> .Zero);
var useEdgeY = Vector.Equals(edgeDirectionIndex, Vector <int> .One);
var useEdgeZ = Vector.Equals(edgeDirectionIndex, new Vector <int>(2));
boxEdgeDirection.X = Vector.ConditionalSelect(useEdgeX, Vector <float> .One, Vector <float> .Zero);
boxEdgeDirection.Y = Vector.ConditionalSelect(useEdgeY, Vector <float> .One, Vector <float> .Zero);
boxEdgeDirection.Z = Vector.ConditionalSelect(useEdgeZ, Vector <float> .One, Vector <float> .Zero);
//From CapsulePairCollisionTask, point of closest approach along the capsule axis, unbounded:
//ta = (da * (b - a) + (db * (a - b)) * (da * db)) / (1 - ((da * db) * (da * db))
//where da = capsuleAxis, db = boxEdgeDirection, a = offsetA, b = boxEdgeCenter
Vector3Wide.Subtract(boxEdgeCenter, offsetA, out var ab);
Vector3Wide.Dot(ab, capsuleAxis, out var abda);
Vector3Wide.Dot(ab, boxEdgeDirection, out var abdb);
Vector3Wide.Dot(capsuleAxis, boxEdgeDirection, out var dadb);
//Note division by zero guard.
var ta = (abda - abdb * dadb) / Vector.Max(new Vector <float>(1e-15f), (Vector <float> .One - dadb * dadb));
//In some cases, ta won't be in a useful location. Need to constrain it to the projection of the box edge onto the capsule edge.
//B onto A: +-BHalfExtent * (da * db) + da * ab
var bHalfExtent = Vector.ConditionalSelect(useEdgeX, box.HalfWidth, Vector.ConditionalSelect(useEdgeY, box.HalfHeight, box.HalfLength));
var bOntoAOffset = bHalfExtent * Vector.Abs(dadb);
var taMin = Vector.Max(-capsuleHalfLength, Vector.Min(capsuleHalfLength, abda - bOntoAOffset));
var taMax = Vector.Min(capsuleHalfLength, Vector.Max(-capsuleHalfLength, abda + bOntoAOffset));
ta = Vector.Min(Vector.Max(ta, taMin), taMax);
Vector3Wide.Scale(capsuleAxis, ta, out var offsetAlongCapsule);
Vector3Wide.Add(offsetA, offsetAlongCapsule, out closestPointFromEdge);
}
public static void ComputeCorrectiveImpulse(ref Vector3Wide angularJacobianA, ref TwistFrictionProjection projection,
ref BodyVelocities wsvA, ref Vector <float> maximumImpulse,
ref Vector <float> accumulatedImpulse, out Vector <float> correctiveCSI)
{
Vector3Wide.Dot(wsvA.Angular, angularJacobianA, out var csvA);
var negativeCSI = csvA * projection.EffectiveMass; //Since there is no bias or softness to give us the negative, we just do it when we apply to the accumulated impulse.
var previousAccumulated = accumulatedImpulse;
//The maximum force of friction depends upon the normal impulse.
accumulatedImpulse = Vector.Min(maximumImpulse, Vector.Max(-maximumImpulse, accumulatedImpulse - negativeCSI));
correctiveCSI = accumulatedImpulse - previousAccumulated;
}
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 ComputeCorrectiveImpulse(ref BodyVelocities wsvA,
ref PenetrationLimitOneBodyProjection projection,
ref Vector3Wide normal, ref Vector <float> softnessImpulseScale,
ref Vector <float> accumulatedImpulse, out Vector <float> correctiveCSI)
{
//Note that we do NOT use pretransformed jacobians here; the linear jacobian sharing (normal) meant that we had the effective mass anyway.
Vector3Wide.Dot(wsvA.Linear, normal, out var csvaLinear);
Vector3Wide.Dot(wsvA.Angular, projection.AngularA, out var csvaAngular);
//Compute negated version to avoid the need for an explicit negate.
var negatedCSI = accumulatedImpulse * softnessImpulseScale + (csvaLinear + csvaAngular - projection.BiasVelocity) * projection.EffectiveMass;
var previousAccumulated = accumulatedImpulse;
accumulatedImpulse = Vector.Max(Vector <float> .Zero, accumulatedImpulse - negatedCSI);
correctiveCSI = accumulatedImpulse - previousAccumulated;
}
public void Test(ref SphereWide a, ref CapsuleWide b, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB,
out Vector <int> intersected, out Vector <float> distance, out Vector3Wide closestA, out Vector3Wide normal)
{
//The contact for a sphere-capsule pair is based on the closest point of the sphere center to the capsule internal line segment.
QuaternionWide.TransformUnitXY(orientationB, out var x, out var y);
Vector3Wide.Dot(y, offsetB, out var t);
t = Vector.Min(b.HalfLength, Vector.Max(-b.HalfLength, -t));
Vector3Wide.Scale(y, t, out var capsuleLocalClosestPointOnLineSegment);
Vector3Wide.Add(offsetB, capsuleLocalClosestPointOnLineSegment, out var sphereToInternalSegment);
Vector3Wide.Length(sphereToInternalSegment, out var internalDistance);
//Note that the normal points from B to A by convention. Here, the sphere is A, the capsule is B, so the normalization requires a negation.
var inverseDistance = new Vector <float>(-1f) / internalDistance;
Vector3Wide.Scale(sphereToInternalSegment, inverseDistance, out normal);
var surfaceOffset = -a.Radius;
Vector3Wide.Scale(normal, surfaceOffset, out closestA);
distance = internalDistance - a.Radius - b.Radius;
intersected = Vector.LessThanOrEqual(distance, Vector <float> .Zero);
}
public static void ComputeCorrectiveImpulse(ref BodyVelocities wsvA, ref BodyVelocities wsvB, ref Projection2Body1DOF projection, ref Vector <float> accumulatedImpulse,
out Vector <float> correctiveCSI)
{
//Take the world space velocity of each body into constraint space by transforming by the transpose(jacobian).
//(The jacobian is a row vector by convention, while we treat our velocity vectors as a 12x1 row vector for the purposes of constraint space velocity calculation.
//So we are multiplying v * JT.)
//Then, transform it into an impulse by applying the effective mass.
//Here, we combine the projection and impulse conversion into a precomputed value, i.e. v * (JT * softenedEffectiveMass).
Vector3Wide.Dot(wsvA.Linear, projection.WSVtoCSILinearA, out var csiaLinear);
Vector3Wide.Dot(wsvA.Angular, projection.WSVtoCSIAngularA, out var csiaAngular);
Vector3Wide.Dot(wsvB.Linear, projection.WSVtoCSILinearB, out var csibLinear);
Vector3Wide.Dot(wsvB.Angular, projection.WSVtoCSIAngularB, out var csibAngular);
//Combine it all together, following:
//constraint space impulse = (targetVelocity - currentVelocity) * softenedEffectiveMass
//constraint space impulse = (bias - accumulatedImpulse * softness - wsv * JT) * softenedEffectiveMass
//constraint space impulse = (bias * softenedEffectiveMass) - accumulatedImpulse * (softness * softenedEffectiveMass) - wsv * (JT * softenedEffectiveMass)
var csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular);
var previousAccumulated = accumulatedImpulse;
accumulatedImpulse = Vector.Max(Vector <float> .Zero, accumulatedImpulse + csi);
correctiveCSI = accumulatedImpulse - previousAccumulated;
}
public void Test(ref CapsuleWide a, ref BoxWide b, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB,
out Vector <int> intersected, out Vector <float> distance, out Vector3Wide closestA, out Vector3Wide normal)
{
//Bring the capsule into the box's local space.
Matrix3x3Wide.CreateFromQuaternion(orientationB, out var rB);
QuaternionWide.TransformUnitY(orientationA, out var capsuleAxis);
Matrix3x3Wide.TransformByTransposedWithoutOverlap(capsuleAxis, rB, out var localCapsuleAxis);
Matrix3x3Wide.TransformByTransposedWithoutOverlap(offsetB, rB, out var localOffsetB);
Vector3Wide.Negate(localOffsetB, out var localOffsetA);
Vector3Wide.Scale(localCapsuleAxis, a.HalfLength, out var endpointOffset);
Vector3Wide.Subtract(localOffsetA, endpointOffset, out var endpoint0);
TestEndpointNormal(ref localOffsetA, ref localCapsuleAxis, ref a.HalfLength, ref endpoint0, ref b, out var depth, out var localNormal);
Vector3Wide.Add(localOffsetA, endpointOffset, out var endpoint1);
TestEndpointNormal(ref localOffsetA, ref localCapsuleAxis, ref a.HalfLength, ref endpoint1, ref b, out var depthCandidate, out var localNormalCandidate);
Select(ref depth, ref localNormal, ref depthCandidate, ref localNormalCandidate);
//Note that we did not yet pick a closest point for endpoint cases. That's because each case only generates a normal and interval test, not a minimal distance test.
//The choice of which endpoint is actually closer is deferred until now.
Vector3Wide.Dot(localCapsuleAxis, localNormal, out var endpointChoiceDot);
Vector3Wide.ConditionalSelect(Vector.LessThan(endpointChoiceDot, Vector <float> .Zero), endpoint1, endpoint0, out var localClosest);
Vector3Wide edgeLocalNormal, edgeLocalNormalCandidate;
//Swizzle XYZ -> YZX
TestBoxEdge(ref localOffsetA.Y, ref localOffsetA.Z, ref localOffsetA.X,
ref localCapsuleAxis.Y, ref localCapsuleAxis.Z, ref localCapsuleAxis.X,
ref b.HalfHeight, ref b.HalfLength, ref b.HalfWidth,
out var edgeDepth, out edgeLocalNormal.Y, out edgeLocalNormal.Z, out edgeLocalNormal.X);
var edgeDirectionIndex = Vector <int> .Zero;
//Swizzle XYZ -> ZXY
TestBoxEdge(ref localOffsetA.Z, ref localOffsetA.X, ref localOffsetA.Y,
ref localCapsuleAxis.Z, ref localCapsuleAxis.X, ref localCapsuleAxis.Y,
ref b.HalfLength, ref b.HalfWidth, ref b.HalfHeight,
out var edgeDepthCandidate, out edgeLocalNormalCandidate.Z, out edgeLocalNormalCandidate.X, out edgeLocalNormalCandidate.Y);
SelectForEdge(ref edgeDepth, ref edgeLocalNormal, ref edgeDirectionIndex, ref edgeDepthCandidate, ref edgeLocalNormalCandidate, Vector <int> .One);
//Swizzle XYZ -> XYZ
TestBoxEdge(ref localOffsetA.X, ref localOffsetA.Y, ref localOffsetA.Z,
ref localCapsuleAxis.X, ref localCapsuleAxis.Y, ref localCapsuleAxis.Z,
ref b.HalfWidth, ref b.HalfHeight, ref b.HalfLength,
out edgeDepthCandidate, out edgeLocalNormalCandidate.X, out edgeLocalNormalCandidate.Y, out edgeLocalNormalCandidate.Z);
SelectForEdge(ref edgeDepth, ref edgeLocalNormal, ref edgeDirectionIndex, ref edgeDepthCandidate, ref edgeLocalNormalCandidate, new Vector <int>(2));
//We can skip the edge finalization if they aren't ever used.
if (Vector.LessThanAny(edgeDepth, depth))
{
GetEdgeClosestPoint(ref edgeLocalNormal, ref edgeDirectionIndex, ref b, ref localOffsetA, ref localCapsuleAxis, ref a.HalfLength, out var edgeLocalClosest);
Select(ref depth, ref localNormal, ref localClosest, ref edgeDepth, ref edgeLocalNormal, ref edgeLocalClosest);
}
TestVertexAxis(ref b, ref localOffsetA, ref localCapsuleAxis, ref a.HalfLength, out depthCandidate, out localNormalCandidate, out var localClosestCandidate);
Select(ref depth, ref localNormal, ref localClosest, ref depthCandidate, ref localNormalCandidate, ref localClosestCandidate);
//Transform normal and closest point back into world space.
Matrix3x3Wide.TransformWithoutOverlap(localNormal, rB, out normal);
Matrix3x3Wide.TransformWithoutOverlap(localClosest, rB, out closestA);
Vector3Wide.Add(closestA, offsetB, out closestA);
Vector3Wide.Scale(normal, a.Radius, out var closestOffset);
Vector3Wide.Subtract(closestA, closestOffset, out closestA);
distance = -depth - a.Radius;
intersected = Vector.LessThan(distance, Vector <float> .Zero);
}
public unsafe void Test(ref TriangleWide a, ref ConvexHullWide b, ref Vector <float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, out Convex4ContactManifoldWide manifold)
{
Matrix3x3Wide.CreateFromQuaternion(orientationA, out var triangleOrientation);
Matrix3x3Wide.CreateFromQuaternion(orientationB, out var hullOrientation);
Matrix3x3Wide.MultiplyByTransposeWithoutOverlap(triangleOrientation, hullOrientation, out var hullLocalTriangleOrientation);
Matrix3x3Wide.TransformByTransposedWithoutOverlap(offsetB, hullOrientation, out var localOffsetB);
Vector3Wide.Negate(localOffsetB, out var localOffsetA);
TriangleWide triangle;
Matrix3x3Wide.TransformWithoutOverlap(a.A, hullLocalTriangleOrientation, out triangle.A);
Matrix3x3Wide.TransformWithoutOverlap(a.B, hullLocalTriangleOrientation, out triangle.B);
Matrix3x3Wide.TransformWithoutOverlap(a.C, hullLocalTriangleOrientation, out triangle.C);
Vector3Wide.Add(triangle.A, triangle.B, out var centroid);
Vector3Wide.Add(triangle.C, centroid, out centroid);
Vector3Wide.Scale(centroid, new Vector <float>(1f / 3f), out centroid);
Vector3Wide.Subtract(triangle.A, centroid, out triangle.A);
Vector3Wide.Subtract(triangle.B, centroid, out triangle.B);
Vector3Wide.Subtract(triangle.C, centroid, out triangle.C);
Vector3Wide.Subtract(centroid, localOffsetB, out var localTriangleCenter);
Vector3Wide.Subtract(triangle.B, triangle.A, out var triangleAB);
Vector3Wide.Subtract(triangle.C, triangle.B, out var triangleBC);
Vector3Wide.Subtract(triangle.A, triangle.C, out var triangleCA);
//We'll be using B-local triangle vertices quite a bit, so cache them.
Vector3Wide.Add(triangle.A, localTriangleCenter, out var triangleA);
Vector3Wide.Add(triangle.B, localTriangleCenter, out var triangleB);
Vector3Wide.Add(triangle.C, localTriangleCenter, out var triangleC);
Vector3Wide.CrossWithoutOverlap(triangleAB, triangleCA, out var triangleNormal);
Vector3Wide.Length(triangleNormal, out var triangleNormalLength);
Vector3Wide.Scale(triangleNormal, Vector <float> .One / triangleNormalLength, out triangleNormal);
//Check if the hull's position is within the triangle and below the triangle plane. If so, we can ignore it.
Vector3Wide.Dot(triangleNormal, localTriangleCenter, out var hullToTriangleCenterDot);
var hullBelowPlane = Vector.GreaterThanOrEqual(hullToTriangleCenterDot, Vector <float> .Zero);
Vector <int> hullInsideAndBelowTriangle;
Vector3Wide.CrossWithoutOverlap(triangleAB, triangleNormal, out var edgePlaneAB);
Vector3Wide.CrossWithoutOverlap(triangleBC, triangleNormal, out var edgePlaneBC);
Vector3Wide.CrossWithoutOverlap(triangleCA, triangleNormal, out var edgePlaneCA);
if (Vector.LessThanAny(hullBelowPlane, Vector <int> .Zero))
{
//Is the hull position within the triangle bounds?
Vector3Wide.Dot(edgePlaneAB, triangleA, out var abPlaneTest);
Vector3Wide.Dot(edgePlaneBC, triangleB, out var bcPlaneTest);
Vector3Wide.Dot(edgePlaneCA, triangleC, out var caPlaneTest);
hullInsideAndBelowTriangle = Vector.BitwiseAnd(
Vector.BitwiseAnd(hullBelowPlane, Vector.LessThanOrEqual(abPlaneTest, Vector <float> .Zero)),
Vector.BitwiseAnd(Vector.LessThanOrEqual(bcPlaneTest, Vector <float> .Zero), Vector.LessThanOrEqual(caPlaneTest, Vector <float> .Zero)));
}
else
{
hullInsideAndBelowTriangle = Vector <int> .Zero;
}
ManifoldCandidateHelper.CreateInactiveMask(pairCount, out var inactiveLanes);
a.EstimateEpsilonScale(out var triangleEpsilonScale);
b.EstimateEpsilonScale(inactiveLanes, out var hullEpsilonScale);
var epsilonScale = Vector.Min(triangleEpsilonScale, hullEpsilonScale);
inactiveLanes = Vector.BitwiseOr(inactiveLanes, Vector.LessThan(triangleNormalLength, epsilonScale * 1e-6f));
inactiveLanes = Vector.BitwiseOr(inactiveLanes, hullInsideAndBelowTriangle);
//Not every lane will generate contacts. Rather than requiring every lane to carefully clear all contactExists states, just clear them up front.
manifold.Contact0Exists = default;
manifold.Contact1Exists = default;
manifold.Contact2Exists = default;
manifold.Contact3Exists = default;
if (Vector.LessThanAll(inactiveLanes, Vector <int> .Zero))
{
//No contacts generated.
return;
}
//Note the use of the triangle center as the initial normal rather than the localOffsetA.
//Triangles are not guaranteed to be centered on their center of mass, and the DepthRefiner
//will converge to a depth which does not oppose the so-far best normal- which, on the early iterations,
//could be the initial normal.
Vector3Wide.Length(localTriangleCenter, out var centerDistance);
Vector3Wide.Scale(localTriangleCenter, Vector <float> .One / centerDistance, out var initialNormal);
var useInitialFallback = Vector.LessThan(centerDistance, new Vector <float>(1e-10f));
initialNormal.X = Vector.ConditionalSelect(useInitialFallback, Vector <float> .Zero, initialNormal.X);
initialNormal.Y = Vector.ConditionalSelect(useInitialFallback, Vector <float> .One, initialNormal.Y);
initialNormal.Z = Vector.ConditionalSelect(useInitialFallback, Vector <float> .Zero, initialNormal.Z);
//Check if the extreme point of the hull toward the triangle along its face normal lies inside the triangle.
//If it is, then there's no need for depth refinement.
Vector <int> triangleNormalIsMinimal;
var hullSupportFinder = default(ConvexHullSupportFinder);
DepthRefiner.SimplexWithWitness simplex;
var triangleSupportFinder = default(PretransformedTriangleSupportFinder);
//Create a simplex entry for the direction from the hull center to triangle center.
DepthRefiner.FindSupport(b, triangle, localTriangleCenter, hullLocalTriangleOrientation, ref hullSupportFinder, ref triangleSupportFinder, initialNormal, inactiveLanes, out simplex.A.Support, out simplex.A.SupportOnA);
Vector3Wide.Dot(simplex.A.Support, initialNormal, out var depth);
simplex.A.Exists = Vector.OnesComplement(inactiveLanes);
//Create a simplex entry for the triangle face normal.
Vector3Wide.Negate(triangleNormal, out var negatedTriangleNormal);
hullSupportFinder.ComputeLocalSupport(b, negatedTriangleNormal, inactiveLanes, out var hullSupportAlongTriangleNormal);
simplex.B.SupportOnA = hullSupportAlongTriangleNormal;
Vector3Wide.Subtract(simplex.B.SupportOnA, localTriangleCenter, out simplex.B.Support);
Vector3Wide.Dot(simplex.B.Support, negatedTriangleNormal, out var triangleFaceDepth);
var useTriangleFace = Vector.LessThan(triangleFaceDepth, depth);
Vector3Wide.ConditionalSelect(useTriangleFace, negatedTriangleNormal, initialNormal, out initialNormal);
depth = Vector.ConditionalSelect(useTriangleFace, triangleFaceDepth, depth);
simplex.B.Exists = simplex.A.Exists;
simplex.C.Exists = default;
//Check if the extreme point on the hull is contained within the bounds of the triangle face. If it is, there is no need for a full depth refinement.
Vector3Wide.Subtract(triangleA, hullSupportAlongTriangleNormal, out var closestToA);
Vector3Wide.Subtract(triangleB, hullSupportAlongTriangleNormal, out var closestToB);
Vector3Wide.Subtract(triangleC, hullSupportAlongTriangleNormal, out var closestToC);
Vector3Wide.Dot(edgePlaneAB, closestToA, out var extremeABPlaneTest);
Vector3Wide.Dot(edgePlaneBC, closestToB, out var extremeBCPlaneTest);
Vector3Wide.Dot(edgePlaneCA, closestToC, out var extremeCAPlaneTest);
triangleNormalIsMinimal = Vector.BitwiseAnd(Vector.LessThanOrEqual(extremeABPlaneTest, Vector <float> .Zero), Vector.BitwiseAnd(Vector.LessThanOrEqual(extremeBCPlaneTest, Vector <float> .Zero), Vector.LessThanOrEqual(extremeCAPlaneTest, Vector <float> .Zero)));
var depthThreshold = -speculativeMargin;
var skipDepthRefine = Vector.BitwiseOr(triangleNormalIsMinimal, inactiveLanes);
Vector3Wide localNormal, closestOnHull;
if (Vector.EqualsAny(skipDepthRefine, Vector <int> .Zero))
{
DepthRefiner.FindMinimumDepth(
b, triangle, localTriangleCenter, hullLocalTriangleOrientation, ref hullSupportFinder, ref triangleSupportFinder, ref simplex, initialNormal, depth, skipDepthRefine, 1e-5f * epsilonScale, depthThreshold,
out var refinedDepth, out var refinedNormal, out var refinedClosestOnHull);
Vector3Wide.ConditionalSelect(skipDepthRefine, hullSupportAlongTriangleNormal, refinedClosestOnHull, out closestOnHull);
Vector3Wide.ConditionalSelect(skipDepthRefine, initialNormal, refinedNormal, out localNormal);
depth = Vector.ConditionalSelect(skipDepthRefine, depth, refinedDepth);
}
else
{
//No depth refine ran; the extreme point prepass did everything we needed. Just use the initial normal.
localNormal = initialNormal;
closestOnHull = hullSupportAlongTriangleNormal;
}
Vector3Wide.Dot(triangleNormal, localNormal, out var triangleNormalDotLocalNormal);
inactiveLanes = Vector.BitwiseOr(inactiveLanes, Vector.BitwiseOr(Vector.GreaterThanOrEqual(triangleNormalDotLocalNormal, Vector <float> .Zero), Vector.LessThan(depth, depthThreshold)));
if (Vector.LessThanAll(inactiveLanes, Vector <int> .Zero))
{
//No contacts generated.
return;
}
//To find the contact manifold, we'll clip the triangle edges against the hull face as usual, but we're dealing with potentially
//distinct convex hulls. Rather than vectorizing over the different hulls, we vectorize within each hull.
Helpers.FillVectorWithLaneIndices(out var slotOffsetIndices);
var boundingPlaneEpsilon = 1e-3f * epsilonScale;
//There can be no more than 6 contacts (provided there are no numerical errors); 2 per triangle edge.
var candidates = stackalloc ManifoldCandidateScalar[6];
for (int slotIndex = 0; slotIndex < pairCount; ++slotIndex)
{
if (inactiveLanes[slotIndex] < 0)
{
continue;
}
ref var hull = ref b.Hulls[slotIndex];
ConvexHullTestHelper.PickRepresentativeFace(ref hull, slotIndex, ref localNormal, closestOnHull, slotOffsetIndices, ref boundingPlaneEpsilon, out var slotFaceNormal, out var slotLocalNormal, out var bestFaceIndex);
//Test each triangle edge against the hull face.
//Note that we do not use the faceNormal x edgeOffset edge plane, but rather edgeOffset x localNormal.
//The faces are wound counterclockwise.
//Note that the triangle edges are packed into a Vector4. Historically, there were some minor codegen issues with Vector3.
//May not matter anymore, but it costs ~nothing to use a dead slot.
ref var aSlot = ref GatherScatter.GetOffsetInstance(ref triangleA, slotIndex);
public void 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 unsafe void Test(
ref BoxWide a, ref BoxWide b, ref Vector <float> speculativeMargin,
ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount,
out Convex4ContactManifoldWide manifold)
{
Matrix3x3Wide.CreateFromQuaternion(orientationA, out var worldRA);
Matrix3x3Wide.CreateFromQuaternion(orientationB, out var worldRB);
Matrix3x3Wide.MultiplyByTransposeWithoutOverlap(worldRB, worldRA, out var rB);
Matrix3x3Wide.TransformByTransposedWithoutOverlap(offsetB, worldRA, out var localOffsetB);
Vector3Wide localNormal;
//b.X
TestEdgeEdge(
ref a.HalfWidth, ref a.HalfHeight, ref a.HalfLength,
ref b.HalfWidth, ref b.HalfHeight, ref b.HalfLength,
ref localOffsetB.X, ref localOffsetB.Y, ref localOffsetB.Z,
ref rB.X, ref rB.Y, ref rB.Z, ref rB.X,
out var depth, out localNormal.X, out localNormal.Y, out localNormal.Z);
//b.Y
TestEdgeEdge(
ref a.HalfWidth, ref a.HalfHeight, ref a.HalfLength,
ref b.HalfWidth, ref b.HalfHeight, ref b.HalfLength,
ref localOffsetB.X, ref localOffsetB.Y, ref localOffsetB.Z,
ref rB.X, ref rB.Y, ref rB.Z, ref rB.Y,
out var edgeYDepth, out var edgeYNX, out var edgeYNY, out var edgeYNZ);
Select(ref depth, ref localNormal,
ref edgeYDepth, ref edgeYNX, ref edgeYNY, ref edgeYNZ);
//b.Z
TestEdgeEdge(
ref a.HalfWidth, ref a.HalfHeight, ref a.HalfLength,
ref b.HalfWidth, ref b.HalfHeight, ref b.HalfLength,
ref localOffsetB.X, ref localOffsetB.Y, ref localOffsetB.Z,
ref rB.X, ref rB.Y, ref rB.Z, ref rB.Z,
out var edgeZDepth, out var edgeZNX, out var edgeZNY, out var edgeZNZ);
Select(ref depth, ref localNormal,
ref edgeZDepth, ref edgeZNX, ref edgeZNY, ref edgeZNZ);
//Test face normals of A. Working in local space of A means potential axes are just (1,0,0) etc.
Vector3Wide.Abs(rB.X, out var absRBX);
Vector3Wide.Abs(rB.Y, out var absRBY);
Vector3Wide.Abs(rB.Z, out var absRBZ);
var faceAXDepth = a.HalfWidth + b.HalfWidth * absRBX.X + b.HalfHeight * absRBY.X + b.HalfLength * absRBZ.X - Vector.Abs(localOffsetB.X);
var one = Vector <float> .One;
var zero = Vector <float> .Zero;
Select(ref depth, ref localNormal, ref faceAXDepth, ref one, ref zero, ref zero);
var faceAYDepth = a.HalfHeight + b.HalfWidth * absRBX.Y + b.HalfHeight * absRBY.Y + b.HalfLength * absRBZ.Y - Vector.Abs(localOffsetB.Y);
Select(ref depth, ref localNormal, ref faceAYDepth, ref zero, ref one, ref zero);
var faceAZDepth = a.HalfLength + b.HalfWidth * absRBX.Z + b.HalfHeight * absRBY.Z + b.HalfLength * absRBZ.Z - Vector.Abs(localOffsetB.Z);
Select(ref depth, ref localNormal, ref faceAZDepth, ref zero, ref zero, ref one);
//Test face normals of B. Rows of A->B rotation.
Matrix3x3Wide.TransformByTransposedWithoutOverlap(localOffsetB, rB, out var bLocalOffsetB);
var faceBXDepth = b.HalfWidth + a.HalfWidth * absRBX.X + a.HalfHeight * absRBX.Y + a.HalfLength * absRBX.Z - Vector.Abs(bLocalOffsetB.X);
Select(ref depth, ref localNormal, ref faceBXDepth, ref rB.X.X, ref rB.X.Y, ref rB.X.Z);
var faceBYDepth = b.HalfHeight + a.HalfWidth * absRBY.X + a.HalfHeight * absRBY.Y + a.HalfLength * absRBY.Z - Vector.Abs(bLocalOffsetB.Y);
Select(ref depth, ref localNormal, ref faceBYDepth, ref rB.Y.X, ref rB.Y.Y, ref rB.Y.Z);
var faceBZDepth = b.HalfLength + a.HalfWidth * absRBZ.X + a.HalfHeight * absRBZ.Y + a.HalfLength * absRBZ.Z - Vector.Abs(bLocalOffsetB.Z);
Select(ref depth, ref localNormal, ref faceBZDepth, ref rB.Z.X, ref rB.Z.Y, ref rB.Z.Z);
//Calibrate the normal to point from B to A, matching convention.
Vector3Wide.Dot(localNormal, localOffsetB, out var normalDotOffsetB);
var shouldNegateNormal = Vector.GreaterThan(normalDotOffsetB, Vector <float> .Zero);
localNormal.X = Vector.ConditionalSelect(shouldNegateNormal, -localNormal.X, localNormal.X);
localNormal.Y = Vector.ConditionalSelect(shouldNegateNormal, -localNormal.Y, localNormal.Y);
localNormal.Z = Vector.ConditionalSelect(shouldNegateNormal, -localNormal.Z, localNormal.Z);
Matrix3x3Wide.TransformWithoutOverlap(localNormal, worldRA, out manifold.Normal);
//Contact generation always assumes face-face clipping. Other forms of contact generation are just special cases of face-face, and since we pay
//for all code paths, there's no point in handling them separately.
//We just have to guarantee that the face chosen on each box is guaranteed to include the deepest feature along the contact normal.
//To do this, choose the face on each box associated with the maximum axis dot with the collision normal.
//We represent each face as a center position, its two tangent axes, and the length along those axes.
//Technically, we could leave A's tangents implicit by swizzling components, but that complicates things a little bit for not much gain.
//Since we're not taking advantage of the dimension reduction of working in A's local space from here on out, just use the world axes to avoid a final retransform.
Vector3Wide.Dot(manifold.Normal, worldRA.X, out var axDot);
Vector3Wide.Dot(manifold.Normal, worldRA.Y, out var ayDot);
Vector3Wide.Dot(manifold.Normal, worldRA.Z, out var azDot);
var absAXDot = Vector.Abs(axDot);
var absAYDot = Vector.Abs(ayDot);
var absAZDot = Vector.Abs(azDot);
var maxADot = Vector.Max(absAXDot, Vector.Max(absAYDot, absAZDot));
var useAX = Vector.Equals(maxADot, absAXDot);
var useAY = Vector.AndNot(Vector.Equals(maxADot, absAYDot), useAX);
Vector3Wide.ConditionalSelect(useAX, worldRA.X, worldRA.Z, out var normalA);
Vector3Wide.ConditionalSelect(useAY, worldRA.Y, normalA, out normalA);
Vector3Wide.ConditionalSelect(useAX, worldRA.Z, worldRA.Y, out var tangentAX);
Vector3Wide.ConditionalSelect(useAY, worldRA.X, tangentAX, out tangentAX);
Vector3Wide.ConditionalSelect(useAX, worldRA.Y, worldRA.X, out var tangentAY);
Vector3Wide.ConditionalSelect(useAY, worldRA.Z, tangentAY, out tangentAY);
var halfSpanAX = Vector.ConditionalSelect(useAX, a.HalfLength, Vector.ConditionalSelect(useAY, a.HalfWidth, a.HalfHeight));
var halfSpanAY = Vector.ConditionalSelect(useAX, a.HalfHeight, Vector.ConditionalSelect(useAY, a.HalfLength, a.HalfWidth));
var halfSpanAZ = Vector.ConditionalSelect(useAX, a.HalfWidth, Vector.ConditionalSelect(useAY, a.HalfHeight, a.HalfLength));
//We'll construct vertex feature ids from axis ids.
//Vertex ids will be constructed by setting or not setting the relevant bit for each axis.
var localXId = new Vector <int>(1);
var localYId = new Vector <int>(4);
var localZId = new Vector <int>(16);
var axisIdAX = Vector.ConditionalSelect(useAX, localZId, Vector.ConditionalSelect(useAY, localXId, localYId));
var axisIdAY = Vector.ConditionalSelect(useAX, localYId, Vector.ConditionalSelect(useAY, localZId, localXId));
var axisIdAZ = Vector.ConditionalSelect(useAX, localXId, Vector.ConditionalSelect(useAY, localYId, localZId));
Vector3Wide.Dot(manifold.Normal, worldRB.X, out var bxDot);
Vector3Wide.Dot(manifold.Normal, worldRB.Y, out var byDot);
Vector3Wide.Dot(manifold.Normal, worldRB.Z, out var bzDot);
var absBXDot = Vector.Abs(bxDot);
var absBYDot = Vector.Abs(byDot);
var absBZDot = Vector.Abs(bzDot);
var maxBDot = Vector.Max(absBXDot, Vector.Max(absBYDot, absBZDot));
var useBX = Vector.Equals(maxBDot, absBXDot);
var useBY = Vector.AndNot(Vector.Equals(maxBDot, absBYDot), useBX);
Vector3Wide.ConditionalSelect(useBX, worldRB.X, worldRB.Z, out var normalB);
Vector3Wide.ConditionalSelect(useBY, worldRB.Y, normalB, out normalB);
Vector3Wide.ConditionalSelect(useBX, worldRB.Z, worldRB.Y, out var tangentBX);
Vector3Wide.ConditionalSelect(useBY, worldRB.X, tangentBX, out tangentBX);
Vector3Wide.ConditionalSelect(useBX, worldRB.Y, worldRB.X, out var tangentBY);
Vector3Wide.ConditionalSelect(useBY, worldRB.Z, tangentBY, out tangentBY);
var halfSpanBX = Vector.ConditionalSelect(useBX, b.HalfLength, Vector.ConditionalSelect(useBY, b.HalfWidth, b.HalfHeight));
var halfSpanBY = Vector.ConditionalSelect(useBX, b.HalfHeight, Vector.ConditionalSelect(useBY, b.HalfLength, b.HalfWidth));
var halfSpanBZ = Vector.ConditionalSelect(useBX, b.HalfWidth, Vector.ConditionalSelect(useBY, b.HalfHeight, b.HalfLength));
//We'll construct edge feature ids from axis ids.
//Edge ids will be 6 bits total, representing 3 possible states (-1, 0, 1) for each of the 3 axes. Multiply the axis id by 1, 2, or 3 to get the edge id contribution for the axis.
var axisIdBX = Vector.ConditionalSelect(useBX, localZId, Vector.ConditionalSelect(useBY, localXId, localYId));
var axisIdBY = Vector.ConditionalSelect(useBX, localYId, Vector.ConditionalSelect(useBY, localZId, localXId));
var axisIdBZ = Vector.ConditionalSelect(useBX, localXId, Vector.ConditionalSelect(useBY, localYId, localZId));
//Calibrate normalB to face toward A, and normalA to face toward B.
Vector3Wide.Dot(normalA, manifold.Normal, out var calibrationDotA);
var shouldNegateNormalA = Vector.GreaterThan(calibrationDotA, Vector <float> .Zero);
normalA.X = Vector.ConditionalSelect(shouldNegateNormalA, -normalA.X, normalA.X);
normalA.Y = Vector.ConditionalSelect(shouldNegateNormalA, -normalA.Y, normalA.Y);
normalA.Z = Vector.ConditionalSelect(shouldNegateNormalA, -normalA.Z, normalA.Z);
Vector3Wide.Dot(normalB, manifold.Normal, out var calibrationDotB);
var shouldNegateNormalB = Vector.LessThan(calibrationDotB, Vector <float> .Zero);
normalB.X = Vector.ConditionalSelect(shouldNegateNormalB, -normalB.X, normalB.X);
normalB.Y = Vector.ConditionalSelect(shouldNegateNormalB, -normalB.Y, normalB.Y);
normalB.Z = Vector.ConditionalSelect(shouldNegateNormalB, -normalB.Z, normalB.Z);
//Clip edges of B against the face bounds of A to collect all edge-bound contacts.
Vector3Wide.Scale(normalB, halfSpanBZ, out var faceCenterB);
Vector3Wide.Add(faceCenterB, offsetB, out faceCenterB);
Vector3Wide.Scale(tangentBY, halfSpanBY, out var edgeOffsetBX);
Vector3Wide.Scale(tangentBX, halfSpanBX, out var edgeOffsetBY);
Vector3Wide.Dot(tangentAX, tangentBX, out var axbx);
Vector3Wide.Dot(tangentAY, tangentBX, out var aybx);
Vector3Wide.Dot(tangentAX, tangentBY, out var axby);
Vector3Wide.Dot(tangentAY, tangentBY, out var ayby);
GetEdgeVersusFaceBoundsIntervals(ref tangentAX, ref tangentAY, ref halfSpanAX, ref halfSpanAY, ref axbx, ref aybx, ref faceCenterB, ref halfSpanBX, ref edgeOffsetBX,
out var bX0Min, out var bX0Max, out var bX1Min, out var bX1Max);
GetEdgeVersusFaceBoundsIntervals(ref tangentAX, ref tangentAY, ref halfSpanAX, ref halfSpanAY, ref axby, ref ayby, ref faceCenterB, ref halfSpanBY, ref edgeOffsetBY,
out var bY0Min, out var bY0Max, out var bY1Min, out var bY1Max);
//Note that we only allocate up to 8 candidates. It is not possible for this process to generate more than 8 (unless there are numerical problems, which we guard against).
int byteCount = Unsafe.SizeOf <ManifoldCandidate>() * 8;
var buffer = stackalloc byte[byteCount];
ref var candidates = ref Unsafe.As <byte, ManifoldCandidate>(ref *buffer);
public void Do()
{
Matrix3x3Wide.TransformWithoutOverlap(v, symmetric, out var intermediate);
Vector3Wide.Dot(v, intermediate, out result);
}
public void Do()
{
Matrix3x3Wide.Transform(ref v, ref symmetric, out var intermediate);
Vector3Wide.Dot(ref v, ref intermediate, out result);
}
public static void Prestep(ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref TwoBody1DOFJacobians jacobians, ref SpringSettingsWide springSettings, ref Vector <float> maximumRecoveryVelocity,
ref Vector <float> positionError, float dt, float inverseDt, out Projection2Body1DOF projection)
{
//unsoftened effective mass = (J * M^-1 * JT)^-1
//where J is a constraintDOF x bodyCount*6 sized matrix, JT is its transpose, and for two bodies3D M^-1 is:
//[inverseMassA, 0, 0, 0]
//[0, inverseInertiaA, 0, 0]
//[0, 0, inverseMassB, 0]
//[0, 0, 0, inverseInertiaB]
//The entries of J match up to this convention, containing the linear and angular components of each body in sequence, so for a 2 body 1DOF constraint J would look like:
//[linearA 1x3, angularA 1x3, linearB 1x3, angularB 1x3]
//Note that it is a row vector by convention. When transforming velocities from world space into constraint space, it is assumed that the velocity vector is organized as a
//row vector matching up to the jacobian (that is, [linearA 1x3, angularA 1x3, linearB 1x3, angularB 1x3]), so for a 2 body 2 DOF constraint,
//worldVelocity * JT would be a [worldVelocity: 1x12] * [JT: 12x2], resulting in a 1x2 constraint space velocity row vector.
//Similarly, when going from constraint space impulse to world space impulse in the above example, we would do [csi: 1x2] * [J: 2x12] to get a 1x12 world impulse row vector.
//Note that the engine uses row vectors for all velocities and positions and so on. Rotation and inertia tensors are constructed for premultiplication.
//In other words, unlike many of the presentations in the space, we use v * JT and csi * J instead of J * v and JT * csi.
//There is no meaningful difference- the two conventions are just transpositions of each other.
//(If you want to know how this stuff works, go read the constraint related presentations: http://box2d.org/downloads/
//Be mindful of the difference in conventions. You'll see J * v instead of v * JT, for example. Everything is still fundamentally the same, though.)
//Due to the block structure of the mass matrix, we can handle each component separately and then sum the results.
//For this 1DOF constraint, the result is a simple scalar.
//Note that we store the intermediate results of J * M^-1 for use when projecting from constraint space impulses to world velocity changes.
//If we didn't store those intermediate values, we could just scale the dot product of jacobians.LinearA with itself to save 4 multiplies.
Vector3Wide.Scale(jacobians.LinearA, inertiaA.InverseMass, out projection.CSIToWSVLinearA);
Vector3Wide.Scale(jacobians.LinearB, inertiaB.InverseMass, out projection.CSIToWSVLinearB);
Vector3Wide.Dot(projection.CSIToWSVLinearA, jacobians.LinearA, out var linearA);
Vector3Wide.Dot(projection.CSIToWSVLinearB, jacobians.LinearB, out var linearB);
//The angular components are a little more involved; (J * I^-1) * JT is explicitly computed.
Symmetric3x3Wide.TransformWithoutOverlap(jacobians.AngularA, inertiaA.InverseInertiaTensor, out projection.CSIToWSVAngularA);
Symmetric3x3Wide.TransformWithoutOverlap(jacobians.AngularB, inertiaB.InverseInertiaTensor, out projection.CSIToWSVAngularB);
Vector3Wide.Dot(projection.CSIToWSVAngularA, jacobians.AngularA, out var angularA);
Vector3Wide.Dot(projection.CSIToWSVAngularB, jacobians.AngularB, out var angularB);
//Now for a digression!
//Softness is applied along the diagonal (which, for a 1DOF constraint, is just the only element).
//Check the the ODE reference for a bit more information: http://ode.org/ode-latest-userguide.html#sec_3_8_0
//And also see Erin Catto's Soft Constraints presentation for more details: http://box2d.org/files/GDC2011/GDC2011_Catto_Erin_Soft_Constraints.pdf)
//There are some very interesting tricks you can use here, though.
//Our core tuning variables are the damping ratio and natural frequency.
//Our runtime used variables are softness and an error reduction feedback scale..
//(For the following, I'll use the ODE terms CFM and ERP, constraint force mixing and error reduction parameter.)
//So first, we need to get from damping ratio and natural frequency to stiffness and damping spring constants.
//From there, we'll go to CFM/ERP.
//Then, we'll create an expression for a softened effective mass matrix (i.e. one that takes into account the CFM term),
//and an expression for the contraint force mixing term in the solve iteration.
//Finally, compute ERP.
//(And then some tricks.)
//1) Convert from damping ratio and natural frequency to stiffness and damping constants.
//The raw expressions are:
//stiffness = effectiveMass * naturalFrequency^2
//damping = effectiveMass * 2 * dampingRatio * naturalFrequency
//Rather than using any single object as the reference for the 'mass' term involved in this conversion, use the effective mass of the constraint.
//In other words, we're dynamically picking the spring constants necessary to achieve the desired behavior for the current constraint configuration.
//(See Erin Catto's presentation above for more details on this.)
//(Note that this is different from BEPUphysics v1. There, users configured stiffness and damping constants. That worked okay, but people often got confused about
//why constraints didn't behave the same when they changed masses. Usually it manifested as someone creating an incredibly high mass object relative to the default
//stiffness/damping, and they'd post on the forum wondering why constraints were so soft. Basically, the defaults were another sneaky tuning factor to get wrong.
//Since damping ratio and natural frequency define the behavior independent of the mass, this problem goes away- and it makes some other interesting things happen...)
//2) Convert from stiffness and damping constants to CFM and ERP.
//CFM = (stiffness * dt + damping)^-1
//ERP = (stiffness * dt) * (stiffness * dt + damping)^-1
//Or, to rephrase:
//ERP = (stiffness * dt) * CFM
//3) Use CFM and ERP to create a softened effective mass matrix and a force mixing term for the solve iterations.
//Start with a base definition which we won't be deriving, the velocity constraint itself (stated as an equality constraint here):
//This means 'world space velocity projected into constraint space should equal the velocity bias term combined with the constraint force mixing term'.
//(The velocity bias term will be computed later- it's the position error scaled by the error reduction parameter, ERP. Position error is used to create a velocity motor goal.)
//We're pulling back from the implementation of sequential impulses here, so rather than using the term 'accumulated impulse', we'll use 'lambda'
//(which happens to be consistent with the ODE documentation covering the same topic). Lambda is impulse that satisfies the constraint.
//wsv * JT = bias - lambda * CFM/dt
//This can be phrased as:
//currentVelocity = targetVelocity
//Or:
//goalVelocityChange = targetVelocity - currentVelocity
//lambda = goalVelocityChange * effectiveMass
//lambda = (targetVelocity - currentVelocity) * effectiveMass
//lambda = (bias - lambda * CFM/dt - currentVelocity) * effectiveMass
//Solving for lambda:
//lambda = (bias - currentVelocity) * effectiveMass - lambda * CFM/dt * effectiveMass
//lambda + lambda * CFM/dt * effectiveMass = (bias - currentVelocity) * effectiveMass
//(lambda + lambda * CFM/dt * effectiveMass) * effectiveMass^-1 = bias - currentVelocity
//lambda * effectiveMass^-1 + lambda * CFM/dt = bias - currentVelocity
//lambda * (effectiveMass^-1 + CFM/dt) = bias - currentVelocity
//lambda = (bias - currentVelocity) * (effectiveMass^-1 + CFM/dt)^-1
//lambda = (bias - wsv * JT) * (effectiveMass^-1 + CFM/dt)^-1
//In other words, we transform the velocity change (bias - wsv * JT) into the constraint-satisfying impulse, lambda, using a matrix (effectiveMass^-1 + CFM/dt)^-1.
//That matrix is the softened effective mass:
//softenedEffectiveMass = (effectiveMass^-1 + CFM/dt)^-1
//Here's where some trickiness occurs. (Be mindful of the distinction between the softened and unsoftened effective mass).
//Start by substituting CFM into the softened effective mass definition:
//CFM/dt = (stiffness * dt + damping)^-1 / dt = (dt * (stiffness * dt + damping))^-1 = (stiffness * dt^2 + damping*dt)^-1
//softenedEffectiveMass = (effectiveMass^-1 + (stiffness * dt^2 + damping * dt)^-1)^-1
//Now substitute the definitions of stiffness and damping, treating the scalar components as uniform scaling matrices of dimension equal to effectiveMass:
//softenedEffectiveMass = (effectiveMass^-1 + ((effectiveMass * naturalFrequency^2) * dt^2 + (effectiveMass * 2 * dampingRatio * naturalFrequency) * dt)^-1)^-1
//Combine the inner effectiveMass coefficients, given matrix multiplication distributes over addition:
//softenedEffectiveMass = (effectiveMass^-1 + (effectiveMass * (naturalFrequency^2 * dt^2) + effectiveMass * (2 * dampingRatio * naturalFrequency * dt))^-1)^-1
//softenedEffectiveMass = (effectiveMass^-1 + (effectiveMass * (naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt))^-1)^-1
//Apply the inner matrix inverse:
//softenedEffectiveMass = (effectiveMass^-1 + (naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1 * effectiveMass^-1)^-1
//Once again, combine coefficients of the inner effectiveMass^-1 terms:
//softenedEffectiveMass = ((1 + (naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1) * effectiveMass^-1)^-1
//Apply the inverse again:
//softenedEffectiveMass = effectiveMass * (1 + (naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1)^-1
//So, to put it another way- because CFM is based on the effective mass, applying it to the effective mass results in a simple downscale.
//What has been gained? Consider what happens in the solve iteration.
//We take the velocity error:
//velocityError = bias - accumulatedImpulse * CFM/dt - wsv * JT
//and convert it to a corrective impulse with the effective mass:
//impulse = (bias - accumulatedImpulse * CFM/dt - wsv * JT) * softenedEffectiveMass
//The effective mass distributes over the set:
//impulse = bias * softenedEffectiveMass - accumulatedImpulse * CFM/dt * softenedEffectiveMass - wsv * JT * softenedEffectiveMass
//Focus on the CFM term:
//-accumulatedImpulse * CFM/dt * softenedEffectiveMass
//What is CFM/dt * softenedEffectiveMass? Substitute.
//(stiffness * dt^2 + damping * dt)^-1 * softenedEffectiveMass
//((effectiveMass * naturalFrequency^2) * dt^2 + (effectiveMass * 2 * dampingRatio * naturalFrequency * dt))^-1 * softenedEffectiveMass
//Combine terms:
//(effectiveMass * (naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt))^-1 * softenedEffectiveMass
//Apply inverse:
//(naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1 * effectiveMass^-1 * softenedEffectiveMass
//Expand softened effective mass from earlier:
//(naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1 * effectiveMass^-1 * effectiveMass * (1 + (naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1)^-1
//Cancel effective masses: (!)
//(naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1 * (1 + (naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1)^-1
//Because CFM was created from effectiveMass, the CFM/dt * effectiveMass term is actually independent of the effectiveMass!
//The remaining expression is still a matrix, but fortunately it is a simple uniform scaling matrix that we can store and apply as a single scalar.
//4) How do you compute ERP?
//ERP = (stiffness * dt) * CFM
//ERP = (stiffness * dt) * (stiffness * dt + damping)^-1
//ERP = ((effectiveMass * naturalFrequency^2) * dt) * ((effectiveMass * naturalFrequency^2) * dt + (effectiveMass * 2 * dampingRatio * naturalFrequency))^-1
//Combine denominator terms:
//ERP = ((effectiveMass * naturalFrequency^2) * dt) * ((effectiveMass * (naturalFrequency^2 * dt + 2 * dampingRatio * naturalFrequency))^-1
//Apply denominator inverse:
//ERP = ((effectiveMass * naturalFrequency^2) * dt) * (naturalFrequency^2 * dt + 2 * dampingRatio * naturalFrequency)^-1 * effectiveMass^-1
//Uniform scaling matrices commute:
//ERP = (naturalFrequency^2 * dt) * effectiveMass * effectiveMass^-1 * (naturalFrequency^2 * dt + 2 * dampingRatio * naturalFrequency)^-1
//Cancellation!
//ERP = (naturalFrequency^2 * dt) * (naturalFrequency^2 * dt + 2 * dampingRatio * naturalFrequency)^-1
//ERP = (naturalFrequency * dt) * (naturalFrequency * dt + 2 * dampingRatio)^-1
//ERP is a simple scalar, independent of mass.
//5) So we can compute CFM, ERP, the softened effective mass matrix, and we have an interesting shortcut on the constraint force mixing term of the solve iterations.
//Is there anything more that can be done? You bet!
//Let's look at the post-distribution impulse computation again:
//impulse = bias * effectiveMass - accumulatedImpulse * CFM/dt * effectiveMass - wsv * JT * effectiveMass
//During the solve iterations, the only quantities that vary are the accumulated impulse and world space velocities. So the rest can be precomputed.
//bias * effectiveMass,
//CFM/dt * effectiveMass,
//JT * effectiveMass
//In other words, we bypass the intermediate velocity state and go directly from source velocities to an impulse.
//Note the sizes of the precomputed types above:
//bias * effective mass is the same size as bias (vector with dimension equal to constrained DOFs)
//CFM/dt * effectiveMass is a single scalar regardless of constrained DOFs,
//JT * effectiveMass is the same size as JT
//But note that we no longer need to load the effective mass! It is implicit.
//The resulting computation is:
//impulse = a - accumulatedImpulse * b - wsv * c
//two DOF-width adds (add/subtract), one DOF-width multiply, and a 1xDOF * DOFx12 jacobian-sized transform.
//Compare to;
//(bias - accumulatedImpulse * CFM/dt - wsv * JT) * effectiveMass
//two DOF-width adds (add/subtract), one DOF width multiply, a 1xDOF * DOFx12 jacobian-sized transform, and a 1xDOF * DOFxDOF transform.
//In other words, we shave off a whole 1xDOF * DOFxDOF transform per iteration.
//So, taken in isolation, this is a strict win both in terms of memory and the amount of computation.
//Unfortunately, it's not quite so simple- jacobians are ALSO used to transform the impulse into world space so that it can be used to change the body velocities.
//We still need to have those around. So while we no longer store the effective mass, our jacobian has sort of been duplicated.
//But wait, there's more!
//That process looks like:
//wsv += impulse * J * M^-1
//So while we need to store something here, we can take advantage of the fact that we aren't using the jacobian anywhere else (it's replaced by the JT * effectiveMass term above).
//Precompute J*M^-1, too.
//So you're still loading a jacobian-sized matrix, but you don't need to load M^-1! That saves you 14 scalars. (symmetric 3x3 + 1 + symmetric 3x3 + 1)
//That saves you the multiplication of (impulse * J) * M^-1, which is 6 multiplies and 6 dot products.
//Note that this optimization's value depends on the number of constrained DOFs.
//Net memory change, opt vs no opt, in scalars:
//1DOF: costs 1x12, saves 1x1 effective mass and the 14 scalar M^-1: -3
//2DOF: costs 2x12, saves 2x2 symmetric effective mass and the 14 scalar M^-1: 7
//3DOF: costs 3x12, saves 3x3 symmetric effective mass and the 14 scalar M^-1: 16
//4DOF: costs 4x12, saves 4x4 symmetric effective mass and the 14 scalar M^-1: 24
//5DOF: costs 5x12, saves 5x5 symmetric effective mass and the 14 scalar M^-1: 31
//6DOF: costs 6x12, saves 6x6 symmetric effective mass and the 14 scalar M^-1: 37
//Net compute savings, opt vs no opt:
//DOF savings = 1xDOF * DOFxDOF (DOF DOFdot products), 2 1x3 * scalar (6 multiplies), 2 1x3 * 3x3 (6 3dot products)
// = (DOF*DOF multiplies + DOF*(DOF-1) adds) + (6 multiplies) + (18 multiplies + 12 adds)
// = DOF*DOF + 24 multiplies, DOF*DOF-DOF + 12 adds
//1DOF: 25 multiplies, 12 adds
//2DOF: 28 multiplies, 14 adds
//3DOF: 33 multiplies, 18 adds
//4DOF: 40 multiplies, 24 adds
//5DOF: 49 multiplies, 32 adds
//6DOF: 60 multiplies, 42 adds
//So does our 'optimization' actually do anything useful?
//In 1 DOF constraints, it's often a win with no downsides.
//2+ are difficult to determine.
//This depends on heavily on the machine's SIMD width. You do every lane's ALU ops in parallel, but the loads are still fundamentally bound by memory bandwidth.
//The loads are coherent, at least- no gathers on this stuff. But I wouldn't be surprised if 3DOF+ constraints end up being faster *without* the pretransformations on wide SIMD.
//This is just something that will require case by case analysis. Constraints can have special structure which change the judgment.
//(Also, note that large DOF jacobians are often very sparse. Consider the jacobians used by a 6DOF weld joint. You could likely do special case optimizations to reduce the
//load further. It is unlikely that you could find a way to do the same to JT * effectiveMass. J * M^-1 might have some savings, though. But J*M^-1 isn't *sparser*
//than J by itself, so the space savings are limited. As long as you precompute, the above load requirement offset will persist.)
//Good news, though! There are a lot of constraints where this trick is applicable.
//We'll start with the unsoftened effective mass, constructed from the contributions computed above:
var effectiveMass = Vector <float> .One / (linearA + linearB + angularA + angularB);
SpringSettingsWide.ComputeSpringiness(springSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale);
var softenedEffectiveMass = effectiveMass * effectiveMassCFMScale;
//Note that we use a bit of a hack when computing the bias velocity- even if our damping ratio/natural frequency implies a strongly springy response
//that could cause a significant velocity overshoot, we apply an arbitrary clamping value to keep it reasonable.
//This is useful for a variety of inequality constraints (like contacts) because you don't always want them behaving as true springs.
var biasVelocity = Vector.Min(positionError * positionErrorToVelocity, maximumRecoveryVelocity);
projection.BiasImpulse = biasVelocity * softenedEffectiveMass;
//Precompute the wsv * (JT * softenedEffectiveMass) term.
//Note that we store it in a Vector3Wide as if it's a row vector, but this is really a column (because JT is a column vector).
//So we're really storing (JT * softenedEffectiveMass)T = softenedEffectiveMassT * J.
//Since this constraint is 1DOF, the softenedEffectiveMass is a scalar and the order doesn't matter.
//In the solve iterations, the WSVtoCSI term will be transposed during transformation,
//resulting in the proper wsv * (softenedEffectiveMassT * J)T = wsv * (JT * softenedEffectiveMass).
//You'll see this pattern repeated in higher DOF constraints. We explicitly compute softenedEffectiveMassT * J, and then apply the transpose in the solves.
//(Why? Because creating a Matrix3x2 and Matrix2x3 and 4x3 and 3x4 and 5x3 and 3x5 and so on just doubles the number of representations with little value.)
Vector3Wide.Scale(jacobians.LinearA, softenedEffectiveMass, out projection.WSVtoCSILinearA);
Vector3Wide.Scale(jacobians.AngularA, softenedEffectiveMass, out projection.WSVtoCSIAngularA);
Vector3Wide.Scale(jacobians.LinearB, softenedEffectiveMass, out projection.WSVtoCSILinearB);
Vector3Wide.Scale(jacobians.AngularB, softenedEffectiveMass, out projection.WSVtoCSIAngularB);
}
public void Test2(
ref CapsuleWide a, ref BoxWide b,
ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB,
out Convex2ContactManifoldWide manifold)
{
QuaternionWide.Conjugate(ref orientationB, out var toLocalB);
QuaternionWide.TransformWithoutOverlap(ref offsetB, ref toLocalB, out var localOffsetA);
Vector3Wide.Negate(ref localOffsetA);
QuaternionWide.ConcatenateWithoutOverlap(ref orientationA, ref toLocalB, out var boxLocalOrientationA);
QuaternionWide.TransformUnitY(ref boxLocalOrientationA, out var capsuleAxis);
//For each face of the box, clip the capsule axis against its bounding planes and then clamp the result to the box's extent along that axis.
//The face which has the largest clipped interval of the capsule line segment will be picked as the representative.
var positive = Vector <float> .One;
var negative = -positive;
var divisionGuard = new Vector <float>(1e-15f);
//Division by zero is hacked away by clamping the magnitude to some nonzero value and recovering the sign in the numerator.
//The resulting interval will be properly signed and enormous, so it serves the necessary purpose.
//Note the negation: t = dot(N, +-halfExtent - offsetA) / dot(N, capsuleAxis) = dot(N, offsetA +- halfExtent) / -dot(N, capsuleAxis)
var scaleX = Vector.ConditionalSelect(Vector.LessThan(capsuleAxis.X, Vector <float> .Zero), positive, negative) / Vector.Max(Vector.Abs(capsuleAxis.X), divisionGuard);
var scaleY = Vector.ConditionalSelect(Vector.LessThan(capsuleAxis.Y, Vector <float> .Zero), positive, negative) / Vector.Max(Vector.Abs(capsuleAxis.Y), divisionGuard);
var scaleZ = Vector.ConditionalSelect(Vector.LessThan(capsuleAxis.Z, Vector <float> .Zero), positive, negative) / Vector.Max(Vector.Abs(capsuleAxis.Z), divisionGuard);
Vector3Wide scaledExtents, scaledOffset;
scaledExtents.X = b.HalfWidth * Vector.Abs(scaleX);
scaledExtents.Y = b.HalfHeight * Vector.Abs(scaleY);
scaledExtents.Z = b.HalfLength * Vector.Abs(scaleZ);
scaledOffset.X = localOffsetA.X * scaleX;
scaledOffset.Y = localOffsetA.Y * scaleY;
scaledOffset.Z = localOffsetA.Z * scaleZ;
Vector3Wide.Subtract(ref scaledOffset, ref scaledExtents, out var tCandidateMin);
var negativeHalfLength = -a.HalfLength;
Vector3Wide.Max(ref negativeHalfLength, ref tCandidateMin, out tCandidateMin);
Vector3Wide.Add(ref scaledOffset, ref scaledExtents, out var tCandidateMax);
Vector3Wide.Min(ref a.HalfLength, ref tCandidateMax, out tCandidateMax);
//We now have clipped intervals for all faces. The faces along axis X, for example, use interval max(tMin.Y, tMin.Z) to min(tMax.Y, tMax.Z).
//Find the longest interval. Note that it's possible for the interval length to be negative when a line segment does not enter the face's voronoi region.
//It may be negative for all axes, even; that can happen when the line segment is outside all face voronoi regions. That's fine, though.
//It's simply a signal that the line segment is in one of the face-adjacent voronoi regions, and clamping the endpoints will provide the closest point on the box to the segment.
var intervalMaxX = Vector.Min(tCandidateMax.Y, tCandidateMax.Z);
var intervalMaxY = Vector.Min(tCandidateMax.X, tCandidateMax.Z);
var intervalMaxZ = Vector.Min(tCandidateMax.X, tCandidateMax.Y);
var intervalMinX = Vector.Max(tCandidateMin.Y, tCandidateMin.Z);
var intervalMinY = Vector.Max(tCandidateMin.X, tCandidateMin.Z);
var intervalMinZ = Vector.Max(tCandidateMin.X, tCandidateMin.Y);
var intervalLengthX = Vector.Max(Vector <float> .Zero, intervalMaxX - intervalMinX);
var intervalLengthY = Vector.Max(Vector <float> .Zero, intervalMaxY - intervalMinY);
var intervalLengthZ = Vector.Max(Vector <float> .Zero, intervalMaxZ - intervalMinZ);
var x2 = capsuleAxis.X * capsuleAxis.X;
var y2 = capsuleAxis.Y * capsuleAxis.Y;
var z2 = capsuleAxis.Z * capsuleAxis.Z;
var projectedLengthSquaredX = (y2 + z2) * intervalLengthX * intervalLengthX;
var projectedLengthSquaredY = (x2 + z2) * intervalLengthY * intervalLengthY;
var projectedLengthSquaredZ = (x2 + y2) * intervalLengthZ * intervalLengthZ;
var maxLengthSquared = Vector.Max(projectedLengthSquaredX, Vector.Max(projectedLengthSquaredY, projectedLengthSquaredZ));
var useX = Vector.Equals(projectedLengthSquaredX, maxLengthSquared);
var useY = Vector.Equals(projectedLengthSquaredY, maxLengthSquared);
var tMin = Vector.ConditionalSelect(useX, intervalMinX, Vector.ConditionalSelect(useY, intervalMinY, intervalMinZ));
var tMax = Vector.ConditionalSelect(useX, intervalMaxX, Vector.ConditionalSelect(useY, intervalMaxY, intervalMaxZ));
Vector3Wide.Scale(ref capsuleAxis, ref tMin, out var capsuleStart);
Vector3Wide.Scale(ref capsuleAxis, ref tMax, out var capsuleEnd);
Vector3Wide.Add(ref localOffsetA, ref capsuleStart, out capsuleStart);
Vector3Wide.Add(ref localOffsetA, ref capsuleEnd, out capsuleEnd);
//Clamp the start and end to the box. Note that we haven't kept track of which axis we're working on, so we just do clamping on all three axes. Two axes will be wasted,
//but it's easier to do this than to try to reconstruct which axis is the one we're supposed to work on.
Vector3Wide boxStart, boxEnd;
boxStart.X = Vector.Max(Vector.Min(capsuleStart.X, b.HalfWidth), -b.HalfWidth);
boxStart.Y = Vector.Max(Vector.Min(capsuleStart.Y, b.HalfHeight), -b.HalfHeight);
boxStart.Z = Vector.Max(Vector.Min(capsuleStart.Z, b.HalfLength), -b.HalfLength);
boxEnd.X = Vector.Max(Vector.Min(capsuleEnd.X, b.HalfWidth), -b.HalfWidth);
boxEnd.Y = Vector.Max(Vector.Min(capsuleEnd.Y, b.HalfHeight), -b.HalfHeight);
boxEnd.Z = Vector.Max(Vector.Min(capsuleEnd.Z, b.HalfLength), -b.HalfLength);
//Compute the closest point on the line segment to the clamped start and end.
Vector3Wide.Subtract(ref boxStart, ref localOffsetA, out var aToBoxStart);
Vector3Wide.Subtract(ref boxEnd, ref localOffsetA, out var aToBoxEnd);
Vector3Wide.Dot(ref capsuleAxis, ref aToBoxStart, out var dotStart);
Vector3Wide.Dot(ref capsuleAxis, ref aToBoxEnd, out var dotEnd);
dotStart = Vector.Max(Vector.Min(dotStart, a.HalfLength), negativeHalfLength);
dotEnd = Vector.Max(Vector.Min(dotEnd, a.HalfLength), negativeHalfLength);
//Now compute the distances. The normal will be defined by the shorter of the two offsets.
Vector3Wide.Scale(ref capsuleAxis, ref dotStart, out var closestFromBoxStart);
Vector3Wide.Scale(ref capsuleAxis, ref dotEnd, out var closestFromBoxEnd);
Vector3Wide.Add(ref closestFromBoxStart, ref localOffsetA, out closestFromBoxStart);
Vector3Wide.Add(ref closestFromBoxEnd, ref localOffsetA, out closestFromBoxEnd);
//Note that these offsets are pointing from the box to the capsule (B to A), matching contact normal convention.
Vector3Wide.Subtract(ref closestFromBoxStart, ref boxStart, out var startOffset);
Vector3Wide.Subtract(ref closestFromBoxEnd, ref boxEnd, out var endOffset);
Vector3Wide.LengthSquared(ref startOffset, out var startLengthSquared);
Vector3Wide.LengthSquared(ref endOffset, out var endLengthSquared);
var minLengthSquared = Vector.Min(startLengthSquared, endLengthSquared);
var useStart = Vector.Equals(minLengthSquared, startLengthSquared);
Vector3Wide.ConditionalSelect(ref useStart, ref startOffset, ref endOffset, out var localNormal);
var inverseLength = Vector <float> .One / Vector.SquareRoot(minLengthSquared);
Vector3Wide.Scale(ref localNormal, ref inverseLength, out localNormal);
//Note that we defer contact position offseting by the normal * depth until later when we know the normal isn't going to change.
Vector3Wide.Subtract(ref closestFromBoxStart, ref localOffsetA, out var localA0);
Vector3Wide.Subtract(ref closestFromBoxEnd, ref localOffsetA, out var localA1);
Vector3Wide.Dot(ref localNormal, ref startOffset, out var dot0);
Vector3Wide.Dot(ref localNormal, ref endOffset, out var dot1);
manifold.Depth0 = a.Radius - dot0;
manifold.Depth1 = a.Radius - dot1;
manifold.FeatureId0 = Vector <int> .Zero;
manifold.FeatureId1 = Vector <int> .One;
//Capsule-box has an extremely important property:
//Capsule internal line segments almost never intersect the box due to the capsule's radius.
//We can effectively split this implementation into two tests- exterior and interior.
//The interior test only needs to be run if one of the subpairs happens to be deeply intersecting, which should be ~never in practice.
//That's fortunate, because computing an accurate interior normal is painful!
var useInterior = Vector.LessThan(minLengthSquared, new Vector <float>(1e-10f));
if (Vector.EqualsAny(useInterior, new Vector <int>(-1)))
{
//There are six possible separating axes when the capsule's internal line segment intersects the box:
//box.X
//box.Y
//box.Z
//box.X x capsule.Y
//box.Y x capsule.Y
//box.Z x capsule.Y
//Choose the axis with the minimum penetration depth.
//By working in the box's local space, box.X/Y/Z become simply (1,0,0), (0,1,0), and (0,0,1). This cancels out a whole bunch of arithmetic.
//Note that these depths will not take into account the radius until the very end. We pretend that the capsule is simply a line until then, since the radius changes nothing.
var faceDepthX = b.HalfWidth - localOffsetA.X + Vector.Abs(capsuleAxis.X * a.HalfLength);
var faceDepthY = b.HalfHeight - localOffsetA.Y + Vector.Abs(capsuleAxis.Y * a.HalfLength);
var faceDepthZ = b.HalfLength - localOffsetA.Z + Vector.Abs(capsuleAxis.Z * a.HalfLength);
var faceDepth = Vector.Min(Vector.Min(faceDepthX, faceDepthY), faceDepthZ);
var useFaceX = Vector.Equals(faceDepth, faceDepthX);
var useFaceY = Vector.AndNot(Vector.Equals(faceDepth, faceDepthY), useFaceX);
var useFaceZ = Vector.OnesComplement(Vector.BitwiseOr(useFaceX, useFaceY));
//Note that signs are calibrated as a last step so that edge normals do not need their own calibration.
Vector3Wide faceNormal;
faceNormal.X = Vector.ConditionalSelect(useFaceX, Vector <float> .One, Vector <float> .Zero);
faceNormal.Y = Vector.ConditionalSelect(useFaceY, Vector <float> .One, Vector <float> .Zero);
faceNormal.Z = Vector.ConditionalSelect(useFaceZ, Vector <float> .One, Vector <float> .Zero);
//For each edge, given the zero components, the cross product boils down to creating a perpendicular 2d vector on the plane of the box axis from the cylinder axis.
var infiniteDepth = new Vector <float>(float.MaxValue);
var bestDepth = infiniteDepth;
var edgeXLength = Vector.SquareRoot(y2 + z2);
var edgeYLength = Vector.SquareRoot(x2 + z2);
var edgeZLength = Vector.SquareRoot(x2 + y2);
//depth = dot(N, halfExtents - offsetA), where N = (edgeAxis x capsuleAxis) / ||(edgeAxis x capsuleAxis)||, and both N and halfExtents have their signs calibrated.
//Using abs allows us to handle the sign calibration inline at the cost of splitting the dot product.
var inverseLengthX = Vector <float> .One / edgeXLength;
var inverseLengthY = Vector <float> .One / edgeYLength;
var inverseLengthZ = Vector <float> .One / edgeZLength;
var edgeXDepth = (Vector.Abs(b.HalfHeight * capsuleAxis.Z - b.HalfLength * capsuleAxis.Y) - Vector.Abs(offsetB.Y * capsuleAxis.Z - offsetB.Z * capsuleAxis.Y)) * inverseLengthX;
var edgeYDepth = (Vector.Abs(b.HalfWidth * capsuleAxis.Z - b.HalfLength * capsuleAxis.X) - Vector.Abs(offsetB.X * capsuleAxis.Z - offsetB.Z * capsuleAxis.X)) * inverseLengthY;
var edgeZDepth = (Vector.Abs(b.HalfWidth * capsuleAxis.Y - b.HalfHeight * capsuleAxis.X) - Vector.Abs(offsetB.X * capsuleAxis.Y - offsetB.Y * capsuleAxis.X)) * inverseLengthZ;
var epsilon = new Vector <float>(1e-9f);
//If the capsule internal segment and the edge are parallel, we ignore the edge by replacing its depth with ~infinity.
//Such parallel axes are guaranteed to do no better than one of the face directions since this path is only relevant during segment-box deep collisions.
edgeXDepth = Vector.ConditionalSelect(Vector.LessThan(edgeXLength, epsilon), infiniteDepth, edgeXDepth);
edgeYDepth = Vector.ConditionalSelect(Vector.LessThan(edgeYLength, epsilon), infiniteDepth, edgeYDepth);
edgeZDepth = Vector.ConditionalSelect(Vector.LessThan(edgeZLength, epsilon), infiniteDepth, edgeZDepth);
var edgeDepth = Vector.Min(Vector.Min(edgeXDepth, edgeYDepth), edgeZDepth);
var useEdgeX = Vector.Equals(edgeDepth, edgeXDepth);
var useEdgeY = Vector.Equals(edgeDepth, edgeYDepth);
Vector3Wide edgeNormal;
edgeNormal.X = Vector.ConditionalSelect(useEdgeX, Vector <float> .Zero, Vector.ConditionalSelect(useEdgeY, capsuleAxis.Z, capsuleAxis.Y));
edgeNormal.Y = Vector.ConditionalSelect(useEdgeX, capsuleAxis.Z, Vector.ConditionalSelect(useEdgeY, Vector <float> .Zero, -capsuleAxis.X));
edgeNormal.Z = Vector.ConditionalSelect(useEdgeX, -capsuleAxis.Y, Vector.ConditionalSelect(useEdgeY, -capsuleAxis.X, Vector <float> .Zero));
var useEdge = Vector.LessThan(edgeDepth, faceDepth);
Vector3Wide.ConditionalSelect(ref useEdge, ref edgeNormal, ref faceNormal, out var interiorNormal);
//Calibrate the normal such that it points from B to A.
Vector3Wide.Dot(ref interiorNormal, ref offsetB, out var dot);
var shouldNegateNormal = Vector.GreaterThan(dot, Vector <float> .Zero);
interiorNormal.X = Vector.ConditionalSelect(shouldNegateNormal, -interiorNormal.X, interiorNormal.X);
interiorNormal.Y = Vector.ConditionalSelect(shouldNegateNormal, -interiorNormal.Y, interiorNormal.Y);
interiorNormal.Z = Vector.ConditionalSelect(shouldNegateNormal, -interiorNormal.Z, interiorNormal.Z);
//Each contact may have its own depth.
//Imagine a face collision- if the capsule axis isn't fully parallel with the plane's surface, it would be strange to use the same depth for both contacts.
//Compute the interval of the box on the normal. Note that the normal is already calibrated to point from B to A (box to capsule).
var boxExtreme = Vector.Abs(localNormal.X * b.HalfWidth) + Vector.Abs(localNormal.Y * b.HalfHeight) + Vector.Abs(localNormal.Z * b.HalfLength);
Vector3Wide.Dot(ref localNormal, ref closestFromBoxStart, out dot0);
Vector3Wide.Dot(ref localNormal, ref closestFromBoxEnd, out dot1);
manifold.Depth0 = Vector.ConditionalSelect(useInterior, a.Radius + boxExtreme - dot0, manifold.Depth0);
manifold.Depth1 = Vector.ConditionalSelect(useInterior, a.Radius + boxExtreme - dot1, manifold.Depth1);
//The interior normal doesn't check all possible separating axes in the non-segment-intersecting case.
//It would be wrong to overwrite the initial test if the interior test was unnecessary.
Vector3Wide.ConditionalSelect(ref useInterior, ref interiorNormal, ref localNormal, out localNormal);
}
//Transform A0, A1, and the normal into world space.
Matrix3x3Wide.CreateFromQuaternion(ref orientationB, out var orientationMatrixB);
Matrix3x3Wide.TransformWithoutOverlap(ref localNormal, ref orientationMatrixB, out manifold.Normal);
Matrix3x3Wide.TransformWithoutOverlap(ref localA0, ref orientationMatrixB, out manifold.OffsetA0);
Matrix3x3Wide.TransformWithoutOverlap(ref localA1, ref orientationMatrixB, out manifold.OffsetA1);
//Apply the normal offset to the contact positions.
var negativeOffsetFromA0 = manifold.Depth0 * 0.5f - a.Radius;
var negativeOffsetFromA1 = manifold.Depth1 * 0.5f - a.Radius;
Vector3Wide.Scale(ref manifold.Normal, ref negativeOffsetFromA0, out var normalPush0);
Vector3Wide.Scale(ref manifold.Normal, ref negativeOffsetFromA1, out var normalPush1);
Vector3Wide.Add(ref manifold.OffsetA0, ref normalPush0, out manifold.OffsetA0);
Vector3Wide.Add(ref manifold.OffsetA1, ref normalPush1, out manifold.OffsetA1);
//Note that it is possible that the two contacts is redundant. Only keep them both if they're separated.
manifold.Count = Vector.ConditionalSelect(Vector.LessThan(Vector.Abs(dotStart - dotEnd), a.HalfLength * new Vector <float>(1e-7f)), Vector <int> .One, new Vector <int>(2));
}
public void Test(
ref CapsuleWide a, ref CapsuleWide b, ref Vector <float> speculativeMargin,
ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount,
out Convex2ContactManifoldWide manifold)
{
//Compute the closest points between the two line segments. No clamping to begin with.
//We want to minimize distance = ||(a + da * ta) - (b + db * tb)||.
//Taking the derivative with respect to ta and doing some algebra (taking into account ||da|| == ||db|| == 1) to solve for ta yields:
//ta = (da * (b - a) + (db * (a - b)) * (da * db)) / (1 - ((da * db) * (da * db))
QuaternionWide.TransformUnitXY(orientationA, out var xa, out var da);
QuaternionWide.TransformUnitY(orientationB, out var db);
Vector3Wide.Dot(da, offsetB, out var daOffsetB);
Vector3Wide.Dot(db, offsetB, out var dbOffsetB);
Vector3Wide.Dot(da, db, out var dadb);
//Note potential division by zero when the axes are parallel. Arbitrarily clamp; near zero values will instead produce extreme values which get clamped to reasonable results.
var ta = (daOffsetB - dbOffsetB * dadb) / Vector.Max(new Vector <float>(1e-15f), Vector <float> .One - dadb * dadb);
//tb = ta * (da * db) - db * (b - a)
var tb = ta * dadb - dbOffsetB;
//We cannot simply clamp the ta and tb values to the capsule line segments. Instead, project each line segment onto the other line segment, clamping against the target's interval.
//That new clamped projected interval is the valid solution space on that line segment. We can clamp the t value by that interval to get the correctly bounded solution.
//The projected intervals are:
//B onto A: +-BHalfLength * (da * db) + da * offsetB
//A onto B: +-AHalfLength * (da * db) - db * offsetB
var absdadb = Vector.Abs(dadb);
var bOntoAOffset = b.HalfLength * absdadb;
var aOntoBOffset = a.HalfLength * absdadb;
var aMin = Vector.Max(-a.HalfLength, Vector.Min(a.HalfLength, daOffsetB - bOntoAOffset));
var aMax = Vector.Min(a.HalfLength, Vector.Max(-a.HalfLength, daOffsetB + bOntoAOffset));
var bMin = Vector.Max(-b.HalfLength, Vector.Min(b.HalfLength, -aOntoBOffset - dbOffsetB));
var bMax = Vector.Min(b.HalfLength, Vector.Max(-b.HalfLength, aOntoBOffset - dbOffsetB));
ta = Vector.Min(Vector.Max(ta, aMin), aMax);
tb = Vector.Min(Vector.Max(tb, bMin), bMax);
Vector3Wide.Scale(da, ta, out var closestPointOnA);
Vector3Wide.Scale(db, tb, out var closestPointOnB);
Vector3Wide.Add(closestPointOnB, offsetB, out closestPointOnB);
//Note that normals are calibrated to point from B to A by convention.
Vector3Wide.Subtract(closestPointOnA, closestPointOnB, out manifold.Normal);
Vector3Wide.Length(manifold.Normal, out var distance);
var inverseDistance = Vector <float> .One / distance;
Vector3Wide.Scale(manifold.Normal, inverseDistance, out manifold.Normal);
//In the event that the line segments are touching, the normal doesn't exist and we need an alternative. Any direction along the local horizontal (XZ) plane of either capsule
//is valid. (Normals along the local Y axes are not guaranteed to be as quick of a path to separation due to nonzero line length.)
var normalIsValid = Vector.GreaterThan(distance, new Vector <float>(1e-7f));
Vector3Wide.ConditionalSelect(normalIsValid, manifold.Normal, xa, out manifold.Normal);
//In the event that the two capsule axes are coplanar, we accept the whole interval as a source of contact.
//As the axes drift away from coplanarity, the accepted interval rapidly narrows to zero length, centered on ta and tb.
//We rate the degree of coplanarity based on the angle between the capsule axis and the plane defined by the opposing segment and contact normal:
//sin(angle) = dot(da, (db x normal)/||db x normal||)
//Finally, note that we are dealing with extremely small angles, and for small angles sin(angle) ~= angle,
//and also that fade behavior is completely arbitrary, so we can directly use squared angle without any concern.
//angle^2 ~= dot(da, (db x normal))^2 / ||db x normal||^2
//Note that if ||db x normal|| is zero, then any da should be accepted as being coplanar because there is no restriction. ConditionalSelect away the discontinuity.
Vector3Wide.CrossWithoutOverlap(db, manifold.Normal, out var planeNormal);
Vector3Wide.LengthSquared(planeNormal, out var planeNormalLengthSquared);
Vector3Wide.Dot(da, planeNormal, out var numeratorUnsquared);
var squaredAngle = Vector.ConditionalSelect(Vector.LessThan(planeNormalLengthSquared, new Vector <float>(1e-10f)), Vector <float> .Zero, numeratorUnsquared * numeratorUnsquared / planeNormalLengthSquared);
//Convert the squared angle to a lerp parameter. For squared angle from 0 to lowerThreshold, we should use the full interval (1). From lowerThreshold to upperThreshold, lerp to 0.
const float lowerThresholdAngle = 0.01f;
const float upperThresholdAngle = 0.05f;
const float lowerThreshold = lowerThresholdAngle * lowerThresholdAngle;
const float upperThreshold = upperThresholdAngle * upperThresholdAngle;
var intervalWeight = Vector.Max(Vector <float> .Zero, Vector.Min(Vector <float> .One, (new Vector <float>(upperThreshold) - squaredAngle) * new Vector <float>(1f / (upperThreshold - lowerThreshold))));
//If the line segments intersect, even if they're coplanar, we would ideally stick to using a single point. Would be easy enough,
//but we don't bother because it's such a weird and extremely temporary corner case. Not really worth handling.
var weightedTa = ta - ta * intervalWeight;
aMin = intervalWeight * aMin + weightedTa;
aMax = intervalWeight * aMax + weightedTa;
Vector3Wide.Scale(da, aMin, out manifold.OffsetA0);
Vector3Wide.Scale(da, aMax, out manifold.OffsetA1);
//In the coplanar case, there are two points. We need a method of computing depth which gives a reasonable result to the second contact.
//Note that one of the two contacts should end up with a distance equal to the previously computed segment distance, so we're doing some redundant work here.
//It's just easier to do that extra work than it would be to track which endpoint contributed the lower distance.
//Unproject the final interval endpoints from a back onto b.
//dot(offsetB + db * tb0, da) = ta0
//tb0 = (ta0 - daOffsetB) / dadb
//distance0 = dot(a0 - (offsetB + tb0 * db), normal)
//distance1 = dot(a1 - (offsetB + tb1 * db), normal)
Vector3Wide.Dot(db, manifold.Normal, out var dbNormal);
Vector3Wide.Subtract(manifold.OffsetA0, offsetB, out var offsetB0);
Vector3Wide.Subtract(manifold.OffsetA1, offsetB, out var offsetB1);
//Note potential division by zero. In that case, treat both projected points as the closest point. (Handled by the conditional select that chooses the previously computed distance.)
var inverseDadb = Vector <float> .One / dadb;
var projectedTb0 = Vector.Max(bMin, Vector.Min(bMax, (aMin - daOffsetB) * inverseDadb));
var projectedTb1 = Vector.Max(bMin, Vector.Min(bMax, (aMax - daOffsetB) * inverseDadb));
Vector3Wide.Dot(offsetB0, manifold.Normal, out var b0Normal);
Vector3Wide.Dot(offsetB1, manifold.Normal, out var b1Normal);
var capsulesArePerpendicular = Vector.LessThan(Vector.Abs(dadb), new Vector <float>(1e-7f));
var distance0 = Vector.ConditionalSelect(capsulesArePerpendicular, distance, b0Normal - dbNormal * projectedTb0);
var distance1 = Vector.ConditionalSelect(capsulesArePerpendicular, distance, b1Normal - dbNormal * projectedTb1);
var combinedRadius = a.Radius + b.Radius;
manifold.Depth0 = combinedRadius - distance0;
manifold.Depth1 = combinedRadius - distance1;
//Apply the normal offset to the contact positions.
var negativeOffsetFromA0 = manifold.Depth0 * 0.5f - a.Radius;
var negativeOffsetFromA1 = manifold.Depth1 * 0.5f - a.Radius;
Vector3Wide.Scale(manifold.Normal, negativeOffsetFromA0, out var normalPush0);
Vector3Wide.Scale(manifold.Normal, negativeOffsetFromA1, out var normalPush1);
Vector3Wide.Add(manifold.OffsetA0, normalPush0, out manifold.OffsetA0);
Vector3Wide.Add(manifold.OffsetA1, normalPush1, out manifold.OffsetA1);
manifold.FeatureId0 = Vector <int> .Zero;
manifold.FeatureId1 = Vector <int> .One;
var minimumAcceptedDepth = -speculativeMargin;
manifold.Contact0Exists = Vector.GreaterThanOrEqual(manifold.Depth0, minimumAcceptedDepth);
manifold.Contact1Exists = Vector.BitwiseAnd(
Vector.GreaterThanOrEqual(manifold.Depth1, minimumAcceptedDepth),
Vector.GreaterThan(aMax - aMin, new Vector <float>(1e-7f) * a.HalfLength));
//TODO: Since we added in the complexity of 2 contact support, this is probably large enough to benefit from working in the local space of one of the capsules.
//Worth looking into later.
}
public void Test(ref SphereWide a, ref TriangleWide b, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB,
out Vector <int> intersected, out Vector <float> distance, out Vector3Wide closestA, out Vector3Wide normal)
{
//Note that we're borrowing a lot here from the SphereTriangleCollisionTask. Could share more if you find yourself needing to change things dramatically.
//Main difficulty in fully sharing is that sweep tests do not honor one sidedness, so some of the conditions change.
//Work in the local space of the triangle, since it's quicker to transform the sphere position than the vertices of the triangle.
Matrix3x3Wide.CreateFromQuaternion(orientationB, out var rB);
Matrix3x3Wide.TransformByTransposedWithoutOverlap(offsetB, rB, out var localOffsetB);
Vector3Wide.Subtract(b.B, b.A, out var ab);
Vector3Wide.Subtract(b.C, b.A, out var ac);
//localOffsetA = -localOffsetB, so pa = triangle.A + localOffsetB.
Vector3Wide.Add(b.A, localOffsetB, out var pa);
Vector3Wide.CrossWithoutOverlap(ab, ac, out var localTriangleNormal);
Vector3Wide.Dot(localTriangleNormal, pa, out var paN);
//EdgeAB plane test: (pa x ab) * (ab x ac) >= 0
//EdgeAC plane test: (ac x pa) * (ab x ac) >= 0
//Note that these are scaled versions of the barycentric coordinates.
//To normalize them such that the weights of a point within the triangle equal 1, we just need to divide by dot(ab x ac, ab x ac).
//In other words, to test the third edge plane, we can ensure that the unnormalized weights are both positive and sum to a value less than dot(ab x ac, ab x ac).
//If a point is outside of an edge plane, we know that it's not in the face region or any other edge region. It could, however, be in an adjacent vertex region.
//Vertex cases can be handled by clamping an edge case.
//Further, note that for any query location, it is sufficient to only test one edge even if the point is outside two edge planes. If it's outside two edge planes,
//that just means it's going to be on the shared vertex, so a clamped edge test captures the correct closest point.
//So, at each edge, if the point is outside the plane, cache the edge. The last edge registering an outside result will be tested.
//(pa x ab) * (ab x ac) = (pa * ab) * (ab * ac) - (pa * ac) * (ab * ab)
//(ac x pa) * (ab x ac) = (ac * ab) * (pa * ac) - (ac * ac) * (pa * ab)
//(ab x ac) * (ab x ac) = (ab * ab) * (ac * ac) - (ab * ac) * (ab * ac)
Vector3Wide.Dot(pa, ab, out var abpa);
Vector3Wide.Dot(ab, ac, out var abac);
Vector3Wide.Dot(ac, pa, out var acpa);
Vector3Wide.Dot(ac, ac, out var acac);
Vector3Wide.Dot(ab, ab, out var abab);
var edgePlaneTestAB = abpa * abac - acpa * abab;
var edgePlaneTestAC = abac * acpa - acac * abpa;
var triangleNormalLengthSquared = abab * acac - abac * abac;
var edgePlaneTestBC = triangleNormalLengthSquared - edgePlaneTestAB - edgePlaneTestAC;
var outsideAB = Vector.LessThan(edgePlaneTestAB, Vector <float> .Zero);
var outsideAC = Vector.LessThan(edgePlaneTestAC, Vector <float> .Zero);
var outsideBC = Vector.LessThan(edgePlaneTestBC, Vector <float> .Zero);
var outsideAnyEdge = Vector.BitwiseOr(outsideAB, Vector.BitwiseOr(outsideAC, outsideBC));
Vector3Wide localClosestOnTriangle;
var negativeOne = new Vector <int>(-1);
if (Vector.EqualsAny(outsideAnyEdge, negativeOne))
{
//At least one lane detected a point outside of the triangle. Choose one edge which is outside as the representative.
Vector3Wide.ConditionalSelect(outsideAC, ac, ab, out var edgeDirection);
Vector3Wide.Subtract(b.C, b.B, out var bc);
Vector3Wide.ConditionalSelect(outsideBC, bc, edgeDirection, out edgeDirection);
Vector3Wide.ConditionalSelect(outsideBC, b.B, b.A, out var edgeStart);
Vector3Wide.Add(localOffsetB, edgeStart, out var negativeEdgeStartToP);
//This does some partially redundant work if the edge is AB or AC, but given that we didn't have bcbc or bcpb, it's fine.
Vector3Wide.Dot(negativeEdgeStartToP, edgeDirection, out var negativeOffsetDotEdge);
Vector3Wide.Dot(edgeDirection, edgeDirection, out var edgeDotEdge);
var edgeScale = Vector.Max(Vector <float> .Zero, Vector.Min(Vector <float> .One, -negativeOffsetDotEdge / edgeDotEdge));
Vector3Wide.Scale(edgeDirection, edgeScale, out var pointOnEdge);
Vector3Wide.Add(edgeStart, pointOnEdge, out pointOnEdge);
Vector3Wide.ConditionalSelect(outsideAnyEdge, pointOnEdge, localClosestOnTriangle, out localClosestOnTriangle);
}
if (Vector.EqualsAny(outsideAnyEdge, Vector <int> .Zero))
{
//p + N * (pa * N) / ||N||^2 = N * (pa * N) / ||N||^2 - (-p)
var nScale = paN / triangleNormalLengthSquared;
Vector3Wide.Scale(localTriangleNormal, nScale, out var offsetToPlane);
Vector3Wide.Subtract(offsetToPlane, localOffsetB, out var pointOnFace);
Vector3Wide.ConditionalSelect(outsideAnyEdge, localClosestOnTriangle, pointOnFace, out localClosestOnTriangle);
}
//normal = normalize(localOffsetA - localClosestOnTriangle) = (localOffsetB + localClosestOnTriangle) / (-||localOffsetB + localClosestOnTriangle||)
Vector3Wide.Add(localOffsetB, localClosestOnTriangle, out var localNormal);
Vector3Wide.Length(localNormal, out var localNormalLength);
Vector3Wide.Scale(localNormal, new Vector <float>(-1f) / localNormalLength, out localNormal);
Matrix3x3Wide.TransformWithoutOverlap(localNormal, rB, out normal);
Vector3Wide.Scale(normal, -a.Radius, out closestA);
distance = localNormalLength - a.Radius;
intersected = Vector.LessThanOrEqual(distance, Vector <float> .Zero);
}