From faec3d5d46c88e2949179dd2789899e5cf26ed48 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 12 Oct 2020 18:33:58 +0200 Subject: Start adding cylinders. --- src/geometry/collider.rs | 48 +++++++- .../ball_convex_contact_generator.rs | 2 + .../contact_generator/contact_dispatcher.rs | 14 ++- src/geometry/contact_generator/mod.rs | 6 + .../contact_generator/pfm_pfm_contact_generator.rs | 123 +++++++++++++++++++++ src/geometry/mod.rs | 7 ++ src/geometry/polygonal_feature_map.rs | 65 +++++++++++ src/geometry/polyhedron_feature3d.rs | 10 ++ 8 files changed, 268 insertions(+), 7 deletions(-) create mode 100644 src/geometry/contact_generator/pfm_pfm_contact_generator.rs create mode 100644 src/geometry/polygonal_feature_map.rs (limited to 'src/geometry') diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs index 7c293b6..fe42bd7 100644 --- a/src/geometry/collider.rs +++ b/src/geometry/collider.rs @@ -1,7 +1,9 @@ use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet}; +#[cfg(feature = "dim3")] +use crate::geometry::PolygonalFeatureMap; use crate::geometry::{ - Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph, Polygon, - Proximity, Ray, RayIntersection, Triangle, Trimesh, + Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, Cylinder, HeightField, InteractionGraph, + Polygon, Proximity, Ray, RayIntersection, Triangle, Trimesh, }; use crate::math::{AngVector, Isometry, Point, Rotation, Vector}; use na::Point3; @@ -27,6 +29,9 @@ pub enum Shape { Trimesh(Trimesh), /// A heightfield shape. HeightField(HeightField), + #[cfg(feature = "dim3")] + /// A cylindrical shape. + Cylinder(Cylinder), } impl Shape { @@ -86,6 +91,25 @@ impl Shape { } } + /// Gets a reference to the underlying cylindrical shape, if `self` is one. + pub fn as_cylinder(&self) -> Option<&Cylinder> { + match self { + Shape::Cylinder(c) => Some(c), + _ => None, + } + } + + /// gets a reference to this shape seen as a PolygonalFeatureMap. + #[cfg(feature = "dim3")] + pub fn as_polygonal_feature_map(&self) -> Option<&dyn PolygonalFeatureMap> { + match self { + Shape::Triangle(t) => Some(t), + Shape::Cuboid(c) => Some(c), + Shape::Cylinder(c) => Some(c), + _ => None, + } + } + /// Computes the axis-aligned bounding box of this shape. pub fn compute_aabb(&self, position: &Isometry) -> AABB { match self { @@ -96,6 +120,7 @@ impl Shape { Shape::Triangle(triangle) => triangle.bounding_volume(position), Shape::Trimesh(trimesh) => trimesh.aabb(position), Shape::HeightField(heightfield) => heightfield.bounding_volume(position), + Shape::Cylinder(cylinder) => cylinder.bounding_volume(position), } } @@ -139,6 +164,10 @@ impl Shape { Shape::HeightField(heightfield) => { heightfield.toi_and_normal_with_ray(position, ray, max_toi, true) } + #[cfg(feature = "dim3")] + Shape::Cylinder(cylinder) => { + cylinder.toi_and_normal_with_ray(position, ray, max_toi, true) + } } } } @@ -242,9 +271,12 @@ impl Collider { Shape::Capsule(caps) => { MassProperties::from_capsule(self.density, caps.a, caps.b, caps.radius) } - Shape::Triangle(_) => MassProperties::zero(), - Shape::Trimesh(_) => MassProperties::zero(), - Shape::HeightField(_) => MassProperties::zero(), + Shape::Triangle(_) | Shape::Trimesh(_) | Shape::HeightField(_) => { + MassProperties::zero() + } + Shape::Cylinder(c) => { + MassProperties::from_cylinder(self.density, c.half_height, c.radius) + } } } } @@ -291,6 +323,12 @@ impl ColliderBuilder { Self::new(Shape::Ball(Ball::new(radius))) } + /// Initialize a new collider builder with a cylindrical shape defined by its half-height + /// (along along the y axis) and its radius. + pub fn cylinder(half_height: f32, radius: f32) -> Self { + Self::new(Shape::Cylinder(Cylinder::new(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/ball_convex_contact_generator.rs b/src/geometry/contact_generator/ball_convex_contact_generator.rs index a187832..0856029 100644 --- a/src/geometry/contact_generator/ball_convex_contact_generator.rs +++ b/src/geometry/contact_generator/ball_convex_contact_generator.rs @@ -12,6 +12,7 @@ pub fn generate_contacts_ball_convex(ctxt: &mut PrimitiveContactGenerationContex Shape::Triangle(tri2) => do_generate_contacts(tri2, ball1, ctxt, true), Shape::Cuboid(cube2) => do_generate_contacts(cube2, ball1, ctxt, true), Shape::Capsule(capsule2) => do_generate_contacts(capsule2, ball1, ctxt, true), + Shape::Cylinder(cylinder2) => do_generate_contacts(cylinder2, ball1, ctxt, true), _ => unimplemented!(), } } else if let Shape::Ball(ball2) = ctxt.shape2 { @@ -19,6 +20,7 @@ pub fn generate_contacts_ball_convex(ctxt: &mut PrimitiveContactGenerationContex Shape::Triangle(tri1) => do_generate_contacts(tri1, ball2, ctxt, false), Shape::Cuboid(cube1) => do_generate_contacts(cube1, ball2, ctxt, false), Shape::Capsule(capsule1) => do_generate_contacts(capsule1, ball2, ctxt, false), + Shape::Cylinder(cylinder1) => do_generate_contacts(cylinder1, ball2, ctxt, false), _ => unimplemented!(), } } diff --git a/src/geometry/contact_generator/contact_dispatcher.rs b/src/geometry/contact_generator/contact_dispatcher.rs index 8c846e0..e925fd5 100644 --- a/src/geometry/contact_generator/contact_dispatcher.rs +++ b/src/geometry/contact_generator/contact_dispatcher.rs @@ -1,6 +1,7 @@ use crate::geometry::contact_generator::{ ContactGenerator, ContactPhase, HeightFieldShapeContactGeneratorWorkspace, - PrimitiveContactGenerator, TrimeshShapeContactGeneratorWorkspace, + PfmPfmContactManifoldGeneratorWorkspace, PrimitiveContactGenerator, + TrimeshShapeContactGeneratorWorkspace, }; use crate::geometry::Shape; use std::any::Any; @@ -73,7 +74,9 @@ impl ContactDispatcher for DefaultContactDispatcher { | (Shape::Triangle(_), Shape::Ball(_)) | (Shape::Ball(_), Shape::Triangle(_)) | (Shape::Capsule(_), Shape::Ball(_)) - | (Shape::Ball(_), Shape::Capsule(_)) => ( + | (Shape::Ball(_), Shape::Capsule(_)) + | (Shape::Cylinder(_), Shape::Ball(_)) + | (Shape::Ball(_), Shape::Cylinder(_)) => ( PrimitiveContactGenerator { generate_contacts: super::generate_contacts_ball_convex, ..PrimitiveContactGenerator::default() @@ -94,6 +97,13 @@ impl ContactDispatcher for DefaultContactDispatcher { }, None, ), + (Shape::Cylinder(_), _) | (_, Shape::Cylinder(_)) => ( + PrimitiveContactGenerator { + generate_contacts: super::generate_contacts_pfm_pfm, + ..PrimitiveContactGenerator::default() + }, + Some(Box::new(PfmPfmContactManifoldGeneratorWorkspace::default())), + ), _ => (PrimitiveContactGenerator::default(), None), } } diff --git a/src/geometry/contact_generator/mod.rs b/src/geometry/contact_generator/mod.rs index ecd2540..a6bad05 100644 --- a/src/geometry/contact_generator/mod.rs +++ b/src/geometry/contact_generator/mod.rs @@ -18,6 +18,10 @@ pub use self::cuboid_triangle_contact_generator::generate_contacts_cuboid_triang pub use self::heightfield_shape_contact_generator::{ generate_contacts_heightfield_shape, HeightFieldShapeContactGeneratorWorkspace, }; +#[cfg(feature = "dim3")] +pub use self::pfm_pfm_contact_generator::{ + generate_contacts_pfm_pfm, PfmPfmContactManifoldGeneratorWorkspace, +}; pub use self::polygon_polygon_contact_generator::generate_contacts_polygon_polygon; pub use self::trimesh_shape_contact_generator::{ generate_contacts_trimesh_shape, TrimeshShapeContactGeneratorWorkspace, @@ -39,6 +43,8 @@ mod cuboid_cuboid_contact_generator; mod cuboid_polygon_contact_generator; mod cuboid_triangle_contact_generator; mod heightfield_shape_contact_generator; +#[cfg(feature = "dim3")] +mod pfm_pfm_contact_generator; mod polygon_polygon_contact_generator; mod trimesh_shape_contact_generator; diff --git a/src/geometry/contact_generator/pfm_pfm_contact_generator.rs b/src/geometry/contact_generator/pfm_pfm_contact_generator.rs new file mode 100644 index 0000000..cfb4472 --- /dev/null +++ b/src/geometry/contact_generator/pfm_pfm_contact_generator.rs @@ -0,0 +1,123 @@ +use crate::geometry::contact_generator::PrimitiveContactGenerationContext; +use crate::geometry::{Contact, KinematicsCategory, PolygonalFeatureMap, PolyhedronFace}; +use crate::math::{Isometry, Vector}; +use na::Unit; +use ncollide::query; +use ncollide::query::algorithms::{gjk::GJKResult, VoronoiSimplex}; + +pub struct PfmPfmContactManifoldGeneratorWorkspace { + simplex: VoronoiSimplex, + last_gjk_dir: Option>>, + last_optimal_dir: Option>>, + feature1: PolyhedronFace, + feature2: PolyhedronFace, +} + +impl Default for PfmPfmContactManifoldGeneratorWorkspace { + fn default() -> Self { + Self { + simplex: VoronoiSimplex::new(), + last_gjk_dir: None, + last_optimal_dir: None, + feature1: PolyhedronFace::new(), + feature2: PolyhedronFace::new(), + } + } +} + +pub fn generate_contacts_pfm_pfm(ctxt: &mut PrimitiveContactGenerationContext) { + if let (Some(pfm1), Some(pfm2)) = ( + ctxt.collider1.shape().as_polygonal_feature_map(), + ctxt.collider2.shape().as_polygonal_feature_map(), + ) { + do_generate_contacts(pfm1, pfm2, ctxt); + ctxt.manifold.update_warmstart_multiplier(); + ctxt.manifold.sort_contacts(ctxt.prediction_distance); + } +} + +fn do_generate_contacts( + pfm1: &dyn PolygonalFeatureMap, + pfm2: &dyn PolygonalFeatureMap, + ctxt: &mut PrimitiveContactGenerationContext, +) { + let pos12 = ctxt.position1.inverse() * ctxt.position2; + let pos21 = pos12.inverse(); + + // if ctxt.manifold.try_update_contacts(&pos12) { + // return; + // } + + let workspace: &mut PfmPfmContactManifoldGeneratorWorkspace = ctxt + .workspace + .as_mut() + .expect("The PfmPfmContactManifoldGeneratorWorkspace is missing.") + .downcast_mut() + .expect("Invalid workspace type, expected a PfmPfmContactManifoldGeneratorWorkspace."); + + let contact = query::contact_support_map_support_map_with_params( + &Isometry::identity(), + pfm1, + &pos12, + pfm2, + ctxt.prediction_distance, + &mut workspace.simplex, + workspace.last_gjk_dir, + ); + + let old_manifold_points = ctxt.manifold.points.clone(); + ctxt.manifold.points.clear(); + + match contact { + GJKResult::ClosestPoints(local_p1, local_p2, dir) => { + workspace.last_gjk_dir = Some(dir); + let normal1 = dir; + let normal2 = pos21 * -dir; + pfm1.local_support_feature(&normal1, &mut workspace.feature1); + pfm2.local_support_feature(&normal2, &mut workspace.feature2); + workspace.feature2.transform_by(&pos12); + + // PolyhedronFace::contacts( + // ctxt.prediction_distance, + // &workspace.feature1, + // &normal1, + // &workspace.feature2, + // &pos21, + // ctxt.manifold, + // ); + + println!( + "Contact patatrac: {:?}, {:?}, {}, {}", + ctxt.manifold.points.len(), + ctxt.position1 * dir, + workspace.feature1.num_vertices, + workspace.feature2.num_vertices + ); + + if ctxt.manifold.all_contacts().is_empty() { + // Add at least the deepest contact. + let dist = (local_p2 - local_p1).dot(&dir); + ctxt.manifold.points.push(Contact { + local_p1, + local_p2: pos21 * local_p2, + impulse: 0.0, + tangent_impulse: Contact::zero_tangent_impulse(), + fid1: 0, // FIXME + fid2: 0, // FIXME + dist, + }); + } + + // Adjust points to take the radius into account. + ctxt.manifold.local_n1 = *normal1; + ctxt.manifold.local_n2 = *normal2; + ctxt.manifold.kinematics.category = KinematicsCategory::PlanePoint; // FIXME + ctxt.manifold.kinematics.radius1 = 0.0; + ctxt.manifold.kinematics.radius2 = 0.0; + } + GJKResult::NoIntersection(dir) => { + workspace.last_gjk_dir = Some(dir); + } + _ => {} + } +} diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs index 562f962..1ccb2c8 100644 --- a/src/geometry/mod.rs +++ b/src/geometry/mod.rs @@ -30,6 +30,9 @@ pub type Triangle = ncollide::shape::Triangle; pub type Ball = ncollide::shape::Ball; /// A heightfield shape. pub type HeightField = ncollide::shape::HeightField; +/// A cylindrical shape. +#[cfg(feature = "dim3")] +pub type Cylinder = ncollide::shape::Cylinder; /// 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. @@ -51,6 +54,8 @@ pub(crate) use self::contact::WContact; pub(crate) use self::contact_generator::{clip_segments, clip_segments_with_normal}; pub(crate) use self::narrow_phase::ContactManifoldIndex; #[cfg(feature = "dim3")] +pub(crate) use self::polygonal_feature_map::PolygonalFeatureMap; +#[cfg(feature = "dim3")] pub(crate) use self::polyhedron_feature3d::PolyhedronFace; pub(crate) use self::waabb::{WRay, WAABB}; pub(crate) use self::wquadtree::WQuadtree; @@ -81,3 +86,5 @@ mod trimesh; mod waabb; mod wquadtree; //mod z_order; +#[cfg(feature = "dim3")] +mod polygonal_feature_map; diff --git a/src/geometry/polygonal_feature_map.rs b/src/geometry/polygonal_feature_map.rs new file mode 100644 index 0000000..8b047fc --- /dev/null +++ b/src/geometry/polygonal_feature_map.rs @@ -0,0 +1,65 @@ +use crate::geometry::PolyhedronFace; +use crate::geometry::{cuboid, Cuboid, Cylinder, Triangle}; +use crate::math::{Point, Vector}; +use approx::AbsDiffEq; +use na::{Unit, Vector2, Vector3}; +use ncollide::shape::Segment; +use ncollide::shape::SupportMap; + +/// Trait implemented by convex shapes with features with polyhedral approximations. +pub trait PolygonalFeatureMap: SupportMap { + fn local_support_feature(&self, dir: &Unit>, out_feature: &mut PolyhedronFace); +} + +impl PolygonalFeatureMap for Segment { + fn local_support_feature(&self, _: &Unit>, out_feature: &mut PolyhedronFace) { + *out_feature = PolyhedronFace::from(*self); + } +} + +impl PolygonalFeatureMap for Triangle { + fn local_support_feature(&self, _: &Unit>, out_feature: &mut PolyhedronFace) { + *out_feature = PolyhedronFace::from(*self); + } +} + +impl PolygonalFeatureMap for Cuboid { + fn local_support_feature(&self, dir: &Unit>, out_feature: &mut PolyhedronFace) { + let face = cuboid::support_face(self, **dir); + *out_feature = PolyhedronFace::from(face); + } +} + +impl PolygonalFeatureMap for Cylinder { + fn local_support_feature(&self, dir: &Unit>, out_features: &mut PolyhedronFace) { + let dir2 = Vector2::new(dir.x, dir.z) + .try_normalize(f32::default_epsilon()) + .unwrap_or(Vector2::x()); + + if dir.y.abs() < 0.5 { + // We return a segment lying on the cylinder'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(dir2.x * self.radius, self.half_height, dir2.y * self.radius); + out_features.eids = [0, 0, 0, 0]; // FIXME + out_features.fid = 1; + out_features.num_vertices = 2; + out_features.vids = [0, 1, 1, 1]; // FIXME + } else { + // We return a square approximation of the cylinder cap. + let y = self.half_height.copysign(dir.y); + 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 = [0, 1, 2, 3]; // FIXME + out_features.fid = if dir.y < 0.0 { 0 } else { 2 }; + out_features.num_vertices = 4; + out_features.vids = [0, 1, 2, 3]; // FIXME + } + } +} diff --git a/src/geometry/polyhedron_feature3d.rs b/src/geometry/polyhedron_feature3d.rs index dfeee29..5655f64 100644 --- a/src/geometry/polyhedron_feature3d.rs +++ b/src/geometry/polyhedron_feature3d.rs @@ -50,6 +50,16 @@ impl From> for PolyhedronFace { } impl PolyhedronFace { + pub fn new() -> Self { + Self { + vertices: [Point::origin(); 4], + vids: [0; 4], + eids: [0; 4], + fid: 0, + num_vertices: 0, + } + } + pub fn transform_by(&mut self, iso: &Isometry) { for v in &mut self.vertices[0..self.num_vertices] { *v = iso * *v; -- cgit From faf3e7e0f7f2b528da99343f9a3f8ce2b8fa6876 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Tue, 13 Oct 2020 18:40:59 +0200 Subject: Implement a special case for edge-edge 3D polygonal clipping. --- src/geometry/contact.rs | 8 +- .../cuboid_capsule_contact_generator.rs | 4 +- .../cuboid_cuboid_contact_generator.rs | 2 +- .../cuboid_triangle_contact_generator.rs | 4 +- src/geometry/contact_generator/mod.rs | 5 +- .../contact_generator/pfm_pfm_contact_generator.rs | 159 +++++++++++++++++---- .../polygon_polygon_contact_generator.rs | 2 +- src/geometry/mod.rs | 3 +- src/geometry/polyhedron_feature3d.rs | 137 +++++++++++++++++- 9 files changed, 283 insertions(+), 41 deletions(-) (limited to 'src/geometry') diff --git a/src/geometry/contact.rs b/src/geometry/contact.rs index 7e235c2..782175a 100644 --- a/src/geometry/contact.rs +++ b/src/geometry/contact.rs @@ -429,7 +429,7 @@ impl ContactManifold { } #[inline] - pub(crate) fn try_update_contacts(&mut self, pos12: &Isometry) -> bool { + pub(crate) fn try_update_contacts(&mut self, pos12: &Isometry, early_stop: bool) -> bool { if self.points.len() == 0 { return false; } @@ -439,7 +439,7 @@ impl ContactManifold { let local_n2 = pos12 * self.local_n2; - if -self.local_n1.dot(&local_n2) < DOT_THRESHOLD { + if early_stop && -self.local_n1.dot(&local_n2) < DOT_THRESHOLD { return false; } @@ -448,7 +448,7 @@ impl ContactManifold { let dpt = local_p2 - pt.local_p1; let dist = dpt.dot(&self.local_n1); - if dist * pt.dist < 0.0 { + if early_stop && dist * pt.dist < 0.0 { // We switched between penetrating/non-penetrating. // The may result in other contacts to appear. return false; @@ -456,7 +456,7 @@ impl ContactManifold { let new_local_p1 = local_p2 - self.local_n1 * dist; let dist_threshold = 0.001; // FIXME: this should not be hard-coded. - if na::distance_squared(&pt.local_p1, &new_local_p1) > dist_threshold { + if early_stop && na::distance_squared(&pt.local_p1, &new_local_p1) > dist_threshold { return false; } diff --git a/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs b/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs index a7857a1..c94b300 100644 --- a/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs +++ b/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs @@ -47,8 +47,8 @@ pub fn generate_contacts<'a>( let mut pos12 = pos1.inverse() * pos2; let mut pos21 = pos12.inverse(); - if (!swapped && manifold.try_update_contacts(&pos12)) - || (swapped && manifold.try_update_contacts(&pos21)) + if (!swapped && manifold.try_update_contacts(&pos12, true)) + || (swapped && manifold.try_update_contacts(&pos21, true)) { return; } diff --git a/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs b/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs index d879a22..04ac43a 100644 --- a/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs +++ b/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs @@ -34,7 +34,7 @@ pub fn generate_contacts<'a>( let mut pos12 = pos1.inverse() * pos2; let mut pos21 = pos12.inverse(); - if manifold.try_update_contacts(&pos12) { + if manifold.try_update_contacts(&pos12, true) { return; } diff --git a/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs b/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs index 1a0358d..d73e2eb 100644 --- a/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs +++ b/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs @@ -48,8 +48,8 @@ pub fn generate_contacts<'a>( let mut pos12 = pos1.inverse() * pos2; let mut pos21 = pos12.inverse(); - if (!swapped && manifold.try_update_contacts(&pos12)) - || (swapped && manifold.try_update_contacts(&pos21)) + if (!swapped && manifold.try_update_contacts(&pos12, true)) + || (swapped && manifold.try_update_contacts(&pos21, true)) { return; } diff --git a/src/geometry/contact_generator/mod.rs b/src/geometry/contact_generator/mod.rs index a6bad05..d8a523f 100644 --- a/src/geometry/contact_generator/mod.rs +++ b/src/geometry/contact_generator/mod.rs @@ -27,10 +27,9 @@ pub use self::trimesh_shape_contact_generator::{ generate_contacts_trimesh_shape, TrimeshShapeContactGeneratorWorkspace, }; +pub(crate) use self::polygon_polygon_contact_generator::clip_segments; #[cfg(feature = "dim2")] -pub(crate) use self::polygon_polygon_contact_generator::{ - clip_segments, clip_segments_with_normal, -}; +pub(crate) use self::polygon_polygon_contact_generator::clip_segments_with_normal; mod ball_ball_contact_generator; mod ball_convex_contact_generator; diff --git a/src/geometry/contact_generator/pfm_pfm_contact_generator.rs b/src/geometry/contact_generator/pfm_pfm_contact_generator.rs index cfb4472..a5e8014 100644 --- a/src/geometry/contact_generator/pfm_pfm_contact_generator.rs +++ b/src/geometry/contact_generator/pfm_pfm_contact_generator.rs @@ -1,6 +1,9 @@ use crate::geometry::contact_generator::PrimitiveContactGenerationContext; -use crate::geometry::{Contact, KinematicsCategory, PolygonalFeatureMap, PolyhedronFace}; +use crate::geometry::{ + Contact, ContactManifold, KinematicsCategory, PolygonalFeatureMap, PolyhedronFace, +}; use crate::math::{Isometry, Vector}; +use crate::na::UnitQuaternion; use na::Unit; use ncollide::query; use ncollide::query::algorithms::{gjk::GJKResult, VoronoiSimplex}; @@ -44,7 +47,7 @@ fn do_generate_contacts( let pos12 = ctxt.position1.inverse() * ctxt.position2; let pos21 = pos12.inverse(); - // if ctxt.manifold.try_update_contacts(&pos12) { + // if ctxt.manifold.try_update_contacts(&pos12, true) { // return; // } @@ -77,27 +80,86 @@ fn do_generate_contacts( pfm2.local_support_feature(&normal2, &mut workspace.feature2); workspace.feature2.transform_by(&pos12); - // PolyhedronFace::contacts( - // ctxt.prediction_distance, - // &workspace.feature1, - // &normal1, - // &workspace.feature2, - // &pos21, - // ctxt.manifold, - // ); - - println!( - "Contact patatrac: {:?}, {:?}, {}, {}", - ctxt.manifold.points.len(), - ctxt.position1 * dir, - workspace.feature1.num_vertices, - workspace.feature2.num_vertices + PolyhedronFace::contacts( + ctxt.prediction_distance, + &workspace.feature1, + &normal1, + &workspace.feature2, + &pos21, + ctxt.manifold, ); - if ctxt.manifold.all_contacts().is_empty() { + // if ctxt.manifold.all_contacts().is_empty() { + // // Add at least the deepest contact. + // let dist = (local_p2 - local_p1).dot(&dir); + // ctxt.manifold.points.push(Contact { + // local_p1, + // local_p2: pos21 * local_p2, + // impulse: 0.0, + // tangent_impulse: Contact::zero_tangent_impulse(), + // fid1: 0, // FIXME + // fid2: 0, // FIXME + // dist, + // }); + // } + + // Adjust points to take the radius into account. + ctxt.manifold.local_n1 = *normal1; + ctxt.manifold.local_n2 = *normal2; + ctxt.manifold.kinematics.category = KinematicsCategory::PlanePoint; // FIXME + ctxt.manifold.kinematics.radius1 = 0.0; + ctxt.manifold.kinematics.radius2 = 0.0; + } + GJKResult::NoIntersection(dir) => { + workspace.last_gjk_dir = Some(dir); + } + _ => {} + } +} + +fn do_generate_contacts2( + pfm1: &dyn PolygonalFeatureMap, + pfm2: &dyn PolygonalFeatureMap, + ctxt: &mut PrimitiveContactGenerationContext, +) { + let pos12 = ctxt.position1.inverse() * ctxt.position2; + let pos21 = pos12.inverse(); + + // if ctxt.manifold.try_update_contacts(&pos12, true) { + // return; + // } + + let workspace: &mut PfmPfmContactManifoldGeneratorWorkspace = ctxt + .workspace + .as_mut() + .expect("The PfmPfmContactManifoldGeneratorWorkspace is missing.") + .downcast_mut() + .expect("Invalid workspace type, expected a PfmPfmContactManifoldGeneratorWorkspace."); + + fn generate_single_contact_pair( + pfm1: &dyn PolygonalFeatureMap, + pfm2: &dyn PolygonalFeatureMap, + pos12: &Isometry, + pos21: &Isometry, + prediction_distance: f32, + manifold: &mut ContactManifold, + workspace: &mut PfmPfmContactManifoldGeneratorWorkspace, + ) -> Option>> { + let contact = query::contact_support_map_support_map_with_params( + &Isometry::identity(), + pfm1, + &pos12, + pfm2, + prediction_distance, + &mut workspace.simplex, + workspace.last_gjk_dir, + ); + + match contact { + GJKResult::ClosestPoints(local_p1, local_p2, dir) => { // Add at least the deepest contact. let dist = (local_p2 - local_p1).dot(&dir); - ctxt.manifold.points.push(Contact { + manifold.points.push(Contact { local_p1, local_p2: pos21 * local_p2, impulse: 0.0, @@ -106,18 +168,63 @@ fn do_generate_contacts( fid2: 0, // FIXME dist, }); + + Some(dir) } + GJKResult::NoIntersection(dir) => Some(dir), + _ => None, + } + } - // Adjust points to take the radius into account. - ctxt.manifold.local_n1 = *normal1; - ctxt.manifold.local_n2 = *normal2; + let old_manifold_points = ctxt.manifold.points.clone(); + ctxt.manifold.points.clear(); + + if let Some(local_n1) = generate_single_contact_pair( + pfm1, + pfm2, + &pos12, + &pos21, + ctxt.prediction_distance, + ctxt.manifold, + workspace, + ) { + workspace.last_gjk_dir = Some(local_n1); + + if !ctxt.manifold.points.is_empty() { + use crate::utils::WBasis; + // Use perturbations to generate other contact points. + let basis = local_n1.orthonormal_basis(); + let perturbation_angle = std::f32::consts::PI / 180.0 * 15.0; // FIXME: this should be a function of the shape size. + let perturbations = [ + UnitQuaternion::new(basis[0] * perturbation_angle), + UnitQuaternion::new(basis[0] * -perturbation_angle), + UnitQuaternion::new(basis[1] * perturbation_angle), + UnitQuaternion::new(basis[1] * -perturbation_angle), + ]; + + for rot in &perturbations { + let new_pos12 = pos12 * rot; + let new_pos21 = new_pos12.inverse(); + generate_single_contact_pair( + pfm1, + pfm2, + &new_pos12, + &new_pos21, + ctxt.prediction_distance, + ctxt.manifold, + workspace, + ); + println!("After perturbation: {}", ctxt.manifold.points.len()); + } + + // Set manifold normal. + ctxt.manifold.local_n1 = *local_n1; + ctxt.manifold.local_n2 = pos21 * -*local_n1; ctxt.manifold.kinematics.category = KinematicsCategory::PlanePoint; // FIXME ctxt.manifold.kinematics.radius1 = 0.0; ctxt.manifold.kinematics.radius2 = 0.0; + + ctxt.manifold.try_update_contacts(&pos12, false); } - GJKResult::NoIntersection(dir) => { - workspace.last_gjk_dir = Some(dir); - } - _ => {} } } diff --git a/src/geometry/contact_generator/polygon_polygon_contact_generator.rs b/src/geometry/contact_generator/polygon_polygon_contact_generator.rs index 33b54e4..9fc1591 100644 --- a/src/geometry/contact_generator/polygon_polygon_contact_generator.rs +++ b/src/geometry/contact_generator/polygon_polygon_contact_generator.rs @@ -31,7 +31,7 @@ fn generate_contacts<'a>( let mut m12 = m1.inverse() * m2; let mut m21 = m12.inverse(); - if manifold.try_update_contacts(&m12) { + if manifold.try_update_contacts(&m12, true) { return; } diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs index 1ccb2c8..efbb35c 100644 --- a/src/geometry/mod.rs +++ b/src/geometry/mod.rs @@ -50,8 +50,9 @@ pub(crate) use self::broad_phase_multi_sap::{BroadPhasePairEvent, ColliderPair}; pub(crate) use self::collider_set::RemovedCollider; #[cfg(feature = "simd-is-enabled")] pub(crate) use self::contact::WContact; +pub(crate) use self::contact_generator::clip_segments; #[cfg(feature = "dim2")] -pub(crate) use self::contact_generator::{clip_segments, clip_segments_with_normal}; +pub(crate) use self::contact_generator::clip_segments_with_normal; pub(crate) use self::narrow_phase::ContactManifoldIndex; #[cfg(feature = "dim3")] pub(crate) use self::polygonal_feature_map::PolygonalFeatureMap; diff --git a/src/geometry/polyhedron_feature3d.rs b/src/geometry/polyhedron_feature3d.rs index 5655f64..8f5de04 100644 --- a/src/geometry/polyhedron_feature3d.rs +++ b/src/geometry/polyhedron_feature3d.rs @@ -1,4 +1,5 @@ -use crate::geometry::{Contact, ContactManifold, CuboidFeatureFace, Triangle}; +use crate::approx::AbsDiffEq; +use crate::geometry::{self, Contact, ContactManifold, CuboidFeatureFace, Triangle}; use crate::math::{Isometry, Point, Vector}; use crate::utils::WBasis; use na::Point2; @@ -73,6 +74,140 @@ impl PolyhedronFace { face2: &PolyhedronFace, pos21: &Isometry, manifold: &mut ContactManifold, + ) { + match (face1.num_vertices, face2.num_vertices) { + (2, 2) => Self::contacts_edge_edge( + prediction_distance, + face1, + sep_axis1, + face2, + pos21, + manifold, + ), + _ => Self::contacts_face_face( + prediction_distance, + face1, + sep_axis1, + face2, + pos21, + manifold, + ), + } + } + + fn contacts_edge_edge( + prediction_distance: f32, + face1: &PolyhedronFace, + sep_axis1: &Vector, + face2: &PolyhedronFace, + pos21: &Isometry, + manifold: &mut ContactManifold, + ) { + // Project the faces to a 2D plane for contact clipping. + // The plane they are projected onto has normal sep_axis1 + // and contains the origin (this is numerically OK because + // we are not working in world-space here). + let basis = sep_axis1.orthonormal_basis(); + let projected_edge1 = [ + Point2::new( + face1.vertices[0].coords.dot(&basis[0]), + face1.vertices[0].coords.dot(&basis[1]), + ), + Point2::new( + face1.vertices[1].coords.dot(&basis[0]), + face1.vertices[1].coords.dot(&basis[1]), + ), + ]; + let projected_edge2 = [ + Point2::new( + face2.vertices[0].coords.dot(&basis[0]), + face2.vertices[0].coords.dot(&basis[1]), + ), + Point2::new( + face2.vertices[1].coords.dot(&basis[0]), + face2.vertices[1].coords.dot(&basis[1]), + ), + ]; + + let tangent1 = + (projected_edge1[1] - projected_edge1[0]).try_normalize(f32::default_epsilon()); + let tangent2 = + (projected_edge2[1] - projected_edge2[0]).try_normalize(f32::default_epsilon()); + + // TODO: not sure what the best value for eps is. + // Empirically, it appears that an epsilon smaller than 1.0e-3 is too small. + if let (Some(tangent1), Some(tangent2)) = (tangent1, tangent2) { + let parallel = tangent1.dot(&tangent2) >= crate::utils::COS_FRAC_PI_8; + + if !parallel { + let seg1 = (&projected_edge1[0], &projected_edge1[1]); + let seg2 = (&projected_edge2[0], &projected_edge2[1]); + let (loc1, loc2) = + ncollide::query::closest_points_segment_segment_with_locations_nD(seg1, seg2); + + // Found a contact between the two edges. + let bcoords1 = loc1.barycentric_coordinates(); + let bcoords2 = loc2.barycentric_coordinates(); + + let edge1 = (face1.vertices[0], face1.vertices[1]); + let edge2 = (face2.vertices[0], face2.vertices[1]); + let local_p1 = edge1.0 * bcoords1[0] + edge1.1.coords * bcoords1[1]; + let local_p2 = edge2.0 * bcoords2[0] + edge2.1.coords * bcoords2[1]; + let dist = (local_p2 - local_p1).dot(&sep_axis1); + + if dist <= prediction_distance { + manifold.points.push(Contact { + local_p1, + local_p2: pos21 * local_p2, + impulse: 0.0, + tangent_impulse: Contact::zero_tangent_impulse(), + fid1: face1.eids[0], + fid2: face2.eids[0], + dist, + }); + } + + return; + } + } + + // The lines are parallel so we are having a conformal contact. + // Let's use a range-based clipping to extract two contact points. + // TODO: would it be better and/or more efficient to do the + //clipping in 2D? + if let Some(clips) = geometry::clip_segments( + (face1.vertices[0], face1.vertices[1]), + (face2.vertices[0], face2.vertices[1]), + ) { + manifold.points.push(Contact { + local_p1: (clips.0).0, + local_p2: pos21 * (clips.0).1, + impulse: 0.0, + tangent_impulse: Contact::zero_tangent_impulse(), + fid1: 0, // FIXME + fid2: 0, // FIXME + dist: ((clips.0).1 - (clips.0).0).dot(&sep_axis1), + }); + + manifold.points.push(Contact { + local_p1: (clips.1).0, + local_p2: pos21 * (clips.1).1, + impulse: 0.0, + tangent_impulse: Contact::zero_tangent_impulse(), + fid1: 0, // FIXME + fid2: 0, // FIXME + dist: ((clips.1).1 - (clips.1).0).dot(&sep_axis1), + }); + } + } + + fn contacts_face_face( + prediction_distance: f32, + face1: &PolyhedronFace, + sep_axis1: &Vector, + face2: &PolyhedronFace, + pos21: &Isometry, + manifold: &mut ContactManifold, ) { // Project the faces to a 2D plane for contact clipping. // The plane they are projected onto has normal sep_axis1 -- cgit From 947c4813c9666fd8215743de298fe17780fa3ef2 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 19 Oct 2020 16:51:40 +0200 Subject: Complete the pfm/pfm contact generator. --- src/geometry/contact.rs | 25 ++-- .../cuboid_capsule_contact_generator.rs | 4 +- .../cuboid_cuboid_contact_generator.rs | 2 +- .../cuboid_triangle_contact_generator.rs | 4 +- .../contact_generator/pfm_pfm_contact_generator.rs | 147 +++------------------ .../polygon_polygon_contact_generator.rs | 2 +- src/geometry/polygonal_feature_map.rs | 36 ++++- 7 files changed, 67 insertions(+), 153 deletions(-) (limited to 'src/geometry') diff --git a/src/geometry/contact.rs b/src/geometry/contact.rs index 782175a..d211cf1 100644 --- a/src/geometry/contact.rs +++ b/src/geometry/contact.rs @@ -429,17 +429,27 @@ impl ContactManifold { } #[inline] - pub(crate) fn try_update_contacts(&mut self, pos12: &Isometry, early_stop: bool) -> bool { + pub(crate) fn try_update_contacts(&mut self, pos12: &Isometry) -> bool { + // const DOT_THRESHOLD: f32 = 0.crate::COS_10_DEGREES; + const DOT_THRESHOLD: f32 = crate::utils::COS_5_DEGREES; + const DIST_SQ_THRESHOLD: f32 = 0.001; // FIXME: this should not be hard-coded. + self.try_update_contacts_eps(pos12, DOT_THRESHOLD, DIST_SQ_THRESHOLD) + } + + #[inline] + pub(crate) fn try_update_contacts_eps( + &mut self, + pos12: &Isometry, + angle_dot_threshold: f32, + dist_sq_threshold: f32, + ) -> bool { if self.points.len() == 0 { return false; } - // const DOT_THRESHOLD: f32 = 0.crate::COS_10_DEGREES; - const DOT_THRESHOLD: f32 = crate::utils::COS_5_DEGREES; - let local_n2 = pos12 * self.local_n2; - if early_stop && -self.local_n1.dot(&local_n2) < DOT_THRESHOLD { + if -self.local_n1.dot(&local_n2) < angle_dot_threshold { return false; } @@ -448,15 +458,14 @@ impl ContactManifold { let dpt = local_p2 - pt.local_p1; let dist = dpt.dot(&self.local_n1); - if early_stop && dist * pt.dist < 0.0 { + if dist * pt.dist < 0.0 { // We switched between penetrating/non-penetrating. // The may result in other contacts to appear. return false; } let new_local_p1 = local_p2 - self.local_n1 * dist; - let dist_threshold = 0.001; // FIXME: this should not be hard-coded. - if early_stop && na::distance_squared(&pt.local_p1, &new_local_p1) > dist_threshold { + if na::distance_squared(&pt.local_p1, &new_local_p1) > dist_sq_threshold { return false; } diff --git a/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs b/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs index c94b300..a7857a1 100644 --- a/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs +++ b/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs @@ -47,8 +47,8 @@ pub fn generate_contacts<'a>( let mut pos12 = pos1.inverse() * pos2; let mut pos21 = pos12.inverse(); - if (!swapped && manifold.try_update_contacts(&pos12, true)) - || (swapped && manifold.try_update_contacts(&pos21, true)) + if (!swapped && manifold.try_update_contacts(&pos12)) + || (swapped && manifold.try_update_contacts(&pos21)) { return; } diff --git a/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs b/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs index 04ac43a..d879a22 100644 --- a/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs +++ b/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs @@ -34,7 +34,7 @@ pub fn generate_contacts<'a>( let mut pos12 = pos1.inverse() * pos2; let mut pos21 = pos12.inverse(); - if manifold.try_update_contacts(&pos12, true) { + if manifold.try_update_contacts(&pos12) { return; } diff --git a/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs b/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs index d73e2eb..1a0358d 100644 --- a/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs +++ b/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs @@ -48,8 +48,8 @@ pub fn generate_contacts<'a>( let mut pos12 = pos1.inverse() * pos2; let mut pos21 = pos12.inverse(); - if (!swapped && manifold.try_update_contacts(&pos12, true)) - || (swapped && manifold.try_update_contacts(&pos21, true)) + if (!swapped && manifold.try_update_contacts(&pos12)) + || (swapped && manifold.try_update_contacts(&pos21)) { return; } diff --git a/src/geometry/contact_generator/pfm_pfm_contact_generator.rs b/src/geometry/contact_generator/pfm_pfm_contact_generator.rs index a5e8014..c3815dd 100644 --- a/src/geometry/contact_generator/pfm_pfm_contact_generator.rs +++ b/src/geometry/contact_generator/pfm_pfm_contact_generator.rs @@ -30,8 +30,8 @@ impl Default for PfmPfmContactManifoldGeneratorWorkspace { pub fn generate_contacts_pfm_pfm(ctxt: &mut PrimitiveContactGenerationContext) { if let (Some(pfm1), Some(pfm2)) = ( - ctxt.collider1.shape().as_polygonal_feature_map(), - ctxt.collider2.shape().as_polygonal_feature_map(), + ctxt.shape1.as_polygonal_feature_map(), + ctxt.shape2.as_polygonal_feature_map(), ) { do_generate_contacts(pfm1, pfm2, ctxt); ctxt.manifold.update_warmstart_multiplier(); @@ -47,9 +47,15 @@ fn do_generate_contacts( let pos12 = ctxt.position1.inverse() * ctxt.position2; let pos21 = pos12.inverse(); - // if ctxt.manifold.try_update_contacts(&pos12, true) { - // return; - // } + // We use very small thresholds for the manifold update because something to high would + // cause numerical drifts with the effect of introducing bumps in + // what should have been smooth rolling motions. + if ctxt + .manifold + .try_update_contacts_eps(&pos12, crate::utils::COS_1_DEGREES, 1.0e-6) + { + return; + } let workspace: &mut PfmPfmContactManifoldGeneratorWorkspace = ctxt .workspace @@ -72,7 +78,7 @@ fn do_generate_contacts( ctxt.manifold.points.clear(); match contact { - GJKResult::ClosestPoints(local_p1, local_p2, dir) => { + GJKResult::ClosestPoints(_, _, dir) => { workspace.last_gjk_dir = Some(dir); let normal1 = dir; let normal2 = pos21 * -dir; @@ -89,24 +95,10 @@ fn do_generate_contacts( ctxt.manifold, ); - // if ctxt.manifold.all_contacts().is_empty() { - // // Add at least the deepest contact. - // let dist = (local_p2 - local_p1).dot(&dir); - // ctxt.manifold.points.push(Contact { - // local_p1, - // local_p2: pos21 * local_p2, - // impulse: 0.0, - // tangent_impulse: Contact::zero_tangent_impulse(), - // fid1: 0, // FIXME - // fid2: 0, // FIXME - // dist, - // }); - // } - // Adjust points to take the radius into account. ctxt.manifold.local_n1 = *normal1; ctxt.manifold.local_n2 = *normal2; - ctxt.manifold.kinematics.category = KinematicsCategory::PlanePoint; // FIXME + ctxt.manifold.kinematics.category = KinematicsCategory::PlanePoint; // TODO: is this the more appropriate? ctxt.manifold.kinematics.radius1 = 0.0; ctxt.manifold.kinematics.radius2 = 0.0; } @@ -115,116 +107,7 @@ fn do_generate_contacts( } _ => {} } -} - -fn do_generate_contacts2( - pfm1: &dyn PolygonalFeatureMap, - pfm2: &dyn PolygonalFeatureMap, - ctxt: &mut PrimitiveContactGenerationContext, -) { - let pos12 = ctxt.position1.inverse() * ctxt.position2; - let pos21 = pos12.inverse(); - - // if ctxt.manifold.try_update_contacts(&pos12, true) { - // return; - // } - - let workspace: &mut PfmPfmContactManifoldGeneratorWorkspace = ctxt - .workspace - .as_mut() - .expect("The PfmPfmContactManifoldGeneratorWorkspace is missing.") - .downcast_mut() - .expect("Invalid workspace type, expected a PfmPfmContactManifoldGeneratorWorkspace."); - - fn generate_single_contact_pair( - pfm1: &dyn PolygonalFeatureMap, - pfm2: &dyn PolygonalFeatureMap, - pos12: &Isometry, - pos21: &Isometry, - prediction_distance: f32, - manifold: &mut ContactManifold, - workspace: &mut PfmPfmContactManifoldGeneratorWorkspace, - ) -> Option>> { - let contact = query::contact_support_map_support_map_with_params( - &Isometry::identity(), - pfm1, - &pos12, - pfm2, - prediction_distance, - &mut workspace.simplex, - workspace.last_gjk_dir, - ); - - match contact { - GJKResult::ClosestPoints(local_p1, local_p2, dir) => { - // Add at least the deepest contact. - let dist = (local_p2 - local_p1).dot(&dir); - manifold.points.push(Contact { - local_p1, - local_p2: pos21 * local_p2, - impulse: 0.0, - tangent_impulse: Contact::zero_tangent_impulse(), - fid1: 0, // FIXME - fid2: 0, // FIXME - dist, - }); - Some(dir) - } - GJKResult::NoIntersection(dir) => Some(dir), - _ => None, - } - } - - let old_manifold_points = ctxt.manifold.points.clone(); - ctxt.manifold.points.clear(); - - if let Some(local_n1) = generate_single_contact_pair( - pfm1, - pfm2, - &pos12, - &pos21, - ctxt.prediction_distance, - ctxt.manifold, - workspace, - ) { - workspace.last_gjk_dir = Some(local_n1); - - if !ctxt.manifold.points.is_empty() { - use crate::utils::WBasis; - // Use perturbations to generate other contact points. - let basis = local_n1.orthonormal_basis(); - let perturbation_angle = std::f32::consts::PI / 180.0 * 15.0; // FIXME: this should be a function of the shape size. - let perturbations = [ - UnitQuaternion::new(basis[0] * perturbation_angle), - UnitQuaternion::new(basis[0] * -perturbation_angle), - UnitQuaternion::new(basis[1] * perturbation_angle), - UnitQuaternion::new(basis[1] * -perturbation_angle), - ]; - - for rot in &perturbations { - let new_pos12 = pos12 * rot; - let new_pos21 = new_pos12.inverse(); - generate_single_contact_pair( - pfm1, - pfm2, - &new_pos12, - &new_pos21, - ctxt.prediction_distance, - ctxt.manifold, - workspace, - ); - println!("After perturbation: {}", ctxt.manifold.points.len()); - } - - // Set manifold normal. - ctxt.manifold.local_n1 = *local_n1; - ctxt.manifold.local_n2 = pos21 * -*local_n1; - ctxt.manifold.kinematics.category = KinematicsCategory::PlanePoint; // FIXME - ctxt.manifold.kinematics.radius1 = 0.0; - ctxt.manifold.kinematics.radius2 = 0.0; - - ctxt.manifold.try_update_contacts(&pos12, false); - } - } + // Transfer impulses. + super::match_contacts(&mut ctxt.manifold, &old_manifold_points, false); } diff --git a/src/geometry/contact_generator/polygon_polygon_contact_generator.rs b/src/geometry/contact_generator/polygon_polygon_contact_generator.rs index 9fc1591..33b54e4 100644 --- a/src/geometry/contact_generator/polygon_polygon_contact_generator.rs +++ b/src/geometry/contact_generator/polygon_polygon_contact_generator.rs @@ -31,7 +31,7 @@ fn generate_contacts<'a>( let mut m12 = m1.inverse() * m2; let mut m21 = m12.inverse(); - if manifold.try_update_contacts(&m12, true) { + if manifold.try_update_contacts(&m12) { return; } diff --git a/src/geometry/polygonal_feature_map.rs b/src/geometry/polygonal_feature_map.rs index 8b047fc..70be5d0 100644 --- a/src/geometry/polygonal_feature_map.rs +++ b/src/geometry/polygonal_feature_map.rs @@ -32,6 +32,20 @@ impl PolygonalFeatureMap for Cuboid { impl PolygonalFeatureMap for Cylinder { fn local_support_feature(&self, dir: &Unit>, out_features: &mut PolyhedronFace) { + // About feature ids. + // At all times, we consider our cylinder to be approximated as follows: + // - The curved part is approximated by a single segment. + // - Each flat cap of the cylinder 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. + // - The feature IDs of the top cap are the same as the bottom cap to which we add 10. + // So its vertices have IDs 11,13,15,17, its edges 12,14,16,18, and its face 19. + // - Note that at all times, one of each 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()); @@ -45,10 +59,10 @@ impl PolygonalFeatureMap for Cylinder { ); out_features.vertices[1] = Point::new(dir2.x * self.radius, self.half_height, dir2.y * self.radius); - out_features.eids = [0, 0, 0, 0]; // FIXME - out_features.fid = 1; + out_features.eids = [0, 0, 0, 0]; + out_features.fid = 0; out_features.num_vertices = 2; - out_features.vids = [0, 1, 1, 1]; // FIXME + out_features.vids = [1, 11, 11, 11]; } else { // We return a square approximation of the cylinder cap. let y = self.half_height.copysign(dir.y); @@ -56,10 +70,18 @@ impl PolygonalFeatureMap for Cylinder { 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 = [0, 1, 2, 3]; // FIXME - out_features.fid = if dir.y < 0.0 { 0 } else { 2 }; - out_features.num_vertices = 4; - out_features.vids = [0, 1, 2, 3]; // FIXME + + if dir.y < 0.0 { + out_features.eids = [2, 4, 6, 8]; + out_features.fid = 9; + out_features.num_vertices = 4; + out_features.vids = [1, 3, 5, 7]; + } else { + out_features.eids = [12, 14, 16, 18]; + out_features.fid = 19; + out_features.num_vertices = 4; + out_features.vids = [11, 13, 15, 17]; + } } } } -- cgit From 865ce8a8e5301b23ca474adaaffe8b43e725803e Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Tue, 20 Oct 2020 11:55:33 +0200 Subject: Collider shape: use a trait-object instead of an enum. --- src/geometry/broad_phase_multi_sap.rs | 16 +- src/geometry/collider.rs | 367 ++++++++++----------- .../ball_convex_contact_generator.rs | 23 +- .../capsule_capsule_contact_generator.rs | 15 +- .../contact_generator/contact_dispatcher.rs | 78 ++--- .../cuboid_capsule_contact_generator.rs | 8 +- .../cuboid_cuboid_contact_generator.rs | 2 +- .../cuboid_triangle_contact_generator.rs | 6 +- .../heightfield_shape_contact_generator.rs | 35 +- src/geometry/contact_generator/mod.rs | 2 +- .../polygon_polygon_contact_generator.rs | 29 +- .../trimesh_shape_contact_generator.rs | 11 +- src/geometry/mod.rs | 12 +- src/geometry/narrow_phase.rs | 12 +- .../ball_convex_proximity_detector.rs | 18 +- .../cuboid_cuboid_proximity_detector.rs | 2 +- .../cuboid_triangle_proximity_detector.rs | 6 +- .../polygon_polygon_proximity_detector.rs | 23 +- .../proximity_detector/proximity_dispatcher.rs | 40 +-- .../trimesh_shape_proximity_detector.rs | 11 +- src/geometry/rounded.rs | 7 + src/geometry/sat.rs | 20 +- src/geometry/shape.rs | 275 +++++++++++++++ src/geometry/trimesh.rs | 45 ++- 24 files changed, 676 insertions(+), 387 deletions(-) create mode 100644 src/geometry/rounded.rs create mode 100644 src/geometry/shape.rs (limited to 'src/geometry') diff --git a/src/geometry/broad_phase_multi_sap.rs b/src/geometry/broad_phase_multi_sap.rs index 8e69d24..3562c2e 100644 --- a/src/geometry/broad_phase_multi_sap.rs +++ b/src/geometry/broad_phase_multi_sap.rs @@ -335,7 +335,7 @@ impl SAPAxis { #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] struct SAPRegion { - axii: [SAPAxis; DIM], + axes: [SAPAxis; DIM], existing_proxies: BitVec, #[cfg_attr(feature = "serde-serialize", serde(skip))] to_insert: Vec, // Workspace @@ -344,14 +344,14 @@ struct SAPRegion { impl SAPRegion { pub fn new(bounds: AABB) -> Self { - let axii = [ + let axes = [ SAPAxis::new(bounds.mins.x, bounds.maxs.x), SAPAxis::new(bounds.mins.y, bounds.maxs.y), #[cfg(feature = "dim3")] SAPAxis::new(bounds.mins.z, bounds.maxs.z), ]; SAPRegion { - axii, + axes, existing_proxies: BitVec::new(), to_insert: Vec::new(), need_update: false, @@ -386,15 +386,15 @@ impl SAPRegion { // Update endpoints. let mut deleted_any = false; for dim in 0..DIM { - self.axii[dim].update_endpoints(dim, proxies, reporting); - deleted_any = self.axii[dim] + self.axes[dim].update_endpoints(dim, proxies, reporting); + deleted_any = self.axes[dim] .delete_out_of_bounds_proxies(&mut self.existing_proxies) || deleted_any; } if deleted_any { for dim in 0..DIM { - self.axii[dim].delete_out_of_bounds_endpoints(&self.existing_proxies); + self.axes[dim].delete_out_of_bounds_endpoints(&self.existing_proxies); } } @@ -404,9 +404,9 @@ impl SAPRegion { if !self.to_insert.is_empty() { // Insert new proxies. for dim in 1..DIM { - self.axii[dim].batch_insert(dim, &self.to_insert, proxies, None); + self.axes[dim].batch_insert(dim, &self.to_insert, proxies, None); } - self.axii[0].batch_insert(0, &self.to_insert, proxies, Some(reporting)); + self.axes[0].batch_insert(0, &self.to_insert, proxies, Some(reporting)); self.to_insert.clear(); } } diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs index fe42bd7..c6cf6d0 100644 --- a/src/geometry/collider.rs +++ b/src/geometry/collider.rs @@ -3,172 +3,186 @@ use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet}; use crate::geometry::PolygonalFeatureMap; use crate::geometry::{ Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, Cylinder, HeightField, InteractionGraph, - Polygon, Proximity, Ray, RayIntersection, Triangle, Trimesh, + Polygon, Proximity, Ray, RayIntersection, Shape, ShapeType, Triangle, Trimesh, }; 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::query::RayCast; use num::Zero; +use std::any::Any; +use std::ops::Deref; +use std::sync::Arc; +/// The shape of a collider. #[derive(Clone)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -/// An enum grouping all the possible shape of a collider. -pub enum Shape { - /// A ball shape. - Ball(Ball), - /// A convex polygon shape. - Polygon(Polygon), - /// A cuboid shape. - Cuboid(Cuboid), - /// A capsule shape. - Capsule(Capsule), - /// A triangle shape. - Triangle(Triangle), - /// A triangle mesh shape. - Trimesh(Trimesh), - /// A heightfield shape. - HeightField(HeightField), - #[cfg(feature = "dim3")] - /// A cylindrical shape. - Cylinder(Cylinder), -} +pub struct ColliderShape(pub Arc); -impl Shape { - /// Gets a reference to the underlying ball shape, if `self` is one. - pub fn as_ball(&self) -> Option<&Ball> { - match self { - Shape::Ball(b) => Some(b), - _ => None, - } +impl Deref for ColliderShape { + type Target = Shape; + fn deref(&self) -> &Shape { + &*self.0 } +} - /// Gets a reference to the underlying polygon shape, if `self` is one. - pub fn as_polygon(&self) -> Option<&Polygon> { - match self { - Shape::Polygon(p) => Some(p), - _ => None, - } +impl ColliderShape { + /// Initialize a ball shape defined by its radius. + pub fn ball(radius: f32) -> Self { + ColliderShape(Arc::new(Ball::new(radius))) } - /// Gets a reference to the underlying cuboid shape, if `self` is one. - pub fn as_cuboid(&self) -> Option<&Cuboid> { - match self { - Shape::Cuboid(c) => Some(c), - _ => None, - } + /// Initialize a cylindrical shape defined by its half-height + /// (along along the y axis) and its radius. + #[cfg(feature = "dim3")] + pub fn cylinder(half_height: f32, radius: f32) -> Self { + ColliderShape(Arc::new(Cylinder::new(half_height, radius))) } - /// Gets a reference to the underlying capsule shape, if `self` is one. - pub fn as_capsule(&self) -> Option<&Capsule> { - match self { - Shape::Capsule(c) => Some(c), - _ => None, - } + /// Initialize a cuboid shape defined by its half-extents. + pub fn cuboid(half_extents: Vector) -> Self { + ColliderShape(Arc::new(Cuboid::new(half_extents))) } - /// Gets a reference to the underlying triangle mesh shape, if `self` is one. - pub fn as_trimesh(&self) -> Option<&Trimesh> { - match self { - Shape::Trimesh(c) => Some(c), - _ => None, - } + /// Initialize a capsule shape aligned with the `y` axis. + pub fn capsule(half_height: f32, radius: f32) -> Self { + ColliderShape(Arc::new(Capsule::new(half_height, radius))) } - /// Gets a reference to the underlying heightfield shape, if `self` is one. - pub fn as_heightfield(&self) -> Option<&HeightField> { - match self { - Shape::HeightField(h) => Some(h), - _ => None, - } + /// Initializes a triangle shape. + pub fn triangle(a: Point, b: Point, c: Point) -> Self { + ColliderShape(Arc::new(Triangle::new(a, b, c))) } - /// Gets a reference to the underlying triangle shape, if `self` is one. - pub fn as_triangle(&self) -> Option<&Triangle> { - match self { - Shape::Triangle(c) => Some(c), - _ => None, - } + /// Initializes a triangle mesh shape defined by its vertex and index buffers. + pub fn trimesh(vertices: Vec>, indices: Vec>) -> Self { + ColliderShape(Arc::new(Trimesh::new(vertices, indices))) } - /// Gets a reference to the underlying cylindrical shape, if `self` is one. - pub fn as_cylinder(&self) -> Option<&Cylinder> { - match self { - Shape::Cylinder(c) => Some(c), - _ => None, - } + /// Initializes an heightfield shape defined by its set of height and a scale + /// factor along each coordinate axis. + #[cfg(feature = "dim2")] + pub fn heightfield(heights: na::DVector, scale: Vector) -> Self { + ColliderShape(Arc::new(HeightField::new(heights, scale))) } - /// gets a reference to this shape seen as a PolygonalFeatureMap. + /// Initializes an heightfield shape on the x-z plane defined by its set of height and a scale + /// factor along each coordinate axis. #[cfg(feature = "dim3")] - pub fn as_polygonal_feature_map(&self) -> Option<&dyn PolygonalFeatureMap> { - match self { - Shape::Triangle(t) => Some(t), - Shape::Cuboid(c) => Some(c), - Shape::Cylinder(c) => Some(c), - _ => None, - } + pub fn heightfield(heights: na::DMatrix, scale: Vector) -> Self { + ColliderShape(Arc::new(HeightField::new(heights, scale))) } +} - /// Computes the axis-aligned bounding box of this shape. - pub fn compute_aabb(&self, position: &Isometry) -> AABB { - match self { - Shape::Ball(ball) => ball.bounding_v