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
|
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<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,
line: Line::new(point, [direction]),
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 point = Point::from_coordinates(position.into());
self.line.distance(point)
}
fn set_jacobian(&mut self, jacobian: RowDVector<Scalar>) {
self.jacobian = jacobian
}
fn jacobian_prev(&self) -> RowDVector<Scalar> {
self.jacobian.clone()
}
}
|
