//! The contract a law signs to take part in [`crate::solver::PhysicsWorld`]'s
//! substep loop: [`Participant`] and the small types around it.
//!
//! A participant owns its own state (a particle set, a grid, a pedestrian
//! crowd, a force field) and advances it once per world substep. It reads the
//! rigid bodies as they were at the start of the substep and hands its effect
//! on them back as forces and torques in a [`ForceAccumulator`]; it never
//! writes a body's position or velocity. Snapshots carry its state as an
//! opaque payload tagged with its [`ParticipantKind`].
//!
//! **Unstable.** The types and rules of this module may still change before
//! the participant API is declared stable, and such changes may ship in a
//! minor release.
//!
//! # Contract
//!
//! | rule | how it is enforced |
//! |---|---|
//! | participants run in registration order, once per substep (before the rigid integration of that substep) | world side |
//! | [`Participant::substep`] returning `Err` leaves the participant unchanged | participant side, checked by the conformance tests |
//! | forces a participant staged in a substep that returned `Err` never reach the bodies | world side (staged per participant, merged on `Ok`) |
//! | a participant cannot write a body | type: [`SubstepCtx`] only hands out `&[RigidBody]` and the accumulator |
//! | [`Participant::check_state`] accepts every payload [`Participant::write_state`] produced, and [`Participant::read_state`] then restores the participant bit for bit | participant side, checked by the conformance tests |
//! | a restore checks the target world holds the same number of participants, of the same kinds, in the same order | world side, [`ParticipantMismatch::classify`] |
//! | participants run in the order [`execution_order`] gives for their [`Participant::ports`]: a writer of a port before its readers, registration order otherwise; with no ports declared that is registration order | world side calls [`execution_order`] at registration |
//! | a loop of ports (a cycle) is refused at registration, never ordered silently | type: [`execution_order`] returns [`OrderError::Cycle`], the world returns it as [`RegisterError::Order`] |
//! | the value behind a port that is not a shared field is state of the participant that holds it, so it is in that participant's [`Participant::write_state`] payload | participant side |
//! | the summed force and torque reach a body through `v += F·h·inv_mass`, `ω += I⁻¹·τ·h`; a product out of range is a fault ([`WorldFault::ForceOutOfRange`]), never clamped | world side |
//! | the rigid integration going out of range ([`WorldFault::RigidOverflow`]) is a fault like a participant's | world side |
//! | a parked (sleeping) dynamic body is woken (and unparked) by participant forces whenever the change the world applies for the forces and torques of all participants, `Δv = F·inv_mass·h` or `Δω = I⁻¹τ·h`, is non-zero in any component; there is no threshold, so a participant force never leaves a sleeping body unchanged while the participant books its reaction; when both changes round to exactly zero the body stays asleep ([`wakes_parked_body`]; this replaces the earlier rule, which left a body asleep below the sleep thresholds) | world side calls [`wakes_parked_body`] |
//! | a shared field ([`FieldBoard`]) is owned by the world alone; a participant reads the value committed before the substep began and writes only into its own [`FieldStage`] | type: [`SubstepCtx::field`] hands out `&[Fix128]` of the board, [`SubstepCtx::stage_field`] a buffer of the stage |
//! | staged field writes are committed at the end of the substep, in [`execution_order`], all fields at once; a commit that would leave the range of [`Fix128`] changes no field and is a fault ([`WorldFault::FieldOutOfRange`]) | [`run_substep`] |
//! | a participant that returned `Err` has its staged field writes dropped with its forces | [`run_substep`] |
//! | a [`FieldMode::Replace`] field has at most one writer, a plain [`PortAccess::Read`] of a field and a [`PortAccess::ReadCommitted`] of a port that is not a field are refused at registration | [`check_field_ports`], returned as [`RegisterError::Field`] |
//! | moving an extensive amount between layouts keeps its total exactly | [`remap_conserving`], [`deposit_bodies`] |
//! | a snapshot holds the committed field values once, in the world's section, never in a participant payload | world side, bytes from [`FieldBoard::write_values`] |
//! | participants can be registered only in builds with the `std` feature; a `no_std` world has no participants (the world-side participant API requires `std`) | world side |
//!
//! # Participant kinds
//!
//! A participant type in this crate takes as its [`ParticipantKind`] four
//! ASCII bytes read big endian (`u32::from_be_bytes(*b"THRM")`), declared as
//! a `pub const` next to the type. An assigned value never changes: a restore
//! matches the participants of a snapshot to those of the world by kind
//! value, so changing one makes every earlier snapshot refuse to load. Values
//! below `0x0100_0000` (small integers) are left for tests and user code and
//! are never assigned here.
//!
//! | code | value | type |
//! |---|---|---|
//! | `THRM` | `0x5448_524d` | `thermal::ThermalModifier` |
//! | `PHAS` | `0x5048_4153` | `phase_change::PhaseChangeModifier` |
//! | `PRES` | `0x5052_4553` | `pressure::PressureModifier` |
//! | `FRAC` | `0x4652_4143` | `fracture::FractureModifier` |
//! | `EROS` | `0x4552_4f53` | `erosion::ErosionModifier` |
//! | `CRWD` | `0x4352_5744` | `crowd_force::CrowdParticipant` |
//! | `MDVV` | `0x4d44_5656` | `molecular_dynamics::MdParticipant` |
//! | `MEDM` | `0x4d45_444d` | `coupling_medium::DragMedium` |
//!
//! # Coupling through ports
//!
//! A participant declares the values it reads and writes as [`Port`]s
//! ([`Participant::ports`], empty by default). A port is named by a
//! [`PortId`], which, like [`ParticipantKind`], keeps its value across
//! snapshots and versions. Within a substep, a participant that reads a port
//! runs after every participant that writes it, so it sees the value of this
//! substep. Participants that do not depend on each other keep their
//! registration order. A participant that reads and writes the same port has
//! no ordering constraint with itself.
//!
//! When the ports form a loop (A writes what B reads and B writes what A
//! reads), no order lets every read see the value of this substep. The v1
//! rule for such a loop is that its reads see the previous substep's values
//! (explicit coupling); [`execution_order`] does not pick an order for it
//! and returns [`OrderError::Cycle`] instead, so a loop is never coupled
//! without being declared as one. How a participant declares a lagged read is
//! not part of this version ([`PortAccess`] is `#[non_exhaustive]`). Implicit
//! iteration between coupled participants is not part of this module either.
//!
//! # Shared fields
//!
//! A port whose value several participants exchange (a temperature on a grid,
//! a heat source per body) is a *field* on the world's [`FieldBoard`]. A field
//! is named by the same [`PortId`]: the id is what participants declare in
//! [`Participant::ports`], so the execution order, the registration checks and
//! the snapshot all speak of one name, and a port is either held by its
//! participant (no field with that id) or held by the world (a field). The
//! board, not a participant, owns the value, so a snapshot carries each field
//! once however many participants use it.
//!
//! * **Values.** A field is an array of [`Fix128`] on a [`FieldLayout`]: one
//! sample per rigid body ([`FieldLayout::PerBody`], the way to hand a body a
//! quantity other than a force: heat, charge, deposited mass) or a uniform
//! grid of cubic cells ([`FieldLayout::Grid`]). A vector quantity is three
//! fields, one per component, so every rule below (sums, conservation,
//! snapshot) is the scalar one.
//! * **Reads.** [`SubstepCtx::field`] returns the value committed at the end of
//! the previous substep, whoever runs first. A field read is declared as
//! [`Port::reads_committed`], which adds no ordering constraint, so two
//! participants may each read what the other writes (thermal ⇄ structure)
//! without forming a refused loop. The coupling through a field is explicit:
//! one substep of lag, the same as through the rigid bodies.
//! * **Writes.** [`SubstepCtx::stage_field`] hands out the participant's own
//! staged buffer. Staged buffers are committed at the end of the substep in
//! [`execution_order`], all fields at once ([`run_substep`]); a participant
//! that returned `Err` has its staged writes dropped, as its forces are.
//! * **Several writers.** A [`FieldMode::Replace`] field (a state some
//! participant integrates, e.g. a temperature) has one writer, checked at
//! registration; its staged buffer starts as the committed value and
//! replaces it. A [`FieldMode::Sum`] field (a load or source, e.g. Joule
//! heat) takes any number of writers; each staged buffer starts at zero and
//! the committed value is the sum of this substep's buffers, partial sums
//! taken in execution order and checked, so the first writer that leaves the
//! range is named the same way on every run. Nothing written in a substep
//! gives zero, like the force accumulator.
//! * **Layouts.** Moving an extensive amount (heat in J per cell, not a
//! temperature) between nested grids or from bodies into a grid keeps the
//! total exactly ([`remap_conserving`], [`deposit_bodies`]); converting a
//! density to an amount is the participant's job.
//! * **Snapshot.** Proposed section of snapshot format version 2, after
//! `participants` and `fault`: `fields` = [`FieldBoard::write_values`]
//! (`count: u64`, then per field in ascending id: `id: u32`, `mode: u8`,
//! `layout: u8` and its parameters, the values as `hi: i64`, `lo: u64`
//! little endian). Staged buffers are empty between steps and are not
//! stored. A restore checks the layouts against the target world's board
//! ([`FieldBoard::check_values`]) before reading any value, next to the
//! participant checks. The `fault` section codes, with the field fault
//! added: `0` none, `1` [`WorldFault::Participant`] (`index: u64`,
//! `kind: u32`, fault tag `u8`), `2` [`WorldFault::RigidOverflow`], `3`
//! [`WorldFault::ForceOutOfRange`] (`body: u64`), `4`
//! [`WorldFault::FieldOutOfRange`] (`field: u32`, `index: u64`).
//!
//! A participant cannot write a body because [`SubstepCtx`] has no way to
//! reach one mutably:
//!
//! ```compile_fail
//! use alice_physics::world_participant::{ForceAccumulator, SubstepCtx};
//! use alice_physics::{Fix128, RigidBody, Vec3Fix};
//!
//! let bodies = [RigidBody::new_dynamic(Vec3Fix::ZERO, Fix128::ONE)];
//! let mut forces = ForceAccumulator::new(1);
//! let ctx = SubstepCtx::new(&bodies, &mut forces, 0, 1, Fix128::ONE).unwrap();
//! ctx.bodies()[0].velocity = Vec3Fix::ZERO; // `bodies()` is `&[RigidBody]`
//! ```
use crate::math::{Fix128, Vec3Fix};
use crate::sleeping::SleepConfig;
use crate::solver::RigidBody;
#[cfg(not(feature = "std"))]
use alloc::{boxed::Box, vec, vec::Vec};
/// Stable tag of a participant type, written in front of its snapshot
/// payload. A type keeps its tag across versions.
#[derive(Clone, Copy, Debug, PartialEq, Eq, PartialOrd, Ord, Hash)]
pub struct ParticipantKind(u32);
impl ParticipantKind {
/// The tag `raw`.
#[must_use]
pub const fn new(raw: u32) -> Self {
Self(raw)
}
/// The raw tag.
#[must_use]
pub const fn get(self) -> u32 {
self.0
}
}
/// Stable name of a value participants exchange (a field, a set of loads).
/// A port keeps its id across snapshots and versions.
#[derive(Clone, Copy, Debug, PartialEq, Eq, PartialOrd, Ord, Hash)]
pub struct PortId(u32);
impl PortId {
/// The port `raw`.
#[must_use]
pub const fn new(raw: u32) -> Self {
Self(raw)
}
/// The raw id.
#[must_use]
pub const fn get(self) -> u32 {
self.0
}
}
/// Whether a participant reads or writes a port.
#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash)]
#[non_exhaustive]
pub enum PortAccess {
/// Reads the value of the current substep (runs after every writer).
Read,
/// Writes the value (runs before every reader).
Write,
/// Reads a shared field's value as committed at the end of the previous
/// substep. Adds no ordering constraint; the port must be a field of the
/// world's [`FieldBoard`].
ReadCommitted,
}
/// One declared access of a participant to a port.
#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash)]
pub struct Port {
id: PortId,
access: PortAccess,
}
impl Port {
/// A read of `id`.
#[must_use]
pub const fn reads(id: PortId) -> Self {
Self {
id,
access: PortAccess::Read,
}
}
/// A read of the committed value of the shared field `id`.
#[must_use]
pub const fn reads_committed(id: PortId) -> Self {
Self {
id,
access: PortAccess::ReadCommitted,
}
}
/// A write of `id`.
#[must_use]
pub const fn writes(id: PortId) -> Self {
Self {
id,
access: PortAccess::Write,
}
}
/// The port.
#[must_use]
pub const fn id(self) -> PortId {
self.id
}
/// Read or write.
#[must_use]
pub const fn access(self) -> PortAccess {
self.access
}
}
/// Why the participants' ports admit no execution order.
#[derive(Clone, Debug, PartialEq, Eq)]
#[non_exhaustive]
pub enum OrderError {
/// The ports form a loop within one substep.
Cycle {
/// Registration indices of every participant on a loop, ascending.
/// Participants that only depend on a loop are not listed.
members: Vec<usize>,
},
}
/// The order participants run in within a substep, from their declared
/// ports: `ports[i]` is what [`Participant::ports`] returns for the
/// participant registered at index `i`, and the result lists registration
/// indices in run order.
///
/// A participant that writes a port runs before every other participant that
/// reads it. Among participants free to run, the one registered first runs
/// first, so with no ports at all the order is `0, 1, …, n-1`, and the result
/// depends only on `ports`. Two writers of one port are not ordered against
/// each other by that port.
///
/// # Errors
///
/// [`OrderError::Cycle`] when the dependencies form a loop; no order is
/// chosen.
pub fn execution_order(ports: &[&[Port]]) -> Result<Vec<usize>, OrderError> {
let n = ports.len();
// succ[w] = readers that must run after writer w
let mut succ: Vec<Vec<usize>> = vec![Vec::new(); n];
for (w, wp) in ports.iter().enumerate() {
for p in wp.iter().filter(|p| p.access == PortAccess::Write) {
for (r, rp) in ports.iter().enumerate() {
if r != w
&& rp
.iter()
.any(|q| q.access == PortAccess::Read && q.id == p.id)
{
succ[w].push(r);
}
}
}
}
let mut indegree = vec![0_usize; n];
for s in &mut succ {
s.sort_unstable();
s.dedup();
for &r in s.iter() {
indegree[r] += 1;
}
}
let mut done = vec![false; n];
let mut order = Vec::with_capacity(n);
while let Some(next) = (0..n).find(|&i| !done[i] && indegree[i] == 0) {
done[next] = true;
order.push(next);
for &r in &succ[next] {
indegree[r] -= 1;
}
}
if order.len() == n {
return Ok(order);
}
// A participant left over is on a loop iff it reaches itself through
// other left-over participants.
let members = (0..n)
.filter(|&v| !done[v] && reaches_itself(v, &succ, &done))
.collect();
Err(OrderError::Cycle { members })
}
fn reaches_itself(start: usize, succ: &[Vec<usize>], done: &[bool]) -> bool {
let mut seen = vec![false; succ.len()];
let mut stack: Vec<usize> = succ[start].clone();
while let Some(v) = stack.pop() {
if v == start {
return true;
}
if done[v] || seen[v] {
continue;
}
seen[v] = true;
stack.extend_from_slice(&succ[v]);
}
false
}
/// How a participant's own time step relates to the world substep `h`.
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
#[non_exhaustive]
pub enum StepRule {
/// One call per world substep, advancing by `h`.
FollowSubstep,
/// The participant advances by a fixed `Δt` of its own; registration is
/// refused unless `h = n·Δt` for an integer `n ≥ 1` (exactly, in
/// [`Fix128`]). The world still calls it once per substep with `h`; the
/// participant takes `n` internal steps.
Fixed(Fix128),
/// One call per world substep with `h`; the participant splits `h`
/// itself, deterministically from its own state (e.g. a CFL limit).
Subcycle,
}
impl StepRule {
/// Internal steps per world substep of width `h`, or why this rule cannot
/// follow `h`. `FollowSubstep` and `Subcycle` give 1 (the participant is
/// called once); `Fixed(Δt)` gives `n` with `h = n·Δt`.
///
/// # Errors
///
/// [`RegisterError::NonPositiveStep`] when `h ≤ 0` or `Δt ≤ 0`;
/// [`RegisterError::StepRuleMismatch`] when `h` is not an integer multiple
/// of `Δt`.
pub fn steps_per_substep(self, h: Fix128) -> Result<u64, RegisterError> {
if h <= Fix128::ZERO {
return Err(RegisterError::NonPositiveStep);
}
match self {
Self::FollowSubstep | Self::Subcycle => Ok(1),
Self::Fixed(step) => {
if step <= Fix128::ZERO {
return Err(RegisterError::NonPositiveStep);
}
let n = h / step;
let whole = n.lo == 0 && n.hi >= 1;
if whole && Fix128::from_int(n.hi) * step == h {
Ok(n.hi.unsigned_abs())
} else {
Err(RegisterError::StepRuleMismatch { substep: h, step })
}
}
}
}
}
/// Why a participant could not be registered.
#[derive(Clone, Debug, PartialEq, Eq)]
#[non_exhaustive]
pub enum RegisterError {
/// The world substep or the participant's own step is not positive.
NonPositiveStep,
/// `StepRule::Fixed(step)` does not divide the world substep.
StepRuleMismatch {
/// World substep `h`.
substep: Fix128,
/// The participant's fixed step.
step: Fix128,
},
/// With the new participant the declared ports form a loop
/// ([`execution_order`]); the participant was not registered.
Order(OrderError),
/// The declared ports do not fit the world's shared fields
/// ([`check_field_ports`]); the participant was not registered.
Field(FieldPortError),
}
/// Why a participant's substep failed. The participant is unchanged and its
/// forces of that substep are dropped; the world records the fault.
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
#[non_exhaustive]
pub enum ParticipantFault {
/// A value left the representable or valid range.
OutOfRange,
/// The participant found its own state inconsistent.
InvalidState,
}
impl ParticipantFault {
/// Snapshot tag of this fault.
#[must_use]
pub const fn tag(self) -> u8 {
match self {
Self::OutOfRange => 1,
Self::InvalidState => 2,
}
}
/// The fault with snapshot tag `tag`, if any.
#[must_use]
pub const fn from_tag(tag: u8) -> Option<Self> {
match tag {
1 => Some(Self::OutOfRange),
2 => Some(Self::InvalidState),
_ => None,
}
}
}
/// The first fault the world recorded (sticky until cleared).
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
#[non_exhaustive]
pub enum WorldFault {
/// A participant's substep returned `Err`.
Participant {
/// Registration index of the participant that failed.
index: usize,
/// Its kind.
kind: ParticipantKind,
/// What failed.
fault: ParticipantFault,
},
/// The rigid integration (position prediction, velocity from positions)
/// left the range of [`Fix128`]; the world's overflow flag in fault form.
/// It names no body because the flag does not.
///
/// The flag covers only part of the products today: the XPBD position
/// prediction and the velocity derived from jointed positions. The TGS
/// path does not raise it, and a wrapping addition of a position (XPBD or
/// TGS) is not detected.
RigidOverflow,
/// Applying the summed participant force or torque to body `body`
/// (`F·h·inv_mass`, `I⁻¹·τ·h`) left the range of [`Fix128`]. Nothing is
/// clamped.
ForceOutOfRange {
/// Index of the body.
body: usize,
},
/// Committing the staged writes of a [`FieldMode::Sum`] field left the
/// range of [`Fix128`] when the writes of participant `index` were added.
/// No field changed in that substep.
FieldOutOfRange {
/// The field.
field: PortId,
/// Registration index of the participant whose write left the range.
index: usize,
},
}
/// Why a checked world step did not run normally.
#[derive(Clone, Debug, PartialEq, Eq)]
#[non_exhaustive]
pub enum StepError {
/// A fault was already recorded; the world was not touched.
Faulted(WorldFault),
/// A participant failed during this step. The step ran to the end (the
/// failed participant was frozen at its state before the failing call and
/// its forces of that substep were dropped); the fault is now recorded.
FaultRaised(WorldFault),
/// The participant at `index` has a [`StepRule::Fixed`] step that does not
/// divide this frame's substep width, the one the running path hands its
/// participants (`dt / substeps`, or on `PhysicsWorld::try_step` under
/// `SolverBackend::Tgs` the width its solve uses); nothing ran. The step is
/// only known when the world steps, so the check runs at the start of
/// every step rather than at registration.
Rule {
/// Registration index of the participant.
index: usize,
/// Why its rule cannot follow the substep.
error: RegisterError,
},
/// The [`FieldLayout::PerBody`] field `field` has `samples` samples but
/// the world holds `bodies` bodies (a body was added or removed after the
/// field was declared): the step is refused and the world is left
/// unchanged, nothing ran. The world-side form of
/// [`ExchangeError::BodyCount`], checked at the start of every step like
/// [`Self::Rule`].
BodyCount {
/// The field.
field: PortId,
/// Samples of the field.
samples: usize,
/// Bodies in the world.
bodies: usize,
},
}
/// Why a participant rejected a snapshot payload.
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
#[non_exhaustive]
pub enum StateError {
/// The payload has a different length than the participant needs.
Length {
/// Bytes needed.
expected: usize,
/// Bytes given.
found: usize,
},
/// A value in the payload is not valid for the participant.
InvalidValue,
}
/// Why a restore refused the participants of the target world.
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
#[non_exhaustive]
pub enum ParticipantMismatch {
/// Different numbers of participants.
Count {
/// Participants in the snapshot.
snapshot: usize,
/// Participants in the target world.
world: usize,
},
/// The same number, but not the same kinds (as multisets).
Kind {
/// First index whose kind differs.
index: usize,
/// Kind in the snapshot.
snapshot: ParticipantKind,
/// Kind in the target world.
world: ParticipantKind,
},
/// The same kinds, in another order.
Order {
/// First index whose kind differs.
index: usize,
/// Kind in the snapshot.
snapshot: ParticipantKind,
/// Kind in the target world.
world: ParticipantKind,
},
}
impl ParticipantMismatch {
/// Compare the kinds stored in a snapshot with those registered in the
/// target world. `None` when they agree (same count, same kinds, same
/// order).
#[must_use]
pub fn classify(snapshot: &[ParticipantKind], world: &[ParticipantKind]) -> Option<Self> {
if snapshot.len() != world.len() {
return Some(Self::Count {
snapshot: snapshot.len(),
world: world.len(),
});
}
let index = snapshot.iter().zip(world).position(|(s, w)| s != w)?;
let (s, w) = (snapshot[index], world[index]);
let mut a: Vec<ParticipantKind> = snapshot.to_vec();
let mut b: Vec<ParticipantKind> = world.to_vec();
a.sort_unstable();
b.sort_unstable();
Some(if a == b {
Self::Order {
index,
snapshot: s,
world: w,
}
} else {
Self::Kind {
index,
snapshot: s,
world: w,
}
})
}
}
/// Why a force could not be staged.
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
#[non_exhaustive]
pub enum AccumulateError {
/// The body index is past the end of the body list.
BodyOutOfRange {
/// The index.
index: usize,
/// Number of bodies.
len: usize,
},
/// The accumulator does not have one slot per body.
LengthMismatch {
/// Slots in the accumulator.
slots: usize,
/// Bodies.
bodies: usize,
},
}
/// Per-body sums of force and torque (world frame, [`Fix128`]) for one
/// substep.
///
/// The sums use the wrapping addition of [`Fix128`], which is associative and
/// commutative, so the totals do not depend on the order the forces were
/// added in. The world still fixes the order (registration order, then each
/// participant's own order) so that any later checked accumulation stays
/// deterministic.
#[derive(Clone, Debug, PartialEq, Eq)]
pub struct ForceAccumulator {
force: Vec<Vec3Fix>,
torque: Vec<Vec3Fix>,
}
impl ForceAccumulator {
/// Zero force and torque for `bodies` bodies.
#[must_use]
pub fn new(bodies: usize) -> Self {
Self {
force: vec![Vec3Fix::ZERO; bodies],
torque: vec![Vec3Fix::ZERO; bodies],
}
}
/// Number of body slots.
#[must_use]
pub fn len(&self) -> usize {
self.force.len()
}
/// Whether there are no body slots.
#[must_use]
pub fn is_empty(&self) -> bool {
self.force.is_empty()
}
fn slot(&self, body: usize) -> Result<usize, AccumulateError> {
if body < self.force.len() {
Ok(body)
} else {
Err(AccumulateError::BodyOutOfRange {
index: body,
len: self.force.len(),
})
}
}
/// Add `force` to body `body`.
///
/// # Errors
///
/// [`AccumulateError::BodyOutOfRange`] when `body` has no slot (nothing
/// is added).
pub fn add_force(&mut self, body: usize, force: Vec3Fix) -> Result<(), AccumulateError> {
let i = self.slot(body)?;
self.force[i] = self.force[i] + force;
Ok(())
}
/// Add `torque` to body `body`.
///
/// # Errors
///
/// [`AccumulateError::BodyOutOfRange`] when `body` has no slot (nothing
/// is added).
pub fn add_torque(&mut self, body: usize, torque: Vec3Fix) -> Result<(), AccumulateError> {
let i = self.slot(body)?;
self.torque[i] = self.torque[i] + torque;
Ok(())
}
/// Summed force on `body` (`None` past the end).
#[must_use]
pub fn force(&self, body: usize) -> Option<Vec3Fix> {
self.force.get(body).copied()
}
/// Summed torque on `body` (`None` past the end).
#[must_use]
pub fn torque(&self, body: usize) -> Option<Vec3Fix> {
self.torque.get(body).copied()
}
/// Add every slot of `other` into `self`, slot by slot.
///
/// # Errors
///
/// [`AccumulateError::LengthMismatch`] when the slot counts differ
/// (`self` is unchanged).
pub fn merge(&mut self, other: &Self) -> Result<(), AccumulateError> {
if other.len() != self.len() {
return Err(AccumulateError::LengthMismatch {
slots: other.len(),
bodies: self.len(),
});
}
for i in 0..self.len() {
self.force[i] = self.force[i] + other.force[i];
self.torque[i] = self.torque[i] + other.torque[i];
}
Ok(())
}
/// Reset every slot to zero, keeping the slot count.
pub fn clear(&mut self) {
self.force.fill(Vec3Fix::ZERO);
self.torque.fill(Vec3Fix::ZERO);
}
}
/// What a participant sees during one substep: the bodies as they were at the
/// start of the substep (read only) and an accumulator for its forces.
#[derive(Debug)]
pub struct SubstepCtx<'a> {
bodies: &'a [RigidBody],
forces: &'a mut ForceAccumulator,
substep_index: usize,
substeps: usize,
h: Fix128,
board: Option<&'a FieldBoard>,
stage: Option<&'a mut FieldStage>,
ports: &'a [Port],
sdf_contacts: &'a [crate::sdf_collider::SdfContact],
}
impl<'a> SubstepCtx<'a> {
/// A context over `bodies`, staging into `forces` (one slot per body),
/// for substep `substep_index` of `substeps` with width `h`.
///
/// # Errors
///
/// [`AccumulateError::LengthMismatch`] when `forces` does not have one
/// slot per body.
pub fn new(
bodies: &'a [RigidBody],
forces: &'a mut ForceAccumulator,
substep_index: usize,
substeps: usize,
h: Fix128,
) -> Result<Self, AccumulateError> {
if forces.len() != bodies.len() {
return Err(AccumulateError::LengthMismatch {
slots: forces.len(),
bodies: bodies.len(),
});
}
Ok(Self {
bodies,
forces,
substep_index,
substeps,
h,
board: None,
stage: None,
ports: &[],
sdf_contacts: &[],
})
}
/// The same context with the world's shared fields: reads come from
/// `board` (committed values), writes go to `stage`, and only the ports in
/// `ports` (what the participant declared) may be read or written.
#[must_use]
pub fn with_fields(
mut self,
board: &'a FieldBoard,
stage: &'a mut FieldStage,
ports: &'a [Port],
) -> Self {
self.board = Some(board);
self.stage = Some(stage);
self.ports = ports;
self
}
/// The same context with the SDF contacts the world resolved so far in
/// the current step ([`Self::sdf_contacts`]).
#[must_use]
pub fn with_sdf_contacts(mut self, contacts: &'a [crate::sdf_collider::SdfContact]) -> Self {
self.sdf_contacts = contacts;
self
}
fn declared(&self, id: PortId, access: PortAccess) -> Result<(), FieldAccessError> {
if self.ports.iter().any(|p| p.id == id && p.access == access) {
Ok(())
} else {
Err(FieldAccessError::NotDeclared { field: id, access })
}
}
/// The value of field `id` committed before this substep began.
///
/// # Errors
///
/// [`FieldAccessError::Unknown`] when there is no such field (or no
/// board); [`FieldAccessError::NotDeclared`] when the participant did not
/// declare [`Port::reads_committed`] for it.
pub fn field(&self, id: PortId) -> Result<&'a [Fix128], FieldAccessError> {
let board = self.board.ok_or(FieldAccessError::Unknown(id))?;
let slot = board.slot(id).ok_or(FieldAccessError::Unknown(id))?;
self.declared(id, PortAccess::ReadCommitted)?;
Ok(&slot.value)
}
/// The participant's staged buffer for field `id`, created on first use:
/// a copy of the committed value for a [`FieldMode::Replace`] field,
/// zeros for a [`FieldMode::Sum`] field. What it holds when the substep
/// returns `Ok` is committed at the end of the substep.
///
/// # Errors
///
/// [`FieldAccessError::Unknown`] when there is no such field (or no
/// board); [`FieldAccessError::NotDeclared`] when the participant did not
/// declare [`Port::writes`] for it.
pub fn stage_field(&mut self, id: PortId) -> Result<&mut [Fix128], FieldAccessError> {
let board = self.board.ok_or(FieldAccessError::Unknown(id))?;
let slot = board.slot(id).ok_or(FieldAccessError::Unknown(id))?;
self.declared(id, PortAccess::Write)?;
let stage = self
.stage
.as_deref_mut()
.ok_or(FieldAccessError::Unknown(id))?;
Ok(stage.buffer(id, || match slot.mode {
FieldMode::Replace => slot.value.clone(),
FieldMode::Sum => vec![Fix128::ZERO; slot.value.len()],
}))
}
/// The bodies at the start of this substep. With TGS (one detection per
/// tick) as well as XPBD, positions and velocities are those at the head
/// of the substep.
#[must_use]
pub fn bodies(&self) -> &[RigidBody] {
self.bodies
}
/// The SDF contacts the world resolved in the current step before this
/// participant runs, in the order they were applied (see
/// [`crate::PhysicsWorld::last_step_sdf_contacts`]).
///
/// Participants run at the head of each substep, before its SDF
/// resolution, so with the XPBD paths (`step`, `step_parallel`,
/// `step_with_bridge`) substep `i` sees the contacts of substeps
/// `0..i` of this step (none in substep 0). The TGS backend resolves SDF
/// overlap once, before its substeps, so every substep sees all of the
/// step's contacts. Empty for a context built by [`Self::new`] alone.
#[must_use]
pub fn sdf_contacts(&self) -> &'a [crate::sdf_collider::SdfContact] {
self.sdf_contacts
}
/// Index of this substep within the frame (`0..substeps`).
#[must_use]
pub fn substep_index(&self) -> usize {
self.substep_index
}
/// Substeps in the frame.
#[must_use]
pub fn substeps(&self) -> usize {
self.substeps
}
/// Width of this substep.
#[must_use]
pub fn h(&self) -> Fix128 {
self.h
}
/// Stage `force` on body `body` for this substep.
///
/// # Errors
///
/// [`AccumulateError::BodyOutOfRange`] for a body that does not exist.
pub fn add_force(&mut self, body: usize, force: Vec3Fix) -> Result<(), AccumulateError> {
self.forces.add_force(body, force)
}
/// Stage `torque` on body `body` for this substep.
///
/// # Errors
///
/// [`AccumulateError::BodyOutOfRange`] for a body that does not exist.
pub fn add_torque(&mut self, body: usize, torque: Vec3Fix) -> Result<(), AccumulateError> {
self.forces.add_torque(body, torque)
}
}
/// A value read from the world: exact, or undecided because a fault (a value
/// out of range) was recorded.
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum Observed<T> {
/// The exact value.
Exact(T),
/// No value can be stated: the world recorded a fault.
Undecided,
}
/// Three-valued outcome of a predicate over an [`Observed`] value.
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum Verdict {
/// The value is exact and the predicate holds.
Holds,
/// The value is exact and the predicate does not hold.
Violated,
/// The value is undecided.
Undecided,
}
impl<T> Observed<T> {
/// Evaluate `predicate` on the exact value; [`Verdict::Undecided`] when
/// there is none.
pub fn verdict(&self, predicate: impl FnOnce(&T) -> bool) -> Verdict {
match self {
Self::Exact(v) if predicate(v) => Verdict::Holds,
Self::Exact(_) => Verdict::Violated,
Self::Undecided => Verdict::Undecided,
}
}
}
/// Channels a participant reports from [`Participant::observe`], in the order
/// it pushed them.
#[derive(Clone, Debug, Default, PartialEq, Eq)]
pub struct ObservationSink {
values: Vec<(u32, Fix128)>,
}
impl ObservationSink {
/// An empty sink.
#[must_use]
pub fn new() -> Self {
Self::default()
}
/// Report `value` on `channel`.
pub fn push(&mut self, channel: u32, value: Fix128) {
self.values.push((channel, value));
}
/// Every reported `(channel, value)`, in push order.
#[must_use]
pub fn values(&self) -> &[(u32, Fix128)] {
&self.values
}
}
/// A law that advances its own state inside the world's substep loop.
///
/// See the module documentation for the contract.
pub trait Participant: Send {
/// Snapshot tag of this participant type.
fn kind(&self) -> ParticipantKind;
/// How this participant's time step relates to the world substep.
fn step_rule(&self) -> StepRule {
StepRule::FollowSubstep
}
/// The ports this participant reads and writes, empty by default. The
/// world reads them once, at registration, to fix the execution order
/// ([`execution_order`]); they must not change while the participant is
/// registered.
fn ports(&self) -> &[Port] {
&[]
}
/// Advance by one world substep of width `h` (`ctx.h()`). Read bodies
/// from `ctx`, stage forces on them through `ctx`.
///
/// # Errors
///
/// A [`ParticipantFault`]; the participant must then be exactly as it
/// was before the call (the forces it staged are dropped by the world).
fn substep(&mut self, ctx: &mut SubstepCtx<'_>, h: Fix128) -> Result<(), ParticipantFault>;
/// Report observable values. The world reports them as
/// [`Observed::Undecided`] while a fault is recorded.
fn observe(&self, out: &mut ObservationSink);
/// Append the whole state to `out`.
fn write_state(&self, out: &mut Vec<u8>);
/// Check that `bytes` is a payload [`Self::read_state`] can apply. Must
/// not change `self`.
///
/// # Errors
///
/// A [`StateError`] saying why the payload is unusable.
fn check_state(&self, bytes: &[u8]) -> Result<(), StateError>;
/// Replace the state with `bytes`. Only called with a payload
/// [`Self::check_state`] accepted.
fn read_state(&mut self, bytes: &[u8]);
}
// ============================================================================
// Shared fields
// ============================================================================
/// Where the samples of a shared field sit.
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
#[non_exhaustive]
pub enum FieldLayout {
/// One sample per rigid body, indexed like the world's body list.
PerBody {
/// Number of bodies.
bodies: usize,
},
/// A uniform grid of cubic cells. Cell `(ix, iy, iz)` covers
/// `origin + [ix, ix+1)·cell` (and so on per axis); samples are stored row
/// major with `ix` fastest, `index = (iz·ny + iy)·nx + ix`.
Grid {
/// Lower corner of cell `(0, 0, 0)`.
origin: Vec3Fix,
/// Edge length of a cell (positive).
cell: Fix128,
/// Cells per axis `[nx, ny, nz]` (each at least 1).
dims: [usize; 3],
},
}
impl FieldLayout {
/// Number of samples, or `None` for a grid with a non-positive cell, an
/// axis of 0 cells, or more samples than `usize` holds.
#[must_use]
pub fn samples(&self) -> Option<usize> {
match *self {
Self::PerBody { bodies } => Some(bodies),
Self::Grid { cell, dims, .. } => {
if cell <= Fix128::ZERO || dims.contains(&0) {
return None;
}
dims[0].checked_mul(dims[1])?.checked_mul(dims[2])
}
}
}
}
/// How the staged writes of one substep become the field's value.
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
#[non_exhaustive]
pub enum FieldMode {
/// At most one writer; its staged buffer (starting as the committed value)
/// replaces the value. Without a write in a substep the value stays.
Replace,
/// Any number of writers; each staged buffer starts at zero and the value
/// becomes the sum of this substep's buffers (zero when nobody wrote).
Sum,
}
impl FieldMode {
const fn tag(self) -> u8 {
match self {
Self::Replace => 0,
Self::Sum => 1,
}
}
}
/// Why a field could not be declared or set.
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
#[non_exhaustive]
pub enum FieldError {
/// A field with this id is already declared.
Duplicate(PortId),
/// The layout has no valid sample count ([`FieldLayout::samples`]).
InvalidLayout,
/// No field with this id.
Unknown(PortId),
/// The values do not have one entry per sample.
Length {
/// Samples of the field.
expected: usize,
/// Values given.
found: usize,
},
}
/// Why the declared ports do not fit the shared fields.
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
#[non_exhaustive]
pub enum FieldPortError {
/// A [`FieldMode::Replace`] field declared written by two participants.
SecondWriter {
/// The field.
field: PortId,
/// Registration index of the first writer.
first: usize,
/// Registration index of the second writer.
second: usize,
},
/// A plain [`PortAccess::Read`] of a field: a field read sees the
/// committed value and must be declared [`PortAccess::ReadCommitted`].
ReadOfField {
/// Registration index of the participant.
participant: usize,
/// The field.
field: PortId,
},
/// [`PortAccess::ReadCommitted`] of a port that is not a field.
UnknownField {
/// Registration index of the participant.
participant: usize,
/// The port.
field: PortId,
},
}
/// Why a participant could not read or stage a field.
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
#[non_exhaustive]
pub enum FieldAccessError {
/// No field with this id in the context.
Unknown(PortId),
/// The participant did not declare this access in [`Participant::ports`].
NotDeclared {
/// The field.
field: PortId,
/// The access it would need.
access: PortAccess,
},
}
#[derive(Clone, Debug, PartialEq, Eq)]
struct FieldSlot {
id: PortId,
layout: FieldLayout,
mode: FieldMode,
value: Vec<Fix128>,
}
/// The world's shared fields: committed values, owned by the world alone.
///
/// See the module documentation (*Shared fields*) for the rules.
#[derive(Clone, Debug, Default, PartialEq, Eq)]
pub struct FieldBoard {
// ascending id
fields: Vec<FieldSlot>,
}
impl FieldBoard {
/// A board without fields.
#[must_use]
pub fn new() -> Self {
Self::default()
}
fn slot(&self, id: PortId) -> Option<&FieldSlot> {
self.fields
.binary_search_by_key(&id, |f| f.id)
.ok()
.map(|i| &self.fields[i])
}
/// Declare field `id`, all samples zero.
///
/// # Errors
///
/// [`FieldError::Duplicate`] for an id already declared,
/// [`FieldError::InvalidLayout`] for a layout without a sample count; the
/// board is unchanged.
pub fn declare(
&mut self,
id: PortId,
layout: FieldLayout,
mode: FieldMode,
) -> Result<(), FieldError> {
let n = layout.samples().ok_or(FieldError::InvalidLayout)?;
match self.fields.binary_search_by_key(&id, |f| f.id) {
Ok(_) => Err(FieldError::Duplicate(id)),
Err(at) => {
self.fields.insert(
at,
FieldSlot {
id,
layout,
mode,
value: vec![Fix128::ZERO; n],
},
);
Ok(())
}
}
}
/// Set the committed value of field `id` (an initial condition, set
/// between steps).
///
/// # Errors
///
/// [`FieldError::Unknown`] or [`FieldError::Length`]; the board is
/// unchanged.
pub fn set(&mut self, id: PortId, values: &[Fix128]) -> Result<(), FieldError> {
let i = self
.fields
.binary_search_by_key(&id, |f| f.id)
.map_err(|_| FieldError::Unknown(id))?;
let slot = &mut self.fields[i];
if values.len() != slot.value.len() {
return Err(FieldError::Length {
expected: slot.value.len(),
found: values.len(),
});
}
slot.value.copy_from_slice(values);
Ok(())
}
/// The committed value of field `id`.
#[must_use]
pub fn value(&self, id: PortId) -> Option<&[Fix128]> {
self.slot(id).map(|f| f.value.as_slice())
}
/// The layout of field `id`.
#[must_use]
pub fn layout(&self, id: PortId) -> Option<FieldLayout> {
self.slot(id).map(|f| f.layout)
}
/// The mode of field `id`.
#[must_use]
pub fn mode(&self, id: PortId) -> Option<FieldMode> {
self.slot(id).map(|f| f.mode)
}
/// Every declared field id, ascending.
#[must_use]
pub fn ids(&self) -> Vec<PortId> {
self.fields.iter().map(|f| f.id).collect()
}
/// Commit `stages` (registration index, stage) in run order. Either every
/// field takes its new value or, on error, none changes.
fn commit(&mut self, stages: &[(usize, FieldStage)]) -> Result<(), WorldFault> {
let mut next: Vec<Vec<Fix128>> = Vec::with_capacity(self.fields.len());
for slot in &self.fields {
let mut v = match slot.mode {
FieldMode::Replace => slot.value.clone(),
FieldMode::Sum => vec![Fix128::ZERO; slot.value.len()],
};
for (index, stage) in stages {
let Some(staged) = stage.staged(slot.id) else {
continue;
};
match slot.mode {
FieldMode::Replace => v.copy_from_slice(staged),
FieldMode::Sum => {
for (acc, add) in v.iter_mut().zip(staged) {
*acc = checked_add(*acc, *add).ok_or(WorldFault::FieldOutOfRange {
field: slot.id,
index: *index,
})?;
}
}
}
}
next.push(v);
}
for (slot, v) in self.fields.iter_mut().zip(next) {
slot.value = v;
}
Ok(())
}
fn write_descriptor(slot: &FieldSlot, out: &mut Vec<u8>) {
out.extend_from_slice(&slot.id.get().to_le_bytes());
out.push(slot.mode.tag());
match slot.layout {
FieldLayout::PerBody { bodies } => {
out.push(0);
out.extend_from_slice(&(bodies as u64).to_le_bytes());
}
FieldLayout::Grid { origin, cell, dims } => {
out.push(1);
for f in [origin.x, origin.y, origin.z, cell] {
put_fix(out, f);
}
for d in dims {
out.extend_from_slice(&(d as u64).to_le_bytes());
}
}
}
}
/// Append every field (descriptor and committed values) to `out`, the
/// bytes of the snapshot section `fields`.
pub fn write_values(&self, out: &mut Vec<u8>) {
out.extend_from_slice(&(self.fields.len() as u64).to_le_bytes());
for slot in &self.fields {
Self::write_descriptor(slot, out);
for &f in &slot.value {
put_fix(out, f);
}
}
}
/// Check that `bytes` (from [`Self::write_values`]) describes exactly the
/// fields of this board (ids, modes, layouts); the values are not checked.
/// Does not change the board.
///
/// # Errors
///
/// [`StateError::Length`] when the byte count differs from what this board
/// needs, [`StateError::InvalidValue`] when a descriptor differs.
pub fn check_values(&self, bytes: &[u8]) -> Result<(), StateError> {
let mut own = Vec::new();
self.write_values(&mut own);
if own.len() != bytes.len() {
return Err(StateError::Length {
expected: own.len(),
found: bytes.len(),
});
}
let mut at = 8;
if own[..at] != bytes[..at] {
return Err(StateError::InvalidValue);
}
for slot in &self.fields {
let mut d = Vec::new();
Self::write_descriptor(slot, &mut d);
if bytes[at..at + d.len()] != d[..] {
return Err(StateError::InvalidValue);
}
at += d.len() + 16 * slot.value.len();
}
Ok(())
}
/// Replace every committed value with those in `bytes`. Only called with
/// bytes [`Self::check_values`] accepted.
pub fn read_values(&mut self, bytes: &[u8]) {
let mut at = 8;
for slot in &mut self.fields {
let mut d = Vec::new();
Self::write_descriptor(slot, &mut d);
at += d.len();
for f in &mut slot.value {
*f = get_fix(&bytes[at..at + 16]);
at += 16;
}
}
}
}
fn put_fix(out: &mut Vec<u8>, f: Fix128) {
out.extend_from_slice(&f.hi.to_le_bytes());
out.extend_from_slice(&f.lo.to_le_bytes());
}
fn get_fix(b: &[u8]) -> Fix128 {
let mut hi = [0_u8; 8];
let mut lo = [0_u8; 8];
hi.copy_from_slice(&b[0..8]);
lo.copy_from_slice(&b[8..16]);
Fix128 {
hi: i64::from_le_bytes(hi),
lo: u64::from_le_bytes(lo),
}
}
/// `a + b`, or `None` when the sum leaves the range of [`Fix128`].
fn checked_add(a: Fix128, b: Fix128) -> Option<Fix128> {
let wide = |f: Fix128| (i128::from(f.hi) << 64) | i128::from(f.lo);
let s = wide(a).checked_add(wide(b))?;
Some(Fix128 {
hi: (s >> 64) as i64,
lo: s as u64,
})
}
/// One participant's staged field writes for one substep.
#[derive(Clone, Debug, Default, PartialEq, Eq)]
pub struct FieldStage {
// ascending id
staged: Vec<(PortId, Vec<Fix128>)>,
}
impl FieldStage {
/// An empty stage.
#[must_use]
pub fn new() -> Self {
Self::default()
}
/// Whether nothing was staged.
#[must_use]
pub fn is_empty(&self) -> bool {
self.staged.is_empty()
}
/// The staged buffer of field `id`, if one was created.
#[must_use]
pub fn staged(&self, id: PortId) -> Option<&[Fix128]> {
self.staged
.binary_search_by_key(&id, |s| s.0)
.ok()
.map(|i| self.staged[i].1.as_slice())
}
fn buffer(&mut self, id: PortId, init: impl FnOnce() -> Vec<Fix128>) -> &mut [Fix128] {
let i = match self.staged.binary_search_by_key(&id, |s| s.0) {
Ok(i) => i,
Err(at) => {
self.staged.insert(at, (id, init()));
at
}
};
&mut self.staged[i].1
}
}
/// Check the declared ports of every participant (`ports[i]` for the
/// participant registered at `i`) against the fields of `board`.
///
/// # Errors
///
/// The first [`FieldPortError`] in registration order, then port order.
pub fn check_field_ports(board: &FieldBoard, ports: &[&[Port]]) -> Result<(), FieldPortError> {
let mut writers: Vec<(PortId, usize)> = Vec::new();
for (i, ps) in ports.iter().enumerate() {
for p in ps.iter() {
let field = board.slot(p.id);
match (p.access, field) {
(PortAccess::Read, Some(_)) => {
return Err(FieldPortError::ReadOfField {
participant: i,
field: p.id,
})
}
(PortAccess::ReadCommitted, None) => {
return Err(FieldPortError::UnknownField {
participant: i,
field: p.id,
})
}
(PortAccess::Write, Some(slot)) if slot.mode == FieldMode::Replace => {
match writers.iter().find(|w| w.0 == p.id) {
Some(&(_, first)) if first != i => {
return Err(FieldPortError::SecondWriter {
field: p.id,
first,
second: i,
})
}
Some(_) => {}
None => writers.push((p.id, i)),
}
}
_ => {}
}
}
}
Ok(())
}
/// What the world fixes at registration for a list of participants: the run
/// order within a substep and each participant's declared ports.
#[derive(Clone, Debug, PartialEq, Eq)]
pub struct ParticipantPlan {
order: Vec<usize>,
ports: Vec<Vec<Port>>,
}
impl ParticipantPlan {
/// The plan for `participants` (registration order) over the fields of
/// `board`. The world builds it again whenever a participant or a field is
/// added.
///
/// # Errors
///
/// [`RegisterError::Order`] for a loop of ports, [`RegisterError::Field`]
/// when the ports do not fit the fields.
pub fn new(
participants: &[Box<dyn Participant>],
board: &FieldBoard,
) -> Result<Self, RegisterError> {
let ports: Vec<Vec<Port>> = participants.iter().map(|p| p.ports().to_vec()).collect();
let views: Vec<&[Port]> = ports.iter().map(Vec::as_slice).collect();
check_field_ports(board, &views).map_err(RegisterError::Field)?;
let order = execution_order(&views).map_err(RegisterError::Order)?;
Ok(Self { order, ports })
}
/// Registration indices in run order.
#[must_use]
pub fn order(&self) -> &[usize] {
&self.order
}
}
/// Position and width of one substep.
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub struct SubstepTime {
/// Index of the substep within the frame.
pub index: usize,
/// Substeps in the frame.
pub count: usize,
/// Width of the substep.
pub h: Fix128,
}
/// Why [`run_substep`] did not run (nothing was called or changed).
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
#[non_exhaustive]
pub enum ExchangeError {
/// The plan was built for another number of participants.
Plan {
/// Participants given.
participants: usize,
/// Participants in the plan.
planned: usize,
},
/// `frozen` does not have one flag per participant.
Frozen {
/// Flags given.
flags: usize,
/// Participants given.
participants: usize,
},
/// The force accumulator does not have one slot per body.
Accumulate(AccumulateError),
/// A [`FieldLayout::PerBody`] field does not have one sample per body.
BodyCount {
/// The field.
field: PortId,
/// Samples of the field.
samples: usize,
/// Bodies.
bodies: usize,
},
}
/// One substep of every participant, the part of the world step that runs the
/// participants and exchanges their results.
///
/// Participants that are not `frozen` run in the plan's order. Each gets the
/// bodies at the start of the substep, the fields as committed before the
/// substep, and its own empty force accumulator and field stage. On `Ok` its
/// forces are merged into `forces`; on `Err` its forces and staged writes are
/// dropped, it is marked frozen and a [`WorldFault::Participant`] is
/// reported. After every participant ran, the staged writes of those that
/// returned `Ok` are committed in run order ([`FieldBoard`] rules); a commit
/// out of range changes no field and is reported as
/// [`WorldFault::FieldOutOfRange`].
///
/// Returns the faults of this substep in the order they happened.
///
/// # Errors
///
/// An [`ExchangeError`] when the inputs do not fit together; nothing ran.
pub fn run_substep(
participants: &mut [Box<dyn Participant>],
plan: &ParticipantPlan,
frozen: &mut [bool],
bodies: &[RigidBody],
board: &mut FieldBoard,
forces: &mut ForceAccumulator,
time: SubstepTime,
) -> Result<Vec<WorldFault>, ExchangeError> {
run_substep_with_sdf_contacts(participants, plan, frozen, bodies, board, forces, time, &[])
}
/// [`run_substep`] with the SDF contacts each participant reads through
/// [`SubstepCtx::sdf_contacts`]; `run_substep` passes none.
///
/// # Errors
///
/// As [`run_substep`].
#[allow(clippy::too_many_arguments)]
pub fn run_substep_with_sdf_contacts(
participants: &mut [Box<dyn Participant>],
plan: &ParticipantPlan,
frozen: &mut [bool],
bodies: &[RigidBody],
board: &mut FieldBoard,
forces: &mut ForceAccumulator,
time: SubstepTime,
sdf_contacts: &[crate::sdf_collider::SdfContact],
) -> Result<Vec<WorldFault>, ExchangeError> {
let n = participants.len();
if plan.order.len() != n {
return Err(ExchangeError::Plan {
participants: n,
planned: plan.order.len(),
});
}
if frozen.len() != n {
return Err(ExchangeError::Frozen {
flags: frozen.len(),
participants: n,
});
}
if forces.len() != bodies.len() {
return Err(ExchangeError::Accumulate(AccumulateError::LengthMismatch {
slots: forces.len(),
bodies: bodies.len(),
}));
}
for slot in &board.fields {
if let FieldLayout::PerBody { .. } = slot.layout {
if slot.value.len() != bodies.len() {
return Err(ExchangeError::BodyCount {
field: slot.id,
samples: slot.value.len(),
bodies: bodies.len(),
});
}
}
}
let mut faults = Vec::new();
let mut kept: Vec<(usize, FieldStage)> = Vec::with_capacity(n);
for &i in &plan.order {
if frozen[i] {
continue;
}
let mut staged_forces = ForceAccumulator::new(bodies.len());
let mut stage = FieldStage::new();
let result = {
let ctx = SubstepCtx::new(bodies, &mut staged_forces, time.index, time.count, time.h)
.map_err(ExchangeError::Accumulate)?;
let mut ctx = ctx
.with_fields(board, &mut stage, &plan.ports[i])
.with_sdf_contacts(sdf_contacts);
participants[i].substep(&mut ctx, time.h)
};
match result {
Ok(()) => {
forces
.merge(&staged_forces)
.map_err(ExchangeError::Accumulate)?;
kept.push((i, stage));
}
Err(fault) => {
frozen[i] = true;
faults.push(WorldFault::Participant {
index: i,
kind: participants[i].kind(),
fault,
});
}
}
}
if let Err(fault) = board.commit(&kept) {
faults.push(fault);
}
Ok(faults)
}
/// Why an amount could not be moved between layouts.
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
#[non_exhaustive]
pub enum RemapError {
/// The values do not have one entry per source sample.
Length {
/// Samples of the source.
expected: usize,
/// Values given.
found: usize,
},
/// A layout is not a valid grid ([`FieldLayout::samples`]).
InvalidLayout,
/// The grids are not nested: not the same origin, or cell sizes not an
/// integer ratio, or not covering the same box.
NotNested,
/// A sum left the range of [`Fix128`].
OutOfRange,
/// Body `body` lies outside the grid.
OutsideGrid {
/// Index of the body.
body: usize,
},
}
/// `big = n·small` exactly with an integer `n ≥ 1`.
fn integer_ratio(big: Fix128, small: Fix128) -> Option<usize> {
let n = big / small;
if n.lo == 0 && n.hi >= 1 && Fix128::from_int(n.hi) * small == big {
usize::try_from(n.hi).ok()
} else {
None
}
}
fn grid_of(layout: &FieldLayout) -> Result<(Vec3Fix, Fix128, [usize; 3]), RemapError> {
match *layout {
FieldLayout::Grid { origin, cell, dims } if layout.samples().is_some() => {
Ok((origin, cell, dims))
}
_ => Err(RemapError::InvalidLayout),
}
}
/// Move an extensive amount (per cell, not a density) from grid `src` to grid
/// `dst`, keeping the total exactly.
///
/// The grids must be nested: the same origin, cell sizes in an integer ratio
/// `r`, and the same box. Coarsening sums the `r³` fine cells of each coarse
/// cell. Refining gives each fine cell `q / r³` of its coarse cell's amount
/// `q`, and the first fine cell (lowest index) also the remainder
/// `q − r³·(q / r³)`, so the fine cells add up to `q` exactly. Equal grids
/// copy.
///
/// # Errors
///
/// [`RemapError::InvalidLayout`] when a layout is not a grid,
/// [`RemapError::Length`] for `values` of the wrong length,
/// [`RemapError::NotNested`] for grids that are not nested,
/// [`RemapError::OutOfRange`] when a coarse sum leaves the range.
pub fn remap_conserving(
src: &FieldLayout,
values: &[Fix128],
dst: &FieldLayout,
) -> Result<Vec<Fix128>, RemapError> {
let (so, sc, sd) = grid_of(src)?;
let (d_o, dc, dd) = grid_of(dst)?;
let n_src = src.samples().ok_or(RemapError::InvalidLayout)?;
let n_dst = dst.samples().ok_or(RemapError::InvalidLayout)?;
if values.len() != n_src {
return Err(RemapError::Length {
expected: n_src,
found: values.len(),
});
}
if so != d_o {
return Err(RemapError::NotNested);
}
let coarsen = dc >= sc;
let r = if coarsen {
integer_ratio(dc, sc)
} else {
integer_ratio(sc, dc)
}
.ok_or(RemapError::NotNested)?;
let (fine, coarse) = if coarsen { (sd, dd) } else { (dd, sd) };
for a in 0..3 {
if coarse[a].checked_mul(r) != Some(fine[a]) {
return Err(RemapError::NotNested);
}
}
let parent =
|ix: usize, iy: usize, iz: usize| ((iz / r) * coarse[1] + iy / r) * coarse[0] + ix / r;
if coarsen {
let mut out = vec![Fix128::ZERO; n_dst];
for iz in 0..fine[2] {
for iy in 0..fine[1] {
for ix in 0..fine[0] {
let f = (iz * fine[1] + iy) * fine[0] + ix;
let c = parent(ix, iy, iz);
out[c] = checked_add(out[c], values[f]).ok_or(RemapError::OutOfRange)?;
}
}
}
Ok(out)
} else {
let k = r
.checked_mul(r)
.and_then(|v| v.checked_mul(r))
.and_then(|v| i64::try_from(v).ok())
.ok_or(RemapError::NotNested)?;
let kf = Fix128::from_int(k);
let mut out = vec![Fix128::ZERO; n_dst];
for iz in 0..fine[2] {
for iy in 0..fine[1] {
for ix in 0..fine[0] {
let q = values[parent(ix, iy, iz)];
let base = q / kf;
let first = ix % r == 0 && iy % r == 0 && iz % r == 0;
out[(iz * fine[1] + iy) * fine[0] + ix] =
if first { base + (q - base * kf) } else { base };
}
}
}
Ok(out)
}
}
/// Deposit an extensive amount per body (`amounts[i]` for `bodies[i]`) into
/// the grid cell that holds each body's position, keeping the total exactly.
///
/// # Errors
///
/// [`RemapError::InvalidLayout`] when `dst` is not a grid,
/// [`RemapError::Length`] when `amounts` is not one per body,
/// [`RemapError::OutsideGrid`] for a body outside the grid (nothing is
/// dropped silently), [`RemapError::OutOfRange`] when a cell sum leaves the
/// range.
pub fn deposit_bodies(
bodies: &[RigidBody],
amounts: &[Fix128],
dst: &FieldLayout,
) -> Result<Vec<Fix128>, RemapError> {
let (origin, cell, dims) = grid_of(dst)?;
let n = dst.samples().ok_or(RemapError::InvalidLayout)?;
if amounts.len() != bodies.len() {
return Err(RemapError::Length {
expected: bodies.len(),
found: amounts.len(),
});
}
let mut out = vec![Fix128::ZERO; n];
for (b, (body, &q)) in bodies.iter().zip(amounts).enumerate() {
let p = body.position;
let mut idx = [0_usize; 3];
for (a, (x, o)) in [(p.x, origin.x), (p.y, origin.y), (p.z, origin.z)]
.into_iter()
.enumerate()
{
let t = (x - o) / cell;
idx[a] = usize::try_from(t.hi)
.ok()
.filter(|&i| i < dims[a])
.ok_or(RemapError::OutsideGrid { body: b })?;
}
let c = (idx[2] * dims[1] + idx[1]) * dims[0] + idx[0];
out[c] = checked_add(out[c], q).ok_or(RemapError::OutOfRange)?;
}
Ok(out)
}
// ============================================================================
// Waking a parked body
// ============================================================================
/// Whether a change is non-zero in a component; a change too large to
/// represent (`None`) counts as non-zero.
fn changes(d: Option<Vec3Fix>) -> bool {
d.is_none_or(|d| !(d.x.is_zero() && d.y.is_zero() && d.z.is_zero()))
}
/// Whether participant force `force` and torque `torque` (summed over every
/// participant for this substep) wake the parked or sleeping body `body`: the
/// change the world would apply in one substep of width `h`,
/// `Δv = (F·inv_mass)·h` or `Δω = (I⁻¹τ)·h` (`I⁻¹` in the world frame, the
/// same [`Fix128`] products the world uses), is non-zero in any component.
/// There is no threshold: a participant force that changes the body at all
/// wakes it, so the momentum a participant books for its force always
/// reaches the body. When both changes round to exactly zero the body stays
/// asleep (a force of a few raw units does not keep bodies awake). A change
/// too large to represent wakes the body (the world then reports it as
/// [`WorldFault::ForceOutOfRange`]). A static body never wakes.
///
/// This replaces the earlier rule for participant forces, which compared the
/// change with the sleep thresholds of `sleep` and left a body asleep below
/// them (the force then had no effect on it). `sleep` is no longer read; it
/// stays in the signature of this unstable function. Wakes from contacts,
/// impulses and other sources keep their own thresholds, and waking spreads
/// through an island by the existing rules.
#[must_use]
pub fn wakes_parked_body(
body: &RigidBody,
force: Vec3Fix,
torque: Vec3Fix,
h: Fix128,
_sleep: &SleepConfig,
) -> bool {
if body.is_static() {
return false;
}
let dv = force
.checked_scale(body.inv_mass)
.and_then(|a| a.checked_scale(h));
let dw = body
.checked_world_inv_inertia_apply(torque)
.and_then(|a| a.checked_scale(h));
changes(dv) || changes(dw)
}