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 unsafe void Test(ref BoxWide a, ref ConvexHullWide b, ref Vector <float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, out Convex4ContactManifoldWide manifold) { Matrix3x3Wide.CreateFromQuaternion(orientationA, out var boxOrientation); Matrix3x3Wide.CreateFromQuaternion(orientationB, out var hullOrientation); Matrix3x3Wide.MultiplyByTransposeWithoutOverlap(boxOrientation, hullOrientation, out var hullLocalBoxOrientation); Matrix3x3Wide.TransformByTransposedWithoutOverlap(offsetB, hullOrientation, out var localOffsetB); Vector3Wide.Negate(localOffsetB, out var localOffsetA); Vector3Wide.Length(localOffsetA, out var centerDistance); Vector3Wide.Scale(localOffsetA, Vector <float> .One / centerDistance, out var initialNormal); var useInitialFallback = Vector.LessThan(centerDistance, new Vector <float>(1e-8f)); initialNormal.X = Vector.ConditionalSelect(useInitialFallback, Vector <float> .Zero, initialNormal.X); initialNormal.Y = Vector.ConditionalSelect(useInitialFallback, Vector <float> .One, initialNormal.Y); initialNormal.Z = Vector.ConditionalSelect(useInitialFallback, Vector <float> .Zero, initialNormal.Z); var hullSupportFinder = default(ConvexHullSupportFinder); var boxSupportFinder = default(BoxSupportFinder); ManifoldCandidateHelper.CreateInactiveMask(pairCount, out var inactiveLanes); b.EstimateEpsilonScale(inactiveLanes, out var hullEpsilonScale); var epsilonScale = Vector.Min(Vector.Max(a.HalfWidth, Vector.Max(a.HalfHeight, a.HalfLength)), hullEpsilonScale); var depthThreshold = -speculativeMargin; DepthRefiner <ConvexHull, ConvexHullWide, ConvexHullSupportFinder, PhysicsBox, BoxWide, BoxSupportFinder> .FindMinimumDepth( b, a, localOffsetA, hullLocalBoxOrientation, ref hullSupportFinder, ref boxSupportFinder, initialNormal, inactiveLanes, 1e-5f *epsilonScale, depthThreshold, out var depth, out var localNormal, out var closestOnHull); inactiveLanes = Vector.BitwiseOr(inactiveLanes, Vector.LessThan(depth, depthThreshold)); //Not every lane will generate contacts. Rather than requiring every lane to carefully clear all contactExists states, just clear them up front. manifold = default; manifold.Contact0Exists = default; manifold.Contact1Exists = default; manifold.Contact2Exists = default; manifold.Contact3Exists = default; if (Vector.LessThanAll(inactiveLanes, Vector <int> .Zero)) { //No contacts generated. return; } //Identify the box face. Matrix3x3Wide.TransformByTransposedWithoutOverlap(localNormal, hullLocalBoxOrientation, out var localNormalInA); Vector3Wide.Abs(localNormalInA, out var absLocalNormalInA); var useX = Vector.BitwiseAnd(Vector.GreaterThan(absLocalNormalInA.X, absLocalNormalInA.Y), Vector.GreaterThan(absLocalNormalInA.X, absLocalNormalInA.Z)); var useY = Vector.AndNot(Vector.GreaterThan(absLocalNormalInA.Y, absLocalNormalInA.Z), useX); Vector3Wide.ConditionalSelect(useX, hullLocalBoxOrientation.X, hullLocalBoxOrientation.Z, out var boxFaceNormal); Vector3Wide.ConditionalSelect(useY, hullLocalBoxOrientation.Y, boxFaceNormal, out boxFaceNormal); Vector3Wide.ConditionalSelect(useX, hullLocalBoxOrientation.Y, hullLocalBoxOrientation.X, out var boxFaceX); Vector3Wide.ConditionalSelect(useY, hullLocalBoxOrientation.Z, boxFaceX, out boxFaceX); Vector3Wide.ConditionalSelect(useX, hullLocalBoxOrientation.Z, hullLocalBoxOrientation.Y, out var boxFaceY); Vector3Wide.ConditionalSelect(useY, hullLocalBoxOrientation.X, boxFaceY, out boxFaceY); var negateFace = Vector.ConditionalSelect(useX, Vector.GreaterThan(localNormalInA.X, Vector <float> .Zero), Vector.ConditionalSelect(useY, Vector.GreaterThan(localNormalInA.Y, Vector <float> .Zero), Vector.GreaterThan(localNormalInA.Z, Vector <float> .Zero))); Vector3Wide.ConditionallyNegate(negateFace, ref boxFaceNormal); //Winding is important; flip the face bases if necessary. Vector3Wide.ConditionallyNegate(Vector.OnesComplement(negateFace), ref boxFaceX); var boxFaceHalfWidth = Vector.ConditionalSelect(useX, a.HalfHeight, Vector.ConditionalSelect(useY, a.HalfLength, a.HalfWidth)); var boxFaceHalfHeight = Vector.ConditionalSelect(useX, a.HalfLength, Vector.ConditionalSelect(useY, a.HalfWidth, a.HalfHeight)); var boxFaceNormalOffset = Vector.ConditionalSelect(useX, a.HalfWidth, Vector.ConditionalSelect(useY, a.HalfHeight, a.HalfLength)); Vector3Wide.Scale(boxFaceNormal, boxFaceNormalOffset, out var boxFaceCenterOffset); Vector3Wide.Add(boxFaceCenterOffset, localOffsetA, out var boxFaceCenter); Vector3Wide.Scale(boxFaceX, boxFaceHalfWidth, out var boxFaceXOffset); Vector3Wide.Scale(boxFaceY, boxFaceHalfHeight, out var boxFaceYOffset); Vector3Wide.Subtract(boxFaceCenter, boxFaceXOffset, out var v0); Vector3Wide.Add(boxFaceCenter, boxFaceXOffset, out var v1); Vector3Wide.Subtract(v0, boxFaceYOffset, out var v00); Vector3Wide.Add(v0, boxFaceYOffset, out var v01); Vector3Wide.Subtract(v1, boxFaceYOffset, out var v10); Vector3Wide.Add(v1, boxFaceYOffset, out var v11); //To find the contact manifold, we'll clip the box edges against the hull face as usual, but we're dealing with potentially //distinct convex hulls. Rather than vectorizing over the different hulls, we vectorize within each hull. Helpers.FillVectorWithLaneIndices(out var slotOffsetIndices); var boundingPlaneEpsilon = 1e-3f * epsilonScale; //There can be no more than 8 contacts (provided there are no numerical errors); 2 per box edge. var candidates = stackalloc ManifoldCandidateScalar[8]; for (int slotIndex = 0; slotIndex < pairCount; ++slotIndex) { if (inactiveLanes[slotIndex] < 0) { continue; } ref var hull = ref b.Hulls[slotIndex]; ConvexHullTestHelper.PickRepresentativeFace(ref hull, slotIndex, ref localNormal, closestOnHull, slotOffsetIndices, ref boundingPlaneEpsilon, out var slotFaceNormal, out var slotLocalNormal, out var bestFaceIndex); //Test each face edge plane against the box face. //Note that we do not use the faceNormal x edgeOffset edge plane, but rather edgeOffset x localNormal. //The faces are wound counterclockwise in right handed coordinates. //X is 00->10; Y is 10->11; Z is 11->01; W is 01->00. ref var v00Slot = ref GatherScatter.GetOffsetInstance(ref v00, slotIndex);
public unsafe void Test(ref ConvexHullWide a, ref ConvexHullWide b, ref Vector <float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, out Convex4ContactManifoldWide manifold) { Matrix3x3Wide.CreateFromQuaternion(orientationA, out var rA); Matrix3x3Wide.CreateFromQuaternion(orientationB, out var rB); Matrix3x3Wide.MultiplyByTransposeWithoutOverlap(rA, rB, out var bLocalOrientationA); Matrix3x3Wide.TransformByTransposedWithoutOverlap(offsetB, rB, out var localOffsetB); Vector3Wide.Negate(localOffsetB, out var localOffsetA); Vector3Wide.Length(localOffsetA, out var centerDistance); Vector3Wide.Scale(localOffsetA, Vector <float> .One / centerDistance, out var initialNormal); var useInitialFallback = Vector.LessThan(centerDistance, new Vector <float>(1e-8f)); initialNormal.X = Vector.ConditionalSelect(useInitialFallback, Vector <float> .Zero, initialNormal.X); initialNormal.Y = Vector.ConditionalSelect(useInitialFallback, Vector <float> .One, initialNormal.Y); initialNormal.Z = Vector.ConditionalSelect(useInitialFallback, Vector <float> .Zero, initialNormal.Z); var hullSupportFinder = default(ConvexHullSupportFinder); ManifoldCandidateHelper.CreateInactiveMask(pairCount, out var inactiveLanes); a.EstimateEpsilonScale(inactiveLanes, out var aEpsilonScale); b.EstimateEpsilonScale(inactiveLanes, out var bEpsilonScale); var epsilonScale = Vector.Min(aEpsilonScale, bEpsilonScale); var depthThreshold = -speculativeMargin; DepthRefiner <ConvexHull, ConvexHullWide, ConvexHullSupportFinder, ConvexHull, ConvexHullWide, ConvexHullSupportFinder> .FindMinimumDepth( b, a, localOffsetA, bLocalOrientationA, ref hullSupportFinder, ref hullSupportFinder, initialNormal, inactiveLanes, 1e-5f *epsilonScale, depthThreshold, out var depth, out var localNormal, out var closestOnB); inactiveLanes = Vector.BitwiseOr(inactiveLanes, Vector.LessThan(depth, depthThreshold)); //Not every lane will generate contacts. Rather than requiring every lane to carefully clear all contactExists states, just clear them up front. manifold = default; manifold.Contact0Exists = default; manifold.Contact1Exists = default; manifold.Contact2Exists = default; manifold.Contact3Exists = default; if (Vector.LessThanAll(inactiveLanes, Vector <int> .Zero)) { //No contacts generated. return; } Matrix3x3Wide.TransformByTransposedWithoutOverlap(localNormal, bLocalOrientationA, out var localNormalInA); Vector3Wide.Negate(localNormalInA, out var negatedLocalNormalInA); Vector3Wide.Scale(localNormal, depth, out var negatedOffsetToClosestOnA); Vector3Wide.Subtract(closestOnB, negatedOffsetToClosestOnA, out var closestOnA); Vector3Wide.Subtract(closestOnA, localOffsetA, out var aToClosestOnA); Matrix3x3Wide.TransformByTransposedWithoutOverlap(aToClosestOnA, bLocalOrientationA, out var closestOnAInA); //To find the contact manifold, we'll clip the capsule axis against the face as usual, but we're dealing with potentially //distinct convex hulls. Rather than vectorizing over the different hulls, we vectorize within each hull. Helpers.FillVectorWithLaneIndices(out var slotOffsetIndices); var boundingPlaneEpsilon = 1e-3f * epsilonScale; for (int slotIndex = 0; slotIndex < pairCount; ++slotIndex) { if (inactiveLanes[slotIndex] < 0) { continue; } ref var aSlot = ref a.Hulls[slotIndex]; ref var bSlot = ref b.Hulls[slotIndex];
public 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); Vector3Wide.CrossWithoutOverlap(triangleAB, triangleNormal, out var edgePlaneAB); Vector3Wide.CrossWithoutOverlap(triangleBC, triangleNormal, out var edgePlaneBC); Vector3Wide.CrossWithoutOverlap(triangleCA, triangleNormal, out var edgePlaneCA); Vector3Wide.Dot(edgePlaneAB, triangleA, out var abPlaneTest); Vector3Wide.Dot(edgePlaneBC, triangleB, out var bcPlaneTest); Vector3Wide.Dot(edgePlaneCA, triangleC, out var caPlaneTest); var hullInsideTriangleEdgePlanes = Vector.BitwiseAnd(Vector.LessThanOrEqual(abPlaneTest, Vector <float> .Zero), Vector.BitwiseAnd(Vector.LessThanOrEqual(bcPlaneTest, Vector <float> .Zero), Vector.LessThanOrEqual(caPlaneTest, Vector <float> .Zero))); var hullInsideAndBelowTriangle = Vector.BitwiseAnd(hullBelowPlane, hullInsideTriangleEdgePlanes); 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 = default; 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. var hullSupportFinder = default(ConvexHullSupportFinder); var triangleSupportFinder = default(PretransformedTriangleSupportFinder); //Sample the hull's extreme point along the triangle face normal- if it's contained within the triangle edge planes, we can avoid doing more expensive refinement. Vector3Wide.Negate(triangleNormal, out var negatedTriangleNormal); hullSupportFinder.ComputeLocalSupport(b, negatedTriangleNormal, inactiveLanes, out var hullSupportAlongNegatedTriangleNormal); Vector3Wide.Subtract(hullSupportAlongNegatedTriangleNormal, localTriangleCenter, out var supportAlongNegatedTriangleNormal); Vector3Wide.Dot(supportAlongNegatedTriangleNormal, negatedTriangleNormal, out var triangleFaceDepth); Vector3Wide.Subtract(triangleA, hullSupportAlongNegatedTriangleNormal, out var closestToA); Vector3Wide.Subtract(triangleB, hullSupportAlongNegatedTriangleNormal, out var closestToB); Vector3Wide.Subtract(triangleC, hullSupportAlongNegatedTriangleNormal, out var closestToC); Vector3Wide.Dot(edgePlaneAB, closestToA, out var extremeABPlaneTest); Vector3Wide.Dot(edgePlaneBC, closestToB, out var extremeBCPlaneTest); Vector3Wide.Dot(edgePlaneCA, closestToC, out var extremeCAPlaneTest); //Note that the triangle face extreme point can only be trusted if the hull's center is above the triangle's surface *AND* contained within the edge normals. //Merely being above the surface is insufficient- imagine a hull off to the side of the triangle, wedged beneath it. var triangleNormalIsMinimal = Vector.BitwiseAnd( Vector.BitwiseAnd( Vector.AndNot(hullInsideTriangleEdgePlanes, hullBelowPlane), 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; Vector <float> depth; if (Vector.EqualsAny(skipDepthRefine, Vector <int> .Zero)) { DepthRefiner.FindMinimumDepth( b, triangle, localTriangleCenter, hullLocalTriangleOrientation, ref hullSupportFinder, ref triangleSupportFinder, initialNormal, skipDepthRefine, 1e-4f * epsilonScale, depthThreshold, out var refinedDepth, out var refinedNormal, out var refinedClosestOnHull); Vector3Wide.ConditionalSelect(skipDepthRefine, hullSupportAlongNegatedTriangleNormal, refinedClosestOnHull, out closestOnHull); Vector3Wide.ConditionalSelect(skipDepthRefine, negatedTriangleNormal, refinedNormal, out localNormal); depth = Vector.ConditionalSelect(skipDepthRefine, triangleFaceDepth, refinedDepth); } else { //No depth refine ran; the extreme point prepass did everything we needed. Just use the initial normal. localNormal = negatedTriangleNormal; closestOnHull = hullSupportAlongNegatedTriangleNormal; depth = triangleFaceDepth; } 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 in right handed coordinates. //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);