parry3d/query/shape_cast/
shape_cast_voxels_shape.rs

1use crate::math::{IVector, IVectorExt, Pose, Real, Vector, VectorExt};
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: &Pose,
9    vel12: Vector,
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: [IVector; 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 = Pose::from_translation(center).inverse() * pos12;
30                if let Some(mut 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                        // The hit is expressed in the voxel cuboid's frame, i.e. the shape's
37                        // frame translated by `center`. Shift `witness1` back; the normals
38                        // and `witness2` are unaffected by a translation.
39                        new_hit.witness1 += center;
40                        smallest_t = new_hit.time_of_impact;
41                        hit = Some(new_hit);
42                    }
43                }
44            }
45        }
46    };
47
48    let mut search_domain = g1.voxel_range_intersecting_local_aabb(&start_aabb2_1);
49    check_voxels_in_range(search_domain);
50
51    // Run the propagation.
52    let [domain_mins, domain_maxs] = g1.domain();
53
54    loop {
55        let search_domain_aabb = g1.voxel_range_aabb(search_domain[0], search_domain[1]);
56
57        // Figure out if we should move the aabb up/down/right/left.
58        #[cfg(feature = "dim2")]
59        let ii = [0, 1];
60        #[cfg(feature = "dim3")]
61        let ii = [0, 1, 2];
62
63        let toi = ii.map(|i| {
64            if vel12.vget(i) > 0.0 {
65                let t =
66                    (search_domain_aabb.maxs.vget(i) - start_aabb2_1.maxs.vget(i)) / vel12.vget(i);
67                if t < 0.0 {
68                    (Real::max_value(), true)
69                } else {
70                    (t, true)
71                }
72            } else if vel12.vget(i) < 0.0 {
73                let t =
74                    (search_domain_aabb.mins.vget(i) - start_aabb2_1.mins.vget(i)) / vel12.vget(i);
75                if t < 0.0 {
76                    (Real::max_value(), false)
77                } else {
78                    (t, false)
79                }
80            } else {
81                (Real::max_value(), false)
82            }
83        });
84
85        #[cfg(feature = "dim2")]
86        if toi[0].0 > options.max_time_of_impact && toi[1].0 > options.max_time_of_impact {
87            break;
88        }
89
90        #[cfg(feature = "dim3")]
91        if toi[0].0 > options.max_time_of_impact
92            && toi[1].0 > options.max_time_of_impact
93            && toi[2].0 > options.max_time_of_impact
94        {
95            break;
96        }
97
98        let imin = Vector::from(toi.map(|t| t.0)).min_position();
99
100        if toi[imin].1 {
101            search_domain[0].ivset(imin, search_domain[0].ivget(imin) + 1);
102            search_domain[1].ivset(imin, search_domain[1].ivget(imin) + 1);
103
104            if search_domain[1].ivget(imin) <= domain_maxs.ivget(imin) {
105                // Check the voxels on the added row.
106                let mut prev_row = search_domain[0];
107                prev_row.ivset(imin, search_domain[1].ivget(imin) - 1);
108
109                let range_to_check = [prev_row, search_domain[1]];
110                check_voxels_in_range(range_to_check);
111            } else if search_domain[0].ivget(imin) >= domain_maxs.ivget(imin) {
112                // Leaving the shape's bounds.
113                break;
114            }
115        } else {
116            search_domain[0].ivset(imin, search_domain[0].ivget(imin) - 1);
117            search_domain[1].ivset(imin, search_domain[1].ivget(imin) - 1);
118
119            if search_domain[0].ivget(imin) >= domain_mins.ivget(imin) {
120                // Check the voxels on the added row.
121                let mut next_row = search_domain[1];
122                next_row.ivset(imin, search_domain[0].ivget(imin) + 1);
123
124                let range_to_check = [search_domain[0], next_row];
125                check_voxels_in_range(range_to_check);
126            } else if search_domain[1].ivget(imin) <= domain_mins.ivget(imin) {
127                // Leaving the shape's bounds.
128                break;
129            }
130        }
131    }
132
133    hit
134}
135
136/// Time Of Impact of any shape with a composite shape, under a rigid motion (translation + rotation).
137pub fn cast_shapes_shape_voxels<D>(
138    dispatcher: &D,
139    pos12: &Pose,
140    vel12: Vector,
141    g1: &dyn Shape,
142    g2: &Voxels,
143    options: ShapeCastOptions,
144) -> Option<ShapeCastHit>
145where
146    D: ?Sized + QueryDispatcher,
147{
148    cast_shapes_voxels_shape(
149        dispatcher,
150        &pos12.inverse(),
151        -(pos12.rotation.inverse() * vel12),
152        g2,
153        g1,
154        options,
155    )
156    .map(|time_of_impact| time_of_impact.swapped())
157}
parry3d/query/shape_cast/shape_cast_voxels_shape.rs

parry3d/query/shape_cast/
shape_cast_voxels_shape.rs
