rapier2d 0.5.0

use super::TOIEntry;
use crate::counters::Counters;
use crate::data::Coarena;
use crate::dynamics::ccd::CCDData;
use crate::dynamics::{IntegrationParameters, RigidBodyHandle, RigidBodySet};
use crate::geometry::{ColliderHandle, ColliderSet, NarrowPhase};
use crate::math::Real;
use crate::parry::utils::SortedPair;
use parry::query::QueryDispatcher;
use std::collections::{BinaryHeap, HashMap, HashSet};

pub enum PredictedImpacts {
    Impacts(HashMap<RigidBodyHandle, Real>),
    ImpactsAfterEndTime(Real),
    NoImpacts,
}

// Outputs a sorted list of TOI event (in ascending order) for the given time interval,
// assuming body motions clamped at their first TOI.
pub fn predict_next_impacts(
    params: &IntegrationParameters,
    bodies: &RigidBodySet,
    colliders: &ColliderSet,
    narrow_phase: &NarrowPhase,
    body_params: &Coarena<CCDData>,
    end_time: Real,
) -> PredictedImpacts {
    dbg!("Solving ccd");
    let mut frozen = HashMap::<_, Real>::new();
    let mut all_toi = BinaryHeap::new();
    let mut pairs_seen = HashSet::new();
    let mut min_overstep = params.dt();

    /*
     *
     * First, collect all TOIs.
     *
     */
    // TODO: don't iterate through all the colliders.
    for (coll_handle, coll) in colliders.iter() {
        if bodies[coll.parent()].is_ccd_enabled(params.dt()) {
            // FIXME: what about triggers?
            let contacts = narrow_phase.contacts_with(coll_handle);
            if contacts.is_none() {
                continue;
            }

            for (ch1, ch2, inter) in contacts.unwrap() {
                if pairs_seen.insert(SortedPair::new(
                    ch1.into_raw_parts().0,
                    ch2.into_raw_parts().0,
                )) {
                    let c1 = colliders.get(ch1).unwrap();
                    let c2 = colliders.get(ch2).unwrap();
                    let bh1 = c1.parent();
                    let bh2 = c2.parent();

                    let b1 = bodies.get(bh1).unwrap();
                    let b2 = bodies.get(bh2).unwrap();

                    if let Some(toi) = TOIEntry::try_from_colliders(
                        params,
                        narrow_phase.query_dispatcher(),
                        ch1,
                        ch2,
                        c1,
                        c2,
                        b1,
                        b2,
                        false, // FIXME: what about triggers?
                        None,
                        None,
                        // NOTE: we use end_time here only once we know that
                        // there is at least one TOI before end_time.
                        min_overstep,
                        &body_params,
                    ) {
                        if toi.toi > end_time {
                            min_overstep = min_overstep.min(toi.toi);
                        } else {
                            min_overstep = end_time;
                            all_toi.push(toi);
                        }
                    }
                }
            }
        }
    }

    /*
     *
     * If the smallest TOI is outside of the time interval, return.
     *
     */
    if min_overstep == params.dt() && all_toi.is_empty() {
        return PredictedImpacts::NoImpacts;
    } else if min_overstep > end_time {
        return PredictedImpacts::ImpactsAfterEndTime(min_overstep);
    }

    // FIXME: make this resweep not mendatory.
    // NOTE: all static bodies should be considered as "frozen", this
    // may avoid some resweeps.
    while let Some(toi) = all_toi.pop() {
        assert!(toi.toi <= end_time);

        if toi.is_intersection {
            // This is only an intersection so we don't have to freeze and there is no need to resweep.
            continue;
        }

        let body1 = bodies.get(toi.b1).unwrap();
        let body2 = bodies.get(toi.b2).unwrap();

        let mut colliders_to_check = Vec::new();
        if body1.is_ccd_enabled(params.dt()) && !frozen.contains_key(&toi.b1) {
            let _ = frozen.insert(toi.b1, toi.toi);
            colliders_to_check.extend_from_slice(&body1.colliders);
        }

        if body2.is_ccd_enabled(params.dt()) && !frozen.contains_key(&toi.b2) {
            let _ = frozen.insert(toi.b2, toi.toi);
            colliders_to_check.extend_from_slice(&body2.colliders);
        }

        for coll_handle in &colliders_to_check {
            let contacts = narrow_phase.contacts_with(*coll_handle);
            if contacts.is_none() {
                continue;
            }

            for (ch1, ch2, inter) in contacts.unwrap() {
                let c1 = colliders.get(ch1).unwrap();
                let c2 = colliders.get(ch2).unwrap();
                let bh1 = c1.parent();
                let bh2 = c2.parent();

                let frozen1 = frozen.get(&bh1);
                let frozen2 = frozen.get(&bh2);

                let b1 = bodies.get(bh1).unwrap();
                let b2 = bodies.get(bh2).unwrap();

                if (frozen1.is_some() || !b1.is_ccd_enabled(params.dt()))
                    && (frozen2.is_some() || !b2.is_ccd_enabled(params.dt()))
                {
                    // We already did a resweep.
                    continue;
                }

                if let Some(toi) = TOIEntry::try_from_colliders(
                    params,
                    narrow_phase.query_dispatcher(),
                    ch1,
                    ch2,
                    c1,
                    c2,
                    b1,
                    b2,
                    false, // FIXME: what about triggers?
                    frozen1.copied(),
                    frozen2.copied(),
                    end_time,
                    &body_params,
                ) {
                    all_toi.push(toi);
                }
            }
        }
    }

    PredictedImpacts::Impacts(frozen)
}

