fgm24/Assets/Scripts/Rope/RopeSimulator.cs

using System;
using System.Collections.Generic;
using System.Linq;
using Unity.Netcode;
using UnityEngine;
using UnityEngine.Assertions;

public class RopeSimulator : NetworkBehaviour
{
    [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]
    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("Rope Colliders")]
    [SerializeField] GameObject colliderPrefab;
    [SerializeField] string colliderTag = "Rope";
    [SerializeField] Transform ropeCollidersParent;

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

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

    [Header("Netcode")]
    private const int k_rngSeed = 6969;
    private const float k_sendRopeDataRPCThreshold = 0.1f;
    private const float k_ropeReconciliateThreshold = 0.25f;
    private System.Random rng = new System.Random(k_rngSeed);

    private int[] order;

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

    public Rope rope;

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

    public static RopeSimulator instance;

    private bool IsInitialized => start != null || end != null;

    private void Awake()
    {
        if (instance == null)
        {
            instance = this;
        }
        else
        {
            Destroy(instance);
        }
    }

    private void OnEnable()
    {
        GameManager.OnPlayersReady += PlayersReady;
    }

    private void OnDisable()
    {
        GameManager.OnPlayersReady -= PlayersReady;
    }

    public void PlayersReady(GameObject[] players)
    {
        BuildRope(players[0].GetComponent<RopeJoint>(), players[1].GetComponent<RopeJoint>());
    }

    public void BuildRope(RopeJoint start, RopeJoint end)
    {
        Assert.IsNotNull(start);
        Assert.IsNotNull(end);

        // Sanity check if rope simulator was initialized before - we are re-building the rope
        if (this.start != null)
        {
            this.start.playerInput.ropeLengthExtend -= ExtendRope;
            this.start.playerInput.ropeLengthShrinken -= ShrinkenRope;
        }
        if (this.end != null)
        {
            this.end.playerInput.ropeLengthExtend -= ExtendRope;
            this.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;
        subDivision = Mathf.Clamp(subDivision, ropeMinLength, ropeMaxLength);

        // Only shrinken if the numeric value has changed
        if (prevSubDivision - (int) subDivision <= 0) return;

        // Shrink from rope point after start rope joint
        List<Point> newPoints = new(rope.points.Length - 1);
        for (int i = 0; i < (rope.points.Length - 1); i++)
        {
            newPoints.Add(rope.points[i]);
        }

        var builder = new RopeBuilder(newPoints, new List<Stick>());

        // 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;
        subDivision = Mathf.Clamp(subDivision, ropeMinLength, ropeMaxLength);

        // Only extend if the numeric value has changed
        if (prevSubDivision - (int) subDivision >= 0) return;

        // Extend from rope point after start rope point
        List<Point> newPoints = new (rope.points.Length + 1);
        newPoints.Add(new Point(rope.points[1].position));
        for (int i = 1; i < rope.points.Length; i++)
        {
            newPoints.Add(rope.points[i]);
        }

        var builder = new RopeBuilder(newPoints, new List<Stick>());

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

    public override void OnDestroy()
    {
        base.OnDestroy();

        // May never have been initialized
        if (!IsInitialized) return;

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

        // Build rope points
        for (int i = 1; i < (int) subDivision; i++)
        {
            Vector3 pointPos = Vector3.Lerp(start.position, end.position, (float)i / Mathf.Floor(subDivision));
            Debug.DrawRay(pointPos, (end.position - start.position).normalized);
            builder.AddPoint(new Point(pointPos));
        }

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

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

        RebuildRopeColliders();

        CreateOrderArray();
    }

    private void RebuildRopeColliders()
    {
        for (int i = 0; i < ropeCollidersParent.childCount; i++)
        {
            Destroy(ropeCollidersParent.GetChild(i));
        }

        foreach (var point in rope.points)
        {
            GameObject ropeCollider = Instantiate(colliderPrefab);
            ropeCollider.transform.parent = ropeCollidersParent;

            ropeCollider.transform.position = point.position;
            ropeCollider.tag = colliderTag;
            ropeCollider.layer = LayerMask.NameToLayer("Rope");

            ropeCollider.GetComponent<CircleCollider2D>().radius = ropeRadius;
            var rigidBody = ropeCollider.GetComponent<Rigidbody2D>();
            rigidBody.isKinematic = true;

        }
    }

    private void DrawRope()
    {
        // Update line renderer
        List<Vector3> positions = new List<Vector3>(ropeCollidersParent.childCount);
        for (int i = 0; i < ropeCollidersParent.childCount; i++)
        {
            positions.Add(ropeCollidersParent.GetChild(i).position);
        }
        lineRenderer.positionCount = positions.Count;
        lineRenderer.SetPositions(positions.ToArray());
    }

    [Rpc(SendTo.NotServer)]
    private void ServerRopeDataReceivedRpc(NetworkRope nrope)
    {
        Debug.Log($"Received rope data from server: {nrope}");
        Rope serverRope = Rope.FromNetworkRope(nrope, distBetweenRopePoints);

        float diff = Rope.CalcDiff(this.rope, serverRope);
        Debug.Log(diff);
        if (diff > k_ropeReconciliateThreshold)
        {
            Debug.LogWarning("Reconciliating rope!");
            this.rope = serverRope;
            Debug.Log(Rope.CalcDiff(this.rope, serverRope));
        }
    }

    private void Update()
    {
        if (!IsInitialized)
            return;

        colliderToSquezeForce.Clear();

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

        float ropeDiff = Simulate(Time.fixedDeltaTime);
        if (IsServer && ropeDiff > k_sendRopeDataRPCThreshold)
        {
            Debug.Log($"Sending rope to client");
            NetworkRope nrope = Rope.ToNetworkRope(this.rope);

            // Send server rope to client for reconciliation
            ServerRopeDataReceivedRpc(nrope);
        }

        // Update the rope collider positions
        for (int i = 0; i < rope.points.Length; 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();
        PlayerPullAnimation(overshoot);
        PullPlayers(overshoot);

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

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

        DrawRope();
    }

    private void PlayerPullAnimation(float overshoot)
    {

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

    private void PullPlayers(float overshoot)
    {
        if (overshoot <= 0f) return;

        //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.Length - 2].position - end.position).normalized;
            Vector2 force = pullDirection * overshoot * (pullForce / divider);
            end.body.AddForce(force);
        }
        else
        {
            end.body.velocity *= 0;
        }
    }

    private void OnDrawGizmos()
    {
        return;

        if (IsInitialized) return;
        if (!Application.isPlaying) return;

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

    float Simulate(float dt)
    {
        float diff = 0f;
        foreach (Point p in rope.points)
        {
            if (!p.locked)
            {
                Vector3 positionBeforeUpdate = p.position;
                p.position += p.position - p.prevPosition;
                p.position.z -= gravity * dt * dt;
                diff += Mathf.Abs(Vector3.Distance(p.prevPosition, p.position));
                p.prevPosition = positionBeforeUpdate;
            }
        }

        for (int i = 0; i < solveIterations; i++)
        {
            for (int s = 0; s < rope.sticks.Length; 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);
                    }
                }
            }
        }
        return diff;
    }

    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*1.1f, 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.Length];
        for (int i = 0; i < order.Length; i++)
        {
            order[i] = i;
        }
        ShuffleArray(order, rng);
    }

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