diff options
Diffstat (limited to 'src/geometry/contact_generator')
6 files changed, 141 insertions, 35 deletions
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<f32>, + pos21: &Isometry<f32>, + prediction_distance: f32, + manifold: &mut ContactManifold, + workspace: &mut PfmPfmContactManifoldGeneratorWorkspace, + ) -> Option<Unit<Vector<f32>>> { + 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; } |