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

Implement a special case for edge-edge 3D polygonal clipping.

Browse Source
This commit is contained in:
Crozet Sébastien
2020-10-13 18:40:59 +02:00
parent 8ee3c703d6
commit faf3e7e0f7
13 changed files with 293 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
src/geometry/contact.rs
View File

@@ -429,7 +429,7 @@ impl ContactManifold {
}
#[inline]
pub(crate) fn try_update_contacts(&mut self, pos12: &Isometry<f32>) -> bool {
pub(crate) fn try_update_contacts(&mut self, pos12: &Isometry<f32>, 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;
}

4
src/geometry/contact_generator/cuboid_capsule_contact_generator.rs
View File

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

2
src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs
View File

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

4
src/geometry/contact_generator/cuboid_triangle_contact_generator.rs
View File

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

5
src/geometry/contact_generator/mod.rs
View File

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

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

@@ -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,36 +80,28 @@ 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() {
// 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,
});
}
// 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;
@@ -121,3 +116,115 @@ 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<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);
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);
}
}
}

2
src/geometry/contact_generator/polygon_polygon_contact_generator.rs
View File

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

3
src/geometry/mod.rs
View File

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

137
src/geometry/polyhedron_feature3d.rs
View File

@@ -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<f32>,
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<f32>,
face2: &PolyhedronFace,
pos21: &Isometry<f32>,
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<f32>,
face2: &PolyhedronFace,
pos21: &Isometry<f32>,
manifold: &mut ContactManifold,
) {
// Project the faces to a 2D plane for contact clipping.
// The plane they are projected onto has normal sep_axis1