feat: add slider constraint

1 files changed, 57 insertions, 0 deletions

diff --git a/src/constraint/slider.rs b/src/constraint/slider.rs
new file mode 100644
index 0000000..526b6f8
--- /dev/null
+++ b/src/constraint/slider.rs
@@ -0,0 +1,57 @@
+use std::usize;
+
+use nalgebra::{DVector, RowDVector};
+
+use crate::{particle_system::{Scalar, N}, ParticleSystem, Point, Vector};
+
+use super::Constraint;
+
+
+pub struct SliderConstraint {
+    pub particle_id: usize,
+
+    pub point: Point,
+
+    /// Has to be normalized
+    pub dir: Vector,
+
+    jacobian: RowDVector<Scalar>,
+}
+
+impl ParticleSystem {
+    pub fn add_slider_constraint(&mut self, particle_id: usize, direction: Vector) {
+        let point = self.particles[particle_id].position;
+        self.constraints.push(Box::new(
+            SliderConstraint {
+                particle_id,
+                point,
+                dir: direction.normalize(),
+                jacobian: RowDVector::zeros(self.particles.len() * N),
+            }
+        ));
+
+    }
+}
+
+impl Constraint for SliderConstraint {
+    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);
+        let d = position - self.point.coords;
+        let lambda = d.dot(&self.dir);
+        let distance_to_line = d - lambda * self.dir;
+
+        distance_to_line.norm()
+    }
+
+    fn set_jacobian(&mut self, jacobian: RowDVector<Scalar>) {
+        self.jacobian = jacobian
+    }
+
+    fn jacobian_prev(&self) -> RowDVector<Scalar> {
+        self.jacobian.clone()
+    }
+}