Skip to content

Next Gen STEP AP242 Native-ish BREP Geometric Modeling Kernel

29.6 KB
Raw Blame
using System.Collections.Immutable;
using System.Numerics;
using Aetheris.Kernel.Core.Math;

namespace Aetheris.Cloth3D;

public sealed record ClothStepOptions3D
{
    public float DeltaSeconds { get; init; } = 1f / 60;
    public int Substeps { get; init; } = 8;
    public int Iterations { get; init; } = 4;
    public int ContactIterations { get; init; } = 2;
    public Vector3 Gravity { get; init; } = new(0, -9.81f, 0);
    public float VelocityDamping { get; init; } = 1.5f;
    public float ContactSafetyFactor { get; init; } = .4f;
    public float StitchComplianceScale { get; init; } = 1;
    public float OffsetContactCompliance { get; init; } = .000001f;

    public void Validate()
    {
        ClothValidation3D.Finite(Gravity);
        if (!float.IsFinite(DeltaSeconds) || DeltaSeconds is < .00001f or > .1f
            || Substeps is < 1 or > 64 || Iterations is < 1 or > 64 || ContactIterations is < 1 or > 32
            || !float.IsFinite(VelocityDamping) || VelocityDamping < 0
            || !float.IsFinite(ContactSafetyFactor) || ContactSafetyFactor <= 0 || ContactSafetyFactor >= .5f
            || !float.IsFinite(StitchComplianceScale) || StitchComplianceScale is < 1 or > 100000000
            || !float.IsFinite(OffsetContactCompliance) || OffsetContactCompliance < 0)
        {
            throw new ArgumentException("Invalid bounded cloth step options.");
        }
    }
}

/// <summary>One-way, discrete contact baseline. Plane equation dot(normal,p) >= offset;
/// sphere contact covers vertices AND triangle interiors. No CCD or self-contact claim.</summary>
public sealed record ClothContacts3D
{
    public ImmutableArray<ClothHalfSpaceGuide3D> Guides { get; init; } = [];
    public IClothBodySurface3D? BodySurface { get; init; }
    public double BodyTimeSeconds { get; init; }
    public float BodyClearance { get; init; }
    public Vector3? PlaneNormal { get; init; }
    public float PlaneOffset { get; init; }
    public Vector3? SphereCenter { get; init; }
    public float SphereRadius { get; init; }

    public void Validate()
    {
        if (Guides.IsDefault || Guides.Length > 32 || Guides.Sum(guide => (long)guide.Vertices.Length) > 32768)
        {
            throw new ArgumentException("Cloth fitting guides exceed the bounded guide budget.");
        }
        var names = new HashSet<string>(StringComparer.Ordinal);
        foreach (var guide in Guides)
        {
            ClothValidation3D.Finite(guide.Normal);
            if (string.IsNullOrWhiteSpace(guide.Id) || !names.Add(guide.Id)
                || guide.Vertices.IsDefaultOrEmpty || guide.Vertices.Distinct().Count() != guide.Vertices.Length
                || MathF.Abs(guide.Normal.LengthSquared() - 1) > .0001f
                || !float.IsFinite(guide.Offset) || !float.IsFinite(guide.Compliance) || guide.Compliance < 0)
            {
                throw new ArgumentException("Invalid cloth fitting guide identity, vertices, plane or compliance.");
            }
        }
        if (!double.IsFinite(BodyTimeSeconds) || BodyTimeSeconds < 0)
        {
            throw new ArgumentException("Body sample time must be finite and nonnegative.");
        }
        if (PlaneNormal is { } normal)
        {
            ClothValidation3D.Finite(normal);
            if (MathF.Abs(normal.LengthSquared() - 1) > .0001f)
            {
                throw new ArgumentException("Cloth plane requires a unit normal.");
            }
        }
        if (SphereCenter is { } center)
        {
            ClothValidation3D.Finite(center);
            if (!float.IsFinite(SphereRadius) || SphereRadius <= 0)
            {
                throw new ArgumentException("Cloth sphere radius must be positive.");
            }
        }
        if (!float.IsFinite(BodyClearance) || BodyClearance is < 0 or > .03f)
        {
            throw new ArgumentException("Body clearance must be finite and between zero and 30mm.");
        }
        if (!float.IsFinite(PlaneOffset))
        {
            throw new ArgumentException("Nonfinite plane offset.");
        }
    }
}

