HillLike/SimpleCar.cs

using osu.Framework.Input.Events;
using osuTK;
using osuTK.Input;

namespace HillLike;

public class SimpleCar
{
    public const float WheelRadius = 0.45f;

    private const float Gravity = -10f;
    private const float EngineForce = 34f;
    private const float BrakeForce = 34f;
    private const float RollingFriction = 2f;
    private const float AirDrag = 0.1f;

    private const float SuspensionRest = 0f;
    private const float SuspensionK = 140f;
    private const float SuspensionD = 18f;

    private const float AngularDamp = 2f;
    private const float ChassisInertia = 2.4f;
    private const float SuspensionTorqueInfluence = 0.35f;
    private const float GroundAlignStrength = 8f;
    private const float GroundAlignDamping = 2f;
    private const float WheelAngularAccel = 60f;
    private const float WheelAngularDamping = 1.2f;
    private const float WheelGroundGrip = 14f;
    private const float AirControlFromWheelSpin = 0.12f;

    private readonly Vector2 _wheelBackLocal = new(-0.85f, -0.35f);
    private readonly Vector2 _wheelFrontLocal = new(0.85f, -0.35f);
    private bool _brake;

    private bool _throttle;
    private float _wheelSpin;
    public float AngularVelocity;
    public Vector2 Position;

    public float Rotation;
    public Vector2 Velocity;

    public Vector2 WheelBackPos { get; private set; }
    public Vector2 WheelFrontPos { get; private set; }

    public float RotationDegrees => Rotation * 180f / (float)Math.PI;

    public void Reset(Vector2 startPos)
    {
        Position = startPos;
        Velocity = Vector2.Zero;
        Rotation = 0;
        AngularVelocity = 0;
        _wheelSpin = 0;
        _throttle = _brake = false;
        UpdateWheelWorldPositions();
    }

    public void HandleKeyDown(KeyDownEvent e)
    {
        switch (e.Key)
        {
        case Key.D or Key.Right:
            _throttle = true;
            break;
        case Key.A or Key.Left:
            _brake = true;
            break;
        }
    }

    public void HandleKeyUp(KeyUpEvent e)
    {
        switch (e.Key)
        {
        case Key.D or Key.Right:
            _throttle = false;
            break;
        case Key.A or Key.Left:
            _brake = false;
            break;
        }
    }

    public void Step(float dt)
    {
        var accel = new Vector2(0, Gravity);
        var torque = 0f;

        accel += -AirDrag * Velocity;

        UpdateWheelWorldPositions();

        var backGrounded = SolveWheelSuspension(dt, WheelBackPos, out var backForce, out var backTangent,
            out _);
        var frontGrounded = SolveWheelSuspension(dt, WheelFrontPos, out var frontForce, out var frontTangent,
            out _);

        accel += backForce + frontForce;
        if (backGrounded)
            torque += TorqueFromForce(WheelBackPos, backForce) * SuspensionTorqueInfluence;

        if (frontGrounded)
            torque += TorqueFromForce(WheelFrontPos, frontForce) * SuspensionTorqueInfluence;

        var grounded = backGrounded || frontGrounded;

        var driveInput = 0f;

        if (_throttle) driveInput += 1f;
        if (_brake) driveInput -= 1f;

        _wheelSpin += driveInput * WheelAngularAccel * dt;

        //if (grounded)
        //{
        //    var driveForce = backTangent * EngineForce;
        //    accel += driveForce;
        //    torque += TorqueFromForce(WheelBackPos, driveForce);
        //}

        if (grounded)
            accel += new Vector2(-RollingFriction * Velocity.X, 0);

        if (grounded)
        {
            var targetNormal = Vector2.Zero;
            var groundedCount = 0;

            if (backGrounded)
            {
                targetNormal += new Vector2(-backTangent.Y, backTangent.X);
                groundedCount++;
            }

            if (frontGrounded)
            {
                targetNormal += new Vector2(-frontTangent.Y, frontTangent.X);
                groundedCount++;
            }

            if (groundedCount > 0)
            {
                targetNormal /= groundedCount;
                targetNormal = targetNormal.LengthSquared > 0 ? targetNormal.Normalized() : Vector2.UnitY;

                var bodyUpDot = Vector2.Dot(BodyUp(), targetNormal);
                if (bodyUpDot > 0f)
                {
                    var angleError = SignedAngle(BodyUp(), targetNormal);
                    torque += angleError * GroundAlignStrength - AngularVelocity * GroundAlignDamping;
                }
            }

            var groundedTangentSpeed = 0f;
            if (backGrounded) groundedTangentSpeed += Vector2.Dot(Velocity, backTangent);
            if (frontGrounded) groundedTangentSpeed += Vector2.Dot(Velocity, frontTangent);
            groundedTangentSpeed /= groundedCount;

            var targetSpin = groundedTangentSpeed / WheelRadius;
            var gripLerp = 1f - MathF.Exp(-WheelGroundGrip * dt);
            _wheelSpin = _wheelSpin + (targetSpin - _wheelSpin) * gripLerp;
        }
        else if (driveInput != 0f)
        {
            torque += driveInput * (2.0f + MathF.Abs(_wheelSpin) * AirControlFromWheelSpin);
        }

        _wheelSpin *= (float)Math.Exp(-WheelAngularDamping * dt);

        AngularVelocity += (torque / ChassisInertia) * dt;

        AngularVelocity *= (float)Math.Exp(-AngularDamp * dt);

        Velocity += accel * dt;
        Position += Velocity * dt;
        Rotation += AngularVelocity * dt;

        if (Rotation > MathF.PI) Rotation -= 2 * MathF.PI;
        if (Rotation < -MathF.PI) Rotation += 2 * MathF.PI;

        UpdateWheelWorldPositions();

        if (!grounded)
            return;
    }

