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 --- physics/src/constraint/anchor.rs | 43 ++++++++++++ physics/src/constraint/beam.rs | 47 +++++++++++++ physics/src/constraint/mod.rs | 140 +++++++++++++++++++++++++++++++++++++++ physics/src/constraint/slider.rs | 47 +++++++++++++ 4 files changed, 277 insertions(+) create mode 100644 physics/src/constraint/anchor.rs create mode 100644 physics/src/constraint/beam.rs create mode 100644 physics/src/constraint/mod.rs create mode 100644 physics/src/constraint/slider.rs (limited to 'physics/src/constraint') diff --git a/physics/src/constraint/anchor.rs b/physics/src/constraint/anchor.rs new file mode 100644 index 0000000..c90cfb2 --- /dev/null +++ b/physics/src/constraint/anchor.rs @@ -0,0 +1,43 @@ +use nalgebra::{DVector, RowDVector}; + +use crate::particle_system::ParticleSystem; +use crate::algebra::{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/physics/src/constraint/beam.rs b/physics/src/constraint/beam.rs new file mode 100644 index 0000000..14b1c1f --- /dev/null +++ b/physics/src/constraint/beam.rs @@ -0,0 +1,47 @@ +use nalgebra::{DVector, RowDVector}; + +use crate::particle_system::ParticleSystem; +use crate::algebra::{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/physics/src/constraint/mod.rs b/physics/src/constraint/mod.rs new file mode 100644 index 0000000..b2958ec --- /dev/null +++ b/physics/src/constraint/mod.rs @@ -0,0 +1,140 @@ +use nalgebra::{DMatrix, DVector, RowDVector}; + +use crate::particle_system::ParticleSystem; +use crate::algebra::{Scalar, Vector, N}; +pub mod anchor; +pub mod slider; +pub mod beam; + +const SPRING_CONSTANT: Scalar = 0.65; +const DAMPING_CONSTANT: Scalar = 0.85; + +/// 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/physics/src/constraint/slider.rs b/physics/src/constraint/slider.rs new file mode 100644 index 0000000..fa7e840 --- /dev/null +++ b/physics/src/constraint/slider.rs @@ -0,0 +1,47 @@ +use nalgebra::{DVector, RowDVector}; + +use crate::{ + algebra::{subspace::Line, Point, Scalar, Vector, N}, + particle_system::ParticleSystem, +}; + +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