/// <summary>Temporary vertex subset constraint: dot(normal, position) >= offset.
/// Construction geometry is compiled away; this is an explicit solver command.</summary>
public sealed record ClothHalfSpaceGuide3D(string Id, ImmutableArray<int> Vertices,
    Vector3 Normal, float Offset, float Compliance);

public sealed record ClothSnapshot3D(string ContentKey, long Tick, ImmutableArray<Vector3> Positions,
    ImmutableArray<Vector3> Velocities, ImmutableArray<Vector3> PinTargets);

public sealed record ClothMetrics3D(float MaximumStretch, float MaximumPinError,
    float MaximumSurfacePenetration, float BendEnergy, bool Finite)
{
    public ClothOffsetMetrics3D? OffsetContact { get; init; }
    public float MaximumMaterialShear { get; init; }
}

public sealed record ClothOffsetMetrics3D(float MinimumFeatureClearance,
    float MaximumActiveOffsetOverlap, int ActivePairs);

public interface IClothSolver3D : IDisposable
{
    CompiledCloth3D Plan { get; }
    long Tick { get; }
    void SetPin(int vertex, Vector3 target);
    void Step(ClothStepOptions3D options, ClothContacts3D contacts);
    ClothSnapshot3D Capture();
    void Restore(ClothSnapshot3D snapshot);
}

/// <summary>Deterministic CPU reference for the same colored schedule used by Vulkan.
/// Multipliers reset per substep, accumulate across iterations, and are not warm-started.</summary>
public sealed class ClothSolver3D : IClothSolver3D
{
    private readonly Vector3[] positions;
    private readonly Vector3[] velocities;
    private readonly Vector3[] previous;
    private readonly Vector3[] targets;
    private readonly Vector3[] pinStarts;
    private readonly Vector3[] lambda;
    private readonly Vector3[] contactAnchor;
    private float[]? contactBounds;
    private readonly float[] contactLambda;

    public CompiledCloth3D Plan { get; }
    public long Tick { get; private set; }

    public ClothSolver3D(CompiledCloth3D plan)
    {
        ArgumentNullException.ThrowIfNull(plan);
        Plan = plan;
        positions = (plan.Definition.InitialPositions.IsEmpty
            ? plan.Definition.Positions : plan.Definition.InitialPositions).ToArray();
        previous = new Vector3[positions.Length];
        velocities = new Vector3[positions.Length];
        targets = positions.ToArray();
        pinStarts = new Vector3[positions.Length];
        lambda = new Vector3[plan.Constraints.Length];
        contactAnchor = new Vector3[positions.Length];
        contactLambda = new float[plan.ContactTopology?.Pairs.Length ?? 0];
    }

    public void SetPin(int vertex, Vector3 target)
    {
        ClothState3D.ValidatePin(Plan, vertex, target);
        targets[vertex] = target;
    }

