From f3f2b57f88475361dd10475a94a6f3ac2793c5f2 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 28 Sep 2020 11:01:15 +0200 Subject: Fix NaN when computing contacts between a cuboid and a perfectly vertical triangle. --- src/geometry/polyhedron_feature3d.rs | 118 ++++++++++++++++++----------------- src/lib.rs | 2 + 2 files changed, 63 insertions(+), 57 deletions(-) (limited to 'src') diff --git a/src/geometry/polyhedron_feature3d.rs b/src/geometry/polyhedron_feature3d.rs index 94870a4..dfeee29 100644 --- a/src/geometry/polyhedron_feature3d.rs +++ b/src/geometry/polyhedron_feature3d.rs @@ -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, + }); + } } } } diff --git a/src/lib.rs b/src/lib.rs index aae0bd0..118ac23 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -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; -- cgit From ba0a85a0a3f2d31f42624a9c7eb20adf92e36818 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 28 Sep 2020 11:01:42 +0200 Subject: Fix trimesh/shape collision-detection when the trimesh was input as the second shape. --- .../trimesh_shape_contact_generator.rs | 52 +++++++++++----------- 1 file changed, 26 insertions(+), 26 deletions(-) (limited to 'src') diff --git a/src/geometry/contact_generator/trimesh_shape_contact_generator.rs b/src/geometry/contact_generator/trimesh_shape_contact_generator.rs index 2d2309b..78f26bb 100644 --- a/src/geometry/contact_generator/trimesh_shape_contact_generator.rs +++ b/src/geometry/contact_generator/trimesh_shape_contact_generator.rs @@ -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, -- cgit