2D small-step XPBD strip implementation with bending constraint for Unity.

PbdStrip.cs

using System.Runtime.InteropServices;
using System.Threading;
using Unity.Burst;
using Unity.Collections.LowLevel.Unsafe;
using Unity.Jobs;
using Unity.Mathematics;
using UnityEngine;

namespace Spring2D
{
    /// <summary>
    /// 2D small-step XPBD strip implementation.
    /// </summary>
    public class PbdStrip
    {
        /// <summary>
        /// Points of this strip.
        /// You can freely modify properties of the points to interact with external physics elements.
        /// </summary>
        public readonly PbdPoint[] points = default;

        private int isInUpdate = 0;

        public PbdStrip(int numPoints)
        {
            this.points = new PbdPoint[numPoints];
        }

        /// <summary>
        /// Set current distances between points as default for stretch constraint.
        /// </summary>
        public void SetStretchDefault()
        {
            for (int i = 0; i < points.Length - 1; i++)
            {
                ref var p0 = ref points[i];
                ref var p1 = ref points[i + 1];

                p0.stretchRestLength = (float)math.distance(p0.position, p1.position);
            }
        }

        /// <summary>
        /// Set current angles between edges as default for bending constraint.
        /// </summary>
        public void SetBendingDefault()
        {
            for (int i = 1; i < points.Length - 1; i++)
            {
                ref var p0 = ref points[i - 1];
                ref var p1 = ref points[i];
                ref var p2 = ref points[i + 1];

                var pdn0 = math.normalize(p0.position - p1.position);
                var pdn1 = math.normalize(p2.position - p1.position);

                var angle = math.acos(math.dot(pdn0, pdn1));
                var direction = (pdn0.x * pdn1.y - pdn0.y * pdn1.x) > 0;
                if (direction) angle = math.PI * 2.0 - angle;

                p1.bendingRestAngle = (float)angle;
            }
        }

        /// <summary>
        /// Update points.
        /// This method is blocking and executed on current thread.
        /// </summary>
        /// <param name="dt">delta time</param>
        /// <param name="numSubsteps">number of substeps of small-step XPBD</param>
        public void Update(float dt, int numSubsteps)
        {
            unsafe
            {
                var points = this.points;
                fixed (PbdPoint* ptrPoints = &points[0])
                {
                    UpdatePbdBurst(ptrPoints, points.Length, dt, numSubsteps);
                }
            }
        }

        /// <summary>
        /// Schedule update of points.
        /// This method only shcedules jobs and they are executed on a worker thread.
        /// Call UpdateJobHandle.Complete() of the returned handle to complete the job.
        /// </summary>
        /// <param name="dt">delta time</param>
        /// <param name="numSubsteps">number of substeps of small-step XPBD</param>
        /// <param name="handle">a handle of the scheduled job</param>
        /// <returns>returns false when the previous job is not completed.</returns>
        public bool TryUpdateWithJob(float dt, int numSubsteps, out UpdateJobHandle handle)
        {
            if (Interlocked.CompareExchange(ref isInUpdate, 1, 0) == 1)
            {
                // in used
                Debug.LogWarning("Failed to update");
                handle = default;
                return false;
            }

            var points = this.points;

            unsafe
            {
                var ptrHandle = GCHandle.Alloc(points, GCHandleType.Pinned);
                var jobHandle =
                    new PbdUpdateJob((PbdPoint*)ptrHandle.AddrOfPinnedObject(), points.Length, dt, numSubsteps)
                        .Schedule();
                handle = new UpdateJobHandle(this, ptrHandle, jobHandle);
                return true;
            }
        }

        public struct UpdateJobHandle
        {
            private readonly PbdStrip container;
            private GCHandle ptrHandle;
            private JobHandle jobHandle;

            public UpdateJobHandle(PbdStrip container, GCHandle ptrHandle, JobHandle jobHandle)
            {
                this.container = container;
                this.ptrHandle = ptrHandle;
                this.jobHandle = jobHandle;
            }

            public void Complete()
            {
                if (container == null) return;
                jobHandle.Complete();
                ptrHandle.Free();
                Interlocked.Decrement(ref container.isInUpdate);
            }
        }

