HighGroundRoyaleFusion/Assets/Photon/FusionAddons/SimpleKCC/Processors/StepUpProcessor.cs

using System;
using UnityEngine;

namespace Fusion.Addons.SimpleKCC
{
    internal static class StepUpProcessor
    {
        private static bool _clearDynamicVelocityOnEnd = true;

        private static KCCOverlapInfo _overlapInfo = new KCCOverlapInfo();

        private static KCCShapeCastInfo _shapeCastInfo = new KCCShapeCastInfo();

        public static void AfterMoveStep(KCC kcc, KCCData data, KCCOverlapInfo baseOverlapInfo)
        {
            KCCSettings settings = kcc.Settings;
            if (settings.StepHeight <= 0f)
            {
                return;
            }

            if (data.JumpFrames > 0 || data.HasTeleported)
            {
                ProcessStepUpResult(kcc, data, isSteppingUp: false);
                return;
            }

            bool flag = false;
            if (HasCollisionsWithinExtent(baseOverlapInfo, (ECollisionType)14))
            {
                flag = true;
            }
            else
            {
                float magnitude = (data.DesiredPosition - data.BasePosition).magnitude;
                if (magnitude > 0.001f && (data.TargetPosition - data.BasePosition).magnitude / magnitude < settings.StepMinPushBack)
                {
                    flag = true;
                }
            }

            if (!flag)
            {
                ProcessStepUpResult(kcc, data, isSteppingUp: false);
                return;
            }

            Vector3 basePosition = data.BasePosition;
            Vector3 desiredPosition = data.DesiredPosition;
            Vector3 vector = data.TargetPosition;
            Vector3 vector2 = desiredPosition - basePosition;
            Vector3 vector3 = Vector3.Normalize(vector2);
            if (vector3 == default(Vector3))
            {
                ProcessStepUpResult(kcc, data, isSteppingUp: false);
                return;
            }

            if (Vector3.Dot(vector3, Vector3.down) >= 0.9f)
            {
                ProcessStepUpResult(kcc, data, isSteppingUp: false);
                return;
            }

            Vector3 vector4 = vector - desiredPosition;
            float magnitude2 = vector4.magnitude;
            Vector3 vector5 = ((magnitude2 > 0.001f) ? (vector4 / magnitude2) : (-vector3));
            if (Vector3.Dot(vector3, vector5) >= 0f)
            {
                ProcessStepUpResult(kcc, data, isSteppingUp: false);
                return;
            }

            Vector3 vector6 = Vector3.Normalize(vector3.OnlyXZ());
            Vector3 vector7 = Vector3.Normalize(-vector5.OnlyXZ());
            Vector3 vector8 = Vector3.Normalize(vector6 + vector7);
            if (Vector3.Dot(vector6, vector7) < 0.1f)
            {
                ProcessStepUpResult(kcc, data, isSteppingUp: false);
                return;
            }

            if (!vector5.IsZero() && !HasCollisionsWithinExtent(baseOverlapInfo, ECollisionType.Slope))
            {
                Ray ray = new Ray(basePosition - vector2 * 2f, vector3);
                if (new Plane(vector5, vector).Raycast(ray, out var enter))
                {
                    vector = ray.GetPoint(enter);
                }
            }

            float num = kcc.Settings.Radius - kcc.Settings.Extent;
            Vector3 vector9 = vector + new Vector3(0f, settings.StepHeight, 0f);
            if (kcc.CapsuleOverlap(_overlapInfo, vector9, num, kcc.Settings.Height, QueryTriggerInteraction.Ignore))
            {
                ProcessStepUpResult(kcc, data, isSteppingUp: false);
                return;
            }

            vector9 += vector8 * settings.StepDepth;
            if (kcc.CapsuleOverlap(_overlapInfo, vector9, num, kcc.Settings.Height, QueryTriggerInteraction.Ignore))
            {
                ProcessStepUpResult(kcc, data, isSteppingUp: false);
                return;
            }

            float num2 = settings.StepHeight;
            bool flag2 = false;
            Vector3 lhs = default(Vector3);
            if (settings.StepGroundCheckRadiusScale < 1f)
            {
                num = kcc.Settings.Radius * settings.StepGroundCheckRadiusScale;
                vector9 += vector8 * (kcc.Settings.Radius - kcc.Settings.Extent - num);
            }

            if (kcc.SphereCast(_shapeCastInfo, vector9 + new Vector3(0f, kcc.Settings.Radius, 0f), num, Vector3.down, num2 + kcc.Settings.Radius, QueryTriggerInteraction.Ignore, trackInitialOverlaps: false))
            {
                Vector3 vector10 = new Vector3(0f, float.MinValue, 0f);
                int i = 0;
                for (int colliderHitCount = _shapeCastInfo.ColliderHitCount; i < colliderHitCount; i++)
                {
                    RaycastHit raycastHit = _shapeCastInfo.ColliderHits[i].RaycastHit;
                    Vector3 point = raycastHit.point;
                    if (point.y > vector.y && point.y > vector10.y)
                    {
                        vector10 = point;
                        lhs = raycastHit.normal;
                        flag2 = true;
                    }
                }

                if (flag2)
                {
                    num2 = Mathf.Clamp(vector10.y - vector.y, 0f, settings.StepHeight);
                }
            }

            if (settings.StepRequireGroundTarget && !data.IsSteppingUp && !data.WasSteppingUp && flag2)
            {
                float num3 = Mathf.Cos(Mathf.Clamp(data.MaxGroundAngle, 0f, 90f) * (MathF.PI / 180f));
                if (Vector3.Dot(lhs, Vector3.up) < num3)
                {
                    ProcessStepUpResult(kcc, data, isSteppingUp: false);
                    return;
                }
            }

            float num4 = Vector3.Distance(basePosition, desiredPosition);
            float num5 = Vector3.Distance(basePosition, vector);
            float num6 = Mathf.Clamp((num4 - num5) * settings.StepSpeed, 0f, num2);
            num6 *= Mathf.Clamp01(Vector3.Dot(vector3, -vector5));
            data.TargetPosition = vector + new Vector3(0f, num6, 0f);
            data.IsGrounded = true;
            data.GroundNormal = Vector3.up;
            data.GroundDistance = kcc.Settings.Extent;
            data.GroundPosition = data.TargetPosition;
            data.GroundTangent = data.TransformDirection;
            ProcessStepUpResult(kcc, data, isSteppingUp: true);
            kcc.UpdateHits(baseOverlapInfo, EKCCHitsOverlapQuery.Default);
        }

