Skip to content
Permalink
Branch: master
Find file Copy path
Find file Copy path
Fetching contributors…
Cannot retrieve contributors at this time
741 lines (645 sloc) 52.3 KB
//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
//This file is automatically generated by a text template. If you want to make modifications, do so in the .tt file.
//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
using BepuPhysics.Collidables;
using BepuPhysics.CollisionDetection.SweepTasks;
using BepuUtilities;
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Numerics;
using System.Runtime.CompilerServices;
using System.Text;
namespace BepuPhysics.CollisionDetection
{
public static class DepthRefiner<TShapeA, TShapeWideA, TSupportFinderA, TShapeB, TShapeWideB, TSupportFinderB>
where TShapeA : IConvexShape
where TShapeWideA : IShapeWide<TShapeA>
where TSupportFinderA : ISupportFinder<TShapeA, TShapeWideA>
where TShapeB : IConvexShape
where TShapeWideB : IShapeWide<TShapeB>
where TSupportFinderB : ISupportFinder<TShapeB, TShapeWideB>
{
struct HasNewSupport { }
struct HasNoNewSupport { }
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static void FindSupport(in TShapeWideA a, in TShapeWideB b, in Vector3Wide localOffsetB, in Matrix3x3Wide localOrientationB, ref TSupportFinderA supportFinderA, ref TSupportFinderB supportFinderB, in Vector3Wide direction,
in Vector<int> terminatedLanes, out Vector3Wide support)
{
//support(N, A) - support(-N, B)
supportFinderA.ComputeLocalSupport(a, direction, terminatedLanes, out var supportOnA);
Vector3Wide.Negate(direction, out var negatedDirection);
supportFinderB.ComputeSupport(b, localOrientationB, negatedDirection, terminatedLanes, out var extremeB);
Vector3Wide.Add(extremeB, localOffsetB, out extremeB);
Vector3Wide.Subtract(supportOnA, extremeB, out support);
}
public struct Vertex
{
public Vector3Wide Support;
public Vector<int> Exists;
}
public struct Simplex
{
public Vertex A;
public Vertex B;
public Vertex C;
}
[MethodImpl(MethodImplOptions.AggressiveInlining)]
static void FillSlot(ref Vertex vertex, in Vector3Wide support, in Vector<int> terminatedLanes)
{
//Note that this always fills empty slots. That's important- we avoid figuring out what subsimplex is active
//and instead just treat it as a degenerate simplex with some duplicates. (Shares code with the actual degenerate path.)
var dontFillSlot = Vector.BitwiseOr(vertex.Exists, terminatedLanes);
Vector3Wide.ConditionalSelect(dontFillSlot, vertex.Support, support, out vertex.Support);
vertex.Exists = Vector.ConditionalSelect(dontFillSlot, vertex.Exists, new Vector<int>(-1));
}
[MethodImpl(MethodImplOptions.AggressiveInlining)]
static void ForceFillSlot(in Vector<int> shouldFill, ref Vertex vertex, in Vector3Wide support)
{
vertex.Exists = Vector.BitwiseOr(vertex.Exists, shouldFill);
Vector3Wide.ConditionalSelect(shouldFill, support, vertex.Support, out vertex.Support);
}
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static void Create(in Vector3Wide normal, in Vector3Wide support, out Simplex simplex)
{
//While only one slot is actually full, GetNextNormal expects every slot to have some kind of data-
//for those slots which are not yet filled, it should be duplicates of other data.
//(The sub-triangle case is treated the same as the degenerate case.)
simplex.A.Support = support;
simplex.B.Support = support;
simplex.C.Support = support;
simplex.A.Exists = new Vector<int>(-1);
simplex.B.Exists = Vector<int>.Zero;
simplex.C.Exists = Vector<int>.Zero;
}
static void GetNextNormal<T>(ref Simplex simplex, in Vector3Wide support, ref Vector<int> terminatedLanes,
in Vector3Wide bestNormal, in Vector<float> bestDepth, in Vector<float> convergenceThreshold,
out Vector3Wide nextNormal)
{
//In the penetrating case, the search target is the closest point to the origin on the so-far-best bounding plane.
//In the separated case, it's just the origin itself.
//Termination conditions are based on the distance to the search target. In the penetrating case, we try to approach zero distance.
//The separated case makes use of the fact that the bestDepth and distance to closest point only converge when the offset and best normal align.
Vector3Wide.Scale(bestNormal, Vector.Max(Vector<float>.Zero, bestDepth), out var searchTarget);
var terminationEpsilon = Vector.ConditionalSelect(Vector.LessThan(bestDepth, Vector<float>.Zero), convergenceThreshold - bestDepth, convergenceThreshold);
var terminationEpsilonSquared = terminationEpsilon * terminationEpsilon;
if (typeof(T) == typeof(HasNewSupport))
{
var simplexFull = Vector.AndNot(Vector.BitwiseAnd(simplex.A.Exists, Vector.BitwiseAnd(simplex.B.Exists, simplex.C.Exists)), terminatedLanes);
//Fill any empty slots with the new support. Combines partial simplex case with degenerate simplex case.
FillSlot(ref simplex.A, support, terminatedLanes);
FillSlot(ref simplex.B, support, terminatedLanes);
FillSlot(ref simplex.C, support, terminatedLanes);
if (Vector.LessThanAny(simplexFull, Vector<int>.Zero))
{
//At least one active lane has a full simplex and an incoming new sample.
//Choose the subtriangle based on the edge plane tests of AD, BD, and CD, where D is the new support point.
Vector3Wide.Subtract(simplex.B.Support, simplex.A.Support, out var abEarly);
Vector3Wide.Subtract(simplex.A.Support, simplex.C.Support, out var caEarly);
Vector3Wide.Subtract(support, simplex.A.Support, out var ad);
Vector3Wide.Subtract(support, simplex.B.Support, out var bd);
Vector3Wide.Subtract(support, simplex.C.Support, out var cd);
Vector3Wide.CrossWithoutOverlap(abEarly, caEarly, out var triangleNormalEarly);
//(ad x n) * (d - searchTarget) = (n x (d - searchTarget)) * ad
Vector3Wide.Subtract(support, searchTarget, out var targetToSupport);
Vector3Wide.CrossWithoutOverlap(triangleNormalEarly, targetToSupport, out var nxOffset);
Vector3Wide.Dot(nxOffset, ad, out var adPlaneTest);
Vector3Wide.Dot(nxOffset, bd, out var bdPlaneTest);
Vector3Wide.Dot(nxOffset, cd, out var cdPlaneTest);
var useABD = Vector.BitwiseAnd(Vector.GreaterThanOrEqual(adPlaneTest, Vector<float>.Zero), Vector.LessThan(bdPlaneTest, Vector<float>.Zero));
var useBCD = Vector.BitwiseAnd(Vector.GreaterThanOrEqual(bdPlaneTest, Vector<float>.Zero), Vector.LessThan(cdPlaneTest, Vector<float>.Zero));
var useCAD = Vector.BitwiseAnd(Vector.GreaterThanOrEqual(cdPlaneTest, Vector<float>.Zero), Vector.LessThan(adPlaneTest, Vector<float>.Zero));
//Because the best normal may have changed due to the latest sample, ABC's portal may not contain the best normal anymore, which may mean
//that none of the subtriangles do either. This is fairly rare and the fallback heuristic doesn't matter much- this won't cause cycles because
//it can only occur on iterations where depth improvement has been made (and thus the best normal has changed).
//So we'll do something stupid and cheap!
useABD = Vector.ConditionalSelect(Vector.OnesComplement(Vector.BitwiseOr(Vector.BitwiseOr(useABD, useBCD), useCAD)), new Vector<int>(-1), useABD);
ForceFillSlot(Vector.BitwiseAnd(useBCD, simplexFull), ref simplex.A, support);
ForceFillSlot(Vector.BitwiseAnd(useCAD, simplexFull), ref simplex.B, support);
ForceFillSlot(Vector.BitwiseAnd(useABD, simplexFull), ref simplex.C, support);
}
}
else
{
FillSlot(ref simplex.A, simplex.A.Support, terminatedLanes);
FillSlot(ref simplex.B, simplex.A.Support, terminatedLanes);
FillSlot(ref simplex.C, simplex.A.Support, terminatedLanes);
}
Vector3Wide.Subtract(simplex.B.Support, simplex.A.Support, out var ab);
Vector3Wide.Subtract(simplex.A.Support, simplex.C.Support, out var ca);
Vector3Wide.Subtract(simplex.C.Support, simplex.B.Support, out var bc);
Vector3Wide.CrossWithoutOverlap(ab, ca, out var triangleNormal);
Vector3Wide.LengthSquared(triangleNormal, out var triangleNormalLengthSquared);
//Compute the plane sign tests. Note that these are barycentric weights that have not been scaled by the inverse triangle normal length squared;
//we do not have to compute the correct magnitude to know the sign, and the sign is all we care about.
Vector3Wide.Subtract(simplex.A.Support, searchTarget, out var targetToA);
Vector3Wide.Subtract(simplex.C.Support, searchTarget, out var targetToC);
Vector3Wide.CrossWithoutOverlap(ab, targetToA, out var abxta);
Vector3Wide.CrossWithoutOverlap(ca, targetToC, out var caxtc);
Vector3Wide.Dot(abxta, triangleNormal, out var abPlaneTest);
Vector3Wide.Dot(caxtc, triangleNormal, out var caPlaneTest);
var bcPlaneTest = triangleNormalLengthSquared - caPlaneTest - abPlaneTest;
var outsideAB = Vector.LessThan(abPlaneTest, Vector<float>.Zero);
var outsideBC = Vector.LessThan(bcPlaneTest, Vector<float>.Zero);
var outsideCA = Vector.LessThan(caPlaneTest, Vector<float>.Zero);
Vector3Wide.LengthSquared(ab, out var abLengthSquared);
Vector3Wide.LengthSquared(bc, out var bcLengthSquared);
Vector3Wide.LengthSquared(ca, out var caLengthSquared);
var longestEdgeLengthSquared = Vector.Max(Vector.Max(abLengthSquared, bcLengthSquared), caLengthSquared);
var simplexDegenerate = Vector.LessThanOrEqual(triangleNormalLengthSquared, longestEdgeLengthSquared * 1e-10f);
var degeneracyEpsilon = new Vector<float>(1e-14f);
var simplexIsAVertex = Vector.LessThan(longestEdgeLengthSquared, degeneracyEpsilon);
var simplexIsAnEdge = Vector.AndNot(simplexDegenerate, simplexIsAVertex);
Vector3Wide.Dot(triangleNormal, bestNormal, out var calibrationDot);
Vector3Wide.ConditionallyNegate(Vector.LessThan(calibrationDot, Vector<float>.Zero), ref triangleNormal);
var targetOutsideTriangleEdges = Vector.BitwiseOr(outsideAB, Vector.BitwiseOr(outsideBC, outsideCA));
//Compute the direction from the origin to the closest point on the triangle.
//If the simplex is degenerate and just a vertex, pick the first simplex entry as representative.
Vector3Wide.Negate(targetToA, out var triangleToTarget);
var relevantFeatures = Vector<int>.One;
//If this is a vertex case and the sample is right on top of the origin, immediately quit.
Vector3Wide.LengthSquared(targetToA, out var targetToALengthSquared);
terminatedLanes = Vector.BitwiseOr(terminatedLanes, Vector.BitwiseAnd(simplexIsAVertex, Vector.LessThan(targetToALengthSquared, terminationEpsilonSquared)));
var useEdge = Vector.AndNot(Vector.BitwiseOr(targetOutsideTriangleEdges, simplexIsAnEdge), terminatedLanes);
if (Vector.LessThanAny(useEdge, Vector<int>.Zero))
{
//Choose the edge that is closest to the search target. Note that we can compute the closest edge distance without performing a division:
//distance squared from ab to target o = ||a - o + ab * t||^2
//t = clamp(dot(ab, ao), 0, ||ab||^2) / ||ab||^2
//dot(oa, oa) + 2 * dot(oa, ab) * t + dot(ab, ab) * t^2
//dot(oa, oa) + 2 * dot(oa, ab) * clamp(dot(ab, ao)) / ||ab||^2 + dot(ab, ab) * t^2
//dot(oa, oa) + 2 * dot(oa, ab) * clamp(dot(ab, ao)) / ||ab||^2 + dot(ab, ab) * clamp(dot(ab, ao))^2 / ||ab||^4
//dot(oa, oa) + 2 * dot(oa, ab) * clamp(-dot(ab, oa)) / ||ab||^2 + clamp(-dot(ab, oa))^2 / ||ab||^2
//abDistanceSquared = (dot(oa, oa) * ||ab||^2 + 2 * dot(oa, ab) * clamp(-dot(ab, oa)) + clamp(-dot(ab, oa))^2) / ||ab||^2
//abDistanceSquaredNumerator / ||ab||^2 <= bcDistanceSquaredNumerator / ||bc||^2
//abDistanceSquaredNumerator * ||bc||^2 <= bcDistanceSquaredNumerator * ||ab||^2
Vector3Wide.Subtract(simplex.B.Support, searchTarget, out var targetToB);
Vector3Wide.LengthSquared(targetToC, out var targetToCLengthSquared);
Vector3Wide.LengthSquared(targetToB, out var targetToBLengthSquared);
Vector3Wide.Dot(targetToA, ab, out var oaDotAB);
Vector3Wide.Dot(targetToB, bc, out var obDotBC);
Vector3Wide.Dot(targetToC, ca, out var ocDotCA);
var abScaledT = Vector.Max(Vector<float>.Zero, Vector.Min(abLengthSquared, -oaDotAB));
var bcScaledT = Vector.Max(Vector<float>.Zero, Vector.Min(bcLengthSquared, -obDotBC));
var caScaledT = Vector.Max(Vector<float>.Zero, Vector.Min(caLengthSquared, -ocDotCA));
var two = new Vector<float>(2);
var abScaledDistanceSquared = targetToALengthSquared * abLengthSquared + two * oaDotAB * abScaledT + abScaledT * abScaledT;
var bcScaledDistanceSquared = targetToBLengthSquared * bcLengthSquared + two * obDotBC * bcScaledT + bcScaledT * bcScaledT;
var caScaledDistanceSquared = targetToCLengthSquared * caLengthSquared + two * ocDotCA * caScaledT + caScaledT * caScaledT;
var bcDegenerate = Vector.Equals(bcLengthSquared, Vector<float>.Zero);
var caDegenerate = Vector.Equals(caLengthSquared, Vector<float>.Zero);
var abCloserThanBC = Vector.BitwiseOr(bcDegenerate, Vector.LessThan(abScaledDistanceSquared * bcLengthSquared, bcScaledDistanceSquared * abLengthSquared));
var abCloserThanCA = Vector.BitwiseOr(caDegenerate, Vector.LessThan(abScaledDistanceSquared * caLengthSquared, caScaledDistanceSquared * abLengthSquared));
var bcCloserThanCA = Vector.BitwiseOr(caDegenerate, Vector.LessThan(bcScaledDistanceSquared * caLengthSquared, caScaledDistanceSquared * bcLengthSquared));
var useAB = Vector.BitwiseAnd(abCloserThanBC, abCloserThanCA);
var useBC = Vector.AndNot(bcCloserThanCA, useAB);
var bestScaledDistanceSquared = Vector.ConditionalSelect(useAB, abScaledDistanceSquared, Vector.ConditionalSelect(useBC, bcScaledDistanceSquared, caScaledDistanceSquared));
var edgeLengthSquared = Vector.ConditionalSelect(useAB, abLengthSquared, Vector.ConditionalSelect(useBC, bcLengthSquared, caLengthSquared));
//If the search target is on the edge, we can immediately quit.
terminatedLanes = Vector.BitwiseOr(terminatedLanes, Vector.BitwiseAnd(useEdge, Vector.LessThanOrEqual(bestScaledDistanceSquared, terminationEpsilonSquared * edgeLengthSquared)));
//TODO: Wrapping this in a condition under the assumption that we just terminated is a little iffy. Measure.
if (Vector.LessThanAny(Vector.AndNot(useEdge, terminatedLanes), Vector<int>.Zero))
{
var t = Vector.ConditionalSelect(useAB, abScaledT, Vector.ConditionalSelect(useBC, bcScaledT, caScaledT)) / edgeLengthSquared;
Vector3Wide.ConditionalSelect(useAB, ab, ca, out var edgeOffset);
Vector3Wide.ConditionalSelect(useAB, targetToA, targetToC, out var edgeStart);
Vector3Wide.ConditionalSelect(useBC, bc, edgeOffset, out edgeOffset);
Vector3Wide.ConditionalSelect(useBC, targetToB, edgeStart, out edgeStart);
Vector3Wide.Scale(edgeOffset, -t, out var scaledOffset);
Vector3Wide.Subtract(scaledOffset, edgeStart, out var triangleToTargetCandidate);
//Vector3Wide.Subtract(searchTarget, nearestPointOnEdge, out var triangleToTargetCandidate);
var originNearestStart = Vector.Equals(t, Vector<float>.Zero);
var originNearestEnd = Vector.Equals(t, Vector<float>.One);
var featureForAB = Vector.ConditionalSelect(originNearestStart, Vector<int>.One, Vector.ConditionalSelect(originNearestEnd, new Vector<int>(2), new Vector<int>(1 + 2)));
var featureForBC = Vector.ConditionalSelect(originNearestStart, new Vector<int>(2), Vector.ConditionalSelect(originNearestEnd, new Vector<int>(4), new Vector<int>(2 + 4)));
var featureForCA = Vector.ConditionalSelect(originNearestStart, new Vector<int>(4), Vector.ConditionalSelect(originNearestEnd, Vector<int>.One, new Vector<int>(4 + 1)));
relevantFeatures = Vector.ConditionalSelect(useEdge, Vector.ConditionalSelect(useAB, featureForAB, Vector.ConditionalSelect(useBC, featureForBC, featureForCA)), relevantFeatures);
Vector3Wide.ConditionalSelect(useEdge, triangleToTargetCandidate, triangleToTarget, out triangleToTarget);
}
}
//We've examined the vertex and edge case, now we need to check the triangle face case.
var targetContainedInEdgePlanes = Vector.AndNot(Vector.AndNot(Vector.OnesComplement(targetOutsideTriangleEdges), simplexDegenerate), terminatedLanes);
if (Vector.LessThanAny(targetContainedInEdgePlanes, Vector<int>.Zero))
{
//At least one lane needs a face test.
//Note that we don't actually need to compute the closest point here- we can just use the triangleNormal.
//We do need to calculate the distance from the closest point to the search target, but that's just:
//||searchTarget-closestOnTriangle||^2 = ||(dot(n/||n||, searchTarget - a) * n/||n||)||^2
//||(dot(n, searchTarget - a) / ||n||^2) * n||^2
//((dot(n, searchTarget - a) / ||n||^2))^2 * ||n||^2
//((dot(n, searchTarget - a)^2 / ||n||^4)) * ||n||^2
//dot(n, searchTarget - a)^2 / ||n||^2
//Then the comparison can performed with a multiplication rather than a division.
Vector3Wide.Dot(targetToA, triangleNormal, out var targetToADot);
var targetOnTriangleSurface = Vector.LessThan(targetToADot * targetToADot, terminationEpsilonSquared * triangleNormalLengthSquared);
terminatedLanes = Vector.BitwiseOr(Vector.BitwiseAnd(targetContainedInEdgePlanes, targetOnTriangleSurface), terminatedLanes);
Vector3Wide.ConditionalSelect(targetContainedInEdgePlanes, triangleNormal, triangleToTarget, out triangleToTarget);
relevantFeatures = Vector.ConditionalSelect(targetContainedInEdgePlanes, new Vector<int>(1 + 2 + 4), relevantFeatures);
}
simplex.A.Exists = Vector.GreaterThan(Vector.BitwiseAnd(relevantFeatures, Vector<int>.One), Vector<int>.Zero);
simplex.B.Exists = Vector.GreaterThan(Vector.BitwiseAnd(relevantFeatures, new Vector<int>(2)), Vector<int>.Zero);
simplex.C.Exists = Vector.GreaterThan(Vector.BitwiseAnd(relevantFeatures, new Vector<int>(4)), Vector<int>.Zero);
if (Vector.EqualsAny(terminatedLanes, Vector<int>.Zero))
{
//In fairly rare cases near penetrating convergence, it's possible for the triangle->target offset to point nearly 90 degrees away from the previous best.
//This doesn't break convergence, but it can slow it down. To avoid it, use the offset to tilt the normal rather than using the offset directly.
Vector3Wide.Scale(triangleToTarget, new Vector<float>(4f), out var pushOffset);
Vector3Wide.Add(searchTarget, pushOffset, out var pushNormalCandidate);
Vector3Wide.ConditionalSelect(Vector.BitwiseOr(Vector.LessThanOrEqual(bestDepth, Vector<float>.Zero), targetContainedInEdgePlanes), triangleToTarget, pushNormalCandidate, out triangleToTarget);
//No active lanes can have a zero length targetToTriangle, so we can normalize safely.
Vector3Wide.LengthSquared(triangleToTarget, out var lengthSquared);
Vector3Wide.Scale(triangleToTarget, Vector<float>.One / Vector.SquareRoot(lengthSquared), out nextNormal);
}
}
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static void FindMinimumDepth(in TShapeWideA a, in TShapeWideB b, in Vector3Wide localOffsetB, in Matrix3x3Wide localOrientationB, ref TSupportFinderA supportFinderA, ref TSupportFinderB supportFinderB,
in Vector3Wide initialNormal, in Vector<int> inactiveLanes, in Vector<float> searchEpsilon, in Vector<float> minimumDepthThreshold,
out Vector<float> depth, out Vector3Wide refinedNormal, int maximumIterations = 25)
{
#if DEBUG
Vector3Wide.LengthSquared(initialNormal, out var initialNormalLengthSquared);
Debug.Assert(Vector.LessThanAll(Vector.BitwiseOr(inactiveLanes, Vector.LessThan(Vector.Abs(initialNormalLengthSquared - Vector<float>.One), new Vector<float>(1e-6f))), Vector<int>.Zero));
#endif
FindSupport(a, b, localOffsetB, localOrientationB, ref supportFinderA, ref supportFinderB, initialNormal, inactiveLanes, out var initialSupport);
Vector3Wide.Dot(initialSupport, initialNormal, out var initialDepth);
Create(initialNormal, initialSupport, out Simplex simplex);
FindMinimumDepth(a, b, localOffsetB, localOrientationB, ref supportFinderA, ref supportFinderB, ref simplex, initialNormal, initialDepth, inactiveLanes, searchEpsilon, minimumDepthThreshold, out depth, out refinedNormal, maximumIterations);
}
public static void FindMinimumDepth(in TShapeWideA a, in TShapeWideB b, in Vector3Wide localOffsetB, in Matrix3x3Wide localOrientationB, ref TSupportFinderA supportFinderA, ref TSupportFinderB supportFinderB,
ref Simplex simplex, in Vector3Wide initialNormal, in Vector<float> initialDepth,
in Vector<int> inactiveLanes, in Vector<float> convergenceThreshold, in Vector<float> minimumDepthThreshold,
out Vector<float> refinedDepth, out Vector3Wide refinedNormal, int maximumIterations = 50)
{
Vector<float> depthThreshold = minimumDepthThreshold;
if (supportFinderA.HasMargin)
{
supportFinderA.GetMargin(a, out var margin);
depthThreshold -= margin;
}
if (supportFinderB.HasMargin)
{
supportFinderB.GetMargin(b, out var margin);
depthThreshold -= margin;
}
var depthBelowThreshold = Vector.LessThan(initialDepth, depthThreshold);
var terminatedLanes = Vector.BitwiseOr(depthBelowThreshold, inactiveLanes);
refinedNormal = initialNormal;
refinedDepth = initialDepth;
if (Vector.LessThanAll(terminatedLanes, Vector<int>.Zero))
{
return;
}
GetNextNormal<HasNoNewSupport>(ref simplex, default, ref terminatedLanes, refinedNormal, refinedDepth, convergenceThreshold, out var normal);
for (int i = 0; i < maximumIterations; ++i)
{
if (Vector.LessThanAll(terminatedLanes, Vector<int>.Zero))
break;
FindSupport(a, b, localOffsetB, localOrientationB, ref supportFinderA, ref supportFinderB, normal, terminatedLanes, out var support);
Vector3Wide.Dot(support, normal, out var depth);
var useNewDepth = Vector.AndNot(Vector.LessThan(depth, refinedDepth), terminatedLanes);
refinedDepth = Vector.ConditionalSelect(useNewDepth, depth, refinedDepth);
Vector3Wide.ConditionalSelect(useNewDepth, normal, refinedNormal, out refinedNormal);
terminatedLanes = Vector.BitwiseOr(Vector.LessThanOrEqual(refinedDepth, depthThreshold), terminatedLanes);
if (Vector.LessThanAll(terminatedLanes, Vector<int>.Zero))
break;
GetNextNormal<HasNewSupport>(ref simplex, support, ref terminatedLanes, refinedNormal, refinedDepth, convergenceThreshold, out normal);
}
if (supportFinderA.HasMargin)
{
supportFinderA.GetMargin(a, out var margin);
refinedDepth += margin;
}
if (supportFinderB.HasMargin)
{
supportFinderB.GetMargin(b, out var margin);
refinedDepth += margin;
}
}
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static void FindSupport(in TShapeWideA a, in TShapeWideB b, in Vector3Wide localOffsetB, in Matrix3x3Wide localOrientationB, ref TSupportFinderA supportFinderA, ref TSupportFinderB supportFinderB, in Vector3Wide direction,
in Vector<int> terminatedLanes, out Vector3Wide support, out Vector3Wide supportOnA)
{
//support(N, A) - support(-N, B)
supportFinderA.ComputeLocalSupport(a, direction, terminatedLanes, out supportOnA);
Vector3Wide.Negate(direction, out var negatedDirection);
supportFinderB.ComputeSupport(b, localOrientationB, negatedDirection, terminatedLanes, out var extremeB);
Vector3Wide.Add(extremeB, localOffsetB, out extremeB);
Vector3Wide.Subtract(supportOnA, extremeB, out support);
}
public struct VertexWithWitness
{
public Vector3Wide Support;
public Vector3Wide SupportOnA;
public Vector<float> Weight;
public Vector<int> Exists;
}
public struct SimplexWithWitness
{
public VertexWithWitness A;
public VertexWithWitness B;
public VertexWithWitness C;
public Vector<float> WeightDenominator;
}
[MethodImpl(MethodImplOptions.AggressiveInlining)]
static void FillSlot(ref VertexWithWitness vertex, in Vector3Wide support, in Vector3Wide supportOnA, in Vector<int> terminatedLanes)
{
//Note that this always fills empty slots. That's important- we avoid figuring out what subsimplex is active
//and instead just treat it as a degenerate simplex with some duplicates. (Shares code with the actual degenerate path.)
var dontFillSlot = Vector.BitwiseOr(vertex.Exists, terminatedLanes);
Vector3Wide.ConditionalSelect(dontFillSlot, vertex.Support, support, out vertex.Support);
Vector3Wide.ConditionalSelect(dontFillSlot, vertex.SupportOnA, supportOnA, out vertex.SupportOnA);
vertex.Exists = Vector.ConditionalSelect(dontFillSlot, vertex.Exists, new Vector<int>(-1));
}
[MethodImpl(MethodImplOptions.AggressiveInlining)]
static void ForceFillSlot(in Vector<int> shouldFill, ref VertexWithWitness vertex, in Vector3Wide support, in Vector3Wide supportOnA)
{
vertex.Exists = Vector.BitwiseOr(vertex.Exists, shouldFill);
Vector3Wide.ConditionalSelect(shouldFill, support, vertex.Support, out vertex.Support);
Vector3Wide.ConditionalSelect(shouldFill, supportOnA, vertex.SupportOnA, out vertex.SupportOnA);
}
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static void Create(in Vector3Wide normal, in Vector3Wide support, in Vector3Wide supportOnA, out SimplexWithWitness simplex)
{
//While only one slot is actually full, GetNextNormal expects every slot to have some kind of data-
//for those slots which are not yet filled, it should be duplicates of other data.
//(The sub-triangle case is treated the same as the degenerate case.)
simplex.A.Support = support;
simplex.B.Support = support;
simplex.C.Support = support;
simplex.A.SupportOnA = supportOnA;
simplex.B.SupportOnA = supportOnA;
simplex.C.SupportOnA = supportOnA;
simplex.A.Exists = new Vector<int>(-1);
simplex.B.Exists = Vector<int>.Zero;
simplex.C.Exists = Vector<int>.Zero;
}
static void GetNextNormal<T>(ref SimplexWithWitness simplex, in Vector3Wide support, in Vector3Wide supportOnA, ref Vector<int> terminatedLanes,
in Vector3Wide bestNormal, in Vector<float> bestDepth, in Vector<float> convergenceThreshold,
out Vector3Wide nextNormal)
{
//In the penetrating case, the search target is the closest point to the origin on the so-far-best bounding plane.
//In the separated case, it's just the origin itself.
//Termination conditions are based on the distance to the search target. In the penetrating case, we try to approach zero distance.
//The separated case makes use of the fact that the bestDepth and distance to closest point only converge when the offset and best normal align.
Vector3Wide.Scale(bestNormal, Vector.Max(Vector<float>.Zero, bestDepth), out var searchTarget);
var terminationEpsilon = Vector.ConditionalSelect(Vector.LessThan(bestDepth, Vector<float>.Zero), convergenceThreshold - bestDepth, convergenceThreshold);
var terminationEpsilonSquared = terminationEpsilon * terminationEpsilon;
if (typeof(T) == typeof(HasNewSupport))
{
var simplexFull = Vector.AndNot(Vector.BitwiseAnd(simplex.A.Exists, Vector.BitwiseAnd(simplex.B.Exists, simplex.C.Exists)), terminatedLanes);
//Fill any empty slots with the new support. Combines partial simplex case with degenerate simplex case.
FillSlot(ref simplex.A, support, supportOnA, terminatedLanes);
FillSlot(ref simplex.B, support, supportOnA, terminatedLanes);
FillSlot(ref simplex.C, support, supportOnA, terminatedLanes);
if (Vector.LessThanAny(simplexFull, Vector<int>.Zero))
{
//At least one active lane has a full simplex and an incoming new sample.
//Choose the subtriangle based on the edge plane tests of AD, BD, and CD, where D is the new support point.
Vector3Wide.Subtract(simplex.B.Support, simplex.A.Support, out var abEarly);
Vector3Wide.Subtract(simplex.A.Support, simplex.C.Support, out var caEarly);
Vector3Wide.Subtract(support, simplex.A.Support, out var ad);
Vector3Wide.Subtract(support, simplex.B.Support, out var bd);
Vector3Wide.Subtract(support, simplex.C.Support, out var cd);
Vector3Wide.CrossWithoutOverlap(abEarly, caEarly, out var triangleNormalEarly);
//(ad x n) * (d - searchTarget) = (n x (d - searchTarget)) * ad
Vector3Wide.Subtract(support, searchTarget, out var targetToSupport);
Vector3Wide.CrossWithoutOverlap(triangleNormalEarly, targetToSupport, out var nxOffset);
Vector3Wide.Dot(nxOffset, ad, out var adPlaneTest);
Vector3Wide.Dot(nxOffset, bd, out var bdPlaneTest);
Vector3Wide.Dot(nxOffset, cd, out var cdPlaneTest);
var useABD = Vector.BitwiseAnd(Vector.GreaterThanOrEqual(adPlaneTest, Vector<float>.Zero), Vector.LessThan(bdPlaneTest, Vector<float>.Zero));
var useBCD = Vector.BitwiseAnd(Vector.GreaterThanOrEqual(bdPlaneTest, Vector<float>.Zero), Vector.LessThan(cdPlaneTest, Vector<float>.Zero));
var useCAD = Vector.BitwiseAnd(Vector.GreaterThanOrEqual(cdPlaneTest, Vector<float>.Zero), Vector.LessThan(adPlaneTest, Vector<float>.Zero));
//Because the best normal may have changed due to the latest sample, ABC's portal may not contain the best normal anymore, which may mean
//that none of the subtriangles do either. This is fairly rare and the fallback heuristic doesn't matter much- this won't cause cycles because
//it can only occur on iterations where depth improvement has been made (and thus the best normal has changed).
//So we'll do something stupid and cheap!
useABD = Vector.ConditionalSelect(Vector.OnesComplement(Vector.BitwiseOr(Vector.BitwiseOr(useABD, useBCD), useCAD)), new Vector<int>(-1), useABD);
ForceFillSlot(Vector.BitwiseAnd(useBCD, simplexFull), ref simplex.A, support, supportOnA);
ForceFillSlot(Vector.BitwiseAnd(useCAD, simplexFull), ref simplex.B, support, supportOnA);
ForceFillSlot(Vector.BitwiseAnd(useABD, simplexFull), ref simplex.C, support, supportOnA);
}
}
else
{
FillSlot(ref simplex.A, simplex.A.Support, simplex.A.SupportOnA, terminatedLanes);
FillSlot(ref simplex.B, simplex.A.Support, simplex.A.SupportOnA, terminatedLanes);
FillSlot(ref simplex.C, simplex.A.Support, simplex.A.SupportOnA, terminatedLanes);
}
Vector3Wide.Subtract(simplex.B.Support, simplex.A.Support, out var ab);
Vector3Wide.Subtract(simplex.A.Support, simplex.C.Support, out var ca);
Vector3Wide.Subtract(simplex.C.Support, simplex.B.Support, out var bc);
Vector3Wide.CrossWithoutOverlap(ab, ca, out var triangleNormal);
Vector3Wide.LengthSquared(triangleNormal, out var triangleNormalLengthSquared);
//Compute the plane sign tests. Note that these are barycentric weights that have not been scaled by the inverse triangle normal length squared;
//we do not have to compute the correct magnitude to know the sign, and the sign is all we care about.
Vector3Wide.Subtract(simplex.A.Support, searchTarget, out var targetToA);
Vector3Wide.Subtract(simplex.C.Support, searchTarget, out var targetToC);
Vector3Wide.CrossWithoutOverlap(ab, targetToA, out var abxta);
Vector3Wide.CrossWithoutOverlap(ca, targetToC, out var caxtc);
Vector3Wide.Dot(abxta, triangleNormal, out var abPlaneTest);
Vector3Wide.Dot(caxtc, triangleNormal, out var caPlaneTest);
var bcPlaneTest = triangleNormalLengthSquared - caPlaneTest - abPlaneTest;
var outsideAB = Vector.LessThan(abPlaneTest, Vector<float>.Zero);
var outsideBC = Vector.LessThan(bcPlaneTest, Vector<float>.Zero);
var outsideCA = Vector.LessThan(caPlaneTest, Vector<float>.Zero);
Vector3Wide.LengthSquared(ab, out var abLengthSquared);
Vector3Wide.LengthSquared(bc, out var bcLengthSquared);
Vector3Wide.LengthSquared(ca, out var caLengthSquared);
var longestEdgeLengthSquared = Vector.Max(Vector.Max(abLengthSquared, bcLengthSquared), caLengthSquared);
var simplexDegenerate = Vector.LessThanOrEqual(triangleNormalLengthSquared, longestEdgeLengthSquared * 1e-10f);
var degeneracyEpsilon = new Vector<float>(1e-14f);
var simplexIsAVertex = Vector.LessThan(longestEdgeLengthSquared, degeneracyEpsilon);
var simplexIsAnEdge = Vector.AndNot(simplexDegenerate, simplexIsAVertex);
Vector3Wide.Dot(triangleNormal, bestNormal, out var calibrationDot);
Vector3Wide.ConditionallyNegate(Vector.LessThan(calibrationDot, Vector<float>.Zero), ref triangleNormal);
var targetOutsideTriangleEdges = Vector.BitwiseOr(outsideAB, Vector.BitwiseOr(outsideBC, outsideCA));
//Compute the direction from the origin to the closest point on the triangle.
//If the simplex is degenerate and just a vertex, pick the first simplex entry as representative.
Vector3Wide.Negate(targetToA, out var triangleToTarget);
var relevantFeatures = Vector<int>.One;
simplex.A.Weight = Vector.ConditionalSelect(terminatedLanes, simplex.A.Weight, Vector<float>.One);
simplex.B.Weight = Vector.ConditionalSelect(terminatedLanes, simplex.B.Weight, Vector<float>.Zero);
simplex.C.Weight = Vector.ConditionalSelect(terminatedLanes, simplex.C.Weight, Vector<float>.Zero);
simplex.WeightDenominator = Vector.ConditionalSelect(terminatedLanes, simplex.WeightDenominator, Vector<float>.One);
//If this is a vertex case and the sample is right on top of the origin, immediately quit.
Vector3Wide.LengthSquared(targetToA, out var targetToALengthSquared);
terminatedLanes = Vector.BitwiseOr(terminatedLanes, Vector.BitwiseAnd(simplexIsAVertex, Vector.LessThan(targetToALengthSquared, terminationEpsilonSquared)));
var useEdge = Vector.AndNot(Vector.BitwiseOr(targetOutsideTriangleEdges, simplexIsAnEdge), terminatedLanes);
if (Vector.LessThanAny(useEdge, Vector<int>.Zero))
{
//Choose the edge that is closest to the search target. Note that we can compute the closest edge distance without performing a division:
//distance squared from ab to target o = ||a - o + ab * t||^2
//t = clamp(dot(ab, ao), 0, ||ab||^2) / ||ab||^2
//dot(oa, oa) + 2 * dot(oa, ab) * t + dot(ab, ab) * t^2
//dot(oa, oa) + 2 * dot(oa, ab) * clamp(dot(ab, ao)) / ||ab||^2 + dot(ab, ab) * t^2
//dot(oa, oa) + 2 * dot(oa, ab) * clamp(dot(ab, ao)) / ||ab||^2 + dot(ab, ab) * clamp(dot(ab, ao))^2 / ||ab||^4
//dot(oa, oa) + 2 * dot(oa, ab) * clamp(-dot(ab, oa)) / ||ab||^2 + clamp(-dot(ab, oa))^2 / ||ab||^2
//abDistanceSquared = (dot(oa, oa) * ||ab||^2 + 2 * dot(oa, ab) * clamp(-dot(ab, oa)) + clamp(-dot(ab, oa))^2) / ||ab||^2
//abDistanceSquaredNumerator / ||ab||^2 <= bcDistanceSquaredNumerator / ||bc||^2
//abDistanceSquaredNumerator * ||bc||^2 <= bcDistanceSquaredNumerator * ||ab||^2
Vector3Wide.Subtract(simplex.B.Support, searchTarget, out var targetToB);
Vector3Wide.LengthSquared(targetToC, out var targetToCLengthSquared);
Vector3Wide.LengthSquared(targetToB, out var targetToBLengthSquared);
Vector3Wide.Dot(targetToA, ab, out var oaDotAB);
Vector3Wide.Dot(targetToB, bc, out var obDotBC);
Vector3Wide.Dot(targetToC, ca, out var ocDotCA);
var abScaledT = Vector.Max(Vector<float>.Zero, Vector.Min(abLengthSquared, -oaDotAB));
var bcScaledT = Vector.Max(Vector<float>.Zero, Vector.Min(bcLengthSquared, -obDotBC));
var caScaledT = Vector.Max(Vector<float>.Zero, Vector.Min(caLengthSquared, -ocDotCA));
var two = new Vector<float>(2);
var abScaledDistanceSquared = targetToALengthSquared * abLengthSquared + two * oaDotAB * abScaledT + abScaledT * abScaledT;
var bcScaledDistanceSquared = targetToBLengthSquared * bcLengthSquared + two * obDotBC * bcScaledT + bcScaledT * bcScaledT;
var caScaledDistanceSquared = targetToCLengthSquared * caLengthSquared + two * ocDotCA * caScaledT + caScaledT * caScaledT;
var bcDegenerate = Vector.Equals(bcLengthSquared, Vector<float>.Zero);
var caDegenerate = Vector.Equals(caLengthSquared, Vector<float>.Zero);
var abCloserThanBC = Vector.BitwiseOr(bcDegenerate, Vector.LessThan(abScaledDistanceSquared * bcLengthSquared, bcScaledDistanceSquared * abLengthSquared));
var abCloserThanCA = Vector.BitwiseOr(caDegenerate, Vector.LessThan(abScaledDistanceSquared * caLengthSquared, caScaledDistanceSquared * abLengthSquared));
var bcCloserThanCA = Vector.BitwiseOr(caDegenerate, Vector.LessThan(bcScaledDistanceSquared * caLengthSquared, caScaledDistanceSquared * bcLengthSquared));
var useAB = Vector.BitwiseAnd(abCloserThanBC, abCloserThanCA);
var useBC = Vector.AndNot(bcCloserThanCA, useAB);
var bestScaledDistanceSquared = Vector.ConditionalSelect(useAB, abScaledDistanceSquared, Vector.ConditionalSelect(useBC, bcScaledDistanceSquared, caScaledDistanceSquared));
var edgeLengthSquared = Vector.ConditionalSelect(useAB, abLengthSquared, Vector.ConditionalSelect(useBC, bcLengthSquared, caLengthSquared));
//If the search target is on the edge, we can immediately quit.
terminatedLanes = Vector.BitwiseOr(terminatedLanes, Vector.BitwiseAnd(useEdge, Vector.LessThanOrEqual(bestScaledDistanceSquared, terminationEpsilonSquared * edgeLengthSquared)));
{
var t = Vector.ConditionalSelect(useAB, abScaledT, Vector.ConditionalSelect(useBC, bcScaledT, caScaledT)) / edgeLengthSquared;
Vector3Wide.ConditionalSelect(useAB, ab, ca, out var edgeOffset);
Vector3Wide.ConditionalSelect(useAB, targetToA, targetToC, out var edgeStart);
Vector3Wide.ConditionalSelect(useBC, bc, edgeOffset, out edgeOffset);
Vector3Wide.ConditionalSelect(useBC, targetToB, edgeStart, out edgeStart);
Vector3Wide.Scale(edgeOffset, -t, out var scaledOffset);
Vector3Wide.Subtract(scaledOffset, edgeStart, out var triangleToTargetCandidate);
//Vector3Wide.Subtract(searchTarget, nearestPointOnEdge, out var triangleToTargetCandidate);
var originNearestStart = Vector.Equals(t, Vector<float>.Zero);
var originNearestEnd = Vector.Equals(t, Vector<float>.One);
var featureForAB = Vector.ConditionalSelect(originNearestStart, Vector<int>.One, Vector.ConditionalSelect(originNearestEnd, new Vector<int>(2), new Vector<int>(1 + 2)));
var featureForBC = Vector.ConditionalSelect(originNearestStart, new Vector<int>(2), Vector.ConditionalSelect(originNearestEnd, new Vector<int>(4), new Vector<int>(2 + 4)));
var featureForCA = Vector.ConditionalSelect(originNearestStart, new Vector<int>(4), Vector.ConditionalSelect(originNearestEnd, Vector<int>.One, new Vector<int>(4 + 1)));
relevantFeatures = Vector.ConditionalSelect(useEdge, Vector.ConditionalSelect(useAB, featureForAB, Vector.ConditionalSelect(useBC, featureForBC, featureForCA)), relevantFeatures);
Vector3Wide.ConditionalSelect(useEdge, triangleToTargetCandidate, triangleToTarget, out triangleToTarget);
var weightEdgeStart = Vector<float>.One - t;
simplex.A.Weight = Vector.ConditionalSelect(useEdge, Vector.ConditionalSelect(useAB, weightEdgeStart, Vector.ConditionalSelect(useBC, Vector<float>.Zero, t)), simplex.A.Weight);
simplex.B.Weight = Vector.ConditionalSelect(useEdge, Vector.ConditionalSelect(useAB, t, Vector.ConditionalSelect(useBC, weightEdgeStart, Vector<float>.Zero)), simplex.B.Weight);
simplex.C.Weight = Vector.ConditionalSelect(useEdge, Vector.ConditionalSelect(useAB, Vector<float>.Zero, Vector.ConditionalSelect(useBC, t, weightEdgeStart)), simplex.C.Weight);
//Weight denominator is still just one, as it is in the vertex case.
}
}
//We've examined the vertex and edge case, now we need to check the triangle face case.
var targetContainedInEdgePlanes = Vector.AndNot(Vector.AndNot(Vector.OnesComplement(targetOutsideTriangleEdges), simplexDegenerate), terminatedLanes);
if (Vector.LessThanAny(targetContainedInEdgePlanes, Vector<int>.Zero))
{
//At least one lane needs a face test.
//Note that we don't actually need to compute the closest point here- we can just use the triangleNormal.
//We do need to calculate the distance from the closest point to the search target, but that's just:
//||searchTarget-closestOnTriangle||^2 = ||(dot(n/||n||, searchTarget - a) * n/||n||)||^2
//||(dot(n, searchTarget - a) / ||n||^2) * n||^2
//((dot(n, searchTarget - a) / ||n||^2))^2 * ||n||^2
//((dot(n, searchTarget - a)^2 / ||n||^4)) * ||n||^2
//dot(n, searchTarget - a)^2 / ||n||^2
//Then the comparison can performed with a multiplication rather than a division.
Vector3Wide.Dot(targetToA, triangleNormal, out var targetToADot);
var targetOnTriangleSurface = Vector.LessThan(targetToADot * targetToADot, terminationEpsilonSquared * triangleNormalLengthSquared);
terminatedLanes = Vector.BitwiseOr(Vector.BitwiseAnd(targetContainedInEdgePlanes, targetOnTriangleSurface), terminatedLanes);
Vector3Wide.ConditionalSelect(targetContainedInEdgePlanes, triangleNormal, triangleToTarget, out triangleToTarget);
relevantFeatures = Vector.ConditionalSelect(targetContainedInEdgePlanes, new Vector<int>(1 + 2 + 4), relevantFeatures);
simplex.A.Weight = Vector.ConditionalSelect(targetContainedInEdgePlanes, bcPlaneTest, simplex.A.Weight);
simplex.B.Weight = Vector.ConditionalSelect(targetContainedInEdgePlanes, caPlaneTest, simplex.B.Weight);
simplex.C.Weight = Vector.ConditionalSelect(targetContainedInEdgePlanes, abPlaneTest, simplex.C.Weight);
simplex.WeightDenominator = Vector.ConditionalSelect(targetContainedInEdgePlanes, triangleNormalLengthSquared, simplex.WeightDenominator);
}
simplex.A.Exists = Vector.GreaterThan(Vector.BitwiseAnd(relevantFeatures, Vector<int>.One), Vector<int>.Zero);
simplex.B.Exists = Vector.GreaterThan(Vector.BitwiseAnd(relevantFeatures, new Vector<int>(2)), Vector<int>.Zero);
simplex.C.Exists = Vector.GreaterThan(Vector.BitwiseAnd(relevantFeatures, new Vector<int>(4)), Vector<int>.Zero);
if (Vector.EqualsAny(terminatedLanes, Vector<int>.Zero))
{
//In fairly rare cases near penetrating convergence, it's possible for the triangle->target offset to point nearly 90 degrees away from the previous best.
//This doesn't break convergence, but it can slow it down. To avoid it, use the offset to tilt the normal rather than using the offset directly.
Vector3Wide.Scale(triangleToTarget, new Vector<float>(4f), out var pushOffset);
Vector3Wide.Add(searchTarget, pushOffset, out var pushNormalCandidate);
Vector3Wide.ConditionalSelect(Vector.BitwiseOr(Vector.LessThanOrEqual(bestDepth, Vector<float>.Zero), targetContainedInEdgePlanes), triangleToTarget, pushNormalCandidate, out triangleToTarget);
//No active lanes can have a zero length targetToTriangle, so we can normalize safely.
Vector3Wide.LengthSquared(triangleToTarget, out var lengthSquared);
Vector3Wide.Scale(triangleToTarget, Vector<float>.One / Vector.SquareRoot(lengthSquared), out nextNormal);
}
}
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static void FindMinimumDepth(in TShapeWideA a, in TShapeWideB b, in Vector3Wide localOffsetB, in Matrix3x3Wide localOrientationB, ref TSupportFinderA supportFinderA, ref TSupportFinderB supportFinderB,
in Vector3Wide initialNormal, in Vector<int> inactiveLanes, in Vector<float> searchEpsilon, in Vector<float> minimumDepthThreshold,
out Vector<float> depth, out Vector3Wide refinedNormal, out Vector3Wide witnessOnA, int maximumIterations = 25)
{
#if DEBUG
Vector3Wide.LengthSquared(initialNormal, out var initialNormalLengthSquared);
Debug.Assert(Vector.LessThanAll(Vector.BitwiseOr(inactiveLanes, Vector.LessThan(Vector.Abs(initialNormalLengthSquared - Vector<float>.One), new Vector<float>(1e-6f))), Vector<int>.Zero));
#endif
FindSupport(a, b, localOffsetB, localOrientationB, ref supportFinderA, ref supportFinderB, initialNormal, inactiveLanes, out var initialSupport, out var initialSupportOnA);
Vector3Wide.Dot(initialSupport, initialNormal, out var initialDepth);
Create(initialNormal, initialSupport, initialSupportOnA, out SimplexWithWitness simplex);
FindMinimumDepth(a, b, localOffsetB, localOrientationB, ref supportFinderA, ref supportFinderB, ref simplex, initialNormal, initialDepth, inactiveLanes, searchEpsilon, minimumDepthThreshold, out depth, out refinedNormal, out witnessOnA, maximumIterations);
}
public static void FindMinimumDepth(in TShapeWideA a, in TShapeWideB b, in Vector3Wide localOffsetB, in Matrix3x3Wide localOrientationB, ref TSupportFinderA supportFinderA, ref TSupportFinderB supportFinderB,
ref SimplexWithWitness simplex, in Vector3Wide initialNormal, in Vector<float> initialDepth,
in Vector<int> inactiveLanes, in Vector<float> convergenceThreshold, in Vector<float> minimumDepthThreshold,
out Vector<float> refinedDepth, out Vector3Wide refinedNormal, out Vector3Wide witnessOnA, int maximumIterations = 50)
{
Vector<float> depthThreshold = minimumDepthThreshold;
if (supportFinderA.HasMargin)
{
supportFinderA.GetMargin(a, out var margin);
depthThreshold -= margin;
}
if (supportFinderB.HasMargin)
{
supportFinderB.GetMargin(b, out var margin);
depthThreshold -= margin;
}
var depthBelowThreshold = Vector.LessThan(initialDepth, depthThreshold);
var terminatedLanes = Vector.BitwiseOr(depthBelowThreshold, inactiveLanes);
refinedNormal = initialNormal;
refinedDepth = initialDepth;
if (Vector.LessThanAll(terminatedLanes, Vector<int>.Zero))
{
witnessOnA = default;
return;
}
GetNextNormal<HasNoNewSupport>(ref simplex, default, default, ref terminatedLanes, refinedNormal, refinedDepth, convergenceThreshold, out var normal);
for (int i = 0; i < maximumIterations; ++i)
{
if (Vector.LessThanAll(terminatedLanes, Vector<int>.Zero))
break;
FindSupport(a, b, localOffsetB, localOrientationB, ref supportFinderA, ref supportFinderB, normal, terminatedLanes, out var support, out var supportOnA);
Vector3Wide.Dot(support, normal, out var depth);
var useNewDepth = Vector.AndNot(Vector.LessThan(depth, refinedDepth), terminatedLanes);
refinedDepth = Vector.ConditionalSelect(useNewDepth, depth, refinedDepth);
Vector3Wide.ConditionalSelect(useNewDepth, normal, refinedNormal, out refinedNormal);
terminatedLanes = Vector.BitwiseOr(Vector.LessThanOrEqual(refinedDepth, depthThreshold), terminatedLanes);
if (Vector.LessThanAll(terminatedLanes, Vector<int>.Zero))
break;
GetNextNormal<HasNewSupport>(ref simplex, support, supportOnA, ref terminatedLanes, refinedNormal, refinedDepth, convergenceThreshold, out normal);
}
if (supportFinderA.HasMargin)
{
supportFinderA.GetMargin(a, out var margin);
refinedDepth += margin;
}
if (supportFinderB.HasMargin)
{
supportFinderB.GetMargin(b, out var margin);
refinedDepth += margin;
}
//For simplexes terminating in a triangle state, we deferred the division for converting plane tests to barycentric coordinates until now.
var inverseDenominator = Vector<float>.One / simplex.WeightDenominator;
Vector3Wide.Scale(simplex.A.SupportOnA, simplex.A.Weight * inverseDenominator, out var weightedA);
Vector3Wide.Scale(simplex.B.SupportOnA, simplex.B.Weight * inverseDenominator, out var weightedB);
Vector3Wide.Scale(simplex.C.SupportOnA, simplex.C.Weight * inverseDenominator, out var weightedC);
Vector3Wide.Add(weightedA, weightedB, out witnessOnA);
Vector3Wide.Add(weightedC, witnessOnA, out witnessOnA);
if (supportFinderA.HasMargin)
{
supportFinderA.GetMargin(a, out var margin);
Vector3Wide.Scale(refinedNormal, margin, out var witnessOffset);
Vector3Wide.Add(witnessOffset, witnessOnA, out witnessOnA);
}
}
}
}
You can’t perform that action at this time.