Skip to content

Commit

Permalink
Slab expansion by tangent scootage.
Browse files Browse the repository at this point in the history
  • Loading branch information
@RossNordby
RossNordby committed Mar 9, 2025
1 parent c40801d commit 3bc6a6e
Show file tree
Hide file tree
Showing 2 changed files with 33 additions and 22 deletions.
53 changes: 32 additions & 21 deletions BepuPhysics/Collidables/ConvexHullHelper.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -88,7 +88,7 @@ public static class ConvexHullHelper
{
static void FindExtremeFace(
in Vector3Wide basisX, in Vector3Wide basisY, in Vector3Wide basisOrigin, in EdgeEndpoints sourceEdgeEndpoints, ref Buffer<Vector3Wide> pointBundles, in Vector<int> indexOffsets, Buffer<int> allowVertices, int pointCount,
ref Buffer<Vector<float>> projectedOnX, ref Buffer<Vector<float>> projectedOnY, in Vector<float> planeEpsilon, ref QuickList<int> vertexIndices, out Vector3 faceNormal)
ref Buffer<Vector<float>> projectedOnX, ref Buffer<Vector<float>> projectedOnY, Vector<float> planeEpsilon, Vector<float> normalChangePerTangentChange, ref QuickList<int> vertexIndices, out Vector3 faceNormal)
{
Debug.Assert(projectedOnX.Length >= pointBundles.Length && projectedOnY.Length >= pointBundles.Length && vertexIndices.Count == 0 && vertexIndices.Span.Length >= pointBundles.Length * Vector<float>.Count);
//Find the candidate-basisOrigin which has the smallest angle with basisY when projected onto the plane spanned by basisX and basisY.
Expand Down Expand Up @@ -192,19 +192,8 @@ static void FindExtremeFace(
for (int i = 0; i < pointBundles.Length; ++i)
{
var dot = projectedOnX[i] * projectedPlaneNormal.X + projectedOnY[i] * projectedPlaneNormal.Y;
//var dot2 = Vector3Wide.Dot(pointBundles[i] - basisOrigin, faceNormalWide);
//var error = dot2 - dot;
//if (Vector.GreaterThanAny(Vector.Abs(error), planeEpsilon))
//{
// for (int j = 0; j < Vector<float>.Count; ++j)
// {
// if (MathF.Abs(error[j]) > planeEpsilon[j])
// {
// Console.WriteLine($"error: {error[j]}");
// }
// }
//}
var coplanar = Vector.GreaterThan(dot, -planeEpsilon);
var tangentDot = Vector.Abs(projectedOnX[i] * projectedPlaneNormal.Y - projectedOnY[i] * projectedPlaneNormal.X);
var coplanar = Vector.GreaterThan(dot, -planeEpsilon - tangentDot * normalChangePerTangentChange);
if (Vector.LessThanAny(coplanar, Vector<int>.Zero))
{
var bundleBaseIndex = i << BundleIndexing.VectorShift;
Expand Down Expand Up @@ -722,9 +711,15 @@ public static void ComputeHull(Span<Vector3> points, BufferPool pool, out HullDa
Vector3Wide.Broadcast(initialVertex, out var initialVertexBundle);
pool.Take<Vector<float>>(pointBundles.Length, out var projectedOnX);
pool.Take<Vector<float>>(pointBundles.Length, out var projectedOnY);
var planeEpsilonNarrow = MathF.Sqrt(bestDistanceSquared) * 1e-5f;
// Currently using two forms of epsilon for coplanar point testing:
// 1. A 'slab' epsilon, specifying a constant width of the slab around the plane within which points are considered coplanar, and
// 2. A face coplanarity epsilon, which increases the slab width based on the distance from the measurement point.
// The face coplanarity epsilon captures points which could be member of faces that will be considered coplanar by the later face merging phase.
// If we expect they're going to show up as coplanar later, there's not much reason to create separate faces for them now.
// (This can simplify away microgeometry, but that's often actually desirable.)
var planeSlabEpsilonNarrow = MathF.Sqrt(bestDistanceSquared) * 1e-5f;
var normalCoplanarityEpsilon = 1f - 1e-6f;
var planeEpsilon = new Vector<float>(planeEpsilonNarrow);
var planeSlabEpsilon = new Vector<float>(planeSlabEpsilonNarrow);
var rawFaceVertexIndices = new QuickList<int>(pointBundles.Length * Vector<float>.Count, pool);
var initialSourceEdge = new EdgeEndpoints { A = initialIndex, B = initialIndex };

Expand All @@ -736,14 +731,28 @@ public static void ComputeHull(Span<Vector3> points, BufferPool pool, out HullDa
for (int i = points.Length; i < allowVertices.Length; ++i)
allowVertices[i] = 0;

// Note that the coplanarity test becomes more lax with distance from the edge.
// This allows the early coplanarity test to capture faces which would otherwise later be merged
// due to having face normals that are too similar.
// Consider two normals, N and M, where M is a normal maximally far from N that is still mergeable.
// Tn is the tangent to N, and Tm is the tangent to M.
// dot(N, M) == mergeThreshold == dot(Tn, Tm)
// We want the rate of change of dot(p, N) per unit change along dot(p, Tn).
// theta = acos(dot(N, M))
// tan(theta) = normalChangePerTangentChange
// normalChangePerTangentChange = tan(acos(dot(N, M)))
// normalChangePerTangentChange = sqrt(1 - dot(N, M)^2) / dot(N, M)
// normalChangePerTangentChange = sqrt(1 - mergeThreshold^2) / mergeThreshold
var normalChangePerTangentChange = new Vector<float>(MathF.Sqrt(1f - normalCoplanarityEpsilon * normalCoplanarityEpsilon) / normalCoplanarityEpsilon);

FindExtremeFace(initialBasisX, initialBasisY, initialVertexBundle, initialSourceEdge, ref pointBundles, indexOffsetBundle, allowVertices, points.Length,
ref projectedOnX, ref projectedOnY, planeEpsilon, ref rawFaceVertexIndices, out var initialFaceNormal);
ref projectedOnX, ref projectedOnY, planeSlabEpsilon, normalChangePerTangentChange, ref rawFaceVertexIndices, out var initialFaceNormal);
Debug.Assert(rawFaceVertexIndices.Count >= 2);
var facePoints = new QuickList<Vector2>(points.Length, pool);
var reducedFaceIndices = new QuickList<int>(points.Length, pool);


ReduceFace(ref rawFaceVertexIndices, initialFaceNormal, points, planeEpsilonNarrow, ref facePoints, ref allowVertices, ref reducedFaceIndices);
ReduceFace(ref rawFaceVertexIndices, initialFaceNormal, points, planeSlabEpsilonNarrow, ref facePoints, ref allowVertices, ref reducedFaceIndices);

var faces = new QuickList<EarlyFace>(points.Length, pool);
var edgesToTest = new QuickList<EdgeToTest>(points.Length, pool);
Expand Down Expand Up @@ -806,10 +815,10 @@ public static void ComputeHull(Span<Vector3> points, BufferPool pool, out HullDa
Vector3Wide.Broadcast(basisY, out var basisYBundle);
Vector3Wide.Broadcast(edgeA, out var basisOrigin);
rawFaceVertexIndices.Count = 0;
FindExtremeFace(basisXBundle, basisYBundle, basisOrigin, edgeToTest.Endpoints, ref pointBundles, indexOffsetBundle, allowVertices, points.Length, ref projectedOnX, ref projectedOnY, planeEpsilon, ref rawFaceVertexIndices, out var faceNormal);
FindExtremeFace(basisXBundle, basisYBundle, basisOrigin, edgeToTest.Endpoints, ref pointBundles, indexOffsetBundle, allowVertices, points.Length, ref projectedOnX, ref projectedOnY, planeSlabEpsilon, normalChangePerTangentChange, ref rawFaceVertexIndices, out var faceNormal);
reducedFaceIndices.Count = 0;
facePoints.Count = 0;
ReduceFace(ref rawFaceVertexIndices, faceNormal, points, planeEpsilonNarrow, ref facePoints, ref allowVertices, ref reducedFaceIndices);
ReduceFace(ref rawFaceVertexIndices, faceNormal, points, planeSlabEpsilonNarrow, ref facePoints, ref allowVertices, ref reducedFaceIndices);

if (reducedFaceIndices.Count < 3)
{
Expand All @@ -830,9 +839,11 @@ public static void ComputeHull(Span<Vector3> points, BufferPool pool, out HullDa
ref var face = ref faces[i];
if (Vector3.Dot(face.Normal, faceNormal) > normalCoplanarityEpsilon)
{
#if DEBUG_STEPS
Console.WriteLine($"Merging face {i} with new face, dot {Vector3.Dot(face.Normal, faceNormal)}:");
Console.WriteLine($"Existing face: {face.Normal}");
Console.WriteLine($"Candidate: {faceNormal}");
#endif
// The new face is coplanar with an existing face. Merge the new face into the old face.
rawFaceVertexIndices.EnsureCapacity(reducedFaceIndices.Count + face.VertexIndices.Count, pool);
rawFaceVertexIndices.Count = reducedFaceIndices.Count;
Expand All @@ -853,7 +864,7 @@ public static void ComputeHull(Span<Vector3> points, BufferPool pool, out HullDa
face.VertexIndices.Count = 0;
facePoints.Count = 0;
face.VertexIndices.EnsureCapacity(rawFaceVertexIndices.Count, pool);
ReduceFace(ref rawFaceVertexIndices, faceNormal, points, planeEpsilonNarrow, ref facePoints, ref allowVertices, ref face.VertexIndices);
ReduceFace(ref rawFaceVertexIndices, faceNormal, points, planeSlabEpsilonNarrow, ref facePoints, ref allowVertices, ref face.VertexIndices);
#if DEBUG_STEPS
step.UpdateForFaceMerge(rawFaceVertexIndices, face.VertexIndices, allowVertices, i);
#endif
Expand Down
2 changes: 1 addition & 1 deletion Demos/SpecializedTests/ConvexHullTestDemo.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -539,7 +539,7 @@ public override void Initialize(ContentArchive content, Camera camera)
{
camera.Position = new Vector3(0, -2.5f, 10);
camera.Yaw = 0;
camera.Pitch = 0;
camera.Pitch = 0;

Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1));

Expand Down

0 comments on commit 3bc6a6e

Please sign in to comment.