box2d_rust/joint/
mod.rs

1// Port of the joint data model from box2d-cpp-reference/src/joint.h.
2// Logic from joint.c and the per-joint .c files lands in the joints bring-up
3// commits.
4//
5// SPDX-FileCopyrightText: 2023 Erin Catto
6// SPDX-License-Identifier: MIT
7
8use crate::core::NULL_INDEX;
9use crate::math_functions::{Mat22, Transform, Vec2, MAT22_ZERO, TRANSFORM_IDENTITY, VEC2_ZERO};
10use crate::solver::Softness;
11
12/// Joint type enumeration. (types.h: b2JointType)
13#[derive(Debug, Clone, Copy, PartialEq, Eq, Default)]
14pub enum JointType {
15    #[default]
16    Distance,
17    Filter,
18    Motor,
19    Prismatic,
20    Revolute,
21    Weld,
22    Wheel,
23}
24
25/// A joint edge connects bodies and joints together in a joint graph where
26/// each body is a node and each joint is an edge. (b2JointEdge)
27#[derive(Debug, Clone, Copy, PartialEq, Eq)]
28pub struct JointEdge {
29    pub body_id: i32,
30    pub prev_key: i32,
31    pub next_key: i32,
32}
33
34impl Default for JointEdge {
35    fn default() -> Self {
36        JointEdge {
37            body_id: NULL_INDEX,
38            prev_key: NULL_INDEX,
39            next_key: NULL_INDEX,
40        }
41    }
42}
43
44/// Map from JointId to joint data in the solver sets. (b2Joint)
45#[derive(Debug, Clone)]
46pub struct Joint {
47    pub user_data: u64,
48
49    /// index of simulation set stored in World. NULL_INDEX when slot is free.
50    pub set_index: i32,
51
52    /// index into the constraint graph color array, may be NULL_INDEX for
53    /// sleeping/disabled joints. NULL_INDEX when slot is free.
54    pub color_index: i32,
55
56    /// joint index within set or graph color. NULL_INDEX when slot is free.
57    pub local_index: i32,
58
59    pub edges: [JointEdge; 2],
60
61    pub joint_id: i32,
62    pub island_id: i32,
63
64    /// Index into the island's joints array for O(1) swap-removal.
65    /// NULL_INDEX when not in an island.
66    pub island_index: i32,
67
68    pub draw_scale: f32,
69
70    pub type_: JointType,
71
72    /// Monotonically advanced when a joint is allocated in this slot.
73    pub generation: u16,
74
75    pub collide_connected: bool,
76}
77
78impl Default for Joint {
79    fn default() -> Self {
80        Joint {
81            user_data: 0,
82            set_index: NULL_INDEX,
83            color_index: NULL_INDEX,
84            local_index: NULL_INDEX,
85            edges: [JointEdge::default(); 2],
86            joint_id: NULL_INDEX,
87            island_id: NULL_INDEX,
88            island_index: NULL_INDEX,
89            draw_scale: 1.0,
90            type_: JointType::Distance,
91            generation: 0,
92            collide_connected: false,
93        }
94    }
95}
96
97/// (b2DistanceJoint)
98#[derive(Debug, Clone, Copy, PartialEq, Default)]
99pub struct DistanceJoint {
100    pub length: f32,
101    pub hertz: f32,
102    pub damping_ratio: f32,
103    pub lower_spring_force: f32,
104    pub upper_spring_force: f32,
105    pub min_length: f32,
106    pub max_length: f32,
107
108    pub max_motor_force: f32,
109    pub motor_speed: f32,
110
111    pub impulse: f32,
112    pub lower_impulse: f32,
113    pub upper_impulse: f32,
114    pub motor_impulse: f32,
115
116    pub index_a: i32,
117    pub index_b: i32,
118    pub anchor_a: Vec2,
119    pub anchor_b: Vec2,
120    pub delta_center: Vec2,
121    pub distance_softness: Softness,
122    pub axial_mass: f32,
123
124    pub enable_spring: bool,
125    pub enable_limit: bool,
126    pub enable_motor: bool,
127}
128
129/// (b2MotorJoint)
130#[derive(Debug, Clone, Copy, PartialEq)]
131pub struct MotorJoint {
132    pub linear_velocity: Vec2,
133    pub max_velocity_force: f32,
134    pub angular_velocity: f32,
135    pub max_velocity_torque: f32,
136    pub linear_hertz: f32,
137    pub linear_damping_ratio: f32,
138    pub max_spring_force: f32,
139    pub angular_hertz: f32,
140    pub angular_damping_ratio: f32,
141    pub max_spring_torque: f32,
142
143    pub linear_velocity_impulse: Vec2,
144    pub angular_velocity_impulse: f32,
145    pub linear_spring_impulse: Vec2,
146    pub angular_spring_impulse: f32,
147
148    pub linear_spring: Softness,
149    pub angular_spring: Softness,
150
151    pub index_a: i32,
152    pub index_b: i32,
153    pub frame_a: Transform,
154    pub frame_b: Transform,
155    pub delta_center: Vec2,
156    pub linear_mass: Mat22,
157    pub angular_mass: f32,
158}
159
160impl Default for MotorJoint {
161    fn default() -> Self {
162        MotorJoint {
163            linear_velocity: VEC2_ZERO,
164            max_velocity_force: 0.0,
165            angular_velocity: 0.0,
166            max_velocity_torque: 0.0,
167            linear_hertz: 0.0,
168            linear_damping_ratio: 0.0,
169            max_spring_force: 0.0,
170            angular_hertz: 0.0,
171            angular_damping_ratio: 0.0,
172            max_spring_torque: 0.0,
173            linear_velocity_impulse: VEC2_ZERO,
174            angular_velocity_impulse: 0.0,
175            linear_spring_impulse: VEC2_ZERO,
176            angular_spring_impulse: 0.0,
177            linear_spring: Softness::default(),
178            angular_spring: Softness::default(),
179            index_a: NULL_INDEX,
180            index_b: NULL_INDEX,
181            frame_a: TRANSFORM_IDENTITY,
182            frame_b: TRANSFORM_IDENTITY,
183            delta_center: VEC2_ZERO,
184            linear_mass: MAT22_ZERO,
185            angular_mass: 0.0,
186        }
187    }
188}
189
190/// (b2PrismaticJoint)
191#[derive(Debug, Clone, Copy, PartialEq)]
192pub struct PrismaticJoint {
193    pub impulse: Vec2,
194    pub spring_impulse: f32,
195    pub motor_impulse: f32,
196    pub lower_impulse: f32,
197    pub upper_impulse: f32,
198    pub hertz: f32,
199    pub damping_ratio: f32,
200    pub target_translation: f32,
201    pub max_motor_force: f32,
202    pub motor_speed: f32,
203    pub lower_translation: f32,
204    pub upper_translation: f32,
205
206    pub index_a: i32,
207    pub index_b: i32,
208    pub frame_a: Transform,
209    pub frame_b: Transform,
210    pub delta_center: Vec2,
211    pub spring_softness: Softness,
212
213    pub enable_spring: bool,
214    pub enable_limit: bool,
215    pub enable_motor: bool,
216}
217
218impl Default for PrismaticJoint {
219    fn default() -> Self {
220        PrismaticJoint {
221            impulse: VEC2_ZERO,
222            spring_impulse: 0.0,
223            motor_impulse: 0.0,
224            lower_impulse: 0.0,
225            upper_impulse: 0.0,
226            hertz: 0.0,
227            damping_ratio: 0.0,
228            target_translation: 0.0,
229            max_motor_force: 0.0,
230            motor_speed: 0.0,
231            lower_translation: 0.0,
232            upper_translation: 0.0,
233            index_a: NULL_INDEX,
234            index_b: NULL_INDEX,
235            frame_a: TRANSFORM_IDENTITY,
236            frame_b: TRANSFORM_IDENTITY,
237            delta_center: VEC2_ZERO,
238            spring_softness: Softness::default(),
239            enable_spring: false,
240            enable_limit: false,
241            enable_motor: false,
242        }
243    }
244}
245
246/// (b2RevoluteJoint)
247#[derive(Debug, Clone, Copy, PartialEq)]
248pub struct RevoluteJoint {
249    pub linear_impulse: Vec2,
250    pub spring_impulse: f32,
251    pub motor_impulse: f32,
252    pub lower_impulse: f32,
253    pub upper_impulse: f32,
254    pub hertz: f32,
255    pub damping_ratio: f32,
256    pub target_angle: f32,
257    pub max_motor_torque: f32,
258    pub motor_speed: f32,
259    pub lower_angle: f32,
260    pub upper_angle: f32,
261
262    pub index_a: i32,
263    pub index_b: i32,
264    pub frame_a: Transform,
265    pub frame_b: Transform,
266    pub delta_center: Vec2,
267    pub axial_mass: f32,
268    pub spring_softness: Softness,
269
270    pub enable_spring: bool,
271    pub enable_motor: bool,
272    pub enable_limit: bool,
273}
274
275impl Default for RevoluteJoint {
276    fn default() -> Self {
277        RevoluteJoint {
278            linear_impulse: VEC2_ZERO,
279            spring_impulse: 0.0,
280            motor_impulse: 0.0,
281            lower_impulse: 0.0,
282            upper_impulse: 0.0,
283            hertz: 0.0,
284            damping_ratio: 0.0,
285            target_angle: 0.0,
286            max_motor_torque: 0.0,
287            motor_speed: 0.0,
288            lower_angle: 0.0,
289            upper_angle: 0.0,
290            index_a: NULL_INDEX,
291            index_b: NULL_INDEX,
292            frame_a: TRANSFORM_IDENTITY,
293            frame_b: TRANSFORM_IDENTITY,
294            delta_center: VEC2_ZERO,
295            axial_mass: 0.0,
296            spring_softness: Softness::default(),
297            enable_spring: false,
298            enable_motor: false,
299            enable_limit: false,
300        }
301    }
302}
303
304/// (b2WeldJoint)
305#[derive(Debug, Clone, Copy, PartialEq)]
306pub struct WeldJoint {
307    pub linear_hertz: f32,
308    pub linear_damping_ratio: f32,
309    pub angular_hertz: f32,
310    pub angular_damping_ratio: f32,
311
312    pub linear_spring: Softness,
313    pub angular_spring: Softness,
314    pub linear_impulse: Vec2,
315    pub angular_impulse: f32,
316
317    pub index_a: i32,
318    pub index_b: i32,
319    pub frame_a: Transform,
320    pub frame_b: Transform,
321    pub delta_center: Vec2,
322    pub axial_mass: f32,
323}
324
325impl Default for WeldJoint {
326    fn default() -> Self {
327        WeldJoint {
328            linear_hertz: 0.0,
329            linear_damping_ratio: 0.0,
330            angular_hertz: 0.0,
331            angular_damping_ratio: 0.0,
332            linear_spring: Softness::default(),
333            angular_spring: Softness::default(),
334            linear_impulse: VEC2_ZERO,
335            angular_impulse: 0.0,
336            index_a: NULL_INDEX,
337            index_b: NULL_INDEX,
338            frame_a: TRANSFORM_IDENTITY,
339            frame_b: TRANSFORM_IDENTITY,
340            delta_center: VEC2_ZERO,
341            axial_mass: 0.0,
342        }
343    }
344}
345
346/// (b2WheelJoint)
347#[derive(Debug, Clone, Copy, PartialEq)]
348pub struct WheelJoint {
349    pub perp_impulse: f32,
350    pub motor_impulse: f32,
351    pub spring_impulse: f32,
352    pub lower_impulse: f32,
353    pub upper_impulse: f32,
354    pub max_motor_torque: f32,
355    pub motor_speed: f32,
356    pub lower_translation: f32,
357    pub upper_translation: f32,
358    pub hertz: f32,
359    pub damping_ratio: f32,
360
361    pub index_a: i32,
362    pub index_b: i32,
363    pub frame_a: Transform,
364    pub frame_b: Transform,
365    pub delta_center: Vec2,
366    pub perp_mass: f32,
367    pub motor_mass: f32,
368    pub axial_mass: f32,
369    pub spring_softness: Softness,
370
371    pub enable_spring: bool,
372    pub enable_motor: bool,
373    pub enable_limit: bool,
374}
375
376impl Default for WheelJoint {
377    fn default() -> Self {
378        WheelJoint {
379            perp_impulse: 0.0,
380            motor_impulse: 0.0,
381            spring_impulse: 0.0,
382            lower_impulse: 0.0,
383            upper_impulse: 0.0,
384            max_motor_torque: 0.0,
385            motor_speed: 0.0,
386            lower_translation: 0.0,
387            upper_translation: 0.0,
388            hertz: 0.0,
389            damping_ratio: 0.0,
390            index_a: NULL_INDEX,
391            index_b: NULL_INDEX,
392            frame_a: TRANSFORM_IDENTITY,
393            frame_b: TRANSFORM_IDENTITY,
394            delta_center: VEC2_ZERO,
395            perp_mass: 0.0,
396            motor_mass: 0.0,
397            axial_mass: 0.0,
398            spring_softness: Softness::default(),
399            enable_spring: false,
400            enable_motor: false,
401            enable_limit: false,
402        }
403    }
404}
405
406/// The per-type joint payload. The C `b2JointSim` stores a `b2JointType type`
407/// tag plus a union; the Rust port stores this tagged enum. A filter joint has
408/// no simulation data.
409#[derive(Debug, Clone, Copy, PartialEq)]
410pub enum JointPayload {
411    Distance(DistanceJoint),
412    Filter,
413    Motor(MotorJoint),
414    Prismatic(PrismaticJoint),
415    Revolute(RevoluteJoint),
416    Weld(WeldJoint),
417    Wheel(WheelJoint),
418}
419
420impl JointPayload {
421    /// The joint type tag for this payload.
422    pub fn joint_type(&self) -> JointType {
423        match self {
424            JointPayload::Distance(_) => JointType::Distance,
425            JointPayload::Filter => JointType::Filter,
426            JointPayload::Motor(_) => JointType::Motor,
427            JointPayload::Prismatic(_) => JointType::Prismatic,
428            JointPayload::Revolute(_) => JointType::Revolute,
429            JointPayload::Weld(_) => JointType::Weld,
430            JointPayload::Wheel(_) => JointType::Wheel,
431        }
432    }
433}
434
435/// The base joint simulation data. (b2JointSim)
436#[derive(Debug, Clone, Copy, PartialEq)]
437pub struct JointSim {
438    pub joint_id: i32,
439
440    pub body_id_a: i32,
441    pub body_id_b: i32,
442
443    pub local_frame_a: Transform,
444    pub local_frame_b: Transform,
445
446    pub inv_mass_a: f32,
447    pub inv_mass_b: f32,
448    pub inv_i_a: f32,
449    pub inv_i_b: f32,
450
451    pub constraint_hertz: f32,
452    pub constraint_damping_ratio: f32,
453
454    pub constraint_softness: Softness,
455
456    pub force_threshold: f32,
457    pub torque_threshold: f32,
458
459    /// The per-type data (C: type tag + union).
460    pub payload: JointPayload,
461}
462
463impl JointSim {
464    /// The joint type tag. (C: joint->type)
465    pub fn joint_type(&self) -> JointType {
466        self.payload.joint_type()
467    }
468
469    /// (C: &base->distanceJoint — union access checked by the payload tag)
470    pub fn distance(&self) -> &DistanceJoint {
471        match &self.payload {
472            JointPayload::Distance(joint) => joint,
473            _ => unreachable!("joint payload is not a distance joint"),
474        }
475    }
476
477    pub fn distance_mut(&mut self) -> &mut DistanceJoint {
478        match &mut self.payload {
479            JointPayload::Distance(joint) => joint,
480            _ => unreachable!("joint payload is not a distance joint"),
481        }
482    }
483
484    /// (C: &base->motorJoint)
485    pub fn motor(&self) -> &MotorJoint {
486        match &self.payload {
487            JointPayload::Motor(joint) => joint,
488            _ => unreachable!("joint payload is not a motor joint"),
489        }
490    }
491
492    pub fn motor_mut(&mut self) -> &mut MotorJoint {
493        match &mut self.payload {
494            JointPayload::Motor(joint) => joint,
495            _ => unreachable!("joint payload is not a motor joint"),
496        }
497    }
498
499    /// (C: &base->prismaticJoint)
500    pub fn prismatic(&self) -> &PrismaticJoint {
501        match &self.payload {
502            JointPayload::Prismatic(joint) => joint,
503            _ => unreachable!("joint payload is not a prismatic joint"),
504        }
505    }
506
507    pub fn prismatic_mut(&mut self) -> &mut PrismaticJoint {
508        match &mut self.payload {
509            JointPayload::Prismatic(joint) => joint,
510            _ => unreachable!("joint payload is not a prismatic joint"),
511        }
512    }
513
514    /// (C: &base->revoluteJoint)
515    pub fn revolute(&self) -> &RevoluteJoint {
516        match &self.payload {
517            JointPayload::Revolute(joint) => joint,
518            _ => unreachable!("joint payload is not a revolute joint"),
519        }
520    }
521
522    pub fn revolute_mut(&mut self) -> &mut RevoluteJoint {
523        match &mut self.payload {
524            JointPayload::Revolute(joint) => joint,
525            _ => unreachable!("joint payload is not a revolute joint"),
526        }
527    }
528
529    /// (C: &base->weldJoint)
530    pub fn weld(&self) -> &WeldJoint {
531        match &self.payload {
532            JointPayload::Weld(joint) => joint,
533            _ => unreachable!("joint payload is not a weld joint"),
534        }
535    }
536
537    pub fn weld_mut(&mut self) -> &mut WeldJoint {
538        match &mut self.payload {
539            JointPayload::Weld(joint) => joint,
540            _ => unreachable!("joint payload is not a weld joint"),
541        }
542    }
543
544    /// (C: &base->wheelJoint)
545    pub fn wheel(&self) -> &WheelJoint {
546        match &self.payload {
547            JointPayload::Wheel(joint) => joint,
548            _ => unreachable!("joint payload is not a wheel joint"),
549        }
550    }
551
552    pub fn wheel_mut(&mut self) -> &mut WheelJoint {
553        match &mut self.payload {
554            JointPayload::Wheel(joint) => joint,
555            _ => unreachable!("joint payload is not a wheel joint"),
556        }
557    }
558}
559
560mod api;
561mod draw;
562mod lifecycle;
563mod plumbing;
564mod solve;
565
566pub use api::*;
567pub use draw::*;
568pub use lifecycle::*;
569pub use plumbing::*;
570pub use solve::*;
571
572#[cfg(test)]
573mod tests {
574    use super::*;
575    use crate::body::{create_body, destroy_body, get_body_full_id};
576    use crate::broad_phase::update_broad_phase_pairs;
577    use crate::constraint_graph::OVERFLOW_INDEX;
578    use crate::core::NULL_INDEX;
579    use crate::geometry::make_box;
580    use crate::shape::create_polygon_shape;
581    use crate::solver_set::AWAKE_SET;
582    use crate::types::{
583        default_body_def, default_distance_joint_def, default_revolute_joint_def,
584        default_shape_def, default_world_def, BodyType,
585    };
586    use crate::world::World;
587
588    // Joint slice test: creation places joints in the constraint graph, links
589    // islands, and filters contacts; collide_connected toggling and
590    // destruction restore the previous state.
591    #[test]
592    fn create_and_destroy_joints() {
593        let mut world = World::new(&default_world_def());
594
595        let mut body_def = default_body_def();
596        body_def.type_ = BodyType::Dynamic;
597        let body_a = create_body(&mut world, &body_def);
598        let body_b = create_body(&mut world, &body_def);
599        let a_index = get_body_full_id(&world, body_a);
600        let b_index = get_body_full_id(&world, body_b);
601
602        let box_poly = make_box(0.5, 0.5);
603        let shape_def = default_shape_def();
604        let _sa = create_polygon_shape(&mut world, body_a, &shape_def, &box_poly);
605        let _sb = create_polygon_shape(&mut world, body_b, &shape_def, &box_poly);
606
607        update_broad_phase_pairs(&mut world);
608        assert_eq!(world.contact_id_pool.id_count(), 1);
609
610        // The two dynamic bodies start in separate islands.
611        assert_ne!(
612            world.bodies[a_index as usize].island_id,
613            world.bodies[b_index as usize].island_id
614        );
615
616        // A revolute joint with collideConnected=false destroys the contact
617        // and merges the islands.
618        let mut revolute_def = default_revolute_joint_def();
619        revolute_def.base.body_id_a = body_a;
620        revolute_def.base.body_id_b = body_b;
621        let revolute_id = create_revolute_joint(&mut world, &revolute_def);
622
623        assert_eq!(world.contact_id_pool.id_count(), 0);
624        assert_eq!(world.joint_id_pool.id_count(), 1);
625        assert_eq!(world.bodies[a_index as usize].joint_count, 1);
626        assert_eq!(world.bodies[b_index as usize].joint_count, 1);
627        assert_eq!(
628            world.bodies[a_index as usize].island_id,
629            world.bodies[b_index as usize].island_id
630        );
631
632        let raw_revolute = get_joint_full_id(&world, revolute_id);
633        {
634            let joint = &world.joints[raw_revolute as usize];
635            assert_eq!(joint.set_index, AWAKE_SET);
636            assert!(joint.color_index != NULL_INDEX);
637            assert!(joint.island_id != NULL_INDEX);
638            assert_eq!(joint.type_, JointType::Revolute);
639        }
640        assert_eq!(joint_get_type(&world, revolute_id), JointType::Revolute);
641        assert!(!joint_get_collide_connected(&world, revolute_id));
642
643        // Broad-phase pair update does not recreate the filtered contact.
644        update_broad_phase_pairs(&mut world);
645        assert_eq!(world.contact_id_pool.id_count(), 0);
646
647        // A distance joint between a static body and a dynamic body goes in
648        // the awake graph with a static-priority color.
649        let ground = create_body(&mut world, &default_body_def());
650        let mut distance_def = default_distance_joint_def();
651        distance_def.base.body_id_a = ground;
652        distance_def.base.body_id_b = body_a;
653        distance_def.length = 2.0;
654        let distance_id = create_distance_joint(&mut world, &distance_def);
655
656        assert_eq!(world.joint_id_pool.id_count(), 2);
657        let raw_distance = get_joint_full_id(&world, distance_id);
658        {
659            let joint = &world.joints[raw_distance as usize];
660            assert_eq!(joint.set_index, AWAKE_SET);
661            assert!(joint.color_index != NULL_INDEX && joint.color_index <= OVERFLOW_INDEX);
662        }
663        assert_eq!(
664            crate::distance_joint::distance_joint_get_length(&world, distance_id),
665            2.0
666        );
667        assert_eq!(joint_get_body_a(&world, distance_id), ground);
668        assert_eq!(joint_get_body_b(&world, distance_id), body_a);
669
670        // Enabling collision on the revolute joint re-buffers the shapes so
671        // the broad phase can recreate the contact.
672        joint_set_collide_connected(&mut world, revolute_id, true);
673        assert!(joint_get_collide_connected(&world, revolute_id));
674        update_broad_phase_pairs(&mut world);
675        assert_eq!(world.contact_id_pool.id_count(), 1);
676
677        // Destroying the revolute joint unlinks bodies but keeps the contact.
678        destroy_joint(&mut world, revolute_id, true);
679        assert_eq!(world.joint_id_pool.id_count(), 1);
680        assert_eq!(world.bodies[a_index as usize].joint_count, 1); // distance joint remains
681        assert_eq!(world.bodies[b_index as usize].joint_count, 0);
682        assert_eq!(world.contact_id_pool.id_count(), 1);
683
684        // Destroying body A destroys the distance joint too.
685        destroy_body(&mut world, body_a);
686        assert_eq!(world.joint_id_pool.id_count(), 0);
687        assert_eq!(world.contact_id_pool.id_count(), 0);
688
689        world.validate_solver_sets();
690    }
691}
box2d_rust/joint/mod.rs

box2d_rust/joint/
mod.rs
