From d513c22d33ab44b0048355bcfd1db4173b3f7ece Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Tue, 20 Oct 2020 14:16:01 +0200 Subject: Add cone support. --- src/geometry/collider.rs | 29 +++++++++++-- .../contact_generator/contact_dispatcher.rs | 9 +++- src/geometry/mod.rs | 3 ++ src/geometry/polygonal_feature_map.rs | 48 +++++++++++++++++++++- src/geometry/shape.rs | 41 ++++++++++++++++-- 5 files changed, 119 insertions(+), 11 deletions(-) (limited to 'src/geometry') diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs index c6cf6d0..f952b15 100644 --- a/src/geometry/collider.rs +++ b/src/geometry/collider.rs @@ -1,10 +1,10 @@ use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet}; -#[cfg(feature = "dim3")] -use crate::geometry::PolygonalFeatureMap; use crate::geometry::{ - Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, Cylinder, HeightField, InteractionGraph, - Polygon, Proximity, Ray, RayIntersection, Shape, ShapeType, Triangle, Trimesh, + Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph, Polygon, + Proximity, Ray, RayIntersection, Shape, ShapeType, Triangle, Trimesh, }; +#[cfg(feature = "dim3")] +use crate::geometry::{Cone, Cylinder, PolygonalFeatureMap}; use crate::math::{AngVector, Isometry, Point, Rotation, Vector}; use downcast_rs::{impl_downcast, DowncastSync}; use erased_serde::Serialize; @@ -40,6 +40,13 @@ impl ColliderShape { ColliderShape(Arc::new(Cylinder::new(half_height, radius))) } + /// Initialize a cone shape defined by its half-height + /// (along along the y axis) and its basis radius. + #[cfg(feature = "dim3")] + pub fn cone(half_height: f32, radius: f32) -> Self { + ColliderShape(Arc::new(Cone::new(half_height, radius))) + } + /// Initialize a cuboid shape defined by its half-extents. pub fn cuboid(half_extents: Vector) -> Self { ColliderShape(Arc::new(Cuboid::new(half_extents))) @@ -171,6 +178,13 @@ impl<'de> serde::Deserialize<'de> for ColliderShape { .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?; Arc::new(shape) as Arc } + #[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 + } None => { return Err(serde::de::Error::custom( "found invalid shape type to deserialize", @@ -328,6 +342,13 @@ impl ColliderBuilder { Self::new(ColliderShape::cylinder(half_height, 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")] + pub fn cone(half_height: f32, radius: f32) -> Self { + Self::new(ColliderShape::cone(half_height, radius)) + } + /// Initialize a new collider builder with a cuboid shape defined by its half-extents. #[cfg(feature = "dim2")] pub fn cuboid(hx: f32, hy: f32) -> Self { diff --git a/src/geometry/contact_generator/contact_dispatcher.rs b/src/geometry/contact_generator/contact_dispatcher.rs index 01bbc46..70ac84c 100644 --- a/src/geometry/contact_generator/contact_dispatcher.rs +++ b/src/geometry/contact_generator/contact_dispatcher.rs @@ -76,7 +76,9 @@ impl ContactDispatcher for DefaultContactDispatcher { | (ShapeType::Capsule, ShapeType::Ball) | (ShapeType::Ball, ShapeType::Capsule) | (ShapeType::Cylinder, ShapeType::Ball) - | (ShapeType::Ball, ShapeType::Cylinder) => ( + | (ShapeType::Ball, ShapeType::Cylinder) + | (ShapeType::Cone, ShapeType::Ball) + | (ShapeType::Ball, ShapeType::Cone) => ( PrimitiveContactGenerator { generate_contacts: super::generate_contacts_ball_convex, ..PrimitiveContactGenerator::default() @@ -99,7 +101,10 @@ impl ContactDispatcher for DefaultContactDispatcher { None, ) } - (ShapeType::Cylinder, _) | (_, ShapeType::Cylinder) => ( + (ShapeType::Cylinder, _) + | (_, ShapeType::Cylinder) + | (ShapeType::Cone, _) + | (_, ShapeType::Cone) => ( PrimitiveContactGenerator { generate_contacts: super::generate_contacts_pfm_pfm, ..PrimitiveContactGenerator::default() diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs index 71573ed..d569248 100644 --- a/src/geometry/mod.rs +++ b/src/geometry/mod.rs @@ -35,6 +35,9 @@ pub type HeightField = ncollide::shape::HeightField; /// A cylindrical shape. #[cfg(feature = "dim3")] pub type Cylinder = ncollide::shape::Cylinder; +/// A cone shape. +#[cfg(feature = "dim3")] +pub type Cone = ncollide::shape::Cone; /// An axis-aligned bounding box. pub type AABB = ncollide::bounding_volume::AABB; /// Event triggered when two non-sensor colliders start or stop being in contact. diff --git a/src/geometry/polygonal_feature_map.rs b/src/geometry/polygonal_feature_map.rs index 70be5d0..fc5e066 100644 --- a/src/geometry/polygonal_feature_map.rs +++ b/src/geometry/polygonal_feature_map.rs @@ -1,5 +1,5 @@ use crate::geometry::PolyhedronFace; -use crate::geometry::{cuboid, Cuboid, Cylinder, Triangle}; +use crate::geometry::{cuboid, Cone, Cuboid, Cylinder, Triangle}; use crate::math::{Point, Vector}; use approx::AbsDiffEq; use na::{Unit, Vector2, Vector3}; @@ -85,3 +85,49 @@ impl PolygonalFeatureMap for Cylinder { } } } + +impl PolygonalFeatureMap for Cone { + fn local_support_feature(&self, dir: &Unit>, out_features: &mut PolyhedronFace) { + // About feature ids. It is very similar to the feature ids of cylinders. + // At all times, we consider our cone to be approximated as follows: + // - The curved part is approximated by a single segment. + // - The flat cap of the cone is approximated by a square. + // - The curved-part segment has a feature ID of 0, and its endpoint with negative + // `y` coordinate has an ID of 1. + // - The bottom cap has its vertices with feature ID of 1,3,5,7 (in counter-clockwise order + // when looking at the cap with an eye looking towards +y). + // - The bottom cap has its four edge feature IDs of 2,4,6,8, in counter-clockwise order. + // - The bottom cap has its face feature ID of 9. + // - Note that at all times, one of the cap's vertices are the same as the curved-part + // segment endpoints. + let dir2 = Vector2::new(dir.x, dir.z) + .try_normalize(f32::default_epsilon()) + .unwrap_or(Vector2::x()); + + if dir.y > 0.0 { + // We return a segment lying on the cone's curved part. + out_features.vertices[0] = Point::new( + dir2.x * self.radius, + -self.half_height, + dir2.y * self.radius, + ); + out_features.vertices[1] = Point::new(0.0, self.half_height, 0.0); + out_features.eids = [0, 0, 0, 0]; + out_features.fid = 0; + out_features.num_vertices = 2; + out_features.vids = [1, 11, 11, 11]; + } else { + // We return a square approximation of the cone cap. + let y = -self.half_height; + out_features.vertices[0] = Point::new(dir2.x * self.radius, y, dir2.y * self.radius); + out_features.vertices[1] = Point::new(-dir2.y * self.radius, y, dir2.x * self.radius); + out_features.vertices[2] = Point::new(-dir2.x * self.radius, y, -dir2.y * self.radius); + out_features.vertices[3] = Point::new(dir2.y * self.radius, y, -dir2.x * self.radius); + + out_features.eids = [2, 4, 6, 8]; + out_features.fid = 9; + out_features.num_vertices = 4; + out_features.vids = [1, 3, 5, 7]; + } + } +} diff --git a/src/geometry/shape.rs b/src/geometry/shape.rs index c9e0a3a..b972a3e 100644 --- a/src/geometry/shape.rs +++ b/src/geometry/shape.rs @@ -1,10 +1,10 @@ use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet}; -#[cfg(feature = "dim3")] -use crate::geometry::PolygonalFeatureMap; use crate::geometry::{ - Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, Cylinder, HeightField, InteractionGraph, - Polygon, Proximity, Ray, RayIntersection, Triangle, Trimesh, + Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph, Polygon, + Proximity, Ray, RayIntersection, Triangle, Trimesh, }; +#[cfg(feature = "dim3")] +use crate::geometry::{Cone, Cylinder, PolygonalFeatureMap}; use crate::math::{AngVector, Isometry, Point, Rotation, Vector}; use downcast_rs::{impl_downcast, DowncastSync}; use erased_serde::Serialize; @@ -35,6 +35,9 @@ pub enum ShapeType { #[cfg(feature = "dim3")] /// A cylindrical shape. Cylinder, + #[cfg(feature = "dim3")] + /// A cylindrical shape. + Cone, // /// A custom shape type. // Custom(u8), } @@ -104,6 +107,11 @@ impl dyn Shape { pub fn as_cylinder(&self) -> Option<&Cylinder> { self.downcast_ref() } + + /// Converts this abstract shape to a cone, if it is one. + pub fn as_cone(&self) -> Option<&Cone> { + self.downcast_ref() + } } impl Shape for Ball { @@ -273,3 +281,28 @@ impl Shape for Cylinder { Some(self as &dyn PolygonalFeatureMap) } } + +#[cfg(feature = "dim3")] +impl Shape for Cone { + #[cfg(feature = "serde-serialize")] + fn as_serialize(&self) -> Option<&dyn Serialize> { + Some(self as &dyn Serialize) + } + + fn compute_aabb(&self, position: &Isometry) -> AABB { + self.bounding_volume(position) + } + + fn mass_properties(&self, density: f32) -> MassProperties { + MassProperties::from_cone(density, self.half_height, self.radius) + } + + fn shape_type(&self) -> ShapeType { + ShapeType::Cone + } + + #[cfg(feature = "dim3")] + fn as_polygonal_feature_map(&self) -> Option<&dyn PolygonalFeatureMap> { + Some(self as &dyn PolygonalFeatureMap) + } +} -- cgit