From 70afc5a7d871919776a64782e8b93404e6b0defd Mon Sep 17 00:00:00 2001 From: eug-vs Date: Sun, 15 Dec 2024 13:17:43 +0100 Subject: feat!: add raylib rendering --- src/constraint/anchor.rs | 42 -------------- src/constraint/beam.rs | 46 ---------------- src/constraint/mod.rs | 139 ----------------------------------------------- src/constraint/slider.rs | 50 ----------------- 4 files changed, 277 deletions(-) delete mode 100644 src/constraint/anchor.rs delete mode 100644 src/constraint/beam.rs delete mode 100644 src/constraint/mod.rs delete mode 100644 src/constraint/slider.rs (limited to 'src/constraint') 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, -} - -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 { - vec![self.particle_id] - } - - fn c(&self, q: &DVector) -> Scalar { - let position = q.fixed_rows(self.particle_id * N); - (position - self.anchor.coords).norm() - } - - fn set_jacobian(&mut self, jacobian: RowDVector) { - self.jacobian = jacobian - } - - fn jacobian_prev(&self) -> RowDVector { - 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, -} - -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 { - Vec::from(self.particle_ids) - } - - fn c(&self, q: &DVector) -> Scalar { - let a = q.fixed_rows::(self.particle_ids[0] * N); - let b = q.fixed_rows::(self.particle_ids[1] * N); - - (a - b).norm() - self.length - } - - fn set_jacobian(&mut self, jacobian: RowDVector) { - self.jacobian = jacobian - } - - fn jacobian_prev(&self) -> RowDVector { - 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; - - fn c(&self, q: &DVector) -> Scalar; - - fn jacobian_prev(&self) -> RowDVector; - fn set_jacobian(&mut self, jacobian: RowDVector); - - fn partial_derivative(&self, q: &DVector) -> RowDVector { - 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, - q_prev: &DVector, - dt: Scalar, - ) -> (Scalar, Scalar, RowDVector, RowDVector) { - 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::::zeros(self.constraints.len(), size); - let mut jacobian_dot = DMatrix::::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, -} - -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 { - vec![self.particle_id] - } - - fn c(&self, q: &DVector) -> Scalar { - let position = q.fixed_rows::(self.particle_id * N); - let point = Point::from_coordinates(position.into()); - self.line.distance(point) - } - - fn set_jacobian(&mut self, jacobian: RowDVector) { - self.jacobian = jacobian - } - - fn jacobian_prev(&self) -> RowDVector { - self.jacobian.clone() - } -} -- cgit v1.2.3