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(offsetA, tangentX, out jacobians.AngularA.X); Vector3Wide.CrossWithoutOverlap(offsetA, tangentY, out jacobians.AngularA.Y); Vector3Wide.CrossWithoutOverlap(tangentX, offsetB, out jacobians.AngularB.X); Vector3Wide.CrossWithoutOverlap(tangentY, offsetB, out jacobians.AngularB.Y); }
public static void Prestep(ref BodyInertias inertiaA, ref Vector3Wide normal, ref Contact3OneBodyPrestepData prestep, float dt, float inverseDt, out Projection projection) { Vector3Wide.CrossWithoutOverlap(prestep.OffsetA0, normal, out projection.Penetration0.AngularA); Vector3Wide.CrossWithoutOverlap(prestep.OffsetA1, normal, out projection.Penetration1.AngularA); Vector3Wide.CrossWithoutOverlap(prestep.OffsetA2, normal, out projection.Penetration2.AngularA); //effective mass Symmetric3x3Wide.VectorSandwich(projection.Penetration0.AngularA, inertiaA.InverseInertiaTensor, out var angularA0); Symmetric3x3Wide.VectorSandwich(projection.Penetration1.AngularA, inertiaA.InverseInertiaTensor, out var angularA1); Symmetric3x3Wide.VectorSandwich(projection.Penetration2.AngularA, inertiaA.InverseInertiaTensor, out var angularA2); //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); //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 / (inertiaA.InverseMass + angularA0); projection.Penetration1.EffectiveMass = effectiveMassCFMScale / (inertiaA.InverseMass + angularA1); projection.Penetration2.EffectiveMass = effectiveMassCFMScale / (inertiaA.InverseMass + angularA2); //If depth is negative, the bias velocity will permit motion up until the depth hits zero. This works because positionErrorToVelocity * dt will always be <=1. var inverseDtVector = new Vector <float>(inverseDt); projection.Penetration0.BiasVelocity = Vector.Min(prestep.PenetrationDepth0 * inverseDtVector, Vector.Min(prestep.PenetrationDepth0 * positionErrorToVelocity, prestep.MaximumRecoveryVelocity)); projection.Penetration1.BiasVelocity = Vector.Min(prestep.PenetrationDepth1 * inverseDtVector, Vector.Min(prestep.PenetrationDepth1 * positionErrorToVelocity, prestep.MaximumRecoveryVelocity)); projection.Penetration2.BiasVelocity = Vector.Min(prestep.PenetrationDepth2 * inverseDtVector, Vector.Min(prestep.PenetrationDepth2 * positionErrorToVelocity, prestep.MaximumRecoveryVelocity)); }
public 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 static void Prestep(ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref Vector3Wide contactOffsetA, ref Vector3Wide contactOffsetB, ref Vector3Wide normal, ref Vector <float> depth, ref SpringSettingsWide springSettings, ref Vector <float> maximumRecoveryVelocity, 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(contactOffsetA, normal, out projection.Penetration0.AngularA); Vector3Wide.CrossWithoutOverlap(normal, contactOffsetB, out projection.Penetration0.AngularB); //effective mass Symmetric3x3Wide.VectorSandwich(projection.Penetration0.AngularA, inertiaA.InverseInertiaTensor, out var angularA0); Symmetric3x3Wide.VectorSandwich(projection.Penetration0.AngularB, inertiaB.InverseInertiaTensor, out var angularB0); //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 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); //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. projection.Penetration0.BiasVelocity = Vector.Min( depth * new Vector <float>(inverseDt), Vector.Min(depth * positionErrorToVelocity, maximumRecoveryVelocity)); }
public static void ComputeCorrectiveImpulse(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref Vector3Wide offsetA, ref Vector3Wide offsetB, ref Vector3Wide biasVelocity, ref Symmetric3x3Wide effectiveMass, ref Vector <float> softnessImpulseScale, ref Vector3Wide accumulatedImpulse, out Vector3Wide correctiveImpulse) { //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); //Note subtraction; jLinearB = -I. Vector3Wide.Subtract(velocityA.Linear, velocityB.Linear, out var csv); Vector3Wide.CrossWithoutOverlap(velocityA.Angular, offsetA, out var angularCSV); Vector3Wide.Add(csv, angularCSV, out csv); //Note reversed cross order; matches the jacobian -CrossMatrix(offsetB). Vector3Wide.CrossWithoutOverlap(offsetB, velocityB.Angular, out angularCSV); Vector3Wide.Add(csv, angularCSV, out csv); Vector3Wide.Subtract(biasVelocity, csv, out csv); Symmetric3x3Wide.TransformWithoutOverlap(csv, effectiveMass, out correctiveImpulse); Vector3Wide.Scale(accumulatedImpulse, softnessImpulseScale, out var softness); Vector3Wide.Subtract(correctiveImpulse, softness, out correctiveImpulse); }
public static void ApplyImpulse(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref Vector3Wide offsetA, ref Vector3Wide offsetB, ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref Vector3Wide constraintSpaceImpulse) { Vector3Wide.CrossWithoutOverlap(offsetA, constraintSpaceImpulse, out var wsi); Symmetric3x3Wide.TransformWithoutOverlap(wsi, inertiaA.InverseInertiaTensor, out var change); Vector3Wide.Add(velocityA.Angular, change, out velocityA.Angular); Vector3Wide.Scale(constraintSpaceImpulse, inertiaA.InverseMass, out change); Vector3Wide.Add(velocityA.Linear, change, out velocityA.Linear); Vector3Wide.CrossWithoutOverlap(constraintSpaceImpulse, offsetB, out wsi); //note flip-negation Symmetric3x3Wide.TransformWithoutOverlap(wsi, inertiaB.InverseInertiaTensor, out change); Vector3Wide.Add(velocityB.Angular, change, out velocityB.Angular); Vector3Wide.Scale(constraintSpaceImpulse, inertiaB.InverseMass, out change); Vector3Wide.Subtract(velocityB.Linear, change, out velocityB.Linear); //note subtraction; the jacobian is -I }
public void Test(ref 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 TriangleWide a, ref ConvexHullWide b, ref Vector <float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, out Convex4ContactManifoldWide manifold) { Matrix3x3Wide.CreateFromQuaternion(orientationA, out var triangleOrientation); Matrix3x3Wide.CreateFromQuaternion(orientationB, out var hullOrientation); Matrix3x3Wide.MultiplyByTransposeWithoutOverlap(triangleOrientation, hullOrientation, out var hullLocalTriangleOrientation); Matrix3x3Wide.TransformByTransposedWithoutOverlap(offsetB, hullOrientation, out var localOffsetB); Vector3Wide.Negate(localOffsetB, out var localOffsetA); TriangleWide triangle; Matrix3x3Wide.TransformWithoutOverlap(a.A, hullLocalTriangleOrientation, out triangle.A); Matrix3x3Wide.TransformWithoutOverlap(a.B, hullLocalTriangleOrientation, out triangle.B); Matrix3x3Wide.TransformWithoutOverlap(a.C, hullLocalTriangleOrientation, out triangle.C); Vector3Wide.Add(triangle.A, triangle.B, out var centroid); Vector3Wide.Add(triangle.C, centroid, out centroid); Vector3Wide.Scale(centroid, new Vector <float>(1f / 3f), out centroid); Vector3Wide.Subtract(triangle.A, centroid, out triangle.A); Vector3Wide.Subtract(triangle.B, centroid, out triangle.B); Vector3Wide.Subtract(triangle.C, centroid, out triangle.C); Vector3Wide.Subtract(centroid, localOffsetB, out var localTriangleCenter); Vector3Wide.Subtract(triangle.B, triangle.A, out var triangleAB); Vector3Wide.Subtract(triangle.C, triangle.B, out var triangleBC); Vector3Wide.Subtract(triangle.A, triangle.C, out var triangleCA); //We'll be using B-local triangle vertices quite a bit, so cache them. Vector3Wide.Add(triangle.A, localTriangleCenter, out var triangleA); Vector3Wide.Add(triangle.B, localTriangleCenter, out var triangleB); Vector3Wide.Add(triangle.C, localTriangleCenter, out var triangleC); Vector3Wide.CrossWithoutOverlap(triangleAB, triangleCA, out var triangleNormal); Vector3Wide.Length(triangleNormal, out var triangleNormalLength); Vector3Wide.Scale(triangleNormal, Vector <float> .One / triangleNormalLength, out triangleNormal); //Check if the hull's position is within the triangle and below the triangle plane. If so, we can ignore it. Vector3Wide.Dot(triangleNormal, localTriangleCenter, out var hullToTriangleCenterDot); var hullBelowPlane = Vector.GreaterThanOrEqual(hullToTriangleCenterDot, Vector <float> .Zero); Vector <int> hullInsideAndBelowTriangle; Vector3Wide.CrossWithoutOverlap(triangleAB, triangleNormal, out var edgePlaneAB); Vector3Wide.CrossWithoutOverlap(triangleBC, triangleNormal, out var edgePlaneBC); Vector3Wide.CrossWithoutOverlap(triangleCA, triangleNormal, out var edgePlaneCA); if (Vector.LessThanAny(hullBelowPlane, Vector <int> .Zero)) { //Is the hull position within the triangle bounds? Vector3Wide.Dot(edgePlaneAB, triangleA, out var abPlaneTest); Vector3Wide.Dot(edgePlaneBC, triangleB, out var bcPlaneTest); Vector3Wide.Dot(edgePlaneCA, triangleC, out var caPlaneTest); hullInsideAndBelowTriangle = Vector.BitwiseAnd( Vector.BitwiseAnd(hullBelowPlane, Vector.LessThanOrEqual(abPlaneTest, Vector <float> .Zero)), Vector.BitwiseAnd(Vector.LessThanOrEqual(bcPlaneTest, Vector <float> .Zero), Vector.LessThanOrEqual(caPlaneTest, Vector <float> .Zero))); } else { hullInsideAndBelowTriangle = Vector <int> .Zero; } ManifoldCandidateHelper.CreateInactiveMask(pairCount, out var inactiveLanes); a.EstimateEpsilonScale(out var triangleEpsilonScale); b.EstimateEpsilonScale(inactiveLanes, out var hullEpsilonScale); var epsilonScale = Vector.Min(triangleEpsilonScale, hullEpsilonScale); inactiveLanes = Vector.BitwiseOr(inactiveLanes, Vector.LessThan(triangleNormalLength, epsilonScale * 1e-6f)); inactiveLanes = Vector.BitwiseOr(inactiveLanes, hullInsideAndBelowTriangle); //Not every lane will generate contacts. Rather than requiring every lane to carefully clear all contactExists states, just clear them up front. manifold.Contact0Exists = default; manifold.Contact1Exists = default; manifold.Contact2Exists = default; manifold.Contact3Exists = default; if (Vector.LessThanAll(inactiveLanes, Vector <int> .Zero)) { //No contacts generated. return; } //Note the use of the triangle center as the initial normal rather than the localOffsetA. //Triangles are not guaranteed to be centered on their center of mass, and the DepthRefiner //will converge to a depth which does not oppose the so-far best normal- which, on the early iterations, //could be the initial normal. Vector3Wide.Length(localTriangleCenter, out var centerDistance); Vector3Wide.Scale(localTriangleCenter, Vector <float> .One / centerDistance, out var initialNormal); var useInitialFallback = Vector.LessThan(centerDistance, new Vector <float>(1e-10f)); initialNormal.X = Vector.ConditionalSelect(useInitialFallback, Vector <float> .Zero, initialNormal.X); initialNormal.Y = Vector.ConditionalSelect(useInitialFallback, Vector <float> .One, initialNormal.Y); initialNormal.Z = Vector.ConditionalSelect(useInitialFallback, Vector <float> .Zero, initialNormal.Z); //Check if the extreme point of the hull toward the triangle along its face normal lies inside the triangle. //If it is, then there's no need for depth refinement. Vector <int> triangleNormalIsMinimal; var hullSupportFinder = default(ConvexHullSupportFinder); DepthRefiner.SimplexWithWitness simplex; var triangleSupportFinder = default(PretransformedTriangleSupportFinder); //Create a simplex entry for the direction from the hull center to triangle center. DepthRefiner.FindSupport(b, triangle, localTriangleCenter, hullLocalTriangleOrientation, ref hullSupportFinder, ref triangleSupportFinder, initialNormal, inactiveLanes, out simplex.A.Support, out simplex.A.SupportOnA); Vector3Wide.Dot(simplex.A.Support, initialNormal, out var depth); simplex.A.Exists = Vector.OnesComplement(inactiveLanes); //Create a simplex entry for the triangle face normal. Vector3Wide.Negate(triangleNormal, out var negatedTriangleNormal); hullSupportFinder.ComputeLocalSupport(b, negatedTriangleNormal, inactiveLanes, out var hullSupportAlongTriangleNormal); simplex.B.SupportOnA = hullSupportAlongTriangleNormal; Vector3Wide.Subtract(simplex.B.SupportOnA, localTriangleCenter, out simplex.B.Support); Vector3Wide.Dot(simplex.B.Support, negatedTriangleNormal, out var triangleFaceDepth); var useTriangleFace = Vector.LessThan(triangleFaceDepth, depth); Vector3Wide.ConditionalSelect(useTriangleFace, negatedTriangleNormal, initialNormal, out initialNormal); depth = Vector.ConditionalSelect(useTriangleFace, triangleFaceDepth, depth); simplex.B.Exists = simplex.A.Exists; simplex.C.Exists = default; //Check if the extreme point on the hull is contained within the bounds of the triangle face. If it is, there is no need for a full depth refinement. Vector3Wide.Subtract(triangleA, hullSupportAlongTriangleNormal, out var closestToA); Vector3Wide.Subtract(triangleB, hullSupportAlongTriangleNormal, out var closestToB); Vector3Wide.Subtract(triangleC, hullSupportAlongTriangleNormal, out var closestToC); Vector3Wide.Dot(edgePlaneAB, closestToA, out var extremeABPlaneTest); Vector3Wide.Dot(edgePlaneBC, closestToB, out var extremeBCPlaneTest); Vector3Wide.Dot(edgePlaneCA, closestToC, out var extremeCAPlaneTest); triangleNormalIsMinimal = Vector.BitwiseAnd(Vector.LessThanOrEqual(extremeABPlaneTest, Vector <float> .Zero), Vector.BitwiseAnd(Vector.LessThanOrEqual(extremeBCPlaneTest, Vector <float> .Zero), Vector.LessThanOrEqual(extremeCAPlaneTest, Vector <float> .Zero))); var depthThreshold = -speculativeMargin; var skipDepthRefine = Vector.BitwiseOr(triangleNormalIsMinimal, inactiveLanes); Vector3Wide localNormal, closestOnHull; if (Vector.EqualsAny(skipDepthRefine, Vector <int> .Zero)) { DepthRefiner.FindMinimumDepth( b, triangle, localTriangleCenter, hullLocalTriangleOrientation, ref hullSupportFinder, ref triangleSupportFinder, ref simplex, initialNormal, depth, skipDepthRefine, 1e-5f * epsilonScale, depthThreshold, out var refinedDepth, out var refinedNormal, out var refinedClosestOnHull); Vector3Wide.ConditionalSelect(skipDepthRefine, hullSupportAlongTriangleNormal, refinedClosestOnHull, out closestOnHull); Vector3Wide.ConditionalSelect(skipDepthRefine, initialNormal, refinedNormal, out localNormal); depth = Vector.ConditionalSelect(skipDepthRefine, depth, refinedDepth); } else { //No depth refine ran; the extreme point prepass did everything we needed. Just use the initial normal. localNormal = initialNormal; closestOnHull = hullSupportAlongTriangleNormal; } Vector3Wide.Dot(triangleNormal, localNormal, out var triangleNormalDotLocalNormal); inactiveLanes = Vector.BitwiseOr(inactiveLanes, Vector.BitwiseOr(Vector.GreaterThanOrEqual(triangleNormalDotLocalNormal, Vector <float> .Zero), Vector.LessThan(depth, depthThreshold))); if (Vector.LessThanAll(inactiveLanes, Vector <int> .Zero)) { //No contacts generated. return; } //To find the contact manifold, we'll clip the triangle edges against the hull face as usual, but we're dealing with potentially //distinct convex hulls. Rather than vectorizing over the different hulls, we vectorize within each hull. Helpers.FillVectorWithLaneIndices(out var slotOffsetIndices); var boundingPlaneEpsilon = 1e-3f * epsilonScale; //There can be no more than 6 contacts (provided there are no numerical errors); 2 per triangle edge. var candidates = stackalloc ManifoldCandidateScalar[6]; for (int slotIndex = 0; slotIndex < pairCount; ++slotIndex) { if (inactiveLanes[slotIndex] < 0) { continue; } ref var hull = ref b.Hulls[slotIndex]; ConvexHullTestHelper.PickRepresentativeFace(ref hull, slotIndex, ref localNormal, closestOnHull, slotOffsetIndices, ref boundingPlaneEpsilon, out var slotFaceNormal, out var slotLocalNormal, out var bestFaceIndex); //Test each triangle edge against the hull face. //Note that we do not use the faceNormal x edgeOffset edge plane, but rather edgeOffset x localNormal. //The faces are wound counterclockwise. //Note that the triangle edges are packed into a Vector4. Historically, there were some minor codegen issues with Vector3. //May not matter anymore, but it costs ~nothing to use a dead slot. ref var aSlot = ref GatherScatter.GetOffsetInstance(ref triangleA, slotIndex);
public void Test( ref CapsuleWide a, ref CapsuleWide b, ref Vector <float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, out Convex2ContactManifoldWide manifold) { //Compute the closest points between the two line segments. No clamping to begin with. //We want to minimize distance = ||(a + da * ta) - (b + db * tb)||. //Taking the derivative with respect to ta and doing some algebra (taking into account ||da|| == ||db|| == 1) to solve for ta yields: //ta = (da * (b - a) + (db * (a - b)) * (da * db)) / (1 - ((da * db) * (da * db)) QuaternionWide.TransformUnitXY(orientationA, out var xa, out var da); QuaternionWide.TransformUnitY(orientationB, out var db); Vector3Wide.Dot(da, offsetB, out var daOffsetB); Vector3Wide.Dot(db, offsetB, out var dbOffsetB); Vector3Wide.Dot(da, db, out var dadb); //Note potential division by zero when the axes are parallel. Arbitrarily clamp; near zero values will instead produce extreme values which get clamped to reasonable results. var ta = (daOffsetB - dbOffsetB * dadb) / Vector.Max(new Vector <float>(1e-15f), Vector <float> .One - dadb * dadb); //tb = ta * (da * db) - db * (b - a) var tb = ta * dadb - dbOffsetB; //We cannot simply clamp the ta and tb values to the capsule line segments. Instead, project each line segment onto the other line segment, clamping against the target's interval. //That new clamped projected interval is the valid solution space on that line segment. We can clamp the t value by that interval to get the correctly bounded solution. //The projected intervals are: //B onto A: +-BHalfLength * (da * db) + da * offsetB //A onto B: +-AHalfLength * (da * db) - db * offsetB var absdadb = Vector.Abs(dadb); var bOntoAOffset = b.HalfLength * absdadb; var aOntoBOffset = a.HalfLength * absdadb; var aMin = Vector.Max(-a.HalfLength, Vector.Min(a.HalfLength, daOffsetB - bOntoAOffset)); var aMax = Vector.Min(a.HalfLength, Vector.Max(-a.HalfLength, daOffsetB + bOntoAOffset)); var bMin = Vector.Max(-b.HalfLength, Vector.Min(b.HalfLength, -aOntoBOffset - dbOffsetB)); var bMax = Vector.Min(b.HalfLength, Vector.Max(-b.HalfLength, aOntoBOffset - dbOffsetB)); ta = Vector.Min(Vector.Max(ta, aMin), aMax); tb = Vector.Min(Vector.Max(tb, bMin), bMax); Vector3Wide.Scale(da, ta, out var closestPointOnA); Vector3Wide.Scale(db, tb, out var closestPointOnB); Vector3Wide.Add(closestPointOnB, offsetB, out closestPointOnB); //Note that normals are calibrated to point from B to A by convention. Vector3Wide.Subtract(closestPointOnA, closestPointOnB, out manifold.Normal); Vector3Wide.Length(manifold.Normal, out var distance); var inverseDistance = Vector <float> .One / distance; Vector3Wide.Scale(manifold.Normal, inverseDistance, out manifold.Normal); //In the event that the line segments are touching, the normal doesn't exist and we need an alternative. Any direction along the local horizontal (XZ) plane of either capsule //is valid. (Normals along the local Y axes are not guaranteed to be as quick of a path to separation due to nonzero line length.) var normalIsValid = Vector.GreaterThan(distance, new Vector <float>(1e-7f)); Vector3Wide.ConditionalSelect(normalIsValid, manifold.Normal, xa, out manifold.Normal); //In the event that the two capsule axes are coplanar, we accept the whole interval as a source of contact. //As the axes drift away from coplanarity, the accepted interval rapidly narrows to zero length, centered on ta and tb. //We rate the degree of coplanarity based on the angle between the capsule axis and the plane defined by the opposing segment and contact normal: //sin(angle) = dot(da, (db x normal)/||db x normal||) //Finally, note that we are dealing with extremely small angles, and for small angles sin(angle) ~= angle, //and also that fade behavior is completely arbitrary, so we can directly use squared angle without any concern. //angle^2 ~= dot(da, (db x normal))^2 / ||db x normal||^2 //Note that if ||db x normal|| is zero, then any da should be accepted as being coplanar because there is no restriction. ConditionalSelect away the discontinuity. Vector3Wide.CrossWithoutOverlap(db, manifold.Normal, out var planeNormal); Vector3Wide.LengthSquared(planeNormal, out var planeNormalLengthSquared); Vector3Wide.Dot(da, planeNormal, out var numeratorUnsquared); var squaredAngle = Vector.ConditionalSelect(Vector.LessThan(planeNormalLengthSquared, new Vector <float>(1e-10f)), Vector <float> .Zero, numeratorUnsquared * numeratorUnsquared / planeNormalLengthSquared); //Convert the squared angle to a lerp parameter. For squared angle from 0 to lowerThreshold, we should use the full interval (1). From lowerThreshold to upperThreshold, lerp to 0. const float lowerThresholdAngle = 0.01f; const float upperThresholdAngle = 0.05f; const float lowerThreshold = lowerThresholdAngle * lowerThresholdAngle; const float upperThreshold = upperThresholdAngle * upperThresholdAngle; var intervalWeight = Vector.Max(Vector <float> .Zero, Vector.Min(Vector <float> .One, (new Vector <float>(upperThreshold) - squaredAngle) * new Vector <float>(1f / (upperThreshold - lowerThreshold)))); //If the line segments intersect, even if they're coplanar, we would ideally stick to using a single point. Would be easy enough, //but we don't bother because it's such a weird and extremely temporary corner case. Not really worth handling. var weightedTa = ta - ta * intervalWeight; aMin = intervalWeight * aMin + weightedTa; aMax = intervalWeight * aMax + weightedTa; Vector3Wide.Scale(da, aMin, out manifold.OffsetA0); Vector3Wide.Scale(da, aMax, out manifold.OffsetA1); //In the coplanar case, there are two points. We need a method of computing depth which gives a reasonable result to the second contact. //Note that one of the two contacts should end up with a distance equal to the previously computed segment distance, so we're doing some redundant work here. //It's just easier to do that extra work than it would be to track which endpoint contributed the lower distance. //Unproject the final interval endpoints from a back onto b. //dot(offsetB + db * tb0, da) = ta0 //tb0 = (ta0 - daOffsetB) / dadb //distance0 = dot(a0 - (offsetB + tb0 * db), normal) //distance1 = dot(a1 - (offsetB + tb1 * db), normal) Vector3Wide.Dot(db, manifold.Normal, out var dbNormal); Vector3Wide.Subtract(manifold.OffsetA0, offsetB, out var offsetB0); Vector3Wide.Subtract(manifold.OffsetA1, offsetB, out var offsetB1); //Note potential division by zero. In that case, treat both projected points as the closest point. (Handled by the conditional select that chooses the previously computed distance.) var inverseDadb = Vector <float> .One / dadb; var projectedTb0 = Vector.Max(bMin, Vector.Min(bMax, (aMin - daOffsetB) * inverseDadb)); var projectedTb1 = Vector.Max(bMin, Vector.Min(bMax, (aMax - daOffsetB) * inverseDadb)); Vector3Wide.Dot(offsetB0, manifold.Normal, out var b0Normal); Vector3Wide.Dot(offsetB1, manifold.Normal, out var b1Normal); var capsulesArePerpendicular = Vector.LessThan(Vector.Abs(dadb), new Vector <float>(1e-7f)); var distance0 = Vector.ConditionalSelect(capsulesArePerpendicular, distance, b0Normal - dbNormal * projectedTb0); var distance1 = Vector.ConditionalSelect(capsulesArePerpendicular, distance, b1Normal - dbNormal * projectedTb1); var combinedRadius = a.Radius + b.Radius; manifold.Depth0 = combinedRadius - distance0; manifold.Depth1 = combinedRadius - distance1; //Apply the normal offset to the contact positions. var negativeOffsetFromA0 = manifold.Depth0 * 0.5f - a.Radius; var negativeOffsetFromA1 = manifold.Depth1 * 0.5f - a.Radius; Vector3Wide.Scale(manifold.Normal, negativeOffsetFromA0, out var normalPush0); Vector3Wide.Scale(manifold.Normal, negativeOffsetFromA1, out var normalPush1); Vector3Wide.Add(manifold.OffsetA0, normalPush0, out manifold.OffsetA0); Vector3Wide.Add(manifold.OffsetA1, normalPush1, out manifold.OffsetA1); manifold.FeatureId0 = Vector <int> .Zero; manifold.FeatureId1 = Vector <int> .One; var minimumAcceptedDepth = -speculativeMargin; manifold.Contact0Exists = Vector.GreaterThanOrEqual(manifold.Depth0, minimumAcceptedDepth); manifold.Contact1Exists = Vector.BitwiseAnd( Vector.GreaterThanOrEqual(manifold.Depth1, minimumAcceptedDepth), Vector.GreaterThan(aMax - aMin, new Vector <float>(1e-7f) * a.HalfLength)); //TODO: Since we added in the complexity of 2 contact support, this is probably large enough to benefit from working in the local space of one of the capsules. //Worth looking into later. }
public void Test(ref SphereWide a, ref TriangleWide b, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, out Vector <int> intersected, out Vector <float> distance, out Vector3Wide closestA, out Vector3Wide normal) { //Note that we're borrowing a lot here from the SphereTriangleCollisionTask. Could share more if you find yourself needing to change things dramatically. //Main difficulty in fully sharing is that sweep tests do not honor one sidedness, so some of the conditions change. //Work in the local space of the triangle, since it's quicker to transform the sphere position than the vertices of the triangle. Matrix3x3Wide.CreateFromQuaternion(orientationB, out var rB); Matrix3x3Wide.TransformByTransposedWithoutOverlap(offsetB, rB, out var localOffsetB); Vector3Wide.Subtract(b.B, b.A, out var ab); Vector3Wide.Subtract(b.C, b.A, out var ac); //localOffsetA = -localOffsetB, so pa = triangle.A + localOffsetB. Vector3Wide.Add(b.A, localOffsetB, out var pa); Vector3Wide.CrossWithoutOverlap(ab, ac, out var localTriangleNormal); Vector3Wide.Dot(localTriangleNormal, pa, out var paN); //EdgeAB plane test: (pa x ab) * (ab x ac) >= 0 //EdgeAC plane test: (ac x pa) * (ab x ac) >= 0 //Note that these are scaled versions of the barycentric coordinates. //To normalize them such that the weights of a point within the triangle equal 1, we just need to divide by dot(ab x ac, ab x ac). //In other words, to test the third edge plane, we can ensure that the unnormalized weights are both positive and sum to a value less than dot(ab x ac, ab x ac). //If a point is outside of an edge plane, we know that it's not in the face region or any other edge region. It could, however, be in an adjacent vertex region. //Vertex cases can be handled by clamping an edge case. //Further, note that for any query location, it is sufficient to only test one edge even if the point is outside two edge planes. If it's outside two edge planes, //that just means it's going to be on the shared vertex, so a clamped edge test captures the correct closest point. //So, at each edge, if the point is outside the plane, cache the edge. The last edge registering an outside result will be tested. //(pa x ab) * (ab x ac) = (pa * ab) * (ab * ac) - (pa * ac) * (ab * ab) //(ac x pa) * (ab x ac) = (ac * ab) * (pa * ac) - (ac * ac) * (pa * ab) //(ab x ac) * (ab x ac) = (ab * ab) * (ac * ac) - (ab * ac) * (ab * ac) Vector3Wide.Dot(pa, ab, out var abpa); Vector3Wide.Dot(ab, ac, out var abac); Vector3Wide.Dot(ac, pa, out var acpa); Vector3Wide.Dot(ac, ac, out var acac); Vector3Wide.Dot(ab, ab, out var abab); var edgePlaneTestAB = abpa * abac - acpa * abab; var edgePlaneTestAC = abac * acpa - acac * abpa; var triangleNormalLengthSquared = abab * acac - abac * abac; var edgePlaneTestBC = triangleNormalLengthSquared - edgePlaneTestAB - edgePlaneTestAC; var outsideAB = Vector.LessThan(edgePlaneTestAB, Vector <float> .Zero); var outsideAC = Vector.LessThan(edgePlaneTestAC, Vector <float> .Zero); var outsideBC = Vector.LessThan(edgePlaneTestBC, Vector <float> .Zero); var outsideAnyEdge = Vector.BitwiseOr(outsideAB, Vector.BitwiseOr(outsideAC, outsideBC)); Vector3Wide localClosestOnTriangle; var negativeOne = new Vector <int>(-1); if (Vector.EqualsAny(outsideAnyEdge, negativeOne)) { //At least one lane detected a point outside of the triangle. Choose one edge which is outside as the representative. Vector3Wide.ConditionalSelect(outsideAC, ac, ab, out var edgeDirection); Vector3Wide.Subtract(b.C, b.B, out var bc); Vector3Wide.ConditionalSelect(outsideBC, bc, edgeDirection, out edgeDirection); Vector3Wide.ConditionalSelect(outsideBC, b.B, b.A, out var edgeStart); Vector3Wide.Add(localOffsetB, edgeStart, out var negativeEdgeStartToP); //This does some partially redundant work if the edge is AB or AC, but given that we didn't have bcbc or bcpb, it's fine. Vector3Wide.Dot(negativeEdgeStartToP, edgeDirection, out var negativeOffsetDotEdge); Vector3Wide.Dot(edgeDirection, edgeDirection, out var edgeDotEdge); var edgeScale = Vector.Max(Vector <float> .Zero, Vector.Min(Vector <float> .One, -negativeOffsetDotEdge / edgeDotEdge)); Vector3Wide.Scale(edgeDirection, edgeScale, out var pointOnEdge); Vector3Wide.Add(edgeStart, pointOnEdge, out pointOnEdge); Vector3Wide.ConditionalSelect(outsideAnyEdge, pointOnEdge, localClosestOnTriangle, out localClosestOnTriangle); } if (Vector.EqualsAny(outsideAnyEdge, Vector <int> .Zero)) { //p + N * (pa * N) / ||N||^2 = N * (pa * N) / ||N||^2 - (-p) var nScale = paN / triangleNormalLengthSquared; Vector3Wide.Scale(localTriangleNormal, nScale, out var offsetToPlane); Vector3Wide.Subtract(offsetToPlane, localOffsetB, out var pointOnFace); Vector3Wide.ConditionalSelect(outsideAnyEdge, localClosestOnTriangle, pointOnFace, out localClosestOnTriangle); } //normal = normalize(localOffsetA - localClosestOnTriangle) = (localOffsetB + localClosestOnTriangle) / (-||localOffsetB + localClosestOnTriangle||) Vector3Wide.Add(localOffsetB, localClosestOnTriangle, out var localNormal); Vector3Wide.Length(localNormal, out var localNormalLength); Vector3Wide.Scale(localNormal, new Vector <float>(-1f) / localNormalLength, out localNormal); Matrix3x3Wide.TransformWithoutOverlap(localNormal, rB, out normal); Vector3Wide.Scale(normal, -a.Radius, out closestA); distance = localNormalLength - a.Radius; intersected = Vector.LessThanOrEqual(distance, Vector <float> .Zero); }