1 files changed, 28 insertions, 9 deletions

diff --git a/physics/src/constraint/beam.rs b/physics/src/constraint/beam.rs
index b353e13..f6907c0 100644
--- a/physics/src/constraint/beam.rs
+++ b/physics/src/constraint/beam.rs
@@ -14,8 +14,6 @@ enum BeamPoint {
 pub struct BeamConstraint {
     points: [BeamPoint; 2],
     pub length: Scalar,
-
-    jacobian: RowDVector<Scalar>,
 }
 
 impl ParticleSystem {
@@ -29,7 +27,6 @@ impl ParticleSystem {
                 BeamPoint::FromParticle(particle_ids[1]),
             ],
             length: (a.position - b.position).norm(),
-            jacobian: RowDVector::zeros(self.particles.len() * N),
         }));
     }
 
@@ -42,7 +39,6 @@ impl ParticleSystem {
                 BeamPoint::Static(point),
             ],
             length: (position - point).norm(),
-            jacobian: RowDVector::zeros(self.particles.len() * N),
         }));
     }
 }
@@ -66,11 +62,34 @@ impl Constraint for BeamConstraint {
         (a - b).norm() - self.length
     }
 
-    fn set_jacobian(&mut self, jacobian: RowDVector<Scalar>) {
-        self.jacobian = jacobian
-    }
+    fn jacobian(&self, q: &DVector<Scalar>) -> RowDVector<Scalar> {
+        let [a, b] = self.points.map(|p| {
+            match p {
+                BeamPoint::Static(p) => p.coords,
+                BeamPoint::FromParticle(id) => q.fixed_rows::<N>(id * N).into(),
+            }
+        });
+        let mut result = RowDVector::zeros(q.len());
+        match (a - b).try_normalize(0.0) {
+            Some(normal) => {
+                for i in 0..N {
+                    match self.points[0] {
+                        BeamPoint::FromParticle(id) => {
+                            result[id * N + i] = normal[i];
+                        }
+                        _ => {}
+                    }
+                    match self.points[1] {
+                        BeamPoint::FromParticle(id) => {
+                            result[id * N + i] = -normal[i];
+                        }
+                        _ => {}
+                    }
+                }
+            }
+            None => ()
+        };
 
-    fn jacobian_prev(&self) -> RowDVector<Scalar> {
-        self.jacobian.clone()
+        result
     }
 }