1use crate::core::NULL_INDEX;
8use crate::math_functions::{
9 Matrix3, Quat, Transform, Vec2, Vec3, MAT3_ZERO, QUAT_IDENTITY, TRANSFORM_IDENTITY, VEC2_ZERO,
10 VEC3_ZERO,
11};
12use crate::solver::Softness;
13
14#[derive(Debug, Clone, Copy, PartialEq, Eq, Default)]
16#[repr(i32)]
17pub enum JointType {
18 #[default]
19 Parallel = 0,
20 Distance = 1,
21 Filter = 2,
22 Motor = 3,
23 Prismatic = 4,
24 Revolute = 5,
25 Spherical = 6,
26 Weld = 7,
27 Wheel = 8,
28}
29
30#[derive(Debug, Clone, Copy, PartialEq, Eq)]
32pub struct JointEdge {
33 pub body_id: i32,
34 pub prev_key: i32,
35 pub next_key: i32,
36}
37
38impl Default for JointEdge {
39 fn default() -> Self {
40 JointEdge {
41 body_id: NULL_INDEX,
42 prev_key: NULL_INDEX,
43 next_key: NULL_INDEX,
44 }
45 }
46}
47
48#[derive(Debug, Clone)]
50pub struct Joint {
51 pub user_data: u64,
52
53 pub set_index: i32,
55
56 pub color_index: i32,
59
60 pub local_index: i32,
62
63 pub edges: [JointEdge; 2],
64
65 pub joint_id: i32,
66 pub island_id: i32,
67
68 pub island_index: i32,
71
72 pub draw_scale: f32,
73
74 pub type_: JointType,
75
76 pub generation: u16,
78
79 pub collide_connected: bool,
80}
81
82impl Default for Joint {
83 fn default() -> Self {
84 Joint {
85 user_data: 0,
86 set_index: NULL_INDEX,
87 color_index: NULL_INDEX,
88 local_index: NULL_INDEX,
89 edges: [JointEdge::default(); 2],
90 joint_id: NULL_INDEX,
91 island_id: NULL_INDEX,
92 island_index: NULL_INDEX,
93 draw_scale: 1.0,
94 type_: JointType::Distance,
95 generation: 0,
96 collide_connected: false,
97 }
98 }
99}
100
101#[derive(Debug, Clone, Copy, PartialEq, Default)]
103pub struct DistanceJoint {
104 pub length: f32,
105 pub hertz: f32,
106 pub damping_ratio: f32,
107 pub lower_spring_force: f32,
108 pub upper_spring_force: f32,
109 pub min_length: f32,
110 pub max_length: f32,
111
112 pub max_motor_force: f32,
113 pub motor_speed: f32,
114
115 pub impulse: f32,
116 pub lower_impulse: f32,
117 pub upper_impulse: f32,
118 pub motor_impulse: f32,
119
120 pub index_a: i32,
121 pub index_b: i32,
122 pub anchor_a: Vec3,
123 pub anchor_b: Vec3,
124 pub delta_center: Vec3,
125 pub distance_softness: Softness,
126 pub axial_mass: f32,
127
128 pub enable_spring: bool,
129 pub enable_limit: bool,
130 pub enable_motor: bool,
131}
132
133#[derive(Debug, Clone, Copy, PartialEq)]
135pub struct MotorJoint {
136 pub linear_velocity: Vec3,
137 pub angular_velocity: Vec3,
138 pub max_velocity_force: f32,
139 pub max_velocity_torque: f32,
140 pub linear_hertz: f32,
141 pub linear_damping_ratio: f32,
142 pub max_spring_force: f32,
143 pub angular_hertz: f32,
144 pub angular_damping_ratio: f32,
145 pub max_spring_torque: f32,
146
147 pub linear_velocity_impulse: Vec3,
148 pub angular_velocity_impulse: Vec3,
149 pub linear_spring_impulse: Vec3,
150 pub angular_spring_impulse: Vec3,
151
152 pub linear_spring: Softness,
153 pub angular_spring: Softness,
154
155 pub index_a: i32,
156 pub index_b: i32,
157 pub frame_a: Transform,
158 pub frame_b: Transform,
159 pub delta_center: Vec3,
160 pub angular_mass: Matrix3,
161}
162
163impl Default for MotorJoint {
164 fn default() -> Self {
165 MotorJoint {
166 linear_velocity: VEC3_ZERO,
167 angular_velocity: VEC3_ZERO,
168 max_velocity_force: 0.0,
169 max_velocity_torque: 0.0,
170 linear_hertz: 0.0,
171 linear_damping_ratio: 0.0,
172 max_spring_force: 0.0,
173 angular_hertz: 0.0,
174 angular_damping_ratio: 0.0,
175 max_spring_torque: 0.0,
176 linear_velocity_impulse: VEC3_ZERO,
177 angular_velocity_impulse: VEC3_ZERO,
178 linear_spring_impulse: VEC3_ZERO,
179 angular_spring_impulse: VEC3_ZERO,
180 linear_spring: Softness::default(),
181 angular_spring: Softness::default(),
182 index_a: NULL_INDEX,
183 index_b: NULL_INDEX,
184 frame_a: TRANSFORM_IDENTITY,
185 frame_b: TRANSFORM_IDENTITY,
186 delta_center: VEC3_ZERO,
187 angular_mass: MAT3_ZERO,
188 }
189 }
190}
191
192#[derive(Debug, Clone, Copy, PartialEq)]
194pub struct ParallelJoint {
195 pub hertz: f32,
196 pub damping_ratio: f32,
197 pub max_torque: f32,
198
199 pub perp_impulse: Vec2,
200 pub perp_axis_x: Vec3,
201 pub perp_axis_y: Vec3,
202
203 pub quat_a: Quat,
204 pub quat_b: Quat,
205 pub index_a: i32,
206 pub index_b: i32,
207 pub softness: Softness,
208}
209
210impl Default for ParallelJoint {
211 fn default() -> Self {
212 ParallelJoint {
213 hertz: 0.0,
214 damping_ratio: 0.0,
215 max_torque: 0.0,
216 perp_impulse: VEC2_ZERO,
217 perp_axis_x: VEC3_ZERO,
218 perp_axis_y: VEC3_ZERO,
219 quat_a: QUAT_IDENTITY,
220 quat_b: QUAT_IDENTITY,
221 index_a: NULL_INDEX,
222 index_b: NULL_INDEX,
223 softness: Softness::default(),
224 }
225 }
226}
227
228#[derive(Debug, Clone, Copy, PartialEq)]
230pub struct PrismaticJoint {
231 pub perp_impulse: Vec2,
232 pub angular_impulse: Vec3,
233 pub spring_impulse: f32,
234 pub motor_impulse: f32,
235 pub lower_impulse: f32,
236 pub upper_impulse: f32,
237 pub hertz: f32,
238 pub damping_ratio: f32,
239 pub max_motor_force: f32,
240 pub motor_speed: f32,
241 pub target_translation: f32,
242 pub lower_translation: f32,
243 pub upper_translation: f32,
244
245 pub index_a: i32,
246 pub index_b: i32,
247 pub frame_a: Transform,
248 pub frame_b: Transform,
249 pub joint_axis: Vec3,
250 pub perp_axis_y: Vec3,
251 pub perp_axis_z: Vec3,
252 pub delta_center: Vec3,
253 pub delta_angle: f32,
254 pub rotation_mass: Matrix3,
255 pub spring_softness: Softness,
256
257 pub enable_spring: bool,
258 pub enable_limit: bool,
259 pub enable_motor: bool,
260}
261
262impl Default for PrismaticJoint {
263 fn default() -> Self {
264 PrismaticJoint {
265 perp_impulse: VEC2_ZERO,
266 angular_impulse: VEC3_ZERO,
267 spring_impulse: 0.0,
268 motor_impulse: 0.0,
269 lower_impulse: 0.0,
270 upper_impulse: 0.0,
271 hertz: 0.0,
272 damping_ratio: 0.0,
273 max_motor_force: 0.0,
274 motor_speed: 0.0,
275 target_translation: 0.0,
276 lower_translation: 0.0,
277 upper_translation: 0.0,
278 index_a: NULL_INDEX,
279 index_b: NULL_INDEX,
280 frame_a: TRANSFORM_IDENTITY,
281 frame_b: TRANSFORM_IDENTITY,
282 joint_axis: VEC3_ZERO,
283 perp_axis_y: VEC3_ZERO,
284 perp_axis_z: VEC3_ZERO,
285 delta_center: VEC3_ZERO,
286 delta_angle: 0.0,
287 rotation_mass: MAT3_ZERO,
288 spring_softness: Softness::default(),
289 enable_spring: false,
290 enable_limit: false,
291 enable_motor: false,
292 }
293 }
294}
295
296#[derive(Debug, Clone, Copy, PartialEq)]
298pub struct RevoluteJoint {
299 pub linear_impulse: Vec3,
300 pub perp_impulse: Vec2,
301 pub spring_impulse: f32,
302 pub motor_impulse: f32,
303 pub lower_impulse: f32,
304 pub upper_impulse: f32,
305 pub hertz: f32,
306 pub damping_ratio: f32,
307 pub max_motor_torque: f32,
308 pub motor_speed: f32,
309 pub target_angle: f32,
310 pub lower_angle: f32,
311 pub upper_angle: f32,
312
313 pub index_a: i32,
314 pub index_b: i32,
315 pub frame_a: Transform,
316 pub frame_b: Transform,
317 pub rotation_axis_z: Vec3,
318 pub perp_axis_x: Vec3,
319 pub perp_axis_y: Vec3,
320 pub delta_center: Vec3,
321 pub delta_angle: f32,
322 pub axial_mass: f32,
323 pub spring_softness: Softness,
324
325 pub enable_spring: bool,
326 pub enable_motor: bool,
327 pub enable_limit: bool,
328}
329
330impl Default for RevoluteJoint {
331 fn default() -> Self {
332 RevoluteJoint {
333 linear_impulse: VEC3_ZERO,
334 perp_impulse: VEC2_ZERO,
335 spring_impulse: 0.0,
336 motor_impulse: 0.0,
337 lower_impulse: 0.0,
338 upper_impulse: 0.0,
339 hertz: 0.0,
340 damping_ratio: 0.0,
341 max_motor_torque: 0.0,
342 motor_speed: 0.0,
343 target_angle: 0.0,
344 lower_angle: 0.0,
345 upper_angle: 0.0,
346 index_a: NULL_INDEX,
347 index_b: NULL_INDEX,
348 frame_a: TRANSFORM_IDENTITY,
349 frame_b: TRANSFORM_IDENTITY,
350 rotation_axis_z: VEC3_ZERO,
351 perp_axis_x: VEC3_ZERO,
352 perp_axis_y: VEC3_ZERO,
353 delta_center: VEC3_ZERO,
354 delta_angle: 0.0,
355 axial_mass: 0.0,
356 spring_softness: Softness::default(),
357 enable_spring: false,
358 enable_motor: false,
359 enable_limit: false,
360 }
361 }
362}
363
364#[derive(Debug, Clone, Copy, PartialEq)]
366pub struct SphericalJoint {
367 pub linear_impulse: Vec3,
368 pub spring_impulse: Vec3,
369 pub motor_impulse: Vec3,
370 pub lower_twist_impulse: f32,
371 pub upper_twist_impulse: f32,
372 pub swing_impulse: f32,
373 pub hertz: f32,
374 pub damping_ratio: f32,
375 pub max_motor_torque: f32,
376 pub motor_velocity: Vec3,
377 pub lower_twist_angle: f32,
378 pub upper_twist_angle: f32,
379 pub cone_angle: f32,
380 pub target_rotation: Quat,
381
382 pub index_a: i32,
383 pub index_b: i32,
384 pub frame_a: Transform,
385 pub frame_b: Transform,
386 pub delta_center: Vec3,
387 pub swing_axis: Vec3,
388 pub twist_jacobian: Vec3,
389
390 pub rotation_mass: Matrix3,
391 pub swing_mass: f32,
392 pub twist_mass: f32,
393 pub spring_softness: Softness,
394
395 pub enable_spring: bool,
396 pub enable_motor: bool,
397 pub enable_cone_limit: bool,
398 pub enable_twist_limit: bool,
399}
400
401impl Default for SphericalJoint {
402 fn default() -> Self {
403 SphericalJoint {
404 linear_impulse: VEC3_ZERO,
405 spring_impulse: VEC3_ZERO,
406 motor_impulse: VEC3_ZERO,
407 lower_twist_impulse: 0.0,
408 upper_twist_impulse: 0.0,
409 swing_impulse: 0.0,
410 hertz: 0.0,
411 damping_ratio: 0.0,
412 max_motor_torque: 0.0,
413 motor_velocity: VEC3_ZERO,
414 lower_twist_angle: 0.0,
415 upper_twist_angle: 0.0,
416 cone_angle: 0.0,
417 target_rotation: QUAT_IDENTITY,
418 index_a: NULL_INDEX,
419 index_b: NULL_INDEX,
420 frame_a: TRANSFORM_IDENTITY,
421 frame_b: TRANSFORM_IDENTITY,
422 delta_center: VEC3_ZERO,
423 swing_axis: VEC3_ZERO,
424 twist_jacobian: VEC3_ZERO,
425 rotation_mass: MAT3_ZERO,
426 swing_mass: 0.0,
427 twist_mass: 0.0,
428 spring_softness: Softness::default(),
429 enable_spring: false,
430 enable_motor: false,
431 enable_cone_limit: false,
432 enable_twist_limit: false,
433 }
434 }
435}
436
437#[derive(Debug, Clone, Copy, PartialEq)]
439pub struct WeldJoint {
440 pub linear_hertz: f32,
441 pub linear_damping_ratio: f32,
442 pub angular_hertz: f32,
443 pub angular_damping_ratio: f32,
444
445 pub linear_spring: Softness,
446 pub angular_spring: Softness,
447 pub linear_impulse: Vec3,
448 pub angular_impulse: Vec3,
449
450 pub index_a: i32,
451 pub index_b: i32,
452 pub frame_a: Transform,
453 pub frame_b: Transform,
454 pub delta_center: Vec3,
455
456 pub angular_mass: Matrix3,
457}
458
459impl Default for WeldJoint {
460 fn default() -> Self {
461 WeldJoint {
462 linear_hertz: 0.0,
463 linear_damping_ratio: 0.0,
464 angular_hertz: 0.0,
465 angular_damping_ratio: 0.0,
466 linear_spring: Softness::default(),
467 angular_spring: Softness::default(),
468 linear_impulse: VEC3_ZERO,
469 angular_impulse: VEC3_ZERO,
470 index_a: NULL_INDEX,
471 index_b: NULL_INDEX,
472 frame_a: TRANSFORM_IDENTITY,
473 frame_b: TRANSFORM_IDENTITY,
474 delta_center: VEC3_ZERO,
475 angular_mass: MAT3_ZERO,
476 }
477 }
478}
479
480#[derive(Debug, Clone, Copy, PartialEq)]
482pub struct WheelJoint {
483 pub linear_impulse: Vec2,
484 pub angular_impulse: Vec2,
485 pub spin_impulse: f32,
486 pub max_spin_torque: f32,
487 pub spin_speed: f32,
488 pub suspension_spring_impulse: f32,
489 pub lower_suspension_impulse: f32,
490 pub upper_suspension_impulse: f32,
491 pub lower_suspension_limit: f32,
492 pub upper_suspension_limit: f32,
493 pub suspension_hertz: f32,
494 pub suspension_damping_ratio: f32,
495 pub steering_spring_impulse: f32,
496 pub lower_steering_impulse: f32,
497 pub upper_steering_impulse: f32,
498 pub lower_steering_limit: f32,
499 pub upper_steering_limit: f32,
500 pub target_steering_angle: f32,
501 pub max_steering_torque: f32,
502 pub steering_hertz: f32,
503 pub steering_damping_ratio: f32,
504
505 pub index_a: i32,
506 pub index_b: i32,
507 pub frame_a: Transform,
508 pub frame_b: Transform,
509 pub delta_center: Vec3,
510 pub spin_mass: f32,
511 pub suspension_mass: f32,
512 pub steering_mass: f32,
513 pub suspension_softness: Softness,
514 pub steering_softness: Softness,
515
516 pub enable_spin_motor: bool,
517 pub enable_suspension_spring: bool,
518 pub enable_suspension_limit: bool,
519 pub enable_steering: bool,
520 pub enable_steering_limit: bool,
521 pub enable_steering_motor: bool,
522}
523
524impl Default for WheelJoint {
525 fn default() -> Self {
526 WheelJoint {
527 linear_impulse: VEC2_ZERO,
528 angular_impulse: VEC2_ZERO,
529 spin_impulse: 0.0,
530 max_spin_torque: 0.0,
531 spin_speed: 0.0,
532 suspension_spring_impulse: 0.0,
533 lower_suspension_impulse: 0.0,
534 upper_suspension_impulse: 0.0,
535 lower_suspension_limit: 0.0,
536 upper_suspension_limit: 0.0,
537 suspension_hertz: 0.0,
538 suspension_damping_ratio: 0.0,
539 steering_spring_impulse: 0.0,
540 lower_steering_impulse: 0.0,
541 upper_steering_impulse: 0.0,
542 lower_steering_limit: 0.0,
543 upper_steering_limit: 0.0,
544 target_steering_angle: 0.0,
545 max_steering_torque: 0.0,
546 steering_hertz: 0.0,
547 steering_damping_ratio: 0.0,
548 index_a: NULL_INDEX,
549 index_b: NULL_INDEX,
550 frame_a: TRANSFORM_IDENTITY,
551 frame_b: TRANSFORM_IDENTITY,
552 delta_center: VEC3_ZERO,
553 spin_mass: 0.0,
554 suspension_mass: 0.0,
555 steering_mass: 0.0,
556 suspension_softness: Softness::default(),
557 steering_softness: Softness::default(),
558 enable_spin_motor: false,
559 enable_suspension_spring: false,
560 enable_suspension_limit: false,
561 enable_steering: false,
562 enable_steering_limit: false,
563 enable_steering_motor: false,
564 }
565 }
566}
567
568#[derive(Debug, Clone, Copy, PartialEq)]
571pub enum JointUnion {
572 Distance(DistanceJoint),
573 Filter,
574 Motor(MotorJoint),
575 Parallel(ParallelJoint),
576 Revolute(RevoluteJoint),
577 Spherical(SphericalJoint),
578 Prismatic(PrismaticJoint),
579 Weld(WeldJoint),
580 Wheel(WheelJoint),
581}
582
583impl JointUnion {
584 pub fn empty(joint_type: JointType) -> Self {
586 match joint_type {
587 JointType::Distance => JointUnion::Distance(DistanceJoint::default()),
588 JointType::Filter => JointUnion::Filter,
589 JointType::Motor => JointUnion::Motor(MotorJoint::default()),
590 JointType::Parallel => JointUnion::Parallel(ParallelJoint::default()),
591 JointType::Prismatic => JointUnion::Prismatic(PrismaticJoint::default()),
592 JointType::Revolute => JointUnion::Revolute(RevoluteJoint::default()),
593 JointType::Spherical => JointUnion::Spherical(SphericalJoint::default()),
594 JointType::Weld => JointUnion::Weld(WeldJoint::default()),
595 JointType::Wheel => JointUnion::Wheel(WheelJoint::default()),
596 }
597 }
598}
599
600impl Default for JointUnion {
601 fn default() -> Self {
602 JointUnion::Distance(DistanceJoint::default())
603 }
604}
605
606impl JointSim {
607 pub fn distance(&self) -> &DistanceJoint {
609 match &self.union_ {
610 JointUnion::Distance(joint) => joint,
611 _ => unreachable!("joint union is not a distance joint"),
612 }
613 }
614
615 pub fn distance_mut(&mut self) -> &mut DistanceJoint {
616 match &mut self.union_ {
617 JointUnion::Distance(joint) => joint,
618 _ => unreachable!("joint union is not a distance joint"),
619 }
620 }
621
622 pub fn parallel(&self) -> &ParallelJoint {
624 match &self.union_ {
625 JointUnion::Parallel(joint) => joint,
626 _ => unreachable!("joint union is not a parallel joint"),
627 }
628 }
629
630 pub fn parallel_mut(&mut self) -> &mut ParallelJoint {
631 match &mut self.union_ {
632 JointUnion::Parallel(joint) => joint,
633 _ => unreachable!("joint union is not a parallel joint"),
634 }
635 }
636
637 pub fn weld(&self) -> &WeldJoint {
639 match &self.union_ {
640 JointUnion::Weld(joint) => joint,
641 _ => unreachable!("joint union is not a weld joint"),
642 }
643 }
644
645 pub fn weld_mut(&mut self) -> &mut WeldJoint {
646 match &mut self.union_ {
647 JointUnion::Weld(joint) => joint,
648 _ => unreachable!("joint union is not a weld joint"),
649 }
650 }
651
652 pub fn revolute(&self) -> &RevoluteJoint {
654 match &self.union_ {
655 JointUnion::Revolute(joint) => joint,
656 _ => unreachable!("joint union is not a revolute joint"),
657 }
658 }
659
660 pub fn revolute_mut(&mut self) -> &mut RevoluteJoint {
661 match &mut self.union_ {
662 JointUnion::Revolute(joint) => joint,
663 _ => unreachable!("joint union is not a revolute joint"),
664 }
665 }
666
667 pub fn prismatic(&self) -> &PrismaticJoint {
669 match &self.union_ {
670 JointUnion::Prismatic(joint) => joint,
671 _ => unreachable!("joint union is not a prismatic joint"),
672 }
673 }
674
675 pub fn prismatic_mut(&mut self) -> &mut PrismaticJoint {
676 match &mut self.union_ {
677 JointUnion::Prismatic(joint) => joint,
678 _ => unreachable!("joint union is not a prismatic joint"),
679 }
680 }
681
682 pub fn motor(&self) -> &MotorJoint {
684 match &self.union_ {
685 JointUnion::Motor(joint) => joint,
686 _ => unreachable!("joint union is not a motor joint"),
687 }
688 }
689
690 pub fn motor_mut(&mut self) -> &mut MotorJoint {
691 match &mut self.union_ {
692 JointUnion::Motor(joint) => joint,
693 _ => unreachable!("joint union is not a motor joint"),
694 }
695 }
696
697 pub fn spherical(&self) -> &SphericalJoint {
699 match &self.union_ {
700 JointUnion::Spherical(joint) => joint,
701 _ => unreachable!("joint union is not a spherical joint"),
702 }
703 }
704
705 pub fn spherical_mut(&mut self) -> &mut SphericalJoint {
706 match &mut self.union_ {
707 JointUnion::Spherical(joint) => joint,
708 _ => unreachable!("joint union is not a spherical joint"),
709 }
710 }
711
712 pub fn wheel(&self) -> &WheelJoint {
714 match &self.union_ {
715 JointUnion::Wheel(joint) => joint,
716 _ => unreachable!("joint union is not a wheel joint"),
717 }
718 }
719
720 pub fn wheel_mut(&mut self) -> &mut WheelJoint {
721 match &mut self.union_ {
722 JointUnion::Wheel(joint) => joint,
723 _ => unreachable!("joint union is not a wheel joint"),
724 }
725 }
726}
727
728#[derive(Debug, Clone, Copy, PartialEq)]
730pub struct JointSim {
731 pub joint_id: i32,
732
733 pub body_id_a: i32,
734 pub body_id_b: i32,
735
736 pub type_: JointType,
737
738 pub local_frame_a: Transform,
740 pub local_frame_b: Transform,
741
742 pub inv_mass_a: f32,
743 pub inv_mass_b: f32,
744 pub inv_i_a: Matrix3,
745 pub inv_i_b: Matrix3,
746
747 pub constraint_hertz: f32,
748 pub constraint_damping_ratio: f32,
749
750 pub constraint_softness: Softness,
751
752 pub force_threshold: f32,
753 pub torque_threshold: f32,
754
755 pub fixed_rotation: bool,
756
757 pub union_: JointUnion,
758}
759
760impl Default for JointSim {
761 fn default() -> Self {
762 JointSim {
763 joint_id: NULL_INDEX,
764 body_id_a: NULL_INDEX,
765 body_id_b: NULL_INDEX,
766 type_: JointType::Distance,
767 local_frame_a: TRANSFORM_IDENTITY,
768 local_frame_b: TRANSFORM_IDENTITY,
769 inv_mass_a: 0.0,
770 inv_mass_b: 0.0,
771 inv_i_a: MAT3_ZERO,
772 inv_i_b: MAT3_ZERO,
773 constraint_hertz: 0.0,
774 constraint_damping_ratio: 0.0,
775 constraint_softness: Softness::default(),
776 force_threshold: 0.0,
777 torque_threshold: 0.0,
778 fixed_rotation: false,
779 union_: JointUnion::default(),
780 }
781 }
782}