double_pendulum/
lib.rs

1use std::fmt::Formatter;
2mod tests;
3
4#[derive(Debug, Copy, Clone, PartialEq)]
5pub struct Point {
6    x: f64,
7    y: f64,
8}
9
10impl Point {
11    /// Start a new point with default values.
12    pub fn new_def() -> Self {
13        Self { x: 0.0, y: 0.0 }
14    }
15    /// Function to make a new instance of Point struct
16    /// with values x and y of type f64
17    pub fn new(x: f64, y: f64) -> Self {
18        Point { x, y }
19    }
20
21    pub fn location(&mut self, x: f64, y: f64) -> () {
22        self.x = x;
23        self.y = y;
24    }
25
26    pub fn x(&self) -> f64{
27        self.x
28    }
29    pub fn y(&self) -> f64{
30        self.y
31    }
32}
33
34#[derive(Debug, Clone, Copy, PartialEq)]
35pub struct DoublePendulum {
36    m1: f64,
37    m2: f64,
38    a1: f64,
39    a2: f64,
40    l1: f64,
41    l2: f64,
42    c1: Point,
43    c2: Point,
44    v1: f64,
45    v2: f64,
46    acc1: f64,
47    acc2: f64,
48    g: f64,
49    damp_factor: f64,
50}
51
52impl DoublePendulum {
53    pub fn get_joint(&self) -> Point{
54        Point::new(self.c1.x, self.c1.y)
55    }
56
57    pub fn get_end(&self) -> Point{
58        Point::new(self.c2.x, self.c2.y)
59    }
60
61    fn calc_pos(&mut self) {
62        self.c1.x = self.l1 * self.a1.sin();
63        self.c1.y = -1.0*self.l1 * self.a1.cos();
64        self.c2.x = self.c1.x + self.l2 * self.a2.sin();
65        self.c2.y = self.c1.y - self.l2 * self.a2.cos();
66    }
67
68    fn new_acc(&mut self) {
69        let p1 = -self.g*(2.0*self.m1 + self.m2)*self.a1.sin();
70        let p2 = -self.m2*self.g*(self.a1 - 2.0*self.a2).sin();
71        let p3 = -2.0*(self.a1 - self.a2).sin()*self.m2;
72        let p4 = self.v2*self.v2*self.l2 + self.v1*self.v1*(self.a1 - self.a2).cos();
73        let p5 = 2.0*self.m1 + self.m2 - self.m2*(2.0*self.a1 - 2.0*self.a2).cos();
74        
75        self.acc1 = (p1 + p2 + p3*p4) / (self.l1*p5);
76
77        let p1 = 2.0*(self.a1 - self.a2).sin();
78        let p2 = self.v1*self.v1*self.l1 *(self.m1 + self.m2);
79        let p3 = self.g*(self.m1 + self.m2) * self.a1.cos();
80        let p4 = self.v2*self.v2*self.l2*self.m2*(self.a1 - self.a2).cos();
81
82        self.acc2 = p1*(p2 + p3 + p4)/(self.l2*p5);
83    }
84
85    fn new_angle(&mut self){
86        self.a1 += self.v1;
87        self.a2 += self.v2;
88    }
89
90    fn new_vel(&mut self){
91        self.v1 += 0.2*self.acc1;
92        self.v2 += 0.2*self.acc2;
93
94        // todo: add a parameter and setter method for dampening constant. 
95        self.v1 *= 1.0 - self.damp_factor;
96        self.v2 *= 1.0 - self.damp_factor;
97    }
98
99    pub fn new_pos(&mut self){
100        self.new_vel();
101        self.new_acc();
102        self.new_angle();
103        self.calc_pos();
104    }
105
106    pub fn new(m1: f64, m2: f64, a1: f64, a2: f64, l1: f64, l2: f64, damp_factor: f64) -> Self {
107        let mut obj = DoublePendulum {
108            m1,
109            m2,
110            a1,
111            a2,
112            l1,
113            l2,
114            c1: Point::new(0.0, 0.0),
115            c2: Point::new(0.0, 0.0),
116            v1: 0.0,
117            v2: 0.0,
118            acc1: 0.0,
119            acc2: 0.0,
120            g: 1.0,
121            damp_factor,
122        };
123        obj.calc_pos();
124        return obj;
125    }
126
127    pub fn m1(&self) -> f64{
128        return self.m1;
129    }
130    pub fn m2(&self) -> f64{
131        return self.m2;
132    }
133    pub fn l1(&self) -> f64{
134        return self.l1;
135    }
136    pub fn l2(&self) -> f64{
137        return self.l2;
138    }
139}
140
141impl std::fmt::Display for Point {
142    fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
143        write!(f, "({}, {})", self.x, self.y)
144    }
145}
double_pendulum/lib.rs

double_pendulum/
lib.rs