    public void Step(ClothStepOptions3D options, ClothContacts3D contacts)
    {
        ClothState3D.ValidateStep(Plan, options, contacts);
        if (contacts.BodySurface is IClothKinematicBodySurface3D initialBody)
        {
            initialBody.Sample(contacts.BodyTimeSeconds);
        }
        if (Plan.ContactTopology is not null)
        {
            ClothOffsetContact3D.ValidateInitial(Plan, Capture(), contacts);
        }
        float h = options.DeltaSeconds / options.Substeps;
        var guideMultipliers = contacts.Guides.Select(guide => new float[guide.Vertices.Length]).ToArray();
        float damping = MathF.Exp(-options.VelocityDamping * h);
        positions.CopyTo(pinStarts, 0);
        for (int substep = 0; substep < options.Substeps; substep++)
        {
            if (contacts.BodySurface is IClothKinematicBodySurface3D movingBody)
            {
                movingBody.Sample(contacts.BodyTimeSeconds + (substep + 1d) * h);
            }
            Array.Clear(lambda);
            Array.Clear(contactLambda);
            foreach (var multipliers in guideMultipliers)
            {
                Array.Clear(multipliers);
            }
            RefreshBounds(options, contacts);
            float fraction = (substep + 1f) / options.Substeps;
            for (int vertex = 0; vertex < positions.Length; vertex++)
            {
                previous[vertex] = positions[vertex];
                if (Plan.InverseMasses[vertex] == 0)
                {
                    positions[vertex] = Vector3.Lerp(pinStarts[vertex], targets[vertex], fraction);
                }
                else
                {
                    velocities[vertex] += options.Gravity * h;
                    positions[vertex] += velocities[vertex] * h;
                }
            }
            LimitPositions();
            for (int iteration = 0; iteration < options.Iterations; iteration++)
            {
                RefreshBounds(options, contacts);
                foreach (var color in Plan.ConstraintColors)
                {
                    foreach (int index in color)
                    {
                        ProjectConstraint(index, h, options.StitchComplianceScale);
                    }
                }
                for (int contact = 0; contact < options.ContactIterations; contact++)
                {
                    ProjectGuides(contacts.Guides, guideMultipliers, h);
                    ProjectContacts(contacts);
                    ProjectOffsetContacts(options, h);
                }
            }
            for (int vertex = 0; vertex < positions.Length; vertex++)
            {
                velocities[vertex] = (positions[vertex] - previous[vertex]) * (damping / h);
            }
        }
        Tick++;
    }

    private void ProjectGuides(ImmutableArray<ClothHalfSpaceGuide3D> guides, float[][] multipliers, float h)
    {
        for (int index = 0; index < guides.Length; index++)
        {
            var guide = guides[index];
            float alpha = guide.Compliance / (h * h);
            for (int local = 0; local < guide.Vertices.Length; local++)
            {
                int vertex = guide.Vertices[local];
                float weight = Plan.InverseMasses[vertex];
                if (weight == 0)
                {
                    continue;
                }
                float distance = Vector3.Dot(guide.Normal, positions[vertex]) - guide.Offset;
                float previousLambda = multipliers[index][local];
                float nextLambda = MathF.Max(0, previousLambda + (-distance - alpha * previousLambda) / (weight + alpha));
                multipliers[index][local] = nextLambda;
                Move(vertex, guide.Normal * (weight * (nextLambda - previousLambda)));
            }
        }
    }

    private void RefreshBounds(ClothStepOptions3D options, ClothContacts3D contacts)
    {
        if (Plan.ContactTopology is null)
        {
            return;
        }
        positions.CopyTo(contactAnchor, 0);
        contactBounds = ClothOffsetContact3D.Bounds(Plan, contactAnchor, contacts, options.ContactSafetyFactor);
    }

    private void LimitPositions()
    {
        if (contactBounds is null)
        {
            return;
        }
        for (int vertex = 0; vertex < positions.Length; vertex++)
        {
            positions[vertex] = ClothOffsetContact3D.Clip(positions[vertex], contactAnchor[vertex], contactBounds[vertex]);
        }
    }

    private void Move(int vertex, Vector3 correction)
    {
        Vector3 proposed = positions[vertex] + correction;
        positions[vertex] = contactBounds is null ? proposed
            : ClothOffsetContact3D.Clip(proposed, contactAnchor[vertex], contactBounds[vertex]);
    }

