Skip to content

Next Gen STEP AP242 Native-ish BREP Geometric Modeling Kernel

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

namespace Aetheris.Cloth3D;

public readonly record struct ClothBodySample3D(Vector3 Point, Vector3 Normal, float SignedDistance);

public interface IClothBodySurface3D
{
    ClothBodySample3D Query(Vector3 position);
}

/// <summary>Externally authored motion sampled at an explicit absolute time.
/// Replay requires the same BodyTimeSeconds input alongside the cloth snapshot.</summary>
public interface IClothKinematicBodySurface3D : IClothBodySurface3D
{
    void Sample(double seconds);
}

/// <summary>Topology-owned pose snapshot with a nearest-triangle BVH. Refit is an explicit,
/// externally synchronized operation between solver substeps. Outward triangle winding
/// is a caller contract. This is a discrete oriented surface query, not a watertight SDF or CCD.</summary>
public sealed class ClothTriangleBody3D : IClothBodySurface3D
{
    private readonly Vector3[] positions;
    private readonly Vector3[] normals;
    private readonly int[] indices;
    private readonly int[] triangles;
    private readonly List<Node> nodes = [];

    private sealed record Node(Vector3 Minimum, Vector3 Maximum, int Start, int Count, int Left, int Right);

    public ClothTriangleBody3D(ImmutableArray<Vector3> positions, ImmutableArray<int> indices)
    {
        if (positions.IsDefaultOrEmpty || indices.IsDefaultOrEmpty || indices.Length % 3 != 0
            || indices.Any(index => index < 0 || index >= positions.Length))
        {
            throw new ArgumentException("Body surface requires finite vertices and indexed outward triangles.");
        }
        foreach (var position in positions)
        {
            ClothValidation3D.Finite(position);
        }
        this.positions = positions.ToArray();
        this.indices = indices.ToArray();
        normals = new Vector3[positions.Length];
        for (int face = 0; face < indices.Length; face += 3)
        {
            int a = indices[face];
            int b = indices[face + 1];
            int c = indices[face + 2];
            Vector3 normal = Vector3.Cross(positions[b] - positions[a], positions[c] - positions[a]);
            if (normal.LengthSquared() <= 1e-20f)
            {
                throw new ArgumentException("Body surface has a degenerate triangle.");
            }
            normals[a] += normal;
            normals[b] += normal;
            normals[c] += normal;
        }
        for (int vertex = 0; vertex < normals.Length; vertex++)
        {
            if (normals[vertex].LengthSquared() > 1e-20f)
            {
                normals[vertex] = Vector3.Normalize(normals[vertex]);
            }
        }
        triangles = Enumerable.Range(0, indices.Length / 3).ToArray();
        Build(0, triangles.Length);
    }

    public void Refit(IReadOnlyList<Vector3> updatedPositions)
    {
        if (updatedPositions.Count != positions.Length)
        {
            throw new ArgumentException("Body refit must preserve vertex count and triangle topology.");
        }
        var updatedNormals = new Vector3[positions.Length];
        foreach (var position in updatedPositions)
        {
            ClothValidation3D.Finite(position);
        }
        for (int face = 0; face < indices.Length; face += 3)
        {
            int a = indices[face];
            int b = indices[face + 1];
            int c = indices[face + 2];
            var normal = Vector3.Cross(updatedPositions[b] - updatedPositions[a], updatedPositions[c] - updatedPositions[a]);
            if (normal.LengthSquared() <= 1e-20f)
            {
                throw new ArgumentException("Body refit has a degenerate triangle.");
            }
            updatedNormals[a] += normal;
            updatedNormals[b] += normal;
            updatedNormals[c] += normal;
        }
        for (int vertex = 0; vertex < positions.Length; vertex++)
        {
            positions[vertex] = updatedPositions[vertex];
            normals[vertex] = updatedNormals[vertex];
            if (normals[vertex].LengthSquared() > 1e-20f)
            {
                normals[vertex] = Vector3.Normalize(normals[vertex]);
            }
        }
        // Build records parents before children, so the reverse traversal refits leaves first.
        for (int index = nodes.Count - 1; index >= 0; index--)
        {
            var node = nodes[index];
            Vector3 minimum = new(float.PositiveInfinity);
            Vector3 maximum = new(float.NegativeInfinity);
            if (node.Count == 0)
            {
                minimum = Vector3.Min(nodes[node.Left].Minimum, nodes[node.Right].Minimum);
                maximum = Vector3.Max(nodes[node.Left].Maximum, nodes[node.Right].Maximum);
            }
            else
            {
                for (int triangle = node.Start; triangle < node.Start + node.Count; triangle++)
                {
                    for (int corner = 0; corner < 3; corner++)
                    {
                        var point = positions[indices[triangles[triangle] * 3 + corner]];
                        minimum = Vector3.Min(minimum, point);
                        maximum = Vector3.Max(maximum, point);
                    }
                }
            }
            nodes[index] = node with { Minimum = minimum, Maximum = maximum };
        }
    }

