Incomplete implementation of low-level collision routines (untested)

src/math.rs

@@ -1,5 +1,5 @@
 use std::ops::Sub;
-use geo::geometry::Point;
+use geo::{geometry::Point, EuclideanDistance, point};
 
 #[derive(Debug, Clone, Copy, PartialEq)]
 pub struct Line {
@@ -101,6 +101,49 @@ pub fn tangent_point_pair(circle1: Circle, cw1: Option<bool>, circle2: Circle, c
     unreachable!();
 }
 
+pub fn circles_intersection(circle1: &Circle, circle2: &Circle) -> Vec<Point> {
+    let delta = circle2.pos - circle1.pos;
+    let d = circle2.pos.euclidean_distance(&circle1.pos);
+
+    if d > circle1.r + circle2.r {
+        // No intersection.
+        return vec![];
+    }
+
+    if d < (circle2.r - circle1.r).abs() {
+        // One contains the other.
+        return vec![];
+    }
+
+    // Distance from `circle1.pos` to the intersection of the diagonals.
+    let a = (circle1.r*circle1.r - circle2.r*circle2.r + d*d) / (2.0*d);
+
+    // Intersection of the diagonals.
+    let p = circle1.pos + delta*(a/d);
+    let h = (circle1.r*circle1.r - a*a).sqrt();
+
+    if h == 0. {
+        return [p].into();
+    }
+
+    let r = point! {x: -delta.x(), y: delta.y()} * (h/d);
+    
+    [
+        p + r,
+        p - r,
+    ].into()
+}
+
+pub fn between_vectors(v: 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.
+    } else {
+        cross_product(from, v) >= 0. || cross_product(v, to) >= 0.
+    }
+}
+
 pub fn seq_cross_product(start: Point, stop: Point, reference: Point) -> f64 {
     let dx1 = stop.x() - start.x();
     let dy1 = stop.y() - start.y();

src/shape.rs

@@ -1,8 +1,10 @@
-use geo::{Point, EuclideanDistance};
+use std::mem;
+
+use geo::{Point, EuclideanDistance, point, Polygon, Rotate, polygon, Intersects};
 use rstar::{RTreeObject, AABB};
 
 use crate::graph::{TaggedWeight, DotWeight};
-use crate::math::Circle;
+use crate::math::{Circle, self};
 
 #[derive(Debug, PartialEq)]
 pub struct DotShape {
@@ -16,6 +18,23 @@ pub struct SegShape {
     pub width: f64,
 }
 
+impl SegShape {
+    fn polygon(&self) -> Polygon {
+        let tangent_vector = self.to - self.from;
+        let tangent_vector_norm = tangent_vector.euclidean_distance(&point! {x: 0., y: 0.});
+        let unit_tangent_vector = tangent_vector / tangent_vector_norm;
+
+        let normal = unit_tangent_vector.rotate_around_point(-90., point! {x: 0., y: 0.});
+
+        let p1 = self.from - normal * self.width;
+        let p2 = self.from + normal * self.width;
+        let p3 = self.to + normal * self.width;
+        let p4 = self.to - normal * self.width;
+
+        polygon![p1.0, p2.0, p3.0, p4.0]
+    }
+}
+
 #[derive(Debug, PartialEq)]
 pub struct BendShape {
     pub from: Point,
@@ -24,14 +43,104 @@ pub struct BendShape {
     pub width: f64,
 }
 
+impl BendShape {
+    fn inner_circle(&self) -> Circle {
+        Circle {
+            pos: self.center,
+            r: self.center.euclidean_distance(&self.from) - self.width / 2.,
+        }
+    }
+
+    fn outer_circle(&self) -> Circle {
+        Circle {
+            pos: self.center,
+            r: self.center.euclidean_distance(&self.from) + self.width / 2.,
+        }
+    }
+}
+
 #[derive(Debug, PartialEq)]
 pub enum Shape {
+    // Intentionally in different order to reorder `self.intersects(...)` properly.
     Dot(DotShape),
     Seg(SegShape),
     Bend(BendShape),
 }
 
 impl Shape {
+    pub fn principal_point(&self) -> Point {
+        match self {
+            Shape::Dot(dot) => dot.c.pos,
+            Shape::Seg(seg) => seg.from,
+            Shape::Bend(bend) => bend.from,
+        }
+    }
+
+    fn priority(&self) -> i64 {
+        match self {
+            Shape::Dot(..) => 3,
+            Shape::Bend(..) => 2,
+            Shape::Seg(..) => 1,
+        }
+    }
+
+    pub fn intersects(&self, other: &Shape) -> bool {
+        if self.priority() < other.priority() {
+            return other.intersects(self);
+        }
+
+        match self {
+            Shape::Dot(dot) => {
+                match other {
+                    Shape::Dot(other) => {
+                        dot.c.pos.euclidean_distance(&other.c.pos) < dot.c.r + other.c.r
+                    },
+                    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()) {
+                            if !math::between_vectors(point - other.center,
+                                                      other.from - other.center,
+                                                      other.to - other.center) {
+                                return false;
+                            }
+                        }
+
+                        for point in math::circles_intersection(&dot.c, &other.outer_circle()) {
+                            if !math::between_vectors(point - other.center,
+                                                      other.from - other.center,
+                                                      other.to - other.center) {
+                                return false;
+                            }
+                        }
+
+                        true
+                    },
+                }
+            },
+            Shape::Seg(seg) => {
+                match other {
+                    Shape::Dot(..) => unreachable!(),
+                    Shape::Seg(other) => {
+                        seg.polygon().intersects(&other.polygon())
+                    },
+                    Shape::Bend(other) => {
+                        false // TODO.
+                    },
+                }
+            },
+            Shape::Bend(bend) => {
+                match other {
+                    Shape::Dot(..) | Shape::Seg(..) => unreachable!(),
+                    Shape::Bend(other) => {
+                        false // TODO.
+                    },
+                }
+            },
+        }
+    }
+
     pub fn envelope(&self) -> AABB<[f64; 2]> {
         match self {
             Shape::Dot(dot) =>