Fix ball penetrating arena under sustained force

[Z-OMG]

No description provided.

[ZealanL]

Interesting... how does this change affect collision accuracy? Is there a reproducible case for this? How easily can this bug occur? Many questions...

[Z-OMG]

RocketSim V3 has issues with the ball penetration, it is a fact and its tested.
V3's contact solver did

if cp.is_special { …; continue; }

for ball-vs-world contacts, skipping the per point constraint that carries the penetration depth.
So the ball gets zero positional recovery and sinks into the floor/wall under sustained force.
It is tested in the latest version of RocketSim V3.
I tested it by putting a sustained force on the ball towards the ground, then it sank underground.

[Z-OMG]

And what I did to fix it:

1. Accumulate the real penetration alongside the existing sums: total_penetration += cp.distance_1

2. In convert_contact_special, feed the averaged real penetration (total_penetration/num) into the split-impulse positional term of that single averaged constraint.

[VirxEC]

Are we sure let distance = sri.total_dist / num_collisions; shouldn't just be replaced with avg_penetration

[Z-OMG]

No, distance has to stay total_dist / num_collisions. It's used for two unrelated things, and only one of them is the penetration:

1. rel_pos1 = normal_world_on_b * -distance

2. let penetration = distance;

distance = radius (contact geometry), avg_penetration = real depth (positional recovery). They're not interchangeable, so merging them regresses the contact solve.

[Kuenec]

distance is always positive (it's ~ball radius, from rel_pos.length and the solver only applies positional correction when penetration <= 0. So with penetration = distance, the correction was literally never applied, that's why the ball sinks the fix feeds in the real depth, and does nothing when the ball isn't penetrating. Can't swap distance out entirely though, it's still needed for the rel_pos1 lever arm.

MERGE THIS SHIT ZEALAN CHOP CHOP

[ZealanL]

I will verify that this matches what actual RL does, and if so I will merge it.
If it doesn't match RL I will not be merging, even if it results in more consistent and fool-proof collisions.
I would also like a failure-case (like an initial state to set) to produce the issue.

[Z-OMG]

This is a state set to reproduce the same issue:

use glam::Vec3A;
use rocketsim::{Arena, GameMode};

fn run(label: &str, start_z: f32, force_z: f32, ticks: u32) {
    let mut a = Arena::new(GameMode::Soccar);
    let mut bs = *a.get_ball_state();
    bs.phys.pos = Vec3A::new(0.0, 0.0, start_z);
    bs.phys.vel = Vec3A::ZERO;
    a.set_ball_state(bs);

    println!("\n=== {label}  (start z={start_z}, force_z={force_z}/tick) ===");
    print!("z: ");
    let mut min_z = start_z;
    for t in 0..ticks {
        let mut bs = *a.get_ball_state();
        bs.phys.vel.z += force_z; // continues downward force
        a.set_ball_state(bs);
        a.step_tick();
        let z = a.get_ball_state().phys.pos.z;
        min_z = min_z.min(z);
        if t < 8 || t % 30 == 0 {
            print!("{z:.1} ");
        }
    }
    println!(
        "\n   min z over run = {min_z:.1}  (floor rest 93.15, the deeper it is the more the penetration)"
    );
}

fn main() {
    rocketsim::init("Ur collision_meshes folder", true).expect("meshes");
    // A: at rest ball pressed hard into the floor.
    run("A: rest + hard press", 93.15, -2000.0, 120);
    // B: ball already just under the floor, held down (it should come back up in RocektSim V2 and in RL).
    run("B: below-floor", 40.0, -200.0, 120)