parry3d/query/shape_cast/
shape_cast_voxels_shape.rs

1use crate::math::{Isometry, Point, Real, Translation, Vector};
2use crate::query::{QueryDispatcher, ShapeCastHit, ShapeCastOptions};
3use crate::shape::{Cuboid, Shape, Voxels};
4
5/// Time Of Impact of a voxels shape with any other shape, under a translational movement.
6pub fn cast_shapes_voxels_shape<D>(
7    dispatcher: &D,
8    pos12: &Isometry<Real>,
9    vel12: &Vector<Real>,
10    g1: &Voxels,
11    g2: &dyn Shape,
12    options: ShapeCastOptions,
13) -> Option<ShapeCastHit>
14where
15    D: ?Sized + QueryDispatcher,
16{
17    use num_traits::Bounded;
18
19    let mut hit = None;
20    let mut smallest_t = options.max_time_of_impact;
21    let start_aabb2_1 = g2.compute_aabb(pos12);
22
23    let mut check_voxels_in_range = |search_domain: [Point<i32>; 2]| {
24        for vox in g1.voxels_in_range(search_domain[0], search_domain[1]) {
25            if !vox.state.is_empty() {
26                // PERF: could we check the canonical shape instead, and deduplicate accordingly?
27                let center = g1.voxel_center(vox.grid_coords);
28                let cuboid = Cuboid::new(g1.voxel_size() / 2.0);
29                let vox_pos12 = Translation::from(center).inverse() * pos12;
30                if let Some(new_hit) = dispatcher
31                    .cast_shapes(&vox_pos12, vel12, &cuboid, g2, options)
32                    .ok()
33                    .flatten()
34                {
35                    if new_hit.time_of_impact < smallest_t {
36                        smallest_t = new_hit.time_of_impact;
37                        hit = Some(new_hit);
38                    }
39                }
40            }
41        }
42    };
43
44    let mut search_domain = g1.voxel_range_intersecting_local_aabb(&start_aabb2_1);
45    check_voxels_in_range(search_domain);
46
47    // Run the propagation.
48    let [domain_mins, domain_maxs] = g1.domain();
49
50    loop {
51        let search_domain_aabb = g1.voxel_range_aabb(search_domain[0], search_domain[1]);
52
53        // Figure out if we should move the aabb up/down/right/left.
54        #[cfg(feature = "dim2")]
55        let ii = [0, 1];
56        #[cfg(feature = "dim3")]
57        let ii = [0, 1, 2];
58
59        let toi = ii.map(|i| {
60            if vel12[i] > 0.0 {
61                let t = (search_domain_aabb.maxs[i] - start_aabb2_1.maxs[i]) / vel12[i];
62                if t < 0.0 {
63                    (Real::max_value(), true)
64                } else {
65                    (t, true)
66                }
67            } else if vel12[i] < 0.0 {
68                let t = (search_domain_aabb.mins[i] - start_aabb2_1.mins[i]) / vel12[i];
69                if t < 0.0 {
70                    (Real::max_value(), false)
71                } else {
72                    (t, false)
73                }
74            } else {
75                (Real::max_value(), false)
76            }
77        });
78
79        #[cfg(feature = "dim2")]
80        if toi[0].0 > options.max_time_of_impact && toi[1].0 > options.max_time_of_impact {
81            break;
82        }
83
84        #[cfg(feature = "dim3")]
85        if toi[0].0 > options.max_time_of_impact
86            && toi[1].0 > options.max_time_of_impact
87            && toi[2].0 > options.max_time_of_impact
88        {
89            break;
90        }
91
92        let imin = Vector::from(toi.map(|t| t.0)).imin();
93
94        if toi[imin].1 {
95            search_domain[0][imin] += 1;
96            search_domain[1][imin] += 1;
97
98            if search_domain[1][imin] <= domain_maxs[imin] {
99                // Check the voxels on the added row.
100                let mut prev_row = search_domain[0];
101                prev_row[imin] = search_domain[1][imin] - 1;
102
103                let range_to_check = [prev_row, search_domain[1]];
104                check_voxels_in_range(range_to_check);
105            } else if search_domain[0][imin] >= domain_maxs[imin] {
106                // Leaving the shape’s bounds.
107                break;
108            }
109        } else {
110            search_domain[0][imin] -= 1;
111            search_domain[1][imin] -= 1;
112
113            if search_domain[0][imin] >= domain_mins[imin] {
114                // Check the voxels on the added row.
115                let mut next_row = search_domain[1];
116                next_row[imin] = search_domain[0][imin] + 1;
117
118                let range_to_check = [search_domain[0], next_row];
119                check_voxels_in_range(range_to_check);
120            } else if search_domain[1][imin] <= domain_mins[imin] {
121                // Leaving the shape’s bounds.
122                break;
123            }
124        }
125    }
126
127    hit
128}
129
130/// Time Of Impact of any shape with a composite shape, under a rigid motion (translation + rotation).
131pub fn cast_shapes_shape_voxels<D>(
132    dispatcher: &D,
133    pos12: &Isometry<Real>,
134    vel12: &Vector<Real>,
135    g1: &dyn Shape,
136    g2: &Voxels,
137    options: ShapeCastOptions,
138) -> Option<ShapeCastHit>
139where
140    D: ?Sized + QueryDispatcher,
141{
142    cast_shapes_voxels_shape(
143        dispatcher,
144        &pos12.inverse(),
145        &-pos12.inverse_transform_vector(vel12),
146        g2,
147        g1,
148        options,
149    )
150    .map(|time_of_impact| time_of_impact.swapped())
151}
parry3d/query/shape_cast/shape_cast_voxels_shape.rs

parry3d/query/shape_cast/
shape_cast_voxels_shape.rs
