diff --git a/src/math.rs b/src/math.rs index a7f0937..3891e47 100644 --- a/src/math.rs +++ b/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] { - let mut tgs: [Line; 4] = [ +fn _tangents(circle1: Circle, circle2: Circle) -> [CanonicalLine; 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_line(circle2.pos, tgs[0]), + cast_point_to_canonical_line(circle1.pos, tgs[0]), + cast_point_to_canonical_line(circle2.pos, tgs[0]), ), ( - cast_point_to_line(circle1.pos, tgs[1]), - cast_point_to_line(circle2.pos, tgs[1]), + cast_point_to_canonical_line(circle1.pos, tgs[1]), + cast_point_to_canonical_line(circle2.pos, tgs[1]), ), ( - cast_point_to_line(circle1.pos, tgs[2]), - cast_point_to_line(circle2.pos, tgs[2]), + cast_point_to_canonical_line(circle1.pos, tgs[2]), + cast_point_to_canonical_line(circle2.pos, tgs[2]), ), ( - cast_point_to_line(circle1.pos, tgs[3]), - cast_point_to_line(circle2.pos, tgs[3]), + cast_point_to_canonical_line(circle1.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 { +pub fn intersect_circles(circle1: &Circle, circle2: &Circle) -> Vec { 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 { [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 { + 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 { + 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. } } diff --git a/src/shape.rs b/src/shape.rs index d2c8958..c6b37fb 100644 --- a/src/shape.rs +++ b/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()) { - if math::between_vectors( - point - other.center, - other.from - other.center, - other.to - other.center, - ) { + for point in math::intersect_circles(&dot.c, &other.inner_circle()) { + if other.between_ends(point) { return true; } } - 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, - ) { + for point in math::intersect_circles(&dot.c, &other.outer_circle()) { + if other.between_ends(point) { return true; } } @@ -119,23 +119,68 @@ impl Shape { false } }, - Shape::Seg(seg) => { - match other { - Shape::Dot(..) => unreachable!(), - Shape::Seg(other) => seg.polygon().intersects(&other.polygon()), - Shape::Bend(other) => { - false // TODO. + Shape::Seg(seg) => match other { + Shape::Dot(..) => unreachable!(), + Shape::Seg(other) => seg.polygon().intersects(&other.polygon()), + Shape::Bend(other) => { + 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; + } + } } + + false } - } - Shape::Bend(bend) => { - match other { - Shape::Dot(..) | Shape::Seg(..) => unreachable!(), - Shape::Bend(other) => { - false // TODO. + }, + Shape::Bend(bend) => match other { + Shape::Dot(..) | Shape::Seg(..) => unreachable!(), + Shape::Bend(other) => { + 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 } - } + }, } }