1#[derive(Clone, Copy, Debug, PartialEq, Eq)]
2pub enum ConstraintKind {
3 Ball,
4 Distance,
5 Revolute,
6 Prismatic,
7 Fixed,
8 Gear,
9 Pulley,
10 Cone,
11 SixDof,
12}
13
14#[derive(Clone, Copy, Debug, PartialEq, Eq)]
15pub enum JointDof {
16 Separation,
17 Hinge,
18 Slide,
19 SwingA,
20 SwingB,
21 Twist,
22 Cone,
23 Rope,
24 Linear(usize),
25 Angular(usize),
26}
27
28impl ConstraintKind {
29 pub const fn dofs(self) -> &'static [JointDof] {
30 match self {
31 Self::Distance => &[JointDof::Separation],
32 Self::Revolute => &[JointDof::Hinge],
33 Self::Prismatic => &[JointDof::Slide],
34 Self::Ball => &[JointDof::SwingA, JointDof::SwingB, JointDof::Twist],
35 Self::Cone => &[JointDof::Cone],
36 Self::Gear => &[],
37 Self::Pulley => &[JointDof::Rope],
38 Self::SixDof => &[
39 JointDof::Linear(0),
40 JointDof::Linear(1),
41 JointDof::Linear(2),
42 JointDof::Angular(0),
43 JointDof::Angular(1),
44 JointDof::Angular(2),
45 ],
46 Self::Fixed => &[],
47 }
48 }
49}
50
51#[derive(Clone, Copy, Debug, PartialEq)]
52pub struct JointState {
53 kind: ConstraintKind,
54 coordinates: [f32; 6],
55 rates: [f32; 6],
56 impulses: [f32; 6],
57}
58
59impl JointState {
60 pub fn new(
61 kind: ConstraintKind,
62 coordinates: [f32; 6],
63 rates: [f32; 6],
64 impulses: [f32; 6],
65 ) -> Self {
66 Self {
67 kind,
68 coordinates,
69 rates,
70 impulses,
71 }
72 }
73
74 pub const fn kind(&self) -> ConstraintKind {
75 self.kind
76 }
77
78 pub const fn dofs(&self) -> &'static [JointDof] {
79 self.kind.dofs()
80 }
81
82 pub fn coordinate(&self, dof: JointDof) -> f32 {
83 self.coordinates[self.slot(dof)]
84 }
85
86 pub fn rate(&self, dof: JointDof) -> f32 {
87 self.rates[self.slot(dof)]
88 }
89
90 pub fn impulse(&self, dof: JointDof) -> f32 {
91 self.impulses[self.slot(dof)]
92 }
93
94 fn slot(&self, dof: JointDof) -> usize {
95 self.dofs()
96 .iter()
97 .position(|candidate| *candidate == dof)
98 .unwrap_or_else(|| panic!("joint dof {dof:?} is outside the {:?} layout", self.kind))
99 }
100}
101
102#[derive(Clone, Copy, Debug)]
103pub struct ConstraintLimit {
104 pub min: f32,
105 pub max: f32,
106}
107
108#[derive(Clone, Copy, Debug)]
109pub struct ConstraintMotor {
110 pub target_velocity: f32,
111 pub max_force: f32,
112 pub target_position: Option<f32>,
113 pub stiffness: f32,
114 pub damping: f32,
115}
116
117#[derive(Clone, Copy, Debug)]
118pub struct ConstraintSpring {
119 pub frequency: f32,
120 pub damping_ratio: f32,
121}
122
123#[derive(Clone, Copy, Debug)]
124pub struct ConstraintSwing {
125 pub swing_a: f32,
126 pub swing_b: f32,
127}
128
129#[derive(Clone, Copy, Debug)]
130pub struct ConstraintBreak {
131 pub force: f32,
132 pub torque: f32,
133}
134
135#[derive(Clone, Copy, Debug)]
136pub struct DofDesc {
137 pub locked: bool,
138 pub limit: Option<ConstraintLimit>,
139 pub motor: Option<ConstraintMotor>,
140}
141
142impl DofDesc {
143 pub fn free() -> Self {
144 Self {
145 locked: false,
146 limit: None,
147 motor: None,
148 }
149 }
150
151 pub fn locked() -> Self {
152 Self::free().lock()
153 }
154
155 pub fn limited(min: f32, max: f32) -> Self {
156 Self::free().limit(min, max)
157 }
158
159 pub fn driven(motor: ConstraintMotor) -> Self {
160 Self::free().motor(motor)
161 }
162
163 pub fn lock(mut self) -> Self {
164 self.locked = true;
165 self
166 }
167
168 pub fn limit(mut self, min: f32, max: f32) -> Self {
169 assert!(max >= min, "dof limit max must not be below min");
170 self.limit = Some(ConstraintLimit { min, max });
171 self
172 }
173
174 pub fn motor(mut self, motor: ConstraintMotor) -> Self {
175 self.motor = Some(motor);
176 self
177 }
178}
179
180#[derive(Clone, Copy, Debug)]
181pub struct ConstraintDesc {
182 pub kind: ConstraintKind,
183 pub anchor_a: [f32; 3],
184 pub anchor_b: [f32; 3],
185 pub axis_a: [f32; 3],
186 pub axis_b: [f32; 3],
187 pub reference: [f32; 4],
188 pub rest_length: f32,
189 pub limit: Option<ConstraintLimit>,
190 pub swing: Option<ConstraintSwing>,
191 pub motor: Option<ConstraintMotor>,
192 pub spring: Option<ConstraintSpring>,
193 pub break_threshold: Option<ConstraintBreak>,
194 pub gear_ratio: f32,
195 pub pulley_fixed_a: [f32; 3],
196 pub pulley_fixed_b: [f32; 3],
197 pub cone_angle: f32,
198 pub dofs: Option<[DofDesc; 6]>,
199 pub warm_start: bool,
200 pub disable_collisions: bool,
201}
202
203impl ConstraintDesc {
204 fn base(kind: ConstraintKind) -> Self {
205 Self {
206 kind,
207 anchor_a: [0.0; 3],
208 anchor_b: [0.0; 3],
209 axis_a: [0.0, 1.0, 0.0],
210 axis_b: [0.0, 1.0, 0.0],
211 reference: [0.0, 0.0, 0.0, 1.0],
212 rest_length: 0.0,
213 limit: None,
214 swing: None,
215 motor: None,
216 spring: None,
217 break_threshold: None,
218 gear_ratio: 1.0,
219 pulley_fixed_a: [0.0; 3],
220 pulley_fixed_b: [0.0; 3],
221 cone_angle: 0.0,
222 dofs: None,
223 warm_start: true,
224 disable_collisions: true,
225 }
226 }
227
228 pub fn ball(anchor_a: [f32; 3], anchor_b: [f32; 3]) -> Self {
229 Self::base(ConstraintKind::Ball).anchors(anchor_a, anchor_b)
230 }
231
232 pub fn rekind(mut self, kind: ConstraintKind) -> Self {
233 self.kind = kind;
234 self
235 }
236
237 pub fn distance(anchor_a: [f32; 3], anchor_b: [f32; 3], distance: f32) -> Self {
238 assert!(distance >= 0.0, "constraint distance must be non-negative");
239 Self::base(ConstraintKind::Distance)
240 .anchors(anchor_a, anchor_b)
241 .rest_length(distance)
242 }
243
244 pub fn revolute(anchor_a: [f32; 3], anchor_b: [f32; 3], axis: [f32; 3]) -> Self {
245 assert!(axis != [0.0; 3], "revolute axis must be non-zero");
246 Self::base(ConstraintKind::Revolute)
247 .anchors(anchor_a, anchor_b)
248 .axis(axis)
249 }
250
251 pub fn prismatic(anchor_a: [f32; 3], anchor_b: [f32; 3], axis: [f32; 3]) -> Self {
252 assert!(axis != [0.0; 3], "prismatic axis must be non-zero");
253 Self::base(ConstraintKind::Prismatic)
254 .anchors(anchor_a, anchor_b)
255 .axis(axis)
256 }
257
258 pub fn fixed(anchor_a: [f32; 3], anchor_b: [f32; 3]) -> Self {
259 Self::base(ConstraintKind::Fixed).anchors(anchor_a, anchor_b)
260 }
261
262 pub fn gear(axis_a: [f32; 3], axis_b: [f32; 3], ratio: f32) -> Self {
263 assert!(axis_a != [0.0; 3], "gear axis a must be non-zero");
264 assert!(axis_b != [0.0; 3], "gear axis b must be non-zero");
265 assert!(ratio != 0.0, "gear ratio must be non-zero");
266 let mut desc = Self::base(ConstraintKind::Gear).axis(axis_a);
267 desc.axis_b = axis_b;
268 desc.gear_ratio = ratio;
269 desc
270 }
271
272 pub fn pulley(
273 anchor_a: [f32; 3],
274 anchor_b: [f32; 3],
275 fixed_a: [f32; 3],
276 fixed_b: [f32; 3],
277 length: f32,
278 ) -> Self {
279 assert!(length > 0.0, "pulley length must be positive");
280 let mut desc = Self::base(ConstraintKind::Pulley)
281 .anchors(anchor_a, anchor_b)
282 .rest_length(length);
283 desc.pulley_fixed_a = fixed_a;
284 desc.pulley_fixed_b = fixed_b;
285 desc
286 }
287
288 pub fn cone(anchor_a: [f32; 3], anchor_b: [f32; 3], axis: [f32; 3], half_angle: f32) -> Self {
289 assert!(axis != [0.0; 3], "cone axis must be non-zero");
290 assert!(
291 (0.0..=std::f32::consts::PI).contains(&half_angle),
292 "cone half angle must be within [0, pi]"
293 );
294 let mut desc = Self::base(ConstraintKind::Cone)
295 .anchors(anchor_a, anchor_b)
296 .axis(axis);
297 desc.axis_b = axis;
298 desc.cone_angle = half_angle;
299 desc
300 }
301
302 pub fn six_dof(
303 anchor_a: [f32; 3],
304 anchor_b: [f32; 3],
305 axis_a: [f32; 3],
306 axis_b: [f32; 3],
307 ) -> Self {
308 assert!(axis_a != [0.0; 3], "six dof axis a must be non-zero");
309 assert!(axis_b != [0.0; 3], "six dof axis b must be non-zero");
310 let mut desc = Self::base(ConstraintKind::SixDof)
311 .anchors(anchor_a, anchor_b)
312 .axis(axis_a);
313 desc.axis_b = axis_b;
314 desc.dofs = Some([DofDesc::free(); 6]);
315 desc
316 }
317
318 pub fn dofs(mut self, dofs: [DofDesc; 6]) -> Self {
319 assert_eq!(
320 self.kind,
321 ConstraintKind::SixDof,
322 "dofs require a six dof constraint"
323 );
324 self.dofs = Some(dofs);
325 self
326 }
327
328 pub fn dof(mut self, index: usize, desc: DofDesc) -> Self {
329 assert_eq!(
330 self.kind,
331 ConstraintKind::SixDof,
332 "dofs require a six dof constraint"
333 );
334 assert!(index < 6, "dof index must be within 0..6");
335 let dofs = self.dofs.get_or_insert([DofDesc::free(); 6]);
336 dofs[index] = desc;
337 self
338 }
339
340 pub fn anchors(mut self, anchor_a: [f32; 3], anchor_b: [f32; 3]) -> Self {
341 self.anchor_a = anchor_a;
342 self.anchor_b = anchor_b;
343 self
344 }
345
346 pub fn axis(mut self, axis: [f32; 3]) -> Self {
347 assert!(axis != [0.0; 3], "constraint axis must be non-zero");
348 self.axis_a = axis;
349 self
350 }
351
352 pub fn axis_b(mut self, axis_b: [f32; 3]) -> Self {
353 assert!(axis_b != [0.0; 3], "constraint axis b must be non-zero");
354 self.axis_b = axis_b;
355 self
356 }
357
358 pub fn reference(mut self, reference: [f32; 4]) -> Self {
359 assert!(
360 (reference[0] * reference[0]
361 + reference[1] * reference[1]
362 + reference[2] * reference[2]
363 + reference[3] * reference[3]
364 - 1.0)
365 .abs()
366 < 1e-4,
367 "reference must be a unit quaternion"
368 );
369 self.reference = reference;
370 self
371 }
372
373 pub fn rest_length(mut self, rest_length: f32) -> Self {
374 assert!(
375 rest_length >= 0.0,
376 "constraint distance must be non-negative"
377 );
378 self.rest_length = rest_length;
379 self
380 }
381
382 pub fn limit(mut self, min: f32, max: f32) -> Self {
383 assert!(max >= min, "constraint limit max must not be below min");
384 self.limit = Some(ConstraintLimit { min, max });
385 self
386 }
387
388 pub fn swing(mut self, swing_a: f32, swing_b: f32) -> Self {
389 assert!(swing_a >= 0.0, "swing limit must be non-negative");
390 assert!(swing_b >= 0.0, "swing limit must be non-negative");
391 self.swing = Some(ConstraintSwing { swing_a, swing_b });
392 self
393 }
394
395 pub fn motor(mut self, target_velocity: f32) -> Self {
396 let motor = self.motor.get_or_insert(ConstraintMotor {
397 target_velocity,
398 max_force: 0.0,
399 target_position: None,
400 stiffness: 0.0,
401 damping: 0.0,
402 });
403 motor.target_velocity = target_velocity;
404 self
405 }
406
407 pub fn motor_force(mut self, max_force: f32) -> Self {
408 assert!(max_force >= 0.0, "motor force must be non-negative");
409 let motor = self.motor.get_or_insert(ConstraintMotor {
410 target_velocity: 0.0,
411 max_force: 0.0,
412 target_position: None,
413 stiffness: 0.0,
414 damping: 0.0,
415 });
416 motor.max_force = max_force;
417 self
418 }
419
420 pub fn servo(mut self, target_position: f32, stiffness: f32, damping: f32) -> Self {
421 assert!(
422 (0.0..=1.0).contains(&stiffness),
423 "servo stiffness must be within [0, 1]"
424 );
425 assert!(
426 (0.0..=1.0).contains(&damping),
427 "servo damping must be within [0, 1]"
428 );
429 let motor = self.motor.get_or_insert(ConstraintMotor {
430 target_velocity: 0.0,
431 max_force: 0.0,
432 target_position: None,
433 stiffness: 0.0,
434 damping: 0.0,
435 });
436 motor.target_position = Some(target_position);
437 motor.stiffness = stiffness;
438 motor.damping = damping;
439 self
440 }
441
442 pub fn spring(mut self, frequency: f32, damping_ratio: f32) -> Self {
443 assert!(frequency >= 0.0, "spring frequency must be non-negative");
444 assert!(
445 damping_ratio >= 0.0,
446 "spring damping ratio must be non-negative"
447 );
448 self.spring = Some(ConstraintSpring {
449 frequency,
450 damping_ratio,
451 });
452 self
453 }
454
455 pub fn break_threshold(mut self, force: f32, torque: f32) -> Self {
456 assert!(force >= 0.0, "break force must be non-negative");
457 assert!(torque >= 0.0, "break torque must be non-negative");
458 self.break_threshold = Some(ConstraintBreak { force, torque });
459 self
460 }
461
462 pub fn warm_start(mut self, warm_start: bool) -> Self {
463 self.warm_start = warm_start;
464 self
465 }
466
467 pub fn disable_collisions(mut self, disable: bool) -> Self {
468 self.disable_collisions = disable;
469 self
470 }
471}
472
473#[derive(Clone, Copy, Debug, PartialEq)]
474pub struct ConstraintHandle {
475 pub id: u32,
476 pub generation: u32,
477}