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

Merge pull request #21 from dimforge/trimesh_cd_bug

Browse Source 
Fix NaN caused by the collision-detection between a vertical triangle and a cuboid.
This commit is contained in:
Sébastien Crozet
2020-09-28 11:14:53 +02:00
committed by GitHub
parent 90dffc59ed 037a25dce3
commit 55216e45b7
5 changed files with 159 additions and 83 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
examples3d/all_examples3.rs
View File

@@ -14,6 +14,7 @@ mod add_remove3;
mod compound3; mod compound3;
mod debug_boxes3; mod debug_boxes3;
mod debug_triangle3; mod debug_triangle3;
mod debug_trimesh3;
mod domino3; mod domino3;
mod heightfield3; mod heightfield3;
mod joints3; mod joints3;
@@ -74,6 +75,7 @@ pub fn main() {
("Keva tower", keva3::init_world), ("Keva tower", keva3::init_world),
("(Debug) boxes", debug_boxes3::init_world), ("(Debug) boxes", debug_boxes3::init_world),
("(Debug) triangle", debug_triangle3::init_world), ("(Debug) triangle", debug_triangle3::init_world),
("(Debug) trimesh", debug_trimesh3::init_world),
]; ];
// Lexicographic sort, with stress tests moved at the end of the list. // Lexicographic sort, with stress tests moved at the end of the list.

68
examples3d/debug_trimesh3.rs Normal file
View File

@@ -0,0 +1,68 @@
use na::Point3;
use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
use rapier3d::geometry::{ColliderBuilder, ColliderSet};
use rapier_testbed3d::Testbed;
pub fn init_world(testbed: &mut Testbed) {
/*
* World
*/
let mut bodies = RigidBodySet::new();
let mut colliders = ColliderSet::new();
let joints = JointSet::new();
// Triangle ground.
let width = 0.5;
let vtx = vec![
Point3::new(-width, 0.0, -width),
Point3::new(width, 0.0, -width),
Point3::new(width, 0.0, width),
Point3::new(-width, 0.0, width),
Point3::new(-width, -width, -width),
Point3::new(width, -width, -width),
Point3::new(width, -width, width),
Point3::new(-width, -width, width),
];
let idx = vec![
Point3::new(0, 1, 2),
Point3::new(0, 2, 3),
Point3::new(4, 5, 6),
Point3::new(4, 6, 7),
Point3::new(0, 4, 7),
Point3::new(0, 7, 3),
Point3::new(1, 5, 6),
Point3::new(1, 6, 2),
Point3::new(3, 2, 7),
Point3::new(2, 6, 7),
Point3::new(0, 1, 5),
Point3::new(0, 5, 4),
];
// Dynamic box rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic()
.translation(0.0, 35.0, 0.0)
// .rotation(Vector3::new(0.8, 0.2, 0.1))
.can_sleep(false)
.build();
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(1.0, 2.0, 1.0).build();
colliders.insert(collider, handle, &mut bodies);
let rigid_body = RigidBodyBuilder::new_static()
.translation(0.0, 0.0, 0.0)
.build();
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::trimesh(vtx, idx).build();
colliders.insert(collider, handle, &mut bodies);
/*
* Set up the testbed.
*/
testbed.set_world(bodies, colliders, joints);
testbed.look_at(Point3::new(10.0, 10.0, 10.0), Point3::origin());
}
fn main() {
let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]);
testbed.run()
}

52
src/geometry/contact_generator/trimesh_shape_contact_generator.rs
View File

