ferravex_physics/
world.rs

1use crate::{
2    body::{Access, BodySet, Collider, ColliderHandle, ColliderSet, Kind, RigidBodyHandle, Shape},
3    geometry::{self, Bounds, Point},
4};
5use ferravex_math::{Quat, Vec3};
6mod cache;
7mod compounds;
8mod islands;
9mod joints;
10use cache::Manifold;
11pub use cache::{ContactManifold, ContactPoint, SolverSettings};
12pub use compounds::{MAX_CHILDREN, MAX_COLLIDERS};
13use islands::Islands;
14use joints::{JointPhase, JointRow};
15
16/// Contacts include transient impacts from all substeps of the latest outer step.
17#[derive(Clone, Copy, Debug)]
18pub struct ContactPair {
19    pub collider1: ColliderHandle,
20    pub collider2: ColliderHandle,
21    pub normal: [f32; 3],
22    pub sensor: bool,
23}
24impl ContactPair {
25    pub fn has_any_active_contact(&self) -> bool {
26        !self.sensor
27    }
28}
29#[derive(Clone, Copy, Debug, PartialEq, Eq)]
30pub enum ContactEvent {
31    Started(ColliderHandle, ColliderHandle),
32    Stopped(ColliderHandle, ColliderHandle),
33}
34#[derive(Clone, Copy, Debug, Default)]
35pub struct StepStats {
36    pub substeps: usize,
37    pub broadphase_pairs: usize,
38    pub contact_points: usize,
39    pub ccd_impacts: usize,
40    pub sleeping_bodies: usize,
41    pub islands: usize,
42    pub active_islands: usize,
43    pub sleeping_islands: usize,
44    pub persisted_points: usize,
45    pub warm_started_points: usize,
46    pub reused_manifolds: usize,
47    pub cached_manifolds: usize,
48    pub cache_dropped: usize,
49    pub velocity_constraints: usize,
50    pub joint_rows: usize,
51    pub joint_constraints: usize,
52}
53#[derive(Clone, Debug)]
54struct Constraint {
55    a: RigidBodyHandle,
56    b: RigidBodyHandle,
57    normal: Vec3,
58    point: Vec3,
59    local_a: Vec3,
60    local_b: Vec3,
61    normal_impulse: f32,
62    tangent_impulse: Vec3,
63    bounce: f32,
64    friction: f32,
65    key: (ColliderHandle, ColliderHandle),
66    feature: u64,
67    cache: Option<(usize, usize)>,
68}
69#[derive(Clone, Copy)]
70struct Proxy {
71    body: RigidBodyHandle,
72    collider: ColliderHandle,
73    bounds: Bounds,
74}
75/// Bodies and child collider handles use generation counters;
76/// capacity is deliberately limited to 4096 bodies for predictable memory use.
77pub struct World<B> {
78    pub bodies: BodySet<B>,
79    pub colliders: ColliderSet,
80    pub gravity: Vec3,
81    pub joints: crate::JointSet,
82    joint_rows: Vec<JointRow>,
83    joint_solver: Option<fn(&mut Self, JointPhase, f32, &mut StepStats)>,
84    disabled_joint_pairs: Vec<(RigidBodyHandle, RigidBodyHandle)>,
85    planar: bool,
86    contacts: Vec<ContactPair>,
87    next_contacts: Vec<ContactPair>,
88    manifolds: Vec<Manifold>,
89    next_manifolds: Vec<Manifold>,
90    next_feature: u64,
91    settings: SolverSettings,
92    islands: Islands,
93    last_gravity: Vec3,
94    old_positions: Vec<(RigidBodyHandle, Vec3, Quat)>,
95    mass_updates: Vec<(RigidBodyHandle, Option<crate::mass::Mass>)>,
96    events: Vec<ContactEvent>,
97    constraints: Vec<Constraint>,
98    proxies: Vec<Proxy>,
99    points: Vec<Point>,
100    pub last_step: StepStats,
101}
102impl<B: Access> World<B> {
103    pub(crate) fn new(gravity: Vec3, planar: bool) -> Self {
104        assert!(gravity.is_finite());
105        Self {
106            bodies: BodySet::default(),
107            colliders: ColliderSet::default(),
108            gravity,
109            joints: Default::default(),
110            joint_rows: Vec::new(),
111            joint_solver: None,
112            disabled_joint_pairs: Vec::new(),
113            planar,
114            contacts: Vec::new(),
115            next_contacts: Vec::new(),
116            manifolds: Vec::new(),
117            next_manifolds: Vec::new(),
118            next_feature: 1,
119            settings: Default::default(),
120            islands: Default::default(),
121            last_gravity: gravity,
122            old_positions: Vec::new(),
123            mass_updates: Vec::new(),
124            events: Vec::new(),
125            constraints: Vec::new(),
126            proxies: Vec::new(),
127            points: Vec::with_capacity(8),
128            last_step: StepStats::default(),
129        }
130    }
131    pub fn insert(
132        &mut self,
133        b: impl Into<B>,
134        c: impl Into<Collider>,
135    ) -> (RigidBodyHandle, ColliderHandle) {
136        let body = self
137            .insert_compound(b, [c.into()])
138            .expect("valid body/collider within capacities");
139        (body, self.bodies[body].core().colliders[0])
140    }
141
142    pub fn remove_body(&mut self, h: RigidBodyHandle) -> bool {
143        if self.bodies.get(h).is_none() {
144            return false;
145        }
146        loop {
147            let id = self
148                .joints
149                .iter()
150                .find(|(_, j)| j.a == h || j.b == h)
151                .map(|(id, _)| id);
152            let Some(id) = id else {
153                break;
154            };
155            self.remove_joint(id);
156        }
157        let Some(b) = self.bodies.remove(h) else {
158            return false;
159        };
160        for &c in &b.core().colliders {
161            for contact in &self.contacts {
162                if contact.sensor {
163                    continue;
164                }
165                let other = if contact.collider1 == c {
166                    Some(contact.collider2)
167                } else if contact.collider2 == c {
168                    Some(contact.collider1)
169                } else {
170                    None
171                };
172                if let Some(other) = other
173                    .and_then(|id| self.colliders.get(id))
174                    .and_then(|o| o.parent)
175                    && let Some(body) = self.bodies.get_mut(other)
176                {
177                    body.core_mut().wake();
178                }
179            }
180            self.manifolds.retain(|m| m.key.0 != c && m.key.1 != c);
181            self.next_manifolds.retain(|m| m.key.0 != c && m.key.1 != c);
182            self.colliders.remove(c);
183        }
184        true
185    }
186    pub fn contact_pair(&self, a: ColliderHandle, b: ColliderHandle) -> Option<&ContactPair> {
187        let key = pair_key(a, b);
188        self.contacts
189            .binary_search_by_key(&key, |c| pair_key(c.collider1, c.collider2))
190            .ok()
191            .map(|i| &self.contacts[i])
192    }
193    pub fn contact_events(&self) -> &[ContactEvent] {
194        &self.events
195    }
196    pub fn contacts(&self) -> impl Iterator<Item = &ContactPair> {
197        self.contacts.iter()
198    }
199    /// Advance a fixed simulation interval in (0, 0.1] seconds. Forces apply for
200    /// this complete interval and are then cleared. The caller owns accumulation.
201    pub fn step(&mut self, dt: f32) -> StepStats {
202        assert!(
203            dt.is_finite() && dt > 0. && dt <= 0.1,
204            "use a fixed timestep in (0, 0.1]"
205        );
206        assert!(self.gravity.is_finite());
207        self.refresh_mass_properties()
208            .expect("valid compound mass properties");
209        self.run_joints(JointPhase::Refresh, dt, &mut StepStats::default());
210        self.next_contacts.clear();
211        self.events.clear();
212        let mut substeps = 1;
213        for (_, body) in self.bodies.iter_mut() {
214            let b = body.core_mut();
215            if b.kind == Kind::Kinematic {
216                b.velocity = b.next_position.map_or(Vec3::ZERO, |p| {
217                    (p + b.next_rotation.unwrap_or(b.rotation) * b.local_com - b.world_com()) / dt
218                });
219                b.angular_velocity = b
220                    .next_rotation
221                    .map_or(Vec3::ZERO, |r| rotation_delta(b.rotation, r) / dt);
222                substeps = substeps.max((b.angular_velocity.length() * dt / 0.06).ceil() as usize);
223            }
224            if b.kind == Kind::Dynamic && b.ccd {
225                let mut size = f32::INFINITY;
226                for child in &b.colliders {
227                    let extent = match self.colliders[*child].shape {
228                        Shape::Ball(r) => r,
229                        Shape::Box(h) => h.x.min(h.y).min(if self.planar { h.x } else { h.z }),
230                        #[cfg(feature = "dim2")]
231                        Shape::Capsule { radius, .. } => radius,
232                        #[cfg(feature = "dim3")]
233                        Shape::Cylinder {
234                            radius,
235                            half_height,
236                        } => radius.min(half_height),
237                    };
238                    size = size.min(extent);
239                }
240                substeps = substeps.max((b.velocity.length() * dt / (size * 0.75)).ceil() as usize);
241                substeps = substeps.max((b.angular_velocity.length() * dt / 0.06).ceil() as usize);
242            }
243        }
244        for (_, j) in self.joints.iter() {
245            if j.description().enabled {
246                let speed =
247                    joints::motor_speed(j, self.bodies[j.a].core(), self.bodies[j.b].core());
248                substeps = substeps.max((speed * dt / 0.06).ceil() as usize);
249            }
250        }
251        substeps = substeps.clamp(1, 8);
252        let h = dt / substeps as f32;
253        let mut stats = StepStats {
254            substeps,
255            ..StepStats::default()
256        };
257        let gravity_changed = self.gravity != self.last_gravity;
258        self.wake_previous_islands(gravity_changed);
259        if gravity_changed {
260            self.manifolds.clear();
261        }
262        for _ in 0..substeps {
263            self.old_positions.clear();
264            self.old_positions.extend(
265                self.bodies
266                    .iter()
267                    .map(|(id, b)| (id, b.core().position, b.core().rotation)),
268            );
269            for (_, body) in self.bodies.iter_mut() {
270                let b = body.core_mut();
271                if b.kind == Kind::Dynamic && !b.sleeping {
272                    b.velocity += (self.gravity * b.gravity_scale + b.force * b.inv_mass) * h;
273                    b.angular_velocity += b.inverse_inertia(b.torque) * h;
274                    b.velocity /= 1. + b.linear_damping * h;
275                    b.angular_velocity /= 1. + b.angular_damping * h;
276                }
277                if b.kind != Kind::Fixed && !b.sleeping {
278                    b.integrate(h);
279                }
280            }
281            stats.ccd_impacts += self.sweep_balls();
282            self.find_contacts(h, &mut stats);
283            self.wake_contact_islands();
284            self.warm_start(&mut stats);
285            self.run_joints(JointPhase::Prepare, h, &mut stats);
286            self.solve_velocities(&mut stats);
287            self.run_joints(JointPhase::Integrate, h, &mut stats);
288            self.solve_positions();
289            self.store_manifolds(h);
290            self.run_joints(JointPhase::Store, h, &mut stats);
291            self.sleep_islands(h, &mut stats);
292        }
293        for (_, body) in self.bodies.iter_mut() {
294            let b = body.core_mut();
295            b.force = Vec3::ZERO;
296            b.torque = Vec3::ZERO;
297            if b.kind == Kind::Kinematic {
298                if let Some(p) = b.next_position.take() {
299                    b.position = p;
300                }
301                if let Some(q) = b.next_rotation.take() {
302                    b.set_rotation(q);
303                }
304            }
305            b.wake_requested = false;
306            b.solver_dirty = false;
307            if b.sleeping {
308                stats.sleeping_bodies += 1;
309            }
310        }
311        self.next_contacts
312            .sort_unstable_by_key(|c| pair_key(c.collider1, c.collider2));
313        self.next_contacts
314            .dedup_by_key(|c| pair_key(c.collider1, c.collider2));
315        for c in &self.next_contacts {
316            if self
317                .contacts
318                .binary_search_by_key(&pair_key(c.collider1, c.collider2), |c| {
319                    pair_key(c.collider1, c.collider2)
320                })
321                .is_err()
322            {
323                self.events
324                    .push(ContactEvent::Started(c.collider1, c.collider2));
325            }
326        }
327        for c in &self.contacts {
328            if self
329                .next_contacts
330                .binary_search_by_key(&pair_key(c.collider1, c.collider2), |c| {
331                    pair_key(c.collider1, c.collider2)
332                })
333                .is_err()
334            {
335                self.events
336                    .push(ContactEvent::Stopped(c.collider1, c.collider2));
337            }
338        }
339        std::mem::swap(&mut self.contacts, &mut self.next_contacts);
340        self.next_contacts.clear();
341        for (_, c) in self.colliders.iter_mut() {
342            c.dirty = false;
343        }
344        self.last_gravity = self.gravity;
345        stats.cached_manifolds = self.manifolds.len();
346        self.last_step = stats;
347        ferravex_core::telemetry::record_physics(
348            stats.broadphase_pairs,
349            stats.contact_points,
350            stats.ccd_impacts,
351        );
352        stats
353    }
354    fn sweep_balls(&mut self) -> usize {
355        let mut hits = 0;
356        for &(id, start_origin, start_rotation) in &self.old_positions {
357            let body = self.bodies[id].core();
358            if !body.ccd || body.kind != Kind::Dynamic {
359                continue;
360            }
361            let mut closest = 1.;
362            let mut correction = None;
363            for &child in &body.colliders {
364                let collider = &self.colliders[child];
365                if collider.sensor {
366                    continue;
367                }
368                let Shape::Ball(radius) = collider.shape else {
369                    continue;
370                };
371                let start = start_origin + start_rotation * collider.local_translation;
372                let end = collider.pose(body).position;
373                for &(other, old_origin, old_rotation) in &self.old_positions {
374                    if other == id || self.joint_contacts_disabled(id, other) {
375                        continue;
376                    }
377                    let target = self.bodies[other].core();
378                    for &target_child in &target.colliders {
379                        let tc = &self.colliders[target_child];
380                        if tc.sensor
381                            || collider.membership & tc.filter == 0
382                            || tc.membership & collider.filter == 0
383                        {
384                            continue;
385                        }
386                        let target_pose = tc.pose(target);
387                        let target_start = old_origin + old_rotation * tc.local_translation;
388                        let relative_start = start - target_start + target_pose.position;
389                        if relative_start.distance_squared(end) < radius * radius * 0.04 {
390                            continue;
391                        }
392                        let swept = Bounds {
393                            min: relative_start.min(end) - Vec3::splat(radius),
394                            max: relative_start.max(end) + Vec3::splat(radius),
395                        };
396                        if !swept.intersects(geometry::bounds(
397                            tc.shape,
398                            target_pose.position,
399                            target_pose.rotation,
400                        )) {
401                            continue;
402                        }
403                        if let Some((t, n)) = geometry::sweep_ball(
404                            relative_start,
405                            end,
406                            radius,
407                            tc.shape,
408                            target_pose.position,
409                            target_pose.rotation,
410                            self.planar,
411                        ) && t < closest
412                            && (end - relative_start).dot(n) < -1e-7
413                        {
414                            closest = t;
415                            correction = Some(
416                                relative_start + (end - relative_start) * t + n * 0.00002 - end,
417                            );
418                        }
419                    }
420                }
421            }
422            if let Some(delta) = correction {
423                self.bodies[id].core_mut().position += delta;
424                hits += 1;
425            }
426        }
427        hits
428    }
429    fn solve_velocities(&mut self, stats: &mut StepStats) {
430        for _ in 0..self.settings.velocity_iterations {
431            for c in &mut self.constraints {
432                let (a, b) = self.bodies.two_mut(c.a, c.b);
433                let a = a.core_mut();
434                let b = b.core_mut();
435                if a.sleeping && b.sleeping
436                    || a.sleeping && b.kind != Kind::Dynamic
437                    || b.sleeping && a.kind != Kind::Dynamic
438                {
439                    continue;
440                }
441                stats.velocity_constraints += 1;
442                let ra = c.point - a.world_com();
443                let rb = c.point - b.world_com();
444                let denom = effective_mass(a, b, ra, rb, c.normal);
445                if denom <= 1e-10 {
446                    continue;
447                }
448                let relative = b.point_velocity(c.point) - a.point_velocity(c.point);
449                let change = (c.bounce - relative.dot(c.normal)) / denom;
450                let accumulated = (c.normal_impulse + change).max(0.);
451                let j = c.normal * (accumulated - c.normal_impulse);
452                c.normal_impulse = accumulated;
453                a.impulse(-j, c.point);
454                b.impulse(j, c.point);
455                let relative = b.point_velocity(c.point) - a.point_velocity(c.point);
456                let tangent = relative - c.normal * relative.dot(c.normal);
457                let mut candidate = c.tangent_impulse;
458                if let Some(t) = tangent.try_normalize() {
459                    let denominator = effective_mass(a, b, ra, rb, t);
460                    if denominator > 1e-10 {
461                        candidate -= t * (relative.dot(t) / denominator);
462                    }
463                }
464                // Re-clamp even at zero slip: a reduced normal load also reduces
465                // the friction cone of an already accumulated tangent impulse.
466                let new = candidate.clamp_length_max(c.friction * c.normal_impulse);
467                let j = new - c.tangent_impulse;
468                c.tangent_impulse = new;
469                a.impulse(-j, c.point);
470                b.impulse(j, c.point);
471            }
472            self.run_joints(JointPhase::Velocity, 0., stats);
473        }
474    }
475    fn solve_positions(&mut self) {
476        for _ in 0..self.settings.position_iterations {
477            for c in &self.constraints {
478                let (a, b) = self.bodies.two_mut(c.a, c.b);
479                let a = a.core_mut();
480                let b = b.core_mut();
481                if a.contact_inv_mass() + b.contact_inv_mass() == 0. {
482                    continue;
483                }
484                let pa = a.position + a.rotation * c.local_a;
485                let pb = b.position + b.rotation * c.local_b;
486                let ra = a.rotation * (c.local_a - a.local_com);
487                let rb = b.rotation * (c.local_b - b.local_com);
488                let depth = -(pb - pa).dot(c.normal);
489                if depth <= 0.0005 {
490                    continue;
491                }
492                let denom = effective_mass(a, b, ra, rb, c.normal);
493                if denom <= 1e-10 {
494                    continue;
495                }
496                let j = c.normal * ((depth - 0.0005).min(0.2) * 0.4 / denom);
497                a.position_impulse(-j, ra.cross(-j));
498                b.position_impulse(j, rb.cross(j));
499            }
500            self.run_joints(JointPhase::Position, 0., &mut StepStats::default());
501        }
502    }
503    pub(crate) fn cast_ray(
504        &self,
505        origin: Vec3,
506        direction: Vec3,
507        max_distance: f32,
508    ) -> Option<(RigidBodyHandle, ColliderHandle, f32, Vec3, Vec3)> {
509        self.cast_sphere(origin, direction, max_distance, 0., u32::MAX, None)
510    }
511    /// Current-pose sphere sweep. Radius zero preserves the raycast behavior.
512    pub(crate) fn cast_sphere(
513        &self,
514        origin: Vec3,
515        direction: Vec3,
516        max_distance: f32,
517        radius: f32,
518        mask: u32,
519        ignore: Option<RigidBodyHandle>,
520    ) -> Option<(RigidBodyHandle, ColliderHandle, f32, Vec3, Vec3)> {
521        if !origin.is_finite()
522            || !direction.is_finite()
523            || !max_distance.is_finite()
524            || max_distance < 0.
525            || !radius.is_finite()
526            || !(0. ..=1000.).contains(&radius)
527        {
528            return None;
529        }
530        let direction = direction.try_normalize()?;
531        let mut best = None;
532        let mut limit = max_distance;
533        for (id, c) in self.colliders.iter() {
534            if c.sensor || c.membership & mask == 0 || c.parent == ignore {
535                continue;
536            }
537            let body = c.parent.unwrap();
538            let b = self.bodies[body].core();
539            let pose = c.pose(b);
540            let (d, n) =
541                geometry::distance(c.shape, pose.position, pose.rotation, origin, self.planar);
542            let hit = if d <= radius {
543                Some((0., n))
544            } else {
545                geometry::sweep_ball(
546                    origin,
547                    origin + direction * limit,
548                    radius,
549                    c.shape,
550                    pose.position,
551                    pose.rotation,
552                    self.planar,
553                )
554                .map(|(t, n)| (t * limit, n))
555            };
556            if let Some((distance, normal)) = hit {
557                limit = distance;
558                best = Some((body, id, distance, origin + direction * distance, normal));
559            }
560        }
561        best
562    }
563}
564fn pair_key(a: ColliderHandle, b: ColliderHandle) -> (ColliderHandle, ColliderHandle) {
565    if a < b { (a, b) } else { (b, a) }
566}
567fn effective_mass(
568    a: &crate::body::Core,
569    b: &crate::body::Core,
570    ra: Vec3,
571    rb: Vec3,
572    n: Vec3,
573) -> f32 {
574    a.contact_inv_mass()
575        + b.contact_inv_mass()
576        + n.dot(
577            a.contact_inverse_inertia(ra.cross(n)).cross(ra)
578                + b.contact_inverse_inertia(rb.cross(n)).cross(rb),
579        )
580}
581fn rotation_delta(from: Quat, to: Quat) -> Vec3 {
582    let mut delta = (to * from.conjugate()).normalize();
583    if delta.w < 0. {
584        delta = -delta;
585    }
586    let axis = Vec3::new(delta.x, delta.y, delta.z);
587    let length = axis.length();
588    if length < 1e-9 {
589        Vec3::ZERO
590    } else {
591        axis * (2. * length.atan2(delta.w) / length)
592    }
593}
ferravex_physics/world.rs

ferravex_physics/
world.rs
