feat!: add raylib rendering

16 files changed, 841 insertions, 0 deletions

diff --git a/physics/src/algebra/mod.rs b/physics/src/algebra/mod.rs
new file mode 100644
index 0000000..47efba8
--- /dev/null
+++ b/physics/src/algebra/mod.rs
@@ -0,0 +1,9 @@
+use nalgebra::{Point as PointBase, SVector};
+
+pub const N: usize = 2;
+pub type Scalar = f64;
+
+pub type Vector = SVector<Scalar, N>;
+pub type Point = PointBase<Scalar, N>;
+
+pub mod subspace;
diff --git a/physics/src/algebra/subspace.rs b/physics/src/algebra/subspace.rs
new file mode 100644
index 0000000..9fb944e
--- /dev/null
+++ b/physics/src/algebra/subspace.rs
@@ -0,0 +1,70 @@
+use nalgebra::SMatrix;
+
+use super::{Scalar, N, Point, Vector};
+
+
+type ProjectionMatrix = SMatrix<Scalar, N, N>;
+
+pub struct Subspace<const DIM: usize> {
+    point: Point,
+    vectors: [Vector; DIM],
+    projection_matrix: ProjectionMatrix,
+}
+
+pub type Line = Subspace<1>;
+pub type Plane = Subspace<2>;
+
+impl<const DIM: usize> Subspace<DIM> {
+    pub fn new(point: Point, mut vectors: [Vector; DIM]) -> Self {
+        for vector in &mut vectors {
+            vector.normalize_mut();
+        }
+
+        Self {
+            point,
+            vectors,
+            projection_matrix: {
+                let v = SMatrix::<Scalar, N, DIM>::from_columns(&vectors);
+                let transpose = v.transpose();
+                let inverse = (transpose * v).try_inverse().unwrap();
+                v * inverse * transpose
+            }
+        }
+    }
+
+    pub fn project_point(&self, point: Point) -> Point {
+        self.projection_matrix * (point - self.point.coords) + self.point.coords
+    }
+
+    pub fn distance(&self, point: Point) -> Scalar {
+        let projected = self.project_point(point);
+        (projected - point).norm()
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use crate::algebra::{subspace::Line, Point, Vector};
+
+    #[test]
+    fn test_projection_onto_line() {
+        let line = Line::new(Point::new(1.0, 0.0), [Vector::new(3.0, 1.0)]);
+        let point = Point::new(3.0, 4.0);
+
+        let matrix = line.projection_matrix;
+        {
+            let diff = matrix * matrix - matrix;
+            assert!(diff.norm() < 0.001, "Projection matrix squared should be equal to itself: {}, {}", matrix, matrix* matrix);
+        }
+        {
+            let diff = matrix - matrix.transpose();
+            assert!(diff.norm() < 0.001, "Projection matrix transpose should be equal to itself: {}, {}", matrix, matrix.transpose());
+        }
+
+        {
+            let projected = line.project_point(point);
+            let diff = projected - Point::new(4.0, 1.0);
+            assert!(diff.norm() < 0.001, "Point projected incorrectly");
+        }
+    }
+}
diff --git a/physics/src/constraint/anchor.rs b/physics/src/constraint/anchor.rs
new file mode 100644
index 0000000..c90cfb2
--- /dev/null
+++ b/physics/src/constraint/anchor.rs
@@ -0,0 +1,43 @@
+use nalgebra::{DVector, RowDVector};
+
+use crate::particle_system::ParticleSystem;
+use crate::algebra::{Point, Scalar, N};
+
+use super::Constraint;
+
+pub struct AnchorConstraint {
+    pub particle_id: usize,
+    pub anchor: Point,
+
+    jacobian: RowDVector<Scalar>,
+}
+
+impl ParticleSystem {
+    pub fn add_anchor_constraint(&mut self, particle_id: usize) {
+        let anchor = self.particles[particle_id].position;
+        self.constraints.push(Box::new(AnchorConstraint {
+            particle_id,
+            anchor,
+            jacobian: RowDVector::zeros(self.particles.len() * N),
+        }));
+    }
+}
+
+impl Constraint for AnchorConstraint {
+    fn get_particles(&self) -> Vec<usize> {
+        vec![self.particle_id]
+    }
+
+    fn c(&self, q: &DVector<Scalar>) -> Scalar {
+        let position = q.fixed_rows(self.particle_id * N);
+        (position - self.anchor.coords).norm()
+    }
+
+    fn set_jacobian(&mut self, jacobian: RowDVector<Scalar>) {
+        self.jacobian = jacobian
+    }
+
+    fn jacobian_prev(&self) -> RowDVector<Scalar> {
+        self.jacobian.clone()
+    }
+}
diff --git a/physics/src/constraint/beam.rs b/physics/src/constraint/beam.rs
new file mode 100644
index 0000000..14b1c1f
--- /dev/null
+++ b/physics/src/constraint/beam.rs
@@ -0,0 +1,47 @@
+use nalgebra::{DVector, RowDVector};
+
+use crate::particle_system::ParticleSystem;
+use crate::algebra::{Scalar, N};
+
+use super::Constraint;
+
+pub struct BeamConstraint {
+    pub particle_ids: [usize; 2],
+    pub length: Scalar,
+
+    jacobian: RowDVector<Scalar>,
+}
+
+impl ParticleSystem {
+    pub fn add_beam_constraint(&mut self, particle_ids: [usize; 2]) {
+        let a = &self.particles[particle_ids[0]];
+        let b = &self.particles[particle_ids[1]];
+
+        self.constraints.push(Box::new(BeamConstraint {
+            particle_ids,
+            length: (a.position - b.position).norm(),
+            jacobian: RowDVector::zeros(self.particles.len() * N),
+        }));
+    }
+}
+
+impl Constraint for BeamConstraint {
+    fn get_particles(&self) -> Vec<usize> {
+        Vec::from(self.particle_ids)
+    }
+
+    fn c(&self, q: &DVector<Scalar>) -> Scalar {
+        let a = q.fixed_rows::<N>(self.particle_ids[0] * N);
+        let b = q.fixed_rows::<N>(self.particle_ids[1] * N);
+
+        (a - b).norm() - self.length
+    }
+
+    fn set_jacobian(&mut self, jacobian: RowDVector<Scalar>) {
+        self.jacobian = jacobian
+    }
+
+    fn jacobian_prev(&self) -> RowDVector<Scalar> {
+        self.jacobian.clone()
+    }
+}
diff --git a/physics/src/constraint/mod.rs b/physics/src/constraint/mod.rs
new file mode 100644
index 0000000..b2958ec
--- /dev/null
+++ b/physics/src/constraint/mod.rs
@@ -0,0 +1,140 @@
+use nalgebra::{DMatrix, DVector, RowDVector};
+
+use crate::particle_system::ParticleSystem;
+use crate::algebra::{Scalar, Vector, N};
+pub mod anchor;
+pub mod slider;
+pub mod beam;
+
+const SPRING_CONSTANT: Scalar = 0.65;
+const DAMPING_CONSTANT: Scalar = 0.85;
+
+/// SIZE is always 3xN, but this operation can't be done at compile time yet:
+/// "generic parameters may not be used in const operations"
+pub trait Constraint {
+    fn get_particles(&self) -> Vec<usize>;
+
+    fn c(&self, q: &DVector<Scalar>) -> Scalar;
+
+    fn jacobian_prev(&self) -> RowDVector<Scalar>;
+    fn set_jacobian(&mut self, jacobian: RowDVector<Scalar>);
+
+    fn partial_derivative(&self, q: &DVector<Scalar>) -> RowDVector<Scalar> {
+        let dq = 0.001;
+        let c_original = self.c(&q);
+        let mut result = RowDVector::zeros(q.len());
+        // The only non-zero components of derivative vector are
+        // the particles that this constraint affects
+        for particle_id in self.get_particles() {
+            for i in 0..N {
+                let index = particle_id * N + i;
+                let mut q_partial = q.clone();
+                q_partial[index] += dq;
+
+                let c = self.c(&q_partial);
+
+                result[index] = (c - c_original) / dq
+            }
+        }
+
+        result
+    }
+
+    fn compute(
+        &mut self,
+        q: &DVector<Scalar>,
+        q_prev: &DVector<Scalar>,
+        dt: Scalar,
+    ) -> (Scalar, Scalar, RowDVector<Scalar>, RowDVector<Scalar>) {
+        let c = self.c(q);
+        let c_prev = self.c(q_prev);
+
+        let c_dot = (c - c_prev) / dt;
+
+        let jacobian = self.partial_derivative(&q);
+
+        let jacobian_dot = (jacobian.clone() - self.jacobian_prev()) / dt;
+        self.set_jacobian(jacobian.clone());
+
+        (c, c_dot, jacobian, jacobian_dot)
+    }
+}
+
+impl ParticleSystem {
+    pub fn enforce_constraints(&mut self, dt: Scalar) {
+        if self.constraints.len() == 0 {
+            return
+        }
+
+        let size = self.particles.len() * N;
+        let mut q = DVector::zeros(size);
+        let mut q_prev = DVector::zeros(size);
+
+        for (p, particle) in self.particles.iter().enumerate() {
+            for i in 0..N {
+                q[p * N + i] = particle.position[i];
+                q_prev[p * N + i] = particle.position[i] - particle.velocity[i] * dt;
+            }
+        }
+
+        let q_dot = (q.clone() - q_prev.clone()) / dt;
+
+        let mut c = DVector::zeros(self.constraints.len());
+        let mut c_dot = DVector::zeros(self.constraints.len());
+        let mut jacobian = DMatrix::<Scalar>::zeros(self.constraints.len(), size);
+        let mut jacobian_dot = DMatrix::<Scalar>::zeros(self.constraints.len(), size);
+
+        for (constraint_id, constraint) in self.constraints.iter_mut().enumerate() {
+            let (value, derivative, j, jdot) = constraint.compute(&q, &q_prev, dt);
+            jacobian.set_row(constraint_id, &j);
+            jacobian_dot.set_row(constraint_id, &jdot);
+            c[constraint_id] = value;
+            c_dot[constraint_id] = derivative;
+        }
+
+        // println!("C {}\nC' {}", c, c_dot);
+        // println!("J  = {}", jacobian.clone());
+        // println!("J' = {}", jacobian_dot.clone());
+
+        let mut w = DMatrix::zeros(size, size);
+        for (i, particle) in self.particles.iter().enumerate() {
+            for j in 0..N {
+                *w.index_mut((i * N + j, i * N + j)) = 1.0 / particle.mass;
+            }
+        }
+
+        let mut forces = DVector::zeros(size);
+        for (p, particle) in self.particles.iter().enumerate() {
+            for i in 0..N {
+                forces[p * N + i] = particle.force[i];
+            }
+        }
+
+        let lhs = jacobian.clone() * w.clone() * jacobian.transpose();
+        // println!("LHS {}", lhs);
+
+        let rhs = -(jacobian_dot * q_dot
+            + jacobian.clone() * w * forces
+            + SPRING_CONSTANT * c
+            + DAMPING_CONSTANT * c_dot);
+
+        // println!("RHS {}", rhs);
+
+        match lhs.lu().solve(&rhs) {
+            Some(lambda) => {
+                // println!("Lambda = {}", lambda);
+                let constraint_force = jacobian.transpose() * lambda;
+
+                for i in 0..self.particles.len() {
+                    let mut force = Vector::zeros();
+                    for j in 0..N {
+                        force[j] = constraint_force[i * N + j];
+                    }
+
+                    self.particles[i].apply_force(force);
+                }
+            }
+            None => println!("Lambda not found"),
+        }
+    }
+}
diff --git a/physics/src/constraint/slider.rs b/physics/src/constraint/slider.rs
new file mode 100644
index 0000000..fa7e840
--- /dev/null
+++ b/physics/src/constraint/slider.rs
@@ -0,0 +1,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()
+    }
+}
diff --git a/physics/src/force/drag.rs b/physics/src/force/drag.rs
new file mode 100644
index 0000000..c54dc40
--- /dev/null
+++ b/physics/src/force/drag.rs
@@ -0,0 +1,15 @@
+use crate::algebra::Scalar;
+
+use super::Force;
+
+pub struct Drag {
+    pub coefficient: Scalar,
+}
+
+impl Force for Drag {
+    fn apply(&self, particles: &mut Vec<crate::particle_system::Particle>) {
+        for particle in particles {
+            particle.apply_force(-self.coefficient * particle.velocity);
+        }
+    }
+}
diff --git a/physics/src/force/gravity.rs b/physics/src/force/gravity.rs
new file mode 100644
index 0000000..dd600a4
--- /dev/null
+++ b/physics/src/force/gravity.rs
@@ -0,0 +1,15 @@
+use crate::algebra::Vector;
+
+use super::Force;
+
+pub struct Gravity {
+    pub vector: Vector,
+}
+
+impl Force for Gravity {
+    fn apply(&self, particles: &mut Vec<crate::particle_system::Particle>) {
+        for particle in particles {
+            particle.apply_force(self.vector * particle.mass);
+        }
+    }
+}
diff --git a/physics/src/force/mod.rs b/physics/src/force/mod.rs
new file mode 100644
index 0000000..ce10f9f
--- /dev/null
+++ b/physics/src/force/mod.rs
@@ -0,0 +1,24 @@
+use crate::particle_system::{Particle, ParticleSystem};
+
+pub mod gravity;
+pub mod drag;
+pub mod spring;
+
+pub trait Force {
+    fn apply(&self, particles: &mut Vec<Particle>);
+}
+
+impl ParticleSystem {
+    fn reset_forces(&mut self) {
+        for particle in &mut self.particles {
+            particle.reset_force();
+        }
+    }
+    pub fn apply_forces(&mut self) {
+        self.reset_forces();
+
+        for force in &self.forces {
+            force.apply(&mut self.particles)
+        }
+    }
+}
diff --git a/physics/src/force/spring.rs b/physics/src/force/spring.rs
new file mode 100644
index 0000000..6c2cf67
--- /dev/null
+++ b/physics/src/force/spring.rs
@@ -0,0 +1,28 @@
+use crate::algebra::Scalar;
+
+use super::Force;
+
+pub struct Spring {
+    pub particle_ids: [usize; 2],
+    pub rest_length: Scalar,
+    pub spring_constant: Scalar,
+    pub damping_constant: Scalar,
+}
+
+impl Force for Spring {
+    fn apply(&self, particles: &mut Vec<crate::particle_system::Particle>) {
+        let a = &particles[self.particle_ids[0]];
+        let b = &particles[self.particle_ids[1]];
+        let i = a.position - b.position;
+        let i_dot = a.velocity - b.velocity;
+        let i_norm = i.norm();
+
+        let force = -(self.spring_constant * (i_norm - self.rest_length)
+        + (self.damping_constant * i.dot(&i_dot) / i_norm))
+        * i
+        / i_norm;
+
+        particles[self.particle_ids[0]].apply_force(force);
+        particles[self.particle_ids[1]].apply_force(-force);
+    }
+}
diff --git a/physics/src/lib.rs b/physics/src/lib.rs
new file mode 100644
index 0000000..8c24a10
--- /dev/null
+++ b/physics/src/lib.rs
@@ -0,0 +1,9 @@
+pub use nalgebra;
+
+pub mod algebra;
+pub mod constraint;
+pub mod force;
+pub mod particle_system;
+mod ppm;
+pub mod renderer;
+pub mod solver;
diff --git a/physics/src/particle_system.rs b/physics/src/particle_system.rs
new file mode 100644
index 0000000..30255ec
--- /dev/null
+++ b/physics/src/particle_system.rs
@@ -0,0 +1,53 @@
+use crate::{
+    algebra::{Point, Scalar, Vector},
+    constraint::Constraint,
+    force::Force,
+};
+
+#[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()
+    }
+}
+
+// #[derive(Debug)]
+pub struct ParticleSystem {
+    pub particles: Vec<Particle>,
+    pub constraints: Vec<Box<dyn Constraint>>,
+    pub forces: Vec<Box<dyn Force>>,
+
+    /// Simulation clock
+    pub t: Scalar,
+}
+
+impl ParticleSystem {
+    pub fn get_kinetic_energy(&self) -> Scalar {
+        self.particles.iter().fold(0.0, |acc, p| {
+            let velocity = p.velocity.norm();
+            let energy = p.mass * velocity * velocity / 2.0;
+            acc + energy
+        })
+    }
+}
diff --git a/physics/src/ppm.rs b/physics/src/ppm.rs
new file mode 100644
index 0000000..be26127
--- /dev/null
+++ b/physics/src/ppm.rs
@@ -0,0 +1,67 @@
+use std::{fs::File, io::Write, path::PathBuf};
+
+use nalgebra::Point2;
+use crate::algebra::{Scalar, N};
+use crate::renderer::Camera;
+use crate::particle_system::Particle;
+
+pub struct PPM<const WIDTH: usize, const HEIGHT: usize> {
+    pub prefix: PathBuf,
+
+    buffer: [[bool; WIDTH]; HEIGHT],
+    // pixels_per_unit: usize,
+}
+
+impl<const WIDTH: usize, const HEIGHT: usize> PPM<WIDTH, HEIGHT> {
+    pub fn new(prefix: PathBuf) -> Self {
+        Self {
+            prefix,
+            buffer: [[false; WIDTH]; HEIGHT],
+        }
+    }
+
+    pub fn clear_buffer(&mut self) {
+        self.buffer = [[false; WIDTH]; HEIGHT]
+    }
+
+    pub fn draw_circle(&mut self, center: Point2<Scalar>, radius: Scalar) {
+        let screen_center = Point2::new((WIDTH / 2) as Scalar, (HEIGHT / 2) as Scalar);
+
+        for pixel_row in 0..HEIGHT {
+            for pixel_col in 0..WIDTH {
+                let point = Point2::new(pixel_col as Scalar, pixel_row as Scalar);
+
+                if (point - center - screen_center.coords).norm() <= radius {
+                    self.buffer[HEIGHT - pixel_row - 1][pixel_col] = true
+                }
+            }
+        }
+    }
+
+    pub fn render_particles(&mut self, particles: &Vec<Particle>, camera: &Camera) {
+        for p in particles {
+            let point = camera.world_to_screen_space(p.position);
+            self.draw_circle(point, p.mass.powf(1.0 / N as Scalar));
+        }
+    }
+
+    pub fn save_frame(&self, time: Scalar) {
+        let file_name = format!("frame-{:08.3}", time);
+        let path = self.prefix.join(file_name);
+        let mut file = File::create(path).unwrap();
+
+        let mut s = format!("P3\n{} {}\n255\n", WIDTH, HEIGHT);
+        let white = "255 255 255 ";
+        let black = "0 0 0 ";
+
+        for row in self.buffer {
+            for pixel in row {
+                let color = if pixel { black } else { white };
+                s += color
+            }
+            s += "\n";
+        }
+
+        file.write(s.as_bytes()).unwrap();
+    }
+}
diff --git a/physics/src/renderer/mod.rs b/physics/src/renderer/mod.rs
new file mode 100644
index 0000000..63894b3
--- /dev/null
+++ b/physics/src/renderer/mod.rs
@@ -0,0 +1,68 @@
+use nalgebra::{Point2, SMatrix};
+
+use crate::{
+    algebra::subspace::Plane,
+    algebra::{Point, Scalar, Vector, N},
+};
+
+pub struct Camera {
+    plane: Plane,
+    up: Vector,
+    origin: Point,
+    world_to_screen_space: SMatrix<Scalar, 2, N>,
+    screen_space_to_world: SMatrix<Scalar, N, 2>,
+}
+
+impl Camera {
+    pub fn new(origin: Point, up: Vector, right: Vector) -> Self {
+        assert!(
+            up.dot(&right) == 0.0,
+            "Up and right vectors must be orthogonal"
+        );
+        let plane = Plane::new(origin, [up, right]);
+
+        let screen_space_to_world = SMatrix::<Scalar, N, 2>::from_columns(&[right, up]);
+        let world_to_screen_space = screen_space_to_world.pseudo_inverse(0.001).unwrap();
+
+        Self {
+            plane,
+            up,
+            origin,
+            world_to_screen_space,
+            screen_space_to_world,
+        }
+    }
+
+    pub fn world_to_screen_space(&self, point: Point) -> Point2<Scalar> {
+        let projected = self.plane.project_point(point);
+        let in_screen_space = self.world_to_screen_space * (projected - self.origin);
+        in_screen_space.into()
+    }
+    pub fn screen_space_to_world(&self, point: Point2<Scalar>) -> Point {
+        (self.screen_space_to_world * point) + self.origin.coords
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use crate::algebra::{Point, Vector};
+
+    use super::Camera;
+
+    #[test]
+    fn test_projection() {
+        let camera = Camera::new(
+            Point::new(1.0, 0.0),
+            Vector::new(1.0, 2.0),
+            Vector::new(2.0, -1.0),
+        );
+
+        let point = Point::new(3.0, 1.0);
+
+        let diff = camera.world_to_screen_space * point - Point::new(1.0, 1.0);
+        assert!(
+            diff.norm() < 0.001,
+            "Camera translated point into screen_space incorrectly"
+        );
+    }
+}
diff --git a/physics/src/solver/midpoint.rs b/physics/src/solver/midpoint.rs
new file mode 100644
index 0000000..2d71758
--- /dev/null
+++ b/physics/src/solver/midpoint.rs
@@ -0,0 +1,18 @@
+use crate::{algebra::Scalar, particle_system::ParticleSystem};
+use super::{PhaseSpace, Solver};
+
+impl Solver for ParticleSystem {
+    fn step(&mut self, dt: Scalar) {
+        let mut state = self.collect_phase_space();
+
+        // Shift to the midpoint
+        self.scatter_phase_space(&PhaseSpace {
+            0: state.0.clone() + self.compute_derivative().0 * dt / 2.0,
+        });
+
+        state.0 += self.compute_derivative().0 * dt;
+        self.scatter_phase_space(&state);
+
+        self.t += dt;
+    }
+}
diff --git a/physics/src/solver/mod.rs b/physics/src/solver/mod.rs
new file mode 100644
index 0000000..726dcae
--- /dev/null
+++ b/physics/src/solver/mod.rs
@@ -0,0 +1,188 @@
+use crate::particle_system::ParticleSystem;
+use crate::algebra::{Point, Scalar, Vector, N};
+use nalgebra::{Const, DVector, Dyn, Matrix, ViewStorage};
+
+mod midpoint;
+
+/// A vector of concatenated position and velocity components of each particle
+#[derive(Debug, Clone)]
+pub struct PhaseSpace(DVector<Scalar>);
+type ParticleView<'a> = Matrix<
+    Scalar,
+    Const<{ PhaseSpace::PARTICLE_DIM }>,
+    Const<1>,
+    ViewStorage<'a, Scalar, 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];
+        }
+    }
+}
+
+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 trait Solver {
+    fn step(&mut self, dt: Scalar);
+}
+
+#[cfg(test)]
+mod tests {
+    use super::{ParticleSystem, PhaseSpace, Point, Scalar, Solver, Vector};
+    use crate::particle_system::Particle;
+
+    #[test]
+    fn test_collect_phase_space() {
+        let system = ParticleSystem {
+            particles: vec![Particle::new(Point::new(2.0, 3.0), 1.0)],
+            constraints: vec![],
+            forces: vec![],
+            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),
+            ],
+            constraints: vec![],
+            forces: vec![],
+            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,
+        mass: Scalar,
+    ) -> (Vector, Vector, ParticleSystem) {
+        let gravity = -9.8 * Vector::y();
+
+        let mut system = ParticleSystem {
+            particles: vec![Particle::new(Point::origin(), mass)],
+            constraints: vec![],
+            forces: vec![],
+            t: 0.0,
+        };
+
+        let iterations = (fall_time / dt) as usize;
+
+        for _ in 0..iterations {
+            for particle in &mut system.particles {
+                particle.reset_force();
+                particle.apply_force(gravity * particle.mass);
+            }
+            system.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,
+            system,
+        )
+    }
+
+    #[test]
+    fn ball_should_fall() {
+        let (position_error, velocity_error, _) = simulate_falling_ball(10.0, 0.01, 3.0);
+        assert!(
+            position_error.norm() < 0.01,
+            "Position error is too high: {}",
+            position_error,
+        );
+        assert!(
+            velocity_error.norm() < 0.01,
+            "Velocity error is too high: {}",
+            velocity_error,
+        );
+    }
+
+    #[test]
+    fn freefall_different_masses() {
+        let (_, _, system1) = simulate_falling_ball(10.0, 0.01, 2.0);
+        let (_, _, system2) = simulate_falling_ball(10.0, 0.01, 10.0);
+
+        let diff = system1.particles[0].position - system2.particles[0].position;
+        assert!(
+            diff.norm() < 0.01,
+            "Points with different masses should fall with the same speed, diff: {}",
+            diff
+        );
+    }
+}