/*
fn solve_ccd(
    params: &IntegrationParameters,
    bodies: &mut Bodies,
    colliders: &mut Colliders,
    constraints: &mut Constraints,
    counters: &mut Counters,
) {
    let dt0 = params.dt();

    if dt0 == 0.0 || params.max_ccd_substeps == 0 {
        return;
    }

    let substep_length = params.dt() / (params.max_ccd_substeps as Real);

    let mut parameters = params.clone();
    // parameters.max_position_iterations = parameters.max_ccd_position_iterations;
    // parameters.warmstart_coeff = 0.0;

    counters.ccd.reset();

    let mut last_iter = false;
    let mut substep_end_time = 0.0;

    loop {
        if substep_end_time < dt0 {
            substep_end_time = (substep_end_time + substep_length).min(dt0);
        } else {
            last_iter = true;
        }

        /*
         *
         * Update the broad phase.
         *
         */
        counters.ccd.broad_phase_time.resume();
        gworld.sync_colliders(bodies, colliders);
        gworld.perform_broad_phase(bodies, colliders, filter);
        counters.ccd.broad_phase_time.pause();

        /*
         *
         * Compute the TOI events to solve.
         *
         */
        counters.ccd.toi_computation_time.resume();

        let (toi_entries, frozen) = match predict_next_impacts(
            params,
            query_dispatcher,
            bodies,
            colliders,
            narrow_phase,
            substep_end_time,
        ) {
            PredictedImpacts::Impacts(toi_entries, frozen) => (toi_entries, frozen),
            PredictedImpacts::ImpactsAfterEndTime(min_toi) => {
                substep_end_time = (min_toi / substep_length).floor() * substep_length;
                substep_end_time = (substep_end_time + substep_length).min(dt0);

                predict_next_impacts(gworld, bodies, colliders, substep_end_time).unwrap_impacts()
            }
            PredictedImpacts::NoImpacts => (Vec::new(), HashMap::new()),
        };

        counters.ccd.toi_computation_time.pause();

        /*
         *
         * Resolve the TOI events.
         *
         */
        if !last_iter && !toi_entries.is_empty() {
            counters.ccd.num_substeps += 1;

            let mut island = Vec::new();
            let mut contact_manifolds = Vec::new();

            // We will start the integration at the date of the latest TOI before the end_time.
            // (We don't start at end_time directly to avoid clamping some motion needlessly).
            let last_toi = toi_entries.last().unwrap().toi;
            parameters.set_dt(dt0 - last_toi);

            // We will use the companion ID to know which body is already on the island.
            bodies.foreach_mut(&mut |_h, b: &mut dyn Body<N>| {
                b.set_companion_id(0);
            });

            let mut colliders_to_traverse = Vec::new();
            let mut ccd_bodies = Vec::new();

            for entry in &toi_entries {
                if !entry.is_intersection {
                    let all_colliders1 = gworld.body_colliders(entry.b1).unwrap();
                    let all_colliders2 = gworld.body_colliders(entry.b2).unwrap();
                    colliders_to_traverse.extend_from_slice(all_colliders1);
                    colliders_to_traverse.extend_from_slice(all_colliders2);

                    ccd_bodies.push(entry.b1);
                    ccd_bodies.push(entry.b2);
                }
            }

            let mut interaction_ids = Vec::new();
            let mut visited = HashSet::new();

            counters.ccd.narrow_phase_time.resume();

            // Advance colliders and update contact manifolds.
            while let Some(c) = colliders_to_traverse.pop() {
                if visited.contains(&c) {
                    // It is possible that we pop several time the same collider handle.
                    // For example this happens if the collider was involved in a TOI event
                    // and has be pushed again after the narrow-phase contact update bellow.
                    continue;
                }

                let graph_id = colliders.get(c).unwrap().graph_index().unwrap();

                for (ch1, ch2, eid, inter) in gworld.interactions.interactions_with_mut(graph_id) {
                    if (ch1 == c && visited.contains(&ch2)) || (ch2 == c && visited.contains(&ch1))
                    {
                        continue;
                    }

                    match inter {
                        Interaction::Contact(alg, manifold) => {
                            let handle1 = colliders.get(ch1).unwrap().body();
                            let handle2 = colliders.get(ch2).unwrap().body();
                            if bodies.contains(handle1) && bodies.contains(handle2) {
                                let mut prepare_body =
                                    |h, b: &mut dyn Body<N>, c: &mut Collider<N, Handle>| {
                                        b.clear_forces();
                                        b.update_kinematics();
                                        b.update_dynamics(parameters.dt());
                                        b.update_acceleration(&Vector::zeros(), &parameters);

                                        let curr_body_time = self
                                            .substep
                                            .body_times
                                            .entry(h)
                                            .or_insert_with(N::zero);

                                        let target_time =
                                            frozen.get(&h).cloned().unwrap_or(last_toi);
                                        b.advance(target_time - *curr_body_time);
                                        b.clamp_advancement();

                                        // This body will have its motion clamped between the times target_time and last_time.
                                        *curr_body_time = last_toi;

                                        b.set_companion_id(1);
                                        island.push(h);
                                        c.set_position(
                                            b.part(0).unwrap().position() * c.position_wrt_body(),
                                        );
                                    };

                                let b1_needs_preparation = {
                                    let b1 = bodies.get_mut(handle1).unwrap();
                                    let needs_prep = b1.companion_id() == 0 && b1.is_dynamic();

                                    if needs_prep {
                                        prepare_body(handle1, b1, colliders.get_mut(ch1).unwrap())
                                    }
                                    needs_prep
                                };

                                let b2_needs_preparation = {
                                    let b2 = bodies.get_mut(handle2).unwrap();
                                    let needs_prep = b2.companion_id() == 0 && b2.is_dynamic();

                                    if needs_prep {
                                        prepare_body(handle2, b2, colliders.get_mut(ch2).unwrap())
                                    }
                                    needs_prep
                                };

                                let (c1, c2) = colliders.get_pair(ch1, ch2);
                                let (c1, c2) = (c1.unwrap(), c2.unwrap());

                                gworld
                                    .narrow_phase
                                    .update_contact(c1, c2, ch1, ch2, &mut **alg, manifold);

                                if manifold.len() > 0 {
                                    interaction_ids.push(eid);

                                    if ch1 != c && b1_needs_preparation {
                                        let all_colliders1 =
                                            gworld.body_colliders.get(&handle1).unwrap();
                                        colliders_to_traverse
                                            .extend_from_slice(&all_colliders1[..]);
                                    }

                                    if ch2 != c && b2_needs_preparation {
                                        let all_colliders2 =
                                            gworld.body_colliders.get(&handle2).unwrap();
                                        colliders_to_traverse
                                            .extend_from_slice(&all_colliders2[..]);
                                    }
                                }
                            }
                        }
                        Interaction::Intersection(..) => {
                            // Proximities are handled in another loop,
                            // after all the bodies have been advanced.
                        }
                    }
                }

                let _ = visited.insert(c);
            }

            // Handle proximities.
            for toi in &toi_entries {
                if toi.is_intersection {
                    let mut c1 = colliders.get(toi.c1).unwrap();
                    let mut c2 = colliders.get(toi.c2).unwrap();
                    let (ch1, ch2, detector, prox) = gworld
                        .interactions
                        .intersection_pair_mut(c1.graph_index().unwrap(), c2.graph_index().unwrap())
                        .unwrap();

                    if ch1 != toi.c1 {
                        // The order of the colliders may not be the same in the interaction.
                        std::mem::swap(&mut c1, &mut c2)
                    }

                    // Emit an event (the case where we already have *prox == Intersecting will be filtered out automatically).
                    gworld.narrow_phase.emit_intersection_event(
                        ch1,
                        ch2,
                        *prox,
                        Intersection::Intersecting,
                    );

                    // Set the intersection as intersecting.
                    *prox = Intersection::Intersecting;

                    // If we mulitple events is disabled, there is nothing more to do as the
                    // final intersection event will only depend on the final position of the
                    // colliders (and thus will be handled by the final narrow-phase update of
                    // the non-ccd step).
                    if self.params.multiple_ccd_substep_sensor_events_enabled {
                        // If the bodies of the colliders have been
                        // teleported at a time of impact, then we have to update the intersection now
                        // to account for multiple on/off intersection events during consecutive substeps.
                        //
                        // If the bodies have not been teleported, they will be handled later during the
                        // final narrow-phase update of the non-ccd step.
                        //
                        // FIXME: In the case where this is the last substep to
                        // be performed, and the related bodies remain frozen at the end of CCD
                        // handling, then this intersection update will happen twice. The results will
                        // still be correct, but this will unfortunately cost two intersection updates
                        // instead of just one.
                        let b1 = bodies.get(c1.body()).unwrap();
                        let b2 = bodies.get(c2.body()).unwrap();

                        if b1.companion_id() == 1 || b2.companion_id() == 1 {
                            gworld
                                .narrow_phase
                                .update_intersection(c1, c2, ch1, ch2, detector, prox)
                        }
                    }
                }
            }

            // Collect contact manifolds.
            for eid in interaction_ids {
                let (ch1, ch2, inter) = gworld.interactions.index_interaction(eid).unwrap();
                match inter {
                    Interaction::Contact(_, manifold) => {
                        let c1 = colliders.get(ch1).unwrap();
                        let c2 = colliders.get(ch2).unwrap();
                        contact_manifolds
                            .push(ColliderContactManifold::new(ch1, c1, ch2, c2, manifold));
                    }
                    Interaction::Intersection(..) => {}
                }
            }

            counters.ccd.narrow_phase_time.pause();

            // Solve the system and integrate.
            bodies.foreach_mut(&mut |_, b: &mut dyn Body<N>| {
                b.set_companion_id(0);
            });

            counters.ccd.solver_time.resume();
            self.solver.step_ccd(
                &mut counters,
                bodies,
                colliders,
                constraints,
                &contact_manifolds[..],
                &ccd_bodies[..],
                &island[..],
                &[], // FIXME: take constraints into account?
                &parameters,
                &self.material_coefficients,
            );
            counters.ccd.solver_time.pause();

            // Update body kinematics and dynamics
            // after the contact resolution step.
            bodies.foreach_mut(&mut |_, b: &mut dyn Body<N>| {
                b.clear_forces();
                b.update_kinematics();
                b.update_dynamics(parameters.dt());
            });

            // if params.return_after_ccd_substep {
            //     self.substep.active = true;
            //     self.substep.dt = parameters.dt();
            //     break;
            // }
        } else {
            bodies.foreach_mut(&mut |handle, body: &mut dyn Body<N>| {
                if !body.is_static() && frozen.contains_key(&handle) {
                    body.clamp_advancement();
                }
            });

            // self.substep.active = false;
            // self.substep.next_substep = 0;
            substep_end_time = 0.0;
            break;
        }
    }
}
*/