mass_spring/
mass_spring.rs

1use approx::assert_abs_diff_eq;
2use faer::{prelude::SpSolver, sparse::SparseColMat, Col};
3use nalgebra::SVector;
4use raddy::{make::val, sparse::objective::Objective, types::advec};
5
6struct SpringEnergy {
7    k: f64,
8    restlen: f64,
9}
10
11// 2d * 2nodes = 4dof
12impl Objective<4> for SpringEnergy {
13    type EvalArgs = ();
14    fn eval(&self, variables: &raddy::types::advec<4, 4>, _: &()) -> raddy::Ad<4> {
15        let p1 = advec::<4, 2>::new(variables[0].clone(), variables[1].clone());
16        let p2 = advec::<4, 2>::new(variables[2].clone(), variables[3].clone());
17
18        let len = (p2 - p1).norm();
19        // Hooke's law
20        let potential = val::scalar(0.5 * self.k) * (len - val::scalar(self.restlen)).powi(2);
21
22        potential
23    }
24}
25
26fn main() {
27    // todo!("This example is undone");
28
29    let springs = vec![[0, 1, 2, 3], [2, 3, 4, 5], [0, 1, 4, 5]];
30    let x0 = faer::col::from_slice(&[0.0, 0.0, 0.001, 0.0, 0.001, 0.01]).to_owned();
31
32    let obj = SpringEnergy {
33        k: 10000.0,
34        restlen: 1.0,
35    };
36
37    let mut i = 0;
38    let mut x = x0.clone();
39    let mut dir: Col<f64>;
40    // Newton Raphson
41    while {
42        let grad = obj.grad(&x, &springs, &());
43        let mut hesstrip = obj.hess_trips(&x, &springs, &());
44        for i in 0..6 {
45            hesstrip.push((i, i, 1.0));
46        }
47        let hess = SparseColMat::try_new_from_triplets(6, 6, &hesstrip).unwrap();
48
49        // wtf matrix is not invertible?
50        dir = hess.sp_lu().unwrap().solve(-&grad);
51
52        dir.norm_l2() > 1e-8
53    } {
54        x += dir;
55
56        i += 1;
57
58        let p1 = SVector::<f64, 2>::new(x[0], x[1]);
59        let p2 = SVector::<f64, 2>::new(x[2], x[3]);
60        let p3 = SVector::<f64, 2>::new(x[4], x[5]);
61
62        println!("\nIter {}", i);
63        println!("Len 1 = {}", (p2 - p1).norm());
64        println!("Len 2 = {}", (p3 - p2).norm());
65        println!("Len 3 = {}", (p3 - p1).norm());
66    }
67
68    println!("\nFinal potential: {}", obj.value(&x, &springs, &()));
69    let p1 = SVector::<f64, 2>::new(x[0], x[1]);
70    let p2 = SVector::<f64, 2>::new(x[2], x[3]);
71    let p3 = SVector::<f64, 2>::new(x[4], x[5]);
72
73    assert_abs_diff_eq!((p2 - p1).norm(), 1.0, epsilon = 1e-4);
74    assert_abs_diff_eq!((p3 - p2).norm(), 1.0, epsilon = 1e-4);
75    assert_abs_diff_eq!((p3 - p1).norm(), 1.0, epsilon = 1e-4);
76}
mass_spring/mass_spring.rs

mass_spring/
mass_spring.rs