    private void ProjectOffsetContacts(ClothStepOptions3D options, float h)
    {
        if (Plan.ContactTopology is not { } topology)
        {
            return;
        }
        float alpha = options.OffsetContactCompliance / (h * h);
        foreach (var color in topology.Colors)
        {
            foreach (int index in color)
            {
                var pair = topology.Pairs[index];
                var query = ClothOffsetContact3D.Query(topology, pair, positions);
                if (!query.Active || query.Distance >= Plan.Definition.Material.Thickness)
                {
                    contactLambda[index] = 0;
                    continue;
                }
                Vector2 potential = ClothOffsetContact3D.Potential(query.Distance, Plan.Definition.Material.Thickness);
                float movable = 0;
                for (int local = 0; local < pair.Vertices.Length; local++)
                {
                    float weight = query.Weights[local] * potential.Y;
                    movable += Plan.InverseMasses[pair.Vertices[local]] * weight * weight;
                }
                if (movable <= 1e-20f)
                {
                    continue;
                }
                float increment = (-potential.X - alpha * contactLambda[index]) / (movable + alpha);
                contactLambda[index] += increment;
                for (int local = 0; local < pair.Vertices.Length; local++)
                {
                    int vertex = pair.Vertices[local];
                    Move(vertex, query.Normal * (Plan.InverseMasses[vertex] * query.Weights[local] * potential.Y * increment));
                }
            }
        }
    }

    private void ProjectConstraint(int index, float h, float stitchComplianceScale)
    {
        var constraint = Plan.Constraints[index];
        float compliance = constraint.Kind == ClothConstraintKind3D.Stitch
            ? constraint.Compliance * stitchComplianceScale : constraint.Compliance;
        float alpha = compliance / (h * h);
        if (ClothSurfaceMechanics3D.IsSurface(constraint.Kind))
        {
            Span<Vector3> gradients = stackalloc Vector3[6];
            float residual = ClothSurfaceMechanics3D.Evaluate(constraint, positions, gradients);
            float surfaceMovable = 0;
            for (int local = 0; local < constraint.Vertices.Length; local++)
            {
                surfaceMovable += Plan.InverseMasses[constraint.Vertices[local]] * gradients[local].LengthSquared();
            }
            if (surfaceMovable <= 1e-20f)
            {
                return;
            }
            float delta = (-residual - alpha * lambda[index].X) / (surfaceMovable + alpha);
            lambda[index].X += delta;
            for (int local = 0; local < constraint.Vertices.Length; local++)
            {
                int vertex = constraint.Vertices[local];
                Move(vertex, gradients[local] * (Plan.InverseMasses[vertex] * delta));
            }
            return;
        }
        if (constraint.Kind is not (ClothConstraintKind3D.FlatBend or ClothConstraintKind3D.VertexBend or ClothConstraintKind3D.Stitch))
        {
            int a = constraint.Vertices[0];
            int b = constraint.Vertices[1];
            float wa = Plan.InverseMasses[a];
            float wb = Plan.InverseMasses[b];
            Vector3 difference = positions[a] - positions[b];
            float length = difference.Length();
            if (length < 1e-10f || wa + wb == 0)
            {
                return;
            }
            float delta = (-(length - constraint.RestLength) - alpha * lambda[index].X) / (wa + wb + alpha);
            lambda[index].X += delta;
            Vector3 correction = difference * (delta / length);
            Move(a, correction * wa);
            Move(b, -correction * wb);
            return;
        }
        Vector3 curvature = Vector3.Zero;
        float denominator = alpha;
        float movable = 0;
        for (int local = 0; local < constraint.Vertices.Length; local++)
        {
            int vertex = constraint.Vertices[local];
            float weight = constraint.Weights[local];
            curvature += positions[vertex] * weight;
            float term = Plan.InverseMasses[vertex] * weight * weight;
            denominator += term;
            movable += term;
        }
        if (movable <= 1e-20f)
        {
            return;
        }
        Vector3 increment = (-curvature - alpha * lambda[index]) / denominator;
        lambda[index] += increment;
        for (int local = 0; local < constraint.Vertices.Length; local++)
        {
            int vertex = constraint.Vertices[local];
            Move(vertex, increment * (Plan.InverseMasses[vertex] * constraint.Weights[local]));
        }
    }

