jelito/Assets/jelycho/Code/Ropes/RopesManager.cs

using System;
using System.Collections.Generic;
using System.Runtime.CompilerServices;
using Unity.Burst;
using Unity.Collections;
using Unity.Jobs;
using Unity.Mathematics;
using UnityEngine;
using UnityEngine.Profiling;

namespace RebootReality.jelycho.Ropes {
    public class RopesManager : MonoBehaviour {
        [SerializeField] float m_RopeSegmentLength = 0.5f;
        [SerializeField] int m_ConstrainIterations = 50;

        [SerializeField] bool m_ShowGizmos = true;

        readonly List<RopeData> m_Ropes = new List<RopeData>();

        public int RopesCount {
            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            get {
                return m_Ropes.Count;
            }
        }

        void OnDestroy() {
            Clear();
        }

        void OnDrawGizmos() {
            if (!m_ShowGizmos) {
                return;
            }

            foreach (RopeData rope in m_Ropes) {
                for (int i = 0; i < rope.SegmentCount; ++i) {
                    Gizmos.color = rope.IsLocked[i] ? Color.red : Color.green;
                    Gizmos.DrawSphere(rope.Positions[i], 0.1f);
                }
            }
        }

        void FixedUpdate() {
            RopeConfig ropeConfig = RopeConfig.Default;
            ropeConfig.segmentLength = m_RopeSegmentLength;
            ropeConfig.numberOfConstrainIterations = m_ConstrainIterations;

            float deltaTime = Time.fixedDeltaTime;

            Profiler.BeginSample("RopesManager.SimulateRopes");
            NativeArray<JobHandle> jobHandles = new NativeArray<JobHandle>(m_Ropes.Count, Allocator.Temp);

            for (int i = 0; i < m_Ropes.Count; i++) {
                SimulateRopeJob simulateRopeJob = new SimulateRopeJob {
                    Positions = m_Ropes[i].Positions,
                    OldPositions = m_Ropes[i].OldPositions,
                    IsLocked = m_Ropes[i].IsLocked,
                    DeltaTime = deltaTime,
                    Config = ropeConfig
                };
                JobHandle simulateJobHandle = simulateRopeJob.Schedule(m_Ropes[i].SegmentCount, 16);

                ApplyConstraintsJob applyConstraintsJob = new ApplyConstraintsJob {
                    Positions = m_Ropes[i].Positions,
                    IsLocked = m_Ropes[i].IsLocked,
                    Config = ropeConfig
                };
                jobHandles[i] = applyConstraintsJob.Schedule(simulateJobHandle);
            }
            JobHandle.CompleteAll(jobHandles);
            Profiler.EndSample();

            Profiler.BeginSample("RopesManager.CalculateRopeBounds");
            // @TODO: figure out a way to avoid this job dependency chain.
            NativeArray<NativeArray<float3>> boundsArrays =
                new NativeArray<NativeArray<float3>>(m_Ropes.Count, Allocator.Temp);

            for (int i = 0; i < m_Ropes.Count; i++) {
                boundsArrays[i] = new NativeArray<float3>(2, Allocator.TempJob);

                CalculateRopeBoundsJob calculateBoundsJob = new CalculateRopeBoundsJob {
                    Positions = m_Ropes[i].Positions,
                    Bounds = boundsArrays[i]
                };

                jobHandles[i] = calculateBoundsJob.Schedule();
            }
            JobHandle.CompleteAll(jobHandles);

            for (int i = 0; i < m_Ropes.Count; i++) {
                RopeData rope = m_Ropes[i];
                rope.Bounds = new Bounds {
                    min = boundsArrays[i][0],
                    max = boundsArrays[i][1]
                };
                m_Ropes[i] = rope;
            }
            Profiler.EndSample();

            foreach (NativeArray<float3> boundsArray in boundsArrays) {
                boundsArray.Dispose();
            }

            boundsArrays.Dispose();
            jobHandles.Dispose();
        }

        // @TODO: finish the rope spawning logic.
        public void SpawnRope(float3 start, float3 end, bool lockFirst = false, bool lockLast = false) {
            int segmentsCount = (int) (math.distance(start, end) / m_RopeSegmentLength) + 1;
            NativeArray<float3> positions = new NativeArray<float3>(segmentsCount, Allocator.Temp);

            for (int i = 0; i < segmentsCount; ++i) {
                float t = (float) i / (segmentsCount - 1);
                positions[i] = math.lerp(start, end, t);
            }

            RopeData rope = new RopeData(positions);
            rope.IsLocked[0] = lockFirst;
            rope.IsLocked[rope.SegmentCount - 1] = lockLast;
            m_Ropes.Add(rope);
        }