    private void ApplyBrake(bool grounded, Vector2 wheelPos, Vector2 tangent, ref Vector2 accel, ref float torque)
    {
        if (!grounded)
            return;

        var tangentSpeed = Vector2.Dot(Velocity, tangent);
        if (MathF.Abs(tangentSpeed) < 0.02f)
            return;

        var brakeForce = -Math.Sign(tangentSpeed) * BrakeForce * tangent;
        accel += brakeForce;
        torque += TorqueFromForce(wheelPos, brakeForce);
    }

    private float TorqueFromForce(Vector2 applicationPoint, Vector2 force)
    {
        var r = applicationPoint - Position;
        return r.X * force.Y - r.Y * force.X;
    }

    private static float SignedAngle(Vector2 from, Vector2 to)
    {
        var cross = from.X * to.Y - from.Y * to.X;
        var dot = Vector2.Dot(from, to);
        return MathF.Atan2(cross, dot);
    }

    private void UpdateWheelWorldPositions()
    {
        WheelBackPos = Position + Rotate(_wheelBackLocal, Rotation);
        WheelFrontPos = Position + Rotate(_wheelFrontLocal, Rotation);
    }

    private bool SolveWheelSuspension(
        float dt,
        Vector2 wheelCenter,
        out Vector2 suspensionForce,
        out Vector2 tangent,
        out float compression01)
    {
        var groundY = Terrain.HeightAt(wheelCenter.X);
        var normal = Terrain.NormalAt(wheelCenter.X);
        tangent = new Vector2(normal.Y, -normal.X);

        var wheelBottomY = wheelCenter.Y - WheelRadius;
        var penetration = groundY - wheelBottomY;

        var compression = penetration - SuspensionRest;

        if (penetration <= 0)
        {
            suspensionForce = Vector2.Zero;
            compression01 = 0;
            return false;
        }

        var spring = SuspensionK * compression;
        var vAlongN = Vector2.Dot(Velocity, normal);
        var damper = -SuspensionD * vAlongN;

        var forceMag = spring + damper;

        forceMag = Math.Clamp(forceMag, 0, 250);

        suspensionForce = normal * forceMag;

        compression01 = Math.Clamp(penetration / (WheelRadius * 2f), 0, 1);
        return true;
    }

    private Vector2 BodyUp()
    {
        return Rotate(Vector2.UnitY, Rotation);
    }

    private static Vector2 Rotate(Vector2 v, float radians)
    {
        var c = MathF.Cos(radians);
        var s = MathF.Sin(radians);
        return new Vector2(v.X * c - v.Y * s, v.X * s + v.Y * c);
    }
}