add substepping, update solver

src/link.rs

@@ -22,12 +22,10 @@ impl Link {
             let vec_norm = vec / vec_len;
 
             let displacement = self.length - vec_len;
-            let stiffness = 999.;
+            let vec_scaled = vec_norm * displacement / 2.;
-            let vec_scaled = vec_norm * displacement * stiffness;
 
-            let damping = 0.99;
+            self.p1.borrow_mut().apply_vel(-vec_scaled);
-            self.p1.borrow_mut().apply_force(-vec_scaled * damping);
+            self.p2.borrow_mut().apply_vel(vec_scaled);
-            self.p2.borrow_mut().apply_force(vec_scaled * damping);
         }
     }
 }

src/main.rs

@@ -16,7 +16,9 @@ use link::Link;
 const WIN_WIDTH: u32 = 800;
 const WIN_HEIGHT: u32 = 600;
 
-const GRAVITY: Vector2f = Vector2f::new(0., 100.);
+const GRAVITY: Vector2f = Vector2f::new(0., 1500.);
+
+const SUBSTEP_COUNT: u32 = 16;
 
 fn populate_particles(particles: &mut Vec<Rc<RefCell<Particle>>>, num: u32, cols: u32) {
     for i in 0..num {
@@ -72,8 +74,10 @@ fn populate_links(
 
 fn apply_forces(particles: &Vec<Rc<RefCell<Particle>>>) {
     for particle in particles {
+        let velocity = particle.borrow().vel;
         let mut borrowed = particle.borrow_mut();
         borrowed.apply_force(GRAVITY);
+        borrowed.apply_force(-(velocity * 0.5));
     }
 }
 
@@ -89,6 +93,12 @@ fn solve_links(links: &mut Vec<Link>) {
     }
 }
 
+fn update_particle_derivatives(particles: &Vec<Rc<RefCell<Particle>>>, dt: f32) {
+    for particle in particles {
+        particle.borrow_mut().update_derivatives(dt);
+    }
+}
+
 fn update_positions(circles: &mut Vec<CircleShape>, particles: &[Rc<RefCell<Particle>>]) {
     for (index, circle) in circles.iter_mut().enumerate() {
         circle.set_position(particles[index].borrow().pos);
@@ -160,20 +170,21 @@ fn main() -> SfResult<()> {
                 let dist_vec = mouse_coords - p_pos;
                 let dist = math::vec_len(dist_vec);
                 if dist < 20. {
-                    particle.borrow_mut().apply_force(mouse_vel * 128.);
+                    particle.borrow_mut().apply_force(mouse_vel * 8000.);
                 }
             }
         }
 
         let dt = clock.restart().as_seconds();
+        let substep_dt = dt/SUBSTEP_COUNT as f32;
         
-        
+        for _ in 0..SUBSTEP_COUNT {
-        apply_forces(&particles);
+            apply_forces(&particles);
-        update_particles(&particles, dt);
+            update_particles(&particles, substep_dt);
-
+            solve_links(&mut links);
-        solve_links(&mut links);
+            update_particle_derivatives(&particles, substep_dt);
-
+            update_positions(&mut circles, &particles);
-        update_positions(&mut circles, &particles);
+        }
 
         window.clear(Color::BLACK);
         draw_all(&mut window, &circles /* &lines */);

src/particle.rs

@@ -3,6 +3,7 @@ use sfml::system::Vector2f;
 pub struct Particle {
     pub pos: Vector2f,
     prev_pos: Vector2f,
+    pub vel: Vector2f,
     accel: Vector2f,
 
     immovable: bool,
@@ -14,6 +15,7 @@ impl Particle {
         Particle {
             pos,
             prev_pos: pos,
+            vel: Vector2f::default(),
             accel: Vector2f::default(),
 
             immovable,
@@ -21,19 +23,23 @@ impl Particle {
     }
 
     pub fn apply_force<F: Into<Vector2f>>(&mut self, force: F) {
-        if self.immovable {
-            return;
-        }
         self.accel += force.into();
     }
 
     pub fn update(&mut self, dt: f32) {
-        let damping = 0.99;
+        if self.immovable {return}
-        let vel = (self.pos - self.prev_pos) * damping;
-        let new_pos = self.pos + vel + self.accel * (dt * dt);
-
         self.prev_pos = self.pos;
-        self.pos = new_pos;
+        self.vel += self.accel * dt;
+        self.pos += self.vel * dt;
+    }
+
+    pub fn update_derivatives(&mut self, dt: f32) {
+        self.vel = (self.pos - self.prev_pos) / dt;
         self.accel = Vector2f::default();
     }
+
+    pub fn apply_vel(&mut self, vel: Vector2f) {
+        if self.immovable {return}
+        self.pos += vel;
+    }
 }