diff --git a/.cargo/config.toml b/.cargo/config.toml
index c302a44..3b57381 100644
--- a/.cargo/config.toml
+++ b/.cargo/config.toml
@@ -1,2 +1,4 @@
 [target.wasm32-unknown-unknown]
-runner = "wasm-server-runner"
\ No newline at end of file
+runner = "wasm-server-runner"
+# Needed for getrandom/uuid: https://github.com/uuid-rs/uuid/issues/792
+rustflags = ['--cfg', 'getrandom_backend="wasm_js"']
\ No newline at end of file
diff --git a/Cargo.toml b/Cargo.toml
index 8a1452a..cbf629f 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -4,6 +4,9 @@ members = [
     "crates/wgsparkl-testbed3d",
     "crates/wgsparkl2d",
     "crates/wgsparkl3d",
+    "crates/text_to_collider",
+    "examples3d",
+    "examples2d",
 ]
 resolver = "2"
 
@@ -23,6 +26,31 @@ rust.unexpected_cfgs = { level = "warn", check-cfg = [
 ] }
 
 [patch.crates-io]
+# parry2d = { path = "../parry/crates/parry2d" }
+# parry3d = { path = "../parry/crates/parry3d" }
+# encase = { path = "../encase" }
+# wgcore = { path = "../wgmath/crates/wgcore" }
+# wgebra = { path = "../wgmath/crates/wgebra" }
+# wgparry2d = { path = "../wgmath/crates/wgparry/crates/wgparry2d" }
+# wgparry3d = { path = "../wgmath/crates/wgparry/crates/wgparry3d" }
+# wgrapier2d = { path = "../wgmath/crates/wgrapier/crates/wgrapier2d" }
+# wgrapier3d = { path = "../wgmath/crates/wgrapier/crates/wgrapier3d" }
+
+parry2d = { git = "https://github.com/dimforge/parry.git", rev = "407fb449504dde7ba0a3d80a8de7e37ac991a77a" }
+parry3d = { git = "https://github.com/dimforge/parry.git", rev = "407fb449504dde7ba0a3d80a8de7e37ac991a77a" }
+encase = { git = "https://github.com/sebcrozet/encase.git", rev = "6755c1414c225cbeac9fde1ff9394b70d3b05404" }
+# wgcore = { git = "https://github.com/dimforge/wgmath.git", rev = "32b54c802be6040c5f003e7d3def4a33e4730d92" }
+wgcore = { git = "https://github.com/dimforge/wgmath.git", branch = "wgml-chat" }
+wgcore-derive = { git = "https://github.com/dimforge/wgmath.git", branch = "wgml-chat" }
+wgebra = { git = "https://github.com/dimforge/wgmath.git", branch = "wgml-chat" }
+# wgparry2d = { git = "https://github.com/dimforge/wgmath.git", rev = "32b54c802be6040c5f003e7d3def4a33e4730d92" }
+wgparry3d = { git = "https://github.com/dimforge/wgmath.git", branch = "wgml-chat" }
+# wgrapier2d = { git = "https://github.com/dimforge/wgmath.git", rev = "32b54c802be6040c5f003e7d3def4a33e4730d92" }
+wgrapier3d = { git = "https://github.com/dimforge/wgmath.git", branch = "wgml-chat" }
+rapier3d = { git = "https://github.com/vrixyz/rapier.git", branch = "uber_physics_context" }
 
 [profile.release]
 opt-level = 'z'
+
+[profile.dev]
+opt-level = 1
diff --git a/build_wasm.sh b/build_wasm.sh
index 8b2f27d..42cdf7e 100755
--- a/build_wasm.sh
+++ b/build_wasm.sh
@@ -1,11 +1,11 @@
 #!/bin/sh
 
-cargo build --example elasticity2 --release --target wasm32-unknown-unknown --features dim2
-cargo build --example elasticity3 --release --target wasm32-unknown-unknown --features dim3
-wasm-bindgen --no-typescript --target web --out-dir dist2d --out-name elasticity2 ./target/wasm32-unknown-unknown/release/examples/elasticity2.wasm
-wasm-bindgen --no-typescript --target web --out-dir dist3d --out-name elasticity3 ./target/wasm32-unknown-unknown/release/examples/elasticity3.wasm
-wasm-opt -Oz -o ./dist2d/opt.wasm ./dist2d/elasticity2_bg.wasm && mv ./dist2d/opt.wasm ./dist2d/elasticity2_bg.wasm
-wasm-opt -Oz -o ./dist3d/opt.wasm ./dist3d/elasticity3_bg.wasm && mv ./dist3d/opt.wasm ./dist3d/elasticity3_bg.wasm
+# cargo build -p wgsparkl-examples-2d --release --target wasm32-unknown-unknown
+cargo build -p wgsparkl-examples-3d --release --target wasm32-unknown-unknown
+# wasm-bindgen --no-typescript --target web --out-dir dist2d --out-name wgsparkl-examples-2d ./target/wasm32-unknown-unknown/release/wgsparkl-examples-2d.wasm
+wasm-bindgen --no-typescript --target web --out-dir dist3d --out-name wgsparkl-examples-3d ./target/wasm32-unknown-unknown/release/wgsparkl-examples-3d.wasm
+# wasm-opt -Oz -o ./dist2d/opt.wasm ./dist2d/wgsparkl-examples-2d_bg.wasm && mv ./dist2d/opt.wasm ./dist2d/wgsparkl-examples-2d_bg.wasm
+wasm-opt -Oz -o ./dist3d/opt.wasm ./dist3d/wgsparkl-examples-3d_bg.wasm && mv ./dist3d/opt.wasm ./dist3d/wgsparkl-examples-3d_bg.wasm
 
-brotli ./dist2d/elasticity2_bg.wasm && mv ./dist2d/elasticity2_bg.wasm.br ./dist2d/elasticity2_bg.wasm
-brotli ./dist3d/elasticity3_bg.wasm && mv ./dist3d/elasticity3_bg.wasm.br ./dist3d/elasticity3_bg.wasm
\ No newline at end of file
+# brotli ./dist2d/wgsparkl-examples-2d_bg.wasm && mv ./dist2d/wgsparkl-examples-2d_bg.wasm.br ./dist2d/wgsparkl-examples-2d_bg.wasm
+brotli ./dist3d/wgsparkl-examples-3d_bg.wasm && mv ./dist3d/wgsparkl-examples-3d_bg.wasm.br ./dist3d/wgsparkl-examples-3d_bg.wasm
\ No newline at end of file
diff --git a/crates/text_to_collider/Cargo.toml b/crates/text_to_collider/Cargo.toml
new file mode 100644
index 0000000..5e797ee
--- /dev/null
+++ b/crates/text_to_collider/Cargo.toml
@@ -0,0 +1,18 @@
+[package]
+name = "text_to_collider"
+version = "0.1.0"
+license = "Apache-2.0 OR Custom"
+edition = "2021"
+
+[lints]
+workspace = true
+
+[dependencies]
+env_logger = "*"
+cosmic-text = "*"
+image = "*"
+parry3d = "*"
+
+[dev-dependencies]
+bevy = "0.15"
+rkyv = "0.8"
diff --git a/crates/text_to_collider/examples/color_positions.rs b/crates/text_to_collider/examples/color_positions.rs
new file mode 100644
index 0000000..136c272
--- /dev/null
+++ b/crates/text_to_collider/examples/color_positions.rs
@@ -0,0 +1,33 @@
+use std::fs;
+
+use bevy::math::Vec2;
+use cosmic_text::Color;
+use parry3d::{na::Vector2, shape::Cuboid};
+use text_to_collider::{get_rects_for, Rect, HEIGHT, WIDTH};
+
+fn main() {
+    let rects = get_rects_for("Wgsparkl 🌊✨");
+    let positions = std::fs::read("particles.bin")
+        .expect("Could not read particles.bin, generate it by running example sand3.");
+    let positions = rkyv::from_bytes::<Vec<[f32; 3]>, rkyv::rancor::Error>(&positions).unwrap();
+    let detection_radius = 2f32;
+    let colors = positions
+        .iter()
+        .map(|p| {
+            // dismiss the y coordinate
+            let pos_to_check =
+                Vec2::new(p[0], -p[2] + 20f32) * 8f32 + Vec2::new(WIDTH, 0f32) / 2f32;
+            if let Some(c) = rects.iter().find(|c| {
+                let c1 = Vec2::new(c.x as f32, c.y as f32);
+                c1.distance_squared(pos_to_check) < detection_radius
+            }) {
+                let color = c.color.as_rgba();
+                [color[0], color[1], color[2]]
+            } else {
+                [0, 55, 200]
+            }
+        })
+        .collect::<Vec<_>>();
+    let particle_bytes = rkyv::to_bytes::<rkyv::rancor::Error>(&colors).unwrap();
+    std::fs::write("particles_colors.bin", &particle_bytes).unwrap();
+}
diff --git a/crates/text_to_collider/examples/display.rs b/crates/text_to_collider/examples/display.rs
new file mode 100644
index 0000000..381bf06
--- /dev/null
+++ b/crates/text_to_collider/examples/display.rs
@@ -0,0 +1,69 @@
+use bevy::{input::common_conditions::input_toggle_active, prelude::*};
+use cosmic_text::Color;
+use parry3d::{na::Vector2, shape::Cuboid};
+use text_to_collider::{get_rects_for, Rect};
+
+fn main() {
+    let rects = get_rects_for("Wgsparkl 🌊✨");
+
+    App::new()
+        .add_plugins(DefaultPlugins)
+        .insert_resource(Cuboids(rects.iter().map(|r| r.to_cuboid(2f32)).collect()))
+        .insert_resource(RectsStorage(rects))
+        .add_systems(Startup, setup)
+        .add_systems(
+            Update,
+            show_rects.run_if(input_toggle_active(true, KeyCode::Space)),
+        )
+        .add_systems(Update, show_cuboids)
+        .run();
+}
+
+#[derive(Resource)]
+pub struct RectsStorage(pub Vec<Rect>);
+
+#[derive(Resource)]
+pub struct Cuboids(pub Vec<(Cuboid, Vector2<f32>, Color)>);
+
+pub fn setup(mut commands: Commands) {
+    commands.spawn((Camera::default(), Camera2d));
+}
+
+pub fn show_rects(mut g: Gizmos, rects: Res<RectsStorage>) {
+    for r in rects.0.iter() {
+        let color = r.color.as_rgba();
+        g.rect(
+            Isometry3d::from_translation(Vec3::new(r.x as f32, r.y as f32, 0f32)),
+            Vec2::new(r.width as f32, r.height as f32),
+            bevy::prelude::Color::srgba_u8(color[0], color[1], color[2], color[3]),
+        );
+    }
+}
+
+pub fn show_cuboids(time: Res<Time>, mut g: Gizmos, cuboids: Res<Cuboids>) {
+    let detection_radius = 1f32;
+    for i in 0..8000 {
+        let t = ((time.elapsed_secs()) + i as f32 * 10.5) * 0.1f32;
+        let pos_to_check = Vec2::new(
+            (((t * (0.5 + (i + 1) as f32 / 5000f32) * 0.7f32).cos() + 1f32) / 2f32) * 500f32,
+            (((t * (0.5 + (i + 1) as f32 / 5000f32) * 0.71f32).cos() + 1f32) / 2f32) * -84f32,
+        );
+        if let Some(c) = cuboids.0.iter().find(|c| {
+            let c1 = Vec2::new(c.1.x, c.1.y);
+            c1.distance_squared(pos_to_check) < detection_radius
+        }) {
+            let color = c.2.as_rgba();
+            g.sphere(
+                Isometry3d::from_translation(pos_to_check.extend(0f32)),
+                detection_radius,
+                bevy::prelude::Color::srgba_u8(color[0], color[1], color[2], color[3]),
+            );
+        } else {
+            g.sphere(
+                Isometry3d::from_translation(pos_to_check.extend(0f32)),
+                detection_radius,
+                bevy::prelude::Color::srgba_u8(200, 0, 0, 255),
+            );
+        }
+    }
+}
diff --git a/crates/text_to_collider/src/lib.rs b/crates/text_to_collider/src/lib.rs
new file mode 100644
index 0000000..1e8516c
--- /dev/null
+++ b/crates/text_to_collider/src/lib.rs
@@ -0,0 +1,96 @@
+// SPDX-License-Identifier: MIT OR Apache-2.0
+
+use cosmic_text::{
+    Attrs, Buffer, Color, Edit, Editor, Family, FontSystem, Metrics, Shaping, SwashCache,
+};
+use parry3d::{
+    na::{Vector2, Vector3},
+    shape::Cuboid,
+};
+
+pub const WIDTH: f32 = 523f32;
+pub const HEIGHT: f32 = 256f32;
+
+pub fn get_rects_for(string: &str) -> Vec<Rect> {
+    let mut font_system = FontSystem::new();
+    let mut swash_cache = SwashCache::new();
+
+    let metrics = Metrics::new(32.0, 44.0);
+    let mut editor = Editor::new(Buffer::new_empty(metrics.scale(1f32)));
+    let mut editor = editor.borrow_with(&mut font_system);
+
+    editor.with_buffer_mut(|buffer| {
+        buffer.set_size(Some(WIDTH), Some(HEIGHT));
+        let attrs = Attrs::new();
+        let comic_attrs = attrs.family(Family::Name("Comic Neue"));
+
+        let spans: &[(&str, Attrs)] =
+            &[(string, comic_attrs.metrics(Metrics::relative(64.0, 1.2)))];
+
+        buffer.set_rich_text(spans.iter().copied(), comic_attrs, Shaping::Advanced);
+    });
+
+    editor.shape_as_needed(true);
+
+    let font_color = Color::rgb(0xFF, 0xFF, 0xFF);
+    let cursor_color = Color(0);
+    let selection_color = Color::rgba(0xFF, 0xFF, 0xFF, 0x33);
+    let selected_text_color = Color::rgb(0xA0, 0xA0, 0xFF);
+    let mut rects = Vec::new();
+    editor.draw(
+        &mut swash_cache,
+        font_color,
+        cursor_color,
+        selection_color,
+        selected_text_color,
+        |x, y, lw, lh, color| {
+            if color.a() == 0 {
+                return;
+            }
+            rects.push(Rect {
+                x,
+                // invert y for bevy.
+                y: -y,
+                width: lw,
+                height: lh,
+                color,
+            })
+        },
+    );
+    rects
+}
+
+#[derive(Debug)]
+pub struct Rect {
+    /// Left of the rect
+    pub x: i32,
+    /// Top of the rect
+    pub y: i32,
+    /// Width of the rect
+    pub width: u32,
+    /// Height of the rect
+    pub height: u32,
+    /// Color of the rect
+    pub color: Color,
+}
+
+impl Rect {
+    pub fn get_center(&self) -> (f32, f32) {
+        (
+            self.x as f32 - self.width as f32 / 2f32,
+            self.y as f32 + self.height as f32 / 2f32,
+        )
+    }
+    pub fn to_cuboid(&self, half_extent_z: f32) -> (Cuboid, Vector2<f32>, Color) {
+        let center = self.get_center();
+        return (
+            Cuboid::new(Vector3::new(
+                self.width as f32 / 2f32,
+                self.height as f32 / 2f32,
+                half_extent_z,
+            )),
+            Vector2::new(center.0, center.1),
+            self.color,
+        );
+    }
+}
diff --git a/crates/wgsparkl-testbed2d/Cargo.toml b/crates/wgsparkl-testbed2d/Cargo.toml
index 803a2eb..483c7e8 100644
--- a/crates/wgsparkl-testbed2d/Cargo.toml
+++ b/crates/wgsparkl-testbed2d/Cargo.toml
@@ -13,8 +13,8 @@ path = "../../src_testbed/lib.rs"
 required-features = ["dim2"]
 
 [features]
+default = ["dim2"]
 dim2 = []
-default_features = ["dim2"]
 
 [dependencies]
 nalgebra = { workspace = true, features = ["rand"] }
@@ -23,17 +23,25 @@ naga_oil = { workspace = true }
 bytemuck = { workspace = true }
 async-channel = { workspace = true }
 
-wgcore = { version = "0.2", path = "../../../wgmath/crates/wgcore" }
-wgebra = { version = "0.2", path = "../../../wgmath/crates/wgebra" }
-wgparry2d = { version = "0.2", path = "../../../wgmath/crates/wgparry/crates/wgparry2d" }
+wgcore = "0.2"
+wgebra = "0.2"
+wgparry2d = "0.2"
 
 futures-test = "0.3"
 serial_test = "3"
 approx = "0.5"
 async-std = { version = "1", features = ["attributes"] }
-bevy = { version = "0.15.0", features = ["shader_format_glsl", "shader_format_spirv", "webgpu"] }
+bevy = { version = "0.15.0", features = [
+    "shader_format_glsl",
+    "shader_format_spirv",
+    "webgpu",
+] }
 #bevy_wasm_window_resize = "0.4"
-#bevy_editor_cam = "0.3"
-#bevy_mod_picking = { version = "0.20", default-features = false }
-bevy_egui = { version = "0.31", default-features = false, features = ["default_fonts", "render"] }
+bevy_editor_cam = "0.5"
+bevy_egui = { version = "0.31", default-features = false, features = [
+    "default_fonts",
+    "render",
+] }
+bevy_sprite = "0.15"
 wgsparkl2d = { path = "../wgsparkl2d" }
+futures = "0.3"
diff --git a/crates/wgsparkl-testbed3d/Cargo.toml b/crates/wgsparkl-testbed3d/Cargo.toml
index 16a267a..d652796 100644
--- a/crates/wgsparkl-testbed3d/Cargo.toml
+++ b/crates/wgsparkl-testbed3d/Cargo.toml
@@ -13,8 +13,8 @@ path = "../../src_testbed/lib.rs"
 required-features = ["dim3"]
 
 [features]
+default = ["dim3"]
 dim3 = []
-default_features = ["dim3"]
 
 [dependencies]
 nalgebra = { workspace = true, features = ["rand"] }
@@ -23,17 +23,25 @@ naga_oil = { workspace = true }
 bytemuck = { workspace = true }
 async-channel = { workspace = true }
 
-wgcore = { version = "0.2", path = "../../../wgmath/crates/wgcore" }
-wgebra = { version = "0.2", path = "../../../wgmath/crates/wgebra" }
-wgparry3d = { version = "0.2", path = "../../../wgmath/crates/wgparry/crates/wgparry3d" }
+wgcore = "0.2"
+wgebra = "0.2"
+wgparry3d = "0.2"
 
 futures-test = "0.3"
 serial_test = "3"
 approx = "0.5"
 async-std = { version = "1", features = ["attributes"] }
-bevy = { version = "0.15.0", features = ["shader_format_glsl", "shader_format_spirv", "webgpu"] }
-#bevy_editor_cam = "0.3"
+bevy = { version = "0.15.0", features = [
+    "shader_format_glsl",
+    "shader_format_spirv",
+    "webgpu",
+] }
+bevy_editor_cam = "0.5"
 #bevy_mod_picking = { version = "0.20", default-features = false }
 #bevy_wasm_window_resize = "0.4"
-bevy_egui = { version = "0.31", default-features = false, features = ["default_fonts", "render"] }
+bevy_egui = { version = "0.31", default-features = false, features = [
+    "default_fonts",
+    "render",
+] }
 wgsparkl3d = { path = "../wgsparkl3d" }
+futures = "0.3"
diff --git a/crates/wgsparkl2d/Cargo.toml b/crates/wgsparkl2d/Cargo.toml
index 09e3f4b..8602b88 100644
--- a/crates/wgsparkl2d/Cargo.toml
+++ b/crates/wgsparkl2d/Cargo.toml
@@ -23,16 +23,14 @@ naga_oil = { workspace = true }
 bytemuck = { workspace = true }
 encase = { workspace = true }
 
-wgcore = { version = "0.2", path = "../../../wgmath/crates/wgcore", features = ["hot_reloading"] }
-wgebra = { version = "0.2", path = "../../../wgmath/crates/wgebra" }
-wgparry2d = { version = "0.2", path = "../../../wgmath/crates/wgparry/crates/wgparry2d" }
-wgrapier2d = { version = "0.2", path = "../../../wgmath/crates/wgrapier/crates/wgrapier2d" }
+wgcore = "0.2"
+wgebra = "0.2"
+wgparry2d = "0.2"
+wgrapier2d = "0.2"
 
-[dev-dependencies]
-nalgebra = { version = "0.33", features = ["rand"] }
-futures-test = "0.3"
-serial_test = "3"
-approx = "0.5"
-async-std = { version = "1", features = ["attributes"] }
-bevy = { version = "0.15.0", features = ["shader_format_glsl", "shader_format_spirv", "webgpu"] }
-wgsparkl_testbed2d = { path = "../wgsparkl-testbed2d" }
+# TODO: make rapier optional?
+rapier2d = "0.23"
+
+# For wasm?
+getrandom = { version = "0.3.1", features = ["wasm_js"] }
+uuid = { version = "1", features = ["js"] }
diff --git a/crates/wgsparkl2d/examples/elasticity2.rs b/crates/wgsparkl2d/examples/elasticity2.rs
deleted file mode 100644
index deebff0..0000000
--- a/crates/wgsparkl2d/examples/elasticity2.rs
+++ /dev/null
@@ -1,224 +0,0 @@
-use wgsparkl_testbed2d::wgsparkl;
-
-use bevy::prelude::*;
-use bevy::render::renderer::RenderDevice;
-use nalgebra::{vector, Similarity2, Vector2};
-use wgebra::GpuSim2;
-use wgparry2d::cuboid::GpuCuboid;
-use wgrapier2d::dynamics::{BodyDesc, GpuVelocity};
-use wgsparkl::models::DruckerPrager;
-use wgsparkl::solver::ParticlePhase;
-use wgsparkl::{
-    models::ElasticCoefficients,
-    pipeline::MpmData,
-    solver::{Particle, ParticleMassProps, SimulationParams},
-};
-use wgsparkl_testbed2d::{init_testbed, AppState, PhysicsContext, SceneInits};
-
-pub fn main() {
-    let mut app = App::new();
-    init_testbed(&mut app);
-    app.add_systems(
-        Startup,
-        (register_scenes, start_default_scene)
-            .chain()
-            .after(wgsparkl_testbed2d::startup::setup_app),
-    );
-    app.run();
-}
-
-fn register_scenes(world: &mut World) {
-    let scenes = vec![
-        ("sand".to_string(), world.register_system(sand_demo)),
-        ("elastic".to_string(), world.register_system(elastic_demo)),
-    ];
-    let mut inits = world.resource_mut::<SceneInits>();
-    inits.scenes = scenes;
-}
-
-fn start_default_scene(mut commands: Commands, scenes: Res<SceneInits>) {
-    scenes.init_scene(&mut commands, 0);
-}
-
-fn sand_demo(mut commands: Commands, device: Res<RenderDevice>, mut app_state: ResMut<AppState>) {
-    let device = device.wgpu_device();
-
-    let offset_y = 40.0;
-    // let cell_width = 0.1;
-    let cell_width = 0.2;
-    let mut particles = vec![];
-    for i in 0..700 {
-        for j in 0..700 {
-            let position = vector![i as f32 + 0.5, j as f32 + 0.5] * cell_width / 2.0
-                + Vector2::y() * offset_y;
-            let density = 1000.0;
-            particles.push(Particle {
-                position,
-                velocity: Vector2::zeros(),
-                volume: ParticleMassProps::new(
-                    density * (cell_width / 2.0).powi(2),
-                    cell_width / 4.0,
-                ),
-                model: ElasticCoefficients::from_young_modulus(10_000_000.0, 0.2),
-                plasticity: Some(DruckerPrager::new(10_000_000.0, 0.2)),
-                phase: None,
-            });
-        }
-    }
-
-    if !app_state.restarting {
-        app_state.num_substeps = 10;
-        app_state.gravity_factor = 1.0;
-    };
-
-    let params = SimulationParams {
-        gravity: vector![0.0, -9.81] * app_state.gravity_factor,
-        dt: (1.0 / 60.0) / (app_state.num_substeps as f32),
-        padding: 0.0,
-    };
-
-    const ANGVEL: f32 = 0.0; // 2.0;
-    let colliders = vec![
-        BodyDesc {
-            shape: GpuCuboid::new(vector![1000.0, 1.0]),
-            pose: GpuSim2::from(Similarity2::new(vector![0.0, -1.0], 0.0, 1.0)),
-            ..Default::default()
-        },
-        BodyDesc {
-            shape: GpuCuboid::new(vector![1.0, 60.0]),
-            pose: GpuSim2::from(Similarity2::new(vector![-20.0, 0.0], 0.5, 1.0)),
-            ..BodyDesc::default()
-        },
-        BodyDesc {
-            shape: GpuCuboid::new(vector![1.0, 60.0]),
-            pose: GpuSim2::from(Similarity2::new(vector![90.0, 0.0], -0.5, 1.0)),
-            ..BodyDesc::default()
-        },
-        BodyDesc {
-            shape: GpuCuboid::new(vector![5.0, 5.0]),
-            pose: GpuSim2::from(Similarity2::new(vector![5.0, 30.0], 0.0, 1.0)),
-            vel: GpuVelocity {
-                linear: Vector2::zeros(),
-                angular: ANGVEL,
-            },
-            ..BodyDesc::default()
-        },
-        BodyDesc {
-            shape: GpuCuboid::new(vector![5.0, 5.0]),
-            pose: GpuSim2::from(Similarity2::new(vector![35.0, 30.0], 0.0, 1.0)),
-            vel: GpuVelocity {
-                linear: Vector2::zeros(),
-                angular: ANGVEL,
-            },
-            ..BodyDesc::default()
-        },
-        BodyDesc {
-            shape: GpuCuboid::new(vector![5.0, 5.0]),
-            pose: GpuSim2::from(Similarity2::new(vector![65.0, 30.0], 0.0, 1.0)),
-            vel: GpuVelocity {
-                linear: Vector2::zeros(),
-                angular: ANGVEL,
-            },
-            ..BodyDesc::default()
-        },
-        BodyDesc {
-            shape: GpuCuboid::new(vector![5.0, 5.0]),
-            pose: GpuSim2::from(Similarity2::new(vector![20.0, 20.0], 0.0, 1.0)),
-            vel: GpuVelocity {
-                linear: Vector2::zeros(),
-                angular: -ANGVEL,
-            },
-            ..BodyDesc::default()
-        },
-        BodyDesc {
-            shape: GpuCuboid::new(vector![5.0, 5.0]),
-            pose: GpuSim2::from(Similarity2::new(vector![50.0, 20.0], 0.0, 1.0)),
-            vel: GpuVelocity {
-                linear: Vector2::zeros(),
-                angular: -ANGVEL,
-            },
-            ..BodyDesc::default()
-        },
-        BodyDesc {
-            shape: GpuCuboid::new(vector![5.0, 5.0]),
-            pose: GpuSim2::from(Similarity2::new(
-                vector![35.0, 10.0],
-                std::f32::consts::PI / 4.0,
-                1.0,
-            )),
-            vel: GpuVelocity {
-                linear: Vector2::zeros(),
-                angular: ANGVEL,
-            },
-            ..BodyDesc::default()
-        },
-    ];
-    let data = MpmData::new(device, params, &particles, &colliders, cell_width, 60_000);
-    commands.insert_resource(PhysicsContext { data, particles });
-}
-
-fn elastic_demo(
-    mut commands: Commands,
-    device: Res<RenderDevice>,
-    mut app_state: ResMut<AppState>,
-) {
-    let device = device.wgpu_device();
-
-    let offset_y = 10.0;
-    // let cell_width = 0.1;
-    let cell_width = 0.2;
-    let mut particles = vec![];
-    for i in 0..700 {
-        for j in 0..700 {
-            let position =
-                vector![i as f32 + 0.5 + (i / 50) as f32 * 2.0, j as f32 + 0.5] * cell_width / 2.0
-                    + Vector2::y() * offset_y;
-            let density = 1000.0;
-            particles.push(Particle {
-                position,
-                velocity: Vector2::zeros(),
-                volume: ParticleMassProps::new(
-                    density * (cell_width / 2.0).powi(2),
-                    cell_width / 4.0,
-                ),
-                model: ElasticCoefficients::from_young_modulus(5_000_000.0, 0.2),
-                plasticity: None,
-                phase: Some(ParticlePhase {
-                    phase: 1.0,
-                    max_stretch: f32::MAX,
-                }),
-            });
-        }
-    }
-
-    if !app_state.restarting {
-        app_state.num_substeps = 15;
-        app_state.gravity_factor = 2.0;
-    };
-
-    let params = SimulationParams {
-        gravity: vector![0.0, -9.81] * app_state.gravity_factor,
-        dt: (1.0 / 60.0) / (app_state.num_substeps as f32),
-        padding: 0.0,
-    };
-
-    let colliders = vec![
-        BodyDesc {
-            shape: GpuCuboid::new(vector![1000.0, 1.0]),
-            pose: GpuSim2::from(Similarity2::new(vector![0.0, -1.0], 0.0, 1.0)),
-            ..Default::default()
-        },
-        BodyDesc {
-            shape: GpuCuboid::new(vector![1.0, 60.0]),
-            pose: GpuSim2::from(Similarity2::new(vector![-20.0, 0.0], 0.5, 1.0)),
-            ..BodyDesc::default()
-        },
-        BodyDesc {
-            shape: GpuCuboid::new(vector![1.0, 60.0]),
-            pose: GpuSim2::from(Similarity2::new(vector![90.0, 0.0], -0.5, 1.0)),
-            ..BodyDesc::default()
-        },
-    ];
-    let data = MpmData::new(device, params, &particles, &colliders, cell_width, 60_000);
-    commands.insert_resource(PhysicsContext { data, particles });
-}
diff --git a/crates/wgsparkl3d/Cargo.toml b/crates/wgsparkl3d/Cargo.toml
index 620e22e..e77cfac 100644
--- a/crates/wgsparkl3d/Cargo.toml
+++ b/crates/wgsparkl3d/Cargo.toml
@@ -15,6 +15,9 @@ required-features = ["dim3"]
 [features]
 default = ["dim3"]
 dim3 = []
+load_gltf = ["obj-rs", "image"]
+load_obj = ["gltf", "image"]
+load_bevy = ["bevy", "image"]
 
 [dependencies]
 nalgebra = { workspace = true }
@@ -23,16 +26,29 @@ naga_oil = { workspace = true }
 bytemuck = { workspace = true }
 encase = { workspace = true }
 
-wgcore = { version = "0.2", path = "../../../wgmath/crates/wgcore", features = ["hot_reloading"] }
-wgebra = { version = "0.2", path = "../../../wgmath/crates/wgebra" }
-wgparry3d = { version = "0.2", path = "../../../wgmath/crates/wgparry/crates/wgparry3d" }
-wgrapier3d = { version = "0.2", path = "../../../wgmath/crates/wgrapier/crates/wgrapier3d" }
-
-[dev-dependencies]
-nalgebra = { version = "0.33", features = ["rand"] }
-futures-test = "0.3"
-serial_test = "3"
-approx = "0.5"
-async-std = { version = "1", features = ["attributes"] }
-bevy = { version = "0.15.0", features = ["shader_format_glsl", "shader_format_spirv", "webgpu"] }
-wgsparkl_testbed3d = { path = "../wgsparkl-testbed3d" }
\ No newline at end of file
+serde = "1"
+ron = "0.8.1"
+wgcore = "0.2"
+wgebra = "0.2"
+wgparry3d = "0.2"
+wgrapier3d = "0.2"
+
+# TODO: make rapier optional?
+rapier3d = "0.23"
+
+# for loaders
+# TODO: slim bevy features
+bevy = { version = "0.15.0", features = [
+    "shader_format_glsl",
+    "shader_format_spirv",
+    "webgpu",
+    "jpeg",
+], optional = true }
+gltf = { version = "1.3", features = [
+    "KHR_materials_pbrSpecularGlossiness",
+], optional = true }
+obj-rs = { version = "0.7", default-features = false, optional = true }
+image = { version = "0.25", optional = true }
+
+[package.metadata.docs.rs]
+cargo-args = ["-Zunstable-options", "-Zrustdoc-scrape-examples"]
diff --git a/crates/wgsparkl3d/examples/elasticity3.rs b/crates/wgsparkl3d/examples/elasticity3.rs
deleted file mode 100644
index 2d276cd..0000000
--- a/crates/wgsparkl3d/examples/elasticity3.rs
+++ /dev/null
@@ -1,87 +0,0 @@
-use wgsparkl_testbed3d::wgsparkl;
-
-use bevy::prelude::*;
-use bevy::render::renderer::RenderDevice;
-use nalgebra::{vector, Similarity3, Vector3};
-use wgebra::GpuSim3;
-use wgparry3d::cuboid::GpuCuboid;
-use wgrapier3d::dynamics::BodyDesc;
-use wgsparkl::models::DruckerPrager;
-use wgsparkl::{
-    models::ElasticCoefficients,
-    pipeline::MpmData,
-    solver::{Particle, ParticleMassProps, SimulationParams},
-};
-use wgsparkl_testbed3d::{init_testbed, AppState, PhysicsContext, SceneInits};
-
-pub fn main() {
-    let mut app = App::new();
-    init_testbed(&mut app);
-    app.add_systems(
-        Startup,
-        (register_scenes, start_default_scene)
-            .chain()
-            .after(wgsparkl_testbed3d::startup::setup_app),
-    );
-    app.run();
-}
-
-fn register_scenes(world: &mut World) {
-    let scenes = vec![("sand".to_string(), world.register_system(sand_demo))];
-    let mut inits = world.resource_mut::<SceneInits>();
-    inits.scenes = scenes;
-}
-
-fn start_default_scene(mut commands: Commands, scenes: Res<SceneInits>) {
-    scenes.init_scene(&mut commands, 0);
-}
-
-fn sand_demo(mut commands: Commands, device: Res<RenderDevice>, mut app_state: ResMut<AppState>) {
-    let device = device.wgpu_device();
-
-    let cell_width = 1.0;
-    let mut particles = vec![];
-    for i in 0..45 {
-        for j in 0..100 {
-            for k in 0..45 {
-                let position = vector![i as f32 + 0.5, j as f32 + 0.5 + 10.0, k as f32 + 0.5]
-                    * cell_width
-                    / 2.0;
-                let density = 2700.0;
-                particles.push(Particle {
-                    position,
-                    velocity: Vector3::zeros(),
-                    volume: ParticleMassProps::new(
-                        density * (cell_width / 2.0).powi(3),
-                        cell_width / 4.0,
-                    ),
-                    model: ElasticCoefficients::from_young_modulus(2_000_000_000.0, 0.2),
-                    plasticity: Some(DruckerPrager::new(2_000_000_000.0, 0.2)),
-                    phase: None,
-                });
-            }
-        }
-    }
-
-    if !app_state.restarting {
-        app_state.num_substeps = 20;
-        app_state.gravity_factor = 1.0;
-    };
-
-    let params = SimulationParams {
-        gravity: vector![0.0, -9.81, 0.0] * app_state.gravity_factor,
-        dt: (1.0 / 60.0) / (app_state.num_substeps as f32),
-    };
-
-    let bodies = vec![BodyDesc {
-        shape: GpuCuboid::new(vector![10000.0, 4.0, 10000.0]),
-        pose: GpuSim3::from(Similarity3::new(
-            vector![0.0, -4.0, 0.0],
-            Vector3::zeros(),
-            1.0,
-        )),
-        ..BodyDesc::default()
-    }];
-    let data = MpmData::new(device, params, &particles, &bodies, cell_width, 60_000);
-    commands.insert_resource(PhysicsContext { data, particles });
-}
diff --git a/dist2d/index.html b/dist2d/index.html
index 0bb9815..3794f6a 100644
--- a/dist2d/index.html
+++ b/dist2d/index.html
@@ -3,7 +3,7 @@
 
 <body style="margin: 0px;">
     <script type="module">
-        import init from './elasticity2.js'
+        import init from './wgsparkl-examples-2d.js'
 
         init().catch((error) => {
             if (!error.message.startsWith("Using exceptions for control flow, don't mind me. This isn't actually an error!")) {
diff --git a/dist3d/index.html b/dist3d/index.html
index 51922e3..48feb87 100644
--- a/dist3d/index.html
+++ b/dist3d/index.html
@@ -3,7 +3,7 @@
 
 <body style="margin: 0px;">
     <script type="module">
-        import init from './elasticity3.js'
+        import init from './wgsparkl-examples-3d.js'
 
         init().catch((error) => {
             if (!error.message.startsWith("Using exceptions for control flow, don't mind me. This isn't actually an error!")) {
diff --git a/examples2d/Cargo.toml b/examples2d/Cargo.toml
new file mode 100644
index 0000000..87a93f2
--- /dev/null
+++ b/examples2d/Cargo.toml
@@ -0,0 +1,24 @@
+[package]
+name = "wgsparkl-examples-2d"
+version = "0.1.0"
+license = "Apache-2.0 OR Custom"
+edition = "2021"
+
+[dependencies]
+nalgebra = { version = "0.33", features = ["rand"] }
+futures-test = "0.3"
+serial_test = "3"
+approx = "0.5"
+async-std = { version = "1", features = ["attributes"] }
+bevy = { version = "0.15.0", features = [
+    "shader_format_glsl",
+    "shader_format_spirv",
+    "webgpu",
+    "jpeg",
+] }
+wgsparkl_testbed2d = { path = "../crates/wgsparkl-testbed2d" }
+wgsparkl2d = { path = "../crates/wgsparkl2d" }
+rapier3d = "0.23"
+
+[lints]
+workspace = true
diff --git a/examples2d/src/elastic_cut2.rs b/examples2d/src/elastic_cut2.rs
new file mode 100644
index 0000000..885dbd4
--- /dev/null
+++ b/examples2d/src/elastic_cut2.rs
@@ -0,0 +1,127 @@
+use wgsparkl_testbed2d::{wgsparkl, RapierData};
+
+use bevy::prelude::*;
+use bevy::render::renderer::RenderDevice;
+use nalgebra::{point, vector, Similarity2, Vector2};
+use rapier2d::prelude::{ColliderBuilder, RigidBodyBuilder};
+use wgebra::GpuSim2;
+use wgparry2d::parry::shape::Cuboid;
+use wgrapier2d::dynamics::{BodyDesc, GpuVelocity};
+use wgsparkl::models::DruckerPrager;
+use wgsparkl::solver::ParticlePhase;
+use wgsparkl::{
+    models::ElasticCoefficients,
+    pipeline::MpmData,
+    solver::{Particle, SimulationParams},
+};
+use wgsparkl2d::solver::ParticleDynamics;
+use wgsparkl_testbed2d::{init_testbed, AppState, PhysicsContext, SceneInits};
+
+fn main() {
+    panic!("Run the `testbed3` example instead.");
+}
+
+pub fn elastic_cut_demo(
+    device: RenderDevice,
+    app_state: &mut AppState,
+    _callbacks: &mut Callbacks,
+) -> PhysicsContext {
+    let mut rapier_data = RapierData::default();
+    let device = device.wgpu_device();
+
+    let offset_y = 46.0;
+    // let cell_width = 0.1;
+    let cell_width = 0.2;
+    let mut particles = vec![];
+    for i in 0..700 {
+        for j in 0..700 {
+            let position = vector![i as f32 + 0.5, j as f32 + 0.5] * cell_width / 2.0
+                + Vector2::y() * offset_y;
+            let density = 1000.0;
+            let radius = cell_width / 4.0;
+            particles.push(Particle {
+                position,
+                dynamics: ParticleDynamics::with_density(radius, density),
+                model: ElasticCoefficients::from_young_modulus(5_000_000.0, 0.2),
+                plasticity: None,
+                phase: Some(ParticlePhase {
+                    phase: 1.0,
+                    max_stretch: f32::MAX,
+                }),
+                color: None,
+            });
+        }
+    }
+
+    if !app_state.restarting {
+        app_state.num_substeps = 15;
+        app_state.gravity_factor = 1.0;
+    };
+
+    let params = SimulationParams {
+        gravity: vector![0.0, -9.81] * app_state.gravity_factor,
+        dt: (1.0 / 60.0) / (app_state.num_substeps as f32),
+        padding: 0.0,
+    };
+
+    const ANGVEL: f32 = 1.0; // 2.0;
+
+    /*
+     * Static platforms.
+     */
+    let rb = RigidBodyBuilder::fixed().translation(vector![35.0, 20.0]);
+    let rb_handle = rapier_data.bodies.insert(rb);
+    let co = ColliderBuilder::cuboid(70.0, 1.0);
+    rapier_data
+        .colliders
+        .insert_with_parent(co, rb_handle, &mut rapier_data.bodies);
+
+    let mut polyline = vec![];
+    let subdivs = 100;
+    let length = 84.0;
+    let start = point![35.0, 70.0] - vector![length / 2.0, 0.0];
+
+    for i in 0..=subdivs {
+        let step = length / (subdivs as f32);
+        let dx = i as f32 * step;
+        polyline.push(start + vector![dx, dx.sin()])
+    }
+
+    let rb = RigidBodyBuilder::fixed();
+    let rb_handle = rapier_data.bodies.insert(rb);
+    let co = ColliderBuilder::polyline(polyline, None).build();
+    rapier_data
+        .colliders
+        .insert_with_parent(co, rb_handle, &mut rapier_data.bodies);
+
+    for k in 0..6 {
+        let rb = RigidBodyBuilder::fixed();
+        let rb_handle = rapier_data.bodies.insert(rb);
+        let co = ColliderBuilder::polyline(
+            vec![
+                point![0.0 + k as f32 * 15.0, 20.0],
+                point![-10.0 + k as f32 * 15.0, 45.0],
+            ],
+            None,
+        )
+        .build();
+        rapier_data
+            .colliders
+            .insert_with_parent(co, rb_handle, &mut rapier_data.bodies);
+    }
+
+    let data = MpmData::new(
+        device,
+        params,
+        &particles,
+        &rapier_data.bodies,
+        &rapier_data.colliders,
+        cell_width,
+        60_000,
+    );
+    PhysicsContext {
+        data,
+        rapier_data,
+        particles,
+    }
+}
diff --git a/examples2d/src/elasticity2.rs b/examples2d/src/elasticity2.rs
new file mode 100644
index 0000000..28d6bc3
--- /dev/null
+++ b/examples2d/src/elasticity2.rs
@@ -0,0 +1,107 @@
+use wgsparkl_testbed2d::{wgsparkl, RapierData};
+
+use bevy::prelude::*;
+use bevy::render::renderer::RenderDevice;
+use nalgebra::{point, vector, Similarity2, Vector2};
+use rapier2d::prelude::{ColliderBuilder, RigidBodyBuilder};
+use wgebra::GpuSim2;
+use wgparry2d::parry::shape::Cuboid;
+use wgrapier2d::dynamics::{BodyDesc, GpuVelocity};
+use wgsparkl::models::DruckerPrager;
+use wgsparkl::solver::ParticlePhase;
+use wgsparkl::{
+    models::ElasticCoefficients,
+    pipeline::MpmData,
+    solver::{Particle, SimulationParams},
+};
+use wgsparkl2d::solver::ParticleDynamics;
+use wgsparkl_testbed2d::{init_testbed, AppState, PhysicsContext, SceneInits};
+
+fn main() {
+    panic!("Run the `testbed3` example instead.");
+}
+
+pub fn elastic_demo(
+    device: RenderDevice,
+    app_state: &mut AppState,
+    _callbacks: &mut Callbacks,
+) -> PhysicsContext {
+    let device = device.wgpu_device();
+    let mut rapier_data = RapierData::default();
+
+    let offset_y = 10.0;
+    // let cell_width = 0.1;
+    let cell_width = 0.2;
+    let mut particles = vec![];
+    for i in 0..700 {
+        for j in 0..700 {
+            let position =
+                vector![i as f32 + 0.5 + (i / 50) as f32 * 2.0, j as f32 + 0.5] * cell_width / 2.0
+                    + Vector2::y() * offset_y;
+            let density = 1000.0;
+            let radius = cell_width / 4.0;
+            particles.push(Particle {
+                position,
+                dynamics: ParticleDynamics::with_density(radius, density),
+                model: ElasticCoefficients::from_young_modulus(5_000_000.0, 0.2),
+                plasticity: None,
+                phase: Some(ParticlePhase {
+                    phase: 1.0,
+                    max_stretch: f32::MAX,
+                }),
+                color: None,
+            });
+        }
+    }
+
+    if !app_state.restarting {
+        app_state.num_substeps = 15;
+        app_state.gravity_factor = 2.0;
+    };
+
+    let params = SimulationParams {
+        gravity: vector![0.0, -9.81] * app_state.gravity_factor,
+        dt: (1.0 / 60.0) / (app_state.num_substeps as f32),
+        padding: 0.0,
+    };
+
+    let rb = RigidBodyBuilder::fixed().translation(vector![0.0, -1.0]);
+    let rb_handle = rapier_data.bodies.insert(rb);
+    let co = ColliderBuilder::cuboid(1000.0, 1.0);
+    rapier_data
+        .colliders
+        .insert_with_parent(co, rb_handle, &mut rapier_data.bodies);
+
+    let rb = RigidBodyBuilder::fixed()
+        .translation(vector![-20.0, 0.0])
+        .rotation(0.5);
+    let rb_handle = rapier_data.bodies.insert(rb);
+    let co = ColliderBuilder::cuboid(1.0, 60.0);
+    rapier_data
+        .colliders
+        .insert_with_parent(co, rb_handle, &mut rapier_data.bodies);
+
+    let rb = RigidBodyBuilder::fixed()
+        .translation(vector![90.0, 0.0])
+        .rotation(-0.5);
+    let rb_handle = rapier_data.bodies.insert(rb);
+    let co = ColliderBuilder::cuboid(1.0, 60.0);
+    rapier_data
+        .colliders
+        .insert_with_parent(co, rb_handle, &mut rapier_data.bodies);
+
+    let data = MpmData::new(
+        device,
+        params,
+        &particles,
+        &rapier_data.bodies,
+        &rapier_data.colliders,
+        cell_width,
+        60_000,
+    );
+    PhysicsContext {
+        data,
+        rapier_data,
+        particles,
+    }
+}
diff --git a/examples2d/src/main.rs b/examples2d/src/main.rs
new file mode 100644
index 0000000..e5373f5
--- /dev/null
+++ b/examples2d/src/main.rs
@@ -0,0 +1,53 @@
+use wgsparkl_testbed2d::{wgsparkl, RapierData};
+
+use bevy::prelude::*;
+use bevy::render::renderer::RenderDevice;
+use nalgebra::{point, vector, Similarity2, Vector2};
+use rapier2d::prelude::{ColliderBuilder, RigidBodyBuilder};
+use wgebra::GpuSim2;
+use wgparry2d::parry::shape::Cuboid;
+use wgrapier2d::dynamics::{BodyDesc, GpuVelocity};
+use wgsparkl::models::DruckerPrager;
+use wgsparkl::solver::ParticlePhase;
+use wgsparkl::{
+    models::ElasticCoefficients,
+    pipeline::MpmData,
+    solver::{Particle, SimulationParams},
+};
+use wgsparkl_testbed2d::{init_testbed, AppState, PhysicsContext, SceneInits};
+
+mod elastic_cut2;
+mod elasticity2;
+mod sand2;
+
+pub fn main() {
+    let mut app = App::new();
+    init_testbed(&mut app);
+    app.add_systems(
+        Startup,
+        (register_scenes, start_default_scene)
+            .chain()
+            .after(wgsparkl_testbed2d::startup::setup_app),
+    );
+    app.run();
+}
+
+fn register_scenes(world: &mut World) {
+    let scenes = vec![
+        ("sand".to_string(), world.register_system(sand2::sand_demo)),
+        (
+            "elastic".to_string(),
+            world.register_system(elasticity2::elastic_demo),
+        ),
+        (
+            "elastic cut".to_string(),
+            world.register_system(elastic_cut2::elastic_cut_demo),
+        ),
+    ];
+    let mut inits = world.resource_mut::<SceneInits>();
+    inits.scenes = scenes;
+}
+
+fn start_default_scene(mut commands: Commands, scenes: Res<SceneInits>) {
+    scenes.init_scene(&mut commands, 0);
+}
diff --git a/examples2d/src/sand2.rs b/examples2d/src/sand2.rs
new file mode 100644
index 0000000..5322c23
--- /dev/null
+++ b/examples2d/src/sand2.rs
@@ -0,0 +1,185 @@
+use wgsparkl_testbed2d::{wgsparkl, RapierData};
+
+use bevy::prelude::*;
+use bevy::render::renderer::RenderDevice;
+use nalgebra::{point, vector, Similarity2, Vector2};
+use rapier2d::prelude::{ColliderBuilder, RigidBodyBuilder};
+use wgebra::GpuSim2;
+use wgparry2d::parry::shape::Cuboid;
+use wgrapier2d::dynamics::{BodyDesc, GpuVelocity};
+use wgsparkl::models::DruckerPrager;
+use wgsparkl::solver::ParticlePhase;
+use wgsparkl::{
+    models::ElasticCoefficients,
+    pipeline::MpmData,
+    solver::{Particle, SimulationParams},
+};
+use wgsparkl2d::solver::ParticleDynamics;
+use wgsparkl_testbed2d::{init_testbed, AppState, PhysicsContext, SceneInits};
+
+fn main() {
+    panic!("Run the `testbed3` example instead.");
+}
+
+pub fn sand_demo(
+    device: RenderDevice,
+    app_state: &mut AppState,
+    _callbacks: &mut Callbacks,
+) -> PhysicsContext {
+    let mut rapier_data = RapierData::default();
+    let device = device.wgpu_device();
+
+    let offset_y = 46.0;
+    // let cell_width = 0.1;
+    let cell_width = 0.2;
+    let mut particles = vec![];
+    for i in 0..700 {
+        for j in 0..700 {
+            let position = vector![i as f32 + 0.5, j as f32 + 0.5] * cell_width / 2.0
+                + Vector2::y() * offset_y;
+            let density = 1000.0;
+            let radius = cell_width / 4.0;
+            particles.push(Particle {
+                position,
+                dynamics: ParticleDynamics::with_density(radius, density),
+                model: ElasticCoefficients::from_young_modulus(10_000_000.0, 0.2),
+                plasticity: Some(DruckerPrager::new(10_000_000.0, 0.2)),
+                phase: None,
+                color: None,
+            });
+        }
+    }
+
+    if !app_state.restarting {
+        app_state.num_substeps = 10;
+        app_state.gravity_factor = 1.0;
+    };
+
+    let params = SimulationParams {
+        gravity: vector![0.0, -9.81] * app_state.gravity_factor,
+        dt: (1.0 / 60.0) / (app_state.num_substeps as f32),
+        padding: 0.0,
+    };
+
+    const ANGVEL: f32 = 1.0; // 2.0;
+
+    /*
+     * Static platforms.
+     */
+    let rb = RigidBodyBuilder::fixed().translation(vector![35.0, -1.0]);
+    let rb_handle = rapier_data.bodies.insert(rb);
+    let co = ColliderBuilder::cuboid(42.0, 1.0);
+    rapier_data
+        .colliders
+        .insert_with_parent(co, rb_handle, &mut rapier_data.bodies);
+
+    let rb = RigidBodyBuilder::fixed()
+        .translation(vector![-25.0, 45.0])
+        .rotation(0.5);
+    let rb_handle = rapier_data.bodies.insert(rb);
+    let co = ColliderBuilder::cuboid(1.0, 52.0);
+    rapier_data
+        .colliders
+        .insert_with_parent(co, rb_handle, &mut rapier_data.bodies);
+
+    let rb = RigidBodyBuilder::fixed()
+        .translation(vector![95.0, 45.0])
+        .rotation(-0.5);
+    let rb_handle = rapier_data.bodies.insert(rb);
+    let co = ColliderBuilder::cuboid(1.0, 52.0);
+    rapier_data
+        .colliders
+        .insert_with_parent(co, rb_handle, &mut rapier_data.bodies);
+
+    /*
+     * Rotating platforms.
+     */
+    let rb = RigidBodyBuilder::kinematic_velocity_based()
+        .translation(vector![5.0, 35.0])
+        .angvel(ANGVEL);
+    let rb_handle = rapier_data.bodies.insert(rb);
+    let co = ColliderBuilder::cuboid(1.0, 10.0);
+    rapier_data
+        .colliders
+        .insert_with_parent(co, rb_handle, &mut rapier_data.bodies);
+
+    let rb = RigidBodyBuilder::kinematic_velocity_based()
+        .translation(vector![35.0, 35.0])
+        .angvel(-ANGVEL);
+    let rb_handle = rapier_data.bodies.insert(rb);
+    let co = ColliderBuilder::cuboid(10.0, 1.0);
+    rapier_data
+        .colliders
+        .insert_with_parent(co, rb_handle, &mut rapier_data.bodies);
+
+    let rb = RigidBodyBuilder::kinematic_velocity_based()
+        .translation(vector![65.0, 35.0])
+        .angvel(ANGVEL);
+    let rb_handle = rapier_data.bodies.insert(rb);
+    let co = ColliderBuilder::cuboid(1.0, 10.0);
+    rapier_data
+        .colliders
+        .insert_with_parent(co, rb_handle, &mut rapier_data.bodies);
+
+    let rb = RigidBodyBuilder::kinematic_velocity_based()
+        .translation(vector![20.0, 20.0])
+        .angvel(-ANGVEL);
+    let rb_handle = rapier_data.bodies.insert(rb);
+    let co = ColliderBuilder::ball(5.0);
+    rapier_data
+        .colliders
+        .insert_with_parent(co, rb_handle, &mut rapier_data.bodies);
+
+    let rb = RigidBodyBuilder::kinematic_velocity_based()
+        .translation(vector![50.0, 20.0])
+        .angvel(-ANGVEL);
+    let rb_handle = rapier_data.bodies.insert(rb);
+    let co = ColliderBuilder::capsule_y(5.0, 3.0);
+    rapier_data
+        .colliders
+        .insert_with_parent(co, rb_handle, &mut rapier_data.bodies);
+
+    // let rb = RigidBodyBuilder::kinematic_velocity_based()
+    //     .translation(vector![30.0, 0.0])
+    //     // .rotation(std::f32::consts::PI / 4.0)
+    //     .angvel(-ANGVEL)
+    //     .linvel(vector![0.0, 4.0]);
+    // let rb_handle = rapier_data.bodies.insert(rb);
+    // let co = ColliderBuilder::cuboid(30.0, 30.0);
+    // rapier_data
+    //     .colliders
+    //     .insert_with_parent(co, rb_handle, &mut rapier_data.bodies);
+
+    for k in 0..8 {
+        let rb = RigidBodyBuilder::dynamic().translation(vector![35.0 + 3.0 * k as f32, 120.0]);
+        let rb_handle = rapier_data.bodies.insert(rb);
+        let co = ColliderBuilder::cuboid(5.0, 1.0).density(10.0 + k as f32 * 100.0);
+        rapier_data
+            .colliders
+            .insert_with_parent(co, rb_handle, &mut rapier_data.bodies);
+    }
+
+    // let rb = RigidBodyBuilder::kinematic_velocity_based()
+    //     .translation(vector![35.0, 120.0])
+    //     .linvel(Vector2::new(0.0, -10.0));
+    // let rb_handle = rapier_data.bodies.insert(rb);
+    // let co = ColliderBuilder::cuboid(4.0, 1.0).density(100.0);
+    // rapier_data
+    //     .colliders
+    //     .insert_with_parent(co, rb_handle, &mut rapier_data.bodies);
+
+    let data = MpmData::new(
+        device,
+        params,
+        &particles,
+        &rapier_data.bodies,
+        &rapier_data.colliders,
+        cell_width,
+        60_000,
+    );
+    PhysicsContext {
+        data,
+        rapier_data,
+        particles,
+    }
+}
diff --git a/examples3d/Cargo.toml b/examples3d/Cargo.toml
new file mode 100644
index 0000000..d5fc873
--- /dev/null
+++ b/examples3d/Cargo.toml
@@ -0,0 +1,35 @@
+[package]
+name = "wgsparkl-examples-3d"
+version = "0.1.0"
+license = "Apache-2.0 OR Custom"
+edition = "2021"
+
+[dependencies]
+nalgebra = { version = "0.33", features = ["rand", "rkyv-serialize"] }
+futures-test = "0.3"
+serial_test = "3"
+approx = "0.5"
+async-std = { version = "1", features = ["attributes"] }
+bevy = { version = "0.15.0", features = [
+    "shader_format_glsl",
+    "shader_format_spirv",
+    "webgpu",
+    "jpeg",
+] }
+wgsparkl_testbed3d = { path = "../crates/wgsparkl-testbed3d" }
+wgsparkl3d = { path = "../crates/wgsparkl3d", features = [
+    "load_gltf",
+    "load_bevy",
+    "load_obj",
+] }
+wgcore = "0.2"
+futures = "0.3"
+obj-rs = { version = "0.7", default-features = false }
+bevy_editor_cam = "*"
+image = "0.25"
+gltf = { version = "1.3", features = ["KHR_materials_pbrSpecularGlossiness"] }
+rapier3d = "0.23"
+rkyv = "0.8"
+
+[lints]
+workspace = true
diff --git a/examples3d/assets/banana.glb b/examples3d/assets/banana.glb
new file mode 100644
index 0000000..104c1d6
Binary files /dev/null and b/examples3d/assets/banana.glb differ
diff --git a/examples3d/assets/chefs_knife_open_blade.glb b/examples3d/assets/chefs_knife_open_blade.glb
new file mode 100644
index 0000000..7374bc9
Binary files /dev/null and b/examples3d/assets/chefs_knife_open_blade.glb differ
diff --git a/examples3d/assets/credits.txt b/examples3d/assets/credits.txt
new file mode 100644
index 0000000..c7edf64
--- /dev/null
+++ b/examples3d/assets/credits.txt
@@ -0,0 +1,8 @@
+# Thanks 
+
+- Shiba from zixisun02 at https://sketchfab.com/3d-models/shiba-faef9fe5ace445e7b2989d1c1ece361c
+- Banana from ReCo_3D at https://sketchfab.com/3d-models/banana-ed0537c2201d4ad7b8ddb8368d844174
+- Chef's knife from Davide Specchi at https://sketchfab.com/3d-models/chefs-knife-85ededce157545e68e51fa87768a08cb
+  - modifications:
+    - set origin to the middle of the blade + rotated so blade is facing Y down.
+    - inverted faces on a blade side to properly cut
diff --git a/examples3d/assets/shiba.glb b/examples3d/assets/shiba.glb
new file mode 100644
index 0000000..1fc2d6b
Binary files /dev/null and b/examples3d/assets/shiba.glb differ
diff --git a/examples3d/examples/glb_to_point_cloud.rs b/examples3d/examples/glb_to_point_cloud.rs
new file mode 100644
index 0000000..a8bd54f
--- /dev/null
+++ b/examples3d/examples/glb_to_point_cloud.rs
@@ -0,0 +1,71 @@
+//! This example demonstrates how to load a GLB file and display it as a point cloud.
+//!
+//! No MPM simulation is involved here.
+
+use bevy::{
+    app::{App, Startup, Update},
+    core_pipeline::core_3d::Camera3d,
+    ecs::system::Commands,
+    math::Vec3,
+    prelude::*,
+    render::camera::Camera,
+    DefaultPlugins,
+};
+use bevy_editor_cam::{prelude::EditorCam, DefaultEditorCamPlugins};
+use nalgebra::{Isometry3, Transform3, UnitQuaternion, Vector3};
+use std::{fs::File, io::Read};
+use wgsparkl3d::load_mesh3d::load_gltf::load_model_with_colors;
+
+fn main() {
+    App::new()
+        .add_plugins((DefaultPlugins, DefaultEditorCamPlugins))
+        .add_systems(Startup, init_scene)
+        .add_systems(Update, display_point_cloud)
+        .run();
+}
+
+#[derive(Component)]
+pub struct PointCloud {
+    pub positions: Vec<(Vec3, Color)>,
+}
+
+pub const SAMPLE_PER_UNIT: f32 = 20.0;
+
+fn init_scene(mut commands: Commands) {
+    commands.spawn((Camera3d::default(), Camera::default(), EditorCam::default()));
+
+    let mut file = File::open("assets/shiba.glb").expect("Failed to open GLB file");
+    let mut buffer = Vec::new();
+    file.read_to_end(&mut buffer).expect("Failed to read file");
+    let pc_grid = load_model_with_colors(
+        &buffer,
+        Transform3::from_matrix_unchecked(
+            Isometry3::from_parts(
+                Vector3::new(0.0, 0.0, 0.0).into(),
+                UnitQuaternion::identity(),
+            )
+            .to_matrix()
+            .scale(3.0),
+        ),
+        None,
+        SAMPLE_PER_UNIT,
+    );
+
+    commands.spawn(PointCloud {
+        positions: pc_grid
+            .iter()
+            .map(|p| {
+                let pos = Vec3::new(p.0.x, p.0.y, p.0.z);
+                (pos, Color::from(Srgba::from_u8_array(p.1)))
+            })
+            .collect::<Vec<_>>(),
+    });
+}
+
+fn display_point_cloud(pcs: Query<&PointCloud>, mut gizmos: Gizmos) {
+    for pc in pcs.iter() {
+        for p in pc.positions.iter() {
+            gizmos.sphere(p.0, 1f32 / SAMPLE_PER_UNIT / 2f32, p.1);
+        }
+    }
+}
diff --git a/examples3d/src/banana3.rs b/examples3d/src/banana3.rs
new file mode 100644
index 0000000..d23b6c0
--- /dev/null
+++ b/examples3d/src/banana3.rs
@@ -0,0 +1,186 @@
+use std::fs::File;
+use std::io::Read;
+
+use crate::utils::default_scene::{self, SAMPLE_PER_UNIT};
+
+use bevy::color::palettes::css;
+use bevy::{prelude::*, render::renderer::RenderDevice};
+use nalgebra::{vector, Isometry3, Transform3, UnitQuaternion, Vector3};
+use rapier3d::prelude::RigidBodyHandle;
+use rapier3d::prelude::{ColliderBuilder, RigidBodyBuilder, SharedShape, TriMeshFlags};
+use wgsparkl3d::load_mesh3d::load_gltf::{load_model_trimeshes, load_model_with_colors};
+use wgsparkl3d::pipeline::MpmData;
+use wgsparkl3d::solver::SimulationParams;
+use wgsparkl_testbed3d::Callbacks;
+use wgsparkl_testbed3d::{AppState, PhysicsContext};
+use wgsparkl_testbed3d::{Callback, RapierData};
+
+pub fn demo(
+    device: RenderDevice,
+    app_state: &mut AppState,
+    callbacks: &mut Callbacks,
+) -> PhysicsContext {
+    let mut file = File::open("assets/banana.glb").expect("Failed to open GLB file");
+    let mut buffer = Vec::new();
+    file.read_to_end(&mut buffer).expect("Failed to read file");
+
+    let pc_grid = load_model_with_colors(
+        &buffer,
+        Transform3::from_matrix_unchecked(
+            Isometry3::from_parts(
+                Vector3::new(0.0, 2.4, 0.0).into(),
+                UnitQuaternion::from_axis_angle(&Vector3::y_axis(), -90f32.to_radians()),
+            )
+            .to_matrix()
+                * nalgebra::Matrix4::new_scaling(0.3),
+        ),
+        Some(css::BLANCHED_ALMOND.to_u8_array()),
+        SAMPLE_PER_UNIT,
+    )
+    .iter()
+    .map(|p| {
+        (
+            Vec3::new(p.0.x, p.0.y, p.0.z),
+            Color::from(Srgba::from_u8_array(p.1)),
+        )
+    })
+    .collect::<Vec<_>>();
+
+    let params = SimulationParams {
+        gravity: vector![0.0, -9.81, 0.0] * app_state.gravity_factor,
+        dt: (1.0 / 60.0) / (app_state.num_substeps as f32),
+    };
+    let mut rapier_data = RapierData::default();
+    if !app_state.restarting {
+        app_state.num_substeps = 30;
+        app_state.gravity_factor = 1.0;
+    };
+    default_scene::spawn_ground_and_walls(&mut rapier_data);
+
+    let mut particles = vec![];
+    for (pos, color) in pc_grid {
+        let particle =
+            default_scene::create_particle(&pos, Some(color), 1f32 / SAMPLE_PER_UNIT / 2f32);
+        particles.push(particle);
+    }
+    // Slicer
+    let mut file =
+        File::open("assets/chefs_knife_open_blade.glb").expect("Failed to open GLB file");
+    let mut buffer = Vec::new();
+    file.read_to_end(&mut buffer).expect("Failed to read file");
+    let mut slicer_trimeshes = load_model_trimeshes(&buffer);
+    slicer_trimeshes.iter_mut().for_each(|trimesh| {
+        trimesh.0.iter_mut().for_each(|v| {
+            *v *= 10f32;
+        });
+    });
+
+    let rb = RigidBodyBuilder::kinematic_position_based().translation(vector![0.0, 5.0, 0.0]);
+    let parent_handle = rapier_data.bodies.insert(rb);
+    /*
+    // NOTE: Heightfield may be more efficient or more predictable than trimesh for the knife.
+    let heights = nalgebra::DMatrix::zeros(10, 5);
+    let heightfield = rapier3d::prelude::HeightField::new(heights, vector![2.0, 1.0, 10.0]);
+    let (mut vtx, idx) = heightfield.to_trimesh();
+    vtx.iter_mut().for_each(|pt| {
+        *pt = Isometry3::rotation(vector![0f32, 0f32, -90f32.to_radians()]) * *pt
+            + vector![0.0, 1f32, 0f32]
+    });
+    let co = ColliderBuilder::trimesh(vtx, idx).unwrap();
+    rapier_data
+        .colliders
+        .insert_with_parent(co, parent_handle, &mut rapier_data.bodies);
+    */
+    for (vertices, indices) in slicer_trimeshes.iter() {
+        // Insert collider into rapier state.
+
+        let collider = ColliderBuilder::new(
+            SharedShape::trimesh_with_flags(
+                vertices.clone(),
+                indices
+                    .chunks_exact(3)
+                    .map(|i| [i[0] as u32, i[1] as u32, i[2] as u32])
+                    .collect(),
+                TriMeshFlags::FIX_INTERNAL_EDGES,
+            )
+            .unwrap(),
+        );
+        rapier_data
+            .colliders
+            .insert_with_parent(collider, parent_handle, &mut rapier_data.bodies);
+    }
+
+    let data = MpmData::new(
+        device.wgpu_device(),
+        params,
+        &particles,
+        &rapier_data.bodies,
+        &rapier_data.colliders,
+        1f32 / default_scene::SAMPLE_PER_UNIT,
+        60_000,
+    );
+
+    let new_physics = PhysicsContext {
+        data,
+        rapier_data,
+        particles,
+    };
+    //
+    callbacks.0.push(move_knife_function(parent_handle));
+
+    new_physics
+}
+
+fn move_knife_function(body_handle: RigidBodyHandle) -> Callback {
+    Box::new(
+        move |_render,
+              physics: &mut PhysicsContext,
+              _timestamps,
+              app_state: &AppState,
+              _render_queue| {
+            let t = app_state.physics_time_seconds as f32;
+
+            let body = physics.rapier_data.bodies.get_mut(body_handle).unwrap();
+            let length = 1.31;
+            let width = 0.35;
+            let x_pos = 2.8;
+            let y_pos = 1.3;
+            let z_pos = -1.5;
+            let velocity = 0.9;
+            let extended_width = 0.15; // extended width for the horizontal move to the right
+            let period =
+                (2f32 * length + width + extended_width + 2f32 * width + extended_width) / velocity;
+
+            let i = (t / period).floor();
+            let dis = velocity * (t - period * i);
+
+            // Determine the new position of the knife based on the current displacement
+            let new_pos = if dis < length {
+                // Moving downwards
+                vector![x_pos - width * i, y_pos - dis, z_pos]
+            } else if length <= dis && dis < length + width + extended_width {
+                // Moving horizontally to the right with extended width
+                vector![x_pos - width * i + (dis - length), y_pos - length, z_pos]
+            } else if length + width + extended_width <= dis
+                && dis < 2f32 * length + width + extended_width
+            {
+                // Moving upwards
+                vector![
+                    x_pos - width * i + width + extended_width,
+                    y_pos - length + dis - (length + width + extended_width),
+                    z_pos
+                ]
+            } else {
+                // Moving horizontally to the left, back to the starting x position
+                vector![
+                    x_pos - width * i + width + extended_width
+                        - (dis - 2.0 * length - width - extended_width),
+                    y_pos,
+                    z_pos
+                ]
+            };
+
+            body.set_translation(new_pos, true);
+        },
+    )
+}
diff --git a/examples3d/src/elastic_cut3.rs b/examples3d/src/elastic_cut3.rs
new file mode 100644
index 0000000..ccb0876
--- /dev/null
+++ b/examples3d/src/elastic_cut3.rs
@@ -0,0 +1,100 @@
+use wgsparkl_testbed3d::{wgsparkl, Callbacks, RapierData};
+
+use bevy::render::renderer::RenderDevice;
+use nalgebra::{vector, DMatrix, Isometry3};
+use rapier3d::geometry::HeightField;
+use rapier3d::prelude::{ColliderBuilder, RigidBodyBuilder};
+use wgsparkl::{
+    models::ElasticCoefficients,
+    pipeline::MpmData,
+    solver::{Particle, ParticleDynamics, ParticlePhase, SimulationParams},
+};
+use wgsparkl_testbed3d::{AppState, PhysicsContext};
+
+pub fn elastic_cut_demo(
+    device: RenderDevice,
+    app_state: &mut AppState,
+    _callbacks: &mut Callbacks,
+) -> PhysicsContext {
+    let mut rapier_data = RapierData::default();
+    let device = device.wgpu_device();
+
+    let nxz = 50;
+    let cell_width = 1.0;
+    let mut particles = vec![];
+    for i in 0..nxz {
+        for j in 0..30 {
+            for k in 0..nxz {
+                let position = vector![
+                    i as f32 + 0.5 - nxz as f32 / 2.0,
+                    j as f32 + 0.5 + 60.0,
+                    k as f32 + 0.5 - nxz as f32 / 2.0
+                ] * cell_width
+                    / 2.0;
+                let density = 2700.0;
+                let radius = cell_width / 4.0;
+                particles.push(Particle {
+                    position,
+                    dynamics: ParticleDynamics::with_density(radius, density),
+                    model: ElasticCoefficients::from_young_modulus(10_000_000.0, 0.2),
+                    plasticity: None,
+                    phase: Some(ParticlePhase {
+                        phase: 1.0,
+                        max_stretch: f32::MAX,
+                    }),
+                    color: None,
+                });
+            }
+        }
+    }
+
+    if !app_state.restarting {
+        app_state.num_substeps = 20;
+        app_state.gravity_factor = 4.0;
+    };
+
+    let params = SimulationParams {
+        gravity: vector![0.0, -9.81, 0.0] * app_state.gravity_factor,
+        dt: (1.0 / 60.0) / (app_state.num_substeps as f32),
+    };
+
+    let rb = RigidBodyBuilder::fixed().translation(vector![0.0, -4.0, 0.0]);
+    let rb_handle = rapier_data.bodies.insert(rb);
+    let co = ColliderBuilder::cuboid(100.0, 1.0, 100.0);
+    rapier_data
+        .colliders
+        .insert_with_parent(co, rb_handle, &mut rapier_data.bodies);
+
+    // TODO: use only two rectangle per cutting tool.
+    //       We can’t right now since we don’t really sample the triangles.
+    for k in 0..3 {
+        let heights = DMatrix::zeros(10, 10);
+        let heightfield = HeightField::new(heights, vector![35.0, 1.0, 10.0]);
+        let (mut vtx, idx) = heightfield.to_trimesh();
+        vtx.iter_mut().for_each(|pt| {
+            *pt = Isometry3::rotation(vector![1.3, 0.0, 0.0]) * *pt
+                + vector![0.0, 10.0, k as f32 * 10.0 - 10.0]
+        });
+        let rb = RigidBodyBuilder::fixed();
+        let rb_handle = rapier_data.bodies.insert(rb);
+        let co = ColliderBuilder::trimesh(vtx, idx).unwrap();
+        rapier_data
+            .colliders
+            .insert_with_parent(co, rb_handle, &mut rapier_data.bodies);
+    }
+
+    let data = MpmData::new(
+        device,
+        params,
+        &particles,
+        &rapier_data.bodies,
+        &rapier_data.colliders,
+        cell_width,
+        60_000,
+    );
+    PhysicsContext {
+        data,
+        rapier_data,
+        particles,
+    }
+}
diff --git a/examples3d/src/glb_to_point_cloud_color.rs b/examples3d/src/glb_to_point_cloud_color.rs
new file mode 100644
index 0000000..5e03b8a
--- /dev/null
+++ b/examples3d/src/glb_to_point_cloud_color.rs
@@ -0,0 +1,67 @@
+use crate::utils::default_scene::{self, SAMPLE_PER_UNIT};
+
+use bevy::{math::Vec3, prelude::*, render::renderer::RenderDevice};
+use nalgebra::{vector, Isometry3, Transform3, UnitQuaternion, Vector3};
+use std::{fs::File, io::Read};
+use wgsparkl3d::load_mesh3d::load_gltf::load_model_with_colors;
+use wgsparkl3d::{pipeline::MpmData, solver::SimulationParams};
+use wgsparkl_testbed3d::{AppState, Callbacks, PhysicsContext, RapierData};
+
+pub fn elastic_color_model_demo(
+    device: RenderDevice,
+    app_state: &mut AppState,
+    _callbacks: &mut Callbacks,
+) -> PhysicsContext {
+    let mut file = File::open("assets/shiba.glb").expect("Failed to open GLB file");
+    let mut buffer = Vec::new();
+    file.read_to_end(&mut buffer).expect("Failed to read file");
+    let pc_grid = load_model_with_colors(
+        &buffer,
+        Transform3::from_matrix_unchecked(
+            Isometry3::from_parts(
+                Vector3::new(0.0, 6.0, 0.0).into(),
+                UnitQuaternion::identity(),
+            )
+            .to_matrix()
+                * nalgebra::Matrix4::new_scaling(3.0),
+        ),
+        None,
+        SAMPLE_PER_UNIT,
+    );
+    let params = SimulationParams {
+        gravity: vector![0.0, -9.81, 0.0] * app_state.gravity_factor,
+        dt: (1.0 / 60.0) / (app_state.num_substeps as f32),
+    };
+    let mut rapier_data = RapierData::default();
+    if !app_state.restarting {
+        app_state.num_substeps = 20;
+        app_state.gravity_factor = 1.0;
+    };
+    default_scene::spawn_ground_and_walls(&mut rapier_data);
+
+    let mut particles = vec![];
+    for (pos, color) in pc_grid {
+        let particle = default_scene::create_particle(
+            &Vec3::new(pos.x, pos.y, pos.z),
+            Some(Color::from(Srgba::from_u8_array(color))),
+            1f32 / SAMPLE_PER_UNIT / 2f32,
+        );
+        particles.push(particle);
+    }
+    default_scene::spawn_ground_and_walls(&mut rapier_data);
+    let data = MpmData::new(
+        device.wgpu_device(),
+        params,
+        &particles,
+        &rapier_data.bodies,
+        &rapier_data.colliders,
+        1f32 / default_scene::SAMPLE_PER_UNIT,
+        60_000,
+    );
+
+    PhysicsContext {
+        data,
+        rapier_data,
+        particles,
+    }
+}
diff --git a/examples3d/src/heightfield3.rs b/examples3d/src/heightfield3.rs
new file mode 100644
index 0000000..152d1ca
--- /dev/null
+++ b/examples3d/src/heightfield3.rs
@@ -0,0 +1,85 @@
+use wgsparkl_testbed3d::{wgsparkl, Callbacks, RapierData};
+
+use bevy::render::renderer::RenderDevice;
+use nalgebra::{vector, DMatrix};
+use rapier3d::geometry::HeightField;
+use rapier3d::prelude::{ColliderBuilder, RigidBodyBuilder};
+use wgsparkl::models::DruckerPrager;
+use wgsparkl::{
+    models::ElasticCoefficients,
+    pipeline::MpmData,
+    solver::{Particle, ParticleDynamics, SimulationParams},
+};
+use wgsparkl_testbed3d::{AppState, PhysicsContext};
+
+pub fn heightfield_demo(
+    device: RenderDevice,
+    app_state: &mut AppState,
+    _callbacks: &mut Callbacks,
+) -> PhysicsContext {
+    let mut rapier_data = RapierData::default();
+    let device = device.wgpu_device();
+
+    let nxz = 45;
+    let cell_width = 1.0;
+    let mut particles = vec![];
+    for i in 0..nxz {
+        for j in 0..100 {
+            for k in 0..nxz {
+                let position = vector![
+                    i as f32 + 0.5 - nxz as f32 / 2.0,
+                    j as f32 + 0.5 + 10.0,
+                    k as f32 + 0.5 - nxz as f32 / 2.0
+                ] * cell_width
+                    / 2.0;
+                let density = 2700.0;
+                let radius = cell_width / 4.0;
+                particles.push(Particle {
+                    position,
+                    dynamics: ParticleDynamics::with_density(radius, density),
+                    model: ElasticCoefficients::from_young_modulus(2_000_000_000.0, 0.2),
+                    plasticity: Some(DruckerPrager::new(2_000_000_000.0, 0.2)),
+                    phase: None,
+                    color: None,
+                });
+            }
+        }
+    }
+
+    if !app_state.restarting {
+        app_state.num_substeps = 20;
+        app_state.gravity_factor = 1.0;
+    };
+
+    let params = SimulationParams {
+        gravity: vector![0.0, -9.81, 0.0] * app_state.gravity_factor,
+        dt: (1.0 / 60.0) / (app_state.num_substeps as f32),
+    };
+
+    let heights = DMatrix::from_fn(200, 200, |i, j| {
+        (i as f32 / 10.0).sin() * (j as f32 / 10.0).cos()
+    });
+    let heightfield = HeightField::new(heights, vector![100.0, 5.0, 100.0]);
+    let (vtx, idx) = heightfield.to_trimesh();
+    let rb = RigidBodyBuilder::fixed();
+    let rb_handle = rapier_data.bodies.insert(rb);
+    let co = ColliderBuilder::trimesh(vtx, idx).unwrap();
+    rapier_data
+        .colliders
+        .insert_with_parent(co, rb_handle, &mut rapier_data.bodies);
+
+    let data = MpmData::new(
+        device,
+        params,
+        &particles,
+        &rapier_data.bodies,
+        &rapier_data.colliders,
+        cell_width,
+        60_000,
+    );
+    PhysicsContext {
+        data,
+        rapier_data,
+        particles,
+    }
+}
diff --git a/examples3d/src/main.rs b/examples3d/src/main.rs
new file mode 100644
index 0000000..09158a5
--- /dev/null
+++ b/examples3d/src/main.rs
@@ -0,0 +1,47 @@
+use bevy::prelude::*;
+use wgsparkl_testbed3d::{init_testbed, SceneInitFn, SceneInits};
+
+pub mod utils;
+
+mod banana3;
+mod elastic_cut3;
+mod glb_to_point_cloud_color;
+mod heightfield3;
+mod sand3;
+
+pub fn main() {
+    let mut app = App::new();
+    init_testbed(&mut app);
+    app.add_systems(
+        Startup,
+        (register_scenes, start_default_scene)
+            .chain()
+            .after(wgsparkl_testbed3d::startup::setup_app),
+    );
+    app.run();
+}
+
+fn register_scenes(world: &mut World) {
+    let scenes: Vec<(String, SceneInitFn)> = vec![
+        ("sand".to_string(), Box::new(sand3::sand_demo)),
+        (
+            "heightfield".to_string(),
+            Box::new(heightfield3::heightfield_demo),
+        ),
+        (
+            "elastic_cut".to_string(),
+            Box::new(elastic_cut3::elastic_cut_demo),
+        ),
+        (
+            "elastic_model_colors".to_string(),
+            Box::new(glb_to_point_cloud_color::elastic_color_model_demo),
+        ),
+        ("taichi_banana".to_string(), Box::new(banana3::demo)),
+    ];
+    let mut inits = world.resource_mut::<SceneInits>();
+    inits.scenes = scenes;
+}
+
+fn start_default_scene(mut commands: Commands, scenes: Res<SceneInits>) {
+    scenes.init_scene(&mut commands, 0);
+}
diff --git a/examples3d/src/sand3.rs b/examples3d/src/sand3.rs
new file mode 100644
index 0000000..ad31252
--- /dev/null
+++ b/examples3d/src/sand3.rs
@@ -0,0 +1,196 @@
+use bevy::log::warn;
+use wgsparkl_testbed3d::step::callbacks;
+use wgsparkl_testbed3d::{wgsparkl, Callbacks, RapierData};
+
+use bevy::render::renderer::RenderDevice;
+use nalgebra::vector;
+use rapier3d::prelude::{ColliderBuilder, RigidBodyBuilder};
+use wgsparkl::models::DruckerPrager;
+use wgsparkl::{
+    models::ElasticCoefficients,
+    pipeline::MpmData,
+    solver::{Particle, ParticleDynamics, SimulationParams},
+};
+use wgsparkl_testbed3d::{AppState, PhysicsContext};
+
+pub fn sand_demo(
+    render_device: RenderDevice,
+    app_state: &mut AppState,
+    callbacks: &mut Callbacks,
+) -> PhysicsContext {
+    let mut rapier_data = RapierData::default();
+    let captured_render_device = render_device.clone();
+    let device = render_device.wgpu_device();
+
+    let nxz = 45;
+    let cell_width = 1.0;
+    let mut particles = vec![];
+
+    let colors = std::fs::read("particles_colors.bin");
+    let colors = if let Ok(colors) = colors {
+        Some(rkyv::from_bytes::<Vec<[u8; 3]>, rkyv::rancor::Error>(&colors).unwrap())
+    } else {
+        warn!("Could not find particles_colors.bin, generate it with `cargo run --example color_positions`.");
+        None
+    };
+
+    let mut particle_index = 0;
+    for i in 0..nxz {
+        for j in 0..100 {
+            for k in 0..nxz {
+                let position = vector![
+                    i as f32 + 0.5 - nxz as f32 / 2.0,
+                    j as f32 + 0.5 + 10.0,
+                    k as f32 + 0.5 - nxz as f32 / 2.0
+                ] * cell_width
+                    / 2.0;
+                let density = 2700.0;
+                let radius = cell_width / 4.0;
+                particles.push(Particle {
+                    position,
+                    dynamics: ParticleDynamics::with_density(radius, density),
+                    model: ElasticCoefficients::from_young_modulus(2_000_000_000.0, 0.2),
+                    plasticity: Some(DruckerPrager::new(2_000_000_000.0, 0.2)),
+                    phase: None,
+                    color: colors.as_ref().map(|colors| {
+                        [
+                            colors[particle_index][0],
+                            colors[particle_index][1],
+                            colors[particle_index][2],
+                            255,
+                        ]
+                    }),
+                });
+                particle_index += 1;
+            }
+        }
+    }
+
+    if !app_state.restarting {
+        app_state.num_substeps = 20;
+        app_state.gravity_factor = 1.0;
+    };
+
+    let params = SimulationParams {
+        gravity: vector![0.0, -9.81, 0.0] * app_state.gravity_factor,
+        dt: (1.0 / 60.0) / (app_state.num_substeps as f32),
+    };
+
+    rapier_data.insert_body_and_collider(
+        RigidBodyBuilder::fixed().translation(vector![0.0, -4.0, 0.0]),
+        ColliderBuilder::cuboid(100.0, 4.0, 100.0),
+    );
+
+    rapier_data.insert_body_and_collider(
+        RigidBodyBuilder::fixed().translation(vector![0.0, 5.0, -35.0]),
+        ColliderBuilder::cuboid(35.0, 5.0, 0.5),
+    );
+    rapier_data.insert_body_and_collider(
+        RigidBodyBuilder::fixed().translation(vector![0.0, 5.0, 35.0]),
+        ColliderBuilder::cuboid(35.0, 5.0, 0.5),
+    );
+    rapier_data.insert_body_and_collider(
+        RigidBodyBuilder::fixed().translation(vector![-35.0, 5.0, 0.0]),
+        ColliderBuilder::cuboid(0.5, 5.0, 35.0),
+    );
+    rapier_data.insert_body_and_collider(
+        RigidBodyBuilder::fixed().translation(vector![35.0, 5.0, 0.0]),
+        ColliderBuilder::cuboid(0.5, 5.0, 35.0),
+    );
+
+    let rb = RigidBodyBuilder::kinematic_velocity_based()
+        .translation(vector![0.0, 1.5, 0.0])
+        .rotation(vector![0.0, 0.0, -0.5])
+        .angvel(vector![0.0, -1.0, 0.0]);
+    let co = ColliderBuilder::cuboid(0.5, 1.5, 30.0);
+    let (rb_moving_helix, _) = rapier_data.insert_body_and_collider(rb, co);
+
+    let mut step_left_before_stop = 600;
+    callbacks.0.push(Box::new(
+        move |_render,
+              physics: &mut PhysicsContext,
+              _timestamps,
+              _app_state: &AppState,
+              render_queue| {
+            step_left_before_stop -= 1;
+            if step_left_before_stop < 0 {
+                physics
+                    .rapier_data
+                    .bodies
+                    .get_mut(rb_moving_helix)
+                    .unwrap()
+                    .set_angvel(vector![0.0, 0.0, 0.0], false);
+            }
+            if step_left_before_stop == -300 {
+                let particles_positions = read_particles::read_particles_positions(
+                    captured_render_device.clone(),
+                    render_queue,
+                    &physics.data.particles,
+                );
+                // FIXME: when wgsparkl updates to rkyv 0.8, we can get rid of the mapping.
+                let particle_bytes = rkyv::to_bytes::<rkyv::rancor::Error>(
+                    &particles_positions
+                        .iter()
+                        .map(|p| [p.x, p.y, p.z])
+                        .collect::<Vec<_>>(),
+                )
+                .unwrap();
+                std::fs::write("particles.bin", &particle_bytes).unwrap();
+                println!("Exported particles.bin");
+            }
+        },
+    ));
+
+    let data = MpmData::new(
+        device,
+        params,
+        &particles,
+        &rapier_data.bodies,
+        &rapier_data.colliders,
+        cell_width,
+        60_000,
+    );
+    PhysicsContext {
+        data,
+        rapier_data,
+        particles,
+    }
+}
+
+pub mod read_particles {
+    use super::*;
+
+    use bevy::render::{render_resource::BufferUsages, renderer::RenderQueue};
+    use nalgebra::Vector4;
+    use wgcore::tensor::GpuVector;
+    use wgsparkl3d::solver::GpuParticles;
+
+    pub fn read_particles_positions(
+        render_device: RenderDevice,
+        render_queue: RenderQueue,
+        particles: &GpuParticles,
+    ) -> Vec<Vector4<f32>> {
+        // Get the `render_device` is the `Res<RenderDevice>` bevy resource.
+        let device = render_device.wgpu_device();
+        // Create the staging buffer.
+        // Here `particles` is of type `GpuParticles`, accessible from
+        // `PhysicsContext::data::particles`.
+        let positions_staging: GpuVector<Vector4<f32>> = GpuVector::uninit(
+            device,
+            particles.len() as u32,
+            BufferUsages::COPY_DST | BufferUsages::MAP_READ,
+        );
+
+        // Copy the buffer.
+        // `render_queue` the `Res<RenderQueue>` bevy resource.
+        let compute_queue = &*render_queue.0;
+        let mut encoder = device.create_command_encoder(&Default::default());
+        positions_staging.copy_from(&mut encoder, &particles.positions);
+        compute_queue.submit(Some(encoder.finish()));
+
+        // Run the copy. The fourth component of each entry can be ignored.
+        let positions: Vec<Vector4<f32>> =
+            futures::executor::block_on(positions_staging.read(device)).unwrap();
+        positions
+    }
+}
diff --git a/examples3d/src/utils/default_scene.rs b/examples3d/src/utils/default_scene.rs
new file mode 100644
index 0000000..7add5de
--- /dev/null
+++ b/examples3d/src/utils/default_scene.rs
@@ -0,0 +1,52 @@
+#![allow(unused)]
+
+use bevy::prelude::*;
+use nalgebra::vector;
+use rapier3d::prelude::{ColliderBuilder, RigidBodyBuilder};
+use wgsparkl3d::{
+    models::ElasticCoefficients,
+    solver::{Particle, ParticleDynamics, ParticlePhase},
+};
+use wgsparkl_testbed3d::{AppState, RapierData};
+
+pub const SAMPLE_PER_UNIT: f32 = 10.0;
+
+/// Spawns a ground and 4 walls.
+pub fn spawn_ground_and_walls(rapier_data: &mut RapierData) {
+    rapier_data.insert_body_and_collider(
+        RigidBodyBuilder::fixed().translation(vector![0.0, -4.0, 0.0]),
+        ColliderBuilder::cuboid(100.0, 4.0, 100.0),
+    );
+
+    rapier_data.insert_body_and_collider(
+        RigidBodyBuilder::fixed().translation(vector![0.0, 5.0, -35.0]),
+        ColliderBuilder::cuboid(35.0, 5.0, 0.5),
+    );
+    rapier_data.insert_body_and_collider(
+        RigidBodyBuilder::fixed().translation(vector![0.0, 5.0, 35.0]),
+        ColliderBuilder::cuboid(35.0, 5.0, 0.5),
+    );
+    rapier_data.insert_body_and_collider(
+        RigidBodyBuilder::fixed().translation(vector![-35.0, 5.0, 0.0]),
+        ColliderBuilder::cuboid(0.5, 5.0, 35.0),
+    );
+    rapier_data.insert_body_and_collider(
+        RigidBodyBuilder::fixed().translation(vector![35.0, 5.0, 0.0]),
+        ColliderBuilder::cuboid(0.5, 5.0, 35.0),
+    );
+}
+
+pub fn create_particle(pos: &Vec3, color: Option<Color>, radius: f32) -> Particle {
+    let density = 3700.0;
+    Particle {
+        position: pos.to_array().into(),
+        dynamics: ParticleDynamics::with_density(radius, density),
+        model: ElasticCoefficients::from_young_modulus(10_000_000.0, 0.22),
+        plasticity: None,
+        phase: Some(ParticlePhase {
+            phase: 1f32,
+            max_stretch: f32::MAX,
+        }),
+        color: color.map(|c| c.to_linear().to_u8_array()),
+    }
+}
diff --git a/examples3d/src/utils/mod.rs b/examples3d/src/utils/mod.rs
new file mode 100644
index 0000000..d954233
--- /dev/null
+++ b/examples3d/src/utils/mod.rs
@@ -0,0 +1 @@
+pub mod default_scene;
diff --git a/notes.thierry.md b/notes.thierry.md
new file mode 100644
index 0000000..224b579
--- /dev/null
+++ b/notes.thierry.md
@@ -0,0 +1,6 @@
+# Notes
+
+banana example:
+
+- oneway+dynamic coupling makes the knife going places (bouncing weirdly).
+  - workaround: not use dynamic, it wasn't planned anyway.
diff --git a/src/collision/collide.rs b/src/collision/collide.rs
index 0252d50..ecdcc51 100644
--- a/src/collision/collide.rs
+++ b/src/collision/collide.rs
@@ -1,15 +1,16 @@
+use crate::grid::grid::WgGrid;
 use crate::{dim_shader_defs, substitute_aliases};
 use wgcore::Shader;
-use wgparry::cuboid::WgCuboid;
+use wgparry::shape::WgShape;
 use wgrapier::dynamics::WgBody;
 
 #[derive(Shader)]
 #[shader(
-    derive(WgCuboid, WgBody),
+    derive(WgShape, WgBody, WgGrid),
     src = "collide.wgsl",
     src_fn = "substitute_aliases",
     shader_defs = "dim_shader_defs"
 )]
 pub struct WgCollide;
 
-wgcore::test_shader_compilation!(WgCollide);
+wgcore::test_shader_compilation!(WgCollide, wgcore, crate::dim_shader_defs());
diff --git a/src/collision/collide.wgsl b/src/collision/collide.wgsl
index f33bfe1..de0a838 100644
--- a/src/collision/collide.wgsl
+++ b/src/collision/collide.wgsl
@@ -1,6 +1,7 @@
 #define_import_path wgsparkl::collision::collide
-#import wgparry::cuboid as Cuboid;
+#import wgparry::shape as Shape;
 #import wgrapier::body as Body;
+#import wgsparkl::grid::grid as Grid;
 
 #if DIM == 2
 #import wgebra::sim2 as Pose;
@@ -10,7 +11,7 @@
 
 
 @group(2) @binding(0)
-var<storage, read> collision_shapes: array<Cuboid::Cuboid>;
+var<storage, read> collision_shapes: array<Shape::Shape>;
 @group(2) @binding(1)
 var<storage, read> collision_shape_poses: array<Transform>;
 //@group(2) @binding(2)
@@ -19,40 +20,37 @@ var<storage, read> collision_shape_poses: array<Transform>;
 //var<storage, read> body_mprops: array<Body::MassProperties>;
 
 
-fn collide(point: Vector, velocity: Vector) -> Vector {
-    var result_vel = velocity;
+fn collide(cell_width: f32, point: Vector) -> Grid::NodeCdf {
+    const MAX_FLT: f32 = 1.0e10; // Is the f32::MAX constant defined somewhere in WGSL?
+    var cdf = Grid::NodeCdf(MAX_FLT, 0u, Grid::NONE);
+
+#if DIM == 2
+    let dist_cap = vec2(cell_width * 1.5);
+#else
+    let dist_cap = vec3(cell_width * 1.5);
+#endif
 
     // TODO: don’t  rely on the array length, e.g., if the user wants to
     //       preallocate the array to add more dynamically.
     for (var i = 0u; i < arrayLength(&collision_shapes); i++) {
+        // FIXME: figure out a way to support more than 16 colliders.
         let shape = collision_shapes[i];
         let shape_pose = collision_shape_poses[i];
-        let proj = Cuboid::projectPointOnBoundary(shape, shape_pose, point);
-
-        // TODO: make a branchless version of this?
-        if proj.is_inside {
-            // Apply the unilateral constraint (prevent further penetrations).
+        let shape_type = Shape::shape_type(shape);
+        if shape_type != Shape::SHAPE_TYPE_POLYLINE && shape_type != Shape::SHAPE_TYPE_TRIMESH {
+            let proj = Shape::projectPointOnBoundary(shape, shape_pose, point);
             let dpt = proj.point - point;
-            let dist = length(dpt);
-
-            if dist > 0.0 {
-                let normal = dpt / dist;
-                let signed_normal_vel = dot(normal, velocity);
-
-                if signed_normal_vel < 0.0 {
-                    let tangent_vel = velocity - signed_normal_vel * normal;
-                    let tangent_vel_norm = length(tangent_vel);
-
-                    if tangent_vel_norm > 1.0e-10 {
-                        let friction = 0.7; // TODO: make this configurable
-                        result_vel = tangent_vel / tangent_vel_norm * max(tangent_vel_norm + signed_normal_vel * friction, 0.0f);
-                    } else {
-                        result_vel = Vector(0.0);
-                    }
-                }
+
+            if proj.is_inside || all(abs(dpt) <= dist_cap) {
+                let dist = length(dpt);
+                // TODO: take is_inside into account to select the deepest
+                //       penetration as the closest collider?
+                cdf.closest_id = select(cdf.closest_id, i, dist < cdf.distance);
+                cdf.distance = min(cdf.distance, dist);
+                cdf.affinities |= select(0x00000001u, 0x00010001u, proj.is_inside) << i;
             }
         }
     }
 
-    return result_vel;
+    return cdf;
 }
\ No newline at end of file
diff --git a/src/grid/grid.rs b/src/grid/grid.rs
index 00963ca..2664916 100644
--- a/src/grid/grid.rs
+++ b/src/grid/grid.rs
@@ -1,12 +1,11 @@
-use crate::dim_shader_defs;
 use crate::grid::prefix_sum::{PrefixSumWorkspace, WgPrefixSum};
 use crate::grid::sort::WgSort;
-use crate::solver::{GpuParticles, WgParams};
-use naga_oil::compose::NagaModuleDescriptor;
+use crate::solver::{GpuParticles, GpuRigidParticles, WgParams};
+use crate::{dim_shader_defs, substitute_aliases};
 use std::sync::Arc;
 use wgcore::kernel::{KernelInvocationBuilder, KernelInvocationQueue};
 use wgcore::tensor::{GpuScalar, GpuVector};
-use wgcore::{utils, Shader};
+use wgcore::Shader;
 use wgpu::util::DispatchIndirectArgs;
 use wgpu::{Buffer, BufferAddress, BufferDescriptor, BufferUsages, ComputePipeline, Device};
 
@@ -14,7 +13,12 @@ use wgpu::{Buffer, BufferAddress, BufferDescriptor, BufferUsages, ComputePipelin
 use crate::grid::sort::TouchParticleBlocks;
 
 #[derive(Shader)]
-#[shader(derive(WgParams), src = "grid.wgsl", shader_defs = "dim_shader_defs")]
+#[shader(
+    derive(WgParams),
+    src = "grid.wgsl",
+    shader_defs = "dim_shader_defs",
+    src_fn = "substitute_aliases"
+)]
 pub struct WgGrid {
     reset_hmap: ComputePipeline,
     reset: ComputePipeline,
@@ -26,6 +30,7 @@ impl WgGrid {
     pub fn queue_sort<'a>(
         &'a self,
         particles: &GpuParticles,
+        rigid_particles: &GpuRigidParticles,
         grid: &GpuGrid,
         prefix_sum: &mut PrefixSumWorkspace,
         sort_module: &'a WgSort,
@@ -50,11 +55,11 @@ impl WgGrid {
                 .queue(grid.cpu_meta.hmap_capacity.div_ceil(GRID_WORKGROUP_SIZE));
 
             #[cfg(not(target_os = "macos"))]
-            let touch_particle_blocks = &sort_module.touch_particle_blocks;
+            let touch_particle_blocks_pipeline = &sort_module.touch_particle_blocks;
             #[cfg(target_os = "macos")]
-            let touch_particle_blocks = &touch_particle_blocks.touch_particle_blocks;
+            let touch_particle_blocks_pipeline = &touch_particle_blocks.touch_particle_blocks;
 
-            KernelInvocationBuilder::new(queue, touch_particle_blocks)
+            KernelInvocationBuilder::new(queue, touch_particle_blocks_pipeline)
                 .bind_at(
                     0,
                     [
@@ -67,6 +72,47 @@ impl WgGrid {
                 .bind(1, [particles.positions.buffer()])
                 .queue((particles.len() as u32).div_ceil(GRID_WORKGROUP_SIZE));
 
+            // Ensure blocks exist wherever we have rigid particles that might affect
+            // other blocks. This is done in two passes:
+            // 1. Mark all rigid particles that need to ensure it’s associated block exists
+            // 2. Touch the blocks with marked rigid particles.
+            KernelInvocationBuilder::new(queue, &sort_module.mark_rigid_particles_needing_block)
+                .bind_at(
+                    0,
+                    [(grid.meta.buffer(), 0), (grid.hmap_entries.buffer(), 1)],
+                )
+                .bind_at(
+                    1,
+                    [
+                        (rigid_particles.sample_points.buffer(), 0),
+                        (rigid_particles.rigid_particle_needs_block.buffer(), 6),
+                    ],
+                )
+                .queue((rigid_particles.len() as u32).div_ceil(GRID_WORKGROUP_SIZE));
+
+            #[cfg(not(target_os = "macos"))]
+            let touch_rigid_particle_blocks = &sort_module.touch_rigid_particle_blocks;
+            #[cfg(target_os = "macos")]
+            let touch_rigid_particle_blocks = &touch_particle_blocks.touch_rigid_particle_blocks;
+            KernelInvocationBuilder::new(queue, &touch_rigid_particle_blocks)
+                .bind_at(
+                    0,
+                    [
+                        (grid.meta.buffer(), 0),
+                        (grid.hmap_entries.buffer(), 1),
+                        (grid.active_blocks.buffer(), 2),
+                        (grid.debug.buffer(), 8),
+                    ],
+                )
+                .bind_at(
+                    1,
+                    [
+                        (rigid_particles.sample_points.buffer(), 0),
+                        (rigid_particles.rigid_particle_needs_block.buffer(), 6),
+                    ],
+                )
+                .queue((rigid_particles.len() as u32).div_ceil(GRID_WORKGROUP_SIZE));
+
             // TODO: handle grid buffer resizing
             sparse_grid_has_the_correct_size = true;
         }
@@ -133,6 +179,7 @@ impl WgGrid {
                     (grid.meta.buffer(), 0),
                     (grid.nodes.buffer(), 3),
                     (grid.nodes_linked_lists.buffer(), 6),
+                    (grid.nodes_rigid_linked_lists.buffer(), 10),
                 ],
             )
             .queue_indirect(n_block_groups.clone());
@@ -168,10 +215,11 @@ pub struct GpuGridMetadata {
     capacity: u32,
 }
 
-#[derive(bytemuck::Pod, bytemuck::Zeroable, Copy, Clone, PartialEq)]
+#[derive(Copy, Clone, PartialEq, encase::ShaderType)]
 #[repr(C)]
 pub struct GpuGridNode {
     momentum_velocity_mass: nalgebra::Vector4<f32>,
+    cdf: GpuGridNodeCdf,
 }
 
 #[derive(bytemuck::Pod, bytemuck::Zeroable, Copy, Clone, PartialEq)]
@@ -207,6 +255,14 @@ pub struct GpuActiveBlockHeader {
     num_particles: u32,
 }
 
+#[derive(Copy, Clone, PartialEq, Default, Debug, encase::ShaderType)]
+#[repr(C)]
+pub struct GpuGridNodeCdf {
+    pub distance: f32,
+    pub affinities: u32,
+    pub closest_id: u32,
+}
+
 pub struct GpuGrid {
     pub cpu_meta: GpuGridMetadata,
     pub meta: GpuScalar<GpuGridMetadata>,
@@ -215,6 +271,7 @@ pub struct GpuGrid {
     pub active_blocks: GpuVector<GpuActiveBlockHeader>,
     pub scan_values: GpuVector<u32>,
     pub nodes_linked_lists: GpuVector<[u32; 2]>,
+    pub nodes_rigid_linked_lists: GpuVector<[u32; 2]>,
     pub indirect_n_blocks_groups: Arc<Buffer>,
     pub indirect_n_g2p_p2g_groups: Arc<Buffer>,
     pub debug: GpuVector<u32>,
@@ -236,9 +293,12 @@ impl GpuGrid {
             BufferUsages::STORAGE | BufferUsages::COPY_SRC,
         );
         let hmap_entries = GpuVector::uninit(device, capacity, BufferUsages::STORAGE);
-        let nodes = GpuVector::uninit(device, capacity * NODES_PER_BLOCK, BufferUsages::STORAGE);
+        let nodes =
+            GpuVector::uninit_encased(device, capacity * NODES_PER_BLOCK, BufferUsages::STORAGE);
         let nodes_linked_lists =
             GpuVector::uninit(device, capacity * NODES_PER_BLOCK, BufferUsages::STORAGE);
+        let nodes_rigid_linked_lists =
+            GpuVector::uninit(device, capacity * NODES_PER_BLOCK, BufferUsages::STORAGE);
         let active_blocks = GpuVector::uninit(device, capacity, BufferUsages::STORAGE);
         let scan_values = GpuVector::uninit(device, capacity, BufferUsages::STORAGE);
         let indirect_n_blocks_groups = Arc::new(device.create_buffer(&BufferDescriptor {
@@ -265,6 +325,7 @@ impl GpuGrid {
             indirect_n_blocks_groups,
             indirect_n_g2p_p2g_groups,
             nodes_linked_lists,
+            nodes_rigid_linked_lists,
             debug,
         }
     }
diff --git a/src/grid/grid.wgsl b/src/grid/grid.wgsl
index 1ccd3c7..0449c83 100644
--- a/src/grid/grid.wgsl
+++ b/src/grid/grid.wgsl
@@ -14,12 +14,16 @@ var<storage, read_write> nodes: array<Node>;
 var<uniform> sim_params: Params::SimulationParams;
 @group(0) @binding(5)
 var<storage, read_write> n_block_groups: DispatchIndirectArgs;
+// Per-node linked list for MPM particles.
 @group(0) @binding(6)
 var<storage, read_write> nodes_linked_lists: array<NodeLinkedListAtomic>;
 @group(0) @binding(7)
 var<storage, read_write> n_g2p_p2g_groups: DispatchIndirectArgs;
 @group(0) @binding(8)
 var<storage, read_write> num_collisions: array<atomic<u32>>;
+// Per-node linked list for rigid-body particles.
+@group(0) @binding(10)
+var<storage, read_write> nodes_rigid_linked_lists: array<NodeLinkedListAtomic>;
 
 
 struct NodeLinkedListAtomic {
@@ -223,14 +227,43 @@ struct Grid {
     capacity: u32,
 }
 
+const AFFINITY_BITS_MASK: u32 = 0x0000ffffu;
+const SIGN_BITS_SHIFT: u32 = 16;
+
+struct NodeCdf {
+    distance: f32,
+    // Two bits per collider.
+    // The 16 first bits are for affinity, the 16 last are for signs.
+    affinities: u32,
+    // Index to the closest collider.
+    closest_id: u32,
+}
+
+fn affinity_bit(i_collider: u32, affinity: u32) -> bool {
+    return (affinity & (1u << i_collider)) != 0;
+}
+
+fn sign_bit(i_collider: u32, affinity: u32) -> bool {
+    return ((affinity >> SIGN_BITS_SHIFT) & (1u << i_collider)) != 0;
+}
+
+fn affinities_are_compatible(affinity1: u32, affinity2: u32) -> bool {
+    let affinities_in_common = affinity1 & affinity2 & AFFINITY_BITS_MASK;
+    let signs1 = (affinity1 >> SIGN_BITS_SHIFT) & affinities_in_common;
+    let signs2 = (affinity2 >> SIGN_BITS_SHIFT) & affinities_in_common;
+    return signs1 == signs2;
+}
+
 struct Node {
     /// The first three components contains either the cell’s momentum or its velocity
     /// (depending on the context). The fourth component contains the cell’s mass.
+    // TODO: we don’t really need to pack ourself, wgsl will do it automatically.
     #if DIM == 2
     momentum_velocity_mass: vec3<f32>,
     #else
     momentum_velocity_mass: vec4<f32>,
     #endif
+    cdf: NodeCdf,
 }
 
 #if DIM == 2
@@ -337,8 +370,11 @@ fn reset(@builtin(global_invocation_id) invocation_id: vec3<u32>, @builtin(num_w
        #else
        nodes[i].momentum_velocity_mass = vec4(0.0);
        #endif
+       nodes[i].cdf = NodeCdf(0.0, 0, NONE);
        nodes_linked_lists[i].head = NONE;
        nodes_linked_lists[i].len = 0u;
+       nodes_rigid_linked_lists[i].head = NONE;
+       nodes_rigid_linked_lists[i].len = 0u;
    }
 }
 
@@ -350,3 +386,19 @@ struct SimulationParameters {
     gravity: vec3<f32>,
     #endif
 }
+
+fn project_velocity(vel: Vector, n: Vector) -> Vector {
+    // TODO: this should depend on the collider’s material
+    //       properties.
+    let normal_vel = dot(vel, n);
+
+    if normal_vel < 0.0 {
+        let friction = 20.0;
+        let tangent_vel = vel - n * normal_vel;
+        let tangent_vel_len = length(tangent_vel);
+        let tangent_vel_dir = select(Vector(0.0), tangent_vel / tangent_vel_len, tangent_vel_len > 1.0e-8);
+        return tangent_vel_dir * max(0.0, tangent_vel_len + friction * normal_vel);
+    } else {
+        return vel;
+    }
+}
\ No newline at end of file
diff --git a/src/grid/kernel.rs b/src/grid/kernel.rs
index 66cbdb5..49e17c2 100644
--- a/src/grid/kernel.rs
+++ b/src/grid/kernel.rs
@@ -5,4 +5,4 @@ use wgcore::Shader;
 #[shader(src = "kernel.wgsl", shader_defs = "dim_shader_defs")]
 pub struct WgKernel;
 
-wgcore::test_shader_compilation!(WgKernel);
+wgcore::test_shader_compilation!(WgKernel, wgcore, crate::dim_shader_defs());
diff --git a/src/grid/mod.rs b/src/grid/mod.rs
index 4c73ec3..bde70ce 100644
--- a/src/grid/mod.rs
+++ b/src/grid/mod.rs
@@ -1,4 +1,4 @@
 pub mod grid;
 pub mod kernel;
-pub mod sort;
 pub mod prefix_sum;
+pub mod sort;
diff --git a/src/grid/prefix_sum.rs b/src/grid/prefix_sum.rs
index c11d11c..33a9ab0 100644
--- a/src/grid/prefix_sum.rs
+++ b/src/grid/prefix_sum.rs
@@ -1,4 +1,4 @@
-use naga_oil::compose::{ NagaModuleDescriptor};
+use naga_oil::compose::NagaModuleDescriptor;
 use nalgebra::DVector;
 use wgcore::kernel::{KernelInvocationBuilder, KernelInvocationQueue};
 use wgcore::tensor::GpuVector;
@@ -175,6 +175,7 @@ mod test {
     use wgcore::gpu::GpuInstance;
     use wgcore::kernel::KernelInvocationQueue;
     use wgcore::tensor::{GpuVector, TensorBuilder};
+    use wgcore::Shader;
     use wgpu::BufferUsages;
 
     #[futures_test::test]
@@ -183,8 +184,8 @@ mod test {
         const LEN: u32 = 15071;
 
         let gpu = GpuInstance::new().await.unwrap();
-        let prefix_sum = WgPrefixSum::new(gpu.device());
-        let mut queue = KernelInvocationQueue::new(gpu.device_arc());
+        let prefix_sum = WgPrefixSum::from_device(gpu.device()).unwrap();
+        let mut queue = KernelInvocationQueue::new(gpu.device());
 
         let inputs = vec![
             DVector::<u32>::from_fn(LEN as usize, |i, _| 1),
@@ -208,7 +209,7 @@ mod test {
             let mut workspace = PrefixSumWorkspace::with_capacity(gpu.device(), v_cpu.len() as u32);
             prefix_sum.queue(&mut queue, &mut workspace, &v_gpu);
 
-            queue.encode(&mut encoder);
+            queue.encode(&mut encoder, None);
             staging.copy_from(&mut encoder, &v_gpu);
 
             let t0 = std::time::Instant::now();
diff --git a/src/grid/sort.rs b/src/grid/sort.rs
index 85560d7..3d371de 100644
--- a/src/grid/sort.rs
+++ b/src/grid/sort.rs
@@ -1,8 +1,8 @@
 use crate::dim_shader_defs;
-use crate::grid::grid::WgGrid;
-use crate::solver::WgParticle;
-use naga_oil::compose::NagaModuleDescriptor;
-use wgcore::{utils, Shader};
+use crate::grid::grid::{GpuGrid, WgGrid};
+use crate::solver::{GpuRigidParticles, WgParticle};
+use wgcore::kernel::{KernelInvocationBuilder, KernelInvocationQueue};
+use wgcore::Shader;
 use wgpu::ComputePipeline;
 
 #[derive(Shader)]
@@ -14,15 +14,49 @@ use wgpu::ComputePipeline;
 pub struct WgSort {
     #[cfg(not(target_os = "macos"))]
     pub(crate) touch_particle_blocks: ComputePipeline,
+    #[cfg(not(target_os = "macos"))]
+    pub(crate) touch_rigid_particle_blocks: ComputePipeline,
+    pub(crate) mark_rigid_particles_needing_block: ComputePipeline,
     pub(crate) update_block_particle_count: ComputePipeline,
     pub(crate) copy_particles_len_to_scan_value: ComputePipeline,
     pub(crate) copy_scan_values_to_first_particles: ComputePipeline,
     pub(crate) finalize_particles_sort: ComputePipeline,
+    pub(crate) sort_rigid_particles: ComputePipeline,
+}
+
+impl WgSort {
+    pub fn queue_sort_rigid_particles<'a>(
+        &'a self,
+        particles: &GpuRigidParticles,
+        grid: &GpuGrid,
+        queue: &mut KernelInvocationQueue<'a>,
+    ) {
+        const GRID_WORKGROUP_SIZE: u32 = 64;
+        let n_groups = (particles.len() as u32).div_ceil(GRID_WORKGROUP_SIZE);
+        KernelInvocationBuilder::new(queue, &self.sort_rigid_particles)
+            .bind_at(
+                0,
+                [
+                    (grid.meta.buffer(), 0),
+                    (grid.hmap_entries.buffer(), 1),
+                    (grid.nodes_rigid_linked_lists.buffer(), 10),
+                ],
+            )
+            .bind_at(
+                1,
+                [
+                    (particles.sample_points.buffer(), 4),
+                    (particles.node_linked_lists.buffer(), 5),
+                ],
+            )
+            .queue(n_groups);
+    }
 }
 
 #[cfg(target_os = "macos")]
 pub struct TouchParticleBlocks {
     pub(crate) touch_particle_blocks: ComputePipeline,
+    pub(crate) touch_rigid_particle_blocks: ComputePipeline,
 }
 
 #[cfg(target_os = "macos")]
@@ -41,8 +75,20 @@ impl TouchParticleBlocks {
             compilation_options: Default::default(),
             cache: None,
         });
-        Self { touch_particle_blocks: compute_pipeline }
+        let rigid_compute_pipeline =
+            device.create_compute_pipeline(&wgpu::ComputePipelineDescriptor {
+                label: None,
+                layout: None,
+                module: &cs_module,
+                entry_point: Some("touch_rigid_particle_blocks"),
+                compilation_options: Default::default(),
+                cache: None,
+            });
+        Self {
+            touch_particle_blocks: compute_pipeline,
+            touch_rigid_particle_blocks: rigid_compute_pipeline,
+        }
     }
 }
 
-wgcore::test_shader_compilation!(WgSort);
+wgcore::test_shader_compilation!(WgSort, wgcore, crate::dim_shader_defs());
diff --git a/src/grid/sort.wgsl b/src/grid/sort.wgsl
index d46bb7c..979760c 100644
--- a/src/grid/sort.wgsl
+++ b/src/grid/sort.wgsl
@@ -12,10 +12,16 @@ var<storage, read_write> scan_values: array<atomic<u32>>; // This has to be atom
 var<storage, read_write> sorted_particle_ids: array<u32>;
 @group(1) @binding(3)
 var<storage, read_write> particle_node_linked_lists: array<u32>;
+@group(1) @binding(4)
+var<storage, read> rigid_particles_pos: array<Particle::Position>;
+@group(1) @binding(5)
+var<storage, read_write> rigid_particle_node_linked_lists: array<u32>;
+@group(1) @binding(6)
+var<storage, read_write> rigid_particle_needs_block: array<atomic<u32>>;
 
 // Disable this kernel on macos because of the underlying compareExchangeMap which is
 // not working well with naga-oil. This is why we currently have the flattened
-// toouch_particle_block2/3d.wgsl shaders as a workaround currently.
+// touch_particle_block2/3d.wgsl shaders as a workaround currently.
 #if MACOS == 0
 @compute @workgroup_size(Grid::GRID_WORKGROUP_SIZE, 1, 1)
 fn touch_particle_blocks(@builtin(global_invocation_id) invocation_id: vec3<u32>) {
@@ -28,8 +34,57 @@ fn touch_particle_blocks(@builtin(global_invocation_id) invocation_id: vec3<u32>
         }
     }
 }
+
+@compute @workgroup_size(Grid::GRID_WORKGROUP_SIZE, 1, 1)
+fn touch_rigid_particle_blocks(@builtin(global_invocation_id) invocation_id: vec3<u32>) {
+    let id = invocation_id.x;
+    if id < arrayLength(&particles_pos) {
+        let entry_id = id / 32u;
+        let entry_bit = 1u << (id % 32u);
+        let needs_block = (rigid_particle_needs_block[entry_id] & entry_bit) != 0;
+
+        if needs_block {
+            let particle = particles_pos[id];
+            var block = Grid::block_associated_to_point(particle.pt);
+            Grid::mark_block_as_active(block);
+        }
+    }
+}
 #endif
 
+@compute @workgroup_size(Grid::GRID_WORKGROUP_SIZE, 1, 1)
+fn mark_rigid_particles_needing_block(@builtin(global_invocation_id) invocation_id: vec3<u32>) {
+    let id = invocation_id.x;
+    if id < arrayLength(&particles_pos) {
+        let particle = particles_pos[id];
+        // PERF: we should look at the local cell coordinates of the point
+        //       in the block and only touch adjacent blocks if the
+        //       cell coordinate along the corresponding dimension is 1
+        //       or 2. Because particles at cells 0 or 1 won’t contribute
+        //       to neighbor cells.
+        var blocks = Grid::blocks_associated_to_point(particle.pt);
+        var i = 0u;
+
+        for (; i < Grid::NUM_ASSOC_BLOCKS; i += 1u) {
+            if Grid::find_block_header_id(blocks[i]).id != Grid::NONE {
+                break;
+            }
+        }
+
+
+        let entry_id = id / 32u;
+        let entry_bit = 1u << (id % 32u);
+
+        // PERF: this should be a workgroup reduction instead of
+        //       global-memory atomics.
+        if i > 0u && i < Grid::NUM_ASSOC_BLOCKS {
+            atomicOr(&rigid_particle_needs_block[entry_id], entry_bit);
+        } else {
+            atomicAnd(&rigid_particle_needs_block[entry_id], ~entry_bit);
+        }
+    }
+}
+
 // TODO: can this kernel be combined with touch_particle_blocks?
 @compute @workgroup_size(Grid::GRID_WORKGROUP_SIZE, 1, 1)
 fn update_block_particle_count(@builtin(global_invocation_id) invocation_id: vec3<u32>) {
@@ -79,4 +134,28 @@ fn finalize_particles_sort(@builtin(global_invocation_id) invocation_id: vec3<u3
         atomicAdd(&(*node_linked_list).len, 1u);
         particle_node_linked_lists[id] = prev_head;
     }
+}
+
+@compute @workgroup_size(Grid::GRID_WORKGROUP_SIZE, 1, 1)
+fn sort_rigid_particles(@builtin(global_invocation_id) invocation_id: vec3<u32>) {
+    let id = invocation_id.x;
+    if id < arrayLength(&rigid_particles_pos) {
+        let particle = rigid_particles_pos[id];
+        let block_id = Grid::block_associated_to_point(particle.pt);
+
+        // Place the particle to its sorted place.
+        let active_block_id = Grid::find_block_header_id(block_id);
+
+        // NOTE: if the rigid particle doesn’t map to any block, we can just ignore it
+        //       is it won’t affect the simulation.
+        if active_block_id.id != Grid::NONE {
+            // Setup the per-node rigid particle linked-list.
+            let node_local_id = Particle::associated_cell_index_in_block_off_by_one(particle, Grid::grid.cell_width);
+            let node_global_id = Grid::node_id(Grid::block_header_id_to_physical_id(active_block_id), node_local_id);
+            let node_linked_list = &Grid::nodes_rigid_linked_lists[node_global_id.id];
+            let prev_head = atomicExchange(&(*node_linked_list).head, id);
+            atomicAdd(&(*node_linked_list).len, 1u);
+            rigid_particle_node_linked_lists[id] = prev_head;
+        }
+    }
 }
\ No newline at end of file
diff --git a/src/grid/touch_particle_blocks2d.wgsl b/src/grid/touch_particle_blocks2d.wgsl
index 9d3c4d2..90c167e 100644
--- a/src/grid/touch_particle_blocks2d.wgsl
+++ b/src/grid/touch_particle_blocks2d.wgsl
@@ -7,7 +7,6 @@ var<storage, read_write> active_blocks: array<ActiveBlockHeader>;
 @group(0) @binding(8)
 var<storage, read_write> num_collisions: array<atomic<u32>>;
 
-
 struct NodeLinkedListAtomic {
     head: atomic<u32>,
     len: atomic<u32>,
@@ -192,6 +191,35 @@ fn mark_block_as_active(block: BlockVirtualId) {
     }
 }
 
+fn find_block_header_id(key: BlockVirtualId) -> BlockHeaderId {
+    let packed_key = pack_key(key);
+    var slot = hash(packed_key) & (grid.hmap_capacity - 1);
+
+    loop {
+        let entry = &hmap_entries[slot];
+        let state = (*entry).state;
+        if state == packed_key {
+            return (*entry).value;
+        } else if state == NONE {
+            return BlockHeaderId(NONE);
+        }
+
+         slot = (slot + 1) & (grid.hmap_capacity - 1);
+    }
+
+    return BlockHeaderId(NONE);
+}
+
+fn block_has_particles(block: BlockVirtualId) -> bool {
+    let block_header_id = find_block_header_id(block);
+
+    if block_header_id.id != NONE {
+        return active_blocks[block_header_id.id].num_particles != 0;
+    } else {
+        return false;
+    }
+}
+
 struct SimulationParameters {
     dt: f32,
     gravity: vec2<f32>,
@@ -204,6 +232,8 @@ struct Position {
 
 @group(1) @binding(0)
 var<storage, read_write> particles_pos: array<Position>;
+@group(1) @binding(6)
+var<storage, read_write> rigid_particle_needs_block: array<u32>;
 
 @compute @workgroup_size(GRID_WORKGROUP_SIZE, 1, 1)
 fn touch_particle_blocks(@builtin(global_invocation_id) invocation_id: vec3<u32>) {
@@ -215,4 +245,20 @@ fn touch_particle_blocks(@builtin(global_invocation_id) invocation_id: vec3<u32>
             mark_block_as_active(blocks[i]);
         }
     }
-}
\ No newline at end of file
+}
+
+@compute @workgroup_size(GRID_WORKGROUP_SIZE, 1, 1)
+fn touch_rigid_particle_blocks(@builtin(global_invocation_id) invocation_id: vec3<u32>) {
+    let id = invocation_id.x;
+    if id < arrayLength(&particles_pos) {
+        let entry_id = id / 32u;
+        let entry_bit = 1u << (id % 32u);
+        let needs_block = (rigid_particle_needs_block[entry_id] & entry_bit) != 0;
+
+        if needs_block {
+            let particle = particles_pos[id];
+            var block = block_associated_to_point(particle.pt);
+            mark_block_as_active(block);
+        }
+    }
+}
diff --git a/src/grid/touch_particle_blocks3d.wgsl b/src/grid/touch_particle_blocks3d.wgsl
index ca6950f..af91629 100644
--- a/src/grid/touch_particle_blocks3d.wgsl
+++ b/src/grid/touch_particle_blocks3d.wgsl
@@ -7,7 +7,6 @@ var<storage, read_write> active_blocks: array<ActiveBlockHeader>;
 @group(0) @binding(8)
 var<storage, read_write> num_collisions: array<atomic<u32>>;
 
-
 struct NodeLinkedListAtomic {
     head: atomic<u32>,
     len: atomic<u32>,
@@ -201,6 +200,35 @@ fn mark_block_as_active(block: BlockVirtualId) {
     }
 }
 
+fn find_block_header_id(key: BlockVirtualId) -> BlockHeaderId {
+    let packed_key = pack_key(key);
+    var slot = hash(packed_key) & (grid.hmap_capacity - 1);
+
+    loop {
+        let entry = &hmap_entries[slot];
+        let state = (*entry).state;
+        if state == packed_key {
+            return (*entry).value;
+        } else if state == NONE {
+            return BlockHeaderId(NONE);
+        }
+
+         slot = (slot + 1) & (grid.hmap_capacity - 1);
+    }
+
+    return BlockHeaderId(NONE);
+}
+
+fn block_has_particles(block: BlockVirtualId) -> bool {
+    let block_header_id = find_block_header_id(block);
+
+    if block_header_id.id != NONE {
+        return active_blocks[block_header_id.id].num_particles != 0;
+    } else {
+        return false;
+    }
+}
+
 struct SimulationParameters {
     dt: f32,
     gravity: vec3<f32>,
@@ -213,15 +241,38 @@ struct Position {
 
 @group(1) @binding(0)
 var<storage, read_write> particles_pos: array<Position>;
+@group(1) @binding(6)
+var<storage, read_write> rigid_particle_needs_block: array<u32>;
 
 @compute @workgroup_size(GRID_WORKGROUP_SIZE, 1, 1)
 fn touch_particle_blocks(@builtin(global_invocation_id) invocation_id: vec3<u32>) {
     let id = invocation_id.x;
     if id < arrayLength(&particles_pos) {
         let particle = particles_pos[id];
+        // PERF: we should look at the local cell coordinates of the point
+        //       in the block and only touch adjacent blocks if the
+        //       cell coordinate along the corresponding dimension is 1
+        //       or 2. Because particles at cells 0 or 1 won’t contribute
+        //       to neighbor cells.
         var blocks = blocks_associated_to_point(particle.pt);
         for (var i = 0u; i < NUM_ASSOC_BLOCKS; i += 1u) {
             mark_block_as_active(blocks[i]);
         }
     }
-}
\ No newline at end of file
+}
+
+@compute @workgroup_size(GRID_WORKGROUP_SIZE, 1, 1)
+fn touch_rigid_particle_blocks(@builtin(global_invocation_id) invocation_id: vec3<u32>) {
+    let id = invocation_id.x;
+    if id < arrayLength(&particles_pos) {
+        let entry_id = id / 32u;
+        let entry_bit = 1u << (id % 32u);
+        let needs_block = (rigid_particle_needs_block[entry_id] & entry_bit) != 0;
+
+        if needs_block {
+            let particle = particles_pos[id];
+            var block = block_associated_to_point(particle.pt);
+            mark_block_as_active(block);
+        }
+    }
+}
diff --git a/src/lib.rs b/src/lib.rs
index 478050f..680d7f9 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -1,24 +1,33 @@
 #[cfg(feature = "dim2")]
-extern crate wgparry2d as wgparry;
+pub extern crate rapier2d as rapier;
 #[cfg(feature = "dim3")]
-extern crate wgparry3d as wgparry;
+pub extern crate rapier3d as rapier;
 #[cfg(feature = "dim2")]
-extern crate wgrapier2d as wgrapier;
+pub extern crate wgparry2d as wgparry;
 #[cfg(feature = "dim3")]
-extern crate wgrapier3d as wgrapier;
+pub extern crate wgparry3d as wgparry;
+#[cfg(feature = "dim2")]
+pub extern crate wgrapier2d as wgrapier;
+#[cfg(feature = "dim3")]
+pub extern crate wgrapier3d as wgrapier;
 
 mod collision;
 pub mod grid;
+#[cfg(feature = "dim3")]
+pub mod load_mesh3d;
 pub mod models;
 pub mod pipeline;
 pub mod solver;
 
 pub(crate) fn dim_shader_defs() -> HashMap<String, ShaderDefValue> {
     let mut result = wgparry::dim_shader_defs();
-    result.insert("MACOS".to_string(), ShaderDefValue::Int(if cfg!(target_os = "macos") { 1 } else { 0 }));
+    result.insert(
+        "MACOS".to_string(),
+        ShaderDefValue::Int(if cfg!(target_os = "macos") { 1 } else { 0 }),
+    );
     result
 }
 
-use std::collections::HashMap;
 use naga_oil::compose::ShaderDefValue;
-pub(crate) use wgparry::{substitute_aliases};
+use std::collections::HashMap;
+pub(crate) use wgparry::substitute_aliases;
diff --git a/src/load_mesh3d/load_bevy.rs b/src/load_mesh3d/load_bevy.rs
new file mode 100644
index 0000000..5a14e87
--- /dev/null
+++ b/src/load_mesh3d/load_bevy.rs
@@ -0,0 +1,31 @@
+use bevy::render::mesh::{Indices, Mesh, VertexAttributeValues};
+use nalgebra::Point3;
+
+use f32 as Real;
+
+pub fn load_mesh_vertices_indices(mesh: &Mesh) -> Option<(Vec<Point3<Real>>, Vec<[u32; 3]>)> {
+    use rapier3d::na::point;
+
+    let vertices = mesh.attribute(Mesh::ATTRIBUTE_POSITION)?;
+    let indices = mesh.indices()?;
+
+    let vtx: Vec<_> = match vertices {
+        VertexAttributeValues::Float32(vtx) => {
+            Some(vtx.chunks(3).map(|v| point![v[0], v[1], v[2]]).collect())
+        }
+        VertexAttributeValues::Float32x3(vtx) => {
+            Some(vtx.iter().map(|v| point![v[0], v[1], v[2]]).collect())
+        }
+        _ => None,
+    }?;
+
+    let idx = match indices {
+        Indices::U16(idx) => idx
+            .chunks_exact(3)
+            .map(|i| [i[0] as u32, i[1] as u32, i[2] as u32])
+            .collect(),
+        Indices::U32(idx) => idx.chunks_exact(3).map(|i| [i[0], i[1], i[2]]).collect(),
+    };
+
+    Some((vtx, idx))
+}
diff --git a/src/load_mesh3d/load_gltf.rs b/src/load_mesh3d/load_gltf.rs
new file mode 100644
index 0000000..be00229
--- /dev/null
+++ b/src/load_mesh3d/load_gltf.rs
@@ -0,0 +1,282 @@
+use f32 as Real;
+use image::RgbaImage;
+use nalgebra::{distance_squared, Matrix4, Point3, Transform3};
+
+use super::get_point_cloud_from_trimesh;
+
+fn extract_embedded_texture(
+    gltf: &gltf::Document,
+    buffers: &[gltf::buffer::Data],
+) -> Option<RgbaImage> {
+    for image in gltf.images() {
+        dbg!(image.name());
+        if let gltf::image::Source::View { view, .. } = image.source() {
+            let buffer = &buffers[view.buffer().index()];
+            let start = view.offset();
+            let end = start + view.length();
+            let image_data = &buffer.0[start..end];
+
+            // Decode based on MIME type
+            if let Ok(img) = image::load_from_memory(image_data) {
+                return Some(img.to_rgba8());
+            } else {
+                eprintln!("Failed to decode texture.");
+            }
+        }
+    }
+    None
+}
+
+fn sample_texture(texture: &RgbaImage, uv: [f32; 2]) -> [u8; 4] {
+    let (width, height) = texture.dimensions();
+
+    // Convert UV (0.0 - 1.0) to pixel coordinates
+    let x = (uv[0] * width as f32).clamp(0.0, (width - 1) as f32) as u32;
+    let y = ((uv[1]) * height as f32).clamp(0.0, (height - 1) as f32) as u32;
+
+    texture.get_pixel(x, y).0
+}
+
+fn closest_point(
+    target: Point3<f32>,
+    points: &[(Point3<f32>, [u8; 4])],
+) -> Option<&(Point3<f32>, [u8; 4])> {
+    points.iter().min_by(|a, b| {
+        distance_squared(&a.0, &target)
+            .partial_cmp(&distance_squared(&b.0, &target))
+            .unwrap()
+    })
+}
+
+// TODO: transform should not be here, but when we spawn the model.
+// this function should have a "resolution" like the amount of points to sample in x/y/z.
+/// Load a glb or gltf file and return a point cloud and a list of trimeshes.
+///
+/// ```
+/// let mut file = File::open("path_to_file.glb").expect("Failed to open GLB file");
+/// let mut buffer = Vec::new();
+/// file.read_to_end(&mut buffer).expect("Failed to read file");
+///
+/// let _ = load_model_with_colors(&buffer);
+/// ```
+pub fn load_model_with_colors<S>(
+    slice: S,
+    transform: Transform3<Real>,
+    color_inside: Option<[u8; 4]>,
+    sample_per_unit: f32,
+) -> Vec<(Point3<Real>, [u8; 4])>
+where
+    S: AsRef<[u8]>,
+{
+    let mut res = load_model_with_vertex_colors(slice);
+    let mut pc_grid = vec![];
+
+    res.0
+        .iter_mut()
+        .for_each(|p| p.0 = transform.transform_point(&p.0));
+
+    for (t_id, trimesh) in res.1.iter_mut().enumerate() {
+        if t_id != 0 {
+            //continue;
+        }
+        trimesh.0.iter_mut().for_each(|p| {
+            let new_p = transform.transform_point(&Point3::new(p.x, p.y, p.z));
+            *p = Point3::new(new_p.x, new_p.y, new_p.z);
+        });
+        let mut pc = get_point_cloud_from_trimesh(&trimesh.0, &trimesh.1, sample_per_unit)
+            .into_iter()
+            .map(|p| {
+                let closest_color = closest_point(p, &res.0).unwrap();
+                if let Some(color_inside) = color_inside {
+                    let distance = distance_squared(&p, &closest_color.0);
+                    (
+                        p,
+                        if distance <= 0.04 {
+                            closest_color.1
+                        } else {
+                            color_inside
+                        },
+                    )
+                } else {
+                    (p, closest_color.1)
+                }
+            })
+            .collect();
+        pc_grid.append(&mut pc);
+    }
+    pc_grid
+}
+
+fn get_node_transform(node: &gltf::Node, parent_transform: Matrix4<f32>) -> Matrix4<f32> {
+    let matrix = node.transform().matrix();
+    parent_transform * Matrix4::from_column_slice(&matrix.concat())
+}
+
+pub struct SceneIterator<'a, T, F>
+where
+    F: Fn(&gltf::Primitive, &Matrix4<f32>) -> T + 'a,
+{
+    stack: Vec<(gltf::Node<'a>, Matrix4<f32>)>,
+    process_primitive: F,
+}
+
+impl<'a, T, F> SceneIterator<'a, T, F>
+where
+    F: Fn(&gltf::Primitive, &Matrix4<f32>) -> T + 'a,
+{
+    fn new(root_nodes: Vec<gltf::Node<'a>>, process_primitive: F) -> Self {
+        let stack = root_nodes
+            .into_iter()
+            .map(|node| (node, Matrix4::identity()))
+            .collect();
+        SceneIterator {
+            stack,
+            process_primitive,
+        }
+    }
+}
+
+impl<'a, T, F> Iterator for SceneIterator<'a, T, F>
+where
+    F: Fn(&gltf::Primitive, &Matrix4<f32>) -> T + 'a,
+{
+    type Item = T;
+
+    fn next(&mut self) -> Option<Self::Item> {
+        while let Some((node, parent_transform)) = self.stack.pop() {
+            let world_transform = get_node_transform(&node, parent_transform);
+
+            if let Some(mesh) = node.mesh() {
+                if let Some(primitive) = mesh.primitives().next() {
+                    return Some((self.process_primitive)(&primitive, &world_transform));
+                }
+            }
+
+            for child in node.children() {
+                self.stack.push((child, world_transform));
+            }
+        }
+        None
+    }
+}
+
+/// Load a glb or gltf file into vertices and indices and its vertex colors from sampled texture.
+///
+/// ```
+/// let mut file = File::open("path_to_file.glb").expect("Failed to open GLB file");
+/// let mut buffer = Vec::new();
+/// file.read_to_end(&mut buffer).expect("Failed to read file");
+///
+/// let (gltf, buffers, _) = gltf::import_slice(slice).expect("Failed to parse GLB");
+///
+/// let _ = load_model_with_point_cloud(&buffer);
+/// ```
+pub fn load_model_with_vertex_colors<S>(
+    slice: S,
+) -> (
+    Vec<(nalgebra::Point3<f32>, [u8; 4])>,
+    Vec<(Vec<nalgebra::Point3<f32>>, Vec<usize>)>,
+)
+where
+    S: AsRef<[u8]>,
+{
+    let (gltf, buffers, _) = gltf::import_slice(slice).expect("Failed to parse GLB");
+
+    // Extract embedded texture
+    let texture = extract_embedded_texture(&gltf, &buffers).expect("Failed to extract texture");
+
+    let mut pcs = vec![];
+    let mut trimeshes = vec![];
+
+    for scene in gltf.scenes() {
+        let iterator = SceneIterator::new(scene.nodes().collect(), |primitive, world_transform| {
+            let reader = primitive.reader(|buffer| Some(&*buffers[buffer.index()].0));
+
+            let mut point_cloud = vec![];
+            let mut positions = vec![];
+
+            if let (Some(positions_iter), Some(tex_coords)) =
+                (reader.read_positions(), reader.read_tex_coords(0))
+            {
+                for (pos, uv) in positions_iter.zip(tex_coords.into_f32()) {
+                    let color = sample_texture(&texture, uv);
+                    let position = Point3::new(pos[0], pos[1], pos[2]);
+                    let transformed = world_transform.transform_point(&position);
+                    point_cloud.push((transformed, color));
+                    positions.push(transformed);
+                }
+
+                let indices = reader
+                    .read_indices()
+                    .expect("No indices found")
+                    .into_u32()
+                    .map(|i| i as usize)
+                    .collect::<Vec<_>>();
+
+                (point_cloud, (positions, indices))
+            } else {
+                (vec![], (vec![], vec![]))
+            }
+        });
+
+        for (pc, trimesh) in iterator {
+            pcs.extend(pc);
+            trimeshes.push(trimesh);
+        }
+    }
+
+    (pcs, trimeshes)
+}
+
+/// Load a glb or gltf file and return a point cloud and a list of trimeshes.
+///
+/// ```
+/// let mut file = File::open("path_to_file.glb").expect("Failed to open GLB file");
+/// let mut buffer = Vec::new();
+/// file.read_to_end(&mut buffer).expect("Failed to read file");
+///
+/// let (gltf, buffers, _) = gltf::import_slice(slice).expect("Failed to parse GLB");
+///
+/// let _ = load_model_trimeshes(&buffer);
+/// ```
+pub fn load_model_trimeshes<S>(slice: S) -> Vec<(Vec<nalgebra::Point3<f32>>, Vec<usize>)>
+where
+    S: AsRef<[u8]>,
+{
+    let (gltf, buffers, _) = gltf::import_slice(slice).expect("Failed to parse GLB");
+
+    let mut trimeshes = vec![];
+
+    for scene in gltf.scenes() {
+        let iterator = SceneIterator::new(scene.nodes().collect(), |primitive, world_transform| {
+            let reader = primitive.reader(|buffer| Some(&*buffers[buffer.index()].0));
+
+            let mut positions = vec![];
+
+            if let Some(positions_iter) = reader.read_positions() {
+                for pos in positions_iter {
+                    let position = Point3::new(pos[0], pos[1], pos[2]);
+                    let transformed = world_transform.transform_point(&position);
+                    positions.push(transformed);
+                }
+
+                let indices = reader
+                    .read_indices()
+                    .expect("No indices found")
+                    .into_u32()
+                    .map(|i| i as usize)
+                    .collect::<Vec<_>>();
+
+                (positions, indices)
+            } else {
+                (vec![], vec![])
+            }
+        });
+
+        for trimesh in iterator {
+            trimeshes.push(trimesh);
+        }
+    }
+
+    trimeshes
+}
diff --git a/src/load_mesh3d/load_obj.rs b/src/load_mesh3d/load_obj.rs
new file mode 100644
index 0000000..992088c
--- /dev/null
+++ b/src/load_mesh3d/load_obj.rs
@@ -0,0 +1,29 @@
+use std::io::BufRead;
+
+use nalgebra::{point, Point3};
+use obj::{raw::object::Polygon, ObjResult};
+
+use super::recenter_and_scale;
+
+pub fn get_point_cloud<T: BufRead>(
+    input: T,
+    sample_per_unit: f32,
+) -> ObjResult<(Vec<Point3<f32>>, Vec<usize>)> {
+    let model = obj::raw::parse_obj(input)?;
+    let mut vertices: Vec<_> = model
+        .positions
+        .iter()
+        .map(|v| point![v.0, v.1, v.2])
+        .collect();
+    let indices: Vec<_> = model
+        .polygons
+        .into_iter()
+        .flat_map(|p| match p {
+            Polygon::P(idx) => idx,
+            Polygon::PT(idx) | Polygon::PN(idx) => idx.iter().map(|i| i.0).collect(),
+            Polygon::PTN(idx) => idx.iter().map(|i| i.0).collect(),
+        })
+        .collect();
+    recenter_and_scale(&mut vertices, sample_per_unit);
+    Ok((vertices, indices))
+}
diff --git a/src/load_mesh3d/mod.rs b/src/load_mesh3d/mod.rs
new file mode 100644
index 0000000..5930fce
--- /dev/null
+++ b/src/load_mesh3d/mod.rs
@@ -0,0 +1,62 @@
+#[cfg(feature = "load_bevy")]
+pub mod load_bevy;
+#[cfg(feature = "load_gltf")]
+pub mod load_gltf;
+#[cfg(feature = "load_obj")]
+pub mod load_obj;
+
+use nalgebra::{Point3, Vector3};
+
+use crate::rapier;
+use f32 as Real;
+use rapier::parry::bounding_volume;
+use rapier::prelude::{PointQuery, TriMesh, TriMeshFlags};
+
+pub fn recenter_and_scale(
+    vertices: &mut Vec<nalgebra::OPoint<f32, nalgebra::Const<3>>>,
+    sample_per_unit: f32,
+) {
+    // Compute the size of the model, to scale it and have similar size for everything.
+    let aabb =
+        bounding_volume::details::point_cloud_aabb(&rapier::na::Isometry::default(), &*vertices);
+    let center = aabb.center();
+    let diag = (aabb.maxs - aabb.mins).norm();
+    vertices
+        .iter_mut()
+        .for_each(|p| *p = (*p - center.coords) * sample_per_unit / diag);
+}
+
+pub fn get_point_cloud_from_trimesh(
+    vertices: &[nalgebra::OPoint<f32, nalgebra::Const<3>>],
+    indices: &[usize],
+    sample_per_unit: f32,
+) -> Vec<Point3<Real>> {
+    let indices: Vec<_> = indices
+        .chunks(3)
+        .map(|idx| [idx[0] as u32, idx[1] as u32, idx[2] as u32])
+        .collect();
+    let trimesh = TriMesh::with_flags(vertices.to_vec(), indices, TriMeshFlags::ORIENTED)
+        .expect("Invalid mesh");
+    let aabb = bounding_volume::details::point_cloud_aabb(
+        &rapier3d::na::Isometry::default(),
+        trimesh.vertices(),
+    );
+    let mut positions = vec![];
+
+    let aabb_sample = aabb.scaled(&Vector3::new(
+        sample_per_unit,
+        sample_per_unit,
+        sample_per_unit,
+    ));
+    for x in aabb_sample.mins.x as i32..aabb_sample.maxs.x as i32 {
+        for y in aabb_sample.mins.y as i32..aabb_sample.maxs.y as i32 {
+            for z in aabb_sample.mins.z as i32..aabb_sample.maxs.z as i32 {
+                let point = Point3::new(x as f32, y as f32, z as f32) / sample_per_unit;
+                if trimesh.contains_local_point(&point) {
+                    positions.push(point);
+                }
+            }
+        }
+    }
+    positions
+}
diff --git a/src/models/drucker_prager.rs b/src/models/drucker_prager.rs
index 8609531..a68aede 100644
--- a/src/models/drucker_prager.rs
+++ b/src/models/drucker_prager.rs
@@ -6,12 +6,12 @@ use wgebra::{WgSvd2, WgSvd3};
 #[derive(bytemuck::Pod, bytemuck::Zeroable, Copy, Clone, PartialEq, Debug)]
 #[repr(C)]
 pub struct DruckerPrager {
-    h0: f32,
-    h1: f32,
-    h2: f32,
-    h3: f32,
-    lambda: f32,
-    mu: f32,
+    pub h0: f32,
+    pub h1: f32,
+    pub h2: f32,
+    pub h3: f32,
+    pub lambda: f32,
+    pub mu: f32,
 }
 
 impl DruckerPrager {
@@ -59,4 +59,4 @@ impl Default for DruckerPragerPlasticState {
 )]
 pub struct WgDruckerPrager;
 
-wgcore::test_shader_compilation!(WgDruckerPrager);
+wgcore::test_shader_compilation!(WgDruckerPrager, wgcore, crate::dim_shader_defs());
diff --git a/src/models/linear_elasticity.rs b/src/models/linear_elasticity.rs
index 6ee1d74..ce2c0a4 100644
--- a/src/models/linear_elasticity.rs
+++ b/src/models/linear_elasticity.rs
@@ -10,4 +10,4 @@ use wgebra::{WgSvd2, WgSvd3};
 )]
 pub struct WgLinearElasticity;
 
-wgcore::test_shader_compilation!(WgLinearElasticity);
+wgcore::test_shader_compilation!(WgLinearElasticity, wgcore, crate::dim_shader_defs());
diff --git a/src/models/neo_hookean_elasticity.rs b/src/models/neo_hookean_elasticity.rs
index d233953..720378b 100644
--- a/src/models/neo_hookean_elasticity.rs
+++ b/src/models/neo_hookean_elasticity.rs
@@ -10,4 +10,4 @@ use wgebra::{WgSvd2, WgSvd3};
 )]
 pub struct WgNeoHookeanElasticity;
 
-wgcore::test_shader_compilation!(WgNeoHookeanElasticity);
+wgcore::test_shader_compilation!(WgNeoHookeanElasticity, wgcore, crate::dim_shader_defs());
diff --git a/src/models/neo_hookean_elasticity.wgsl b/src/models/neo_hookean_elasticity.wgsl
index f1746fd..97836be 100644
--- a/src/models/neo_hookean_elasticity.wgsl
+++ b/src/models/neo_hookean_elasticity.wgsl
@@ -13,9 +13,9 @@ fn kirchoff_stress(model: ElasticCoefficients, deformation_gradient: mat2x2<f32>
 #else
 fn kirchoff_stress(model: ElasticCoefficients, deformation_gradient: mat3x3<f32>) -> mat3x3<f32> {
 #endif
-    let j = determinant(deformation_gradient);
-    let diag = model.lambda * log(j) - model.mu / j;
-    var stress = model.mu / j * (deformation_gradient * transpose(deformation_gradient));
+    let j = max(determinant(deformation_gradient), 1.0e-10);
+    let diag = model.lambda * log(j) - model.mu;
+    var stress = model.mu  * (deformation_gradient * transpose(deformation_gradient));
     stress.x.x += diag;
     stress.y.y += diag;
     #if DIM == 3
diff --git a/src/pipeline.rs b/src/pipeline.rs
index 9404d70..d7663de 100644
--- a/src/pipeline.rs
+++ b/src/pipeline.rs
@@ -1,21 +1,26 @@
 use crate::grid::grid::{GpuGrid, WgGrid};
 use crate::grid::prefix_sum::{PrefixSumWorkspace, WgPrefixSum};
+#[cfg(target_os = "macos")]
+use crate::grid::sort::TouchParticleBlocks;
 use crate::grid::sort::WgSort;
 use crate::models::GpuModels;
 use crate::solver::{
-    GpuParticles, GpuSimulationParams, Particle, SimulationParams, WgG2P, WgGridUpdate, WgP2G,
-    WgParticleUpdate,
+    GpuImpulses, GpuParticles, GpuRigidParticles, GpuSimulationParams, Particle, SimulationParams,
+    WgG2P, WgG2PCdf, WgGridUpdate, WgGridUpdateCdf, WgP2G, WgP2GCdf, WgParticleUpdate,
+    WgRigidImpulses, WgRigidParticleUpdate,
 };
 use naga_oil::compose::ComposerError;
+use rapier::dynamics::{RigidBodyHandle, RigidBodySet};
+use rapier::geometry::{ColliderHandle, ColliderSet};
 use wgcore::hot_reloading::HotReloadState;
 use wgcore::kernel::KernelInvocationQueue;
+use wgcore::tensor::GpuVector;
 use wgcore::Shader;
-use wgpu::Device;
+use wgparry::math::GpuSim;
+use wgpu::{BufferUsages, Device};
+use wgrapier::dynamics::body::{BodyCoupling, BodyCouplingEntry};
 use wgrapier::dynamics::{BodyDesc, GpuBodySet, WgIntegrate};
 
-#[cfg(target_os = "macos")]
-use crate::grid::sort::TouchParticleBlocks;
-
 pub struct MpmPipeline {
     grid: WgGrid,
     prefix_sum: WgPrefixSum,
@@ -23,10 +28,14 @@ pub struct MpmPipeline {
     #[cfg(target_os = "macos")]
     touch_particle_blocks: TouchParticleBlocks,
     p2g: WgP2G,
+    p2g_cdf: WgP2GCdf,
+    grid_update_cdf: WgGridUpdateCdf,
     grid_update: WgGridUpdate,
     particles_update: WgParticleUpdate,
     g2p: WgG2P,
-    integrate_bodies: WgIntegrate,
+    g2p_cdf: WgG2PCdf,
+    rigid_particles_update: WgRigidParticleUpdate,
+    pub impulses: WgRigidImpulses,
 }
 
 impl MpmPipeline {
@@ -35,10 +44,15 @@ impl MpmPipeline {
         WgPrefixSum::watch_sources(state).unwrap(); // TODO: don’t unwrap
         WgSort::watch_sources(state).unwrap(); // TODO: don’t unwrap
         WgP2G::watch_sources(state).unwrap(); // TODO: don’t unwrap
+        WgP2GCdf::watch_sources(state).unwrap(); // TODO: don’t unwrap
         WgGridUpdate::watch_sources(state).unwrap(); // TODO: don’t unwrap
+        WgGridUpdateCdf::watch_sources(state).unwrap(); // TODO: don’t unwrap
         WgParticleUpdate::watch_sources(state).unwrap(); // TODO: don’t unwrap
         WgG2P::watch_sources(state).unwrap(); // TODO: don’t unwrap
+        WgG2PCdf::watch_sources(state).unwrap(); // TODO: don’t unwrap
         WgIntegrate::watch_sources(state).unwrap(); // TODO: don’t unwrap
+        WgRigidImpulses::watch_sources(state).unwrap(); // TODO: don’t unwrap
+        WgRigidParticleUpdate::watch_sources(state).unwrap(); // TODOO: don’t unwrap
     }
 
     pub fn reload_if_changed(
@@ -51,22 +65,38 @@ impl MpmPipeline {
         changed = self.prefix_sum.reload_if_changed(device, state)? || changed;
         changed = self.sort.reload_if_changed(device, state)? || changed;
         changed = self.p2g.reload_if_changed(device, state)? || changed;
+        changed = self.p2g_cdf.reload_if_changed(device, state)? || changed;
         changed = self.grid_update.reload_if_changed(device, state)? || changed;
+        changed = self.grid_update_cdf.reload_if_changed(device, state)? || changed;
         changed = self.particles_update.reload_if_changed(device, state)? || changed;
         changed = self.g2p.reload_if_changed(device, state)? || changed;
-        changed = self.integrate_bodies.reload_if_changed(device, state)? || changed;
+        changed = self.g2p_cdf.reload_if_changed(device, state)? || changed;
+        changed = self.impulses.reload_if_changed(device, state)? || changed;
+        changed = self
+            .rigid_particles_update
+            .reload_if_changed(device, state)?
+            || changed;
 
         Ok(changed)
     }
 }
 
+pub struct PhysicsData<'a> {
+    bodies: &'a RigidBodySet,
+    colliders: &'a ColliderSet,
+}
+
 pub struct MpmData {
     pub sim_params: GpuSimulationParams,
     pub grid: GpuGrid,
     pub particles: GpuParticles, // TODO: keep private?
+    pub rigid_particles: GpuRigidParticles,
     pub bodies: GpuBodySet,
+    pub impulses: GpuImpulses,
+    pub poses_staging: GpuVector<GpuSim>,
     prefix_sum: PrefixSumWorkspace,
     models: GpuModels,
+    coupling: Vec<BodyCouplingEntry>,
 }
 
 impl MpmData {
@@ -74,26 +104,77 @@ impl MpmData {
         device: &Device,
         params: SimulationParams,
         particles: &[Particle],
-        bodies: &[BodyDesc],
+        bodies: &RigidBodySet,
+        colliders: &ColliderSet,
         cell_width: f32,
         grid_capacity: u32,
     ) -> Self {
+        let coupling: Vec<_> = colliders
+            .iter()
+            .filter_map(|(co_handle, co)| {
+                let rb_handle = co.parent()?;
+                Some(BodyCouplingEntry {
+                    body: rb_handle,
+                    collider: co_handle,
+                    mode: BodyCoupling::TwoWays,
+                })
+            })
+            .collect();
+        Self::with_select_coupling(
+            device,
+            params,
+            particles,
+            bodies,
+            colliders,
+            coupling,
+            cell_width,
+            grid_capacity,
+        )
+    }
+
+    pub fn with_select_coupling(
+        device: &Device,
+        params: SimulationParams,
+        particles: &[Particle],
+        bodies: &RigidBodySet,
+        colliders: &ColliderSet,
+        coupling: Vec<BodyCouplingEntry>,
+        cell_width: f32,
+        grid_capacity: u32,
+    ) -> Self {
+        let sampling_step = cell_width; // TODO: * 1.5 ?
+        let bodies = GpuBodySet::from_rapier(device, bodies, colliders, &coupling);
         let sim_params = GpuSimulationParams::new(device, params);
         let models = GpuModels::from_particles(device, particles);
         let particles = GpuParticles::from_particles(device, particles);
+        let rigid_particles =
+            GpuRigidParticles::from_rapier(device, colliders, &bodies, &coupling, sampling_step);
         let grid = GpuGrid::with_capacity(device, grid_capacity, cell_width);
         let prefix_sum = PrefixSumWorkspace::with_capacity(device, grid_capacity);
-        let bodies = GpuBodySet::new(device, bodies);
+        let impulses = GpuImpulses::new(device);
+        let poses_staging = GpuVector::uninit(
+            device,
+            bodies.len(),
+            BufferUsages::COPY_DST | BufferUsages::MAP_READ,
+        );
 
         Self {
             sim_params,
             particles,
+            rigid_particles,
             bodies,
+            impulses,
             grid,
             prefix_sum,
             models,
+            poses_staging,
+            coupling,
         }
     }
+
+    pub fn coupling(&self) -> &[BodyCouplingEntry] {
+        &self.coupling
+    }
 }
 
 impl MpmPipeline {
@@ -103,12 +184,16 @@ impl MpmPipeline {
             prefix_sum: WgPrefixSum::from_device(device)?,
             sort: WgSort::from_device(device)?,
             p2g: WgP2G::from_device(device)?,
+            p2g_cdf: WgP2GCdf::from_device(device)?,
             grid_update: WgGridUpdate::from_device(device)?,
+            grid_update_cdf: WgGridUpdateCdf::from_device(device)?,
             particles_update: WgParticleUpdate::from_device(device)?,
+            rigid_particles_update: WgRigidParticleUpdate::from_device(device)?,
             g2p: WgG2P::from_device(device)?,
-            integrate_bodies: WgIntegrate::from_device(device)?,
+            g2p_cdf: WgG2PCdf::from_device(device)?,
             #[cfg(target_os = "macos")]
             touch_particle_blocks: TouchParticleBlocks::from_device(device),
+            impulses: WgRigidImpulses::from_device(device)?,
         })
     }
 
@@ -118,10 +203,17 @@ impl MpmPipeline {
         queue: &mut KernelInvocationQueue<'a>,
         add_timestamps: bool,
     ) {
-        queue.compute_pass("grid sort", add_timestamps);
+        queue.compute_pass("update rigid particles", add_timestamps);
+
+        self.impulses
+            .queue_update_world_mass_properties(queue, &data.impulses, &data.bodies);
+        self.rigid_particles_update
+            .queue(queue, &data.bodies, &data.rigid_particles);
 
+        queue.compute_pass("grid sort", add_timestamps);
         self.grid.queue_sort(
             &data.particles,
+            &data.rigid_particles,
             &data.grid,
             &mut data.prefix_sum,
             &self.sort,
@@ -130,19 +222,46 @@ impl MpmPipeline {
             &self.prefix_sum,
             queue,
         );
+        self.sort
+            .queue_sort_rigid_particles(&data.rigid_particles, &data.grid, queue);
+
+        queue.compute_pass("grid_update_cdf", add_timestamps);
+
+        self.grid_update_cdf
+            .queue(queue, &data.sim_params, &data.grid, &data.bodies);
+
+        queue.compute_pass("p2g_cdf", add_timestamps);
+
+        self.p2g_cdf
+            .queue(queue, &data.grid, &data.rigid_particles, &data.bodies);
+
+        queue.compute_pass("g2p_cdf", add_timestamps);
+
+        self.g2p_cdf
+            .queue(queue, &data.sim_params, &data.grid, &data.particles);
 
         queue.compute_pass("p2g", add_timestamps);
-        self.p2g.queue(queue, &data.grid, &data.particles);
+        self.p2g.queue(
+            queue,
+            &data.grid,
+            &data.particles,
+            &data.impulses,
+            &data.bodies,
+        );
 
         queue.compute_pass("grid_update", add_timestamps);
 
-        self.grid_update
-            .queue(queue, &data.sim_params, &data.grid, &data.bodies);
+        self.grid_update.queue(queue, &data.sim_params, &data.grid);
 
         queue.compute_pass("g2p", add_timestamps);
 
-        self.g2p
-            .queue(queue, &data.sim_params, &data.grid, &data.particles);
+        self.g2p.queue(
+            queue,
+            &data.sim_params,
+            &data.grid,
+            &data.particles,
+            &data.bodies,
+        );
 
         queue.compute_pass("particles_update", add_timestamps);
 
@@ -158,15 +277,22 @@ impl MpmPipeline {
         queue.compute_pass("integrate_bodies", add_timestamps);
 
         // TODO: should this be in a separate pipeline? Within wgrapier probably?
-        self.integrate_bodies.queue(queue, &data.bodies);
+        self.impulses.queue(
+            queue,
+            &data.grid,
+            &data.sim_params,
+            &data.impulses,
+            &data.bodies,
+        );
     }
 }
 
 #[cfg(test)]
+#[cfg(feature = "dim3")]
 mod test {
     use crate::grid::grid::GpuGrid;
     use crate::grid::prefix_sum::PrefixSumWorkspace;
-    use crate::models::LinearElasticity;
+    use crate::models::WgLinearElasticity;
     use crate::pipeline::{MpmData, MpmPipeline};
     use crate::solver::{GpuParticles, Particle, ParticleMassProps, SimulationParams};
     use nalgebra::{vector, Vector3};
@@ -191,6 +317,8 @@ mod test {
                         velocity: Vector3::zeros(),
                         volume: ParticleMassProps::new(1.0, cell_width / 4.0),
                         model: LinearElasticity::from_young_modulus(100_000.0, 0.33),
+                        plasticity: None,
+                        phase: None,
                     });
                 }
             }
diff --git a/src/solver/g2p.rs b/src/solver/g2p.rs
index f7ae535..538ef0a 100644
--- a/src/solver/g2p.rs
+++ b/src/solver/g2p.rs
@@ -4,14 +4,15 @@ use crate::solver::params::{GpuSimulationParams, WgParams};
 use crate::solver::GpuParticles;
 use crate::solver::WgParticle;
 use crate::{dim_shader_defs, substitute_aliases};
-use naga_oil::compose::NagaModuleDescriptor;
+use std::ops::Deref;
 use wgcore::kernel::{KernelInvocationBuilder, KernelInvocationQueue};
-use wgcore::{utils, Shader};
+use wgcore::Shader;
 use wgpu::ComputePipeline;
+use wgrapier::dynamics::{GpuBodySet, WgBody};
 
 #[derive(Shader)]
 #[shader(
-    derive(WgParams, WgParticle, WgGrid, WgKernel),
+    derive(WgParams, WgParticle, WgGrid, WgKernel, WgBody),
     src = "g2p.wgsl",
     src_fn = "substitute_aliases",
     shader_defs = "dim_shader_defs"
@@ -27,29 +28,30 @@ impl WgG2P {
         sim_params: &GpuSimulationParams,
         grid: &GpuGrid,
         particles: &GpuParticles,
+        bodies: &GpuBodySet,
     ) {
         KernelInvocationBuilder::new(queue, &self.g2p)
-            .bind(
+            .bind_at(
                 0,
                 [
-                    grid.meta.buffer(),
-                    grid.hmap_entries.buffer(),
-                    grid.active_blocks.buffer(),
-                    grid.nodes.buffer(),
+                    (grid.meta.buffer(), 0),
+                    (grid.hmap_entries.buffer(), 1),
+                    (grid.active_blocks.buffer(), 2),
+                    (grid.nodes.buffer(), 3),
                 ],
             )
             .bind(
                 1,
                 [
                     particles.positions.buffer(),
-                    particles.velocities.buffer(),
-                    particles.affines.buffer(),
+                    particles.dynamics.buffer(),
                     particles.sorted_ids.buffer(),
                     sim_params.params.buffer(),
                 ],
             )
+            .bind(2, [bodies.vels().buffer(), bodies.mprops().buffer()])
             .queue_indirect(grid.indirect_n_g2p_p2g_groups.clone());
     }
 }
 
-wgcore::test_shader_compilation!(WgG2P);
+wgcore::test_shader_compilation!(WgG2P, wgcore, crate::dim_shader_defs());
diff --git a/src/solver/g2p.wgsl b/src/solver/g2p.wgsl
index 414090b..bdf7c67 100644
--- a/src/solver/g2p.wgsl
+++ b/src/solver/g2p.wgsl
@@ -6,36 +6,39 @@
 #import wgsparkl::grid::grid as Grid;
 #import wgsparkl::models::linear_elasticity as ConstitutiveModel;
 #import wgsparkl::models::drucker_prager as DruckerPrager;
+#import wgrapier::body as Body;
 
 @group(1) @binding(0)
 var<storage, read> particles_pos: array<Particle::Position>;
 @group(1) @binding(1)
-var<storage, read_write> particles_vel: array<Particle::Velocity>;
-#if DIM == 2
-@group(1) @binding(2)
-var<storage, read_write> particles_affine: array<mat2x2<f32>>;
-#else
+var<storage, read_write> particles_dyn: array<Particle::Dynamics>;
 @group(1) @binding(2)
-var<storage, read_write> particles_affine: array<mat3x3<f32>>;
-#endif
-@group(1) @binding(3)
 var<storage, read> sorted_particle_ids: array<u32>;
-@group(1) @binding(4)
+@group(1) @binding(3)
 var<uniform> params: Params::SimulationParams;
 
+@group(2) @binding(0)
+var<storage, read> body_vels: array<Body::Velocity>;
+@group(2) @binding(1)
+var<storage, read> body_mprops: array<Body::MassProperties>;
+
 #if DIM == 2
 const WORKGROUP_SIZE_X: u32 = 8;
 const WORKGROUP_SIZE_Y: u32 = 8;
 const WORKGROUP_SIZE_Z: u32 = 1;
 const NUM_SHARED_CELLS: u32 = 10 * 10; // block-size plus 2 from adjacent blocks: (8 + 2)^2
+
+var<workgroup> shared_nodes_vel_mass: array<vec3<f32>, NUM_SHARED_CELLS>;
 #else
 const WORKGROUP_SIZE_X: u32 = 4;
 const WORKGROUP_SIZE_Y: u32 = 4;
 const WORKGROUP_SIZE_Z: u32 = 4;
 const NUM_SHARED_CELLS: u32 = 6 * 6 * 6; // block-size plus 2 from adjacent blocks: (4 + 2)^3
+
+var<workgroup> shared_nodes_vel_mass: array<vec4<f32>, NUM_SHARED_CELLS>;
 #endif
 
-var<workgroup> shared_nodes: array<Grid::Node, NUM_SHARED_CELLS>;
+var<workgroup> shared_nodes_cdf: array<Grid::NodeCdf, NUM_SHARED_CELLS>; // PERF: we don’t need the distance field from the cdf
 
 const WORKGROUP_SIZE: u32 = WORKGROUP_SIZE_X * WORKGROUP_SIZE_Y * WORKGROUP_SIZE_Z;
 @compute @workgroup_size(WORKGROUP_SIZE_X, WORKGROUP_SIZE_Y, WORKGROUP_SIZE_Z)
@@ -56,6 +59,8 @@ fn g2p(
     // Particle update. Runs g2p on shared memory only.
     let max_particle_id = (*active_block).first_particle + (*active_block).num_particles;
 
+    // PERF: could we rely on the particle linked lists instead of the sorted ids?
+    //       That could eliminate the cost of particle sorting.
     for (var sorted_particle_id = (*active_block).first_particle + tid_flat;
          sorted_particle_id < max_particle_id;
          sorted_particle_id += WORKGROUP_SIZE) {
@@ -78,17 +83,19 @@ fn global_shared_memory_transfers(tid: vec3<u32>, active_block_vid: Grid::BlockV
             let octant = vec2(i, j);
             let octant_hid = Grid::find_block_header_id(Grid::BlockVirtualId(base_block_pos_int + vec2<i32>(octant)));
             let shared_index = octant * 8 + tid.xy;
-            let shared_node = &shared_nodes[flatten_shared_index(shared_index.x, shared_index.y)];
+            let flat_shared_index = flatten_shared_index(shared_index.x, shared_index.y);
 
             if octant_hid.id != Grid::NONE {
                 let global_chunk_id = Grid::block_header_id_to_physical_id(octant_hid);
                 let global_node_id = Grid::node_id(global_chunk_id, tid.xy);
-                *shared_node = Grid::nodes[global_node_id.id];
+                shared_nodes_vel_mass[flat_shared_index] = Grid::nodes[global_node_id.id].momentum_velocity_mass;
+                shared_nodes_cdf[flat_shared_index] = Grid::nodes[global_node_id.id].cdf;
             } else {
                 // This octant doesn’t exist. Fill shared memory with zeros/NONE.
                 // NOTE: we don’t need to init global_id since it’s only read for the
                 //       current chunk that is guaranteed to exist, not the 2x2 adjacent ones.
-                *shared_node = Grid::Node(vec3(0.0));
+                shared_nodes_vel_mass[flat_shared_index] = vec3(0.0);
+                shared_nodes_cdf[flat_shared_index] = Grid::NodeCdf(0.0, 0, Grid::NONE);
             }
         }
     }
@@ -104,17 +111,19 @@ fn global_shared_memory_transfers(tid: vec3<u32>, active_block_vid: Grid::BlockV
                 let octant = vec3(i, j, k);
                 let octant_hid = Grid::find_block_header_id(Grid::BlockVirtualId(base_block_pos_int + vec3<i32>(octant)));
                 let shared_index = octant * 4 + tid;
-                let shared_node = &shared_nodes[flatten_shared_index(shared_index.x, shared_index.y, shared_index.z)];
+                let flat_shared_index = flatten_shared_index(shared_index.x, shared_index.y, shared_index.z);
 
                 if octant_hid.id != Grid::NONE {
                     let global_chunk_id = Grid::block_header_id_to_physical_id(octant_hid);
                     let global_node_id = Grid::node_id(global_chunk_id, tid);
-                    *shared_node = Grid::nodes[global_node_id.id];
+                    shared_nodes_vel_mass[flat_shared_index] = Grid::nodes[global_node_id.id].momentum_velocity_mass;
+                    shared_nodes_cdf[flat_shared_index] = Grid::nodes[global_node_id.id].cdf;
                 } else {
                     // This octant doesn’t exist. Fill shared memory with zeros/NONE.
                     // NOTE: we don’t need to init global_id since it’s only read for the
                     //       current chunk that is guaranteed to exist, not the 2x2x2 adjacent ones.
-                    *shared_node = Grid::Node(vec4(0.0));
+                    shared_nodes_vel_mass[flat_shared_index] = vec4(0.0);
+                    shared_nodes_cdf[flat_shared_index] = Grid::NodeCdf(0.0, 0, Grid::NONE);
                 }
             }
         }
@@ -129,9 +138,11 @@ fn particle_g2p(particle_id: u32, cell_width: f32, dt: f32) {
     var NBH_SHIFTS_SHARED = Kernel::NBH_SHIFTS_SHARED;
 
 #if DIM == 2
+    var rigid_vel = vec2<f32>(0.0);
     var momentum_velocity_mass = vec3<f32>(0.0);
     var velocity_gradient = mat2x2<f32>(vec2(0.0), vec2(0.0));
 #else
+    var rigid_vel = vec3<f32>(0.0);
     var momentum_velocity_mass = vec4<f32>(0.0);
     var velocity_gradient = mat3x3<f32>(vec3(0.0), vec3(0.0), vec3(0.0));
 #endif
@@ -139,6 +150,9 @@ fn particle_g2p(particle_id: u32, cell_width: f32, dt: f32) {
     // G2P
     {
         let particle_pos = particles_pos[particle_id];
+        let particle_vel = particles_dyn[particle_id].velocity;
+        let particle_cdf = particles_dyn[particle_id].cdf;
+
         let inv_d = Kernel::inv_d(cell_width);
         let ref_elt_pos_minus_particle_pos = Particle::dir_to_associated_grid_node(particle_pos, cell_width);
         let w = Kernel::precompute_weights(ref_elt_pos_minus_particle_pos, cell_width);
@@ -156,31 +170,70 @@ fn particle_g2p(particle_id: u32, cell_width: f32, dt: f32) {
         for (var i = 0u; i < Kernel::NBH_LEN; i += 1u) {
             let shift = NBH_SHIFTS[i];
             let packed_shift = NBH_SHIFTS_SHARED[i];
+            let shared_id = packed_cell_index_in_block + packed_shift;
+            let cell_data = shared_nodes_vel_mass[shared_id];
+            let cell_cdf = shared_nodes_cdf[shared_id];
+            let is_compatible = Grid::affinities_are_compatible(particle_cdf.affinity, cell_cdf.affinities);
 
 #if DIM == 2
             let dpt = ref_elt_pos_minus_particle_pos + vec2<f32>(shift) * cell_width;
-            let weight = w.x[shift.x] * w.y[shift.y];
-            let cell_data = shared_nodes[packed_cell_index_in_block + packed_shift].momentum_velocity_mass;
-            momentum_velocity_mass += cell_data * weight;
-            velocity_gradient += (weight * inv_d) * outer_product(cell_data.xy, dpt);
 #else
             let dpt = ref_elt_pos_minus_particle_pos + vec3<f32>(shift) * cell_width;
+#endif
+
+            var cpic_cell_data = cell_data;
+
+            if !is_compatible {
+                if cell_cdf.closest_id != Grid::NONE {
+                    let body_vel = body_vels[cell_cdf.closest_id]; // TODO: invalid if there is no body.
+                    let body_com = body_mprops[cell_cdf.closest_id].com;
+                    let cell_center = dpt + particle_pos.pt;
+                    let body_pt_vel =  Body::velocity_at_point(body_com, body_vel, cell_center);
+                    let particle_ghost_vel = body_pt_vel + Grid::project_velocity(particle_vel - body_pt_vel, particle_cdf.normal);
+
+#if DIM == 2
+                    cpic_cell_data = vec3(particle_ghost_vel, cell_data.z);
+#else
+                    cpic_cell_data = vec4(particle_ghost_vel, cell_data.w);
+#endif
+                } else {
+                    // If there is no adjacent collider, the ghost vel is the particle vel.
+#if DIM == 2
+                    cpic_cell_data = vec3(particle_vel, cell_data.z);
+#else
+                    cpic_cell_data = vec4(particle_vel, cell_data.w);
+#endif
+                }
+            }
+
+#if DIM == 2
+            let weight = w.x[shift.x] * w.y[shift.y];
+            momentum_velocity_mass += cpic_cell_data * weight;
+            velocity_gradient += (weight * inv_d) * outer_product(cpic_cell_data.xy, dpt);
+#else
             let weight = w.x[shift.x] * w.y[shift.y] * w.z[shift.z];
-            let cell_data = shared_nodes[packed_cell_index_in_block + packed_shift].momentum_velocity_mass;
-            momentum_velocity_mass += cell_data * weight;
-            velocity_gradient += (weight * inv_d) * outer_product(cell_data.xyz, dpt);
+            momentum_velocity_mass += cpic_cell_data * weight;
+            velocity_gradient += (weight * inv_d) * outer_product(cpic_cell_data.xyz, dpt);
 #endif
         }
-    }
 
+        for (var i = 0u; i < 16u; i++) {
+            if Grid::affinity_bit(i, particle_cdf.affinity) {
+                let body_vel = body_vels[i];
+                let body_com = body_mprops[i].com;
+                rigid_vel += Body::velocity_at_point(body_com, body_vel, particle_pos.pt);
+            }
+        }
+    }
 
+    particles_dyn[particle_id].cdf.rigid_vel = rigid_vel;
     // Set the particle velocity, and store the velocity gradient into the affine matrix.
     // The rest will be dealt with in the particle update kernel(s).
-    particles_affine[particle_id] = velocity_gradient;
+    particles_dyn[particle_id].affine = velocity_gradient;
 #if DIM == 2
-    particles_vel[particle_id].v = momentum_velocity_mass.xy;
+    particles_dyn[particle_id].velocity = momentum_velocity_mass.xy;
 #else
-    particles_vel[particle_id].v = momentum_velocity_mass.xyz;
+    particles_dyn[particle_id].velocity = momentum_velocity_mass.xyz;
 #endif
 }
 
@@ -213,4 +266,4 @@ fn outer_product(a: vec3<f32>, b: vec3<f32>) -> mat3x3<f32> {
 fn flatten_shared_index(x: u32, y: u32, z: u32) -> u32 {
     return x + y * 6 + z * 6 * 6;
 }
-#endif
\ No newline at end of file
+#endif
diff --git a/src/solver/g2p_cdf.rs b/src/solver/g2p_cdf.rs
new file mode 100644
index 0000000..bd802fa
--- /dev/null
+++ b/src/solver/g2p_cdf.rs
@@ -0,0 +1,55 @@
+use crate::grid::grid::{GpuGrid, WgGrid};
+use crate::grid::kernel::WgKernel;
+use crate::solver::params::{GpuSimulationParams, WgParams};
+use crate::solver::GpuParticles;
+use crate::solver::WgParticle;
+use crate::{dim_shader_defs, substitute_aliases};
+use naga_oil::compose::NagaModuleDescriptor;
+use wgcore::kernel::{KernelInvocationBuilder, KernelInvocationQueue};
+use wgcore::{utils, Shader};
+use wgebra::WgInv;
+use wgpu::ComputePipeline;
+
+#[derive(Shader)]
+#[shader(
+    derive(WgParams, WgParticle, WgGrid, WgKernel, WgInv),
+    src = "g2p_cdf.wgsl",
+    src_fn = "substitute_aliases",
+    shader_defs = "dim_shader_defs"
+)]
+pub struct WgG2PCdf {
+    pub g2p_cdf: ComputePipeline,
+}
+
+impl WgG2PCdf {
+    pub fn queue<'a>(
+        &'a self,
+        queue: &mut KernelInvocationQueue<'a>,
+        sim_params: &GpuSimulationParams,
+        grid: &GpuGrid,
+        particles: &GpuParticles,
+    ) {
+        KernelInvocationBuilder::new(queue, &self.g2p_cdf)
+            .bind_at(
+                0,
+                [
+                    (grid.meta.buffer(), 0),
+                    (grid.hmap_entries.buffer(), 1),
+                    (grid.active_blocks.buffer(), 2),
+                    (grid.nodes.buffer(), 3),
+                ],
+            )
+            .bind(
+                1,
+                [
+                    particles.positions.buffer(),
+                    particles.dynamics.buffer(),
+                    particles.sorted_ids.buffer(),
+                    sim_params.params.buffer(),
+                ],
+            )
+            .queue_indirect(grid.indirect_n_g2p_p2g_groups.clone());
+    }
+}
+
+wgcore::test_shader_compilation!(WgG2PCdf, wgcore, crate::dim_shader_defs());
diff --git a/src/solver/g2p_cdf.wgsl b/src/solver/g2p_cdf.wgsl
new file mode 100644
index 0000000..cd1f684
--- /dev/null
+++ b/src/solver/g2p_cdf.wgsl
@@ -0,0 +1,289 @@
+/*
+ * TODO: is there a way to re-use the G2P pattern accross shaders?
+ *       In particular, this is very similar to the `g2p.wgsl` file
+ *       but with different quantities being transfered.
+ */
+
+#define_import_path wgsparkl::solver::g2p
+
+#import wgebra::inv as Inv;
+#import wgsparkl::solver::params as Params;
+#import wgsparkl::solver::particle as Particle;
+#import wgsparkl::grid::kernel as Kernel;
+#import wgsparkl::grid::grid as Grid;
+
+@group(1) @binding(0)
+var<storage, read> particles_pos: array<Particle::Position>;
+@group(1) @binding(1)
+var<storage, read_write> particles_dyn: array<Particle::Dynamics>;
+@group(1) @binding(2)
+var<storage, read> sorted_particle_ids: array<u32>;
+@group(1) @binding(3)
+var<uniform> params: Params::SimulationParams;
+
+#if DIM == 2
+const WORKGROUP_SIZE_X: u32 = 8;
+const WORKGROUP_SIZE_Y: u32 = 8;
+const WORKGROUP_SIZE_Z: u32 = 1;
+const NUM_SHARED_CELLS: u32 = 10 * 10; // block-size plus 2 from adjacent blocks: (8 + 2)^2
+#else
+const WORKGROUP_SIZE_X: u32 = 4;
+const WORKGROUP_SIZE_Y: u32 = 4;
+const WORKGROUP_SIZE_Z: u32 = 4;
+const NUM_SHARED_CELLS: u32 = 6 * 6 * 6; // block-size plus 2 from adjacent blocks: (4 + 2)^3
+#endif
+
+var<workgroup> shared_nodes: array<Grid::NodeCdf, NUM_SHARED_CELLS>;
+
+const WORKGROUP_SIZE: u32 = WORKGROUP_SIZE_X * WORKGROUP_SIZE_Y * WORKGROUP_SIZE_Z;
+@compute @workgroup_size(WORKGROUP_SIZE_X, WORKGROUP_SIZE_Y, WORKGROUP_SIZE_Z)
+fn g2p_cdf(
+    @builtin(local_invocation_id) tid: vec3<u32>,
+    @builtin(local_invocation_index) tid_flat: u32,
+    @builtin(workgroup_id) block_id: vec3<u32>
+) {
+    let bid = block_id.x;
+
+    let active_block = &Grid::active_blocks[bid];
+    // Block -> shared memory transfer.
+    global_shared_memory_transfers(tid, (*active_block).virtual_id);
+
+    // Sync after shared memory initialization.
+    workgroupBarrier();
+
+    // Particle update. Runs g2p on shared memory only.
+    let max_particle_id = (*active_block).first_particle + (*active_block).num_particles;
+
+    for (var sorted_particle_id = (*active_block).first_particle + tid_flat;
+         sorted_particle_id < max_particle_id;
+         sorted_particle_id += WORKGROUP_SIZE) {
+        let particle_id = sorted_particle_ids[sorted_particle_id];
+        particle_g2p(particle_id, Grid::grid.cell_width, params.dt);
+    }
+}
+
+fn global_shared_memory_transfers(tid: vec3<u32>, active_block_vid: Grid::BlockVirtualId) {
+    let base_block_pos_int = active_block_vid.id;
+
+#if DIM == 2
+    for (var i = 0u; i <= 1u; i++) {
+        for (var j = 0u; j <= 1u; j++) {
+            if (i == 1 && tid.x > 1) || (j == 1 && tid.y > 1) {
+                // This shared node doesn’t exist.
+                continue;
+            }
+
+            let octant = vec2(i, j);
+            let octant_hid = Grid::find_block_header_id(Grid::BlockVirtualId(base_block_pos_int + vec2<i32>(octant)));
+            let shared_index = octant * 8 + tid.xy;
+            let flat_id = flatten_shared_index(shared_index.x, shared_index.y);
+            let shared_node = &shared_nodes[flat_id];
+
+            if octant_hid.id != Grid::NONE {
+                let global_chunk_id = Grid::block_header_id_to_physical_id(octant_hid);
+                let global_node_id = Grid::node_id(global_chunk_id, tid.xy);
+                *shared_node = Grid::nodes[global_node_id.id].cdf;
+            } else {
+                // This octant doesn’t exist. Fill shared memory with zeros/NONE.
+                // NOTE: we don’t need to init global_id since it’s only read for the
+                //       current chunk that is guaranteed to exist, not the 2x2 adjacent ones.
+                *shared_node = Grid::NodeCdf(0.0, 0, Grid::NONE);
+            }
+        }
+    }
+#else
+    for (var i = 0u; i <= 1u; i++) {
+        for (var j = 0u; j <= 1u; j++) {
+            for (var k = 0u; k <= 1u; k++) {
+                if (i == 1 && tid.x > 1) || (j == 1 && tid.y > 1) || (k == 1 && tid.z > 1) {
+                    // This shared node doesn’t exist.
+                    continue;
+                }
+
+                let octant = vec3(i, j, k);
+                let octant_hid = Grid::find_block_header_id(Grid::BlockVirtualId(base_block_pos_int + vec3<i32>(octant)));
+                let shared_index = octant * 4 + tid;
+                let shared_node = &shared_nodes[flatten_shared_index(shared_index.x, shared_index.y, shared_index.z)];
+
+                if octant_hid.id != Grid::NONE {
+                    let global_chunk_id = Grid::block_header_id_to_physical_id(octant_hid);
+                    let global_node_id = Grid::node_id(global_chunk_id, tid);
+                    *shared_node = Grid::nodes[global_node_id.id].cdf;
+                } else {
+                    // This octant doesn’t exist. Fill shared memory with zeros/NONE.
+                    // NOTE: we don’t need to init global_id since it’s only read for the
+                    //       current chunk that is guaranteed to exist, not the 2x2x2 adjacent ones.
+                    *shared_node = Grid::NodeCdf(0.0, 0, Grid::NONE);
+                }
+            }
+        }
+    }
+#endif
+}
+
+fn particle_g2p(particle_id: u32, cell_width: f32, dt: f32) {
+    // NOTE: having these into a var is needed so we can index [i] them.
+    //       Does this have any impact on performances?
+    var NBH_SHIFTS = Kernel::NBH_SHIFTS;
+    var NBH_SHIFTS_SHARED = Kernel::NBH_SHIFTS_SHARED;
+
+    var contact_dist = 0.0;
+    var contact_normal = vec3(0.0);
+    var particle_affinity = 0u;
+    // TODO: would using a mat4 be faster?
+    var affinity_signs = array(
+        0.0, 0.0, 0.0, 0.0,
+        0.0, 0.0, 0.0, 0.0,
+        0.0, 0.0, 0.0, 0.0,
+        0.0, 0.0, 0.0, 0.0
+    );
+
+    let prev_affinity = particles_dyn[particle_id].cdf.affinity;
+    let particle_pos = particles_pos[particle_id];
+    let inv_d = Kernel::inv_d(cell_width);
+    let ref_elt_pos_minus_particle_pos = Particle::dir_to_associated_grid_node(particle_pos, cell_width);
+    let w = Kernel::precompute_weights(ref_elt_pos_minus_particle_pos, cell_width);
+
+    let assoc_cell_before_integration = round(particle_pos.pt / cell_width);
+    let assoc_cell_index_in_block = Particle::associated_cell_index_in_block_off_by_one(particle_pos, cell_width);
+    let packed_cell_index_in_block = flatten_shared_index(
+        assoc_cell_index_in_block.x,
+        assoc_cell_index_in_block.y,
+#if DIM == 3
+        assoc_cell_index_in_block.z
+#endif
+    );
+
+    // Eqn. 21 to determine the sign bits.
+    // Also combines the affinity masks.
+    for (var i = 0u; i < Kernel::NBH_LEN; i += 1u) {
+        let shift = NBH_SHIFTS[i];
+        let packed_shift = NBH_SHIFTS_SHARED[i];
+        let cell_data = shared_nodes[packed_cell_index_in_block + packed_shift];
+        particle_affinity |= cell_data.affinities & Grid::AFFINITY_BITS_MASK;
+
+#if DIM == 2
+        let weight = w.x[shift.x] * w.y[shift.y];
+#else
+        let weight = w.x[shift.x] * w.y[shift.y] * w.z[shift.z];
+#endif
+
+        for (var i_collider = 0u; i_collider < 16u; i_collider += 1u) {
+            let compatible = f32(Grid::affinity_bit(i_collider, cell_data.affinities));
+            let sign = select(1.0, -1.0, Grid::sign_bit(i_collider, cell_data.affinities) && !shape_has_solid_interior(i_collider));
+            affinity_signs[i_collider] += compatible * weight * sign * cell_data.distance;
+        }
+    }
+
+    // Convert the affinity signs to bits.
+    for (var i_collider = 0u; i_collider < 16u; i_collider += 1u) {
+        // Only change the sign bit matching affinities that didn’t exist before.
+        let mask = 1u << (i_collider + Grid::SIGN_BITS_SHIFT);
+        if (prev_affinity & (1u << i_collider)) == 0 {
+            let sgn_bit = select(0u, mask, affinity_signs[i_collider] < 0.0);
+            particle_affinity |= sgn_bit;
+        } else {
+            particle_affinity |= prev_affinity & mask;
+        }
+    }
+
+    // At this state the `affinity` (+ sign) bitmask is filled.
+    // Now, compute the contact distance/normal using MLS reconstruction (Eq. 4)
+#if DIM == 2
+    var qtq = mat3x3f(); // Matrix M
+    var qtu = vec3(0.0);
+#else
+    var qtq = mat4x4f(); // Matrix M
+    var qtu = vec4(0.0);
+#endif
+
+    for (var i = 0u; i < Kernel::NBH_LEN; i += 1u) {
+        let shift = NBH_SHIFTS[i];
+        let packed_shift = NBH_SHIFTS_SHARED[i];
+        var cell_data = shared_nodes[packed_cell_index_in_block + packed_shift];
+#if DIM == 2
+        let dpt = ref_elt_pos_minus_particle_pos + vec2<f32>(shift) * cell_width;
+        let weight = w.x[shift.x] * w.y[shift.y];
+#else
+        let dpt = ref_elt_pos_minus_particle_pos + vec3<f32>(shift) * cell_width;
+        let weight = w.x[shift.x] * w.y[shift.y] * w.z[shift.z];
+#endif
+        let combined_affinity = cell_data.affinities & particle_affinity & Grid::AFFINITY_BITS_MASK;
+        let sign_differences = ((cell_data.affinities >> Grid::SIGN_BITS_SHIFT)
+            ^ (particle_affinity >> Grid::SIGN_BITS_SHIFT)) & combined_affinity;
+
+#if DIM == 2
+        let p = vec3(dpt, 1.0);
+#else
+        let p = vec4(dpt, 1.0);
+#endif
+
+        if combined_affinity != 0u {
+            if sign_differences == 0u {
+                // All signs match, positive distance.
+                qtq += outer_product(p, p) * weight;
+                qtu += p * weight * cell_data.distance;
+            } else { // if (sign_differences & (sign_differences - 1u)) != 0u {
+                // Exactly one sign difference, negative distance.
+                qtq += outer_product(p, p) * weight;
+                qtu += p * weight * -cell_data.distance;
+            }
+        }
+    }
+
+    if determinant(qtq) > 1.0e-8 {
+#if DIM == 2
+        let result = Inv::inv3(qtq) * qtu;
+        let len = length(result.xy);
+        let normal = select(vec2(0.0), result.xy / len, len > 1.0e-6);
+        // PERF: init the rigid-velocities here instead of in g2p?
+        particles_dyn[particle_id].cdf = Particle::Cdf(normal, vec2(0.0), result.z, particle_affinity);
+#else
+        let result = Inv::inv4(qtq) * qtu;
+        let normal = result.xyz / length(result.xyz);
+        particles_dyn[particle_id].cdf = Particle::Cdf(normal, vec3(0.0), result.w, particle_affinity);
+#endif
+    } else {
+        // TODO: store the affinity in this case too?
+        particles_dyn[particle_id].cdf = Particle::default_cdf();
+    }
+}
+
+fn shape_has_solid_interior(i_collider: u32) -> bool {
+    // TODO: needs to be false for unoriented trimeshes and polylines,
+    //       true for geometric primitives.
+    return false;
+}
+
+// TODO: upstream to wgebra?
+#if DIM == 2
+fn outer_product(a: vec3<f32>, b: vec3<f32>) -> mat3x3<f32> {
+    return mat3x3(
+        a * b.x,
+        a * b.y,
+        a * b.z,
+    );
+}
+
+// Note that this is different from p2g. We don’t need to shift the index since the truncated
+// blocks (the neighbor blocks) are in the quadrants with larger indices.
+fn flatten_shared_index(x: u32, y: u32) -> u32 {
+    return x + y * 10;
+}
+#else
+fn outer_product(a: vec4<f32>, b: vec4<f32>) -> mat4x4<f32> {
+    return mat4x4(
+        a * b.x,
+        a * b.y,
+        a * b.z,
+        a * b.w
+    );
+}
+
+
+// Note that this is different from p2g. We don’t need to shift the index since the truncated
+// blocks (the neighbor blocks) are in the octants with larger indices.
+fn flatten_shared_index(x: u32, y: u32, z: u32) -> u32 {
+    return x + y * 6 + z * 6 * 6;
+}
+#endif
\ No newline at end of file
diff --git a/src/solver/grid_update.rs b/src/solver/grid_update.rs
index 05205b8..1360058 100644
--- a/src/solver/grid_update.rs
+++ b/src/solver/grid_update.rs
@@ -1,16 +1,14 @@
-use crate::collision::WgCollide;
 use crate::dim_shader_defs;
 use crate::grid::grid::{GpuGrid, WgGrid};
 use crate::solver::params::GpuSimulationParams;
-use naga_oil::compose::NagaModuleDescriptor;
 use wgcore::kernel::{KernelInvocationBuilder, KernelInvocationQueue};
-use wgcore::{utils, Shader};
+use wgcore::Shader;
 use wgpu::ComputePipeline;
 use wgrapier::dynamics::GpuBodySet;
 
 #[derive(Shader)]
 #[shader(
-    derive(WgGrid, WgCollide),
+    derive(WgGrid),
     src = "grid_update.wgsl",
     shader_defs = "dim_shader_defs"
 )]
@@ -24,7 +22,6 @@ impl WgGridUpdate {
         queue: &mut KernelInvocationQueue<'a>,
         sim_params: &GpuSimulationParams,
         grid: &GpuGrid,
-        bodies: &GpuBodySet,
     ) {
         KernelInvocationBuilder::new(queue, &self.grid_update)
             .bind_at(
@@ -36,18 +33,8 @@ impl WgGridUpdate {
                     (sim_params.params.buffer(), 4),
                 ],
             )
-            .bind(1, [])
-            .bind(
-                2,
-                [
-                    bodies.shapes().buffer(),
-                    bodies.poses().buffer(),
-                    // bodies.vels().buffer(),
-                    // bodies.mprops().buffer(),
-                ],
-            )
             .queue_indirect(grid.indirect_n_g2p_p2g_groups.clone());
     }
 }
 
-wgcore::test_shader_compilation!(WgGridUpdate);
+wgcore::test_shader_compilation!(WgGridUpdate, wgcore, crate::dim_shader_defs());
diff --git a/src/solver/grid_update.wgsl b/src/solver/grid_update.wgsl
index 1d98b03..59fba5c 100644
--- a/src/solver/grid_update.wgsl
+++ b/src/solver/grid_update.wgsl
@@ -1,7 +1,6 @@
 #define_import_path wgsparkl::solver::grid_update
 
 #import wgsparkl::grid::grid as Grid;
-#import wgsparkl::collision::collide as Collide;
 
 #if DIM == 2
 const WORKGROUP_SIZE_X: u32 = 8;
@@ -14,7 +13,7 @@ const WORKGROUP_SIZE_Z: u32 = 4;
 #endif
 
 // NOTE: the only reason why this is its own kernel is because this makes us
-//       exceed the 10 storage bindings on web paltforms (because of the
+//       exceed the 10 storage bindings on web platforms (because of the
 //       collision-detection buffers).
 //       If we ever end up moving the collision-detection to particles only,
 //       we should consider doing the cell update in the p2g kernel.
@@ -50,7 +49,6 @@ fn update_single_cell(cell_pos: vec2<f32>, momentum_velocity_mass: vec3<f32>) ->
     // Clamp the velocity so it doesn’t exceed 1 grid cell in one step.
     let vel_limit = vec2(Grid::grid.cell_width / Grid::sim_params.dt);
     velocity = clamp(velocity, -vel_limit, vel_limit);
-    velocity = Collide::collide(cell_pos, velocity);
     return vec3(velocity, mass);
 }
 #else
@@ -62,7 +60,6 @@ fn update_single_cell(cell_pos: vec3<f32>, momentum_velocity_mass: vec4<f32>) ->
     // Clamp the velocity so it doesn’t exceed 1 grid cell in one step.
     let vel_limit = vec3(Grid::grid.cell_width / Grid::sim_params.dt);
     velocity = clamp(velocity, -vel_limit, vel_limit);
-    velocity = Collide::collide(cell_pos, velocity);
     return vec4(velocity, mass);
 }
 #endif
diff --git a/src/solver/grid_update_cdf.rs b/src/solver/grid_update_cdf.rs
new file mode 100644
index 0000000..787a597
--- /dev/null
+++ b/src/solver/grid_update_cdf.rs
@@ -0,0 +1,51 @@
+use crate::collision::WgCollide;
+use crate::dim_shader_defs;
+use crate::grid::grid::{GpuGrid, WgGrid};
+use crate::solver::params::GpuSimulationParams;
+use wgcore::kernel::{KernelInvocationBuilder, KernelInvocationQueue};
+use wgcore::Shader;
+use wgpu::ComputePipeline;
+use wgrapier::dynamics::GpuBodySet;
+
+#[derive(Shader)]
+#[shader(
+    derive(WgGrid, WgCollide),
+    src = "grid_update_cdf.wgsl",
+    shader_defs = "dim_shader_defs"
+)]
+pub struct WgGridUpdateCdf {
+    pub grid_update: ComputePipeline,
+}
+
+impl WgGridUpdateCdf {
+    pub fn queue<'a>(
+        &'a self,
+        queue: &mut KernelInvocationQueue<'a>,
+        sim_params: &GpuSimulationParams,
+        grid: &GpuGrid,
+        bodies: &GpuBodySet,
+    ) {
+        KernelInvocationBuilder::new(queue, &self.grid_update)
+            .bind_at(
+                0,
+                [
+                    (grid.meta.buffer(), 0),
+                    (grid.active_blocks.buffer(), 2),
+                    (grid.nodes.buffer(), 3),
+                ],
+            )
+            .bind(1, [])
+            .bind(
+                2,
+                [
+                    bodies.shapes().buffer(),
+                    bodies.poses().buffer(),
+                    // bodies.vels().buffer(),
+                    // bodies.mprops().buffer(),
+                ],
+            )
+            .queue_indirect(grid.indirect_n_g2p_p2g_groups.clone());
+    }
+}
+
+wgcore::test_shader_compilation!(WgGridUpdateCdf, wgcore, crate::dim_shader_defs());
diff --git a/src/solver/grid_update_cdf.wgsl b/src/solver/grid_update_cdf.wgsl
new file mode 100644
index 0000000..eca4004
--- /dev/null
+++ b/src/solver/grid_update_cdf.wgsl
@@ -0,0 +1,39 @@
+#define_import_path wgsparkl::solver::grid_update
+
+#import wgsparkl::grid::grid as Grid;
+#import wgsparkl::collision::collide as Collide;
+
+#if DIM == 2
+const WORKGROUP_SIZE_X: u32 = 8;
+const WORKGROUP_SIZE_Y: u32 = 8;
+const WORKGROUP_SIZE_Z: u32 = 1;
+#else
+const WORKGROUP_SIZE_X: u32 = 4;
+const WORKGROUP_SIZE_Y: u32 = 4;
+const WORKGROUP_SIZE_Z: u32 = 4;
+#endif
+
+@compute @workgroup_size(WORKGROUP_SIZE_X, WORKGROUP_SIZE_Y, WORKGROUP_SIZE_Z)
+fn grid_update(
+    @builtin(local_invocation_id) tid: vec3<u32>,
+    @builtin(local_invocation_index) tid_flat: u32,
+    @builtin(workgroup_id) block_id: vec3<u32>
+) {
+    let bid = block_id.x;
+    let vid = Grid::active_blocks[bid].virtual_id;
+
+    let global_chunk_id = Grid::block_header_id_to_physical_id(Grid::BlockHeaderId(bid));
+#if DIM == 2
+    let global_node_id = Grid::node_id(global_chunk_id, tid.xy);
+    let cell_pos = vec2<f32>(vid.id * 8 + vec2<i32>(tid.xy)) * Grid::grid.cell_width;
+#else
+    let global_node_id = Grid::node_id(global_chunk_id, tid);
+    let cell_pos = vec3<f32>(vid.id * 4 + vec3<i32>(tid)) * Grid::grid.cell_width;
+#endif
+
+    // PERF: store the list of blocks with a collision so we can skip the ones without
+    //       collisions in teh other `_cdf` kernels.
+    //       Or maybe just use some sort of flag and skip the block at the start of the kernel?
+    let global_id = global_node_id.id;
+    Grid::nodes[global_id].cdf = Collide::collide(Grid::grid.cell_width, cell_pos);
+}
diff --git a/src/solver/mod.rs b/src/solver/mod.rs
index a35b1c8..974099a 100644
--- a/src/solver/mod.rs
+++ b/src/solver/mod.rs
@@ -1,20 +1,30 @@
 pub use g2p::WgG2P;
+pub use g2p_cdf::WgG2PCdf;
 pub use p2g::WgP2G;
+pub use p2g_cdf::WgP2GCdf;
 pub use params::{GpuSimulationParams, SimulationParams, WgParams};
 #[cfg(feature = "dim2")]
-pub use particle2d::{GpuParticles, Particle, ParticleMassProps, WgParticle};
+pub use particle2d::{GpuParticles, GpuRigidParticles, Particle, ParticleDynamics, WgParticle};
 #[cfg(feature = "dim3")]
-pub use particle3d::{GpuParticles, Particle, ParticleMassProps, WgParticle};
+pub use particle3d::{GpuParticles, GpuRigidParticles, Particle, ParticleDynamics, WgParticle};
 // pub use particle_update::WgParticleUpdate;
 pub use grid_update::WgGridUpdate;
+pub use grid_update_cdf::WgGridUpdateCdf;
 pub use particle_update::{ParticlePhase, WgParticleUpdate};
+pub use rigid_impulses::{GpuImpulses, RigidImpulse, WgRigidImpulses};
+pub use rigid_particle_update::WgRigidParticleUpdate;
 
 mod g2p;
+mod g2p_cdf;
 mod p2g;
+mod p2g_cdf;
 mod params;
 mod particle_update;
+mod rigid_impulses;
+mod rigid_particle_update;
 
 mod grid_update;
+mod grid_update_cdf;
 #[cfg(feature = "dim2")]
 mod particle2d;
 #[cfg(feature = "dim3")]
diff --git a/src/solver/p2g.rs b/src/solver/p2g.rs
index a7be161..a9ddfc0 100644
--- a/src/solver/p2g.rs
+++ b/src/solver/p2g.rs
@@ -3,17 +3,27 @@ use crate::grid::grid::{GpuGrid, WgGrid};
 use crate::grid::kernel::WgKernel;
 use crate::models::WgLinearElasticity;
 use crate::solver::params::WgParams;
-use crate::solver::GpuParticles;
-use crate::solver::WgParticle;
-use naga_oil::compose::NagaModuleDescriptor;
+use crate::solver::{GpuImpulses, GpuParticles};
+use crate::solver::{WgParticle, WgRigidImpulses};
+use crate::substitute_aliases;
 use wgcore::kernel::{KernelInvocationBuilder, KernelInvocationQueue};
-use wgcore::{utils, Shader};
+use wgcore::Shader;
 use wgpu::ComputePipeline;
+use wgrapier::dynamics::{GpuBodySet, WgBody};
 
 #[derive(Shader)]
 #[shader(
-    derive(WgParams, WgParticle, WgGrid, WgLinearElasticity, WgKernel),
+    derive(
+        WgParams,
+        WgParticle,
+        WgGrid,
+        WgLinearElasticity,
+        WgKernel,
+        WgBody,
+        WgRigidImpulses,
+    ),
     src = "p2g.wgsl",
+    src_fn = "substitute_aliases",
     shader_defs = "dim_shader_defs"
 )]
 pub struct WgP2G {
@@ -26,30 +36,37 @@ impl WgP2G {
         queue: &mut KernelInvocationQueue<'a>,
         grid: &GpuGrid,
         particles: &GpuParticles,
+        impulses: &GpuImpulses,
+        bodies: &GpuBodySet,
     ) {
         KernelInvocationBuilder::new(queue, &self.p2g)
-            .bind(
+            .bind_at(
                 0,
                 [
-                    grid.meta.buffer(),
-                    grid.hmap_entries.buffer(),
-                    grid.active_blocks.buffer(),
-                    grid.nodes.buffer(),
+                    (grid.meta.buffer(), 0),
+                    (grid.hmap_entries.buffer(), 1),
+                    (grid.active_blocks.buffer(), 2),
+                    (grid.nodes.buffer(), 3),
                 ],
             )
             .bind(
                 1,
                 [
                     particles.positions.buffer(),
-                    particles.velocities.buffer(),
-                    particles.volumes.buffer(),
-                    particles.affines.buffer(),
+                    particles.dynamics.buffer(),
                     grid.nodes_linked_lists.buffer(),
                     particles.node_linked_lists.buffer(),
                 ],
             )
+            .bind(
+                2,
+                [
+                    bodies.vels().buffer(),
+                    impulses.incremental_impulses.buffer(),
+                ],
+            )
             .queue_indirect(grid.indirect_n_g2p_p2g_groups.clone());
     }
 }
 
-wgcore::test_shader_compilation!(WgP2G);
+wgcore::test_shader_compilation!(WgP2G, wgcore, crate::dim_shader_defs());
diff --git a/src/solver/p2g.wgsl b/src/solver/p2g.wgsl
index a2ac6db..b56250b 100644
--- a/src/solver/p2g.wgsl
+++ b/src/solver/p2g.wgsl
@@ -4,25 +4,25 @@
 #import wgsparkl::solver::particle as Particle;
 #import wgsparkl::grid::kernel as Kernel;
 #import wgsparkl::grid::grid as Grid;
+#import wgsparkl::solver::impulse as Impulse;
+#import wgrapier::body as Body;
+
 
 @group(1) @binding(0)
 var<storage, read> particles_pos: array<Particle::Position>;
 @group(1) @binding(1)
-var<storage, read> particles_vel: array<Particle::Velocity>;
+var<storage, read> particles_dyn: array<Particle::Dynamics>;
 @group(1) @binding(2)
-var<storage, read> particles_vol: array<Particle::Volume>;
-#if DIM == 2
-@group(1) @binding(3)
-var<storage, read> particles_affine: array<mat2x2<f32>>;
-#else
-@group(1) @binding(3)
-var<storage, read> particles_affine: array<mat3x3<f32>>;
-#endif
-@group(1) @binding(4)
 var<storage, read> nodes_linked_lists: array<Grid::NodeLinkedList>;
-@group(1) @binding(5)
+@group(1) @binding(3)
 var<storage, read> particle_node_linked_lists: array<u32>;
 
+@group(2) @binding(0)
+var<storage, read> body_vels: array<Body::Velocity>;
+@group(2) @binding(1)
+var<storage, read_write> body_impulses: array<Impulse::IntegerImpulseAtomic>;
+
+
 
 #if DIM == 2
 const WORKGROUP_SIZE_X: u32 = 8;
@@ -41,6 +41,8 @@ var<workgroup> shared_affine: array<mat3x3<f32>, NUM_SHARED_CELLS>;
 #endif
 var<workgroup> shared_nodes: array<SharedNode, NUM_SHARED_CELLS>;
 var<workgroup> shared_pos: array<Particle::Position, NUM_SHARED_CELLS>;
+var<workgroup> shared_affinities: array<u32, NUM_SHARED_CELLS>;
+var<workgroup> shared_normals: array<Vector, NUM_SHARED_CELLS>;
 // TODO: is computing themax with an atomic faster than doing a reduction?
 var<workgroup> max_linked_list_length: atomic<u32>;
 // NOTE: workgroupUniformLoad doesn’t work on atomics, so we need that additional variable
@@ -52,6 +54,18 @@ struct SharedNode {
     global_id: u32,
 }
 
+struct P2GStepResult {
+#if DIM == 2
+    new_momentum_velocity_mass: vec3<f32>,
+    impulse: vec2<f32>,
+    ang_impulse: f32,
+#else
+    new_momentum_velocity_mass: vec4<f32>,
+    impulse: vec3<f32>,
+    ang_impulse: vec3<f32>,
+#endif
+}
+
 @compute @workgroup_size(WORKGROUP_SIZE_X, WORKGROUP_SIZE_Y, WORKGROUP_SIZE_Z)
 fn p2g(
     @builtin(local_invocation_id) tid: vec3<u32>,
@@ -86,14 +100,23 @@ fn p2g(
     let packed_cell_index_in_block = flatten_shared_index(tid.x + 4u, tid.y + 4u, tid.z + 4u);
 #endif
 
+    // TODO: we store the global_id in shared memory for convenience. Should we just recompute it instead?
+    let global_id = shared_nodes[packed_cell_index_in_block].global_id;
+    let node_affinities = Grid::nodes[global_id].cdf.affinities;
+    let collider_id = Grid::nodes[global_id].cdf.closest_id;
+    var total_result = P2GStepResult();
+
     // NOTE: read the linked list with workgroupUniformLoad so that is is considered
-    //       part of a uniform execution flow (for the barriers to be valids).
+    //       part of a uniform execution flow (for the barriers to be valid).
     let len = workgroupUniformLoad(&max_linked_list_length_uniform);
     for (var i = 0u; i < len; i += 1u) {
         workgroupBarrier();
         fetch_next_particle(tid);
         workgroupBarrier();
-        new_momentum_velocity_mass += p2g_step(packed_cell_index_in_block, Grid::grid.cell_width);
+        let partial_result = p2g_step(packed_cell_index_in_block, Grid::grid.cell_width, node_affinities, collider_id);
+        total_result.new_momentum_velocity_mass += partial_result.new_momentum_velocity_mass;
+        total_result.impulse += partial_result.impulse;
+        total_result.ang_impulse += partial_result.ang_impulse;
     }
 
     // Grid update.
@@ -110,16 +133,29 @@ fn p2g(
 //       we should consider doing the cell update in the p2g kernel.
 
     // Write the node state to global memory.
-    // TODO: we store the global_id in shared memory for convenience. Should we just recompute it instead?
-    let global_id = shared_nodes[packed_cell_index_in_block].global_id;
-    Grid::nodes[global_id].momentum_velocity_mass = new_momentum_velocity_mass;
-}
+    Grid::nodes[global_id].momentum_velocity_mass = total_result.new_momentum_velocity_mass;
+    // Apply the impulse to the closest body.
+    // PERF: we should probably run a reduction here to get per-collider accumulated impulses
+    //       before adding to global memory. Because it is very likely that every single thread
+    //       here targets the same body.
 
+    if collider_id != Grid::NONE {
 #if DIM == 2
-fn p2g_step(packed_cell_index_in_block: u32, cell_width: f32) -> vec3<f32> {
+        atomicAdd(&body_impulses[collider_id].linear_x, Impulse::flt2int(total_result.impulse.x));
+        atomicAdd(&body_impulses[collider_id].linear_y, Impulse::flt2int(total_result.impulse.y));
+        atomicAdd(&body_impulses[collider_id].angular, Impulse::flt2int(total_result.ang_impulse));
 #else
-fn p2g_step(packed_cell_index_in_block: u32, cell_width: f32) -> vec4<f32> {
+        atomicAdd(&body_impulses[collider_id].linear_x, Impulse::flt2int(total_result.impulse.x));
+        atomicAdd(&body_impulses[collider_id].linear_y, Impulse::flt2int(total_result.impulse.y));
+        atomicAdd(&body_impulses[collider_id].linear_z, Impulse::flt2int(total_result.impulse.z));
+        atomicAdd(&body_impulses[collider_id].angular_x, Impulse::flt2int(total_result.ang_impulse.x));
+        atomicAdd(&body_impulses[collider_id].angular_y, Impulse::flt2int(total_result.ang_impulse.y));
+        atomicAdd(&body_impulses[collider_id].angular_z, Impulse::flt2int(total_result.ang_impulse.z));
 #endif
+    }
+}
+
+fn p2g_step(packed_cell_index_in_block: u32, cell_width: f32, node_affinity: u32, collider_id: u32) -> P2GStepResult {
     // NOTE: having these into a var is needed so we can index [i] them.
     //       Does this have any impact on performances?
     var NBH_SHIFTS = Kernel::NBH_SHIFTS;
@@ -133,16 +169,12 @@ fn p2g_step(packed_cell_index_in_block: u32, cell_width: f32) -> vec4<f32> {
     let bottommost_contributing_node = flatten_shared_shift(2u, 2u, 2u);
     var new_momentum_velocity_mass = vec4(0.0);
 #endif
+    var impulse = Vector(0.0);
+    var ang_impulse = AngVector(0.0);
 
     for (var i = 0u; i < Kernel::NBH_LEN; i += 1u) {
         let packed_shift = NBH_SHIFTS_SHARED[i];
         let nbh_shared_index = packed_cell_index_in_block - bottommost_contributing_node + packed_shift;
-
-#if DIM == 2
-        let shift = vec2(2u, 2) - NBH_SHIFTS[i];
-#else
-        let shift = vec3(2u, 2, 2) - NBH_SHIFTS[i];
-#endif
         let particle_pos = shared_pos[nbh_shared_index];
         let particle_vel_mass = shared_vel_mass[nbh_shared_index];
         let particle_affine = shared_affine[nbh_shared_index];
@@ -151,19 +183,56 @@ fn p2g_step(packed_cell_index_in_block: u32, cell_width: f32) -> vec4<f32> {
         let w = Kernel::precompute_weights(ref_elt_pos_minus_particle_pos, cell_width);
 
 #if DIM == 2
-        let momentum = particle_vel_mass.xy * particle_vel_mass.z;
+        let particle_vel = particle_vel_mass.xy;
+        let particle_mass = particle_vel_mass.z;
+        let shift = vec2(2u, 2) - NBH_SHIFTS[i];
+        let momentum = particle_vel * particle_mass;
         let dpt = ref_elt_pos_minus_particle_pos + vec2<f32>(shift) * cell_width; // cell_pos - particle_pos
         let weight = w.x[shift.x] * w.y[shift.y];
-        new_momentum_velocity_mass += vec3(particle_affine * dpt + momentum, particle_vel_mass.z) * weight;
 #else
-        let momentum = particle_vel_mass.xyz * particle_vel_mass.w;
+        let particle_vel = particle_vel_mass.xyz;
+        let particle_mass = particle_vel_mass.w;
+        let shift = vec3(2u, 2, 2) - NBH_SHIFTS[i];
+        let momentum = particle_vel * particle_mass;
         let dpt = ref_elt_pos_minus_particle_pos + vec3<f32>(shift) * cell_width; // cell_pos - particle_pos
         let weight = w.x[shift.x] * w.y[shift.y] * w.z[shift.z];
-        new_momentum_velocity_mass += vec4(particle_affine * dpt + momentum, particle_vel_mass.w) * weight;
 #endif
+
+        let particle_affinity = shared_affinities[nbh_shared_index];
+        if !Grid::affinities_are_compatible(node_affinity, particle_affinity) {
+            if collider_id != Grid::NONE {
+                let particle_normal = shared_normals[nbh_shared_index];
+                let body_vel = body_vels[collider_id];
+                let body_com = body_impulses[collider_id].com;
+                let cell_center = dpt + particle_pos.pt;
+                let body_pt_vel =  Body::velocity_at_point(body_com, body_vel, cell_center);
+                let particle_ghost_vel = body_pt_vel + Grid::project_velocity(particle_vel - body_pt_vel, particle_normal);
+                let delta_impulse = (particle_vel - particle_ghost_vel) * (weight * particle_mass);
+
+                // TODO: we could do the ang impulse calcs only once after all the `p2g_step` executions.
+                let lever_arm = body_com - cell_center;
+
+    #if DIM == 2
+                let delta_ang_impulse = dot(delta_impulse, vec2(lever_arm.y, -lever_arm.x));
+    #else
+                let delta_ang_impulse = cross(delta_impulse, lever_arm);
+    #endif
+
+                ang_impulse += delta_ang_impulse;
+                impulse += delta_impulse;
+
+                continue;
+            }
+        } else {
+#if DIM == 2
+            new_momentum_velocity_mass += vec3(particle_affine * dpt + momentum, particle_mass) * weight;
+#else
+            new_momentum_velocity_mass += vec4(particle_affine * dpt + momentum, particle_mass) * weight;
+#endif
+        }
     }
 
-    return new_momentum_velocity_mass;
+    return P2GStepResult(new_momentum_velocity_mass, impulse, ang_impulse);
 }
 
 #if DIM == 2
@@ -292,13 +361,15 @@ fn fetch_next_particle(tid: vec3<u32>) {
                 let curr_particle_id = (*shared_node).particle_id;
 
                 if curr_particle_id != Grid::NONE {
+                    shared_affinities[shared_flat_index] = particles_dyn[curr_particle_id].cdf.affinity;
+                    shared_normals[shared_flat_index] = particles_dyn[curr_particle_id].cdf.normal;
                     shared_pos[shared_flat_index] = particles_pos[curr_particle_id];
-                    shared_affine[shared_flat_index] = particles_affine[curr_particle_id];
+                    shared_affine[shared_flat_index] = particles_dyn[curr_particle_id].affine;
 
 #if DIM == 2
-                    shared_vel_mass[shared_flat_index] = vec3(particles_vel[curr_particle_id].v, Particle::mass(particles_vol[curr_particle_id]));
+                    shared_vel_mass[shared_flat_index] = vec3(particles_dyn[curr_particle_id].velocity, particles_dyn[curr_particle_id].mass);
 #else
-                    shared_vel_mass[shared_flat_index] = vec4(particles_vel[curr_particle_id].v, Particle::mass(particles_vol[curr_particle_id]));
+                    shared_vel_mass[shared_flat_index] = vec4(particles_dyn[curr_particle_id].velocity, particles_dyn[curr_particle_id].mass);
 #endif
 
                     let next_particle_id = particle_node_linked_lists[curr_particle_id];
@@ -307,6 +378,8 @@ fn fetch_next_particle(tid: vec3<u32>) {
                     // TODO: would it be worth skipping writing zeros if we already
                     //       did it at the previous step? (if we already reached the end
                     //       of the particle linked list)
+                    shared_affinities[shared_flat_index] = 0u;
+                    shared_normals[shared_flat_index] = Vector(0.0);
 #if DIM == 2
                     shared_pos[shared_flat_index].pt = vec2(0.0);
                     shared_affine[shared_flat_index] = mat2x2(vec2(0.0), vec2(0.0));
diff --git a/src/solver/p2g_cdf.rs b/src/solver/p2g_cdf.rs
new file mode 100644
index 0000000..77347ad
--- /dev/null
+++ b/src/solver/p2g_cdf.rs
@@ -0,0 +1,58 @@
+use crate::dim_shader_defs;
+use crate::grid::grid::{GpuGrid, WgGrid};
+use crate::grid::kernel::WgKernel;
+use crate::models::WgLinearElasticity;
+use crate::solver::params::WgParams;
+use crate::solver::{GpuImpulses, GpuParticles, GpuRigidParticles};
+use crate::solver::{WgParticle, WgRigidImpulses};
+use crate::substitute_aliases;
+use wgcore::kernel::{KernelInvocationBuilder, KernelInvocationQueue};
+use wgcore::Shader;
+use wgparry::segment::WgSegment;
+use wgparry::triangle::WgTriangle;
+use wgpu::ComputePipeline;
+use wgrapier::dynamics::{GpuBodySet, WgBody};
+
+#[derive(Shader)]
+#[shader(
+    derive(WgParams, WgParticle, WgKernel, WgGrid, WgBody, WgSegment, WgTriangle),
+    src = "p2g_cdf.wgsl",
+    src_fn = "substitute_aliases",
+    shader_defs = "dim_shader_defs"
+)]
+pub struct WgP2GCdf {
+    pub p2g_cdf: ComputePipeline,
+}
+
+impl WgP2GCdf {
+    pub fn queue<'a>(
+        &'a self,
+        queue: &mut KernelInvocationQueue<'a>,
+        grid: &GpuGrid,
+        particles: &GpuRigidParticles,
+        bodies: &GpuBodySet,
+    ) {
+        KernelInvocationBuilder::new(queue, &self.p2g_cdf)
+            .bind_at(
+                0,
+                [
+                    (grid.meta.buffer(), 0),
+                    (grid.hmap_entries.buffer(), 1),
+                    (grid.active_blocks.buffer(), 2),
+                    (grid.nodes.buffer(), 3),
+                ],
+            )
+            .bind(
+                1,
+                [
+                    grid.nodes_rigid_linked_lists.buffer(),
+                    particles.node_linked_lists.buffer(),
+                    particles.sample_ids.buffer(),
+                    bodies.shapes_vertex_buffers().buffer(),
+                ],
+            )
+            .queue_indirect(grid.indirect_n_g2p_p2g_groups.clone());
+    }
+}
+
+wgcore::test_shader_compilation!(WgP2GCdf, wgcore, crate::dim_shader_defs());
diff --git a/src/solver/p2g_cdf.wgsl b/src/solver/p2g_cdf.wgsl
new file mode 100644
index 0000000..0070159
--- /dev/null
+++ b/src/solver/p2g_cdf.wgsl
@@ -0,0 +1,367 @@
+#define_import_path wgsparkl::solver::p2g
+
+#if DIM == 2
+    #import wgparry::segment as Shape;
+#else
+    #import wgparry::triangle as Shape;
+#endif
+
+#import wgsparkl::solver::params as Params;
+#import wgsparkl::solver::particle as Particle;
+#import wgsparkl::grid::kernel as Kernel;
+#import wgsparkl::grid::grid as Grid;
+#import wgrapier::body as Body;
+
+@group(1) @binding(0)
+var<storage, read> nodes_linked_lists: array<Grid::NodeLinkedList>;
+@group(1) @binding(1)
+var<storage, read> particle_node_linked_lists: array<u32>;
+@group(1) @binding(2)
+var<storage, read> rigid_particle_indices: array<Particle::RigidParticleIndices>;
+@group(1) @binding(3)
+var<storage, read> collider_vertices: array<Vector>;
+
+
+#if DIM == 2
+const WORKGROUP_SIZE_X: u32 = 8;
+const WORKGROUP_SIZE_Y: u32 = 8;
+const WORKGROUP_SIZE_Z: u32 = 1;
+const NUM_SHARED_CELLS: u32 = 10 * 10; // block-size plus 2 from adjacent blocks: (8 + 2)^2
+var<workgroup> shared_primitives: array<Shape::Segment, NUM_SHARED_CELLS>;
+#else
+const WORKGROUP_SIZE_X: u32 = 4;
+const WORKGROUP_SIZE_Y: u32 = 4;
+const WORKGROUP_SIZE_Z: u32 = 4;
+const NUM_SHARED_CELLS: u32 = 6 * 6 * 6; // block-size plus 2 from adjacent blocks: (4 + 2)^3
+var<workgroup> shared_primitives: array<Shape::Triangle, NUM_SHARED_CELLS>;
+#endif
+var<workgroup> shared_nodes: array<SharedNode, NUM_SHARED_CELLS>;
+var<workgroup> shared_collider_ids: array<u32, NUM_SHARED_CELLS>;
+// TODO: is computing themax with an atomic faster than doing a reduction?
+var<workgroup> max_linked_list_length: atomic<u32>;
+// NOTE: workgroupUniformLoad doesn’t work on atomics, so we need that additional variable
+//       to write `max_linked_list_length` into and then read with workgroupUniformLoad.
+var<workgroup> max_linked_list_length_uniform: u32;
+
+struct SharedNode {
+    particle_id: u32,
+    global_id: u32,
+}
+
+@compute @workgroup_size(WORKGROUP_SIZE_X, WORKGROUP_SIZE_Y, WORKGROUP_SIZE_Z)
+fn p2g_cdf(
+    @builtin(local_invocation_id) tid: vec3<u32>,
+    @builtin(local_invocation_index) tid_flat: u32,
+    @builtin(workgroup_id) block_id: vec3<u32>
+) {
+    let bid = block_id.x;
+    let vid = Grid::active_blocks[bid].virtual_id;
+
+    // Figure out how many time we’ll have to iterate through
+    // the particle linked-list to traverse them all.
+    if tid_flat == 0 {
+        // Technically not needed because the WebGpu spec. requires zeroing shared memory, but wgpu has an option to disable that.
+        atomicStore(&max_linked_list_length, 0u);
+    }
+    workgroupBarrier();
+    fetch_max_linked_lists_length(tid, vid, bid);
+    workgroupBarrier();
+    max_linked_list_length_uniform = max_linked_list_length;
+
+    // Block -> shared memory transfer.
+    fetch_nodes(tid, vid, bid);
+
+    /* Run p2g. Note that we have one thread per cell we want to gather data into. */
+    // NOTE: we shift by (8, 8) or (4, 4, 4) so we are in the top-most octant. This is the octant we
+    //       got enough information for a full gather.
+#if DIM == 2
+    let packed_cell_index_in_block = flatten_shared_index(tid.x + 8u, tid.y + 8u);
+    let cell_pos = vec2<f32>(vid.id * 8 + vec2<i32>(tid.xy)) * Grid::grid.cell_width;
+#else
+    let packed_cell_index_in_block = flatten_shared_index(tid.x + 4u, tid.y + 4u, tid.z + 4u);
+    let cell_pos = vec3<f32>(vid.id * 4 + vec3<i32>(tid)) * Grid::grid.cell_width;
+#endif
+
+    // TODO: we store the global_id in shared memory for convenience. Should we just recompute it instead?
+    let global_id = shared_nodes[packed_cell_index_in_block].global_id;
+    var node_cdf = Grid::nodes[global_id].cdf;
+    let node_affinities = node_cdf.affinities;
+
+    // NOTE: read the linked list with workgroupUniformLoad so that is is considered
+    //       part of a uniform execution flow (for the barriers to be valid).
+    let len = workgroupUniformLoad(&max_linked_list_length_uniform);
+    for (var i = 0u; i < len; i += 1u) {
+        workgroupBarrier();
+        fetch_next_particle(tid);
+        workgroupBarrier();
+        let partial_result = p2g_step(packed_cell_index_in_block, Grid::grid.cell_width, cell_pos);
+
+        if partial_result.closest_id != Grid::NONE {
+            node_cdf.affinities |= partial_result.affinities;
+
+            if partial_result.distance < node_cdf.distance {
+                node_cdf.distance = partial_result.distance;
+                node_cdf.closest_id = partial_result.closest_id;
+            }
+        }
+    }
+
+    // Write the node cdf to global memory.
+    Grid::nodes[global_id].cdf = node_cdf;
+}
+
+fn p2g_step(packed_cell_index_in_block: u32, cell_width: f32, cell_pos: Vector) -> Grid::NodeCdf {
+    // NOTE: having these into a var is needed so we can index [i] them.
+    //       Does this have any impact on performances?
+    var NBH_SHIFTS_SHARED = Kernel::NBH_SHIFTS_SHARED;
+
+    // Shift to reach the first node with particles contibuting to the current cell’s data.
+#if DIM == 2
+    let bottommost_contributing_node = flatten_shared_shift(2u, 2u);
+#else
+    let bottommost_contributing_node = flatten_shared_shift(2u, 2u, 2u);
+#endif
+    var result = Grid::NodeCdf(1.0e10, 0u, Grid::NONE);
+
+    for (var i = 0u; i < Kernel::NBH_LEN; i += 1u) {
+        let packed_shift = NBH_SHIFTS_SHARED[i];
+        let nbh_shared_index = packed_cell_index_in_block - bottommost_contributing_node + packed_shift;
+
+        let collider_id = shared_collider_ids[nbh_shared_index];
+
+        if collider_id == Grid::NONE {
+            continue;
+        }
+
+        let primitive = shared_primitives[nbh_shared_index];
+
+#if DIM == 2
+        /*
+         * Project on Segment
+         */
+        let proj = Shape::projectLocalPoint(primitive, cell_pos);
+        if any(proj != primitive.a) && any(proj != primitive.b) {
+            // This is a valid projection.
+            let dpt = cell_pos - proj;
+            let distance = length(dpt);
+            let ab = primitive.b - primitive.a;
+            let sign = dot(dpt, vec2(-ab.y, ab.x)) < 0.0;
+            result.affinities |= (1u << collider_id)
+                | (u32(sign) << (collider_id + Grid::SIGN_BITS_SHIFT));
+
+            if distance < result.distance {
+                result.distance = min(result.distance, distance);
+                result.closest_id = collider_id;
+            }
+        }
+#else
+        /*
+         * Project on Triangle
+         */
+        let ap = cell_pos - primitive.a;
+        let bp = cell_pos - primitive.b;
+        let cp = cell_pos - primitive.c;
+        let ab = primitive.b - primitive.a;
+        let ac = primitive.c - primitive.a;
+        let bc = primitive.c - primitive.b;
+        let n = cross(ab, ac);
+        let n_length = length(n);
+
+        if n_length != 0.0
+            && dot(cross(ab, n), ap) <= 0.0
+            && dot(cross(bc, n), bp) <= 0.0
+            && dot(cross(ac, n), cp) >= 0.0 // Positive sign due to calculating with `ac` instead of `ca`.
+         {
+             // This is a valid projection on the face interior.
+             let signed_dist = dot(n, ap) / n_length;
+             let sign = signed_dist < 0.0;
+             let distance = abs(signed_dist);
+             result.affinities |= (1u << collider_id)
+                 | (u32(sign) << (collider_id + Grid::SIGN_BITS_SHIFT));
+
+             if distance < result.distance {
+                 result.distance = min(result.distance, distance);
+                 result.closest_id = collider_id;
+             }
+        }
+#endif
+    }
+
+    return result;
+}
+
+#if DIM == 2
+    const K_RANGE: u32 = 0;
+#else
+    const K_RANGE: u32 = 1;
+#endif
+
+
+fn fetch_max_linked_lists_length(tid: vec3<u32>, active_block_vid: Grid::BlockVirtualId, bid: u32) {
+#if DIM == 2
+    let base_block_pos_int = active_block_vid.id - vec2<i32>(1i, 1i);
+#else
+    let base_block_pos_int = active_block_vid.id - vec3<i32>(1i, 1i, 1i);
+#endif
+
+    for (var i = 0u; i <= 1u; i++) {
+        for (var j = 0u; j <= 1u; j++) {
+            for (var k = 0u; k <= K_RANGE; k++) {
+#if DIM == 2
+                if (i == 0 && tid.x < 6) || (j == 0 && tid.y < 6) {
+                    // This thread is targetting a non-existent cell in shared memory.
+                    continue;
+                }
+
+                let octant = vec2(i, j);
+                let octant_hid = Grid::find_block_header_id(Grid::BlockVirtualId(base_block_pos_int + vec2<i32>(octant)));
+#else
+                if (i == 0 && tid.x < 2) || (j == 0 && tid.y < 2) || (k == 0 && tid.z < 2) {
+                    // This thread is targetting a non-existent cell in shared memory.
+                    continue;
+                }
+
+                let octant = vec3(i, j, k);
+                let octant_hid = Grid::find_block_header_id(Grid::BlockVirtualId(base_block_pos_int + vec3<i32>(octant)));
+#endif
+                if octant_hid.id != Grid::NONE {
+                    let global_chunk_id = Grid::block_header_id_to_physical_id(octant_hid);
+#if DIM == 2
+                    let global_node_id = Grid::node_id(global_chunk_id, tid.xy);
+#else
+                    let global_node_id = Grid::node_id(global_chunk_id, tid);
+#endif
+                    let len = nodes_linked_lists[global_node_id.id].len;
+                    atomicMax(&max_linked_list_length, len);
+                }
+            }
+        }
+    }
+}
+
+fn fetch_nodes(tid: vec3<u32>, active_block_vid: Grid::BlockVirtualId, bid: u32) {
+#if DIM == 2
+    let base_block_pos_int = active_block_vid.id - vec2<i32>(1i, 1i);
+#else
+    let base_block_pos_int = active_block_vid.id - vec3<i32>(1i, 1i, 1i);
+#endif
+
+    for (var i = 0u; i <= 1u; i++) {
+        for (var j = 0u; j <= 1u; j++) {
+            for (var k = 0u; k <= K_RANGE; k++) {
+#if DIM == 2
+                if (i == 0 && tid.x < 6) || (j == 0 && tid.y < 6) {
+                    // This thread is targetting a non-existent cell in shared memory.
+                    continue;
+                }
+
+                let octant = vec2(i, j);
+                let octant_hid = Grid::find_block_header_id(Grid::BlockVirtualId(base_block_pos_int + vec2<i32>(octant)));
+                let shared_index = octant * 8 + tid.xy;
+                let shared_node_index = flatten_shared_index(shared_index.x, shared_index.y);
+#else
+                if (i == 0 && tid.x < 2) || (j == 0 && tid.y < 2) || (k == 0 && tid.z < 2) {
+                    // This thread is targetting a non-existent cell in shared memory.
+                    continue;
+                }
+
+                let octant = vec3(i, j, k);
+                let octant_hid = Grid::find_block_header_id(Grid::BlockVirtualId(base_block_pos_int + vec3<i32>(octant)));
+                let shared_index = octant * 4 + tid;
+                let shared_node_index = flatten_shared_index(shared_index.x, shared_index.y, shared_index.z);
+#endif
+                let shared_node = &shared_nodes[shared_node_index];
+
+                if octant_hid.id != Grid::NONE {
+                    let global_chunk_id = Grid::block_header_id_to_physical_id(octant_hid);
+#if DIM == 2
+                    let global_node_id = Grid::node_id(global_chunk_id, tid.xy);
+#else
+                    let global_node_id = Grid::node_id(global_chunk_id, tid);
+#endif
+                    let particle_id = nodes_linked_lists[global_node_id.id].head;
+                    (*shared_node).particle_id = particle_id;
+                    (*shared_node).global_id = global_node_id.id;
+                } else {
+                    // This octant doesn’t exist. Fill shared memory with zeros/NONE.
+                    // NOTE: we don’t need to init global_id since it’s only read for the
+                    //       current chunk that is guaranteed to exist, not the 2x2x2 adjacent ones.
+                    (*shared_node).particle_id = Grid::NONE;
+                }
+            }
+        }
+    }
+}
+
+fn fetch_next_particle(tid: vec3<u32>) {
+    for (var i = 0u; i <= 1u; i++) {
+        for (var j = 0u; j <= 1u; j++) {
+            for (var k = 0u; k <= K_RANGE; k++) {
+#if DIM == 2
+                if (i == 0 && tid.x < 6) || (j == 0 && tid.y < 6) {
+                    continue;
+                }
+                let octant = vec2(i, j);
+                let shared_index = octant * 8 + tid.xy;
+                let shared_flat_index = flatten_shared_index(shared_index.x, shared_index.y);
+#else
+                if (i == 0 && tid.x < 2) || (j == 0 && tid.y < 2) || (k == 0 && tid.z < 2) {
+                    continue;
+                }
+                let octant = vec3(i, j, k);
+                let shared_index = octant * 4 + tid;
+                let shared_flat_index = flatten_shared_index(shared_index.x, shared_index.y, shared_index.z);
+#endif
+                let shared_node = &shared_nodes[shared_flat_index];
+                let curr_particle_id = (*shared_node).particle_id;
+
+                if curr_particle_id != Grid::NONE {
+                    let rigid_idx = rigid_particle_indices[curr_particle_id];
+                    shared_collider_ids[shared_flat_index] = rigid_idx.collider;
+
+#if DIM == 2
+                    shared_primitives[shared_flat_index] = Shape::Segment(
+                        collider_vertices[rigid_idx.segment.x],
+                        collider_vertices[rigid_idx.segment.y]
+                    );
+#else
+                    shared_primitives[shared_flat_index] = Shape::Triangle(
+                        collider_vertices[rigid_idx.triangle.x],
+                        collider_vertices[rigid_idx.triangle.y],
+                        collider_vertices[rigid_idx.triangle.z]
+                    );
+#endif
+
+                    let next_particle_id = particle_node_linked_lists[curr_particle_id];
+                    (*shared_node).particle_id = next_particle_id;
+                } else {
+                    // TODO: would it be worth skipping writing zeros if we already
+                    //       did it at the previous step? (if we already reached the end
+                    //       of the particle linked list)
+                    shared_collider_ids[shared_flat_index] = Grid::NONE;
+#if DIM == 2
+                    shared_primitives[shared_flat_index] = Shape::Segment(Vector(0.0), Vector(0.0));
+#else
+                    shared_primitives[shared_flat_index] = Shape::Triangle(Vector(0.0), Vector(0.0), Vector(0.0));
+#endif
+                }
+            }
+        }
+    }
+}
+
+#if DIM == 2
+fn flatten_shared_index(x: u32, y: u32) -> u32 {
+    return (x - 6) + (y - 6) * 10;
+}
+fn flatten_shared_shift(x: u32, y: u32) -> u32 {
+    return x + y * 10;
+}
+#else
+fn flatten_shared_index(x: u32, y: u32, z: u32) -> u32 {
+    return (x - 2) + (y - 2) * 6 + (z - 2) * 6 * 6;
+}
+fn flatten_shared_shift(x: u32, y: u32, z: u32) -> u32 {
+    return x + y * 6 + z * 6 * 6;
+}
+#endif
diff --git a/src/solver/params.rs b/src/solver/params.rs
index 6f724c8..7aec9c7 100644
--- a/src/solver/params.rs
+++ b/src/solver/params.rs
@@ -35,4 +35,4 @@ impl GpuSimulationParams {
 #[shader(src = "params.wgsl", shader_defs = "dim_shader_defs")]
 pub struct WgParams;
 
-wgcore::test_shader_compilation!(WgParams);
+wgcore::test_shader_compilation!(WgParams, wgcore, crate::dim_shader_defs());
diff --git a/src/solver/particle2d.rs b/src/solver/particle2d.rs
index 7a90134..6501e25 100644
--- a/src/solver/particle2d.rs
+++ b/src/solver/particle2d.rs
@@ -2,50 +2,140 @@ use crate::dim_shader_defs;
 use crate::models::{DruckerPrager, ElasticCoefficients};
 use crate::solver::ParticlePhase;
 use encase::ShaderType;
-use nalgebra::{Matrix2, Vector2};
+use nalgebra::{vector, Matrix2, Point2, Vector2};
+use rapier::geometry::{ColliderSet, Polyline, Segment};
+use std::ops::Div;
 use wgcore::tensor::GpuVector;
 use wgcore::Shader;
 use wgpu::{BufferUsages, Device};
+use wgrapier::dynamics::body::BodyCouplingEntry;
+use wgrapier::dynamics::GpuBodySet;
 
 #[derive(Copy, Clone, PartialEq, Debug, ShaderType)]
 #[repr(C)]
-pub struct ParticleMassProps {
-    def_grad: Matrix2<f32>,
-    init_volume_radius: Vector2<f32>,
-    mass: f32,
+pub struct ParticleDynamics {
+    pub velocity: Vector2<f32>,
+    pub def_grad: Matrix2<f32>,
+    pub affine: Matrix2<f32>,
+    pub cdf: Cdf,
+    pub init_volume: f32,
+    pub init_radius: f32,
+    pub mass: f32,
 }
 
-impl ParticleMassProps {
-    pub fn new(mass: f32, init_radius: f32) -> Self {
+impl ParticleDynamics {
+    pub fn with_density(radius: f32, density: f32) -> Self {
         let exponent = if cfg!(feature = "dim2") { 2 } else { 3 };
-        let init_volume = (init_radius * 2.0).powi(exponent); // NOTE: the particles are square-ish.
+        let init_volume = (radius * 2.0).powi(exponent); // NOTE: the particles are square-ish.
         Self {
+            velocity: Vector2::zeros(),
             def_grad: Matrix2::identity(),
-            init_volume_radius: Vector2::new(init_volume, init_radius),
-            mass,
+            affine: Matrix2::zeros(),
+            init_volume,
+            init_radius: radius,
+            mass: init_volume * density,
+            cdf: Cdf::default(),
         }
     }
+}
 
-    pub fn init_radius(&self) -> f32 {
-        self.init_volume_radius[1]
-    }
+#[derive(Copy, Clone, PartialEq, Debug, Default, ShaderType)]
+#[repr(C)]
+pub struct Cdf {
+    pub normal: Vector2<f32>,
+    pub rigid_vel: Vector2<f32>,
+    pub signed_distance: f32,
+    pub affinity: u32,
 }
 
 #[derive(Copy, Clone, Debug)]
 pub struct Particle {
     pub position: Vector2<f32>,
-    pub velocity: Vector2<f32>,
-    pub volume: ParticleMassProps,
+    pub dynamics: ParticleDynamics,
     pub model: ElasticCoefficients,
     pub plasticity: Option<DruckerPrager>,
     pub phase: Option<ParticlePhase>,
+    pub color: Option<[u8; 4]>,
+}
+
+pub struct GpuRigidParticles {
+    pub local_sample_points: GpuVector<Point2<f32>>,
+    pub sample_points: GpuVector<Point2<f32>>,
+    pub rigid_particle_needs_block: GpuVector<u32>,
+    pub node_linked_lists: GpuVector<u32>,
+    pub sample_ids: GpuVector<GpuSampleIds>,
+}
+
+impl GpuRigidParticles {
+    pub fn from_rapier(
+        device: &Device,
+        colliders: &ColliderSet,
+        gpu_bodies: &GpuBodySet,
+        coupling: &[BodyCouplingEntry],
+        sampling_step: f32,
+    ) -> Self {
+        let mut sampling_buffers = SamplingBuffers::default();
+
+        for (collider_id, (coupling, gpu_data)) in coupling
+            .iter()
+            .zip(gpu_bodies.shapes_data().iter())
+            .enumerate()
+        {
+            let collider = &colliders[coupling.collider];
+            if let Some(polyline) = collider.shape().as_polyline() {
+                let rngs = gpu_data.polyline_rngs();
+                let sampling_params = SamplingParams {
+                    collider_id: collider_id as u32,
+                    base_vid: rngs[0],
+                    sampling_step,
+                };
+                sample_polyline(polyline, &sampling_params, &mut sampling_buffers)
+            }
+        }
+
+        Self {
+            local_sample_points: GpuVector::encase(
+                device,
+                &sampling_buffers.samples,
+                BufferUsages::STORAGE,
+            ),
+            sample_points: GpuVector::init(
+                device,
+                &sampling_buffers.samples,
+                BufferUsages::STORAGE,
+            ),
+            node_linked_lists: GpuVector::uninit(
+                device,
+                sampling_buffers.samples.len() as u32,
+                BufferUsages::STORAGE,
+            ),
+            sample_ids: GpuVector::encase(
+                device,
+                &sampling_buffers.samples_ids,
+                BufferUsages::STORAGE,
+            ),
+            // NOTE: this is a packed bitmask so each u32 contains
+            //       the flag for 32 particles.
+            rigid_particle_needs_block: GpuVector::uninit(
+                device,
+                sampling_buffers.samples.len().div_ceil(32) as u32,
+                BufferUsages::STORAGE,
+            ),
+        }
+    }
+
+    pub fn len(&self) -> u64 {
+        self.sample_points.len()
+    }
+
+    pub fn is_empty(&self) -> bool {
+        self.len() == 0
+    }
 }
 
 pub struct GpuParticles {
     pub positions: GpuVector<Vector2<f32>>,
-    pub velocities: GpuVector<Vector2<f32>>,
-    pub volumes: GpuVector<ParticleMassProps>,
-    pub affines: GpuVector<Matrix2<f32>>,
+    pub dynamics: GpuVector<ParticleDynamics>,
     pub sorted_ids: GpuVector<u32>,
     pub node_linked_lists: GpuVector<u32>,
 }
@@ -57,15 +147,16 @@ impl GpuParticles {
 
     pub fn from_particles(device: &Device, particles: &[Particle]) -> Self {
         let positions: Vec<_> = particles.iter().map(|p| p.position).collect();
-        let velocities: Vec<_> = particles.iter().map(|p| p.velocity).collect();
-        let volumes: Vec<_> = particles.iter().map(|p| p.volume).collect();
+        let dynamics: Vec<_> = particles.iter().map(|p| p.dynamics).collect();
 
         Self {
-            positions: GpuVector::init(device, &positions, BufferUsages::STORAGE),
-            velocities: GpuVector::init(device, &velocities, BufferUsages::STORAGE),
-            volumes: GpuVector::encase(device, &volumes, BufferUsages::STORAGE),
+            positions: GpuVector::init(
+                device,
+                &positions,
+                BufferUsages::STORAGE | BufferUsages::COPY_SRC,
+            ),
+            dynamics: GpuVector::encase(device, &dynamics, BufferUsages::STORAGE),
             sorted_ids: GpuVector::uninit(device, particles.len() as u32, BufferUsages::STORAGE),
-            affines: GpuVector::uninit(device, particles.len() as u32, BufferUsages::STORAGE),
             node_linked_lists: GpuVector::uninit(
                 device,
                 particles.len() as u32,
@@ -79,4 +170,54 @@ impl GpuParticles {
 #[shader(src = "particle2d.wgsl", shader_defs = "dim_shader_defs")]
 pub struct WgParticle;
 
+#[derive(Copy, Clone, Debug, ShaderType)]
+pub struct GpuSampleIds {
+    pub segment: Vector2<u32>,
+    pub collider: u32,
+}
+
+#[derive(Copy, Clone, Debug)]
+struct SamplingParams {
+    base_vid: u32,
+    collider_id: u32,
+    sampling_step: f32,
+}
+
+#[derive(Default, Clone)]
+struct SamplingBuffers {
+    samples: Vec<Point2<f32>>,
+    samples_ids: Vec<GpuSampleIds>,
+}
+
+// TODO: move this elsewhere?
+fn sample_polyline(polyline: &Polyline, params: &SamplingParams, buffers: &mut SamplingBuffers) {
+    for seg_idx in polyline.indices() {
+        let seg = Segment::new(
+            polyline.vertices()[seg_idx[0] as usize],
+            polyline.vertices()[seg_idx[1] as usize],
+        );
+        let sample_id = GpuSampleIds {
+            segment: vector![params.base_vid + seg_idx[0], params.base_vid + seg_idx[1]],
+            collider: params.collider_id,
+        };
+        buffers.samples.push(seg.a);
+        buffers.samples_ids.push(sample_id);
+
+        if let Some(dir) = seg.direction() {
+            for i in 0.. {
+                let shift = (i as f32) * params.sampling_step;
+                if shift > seg.length() {
+                    break;
+                }
+
+                buffers.samples.push(seg.a + *dir * shift);
+                buffers.samples_ids.push(sample_id);
+            }
+
+            buffers.samples.push(seg.b);
+            buffers.samples_ids.push(sample_id);
+        }
+    }
+}
+
 wgcore::test_shader_compilation!(WgParticle);
diff --git a/src/solver/particle2d.wgsl b/src/solver/particle2d.wgsl
index b130296..2797f3b 100644
--- a/src/solver/particle2d.wgsl
+++ b/src/solver/particle2d.wgsl
@@ -4,34 +4,36 @@ struct Position {
     pt: vec2<f32>,
 }
 
-struct Velocity {
-    v: vec2<f32>,
-}
-
-struct Volume {
+struct Dynamics {
+    // NOTE: with this arrangement, we have
+    //       only a 4-bytes padding at the end
+    //       of the struct.
+    velocity: vec2<f32>,
     def_grad: mat2x2<f32>,
-    init_volume_radius: vec2<f32>, // init volume and init radius
+    affine: mat2x2<f32>,
+    cdf: Cdf,
+    init_volume: f32,
+    init_radius: f32,
     mass: f32,
 }
 
-fn deformation_gradient(volume: Volume) -> mat2x2<f32> {
-    return volume.def_grad;
-}
-
-fn set_deformation_gradient(vol: Volume, new_def_grad: mat2x2<f32>) -> Volume {
-    return Volume(new_def_grad, vol.init_volume_radius, vol.mass);
-}
-
-fn mass(volume: Volume) -> f32 {
-    return volume.mass;
+struct RigidParticleIndices {
+    segment: vec2<u32>,
+    collider: u32,
 }
 
-fn init_volume(volume: Volume) -> f32 {
-    return volume.init_volume_radius.x;
+struct Cdf {
+    // Should we pack this?
+    normal: vec2<f32>,
+    rigid_vel: vec2<f32>,
+    signed_distance: f32,
+    affinity: u32,
+//    // Index to the closest collider.
+//    closest_id: u32,
 }
 
-fn init_radius(volume: Volume) -> f32 {
-    return volume.init_volume_radius.y;
+fn default_cdf() -> Cdf {
+    return Cdf(vec2(0.0), vec2(0.0), 0.0, 0);
 }
 
 fn closest_grid_pos(part_pos: Position, cell_width: f32) -> vec2<f32> {
diff --git a/src/solver/particle3d.rs b/src/solver/particle3d.rs
index 52ab853..565a2e6 100644
--- a/src/solver/particle3d.rs
+++ b/src/solver/particle3d.rs
@@ -1,53 +1,171 @@
 use crate::dim_shader_defs;
 use crate::models::{DruckerPrager, ElasticCoefficients};
 use crate::solver::ParticlePhase;
-use nalgebra::{Matrix4, Matrix4x3, Vector3, Vector4};
+use encase::ShaderType;
+use nalgebra::{vector, Isometry3, Matrix3, Matrix4, Matrix4x3, Point3, Vector3, Vector4};
+use rapier::geometry::{Segment, Triangle};
+use rapier::prelude::{ColliderSet, TriMesh};
+use std::collections::HashSet;
 use wgcore::tensor::GpuVector;
 use wgcore::Shader;
 use wgpu::{BufferUsages, Device};
+use wgrapier::dynamics::body::BodyCouplingEntry;
+use wgrapier::dynamics::GpuBodySet;
 
-#[derive(bytemuck::Pod, bytemuck::Zeroable, Copy, Clone, PartialEq, Debug)]
+#[derive(Copy, Clone, PartialEq, Debug, ShaderType)]
 #[repr(C)]
-pub struct ParticleMassProps {
-    // First free rows contains the deformation gradient.
-    // Bottom row contains (mass, init_volume, init_radius)
-    props: Matrix4x3<f32>,
+pub struct ParticleDynamics {
+    pub velocity: Vector3<f32>,
+    pub def_grad: Matrix3<f32>,
+    pub affine: Matrix3<f32>,
+    pub cdf: Cdf,
+    pub init_volume: f32,
+    pub init_radius: f32,
+    pub mass: f32,
 }
 
-impl ParticleMassProps {
-    pub fn new(mass: f32, init_radius: f32) -> Self {
-        let init_volume = (init_radius * 2.0).powi(3); // NOTE: the particles are square-ish.
+impl ParticleDynamics {
+    pub fn with_density(radius: f32, density: f32) -> Self {
+        let exponent = if cfg!(feature = "dim2") { 2 } else { 3 };
+        let init_volume = (radius * 2.0).powi(exponent); // NOTE: the particles are square-ish.
         Self {
-            #[rustfmt::skip]
-            props: Matrix4x3::new(
-                1.0, 0.0, 0.0,
-                0.0, 1.0, 0.0,
-                0.0, 0.0, 1.0,
-                mass, init_volume, init_radius,
-            ),
+            velocity: Vector3::zeros(),
+            def_grad: Matrix3::identity(),
+            affine: Matrix3::zeros(),
+            init_volume,
+            init_radius: radius,
+            mass: init_volume * density,
+            cdf: Cdf::default(),
         }
     }
+}
 
-    pub fn init_radius(&self) -> f32 {
-        self.props.m43
-    }
+#[derive(Copy, Clone, PartialEq, Debug, Default, ShaderType)]
+#[repr(C)]
+pub struct Cdf {
+    pub normal: Vector3<f32>,
+    pub rigid_vel: Vector3<f32>,
+    pub signed_distance: f32,
+    pub affinity: u32,
 }
 
 #[derive(Copy, Clone, Debug)]
 pub struct Particle {
     pub position: Vector3<f32>,
-    pub velocity: Vector3<f32>,
-    pub volume: ParticleMassProps,
+    pub dynamics: ParticleDynamics,
     pub model: ElasticCoefficients,
     pub plasticity: Option<DruckerPrager>,
     pub phase: Option<ParticlePhase>,
+    pub color: Option<[u8; 4]>,
+}
+
+#[derive(Copy, Clone, Debug, ShaderType)]
+pub struct GpuSampleIds {
+    pub triangle: Vector3<u32>,
+    pub collider: u32,
+}
+
+#[derive(Copy, Clone, Debug)]
+struct SamplingParams {
+    base_vid: u32,
+    collider_id: u32,
+    sampling_step: f32,
+}
+
+#[derive(Default, Clone)]
+struct SamplingBuffers {
+    local_samples: Vec<Point3<f32>>,
+    samples: Vec<Point3<f32>>,
+    samples_ids: Vec<GpuSampleIds>,
+}
+
+pub struct GpuRigidParticles {
+    pub local_sample_points: GpuVector<Point3<f32>>,
+    pub sample_points: GpuVector<Point3<f32>>,
+    pub rigid_particle_needs_block: GpuVector<u32>,
+    pub node_linked_lists: GpuVector<u32>,
+    pub sample_ids: GpuVector<GpuSampleIds>,
+}
+
+impl GpuRigidParticles {
+    pub fn from_rapier(
+        device: &Device,
+        colliders: &ColliderSet,
+        gpu_bodies: &GpuBodySet,
+        coupling: &[BodyCouplingEntry],
+        sampling_step: f32,
+    ) -> Self {
+        let mut sampling_buffers = SamplingBuffers::default();
+        for (collider_id, (coupling, gpu_data)) in coupling
+            .iter()
+            .zip(gpu_bodies.shapes_data().iter())
+            .enumerate()
+        {
+            let collider = &colliders[coupling.collider];
+            if let Some(trimesh) = collider.shape().as_trimesh() {
+                let rngs = gpu_data.trimesh_rngs();
+                let sampling_params = SamplingParams {
+                    collider_id: collider_id as u32,
+                    base_vid: rngs[0],
+                    sampling_step,
+                };
+                sample_trimesh(trimesh, &sampling_params, &mut sampling_buffers)
+            } else if let Some(heightfield) = collider.shape().as_heightfield() {
+                let (vtx, idx) = heightfield.to_trimesh();
+                let trimesh = TriMesh::new(vtx, idx).unwrap();
+                let rngs = gpu_data.trimesh_rngs();
+                let sampling_params = SamplingParams {
+                    collider_id: collider_id as u32,
+                    base_vid: rngs[0],
+                    sampling_step,
+                };
+                sample_trimesh(&trimesh, &sampling_params, &mut sampling_buffers)
+            }
+        }
+
+        Self {
+            local_sample_points: GpuVector::encase(
+                device,
+                &sampling_buffers.samples,
+                BufferUsages::STORAGE,
+            ),
+            sample_points: GpuVector::encase(
+                device,
+                &sampling_buffers.samples,
+                BufferUsages::STORAGE,
+            ),
+            node_linked_lists: GpuVector::uninit(
+                device,
+                sampling_buffers.samples.len() as u32,
+                BufferUsages::STORAGE,
+            ),
+            sample_ids: GpuVector::encase(
+                device,
+                &sampling_buffers.samples_ids,
+                BufferUsages::STORAGE,
+            ),
+            // NOTE: this is a packed bitmask so each u32 contains
+            //       the flag for 32 particles.
+            rigid_particle_needs_block: GpuVector::uninit(
+                device,
+                sampling_buffers.samples.len().div_ceil(32) as u32,
+                BufferUsages::STORAGE,
+            ),
+        }
+    }
+
+    pub fn len(&self) -> u64 {
+        self.sample_points.len()
+    }
+
+    pub fn is_empty(&self) -> bool {
+        self.len() == 0
+    }
 }
 
 pub struct GpuParticles {
     pub positions: GpuVector<Vector4<f32>>,
-    pub velocities: GpuVector<Vector4<f32>>,
-    pub volumes: GpuVector<ParticleMassProps>,
-    pub affines: GpuVector<Matrix4<f32>>,
+    pub dynamics: GpuVector<ParticleDynamics>,
     pub sorted_ids: GpuVector<u32>,
     pub node_linked_lists: GpuVector<u32>,
 }
@@ -59,15 +177,16 @@ impl GpuParticles {
 
     pub fn from_particles(device: &Device, particles: &[Particle]) -> Self {
         let positions: Vec<_> = particles.iter().map(|p| p.position.push(0.0)).collect();
-        let velocities: Vec<_> = particles.iter().map(|p| p.velocity.push(0.0)).collect();
-        let volumes: Vec<_> = particles.iter().map(|p| p.volume).collect();
+        let dynamics: Vec<_> = particles.iter().map(|p| p.dynamics).collect();
 
         Self {
-            positions: GpuVector::init(device, &positions, BufferUsages::STORAGE),
-            velocities: GpuVector::init(device, &velocities, BufferUsages::STORAGE),
-            volumes: GpuVector::init(device, &volumes, BufferUsages::STORAGE),
+            positions: GpuVector::init(
+                device,
+                &positions,
+                BufferUsages::STORAGE | BufferUsages::COPY_SRC,
+            ),
+            dynamics: GpuVector::encase(device, &dynamics, BufferUsages::STORAGE),
             sorted_ids: GpuVector::uninit(device, particles.len() as u32, BufferUsages::STORAGE),
-            affines: GpuVector::uninit(device, particles.len() as u32, BufferUsages::STORAGE),
             node_linked_lists: GpuVector::uninit(
                 device,
                 particles.len() as u32,
@@ -77,6 +196,228 @@ impl GpuParticles {
     }
 }
 
+// TODO: move this elsewhere?
+fn sample_trimesh(trimesh: &TriMesh, params: &SamplingParams, buffers: &mut SamplingBuffers) {
+    let samples = sample_mesh(trimesh.vertices(), trimesh.indices(), params.sampling_step);
+
+    for sample in samples {
+        let tri_idx = trimesh.indices()[sample.triangle_id as usize];
+        let tri = Triangle::new(
+            trimesh.vertices()[tri_idx[0] as usize],
+            trimesh.vertices()[tri_idx[1] as usize],
+            trimesh.vertices()[tri_idx[2] as usize],
+        );
+        let sample_id = GpuSampleIds {
+            triangle: vector![
+                params.base_vid + tri_idx[0],
+                params.base_vid + tri_idx[1],
+                params.base_vid + tri_idx[2]
+            ],
+            collider: params.collider_id,
+        };
+        buffers.local_samples.push(sample.point);
+        buffers.samples.push(sample.point);
+        buffers.samples_ids.push(sample_id);
+    }
+
+    println!(
+        "Num rigid particles: {}, num triangles: {}",
+        buffers.samples.len(),
+        trimesh.indices().len()
+    );
+}
+
+// Epsilon used as a length threshold in various steps of the sampling. In particular, this avoids
+// degenerate geometries from generating invalid samples.
+const EPS: f32 = 1.0e-5;
+
+pub struct TriangleSample {
+    pub triangle_id: u32,
+    pub point: Point3<f32>,
+}
+
+/// Samples a triangle mesh with a set of points such that at least one point is generated
+/// inside each cell on a grid on the x-y plane with cells sized by `xy_spacing`.
+pub fn sample_mesh(
+    vertices: &[Point3<f32>],
+    indices: &[[u32; 3]],
+    xy_spacing: f32,
+) -> Vec<TriangleSample> {
+    let mut samples = vec![];
+    // TODO: switch to a matrix of boolean to avoid hashing if
+    //       this proves to be a perf bottleneck.
+    let mut visited_segs = HashSet::new();
+
+    let mut seg_needs_sampling = |mut ia: u32, mut ib: u32| {
+        if ib > ia {
+            std::mem::swap(&mut ia, &mut ib);
+        }
+
+        visited_segs.insert([ia, ib])
+    };
+
+    for (tri_id, idx) in indices.iter().enumerate() {
+        let tri = Triangle::new(
+            vertices[idx[0] as usize],
+            vertices[idx[1] as usize],
+            vertices[idx[2] as usize],
+        );
+        sample_triangle(tri, &mut samples, xy_spacing, tri_id as u32);
+
+        if seg_needs_sampling(idx[0], idx[1]) {
+            let seg = Segment::new(vertices[idx[0] as usize], vertices[idx[1] as usize]);
+            sample_edge(seg, &mut samples, xy_spacing, tri_id as u32);
+        }
+
+        if seg_needs_sampling(idx[1], idx[2]) {
+            let seg = Segment::new(vertices[idx[1] as usize], vertices[idx[2] as usize]);
+            sample_edge(seg, &mut samples, xy_spacing, tri_id as u32);
+        }
+
+        if seg_needs_sampling(idx[2], idx[0]) {
+            let seg = Segment::new(vertices[idx[2] as usize], vertices[idx[0] as usize]);
+            sample_edge(seg, &mut samples, xy_spacing, tri_id as u32);
+        }
+    }
+
+    samples
+}
+
+/// Samples a triangle edge with a set of points such that at least one point is generated
+/// inside each cell on a grid on the x-y plane with cells sized by `xy_spacing`.
+///
+/// The returned samples will not contain `edge.a`. It might contain `edge.b` (but it is unlikely)
+/// if it aligns exactly with the internal sampling spacing.
+pub fn sample_edge(
+    edge: Segment,
+    samples: &mut Vec<TriangleSample>,
+    xy_spacing: f32,
+    triangle_id: u32,
+) {
+    let ab = edge.b - edge.a;
+    let edge_length = ab.norm();
+
+    if edge_length > EPS {
+        let edge_dir = ab / edge_length;
+        let spacing = xy_spacing / 2.0f32.sqrt();
+        let nsteps = (edge_length / spacing).ceil() as usize;
+
+        // Start at one so we don’t push edge.a.
+        for i in 1..nsteps {
+            let point = edge.a + edge_dir * (spacing * i as f32);
+            samples.push(TriangleSample { point, triangle_id })
+        }
+    }
+}
+
+/// Samples a triangle with a set of points such that at least one point is generated
+/// inside each cell on a grid on the x-y plane with cells sized by `xy_spacing`.
+///
+/// Tha sampling has the following characteristics:
+/// 1. Guarantees at least one sample per cell in the "ambient" XY grid.
+/// 2. The sampling grid is oriented along the base (longest edge) and height (orthogonal to the
+///    base) of the triangle.
+/// 3. Samples are selected strictly from the domain of the triangle (up to rounding error).
+/// 4. No sample is placed on the base or any of the triangle vertices. Samples will generally not
+///    be on any of the two other edges either (but may due so some fortuitous alignment
+///    with the internal stepping length along the height of the triangle).
+///
+/// Because this does not attempt to sample the edges of the triangles, small or thin triangles
+/// might not result in any samples. Edges should be sampled separately with [`sample_edge`].
+pub fn sample_triangle(
+    triangle: Triangle,
+    samples: &mut Vec<TriangleSample>,
+    xy_spacing: f32,
+    triangle_id: u32,
+) {
+    // select the longest edge as the base
+    let distance_ab = nalgebra::distance(&triangle.b, &triangle.a);
+    let distance_bc = nalgebra::distance(&triangle.c, &triangle.b);
+    let distance_ca = nalgebra::distance(&triangle.a, &triangle.c);
+    let max = distance_ab.max(distance_bc).max(distance_ca);
+
+    let triangle = if max == distance_bc {
+        Triangle {
+            a: triangle.b,
+            b: triangle.c,
+            c: triangle.a,
+        }
+    } else if max == distance_ca {
+        Triangle {
+            a: triangle.c,
+            b: triangle.a,
+            c: triangle.b,
+        }
+    } else {
+        triangle
+    };
+
+    let ac = triangle.c - triangle.a;
+    let base = triangle.b - triangle.a;
+    let base_length = base.norm();
+    let base_dir = base / base_length;
+
+    // Adjust the spacing so it matches the required spacing on the x-y plane.
+    // For simplicity, we just divide by sqrt(2) so that the spacing in any direction is guaranteed
+    // to be smaller or equal to the inner-circle diameter of any cell from the implicit grid with
+    // spacing `xy_spacing`.
+    // We could use a more fine-grained adjustment that depends on the angle between the base-dir
+    // and the world x-y axes. But this doesn’t make a significant difference in point count or
+    // computation times. However, the sampling looks worse (less uniform in practice). So we stick
+    // to the simple sqrt(2) approach.
+    let spacing = xy_spacing / 2.0f32.sqrt();
+
+    // Calculate the step increment on the base.
+    let base_step_count = (base_length / spacing).ceil();
+    let base_step = base_dir * spacing;
+
+    // Project C on the base AB.
+    let ac_offset_length = ac.dot(&base_dir);
+    let bc_offset_length = base_length - ac_offset_length;
+
+    if ac_offset_length < EPS || bc_offset_length < EPS || base_length < EPS {
+        return;
+    }
+
+    // Compute the triangle’s height vector.
+    let height = ac - base_dir * ac_offset_length;
+    let height_length = height.norm();
+    let height_dir = height / height_length;
+    // Calculate the tangents.
+    let tan_alpha = height_length / ac_offset_length;
+    let tan_beta = height_length / bc_offset_length;
+
+    // Start at 1 so we don’t sample the perpendicular edge if it’s at a right angle
+    // with `triangle.a`.
+    for i in 1..base_step_count as u32 {
+        let base_position = triangle.a + (i as f32) * base_step;
+
+        // Compute the height at the current base_position. The point at the
+        // end of that height is either in the line (AC) or (BC), whichever is closer.
+        let height_ac = tan_alpha * nalgebra::distance(&triangle.a, &base_position);
+        let height_bc = tan_beta * nalgebra::distance(&triangle.b, &base_position);
+        let height_length = height_ac.min(height_bc);
+
+        // Calculate the step increment on the height.
+        let height_step_count = (height_length / spacing).ceil();
+        let height_step = height_dir * spacing;
+
+        // Start at 1 so we don’t sample the basis edge.
+        for j in 1..height_step_count as u32 {
+            let particle_position = base_position + (j as f32) * height_step;
+
+            if particle_position.iter().any(|e| !e.is_finite()) {
+                continue;
+            }
+
+            samples.push(TriangleSample {
+                point: particle_position,
+                triangle_id,
+            });
+        }
+    }
+}
+
 #[derive(Shader)]
 #[shader(src = "particle3d.wgsl", shader_defs = "dim_shader_defs")]
 pub struct WgParticle;
diff --git a/src/solver/particle3d.wgsl b/src/solver/particle3d.wgsl
index 7f39a62..ecf9dee 100644
--- a/src/solver/particle3d.wgsl
+++ b/src/solver/particle3d.wgsl
@@ -4,40 +4,34 @@ struct Position {
     pt: vec3<f32>,
 }
 
-struct Velocity {
-    v: vec3<f32>,
+struct Dynamics {
+    velocity: vec3<f32>,
+    def_grad: mat3x3<f32>,
+    affine: mat3x3<f32>,
+    cdf: Cdf,
+    init_volume: f32,
+    init_radius: f32,
+    mass: f32,
 }
 
-struct Volume {
-    // First three rows contains the deformation gradient.
-    // Fourth row contains (mass, init_volume, init_radius).
-    packed: mat3x4<f32>
+struct Cdf {
+    // Should we pack this?
+    normal: vec3<f32>,
+    rigid_vel: vec3<f32>,
+    signed_distance: f32,
+    affinity: u32,
+//    // Index to the closest collider.
+//    closest_id: u32,
 }
 
-fn deformation_gradient(volume: Volume) -> mat3x3<f32> {
-    return mat3x3(volume.packed.x.xyz, volume.packed.y.xyz, volume.packed.z.xyz);
+struct RigidParticleIndices {
+    triangle: vec3<u32>,
+    collider: u32,
 }
 
-fn set_deformation_gradient(vol: Volume, new_def_grad: mat3x3<f32>) -> Volume {
-    return Volume(
-        mat3x4(
-            vec4(new_def_grad.x, vol.packed.x.w),
-            vec4(new_def_grad.y, vol.packed.y.w),
-            vec4(new_def_grad.z, vol.packed.z.w),
-        )
-    );
-}
-
-fn mass(volume: Volume) -> f32 {
-    return volume.packed.x.w;
-}
-
-fn init_volume(volume: Volume) -> f32 {
-    return volume.packed.y.w;
-}
 
-fn init_radius(volume: Volume) -> f32 {
-    return volume.packed.z.w;
+fn default_cdf() -> Cdf {
+    return Cdf(vec3(0.0), vec3(0.0), 0.0, 0);
 }
 
 fn closest_grid_pos(part_pos: Position, cell_width: f32) -> vec3<f32> {
diff --git a/src/solver/particle_update.rs b/src/solver/particle_update.rs
index 4139597..b6a4801 100644
--- a/src/solver/particle_update.rs
+++ b/src/solver/particle_update.rs
@@ -6,9 +6,8 @@ use crate::models::{GpuModels, WgDruckerPrager, WgLinearElasticity, WgNeoHookean
 use crate::solver::params::{GpuSimulationParams, WgParams};
 use crate::solver::GpuParticles;
 use crate::solver::WgParticle;
-use naga_oil::compose::NagaModuleDescriptor;
 use wgcore::kernel::{KernelInvocationBuilder, KernelInvocationQueue};
-use wgcore::{utils, Shader};
+use wgcore::Shader;
 use wgparry::substitute_aliases;
 use wgpu::ComputePipeline;
 use wgrapier::dynamics::GpuBodySet;
@@ -56,9 +55,7 @@ impl WgParticleUpdate {
                 1,
                 [
                     particles.positions.buffer(),
-                    particles.velocities.buffer(),
-                    particles.volumes.buffer(),
-                    particles.affines.buffer(),
+                    particles.dynamics.buffer(),
                     models.linear_elasticity.buffer(),
                     models.drucker_prager_plasticity.buffer(),
                     models.drucker_prager_plastic_state.buffer(),
@@ -71,4 +68,4 @@ impl WgParticleUpdate {
     }
 }
 
-wgcore::test_shader_compilation!(WgParticleUpdate);
+wgcore::test_shader_compilation!(WgParticleUpdate, wgcore, crate::dim_shader_defs());
diff --git a/src/solver/particle_update.wgsl b/src/solver/particle_update.wgsl
index a366065..972045a 100644
--- a/src/solver/particle_update.wgsl
+++ b/src/solver/particle_update.wgsl
@@ -4,8 +4,8 @@
 #import wgsparkl::solver::particle as Particle;
 #import wgsparkl::grid::kernel as Kernel;
 #import wgsparkl::grid::grid as Grid;
-#import wgsparkl::models::neo_hookean_elasticity as ConstitutiveModel;
-//#import wgsparkl::models::linear_elasticity as ConstitutiveModel;
+//#import wgsparkl::models::neo_hookean_elasticity as ConstitutiveModel;
+#import wgsparkl::models::linear_elasticity as ConstitutiveModel;
 #import wgsparkl::models::drucker_prager as DruckerPrager;
 #import wgebra::svd2 as Svd2
 #import wgebra::svd3 as Svd3
@@ -20,25 +20,16 @@
 @group(1) @binding(0)
 var<storage, read_write> particles_pos: array<Particle::Position>;
 @group(1) @binding(1)
-var<storage, read_write> particles_vel: array<Particle::Velocity>;
+var<storage, read_write> particles_dyn: array<Particle::Dynamics>;
 @group(1) @binding(2)
-var<storage, read_write> particles_vol: array<Particle::Volume>;
-#if DIM == 2
-@group(1) @binding(3)
-var<storage, read_write> particles_affine: array<mat2x2<f32>>;
-#else
-@group(1) @binding(3)
-var<storage, read_write> particles_affine: array<mat3x3<f32>>;
-#endif
-@group(1) @binding(4)
 var<storage, read> constitutive_model: array<ConstitutiveModel::ElasticCoefficients>;
-@group(1) @binding(5)
+@group(1) @binding(3)
 var<storage, read> plasticity: array<DruckerPrager::Plasticity>;
-@group(1) @binding(6)
+@group(1) @binding(4)
 var<storage, read_write> plastic_state: array<DruckerPrager::PlasticState>;
-@group(1) @binding(7)
+@group(1) @binding(5)
 var<storage, read_write> phases: array<Phase>;
-@group(1) @binding(8)
+@group(1) @binding(6)
 var<uniform> params: Params::SimulationParams;
 
 @group(2) @binding(0)
@@ -63,51 +54,41 @@ fn main(
 
     let dt = params.dt;
     let cell_width = Grid::grid.cell_width;
-    let velocity_gradient = particles_affine[particle_id]; // The velocity gradient was stored in the affine buffer.
-    var new_particle_vel = particles_vel[particle_id].v;
+    let dynamics = particles_dyn[particle_id];
     let particle_pos = particles_pos[particle_id].pt;
+    var new_particle_vel = dynamics.velocity;
 
     /*
      * Advection.
      */
+    if dynamics.cdf.signed_distance < -0.05 * cell_width {
+        new_particle_vel = dynamics.cdf.rigid_vel + Grid::project_velocity((new_particle_vel - dynamics.cdf.rigid_vel), dynamics.cdf.normal);
+    }
+
+    // Clamp the max velocity a particle can get.
+    // TODO: clamp the grid velocities instead?
+    if length(new_particle_vel) > cell_width / dt {
+        new_particle_vel = new_particle_vel / length(new_particle_vel) * cell_width / dt;
+    }
+
     let new_particle_pos = particle_pos + new_particle_vel * dt;
 
     /*
-     * Collision detection.
+     * Penalty impulse.
      */
-
-     /*
-    for (var i = 0u; i < arrayLength(&collision_shapes); i++) {
-        let shape = collision_shapes[i];
-        let shape_pose = collision_shape_poses[i];
-        let proj = Cuboid::projectPointOnBoundary(shape, shape_pose, new_particle_pos);
-
-        if proj.is_inside {
-            // Apply the unilateral constraint (prevent further penetrations).
-            let dpt = proj.point - new_particle_pos;
-            let dist = length(dpt);
-
-            if dist > 0.0 {
-                // Penalty force.
-                let normal = dpt / dist;
-                new_particle_vel += normal * dist / dt / 100.0;
-
-//                 let mprops = body_mprops[i];
-//                 let vels = body_vels[i];
-//                 result_vel += Body::velocity_at_point(mprops.com, vels, point);
-            }
-        }
-    }
-    */
-
+     const PENALTY_COEFF: f32 = 1.0e3;
+     if dynamics.cdf.signed_distance < -0.05 * cell_width { // && dynamics.cdf.signed_distance > -0.3 * cell_width {
+         let corrected_dist = max(dynamics.cdf.signed_distance, -0.3 * cell_width);
+         let impulse = (dt * -corrected_dist * PENALTY_COEFF) * dynamics.cdf.normal;
+         new_particle_vel += impulse; // / curr_particle_vol.mass;
+     }
 
     /*
      * Deformation gradient update.
      */
-    let curr_particle_vol = particles_vol[particle_id];
-    let curr_deformation_gradient = Particle::deformation_gradient(curr_particle_vol);
-    var new_deformation_gradient = curr_deformation_gradient +
-       (velocity_gradient * dt) * curr_deformation_gradient;
+    // NOTE: the velocity gradient was stored in the affine buffer.
+    var new_deformation_gradient = dynamics.def_grad +
+       (dynamics.affine * dt) * dynamics.def_grad;
 
     /*
      * Constitutive model.
@@ -147,15 +128,14 @@ fn main(
      * Affine matrix for APIC transfer.
      */
     let inv_d = Kernel::inv_d(cell_width);
-    let volume0 = Particle::init_volume(curr_particle_vol);
-    let mass = Particle::mass(curr_particle_vol);
-    let affine = velocity_gradient * mass - stress * (volume0 * inv_d * dt);
+    // NOTE: the velocity gradient was stored in the affine buffer.
+    let affine = dynamics.affine * dynamics.mass - stress * (dynamics.init_volume * inv_d * dt);
 
     /*
      * Write back the new particle properties.
      */
     particles_pos[particle_id].pt = new_particle_pos;
-    particles_vel[particle_id].v = new_particle_vel;
-    particles_vol[particle_id] = Particle::set_deformation_gradient(curr_particle_vol, new_deformation_gradient);
-    particles_affine[particle_id] = affine;
-}
\ No newline at end of file
+    particles_dyn[particle_id].velocity = new_particle_vel;
+    particles_dyn[particle_id].def_grad = new_deformation_gradient;
+    particles_dyn[particle_id].affine = affine;
+}
diff --git a/src/solver/rigid_impulses.rs b/src/solver/rigid_impulses.rs
new file mode 100644
index 0000000..f239c3e
--- /dev/null
+++ b/src/solver/rigid_impulses.rs
@@ -0,0 +1,107 @@
+use crate::collision::WgCollide;
+use crate::dim_shader_defs;
+use crate::grid::grid::{GpuGrid, WgGrid};
+use crate::grid::kernel::WgKernel;
+use crate::models::{GpuModels, WgDruckerPrager, WgLinearElasticity, WgNeoHookeanElasticity};
+use crate::solver::params::{GpuSimulationParams, WgParams};
+use crate::solver::GpuParticles;
+use crate::solver::WgParticle;
+use encase::ShaderType;
+use rapier::math::{AngVector, Point, Vector};
+use wgcore::kernel::{KernelInvocationBuilder, KernelInvocationQueue};
+use wgcore::tensor::GpuVector;
+use wgcore::Shader;
+use wgebra::{WgSim2, WgSim3};
+use wgparry::substitute_aliases;
+use wgpu::{BufferUsages, ComputePipeline, Device};
+use wgrapier::dynamics::{GpuBodySet, WgBody};
+
+#[derive(Shader)]
+#[shader(
+    derive(WgBody, WgSim2, WgSim3, WgParams, WgGrid),
+    src = "rigid_impulses.wgsl",
+    src_fn = "substitute_aliases",
+    shader_defs = "dim_shader_defs"
+)]
+pub struct WgRigidImpulses {
+    pub update: ComputePipeline,
+    pub update_world_mass_properties: ComputePipeline,
+}
+
+#[derive(ShaderType, Copy, Clone, PartialEq, Debug, Default)]
+#[repr(C)]
+pub struct RigidImpulse {
+    pub com: Point<f32>, // For convenience, to reduce the number of bindings
+    pub linear: Vector<f32>,
+    pub angular: AngVector<f32>,
+}
+
+pub struct GpuImpulses {
+    pub incremental_impulses: GpuVector<RigidImpulse>,
+    pub total_impulses: GpuVector<RigidImpulse>,
+    pub total_impulses_staging: GpuVector<RigidImpulse>,
+}
+
+impl GpuImpulses {
+    pub fn new(device: &wgpu::Device) -> Self {
+        const MAX_BODY_COUNT: usize = 16; // CPIC doesnt support more.
+        let impulses = [RigidImpulse::default(); MAX_BODY_COUNT];
+        Self {
+            incremental_impulses: GpuVector::encase(device, &impulses, BufferUsages::STORAGE),
+            total_impulses: GpuVector::encase(
+                device,
+                &impulses,
+                BufferUsages::STORAGE | BufferUsages::COPY_SRC,
+            ),
+            total_impulses_staging: GpuVector::encase(
+                device,
+                &impulses,
+                BufferUsages::MAP_READ | BufferUsages::COPY_DST,
+            ),
+        }
+    }
+}
+
+impl WgRigidImpulses {
+    pub fn queue<'a>(
+        &'a self,
+        queue: &mut KernelInvocationQueue<'a>,
+        grid: &GpuGrid,
+        sim_params: &GpuSimulationParams,
+        impulses: &GpuImpulses,
+        bodies: &GpuBodySet,
+    ) {
+        KernelInvocationBuilder::new(queue, &self.update)
+            .bind0([
+                grid.meta.buffer(),
+                impulses.incremental_impulses.buffer(),
+                bodies.vels().buffer(),
+                bodies.local_mprops().buffer(),
+                bodies.mprops().buffer(),
+                bodies.poses().buffer(),
+                sim_params.params.buffer(),
+            ])
+            .queue(1);
+    }
+
+    pub fn queue_update_world_mass_properties<'a>(
+        &'a self,
+        queue: &mut KernelInvocationQueue<'a>,
+        impulses: &GpuImpulses,
+        bodies: &GpuBodySet,
+    ) {
+        KernelInvocationBuilder::new(queue, &self.update_world_mass_properties)
+            .bind_at(
+                0,
+                [
+                    (impulses.incremental_impulses.buffer(), 1),
+                    (bodies.local_mprops().buffer(), 3),
+                    (bodies.mprops().buffer(), 4),
+                    (bodies.poses().buffer(), 5),
+                ],
+            )
+            .queue(1);
+    }
+}
+
+wgcore::test_shader_compilation!(WgRigidImpulses, wgcore, crate::dim_shader_defs());
diff --git a/src/solver/rigid_impulses.wgsl b/src/solver/rigid_impulses.wgsl
new file mode 100644
index 0000000..0219ef0
--- /dev/null
+++ b/src/solver/rigid_impulses.wgsl
@@ -0,0 +1,150 @@
+#define_import_path wgsparkl::solver::impulse
+
+#import wgsparkl::solver::params as Params;
+#import wgrapier::body as Body;
+#import wgsparkl::grid::grid as Grid;
+
+#if DIM == 2
+    #import wgebra::sim2 as Pose;
+#else
+    #import wgebra::sim3 as Pose;
+#endif
+
+struct IntegerImpulse {
+    // HACK: we store the center of mass here to reduce the number
+    //       of bindings needed for p2g.
+#if DIM == 2
+    com: vec2<f32>,
+    linear: vec2<i32>,
+    angular: i32,
+#else
+    com: vec3<f32>,
+    linear: vec3<i32>,
+    angular: vec3<i32>,
+#endif
+}
+
+struct IntegerImpulseAtomic {
+    // HACK: we store the center of mass here to reduce the number
+    //       of bindings needed for p2g.
+#if DIM == 2
+    com: vec2<f32>,
+    linear_x: atomic<i32>,
+    linear_y: atomic<i32>,
+    angular: atomic<i32>,
+    padding: i32,
+#else
+    com: vec3<f32>,
+    linear_x: atomic<i32>,
+    linear_y: atomic<i32>,
+    linear_z: atomic<i32>,
+    padding_a: i32,
+    angular_x: atomic<i32>,
+    angular_y: atomic<i32>,
+    angular_z: atomic<i32>,
+    padding_b: i32,
+#endif
+}
+
+
+const FLOAT_TO_INT_FACTOR: f32 = 1e5;
+
+fn flt2int(flt: f32) -> i32 {
+    return i32(flt * FLOAT_TO_INT_FACTOR);
+}
+
+fn int2flt(i: i32) -> f32 {
+    return f32(i) / FLOAT_TO_INT_FACTOR;
+}
+
+fn int_impulse_to_float(imp: IntegerImpulse) -> Body::Impulse {
+#if DIM == 2
+    return Body::Impulse(
+        vec2(int2flt(imp.linear.x), int2flt(imp.linear.y)),
+        int2flt(imp.angular)
+    );
+#else
+    return Body::Impulse(
+        vec3(int2flt(imp.linear.x), int2flt(imp.linear.y), int2flt(imp.linear.z)),
+        vec3(int2flt(imp.angular.x), int2flt(imp.angular.y), int2flt(imp.angular.z))
+    );
+#endif
+}
+
+@group(0) @binding(1)
+var<storage, read_write> incremental_impulses: array<IntegerImpulse>;
+@group(0) @binding(2)
+var<storage, read_write> vels: array<Body::Velocity>;
+@group(0) @binding(3)
+var<storage, read_write> local_mprops: array<Body::MassProperties>;
+@group(0) @binding(4)
+var<storage, read_write> mprops: array<Body::MassProperties>;
+#if DIM == 2
+@group(0) @binding(5)
+var<storage, read_write> poses: array<Pose::Sim2>;
+#else
+@group(0) @binding(5)
+var<storage, read_write> poses: array<Pose::Sim3>;
+#endif
+@group(0) @binding(6)
+var<uniform> params: Params::SimulationParams;
+
+// NOTE: this is set to 16 exactly becaure we are currently limited to 16 bodies
+//       due to the CPIC affinity bitmask size.
+@compute @workgroup_size(16, 1, 1)
+fn update(
+    @builtin(global_invocation_id) gid: vec3<u32>,
+) {
+    let id = gid.x;
+
+    if id < arrayLength(&vels) {
+        var inc_impulse = int_impulse_to_float(incremental_impulses[id]);
+
+        // Reset the incremental impulse to zero for the next substep.
+#if DIM == 2
+        incremental_impulses[id] = IntegerImpulse(vec2(0.0), vec2(0i), 0i);
+#else
+        incremental_impulses[id] = IntegerImpulse(vec3(0.0), vec3(0i), vec3(0i));
+#endif
+
+        // Apply impulse and integrate
+        var new_vel = Body::applyImpulse(mprops[id], vels[id], inc_impulse);
+
+        // Cap the velocities to not move more than a fraction of a cell-width in a given substep.
+        let linvel_norm = length(new_vel.linear);
+        let angvel_norm = length(new_vel.angular);
+        let lin_limit = 0.1 * Grid::grid.cell_width / params.dt;
+        let ang_limit = 1.0; // TODO: what’s a good angular limit?
+
+        if (length(inc_impulse.linear) != 0.0 || length(inc_impulse.angular) != 0.0) {
+            if linvel_norm > lin_limit {
+                new_vel.linear = new_vel.linear * (lin_limit / linvel_norm);
+            }
+            if angvel_norm > ang_limit {
+                new_vel.angular = new_vel.angular * (ang_limit / angvel_norm);
+            }
+        }
+
+        var new_pose = Body::integrateVelocity(poses[id], new_vel, local_mprops[id].com, params.dt);
+
+        // Apply gravity.
+        let mass_mask = Vector(mprops[id].inv_mass != Vector(0.0));
+        new_vel.linear += params.gravity * mass_mask * params.dt;
+
+        vels[id] = new_vel;
+        poses[id] = new_pose;
+    }
+}
+
+@compute @workgroup_size(64, 1, 1)
+fn update_world_mass_properties(
+    @builtin(global_invocation_id) gid: vec3<u32>,
+) {
+    let id = gid.x;
+
+    if id < arrayLength(&mprops) {
+        let new_mprops = Body::updateMprops(poses[id], local_mprops[id]);
+        incremental_impulses[id].com = new_mprops.com;
+        mprops[id] = new_mprops;
+    }
+}
\ No newline at end of file
diff --git a/src/solver/rigid_particle_update.rs b/src/solver/rigid_particle_update.rs
new file mode 100644
index 0000000..de3d8b6
--- /dev/null
+++ b/src/solver/rigid_particle_update.rs
@@ -0,0 +1,65 @@
+use crate::collision::WgCollide;
+use crate::dim_shader_defs;
+use crate::grid::grid::{GpuGrid, WgGrid};
+use crate::grid::kernel::WgKernel;
+use crate::models::{GpuModels, WgDruckerPrager, WgLinearElasticity, WgNeoHookeanElasticity};
+use crate::solver::params::{GpuSimulationParams, WgParams};
+use crate::solver::WgParticle;
+use crate::solver::{GpuParticles, GpuRigidParticles};
+use encase::ShaderType;
+use rapier::math::{AngVector, Vector};
+use wgcore::kernel::{KernelInvocationBuilder, KernelInvocationQueue};
+use wgcore::tensor::GpuVector;
+use wgcore::Shader;
+use wgebra::{WgSim2, WgSim3};
+use wgparry::substitute_aliases;
+use wgpu::{BufferUsages, ComputePipeline, Device};
+use wgrapier::dynamics::{GpuBodySet, WgBody};
+
+#[derive(Shader)]
+#[shader(
+    derive(WgBody, WgSim2, WgSim3, WgParticle),
+    src = "rigid_particle_update.wgsl",
+    src_fn = "substitute_aliases",
+    shader_defs = "dim_shader_defs",
+    composable = false
+)]
+pub struct WgRigidParticleUpdate {
+    pub transform_sample_points: ComputePipeline,
+    pub transform_shape_points: ComputePipeline,
+}
+
+impl WgRigidParticleUpdate {
+    pub fn queue<'a>(
+        &'a self,
+        queue: &mut KernelInvocationQueue<'a>,
+        bodies: &GpuBodySet,
+        rigid_particles: &GpuRigidParticles,
+    ) {
+        KernelInvocationBuilder::new(queue, &self.transform_sample_points)
+            .bind_at(
+                0,
+                [
+                    (rigid_particles.local_sample_points.buffer(), 0),
+                    (rigid_particles.sample_points.buffer(), 1),
+                    (rigid_particles.sample_ids.buffer(), 2),
+                    (bodies.poses().buffer(), 4),
+                ],
+            )
+            .queue((rigid_particles.local_sample_points.len() as u32).div_ceil(64));
+
+        KernelInvocationBuilder::new(queue, &self.transform_shape_points)
+            .bind_at(
+                0,
+                [
+                    (bodies.shapes_local_vertex_buffers().buffer(), 0),
+                    (bodies.shapes_vertex_buffers().buffer(), 1),
+                    (bodies.shapes_vertex_collider_id().buffer(), 3),
+                    (bodies.poses().buffer(), 4),
+                ],
+            )
+            .queue((bodies.shapes_vertex_buffers().len() as u32).div_ceil(64));
+    }
+}
+
+wgcore::test_shader_compilation!(WgRigidParticleUpdate, wgcore, crate::dim_shader_defs());
diff --git a/src/solver/rigid_particle_update.wgsl b/src/solver/rigid_particle_update.wgsl
new file mode 100644
index 0000000..1160028
--- /dev/null
+++ b/src/solver/rigid_particle_update.wgsl
@@ -0,0 +1,50 @@
+#import wgrapier::body as Body;
+#import wgsparkl::solver::particle as Particle;
+
+#if DIM == 2
+    #import wgebra::sim2 as Pose;
+#else
+    #import wgebra::sim3 as Pose;
+#endif
+
+@group(0) @binding(0)
+var<storage, read> local_pts: array<Vector>;
+@group(0) @binding(1)
+var<storage, read_write> world_pts: array<Vector>;
+@group(0) @binding(2)
+var<storage, read> rigid_particle_indices: array<Particle::RigidParticleIndices>;
+@group(0) @binding(3)
+var<storage, read> vertex_collider_ids: array<u32>;
+#if DIM == 2
+@group(0) @binding(4)
+var<storage, read_write> poses: array<Pose::Sim2>;
+#else
+@group(0) @binding(4)
+var<storage, read_write> poses: array<Pose::Sim3>;
+#endif
+
+@compute @workgroup_size(64, 1, 1)
+fn transform_sample_points(
+    @builtin(global_invocation_id) gid: vec3<u32>,
+) {
+    let id = gid.x;
+
+    if id < arrayLength(&local_pts) {
+        let collider_id = rigid_particle_indices[id].collider;
+        let pose = poses[collider_id];
+        world_pts[id] = Pose::mulPt(pose, local_pts[id]);
+    }
+}
+
+@compute @workgroup_size(64, 1, 1)
+fn transform_shape_points(
+    @builtin(global_invocation_id) gid: vec3<u32>,
+) {
+    let id = gid.x;
+
+    if id < arrayLength(&local_pts) {
+        let collider_id = vertex_collider_ids[id];
+        let pose = poses[collider_id];
+        world_pts[id] = Pose::mulPt(pose, local_pts[id]);
+    }
+}
\ No newline at end of file
diff --git a/src_testbed/instancing2d.rs b/src_testbed/instancing2d.rs
index b07f0e3..75af34e 100644
--- a/src_testbed/instancing2d.rs
+++ b/src_testbed/instancing2d.rs
@@ -1,5 +1,6 @@
 //! A shader that renders a mesh multiple times in one draw call.
 
+use bevy::render::sync_world::MainEntity;
 use bevy::{
     core_pipeline::core_3d::Transparent3d,
     ecs::{
@@ -27,7 +28,6 @@ use bevy::{
 };
 use bytemuck::{Pod, Zeroable};
 use std::sync::Arc;
-use bevy::render::sync_world::MainEntity;
 
 pub const INSTANCING_SHADER_HANDLE: Handle<Shader> = Handle::weak_from_u128(3222377299100772450);
 
@@ -63,7 +63,7 @@ impl Plugin for ParticlesMaterialPlugin {
     }
 }
 
-#[derive(Clone, Copy, Pod, Zeroable)]
+#[derive(Clone, Copy, Pod, Zeroable, Default)]
 #[repr(C)]
 pub struct InstanceData {
     pub deformation: [Vec4; 3],
@@ -96,7 +96,8 @@ fn queue_custom(
         let view_key = msaa_key | MeshPipelineKey::from_hdr(view.hdr);
         let rangefinder = view.rangefinder3d();
         for (entity, main_entity) in &material_meshes {
-            let Some(mesh_instance) = render_mesh_instances.render_mesh_queue_data(*main_entity) else {
+            let Some(mesh_instance) = render_mesh_instances.render_mesh_queue_data(*main_entity)
+            else {
                 continue;
             };
             let Some(mesh) = meshes.get(mesh_instance.mesh_asset_id) else {
diff --git a/src_testbed/instancing2d.wgsl b/src_testbed/instancing2d.wgsl
index af1b68c..454ba13 100644
--- a/src_testbed/instancing2d.wgsl
+++ b/src_testbed/instancing2d.wgsl
@@ -23,12 +23,14 @@ fn vertex(vertex: Vertex) -> VertexOutput {
     let deformation = mat3x3(vertex.def_x, vertex.def_y, vertex.def_z);
     let position = deformation * vertex.position + vertex.pos;
     var out: VertexOutput;
-    // NOTE: Passing 0 as the instance_index to get_world_from_local() is a hack
-    // for this example as the instance_index builtin would map to the wrong
-    // index in the Mesh array. This index could be passed in via another
-    // uniform instead but it's unnecessary for the example.
+    let identity = mat4x4f(
+        1.0, 0.0, 0.0, 0.0,
+        0.0, 1.0, 0.0, 0.0,
+        0.0, 0.0, 1.0, 0.0,
+        0.0, 0.0, 0.0, 1.0
+    );
     out.clip_position = mesh_position_local_to_clip(
-        get_world_from_local(0u),
+        identity,
         vec4<f32>(position, 1.0)
     );
     out.color = vertex.i_color;
diff --git a/src_testbed/instancing3d.rs b/src_testbed/instancing3d.rs
index 74aec57..1e89f54 100644
--- a/src_testbed/instancing3d.rs
+++ b/src_testbed/instancing3d.rs
@@ -1,5 +1,6 @@
 //! A shader that renders a mesh multiple times in one draw call.
 
+use bevy::render::sync_world::MainEntity;
 use bevy::{
     core_pipeline::core_3d::Transparent3d,
     ecs::{
@@ -27,7 +28,6 @@ use bevy::{
 };
 use bytemuck::{Pod, Zeroable};
 use std::sync::Arc;
-use bevy::render::sync_world::MainEntity;
 
 pub const INSTANCING_SHADER_HANDLE: Handle<Shader> = Handle::weak_from_u128(3222377299100772450);
 
@@ -63,7 +63,7 @@ impl Plugin for ParticlesMaterialPlugin {
     }
 }
 
-#[derive(Clone, Copy, Pod, Zeroable)]
+#[derive(Clone, Copy, Pod, Zeroable, Default)]
 #[repr(C)]
 pub struct InstanceData {
     pub deformation: [Vec4; 3],
@@ -95,7 +95,8 @@ fn queue_custom(
         let view_key = msaa_key | MeshPipelineKey::from_hdr(view.hdr);
         let rangefinder = view.rangefinder3d();
         for (entity, main_entity) in &material_meshes {
-            let Some(mesh_instance) = render_mesh_instances.render_mesh_queue_data(*main_entity) else {
+            let Some(mesh_instance) = render_mesh_instances.render_mesh_queue_data(*main_entity)
+            else {
                 continue;
             };
             let Some(mesh) = meshes.get(mesh_instance.mesh_asset_id) else {
diff --git a/src_testbed/instancing3d.wgsl b/src_testbed/instancing3d.wgsl
index 2ef1a57..f2b73e7 100644
--- a/src_testbed/instancing3d.wgsl
+++ b/src_testbed/instancing3d.wgsl
@@ -26,12 +26,14 @@ fn vertex(vertex: Vertex) -> VertexOutput {
     let deformation = mat3x3(vertex.def_x, vertex.def_y, vertex.def_z);
     let position = deformation * vertex.position + vertex.pos;
     var out: VertexOutput;
-    // NOTE: Passing 0 as the instance_index to get_world_from_local() is a hack
-    // for this example as the instance_index builtin would map to the wrong
-    // index in the Mesh array. This index could be passed in via another
-    // uniform instead but it's unnecessary for the example.
+    let identity = mat4x4f(
+        1.0, 0.0, 0.0, 0.0,
+        0.0, 1.0, 0.0, 0.0,
+        0.0, 0.0, 1.0, 0.0,
+        0.0, 0.0, 0.0, 1.0
+    );
     out.clip_position = mesh_position_local_to_clip(
-        get_world_from_local(0u),
+        identity,
         vec4<f32>(position, 1.0)
     );
     out.color = vertex.i_color;
diff --git a/src_testbed/lib.rs b/src_testbed/lib.rs
index 6e2454f..27413f9 100644
--- a/src_testbed/lib.rs
+++ b/src_testbed/lib.rs
@@ -3,10 +3,12 @@ pub extern crate wgsparkl2d as wgsparkl;
 #[cfg(feature = "dim3")]
 pub extern crate wgsparkl3d as wgsparkl;
 
+use bevy::render::renderer::{RenderDevice, RenderQueue};
 #[cfg(feature = "dim2")]
 pub use instancing2d as instancing;
 #[cfg(feature = "dim3")]
 pub use instancing3d as instancing;
+use std::collections::HashMap;
 
 #[cfg(feature = "dim2")]
 pub mod instancing2d;
@@ -15,19 +17,30 @@ pub mod instancing3d;
 
 mod hot_reload;
 pub mod prep_vertex_buffer;
+mod rigid_graphics;
 pub mod startup;
 pub mod step;
 pub mod ui;
 
 use bevy::asset::load_internal_asset;
-use bevy::ecs::system::SystemId;
+use bevy::ecs::system::{BoxedSystem, SystemId};
+use bevy::pbr::wireframe::WireframePlugin;
 use bevy::prelude::*;
-// use bevy_editor_cam::prelude::DefaultEditorCamPlugins;
+use bevy_editor_cam::prelude::DefaultEditorCamPlugins;
 // use bevy_wasm_window_resize::WindowResizePlugin;
+use crate::rigid_graphics::{EntityWithGraphics, InstancedMaterials};
 use instancing::INSTANCING_SHADER_HANDLE;
 use prep_vertex_buffer::{GpuRenderConfig, RenderConfig, WgPrepVertexBuffer};
 use wgcore::hot_reloading::HotReloadState;
 use wgcore::timestamps::GpuTimestamps;
+use wgsparkl::rapier::dynamics::{
+    CCDSolver, ImpulseJoint, IntegrationParameters, MultibodyJoint, RigidBodySet,
+};
+use wgsparkl::rapier::geometry::{BroadPhase, ColliderSet, NarrowPhase};
+use wgsparkl::rapier::prelude::{
+    DefaultBroadPhase, ImpulseJointSet, IslandManager, MultibodyJointSet, PhysicsPipeline,
+    ShapeType,
+};
 use wgsparkl::{
     pipeline::{MpmData, MpmPipeline},
     solver::Particle,
@@ -36,23 +49,31 @@ use wgsparkl::{
 pub fn init_testbed(app: &mut App) {
     app.add_plugins(DefaultPlugins)
         // .add_plugins(WindowResizePlugin)
-        // .add_plugins((
-        //     bevy_mod_picking::DefaultPickingPlugins,
-        //     DefaultEditorCamPlugins,
-        // ))
+        .add_plugins((
+            // bevy_mod_picking::DefaultPickingPlugins,
+            DefaultEditorCamPlugins,
+        ))
         .add_plugins(instancing::ParticlesMaterialPlugin)
         .add_plugins(bevy_egui::EguiPlugin)
         .init_resource::<SceneInits>()
+        .init_resource::<Callbacks>()
         .add_systems(Startup, startup::setup_app)
         .add_systems(
             Update,
             (
                 ui::update_ui,
-                step::step_simulation,
+                (step::step_simulation, step::callbacks)
+                    .chain()
+                    .run_if(|state: Res<AppState>| state.run_state != RunState::Paused),
+                rigid_graphics::update_rigid_graphics,
                 hot_reload::handle_hot_reloading,
-            ),
+            )
+                .chain(),
         );
 
+    #[cfg(not(target_arch = "wasm32"))]
+    app.add_plugins(WireframePlugin);
+
     #[cfg(feature = "dim2")]
     load_internal_asset!(
         app,
@@ -81,18 +102,43 @@ pub struct AppState {
     pub restarting: bool,
     pub selected_scene: usize,
     pub hot_reload: HotReloadState,
+    pub show_rigid_particles: bool,
+    pub physics_time_seconds: f64,
 }
 
+pub use wgsparkl::rapier::prelude::PhysicsContext as RapierData;
+
 #[derive(Resource)]
 pub struct PhysicsContext {
     pub data: MpmData,
+    pub rapier_data: RapierData,
     pub particles: Vec<Particle>,
 }
 
+#[derive(Resource, Default)]
+pub struct Callbacks(pub Vec<Callback>);
+
+pub type Callback = Box<
+    dyn FnMut(Option<&mut RenderContext>, &mut PhysicsContext, &Timestamps, &AppState, RenderQueue)
+        + Send
+        + Sync,
+>;
+
+#[derive(Resource, Default)]
+pub struct RenderContext {
+    pub instanced_materials: InstancedMaterials,
+    pub prefab_meshes: HashMap<ShapeType, Handle<Mesh>>,
+    pub rigid_entities: Vec<EntityWithGraphics>,
+}
+
 #[derive(Resource, Default)]
 pub struct Timestamps {
     pub timestamps: Option<GpuTimestamps>,
+    pub update_rigid_particles: f64,
     pub grid_sort: f64,
+    pub grid_update_cdf: f64,
+    pub p2g_cdf: f64,
+    pub g2p_cdf: f64,
     pub p2g: f64,
     pub grid_update: f64,
     pub g2p: f64,
@@ -102,7 +148,11 @@ pub struct Timestamps {
 
 impl Timestamps {
     pub fn total_time(&self) -> f64 {
-        self.grid_sort
+        self.update_rigid_particles
+            + self.grid_sort
+            + self.grid_update_cdf
+            + self.p2g_cdf
+            + self.g2p_cdf
             + self.p2g
             + self.grid_update
             + self.g2p
@@ -120,22 +170,42 @@ pub enum RunState {
 
 #[derive(Resource)]
 pub struct SceneInits {
-    pub scenes: Vec<(String, SystemId)>,
+    pub scenes: Vec<(String, SceneInitFn)>,
     reset_graphics: SystemId,
+    reset_app_state: SystemId,
 }
+pub type SceneInitFn =
+    Box<dyn FnMut(RenderDevice, &mut AppState, &mut Callbacks) -> PhysicsContext + Send + Sync>;
 
 impl SceneInits {
     pub fn init_scene(&self, commands: &mut Commands, scene_id: usize) {
-        commands.run_system(self.scenes[scene_id].1);
+        commands.run_system(self.reset_app_state);
+        commands.run_system_cached_with(run_scene_init, scene_id);
         commands.run_system(self.reset_graphics);
     }
 }
 
+pub fn run_scene_init(
+    scene_id: In<usize>,
+    mut commands: Commands,
+    mut scenes: ResMut<SceneInits>,
+    device: Res<RenderDevice>,
+    mut app_state: ResMut<AppState>,
+    mut callbacks: ResMut<Callbacks>,
+) {
+    commands.insert_resource(scenes.scenes[scene_id.0].1(
+        device.clone(),
+        &mut app_state,
+        &mut callbacks,
+    ));
+}
+
 impl FromWorld for SceneInits {
     fn from_world(world: &mut World) -> Self {
         Self {
             scenes: vec![],
             reset_graphics: world.register_system(startup::setup_graphics),
+            reset_app_state: world.register_system(startup::setup_app_state),
         }
     }
 }
diff --git a/src_testbed/prep_vertex_buffer.rs b/src_testbed/prep_vertex_buffer.rs
index b9fc3d3..44a3509 100644
--- a/src_testbed/prep_vertex_buffer.rs
+++ b/src_testbed/prep_vertex_buffer.rs
@@ -1,19 +1,20 @@
-use naga_oil::compose::{ComposableModuleDescriptor, Composer, NagaModuleDescriptor};
-use wgcore::composer::ComposerExt;
 use wgcore::kernel::{KernelInvocationBuilder, KernelInvocationQueue};
 use wgcore::tensor::GpuScalar;
-use wgcore::{utils, Shader};
+use wgcore::Shader;
 use wgebra::WgSvd2;
 use wgebra::WgSvd3;
 use wgpu::{Buffer, BufferUsages, ComputePipeline, Device};
 use wgsparkl::grid::grid::{GpuGrid, WgGrid};
-use wgsparkl::solver::WgParticle;
 use wgsparkl::solver::{GpuParticles, GpuSimulationParams};
+use wgsparkl::solver::{GpuRigidParticles, WgParticle};
 
 pub enum RenderMode {
     Default = 0,
     Volume = 1,
     Velocity = 2,
+    CdfNormals = 3,
+    CdfDistances = 4,
+    CdfSigns = 5,
 }
 
 impl RenderMode {
@@ -22,6 +23,9 @@ impl RenderMode {
             Self::Default => "default",
             Self::Volume => "volume",
             Self::Velocity => "velocity",
+            Self::CdfNormals => "cdf (normals)",
+            Self::CdfDistances => "cdf (distances)",
+            Self::CdfSigns => "cdf (signs)",
         }
     }
 
@@ -30,6 +34,9 @@ impl RenderMode {
             0 => Self::Default,
             1 => Self::Volume,
             2 => Self::Velocity,
+            3 => Self::CdfNormals,
+            4 => Self::CdfDistances,
+            5 => Self::CdfSigns,
             _ => unreachable!(),
         }
     }
@@ -69,6 +76,7 @@ impl GpuRenderConfig {
 #[cfg_attr(feature = "dim3", shader(src = "prep_vertex_buffer3d.wgsl"))]
 pub struct WgPrepVertexBuffer {
     main: ComputePipeline,
+    main_rigid_particles: ComputePipeline,
 }
 
 impl WgPrepVertexBuffer {
@@ -77,20 +85,27 @@ impl WgPrepVertexBuffer {
         queue: &mut KernelInvocationQueue<'a>,
         config: &GpuRenderConfig,
         particles: &GpuParticles,
+        rigid_particles: &GpuRigidParticles,
         grid: &GpuGrid,
         params: &GpuSimulationParams,
         vertex_buffer: &Buffer,
+        vertex_buffer_rigid_particles: Option<&Buffer>,
     ) {
         KernelInvocationBuilder::new(queue, &self.main)
             .bind0([
                 vertex_buffer,
                 particles.positions.buffer(),
-                particles.volumes.buffer(),
-                particles.velocities.buffer(),
+                particles.dynamics.buffer(),
                 grid.meta.buffer(),
                 params.params.buffer(),
                 config.buffer.buffer(),
             ])
             .queue(particles.positions.len().div_ceil(64) as u32);
+
+        if let Some(vertex_buffer) = vertex_buffer_rigid_particles {
+            KernelInvocationBuilder::new(queue, &self.main_rigid_particles)
+                .bind0([vertex_buffer, rigid_particles.sample_points.buffer()])
+                .queue(rigid_particles.sample_points.len().div_ceil(64) as u32);
+        }
     }
 }
diff --git a/src_testbed/prep_vertex_buffer2d.wgsl b/src_testbed/prep_vertex_buffer2d.wgsl
index 437a55c..b36e035 100644
--- a/src_testbed/prep_vertex_buffer2d.wgsl
+++ b/src_testbed/prep_vertex_buffer2d.wgsl
@@ -10,14 +10,12 @@ var<storage, read_write> instances: array<InstanceData>;
 @group(0) @binding(1)
 var<storage, read> particles_pos: array<Particle::Position>;
 @group(0) @binding(2)
-var<storage, read> particles_vol: array<Particle::Volume>;
+var<storage, read> particles_dyn: array<Particle::Dynamics>;
 @group(0) @binding(3)
-var<storage, read> particles_vel: array<Particle::Velocity>;
-@group(0) @binding(4)
 var<storage, read_write> grid: Grid::Grid;
-@group(0) @binding(5)
+@group(0) @binding(4)
 var<uniform> params: Params::SimulationParams;
-@group(0) @binding(6)
+@group(0) @binding(5)
 var<storage, read> config: RenderConfig;
 
 struct RenderConfig {
@@ -27,6 +25,9 @@ struct RenderConfig {
 const DEFAULT: u32 = 0;
 const VOLUME: u32 = 1;
 const VELOCITY: u32 = 2;
+const CDF_NORMALS: u32 = 3;
+const CDF_DISTANCES: u32 = 4;
+const CDF_SIGNS: u32 = 5;
 
 struct InstanceData {
     deformation: mat3x3<f32>,
@@ -42,7 +43,7 @@ fn main(
     let particle_id = tid.x;
 
     if particle_id < arrayLength(&instances) {
-        let def_grad = Particle::deformation_gradient(particles_vol[particle_id]);
+        let def_grad = particles_dyn[particle_id].def_grad;
         instances[particle_id].deformation = mat3x3(vec3(def_grad.x, 0.0), vec3(def_grad.y, 0.0), vec3(0.0, 0.0, 1.0));
         instances[particle_id].position = vec3(particles_pos[particle_id].pt, 0.0);
 
@@ -54,12 +55,54 @@ fn main(
         if config.mode == DEFAULT {
             instances[particle_id].color = color;
         } else if config.mode == VELOCITY {
-            let vel = particles_vel[particle_id].v;
+            let vel = particles_dyn[particle_id].velocity;
             instances[particle_id].color = vec4(abs(vel) * dt * 100.0 + vec2(0.2), color.z, color.w);
         } else if config.mode == VOLUME {
             let svd = Svd2::svd(def_grad);
             let color_xy = (vec2(1.0) - svd.S) / 0.005 + vec2(0.2);
             instances[particle_id].color = vec4(color_xy, color.z, color.w);
-        }
+        } else if config.mode == CDF_NORMALS {
+            let particle_normal = particles_dyn[particle_id].cdf.normal;
+            if all(particle_normal == vec2(0.0)) {
+                instances[particle_id].color = vec4(0.0, 0.0, 0.0, color.w);
+            } else {
+                let n = (particle_normal + vec2(1.0)) / 2.0;
+                instances[particle_id].color = vec4(n.x, n.y, 0.0, color.w);
+            }
+        } else if config.mode == CDF_DISTANCES {
+            let d = particles_dyn[particle_id].cdf.signed_distance / (cell_width * 1.5);
+            if d > 0.0 {
+                instances[particle_id].color = vec4(0.0, abs(d), 0.0, color.w);
+            } else {
+                instances[particle_id].color = vec4(abs(d), 0.0, 0.0, color.w);
+            }
+        } else if config.mode == CDF_SIGNS {
+             let d = particles_dyn[particle_id].cdf.affinity;
+             let a = (d >> 16) & (d & 0x0000ffff);
+             if d == 0 {
+                 instances[particle_id].color = vec4(0.0, 0.0, 0.0, color.w);
+             } else if a == 0 {
+                 instances[particle_id].color = vec4(0.0, 1.0, 0.0, color.w);
+             } else {
+                 instances[particle_id].color = vec4(1.0, 0.0, 0.0, color.w);
+             }
+         }
+    }
+}
+
+@compute @workgroup_size(64, 1, 1)
+fn main_rigid_particles(
+    @builtin(global_invocation_id) tid: vec3<u32>,
+) {
+    let particle_id = tid.x;
+
+    if particle_id < arrayLength(&instances) {
+        instances[particle_id].deformation = mat3x3f(
+            0.4, 0.0, 0.0,
+            0.0, 0.4, 0.0,
+            0.0, 0.0, 0.4,
+        );
+        instances[particle_id].position = vec3(particles_pos[particle_id].pt, 0.0);
+        instances[particle_id].color = instances[particle_id].base_color;
     }
 }
\ No newline at end of file
diff --git a/src_testbed/prep_vertex_buffer3d.wgsl b/src_testbed/prep_vertex_buffer3d.wgsl
index b243b56..4dca049 100644
--- a/src_testbed/prep_vertex_buffer3d.wgsl
+++ b/src_testbed/prep_vertex_buffer3d.wgsl
@@ -10,14 +10,12 @@ var<storage, read_write> instances: array<InstanceData>;
 @group(0) @binding(1)
 var<storage, read> particles_pos: array<Particle::Position>;
 @group(0) @binding(2)
-var<storage, read> particles_vol: array<Particle::Volume>;
+var<storage, read> particles_dyn: array<Particle::Dynamics>;
 @group(0) @binding(3)
-var<storage, read> particles_vel: array<Particle::Velocity>;
-@group(0) @binding(4)
 var<storage, read_write> grid: Grid::Grid;
-@group(0) @binding(5)
+@group(0) @binding(4)
 var<uniform> params: Params::SimulationParams;
-@group(0) @binding(6)
+@group(0) @binding(5)
 var<storage, read> config: RenderConfig;
 
 struct RenderConfig {
@@ -27,6 +25,9 @@ struct RenderConfig {
 const DEFAULT: u32 = 0;
 const VOLUME: u32 = 1;
 const VELOCITY: u32 = 2;
+const CDF_NORMALS: u32 = 3;
+const CDF_DISTANCES: u32 = 4;
+const CDF_SIGNS: u32 = 5;
 
 
 struct InstanceData {
@@ -43,7 +44,7 @@ fn main(
     let particle_id = tid.x;
 
     if particle_id < arrayLength(&instances) {
-        let def_grad = Particle::deformation_gradient(particles_vol[particle_id]);
+        let def_grad = particles_dyn[particle_id].def_grad;
         instances[particle_id].deformation = def_grad;
         instances[particle_id].position = particles_pos[particle_id].pt;
 
@@ -55,12 +56,54 @@ fn main(
         if config.mode == DEFAULT {
             instances[particle_id].color = color;
         } else if config.mode == VELOCITY {
-            let vel = particles_vel[particle_id].v;
+            let vel = particles_dyn[particle_id].velocity;
             instances[particle_id].color = vec4(abs(vel) * dt * 100.0 + vec3(0.2), color.w);
         } else if config.mode == VOLUME {
             let svd = Svd3::svd(def_grad);
             let color_xyz = (vec3(1.0) - svd.S) / 0.005 + vec3(0.2);
             instances[particle_id].color = vec4(color_xyz, color.w);
-        }
+        } else if config.mode == CDF_NORMALS {
+            let particle_normal = particles_dyn[particle_id].cdf.normal;
+            if all(particle_normal == vec3(0.0)) {
+                instances[particle_id].color = vec4(0.0, 0.0, 0.0, color.w);
+            } else {
+                let n = (particle_normal + vec3(1.0)) / 2.0;
+                instances[particle_id].color = vec4(n.x, n.y, n.z, color.w);
+            }
+        } else if config.mode == CDF_DISTANCES {
+            let d = particles_dyn[particle_id].cdf.signed_distance / (cell_width * 1.5);
+            if d > 0.0 {
+                instances[particle_id].color = vec4(0.0, abs(d), 0.0, color.w);
+            } else {
+                instances[particle_id].color = vec4(abs(d), 0.0, 0.0, color.w);
+            }
+        } else if config.mode == CDF_SIGNS {
+             let d = particles_dyn[particle_id].cdf.affinity;
+             let a = (d >> 16) & (d & 0x0000ffff);
+             if d == 0 {
+                 instances[particle_id].color = vec4(0.0, 0.0, 0.0, color.w);
+             } else if a == 0 {
+                 instances[particle_id].color = vec4(0.0, 1.0, 0.0, color.w);
+             } else {
+                 instances[particle_id].color = vec4(1.0, 0.0, 0.0, color.w);
+             }
+         }
+    }
+}
+
+@compute @workgroup_size(64, 1, 1)
+fn main_rigid_particles(
+    @builtin(global_invocation_id) tid: vec3<u32>,
+) {
+    let particle_id = tid.x;
+
+    if particle_id < arrayLength(&instances) {
+        instances[particle_id].deformation = mat3x3f(
+            0.4, 0.0, 0.0,
+            0.0, 0.4, 0.0,
+            0.0, 0.0, 0.4
+        );
+        instances[particle_id].position = particles_pos[particle_id].pt;
+        instances[particle_id].color = instances[particle_id].base_color;
     }
 }
\ No newline at end of file
diff --git a/src_testbed/rigid_graphics.rs b/src_testbed/rigid_graphics.rs
new file mode 100644
index 0000000..baa4522
--- /dev/null
+++ b/src_testbed/rigid_graphics.rs
@@ -0,0 +1,625 @@
+#![allow(clippy::unnecessary_cast)] // Casts are needed for switching between f32/f64.
+
+use bevy::prelude::*;
+use bevy::render::mesh::{Indices, VertexAttributeValues};
+
+//use crate::objects::plane::Plane;
+use bevy::pbr::wireframe::Wireframe;
+use bevy::render::render_resource::PrimitiveTopology;
+use nalgebra::{point, Point2, Point3, Vector3};
+use std::collections::HashMap;
+use wgsparkl::rapier::geometry::{ColliderHandle, ColliderSet, Shape, ShapeType};
+#[cfg(feature = "dim3")]
+use wgsparkl::rapier::geometry::{Cone, Cylinder};
+use wgsparkl::rapier::math::{Isometry, Real, Vector};
+
+use crate::{PhysicsContext, RenderContext};
+#[cfg(feature = "dim2")]
+use {
+    nalgebra::Vector2,
+    wgsparkl::rapier::geometry::{Ball, Cuboid},
+};
+
+// #[cfg(feature = "dim2")]
+// pub type BevyMaterial = bevy_sprite::ColorMaterial;
+// #[cfg(feature = "dim3")]
+pub type BevyMaterial = StandardMaterial;
+// #[cfg(feature = "dim2")]
+// pub type BevyMaterialComponent = MeshMaterial2d<BevyMaterial>;
+// #[cfg(feature = "dim3")]
+pub type BevyMaterialComponent = MeshMaterial3d<BevyMaterial>;
+
+pub type InstancedMaterials = HashMap<Point3<usize>, Handle<BevyMaterial>>;
+pub const SELECTED_OBJECT_MATERIAL_KEY: Point3<usize> = point![42, 42, 42];
+
+#[derive(Clone, Debug)]
+pub struct EntityWithGraphics {
+    pub entity: Entity,
+    pub color: Point3<f32>,
+    pub base_color: Point3<f32>,
+    pub collider: Option<ColliderHandle>,
+    pub delta: Isometry<Real>,
+    pub opacity: f32,
+    pub material: Handle<BevyMaterial>,
+}
+
+impl EntityWithGraphics {
+    pub fn register_selected_object_material(
+        materials: &mut Assets<BevyMaterial>,
+        instanced_materials: &mut InstancedMaterials,
+    ) {
+        if instanced_materials.contains_key(&SELECTED_OBJECT_MATERIAL_KEY) {
+            return; // Already added.
+        }
+
+        // #[cfg(feature = "dim2")]
+        // let selection_material = bevy_sprite::ColorMaterial {
+        //     color: Color::from(Srgba::rgb(1.0, 0.0, 0.0)),
+        //     texture: None,
+        //     ..default()
+        // };
+        // #[cfg(feature = "dim3")]
+
+        let selection_material = StandardMaterial {
+            metallic: 0.5,
+            perceptual_roughness: 0.5,
+            double_sided: true, // TODO: this doesn't do anything?
+            ..StandardMaterial::from(Color::from(Srgba::rgb(1.0, 0.0, 0.0)))
+        };
+
+        instanced_materials.insert(
+            SELECTED_OBJECT_MATERIAL_KEY,
+            materials.add(selection_material),
+        );
+    }
+
+    pub fn spawn(
+        commands: &mut Commands,
+        meshes: &mut Assets<Mesh>,
+        materials: &mut Assets<BevyMaterial>,
+        prefab_meshes: &mut HashMap<ShapeType, Handle<Mesh>>,
+        instanced_materials: &mut InstancedMaterials,
+        shape: &dyn Shape,
+        collider: Option<ColliderHandle>,
+        collider_pos: Isometry<Real>,
+        delta: Isometry<Real>,
+        color: Point3<f32>,
+        sensor: bool,
+    ) -> Self {
+        // Make sure prefabs are generated.
+        gen_prefab_meshes(prefab_meshes, meshes);
+
+        Self::register_selected_object_material(materials, instanced_materials);
+
+        let entity = commands.spawn_empty().id();
+
+        let scale = collider_mesh_scale(shape);
+        let mesh = prefab_meshes
+            .get(&shape.shape_type())
+            .cloned()
+            .or_else(|| generate_collider_mesh(shape).map(|m| meshes.add(m)));
+
+        let opacity = 1.0;
+        let bevy_color = Color::from(Srgba::new(color.x, color.y, color.z, opacity));
+        let shape_pos = collider_pos * delta;
+        let mut transform = Transform::from_scale(scale);
+        transform.translation.x = shape_pos.translation.vector.x as f32;
+        transform.translation.y = shape_pos.translation.vector.y as f32;
+        #[cfg(feature = "dim3")]
+        {
+            transform.translation.z = shape_pos.translation.vector.z as f32;
+            transform.rotation = Quat::from_xyzw(
+                shape_pos.rotation.i as f32,
+                shape_pos.rotation.j as f32,
+                shape_pos.rotation.k as f32,
+                shape_pos.rotation.w as f32,
+            );
+        }
+        #[cfg(feature = "dim2")]
+        {
+            if sensor {
+                transform.translation.z = -10.0;
+            }
+            transform.rotation = Quat::from_rotation_z(shape_pos.rotation.angle() as f32);
+        }
+
+        // #[cfg(feature = "dim2")]
+        // let material = bevy_sprite::ColorMaterial {
+        //     color: bevy_color,
+        //     texture: None,
+        //     ..default()
+        // };
+        // #[cfg(feature = "dim3")]
+        let material = StandardMaterial {
+            metallic: 0.5,
+            perceptual_roughness: 0.5,
+            double_sided: true, // TODO: this doesn't do anything?
+            ..StandardMaterial::from(bevy_color)
+        };
+        let material_handle = instanced_materials
+            .entry(color.coords.map(|c| (c * 255.0) as usize).into())
+            .or_insert_with(|| materials.add(material));
+        let material_weak_handle = material_handle.clone_weak();
+
+        if let Some(mesh) = mesh {
+            // #[cfg(feature = "dim2")]
+            // let bundle = (
+            //     Mesh2d(mesh),
+            //     MeshMaterial2d(material_handle.clone_weak()),
+            //     transform,
+            // );
+            // #[cfg(feature = "dim3")]
+            let bundle = (
+                Mesh3d(mesh),
+                // MeshMaterial3d(materials.add(Color::srgb_u8(124, 144, 255))),
+                MeshMaterial3d(material_handle.clone_weak()),
+                Wireframe,
+                transform,
+            );
+
+            let mut entity_commands = commands.entity(entity);
+            entity_commands.insert(bundle);
+        }
+
+        EntityWithGraphics {
+            entity,
+            color,
+            base_color: color,
+            collider,
+            delta,
+            material: material_weak_handle,
+            opacity,
+        }
+    }
+
+    pub fn despawn(&mut self, commands: &mut Commands) {
+        //FIXME: Should this be despawn_recursive?
+        commands.entity(self.entity).despawn();
+    }
+
+    pub fn set_color(&mut self, materials: &mut Assets<BevyMaterial>, color: Point3<f32>) {
+        if let Some(material) = materials.get_mut(&self.material) {
+            // #[cfg(feature = "dim2")]
+            // {
+            //     material.color = Color::from(Srgba::new(color.x, color.y, color.z, self.opacity));
+            // }
+            // #[cfg(feature = "dim3")]
+            {
+                material.base_color =
+                    Color::from(Srgba::new(color.x, color.y, color.z, self.opacity));
+            }
+        }
+        self.color = color;
+        self.base_color = color;
+    }
+
+    pub fn update(
+        &self,
+        colliders: &ColliderSet,
+        components: &mut Query<&mut Transform>,
+        gfx_shift: &Vector<Real>,
+    ) {
+        if let Some(Some(co)) = self.collider.map(|c| colliders.get(c)) {
+            if let Ok(mut pos) = components.get_mut(self.entity) {
+                let co_pos = co.position() * self.delta;
+                pos.translation.x = (co_pos.translation.vector.x + gfx_shift.x) as f32;
+                pos.translation.y = (co_pos.translation.vector.y + gfx_shift.y) as f32;
+                #[cfg(feature = "dim3")]
+                {
+                    pos.translation.z = (co_pos.translation.vector.z + gfx_shift.z) as f32;
+                    pos.rotation = Quat::from_xyzw(
+                        co_pos.rotation.i as f32,
+                        co_pos.rotation.j as f32,
+                        co_pos.rotation.k as f32,
+                        co_pos.rotation.w as f32,
+                    );
+                }
+                #[cfg(feature = "dim2")]
+                {
+                    pos.rotation = Quat::from_rotation_z(co_pos.rotation.angle() as f32);
+                }
+            }
+        }
+    }
+
+    pub fn object(&self) -> Option<ColliderHandle> {
+        self.collider
+    }
+
+    #[cfg(feature = "dim2")]
+    pub fn gen_prefab_meshes(
+        out: &mut HashMap<ShapeType, Handle<Mesh>>,
+        meshes: &mut Assets<Mesh>,
+    ) {
+        //
+        // Cuboid mesh
+        //
+        let cuboid = bevy_mesh_from_polyline(Cuboid::new(Vector2::new(1.0, 1.0)).to_polyline());
+        out.insert(ShapeType::Cuboid, meshes.add(cuboid.clone()));
+        out.insert(ShapeType::RoundCuboid, meshes.add(cuboid));
+
+        //
+        // Ball mesh
+        //
+        let ball = bevy_mesh_from_polyline(Ball::new(1.0).to_polyline(30));
+        out.insert(ShapeType::Ball, meshes.add(ball));
+    }
+
+    #[cfg(feature = "dim3")]
+    pub fn gen_prefab_meshes(
+        out: &mut HashMap<ShapeType, Handle<Mesh>>,
+        meshes: &mut Assets<Mesh>,
+    ) {
+        //
+        // Cuboid mesh
+        //
+        let cuboid = Mesh::from(bevy::math::primitives::Cuboid::new(2.0, 2.0, 2.0));
+        out.insert(ShapeType::Cuboid, meshes.add(cuboid.clone()));
+        out.insert(ShapeType::RoundCuboid, meshes.add(cuboid));
+
+        //
+        // Ball mesh
+        //
+        let ball = Mesh::from(bevy::math::primitives::Sphere::new(1.0));
+        out.insert(ShapeType::Ball, meshes.add(ball));
+
+        //
+        // Cylinder mesh
+        //
+        let cylinder = Cylinder::new(1.0, 1.0);
+        let mesh = bevy_mesh(cylinder.to_trimesh(20));
+        out.insert(ShapeType::Cylinder, meshes.add(mesh.clone()));
+        out.insert(ShapeType::RoundCylinder, meshes.add(mesh));
+
+        //
+        // Cone mesh
+        //
+        let cone = Cone::new(1.0, 1.0);
+        let mesh = bevy_mesh(cone.to_trimesh(10));
+        out.insert(ShapeType::Cone, meshes.add(mesh.clone()));
+        out.insert(ShapeType::RoundCone, meshes.add(mesh));
+
+        //
+        // Halfspace
+        //
+        let vertices = vec![
+            point![-1000.0, 0.0, -1000.0],
+            point![1000.0, 0.0, -1000.0],
+            point![1000.0, 0.0, 1000.0],
+            point![-1000.0, 0.0, 1000.0],
+        ];
+        let indices = vec![[0, 1, 2], [0, 2, 3]];
+        let mesh = bevy_mesh((vertices, indices));
+        out.insert(ShapeType::HalfSpace, meshes.add(mesh));
+    }
+}
+
+#[cfg(feature = "dim2")]
+fn bevy_mesh_from_polyline(vertices: Vec<Point2<Real>>) -> Mesh {
+    let n = vertices.len();
+    let idx = (1..n as u32 - 1).map(|i| [0, i, i + 1]).collect();
+    let vtx = vertices
+        .into_iter()
+        .map(|v| Point3::new(v.x, v.y, 0.0))
+        .collect();
+    bevy_mesh3d((vtx, idx))
+}
+
+#[cfg(feature = "dim2")]
+fn bevy_polyline(buffers: (Vec<Point2<Real>>, Option<Vec<[u32; 2]>>)) -> Mesh {
+    let (vtx, idx) = buffers;
+    // let mut normals: Vec<[f32; 3]> = vec![];
+    let mut vertices: Vec<[f32; 3]> = vec![];
+
+    if let Some(idx) = idx {
+        for idx in idx {
+            let a = vtx[idx[0] as usize];
+            let b = vtx[idx[1] as usize];
+
+            vertices.push([a.x as f32, a.y as f32, 0.0]);
+            vertices.push([b.x as f32, b.y as f32, 0.0]);
+        }
+    } else {
+        vertices = vtx.iter().map(|v| [v.x as f32, v.y as f32, 0.0]).collect();
+    }
+
+    let indices: Vec<_> = (0..vertices.len() as u32).collect();
+    let uvs: Vec<_> = (0..vertices.len()).map(|_| [0.0, 0.0]).collect();
+    let normals: Vec<_> = (0..vertices.len()).map(|_| [0.0, 0.0, 1.0]).collect();
+
+    // Generate the mesh
+    let mut mesh = Mesh::new(PrimitiveTopology::LineStrip, Default::default());
+    mesh.insert_attribute(
+        Mesh::ATTRIBUTE_POSITION,
+        VertexAttributeValues::from(vertices),
+    );
+    mesh.insert_attribute(Mesh::ATTRIBUTE_NORMAL, VertexAttributeValues::from(normals));
+    mesh.insert_attribute(Mesh::ATTRIBUTE_UV_0, VertexAttributeValues::from(uvs));
+    mesh.insert_indices(Indices::U32(indices));
+    mesh
+}
+
+#[cfg(feature = "dim2")]
+fn bevy_mesh(buffers: (Vec<Point2<Real>>, Vec<[u32; 3]>)) -> Mesh {
+    bevy_mesh2d(buffers)
+}
+
+#[cfg(feature = "dim3")]
+fn bevy_mesh(buffers: (Vec<Point3<Real>>, Vec<[u32; 3]>)) -> Mesh {
+    bevy_mesh3d(buffers)
+}
+
+fn bevy_mesh2d(buffers: (Vec<Point2<Real>>, Vec<[u32; 3]>)) -> Mesh {
+    bevy_mesh3d((
+        buffers
+            .0
+            .into_iter()
+            .map(|pt| pt.coords.push(0.0).into())
+            .collect(),
+        buffers.1,
+    ))
+}
+
+fn bevy_mesh3d(buffers: (Vec<Point3<Real>>, Vec<[u32; 3]>)) -> Mesh {
+    let (vtx, idx) = buffers;
+    let mut normals: Vec<[f32; 3]> = vec![];
+    let mut vertices: Vec<[f32; 3]> = vec![];
+
+    for idx in idx {
+        let a = vtx[idx[0] as usize];
+        let b = vtx[idx[1] as usize];
+        let c = vtx[idx[2] as usize];
+
+        vertices.push(a.cast::<f32>().into());
+        vertices.push(b.cast::<f32>().into());
+        vertices.push(c.cast::<f32>().into());
+    }
+
+    for vtx in vertices.chunks(3) {
+        let a = Point3::from(vtx[0]);
+        let b = Point3::from(vtx[1]);
+        let c = Point3::from(vtx[2]);
+        let n = (b - a).cross(&(c - a)).normalize();
+        normals.push(n.cast::<f32>().into());
+        normals.push(n.cast::<f32>().into());
+        normals.push(n.cast::<f32>().into());
+    }
+
+    normals
+        .iter_mut()
+        .for_each(|n| *n = Vector3::from(*n).normalize().into());
+    let indices: Vec<_> = (0..vertices.len() as u32).collect();
+    let uvs: Vec<_> = (0..vertices.len()).map(|_| [0.0, 0.0]).collect();
+
+    // Generate the mesh
+    let mut mesh = Mesh::new(PrimitiveTopology::TriangleList, Default::default());
+    mesh.insert_attribute(
+        Mesh::ATTRIBUTE_POSITION,
+        VertexAttributeValues::from(vertices),
+    );
+    mesh.insert_attribute(Mesh::ATTRIBUTE_NORMAL, VertexAttributeValues::from(normals));
+    mesh.insert_attribute(Mesh::ATTRIBUTE_UV_0, VertexAttributeValues::from(uvs));
+    mesh.insert_indices(Indices::U32(indices));
+    mesh
+}
+
+fn collider_mesh_scale(co_shape: &dyn Shape) -> Vec3 {
+    match co_shape.shape_type() {
+        #[cfg(feature = "dim2")]
+        ShapeType::Cuboid => {
+            let c = co_shape.as_cuboid().unwrap();
+            Vec3::new(c.half_extents.x as f32, c.half_extents.y as f32, 1.0)
+        }
+        #[cfg(feature = "dim2")]
+        ShapeType::RoundCuboid => {
+            let c = &co_shape.as_round_cuboid().unwrap().inner_shape;
+            Vec3::new(c.half_extents.x as f32, c.half_extents.y as f32, 1.0)
+        }
+        ShapeType::Ball => {
+            let b = co_shape.as_ball().unwrap();
+            Vec3::new(b.radius as f32, b.radius as f32, b.radius as f32)
+        }
+        #[cfg(feature = "dim3")]
+        ShapeType::Cuboid => {
+            let c = co_shape.as_cuboid().unwrap();
+            Vec3::from_slice(c.half_extents.cast::<f32>().as_slice())
+        }
+        #[cfg(feature = "dim3")]
+        ShapeType::RoundCuboid => {
+            let c = co_shape.as_round_cuboid().unwrap();
+            Vec3::from_slice(c.inner_shape.half_extents.cast::<f32>().as_slice())
+        }
+        #[cfg(feature = "dim3")]
+        ShapeType::Cylinder => {
+            let c = co_shape.as_cylinder().unwrap();
+            Vec3::new(c.radius as f32, c.half_height as f32, c.radius as f32)
+        }
+        #[cfg(feature = "dim3")]
+        ShapeType::RoundCylinder => {
+            let c = &co_shape.as_round_cylinder().unwrap().inner_shape;
+            Vec3::new(c.radius as f32, c.half_height as f32, c.radius as f32)
+        }
+        #[cfg(feature = "dim3")]
+        ShapeType::Cone => {
+            let c = co_shape.as_cone().unwrap();
+            Vec3::new(c.radius as f32, c.half_height as f32, c.radius as f32)
+        }
+        #[cfg(feature = "dim3")]
+        ShapeType::RoundCone => {
+            let c = &co_shape.as_round_cone().unwrap().inner_shape;
+            Vec3::new(c.radius as f32, c.half_height as f32, c.radius as f32)
+        }
+        _ => Vec3::ONE,
+    }
+}
+
+#[cfg(feature = "dim2")]
+fn generate_collider_mesh(co_shape: &dyn Shape) -> Option<Mesh> {
+    let mesh = match co_shape.shape_type() {
+        ShapeType::Capsule => {
+            let capsule = co_shape.as_capsule().unwrap();
+            bevy_mesh_from_polyline(capsule.to_polyline(10))
+        }
+        ShapeType::Triangle => {
+            let tri = co_shape.as_triangle().unwrap();
+            bevy_mesh_from_polyline(vec![tri.a, tri.b, tri.c])
+        }
+        ShapeType::TriMesh => {
+            let trimesh = co_shape.as_trimesh().unwrap();
+            let vertices = trimesh
+                .vertices()
+                .iter()
+                .map(|p| point![p.x, p.y])
+                .collect();
+            bevy_mesh((vertices, trimesh.indices().to_vec()))
+        }
+        ShapeType::Polyline => {
+            let polyline = co_shape.as_polyline().unwrap();
+            bevy_polyline((
+                polyline.vertices().to_vec(),
+                Some(polyline.indices().to_vec()),
+            ))
+        }
+        ShapeType::HeightField => {
+            let heightfield = co_shape.as_heightfield().unwrap();
+            let vertices: Vec<_> = heightfield
+                .segments()
+                .flat_map(|s| vec![s.a, s.b])
+                .collect();
+            bevy_polyline((vertices, None))
+        }
+        ShapeType::ConvexPolygon => {
+            let poly = co_shape.as_convex_polygon().unwrap();
+            bevy_mesh_from_polyline(poly.points().to_vec())
+        }
+        ShapeType::RoundConvexPolygon => {
+            let poly = co_shape.as_round_convex_polygon().unwrap();
+            bevy_mesh_from_polyline(poly.inner_shape.points().to_vec())
+        }
+        _ => return None,
+    };
+
+    Some(mesh)
+}
+
+#[cfg(feature = "dim3")]
+fn generate_collider_mesh(co_shape: &dyn Shape) -> Option<Mesh> {
+    let mesh = match co_shape.shape_type() {
+        #[cfg(feature = "dim3")]
+        ShapeType::Capsule => {
+            let capsule = co_shape.as_capsule().unwrap();
+            bevy_mesh(capsule.to_trimesh(20, 10))
+        }
+        #[cfg(feature = "dim3")]
+        ShapeType::Triangle => {
+            let tri = co_shape.as_triangle().unwrap();
+            bevy_mesh((vec![tri.a, tri.b, tri.c], vec![[0, 1, 2], [0, 2, 1]]))
+        }
+        #[cfg(feature = "dim3")]
+        ShapeType::TriMesh => {
+            let trimesh = co_shape.as_trimesh().unwrap();
+            bevy_mesh((trimesh.vertices().to_vec(), trimesh.indices().to_vec()))
+        }
+        #[cfg(feature = "dim3")]
+        ShapeType::HeightField => {
+            let heightfield = co_shape.as_heightfield().unwrap();
+            bevy_mesh(heightfield.to_trimesh())
+        }
+        #[cfg(feature = "dim3")]
+        ShapeType::ConvexPolyhedron => {
+            let poly = co_shape.as_convex_polyhedron().unwrap();
+            bevy_mesh(poly.to_trimesh())
+        }
+        #[cfg(feature = "dim3")]
+        ShapeType::RoundConvexPolyhedron => {
+            let poly = co_shape.as_round_convex_polyhedron().unwrap();
+            bevy_mesh(poly.inner_shape.to_trimesh())
+        }
+        _ => return None,
+    };
+
+    Some(mesh)
+}
+
+#[cfg(feature = "dim2")]
+pub fn gen_prefab_meshes(out: &mut HashMap<ShapeType, Handle<Mesh>>, meshes: &mut Assets<Mesh>) {
+    if !out.is_empty() {
+        return;
+    }
+
+    //
+    // Cuboid mesh
+    //
+    let cuboid = bevy_mesh_from_polyline(Cuboid::new(Vector2::new(1.0, 1.0)).to_polyline());
+    out.insert(ShapeType::Cuboid, meshes.add(cuboid.clone()));
+    out.insert(ShapeType::RoundCuboid, meshes.add(cuboid));
+
+    //
+    // Ball mesh
+    //
+    let ball = bevy_mesh_from_polyline(Ball::new(1.0).to_polyline(30));
+    out.insert(ShapeType::Ball, meshes.add(ball));
+}
+
+#[cfg(feature = "dim3")]
+pub fn gen_prefab_meshes(out: &mut HashMap<ShapeType, Handle<Mesh>>, meshes: &mut Assets<Mesh>) {
+    if !out.is_empty() {
+        return;
+    }
+
+    //
+    // Cuboid mesh
+    //
+    let cuboid = Mesh::from(bevy::math::primitives::Cuboid::new(2.0, 2.0, 2.0));
+    out.insert(ShapeType::Cuboid, meshes.add(cuboid.clone()));
+    out.insert(ShapeType::RoundCuboid, meshes.add(cuboid));
+
+    //
+    // Ball mesh
+    //
+    let ball = Mesh::from(bevy::math::primitives::Sphere::new(1.0));
+    out.insert(ShapeType::Ball, meshes.add(ball));
+
+    //
+    // Cylinder mesh
+    //
+    let cylinder = Cylinder::new(1.0, 1.0);
+    let mesh = bevy_mesh(cylinder.to_trimesh(20));
+    out.insert(ShapeType::Cylinder, meshes.add(mesh.clone()));
+    out.insert(ShapeType::RoundCylinder, meshes.add(mesh));
+
+    //
+    // Cone mesh
+    //
+    let cone = Cone::new(1.0, 1.0);
+    let mesh = bevy_mesh(cone.to_trimesh(10));
+    out.insert(ShapeType::Cone, meshes.add(mesh.clone()));
+    out.insert(ShapeType::RoundCone, meshes.add(mesh));
+
+    //
+    // Halfspace
+    //
+    let vertices = vec![
+        point![-1000.0, 0.0, -1000.0],
+        point![1000.0, 0.0, -1000.0],
+        point![1000.0, 0.0, 1000.0],
+        point![-1000.0, 0.0, 1000.0],
+    ];
+    let indices = vec![[0, 1, 2], [0, 2, 3]];
+    let mesh = bevy_mesh((vertices, indices));
+    out.insert(ShapeType::HalfSpace, meshes.add(mesh));
+}
+
+pub fn update_rigid_graphics(
+    physics_context: ResMut<PhysicsContext>,
+    rigid_render: Res<RenderContext>,
+    mut transforms: Query<&mut Transform>,
+) {
+    for object in &rigid_render.rigid_entities {
+        object.update(
+            &physics_context.rapier_data.colliders,
+            &mut transforms,
+            &Default::default(),
+        );
+    }
+}
diff --git a/src_testbed/startup.rs b/src_testbed/startup.rs
index 37999ff..0a32a71 100644
--- a/src_testbed/startup.rs
+++ b/src_testbed/startup.rs
@@ -1,7 +1,8 @@
 use crate::instancing::{InstanceBuffer, InstanceData, InstanceMaterialData};
 use crate::prep_vertex_buffer::{GpuRenderConfig, RenderConfig, RenderMode, WgPrepVertexBuffer};
+use crate::rigid_graphics::{BevyMaterial, EntityWithGraphics, InstancedMaterials};
 use crate::step::TimestampChannel;
-use crate::{AppState, PhysicsContext, RunState, Timestamps};
+use crate::{AppState, Callbacks, PhysicsContext, RenderContext, RunState, Timestamps};
 use bevy::asset::Assets;
 use bevy::color::Color;
 use bevy::hierarchy::DespawnRecursiveExt;
@@ -11,7 +12,9 @@ use bevy::prelude::*;
 use bevy::render::render_resource::BufferUsages;
 use bevy::render::renderer::RenderDevice;
 use bevy::render::view::NoFrustumCulling;
-// use bevy_editor_cam::prelude::{EditorCam, EnabledMotion};
+use bevy_editor_cam::prelude::{EditorCam, EnabledMotion};
+use nalgebra::point;
+use std::collections::HashMap;
 use std::sync::Arc;
 use wgcore::hot_reloading::HotReloadState;
 use wgcore::tensor::GpuVector;
@@ -19,6 +22,7 @@ use wgcore::timestamps::GpuTimestamps;
 use wgcore::Shader;
 use wgpu::Features;
 use wgsparkl::pipeline::MpmPipeline;
+use wgsparkl::rapier::parry::math::Isometry;
 
 /// set up a simple 3D scene
 pub fn setup_app(mut commands: Commands, device: Res<RenderDevice>) {
@@ -42,7 +46,10 @@ pub fn setup_app(mut commands: Commands, device: Res<RenderDevice>) {
         restarting: false,
         selected_scene: 0,
         hot_reload,
+        show_rigid_particles: false,
+        physics_time_seconds: 0.0,
     });
+    commands.init_resource::<RenderContext>();
 
     let (snd, rcv) = async_channel::unbounded();
     commands.insert_resource(TimestampChannel { snd, rcv });
@@ -50,7 +57,7 @@ pub fn setup_app(mut commands: Commands, device: Res<RenderDevice>) {
     let features = device.features();
     let timestamps = features
         .contains(Features::TIMESTAMP_QUERY)
-        .then(|| GpuTimestamps::new(device.wgpu_device(), 256));
+        .then(|| GpuTimestamps::new(device.wgpu_device(), 512));
     commands.insert_resource(Timestamps {
         timestamps,
         ..Default::default()
@@ -74,14 +81,14 @@ pub fn setup_app(mut commands: Commands, device: Res<RenderDevice>) {
                 // }),
                 ..default()
             },
-            // EditorCam {
-            //     enabled_motion: EnabledMotion {
-            //         orbit: false,
-            //         ..Default::default()
-            //     },
-            //     last_anchor_depth: -99.0,
-            //     ..Default::default()
-            // },
+            EditorCam {
+                enabled_motion: EnabledMotion {
+                    orbit: false,
+                    ..Default::default()
+                },
+                last_anchor_depth: -99.0,
+                ..Default::default()
+            },
         ));
     }
 
@@ -92,19 +99,62 @@ pub fn setup_app(mut commands: Commands, device: Res<RenderDevice>) {
                 transform: Transform::from_translation(Vec3::new(0.0, 1.5, 5.0)),
                 ..default()
             },
-            // EditorCam::default(),
+            EditorCam::default(),
         ));
     }
 }
 
-pub fn setup_graphics(
-    mut commands: Commands,
-    device: Res<RenderDevice>,
-    physics: Res<PhysicsContext>,
-    mut meshes: ResMut<Assets<Mesh>>,
-    to_clear: Query<Entity, With<InstanceMaterialData>>,
+fn setup_rapier_graphics(
+    commands: &mut Commands,
+    device: &RenderDevice,
+    physics: &PhysicsContext,
+    rigid_render: &mut RenderContext,
+    meshes: &mut Assets<Mesh>,
+    materials: &mut Assets<BevyMaterial>,
+    to_clear: &Query<Entity, With<InstanceMaterialData>>,
+) {
+    for rigid in rigid_render.rigid_entities.drain(..) {
+        commands.entity(rigid.entity).despawn_recursive();
+    }
+
+    for (handle, collider) in physics.rapier_data.colliders.iter() {
+        let parent = &physics.rapier_data.bodies[collider.parent().unwrap()];
+        let color = if parent.is_fixed() {
+            point![0.5, 0.5, 0.5]
+        } else if parent.is_kinematic() {
+            point![0.0, 0.0, 1.0]
+        } else {
+            point![0.0, 1.0, 0.0]
+        };
+        let e = EntityWithGraphics::spawn(
+            commands,
+            meshes,
+            materials,
+            &mut rigid_render.prefab_meshes,
+            &mut rigid_render.instanced_materials,
+            collider.shape(),
+            Some(handle),
+            *collider.position(),
+            Isometry::identity(), // TODO
+            color,
+            false,
+        );
+        rigid_render.rigid_entities.push(e);
+    }
+}
+
+#[derive(Component)]
+pub struct RigidParticlesTag;
+
+fn setup_particles_graphics(
+    commands: &mut Commands,
+    device: &RenderDevice,
+    app_state: &AppState,
+    physics: &PhysicsContext,
+    meshes: &mut Assets<Mesh>,
+    to_clear: &Query<Entity, With<InstanceMaterialData>>,
 ) {
-    if let Ok(to_clear) = to_clear.get_single() {
+    for to_clear in to_clear.iter() {
         commands.entity(to_clear).despawn_recursive();
     }
 
@@ -117,14 +167,20 @@ pub fn setup_graphics(
         Color::srgb_u8(200, 37, 255),
         Color::srgb_u8(124, 230, 25),
     ];
-    let radius = physics.particles[0].volume.init_radius();
+    let radius = physics.particles[0].dynamics.init_radius;
     let cube = meshes.add(Cuboid {
         half_size: Vec3::splat(radius),
     });
 
+    /*
+     * Particles rendering.
+     */
     let mut instances = vec![];
     for (rb_id, particle) in physics.particles.iter().enumerate() {
-        let base_color = colors[rb_id % colors.len()].to_linear().to_f32_array();
+        let base_color = particle.color.map_or_else(
+            || colors[rb_id % colors.len()].to_linear().to_f32_array(),
+            |c| c.map(|c| c as f32 / 255f32),
+        );
         instances.push(InstanceData {
             deformation: [Vec4::X, Vec4::Y, Vec4::Z],
             #[cfg(feature = "dim2")]
@@ -149,7 +205,7 @@ pub fn setup_graphics(
 
     let num_instances = instances.len();
     commands.spawn((
-        Mesh3d(cube),
+        Mesh3d(cube.clone()),
         SpatialBundle::INHERITED_IDENTITY,
         InstanceMaterialData {
             data: instances,
@@ -160,4 +216,81 @@ pub fn setup_graphics(
         },
         NoFrustumCulling,
     ));
+
+    /*
+     * Rigid particles rendering.
+     */
+    if !physics.data.rigid_particles.is_empty() {
+        let mut instances = vec![];
+        for id in 0..physics.data.rigid_particles.len() as usize {
+            let base_color = colors[id % colors.len()].to_linear().to_f32_array();
+            instances.push(InstanceData {
+                base_color,
+                color: base_color,
+                ..Default::default()
+            });
+        }
+
+        let instances_buffer = GpuVector::init(
+            device,
+            &instances,
+            BufferUsages::STORAGE | BufferUsages::VERTEX,
+        );
+
+        let num_instances = instances.len();
+        let visibility = if app_state.show_rigid_particles {
+            Visibility::Inherited
+        } else {
+            Visibility::Hidden
+        };
+
+        commands.spawn((
+            Mesh3d(cube),
+            Transform::IDENTITY,
+            visibility,
+            InstanceMaterialData {
+                data: instances,
+                buffer: InstanceBuffer {
+                    buffer: Arc::new(instances_buffer.into_inner().into()),
+                    length: num_instances,
+                },
+            },
+            NoFrustumCulling,
+            RigidParticlesTag,
+        ));
+    }
+}
+
+pub fn setup_graphics(
+    mut commands: Commands,
+    app_state: Res<AppState>,
+    device: Res<RenderDevice>,
+    physics: Res<PhysicsContext>,
+    mut rigid_render: ResMut<RenderContext>,
+    mut meshes: ResMut<Assets<Mesh>>,
+    mut materials: ResMut<Assets<BevyMaterial>>,
+    to_clear: Query<Entity, With<InstanceMaterialData>>,
+) {
+    setup_particles_graphics(
+        &mut commands,
+        &device,
+        &app_state,
+        &physics,
+        &mut meshes,
+        &to_clear,
+    );
+    setup_rapier_graphics(
+        &mut commands,
+        &device,
+        &physics,
+        &mut rigid_render,
+        &mut meshes,
+        &mut materials,
+        &to_clear,
+    );
+}
+
+pub fn setup_app_state(mut app_state: ResMut<AppState>, mut callbacks: ResMut<Callbacks>) {
+    app_state.physics_time_seconds = 0.0;
+    callbacks.0.clear();
 }
diff --git a/src_testbed/step.rs b/src_testbed/step.rs
index 365d4a7..ba7ec5f 100644
--- a/src_testbed/step.rs
+++ b/src_testbed/step.rs
@@ -1,11 +1,17 @@
 use crate::instancing::InstanceMaterialData;
-use crate::{AppState, PhysicsContext, RunState, Timestamps};
+use crate::startup::RigidParticlesTag;
+use crate::{AppState, Callbacks, PhysicsContext, RenderContext, RunState, Timestamps};
 use async_channel::{Receiver, Sender};
 use bevy::prelude::*;
 use bevy::render::renderer::{RenderDevice, RenderQueue};
 use bevy::tasks::ComputeTaskPool;
 use wgcore::kernel::KernelInvocationQueue;
+use wgcore::re_exports::encase::StorageBuffer;
 use wgcore::timestamps::GpuTimestamps;
+use wgsparkl::rapier::math::Vector;
+use wgsparkl::rapier::prelude::RigidBodyPosition;
+use wgsparkl::wgparry::math::GpuSim;
+use wgsparkl::wgrapier::dynamics::GpuVelocity;
 
 #[derive(Resource)]
 pub struct TimestampChannel {
@@ -13,19 +19,59 @@ pub struct TimestampChannel {
     pub rcv: Receiver<Timestamps>,
 }
 
+pub fn callbacks(
+    render_queue: ResMut<RenderQueue>,
+    mut render: ResMut<RenderContext>,
+    mut physics: ResMut<PhysicsContext>,
+    app_state: ResMut<AppState>,
+    timings: Res<Timestamps>,
+    mut callbacks: ResMut<Callbacks>,
+) {
+    for to_call in callbacks.0.iter_mut() {
+        to_call(
+            Some(&mut render),
+            &mut physics,
+            timings.as_ref(),
+            &app_state,
+            render_queue.clone(),
+        );
+    }
+}
+
 pub fn step_simulation(
     mut timings: ResMut<Timestamps>,
     render_device: Res<RenderDevice>,
     render_queue: Res<RenderQueue>,
     mut physics: ResMut<PhysicsContext>,
     mut app_state: ResMut<AppState>,
-    particles: Query<&InstanceMaterialData>,
+    particles: Query<&InstanceMaterialData, Without<RigidParticlesTag>>,
+    rigid_particles: Query<&InstanceMaterialData, With<RigidParticlesTag>>,
     timings_channel: Res<TimestampChannel>,
 ) {
-    if app_state.run_state == RunState::Paused {
-        return;
-    }
+    // for _ in 0..10 {
+    step_simulation_legacy(
+        &mut timings,
+        &render_device,
+        &render_queue,
+        &mut physics,
+        &mut app_state,
+        &particles,
+        &rigid_particles,
+        &timings_channel,
+    )
+    // }
+}
 
+pub fn step_simulation_legacy(
+    mut timings: &mut Timestamps,
+    render_device: &RenderDevice,
+    render_queue: &RenderQueue,
+    mut physics: &mut PhysicsContext,
+    mut app_state: &mut AppState,
+    particles: &Query<&InstanceMaterialData, Without<RigidParticlesTag>>,
+    rigid_particles: &Query<&InstanceMaterialData, With<RigidParticlesTag>>,
+    timings_channel: &TimestampChannel,
+) {
     let timings = &mut *timings;
 
     while let Ok(new_timings) = timings_channel.rcv.try_recv() {
@@ -41,7 +87,52 @@ pub fn step_simulation(
     let mut queue = KernelInvocationQueue::new(device);
     let mut encoder = device.create_command_encoder(&Default::default());
 
-    // TODO: we could do the queue_step just once?
+    // Send updated bodies information to the gpu.
+    // PERF: don’t reallocate the buffers at each step.
+    let poses_data: Vec<GpuSim> = physics
+        .data
+        .coupling()
+        .iter()
+        .map(|coupling| {
+            let c = &physics.rapier_data.colliders[coupling.collider];
+            #[cfg(feature = "dim2")]
+            return (*c.position()).into();
+            #[cfg(feature = "dim3")]
+            return GpuSim::from_isometry(*c.position(), 1.0);
+        })
+        .collect();
+    // println!("poses: {:?}", poses_data);
+    compute_queue.write_buffer(
+        physics.data.bodies.poses().buffer(),
+        0,
+        bytemuck::cast_slice(&poses_data),
+    );
+
+    let divisor = 1.0; // app_state.num_substeps as f32;
+    let gravity = Vector::y() * -9.81;
+    let vels_data: Vec<_> = physics
+        .data
+        .coupling()
+        .iter()
+        .map(|coupling| {
+            let rb = &physics.rapier_data.bodies[coupling.body];
+            GpuVelocity {
+                linear: *rb.linvel()
+                    + gravity
+                        * physics.rapier_data.integration_parameters.dt
+                        * (rb.is_dynamic() as u32 as f32)
+                        / (app_state.num_substeps as f32),
+                angular: rb.angvel().clone(),
+            }
+        })
+        .collect();
+
+    let mut vels_bytes = vec![];
+    let mut buffer = StorageBuffer::new(&mut vels_bytes);
+    buffer.write(&vels_data).unwrap();
+    compute_queue.write_buffer(physics.data.bodies.vels().buffer(), 0, &vels_bytes);
+
+    //// Step the simulation.
     app_state
         .pipeline
         .queue_step(&mut physics.data, &mut queue, timings.timestamps.is_some());
@@ -49,19 +140,39 @@ pub fn step_simulation(
     for _ in 0..app_state.num_substeps {
         queue.encode(&mut encoder, timings.timestamps.as_mut());
     }
+    physics
+        .data
+        .poses_staging
+        .copy_from(&mut encoder, &physics.data.bodies.poses());
+
+    // physics
+    //     .data
+    //     .grid
+    //     .nodes_cdf_staging
+    //     .copy_from(&mut encoder, &physics.data.grid.nodes_cdf);
+    // physics
+    //     .data
+    //     .particles
+    //     .cdf_read
+    //     .copy_from_encased(&mut encoder, &physics.data.particles.cdf);
 
     timings.timestamps.as_mut().map(|t| t.resolve(&mut encoder));
 
-    // Prepare the vertex buffer.
+    // Prepare the vertex buffer for rendering the particles.
     if let Ok(instances_buffer) = particles.get_single() {
         queue.clear();
         app_state.prep_vertex_buffer.queue(
             &mut queue,
             &app_state.gpu_render_config,
             &physics.data.particles,
+            &physics.data.rigid_particles,
             &physics.data.grid,
             &physics.data.sim_params,
             &instances_buffer.buffer.buffer,
+            rigid_particles
+                .get_single()
+                .ok()
+                .map(|b| &**b.buffer.buffer),
         );
         queue.encode(&mut encoder, timings.timestamps.as_mut());
     }
@@ -69,6 +180,56 @@ pub fn step_simulation(
     // Submit.
     compute_queue.submit(Some(encoder.finish()));
 
+    // FIXME: make the readback work on wasm too.
+    //        Currently, this means there won’t be any two-ways coupling on wasm.
+    #[cfg(not(target_arch = "wasm32"))]
+    {
+        let new_poses =
+            futures::executor::block_on(physics.data.poses_staging.read(device)).unwrap();
+
+        // println!("Impulses: {:?}", new_poses[8]);
+
+        for (i, coupling) in physics.data.coupling().iter().enumerate() {
+            let rb = &mut physics.rapier_data.bodies[coupling.body];
+            if rb.is_dynamic() {
+                let vel_before = *rb.linvel();
+                let interpolator = RigidBodyPosition {
+                    position: *rb.position(),
+                    #[cfg(feature = "dim2")]
+                    next_position: new_poses[i].similarity.isometry,
+                    #[cfg(feature = "dim3")]
+                    next_position: new_poses[i].isometry,
+                };
+                let vel = interpolator.interpolate_velocity(
+                    1.0 / (physics.rapier_data.integration_parameters.dt / divisor),
+                    &rb.mass_properties().local_mprops.local_com,
+                );
+                rb.set_linvel(vel.linvel, true);
+                rb.set_angvel(vel.angvel, true);
+                // println!("dvel: {:?}", vel.linvel - vel_before);
+            }
+        }
+    }
+
+    let mut params = physics.rapier_data.integration_parameters;
+    params.dt = params.dt / divisor;
+    physics.rapier_data.physics_pipeline.step(
+        &nalgebra::zero(),
+        &params, // physics.rapier_data.params,
+        &mut physics.rapier_data.island_manager,
+        &mut physics.rapier_data.broad_phase,
+        &mut physics.rapier_data.narrow_phase,
+        &mut physics.rapier_data.bodies,
+        &mut physics.rapier_data.colliders,
+        &mut physics.rapier_data.impulse_joints,
+        &mut physics.rapier_data.multibody_joints,
+        &mut physics.rapier_data.ccd_solver,
+        None,
+        &(),
+        &(),
+    );
+    app_state.physics_time_seconds += params.dt as f64;
+
     if let Some(timestamps) = std::mem::take(&mut timings.timestamps) {
         let timings_snd = timings_channel.snd.clone();
         let timestamp_period = compute_queue.get_timestamp_period();
@@ -82,13 +243,23 @@ pub fn step_simulation(
             };
 
             for i in 0..num_substeps {
-                let times = &timestamps_ms[i * 12..];
-                new_timings.grid_sort += times[1] - times[0];
-                new_timings.p2g += times[3] - times[2];
-                new_timings.grid_update += times[5] - times[4];
-                new_timings.g2p += times[7] - times[6];
-                new_timings.particles_update += times[9] - times[8];
-                new_timings.integrate_bodies += times[11] - times[10];
+                let mut timings = [
+                    &mut new_timings.update_rigid_particles,
+                    &mut new_timings.grid_sort,
+                    &mut new_timings.grid_update_cdf,
+                    &mut new_timings.p2g_cdf,
+                    &mut new_timings.g2p_cdf,
+                    &mut new_timings.p2g,
+                    &mut new_timings.grid_update,
+                    &mut new_timings.g2p,
+                    &mut new_timings.particles_update,
+                    &mut new_timings.integrate_bodies,
+                ];
+                let times = &timestamps_ms[i * timings.len() * 2..];
+
+                for (k, timing) in timings.iter_mut().enumerate() {
+                    **timing += times[k * 2 + 1] - times[k * 2];
+                }
             }
             timings_snd.send(new_timings).await.unwrap();
         };
diff --git a/src_testbed/ui.rs b/src_testbed/ui.rs
index f4e55cd..c454015 100644
--- a/src_testbed/ui.rs
+++ b/src_testbed/ui.rs
@@ -1,4 +1,5 @@
 use crate::prep_vertex_buffer::RenderMode;
+use crate::startup::RigidParticlesTag;
 use crate::{AppState, PhysicsContext, RunState, SceneInits, Timestamps};
 use bevy::prelude::*;
 use bevy::render::renderer::RenderQueue;
@@ -7,6 +8,7 @@ use bevy_egui::{egui, EguiContexts};
 use nalgebra::vector;
 use wgsparkl::solver::SimulationParams;
 
+#[allow(clippy::too_many_arguments)]
 pub fn update_ui(
     mut commands: Commands,
     mut ui_context: EguiContexts,
@@ -15,6 +17,7 @@ pub fn update_ui(
     scenes: Res<SceneInits>,
     timings: Res<Timestamps>,
     queue: Res<RenderQueue>,
+    mut rigid_particles: Query<&mut Visibility, With<RigidParticlesTag>>,
 ) {
     egui::Window::new("Parameters").show(ui_context.ctx_mut(), |ui| {
         let mut changed = false;
@@ -37,7 +40,7 @@ pub fn update_ui(
         egui::ComboBox::from_label("render mode")
             .selected_text(RenderMode::from_u32(app_state.render_config.mode).text())
             .show_ui(ui, |ui| {
-                for i in 0..3 {
+                for i in 0..6 {
                     changed = ui
                         .selectable_value(
                             &mut app_state.render_config.mode,
@@ -68,6 +71,19 @@ pub fn update_ui(
             .changed()
             || sim_params_changed;
 
+        if ui
+            .checkbox(&mut app_state.show_rigid_particles, "show rigid_particles")
+            .changed()
+        {
+            for mut visibility in rigid_particles.iter_mut() {
+                if app_state.show_rigid_particles {
+                    *visibility = Visibility::Inherited;
+                } else {
+                    *visibility = Visibility::Hidden;
+                }
+            }
+        }
+
         #[cfg(feature = "dim2")]
         let gravity = vector![0.0, -9.81];
         #[cfg(feature = "dim3")]
@@ -89,11 +105,22 @@ pub fn update_ui(
         }
 
         ui.label(format!("Particle count: {}", physics.particles.len()));
+        ui.label(format!(
+            "Rigid particle count: {}",
+            physics.data.rigid_particles.len()
+        ));
 
         CollapsingHeader::new(format!("GPU runtime: {:.3}ms", timings.total_time()))
-            .id_source("GPU runtimes")
+            .id_salt("GPU runtimes")
             .show(ui, |ui| {
+                ui.label(format!(
+                    "Rigid update: {:.3}ms",
+                    timings.update_rigid_particles
+                ));
                 ui.label(format!("Grid sort: {:.3}ms", timings.grid_sort));
+                ui.label(format!("CDF Grid update: {:.3}ms", timings.grid_update_cdf));
+                ui.label(format!("CDF P2G: {:.3}ms", timings.p2g_cdf));
+                ui.label(format!("CDF G2P: {:.3}ms", timings.g2p_cdf));
                 ui.label(format!("P2G: {:.3}ms", timings.p2g));
                 ui.label(format!("Grid update: {:.3}ms", timings.grid_update));
                 ui.label(format!("G2P: {:.3}ms", timings.g2p));