Skip to content

epa::Face::normal Degenerate variant. #4

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Closed
ThierryBerger wants to merge 16 commits into from
Closed

epa::Face::normal Degenerate variant. #4

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
16 commits
Select commit Hold shift + click to select a range
5a9ebff
explaination on #[allow(dead_code)] usage (#272)
ThierryBerger Sep 20, 2024
22a4261
Fix wavefront tests + new test for different pos + add them in ci (#268)
ThierryBerger Sep 20, 2024
c385c9b
Convex hull example (#256)
ThierryBerger Sep 20, 2024
5d550d8
aabb example using macroquad (#257)
ThierryBerger Sep 20, 2024
e09f966
Example bounding sphere using macroquad (#259)
ThierryBerger Sep 20, 2024
a641209
epa: Adds an enum to detect when a normal couldn´t be generated becau…
ThierryBerger Sep 26, 2024
34bb714
remove the initial todo which prompted those changes
ThierryBerger Sep 26, 2024
d6fa3b9
Merge branch 'todo_simple' into degenerate_face_normal_enum
ThierryBerger Sep 26, 2024
85e6a1f
add comments to common_macroquad* files (#275)
ThierryBerger Nov 4, 2024
68abfc1
Address a few TODOs (#273)
ThierryBerger Nov 4, 2024
e0535b1
pr feedbacks
ThierryBerger Nov 6, 2024
4854659
Merge branch 'master' into degenerate_face_normal_enum
ThierryBerger Nov 6, 2024
38b4a6a
face degenerate in upper module
ThierryBerger Nov 6, 2024
f7acdf8
remove unneeded comment
ThierryBerger Nov 6, 2024
6a98432
fix facedegenerate
ThierryBerger Nov 7, 2024
253bf30
fix doc
ThierryBerger Nov 8, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 6 additions & 1 deletion src/query/contact/contact_support_map_support_map.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,12 +19,16 @@ where
{
let simplex = &mut VoronoiSimplex::new();
match contact_support_map_support_map_with_params(pos12, g1, g2, prediction, simplex, None) {
GJKResult::ClosestPoints(point1, point2_1, normal1) => {
GJKResult::ClosestPoints(point1, point2_1, Ok(normal1)) => {
let dist = (point2_1 - point1).dot(&normal1);
let point2 = pos12.inverse_transform_point(&point2_1);
let normal2 = pos12.inverse_transform_unit_vector(&-normal1);
Some(Contact::new(point1, point2, normal1, normal2, dist))
}
GJKResult::ClosestPoints(_, _, Err(_)) => {
// TODO: propagate the error.
None
}
GJKResult::NoIntersection(_) => None,
GJKResult::Intersection => unreachable!(),
GJKResult::Proximity(_) => unreachable!(),
Expand Down Expand Up @@ -68,6 +72,7 @@ where

// The point is inside of the CSO: use the fallback algorithm
let mut epa = EPA::new();

if let Some((p1, p2, n)) = epa.closest_points(pos12, g1, g2, simplex) {
return GJKResult::ClosestPoints(p1, p2, n);
}
Expand Down
2 changes: 1 addition & 1 deletion src/query/contact_manifolds/contact_manifolds_pfm_pfm.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -79,7 +79,7 @@ pub fn contact_manifold_pfm_pfm<'a, ManifoldData, ContactData, S1, S2>(
manifold.clear();

match contact {
GJKResult::ClosestPoints(p1, p2_1, dir) => {
GJKResult::ClosestPoints(p1, p2_1, Ok(dir)) => {
let mut local_n1 = dir;
let mut local_n2 = pos12.inverse_transform_unit_vector(&-dir);
let dist = (p2_1 - p1).dot(&local_n1);
Expand Down
68 changes: 49 additions & 19 deletions src/query/epa/epa2.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -49,10 +49,14 @@ impl Ord for FaceId {
}
}

/// Represents a degenerate [`Face`] normal.
#[derive(Clone, Debug, PartialEq, Eq)]
pub struct Degenerate;

#[derive(Clone, Debug)]
struct Face {
pts: [usize; 2],
normal: Unit<Vector<Real>>,
normal: Result<Unit<Vector<Real>>, Degenerate>,
proj: Point<Real>,
bcoords: [Real; 2],
deleted: bool,
Expand Down Expand Up @@ -83,10 +87,10 @@ impl Face {

if let Some(n) = utils::ccw_face_normal([&vertices[pts[0]].point, &vertices[pts[1]].point])
{
normal = n;
normal = Ok(n);
deleted = false;
} else {
normal = Unit::new_unchecked(na::zero());
normal = Err(Degenerate);
deleted = true;
}

Expand Down Expand Up @@ -158,7 +162,11 @@ impl EPA {
g1: &G1,
g2: &G2,
simplex: &VoronoiSimplex,
) -> Option<(Point<Real>, Point<Real>, Unit<Vector<Real>>)>
) -> Option<(
Point<Real>,
Point<Real>,
Result<Unit<Vector<Real>>, Degenerate>,
)>
where
G1: ?Sized + SupportMap,
G2: ?Sized + SupportMap,
Expand Down Expand Up @@ -213,7 +221,7 @@ impl EPA {
}
}

return Some((Point::origin(), Point::origin(), n));
return Some((Point::origin(), Point::origin(), Ok(n)));
} else if simplex.dimension() == 2 {
let dp1 = self.vertices[1] - self.vertices[0];
let dp2 = self.vertices[2] - self.vertices[0];
Expand All @@ -235,18 +243,24 @@ impl EPA {
self.faces.push(face3);

if proj_is_inside1 {
let dist1 = self.faces[0].normal.dot(&self.vertices[0].point.coords);
self.heap.push(FaceId::new(0, -dist1)?);
if let Ok(normal) = self.faces[0].normal {
let dist1 = normal.dot(&self.vertices[0].point.coords);
self.heap.push(FaceId::new(0, -dist1)?);
}
}

if proj_is_inside2 {
let dist2 = self.faces[1].normal.dot(&self.vertices[1].point.coords);
self.heap.push(FaceId::new(1, -dist2)?);
if let Ok(normal) = self.faces[1].normal {
let dist2 = normal.dot(&self.vertices[1].point.coords);
self.heap.push(FaceId::new(1, -dist2)?);
}
}

if proj_is_inside3 {
let dist3 = self.faces[2].normal.dot(&self.vertices[2].point.coords);
self.heap.push(FaceId::new(2, -dist3)?);
if let Ok(normal) = self.faces[2].normal {
let dist3 = normal.dot(&self.vertices[2].point.coords);
self.heap.push(FaceId::new(2, -dist3)?);
}
}
} else {
let pts1 = [0, 1];
Expand All @@ -265,8 +279,16 @@ impl EPA {
pts2,
));

let dist1 = self.faces[0].normal.dot(&self.vertices[0].point.coords);
let dist2 = self.faces[1].normal.dot(&self.vertices[1].point.coords);
let dist1 = self.faces[0]
.normal
.as_ref()
.unwrap_or(&Unit::new_unchecked(Vector::zeros()))
.dot(&self.vertices[0].point.coords);
Comment thread
ThierryBerger marked this conversation as resolved.
Outdated
let dist2 = self.faces[1]
.normal
.as_ref()
.unwrap_or(&Unit::new_unchecked(Vector::zeros()))
.dot(&self.vertices[1].point.coords);

self.heap.push(FaceId::new(0, dist1)?);
self.heap.push(FaceId::new(1, dist2)?);
Expand All @@ -287,12 +309,15 @@ impl EPA {
if face.deleted {
continue;
}
let Ok(face_normal) = face.normal else {
continue;
};

let cso_point = CSOPoint::from_shapes(pos12, g1, g2, &face.normal);
let cso_point = CSOPoint::from_shapes(pos12, g1, g2, &face_normal);
let support_point_id = self.vertices.len();
self.vertices.push(cso_point);

let candidate_max_dist = cso_point.point.coords.dot(&face.normal);
let candidate_max_dist = cso_point.point.coords.dot(&face_normal);

if candidate_max_dist < max_dist {
best_face_id = face_id;
Expand All @@ -308,7 +333,7 @@ impl EPA {
{
let best_face = &self.faces[best_face_id.id];
let cpts = best_face.closest_points(&self.vertices);
return Some((cpts.0, cpts.1, best_face.normal));
return Some((cpts.0, cpts.1, best_face.normal.clone()));
}

old_dist = curr_dist;
Expand All @@ -323,12 +348,17 @@ impl EPA {

for f in new_faces.iter() {
if f.1 {
let dist = f.0.normal.dot(&f.0.proj.coords);
let new_face_normal =
f.0.normal
.clone()
.unwrap_or(Unit::new_unchecked(Vector::zeros()));

let dist = new_face_normal.dot(&f.0.proj.coords);
if dist < curr_dist {
// TODO: if we reach this point, there were issues due to
// numerical errors.
let cpts = f.0.closest_points(&self.vertices);
return Some((cpts.0, cpts.1, f.0.normal));
return Some((cpts.0, cpts.1, Ok(new_face_normal)));
}

if !f.0.deleted {
Expand All @@ -349,7 +379,7 @@ impl EPA {

let best_face = &self.faces[best_face_id.id];
let cpts = best_face.closest_points(&self.vertices);
Some((cpts.0, cpts.1, best_face.normal))
Some((cpts.0, cpts.1, best_face.normal.clone()))
}
}

Expand Down
78 changes: 51 additions & 27 deletions src/query/epa/epa3.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,11 +48,15 @@ impl Ord for FaceId {
}
}

/// Represents a degenerate [`Face`] normal.
#[derive(Clone, Debug, PartialEq, Eq)]
pub struct Degenerate;

#[derive(Clone, Debug)]
struct Face {
pts: [usize; 3],
adj: [usize; 3],
normal: Unit<Vector<Real>>,
normal: Result<Unit<Vector<Real>>, Degenerate>,
bcoords: [Real; 3],
deleted: bool,
}
Expand All @@ -71,13 +75,9 @@ impl Face {
&vertices[pts[1]].point,
&vertices[pts[2]].point,
]) {
normal = n;
normal = Ok(n);
} else {
// This is a bit of a hack for degenerate faces.
// TODO: It will work OK with our current code, though
// we should do this in another way to avoid any risk
// of misusing the face normal in the future.
normal = Unit::new_unchecked(na::zero());
normal = Err(Degenerate);
Comment thread
ThierryBerger marked this conversation as resolved.
Outdated
}

Face {
Expand Down Expand Up @@ -149,8 +149,13 @@ impl Face {
// have a zero normal, causing the dot product to be zero.
// So return true for these case will let us skip the triangle
// during silhouette computation.
(*pt - *p0).dot(&self.normal) >= -gjk::eps_tol()
|| Triangle::new(*p1, *p2, *pt).is_affinely_dependent()
match &self.normal {
Ok(normal) => {
(*pt - *p0).dot(normal) >= -gjk::eps_tol()
|| Triangle::new(*p1, *p2, *pt).is_affinely_dependent()
}
Err(_) => true,
}
}
}

Expand Down Expand Up @@ -215,7 +220,11 @@ impl EPA {
g1: &G1,
g2: &G2,
simplex: &VoronoiSimplex,
) -> Option<(Point<Real>, Point<Real>, Unit<Vector<Real>>)>
) -> Option<(
Point<Real>,
Point<Real>,
Result<Unit<Vector<Real>>, Degenerate>,
)>
where
G1: ?Sized + SupportMap,
G2: ?Sized + SupportMap,
Expand All @@ -235,7 +244,7 @@ impl EPA {
if simplex.dimension() == 0 {
let mut n: Vector<Real> = na::zero();
n[1] = 1.0;
return Some((Point::origin(), Point::origin(), Unit::new_unchecked(n)));
return Some((Point::origin(), Point::origin(), Ok(Unit::new_unchecked(n))));
} else if simplex.dimension() == 3 {
let dp1 = self.vertices[1] - self.vertices[0];
let dp2 = self.vertices[2] - self.vertices[0];
Expand Down Expand Up @@ -266,23 +275,31 @@ impl EPA {
self.faces.push(face4);

if proj_inside1 {
let dist1 = self.faces[0].normal.dot(&self.vertices[0].point.coords);
self.heap.push(FaceId::new(0, -dist1)?);
if let Ok(normal) = self.faces[0].normal {
let dist1 = normal.dot(&self.vertices[0].point.coords);
self.heap.push(FaceId::new(0, -dist1)?);
}
}

if proj_inside2 {
let dist2 = self.faces[1].normal.dot(&self.vertices[1].point.coords);
self.heap.push(FaceId::new(1, -dist2)?);
if let Ok(normal) = self.faces[1].normal {
let dist2 = normal.dot(&self.vertices[1].point.coords);
self.heap.push(FaceId::new(1, -dist2)?);
}
}

if proj_inside3 {
let dist3 = self.faces[2].normal.dot(&self.vertices[2].point.coords);
self.heap.push(FaceId::new(2, -dist3)?);
if let Ok(normal) = self.faces[2].normal {
let dist3 = normal.dot(&self.vertices[2].point.coords);
self.heap.push(FaceId::new(2, -dist3)?);
}
}

if proj_inside4 {
let dist4 = self.faces[3].normal.dot(&self.vertices[3].point.coords);
self.heap.push(FaceId::new(3, -dist4)?);
if let Ok(normal) = self.faces[3].normal {
let dist4 = normal.dot(&self.vertices[3].point.coords);
self.heap.push(FaceId::new(3, -dist4)?);
}
}
} else {
if simplex.dimension() == 1 {
Expand Down Expand Up @@ -327,13 +344,15 @@ impl EPA {
if face.deleted {
continue;
}

let cso_point = CSOPoint::from_shapes(pos12, g1, g2, &face.normal);
let face_normal = &face
.normal
Comment thread
ThierryBerger marked this conversation as resolved.
Outdated
.clone()
.unwrap_or(Unit::new_unchecked(Vector::zeros()));
let cso_point = CSOPoint::from_shapes(pos12, g1, g2, face_normal);
let candidate_max_dist = cso_point.point.coords.dot(face_normal);
let support_point_id = self.vertices.len();
self.vertices.push(cso_point);

let candidate_max_dist = cso_point.point.coords.dot(&face.normal);

if candidate_max_dist < max_dist {
best_face_id = face_id;
max_dist = candidate_max_dist;
Expand All @@ -347,8 +366,9 @@ impl EPA {
((curr_dist - old_dist).abs() < _eps && candidate_max_dist < max_dist)
{
let best_face = &self.faces[best_face_id.id];
let best_face_normal = face.normal.unwrap_or(Unit::new_unchecked(Vector::zeros()));
let points = best_face.closest_points(&self.vertices);
return Some((points.0, points.1, best_face.normal));
return Some((points.0, points.1, Ok(best_face_normal)));
}

old_dist = curr_dist;
Expand Down Expand Up @@ -389,12 +409,16 @@ impl EPA {

if new_face.1 {
let pt = self.vertices[self.faces[new_face_id].pts[0]].point.coords;
let dist = self.faces[new_face_id].normal.dot(&pt);
let new_face_normal = self.faces[new_face_id]
.normal
.clone()
.unwrap_or(Unit::new_unchecked(Vector::zeros()));
let dist = new_face_normal.dot(&pt);
if dist < curr_dist {
// TODO: if we reach this point, there were issues due to
// numerical errors.
let points = face.closest_points(&self.vertices);
return Some((points.0, points.1, face.normal));
return Some((points.0, points.1, Ok(new_face_normal)));
}

self.heap.push(FaceId::new(new_face_id, -dist)?);
Expand Down Expand Up @@ -424,7 +448,7 @@ impl EPA {

let best_face = &self.faces[best_face_id.id];
let points = best_face.closest_points(&self.vertices);
Some((points.0, points.1, best_face.normal))
Some((points.0, points.1, best_face.normal.clone()))
}

fn compute_silhouette(&mut self, point: usize, id: usize, opp_pt_id: usize) {
Expand Down
4 changes: 2 additions & 2 deletions src/query/epa/mod.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,9 +1,9 @@
//! The EPA algorithm for penetration depth computation.
//!
#[cfg(feature = "dim2")]
pub use self::epa2::EPA;
pub use self::epa2::{Degenerate, EPA};
#[cfg(feature = "dim3")]
pub use self::epa3::EPA;
pub use self::epa3::{Degenerate, EPA};

#[cfg(feature = "dim2")]
pub mod epa2;
Expand Down
Loading