From 0c049725f21fcdc8af36c12bee5fcb0a5865f8a7 Mon Sep 17 00:00:00 2001 From: Benjamin Saunders Date: Wed, 9 Jun 2021 17:29:30 -0700 Subject: [PATCH] Introduce ScaledTriMesh shape --- .../bounding_sphere_trimesh.rs | 16 ++- src/query/point/point_composite_shape.rs | 47 ++++++- src/query/ray/ray_composite_shape.rs | 39 ++++- src/shape/mod.rs | 2 + src/shape/scaled_trimesh.rs | 133 ++++++++++++++++++ src/shape/shape.rs | 59 +++++++- src/shape/trimesh.rs | 2 +- 7 files changed, 292 insertions(+), 6 deletions(-) create mode 100644 src/shape/scaled_trimesh.rs diff --git a/src/bounding_volume/bounding_sphere_trimesh.rs b/src/bounding_volume/bounding_sphere_trimesh.rs index 6b1a4a7a..c9746921 100644 --- a/src/bounding_volume/bounding_sphere_trimesh.rs +++ b/src/bounding_volume/bounding_sphere_trimesh.rs @@ -1,6 +1,6 @@ use crate::bounding_volume::BoundingSphere; use crate::math::{Isometry, Real}; -use crate::shape::TriMesh; +use crate::shape::{ScaledTriMesh, TriMesh}; impl TriMesh { /// Computes the world-space bounding sphere of this triangle mesh, transformed by `pos`. @@ -15,3 +15,17 @@ impl TriMesh { self.local_aabb().bounding_sphere() } } + +impl ScaledTriMesh { + /// Computes the world-space bounding sphere of this triangle mesh, transformed by `pos`. + #[inline] + pub fn bounding_sphere(&self, pos: &Isometry) -> BoundingSphere { + self.local_aabb().bounding_sphere().transform_by(pos) + } + + /// Computes the local-space bounding sphere of this triangle mesh. + #[inline] + pub fn local_bounding_sphere(&self) -> BoundingSphere { + self.local_aabb().bounding_sphere() + } +} diff --git a/src/query/point/point_composite_shape.rs b/src/query/point/point_composite_shape.rs index 54beb6df..0223708e 100644 --- a/src/query/point/point_composite_shape.rs +++ b/src/query/point/point_composite_shape.rs @@ -7,8 +7,8 @@ use crate::query::{ visitors::CompositePointContainmentTest, PointProjection, PointQuery, PointQueryWithLocation, }; use crate::shape::{ - Compound, FeatureId, Polyline, SegmentPointLocation, TriMesh, TrianglePointLocation, - TypedSimdCompositeShape, + Compound, FeatureId, Polyline, ScaledTriMesh, SegmentPointLocation, TriMesh, + TrianglePointLocation, TypedSimdCompositeShape, }; use na; use simba::simd::{SimdBool as _, SimdPartialOrd, SimdValue}; @@ -70,6 +70,34 @@ impl PointQuery for TriMesh { } } +impl PointQuery for ScaledTriMesh { + #[inline] + fn project_local_point(&self, point: &Point, solid: bool) -> PointProjection { + self.project_local_point_and_get_location(point, solid).0 + } + + #[inline] + fn project_local_point_and_get_feature( + &self, + point: &Point, + ) -> (PointProjection, FeatureId) { + let mut visitor = + PointCompositeShapeProjWithFeatureBestFirstVisitor::new(self, point, false); + let (proj, (id, _feature)) = self.quadtree().traverse_best_first(&mut visitor).unwrap().1; + let feature_id = FeatureId::Face(id); + (proj, feature_id) + } + + // FIXME: implement distance_to_point too? + + #[inline] + fn contains_local_point(&self, point: &Point) -> bool { + let mut visitor = CompositePointContainmentTest::new(self, point); + self.quadtree().traverse_depth_first(&mut visitor); + visitor.found + } +} + impl PointQuery for Compound { #[inline] fn project_local_point(&self, point: &Point, solid: bool) -> PointProjection { @@ -127,6 +155,21 @@ impl PointQueryWithLocation for TriMesh { } } +impl PointQueryWithLocation for ScaledTriMesh { + type Location = (u32, TrianglePointLocation); + + #[inline] + fn project_local_point_and_get_location( + &self, + point: &Point, + solid: bool, + ) -> (PointProjection, Self::Location) { + let mut visitor = + PointCompositeShapeProjWithLocationBestFirstVisitor::new(self, point, solid); + self.quadtree().traverse_best_first(&mut visitor).unwrap().1 + } +} + /* * Visitors */ diff --git a/src/query/ray/ray_composite_shape.rs b/src/query/ray/ray_composite_shape.rs index 3bd141c1..60e984f4 100644 --- a/src/query/ray/ray_composite_shape.rs +++ b/src/query/ray/ray_composite_shape.rs @@ -2,7 +2,9 @@ use crate::bounding_volume::SimdAABB; use crate::math::{Real, SimdBool, SimdReal, SIMD_WIDTH}; use crate::partitioning::{SimdBestFirstVisitStatus, SimdBestFirstVisitor}; use crate::query::{Ray, RayCast, RayIntersection, SimdRay}; -use crate::shape::{Compound, FeatureId, Polyline, TriMesh, TypedSimdCompositeShape}; +use crate::shape::{ + Compound, FeatureId, Polyline, ScaledTriMesh, TriMesh, TypedSimdCompositeShape, +}; use simba::simd::{SimdBool as _, SimdPartialOrd, SimdValue}; impl RayCast for TriMesh { @@ -40,6 +42,41 @@ impl RayCast for TriMesh { } } +impl RayCast for ScaledTriMesh { + #[inline] + fn cast_local_ray(&self, ray: &Ray, max_toi: Real, solid: bool) -> Option { + let mut visitor = RayCompositeShapeToiBestFirstVisitor::new(self, ray, max_toi, solid); + + self.quadtree() + .traverse_best_first(&mut visitor) + .map(|res| res.1 .1) + } + + #[inline] + fn cast_local_ray_and_get_normal( + &self, + ray: &Ray, + max_toi: Real, + solid: bool, + ) -> Option { + let mut visitor = + RayCompositeShapeToiAndNormalBestFirstVisitor::new(self, ray, max_toi, solid); + + self.quadtree() + .traverse_best_first(&mut visitor) + .map(|(_, (best, mut res))| { + // We hit a backface. + // NOTE: we need this for `TriMesh::is_backface` to work properly. + if res.feature == FeatureId::Face(1) { + res.feature = FeatureId::Face(best + self.trimesh().indices().len() as u32) + } else { + res.feature = FeatureId::Face(best); + } + res + }) + } +} + impl RayCast for Polyline { #[inline] fn cast_local_ray(&self, ray: &Ray, max_toi: Real, solid: bool) -> Option { diff --git a/src/shape/mod.rs b/src/shape/mod.rs index 7c57566c..a300570d 100644 --- a/src/shape/mod.rs +++ b/src/shape/mod.rs @@ -38,6 +38,7 @@ pub use self::cylinder::Cylinder; pub use self::heightfield3::{HeightField, HeightFieldCellStatus}; #[cfg(feature = "dim3")] pub use self::polygonal_feature3d::PolygonalFeature; +pub use self::scaled_trimesh::ScaledTriMesh; #[cfg(feature = "dim3")] pub use self::tetrahedron::{Tetrahedron, TetrahedronPointLocation}; pub use self::trimesh::TriMesh; @@ -91,6 +92,7 @@ mod heightfield3; #[cfg(feature = "dim3")] mod polygonal_feature3d; mod polygonal_feature_map; +mod scaled_trimesh; #[cfg(feature = "dim3")] mod tetrahedron; mod trimesh; diff --git a/src/shape/scaled_trimesh.rs b/src/shape/scaled_trimesh.rs new file mode 100644 index 00000000..c1907b29 --- /dev/null +++ b/src/shape/scaled_trimesh.rs @@ -0,0 +1,133 @@ +use std::sync::Arc; + +use crate::bounding_volume::AABB; +use crate::math::{Isometry, Point, Real, Vector}; +use crate::partitioning::QBVH; +use crate::shape::composite_shape::SimdCompositeShape; +use crate::shape::TriMesh; +use crate::shape::{Shape, Triangle, TypedSimdCompositeShape}; + +#[derive(Clone)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +/// A scaled [`TriMesh`] +pub struct ScaledTriMesh { + /// The underlying triangle mesh + trimesh: Arc, + /// Scaling factors for each dimension + // This could easily be expanded into an arbitrary transform, if a use case arises. + scaling_factors: Vector, + quadtree: QBVH, +} + +impl ScaledTriMesh { + /// Creates a triangle mesh by scaling `trimesh` along each axis + pub fn new(trimesh: Arc, scaling_factors: Vector) -> Self { + // Future work: Would it be more efficient to scale trimesh.quadtree rather than building a + // new one from scratch? + let data = trimesh.triangles().enumerate().map(|(i, tri)| { + let aabb = scale_tri(&tri, &scaling_factors).local_aabb(); + (i as u32, aabb) + }); + let mut quadtree = QBVH::new(); + quadtree.clear_and_rebuild(data, 0.0); + Self { + trimesh, + scaling_factors, + quadtree, + } + } + + /// The underlying unscaled trimesh + pub fn trimesh(&self) -> &Arc { + &self.trimesh + } + + /// Scaling factors used to derive this shape from the underlying trimesh + pub fn scaling_factors(&self) -> Vector { + self.scaling_factors + } + + /// Compute the axis-aligned bounding box of this triangle mesh. + pub fn aabb(&self, pos: &Isometry) -> AABB { + self.quadtree.root_aabb().transform_by(pos) + } + + /// Gets the local axis-aligned bounding box of this triangle mesh. + pub fn local_aabb(&self) -> &AABB { + self.quadtree.root_aabb() + } + + /// The acceleration structure used by this triangle-mesh. + pub fn quadtree(&self) -> &QBVH { + &self.quadtree + } + + /// An iterator through all the scaled triangles of this mesh. + pub fn triangles(&self) -> impl ExactSizeIterator + '_ { + self.trimesh + .triangles() + .map(move |tri| scale_tri(&tri, &self.scaling_factors)) + } + + /// Get the `i`-th scaled triangle of this mesh. + pub fn triangle(&self, i: u32) -> Triangle { + scale_tri(&self.trimesh.triangle(i), &self.scaling_factors) + } + + /// The vertex buffer of this mesh. + pub fn vertices(&self) -> impl ExactSizeIterator> + '_ { + self.trimesh + .vertices() + .iter() + .map(move |v| v.coords.component_mul(&self.scaling_factors).into()) + } + + /// The index buffer of this mesh. + pub fn indices(&self) -> &[[u32; 3]] { + self.trimesh.indices() + } +} + +fn scale_tri(tri: &Triangle, factors: &Vector) -> Triangle { + Triangle { + a: tri.a.coords.component_mul(factors).into(), + b: tri.b.coords.component_mul(factors).into(), + c: tri.c.coords.component_mul(factors).into(), + } +} + +impl SimdCompositeShape for ScaledTriMesh { + fn map_part_at(&self, i: u32, f: &mut dyn FnMut(Option<&Isometry>, &dyn Shape)) { + let tri = self.triangle(i); + f(None, &tri) + } + + fn quadtree(&self) -> &QBVH { + &self.quadtree + } +} + +impl TypedSimdCompositeShape for ScaledTriMesh { + type PartShape = Triangle; + type PartId = u32; + + #[inline(always)] + fn map_typed_part_at( + &self, + i: u32, + mut f: impl FnMut(Option<&Isometry>, &Self::PartShape), + ) { + let tri = self.triangle(i); + f(None, &tri) + } + + #[inline(always)] + fn map_untyped_part_at(&self, i: u32, mut f: impl FnMut(Option<&Isometry>, &dyn Shape)) { + let tri = self.triangle(i); + f(None, &tri) + } + + fn typed_quadtree(&self) -> &QBVH { + &self.quadtree + } +} diff --git a/src/shape/shape.rs b/src/shape/shape.rs index 01865e97..99170f41 100644 --- a/src/shape/shape.rs +++ b/src/shape/shape.rs @@ -7,7 +7,8 @@ use crate::shape::composite_shape::SimdCompositeShape; use crate::shape::SharedShape; use crate::shape::{ Ball, Capsule, Compound, Cuboid, FeatureId, HalfSpace, HeightField, PolygonalFeatureMap, - Polyline, RoundCuboid, RoundShape, RoundTriangle, Segment, SupportMap, TriMesh, Triangle, + Polyline, RoundCuboid, RoundShape, RoundTriangle, ScaledTriMesh, Segment, SupportMap, TriMesh, + Triangle, }; #[cfg(feature = "dim3")] use crate::shape::{ @@ -35,6 +36,8 @@ pub enum ShapeType { Triangle, /// A triangle mesh shape. TriMesh, + /// A triangle mesh shape with scaling factors. + ScaledTriMesh, /// A set of segments. Polyline, /// A shape representing a full half-space. @@ -96,6 +99,8 @@ pub enum TypedShape<'a> { Triangle(&'a Triangle), /// A triangle mesh shape. TriMesh(&'a TriMesh), + /// A triangle mesh shape with scaling factors. + ScaledTriMesh(&'a ScaledTriMesh), /// A set of segments. Polyline(&'a Polyline), /// A shape representing a full half-space. @@ -953,6 +958,58 @@ impl Shape for TriMesh { } } +impl Shape for ScaledTriMesh { + fn clone_box(&self) -> Box { + Box::new(self.clone()) + } + + fn compute_local_aabb(&self) -> AABB { + *self.local_aabb() + } + + fn compute_local_bounding_sphere(&self) -> BoundingSphere { + self.local_bounding_sphere() + } + + fn compute_aabb(&self, position: &Isometry) -> AABB { + self.aabb(position) + } + + fn mass_properties(&self, _density: Real) -> MassProperties { + #[cfg(feature = "dim2")] + return MassProperties::from_trimesh( + _density * self.scaling_factors().iter().product::(), + self.trimesh().vertices(), + self.trimesh().indices(), + ); + #[cfg(feature = "dim3")] + return MassProperties::zero(); + } + + fn shape_type(&self) -> ShapeType { + ShapeType::ScaledTriMesh + } + + fn as_typed_shape(&self) -> TypedShape { + TypedShape::ScaledTriMesh(self) + } + + fn ccd_thickness(&self) -> Real { + // TODO: in 2D, return the smallest CCD thickness among triangles? + 0.0 + } + + fn ccd_angular_thickness(&self) -> Real { + // TODO: the value should depend on the angles between + // adjacent triangles of the trimesh. + Real::frac_pi_4() + } + + fn as_composite_shape(&self) -> Option<&dyn SimdCompositeShape> { + Some(self as &dyn SimdCompositeShape) + } +} + impl Shape for HeightField { fn clone_box(&self) -> Box { Box::new(self.clone()) diff --git a/src/shape/trimesh.rs b/src/shape/trimesh.rs index 933071ed..c7eeef37 100644 --- a/src/shape/trimesh.rs +++ b/src/shape/trimesh.rs @@ -75,7 +75,7 @@ impl TriMesh { } /// An iterator through all the triangles of this mesh. - pub fn triangles(&self) -> impl Iterator + '_ { + pub fn triangles(&self) -> impl ExactSizeIterator + '_ { self.indices.iter().map(move |ids| { Triangle::new( self.vertices[ids[0] as usize],