aboutsummaryrefslogtreecommitdiff
path: root/src/geometry
diff options
context:
space:
mode:
Diffstat (limited to 'src/geometry')
-rw-r--r--src/geometry/collider.rs95
-rw-r--r--src/geometry/contact_generator/contact_dispatcher.rs15
-rw-r--r--src/geometry/contact_generator/pfm_pfm_contact_generator.rs19
-rw-r--r--src/geometry/mod.rs2
-rw-r--r--src/geometry/rounded.rs110
-rw-r--r--src/geometry/shape.rs68
6 files changed, 232 insertions, 77 deletions
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<Arc<dyn Shape>, 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<dyn Shape>)
+ }
+
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<dyn Shape>
- }
+ Some(ShapeType::Ball) => deser::<A, Ball>(&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<dyn Shape>
}
- Some(ShapeType::Cuboid) => {
- let shape: Cuboid = seq
- .next_element()?
- .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?;
- Arc::new(shape) as Arc<dyn Shape>
- }
- Some(ShapeType::Capsule) => {
- let shape: Capsule = seq
- .next_element()?
- .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?;
- Arc::new(shape) as Arc<dyn Shape>
- }
- Some(ShapeType::Triangle) => {
- let shape: Triangle = seq
- .next_element()?
- .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?;
- Arc::new(shape) as Arc<dyn Shape>
- }
- Some(ShapeType::Trimesh) => {
- let shape: Trimesh = seq
- .next_element()?
- .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?;
- Arc::new(shape) as Arc<dyn Shape>
- }
- Some(ShapeType::HeightField) => {
- let shape: HeightField = seq
- .next_element()?
- .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?;
- Arc::new(shape) as Arc<dyn Shape>
- }
+ Some(ShapeType::Cuboid) => deser::<A, Cuboid>(&mut seq)?,
+ Some(ShapeType::Capsule) => deser::<A, Capsule>(&mut seq)?,
+ Some(ShapeType::Triangle) => deser::<A, Triangle>(&mut seq)?,
+ Some(ShapeType::Trimesh) => deser::<A, Trimesh>(&mut seq)?,
+ Some(ShapeType::HeightField) => deser::<A, HeightField>(&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<dyn Shape>
- }
+ Some(ShapeType::Cylinder) => deser::<A, Cylinder>(&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<dyn Shape>
- }
+ Some(ShapeType::Cone) => deser::<A, Cone>(&mut seq)?,
+ #[cfg(feature = "dim3")]
+ Some(ShapeType::RoundedCylinder) => deser::<A, Rounded<Cylinder>>(&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<S> {
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+pub struct Rounded<S: Roundable> {
/// The shape being rounded.
pub shape: S,
/// The rounding radius.
pub radius: f32,
}
+
+impl<S: Roundable> Rounded<S> {
+ /// 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<S: Roundable + SupportMap<f32>> SupportMap<f32> for Rounded<S> {
+ fn local_support_point(&self, dir: &Vector<f32>) -> Point<f32> {
+ self.local_support_point_toward(&Unit::new_normalize(*dir))
+ }
+
+ fn local_support_point_toward(&self, dir: &Unit<Vector<f32>>) -> Point<f32> {
+ self.shape.local_support_point_toward(dir) + **dir * self.radius
+ }
+
+ fn support_point(&self, transform: &Isometry<f32>, dir: &Vector<f32>) -> Point<f32> {
+ let local_dir = transform.inverse_transform_vector(dir);
+ transform * self.local_support_point(&local_dir)
+ }
+
+ fn support_point_toward(
+ &self,
+ transform: &Isometry<f32>,
+ dir: &Unit<Vector<f32>>,
+ ) -> Point<f32> {
+ let local_dir = Unit::new_unchecked(transform.inverse_transform_vector(dir));
+ transform * self.local_support_point_toward(&local_dir)
+ }
+}
+
+impl<S: Roundable + SupportMap<f32>> RayCast<f32> for Rounded<S> {
+ fn toi_and_normal_with_ray(
+ &self,
+ m: &Isometry<f32>,
+ ray: &Ray<f32>,
+ max_toi: f32,
+ solid: bool,
+ ) -> Option<RayIntersection<f32>> {
+ 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<S: Roundable + SupportMap<f32>> PointQuery<f32> for Rounded<S> {
+ #[inline]
+ fn project_point(
+ &self,
+ m: &Isometry<f32>,
+ point: &Point<f32>,
+ solid: bool,
+ ) -> PointProjection<f32> {
+ ncollide::query::point_projection_on_support_map(
+ m,
+ self,
+ &mut VoronoiSimplex::new(),
+ point,
+ solid,
+ )
+ }
+
+ #[inline]
+ fn project_point_with_feature(
+ &self,
+ m: &Isometry<f32>,
+ point: &Point<f32>,
+ ) -> (PointProjection<f32>, 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<f32> + PointQuery<f32> + 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<S>(&self) -> Option<&Rounded<S>>
+ where
+ S: Roundable,
+ Rounded<S>: 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<Cylinder> {
+ fn as_serialize(&self) -> Option<&dyn Serialize> {
+ Some(self as &dyn Serialize)
+ }
+
+ fn compute_aabb(&self, position: &Isometry<f32>) -> AABB<f32> {
+ 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))
}
}
generated by cgit (git 2.34.1) at 2026-02-03 16:23:54 +0000