public unsafe BoundingBoxWide(BoundingBox* boundingBoxes, Vector<int>[] masks)
{
Min = new Vector3Wide(ref boundingBoxes[0].Min);
Max = new Vector3Wide(ref boundingBoxes[0].Max);
for (int i = 1; i < Vector<float>.Count; ++i)
{
BoundingBoxWide wide = new BoundingBoxWide(ref boundingBoxes[i]);
ConditionalSelect(ref masks[i], ref wide, ref this, out this);
}
}
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 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);
}
public static void Prestep(ref Vector3Wide tangentX, ref Vector3Wide tangentY, ref Vector3Wide offsetA, ref BodyInertias inertiaA,
out Projection projection)
{
ComputeJacobians(ref tangentX, ref tangentY, ref offsetA, out var jacobians);
//Compute effective mass matrix contributions.
Symmetric2x2Wide.SandwichScale(jacobians.LinearA, inertiaA.InverseMass, out var linearContributionA);
Symmetric3x3Wide.MatrixSandwich(jacobians.AngularA, inertiaA.InverseInertiaTensor, out var angularContributionA);
//No softening; this constraint is rigid by design. (It does support a maximum force, but that is distinct from a proper damping ratio/natural frequency.)
Symmetric2x2Wide.Add(linearContributionA, angularContributionA, out var inverseEffectiveMass);
Symmetric2x2Wide.InvertWithoutOverlap(inverseEffectiveMass, out projection.EffectiveMass);
projection.OffsetA = offsetA;
//Note that friction constraints have no bias velocity. They target zero velocity.
}
public void Test(ref SphereWide a, ref TriangleWide b, ref Vector <float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount,
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.
Vector3Wide.CrossWithoutOverlap(pa, ab, out var paxab);
Vector3Wide.CrossWithoutOverlap(ac, pa, out var acxpa);
Vector3Wide.Dot(paxab, localTriangleNormal, out var edgePlaneTestAB);
Vector3Wide.Dot(acxpa, localTriangleNormal, out var edgePlaneTestAC);
Vector3Wide.Dot(localTriangleNormal, localTriangleNormal, out var triangleNormalLengthSquared);
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 void Test(ref SphereWide a, ref TriangleWide b, ref Vector <float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB,
int pairCount, out Convex1ContactManifoldWide manifold)
{
throw new NotImplementedException();
}
public unsafe override void Initialize(ContentArchive content, Camera camera)
{
{
SphereTriangleTester tester;
SphereWide sphere = default;
sphere.Broadcast(new Sphere(0.5f));
TriangleWide triangle = default;
var a = new Vector3(0, 0, 0);
var b = new Vector3(1, 0, 0);
var c = new Vector3(0, 0, 1);
//var center = (a + b + c) / 3f;
//a -= center;
//b -= center;
//c -= center;
triangle.Broadcast(new Triangle(a, b, c));
var margin = new Vector <float>(1f);
Vector3Wide.Broadcast(new Vector3(1, -1, 0), out var offsetB);
QuaternionWide.Broadcast(BepuUtilities.QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2), out var orientationB);
tester.Test(ref sphere, ref triangle, ref margin, ref offsetB, ref orientationB, Vector <float> .Count, out var manifold);
}
{
CapsuleTriangleTester tester;
CapsuleWide capsule = default;
capsule.Broadcast(new Capsule(0.5f, 0.5f));
TriangleWide triangle = default;
var a = new Vector3(0, 0, 0);
var b = new Vector3(1, 0, 0);
var c = new Vector3(0, 0, 1);
//var center = (a + b + c) / 3f;
//a -= center;
//b -= center;
//c -= center;
triangle.Broadcast(new Triangle(a, b, c));
var margin = new Vector <float>(2f);
Vector3Wide.Broadcast(new Vector3(-1f, -0.5f, -1f), out var offsetB);
QuaternionWide.Broadcast(BepuUtilities.QuaternionEx.CreateFromAxisAngle(Vector3.Normalize(new Vector3(-1, 0, 1)), MathHelper.PiOver2), out var orientationA);
QuaternionWide.Broadcast(BepuUtilities.QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), 0), out var orientationB);
tester.Test(ref capsule, ref triangle, ref margin, ref offsetB, ref orientationA, ref orientationB, Vector <float> .Count, out var manifold);
}
{
BoxTriangleTester tester;
BoxWide shape = default;
shape.Broadcast(new Box(1f, 1f, 1f));
TriangleWide triangle = default;
var a = new Vector3(0, 0, 0);
var b = new Vector3(1, 0, 0);
var c = new Vector3(0, 0, 1);
//var center = (a + b + c) / 3f;
//a -= center;
//b -= center;
//c -= center;
triangle.Broadcast(new Triangle(a, b, c));
var margin = new Vector <float>(2f);
Vector3Wide.Broadcast(new Vector3(-1f, -0.5f, -1f), out var offsetB);
QuaternionWide.Broadcast(BepuUtilities.QuaternionEx.CreateFromAxisAngle(Vector3.Normalize(new Vector3(-1, 0, 1)), MathHelper.PiOver2), out var orientationA);
QuaternionWide.Broadcast(BepuUtilities.QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), 0), out var orientationB);
tester.Test(ref shape, ref triangle, ref margin, ref offsetB, ref orientationA, ref orientationB, Vector <float> .Count, out var manifold);
}
{
TrianglePairTester tester;
TriangleWide a = default, b = default;
public static void Solve(ref Projection projection, ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref Vector3Wide normal,
ref Vector <float> accumulatedImpulse0,
ref Vector <float> accumulatedImpulse1,
ref Vector <float> accumulatedImpulse2,
ref Vector <float> accumulatedImpulse3, ref BodyVelocities wsvA, ref BodyVelocities wsvB)
{
ComputeCorrectiveImpulse(ref wsvA, ref wsvB, ref projection.Penetration0, ref normal, ref projection.SoftnessImpulseScale, ref accumulatedImpulse0, out var correctiveCSI0);
ApplyImpulse(ref projection.Penetration0, ref inertiaA, ref inertiaB, ref normal, ref correctiveCSI0, ref wsvA, ref wsvB);
ComputeCorrectiveImpulse(ref wsvA, ref wsvB, ref projection.Penetration1, ref normal, ref projection.SoftnessImpulseScale, ref accumulatedImpulse1, out var correctiveCSI1);
ApplyImpulse(ref projection.Penetration1, ref inertiaA, ref inertiaB, ref normal, ref correctiveCSI1, ref wsvA, ref wsvB);
ComputeCorrectiveImpulse(ref wsvA, ref wsvB, ref projection.Penetration2, ref normal, ref projection.SoftnessImpulseScale, ref accumulatedImpulse2, out var correctiveCSI2);
ApplyImpulse(ref projection.Penetration2, ref inertiaA, ref inertiaB, ref normal, ref correctiveCSI2, ref wsvA, ref wsvB);
ComputeCorrectiveImpulse(ref wsvA, ref wsvB, ref projection.Penetration3, ref normal, ref projection.SoftnessImpulseScale, ref accumulatedImpulse3, out var correctiveCSI3);
ApplyImpulse(ref projection.Penetration3, ref inertiaA, ref inertiaB, ref normal, ref correctiveCSI3, ref wsvA, ref wsvB);
}
public void GetPoint(int pointIndex, out Vector3 point)
{
BundleIndexing.GetBundleIndices(pointIndex, out var bundleIndex, out var innerIndex);
Vector3Wide.ReadSlot(ref Points[bundleIndex], innerIndex, out point);
}
public void GetPoint(HullVertexIndex pointIndex, out Vector3 point)
{
Vector3Wide.ReadSlot(ref Points[pointIndex.BundleIndex], pointIndex.InnerIndex, out point);
}
public void Test(ref CapsuleWide a, ref ConvexHullWide b, ref Vector <float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, out Convex2ContactManifoldWide manifold)
{
Matrix3x3Wide.CreateFromQuaternion(orientationA, out var capsuleOrientation);
Matrix3x3Wide.CreateFromQuaternion(orientationB, out var hullOrientation);
Matrix3x3Wide.MultiplyByTransposeWithoutOverlap(capsuleOrientation, hullOrientation, out var hullLocalCapsuleOrientation);
ref var localCapsuleAxis = ref hullLocalCapsuleOrientation.Y;
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 box edge 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 the 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 BoundingBoxWide(ref BoundingBox boundingBox)
{
Min = new Vector3Wide(ref boundingBox.Min);
Max = new Vector3Wide(ref boundingBox.Max);
}
public static void Solve(ref Vector3Wide angularJacobianA, ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref TwistFrictionProjection projection,
ref Vector <float> maximumImpulse, ref Vector <float> accumulatedImpulse, ref BodyVelocities wsvA, ref BodyVelocities wsvB)
{
ComputeCorrectiveImpulse(ref angularJacobianA, ref projection, ref wsvA, ref maximumImpulse, ref accumulatedImpulse, out var correctiveCSI);
ApplyImpulse(ref angularJacobianA, ref inertiaA, ref correctiveCSI, ref wsvA);
}
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);
}
public static void ComputeJacobians(ref Vector3Wide tangentX, ref Vector3Wide tangentY, ref Vector3Wide offsetA, ref Vector3Wide offsetB,
out Jacobians jacobians)
{
//Two velocity constraints:
//dot(velocity(p, A), tangentX) = dot(velocity(p, B), tangentX)
//dot(velocity(p, A), tangentY) = dot(velocity(p, B), tangentY)
//where velocity(p, A) is the velocity of a point p attached to object A.
//velocity(p, A) = linearVelocityA + angularVelocityA x (p - positionA) = linearVelocityA + angularVelocityA x offsetA
//so:
//dot(velocity(p, A), tangentX) = dot(linearVelocityA, tangentX) + dot(angularVelocityA x offsetA, tangentX)
//dot(velocity(p, A), tangentX) = dot(linearVelocityA, tangentX) + dot(offsetA x tangentX, angularVelocityA)
//Restating the two constraints:
//dot(linearVelocityA, tangentX) + dot(offsetA x tangentX, angularVelocityA) = dot(linearVelocityB, tangentX) + dot(offsetB x tangentX, angularVelocityB)
//dot(linearVelocityA, tangentY) + dot(offsetA x tangentY, angularVelocityA) = dot(linearVelocityB, tangentY) + dot(offsetB x tangentY, angularVelocityB)
//dot(linearVelocityA, tangentX) + dot(offsetA x tangentX, angularVelocityA) - dot(linearVelocityB, tangentX) - dot(offsetB x tangentX, angularVelocityB) = 0
//dot(linearVelocityA, tangentY) + dot(offsetA x tangentY, angularVelocityA) - dot(linearVelocityB, tangentY) - dot(offsetB x tangentY, angularVelocityB) = 0
//Since there are two constraints (2DOFs), there are two rows in the jacobian, which based on the above is:
//jLinearA = [ tangentX ]
// [ tangentY ]
//jAngularA = [ offsetA x tangentX ]
// [ offsetA x tangentY ]
//jLinearB = [ -tangentX ]
// [ -tangentY ]
//jAngularB = [ -offsetB x tangentX ] = [ tangentX x offsetB ]
// [ -offsetB x tangentY ] [ tangentY x offsetB ]
//TODO: there would be a minor benefit in eliminating this copy manually, since it's very likely that the compiler won't. And it's probably also introducing more locals init.
jacobians.LinearA.X = tangentX;
jacobians.LinearA.Y = tangentY;
Vector3Wide.CrossWithoutOverlap(ref offsetA, ref tangentX, out jacobians.AngularA.X);
Vector3Wide.CrossWithoutOverlap(ref offsetA, ref tangentY, out jacobians.AngularA.Y);
Vector3Wide.CrossWithoutOverlap(ref tangentX, ref offsetB, out jacobians.AngularB.X);
Vector3Wide.CrossWithoutOverlap(ref tangentY, ref offsetB, out jacobians.AngularB.Y);
}
public static void Prestep(ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref TwoBody1DOFJacobians jacobians, ref SpringSettingsWide springSettings, ref Vector <float> maximumRecoveryVelocity,
ref Vector <float> positionError, float dt, float inverseDt, out Projection2Body1DOF projection)
{
//unsoftened effective mass = (J * M^-1 * JT)^-1
//where J is a constraintDOF x bodyCount*6 sized matrix, JT is its transpose, and for two bodies M^-1 is:
//[inverseMassA, 0, 0, 0]
//[0, inverseInertiaA, 0, 0]
//[0, 0, inverseMassB, 0]
//[0, 0, 0, inverseInertiaB]
//The entries of J match up to this convention, containing the linear and angular components of each body in sequence, so for a 2 body 1DOF constraint J would look like:
//[linearA 1x3, angularA 1x3, linearB 1x3, angularB 1x3]
//Note that it is a row vector by convention. When transforming velocities from world space into constraint space, it is assumed that the velocity vector is organized as a
//row vector matching up to the jacobian (that is, [linearA 1x3, angularA 1x3, linearB 1x3, angularB 1x3]), so for a 2 body 2 DOF constraint,
//worldVelocity * JT would be a [worldVelocity: 1x12] * [JT: 12x2], resulting in a 1x2 constraint space velocity row vector.
//Similarly, when going from constraint space impulse to world space impulse in the above example, we would do [csi: 1x2] * [J: 2x12] to get a 1x12 world impulse row vector.
//Note that the engine uses row vectors for all velocities and positions and so on. Rotation and inertia tensors are constructed for premultiplication.
//In other words, unlike many of the presentations in the space, we use v * JT and csi * J instead of J * v and JT * csi.
//There is no meaningful difference- the two conventions are just transpositions of each other.
//(If you want to know how this stuff works, go read the constraint related presentations: http://box2d.org/downloads/
//Be mindful of the difference in conventions. You'll see J * v instead of v * JT, for example. Everything is still fundamentally the same, though.)
//Due to the block structure of the mass matrix, we can handle each component separately and then sum the results.
//For this 1DOF constraint, the result is a simple scalar.
//Note that we store the intermediate results of J * M^-1 for use when projecting from constraint space impulses to world velocity changes.
//If we didn't store those intermediate values, we could just scale the dot product of jacobians.LinearA with itself to save 4 multiplies.
Vector3Wide.Scale(jacobians.LinearA, inertiaA.InverseMass, out projection.CSIToWSVLinearA);
Vector3Wide.Scale(jacobians.LinearB, inertiaB.InverseMass, out projection.CSIToWSVLinearB);
Vector3Wide.Dot(projection.CSIToWSVLinearA, jacobians.LinearA, out var linearA);
Vector3Wide.Dot(projection.CSIToWSVLinearB, jacobians.LinearB, out var linearB);
//The angular components are a little more involved; (J * I^-1) * JT is explicitly computed.
Symmetric3x3Wide.TransformWithoutOverlap(jacobians.AngularA, inertiaA.InverseInertiaTensor, out projection.CSIToWSVAngularA);
Symmetric3x3Wide.TransformWithoutOverlap(jacobians.AngularB, inertiaB.InverseInertiaTensor, out projection.CSIToWSVAngularB);
Vector3Wide.Dot(projection.CSIToWSVAngularA, jacobians.AngularA, out var angularA);
Vector3Wide.Dot(projection.CSIToWSVAngularB, jacobians.AngularB, out var angularB);
//Now for a digression!
//Softness is applied along the diagonal (which, for a 1DOF constraint, is just the only element).
//Check the the ODE reference for a bit more information: http://ode.org/ode-latest-userguide.html#sec_3_8_0
//And also see Erin Catto's Soft Constraints presentation for more details: http://box2d.org/files/GDC2011/GDC2011_Catto_Erin_Soft_Constraints.pdf)
//There are some very interesting tricks you can use here, though.
//Our core tuning variables are the damping ratio and natural frequency.
//Our runtime used variables are softness and an error reduction feedback scale..
//(For the following, I'll use the ODE terms CFM and ERP, constraint force mixing and error reduction parameter.)
//So first, we need to get from damping ratio and natural frequency to stiffness and damping spring constants.
//From there, we'll go to CFM/ERP.
//Then, we'll create an expression for a softened effective mass matrix (i.e. one that takes into account the CFM term),
//and an expression for the contraint force mixing term in the solve iteration.
//Finally, compute ERP.
//(And then some tricks.)
//1) Convert from damping ratio and natural frequency to stiffness and damping constants.
//The raw expressions are:
//stiffness = effectiveMass * naturalFrequency^2
//damping = effectiveMass * 2 * dampingRatio * naturalFrequency
//Rather than using any single object as the reference for the 'mass' term involved in this conversion, use the effective mass of the constraint.
//In other words, we're dynamically picking the spring constants necessary to achieve the desired behavior for the current constraint configuration.
//(See Erin Catto's presentation above for more details on this.)
//(Note that this is different from BEPUphysics v1. There, users configured stiffness and damping constants. That worked okay, but people often got confused about
//why constraints didn't behave the same when they changed masses. Usually it manifested as someone creating an incredibly high mass object relative to the default
//stiffness/damping, and they'd post on the forum wondering why constraints were so soft. Basically, the defaults were another sneaky tuning factor to get wrong.
//Since damping ratio and natural frequency define the behavior independent of the mass, this problem goes away- and it makes some other interesting things happen...)
//2) Convert from stiffness and damping constants to CFM and ERP.
//CFM = (stiffness * dt + damping)^-1
//ERP = (stiffness * dt) * (stiffness * dt + damping)^-1
//Or, to rephrase:
//ERP = (stiffness * dt) * CFM
//3) Use CFM and ERP to create a softened effective mass matrix and a force mixing term for the solve iterations.
//Start with a base definition which we won't be deriving, the velocity constraint itself (stated as an equality constraint here):
//This means 'world space velocity projected into constraint space should equal the velocity bias term combined with the constraint force mixing term'.
//(The velocity bias term will be computed later- it's the position error scaled by the error reduction parameter, ERP. Position error is used to create a velocity motor goal.)
//We're pulling back from the implementation of sequential impulses here, so rather than using the term 'accumulated impulse', we'll use 'lambda'
//(which happens to be consistent with the ODE documentation covering the same topic). Lambda is impulse that satisfies the constraint.
//wsv * JT = bias - lambda * CFM/dt
//This can be phrased as:
//currentVelocity = targetVelocity
//Or:
//goalVelocityChange = targetVelocity - currentVelocity
//lambda = goalVelocityChange * effectiveMass
//lambda = (targetVelocity - currentVelocity) * effectiveMass
//lambda = (bias - lambda * CFM/dt - currentVelocity) * effectiveMass
//Solving for lambda:
//lambda = (bias - currentVelocity) * effectiveMass - lambda * CFM/dt * effectiveMass
//lambda + lambda * CFM/dt * effectiveMass = (bias - currentVelocity) * effectiveMass
//(lambda + lambda * CFM/dt * effectiveMass) * effectiveMass^-1 = bias - currentVelocity
//lambda * effectiveMass^-1 + lambda * CFM/dt = bias - currentVelocity
//lambda * (effectiveMass^-1 + CFM/dt) = bias - currentVelocity
//lambda = (bias - currentVelocity) * (effectiveMass^-1 + CFM/dt)^-1
//lambda = (bias - wsv * JT) * (effectiveMass^-1 + CFM/dt)^-1
//In other words, we transform the velocity change (bias - wsv * JT) into the constraint-satisfying impulse, lambda, using a matrix (effectiveMass^-1 + CFM/dt)^-1.
//That matrix is the softened effective mass:
//softenedEffectiveMass = (effectiveMass^-1 + CFM/dt)^-1
//Here's where some trickiness occurs. (Be mindful of the distinction between the softened and unsoftened effective mass).
//Start by substituting CFM into the softened effective mass definition:
//CFM/dt = (stiffness * dt + damping)^-1 / dt = (dt * (stiffness * dt + damping))^-1 = (stiffness * dt^2 + damping*dt)^-1
//softenedEffectiveMass = (effectiveMass^-1 + (stiffness * dt^2 + damping * dt)^-1)^-1
//Now substitute the definitions of stiffness and damping, treating the scalar components as uniform scaling matrices of dimension equal to effectiveMass:
//softenedEffectiveMass = (effectiveMass^-1 + ((effectiveMass * naturalFrequency^2) * dt^2 + (effectiveMass * 2 * dampingRatio * naturalFrequency) * dt)^-1)^-1
//Combine the inner effectiveMass coefficients, given matrix multiplication distributes over addition:
//softenedEffectiveMass = (effectiveMass^-1 + (effectiveMass * (naturalFrequency^2 * dt^2) + effectiveMass * (2 * dampingRatio * naturalFrequency * dt))^-1)^-1
//softenedEffectiveMass = (effectiveMass^-1 + (effectiveMass * (naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt))^-1)^-1
//Apply the inner matrix inverse:
//softenedEffectiveMass = (effectiveMass^-1 + (naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1 * effectiveMass^-1)^-1
//Once again, combine coefficients of the inner effectiveMass^-1 terms:
//softenedEffectiveMass = ((1 + (naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1) * effectiveMass^-1)^-1
//Apply the inverse again:
//softenedEffectiveMass = effectiveMass * (1 + (naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1)^-1
//So, to put it another way- because CFM is based on the effective mass, applying it to the effective mass results in a simple downscale.
//What has been gained? Consider what happens in the solve iteration.
//We take the velocity error:
//velocityError = bias - accumulatedImpulse * CFM/dt - wsv * JT
//and convert it to a corrective impulse with the effective mass:
//impulse = (bias - accumulatedImpulse * CFM/dt - wsv * JT) * softenedEffectiveMass
//The effective mass distributes over the set:
//impulse = bias * softenedEffectiveMass - accumulatedImpulse * CFM/dt * softenedEffectiveMass - wsv * JT * softenedEffectiveMass
//Focus on the CFM term:
//-accumulatedImpulse * CFM/dt * softenedEffectiveMass
//What is CFM/dt * softenedEffectiveMass? Substitute.
//(stiffness * dt^2 + damping * dt)^-1 * softenedEffectiveMass
//((effectiveMass * naturalFrequency^2) * dt^2 + (effectiveMass * 2 * dampingRatio * naturalFrequency * dt))^-1 * softenedEffectiveMass
//Combine terms:
//(effectiveMass * (naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt))^-1 * softenedEffectiveMass
//Apply inverse:
//(naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1 * effectiveMass^-1 * softenedEffectiveMass
//Expand softened effective mass from earlier:
//(naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1 * effectiveMass^-1 * effectiveMass * (1 + (naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1)^-1
//Cancel effective masses: (!)
//(naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1 * (1 + (naturalFrequency^2 * dt^2 + 2 * dampingRatio * naturalFrequency * dt)^-1)^-1
//Because CFM was created from effectiveMass, the CFM/dt * effectiveMass term is actually independent of the effectiveMass!
//The remaining expression is still a matrix, but fortunately it is a simple uniform scaling matrix that we can store and apply as a single scalar.
//4) How do you compute ERP?
//ERP = (stiffness * dt) * CFM
//ERP = (stiffness * dt) * (stiffness * dt + damping)^-1
//ERP = ((effectiveMass * naturalFrequency^2) * dt) * ((effectiveMass * naturalFrequency^2) * dt + (effectiveMass * 2 * dampingRatio * naturalFrequency))^-1
//Combine denominator terms:
//ERP = ((effectiveMass * naturalFrequency^2) * dt) * ((effectiveMass * (naturalFrequency^2 * dt + 2 * dampingRatio * naturalFrequency))^-1
//Apply denominator inverse:
//ERP = ((effectiveMass * naturalFrequency^2) * dt) * (naturalFrequency^2 * dt + 2 * dampingRatio * naturalFrequency)^-1 * effectiveMass^-1
//Uniform scaling matrices commute:
//ERP = (naturalFrequency^2 * dt) * effectiveMass * effectiveMass^-1 * (naturalFrequency^2 * dt + 2 * dampingRatio * naturalFrequency)^-1
//Cancellation!
//ERP = (naturalFrequency^2 * dt) * (naturalFrequency^2 * dt + 2 * dampingRatio * naturalFrequency)^-1
//ERP = (naturalFrequency * dt) * (naturalFrequency * dt + 2 * dampingRatio)^-1
//ERP is a simple scalar, independent of mass.
//5) So we can compute CFM, ERP, the softened effective mass matrix, and we have an interesting shortcut on the constraint force mixing term of the solve iterations.
//Is there anything more that can be done? You bet!
//Let's look at the post-distribution impulse computation again:
//impulse = bias * effectiveMass - accumulatedImpulse * CFM/dt * effectiveMass - wsv * JT * effectiveMass
//During the solve iterations, the only quantities that vary are the accumulated impulse and world space velocities. So the rest can be precomputed.
//bias * effectiveMass,
//CFM/dt * effectiveMass,
//JT * effectiveMass
//In other words, we bypass the intermediate velocity state and go directly from source velocities to an impulse.
//Note the sizes of the precomputed types above:
//bias * effective mass is the same size as bias (vector with dimension equal to constrained DOFs)
//CFM/dt * effectiveMass is a single scalar regardless of constrained DOFs,
//JT * effectiveMass is the same size as JT
//But note that we no longer need to load the effective mass! It is implicit.
//The resulting computation is:
//impulse = a - accumulatedImpulse * b - wsv * c
//two DOF-width adds (add/subtract), one DOF-width multiply, and a 1xDOF * DOFx12 jacobian-sized transform.
//Compare to;
//(bias - accumulatedImpulse * CFM/dt - wsv * JT) * effectiveMass
//two DOF-width adds (add/subtract), one DOF width multiply, a 1xDOF * DOFx12 jacobian-sized transform, and a 1xDOF * DOFxDOF transform.
//In other words, we shave off a whole 1xDOF * DOFxDOF transform per iteration.
//So, taken in isolation, this is a strict win both in terms of memory and the amount of computation.
//Unfortunately, it's not quite so simple- jacobians are ALSO used to transform the impulse into world space so that it can be used to change the body velocities.
//We still need to have those around. So while we no longer store the effective mass, our jacobian has sort of been duplicated.
//But wait, there's more!
//That process looks like:
//wsv += impulse * J * M^-1
//So while we need to store something here, we can take advantage of the fact that we aren't using the jacobian anywhere else (it's replaced by the JT * effectiveMass term above).
//Precompute J*M^-1, too.
//So you're still loading a jacobian-sized matrix, but you don't need to load M^-1! That saves you 20 scalars. (3x3 + 1 + 3x3 + 1, inverse inertia and mass of two bodies.)
//That saves you the multiplication of (impulse * J) * M^-1, which is 6 multiplies and 6 dot products.
//Note that this optimization's value depends on the number of constrained DOFs.
//Net memory change, opt vs no opt, in scalars:
//1DOF: costs 1x12, saves 1x1 effective mass and the 20 scalar M^-1: -9
//2DOF: costs 2x12, saves 2x2 effective mass and the 20 scalar M^-1: 0
//3DOF: costs 3x12, saves 3x3 effective mass and the 20 scalar M^-1: 7
//4DOF: costs 4x12, saves 4x4 effective mass and the 20 scalar M^-1: 12
//5DOF: costs 5x12, saves 5x5 effective mass and the 20 scalar M^-1: 15
//6DOF: costs 6x12, saves 6x6 effective mass and the 20 scalar M^-1: 16
//Net compute savings, opt vs no opt:
//DOF savings = 1xDOF * DOFxDOF (DOF DOFdot products), 2 1x3 * scalar (6 multiplies), 2 1x3 * 3x3 (6 3dot products)
// = (DOF*DOF multiplies + DOF*(DOF-1) adds) + (6 multiplies) + (18 multiplies + 12 adds)
// = DOF*DOF + 24 multiplies, DOF*DOF-DOF + 12 adds
//1DOF: 25 multiplies, 12 adds
//2DOF: 28 multiplies, 14 adds
//3DOF: 33 multiplies, 18 adds
//4DOF: 40 multiplies, 24 adds
//5DOF: 49 multiplies, 32 adds
//6DOF: 60 multiplies, 42 adds
//So does our 'optimization' actually do anything useful?
//In 1 or 2 DOF constraints, it's a win with no downsides.
//3+ are difficult to determine.
//This depends on heavily on the machine's SIMD width. You do every lane's ALU ops in parallel, but the loads are still fundamentally bound by memory bandwidth.
//The loads are coherent, at least- no gathers on this stuff. But I wouldn't be surprised if 3DOF+ constraints end up being faster *without* the pretransformations on wide SIMD.
//This is just something that will require testing.
//(Also, note that large DOF jacobians are often very sparse. Consider the jacobians used by a 6DOF weld joint. You could likely do special case optimizations to reduce the
//load further. It is unlikely that you could find a way to do the same to JT * effectiveMass. J * M^-1 might have some savings, though. But J*M^-1 isn't *sparser*
//than J by itself, so the space savings are limited. As long as you precompute, the above load requirement offset will persist.)
//Good news, though! There are a lot of 1DOF and 2DOF constraints where this is an unambiguous win.
//We'll start with the unsoftened effective mass, constructed from the contributions computed above:
var effectiveMass = Vector <float> .One / (linearA + linearB + angularA + angularB);
SpringSettingsWide.ComputeSpringiness(ref springSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale);
var softenedEffectiveMass = effectiveMass * effectiveMassCFMScale;
//Note that we use a bit of a hack when computing the bias velocity- even if our damping ratio/natural frequency implies a strongly springy response
//that could cause a significant velocity overshoot, we apply an arbitrary clamping value to keep it reasonable.
//This is useful for a variety of inequality constraints (like contacts) because you don't always want them behaving as true springs.
var biasVelocity = Vector.Min(positionError * positionErrorToVelocity, maximumRecoveryVelocity);
projection.BiasImpulse = biasVelocity * softenedEffectiveMass;
//Precompute the wsv * (JT * softenedEffectiveMass) term.
//Note that we store it in a Vector3Wide as if it's a row vector, but this is really a column (because JT is a column vector).
//So we're really storing (JT * softenedEffectiveMass)T = softenedEffectiveMassT * J.
//Since this constraint is 1DOF, the softenedEffectiveMass is a scalar and the order doesn't matter.
//In the solve iterations, the WSVtoCSI term will be transposed during transformation,
//resulting in the proper wsv * (softenedEffectiveMassT * J)T = wsv * (JT * softenedEffectiveMass).
//You'll see this pattern repeated in higher DOF constraints. We explicitly compute softenedEffectiveMassT * J, and then apply the transpose in the solves.
//(Why? Because creating a Matrix3x2 and Matrix2x3 and 4x3 and 3x4 and 5x3 and 3x5 and so on just doubles the number of representations with little value.)
Vector3Wide.Scale(jacobians.LinearA, softenedEffectiveMass, out projection.WSVtoCSILinearA);
Vector3Wide.Scale(jacobians.AngularA, softenedEffectiveMass, out projection.WSVtoCSIAngularA);
Vector3Wide.Scale(jacobians.LinearB, softenedEffectiveMass, out projection.WSVtoCSILinearB);
Vector3Wide.Scale(jacobians.AngularB, softenedEffectiveMass, out projection.WSVtoCSIAngularB);
}
public static void Prestep(ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref Vector3Wide normal, ref Contact4PrestepData prestep, float dt, float inverseDt,
out Projection projection)
{
//We directly take the prestep data here since the jacobians and error don't undergo any processing.
//The contact penetration constraint takes the form:
//dot(positionA + offsetA, N) >= dot(positionB + offsetB, N)
//Or:
//dot(positionA + offsetA, N) - dot(positionB + offsetB, N) >= 0
//dot(positionA + offsetA - positionB - offsetB, N) >= 0
//where positionA and positionB are the center of mass positions of the bodies offsetA and offsetB are world space offsets from the center of mass to the contact,
//and N is a unit length vector calibrated to point from B to A. (The normal pointing direction is important; it changes the sign.)
//In practice, we'll use the collision detection system's penetration depth instead of trying to recompute the error here.
//So, treating the normal as constant, the velocity constraint is:
//dot(d/dt(positionA + offsetA - positionB - offsetB), N) >= 0
//dot(linearVelocityA + d/dt(offsetA) - linearVelocityB - d/dt(offsetB)), N) >= 0
//The velocity of the offsets are defined by the angular velocity.
//dot(linearVelocityA + angularVelocityA x offsetA - linearVelocityB - angularVelocityB x offsetB), N) >= 0
//dot(linearVelocityA, N) + dot(angularVelocityA x offsetA, N) - dot(linearVelocityB, N) - dot(angularVelocityB x offsetB), N) >= 0
//Use the properties of the scalar triple product:
//dot(linearVelocityA, N) + dot(offsetA x N, angularVelocityA) - dot(linearVelocityB, N) - dot(offsetB x N, angularVelocityB) >= 0
//Bake in the negations:
//dot(linearVelocityA, N) + dot(offsetA x N, angularVelocityA) + dot(linearVelocityB, -N) + dot(-offsetB x N, angularVelocityB) >= 0
//A x B = -B x A:
//dot(linearVelocityA, N) + dot(offsetA x N, angularVelocityA) + dot(linearVelocityB, -N) + dot(N x offsetB, angularVelocityB) >= 0
//And there you go, the jacobians!
//linearA: N
//angularA: offsetA x N
//linearB: -N
//angularB: N x offsetB
//Note that we leave the penetration depth as is, even when it's negative. Speculative contacts!
Vector3Wide.CrossWithoutOverlap(prestep.OffsetA0, normal, out projection.Penetration0.AngularA);
Vector3Wide.Subtract(prestep.OffsetA0, prestep.OffsetB, out var offsetB0);
Vector3Wide.CrossWithoutOverlap(normal, offsetB0, out projection.Penetration0.AngularB);
Vector3Wide.CrossWithoutOverlap(prestep.OffsetA1, normal, out projection.Penetration1.AngularA);
Vector3Wide.Subtract(prestep.OffsetA1, prestep.OffsetB, out var offsetB1);
Vector3Wide.CrossWithoutOverlap(normal, offsetB1, out projection.Penetration1.AngularB);
Vector3Wide.CrossWithoutOverlap(prestep.OffsetA2, normal, out projection.Penetration2.AngularA);
Vector3Wide.Subtract(prestep.OffsetA2, prestep.OffsetB, out var offsetB2);
Vector3Wide.CrossWithoutOverlap(normal, offsetB2, out projection.Penetration2.AngularB);
Vector3Wide.CrossWithoutOverlap(prestep.OffsetA3, normal, out projection.Penetration3.AngularA);
Vector3Wide.Subtract(prestep.OffsetA3, prestep.OffsetB, out var offsetB3);
Vector3Wide.CrossWithoutOverlap(normal, offsetB3, out projection.Penetration3.AngularB);
//effective mass
Symmetric3x3Wide.VectorSandwich(projection.Penetration0.AngularA, inertiaA.InverseInertiaTensor, out var angularA0);
Symmetric3x3Wide.VectorSandwich(projection.Penetration0.AngularB, inertiaB.InverseInertiaTensor, out var angularB0);
Symmetric3x3Wide.VectorSandwich(projection.Penetration1.AngularA, inertiaA.InverseInertiaTensor, out var angularA1);
Symmetric3x3Wide.VectorSandwich(projection.Penetration1.AngularB, inertiaB.InverseInertiaTensor, out var angularB1);
Symmetric3x3Wide.VectorSandwich(projection.Penetration2.AngularA, inertiaA.InverseInertiaTensor, out var angularA2);
Symmetric3x3Wide.VectorSandwich(projection.Penetration2.AngularB, inertiaB.InverseInertiaTensor, out var angularB2);
Symmetric3x3Wide.VectorSandwich(projection.Penetration3.AngularA, inertiaA.InverseInertiaTensor, out var angularA3);
Symmetric3x3Wide.VectorSandwich(projection.Penetration3.AngularB, inertiaB.InverseInertiaTensor, out var angularB3);
//Linear effective mass contribution notes:
//1) The J * M^-1 * JT can be reordered to J * JT * M^-1 for the linear components, since M^-1 is a scalar and dot(n * scalar, n) = dot(n, n) * scalar.
//2) dot(normal, normal) == 1, so the contribution from each body is just its inverse mass.
SpringSettingsWide.ComputeSpringiness(ref prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale);
var linear = inertiaA.InverseMass + inertiaB.InverseMass;
//Note that we don't precompute the JT * effectiveMass term. Since the jacobians are shared, we have to do that multiply anyway.
projection.Penetration0.EffectiveMass = effectiveMassCFMScale / (linear + angularA0 + angularB0);
projection.Penetration1.EffectiveMass = effectiveMassCFMScale / (linear + angularA1 + angularB1);
projection.Penetration2.EffectiveMass = effectiveMassCFMScale / (linear + angularA2 + angularB2);
projection.Penetration3.EffectiveMass = effectiveMassCFMScale / (linear + angularA3 + angularB3);
//If depth is negative, the bias velocity will permit motion up until the depth hits zero. This works because positionErrorToVelocity * dt will always be <=1.
var inverseDtVector = new Vector <float>(inverseDt);
projection.Penetration0.BiasVelocity = Vector.Min(prestep.PenetrationDepth0 * inverseDtVector, Vector.Min(prestep.PenetrationDepth0 * positionErrorToVelocity, prestep.MaximumRecoveryVelocity));
projection.Penetration1.BiasVelocity = Vector.Min(prestep.PenetrationDepth1 * inverseDtVector, Vector.Min(prestep.PenetrationDepth1 * positionErrorToVelocity, prestep.MaximumRecoveryVelocity));
projection.Penetration2.BiasVelocity = Vector.Min(prestep.PenetrationDepth2 * inverseDtVector, Vector.Min(prestep.PenetrationDepth2 * positionErrorToVelocity, prestep.MaximumRecoveryVelocity));
projection.Penetration3.BiasVelocity = Vector.Min(prestep.PenetrationDepth3 * inverseDtVector, Vector.Min(prestep.PenetrationDepth3 * positionErrorToVelocity, prestep.MaximumRecoveryVelocity));
}
public void ComputeSupport(ref CapsuleWide shape, ref Matrix3x3Wide orientation, ref Vector3Wide direction, out Vector3Wide support)
{
Vector3Wide.Scale(orientation.Y, shape.HalfLength, out support);
Vector3Wide.Negate(support, out var negated);
Vector3Wide.Dot(orientation.Y, direction, out var dot);
var shouldNegate = Vector.LessThan(dot, Vector <float> .Zero);
Vector3Wide.ConditionalSelect(shouldNegate, negated, support, out support);
}
public void 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 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}");
}
}
static void Select(ref Vector <float> distanceSquared, ref Vector3Wide localNormal, ref Vector <float> distanceSquaredCandidate, ref Vector3Wide localNormalCandidate)
{
var useCandidate = Vector.LessThan(distanceSquaredCandidate, distanceSquared);
distanceSquared = Vector.Min(distanceSquaredCandidate, distanceSquared);
Vector3Wide.ConditionalSelect(useCandidate, localNormalCandidate, localNormal, out localNormal);
}
public void Test(ref SphereWide a, ref ConvexHullWide b, ref Vector <float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex1ContactManifoldWide manifold)
{
Matrix3x3Wide.CreateFromQuaternion(orientationB, out var hullOrientation);
Matrix3x3Wide.TransformByTransposedWithoutOverlap(offsetB, hullOrientation, out var localOffsetB);
Vector3Wide.Negate(localOffsetB, out var localOffsetA);
Matrix3x3Wide.CreateIdentity(out var identity);
Vector3Wide.Length(localOffsetA, out var centerDistance);
Vector3Wide.Scale(localOffsetA, Vector <float> .One / centerDistance, out var initialNormal);
var useInitialFallback = Vector.LessThan(centerDistance, new Vector <float>(1e-8f));
initialNormal.X = Vector.ConditionalSelect(useInitialFallback, Vector <float> .Zero, initialNormal.X);
initialNormal.Y = Vector.ConditionalSelect(useInitialFallback, Vector <float> .One, initialNormal.Y);
initialNormal.Z = Vector.ConditionalSelect(useInitialFallback, Vector <float> .Zero, initialNormal.Z);
var hullSupportFinder = default(ConvexHullSupportFinder);
var sphereSupportFinder = default(SphereSupportFinder);
ManifoldCandidateHelper.CreateInactiveMask(pairCount, out var inactiveLanes);
b.EstimateEpsilonScale(inactiveLanes, out var hullEpsilonScale);
var epsilonScale = Vector.Min(a.Radius, hullEpsilonScale);
DepthRefiner <ConvexHull, ConvexHullWide, ConvexHullSupportFinder, PhysicsSphere, SphereWide, SphereSupportFinder> .FindMinimumDepth(
b, a, localOffsetA, identity, ref hullSupportFinder, ref sphereSupportFinder, initialNormal, inactiveLanes, 1e-5f *epsilonScale, -speculativeMargin,
out var depth, out var localNormal, out var closestOnHull);
Matrix3x3Wide.TransformWithoutOverlap(closestOnHull, hullOrientation, out var hullToContact);
Matrix3x3Wide.TransformWithoutOverlap(localNormal, hullOrientation, out manifold.Normal);
Vector3Wide.Add(hullToContact, offsetB, out manifold.OffsetA);
manifold.FeatureId = Vector <int> .Zero;
manifold.Depth = depth;
manifold.ContactExists = Vector.GreaterThanOrEqual(manifold.Depth, -speculativeMargin);
}
public static void ComputeJacobians(ref Vector3Wide tangentX, ref Vector3Wide tangentY, ref Vector3Wide offsetA, out Jacobians jacobians)
{
//TODO: there would be a minor benefit in eliminating this copy manually, since it's very likely that the compiler won't. And it's probably also introducing more locals init.
jacobians.LinearA.X = tangentX;
jacobians.LinearA.Y = tangentY;
Vector3Wide.CrossWithoutOverlap(offsetA, tangentX, out jacobians.AngularA.X);
Vector3Wide.CrossWithoutOverlap(offsetA, tangentY, out jacobians.AngularA.Y);
}
public void Test(ref SphereWide a, ref ConvexHullWide b, ref Vector <float> speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex1ContactManifoldWide manifold)
{
throw new NotImplementedException();
}
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 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 not have a maximum impulse limit, so no clamping is required.
Vector3Wide.Add(accumulatedImpulse, correctiveImpulse, out accumulatedImpulse);
ApplyImpulse(ref velocityA, ref velocityB, ref offsetA, ref offsetB, ref inertiaA, ref inertiaB, ref correctiveImpulse);
}
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, Box, 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.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 static void ApplyImpulse(ref BodyVelocities velocityA, ref BodyVelocities velocityB,
ref Vector3Wide offsetA, ref Vector3Wide offsetB, ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref Vector3Wide constraintSpaceImpulse)
{
Vector3Wide.CrossWithoutOverlap(offsetA, constraintSpaceImpulse, out var wsi);
Symmetric3x3Wide.TransformWithoutOverlap(wsi, inertiaA.InverseInertiaTensor, out var change);
Vector3Wide.Add(velocityA.Angular, change, out velocityA.Angular);
Vector3Wide.Scale(constraintSpaceImpulse, inertiaA.InverseMass, out change);
Vector3Wide.Add(velocityA.Linear, change, out velocityA.Linear);
Vector3Wide.CrossWithoutOverlap(constraintSpaceImpulse, offsetB, out wsi); //note flip-negation
Symmetric3x3Wide.TransformWithoutOverlap(wsi, inertiaB.InverseInertiaTensor, out change);
Vector3Wide.Add(velocityB.Angular, change, out velocityB.Angular);
Vector3Wide.Scale(constraintSpaceImpulse, inertiaB.InverseMass, out change);
Vector3Wide.Subtract(velocityB.Linear, change, out velocityB.Linear); //note subtraction; the jacobian is -I
}
public static void ApplyImpulse(ref PenetrationLimitProjection projection, ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref Vector3Wide normal,
ref Vector <float> correctiveImpulse,
ref BodyVelocities wsvA, ref BodyVelocities wsvB)
{
var linearVelocityChangeA = correctiveImpulse * inertiaA.InverseMass;
Vector3Wide.Scale(normal, linearVelocityChangeA, out var correctiveVelocityALinearVelocity);
Vector3Wide.Scale(projection.AngularA, correctiveImpulse, out var correctiveAngularImpulseA);
Symmetric3x3Wide.TransformWithoutOverlap(correctiveAngularImpulseA, inertiaA.InverseInertiaTensor, out var correctiveVelocityAAngularVelocity);
var linearVelocityChangeB = correctiveImpulse * inertiaB.InverseMass;
Vector3Wide.Scale(normal, linearVelocityChangeB, out var correctiveVelocityBLinearVelocity);
Vector3Wide.Scale(projection.AngularB, correctiveImpulse, out var correctiveAngularImpulseB);
Symmetric3x3Wide.TransformWithoutOverlap(correctiveAngularImpulseB, inertiaB.InverseInertiaTensor, out var correctiveVelocityBAngularVelocity);
Vector3Wide.Add(wsvA.Linear, correctiveVelocityALinearVelocity, out wsvA.Linear);
Vector3Wide.Add(wsvA.Angular, correctiveVelocityAAngularVelocity, out wsvA.Angular);
Vector3Wide.Subtract(wsvB.Linear, correctiveVelocityBLinearVelocity, out wsvB.Linear); //Note subtract; normal = -jacobianLinearB
Vector3Wide.Add(wsvB.Angular, correctiveVelocityBAngularVelocity, out wsvB.Angular);
}
public static void WarmStart(ref Vector3Wide angularJacobianA, ref BodyInertias inertiaA,
ref Vector <float> accumulatedImpulse, ref BodyVelocities wsvA)
{
ApplyImpulse(ref angularJacobianA, ref inertiaA, ref accumulatedImpulse, ref wsvA);
}
public static void WarmStart(
ref Projection projection, ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref Vector3Wide normal,
ref Vector <float> accumulatedImpulse0,
ref Vector <float> accumulatedImpulse1,
ref Vector <float> accumulatedImpulse2,
ref Vector <float> accumulatedImpulse3, ref BodyVelocities wsvA, ref BodyVelocities wsvB)
{
ApplyImpulse(ref projection.Penetration0, ref inertiaA, ref inertiaB, ref normal, ref accumulatedImpulse0, ref wsvA, ref wsvB);
ApplyImpulse(ref projection.Penetration1, ref inertiaA, ref inertiaB, ref normal, ref accumulatedImpulse1, ref wsvA, ref wsvB);
ApplyImpulse(ref projection.Penetration2, ref inertiaA, ref inertiaB, ref normal, ref accumulatedImpulse2, ref wsvA, ref wsvB);
ApplyImpulse(ref projection.Penetration3, ref inertiaA, ref inertiaB, ref normal, ref accumulatedImpulse3, ref wsvA, ref wsvB);
}
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 static void Solve(ref Vector3Wide tangentX, ref Vector3Wide tangentY, ref TangentFriction.Projection projection, ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref Vector <float> maximumImpulse, ref Vector2Wide accumulatedImpulse, ref BodyVelocities wsvA, ref BodyVelocities wsvB)
{
ComputeJacobians(ref tangentX, ref tangentY, ref projection.OffsetA, ref projection.OffsetB, out var jacobians);
ComputeCorrectiveImpulse(ref wsvA, ref wsvB, ref projection, ref jacobians, ref maximumImpulse, ref accumulatedImpulse, out var correctiveCSI);
ApplyImpulse(ref jacobians, ref inertiaA, ref inertiaB, ref correctiveCSI, ref wsvA, ref wsvB);
}
