fgm24/Assets/Scripts/Rope/RopeSimulator.cs

using System;
using System.Collections;
using System.Collections.Generic;
using System.Linq;
using Unity.VisualScripting;
using UnityEditor.ShaderGraph.Drawing;
using UnityEngine;
using UnityEngine.Analytics;
using UnityEngine.Rendering;
using UnityUtils;

public class RopeSimulator : MonoBehaviour
{
    [SerializeField]
    private float gravity = 10;

    [SerializeField]
    private int solveIterations = 10;

    [SerializeField]
    private bool constrainStickMinLength;

    [SerializeField]
    Transform start, end;

    [SerializeField]
    int subDivision = 50;

    [SerializeField]
    float collisionCheckDist = 0.5f;

    [SerializeField, Range(0f, 1f)]
    float distBetweenRopePoints = 0.1f;

    [SerializeField, Range(0.01f, 1f)]
    float ropeRadius;

    [SerializeField]
    float ignoreResolveThreshold = 0.08f;

    [SerializeField]
    Transform ropeCollidersParent;

    [SerializeField]
    LayerMask staticColliderMask;

    [SerializeField, Range(0f, 100f)]
    float pullForce = 20f;

    [SerializeField]
    float xyGravityDampScalor = 1f;

    [Header("Rendering")]
    [SerializeField] LineRenderer lineRenderer;

    int[] order;

    public Vector2 testPos;

    Rope rope;

    Dictionary<Collider2D, float> colliderToSquezeForce = new();

    private void Start()
    {
        //rope = new RopeBuilder()
        //    .AddPoint(new Point(testPos, locked: true))
        //    .AddPoint(new Point(testPos.Add(x:5f)))
        //    .AddPoint(new Point(testPos.Add(x: 10f)))
        //    .AddPoint(new Point(testPos.Add(x: 15f)))
        //    .AddPoint(new Point(testPos.Add(x: 20f)))
        //    .ConnectPoints(0, 1)
        //    .ConnectPoints(1, 2)
        //    .ConnectPoints(2, 3)
        //    .ConnectPoints(3, 4)
        //    .Build();
        RopeBuilder builder = new RopeBuilder();
        builder.AddPoint(new Point(start.position, locked: true));
        for (int i = 1; i < subDivision; i++)
        {
            Vector3 pointPos = Vector3.Lerp(start.position, end.position, (float)i / (float)subDivision);
            //Debug.Log($"pos: {pointPos}, t={i / subDivision}");
            Debug.DrawRay(pointPos, (end.position - start.position).normalized);
            builder.AddPoint(new Point(pointPos));
        }

        builder.AddPoint(new Point(end.position, locked: true));

        for (int i = 0; i < subDivision; i++)
        {
            builder.ConnectPointsWithDesiredLength(i, i + 1, desiredLength: distBetweenRopePoints);
        }
        rope = builder.Build();

        foreach (var point in rope.points)
        {
            GameObject ropeCollider = new GameObject("Rope Collider");
            ropeCollider.transform.parent = ropeCollidersParent;
            ropeCollider.transform.position = point.position;
            ropeCollider.layer = LayerMask.NameToLayer("Rope");

            var colliderComponent = ropeCollider.AddComponent<CircleCollider2D>();
            colliderComponent.radius = ropeRadius;
        }
        CreateOrderArray();
    }

    private void Update()
    {
        colliderToSquezeForce.Clear();

        rope.points.First().position = start.position;
        rope.points.Last().position = end.position;

        Simulate();

        // Update the rope collider positions
        for (int i = 0; i < rope.points.Count; i++)
        {
            ropeCollidersParent.GetChild(i).position = rope.points[i].position;
        }

        // Handle static colliders
        foreach (var point in rope.points)
        {
            if (point.locked) continue;

            HandleStaticCollidersOfPoint(point);
        }

        // Constrain start transform based on overshoot
        float overshoot = rope.CalculateLengthOvershoot();
        if (overshoot > 0)
        {
            //start.position = prevStartPos;
            Vector2 pullDirection = (rope.points.ElementAt(1).position - start.position).normalized;
            start.gameObject.GetComponent<Rigidbody2D>().AddForce(pullDirection * overshoot * pullForce);
        }

        // Handle squeze kills
        foreach (var collider in colliderToSquezeForce)
        {
            ISquezeDamageReceiver squezeDamageReceiver = collider.Key.GetComponent<ISquezeDamageReceiver>();
            if (squezeDamageReceiver == null) continue;

            squezeDamageReceiver.TakeSquezeDamage(collider.Value);
        }

        // Update line renderer
        var positions = rope.points.Select(p => new Vector3(p.position.x, p.position.y, 0f)).ToArray();
        lineRenderer.positionCount = positions.Length;
        lineRenderer.SetPositions(positions);

        // Handle xy dampening on z gravity
        foreach (var point in rope.points)
        {
            if (point.position.z >= 0f) continue;

            Vector2 newXYPos = Vector2.MoveTowards(new Vector2(point.position.x, point.position.y), new Vector2(point.prevPosition.x, point.prevPosition.y), Mathf.Abs(point.position.z * xyGravityDampScalor));
            point.position.Set(newXYPos.x, newXYPos.y, 0f);
        }
    }

    private void OnDrawGizmos()
    {
        if (!Application.isPlaying) return;

        foreach (var point in rope.points)
        {
            //Debug.Log($"pos: {point.position}");
            Gizmos.DrawSphere(point.position, ropeRadius);
        }
    }

    void Simulate()
    {
        foreach (Point p in rope.points)
        {
            if (!p.locked)
            {
                Vector3 positionBeforeUpdate = p.position;
                p.position += p.position - p.prevPosition;
                p.position.z -= gravity * Time.deltaTime * Time.deltaTime;
                p.prevPosition = positionBeforeUpdate;
            }
        }

        for (int i = 0; i < solveIterations; i++)
        {
            for (int s = 0; s < rope.sticks.Count; s++)
            {
                Stick stick = rope.sticks[order[s]];
                if (stick.dead)
                {
                    continue;
                }

                Vector3 stickCentre = (stick.A.position + stick.B.position) / 2;
                Vector3 stickDir = (stick.A.position - stick.B.position).normalized;
                float length = (stick.A.position - stick.B.position).magnitude;

                if (length > stick.desiredLength || constrainStickMinLength)
                {
                    if (!stick.A.locked)
                    {
                        TryMovePointToPosition(stick.A, stickCentre + stickDir * stick.desiredLength / 2);
                    }
                    if (!stick.B.locked)
                    {
                        TryMovePointToPosition(stick.B, stickCentre - stickDir * stick.desiredLength / 2);
                    }
                }
            }
        }
    }

    private void TryMovePointToPosition(Point point, Vector3 position)
    {
        Vector2 moveDir = new Vector2(position.x, position.y) - new Vector2(point.position.x, point.position.y);
        int stepsRequired = (int) Mathf.Ceil(moveDir.magnitude / collisionCheckDist);
        moveDir.Normalize();

        Vector2 initialPos = new Vector2(point.position.x, point.position.y);
        for (int i = 0 ; i < stepsRequired; i++)
        {
            Vector2 newPos = Vector2.MoveTowards(new Vector2(point.position.x, point.position.y), new Vector2(position.x, position.y), collisionCheckDist);
            point.position.Set(newPos.x, newPos.y, point.position.z); 
            Collider2D collider = Physics2D.OverlapCircle(point.position, ropeRadius, staticColliderMask);
            if (collider == null) continue;

            // A static collider was met, dont move any further
            Vector2 resolvedPos = collider.ClosestPoint(initialPos);

            if (Vector2.Distance(initialPos, resolvedPos) < ignoreResolveThreshold) continue;

            Vector2 penetrationDir = (resolvedPos - new Vector2(point.position.x, point.position.y)).normalized;
            Vector2 finalPos = resolvedPos - penetrationDir * ropeRadius;
            //Debug.Log($"resolved pos: {point.position}->{finalPos}");
            point.position.Set(finalPos.x, finalPos.y, point.position.z);
            break;
        }

        // Move z position
        point.position.z = position.z;
    }

    private void HandleStaticCollidersOfPoint(Point p)
    {
        Collider2D hitCollider = Physics2D.OverlapCircle(p.position, ropeRadius, staticColliderMask);
        if (hitCollider == null) return;

        // Register the squeze force this rope particle is squezing the collider
        Vector2 pointPos = new Vector2(p.position.x, p.position.y);
        Vector2 resolvedPos = hitCollider.ClosestPoint(pointPos);
        Vector2 penetration = resolvedPos - pointPos;
        Vector2 finalPos = resolvedPos - penetration.normalized * ropeRadius;

        float squezeForce;
        if (!colliderToSquezeForce.TryGetValue(hitCollider, out squezeForce))
            colliderToSquezeForce.Add(hitCollider, squezeForce + penetration.magnitude);
        else
            colliderToSquezeForce[hitCollider] = squezeForce + penetration.magnitude;

        p.position.Set(finalPos.x, finalPos.y, p.position.z);
    }

    void CreateOrderArray()
    {
        order = new int[rope.sticks.Count];
        for (int i = 0; i < order.Length; i++)
        {
            order[i] = i;
        }
        ShuffleArray(order, new System.Random());
    }

    public static T[] ShuffleArray<T>(T[] array, System.Random prng)
    {

        int elementsRemainingToShuffle = array.Length;
        int randomIndex = 0;

        while (elementsRemainingToShuffle > 1)
        {

            // Choose a random element from array
            randomIndex = prng.Next(0, elementsRemainingToShuffle);
            T chosenElement = array[randomIndex];

            // Swap the randomly chosen element with the last unshuffled element in the array
            elementsRemainingToShuffle--;
            array[randomIndex] = array[elementsRemainingToShuffle];
            array[elementsRemainingToShuffle] = chosenElement;
        }

        return array;
    }
}