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 bool IsMantlePossible { get; private set; } = false;
    public int NumberOfValidPofiles { get; private set; } = -1;
    public const int WallProfileCastCount = 7;
    
    private ShapeCast3D[] _wallProfileShapecasts = new ShapeCast3D[WallProfileCastCount];
    public Vector3[] WallProfilePositions { get; private set; } = new Vector3[WallProfileCastCount];
    public Vector3[] WallProfileNormals { get; private set; } = new Vector3[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
        NumberOfValidPofiles = -1;
        IsMantlePossible = false;
        if (!isColliding)
        {
            return;
        }
        
        // Check if collide with wall
        var collisionNormal = isGrounded ? _groundedWallDetect.GetCollisionNormal(0) : _inAirWallDetect.GetCollisionNormal(0);
        if (collisionNormal.Y > 0.8f)
        {
            return;
        }

        NumberOfValidPofiles = 0;
        var hasFirstProfileHit = false;
        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())
            {
                WallProfilePositions[NumberOfValidPofiles] = globalTargetPosition;
                WallProfileNormals[NumberOfValidPofiles] = Vector3.Zero;
                NumberOfValidPofiles += 1;
                break;
            }

            var profilePoint = wallProfileShapecast.GetCollisionPoint(0);
            var profileNormal = wallProfileShapecast.GetCollisionNormal(0);
            
            // Check if we collided parallel to a wall
            var isCollisionSameAsTarget = globalTargetPosition.IsEqualApprox(profilePoint);
            var isCollidingWithWall = profileNormal.Y < 0.9f;
            if (isCollisionSameAsTarget || isCollidingWithWall) continue;
            
            // We have a valid collision
            WallProfilePositions[NumberOfValidPofiles] = profilePoint;
            WallProfileNormals[NumberOfValidPofiles] = profileNormal;
            hasFirstProfileHit = true;
            NumberOfValidPofiles += 1;
        }
        IsMantlePossible = NumberOfValidPofiles > 0; // Should always be true
    }

    public Option<Vector3> FindMantle()
    {
        if (!_wallInFrontCast3D.IsColliding())
        {
            return Option<Vector3>.None;
        }
        if (_wallInFrontCast3D.GetCollisionNormal(0).Y > 0.8f)
        {
            return Option<Vector3>.None;
        }
        
        var collisionPoint = _wallInFrontCast3D.GetCollisionPoint(0);
        var collisionNormal = _wallInFrontCast3D.GetCollisionNormal(0);
        return FindMantleLocationAtPoint(collisionPoint, collisionNormal);
    }

    public Option<Vector3> FindMantleLocationAtPoint(Vector3 point, Vector3 wallNormal)
    {
        var horizontalEndLocation = point - wallNormal * MantleEndLocationDistanceFromWall;
        var shapeCastStartLocation = horizontalEndLocation + Vector3.Up * MantleHeightCastStart;
        
        _mantleCast3D.SetGlobalPosition(shapeCastStartLocation);
        var targetLocation = Vector3.Down * MantleHeightCastStart + Vector3.Up * MaxStepHeight;
        _mantleCast3D.SetTargetPosition(targetLocation);
        
        if (_mantleCast3D.IsColliding() && _mantleCast3D.GetCollisionNormal(0).Y >= 0.1f)
            return Option.Some(_mantleCast3D.GetCollisionPoint(0));
        return Option<Vector3>.None;
    }
}