    private void ProjectContacts(ClothContacts3D contacts)
    {
        float thickness = Plan.Definition.Material.Thickness * .5f;
        for (int vertex = 0; vertex < positions.Length; vertex++)
        {
            if (Plan.InverseMasses[vertex] == 0)
            {
                continue;
            }
            if (contacts.BodySurface is { } body)
            {
                var sample = body.Query(positions[vertex]);
                float clearance = thickness + contacts.BodyClearance;
                if (sample.SignedDistance < clearance)
                {
                    Move(vertex, sample.Normal * (clearance - sample.SignedDistance));
                }
            }
            if (contacts.PlaneNormal is { } normal)
            {
                float gap = Vector3.Dot(normal, positions[vertex]) - contacts.PlaneOffset - thickness;
                if (gap < 0)
                {
                    Move(vertex, -normal * gap);
                }
            }
            if (contacts.SphereCenter is { } center)
            {
                Vector3 delta = positions[vertex] - center;
                float distance = delta.Length();
                float radius = contacts.SphereRadius + thickness;
                if (distance < radius)
                {
                    Vector3 outward = distance > 1e-10f ? delta / distance : Vector3.UnitY;
                    Move(vertex, center + outward * radius - positions[vertex]);
                }
            }
        }
        if (contacts.BodySurface is { } bodySurface)
        {
            foreach (var color in Plan.TriangleColors)
            {
                foreach (int triangle in color)
                {
                    int a = Plan.Definition.Indices[triangle * 3];
                    int b = Plan.Definition.Indices[triangle * 3 + 1];
                    int c = Plan.Definition.Indices[triangle * 3 + 2];
                    Vector3 center = (positions[a] + positions[b] + positions[c]) / 3;
                    float radius = MathF.Max(Vector3.Distance(center, positions[a]),
                        MathF.Max(Vector3.Distance(center, positions[b]), Vector3.Distance(center, positions[c])));
                    // A positive nearest-surface distance beyond the containing sphere
                    // certifies this triangle is outside the static contact neighborhood.
                    if (bodySurface.Query(center).SignedDistance > radius + thickness + contacts.BodyClearance) continue;
                    foreach (Vector3 barycentric in ClothBodyContact3D.Samples)
                    {
                        Vector3 point = positions[a] * barycentric.X + positions[b] * barycentric.Y + positions[c] * barycentric.Z;
                        var sample = bodySurface.Query(point);
                        float penetration = thickness + contacts.BodyClearance - sample.SignedDistance;
                        if (penetration <= 0) continue;
                        float denominator = Plan.InverseMasses[a] * barycentric.X * barycentric.X
                            + Plan.InverseMasses[b] * barycentric.Y * barycentric.Y
                            + Plan.InverseMasses[c] * barycentric.Z * barycentric.Z;
                        if (denominator <= 1e-20f) continue;
                        Vector3 correction = sample.Normal * (penetration / denominator);
                        Move(a, correction * (Plan.InverseMasses[a] * barycentric.X));
                        Move(b, correction * (Plan.InverseMasses[b] * barycentric.Y));
                        Move(c, correction * (Plan.InverseMasses[c] * barycentric.Z));
                    }
                }
            }
        }
        if (contacts.SphereCenter is not { } sphere)
        {
            return;
        }
        foreach (var color in Plan.TriangleColors)
        {
            foreach (int triangle in color)
            {
                int a = Plan.Definition.Indices[triangle * 3];
                int b = Plan.Definition.Indices[triangle * 3 + 1];
                int c = Plan.Definition.Indices[triangle * 3 + 2];
                var closest = TriangleSurface3D.ClosestPoint(sphere, positions[a], positions[b], positions[c]);
                Vector3 delta = closest.Point - sphere;
                float distance = delta.Length();
                float penetration = contacts.SphereRadius + thickness - distance;
                if (penetration <= 0)
                {
                    continue;
                }
                Vector3 barycentric = closest.Barycentric;
                float denominator = Plan.InverseMasses[a] * barycentric.X * barycentric.X
                    + Plan.InverseMasses[b] * barycentric.Y * barycentric.Y
                    + Plan.InverseMasses[c] * barycentric.Z * barycentric.Z;
                if (denominator <= 1e-20f)
                {
                    continue;
                }
                Vector3 normal = distance > 1e-10f ? delta / distance : Vector3.UnitY;
                Vector3 correction = normal * (penetration / denominator);
                Move(a, correction * (Plan.InverseMasses[a] * barycentric.X));
                Move(b, correction * (Plan.InverseMasses[b] * barycentric.Y));
                Move(c, correction * (Plan.InverseMasses[c] * barycentric.Z));
            }
        }
    }

