refactor: generalize anchor constraint via beam

1 files changed, 35 insertions, 6 deletions

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
     }