feat: solid foundation for particle system

2 files changed, 219 insertions, 154 deletions

diff --git a/src/main.rs b/src/main.rs
index ab2e15f..fcf19c7 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -1,167 +1,28 @@
-use std::io::Write;
-use std::{fs::File, path::PathBuf};
-use cgmath::{Angle, ElementWise, InnerSpace, Matrix3, MetricSpace, Rad, Vector3, Zero};
-
-type Vector = Vector3<f32>;
-
-#[derive(Debug, Clone, Copy)]
-struct Particle {
-    position: Vector,
-    position_old: Vector,
-    // velocity: Vector,
-    acceleration: Vector,
-    mass: f32,
-    fixed: bool,
-}
-
-impl Particle {
-    pub fn new(position: Vector, mass: f32, fixed: bool) -> Self {
-        Self {
-            position,
-            position_old: position,
-            acceleration: Vector::zero(),
-            mass,
-            fixed,
-        }
-
-    }
-
-    fn apply_force(&mut self, force: Vector) {
-        self.acceleration += force / self.mass;
-    }
-
-    fn tick(&mut self, dt: f32) {
-        if !self.fixed {
-            // Verlet method
-            let velocity = self.position - self.position_old;
-            self.position_old = self.position;
-            self.position += velocity + self.acceleration * dt * dt;
-        }
-
-        self.acceleration = Vector::zero();
-    }
-}
-
-#[derive(Debug, Clone, Copy)]
-struct Spring {
-    rest_length: f32,
-    stiffness: f32,
-    particle_ids: [usize; 2],
-}
-
-#[derive(Debug, Clone)]
-struct Simulation {
-    particles: Vec<Particle>,
-    springs: Vec<Spring>,
-}
-
-impl Simulation {
-    fn tick(&mut self, dt: f32) {
-        for spring in self.springs.iter() {
-            let [a, b] = spring.particle_ids;
-            let dist_vec = self.particles[b].position - self.particles[a].position;
-            let force_magnitude = spring.stiffness * (dist_vec.magnitude() - spring.rest_length);
-            let normalized = dist_vec.normalize();
-
-            self.particles[a].apply_force(normalized.mul_element_wise(force_magnitude));
-            self.particles[b].apply_force(normalized.mul_element_wise(force_magnitude * -1.0));
-        }
-        for p in &mut self.particles {
-            p.tick(dt);
-        }
-    }
-}
-
-
-struct PPM {
-    prefix: PathBuf,
-    width: usize,
-    height: usize,
-    // pixels_per_unit: usize,
-}
-
-impl PPM {
-    fn render_particles(&self, particles: &Vec<Particle>) -> String {
-        let mut s = format!("P3\n{} {}\n255\n", self.width, self.height);
-        let white = "255 255 255 ";
-        let black = "0 0 0 ";
-
-        for pixel_row in 0..self.height {
-            for pixel_col in 0..self.width {
-                let point = Vector::new(pixel_col as f32, 0.0, (pixel_row as f32) * -1.0) + Vector::new(self.width as f32 / -2.0, 0.0, self.height as f32 / 2.0);
-                let color = match particles.iter().any(|p| p.position.distance(point) <= p.mass / 2.0) {
-                    true => black,
-                    false => white,
-                };
-                s += color;
-            }
-        }
-        s
-    }
-
-    fn save_frame(&self, particles: &Vec<Particle>, time: f32) {
-        let file_name = format!("frame-{:08.3}", time);
-        let path = self.prefix.join(file_name);
-        let mut file = File::create(path).unwrap();
-        let data = self.render_particles(particles);
-        file.write(data.as_bytes()).unwrap();
-    }
-}
+use particle_system::{Particle, ParticleSystem, Point, Vector};
 
