Motion hitches with CCD enabled with largeish dt

[Ralith]

When simulating collisions with dt = 0.25, the real linear and angular velocity of bodies under CCD is prone to hitching, i.e. abruptly dropping close to 0 before returning to a more reasonable value. The effect seems to weaken with smaller dt, e.g. becoming small at 0.1 and invisible at 0.025 even when visualized. I haven't been able to reproduce the issue with CCD disabled. I have observed this with other combinations of shapes, e.g. capsule vs. cuboid, cuboid vs. trimesh. No amount of increase to CCD substeps seems to help. All testing was done on rapier3d-f64.

I understand that accuracy is compromised for sufficiently large values of dt, but this seems like a pretty weird error even so.

Example motion log at dt = 0.25, with CCD:

step - angle between orientations - distance between positions - position
0 - 0.0000 0.0000 at [[0.0000, 4.9998, 0.0500]]
1 - 0.0000 0.6131 at [[0.0000, 4.3866, 0.0500]]
2 - 0.0000 1.2263 at [[0.0000, 3.1604, 0.0500]]
3 - 0.0000 2.1594 at [[0.0000, 1.0009, 0.0500]] // Cylinder impacts surface
4 - 0.2203 0.2404 at [[0.0036, 1.0229, -0.1894]]
5 - 0.0094 0.0271 at [[0.0037, 0.9978, -0.1996]] // First hitch
6 - 0.3407 0.3422 at [[0.0120, 0.9568, -0.5392]]
7 - 0.0388 0.0840 at [[0.0129, 0.8823, -0.5779]] // Second hitch
8 - 0.7730 0.7606 at [[0.0332, 0.5106, -1.2411]]
9 - 0.7730 0.7606 at [[0.0535, 0.1388, -1.9044]]
10 - 0.7730 1.1876 at [[0.0739, -0.8461, -2.5676]]
11 - 0.7730 1.7303 at [[0.0942, -2.4441, -3.2308]]
12 - 0.7730 2.3086 at [[0.1145, -4.6553, -3.8940]]
13 - 0.7730 2.9012 at [[0.1348, -7.4796, -4.5572]]
14 - 0.7730 3.5009 at [[0.1551, -10.9170, -5.2204]]
15 - 0.7730 4.1045 at [[0.1754, -14.9675, -5.8837]]
16 - 0.7730 4.7106 at [[0.1957, -19.6311, -6.5469]]
17 - 0.7730 5.3183 at [[0.2161, -24.9079, -7.2101]]
18 - 0.7730 5.9272 at [[0.2364, -30.7978, -7.8733]]
19 - 0.7730 6.5368 at [[0.2567, -37.3008, -8.5365]]
20 - 0.7730 7.1470 at [[0.2770, -44.4170, -9.1998]]
21 - 0.7730 7.7577 at [[0.2973, -52.1463, -9.8630]]
22 - 0.7730 8.3687 at [[0.3176, -60.4886, -10.5262]]
23 - 0.7730 8.9801 at [[0.3379, -69.4442, -11.1894]]
24 - 0.7730 9.5916 at [[0.3582, -79.0128, -11.8526]]

dt = 0.1, with CCD:

0 - 0.0000 0.0000 at [[0.0000, 4.9998, 0.0500]]
1 - 0.0000 0.0981 at [[0.0000, 4.9017, 0.0500]]
2 - 0.0000 0.1962 at [[0.0000, 4.7055, 0.0500]]
3 - 0.0000 0.2943 at [[0.0000, 4.4112, 0.0500]]
4 - 0.0000 0.3924 at [[0.0000, 4.0188, 0.0500]]
5 - 0.0000 0.4905 at [[0.0000, 3.5283, 0.0500]]
6 - 0.0000 0.5886 at [[0.0000, 2.9397, 0.0500]]
7 - 0.0000 0.6867 at [[0.0000, 2.2530, 0.0500]]
8 - 0.0000 0.7848 at [[0.0000, 1.4682, 0.0500]]
9 - 0.0262 0.4684 at [[0.0013, 1.0006, 0.0215]] // Cylinder impacts surface
10 - 0.0526 0.0547 at [[0.0034, 1.0050, -0.0330]]
11 - 0.0519 0.0519 at [[0.0057, 1.0061, -0.0848]]
12 - 0.0535 0.0534 at [[0.0081, 1.0037, -0.1381]]
13 - 0.0589 0.0589 at [[0.0109, 0.9979, -0.1967]]
14 - 0.0686 0.0686 at [[0.0144, 0.9872, -0.2643]]
15 - 0.0686 0.0686 at [[0.0180, 0.9765, -0.3319]]
16 - 0.0686 0.1282 at [[0.0215, 0.8677, -0.3996]]
17 - 0.1675 0.1365 at [[0.0303, 0.8337, -0.5315]]
18 - 0.1870 0.1748 at [[0.0413, 0.7531, -0.6862]]
19 - 0.2254 0.2213 at [[0.0544, 0.6117, -0.8560]]
20 - 0.2832 0.2759 at [[0.0689, 0.3940, -1.0248]]
21 - 0.0723 0.0915 at [[0.0726, 0.3133, -1.0679]] // Hitch
22 - 0.2455 0.2425 at [[0.0813, 0.0882, -1.1576]]
23 - 0.6089 0.4004 at [[0.0934, -0.2972, -1.2654]]
24 - 0.6093 0.4010 at [[0.1056, -0.6832, -1.3732]]

