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
|
use std::usize;
use nalgebra::{DVector, RowDVector};
use crate::{particle_system::{Scalar, N}, ParticleSystem, Point, Vector};
use super::Constraint;
pub struct SliderConstraint {
pub particle_id: usize,
pub point: Point,
/// Has to be normalized
pub dir: Vector,
jacobian: RowDVector<Scalar>,
}
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,
point,
dir: direction.normalize(),
jacobian: RowDVector::zeros(self.particles.len() * N),
}
));
}
}
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 d = position - self.point.coords;
let lambda = d.dot(&self.dir);
let distance_to_line = d - lambda * self.dir;
distance_to_line.norm()
}
fn set_jacobian(&mut self, jacobian: RowDVector<Scalar>) {
self.jacobian = jacobian
}
fn jacobian_prev(&self) -> RowDVector<Scalar> {
self.jacobian.clone()
}
}
|
