use euclid::default::{Point2D, Vector2D};
use rapier2d::prelude::*;
use crate::{Force, ForceContext};
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct BarnesHutConfig {
pub theta: f32,
pub min_cell_size: f32,
}
impl Default for BarnesHutConfig {
fn default() -> Self {
Self {
theta: 0.5,
min_cell_size: 1.0,
}
}
}
pub fn repulsion_forces(
positions: &[Point2D<f32>],
config: BarnesHutConfig,
k: f32,
c_repulse: f32,
epsilon: f32,
) -> Vec<Vector2D<f32>> {
if positions.is_empty() {
return Vec::new();
}
let bounds = compute_bounds(positions, config.min_cell_size);
let tree = Quadtree::build(positions, bounds, config.min_cell_size);
positions
.iter()
.map(|p| tree.compute_repulsion(*p, k, epsilon, c_repulse, config.theta))
.collect()
}
#[derive(Clone, Copy, Debug)]
pub struct BarnesHutRepulsion {
pub strength: f32,
pub min_distance: f32,
pub config: BarnesHutConfig,
}
impl Default for BarnesHutRepulsion {
fn default() -> Self {
Self {
strength: 2_400.0,
min_distance: 8.0,
config: BarnesHutConfig::default(),
}
}
}
impl Force for BarnesHutRepulsion {
fn apply(&self, ctx: &mut ForceContext<'_>, _dt: f32) {
let mut handles: Vec<RigidBodyHandle> = Vec::with_capacity(ctx.bodies_by_node.len());
let mut positions: Vec<Point2D<f32>> = Vec::with_capacity(ctx.bodies_by_node.len());
for &handle in ctx.bodies_by_node.values() {
if let Some(body) = ctx.bodies.get(handle) {
let t = body.translation();
handles.push(handle);
positions.push(Point2D::new(t.x, t.y));
}
}
let forces = repulsion_forces(
&positions,
self.config,
1.0,
self.strength,
self.min_distance,
);
for (handle, f) in handles.iter().zip(forces) {
if let Some(body) = ctx.bodies.get_mut(*handle) {
body.add_force(Vector::new(f.x, f.y), true);
}
}
}
}
#[derive(Debug, Clone, Copy)]
struct Bounds {
min: Point2D<f32>,
max: Point2D<f32>,
}
impl Bounds {
fn size(&self) -> f32 {
(self.max.x - self.min.x).max(self.max.y - self.min.y)
}
fn center(&self) -> Point2D<f32> {
Point2D::new(
(self.min.x + self.max.x) * 0.5,
(self.min.y + self.max.y) * 0.5,
)
}
fn quadrant_for(&self, p: Point2D<f32>) -> usize {
let c = self.center();
let right = (p.x >= c.x) as usize;
let bottom = (p.y >= c.y) as usize;
(bottom << 1) | right
}
fn child_bounds(&self, quadrant: usize) -> Bounds {
let c = self.center();
match quadrant {
0 => Bounds {
min: self.min,
max: c,
},
1 => Bounds {
min: Point2D::new(c.x, self.min.y),
max: Point2D::new(self.max.x, c.y),
},
2 => Bounds {
min: Point2D::new(self.min.x, c.y),
max: Point2D::new(c.x, self.max.y),
},
_ => Bounds {
min: c,
max: self.max,
},
}
}
}
fn compute_bounds(positions: &[Point2D<f32>], min_cell_size: f32) -> Bounds {
if positions.is_empty() {
return Bounds {
min: Point2D::new(-1.0, -1.0),
max: Point2D::new(1.0, 1.0),
};
}
let mut min = positions[0];
let mut max = positions[0];
for p in positions.iter().skip(1) {
min.x = min.x.min(p.x);
min.y = min.y.min(p.y);
max.x = max.x.max(p.x);
max.y = max.y.max(p.y);
}
if (max.x - min.x).abs() < min_cell_size {
max.x = min.x + min_cell_size;
}
if (max.y - min.y).abs() < min_cell_size {
max.y = min.y + min_cell_size;
}
let size = (max.x - min.x).max(max.y - min.y);
Bounds {
min,
max: Point2D::new(min.x + size, min.y + size),
}
}
#[derive(Debug)]
enum Quadtree {
Empty,
Leaf {
position: Point2D<f32>,
mass: f32,
},
Internal {
bounds: Bounds,
center_of_mass: Point2D<f32>,
total_mass: f32,
children: [Box<Quadtree>; 4],
},
}
impl Quadtree {
fn build(positions: &[Point2D<f32>], bounds: Bounds, min_cell_size: f32) -> Self {
let mut root = Quadtree::Empty;
for p in positions {
root.insert(*p, 1.0, bounds, min_cell_size);
}
root
}
fn insert(&mut self, position: Point2D<f32>, mass: f32, bounds: Bounds, min_cell_size: f32) {
match self {
Quadtree::Empty => {
*self = Quadtree::Leaf { position, mass };
}
Quadtree::Leaf {
position: existing_pos,
mass: existing_mass,
} => {
if bounds.size() <= min_cell_size {
let total = *existing_mass + mass;
let mid = Point2D::new(
(existing_pos.x * *existing_mass + position.x * mass) / total,
(existing_pos.y * *existing_mass + position.y * mass) / total,
);
*existing_pos = mid;
*existing_mass = total;
return;
}
let existing_position = *existing_pos;
let existing_m = *existing_mass;
let mut children: [Box<Quadtree>; 4] = [
Box::new(Quadtree::Empty),
Box::new(Quadtree::Empty),
Box::new(Quadtree::Empty),
Box::new(Quadtree::Empty),
];
let q_existing = bounds.quadrant_for(existing_position);
children[q_existing].insert(
existing_position,
existing_m,
bounds.child_bounds(q_existing),
min_cell_size,
);
let q_new = bounds.quadrant_for(position);
children[q_new].insert(position, mass, bounds.child_bounds(q_new), min_cell_size);
let total_mass = existing_m + mass;
let center_of_mass = Point2D::new(
(existing_position.x * existing_m + position.x * mass) / total_mass,
(existing_position.y * existing_m + position.y * mass) / total_mass,
);
*self = Quadtree::Internal {
bounds,
center_of_mass,
total_mass,
children,
};
}
Quadtree::Internal {
bounds: b,
center_of_mass,
total_mass,
children,
} => {
let q = b.quadrant_for(position);
children[q].insert(position, mass, b.child_bounds(q), min_cell_size);
let new_total = *total_mass + mass;
*center_of_mass = Point2D::new(
(center_of_mass.x * *total_mass + position.x * mass) / new_total,
(center_of_mass.y * *total_mass + position.y * mass) / new_total,
);
*total_mass = new_total;
}
}
}
fn compute_repulsion(
&self,
target: Point2D<f32>,
k: f32,
epsilon: f32,
c_repulse: f32,
theta: f32,
) -> Vector2D<f32> {
match self {
Quadtree::Empty => Vector2D::zero(),
Quadtree::Leaf { position, mass } => {
let delta = target - *position;
let distance_sq = delta.square_length();
if distance_sq < epsilon * epsilon {
return Vector2D::zero();
}
let distance = distance_sq.sqrt();
let force = c_repulse * (k * k) * *mass / distance;
(delta / distance) * force
}
Quadtree::Internal {
bounds,
center_of_mass,
total_mass,
children,
} => {
let delta = target - *center_of_mass;
let distance_sq = delta.square_length().max(epsilon * epsilon);
let distance = distance_sq.sqrt();
let s = bounds.size();
if s / distance < theta {
let force = c_repulse * (k * k) * *total_mass / distance;
return (delta / distance) * force;
}
let mut acc = Vector2D::zero();
for child in children.iter() {
acc += child.compute_repulsion(target, k, epsilon, c_repulse, theta);
}
acc
}
}
}
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn repulsion_pushes_nearby_bodies_apart() {
let positions = [Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0)];
let forces = repulsion_forces(&positions, BarnesHutConfig::default(), 30.0, 1.0, 0.01);
assert_eq!(forces.len(), 2);
assert!(
forces[0].x < 0.0,
"left body should be pushed left: {forces:?}"
);
assert!(
forces[1].x > 0.0,
"right body should be pushed right: {forces:?}"
);
}
#[test]
fn coincident_bodies_do_not_infinite_recurse() {
let positions = [
Point2D::new(50.0, 50.0),
Point2D::new(50.0, 50.0),
Point2D::new(50.0, 50.0),
];
let forces = repulsion_forces(&positions, BarnesHutConfig::default(), 30.0, 1.0, 0.01);
assert_eq!(forces.len(), 3);
}
#[test]
fn empty_input_yields_no_forces() {
let forces = repulsion_forces(&[], BarnesHutConfig::default(), 30.0, 1.0, 0.01);
assert!(forces.is_empty());
}
#[test]
fn far_apart_bodies_repel_less_than_near_ones() {
let near = [Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0)];
let far = [Point2D::new(0.0, 0.0), Point2D::new(1000.0, 0.0)];
let cfg = BarnesHutConfig::default();
let near_f = repulsion_forces(&near, cfg, 30.0, 1.0, 0.01);
let far_f = repulsion_forces(&far, cfg, 30.0, 1.0, 0.01);
assert!(near_f[0].length() > far_f[0].length());
}
#[test]
fn repulsion_force_separates_bodies_over_a_tick() {
use crate::{NodeKey, Simulation};
let a = NodeKey::new(0);
let b = NodeKey::new(1);
let mut sim = Simulation::new();
sim.sync_nodes([(a, Point2D::new(0.0, 0.0)), (b, Point2D::new(20.0, 0.0))]);
sim.add_force(BarnesHutRepulsion::default());
let dist0 = (sim.position_of(a).unwrap() - sim.position_of(b).unwrap()).length();
for _ in 0..60 {
sim.tick(1.0 / 60.0);
}
let dist1 = (sim.position_of(a).unwrap() - sim.position_of(b).unwrap()).length();
assert!(
dist1 > dist0,
"barnes-hut repulsion should push bodies apart: {dist0} -> {dist1}"
);
}
}