        private static void ProcessStepUpResult(KCC kcc, KCCData data, bool isSteppingUp)
        {
            data.IsSteppingUp = isSteppingUp;
            if (!data.IsSteppingUp && data.WasSteppingUp && _clearDynamicVelocityOnEnd)
            {
                data.DynamicVelocity = default(Vector3);
            }
        }

        public static void AfterMoveStep(KCCOffline kcc, KCCData data, KCCOverlapInfo baseOverlapInfo)
        {
            KCCSettings settings = kcc.Settings;
            if (settings.StepHeight <= 0f)
            {
                return;
            }

            if (data.JumpFrames > 0 || data.HasTeleported)
            {
                ProcessStepUpResult(kcc, data, isSteppingUp: false);
                return;
            }

            bool flag = false;
            if (HasCollisionsWithinExtent(baseOverlapInfo, (ECollisionType)14))
            {
                flag = true;
            }
            else
            {
                float magnitude = (data.DesiredPosition - data.BasePosition).magnitude;
                if (magnitude > 0.001f && (data.TargetPosition - data.BasePosition).magnitude / magnitude < settings.StepMinPushBack)
                {
                    flag = true;
                }
            }

            if (!flag)
            {
                ProcessStepUpResult(kcc, data, isSteppingUp: false);
                return;
            }

            Vector3 basePosition = data.BasePosition;
            Vector3 desiredPosition = data.DesiredPosition;
            Vector3 vector = data.TargetPosition;
            Vector3 vector2 = desiredPosition - basePosition;
            Vector3 vector3 = Vector3.Normalize(vector2);
            if (vector3 == default(Vector3))
            {
                ProcessStepUpResult(kcc, data, isSteppingUp: false);
                return;
            }

            if (Vector3.Dot(vector3, Vector3.down) >= 0.9f)
            {
                ProcessStepUpResult(kcc, data, isSteppingUp: false);
                return;
            }

            Vector3 vector4 = vector - desiredPosition;
            float magnitude2 = vector4.magnitude;
            Vector3 vector5 = ((magnitude2 > 0.001f) ? (vector4 / magnitude2) : (-vector3));
            if (Vector3.Dot(vector3, vector5) >= 0f)
            {
                ProcessStepUpResult(kcc, data, isSteppingUp: false);
                return;
            }

            Vector3 vector6 = Vector3.Normalize(vector3.OnlyXZ());
            Vector3 vector7 = Vector3.Normalize(-vector5.OnlyXZ());
            Vector3 vector8 = Vector3.Normalize(vector6 + vector7);
            if (Vector3.Dot(vector6, vector7) < 0.1f)
            {
                ProcessStepUpResult(kcc, data, isSteppingUp: false);
                return;
            }

            if (!vector5.IsZero() && !HasCollisionsWithinExtent(baseOverlapInfo, ECollisionType.Slope))
            {
                Ray ray = new Ray(basePosition - vector2 * 2f, vector3);
                if (new Plane(vector5, vector).Raycast(ray, out var enter))
                {
                    vector = ray.GetPoint(enter);
                }
            }

            float num = kcc.Settings.Radius - kcc.Settings.Extent;
            Vector3 vector9 = vector + new Vector3(0f, settings.StepHeight, 0f);
            if (kcc.CapsuleOverlap(_overlapInfo, vector9, num, kcc.Settings.Height, QueryTriggerInteraction.Ignore))
            {
                ProcessStepUpResult(kcc, data, isSteppingUp: false);
                return;
            }

            vector9 += vector8 * settings.StepDepth;
            if (kcc.CapsuleOverlap(_overlapInfo, vector9, num, kcc.Settings.Height, QueryTriggerInteraction.Ignore))
            {
                ProcessStepUpResult(kcc, data, isSteppingUp: false);
                return;
            }

            float num2 = settings.StepHeight;
            bool flag2 = false;
            Vector3 lhs = default(Vector3);
            if (settings.StepGroundCheckRadiusScale < 1f)
            {
                num = kcc.Settings.Radius * settings.StepGroundCheckRadiusScale;
                vector9 += vector8 * (kcc.Settings.Radius - kcc.Settings.Extent - num);
            }

            if (kcc.SphereCast(_shapeCastInfo, vector9 + new Vector3(0f, kcc.Settings.Radius, 0f), num, Vector3.down, num2 + kcc.Settings.Radius, QueryTriggerInteraction.Ignore, trackInitialOverlaps: false))
            {
                Vector3 vector10 = new Vector3(0f, float.MinValue, 0f);
                int i = 0;
                for (int colliderHitCount = _shapeCastInfo.ColliderHitCount; i < colliderHitCount; i++)
                {
                    RaycastHit raycastHit = _shapeCastInfo.ColliderHits[i].RaycastHit;
                    Vector3 point = raycastHit.point;
                    if (point.y > vector.y && point.y > vector10.y)
                    {
                        vector10 = point;
                        lhs = raycastHit.normal;
                        flag2 = true;
                    }
                }

                if (flag2)
                {
                    num2 = Mathf.Clamp(vector10.y - vector.y, 0f, settings.StepHeight);
                }
            }

            if (settings.StepRequireGroundTarget && !data.IsSteppingUp && !data.WasSteppingUp && flag2)
            {
                float num3 = Mathf.Cos(Mathf.Clamp(data.MaxGroundAngle, 0f, 90f) * (MathF.PI / 180f));
                if (Vector3.Dot(lhs, Vector3.up) < num3)
                {
                    ProcessStepUpResult(kcc, data, isSteppingUp: false);
                    return;
                }
            }

            float num4 = Vector3.Distance(basePosition, desiredPosition);
            float num5 = Vector3.Distance(basePosition, vector);
            float num6 = Mathf.Clamp((num4 - num5) * settings.StepSpeed, 0f, num2);
            num6 *= Mathf.Clamp01(Vector3.Dot(vector3, -vector5));
            data.TargetPosition = vector + new Vector3(0f, num6, 0f);
            data.IsGrounded = true;
            data.GroundNormal = Vector3.up;
            data.GroundDistance = kcc.Settings.Extent;
            data.GroundPosition = data.TargetPosition;
            data.GroundTangent = data.TransformDirection;
            ProcessStepUpResult(kcc, data, isSteppingUp: true);
            kcc.UpdateHits(baseOverlapInfo, EKCCHitsOverlapQuery.Default);
        }

        private static void ProcessStepUpResult(KCCOffline kcc, KCCData data, bool isSteppingUp)
        {
            data.IsSteppingUp = isSteppingUp;
            if (!data.IsSteppingUp && data.WasSteppingUp && _clearDynamicVelocityOnEnd)
            {
                data.DynamicVelocity = default(Vector3);
            }
        }

        private static bool HasCollisionsWithinExtent(KCCOverlapInfo overlapInfo, ECollisionType collisionTypes)
        {
            for (int i = 0; i < overlapInfo.ColliderHitCount; i++)
            {
                KCCOverlapHit kCCOverlapHit = overlapInfo.ColliderHits[i];
                if (kCCOverlapHit.IsWithinExtent && (collisionTypes & kCCOverlapHit.CollisionType) != 0)
                {
                    return true;
                }
            }

            return false;
        }
    }
}