diff options
Diffstat (limited to 'physics/src/constraint/mod.rs')
| -rw-r--r-- | physics/src/constraint/mod.rs | 198 |
1 files changed, 114 insertions, 84 deletions
diff --git a/physics/src/constraint/mod.rs b/physics/src/constraint/mod.rs index 15ca986..4df1c8e 100644 --- a/physics/src/constraint/mod.rs +++ b/physics/src/constraint/mod.rs @@ -1,135 +1,165 @@ use nalgebra::{DMatrix, DVector, RowDVector}; +use crate::algebra::{Scalar, N}; use crate::particle_system::ParticleSystem; -use crate::algebra::{Scalar, Vector, N}; -pub mod slider; pub mod beam; +pub mod slider; +pub mod subspace_distance; -const SPRING_CONSTANT: Scalar = 0.65; -const DAMPING_CONSTANT: Scalar = 0.85; +pub const SPRING_CONSTANT: Scalar = 16.0; +pub const DAMPING_CONSTANT: Scalar = 4.0; -/// 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>; + /// Constraint function: C = 0 <=> constrained satisfied 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.00001; + /// J = dC / dq + /// This implementation computes partial derivative via + /// finite differences method: stepping +-dQ along each axis + /// + /// NOTE: this is rather computationally intensive so + /// providing constraint-specific analytical implementation is preferred + fn jacobian(&self, q: &DVector<Scalar>) -> RowDVector<Scalar> { + let dq = 1e-10; 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 a = q.clone(); - let mut b = q.clone(); - a[index] += dq; - b[index] -= dq; + let mut q = q.clone(); + + q[index] += dq; + let c_a = self.c(&q); + + q[index] -= 2.0 * dq; + let c_b = self.c(&q); - result[index] = (self.c(&a) - self.c(&b)) / (2.0 * dq) + result[index] = (c_a - c_b) / (2.0 * 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); + /// C' = J * q' + fn c_dot(&self, jacobian: &RowDVector<Scalar>, q_dot: &DVector<Scalar>) -> Scalar { + (jacobian * q_dot)[0] + } - let c_dot = (c - c_prev) / dt; + /// J' = dC' / dq + fn jacobian_dot(&self, q: &DVector<Scalar>, q_dot: &DVector<Scalar>) -> RowDVector<Scalar> { + let dq = 1e-4; + 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 = q.clone(); - let jacobian = self.partial_derivative(&q); + q[index] += dq; + let j_a = self.jacobian(&q); - let jacobian_dot = (jacobian.clone() - self.jacobian_prev()) / dt; - self.set_jacobian(jacobian.clone()); + q[index] -= 2.0 * dq; + let j_b = self.jacobian(&q); - (c, c_dot, jacobian, jacobian_dot) + result[index] = (self.c_dot(&j_a, q_dot) - self.c_dot(&j_b, q_dot)) / (2.0 * dq) + } + } + result } } impl ParticleSystem { - pub fn enforce_constraints(&mut self, dt: Scalar) { - if self.constraints.len() == 0 { - return - } - - let size = self.particles.len() * N; + /// q is a state vector - concatenated positions of particles + /// q' - its derivative - concatenated velocities + pub fn collect_q(&self) -> (DVector<Scalar>, DVector<Scalar>) { + let size = self.q_dim(); let mut q = DVector::zeros(size); - let mut q_prev = DVector::zeros(size); + let mut q_dot = 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; + q_dot[p * N + i] = particle.velocity[i]; } } - 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()); + (q, q_dot) + } + /// dim(q) = #particles * N + fn q_dim(&self) -> usize { + self.particles.len() * N + } - 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; - } - } + /// Diagonal matrix of particle masses (each repeated N times to match dim(q)) + fn mass_matrix(&self) -> DMatrix<Scalar> { + DMatrix::from_diagonal(&DVector::from_iterator( + self.q_dim(), + self.particles + .iter() + .flat_map(|particle| (0..N).map(|_i| 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]; - } + pub fn enforce_constraints(&mut self) { + if self.constraints.len() == 0 { + return; } - 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); + let (q, q_dot) = self.collect_q(); + let dim_q = self.q_dim(); + let dim_c = self.constraints.len(); + + let c = DVector::from_iterator( + dim_c, + self.constraints.iter().map(|constraint| constraint.c(&q)), + ); + + let jacobian = DMatrix::from_rows( + &self + .constraints + .iter() + .map(|constraint| constraint.jacobian(&q)) + .collect::<Vec<_>>(), + ); + + let jacobian_dot = DMatrix::from_rows( + &self + .constraints + .iter() + .map(|constraint| constraint.jacobian_dot(&q, &q_dot)) + .collect::<Vec<_>>(), + ); + + let c_dot = &jacobian * &q_dot; + + let forces = DVector::from_iterator( + dim_q, + self.particles + .iter() + .flat_map(|particle| particle.force.iter().map(|&v| v)), + ); + + let jacobian_times_w = &jacobian * self.mass_matrix(); + let jacobian_transpose = &jacobian.transpose(); + + let lhs = &jacobian_times_w * jacobian_transpose; + + let rhs = -(&jacobian_dot * &q_dot + + jacobian_times_w * &forces + + SPRING_CONSTANT * &c + + DAMPING_CONSTANT * &c_dot); match lhs.lu().solve(&rhs) { Some(lambda) => { - // println!("Lambda = {}", lambda); - let constraint_force = jacobian.transpose() * 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); + let force = constraint_force.fixed_rows::<N>(i * N); + self.particles[i].apply_force(force.into()); } } None => println!("Lambda not found"), |