Remove constraint while retaining other constraint references

[Sardelka9515]

Is there a way to remove a constraint without invalidating other handles, or a way to get updated handles?

[RossNordby]

Not sure I understand; removing a constraint does not invalidate other handles. It does invalidate any outstanding handles to the removed constraint, and it does potentially invalidate the direct memory location of other constraints that you'd get if you did the handle->location lookup, but a constraint's handle itself is immutable for the lifespan of the constraint in the simulation.

[Sardelka9515]

Ah, sorry. It's indeed not invalidated. But Simulation.Solver.ConstraintExists doesn't seem to return the correct result, so it fails when i try to apply a new description.

Tracking the constraint handles manually does fix the error

[RossNordby]

Presumably it returns true for a constraint that was removed? That is an annoying byproduct of the fact that handles are not currently versioned. If a constraint is removed and another unrelated constraint is added, it's possible for the other unrelated constraint to now have the handle that was previously associated with the removed constraint, and the handle will "exist" in the same way a dangling pointer that is reused "exists."

I intend to version the handles eventually; no strong reason that they've haven't been other than time.

[Sardelka9515]

Presumably it returns true for a constraint that was removed?

Indeed, but it seems a bit more complicated because it still returns wrong value when no new constraints are added. I used another hashset to track handles and it gave me inconsistent result with ConstraintExists. what's more intriguing is that it doesn't necessarily happen immediately after the constraint is broken but after a few other steps.

Although it might be caused by other parts of my bad code.💩

I'll open another issue if I managed to reproduce it in a more simplified simulation.

[Sardelka9515]

I finally managed to reproduce it, below is the full code:

using BepuPhysics;
using BepuPhysics.Collidables;
using BepuPhysics.CollisionDetection;
using BepuPhysics.Constraints;
using BepuUtilities;
using BepuUtilities.Memory;
using System.Diagnostics;
using System.Numerics;
using System.Runtime.CompilerServices;
namespace BepuTest
{
    public struct DemoPoseIntegratorCallbacks : IPoseIntegratorCallbacks
    {
        /// <summary>
        /// Gravity to apply to dynamic bodies in the simulation.
        /// </summary>
        public Vector3 Gravity;
        /// <summary>
        /// Fraction of dynamic body linear velocity to remove per unit of time. Values range from 0 to 1. 0 is fully undamped, while values very close to 1 will remove most velocity.
        /// </summary>
        public float LinearDamping;
        /// <summary>
        /// Fraction of dynamic body angular velocity to remove per unit of time. Values range from 0 to 1. 0 is fully undamped, while values very close to 1 will remove most velocity.
        /// </summary>
        public float AngularDamping;

        /// <summary>
        /// Gets how the pose integrator should handle angular velocity integration.
        /// </summary>
        public readonly AngularIntegrationMode AngularIntegrationMode => AngularIntegrationMode.Nonconserving;

        /// <summary>
        /// Gets whether the integrator should use substepping for unconstrained bodies when using a substepping solver.
        /// If true, unconstrained bodies will be integrated with the same number of substeps as the constrained bodies in the solver.
        /// If false, unconstrained bodies use a single step of length equal to the dt provided to Simulation.Timestep. 
        /// </summary>
        public readonly bool AllowSubstepsForUnconstrainedBodies => false;

        /// <summary>
        /// Gets whether the velocity integration callback should be called for kinematic bodies.
        /// If true, IntegrateVelocity will be called for bundles including kinematic bodies.
        /// If false, kinematic bodies will just continue using whatever velocity they have set.
        /// Most use cases should set this to false.
        /// </summary>
        public readonly bool IntegrateVelocityForKinematics => false;

        public void Initialize(Simulation simulation)
        {
            //In this demo, we don't need to initialize anything.
            //If you had a simulation with per body gravity stored in a CollidableProperty<T> or something similar, having the simulation provided in a callback can be helpful.
        }

        /// <summary>
        /// Creates a new set of simple callbacks for the demos.
        /// </summary>
        /// <param name="gravity">Gravity to apply to dynamic bodies in the simulation.</param>
        /// <param name="linearDamping">Fraction of dynamic body linear velocity to remove per unit of time. Values range from 0 to 1. 0 is fully undamped, while values very close to 1 will remove most velocity.</param>
        /// <param name="angularDamping">Fraction of dynamic body angular velocity to remove per unit of time. Values range from 0 to 1. 0 is fully undamped, while values very close to 1 will remove most velocity.</param>
        public DemoPoseIntegratorCallbacks(Vector3 gravity, float linearDamping = .03f, float angularDamping = .03f) : this()
        {
            Gravity = gravity;
            LinearDamping = linearDamping;
            AngularDamping = angularDamping;
        }

