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 debug_boxes3;
mod debug_triangle3;
mod debug_trimesh3;
mod domino3;
mod heightfield3;
mod joints3;
@@ -74,6 +75,7 @@ pub fn main() {
("Keva tower", keva3::init_world),
("(Debug) boxes", debug_boxes3::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.

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

118
src/geometry/polyhedron_feature3d.rs
View File

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

2
src/lib.rs
View File

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