1 files changed, 75 insertions, 8 deletions

diff --git a/physics/src/constraint/slider.rs b/physics/src/constraint/slider.rs
index fa7e840..be8e261 100644
--- a/physics/src/constraint/slider.rs
+++ b/physics/src/constraint/slider.rs
@@ -9,10 +9,7 @@ use super::Constraint;
 
 pub struct SliderConstraint {
     pub particle_id: usize,
-
     pub line: Line,
-
-    jacobian: RowDVector<Scalar>,
 }
 
 impl ParticleSystem {
@@ -21,7 +18,6 @@ impl ParticleSystem {
         self.constraints.push(Box::new(SliderConstraint {
             particle_id,
             line: Line::new(point, [direction]),
-            jacobian: RowDVector::zeros(self.particles.len() * N),
         }));
     }
 }
@@ -37,11 +33,82 @@ impl Constraint for SliderConstraint {
         self.line.distance(point)
     }
 
-    fn set_jacobian(&mut self, jacobian: RowDVector<Scalar>) {
-        self.jacobian = jacobian
+    fn jacobian(&self, q: &DVector<Scalar>) -> RowDVector<Scalar> {
+        let position = q.fixed_rows::<N>(self.particle_id * N);
+        let point = Point::from_coordinates(position.into());
+        let mut result = RowDVector::zeros(q.len());
+
+        match (point - self.line.project_point(point)).try_normalize(0.0) {
+            Some(normal) => {
+                for i in 0..N {
+                    result[self.particle_id * N + i] = normal[i]
+                }
+            }
+            None => ()
+        };
+
+
+        result
     }
+}
+
+#[cfg(test)]
+mod tests {
+    use std::{fs::File, io::Write};
+
+    use crate::{algebra::{Point, Vector}, constraint::{DAMPING_CONSTANT, SPRING_CONSTANT}, force::gravity::Gravity, particle_system::{Particle, ParticleSystem}, solver::Solver};
+
+    #[test]
+    fn test_drift_resiliance() {
+        let origin = Point::origin();
+        let mut system = ParticleSystem {
+            particles: vec![Particle::new(origin, 1.0)],
+            constraints: vec![],
+            forces: vec![Box::new(Gravity {
+                vector: Vector::new(-5.0, -1.0, 20.0),
+
+            })],
+            t: 0.0,
+        };
+        system.add_slider_constraint(0, Vector::x());
+
+        let mut errors = vec![];
+        let mut positions = vec![];
+
+        let sim_time = 10.0;
+
+        let dt = 1e-5;
+        for i in 0..(sim_time / dt) as usize {
+            system.apply_forces();
+            system.enforce_constraints();
+            system.step(dt);
+
+            let (q, _) = system.collect_q();
+            let error = system.constraints[0].c(&q);
+            errors.push(error);
+            positions.push(system.particles[0].position);
+            // println!("Iteration {}, error = {}", i, error);
+        }
+
+        let error_max = errors.clone().into_iter().reduce(f64::max).unwrap();
+
+        let mut error_sorted = errors.clone();
+        error_sorted.sort_by(f64::total_cmp);
+        let mean = error_sorted[errors.len() / 2];
 
-    fn jacobian_prev(&self) -> RowDVector<Scalar> {
-        self.jacobian.clone()
+        {
+            let filename = format!("./plots/constraint/errors-s{}-d{}.txt", SPRING_CONSTANT, DAMPING_CONSTANT);
+            let mut f = File::create(filename).unwrap();
+            let s = errors.iter().fold(String::new(), |acc, e| acc + &e.to_string() + "\n");
+            f.write(s.as_bytes()).unwrap();
+        }
+        {
+            let filename = format!("./plots/slider/pos-s{}-d{}.txt", SPRING_CONSTANT, DAMPING_CONSTANT);
+            let mut f = File::create(filename).unwrap();
+            let s = positions.iter().fold(String::new(), |acc, e| acc + &e.y.to_string() + " " + &e.z.to_string() + "\n");
+            f.write(s.as_bytes()).unwrap();
+        }
+        // assert!(error < error_max, "Error must decrease over time");
+        assert!(error_max < 1.0e-5, "Error too high: MAX = {}, MEAN = {}", error_max, mean);
     }
 }