        [BurstCompile]
        private readonly unsafe struct PbdUpdateJob : IJob
        {
            private readonly bool isValid;

            [NativeDisableUnsafePtrRestriction] private readonly PbdPoint* points;
            private readonly int numPoints;
            private readonly float globalDt;
            private readonly int numSubsteps;

            public PbdUpdateJob(PbdPoint* points, int numPoints, float globalDt, int numSubsteps)
            {
                this.isValid = true;
                this.points = points;
                this.numPoints = numPoints;
                this.globalDt = globalDt;
                this.numSubsteps = numSubsteps;
            }

            public void Execute()
            {
                if (!isValid) return;
                UpdatePbdCore(this.points, this.numPoints, this.globalDt, this.numSubsteps);
            }
        }

        [BurstCompile]
        private static unsafe void UpdatePbdBurst([NoAlias] PbdPoint* points, int numPoints, float globalDt,
            int numSubsteps)
        {
            UpdatePbdCore(points, numPoints, globalDt, numSubsteps);
        }

        private static unsafe void UpdatePbdNonBurst(PbdPoint* points, int numPoints, float globalDt, int numSubsteps)
        {
            UpdatePbdCore(points, numPoints, globalDt, numSubsteps);
        }

        private static unsafe void UpdatePbdCore([NoAlias] PbdPoint* points, int numPoints, float globalDt,
            int numSubsteps)
        {
            double stepDt = (double)globalDt / numSubsteps;

            for (int step = 0; step < numSubsteps; step++)
            {
                for (int i = 0; i < numPoints; i++)
                {
                    ref var p = ref points[i];

                    p.velocity += (double2)p.force * (stepDt * p.MassInverse);

                    if (p.isFixed) p.velocity = double2.zero;

                    p.prevPosition = p.position;
                    p.position += p.velocity * stepDt;
                }

                // apply constraint
                StretchConstraint(points, numPoints, stepDt);
                BendingConstraint(points, numPoints, stepDt);

                for (int i = 0; i < numPoints; i++)
                {
                    ref var p = ref points[i];

                    p.velocity = (p.position - p.prevPosition) / stepDt;

                    // solve velocity
                    p.velocity -= p.velocity * math.min(1.0, p.generalDamp * stepDt * p.MassInverse);
                }

                StretchConstraintSolveVelocity(points, numPoints, stepDt);
            }
        }

        private static unsafe void StretchConstraint(PbdPoint* points, int numPoints, double dt)
        {
            for (int i = 0; i < numPoints - 1; i++)
            {
                ref var p0 = ref points[i];
                ref var p1 = ref points[i + 1];

                if (math.isinf(p0.stretchCompliance)) continue;

                var alpha = p0.stretchCompliance / (dt * dt);

                var w = p0.MassInverse + p1.MassInverse;

                var diff = p1.position - p0.position;
                var length = math.length(diff);
                var grad = diff / length;

                var c = length - p0.stretchRestLength;

                var dLambda = c / (w + alpha);
                var v = grad * dLambda;

                p0.position += v * p0.MassInverse;
                p1.position -= v * p1.MassInverse;
            }
        }

