Add seg-bend and bend-bend collision detection

src/math.rs

@@ -1,8 +1,8 @@
-use geo::{geometry::Point, point, EuclideanDistance};
+use geo::{geometry::Point, point, EuclideanDistance, Line};
 use std::ops::Sub;
 
 #[derive(Debug, Clone, Copy, PartialEq)]
-pub struct Line {
+pub struct CanonicalLine {
     pub a: f64,
     pub b: f64,
     pub c: f64,
@@ -18,7 +18,6 @@ impl Sub for Circle {
     type Output = Self;
 
     fn sub(self, other: Self) -> Self {
-        //return Self{pos: Point{x: self.pos.x() - other.pos.x(), y: self.pos.y() - other.pos.y()}, r: self.r};
         return Self {
             pos: self.pos - other.pos,
             r: self.r,
@@ -26,7 +25,7 @@ impl Sub for Circle {
     }
 }
 
-fn _tangent(center: Point, r1: f64, r2: f64) -> Line {
+fn _tangent(center: Point, r1: f64, r2: f64) -> CanonicalLine {
     let epsilon = 1e-9;
     let dr = r2 - r1;
     let norm = center.x() * center.x() + center.y() * center.y();
@@ -38,15 +37,15 @@ fn _tangent(center: Point, r1: f64, r2: f64) -> Line {
 
     let sqrt_discriminant = f64::sqrt(f64::abs(discriminant));
 
-    Line {
+    CanonicalLine {
         a: (center.x() * dr + center.y() * sqrt_discriminant) / norm,
         b: (center.y() * dr - center.x() * sqrt_discriminant) / norm,
         c: r1,
     }
 }
 
-fn _tangents(circle1: Circle, circle2: Circle) -> [Line; 4] {
+fn _tangents(circle1: Circle, circle2: Circle) -> [CanonicalLine; 4] {
-    let mut tgs: [Line; 4] = [
+    let mut tgs: [CanonicalLine; 4] = [
         _tangent((circle2 - circle1).pos, -circle1.r, -circle2.r),
         _tangent((circle2 - circle1).pos, -circle1.r, circle2.r),
         _tangent((circle2 - circle1).pos, circle1.r, -circle2.r),
@@ -60,7 +59,7 @@ fn _tangents(circle1: Circle, circle2: Circle) -> [Line; 4] {
     return tgs;
 }
 
-fn cast_point_to_line(pt: Point, line: Line) -> Point {
+fn cast_point_to_canonical_line(pt: Point, line: CanonicalLine) -> Point {
     return (
         (line.b * (line.b * pt.x() - line.a * pt.y()) - line.a * line.c)
             / (line.a * line.a + line.b * line.b),
@@ -75,20 +74,20 @@ pub fn tangent_point_pairs(circle1: Circle, circle2: Circle) -> [(Point, Point);
 
     [
         (
-            cast_point_to_line(circle1.pos, tgs[0]),
+            cast_point_to_canonical_line(circle1.pos, tgs[0]),
-            cast_point_to_line(circle2.pos, tgs[0]),
+            cast_point_to_canonical_line(circle2.pos, tgs[0]),
         ),
         (
-            cast_point_to_line(circle1.pos, tgs[1]),
+            cast_point_to_canonical_line(circle1.pos, tgs[1]),
-            cast_point_to_line(circle2.pos, tgs[1]),
+            cast_point_to_canonical_line(circle2.pos, tgs[1]),
         ),
         (
-            cast_point_to_line(circle1.pos, tgs[2]),
+            cast_point_to_canonical_line(circle1.pos, tgs[2]),
-            cast_point_to_line(circle2.pos, tgs[2]),
+            cast_point_to_canonical_line(circle2.pos, tgs[2]),
         ),
         (
-            cast_point_to_line(circle1.pos, tgs[3]),
+            cast_point_to_canonical_line(circle1.pos, tgs[3]),
-            cast_point_to_line(circle2.pos, tgs[3]),
+            cast_point_to_canonical_line(circle2.pos, tgs[3]),
         ),
     ]
 }
@@ -124,7 +123,7 @@ pub fn tangent_point_pair(
     unreachable!();
 }
 
-pub fn circles_intersection(circle1: &Circle, circle2: &Circle) -> Vec<Point> {
+pub fn intersect_circles(circle1: &Circle, circle2: &Circle) -> Vec<Point> {
     let delta = circle2.pos - circle1.pos;
     let d = circle2.pos.euclidean_distance(&circle1.pos);
 
@@ -154,13 +153,54 @@ pub fn circles_intersection(circle1: &Circle, circle2: &Circle) -> Vec<Point> {
     [p + r, p - r].into()
 }
 
-pub fn between_vectors(v: Point, from: Point, to: Point) -> bool {
+/*pub fn cast_point_to_segment(p: &Point, segment: &Line) -> Option<Point> {
+    let delta = segment.end_point() - segment.start_point();
+    let rel_p = *p - segment.start_point();
+    let t = delta.dot(rel_p) / delta.dot(delta);
+
+    if t < 0.0 || t > 1.0 {
+        return None;
+    }
+
+    Some(segment.start_point() + delta * t)
+}*/
+
+pub fn intersect_circle_segment(circle: &Circle, segment: &Line) -> Vec<Point> {
+    let delta: Point = segment.delta().into();
+    let from = segment.start_point();
+    let to = segment.end_point();
+    let epsilon = 1e-9;
+
+    let a = delta.dot(delta);
+    let b =
+        2.0 * (delta.x() * (from.x() - circle.pos.x()) + delta.y() * (from.y() - circle.pos.y()));
+    let c = circle.pos.dot(circle.pos) + from.dot(from)
+        - 2.0 * circle.pos.dot(from)
+        - circle.r * circle.r;
+    let discriminant = b * b - 4.0 * a * c;
+
+    if a.abs() < epsilon || discriminant < 0.0 {
+        return [].into();
+    }
+
+    if discriminant == 0.0 {
+        return [from + (to - from) * -b / (2.0 * a)].into();
+    }
+
+    [
+        from + (to - from) * (-b + discriminant.sqrt()) / (2.0 * a),
+        from + (to - from) * (-b - discriminant.sqrt()) / (2.0 * a),
+    ]
+    .into()
+}
+
+pub fn between_vectors(p: Point, from: Point, to: Point) -> bool {
     let cross = cross_product(from, to);
 
     if cross >= 0. {
-        cross_product(from, v) >= 0. && cross_product(v, to) >= 0.
+        cross_product(from, p) >= 0. && cross_product(p, to) >= 0.
     } else {
-        cross_product(from, v) >= 0. || cross_product(v, to) >= 0.
+        cross_product(from, p) >= 0. || cross_product(p, to) >= 0.
     }
 }
 

src/shape.rs

@@ -57,6 +57,14 @@ impl BendShape {
             r: self.center.euclidean_distance(&self.from) + self.width / 2.0,
         }
     }
+
+    fn between_ends(&self, point: Point) -> bool {
+        math::between_vectors(
+            point - self.center,
+            self.from - self.center,
+            self.to - self.center,
+        )
+    }
 }
 
 #[derive(Debug, PartialEq)]
@@ -96,22 +104,14 @@ impl Shape {
                 }
                 Shape::Seg(other) => dot.c.pos.euclidean_distance(&other.polygon()) < dot.c.r,
                 Shape::Bend(other) => {
-                    for point in math::circles_intersection(&dot.c, &other.inner_circle()) {
+                    for point in math::intersect_circles(&dot.c, &other.inner_circle()) {
-                        if math::between_vectors(
+                        if other.between_ends(point) {
-                            point - other.center,
-                            other.from - other.center,
-                            other.to - other.center,
-                        ) {
                             return true;
                         }
                     }
 
-                    for point in math::circles_intersection(&dot.c, &other.outer_circle()) {
+                    for point in math::intersect_circles(&dot.c, &other.outer_circle()) {
-                        if math::between_vectors(
+                        if other.between_ends(point) {
-                            point - other.center,
-                            other.from - other.center,
-                            other.to - other.center,
-                        ) {
                             return true;
                         }
                     }
@@ -119,24 +119,69 @@ impl Shape {
                     false
                 }
             },
-            Shape::Seg(seg) => {
+            Shape::Seg(seg) => match other {
-                match other {
                 Shape::Dot(..) => unreachable!(),
                 Shape::Seg(other) => seg.polygon().intersects(&other.polygon()),
                 Shape::Bend(other) => {
-                        false // TODO.
+                    for segment in seg.polygon().exterior().lines() {
+                        let inner_circle = other.inner_circle();
+                        let outer_circle = other.outer_circle();
+
+                        for point in math::intersect_circle_segment(&inner_circle, &segment) {
+                            if other.between_ends(point) {
+                                return true;
+                            }
+                        }
+
+                        for point in math::intersect_circle_segment(&outer_circle, &segment) {
+                            if other.between_ends(point) {
+                                return true;
                             }
                         }
                     }
-            Shape::Bend(bend) => {
+
-                match other {
+                    false
+                }
+            },
+            Shape::Bend(bend) => match other {
                 Shape::Dot(..) | Shape::Seg(..) => unreachable!(),
                 Shape::Bend(other) => {
-                        false // TODO.
+                    for point in
+                        math::intersect_circles(&bend.inner_circle(), &other.inner_circle())
+                    {
+                        if bend.between_ends(point) && other.between_ends(point) {
+                            return true;
                         }
                     }
+
+                    for point in
+                        math::intersect_circles(&bend.inner_circle(), &other.outer_circle())
+                    {
+                        if bend.between_ends(point) && other.between_ends(point) {
+                            return true;
                         }
                     }
+
+                    for point in
+                        math::intersect_circles(&bend.outer_circle(), &other.inner_circle())
+                    {
+                        if bend.between_ends(point) && other.between_ends(point) {
+                            return true;
+                        }
+                    }
+
+                    for point in
+                        math::intersect_circles(&bend.outer_circle(), &other.outer_circle())
+                    {
+                        if bend.between_ends(point) && other.between_ends(point) {
+                            return true;
+                        }
+                    }
+
+                    false
+                }
+            },
+        }
     }
 
     pub fn envelope(&self) -> AABB<[f64; 2]> {