    public ClothSnapshot3D Capture() => new(Plan.ContentKey, Tick, positions.ToImmutableArray(),
        velocities.ToImmutableArray(), targets.ToImmutableArray());

    public void Restore(ClothSnapshot3D snapshot)
    {
        ClothState3D.ValidateSnapshot(Plan, snapshot);
        if (Plan.ContactTopology is not null)
        {
            ClothOffsetContact3D.ValidateInitial(Plan, snapshot, new());
        }
        snapshot.Positions.CopyTo(positions);
        snapshot.Velocities.CopyTo(velocities);
        snapshot.PinTargets.CopyTo(targets);
        Tick = snapshot.Tick;
    }

    public void Dispose() { }
}

public static class ClothState3D
{
    public static void ValidateStep(CompiledCloth3D plan, ClothStepOptions3D options, ClothContacts3D contacts)
    {
        ArgumentNullException.ThrowIfNull(options);
        ArgumentNullException.ThrowIfNull(contacts);
        options.Validate();
        contacts.Validate();
        float h = options.DeltaSeconds / options.Substeps;
        foreach (var guide in contacts.Guides)
        {
            if (guide.Vertices.Any(vertex => vertex < 0 || vertex >= plan.InverseMasses.Length)
                || !float.IsFinite(guide.Compliance / (h * h)))
            {
                throw new ArgumentException("Cloth fitting guide vertices or timestep are invalid.");
            }
        }
        if (plan.Constraints.Any(constraint => !float.IsFinite(constraint.Compliance / (h * h))))
        {
            throw new ArgumentException("Cloth compliance and timestep exceed finite solver arithmetic.");
        }
        if (!float.IsFinite(options.OffsetContactCompliance / (h * h)))
        {
            throw new ArgumentException("Offset contact compliance exceeds finite solver arithmetic.");
        }
    }

    public static void ValidatePin(CompiledCloth3D plan, int vertex, Vector3 target)
    {
        ClothValidation3D.Finite(target);
        if (vertex < 0 || vertex >= plan.InverseMasses.Length || plan.InverseMasses[vertex] != 0)
        {
            throw new ArgumentException("Pin commands require an authored pinned vertex.");
        }
    }

    public static void ValidateSnapshot(CompiledCloth3D plan, ClothSnapshot3D snapshot)
    {
        ArgumentNullException.ThrowIfNull(snapshot);
        int count = plan.Definition.Positions.Length;
        if (snapshot.ContentKey != plan.ContentKey || snapshot.Tick < 0 || snapshot.Positions.IsDefault
            || snapshot.Velocities.IsDefault || snapshot.PinTargets.IsDefault || snapshot.Positions.Length != count
            || snapshot.Velocities.Length != count || snapshot.PinTargets.Length != count)
        {
            throw new ArgumentException("Cloth snapshot belongs to another topology/material or has invalid state dimensions.");
        }
        foreach (Vector3 value in snapshot.Positions)
        {
            ValidateVector(value);
        }
        foreach (Vector3 value in snapshot.Velocities)
        {
            ValidateVector(value);
        }
        foreach (Vector3 value in snapshot.PinTargets)
        {
            ValidateVector(value);
        }
    }

    private static void ValidateVector(Vector3 value) => ClothValidation3D.Finite(value);

