summaryrefslogtreecommitdiff
path: root/src/constraint
diff options
context:
space:
mode:
Diffstat (limited to 'src/constraint')
-rw-r--r--src/constraint/anchor.rs42
-rw-r--r--src/constraint/beam.rs46
-rw-r--r--src/constraint/mod.rs139
-rw-r--r--src/constraint/slider.rs50
4 files changed, 0 insertions, 277 deletions
diff --git a/src/constraint/anchor.rs b/src/constraint/anchor.rs
deleted file mode 100644
index 3ef94ff..0000000
--- a/src/constraint/anchor.rs
+++ /dev/null
@@ -1,42 +0,0 @@
-use nalgebra::{DVector, RowDVector};
-
-use crate::particle_system::{ParticleSystem, 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/src/constraint/beam.rs b/src/constraint/beam.rs
deleted file mode 100644
index c953920..0000000
--- a/src/constraint/beam.rs
+++ /dev/null
@@ -1,46 +0,0 @@
-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()
- }
-}
diff --git a/src/constraint/mod.rs b/src/constraint/mod.rs
deleted file mode 100644
index 6028b42..0000000
--- a/src/constraint/mod.rs
+++ /dev/null
@@ -1,139 +0,0 @@
-use nalgebra::{DMatrix, DVector, RowDVector};
-
-use crate::particle_system::{ParticleSystem, Scalar, Vector, N};
-pub mod anchor;
-pub mod slider;
-pub mod beam;
-
-const SPRING_CONSTANT: Scalar = 0.75;
-const DAMPING_CONSTANT: Scalar = 0.45;
-
-/// SIZE is always 3xN, but this operation can't be done at compile time yet:
-/// "generic parameters may not be used in const operations"
-pub trait Constraint {
- fn get_particles(&self) -> Vec<usize>;
-
- fn c(&self, q: &DVector<Scalar>) -> Scalar;
-
- fn jacobian_prev(&self) -> RowDVector<Scalar>;
- fn set_jacobian(&mut self, jacobian: RowDVector<Scalar>);
-
- fn partial_derivative(&self, q: &DVector<Scalar>) -> RowDVector<Scalar> {
- let dq = 0.001;
- let c_original = self.c(&q);
- let mut result = RowDVector::zeros(q.len());
- // The only non-zero components of derivative vector are
- // the particles that this constraint affects
- for particle_id in self.get_particles() {
- for i in 0..N {
- let index = particle_id * N + i;
- let mut q_partial = q.clone();
- q_partial[index] += dq;
-
- let c = self.c(&q_partial);
-
- result[index] = (c - c_original) / dq
- }
- }
-
- result
- }
-
- fn compute(
- &mut self,
- q: &DVector<Scalar>,
- q_prev: &DVector<Scalar>,
- dt: Scalar,
- ) -> (Scalar, Scalar, RowDVector<Scalar>, RowDVector<Scalar>) {
- let c = self.c(q);
- let c_prev = self.c(q_prev);
-
- let c_dot = (c - c_prev) / dt;
-
- let jacobian = self.partial_derivative(&q);
-
- let jacobian_dot = (jacobian.clone() - self.jacobian_prev()) / dt;
- self.set_jacobian(jacobian.clone());
-
- (c, c_dot, jacobian, jacobian_dot)
- }
-}
-
-impl ParticleSystem {
- pub fn enforce_constraints(&mut self, dt: Scalar) {
- if self.constraints.len() == 0 {
- return
- }
-
- let size = self.particles.len() * N;
- let mut q = DVector::zeros(size);
- let mut q_prev = DVector::zeros(size);
-
- for (p, particle) in self.particles.iter().enumerate() {
- for i in 0..N {
- q[p * N + i] = particle.position[i];
- q_prev[p * N + i] = particle.position[i] - particle.velocity[i] * dt;
- }
- }
-
- let q_dot = (q.clone() - q_prev.clone()) / dt;
-
- let mut c = DVector::zeros(self.constraints.len());
- let mut c_dot = DVector::zeros(self.constraints.len());
- let mut jacobian = DMatrix::<Scalar>::zeros(self.constraints.len(), size);
- let mut jacobian_dot = DMatrix::<Scalar>::zeros(self.constraints.len(), size);
-
- for (constraint_id, constraint) in self.constraints.iter_mut().enumerate() {
- let (value, derivative, j, jdot) = constraint.compute(&q, &q_prev, dt);
- jacobian.set_row(constraint_id, &j);
- jacobian_dot.set_row(constraint_id, &jdot);
- c[constraint_id] = value;
- c_dot[constraint_id] = derivative;
- }
-
- // println!("C {}\nC' {}", c, c_dot);
- // println!("J = {}", jacobian.clone());
- // println!("J' = {}", jacobian_dot.clone());
-
- let mut w = DMatrix::zeros(size, size);
- for (i, particle) in self.particles.iter().enumerate() {
- for j in 0..N {
- *w.index_mut((i * N + j, i * N + j)) = 1.0 / particle.mass;
- }
- }
-
- let mut forces = DVector::zeros(size);
- for (p, particle) in self.particles.iter().enumerate() {
- for i in 0..N {
- forces[p * N + i] = particle.force[i];
- }
- }
-
- let lhs = jacobian.clone() * w.clone() * jacobian.transpose();
- // println!("LHS {}", lhs);
-
- let rhs = -(jacobian_dot * q_dot
- + jacobian.clone() * w * forces
- + SPRING_CONSTANT * c
- + DAMPING_CONSTANT * c_dot);
-
- // println!("RHS {}", rhs);
-
- match lhs.lu().solve(&rhs) {
- Some(lambda) => {
- // println!("Lambda = {}", lambda);
- let constraint_force = jacobian.transpose() * lambda;
-
- for i in 0..self.particles.len() {
- let mut force = Vector::zeros();
- for j in 0..N {
- force[j] = constraint_force[i * N + j];
- }
-
- self.particles[i].apply_force(force);
- }
- }
- None => println!("Lambda not found"),
- }
- }
-}
diff --git a/src/constraint/slider.rs b/src/constraint/slider.rs
deleted file mode 100644
index 9f7973b..0000000
--- a/src/constraint/slider.rs
+++ /dev/null
@@ -1,50 +0,0 @@
-use std::usize;
-
-use nalgebra::{DVector, RowDVector};
-
-use crate::{
- algebra::subspace::Line,
- particle_system::{Scalar, N},
- ParticleSystem, Point, Vector,
-};
-
-use super::Constraint;
-
-pub struct SliderConstraint {
- pub particle_id: usize,
-
- pub line: Line,
-
- jacobian: RowDVector<Scalar>,
-}
-
-impl ParticleSystem {
- pub fn add_slider_constraint(&mut self, particle_id: usize, direction: Vector) {
- let point = self.particles[particle_id].position;
- self.constraints.push(Box::new(SliderConstraint {
- particle_id,
- line: Line::new(point, [direction]),
- jacobian: RowDVector::zeros(self.particles.len() * N),
- }));
- }
-}
-
-impl Constraint for SliderConstraint {
- 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);
- let point = Point::from_coordinates(position.into());
- self.line.distance(point)
- }
-
- fn set_jacobian(&mut self, jacobian: RowDVector<Scalar>) {
- self.jacobian = jacobian
- }
-
- fn jacobian_prev(&self) -> RowDVector<Scalar> {
- self.jacobian.clone()
- }
-}
generated by cgit v1.2.3 (git 2.25.1) at 2025-05-10 15:14:29 +0000