2 files changed, 79 insertions, 0 deletions
diff --git a/physics/src/algebra/mod.rs b/physics/src/algebra/mod.rs
new file mode 100644
index 0000000..47efba8
--- /dev/null
+++ b/physics/src/algebra/mod.rs
@@ -0,0 +1,9 @@
+use nalgebra::{Point as PointBase, SVector};
+
+pub const N: usize = 2;
+pub type Scalar = f64;
+
+pub type Vector = SVector<Scalar, N>;
+pub type Point = PointBase<Scalar, N>;
+
+pub mod subspace;
diff --git a/physics/src/algebra/subspace.rs b/physics/src/algebra/subspace.rs
new file mode 100644
index 0000000..9fb944e
--- /dev/null
+++ b/physics/src/algebra/subspace.rs
@@ -0,0 +1,70 @@
+use nalgebra::SMatrix;
+
+use super::{Scalar, 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");
+        }
+    }
+}