From 1df8234451bc0eabc710d1536b4b7316488643e2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sun, 2 Oct 2022 16:24:51 +0200 Subject: [PATCH 1/6] Add Heightfield::to_polyline --- .../to_polyline/heightfield_to_polyline.rs | 25 +++++++++++++++++++ src/transformation/to_polyline/mod.rs | 1 + 2 files changed, 26 insertions(+) create mode 100644 src/transformation/to_polyline/heightfield_to_polyline.rs diff --git a/src/transformation/to_polyline/heightfield_to_polyline.rs b/src/transformation/to_polyline/heightfield_to_polyline.rs new file mode 100644 index 00000000..4e8df586 --- /dev/null +++ b/src/transformation/to_polyline/heightfield_to_polyline.rs @@ -0,0 +1,25 @@ +use crate::math::Real; +use crate::shape::HeightField; +use na::Point2; + +impl HeightField { + /// Rasterize this heightfield as a (potentially discontinuous) polyline. + pub fn to_polyline(&self) -> (Vec>, Vec<[u32; 2]>) { + let mut vertices = vec![]; + let mut indices = vec![]; + + for seg in self.segments() { + let base_id = vertices.len() as u32; + if vertices.last().map(|pt| seg.a != *pt).unwrap_or(true) { + indices.push([base_id, base_id + 1]); + vertices.push(seg.a); + } else { + indices.push([base_id - 1, base_id]); + } + + vertices.push(seg.b); + } + + (vertices, indices) + } +} diff --git a/src/transformation/to_polyline/mod.rs b/src/transformation/to_polyline/mod.rs index d3c3ca9f..4a60b759 100644 --- a/src/transformation/to_polyline/mod.rs +++ b/src/transformation/to_polyline/mod.rs @@ -1,5 +1,6 @@ mod ball_to_polyline; mod capsule_to_polyline; mod cuboid_to_polyline; +mod heightfield_to_polyline; mod round_convex_polygon_to_polyline; mod round_cuboid_to_polyline; From 477ae9c4aaa7d5ef618e5f9b1227c1b8ad86c3d9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sun, 2 Oct 2022 16:25:24 +0200 Subject: [PATCH 2/6] Add heigthfield shape-casting --- .../time_of_impact_heightfield_shape.rs | 310 ++++++++++++++++++ src/shape/heightfield2.rs | 59 ++-- src/shape/heightfield3.rs | 103 ++++-- 3 files changed, 419 insertions(+), 53 deletions(-) create mode 100644 src/query/time_of_impact/time_of_impact_heightfield_shape.rs diff --git a/src/query/time_of_impact/time_of_impact_heightfield_shape.rs b/src/query/time_of_impact/time_of_impact_heightfield_shape.rs new file mode 100644 index 00000000..a380919e --- /dev/null +++ b/src/query/time_of_impact/time_of_impact_heightfield_shape.rs @@ -0,0 +1,310 @@ +use crate::math::{Isometry, Real, Vector}; +use crate::query::{QueryDispatcher, Ray, Unsupported, TOI}; +use crate::shape::{GenericHeightField, HeightFieldCellStatus, HeightFieldStorage, Shape}; +#[cfg(feature = "dim3")] +use crate::{bounding_volume::AABB, query::RayCast}; + +/// Time Of Impact between a moving shape and a heightfield. +#[cfg(feature = "dim2")] +pub fn time_of_impact_heightfield_shape( + dispatcher: &D, + pos12: &Isometry, + vel12: &Vector, + heightfield1: &GenericHeightField, + g2: &dyn Shape, + max_toi: Real, + stop_at_penetration: bool, +) -> Result, Unsupported> +where + Heights: HeightFieldStorage, + Status: HeightFieldStorage, + D: QueryDispatcher, +{ + let aabb2_1 = g2.compute_aabb(pos12); + let ray = Ray::new(aabb2_1.center(), *vel12); + + let mut curr_range = heightfield1.unclamped_elements_range_in_local_aabb(&aabb2_1); + // Enlarge the range by 1 to account for movement within a cell. + let right = ray.dir.x > 0.0; + + if right { + curr_range.end += 1; + } else { + curr_range.start -= 1; + } + + let mut best_hit = None::; + + /* + * Test the segment under the ray. + */ + let clamped_curr_range = curr_range.start.clamp(0, heightfield1.num_cells() as isize) as usize + ..curr_range.end.clamp(0, heightfield1.num_cells() as isize) as usize; + for curr in clamped_curr_range { + if let Some(seg) = heightfield1.segment_at(curr) { + // TODO: pre-check using a ray-cast on the AABBs first? + if let Some(hit) = + dispatcher.time_of_impact(pos12, vel12, &seg, g2, max_toi, stop_at_penetration)? + { + if hit.toi < best_hit.map(|toi| toi.toi).unwrap_or(Real::MAX) { + best_hit = Some(hit); + } + } + } + } + + /* + * Test other segments in the path of the ray. + */ + if ray.dir.x == 0.0 { + return Ok(best_hit); + } + + let cell_width = heightfield1.cell_width(); + let start_x = heightfield1.start_x(); + + let mut curr_elt = if right { + (curr_range.end - 1).max(0) + } else { + curr_range.start.min(heightfield1.num_cells() as isize - 1) + }; + + while (right && curr_elt < heightfield1.num_cells() as isize - 1) || (!right && curr_elt > 0) { + let curr_param; + + if right { + curr_elt += 1; + curr_param = (cell_width * na::convert::(curr_elt as f64) + start_x + - ray.origin.x) + / ray.dir.x; + } else { + curr_param = + (ray.origin.x - cell_width * na::convert::(curr_elt as f64) - start_x) + / ray.dir.x; + curr_elt -= 1; + } + + if curr_param >= max_toi { + break; + } + + if let Some(seg) = heightfield1.segment_at(curr_elt as usize) { + // TODO: pre-check using a ray-cast on the AABBs first? + if let Some(hit) = + dispatcher.time_of_impact(pos12, vel12, &seg, g2, max_toi, stop_at_penetration)? + { + if hit.toi < best_hit.map(|toi| toi.toi).unwrap_or(Real::MAX) { + best_hit = Some(hit); + } + } + } + } + + Ok(best_hit) +} + +/// Time Of Impact between a moving shape and a heightfield. +#[cfg(feature = "dim3")] +pub fn time_of_impact_heightfield_shape( + dispatcher: &D, + pos12: &Isometry, + vel12: &Vector, + heightfield1: &GenericHeightField, + g2: &dyn Shape, + max_toi: Real, + stop_at_penetration: bool, +) -> Result, Unsupported> +where + Heights: HeightFieldStorage, + Status: HeightFieldStorage, + D: QueryDispatcher, +{ + let aabb1 = heightfield1.local_aabb(); + let mut aabb2_1 = g2.compute_aabb(pos12); + let ray = Ray::new(aabb2_1.center(), *vel12); + + // Find the first hit between the aabbs. + let hext2_1 = aabb2_1.half_extents(); + let msum = AABB::new(aabb1.mins - hext2_1, aabb1.maxs + hext2_1); + if let Some(toi) = msum.cast_local_ray(&ray, max_toi, true) { + // Advance the aabb2 to the hit point. + aabb2_1.mins += ray.dir * toi; + aabb2_1.maxs += ray.dir * toi; + } else { + return Ok(None); + } + + let (mut curr_range_i, mut curr_range_j) = + heightfield1.unclamped_elements_range_in_local_aabb(&aabb2_1); + let (ncells_i, ncells_j) = heightfield1.num_cells_ij(); + let mut best_hit = None::; + + /* + * Enlarge the ranges by 1 to account for any movement within one cell. + */ + if ray.dir.z > 0.0 { + curr_range_i.end += 1; + } else if ray.dir.z < 0.0 { + curr_range_i.start -= 1; + } + + if ray.dir.x > 0.0 { + curr_range_j.end += 1; + } else if ray.dir.x < 0.0 { + curr_range_j.start -= 1; + } + + /* + * Test the segment under the ray. + */ + let clamped_curr_range_i = curr_range_i.start.clamp(0, ncells_i as isize) + ..curr_range_i.end.clamp(0, ncells_i as isize); + let clamped_curr_range_j = curr_range_j.start.clamp(0, ncells_j as isize) + ..curr_range_j.end.clamp(0, ncells_j as isize); + + let mut hit_triangles = |i, j| { + if i >= 0 && j >= 0 { + let (tri_a, tri_b) = heightfield1.triangles_at(i as usize, j as usize); + for tri in [tri_a, tri_b] { + if let Some(tri) = tri { + // TODO: pre-check using a ray-cast on the AABBs first? + if let Some(hit) = dispatcher.time_of_impact( + pos12, + vel12, + &tri, + g2, + max_toi, + stop_at_penetration, + )? { + if hit.toi < best_hit.map(|toi| toi.toi).unwrap_or(Real::MAX) { + best_hit = Some(hit); + } + } + } + } + } + + Ok(()) + }; + + for i in clamped_curr_range_i { + for j in clamped_curr_range_j.clone() { + hit_triangles(i, j)?; + } + } + + if ray.dir.y == 0.0 { + return Ok(best_hit); + } + + let mut cell = heightfield1.unclamped_cell_at_point(&aabb2_1.center()); + + loop { + let prev_cell = cell; + + /* + * Find the next cell to cast the ray on. + */ + let toi_x = if ray.dir.x > 0.0 { + let x = heightfield1.signed_x_at(cell.1 + 1); + (x - ray.origin.x) / ray.dir.x + } else if ray.dir.x < 0.0 { + let x = heightfield1.signed_x_at(cell.1 + 0); + (x - ray.origin.x) / ray.dir.x + } else { + Real::MAX + }; + + let toi_z = if ray.dir.z > 0.0 { + let z = heightfield1.signed_z_at(cell.0 + 1); + (z - ray.origin.z) / ray.dir.z + } else if ray.dir.z < 0.0 { + let z = heightfield1.signed_z_at(cell.0 + 0); + (z - ray.origin.z) / ray.dir.z + } else { + Real::MAX + }; + + if toi_x > max_toi && toi_z > max_toi { + break; + } + + if toi_x >= 0.0 && toi_x <= toi_z { + cell.1 += ray.dir.x.signum() as isize; + } + + if toi_z >= 0.0 && toi_z <= toi_x { + cell.0 += ray.dir.z.signum() as isize; + } + + if cell == prev_cell { + break; + } + + let cell_diff = (cell.0 - prev_cell.0, cell.1 - prev_cell.1); + curr_range_i.start += cell_diff.0; + curr_range_i.end += cell_diff.0; + curr_range_j.start += cell_diff.1; + curr_range_j.end += cell_diff.1; + + let new_line_i = if cell_diff.0 > 0 { + curr_range_i.end + } else { + curr_range_i.start + }; + + let new_line_j = if cell_diff.1 > 0 { + curr_range_j.end + } else { + curr_range_j.start + }; + + let ignore_line_i = new_line_i < 0 || new_line_i >= ncells_i as isize; + let ignore_line_j = new_line_j < 0 || new_line_j >= ncells_j as isize; + + if ignore_line_i && ignore_line_j { + break; + } + + if !ignore_line_i && cell_diff.0 != 0 { + for j in curr_range_j.clone() { + hit_triangles(new_line_i, j)?; + } + } + + if !ignore_line_j && cell_diff.1 != 0 { + for i in curr_range_i.clone() { + hit_triangles(i, new_line_j)?; + } + } + } + + Ok(best_hit) +} + +/// Time Of Impact between a moving shape and a heightfield. +pub fn time_of_impact_shape_heightfield( + dispatcher: &D, + pos12: &Isometry, + vel12: &Vector, + g1: &dyn Shape, + heightfield2: &GenericHeightField, + max_toi: Real, + stop_at_penetration: bool, +) -> Result, Unsupported> +where + Heights: HeightFieldStorage, + Status: HeightFieldStorage, + D: QueryDispatcher, +{ + Ok(time_of_impact_heightfield_shape( + dispatcher, + &pos12.inverse(), + &-pos12.inverse_transform_vector(&vel12), + heightfield2, + g1, + max_toi, + stop_at_penetration, + )? + .map(|toi| toi.swapped())) +} diff --git a/src/shape/heightfield2.rs b/src/shape/heightfield2.rs index e176c2c9..a1e89a52 100644 --- a/src/shape/heightfield2.rs +++ b/src/shape/heightfield2.rs @@ -2,6 +2,7 @@ use na::ComplexField; #[cfg(feature = "std")] use na::DVector; +use std::ops::Range; #[cfg(all(feature = "std", feature = "cuda"))] use {crate::utils::CudaArray1, cust::error::CudaResult}; @@ -173,41 +174,44 @@ where /// The width of a single cell of this heightfield, without taking the scale factor into account. pub fn unit_cell_width(&self) -> Real { - 1.0 / na::convert::(self.heights.len() as f64 - 1.0) + 1.0 / (self.heights.len() as Real - 1.0) } /// The left-most x-coordinate of this heightfield. pub fn start_x(&self) -> Real { - self.scale.x * na::convert::(-0.5) + self.scale.x * -0.5 + } + + fn quantize_floor_unclamped(&self, val: Real, seg_length: Real) -> isize { + ((val + 0.5) / seg_length).floor() as isize + } + + fn quantize_ceil_unclamped(&self, val: Real, seg_length: Real) -> isize { + ((val + 0.5) / seg_length).ceil() as isize } fn quantize_floor(&self, val: Real, seg_length: Real) -> usize { - let _0_5: Real = na::convert::(0.5); - let i = na::clamp( - ((val + _0_5) / seg_length).floor(), + na::clamp( + ((val + 0.5) / seg_length).floor(), 0.0, - na::convert::((self.num_cells() - 1) as f64), - ); - na::convert_unchecked::(i) as usize + (self.num_cells() - 1) as Real, + ) as usize } fn quantize_ceil(&self, val: Real, seg_length: Real) -> usize { - let _0_5: Real = na::convert::(0.5); - let i = na::clamp( - ((val + _0_5) / seg_length).ceil(), + na::clamp( + ((val + 0.5) / seg_length).ceil(), 0.0, - na::convert::(self.num_cells() as f64), - ); - na::convert_unchecked::(i) as usize + self.num_cells() as Real, + ) as usize } /// Index of the cell a point is on after vertical projection. pub fn cell_at_point(&self, pt: &Point2) -> Option { - let _0_5: Real = na::convert::(0.5); let scaled_pt = pt.coords.component_div(&self.scale); let seg_length = self.unit_cell_width(); - if scaled_pt.x < -_0_5 || scaled_pt.x > _0_5 { + if scaled_pt.x < -0.5 || scaled_pt.x > 0.5 { // Outside of the heightfield bounds. None } else { @@ -241,10 +245,9 @@ where return None; } - let _0_5: Real = na::convert::(0.5); - let seg_length = 1.0 / na::convert::(self.heights.len() as f64 - 1.0); + let seg_length = 1.0 / (self.heights.len() as Real - 1.0); - let x0 = -_0_5 + seg_length * na::convert::(i as f64); + let x0 = -0.5 + seg_length * (i as Real); let x1 = x0 + seg_length; let y0 = self.heights.get(i + 0); @@ -270,14 +273,24 @@ where !self.status.get(i) } + /// The range of segment ids that may intersect the given local AABB. + pub fn unclamped_elements_range_in_local_aabb(&self, aabb: &AABB) -> Range { + let ref_mins = aabb.mins.coords.component_div(&self.scale); + let ref_maxs = aabb.maxs.coords.component_div(&self.scale); + let seg_length = 1.0 / (self.heights.len() as Real - 1.0); + + let min_x = self.quantize_floor_unclamped(ref_mins.x, seg_length); + let max_x = self.quantize_ceil_unclamped(ref_maxs.x, seg_length); + min_x..max_x + } + /// Applies `f` to each segment of this heightfield that intersects the given `aabb`. pub fn map_elements_in_local_aabb(&self, aabb: &AABB, f: &mut impl FnMut(u32, &Segment)) { - let _0_5: Real = na::convert::(0.5); let ref_mins = aabb.mins.coords.component_div(&self.scale); let ref_maxs = aabb.maxs.coords.component_div(&self.scale); - let seg_length = 1.0 / na::convert::(self.heights.len() as f64 - 1.0); + let seg_length = 1.0 / (self.heights.len() as Real - 1.0); - if ref_maxs.x < -_0_5 || ref_mins.x > _0_5 { + if ref_maxs.x < -0.5 || ref_mins.x > 0.5 { // Outside of the heightfield bounds. return; } @@ -292,7 +305,7 @@ where continue; } - let x0 = -_0_5 + seg_length * na::convert::(i as f64); + let x0 = -0.5 + seg_length * (i as Real); let x1 = x0 + seg_length; let y0 = self.heights.get(i + 0); diff --git a/src/shape/heightfield3.rs b/src/shape/heightfield3.rs index 7f317346..9123a664 100644 --- a/src/shape/heightfield3.rs +++ b/src/shape/heightfield3.rs @@ -1,5 +1,6 @@ #[cfg(feature = "std")] use na::DMatrix; +use std::ops::Range; #[cfg(all(feature = "std", feature = "cuda"))] use {crate::utils::CudaArray2, cust::error::CudaResult}; @@ -106,7 +107,7 @@ impl HeightField { ); let max = heights.max(); let min = heights.min(); - let hscale = scale * na::convert::<_, Real>(0.5); + let hscale = scale * 0.5; let aabb = AABB::new( Point3::new(-hscale.x, min * scale.y, -hscale.z), Point3::new(hscale.x, max * scale.y, hscale.z), @@ -187,29 +188,28 @@ where } } + fn quantize_floor_unclamped(&self, val: Real, cell_size: Real) -> isize { + ((val + 0.5) / cell_size).floor() as isize + } + + fn quantize_ceil_unclamped(&self, val: Real, cell_size: Real) -> isize { + ((val + 0.5) / cell_size).ceil() as isize + } + fn quantize_floor(&self, val: Real, cell_size: Real, num_cells: usize) -> usize { - let _0_5: Real = na::convert::(0.5); - let i = na::clamp( - ((val + _0_5) / cell_size).floor(), + na::clamp( + ((val + 0.5) / cell_size).floor(), 0.0, - na::convert::((num_cells - 1) as f64), - ); - na::convert_unchecked::(i) as usize + (num_cells - 1) as Real, + ) as usize } fn quantize_ceil(&self, val: Real, cell_size: Real, num_cells: usize) -> usize { - let _0_5: Real = na::convert::(0.5); - let i = na::clamp( - ((val + _0_5) / cell_size).ceil(), - 0.0, - na::convert::(num_cells as f64), - ); - na::convert_unchecked::(i) as usize + na::clamp(((val + 0.5) / cell_size).ceil(), 0.0, num_cells as Real) as usize } /// The pair of index of the cell containing the vertical projection of the given point. pub fn closest_cell_at_point(&self, pt: &Point3) -> (usize, usize) { - let _0_5: Real = na::convert::(0.5); let scaled_pt = pt.coords.component_div(&self.scale); let cell_width = self.unit_cell_width(); let cell_height = self.unit_cell_height(); @@ -223,14 +223,13 @@ where /// The pair of index of the cell containing the vertical projection of the given point. pub fn cell_at_point(&self, pt: &Point3) -> Option<(usize, usize)> { - let _0_5: Real = na::convert::(0.5); let scaled_pt = pt.coords.component_div(&self.scale); let cell_width = self.unit_cell_width(); let cell_height = self.unit_cell_height(); let ncells_x = self.ncols(); let ncells_z = self.nrows(); - if scaled_pt.x < -_0_5 || scaled_pt.x > _0_5 || scaled_pt.z < -_0_5 || scaled_pt.z > _0_5 { + if scaled_pt.x < -0.5 || scaled_pt.x > 0.5 || scaled_pt.z < -0.5 || scaled_pt.z > 0.5 { // Outside of the heightfield bounds. None } else { @@ -240,16 +239,35 @@ where } } + /// The pair of index of the cell containing the vertical projection of the given point. + pub fn unclamped_cell_at_point(&self, pt: &Point3) -> (isize, isize) { + let scaled_pt = pt.coords.component_div(&self.scale); + let cell_width = self.unit_cell_width(); + let cell_height = self.unit_cell_height(); + + let j = self.quantize_floor_unclamped(scaled_pt.x, cell_width); + let i = self.quantize_floor_unclamped(scaled_pt.z, cell_height); + (i, j) + } + /// The smallest x coordinate of the `j`-th column of this heightfield. pub fn x_at(&self, j: usize) -> Real { - let _0_5: Real = na::convert::(0.5); - (-_0_5 + self.unit_cell_width() * na::convert::(j as f64)) * self.scale.x + (-0.5 + self.unit_cell_width() * (j as Real)) * self.scale.x } /// The smallest z coordinate of the start of the `i`-th row of this heightfield. pub fn z_at(&self, i: usize) -> Real { - let _0_5: Real = na::convert::(0.5); - (-_0_5 + self.unit_cell_height() * na::convert::(i as f64)) * self.scale.z + (-0.5 + self.unit_cell_height() * (i as Real)) * self.scale.z + } + + /// The smallest x coordinate of the `j`-th column of this heightfield. + pub fn signed_x_at(&self, j: isize) -> Real { + (-0.5 + self.unit_cell_width() * (j as Real)) * self.scale.x + } + + /// The smallest z coordinate of the start of the `i`-th row of this heightfield. + pub fn signed_z_at(&self, i: isize) -> Real { + (-0.5 + self.unit_cell_height() * (i as Real)) * self.scale.z } /// An iterator through all the triangles of this heightfield. @@ -281,6 +299,10 @@ where /// Returns `None` fore triangles that have been removed because of their user-defined status /// flags (described by the `HeightFieldCellStatus` bitfield). pub fn triangles_at(&self, i: usize, j: usize) -> (Option, Option) { + if i >= self.heights.nrows() - 1 || j >= self.heights.ncols() - 1 { + return (None, None); + } + let status = self.status.get(i, j); if status.contains( @@ -293,11 +315,10 @@ where let cell_width = self.unit_cell_width(); let cell_height = self.unit_cell_height(); - let _0_5: Real = na::convert::(0.5); - let z0 = -_0_5 + cell_height * na::convert::(i as f64); + let z0 = -0.5 + cell_height * (i as Real); let z1 = z0 + cell_height; - let x0 = -_0_5 + cell_width * na::convert::(j as f64); + let x0 = -0.5 + cell_width * (j as Real); let x1 = x0 + cell_width; let y00 = self.heights.get(i + 0, j + 0); @@ -347,6 +368,11 @@ where } } + /// The number of cells of this heightfield along each dimension. + pub fn num_cells_ij(&self) -> (usize, usize) { + (self.nrows(), self.ncols()) + } + /// The status of the `(i, j)`-th cell. pub fn cell_status(&self, i: usize, j: usize) -> HeightFieldCellStatus { self.status.get(i, j) @@ -403,12 +429,12 @@ where /// The width (extent along its local `x` axis) of each cell of this heightmap, excluding the scale factor. pub fn unit_cell_width(&self) -> Real { - 1.0 / na::convert::(self.heights.ncols() as f64 - 1.0) + 1.0 / (self.heights.ncols() as Real - 1.0) } /// The height (extent along its local `z` axis) of each cell of this heightmap, excluding the scale factor. pub fn unit_cell_height(&self) -> Real { - 1.0 / na::convert::(self.heights.nrows() as f64 - 1.0) + 1.0 / (self.heights.nrows() as Real - 1.0) } /// The AABB of this heightmap. @@ -510,9 +536,26 @@ where } } + /// The range of segment ids that may intersect the given local AABB. + pub fn unclamped_elements_range_in_local_aabb( + &self, + aabb: &AABB, + ) -> (Range, Range) { + let ref_mins = aabb.mins.coords.component_div(&self.scale); + let ref_maxs = aabb.maxs.coords.component_div(&self.scale); + let cell_width = self.unit_cell_width(); + let cell_height = self.unit_cell_height(); + + let min_x = self.quantize_floor_unclamped(ref_mins.x, cell_width); + let min_z = self.quantize_floor_unclamped(ref_mins.z, cell_height); + + let max_x = self.quantize_ceil_unclamped(ref_maxs.x, cell_width); + let max_z = self.quantize_ceil_unclamped(ref_maxs.z, cell_height); + (min_z..max_z, min_x..max_x) + } + /// Applies the function `f` to all the triangles of this heightfield intersecting the given AABB. pub fn map_elements_in_local_aabb(&self, aabb: &AABB, f: &mut impl FnMut(u32, &Triangle)) { - let _0_5: Real = na::convert::(0.5); let ncells_x = self.ncols(); let ncells_z = self.nrows(); @@ -521,7 +564,7 @@ where let cell_width = self.unit_cell_width(); let cell_height = self.unit_cell_height(); - if ref_maxs.x <= -_0_5 || ref_maxs.z <= -_0_5 || ref_mins.x >= _0_5 || ref_mins.z >= _0_5 { + if ref_maxs.x <= -0.5 || ref_maxs.z <= -0.5 || ref_mins.x >= 0.5 || ref_mins.z >= 0.5 { // Outside of the heightfield bounds. return; } @@ -542,10 +585,10 @@ where continue; } - let z0 = -_0_5 + cell_height * na::convert::(i as f64); + let z0 = -0.5 + cell_height * (i as Real); let z1 = z0 + cell_height; - let x0 = -_0_5 + cell_width * na::convert::(j as f64); + let x0 = -0.5 + cell_width * (j as Real); let x1 = x0 + cell_width; let y00 = self.heights.get(i + 0, j + 0); From 9ed21c1dc8147553832b65eea06139fd096a04e7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sun, 2 Oct 2022 16:26:01 +0200 Subject: [PATCH 3/6] Add a stop_at_penetration argument to the linear shape-cast --- src/query/default_query_dispatcher.rs | 28 ++++++++++++++++++- src/query/query_dispatcher.rs | 2 ++ src/query/time_of_impact/mod.rs | 4 +++ src/query/time_of_impact/time_of_impact.rs | 3 +- .../time_of_impact_composite_shape_shape.rs | 27 ++++++++++++++++-- .../time_of_impact_halfspace_support_map.rs | 7 +++++ .../time_of_impact_support_map_support_map.rs | 18 ++++++++++++ 7 files changed, 84 insertions(+), 5 deletions(-) diff --git a/src/query/default_query_dispatcher.rs b/src/query/default_query_dispatcher.rs index 33674d06..c1367723 100644 --- a/src/query/default_query_dispatcher.rs +++ b/src/query/default_query_dispatcher.rs @@ -8,7 +8,7 @@ use crate::query::{ contact_manifolds::ContactManifoldsWorkspace, query_dispatcher::PersistentQueryDispatcher, ContactManifold, }; -use crate::shape::{HalfSpace, Segment, Shape, ShapeType}; +use crate::shape::{HalfSpace, HeightField, Segment, Shape, ShapeType}; /// A dispatcher that exposes built-in queries #[derive(Debug, Clone)] @@ -274,6 +274,7 @@ impl QueryDispatcher for DefaultQueryDispatcher { shape1: &dyn Shape, shape2: &dyn Shape, max_toi: Real, + stop_at_penetration: bool, ) -> Result, Unsupported> { if let (Some(b1), Some(b2)) = (shape1.as_ball(), shape2.as_ball()) { Ok(query::details::time_of_impact_ball_ball( @@ -292,6 +293,7 @@ impl QueryDispatcher for DefaultQueryDispatcher { p1, s2, max_toi, + stop_at_penetration, )) } else if let (Some(s1), Some(p2)) = (shape1.as_support_map(), shape2.as_shape::()) @@ -302,7 +304,28 @@ impl QueryDispatcher for DefaultQueryDispatcher { s1, p2, max_toi, + stop_at_penetration, )) + } else if let Some(heightfield1) = shape1.as_shape::() { + query::details::time_of_impact_heightfield_shape( + self, + pos12, + local_vel12, + heightfield1, + shape2, + max_toi, + stop_at_penetration, + ) + } else if let Some(heightfield2) = shape1.as_shape::() { + query::details::time_of_impact_shape_heightfield( + self, + pos12, + local_vel12, + shape1, + heightfield2, + max_toi, + stop_at_penetration, + ) } else if let (Some(s1), Some(s2)) = (shape1.as_support_map(), shape2.as_support_map()) { Ok(query::details::time_of_impact_support_map_support_map( pos12, @@ -310,6 +333,7 @@ impl QueryDispatcher for DefaultQueryDispatcher { s1, s2, max_toi, + stop_at_penetration, )) } else { #[cfg(feature = "std")] @@ -321,6 +345,7 @@ impl QueryDispatcher for DefaultQueryDispatcher { c1, shape2, max_toi, + stop_at_penetration, )); } else if let Some(c2) = shape2.as_composite_shape() { return Ok(query::details::time_of_impact_shape_composite_shape( @@ -330,6 +355,7 @@ impl QueryDispatcher for DefaultQueryDispatcher { shape1, c2, max_toi, + stop_at_penetration, )); } diff --git a/src/query/query_dispatcher.rs b/src/query/query_dispatcher.rs index c98917bd..8fb5891a 100644 --- a/src/query/query_dispatcher.rs +++ b/src/query/query_dispatcher.rs @@ -103,6 +103,7 @@ pub trait QueryDispatcher: Send + Sync { g1: &dyn Shape, g2: &dyn Shape, max_toi: Real, + stop_at_penetration: bool, ) -> Result, Unsupported>; /// Construct a `QueryDispatcher` that falls back on `other` for cases not handled by `self` @@ -187,6 +188,7 @@ where g1: &dyn Shape, g2: &dyn Shape, max_toi: Real, + stop_at_penetration: bool, ) -> Option); chain_method!(nonlinear_time_of_impact( diff --git a/src/query/time_of_impact/mod.rs b/src/query/time_of_impact/mod.rs index 0e96fba5..97ea2b4c 100644 --- a/src/query/time_of_impact/mod.rs +++ b/src/query/time_of_impact/mod.rs @@ -10,6 +10,9 @@ pub use self::time_of_impact_composite_shape_shape::{ pub use self::time_of_impact_halfspace_support_map::{ time_of_impact_halfspace_support_map, time_of_impact_support_map_halfspace, }; +pub use self::time_of_impact_heightfield_shape::{ + time_of_impact_heightfield_shape, time_of_impact_shape_heightfield, +}; pub use self::time_of_impact_support_map_support_map::time_of_impact_support_map_support_map; mod time_of_impact; @@ -17,4 +20,5 @@ mod time_of_impact_ball_ball; #[cfg(feature = "std")] mod time_of_impact_composite_shape_shape; mod time_of_impact_halfspace_support_map; +mod time_of_impact_heightfield_shape; mod time_of_impact_support_map_support_map; diff --git a/src/query/time_of_impact/time_of_impact.rs b/src/query/time_of_impact/time_of_impact.rs index a30b52bd..a612c963 100644 --- a/src/query/time_of_impact/time_of_impact.rs +++ b/src/query/time_of_impact/time_of_impact.rs @@ -90,8 +90,9 @@ pub fn time_of_impact( vel2: &Vector, g2: &dyn Shape, max_toi: Real, + stop_at_penetration: bool, ) -> Result, Unsupported> { let pos12 = pos1.inv_mul(pos2); let vel12 = pos1.inverse_transform_vector(&(vel2 - vel1)); - DefaultQueryDispatcher.time_of_impact(&pos12, &vel12, g1, g2, max_toi) + DefaultQueryDispatcher.time_of_impact(&pos12, &vel12, g1, g2, max_toi, stop_at_penetration) } diff --git a/src/query/time_of_impact/time_of_impact_composite_shape_shape.rs b/src/query/time_of_impact/time_of_impact_composite_shape_shape.rs index 7f578ab0..cfb592bc 100644 --- a/src/query/time_of_impact/time_of_impact_composite_shape_shape.rs +++ b/src/query/time_of_impact/time_of_impact_composite_shape_shape.rs @@ -13,13 +13,21 @@ pub fn time_of_impact_composite_shape_shape( g1: &G1, g2: &dyn Shape, max_toi: Real, + stop_at_penetration: bool, ) -> Option where D: QueryDispatcher, G1: TypedSimdCompositeShape, { - let mut visitor = - TOICompositeShapeShapeBestFirstVisitor::new(dispatcher, pos12, vel12, g1, g2, max_toi); + let mut visitor = TOICompositeShapeShapeBestFirstVisitor::new( + dispatcher, + pos12, + vel12, + g1, + g2, + max_toi, + stop_at_penetration, + ); g1.typed_qbvh() .traverse_best_first(&mut visitor) .map(|res| res.1 .1) @@ -33,6 +41,7 @@ pub fn time_of_impact_shape_composite_shape( g1: &dyn Shape, g2: &G2, max_toi: Real, + stop_at_penetration: bool, ) -> Option where D: QueryDispatcher, @@ -45,6 +54,7 @@ where g2, g1, max_toi, + stop_at_penetration, ) .map(|toi| toi.swapped()) } @@ -61,6 +71,7 @@ pub struct TOICompositeShapeShapeBestFirstVisitor<'a, D: ?Sized, G1: ?Sized + 'a g1: &'a G1, g2: &'a dyn Shape, max_toi: Real, + stop_at_penetration: bool, } impl<'a, D: ?Sized, G1: ?Sized> TOICompositeShapeShapeBestFirstVisitor<'a, D, G1> @@ -76,6 +87,7 @@ where g1: &'a G1, g2: &'a dyn Shape, max_toi: Real, + stop_at_penetration: bool, ) -> TOICompositeShapeShapeBestFirstVisitor<'a, D, G1> { let ls_aabb2 = g2.compute_aabb(pos12); let ray = Ray::new(Point::origin(), *vel12); @@ -90,6 +102,7 @@ where g1, g2, max_toi, + stop_at_penetration, } } } @@ -139,13 +152,21 @@ where g1, self.g2, self.max_toi, + self.stop_at_penetration, ) .unwrap_or(None) .map(|toi| toi.transform1_by(part_pos1)); } else { toi = self .dispatcher - .time_of_impact(&self.pos12, self.vel12, g1, self.g2, self.max_toi) + .time_of_impact( + &self.pos12, + self.vel12, + g1, + self.g2, + self.max_toi, + self.stop_at_penetration, + ) .unwrap_or(None); } }); diff --git a/src/query/time_of_impact/time_of_impact_halfspace_support_map.rs b/src/query/time_of_impact/time_of_impact_halfspace_support_map.rs index 264d85d2..bef6718b 100644 --- a/src/query/time_of_impact/time_of_impact_halfspace_support_map.rs +++ b/src/query/time_of_impact/time_of_impact_halfspace_support_map.rs @@ -9,12 +9,17 @@ pub fn time_of_impact_halfspace_support_map( halfspace: &HalfSpace, other: &G, max_toi: Real, + stop_at_penetration: bool, ) -> Option where G: SupportMap, { // FIXME: add method to get only the local support point. // This would avoid the `inverse_transform_point` later. + if !stop_at_penetration && vel12.dot(&halfspace.normal) > 0.0 { + return None; + } + let support_point = other.support_point(pos12, &-halfspace.normal); let closest_point = support_point; let ray = Ray::new(closest_point, *vel12); @@ -57,6 +62,7 @@ pub fn time_of_impact_support_map_halfspace( other: &G, halfspace: &HalfSpace, max_toi: Real, + stop_at_penetration: bool, ) -> Option where G: SupportMap, @@ -67,6 +73,7 @@ where halfspace, other, max_toi, + stop_at_penetration, ) .map(|toi| toi.swapped()) } diff --git a/src/query/time_of_impact/time_of_impact_support_map_support_map.rs b/src/query/time_of_impact/time_of_impact_support_map_support_map.rs index df0581e6..499c5adb 100644 --- a/src/query/time_of_impact/time_of_impact_support_map_support_map.rs +++ b/src/query/time_of_impact/time_of_impact_support_map_support_map.rs @@ -1,6 +1,7 @@ use na::Unit; use crate::math::{Isometry, Real, Vector}; +use crate::query::details; use crate::query::gjk::{self, VoronoiSimplex}; use crate::query::{TOIStatus, TOI}; use crate::shape::SupportMap; @@ -13,6 +14,7 @@ pub fn time_of_impact_support_map_support_map( g1: &G1, g2: &G2, max_toi: Real, + stop_at_penetration: bool, ) -> Option where G1: SupportMap, @@ -22,6 +24,22 @@ where |(toi, normal1, witness1, witness2)| { if toi > max_toi { None + } else if !stop_at_penetration && toi < 1.0e-5 { + let contact = details::contact_support_map_support_map(pos12, g1, g2, Real::MAX)?; + let normal_vel = contact.normal1.dot(&vel12); + + if normal_vel >= 0.0 { + None + } else { + Some(TOI { + toi, + normal1: contact.normal1, + normal2: contact.normal2, + witness1: contact.point1, + witness2: contact.point2, + status: TOIStatus::Penetrating, + }) + } } else { Some(TOI { toi, From 36b9cde85b27bb4d258a37550a9de1a2b598166f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sun, 2 Oct 2022 16:35:54 +0200 Subject: [PATCH 4/6] Update changelog --- CHANGELOG.md | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 0a05210b..514a6636 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,5 +1,23 @@ # Change Log +## Unreleased + +### Modified +- Add to `query::time_of_impact` a boolean argument `stop_at_penetration`. If set to `false` + the linear shape-cast won’t immediately stop if the shape is penetrating another shape at its + starting point **and** its trajectory is such that it’s existing that penetration configuration. + +### Added +- Add 2D `Heightfield::to_polyline` to get the explicit vertices/indices of a 2D heightfield + seen as a polyline. +- Add the support for linear shape-cast (`query::time_of_impact`) for heightfields. +- Make the convex polyhedron scaling more forgiving regarding normals to avoid frequent unjustified panics. +- Fix panic happening when building a convex polyhedron with empty inputs. + + +### Fixed +- Fix the application of non-uniform scaling to balls. + ## v0.9.0 (30 Apr. 2022) ### Modified From 70e04f3ead6e8ae84c8c007ff509c8e9728acb53 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sun, 2 Oct 2022 16:50:07 +0200 Subject: [PATCH 5/6] Fix no-std build --- src/query/default_query_dispatcher.rs | 63 ++++++++++++++------------- src/query/time_of_impact/mod.rs | 20 +++++---- 2 files changed, 44 insertions(+), 39 deletions(-) diff --git a/src/query/default_query_dispatcher.rs b/src/query/default_query_dispatcher.rs index c1367723..f4344299 100644 --- a/src/query/default_query_dispatcher.rs +++ b/src/query/default_query_dispatcher.rs @@ -8,7 +8,7 @@ use crate::query::{ contact_manifolds::ContactManifoldsWorkspace, query_dispatcher::PersistentQueryDispatcher, ContactManifold, }; -use crate::shape::{HalfSpace, HeightField, Segment, Shape, ShapeType}; +use crate::shape::{HalfSpace, Segment, Shape, ShapeType}; /// A dispatcher that exposes built-in queries #[derive(Debug, Clone)] @@ -306,38 +306,39 @@ impl QueryDispatcher for DefaultQueryDispatcher { max_toi, stop_at_penetration, )) - } else if let Some(heightfield1) = shape1.as_shape::() { - query::details::time_of_impact_heightfield_shape( - self, - pos12, - local_vel12, - heightfield1, - shape2, - max_toi, - stop_at_penetration, - ) - } else if let Some(heightfield2) = shape1.as_shape::() { - query::details::time_of_impact_shape_heightfield( - self, - pos12, - local_vel12, - shape1, - heightfield2, - max_toi, - stop_at_penetration, - ) - } else if let (Some(s1), Some(s2)) = (shape1.as_support_map(), shape2.as_support_map()) { - Ok(query::details::time_of_impact_support_map_support_map( - pos12, - local_vel12, - s1, - s2, - max_toi, - stop_at_penetration, - )) } else { #[cfg(feature = "std")] - if let Some(c1) = shape1.as_composite_shape() { + if let Some(heightfield1) = shape1.as_heightfield() { + return query::details::time_of_impact_heightfield_shape( + self, + pos12, + local_vel12, + heightfield1, + shape2, + max_toi, + stop_at_penetration, + ); + } else if let Some(heightfield2) = shape1.as_heightfield() { + return query::details::time_of_impact_shape_heightfield( + self, + pos12, + local_vel12, + shape1, + heightfield2, + max_toi, + stop_at_penetration, + ); + } else if let (Some(s1), Some(s2)) = (shape1.as_support_map(), shape2.as_support_map()) + { + return Ok(query::details::time_of_impact_support_map_support_map( + pos12, + local_vel12, + s1, + s2, + max_toi, + stop_at_penetration, + )); + } else if let Some(c1) = shape1.as_composite_shape() { return Ok(query::details::time_of_impact_composite_shape_shape( self, pos12, diff --git a/src/query/time_of_impact/mod.rs b/src/query/time_of_impact/mod.rs index 97ea2b4c..915c0c0d 100644 --- a/src/query/time_of_impact/mod.rs +++ b/src/query/time_of_impact/mod.rs @@ -2,23 +2,27 @@ pub use self::time_of_impact::{time_of_impact, TOIStatus, TOI}; pub use self::time_of_impact_ball_ball::time_of_impact_ball_ball; -#[cfg(feature = "std")] -pub use self::time_of_impact_composite_shape_shape::{ - time_of_impact_composite_shape_shape, time_of_impact_shape_composite_shape, - TOICompositeShapeShapeBestFirstVisitor, -}; pub use self::time_of_impact_halfspace_support_map::{ time_of_impact_halfspace_support_map, time_of_impact_support_map_halfspace, }; -pub use self::time_of_impact_heightfield_shape::{ - time_of_impact_heightfield_shape, time_of_impact_shape_heightfield, +#[cfg(feature = "std")] +pub use self::{ + time_of_impact_composite_shape_shape::{ + time_of_impact_composite_shape_shape, time_of_impact_shape_composite_shape, + TOICompositeShapeShapeBestFirstVisitor, + }, + time_of_impact_heightfield_shape::{ + time_of_impact_heightfield_shape, time_of_impact_shape_heightfield, + }, + time_of_impact_support_map_support_map::time_of_impact_support_map_support_map, }; -pub use self::time_of_impact_support_map_support_map::time_of_impact_support_map_support_map; mod time_of_impact; mod time_of_impact_ball_ball; #[cfg(feature = "std")] mod time_of_impact_composite_shape_shape; mod time_of_impact_halfspace_support_map; +#[cfg(feature = "std")] mod time_of_impact_heightfield_shape; +#[cfg(feature = "std")] mod time_of_impact_support_map_support_map; From 8e106ecdd73c287e9cafc8810152438c506b8146 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sun, 2 Oct 2022 17:07:17 +0200 Subject: [PATCH 6/6] Update examples --- .../examples/time_of_impact_query2d.rs | 3 +++ .../parry2d/tests/geometry/ball_ball_toi.rs | 2 +- .../parry2d/tests/geometry/time_of_impact2.rs | 19 +++++++++++++++++-- .../examples/time_of_impact_query3d.rs | 3 +++ .../parry3d/tests/geometry/ball_ball_toi.rs | 2 +- .../tests/geometry/ball_triangle_toi.rs | 2 +- .../tests/geometry/still_objects_toi.rs | 15 ++++++++++++--- .../parry3d/tests/geometry/time_of_impact3.rs | 3 +++ .../tests/geometry/trimesh_trimesh_toi.rs | 1 + 9 files changed, 42 insertions(+), 8 deletions(-) diff --git a/crates/parry2d/examples/time_of_impact_query2d.rs b/crates/parry2d/examples/time_of_impact_query2d.rs index 086ad6ac..15104ee9 100644 --- a/crates/parry2d/examples/time_of_impact_query2d.rs +++ b/crates/parry2d/examples/time_of_impact_query2d.rs @@ -28,6 +28,7 @@ fn main() { &box_vel1, &cuboid, Real::MAX, + true, ) .unwrap(); let toi_will_touch = query::time_of_impact( @@ -38,6 +39,7 @@ fn main() { &box_vel2, &cuboid, Real::MAX, + true, ) .unwrap(); let toi_wont_touch = query::time_of_impact( @@ -48,6 +50,7 @@ fn main() { &box_vel1, &cuboid, Real::MAX, + true, ) .unwrap(); diff --git a/crates/parry2d/tests/geometry/ball_ball_toi.rs b/crates/parry2d/tests/geometry/ball_ball_toi.rs index 3d15f797..8ddf5507 100644 --- a/crates/parry2d/tests/geometry/ball_ball_toi.rs +++ b/crates/parry2d/tests/geometry/ball_ball_toi.rs @@ -13,7 +13,7 @@ fn test_ball_ball_toi() { let v1 = Vector2::new(0.0, 10.0); let v2 = Vector2::zeros(); - let cast = query::time_of_impact(&m1, &v1, &b, &m2, &v2, &b, Real::MAX).unwrap(); + let cast = query::time_of_impact(&m1, &v1, &b, &m2, &v2, &b, Real::MAX, true).unwrap(); assert_eq!(cast.unwrap().toi, 0.9); } diff --git a/crates/parry2d/tests/geometry/time_of_impact2.rs b/crates/parry2d/tests/geometry/time_of_impact2.rs index 1e835d2f..95af766f 100644 --- a/crates/parry2d/tests/geometry/time_of_impact2.rs +++ b/crates/parry2d/tests/geometry/time_of_impact2.rs @@ -27,6 +27,7 @@ fn ball_cuboid_toi() { &cuboid_vel1, &cuboid, Real::MAX, + true, ) .unwrap(); let toi_will_touch = query::time_of_impact( @@ -37,6 +38,7 @@ fn ball_cuboid_toi() { &cuboid_vel2, &cuboid, Real::MAX, + true, ) .unwrap(); let toi_wont_touch = query::time_of_impact( @@ -47,6 +49,7 @@ fn ball_cuboid_toi() { &cuboid_vel1, &cuboid, Real::MAX, + true, ) .unwrap(); @@ -69,8 +72,17 @@ fn cuboid_cuboid_toi_issue_214() { let vel1 = Vector2::new(1.0, 0.0); let vel2 = Vector2::new(0.0, 0.0); - let toi = - query::time_of_impact(&pos1, &vel1, &shape1, &pos2, &vel2, &shape2, Real::MAX).unwrap(); + let toi = query::time_of_impact( + &pos1, + &vel1, + &shape1, + &pos2, + &vel2, + &shape2, + Real::MAX, + true, + ) + .unwrap(); assert!(toi.is_some()); } @@ -100,6 +112,7 @@ fn time_of_impact_should_return_toi_for_ball_and_rotated_polyline() { &polyline_velocity, &polyline, 1.0, + true, ) .unwrap(); @@ -132,6 +145,7 @@ fn time_of_impact_should_return_toi_for_ball_and_rotated_segment() { &segment_velocity, &segment, 1.0, + true, ) .unwrap(); @@ -164,6 +178,7 @@ fn time_of_impact_should_return_toi_for_rotated_segment_and_ball() { &ball_velocity, &ball, 1.0, + true, ) .unwrap(); diff --git a/crates/parry3d/examples/time_of_impact_query3d.rs b/crates/parry3d/examples/time_of_impact_query3d.rs index 66c95f0e..2cdfa3d5 100644 --- a/crates/parry3d/examples/time_of_impact_query3d.rs +++ b/crates/parry3d/examples/time_of_impact_query3d.rs @@ -28,6 +28,7 @@ fn main() { &cuboid_vel1, &cuboid, Real::MAX, + true, ) .unwrap(); let toi_will_touch = query::time_of_impact( @@ -38,6 +39,7 @@ fn main() { &cuboid_vel2, &cuboid, Real::MAX, + true, ) .unwrap(); let toi_wont_touch = query::time_of_impact( @@ -48,6 +50,7 @@ fn main() { &cuboid_vel1, &cuboid, Real::MAX, + true, ) .unwrap(); diff --git a/crates/parry3d/tests/geometry/ball_ball_toi.rs b/crates/parry3d/tests/geometry/ball_ball_toi.rs index 7e4cbab7..46bc35e1 100644 --- a/crates/parry3d/tests/geometry/ball_ball_toi.rs +++ b/crates/parry3d/tests/geometry/ball_ball_toi.rs @@ -13,7 +13,7 @@ fn test_ball_ball_toi() { let vel1 = Vector3::new(0.0, 10.0, 0.0); let vel2 = Vector3::zeros(); - let cast = query::time_of_impact(&m1, &vel1, &b, &m2, &vel2, &b, Real::MAX).unwrap(); + let cast = query::time_of_impact(&m1, &vel1, &b, &m2, &vel2, &b, Real::MAX, true).unwrap(); assert_eq!(cast.unwrap().toi, 0.9); } diff --git a/crates/parry3d/tests/geometry/ball_triangle_toi.rs b/crates/parry3d/tests/geometry/ball_triangle_toi.rs index c19fefad..1e83bfbf 100644 --- a/crates/parry3d/tests/geometry/ball_triangle_toi.rs +++ b/crates/parry3d/tests/geometry/ball_triangle_toi.rs @@ -18,7 +18,7 @@ fn ball_triangle_toi_infinite_loop_issue() { let vel1 = Vector3::new(0.0, 0.000000000000000000000000000000000000000006925, 0.0); let vel2 = Vector3::zeros(); - let cast = query::time_of_impact(&m1, &vel1, &b, &m2, &vel2, &t, std::f32::MAX).unwrap(); + let cast = query::time_of_impact(&m1, &vel1, &b, &m2, &vel2, &t, std::f32::MAX, true).unwrap(); println!("TOI: {:?}", cast); assert!(cast.is_none()); // The provided velocity is too small. diff --git a/crates/parry3d/tests/geometry/still_objects_toi.rs b/crates/parry3d/tests/geometry/still_objects_toi.rs index 1f436d08..dff2d1c8 100644 --- a/crates/parry3d/tests/geometry/still_objects_toi.rs +++ b/crates/parry3d/tests/geometry/still_objects_toi.rs @@ -25,9 +25,18 @@ fn collide(v_y: f32) -> Option { let vel2 = Vector3::zeros(); let cuboid = Cuboid::new(Vector3::new(0.5, 0.5, 0.5)); - time_of_impact(&pos1, &vel1, &cuboid, &pos2, &vel2, &cuboid, std::f32::MAX) - .unwrap() - .map(|toi| toi.toi) + time_of_impact( + &pos1, + &vel1, + &cuboid, + &pos2, + &vel2, + &cuboid, + std::f32::MAX, + true, + ) + .unwrap() + .map(|toi| toi.toi) } #[test] diff --git a/crates/parry3d/tests/geometry/time_of_impact3.rs b/crates/parry3d/tests/geometry/time_of_impact3.rs index 1d11f185..8e014d97 100644 --- a/crates/parry3d/tests/geometry/time_of_impact3.rs +++ b/crates/parry3d/tests/geometry/time_of_impact3.rs @@ -27,6 +27,7 @@ fn ball_cuboid_toi() { &cuboid_vel1, &cuboid, Real::MAX, + true, ) .unwrap() .map(|toi| toi.toi); @@ -38,6 +39,7 @@ fn ball_cuboid_toi() { &cuboid_vel2, &cuboid, Real::MAX, + true, ) .unwrap() .map(|toi| toi.toi); @@ -49,6 +51,7 @@ fn ball_cuboid_toi() { &cuboid_vel1, &cuboid, Real::MAX, + true, ) .unwrap() .map(|toi| toi.toi); diff --git a/crates/parry3d/tests/geometry/trimesh_trimesh_toi.rs b/crates/parry3d/tests/geometry/trimesh_trimesh_toi.rs index 96ad3710..2c94c93d 100644 --- a/crates/parry3d/tests/geometry/trimesh_trimesh_toi.rs +++ b/crates/parry3d/tests/geometry/trimesh_trimesh_toi.rs @@ -41,6 +41,7 @@ fn do_toi_test() -> Option { &vel_two, &shape_two, Real::MAX, + true, ) .unwrap() .map(|toi| toi.toi)