From 64958470950cd9832a669b1bd5d70a2aeb6a85ef Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Tue, 20 Oct 2020 15:57:54 +0200 Subject: Add rounded cylinder. --- examples3d/heightfield3.rs | 4 +- examples3d/primitives3.rs | 4 +- examples3d/trimesh3.rs | 4 +- src/geometry/collider.rs | 95 +++++++++--------- .../contact_generator/contact_dispatcher.rs | 15 +-- .../contact_generator/pfm_pfm_contact_generator.rs | 19 +++- src/geometry/mod.rs | 2 +- src/geometry/rounded.rs | 110 ++++++++++++++++++++- src/geometry/shape.rs | 68 ++++++++++--- src_testbed/engine.rs | 2 +- 10 files changed, 242 insertions(+), 81 deletions(-) diff --git a/examples3d/heightfield3.rs b/examples3d/heightfield3.rs index 2f26dde..8c3386a 100644 --- a/examples3d/heightfield3.rs +++ b/examples3d/heightfield3.rs @@ -57,7 +57,9 @@ pub fn init_world(testbed: &mut Testbed) { let collider = match j % 4 { 0 => ColliderBuilder::cuboid(rad, rad, rad).build(), 1 => ColliderBuilder::ball(rad).build(), - 2 => ColliderBuilder::cylinder(rad, rad).build(), + // Rounded cylinders are much more efficient that cylinder, even if the + // rounding margin is small. + 2 => ColliderBuilder::rounded_cylinder(rad, rad, rad / 10.0).build(), _ => ColliderBuilder::cone(rad, rad).build(), }; diff --git a/examples3d/primitives3.rs b/examples3d/primitives3.rs index 4e2fc19..daabd23 100644 --- a/examples3d/primitives3.rs +++ b/examples3d/primitives3.rs @@ -53,7 +53,9 @@ pub fn init_world(testbed: &mut Testbed) { let collider = match j % 4 { 0 => ColliderBuilder::cuboid(rad, rad, rad).build(), 1 => ColliderBuilder::ball(rad).build(), - 2 => ColliderBuilder::cylinder(rad, rad).build(), + // Rounded cylinders are much more efficient that cylinder, even if the + // rounding margin is small. + 2 => ColliderBuilder::rounded_cylinder(rad, rad, rad / 10.0).build(), _ => ColliderBuilder::cone(rad, rad).build(), }; diff --git a/examples3d/trimesh3.rs b/examples3d/trimesh3.rs index 2d6c0bb..8fee784 100644 --- a/examples3d/trimesh3.rs +++ b/examples3d/trimesh3.rs @@ -67,7 +67,9 @@ pub fn init_world(testbed: &mut Testbed) { let collider = match j % 4 { 0 => ColliderBuilder::cuboid(rad, rad, rad).build(), 1 => ColliderBuilder::ball(rad).build(), - 2 => ColliderBuilder::cylinder(rad, rad).build(), + // Rounded cylinders are much more efficient that cylinder, even if the + // rounding margin is small. + 2 => ColliderBuilder::rounded_cylinder(rad, rad, rad / 10.0).build(), _ => ColliderBuilder::cone(rad, rad).build(), }; diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs index f952b15..4b3b115 100644 --- a/src/geometry/collider.rs +++ b/src/geometry/collider.rs @@ -1,7 +1,7 @@ use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet}; use crate::geometry::{ Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph, Polygon, - Proximity, Ray, RayIntersection, Shape, ShapeType, Triangle, Trimesh, + Proximity, Ray, RayIntersection, Rounded, Shape, ShapeType, Triangle, Trimesh, }; #[cfg(feature = "dim3")] use crate::geometry::{Cone, Cylinder, PolygonalFeatureMap}; @@ -40,6 +40,17 @@ impl ColliderShape { ColliderShape(Arc::new(Cylinder::new(half_height, radius))) } + /// Initialize a rounded cylindrical shape defined by its half-height + /// (along along the y axis), its radius, and its roundedness (the + /// radius of the sphere used for dilating the cylinder). + #[cfg(feature = "dim3")] + pub fn rounded_cylinder(half_height: f32, radius: f32, rounding_radius: f32) -> Self { + ColliderShape(Arc::new(Rounded::new( + Cylinder::new(half_height, radius), + rounding_radius, + ))) + } + /// Initialize a cone shape defined by its half-height /// (along along the y axis) and its basis radius. #[cfg(feature = "dim3")] @@ -127,13 +138,20 @@ impl<'de> serde::Deserialize<'de> for ColliderShape { .next_element()? .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?; + fn deser<'de, A, S: Shape + serde::Deserialize<'de>>( + seq: &mut A, + ) -> Result, A::Error> + where + A: serde::de::SeqAccess<'de>, + { + let shape: S = seq.next_element()?.ok_or_else(|| { + serde::de::Error::custom("Failed to deserialize builtin shape.") + })?; + Ok(Arc::new(shape) as Arc) + } + let shape = match ShapeType::from_i32(tag) { - Some(ShapeType::Ball) => { - let shape: Ball = seq - .next_element()? - .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?; - Arc::new(shape) as Arc - } + Some(ShapeType::Ball) => deser::(&mut seq)?, Some(ShapeType::Polygon) => { unimplemented!() // let shape: Polygon = seq @@ -141,50 +159,17 @@ impl<'de> serde::Deserialize<'de> for ColliderShape { // .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?; // Arc::new(shape) as Arc } - Some(ShapeType::Cuboid) => { - let shape: Cuboid = seq - .next_element()? - .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?; - Arc::new(shape) as Arc - } - Some(ShapeType::Capsule) => { - let shape: Capsule = seq - .next_element()? - .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?; - Arc::new(shape) as Arc - } - Some(ShapeType::Triangle) => { - let shape: Triangle = seq - .next_element()? - .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?; - Arc::new(shape) as Arc - } - Some(ShapeType::Trimesh) => { - let shape: Trimesh = seq - .next_element()? - .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?; - Arc::new(shape) as Arc - } - Some(ShapeType::HeightField) => { - let shape: HeightField = seq - .next_element()? - .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?; - Arc::new(shape) as Arc - } + Some(ShapeType::Cuboid) => deser::(&mut seq)?, + Some(ShapeType::Capsule) => deser::(&mut seq)?, + Some(ShapeType::Triangle) => deser::(&mut seq)?, + Some(ShapeType::Trimesh) => deser::(&mut seq)?, + Some(ShapeType::HeightField) => deser::(&mut seq)?, #[cfg(feature = "dim3")] - Some(ShapeType::Cylinder) => { - let shape: Cylinder = seq - .next_element()? - .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?; - Arc::new(shape) as Arc - } + Some(ShapeType::Cylinder) => deser::(&mut seq)?, #[cfg(feature = "dim3")] - Some(ShapeType::Cone) => { - let shape: Cone = seq - .next_element()? - .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?; - Arc::new(shape) as Arc - } + Some(ShapeType::Cone) => deser::(&mut seq)?, + #[cfg(feature = "dim3")] + Some(ShapeType::RoundedCylinder) => deser::>(&mut seq)?, None => { return Err(serde::de::Error::custom( "found invalid shape type to deserialize", @@ -342,6 +327,18 @@ impl ColliderBuilder { Self::new(ColliderShape::cylinder(half_height, radius)) } + /// Initialize a new collider builder with a rounded cylindrical shape defined by its half-height + /// (along along the y axis), its radius, and its roundedness (the + /// radius of the sphere used for dilating the cylinder). + #[cfg(feature = "dim3")] + pub fn rounded_cylinder(half_height: f32, radius: f32, rounding_radius: f32) -> Self { + Self::new(ColliderShape::rounded_cylinder( + half_height, + radius, + rounding_radius, + )) + } + /// Initialize a new collider builder with a cone shape defined by its half-height /// (along along the y axis) and its basis radius. #[cfg(feature = "dim3")] diff --git a/src/geometry/contact_generator/contact_dispatcher.rs b/src/geometry/contact_generator/contact_dispatcher.rs index 70ac84c..14b7f79 100644 --- a/src/geometry/contact_generator/contact_dispatcher.rs +++ b/src/geometry/contact_generator/contact_dispatcher.rs @@ -69,16 +69,7 @@ impl ContactDispatcher for DefaultContactDispatcher { }, None, ), - (ShapeType::Cuboid, ShapeType::Ball) - | (ShapeType::Ball, ShapeType::Cuboid) - | (ShapeType::Triangle, ShapeType::Ball) - | (ShapeType::Ball, ShapeType::Triangle) - | (ShapeType::Capsule, ShapeType::Ball) - | (ShapeType::Ball, ShapeType::Capsule) - | (ShapeType::Cylinder, ShapeType::Ball) - | (ShapeType::Ball, ShapeType::Cylinder) - | (ShapeType::Cone, ShapeType::Ball) - | (ShapeType::Ball, ShapeType::Cone) => ( + (_, ShapeType::Ball) | (ShapeType::Ball, _) => ( PrimitiveContactGenerator { generate_contacts: super::generate_contacts_ball_convex, ..PrimitiveContactGenerator::default() @@ -104,7 +95,9 @@ impl ContactDispatcher for DefaultContactDispatcher { (ShapeType::Cylinder, _) | (_, ShapeType::Cylinder) | (ShapeType::Cone, _) - | (_, ShapeType::Cone) => ( + | (_, ShapeType::Cone) + | (ShapeType::RoundedCylinder, _) + | (_, ShapeType::RoundedCylinder) => ( PrimitiveContactGenerator { generate_contacts: super::generate_contacts_pfm_pfm, ..PrimitiveContactGenerator::default() diff --git a/src/geometry/contact_generator/pfm_pfm_contact_generator.rs b/src/geometry/contact_generator/pfm_pfm_contact_generator.rs index c3815dd..37f8629 100644 --- a/src/geometry/contact_generator/pfm_pfm_contact_generator.rs +++ b/src/geometry/contact_generator/pfm_pfm_contact_generator.rs @@ -29,11 +29,11 @@ impl Default for PfmPfmContactManifoldGeneratorWorkspace { } pub fn generate_contacts_pfm_pfm(ctxt: &mut PrimitiveContactGenerationContext) { - if let (Some(pfm1), Some(pfm2)) = ( + if let (Some((pfm1, round_radius1)), Some((pfm2, round_radius2))) = ( ctxt.shape1.as_polygonal_feature_map(), ctxt.shape2.as_polygonal_feature_map(), ) { - do_generate_contacts(pfm1, pfm2, ctxt); + do_generate_contacts(pfm1, round_radius1, pfm2, round_radius2, ctxt); ctxt.manifold.update_warmstart_multiplier(); ctxt.manifold.sort_contacts(ctxt.prediction_distance); } @@ -41,7 +41,9 @@ pub fn generate_contacts_pfm_pfm(ctxt: &mut PrimitiveContactGenerationContext) { fn do_generate_contacts( pfm1: &dyn PolygonalFeatureMap, + round_radius1: f32, pfm2: &dyn PolygonalFeatureMap, + round_radius2: f32, ctxt: &mut PrimitiveContactGenerationContext, ) { let pos12 = ctxt.position1.inverse() * ctxt.position2; @@ -64,12 +66,13 @@ fn do_generate_contacts( .downcast_mut() .expect("Invalid workspace type, expected a PfmPfmContactManifoldGeneratorWorkspace."); + let total_prediction = ctxt.prediction_distance + round_radius1 + round_radius2; let contact = query::contact_support_map_support_map_with_params( &Isometry::identity(), pfm1, &pos12, pfm2, - ctxt.prediction_distance, + total_prediction, &mut workspace.simplex, workspace.last_gjk_dir, ); @@ -87,7 +90,7 @@ fn do_generate_contacts( workspace.feature2.transform_by(&pos12); PolyhedronFace::contacts( - ctxt.prediction_distance, + total_prediction, &workspace.feature1, &normal1, &workspace.feature2, @@ -95,6 +98,14 @@ fn do_generate_contacts( ctxt.manifold, ); + if round_radius1 != 0.0 || round_radius2 != 0.0 { + for contact in &mut ctxt.manifold.points { + contact.local_p1 += *normal1 * round_radius1; + contact.local_p2 += *normal2 * round_radius2; + contact.dist -= round_radius1 + round_radius2; + } + } + // Adjust points to take the radius into account. ctxt.manifold.local_n1 = *normal1; ctxt.manifold.local_n2 = *normal2; diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs index d569248..f73de98 100644 --- a/src/geometry/mod.rs +++ b/src/geometry/mod.rs @@ -18,7 +18,7 @@ pub use self::narrow_phase::NarrowPhase; pub use self::polygon::Polygon; pub use self::proximity::ProximityPair; pub use self::proximity_detector::{DefaultProximityDispatcher, ProximityDispatcher}; -pub use self::rounded::Rounded; +pub use self::rounded::{Roundable, Rounded}; pub use self::trimesh::Trimesh; pub use ncollide::query::Proximity; diff --git a/src/geometry/rounded.rs b/src/geometry/rounded.rs index 615d408..59c6a72 100644 --- a/src/geometry/rounded.rs +++ b/src/geometry/rounded.rs @@ -1,7 +1,115 @@ +use crate::geometry::{Cylinder, ShapeType}; +use crate::math::{Isometry, Point, Vector}; +use na::Unit; +use ncollide::query::{ + algorithms::VoronoiSimplex, PointProjection, PointQuery, Ray, RayCast, RayIntersection, +}; +use ncollide::shape::{FeatureId, SupportMap}; + +/// A shape which corner can be rounded. +pub trait Roundable { + /// The ShapeType fo this shape after rounding its corners. + fn rounded_shape_type() -> ShapeType; +} + +impl Roundable for Cylinder { + fn rounded_shape_type() -> ShapeType { + ShapeType::RoundedCylinder + } +} + /// A rounded shape. -pub struct Rounded { +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct Rounded { /// The shape being rounded. pub shape: S, /// The rounding radius. pub radius: f32, } + +impl Rounded { + /// Create sa new shape where all its edges and vertices are rounded by a radius of `radius`. + /// + /// This is done by applying a dilation of the given radius to the shape. + pub fn new(shape: S, radius: f32) -> Self { + Self { shape, radius } + } +} + +impl> SupportMap for Rounded { + fn local_support_point(&self, dir: &Vector) -> Point { + self.local_support_point_toward(&Unit::new_normalize(*dir)) + } + + fn local_support_point_toward(&self, dir: &Unit>) -> Point { + self.shape.local_support_point_toward(dir) + **dir * self.radius + } + + fn support_point(&self, transform: &Isometry, dir: &Vector) -> Point { + let local_dir = transform.inverse_transform_vector(dir); + transform * self.local_support_point(&local_dir) + } + + fn support_point_toward( + &self, + transform: &Isometry, + dir: &Unit>, + ) -> Point { + let local_dir = Unit::new_unchecked(transform.inverse_transform_vector(dir)); + transform * self.local_support_point_toward(&local_dir) + } +} + +impl> RayCast for Rounded { + fn toi_and_normal_with_ray( + &self, + m: &Isometry, + ray: &Ray, + max_toi: f32, + solid: bool, + ) -> Option> { + let ls_ray = ray.inverse_transform_by(m); + + ncollide::query::ray_intersection_with_support_map_with_params( + &Isometry::identity(), + self, + &mut VoronoiSimplex::new(), + &ls_ray, + max_toi, + solid, + ) + .map(|mut res| { + res.normal = m * res.normal; + res + }) + } +} + +// TODO: if PointQuery had a `project_point_with_normal` method, we could just +// call this and adjust the projected point accordingly. +impl> PointQuery for Rounded { + #[inline] + fn project_point( + &self, + m: &Isometry, + point: &Point, + solid: bool, + ) -> PointProjection { + ncollide::query::point_projection_on_support_map( + m, + self, + &mut VoronoiSimplex::new(), + point, + solid, + ) + } + + #[inline] + fn project_point_with_feature( + &self, + m: &Isometry, + point: &Point, + ) -> (PointProjection, FeatureId) { + (self.project_point(m, point, false), FeatureId::Unknown) + } +} diff --git a/src/geometry/shape.rs b/src/geometry/shape.rs index b972a3e..822a5b0 100644 --- a/src/geometry/shape.rs +++ b/src/geometry/shape.rs @@ -1,7 +1,7 @@ use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet}; use crate::geometry::{ Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph, Polygon, - Proximity, Ray, RayIntersection, Triangle, Trimesh, + Proximity, Ray, RayIntersection, Roundable, Rounded, Triangle, Trimesh, }; #[cfg(feature = "dim3")] use crate::geometry::{Cone, Cylinder, PolygonalFeatureMap}; @@ -9,7 +9,7 @@ use crate::math::{AngVector, Isometry, Point, Rotation, Vector}; use downcast_rs::{impl_downcast, DowncastSync}; use erased_serde::Serialize; use na::Point3; -use ncollide::bounding_volume::{HasBoundingVolume, AABB}; +use ncollide::bounding_volume::{BoundingVolume, HasBoundingVolume, AABB}; use ncollide::query::{PointQuery, RayCast}; use num::Zero; use num_derive::FromPrimitive; @@ -40,6 +40,18 @@ pub enum ShapeType { Cone, // /// A custom shape type. // Custom(u8), + // /// A cuboid with rounded corners. + // RoundedCuboid, + // /// A triangle with rounded corners. + // RoundedTriangle, + // /// A triangle-mesh with rounded corners. + // RoundedTrimesh, + // /// An heightfield with rounded corners. + // RoundedHeightField, + /// A cylinder with rounded corners. + RoundedCylinder, + // /// A cone with rounded corners. + // RoundedCone, } /// Trait implemented by shapes usable by Rapier. @@ -61,7 +73,7 @@ pub trait Shape: RayCast + PointQuery + DowncastSync { /// Converts this shape to a polygonal feature-map, if it is one. #[cfg(feature = "dim3")] - fn as_polygonal_feature_map(&self) -> Option<&dyn PolygonalFeatureMap> { + fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> { None } @@ -112,6 +124,15 @@ impl dyn Shape { pub fn as_cone(&self) -> Option<&Cone> { self.downcast_ref() } + + /// Converts this abstract shape to a cone, if it is one. + pub fn as_rounded(&self) -> Option<&Rounded> + where + S: Roundable, + Rounded: Shape, + { + self.downcast_ref() + } } impl Shape for Ball { @@ -171,8 +192,8 @@ impl Shape for Cuboid { } #[cfg(feature = "dim3")] - fn as_polygonal_feature_map(&self) -> Option<&dyn PolygonalFeatureMap> { - Some(self as &dyn PolygonalFeatureMap) + fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> { + Some((self as &dyn PolygonalFeatureMap, 0.0)) } } @@ -214,8 +235,8 @@ impl Shape for Triangle { } #[cfg(feature = "dim3")] - fn as_polygonal_feature_map(&self) -> Option<&dyn PolygonalFeatureMap> { - Some(self as &dyn PolygonalFeatureMap) + fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> { + Some((self as &dyn PolygonalFeatureMap, 0.0)) } } @@ -277,8 +298,8 @@ impl Shape for Cylinder { } #[cfg(feature = "dim3")] - fn as_polygonal_feature_map(&self) -> Option<&dyn PolygonalFeatureMap> { - Some(self as &dyn PolygonalFeatureMap) + fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> { + Some((self as &dyn PolygonalFeatureMap, 0.0)) } } @@ -302,7 +323,32 @@ impl Shape for Cone { } #[cfg(feature = "dim3")] - fn as_polygonal_feature_map(&self) -> Option<&dyn PolygonalFeatureMap> { - Some(self as &dyn PolygonalFeatureMap) + fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> { + Some((self as &dyn PolygonalFeatureMap, 0.0)) + } +} + +#[cfg(feature = "dim3")] +impl Shape for Rounded { + fn as_serialize(&self) -> Option<&dyn Serialize> { + Some(self as &dyn Serialize) + } + + fn compute_aabb(&self, position: &Isometry) -> AABB { + self.shape.compute_aabb(position).loosened(self.radius) + } + + fn mass_properties(&self, density: f32) -> MassProperties { + // We ignore the margin here. + self.shape.mass_properties(density) + } + + fn shape_type(&self) -> ShapeType { + ShapeType::RoundedCylinder + } + + #[cfg(feature = "dim3")] + fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> { + Some((&self.shape as &dyn PolygonalFeatureMap, self.radius)) } } diff --git a/src_testbed/engine.rs b/src_testbed/engine.rs index 1908745..7bdc812 100644 --- a/src_testbed/engine.rs +++ b/src_testbed/engine.rs @@ -411,7 +411,7 @@ impl GraphicsManager { } #[cfg(feature = "dim3")] - if let Some(cylinder) = shape.as_cylinder() { + if let Some(cylinder) = shape.as_cylinder().or(shape.as_rounded().map(|r| &r.shape)) { out.push(Node::Cylinder(Cylinder::new( handle, cylinder.half_height, -- cgit