1 files changed, 36 insertions, 22 deletions

diff --git a/src/math/triangle.rs b/src/math/triangle.rs
index b5c1bda..2b0b9ac 100644
--- a/src/math/triangle.rs
+++ b/src/math/triangle.rs
@@ -2,6 +2,7 @@
 
 use super::{LineSegment, Vec2};
 use alga::general::{ClosedMul, ClosedSub};
+use float_cmp::ApproxEq;
 use nalgebra::{RealField, Scalar};
 use num_traits::Zero;
 
@@ -15,12 +16,13 @@ pub struct Triangle<T: Scalar + Copy> {
 
 impl<T: Scalar + Copy> Triangle<T> {
     /// Create a new Triangle as defined by its three corner points
-    pub fn new(a: Vec2<T>, b: Vec2<T>, c: Vec2<T>) -> Self
+    pub fn new<M>(a: Vec2<T>, b: Vec2<T>, c: Vec2<T>, margin: M) -> Self
     where
-        T: ClosedSub + ClosedMul + PartialOrd + Zero,
+        T: ClosedSub + ClosedMul + PartialOrd + Zero + ApproxEq<Margin = M>,
+        M: Copy,
     {
         // Make sure the three points are in counterclockwise order.
-        match triplet_orientation(a, b, c) {
+        match triplet_orientation(a, b, c, margin) {
             TripletOrientation::Counterclockwise => Self { corners: [a, b, c] },
             TripletOrientation::Clockwise => Self { corners: [a, c, b] },
             TripletOrientation::Collinear => {
@@ -40,24 +42,26 @@ impl<T: Scalar + Copy> Triangle<T> {
 
     /// Create a new Triangle from a three-point slice, instead of the three points one after
     /// another.
-    pub fn from_slice(corners: [Vec2<T>; 3]) -> Self
+    pub fn from_slice<M>(corners: [Vec2<T>; 3], margin: M) -> Self
     where
-        T: ClosedSub + ClosedMul + PartialOrd + Zero,
+        T: ClosedSub + ClosedMul + PartialOrd + Zero + ApproxEq<Margin = M>,
+        M: Copy,
     {
-        Self::new(corners[0], corners[1], corners[2])
+        Self::new(corners[0], corners[1], corners[2], margin)
     }
 
     /// Check if the triangle contains a given point. If the point is right on an edge, it still
     /// counts as inside it.
-    pub fn contains_point(&self, point: Vec2<T>) -> bool
+    pub fn contains_point<M>(&self, point: Vec2<T>, margin: M) -> bool
     where
-        T: ClosedSub + ClosedMul + PartialOrd + Zero,
+        T: ClosedSub + ClosedMul + PartialOrd + Zero + ApproxEq<Margin = M>,
+        M: Copy,
     {
         // Since the points are ordered counterclockwise, check if the point is to the left of all
         // edges (or on an edge) from one point to the next. When the point is to the left of all
         // edges, it must be inside the triangle.
         for i in 0..3 {
-            if triplet_orientation(self.corners[i], self.corners[(i + 1) % 3], point)
+            if triplet_orientation(self.corners[i], self.corners[(i + 1) % 3], point, margin)
                 == TripletOrientation::Clockwise
             {
                 return false;
@@ -69,11 +73,13 @@ impl<T: Scalar + Copy> Triangle<T> {
 }
 
 /// Convert a three-point-slice into a triangle
-impl<T: Scalar + Copy + ClosedSub + ClosedMul + PartialOrd + Zero> From<[Vec2<T>; 3]>
-    for Triangle<T>
+impl<T, M> From<([Vec2<T>; 3], M)> for Triangle<T>
+where
+    T: Scalar + Copy + ClosedSub + ClosedMul + PartialOrd + Zero + ApproxEq<Margin = M>,
+    M: Copy,
 {
-    fn from(corners: [Vec2<T>; 3]) -> Self {
-        Self::new(corners[0], corners[1], corners[2])
+    fn from((corners, margin): ([Vec2<T>; 3], M)) -> Self {
+        Self::new(corners[0], corners[1], corners[2], margin)
     }
 }
 /// Convert a triangle into a three-point-slice. The corners are in counterclockwise order.
@@ -112,18 +118,26 @@ pub(crate) enum TripletOrientation {
 /// Helper function to determine which direction one would turn to traverse from the first point
 /// over the second to the third point. The third option is collinear, in which case the three points
 /// are on the same line.
-pub(crate) fn triplet_orientation<T>(a: Vec2<T>, b: Vec2<T>, c: Vec2<T>) -> TripletOrientation
+pub(crate) fn triplet_orientation<T, M>(
+    a: Vec2<T>,
+    b: Vec2<T>,
+    c: Vec2<T>,
+    margin: M,
+) -> TripletOrientation
 where
-    T: Scalar + Copy + ClosedSub + ClosedMul + PartialOrd + Zero,
+    T: Scalar + Copy + ClosedSub + ClosedMul + PartialOrd + Zero + ApproxEq<Margin = M>,
 {
     /* Check the slopes of the vector from a to b and b to c. If the slope of ab is greater than
      * that of bc, the rotation is clockwise. If ab is smaller than bc it's counterclockwise. If
      * they are the same it follows that the three points are collinear.
      */
-    match (b.y - a.y) * (c.x - b.x) - (b.x - a.x) * (c.y - b.y) {
-        q if q > T::zero() => TripletOrientation::Counterclockwise,
-        q if q < T::zero() => TripletOrientation::Clockwise,
-        _ => TripletOrientation::Collinear,
+    let slope_diff = (b.y - a.y) * (c.x - b.x) - (b.x - a.x) * (c.y - b.y);
+    if slope_diff.approx_eq(T::zero(), margin) {
+        TripletOrientation::Collinear
+    } else if slope_diff > T::zero() {
+        TripletOrientation::Counterclockwise
+    } else {
+        TripletOrientation::Clockwise
     }
 }
 
@@ -133,15 +147,15 @@ where
 ///
 /// # Panics
 /// If the length from a to b or the length from b to c is zero.
-pub(crate) fn triplet_angle<T>(a: Vec2<T>, b: Vec2<T>, c: Vec2<T>) -> T
+pub(crate) fn triplet_angle<T, M>(a: Vec2<T>, b: Vec2<T>, c: Vec2<T>, margin: M) -> T
 where
-    T: Scalar + Copy + ClosedSub + RealField + Zero,
+    T: Scalar + Copy + ClosedSub + RealField + Zero + ApproxEq<Margin = M>,
 {
     assert!(a != b);
     assert!(b != c);
 
     // Handle the extreme 0 and 180 degree cases
-    let orientation = triplet_orientation(a, b, c);
+    let orientation = triplet_orientation(a, b, c, margin);
     if orientation == TripletOrientation::Collinear {
         if LineSegment::new(a, b).contains_collinear(c)
             || LineSegment::new(b, c).contains_collinear(a)