1use super::World;
2use super::ids::IdSpace;
3use dynamis_layout::{ConstraintCommandRecord, ConstraintDescriptorRecord};
4use dynamis_model::{
5 BodyHandle, ConstraintBreak, ConstraintDesc, ConstraintHandle, ConstraintKind, ConstraintLimit,
6 ConstraintMotor, ConstraintSpring, ConstraintSwing, DofDesc,
7};
8
9pub(crate) struct Constraints {
10 pub(crate) alive: Vec<ConstraintHandle>,
11 pub(crate) ids: IdSpace,
12 pub(crate) index_of: Vec<u32>,
13 pub(crate) records: Vec<ConstraintDescriptorRecord>,
14 pub(crate) commands: Vec<ConstraintCommandRecord>,
15 pub(crate) dirty: Vec<u32>,
16 pub(crate) last_moves: u32,
17 pub(crate) last_commands: u32,
18 pub(crate) broken: Vec<ConstraintHandle>,
19}
20
21impl Constraints {
22 pub(crate) const fn new() -> Self {
23 Self {
24 alive: Vec::new(),
25 ids: IdSpace::new(),
26 index_of: Vec::new(),
27 records: Vec::new(),
28 commands: Vec::new(),
29 dirty: Vec::new(),
30 last_moves: 0,
31 last_commands: 0,
32 broken: Vec::new(),
33 }
34 }
35
36 fn grow_to(&mut self, id: u32) {
37 let rows = id as usize + 1;
38 if rows > self.index_of.len() {
39 self.index_of.resize(rows, u32::MAX);
40 }
41 }
42}
43
44impl World {
45 pub fn add_constraint(
46 &mut self,
47 first: BodyHandle,
48 second: BodyHandle,
49 desc: ConstraintDesc,
50 ) -> ConstraintHandle {
51 self.validate(first);
52 self.validate(second);
53 if first == second {
54 panic!("constraint bodies must be distinct");
55 }
56 self.validate_constraint_desc(&desc);
57 let mut desc = desc;
58 if desc.kind == ConstraintKind::SixDof
59 && desc.reference == [0.0, 0.0, 0.0, 1.0]
60 && let (Some(state_a), Some(state_b)) = (
61 self.state_snapshot(first.id as usize),
62 self.state_snapshot(second.id as usize),
63 )
64 {
65 desc.reference = relative_reference(state_a.orientation, state_b.orientation);
66 }
67 let (id, generation) = self.constraints.ids.acquire();
68 self.constraints.grow_to(id);
69 let handle = ConstraintHandle { id, generation };
70 let slot = self.constraints.alive.len() as u32;
71 self.constraints.index_of[id as usize] = slot;
72 self.constraints.alive.push(handle);
73 self.constraints.dirty.push(slot);
74 let record = ConstraintDescriptorRecord::build(
75 &desc,
76 self.bodies.index_of[first.id as usize],
77 self.bodies.index_of[second.id as usize],
78 );
79 self.constraints.records.push(record);
80 self.constraints
81 .commands
82 .push(ConstraintCommandRecord::add(slot, id, generation));
83 handle
84 }
85
86 pub fn update_constraint(&mut self, handle: ConstraintHandle, desc: ConstraintDesc) {
87 self.validate_constraint(handle);
88 self.validate_constraint_desc(&desc);
89 let slot = self.constraints.index_of[handle.id as usize] as usize;
90 let existing = self.constraints.records[slot];
91 let record = ConstraintDescriptorRecord::build(&desc, existing.a, existing.b);
92 self.constraints.records[slot] = record;
93 self.constraints.dirty.push(slot as u32);
94 }
95
96 pub fn set_motor(&mut self, handle: ConstraintHandle, target_velocity: f32, max_force: f32) {
97 assert!(max_force >= 0.0, "motor force must be non-negative");
98 self.patch_constraint(handle, |desc| {
99 let motor = desc.motor.get_or_insert(ConstraintMotor {
100 target_velocity: 0.0,
101 max_force: 0.0,
102 target_position: None,
103 stiffness: 0.0,
104 damping: 0.0,
105 });
106 motor.target_velocity = target_velocity;
107 motor.max_force = max_force;
108 motor.target_position = None;
109 motor.stiffness = 0.0;
110 motor.damping = 0.0;
111 });
112 }
113
114 pub fn set_limit(&mut self, handle: ConstraintHandle, limit: Option<ConstraintLimit>) {
115 self.patch_constraint(handle, |desc| {
116 desc.limit = limit;
117 });
118 }
119
120 pub fn set_spring(&mut self, handle: ConstraintHandle, spring: Option<ConstraintSpring>) {
121 self.patch_constraint(handle, |desc| {
122 desc.spring = spring;
123 });
124 }
125
126 pub fn set_break_threshold(
127 &mut self,
128 handle: ConstraintHandle,
129 threshold: Option<ConstraintBreak>,
130 ) {
131 self.patch_constraint(handle, |desc| {
132 desc.break_threshold = threshold;
133 });
134 }
135
136 pub fn set_warm_start(&mut self, handle: ConstraintHandle, warm_start: bool) {
137 self.patch_constraint(handle, |desc| {
138 desc.warm_start = warm_start;
139 });
140 }
141
142 pub fn set_servo(
143 &mut self,
144 handle: ConstraintHandle,
145 target_position: f32,
146 stiffness: f32,
147 damping: f32,
148 ) {
149 self.patch_constraint(handle, |desc| {
150 let motor = desc.motor.get_or_insert(ConstraintMotor {
151 target_velocity: 0.0,
152 max_force: 0.0,
153 target_position: None,
154 stiffness: 0.0,
155 damping: 0.0,
156 });
157 motor.target_position = Some(target_position);
158 motor.stiffness = stiffness;
159 motor.damping = damping;
160 });
161 }
162
163 pub fn set_swing_limits(&mut self, handle: ConstraintHandle, swing: Option<ConstraintSwing>) {
164 self.patch_constraint(handle, |desc| {
165 desc.swing = swing;
166 });
167 }
168
169 pub fn set_constraint_disable_collisions(&mut self, handle: ConstraintHandle, disable: bool) {
170 self.patch_constraint(handle, |desc| {
171 desc.disable_collisions = disable;
172 });
173 }
174
175 fn patch_constraint(
176 &mut self,
177 handle: ConstraintHandle,
178 change: impl FnOnce(&mut ConstraintDesc),
179 ) {
180 self.validate_constraint(handle);
181 let slot = self.constraints.index_of[handle.id as usize] as usize;
182 let mut desc = constraint_desc_from_record(&self.constraints.records[slot]);
183 change(&mut desc);
184 self.validate_constraint_desc(&desc);
185 let existing = self.constraints.records[slot];
186 let record = ConstraintDescriptorRecord::build(&desc, existing.a, existing.b);
187 self.constraints.records[slot] = record;
188 self.constraints.dirty.push(slot as u32);
189 }
190
191 pub fn remove_constraint(&mut self, handle: ConstraintHandle) {
192 self.validate_constraint(handle);
193 let id = handle.id as usize;
194 let slot = self.constraints.index_of[id] as usize;
195 let tail = self.constraints.alive.len() - 1;
196 self.constraints.alive.swap_remove(slot);
197 self.constraints.records.remove(slot);
198 if slot < tail {
199 let moved = self.constraints.alive[slot];
200 self.constraints.index_of[moved.id as usize] = slot as u32;
201 self.constraints.dirty.push(slot as u32);
202 self.constraints
203 .commands
204 .push(ConstraintCommandRecord::swap(slot as u32, tail as u32));
205 }
206 self.constraints.index_of[id] = u32::MAX;
207 self.constraints.ids.release(handle.id);
208 self.constraints.dirty.retain(|dirty| *dirty != tail as u32);
209 }
210
211 pub fn constraints(&self) -> &[ConstraintHandle] {
212 &self.constraints.alive
213 }
214
215 fn validate_constraint(&self, handle: ConstraintHandle) {
216 let id = handle.id as usize;
217 if id >= self.constraints.ids.len() {
218 panic!("constraint handle {handle:?} is out of range");
219 }
220 if self.constraints.ids.generation(handle.id) != handle.generation {
221 panic!("constraint handle {handle:?} is stale");
222 }
223 if self.constraints.index_of[id] == u32::MAX {
224 panic!("constraint handle {handle:?} is not alive");
225 }
226 }
227
228 fn validate_constraint_desc(&self, desc: &ConstraintDesc) {
229 match desc.kind {
230 ConstraintKind::Ball
231 | ConstraintKind::Distance
232 | ConstraintKind::Pulley
233 | ConstraintKind::Gear => {}
234 ConstraintKind::Cone => {
235 if desc.axis_a == [0.0; 3] || desc.axis_b == [0.0; 3] {
236 panic!("constraint axis must be non-zero");
237 }
238 }
239 _ => {
240 if desc.axis_a == [0.0; 3] {
241 panic!("constraint axis must be non-zero");
242 }
243 }
244 }
245 }
246
247 pub(super) fn remap_constraint_slots(&mut self, first: u32, second: u32) {
248 for index in 0..self.constraints.records.len() {
249 let record = &mut self.constraints.records[index];
250 let mut changed = false;
251 if record.a == first && record.b == second {
252 record.a = second;
253 record.b = first;
254 changed = true;
255 } else if record.a == second && record.b == first {
256 record.a = first;
257 record.b = second;
258 changed = true;
259 } else {
260 if record.a == first {
261 record.a = second;
262 changed = true;
263 } else if record.a == second {
264 record.a = first;
265 changed = true;
266 }
267 if record.b == first {
268 record.b = second;
269 changed = true;
270 } else if record.b == second {
271 record.b = first;
272 changed = true;
273 }
274 }
275 if changed {
276 self.constraints.dirty.push(index as u32);
277 }
278 }
279 }
280
281 pub(super) fn assert_no_constraints(&self, handle: BodyHandle) {
282 for constraint in &self.constraints.alive {
283 if constraint.id == handle.id {
284 panic!(
285 "body handle {handle:?} is referenced by a live constraint; remove it first"
286 );
287 }
288 }
289 }
290}
291
292fn relative_reference(orientation_a: [f32; 4], orientation_b: [f32; 4]) -> [f32; 4] {
293 let a = [
294 -orientation_a[0],
295 -orientation_a[1],
296 -orientation_a[2],
297 orientation_a[3],
298 ];
299 dynamis_math::quat_mul(a, orientation_b)
300}
301
302fn constraint_desc_from_record(record: &ConstraintDescriptorRecord) -> ConstraintDesc {
303 let kind = match record.kind {
304 dynamis_layout::CONSTRAINT_BALL => ConstraintKind::Ball,
305 dynamis_layout::CONSTRAINT_DISTANCE => ConstraintKind::Distance,
306 dynamis_layout::CONSTRAINT_REVOLUTE => ConstraintKind::Revolute,
307 dynamis_layout::CONSTRAINT_PRISMATIC => ConstraintKind::Prismatic,
308 dynamis_layout::CONSTRAINT_FIXED => ConstraintKind::Fixed,
309 dynamis_layout::CONSTRAINT_GEAR => ConstraintKind::Gear,
310 dynamis_layout::CONSTRAINT_CONE => ConstraintKind::Cone,
311 dynamis_layout::CONSTRAINT_SIXDOF => ConstraintKind::SixDof,
312 other => panic!("constraint record has an invalid kind {other}"),
313 };
314 let mut desc = ConstraintDesc::ball(record.anchor_a, record.anchor_b).rekind(kind);
315 desc.axis_a = record.axis_a;
316 desc.axis_b = record.axis_b;
317 desc.reference = record.reference;
318 desc.rest_length = record.distance;
319 desc.cone_angle = record.cone_angle;
320 desc.warm_start = record.flags & dynamis_layout::CONSTRAINT_WARM_START != 0;
321 if record.flags & dynamis_layout::CONSTRAINT_HAS_LIMIT != 0 {
322 desc.limit = Some(ConstraintLimit {
323 min: record.limit_min,
324 max: record.limit_max,
325 });
326 }
327 if record.flags & dynamis_layout::CONSTRAINT_HAS_SWING != 0 {
328 desc.swing = Some(ConstraintSwing {
329 swing_a: record.swing_a,
330 swing_b: record.swing_b,
331 });
332 }
333 if record.flags & dynamis_layout::CONSTRAINT_HAS_MOTOR != 0 {
334 desc.motor = Some(ConstraintMotor {
335 target_velocity: record.motor_speed,
336 max_force: record.motor_max_force,
337 target_position: (record.motor_stiffness > 0.0).then_some(record.motor_target),
338 stiffness: record.motor_stiffness,
339 damping: record.motor_damping,
340 });
341 }
342 if record.flags & dynamis_layout::CONSTRAINT_IS_SPRING != 0 {
343 desc.spring = Some(ConstraintSpring {
344 frequency: record.spring_frequency,
345 damping_ratio: record.spring_damping_ratio,
346 });
347 }
348 if record.flags & dynamis_layout::CONSTRAINT_HAS_BREAK != 0 {
349 desc.break_threshold = Some(ConstraintBreak {
350 force: record.break_force,
351 torque: record.break_torque,
352 });
353 }
354 desc.gear_ratio = record.gear_ratio;
355 desc.pulley_fixed_a = record.pulley_fixed_a;
356 desc.pulley_fixed_b = record.pulley_fixed_b;
357 desc.disable_collisions = record.flags & dynamis_layout::CONSTRAINT_DISABLE_COLLISIONS != 0;
358 if desc.kind == ConstraintKind::SixDof || (record.kind == dynamis_layout::CONSTRAINT_SIXDOF) {
359 let mode_of = |index: u32| -> DofDesc {
360 let mode = dynamis_layout::dof_mode(record.flags, index);
361 match mode {
362 dynamis_layout::DOF_LOCKED => DofDesc::locked(),
363 dynamis_layout::DOF_LIMITED => {
364 let (min, max) = if index < 3 {
365 (
366 record.linear_limit_min[index as usize],
367 record.linear_limit_max[index as usize],
368 )
369 } else {
370 (
371 record.angular_limit_min[index as usize - 3],
372 record.angular_limit_max[index as usize - 3],
373 )
374 };
375 DofDesc::limited(min, max)
376 }
377 dynamis_layout::DOF_DRIVEN => {
378 let (target, stiffness, damping, force) = if index < 3 {
379 (
380 record.linear_motor_target[index as usize],
381 record.linear_motor_stiffness[index as usize],
382 record.linear_motor_damping[index as usize],
383 record.linear_motor_force[index as usize],
384 )
385 } else {
386 (
387 record.angular_motor_target[index as usize - 3],
388 record.angular_motor_stiffness[index as usize - 3],
389 record.angular_motor_damping[index as usize - 3],
390 record.angular_motor_force[index as usize - 3],
391 )
392 };
393 DofDesc::driven(ConstraintMotor {
394 target_velocity: if stiffness <= 0.0 { target } else { 0.0 },
395 max_force: force,
396 target_position: (stiffness > 0.0).then_some(target),
397 stiffness,
398 damping,
399 })
400 }
401 _ => DofDesc::free(),
402 }
403 };
404 desc.dofs = Some(std::array::from_fn(|index| mode_of(index as u32)));
405 }
406 desc
407}