nichkara/rapier
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add rounded cylinder.

Browse Source
This commit is contained in:
Crozet Sébastien
2020-10-20 15:57:54 +02:00
parent d513c22d33
commit 6495847095
10 changed files with 242 additions and 81 deletions
Download Patch File Download Diff File Expand all files Collapse all files

95
src/geometry/collider.rs
View File

@@ -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")]

15
src/geometry/contact_generator/contact_dispatcher.rs
View File

@@ -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()

19
src/geometry/contact_generator/pfm_pfm_contact_generator.rs
View File

@@ -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;

2
src/geometry/mod.rs
View File

@@ -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;

110
src/geometry/rounded.rs
View File

@@ -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)
}
}

68
src/geometry/shape.rs
View File

@@ -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))
}
}