use alloc::vec::Vec;
use he3d_math::{Quat, Vec3};
use crate::aabb::Aabb;
use crate::body::{Body, BodyDesc, BodyId, MotionType, NO_SLEEP_GROUP};
use crate::broadphase::{DEFAULT_CELL_SIZE, SpatialGrid};
use crate::contact::{
ConstraintPoint, Contact, ContactConstraint, ContactPartner, ContactPhase, MAX_MANIFOLD_POINTS,
tangent_basis,
};
use crate::narrowphase;
use crate::shape::{MassProperties, Shape};
use crate::solver::{self, SolverSettings};
use crate::static_geometry::{StaticMesh, StaticMeshId, StaticTopology};
use crate::storage::{STATIC_PARTNER, Slot};
pub const SLEEP_LINEAR_THRESHOLD: f32 = 0.05;
pub const SLEEP_ANGULAR_THRESHOLD: f32 = 0.05;
pub const SLEEP_TIME: f32 = 0.5;
pub const DEFAULT_GRAVITY: Vec3 = Vec3::new(0.0, -9.81, 0.0);
#[derive(Clone, Debug)]
struct StaticSlot {
generation: u32,
mesh: Option<StaticMesh>,
}
#[derive(Clone, Debug)]
pub struct World {
slots: Vec<Slot>,
free: Vec<u32>,
gravity: Vec3,
settings: SolverSettings,
grid: SpatialGrid,
active: Vec<BodyId>,
aabbs: Vec<Aabb>,
constraints: Vec<ContactConstraint>,
previous: Vec<ContactConstraint>,
wakers: Vec<usize>,
contacts: Vec<Contact>,
static_meshes: Vec<StaticSlot>,
static_free: Vec<u32>,
topology: StaticTopology,
static_friction: f32,
sleep_groups: Vec<Vec<u32>>,
free_sleep_groups: Vec<u32>,
woken_groups: Vec<u32>,
island_parent: Vec<u32>,
island_root: Vec<u32>,
island_settled: Vec<bool>,
island_has_dynamic: Vec<bool>,
island_group: Vec<u32>,
}
impl Default for World {
fn default() -> Self {
Self::new()
}
}
impl World {
#[must_use]
pub fn new() -> Self {
Self {
slots: Vec::new(),
free: Vec::new(),
gravity: DEFAULT_GRAVITY,
settings: SolverSettings::default(),
grid: SpatialGrid::new(DEFAULT_CELL_SIZE),
active: Vec::new(),
aabbs: Vec::new(),
constraints: Vec::new(),
previous: Vec::new(),
wakers: Vec::new(),
contacts: Vec::new(),
static_meshes: Vec::new(),
static_free: Vec::new(),
topology: StaticTopology::default(),
static_friction: crate::body::DEFAULT_FRICTION,
sleep_groups: Vec::new(),
free_sleep_groups: Vec::new(),
woken_groups: Vec::new(),
island_parent: Vec::new(),
island_root: Vec::new(),
island_settled: Vec::new(),
island_has_dynamic: Vec::new(),
island_group: Vec::new(),
}
}
#[must_use]
pub fn gravity(&self) -> Vec3 {
self.gravity
}
pub fn set_gravity(&mut self, gravity: Vec3) {
if gravity.is_finite() {
self.gravity = gravity;
}
}
pub fn set_broadphase_cell_size(&mut self, cell_size: f32) {
if cell_size.is_finite() && cell_size > 0.0 {
self.grid = SpatialGrid::new(cell_size);
}
}
pub fn set_velocity_iterations(&mut self, iterations: u32) {
self.settings.velocity_iterations = iterations.max(1);
}
pub fn set_static_friction(&mut self, friction: f32) {
if friction.is_finite() && friction >= 0.0 {
self.static_friction = friction;
}
}
pub fn add_static_mesh(&mut self, positions: &[Vec3]) -> Option<StaticMeshId> {
if positions.is_empty() || positions.len() % 3 != 0 {
return None;
}
if !positions.iter().all(|vertex| vertex.is_finite()) {
return None;
}
let triangle_count = positions.len() / 3;
let mut owned = Vec::with_capacity(positions.len());
let mut triangle_bounds = Vec::with_capacity(triangle_count);
let mut welded = Vec::with_capacity(triangle_count);
for triangle_index in 0..triangle_count {
let base = triangle_index * 3;
let vertices = [positions[base], positions[base + 1], positions[base + 2]];
let ids = [
self.topology.weld(vertices[0]),
self.topology.weld(vertices[1]),
self.topology.weld(vertices[2]),
];
self.topology.add_triangle(ids);
triangle_bounds.push(Aabb::new(
vertices[0].min(vertices[1]).min(vertices[2]),
vertices[0].max(vertices[1]).max(vertices[2]),
));
welded.push(ids);
owned.extend(vertices);
}
let mut bounds = *triangle_bounds.first()?;
for triangle_aabb in &triangle_bounds[1..] {
bounds = bounds.merged(triangle_aabb);
}
let mesh = StaticMesh {
positions: owned,
triangle_bounds,
welded,
bounds,
};
if let Some(index) = self.static_free.pop() {
let slot = &mut self.static_meshes[index as usize];
slot.mesh = Some(mesh);
Some(StaticMeshId::new(index, slot.generation))
} else {
let index = u32::try_from(self.static_meshes.len()).ok()?;
self.static_meshes.push(StaticSlot {
generation: 0,
mesh: Some(mesh),
});
Some(StaticMeshId::new(index, 0))
}
}
pub fn remove_static_mesh(&mut self, id: StaticMeshId) -> bool {
let Some(slot) = self.static_meshes.get_mut(id.index() as usize) else {
return false;
};
if slot.generation != id.generation() {
return false;
}
let Some(mesh) = slot.mesh.take() else {
return false;
};
let bounds = mesh.bounds;
for triangle in &mesh.welded {
self.topology.remove_triangle(*triangle);
}
slot.generation = slot.generation.wrapping_add(1);
self.static_free.push(id.index());
for body_slot in &mut self.slots {
let Some(body) = body_slot.get_mut() else {
continue;
};
if body.sleeping && body.motion.is_simulated() && body.aabb().overlaps(&bounds) {
body.wake();
}
}
true
}
#[must_use]
pub fn contains_static_mesh(&self, id: StaticMeshId) -> bool {
self
.static_meshes
.get(id.index() as usize)
.is_some_and(|slot| slot.generation == id.generation() && slot.mesh.is_some())
}
#[must_use]
pub fn static_mesh_count(&self) -> usize {
self
.static_meshes
.iter()
.filter(|slot| slot.mesh.is_some())
.count()
}
#[must_use]
pub fn contacts(&self) -> &[Contact] {
&self.contacts
}
#[must_use]
pub fn len(&self) -> usize {
self
.slots
.iter()
.filter(|slot| slot.get().is_some())
.count()
}
#[must_use]
pub fn is_empty(&self) -> bool {
self.slots.iter().all(|slot| slot.get().is_none())
}
pub fn add_body(&mut self, desc: BodyDesc) -> BodyId {
let body = Body::from_desc(&desc);
if let Some(index) = self.free.pop() {
let slot = &mut self.slots[index as usize];
slot.body = Some(body);
BodyId::new(index, slot.generation)
} else {
let index = u32::try_from(self.slots.len()).expect("body count fits u32");
self.slots.push(Slot {
generation: 0,
body: Some(body),
});
BodyId::new(index, 0)
}
}
pub fn remove_body(&mut self, id: BodyId) -> bool {
let Some(slot) = self.slots.get_mut(id.index() as usize) else {
return false;
};
if slot.generation != id.generation() || slot.get().is_none() {
return false;
}
slot.body = None;
slot.generation = slot.generation.wrapping_add(1);
self.free.push(id.index());
true
}
#[must_use]
pub fn contains(&self, id: BodyId) -> bool {
self.body(id).is_some()
}
pub fn ids(&self) -> impl Iterator<Item = BodyId> + '_ {
self.slots.iter().enumerate().filter_map(|(index, slot)| {
slot.get().map(|_| {
BodyId::new(
u32::try_from(index).expect("slot count fits u32"),
slot.generation,
)
})
})
}
#[must_use]
pub fn shape(&self, id: BodyId) -> Option<Shape> {
self.body(id).map(|body| body.shape)
}
#[must_use]
pub fn motion(&self, id: BodyId) -> Option<MotionType> {
self.body(id).map(|body| body.motion)
}
#[must_use]
pub fn mass_properties(&self, id: BodyId) -> Option<MassProperties> {
self.body(id).map(|body| body.mass)
}
#[must_use]
pub fn position(&self, id: BodyId) -> Option<Vec3> {
self.body(id).map(|body| body.position)
}
#[must_use]
pub fn orientation(&self, id: BodyId) -> Option<Quat> {
self.body(id).map(|body| body.orientation)
}
#[must_use]
pub fn aabb(&self, id: BodyId) -> Option<Aabb> {
self.body(id).map(Body::aabb)
}
#[must_use]
pub fn velocity(&self, id: BodyId) -> Option<Vec3> {
self.body(id).map(|body| body.linear_velocity)
}
#[must_use]
pub fn angular_velocity(&self, id: BodyId) -> Option<Vec3> {
self.body(id).map(|body| body.angular_velocity)
}
#[must_use]
pub fn friction(&self, id: BodyId) -> Option<f32> {
self.body(id).map(|body| body.friction)
}
#[must_use]
pub fn restitution(&self, id: BodyId) -> Option<f32> {
self.body(id).map(|body| body.restitution)
}
#[must_use]
pub fn is_sleeping(&self, id: BodyId) -> Option<bool> {
self.body(id).map(|body| body.sleeping)
}
pub fn set_pose(&mut self, id: BodyId, position: Vec3, orientation: Quat) -> bool {
if !position.is_finite() || !orientation.is_finite() || orientation.length_squared() <= 0.0 {
return false;
}
let Some(body) = self.body_mut(id) else {
return false;
};
body.position = position;
body.orientation = orientation.normalize();
if body.motion.is_simulated() {
body.wake();
}
true
}
pub fn set_velocity(&mut self, id: BodyId, velocity: Vec3) -> bool {
if !velocity.is_finite() {
return false;
}
let Some(body) = self.body_mut(id) else {
return false;
};
if !body.motion.moves() {
return false;
}
body.linear_velocity = velocity;
if body.motion.is_simulated() {
body.wake();
}
true
}
pub fn set_angular_velocity(&mut self, id: BodyId, angular_velocity: Vec3) -> bool {
if !angular_velocity.is_finite() {
return false;
}
let Some(body) = self.body_mut(id) else {
return false;
};
if !body.motion.moves() {
return false;
}
body.angular_velocity = angular_velocity;
if body.motion.is_simulated() {
body.wake();
}
true
}
pub fn set_friction(&mut self, id: BodyId, friction: f32) -> bool {
if !friction.is_finite() {
return false;
}
let Some(body) = self.body_mut(id) else {
return false;
};
body.friction = friction.max(0.0);
true
}
pub fn set_restitution(&mut self, id: BodyId, restitution: f32) -> bool {
if !restitution.is_finite() {
return false;
}
let Some(body) = self.body_mut(id) else {
return false;
};
body.restitution = restitution.max(0.0);
true
}
pub fn set_damping(&mut self, id: BodyId, linear: f32, angular: f32) -> bool {
if !linear.is_finite() || !angular.is_finite() {
return false;
}
let Some(body) = self.body_mut(id) else {
return false;
};
body.linear_damping = linear.max(0.0);
body.angular_damping = angular.max(0.0);
true
}
pub fn set_gravity_scale(&mut self, id: BodyId, scale: f32) -> bool {
let scale = if scale.is_finite() && scale > 0.0 {
scale
} else {
0.0
};
let Some(body) = self.body_mut(id) else {
return false;
};
#[expect(clippy::float_cmp)]
let changed = body.gravity_scale != scale;
if changed {
body.wake();
}
body.gravity_scale = scale;
true
}
pub fn apply_force(&mut self, id: BodyId, force: Vec3) -> bool {
if !force.is_finite() {
return false;
}
let Some(body) = self.body_mut(id) else {
return false;
};
if !body.motion.is_simulated() {
return false;
}
body.force += force;
body.wake();
true
}
pub fn apply_torque(&mut self, id: BodyId, torque: Vec3) -> bool {
if !torque.is_finite() {
return false;
}
let Some(body) = self.body_mut(id) else {
return false;
};
if !body.motion.is_simulated() {
return false;
}
body.torque += torque;
body.wake();
true
}
pub fn apply_impulse(&mut self, id: BodyId, impulse: Vec3) -> bool {
if !impulse.is_finite() {
return false;
}
let Some(body) = self.body_mut(id) else {
return false;
};
if !body.motion.is_simulated() {
return false;
}
body.linear_velocity += impulse * body.mass.inv_mass;
body.wake();
true
}
pub fn apply_angular_impulse(&mut self, id: BodyId, impulse: Vec3) -> bool {
if !impulse.is_finite() {
return false;
}
let Some(body) = self.body_mut(id) else {
return false;
};
if !body.motion.is_simulated() {
return false;
}
let delta = body.apply_inv_inertia(impulse);
body.angular_velocity += delta;
body.wake();
true
}
pub fn wake(&mut self, id: BodyId) -> bool {
let Some(body) = self.body_mut(id) else {
return false;
};
body.wake();
true
}
pub fn clear_forces(&mut self) {
for slot in &mut self.slots {
if let Some(body) = slot.get_mut() {
body.force = Vec3::ZERO;
body.torque = Vec3::ZERO;
}
}
}
pub fn step(&mut self, delta_time: f32) {
if !delta_time.is_finite() || delta_time <= 0.0 {
return;
}
self.integrate_velocities(delta_time);
self.detect_contacts();
self.carry_impulses();
solver::prepare(&self.slots, &mut self.constraints, delta_time);
solver::warm_start(&mut self.slots, &self.constraints);
solver::solve_velocities(&mut self.slots, &mut self.constraints, self.settings);
solver::solve_positions(&mut self.slots, &mut self.constraints);
self.integrate_positions(delta_time);
self.publish_contacts();
core::mem::swap(&mut self.constraints, &mut self.previous);
self.update_sleep(delta_time);
self.refresh_movement();
self.clear_forces();
}
fn refresh_movement(&mut self) {
for slot in &mut self.slots {
if let Some(body) = slot.get_mut() {
body.refresh_movement(SLEEP_LINEAR_THRESHOLD, SLEEP_ANGULAR_THRESHOLD);
}
}
}
fn integrate_velocities(&mut self, delta_time: f32) {
let gravity = self.gravity;
for slot in &mut self.slots {
let Some(body) = slot.get_mut() else {
continue;
};
if !body.motion.is_simulated() || body.sleeping {
continue;
}
if body.mass.inv_mass > 0.0 && body.gravity_scale > 0.0 {
body.linear_velocity += gravity * (body.gravity_scale * delta_time);
}
body.linear_velocity += body.force * body.mass.inv_mass * delta_time;
body.linear_velocity *= 1.0 / (1.0 + body.linear_damping * delta_time);
let angular_acceleration = body.apply_inv_inertia(body.torque);
body.angular_velocity += angular_acceleration * delta_time;
body.angular_velocity *= 1.0 / (1.0 + body.angular_damping * delta_time);
}
}
fn integrate_positions(&mut self, delta_time: f32) {
for slot in &mut self.slots {
let Some(body) = slot.get_mut() else {
continue;
};
if !body.motion.moves() || body.sleeping {
continue;
}
let linear = body.linear_velocity + body.pseudo_linear_velocity;
body.position += linear * delta_time;
let spin = body.angular_velocity + body.pseudo_angular_velocity;
let speed = spin.length();
if speed > 1e-8 {
let axis = spin / speed;
let step = Quat::from_axis_angle(axis, speed * delta_time);
body.orientation = (step * body.orientation).normalize();
}
body.clear_pseudo_velocities();
}
}
fn detect_contacts(&mut self) {
self.active.clear();
self.aabbs.clear();
for (index, slot) in self.slots.iter().enumerate() {
if let Some(body) = slot.get() {
let Ok(index) = u32::try_from(index) else {
break;
};
self.active.push(BodyId::new(index, slot.generation));
self.aabbs.push(body.aabb());
}
}
let static_friction = self.static_friction;
let Self {
slots,
grid,
active,
aabbs,
constraints,
wakers,
static_meshes,
topology,
..
} = self;
grid.rebuild(aabbs);
constraints.clear();
wakers.clear();
grid.for_each_candidate(aabbs, |first_compact, second_compact| {
let first_index = active[first_compact as usize].index() as usize;
let second_index = active[second_compact as usize].index() as usize;
let (Some(first), Some(second)) = (slots[first_index].get(), slots[second_index].get())
else {
return;
};
if !solver::is_solvable(first, second) {
return;
}
match (first.sleeping, second.sleeping) {
(true, true) => return,
(true, false) => {
if second.was_moving {
wakers.push(first_index);
} else {
return;
}
}
(false, true) => {
if first.was_moving {
wakers.push(second_index);
} else {
return;
}
}
(false, false) => {}
}
let Some(manifold) = narrowphase::collide(
first.shape,
first.position,
first.orientation,
second.shape,
second.position,
second.orientation,
) else {
return;
};
let mut constraint = ContactConstraint {
id_a: active[first_compact as usize],
id_b: ContactPartner::Body(active[second_compact as usize]),
a: u32::try_from(first_index).unwrap_or(0),
b: u32::try_from(second_index).unwrap_or(0),
partner_key: 0,
normal: manifold.normal,
tangents: tangent_basis(manifold.normal),
friction: solver::combine_friction(first.friction, second.friction),
point_count: 0,
points: [ConstraintPoint::default(); MAX_MANIFOLD_POINTS],
};
for (slot, point) in manifold.points[..manifold.count].iter().enumerate() {
constraint.points[slot] = ConstraintPoint {
position: point.position,
anchor_a: point.position - first.position,
anchor_b: point.position - second.position,
depth: point.depth,
feature: point.feature,
..ConstraintPoint::default()
};
}
constraint.point_count = u8::try_from(manifold.count).unwrap_or(0);
constraints.push(constraint);
});
detect_static_contacts(slots, static_friction, static_meshes, topology, constraints);
for index in wakers.drain(..) {
if let Some(body) = slots[index].get_mut() {
body.wake();
}
}
}
fn carry_impulses(&mut self) {
self
.constraints
.sort_unstable_by_key(ContactConstraint::sort_key);
for current in &mut self.constraints {
let key = current.sort_key();
let Ok(found) = self
.previous
.binary_search_by_key(&key, ContactConstraint::sort_key)
else {
continue;
};
let old = &self.previous[found];
for point in current.points_mut() {
if let Some(previous) = old
.points()
.iter()
.find(|entry| entry.feature == point.feature)
{
point.normal_impulse = previous.normal_impulse;
point.tangent_impulse = previous.tangent_impulse;
}
}
}
}
fn publish_contacts(&mut self) {
let Self {
constraints,
previous,
contacts,
..
} = self;
contacts.clear();
for current in constraints.iter() {
let key = current.sort_key();
let old = previous
.binary_search_by_key(&key, ContactConstraint::sort_key)
.ok()
.map(|found| &previous[found]);
for point in current.points() {
let existed = old.is_some_and(|entry| {
entry
.points()
.iter()
.any(|candidate| candidate.feature == point.feature)
});
contacts.push(Contact {
a: current.id_a,
b: current.id_b,
point: point.position,
normal: current.normal,
depth: point.depth,
phase: if existed {
ContactPhase::Persist
} else {
ContactPhase::Begin
},
});
}
}
for old in previous.iter() {
let key = old.sort_key();
let current = constraints
.binary_search_by_key(&key, ContactConstraint::sort_key)
.ok()
.map(|found| &constraints[found]);
for point in old.points() {
let survives = current.is_some_and(|entry| {
entry
.points()
.iter()
.any(|candidate| candidate.feature == point.feature)
});
if !survives {
contacts.push(Contact {
a: old.id_a,
b: old.id_b,
point: point.position,
normal: old.normal,
depth: point.depth,
phase: ContactPhase::End,
});
}
}
}
}
fn update_sleep(&mut self, delta_time: f32) {
self.dissolve_woken_groups();
for slot in &mut self.slots {
let Some(body) = slot.get_mut() else {
continue;
};
if !body.motion.is_simulated() || body.sleeping {
continue;
}
if body.is_below_sleep_threshold(SLEEP_LINEAR_THRESHOLD, SLEEP_ANGULAR_THRESHOLD) {
body.sleep_timer += delta_time;
} else {
body.sleep_timer = 0.0;
}
}
self.sleep_settled_islands();
}
fn dissolve_woken_groups(&mut self) {
self.woken_groups.clear();
for slot in &self.slots {
let Some(body) = slot.get() else {
continue;
};
if !body.sleeping
&& body.sleep_group != NO_SLEEP_GROUP
&& !self.woken_groups.contains(&body.sleep_group)
{
self.woken_groups.push(body.sleep_group);
}
}
if self.woken_groups.is_empty() {
return;
}
for slot in &mut self.slots {
let Some(body) = slot.get_mut() else {
continue;
};
if self.woken_groups.contains(&body.sleep_group) {
body.wake();
body.sleep_group = NO_SLEEP_GROUP;
}
}
for group in self.woken_groups.drain(..) {
if let Some(members) = self.sleep_groups.get_mut(group as usize) {
members.clear();
}
self.free_sleep_groups.push(group);
}
}
fn sleep_settled_islands(&mut self) {
let count = self.slots.len();
let Ok(count_u32) = u32::try_from(count) else {
return;
};
if count == 0 {
return;
}
self.island_parent.clear();
self.island_parent.extend(0..count_u32);
self.island_root.clear();
self.island_root.resize(count, NO_SLEEP_GROUP);
self.island_settled.clear();
self.island_settled.resize(count, true);
self.island_has_dynamic.clear();
self.island_has_dynamic.resize(count, false);
self.island_group.clear();
self.island_group.resize(count, NO_SLEEP_GROUP);
for constraint in &self.constraints {
if constraint.b == STATIC_PARTNER {
continue;
}
let first = find_root(&mut self.island_parent, constraint.a);
let second = find_root(&mut self.island_parent, constraint.b);
if first != second {
self.island_parent[second as usize] = first;
}
}
for index in 0..count {
let Some(body) = self.slots[index].get() else {
continue;
};
if !body.motion.is_simulated() || body.sleeping {
continue;
}
let root = find_root(&mut self.island_parent, u32::try_from(index).unwrap_or(0));
self.island_root[index] = root;
self.island_has_dynamic[root as usize] = true;
if body.sleep_timer < SLEEP_TIME {
self.island_settled[root as usize] = false;
}
}
for index in 0..count {
let root = self.island_root[index];
if root == NO_SLEEP_GROUP
|| !self.island_has_dynamic[root as usize]
|| !self.island_settled[root as usize]
{
continue;
}
let mut group = self.island_group[root as usize];
if group == NO_SLEEP_GROUP {
group = self.allocate_sleep_group();
self.island_group[root as usize] = group;
}
if let Some(members) = self.sleep_groups.get_mut(group as usize) {
members.push(u32::try_from(index).unwrap_or(0));
}
if let Some(body) = self.slots[index].get_mut() {
body.put_to_sleep();
body.sleep_group = group;
}
}
}
fn allocate_sleep_group(&mut self) -> u32 {
if let Some(group) = self.free_sleep_groups.pop() {
return group;
}
let index = u32::try_from(self.sleep_groups.len()).unwrap_or(u32::MAX);
self.sleep_groups.push(Vec::new());
index
}
fn body(&self, id: BodyId) -> Option<&Body> {
let slot = self.slots.get(id.index() as usize)?;
if slot.generation != id.generation() {
return None;
}
slot.get()
}
fn body_mut(&mut self, id: BodyId) -> Option<&mut Body> {
let slot = self.slots.get_mut(id.index() as usize)?;
if slot.generation != id.generation() {
return None;
}
slot.get_mut()
}
}
fn detect_static_contacts(
slots: &[Slot],
static_friction: f32,
static_meshes: &[StaticSlot],
topology: &StaticTopology,
constraints: &mut Vec<ContactConstraint>,
) {
for (mesh_index, slot) in static_meshes.iter().enumerate() {
let Some(mesh) = slot.mesh.as_ref() else {
continue;
};
let Ok(mesh_index) = u32::try_from(mesh_index) else {
break;
};
let mesh_id = StaticMeshId::new(mesh_index, slot.generation);
for (body_index, body_slot) in slots.iter().enumerate() {
let Some(body) = body_slot.get() else {
continue;
};
if body.sleeping || !body.motion.moves() {
continue;
}
let body_bounds = body.aabb();
if !body_bounds.overlaps(&mesh.bounds) {
continue;
}
let Ok(body_index) = u32::try_from(body_index) else {
break;
};
for triangle_index in 0..mesh.triangle_count() {
if !body_bounds.overlaps(&mesh.triangle_bounds[triangle_index]) {
continue;
}
let triangle = mesh.triangle(triangle_index);
let hit = match body.shape {
Shape::Box { half_extents } => {
let frame =
narrowphase::BoxFrame::new(body.position, body.orientation, half_extents);
narrowphase::box_triangle(&frame, triangle)
}
Shape::Sphere { radius } => {
narrowphase::sphere_triangle(body.position, radius, triangle).map(|manifold| {
narrowphase::TriangleHit {
manifold,
source_edge: None,
}
})
}
};
let Some(hit) = hit else {
continue;
};
if let Some(edge) = hit.source_edge {
let ids = mesh.welded[triangle_index];
if topology.is_internal_edge(ids[edge], ids[(edge + 1) % 3]) {
continue;
}
}
let key =
(u64::from(mesh_id.index()) << 32) | u64::try_from(triangle_index).unwrap_or(0);
let mut constraint = ContactConstraint {
id_a: BodyId::new(body_index, body_slot.generation),
id_b: ContactPartner::Static(mesh_id),
a: body_index,
b: STATIC_PARTNER,
partner_key: key,
normal: hit.manifold.normal,
tangents: tangent_basis(hit.manifold.normal),
friction: solver::combine_friction(body.friction, static_friction),
point_count: 0,
points: [ConstraintPoint::default(); MAX_MANIFOLD_POINTS],
};
for (slot, point) in hit.manifold.points[..hit.manifold.count].iter().enumerate() {
constraint.points[slot] = ConstraintPoint {
position: point.position,
anchor_a: point.position - body.position,
anchor_b: Vec3::ZERO,
depth: point.depth,
feature: point.feature,
..ConstraintPoint::default()
};
}
constraint.point_count = u8::try_from(hit.manifold.count).unwrap_or(0);
constraints.push(constraint);
}
}
}
}
fn find_root(parent: &mut [u32], start: u32) -> u32 {
let mut index = start;
loop {
let Some(&up) = parent.get(index as usize) else {
return index;
};
if up == index {
return index;
}
let grandparent = parent.get(up as usize).copied().unwrap_or(up);
parent[index as usize] = grandparent;
index = grandparent;
}
}
#[cfg(test)]
#[allow(
clippy::cast_precision_loss,
clippy::cast_possible_truncation,
clippy::cast_sign_loss
)]
mod tests {
use super::{DEFAULT_GRAVITY, SLEEP_TIME, World};
use crate::body::{BodyDesc, BodyId};
use crate::contact::{ContactPartner, ContactPhase};
use crate::shape::Shape;
use he3d_math::Vec3;
fn free_fall_world() -> (World, BodyId) {
let mut world = World::new();
let ball =
world.add_body(BodyDesc::dynamic(Shape::sphere(0.5), 1.0).at(Vec3::new(0.0, 10.0, 0.0)));
(world, ball)
}
fn ground(world: &mut World, top: f32) -> BodyId {
let half = Vec3::new(20.0, 0.5, 20.0);
world.add_body(BodyDesc::stationary(Shape::cuboid(half)).at(Vec3::new(0.0, top - 0.5, 0.0)))
}
fn settle(world: &mut World, seconds: f32) {
let dt = 1.0 / 120.0;
let steps = (seconds / dt) as u32;
for _ in 0..steps {
world.step(dt);
}
}
#[test]
fn gravity_accelerates_a_dynamic_body() {
let (mut world, ball) = free_fall_world();
world.step(0.5);
let velocity = world.velocity(ball).expect("ball exists");
assert!((velocity.y + 4.905).abs() < 1e-3, "got {velocity:?}");
}
#[test]
fn a_static_body_never_moves() {
let mut world = World::new();
let slab = world.add_body(BodyDesc::stationary(Shape::cuboid(Vec3::splat(5.0))));
for _ in 0..120 {
world.step(1.0 / 60.0);
}
assert_eq!(world.position(slab), Some(Vec3::ZERO));
assert_eq!(world.velocity(slab), Some(Vec3::ZERO));
}
#[test]
fn a_kinematic_body_ignores_gravity_and_holds_its_velocity() {
let mut world = World::new();
let platform = world.add_body(BodyDesc::kinematic(Shape::cuboid(Vec3::splat(0.5))));
assert!(world.set_velocity(platform, Vec3::new(2.0, 0.0, 0.0)));
for _ in 0..120 {
world.step(1.0 / 60.0);
}
let position = world.position(platform).expect("platform exists");
assert!((position.x - 4.0).abs() < 1e-3, "got {position:?}");
assert!(position.y.abs() < 1e-6, "got {position:?}");
}
#[test]
fn a_kinematic_body_ignores_forces_and_keeps_its_speed() {
let mut world = World::new();
let platform = world.add_body(BodyDesc::kinematic(Shape::cuboid(Vec3::splat(0.5))));
world.set_velocity(platform, Vec3::new(1.0, 0.0, 0.0));
assert!(!world.apply_force(platform, Vec3::new(1000.0, 0.0, 0.0)));
world.step(0.1);
let velocity = world.velocity(platform).expect("platform exists");
assert!((velocity.x - 1.0).abs() < 1e-6, "got {velocity:?}");
}
#[test]
fn damping_decays_velocity_by_the_discrete_factor_each_step() {
let mut world = World::new();
world.set_gravity(Vec3::ZERO);
let dt = 1.0 / 60.0;
let damping = 2.0;
let body = world.add_body(
BodyDesc::dynamic(Shape::sphere(0.5), 1.0)
.with_velocity(Vec3::new(10.0, 0.0, 0.0))
.with_damping(damping, 0.0),
);
let factor = 1.0 / (1.0 + damping * dt);
let mut expected = 10.0;
for _ in 0..120 {
world.step(dt);
expected *= factor;
let speed = world.velocity(body).expect("body exists").x;
assert!(speed > 0.0, "damping must not reverse the direction");
assert!(
(speed - expected).abs() < 1e-3,
"got {speed}, expected {expected}"
);
}
}
#[test]
fn a_zero_mass_dynamic_body_ignores_gravity() {
let mut world = World::new();
let ghost = world.add_body(BodyDesc::dynamic(Shape::sphere(0.5), 0.0).at(Vec3::Y));
world.step(1.0);
assert_eq!(world.position(ghost), Some(Vec3::Y));
}
#[test]
fn removing_a_body_invalidates_its_handle_without_disturbing_others() {
let mut world = World::new();
let first = world.add_body(BodyDesc::dynamic(Shape::sphere(0.5), 1.0));
let second = world.add_body(BodyDesc::dynamic(Shape::sphere(0.5), 1.0));
assert!(world.remove_body(first));
assert!(!world.contains(first));
assert!(world.contains(second));
assert!(!world.remove_body(first), "double removal must fail");
let third = world.add_body(BodyDesc::dynamic(Shape::sphere(0.5), 1.0));
assert_eq!(third.index(), first.index());
assert_ne!(third.generation(), first.generation());
assert!(!world.contains(first));
}
#[test]
fn a_degenerate_step_is_ignored() {
let (mut world, ball) = free_fall_world();
let before = world.position(ball).expect("ball exists");
world.step(0.0);
world.step(-1.0);
world.step(f32::NAN);
world.step(f32::INFINITY);
assert_eq!(world.position(ball), Some(before));
}
#[test]
fn a_force_acts_for_exactly_one_step() {
let mut world = World::new();
world.set_gravity(Vec3::ZERO);
let body = world.add_body(BodyDesc::dynamic(Shape::sphere(0.5), 1.0));
assert!(world.apply_force(body, Vec3::new(0.0, 100.0, 0.0)));
world.step(0.1);
let after_push = world.velocity(body).expect("body exists").y;
assert!((after_push - 10.0).abs() < 1e-3, "got {after_push}");
world.step(0.1);
let after_idle = world.velocity(body).expect("body exists").y;
assert!(
(after_idle - after_push).abs() < 1e-3,
"the force must not persist: {after_idle}"
);
}
#[test]
fn a_spinning_body_keeps_a_unit_quaternion() {
let mut world = World::new();
world.set_gravity(Vec3::ZERO);
let body = world.add_body(
BodyDesc::dynamic(Shape::cuboid(Vec3::splat(0.5)), 1.0)
.with_angular_velocity(Vec3::new(3.0, 1.0, 2.0)),
);
for _ in 0..2000 {
world.step(1.0 / 120.0);
let orientation = world.orientation(body).expect("body exists");
assert!(
(orientation.length() - 1.0).abs() < 1e-4,
"quaternion drifted: {}",
orientation.length()
);
}
}
#[test]
fn ids_reports_every_live_body() {
let mut world = World::new();
let a = world.add_body(BodyDesc::dynamic(Shape::sphere(0.5), 1.0));
let b = world.add_body(BodyDesc::dynamic(Shape::sphere(0.5), 1.0));
let mut ids: alloc::vec::Vec<_> = world.ids().collect();
ids.sort_unstable();
assert_eq!(ids, alloc::vec![a.min(b), a.max(b)]);
assert_eq!(world.len(), 2);
assert!(!world.is_empty());
world.remove_body(a);
assert_eq!(world.len(), 1);
}
#[test]
fn a_ball_lands_on_the_ground_and_stops() {
let mut world = World::new();
let slab = ground(&mut world, 0.0);
let ball = world.add_body(
BodyDesc::dynamic(Shape::sphere(0.5), 1.0)
.at(Vec3::new(0.0, 3.0, 0.0))
.with_friction(0.6),
);
settle(&mut world, 3.0);
let position = world.position(ball).expect("ball exists");
assert!(
(position.y - 0.5).abs() < 0.05,
"the ball should rest on the slab, got {position:?}"
);
let speed = world.velocity(ball).expect("ball exists").length();
assert!(speed < 0.1, "the ball should be at rest, got {speed}");
assert_eq!(world.position(slab), Some(Vec3::new(0.0, -0.5, 0.0)));
}
#[test]
fn a_ball_does_not_fall_through_the_ground() {
let mut world = World::new();
ground(&mut world, 0.0);
let ball = world.add_body(
BodyDesc::dynamic(Shape::sphere(0.5), 1.0)
.at(Vec3::new(0.0, 3.0, 0.0))
.with_velocity(Vec3::new(0.0, -20.0, 0.0)),
);
settle(&mut world, 2.0);
let y = world.position(ball).expect("ball exists").y;
assert!(y > -0.5, "the ball tunnelled through, got y={y}");
}
#[test]
fn a_single_box_lands_flat_and_stops() {
let mut world = World::new();
ground(&mut world, 0.0);
let cube = world.add_body(
BodyDesc::dynamic(Shape::cuboid(Vec3::splat(0.5)), 1.0).at(Vec3::new(0.0, 2.0, 0.0)),
);
settle(&mut world, 3.0);
let position = world.position(cube).expect("cube exists");
assert!(
(position.y - 0.5).abs() < 0.05,
"cube should rest at 0.5, got {position:?}"
);
assert!(
world.velocity(cube).expect("cube exists").length() < 0.1,
"cube should be at rest"
);
}
#[test]
fn a_stack_of_boxes_settles_without_sinking() {
let mut world = World::new();
ground(&mut world, 0.0);
let mut cubes = alloc::vec::Vec::new();
for level in 0..5 {
let y = 0.5 + level as f32 * 1.01;
cubes.push(
world.add_body(
BodyDesc::dynamic(Shape::cuboid(Vec3::splat(0.5)), 1.0)
.at(Vec3::new(0.0, y, 0.0))
.with_friction(0.9),
),
);
}
settle(&mut world, 6.0);
for (level, cube) in cubes.iter().enumerate() {
let y = world.position(*cube).expect("cube exists").y;
let expected = 0.5 + level as f32;
assert!(
(y - expected).abs() < 0.15,
"level {level} should rest near {expected}, got {y}"
);
}
}
#[test]
fn a_resting_stack_does_not_jitter() {
let mut world = World::new();
ground(&mut world, 0.0);
let mut cubes = alloc::vec::Vec::new();
for level in 0..4 {
cubes.push(
world.add_body(
BodyDesc::dynamic(Shape::cuboid(Vec3::splat(0.5)), 1.0)
.at(Vec3::new(0.0, 0.5 + level as f32 * 1.01, 0.0))
.with_friction(0.9),
),
);
}
settle(&mut world, 4.0);
let before: alloc::vec::Vec<_> = cubes
.iter()
.map(|cube| world.position(*cube).expect("cube exists"))
.collect();
settle(&mut world, 2.0);
for (index, cube) in cubes.iter().enumerate() {
let after = world.position(*cube).expect("cube exists");
assert!(
(after - before[index]).length() < 0.02,
"level {index} drifted from {:?} to {after:?}",
before[index]
);
}
}
#[test]
fn a_resting_stack_falls_asleep() {
let mut world = World::new();
ground(&mut world, 0.0);
let mut cubes = alloc::vec::Vec::new();
for level in 0..3 {
cubes.push(
world.add_body(
BodyDesc::dynamic(Shape::cuboid(Vec3::splat(0.5)), 1.0)
.at(Vec3::new(0.0, 0.5 + level as f32 * 1.01, 0.0))
.with_friction(0.9),
),
);
}
settle(&mut world, SLEEP_TIME + 6.0);
for cube in &cubes {
assert_eq!(
world.is_sleeping(*cube),
Some(true),
"a settled stack should sleep"
);
}
}
#[test]
fn a_sleeping_stack_wakes_when_a_neighbour_arrives() {
let mut world = World::new();
ground(&mut world, 0.0);
let cube = world.add_body(
BodyDesc::dynamic(Shape::cuboid(Vec3::splat(0.5)), 1.0)
.at(Vec3::new(0.0, 0.5, 0.0))
.with_friction(0.9),
);
settle(&mut world, SLEEP_TIME + 4.0);
assert_eq!(world.is_sleeping(cube), Some(true));
let projectile = world.add_body(
BodyDesc::dynamic(Shape::sphere(0.4), 1.0)
.at(Vec3::new(-3.0, 0.5, 0.0))
.with_velocity(Vec3::new(12.0, 0.0, 0.0)),
);
settle(&mut world, 0.5);
assert_eq!(
world.is_sleeping(cube),
Some(false),
"the impact should have woken the settled cube"
);
let moved = world.position(cube).expect("cube exists").x;
assert!(moved.abs() > 1e-3, "the cube should have been pushed");
let _ = projectile;
}
#[test]
fn contacts_report_begin_then_persist_then_end() {
let mut world = World::new();
let slab = ground(&mut world, 0.0);
world.add_body(BodyDesc::dynamic(Shape::sphere(0.5), 1.0).at(Vec3::new(0.0, 1.2, 0.0)));
let mut saw_begin = false;
for _ in 0..240 {
world.step(1.0 / 120.0);
if world
.contacts()
.iter()
.any(|contact| contact.phase == ContactPhase::Begin)
{
saw_begin = true;
break;
}
}
assert!(saw_begin, "landing must report a Begin phase");
settle(&mut world, 0.5);
assert!(
world
.contacts()
.iter()
.any(|contact| contact.phase == ContactPhase::Persist),
"a resting contact must report Persist"
);
assert!(world.remove_body(slab));
world.step(1.0 / 120.0);
assert!(
world
.contacts()
.iter()
.any(|contact| contact.phase == ContactPhase::End),
"removing the ground must report an End"
);
}
#[test]
fn contacts_are_not_reported_for_bodies_that_are_apart() {
let mut world = World::new();
world.add_body(BodyDesc::dynamic(Shape::sphere(0.5), 1.0).at(Vec3::new(0.0, 10.0, 0.0)));
world.add_body(BodyDesc::dynamic(Shape::sphere(0.5), 1.0).at(Vec3::new(50.0, 10.0, 0.0)));
world.step(1.0 / 120.0);
assert!(world.contacts().is_empty());
}
#[test]
fn two_static_bodies_are_never_solved_against_each_other() {
let mut world = World::new();
world.add_body(BodyDesc::stationary(Shape::cuboid(Vec3::splat(1.0))));
world.add_body(
BodyDesc::stationary(Shape::cuboid(Vec3::splat(1.0))).at(Vec3::new(0.5, 0.0, 0.0)),
);
world.step(1.0 / 120.0);
assert!(world.contacts().is_empty());
}
#[test]
fn a_settled_tower_sleeps_and_then_does_not_move_at_all() {
let mut world = World::new();
ground(&mut world, 0.0);
let mut cubes = alloc::vec::Vec::new();
for level in 0..4 {
cubes.push(
world.add_body(
BodyDesc::dynamic(Shape::cuboid(Vec3::splat(0.5)), 1.0)
.at(Vec3::new(0.0, 0.5 + level as f32 * 1.02, 0.0))
.with_friction(1.0),
),
);
}
settle(&mut world, 8.0);
for (index, cube) in cubes.iter().enumerate() {
assert_eq!(
world.is_sleeping(*cube),
Some(true),
"level {index} of a settled tower should sleep"
);
let y = world.position(*cube).expect("cube exists").y;
assert!(
(y - (0.5 + index as f32)).abs() < 0.1,
"level {index} sits at {y}"
);
}
let resting: alloc::vec::Vec<_> = cubes
.iter()
.map(|cube| world.position(*cube).expect("cube exists"))
.collect();
settle(&mut world, 10.0);
for (index, cube) in cubes.iter().enumerate() {
let after = world.position(*cube).expect("cube exists");
assert!(
(after - resting[index]).length() < 1e-5,
"level {index} moved from {:?} to {after:?} after sleeping",
resting[index]
);
}
}
#[test]
fn a_taller_tower_holds_its_height_without_sinking() {
let mut world = World::new();
ground(&mut world, 0.0);
let mut cubes = alloc::vec::Vec::new();
for level in 0..6 {
cubes.push(
world.add_body(
BodyDesc::dynamic(Shape::cuboid(Vec3::splat(0.5)), 1.0)
.at(Vec3::new(0.0, 0.5 + level as f32 * 1.02, 0.0))
.with_friction(1.0),
),
);
}
settle(&mut world, 12.0);
for (index, cube) in cubes.iter().enumerate() {
let y = world.position(*cube).expect("cube exists").y;
let expected = 0.5 + index as f32;
assert!(
(y - expected).abs() < 0.1,
"level {index} should rest near {expected}, got {y}"
);
}
}
fn flat_terrain_cells(start_x: usize, cells: usize, step: f32) -> alloc::vec::Vec<Vec3> {
let mut positions = alloc::vec::Vec::new();
for z in 0..cells {
for x in start_x..(start_x + cells) {
let x0 = x as f32 * step;
let x1 = x0 + step;
let z0 = z as f32 * step;
let z1 = z0 + step;
positions.extend([
Vec3::new(x0, 0.0, z0),
Vec3::new(x0, 0.0, z1),
Vec3::new(x1, 0.0, z0),
Vec3::new(x1, 0.0, z0),
Vec3::new(x0, 0.0, z1),
Vec3::new(x1, 0.0, z1),
]);
}
}
positions
}
fn flat_terrain(cells: usize, step: f32) -> alloc::vec::Vec<Vec3> {
flat_terrain_cells(0, cells, step)
}
#[test]
fn a_body_rests_on_static_terrain() {
let mut world = World::new();
world
.add_static_mesh(&flat_terrain(2, 2.0))
.expect("terrain");
let ball =
world.add_body(BodyDesc::dynamic(Shape::sphere(0.5), 1.0).at(Vec3::new(1.0, 3.0, 1.0)));
settle(&mut world, 3.0);
let y = world.position(ball).expect("ball exists").y;
assert!(
(y - 0.5).abs() < 0.1,
"the ball should rest on the terrain, got y={y}"
);
}
#[test]
fn a_contact_with_static_terrain_names_the_mesh() {
let mut world = World::new();
let mesh = world
.add_static_mesh(&flat_terrain(1, 2.0))
.expect("terrain");
world.add_body(BodyDesc::dynamic(Shape::sphere(0.5), 1.0).at(Vec3::new(1.0, 0.4, 1.0)));
settle(&mut world, 0.2);
assert!(
world
.contacts()
.iter()
.any(|contact| contact.b == ContactPartner::Static(mesh)),
"the terrain contact must name the mesh"
);
}
#[test]
fn a_box_slides_across_triangle_seams_without_catching() {
let mut world = World::new();
world
.add_static_mesh(&flat_terrain(4, 2.0))
.expect("terrain");
let cube = world.add_body(
BodyDesc::dynamic(Shape::cuboid(Vec3::splat(0.5)), 1.0)
.at(Vec3::new(0.5, 0.55, 0.5))
.with_friction(0.0)
.with_velocity(Vec3::new(4.0, 0.0, 0.0)),
);
settle(&mut world, 1.0);
let position = world.position(cube).expect("cube exists");
assert!(
position.x > 3.5,
"the box caught on a triangle seam, ended at {position:?}"
);
assert!(
(position.y - 0.5).abs() < 0.1,
"the box sank or floated, ended at {position:?}"
);
}
#[test]
fn a_box_slides_across_a_seam_between_two_separate_meshes() {
let mut world = World::new();
world
.add_static_mesh(&flat_terrain_cells(0, 2, 2.0))
.expect("left tile");
world
.add_static_mesh(&flat_terrain_cells(2, 2, 2.0))
.expect("right tile");
let cube = world.add_body(
BodyDesc::dynamic(Shape::cuboid(Vec3::splat(0.5)), 1.0)
.at(Vec3::new(0.5, 0.55, 0.5))
.with_friction(0.0)
.with_velocity(Vec3::new(4.0, 0.0, 0.0)),
);
settle(&mut world, 1.5);
let position = world.position(cube).expect("cube exists");
assert!(
position.x > 5.0,
"the box caught on the seam between two meshes, ended at {position:?}"
);
assert!(
(position.y - 0.5).abs() < 0.1,
"the box sank or floated, ended at {position:?}"
);
}
#[test]
fn removing_static_geometry_lets_bodies_fall_through() {
let mut world = World::new();
let mesh = world
.add_static_mesh(&flat_terrain(2, 2.0))
.expect("terrain");
let ball =
world.add_body(BodyDesc::dynamic(Shape::sphere(0.5), 1.0).at(Vec3::new(1.0, 0.5, 1.0)));
settle(&mut world, 1.0);
assert!((world.position(ball).expect("ball").y - 0.5).abs() < 0.1);
assert!(world.remove_static_mesh(mesh));
assert!(!world.contains_static_mesh(mesh));
assert_eq!(world.static_mesh_count(), 0);
assert!(!world.remove_static_mesh(mesh), "double removal must fail");
settle(&mut world, 1.0);
assert!(
world.position(ball).expect("ball").y < -1.0,
"the ball should fall once its terrain is gone"
);
}
#[test]
fn removing_one_mesh_releases_only_its_own_edges() {
let mut world = World::new();
world
.add_static_mesh(&flat_terrain_cells(0, 2, 2.0))
.expect("left tile");
let right = world
.add_static_mesh(&flat_terrain_cells(1, 2, 2.0))
.expect("right tile");
let cube = world.add_body(
BodyDesc::dynamic(Shape::cuboid(Vec3::splat(0.5)), 1.0)
.at(Vec3::new(0.5, 0.55, 0.5))
.with_friction(0.0)
.with_velocity(Vec3::new(4.0, 0.0, 0.0)),
);
settle(&mut world, 0.6);
assert!(
world.position(cube).expect("cube").x > 2.0,
"the box should have crossed the shared boundary"
);
assert!(world.remove_static_mesh(right));
settle(&mut world, 0.6);
assert!(world.position(cube).expect("cube").is_finite());
}
#[test]
fn malformed_static_geometry_is_rejected() {
let mut world = World::new();
assert!(world.add_static_mesh(&[]).is_none(), "empty input");
assert!(
world.add_static_mesh(&[Vec3::ZERO, Vec3::X]).is_none(),
"not a whole number of triangles"
);
assert!(
world
.add_static_mesh(&[Vec3::ZERO, Vec3::X, Vec3::new(f32::NAN, 0.0, 0.0)])
.is_none(),
"a non-finite coordinate"
);
assert_eq!(world.static_mesh_count(), 0);
}
fn ledge_block(overhang: f32, platform_half: Vec3, block_half: f32) -> (Vec3, f32, bool) {
let mut world = World::new();
world.add_body(BodyDesc::stationary(Shape::cuboid(platform_half)).at(Vec3::ZERO));
let centre_x = platform_half.x - block_half + overhang * block_half * 2.0;
let block = world.add_body(
BodyDesc::dynamic(Shape::cuboid(Vec3::splat(block_half)), 1.0)
.at(Vec3::new(
centre_x,
platform_half.y + block_half + 0.02,
0.0,
))
.with_friction(0.6),
);
for _ in 0..600 {
world.step(1.0 / 120.0);
}
let position = world.position(block).expect("block exists");
let up = world.orientation(block).expect("block exists") * Vec3::Y;
let tilt = up.dot(Vec3::Y).clamp(-1.0, 1.0).acos().to_degrees();
(position, tilt, world.is_sleeping(block) == Some(true))
}
#[test]
fn a_block_tips_off_once_its_centre_passes_the_edge() {
let platform_half = Vec3::new(3.0, 0.5, 3.0);
let block_half = 0.5;
let resting_height = platform_half.y + block_half;
let mut worst_tilt = 0.0_f32;
for overhang in [0.1_f32, 0.2, 0.3, 0.4] {
let (position, tilt, sleeping) = ledge_block(overhang, platform_half, block_half);
assert!(
(position.y - resting_height).abs() < 0.02,
"{overhang} overhang should rest on the ledge, got y={}",
position.y
);
assert!(sleeping, "{overhang} overhang should settle and sleep");
worst_tilt = worst_tilt.max(tilt);
}
assert!(
worst_tilt < 0.01,
"a block resting on a ledge must stay level, it tilted {worst_tilt} deg"
);
for overhang in [0.6_f32, 0.7, 0.8, 0.9] {
let (position, _, _) = ledge_block(overhang, platform_half, block_half);
assert!(
position.y < 0.0,
"{overhang} overhang is past the centre of mass and must fall, got y={}",
position.y
);
}
}
#[test]
fn a_block_sliding_towards_a_ledge_leaves_it_instead_of_hanging() {
let mut world = World::new();
world.add_body(
BodyDesc::stationary(Shape::cuboid(Vec3::new(3.0, 0.5, 3.0)))
.at(Vec3::ZERO)
.with_friction(0.02),
);
let block = world.add_body(
BodyDesc::dynamic(Shape::cuboid(Vec3::splat(0.5)), 1.0)
.at(Vec3::new(0.0, 1.02, 0.0))
.with_friction(0.02)
.with_velocity(Vec3::new(3.0, 0.0, 0.0)),
);
let mut left_the_ledge = false;
for _ in 0..600 {
world.step(1.0 / 120.0);
let position = world.position(block).expect("block exists");
if position.x > 3.0 && position.y < 0.0 {
left_the_ledge = true;
break;
}
}
assert!(
left_the_ledge,
"the block should have slid off the ledge, ended at {:?}",
world.position(block).expect("block exists")
);
}
fn spanning_plank(gap: f32, plank_half: f32, platform_half: f32) -> (f32, f32, bool) {
let mut world = World::new();
let offset = gap / 2.0 + platform_half;
for side in [-1.0_f32, 1.0] {
world.add_body(
BodyDesc::stationary(Shape::cuboid(Vec3::new(platform_half, 0.5, 3.0))).at(Vec3::new(
side * offset,
0.0,
0.0,
)),
);
}
let plank = world.add_body(
BodyDesc::dynamic(Shape::cuboid(Vec3::new(plank_half, 0.25, 0.5)), 1.0)
.at(Vec3::new(0.0, 0.5 + 0.25 + 0.02, 0.0))
.with_friction(0.8),
);
for _ in 0..600 {
world.step(1.0 / 120.0);
}
let y = world.position(plank).expect("plank exists").y;
let up = world.orientation(plank).expect("plank exists") * Vec3::Y;
let tilt = up.dot(Vec3::Y).clamp(-1.0, 1.0).acos().to_degrees();
(y, tilt, world.is_sleeping(plank) == Some(true))
}
#[test]
fn a_plank_spanning_a_gap_rests_level_until_the_gap_outgrows_it() {
let plank_half = 0.9;
let platform_half = 1.5;
let resting = 0.5 + 0.25;
for gap in [0.4_f32, 0.8, 1.2, 1.6, 1.7] {
let (y, tilt, sleeping) = spanning_plank(gap, plank_half, platform_half);
assert!(
(y - resting).abs() < 0.02,
"gap {gap} should rest at {resting}, got y={y}"
);
assert!(tilt < 0.01, "gap {gap} tilted the plank by {tilt} deg");
assert!(sleeping, "gap {gap} should settle and sleep");
}
let (y, _, _) = spanning_plank(1.9, plank_half, platform_half);
assert!(
y < 0.0,
"a gap wider than the plank must let it through, got y={y}"
);
}
#[test]
fn a_sphere_rests_on_a_ledge_up_to_the_edge_and_falls_past_it() {
let radius = 0.4;
let resting = 0.5 + radius;
for centre_x in [2.0_f32, 2.6, 2.99] {
let mut world = World::new();
world
.add_body(BodyDesc::stationary(Shape::cuboid(Vec3::new(3.0, 0.5, 3.0))).at(Vec3::ZERO));
let ball = world.add_body(
BodyDesc::dynamic(Shape::sphere(radius), 1.0)
.at(Vec3::new(centre_x, resting + 0.02, 0.0))
.with_friction(0.6),
);
for _ in 0..600 {
world.step(1.0 / 120.0);
}
let y = world.position(ball).expect("ball exists").y;
assert!(
(y - resting).abs() < 0.02,
"centre at {centre_x} is over the ledge and should rest, got y={y}"
);
assert_eq!(world.is_sleeping(ball), Some(true));
}
let mut world = World::new();
world.add_body(BodyDesc::stationary(Shape::cuboid(Vec3::new(3.0, 0.5, 3.0))).at(Vec3::ZERO));
let ball = world.add_body(
BodyDesc::dynamic(Shape::sphere(radius), 1.0)
.at(Vec3::new(3.01, resting + 0.02, 0.0))
.with_friction(0.6),
);
for _ in 0..600 {
world.step(1.0 / 120.0);
}
assert!(
world.position(ball).expect("ball exists").y < 0.0,
"a sphere whose centre is past the edge must fall"
);
}
#[test]
fn a_sphere_rolling_across_mesh_seams_keeps_its_speed() {
let mut world = World::new();
world
.add_static_mesh(&flat_terrain_cells(0, 2, 2.0))
.expect("left tile");
world
.add_static_mesh(&flat_terrain_cells(2, 2, 2.0))
.expect("right tile");
let ball = world.add_body(
BodyDesc::dynamic(Shape::sphere(0.4), 1.0)
.at(Vec3::new(0.5, 0.4005, 0.5))
.with_friction(0.0)
.with_velocity(Vec3::new(4.0, 0.0, 0.0)),
);
let mut lowest = f32::MAX;
for _ in 0..180 {
world.step(1.0 / 120.0);
lowest = lowest.min(world.position(ball).expect("ball exists").y);
}
let velocity = world.velocity(ball).expect("ball exists");
assert!(
(velocity.x - 4.0).abs() < 0.05,
"the seam stole horizontal speed, vx={}",
velocity.x
);
assert!(
lowest > 0.39,
"the ball dipped into the seam, lowest y={lowest}"
);
}
fn corner_slide(centre: f32) -> (Vec3, f32) {
let mut world = World::new();
world.add_body(BodyDesc::stationary(Shape::cuboid(Vec3::new(3.0, 0.5, 3.0))).at(Vec3::ZERO));
let block = world.add_body(
BodyDesc::dynamic(Shape::cuboid(Vec3::new(0.5, 0.25, 0.5)), 1.0)
.at(Vec3::new(centre, 0.5 + 0.25 + 0.02, centre))
.with_friction(0.7),
);
for _ in 0..900 {
world.step(1.0 / 120.0);
}
let position = world.position(block).expect("block exists");
let up = world.orientation(block).expect("block exists") * Vec3::Y;
(
position,
up.dot(Vec3::Y).clamp(-1.0, 1.0).acos().to_degrees(),
)
}
#[test]
fn a_box_leaves_a_platform_corner_once_its_centre_passes_it() {
let resting = 0.5 + 0.25;
for centre in [1.0_f32, 2.0, 2.5, 2.9, 2.99] {
let (position, tilt) = corner_slide(centre);
assert!(
(position.y - resting).abs() < 0.02,
"centre {centre} is over the platform and should rest, got y={}",
position.y
);
assert!(
tilt < 1.0,
"centre {centre} should stay level, tilted {tilt} deg"
);
}
for centre in [3.2_f32, 3.4, 4.0] {
let (position, _) = corner_slide(centre);
assert!(
position.y < 0.0,
"centre {centre} is past the platform corner and must fall, got y={}",
position.y
);
}
}
#[test]
fn a_stack_pushed_over_a_ledge_leaves_no_body_hanging() {
for friction in [0.05_f32, 0.8] {
let mut world = World::new();
world.add_body(
BodyDesc::stationary(Shape::cuboid(Vec3::new(4.0, 0.5, 3.0)))
.at(Vec3::ZERO)
.with_friction(friction),
);
let mut stack = alloc::vec::Vec::new();
for level in 0..4 {
stack.push(
world.add_body(
BodyDesc::dynamic(Shape::cuboid(Vec3::splat(0.5)), 1.0)
.at(Vec3::new(2.0, 0.5 + level as f32 * 1.01, 0.0))
.with_friction(friction)
.with_velocity(Vec3::new(2.0, 0.0, 0.0)),
),
);
}
settle(&mut world, 10.0);
let mut left_the_platform = 0;
for (level, body) in stack.iter().enumerate() {
let position = world.position(*body).expect("body exists");
if position.y < 0.0 {
left_the_platform += 1;
}
assert!(
position.y < 0.0 || position.x.abs() <= 4.0,
"friction {friction}: level {level} is hanging past the ledge at {position:?}"
);
}
assert!(
left_the_platform > 0,
"friction {friction}: the stack never reached the ledge, so nothing was tested"
);
}
}
struct SlabOutcome {
slab_y: f32,
slab_tilt: f32,
box_y: f32,
box_slept: bool,
}
fn loaded_slab(box_x: f32, box_mass: f32, friction: f32) -> SlabOutcome {
let mut world = World::new();
world.add_body(
BodyDesc::stationary(Shape::cuboid(Vec3::new(2.0, 0.5, 3.0)))
.at(Vec3::new(-2.0, -0.5, 0.0))
.with_friction(friction),
);
let slab = world.add_body(
BodyDesc::dynamic(Shape::cuboid(Vec3::new(2.0, 0.1, 1.0)), 1.0)
.at(Vec3::new(-0.5, 0.1 + 0.02, 0.0))
.with_friction(friction),
);
let box_body = world.add_body(
BodyDesc::dynamic(Shape::cuboid(Vec3::splat(0.4)), box_mass)
.at(Vec3::new(box_x, 0.2 + 0.4 + 0.04, 0.0))
.with_friction(friction),
);
settle(&mut world, 10.0);
let slab_position = world.position(slab).expect("slab exists");
let box_position = world.position(box_body).expect("box exists");
SlabOutcome {
slab_y: slab_position.y,
slab_tilt: {
let up = world.orientation(slab).expect("slab exists") * Vec3::Y;
up.dot(Vec3::Y).clamp(-1.0, 1.0).acos().to_degrees()
},
box_y: box_position.y,
box_slept: world.is_sleeping(box_body) == Some(true),
}
}
#[test]
fn a_loaded_dynamic_slab_tips_by_its_combined_centre_of_mass() {
let combined = |box_x: f32, mass: f32| (1.0 * -0.5 + mass * box_x) / (1.0 + mass);
let stable = [
(-0.5_f32, 1.0_f32),
(0.0, 1.0),
(0.4, 1.0),
(0.0, 3.0),
(0.1, 3.0),
];
for (box_x, mass) in stable {
let centre = combined(box_x, mass);
assert!(centre < 0.0, "the stable case must be inside the ledge");
let outcome = loaded_slab(box_x, mass, 0.6);
assert!(
outcome.slab_y > -0.5,
"combined centre {centre} is over the ledge, so the slab should stand; got slab y={}",
outcome.slab_y
);
assert!(
outcome.box_y > 0.0,
"the box rode a standing slab, so it should still be up; got y={}",
outcome.box_y
);
assert!(
outcome.slab_tilt < 2.0,
"a standing slab should stay level, tilted {} deg",
outcome.slab_tilt
);
if centre < -0.2 {
assert!(
outcome.box_slept,
"a box riding a settled slab should sleep, centre was {centre}"
);
}
}
let tipping = [(0.6_f32, 1.0_f32), (1.0, 1.0), (1.4, 1.0), (0.3, 3.0)];
for (box_x, mass) in tipping {
let centre = combined(box_x, mass);
assert!(centre > 0.0, "the tipping case must be past the ledge");
let outcome = loaded_slab(box_x, mass, 0.6);
assert!(
outcome.slab_y < -1.0,
"combined centre {centre} is past the ledge, so the slab should go over; got slab y={}",
outcome.slab_y
);
assert!(
outcome.slab_tilt > 5.0,
"the slab should have rotated rather than slid off, tilted only {} deg",
outcome.slab_tilt
);
assert!(
outcome.box_y < 0.0,
"the box should have followed the slab down, got y={}",
outcome.box_y
);
}
}
#[test]
fn gravity_scale_decides_how_much_gravity_reaches_a_body() {
let mut world = World::new();
let normal =
world.add_body(BodyDesc::dynamic(Shape::sphere(0.5), 1.0).at(Vec3::new(0.0, 10.0, 0.0)));
let weightless = world.add_body(
BodyDesc::dynamic(Shape::sphere(0.5), 1.0)
.at(Vec3::new(10.0, 10.0, 0.0))
.with_gravity_scale(0.0),
);
let heavy = world.add_body(
BodyDesc::dynamic(Shape::sphere(0.5), 1.0)
.at(Vec3::new(20.0, 10.0, 0.0))
.with_gravity_scale(2.0),
);
world.step(1.0 / 120.0);
let vertical = |body| world.velocity(body).expect("body exists").y;
let one_step = DEFAULT_GRAVITY.y / 120.0;
assert!(
vertical(weightless).abs() < 1e-6,
"a zero scale must ignore gravity entirely, got {}",
vertical(weightless)
);
assert!(
(vertical(normal) - one_step).abs() < 1e-4,
"the default must be full gravity, got {}",
vertical(normal)
);
assert!(
(vertical(heavy) - 2.0 * one_step).abs() < 1e-4,
"the scale must multiply, got {}",
vertical(heavy)
);
}
#[test]
fn gravity_scale_can_be_changed_on_a_live_body() {
let mut world = World::new();
let body =
world.add_body(BodyDesc::dynamic(Shape::sphere(0.5), 1.0).at(Vec3::new(0.0, 10.0, 0.0)));
assert!(world.set_gravity_scale(body, 0.0));
for _ in 0..600 {
world.step(1.0 / 120.0);
}
let held = world.position(body).expect("body exists").y;
assert!(
(held - 10.0).abs() < 1e-3,
"a zero scale must hover, got y={held}"
);
assert_eq!(world.is_sleeping(body), Some(true), "it should have slept");
assert!(world.set_gravity_scale(body, 1.0));
assert_eq!(
world.is_sleeping(body),
Some(false),
"changing the scale must wake the Body"
);
world.step(1.0 / 120.0);
assert!(
world.velocity(body).expect("body exists").y < 0.0,
"gravity must reach it again"
);
let dead = world.add_body(BodyDesc::dynamic(Shape::sphere(0.5), 1.0));
assert!(world.remove_body(dead));
assert!(
!world.set_gravity_scale(dead, 1.0),
"a dead handle must be rejected"
);
}
#[test]
fn a_negative_or_non_finite_gravity_scale_is_clamped_to_zero() {
let mut world = World::new();
let negative = world.add_body(
BodyDesc::dynamic(Shape::sphere(0.5), 1.0)
.at(Vec3::new(0.0, 10.0, 0.0))
.with_gravity_scale(-1.0),
);
let nan = world.add_body(
BodyDesc::dynamic(Shape::sphere(0.5), 1.0)
.at(Vec3::new(10.0, 10.0, 0.0))
.with_gravity_scale(f32::NAN),
);
world.step(1.0 / 120.0);
for body in [negative, nan] {
let velocity = world.velocity(body).expect("body exists");
assert!(
velocity.is_finite() && velocity.y.abs() < 1e-6,
"expected no gravity, got {velocity:?}"
);
}
}
#[test]
fn disturbing_one_member_wakes_the_whole_island() {
let mut world = World::new();
ground(&mut world, 0.0);
let mut cubes = alloc::vec::Vec::new();
for level in 0..4 {
cubes.push(
world.add_body(
BodyDesc::dynamic(Shape::cuboid(Vec3::splat(0.5)), 1.0)
.at(Vec3::new(0.0, 0.5 + level as f32 * 1.01, 0.0))
.with_friction(0.9),
),
);
}
settle(&mut world, 8.0);
for cube in &cubes {
assert_eq!(
world.is_sleeping(*cube),
Some(true),
"the stack should be asleep before it is disturbed"
);
}
assert!(world.apply_impulse(cubes[0], Vec3::new(3.0, 0.0, 0.0)));
world.step(1.0 / 120.0);
for (index, cube) in cubes.iter().enumerate() {
assert_eq!(
world.is_sleeping(*cube),
Some(false),
"level {index} of the Island should have woken"
);
}
}
#[test]
fn an_island_sleeps_as_a_unit() {
let mut world = World::new();
ground(&mut world, 0.0);
let mut cubes = alloc::vec::Vec::new();
for level in 0..5 {
cubes.push(
world.add_body(
BodyDesc::dynamic(Shape::cuboid(Vec3::splat(0.5)), 1.0)
.at(Vec3::new(0.0, 0.5 + level as f32 * 1.01, 0.0))
.with_friction(0.9),
),
);
}
let mut slept = false;
for step in 0..1200 {
world.step(1.0 / 120.0);
let asleep = cubes
.iter()
.filter(|cube| world.is_sleeping(**cube) == Some(true))
.count();
assert!(
asleep == 0 || asleep == cubes.len(),
"the Island must sleep as a unit, {asleep} of {} were asleep at step {step}",
cubes.len()
);
if asleep == cubes.len() {
slept = true;
break;
}
}
assert!(slept, "the stack never slept");
}
}