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/polyhedron_feature3d.rs | 10 ++++++++++ 1 file changed, 10 insertions(+) (limited to 'src/geometry/polyhedron_feature3d.rs') 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/polyhedron_feature3d.rs | 137 ++++++++++++++++++++++++++++++++++- 1 file changed, 136 insertions(+), 1 deletion(-) (limited to 'src/geometry/polyhedron_feature3d.rs') 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 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. --- src/geometry/polyhedron_feature3d.rs | 2 -- 1 file changed, 2 deletions(-) (limited to 'src/geometry/polyhedron_feature3d.rs') diff --git a/src/geometry/polyhedron_feature3d.rs b/src/geometry/polyhedron_feature3d.rs index 8f5de04..5a86ef2 100644 --- a/src/geometry/polyhedron_feature3d.rs +++ b/src/geometry/polyhedron_feature3d.rs @@ -387,8 +387,6 @@ impl PolyhedronFace { /// Compute the barycentric coordinates of the intersection between the two given lines. /// Returns `None` if the lines are parallel. fn closest_points_line2d(edge1: [Point2; 2], edge2: [Point2; 2]) -> Option<(f32, f32)> { - use approx::AbsDiffEq; - // Inspired by Real-time collision detection by Christer Ericson. let dir1 = edge1[1] - edge1[0]; let dir2 = edge2[1] - edge2[0]; -- cgit From 2b628f9580a826722346983ef42672d4e8dd8053 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 26 Oct 2020 15:58:30 +0100 Subject: Redefine capsules as a segment with a radius, allowing us to reuse the pfm_pfm_contact generator for it. --- src/geometry/polyhedron_feature3d.rs | 2 ++ 1 file changed, 2 insertions(+) (limited to 'src/geometry/polyhedron_feature3d.rs') diff --git a/src/geometry/polyhedron_feature3d.rs b/src/geometry/polyhedron_feature3d.rs index 5a86ef2..83088fe 100644 --- a/src/geometry/polyhedron_feature3d.rs +++ b/src/geometry/polyhedron_feature3d.rs @@ -40,6 +40,8 @@ impl From for PolyhedronFace { impl From> for PolyhedronFace { fn from(seg: Segment) -> Self { + // Vertices have feature ids 0 and 2. + // The segment interior has feature id 1. Self { vertices: [seg.a, seg.b, seg.b, seg.b], vids: [0, 2, 2, 2], -- cgit From df8e2fb9feb0d9dee0c92d117786ab74cf35ef36 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Tue, 27 Oct 2020 09:08:06 +0100 Subject: Variable renaming. --- src/geometry/polyhedron_feature3d.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/geometry/polyhedron_feature3d.rs') diff --git a/src/geometry/polyhedron_feature3d.rs b/src/geometry/polyhedron_feature3d.rs index 83088fe..e88674e 100644 --- a/src/geometry/polyhedron_feature3d.rs +++ b/src/geometry/polyhedron_feature3d.rs @@ -64,8 +64,8 @@ impl PolyhedronFace { } pub fn transform_by(&mut self, iso: &Isometry) { - for v in &mut self.vertices[0..self.num_vertices] { - *v = iso * *v; + for p in &mut self.vertices[0..self.num_vertices] { + *p = iso * *p; } } -- cgit