refactor: generalize anchor constraint via beam

3 files changed, 35 insertions, 50 deletions

diff --git a/physics/src/constraint/anchor.rs b/physics/src/constraint/anchor.rs
deleted file mode 100644
index c90cfb2..0000000
--- a/physics/src/constraint/anchor.rs
+++ /dev/null
@@ -1,43 +0,0 @@
-use nalgebra::{DVector, RowDVector};
-
-use crate::particle_system::ParticleSystem;
-use crate::algebra::{Point, Scalar, N};
-
-use super::Constraint;
-
-pub struct AnchorConstraint {
-    pub particle_id: usize,
-    pub anchor: Point,
-
-    jacobian: RowDVector<Scalar>,
-}
-
-impl ParticleSystem {
-    pub fn add_anchor_constraint(&mut self, particle_id: usize) {
-        let anchor = self.particles[particle_id].position;
-        self.constraints.push(Box::new(AnchorConstraint {
-            particle_id,
-            anchor,
-            jacobian: RowDVector::zeros(self.particles.len() * N),
-        }));
-    }
-}
-
-impl Constraint for AnchorConstraint {
-    fn get_particles(&self) -> Vec<usize> {
-        vec![self.particle_id]
-    }
-
-    fn c(&self, q: &DVector<Scalar>) -> Scalar {
-        let position = q.fixed_rows(self.particle_id * N);
-        (position - self.anchor.coords).norm()
-    }
-
-    fn set_jacobian(&mut self, jacobian: RowDVector<Scalar>) {
-        self.jacobian = jacobian
-    }
-
-    fn jacobian_prev(&self) -> RowDVector<Scalar> {
-        self.jacobian.clone()
-    }
-}
diff --git a/physics/src/constraint/beam.rs b/physics/src/constraint/beam.rs
index 14b1c1f..b353e13 100644
--- a/physics/src/constraint/beam.rs
+++ b/physics/src/constraint/beam.rs
@@ -1,12 +1,18 @@
 use nalgebra::{DVector, RowDVector};
 
+use crate::algebra::{Point, Scalar, N};
 use crate::particle_system::ParticleSystem;
-use crate::algebra::{Scalar, N};
 
 use super::Constraint;
 
+#[derive(Clone, Copy)]
+enum BeamPoint {
+    Static(Point),
+    FromParticle(usize),
+}
+
 pub struct BeamConstraint {
-    pub particle_ids: [usize; 2],
+    points: [BeamPoint; 2],
     pub length: Scalar,
 
     jacobian: RowDVector<Scalar>,
@@ -18,21 +24,44 @@ impl ParticleSystem {
         let b = &self.particles[particle_ids[1]];
 
         self.constraints.push(Box::new(BeamConstraint {
-            particle_ids,
+            points: [
+                BeamPoint::FromParticle(particle_ids[0]),
+                BeamPoint::FromParticle(particle_ids[1]),
+            ],
             length: (a.position - b.position).norm(),
             jacobian: RowDVector::zeros(self.particles.len() * N),
         }));
     }
+
+    pub fn add_anchor_constraint(&mut self, particle_id: usize, point: Point) {
+        let position = &self.particles[particle_id].position;
+
+        self.constraints.push(Box::new(BeamConstraint {
+            points: [
+                BeamPoint::FromParticle(particle_id),
+                BeamPoint::Static(point),
+            ],
+            length: (position - point).norm(),
+            jacobian: RowDVector::zeros(self.particles.len() * N),
+        }));
+    }
 }
 
 impl Constraint for BeamConstraint {
     fn get_particles(&self) -> Vec<usize> {
-        Vec::from(self.particle_ids)
+        self.points.iter().filter_map(|p| match p {
+            BeamPoint::FromParticle(id) => Some(*id),
+            BeamPoint::Static(_) => None,
+        }).collect()
     }
 
     fn c(&self, q: &DVector<Scalar>) -> Scalar {
-        let a = q.fixed_rows::<N>(self.particle_ids[0] * N);
-        let b = q.fixed_rows::<N>(self.particle_ids[1] * N);
+        let [a, b] = self.points.map(|p| {
+            match p {
+                BeamPoint::Static(p) => p.coords,
+                BeamPoint::FromParticle(id) => q.fixed_rows::<N>(id * N).into(),
+            }
+        });
 
         (a - b).norm() - self.length
     }
diff --git a/physics/src/constraint/mod.rs b/physics/src/constraint/mod.rs
index 5c5034c..15ca986 100644
--- a/physics/src/constraint/mod.rs
+++ b/physics/src/constraint/mod.rs
@@ -2,7 +2,6 @@ use nalgebra::{DMatrix, DVector, RowDVector};
 
 use crate::particle_system::ParticleSystem;
 use crate::algebra::{Scalar, Vector, N};
-pub mod anchor;
 pub mod slider;
 pub mod beam;