rapier2d/dynamics/ccd/
toi_entry.rs

1use crate::dynamics::{RigidBody, RigidBodyHandle};
2use crate::geometry::{Collider, ColliderHandle};
3use crate::math::Real;
4use parry::query::{NonlinearRigidMotion, QueryDispatcher, ShapeCastOptions};
5
6#[derive(Copy, Clone, Debug)]
7pub struct TOIEntry {
8    pub toi: Real,
9    pub c1: ColliderHandle,
10    pub b1: Option<RigidBodyHandle>,
11    pub c2: ColliderHandle,
12    pub b2: Option<RigidBodyHandle>,
13    // We call this "pseudo" intersection because this also
14    // includes colliders pairs with mismatching solver_groups.
15    pub is_pseudo_intersection_test: bool,
16}
17
18impl TOIEntry {
19    fn new(
20        toi: Real,
21        c1: ColliderHandle,
22        b1: Option<RigidBodyHandle>,
23        c2: ColliderHandle,
24        b2: Option<RigidBodyHandle>,
25        is_pseudo_intersection_test: bool,
26    ) -> Self {
27        Self {
28            toi,
29            c1,
30            b1,
31            c2,
32            b2,
33            is_pseudo_intersection_test,
34        }
35    }
36
37    #[profiling::function]
38    pub fn try_from_colliders<QD: ?Sized + QueryDispatcher>(
39        query_dispatcher: &QD,
40        ch1: ColliderHandle,
41        ch2: ColliderHandle,
42        co1: &Collider,
43        co2: &Collider,
44        rb1: Option<&RigidBody>,
45        rb2: Option<&RigidBody>,
46        frozen1: Option<Real>,
47        frozen2: Option<Real>,
48        start_time: Real,
49        end_time: Real,
50        smallest_contact_dist: Real,
51    ) -> Option<Self> {
52        assert!(start_time <= end_time);
53        if rb1.is_none() && rb2.is_none() {
54            return None;
55        }
56
57        let linvel1 = frozen1.is_none() as u32 as Real
58            * rb1.map(|b| b.integrated_vels.linvel).unwrap_or(na::zero());
59        let linvel2 = frozen2.is_none() as u32 as Real
60            * rb2.map(|b| b.integrated_vels.linvel).unwrap_or(na::zero());
61        let angvel1 = frozen1.is_none() as u32 as Real
62            * rb1.map(|b| b.integrated_vels.angvel).unwrap_or(na::zero());
63        let angvel2 = frozen2.is_none() as u32 as Real
64            * rb2.map(|b| b.integrated_vels.angvel).unwrap_or(na::zero());
65
66        #[cfg(feature = "dim2")]
67        let vel12 = (linvel2 - linvel1).norm()
68            + angvel1.abs() * rb1.map(|b| b.ccd.ccd_max_dist).unwrap_or(0.0)
69            + angvel2.abs() * rb2.map(|b| b.ccd.ccd_max_dist).unwrap_or(0.0);
70        #[cfg(feature = "dim3")]
71        let vel12 = (linvel2 - linvel1).norm()
72            + angvel1.norm() * rb1.map(|b| b.ccd.ccd_max_dist).unwrap_or(0.0)
73            + angvel2.norm() * rb2.map(|b| b.ccd.ccd_max_dist).unwrap_or(0.0);
74
75        // We may be slightly over-conservative by taking the `max(0.0)` here.
76        // But removing the `max` doesn't really affect performances so let's
77        // keep it since more conservatism is good at this stage.
78        let thickness = (co1.shape.0.ccd_thickness() + co2.shape.0.ccd_thickness())
79            + smallest_contact_dist.max(0.0);
80        let is_pseudo_intersection_test = co1.is_sensor()
81            || co2.is_sensor()
82            || !co1.flags.solver_groups.test(co2.flags.solver_groups);
83
84        if (end_time - start_time) * vel12 < thickness {
85            return None;
86        }
87
88        // Compute the TOI.
89        let identity = NonlinearRigidMotion::identity();
90        let mut motion1 = rb1.map(Self::body_motion).unwrap_or(identity);
91        let mut motion2 = rb2.map(Self::body_motion).unwrap_or(identity);
92
93        if let Some(t) = frozen1 {
94            motion1.freeze(t);
95        }
96
97        if let Some(t) = frozen2 {
98            motion2.freeze(t);
99        }
100
101        let motion_c1 = motion1.prepend(co1.parent.map(|p| p.pos_wrt_parent).unwrap_or(co1.pos.0));
102        let motion_c2 = motion2.prepend(co2.parent.map(|p| p.pos_wrt_parent).unwrap_or(co2.pos.0));
103
104        // println!("start_time: {}", start_time);
105
106        // If this is just an intersection test (i.e. with sensors)
107        // then we can stop the TOI search immediately if it starts with
108        // a penetration because we don't care about the whether the velocity
109        // at the impact is a separating velocity or not.
110        // If the TOI search involves two non-sensor colliders then
111        // we don't want to stop the TOI search at the first penetration
112        // because the colliders may be in a separating trajectory.
113        let stop_at_penetration = is_pseudo_intersection_test;
114
115        const USE_NONLINEAR_SHAPE_CAST: bool = true;
116
117        let toi = if USE_NONLINEAR_SHAPE_CAST {
118            query_dispatcher
119                .cast_shapes_nonlinear(
120                    &motion_c1,
121                    co1.shape.as_ref(),
122                    &motion_c2,
123                    co2.shape.as_ref(),
124                    start_time,
125                    end_time,
126                    stop_at_penetration,
127                )
128                .ok()??
129        } else {
130            let pos12 = motion_c1
131                .position_at_time(start_time)
132                .inv_mul(&motion_c2.position_at_time(start_time));
133            let vel12 = linvel2 - linvel1;
134            let options = ShapeCastOptions::with_max_time_of_impact(end_time - start_time);
135            let mut hit = query_dispatcher
136                .cast_shapes(
137                    &pos12,
138                    &vel12,
139                    co1.shape.as_ref(),
140                    co2.shape.as_ref(),
141                    options,
142                )
143                .ok()??;
144            hit.time_of_impact += start_time;
145            hit
146        };
147
148        Some(Self::new(
149            toi.time_of_impact,
150            ch1,
151            co1.parent.map(|p| p.handle),
152            ch2,
153            co2.parent.map(|p| p.handle),
154            is_pseudo_intersection_test,
155        ))
156    }
157
158    fn body_motion(rb: &RigidBody) -> NonlinearRigidMotion {
159        if rb.ccd.ccd_active {
160            NonlinearRigidMotion::new(
161                rb.pos.position,
162                rb.mprops.local_mprops.local_com,
163                rb.integrated_vels.linvel,
164                rb.integrated_vels.angvel,
165            )
166        } else {
167            NonlinearRigidMotion::constant_position(rb.pos.next_position)
168        }
169    }
170}
171
172impl PartialOrd for TOIEntry {
173    fn partial_cmp(&self, other: &Self) -> Option<std::cmp::Ordering> {
174        Some(self.cmp(other))
175    }
176}
177
178impl Ord for TOIEntry {
179    fn cmp(&self, other: &Self) -> std::cmp::Ordering {
180        (-self.toi)
181            .partial_cmp(&(-other.toi))
182            .unwrap_or(std::cmp::Ordering::Equal)
183    }
184}
185
186impl PartialEq for TOIEntry {
187    fn eq(&self, other: &Self) -> bool {
188        self.toi == other.toi
189    }
190}
191
192impl Eq for TOIEntry {}
rapier2d/dynamics/ccd/toi_entry.rs

rapier2d/dynamics/ccd/
toi_entry.rs
