gizmo_physics_rigid/joints/
data.rs

1use gizmo_physics_core::BodyHandle;
2use gizmo_math::{Quat, Vec3};
3use serde::{Deserialize, Serialize};
4
5#[derive(Debug, Clone, Serialize, Deserialize)]
6#[non_exhaustive]
7pub struct Joint {
8    pub entity_a: BodyHandle,
9    pub entity_b: BodyHandle,
10    pub local_anchor_a: Vec3,
11    pub local_anchor_b: Vec3,
12    pub break_force: f32,
13    pub break_torque: f32,
14    #[serde(skip)]
15    pub is_broken: bool,
16    pub collision_enabled: bool,
17    pub data: JointData,
18}
19
20#[derive(Debug, Clone, Serialize, Deserialize)]
21#[non_exhaustive]
22pub enum JointData {
23    Fixed,
24    Hinge(HingeJointData),
25    BallSocket(BallSocketJointData),
26    Slider(SliderJointData),
27    Spring(SpringJointData),
28    Distance(DistanceJointData),
29    D6(D6JointData),
30}
31
32#[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize)]
33#[non_exhaustive]
34pub enum JointType {
35    Fixed,
36    Hinge,
37    BallSocket,
38    Slider,
39    Spring,
40    Distance,
41    D6,
42}
43
44/// Compile-forced mapping so `JointType` (the authoring descriptor) and `JointData`
45/// (the runtime payload) can never silently drift: adding a `JointData` variant without
46/// a matching `JointType` is a compile error here. Used by the solver dispatch.
47impl From<&JointData> for JointType {
48    fn from(data: &JointData) -> Self {
49        match data {
50            JointData::Fixed => JointType::Fixed,
51            JointData::Hinge(_) => JointType::Hinge,
52            JointData::BallSocket(_) => JointType::BallSocket,
53            JointData::Slider(_) => JointType::Slider,
54            JointData::Spring(_) => JointType::Spring,
55            JointData::Distance(_) => JointType::Distance,
56            JointData::D6(_) => JointType::D6,
57        }
58    }
59}
60
61#[derive(Debug, Clone, Copy, Serialize, Deserialize, Default)]
62#[non_exhaustive]
63pub struct HingeJointData {
64    pub axis: Vec3,
65    pub use_limits: bool,
66    pub lower_limit: f32,
67    pub upper_limit: f32,
68    pub use_motor: bool,
69    pub motor_target_velocity: f32,
70    pub motor_max_force: f32,
71    /// When true (and `use_motor`), the motor is a POSITION SERVO: it drives toward
72    /// `motor_target_position` (target angle, rad) instead of holding a target velocity,
73    /// force-limited by `motor_max_force`. When false it is the classic velocity motor.
74    pub motor_is_servo: bool,
75    pub motor_target_position: f32,
76    /// Torsional spring / return-to-center: a soft restoring torque toward `rest_angle`
77    /// (stiffness + damping) about the hinge axis — self-closing doors, spring flaps, soft
78    /// ragdoll joint stiffness. The angular analogue of the Slider suspension spring;
79    /// force-based (applied once per step).
80    pub use_torsional_spring: bool,
81    pub torsional_stiffness: f32,
82    pub torsional_damping: f32,
83    pub rest_angle: f32,
84    #[serde(skip)]
85    pub current_angle: f32,
86}
87
88#[derive(Debug, Clone, Copy, Serialize, Deserialize, Default)]
89#[non_exhaustive]
90pub struct BallSocketJointData {
91    pub use_cone_limit: bool,
92    pub cone_limit_angle: f32,
93    /// Twist (roll about `twist_axis`) limit — the second half of a cone-twist joint.
94    /// The cone limits SWING (how far the axis tips); this limits TWIST (spin about it),
95    /// so a ragdoll limb no longer spins freely about its own bone. `twist_axis` is in
96    /// A's local frame. Two-sided: `[twist_lower, twist_upper]` (radians).
97    pub use_twist_limit: bool,
98    pub twist_axis: Vec3,
99    pub twist_lower: f32,
100    pub twist_upper: f32,
101    /// Asymmetric (per-axis) swing limits: clamp the swing about the two axes perpendicular
102    /// to `twist_axis` independently, so a shoulder/hip can have a different range in each
103    /// direction — unlike the single circular `cone_limit_angle`. Radians about each perp.
104    pub use_swing_limits: bool,
105    pub swing_limit_1: f32,
106    pub swing_limit_2: f32,
107    /// Inverse stiffness (CFM) applied to the cone/twist/swing LIMITS: 0 = hard stop;
108    /// larger = a soft, springy limit that gives under load (natural ragdoll joint feel).
109    pub compliance: f32,
110    #[serde(default)]
111    pub initial_relative_rotation: Option<Quat>,
112}
113
114#[derive(Debug, Clone, Copy, Serialize, Deserialize, Default)]
115#[non_exhaustive]
116pub struct SliderJointData {
117    pub axis: Vec3,
118    pub use_limits: bool,
119    pub lower_limit: f32,
120    pub upper_limit: f32,
121    pub use_motor: bool,
122    pub motor_target_velocity: f32,
123    pub motor_max_force: f32,
124    /// When true (and `use_motor`), the motor is a POSITION SERVO driving toward
125    /// `motor_target_position` (target offset along the axis) instead of a target velocity.
126    pub motor_is_servo: bool,
127    pub motor_target_position: f32,
128    /// Suspension spring along the free axis: a soft PD force toward `spring_rest_position`
129    /// (stiffness + damping). This is the canonical shock/suspension/elevator-buffer
130    /// primitive — a springy prismatic, applied once per step (force-based, like Spring).
131    pub use_spring: bool,
132    pub spring_stiffness: f32,
133    pub spring_damping: f32,
134    pub spring_rest_position: f32,
135    #[serde(skip)]
136    pub current_position: f32,
137    #[serde(default)]
138    pub initial_relative_rotation: Option<Quat>,
139}
140
141#[derive(Debug, Clone, Copy, Serialize, Deserialize, Default)]
142#[non_exhaustive]
143pub struct SpringJointData {
144    pub rest_length: f32,
145    pub stiffness: f32,
146    pub damping: f32,
147    pub min_length: f32,
148    pub max_length: Option<f32>,
149}
150
151/// Distance/rope joint: keeps the anchor separation within `[min_length, max_length]`
152/// as a HARD (inequality) constraint — unlike `Spring`, which is a soft force toward a
153/// rest length. A **rope** is `{min: 0, max: L}`: it only pulls when taut (`len > L`)
154/// and is limp when slack (`len < L`), so a released slack body free-falls until the
155/// rope catches it — no rigid-rod snap. A **rigid rod** is `{min: L, max: L}`.
156#[derive(Debug, Clone, Copy, Serialize, Deserialize, Default)]
157#[non_exhaustive]
158pub struct DistanceJointData {
159    pub min_length: f32,
160    pub max_length: f32,
161    /// Inverse stiffness (CFM): 0 = rigid rope/rod (hard bounds); larger = a stretchy,
162    /// elastic rope that gives under load. See the soft constraint primitives.
163    pub compliance: f32,
164}
165
166/// Per-degree-of-freedom mode for the generic 6-DOF ([`D6JointData`]) joint.
167#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize, Default)]
168#[non_exhaustive]
169pub enum D6Motion {
170    /// Fully constrained (0 relative motion on this axis) — the Fixed-joint behaviour.
171    #[default]
172    Locked,
173    /// Unconstrained — free to translate/rotate on this axis (Slider/Hinge behaviour).
174    Free,
175    /// Constrained to `[lower, upper]` on this axis (a limited slider/hinge).
176    Limited { lower: f32, upper: f32 },
177}
178
179/// Per-axis DRIVE for a [`D6JointData`]: a spring-damper toward a target that unifies a
180/// motor (`damping` pulls the velocity toward `target_velocity`) and a spring (`stiffness`
181/// pulls the position toward `target_position`), force-limited by `max_force` (≤0 ⇒
182/// unlimited). PhysX-D6-style. `enabled: false` (the default) ⇒ no drive on that axis.
183// Exhaustive (a plain config value users build with a struct literal), like PhysicsMaterial.
184#[derive(Debug, Clone, Copy, Serialize, Deserialize, Default)]
185pub struct D6Drive {
186    pub enabled: bool,
187    pub stiffness: f32,
188    pub damping: f32,
189    pub target_position: f32,
190    pub target_velocity: f32,
191    pub max_force: f32,
192}
193
194/// Generic 6-DOF (D6) joint: per-axis Lock / Free / Limited over 3 translational + 3
195/// rotational DOFs, in a configurable local frame. Subsumes Fixed (all locked), Slider
196/// (one linear Free/Limited), Hinge (one angular Free/Limited) and hybrids (universal,
197/// cylindrical, planar) — the modern default joint (PhysX D6 / Rapier GenericJoint).
198/// Pure orchestration of the existing 1-DOF constraint primitives.
199#[derive(Debug, Clone, Copy, Serialize, Deserialize, Default)]
200#[non_exhaustive]
201pub struct D6JointData {
202    /// Local frame (in A's space) whose X/Y/Z axes define the six DOFs. Identity = A's axes.
203    pub frame: Quat,
204    /// Translation modes along the frame's X, Y, Z axes.
205    pub linear: [D6Motion; 3],
206    /// Rotation modes about the frame's X, Y, Z axes.
207    pub angular: [D6Motion; 3],
208    /// Optional spring-damper drives (motor+spring) per translational axis.
209    pub linear_drives: [D6Drive; 3],
210    /// Optional spring-damper drives (motor+spring) per rotational axis.
211    pub angular_drives: [D6Drive; 3],
212    /// Inverse stiffness (CFM) for every locked/limited DOF (0 = rigid).
213    pub compliance: f32,
214    #[serde(default)]
215    pub initial_relative_rotation: Option<Quat>,
216}
217
218impl Joint {
219    pub fn joint_type(&self) -> &'static str {
220        match &self.data {
221            JointData::Fixed => "Fixed",
222            JointData::Hinge(_) => "Hinge",
223            JointData::BallSocket(_) => "BallSocket",
224            JointData::Slider(_) => "Slider",
225            JointData::Spring(_) => "Spring",
226            JointData::Distance(_) => "Distance",
227            JointData::D6(_) => "D6",
228        }
229    }
230
231    pub fn fixed(
232        entity_a: BodyHandle,
233        entity_b: BodyHandle,
234        local_anchor_a: Vec3,
235        local_anchor_b: Vec3,
236    ) -> Self {
237        debug_assert_ne!(
238            entity_a, entity_b,
239            "Joint: entity_a and entity_b must be different"
240        );
241        Self {
242            entity_a,
243            entity_b,
244            local_anchor_a,
245            local_anchor_b,
246            break_force: f32::INFINITY,
247            break_torque: f32::INFINITY,
248            is_broken: false,
249            collision_enabled: false,
250            data: JointData::Fixed,
251        }
252    }
253
254    pub fn hinge(
255        entity_a: BodyHandle,
256        entity_b: BodyHandle,
257        local_anchor_a: Vec3,
258        local_anchor_b: Vec3,
259        axis: Vec3,
260    ) -> Self {
261        debug_assert_ne!(
262            entity_a, entity_b,
263            "Joint: entity_a and entity_b must be different"
264        );
265        let safe_axis = if axis.length_squared() > 1e-6 {
266            axis.normalize()
267        } else {
268            Vec3::Y
269        };
270        Self {
271            entity_a,
272            entity_b,
273            local_anchor_a,
274            local_anchor_b,
275            break_force: f32::INFINITY,
276            break_torque: f32::INFINITY,
277            is_broken: false,
278            collision_enabled: false,
279            data: JointData::Hinge(HingeJointData {
280                axis: safe_axis,
281                use_limits: false,
282                lower_limit: -std::f32::consts::PI,
283                upper_limit: std::f32::consts::PI,
284                use_motor: false,
285                motor_target_velocity: 0.0,
286                motor_max_force: 0.0,
287                motor_is_servo: false,
288                motor_target_position: 0.0,
289                use_torsional_spring: false,
290                torsional_stiffness: 0.0,
291                torsional_damping: 0.0,
292                rest_angle: 0.0,
293                current_angle: 0.0,
294            }),
295        }
296    }
297
298    pub fn ball_socket(
299        entity_a: BodyHandle,
300        entity_b: BodyHandle,
301        local_anchor_a: Vec3,
302        local_anchor_b: Vec3,
303    ) -> Self {
304        debug_assert_ne!(
305            entity_a, entity_b,
306            "Joint: entity_a and entity_b must be different"
307        );
308        Self {
309            entity_a,
310            entity_b,
311            local_anchor_a,
312            local_anchor_b,
313            break_force: f32::INFINITY,
314            break_torque: f32::INFINITY,
315            is_broken: false,
316            collision_enabled: false,
317            data: JointData::BallSocket(BallSocketJointData {
318                use_cone_limit: false,
319                cone_limit_angle: std::f32::consts::PI,
320                use_twist_limit: false,
321                twist_axis: Vec3::Y,
322                twist_lower: -std::f32::consts::PI,
323                twist_upper: std::f32::consts::PI,
324                use_swing_limits: false,
325                swing_limit_1: std::f32::consts::PI,
326                swing_limit_2: std::f32::consts::PI,
327                compliance: 0.0,
328                initial_relative_rotation: None,
329            }),
330        }
331    }
332
333    pub fn slider(
334        entity_a: BodyHandle,
335        entity_b: BodyHandle,
336        local_anchor_a: Vec3,
337        local_anchor_b: Vec3,
338        axis: Vec3,
339    ) -> Self {
340        debug_assert_ne!(
341            entity_a, entity_b,
342            "Joint: entity_a and entity_b must be different"
343        );
344        let safe_axis = if axis.length_squared() > 1e-6 {
345            axis.normalize()
346        } else {
347            Vec3::Y
348        };
349        Self {
350            entity_a,
351            entity_b,
352            local_anchor_a,
353            local_anchor_b,
354            break_force: f32::INFINITY,
355            break_torque: f32::INFINITY,
356            is_broken: false,
357            collision_enabled: false,
358            data: JointData::Slider(SliderJointData {
359                axis: safe_axis,
360                use_limits: false,
361                lower_limit: -10.0,
362                upper_limit: 10.0,
363                use_motor: false,
364                motor_target_velocity: 0.0,
365                motor_max_force: 0.0,
366                motor_is_servo: false,
367                motor_target_position: 0.0,
368                use_spring: false,
369                spring_stiffness: 0.0,
370                spring_damping: 0.0,
371                spring_rest_position: 0.0,
372                current_position: 0.0,
373                initial_relative_rotation: None,
374            }),
375        }
376    }
377
378    pub fn spring(
379        entity_a: BodyHandle,
380        entity_b: BodyHandle,
381        local_anchor_a: Vec3,
382        local_anchor_b: Vec3,
383        rest_length: f32,
384        stiffness: f32,
385        damping: f32,
386    ) -> Self {
387        debug_assert_ne!(
388            entity_a, entity_b,
389            "Joint: entity_a and entity_b must be different"
390        );
391        Self {
392            entity_a,
393            entity_b,
394            local_anchor_a,
395            local_anchor_b,
396            break_force: f32::INFINITY,
397            break_torque: f32::INFINITY,
398            is_broken: false,
399            collision_enabled: false,
400            data: JointData::Spring(SpringJointData {
401                rest_length,
402                stiffness,
403                damping,
404                min_length: 0.0,
405                max_length: None,
406            }),
407        }
408    }
409
410    /// Distance joint: constrains the anchor separation to `[min_length, max_length]`
411    /// as a hard inequality. `min == max` ⇒ rigid rod; `min == 0` ⇒ rope (see [`Self::rope`]).
412    pub fn distance(
413        entity_a: BodyHandle,
414        entity_b: BodyHandle,
415        local_anchor_a: Vec3,
416        local_anchor_b: Vec3,
417        min_length: f32,
418        max_length: f32,
419    ) -> Self {
420        debug_assert_ne!(
421            entity_a, entity_b,
422            "Joint: entity_a and entity_b must be different"
423        );
424        Self {
425            entity_a,
426            entity_b,
427            local_anchor_a,
428            local_anchor_b,
429            break_force: f32::INFINITY,
430            break_torque: f32::INFINITY,
431            is_broken: false,
432            collision_enabled: false,
433            data: JointData::Distance(DistanceJointData {
434                min_length: min_length.max(0.0),
435                max_length: max_length.max(min_length.max(0.0)),
436                compliance: 0.0,
437            }),
438        }
439    }
440
441    /// Rope: inextensible but can go slack. The anchors cannot separate beyond `length`
442    /// (pulls when taut), but may come closer (limp when slack) — a released slack body
443    /// free-falls until the rope catches, with no rigid-rod snap. Shorthand for
444    /// `distance(.., 0.0, length)`.
445    pub fn rope(
446        entity_a: BodyHandle,
447        entity_b: BodyHandle,
448        local_anchor_a: Vec3,
449        local_anchor_b: Vec3,
450        length: f32,
451    ) -> Self {
452        Self::distance(entity_a, entity_b, local_anchor_a, local_anchor_b, 0.0, length)
453    }
454
455    /// Generic 6-DOF joint. Starts fully locked (a weld); set `data.linear[i]` /
456    /// `data.angular[i]` to [`D6Motion::Free`]/[`D6Motion::Limited`] to open DOFs — e.g. one
457    /// angular axis Free ⇒ hinge, one linear axis Free ⇒ slider. `frame` (in A's space)
458    /// orients the six axes.
459    pub fn d6(
460        entity_a: BodyHandle,
461        entity_b: BodyHandle,
462        local_anchor_a: Vec3,
463        local_anchor_b: Vec3,
464    ) -> Self {
465        debug_assert_ne!(
466            entity_a, entity_b,
467            "Joint: entity_a and entity_b must be different"
468        );
469        Self {
470            entity_a,
471            entity_b,
472            local_anchor_a,
473            local_anchor_b,
474            break_force: f32::INFINITY,
475            break_torque: f32::INFINITY,
476            is_broken: false,
477            collision_enabled: false,
478            data: JointData::D6(D6JointData {
479                frame: Quat::IDENTITY,
480                linear: [D6Motion::Locked; 3],
481                angular: [D6Motion::Locked; 3],
482                linear_drives: [D6Drive::default(); 3],
483                angular_drives: [D6Drive::default(); 3],
484                compliance: 0.0,
485                initial_relative_rotation: None,
486            }),
487        }
488    }
489
490    pub fn with_break_force(mut self, force: f32, torque: f32) -> Self {
491        self.break_force = force;
492        self.break_torque = torque;
493        self
494    }
495
496    pub fn with_collision(mut self, enabled: bool) -> Self {
497        self.collision_enabled = enabled;
498        self
499    }
500
501    pub fn check_break(&mut self, applied_force: f32, applied_torque: f32) -> bool {
502        if applied_force > self.break_force {
503            self.is_broken = true;
504            return true;
505        }
506        if applied_torque > self.break_torque {
507            self.is_broken = true;
508            return true;
509        }
510        false
511    }
512}
gizmo_physics_rigid/joints/data.rs

gizmo_physics_rigid/joints/
data.rs
