parry3d/query/shape_cast/
shape_cast_heightfield_shape.rs

1use crate::bounding_volume::BoundingVolume;
2use crate::math::{Isometry, Real, Vector};
3use crate::query::details::ShapeCastOptions;
4use crate::query::{QueryDispatcher, Ray, ShapeCastHit, Unsupported};
5use crate::shape::{HeightField, Shape};
6#[cfg(feature = "dim3")]
7use crate::{bounding_volume::Aabb, query::RayCast};
8
9/// Time Of Impact between a moving shape and a heightfield.
10#[cfg(feature = "dim2")]
11pub fn cast_shapes_heightfield_shape<D: ?Sized + QueryDispatcher>(
12    dispatcher: &D,
13    pos12: &Isometry<Real>,
14    vel12: &Vector<Real>,
15    heightfield1: &HeightField,
16    g2: &dyn Shape,
17    options: ShapeCastOptions,
18) -> Result<Option<ShapeCastHit>, Unsupported> {
19    let aabb2_1 = g2.compute_aabb(pos12).loosened(options.target_distance);
20    let ray = Ray::new(aabb2_1.center(), *vel12);
21
22    let mut curr_range = heightfield1.unclamped_elements_range_in_local_aabb(&aabb2_1);
23    // Enlarge the range by 1 to account for movement within a cell.
24    let right = ray.dir.x > 0.0;
25
26    if right {
27        curr_range.end += 1;
28    } else {
29        curr_range.start -= 1;
30    }
31
32    let mut best_hit = None::<ShapeCastHit>;
33
34    /*
35     * Test the segment under the ray.
36     */
37    let clamped_curr_range = curr_range.start.clamp(0, heightfield1.num_cells() as isize) as usize
38        ..curr_range.end.clamp(0, heightfield1.num_cells() as isize) as usize;
39    for curr in clamped_curr_range {
40        if let Some(seg) = heightfield1.segment_at(curr) {
41            // TODO: pre-check using a ray-cast on the Aabbs first?
42            if let Some(hit) = dispatcher.cast_shapes(pos12, vel12, &seg, g2, options)? {
43                if hit.time_of_impact < best_hit.map(|h| h.time_of_impact).unwrap_or(Real::MAX) {
44                    best_hit = Some(hit);
45                }
46            }
47        }
48    }
49
50    /*
51     * Test other segments in the path of the ray.
52     */
53    if ray.dir.x == 0.0 {
54        return Ok(best_hit);
55    }
56
57    let cell_width = heightfield1.cell_width();
58    let start_x = heightfield1.start_x();
59
60    let mut curr_elt = if right {
61        (curr_range.end - 1).max(0)
62    } else {
63        curr_range.start.min(heightfield1.num_cells() as isize - 1)
64    };
65
66    while (right && curr_elt < heightfield1.num_cells() as isize - 1) || (!right && curr_elt > 0) {
67        let curr_param;
68
69        if right {
70            curr_elt += 1;
71            curr_param = (cell_width * na::convert::<f64, Real>(curr_elt as f64) + start_x
72                - ray.origin.x)
73                / ray.dir.x;
74        } else {
75            curr_param =
76                (ray.origin.x - cell_width * na::convert::<f64, Real>(curr_elt as f64) - start_x)
77                    / ray.dir.x;
78            curr_elt -= 1;
79        }
80
81        if curr_param >= options.max_time_of_impact {
82            break;
83        }
84
85        if let Some(seg) = heightfield1.segment_at(curr_elt as usize) {
86            // TODO: pre-check using a ray-cast on the Aabbs first?
87            if let Some(hit) = dispatcher.cast_shapes(pos12, vel12, &seg, g2, options)? {
88                if hit.time_of_impact < best_hit.map(|h| h.time_of_impact).unwrap_or(Real::MAX) {
89                    best_hit = Some(hit);
90                }
91            }
92        }
93    }
94
95    Ok(best_hit)
96}
97
98/// Time Of Impact between a moving shape and a heightfield.
99#[cfg(feature = "dim3")]
100pub fn cast_shapes_heightfield_shape<D: ?Sized + QueryDispatcher>(
101    dispatcher: &D,
102    pos12: &Isometry<Real>,
103    vel12: &Vector<Real>,
104    heightfield1: &HeightField,
105    g2: &dyn Shape,
106    options: ShapeCastOptions,
107) -> Result<Option<ShapeCastHit>, Unsupported> {
108    let aabb1 = heightfield1.local_aabb();
109    let mut aabb2_1 = g2.compute_aabb(pos12).loosened(options.target_distance);
110    let ray = Ray::new(aabb2_1.center(), *vel12);
111
112    // Find the first hit between the aabbs.
113    let hext2_1 = aabb2_1.half_extents();
114    let msum = Aabb::new(aabb1.mins - hext2_1, aabb1.maxs + hext2_1);
115    if let Some(time_of_impact) = msum.cast_local_ray(&ray, options.max_time_of_impact, true) {
116        // Advance the aabb2 to the hit point.
117        aabb2_1.mins += ray.dir * time_of_impact;
118        aabb2_1.maxs += ray.dir * time_of_impact;
119    } else {
120        return Ok(None);
121    }
122
123    let (mut curr_range_i, mut curr_range_j) =
124        heightfield1.unclamped_elements_range_in_local_aabb(&aabb2_1);
125    let (ncells_i, ncells_j) = heightfield1.num_cells_ij();
126    let mut best_hit = None::<ShapeCastHit>;
127
128    /*
129     * Enlarge the ranges by 1 to account for any movement within one cell.
130     */
131    if ray.dir.z > 0.0 {
132        curr_range_i.end += 1;
133    } else if ray.dir.z < 0.0 {
134        curr_range_i.start -= 1;
135    }
136
137    if ray.dir.x > 0.0 {
138        curr_range_j.end += 1;
139    } else if ray.dir.x < 0.0 {
140        curr_range_j.start -= 1;
141    }
142
143    /*
144     * Test the segment under the ray.
145     */
146    let clamped_curr_range_i = curr_range_i.start.clamp(0, ncells_i as isize)
147        ..curr_range_i.end.clamp(0, ncells_i as isize);
148    let clamped_curr_range_j = curr_range_j.start.clamp(0, ncells_j as isize)
149        ..curr_range_j.end.clamp(0, ncells_j as isize);
150
151    let mut hit_triangles = |i, j| {
152        if i >= 0 && j >= 0 {
153            let (tri_a, tri_b) = heightfield1.triangles_at(i as usize, j as usize);
154            for tri in [tri_a, tri_b].into_iter().flatten() {
155                // TODO: pre-check using a ray-cast on the Aabbs first?
156                if let Some(hit) = dispatcher.cast_shapes(pos12, vel12, &tri, g2, options)? {
157                    if hit.time_of_impact < best_hit.map(|h| h.time_of_impact).unwrap_or(Real::MAX)
158                    {
159                        best_hit = Some(hit);
160                    }
161                }
162            }
163        }
164
165        Ok(())
166    };
167
168    for i in clamped_curr_range_i {
169        for j in clamped_curr_range_j.clone() {
170            hit_triangles(i, j)?;
171        }
172    }
173
174    if ray.dir.y == 0.0 {
175        return Ok(best_hit);
176    }
177
178    let mut cell = heightfield1.unclamped_cell_at_point(&aabb2_1.center());
179
180    loop {
181        let prev_cell = cell;
182
183        /*
184         * Find the next cell to cast the ray on.
185         */
186        let toi_x = if ray.dir.x > 0.0 {
187            let x = heightfield1.signed_x_at(cell.1 + 1);
188            (x - ray.origin.x) / ray.dir.x
189        } else if ray.dir.x < 0.0 {
190            let x = heightfield1.signed_x_at(cell.1);
191            (x - ray.origin.x) / ray.dir.x
192        } else {
193            Real::MAX
194        };
195
196        let toi_z = if ray.dir.z > 0.0 {
197            let z = heightfield1.signed_z_at(cell.0 + 1);
198            (z - ray.origin.z) / ray.dir.z
199        } else if ray.dir.z < 0.0 {
200            let z = heightfield1.signed_z_at(cell.0);
201            (z - ray.origin.z) / ray.dir.z
202        } else {
203            Real::MAX
204        };
205
206        if toi_x > options.max_time_of_impact && toi_z > options.max_time_of_impact {
207            break;
208        }
209
210        if toi_x >= 0.0 && toi_x <= toi_z {
211            cell.1 += ray.dir.x.signum() as isize;
212        }
213
214        if toi_z >= 0.0 && toi_z <= toi_x {
215            cell.0 += ray.dir.z.signum() as isize;
216        }
217
218        if cell == prev_cell {
219            break;
220        }
221
222        let cell_diff = (cell.0 - prev_cell.0, cell.1 - prev_cell.1);
223        curr_range_i.start += cell_diff.0;
224        curr_range_i.end += cell_diff.0;
225        curr_range_j.start += cell_diff.1;
226        curr_range_j.end += cell_diff.1;
227
228        let new_line_i = if cell_diff.0 > 0 {
229            curr_range_i.end
230        } else {
231            curr_range_i.start
232        };
233
234        let new_line_j = if cell_diff.1 > 0 {
235            curr_range_j.end
236        } else {
237            curr_range_j.start
238        };
239
240        let ignore_line_i = new_line_i < 0 || new_line_i >= ncells_i as isize;
241        let ignore_line_j = new_line_j < 0 || new_line_j >= ncells_j as isize;
242
243        if ignore_line_i && ignore_line_j {
244            break;
245        }
246
247        if !ignore_line_i && cell_diff.0 != 0 {
248            for j in curr_range_j.clone() {
249                hit_triangles(new_line_i, j)?;
250            }
251        }
252
253        if !ignore_line_j && cell_diff.1 != 0 {
254            for i in curr_range_i.clone() {
255                hit_triangles(i, new_line_j)?;
256            }
257        }
258    }
259
260    Ok(best_hit)
261}
262
263/// Time Of Impact between a moving shape and a heightfield.
264pub fn cast_shapes_shape_heightfield<D: ?Sized + QueryDispatcher>(
265    dispatcher: &D,
266    pos12: &Isometry<Real>,
267    vel12: &Vector<Real>,
268    g1: &dyn Shape,
269    heightfield2: &HeightField,
270    options: ShapeCastOptions,
271) -> Result<Option<ShapeCastHit>, Unsupported> {
272    Ok(cast_shapes_heightfield_shape(
273        dispatcher,
274        &pos12.inverse(),
275        &-pos12.inverse_transform_vector(vel12),
276        heightfield2,
277        g1,
278        options,
279    )?
280    .map(|hit| hit.swapped()))
281}