1 files changed, 50 insertions, 0 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
+    }
+}