MovementTests/systems/mantle/MantleSystem.cs

using Godot;
using RustyOptions;

namespace Movementtests.systems;

public partial class MantleSystem: Node3D
{
    [Export(PropertyHint.Range, "0,2,0.01,suffix:m,or_greater")]
    public float MantleEndLocationDistanceFromWall { get; set; } = 1f;
    [Export(PropertyHint.Range, "0,10,0.1,suffix:m,or_greater")]
    public float MantleHeightCastStart { get; set; } = 2f;
    [Export(PropertyHint.Range, "0,10,0.01,suffix:m,or_greater")]
    public float MaxStepHeight = 0.5f;
    
    private ShapeCast3D _wallInFrontCast3D;
    private ShapeCast3D _mantleCast3D;

    private ShapeCast3D _inAirWallDetect;
    private ShapeCast3D _groundedWallDetect;
    public Curve3D MantleCurve { get; private set; }
    public Vector3 FirstMantleProfilePoint { get; private set; } = Vector3.Zero;
    
    public bool IsMantlePossible { get; private set; } = false;
    public bool EndedOnOtherSideOfWall { get; private set; } = false;
    public bool FoundGround { get; private set; } = false;
    public const int WallProfileCastCount = 7;
    
    private ShapeCast3D[] _wallProfileShapecasts = new ShapeCast3D[WallProfileCastCount];

    public void Init()
    {
        _wallInFrontCast3D = GetNode<ShapeCast3D>("WallInFrontCast3D");
        _mantleCast3D = GetNode<ShapeCast3D>("MantleCast3D");
        
        _inAirWallDetect = GetNode<ShapeCast3D>("InAirWallDetect");
        _groundedWallDetect = GetNode<ShapeCast3D>("GroundedWallDetect");
        for (int i = 0; i < _wallProfileShapecasts.Length; i++)
        {
            _wallProfileShapecasts[i] = GetNode<ShapeCast3D>($"WallProfileShapeCasts/ShapeCast{i + 1}");
        }
    }

    private void SetCastsEnabled(bool enabled)
    {
        foreach (var wallProfileShapecast in _wallProfileShapecasts)
        {
            wallProfileShapecast.SetEnabled(enabled);
        }
    }

    public void ProcessMantle(bool isGrounded)
    {
        _inAirWallDetect.SetEnabled(!isGrounded);
        _groundedWallDetect.SetEnabled(isGrounded);
        var isColliding = _inAirWallDetect.IsColliding() || _groundedWallDetect.IsColliding(); 
        SetCastsEnabled(isColliding);
        
        // Reset state
        IsMantlePossible = false;
        EndedOnOtherSideOfWall = false;
        FoundGround = false;
        if (!isColliding) return;
        
        // Check if face something wall-like that should be climbable 
        var collisionNormal = isGrounded ? _groundedWallDetect.GetCollisionNormal(0) : _inAirWallDetect.GetCollisionNormal(0);
        if (collisionNormal.Y > 0.7f) return;
        
        var spaceState = GetWorld3D().DirectSpaceState;
        
        MantleCurve = new Curve3D();
        MantleCurve.AddPoint(Vector3.Zero);
        var hasFirstProfileHit = false; 
        var previousProfilePoint = GlobalPosition;
        foreach (var wallProfileShapecast in _wallProfileShapecasts)
        {
            // Haven't met the wall yet
            if (!wallProfileShapecast.IsColliding() && !hasFirstProfileHit) continue;
            
            var globalTargetPosition = wallProfileShapecast.GlobalPosition + wallProfileShapecast.TargetPosition;
            
            // Got to the other side of the wall, we stop there
            if (!wallProfileShapecast.IsColliding())
            {
                break;
            }

            var profilePoint = wallProfileShapecast.GetCollisionPoint(0);
            var profileNormal = wallProfileShapecast.GetCollisionNormal(0);
            var shape = wallProfileShapecast.Shape as SphereShape3D;
            var shapeRadius = shape == null ? 0.125f : shape.Radius;
            var centerOfShape = profilePoint + profileNormal * shapeRadius;
            
            // Check if we collided parallel to a wall
            var isCollisionSameAsTarget = globalTargetPosition.IsEqualApprox(centerOfShape);
            var isCollidingWithWall = profileNormal.Y < 0.1f;
            if (isCollisionSameAsTarget || isCollidingWithWall) continue;
            
            // Check if the path from the previous point makes us go through a wall
            var query = PhysicsRayQueryParameters3D.Create(previousProfilePoint, centerOfShape, wallProfileShapecast.CollisionMask);
            var result = spaceState.IntersectRay(query);
            if (result.Count > 0) break; // We are going through a wall, we stop there
            
            // We have a valid collision
            if (!hasFirstProfileHit) FirstMantleProfilePoint = centerOfShape;
            hasFirstProfileHit = true;
            previousProfilePoint = centerOfShape;
            MantleCurve.AddPoint(ToLocal(centerOfShape));
        }
        if (MantleCurve.PointCount == 1) return;
        
        IsMantlePossible = true;
    }
}