3 files changed, 52 insertions, 1 deletions
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,
 };