1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
|
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<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.00001;
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;
result[index] = (self.c(&a) - self.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);
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"),
}
}
}
|
