Use meshloader to support multiple file formats loading (#744)

Co-authored-by: Tin Lai <tin@tinyiu.com>

1 files changed, 57 insertions, 53 deletions

diff --git a/crates/rapier3d-urdf/src/lib.rs b/crates/rapier3d-urdf/src/lib.rs
index 1f913da..8e858c3 100644
--- a/crates/rapier3d-urdf/src/lib.rs
+++ b/crates/rapier3d-urdf/src/lib.rs
@@ -1,4 +1,4 @@
-//! ## STL loader for the Rapier physics engine
+//! ## URDF loader for the Rapier physics engine
 //!
 //! Rapier is a set of 2D and 3D physics engines for games, animation, and robotics. The `rapier3d-urdf`
 //! crate lets you convert an URDF file into a set of rigid-bodies, colliders, and joints, for usage with the
@@ -7,6 +7,8 @@
 //! ## Optional cargo features
 //!
 //! - `stl`: enables loading STL meshes referenced by the URDF file.
+//! - `collada`: enables loading Collada (`.dae`) meshes referenced by the URDF file.
+//! - `wavefront`: enables loading Wavefront (`.obj`) meshes referenced by the URDF file.
 //!
 //! ## Limitations
 //!
@@ -14,7 +16,7 @@
 //! improve
 //! these elements are very welcome!
 //!
-//! - Mesh file types other than `stl` are not supported yet. Contributions are welcome. You my check the `rapier3d-stl`
+//! - Mesh file types are limited. Contributions are welcome. You may check the `rapier3d-meshloader`
 //!   repository for an example of mesh loader.
 //! - When inserting joints as multibody joints, they will be reset to their neutral position (all coordinates = 0).
 //! - The following fields are currently ignored:
@@ -35,6 +37,7 @@ use rapier3d::{
     geometry::{Collider, ColliderBuilder, ColliderHandle, ColliderSet, SharedShape, TriMeshFlags},
     math::{Isometry, Point, Real, Vector},
     na,
+    prelude::MeshConverter,
 };
 use std::collections::HashMap;
 use std::path::Path;
@@ -289,7 +292,7 @@ impl UrdfRobot {
     /// (`UrdfRobot`). Both structures are arranged the same way, with matching indices for each part.
     ///
     /// If the URDF file references external meshes, they will be loaded automatically if the format
-    /// is supported. The format is detected from the file’s extension. All the mesh formats are
+    /// is supported. The format is detected mostly from the file’s extension. All the mesh formats are
     /// disabled by default and can be enabled through cargo features (e.g. the `stl` feature of
     /// this crate enabled loading referenced meshes in stl format).
     ///
@@ -310,13 +313,13 @@ impl UrdfRobot {
                 name_to_link_id.insert(&link.name, id);
                 let mut colliders = vec![];
                 if options.create_colliders_from_collision_shapes {
-                    colliders.extend(link.collisions.iter().filter_map(|co| {
-                        urdf_to_collider(&options, mesh_dir, &co.geometry, &co.origin)
+                    colliders.extend(link.collisions.iter().flat_map(|co| {
+                        urdf_to_colliders(&options, mesh_dir, &co.geometry, &co.origin)
                     }))
                 }
                 if options.create_colliders_from_visual_shapes {
-                    colliders.extend(link.visuals.iter().filter_map(|vis| {
-                        urdf_to_collider(&options, mesh_dir, &vis.geometry, &vis.origin)
+                    colliders.extend(link.visuals.iter().flat_map(|vis| {
+                        urdf_to_colliders(&options, mesh_dir, &vis.geometry, &vis.origin)
                     }))
                 }
                 let mut body = urdf_to_rigid_body(&options, &link.inertial);
@@ -488,66 +491,67 @@ fn urdf_to_rigid_body(options: &UrdfLoaderOptions, inertial: &Inertial) -> Rigid
     builder.build()
 }
 
-fn urdf_to_collider(
+fn urdf_to_colliders(
     options: &UrdfLoaderOptions,
-    _mesh_dir: &Path, // NOTO: this isn’t used if there is no external mesh feature enabled (like stl).
+    mesh_dir: &Path,
     geometry: &Geometry,
     origin: &Pose,
-) -> Option<Collider> {
-    let mut builder = options.collider_blueprint.clone();
+) -> Vec<Collider> {
     let mut shape_transform = Isometry::identity();
-    let shape = match &geometry {
-        Geometry::Box { size } => SharedShape::cuboid(
-            size[0] as Real / 2.0,
-            size[1] as Real / 2.0,
-            size[2] as Real / 2.0,
-        ),
+
+    let mut colliders = Vec::new();
+
+    match &geometry {
+        Geometry::Box { size } => {
+            colliders.push(SharedShape::cuboid(
+                size[0] as Real / 2.0,
+                size[1] as Real / 2.0,
+                size[2] as Real / 2.0,
+            ));
+        }
         Geometry::Cylinder { radius, length } => {
             // This rotation will make the cylinder Z-up as per the URDF spec,
             // instead of rapier’s default Y-up.
             shape_transform = Isometry::rotation(Vector::x() * Real::frac_pi_2());
-            SharedShape::cylinder(*length as Real / 2.0, *radius as Real)
+            colliders.push(SharedShape::cylinder(
+                *length as Real / 2.0,
+                *radius as Real,
+            ));
+        }
+        Geometry::Sphere { radius } => {
+            colliders.push(SharedShape::ball(*radius as Real));
         }
-        Geometry::Sphere { radius } => SharedShape::ball(*radius as Real),
         Geometry::Mesh { filename, scale } => {
-            let path: &Path = filename.as_ref();
-            let _scale = scale
+            let full_path = mesh_dir.join(filename);
+            let scale = scale
                 .map(|s| Vector::new(s.x as Real, s.y as Real, s.z as Real))
                 .unwrap_or_else(|| Vector::<Real>::repeat(1.0));
-            match path.extension().and_then(|ext| ext.to_str()) {
-                #[cfg(feature = "stl")]
-                Some("stl") | Some("STL") => {
-                    use rapier3d::geometry::MeshConverter;
-                    let full_path = _mesh_dir.join(filename);
-                    match rapier3d_stl::load_from_path(
-                        full_path,
-                        MeshConverter::TriMeshWithFlags(options.trimesh_flags),
-                        _scale,
-                    ) {
-                        Ok(stl_shape) => {
-                            shape_transform = stl_shape.pose;
-                            stl_shape.shape
-                        }
-                        Err(e) => {
-                            log::error!("failed to load STL file {filename}: {e}");
-                            return None;
-                        }
-                    }
-                }
-                _ => {
-                    log::error!("failed to load file with unknown type {filename}");
-                    return None;
-                }
-            }
+            let Ok(loaded_mesh) = rapier3d_meshloader::load_from_path(
+                full_path,
+                &MeshConverter::TriMeshWithFlags(options.trimesh_flags),
+                scale,
+            ) else {
+                return Vec::new();
+            };
+            colliders.append(
+                &mut loaded_mesh
+                    .into_iter()
+                    .filter_map(|x| x.map(|s| s.shape).ok())
+                    .collect(),
+            );
         }
-    };
+    }
 
-    builder.shape = shape;
-    Some(
-        builder
-            .position(urdf_to_isometry(origin) * shape_transform)
-            .build(),
-    )
+    colliders
+        .drain(..)
+        .map(move |shape| {
+            let mut builder = options.collider_blueprint.clone();
+            builder.shape = shape;
+            builder
+                .position(urdf_to_isometry(origin) * shape_transform)
+                .build()
+        })
+        .collect()
 }
 
 fn urdf_to_isometry(pose: &Pose) -> Isometry<Real> {