tmp: add most recent progressexperiments

8 files changed, 100 insertions, 19 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);
+    }
 }
diff --git a/physics/src/constraint/distance.rs b/physics/src/constraint/distance.rs
new file mode 100644
index 0000000..abdbdeb
--- /dev/null
+++ b/physics/src/constraint/distance.rs
@@ -0,0 +1,50 @@
+use nalgebra::{DVector, RowDVector};
+
+use crate::algebra::distance_field::DistanceField;
+use crate::algebra::subspace::{Line, Subspace};
+use crate::algebra::{Point, Scalar, N};
+use crate::particle_system::ParticleSystem;
+
+use super::Constraint;
+
+/// Keep a particle at certain distance within distance field
+pub struct DistanceConstraint<D: DistanceField> {
+    particle_id: usize,
+    distance_field: D,
+    distance: Scalar,
+}
+
+impl ParticleSystem {
+    pub fn add_distance_constraint<const DIM: usize>(&mut self, particle_id: usize, subspace: Subspace<DIM>, distance: Scalar) {
+        let particle = &self.particles[particle_id];
+
+        self.constraints.push(Box::new(DistanceConstraint {
+            particle_id,
+            distance,
+            distance_field: subspace,
+        }));
+    }
+}
+
+impl<D: DistanceField> Constraint for DistanceConstraint<D> {
+    fn get_particles(&self) -> Vec<usize> {
+        vec![self.particle_id]
+    }
+
+    fn c(&self, q: &DVector<Scalar>) -> Scalar {
+        let position = q.fixed_rows::<N>(self.particle_id * N);
+
+        self.distance_field.distance(Point::from_coordinates(position.into())) - self.distance
+    }
+
+    fn jacobian(&self, q: &DVector<Scalar>) -> RowDVector<Scalar> {
+        let point = Point::from_coordinates(q.fixed_rows::<N>(self.particle_id * N).into());
+        let normal = self.distance_field.project_point(point) - point;
+
+        let mut result = RowDVector::zeros(q.len());
+        for i in 0..N {
+            result[self.particle_id * N + i] = normal[i];
+        }
+        result
+    }
+}
diff --git a/physics/src/constraint/mod.rs b/physics/src/constraint/mod.rs
index ed9e603..3f76d33 100644
--- a/physics/src/constraint/mod.rs
+++ b/physics/src/constraint/mod.rs
@@ -4,6 +4,7 @@ use crate::algebra::{Scalar, N};
 use crate::particle_system::ParticleSystem;
 pub mod beam;
 pub mod slider;
+// pub mod distance;
 
 pub const SPRING_CONSTANT: Scalar = 16.0;
 pub const DAMPING_CONSTANT: Scalar = 4.0;
diff --git a/physics/src/constraint/slider.rs b/physics/src/constraint/slider.rs
index be8e261..a50a840 100644
--- a/physics/src/constraint/slider.rs
+++ b/physics/src/constraint/slider.rs
@@ -1,7 +1,7 @@
 use nalgebra::{DVector, RowDVector};
 
 use crate::{
-    algebra::{subspace::Line, Point, Scalar, Vector, N},
+    algebra::{distance_field::DistanceField, subspace::Line, Point, Scalar, Vector, N},
     particle_system::ParticleSystem,
 };
 
diff --git a/physics/src/renderer/mod.rs b/physics/src/renderer/mod.rs
index 63894b3..72b922d 100644
--- a/physics/src/renderer/mod.rs
+++ b/physics/src/renderer/mod.rs
@@ -45,21 +45,23 @@ impl Camera {
 
 #[cfg(test)]
 mod tests {
-    use crate::algebra::{Point, Vector};
+    use nalgebra::Point2;
+
+    use crate::algebra::{Point, Scalar, Vector};
 
     use super::Camera;
 
     #[test]
     fn test_projection() {
         let camera = Camera::new(
-            Point::new(1.0, 0.0),
-            Vector::new(1.0, 2.0),
-            Vector::new(2.0, -1.0),
+            Point::new(1.0, 0.0, 0.0),
+            Vector::new(1.0, 2.0, 0.0),
+            Vector::new(2.0, -1.0, 0.0),
         );
 
-        let point = Point::new(3.0, 1.0);
+        let point = Point::new(3.0, 1.0, 0.0);
 
-        let diff = camera.world_to_screen_space * point - Point::new(1.0, 1.0);
+        let diff = camera.world_to_screen_space * point - Point2::<Scalar>::new(1.0, 1.0);
         assert!(
             diff.norm() < 0.001,
             "Camera translated point into screen_space incorrectly"
diff --git a/physics/src/solver/mod.rs b/physics/src/solver/mod.rs
index 527bb06..50fe9a7 100644
--- a/physics/src/solver/mod.rs
+++ b/physics/src/solver/mod.rs
@@ -84,7 +84,7 @@ mod tests {
     #[test]
     fn test_collect_phase_space() {
         let system = ParticleSystem {
-            particles: vec![Particle::new(Point::new(2.0, 3.0), 1.0)],
+            particles: vec![Particle::new(Point::new(2.0, 3.0, 0.0), 1.0)],
             constraints: vec![],
             forces: vec![],
             t: 0.0,
@@ -100,12 +100,12 @@ mod tests {
     #[test]
     fn test_scatter_phase_space() {
         let mut phase_space = PhaseSpace::new(2);
-        phase_space.set_particle(1, Point::new(5.0, 7.0), Vector::x());
+        phase_space.set_particle(1, Point::new(5.0, 7.0, 0.0), Vector::x());
 
         let mut system = ParticleSystem {
             particles: vec![
-                Particle::new(Point::new(0.0, 0.0), 1.0),
-                Particle::new(Point::new(0.0, 0.0), 1.0),
+                Particle::new(Point::new(0.0, 0.0, 0.0), 1.0),
+                Particle::new(Point::new(0.0, 0.0, 0.0), 1.0),
             ],
             constraints: vec![],
             forces: vec![],