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;

    
    private void CalcPlaneVelocity(float delta, Entity entity, Vector3 targetPosition)
    {
        Vector2 planeTargetVector = new Vector2(targetPosition.x - entity.GlobalTranslation.x,
            targetPosition.z - entity.GlobalTranslation.z);
        float targetDistance = planeTargetVector.Length();
        Vector2 planeTargetDirection = planeTargetVector / targetDistance;
        Vector2 planeOrientation = new Vector2(entity.GlobalTransform.basis.z[0], entity.GlobalTransform.basis.z[2]);
        
        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 (targetDistance < 0.01 || planeOrientation.Dot(planeTargetDirection) < 0.9)
        {
            planeVelocity = Vector2.Zero;
        } else {
            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 > targetDistance)
            {
                planeVelocity *= targetDistance / projectedStep;
                projectedStep = planeTargetDirection.Dot(planeVelocity * delta);
                // GD.Print("  corr speed: " + planeVelocity.Length() + " step: " + projectedStep);                
            }
            
            float planeSpeed = planeVelocity.Length();
            if (planeSpeed > MaxSpeed)
            {
                planeVelocity *= MaxSpeed / planeSpeed;
            }
        }

        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)
    {
        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);
    }

    private void CalcAndApplyOrientation(float delta, Entity entity, Vector3 targetPosition, Quat targetOrientation)
    {
        Vector3 direction_to_target = targetPosition - entity.GlobalTranslation;

        if (direction_to_target.LengthSquared() > Globals.EpsPositionSquared)
        {
            direction_to_target = direction_to_target.Normalized();
            Vector3 localXAxis = Vector3.Up.Cross(direction_to_target);
            targetOrientation = new Basis(localXAxis, Vector3.Up, direction_to_target).Quat().Normalized();
        }
        
        float orientationErrorRadians = entity.GlobalTransform.basis.Quat().AngleTo(targetOrientation);
        if (Mathf.Abs(orientationErrorRadians) > Mathf.Pi * 1.1)
        {
            GD.Print("moep");
        }
        if (orientationErrorRadians > 0)
        {
            Transform entityTransform = entity.Transform;
            if (orientationErrorRadians < RotationSpeedRadPerSecond * delta)
            {
                entityTransform.basis = new Basis(targetOrientation);
            }
            else if (orientationErrorRadians > 0f)
            {
                Quat entityRotation = new Quat(entityTransform.basis);
                float slerpWeight = RotationSpeedRadPerSecond / (orientationErrorRadians / delta);
                entityRotation = entityRotation.Slerp(targetOrientation, slerpWeight).Normalized();
                entityTransform.basis = new Basis(entityRotation);
            }

            entity.Transform = entityTransform;
        }
    }
}