alice_physics/
multi_world.rs

1//! Multiple Independent Physics Worlds
2//!
3//! Manages several independent physics worlds that can run simultaneously.
4//! Supports transferring rigid bodies between worlds and portal connections.
5//!
6//! # Features
7//!
8//! - Create and manage multiple independent physics worlds
9//! - Step all worlds sequentially or in parallel (`parallel` feature)
10//! - Transfer bodies between worlds with position remapping
11//! - Portal connections between world pairs
12
13use crate::math::{Fix128, QuatFix, Vec3Fix};
14use crate::solver::{PhysicsConfig, PhysicsWorld};
15
16#[cfg(not(feature = "std"))]
17use alloc::vec::Vec;
18
19#[cfg(feature = "parallel")]
20use rayon::prelude::*;
21
22// ============================================================================
23// Multi-World Types
24// ============================================================================
25
26/// Container managing multiple independent physics worlds.
27pub struct MultiWorld {
28    /// All physics worlds
29    pub worlds: Vec<PhysicsWorld>,
30}
31
32/// A portal connecting two worlds.
33///
34/// Objects passing through the portal in world A appear in world B
35/// at a transformed position/orientation, and vice versa.
36#[derive(Clone, Debug)]
37pub struct Portal {
38    /// Index of the first world
39    pub world_a: usize,
40    /// Index of the second world
41    pub world_b: usize,
42    /// Transform applied when crossing from A to B: (translation, rotation)
43    pub transform: (Vec3Fix, QuatFix),
44}
45
46impl MultiWorld {
47    /// Create a new empty multi-world container.
48    #[must_use]
49    pub const fn new() -> Self {
50        Self { worlds: Vec::new() }
51    }
52
53    /// Add a new physics world with the given configuration.
54    ///
55    /// Returns the index of the newly added world.
56    pub fn add_world(&mut self, config: PhysicsConfig) -> usize {
57        let idx = self.worlds.len();
58        self.worlds.push(PhysicsWorld::new(config));
59        idx
60    }
61
62    /// Step all worlds sequentially.
63    ///
64    /// Each world is advanced by `dt` independently.
65    pub fn step_all(&mut self, dt: Fix128) {
66        for world in &mut self.worlds {
67            world.step(dt);
68        }
69    }
70
71    /// Step all worlds in parallel using Rayon.
72    ///
73    /// Each world is advanced by `dt` independently on separate threads.
74    /// Requires the `parallel` feature.
75    #[cfg(feature = "parallel")]
76    pub fn step_all_parallel(&mut self, dt: Fix128) {
77        self.worlds.par_iter_mut().for_each(|world| {
78            world.step(dt);
79        });
80    }
81
82    /// Transfer a rigid body from one world to another.
83    ///
84    /// Removes the body from `from_world` and adds it to `to_world`
85    /// at the given `new_position`. Returns the body's new index in
86    /// the destination world, or `None` if the indices are invalid.
87    ///
88    /// # Arguments
89    ///
90    /// - `from_world`: Source world index
91    /// - `body_id`: Body index within the source world
92    /// - `to_world`: Destination world index
93    /// - `new_position`: Position in the destination world
94    pub fn transfer_body(
95        &mut self,
96        from_world: usize,
97        body_id: usize,
98        to_world: usize,
99        new_position: Vec3Fix,
100    ) -> Option<usize> {
101        if from_world >= self.worlds.len() || to_world >= self.worlds.len() {
102            return None;
103        }
104        if from_world == to_world {
105            return None;
106        }
107
108        // Remove the body with its per-body settings (material, collision
109        // radius, collider, filter) so it collides in the destination exactly
110        // like a body added there with them
111        let (mut body, attachments) =
112            self.worlds[from_world].remove_body_with_attachments(body_id)?;
113
114        // Update position for destination world
115        body.position = new_position;
116        body.prev_position = new_position;
117
118        let new_id = self.worlds[to_world].add_body_with_attachments(body, attachments);
119
120        Some(new_id)
121    }
122
123    /// Get the number of worlds.
124    #[must_use]
125    pub fn world_count(&self) -> usize {
126        self.worlds.len()
127    }
128
129    /// Get the total number of bodies across all worlds.
130    #[must_use]
131    pub fn total_body_count(&self) -> usize {
132        self.worlds.iter().map(|w| w.bodies.len()).sum()
133    }
134}
135
136impl Default for MultiWorld {
137    fn default() -> Self {
138        Self::new()
139    }
140}
141
142impl core::fmt::Debug for MultiWorld {
143    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
144        f.debug_struct("MultiWorld")
145            .field("worlds", &self.worlds.len())
146            .finish()
147    }
148}
149
150impl Portal {
151    /// Create a new portal between two worlds.
152    #[must_use]
153    pub const fn new(
154        world_a: usize,
155        world_b: usize,
156        translation: Vec3Fix,
157        rotation: QuatFix,
158    ) -> Self {
159        Self {
160            world_a,
161            world_b,
162            transform: (translation, rotation),
163        }
164    }
165
166    /// Transform a position from world A coordinates to world B coordinates.
167    #[must_use]
168    pub fn transform_a_to_b(&self, position: Vec3Fix) -> Vec3Fix {
169        let (translation, rotation) = &self.transform;
170        // a non-unit rotation would scale the position (AUD-A-S4W1-004)
171        rotation.unit_rotation().rotate_vec(position) + *translation
172    }
173
174    /// Transform a position from world B coordinates to world A coordinates.
175    #[must_use]
176    pub fn transform_b_to_a(&self, position: Vec3Fix) -> Vec3Fix {
177        let (translation, rotation) = &self.transform;
178        let inv_rot = rotation.unit_rotation().conjugate();
179        inv_rot.rotate_vec(position - *translation)
180    }
181}
182
183// ============================================================================
184// Tests
185// ============================================================================
186
187#[cfg(all(test, feature = "std"))]
188mod tests {
189    use super::*;
190    use crate::solver::RigidBody;
191
192    #[test]
193    fn test_new_multi_world() {
194        let mw = MultiWorld::new();
195        assert_eq!(mw.world_count(), 0);
196    }
197
198    #[test]
199    fn test_default_multi_world() {
200        let mw = MultiWorld::default();
201        assert_eq!(mw.world_count(), 0);
202    }
203
204    #[test]
205    fn test_add_world() {
206        let mut mw = MultiWorld::new();
207        let idx = mw.add_world(PhysicsConfig::default());
208        assert_eq!(idx, 0);
209        assert_eq!(mw.world_count(), 1);
210    }
211
212    #[test]
213    fn test_add_multiple_worlds() {
214        let mut mw = MultiWorld::new();
215        let idx0 = mw.add_world(PhysicsConfig::default());
216        let idx1 = mw.add_world(PhysicsConfig::default());
217        let idx2 = mw.add_world(PhysicsConfig::default());
218        assert_eq!(idx0, 0);
219        assert_eq!(idx1, 1);
220        assert_eq!(idx2, 2);
221        assert_eq!(mw.world_count(), 3);
222    }
223
224    #[test]
225    fn test_step_all_empty() {
226        let mut mw = MultiWorld::new();
227        mw.step_all(Fix128::from_ratio(1, 60));
228        // Should not panic
229    }
230
231    #[test]
232    fn test_step_all_with_body() {
233        let mut mw = MultiWorld::new();
234        mw.add_world(PhysicsConfig::default());
235
236        let body = RigidBody::new_dynamic(Vec3Fix::from_int(0, 10, 0), Fix128::ONE);
237        mw.worlds[0].add_body(body);
238
239        let dt = Fix128::from_ratio(1, 60);
240        mw.step_all(dt);
241
242        // Body should have moved under gravity
243        let pos = mw.worlds[0].bodies[0].position;
244        assert!(pos.y < Fix128::from_int(10));
245    }
246
247    #[test]
248    fn test_transfer_body() {
249        let mut mw = MultiWorld::new();
250        mw.add_world(PhysicsConfig::default());
251        mw.add_world(PhysicsConfig::default());
252
253        let body = RigidBody::new_dynamic(Vec3Fix::from_int(0, 5, 0), Fix128::ONE);
254        mw.worlds[0].add_body(body);
255
256        assert_eq!(mw.worlds[0].bodies.len(), 1);
257        assert_eq!(mw.worlds[1].bodies.len(), 0);
258
259        let new_pos = Vec3Fix::from_int(10, 20, 30);
260        let new_id = mw.transfer_body(0, 0, 1, new_pos);
261
262        assert!(new_id.is_some());
263        assert_eq!(new_id.unwrap(), 0);
264        assert_eq!(mw.worlds[0].bodies.len(), 0);
265        assert_eq!(mw.worlds[1].bodies.len(), 1);
266        assert_eq!(mw.worlds[1].bodies[0].position.x.hi, 10);
267        assert_eq!(mw.worlds[1].bodies[0].position.y.hi, 20);
268    }
269
270    #[test]
271    fn test_transfer_invalid_world() {
272        let mut mw = MultiWorld::new();
273        mw.add_world(PhysicsConfig::default());
274
275        let result = mw.transfer_body(0, 0, 99, Vec3Fix::ZERO);
276        assert!(result.is_none());
277    }
278
279    #[test]
280    fn test_transfer_same_world() {
281        let mut mw = MultiWorld::new();
282        mw.add_world(PhysicsConfig::default());
283
284        let body = RigidBody::new_dynamic(Vec3Fix::ZERO, Fix128::ONE);
285        mw.worlds[0].add_body(body);
286
287        let result = mw.transfer_body(0, 0, 0, Vec3Fix::ZERO);
288        assert!(result.is_none());
289    }
290
291    #[test]
292    fn test_total_body_count() {
293        let mut mw = MultiWorld::new();
294        mw.add_world(PhysicsConfig::default());
295        mw.add_world(PhysicsConfig::default());
296
297        mw.worlds[0].add_body(RigidBody::new_dynamic(Vec3Fix::ZERO, Fix128::ONE));
298        mw.worlds[0].add_body(RigidBody::new_dynamic(Vec3Fix::ZERO, Fix128::ONE));
299        mw.worlds[1].add_body(RigidBody::new_dynamic(Vec3Fix::ZERO, Fix128::ONE));
300
301        assert_eq!(mw.total_body_count(), 3);
302    }
303
304    #[test]
305    fn test_portal_transform_identity() {
306        let portal = Portal::new(0, 1, Vec3Fix::ZERO, QuatFix::IDENTITY);
307        let pos = Vec3Fix::from_int(5, 10, 15);
308        let transformed = portal.transform_a_to_b(pos);
309        assert_eq!(transformed.x.hi, 5);
310        assert_eq!(transformed.y.hi, 10);
311        assert_eq!(transformed.z.hi, 15);
312    }
313
314    #[test]
315    fn test_portal_transform_translation() {
316        let portal = Portal::new(0, 1, Vec3Fix::from_int(100, 0, 0), QuatFix::IDENTITY);
317        let pos = Vec3Fix::from_int(5, 10, 15);
318        let transformed = portal.transform_a_to_b(pos);
319        assert_eq!(transformed.x.hi, 105);
320        assert_eq!(transformed.y.hi, 10);
321    }
322
323    #[test]
324    fn test_portal_roundtrip() {
325        let portal = Portal::new(0, 1, Vec3Fix::from_int(50, -20, 10), QuatFix::IDENTITY);
326        let pos = Vec3Fix::from_int(7, 3, -5);
327        let to_b = portal.transform_a_to_b(pos);
328        let back = portal.transform_b_to_a(to_b);
329
330        let eps = Fix128::from_ratio(1, 1000);
331        assert!((back.x - pos.x).abs() < eps);
332        assert!((back.y - pos.y).abs() < eps);
333        assert!((back.z - pos.z).abs() < eps);
334    }
335
336    /// 3 world (それぞれ異なる gravity / 衝突 / joint 構成) を同じ内容で 2 組作る
337    #[cfg(feature = "parallel")]
338    fn build_scene() -> MultiWorld {
339        let mut mw = MultiWorld::new();
340        // world 0: 既定重力、球 2 個が衝突する
341        let w0 = mw.add_world(PhysicsConfig::default());
342        mw.worlds[w0].add_body_with_radius(
343            RigidBody::new_dynamic(Vec3Fix::from_int(0, 4, 0), Fix128::ONE),
344            Fix128::ONE,
345        );
346        mw.worlds[w0].add_body_with_radius(RigidBody::new_static(Vec3Fix::ZERO), Fix128::ONE);
347        // world 1: 横向き重力、3 body が距離拘束で連結
348        let w1 = mw.add_world(PhysicsConfig {
349            gravity: Vec3Fix::from_int(3, 0, 0),
350            ..PhysicsConfig::default()
351        });
352        let a = mw.worlds[w1].add_body(RigidBody::new_static(Vec3Fix::ZERO));
353        let b = mw.worlds[w1].add_body(RigidBody::new_dynamic(
354            Vec3Fix::from_int(0, -2, 0),
355            Fix128::ONE,
356        ));
357        let c = mw.worlds[w1].add_body(RigidBody::new_dynamic(
358            Vec3Fix::from_int(0, -4, 0),
359            Fix128::from_int(2),
360        ));
361        mw.worlds[w1].add_distance_constraint(crate::solver::DistanceConstraint::new(
362            a,
363            b,
364            Vec3Fix::ZERO,
365            Vec3Fix::ZERO,
366            Fix128::from_int(2),
367        ));
368        mw.worlds[w1].add_distance_constraint(crate::solver::DistanceConstraint::new(
369            b,
370            c,
371            Vec3Fix::ZERO,
372            Vec3Fix::ZERO,
373            Fix128::from_int(2),
374        ));
375        // world 2: 空 (step が no-op でも壊れない)
376        mw.add_world(PhysicsConfig::default());
377        mw
378    }
379
380    #[cfg(feature = "parallel")]
381    #[test]
382    fn step_all_parallel_is_bit_identical_to_sequential_step_all() {
383        let mut seq = build_scene();
384        let mut par = build_scene();
385        let dt = Fix128::from_ratio(1, 60);
386        for frame in 0..30 {
387            seq.step_all(dt);
388            par.step_all_parallel(dt);
389            assert_eq!(seq.world_count(), par.world_count());
390            for (w, (ws, wp)) in seq.worlds.iter().zip(par.worlds.iter()).enumerate() {
391                assert_eq!(ws.bodies.len(), wp.bodies.len(), "frame {frame} world {w}");
392                for (i, (bs, bp)) in ws.bodies.iter().zip(wp.bodies.iter()).enumerate() {
393                    assert_eq!(bs.position, bp.position, "frame {frame} world {w} body {i}");
394                    assert_eq!(bs.velocity, bp.velocity, "frame {frame} world {w} body {i}");
395                    assert_eq!(bs.rotation, bp.rotation, "frame {frame} world {w} body {i}");
396                    assert_eq!(
397                        bs.angular_velocity, bp.angular_velocity,
398                        "frame {frame} world {w} body {i}"
399                    );
400                }
401                assert_eq!(
402                    ws.contact_events().len(),
403                    wp.contact_events().len(),
404                    "frame {frame} world {w}"
405                );
406            }
407        }
408        // 実際に何かが動いた (自明な一致ではない): world 0 は落下して球に接触、world 1 は +x へ振れる
409        let w0 = &par.worlds[0].bodies[0].position;
410        assert!(w0.y < Fix128::from_int(4) && w0.y > Fix128::ONE, "{w0:?}");
411        assert!(par.worlds[1].bodies[2].position.x > Fix128::ZERO);
412        // world 間の独立性: world 2 は空のまま、world 0 の body 数も不変
413        assert_eq!(par.worlds[2].bodies.len(), 0);
414        assert_eq!(par.total_body_count(), 5);
415        // 空 MultiWorld でも parallel step は no-op
416        let mut empty = MultiWorld::new();
417        empty.step_all_parallel(dt);
418        assert_eq!(empty.world_count(), 0);
419    }
420}
alice_physics/multi_world.rs

alice_physics/
multi_world.rs