+mod particle_system;
 
 fn main() {
-    let rotation_matrix = Matrix3::from_axis_angle(Vector::unit_y(), -Rad::full_turn() / 12.0);
-
-    let mut simulation = Simulation {
-        particles: vec![
-            Particle::new(Vector::zero(), 5.0, true),
-            Particle::new(rotation_matrix * Vector::unit_x() * 40.0, 30.0, false),
-            Particle::new(rotation_matrix * Vector::unit_x() * 40.0 + rotation_matrix * rotation_matrix * Vector::unit_x() * 50.0, 15.0, false)
-        ],
-        springs: vec![
-            Spring {
-                particle_ids: [0, 1],
-                rest_length: 40.0,
-                stiffness: 400.0,
-            },
-            Spring {
-                particle_ids: [1, 2],
-                rest_length: 50.0,
-                stiffness: 5.0,
-            },
-        ],
+    let dt = 0.01;
+    let mut system = ParticleSystem {
+        particles: vec![Particle::new(Point::origin(), 1.0)],
+        t: 0.0,
     };
 
+    let gravity = -9.8 * Vector::y();
 
-    let ppm = PPM {
-        prefix: PathBuf::from("./out"),
-        width: 150,
-        height: 800,
-    };
-
-    let gravity = Vector::new(0.0, 0.0, -9.8);
-
-    let time_step = 0.01;
-    let mut tick = 1;
-    let mut time = 0.0;
-    while time < 130.0 {
-        {
-            let p = &mut simulation.particles[0];
-            p.apply_force(gravity * -p.mass);
-        }
-        let mut all_particles = Vec::new();
-        for p in &mut simulation.particles {
-            p.apply_force(gravity * p.mass);
+    for i in 0..3 {
+        println!("Iteration #{i}");
+        for particle in &mut system.particles {
+            particle.reset_force();
         }
-        simulation.tick(time_step);
-        all_particles.append(&mut simulation.particles.clone());
 
-        if tick % 10 == 0 {
-            ppm.save_frame(&all_particles, time);
+        for particle in &mut system.particles {
+            particle.apply_force(gravity);
         }
 
+        system.euler_step(dt);
 
-        time += time_step;
-        tick +=1;
+        println!("{:?}", system);
     }
 }
diff --git a/src/particle_system.rs b/src/particle_system.rs
new file mode 100644
index 0000000..0556764
--- /dev/null
+++ b/src/particle_system.rs
@@ -0,0 +1,204 @@
+use nalgebra::{Const, DVector, Dyn, Matrix, Point as PointBase, SVector, ViewStorage};
+
+const N: usize = 2;
+pub type Scalar = f32;
+
+pub type Vector = SVector<Scalar, N>;
+pub type Point = PointBase<Scalar, N>;
+
+#[derive(Debug)]
+pub struct Particle {
+    pub mass: Scalar,
+    pub position: Point,
+    pub velocity: Vector,
+
+    /// Force accumulator
+    pub force: Vector,
+}
+
+impl Particle {
+    pub fn new(position: Point, mass: Scalar) -> Self {
+        Self {
+            mass,
+            position,
+            velocity: Vector::zeros(),
+            force: Vector::zeros(),
+        }
+    }
+
+    pub fn apply_force(&mut self, force: Vector) {
+        self.force += force;
+    }
+    pub fn reset_force(&mut self) {
+        self.force = Vector::zeros()
+    }
+}
+
+/// A vector of concatenated position and velocity components of each particle
+#[derive(Debug)]
+pub struct PhaseSpace(DVector<Scalar>);
+type ParticleView<'a> = Matrix<
+    f32,
+    Const<{ PhaseSpace::PARTICLE_DIM }>,
+    Const<1>,
+    ViewStorage<'a, f32, Const<{ PhaseSpace::PARTICLE_DIM }>, Const<1>, Const<1>, Dyn>,
+>;
+
+impl PhaseSpace {
+    /// Each particle spans 2N elements in a vector
+    /// first N for position, then N more for velocity
+    const PARTICLE_DIM: usize = N * 2;
+
+    pub fn new(particle_count: usize) -> Self {
+        let dimension = particle_count * PhaseSpace::PARTICLE_DIM;
+        Self(DVector::<Scalar>::zeros(dimension))
+    }
+
+    pub fn particle_view(&self, i: usize) -> ParticleView {
+        self.0
+            .fixed_rows::<{ PhaseSpace::PARTICLE_DIM }>(i * PhaseSpace::PARTICLE_DIM)
+    }
+
+    pub fn set_particle(&mut self, i: usize, position: Point, velocity: Vector) {
+        let mut view = self
+            .0
+            .fixed_rows_mut::<{ PhaseSpace::PARTICLE_DIM }>(i * PhaseSpace::PARTICLE_DIM);
+        for i in 0..N {
+            view[i] = position[i];
+            view[i + N] = velocity[i];
+        }
+    }
+}
+
+#[derive(Debug)]
+pub struct ParticleSystem {
+    pub particles: Vec<Particle>,
+
+    /// Simulation clock
+    pub t: Scalar,
+}
+
+impl ParticleSystem {
+    fn collect_phase_space(&self) -> PhaseSpace {
+        let mut phase_space = PhaseSpace::new(self.particles.len());
+        for (particle_index, particle) in self.particles.iter().enumerate() {
+            phase_space.set_particle(particle_index, particle.position, particle.velocity);
+        }
+        phase_space
+    }
+
+    fn compute_derivative(&self) -> PhaseSpace {
+        let mut phase_space = PhaseSpace::new(self.particles.len());
+        for (particle_index, particle) in self.particles.iter().enumerate() {
+            phase_space.set_particle(
+                particle_index,
+                particle.velocity.into(),
+                particle.force / particle.mass,
+            );
+        }
+        phase_space
+    }
+
+    fn scatter_phase_space(&mut self, phase_space: &PhaseSpace) {
+        for (particle_index, particle) in &mut self.particles.iter_mut().enumerate() {
+            let view = phase_space.particle_view(particle_index);
+
+            for i in 0..N {
+                particle.position[i] = view[i];
+                particle.velocity[i] = view[i + N];
+            }
+        }
+    }
+
+    pub fn euler_step(&mut self, dt: Scalar) {
+        let derivative = self.compute_derivative();
+        let mut state = self.collect_phase_space();
+
+        state.0 += derivative.0 * dt;
+        self.scatter_phase_space(&state);
+
+        self.t += dt;
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::{Particle, ParticleSystem, PhaseSpace, Point, Scalar, Vector};
+
+    #[test]
+    fn test_collect_phase_space() {
+        let system = ParticleSystem {
+            particles: vec![Particle::new(Point::new(2.0, 3.0), 1.0)],
+            t: 0.0,
+        };
+        let phase_space = system.collect_phase_space();
+
+        assert!(
+            !phase_space.0.is_empty(),
+            "Phase space has to contain non-zero values"
+        );
+    }
+
+    #[test]
+    fn test_scatter_phase_space() {
+        let mut phase_space = PhaseSpace::new(2);
+        phase_space.set_particle(1, Point::new(5.0, 7.0), Vector::x());
+
+        let mut system = ParticleSystem {
+            particles: vec![
+                Particle::new(Point::new(0.0, 0.0), 1.0),
+                Particle::new(Point::new(0.0, 0.0), 1.0),
+            ],
+            t: 0.0,
+        };
+
+        system.scatter_phase_space(&phase_space);
+
+        assert!(
+            !system.particles[1].velocity.is_empty(),
+            "Velocity has to be set"
+        );
+        assert!(
+            !system.particles[1].position.is_empty(),
+            "Position has to be set"
+        );
+    }
+
+    fn simulate_falling_ball(fall_time: Scalar, dt: Scalar) -> (Vector, Vector) {
+        let gravity = -9.8 * Vector::y();
+
+        let mut system = ParticleSystem {
+            particles: vec![Particle::new(Point::origin(), 1.0)],
+            t: 0.0,
+        };
+
+        let iterations = (fall_time / dt) as usize;
+
+        for _ in 0..iterations {
+            for particle in &mut system.particles {
+                particle.reset_force();
+            }
+
+            for particle in &mut system.particles {
+                particle.apply_force(gravity);
+            }
+
+            system.euler_step(dt);
+        }
+
+        let expected_velocity = gravity * fall_time; // vt
+        let expected_position = gravity * fall_time * fall_time / 2.0; // at^2 / 2
+
+        (
+            system.particles[0].position.coords - expected_position,
+            system.particles[0].velocity - expected_velocity,
+        )
+    }
+
+    #[test]
+    fn ball_should_fall() {
+        let (position_error, velocity_error) = simulate_falling_ball(10.0, 0.01);
+        assert!(position_error.norm() < 0.5, "Position error has is too high");
+        assert!(velocity_error.norm() < 0.5, "Velocity error has is too high");
+    }
+}