Introduce ScaledTriMesh shape

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<Real>) -> 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()
+    }
+}

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<Real>, solid: bool) -> PointProjection {
+        self.project_local_point_and_get_location(point, solid).0
+    }
+
+    #[inline]
+    fn project_local_point_and_get_feature(
+        &self,
+        point: &Point<Real>,
+    ) -> (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<Real>) -> 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<Real>, 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<Real>,
+        solid: bool,
+    ) -> (PointProjection, Self::Location) {
+        let mut visitor =
+            PointCompositeShapeProjWithLocationBestFirstVisitor::new(self, point, solid);
+        self.quadtree().traverse_best_first(&mut visitor).unwrap().1
+    }
+}
+
 /*
  * Visitors
  */

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<Real> {
+        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<RayIntersection> {
+        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<Real> {

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;

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<TriMesh>,
+    /// Scaling factors for each dimension
+    // This could easily be expanded into an arbitrary transform, if a use case arises.
+    scaling_factors: Vector<Real>,
+    quadtree: QBVH<u32>,
+}
+
+impl ScaledTriMesh {
+    /// Creates a triangle mesh by scaling `trimesh` along each axis
+    pub fn new(trimesh: Arc<TriMesh>, scaling_factors: Vector<Real>) -> 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<TriMesh> {
+        &self.trimesh
+    }
+
+    /// Scaling factors used to derive this shape from the underlying trimesh
+    pub fn scaling_factors(&self) -> Vector<Real> {
+        self.scaling_factors
+    }
+
+    /// Compute the axis-aligned bounding box of this triangle mesh.
+    pub fn aabb(&self, pos: &Isometry<Real>) -> 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<u32> {
+        &self.quadtree
+    }
+
+    /// An iterator through all the scaled triangles of this mesh.
+    pub fn triangles(&self) -> impl ExactSizeIterator<Item = Triangle> + '_ {
+        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<Item = Point<Real>> + '_ {
+        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<Real>) -> 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<Real>>, &dyn Shape)) {
+        let tri = self.triangle(i);
+        f(None, &tri)
+    }
+
+    fn quadtree(&self) -> &QBVH<u32> {
+        &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<Real>>, &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<Real>>, &dyn Shape)) {
+        let tri = self.triangle(i);
+        f(None, &tri)
+    }
+
+    fn typed_quadtree(&self) -> &QBVH<u32> {
+        &self.quadtree
+    }
+}

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<dyn Shape> {
+        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<Real>) -> AABB {
+        self.aabb(position)
+    }
+
+    fn mass_properties(&self, _density: Real) -> MassProperties {
+        #[cfg(feature = "dim2")]
+        return MassProperties::from_trimesh(
+            _density * self.scaling_factors().iter().product::<Real>(),
+            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<dyn Shape> {
         Box::new(self.clone())

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<Item = Triangle> + '_ {
+    pub fn triangles(&self) -> impl ExactSizeIterator<Item = Triangle> + '_ {
         self.indices.iter().map(move |ids| {
             Triangle::new(
                 self.vertices[ids[0] as usize],