False negatives in capsule-cuboid intersection tests

[dataphract]

Sweeping a capsule through a cuboid with repeated intersection tests seems to produce multiple false negatives:

use parry3d::{
    na::{Isometry3, Translation3, Unit, Vector3},
    query::intersection_test,
    shape::{Ball, Capsule, Cuboid, HalfSpace},
};

fn main() {
    let capsule = Capsule::new([0.0, -0.5, 0.0].into(), [0.0, 0.5, 0.0].into(), 0.5);

    // This capsule, equivalent to the ball, also produces false negatives
    // let capsule = Capsule::new([0.0, 0.0, 0.0].into(), [0.0, 0.0, 0.0].into(), 0.5);

    let ball = Ball::new(0.5);

    let halfspace = HalfSpace::new(Unit::new_normalize(Vector3::from([0.0, 1.0, 0.0])));

    // Upper face of the cuboid is coplanar with the outer face of the halfspace
    let cuboid = Cuboid::new([50.0, 50.0, 50.0].into());
    let cuboid_pos = Isometry3 {
        translation: Translation3::from(Vector3::from([0.0, -50.0, 0.0])),
        ..Default::default()
    };

    let steps = 200;
    let y_max = 0.5;
    let y_min = -0.5;
    let step_size = (y_max - y_min) / steps as f32;

    let mut capsule_cuboid = 0;
    let mut capsule_halfspace = 0;
    let mut ball_cuboid = 0;
    let mut ball_halfspace = 0;

    for step in 0..steps {
        let y = y_min + step_size * step as f32;

        let test_pos = Isometry3 {
            translation: Translation3::from([0.0, y, 0.0]),
            ..Default::default()
        };

        if intersection_test(&test_pos, &capsule, &Isometry3::default(), &halfspace).unwrap() {
            capsule_halfspace += 1;
        }

        if intersection_test(&test_pos, &capsule, &cuboid_pos, &cuboid).unwrap() {
            capsule_cuboid += 1;
        }

        if intersection_test(&test_pos, &ball, &Isometry3::default(), &halfspace).unwrap() {
            ball_halfspace += 1;
        }

        if intersection_test(&test_pos, &ball, &cuboid_pos, &cuboid).unwrap() {
            ball_cuboid += 1;
        }
    }

    println!("capsule-cuboid intersections:    {}/{}", capsule_cuboid, steps);
    println!("capsule-halfspace intersections: {}/{}", capsule_halfspace, steps);
    println!("ball-cuboid intersections:       {}/{}", ball_cuboid, steps);
    println!("ball-halfspace intersections:    {}/{}", ball_halfspace, steps);
}

Outputs:

capsule-cuboid intersections:    86/200
capsule-halfspace intersections: 200/200
ball-cuboid intersections:       200/200
ball-halfspace intersections:    200/200

[ThierryBerger]

This is another case that would be fixed by increasing the absolute tolerance of the GJK algorithm (related to #298)

MRP reformatted to test

#[cfg(test)]
#[cfg(feature = "dim3")]
#[cfg(feature = "f32")]
mod test {

    use std::println;

    use crate::{
        na::{Isometry3, Translation3, Unit, Vector3},
        query::intersection_test,
        shape::{Ball, Capsule, Cuboid, HalfSpace},
    };

    #[test]
    fn test_capsule_cuboid() {
        let capsule = Capsule::new([0.0, -0.5, 0.0].into(), [0.0, 0.5, 0.0].into(), 0.5);

        // This capsule, equivalent to the ball, also produces false negatives
        // let capsule = Capsule::new([0.0, 0.0, 0.0].into(), [0.0, 0.0, 0.0].into(), 0.5);

        let ball = Ball::new(0.5);

        let halfspace = HalfSpace::new(Unit::new_normalize(Vector3::from([0.0, 1.0, 0.0])));

        // Upper face of the cuboid is coplanar with the outer face of the halfspace
        let cuboid = Cuboid::new([50.0, 50.0, 50.0].into());
        let cuboid_pos = Isometry3 {
            translation: Translation3::from(Vector3::from([0.0, -50.0, 0.0])),
            ..Default::default()
        };

        let steps = 200;
        let y_max = 0.5;
        let y_min = -0.5;
        let step_size = (y_max - y_min) / steps as f32;

        let mut capsule_cuboid = 0;
        let mut capsule_halfspace = 0;
        let mut ball_cuboid = 0;
        let mut ball_halfspace = 0;

        for step in 0..steps {
            let y = y_min + step_size * step as f32;

            let test_pos = Isometry3 {
                translation: Translation3::from([0.0, y, 0.0]),
                ..Default::default()
            };

            if intersection_test(&test_pos, &capsule, &Isometry3::default(), &halfspace).unwrap() {
                capsule_halfspace += 1;
            }

            if intersection_test(&test_pos, &capsule, &cuboid_pos, &cuboid).unwrap() {
                capsule_cuboid += 1;
            }

            if intersection_test(&test_pos, &ball, &Isometry3::default(), &halfspace).unwrap() {
                ball_halfspace += 1;
            }

            if intersection_test(&test_pos, &ball, &cuboid_pos, &cuboid).unwrap() {
                ball_cuboid += 1;
            }
        }

        println!(
            "capsule-cuboid intersections:    {}/{}",
            capsule_cuboid, steps
        );
        println!(
            "capsule-halfspace intersections: {}/{}",
            capsule_halfspace, steps
        );
        println!("ball-cuboid intersections:       {}/{}", ball_cuboid, steps);
        println!(
            "ball-halfspace intersections:    {}/{}",
            ball_halfspace, steps
        );
    }
}