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;

    int[] order;

    public Vector2 testPos;

    Vector2 prevStartPos;

    Rope rope;

    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++)
        {
            Vector2 pointPos = Vector2.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.layer = LayerMask.NameToLayer("Rope");

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

    private void Update()
    {
        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 - new Vector2(start.position.x, start.position.y)).normalized;
            start.gameObject.GetComponent<Rigidbody2D>().AddForce(pullDirection * overshoot * pullForce);
        }

        prevStartPos = start.position;
    }

    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)
            {
                Vector2 positionBeforeUpdate = p.position;
                p.position += p.position - p.prevPosition;
                p.position += Vector2.down * 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;
                }

                Vector2 stickCentre = (stick.A.position + stick.B.position) / 2;
                Vector2 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, Vector2 position)
    {
        Vector2 moveDir = position - point.position;
        int stepsRequired = (int) Mathf.Ceil(moveDir.magnitude / collisionCheckDist);
        moveDir.Normalize();
        Vector2 initialPos = point.position;
        for (int i = 0 ; i < stepsRequired; i++)
        {
            Vector2 newPos = Vector2.MoveTowards(point.position, position, collisionCheckDist);
            point.position = newPos; 
            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 - point.position).normalized;
            Vector2 finalPos = resolvedPos - penetrationDir * ropeRadius;
            //Debug.Log($"resolved pos: {point.position}->{finalPos}");
            point.position = finalPos;
            //point.prevPosition = finalPos;
            break;
        }
    }

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

        Vector2 resolvedPos = hitCollider.ClosestPoint(p.position);
        Vector2 penetrationDir = (resolvedPos - p.position).normalized;
        Vector2 finalPos = resolvedPos - penetrationDir * ropeRadius;
        
        p.position = finalPos;
    }

    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;
    }
}