feat: add generic Subspace struct with distance calculation

4 files changed, 70 insertions, 32 deletions

diff --git a/src/algebra/line.rs b/src/algebra/line.rs
deleted file mode 100644
index 45733b9..0000000
--- a/src/algebra/line.rs
+++ /dev/null
@@ -1,24 +0,0 @@
-use crate::{particle_system::Scalar, Point, Vector};
-
-pub struct Line {
-    pub point: Point,
-
-    /// Has to be normalized
-    pub vector: Vector,
-}
-
-impl Line {
-    pub fn new(point: Point, vector: Vector) -> Self {
-        Self {
-            point,
-            vector: vector.normalize(),
-        }
-    }
-
-    pub fn distance_to_point(&self, point: Point) -> Scalar {
-        let diff = point - self.point;
-        let lambda = diff.dot(&self.vector);
-
-        (diff - lambda * self.vector).norm()
-    }
-}
diff --git a/src/algebra/mod.rs b/src/algebra/mod.rs
index 884ccfc..13dcd6a 100644
--- a/src/algebra/mod.rs
+++ b/src/algebra/mod.rs
@@ -1,2 +1 @@
-pub mod plane;
-pub mod line;
+pub mod subspace;
diff --git a/src/algebra/plane.rs b/src/algebra/plane.rs
deleted file mode 100644
index e82c843..0000000
--- a/src/algebra/plane.rs
+++ /dev/null
@@ -1,6 +0,0 @@
-use crate::{Point, Vector};
-
-pub struct Plane {
-    point: Point,
-    vectors: [Vector; 2],
-}
diff --git a/src/algebra/subspace.rs b/src/algebra/subspace.rs
new file mode 100644
index 0000000..3b32ba7
--- /dev/null
+++ b/src/algebra/subspace.rs
@@ -0,0 +1,69 @@
+use nalgebra::SMatrix;
+
+use crate::{particle_system::Scalar, particle_system::N, Point, Vector};
+
+type ProjectionMatrix = SMatrix<Scalar, N, N>;
+
+pub struct Subspace<const DIM: usize> {
+    point: Point,
+    vectors: [Vector; DIM],
+    projection_matrix: ProjectionMatrix,
+}
+
+pub type Line = Subspace<1>;
+pub type Plane = Subspace<2>;
+
+impl<const DIM: usize> Subspace<DIM> {
+    pub fn new(point: Point, mut vectors: [Vector; DIM]) -> Self {
+        for vector in &mut vectors {
+            vector.normalize_mut();
+        }
+
+        Self {
+            point,
+            vectors,
+            projection_matrix: {
+                let v = SMatrix::<Scalar, N, DIM>::from_columns(&vectors);
+                let transpose = v.transpose();
+                let inverse = (transpose * v).try_inverse().unwrap();
+                v * inverse * transpose
+            }
+        }
+    }
+
+    pub fn project_point(&self, point: Point) -> Point {
+        self.projection_matrix * (point - self.point.coords) + self.point.coords
+    }
+
+    pub fn distance(&self, point: Point) -> Scalar {
+        let projected = self.project_point(point);
+        (projected - point).norm()
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use crate::{algebra::subspace::Line, Point, Vector};
+
+    #[test]
+    fn test_projection_onto_line() {
+        let line = Line::new(Point::new(1.0, 0.0), [Vector::new(3.0, 1.0)]);
+        let point = Point::new(3.0, 4.0);
+
+        let matrix = line.projection_matrix;
+        {
+            let diff = matrix * matrix - matrix;
+            assert!(diff.norm() < 0.001, "Projection matrix squared should be equal to itself: {}, {}", matrix, matrix* matrix);
+        }
+        {
+            let diff = matrix - matrix.transpose();
+            assert!(diff.norm() < 0.001, "Projection matrix transpose should be equal to itself: {}, {}", matrix, matrix.transpose());
+        }
+
+        {
+            let projected = line.project_point(point);
+            let diff = projected - Point::new(4.0, 1.0);
+            assert!(diff.norm() < 0.001, "Point projected incorrectly");
+        }
+    }
+}