mod geometry;
mod grid;
mod solver;
use crate::collision::{Aabb2, Aabb3};
use crate::events::Events;
use crate::time::Tick;
use geometry::Shape;
use glam::{Vec2, Vec3};
use std::collections::{BTreeMap, BTreeSet};
#[doc = include_str!("../docs/physics.md")]
pub mod guide {}
#[derive(Clone, Copy, Debug, PartialEq, Eq, PartialOrd, Ord, Hash)]
pub struct BodyId(pub u64);
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum BodyKind {
Static,
Kinematic,
Dynamic,
}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub struct CollisionFilter {
pub layers: u32,
pub mask: u32,
}
impl Default for CollisionFilter {
fn default() -> Self {
Self {
layers: 1,
mask: u32::MAX,
}
}
}
impl CollisionFilter {
pub fn allows(self, other: Self) -> bool {
self.mask & other.layers != 0 && other.mask & self.layers != 0
}
}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum TriggerPhase {
Enter,
Stay,
Exit,
}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub struct TriggerEvent {
pub trigger: BodyId,
pub other: BodyId,
pub phase: TriggerPhase,
}
#[derive(Clone, Copy, Debug, Default, PartialEq)]
pub struct StepReport {
pub contacts: usize,
pub dropped_time: f32,
pub unresolved_overlaps: bool,
}
#[derive(Clone, Copy)]
pub(super) struct Contact<const N: usize> {
normal: [f32; N],
depth: f32,
}
macro_rules! dimension {
($n:literal, $vec:ident, $aabb:ident, $shape:ident, $round_variant:ident, $body:ident, $world:ident, $grid:ident, $penetration:ident) => {
#[derive(Clone, Copy, Debug, PartialEq)]
pub enum $shape {
Box {
half_size: $vec,
},
$round_variant {
radius: f32,
},
}
impl $shape {
pub fn box_shape(size: $vec) -> Self {
let shape = Self::Box {
half_size: size * 0.5,
};
assert!(shape.internal().valid());
shape
}
pub fn round(radius: f32) -> Self {
assert!(radius.is_finite() && radius > 0.0);
Self::$round_variant { radius }
}
fn internal(self) -> Shape<$n> {
match self {
Self::Box { half_size } => Shape::Box(half_size.to_array()),
Self::$round_variant { radius } => Shape::Round(radius),
}
}
pub fn bounds(self, center: $vec) -> $aabb {
assert!(center.is_finite() && self.internal().valid());
let (min, max) = self.internal().bounds(center.to_array());
$aabb {
min: $vec::from_array(min),
max: $vec::from_array(max),
}
}
pub fn overlap(
self,
center: $vec,
other: Self,
other_center: $vec,
) -> Option<$penetration> {
assert!(
center.is_finite()
&& other_center.is_finite()
&& self.internal().valid()
&& other.internal().valid()
);
geometry::overlap(
self.internal(),
center.to_array(),
other.internal(),
other_center.to_array(),
)
.map(|c| $penetration {
normal: $vec::from_array(c.normal),
depth: c.depth,
})
}
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct $penetration {
/// Unit direction from the second shape toward the first.
pub normal: $vec,
pub depth: f32,
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct $body {
/// World-space center.
pub position: $vec,
pub shape: $shape,
pub velocity: $vec,
pub kind: BodyKind,
pub filter: CollisionFilter,
pub is_trigger: bool,
pub mass: f32,
pub gravity: Option<$vec>,
pub restitution: f32,
pub friction: f32,
pub drag: f32,
pub max_speed: Option<f32>,
pub grounded: bool,
}
impl $body {
pub fn new(position: $vec, shape: $shape) -> Self {
assert!(position.is_finite() && shape.internal().valid());
Self {
position,
shape,
velocity: $vec::ZERO,
kind: BodyKind::Dynamic,
filter: CollisionFilter::default(),
is_trigger: false,
mass: 1.0,
gravity: None,
restitution: 0.0,
friction: 0.0,
drag: 0.0,
max_speed: None,
grounded: false,
}
}
pub fn bounds(&self) -> $aabb {
self.shape.bounds(self.position)
}
}
#[derive(Debug)]
pub struct $grid {
inner: grid::Grid<$n>,
}
impl $grid {
pub fn new(cell_size: f32) -> Self {
Self {
inner: grid::Grid::new(cell_size),
}
}
pub fn insert(&mut self, id: u64, bounds: $aabb) {
self.inner
.insert(id, bounds.min.to_array(), bounds.max.to_array());
}
pub fn remove(&mut self, id: u64) {
self.inner.remove(id);
}
pub fn clear(&mut self) {
self.inner.clear();
}
pub fn query(&self, bounds: $aabb) -> Vec<u64> {
self.inner
.query(bounds.min.to_array(), bounds.max.to_array())
}
}
#[derive(Debug)]
pub struct $world {
bodies: BTreeMap<BodyId, $body>,
next_id: u64,
overlaps: BTreeSet<(BodyId, BodyId)>,
grid: grid::Grid<$n>,
}
impl $world {
pub fn new(cell_size: f32) -> Self {
Self {
bodies: BTreeMap::new(),
next_id: 0,
overlaps: BTreeSet::new(),
grid: grid::Grid::new(cell_size),
}
}
pub fn insert(&mut self, body: $body) -> BodyId {
let id = BodyId(self.next_id);
self.next_id = self
.next_id
.checked_add(1)
.expect("physics identity exhausted");
self.bodies.insert(id, body);
id
}
pub fn remove(&mut self, id: BodyId) -> Option<$body> {
self.bodies.remove(&id)
}
pub fn body(&self, id: BodyId) -> Option<&$body> {
self.bodies.get(&id)
}
pub fn body_mut(&mut self, id: BodyId) -> Option<&mut $body> {
self.bodies.get_mut(&id)
}
pub fn iter(&self) -> impl Iterator<Item = (BodyId, &$body)> {
self.bodies.iter().map(|(id, b)| (*id, b))
}
pub fn step(&mut self, tick: Tick, events: &mut Events<TriggerEvent>) -> StepReport {
let mut nodes: Vec<_> = self
.bodies
.iter()
.map(|(id, b)| solver::Node {
id: *id,
position: b.position.to_array(),
shape: b.shape.internal(),
velocity: b.velocity.to_array(),
kind: b.kind,
filter: b.filter,
trigger: b.is_trigger,
mass: b.mass,
gravity: b.gravity.map(|g| g.to_array()),
restitution: b.restitution,
friction: b.friction,
drag: b.drag,
max_speed: b.max_speed,
grounded: false,
was_grounded: b.grounded,
touched_ground: false,
carry: [0.0; $n],
})
.collect();
let report = solver::step(
&mut nodes,
tick.dt,
&mut self.grid,
&mut self.overlaps,
events,
);
for node in nodes {
let b = self.bodies.get_mut(&node.id).unwrap();
b.position = $vec::from_array(node.position);
b.velocity = $vec::from_array(node.velocity);
b.grounded = node.grounded;
}
report
}
}
};
}
dimension!(
2,
Vec2,
Aabb2,
Shape2D,
Circle,
PhysicsBody2D,
PhysicsWorld2D,
UniformGrid2D,
Penetration2D
);
dimension!(
3,
Vec3,
Aabb3,
Shape3D,
Sphere,
PhysicsBody3D,
PhysicsWorld3D,
UniformGrid3D,
Penetration3D
);
#[cfg(test)]
mod tests;
pub(crate) fn depenetrate_box<const N: usize>(
position: &mut [f32; N],
half: [f32; N],
solids: impl Iterator<Item = ([f32; N], [f32; N])> + Clone,
) {
for _ in 0..16 {
let mut changed = false;
for (min, max) in solids.clone() {
let center = std::array::from_fn(|i| (min[i] + max[i]) * 0.5);
let extent = std::array::from_fn(|i| (max[i] - min[i]) * 0.5);
if let Some(contact) =
geometry::overlap(Shape::Box(half), *position, Shape::Box(extent), center)
{
*position = geometry::outward(*position, contact.normal, contact.depth + 1e-5);
changed = true;
}
}
if !changed {
break;
}
}
}