Shape casting inconsistencies

[Mik-pe]

While attempting a sort of height-clamping I noticed my bodies were colliding when I set them to the given TOI result. I narrowed down the issue and noticed the same issue with Cuboid colliders.

My least means of reproducing is as follows:

2x Cuboids (10x15x5 in the video), one slightly rotated as it seems to trigger the overlap/collisions. one HalfSpace representing the "ground".

I do a shape cast with the rotated cuboid straight down, filtering the rotated collider. I set the position of the rotated collider similar as to how the rapier character controller does its' ground snapping.

In my test application, I color colliding colliders red and re-clamp the "moving" cuboid each frame. The intersection seems to be stable most of the times, except what I would guess is at the edges. I get no issues when clamping the collider on top of the underlying cuboid.

https://user-images.githubusercontent.com/5653426/211317672-d4b20a8a-5ec2-47f2-8b05-ddca720b7987.mp4

[sebcrozet]

Thanks for pointing this out. This is probably a numerical/rounding issue in the shape-cast code. If you can, could you please provide the exact initial position, and shape-cast parameter for one of the configuration where the shape-cast appears to fail ? That way we can reproduce the proble reliably for debugging.

[Mik-pe]

Sure!

Here's a small test case with some values I found. It doesn't fail for only these values but hopefully this points in the right direction:

#[test]
fn test_height_clamp() {
    let mut colliders = ColliderSet::new();
    let mut bodies = RigidBodySet::new();
    let mut query_pipeline = QueryPipeline::new();

    let cuboid = ColliderBuilder::cuboid(10.0, 15.0, 5.0).build();
    let rb = RigidBodyBuilder::dynamic()
        .translation(vector![100.0, 100.0, 20.0])
        .build();
    let body1_handle = bodies.insert(rb);
    colliders.insert_with_parent(cuboid, body1_handle, &mut bodies);

    let cuboid = ColliderBuilder::cuboid(10.0, 15.0, 5.0)
        .rotation(AngVector::new(0.0, PI / 4.0, 0.0))
        .build();
    let rb = RigidBodyBuilder::dynamic()
        .translation(vector![100.0, 100.0, 20.0])
        .build();
    let body2_handle = bodies.insert(rb);
    let b2_co = colliders.insert_with_parent(cuboid, body2_handle, &mut bodies);

    //This ground plane does make a difference since we don't get any shape collision with b1 in the shape cast
    //the b2 cast will hit the ground and clamp to ~10 in Z
    let ground_plane = ColliderBuilder::halfspace(Vector::z_axis()).build();
    colliders.insert(ground_plane);

    query_pipeline.update(&bodies, &colliders);

    let dist = 5000.0;
    let down = -Vector::z();
    let shape = colliders.get(b2_co).unwrap().shape();
    let mut pos = *colliders.get(b2_co).unwrap().position();

    pos.translation.x = 112.05581;
    //Changing this to e.g. 103.65648; makes the test succeed
    pos.translation.y = 103.75648;
    pos.translation.z = dist;

    let query_filter = QueryFilter::default().exclude_rigid_body(body2_handle);
    if let Some((_, hit)) = query_pipeline.cast_shape(
        &bodies,
        &colliders,
        &pos,
        &down,
        shape,
        dist,
        true,
        query_filter,
    ) {
        let res_z = dist - hit.toi;
        //"Real" cast should hit b1 at ~30Z
        println!("Hit at Z: {}", res_z);
        assert!(res_z > 20.0);
        pos.translation.z = res_z;
    }
}

The resulting position of the bodies after shape casting: