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. --- .../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 +++++++++++++++++++++ 4 files changed, 143 insertions(+), 2 deletions(-) create mode 100644 src/geometry/contact_generator/pfm_pfm_contact_generator.rs (limited to 'src/geometry/contact_generator') 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); + } + _ => {} + } +} -- 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. --- .../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 +- 6 files changed, 141 insertions(+), 35 deletions(-) (limited to 'src/geometry/contact_generator') 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; } -- 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. --- .../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 +- 5 files changed, 21 insertions(+), 138 deletions(-) (limited to 'src/geometry/contact_generator') 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; } -- 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. --- .../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 +-- 10 files changed, 101 insertions(+), 108 deletions(-) (limited to 'src/geometry/contact_generator') diff --git a/src/geometry/contact_generator/ball_convex_contact_generator.rs b/src/geometry/contact_generator/ball_convex_contact_generator.rs index 0856029..62ebfab 100644 --- a/src/geometry/contact_generator/ball_convex_contact_generator.rs +++ b/src/geometry/contact_generator/ball_convex_contact_generator.rs @@ -5,30 +5,17 @@ use na::Unit; use ncollide::query::PointQuery; pub fn generate_contacts_ball_convex(ctxt: &mut PrimitiveContactGenerationContext) { - if let Shape::Ball(ball1) = ctxt.shape1 { + if let Some(ball1) = ctxt.shape1.as_ball() { ctxt.manifold.swap_identifiers(); - - match ctxt.shape2 { - 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 { - match ctxt.shape1 { - 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!(), - } + do_generate_contacts(ctxt.shape2, ball1, ctxt, true); + } else if let Some(ball2) = ctxt.shape2.as_ball() { + do_generate_contacts(ctxt.shape1, ball2, ctxt, false); } ctxt.manifold.sort_contacts(ctxt.prediction_distance); } -fn do_generate_contacts>( +fn do_generate_contacts>( point_query1: &P, ball2: &Ball, ctxt: &mut PrimitiveContactGenerationContext, diff --git a/src/geometry/contact_generator/capsule_capsule_contact_generator.rs b/src/geometry/contact_generator/capsule_capsule_contact_generator.rs index 3800ce6..4d9bbc7 100644 --- a/src/geometry/contact_generator/capsule_capsule_contact_generator.rs +++ b/src/geometry/contact_generator/capsule_capsule_contact_generator.rs @@ -8,7 +8,7 @@ use na::Unit; use ncollide::shape::{Segment, SegmentPointLocation}; pub fn generate_contacts_capsule_capsule(ctxt: &mut PrimitiveContactGenerationContext) { - if let (Shape::Capsule(capsule1), Shape::Capsule(capsule2)) = (ctxt.shape1, ctxt.shape2) { + if let (Some(capsule1), Some(capsule2)) = (ctxt.shape1.as_capsule(), ctxt.shape2.as_capsule()) { generate_contacts( ctxt.prediction_distance, capsule1, @@ -94,7 +94,7 @@ pub fn generate_contacts<'a>( if dir1.dot(&dir2).abs() >= crate::utils::COS_FRAC_PI_8 && dir1.dot(&local_n1).abs() < crate::utils::SIN_FRAC_PI_8 { - // Capsules axii are almost parallel and are almost perpendicular to the normal. + // Capsules axes are almost parallel and are almost perpendicular to the normal. // Find a second contact point. if let Some((clip_a, clip_b)) = crate::geometry::clip_segments_with_normal( (capsule1.a, capsule1.b), @@ -156,17 +156,18 @@ pub fn generate_contacts<'a>( let pos12 = pos1.inverse() * pos2; let pos21 = pos12.inverse(); - let capsule2_1 = capsule1.transform_by(&pos12); + let seg1 = capsule1.segment(); + let seg2_1 = capsule2.segment().transformed(&pos12); let (loc1, loc2) = ncollide::query::closest_points_segment_segment_with_locations_nD( - (&capsule1.a, &capsule1.b), - (&capsule2_1.a, &capsule2_1.b), + (&seg1.a, &seg1.b), + (&seg2_1.a, &seg2_1.b), ); { let bcoords1 = loc1.barycentric_coordinates(); let bcoords2 = loc2.barycentric_coordinates(); - let local_p1 = capsule1.a * bcoords1[0] + capsule1.b.coords * bcoords1[1]; - let local_p2 = capsule2_1.a * bcoords2[0] + capsule2_1.b.coords * bcoords2[1]; + let local_p1 = seg1.a * bcoords1[0] + seg1.b.coords * bcoords1[1]; + let local_p2 = seg2_1.a * bcoords2[0] + seg2_1.b.coords * bcoords2[1]; let local_n1 = Unit::try_new(local_p2 - local_p1, f32::default_epsilon()).unwrap_or(Vector::y_axis()); diff --git a/src/geometry/contact_generator/contact_dispatcher.rs b/src/geometry/contact_generator/contact_dispatcher.rs index e925fd5..01bbc46 100644 --- a/src/geometry/contact_generator/contact_dispatcher.rs +++ b/src/geometry/contact_generator/contact_dispatcher.rs @@ -3,7 +3,7 @@ use crate::geometry::contact_generator::{ PfmPfmContactManifoldGeneratorWorkspace, PrimitiveContactGenerator, TrimeshShapeContactGeneratorWorkspace, }; -use crate::geometry::Shape; +use crate::geometry::{Shape, ShapeType}; use std::any::Any; /// Trait implemented by structures responsible for selecting a collision-detection algorithm @@ -12,8 +12,8 @@ pub trait ContactDispatcher { /// Select the collision-detection algorithm for the given pair of primitive shapes. fn dispatch_primitives( &self, - shape1: &Shape, - shape2: &Shape, + shape1: ShapeType, + shape2: ShapeType, ) -> ( PrimitiveContactGenerator, Option>, @@ -21,8 +21,8 @@ pub trait ContactDispatcher { /// Select the collision-detection algorithm for the given pair of non-primitive shapes. fn dispatch( &self, - shape1: &Shape, - shape2: &Shape, + shape1: ShapeType, + shape2: ShapeType, ) -> (ContactPhase, Option>); } @@ -32,14 +32,14 @@ pub struct DefaultContactDispatcher; impl ContactDispatcher for DefaultContactDispatcher { fn dispatch_primitives( &self, - shape1: &Shape, - shape2: &Shape, + shape1: ShapeType, + shape2: ShapeType, ) -> ( PrimitiveContactGenerator, Option>, ) { match (shape1, shape2) { - (Shape::Ball(_), Shape::Ball(_)) => ( + (ShapeType::Ball, ShapeType::Ball) => ( PrimitiveContactGenerator { generate_contacts: super::generate_contacts_ball_ball, #[cfg(feature = "simd-is-enabled")] @@ -48,56 +48,58 @@ impl ContactDispatcher for DefaultContactDispatcher { }, None, ), - (Shape::Cuboid(_), Shape::Cuboid(_)) => ( + (ShapeType::Cuboid, ShapeType::Cuboid) => ( PrimitiveContactGenerator { generate_contacts: super::generate_contacts_cuboid_cuboid, ..PrimitiveContactGenerator::default() }, None, ), - (Shape::Polygon(_), Shape::Polygon(_)) => ( - PrimitiveContactGenerator { - generate_contacts: super::generate_contacts_polygon_polygon, - ..PrimitiveContactGenerator::default() - }, - None, - ), - (Shape::Capsule(_), Shape::Capsule(_)) => ( + // (ShapeType::Polygon, ShapeType::Polygon) => ( + // PrimitiveContactGenerator { + // generate_contacts: super::generate_contacts_polygon_polygon, + // ..PrimitiveContactGenerator::default() + // }, + // None, + // ), + (ShapeType::Capsule, ShapeType::Capsule) => ( PrimitiveContactGenerator { generate_contacts: super::generate_contacts_capsule_capsule, ..PrimitiveContactGenerator::default() }, None, ), - (Shape::Cuboid(_), Shape::Ball(_)) - | (Shape::Ball(_), Shape::Cuboid(_)) - | (Shape::Triangle(_), Shape::Ball(_)) - | (Shape::Ball(_), Shape::Triangle(_)) - | (Shape::Capsule(_), Shape::Ball(_)) - | (Shape::Ball(_), Shape::Capsule(_)) - | (Shape::Cylinder(_), Shape::Ball(_)) - | (Shape::Ball(_), Shape::Cylinder(_)) => ( + (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) => ( PrimitiveContactGenerator { generate_contacts: super::generate_contacts_ball_convex, ..PrimitiveContactGenerator::default() }, None, ), - (Shape::Capsule(_), Shape::Cuboid(_)) | (Shape::Cuboid(_), Shape::Capsule(_)) => ( + (ShapeType::Capsule, ShapeType::Cuboid) | (ShapeType::Cuboid, ShapeType::Capsule) => ( PrimitiveContactGenerator { generate_contacts: super::generate_contacts_cuboid_capsule, ..PrimitiveContactGenerator::default() }, None, ), - (Shape::Triangle(_), Shape::Cuboid(_)) | (Shape::Cuboid(_), Shape::Triangle(_)) => ( - PrimitiveContactGenerator { - generate_contacts: super::generate_contacts_cuboid_triangle, - ..PrimitiveContactGenerator::default() - }, - None, - ), - (Shape::Cylinder(_), _) | (_, Shape::Cylinder(_)) => ( + (ShapeType::Triangle, ShapeType::Cuboid) | (ShapeType::Cuboid, ShapeType::Triangle) => { + ( + PrimitiveContactGenerator { + generate_contacts: super::generate_contacts_cuboid_triangle, + ..PrimitiveContactGenerator::default() + }, + None, + ) + } + (ShapeType::Cylinder, _) | (_, ShapeType::Cylinder) => ( PrimitiveContactGenerator { generate_contacts: super::generate_contacts_pfm_pfm, ..PrimitiveContactGenerator::default() @@ -110,18 +112,18 @@ impl ContactDispatcher for DefaultContactDispatcher { fn dispatch( &self, - shape1: &Shape, - shape2: &Shape, + shape1: ShapeType, + shape2: ShapeType, ) -> (ContactPhase, Option>) { match (shape1, shape2) { - (Shape::Trimesh(_), _) | (_, Shape::Trimesh(_)) => ( + (ShapeType::Trimesh, _) | (_, ShapeType::Trimesh) => ( ContactPhase::NearPhase(ContactGenerator { generate_contacts: super::generate_contacts_trimesh_shape, ..ContactGenerator::default() }), Some(Box::new(TrimeshShapeContactGeneratorWorkspace::new())), ), - (Shape::HeightField(_), _) | (_, Shape::HeightField(_)) => ( + (ShapeType::HeightField, _) | (_, ShapeType::HeightField) => ( ContactPhase::NearPhase(ContactGenerator { generate_contacts: super::generate_contacts_heightfield_shape, ..ContactGenerator::default() diff --git a/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs b/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs index a7857a1..c3cf588 100644 --- a/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs +++ b/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs @@ -9,7 +9,7 @@ use crate::math::Vector; use ncollide::shape::Segment; pub fn generate_contacts_cuboid_capsule(ctxt: &mut PrimitiveContactGenerationContext) { - if let (Shape::Cuboid(cube1), Shape::Capsule(capsule2)) = (ctxt.shape1, ctxt.shape2) { + if let (Some(cube1), Some(capsule2)) = (ctxt.shape1.as_cuboid(), ctxt.shape2.as_capsule()) { generate_contacts( ctxt.prediction_distance, cube1, @@ -20,7 +20,9 @@ pub fn generate_contacts_cuboid_capsule(ctxt: &mut PrimitiveContactGenerationCon false, ); ctxt.manifold.update_warmstart_multiplier(); - } else if let (Shape::Capsule(capsule1), Shape::Cuboid(cube2)) = (ctxt.shape1, ctxt.shape2) { + } else if let (Some(capsule1), Some(cube2)) = + (ctxt.shape1.as_capsule(), ctxt.shape2.as_cuboid()) + { generate_contacts( ctxt.prediction_distance, cube2, @@ -53,7 +55,7 @@ pub fn generate_contacts<'a>( return; } - let segment2 = Segment::new(capsule2.a, capsule2.b); + let segment2 = capsule2.segment(); /* * diff --git a/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs b/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs index d879a22..596cfb1 100644 --- a/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs +++ b/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs @@ -6,7 +6,7 @@ use crate::math::Vector; use ncollide::shape::Cuboid; pub fn generate_contacts_cuboid_cuboid(ctxt: &mut PrimitiveContactGenerationContext) { - if let (Shape::Cuboid(cube1), Shape::Cuboid(cube2)) = (ctxt.shape1, ctxt.shape2) { + if let (Some(cube1), Some(cube2)) = (ctxt.shape1.as_cuboid(), ctxt.shape2.as_cuboid()) { generate_contacts( ctxt.prediction_distance, cube1, diff --git a/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs b/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs index 1a0358d..6744c0e 100644 --- a/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs +++ b/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs @@ -10,7 +10,7 @@ use crate::{ }; pub fn generate_contacts_cuboid_triangle(ctxt: &mut PrimitiveContactGenerationContext) { - if let (Shape::Cuboid(cube1), Shape::Triangle(triangle2)) = (ctxt.shape1, ctxt.shape2) { + if let (Some(cube1), Some(triangle2)) = (ctxt.shape1.as_cuboid(), ctxt.shape2.as_triangle()) { generate_contacts( ctxt.prediction_distance, cube1, @@ -21,7 +21,9 @@ pub fn generate_contacts_cuboid_triangle(ctxt: &mut PrimitiveContactGenerationCo false, ); ctxt.manifold.update_warmstart_multiplier(); - } else if let (Shape::Triangle(triangle1), Shape::Cuboid(cube2)) = (ctxt.shape1, ctxt.shape2) { + } else if let (Some(triangle1), Some(cube2)) = + (ctxt.shape1.as_triangle(), ctxt.shape2.as_cuboid()) + { generate_contacts( ctxt.prediction_distance, cube2, diff --git a/src/geometry/contact_generator/heightfield_shape_contact_generator.rs b/src/geometry/contact_generator/heightfield_shape_contact_generator.rs index 04afc65..f507caf 100644 --- a/src/geometry/contact_generator/heightfield_shape_contact_generator.rs +++ b/src/geometry/contact_generator/heightfield_shape_contact_generator.rs @@ -3,7 +3,7 @@ use crate::geometry::contact_generator::{ }; #[cfg(feature = "dim2")] use crate::geometry::Capsule; -use crate::geometry::{Collider, ContactManifold, HeightField, Shape}; +use crate::geometry::{Collider, ContactManifold, HeightField, Shape, ShapeType}; use crate::ncollide::bounding_volume::BoundingVolume; #[cfg(feature = "dim3")] use crate::{geometry::Triangle, math::Point}; @@ -38,9 +38,9 @@ pub fn generate_contacts_heightfield_shape(ctxt: &mut ContactGenerationContext) let collider1 = &ctxt.colliders[ctxt.pair.pair.collider1]; let collider2 = &ctxt.colliders[ctxt.pair.pair.collider2]; - if let Shape::HeightField(heightfield1) = collider1.shape() { + if let Some(heightfield1) = collider1.shape().as_heightfield() { do_generate_contacts(heightfield1, collider1, collider2, ctxt, false) - } else if let Shape::HeightField(heightfield2) = collider2.shape() { + } else if let Some(heightfield2) = collider2.shape().as_heightfield() { do_generate_contacts(heightfield2, collider2, collider1, ctxt, true) } } @@ -59,6 +59,7 @@ fn do_generate_contacts( .expect("The HeightFieldShapeContactGeneratorWorkspace is missing.") .downcast_mut() .expect("Invalid workspace type, expected a HeightFieldShapeContactGeneratorWorkspace."); + let shape_type2 = collider2.shape().shape_type(); /* * Detect if the detector context has been reset. @@ -76,19 +77,9 @@ fn do_generate_contacts( // subshape_id, manifold.subshape_index_pair // ); - // Use dummy shapes for the dispatch. - #[cfg(feature = "dim2")] - let sub_shape1 = - Shape::Capsule(Capsule::new(na::Point::origin(), na::Point::origin(), 0.0)); - #[cfg(feature = "dim3")] - let sub_shape1 = Shape::Triangle(Triangle::new( - Point::origin(), - Point::origin(), - Point::origin(), - )); let (generator, workspace2) = ctxt .dispatcher - .dispatch_primitives(&sub_shape1, collider2.shape()); + .dispatch_primitives(ShapeType::Capsule, shape_type2); let sub_detector = SubDetector { generator, @@ -120,12 +111,18 @@ fn do_generate_contacts( let manifolds = &mut ctxt.pair.manifolds; let prediction_distance = ctxt.prediction_distance; let dispatcher = ctxt.dispatcher; + let shape_type2 = collider2.shape().shape_type(); heightfield1.map_elements_in_local_aabb(&ls_aabb2, &mut |i, part1, _| { + let position1 = *collider1.position(); #[cfg(feature = "dim2")] - let sub_shape1 = Shape::Capsule(Capsule::new(part1.a, part1.b, 0.0)); + let (position1, sub_shape1) = { + let (dpos, height) = crate::utils::segment_to_capsule(&part1.a, &part1.b); + (position1 * dpos, Capsule::new(height, 0.0)); + }; #[cfg(feature = "dim3")] - let sub_shape1 = Shape::Triangle(*part1); + let sub_shape1 = *part1; + let sub_detector = match workspace.sub_detectors.entry(i) { Entry::Occupied(entry) => { let sub_detector = entry.into_mut(); @@ -137,7 +134,7 @@ fn do_generate_contacts( } Entry::Vacant(entry) => { let (generator, workspace2) = - dispatcher.dispatch_primitives(&sub_shape1, collider2.shape()); + dispatcher.dispatch_primitives(ShapeType::Triangle, shape_type2); let sub_detector = SubDetector { generator, manifold_id: manifolds.len(), @@ -162,7 +159,7 @@ fn do_generate_contacts( shape1: collider2.shape(), shape2: &sub_shape1, position1: collider2.position(), - position2: collider1.position(), + position2: &position1, manifold, workspace: sub_detector.workspace.as_deref_mut(), } @@ -173,7 +170,7 @@ fn do_generate_contacts( collider2, shape1: &sub_shape1, shape2: collider2.shape(), - position1: collider1.position(), + position1: &position1, position2: collider2.position(), manifold, workspace: sub_detector.workspace.as_deref_mut(), diff --git a/src/geometry/contact_generator/mod.rs b/src/geometry/contact_generator/mod.rs index d8a523f..0549420 100644 --- a/src/geometry/contact_generator/mod.rs +++ b/src/geometry/contact_generator/mod.rs @@ -22,7 +22,7 @@ pub use self::heightfield_shape_contact_generator::{ 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::polygon_polygon_contact_generator::generate_contacts_polygon_polygon; pub use self::trimesh_shape_contact_generator::{ generate_contacts_trimesh_shape, TrimeshShapeContactGeneratorWorkspace, }; diff --git a/src/geometry/contact_generator/polygon_polygon_contact_generator.rs b/src/geometry/contact_generator/polygon_polygon_contact_generator.rs index 33b54e4..c150e83 100644 --- a/src/geometry/contact_generator/polygon_polygon_contact_generator.rs +++ b/src/geometry/contact_generator/polygon_polygon_contact_generator.rs @@ -5,20 +5,21 @@ use crate::math::{Isometry, Point}; use crate::{math::Vector, utils}; pub fn generate_contacts_polygon_polygon(ctxt: &mut PrimitiveContactGenerationContext) { - if let (Shape::Polygon(polygon1), Shape::Polygon(polygon2)) = (ctxt.shape1, ctxt.shape2) { - generate_contacts( - polygon1, - &ctxt.position1, - polygon2, - &ctxt.position2, - ctxt.manifold, - ); - ctxt.manifold.update_warmstart_multiplier(); - } else { - unreachable!() - } - - ctxt.manifold.sort_contacts(ctxt.prediction_distance); + unimplemented!() + // if let (Shape::Polygon(polygon1), Shape::Polygon(polygon2)) = (ctxt.shape1, ctxt.shape2) { + // generate_contacts( + // polygon1, + // &ctxt.position1, + // polygon2, + // &ctxt.position2, + // ctxt.manifold, + // ); + // ctxt.manifold.update_warmstart_multiplier(); + // } else { + // unreachable!() + // } + // + // ctxt.manifold.sort_contacts(ctxt.prediction_distance); } fn generate_contacts<'a>( diff --git a/src/geometry/contact_generator/trimesh_shape_contact_generator.rs b/src/geometry/contact_generator/trimesh_shape_contact_generator.rs index 52ba9b7..49e9b40 100644 --- a/src/geometry/contact_generator/trimesh_shape_contact_generator.rs +++ b/src/geometry/contact_generator/trimesh_shape_contact_generator.rs @@ -1,7 +1,7 @@ use crate::geometry::contact_generator::{ ContactGenerationContext, PrimitiveContactGenerationContext, }; -use crate::geometry::{Collider, ContactManifold, Shape, Trimesh}; +use crate::geometry::{Collider, ContactManifold, Shape, ShapeType, Trimesh}; use crate::ncollide::bounding_volume::{BoundingVolume, AABB}; pub struct TrimeshShapeContactGeneratorWorkspace { @@ -26,9 +26,9 @@ pub fn generate_contacts_trimesh_shape(ctxt: &mut ContactGenerationContext) { let collider1 = &ctxt.colliders[ctxt.pair.pair.collider1]; let collider2 = &ctxt.colliders[ctxt.pair.pair.collider2]; - if let Shape::Trimesh(trimesh1) = collider1.shape() { + if let Some(trimesh1) = collider1.shape().as_trimesh() { do_generate_contacts(trimesh1, collider1, collider2, ctxt, false) - } else if let Shape::Trimesh(trimesh2) = collider2.shape() { + } else if let Some(trimesh2) = collider2.shape().as_trimesh() { do_generate_contacts(trimesh2, collider2, collider1, ctxt, true) } } @@ -121,6 +121,7 @@ fn do_generate_contacts( let new_interferences = &workspace.interferences; let mut old_inter_it = workspace.old_interferences.drain(..).peekable(); let mut old_manifolds_it = workspace.old_manifolds.drain(..); + let shape_type2 = collider2.shape().shape_type(); for (i, triangle_id) in new_interferences.iter().enumerate() { if *triangle_id >= trimesh1.num_triangles() { @@ -159,10 +160,10 @@ fn do_generate_contacts( } let manifold = &mut ctxt.pair.manifolds[i]; - let triangle1 = Shape::Triangle(trimesh1.triangle(*triangle_id)); + let triangle1 = trimesh1.triangle(*triangle_id); let (generator, mut workspace2) = ctxt .dispatcher - .dispatch_primitives(&triangle1, collider2.shape()); + .dispatch_primitives(ShapeType::Triangle, shape_type2); let mut ctxt2 = if ctxt_pair_pair.collider1 != manifold.pair.collider1 { PrimitiveContactGenerationContext { -- cgit 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/contact_generator/contact_dispatcher.rs | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) (limited to 'src/geometry/contact_generator') 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() -- cgit From 64958470950cd9832a669b1bd5d70a2aeb6a85ef Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Tue, 20 Oct 2020 15:57:54 +0200 Subject: Add rounded cylinder. --- src/geometry/contact_generator/contact_dispatcher.rs | 15 ++++----------- .../contact_generator/pfm_pfm_contact_generator.rs | 19 +++++++++++++++---- 2 files changed, 19 insertions(+), 15 deletions(-) (limited to 'src/geometry/contact_generator') 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; -- cgit From 949e3f5384a366c3bff5415c5db4635e811a580e Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Tue, 20 Oct 2020 16:22:53 +0200 Subject: Fix many warnings. --- .../ball_convex_contact_generator.rs | 2 +- .../capsule_capsule_contact_generator.rs | 24 ++++++++++------------ .../contact_generator/contact_dispatcher.rs | 8 +++++--- .../contact_generator/contact_generator.rs | 8 ++++---- .../cuboid_capsule_contact_generator.rs | 3 +-- .../cuboid_cuboid_contact_generator.rs | 2 +- .../cuboid_triangle_contact_generator.rs | 2 +- .../heightfield_shape_contact_generator.rs | 6 +++--- .../polygon_polygon_contact_generator.rs | 4 ++-- .../trimesh_shape_contact_generator.rs | 2 +- 10 files changed, 30 insertions(+), 31 deletions(-) (limited to 'src/geometry/contact_generator') diff --git a/src/geometry/contact_generator/ball_convex_contact_generator.rs b/src/geometry/contact_generator/ball_convex_contact_generator.rs index 62ebfab..69bc5e3 100644 --- a/src/geometry/contact_generator/ball_convex_contact_generator.rs +++ b/src/geometry/contact_generator/ball_convex_contact_gener