Per-body gravity

[Grimelios]

Hi! While integrating velocity (via the IntegrateVelocity function of IPoseIntegratorCallbacks), I'd like to selectively apply gravity or not based on a boolean associated with each body (e.g. a bird that's marked to ignore gravity). Using 2.4.0, the signature includes Vector<int> bodyIndices, so I'm hoping there's an efficient way to use those indices to pull associated booleans. So far, this is my best guess:

for (var i = 0; i < Vector<int>.Count; i++)
{
    // Where bodiesUsingGravity is either Dictionary<int, bool> or, ideally, an array.
    var usesGravity = bodiesUsingGravity[bodyIndices[i]];
}

There are a few aspects that confuse me:

1. As stated in the comment, bodiesUsingGravity could be a dictionary, but doing so feels counter to the optimized, low-level approach Bepu takes. Feels more correct to use an array, but then I'm unsure how to actually build that mapping. I also found https://forum.bepuentertainment.com/viewtopic.php?t=2602, which states that each index refers to the active set. Unfortunately, I don't know enough about Bepu's inner workings to know whether that's still accurate or how to code using that knowledge.

2. How do I keep my mapping accurate as bodies are added and removed to/from the simulation?

3. My (albeit shallow) understanding of Vector is that it's designed for hardware optimization. Explicitly looping over bodyIndices feels like I'll wreck that performance gain. Hard to know since that kind of optimization isn't my area of expertise.

Hoping to get some clarification on this, or at least gain confidence that I'm on the right track. Thank you!

[RossNordby]

Just added a PerBodyGravityDemo in a703e19.

Feels more correct to use an array, but then I'm unsure how to actually build that mapping.
...
How do I keep my mapping accurate as bodies are added and removed to/from the simulation?

It's pretty annoying to try to keep an active set aligned data store up to date with potential memory moves caused by removals or sleeping. I usually just recommend instead storing data aligned with the internal values of the BodyHandle, like in the CollidableProperty<T>. The demo above uses the active body index to look up the handle, and from there, the per-body gravity value.

Explicitly looping over bodyIndices feels like I'll wreck that performance gain.

You're correct that the benefits of vectorization are mostly thrown out the window when the lanes are enumerated, but the impact in the velocity integration callback is not that big. For the demo linked above on my CPU, it's 50-100 microseconds of total overhead. The callback is vectorized primarily because the calling context is vectorized, and I didn't want to introduce mandatory overhead for all simulations.

The demo above also stores more data than your use case technically requires. If you just want a boolean version, you could do something like this:

using BepuPhysics;
using BepuPhysics.Collidables;
using BepuPhysics.Constraints;
using BepuUtilities;
using BepuUtilities.Collections;
using DemoContentLoader;
using DemoRenderer;
using System;
using System.Numerics;

namespace Demos.Demos;

/// <summary>
/// Shows how to use custom velocity integration to implement per-body boolean gravity.
/// </summary>
public class BooleanBodyGravityDemo : Demo
{
    struct BooleanGravityCallbacks : IPoseIntegratorCallbacks
    {
        /// <summary>
        /// The IndexSet stores one bit per index. In these callbacks, we'll index into it using BodyHandles, and the bit will represent whether the body should have gravity.
        /// </summary>
        public IndexSet BodyRequiresGravity;
        /// <summary>
        /// Used to look up body handles using the callback-provided body indices.
        /// </summary>
        private Bodies bodies;

        public readonly AngularIntegrationMode AngularIntegrationMode => AngularIntegrationMode.Nonconserving;

        public readonly bool AllowSubstepsForUnconstrainedBodies => false;

        public readonly bool IntegrateVelocityForKinematics => false;

        public void Initialize(Simulation simulation)
        {
            bodies = simulation.Bodies;
        }

        public void PrepareForIntegration(float dt) { }

