Add intersection point algorithm

1 files changed, 40 insertions, 3 deletions

diff --git a/src/math/line_segment.rs b/src/math/line_segment.rs
index 338a681..7dacf54 100644
--- a/src/math/line_segment.rs
+++ b/src/math/line_segment.rs
@@ -1,6 +1,6 @@
-use super::Vec2;
-use alga::general::{ClosedMul, ClosedSub};
-use nalgebra::Scalar;
+use super::{Rect, Vec2};
+use alga::general::{ClosedDiv, ClosedMul, ClosedSub};
+use nalgebra::{RealField, Scalar};
 use num_traits::Zero;
 
 #[derive(Debug)]
@@ -89,6 +89,43 @@ impl<T: Scalar + Copy> LineSegment<T> {
 
         false
     }
+
+    pub fn intersection(line_a: &LineSegment<T>, line_b: &LineSegment<T>) -> Option<Vec2<T>>
+    where
+        T: ClosedSub + ClosedMul + ClosedDiv + Zero + RealField,
+    {
+        // Calculate the differences of each line value from starting point to end point
+        // coordinate.
+        let dax = line_a.end.x - line_a.start.x;
+        let dbx = line_b.end.x - line_b.start.x;
+        let day = line_a.end.y - line_a.start.y;
+        let dby = line_b.end.y - line_b.start.y;
+
+        // Calculate determinant to see, if the lines are parallel or not
+        // TODO: Collinearity check?
+        let d = (dax * dby) - (day * dbx);
+        if d == T::zero() {
+            None
+        } else {
+            let ad = (line_a.start.x * line_a.end.y) - (line_a.start.y * line_a.end.x);
+            let bd = (line_b.start.x * line_b.end.y) - (line_b.start.y * line_b.end.x);
+
+            let out = Vec2::new(((ad * dbx) - (dax * bd)) / d, ((ad * dby) - (day * bd)) / d);
+
+            /* Since the line intersection, not the line segment intersection is calculated, a
+             * bounding check is necessary, to see if the intersection still lies inside both of
+             * the segments. We know it's on the lines, so checking with the lines bounding box is
+             * faster than checking where on the line exactly it would be.
+             */
+            if Rect::bounding_rect(line_a.start, line_a.end).contains(out)
+                && Rect::bounding_rect(line_b.start, line_b.end).contains(out)
+            {
+                Some(out)
+            } else {
+                None
+            }
+        }
+    }
 }
 
 #[derive(PartialEq, Eq)]