        private static unsafe void BendingConstraint(PbdPoint* points, int numPoints, double dt)
        {
            for (int i = 1; i < numPoints - 1; i++)
            {
                ref var p0 = ref points[i - 1];
                ref var p1 = ref points[i];
                ref var p2 = ref points[i + 1];

                if (math.isinf(p1.bendingCompliance)) continue;

                var pd0 = p0.position - p1.position;
                var pd1 = p2.position - p1.position;

                var l0 = math.length(pd0);
                var l1 = math.length(pd1);

                var pdn0 = pd0 / l0;
                var pdn1 = pd1 / l1;

                var d = math.dot(pdn0, pdn1);
                var acosd2 = 1 - d * d;
                var acosd = math.sqrt(acosd2);

                var actualAngle = math.acos(d);
                
                // workaround for flipping problem of bending constraint
                // see also: https://youtu.be/B-Bakl-VPpg?si=0yv8XqPQFv5TUqBU&t=433
                var direction = (pdn0.x * pdn1.y - pdn0.y * pdn1.x) > 0;

#if SPRING2D_DEBUG
                {
                    var nd0 = math.double2(pd0.y, -pd0.x);
                    var nd1 = math.double2(pd1.y, -pd1.x);

                    var n0 = nd0 / l0;
                    var n1 = nd1 / l1;
                    p1.debugInfo.normal0 = n0;
                    p1.debugInfo.normal1 = n1;
                    p1.debugInfo.direction = direction;
                }
#endif
                if (direction) actualAngle = math.PI * 2.0 - actualAngle;

                var constraint = actualAngle - p1.bendingRestAngle;

                var grad0 = (pd1 - math.dot(pd0, pd1) * pd0 / (l0 * l0)) / (l0 * l1);
                var grad2 = (pd0 - math.dot(pd0, pd1) * pd1 / (l1 * l1)) / (l0 * l1);

                var grad1 = 0.0 - grad0 - grad2;

                var alpha = p1.bendingCompliance / (dt * dt);

                var s = constraint * acosd / (
                    math.lengthsq(grad0) * p0.MassInverse +
                    math.lengthsq(grad1) * p1.MassInverse +
                    math.lengthsq(grad2) * p2.MassInverse +
                    alpha * acosd2
                );

                if (direction) s = -s;

                // TODO: grad will be zero and s will be NaN when actualAngle equals to PI
                if (math.isnan(s)) return;

                p0.position += s * p0.MassInverse * grad0;
                p1.position += s * p1.MassInverse * grad1;
                p2.position += s * p2.MassInverse * grad2;
            }
        }

        private static unsafe void StretchConstraintSolveVelocity(PbdPoint* points, int numPoints, double dt)
        {
            for (int i = 0; i < numPoints - 1; i++)
            {
                ref var p0 = ref points[i];
                ref var p1 = ref points[i + 1];

                var distanceDamp = p0.stretchDamp;

                var n = math.normalize(p1.position - p0.position);
                var v0 = math.dot(n, p0.velocity);
                var v1 = math.dot(n, p1.velocity);
                var dv0 = (v1 - v0) * math.min(0.5, distanceDamp * dt * p0.MassInverse);
                var dv1 = (v0 - v1) * math.min(0.5, distanceDamp * dt * p1.MassInverse);

                p0.velocity += n * dv0;
                p1.velocity += n * dv1;
            }
        }
    }

    public struct PbdPoint
    {
        public float MassInverse => isFixed ? 0f : 1f / mass;
        private readonly float massInverse;
        public float mass;
        
        public double2 position;
        internal double2 prevPosition;
        
        public float2 force;
        internal double2 velocity;
        
        /// <summary>
        /// is this point fixed.
        /// </summary>
        public bool isFixed;
        
        /// <summary>
        /// damping intensity.
        /// set appropriate value to stabilize position calculation.
        /// </summary>
        public float generalDamp;
        
        /// <summary>
        /// damping intensity along stretch axis.
        /// set appropriate value to stabilize position calculation.
        /// </summary>
        public float stretchDamp;
        
        /// <summary>
        /// softness of stretch constraint.
        /// see: https://blog.mmacklin.com/2016/10/12/xpbd-slides-and-stiffness/
        /// </summary>
        public double stretchCompliance;
        
        /// <summary>
        /// softness of bending constraint.
        /// </summary>
        public double bendingCompliance;

        /// <summary>
        /// default length of the edge between this point and next point. 
        /// </summary>
        public float stretchRestLength;
        
        /// <summary>
        /// default angle (rad) between the edges around this point. 
        /// </summary>
        public float bendingRestAngle;

#if SPRING2D_DEBUG
        public PbdPointDebugInfo debugInfo;
#endif

        public PbdPoint(Vector2 position, float mass, float generalDamp, float stretchDamp, double stretchCompliance,
            double bendingCompliance)
        {
            this = default;
            this.position = new double2(position);
            this.mass = mass;
            this.generalDamp = generalDamp;
            this.stretchDamp = stretchDamp;
            this.stretchCompliance = stretchCompliance;
            this.bendingCompliance = bendingCompliance;
        }
    }
    
#if SPRING2D_DEBUG
    public struct PbdPointDebugInfo
    {
        public double2 normal0;
        public double2 normal1;
        public bool direction;
    }
#endif
}