ferravex_dynamics/
lib.rs

1//! Original, dependency-free 2D/3D translational rigid-body physics.
2//!
3//! The same fixed-step solver handles circles/spheres and **axis-aligned** boxes.
4//! This foundation deliberately does not claim a full rotational rigid-body or
5//! general-purpose dynamics API. See the crate README for numerical and feature limits.
6//!
7//! ```
8//! use ferravex_dynamics::{BodyDesc, PhysicsWorld, Shape};
9//! let mut world = PhysicsWorld::<3>::new([0., -9.81, 0.]);
10//! world.insert(BodyDesc::fixed(Shape::cuboid([5., 0.5, 5.]), [0., -0.5, 0.])).unwrap();
11//! let ball = world.insert(BodyDesc::dynamic(Shape::ball(0.5), [0., 3., 0.], 1.)).unwrap();
12//! for _ in 0..240 { world.step(1. / 120.).unwrap(); }
13//! assert!((world.body(ball).unwrap().position()[1] - 0.5).abs() < 0.02);
14//! ```
15mod character;
16mod collision;
17mod math;
18mod world;
19pub use character::{CharacterMotion, CharacterSettings};
20use math::*;
21pub use world::{PhysicsWorld, StepStats};
22
23/// Non-reusable identity: removing a body invalidates its handle immediately.
24#[derive(Clone, Copy, Debug, PartialEq, Eq, PartialOrd, Ord, Hash)]
25pub struct BodyHandle {
26    pub(crate) index: u32,
27    pub(crate) generation: u32,
28}
29impl BodyHandle {
30    pub fn index(self) -> u32 {
31        self.index
32    }
33    pub fn generation(self) -> u32 {
34        self.generation
35    }
36}
37
38#[derive(Clone, Copy, Debug, PartialEq)]
39pub enum Shape<const D: usize> {
40    Ball { radius: f32 },
41    Box { half_extents: [f32; D] },
42}
43impl<const D: usize> Shape<D> {
44    pub const fn ball(radius: f32) -> Self {
45        Self::Ball { radius }
46    }
47    /// All sizes are half extents, not full dimensions.
48    pub const fn cuboid(half_extents: [f32; D]) -> Self {
49        Self::Box { half_extents }
50    }
51    pub(crate) fn valid(self) -> bool {
52        match self {
53            Self::Ball { radius } => radius.is_finite() && (1e-5..=1_000_000.).contains(&radius),
54            Self::Box { half_extents } => {
55                valid(half_extents) && half_extents.iter().all(|v| *v >= 1e-5)
56            }
57        }
58    }
59    pub(crate) fn half(self) -> [f32; D] {
60        match self {
61            Self::Ball { radius } => [radius; D],
62            Self::Box { half_extents } => half_extents,
63        }
64    }
65}
66
67#[derive(Clone, Copy, Debug, PartialEq)]
68pub enum BodyKind {
69    Static,
70    /// Kinematic bodies integrate their velocity but never receive solver impulses.
71    Kinematic,
72    Dynamic {
73        mass: f32,
74    },
75}
76#[derive(Clone, Copy, Debug, PartialEq, Eq)]
77pub struct CollisionLayers {
78    pub membership: u32,
79    pub filter: u32,
80}
81impl Default for CollisionLayers {
82    fn default() -> Self {
83        Self {
84            membership: 1,
85            filter: u32::MAX,
86        }
87    }
88}
89impl CollisionLayers {
90    pub const fn new(membership: u32, filter: u32) -> Self {
91        Self { membership, filter }
92    }
93    pub(crate) fn allows(self, other: Self) -> bool {
94        self.membership & other.filter != 0 && other.membership & self.filter != 0
95    }
96}
97
98/// Validated when inserted. One primitive collider belongs to each body.
99#[derive(Clone, Copy, Debug)]
100pub struct BodyDesc<const D: usize> {
101    pub shape: Shape<D>,
102    pub position: [f32; D],
103    pub velocity: [f32; D],
104    pub kind: BodyKind,
105    pub restitution: f32,
106    pub friction: f32,
107    pub linear_damping: f32,
108    pub gravity_scale: f32,
109    pub sensor: bool,
110    pub layers: CollisionLayers,
111    /// Continuous ball collision against static, non-sensor terrain. Boxes use discrete collision.
112    pub ccd: bool,
113    pub user_data: u64,
114}
115impl<const D: usize> BodyDesc<D> {
116    pub fn fixed(shape: Shape<D>, position: [f32; D]) -> Self {
117        Self {
118            shape,
119            position,
120            velocity: [0.; D],
121            kind: BodyKind::Static,
122            restitution: 0.,
123            friction: 0.6,
124            linear_damping: 0.,
125            gravity_scale: 1.,
126            sensor: false,
127            layers: CollisionLayers::default(),
128            ccd: false,
129            user_data: 0,
130        }
131    }
132    pub fn dynamic(shape: Shape<D>, position: [f32; D], mass: f32) -> Self {
133        Self {
134            kind: BodyKind::Dynamic { mass },
135            ..Self::fixed(shape, position)
136        }
137    }
138    pub fn kinematic(shape: Shape<D>, position: [f32; D]) -> Self {
139        Self {
140            kind: BodyKind::Kinematic,
141            ..Self::fixed(shape, position)
142        }
143    }
144    pub(crate) fn valid(self) -> bool {
145        self.shape.valid()
146            && valid(self.position)
147            && valid(self.velocity)
148            && self.restitution.is_finite()
149            && (0. ..=1.).contains(&self.restitution)
150            && self.friction.is_finite()
151            && (0. ..=10.).contains(&self.friction)
152            && self.linear_damping.is_finite()
153            && (0. ..=10_000.).contains(&self.linear_damping)
154            && self.gravity_scale.is_finite()
155            && self.gravity_scale.abs() <= 1_000.
156            && match self.kind {
157                BodyKind::Dynamic { mass } => mass.is_finite() && (1e-6..=1e8).contains(&mass),
158                _ => true,
159            }
160    }
161}
162
163#[derive(Clone, Debug)]
164pub struct Body<const D: usize> {
165    pub(crate) desc: BodyDesc<D>,
166    pub(crate) force: [f32; D],
167}
168impl<const D: usize> Body<D> {
169    pub fn position(&self) -> [f32; D] {
170        self.desc.position
171    }
172    pub fn velocity(&self) -> [f32; D] {
173        self.desc.velocity
174    }
175    pub fn shape(&self) -> Shape<D> {
176        self.desc.shape
177    }
178    pub fn kind(&self) -> BodyKind {
179        self.desc.kind
180    }
181    pub fn is_sensor(&self) -> bool {
182        self.desc.sensor
183    }
184    pub fn user_data(&self) -> u64 {
185        self.desc.user_data
186    }
187    pub fn layers(&self) -> CollisionLayers {
188        self.desc.layers
189    }
190    pub fn description(&self) -> BodyDesc<D> {
191        self.desc
192    }
193    pub fn set_position(&mut self, position: [f32; D]) -> Result<(), PhysicsError> {
194        if !valid(position) {
195            return Err(PhysicsError::InvalidBody);
196        }
197        self.desc.position = position;
198        Ok(())
199    }
200    pub fn set_velocity(&mut self, velocity: [f32; D]) -> Result<(), PhysicsError> {
201        if !valid(velocity) || self.desc.kind == BodyKind::Static && velocity != [0.; D] {
202            return Err(PhysicsError::InvalidBody);
203        }
204        self.desc.velocity = velocity;
205        Ok(())
206    }
207    pub fn set_layers(&mut self, layers: CollisionLayers) {
208        self.desc.layers = layers;
209    }
210    pub fn set_user_data(&mut self, data: u64) {
211        self.desc.user_data = data;
212    }
213    pub fn clear_forces(&mut self) {
214        self.force = [0.; D];
215    }
216    pub fn apply_force(&mut self, force: [f32; D]) -> Result<(), PhysicsError> {
217        if !valid(force) {
218            return Err(PhysicsError::InvalidBody);
219        }
220        if self.inv_mass() == 0. {
221            return Ok(());
222        }
223        let sum = add(self.force, force);
224        if !valid(sum) {
225            return Err(PhysicsError::InvalidBody);
226        }
227        self.force = sum;
228        Ok(())
229    }
230    pub fn apply_impulse(&mut self, impulse: [f32; D]) -> Result<(), PhysicsError> {
231        if !valid(impulse) {
232            return Err(PhysicsError::InvalidBody);
233        }
234        if self.inv_mass() == 0. {
235            return Ok(());
236        }
237        self.set_velocity(add(self.desc.velocity, mul(impulse, self.inv_mass())))
238    }
239    pub(crate) fn inv_mass(&self) -> f32 {
240        match self.desc.kind {
241            BodyKind::Dynamic { mass } => mass.recip(),
242            _ => 0.,
243        }
244    }
245}
246
247#[derive(Clone, Copy, Debug, PartialEq, Eq)]
248pub enum PhysicsError {
249    InvalidDimension,
250    InvalidConfiguration,
251    InvalidBody,
252    InvalidTimestep,
253    BodyCapacity,
254    PairCapacity,
255    NumericalLimit,
256}
257impl std::fmt::Display for PhysicsError {
258    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
259        f.write_str(match self {
260            Self::InvalidDimension => "physics supports only dimension 2 or 3",
261            Self::InvalidConfiguration => "invalid physics world configuration",
262            Self::InvalidBody => "body contains invalid or out-of-range parameters",
263            Self::InvalidTimestep => "physics timestep must be finite and within (0, 0.1] seconds",
264            Self::BodyCapacity => "physics body capacity reached",
265            Self::PairCapacity => "broadphase pair capacity reached; step rolled back",
266            Self::NumericalLimit => "physics numerical range exceeded; step rolled back",
267        })
268    }
269}
270impl std::error::Error for PhysicsError {}
271
272#[derive(Clone, Copy, Debug)]
273pub struct WorldConfig<const D: usize> {
274    pub gravity: [f32; D],
275    pub max_bodies: usize,
276    pub max_pairs: usize,
277    /// Bound work even when many AABBs overlap along the sweep axis only.
278    pub max_pair_tests: usize,
279    pub solver_iterations: u32,
280    pub contact_slop: f32,
281    /// At most this many terrain impacts are processed per CCD body per step.
282    pub ccd_iterations: u32,
283}
284impl<const D: usize> WorldConfig<D> {
285    pub fn new(gravity: [f32; D]) -> Self {
286        Self {
287            gravity,
288            max_bodies: 4096,
289            max_pairs: 65_536,
290            max_pair_tests: 2_000_000,
291            solver_iterations: 12,
292            contact_slop: 0.001,
293            ccd_iterations: 4,
294        }
295    }
296}
297
298#[derive(Clone, Copy, Debug, PartialEq)]
299pub struct Contact<const D: usize> {
300    pub a: BodyHandle,
301    pub b: BodyHandle,
302    /// Unit vector from A toward B.
303    pub normal: [f32; D],
304    pub penetration: f32,
305    pub sensor: bool,
306}
307#[derive(Clone, Copy, Debug, PartialEq, Eq)]
308pub enum ContactEventKind {
309    Started,
310    Stopped,
311}
312#[derive(Clone, Copy, Debug, PartialEq)]
313pub struct ContactEvent<const D: usize> {
314    pub kind: ContactEventKind,
315    pub contact: Contact<D>,
316}
317
318#[derive(Clone, Copy, Debug)]
319pub struct QueryFilter {
320    pub exclude: Option<BodyHandle>,
321    pub layers: CollisionLayers,
322    pub include_sensors: bool,
323    pub only_static: bool,
324}
325impl Default for QueryFilter {
326    fn default() -> Self {
327        Self {
328            exclude: None,
329            layers: CollisionLayers {
330                membership: u32::MAX,
331                filter: u32::MAX,
332            },
333            include_sensors: false,
334            only_static: false,
335        }
336    }
337}
338#[derive(Clone, Copy, Debug, PartialEq)]
339pub struct RayHit<const D: usize> {
340    pub body: BodyHandle,
341    pub distance: f32,
342    pub point: [f32; D],
343    /// Unit surface normal pointing out of the hit collider.
344    pub normal: [f32; D],
345}
346
347pub mod d2 {
348    pub type PhysicsWorld = crate::PhysicsWorld<2>;
349    pub type BodyDesc = crate::BodyDesc<2>;
350    pub type Shape = crate::Shape<2>;
351    pub type Body = crate::Body<2>;
352}
353pub mod d3 {
354    pub type PhysicsWorld = crate::PhysicsWorld<3>;
355    pub type BodyDesc = crate::BodyDesc<3>;
356    pub type Shape = crate::Shape<3>;
357    pub type Body = crate::Body<3>;
358}
ferravex_dynamics/lib.rs

ferravex_dynamics/
lib.rs
