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
|
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,
}
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]),
}));
}
}
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 jacobian(&self, q: &DVector<Scalar>) -> RowDVector<Scalar> {
let position = q.fixed_rows::<N>(self.particle_id * N);
let point = Point::from_coordinates(position.into());
let mut result = RowDVector::zeros(q.len());
match (point - self.line.project_point(point)).try_normalize(0.0) {
Some(normal) => {
for i in 0..N {
result[self.particle_id * N + i] = normal[i]
}
}
None => ()
};
result
}
}
#[cfg(test)]
mod tests {
use std::{fs::File, io::Write};
use crate::{algebra::{Point, Vector}, constraint::{DAMPING_CONSTANT, SPRING_CONSTANT}, force::gravity::Gravity, particle_system::{Particle, ParticleSystem}, solver::Solver};
#[test]
fn test_drift_resiliance() {
let origin = Point::origin();
let mut system = ParticleSystem {
particles: vec![Particle::new(origin, 1.0)],
constraints: vec![],
forces: vec![Box::new(Gravity {
vector: Vector::new(-5.0, -1.0, 20.0),
})],
t: 0.0,
};
system.add_slider_constraint(0, Vector::x());
let mut errors = vec![];
let mut positions = vec![];
let sim_time = 10.0;
let dt = 1e-5;
for i in 0..(sim_time / dt) as usize {
system.apply_forces();
system.enforce_constraints();
system.step(dt);
let (q, _) = system.collect_q();
let error = system.constraints[0].c(&q);
errors.push(error);
positions.push(system.particles[0].position);
// println!("Iteration {}, error = {}", i, error);
}
let error_max = errors.clone().into_iter().reduce(f64::max).unwrap();
let mut error_sorted = errors.clone();
error_sorted.sort_by(f64::total_cmp);
let mean = error_sorted[errors.len() / 2];
{
let filename = format!("./plots/constraint/errors-s{}-d{}.txt", SPRING_CONSTANT, DAMPING_CONSTANT);
let mut f = File::create(filename).unwrap();
let s = errors.iter().fold(String::new(), |acc, e| acc + &e.to_string() + "\n");
f.write(s.as_bytes()).unwrap();
}
{
let filename = format!("./plots/slider/pos-s{}-d{}.txt", SPRING_CONSTANT, DAMPING_CONSTANT);
let mut f = File::create(filename).unwrap();
let s = positions.iter().fold(String::new(), |acc, e| acc + &e.y.to_string() + " " + &e.z.to_string() + "\n");
f.write(s.as_bytes()).unwrap();
}
// assert!(error < error_max, "Error must decrease over time");
assert!(error_max < 1.0e-5, "Error too high: MAX = {}, MEAN = {}", error_max, mean);
}
}
|
