Fix handling of duplicate points in approximation

src/kernel/approximation.rs

@@ -1,11 +1,11 @@
-use std::collections::HashSet;
+use std::{cmp::Ordering, collections::HashSet};
 
 use decorum::R64;
 use parry3d_f64::shape::Segment;
 
 use crate::math::Point;
 
-use super::topology::edges::Edge;
+use super::topology::edges::{Cycle, Edge};
 
 /// An approximation of an edge, multiple edges, or a face
 #[derive(Debug, PartialEq)]
@@ -61,6 +61,50 @@ impl Approximation {
         Self { points, segments }
     }
 
+    /// Compute an approximation for a cycle
+    ///
+    /// `tolerance` defines how far the approximation is allowed to deviate from
+    /// the actual cycle.
+    pub fn for_cycle(cycle: &Cycle, tolerance: f64) -> Self {
+        let mut points = Vec::new();
+        let mut segments = Vec::new();
+
+        for edge in &cycle.edges {
+            let approx = Self::for_edge(edge, tolerance);
+
+            points.extend(approx.points);
+            segments.extend(approx.segments);
+        }
+
+        // As this is a cycle, neighboring edges are going to share vertices.
+        // Let's remove all those duplicates.
+        points.sort_by(|a, b| {
+            if a.x < b.x {
+                return Ordering::Less;
+            }
+            if a.x > b.x {
+                return Ordering::Greater;
+            }
+            if a.y < b.y {
+                return Ordering::Less;
+            }
+            if a.y > b.y {
+                return Ordering::Greater;
+            }
+            if a.z < b.z {
+                return Ordering::Less;
+            }
+            if a.z > b.z {
+                return Ordering::Greater;
+            }
+
+            Ordering::Equal
+        });
+        points.dedup();
+
+        Self { points, segments }
+    }
+
     /// Validate the approximation
     ///
     /// Returns an `Err(ValidationError)`, if the validation is not valid. See
@@ -147,7 +191,10 @@ mod tests {
     use nalgebra::point;
     use parry3d_f64::shape::Segment;
 
-    use crate::kernel::{geometry::Curve, topology::edges::Edge};
+    use crate::kernel::{
+        geometry::Curve,
+        topology::edges::{Cycle, Edge},
+    };
 
     use super::Approximation;
 
@@ -200,6 +247,47 @@ mod tests {
         );
     }
 
+    #[test]
+    fn test_for_cycle() {
+        let tolerance = 1.;
+
+        let a = point![1., 2., 3.];
+        let b = point![2., 3., 5.];
+        let c = point![3., 5., 8.];
+
+        let ab = Edge {
+            curve: Curve::Mock { approx: vec![a, b] },
+            vertices: Some([(), ()]),
+            reverse: false,
+        };
+        let bc = Edge {
+            curve: Curve::Mock { approx: vec![b, c] },
+            vertices: Some([(), ()]),
+            reverse: false,
+        };
+        let ca = Edge {
+            curve: Curve::Mock { approx: vec![c, a] },
+            vertices: Some([(), ()]),
+            reverse: false,
+        };
+
+        let cycle = Cycle {
+            edges: vec![ab, bc, ca],
+        };
+
+        assert_eq!(
+            Approximation::for_cycle(&cycle, tolerance),
+            Approximation {
+                points: vec![a, b, c],
+                segments: vec![
+                    Segment { a: a, b: b },
+                    Segment { a: b, b: c },
+                    Segment { a: c, b: a },
+                ],
+            }
+        );
+    }
+
     #[test]
     fn test_validate() {
         let a = point![0., 1., 2.];

src/kernel/topology/edges.rs

@@ -70,7 +70,7 @@ impl Edges {
         let mut segments = Vec::new();
 
         for cycle in &self.cycles {
-            let approx = cycle.approx(tolerance);
+            let approx = Approximation::for_cycle(cycle, tolerance);
 
             points.extend(approx.points);
             segments.extend(approx.segments);
@@ -90,30 +90,6 @@ pub struct Cycle {
     pub edges: Vec<Edge>,
 }
 
-impl Cycle {
-    /// Compute an approximation of the cycle
-    ///
-    /// `tolerance` defines how far the approximation is allowed to deviate from
-    /// the actual cycle.
-    pub fn approx(&self, tolerance: f64) -> Approximation {
-        let mut points = Vec::new();
-        let mut segments = Vec::new();
-
-        for edge in &self.edges {
-            let approx = Approximation::for_edge(&edge, tolerance);
-
-            points.extend(approx.points);
-            segments.extend(approx.segments);
-        }
-
-        // As this is a cycle, the last vertex of an edge could be identical to
-        // the first vertex of the next. Let's remove those duplicates.
-        points.dedup();
-
-        Approximation { points, segments }
-    }
-}
-
 /// An edge of a shape
 #[derive(Clone, Debug)]
 pub struct Edge {

src/kernel/topology/faces.rs

@@ -11,7 +11,6 @@ use parry3d_f64::{
     query::Ray as Ray3,
     shape::{Segment as Segment3, Triangle},
 };
-use tracing::warn;
 
 use crate::{
     debug::{DebugInfo, TriangleEdgeCheck},
@@ -110,12 +109,7 @@ impl Face {
         match self {
             Self::Face { edges, surface } => {
                 let approx = edges.approx(tolerance);
-
-                // Can't make this a panic, as the current approximation code
-                // actually produces invalid approximations.
-                if let Err(err) = approx.validate() {
-                    warn!("Invalid approximation: {:?}", err);
-                }
+                approx.validate().expect("Invalid approximation");
 
                 let points: Vec<_> = approx
                     .points