fgm24/Assets/Scripts/Rope/RopeSimulator.cs

using System;
using System.Collections;
using System.Collections.Generic;
using System.Linq;
using UnityEngine;

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

    [SerializeField]
    private int solveIterations = 10;

    [SerializeField]
    private bool constrainStickMinLength;

    [SerializeField]
    public RopeJoint start, end;

    [SerializeField]
    float subDivision = 50f;

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

    [SerializeField, Range(0f, 20f)]
    public float ropeExtendSpeed, ropeShrinkSpeed;

    public float squezeDamage = 1f;
    public AnimationCurve swingSpeedToDamageMultiplier;

    [SerializeField]
    public float ropeMaxLength, ropeMinLength;


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

    [Header("Animaion")]
    [SerializeField] float pullAnimationOvershootThreshold = 0.2f;

    int[] order;

    public float Overshoot => rope.CalculateLengthOvershoot();
    public bool InSwingMode => start.locked || end.locked;

    public Rope rope;

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

    private void Start()
    {
        // Example of using rope builder
        //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();

        // Build rope if rope joints specified in inspector
        if (start != null && end != null)
            BuildRope(start, end);
    }

    public void BuildRope(RopeJoint start, RopeJoint end)
    {
        // Sanity check if rope simulator was initialized before
        if (this.start != null)
        {
            start.playerInput.ropeLengthExtend -= ExtendRope;
            start.playerInput.ropeLengthShrinken -= ShrinkenRope;
        }
        if (this.end != null)
        {
            end.playerInput.ropeLengthExtend -= ExtendRope;
            end.playerInput.ropeLengthShrinken -= ShrinkenRope;
        }

        this.start = start;
        this.end = end;

        Rebuild();

        this.start.playerInput.ropeLengthShrinken += ShrinkenRope;
        this.end.playerInput.ropeLengthShrinken += ShrinkenRope;

        this.start.playerInput.ropeLengthExtend += ExtendRope;
        this.end.playerInput.ropeLengthExtend += ExtendRope;
    }

    void ShrinkenRope(int playerNumber)
    {
        int prevSubDivision = (int) subDivision;
        subDivision -= ropeShrinkSpeed * Time.deltaTime;
        if (subDivision < ropeMinLength)
            subDivision = ropeMinLength;

        if (prevSubDivision - (int)subDivision <= 0) return;

        // Shrink from start
        rope.sticks.Clear();
        rope.points.RemoveAt(1);

        var builder = new RopeBuilder(rope.points, rope.sticks);

        // Re-gen sticks
        for (int i = 0; i < (int) subDivision; i++)
        {
            builder.ConnectPointsWithDesiredLength(i, i + 1, distBetweenRopePoints);
        }
        rope = builder.Build();

        RebuildRopeColliders();
        CreateOrderArray();
    }

    void ExtendRope(int playerNumber)
    {
        int prevSubDivision = (int)subDivision;
        subDivision += ropeExtendSpeed * Time.deltaTime;
        if (subDivision > ropeMaxLength)
            subDivision = ropeMaxLength;

        if (prevSubDivision - (int) subDivision >= 0) return;

        // Max from start
        rope.points.Insert(1, new Point(rope.points[1].position));
        rope.sticks.Clear();

        // Ripple existing rope points
        //for (int i = 2; i < (int) subDivision; i++)
        //{
        //    rope.points[i].position = rope.points[i + 1].position;
        //}

        var builder = new RopeBuilder(rope.points, rope.sticks);

        // Re-gen sticks
        for (int i = 0; i < (int)subDivision; i++)
        {
            //Debug.Log($"Reg-gen stick. from: {i} to {i + 1}, with dist: {distBetweenRopePoints}");
            builder.ConnectPointsWithDesiredLength(i, i + 1, distBetweenRopePoints);
        }
        rope = builder.Build();

        RebuildRopeColliders();
        CreateOrderArray();

    }

    private void OnDestroy()
    {
        start.playerInput.ropeLengthShrinken -= ShrinkenRope;
        end.playerInput.ropeLengthShrinken -= ShrinkenRope;

        start.playerInput.ropeLengthExtend -= ExtendRope;
        end.playerInput.ropeLengthExtend -= ExtendRope;
    }

    private void Rebuild()
    {
        Debug.Log("rebuild");

        RopeBuilder builder = new RopeBuilder();
        builder.AddPoint(new Point(start.position, locked: true));

        for (int i = 1; i < (int) subDivision; i++)
        {
            Vector3 pointPos = Vector3.Lerp(start.position, end.position, (float)i / (float)(int)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 < (int) subDivision; i++)
        {
            builder.ConnectPointsWithDesiredLength(i, i + 1, desiredLength: distBetweenRopePoints);
        }
        rope = builder.Build();

        RebuildRopeColliders();

        CreateOrderArray();
    }

    private void RebuildRopeColliders()
    {
        // ropeCollidersParent.DestroyChildren(); Did this to avoid unityutils
        for (int i = 0; i < ropeCollidersParent.childCount; i++)
        {
            Destroy(ropeCollidersParent.GetChild(i));
        }

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

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

            var rigidBody = ropeCollider.AddComponent<Rigidbody2D>();
            rigidBody.isKinematic = true;
        }
    }

    private void Update()
    {
        // Dont update if no rope is initialized
        if (start == null || end == null)
            return;

        //Debug.Log($"overshoot: {rope.CalculateLengthOvershoot()}");
        //ShrinkenRope(1);
        //ExtendRope(0);
        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)
        {
            if (overshoot > pullAnimationOvershootThreshold)
            {
                float startDot = Vector2.Dot((start.position - rope.points[1].position).normalized, start.playerInput.movement);
                if (startDot > 0.35f)
                {
                    start.playerAnimationHandler?.animator.SetBool("IsPulling", true);
                }

                float endDot = Vector2.Dot((end.position - rope.points[rope.points.Count - 2].position).normalized, end.playerInput.movement);
                if (endDot > 0.35f)
                {
                    end.playerAnimationHandler?.animator.SetBool("IsPulling", true);
                }

            }
            else
            {
                start.playerAnimationHandler?.animator.SetBool("IsPulling", false);
                end.playerAnimationHandler?.animator.SetBool("IsPulling", false);
            }

            //start.position = prevStartPos;
            float divider = !start.locked && !end.locked ? 2f : 1f;

            if (!start.locked)
            {
                Vector2 pullDirection = (rope.points[1].position - start.position).normalized;
                Vector2 force = pullDirection * overshoot * (pullForce / divider);
                start.body.AddForce(force);
            }
            else
            {
                start.body.velocity *= 0;
            }
            if (!end.locked)
            {
                Vector2 pullDirection = (rope.points[rope.points.Count - 2].position - end.position).normalized;
                Vector2 force = pullDirection * overshoot * (pullForce / divider);
                end.body.AddForce(force);
            }
            else
            {
                end.body.velocity *= 0;
            }
        }

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

            if (squezeDamageReceiver == null) continue;


            float swingMultiplier = InSwingMode ? swingSpeedToDamageMultiplier.Evaluate((start.locked ? end : start).body.velocity.magnitude) : 1f;
            squezeDamageReceiver.TakeSquezeDamage(collider.Value * squezeDamage * swingMultiplier);
        }

        // 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 = Vector2.Distance(stick.A.position, stick.B.position);

                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);
        bool shouldBreak = false;
        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);

            foreach (var collider in Physics2D.OverlapCircleAll(point.position, ropeRadius, staticColliderMask))
            {
                if (collider == null) continue;
                if (collider.isTrigger) 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);
                shouldBreak = true;
                break;
            }
            if (shouldBreak)
                break;
        }

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

    private void HandleStaticCollidersOfPoint(Point p)
    {
        foreach (var hitCollider in Physics2D.OverlapCircleAll(p.position, ropeRadius, staticColliderMask))
        {
            if (hitCollider == null) continue;
            if (hitCollider.isTrigger) continue;

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