aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--build/rapier2d/Cargo.toml2
-rw-r--r--build/rapier3d/Cargo.toml2
-rw-r--r--build/rapier_testbed2d/Cargo.toml4
-rw-r--r--build/rapier_testbed3d/Cargo.toml4
-rw-r--r--src/geometry/contact.rs8
-rw-r--r--src/geometry/contact_generator/cuboid_capsule_contact_generator.rs4
-rw-r--r--src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs2
-rw-r--r--src/geometry/contact_generator/cuboid_triangle_contact_generator.rs4
-rw-r--r--src/geometry/contact_generator/mod.rs5
-rw-r--r--src/geometry/contact_generator/pfm_pfm_contact_generator.rs159
-rw-r--r--src/geometry/contact_generator/polygon_polygon_contact_generator.rs2
-rw-r--r--src/geometry/mod.rs3
-rw-r--r--src/geometry/polyhedron_feature3d.rs137
13 files changed, 289 insertions, 47 deletions
diff --git a/build/rapier2d/Cargo.toml b/build/rapier2d/Cargo.toml
index 6d60b37..03ccd98 100644
--- a/build/rapier2d/Cargo.toml
+++ b/build/rapier2d/Cargo.toml
@@ -36,7 +36,7 @@ vec_map = "0.8"
instant = { version = "0.1", features = [ "now" ]}
num-traits = "0.2"
nalgebra = "0.22"
-ncollide2d = "0.24"
+ncollide2d = "0.25"
simba = "^0.2.1"
approx = "0.3"
rayon = { version = "1", optional = true }
diff --git a/build/rapier3d/Cargo.toml b/build/rapier3d/Cargo.toml
index dcc2125..7a0139e 100644
--- a/build/rapier3d/Cargo.toml
+++ b/build/rapier3d/Cargo.toml
@@ -36,7 +36,7 @@ vec_map = "0.8"
instant = { version = "0.1", features = [ "now" ]}
num-traits = "0.2"
nalgebra = "0.22"
-ncollide3d = "0.24"
+ncollide3d = "0.25"
simba = "^0.2.1"
approx = "0.3"
rayon = { version = "1", optional = true }
diff --git a/build/rapier_testbed2d/Cargo.toml b/build/rapier_testbed2d/Cargo.toml
index eeecb2a..95595bb 100644
--- a/build/rapier_testbed2d/Cargo.toml
+++ b/build/rapier_testbed2d/Cargo.toml
@@ -24,14 +24,14 @@ other-backends = [ "wrapped2d", "nphysics2d" ]
[dependencies]
nalgebra = "0.22"
-kiss3d = { version = "0.25", features = [ "conrod" ] }
+kiss3d = { version = "0.26", features = [ "conrod" ] }
rand = "0.7"
rand_pcg = "0.2"
instant = { version = "0.1", features = [ "web-sys", "now" ]}
bitflags = "1"
num_cpus = { version = "1", optional = true }
wrapped2d = { version = "0.4", optional = true }
-ncollide2d = "0.24"
+ncollide2d = "0.25"
nphysics2d = { version = "0.17", optional = true }
crossbeam = "0.7"
bincode = "1"
diff --git a/build/rapier_testbed3d/Cargo.toml b/build/rapier_testbed3d/Cargo.toml
index 7675701..46edd3b 100644
--- a/build/rapier_testbed3d/Cargo.toml
+++ b/build/rapier_testbed3d/Cargo.toml
@@ -23,14 +23,14 @@ other-backends = [ "physx", "physx-sys", "glam", "nphysics3d" ]
[dependencies]
nalgebra = "0.22"
-kiss3d = { version = "0.25", features = [ "conrod" ] }
+kiss3d = { version = "0.26", features = [ "conrod" ] }
rand = "0.7"
rand_pcg = "0.2"
instant = { version = "0.1", features = [ "web-sys", "now" ]}
bitflags = "1"
glam = { version = "0.8", optional = true }
num_cpus = { version = "1", optional = true }
-ncollide3d = "0.24"
+ncollide3d = "0.25"
nphysics3d = { version = "0.17", optional = true }
physx = { version = "0.6", optional = true }
physx-sys = { version = "0.4", optional = true }
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<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;
}
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;
}
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;
@@ -74,6 +75,140 @@ impl 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
// and contains the origin (this is numerically OK because
generated by cgit (git 2.34.1) at 2026-01-16 17:42:16 +0000