GodotComponentTest/components/GroundMotionComponent.cs

using System;
using System.Numerics;
using Godot;
using Vector2 = Godot.Vector2;
using Vector3 = Godot.Vector3;

/// <summary>
/// </summary>
public class GroundMotionComponent : Component
{
    public float Accel = 50;
    public float Damping = 0.2f;
    public float MaxSpeed = 8;
    
    public float RotationSpeedRadPerSecond = 270 * Mathf.Pi / 180;
    public Vector3 DirectionToTarget = Vector3.Zero;
    public float DistanceToTarget = 0;
    public float TargetAngle = 0;
    public float TargetDeltaAngle = 0;
    
    
    private void CalcAndApplyOrientation(float delta, Entity entity, Vector3 targetPosition, Quat targetOrientation)
    {
        float deltaAngleAbsolute = Mathf.Abs(TargetDeltaAngle);
        if (deltaAngleAbsolute > 0.001)
        {
            if (RotationSpeedRadPerSecond * delta + 0.001 >= deltaAngleAbsolute)
            {
                GD.Print("Target Angle " + TargetAngle + " reached! Current Angle: " + entity.PlaneAngle + " TargetDeltaAngle = " + TargetDeltaAngle);
                entity.PlaneAngle = TargetAngle;
                TargetDeltaAngle = 0;
            }
            else
            {
                entity.PlaneAngle += Mathf.Sign(TargetDeltaAngle) * RotationSpeedRadPerSecond * delta;
            }
        }
        else
        {
            TargetDeltaAngle = 0;
        }
    }
    
    private void CalcPlaneVelocity(float delta, Entity entity, Vector3 targetPosition)
    {
        Vector2 planeTargetDirection = new Vector2(DirectionToTarget.x, DirectionToTarget.z);
        
        Vector2 planeVelocity = new Vector2(entity.Velocity.x, entity.Velocity.z);
        // GD.Print("-- Step: distance: " + targetDistance + " dir: " + planeTargetDirection + " speed: " + planeVelocity.Length() + " vel dir: " + planeVelocity.Normalized());
        planeVelocity -= planeVelocity * Damping;
        // GD.Print("  damp : speed: " + planeVelocity.Length() + " vel dir: " + planeVelocity.Normalized());
        
        if (DistanceToTarget < 0.01)
        {
            planeVelocity = Vector2.Zero;
        } else if (TargetDeltaAngle == 0.0) {
            planeVelocity = planeVelocity.Length() * planeTargetDirection + planeTargetDirection * Accel * delta;
            // GD.Print("  accel: speed: " + planeVelocity.Length() + " vel dir: " + planeVelocity.Normalized());            

            float projectedStep = planeTargetDirection.Dot(planeVelocity * delta);
            // GD.Print("  Projected step: " + projectedStep + " Speed: " + planeVelocity.Length() + " delta: " + delta);
            if (projectedStep > DistanceToTarget)
            {
                planeVelocity *= DistanceToTarget / projectedStep;
                projectedStep = planeTargetDirection.Dot(planeVelocity * delta);
                // GD.Print("  corr speed: " + planeVelocity.Length() + " step: " + projectedStep);                
            }
            
            float planeSpeed = planeVelocity.Length();
            if (planeSpeed > MaxSpeed)
            {
                planeVelocity *= MaxSpeed / planeSpeed;
            }
        }
        else
        {
            planeVelocity = Vector2.Zero;
        }

        entity.Velocity = new Vector3(planeVelocity.x, entity.Velocity.y, planeVelocity.y);
    }


    private void CalcVerticalVelocity(float delta, Entity entity, TileWorld tileWorld)
    {
        Vector3 entityVelocity = entity.Velocity;

        Vector2 currentTile = tileWorld.WorldToOffsetCoords(entity.GlobalTransform.origin);
        Vector2 nextTile = tileWorld.WorldToOffsetCoords(entity.GlobalTransform.origin + entityVelocity * delta);

        if (nextTile != currentTile)
        {
            float currentHeight = tileWorld.GetHeightAtOffset(currentTile);
            float nextHeight = tileWorld.GetHeightAtOffset(nextTile);

            bool isOnFloor = entity.IsOnFloor();
            
            if (nextHeight - entity.GlobalTransform.origin.y > 0.1)
            {
                GD.Print("Jump!");
                entityVelocity.y = 10;

                Transform entityTransform = entity.GlobalTransform;
                entityTransform.origin.y = nextHeight;
                entity.GlobalTransform = entityTransform;
            }
        }

        entityVelocity.y -= 9.81f * delta;
        
        entity.Velocity = entityVelocity;
    }
    
    
    public void PhysicsProcess(float delta, Entity entity, Vector3 targetPosition, Quat targetOrientation, TileWorld tileWorld)
    {
        DirectionToTarget = (targetPosition - entity.GlobalTranslation);
        DirectionToTarget.y = 0;
        DistanceToTarget = DirectionToTarget.Length();

        if (DistanceToTarget >= Globals.EpsPosition)
        {
            DirectionToTarget = DirectionToTarget.Normalized();
            TargetAngle = Vector3.Right.SignedAngleTo(DirectionToTarget, Vector3.Up);
            TargetDeltaAngle = entity.CalcShortestPlaneRotationToTargetDirection(DirectionToTarget);            
        }
        else
        {
            GD.Print("Target Position reached!");
            DirectionToTarget = Vector3.Right;
            TargetAngle = entity.PlaneAngle;
            TargetDeltaAngle = 0;
            entity.GlobalTranslation = targetPosition;
            entity.Velocity = new Vector3(0, entity.Velocity.y, 0);
            return;
        }
        
        CalcAndApplyOrientation(delta, entity, targetPosition, targetOrientation);
        CalcPlaneVelocity(delta, entity, targetPosition);
//        CalcVerticalVelocity(delta, entity, tileWorld);

        entity.Velocity = entity.MoveAndSlide(entity.Velocity);
        
        //GD.Print("Pre : speed: " + prePhysicsVelocity.Length() + " Velocity: " + prePhysicsVelocity);
        //GD.Print("Post: speed: " + entity.Velocity.Length() + " Velocity: " + entity.Velocity);
    }

}