1use super::World;
2use super::commands::BodyCommand;
3use super::ids::IdSpace;
4use bytemuck::Zeroable;
5use dynamis_abi::{
6 BODY_CCD, BODY_KINEMATIC, BodyDescriptorRecord, BodyStateRecord, ColliderRecord,
7 OVERRIDE_SLEEP_ANGULAR, OVERRIDE_SLEEP_LINEAR, PATCH_ANGULAR_VELOCITY, PATCH_ORIENTATION,
8 PATCH_POSITION, PATCH_VELOCITY,
9};
10use dynamis_model::{
11 BodyDesc, BodyHandle, BodyState, ColliderDesc, ContactEventMode, MassProperties, Shape,
12};
13
14pub(crate) const NEVER_REPORTED: u64 = u64::MAX;
15
16#[derive(Clone)]
17pub(crate) struct Bodies {
18 pub(crate) alive: Vec<BodyHandle>,
19 pub(crate) ids: IdSpace,
20 pub(crate) index_of: Vec<u32>,
21 pub(crate) collider_descs: Vec<Vec<ColliderDesc>>,
22 pub(crate) masses: Vec<f32>,
23 pub(crate) com_overrides: Vec<Option<[f32; 3]>>,
24 pub(crate) inertia_overrides: Vec<Option<[f32; 6]>>,
25 pub(crate) descriptors: Vec<BodyDescriptorRecord>,
26 pub(crate) dynamic_count: usize,
27 pub(crate) kinematic: Vec<bool>,
28 pub(crate) ccd_count: u32,
29 pub(crate) states: Vec<Option<BodyState>>,
30 pub(crate) covered: Vec<u64>,
31 pub(crate) device_count: u32,
32 pub(crate) commands: Vec<BodyCommand>,
33 pub(crate) dirty: Vec<u32>,
34 pub(crate) last_moves: u32,
35 pub(crate) last_edits: u32,
36}
37
38impl Bodies {
39 pub(crate) const fn new() -> Self {
40 Self {
41 alive: Vec::new(),
42 ids: IdSpace::new(),
43 index_of: Vec::new(),
44 collider_descs: Vec::new(),
45 masses: Vec::new(),
46 com_overrides: Vec::new(),
47 inertia_overrides: Vec::new(),
48 descriptors: Vec::new(),
49 dynamic_count: 0,
50 kinematic: Vec::new(),
51 ccd_count: 0,
52 states: Vec::new(),
53 covered: Vec::new(),
54 device_count: 0,
55 commands: Vec::new(),
56 dirty: Vec::new(),
57 last_moves: 0,
58 last_edits: 0,
59 }
60 }
61
62 fn grow_to(&mut self, id: u32) {
63 let rows = id as usize + 1;
64 if rows <= self.index_of.len() {
65 return;
66 }
67 self.index_of.resize(rows, u32::MAX);
68 self.collider_descs.resize(rows, Vec::new());
69 self.masses.resize(rows, 1.0);
70 self.com_overrides.resize(rows, None);
71 self.inertia_overrides.resize(rows, None);
72 self.descriptors
73 .resize(rows, BodyDescriptorRecord::zeroed());
74 self.kinematic.resize(rows, false);
75 self.states.resize(rows, None);
76 self.covered.resize(rows, NEVER_REPORTED);
77 }
78}
79
80impl World {
81 pub fn spawn(&mut self, desc: BodyDesc) -> BodyHandle {
82 let (id, generation) = self.bodies.ids.acquire();
83 self.bodies.grow_to(id);
84 let handle = BodyHandle { id, generation };
85 let id = id as usize;
86 self.validate_world_geometry(&desc);
87 let mass = desc.effective_mass(|shape| self.shape_solid(shape));
88 let mut spawn_desc = desc.clone();
89 spawn_desc.mass = mass;
90 self.bodies.masses[id] = mass;
91 self.bodies.com_overrides[id] = desc.com;
92 self.bodies.inertia_overrides[id] = desc.inertia;
93 self.bodies.kinematic[id] = desc.kinematic;
94 self.bodies.collider_descs[id] = desc.colliders.clone();
95 for collider in &desc.colliders {
96 self.retain_shape_ref(&collider.shape);
97 }
98 let mass_properties = self.mass_properties_of(id);
99 self.bodies.descriptors[id] =
100 BodyDescriptorRecord::build(&spawn_desc, mass_properties, &self.config);
101 self.record_state(id, &spawn_desc);
102 self.repool_colliders(id as u32);
103 let state = BodyStateRecord::initial(&spawn_desc, id as u32, handle.generation);
104 let slot = self.bodies.alive.len() as u32;
105 self.bodies.index_of[id] = slot;
106 self.bodies.alive.push(handle);
107 self.bodies.dirty.push(slot);
108 self.bodies
109 .commands
110 .push(BodyCommand::Add { row: slot, state });
111 if spawn_desc.ccd {
112 self.bodies.ccd_count += 1;
113 }
114 if mass > 0.0 || desc.kinematic {
115 if (self.bodies.dynamic_count as u32) < slot {
116 self.swap_slots(self.bodies.dynamic_count as u32, slot);
117 }
118 self.bodies.dynamic_count += 1;
119 }
120 handle
121 }
122
123 pub fn remove(&mut self, handle: BodyHandle) {
124 self.validate(handle);
125 self.assert_no_constraints(handle);
126 assert!(
127 self.soft.attachment_refs(handle.id) == 0,
128 "body handle {handle:?} still anchors a soft attachment"
129 );
130 let id = handle.id as usize;
131 let slot = self.bodies.index_of[id];
132 if (slot as usize) < self.bodies.dynamic_count {
133 let tail_dynamic = (self.bodies.dynamic_count - 1) as u32;
134 if slot != tail_dynamic {
135 self.swap_slots(slot, tail_dynamic);
136 }
137 self.bodies.dynamic_count -= 1;
138 let last = (self.bodies.alive.len() - 1) as u32;
139 if tail_dynamic != last {
140 self.swap_slots(tail_dynamic, last);
141 }
142 } else {
143 let last = (self.bodies.alive.len() - 1) as u32;
144 if slot != last {
145 self.swap_slots(slot, last);
146 }
147 }
148 self.discard_tail();
149 }
150
151 fn discard_tail(&mut self) {
152 let last = (self.bodies.alive.len() - 1) as u32;
153 let handle = self.bodies.alive[last as usize];
154 let id = handle.id as usize;
155 self.bodies.index_of[id] = u32::MAX;
156 self.bodies.ids.release(handle.id);
157 self.bodies.alive.pop();
158 self.bodies.dirty.retain(|dirty| *dirty != last);
159 let removed = std::mem::take(&mut self.bodies.collider_descs[id]);
160 for collider in &removed {
161 self.release_shape_ref(&collider.shape);
162 }
163 self.colliders.release(id as u32);
164 self.bodies.states[id] = None;
165 self.bodies.covered[id] = NEVER_REPORTED;
166 self.view.forget(handle.id);
167 self.bodies.kinematic[id] = false;
168 if self.bodies.descriptors[id].flags & BODY_CCD != 0 {
169 self.bodies.ccd_count -= 1;
170 }
171 self.bodies.descriptors[id] = BodyDescriptorRecord::zeroed();
172 self.bodies.commands.push(BodyCommand::Remove {
173 hole: last,
174 tail: last,
175 });
176 }
177
178 fn swap_slots(&mut self, first: u32, second: u32) {
179 self.bodies.alive.swap(first as usize, second as usize);
180 self.bodies.index_of[self.bodies.alive[first as usize].id as usize] = first;
181 self.bodies.index_of[self.bodies.alive[second as usize].id as usize] = second;
182 self.bodies
183 .commands
184 .push(BodyCommand::Swap { first, second });
185 self.bodies.dirty.push(first);
186 self.bodies.dirty.push(second);
187 }
188
189 fn migrate_partition(&mut self, handle: BodyHandle) {
190 let id = handle.id as usize;
191 let static_now = self.is_static_id(id);
192 let slot = self.bodies.index_of[id];
193 let in_dynamic = (slot as usize) < self.bodies.dynamic_count;
194 if static_now == !in_dynamic {
195 return;
196 }
197 if static_now {
198 let tail = (self.bodies.dynamic_count - 1) as u32;
199 if slot != tail {
200 self.swap_slots(slot, tail);
201 }
202 self.bodies.dynamic_count -= 1;
203 self.bodies.commands.push(BodyCommand::Patch {
204 row: tail,
205 mask: PATCH_VELOCITY,
206 state: BodyStateRecord::zeroed(),
207 });
208 if let Some(state) = self.bodies.states[id].as_mut() {
209 state.velocity = [0.0; 3];
210 state.angular_velocity = [0.0; 3];
211 }
212 self.bodies.dirty.push(tail);
213 } else {
214 let boundary = self.bodies.dynamic_count as u32;
215 if slot != boundary {
216 self.swap_slots(slot, boundary);
217 }
218 self.bodies.dynamic_count += 1;
219 }
220 self.bodies.dirty.push(slot);
221 }
222
223 pub fn read_state(&self, handle: BodyHandle) -> BodyState {
224 self.validate(handle);
225 assert!(
226 self.body_current(handle.id),
227 "a body state older than the last step requires wait() or an observation of that body"
228 );
229 self.bodies.states[handle.id as usize].expect("body state is unavailable")
230 }
231
232 fn record_state(&mut self, id: usize, desc: &BodyDesc) {
233 self.bodies.states[id] = Some(BodyState {
234 position: desc.position,
235 prev_position: desc.position,
236 orientation: desc.orientation,
237 velocity: desc.velocity,
238 angular_velocity: desc.angular_velocity,
239 inverse_mass: self.bodies.descriptors[id].inverse_mass,
240 com: self.bodies.descriptors[id].com,
241 sleeping: false,
242 step: self.clock.step,
243 });
244 self.bodies.covered[id] = self.clock.step;
245 }
246
247 fn validate_world_geometry(&self, desc: &BodyDesc) {
248 for collider in &desc.colliders {
249 if collider.shape.is_world_geometry() && (desc.mass > 0.0 && !desc.kinematic) {
250 panic!("world geometry colliders must be static or kinematic");
251 }
252 }
253 }
254
255 fn is_static_id(&self, id: usize) -> bool {
256 self.bodies.masses[id] <= 0.0 && !self.bodies.kinematic[id]
257 }
258
259 fn patch_descriptor(
260 &mut self,
261 handle: BodyHandle,
262 edit: impl FnOnce(&mut BodyDescriptorRecord),
263 ) {
264 self.validate(handle);
265 let id = handle.id as usize;
266 edit(&mut self.bodies.descriptors[id]);
267 self.bodies.dirty.push(self.bodies.index_of[id]);
268 }
269
270 fn refresh_descriptor(&mut self, handle: BodyHandle) {
271 let id = handle.id as usize;
272 let mass = self.mass_properties_of(id);
273 let kinematic = self.bodies.kinematic[id];
274 let inverse_mass = if kinematic || self.bodies.masses[id] <= 0.0 {
275 0.0
276 } else {
277 1.0 / self.bodies.masses[id]
278 };
279 let descriptor = &mut self.bodies.descriptors[id];
280 descriptor.com = mass.com;
281 descriptor.inverse_inertia = mass.inverse_inertia;
282 descriptor.inverse_mass = inverse_mass;
283 descriptor.flags =
284 (descriptor.flags & !BODY_KINEMATIC) | if kinematic { BODY_KINEMATIC } else { 0 };
285 let row = *descriptor;
286 self.bodies.dirty.push(self.bodies.index_of[id]);
287 if let Some(state) = self.bodies.states[id].as_mut() {
288 state.inverse_mass = row.inverse_mass;
289 state.com = row.com;
290 }
291 }
292
293 fn apply_mass(&mut self, handle: BodyHandle, mass: f32) {
294 self.bodies.masses[handle.id as usize] = mass;
295 self.refresh_descriptor(handle);
296 }
297
298 fn command_slot(&self, handle: BodyHandle) -> u32 {
299 self.validate(handle);
300 self.bodies.index_of[handle.id as usize]
301 }
302
303 fn schedule_patch(&mut self, handle: BodyHandle, mask: u32, state: BodyStateRecord) {
304 let slot = self.command_slot(handle);
305 self.bodies.commands.push(BodyCommand::Patch {
306 row: slot,
307 mask,
308 state,
309 });
310 }
311
312 pub fn set_position(&mut self, handle: BodyHandle, position: [f32; 3]) {
313 self.validate(handle);
314 if let Some(state) = self.bodies.states[handle.id as usize].as_mut() {
315 state.position = position;
316 state.prev_position = position;
317 }
318 let mut payload = BodyStateRecord::zeroed();
319 payload.position = position;
320 self.schedule_patch(handle, PATCH_POSITION, payload);
321 self.bodies
322 .dirty
323 .push(self.bodies.index_of[handle.id as usize]);
324 }
325
326 pub fn set_orientation(&mut self, handle: BodyHandle, orientation: [f32; 4]) {
327 self.assert_unit(orientation);
328 self.validate(handle);
329 if let Some(state) = self.bodies.states[handle.id as usize].as_mut() {
330 state.orientation = orientation;
331 }
332 let mut payload = BodyStateRecord::zeroed();
333 payload.orientation = orientation;
334 self.schedule_patch(handle, PATCH_ORIENTATION, payload);
335 self.bodies
336 .dirty
337 .push(self.bodies.index_of[handle.id as usize]);
338 }
339
340 pub fn set_velocity(&mut self, handle: BodyHandle, velocity: [f32; 3]) {
341 self.validate(handle);
342 if let Some(state) = self.bodies.states[handle.id as usize].as_mut() {
343 state.velocity = velocity;
344 }
345 let mut payload = BodyStateRecord::zeroed();
346 payload.velocity = velocity;
347 self.schedule_patch(handle, PATCH_VELOCITY, payload);
348 }
349
350 pub fn set_angular_velocity(&mut self, handle: BodyHandle, angular_velocity: [f32; 3]) {
351 self.validate(handle);
352 if let Some(state) = self.bodies.states[handle.id as usize].as_mut() {
353 state.angular_velocity = angular_velocity;
354 }
355 let mut payload = BodyStateRecord::zeroed();
356 payload.angular_velocity = angular_velocity;
357 self.schedule_patch(handle, PATCH_ANGULAR_VELOCITY, payload);
358 }
359
360 pub fn set_mass(&mut self, handle: BodyHandle, mass: f32) {
361 assert!(mass >= 0.0, "mass must be non-negative");
362 self.validate(handle);
363 self.apply_mass(handle, mass);
364 self.migrate_partition(handle);
365 }
366
367 pub fn set_density(&mut self, handle: BodyHandle, density: f32) {
368 assert!(density >= 0.0, "density must be non-negative");
369 self.validate(handle);
370 let volume = dynamis_model::solid_volume_of(
371 &self.bodies.collider_descs[handle.id as usize],
372 |shape| self.shape_solid(shape),
373 );
374 self.apply_mass(handle, density * volume);
375 self.migrate_partition(handle);
376 }
377
378 pub fn set_com(&mut self, handle: BodyHandle, com: [f32; 3]) {
379 self.validate(handle);
380 self.bodies.com_overrides[handle.id as usize] = Some(com);
381 self.refresh_descriptor(handle);
382 }
383
384 pub fn set_inertia(&mut self, handle: BodyHandle, inertia: [f32; 6]) {
385 assert!(
386 inertia.iter().all(|value| value.is_finite()),
387 "inertia tensor must be finite"
388 );
389 self.validate(handle);
390 self.bodies.inertia_overrides[handle.id as usize] = Some(inertia);
391 self.refresh_descriptor(handle);
392 }
393
394 pub fn set_linear_damping(&mut self, handle: BodyHandle, damping: f32) {
395 assert!(damping >= 0.0, "damping must be non-negative");
396 self.patch_descriptor(handle, |row| row.linear_damping = damping);
397 }
398
399 pub fn set_angular_damping(&mut self, handle: BodyHandle, damping: f32) {
400 assert!(damping >= 0.0, "angular damping must be non-negative");
401 self.patch_descriptor(handle, |row| row.angular_damping = damping);
402 }
403
404 pub fn set_gravity_scale(&mut self, handle: BodyHandle, gravity_scale: f32) {
405 self.patch_descriptor(handle, |row| row.gravity_scale = gravity_scale);
406 }
407
408 pub fn set_sleep_thresholds(
409 &mut self,
410 handle: BodyHandle,
411 velocity: f32,
412 angular_velocity: f32,
413 ) {
414 assert!(velocity >= 0.0, "sleep velocity must be non-negative");
415 assert!(
416 angular_velocity >= 0.0,
417 "sleep angular velocity must be non-negative"
418 );
419 self.patch_descriptor(handle, |row| {
420 row.sleep_velocity = velocity;
421 row.sleep_angular_velocity = angular_velocity;
422 row.flags |= OVERRIDE_SLEEP_LINEAR | OVERRIDE_SLEEP_ANGULAR;
423 });
424 }
425
426 pub fn set_collision_group(&mut self, handle: BodyHandle, group: u32) {
427 self.patch_descriptor(handle, |row| row.collision_group = group);
428 }
429
430 pub fn set_collision_mask(&mut self, handle: BodyHandle, mask: u32) {
431 self.patch_descriptor(handle, |row| row.collision_mask = mask);
432 }
433
434 pub fn set_ccd(&mut self, handle: BodyHandle, ccd: bool) {
435 self.validate(handle);
436 let enabled = self.bodies.descriptors[handle.id as usize].flags & BODY_CCD != 0;
437 if enabled == ccd {
438 return;
439 }
440 self.bodies.ccd_count = if ccd {
441 self.bodies.ccd_count + 1
442 } else {
443 self.bodies.ccd_count - 1
444 };
445 self.patch_descriptor(handle, |row| {
446 row.flags = (row.flags & !BODY_CCD) | if ccd { BODY_CCD } else { 0 };
447 });
448 }
449
450 pub(crate) fn ccd_active(&self) -> bool {
451 debug_assert_eq!(
452 self.bodies.ccd_count,
453 self.bodies
454 .descriptors
455 .iter()
456 .filter(|row| row.flags & BODY_CCD != 0)
457 .count() as u32,
458 "the ccd body count must mirror the body descriptors"
459 );
460 self.bodies.ccd_count > 0
461 }
462
463 pub fn set_kinematic(&mut self, handle: BodyHandle, kinematic: bool) {
464 self.validate(handle);
465 self.bodies.kinematic[handle.id as usize] = kinematic;
466 self.refresh_descriptor(handle);
467 self.migrate_partition(handle);
468 }
469
470 pub fn set_collider(&mut self, handle: BodyHandle, index: usize, collider: ColliderDesc) {
471 self.validate(handle);
472 let id = handle.id as usize;
473 assert!(
474 index < self.bodies.collider_descs[id].len(),
475 "collider index out of range"
476 );
477 let existing = std::mem::replace(&mut self.bodies.collider_descs[id][index], collider);
478 self.release_shape_ref(&existing.shape);
479 self.retain_shape_ref(&collider.shape);
480 self.repool_colliders(handle.id);
481 self.refresh_descriptor(handle);
482 }
483
484 pub fn add_collider(&mut self, handle: BodyHandle, collider: ColliderDesc) {
485 self.validate(handle);
486 let id = handle.id as usize;
487 self.retain_shape_ref(&collider.shape);
488 self.bodies.collider_descs[id].push(collider);
489 self.repool_colliders(handle.id);
490 self.refresh_descriptor(handle);
491 }
492
493 pub fn remove_collider(&mut self, handle: BodyHandle, index: usize) {
494 self.validate(handle);
495 let id = handle.id as usize;
496 assert!(
497 index < self.bodies.collider_descs[id].len(),
498 "collider index out of range"
499 );
500 assert!(
501 self.bodies.collider_descs[id].len() > 1,
502 "a body requires at least one collider"
503 );
504 let removed = self.bodies.collider_descs[id].swap_remove(index);
505 self.release_shape_ref(&removed.shape);
506 self.repool_colliders(handle.id);
507 self.refresh_descriptor(handle);
508 }
509
510 pub fn set_shape(&mut self, handle: BodyHandle, shape: Shape) {
511 self.validate(handle);
512 let id = handle.id as usize;
513 let replaced = std::mem::replace(&mut self.bodies.collider_descs[id][0].shape, shape);
514 self.release_shape_ref(&replaced);
515 self.retain_shape_ref(&shape);
516 self.repool_colliders(handle.id);
517 self.refresh_descriptor(handle);
518 }
519
520 pub fn set_restitution(&mut self, handle: BodyHandle, restitution: f32) {
521 self.validate(handle);
522 self.bodies.collider_descs[handle.id as usize][0].restitution = restitution;
523 self.repool_colliders(handle.id);
524 self.bodies
525 .dirty
526 .push(self.bodies.index_of[handle.id as usize]);
527 }
528
529 pub fn set_friction(&mut self, handle: BodyHandle, friction: f32) {
530 assert!(friction >= 0.0, "friction must be non-negative");
531 self.validate(handle);
532 self.bodies.collider_descs[handle.id as usize][0].friction = friction;
533 self.repool_colliders(handle.id);
534 self.bodies
535 .dirty
536 .push(self.bodies.index_of[handle.id as usize]);
537 }
538
539 pub fn set_collider_events(
540 &mut self,
541 handle: BodyHandle,
542 index: usize,
543 events: ContactEventMode,
544 ) {
545 self.validate(handle);
546 let id = handle.id as usize;
547 assert!(
548 index < self.bodies.collider_descs[id].len(),
549 "collider index out of range"
550 );
551 self.bodies.collider_descs[id][index].events = events;
552 self.repool_colliders(handle.id);
553 self.bodies.dirty.push(self.bodies.index_of[id]);
554 }
555
556 pub fn apply_force(&mut self, handle: BodyHandle, force: [f32; 3]) {
557 let slot = self.command_slot(handle);
558 self.bodies
559 .commands
560 .push(BodyCommand::Force { row: slot, force });
561 }
562
563 pub fn apply_force_at_point(&mut self, handle: BodyHandle, force: [f32; 3], point: [f32; 3]) {
564 let slot = self.command_slot(handle);
565 self.bodies.commands.push(BodyCommand::ForceAtPoint {
566 row: slot,
567 force,
568 point,
569 });
570 }
571
572 pub fn apply_torque(&mut self, handle: BodyHandle, torque: [f32; 3]) {
573 let slot = self.command_slot(handle);
574 self.bodies
575 .commands
576 .push(BodyCommand::Torque { row: slot, torque });
577 }
578
579 pub fn apply_impulse(&mut self, handle: BodyHandle, impulse: [f32; 3]) {
580 let slot = self.command_slot(handle);
581 self.bodies
582 .commands
583 .push(BodyCommand::Impulse { row: slot, impulse });
584 }
585
586 pub fn apply_impulse_at_point(
587 &mut self,
588 handle: BodyHandle,
589 impulse: [f32; 3],
590 point: [f32; 3],
591 ) {
592 let slot = self.command_slot(handle);
593 self.bodies.commands.push(BodyCommand::ImpulseAtPoint {
594 row: slot,
595 impulse,
596 point,
597 });
598 }
599
600 pub fn apply_angular_impulse(&mut self, handle: BodyHandle, impulse: [f32; 3]) {
601 let slot = self.command_slot(handle);
602 self.bodies
603 .commands
604 .push(BodyCommand::AngularImpulse { row: slot, impulse });
605 }
606
607 pub fn wake(&mut self, handle: BodyHandle) {
608 let slot = self.command_slot(handle);
609 self.bodies.commands.push(BodyCommand::Wake { row: slot });
610 }
611
612 pub fn sleep(&mut self, handle: BodyHandle) {
613 let slot = self.command_slot(handle);
614 self.bodies.commands.push(BodyCommand::Sleep { row: slot });
615 }
616
617 pub(crate) fn repool_colliders(&mut self, id: u32) {
618 let records = self.collider_block_of(id as usize);
619 self.colliders.assign(id, &records);
620 }
621
622 pub(crate) fn collider_block_of(&self, id: usize) -> Vec<ColliderRecord> {
623 self.bodies.collider_descs[id]
624 .iter()
625 .enumerate()
626 .map(|(slot, collider)| self.collider_record(collider, slot as u32))
627 .collect()
628 }
629
630 pub(super) fn validate(&self, handle: BodyHandle) {
631 let id = handle.id as usize;
632 if id >= self.bodies.ids.len() {
633 panic!("body handle {handle:?} is out of range");
634 }
635 if self.bodies.ids.generation(handle.id) != handle.generation {
636 panic!("body handle {handle:?} is stale");
637 }
638 if self.bodies.index_of[id] == u32::MAX {
639 panic!("body handle {handle:?} is not alive");
640 }
641 }
642
643 pub(super) fn assert_unit(&self, orientation: [f32; 4]) {
644 assert!(
645 (orientation[0] * orientation[0]
646 + orientation[1] * orientation[1]
647 + orientation[2] * orientation[2]
648 + orientation[3] * orientation[3]
649 - 1.0)
650 .abs()
651 < 1e-4,
652 "orientation must be a unit quaternion"
653 );
654 }
655
656 fn retain_shape_ref(&mut self, shape: &Shape) {
657 if let Shape::Hull(handle) | Shape::Mesh(handle) | Shape::HeightField(handle) = shape {
658 self.shapes.pool.retain(*handle);
659 }
660 }
661
662 fn release_shape_ref(&mut self, shape: &Shape) {
663 if let Shape::Hull(handle) | Shape::Mesh(handle) | Shape::HeightField(handle) = shape {
664 self.shapes.pool.release(*handle);
665 }
666 }
667}
668
669impl World {
670 pub(crate) fn state_snapshot(&self, id: usize) -> Option<BodyState> {
671 assert!(
672 self.body_current(id as u32),
673 "a constraint frame requires a current body state; call wait() or observe the body"
674 );
675 self.bodies.states[id]
676 }
677
678 pub(crate) fn mass_properties_of(&self, id: usize) -> MassProperties {
679 dynamis_model::mass_properties_of_intent(
680 &self.bodies.collider_descs[id],
681 self.bodies.masses[id],
682 self.bodies.com_overrides[id],
683 self.bodies.inertia_overrides[id],
684 |shape| self.shape_solid(shape),
685 )
686 }
687
688 pub(crate) fn collider_record(&self, desc: &ColliderDesc, slot: u32) -> ColliderRecord {
689 let source = match desc.shape {
690 Shape::Hull(handle) | Shape::Mesh(handle) | Shape::HeightField(handle) => handle.id,
691 _ => 0,
692 };
693 ColliderRecord::build(desc, source, slot)
694 }
695}