dynamis_world/
constraint.rs

1use super::World;
2use super::commands::ConstraintCommand;
3use super::ids::IdSpace;
4use dynamis_abi::{BrokenConstraintRecord, COUNTER_BREAKS, ConstraintDescriptorRecord};
5use dynamis_gpu::EVENT_SLOTS;
6use dynamis_gpu::SubmissionEncoder;
7use dynamis_model::{
8    BodyHandle, ConstraintBreak, ConstraintDesc, ConstraintHandle, ConstraintKind, ConstraintLimit,
9    ConstraintMotor, ConstraintSpring, ConstraintSwing,
10};
11use std::collections::VecDeque;
12use std::mem::size_of;
13
14#[derive(Clone)]
15pub(crate) struct Constraints {
16    pub(crate) alive: Vec<ConstraintHandle>,
17    pub(crate) ids: IdSpace,
18    pub(crate) index_of: Vec<u32>,
19    pub(crate) records: Vec<ConstraintDescriptorRecord>,
20    pub(crate) attached: Vec<Vec<u32>>,
21    pub(crate) commands: Vec<ConstraintCommand>,
22    pub(crate) dirty: Vec<u32>,
23    pub(crate) last_moves: u32,
24    pub(crate) last_commands: u32,
25    pub(crate) broken: Vec<ConstraintHandle>,
26    pub(crate) due: VecDeque<(u64, u32)>,
27}
28
29impl Constraints {
30    pub(crate) const fn new() -> Self {
31        Self {
32            alive: Vec::new(),
33            ids: IdSpace::new(),
34            index_of: Vec::new(),
35            records: Vec::new(),
36            attached: Vec::new(),
37            commands: Vec::new(),
38            dirty: Vec::new(),
39            last_moves: 0,
40            last_commands: 0,
41            broken: Vec::new(),
42            due: VecDeque::new(),
43        }
44    }
45
46    fn grow_to(&mut self, id: u32) {
47        let rows = id as usize + 1;
48        if rows > self.index_of.len() {
49            self.index_of.resize(rows, u32::MAX);
50        }
51    }
52
53    pub(crate) fn attached_to(&self, body_id: u32) -> &[u32] {
54        self.attached
55            .get(body_id as usize)
56            .map_or(&[], Vec::as_slice)
57    }
58
59    fn attach_to(&mut self, body_id: u32, constraint_id: u32) {
60        let index = body_id as usize;
61        if self.attached.len() <= index {
62            self.attached.resize(index + 1, Vec::new());
63        }
64        self.attached[index].push(constraint_id);
65    }
66
67    fn detach_from(&mut self, body_id: u32, constraint_id: u32) {
68        let attached = &mut self.attached[body_id as usize];
69        let slot = attached
70            .iter()
71            .position(|id| *id == constraint_id)
72            .expect("an attached constraint must exist");
73        attached.swap_remove(slot);
74    }
75
76    fn attach(&mut self, handle: ConstraintHandle, record: ConstraintDescriptorRecord) -> u32 {
77        let slot = self.alive.len() as u32;
78        self.index_of[handle.id as usize] = slot;
79        self.alive.push(handle);
80        self.records.push(record);
81        slot
82    }
83
84    fn detach(&mut self, slot: usize) -> bool {
85        let tail = self.alive.len() - 1;
86        self.alive.swap_remove(slot);
87        self.records.swap_remove(slot);
88        if slot == tail {
89            return false;
90        }
91        let moved = self.alive[slot];
92        self.index_of[moved.id as usize] = slot as u32;
93        true
94    }
95}
96
97impl World {
98    pub fn add_constraint(
99        &mut self,
100        first: BodyHandle,
101        second: BodyHandle,
102        desc: ConstraintDesc,
103    ) -> ConstraintHandle {
104        self.validate(first);
105        self.validate(second);
106        if first == second {
107            panic!("constraint bodies must be distinct");
108        }
109        self.validate_constraint_desc(&desc);
110        let mut desc = desc;
111        if constrains_joint_frame(desc.kind)
112            && desc.reference == [0.0, 0.0, 0.0, 1.0]
113            && let (Some(state_a), Some(state_b)) = (
114                self.state_snapshot(first.id as usize),
115                self.state_snapshot(second.id as usize),
116            )
117        {
118            desc.reference = relative_reference(state_a.orientation, state_b.orientation);
119        }
120        let (id, generation) = self.constraints.ids.acquire();
121        self.constraints.grow_to(id);
122        let handle = ConstraintHandle { id, generation };
123        let record = ConstraintDescriptorRecord::build(&desc, first.id, second.id);
124        let slot = self.constraints.attach(handle, record);
125        self.constraints.attach_to(first.id, handle.id);
126        self.constraints.attach_to(second.id, handle.id);
127        self.constraints.dirty.push(slot);
128        self.constraints.commands.push(ConstraintCommand::Add {
129            slot,
130            id,
131            generation,
132        });
133        handle
134    }
135
136    pub fn update_constraint(&mut self, handle: ConstraintHandle, desc: ConstraintDesc) {
137        self.validate_constraint(handle);
138        self.validate_constraint_desc(&desc);
139        let slot = self.constraints.index_of[handle.id as usize] as usize;
140        let existing = self.constraints.records[slot];
141        let mut desc = desc;
142        if constrains_joint_frame(desc.kind) && desc.reference == [0.0, 0.0, 0.0, 1.0] {
143            desc.reference = existing.reference;
144        }
145        let record = ConstraintDescriptorRecord::build(
146            &desc,
147            existing.first_body_id,
148            existing.second_body_id,
149        );
150        self.constraints.records[slot] = record;
151        self.constraints.dirty.push(slot as u32);
152    }
153
154    pub fn set_motor(&mut self, handle: ConstraintHandle, target_velocity: f32, max_force: f32) {
155        assert!(max_force >= 0.0, "motor force must be non-negative");
156        self.patch_record(handle, |record| {
157            record.motor_speed = target_velocity;
158            record.motor_max_force = max_force;
159            record.motor_target = 0.0;
160            record.motor_stiffness = 0.0;
161            record.motor_damping = 0.0;
162            record.flags |= dynamis_abi::CONSTRAINT_HAS_MOTOR;
163        });
164    }
165
166    pub fn set_limit(&mut self, handle: ConstraintHandle, limit: Option<ConstraintLimit>) {
167        if let Some(limit) = limit {
168            assert!(
169                limit.max >= limit.min,
170                "constraint limit max must not be below min"
171            );
172        }
173        self.patch_record(handle, |record| match limit {
174            Some(limit) => {
175                record.limit_min = limit.min;
176                record.limit_max = limit.max;
177                record.flags |= dynamis_abi::CONSTRAINT_HAS_LIMIT;
178            }
179            None => record.flags &= !dynamis_abi::CONSTRAINT_HAS_LIMIT,
180        });
181    }
182
183    pub fn set_spring(&mut self, handle: ConstraintHandle, spring: Option<ConstraintSpring>) {
184        if let Some(spring) = spring {
185            assert!(
186                spring.frequency >= 0.0,
187                "spring frequency must be non-negative"
188            );
189            assert!(
190                spring.damping_ratio >= 0.0,
191                "spring damping ratio must be non-negative"
192            );
193        }
194        self.patch_record(handle, |record| match spring {
195            Some(spring) => {
196                record.spring_frequency = spring.frequency;
197                record.spring_damping_ratio = spring.damping_ratio;
198                record.flags |= dynamis_abi::CONSTRAINT_IS_SPRING;
199            }
200            None => record.flags &= !dynamis_abi::CONSTRAINT_IS_SPRING,
201        });
202    }
203
204    pub fn set_break_threshold(
205        &mut self,
206        handle: ConstraintHandle,
207        threshold: Option<ConstraintBreak>,
208    ) {
209        if let Some(threshold) = threshold {
210            assert!(threshold.force >= 0.0, "break force must be non-negative");
211            assert!(threshold.torque >= 0.0, "break torque must be non-negative");
212        }
213        self.patch_record(handle, |record| match threshold {
214            Some(threshold) => {
215                record.break_force = threshold.force;
216                record.break_torque = threshold.torque;
217                record.flags |= dynamis_abi::CONSTRAINT_HAS_BREAK;
218            }
219            None => record.flags &= !dynamis_abi::CONSTRAINT_HAS_BREAK,
220        });
221    }
222
223    pub fn set_warm_start(&mut self, handle: ConstraintHandle, warm_start: bool) {
224        self.patch_record(handle, |record| {
225            record.flags = (record.flags & !dynamis_abi::CONSTRAINT_WARM_START)
226                | if warm_start {
227                    dynamis_abi::CONSTRAINT_WARM_START
228                } else {
229                    0
230                };
231        });
232    }
233
234    pub fn set_servo(
235        &mut self,
236        handle: ConstraintHandle,
237        target_position: f32,
238        stiffness: f32,
239        damping: f32,
240    ) {
241        assert!(
242            (0.0..=1.0).contains(&stiffness),
243            "servo stiffness must be within [0, 1]"
244        );
245        assert!(
246            (0.0..=1.0).contains(&damping),
247            "servo damping must be within [0, 1]"
248        );
249        self.patch_record(handle, |record| {
250            record.motor_target = target_position;
251            record.motor_stiffness = stiffness;
252            record.motor_damping = damping;
253            record.flags |= dynamis_abi::CONSTRAINT_HAS_MOTOR;
254        });
255    }
256
257    pub fn set_swing_limits(&mut self, handle: ConstraintHandle, swing: Option<ConstraintSwing>) {
258        if let Some(swing) = swing {
259            assert!(swing.swing_a >= 0.0, "swing limit must be non-negative");
260            assert!(swing.swing_b >= 0.0, "swing limit must be non-negative");
261        }
262        self.patch_record(handle, |record| match swing {
263            Some(swing) => {
264                record.swing_a = swing.swing_a;
265                record.swing_b = swing.swing_b;
266                record.flags |= dynamis_abi::CONSTRAINT_HAS_SWING;
267            }
268            None => record.flags &= !dynamis_abi::CONSTRAINT_HAS_SWING,
269        });
270    }
271
272    pub fn set_constraint_disable_collisions(&mut self, handle: ConstraintHandle, disable: bool) {
273        self.patch_record(handle, |record| {
274            record.flags = (record.flags & !dynamis_abi::CONSTRAINT_DISABLE_COLLISIONS)
275                | if disable {
276                    dynamis_abi::CONSTRAINT_DISABLE_COLLISIONS
277                } else {
278                    0
279                };
280        });
281    }
282
283    pub fn set_dof_locked(&mut self, handle: ConstraintHandle, index: usize, locked: bool) {
284        self.assert_dof_index(index);
285        self.patch_record(handle, |record| {
286            record.flags = dynamis_abi::set_dof_locked(record.flags, index as u32, locked);
287        });
288    }
289
290    pub fn set_dof_limit(
291        &mut self,
292        handle: ConstraintHandle,
293        index: usize,
294        limit: Option<ConstraintLimit>,
295    ) {
296        self.assert_dof_index(index);
297        if let Some(limit) = limit {
298            assert!(
299                limit.max >= limit.min,
300                "dof limit max must not be below min"
301            );
302        }
303        self.patch_record(handle, |record| {
304            let (min, max) = dof_limit_pair(record, index);
305            match limit {
306                Some(limit) => {
307                    *min = limit.min;
308                    *max = limit.max;
309                    record.flags = dynamis_abi::set_dof_limited(record.flags, index as u32, true);
310                }
311                None => {
312                    *min = 0.0;
313                    *max = 0.0;
314                    record.flags = dynamis_abi::set_dof_limited(record.flags, index as u32, false);
315                }
316            }
317        });
318    }
319
320    pub fn set_dof_motor(
321        &mut self,
322        handle: ConstraintHandle,
323        index: usize,
324        motor: Option<ConstraintMotor>,
325    ) {
326        self.assert_dof_index(index);
327        self.patch_record(handle, |record| {
328            let (target, stiffness, damping, force) = dof_motor_slots(record, index);
329            match motor {
330                Some(motor) => {
331                    *target = motor.target_position.unwrap_or(motor.target_velocity);
332                    *stiffness = motor.stiffness;
333                    *damping = motor.damping;
334                    *force = motor.max_force;
335                    record.flags = dynamis_abi::set_dof_driven(record.flags, index as u32, true);
336                }
337                None => {
338                    *target = 0.0;
339                    *stiffness = 0.0;
340                    *damping = 0.0;
341                    *force = 0.0;
342                    record.flags = dynamis_abi::set_dof_driven(record.flags, index as u32, false);
343                }
344            }
345        });
346    }
347
348    fn patch_record(
349        &mut self,
350        handle: ConstraintHandle,
351        change: impl FnOnce(&mut ConstraintDescriptorRecord),
352    ) {
353        self.validate_constraint(handle);
354        let slot = self.constraints.index_of[handle.id as usize] as usize;
355        change(&mut self.constraints.records[slot]);
356        self.constraints.dirty.push(slot as u32);
357    }
358
359    fn assert_dof_index(&self, index: usize) {
360        assert!(
361            index < dynamis_abi::DOF_COUNT as usize,
362            "dof index must be below {}",
363            dynamis_abi::DOF_COUNT
364        );
365    }
366
367    pub fn remove_constraint(&mut self, handle: ConstraintHandle) {
368        self.validate_constraint(handle);
369        let id = handle.id as usize;
370        let slot = self.constraints.index_of[id] as usize;
371        let record = self.constraints.records[slot];
372        self.constraints
373            .detach_from(record.first_body_id, handle.id);
374        self.constraints
375            .detach_from(record.second_body_id, handle.id);
376        let tail = self.constraints.alive.len() - 1;
377        if self.constraints.detach(slot) {
378            self.constraints.dirty.push(slot as u32);
379            self.constraints.commands.push(ConstraintCommand::Swap {
380                slot: slot as u32,
381                tail: tail as u32,
382            });
383        }
384        self.constraints.index_of[id] = u32::MAX;
385        self.constraints.ids.release(handle.id);
386        self.constraints.dirty.retain(|dirty| *dirty != tail as u32);
387    }
388
389    pub(crate) fn note_breaks_due(&mut self, step: u64) {
390        let count = self.backend.measured[COUNTER_BREAKS];
391        if count > 0 {
392            self.constraints.due.push_back((step, count));
393        }
394    }
395
396    pub(crate) fn copy_breaks(&mut self, encoder: &mut SubmissionEncoder) {
397        while let Some((step, count)) = self.constraints.due.pop_front() {
398            assert!(
399                self.clock.step <= step + EVENT_SLOTS as u64,
400                "constraint break reports for step {step} were overwritten before step {} could copy them",
401                self.clock.step
402            );
403            let segment = self.backend.streams.state.constraint_breaks.size() / EVENT_SLOTS as u64;
404            let offset = (step % EVENT_SLOTS as u64) * segment;
405            let bytes = count as u64 * size_of::<BrokenConstraintRecord>() as u64;
406            assert!(
407                bytes <= segment,
408                "constraint break reports for step {step} outrun their segment"
409            );
410            let displaced = self.backend.readback.breaks.enqueue(
411                encoder,
412                self.backend.streams.state.constraint_breaks.buffer(),
413                offset,
414                bytes,
415                step,
416            );
417            if let Some((_, bytes)) = displaced {
418                self.consume_breaks(&bytes);
419            }
420        }
421    }
422
423    pub(crate) fn sync_breaks(&mut self) {
424        if self.constraints.due.is_empty() {
425            return;
426        }
427        let device = self.backend.gpu.device().clone();
428        let mut encoder = SubmissionEncoder::new(&device, "dynamis constraint break readback");
429        self.copy_breaks(&mut encoder);
430        self.submit(encoder);
431        for (_, bytes) in self.backend.readback.breaks.drain() {
432            self.consume_breaks(&bytes);
433        }
434    }
435
436    pub(crate) fn consume_breaks(&mut self, bytes: &[u8]) {
437        for record in dynamis_abi::decode::<BrokenConstraintRecord>(bytes) {
438            self.accept_constraint_break(record.constraint_id, record.generation);
439        }
440    }
441
442    pub fn drain_constraint_breaks(&mut self) -> Vec<ConstraintHandle> {
443        self.backend.gpu.assert_alive();
444        self.collect_readbacks();
445        self.sync_breaks();
446        std::mem::take(&mut self.constraints.broken)
447    }
448
449    pub fn constraints(&self) -> &[ConstraintHandle] {
450        &self.constraints.alive
451    }
452
453    pub fn body_constraints(&self, handle: BodyHandle) -> Vec<ConstraintHandle> {
454        self.validate(handle);
455        self.constraints
456            .attached_to(handle.id)
457            .iter()
458            .map(|id| ConstraintHandle {
459                id: *id,
460                generation: self.constraints.ids.generation(*id),
461            })
462            .collect()
463    }
464
465    pub fn constraint_bodies(&self, handle: ConstraintHandle) -> (BodyHandle, BodyHandle) {
466        self.validate_constraint(handle);
467        let record =
468            self.constraints.records[self.constraints.index_of[handle.id as usize] as usize];
469        (
470            BodyHandle {
471                id: record.first_body_id,
472                generation: self.bodies.ids.generation(record.first_body_id),
473            },
474            BodyHandle {
475                id: record.second_body_id,
476                generation: self.bodies.ids.generation(record.second_body_id),
477            },
478        )
479    }
480
481    pub(crate) fn validate_constraint(&self, handle: ConstraintHandle) {
482        let id = handle.id as usize;
483        if id >= self.constraints.ids.len() {
484            panic!("constraint handle {handle:?} is out of range");
485        }
486        if self.constraints.ids.generation(handle.id) != handle.generation {
487            panic!("constraint handle {handle:?} is stale");
488        }
489        if self.constraints.index_of[id] == u32::MAX {
490            panic!("constraint handle {handle:?} is not alive");
491        }
492    }
493
494    fn validate_constraint_desc(&self, desc: &ConstraintDesc) {
495        match desc.kind {
496            ConstraintKind::Ball
497            | ConstraintKind::Distance
498            | ConstraintKind::Pulley
499            | ConstraintKind::Gear => {}
500            ConstraintKind::Cone => {
501                if desc.axis_a == [0.0; 3] || desc.axis_b == [0.0; 3] {
502                    panic!("constraint axis must be non-zero");
503                }
504            }
505            _ => {
506                if desc.axis_a == [0.0; 3] {
507                    panic!("constraint axis must be non-zero");
508                }
509            }
510        }
511    }
512
513    pub(super) fn assert_no_constraints(&self, handle: BodyHandle) {
514        assert!(
515            self.constraints.attached_to(handle.id).is_empty(),
516            "body handle {handle:?} is referenced by a live constraint; remove it first"
517        );
518    }
519}
520
521fn constrains_joint_frame(kind: ConstraintKind) -> bool {
522    matches!(
523        kind,
524        ConstraintKind::Fixed
525            | ConstraintKind::Revolute
526            | ConstraintKind::Prismatic
527            | ConstraintKind::SixDof
528    )
529}
530
531fn relative_reference(orientation_a: [f32; 4], orientation_b: [f32; 4]) -> [f32; 4] {
532    let a = [
533        -orientation_a[0],
534        -orientation_a[1],
535        -orientation_a[2],
536        orientation_a[3],
537    ];
538    dynamis_model::math::quat_mul(a, orientation_b)
539}
540
541fn dof_limit_pair(record: &mut ConstraintDescriptorRecord, index: usize) -> (&mut f32, &mut f32) {
542    if index < 3 {
543        (
544            &mut record.linear_limit_min[index],
545            &mut record.linear_limit_max[index],
546        )
547    } else {
548        let axis = index - 3;
549        (
550            &mut record.angular_limit_min[axis],
551            &mut record.angular_limit_max[axis],
552        )
553    }
554}
555
556type DofMotorSlots<'a> = (&'a mut f32, &'a mut f32, &'a mut f32, &'a mut f32);
557
558fn dof_motor_slots(record: &mut ConstraintDescriptorRecord, index: usize) -> DofMotorSlots<'_> {
559    let axis = index % 3;
560    if index < 3 {
561        (
562            &mut record.linear_motor_target[axis],
563            &mut record.linear_motor_stiffness[axis],
564            &mut record.linear_motor_damping[axis],
565            &mut record.linear_motor_force[axis],
566        )
567    } else {
568        (
569            &mut record.angular_motor_target[axis],
570            &mut record.angular_motor_stiffness[axis],
571            &mut record.angular_motor_damping[axis],
572            &mut record.angular_motor_force[axis],
573        )
574    }
575}
dynamis_world/constraint.rs

dynamis_world/
constraint.rs
