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)]
15pub struct ConstraintLimit {
16 pub min: f32,
17 pub max: f32,
18}
19
20#[derive(Clone, Copy, Debug)]
21pub struct ConstraintMotor {
22 pub target_velocity: f32,
23 pub max_force: f32,
24 pub target_position: Option<f32>,
25 pub stiffness: f32,
26 pub damping: f32,
27}
28
29#[derive(Clone, Copy, Debug)]
30pub struct ConstraintSpring {
31 pub frequency: f32,
32 pub damping_ratio: f32,
33}
34
35#[derive(Clone, Copy, Debug)]
36pub struct ConstraintSwing {
37 pub swing_a: f32,
38 pub swing_b: f32,
39}
40
41#[derive(Clone, Copy, Debug)]
42pub struct ConstraintBreak {
43 pub force: f32,
44 pub torque: f32,
45}
46
47#[derive(Clone, Copy, Debug)]
48pub struct DofDesc {
49 pub locked: bool,
50 pub limit: Option<ConstraintLimit>,
51 pub motor: Option<ConstraintMotor>,
52}
53
54impl DofDesc {
55 pub fn free() -> Self {
56 Self {
57 locked: false,
58 limit: None,
59 motor: None,
60 }
61 }
62
63 pub fn locked() -> Self {
64 Self {
65 locked: true,
66 limit: None,
67 motor: None,
68 }
69 }
70
71 pub fn limited(min: f32, max: f32) -> Self {
72 assert!(max >= min, "dof limit max must not be below min");
73 Self {
74 locked: false,
75 limit: Some(ConstraintLimit { min, max }),
76 motor: None,
77 }
78 }
79
80 pub fn driven(motor: ConstraintMotor) -> Self {
81 Self {
82 locked: false,
83 limit: None,
84 motor: Some(motor),
85 }
86 }
87}
88
89#[derive(Clone, Copy, Debug)]
90pub struct ConstraintDesc {
91 pub kind: ConstraintKind,
92 pub anchor_a: [f32; 3],
93 pub anchor_b: [f32; 3],
94 pub axis_a: [f32; 3],
95 pub axis_b: [f32; 3],
96 pub reference: [f32; 4],
97 pub rest_length: f32,
98 pub limit: Option<ConstraintLimit>,
99 pub swing: Option<ConstraintSwing>,
100 pub motor: Option<ConstraintMotor>,
101 pub spring: Option<ConstraintSpring>,
102 pub break_threshold: Option<ConstraintBreak>,
103 pub gear_ratio: f32,
104 pub pulley_fixed_a: [f32; 3],
105 pub pulley_fixed_b: [f32; 3],
106 pub cone_angle: f32,
107 pub dofs: Option<[DofDesc; 6]>,
108 pub warm_start: bool,
109 pub disable_collisions: bool,
110}
111
112impl ConstraintDesc {
113 fn base(kind: ConstraintKind) -> Self {
114 Self {
115 kind,
116 anchor_a: [0.0; 3],
117 anchor_b: [0.0; 3],
118 axis_a: [0.0, 1.0, 0.0],
119 axis_b: [0.0, 1.0, 0.0],
120 reference: [0.0, 0.0, 0.0, 1.0],
121 rest_length: 0.0,
122 limit: None,
123 swing: None,
124 motor: None,
125 spring: None,
126 break_threshold: None,
127 gear_ratio: 1.0,
128 pulley_fixed_a: [0.0; 3],
129 pulley_fixed_b: [0.0; 3],
130 cone_angle: 0.0,
131 dofs: None,
132 warm_start: true,
133 disable_collisions: true,
134 }
135 }
136
137 pub fn ball(anchor_a: [f32; 3], anchor_b: [f32; 3]) -> Self {
138 Self::base(ConstraintKind::Ball).anchors(anchor_a, anchor_b)
139 }
140
141 pub fn rekind(mut self, kind: ConstraintKind) -> Self {
142 self.kind = kind;
143 self
144 }
145
146 pub fn distance(anchor_a: [f32; 3], anchor_b: [f32; 3], distance: f32) -> Self {
147 assert!(distance >= 0.0, "constraint distance must be non-negative");
148 Self::base(ConstraintKind::Distance)
149 .anchors(anchor_a, anchor_b)
150 .rest_length(distance)
151 }
152
153 pub fn revolute(anchor_a: [f32; 3], anchor_b: [f32; 3], axis: [f32; 3]) -> Self {
154 assert!(axis != [0.0; 3], "revolute axis must be non-zero");
155 Self::base(ConstraintKind::Revolute)
156 .anchors(anchor_a, anchor_b)
157 .axis(axis)
158 }
159
160 pub fn prismatic(anchor_a: [f32; 3], anchor_b: [f32; 3], axis: [f32; 3]) -> Self {
161 assert!(axis != [0.0; 3], "prismatic axis must be non-zero");
162 Self::base(ConstraintKind::Prismatic)
163 .anchors(anchor_a, anchor_b)
164 .axis(axis)
165 }
166
167 pub fn fixed(anchor_a: [f32; 3], anchor_b: [f32; 3]) -> Self {
168 Self::base(ConstraintKind::Fixed).anchors(anchor_a, anchor_b)
169 }
170
171 pub fn gear(axis_a: [f32; 3], axis_b: [f32; 3], ratio: f32) -> Self {
172 assert!(axis_a != [0.0; 3], "gear axis a must be non-zero");
173 assert!(axis_b != [0.0; 3], "gear axis b must be non-zero");
174 assert!(ratio != 0.0, "gear ratio must be non-zero");
175 let mut desc = Self::base(ConstraintKind::Gear).axis(axis_a);
176 desc.axis_b = axis_b;
177 desc.gear_ratio = ratio;
178 desc
179 }
180
181 pub fn pulley(
182 anchor_a: [f32; 3],
183 anchor_b: [f32; 3],
184 fixed_a: [f32; 3],
185 fixed_b: [f32; 3],
186 length: f32,
187 ) -> Self {
188 assert!(length > 0.0, "pulley length must be positive");
189 let mut desc = Self::base(ConstraintKind::Pulley)
190 .anchors(anchor_a, anchor_b)
191 .rest_length(length);
192 desc.pulley_fixed_a = fixed_a;
193 desc.pulley_fixed_b = fixed_b;
194 desc
195 }
196
197 pub fn cone(anchor_a: [f32; 3], anchor_b: [f32; 3], axis: [f32; 3], half_angle: f32) -> Self {
198 assert!(axis != [0.0; 3], "cone axis must be non-zero");
199 assert!(
200 (0.0..=std::f32::consts::PI).contains(&half_angle),
201 "cone half angle must be within [0, pi]"
202 );
203 let mut desc = Self::base(ConstraintKind::Cone)
204 .anchors(anchor_a, anchor_b)
205 .axis(axis);
206 desc.axis_b = axis;
207 desc.cone_angle = half_angle;
208 desc
209 }
210
211 pub fn six_dof(
212 anchor_a: [f32; 3],
213 anchor_b: [f32; 3],
214 axis_a: [f32; 3],
215 axis_b: [f32; 3],
216 ) -> Self {
217 assert!(axis_a != [0.0; 3], "six dof axis a must be non-zero");
218 assert!(axis_b != [0.0; 3], "six dof axis b must be non-zero");
219 let mut desc = Self::base(ConstraintKind::SixDof)
220 .anchors(anchor_a, anchor_b)
221 .axis(axis_a);
222 desc.axis_b = axis_b;
223 desc.dofs = Some([DofDesc::free(); 6]);
224 desc
225 }
226
227 pub fn dofs(mut self, dofs: [DofDesc; 6]) -> Self {
228 assert_eq!(
229 self.kind,
230 ConstraintKind::SixDof,
231 "dofs require a six dof constraint"
232 );
233 self.dofs = Some(dofs);
234 self
235 }
236
237 pub fn dof(mut self, index: usize, desc: DofDesc) -> Self {
238 assert_eq!(
239 self.kind,
240 ConstraintKind::SixDof,
241 "dofs require a six dof constraint"
242 );
243 assert!(index < 6, "dof index must be within 0..6");
244 let dofs = self.dofs.get_or_insert([DofDesc::free(); 6]);
245 dofs[index] = desc;
246 self
247 }
248
249 pub fn anchors(mut self, anchor_a: [f32; 3], anchor_b: [f32; 3]) -> Self {
250 self.anchor_a = anchor_a;
251 self.anchor_b = anchor_b;
252 self
253 }
254
255 pub fn axis(mut self, axis: [f32; 3]) -> Self {
256 assert!(axis != [0.0; 3], "constraint axis must be non-zero");
257 self.axis_a = axis;
258 self
259 }
260
261 pub fn axis_b(mut self, axis_b: [f32; 3]) -> Self {
262 assert!(axis_b != [0.0; 3], "constraint axis b must be non-zero");
263 self.axis_b = axis_b;
264 self
265 }
266
267 pub fn reference(mut self, reference: [f32; 4]) -> Self {
268 assert!(
269 (reference[0] * reference[0]
270 + reference[1] * reference[1]
271 + reference[2] * reference[2]
272 + reference[3] * reference[3]
273 - 1.0)
274 .abs()
275 < 1e-4,
276 "reference must be a unit quaternion"
277 );
278 self.reference = reference;
279 self
280 }
281
282 pub fn rest_length(mut self, rest_length: f32) -> Self {
283 assert!(
284 rest_length >= 0.0,
285 "constraint distance must be non-negative"
286 );
287 self.rest_length = rest_length;
288 self
289 }
290
291 pub fn limit(mut self, min: f32, max: f32) -> Self {
292 assert!(max >= min, "constraint limit max must not be below min");
293 self.limit = Some(ConstraintLimit { min, max });
294 self
295 }
296
297 pub fn swing(mut self, swing_a: f32, swing_b: f32) -> Self {
298 assert!(swing_a >= 0.0, "swing limit must be non-negative");
299 assert!(swing_b >= 0.0, "swing limit must be non-negative");
300 self.swing = Some(ConstraintSwing { swing_a, swing_b });
301 self
302 }
303
304 pub fn motor(mut self, target_velocity: f32) -> Self {
305 let motor = self.motor.get_or_insert(ConstraintMotor {
306 target_velocity,
307 max_force: 0.0,
308 target_position: None,
309 stiffness: 0.0,
310 damping: 0.0,
311 });
312 motor.target_velocity = target_velocity;
313 self
314 }
315
316 pub fn motor_force(mut self, max_force: f32) -> Self {
317 assert!(max_force >= 0.0, "motor force must be non-negative");
318 let motor = self.motor.get_or_insert(ConstraintMotor {
319 target_velocity: 0.0,
320 max_force: 0.0,
321 target_position: None,
322 stiffness: 0.0,
323 damping: 0.0,
324 });
325 motor.max_force = max_force;
326 self
327 }
328
329 pub fn servo(mut self, target_position: f32, stiffness: f32, damping: f32) -> Self {
330 assert!(
331 (0.0..=1.0).contains(&stiffness),
332 "servo stiffness must be within [0, 1]"
333 );
334 assert!(
335 (0.0..=1.0).contains(&damping),
336 "servo damping must be within [0, 1]"
337 );
338 let motor = self.motor.get_or_insert(ConstraintMotor {
339 target_velocity: 0.0,
340 max_force: 0.0,
341 target_position: None,
342 stiffness: 0.0,
343 damping: 0.0,
344 });
345 motor.target_position = Some(target_position);
346 motor.stiffness = stiffness;
347 motor.damping = damping;
348 self
349 }
350
351 pub fn spring(mut self, frequency: f32, damping_ratio: f32) -> Self {
352 assert!(frequency >= 0.0, "spring frequency must be non-negative");
353 assert!(
354 damping_ratio >= 0.0,
355 "spring damping ratio must be non-negative"
356 );
357 self.spring = Some(ConstraintSpring {
358 frequency,
359 damping_ratio,
360 });
361 self
362 }
363
364 pub fn break_threshold(mut self, force: f32, torque: f32) -> Self {
365 assert!(force >= 0.0, "break force must be non-negative");
366 assert!(torque >= 0.0, "break torque must be non-negative");
367 self.break_threshold = Some(ConstraintBreak { force, torque });
368 self
369 }
370
371 pub fn warm_start(mut self, warm_start: bool) -> Self {
372 self.warm_start = warm_start;
373 self
374 }
375
376 pub fn disable_collisions(mut self, disable: bool) -> Self {
377 self.disable_collisions = disable;
378 self
379 }
380}
381
382#[derive(Clone, Copy, Debug, PartialEq)]
383pub struct ConstraintHandle {
384 pub id: u32,
385 pub generation: u32,
386}