        Vector3Wide gravityWideDt;
        Vector<float> linearDampingDt;
        Vector<float> angularDampingDt;

        /// <summary>
        /// Callback invoked ahead of dispatches that may call into <see cref="IntegrateVelocity"/>.
        /// It may be called more than once with different values over a frame. For example, when performing bounding box prediction, velocity is integrated with a full frame time step duration.
        /// During substepped solves, integration is split into substepCount steps, each with fullFrameDuration / substepCount duration.
        /// The final integration pass for unconstrained bodies may be either fullFrameDuration or fullFrameDuration / substepCount, depending on the value of AllowSubstepsForUnconstrainedBodies. 
        /// </summary>
        /// <param name="dt">Current integration time step duration.</param>
        /// <remarks>This is typically used for precomputing anything expensive that will be used across velocity integration.</remarks>
        public void PrepareForIntegration(float dt)
        {
            //No reason to recalculate gravity * dt for every body; just cache it ahead of time.
            //Since these callbacks don't use per-body damping values, we can precalculate everything.
            linearDampingDt = new Vector<float>(MathF.Pow(MathHelper.Clamp(1 - LinearDamping, 0, 1), dt));
            angularDampingDt = new Vector<float>(MathF.Pow(MathHelper.Clamp(1 - AngularDamping, 0, 1), dt));
            gravityWideDt = Vector3Wide.Broadcast(Gravity * dt);
        }

        /// <summary>
        /// Callback for a bundle of bodies being integrated.
        /// </summary>
        /// <param name="bodyIndices">Indices of the bodies being integrated in this bundle.</param>
        /// <param name="position">Current body positions.</param>
        /// <param name="orientation">Current body orientations.</param>
        /// <param name="localInertia">Body's current local inertia.</param>
        /// <param name="integrationMask">Mask indicating which lanes are active in the bundle. Active lanes will contain 0xFFFFFFFF, inactive lanes will contain 0.</param>
        /// <param name="workerIndex">Index of the worker thread processing this bundle.</param>
        /// <param name="dt">Durations to integrate the velocity over. Can vary over lanes.</param>
        /// <param name="velocity">Velocity of bodies in the bundle. Any changes to lanes which are not active by the integrationMask will be discarded.</param>
        public void IntegrateVelocity(Vector<int> bodyIndices, Vector3Wide position, QuaternionWide orientation, BodyInertiaWide localInertia, Vector<int> integrationMask, int workerIndex, Vector<float> dt, ref BodyVelocityWide velocity)
        {
            //This is a handy spot to implement things like position dependent gravity or per-body damping.
            //This implementation uses a single damping value for all bodies that allows it to be precomputed.
            //We don't have to check for kinematics; IntegrateVelocityForKinematics returns false, so we'll never see them in this callback.
            //Note that these are SIMD operations and "Wide" types. There are Vector<float>.Count lanes of execution being evaluated simultaneously.
            //The types are laid out in array-of-structures-of-arrays (AOSOA) format. That's because this function is frequently called from vectorized contexts within the solver.
            //Transforming to "array of structures" (AOS) format for the callback and then back to AOSOA would involve a lot of overhead, so instead the callback works on the AOSOA representation directly.
            velocity.Linear = (velocity.Linear + gravityWideDt) * linearDampingDt;
            velocity.Angular = velocity.Angular * angularDampingDt;
        }
    }
    public struct DemoNarrowPhaseCallbacks : INarrowPhaseCallbacks
    {
        public SpringSettings ContactSpringiness;
        public float MaximumRecoveryVelocity;
        public float FrictionCoefficient;

        public DemoNarrowPhaseCallbacks(SpringSettings contactSpringiness, float maximumRecoveryVelocity = 2f, float frictionCoefficient = 1f)
        {
            ContactSpringiness = contactSpringiness;
            MaximumRecoveryVelocity = maximumRecoveryVelocity;
            FrictionCoefficient = frictionCoefficient;
        }