dt = 0.1, without CCD:

0 - 0.0000 0.0000 at [[0.0000, 4.9998, 0.0500]]
1 - 0.0000 0.0981 at [[0.0000, 4.9017, 0.0500]]
2 - 0.0000 0.1962 at [[0.0000, 4.7055, 0.0500]]
3 - 0.0000 0.2943 at [[0.0000, 4.4112, 0.0500]]
4 - 0.0000 0.3924 at [[0.0000, 4.0188, 0.0500]]
5 - 0.0000 0.4905 at [[0.0000, 3.5283, 0.0500]]
6 - 0.0000 0.5886 at [[0.0000, 2.9397, 0.0500]]
7 - 0.0000 0.6867 at [[0.0000, 2.2530, 0.0500]]
8 - 0.0000 0.7848 at [[0.0000, 1.4682, 0.0500]]
9 - 0.0000 0.8829 at [[0.0000, 0.5853, 0.0500]] // Cylinder intersects surface
10 - 0.1560 0.2743 at [[0.0037, 0.8506, -0.0194]] // Note almost-monotonic increase in angular velocity
11 - 0.1322 0.1073 at [[0.0110, 0.9215, -0.0996]]
12 - 0.1426 0.0944 at [[0.0195, 0.9219, -0.1936]]
13 - 0.1552 0.1169 at [[0.0349, 0.8938, -0.3061]]
14 - 0.1847 0.1475 at [[0.0548, 0.8341, -0.4395]]
15 - 0.2460 0.1869 at [[0.0816, 0.7325, -0.5940]]
16 - 0.2460 0.1869 at [[0.1084, 0.6309, -0.7486]]
17 - 0.2460 0.2539 at [[0.1353, 0.4312, -0.9031]]
18 - 0.5963 0.3594 at [[0.1787, 0.1078, -1.0537]]
19 - 0.9046 0.3347 at [[0.2146, -0.2095, -1.1540]]
20 - 0.9590 0.3371 at [[0.2505, -0.5293, -1.2544]]
21 - 0.9590 0.4313 at [[0.2863, -0.9473, -1.3547]]
22 - 0.9590 0.5269 at [[0.3222, -1.4633, -1.4550]]
23 - 0.9590 0.6233 at [[0.3580, -2.0774, -1.5553]]
24 - 0.9590 0.7202 at [[0.3939, -2.7897, -1.6556]]

Full test case:

extern crate nalgebra as na;
use rapier3d_f64::{
    dynamics::{
        CCDSolver, IntegrationParameters, IslandManager, JointSet, RigidBodyBuilder, RigidBodySet,
        RigidBodyType,
    },
    geometry::{
        BroadPhase, ColliderBuilder, ColliderSet, Cuboid, Cylinder, NarrowPhase, SharedShape,
    },
    pipeline::PhysicsPipeline,
};

fn main() {
    let mut islands = IslandManager::new();
    let mut bodies = RigidBodySet::new();
    let mut colliders = ColliderSet::new();

    let mut pipeline = PhysicsPipeline::new();
    let integration_parameters = IntegrationParameters {
        dt: 0.25,
        max_ccd_substeps: 128,

        ..IntegrationParameters::default()
    };
    let mut broad_phase = BroadPhase::new();
    let mut narrow_phase = NarrowPhase::new();
    let mut joints = JointSet::new();
    let mut ccd_solver = CCDSolver::new();

    // Unit cube with its +Y face at the origin
    colliders.insert(
        ColliderBuilder::new(SharedShape::new(Cuboid::new(na::Vector3::new(
            0.5, 0.5, 0.5,
        ))))
        .position(na::Isometry3::translation(0.0, -0.5, 0.0))
        .build(),
    );

    let body = bodies.insert(
        RigidBodyBuilder::new(RigidBodyType::Dynamic)
            .ccd_enabled(true)
            .position(
                na::UnitQuaternion::from_axis_angle(&na::Vector3::x_axis(), 1e-2)
                    * na::Isometry3::translation(0.0, 5.0, 0.0),
            )
            .build(),
    );
    colliders.insert_with_parent(
        ColliderBuilder::new(SharedShape::new(Cylinder::new(1.0, 0.1))).build(),
        body,
        &mut bodies,
    );

    let mut prev_pos = *bodies.get(body).unwrap().position();
    for i in 0..25 {
        pipeline.step(
            &na::Vector3::new(0.0, -9.81, 0.0),
            &integration_parameters,
            &mut islands,
            &mut broad_phase,
            &mut narrow_phase,
            &mut bodies,
            &mut colliders,
            &mut joints,
            &mut ccd_solver,
            &(),
            &(),
        );
        let body = bodies.get(body).unwrap();
        let angle_change = prev_pos.rotation.angle_to(&body.position().rotation);
        let pos_change = body.position().translation.vector - prev_pos.translation.vector;
        println!(
            "{} - {:.4} {:.4?} at {:.4?}",
            i,
            angle_change,
            pos_change.norm(),
            (body.position() * na::Point3::origin()).coords.data,
        );
        prev_pos = *body.position();
    }
}