Syntriax.Engine/Engine.Physics2D/RigidBody2D.cs

using System.Collections.Generic;

using Engine.Core;

namespace Engine.Physics2D;

public class RigidBody2D : Behaviour2D, IRigidBody2D, IFirstFrameUpdate, ILastFrameUpdate
{
    private const float LOWEST_ALLOWED_MASS = 0.00001f;

    public IPhysicsMaterial2D Material { get; set; } = ReadOnlyPhysicsMaterial2D.Default;

    public Vector2D Velocity { get; set; } = Vector2D.Zero;
    public float AngularVelocity { get; set; } = 0f;
    public bool IsStatic { get; set; } = false;

    public float Mass { get; set { field = Math.Max(value, LOWEST_ALLOWED_MASS); } } = 1f;
    public float InverseMass { get; protected set; } = 1f;

    public float Inertia { get; protected set; } = 1f;
    public float InverseInertia { get; protected set; } = 1f;

    private readonly List<ICollider2D> childColliders = [];

    public void LastActiveFrame() => DisconnectColliders();
    public void FirstActiveFrame()
    {
        ReconnectColliders();
        UpdateValues();
    }

    private void ReconnectColliders()
    {
        DisconnectColliders();

        BehaviourController.GetBehavioursInChildren(childColliders);

        foreach (ICollider2D collider in childColliders)
            collider.OnRecalculated.AddListener(RecalculateCallback);
    }

    private void DisconnectColliders()
    {
        foreach (ICollider2D collider in childColliders)
            collider.OnRecalculated.RemoveListener(RecalculateCallback);
    }

    private void RecalculateCallback(ICollider2D _) => UpdateValues();
    private void UpdateValues()
    {
        InverseMass = Mass.OneOver();

        Vector2D center = Transform.Position;
        Inertia = 0f;

        float totalColliderArea = 0f;

        foreach (ICollider2D collider in childColliders)
            totalColliderArea += collider.Area;

        foreach (ICollider2D collider in childColliders)
        {
            float colliderMass = Mass * (collider.Area / totalColliderArea);
            float colliderInertia = collider.GeometricInertia * colliderMass;
            float distanceSquared = center.FromTo(collider.Transform.Position).MagnitudeSquared;

            Inertia += colliderInertia + colliderMass * distanceSquared;
        }

        if (childColliders.Count == 0)
            Inertia = 1f;

        InverseInertia = Inertia.OneOver();
    }
}