        public void Initialize(Simulation simulation)
        {
            //Use a default if the springiness value wasn't initialized... at least until struct field initializers are supported outside of previews.
            if (ContactSpringiness.AngularFrequency == 0 && ContactSpringiness.TwiceDampingRatio == 0)
            {
                ContactSpringiness = new(30, 1);
                MaximumRecoveryVelocity = 2f;
                FrictionCoefficient = 1f;
            }
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b, ref float speculativeMargin)
        {
            //While the engine won't even try creating pairs between statics at all, it will ask about kinematic-kinematic pairs.
            //Those pairs cannot emit constraints since both involved bodies have infinite inertia. Since most of the demos don't need
            //to collect information about kinematic-kinematic pairs, we'll require that at least one of the bodies needs to be dynamic.
            return a.Mobility == CollidableMobility.Dynamic || b.Mobility == CollidableMobility.Dynamic;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB)
        {
            return true;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public bool ConfigureContactManifold<TManifold>(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold<TManifold>
        {
            pairMaterial.FrictionCoefficient = FrictionCoefficient;
            pairMaterial.MaximumRecoveryVelocity = MaximumRecoveryVelocity;
            pairMaterial.SpringSettings = ContactSpringiness;
            return true;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref ConvexContactManifold manifold)
        {
            return true;
        }

        public void Dispose()
        {
        }
    }

    internal class TestBody
    {
        public static readonly Vector3 RightOffset = new(0.51f, -0.015405f, 0.012604f);
        public static readonly Vector3 LeftOffset = new(-0.51f, -0.015405f, 0.012604f);

        public ConstraintHandle RightSocket;
        public ConstraintHandle LeftSocket;
        BallSocket LeftSocketDescription;
        BallSocket RightSocketDescription;

        public ConstraintHandle RightServo;
        public ConstraintHandle LeftServo;
        AngularServo RightServoDescription;
        AngularServo LeftServoDescription;
        public TestBody(Simulation sim, BodyReference RightWeapon, BodyReference LeftWeapon, BodyReference Body)
        {
            RightSocketDescription = new BallSocket()
            {
                SpringSettings = new SpringSettings(60, 1),
                LocalOffsetA = RightOffset,
                LocalOffsetB = new(-0.1f, 0, 0)
            };
            LeftSocketDescription = new BallSocket()
            {
                SpringSettings = new SpringSettings(60, 1),
                LocalOffsetA = LeftOffset,
                LocalOffsetB = new(0.1f, 0, 0)
            };
            RightSocket = sim.Solver.Add(Body, RightWeapon, RightSocketDescription);
            LeftSocket = sim.Solver.Add(Body, LeftWeapon, LeftSocketDescription);

            RightServoDescription = new AngularServo()
            {
                ServoSettings = new(1, 0, 40),
                TargetRelativeRotationLocalA = Quaternion.Identity,
                SpringSettings = new(30, 1)
            };
            LeftServoDescription = RightServoDescription;
            RightServo = sim.Solver.Add(Body, RightWeapon, RightServoDescription);
            LeftServo = sim.Solver.Add(Body, LeftWeapon, LeftServoDescription);
        }
    }
    internal class Program
    {
        static unsafe void Main(string[] args)
        {
            var dt = 1 / 60f;
            var pool = new BufferPool();
            var sim = Simulation.Create(pool, new DemoNarrowPhaseCallbacks(new(60, 1)), new DemoPoseIntegratorCallbacks(default), new SolveDescription(1, 8));
            var bodyShape = new Box(1.01f, 1.5f, 2f);
            var partShape = new Box(0.2f, 1.06f, 1.2f);
            var body = sim.Shapes.Add(bodyShape);
            var part = sim.Shapes.Add(partShape);
            var right = sim.Bodies.Add(BodyDescription.CreateDynamic(new RigidPose(default), partShape.ComputeInertia(10), part, 0.01f));
            var left = sim.Bodies.Add(BodyDescription.CreateDynamic(new RigidPose(default), partShape.ComputeInertia(10), part, 0.01f));
            var center = sim.Bodies.Add(BodyDescription.CreateDynamic(new RigidPose(default), bodyShape.ComputeInertia(200), body, 0.01f));

            var test = new TestBody(sim, sim.Bodies[right], sim.Bodies[left], sim.Bodies[center]);
            for (int i = 0; i < 600; i++)
            {
                sim.Timestep(dt, null);
            }
            sim.Solver.Remove(test.LeftServo);
            sim.Solver.Remove(test.LeftSocket);
            sim.Solver.Remove(test.RightServo);
            sim.Solver.Remove(test.RightSocket);
            int stepped = 0;
            while (true)
            {
                Debug.Assert(!sim.Solver.ConstraintExists(test.LeftServo));
                Debug.Assert(!sim.Solver.ConstraintExists(test.LeftSocket));
                Debug.Assert(!sim.Solver.ConstraintExists(test.RightServo));
                Debug.Assert(!sim.Solver.ConstraintExists(test.RightSocket));
                sim.Timestep(dt, null);
                stepped++;
            }
        }
    }
}

The callbacks are copied from demo. For me the assertion always fails after 600 + 157 steps. @RossNordby could you check if this is worth opening an issue?

[RossNordby]

This appears to be the unversioned handle issue; at the time of the assert, the previously removed constraint handle now points to a two-body convex 3 contact constraint created by collisions.

I did create an issue for versioning, though: #308

[Sardelka9515]

Oops, didn't know that collision could also create new constraints. For now a simple lookup table for versions would do just fine for me.

Thanks for the clarification !