public static void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref Vector3Wide offsetA, ref Vector3Wide offsetB,
ref Vector3Wide biasVelocity, ref Symmetric3x3Wide effectiveMass, ref Vector <float> softnessImpulseScale, ref Vector <float> maximumImpulse, ref Vector3Wide accumulatedImpulse, ref BodyInertias inertiaA, ref BodyInertias inertiaB)
{
ComputeCorrectiveImpulse(ref velocityA, ref velocityB, ref offsetA, ref offsetB, ref biasVelocity, ref effectiveMass, ref softnessImpulseScale, ref accumulatedImpulse, out var correctiveImpulse);
//This function DOES have a maximum impulse limit.
ServoSettingsWide.ClampImpulse(maximumImpulse, ref accumulatedImpulse, ref correctiveImpulse);
Vector3Wide.Add(accumulatedImpulse, correctiveImpulse, out accumulatedImpulse);
ApplyImpulse(ref velocityA, ref velocityB, ref offsetA, ref offsetB, ref inertiaA, ref inertiaB, ref correctiveImpulse);
}
void SampleMinkowskiDifference(
ref TShapeWideA a, ref Matrix3x3Wide rA, ref TSupportFinderA supportFinderA,
ref TShapeWideB b, ref Matrix3x3Wide rB, ref TSupportFinderB supportFinderB, ref Vector3Wide offsetB, ref Vector3Wide direction,
out Vector3Wide supportA, out Vector3Wide support)
{
supportFinderA.ComputeSupport(ref a, ref rA, ref direction, out supportA);
Vector3Wide.Negate(direction, out var negatedDirection);
supportFinderB.ComputeSupport(ref b, ref rB, ref negatedDirection, out var supportB);
Vector3Wide.Add(supportB, offsetB, out supportB);
Vector3Wide.Subtract(supportA, supportB, out support);
}
public static void ApplyImpulse(ref Jacobians jacobians, ref BodyInertias inertiaA,
ref Vector2Wide correctiveImpulse, ref BodyVelocities wsvA)
{
Matrix2x3Wide.Transform(correctiveImpulse, jacobians.LinearA, out var linearImpulseA);
Matrix2x3Wide.Transform(correctiveImpulse, jacobians.AngularA, out var angularImpulseA);
BodyVelocities correctiveVelocityA;
Vector3Wide.Scale(linearImpulseA, inertiaA.InverseMass, out correctiveVelocityA.Linear);
Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseA, inertiaA.InverseInertiaTensor, out correctiveVelocityA.Angular);
Vector3Wide.Add(wsvA.Linear, correctiveVelocityA.Linear, out wsvA.Linear);
Vector3Wide.Add(wsvA.Angular, correctiveVelocityA.Angular, out wsvA.Angular);
}
static void GetEdgeClosestPoint(ref Vector3Wide normal, ref Vector <int> edgeDirectionIndex,
ref BoxWide box,
ref Vector3Wide offsetA, ref Vector3Wide capsuleAxis, ref Vector <float> capsuleHalfLength, out Vector3Wide closestPointFromEdge)
{
Vector3Wide.Dot(normal, offsetA, out var calibrationDot);
var flipNormal = Vector.LessThan(calibrationDot, Vector <float> .Zero);
normal.X = Vector.ConditionalSelect(flipNormal, -normal.X, normal.X);
normal.Y = Vector.ConditionalSelect(flipNormal, -normal.Y, normal.Y);
normal.Z = Vector.ConditionalSelect(flipNormal, -normal.Z, normal.Z);
Vector3Wide boxEdgeCenter;
//Note that this can result in a closest point in the middle of the box when the capsule is parallel to a face. That's fine.
boxEdgeCenter.X = Vector.ConditionalSelect(Vector.Equals(normal.X, Vector <float> .Zero), Vector <float> .Zero,
Vector.ConditionalSelect(Vector.GreaterThan(normal.X, Vector <float> .Zero), box.HalfWidth, -box.HalfWidth));
boxEdgeCenter.Y = Vector.ConditionalSelect(Vector.Equals(normal.Y, Vector <float> .Zero), Vector <float> .Zero,
Vector.ConditionalSelect(Vector.GreaterThan(normal.Y, Vector <float> .Zero), box.HalfHeight, -box.HalfHeight));
boxEdgeCenter.Z = Vector.ConditionalSelect(Vector.Equals(normal.Z, Vector <float> .Zero), Vector <float> .Zero,
Vector.ConditionalSelect(Vector.GreaterThan(normal.Z, Vector <float> .Zero), box.HalfLength, -box.HalfLength));
Vector3Wide boxEdgeDirection;
var useEdgeX = Vector.Equals(edgeDirectionIndex, Vector <int> .Zero);
var useEdgeY = Vector.Equals(edgeDirectionIndex, Vector <int> .One);
var useEdgeZ = Vector.Equals(edgeDirectionIndex, new Vector <int>(2));
boxEdgeDirection.X = Vector.ConditionalSelect(useEdgeX, Vector <float> .One, Vector <float> .Zero);
boxEdgeDirection.Y = Vector.ConditionalSelect(useEdgeY, Vector <float> .One, Vector <float> .Zero);
boxEdgeDirection.Z = Vector.ConditionalSelect(useEdgeZ, Vector <float> .One, Vector <float> .Zero);
//From CapsulePairCollisionTask, point of closest approach along the capsule axis, unbounded:
//ta = (da * (b - a) + (db * (a - b)) * (da * db)) / (1 - ((da * db) * (da * db))
//where da = capsuleAxis, db = boxEdgeDirection, a = offsetA, b = boxEdgeCenter
Vector3Wide.Subtract(boxEdgeCenter, offsetA, out var ab);
Vector3Wide.Dot(ab, capsuleAxis, out var abda);
Vector3Wide.Dot(ab, boxEdgeDirection, out var abdb);
Vector3Wide.Dot(capsuleAxis, boxEdgeDirection, out var dadb);
//Note division by zero guard.
var ta = (abda - abdb * dadb) / Vector.Max(new Vector <float>(1e-15f), (Vector <float> .One - dadb * dadb));
//In some cases, ta won't be in a useful location. Need to constrain it to the projection of the box edge onto the capsule edge.
//B onto A: +-BHalfExtent * (da * db) + da * ab
var bHalfExtent = Vector.ConditionalSelect(useEdgeX, box.HalfWidth, Vector.ConditionalSelect(useEdgeY, box.HalfHeight, box.HalfLength));
var bOntoAOffset = bHalfExtent * Vector.Abs(dadb);
var taMin = Vector.Max(-capsuleHalfLength, Vector.Min(capsuleHalfLength, abda - bOntoAOffset));
var taMax = Vector.Min(capsuleHalfLength, Vector.Max(-capsuleHalfLength, abda + bOntoAOffset));
ta = Vector.Min(Vector.Max(ta, taMin), taMax);
Vector3Wide.Scale(capsuleAxis, ta, out var offsetAlongCapsule);
Vector3Wide.Add(offsetA, offsetAlongCapsule, out closestPointFromEdge);
}
public static void ApplyImpulse(ref PenetrationLimitOneBodyProjection projection, ref BodyInertias inertiaA, ref Vector3Wide normal,
ref Vector <float> correctiveImpulse,
ref BodyVelocities wsvA)
{
var linearVelocityChangeA = correctiveImpulse * inertiaA.InverseMass;
Vector3Wide.Scale(ref normal, ref linearVelocityChangeA, out var correctiveVelocityALinearVelocity);
Vector3Wide.Scale(ref projection.AngularA, ref correctiveImpulse, out var correctiveAngularImpulseA);
Triangular3x3Wide.TransformBySymmetricWithoutOverlap(ref correctiveAngularImpulseA, ref inertiaA.InverseInertiaTensor, out var correctiveVelocityAAngularVelocity);
Vector3Wide.Add(ref wsvA.LinearVelocity, ref correctiveVelocityALinearVelocity, out wsvA.LinearVelocity);
Vector3Wide.Add(ref wsvA.AngularVelocity, ref correctiveVelocityAAngularVelocity, out wsvA.AngularVelocity);
}
static void TestVertexAxis(ref BoxWide box, ref Vector3Wide offsetA, ref Vector3Wide capsuleAxis, ref Vector <float> capsuleHalfLength,
out Vector <float> depth, out Vector3Wide normal, out Vector3Wide closestA)
{
//The available feature pairs between the capsule axis and box are:
//Box edge - capsule endpoints: handled by capsule endpoint clamping
//Box edge - capsule axis: handled explicitly by the edge cases
//Box face - capsule endpoints: handled by capsule endpoint clamping
//Box face - capsule axis: redundant with edge-axis or face-endpoint
//Box vertex - capsule endpoints: handled by capsule endpoint clamping
//Box vertex - capsule axis: unhandled
//So here, we need to identify the maximum separating axis caused by vertices.
//We can safely ignore cases that are handled by the endpoint clamp, too.
//A brute force approach could test every vertex-axis offset as a normal, but more cleverness is possible.
//Note that this case requires that the capsule axis be in the voronoi region of a vertex. That is only possible if the offset from the box origin to the capsule axis
//supports an extreme point at the vertex.
//(That extreme point relationship may also be met in cases of intersection, but that's fine- this distance tester is not concerned with intersection beyond a boolean result.)
//closest point on axis to origin = offsetA - (offsetA * capsuleAxis) * capsuleAxis
Vector3Wide.Dot(offsetA, capsuleAxis, out var dot);
var clampedDot = Vector.Min(capsuleHalfLength, Vector.Max(-capsuleHalfLength, dot));
Vector3Wide.Scale(capsuleAxis, clampedDot, out var axisOffset);
Vector3Wide.Subtract(offsetA, axisOffset, out var closestOnAxis);
Vector3Wide vertex;
vertex.X = Vector.ConditionalSelect(Vector.LessThan(closestOnAxis.X, Vector <float> .Zero), -box.HalfWidth, box.HalfWidth);
vertex.Y = Vector.ConditionalSelect(Vector.LessThan(closestOnAxis.Y, Vector <float> .Zero), -box.HalfHeight, box.HalfHeight);
vertex.Z = Vector.ConditionalSelect(Vector.LessThan(closestOnAxis.Z, Vector <float> .Zero), -box.HalfLength, box.HalfLength);
//closest point on axis to vertex: ((vertex - offsetA) * capsuleAxis) * capsuleAxis + offsetA - vertex
Vector3Wide.Subtract(vertex, offsetA, out var capsuleCenterToVertex);
Vector3Wide.Dot(capsuleCenterToVertex, capsuleAxis, out var vertexDot);
Vector3Wide.Scale(capsuleAxis, vertexDot, out var vertexAxisOffset);
Vector3Wide.Add(vertexAxisOffset, offsetA, out closestA);
Vector3Wide.Subtract(closestA, vertex, out var vertexToClosestOnCapsule);
Vector3Wide.Length(vertexToClosestOnCapsule, out var length);
var inverseLength = Vector <float> .One / length;
Vector3Wide.Scale(vertexToClosestOnCapsule, inverseLength, out normal);
//The normal is perpendicular to the capsule axis by construction, so no need to include the capsule length extent.
depth = Vector.Abs(normal.X) * box.HalfWidth + Vector.Abs(normal.Y) * box.HalfHeight + Vector.Abs(normal.Z) * box.HalfLength -
Vector.Abs(offsetA.X * normal.X + offsetA.Y * normal.Y + offsetA.Z * normal.Z);
//Ignore degenerate cases. Worst outcome is that it reports intersection, which is pretty reasonable.
depth = Vector.ConditionalSelect(Vector.LessThan(length, new Vector <float>(1e-10f)), new Vector <float>(float.MaxValue), depth);
}
public void Test(ref CapsuleWide a, ref CapsuleWide b, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB,
out Vector <int> intersected, out Vector <float> distance, out Vector3Wide closestA, out Vector3Wide normal)
{
//Compute the closest points between the two line segments. No clamping to begin with.
//We want to minimize distance = ||(a + da * ta) - (b + db * tb)||.
//Taking the derivative with respect to ta and doing some algebra (taking into account ||da|| == ||db|| == 1) to solve for ta yields:
//ta = (da * (b - a) + (db * (a - b)) * (da * db)) / (1 - ((da * db) * (da * db))
QuaternionWide.TransformUnitXY(orientationA, out var xa, out var da);
QuaternionWide.TransformUnitY(orientationB, out var db);
Vector3Wide.Dot(da, offsetB, out var daOffsetB);
Vector3Wide.Dot(db, offsetB, out var dbOffsetB);
Vector3Wide.Dot(da, db, out var dadb);
//Note potential division by zero when the axes are parallel. Arbitrarily clamp; near zero values will instead produce extreme values which get clamped to reasonable results.
var ta = (daOffsetB - dbOffsetB * dadb) / Vector.Max(new Vector <float>(1e-15f), Vector <float> .One - dadb * dadb);
//tb = ta * (da * db) - db * (b - a)
var tb = ta * dadb - dbOffsetB;
//We cannot simply clamp the ta and tb values to the capsule line segments. Instead, project each line segment onto the other line segment, clamping against the target's interval.
//That new clamped projected interval is the valid solution space on that line segment. We can clamp the t value by that interval to get the correctly bounded solution.
//The projected intervals are:
//B onto A: +-BHalfLength * (da * db) + da * offsetB
//A onto B: +-AHalfLength * (da * db) - db * offsetB
var absdadb = Vector.Abs(dadb);
var bOntoAOffset = b.HalfLength * absdadb;
var aOntoBOffset = a.HalfLength * absdadb;
var aMin = Vector.Max(-a.HalfLength, Vector.Min(a.HalfLength, daOffsetB - bOntoAOffset));
var aMax = Vector.Min(a.HalfLength, Vector.Max(-a.HalfLength, daOffsetB + bOntoAOffset));
var bMin = Vector.Max(-b.HalfLength, Vector.Min(b.HalfLength, -aOntoBOffset - dbOffsetB));
var bMax = Vector.Min(b.HalfLength, Vector.Max(-b.HalfLength, aOntoBOffset - dbOffsetB));
ta = Vector.Min(Vector.Max(ta, aMin), aMax);
tb = Vector.Min(Vector.Max(tb, bMin), bMax);
Vector3Wide.Scale(da, ta, out closestA);
Vector3Wide.Scale(db, tb, out var closestB);
Vector3Wide.Add(closestB, offsetB, out closestB);
Vector3Wide.Subtract(closestA, closestB, out normal);
Vector3Wide.Length(normal, out distance);
var inverseDistance = Vector <float> .One / distance;
Vector3Wide.Scale(normal, inverseDistance, out normal);
Vector3Wide.Scale(normal, a.Radius, out var aOffset);
Vector3Wide.Subtract(closestA, aOffset, out closestA);
distance = distance - a.Radius - b.Radius;
intersected = Vector.LessThanOrEqual(distance, Vector <float> .Zero);
}
public static void ApplyImpulse(ref BodyVelocities velocityA, ref BodyVelocities velocityB,
ref Vector3Wide offsetA, ref Vector3Wide offsetB, ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref Vector3Wide constraintSpaceImpulse)
{
Vector3Wide.CrossWithoutOverlap(offsetA, constraintSpaceImpulse, out var wsi);
Symmetric3x3Wide.TransformWithoutOverlap(wsi, inertiaA.InverseInertiaTensor, out var change);
Vector3Wide.Add(velocityA.Angular, change, out velocityA.Angular);
Vector3Wide.Scale(constraintSpaceImpulse, inertiaA.InverseMass, out change);
Vector3Wide.Add(velocityA.Linear, change, out velocityA.Linear);
Vector3Wide.CrossWithoutOverlap(constraintSpaceImpulse, offsetB, out wsi); //note flip-negation
Symmetric3x3Wide.TransformWithoutOverlap(wsi, inertiaB.InverseInertiaTensor, out change);
Vector3Wide.Add(velocityB.Angular, change, out velocityB.Angular);
Vector3Wide.Scale(constraintSpaceImpulse, inertiaB.InverseMass, out change);
Vector3Wide.Subtract(velocityB.Linear, change, out velocityB.Linear); //note subtraction; the jacobian is -I
}
public static void ComputeCorrectiveImpulse(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref Vector3Wide offsetA, ref Vector3Wide offsetB,
ref Vector3Wide biasVelocity, ref Symmetric3x3Wide effectiveMass, ref Vector <float> softnessImpulseScale, ref Vector3Wide accumulatedImpulse, out Vector3Wide correctiveImpulse)
{
//csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular);
//Note subtraction; jLinearB = -I.
Vector3Wide.Subtract(velocityA.Linear, velocityB.Linear, out var csv);
Vector3Wide.CrossWithoutOverlap(velocityA.Angular, offsetA, out var angularCSV);
Vector3Wide.Add(csv, angularCSV, out csv);
//Note reversed cross order; matches the jacobian -CrossMatrix(offsetB).
Vector3Wide.CrossWithoutOverlap(offsetB, velocityB.Angular, out angularCSV);
Vector3Wide.Add(csv, angularCSV, out csv);
Vector3Wide.Subtract(biasVelocity, csv, out csv);
Symmetric3x3Wide.TransformWithoutOverlap(csv, effectiveMass, out correctiveImpulse);
Vector3Wide.Scale(accumulatedImpulse, softnessImpulseScale, out var softness);
Vector3Wide.Subtract(correctiveImpulse, softness, out correctiveImpulse);
}
public static void ApplyImpulse(ref Jacobians jacobians, ref BodyInertias inertiaA, ref BodyInertias inertiaB,
ref Vector2Wide correctiveImpulse, ref BodyVelocities wsvA, ref BodyVelocities wsvB)
{
Matrix2x3Wide.Transform(correctiveImpulse, jacobians.LinearA, out var linearImpulseA);
Matrix2x3Wide.Transform(correctiveImpulse, jacobians.AngularA, out var angularImpulseA);
Matrix2x3Wide.Transform(correctiveImpulse, jacobians.AngularB, out var angularImpulseB);
BodyVelocities correctiveVelocityA, correctiveVelocityB;
Vector3Wide.Scale(linearImpulseA, inertiaA.InverseMass, out correctiveVelocityA.Linear);
Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseA, inertiaA.InverseInertiaTensor, out correctiveVelocityA.Angular);
Vector3Wide.Scale(linearImpulseA, inertiaB.InverseMass, out correctiveVelocityB.Linear);
Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseB, inertiaB.InverseInertiaTensor, out correctiveVelocityB.Angular);
Vector3Wide.Add(wsvA.Linear, correctiveVelocityA.Linear, out wsvA.Linear);
Vector3Wide.Add(wsvA.Angular, correctiveVelocityA.Angular, out wsvA.Angular);
Vector3Wide.Subtract(wsvB.Linear, correctiveVelocityB.Linear, out wsvB.Linear); //note subtract- we based it on the LinearA jacobian.
Vector3Wide.Add(wsvB.Angular, correctiveVelocityB.Angular, out wsvB.Angular);
}
public void Test(ref CapsuleWide a, ref TriangleWide b, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB,
out Vector <int> intersected, out Vector <float> distance, out Vector3Wide closestA, out Vector3Wide normal)
{
//While a capsule-triangle dedicated case would likely be ~20-40% faster than using GJK, it's not really worth the effort without some motivating use case.
//Instead, we'll just use GJK, but our capsule support function only considers the internal line segment (plus GJK's epsilon).
//So, here, we'll modify the distance and closestA to account for the disparity.
GJKTester tester = default;
tester.Test(ref a, ref b, ref offsetB, ref orientationA, ref orientationB, out intersected, out distance, out closestA, out normal);
//The containment epsilon expands the minkowski sum a little bit. Get rid of that so that the distance corresponds exactly to the capsule's surface.
//(We don't have this luxury for box-box, but we might as well handle it when we're able.)
var distanceChange = new Vector <float>(GJKTester.ContainmentEpsilonDefault) - a.Radius;
distance += distanceChange;
Vector3Wide.Scale(normal, distanceChange, out var closestPointOffset);
Vector3Wide.Add(closestA, closestPointOffset, out closestA);
intersected = Vector.BitwiseOr(intersected, Vector.LessThan(distance, Vector <float> .Zero));
}
public static void ApplyImpulse(ref Projection2Body1DOF data, ref Vector <float> correctiveImpulse,
ref BodyVelocities wsvA, ref BodyVelocities wsvB)
{
//Applying the impulse requires transforming the constraint space impulse into a world space velocity change.
//The first step is to transform into a world space impulse, which requires transforming by the transposed jacobian
//(transpose(jacobian) goes from world to constraint space, jacobian goes from constraint to world space).
//That world space impulse is then converted to a corrective velocity change by scaling the impulse by the inverse mass/inertia.
//As an optimization for constraints with smaller jacobians, the jacobian * (inertia or mass) transform is precomputed.
BodyVelocities correctiveVelocityA, correctiveVelocityB;
Vector3Wide.Scale(data.CSIToWSVLinearA, correctiveImpulse, out correctiveVelocityA.Linear);
Vector3Wide.Scale(data.CSIToWSVAngularA, correctiveImpulse, out correctiveVelocityA.Angular);
Vector3Wide.Scale(data.CSIToWSVLinearB, correctiveImpulse, out correctiveVelocityB.Linear);
Vector3Wide.Scale(data.CSIToWSVAngularB, correctiveImpulse, out correctiveVelocityB.Angular);
Vector3Wide.Add(correctiveVelocityA.Linear, wsvA.Linear, out wsvA.Linear);
Vector3Wide.Add(correctiveVelocityA.Angular, wsvA.Angular, out wsvA.Angular);
Vector3Wide.Add(correctiveVelocityB.Linear, wsvB.Linear, out wsvB.Linear);
Vector3Wide.Add(correctiveVelocityB.Angular, wsvB.Angular, out wsvB.Angular);
}
public void Test(ref SphereWide a, ref 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 ApplyImpulse(ref PenetrationLimitProjection projection, ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref Vector3Wide normal,
ref Vector <float> correctiveImpulse,
ref BodyVelocities wsvA, ref BodyVelocities wsvB)
{
var linearVelocityChangeA = correctiveImpulse * inertiaA.InverseMass;
Vector3Wide.Scale(normal, linearVelocityChangeA, out var correctiveVelocityALinearVelocity);
Vector3Wide.Scale(projection.AngularA, correctiveImpulse, out var correctiveAngularImpulseA);
Symmetric3x3Wide.TransformWithoutOverlap(correctiveAngularImpulseA, inertiaA.InverseInertiaTensor, out var correctiveVelocityAAngularVelocity);
var linearVelocityChangeB = correctiveImpulse * inertiaB.InverseMass;
Vector3Wide.Scale(normal, linearVelocityChangeB, out var correctiveVelocityBLinearVelocity);
Vector3Wide.Scale(projection.AngularB, correctiveImpulse, out var correctiveAngularImpulseB);
Symmetric3x3Wide.TransformWithoutOverlap(correctiveAngularImpulseB, inertiaB.InverseInertiaTensor, out var correctiveVelocityBAngularVelocity);
Vector3Wide.Add(wsvA.Linear, correctiveVelocityALinearVelocity, out wsvA.Linear);
Vector3Wide.Add(wsvA.Angular, correctiveVelocityAAngularVelocity, out wsvA.Angular);
Vector3Wide.Subtract(wsvB.Linear, correctiveVelocityBLinearVelocity, out wsvB.Linear); //Note subtract; normal = -jacobianLinearB
Vector3Wide.Add(wsvB.Angular, correctiveVelocityBAngularVelocity, out wsvB.Angular);
}
public void Test(ref SphereWide a, ref TriangleWide b, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB,
out Vector <int> intersected, out Vector <float> distance, out Vector3Wide closestA, out Vector3Wide normal)
{
//Note that we're borrowing a lot here from the SphereTriangleCollisionTask. Could share more if you find yourself needing to change things dramatically.
//Main difficulty in fully sharing is that sweep tests do not honor one sidedness, so some of the conditions change.
//Work in the local space of the triangle, since it's quicker to transform the sphere position than the vertices of the triangle.
Matrix3x3Wide.CreateFromQuaternion(orientationB, out var rB);
Matrix3x3Wide.TransformByTransposedWithoutOverlap(offsetB, rB, out var localOffsetB);
Vector3Wide.Subtract(b.B, b.A, out var ab);
Vector3Wide.Subtract(b.C, b.A, out var ac);
//localOffsetA = -localOffsetB, so pa = triangle.A + localOffsetB.
Vector3Wide.Add(b.A, localOffsetB, out var pa);
Vector3Wide.CrossWithoutOverlap(ab, ac, out var localTriangleNormal);
Vector3Wide.Dot(localTriangleNormal, pa, out var paN);
//EdgeAB plane test: (pa x ab) * (ab x ac) >= 0
//EdgeAC plane test: (ac x pa) * (ab x ac) >= 0
//Note that these are scaled versions of the barycentric coordinates.
//To normalize them such that the weights of a point within the triangle equal 1, we just need to divide by dot(ab x ac, ab x ac).
//In other words, to test the third edge plane, we can ensure that the unnormalized weights are both positive and sum to a value less than dot(ab x ac, ab x ac).
//If a point is outside of an edge plane, we know that it's not in the face region or any other edge region. It could, however, be in an adjacent vertex region.
//Vertex cases can be handled by clamping an edge case.
//Further, note that for any query location, it is sufficient to only test one edge even if the point is outside two edge planes. If it's outside two edge planes,
//that just means it's going to be on the shared vertex, so a clamped edge test captures the correct closest point.
//So, at each edge, if the point is outside the plane, cache the edge. The last edge registering an outside result will be tested.
//(pa x ab) * (ab x ac) = (pa * ab) * (ab * ac) - (pa * ac) * (ab * ab)
//(ac x pa) * (ab x ac) = (ac * ab) * (pa * ac) - (ac * ac) * (pa * ab)
//(ab x ac) * (ab x ac) = (ab * ab) * (ac * ac) - (ab * ac) * (ab * ac)
Vector3Wide.Dot(pa, ab, out var abpa);
Vector3Wide.Dot(ab, ac, out var abac);
Vector3Wide.Dot(ac, pa, out var acpa);
Vector3Wide.Dot(ac, ac, out var acac);
Vector3Wide.Dot(ab, ab, out var abab);
var edgePlaneTestAB = abpa * abac - acpa * abab;
var edgePlaneTestAC = abac * acpa - acac * abpa;
var triangleNormalLengthSquared = abab * acac - abac * abac;
var edgePlaneTestBC = triangleNormalLengthSquared - edgePlaneTestAB - edgePlaneTestAC;
var outsideAB = Vector.LessThan(edgePlaneTestAB, Vector <float> .Zero);
var outsideAC = Vector.LessThan(edgePlaneTestAC, Vector <float> .Zero);
var outsideBC = Vector.LessThan(edgePlaneTestBC, Vector <float> .Zero);
var outsideAnyEdge = Vector.BitwiseOr(outsideAB, Vector.BitwiseOr(outsideAC, outsideBC));
Vector3Wide localClosestOnTriangle;
var negativeOne = new Vector <int>(-1);
if (Vector.EqualsAny(outsideAnyEdge, negativeOne))
{
//At least one lane detected a point outside of the triangle. Choose one edge which is outside as the representative.
Vector3Wide.ConditionalSelect(outsideAC, ac, ab, out var edgeDirection);
Vector3Wide.Subtract(b.C, b.B, out var bc);
Vector3Wide.ConditionalSelect(outsideBC, bc, edgeDirection, out edgeDirection);
Vector3Wide.ConditionalSelect(outsideBC, b.B, b.A, out var edgeStart);
Vector3Wide.Add(localOffsetB, edgeStart, out var negativeEdgeStartToP);
//This does some partially redundant work if the edge is AB or AC, but given that we didn't have bcbc or bcpb, it's fine.
Vector3Wide.Dot(negativeEdgeStartToP, edgeDirection, out var negativeOffsetDotEdge);
Vector3Wide.Dot(edgeDirection, edgeDirection, out var edgeDotEdge);
var edgeScale = Vector.Max(Vector <float> .Zero, Vector.Min(Vector <float> .One, -negativeOffsetDotEdge / edgeDotEdge));
Vector3Wide.Scale(edgeDirection, edgeScale, out var pointOnEdge);
Vector3Wide.Add(edgeStart, pointOnEdge, out pointOnEdge);
Vector3Wide.ConditionalSelect(outsideAnyEdge, pointOnEdge, localClosestOnTriangle, out localClosestOnTriangle);
}
if (Vector.EqualsAny(outsideAnyEdge, Vector <int> .Zero))
{
//p + N * (pa * N) / ||N||^2 = N * (pa * N) / ||N||^2 - (-p)
var nScale = paN / triangleNormalLengthSquared;
Vector3Wide.Scale(localTriangleNormal, nScale, out var offsetToPlane);
Vector3Wide.Subtract(offsetToPlane, localOffsetB, out var pointOnFace);
Vector3Wide.ConditionalSelect(outsideAnyEdge, localClosestOnTriangle, pointOnFace, out localClosestOnTriangle);
}
//normal = normalize(localOffsetA - localClosestOnTriangle) = (localOffsetB + localClosestOnTriangle) / (-||localOffsetB + localClosestOnTriangle||)
Vector3Wide.Add(localOffsetB, localClosestOnTriangle, out var localNormal);
Vector3Wide.Length(localNormal, out var localNormalLength);
Vector3Wide.Scale(localNormal, new Vector <float>(-1f) / localNormalLength, out localNormal);
Matrix3x3Wide.TransformWithoutOverlap(localNormal, rB, out normal);
Vector3Wide.Scale(normal, -a.Radius, out closestA);
distance = localNormalLength - a.Radius;
intersected = Vector.LessThanOrEqual(distance, Vector <float> .Zero);
}
public static void ExpandBoundingBoxes(ref Vector3Wide min, ref Vector3Wide max, ref BodyVelocities velocities, float dt,
ref Vector <float> maximumRadius, ref Vector <float> maximumAngularExpansion, ref Vector <float> maximumExpansion)
{
/*
* If an object sitting on a plane had a raw (unexpanded) AABB that is just barely above the plane, no contacts would be generated.
* If the velocity of the object would shove it down into the plane in the next frame, then it would generate contacts in the next frame- and,
* potentially, this cycle would repeat and cause jitter.
*
* To solve this, there are a couple of options:
* 1) Introduce an 'allowed penetration' so that objects can overlap a little bit. This tends to confuse people a little bit when they notice it,
* and in some circumstances objects can be seen settling into the allowed penetration slowly. It looks a bit odd.
* 2) Make contact constraints fight to maintain zero penetration depth, but expand the bounding box with velocity and allow contacts to be generated speculatively-
* contacts with negative penetration depth.
*
#2 is a form of continuous collision detection, but it's handy for general contact stability too.
* In this version of the engine, all objects generate speculative contacts by default, though only within a per-collidable-tuned 'speculative margin'.
* It's kind of like BEPUphysics v1's AllowedPenetration, except inverted. Speculative contacts that fall within the speculative margin-
* that is, those with negative depth of a magnitude less than the margin- are kept.
*
* So, a user could choose to have a very large speculative margin, and the speculative contact generation would provide a form of continuous collision detection.
* The main purpose, though, is just contact stability. With this in isolation, there's no strong reason to expand the bounding box more than the speculative margin.
* This is the 'discrete' mode.
*
* However, consider what would happen if an object A with high velocity and this 'discrete' mode was headed towards an object B in a 'continuous' mode.
* Object B only expands its bounding box by its own velocity, and object A doesn't expand beyond its speculative margin. The collision between A and B could easily be missed.
* To account for this, there is an intermediate mode- 'passive'- where the bounding box is allowed to expand beyond the margin,
* but no further continuous collision detection is performed.
*
* The fully continuous modes fully expand the bounding boxes. Notably, the inner sphere continuous collision detection mode could get by with less,
* but it would be pretty confusing to have the same kind of missed collision possibility if the other object in the pair was a substepping object.
* Two different inner sphere modes could be offered, but I'm unsure about the usefulness versus the complexity.
*
* (Note that there ARE situations where a bounding box which contains the full unconstrained motion will fail to capture constrained motion.
* Consider object A flying at high speed to impact the stationary object B, which sits next to another stationary object C.
* Object B's bounding box doesn't overlap with object C's bounding box- they're both stationary, so there's no velocity expansion. But during one frame,
* object A slams into B, and object B's velocity during that frame now forces it to tunnel all the way through C unimpeded, because no contacts were generated between B and C.
* There are ways to address this- all of which are a bit expensive- but CCD as implemented is not a hard guarantee.
* It's a 'best effort' that compromises with performance. Later on, if it's really necessary, we could consider harder guarantees with higher costs, but...
* given that no one seemed to have much of an issue with v1's rather limited CCD, it'll probably be fine.)
*
* So, how is the velocity expansion calculated?
* There's two parts, linear and angular.
*
* Linear is pretty simple- expand the bounding box in the direction of linear displacement (linearVelocity * dt).
*/
Vector <float> vectorDt = new Vector <float>(dt);
Vector3Wide.Scale(ref velocities.LinearVelocity, ref vectorDt, out var linearDisplacement);
var zero = Vector <float> .Zero;
Vector3Wide.Min(ref zero, ref linearDisplacement, out var minDisplacement);
Vector3Wide.Max(ref zero, ref linearDisplacement, out var maxDisplacement);
/*
* Angular requires a bit more care. Since the goal is to create a tight bound, simply using a v = w * r approximation isn't ideal. A slightly tighter can be found:
* 1) The maximum displacement along ANY axis during an intermediate time is equal to the distance from a starting position at MaximumRadius
* to the position of that point at the intermediate time.
* 2) The expansion cannot exceed the maximum radius, so angular deltas greater than pi/3 do not need to be considered.
* (An expansion equal to the maximum radius would result in an equilateral triangle, which has an angle of 60 degrees in each corner.)
* Larger values can simply be clamped.
* 3) The largest displacement along any axis, at any time, is the distance from the starting position to the position at dt. Note that this only holds because of the clamp:
* if the angle was allowed to wrap around, it the distance would start to go down again.
* 4) position(time) = {radius * sin(angular speed * time), radius * cos(angular speed * time)}
* 5) largest expansion required = ||position(dt) - position(0)|| = sqrt(2 * radius^2 * (1 - cos(dt * w)))
* 6) Don't have any true SIMD sin function, but we can approximate it using a taylor series, like: cos(x) = 1 - x^2 / 2! + x^4 / 4! - x^6 / 6!
* 7) Note that the cosine approximation should stop at a degree where it is smaller than the true value of cosine for the interval 0 to pi/3: this guarantees that the distance,
* which is larger when the cosine is smaller, is conservative and fully bounds the angular motion.
*
* Why do this extra work?
* 1) The bounding box calculation phase, as a part of the pose integration phase, tends to be severely memory bound.
* Spending a little of ALU time to get a smaller bounding box isn't a big concern, even though it includes a couple of sqrts.
* An extra few dozen ALU cycles is unlikely to meaningfully change the execution time.
* 2) Shrinking the bounding box reduces the number of collision pairs. Collision pairs are expensive- many times more expensive than the cost of shrinking the bounding box.
*/
Vector3Wide.Length(ref velocities.AngularVelocity, out var angularVelocityMagnitude);
var a = Vector.Min(angularVelocityMagnitude * vectorDt, new Vector <float>(MathHelper.Pi / 3f));
var a2 = a * a;
var a4 = a2 * a2;
var a6 = a4 * a2;
var cosAngleMinusOne = a2 * new Vector <float>(-1f / 2f) + a4 * new Vector <float>(1f / 24f) - a6 * new Vector <float>(1f / 720f);
//Note that it's impossible for angular motion to cause an increase in bounding box size beyond (maximumRadius-minimumRadius) on any given axis.
//That value, or a conservative approximation, is stored as the maximum angular expansion.
var angularExpansion = Vector.Min(maximumAngularExpansion,
Vector.SquareRoot(new Vector <float>(-2f) * maximumRadius * maximumRadius * cosAngleMinusOne));
Vector3Wide.Subtract(ref minDisplacement, ref angularExpansion, out minDisplacement);
Vector3Wide.Add(ref maxDisplacement, ref angularExpansion, out maxDisplacement);
//The maximum expansion passed into this function is the speculative margin for discrete mode collidables, and ~infinity for passive or continuous ones.
var negativeMaximum = -maximumExpansion;
Vector3Wide.Max(ref negativeMaximum, ref minDisplacement, out minDisplacement);
Vector3Wide.Min(ref maximumExpansion, ref maxDisplacement, out maxDisplacement);
Vector3Wide.Add(ref min, ref minDisplacement, out min);
Vector3Wide.Add(ref max, ref maxDisplacement, out max);
//Note that this is an area that will need to adapt to changes to the body pose representation. If you use a 32 bit fixed point position or a 64 bit double position,
//the bounding box calculation needs to be aware. (In the more extreme cases, so does the broad phase. The amount of conservative padding needed for a 32 bit bounding box
//with 64 bit positions is silly.)
}
public void Test(ref 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 ConvexHullWide b, ref Vector <float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, out Convex4ContactManifoldWide manifold)
{
Matrix3x3Wide.CreateFromQuaternion(orientationA, out var boxOrientation);
Matrix3x3Wide.CreateFromQuaternion(orientationB, out var hullOrientation);
Matrix3x3Wide.MultiplyByTransposeWithoutOverlap(boxOrientation, hullOrientation, out var hullLocalBoxOrientation);
Matrix3x3Wide.TransformByTransposedWithoutOverlap(offsetB, hullOrientation, out var localOffsetB);
Vector3Wide.Negate(localOffsetB, out var localOffsetA);
Vector3Wide.Length(localOffsetA, out var centerDistance);
Vector3Wide.Scale(localOffsetA, Vector <float> .One / centerDistance, out var initialNormal);
var useInitialFallback = Vector.LessThan(centerDistance, new Vector <float>(1e-8f));
initialNormal.X = Vector.ConditionalSelect(useInitialFallback, Vector <float> .Zero, initialNormal.X);
initialNormal.Y = Vector.ConditionalSelect(useInitialFallback, Vector <float> .One, initialNormal.Y);
initialNormal.Z = Vector.ConditionalSelect(useInitialFallback, Vector <float> .Zero, initialNormal.Z);
var hullSupportFinder = default(ConvexHullSupportFinder);
var boxSupportFinder = default(BoxSupportFinder);
ManifoldCandidateHelper.CreateInactiveMask(pairCount, out var inactiveLanes);
b.EstimateEpsilonScale(inactiveLanes, out var hullEpsilonScale);
var epsilonScale = Vector.Min(Vector.Max(a.HalfWidth, Vector.Max(a.HalfHeight, a.HalfLength)), hullEpsilonScale);
var depthThreshold = -speculativeMargin;
DepthRefiner <ConvexHull, ConvexHullWide, ConvexHullSupportFinder, PhysicsBox, BoxWide, BoxSupportFinder> .FindMinimumDepth(
b, a, localOffsetA, hullLocalBoxOrientation, ref hullSupportFinder, ref boxSupportFinder, initialNormal, inactiveLanes, 1e-5f *epsilonScale, depthThreshold,
out var depth, out var localNormal, out var closestOnHull);
inactiveLanes = Vector.BitwiseOr(inactiveLanes, Vector.LessThan(depth, depthThreshold));
//Not every lane will generate contacts. Rather than requiring every lane to carefully clear all contactExists states, just clear them up front.
manifold = default;
manifold.Contact0Exists = default;
manifold.Contact1Exists = default;
manifold.Contact2Exists = default;
manifold.Contact3Exists = default;
if (Vector.LessThanAll(inactiveLanes, Vector <int> .Zero))
{
//No contacts generated.
return;
}
//Identify the box face.
Matrix3x3Wide.TransformByTransposedWithoutOverlap(localNormal, hullLocalBoxOrientation, out var localNormalInA);
Vector3Wide.Abs(localNormalInA, out var absLocalNormalInA);
var useX = Vector.BitwiseAnd(Vector.GreaterThan(absLocalNormalInA.X, absLocalNormalInA.Y), Vector.GreaterThan(absLocalNormalInA.X, absLocalNormalInA.Z));
var useY = Vector.AndNot(Vector.GreaterThan(absLocalNormalInA.Y, absLocalNormalInA.Z), useX);
Vector3Wide.ConditionalSelect(useX, hullLocalBoxOrientation.X, hullLocalBoxOrientation.Z, out var boxFaceNormal);
Vector3Wide.ConditionalSelect(useY, hullLocalBoxOrientation.Y, boxFaceNormal, out boxFaceNormal);
Vector3Wide.ConditionalSelect(useX, hullLocalBoxOrientation.Y, hullLocalBoxOrientation.X, out var boxFaceX);
Vector3Wide.ConditionalSelect(useY, hullLocalBoxOrientation.Z, boxFaceX, out boxFaceX);
Vector3Wide.ConditionalSelect(useX, hullLocalBoxOrientation.Z, hullLocalBoxOrientation.Y, out var boxFaceY);
Vector3Wide.ConditionalSelect(useY, hullLocalBoxOrientation.X, boxFaceY, out boxFaceY);
var negateFace =
Vector.ConditionalSelect(useX, Vector.GreaterThan(localNormalInA.X, Vector <float> .Zero),
Vector.ConditionalSelect(useY, Vector.GreaterThan(localNormalInA.Y, Vector <float> .Zero), Vector.GreaterThan(localNormalInA.Z, Vector <float> .Zero)));
Vector3Wide.ConditionallyNegate(negateFace, ref boxFaceNormal);
//Winding is important; flip the face bases if necessary.
Vector3Wide.ConditionallyNegate(Vector.OnesComplement(negateFace), ref boxFaceX);
var boxFaceHalfWidth = Vector.ConditionalSelect(useX, a.HalfHeight, Vector.ConditionalSelect(useY, a.HalfLength, a.HalfWidth));
var boxFaceHalfHeight = Vector.ConditionalSelect(useX, a.HalfLength, Vector.ConditionalSelect(useY, a.HalfWidth, a.HalfHeight));
var boxFaceNormalOffset = Vector.ConditionalSelect(useX, a.HalfWidth, Vector.ConditionalSelect(useY, a.HalfHeight, a.HalfLength));
Vector3Wide.Scale(boxFaceNormal, boxFaceNormalOffset, out var boxFaceCenterOffset);
Vector3Wide.Add(boxFaceCenterOffset, localOffsetA, out var boxFaceCenter);
Vector3Wide.Scale(boxFaceX, boxFaceHalfWidth, out var boxFaceXOffset);
Vector3Wide.Scale(boxFaceY, boxFaceHalfHeight, out var boxFaceYOffset);
Vector3Wide.Subtract(boxFaceCenter, boxFaceXOffset, out var v0);
Vector3Wide.Add(boxFaceCenter, boxFaceXOffset, out var v1);
Vector3Wide.Subtract(v0, boxFaceYOffset, out var v00);
Vector3Wide.Add(v0, boxFaceYOffset, out var v01);
Vector3Wide.Subtract(v1, boxFaceYOffset, out var v10);
Vector3Wide.Add(v1, boxFaceYOffset, out var v11);
//To find the contact manifold, we'll clip the box edges against the hull face as usual, but we're dealing with potentially
//distinct convex hulls. Rather than vectorizing over the different hulls, we vectorize within each hull.
Helpers.FillVectorWithLaneIndices(out var slotOffsetIndices);
var boundingPlaneEpsilon = 1e-3f * epsilonScale;
//There can be no more than 8 contacts (provided there are no numerical errors); 2 per box edge.
var candidates = stackalloc ManifoldCandidateScalar[8];
for (int slotIndex = 0; slotIndex < pairCount; ++slotIndex)
{
if (inactiveLanes[slotIndex] < 0)
{
continue;
}
ref var hull = ref b.Hulls[slotIndex];
ConvexHullTestHelper.PickRepresentativeFace(ref hull, slotIndex, ref localNormal, closestOnHull, slotOffsetIndices, ref boundingPlaneEpsilon, out var slotFaceNormal, out var slotLocalNormal, out var bestFaceIndex);
//Test each face edge plane against the box face.
//Note that we do not use the faceNormal x edgeOffset edge plane, but rather edgeOffset x localNormal.
//The faces are wound counterclockwise in right handed coordinates.
//X is 00->10; Y is 10->11; Z is 11->01; W is 01->00.
ref var v00Slot = ref GatherScatter.GetOffsetInstance(ref v00, slotIndex);
public void Test(
ref CapsuleWide a, ref BoxWide b, ref Vector <float> speculativeMargin,
ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount,
out Convex2ContactManifoldWide manifold)
{
Prepare(ref a, ref b, ref offsetB, ref orientationA, ref orientationB, out var localOffsetA, out var capsuleAxis, out var edgeCenters);
//Swizzle XYZ -> YZX
Vector3Wide localNormal;
TestAndRefineBoxEdge(ref localOffsetA.Y, ref localOffsetA.Z, ref localOffsetA.X,
ref capsuleAxis.Y, ref capsuleAxis.Z, ref capsuleAxis.X,
ref a.HalfLength,
ref edgeCenters.Y, ref edgeCenters.Z,
ref b.HalfHeight, ref b.HalfLength, ref b.HalfWidth,
out var ta, out var depth, out localNormal.Y, out localNormal.Z, out localNormal.X);
//Swizzle XYZ -> ZXY
TestAndRefineBoxEdge(ref localOffsetA.Z, ref localOffsetA.X, ref localOffsetA.Y,
ref capsuleAxis.Z, ref capsuleAxis.X, ref capsuleAxis.Y,
ref a.HalfLength,
ref edgeCenters.Z, ref edgeCenters.X,
ref b.HalfLength, ref b.HalfWidth, ref b.HalfHeight,
out var eyta, out var eyDepth, out var eynZ, out var eynX, out var eynY);
Select(ref depth, ref ta, ref localNormal.X, ref localNormal.Y, ref localNormal.Z,
ref eyDepth, ref eyta, ref eynX, ref eynY, ref eynZ);
//Swizzle XYZ -> XYZ
TestAndRefineBoxEdge(ref localOffsetA.X, ref localOffsetA.Y, ref localOffsetA.Z,
ref capsuleAxis.X, ref capsuleAxis.Y, ref capsuleAxis.Z,
ref a.HalfLength,
ref edgeCenters.X, ref edgeCenters.Y,
ref b.HalfWidth, ref b.HalfHeight, ref b.HalfLength,
out var ezta, out var ezDepth, out var eznX, out var eznY, out var eznZ);
Select(ref depth, ref ta, ref localNormal.X, ref localNormal.Y, ref localNormal.Z,
ref ezDepth, ref ezta, ref eznX, ref eznY, ref eznZ);
var zero = Vector <float> .Zero;
//Face X
TestBoxFace(ref localOffsetA.X,
ref capsuleAxis.X, ref a.HalfLength,
ref b.HalfWidth,
out var fxDepth, out var fxn);
Select(ref depth, ref localNormal.X, ref localNormal.Y, ref localNormal.Z,
ref fxDepth, ref fxn, ref zero, ref zero);
//Face Y
TestBoxFace(ref localOffsetA.Y,
ref capsuleAxis.Y, ref a.HalfLength,
ref b.HalfHeight,
out var fyDepth, out var fyn);
Select(ref depth, ref localNormal.X, ref localNormal.Y, ref localNormal.Z,
ref fyDepth, ref zero, ref fyn, ref zero);
//Face Z
TestBoxFace(ref localOffsetA.Z,
ref capsuleAxis.Z, ref a.HalfLength,
ref b.HalfLength,
out var fzDepth, out var fzn);
Select(ref depth, ref localNormal.X, ref localNormal.Y, ref localNormal.Z,
ref fzDepth, ref zero, ref zero, ref fzn);
//While the above chooses a minimal depth, edge-edge contact will frequently produce interval lengths of 0 and end up overwriting near-equivalent face intervals.
//One option would be to bias the comparison to accept face contacts more readily, but we can do something a little less fragile, and which also gives us a
//way to safely compute depth on a per contact basis:
//choose a representative box face based on the collision normal detected above, and compute the interval of intersection along the capsule axis of the box face projected onto the capsule axis.
//We'll compute this by unprojecting the capsule axis onto the box face plane along the local normal.
//Note that this interval always includes the closest point.
var xDot = localNormal.X * fxn;
var yDot = localNormal.Y * fyn;
var zDot = localNormal.Z * fzn;
var useX = Vector.GreaterThan(xDot, Vector.Max(yDot, zDot));
var useY = Vector.AndNot(Vector.GreaterThan(yDot, zDot), useX);
var useZ = Vector.AndNot(Vector.OnesComplement(useX), useY);
//Unproject the capsule center and capsule axis onto the representative face plane.
//unprojectedAxis = capsuleAxis - localNormal * dot(capsuleAxis, faceNormal) / dot(localNormal, faceNormal)
//unprojectedCenter = capsuleCenter - localNormal * dot(capsuleCenter - pointOnFace, faceNormal) / dot(localNormal, faceNormal)
var faceNormalDotLocalNormal = Vector.ConditionalSelect(useX, xDot, Vector.ConditionalSelect(useY, yDot, zDot));
var inverseFaceNormalDotLocalNormal = Vector <float> .One / Vector.Max(new Vector <float>(1e-15f), faceNormalDotLocalNormal);
var capsuleAxisDotFaceNormal = Vector.ConditionalSelect(useX, capsuleAxis.X * fxn, Vector.ConditionalSelect(useY, capsuleAxis.Y * fyn, capsuleAxis.Z * fzn));
var capsuleCenterDotFaceNormal = Vector.ConditionalSelect(useX, localOffsetA.X * fxn, Vector.ConditionalSelect(useY, localOffsetA.Y * fyn, localOffsetA.Z * fzn));
var facePlaneOffset = Vector.ConditionalSelect(useX, b.HalfWidth, Vector.ConditionalSelect(useY, b.HalfHeight, b.HalfLength));
var tAxis = capsuleAxisDotFaceNormal * inverseFaceNormalDotLocalNormal;
var tCenter = (capsuleCenterDotFaceNormal - facePlaneOffset) * inverseFaceNormalDotLocalNormal;
//Work in tangent space.
//Face X uses tangents Y and Z.
//Face Y uses tangents X and Z.
//Face Z uses tangents X and Y.
Vector3Wide.Scale(localNormal, tAxis, out var axisOffset);
Vector3Wide.Scale(localNormal, tCenter, out var centerOffset);
Vector3Wide.Subtract(capsuleAxis, axisOffset, out var unprojectedAxis);
Vector3Wide.Subtract(localOffsetA, centerOffset, out var unprojectedCenter);
Vector2Wide tangentSpaceCenter, tangentSpaceAxis;
tangentSpaceAxis.X = Vector.ConditionalSelect(useX, unprojectedAxis.Y, unprojectedAxis.X);
tangentSpaceAxis.Y = Vector.ConditionalSelect(useZ, unprojectedAxis.Y, unprojectedAxis.Z);
tangentSpaceCenter.X = Vector.ConditionalSelect(useX, unprojectedCenter.Y, unprojectedCenter.X);
tangentSpaceCenter.Y = Vector.ConditionalSelect(useZ, unprojectedCenter.Y, unprojectedCenter.Z);
//Slightly boost the size of the face to avoid minor numerical issues that could block coplanar contacts.
var epsilonScale = Vector.Min(Vector.Max(b.HalfWidth, Vector.Max(b.HalfHeight, b.HalfLength)), Vector.Max(a.HalfLength, a.Radius));
var epsilon = epsilonScale * 1e-3f;
var halfExtentX = epsilon + Vector.ConditionalSelect(useX, b.HalfHeight, b.HalfWidth);
var halfExtentY = epsilon + Vector.ConditionalSelect(useZ, b.HalfHeight, b.HalfLength);
//Compute interval bounded by edge normals pointing along tangentX.
//tX = -dot(tangentSpaceCenter +- halfExtentX, edgeNormal) / dot(unprojectedCapsuleAxis, edgeNormal)
var negativeOne = new Vector <float>(-1);
var inverseAxisX = negativeOne / tangentSpaceAxis.X;
var inverseAxisY = negativeOne / tangentSpaceAxis.Y;
var tX0 = (tangentSpaceCenter.X - halfExtentX) * inverseAxisX;
var tX1 = (tangentSpaceCenter.X + halfExtentX) * inverseAxisX;
var tY0 = (tangentSpaceCenter.Y - halfExtentY) * inverseAxisY;
var tY1 = (tangentSpaceCenter.Y + halfExtentY) * inverseAxisY;
var minX = Vector.Min(tX0, tX1);
var maxX = Vector.Max(tX0, tX1);
var minY = Vector.Min(tY0, tY1);
var maxY = Vector.Max(tY0, tY1);
//Protect against division by zero. If the unprojected capsule is within the slab, use an infinite interval. If it's outside and parallel, use an invalid interval.
var useFallbackX = Vector.LessThan(Vector.Abs(tangentSpaceAxis.X), new Vector <float>(1e-15f));
var useFallbackY = Vector.LessThan(Vector.Abs(tangentSpaceAxis.Y), new Vector <float>(1e-15f));
var centerContainedX = Vector.LessThanOrEqual(Vector.Abs(tangentSpaceCenter.X), halfExtentX);
var centerContainedY = Vector.LessThanOrEqual(Vector.Abs(tangentSpaceCenter.Y), halfExtentY);
var largeNegative = new Vector <float>(-float.MaxValue);
var largePositive = new Vector <float>(float.MaxValue);
minX = Vector.ConditionalSelect(useFallbackX, Vector.ConditionalSelect(centerContainedX, largeNegative, largePositive), minX);
maxX = Vector.ConditionalSelect(useFallbackX, Vector.ConditionalSelect(centerContainedX, largePositive, largeNegative), maxX);
minY = Vector.ConditionalSelect(useFallbackY, Vector.ConditionalSelect(centerContainedY, largeNegative, largePositive), minY);
maxY = Vector.ConditionalSelect(useFallbackY, Vector.ConditionalSelect(centerContainedY, largePositive, largeNegative), maxY);
var faceMin = Vector.Max(minX, minY);
var faceMax = Vector.Min(maxX, maxY);
//Clamp the resulting interval to the capsule axis.
var tMin = Vector.Max(Vector.Min(faceMin, a.HalfLength), -a.HalfLength);
var tMax = Vector.Max(Vector.Min(faceMax, a.HalfLength), -a.HalfLength);
var faceIntervalExists = Vector.GreaterThanOrEqual(faceMax, faceMin);
tMin = Vector.ConditionalSelect(faceIntervalExists, Vector.Min(tMin, ta), ta);
tMax = Vector.ConditionalSelect(faceIntervalExists, Vector.Max(tMax, ta), ta);
//Each contact may have its own depth.
//Imagine a face collision- if the capsule axis isn't fully parallel with the plane's surface, it would be strange to use the same depth for both contacts.
//We have two points on the capsule and box. We can reuse the unprojeection from earlier to compute the offset between them.
var separationMin = tCenter + tAxis * tMin;
var separationMax = tCenter + tAxis * tMax;
manifold.Depth0 = a.Radius - separationMin;
manifold.Depth1 = a.Radius - separationMax;
Vector3Wide.Scale(capsuleAxis, tMin, out var localA0);
Vector3Wide.Scale(capsuleAxis, tMax, out var localA1);
Vector3Wide.Add(localOffsetA, localA0, out var bToA0);
Vector3Wide.Add(localOffsetA, localA1, out var bToA1);
manifold.FeatureId0 = Vector <int> .Zero;
manifold.FeatureId1 = Vector <int> .One;
//Transform A0, A1, and the normal into world space.
Matrix3x3Wide.CreateFromQuaternion(orientationB, out var orientationMatrixB);
Matrix3x3Wide.TransformWithoutOverlap(localNormal, orientationMatrixB, out manifold.Normal);
Matrix3x3Wide.TransformWithoutOverlap(localA0, orientationMatrixB, out manifold.OffsetA0);
Matrix3x3Wide.TransformWithoutOverlap(localA1, orientationMatrixB, out manifold.OffsetA1);
//Apply the normal offset to the contact positions.
var negativeOffsetFromA0 = manifold.Depth0 * 0.5f - a.Radius;
var negativeOffsetFromA1 = manifold.Depth1 * 0.5f - a.Radius;
Vector3Wide.Scale(manifold.Normal, negativeOffsetFromA0, out var normalPush0);
Vector3Wide.Scale(manifold.Normal, negativeOffsetFromA1, out var normalPush1);
Vector3Wide.Add(manifold.OffsetA0, normalPush0, out manifold.OffsetA0);
Vector3Wide.Add(manifold.OffsetA1, normalPush1, out manifold.OffsetA1);
var minimumAcceptedDepth = -speculativeMargin;
manifold.Contact0Exists = Vector.GreaterThanOrEqual(manifold.Depth0, minimumAcceptedDepth);
manifold.Contact1Exists = Vector.BitwiseAnd(
Vector.GreaterThanOrEqual(manifold.Depth1, minimumAcceptedDepth),
Vector.GreaterThan(tMax - tMin, new Vector <float>(1e-7f) * a.HalfLength));
}
public 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 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 Test(
ref CapsuleWide a, ref BoxWide b,
ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB,
out Convex2ContactManifoldWide manifold)
{
QuaternionWide.Conjugate(ref orientationB, out var toLocalB);
QuaternionWide.TransformWithoutOverlap(ref offsetB, ref toLocalB, out var localOffsetA);
Vector3Wide.Negate(ref localOffsetA);
QuaternionWide.ConcatenateWithoutOverlap(ref orientationA, ref toLocalB, out var boxLocalOrientationA);
QuaternionWide.TransformUnitY(ref boxLocalOrientationA, out var capsuleAxis);
//Get the capsule-axis-perpendicular offset from the box to the capsule and use it to choose which edges to test.
//(Pointless to test the other 9; they're guaranteed to be further away.)
Vector3Wide.Dot(ref localOffsetA, ref capsuleAxis, out var axisOffsetADot);
Vector3Wide.Scale(ref capsuleAxis, ref axisOffsetADot, out var toRemove);
Vector3Wide.Subtract(ref localOffsetA, ref toRemove, out var perpendicularOffset);
Vector3Wide edgeCenters;
edgeCenters.X = Vector.ConditionalSelect(Vector.LessThan(perpendicularOffset.X, Vector <float> .Zero), -b.HalfWidth, b.HalfWidth);
edgeCenters.Y = Vector.ConditionalSelect(Vector.LessThan(perpendicularOffset.Y, Vector <float> .Zero), -b.HalfHeight, b.HalfHeight);
edgeCenters.Z = Vector.ConditionalSelect(Vector.LessThan(perpendicularOffset.Z, Vector <float> .Zero), -b.HalfLength, b.HalfLength);
//Swizzle XYZ -> YZX
Vector3Wide localNormal;
TestBoxEdge(ref localOffsetA.Y, ref localOffsetA.Z, ref localOffsetA.X,
ref capsuleAxis.Y, ref capsuleAxis.Z, ref capsuleAxis.X,
ref a.HalfLength,
ref edgeCenters.Y, ref edgeCenters.Z,
ref b.HalfHeight, ref b.HalfLength, ref b.HalfWidth,
out var taMin, out var taMax, out var depth, out localNormal.Y, out localNormal.Z, out localNormal.X);
//Swizzle XYZ -> ZXY
TestBoxEdge(ref localOffsetA.Z, ref localOffsetA.X, ref localOffsetA.Y,
ref capsuleAxis.Z, ref capsuleAxis.X, ref capsuleAxis.Y,
ref a.HalfLength,
ref edgeCenters.Z, ref edgeCenters.X,
ref b.HalfLength, ref b.HalfWidth, ref b.HalfHeight,
out var eytaMin, out var eytaMax, out var eyDepth, out var eynZ, out var eynX, out var eynY);
Select(ref depth, ref taMin, ref taMax, ref localNormal.X, ref localNormal.Y, ref localNormal.Z,
ref eyDepth, ref eytaMin, ref eytaMax, ref eynX, ref eynY, ref eynZ);
//Swizzle XYZ -> XYZ
TestBoxEdge(ref localOffsetA.X, ref localOffsetA.Y, ref localOffsetA.Z,
ref capsuleAxis.X, ref capsuleAxis.Y, ref capsuleAxis.Z,
ref a.HalfLength,
ref edgeCenters.X, ref edgeCenters.Y,
ref b.HalfWidth, ref b.HalfHeight, ref b.HalfLength,
out var eztaMin, out var eztaMax, out var ezDepth, out var eznX, out var eznY, out var eznZ);
Select(ref depth, ref taMin, ref taMax, ref localNormal.X, ref localNormal.Y, ref localNormal.Z,
ref ezDepth, ref eztaMin, ref eztaMax, ref eznX, ref eznY, ref eznZ);
//For each face, clip the capsule axis against the face bounds. The closest offset between the clipped capsule axis endpoints and the box is the normal candidate.
var positive = Vector <float> .One;
var negative = -positive;
var divisionGuard = new Vector <float>(1e-15f);
//Division by zero is hacked away by clamping the magnitude to some nonzero value and recovering the sign in the numerator.
//The resulting interval will be properly signed and enormous, so it serves the necessary purpose.
//Note the negation: t = dot(N, +-halfExtent - offsetA) / dot(N, capsuleAxis) = dot(N, offsetA +- halfExtent) / -dot(N, capsuleAxis)
var scaleX = Vector.ConditionalSelect(Vector.LessThan(capsuleAxis.X, Vector <float> .Zero), positive, negative) / Vector.Max(Vector.Abs(capsuleAxis.X), divisionGuard);
var scaleY = Vector.ConditionalSelect(Vector.LessThan(capsuleAxis.Y, Vector <float> .Zero), positive, negative) / Vector.Max(Vector.Abs(capsuleAxis.Y), divisionGuard);
var scaleZ = Vector.ConditionalSelect(Vector.LessThan(capsuleAxis.Z, Vector <float> .Zero), positive, negative) / Vector.Max(Vector.Abs(capsuleAxis.Z), divisionGuard);
Vector3Wide scaledExtents, scaledOffset;
//Clip slightly beyond the actual face limit to avoid pointlessly cutting the interval for near-edge-parallel capsules.
var epsilonScale = new Vector <float>(1f + 1e-6f);
scaledExtents.X = epsilonScale * b.HalfWidth * Vector.Abs(scaleX);
scaledExtents.Y = epsilonScale * b.HalfHeight * Vector.Abs(scaleY);
scaledExtents.Z = epsilonScale * b.HalfLength * Vector.Abs(scaleZ);
scaledOffset.X = localOffsetA.X * scaleX;
scaledOffset.Y = localOffsetA.Y * scaleY;
scaledOffset.Z = localOffsetA.Z * scaleZ;
Vector3Wide.Subtract(ref scaledOffset, ref scaledExtents, out var tCandidateMin);
var negativeHalfLength = -a.HalfLength;
Vector3Wide.Min(ref a.HalfLength, ref tCandidateMin, out tCandidateMin);
Vector3Wide.Max(ref negativeHalfLength, ref tCandidateMin, out tCandidateMin);
Vector3Wide.Add(ref scaledOffset, ref scaledExtents, out var tCandidateMax);
Vector3Wide.Min(ref a.HalfLength, ref tCandidateMax, out tCandidateMax);
Vector3Wide.Max(ref negativeHalfLength, ref tCandidateMax, out tCandidateMax);
var zero = Vector <float> .Zero;
//Face X
TestBoxFace(ref localOffsetA.X,
ref capsuleAxis.X, ref a.HalfLength,
ref tCandidateMin.Y, ref tCandidateMax.Y, ref tCandidateMin.Z, ref tCandidateMax.Z,
ref b.HalfWidth,
out var fxDepth, out var fxtaMin, out var fxtaMax, out var fxn);
Select(ref depth, ref taMin, ref taMax, ref localNormal.X, ref localNormal.Y, ref localNormal.Z,
ref fxDepth, ref fxtaMin, ref fxtaMax, ref fxn, ref zero, ref zero);
//Face Y
TestBoxFace(ref localOffsetA.Y,
ref capsuleAxis.Y, ref a.HalfLength,
ref tCandidateMin.X, ref tCandidateMax.X, ref tCandidateMin.Z, ref tCandidateMax.Z,
ref b.HalfHeight,
out var fyDepth, out var fytaMin, out var fytaMax, out var fyn);
Select(ref depth, ref taMin, ref taMax, ref localNormal.X, ref localNormal.Y, ref localNormal.Z,
ref fyDepth, ref fytaMin, ref fytaMax, ref zero, ref fyn, ref zero);
//Face Z
TestBoxFace(ref localOffsetA.Z,
ref capsuleAxis.Z, ref a.HalfLength,
ref tCandidateMin.X, ref tCandidateMax.X, ref tCandidateMin.Y, ref tCandidateMax.Y,
ref b.HalfLength,
out var fzDepth, out var fztaMin, out var fztaMax, out var fzn);
Select(ref depth, ref taMin, ref taMax, ref localNormal.X, ref localNormal.Y, ref localNormal.Z,
ref fzDepth, ref fztaMin, ref fztaMax, ref zero, ref zero, ref fzn);
//Each contact may have its own depth.
//Imagine a face collision- if the capsule axis isn't fully parallel with the plane's surface, it would be strange to use the same depth for both contacts.
//Compute the interval of the box on the normal. Note that the normal is already calibrated to point from B to A (box to capsule).
//(This is partially redundant with the per-case calculations, but simply redoing some cheap ALU work is easier than trying to keep track of per-contact depths across all cases.)
Vector3Wide.Scale(ref capsuleAxis, ref taMin, out var localA0);
Vector3Wide.Scale(ref capsuleAxis, ref taMax, out var localA1);
Vector3Wide.Add(ref localOffsetA, ref localA0, out var bToA0);
Vector3Wide.Add(ref localOffsetA, ref localA1, out var bToA1);
var boxExtreme = Vector.Abs(localNormal.X * b.HalfWidth) + Vector.Abs(localNormal.Y * b.HalfHeight) + Vector.Abs(localNormal.Z * b.HalfLength);
Vector3Wide.Dot(ref localNormal, ref bToA0, out var dot0);
Vector3Wide.Dot(ref localNormal, ref bToA1, out var dot1);
manifold.Depth0 = a.Radius + boxExtreme - dot0;
manifold.Depth1 = a.Radius + boxExtreme - dot1;
manifold.FeatureId0 = Vector <int> .Zero;
manifold.FeatureId1 = Vector <int> .One;
//Transform A0, A1, and the normal into world space.
Matrix3x3Wide.CreateFromQuaternion(ref orientationB, out var orientationMatrixB);
Matrix3x3Wide.TransformWithoutOverlap(ref localNormal, ref orientationMatrixB, out manifold.Normal);
Matrix3x3Wide.TransformWithoutOverlap(ref localA0, ref orientationMatrixB, out manifold.OffsetA0);
Matrix3x3Wide.TransformWithoutOverlap(ref localA1, ref orientationMatrixB, out manifold.OffsetA1);
//Apply the normal offset to the contact positions.
var negativeOffsetFromA0 = manifold.Depth0 * 0.5f - a.Radius;
var negativeOffsetFromA1 = manifold.Depth1 * 0.5f - a.Radius;
Vector3Wide.Scale(ref manifold.Normal, ref negativeOffsetFromA0, out var normalPush0);
Vector3Wide.Scale(ref manifold.Normal, ref negativeOffsetFromA1, out var normalPush1);
Vector3Wide.Add(ref manifold.OffsetA0, ref normalPush0, out manifold.OffsetA0);
Vector3Wide.Add(ref manifold.OffsetA1, ref normalPush1, out manifold.OffsetA1);
manifold.Count = Vector.ConditionalSelect(Vector.LessThan(taMax - taMin, new Vector <float>(1e-7f) * a.HalfLength), Vector <int> .One, new Vector <int>(2));
}
public static unsafe void Test()
{
const int iterations = 10000000;
{
var vectors = stackalloc Vector3[Vector <float> .Count];
//var vectors = new Vector3[Vector<float>.Count];
for (int j = 0; j < Vector <float> .Count; ++j)
{
vectors[j] = new Vector3();
}
float[] x, y, z;
x = new float[Vector <float> .Count];
y = new float[Vector <float> .Count];
z = new float[Vector <float> .Count];
Vector3Wide acc = new Vector3Wide(x, y, z);
Vector3Wide v1 = new Vector3Wide(x, y, z);
Vector3Wide.Add(ref v1, ref acc, out acc);
var startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
for (int i = 0; i < iterations; ++i)
{
for (int j = 0; j < Vector <float> .Count; ++j)
{
x[j] = vectors[j].X;
y[j] = vectors[j].Y;
z[j] = vectors[j].Z;
}
v1 = new Vector3Wide(x, y, z);
Vector3Wide.Add(ref v1, ref acc, out acc);
}
var endTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
Console.WriteLine($"* Time: {endTime - startTime}, acc: {acc}, size: {Vector<float>.Count}");
}
{
Vector3 v = new Vector3(0, 1, 2);
Vector3Wide acc = new Vector3Wide(ref v);
Vector3Wide v1 = new Vector3Wide(ref v);
Vector3Wide.Add(ref v1, ref acc, out acc);
var startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
for (int i = 0; i < iterations; ++i)
{
v1 = new Vector3Wide(ref v);
Vector3Wide.Add(ref v1, ref acc, out acc);
}
var endTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
Console.WriteLine($"* Single Time: {endTime - startTime}, acc: {acc}, size: {Vector<float>.Count}");
}
{
Vector3 a, b, c, d;
a = b = c = d = new Vector3();
Vector3Width4 acc = new Vector3Width4();
Vector3Width4 v1 = new Vector3Width4(ref a, ref b, ref c, ref d);
Vector3Width4.Add(ref v1, ref acc, out acc);
var startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
for (int i = 0; i < iterations; ++i)
{
v1 = new Vector3Width4(ref a, ref b, ref c, ref d);
Vector3Width4.Add(ref v1, ref acc, out acc);
}
var endTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
Console.WriteLine($"Fixed Time: {endTime - startTime}, acc: {acc}");
}
}
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 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);
