Individual Mesh-Mesh Collision about bepuphysics2 HOT 11 OPEN

var testShapeA = DemoMeshHelper.LoadModel(content, BufferPool, @"Content\newt.obj", new Vector3(1, 1, 1));
var testShapeB = new Box(1, 1, 1);
var poseA = new RigidPose(new Vector3(0, 0, 0));
var poseB = new RigidPose(new Vector3(1f, 0, 0));

Simulation.Statics.Add(new StaticDescription(poseA, Simulation.Shapes.Add(testShapeA)));
Simulation.Statics.Add(new StaticDescription(poseB, Simulation.Shapes.Add(testShapeB)));

unsafe bool Intersects<TShapeA, TShapeB>(TShapeA a, RigidPose poseA, TShapeB b, RigidPose poseB)
    where TShapeA : struct, IShape
    where TShapeB : struct, IShape
{
    var filter = new Filter();

    var task = Simulation.NarrowPhase.SweepTaskRegistry.GetTask(TShapeA.TypeId, TShapeB.TypeId);
    if (task == null)
        return false;
    return task.Sweep(
        Unsafe.AsPointer(ref a), TShapeA.TypeId, poseA.Orientation, default,
        Unsafe.AsPointer(ref b), TShapeB.TypeId, poseB.Position - poseA.Position, poseB.Orientation, default,
        0f, 1e-2f, 1e-5f, 25, ref filter, Simulation.Shapes, Simulation.NarrowPhase.SweepTaskRegistry, BufferPool,
        out _, out _, out _, out _);
}

var intersects = Intersects(testShapeA, poseA, testShapeB, poseB);
Console.WriteLine($"intersects: {intersects}");

    /// <summary>
    /// Adds a shape pair to the collision batcher for testing. Does not flush the batcher.
    /// </summary>
    /// <typeparam name="TShapeA">Type of the first shape to test.</typeparam>
    /// <typeparam name="TShapeB">Type of the second shape to test.</typeparam>
    /// <typeparam name="TCallbacks">Type of the callbacks to use for the collision tests.</typeparam>
    /// <param name="a">First shape to test.</param>
    /// <param name="poseA">Pose of the first shape to test.</param>
    /// <param name="b">Second shape to test.</param>
    /// <param name="poseB">Pose of the second shape to test.</param>
    /// <param name="speculativeMargin">Speculative margin to use for the collision test.</param>
    /// <param name="pairId">Pair id to associate with this test. The pair id will be reported in the callback when the batcher finishes the pair.</param>
    /// <param name="batcher">Batcher to add the pair to.</param>
    public unsafe void AddPairToBatcher<TShapeA, TShapeB, TCallbacks>(TShapeA a, RigidPose poseA, TShapeB b, RigidPose poseB, float speculativeMargin, int pairId, ref CollisionBatcher<TCallbacks> batcher)
        where TShapeA : unmanaged, IShape
        where TShapeB : unmanaged, IShape
        where TCallbacks : struct, ICollisionCallbacks
    {
        batcher.Add(TShapeA.TypeId, TShapeB.TypeId, sizeof(TShapeA), sizeof(TShapeB), &a, &b, poseB.Position - poseA.Position, poseA.Orientation, poseB.Orientation, speculativeMargin, pairId);
    }

    /// <summary>
    /// Simple callbacks for a single unbatched boolean test.
    /// </summary>
    struct BooleanCallbacks : ICollisionCallbacks
    {
        /// <summary>
        /// The intersection state of the tested pair. Since there's only one test, there's no need for fancier state.
        /// </summary>
        public bool Intersected;

        public bool AllowCollisionTesting(int pairId, int childA, int childB) => true;

        public void OnChildPairCompleted(int pairId, int childA, int childB, ref ConvexContactManifold manifold) { }

        public void OnPairCompleted<TManifold>(int pairId, ref TManifold manifold) where TManifold : unmanaged, IContactManifold<TManifold>
        {
            for (int i = 0; i < manifold.Count; ++i)
            {
                if (manifold.GetDepth(i) >= 0)
                {
                    Intersected = true;
                    return;
                }
            }
        }
    }

    /// <summary>
    /// Tests whether to shapes touch.
    /// </summary>
    /// <remarks>This is a simple unbatched test. It will be much slower than running many tests in vectorized bundles, but shows how it could work conceptually.</remarks>
    /// <typeparam name="TShapeA">Type of the first shape to test.</typeparam>
    /// <typeparam name="TShapeB">Type of the second shape to test.</typeparam>
    /// <typeparam name="TCallbacks">Type of the callbacks to use for the collision tests.</typeparam>
    /// <param name="a">First shape to test.</param>
    /// <param name="poseA">Pose of the first shape to test.</param>
    /// <param name="b">Second shape to test.</param>
    /// <param name="poseB">Pose of the second shape to test.</param>
    /// <param name="shapes">Shapes collection to look up child shapes. Needed for compounds.</param>
    /// <param name="collisionTaskRegistry">Collision task registry defining collision tasks for type pairs.</param>
    /// <returns>True if the pair touches, false otherwise.</returns>
    public unsafe bool PairTouches<TShapeA, TShapeB>(TShapeA a, RigidPose poseA, TShapeB b, RigidPose poseB, Shapes shapes, CollisionTaskRegistry collisionTaskRegistry)
       where TShapeA : unmanaged, IShape
       where TShapeB : unmanaged, IShape
    {
        var collisionBatcher = new CollisionBatcher<BooleanCallbacks>(BufferPool, shapes, collisionTaskRegistry, 0, new BooleanCallbacks());
        collisionBatcher.Add(TShapeA.TypeId, TShapeB.TypeId, sizeof(TShapeA), sizeof(TShapeB), &a, &b, poseB.Position - poseA.Position, poseA.Orientation, poseB.Orientation, 0, 0);
        collisionBatcher.Flush();
        return collisionBatcher.Callbacks.Intersected;
    }