        public void SpawnLockedRope(float ropeLength, float3 start, float3 end) {
            int segmentsCount = (int) (ropeLength / m_RopeSegmentLength) + 1;
            NativeArray<float3> positions = new NativeArray<float3>(segmentsCount, Allocator.Temp);

            for (int i = 0; i < segmentsCount; ++i) {
                float t = (float) i / (segmentsCount - 1);
                positions[i] = math.lerp(start, end, t);
            }

            RopeData rope = new RopeData(positions);
            rope.IsLocked[0] = true;
            rope.IsLocked[rope.SegmentCount - 1] = true;
            m_Ropes.Add(rope);
        }

        // @NOTE: Do not dispose the returned array, it is managed by the RopesManager.
        public NativeArray<float3> PeekRopePositions(int index) {
            return m_Ropes[index].Positions;
        }

        static readonly Plane[] s_Planes = new Plane[6];

        public bool IsRopeBoundsInFrustum(int index, Camera cam) {
            Bounds bound = m_Ropes[index].Bounds;

            GeometryUtility.CalculateFrustumPlanes(cam, s_Planes);
            return GeometryUtility.TestPlanesAABB(s_Planes, bound);
        }

        public Bounds GetRopeBounds(int index) {
            return m_Ropes[index].Bounds;
        }

        void Clear() {
            foreach (RopeData rope in m_Ropes) {
                rope.Dispose();
            }

            m_Ropes.Clear();
        }

        struct RopeData : IDisposable {
            public NativeArray<float3> Positions;
            public NativeArray<float3> OldPositions;
            public NativeArray<bool> IsLocked;
            public Bounds Bounds;

            public int SegmentCount {
                [MethodImpl(MethodImplOptions.AggressiveInlining)]
                get {
                    return Positions.Length;
                }
            }

            public RopeData(NativeArray<float3> positions) : this(positions.Length) {
                for (int i = 0; i < positions.Length; ++i) {
                    Positions[i] = positions[i];
                    OldPositions[i] = positions[i];
                    IsLocked[i] = false;
                }
            }

            public RopeData(int segmentCount) {
                Positions = new NativeArray<float3>(segmentCount, Allocator.Persistent);
                OldPositions = new NativeArray<float3>(segmentCount, Allocator.Persistent);
                IsLocked = new NativeArray<bool>(segmentCount, Allocator.Persistent);
                Bounds = new Bounds();
            }

            public void Dispose() {
                Positions.Dispose();
                OldPositions.Dispose();
                IsLocked.Dispose();
            }
        }
    }

    [BurstCompile]
    struct SimulateRopeJob : IJobParallelFor {
        public NativeArray<float3> Positions;
        public NativeArray<float3> OldPositions;
        [ReadOnly] public NativeArray<bool> IsLocked;

        [ReadOnly] public float DeltaTime;
        [ReadOnly] public RopeConfig Config;

        public void Execute(int index) {
            if (IsLocked[index]) {
                return;
            }

            float3 position = Positions[index];
            float3 segmentPositionBeforeUpdate = position;

            position += (position - OldPositions[index]) * Config.dampingFactor;
            position.y += Config.gravity * DeltaTime;

            Positions[index] = position;
            OldPositions[index] = segmentPositionBeforeUpdate;
        }
    }

    [BurstCompile]
    struct ApplyConstraintsJob : IJob {
        public NativeArray<float3> Positions;

        [ReadOnly] public NativeArray<bool> IsLocked;
        [ReadOnly] public RopeConfig Config;

        public void Execute() {
            for (int iteration = 0; iteration < Config.numberOfConstrainIterations; ++iteration) {
                for (int i = 0; i < Positions.Length - 1; ++i) {
                    float3 position = Positions[i];
                    float3 nextPosition = Positions[i + 1];

                    float currentDistance = math.distance(position, nextPosition);
                    float difference = currentDistance - Config.segmentLength;
                    float3 direction = math.normalize(position - nextPosition);
                    float3 change = direction * (difference * 0.5f);

                    if (!IsLocked[i]) {
                        position -= change;
                        Positions[i] = position;
                    }

                    if (!IsLocked[i + 1]) {
                        nextPosition += change;
                        Positions[i + 1] = nextPosition;
                    }
                }
            }
        }
    }

    [BurstCompile]
    struct CalculateRopeBoundsJob : IJob {
        [ReadOnly] public NativeArray<float3> Positions;

        public NativeArray<float3> Bounds;

        public void Execute() {
            if (Positions.Length == 0) {
                Bounds[0] = float3.zero;
                Bounds[1] = float3.zero;
                return;
            }

            float3 min = Positions[0];
            float3 max = Positions[0];

            for (int i = 1; i < Positions.Length; ++i) {
                min = math.min(min, Positions[i]);
                max = math.max(max, Positions[i]);
            }

            Bounds[0] = min;
            Bounds[1] = max;
        }
    }
}