Incorrect Qvbh time_of_impact results

[dubrowgn]

I've been trying to track down an issue for a while in my game where the character "glitches" into walls while sliding against them. I'm using a time-of-impact query via Qbvh. It seemed to happen relatively consistently in roughly the same spots, so I collected position and velocity traces and came up with the following case that seems to 100% reproduce the issue. It appears the Qbvh TOI query inexplicably returns no intersections in cases where it definitely should.

Consider the following minimum reproducible example:

// == Setup ==

let b = Ball::new(96.0);
let b_pos = na::Vector2::new(216.02324, -1632.0032);
let b_vel = na::Vector2::new(-636.3961, -636.3961);

let c = Cuboid::new(na::Vector2::new(2560.0 / 2.0, 192.0 / 2.0));
let c_pos = na::Vector2::new(0.0, -1824.0);

// == TOI via DefaultQueryDispatcher directly ==

let pos12 = na::Isometry2::new(b_pos - c_pos, 0.0);
let res = DefaultQueryDispatcher{}.time_of_impact(
    &pos12,
    &b_vel,
    &b,
    &c,
    1.0 / 60.0,
    true,
);
println!("{:?}", res);
// Ok(Some(TOI { toi: 3.3994795e-8, witness1: [0.59887993, 95.9968], witness2: [-215.42442, -96.0], normal1: [[-0.0, 1.0]], normal2: [[0.0, -1.0]], status: Converged }))

// == TOI via Qbvh ==

#[derive(Copy, Clone)]
pub struct DummyData;

impl IndexedData for DummyData {
    fn default() -> Self { DummyData{} }
    fn index(&self) -> usize { 0 }
}

pub struct SingleCompositeShape<'a> {
    bvh: &'a Qbvh::<DummyData>,
    pos: &'a na::Vector2<Real>,
    shape: &'a Cuboid,
}

impl<'a> TypedSimdCompositeShape for SingleCompositeShape<'a> {
    type PartShape = dyn Shape;
    type PartId = DummyData;
    type QbvhStorage = DefaultStorage;

    fn map_typed_part_at(
        &self,
        _: Self::PartId,
        mut f: impl FnMut(Option<&Isometry<Real>>, &Self::PartShape),
    ) {
        f(Some(&Isometry2::new(*self.pos, 0.0)), self.shape);
    }

    fn map_untyped_part_at(
        &self,
        shape_id: Self::PartId,
        f: impl FnMut(Option<&Isometry<Real>>, &Self::PartShape),
    ) {
        self.map_typed_part_at(shape_id, f);
    }

    fn typed_qbvh(&self) -> &Qbvh<DummyData> {
        &self.bvh
    }
}

struct SingleDataGenerator {
    aabb: Aabb,
}

impl QbvhDataGenerator<DummyData> for SingleDataGenerator {
    fn size_hint(&self) -> usize { 1 }
    fn for_each(&mut self, mut f: impl FnMut(DummyData, Aabb)) {
        f(DummyData{}, self.aabb);
    }
}

let mut bvh = Qbvh::<DummyData>::new();
let gen = SingleDataGenerator {
    aabb: c.compute_aabb(&Isometry2::new(c_pos, 0.0)),
};
bvh.clear_and_rebuild( gen, 0.0);

let dispatcher = DefaultQueryDispatcher{};
let shapes = SingleCompositeShape { bvh: &bvh, pos: &c_pos, shape: &c };
let b_iso = Isometry2::new(b_pos, 0.0);
let mut visitor = TOICompositeShapeShapeBestFirstVisitor::new(
    &dispatcher,
    &b_iso,
    &b_vel,
    &shapes,
    &b,
    1.0/60.0,
    true,
);

match bvh.traverse_best_first(&mut visitor).map(|h| h.1) {
    Some((_, toi)) => println!("Result: {:?}", toi),
    None => println!("Result: None"),
}
// Result: None

As commented in the code, the above code results in the following console output:

Ok(Some(TOI { toi: 3.3994795e-8, witness1: [0.59887993, 95.9968], witness2: [-215.42442, -96.0], normal1: [[-0.0, 1.0]], normal2: [[0.0, -1.0]], status: Converged }))
Result: None

We get the expected results when invoking DefaultQueryDispatcher::time_of_impact() directly, but get no results when calling bvh.traverse_best_first(TOICompositeShapeShapeBestFirstVisitor) with the same data. However, if we tweak the ball's position slightly, we get the expected result in both cases:

let b_pos = na::Vector2::new(216.02324, -1632.0); // instead of (216.02324, -1632.0032)

We then get the following output:

Ok(Some(TOI { toi: 3.0255871e-6, witness1: [0.0, -0.0017995844], witness2: [-216.02127, -191.99988], normal1: [[-0.0, 1.0]], normal2: [[0.0, -1.0]], status: Converged }))
Result: TOI { toi: 3.0255871e-6, witness1: [-0.0016784668, -1824.0018], witness2: [-216.02295, -191.99988], normal1: [[-0.0, 1.0]], normal2: [[0.0, -1.0]], status: Converged }

My Cargo.toml has

parry2d = { version = "0.11.1", features = [ "enhanced-determinism" ] }