From 3385f8a4f20dfb3f8593ecfa2cd87b14a4660952 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 01/17] 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 d3400f6dda97552289ae9b558c6b4efee2bd3404 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 02/17] 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 003d8799cd510ae8ed850a3e5daa496eba201331 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 03/17] 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 e227f09a139c3f270e383e07085f6141d44da7cc 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 04/17] 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 d0424f14d6eb9be58905f6766a021d409068c2e4 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 05/17] 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 b44105786635bf7d0b6ada9139f0140312f3d80c 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 06/17] 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) From 5f8bb6700d063fcb6b5eec23fab556d3a5fdd198 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Thu, 23 Jun 2022 17:35:08 +0200 Subject: [PATCH 07/17] Allow simd for the f64 version --- src/lib.rs | 40 ++++++++------------------------- src/partitioning/mod.rs | 4 +++- src/partitioning/qbvh/mod.rs | 2 +- src/partitioning/qbvh/qbvh.rs | 2 +- src/partitioning/qbvh/update.rs | 2 +- 5 files changed, 15 insertions(+), 35 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index ad398f99..30f7ad43 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -28,10 +28,6 @@ std::compile_error!("The `simd-is-enabled` feature should not be enabled explici std::compile_error!( "SIMD cannot be enabled when the `enhanced-determinism` feature is also enabled." ); -#[cfg(all(feature = "simd-is-enabled", feature = "f64"))] -std::compile_error!( - "Explicit SIMD optimization are not yet supported when the f64 feature is enabled." -); macro_rules! array( ($callback: expr; SIMD_WIDTH) => { @@ -252,36 +248,18 @@ mod simd { #[cfg(feature = "simd-is-enabled")] mod simd { - #[allow(unused_imports)] - #[cfg(feature = "simd-nightly")] - use simba::simd::{f32x16, f32x4, f32x8, m32x16, m32x4, m32x8, u8x16, u8x4, u8x8}; - #[cfg(feature = "simd-stable")] - use simba::simd::{WideBoolF32x4, WideF32x4}; + #[cfg(all(feature = "simd-nightly", feature = "f32"))] + pub use simba::simd::{f32x4 as SimdReal, m32x4 as SimdBool}; + #[cfg(all(feature = "simd-stable", feature = "f32"))] + pub use simba::simd::{WideBoolF32x4 as SimdBool, WideF32x4 as SimdReal}; + + #[cfg(all(feature = "simd-nightly", feature = "f64"))] + pub use simba::simd::{f64x4 as SimdReal, m64x4 as SimdBool}; + #[cfg(all(feature = "simd-stable", feature = "f64"))] + pub use simba::simd::{WideBoolF64x4 as SimdBool, WideF64x4 as SimdReal}; /// The number of lanes of a SIMD number. pub const SIMD_WIDTH: usize = 4; /// SIMD_WIDTH - 1 pub const SIMD_LAST_INDEX: usize = 3; - #[cfg(not(feature = "simd-nightly"))] - /// A SIMD float with SIMD_WIDTH lanes. - pub type SimdReal = WideF32x4; - #[cfg(not(feature = "simd-nightly"))] - /// A SIMD bool with SIMD_WIDTH lanes. - pub type SimdBool = WideBoolF32x4; - #[cfg(feature = "simd-nightly")] - /// A SIMD float with SIMD_WIDTH lanes. - pub type SimdReal = f32x4; - #[cfg(feature = "simd-nightly")] - /// A bool float with SIMD_WIDTH lanes. - pub type SimdBool = m32x4; - - // pub const SIMD_WIDTH: usize = 8; - // pub const SIMD_LAST_INDEX: usize = 7; - // pub type SimdReal = f32x8; - // pub type SimdBool = m32x8; - - // pub const SIMD_WIDTH: usize = 16; - // pub const SIMD_LAST_INDEX: usize = 15; - // pub type SimdReal = f32x16; - // pub type SimdBool = m32x16; } diff --git a/src/partitioning/mod.rs b/src/partitioning/mod.rs index 9fc083ab..10ed2c30 100644 --- a/src/partitioning/mod.rs +++ b/src/partitioning/mod.rs @@ -1,6 +1,8 @@ //! Spatial partitioning tools. -pub use self::qbvh::{IndexedData, QBVHDataGenerator, QbvhNonOverlappingDataSplitter, QBVH}; +pub use self::qbvh::{ + IndexedData, NodeIndex, QBVHDataGenerator, QbvhNonOverlappingDataSplitter, QBVH, +}; pub use self::visitor::{ SimdBestFirstVisitStatus, SimdBestFirstVisitor, SimdSimultaneousVisitStatus, SimdSimultaneousVisitor, SimdVisitStatus, SimdVisitor, diff --git a/src/partitioning/qbvh/mod.rs b/src/partitioning/qbvh/mod.rs index 6b470bd0..0e06432d 100644 --- a/src/partitioning/qbvh/mod.rs +++ b/src/partitioning/qbvh/mod.rs @@ -1,5 +1,5 @@ pub use self::build::{BuilderProxies, QBVHDataGenerator, QbvhNonOverlappingDataSplitter}; -pub use self::qbvh::{IndexedData, QBVH}; +pub use self::qbvh::{IndexedData, NodeIndex, QBVH}; pub(self) use self::qbvh::*; diff --git a/src/partitioning/qbvh/qbvh.rs b/src/partitioning/qbvh/qbvh.rs index 7dff9cfc..eb9f2997 100644 --- a/src/partitioning/qbvh/qbvh.rs +++ b/src/partitioning/qbvh/qbvh.rs @@ -74,7 +74,7 @@ pub struct QBVHNode { pub children: [u32; 4], /// The index of the node parent to the 4 nodes represented by `self`. pub parent: NodeIndex, - /// Are the four nodes represneted by `self` leaves of the `QBVH`? + /// Are the four nodes represented by `self` leaves of the `QBVH`? pub leaf: bool, // TODO: pack this with the NodexIndex.lane? pub(super) dirty: bool, // TODO: move this to a separate bitvec? } diff --git a/src/partitioning/qbvh/update.rs b/src/partitioning/qbvh/update.rs index 564cf0d7..804f8abc 100644 --- a/src/partitioning/qbvh/update.rs +++ b/src/partitioning/qbvh/update.rs @@ -180,7 +180,7 @@ impl QBVH { let dilation_factor = SimdReal::splat(dilation_factor); while let Some(id) = self.dirty_nodes.pop_front() { - // NOTE: this will data the case where we reach the root of the tree. + // NOTE: this will deal with the case where we reach the root of the tree. if let Some(node) = self.nodes.get(id as usize) { // Compute the new aabb. let mut new_aabbs = [AABB::new_invalid(); SIMD_WIDTH]; From 14d4641bdb77d45f865cf3033ec235a0b9e80a8c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Tue, 12 Jul 2022 15:34:04 +0200 Subject: [PATCH 08/17] Add support for rkyv when it is straightforward to do so. --- crates/parry2d-f64/Cargo.toml | 5 ++- crates/parry2d/Cargo.toml | 9 ++++-- crates/parry3d-f64/Cargo.toml | 3 ++ crates/parry3d/Cargo.toml | 4 +++ src/bounding_volume/aabb.rs | 4 +++ src/bounding_volume/bounding_sphere.rs | 4 +++ src/bounding_volume/simd_aabb.rs | 4 +++ src/mass_properties/mass_properties.rs | 4 +++ src/partitioning/qbvh/qbvh.rs | 20 ++++++++++-- src/partitioning/qbvh/update.rs | 8 ++--- src/query/closest_points/closest_points.rs | 4 +++ src/query/contact/contact.rs | 4 +++ .../contact_manifolds/contact_manifold.rs | 4 +++ ...nifolds_composite_shape_composite_shape.rs | 4 +++ ...contact_manifolds_composite_shape_shape.rs | 4 +++ ...t_manifolds_heightfield_composite_shape.rs | 4 +++ .../contact_manifolds_heightfield_shape.rs | 4 +++ .../contact_manifolds_trimesh_shape.rs | 4 +++ src/query/point/point_query.rs | 4 +++ src/query/ray/ray.rs | 8 +++++ src/shape/ball.rs | 4 +++ src/shape/capsule.rs | 6 +++- src/shape/cone.rs | 4 +++ src/shape/convex_polygon.rs | 4 +++ src/shape/convex_polyhedron.rs | 20 ++++++++++++ src/shape/cuboid.rs | 4 +++ src/shape/cylinder.rs | 4 +++ src/shape/feature_id.rs | 4 +++ src/shape/half_space.rs | 4 +++ src/shape/heightfield2.rs | 4 +++ src/shape/heightfield3.rs | 8 +++++ src/shape/polygon.rs | 4 +++ src/shape/polyline.rs | 4 +++ src/shape/round_shape.rs | 4 +++ src/shape/segment.rs | 4 +++ src/shape/tetrahedron.rs | 4 +++ src/shape/triangle.rs | 4 +++ src/shape/trimesh.rs | 32 +++++++++++++++++++ src/utils/sdp_matrix.rs | 8 +++++ src/utils/sorted_pair.rs | 31 ++++++++++++++++++ 40 files changed, 259 insertions(+), 11 deletions(-) diff --git a/crates/parry2d-f64/Cargo.toml b/crates/parry2d-f64/Cargo.toml index 1457a2c4..dd7d5fd1 100644 --- a/crates/parry2d-f64/Cargo.toml +++ b/crates/parry2d-f64/Cargo.toml @@ -23,6 +23,7 @@ std = [ "nalgebra/std", "slab", "rustc-hash", "simba/std", "arrayvec/std", " dim2 = [ ] f64 = [ ] serde-serialize = [ "serde", "nalgebra/serde-serialize", "arrayvec/serde" ] +rkyv-serialize = [ "rkyv", "nalgebra/rkyv-serialize", "simba/rkyv-serialize" ] simd-stable = [ "simba/wide", "simd-is-enabled" ] simd-nightly = [ "simba/packed_simd", "simd-is-enabled" ] enhanced-determinism = [ "simba/libm_force", "indexmap" ] @@ -48,12 +49,14 @@ arrayvec = { version = "0.7", default-features = false } simba = { version = "0.7", default-features = false } nalgebra = { version = "0.31", default-features = false, features = [ "libm" ] } approx = { version = "0.5", default-features = false } -serde = { version = "1.0", optional = true, features = ["derive"]} +serde = { version = "1.0", optional = true, features = ["derive"] } +rkyv = { version = "0.7", optional = true } num-derive = "0.3" indexmap = { version = "1", features = [ "serde-1" ], optional = true } rustc-hash = { version = "1", optional = true } cust_core = { version = "0.1", optional = true } spade = { version = "2", optional = true } # Make this optional? +rayon = { version = "1", optional = true } [target.'cfg(not(target_os = "cuda"))'.dependencies] cust = { version = "0.3", optional = true } diff --git a/crates/parry2d/Cargo.toml b/crates/parry2d/Cargo.toml index c76a251d..c096c154 100644 --- a/crates/parry2d/Cargo.toml +++ b/crates/parry2d/Cargo.toml @@ -23,10 +23,11 @@ std = [ "nalgebra/std", "slab", "rustc-hash", "simba/std", "arrayvec/std", " dim2 = [ ] f32 = [ ] serde-serialize = [ "serde", "nalgebra/serde-serialize", "arrayvec/serde" ] +rkyv-serialize = [ "rkyv", "nalgebra/rkyv-serialize", "simba/rkyv-serialize" ] simd-stable = [ "simba/wide", "simd-is-enabled" ] simd-nightly = [ "simba/packed_simd", "simd-is-enabled" ] enhanced-determinism = [ "simba/libm_force", "indexmap" ] -cuda = [ "cust_core", "cust", "nalgebra/cuda" ] +cuda = [ "cust_core", "cust", "nalgebra/cuda" ] # Do not enable this feature directly. It is automatically # enabled with the "simd-stable" or "simd-nightly" feature. @@ -48,12 +49,14 @@ arrayvec = { version = "0.7", default-features = false } simba = { version = "0.7", default-features = false } nalgebra = { version = "0.31", default-features = false, features = [ "libm" ] } approx = { version = "0.5", default-features = false } -serde = { version = "1.0", optional = true, features = ["derive"]} +serde = { version = "1.0", optional = true, features = ["derive"] } +rkyv = { version = "0.7", optional = true } num-derive = "0.3" indexmap = { version = "1", features = [ "serde-1" ], optional = true } rustc-hash = { version = "1", optional = true } cust_core = { version = "0.1", optional = true } -spade = { version = "2", optional = true } # Make this optional? +spade = { version = "2", optional = true } +rayon = { version = "1", optional = true } [target.'cfg(not(target_os = "cuda"))'.dependencies] cust = { version = "0.3", optional = true } diff --git a/crates/parry3d-f64/Cargo.toml b/crates/parry3d-f64/Cargo.toml index e57b9f0d..a843b75d 100644 --- a/crates/parry3d-f64/Cargo.toml +++ b/crates/parry3d-f64/Cargo.toml @@ -23,6 +23,7 @@ std = [ "nalgebra/std", "slab", "rustc-hash", "simba/std", "arrayvec/std", " dim3 = [ ] f64 = [ ] serde-serialize = [ "serde", "nalgebra/serde-serialize" ] +rkyv-serialize = [ "rkyv", "nalgebra/rkyv-serialize", "simba/rkyv-serialize" ] simd-stable = [ "simba/wide", "simd-is-enabled" ] simd-nightly = [ "simba/packed_simd", "simd-is-enabled" ] enhanced-determinism = [ "simba/libm_force", "indexmap" ] @@ -49,11 +50,13 @@ simba = { version = "0.7", default-features = false } nalgebra = { version = "0.31", default-features = false, features = [ "libm" ] } approx = { version = "0.5", default-features = false } serde = { version = "1.0", optional = true, features = ["derive", "rc"]} +rkyv = { version = "0.7", optional = true } num-derive = "0.3" indexmap = { version = "1", features = [ "serde-1" ], optional = true } rustc-hash = { version = "1", optional = true } cust_core = { version = "0.1", optional = true } spade = { version = "2", optional = true } # Make this optional? +rayon = { version = "1", optional = true } [target.'cfg(not(target_os = "cuda"))'.dependencies] cust = { version = "0.3", optional = true } diff --git a/crates/parry3d/Cargo.toml b/crates/parry3d/Cargo.toml index 32d69184..46b30df4 100644 --- a/crates/parry3d/Cargo.toml +++ b/crates/parry3d/Cargo.toml @@ -23,6 +23,8 @@ std = [ "nalgebra/std", "slab", "rustc-hash", "simba/std", "arrayvec/std", " dim3 = [ ] f32 = [ ] serde-serialize = [ "serde", "nalgebra/serde-serialize" ] +rkyv-serialize = [ "rkyv", "nalgebra/rkyv-serialize", "simba/rkyv-serialize" ] + simd-stable = [ "simba/wide", "simd-is-enabled" ] simd-nightly = [ "simba/packed_simd", "simd-is-enabled" ] enhanced-determinism = [ "simba/libm_force", "indexmap" ] @@ -49,11 +51,13 @@ simba = { version = "0.7", default-features = false } nalgebra = { version = "0.31", default-features = false, features = [ "libm" ] } approx = { version = "0.5", default-features = false } serde = { version = "1.0", optional = true, features = ["derive", "rc"]} +rkyv = { version = "0.7", optional = true } num-derive = "0.3" indexmap = { version = "1", features = [ "serde-1" ], optional = true } rustc-hash = { version = "1", optional = true } cust_core = { version = "0.1", optional = true } spade = { version = "2", optional = true } # Make this optional? +rayon = { version = "1", optional = true } [target.'cfg(not(target_os = "cuda"))'.dependencies] cust = { version = "0.3", optional = true } diff --git a/src/bounding_volume/aabb.rs b/src/bounding_volume/aabb.rs index 8286d199..617a8c83 100644 --- a/src/bounding_volume/aabb.rs +++ b/src/bounding_volume/aabb.rs @@ -13,6 +13,10 @@ use na::ComplexField; // for .abs() /// An Axis Aligned Bounding Box. #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +#[cfg_attr( + feature = "rkyv", + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) +)] #[cfg_attr(feature = "cuda", derive(cust_core::DeviceCopy))] #[derive(Debug, PartialEq, Copy, Clone)] pub struct AABB { diff --git a/src/bounding_volume/bounding_sphere.rs b/src/bounding_volume/bounding_sphere.rs index d644d8cd..93640430 100644 --- a/src/bounding_volume/bounding_sphere.rs +++ b/src/bounding_volume/bounding_sphere.rs @@ -7,6 +7,10 @@ use num::Zero; /// A Bounding Sphere. #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +#[cfg_attr( + feature = "rkyv", + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) +)] #[derive(Debug, PartialEq, Copy, Clone)] pub struct BoundingSphere { pub center: Point, diff --git a/src/bounding_volume/simd_aabb.rs b/src/bounding_volume/simd_aabb.rs index b31641b6..76e0ecff 100644 --- a/src/bounding_volume/simd_aabb.rs +++ b/src/bounding_volume/simd_aabb.rs @@ -7,6 +7,10 @@ use simba::simd::{SimdPartialOrd, SimdValue}; /// Four AABB represented as a single SoA AABB with SIMD components. #[derive(Debug, Copy, Clone)] +#[cfg_attr( + feature = "rkyv", + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) +)] pub struct SimdAABB { /// The min coordinates of the AABBs. pub mins: Point, diff --git a/src/mass_properties/mass_properties.rs b/src/mass_properties/mass_properties.rs index 2093314a..ae1c70ae 100644 --- a/src/mass_properties/mass_properties.rs +++ b/src/mass_properties/mass_properties.rs @@ -10,6 +10,10 @@ const EPSILON: Real = f32::EPSILON as Real; #[derive(Copy, Clone, Debug, Default, PartialEq)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[cfg_attr( + feature = "rkyv", + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) +)] /// The local mass properties of a rigid-body. pub struct MassProperties { /// The center of mass of a rigid-body expressed in its local-space. diff --git a/src/partitioning/qbvh/qbvh.rs b/src/partitioning/qbvh/qbvh.rs index eb9f2997..e1e1c19e 100644 --- a/src/partitioning/qbvh/qbvh.rs +++ b/src/partitioning/qbvh/qbvh.rs @@ -43,6 +43,10 @@ impl IndexedData for u64 { /// The index of a node part of a QBVH. #[derive(Copy, Clone, Debug, PartialEq, Eq)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[cfg_attr( + feature = "rkyv", + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) +)] pub struct NodeIndex { pub(super) index: u32, // Index of the addressed node in the `nodes` array. pub(super) lane: u8, // SIMD lane of the addressed node. @@ -66,6 +70,10 @@ impl NodeIndex { /// This groups four nodes of the QBVH. #[derive(Copy, Clone, Debug)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[cfg_attr( + feature = "rkyv", + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) +)] pub struct QBVHNode { /// The AABBs of the qbvh nodes represented by this node. pub simd_aabb: SimdAABB, @@ -81,6 +89,10 @@ pub struct QBVHNode { #[derive(Copy, Clone, Debug, PartialEq, Eq)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[cfg_attr( + feature = "rkyv", + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) +)] pub struct QBVHProxy { pub node: NodeIndex, pub data: T, // The collider data. TODO: only set the collider generation here? @@ -109,11 +121,15 @@ impl QBVHProxy { /// /// This is a bounding-volume-hierarchy where each node has either four children or none. #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[cfg_attr( + feature = "rkyv", + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) +)] #[derive(Clone, Debug)] pub struct QBVH { pub(super) root_aabb: AABB, pub(super) nodes: Vec, - pub(super) dirty_nodes: VecDeque, + pub(super) dirty_nodes: Vec, pub(super) proxies: Vec>, } @@ -123,7 +139,7 @@ impl QBVH { QBVH { root_aabb: AABB::new_invalid(), nodes: Vec::new(), - dirty_nodes: VecDeque::new(), + dirty_nodes: Vec::new(), proxies: Vec::new(), } } diff --git a/src/partitioning/qbvh/update.rs b/src/partitioning/qbvh/update.rs index 804f8abc..f5f37d57 100644 --- a/src/partitioning/qbvh/update.rs +++ b/src/partitioning/qbvh/update.rs @@ -167,7 +167,7 @@ impl QBVH { let node = &mut self.nodes[node_id as usize]; if !node.dirty { node.dirty = true; - self.dirty_nodes.push_back(node_id); + self.dirty_nodes.push(node_id); } } @@ -179,8 +179,8 @@ impl QBVH { // Loop on the dirty leaves. let dilation_factor = SimdReal::splat(dilation_factor); - while let Some(id) = self.dirty_nodes.pop_front() { - // NOTE: this will deal with the case where we reach the root of the tree. + while let Some(id) = self.dirty_nodes.pop() { + // NOTE: this will data the case where we reach the root of the tree. if let Some(node) = self.nodes.get(id as usize) { // Compute the new aabb. let mut new_aabbs = [AABB::new_invalid(); SIMD_WIDTH]; @@ -203,7 +203,7 @@ impl QBVH { if !node.simd_aabb.contains(&new_simd_aabb).all() { node.simd_aabb = new_simd_aabb; node.simd_aabb.dilate_by_factor(dilation_factor); - self.dirty_nodes.push_back(node.parent.index); + self.dirty_nodes.push(node.parent.index); } node.dirty = false; } diff --git a/src/query/closest_points/closest_points.rs b/src/query/closest_points/closest_points.rs index cab6bce6..991d360a 100644 --- a/src/query/closest_points/closest_points.rs +++ b/src/query/closest_points/closest_points.rs @@ -5,6 +5,10 @@ use std::mem; /// Closest points information. #[derive(Debug, PartialEq, Clone, Copy)] #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +#[cfg_attr( + feature = "rkyv", + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) +)] pub enum ClosestPoints { /// The two objects are intersecting. Intersecting, diff --git a/src/query/contact/contact.rs b/src/query/contact/contact.rs index 22f6637b..a6f7821f 100644 --- a/src/query/contact/contact.rs +++ b/src/query/contact/contact.rs @@ -5,6 +5,10 @@ use std::mem; /// Geometric description of a contact. #[derive(Debug, PartialEq, Copy, Clone)] #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +#[cfg_attr( + feature = "rkyv", + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) +)] pub struct Contact { /// Position of the contact on the first object. pub point1: Point, diff --git a/src/query/contact_manifolds/contact_manifold.rs b/src/query/contact_manifolds/contact_manifold.rs index e206c9ee..d3d30c5e 100644 --- a/src/query/contact_manifolds/contact_manifold.rs +++ b/src/query/contact_manifolds/contact_manifold.rs @@ -2,6 +2,10 @@ use crate::math::{Isometry, Point, Real, Vector}; #[derive(Copy, Clone, Debug)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[cfg_attr( + feature = "rkyv", + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) +)] /// A single contact between two shape. pub struct TrackedContact { /// The contact point in the local-space of the first shape. diff --git a/src/query/contact_manifolds/contact_manifolds_composite_shape_composite_shape.rs b/src/query/contact_manifolds/contact_manifolds_composite_shape_composite_shape.rs index f31b7dc1..d35dfcd6 100644 --- a/src/query/contact_manifolds/contact_manifolds_composite_shape_composite_shape.rs +++ b/src/query/contact_manifolds/contact_manifolds_composite_shape_composite_shape.rs @@ -12,6 +12,10 @@ use crate::utils::hashmap::{Entry, HashMap}; use crate::utils::IsometryOpt; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[cfg_attr( + feature = "rkyv", + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) +)] #[derive(Clone)] struct SubDetector { manifold_id: usize, diff --git a/src/query/contact_manifolds/contact_manifolds_composite_shape_shape.rs b/src/query/contact_manifolds/contact_manifolds_composite_shape_shape.rs index 7c2dc195..ddda2685 100644 --- a/src/query/contact_manifolds/contact_manifolds_composite_shape_shape.rs +++ b/src/query/contact_manifolds/contact_manifolds_composite_shape_shape.rs @@ -12,6 +12,10 @@ use crate::utils::hashmap::{Entry, HashMap}; use crate::utils::IsometryOpt; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[cfg_attr( + feature = "rkyv", + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) +)] #[derive(Clone)] struct SubDetector { manifold_id: usize, diff --git a/src/query/contact_manifolds/contact_manifolds_heightfield_composite_shape.rs b/src/query/contact_manifolds/contact_manifolds_heightfield_composite_shape.rs index c2fcecb3..102c777d 100644 --- a/src/query/contact_manifolds/contact_manifolds_heightfield_composite_shape.rs +++ b/src/query/contact_manifolds/contact_manifolds_heightfield_composite_shape.rs @@ -14,6 +14,10 @@ use crate::utils::hashmap::{Entry, HashMap}; use crate::utils::IsometryOpt; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[cfg_attr( + feature = "rkyv", + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) +)] #[derive(Clone)] struct SubDetector { manifold_id: usize, diff --git a/src/query/contact_manifolds/contact_manifolds_heightfield_shape.rs b/src/query/contact_manifolds/contact_manifolds_heightfield_shape.rs index c7020045..15e9d9a9 100644 --- a/src/query/contact_manifolds/contact_manifolds_heightfield_shape.rs +++ b/src/query/contact_manifolds/contact_manifolds_heightfield_shape.rs @@ -12,6 +12,10 @@ use crate::shape::{HeightField, Shape}; use crate::utils::hashmap::{Entry, HashMap}; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[cfg_attr( + feature = "rkyv", + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) +)] #[derive(Clone)] struct SubDetector { manifold_id: usize, diff --git a/src/query/contact_manifolds/contact_manifolds_trimesh_shape.rs b/src/query/contact_manifolds/contact_manifolds_trimesh_shape.rs index 1c0f560e..2a00837a 100644 --- a/src/query/contact_manifolds/contact_manifolds_trimesh_shape.rs +++ b/src/query/contact_manifolds/contact_manifolds_trimesh_shape.rs @@ -9,6 +9,10 @@ use crate::query::ContactManifold; use crate::shape::{Shape, TriMesh}; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[cfg_attr( + feature = "rkyv", + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) +)] #[derive(Clone)] pub struct TriMeshShapeContactManifoldsWorkspace { interferences: Vec, diff --git a/src/query/point/point_query.rs b/src/query/point/point_query.rs index f1e37d1b..6bc5e4c6 100644 --- a/src/query/point/point_query.rs +++ b/src/query/point/point_query.rs @@ -5,6 +5,10 @@ use na; /// Description of the projection of a point on a shape. #[derive(Copy, Clone, Debug)] #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +#[cfg_attr( + feature = "rkyv", + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) +)] pub struct PointProjection { /// Whether or not the point to project was inside of the shape. pub is_inside: bool, diff --git a/src/query/ray/ray.rs b/src/query/ray/ray.rs index 23ed9427..b28c985d 100644 --- a/src/query/ray/ray.rs +++ b/src/query/ray/ray.rs @@ -6,6 +6,10 @@ use crate::shape::FeatureId; /// A Ray. #[derive(Debug, Clone, Copy)] #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +#[cfg_attr( + feature = "rkyv", + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) +)] #[cfg_attr(feature = "cuda", derive(cust_core::DeviceCopy))] #[repr(C)] pub struct Ray { @@ -54,6 +58,10 @@ impl Ray { /// Structure containing the result of a successful ray cast. #[derive(Copy, Clone, Debug)] #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +#[cfg_attr( + feature = "rkyv", + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) +)] pub struct RayIntersection { /// The time of impact of the ray with the object. The exact contact point can be computed /// with: `ray.point_at(toi)` or equivalently `origin + dir * toi` where `origin` is the origin of the ray; diff --git a/src/shape/ball.rs b/src/shape/ball.rs index 47897d1c..3f5342f8 100644 --- a/src/shape/ball.rs +++ b/src/shape/ball.rs @@ -7,6 +7,10 @@ use crate::shape::SupportMap; /// A Ball shape. #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +// #[cfg_attr( +// feature = "rkyv", +// derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) +// )] #[cfg_attr(feature = "cuda", derive(cust_core::DeviceCopy))] #[derive(PartialEq, Debug, Copy, Clone)] #[repr(C)] diff --git a/src/shape/capsule.rs b/src/shape/capsule.rs index d01e142d..ed4793df 100644 --- a/src/shape/capsule.rs +++ b/src/shape/capsule.rs @@ -6,7 +6,11 @@ use na::Unit; use either::Either; #[derive(Copy, Clone, Debug)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +#[cfg_attr( + feature = "rkyv", + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) +)] #[cfg_attr(feature = "cuda", derive(cust_core::DeviceCopy))] #[repr(C)] /// A capsule shape defined as a round segment. diff --git a/src/shape/cone.rs b/src/shape/cone.rs index 2abc4438..7c1b76cb 100644 --- a/src/shape/cone.rs +++ b/src/shape/cone.rs @@ -13,6 +13,10 @@ use na::RealField; // for .copysign() /// Cone shape with its principal axis aligned with the `y` axis. #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +#[cfg_attr( + feature = "rkyv", + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) +)] #[cfg_attr(feature = "cuda", derive(cust_core::DeviceCopy))] #[derive(PartialEq, Debug, Copy, Clone)] #[repr(C)] diff --git a/src/shape/convex_polygon.rs b/src/shape/convex_polygon.rs index 3c52678c..915089f1 100644 --- a/src/shape/convex_polygon.rs +++ b/src/shape/convex_polygon.rs @@ -5,6 +5,10 @@ use na::{self, ComplexField, RealField, Unit}; /// A 2D convex polygon. #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +#[cfg_attr( + feature = "rkyv", + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) +)] #[derive(Clone, Debug)] pub struct ConvexPolygon { points: Vec>, diff --git a/src/shape/convex_polyhedron.rs b/src/shape/convex_polyhedron.rs index 1ebc7fd9..f64a3cb0 100644 --- a/src/shape/convex_polyhedron.rs +++ b/src/shape/convex_polyhedron.rs @@ -10,6 +10,10 @@ use std::f64; use na::ComplexField; // for .abs() #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +#[cfg_attr( + feature = "rkyv", + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) +)] #[derive(PartialEq, Debug, Copy, Clone)] pub struct Vertex { pub first_adj_face_or_edge: u32, @@ -17,6 +21,10 @@ pub struct Vertex { } #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +#[cfg_attr( + feature = "rkyv", + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) +)] #[derive(PartialEq, Debug, Copy, Clone)] pub struct Edge { pub vertices: Point2, @@ -36,6 +44,10 @@ impl Edge { } #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +#[cfg_attr( + feature = "rkyv", + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) +)] #[derive(PartialEq, Debug, Copy, Clone)] pub struct Face { pub first_vertex_or_edge: u32, @@ -44,6 +56,10 @@ pub struct Face { } #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +#[cfg_attr( + feature = "rkyv", + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) +)] #[derive(PartialEq, Debug, Copy, Clone)] struct Triangle { vertices: [u32; 3], @@ -66,6 +82,10 @@ impl Triangle { } #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +#[cfg_attr( + feature = "rkyv", + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) +)] #[derive(PartialEq, Debug, Clone)] /// A convex polyhedron without degenerate faces. pub struct ConvexPolyhedron { diff --git a/src/shape/cuboid.rs b/src/shape/cuboid.rs index 4fc3eb14..44ac3364 100644 --- a/src/shape/cuboid.rs +++ b/src/shape/cuboid.rs @@ -12,6 +12,10 @@ use na::RealField; // for .copysign() /// Shape of a box. #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +#[cfg_attr( + feature = "rkyv", + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) +)] #[cfg_attr(feature = "cuda", derive(cust_core::DeviceCopy))] #[derive(PartialEq, Debug, Copy, Clone)] #[repr(C)] diff --git a/src/shape/cylinder.rs b/src/shape/cylinder.rs index f3f32316..11f5febc 100644 --- a/src/shape/cylinder.rs +++ b/src/shape/cylinder.rs @@ -13,6 +13,10 @@ use na::RealField; // for .copysign() /// Cylinder shape with its principal axis aligned with the `y` axis. #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +#[cfg_attr( + feature = "rkyv", + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) +)] #[cfg_attr(feature = "cuda", derive(cust_core::DeviceCopy))] #[derive(PartialEq, Debug, Copy, Clone)] #[repr(C)] diff --git a/src/shape/feature_id.rs b/src/shape/feature_id.rs index 1ce05b7f..a54e4189 100644 --- a/src/shape/feature_id.rs +++ b/src/shape/feature_id.rs @@ -4,6 +4,10 @@ /// allows an efficient retrieval of the geometric information of the /// feature. #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +#[cfg_attr( + feature = "rkyv", + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) +)] #[derive(Copy, Clone, Debug, Hash, PartialEq, Eq)] pub enum FeatureId { /// Shape-dependent identifier of a vertex. diff --git a/src/shape/half_space.rs b/src/shape/half_space.rs index 351512d4..24c80524 100644 --- a/src/shape/half_space.rs +++ b/src/shape/half_space.rs @@ -5,6 +5,10 @@ use na::Unit; /// A half-space delimited by an infinite plane. #[derive(PartialEq, Debug, Clone, Copy)] #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +#[cfg_attr( + feature = "rkyv", + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) +)] #[cfg_attr(feature = "cuda", derive(cust_core::DeviceCopy))] #[repr(C)] pub struct HalfSpace { diff --git a/src/shape/heightfield2.rs b/src/shape/heightfield2.rs index a1e89a52..7d8481db 100644 --- a/src/shape/heightfield2.rs +++ b/src/shape/heightfield2.rs @@ -50,6 +50,10 @@ impl HeightFieldStorage for DVector { } #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +#[cfg_attr( + feature = "rkyv", + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) +)] #[cfg_attr(feature = "cuda", derive(cust_core::DeviceCopy))] #[derive(Copy, Clone, Debug)] #[repr(C)] // Needed for Cuda. diff --git a/src/shape/heightfield3.rs b/src/shape/heightfield3.rs index 9123a664..a36d2c68 100644 --- a/src/shape/heightfield3.rs +++ b/src/shape/heightfield3.rs @@ -14,6 +14,10 @@ use na::ComplexField; bitflags! { #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] + #[cfg_attr( + feature = "rkyv", + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) + )] #[cfg_attr(feature = "cuda", derive(cust_core::DeviceCopy))] #[derive(Default)] /// The status of the cell of an heightfield. @@ -69,6 +73,10 @@ impl HeightFieldStorage for DMatrix { } #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +#[cfg_attr( + feature = "rkyv", + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) +)] #[cfg_attr(feature = "cuda", derive(cust_core::DeviceCopy))] #[derive(Copy, Clone, Debug)] #[repr(C)] // Needed for Cuda. diff --git a/src/shape/polygon.rs b/src/shape/polygon.rs index f4a7384c..c04870d6 100644 --- a/src/shape/polygon.rs +++ b/src/shape/polygon.rs @@ -5,6 +5,10 @@ use parry::bounding_volume::AABB; #[derive(Clone)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[cfg_attr( + feature = "rkyv", + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) +)] /// A convex planar polygon. pub struct Polygon { pub(crate) vertices: Vec>, diff --git a/src/shape/polyline.rs b/src/shape/polyline.rs index 67839bab..e8ac52b7 100644 --- a/src/shape/polyline.rs +++ b/src/shape/polyline.rs @@ -10,6 +10,10 @@ use na::ComplexField; // for .abs() #[derive(Clone)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[cfg_attr( + feature = "rkyv", + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) +)] /// A polyline. pub struct Polyline { qbvh: QBVH, diff --git a/src/shape/round_shape.rs b/src/shape/round_shape.rs index f681281e..ecaf9889 100644 --- a/src/shape/round_shape.rs +++ b/src/shape/round_shape.rs @@ -3,6 +3,10 @@ use crate::shape::SupportMap; use na::Unit; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[cfg_attr( + feature = "rkyv", + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) +)] #[cfg_attr(feature = "cuda", derive(cust_core::DeviceCopy))] #[derive(Copy, Clone, Debug)] #[repr(C)] diff --git a/src/shape/segment.rs b/src/shape/segment.rs index d048cac0..9a6fd048 100644 --- a/src/shape/segment.rs +++ b/src/shape/segment.rs @@ -8,6 +8,10 @@ use std::mem; /// A segment shape. #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +#[cfg_attr( + feature = "rkyv", + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) +)] #[cfg_attr(feature = "cuda", derive(cust_core::DeviceCopy))] #[derive(PartialEq, Debug, Copy, Clone)] #[repr(C)] diff --git a/src/shape/tetrahedron.rs b/src/shape/tetrahedron.rs index d4dbcd6e..7d5e7393 100644 --- a/src/shape/tetrahedron.rs +++ b/src/shape/tetrahedron.rs @@ -11,6 +11,10 @@ use na::ComplexField; // for .abs() /// A tetrahedron with 4 vertices. #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +#[cfg_attr( + feature = "rkyv", + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) +)] #[cfg_attr(feature = "cuda", derive(cust_core::DeviceCopy))] #[derive(Copy, Clone, Debug)] #[repr(C)] diff --git a/src/shape/triangle.rs b/src/shape/triangle.rs index bd91ded1..265918e1 100644 --- a/src/shape/triangle.rs +++ b/src/shape/triangle.rs @@ -13,6 +13,10 @@ use std::mem; /// A triangle shape. #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +#[cfg_attr( + feature = "rkyv", + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) +)] #[cfg_attr(feature = "cuda", derive(cust_core::DeviceCopy))] #[derive(PartialEq, Debug, Copy, Clone, Default)] #[repr(C)] diff --git a/src/shape/trimesh.rs b/src/shape/trimesh.rs index 6de6f559..bb4ca5e2 100644 --- a/src/shape/trimesh.rs +++ b/src/shape/trimesh.rs @@ -54,6 +54,10 @@ impl std::error::Error for TopologyError {} /// DOI: 10.1109/TVCG.2005.49 #[derive(Clone)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[cfg_attr( + feature = "rkyv", + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) +)] #[cfg(feature = "dim3")] pub struct TriMeshPseudoNormals { /// The pseudo-normals of the vertices. @@ -75,6 +79,10 @@ impl Default for TriMeshPseudoNormals { /// The connected-components of a triangle mesh. #[derive(Clone, Debug)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[cfg_attr( + feature = "rkyv", + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) +)] pub struct TriMeshConnectedComponents { /// The `face_colors[i]` gives the connected-component index /// of the i-th face. @@ -96,6 +104,10 @@ impl TriMeshConnectedComponents { /// A vertex of a triangle-mesh’s half-edge topology. #[derive(Clone, Copy, Debug)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[cfg_attr( + feature = "rkyv", + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) +)] pub struct TopoVertex { /// One of the half-edge with this vertex as endpoint. pub half_edge: u32, @@ -104,6 +116,10 @@ pub struct TopoVertex { /// A face of a triangle-mesh’s half-edge topology. #[derive(Clone, Copy, Debug)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[cfg_attr( + feature = "rkyv", + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) +)] pub struct TopoFace { /// The half-edge adjascent to this face, whith a starting point equal /// to the first point of this face. @@ -113,6 +129,10 @@ pub struct TopoFace { /// A half-edge of a triangle-mesh’s half-edge topology. #[derive(Clone, Copy, Debug)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[cfg_attr( + feature = "rkyv", + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) +)] pub struct TopoHalfEdge { /// The next half-edge. pub next: u32, @@ -129,6 +149,10 @@ pub struct TopoHalfEdge { /// The half-edge topology information of a triangle mesh. #[derive(Clone, Default)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[cfg_attr( + feature = "rkyv", + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) +)] pub struct TriMeshTopology { /// The vertices of this half-edge representation. pub vertices: Vec, @@ -155,6 +179,10 @@ impl TriMeshTopology { bitflags::bitflags! { #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] + #[cfg_attr( + feature = "rkyv", + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) + )] #[cfg_attr(feature = "cuda", derive(cust_core::DeviceCopy))] #[derive(Default)] /// The status of the cell of an heightfield. @@ -195,6 +223,10 @@ bitflags::bitflags! { #[derive(Clone)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[cfg_attr( + feature = "rkyv", + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) +)] /// A triangle mesh. pub struct TriMesh { qbvh: QBVH, diff --git a/src/utils/sdp_matrix.rs b/src/utils/sdp_matrix.rs index 203203d8..901376ee 100644 --- a/src/utils/sdp_matrix.rs +++ b/src/utils/sdp_matrix.rs @@ -5,6 +5,10 @@ use std::ops::{Add, Mul}; /// A 2x2 symmetric-definite-positive matrix. #[derive(Copy, Clone, Debug, PartialEq)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[cfg_attr( + feature = "rkyv", + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) +)] pub struct SdpMatrix2 { /// The component at the first row and first column of this matrix. pub m11: N, @@ -106,6 +110,10 @@ impl Mul for SdpMatrix2 { /// A 3x3 symmetric-definite-positive matrix. #[derive(Copy, Clone, Debug, PartialEq)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[cfg_attr( + feature = "rkyv", + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) +)] pub struct SdpMatrix3 { /// The component at the first row and first column of this matrix. pub m11: N, diff --git a/src/utils/sorted_pair.rs b/src/utils/sorted_pair.rs index 3820ec26..f87c86f2 100644 --- a/src/utils/sorted_pair.rs +++ b/src/utils/sorted_pair.rs @@ -5,6 +5,10 @@ use std::ops::Deref; /// A pair of elements sorted in increasing order. #[derive(Copy, Clone, Debug, PartialEq, Eq, PartialOrd, Ord, Hash)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[cfg_attr( + feature = "rkyv", + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) +)] pub struct SortedPair([T; 2]); impl SortedPair { @@ -25,3 +29,30 @@ impl Deref for SortedPair { unsafe { mem::transmute(self) } } } + +// TODO: can we avoid these manual impls of Hash/PartialEq/Eq for the archived types? +#[cfg(feature = "rkyv")] +impl std::hash::Hash for ArchivedSortedPair +where + [::Archived; 2]: std::hash::Hash, +{ + fn hash(&self, state: &mut H) { + self.0.hash(state) + } +} + +#[cfg(feature = "rkyv")] +impl PartialEq for ArchivedSortedPair +where + [::Archived; 2]: PartialEq, +{ + fn eq(&self, other: &Self) -> bool { + self.0 == other.0 + } +} + +#[cfg(feature = "rkyv")] +impl Eq for ArchivedSortedPair where + [::Archived; 2]: Eq +{ +} From 1e6d0c27dc510d5c9999728495d1a46e86e315ed Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Wed, 13 Jul 2022 16:59:24 +0200 Subject: [PATCH 09/17] Add parallel simultaneous QBVH traversal --- Cargo.toml | 2 + crates/parry2d-f64/Cargo.toml | 1 + crates/parry2d/Cargo.toml | 1 + crates/parry3d-f64/Cargo.toml | 1 + crates/parry3d/Cargo.toml | 1 + src/partitioning/mod.rs | 2 + src/partitioning/qbvh/traversal.rs | 116 ++++++++++++++++++ src/partitioning/visitor.rs | 17 +++ ...lume_intersections_simultaneous_visitor.rs | 44 ++++++- .../mesh_intersection/mesh_intersection.rs | 8 +- 10 files changed, 186 insertions(+), 7 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index 6f4f0f82..6abca77b 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -8,4 +8,6 @@ parry3d = { path = "crates/parry3d" } parry2d-f64 = { path = "crates/parry2d-f64" } parry3d-f64 = { path = "crates/parry3d-f64" } +#simba = { path = "../simba" } +simba = { git = "https://github.com/dimforge/simba", rev = "b1392df62a0f4cf91e397bbb6bd41b7731afb6ab" } # nalgebra = { git = "https://github.com/dimforge/nalgebra" } \ No newline at end of file diff --git a/crates/parry2d-f64/Cargo.toml b/crates/parry2d-f64/Cargo.toml index dd7d5fd1..93334212 100644 --- a/crates/parry2d-f64/Cargo.toml +++ b/crates/parry2d-f64/Cargo.toml @@ -28,6 +28,7 @@ simd-stable = [ "simba/wide", "simd-is-enabled" ] simd-nightly = [ "simba/packed_simd", "simd-is-enabled" ] enhanced-determinism = [ "simba/libm_force", "indexmap" ] cuda = [ "cust_core", "cust", "nalgebra/cuda" ] +parallel = [ "rayon" ] # Do not enable this feature directly. It is automatically # enabled with the "simd-stable" or "simd-nightly" feature. diff --git a/crates/parry2d/Cargo.toml b/crates/parry2d/Cargo.toml index c096c154..f5721bfc 100644 --- a/crates/parry2d/Cargo.toml +++ b/crates/parry2d/Cargo.toml @@ -28,6 +28,7 @@ simd-stable = [ "simba/wide", "simd-is-enabled" ] simd-nightly = [ "simba/packed_simd", "simd-is-enabled" ] enhanced-determinism = [ "simba/libm_force", "indexmap" ] cuda = [ "cust_core", "cust", "nalgebra/cuda" ] +parallel = [ "rayon" ] # Do not enable this feature directly. It is automatically # enabled with the "simd-stable" or "simd-nightly" feature. diff --git a/crates/parry3d-f64/Cargo.toml b/crates/parry3d-f64/Cargo.toml index a843b75d..b512680b 100644 --- a/crates/parry3d-f64/Cargo.toml +++ b/crates/parry3d-f64/Cargo.toml @@ -28,6 +28,7 @@ simd-stable = [ "simba/wide", "simd-is-enabled" ] simd-nightly = [ "simba/packed_simd", "simd-is-enabled" ] enhanced-determinism = [ "simba/libm_force", "indexmap" ] cuda = [ "cust_core", "cust", "nalgebra/cuda" ] +parallel = [ "rayon" ] # Do not enable this feature directly. It is automatically # enabled with the "simd-stable" or "simd-nightly" feature. diff --git a/crates/parry3d/Cargo.toml b/crates/parry3d/Cargo.toml index 46b30df4..31dea68f 100644 --- a/crates/parry3d/Cargo.toml +++ b/crates/parry3d/Cargo.toml @@ -29,6 +29,7 @@ simd-stable = [ "simba/wide", "simd-is-enabled" ] simd-nightly = [ "simba/packed_simd", "simd-is-enabled" ] enhanced-determinism = [ "simba/libm_force", "indexmap" ] cuda = [ "cust_core", "cust", "nalgebra/cuda" ] +parallel = [ "rayon" ] # Do not enable this feature directly. It is automatically # enabled with the "simd-stable" or "simd-nightly" feature. diff --git a/src/partitioning/mod.rs b/src/partitioning/mod.rs index 10ed2c30..a4274936 100644 --- a/src/partitioning/mod.rs +++ b/src/partitioning/mod.rs @@ -3,6 +3,8 @@ pub use self::qbvh::{ IndexedData, NodeIndex, QBVHDataGenerator, QbvhNonOverlappingDataSplitter, QBVH, }; +#[cfg(feature = "parallel")] +pub use self::visitor::ParallelSimdSimultaneousVisitor; pub use self::visitor::{ SimdBestFirstVisitStatus, SimdBestFirstVisitor, SimdSimultaneousVisitStatus, SimdSimultaneousVisitor, SimdVisitStatus, SimdVisitor, diff --git a/src/partitioning/qbvh/traversal.rs b/src/partitioning/qbvh/traversal.rs index 109a3093..d0b61938 100644 --- a/src/partitioning/qbvh/traversal.rs +++ b/src/partitioning/qbvh/traversal.rs @@ -10,6 +10,13 @@ use crate::utils::WeightedValue; use num::Bounded; use simba::simd::SimdBool; use std::collections::BinaryHeap; +#[cfg(feature = "parallel")] +use { + crate::partitioning::ParallelSimdSimultaneousVisitor, + arrayvec::ArrayVec, + rayon::prelude::*, + std::sync::atomic::{AtomicBool, Ordering as AtomicOrdering}, +}; use super::{IndexedData, NodeIndex, QBVH}; @@ -274,3 +281,112 @@ impl QBVH { } } } + +#[cfg(feature = "parallel")] +impl QBVH { + /// Performs a simultaneous traversal of two QBVH using + /// parallelism internally for better performances with large tree. + pub fn traverse_bvtt_parallel( + &self, + qbvh2: &QBVH, + visitor: &impl ParallelSimdSimultaneousVisitor, + ) { + if !self.nodes.is_empty() && !qbvh2.nodes.is_empty() { + let exit_early = AtomicBool::new(false); + self.traverse_node_parallel(qbvh2, visitor, &exit_early, (0, 0)); + } + } + + pub fn traverse_node_parallel( + &self, + qbvh2: &QBVH, + visitor: &impl ParallelSimdSimultaneousVisitor, + exit_early: &AtomicBool, + entry: (u32, u32), + ) { + if exit_early.load(AtomicOrdering::Relaxed) { + return; + } + + let qbvh1 = self; + let node1 = qbvh1.nodes[entry.0 as usize]; + let node2 = qbvh2.nodes[entry.1 as usize]; + + const SQUARE_SIMD_WIDTH: usize = SIMD_WIDTH * SIMD_WIDTH; + let mut stack: ArrayVec<(u32, u32), SQUARE_SIMD_WIDTH> = ArrayVec::new(); + + let leaf_data1 = if node1.leaf { + Some( + array![|ii| Some(&qbvh1.proxies.get(node1.children[ii] as usize)?.data); SIMD_WIDTH], + ) + } else { + None + }; + + let leaf_data2 = if node2.leaf { + Some( + array![|ii| Some(&qbvh2.proxies.get(node2.children[ii] as usize)?.data); SIMD_WIDTH], + ) + } else { + None + }; + + match visitor.visit(&node1.simd_aabb, leaf_data1, &node2.simd_aabb, leaf_data2) { + SimdSimultaneousVisitStatus::ExitEarly => { + exit_early.store(true, AtomicOrdering::Relaxed); + return; + } + SimdSimultaneousVisitStatus::MaybeContinue(mask) => { + match (node1.leaf, node2.leaf) { + (true, true) => { /* Can’t go deeper. */ } + (true, false) => { + let mut bitmask = 0; + for ii in 0..SIMD_WIDTH { + bitmask |= mask[ii].bitmask(); + } + + for jj in 0..SIMD_WIDTH { + if (bitmask & (1 << jj)) != 0 { + if node2.children[jj] as usize <= qbvh2.nodes.len() { + stack.push((entry.0, node2.children[jj])); + } + } + } + } + (false, true) => { + for ii in 0..SIMD_WIDTH { + let bitmask = mask[ii].bitmask(); + + if bitmask != 0 { + if node1.children[ii] as usize <= qbvh1.nodes.len() { + stack.push((node1.children[ii], entry.1)); + } + } + } + } + (false, false) => { + for ii in 0..SIMD_WIDTH { + let bitmask = mask[ii].bitmask(); + + for jj in 0..SIMD_WIDTH { + if (bitmask & (1 << jj)) != 0 { + if node1.children[ii] as usize <= qbvh1.nodes.len() + && node2.children[jj] as usize <= qbvh2.nodes.len() + { + stack.push((node1.children[ii], node2.children[jj])); + } + } + } + } + } + } + } + } + + stack + .as_slice() + .par_iter() + .copied() + .for_each(|entry| self.traverse_node_parallel(qbvh2, visitor, exit_early, entry)); + } +} diff --git a/src/partitioning/visitor.rs b/src/partitioning/visitor.rs index 9f864ecd..89813c9d 100644 --- a/src/partitioning/visitor.rs +++ b/src/partitioning/visitor.rs @@ -83,3 +83,20 @@ pub trait SimdSimultaneousVisitor { right_data: Option<[Option<&T2>; SIMD_WIDTH]>, ) -> SimdSimultaneousVisitStatus; } + +/// Trait implemented by visitor called during a parallel simultaneous spatial partitioning +/// data structure traversal. +#[cfg(feature = "parallel")] +pub trait ParallelSimdSimultaneousVisitor: Sync { + /// Execute an operation on the content of two nodes, one from each structure. + /// + /// Returns whether the traversal should continue on the nodes children, if it should not continue + /// on those children, or if the whole traversal should be exited early. + fn visit( + &self, + left_bv: &SimdBV, + left_data: Option<[Option<&T1>; SIMD_WIDTH]>, + right_bv: &SimdBV, + right_data: Option<[Option<&T2>; SIMD_WIDTH]>, + ) -> SimdSimultaneousVisitStatus; +} diff --git a/src/query/visitors/bounding_volume_intersections_simultaneous_visitor.rs b/src/query/visitors/bounding_volume_intersections_simultaneous_visitor.rs index b5131a04..01e13934 100644 --- a/src/query/visitors/bounding_volume_intersections_simultaneous_visitor.rs +++ b/src/query/visitors/bounding_volume_intersections_simultaneous_visitor.rs @@ -12,10 +12,7 @@ pub struct BoundingVolumeIntersectionsSimultaneousVisitor { _phantom: PhantomData<(T1, T2)>, } -impl BoundingVolumeIntersectionsSimultaneousVisitor -where - F: FnMut(&T1, &T2) -> bool, -{ +impl BoundingVolumeIntersectionsSimultaneousVisitor { /// Creates a new `BoundingVolumeIntersectionsSimultaneousVisitor`. #[inline] pub fn new(callback: F) -> BoundingVolumeIntersectionsSimultaneousVisitor { @@ -77,3 +74,42 @@ where SimdSimultaneousVisitStatus::MaybeContinue(mask) } } + +#[cfg(feature = "parallel")] +impl crate::partitioning::ParallelSimdSimultaneousVisitor + for BoundingVolumeIntersectionsSimultaneousVisitor +where + F: Sync + Fn(&T1, &T2) -> bool, +{ + #[inline] + fn visit( + &self, + left_bv: &SimdAABB, + left_data: Option<[Option<&T1>; SIMD_WIDTH]>, + right_bv: &SimdAABB, + right_data: Option<[Option<&T2>; SIMD_WIDTH]>, + ) -> SimdSimultaneousVisitStatus { + let mask = if let Some(pos12) = &self.pos12 { + let transformed_right_bv = right_bv.transform_by(pos12); + left_bv.intersects_permutations(&transformed_right_bv) + } else { + left_bv.intersects_permutations(right_bv) + }; + + if let (Some(data1), Some(data2)) = (left_data, right_data) { + for ii in 0..SIMD_WIDTH { + let bitmask = mask[ii].bitmask(); + + for jj in 0..SIMD_WIDTH { + if (bitmask & (1 << jj)) != 0 && data1[ii].is_some() && data2[jj].is_some() { + if !(self.callback)(data1[ii].unwrap(), data2[jj].unwrap()) { + return SimdSimultaneousVisitStatus::ExitEarly; + } + } + } + } + } + + SimdSimultaneousVisitStatus::MaybeContinue(mask) + } +} diff --git a/src/transformation/mesh_intersection/mesh_intersection.rs b/src/transformation/mesh_intersection/mesh_intersection.rs index b56e747e..3a030654 100644 --- a/src/transformation/mesh_intersection/mesh_intersection.rs +++ b/src/transformation/mesh_intersection/mesh_intersection.rs @@ -35,11 +35,13 @@ pub fn intersect_meshes( // 1: collect all the potential triangle-triangle intersections. let mut intersections = vec![]; - let mut visitor = - BoundingVolumeIntersectionsSimultaneousVisitor::with_relative_pos(pos12, |tri1, tri2| { + let mut visitor = BoundingVolumeIntersectionsSimultaneousVisitor::with_relative_pos( + pos12, + |tri1: &u32, tri2: &u32| { intersections.push((*tri1, *tri2)); true - }); + }, + ); mesh1.qbvh().traverse_bvtt(mesh2.qbvh(), &mut visitor); From dab5e2a3900fad260d7bb90ca76e7b2a1c3198f1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Wed, 10 Aug 2022 14:59:29 +0200 Subject: [PATCH 10/17] =?UTF-8?q?Experiment=20with=20QBVH=E2=80=AFcontaini?= =?UTF-8?q?ng=20internal=20node=20data?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/partitioning/mod.rs | 5 +- src/partitioning/qbvh/build.rs | 116 +++++++++++----- src/partitioning/qbvh/mod.rs | 9 +- src/partitioning/qbvh/qbvh.rs | 40 +++--- src/partitioning/qbvh/traversal.rs | 124 +++++++++++++----- src/partitioning/qbvh/update.rs | 19 ++- src/partitioning/visitor.rs | 72 ++++++++-- ...lume_intersections_simultaneous_visitor.rs | 29 ++-- 8 files changed, 295 insertions(+), 119 deletions(-) diff --git a/src/partitioning/mod.rs b/src/partitioning/mod.rs index a4274936..10d819b3 100644 --- a/src/partitioning/mod.rs +++ b/src/partitioning/mod.rs @@ -1,10 +1,11 @@ //! Spatial partitioning tools. pub use self::qbvh::{ - IndexedData, NodeIndex, QBVHDataGenerator, QbvhNonOverlappingDataSplitter, QBVH, + CenterDataSplitter, IndexedData, NodeIndex, QBVHDataGenerator, QBVHNode, QBVHNodeDataGenerator, + QBVHProxy, QbvhNonOverlappingDataSplitter, SimdNodeIndex, QBVH, }; #[cfg(feature = "parallel")] -pub use self::visitor::ParallelSimdSimultaneousVisitor; +pub use self::visitor::{ParallelSimdSimultaneousVisitor, ParallelSimdVisitor}; pub use self::visitor::{ SimdBestFirstVisitStatus, SimdBestFirstVisitor, SimdSimultaneousVisitStatus, SimdSimultaneousVisitor, SimdVisitStatus, SimdVisitor, diff --git a/src/partitioning/qbvh/build.rs b/src/partitioning/qbvh/build.rs index 5a17238f..c8525fc4 100644 --- a/src/partitioning/qbvh/build.rs +++ b/src/partitioning/qbvh/build.rs @@ -8,15 +8,15 @@ use simba::simd::SimdValue; use super::utils::split_indices_wrt_dim; use super::{IndexedData, NodeIndex, QBVHNode, QBVHProxy, QBVH}; -pub struct BuilderProxies<'a, T> { - proxies: &'a mut Vec>, +pub struct BuilderProxies<'a, LeafData> { + proxies: &'a mut Vec>, aabbs: &'a mut Vec, } -impl<'a, T> BuilderProxies<'a, T> { - fn insert(&mut self, data: T, aabb: AABB) +impl<'a, LeafData> BuilderProxies<'a, LeafData> { + fn insert(&mut self, data: LeafData, aabb: AABB) where - T: IndexedData, + LeafData: IndexedData, { let index = data.index(); @@ -30,18 +30,38 @@ impl<'a, T> BuilderProxies<'a, T> { } } -pub trait QBVHDataSplitter { +pub trait QBVHNodeDataGenerator { + fn data_from_array(&self, indices: &[usize], proxies: &[QBVHProxy]) -> NodeData; + fn data_from_leaf(&self, leaf: &LeafData) -> NodeData; + fn merge_data(&self, data: &[NodeData; 4]) -> NodeData; +} + +impl QBVHNodeDataGenerator for () { + fn data_from_array(&self, _: &[usize], _: &[QBVHProxy]) -> () { + () + } + + fn data_from_leaf(&self, _: &LeafData) -> () { + () + } + + fn merge_data(&self, data: &[(); 4]) -> () { + () + } +} + +pub trait QBVHDataSplitter { fn split_dataset<'idx>( &mut self, subdiv_dims: [usize; 2], center: Point, indices: &'idx mut [usize], indices_workspace: &'idx mut Vec, - proxies: BuilderProxies, + proxies: BuilderProxies, ) -> [&'idx mut [usize]; 4]; } -struct CenterDataSplitter { +pub struct CenterDataSplitter { pub enable_fallback_split: bool, } @@ -53,26 +73,26 @@ impl Default for CenterDataSplitter { } } -impl QBVHDataSplitter for CenterDataSplitter { +impl QBVHDataSplitter for CenterDataSplitter { fn split_dataset<'idx>( &mut self, subdiv_dims: [usize; 2], center: Point, indices: &'idx mut [usize], _: &'idx mut Vec, - proxies: BuilderProxies, + proxies: BuilderProxies, ) -> [&'idx mut [usize]; 4] { self.split_dataset_wo_workspace(subdiv_dims, center, indices, proxies) } } impl CenterDataSplitter { - fn split_dataset_wo_workspace<'idx, T>( + fn split_dataset_wo_workspace<'idx, LeafData>( &mut self, subdiv_dims: [usize; 2], center: Point, indices: &'idx mut [usize], - proxies: BuilderProxies, + proxies: BuilderProxies, ) -> [&'idx mut [usize]; 4] { // TODO: should we split wrt. the median instead of the average? // TODO: we should ensure each subslice contains at least 4 elements each (or less if @@ -117,10 +137,10 @@ pub struct QbvhNonOverlappingDataSplitter { pub epsilon: Real, } -impl QBVHDataSplitter for QbvhNonOverlappingDataSplitter +impl QBVHDataSplitter for QbvhNonOverlappingDataSplitter where - T: IndexedData, - F: FnMut(T, usize, Real, Real, AABB, AABB) -> SplitResult<(T, AABB)>, + LeafData: IndexedData, + F: FnMut(LeafData, usize, Real, Real, AABB, AABB) -> SplitResult<(LeafData, AABB)>, { fn split_dataset<'idx>( &mut self, @@ -128,7 +148,7 @@ where center: Point, indices: &'idx mut [usize], indices_workspace: &'idx mut Vec, - mut proxies: BuilderProxies, + mut proxies: BuilderProxies, ) -> [&'idx mut [usize]; 4] { // 1. Snap the spliting point to one fo the AABB min/max, // such that at least one AABB isn’t split along each dimension. @@ -228,52 +248,54 @@ where } /// Trait used for generating the content of the leaves of the QBVH acceleration structure. -pub trait QBVHDataGenerator { +pub trait QBVHDataGenerator { /// Gives an idea of the number of elements this generator contains. /// /// This is primarily used for pre-allocating some arrays for better performances. fn size_hint(&self) -> usize; /// Iterate through all the elements of this generator. - fn for_each(&mut self, f: impl FnMut(T, AABB)); + fn for_each(&mut self, f: impl FnMut(LeafData, AABB)); } -impl QBVHDataGenerator for F +impl QBVHDataGenerator for F where - F: ExactSizeIterator, + F: ExactSizeIterator, { fn size_hint(&self) -> usize { self.len() } #[inline(always)] - fn for_each(&mut self, mut f: impl FnMut(T, AABB)) { + fn for_each(&mut self, mut f: impl FnMut(LeafData, AABB)) { for (elt, aabb) in self { f(elt, aabb) } } } -impl QBVH { +impl QBVH { /// Clears this quaternary BVH and rebuilds it from a new set of data and AABBs. pub fn clear_and_rebuild( &mut self, - data_gen: impl QBVHDataGenerator, + data_gen: impl QBVHDataGenerator, dilation_factor: Real, ) { self.clear_and_rebuild_with_splitter( data_gen, CenterDataSplitter::default(), + (), dilation_factor, ); } } -impl QBVH { +impl QBVH { /// Clears this quaternary BVH and rebuilds it from a new set of data and AABBs. pub fn clear_and_rebuild_with_splitter( &mut self, - mut data_gen: impl QBVHDataGenerator, - mut splitter: impl QBVHDataSplitter, + mut data_gen: impl QBVHDataGenerator, + mut splitter: impl QBVHDataSplitter, + mut node_data: impl QBVHNodeDataGenerator, dilation_factor: Real, ) { self.nodes.clear(); @@ -301,6 +323,12 @@ impl QBVH { simd_aabb: SimdAABB::new_invalid(), children: [1, u32::MAX, u32::MAX, u32::MAX], parent: NodeIndex::invalid(), + data: [ + NodeData::default(), + NodeData::default(), + NodeData::default(), + NodeData::default(), + ], leaf: false, dirty: false, }; @@ -309,6 +337,7 @@ impl QBVH { let root_id = NodeIndex::new(0, 0); let (_, aabb) = self.do_recurse_build_generic( &mut splitter, + &node_data, &mut indices, &mut aabbs, root_id, @@ -326,7 +355,8 @@ impl QBVH { fn do_recurse_build_generic( &mut self, - splitter: &mut impl QBVHDataSplitter, + splitter: &mut impl QBVHDataSplitter, + node_data: &impl QBVHNodeDataGenerator, indices: &mut [usize], aabbs: &mut Vec, parent: NodeIndex, @@ -338,18 +368,26 @@ impl QBVH { let mut my_aabb = AABB::new_invalid(); let mut leaf_aabbs = [AABB::new_invalid(); 4]; let mut proxy_ids = [u32::MAX; 4]; + let mut data = [ + NodeData::default(), + NodeData::default(), + NodeData::default(), + NodeData::default(), + ]; for (k, id) in indices.iter().enumerate() { my_aabb.merge(&aabbs[*id]); leaf_aabbs[k] = aabbs[*id]; proxy_ids[k] = *id as u32; self.proxies[*id].node = NodeIndex::new(my_id as u32, k as u8); + data[k] = node_data.data_from_leaf(&self.proxies[*id].data); } let mut node = QBVHNode { simd_aabb: SimdAABB::from(leaf_aabbs), children: proxy_ids, parent, + data, leaf: true, dirty: false, }; @@ -398,6 +436,12 @@ impl QBVH { simd_aabb: SimdAABB::new_invalid(), children: [0; 4], // Will be set after the recursive call parent, + data: [ + NodeData::default(), + NodeData::default(), + NodeData::default(), + NodeData::default(), + ], leaf: false, dirty: false, }; @@ -408,7 +452,7 @@ impl QBVH { // Split the set along the two subdiv_dims dimensions. let proxies = BuilderProxies { proxies: &mut self.proxies, - aabbs: aabbs, + aabbs, }; // Recurse! @@ -421,16 +465,24 @@ impl QBVH { NodeIndex::new(id, 3), ]; + let data = [ + node_data.data_from_array(splits[0], &self.proxies), + node_data.data_from_array(splits[1], &self.proxies), + node_data.data_from_array(splits[2], &self.proxies), + node_data.data_from_array(splits[3], &self.proxies), + ]; + let children = [ - self.do_recurse_build_generic(splitter, splits[0], aabbs, n[0], dilation), - self.do_recurse_build_generic(splitter, splits[1], aabbs, n[1], dilation), - self.do_recurse_build_generic(splitter, splits[2], aabbs, n[2], dilation), - self.do_recurse_build_generic(splitter, splits[3], aabbs, n[3], dilation), + self.do_recurse_build_generic(splitter, node_data, splits[0], aabbs, n[0], dilation), + self.do_recurse_build_generic(splitter, node_data, splits[1], aabbs, n[1], dilation), + self.do_recurse_build_generic(splitter, node_data, splits[2], aabbs, n[2], dilation), + self.do_recurse_build_generic(splitter, node_data, splits[3], aabbs, n[3], dilation), ]; // Now we know the indices of the child nodes. self.nodes[id as usize].children = [children[0].0, children[1].0, children[2].0, children[3].0]; + self.nodes[id as usize].data = data; self.nodes[id as usize].simd_aabb = SimdAABB::from([children[0].1, children[1].1, children[2].1, children[3].1]); self.nodes[id as usize] diff --git a/src/partitioning/qbvh/mod.rs b/src/partitioning/qbvh/mod.rs index 0e06432d..1d70f68b 100644 --- a/src/partitioning/qbvh/mod.rs +++ b/src/partitioning/qbvh/mod.rs @@ -1,8 +1,11 @@ -pub use self::build::{BuilderProxies, QBVHDataGenerator, QbvhNonOverlappingDataSplitter}; -pub use self::qbvh::{IndexedData, NodeIndex, QBVH}; - pub(self) use self::qbvh::*; +pub use self::build::{ + BuilderProxies, CenterDataSplitter, QBVHDataGenerator, QBVHNodeDataGenerator, + QbvhNonOverlappingDataSplitter, +}; +pub use self::qbvh::{IndexedData, NodeIndex, QBVHNode, QBVHProxy, SimdNodeIndex, QBVH}; + mod build; mod qbvh; mod traversal; diff --git a/src/partitioning/qbvh/qbvh.rs b/src/partitioning/qbvh/qbvh.rs index e1e1c19e..bb829105 100644 --- a/src/partitioning/qbvh/qbvh.rs +++ b/src/partitioning/qbvh/qbvh.rs @@ -1,7 +1,6 @@ use crate::bounding_volume::{SimdAABB, AABB}; use crate::math::{Real, Vector}; use na::SimdValue; -use std::collections::VecDeque; /// A data to which an index is associated. pub trait IndexedData: Copy { @@ -40,6 +39,8 @@ impl IndexedData for u64 { } } +pub type SimdNodeIndex = u32; + /// The index of a node part of a QBVH. #[derive(Copy, Clone, Debug, PartialEq, Eq)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] @@ -48,8 +49,8 @@ impl IndexedData for u64 { derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] pub struct NodeIndex { - pub(super) index: u32, // Index of the addressed node in the `nodes` array. - pub(super) lane: u8, // SIMD lane of the addressed node. + pub index: SimdNodeIndex, // Index of the addressed node in the `nodes` array. + pub lane: u8, // SIMD lane of the addressed node. } impl NodeIndex { @@ -74,7 +75,7 @@ impl NodeIndex { feature = "rkyv", derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] -pub struct QBVHNode { +pub struct QBVHNode { /// The AABBs of the qbvh nodes represented by this node. pub simd_aabb: SimdAABB, /// Index of the nodes of the 4 nodes represented by `self`. @@ -84,6 +85,7 @@ pub struct QBVHNode { pub parent: NodeIndex, /// Are the four nodes represented by `self` leaves of the `QBVH`? pub leaf: bool, // TODO: pack this with the NodexIndex.lane? + pub data: [NodeData; 4], pub(super) dirty: bool, // TODO: move this to a separate bitvec? } @@ -93,23 +95,23 @@ pub struct QBVHNode { feature = "rkyv", derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] -pub struct QBVHProxy { +pub struct QBVHProxy { pub node: NodeIndex, - pub data: T, // The collider data. TODO: only set the collider generation here? + pub data: LeafData, // The collider data. TODO: only set the collider generation here? } -impl QBVHProxy { +impl QBVHProxy { pub(super) fn invalid() -> Self where - T: IndexedData, + LeafData: IndexedData, { Self { node: NodeIndex::invalid(), - data: T::default(), + data: LeafData::default(), } } - pub(super) fn detached(data: T) -> Self { + pub(super) fn detached(data: LeafData) -> Self { Self { node: NodeIndex::invalid(), data, @@ -126,14 +128,14 @@ impl QBVHProxy { derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] #[derive(Clone, Debug)] -pub struct QBVH { +pub struct QBVH { pub(super) root_aabb: AABB, - pub(super) nodes: Vec, + pub(super) nodes: Vec>, pub(super) dirty_nodes: Vec, - pub(super) proxies: Vec>, + pub(super) proxies: Vec>, } -impl QBVH { +impl QBVH { /// Initialize an empty QBVH. pub fn new() -> Self { QBVH { @@ -145,12 +147,12 @@ impl QBVH { } /// Iterates mutably through all the leaf data in this QBVH. - pub fn iter_data_mut(&mut self) -> impl Iterator { + pub fn iter_data_mut(&mut self) -> impl Iterator { self.proxies.iter_mut().map(|p| (p.node, &mut p.data)) } /// Iterate through all the leaf data in this QBVH. - pub fn iter_data(&self) -> impl Iterator { + pub fn iter_data(&self) -> impl Iterator { self.proxies.iter().map(|p| (p.node, &p.data)) } @@ -169,7 +171,7 @@ impl QBVH { /// Returns the data associated to a given leaf. /// /// Returns `None` if the provided node ID does not identify a leaf. - pub fn leaf_data(&mut self, node_id: NodeIndex) -> Option { + pub fn leaf_data(&mut self, node_id: NodeIndex) -> Option { let node = self.nodes.get(node_id.index as usize)?; if !node.leaf { @@ -187,7 +189,7 @@ impl QBVH { /// If this QBVH isn’t empty, the first element of the returned slice is the root of the /// tree. The other elements are not arranged in any particular order. /// The more high-level traversal methods should be used instead of this. - pub fn raw_nodes(&self) -> &[QBVHNode] { + pub fn raw_nodes(&self) -> &[QBVHNode] { &self.nodes } @@ -196,7 +198,7 @@ impl QBVH { /// If this QBVH isn’t empty, the first element of the returned slice is the root of the /// tree. The other elements are not arranged in any particular order. /// The more high-level traversal methods should be used instead of this. - pub fn raw_proxies(&self) -> &[QBVHProxy] { + pub fn raw_proxies(&self) -> &[QBVHProxy] { &self.proxies } diff --git a/src/partitioning/qbvh/traversal.rs b/src/partitioning/qbvh/traversal.rs index d0b61938..59a7a4e5 100644 --- a/src/partitioning/qbvh/traversal.rs +++ b/src/partitioning/qbvh/traversal.rs @@ -12,7 +12,7 @@ use simba::simd::SimdBool; use std::collections::BinaryHeap; #[cfg(feature = "parallel")] use { - crate::partitioning::ParallelSimdSimultaneousVisitor, + crate::partitioning::{ParallelSimdSimultaneousVisitor, ParallelSimdVisitor}, arrayvec::ArrayVec, rayon::prelude::*, std::sync::atomic::{AtomicBool, Ordering as AtomicOrdering}, @@ -20,16 +20,16 @@ use { use super::{IndexedData, NodeIndex, QBVH}; -impl QBVH { +impl QBVH { /// Performs a depth-first traversal on the BVH. - pub fn traverse_depth_first(&self, visitor: &mut impl SimdVisitor) { + pub fn traverse_depth_first(&self, visitor: &mut impl SimdVisitor) { self.traverse_depth_first_with_stack(visitor, &mut Vec::new()) } /// Performs a depth-first traversal on the BVH. pub fn traverse_depth_first_with_stack( &self, - visitor: &mut impl SimdVisitor, + visitor: &mut impl SimdVisitor, stack: &mut Vec, ) { stack.clear(); @@ -38,7 +38,7 @@ impl QBVH { stack.push(0); } while let Some(entry) = stack.pop() { - let node = self.nodes[entry as usize]; + let node = &self.nodes[entry as usize]; let leaf_data = if node.leaf { Some( array![|ii| Some(&self.proxies.get(node.children[ii] as usize)?.data); SIMD_WIDTH], @@ -77,7 +77,7 @@ impl QBVH { /// user-defined type. pub fn traverse_best_first(&self, visitor: &mut BFS) -> Option<(NodeIndex, BFS::Result)> where - BFS: SimdBestFirstVisitor, + BFS: SimdBestFirstVisitor, BFS::Result: Clone, // Because we cannot move out of an array… { if self.nodes.is_empty() { @@ -96,7 +96,7 @@ impl QBVH { break; // Solution found. } - let node = self.nodes[entry.value as usize]; + let node = &self.nodes[entry.value as usize]; let leaf_data = if node.leaf { Some( array![|ii| Some(&self.proxies.get(node.children[ii] as usize)?.data); SIMD_WIDTH], @@ -151,7 +151,7 @@ impl QBVH { /// the given AABB: // FIXME: implement a visitor pattern to merge intersect_aabb // and intersect_ray into a single method. - pub fn intersect_aabb(&self, aabb: &AABB, out: &mut Vec) { + pub fn intersect_aabb(&self, aabb: &AABB, out: &mut Vec) { if self.nodes.is_empty() { return; } @@ -160,7 +160,7 @@ impl QBVH { let mut stack = vec![0u32]; let simd_aabb = SimdAABB::splat(*aabb); while let Some(inode) = stack.pop() { - let node = self.nodes[inode as usize]; + let node = &self.nodes[inode as usize]; let intersections = node.simd_aabb.intersects(&simd_aabb); let bitmask = intersections.bitmask(); @@ -186,19 +186,19 @@ impl QBVH { } /// Performs a simultaneous traversal of two QBVH. - pub fn traverse_bvtt( + pub fn traverse_bvtt( &self, - qbvh2: &QBVH, - visitor: &mut impl SimdSimultaneousVisitor, + qbvh2: &QBVH, + visitor: &mut impl SimdSimultaneousVisitor, ) { self.traverse_bvtt_with_stack(qbvh2, visitor, &mut Vec::new()) } /// Performs a simultaneous traversal of two QBVH. - pub fn traverse_bvtt_with_stack( + pub fn traverse_bvtt_with_stack( &self, - qbvh2: &QBVH, - visitor: &mut impl SimdSimultaneousVisitor, + qbvh2: &QBVH, + visitor: &mut impl SimdSimultaneousVisitor, stack: &mut Vec<(u32, u32)>, ) { let qbvh1 = self; @@ -209,8 +209,8 @@ impl QBVH { } while let Some(entry) = stack.pop() { - let node1 = qbvh1.nodes[entry.0 as usize]; - let node2 = qbvh2.nodes[entry.1 as usize]; + let node1 = &qbvh1.nodes[entry.0 as usize]; + let node2 = &qbvh2.nodes[entry.1 as usize]; let leaf_data1 = if node1.leaf { Some( @@ -283,24 +283,84 @@ impl QBVH { } #[cfg(feature = "parallel")] -impl QBVH { +impl QBVH { + /// Performs a depth-first traversal of two QBVH using + /// parallelism internally for better performances with large tree. + pub fn traverse_depth_first_parallel( + &self, + visitor: &impl ParallelSimdVisitor, + ) { + if !self.nodes.is_empty() { + let exit_early = AtomicBool::new(false); + self.traverse_depth_first_node_parallel(visitor, &exit_early, 0); + } + } + + pub fn traverse_depth_first_node_parallel( + &self, + visitor: &impl ParallelSimdVisitor, + exit_early: &AtomicBool, + entry: u32, + ) { + if exit_early.load(AtomicOrdering::Relaxed) { + return; + } + + let mut stack: ArrayVec = ArrayVec::new(); + let node = &self.nodes[entry as usize]; + let leaf_data = if node.leaf { + Some(array![|ii| Some(&self.proxies.get(node.children[ii] as usize)?.data); SIMD_WIDTH]) + } else { + None + }; + + match visitor.visit(entry, node, leaf_data) { + SimdVisitStatus::ExitEarly => { + exit_early.store(true, AtomicOrdering::Relaxed); + return; + } + SimdVisitStatus::MaybeContinue(mask) => { + let bitmask = mask.bitmask(); + + for ii in 0..SIMD_WIDTH { + if (bitmask & (1 << ii)) != 0 { + if !node.leaf { + // Internal node, visit the child. + // Un fortunately, we have this check because invalid AABBs + // return a hit as well. + if node.children[ii] as usize <= self.nodes.len() { + stack.push(node.children[ii]); + } + } + } + } + } + } + + stack + .as_slice() + .par_iter() + .copied() + .for_each(|entry| self.traverse_depth_first_node_parallel(visitor, exit_early, entry)); + } + /// Performs a simultaneous traversal of two QBVH using /// parallelism internally for better performances with large tree. - pub fn traverse_bvtt_parallel( + pub fn traverse_bvtt_parallel( &self, - qbvh2: &QBVH, - visitor: &impl ParallelSimdSimultaneousVisitor, + qbvh2: &QBVH, + visitor: &impl ParallelSimdSimultaneousVisitor, ) { if !self.nodes.is_empty() && !qbvh2.nodes.is_empty() { let exit_early = AtomicBool::new(false); - self.traverse_node_parallel(qbvh2, visitor, &exit_early, (0, 0)); + self.traverse_bvtt_simd_node_parallel(qbvh2, visitor, &exit_early, (0, 0)); } } - pub fn traverse_node_parallel( + pub fn traverse_bvtt_simd_node_parallel( &self, - qbvh2: &QBVH, - visitor: &impl ParallelSimdSimultaneousVisitor, + qbvh2: &QBVH, + visitor: &impl ParallelSimdSimultaneousVisitor, exit_early: &AtomicBool, entry: (u32, u32), ) { @@ -309,8 +369,8 @@ impl QBVH { } let qbvh1 = self; - let node1 = qbvh1.nodes[entry.0 as usize]; - let node2 = qbvh2.nodes[entry.1 as usize]; + let node1 = &qbvh1.nodes[entry.0 as usize]; + let node2 = &qbvh2.nodes[entry.1 as usize]; const SQUARE_SIMD_WIDTH: usize = SIMD_WIDTH * SIMD_WIDTH; let mut stack: ArrayVec<(u32, u32), SQUARE_SIMD_WIDTH> = ArrayVec::new(); @@ -331,7 +391,7 @@ impl QBVH { None }; - match visitor.visit(&node1.simd_aabb, leaf_data1, &node2.simd_aabb, leaf_data2) { + match visitor.visit(&node1, leaf_data1, &node2, leaf_data2) { SimdSimultaneousVisitStatus::ExitEarly => { exit_early.store(true, AtomicOrdering::Relaxed); return; @@ -383,10 +443,8 @@ impl QBVH { } } - stack - .as_slice() - .par_iter() - .copied() - .for_each(|entry| self.traverse_node_parallel(qbvh2, visitor, exit_early, entry)); + stack.as_slice().par_iter().copied().for_each(|entry| { + self.traverse_bvtt_simd_node_parallel(qbvh2, visitor, exit_early, entry) + }); } } diff --git a/src/partitioning/qbvh/update.rs b/src/partitioning/qbvh/update.rs index f5f37d57..dbc11575 100644 --- a/src/partitioning/qbvh/update.rs +++ b/src/partitioning/qbvh/update.rs @@ -15,15 +15,15 @@ struct QBVHIncrementalBuilderStep { } #[allow(dead_code)] -struct QBVHIncrementalBuilder { - qbvh: QBVH, +struct QBVHIncrementalBuilder { + qbvh: QBVH, to_insert: Vec, aabbs: Vec, indices: Vec, } #[allow(dead_code)] -impl QBVHIncrementalBuilder { +impl QBVHIncrementalBuilder { pub fn new() -> Self { Self { qbvh: QBVH::new(), @@ -54,6 +54,7 @@ impl QBVHIncrementalBuilder { simd_aabb: SimdAABB::from(leaf_aabbs), children: proxy_ids, parent: to_insert.parent, + data: [NodeData::default(); SIMD_WIDTH], leaf: true, dirty: false, }; @@ -120,6 +121,12 @@ impl QBVHIncrementalBuilder { simd_aabb: SimdAABB::new_invalid(), children: [0; 4], // Will be set after the recursive call parent: to_insert.parent, + data: [ + NodeData::default(), + NodeData::default(), + NodeData::default(), + NodeData::default(), + ], leaf: false, dirty: false, }; @@ -158,10 +165,10 @@ impl QBVHIncrementalBuilder { } } -impl QBVH { +impl QBVH { /// Marks a piece of data as dirty so it can be updated during the next /// call to `self.update`. - pub fn pre_update(&mut self, data: T) { + pub fn pre_update(&mut self, data: LeafData) { let id = data.index(); let node_id = self.proxies[id].node.index; let node = &mut self.nodes[node_id as usize]; @@ -174,7 +181,7 @@ impl QBVH { /// Update all the nodes that have been marked as dirty by `self.pre_update`. pub fn update(&mut self, aabb_builder: F, dilation_factor: Real) where - F: Fn(&T) -> AABB, + F: Fn(&LeafData) -> AABB, { // Loop on the dirty leaves. let dilation_factor = SimdReal::splat(dilation_factor); diff --git a/src/partitioning/visitor.rs b/src/partitioning/visitor.rs index 89813c9d..c1de36ef 100644 --- a/src/partitioning/visitor.rs +++ b/src/partitioning/visitor.rs @@ -1,4 +1,7 @@ +use crate::bounding_volume::SimdAABB; use crate::math::{Real, SimdBool, SimdReal, SIMD_WIDTH}; +use crate::partitioning::qbvh::QBVHNode; +use crate::partitioning::{NodeIndex, SimdNodeIndex, QBVH}; /// The next action to be taken by a BVH traversal algorithm after having visited a node with some data. pub enum SimdBestFirstVisitStatus { @@ -20,7 +23,7 @@ pub enum SimdBestFirstVisitStatus { } /// Trait implemented by cost functions used by the best-first search on a `BVT`. -pub trait SimdBestFirstVisitor { +pub trait SimdBestFirstVisitor { /// The result of a best-first traversal. type Result; @@ -29,7 +32,7 @@ pub trait SimdBestFirstVisitor { &mut self, best_cost_so_far: Real, bv: &SimdBV, - value: Option<[Option<&T>; SIMD_WIDTH]>, + value: Option<[Option<&LeafData>; SIMD_WIDTH]>, ) -> SimdBestFirstVisitStatus; } @@ -52,23 +55,30 @@ pub enum SimdSimultaneousVisitStatus { } /// Trait implemented by visitor called during the traversal of a spatial partitioning data structure. -pub trait SimdVisitor { +pub trait SimdVisitor { /// Execute an operation on the content of a node of the spatial partitioning structure. /// /// Returns whether the traversal should continue on the node's children, if it should not continue /// on those children, or if the whole traversal should be exited early. - fn visit(&mut self, bv: &SimdBV, data: Option<[Option<&T>; SIMD_WIDTH]>) -> SimdVisitStatus; + fn visit( + &mut self, + bv: &SimdBV, + data: Option<[Option<&LeafData>; SIMD_WIDTH]>, + ) -> SimdVisitStatus; } -impl SimdVisitor for F +impl SimdVisitor for F where - F: FnMut(&SimdBV, Option<[Option<&T>; SIMD_WIDTH]>) -> SimdVisitStatus, + F: FnMut(&SimdBV, Option<[Option<&LeafData>; SIMD_WIDTH]>) -> SimdVisitStatus, { - fn visit(&mut self, bv: &SimdBV, data: Option<[Option<&T>; SIMD_WIDTH]>) -> SimdVisitStatus { + fn visit( + &mut self, + bv: &SimdBV, + data: Option<[Option<&LeafData>; SIMD_WIDTH]>, + ) -> SimdVisitStatus { (self)(bv, data) } } - /// Trait implemented by visitor called during a simultaneous spatial partitioning data structure tarversal. pub trait SimdSimultaneousVisitor { /// Execute an operation on the content of two nodes, one from each structure. @@ -84,19 +94,55 @@ pub trait SimdSimultaneousVisitor { ) -> SimdSimultaneousVisitStatus; } +/* + * + * Parallel visitors bellow. + * + */ + +/// Trait implemented by visitor called during the parallel traversal of a spatial partitioning data structure. +pub trait ParallelSimdVisitor: Sync { + /// Execute an operation on the content of a node of the spatial partitioning structure. + /// + /// Returns whether the traversal should continue on the node's children, if it should not continue + /// on those children, or if the whole traversal should be exited early. + fn visit( + &self, + node_id: SimdNodeIndex, + bv: &QBVHNode, + data: Option<[Option<&LeafData>; SIMD_WIDTH]>, + ) -> SimdVisitStatus; +} + +impl ParallelSimdVisitor for F +where + F: Sync + Fn(&QBVHNode, Option<[Option<&LeafData>; SIMD_WIDTH]>) -> SimdVisitStatus, +{ + fn visit( + &self, + node_id: SimdNodeIndex, + node: &QBVHNode, + data: Option<[Option<&LeafData>; SIMD_WIDTH]>, + ) -> SimdVisitStatus { + (self)(node, data) + } +} + /// Trait implemented by visitor called during a parallel simultaneous spatial partitioning /// data structure traversal. #[cfg(feature = "parallel")] -pub trait ParallelSimdSimultaneousVisitor: Sync { +pub trait ParallelSimdSimultaneousVisitor: + Sync +{ /// Execute an operation on the content of two nodes, one from each structure. /// /// Returns whether the traversal should continue on the nodes children, if it should not continue /// on those children, or if the whole traversal should be exited early. fn visit( &self, - left_bv: &SimdBV, - left_data: Option<[Option<&T1>; SIMD_WIDTH]>, - right_bv: &SimdBV, - right_data: Option<[Option<&T2>; SIMD_WIDTH]>, + left_node: &QBVHNode, + left_data: Option<[Option<&LeafData1>; SIMD_WIDTH]>, + right_node: &QBVHNode, + right_data: Option<[Option<&LeafData2>; SIMD_WIDTH]>, ) -> SimdSimultaneousVisitStatus; } diff --git a/src/query/visitors/bounding_volume_intersections_simultaneous_visitor.rs b/src/query/visitors/bounding_volume_intersections_simultaneous_visitor.rs index 01e13934..c512d6a1 100644 --- a/src/query/visitors/bounding_volume_intersections_simultaneous_visitor.rs +++ b/src/query/visitors/bounding_volume_intersections_simultaneous_visitor.rs @@ -1,6 +1,8 @@ use crate::bounding_volume::SimdAABB; use crate::math::{Isometry, Real, SimdReal, SIMD_WIDTH}; -use crate::partitioning::{SimdSimultaneousVisitStatus, SimdSimultaneousVisitor}; +use crate::partitioning::{ + QBVHNode, SimdNodeIndex, SimdSimultaneousVisitStatus, SimdSimultaneousVisitor, QBVH, +}; use na::SimdValue; use simba::simd::SimdBool as _; use std::marker::PhantomData; @@ -76,24 +78,29 @@ where } #[cfg(feature = "parallel")] -impl crate::partitioning::ParallelSimdSimultaneousVisitor - for BoundingVolumeIntersectionsSimultaneousVisitor +impl + crate::partitioning::ParallelSimdSimultaneousVisitor + for BoundingVolumeIntersectionsSimultaneousVisitor where - F: Sync + Fn(&T1, &T2) -> bool, + F: Sync + Fn(&LeafData1, &LeafData2) -> bool, { #[inline] fn visit( &self, - left_bv: &SimdAABB, - left_data: Option<[Option<&T1>; SIMD_WIDTH]>, - right_bv: &SimdAABB, - right_data: Option<[Option<&T2>; SIMD_WIDTH]>, + left_node: &QBVHNode, + left_data: Option<[Option<&LeafData1>; SIMD_WIDTH]>, + right_node: &QBVHNode, + right_data: Option<[Option<&LeafData2>; SIMD_WIDTH]>, ) -> SimdSimultaneousVisitStatus { let mask = if let Some(pos12) = &self.pos12 { - let transformed_right_bv = right_bv.transform_by(pos12); - left_bv.intersects_permutations(&transformed_right_bv) + let transformed_right_bv = right_node.simd_aabb.transform_by(pos12); + left_node + .simd_aabb + .intersects_permutations(&transformed_right_bv) } else { - left_bv.intersects_permutations(right_bv) + left_node + .simd_aabb + .intersects_permutations(&right_node.simd_aabb) }; if let (Some(data1), Some(data2)) = (left_data, right_data) { From 96323a96a2614e51218c69ed259068c6b5f10275 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sun, 14 Aug 2022 15:32:04 +0200 Subject: [PATCH 11/17] Remove QBVH internal node data --- src/partitioning/mod.rs | 4 +- src/partitioning/qbvh/build.rs | 62 ++----------------- src/partitioning/qbvh/mod.rs | 3 +- src/partitioning/qbvh/qbvh.rs | 11 ++-- src/partitioning/qbvh/traversal.rs | 31 +++++----- src/partitioning/qbvh/update.rs | 15 ++--- src/partitioning/visitor.rs | 18 +++--- ...lume_intersections_simultaneous_visitor.rs | 8 +-- 8 files changed, 43 insertions(+), 109 deletions(-) diff --git a/src/partitioning/mod.rs b/src/partitioning/mod.rs index 10d819b3..caace136 100644 --- a/src/partitioning/mod.rs +++ b/src/partitioning/mod.rs @@ -1,8 +1,8 @@ //! Spatial partitioning tools. pub use self::qbvh::{ - CenterDataSplitter, IndexedData, NodeIndex, QBVHDataGenerator, QBVHNode, QBVHNodeDataGenerator, - QBVHProxy, QbvhNonOverlappingDataSplitter, SimdNodeIndex, QBVH, + CenterDataSplitter, IndexedData, NodeIndex, QBVHDataGenerator, QBVHNode, QBVHProxy, + QbvhNonOverlappingDataSplitter, SimdNodeIndex, QBVH, }; #[cfg(feature = "parallel")] pub use self::visitor::{ParallelSimdSimultaneousVisitor, ParallelSimdVisitor}; diff --git a/src/partitioning/qbvh/build.rs b/src/partitioning/qbvh/build.rs index c8525fc4..c6465da2 100644 --- a/src/partitioning/qbvh/build.rs +++ b/src/partitioning/qbvh/build.rs @@ -30,26 +30,6 @@ impl<'a, LeafData> BuilderProxies<'a, LeafData> { } } -pub trait QBVHNodeDataGenerator { - fn data_from_array(&self, indices: &[usize], proxies: &[QBVHProxy]) -> NodeData; - fn data_from_leaf(&self, leaf: &LeafData) -> NodeData; - fn merge_data(&self, data: &[NodeData; 4]) -> NodeData; -} - -impl QBVHNodeDataGenerator for () { - fn data_from_array(&self, _: &[usize], _: &[QBVHProxy]) -> () { - () - } - - fn data_from_leaf(&self, _: &LeafData) -> () { - () - } - - fn merge_data(&self, data: &[(); 4]) -> () { - () - } -} - pub trait QBVHDataSplitter { fn split_dataset<'idx>( &mut self, @@ -283,19 +263,17 @@ impl QBVH { self.clear_and_rebuild_with_splitter( data_gen, CenterDataSplitter::default(), - (), dilation_factor, ); } } -impl QBVH { +impl QBVH { /// Clears this quaternary BVH and rebuilds it from a new set of data and AABBs. pub fn clear_and_rebuild_with_splitter( &mut self, mut data_gen: impl QBVHDataGenerator, mut splitter: impl QBVHDataSplitter, - mut node_data: impl QBVHNodeDataGenerator, dilation_factor: Real, ) { self.nodes.clear(); @@ -323,12 +301,6 @@ impl QBVH { simd_aabb: SimdAABB::new_invalid(), children: [1, u32::MAX, u32::MAX, u32::MAX], parent: NodeIndex::invalid(), - data: [ - NodeData::default(), - NodeData::default(), - NodeData::default(), - NodeData::default(), - ], leaf: false, dirty: false, }; @@ -337,7 +309,6 @@ impl QBVH { let root_id = NodeIndex::new(0, 0); let (_, aabb) = self.do_recurse_build_generic( &mut splitter, - &node_data, &mut indices, &mut aabbs, root_id, @@ -356,7 +327,6 @@ impl QBVH { fn do_recurse_build_generic( &mut self, splitter: &mut impl QBVHDataSplitter, - node_data: &impl QBVHNodeDataGenerator, indices: &mut [usize], aabbs: &mut Vec, parent: NodeIndex, @@ -368,26 +338,18 @@ impl QBVH { let mut my_aabb = AABB::new_invalid(); let mut leaf_aabbs = [AABB::new_invalid(); 4]; let mut proxy_ids = [u32::MAX; 4]; - let mut data = [ - NodeData::default(), - NodeData::default(), - NodeData::default(), - NodeData::default(), - ]; for (k, id) in indices.iter().enumerate() { my_aabb.merge(&aabbs[*id]); leaf_aabbs[k] = aabbs[*id]; proxy_ids[k] = *id as u32; self.proxies[*id].node = NodeIndex::new(my_id as u32, k as u8); - data[k] = node_data.data_from_leaf(&self.proxies[*id].data); } let mut node = QBVHNode { simd_aabb: SimdAABB::from(leaf_aabbs), children: proxy_ids, parent, - data, leaf: true, dirty: false, }; @@ -436,12 +398,6 @@ impl QBVH { simd_aabb: SimdAABB::new_invalid(), children: [0; 4], // Will be set after the recursive call parent, - data: [ - NodeData::default(), - NodeData::default(), - NodeData::default(), - NodeData::default(), - ], leaf: false, dirty: false, }; @@ -465,24 +421,16 @@ impl QBVH { NodeIndex::new(id, 3), ]; - let data = [ - node_data.data_from_array(splits[0], &self.proxies), - node_data.data_from_array(splits[1], &self.proxies), - node_data.data_from_array(splits[2], &self.proxies), - node_data.data_from_array(splits[3], &self.proxies), - ]; - let children = [ - self.do_recurse_build_generic(splitter, node_data, splits[0], aabbs, n[0], dilation), - self.do_recurse_build_generic(splitter, node_data, splits[1], aabbs, n[1], dilation), - self.do_recurse_build_generic(splitter, node_data, splits[2], aabbs, n[2], dilation), - self.do_recurse_build_generic(splitter, node_data, splits[3], aabbs, n[3], dilation), + self.do_recurse_build_generic(splitter, splits[0], aabbs, n[0], dilation), + self.do_recurse_build_generic(splitter, splits[1], aabbs, n[1], dilation), + self.do_recurse_build_generic(splitter, splits[2], aabbs, n[2], dilation), + self.do_recurse_build_generic(splitter, splits[3], aabbs, n[3], dilation), ]; // Now we know the indices of the child nodes. self.nodes[id as usize].children = [children[0].0, children[1].0, children[2].0, children[3].0]; - self.nodes[id as usize].data = data; self.nodes[id as usize].simd_aabb = SimdAABB::from([children[0].1, children[1].1, children[2].1, children[3].1]); self.nodes[id as usize] diff --git a/src/partitioning/qbvh/mod.rs b/src/partitioning/qbvh/mod.rs index 1d70f68b..ff30f136 100644 --- a/src/partitioning/qbvh/mod.rs +++ b/src/partitioning/qbvh/mod.rs @@ -1,8 +1,7 @@ pub(self) use self::qbvh::*; pub use self::build::{ - BuilderProxies, CenterDataSplitter, QBVHDataGenerator, QBVHNodeDataGenerator, - QbvhNonOverlappingDataSplitter, + BuilderProxies, CenterDataSplitter, QBVHDataGenerator, QbvhNonOverlappingDataSplitter, }; pub use self::qbvh::{IndexedData, NodeIndex, QBVHNode, QBVHProxy, SimdNodeIndex, QBVH}; diff --git a/src/partitioning/qbvh/qbvh.rs b/src/partitioning/qbvh/qbvh.rs index bb829105..3bb2c763 100644 --- a/src/partitioning/qbvh/qbvh.rs +++ b/src/partitioning/qbvh/qbvh.rs @@ -75,7 +75,7 @@ impl NodeIndex { feature = "rkyv", derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] -pub struct QBVHNode { +pub struct QBVHNode { /// The AABBs of the qbvh nodes represented by this node. pub simd_aabb: SimdAABB, /// Index of the nodes of the 4 nodes represented by `self`. @@ -85,7 +85,6 @@ pub struct QBVHNode { pub parent: NodeIndex, /// Are the four nodes represented by `self` leaves of the `QBVH`? pub leaf: bool, // TODO: pack this with the NodexIndex.lane? - pub data: [NodeData; 4], pub(super) dirty: bool, // TODO: move this to a separate bitvec? } @@ -128,14 +127,14 @@ impl QBVHProxy { derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] #[derive(Clone, Debug)] -pub struct QBVH { +pub struct QBVH { pub(super) root_aabb: AABB, - pub(super) nodes: Vec>, + pub(super) nodes: Vec, pub(super) dirty_nodes: Vec, pub(super) proxies: Vec>, } -impl QBVH { +impl QBVH { /// Initialize an empty QBVH. pub fn new() -> Self { QBVH { @@ -189,7 +188,7 @@ impl QBVH { /// If this QBVH isn’t empty, the first element of the returned slice is the root of the /// tree. The other elements are not arranged in any particular order. /// The more high-level traversal methods should be used instead of this. - pub fn raw_nodes(&self) -> &[QBVHNode] { + pub fn raw_nodes(&self) -> &[QBVHNode] { &self.nodes } diff --git a/src/partitioning/qbvh/traversal.rs b/src/partitioning/qbvh/traversal.rs index 59a7a4e5..c9dc0312 100644 --- a/src/partitioning/qbvh/traversal.rs +++ b/src/partitioning/qbvh/traversal.rs @@ -20,7 +20,7 @@ use { use super::{IndexedData, NodeIndex, QBVH}; -impl QBVH { +impl QBVH { /// Performs a depth-first traversal on the BVH. pub fn traverse_depth_first(&self, visitor: &mut impl SimdVisitor) { self.traverse_depth_first_with_stack(visitor, &mut Vec::new()) @@ -186,18 +186,18 @@ impl QBVH { } /// Performs a simultaneous traversal of two QBVH. - pub fn traverse_bvtt( + pub fn traverse_bvtt( &self, - qbvh2: &QBVH, + qbvh2: &QBVH, visitor: &mut impl SimdSimultaneousVisitor, ) { self.traverse_bvtt_with_stack(qbvh2, visitor, &mut Vec::new()) } /// Performs a simultaneous traversal of two QBVH. - pub fn traverse_bvtt_with_stack( + pub fn traverse_bvtt_with_stack( &self, - qbvh2: &QBVH, + qbvh2: &QBVH, visitor: &mut impl SimdSimultaneousVisitor, stack: &mut Vec<(u32, u32)>, ) { @@ -283,13 +283,10 @@ impl QBVH { } #[cfg(feature = "parallel")] -impl QBVH { +impl QBVH { /// Performs a depth-first traversal of two QBVH using /// parallelism internally for better performances with large tree. - pub fn traverse_depth_first_parallel( - &self, - visitor: &impl ParallelSimdVisitor, - ) { + pub fn traverse_depth_first_parallel(&self, visitor: &impl ParallelSimdVisitor) { if !self.nodes.is_empty() { let exit_early = AtomicBool::new(false); self.traverse_depth_first_node_parallel(visitor, &exit_early, 0); @@ -298,7 +295,7 @@ impl QBVH { pub fn traverse_depth_first_node_parallel( &self, - visitor: &impl ParallelSimdVisitor, + visitor: &impl ParallelSimdVisitor, exit_early: &AtomicBool, entry: u32, ) { @@ -346,10 +343,10 @@ impl QBVH { /// Performs a simultaneous traversal of two QBVH using /// parallelism internally for better performances with large tree. - pub fn traverse_bvtt_parallel( + pub fn traverse_bvtt_parallel( &self, - qbvh2: &QBVH, - visitor: &impl ParallelSimdSimultaneousVisitor, + qbvh2: &QBVH, + visitor: &impl ParallelSimdSimultaneousVisitor, ) { if !self.nodes.is_empty() && !qbvh2.nodes.is_empty() { let exit_early = AtomicBool::new(false); @@ -357,10 +354,10 @@ impl QBVH { } } - pub fn traverse_bvtt_simd_node_parallel( + pub fn traverse_bvtt_simd_node_parallel( &self, - qbvh2: &QBVH, - visitor: &impl ParallelSimdSimultaneousVisitor, + qbvh2: &QBVH, + visitor: &impl ParallelSimdSimultaneousVisitor, exit_early: &AtomicBool, entry: (u32, u32), ) { diff --git a/src/partitioning/qbvh/update.rs b/src/partitioning/qbvh/update.rs index dbc11575..e45c4614 100644 --- a/src/partitioning/qbvh/update.rs +++ b/src/partitioning/qbvh/update.rs @@ -15,15 +15,15 @@ struct QBVHIncrementalBuilderStep { } #[allow(dead_code)] -struct QBVHIncrementalBuilder { - qbvh: QBVH, +struct QBVHIncrementalBuilder { + qbvh: QBVH, to_insert: Vec, aabbs: Vec, indices: Vec, } #[allow(dead_code)] -impl QBVHIncrementalBuilder { +impl QBVHIncrementalBuilder { pub fn new() -> Self { Self { qbvh: QBVH::new(), @@ -54,7 +54,6 @@ impl QBVHIncrementalBuilder QBVHIncrementalBuilder QBVHIncrementalBuilder QBVH { +impl QBVH { /// Marks a piece of data as dirty so it can be updated during the next /// call to `self.update`. pub fn pre_update(&mut self, data: LeafData) { diff --git a/src/partitioning/visitor.rs b/src/partitioning/visitor.rs index c1de36ef..6d883879 100644 --- a/src/partitioning/visitor.rs +++ b/src/partitioning/visitor.rs @@ -101,7 +101,7 @@ pub trait SimdSimultaneousVisitor { */ /// Trait implemented by visitor called during the parallel traversal of a spatial partitioning data structure. -pub trait ParallelSimdVisitor: Sync { +pub trait ParallelSimdVisitor: Sync { /// Execute an operation on the content of a node of the spatial partitioning structure. /// /// Returns whether the traversal should continue on the node's children, if it should not continue @@ -109,19 +109,19 @@ pub trait ParallelSimdVisitor: Sync { fn visit( &self, node_id: SimdNodeIndex, - bv: &QBVHNode, + bv: &QBVHNode, data: Option<[Option<&LeafData>; SIMD_WIDTH]>, ) -> SimdVisitStatus; } -impl ParallelSimdVisitor for F +impl ParallelSimdVisitor for F where - F: Sync + Fn(&QBVHNode, Option<[Option<&LeafData>; SIMD_WIDTH]>) -> SimdVisitStatus, + F: Sync + Fn(&QBVHNode, Option<[Option<&LeafData>; SIMD_WIDTH]>) -> SimdVisitStatus, { fn visit( &self, node_id: SimdNodeIndex, - node: &QBVHNode, + node: &QBVHNode, data: Option<[Option<&LeafData>; SIMD_WIDTH]>, ) -> SimdVisitStatus { (self)(node, data) @@ -131,18 +131,16 @@ where /// Trait implemented by visitor called during a parallel simultaneous spatial partitioning /// data structure traversal. #[cfg(feature = "parallel")] -pub trait ParallelSimdSimultaneousVisitor: - Sync -{ +pub trait ParallelSimdSimultaneousVisitor: Sync { /// Execute an operation on the content of two nodes, one from each structure. /// /// Returns whether the traversal should continue on the nodes children, if it should not continue /// on those children, or if the whole traversal should be exited early. fn visit( &self, - left_node: &QBVHNode, + left_node: &QBVHNode, left_data: Option<[Option<&LeafData1>; SIMD_WIDTH]>, - right_node: &QBVHNode, + right_node: &QBVHNode, right_data: Option<[Option<&LeafData2>; SIMD_WIDTH]>, ) -> SimdSimultaneousVisitStatus; } diff --git a/src/query/visitors/bounding_volume_intersections_simultaneous_visitor.rs b/src/query/visitors/bounding_volume_intersections_simultaneous_visitor.rs index c512d6a1..6c95488f 100644 --- a/src/query/visitors/bounding_volume_intersections_simultaneous_visitor.rs +++ b/src/query/visitors/bounding_volume_intersections_simultaneous_visitor.rs @@ -78,8 +78,8 @@ where } #[cfg(feature = "parallel")] -impl - crate::partitioning::ParallelSimdSimultaneousVisitor +impl + crate::partitioning::ParallelSimdSimultaneousVisitor for BoundingVolumeIntersectionsSimultaneousVisitor where F: Sync + Fn(&LeafData1, &LeafData2) -> bool, @@ -87,9 +87,9 @@ where #[inline] fn visit( &self, - left_node: &QBVHNode, + left_node: &QBVHNode, left_data: Option<[Option<&LeafData1>; SIMD_WIDTH]>, - right_node: &QBVHNode, + right_node: &QBVHNode, right_data: Option<[Option<&LeafData2>; SIMD_WIDTH]>, ) -> SimdSimultaneousVisitStatus { let mask = if let Some(pos12) = &self.pos12 { From 6243678fcc17a92c31dfe5ba6ef2af2cb49b8f8f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Tue, 16 Aug 2022 15:34:29 +0200 Subject: [PATCH 12/17] Fix warnings --- src/partitioning/qbvh/build.rs | 5 ++ src/partitioning/qbvh/mod.rs | 2 - src/partitioning/qbvh/qbvh.rs | 9 +++- src/partitioning/qbvh/traversal.rs | 49 +++++++++++++++++-- src/partitioning/visitor.rs | 5 +- .../contact/contact_composite_shape_shape.rs | 2 +- ...nifolds_composite_shape_composite_shape.rs | 4 +- ...contact_manifolds_composite_shape_shape.rs | 2 +- ...t_manifolds_heightfield_composite_shape.rs | 2 +- src/query/point/point_composite_shape.rs | 6 +-- src/query/split/split_trimesh.rs | 2 +- ...lume_intersections_simultaneous_visitor.rs | 7 +-- src/utils/cuda_array.rs | 10 ++++ 13 files changed, 82 insertions(+), 23 deletions(-) diff --git a/src/partitioning/qbvh/build.rs b/src/partitioning/qbvh/build.rs index c6465da2..954ff30e 100644 --- a/src/partitioning/qbvh/build.rs +++ b/src/partitioning/qbvh/build.rs @@ -41,7 +41,12 @@ pub trait QBVHDataSplitter { ) -> [&'idx mut [usize]; 4]; } +/// A data splitter that arranges a set of AABBs in two sets based on their center’s coordinate +/// along the split axis. pub struct CenterDataSplitter { + /// If all the AABB centers have the same coordinate values along the splitting axis + /// setting this to `true` will allow the spliter to split the AABB set into two + /// subsets arbitrarily. pub enable_fallback_split: bool, } diff --git a/src/partitioning/qbvh/mod.rs b/src/partitioning/qbvh/mod.rs index ff30f136..ab420039 100644 --- a/src/partitioning/qbvh/mod.rs +++ b/src/partitioning/qbvh/mod.rs @@ -1,5 +1,3 @@ -pub(self) use self::qbvh::*; - pub use self::build::{ BuilderProxies, CenterDataSplitter, QBVHDataGenerator, QbvhNonOverlappingDataSplitter, }; diff --git a/src/partitioning/qbvh/qbvh.rs b/src/partitioning/qbvh/qbvh.rs index 3bb2c763..d77397e6 100644 --- a/src/partitioning/qbvh/qbvh.rs +++ b/src/partitioning/qbvh/qbvh.rs @@ -39,6 +39,7 @@ impl IndexedData for u64 { } } +/// The index of an internal SIMD node of a QBVH. pub type SimdNodeIndex = u32; /// The index of a node part of a QBVH. @@ -48,9 +49,12 @@ pub type SimdNodeIndex = u32; feature = "rkyv", derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] +/// The index of one specific node of a QBVH. pub struct NodeIndex { + /// The index of the SIMD node containing the addressed node. pub index: SimdNodeIndex, // Index of the addressed node in the `nodes` array. - pub lane: u8, // SIMD lane of the addressed node. + /// The SIMD lane the addressed node is associated to. + pub lane: u8, // SIMD lane of the addressed node. } impl NodeIndex { @@ -94,8 +98,11 @@ pub struct QBVHNode { feature = "rkyv", derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] +/// Combination of a leaf data and its associated node’s index. pub struct QBVHProxy { + /// Index of the leaf node the leaf data is associated to. pub node: NodeIndex, + /// The data contained in this node. pub data: LeafData, // The collider data. TODO: only set the collider generation here? } diff --git a/src/partitioning/qbvh/traversal.rs b/src/partitioning/qbvh/traversal.rs index c9dc0312..d79cfd48 100644 --- a/src/partitioning/qbvh/traversal.rs +++ b/src/partitioning/qbvh/traversal.rs @@ -22,20 +22,55 @@ use super::{IndexedData, NodeIndex, QBVH}; impl QBVH { /// Performs a depth-first traversal on the BVH. - pub fn traverse_depth_first(&self, visitor: &mut impl SimdVisitor) { - self.traverse_depth_first_with_stack(visitor, &mut Vec::new()) + /// + /// # Return + /// + /// Returns `false` if the traversal exitted early, and `true` otherwise. + pub fn traverse_depth_first(&self, visitor: &mut impl SimdVisitor) -> bool { + self.traverse_depth_first_node(visitor, 0) + } + + /// Performs a depth-first traversal on the BVH, starting at the given node. + /// + /// # Return + /// + /// Returns `false` if the traversal exitted early, and `true` otherwise. + pub fn traverse_depth_first_node( + &self, + visitor: &mut impl SimdVisitor, + start_node: u32, + ) -> bool { + self.traverse_depth_first_node_with_stack(visitor, &mut Vec::new(), start_node) } /// Performs a depth-first traversal on the BVH. + /// + /// # Return + /// + /// Returns `false` if the traversal exited early, and `true` otherwise. pub fn traverse_depth_first_with_stack( &self, visitor: &mut impl SimdVisitor, stack: &mut Vec, - ) { + ) -> bool { + self.traverse_depth_first_node_with_stack(visitor, stack, 0) + } + + /// Performs a depth-first traversal on the BVH. + /// + /// # Return + /// + /// Returns `false` if the traversal exited early, and `true` otherwise. + pub fn traverse_depth_first_node_with_stack( + &self, + visitor: &mut impl SimdVisitor, + stack: &mut Vec, + start_node: u32, + ) -> bool { stack.clear(); if !self.nodes.is_empty() { - stack.push(0); + stack.push(start_node); } while let Some(entry) = stack.pop() { let node = &self.nodes[entry as usize]; @@ -49,7 +84,7 @@ impl QBVH { match visitor.visit(&node.simd_aabb, leaf_data) { SimdVisitStatus::ExitEarly => { - return; + return false; } SimdVisitStatus::MaybeContinue(mask) => { let bitmask = mask.bitmask(); @@ -69,6 +104,8 @@ impl QBVH { } } } + + true } /// Performs a best-first-search on the BVH. @@ -293,6 +330,7 @@ impl QBVH { } } + /// Runs a parallel depth-first traversal of the sub-tree starting at the given node. pub fn traverse_depth_first_node_parallel( &self, visitor: &impl ParallelSimdVisitor, @@ -354,6 +392,7 @@ impl QBVH { } } + /// Runs a parallel simultaneous traversal of the sub-tree starting at the given nodes. pub fn traverse_bvtt_simd_node_parallel( &self, qbvh2: &QBVH, diff --git a/src/partitioning/visitor.rs b/src/partitioning/visitor.rs index 6d883879..73bb7706 100644 --- a/src/partitioning/visitor.rs +++ b/src/partitioning/visitor.rs @@ -1,7 +1,6 @@ -use crate::bounding_volume::SimdAABB; use crate::math::{Real, SimdBool, SimdReal, SIMD_WIDTH}; use crate::partitioning::qbvh::QBVHNode; -use crate::partitioning::{NodeIndex, SimdNodeIndex, QBVH}; +use crate::partitioning::SimdNodeIndex; /// The next action to be taken by a BVH traversal algorithm after having visited a node with some data. pub enum SimdBestFirstVisitStatus { @@ -120,7 +119,7 @@ where { fn visit( &self, - node_id: SimdNodeIndex, + _node_id: SimdNodeIndex, node: &QBVHNode, data: Option<[Option<&LeafData>; SIMD_WIDTH]>, ) -> SimdVisitStatus { diff --git a/src/query/contact/contact_composite_shape_shape.rs b/src/query/contact/contact_composite_shape_shape.rs index 9a8078fe..40418991 100644 --- a/src/query/contact/contact_composite_shape_shape.rs +++ b/src/query/contact/contact_composite_shape_shape.rs @@ -41,7 +41,7 @@ where }; let mut visitor = BoundingVolumeIntersectionsVisitor::new(&ls_aabb2, &mut leaf_callback); - g1.qbvh().traverse_depth_first(&mut visitor); + let _ = g1.qbvh().traverse_depth_first(&mut visitor); res } diff --git a/src/query/contact_manifolds/contact_manifolds_composite_shape_composite_shape.rs b/src/query/contact_manifolds/contact_manifolds_composite_shape_composite_shape.rs index d35dfcd6..f3680bbb 100644 --- a/src/query/contact_manifolds/contact_manifolds_composite_shape_composite_shape.rs +++ b/src/query/contact_manifolds/contact_manifolds_composite_shape_composite_shape.rs @@ -173,14 +173,14 @@ pub fn contact_manifolds_composite_shape_composite_shape<'a, ManifoldData, Conta let mut visitor2 = BoundingVolumeIntersectionsVisitor::new(&ls_part_aabb1_2, &mut leaf_fn2); - qbvh2.traverse_depth_first_with_stack(&mut visitor2, &mut stack2); + let _ = qbvh2.traverse_depth_first_with_stack(&mut visitor2, &mut stack2); }); true }; let mut visitor1 = BoundingVolumeIntersectionsVisitor::new(&ls_aabb2_1, &mut leaf_fn1); - qbvh1.traverse_depth_first(&mut visitor1); + let _ = qbvh1.traverse_depth_first(&mut visitor1); workspace .sub_detectors diff --git a/src/query/contact_manifolds/contact_manifolds_composite_shape_shape.rs b/src/query/contact_manifolds/contact_manifolds_composite_shape_shape.rs index ddda2685..1f3ca8f6 100644 --- a/src/query/contact_manifolds/contact_manifolds_composite_shape_shape.rs +++ b/src/query/contact_manifolds/contact_manifolds_composite_shape_shape.rs @@ -144,7 +144,7 @@ pub fn contact_manifolds_composite_shape_shape( }; let mut visitor1 = BoundingVolumeIntersectionsVisitor::new(&ls_aabb2_1, &mut leaf1_fn); - composite1.qbvh().traverse_depth_first(&mut visitor1); + let _ = composite1.qbvh().traverse_depth_first(&mut visitor1); workspace .sub_detectors diff --git a/src/query/contact_manifolds/contact_manifolds_heightfield_composite_shape.rs b/src/query/contact_manifolds/contact_manifolds_heightfield_composite_shape.rs index 102c777d..e09187e5 100644 --- a/src/query/contact_manifolds/contact_manifolds_heightfield_composite_shape.rs +++ b/src/query/contact_manifolds/contact_manifolds_heightfield_composite_shape.rs @@ -151,7 +151,7 @@ pub fn contact_manifolds_heightfield_composite_shape( }; let mut visitor2 = BoundingVolumeIntersectionsVisitor::new(&ls_aabb1_2, &mut leaf_fn2); - qbvh2.traverse_depth_first_with_stack(&mut visitor2, &mut stack2); + let _ = qbvh2.traverse_depth_first_with_stack(&mut visitor2, &mut stack2); }); workspace diff --git a/src/query/point/point_composite_shape.rs b/src/query/point/point_composite_shape.rs index 6e57445c..d618dbfc 100644 --- a/src/query/point/point_composite_shape.rs +++ b/src/query/point/point_composite_shape.rs @@ -39,7 +39,7 @@ impl PointQuery for Polyline { #[inline] fn contains_local_point(&self, point: &Point) -> bool { let mut visitor = CompositePointContainmentTest::new(self, point); - self.qbvh().traverse_depth_first(&mut visitor); + let _ = self.qbvh().traverse_depth_first(&mut visitor); visitor.found } } @@ -84,7 +84,7 @@ impl PointQuery for TriMesh { } let mut visitor = CompositePointContainmentTest::new(self, point); - self.qbvh().traverse_depth_first(&mut visitor); + let _ = self.qbvh().traverse_depth_first(&mut visitor); visitor.found } } @@ -107,7 +107,7 @@ impl PointQuery for Compound { #[inline] fn contains_local_point(&self, point: &Point) -> bool { let mut visitor = CompositePointContainmentTest::new(self, point); - self.qbvh().traverse_depth_first(&mut visitor); + let _ = self.qbvh().traverse_depth_first(&mut visitor); visitor.found } } diff --git a/src/query/split/split_trimesh.rs b/src/query/split/split_trimesh.rs index 38909b92..e91ff2d9 100644 --- a/src/query/split/split_trimesh.rs +++ b/src/query/split/split_trimesh.rs @@ -419,7 +419,7 @@ impl TriMesh { intersecting_tris.push(*id); true }); - self.qbvh().traverse_depth_first(&mut visitor); + let _ = self.qbvh().traverse_depth_first(&mut visitor); if intersecting_tris.is_empty() { return None; diff --git a/src/query/visitors/bounding_volume_intersections_simultaneous_visitor.rs b/src/query/visitors/bounding_volume_intersections_simultaneous_visitor.rs index 6c95488f..ed03779f 100644 --- a/src/query/visitors/bounding_volume_intersections_simultaneous_visitor.rs +++ b/src/query/visitors/bounding_volume_intersections_simultaneous_visitor.rs @@ -1,12 +1,13 @@ use crate::bounding_volume::SimdAABB; use crate::math::{Isometry, Real, SimdReal, SIMD_WIDTH}; -use crate::partitioning::{ - QBVHNode, SimdNodeIndex, SimdSimultaneousVisitStatus, SimdSimultaneousVisitor, QBVH, -}; +use crate::partitioning::{SimdSimultaneousVisitStatus, SimdSimultaneousVisitor}; use na::SimdValue; use simba::simd::SimdBool as _; use std::marker::PhantomData; +#[cfg(feature = "parallel")] +use crate::partitioning::QBVHNode; + /// Spatial partitioning data structure visitor collecting interferences with a given bounding volume. pub struct BoundingVolumeIntersectionsSimultaneousVisitor { pos12: Option>, diff --git a/src/utils/cuda_array.rs b/src/utils/cuda_array.rs index 43453686..5ea4d178 100644 --- a/src/utils/cuda_array.rs +++ b/src/utils/cuda_array.rs @@ -12,6 +12,7 @@ use cust_core::DeviceCopy; * */ #[cfg(feature = "std")] +/// A 2D array residing on GPU memory. pub struct CudaArray2 { data: DeviceBuffer, nrows: usize, @@ -20,6 +21,7 @@ pub struct CudaArray2 { #[cfg(feature = "std")] impl CudaArray2 { + /// Initialize a 2D cuda array on the GPU. pub fn new(data: &[T], nrows: usize, ncols: usize) -> CudaResult { assert_eq!( data.len(), @@ -29,10 +31,12 @@ impl CudaArray2 { DeviceBuffer::from_slice(data).map(|data| Self { data, nrows, ncols }) } + /// Initialize, using a matrix, a 2D cuda array on the GPU. pub fn from_matrix(mat: &na::DMatrix) -> CudaResult { Self::new(mat.as_slice(), mat.nrows(), mat.ncols()) } + /// Gets the device pointer to the CUDA memory. pub fn as_device_ptr(&self) -> CudaArrayPointer2 { CudaArrayPointer2 { data: self.data.as_device_ptr(), @@ -44,6 +48,7 @@ impl CudaArray2 { #[repr(C)] #[derive(Copy, Clone, cust_core::DeviceCopy)] +/// A pointer to a 2D CUDA array. pub struct CudaArrayPointer2 { data: DevicePointer, nrows: usize, @@ -83,20 +88,24 @@ impl HeightFieldStorage for CudaArrayPointer2 { * */ #[cfg(feature = "std")] +/// A 1D array residing on GPU memory. pub struct CudaArray1 { data: DeviceBuffer, } #[cfg(feature = "std")] impl CudaArray1 { + /// Initialize a 1D cuda array on the GPU. pub fn new(data: &[T]) -> CudaResult { DeviceBuffer::from_slice(data).map(|data| Self { data }) } + /// Initialize a 1D cuda array on the GPU using a dynamically-sized vector. pub fn from_vector(vect: &na::DVector) -> CudaResult { Self::new(vect.as_slice()) } + /// Gets the device pointer to the CUDA memory. pub fn as_device_ptr(&self) -> CudaArrayPointer1 { CudaArrayPointer1 { data: self.data.as_device_ptr(), @@ -107,6 +116,7 @@ impl CudaArray1 { #[repr(C)] #[derive(Copy, Clone, cust_core::DeviceCopy)] +/// A pointer to a 2D CUDA array. pub struct CudaArrayPointer1 { data: DevicePointer, len: usize, From 1a336f2d1b13f06b57a802a8e52977fcfe1fbd02 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Fri, 2 Sep 2022 16:37:28 +0200 Subject: [PATCH 13/17] Add node id and associated data to the parallel simultaneous QBVH visitor --- src/partitioning/qbvh/traversal.rs | 31 ++++++++++++++----- src/partitioning/visitor.rs | 8 ++++- ...lume_intersections_simultaneous_visitor.rs | 13 +++++--- 3 files changed, 40 insertions(+), 12 deletions(-) diff --git a/src/partitioning/qbvh/traversal.rs b/src/partitioning/qbvh/traversal.rs index d79cfd48..13e2035e 100644 --- a/src/partitioning/qbvh/traversal.rs +++ b/src/partitioning/qbvh/traversal.rs @@ -381,23 +381,36 @@ impl QBVH { /// Performs a simultaneous traversal of two QBVH using /// parallelism internally for better performances with large tree. - pub fn traverse_bvtt_parallel( + pub fn traverse_bvtt_parallel< + LeafData2: IndexedData + Sync, + Visitor: ParallelSimdSimultaneousVisitor, + >( &self, qbvh2: &QBVH, - visitor: &impl ParallelSimdSimultaneousVisitor, + visitor: &Visitor, ) { if !self.nodes.is_empty() && !qbvh2.nodes.is_empty() { let exit_early = AtomicBool::new(false); - self.traverse_bvtt_simd_node_parallel(qbvh2, visitor, &exit_early, (0, 0)); + self.traverse_bvtt_simd_node_parallel( + qbvh2, + visitor, + &exit_early, + Visitor::Data::default(), + (0, 0), + ); } } /// Runs a parallel simultaneous traversal of the sub-tree starting at the given nodes. - pub fn traverse_bvtt_simd_node_parallel( + pub fn traverse_bvtt_simd_node_parallel< + LeafData2: IndexedData + Sync, + Visitor: ParallelSimdSimultaneousVisitor, + >( &self, qbvh2: &QBVH, - visitor: &impl ParallelSimdSimultaneousVisitor, + visitor: &Visitor, exit_early: &AtomicBool, + data: Visitor::Data, entry: (u32, u32), ) { if exit_early.load(AtomicOrdering::Relaxed) { @@ -427,7 +440,11 @@ impl QBVH { None }; - match visitor.visit(&node1, leaf_data1, &node2, leaf_data2) { + let (status, data) = visitor.visit( + entry.0, &node1, leaf_data1, entry.1, &node2, leaf_data2, data, + ); + + match status { SimdSimultaneousVisitStatus::ExitEarly => { exit_early.store(true, AtomicOrdering::Relaxed); return; @@ -480,7 +497,7 @@ impl QBVH { } stack.as_slice().par_iter().copied().for_each(|entry| { - self.traverse_bvtt_simd_node_parallel(qbvh2, visitor, exit_early, entry) + self.traverse_bvtt_simd_node_parallel(qbvh2, visitor, exit_early, data, entry) }); } } diff --git a/src/partitioning/visitor.rs b/src/partitioning/visitor.rs index 73bb7706..f364a5c7 100644 --- a/src/partitioning/visitor.rs +++ b/src/partitioning/visitor.rs @@ -131,15 +131,21 @@ where /// data structure traversal. #[cfg(feature = "parallel")] pub trait ParallelSimdSimultaneousVisitor: Sync { + /// Visitor state data that will be passed down the recursion. + type Data: Copy + Sync + Default; + /// Execute an operation on the content of two nodes, one from each structure. /// /// Returns whether the traversal should continue on the nodes children, if it should not continue /// on those children, or if the whole traversal should be exited early. fn visit( &self, + left_node_id: SimdNodeIndex, left_node: &QBVHNode, left_data: Option<[Option<&LeafData1>; SIMD_WIDTH]>, + right_node_id: SimdNodeIndex, right_node: &QBVHNode, right_data: Option<[Option<&LeafData2>; SIMD_WIDTH]>, - ) -> SimdSimultaneousVisitStatus; + visitor_data: Self::Data, + ) -> (SimdSimultaneousVisitStatus, Self::Data); } diff --git a/src/query/visitors/bounding_volume_intersections_simultaneous_visitor.rs b/src/query/visitors/bounding_volume_intersections_simultaneous_visitor.rs index ed03779f..1d5098b1 100644 --- a/src/query/visitors/bounding_volume_intersections_simultaneous_visitor.rs +++ b/src/query/visitors/bounding_volume_intersections_simultaneous_visitor.rs @@ -6,7 +6,7 @@ use simba::simd::SimdBool as _; use std::marker::PhantomData; #[cfg(feature = "parallel")] -use crate::partitioning::QBVHNode; +use crate::partitioning::{QBVHNode, SimdNodeIndex}; /// Spatial partitioning data structure visitor collecting interferences with a given bounding volume. pub struct BoundingVolumeIntersectionsSimultaneousVisitor { @@ -85,14 +85,19 @@ impl where F: Sync + Fn(&LeafData1, &LeafData2) -> bool, { + type Data = (); + #[inline] fn visit( &self, + _: SimdNodeIndex, left_node: &QBVHNode, left_data: Option<[Option<&LeafData1>; SIMD_WIDTH]>, + _: SimdNodeIndex, right_node: &QBVHNode, right_data: Option<[Option<&LeafData2>; SIMD_WIDTH]>, - ) -> SimdSimultaneousVisitStatus { + _: (), + ) -> (SimdSimultaneousVisitStatus, ()) { let mask = if let Some(pos12) = &self.pos12 { let transformed_right_bv = right_node.simd_aabb.transform_by(pos12); left_node @@ -111,13 +116,13 @@ where for jj in 0..SIMD_WIDTH { if (bitmask & (1 << jj)) != 0 && data1[ii].is_some() && data2[jj].is_some() { if !(self.callback)(data1[ii].unwrap(), data2[jj].unwrap()) { - return SimdSimultaneousVisitStatus::ExitEarly; + return (SimdSimultaneousVisitStatus::ExitEarly, ()); } } } } } - SimdSimultaneousVisitStatus::MaybeContinue(mask) + (SimdSimultaneousVisitStatus::MaybeContinue(mask), ()) } } From aad6aed5824acf768783a48d2fbd798f73030d4e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Wed, 14 Sep 2022 09:52:21 +0200 Subject: [PATCH 14/17] Add traverse_best_first_node to start the traversal at a specified node id --- src/partitioning/qbvh/traversal.rs | 21 +++++++++++++++++++-- 1 file changed, 19 insertions(+), 2 deletions(-) diff --git a/src/partitioning/qbvh/traversal.rs b/src/partitioning/qbvh/traversal.rs index 13e2035e..9420f4a0 100644 --- a/src/partitioning/qbvh/traversal.rs +++ b/src/partitioning/qbvh/traversal.rs @@ -121,11 +121,28 @@ impl QBVH { return None; } + self.traverse_best_first_node(visitor, 0, Real::max_value()) + } + + /// Performs a best-first-search on the BVH, starting at the given node. + /// + /// Returns the content of the leaf with the smallest associated cost, and a result of + /// user-defined type. + pub fn traverse_best_first_node( + &self, + visitor: &mut BFS, + start_node: u32, + init_cost: Real, + ) -> Option<(NodeIndex, BFS::Result)> + where + BFS: SimdBestFirstVisitor, + BFS::Result: Clone, // Because we cannot move out of an array… + { let mut queue: BinaryHeap> = BinaryHeap::new(); - let mut best_cost = Real::max_value(); + let mut best_cost = init_cost; let mut best_result = None; - queue.push(WeightedValue::new(0, -best_cost / 2.0)); + queue.push(WeightedValue::new(start_node, -best_cost / 2.0)); while let Some(entry) = queue.pop() { if -entry.cost >= best_cost { From 72251c0406b939658e4d46a0282ab13ffbf26f5a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Tue, 27 Sep 2022 09:51:38 +0200 Subject: [PATCH 15/17] Add pub methods len,get,set to CudaArrayPointer1 --- src/utils/cuda_array.rs | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/src/utils/cuda_array.rs b/src/utils/cuda_array.rs index 5ea4d178..ac43699e 100644 --- a/src/utils/cuda_array.rs +++ b/src/utils/cuda_array.rs @@ -122,6 +122,25 @@ pub struct CudaArrayPointer1 { len: usize, } +#[cfg(target_os = "cuda")] +impl CudaArrayPointer1 { + pub fn len(&self) -> usize { + self.len + } + + pub fn get(&self, i: usize) -> T { + assert!(i < self.len); + unsafe { *self.data.as_ptr().add(i) } + } + + pub fn set(&mut self, i: usize, val: T) { + assert!(i < self.len); + unsafe { + *self.data.as_mut_ptr().add(i) = val; + } + } +} + #[cfg(all(feature = "dim2", target_os = "cuda"))] impl HeightFieldStorage for CudaArrayPointer1 { type Item = T; From de033d50dc5d450d2f8d85ae510132b5f0376236 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sun, 2 Oct 2022 17:02:50 +0200 Subject: [PATCH 16/17] Update CHANGELOG --- CHANGELOG.md | 6 +++++- src/partitioning/qbvh/traversal.rs | 6 +++--- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 514a6636..04616aa3 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -13,7 +13,11 @@ - 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. - +- Add the support of Heightfields on CUDA kernels written in Rust using the `cust` crate. +- Add the `rkyv-serialize` feature that enables the implementation of `rkyv` serialization/deserialization + for most shapes. +- Add the `parallel` feature that enables methods for the parallel traversal of QBVH trees: `QBVH::traverse_bvtt_parallel`, + `QBVH::traverse_bvtt_node_parallel`, `QBVH::traverse_depth_first_parallel`, `QBVH::traverse_depth_first_node_parallel`, ### Fixed - Fix the application of non-uniform scaling to balls. diff --git a/src/partitioning/qbvh/traversal.rs b/src/partitioning/qbvh/traversal.rs index 9420f4a0..2eb7b487 100644 --- a/src/partitioning/qbvh/traversal.rs +++ b/src/partitioning/qbvh/traversal.rs @@ -408,7 +408,7 @@ impl QBVH { ) { if !self.nodes.is_empty() && !qbvh2.nodes.is_empty() { let exit_early = AtomicBool::new(false); - self.traverse_bvtt_simd_node_parallel( + self.traverse_bvtt_node_parallel( qbvh2, visitor, &exit_early, @@ -419,7 +419,7 @@ impl QBVH { } /// Runs a parallel simultaneous traversal of the sub-tree starting at the given nodes. - pub fn traverse_bvtt_simd_node_parallel< + pub fn traverse_bvtt_node_parallel< LeafData2: IndexedData + Sync, Visitor: ParallelSimdSimultaneousVisitor, >( @@ -514,7 +514,7 @@ impl QBVH { } stack.as_slice().par_iter().copied().for_each(|entry| { - self.traverse_bvtt_simd_node_parallel(qbvh2, visitor, exit_early, data, entry) + self.traverse_bvtt_node_parallel(qbvh2, visitor, exit_early, data, entry) }); } } From 3f29e5e59f3a7be161a79dfbef671d0e0286d081 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sun, 2 Oct 2022 17:15:06 +0200 Subject: [PATCH 17/17] Use simba > 0.7.2 and remove cargo patch --- Cargo.toml | 2 +- crates/parry2d-f64/Cargo.toml | 2 +- crates/parry2d/Cargo.toml | 2 +- crates/parry3d-f64/Cargo.toml | 2 +- crates/parry3d/Cargo.toml | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index 6abca77b..904c7f75 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -9,5 +9,5 @@ parry2d-f64 = { path = "crates/parry2d-f64" } parry3d-f64 = { path = "crates/parry3d-f64" } #simba = { path = "../simba" } -simba = { git = "https://github.com/dimforge/simba", rev = "b1392df62a0f4cf91e397bbb6bd41b7731afb6ab" } +#simba = { git = "https://github.com/dimforge/simba", rev = "b1392df62a0f4cf91e397bbb6bd41b7731afb6ab" } # nalgebra = { git = "https://github.com/dimforge/nalgebra" } \ No newline at end of file diff --git a/crates/parry2d-f64/Cargo.toml b/crates/parry2d-f64/Cargo.toml index 93334212..dc151597 100644 --- a/crates/parry2d-f64/Cargo.toml +++ b/crates/parry2d-f64/Cargo.toml @@ -47,7 +47,7 @@ num-traits = { version = "0.2", default-features = false } smallvec = "1" slab = { version = "0.4", optional = true } arrayvec = { version = "0.7", default-features = false } -simba = { version = "0.7", default-features = false } +simba = { version = "^0.7.2", default-features = false } nalgebra = { version = "0.31", default-features = false, features = [ "libm" ] } approx = { version = "0.5", default-features = false } serde = { version = "1.0", optional = true, features = ["derive"] } diff --git a/crates/parry2d/Cargo.toml b/crates/parry2d/Cargo.toml index f5721bfc..5befc83e 100644 --- a/crates/parry2d/Cargo.toml +++ b/crates/parry2d/Cargo.toml @@ -47,7 +47,7 @@ num-traits = { version = "0.2", default-features = false } smallvec = "1" slab = { version = "0.4", optional = true } arrayvec = { version = "0.7", default-features = false } -simba = { version = "0.7", default-features = false } +simba = { version = "^0.7.2", default-features = false } nalgebra = { version = "0.31", default-features = false, features = [ "libm" ] } approx = { version = "0.5", default-features = false } serde = { version = "1.0", optional = true, features = ["derive"] } diff --git a/crates/parry3d-f64/Cargo.toml b/crates/parry3d-f64/Cargo.toml index b512680b..6bf56967 100644 --- a/crates/parry3d-f64/Cargo.toml +++ b/crates/parry3d-f64/Cargo.toml @@ -47,7 +47,7 @@ num-traits = { version = "0.2", default-features = false } smallvec = "1" slab = { version = "0.4", optional = true } arrayvec = { version = "0.7", default-features = false } -simba = { version = "0.7", default-features = false } +simba = { version = "^0.7.2", default-features = false } nalgebra = { version = "0.31", default-features = false, features = [ "libm" ] } approx = { version = "0.5", default-features = false } serde = { version = "1.0", optional = true, features = ["derive", "rc"]} diff --git a/crates/parry3d/Cargo.toml b/crates/parry3d/Cargo.toml index 31dea68f..e7331f5f 100644 --- a/crates/parry3d/Cargo.toml +++ b/crates/parry3d/Cargo.toml @@ -48,7 +48,7 @@ num-traits = { version = "0.2", default-features = false } smallvec = "1" slab = { version = "0.4", optional = true } arrayvec = { version = "0.7", default-features = false } -simba = { version = "0.7", default-features = false } +simba = { version = "^0.7.2", default-features = false } nalgebra = { version = "0.31", default-features = false, features = [ "libm" ] } approx = { version = "0.5", default-features = false } serde = { version = "1.0", optional = true, features = ["derive", "rc"]}