        public void IntegrateVelocity(Vector<int> bodyIndices, Vector3Wide position, QuaternionWide orientation, BodyInertiaWide localInertia, Vector<int> integrationMask, int workerIndex, Vector<float> dt, ref BodyVelocityWide velocity)
        {
            //Similar to the PerBodyGravityDemo, but collecting a mask from packed bitfields instead.                    
            Span<int> shouldUseGravityMask = stackalloc int[Vector<float>.Count];
            for (int bundleSlotIndex = 0; bundleSlotIndex < Vector<int>.Count; ++bundleSlotIndex)
            {
                var bodyIndex = bodyIndices[bundleSlotIndex];
                //Not every slot in the SIMD vector is guaranteed to be filled.
                if (bodyIndex >= 0)
                {
                    var bodyHandle = bodies.ActiveSet.IndexToHandle[bodyIndex];
                    shouldUseGravityMask[bundleSlotIndex] = BodyRequiresGravity.Contains(bodyHandle.Value) ? -1 : 0;
                }
            }
            //Gravity mask is used to select whether to apply gravity on a per-lane basis.
            velocity.Linear.Y += Vector.ConditionalSelect(new Vector<int>(shouldUseGravityMask), new Vector<float>(-10), Vector<float>.Zero) * dt;
        }
    }

    public unsafe override void Initialize(ContentArchive content, Camera camera)
    {
        camera.Position = new Vector3(0, 20, 80);
        camera.Yaw = 0;
        camera.Pitch = 0;

        //IndexSet is a struct, so we pass it in and then grab a reference to the simulation-stored version for later mutation.
        //Attempting to cache the IndexSet as a local variable would just modify the local variable and things would break.
        Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new BooleanGravityCallbacks() { BodyRequiresGravity = new IndexSet(BufferPool, 8192) }, new SolveDescription(4, 1));
        ref var bodyGravities = ref (Simulation.PoseIntegrator as PoseIntegrator<BooleanGravityCallbacks>).Callbacks.BodyRequiresGravity;
        Simulation.Statics.Add(new StaticDescription(new Vector3(), Simulation.Shapes.Add(new Box(1000, 10, 1000))));

        var sphereShape = new Sphere(1f);
        var sphereInertia = sphereShape.ComputeInertia(1);
        var sphereShapeIndex = Simulation.Shapes.Add(sphereShape);
        var capsuleShape = new Capsule(1f, 1f);
        var capsuleInertia = capsuleShape.ComputeInertia(1);
        var capsuleShapeIndex = Simulation.Shapes.Add(capsuleShape);
        var boxShape = new Box(1f, 1f, 1f);
        var boxInertia = boxShape.ComputeInertia(1);
        var boxShapeIndex = Simulation.Shapes.Add(boxShape);
        var spacing = new Vector3(4);
        const int length = 20;
        for (int i = 0; i < length; ++i)
        {
            for (int j = 0; j < 20; ++j)
            {
                const int width = 20;
                var origin = new Vector3(0, 40, 0) + spacing * new Vector3(length * -0.5f, 0, width * -0.5f);
                for (int k = 0; k < width; ++k)
                {
                    BodyInertia inertia;
                    TypedIndex shapeIndex;
                    switch ((i + k) % 3)
                    {
                        case 0:
                            inertia = sphereInertia;
                            shapeIndex = sphereShapeIndex;
                            break;
                        case 1:
                            inertia = capsuleInertia;
                            shapeIndex = capsuleShapeIndex;
                            break;
                        default:
                            inertia = boxInertia;
                            shapeIndex = boxShapeIndex;
                            break;
                    }

                    var bodyHandle = Simulation.Bodies.Add(BodyDescription.CreateDynamic(
                        origin + new Vector3(i, j, k) * spacing, new Vector3(0, 0, 0), inertia, shapeIndex, 0.001f));
                    if (i % 2 == 0)
                        bodyGravities.Add(bodyHandle.Value, BufferPool);
                }
            }
        }

    }
}

[Grimelios]

Thanks so much for the quick response! That's exactly what I was looking for.