aboutsummaryrefslogtreecommitdiff
path: root/src/geometry/polyhedron_feature3d.rs
diff options
context:
space:
mode:
Diffstat (limited to 'src/geometry/polyhedron_feature3d.rs')
-rw-r--r--src/geometry/polyhedron_feature3d.rs445
1 files changed, 0 insertions, 445 deletions
diff --git a/src/geometry/polyhedron_feature3d.rs b/src/geometry/polyhedron_feature3d.rs
deleted file mode 100644
index 5ad62df..0000000
--- a/src/geometry/polyhedron_feature3d.rs
+++ /dev/null
@@ -1,445 +0,0 @@
-use crate::approx::AbsDiffEq;
-use crate::geometry::{self, Contact, ContactManifold, CuboidFeatureFace, Triangle};
-use crate::math::{Isometry, Point, Vector};
-use crate::utils::WBasis;
-use na::Point2;
-use ncollide::shape::Segment;
-
-#[derive(Debug, Clone)]
-pub struct PolyhedronFace {
- pub vertices: [Point<f32>; 4],
- pub vids: [u8; 4], // Feature ID of the vertices.
- pub eids: [u8; 4], // Feature ID of the edges.
- pub fid: u8, // Feature ID of the face.
- pub num_vertices: usize,
-}
-
-impl Default for PolyhedronFace {
- fn default() -> Self {
- Self {
- vertices: [Point::origin(); 4],
- vids: [0; 4],
- eids: [0; 4],
- fid: 0,
- num_vertices: 0,
- }
- }
-}
-
-impl From<CuboidFeatureFace> for PolyhedronFace {
- fn from(face: CuboidFeatureFace) -> Self {
- Self {
- vertices: face.vertices,
- vids: face.vids,
- eids: face.eids,
- fid: face.fid,
- num_vertices: 4,
- }
- }
-}
-
-impl From<Triangle> for PolyhedronFace {
- fn from(tri: Triangle) -> Self {
- Self {
- vertices: [tri.a, tri.b, tri.c, tri.c],
- vids: [0, 2, 4, 4],
- eids: [1, 3, 5, 5],
- fid: 0,
- num_vertices: 3,
- }
- }
-}
-
-impl From<Segment<f32>> for PolyhedronFace {
- fn from(seg: Segment<f32>) -> Self {
- // Vertices have feature ids 0 and 2.
- // The segment interior has feature id 1.
- Self {
- vertices: [seg.a, seg.b, seg.b, seg.b],
- vids: [0, 2, 2, 2],
- eids: [1, 1, 1, 1],
- fid: 0,
- num_vertices: 2,
- }
- }
-}
-
-impl PolyhedronFace {
- pub fn new() -> Self {
- Self {
- vertices: [Point::origin(); 4],
- vids: [0; 4],
- eids: [0; 4],
- fid: 0,
- num_vertices: 0,
- }
- }
-
- pub fn transform_by(&mut self, iso: &Isometry<f32>) {
- for p in &mut self.vertices[0..self.num_vertices] {
- *p = iso * *p;
- }
- }
-
- pub fn contacts(
- prediction_distance: f32,
- face1: &PolyhedronFace,
- sep_axis1: &Vector<f32>,
- face2: &PolyhedronFace,
- pos21: &Isometry<f32>,
- manifold: &mut ContactManifold,
- ) {
- match (face1.num_vertices, face2.num_vertices) {
- (2, 2) => Self::contacts_edge_edge(
- prediction_distance,
- face1,
- sep_axis1,
- face2,
- pos21,
- manifold,
- ),
- _ => Self::contacts_face_face(
- prediction_distance,
- face1,
- sep_axis1,
- face2,
- pos21,
- manifold,
- ),
- }
- }
-
- fn contacts_edge_edge(
- prediction_distance: f32,
- face1: &PolyhedronFace,
- sep_axis1: &Vector<f32>,
- face2: &PolyhedronFace,
- pos21: &Isometry<f32>,
- manifold: &mut ContactManifold,
- ) {
- // Project the faces to a 2D plane for contact clipping.
- // The plane they are projected onto has normal sep_axis1
- // and contains the origin (this is numerically OK because
- // we are not working in world-space here).
- let basis = sep_axis1.orthonormal_basis();
- let projected_edge1 = [
- Point2::new(
- face1.vertices[0].coords.dot(&basis[0]),
- face1.vertices[0].coords.dot(&basis[1]),
- ),
- Point2::new(
- face1.vertices[1].coords.dot(&basis[0]),
- face1.vertices[1].coords.dot(&basis[1]),
- ),
- ];
- let projected_edge2 = [
- Point2::new(
- face2.vertices[0].coords.dot(&basis[0]),
- face2.vertices[0].coords.dot(&basis[1]),
- ),
- Point2::new(
- face2.vertices[1].coords.dot(&basis[0]),
- face2.vertices[1].coords.dot(&basis[1]),
- ),
- ];
-
- let tangent1 =
- (projected_edge1[1] - projected_edge1[0]).try_normalize(f32::default_epsilon());
- let tangent2 =
- (projected_edge2[1] - projected_edge2[0]).try_normalize(f32::default_epsilon());
-
- // TODO: not sure what the best value for eps is.
- // Empirically, it appears that an epsilon smaller than 1.0e-3 is too small.
- if let (Some(tangent1), Some(tangent2)) = (tangent1, tangent2) {
- let parallel = tangent1.dot(&tangent2) >= crate::utils::COS_FRAC_PI_8;
-
- if !parallel {
- let seg1 = (&projected_edge1[0], &projected_edge1[1]);
- let seg2 = (&projected_edge2[0], &projected_edge2[1]);
- let (loc1, loc2) =
- ncollide::query::closest_points_segment_segment_with_locations_nD(seg1, seg2);
-
- // Found a contact between the two edges.
- let bcoords1 = loc1.barycentric_coordinates();
- let bcoords2 = loc2.barycentric_coordinates();
-
- let edge1 = (face1.vertices[0], face1.vertices[1]);
- let edge2 = (face2.vertices[0], face2.vertices[1]);
- let local_p1 = edge1.0 * bcoords1[0] + edge1.1.coords * bcoords1[1];
- let local_p2 = edge2.0 * bcoords2[0] + edge2.1.coords * bcoords2[1];
- let dist = (local_p2 - local_p1).dot(&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.eids[0],
- fid2: face2.eids[0],
- dist,
- });
- }
-
- return;
- }
- }
-
- // The lines are parallel so we are having a conformal contact.
- // Let's use a range-based clipping to extract two contact points.
- // TODO: would it be better and/or more efficient to do the
- //clipping in 2D?
- if let Some(clips) = geometry::clip_segments(
- (face1.vertices[0], face1.vertices[1]),
- (face2.vertices[0], face2.vertices[1]),
- ) {
- manifold.points.push(Contact {
- local_p1: (clips.0).0,
- local_p2: pos21 * (clips.0).1,
- impulse: 0.0,
- tangent_impulse: Contact::zero_tangent_impulse(),
- fid1: 0, // FIXME
- fid2: 0, // FIXME
- dist: ((clips.0).1 - (clips.0).0).dot(&sep_axis1),
- });
-
- manifold.points.push(Contact {
- local_p1: (clips.1).0,
- local_p2: pos21 * (clips.1).1,
- impulse: 0.0,
- tangent_impulse: Contact::zero_tangent_impulse(),
- fid1: 0, // FIXME
- fid2: 0, // FIXME
- dist: ((clips.1).1 - (clips.1).0).dot(&sep_axis1),
- });
- }
- }
-
- fn contacts_face_face(
- prediction_distance: f32,
- face1: &PolyhedronFace,
- sep_axis1: &Vector<f32>,
- face2: &PolyhedronFace,
- pos21: &Isometry<f32>,
- manifold: &mut ContactManifold,
- ) {
- // Project the faces to a 2D plane for contact clipping.
- // The plane they are projected onto has normal sep_axis1
- // and contains the origin (this is numerically OK because
- // we are not working in world-space here).
- let basis = sep_axis1.orthonormal_basis();
- let projected_face1 = [
- Point2::new(
- face1.vertices[0].coords.dot(&basis[0]),
- face1.vertices[0].coords.dot(&basis[1]),
- ),
- Point2::new(
- face1.vertices[1].coords.dot(&basis[0]),
- face1.vertices[1].coords.dot(&basis[1]),
- ),
- Point2::new(
- face1.vertices[2].coords.dot(&basis[0]),
- face1.vertices[2].coords.dot(&basis[1]),
- ),
- Point2::new(
- face1.vertices[3].coords.dot(&basis[0]),
- face1.vertices[3].coords.dot(&basis[1]),
- ),
- ];
- let projected_face2 = [
- Point2::new(
- face2.vertices[0].coords.dot(&basis[0]),
- face2.vertices[0].coords.dot(&basis[1]),
- ),
- Point2::new(
- face2.vertices[1].coords.dot(&basis[0]),
- face2.vertices[1].coords.dot(&basis[1]),
- ),
- Point2::new(
- face2.vertices[2].coords.dot(&basis[0]),
- face2.vertices[2].coords.dot(&basis[1]),
- ),
- Point2::new(
- face2.vertices[3].coords.dot(&basis[0]),
- face2.vertices[3].coords.dot(&basis[1]),
- ),
- ];
-
- // Also find all the vertices located inside of the other projected face.
- 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);
-
- 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;
- }
- }
-
- // 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 face1.num_vertices > 2 {
- let normal1 = (face1.vertices[2] - face1.vertices[1])
- .cross(&(face1.vertices[0] - face1.vertices[1]));
-
- 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]));
-
- 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 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,
- });
- }
- }
- }
- }
-
- // Now we have to compute the intersection between all pairs of
- // edges from the face 1 and from the face2.
- for j in 0..face2.num_vertices {
- let projected_edge2 = [
- projected_face2[j],
- projected_face2[(j + 1) % face2.num_vertices],
- ];
-
- for i in 0..face1.num_vertices {
- let projected_edge1 = [
- projected_face1[i],
- projected_face1[(i + 1) % face1.num_vertices],
- ];
- if let Some(bcoords) = closest_points_line2d(projected_edge1, projected_edge2) {
- if bcoords.0 > 0.0 && bcoords.0 < 1.0 && bcoords.1 > 0.0 && bcoords.1 < 1.0 {
- // Found a contact between the two edges.
- let edge1 = (
- face1.vertices[i],
- face1.vertices[(i + 1) % face1.num_vertices],
- );
- let edge2 = (
- face2.vertices[j],
- face2.vertices[(j + 1) % face2.num_vertices],
- );
- let local_p1 = edge1.0 * (1.0 - bcoords.0) + edge1.1.coords * bcoords.0;
- let local_p2 = edge2.0 * (1.0 - bcoords.1) + edge2.1.coords * bcoords.1;
- let dist = (local_p2 - local_p1).dot(&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.eids[i],
- fid2: face2.eids[j],
- dist,
- });
- }
- }
- }
- }
- }
- }
-}
-
-/// Compute the barycentric coordinates of the intersection between the two given lines.
-/// Returns `None` if the lines are parallel.
-fn closest_points_line2d(edge1: [Point2<f32>; 2], edge2: [Point2<f32>; 2]) -> Option<(f32, f32)> {
- // Inspired by Real-time collision detection by Christer Ericson.
- let dir1 = edge1[1] - edge1[0];
- let dir2 = edge2[1] - edge2[0];
- let r = edge1[0] - edge2[0];
-
- let a = dir1.norm_squared();
- let e = dir2.norm_squared();
- let f = dir2.dot(&r);
-
- let eps = f32::default_epsilon();
-
- if a <= eps && e <= eps {
- Some((0.0, 0.0))
- } else if a <= eps {
- Some((0.0, f / e))
- } else {
- let c = dir1.dot(&r);
- if e <= eps {
- Some((-c / a, 0.0))
- } else {
- let b = dir1.dot(&dir2);
- let ae = a * e;
- let bb = b * b;
- let denom = ae - bb;
-
- // Use absolute and ulps error to test collinearity.
- let parallel = denom <= eps || approx::ulps_eq!(ae, bb);
-
- let s = if !parallel {
- (b * f - c * e) / denom
- } else {
- 0.0
- };
-
- if parallel {
- None
- } else {
- Some((s, (b * s + f) / e))
- }
- }
- }
-}
generated by cgit (git 2.34.1) at 2026-04-19 06:01:11 +0000