    public ClothBodySample3D Query(Vector3 position)
    {
        ClothValidation3D.Finite(position);
        float bestDistance = float.PositiveInfinity;
        int bestTriangle = -1;
        TriangleProximity3D closest = default;
        Visit(0);
        int offset = bestTriangle * 3;
        Vector3 normal = normals[indices[offset]] * closest.Barycentric.X
            + normals[indices[offset + 1]] * closest.Barycentric.Y
            + normals[indices[offset + 2]] * closest.Barycentric.Z;
        if (normal.LengthSquared() <= 1e-20f)
        {
            normal = Vector3.Cross(positions[indices[offset + 1]] - positions[indices[offset]],
                positions[indices[offset + 2]] - positions[indices[offset]]);
        }
        normal = Vector3.Normalize(normal);
        Vector3 delta = position - closest.Point;
        float distance = MathF.Sqrt(bestDistance);
        float sign = Vector3.Dot(delta, normal) < 0 ? -1 : 1;
        // The oriented pseudonormal chooses the side; the geometric nearest-point
        // direction resolves contact. Interpolated normals are not distance gradients.
        if (distance > 1e-10f)
        {
            normal = delta * (sign / distance);
        }
        return new(closest.Point, normal, distance * sign);

        void Visit(int nodeIndex)
        {
            var node = nodes[nodeIndex];
            if (DistanceToBox(position, node.Minimum, node.Maximum) > bestDistance)
            {
                return;
            }
            if (node.Count > 0)
            {
                for (int i = node.Start; i < node.Start + node.Count; i++)
                {
                    int triangle = triangles[i];
                    int face = triangle * 3;
                    var candidate = TriangleSurface3D.ClosestPoint(position, positions[indices[face]],
                        positions[indices[face + 1]], positions[indices[face + 2]]);
                    float distance = Vector3.DistanceSquared(position, candidate.Point);
                    if (distance < bestDistance || (distance == bestDistance && triangle < bestTriangle))
                    {
                        bestDistance = distance;
                        bestTriangle = triangle;
                        closest = candidate;
                    }
                }
                return;
            }
            var left = nodes[node.Left];
            var right = nodes[node.Right];
            if (DistanceToBox(position, left.Minimum, left.Maximum)
                <= DistanceToBox(position, right.Minimum, right.Maximum))
            {
                Visit(node.Left);
                Visit(node.Right);
            }
            else
            {
                Visit(node.Right);
                Visit(node.Left);
            }
        }
    }

    private int Build(int start, int count)
    {
        Vector3 minimum = new(float.PositiveInfinity);
        Vector3 maximum = new(float.NegativeInfinity);
        for (int i = start; i < start + count; i++)
        {
            for (int corner = 0; corner < 3; corner++)
            {
                var point = positions[indices[triangles[i] * 3 + corner]];
                minimum = Vector3.Min(minimum, point);
                maximum = Vector3.Max(maximum, point);
            }
        }
        int nodeIndex = nodes.Count;
        nodes.Add(new(minimum, maximum, start, count, -1, -1));
        if (count <= 8)
        {
            return nodeIndex;
        }
        Vector3 size = maximum - minimum;
        int axis = 0;
        if (size.Y > size.X)
        {
            axis = 1;
        }
        if (size.Z > size[axis])
        {
            axis = 2;
        }
        Array.Sort(triangles, start, count, Comparer<int>.Create((a, b) =>
        {
            float ca = Center(a)[axis];
            float cb = Center(b)[axis];
            int comparison = ca.CompareTo(cb);
            return comparison == 0 ? a.CompareTo(b) : comparison;
        }));
        int left = Build(start, count / 2);
        int right = Build(start + count / 2, count - count / 2);
        nodes[nodeIndex] = new(minimum, maximum, start, 0, left, right);
        return nodeIndex;
    }

    private Vector3 Center(int triangle)
    {
        int face = triangle * 3;
        return (positions[indices[face]] + positions[indices[face + 1]] + positions[indices[face + 2]]) / 3;
    }

    private static float DistanceToBox(Vector3 point, Vector3 minimum, Vector3 maximum)
    {
        return Vector3.DistanceSquared(point, Vector3.Clamp(point, minimum, maximum));
    }
}