3 files changed, 36 insertions, 8 deletions
diff --git a/physics/src/algebra/distance_field.rs b/physics/src/algebra/distance_field.rs
new file mode 100644
index 0000000..3b511f1
--- /dev/null
+++ b/physics/src/algebra/distance_field.rs
@@ -0,0 +1,11 @@
+use super::{Point, Scalar};
+
+pub trait DistanceField {
+    fn distance(&self, point: Point) -> Scalar;
+}
+
+impl DistanceField for Point {
+    fn distance(&self, point: Point) -> Scalar {
+        (self - point).norm()
+    }
+}
diff --git a/physics/src/algebra/mod.rs b/physics/src/algebra/mod.rs
index 47efba8..116d44d 100644
--- a/physics/src/algebra/mod.rs
+++ b/physics/src/algebra/mod.rs
@@ -1,9 +1,10 @@
 use nalgebra::{Point as PointBase, SVector};
 
-pub const N: usize = 2;
+pub const N: usize = 3;
 pub type Scalar = f64;
 
 pub type Vector = SVector<Scalar, N>;
 pub type Point = PointBase<Scalar, N>;
 
 pub mod subspace;
+pub mod distance_field;
diff --git a/physics/src/algebra/subspace.rs b/physics/src/algebra/subspace.rs
index 9fb944e..3d36b52 100644
--- a/physics/src/algebra/subspace.rs
+++ b/physics/src/algebra/subspace.rs
@@ -1,6 +1,6 @@
 use nalgebra::SMatrix;
 
-use super::{Scalar, N, Point, Vector};
+use super::{distance_field::DistanceField, Point, Scalar, Vector, N};
 
 
 type ProjectionMatrix = SMatrix<Scalar, N, N>;
@@ -36,7 +36,10 @@ impl<const DIM: usize> Subspace<DIM> {
         self.projection_matrix * (point - self.point.coords) + self.point.coords
     }
 
-    pub fn distance(&self, point: Point) -> Scalar {
+}
+
+impl<const DIM: usize> DistanceField for Subspace<DIM> {
+    fn distance(&self, point: Point) -> Scalar {
         let projected = self.project_point(point);
         (projected - point).norm()
     }
@@ -44,13 +47,13 @@ impl<const DIM: usize> Subspace<DIM> {
 
 #[cfg(test)]
 mod tests {
-    use crate::algebra::{subspace::Line, Point, Vector};
+    use crate::algebra::{subspace::Line, Point, Vector, distance_field::DistanceField};
 
     #[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 line = Line::new(Point::new(1.0, 0.0, 0.0), [Vector::new(3.0, 1.0, 0.0)]);
+        let point = Point::new(3.0, 4.0, 0.0);
+        
         let matrix = line.projection_matrix;
         {
             let diff = matrix * matrix - matrix;
@@ -63,8 +66,21 @@ mod tests {
 
         {
             let projected = line.project_point(point);
-            let diff = projected - Point::new(4.0, 1.0);
+            let diff = projected - Point::new(4.0, 1.0, 0.0);
             assert!(diff.norm() < 0.001, "Point projected incorrectly");
         }
     }
+
+    #[test]
+    fn test_distance() {
+        let origin = Point::new(0.0, 0.0, 0.0);
+        let line = Line::new(origin, [Vector::x()]);
+        
+        let dq = 0.001;
+        let dx = line.distance(origin + Vector::x() * dq);
+        let dy = line.distance(origin + Vector::y() * dq);
+        let dz = line.distance(origin + Vector::z() * dq);
+        assert_eq!(dx, 0.0);
+        assert_eq!(dy, dz);
+    }
 }