feat: add generic Subspace struct with distance calculation

6 files changed, 76 insertions, 35 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");
+        }
+    }
+}
diff --git a/src/constraint/slider.rs b/src/constraint/slider.rs
index ceea4ea..9f7973b 100644
--- a/src/constraint/slider.rs
+++ b/src/constraint/slider.rs
@@ -3,7 +3,7 @@ use std::usize;
 use nalgebra::{DVector, RowDVector};
 
 use crate::{
-    algebra::line::Line,
+    algebra::subspace::Line,
     particle_system::{Scalar, N},
     ParticleSystem, Point, Vector,
 };
@@ -23,7 +23,7 @@ impl ParticleSystem {
         let point = self.particles[particle_id].position;
         self.constraints.push(Box::new(SliderConstraint {
             particle_id,
-            line: Line::new(point, direction),
+            line: Line::new(point, [direction]),
             jacobian: RowDVector::zeros(self.particles.len() * N),
         }));
     }
@@ -37,7 +37,7 @@ impl Constraint for SliderConstraint {
     fn c(&self, q: &DVector<Scalar>) -> Scalar {
         let position = q.fixed_rows::<N>(self.particle_id * N);
         let point = Point::from_coordinates(position.into());
-        self.line.distance_to_point(point)
+        self.line.distance(point)
     }
 
     fn set_jacobian(&mut self, jacobian: RowDVector<Scalar>) {
diff --git a/src/solver/mod.rs b/src/solver/mod.rs
index 4bb8747..1544378 100644
--- a/src/solver/mod.rs
+++ b/src/solver/mod.rs
@@ -85,6 +85,7 @@ mod tests {
         let system = ParticleSystem {
             particles: vec![Particle::new(Point::new(2.0, 3.0), 1.0)],
             constraints: vec![],
+            forces: vec![],
             t: 0.0,
         };
         let phase_space = system.collect_phase_space();
@@ -106,6 +107,7 @@ mod tests {
                 Particle::new(Point::new(0.0, 0.0), 1.0),
             ],
             constraints: vec![],
+            forces: vec![],
             t: 0.0,
         };
 
@@ -131,6 +133,7 @@ mod tests {
         let mut system = ParticleSystem {
             particles: vec![Particle::new(Point::origin(), mass)],
             constraints: vec![],
+            forces: vec![],
             t: 0.0,
         };