    public static ClothMetrics3D Measure(CompiledCloth3D plan, ClothSnapshot3D snapshot, ClothContacts3D contacts)
    {
        ValidateSnapshot(plan, snapshot);
        contacts.Validate();
        var positions = snapshot.Positions;
        float stretch = 0;
        float bending = 0;
        float shear = 0;
        Span<Vector3> surfaceGradients = stackalloc Vector3[6];
        foreach (var constraint in plan.Constraints)
        {
            if (ClothSurfaceMechanics3D.IsSurface(constraint.Kind))
            {
                float residual = ClothSurfaceMechanics3D.Evaluate(constraint, positions.AsSpan(), surfaceGradients);
                if (constraint.Kind == ClothConstraintKind3D.SurfaceBend)
                {
                    bending += .5f * residual * residual;
                }
                else if (constraint.Kind == ClothConstraintKind3D.SurfaceShear)
                {
                    shear = MathF.Max(shear, MathF.Abs(residual));
                }
                else
                {
                    stretch = MathF.Max(stretch, MathF.Sqrt(MathF.Max(0, 1 + 2 * residual)) - 1);
                }
                continue;
            }
            if (constraint.Kind is not (ClothConstraintKind3D.FlatBend or ClothConstraintKind3D.VertexBend or ClothConstraintKind3D.Stitch))
            {
                float length = Vector3.Distance(positions[constraint.Vertices[0]], positions[constraint.Vertices[1]]);
                stretch = MathF.Max(stretch, length / constraint.RestLength - 1);
            }
            else
            {
                Vector3 curvature = Vector3.Zero;
                for (int local = 0; local < constraint.Vertices.Length; local++)
                {
                    curvature += positions[constraint.Vertices[local]] * constraint.Weights[local];
                }
                if (constraint.Kind != ClothConstraintKind3D.Stitch)
                {
                    bending += .5f * curvature.LengthSquared();
                }
            }
        }
        float pinError = 0;
        foreach (int vertex in plan.Definition.Pins)
        {
            pinError = MathF.Max(pinError, Vector3.Distance(positions[vertex], snapshot.PinTargets[vertex]));
        }
        float penetration = 0;
        float halfThickness = plan.Definition.Material.Thickness * .5f;
        if (contacts.PlaneNormal is { } normal)
        {
            foreach (Vector3 position in positions)
            {
                penetration = MathF.Max(penetration, contacts.PlaneOffset + halfThickness - Vector3.Dot(normal, position));
            }
        }
        if (contacts.SphereCenter is { } sphere)
        {
            for (int triangle = 0; triangle < plan.Definition.Indices.Length; triangle += 3)
            {
                var indices = plan.Definition.Indices;
                var closest = TriangleSurface3D.ClosestPoint(sphere, positions[indices[triangle]],
                    positions[indices[triangle + 1]], positions[indices[triangle + 2]]);
                penetration = MathF.Max(penetration, contacts.SphereRadius + halfThickness - Vector3.Distance(sphere, closest.Point));
            }
        }
        if (contacts.BodySurface is { } body)
        {
            foreach (Vector3 position in positions)
            {
                penetration = MathF.Max(penetration, halfThickness + contacts.BodyClearance - body.Query(position).SignedDistance);
            }
            for (int face = 0; face < plan.Definition.Indices.Length; face += 3)
            {
                int a = plan.Definition.Indices[face];
                int b = plan.Definition.Indices[face + 1];
                int c = plan.Definition.Indices[face + 2];
                foreach (Vector3 barycentric in ClothBodyContact3D.Samples)
                {
                    Vector3 point = positions[a] * barycentric.X + positions[b] * barycentric.Y + positions[c] * barycentric.Z;
                    penetration = MathF.Max(penetration, halfThickness + contacts.BodyClearance - body.Query(point).SignedDistance);
                }
            }
        }
        ClothOffsetMetrics3D? offsetMetrics = null;
        if (plan.ContactTopology is { } topology)
        {
            float clearance = 1e6f;
            float overlap = 0;
            int active = 0;
            foreach (var pair in topology.Pairs)
            {
                var query = ClothOffsetContact3D.Query(topology, pair, positions);
                if (pair.Kind is ClothContactFeature3D.Face or ClothContactFeature3D.EdgePair)
                {
                    clearance = MathF.Min(clearance, query.Distance);
                }
                if (query.Active && query.Distance < plan.Definition.Material.Thickness)
                {
                    overlap = MathF.Max(overlap, plan.Definition.Material.Thickness - query.Distance);
                    active++;
                }
            }
            offsetMetrics = new(clearance, overlap, active);
        }
        return new(stretch, pinError, penetration, bending,
            float.IsFinite(stretch) && float.IsFinite(bending) && float.IsFinite(shear))
        {
            OffsetContact = offsetMetrics,
            MaximumMaterialShear = shear,
        };
    }
}