public struct CollisionQuery(Simulation simulation)
{
    private const float SweepMinimumProgression = 1e-1f;
    private const float SweepConvergenceThreshold = 1e-5f;
    private const int SweepMaximumIterationCount = 1;

    public float BroadPhaseSearchRadius { get; init; } = 1;
    public CollidableMobility TargetMobility { get; init; } = CollidableMobility.Dynamic;

    public IEnumerable<BodyReference> GetCollisionsAtDirection(
        BodyReference body,
        Vector3 translation
    )
    {
        var enumerator = new BroadPhaseOverlapEnumerator(simulation)
        {
            References = new QuickList<CollidableReference>()
        };

        GetBodiesWithinRadius(body.Pose.Position, ref enumerator);

        var shape = simulation.Shapes.GetShape<Box>(body.Collidable.Shape.Index);

        foreach (var reference in enumerator.References)
        {
            if (reference.Mobility != TargetMobility || reference.BodyHandle == body.Handle)
                continue;

            var otherBody = simulation.Bodies.GetBodyReference(reference.BodyHandle);
            var otherShapeType = otherBody.Collidable.Shape;
            var otherShape = simulation.Shapes.GetShape<Box>(otherShapeType.Index);

            if (WillOverlap(body, ref shape, otherBody, ref otherShape, translation))
            {
                yield return otherBody;
            }
        }

        enumerator.Dispose();
    }

    private void GetBodiesWithinRadius(
        in Vector3 offset,
        scoped ref BroadPhaseOverlapEnumerator enumerator
    )
    {
        var area = new Sphere(BroadPhaseSearchRadius);

        area.ComputeBounds(Quaternion.Identity, out var min, out var max);
        min += offset;
        max += offset;

        simulation.BroadPhase.GetOverlaps(min, max, ref enumerator);
    }

    private bool WillOverlap(
        in BodyReference body1,
        ref Box shape1,
        in BodyReference body2,
        ref Box shape2,
        in Vector3 translation
    )
    {
        unsafe
        {
            var task = simulation.NarrowPhase.SweepTaskRegistry.GetTask(Box.TypeId, Box.TypeId);
            var filter = Filters.TrueForAll;

            if (task is null)
                return false;

            var expectedPosition = body1.Pose.Position + translation;
            var target = body2.Pose.Position - expectedPosition;

            var overlaps = task.Sweep(
                shapeDataA: Unsafe.AsPointer(ref shape1),
                shapeTypeA: Box.TypeId,
                orientationA: body1.Pose.Orientation,
                velocityA: default,
                shapeDataB: Unsafe.AsPointer(ref shape2),
                shapeTypeB: Box.TypeId,
                offsetB: target,
                orientationB: body2.Pose.Orientation,
                velocityB: default,
                maximumT: 0,
                minimumProgression: SweepMinimumProgression,
                convergenceThreshold: SweepConvergenceThreshold,
                maximumIterationCount: SweepMaximumIterationCount,
                filter: ref filter,
                shapes: simulation.Shapes,
                sweepTasks: simulation.NarrowPhase.SweepTaskRegistry,
                pool: simulation.BufferPool,
                t0: out _,
                t1: out _,
                hitLocation: out _,
                hitNormal: out _
            );

            return overlaps;
        }
    }
}