@@ -40,6 +40,12 @@ fn do_generate_contacts(
ctxt: &mut ContactGenerationContext, ctxt: &mut ContactGenerationContext,
flipped: bool, flipped: bool,
) { ) {
let ctxt_pair_pair = if flipped {
ctxt.pair.pair.swap()
} else {
ctxt.pair.pair
};
let workspace: &mut TrimeshShapeContactGeneratorWorkspace = ctxt let workspace: &mut TrimeshShapeContactGeneratorWorkspace = ctxt
.pair .pair
.generator_workspace .generator_workspace
@@ -78,7 +84,7 @@ fn do_generate_contacts(
// and rebuilt. In this case, we hate to reconstruct the `old_interferences` // and rebuilt. In this case, we hate to reconstruct the `old_interferences`
// array using the subshape ids from the contact manifolds. // array using the subshape ids from the contact manifolds.
// TODO: always rely on the subshape ids instead of maintaining `.ord_interferences` ? // TODO: always rely on the subshape ids instead of maintaining `.ord_interferences` ?
let ctxt_collider1 = ctxt.pair.pair.collider1; let ctxt_collider1 = ctxt_pair_pair.collider1;
workspace.old_interferences = workspace workspace.old_interferences = workspace
.old_manifolds .old_manifolds
.iter() .iter()
@@ -91,13 +97,16 @@ fn do_generate_contacts(
}) })
.collect(); .collect();
} }
assert_eq!( // This assertion may fire due to the invalid triangle_ids that the
workspace // near-phase may return (due to SIMD sentinels).
.old_interferences //
.len() // assert_eq!(
.min(trimesh1.num_triangles()), // workspace
workspace.old_manifolds.len() // .old_interferences
); // .len()
// .min(trimesh1.num_triangles()),
// workspace.old_manifolds.len()
// );
trimesh1 trimesh1
.waabbs() .waabbs()
@@ -118,6 +127,7 @@ fn do_generate_contacts(
// than the max. // than the max.
continue; continue;
} }
if !same_local_aabb2 { if !same_local_aabb2 {
loop { loop {
match old_inter_it.peek() { match old_inter_it.peek() {
@@ -131,23 +141,13 @@ fn do_generate_contacts(
let manifold = if old_inter_it.peek() != Some(triangle_id) { let manifold = if old_inter_it.peek() != Some(triangle_id) {
// We don't have a manifold for this triangle yet. // We don't have a manifold for this triangle yet.
if flipped { ContactManifold::with_subshape_indices(
ContactManifold::with_subshape_indices( ctxt_pair_pair,
ctxt.pair.pair, collider1,
collider2, collider2,
collider1, *triangle_id,
*triangle_id, 0,
0, )
)
} else {
ContactManifold::with_subshape_indices(
ctxt.pair.pair,
collider1,
collider2,
0,
*triangle_id,
)
}
} else { } else {
// We already have a manifold for this triangle. // We already have a manifold for this triangle.
old_inter_it.next(); old_inter_it.next();
@@ -163,7 +163,7 @@ fn do_generate_contacts(
.dispatcher .dispatcher
.dispatch_primitives(&triangle1, collider2.shape()); .dispatch_primitives(&triangle1, collider2.shape());
let mut ctxt2 = if ctxt.pair.pair.collider1 != manifold.pair.collider1 { let mut ctxt2 = if ctxt_pair_pair.collider1 != manifold.pair.collider1 {
PrimitiveContactGenerationContext { PrimitiveContactGenerationContext {
prediction_distance: ctxt.prediction_distance, prediction_distance: ctxt.prediction_distance,
collider1: collider2, collider1: collider2,

118
src/geometry/polyhedron_feature3d.rs
View File

@@ -110,39 +110,41 @@ impl PolyhedronFace {
if face2.num_vertices > 2 { if face2.num_vertices > 2 {
let normal2 = (face2.vertices[2] - face2.vertices[1]) let normal2 = (face2.vertices[2] - face2.vertices[1])
.cross(&(face2.vertices[0] - face2.vertices[1])); .cross(&(face2.vertices[0] - face2.vertices[1]));
let denom = normal2.dot(&sep_axis1);
let last_index2 = face2.num_vertices as usize - 1; if !relative_eq!(denom, 0.0) {
'point_loop1: for i in 0..face1.num_vertices as usize { let last_index2 = face2.num_vertices as usize - 1;
let p1 = projected_face1[i]; 'point_loop1: for i in 0..face1.num_vertices as usize {
let p1 = projected_face1[i];
let sign = (projected_face2[0] - projected_face2[last_index2]) let sign = (projected_face2[0] - projected_face2[last_index2])
.perp(&(p1 - projected_face2[last_index2])); .perp(&(p1 - projected_face2[last_index2]));
for j in 0..last_index2 { for j in 0..last_index2 {
let new_sign = (projected_face2[j + 1] - projected_face2[j]) let new_sign = (projected_face2[j + 1] - projected_face2[j])
.perp(&(p1 - projected_face2[j])); .perp(&(p1 - projected_face2[j]));
if new_sign * sign < 0.0 { if new_sign * sign < 0.0 {
// The point lies outside. // The point lies outside.
continue 'point_loop1; continue 'point_loop1;
}
} }
}
// All the perp had the same sign: the point is inside of the other shapes projection. // All the perp had the same sign: the point is inside of the other shapes projection.
// Output the contact. // Output the contact.
let denom = normal2.dot(&sep_axis1); let dist = (face2.vertices[0] - face1.vertices[i]).dot(&normal2) / denom;
let dist = (face2.vertices[0] - face1.vertices[i]).dot(&normal2) / denom; let local_p1 = face1.vertices[i];
let local_p1 = face1.vertices[i]; let local_p2 = face1.vertices[i] + dist * sep_axis1;
let local_p2 = face1.vertices[i] + dist * sep_axis1;
if dist <= prediction_distance { if dist <= prediction_distance {
manifold.points.push(Contact { manifold.points.push(Contact {
local_p1, local_p1,
local_p2: pos21 * local_p2, local_p2: pos21 * local_p2,
impulse: 0.0, impulse: 0.0,
tangent_impulse: Contact::zero_tangent_impulse(), tangent_impulse: Contact::zero_tangent_impulse(),
fid1: face1.vids[i], fid1: face1.vids[i],
fid2: face2.fid, fid2: face2.fid,
dist, dist,
}); });
}
} }
} }
} }
@@ -151,40 +153,42 @@ impl PolyhedronFace {
let normal1 = (face1.vertices[2] - face1.vertices[1]) let normal1 = (face1.vertices[2] - face1.vertices[1])
.cross(&(face1.vertices[0] - face1.vertices[1])); .cross(&(face1.vertices[0] - face1.vertices[1]));
let last_index1 = face1.num_vertices as usize - 1; let denom = -normal1.dot(&sep_axis1);
'point_loop2: for i in 0..face2.num_vertices as usize { if !relative_eq!(denom, 0.0) {
let p2 = projected_face2[i]; let last_index1 = face1.num_vertices as usize - 1;
'point_loop2: for i in 0..face2.num_vertices as usize {
let p2 = projected_face2[i];
let sign = (projected_face1[0] - projected_face1[last_index1]) let sign = (projected_face1[0] - projected_face1[last_index1])
.perp(&(p2 - projected_face1[last_index1])); .perp(&(p2 - projected_face1[last_index1]));
for j in 0..last_index1 { for j in 0..last_index1 {
let new_sign = (projected_face1[j + 1] - projected_face1[j]) let new_sign = (projected_face1[j + 1] - projected_face1[j])
.perp(&(p2 - projected_face1[j])); .perp(&(p2 - projected_face1[j]));
if new_sign * sign < 0.0 { if new_sign * sign < 0.0 {
// The point lies outside. // The point lies outside.
continue 'point_loop2; continue 'point_loop2;
}
} }
}
// All the perp had the same sign: the point is inside of the other shapes projection. // All the perp had the same sign: the point is inside of the other shapes projection.
// Output the contact. // Output the contact.
let denom = -normal1.dot(&sep_axis1); let dist = (face1.vertices[0] - face2.vertices[i]).dot(&normal1) / denom;
let dist = (face1.vertices[0] - face2.vertices[i]).dot(&normal1) / denom; let local_p2 = face2.vertices[i];
let local_p2 = face2.vertices[i]; let local_p1 = face2.vertices[i] - dist * sep_axis1;
let local_p1 = face2.vertices[i] - dist * sep_axis1;
if true { if true {
// dist <= prediction_distance { // dist <= prediction_distance {
manifold.points.push(Contact { manifold.points.push(Contact {
local_p1, local_p1,
local_p2: pos21 * local_p2, local_p2: pos21 * local_p2,
impulse: 0.0, impulse: 0.0,
tangent_impulse: Contact::zero_tangent_impulse(), tangent_impulse: Contact::zero_tangent_impulse(),
fid1: face1.fid, fid1: face1.fid,
fid2: face2.vids[i], fid2: face2.vids[i],
dist, dist,
}); });
}
} }
} }
} }

2
src/lib.rs
View File

@@ -18,6 +18,8 @@ pub extern crate ncollide3d as ncollide;
#[cfg(feature = "serde")] #[cfg(feature = "serde")]
#[macro_use] #[macro_use]
extern crate serde; extern crate serde;
#[macro_use]
extern crate approx;
extern crate num_traits as num; extern crate num_traits as num;
// #[macro_use] // #[macro_use]
// extern crate array_macro; // extern crate array_macro;