Skip to content

Degenerate face normal enum #284

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

Open
ThierryBerger wants to merge 18 commits into
base: master
Choose a base branch
from
Open

Degenerate face normal enum #284

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
18 commits
Select commit Hold shift + click to select a range
f881e7c
addressed a few simple todos
ThierryBerger Sep 18, 2024
8d2b591
compute_axes_aligned_clipping_planes now a VoxelSet function
ThierryBerger Sep 18, 2024
9a5695c
remove comment/scope about nll
ThierryBerger Sep 26, 2024
82cd28f
remove IntersectionCompositeShapeShapeBestFirstVisitor
ThierryBerger Sep 26, 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
de82965
fix clippy
ThierryBerger Sep 26, 2024
d6fa3b9
Merge branch 'todo_simple' into degenerate_face_normal_enum
ThierryBerger Sep 26, 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
4731852
remove todo
ThierryBerger Nov 12, 2024
a7f8dce
add warn! call
ThierryBerger Nov 15, 2024
92cd297
avoid a panic in
ThierryBerger Nov 19, 2024
7021a2e
Merge branch 'master' into degenerate_face_normal_enum
ThierryBerger Nov 19, 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
8 changes: 7 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 @@ -4,6 +4,7 @@ use crate::query::gjk::{self, CSOPoint, GJKResult, VoronoiSimplex};
use crate::query::Contact;
use crate::shape::SupportMap;

use log::warn;
use na::Unit;

/// Contact between support-mapped shapes (`Cuboid`, `ConvexHull`, etc.)
Expand All @@ -19,12 +20,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(_)) => {
warn!("`contact_support_map_support_map` found the closest points on a degenerate face: verify your shapes' correctness.");
None
}
GJKResult::NoIntersection(_) => None,
GJKResult::Intersection => unreachable!(),
GJKResult::Proximity(_) => unreachable!(),
Expand Down Expand Up @@ -68,6 +73,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
66 changes: 47 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 @@ -8,6 +8,7 @@ use num::Bounded;

use crate::math::{Isometry, Point, Real, Vector};
use crate::query::gjk::{self, CSOPoint, ConstantOrigin, VoronoiSimplex};
use crate::query::FaceDegenerate;
use crate::shape::SupportMap;
use crate::utils;

Expand Down Expand Up @@ -52,7 +53,7 @@ impl Ord for FaceId {
#[derive(Clone, Debug)]
struct Face {
pts: [usize; 2],
normal: Unit<Vector<Real>>,
normal: Result<Unit<Vector<Real>>, FaceDegenerate>,
proj: Point<Real>,
bcoords: [Real; 2],
deleted: bool,
Expand Down Expand Up @@ -83,10 +84,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(FaceDegenerate);
deleted = true;
}

Expand Down Expand Up @@ -158,7 +159,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>>, FaceDegenerate>,
)>
where
G1: ?Sized + SupportMap,
G2: ?Sized + SupportMap,
Expand Down Expand Up @@ -213,7 +218,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 +240,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 +276,17 @@ 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()
.map(|normal| normal.dot(&self.vertices[0].point.coords))
.unwrap_or(0.0);

let dist2 = self.faces[1]
.normal
.as_ref()
.map(|normal| normal.dot(&self.vertices[1].point.coords))
.unwrap_or(0.0);

self.heap.push(FaceId::new(0, dist1)?);
self.heap.push(FaceId::new(1, dist2)?);
Expand All @@ -287,12 +307,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 +331,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 +346,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 +377,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
76 changes: 47 additions & 29 deletions src/query/epa/epa3.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@

use crate::math::{Isometry, Point, Real, Vector};
use crate::query::gjk::{self, CSOPoint, ConstantOrigin, VoronoiSimplex};
use crate::query::PointQueryWithLocation;
use crate::query::{FaceDegenerate, PointQueryWithLocation};
use crate::shape::{SupportMap, Triangle, TrianglePointLocation};
use crate::utils;
use na::{self, Unit};
Expand Down Expand Up @@ -52,7 +52,7 @@ impl Ord for FaceId {
struct Face {
pts: [usize; 3],
adj: [usize; 3],
normal: Unit<Vector<Real>>,
normal: Result<Unit<Vector<Real>>, FaceDegenerate>,
bcoords: [Real; 3],
deleted: bool,
}
Expand All @@ -71,13 +71,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(FaceDegenerate);
}

Face {
Expand Down Expand Up @@ -149,8 +145,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 +216,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>>, FaceDegenerate>,
)>
where
G1: ?Sized + SupportMap,
G2: ?Sized + SupportMap,
Expand All @@ -235,7 +240,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 +271,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 @@ -323,17 +336,17 @@ impl EPA {
while let Some(face_id) = self.heap.pop() {
// Create new faces.
let face = self.faces[face_id.id].clone();

if face.deleted {
continue;
}

let cso_point = CSOPoint::from_shapes(pos12, g1, g2, &face.normal);
let Ok(face_normal) = &face.normal else {
continue;
};
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 +360,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 @@ -388,12 +402,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 @@ -423,7 +441,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
Loading