From 11031f246a8ec47eb0ffca285138220eb717415e Mon Sep 17 00:00:00 2001
From: eug-vs <eugene@eug-vs.xyz>
Date: Fri, 31 Jan 2025 03:35:28 +0100
Subject: tmp: add most recent progress

---
 physics/src/constraint/distance.rs | 50 ++++++++++++++++++++++++++++++++++++++
 physics/src/constraint/mod.rs      |  1 +
 physics/src/constraint/slider.rs   |  2 +-
 3 files changed, 52 insertions(+), 1 deletion(-)
 create mode 100644 physics/src/constraint/distance.rs

(limited to 'physics/src/constraint')

diff --git a/physics/src/constraint/distance.rs b/physics/src/constraint/distance.rs
new file mode 100644
index 0000000..abdbdeb
--- /dev/null
+++ b/physics/src/constraint/distance.rs
@@ -0,0 +1,50 @@
+use nalgebra::{DVector, RowDVector};
+
+use crate::algebra::distance_field::DistanceField;
+use crate::algebra::subspace::{Line, Subspace};
+use crate::algebra::{Point, Scalar, N};
+use crate::particle_system::ParticleSystem;
+
+use super::Constraint;
+
+/// Keep a particle at certain distance within distance field
+pub struct DistanceConstraint<D: DistanceField> {
+    particle_id: usize,
+    distance_field: D,
+    distance: Scalar,
+}
+
+impl ParticleSystem {
+    pub fn add_distance_constraint<const DIM: usize>(&mut self, particle_id: usize, subspace: Subspace<DIM>, distance: Scalar) {
+        let particle = &self.particles[particle_id];
+
+        self.constraints.push(Box::new(DistanceConstraint {
+            particle_id,
+            distance,
+            distance_field: subspace,
+        }));
+    }
+}
+
+impl<D: DistanceField> Constraint for DistanceConstraint<D> {
+    fn get_particles(&self) -> Vec<usize> {
+        vec![self.particle_id]
+    }
+
+    fn c(&self, q: &DVector<Scalar>) -> Scalar {
+        let position = q.fixed_rows::<N>(self.particle_id * N);
+
+        self.distance_field.distance(Point::from_coordinates(position.into())) - self.distance
+    }
+
+    fn jacobian(&self, q: &DVector<Scalar>) -> RowDVector<Scalar> {
+        let point = Point::from_coordinates(q.fixed_rows::<N>(self.particle_id * N).into());
+        let normal = self.distance_field.project_point(point) - point;
+
+        let mut result = RowDVector::zeros(q.len());
+        for i in 0..N {
+            result[self.particle_id * N + i] = normal[i];
+        }
+        result
+    }
+}
diff --git a/physics/src/constraint/mod.rs b/physics/src/constraint/mod.rs
index ed9e603..3f76d33 100644
--- a/physics/src/constraint/mod.rs
+++ b/physics/src/constraint/mod.rs
@@ -4,6 +4,7 @@ use crate::algebra::{Scalar, N};
 use crate::particle_system::ParticleSystem;
 pub mod beam;
 pub mod slider;
+// pub mod distance;
 
 pub const SPRING_CONSTANT: Scalar = 16.0;
 pub const DAMPING_CONSTANT: Scalar = 4.0;
diff --git a/physics/src/constraint/slider.rs b/physics/src/constraint/slider.rs
index be8e261..a50a840 100644
--- a/physics/src/constraint/slider.rs
+++ b/physics/src/constraint/slider.rs
@@ -1,7 +1,7 @@
 use nalgebra::{DVector, RowDVector};
 
 use crate::{
-    algebra::{subspace::Line, Point, Scalar, Vector, N},
+    algebra::{distance_field::DistanceField, subspace::Line, Point, Scalar, Vector, N},
     particle_system::ParticleSystem,
 };
 
-- 
cgit v1.2.3