1use glam::Vec4;
2use loam::math::{Bivector4, EuclideanR4, Rotor, Rotor4};
3use loam::physics::euclidean_r4::{
4 halfspace4_body_r4, polytope_body_r4, register_default_narrowphase, regular_polytope4_inertia,
5};
6use loam::physics::manifold::PENETRATION_SLOP;
7use loam::physics::{Collider, EditError, RigidBody};
8use loam::runtime::{
9 Dispatch, DomainError, DomainHandle, Domains, Entity, GrabConfig, Instance, Physics,
10 PhysicsConfig, Pose, Rejection, Session, SpawnBundle, Step,
11};
12use loam::shape::polytope::Polytope4;
13
14use crate::catalog::{ShapeEntry, SHAPE_CATALOG};
15use crate::consts::{FLOOR_Y, GRAVITY};
16use crate::{Card, HiddenSlot, Playground, Slot, Toy, Wall};
17
18pub(crate) const BODY_SIZE: f32 = 0.45;
19pub(crate) const ARENA_HALF: f32 = 3.6;
20const ARENA_TOP: f32 = FLOOR_Y + 2.0 * ARENA_HALF;
21
22const BODY_MASS: f32 = 1.0;
23const BODY_RESTITUTION: f32 = 0.05;
24const WALL_RESTITUTION: f32 = 0.4;
25const PHYSICS_FLOOR_Y: f32 = FLOOR_Y + 2.0 * PENETRATION_SLOP;
26const SPAWN_SPACING: f32 = 1.4;
27const SPAWN_CLEARANCE: f32 = 0.20;
28const TICK_DT: f32 = 1.0 / 60.0;
29const BASE_SUBSTEPS: u32 = 4;
30const MAX_SUBSTEPS: u32 = 16;
31const STEP_TRAVEL_BUDGET: f32 = 0.135;
32const GRAB_STIFFNESS: f32 = 40.0;
33const MAX_CARRY_SPEED: f32 = 35.0;
34const MAX_GRAB_ACCELERATION: f32 = 800.0;
35const RELEASE_GAIN: f32 = 0.3;
36const MAX_RELEASE_SPEED: f32 = 0.5 * STEP_TRAVEL_BUDGET / (TICK_DT / MAX_SUBSTEPS as f32);
37const RELEASE_SPIN_GAIN: f32 = 0.35;
38const MAX_ANGULAR_SPEED: f32 =
39 0.5 * STEP_TRAVEL_BUDGET / (BODY_SIZE * (TICK_DT / MAX_SUBSTEPS as f32));
40const ANGULAR_DAMPING: f32 = 1.2;
41const REST_TRAVEL: f32 = 0.02;
42const REST_WINDOW: f32 = 30.0 * TICK_DT;
43const REST_SPEED: f32 = 0.15;
44const REST_ANGULAR_SPEED: f32 = 0.3;
45const TOYS: [Polytope4; 5] = [
46 Polytope4::Cell24,
47 Polytope4::Tesseract,
48 Polytope4::Pentatope,
49 Polytope4::Cell16,
50 Polytope4::Tesseract,
51];
52
53fn motion_speed(body: &RigidBody<EuclideanR4>) -> f32 {
54 body.velocity.length() + body.angular_velocity.magnitude() * BODY_SIZE
55}
56
57pub(crate) fn physics_config() -> PhysicsConfig<EuclideanR4> {
58 PhysicsConfig::new(register_default_narrowphase)
59 .gravity(Vec4::NEG_Y * GRAVITY)
60 .grab(
61 GrabConfig::new(GRAB_STIFFNESS, MAX_CARRY_SPEED, MAX_GRAB_ACCELERATION)
62 .anchor_point(grab_anchor)
63 .constrain_target(clamp_target_to_arena),
64 )
65 .adaptive_substeps(
66 BASE_SUBSTEPS,
67 MAX_SUBSTEPS,
68 STEP_TRAVEL_BUDGET,
69 motion_speed,
70 )
71}
72
73fn grab_anchor(body: Vec4, picked: Vec4) -> Vec4 {
74 Vec4::new(picked.x, picked.y, picked.z, body.w)
75}
76
77fn clamp_target_to_arena(current: Vec4, target: Vec4) -> Vec4 {
78 let reach = ARENA_HALF - BODY_SIZE;
79 Vec4::new(
80 target.x.clamp(-reach, reach),
81 target
82 .y
83 .clamp(PHYSICS_FLOOR_Y + BODY_SIZE, ARENA_TOP - BODY_SIZE),
84 target.z.clamp(-reach, reach),
85 current.w,
86 )
87}
88
89pub(crate) fn release(
90 physics: &mut Physics<EuclideanR4>,
91 entity: Entity,
92 pointer: [f32; 3],
93 rope: bool,
94) -> Result<(), EditError> {
95 let body = *physics
96 .world()
97 .body(physics.body(entity).ok_or(EditError::StaleHandle)?)
98 .ok_or(EditError::StaleHandle)?;
99 let mut linear = body.velocity;
100 let mut angular = body.angular_velocity;
101 if !rope {
102 let flick = Vec4::new(pointer[0], pointer[1], pointer[2], 0.0);
103 let throw = flick * RELEASE_GAIN;
104 linear = throw * (MAX_RELEASE_SPEED / throw.length().max(MAX_RELEASE_SPEED))
105 + Vec4::W * body.velocity.w;
106 let lever = physics
107 .released_anchor(entity)
108 .and_then(|handle| (handle - body.position).truncate().try_normalize());
109 if let Some(lever) = lever {
110 angular = angular
111 + Bivector4::wedge(lever.extend(0.0), flick) * (RELEASE_SPIN_GAIN / BODY_SIZE);
112 }
113 }
114 let speed = angular.magnitude();
115 if speed > MAX_ANGULAR_SPEED {
116 angular = angular * (MAX_ANGULAR_SPEED / speed);
117 }
118 physics.set_velocity(entity, linear, angular)
119}
120
121#[derive(Clone, Copy, Debug, PartialEq)]
122pub(crate) struct DepthBand {
123 pub(crate) entity: Entity,
124 pub(crate) index: usize,
125 pub(crate) center: f32,
126 pub(crate) min: f32,
127 pub(crate) max: f32,
128 pub(crate) color: [f32; 3],
129 pub(crate) label: &'static str,
130 pub(crate) asleep: bool,
131}
132
133fn depth_bounds(position: Vec4, rotation: Rotor4, vertices: &[Vec4]) -> (f32, f32) {
134 let mut min = f32::INFINITY;
135 let mut max = f32::NEG_INFINITY;
136 for vertex in vertices {
137 let w = (position + rotation.apply(*vertex)).w;
138 min = min.min(w);
139 max = max.max(w);
140 }
141 (min, max)
142}
143
144pub(crate) fn fill_depth_bands(
145 session: &Session<Playground>,
146 domain: DomainHandle<EuclideanR4>,
147 alpha: f32,
148 out: &mut Vec<DepthBand>,
149) {
150 out.clear();
151 for (entity, _) in session.app.toys.iter() {
152 let Some(slot) = session.app.slots.get(entity) else {
153 continue;
154 };
155 out.push(DepthBand {
156 entity,
157 index: slot.index,
158 center: 0.0,
159 min: 0.0,
160 max: 0.0,
161 color: slot.entry.body_color,
162 label: slot.entry.label,
163 asleep: false,
164 });
165 }
166 let Ok(r4) = session.domains().read(domain) else {
167 out.clear();
168 return;
169 };
170 let Some(physics) = r4.physics() else {
171 out.clear();
172 return;
173 };
174 out.retain_mut(|band| {
175 let Some(id) = physics.body(band.entity) else {
176 return false;
177 };
178 let Some(body) = physics.world().body(id) else {
179 return false;
180 };
181 let Some(Collider::ConvexPolytope4D { vertices }) = physics.world().collider(body) else {
182 return false;
183 };
184 let Some(pose) = r4.drawn_pose(band.entity, alpha) else {
185 return false;
186 };
187 band.center = pose.point.w;
188 (band.min, band.max) = depth_bounds(pose.point, pose.frame, vertices);
189 band.asleep = body.is_sleeping();
190 true
191 });
192 out.sort_by_key(|band| band.index);
193}
194
195fn arena() -> [(Vec4, f32, f32); 8] {
196 [
197 (Vec4::Y, PHYSICS_FLOOR_Y, BODY_RESTITUTION),
198 (-Vec4::Y, -ARENA_TOP, WALL_RESTITUTION),
199 (Vec4::X, -ARENA_HALF, WALL_RESTITUTION),
200 (-Vec4::X, -ARENA_HALF, WALL_RESTITUTION),
201 (Vec4::Z, -ARENA_HALF, WALL_RESTITUTION),
202 (-Vec4::Z, -ARENA_HALF, WALL_RESTITUTION),
203 (Vec4::W, -ARENA_HALF, WALL_RESTITUTION),
204 (-Vec4::W, -ARENA_HALF, WALL_RESTITUTION),
205 ]
206}
207
208fn face_down(polytope: Polytope4) -> Rotor4 {
209 let topology = polytope.topology();
210 let mut centroid = Vec4::ZERO;
211 for index in topology.cells[0] {
212 centroid += topology.vertices[*index as usize];
213 }
214 Rotor4::from_rotation_arc(centroid.normalize(), -Vec4::Y)
215}
216
217pub(crate) fn pose_at(polytope: Polytope4, x: f32) -> Pose<EuclideanR4> {
218 let frame = face_down(polytope);
219 let lowest = polytope
220 .topology()
221 .vertices
222 .iter()
223 .map(|vertex| frame.apply(*vertex * BODY_SIZE).y)
224 .fold(f32::INFINITY, f32::min);
225 Pose {
226 point: Vec4::new(x, SPAWN_CLEARANCE - lowest, 0.0, 0.0),
227 frame,
228 }
229}
230
231fn asset_of(polytope: Polytope4, cards: &[Card]) -> Option<(ShapeEntry, Instance)> {
232 SHAPE_CATALOG
233 .iter()
234 .position(|entry| entry.collider_polytope() == Some(polytope))
235 .and_then(|index| Some((SHAPE_CATALOG[index], cards.get(index)?.toy()?)))
236}
237
238pub(crate) fn add_body(
239 dispatch: &mut Dispatch<'_, Playground>,
240 domain: DomainHandle<EuclideanR4>,
241 entity: Entity,
242 polytope: Polytope4,
243 pose: Pose<EuclideanR4>,
244) -> Result<(), Rejection> {
245 let vertices = polytope
246 .topology()
247 .vertices
248 .iter()
249 .map(|vertex| *vertex * BODY_SIZE)
250 .collect();
251 let body = polytope_body_r4(pose.point, Vec4::ZERO, vertices, BODY_MASS)
252 .ok_or(Rejection::Unsupported("invalid toy body"))?;
253 let domain = dispatch.domains.typed(domain)?;
254 domain.spawn_body(entity, body)?;
255 let physics = domain
256 .physics_mut()
257 .ok_or(Rejection::Unsupported("the domain has no physics"))?;
258 physics.set_mass_properties(
259 entity,
260 BODY_MASS,
261 regular_polytope4_inertia(polytope, BODY_MASS, BODY_SIZE),
262 )?;
263 physics.set_restitution(entity, BODY_RESTITUTION)?;
264 Ok(())
265}
266
267fn spawn_toy(
268 dispatch: &mut Dispatch<'_, Playground>,
269 domain: DomainHandle<EuclideanR4>,
270 polytope: Polytope4,
271 entry: ShapeEntry,
272 instance: Instance,
273 at: usize,
274 count: usize,
275) -> Result<(), Rejection> {
276 let centre = count.saturating_sub(1) as f32 * 0.5;
277 let pose = pose_at(polytope, (at as f32 - centre) * SPAWN_SPACING);
278 let entity = dispatch.spawn(
279 SpawnBundle::new()
280 .at(domain, pose)
281 .instance(instance)
282 .row(Slot {
283 index: at,
284 entry,
285 rest: pose.point,
286 })
287 .row(Toy {
288 rest_anchor: pose.point,
289 rest_time: 0.0,
290 }),
291 )?;
292 add_body(dispatch, domain, entity, polytope, pose)
293}
294
295fn spawn_walls(
296 dispatch: &mut Dispatch<'_, Playground>,
297 domain: DomainHandle<EuclideanR4>,
298) -> Result<(), Rejection> {
299 for (normal, offset, restitution) in arena() {
300 let plane = halfspace4_body_r4(normal, offset)
301 .ok_or(Rejection::Unsupported("invalid arena plane"))?;
302 let entity = dispatch.spawn(
303 SpawnBundle::new()
304 .at(domain, Pose::at(Vec4::ZERO))
305 .row(Wall),
306 )?;
307 let domain = dispatch.domains.typed(domain)?;
308 domain.spawn_body(entity, plane)?;
309 domain
310 .physics_mut()
311 .ok_or(Rejection::Unsupported("the domain has no physics"))?
312 .set_restitution(entity, restitution)?;
313 }
314 Ok(())
315}
316
317pub(crate) fn populate(
318 dispatch: &mut Dispatch<'_, Playground>,
319 domain: DomainHandle<EuclideanR4>,
320 cards: &[Card],
321) -> Result<(), Rejection> {
322 let visible: Vec<(Entity, Slot)> = dispatch
323 .app
324 .slots
325 .iter()
326 .map(|(entity, slot)| (entity, *slot))
327 .collect();
328 for (entity, slot) in visible {
329 let instance = dispatch
330 .domains
331 .typed(domain)?
332 .instances()
333 .get(entity)
334 .copied();
335 if instance.is_some() {
336 dispatch.domains.typed(domain)?.remove_instance(entity)?;
337 }
338 dispatch.app.slots.remove(entity)?;
339 dispatch.attach(entity, HiddenSlot { slot, instance })?;
340 }
341
342 for (at, polytope) in TOYS.into_iter().enumerate() {
343 let (entry, instance) = asset_of(polytope, cards)
344 .ok_or(Rejection::Unsupported("the toy has no prepared asset"))?;
345 spawn_toy(dispatch, domain, polytope, entry, instance, at, TOYS.len())?;
346 }
347 spawn_walls(dispatch, domain)
348}
349
350pub(crate) fn clear(
351 dispatch: &mut Dispatch<'_, Playground>,
352 domain: DomainHandle<EuclideanR4>,
353) -> Result<(), Rejection> {
354 let spawned: Vec<Entity> = dispatch
355 .app
356 .toys
357 .iter()
358 .map(|(entity, _)| entity)
359 .chain(dispatch.app.walls.iter().map(|(entity, _)| entity))
360 .collect();
361 for entity in spawned {
362 dispatch.despawn(entity)?;
363 }
364
365 let hidden: Vec<(Entity, HiddenSlot)> = dispatch
366 .app
367 .hidden
368 .iter()
369 .map(|(entity, row)| (entity, *row))
370 .collect();
371 for (entity, row) in hidden {
372 dispatch.app.hidden.remove(entity)?;
373 dispatch.attach(entity, row.slot)?;
374 if let Some(instance) = row.instance {
375 dispatch.attach_instance(domain, entity, instance)?;
376 }
377 }
378 Ok(())
379}
380
381pub(crate) fn reset(
382 dispatch: &mut Dispatch<'_, Playground>,
383 domain: DomainHandle<EuclideanR4>,
384 cards: &[Card],
385) -> Result<(), Rejection> {
386 clear(dispatch, domain)?;
387 populate(dispatch, domain, cards)
388}
389
390pub(crate) fn settle(
391 app: &mut Playground,
392 domains: &mut Domains,
393 domain: DomainHandle<EuclideanR4>,
394 step: Step,
395) -> Result<(), DomainError> {
396 let r4 = domains.typed(domain)?;
397 let physics = r4
398 .physics_mut()
399 .ok_or(DomainError::Unsupported("physics on r4"))?;
400 let decay = (-ANGULAR_DAMPING * step.dt).exp();
401 for (entity, toy) in app.toys.iter_mut() {
402 let Some(id) = physics.body(entity) else {
403 continue;
404 };
405 let held = physics.is_held(entity);
406 let Some(mut body) = physics.world().body(id).copied() else {
407 continue;
408 };
409 let angular = body.angular_velocity * decay;
410 if angular != body.angular_velocity {
411 physics.set_velocity(entity, body.velocity, angular)?;
412 body.angular_velocity = angular;
413 }
414 if held {
415 toy.rest_anchor = body.position;
416 toy.rest_time = 0.0;
417 continue;
418 }
419 if !body.is_sleeping() && toy.rest_time >= REST_WINDOW {
420 toy.rest_anchor = body.position;
421 toy.rest_time = 0.0;
422 }
423 let travelled = (body.position - toy.rest_anchor).length();
424 let moving = body.velocity.length() > REST_SPEED
425 || body.angular_velocity.magnitude() > REST_ANGULAR_SPEED;
426 if travelled > REST_TRAVEL || moving {
427 toy.rest_anchor = body.position;
428 toy.rest_time = 0.0;
429 } else {
430 toy.rest_time += step.dt;
431 if toy.rest_time >= REST_WINDOW {
432 physics.sleep_body(entity)?;
433 }
434 }
435 }
436 Ok(())
437}
438
439#[cfg(test)]
440mod tests {
441 use super::*;
442
443 #[test]
444 fn a_drag_target_cannot_pull_the_toy_hull_through_the_arena() {
445 let current = Vec4::new(0.0, 1.0, 0.0, 2.0);
446 let rotation =
447 Rotor4::from_rotation_arc(Vec4::X, Vec4::new(1.0, 1.0, 0.0, 1.0).normalize());
448 for far in [
449 Vec4::new(99.0, -99.0, -99.0, -8.0),
450 Vec4::new(-99.0, 99.0, 99.0, 8.0),
451 ] {
452 let target = clamp_target_to_arena(current, far);
453 for vertex in Polytope4::Tesseract.topology().vertices {
454 let point = target + rotation.apply(*vertex * BODY_SIZE);
455 assert!(point.x.abs() <= ARENA_HALF + 1e-6);
456 assert!(point.z.abs() <= ARENA_HALF + 1e-6);
457 assert!(point.y >= FLOOR_Y - 1e-6);
458 assert!(point.y <= ARENA_TOP + 1e-6);
459 }
460 assert_eq!(target.w, current.w);
461 }
462 }
463
464 #[test]
465 fn a_depth_band_uses_the_oriented_hull_w_extent() {
466 let position = Vec4::new(0.0, 0.0, 0.0, 0.7);
467 let vertices = [Vec4::X * BODY_SIZE, -Vec4::X * BODY_SIZE];
468 let flat = depth_bounds(position, Rotor4::IDENTITY, &vertices);
469 let turned = depth_bounds(
470 position,
471 Rotor4::from_rotation_arc(Vec4::X, Vec4::W),
472 &vertices,
473 );
474 assert_eq!(flat, (position.w, position.w));
475 assert!((turned.0 - (position.w - BODY_SIZE)).abs() < 1e-6);
476 assert!((turned.1 - (position.w + BODY_SIZE)).abs() < 1e-6);
477 }
478}