When EPA is stuck it returns pt2 in the wrong spot

[dylanowen]

In src/query/contact/contact_support_map_support_map.rs:40 when GJK detects an intersection because the shapes are within the prediction range EPA gives the wrong value for point2 of the contact.

Test Case

#[cfg(test)]
mod tests {
    use std::println;
    use crate::math::Point;
    use crate::shape::SharedShape;
    use super::*;

    #[test]
    fn test() {
        let one = SharedShape::capsule(Point::new(0., -0.25), Point::new(0., 0.25), 0.5);
        let two = SharedShape::cuboid(4., 0.5);

        let good = (Vector::new(4.0, 1.5) - Vector::new(6.0373735, 2.7391)).into();
        let bad = (Vector::new(4.0, 1.5) - Vector::new(6.0373735, 2.75)).into();

        println!("good {:?}", contact_support_map_support_map(&good, one.as_support_map().unwrap(), two.as_support_map().unwrap(), 0.002));
        println!("bad {:?}", contact_support_map_support_map(&bad, one.as_support_map().unwrap(), two.as_support_map().unwrap(), 0.002));
    }
}

Gives us

good Some(Contact { point1: [-7.131596e-7, -0.75], point2: [2.0373726, 0.5], normal1: [[0.0, -1.0]], normal2: [[0.0, 1.0]], dist: -0.010900021 })
bad Some(Contact { point1: [0.0, -0.75], point2: [4.0, 0.5], normal1: [[3.426553e-7, -1.0]], normal2: [[-3.426553e-7, 1.0]], dist: 6.7250437e-7 })

For point2 in bad ([4.0, 0.5]) the point is much further from the expected spot.

This is a rendering, that right circle is the point2 contact location.

After reading the discussions and #245 maybe this is already as solved as it can be? The current state is certainly better than it was before 😄