3D collisions: primitive shapes and compound bodies

using System;
using Reckfell;

public partial class LabBody3D : RigidBody3D
{
    private Vector3? _resetPosition;
    public int ResetCount { get; private set; }
    public void RequestReset(Vector3 worldPosition)
    {
        if (!worldPosition.IsFinite()) throw new ArgumentOutOfRangeException(nameof(worldPosition));
        _resetPosition = worldPosition;
        Sleeping = false;
    }
    public override void _IntegrateForces(PhysicsDirectBodyState3D state)
    {
        if (_resetPosition is not Vector3 position) return;
        state.Transform = new Transform3D(Basis.Identity, position);
        state.LinearVelocity = Vector3.Zero;
        state.AngularVelocity = Vector3.Zero;
        _resetPosition = null;
        ResetCount++;
    }
}

using Reckfell;
using Hit = Reckfell.Collections.Dictionary;

public partial class ScreenPicker3D : Entity
{
    [Export] public Camera3D? Camera { get; set; }
    [Export] public uint Mask { get; set; } = 4;
    [Export] public float Distance { get; set; } = 100;
    private Vector2? _pending;
    public Hit LastHit { get; private set; } = new();
    public int QueryCount { get; private set; }
    public void RequestPick(Vector2 viewportPosition) { if (viewportPosition.IsFinite()) _pending = viewportPosition; }
    public override void _UnhandledInput(InputEvent @event)
    {
        if (@event is InputEventMouseButton { Pressed: true, ButtonIndex: MouseButton.Left } click)
            RequestPick(click.Position);
    }
    public override void _PhysicsProcess(double delta)
    {
        if (_pending is not Vector2 screen) return;
        _pending = null; LastHit = new();
        if (Camera is null || !ReckfellObject.IsInstanceValid(Camera) || !Camera.IsInsideTree() || !float.IsFinite(Distance) || Distance <= 0) return;
        Vector3 origin = Camera.ProjectRayOrigin(screen);
        Vector3 end = origin + Camera.ProjectRayNormal(screen) * Distance;
        var query = PhysicsRayQueryParameters3D.Create(origin, end, Mask);
        query.CollideWithAreas = false;
        LastHit = Camera.GetWorld3D().DirectSpaceState.IntersectRay(query);
        QueryCount++;
    }
}

using Reckfell;
using Hit = Reckfell.Collections.Dictionary;

public partial class CollisionLab3D : Entity3D
{
    public LabBody3D Crate { get; private set; } = null!;
    public StaticBody3D Floor { get; private set; } = null!;
    public StaticBody3D Wall { get; private set; } = null!;
    public RayCast3D Ray { get; private set; } = null!;
    public Camera3D Camera { get; private set; } = null!;
    public ScreenPicker3D Picker { get; private set; } = null!;
    public int Contacts { get; private set; }
    public override void _Ready()
    {
        Floor = StaticBox("Floor", new Vector3(0, -.25f, 0), new Vector3(12, .5f, 8));
        Wall = StaticBox("Wall", new Vector3(3, 1.5f, 0), new Vector3(.5f, 3, 4));
        Crate = new LabBody3D { Name = "Crate", Position = new Vector3(0, 3, 0), Mass = 2,
            LockRotation = true, CollisionLayer = 4, CollisionMask = 1, ContactMonitor = true, MaxContactsReported = 8,
            PhysicsMaterialOverride = new PhysicsMaterial { Bounce = 0, Friction = 1 } };
        Crate.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = Vector3.One } });
        AddVisual(Crate, Vector3.One, new Color(.25f, .45f, 1));
        Crate.BodyEntered += body => { if (body == Floor) Contacts++; };
        AddChild(Crate);
        Ray = new RayCast3D { Position = new Vector3(-4, 1.5f, 0), TargetPosition = new Vector3(8, 0, 0),
            CollisionMask = 1, CollideWithAreas = false, Enabled = true }; AddChild(Ray);
        Camera = new Camera3D { Position = new Vector3(8, 7, 10), Current = true }; AddChild(Camera);
        Camera.LookAt(ToGlobal(new Vector3(0, 1, 0)));
        Picker = new ScreenPicker3D { Camera = Camera }; AddChild(Picker);
        var light = new DirectionalLight3D { RotationDegrees = new Vector3(-55, -30, 0), LightEnergy = 1.3f }; AddChild(light);
        var environment = new Reckfell.Environment { BackgroundMode = Reckfell.Environment.BGMode.Color,
            BackgroundColor = new Color(.035f, .035f, .045f), AmbientLightSource = Reckfell.Environment.AmbientSource.Color,
            AmbientLightColor = Colors.White, AmbientLightEnergy = .55f };
        AddChild(new WorldEnvironment { Environment = environment });
    }
    private StaticBody3D StaticBox(string name, Vector3 position, Vector3 size)
    {
        var body = new StaticBody3D { Name = name, Position = position, CollisionLayer = 1, CollisionMask = 0 };
        body.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = size } });
        AddVisual(body, size, new Color(.3f, .3f, .34f)); AddChild(body); return body;
    }
    private static void AddVisual(Entity3D parent, Vector3 size, Color color)
    {
        parent.AddChild(new MeshInstance3D { Mesh = new BoxMesh { Size = size }, MaterialOverride =
            new StandardMaterial3D { AlbedoColor = color, Roughness = .85f } });
    }
    public Hit Cast(Vector3 worldFrom, Vector3 worldTo, uint mask = 1, Rid? exclude = null, bool inside = false)
    {
        var query = PhysicsRayQueryParameters3D.Create(worldFrom, worldTo, mask);
        query.HitFromInside = inside;
        if (exclude is Rid rid) query.Exclude = new Reckfell.Collections.Array<Rid> { rid };
        return GetWorld3D().DirectSpaceState.IntersectRay(query);
    }
    public override void _UnhandledKeyInput(InputEvent @event)
    {
        if (@event is not InputEventKey { Pressed: true, Echo: false } key) return;
        if (key.PhysicalKeycode == Key.R) Crate.RequestReset(ToGlobal(new Vector3(0, 3, 0)));
        if (key.PhysicalKeycode == Key.Space) Crate.ApplyCentralImpulse(Vector3.Up * 10);
    }
}