From 783070635b568b44b6902bfdc01bdadf12b86bc8 Mon Sep 17 00:00:00 2001
From: eug-vs <eugene@eug-vs.xyz>
Date: Thu, 12 Dec 2024 15:23:01 +0100
Subject: feat: implement constraint system and PPM rendering

---
 src/constraint/beam.rs | 46 ++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 46 insertions(+)
 create mode 100644 src/constraint/beam.rs

(limited to 'src/constraint/beam.rs')

diff --git a/src/constraint/beam.rs b/src/constraint/beam.rs
new file mode 100644
index 0000000..c953920
--- /dev/null
+++ b/src/constraint/beam.rs
@@ -0,0 +1,46 @@
+use nalgebra::{DVector, RowDVector};
+
+use crate::particle_system::{ParticleSystem, Scalar, N};
+
+use super::Constraint;
+
+pub struct BeamConstraint {
+    pub particle_ids: [usize; 2],
+    pub length: Scalar,
+
+    jacobian: RowDVector<Scalar>,
+}
+
+impl ParticleSystem {
+    pub fn add_beam_constraint(&mut self, particle_ids: [usize; 2]) {
+        let a = &self.particles[particle_ids[0]];
+        let b = &self.particles[particle_ids[1]];
+
+        self.constraints.push(Box::new(BeamConstraint {
+            particle_ids,
+            length: (a.position - b.position).norm(),
+            jacobian: RowDVector::zeros(self.particles.len() * N),
+        }));
+    }
+}
+
+impl Constraint for BeamConstraint {
+    fn get_particles(&self) -> Vec<usize> {
+        Vec::from(self.particle_ids)
+    }
+
+    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);
+
+        (a - b).norm() - self.length
+    }
+
+    fn set_jacobian(&mut self, jacobian: RowDVector<Scalar>) {
+        self.jacobian = jacobian
+    }
+
+    fn jacobian_prev(&self) -> RowDVector<Scalar> {
+        self.jacobian.clone()
+    }
+}
-- 
cgit v1.2.3