parry3d/query/shape_cast/
shape_cast_heightfield_shape.rs1use 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#[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 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 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 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 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 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#[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 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 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 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 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 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 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
263pub 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}