From f96c4e0a6a6620cede9930455f23a9bd975e914d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Manuel=20V=C3=B6gele?= <develop@manuel-voegele.de>
Date: Thu, 20 Oct 2022 09:28:08 +0200
Subject: [PATCH] Do a cheap bouding box collission check before doing an
 expensive line intersection test

---
 rust/src/geometry.rs   | 43 ++++++++++++++++++++++++++++++++++++++++++
 rust/src/pathfinder.rs | 15 ++++++++++++---
 2 files changed, 55 insertions(+), 3 deletions(-)

diff --git a/rust/src/geometry.rs b/rust/src/geometry.rs
index 0c219d0..b5288bb 100644
--- a/rust/src/geometry.rs
+++ b/rust/src/geometry.rs
@@ -179,9 +179,52 @@ impl LineSegment {
 		}
 		between(intersection.x, self.p1.x, self.p2.x)
 	}
+
+	pub fn bounding_rect(&self) -> Rect {
+		let (left, right) = minmax(self.p1.x, self.p2.x);
+		let (top, bottom) = minmax(self.p1.y, self.p2.y);
+		Rect {
+			left,
+			right,
+			top,
+			bottom,
+		}
+	}
 }
 
 pub fn between<T: Copy + PartialOrd>(num: T, a: T, b: T) -> bool {
 	let (min, max) = if a < b { (a, b) } else { (b, a) };
 	num >= min && num <= max
 }
+
+pub fn minmax<T: PartialOrd>(a: T, b: T) -> (T, T) {
+	if a <= b {
+		(a, b)
+	} else {
+		(b, a)
+	}
+}
+
+pub fn minmax_by_key<T, F: Fn(&T) -> K, K: PartialOrd>(a: T, b: T, f: F) -> (T, T) {
+	if f(&a) < f(&b) {
+		(a, b)
+	} else {
+		(b, a)
+	}
+}
+
+#[derive(Debug, Clone, Copy)]
+pub struct Rect {
+	pub left: f64,
+	pub right: f64,
+	pub top: f64,
+	pub bottom: f64,
+}
+
+impl Rect {
+	pub fn intersects(&self, other: &Rect) -> bool {
+		let (left_rect, right_rect) = minmax_by_key(self, other, |rect| rect.left);
+		let (top_rect, bottom_rect) = minmax_by_key(self, other, |rect| rect.top);
+		left_rect.right >= right_rect.left && top_rect.bottom >= bottom_rect.top
+	}
+}
diff --git a/rust/src/pathfinder.rs b/rust/src/pathfinder.rs
index a124b29..6316985 100644
--- a/rust/src/pathfinder.rs
+++ b/rust/src/pathfinder.rs
@@ -150,11 +150,20 @@ impl Pathfinder {
 	}
 
 	fn points_connected(p1: Point, p2: Point, walls: &[LineSegment]) -> bool {
-		!Self::collides_with_wall(&LineSegment::new(p1, p2), walls)
+		!Self::collides_with_any_wall(&LineSegment::new(p1, p2), walls)
 	}
 
-	fn collides_with_wall(line: &LineSegment, walls: &[LineSegment]) -> bool {
+	fn collides_with_any_wall(line: &LineSegment, walls: &[LineSegment]) -> bool {
 		// TODO Directional walls
-		walls.iter().any(|wall| line.intersection(wall).is_some())
+		walls
+			.iter()
+			.any(|wall| Self::collides_with_wall(line, wall))
+	}
+
+	fn collides_with_wall(line: &LineSegment, wall: &LineSegment) -> bool {
+		if !line.bounding_rect().intersects(&wall.bounding_rect()) {
+			return false;
+		}
+		line.intersection(wall).is_some()
 	}
 }