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

Fix NaN when computing contacts between a cuboid and a perfectly vertical triangle.

Browse Source
This commit is contained in:
Crozet Sébastien
2020-09-28 11:01:15 +02:00
parent 90dffc59ed
commit f3f2b57f88
3 changed files with 65 additions and 57 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.

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;