rapier2d/dynamics/joint/
generic_joint.rs

1#![allow(clippy::bad_bit_mask)] // Clippy will complain about the bitmasks due to JointAxesMask::FREE_FIXED_AXES being 0.
2#![allow(clippy::unnecessary_cast)] // Casts are needed for switching between f32/f64.
3
4#[cfg(feature = "alloc")]
5use crate::dynamics::RigidBody;
6use crate::dynamics::integration_parameters::SpringCoefficients;
7#[cfg(feature = "alloc")]
8use crate::dynamics::solver::MotorParameters;
9use crate::dynamics::{FixedJoint, MotorModel, PrismaticJoint, RevoluteJoint, RopeJoint};
10use crate::math::{Pose, Real, Rotation, SPATIAL_DIM, Vector};
11#[cfg(feature = "dim2")]
12use crate::utils::OrthonormalBasis;
13use crate::utils::SimdRealCopy;
14#[cfg(feature = "dim2")]
15use parry::math::Matrix;
16
17#[cfg(feature = "dim3")]
18use crate::dynamics::SphericalJoint;
19
20#[cfg(feature = "dim3")]
21bitflags::bitflags! {
22    /// A bit mask identifying multiple degrees of freedom of a joint.
23    #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
24    #[derive(Copy, Clone, PartialEq, Eq, Debug)]
25    pub struct JointAxesMask: u8 {
26        /// The linear (translational) degree of freedom along the local X axis of a joint.
27        const LIN_X = 1 << 0;
28        /// The linear (translational) degree of freedom along the local Y axis of a joint.
29        const LIN_Y = 1 << 1;
30        /// The linear (translational) degree of freedom along the local Z axis of a joint.
31        const LIN_Z = 1 << 2;
32        /// The angular degree of freedom along the local X axis of a joint.
33        const ANG_X = 1 << 3;
34        /// The angular degree of freedom along the local Y axis of a joint.
35        const ANG_Y = 1 << 4;
36        /// The angular degree of freedom along the local Z axis of a joint.
37        const ANG_Z = 1 << 5;
38        /// The set of degrees of freedom locked by a revolute joint.
39        const LOCKED_REVOLUTE_AXES = Self::LIN_X.bits() | Self::LIN_Y.bits() | Self::LIN_Z.bits() | Self::ANG_Y.bits() | Self::ANG_Z.bits();
40        /// The set of degrees of freedom locked by a prismatic joint.
41        const LOCKED_PRISMATIC_AXES = Self::LIN_Y.bits() | Self::LIN_Z.bits() | Self::ANG_X.bits() | Self::ANG_Y.bits() | Self::ANG_Z.bits();
42        /// The set of degrees of freedom locked by a fixed joint.
43        const LOCKED_FIXED_AXES = Self::LIN_X.bits() | Self::LIN_Y.bits() | Self::LIN_Z.bits() | Self::ANG_X.bits() | Self::ANG_Y.bits() | Self::ANG_Z.bits();
44        /// The set of degrees of freedom locked by a spherical joint.
45        const LOCKED_SPHERICAL_AXES = Self::LIN_X.bits() | Self::LIN_Y.bits() | Self::LIN_Z.bits();
46        /// The set of degrees of freedom left free by a revolute joint.
47        const FREE_REVOLUTE_AXES = Self::ANG_X.bits();
48        /// The set of degrees of freedom left free by a prismatic joint.
49        const FREE_PRISMATIC_AXES = Self::LIN_X.bits();
50        /// The set of degrees of freedom left free by a fixed joint.
51        const FREE_FIXED_AXES = 0;
52        /// The set of degrees of freedom left free by a spherical joint.
53        const FREE_SPHERICAL_AXES = Self::ANG_X.bits() | Self::ANG_Y.bits() | Self::ANG_Z.bits();
54        /// The set of all translational degrees of freedom.
55        const LIN_AXES = Self::LIN_X.bits() | Self::LIN_Y.bits() | Self::LIN_Z.bits();
56        /// The set of all angular degrees of freedom.
57        const ANG_AXES = Self::ANG_X.bits() | Self::ANG_Y.bits() | Self::ANG_Z.bits();
58    }
59}
60
61#[cfg(feature = "dim2")]
62bitflags::bitflags! {
63    /// A bit mask identifying multiple degrees of freedom of a joint.
64    #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
65    #[derive(Copy, Clone, PartialEq, Eq, Debug)]
66    pub struct JointAxesMask: u8 {
67        /// The linear (translational) degree of freedom along the local X axis of a joint.
68        const LIN_X = 1 << 0;
69        /// The linear (translational) degree of freedom along the local Y axis of a joint.
70        const LIN_Y = 1 << 1;
71        /// The angular degree of freedom of a joint.
72        const ANG_X = 1 << 2;
73        /// The set of degrees of freedom locked by a revolute joint.
74        const LOCKED_REVOLUTE_AXES = Self::LIN_X.bits() | Self::LIN_Y.bits();
75        /// The set of degrees of freedom locked by a prismatic joint.
76        const LOCKED_PRISMATIC_AXES = Self::LIN_Y.bits() | Self::ANG_X.bits();
77        /// The set of degrees of freedom locked by a pin slot joint.
78        const LOCKED_PIN_SLOT_AXES = Self::LIN_Y.bits();
79        /// The set of degrees of freedom locked by a fixed joint.
80        const LOCKED_FIXED_AXES = Self::LIN_X.bits() | Self::LIN_Y.bits() | Self::ANG_X.bits();
81        /// The set of degrees of freedom left free by a revolute joint.
82        const FREE_REVOLUTE_AXES = Self::ANG_X.bits();
83        /// The set of degrees of freedom left free by a prismatic joint.
84        const FREE_PRISMATIC_AXES = Self::LIN_X.bits();
85        /// The set of degrees of freedom left free by a fixed joint.
86        const FREE_FIXED_AXES = 0;
87        /// The set of all translational degrees of freedom.
88        const LIN_AXES = Self::LIN_X.bits() | Self::LIN_Y.bits();
89        /// The set of all angular degrees of freedom.
90        const ANG_AXES = Self::ANG_X.bits();
91    }
92}
93
94impl Default for JointAxesMask {
95    fn default() -> Self {
96        Self::empty()
97    }
98}
99
100/// Identifiers of degrees of freedoms of a joint.
101#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
102#[derive(Copy, Clone, Debug, PartialEq)]
103pub enum JointAxis {
104    /// The linear (translational) degree of freedom along the joint’s local X axis.
105    LinX = 0,
106    /// The linear (translational) degree of freedom along the joint’s local Y axis.
107    LinY,
108    /// The linear (translational) degree of freedom along the joint’s local Z axis.
109    #[cfg(feature = "dim3")]
110    LinZ,
111    /// The rotational degree of freedom along the joint’s local X axis.
112    AngX,
113    /// The rotational degree of freedom along the joint’s local Y axis.
114    #[cfg(feature = "dim3")]
115    AngY,
116    /// The rotational degree of freedom along the joint’s local Z axis.
117    #[cfg(feature = "dim3")]
118    AngZ,
119}
120
121impl From<JointAxis> for JointAxesMask {
122    fn from(axis: JointAxis) -> Self {
123        JointAxesMask::from_bits(1 << axis as usize).unwrap()
124    }
125}
126
127/// Limits that restrict a joint's range of motion along one axis.
128///
129/// Use to constrain how far a joint can move/rotate. Examples:
130/// - Door that only opens 90°: revolute joint with limits `[0.0, PI/2.0]`
131/// - Piston with 2-unit stroke: prismatic joint with limits `[0.0, 2.0]`
132/// - Elbow that bends 0-150°: revolute joint with limits `[0.0, 5*PI/6]`
133///
134/// When a joint hits its limit, forces are applied to prevent further movement in that direction.
135///
136/// An angular range may sit anywhere on the circle (`[0, 3π/2]` and `[π, 3π/2]` both work), but
137/// it can't be wider than a full turn: the joint's angle is derived from the bodies' relative
138/// rotation, which doesn't count revolutions, so a wider range is indistinguishable from no
139/// limit at all and leaves the axis free.
140#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
141#[derive(Copy, Clone, Debug, PartialEq)]
142pub struct JointLimits<N> {
143    /// Minimum allowed value (angle for revolute, distance for prismatic).
144    pub min: N,
145    /// Maximum allowed value (angle for revolute, distance for prismatic).
146    pub max: N,
147    /// Internal: impulse being applied to enforce the limit.
148    pub impulse: N,
149}
150
151impl<N: SimdRealCopy> Default for JointLimits<N> {
152    fn default() -> Self {
153        Self {
154            min: -N::splat(Real::MAX),
155            max: N::splat(Real::MAX),
156            impulse: N::splat(0.0),
157        }
158    }
159}
160
161impl<N: SimdRealCopy> From<[N; 2]> for JointLimits<N> {
162    fn from(value: [N; 2]) -> Self {
163        Self {
164            min: value[0],
165            max: value[1],
166            impulse: N::splat(0.0),
167        }
168    }
169}
170
171/// A powered motor that drives a joint toward a target position/velocity.
172///
173/// Motors add actuation to joints - they apply forces to make the joint move toward
174/// a desired state. Think of them as servos, electric motors, or hydraulic actuators.
175///
176/// ## Two control modes
177///
178/// 1. **Velocity control**: Set `target_vel` to make the motor spin/slide at constant speed
179/// 2. **Position control**: Set `target_pos` with `stiffness`/`damping` to reach a target angle/position
180///
181/// You can combine both for precise control.
182///
183/// ## Parameters
184///
185/// - `stiffness`: How strongly to pull toward target (spring constant)
186/// - `damping`: Resistance to motion (prevents oscillation)
187/// - `max_force`: Maximum force/torque the motor can apply
188///
189/// # Example
190/// ```
191/// # use rapier3d::prelude::*;
192/// # use rapier3d::dynamics::{RevoluteJoint, PrismaticJoint};
193/// # let mut revolute_joint = RevoluteJoint::new(Vector::X);
194/// # let mut prismatic_joint = PrismaticJoint::new(Vector::X);
195/// // Motor that spins a wheel at 10 rad/s
196/// revolute_joint.set_motor_velocity(10.0, 0.8);
197///
198/// // Motor that moves to position 5.0
199/// prismatic_joint.set_motor_position(5.0, 100.0, 10.0);  // stiffness=100, damping=10
200/// ```
201#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
202#[derive(Copy, Clone, Debug, PartialEq)]
203pub struct JointMotor {
204    /// Target velocity (units/sec for prismatic, rad/sec for revolute).
205    pub target_vel: Real,
206    /// Target position (units for prismatic, radians for revolute).
207    pub target_pos: Real,
208    /// Spring constant - how strongly to pull toward target position.
209    pub stiffness: Real,
210    /// Damping coefficient - resistance to motion (prevents oscillation).
211    pub damping: Real,
212    /// Maximum force the motor can apply (Newtons for prismatic, Nm for revolute).
213    pub max_force: Real,
214    /// Internal: current impulse being applied.
215    pub impulse: Real,
216    /// Force-based or acceleration-based motor model.
217    pub model: MotorModel,
218}
219
220impl Default for JointMotor {
221    fn default() -> Self {
222        Self {
223            target_pos: 0.0,
224            target_vel: 0.0,
225            stiffness: 0.0,
226            damping: 0.0,
227            max_force: Real::MAX,
228            impulse: 0.0,
229            model: MotorModel::AccelerationBased,
230        }
231    }
232}
233
234#[cfg(feature = "alloc")]
235impl JointMotor {
236    pub(crate) fn motor_params(&self, dt: Real) -> MotorParameters<Real> {
237        let (erp_inv_dt, cfm_coeff, cfm_gain) =
238            self.model
239                .combine_coefficients(dt, self.stiffness, self.damping);
240        MotorParameters {
241            erp_inv_dt,
242            cfm_coeff,
243            cfm_gain,
244            // keep_lhs,
245            target_pos: self.target_pos,
246            target_vel: self.target_vel,
247            max_impulse: Self::max_impulse(self.max_force, dt),
248        }
249    }
250
251    /// The largest impulse a motor with the given max force can apply during `dt`.
252    ///
253    /// This is zero if `dt` is zero, even for an infinite `max_force` (whose product with `dt`
254    /// would be NaN).
255    pub(crate) fn max_impulse(max_force: Real, dt: Real) -> Real {
256        if dt == 0.0 { 0.0 } else { max_force * dt }
257    }
258}
259
260#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
261#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
262/// Enum indicating whether or not a joint is enabled.
263pub enum JointEnabled {
264    /// The joint is enabled.
265    Enabled,
266    /// The joint wasn’t disabled by the user explicitly but it is attached to
267    /// a disabled rigid-body.
268    DisabledByAttachedBody,
269    /// The joint is disabled by the user explicitly.
270    Disabled,
271}
272
273#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
274#[derive(Copy, Clone, Debug, PartialEq)]
275/// A generic joint.
276pub struct GenericJoint {
277    /// The joint’s frame, expressed in the first rigid-body’s local-space.
278    pub local_frame1: Pose,
279    /// The joint’s frame, expressed in the second rigid-body’s local-space.
280    pub local_frame2: Pose,
281    /// The degrees-of-freedoms locked by this joint.
282    pub locked_axes: JointAxesMask,
283    /// The degrees-of-freedoms limited by this joint.
284    pub limit_axes: JointAxesMask,
285    /// The degrees-of-freedoms motorised by this joint.
286    pub motor_axes: JointAxesMask,
287    /// The coupled degrees of freedom of this joint.
288    ///
289    /// Note that coupling degrees of freedoms (DoF) changes the interpretation of the coupled joint’s limits and motors.
290    /// If multiple linear DoF are limited/motorized, only the limits/motor configuration for the first
291    /// coupled linear DoF is applied to all coupled linear DoF. Similarly, if multiple angular DoF are limited/motorized
292    /// only the limits/motor configuration for the first coupled angular DoF is applied to all coupled angular DoF.
293    pub coupled_axes: JointAxesMask,
294    /// The limits, along each degree of freedoms of this joint.
295    ///
296    /// Note that the limit must also be explicitly enabled by the `limit_axes` bitmask.
297    /// For coupled degrees of freedoms (DoF), only the first linear (resp. angular) coupled DoF limit and `limit_axis`
298    /// bitmask is applied to the coupled linear (resp. angular) axes.
299    pub limits: [JointLimits<Real>; SPATIAL_DIM],
300    /// The motors, along each degree of freedoms of this joint.
301    ///
302    /// Note that the motor must also be explicitly enabled by the `motor_axes` bitmask.
303    /// For coupled degrees of freedoms (DoF), only the first linear (resp. angular) coupled DoF motor and `motor_axes`
304    /// bitmask is applied to the coupled linear (resp. angular) axes.
305    pub motors: [JointMotor; SPATIAL_DIM],
306    /// The coefficients controlling the joint constraints’ softness.
307    pub softness: SpringCoefficients<Real>,
308    /// Are contacts between the attached rigid-bodies enabled?
309    pub contacts_enabled: bool,
310    /// Whether the joint is enabled.
311    pub enabled: JointEnabled,
312    /// User-defined data associated to this joint.
313    pub user_data: u128,
314}
315
316impl Default for GenericJoint {
317    fn default() -> Self {
318        Self {
319            local_frame1: Pose::IDENTITY,
320            local_frame2: Pose::IDENTITY,
321            locked_axes: JointAxesMask::empty(),
322            limit_axes: JointAxesMask::empty(),
323            motor_axes: JointAxesMask::empty(),
324            coupled_axes: JointAxesMask::empty(),
325            limits: [JointLimits::default(); SPATIAL_DIM],
326            motors: [JointMotor::default(); SPATIAL_DIM],
327            softness: SpringCoefficients::joint_defaults(),
328            contacts_enabled: true,
329            enabled: JointEnabled::Enabled,
330            user_data: 0,
331        }
332    }
333}
334
335impl GenericJoint {
336    /// Creates a new generic joint that locks the specified degrees of freedom.
337    #[must_use]
338    pub fn new(locked_axes: JointAxesMask) -> Self {
339        *Self::default().lock_axes(locked_axes)
340    }
341
342    /// Can this joint use SIMD-accelerated constraint formulations?
343    ///
344    /// Locked axes and uncoupled limits have wide row formulations, as does the
345    /// 2D angular motor (the workhorse of ragdoll joints); linear motors, 3D
346    /// motors and coupled limit rows don't (yet) and fall back to the scalar
347    /// path.
348    #[cfg(feature = "alloc")]
349    pub(crate) fn supports_simd_constraints(&self) -> bool {
350        #[cfg(feature = "dim2")]
351        let motors_ok =
352            (self.motor_axes.bits() & !self.locked_axes.bits() & JointAxesMask::LIN_AXES.bits())
353                == 0;
354        #[cfg(feature = "dim3")]
355        let motors_ok = (self.motor_axes.bits() & !self.locked_axes.bits()) == 0;
356        motors_ok && (self.limit_axes & self.coupled_axes).is_empty()
357    }
358
359    /// The constraint-row layout signature of this joint: joints sharing it emit
360    /// the same row sequence (kinds, axes and count), so they can share the
361    /// lanes of one SIMD constraint group.
362    #[cfg(feature = "alloc")]
363    pub(crate) fn simd_row_signature(&self) -> u32 {
364        let locked = self.locked_axes.bits() as u32;
365        let limits = (self.limit_axes.bits() & !self.locked_axes.bits()) as u32;
366        #[cfg(feature = "dim2")]
367        {
368            // The angular motor row's coefficient formula depends on the motor
369            // model, so lanes must also share it.
370            let motors = (self.motor_axes.bits() & !self.locked_axes.bits()) as u32;
371            let model = (self.motors[crate::math::DIM].model
372                == crate::dynamics::MotorModel::ForceBased) as u32;
373            locked | (limits << 8) | (motors << 16) | (model << 24)
374        }
375        #[cfg(feature = "dim3")]
376        {
377            locked | (limits << 8)
378        }
379    }
380
381    #[doc(hidden)]
382    pub fn complete_ang_frame(axis: Vector) -> Rotation {
383        #[cfg(feature = "dim2")]
384        {
385            let basis = axis.orthonormal_basis();
386            let mat = Matrix::from_cols(axis, basis[0]);
387            Rotation::from_matrix_unchecked(mat)
388        }
389
390        #[cfg(feature = "dim3")]
391        {
392            // Minimal rotation taking +X to `axis`, NOT an arbitrary orthonormal basis:
393            // frames completed from two independently-set axes must not disagree by a twist,
394            // which fights a prismatic-like joint's angular locks and can diverge.
395            Rotation::from_rotation_arc(Vector::X, axis)
396        }
397    }
398
399    /// Is this joint enabled?
400    pub fn is_enabled(&self) -> bool {
401        self.enabled == JointEnabled::Enabled
402    }
403
404    /// Set whether this joint is enabled or not.
405    pub fn set_enabled(&mut self, enabled: bool) {
406        match self.enabled {
407            JointEnabled::Enabled | JointEnabled::DisabledByAttachedBody => {
408                if !enabled {
409                    self.enabled = JointEnabled::Disabled;
410                }
411            }
412            JointEnabled::Disabled => {
413                if enabled {
414                    self.enabled = JointEnabled::Enabled;
415                }
416            }
417        }
418    }
419
420    /// Add the specified axes to the set of axes locked by this joint.
421    pub fn lock_axes(&mut self, axes: JointAxesMask) -> &mut Self {
422        self.locked_axes |= axes;
423        self
424    }
425
426    /// Sets the joint’s frame, expressed in the first rigid-body’s local-space.
427    pub fn set_local_frame1(&mut self, local_frame: Pose) -> &mut Self {
428        self.local_frame1 = local_frame;
429        self
430    }
431
432    /// Sets the joint’s frame, expressed in the second rigid-body’s local-space.
433    pub fn set_local_frame2(&mut self, local_frame: Pose) -> &mut Self {
434        self.local_frame2 = local_frame;
435        self
436    }
437
438    /// The principal (local X) axis of this joint, expressed in the first rigid-body’s local-space.
439    #[must_use]
440    pub fn local_axis1(&self) -> Vector {
441        self.local_frame1 * Vector::X
442    }
443
444    /// Sets the principal (local X) axis of this joint, expressed in the first rigid-body’s local-space.
445    ///
446    /// The tangent axes of the joint frame are completed deterministically with the minimal
447    /// rotation taking +X to `local_axis`, so frames set from two rotated-but-matching axes
448    /// remain twist-consistent. For exact control over the tangent axes, set the full frame
449    /// with [`Self::set_local_frame1`] instead.
450    pub fn set_local_axis1(&mut self, local_axis: Vector) -> &mut Self {
451        self.local_frame1.rotation = Self::complete_ang_frame(local_axis);
452        self
453    }
454
455    /// The principal (local X) axis of this joint, expressed in the second rigid-body’s local-space.
456    #[must_use]
457    pub fn local_axis2(&self) -> Vector {
458        self.local_frame2 * Vector::X
459    }
460
461    /// Sets the principal (local X) axis of this joint, expressed in the second rigid-body’s local-space.
462    ///
463    /// The tangent axes of the joint frame are completed deterministically with the minimal
464    /// rotation taking +X to `local_axis`, so frames set from two rotated-but-matching axes
465    /// remain twist-consistent. For exact control over the tangent axes, set the full frame
466    /// with [`Self::set_local_frame2`] instead.
467    pub fn set_local_axis2(&mut self, local_axis: Vector) -> &mut Self {
468        self.local_frame2.rotation = Self::complete_ang_frame(local_axis);
469        self
470    }
471
472    /// The anchor of this joint, expressed in the first rigid-body’s local-space.
473    #[must_use]
474    pub fn local_anchor1(&self) -> Vector {
475        self.local_frame1.translation
476    }
477
478    /// Sets anchor of this joint, expressed in the first rigid-body's local-space.
479    pub fn set_local_anchor1(&mut self, anchor1: Vector) -> &mut Self {
480        self.local_frame1.translation = anchor1;
481        self
482    }
483
484    /// The anchor of this joint, expressed in the second rigid-body's local-space.
485    #[must_use]
486    pub fn local_anchor2(&self) -> Vector {
487        self.local_frame2.translation
488    }
489
490    /// Sets anchor of this joint, expressed in the second rigid-body's local-space.
491    pub fn set_local_anchor2(&mut self, anchor2: Vector) -> &mut Self {
492        self.local_frame2.translation = anchor2;
493        self
494    }
495
496    /// Are contacts between the attached rigid-bodies enabled?
497    pub fn contacts_enabled(&self) -> bool {
498        self.contacts_enabled
499    }
500
501    /// Sets whether contacts between the attached rigid-bodies are enabled.
502    pub fn set_contacts_enabled(&mut self, enabled: bool) -> &mut Self {
503        self.contacts_enabled = enabled;
504        self
505    }
506
507    /// Sets the spring coefficients controlling this joint constraint’s softness.
508    #[must_use]
509    pub fn set_softness(&mut self, softness: SpringCoefficients<Real>) -> &mut Self {
510        self.softness = softness;
511        self
512    }
513
514    /// The joint limits along the specified axis.
515    #[must_use]
516    pub fn limits(&self, axis: JointAxis) -> Option<&JointLimits<Real>> {
517        let i = axis as usize;
518        if self.limit_axes.contains(axis.into()) {
519            Some(&self.limits[i])
520        } else {
521            None
522        }
523    }
524
525    /// Sets the joint limits along the specified axis.
526    pub fn set_limits(&mut self, axis: JointAxis, limits: [Real; 2]) -> &mut Self {
527        let i = axis as usize;
528        self.limit_axes |= axis.into();
529        self.limits[i].min = limits[0];
530        self.limits[i].max = limits[1];
531        self
532    }
533
534    /// The spring-like motor model along the specified axis of this joint.
535    #[must_use]
536    pub fn motor_model(&self, axis: JointAxis) -> Option<MotorModel> {
537        let i = axis as usize;
538        if self.motor_axes.contains(axis.into()) {
539            Some(self.motors[i].model)
540        } else {
541            None
542        }
543    }
544
545    /// Set the spring-like model used by the motor to reach the desired target velocity and position.
546    pub fn set_motor_model(&mut self, axis: JointAxis, model: MotorModel) -> &mut Self {
547        self.motors[axis as usize].model = model;
548        self
549    }
550
551    /// Sets the target velocity this motor needs to reach.
552    pub fn set_motor_velocity(
553        &mut self,
554        axis: JointAxis,
555        target_vel: Real,
556        factor: Real,
557    ) -> &mut Self {
558        self.set_motor(
559            axis,
560            self.motors[axis as usize].target_pos,
561            target_vel,
562            0.0,
563            factor,
564        )
565    }
566
567    /// Sets the target angle this motor needs to reach.
568    pub fn set_motor_position(
569        &mut self,
570        axis: JointAxis,
571        target_pos: Real,
572        stiffness: Real,
573        damping: Real,
574    ) -> &mut Self {
575        self.set_motor(axis, target_pos, 0.0, stiffness, damping)
576    }
577
578    /// Sets the maximum force the motor can deliver along the specified axis.
579    pub fn set_motor_max_force(&mut self, axis: JointAxis, max_force: Real) -> &mut Self {
580        self.motors[axis as usize].max_force = max_force;
581        self
582    }
583
584    /// The motor affecting the joint’s degree of freedom along the specified axis.
585    #[must_use]
586    pub fn motor(&self, axis: JointAxis) -> Option<&JointMotor> {
587        let i = axis as usize;
588        if self.motor_axes.contains(axis.into()) {
589            Some(&self.motors[i])
590        } else {
591            None
592        }
593    }
594
595    /// Configure both the target angle and target velocity of the motor.
596    pub fn set_motor(
597        &mut self,
598        axis: JointAxis,
599        target_pos: Real,
600        target_vel: Real,
601        stiffness: Real,
602        damping: Real,
603    ) -> &mut Self {
604        self.motor_axes |= axis.into();
605        let i = axis as usize;
606        self.motors[i].target_vel = target_vel;
607        self.motors[i].target_pos = target_pos;
608        self.motors[i].stiffness = stiffness;
609        self.motors[i].damping = damping;
610        self
611    }
612
613    /// Flips the orientation of the joint, including limits and motors.
614    pub fn flip(&mut self) {
615        core::mem::swap(&mut self.local_frame1, &mut self.local_frame2);
616
617        let coupled_bits = self.coupled_axes.bits();
618
619        for dim in 0..SPATIAL_DIM {
620            if coupled_bits & (1 << dim) == 0 {
621                let limit = self.limits[dim];
622                self.limits[dim].min = -limit.max;
623                self.limits[dim].max = -limit.min;
624            }
625
626            self.motors[dim].target_vel = -self.motors[dim].target_vel;
627            self.motors[dim].target_pos = -self.motors[dim].target_pos;
628        }
629    }
630
631    #[cfg(feature = "alloc")]
632    pub(crate) fn transform_to_solver_body_space(&mut self, rb1: &RigidBody, rb2: &RigidBody) {
633        if rb1.is_fixed() {
634            self.local_frame1 = rb1.pos.position * self.local_frame1;
635        } else {
636            self.local_frame1.translation -= rb1.mprops.local_mprops.local_com;
637        }
638
639        if rb2.is_fixed() {
640            self.local_frame2 = rb2.pos.position * self.local_frame2;
641        } else {
642            self.local_frame2.translation -= rb2.mprops.local_mprops.local_com;
643        }
644    }
645}
646
647macro_rules! joint_conversion_methods(
648    ($as_joint: ident, $as_joint_mut: ident, $Joint: ty, $axes: expr) => {
649        /// Converts the joint to its specific variant, if it is one.
650        #[must_use]
651        pub fn $as_joint(&self) -> Option<&$Joint> {
652            if self.locked_axes == $axes {
653                // SAFETY: this is OK because the target joint type is
654                //         a `repr(transparent)` newtype of `Joint`.
655                Some(unsafe { core::mem::transmute::<&Self, &$Joint>(self) })
656            } else {
657                None
658            }
659        }
660
661        /// Converts the joint to its specific mutable variant, if it is one.
662        #[must_use]
663        pub fn $as_joint_mut(&mut self) -> Option<&mut $Joint> {
664            if self.locked_axes == $axes {
665                // SAFETY: this is OK because the target joint type is
666                //         a `repr(transparent)` newtype of `Joint`.
667                Some(unsafe { core::mem::transmute::<&mut Self, &mut $Joint>(self) })
668            } else {
669                None
670            }
671        }
672    }
673);
674
675impl GenericJoint {
676    joint_conversion_methods!(
677        as_revolute,
678        as_revolute_mut,
679        RevoluteJoint,
680        JointAxesMask::LOCKED_REVOLUTE_AXES
681    );
682    joint_conversion_methods!(
683        as_fixed,
684        as_fixed_mut,
685        FixedJoint,
686        JointAxesMask::LOCKED_FIXED_AXES
687    );
688    joint_conversion_methods!(
689        as_prismatic,
690        as_prismatic_mut,
691        PrismaticJoint,
692        JointAxesMask::LOCKED_PRISMATIC_AXES
693    );
694    joint_conversion_methods!(
695        as_rope,
696        as_rope_mut,
697        RopeJoint,
698        JointAxesMask::FREE_FIXED_AXES
699    );
700
701    #[cfg(feature = "dim3")]
702    joint_conversion_methods!(
703        as_spherical,
704        as_spherical_mut,
705        SphericalJoint,
706        JointAxesMask::LOCKED_SPHERICAL_AXES
707    );
708}
709
710/// Create generic joints using the builder pattern.
711#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
712#[derive(Copy, Clone, Debug, PartialEq)]
713pub struct GenericJointBuilder(pub GenericJoint);
714
715impl GenericJointBuilder {
716    /// Creates a new generic joint builder.
717    #[must_use]
718    pub fn new(locked_axes: JointAxesMask) -> Self {
719        Self(GenericJoint::new(locked_axes))
720    }
721
722    /// Sets the degrees of freedom locked by the joint.
723    #[must_use]
724    pub fn locked_axes(mut self, axes: JointAxesMask) -> Self {
725        self.0.locked_axes = axes;
726        self
727    }
728
729    /// Sets whether contacts between the attached rigid-bodies are enabled.
730    #[must_use]
731    pub fn contacts_enabled(mut self, enabled: bool) -> Self {
732        self.0.contacts_enabled = enabled;
733        self
734    }
735
736    /// Sets the joint’s frame, expressed in the first rigid-body’s local-space.
737    #[must_use]
738    pub fn local_frame1(mut self, local_frame: Pose) -> Self {
739        self.0.set_local_frame1(local_frame);
740        self
741    }
742
743    /// Sets the joint’s frame, expressed in the second rigid-body’s local-space.
744    #[must_use]
745    pub fn local_frame2(mut self, local_frame: Pose) -> Self {
746        self.0.set_local_frame2(local_frame);
747        self
748    }
749
750    /// Sets the principal (local X) axis of this joint, expressed in the first rigid-body’s local-space.
751    #[must_use]
752    pub fn local_axis1(mut self, local_axis: Vector) -> Self {
753        self.0.set_local_axis1(local_axis);
754        self
755    }
756
757    /// Sets the principal (local X) axis of this joint, expressed in the second rigid-body’s local-space.
758    #[must_use]
759    pub fn local_axis2(mut self, local_axis: Vector) -> Self {
760        self.0.set_local_axis2(local_axis);
761        self
762    }
763
764    /// Sets the anchor of this joint, expressed in the first rigid-body’s local-space.
765    #[must_use]
766    pub fn local_anchor1(mut self, anchor1: Vector) -> Self {
767        self.0.set_local_anchor1(anchor1);
768        self
769    }
770
771    /// Sets the anchor of this joint, expressed in the second rigid-body’s local-space.
772    #[must_use]
773    pub fn local_anchor2(mut self, anchor2: Vector) -> Self {
774        self.0.set_local_anchor2(anchor2);
775        self
776    }
777
778    /// Sets the joint limits along the specified axis.
779    #[must_use]
780    pub fn limits(mut self, axis: JointAxis, limits: [Real; 2]) -> Self {
781        self.0.set_limits(axis, limits);
782        self
783    }
784
785    /// Sets the coupled degrees of freedom for this joint’s limits and motor.
786    #[must_use]
787    pub fn coupled_axes(mut self, axes: JointAxesMask) -> Self {
788        self.0.coupled_axes = axes;
789        self
790    }
791
792    /// Set the spring-like model used by the motor to reach the desired target velocity and position.
793    #[must_use]
794    pub fn motor_model(mut self, axis: JointAxis, model: MotorModel) -> Self {
795        self.0.set_motor_model(axis, model);
796        self
797    }
798
799    /// Sets the target velocity this motor needs to reach.
800    #[must_use]
801    pub fn motor_velocity(mut self, axis: JointAxis, target_vel: Real, factor: Real) -> Self {
802        self.0.set_motor_velocity(axis, target_vel, factor);
803        self
804    }
805
806    /// Sets the target angle this motor needs to reach.
807    #[must_use]
808    pub fn motor_position(
809        mut self,
810        axis: JointAxis,
811        target_pos: Real,
812        stiffness: Real,
813        damping: Real,
814    ) -> Self {
815        self.0
816            .set_motor_position(axis, target_pos, stiffness, damping);
817        self
818    }
819
820    /// Configure both the target angle and target velocity of the motor.
821    #[must_use]
822    pub fn set_motor(
823        mut self,
824        axis: JointAxis,
825        target_pos: Real,
826        target_vel: Real,
827        stiffness: Real,
828        damping: Real,
829    ) -> Self {
830        self.0
831            .set_motor(axis, target_pos, target_vel, stiffness, damping);
832        self
833    }
834
835    /// Sets the maximum force the motor can deliver along the specified axis.
836    #[must_use]
837    pub fn motor_max_force(mut self, axis: JointAxis, max_force: Real) -> Self {
838        self.0.set_motor_max_force(axis, max_force);
839        self
840    }
841
842    /// Sets the softness of this joint’s locked degrees of freedom.
843    #[must_use]
844    pub fn softness(mut self, softness: SpringCoefficients<Real>) -> Self {
845        self.0.softness = softness;
846        self
847    }
848
849    /// An arbitrary user-defined 128-bit integer associated to the joints built by this builder.
850    pub fn user_data(mut self, data: u128) -> Self {
851        self.0.user_data = data;
852        self
853    }
854
855    /// Builds the generic joint.
856    #[must_use]
857    pub fn build(self) -> GenericJoint {
858        self.0
859    }
860}
861
862impl From<GenericJointBuilder> for GenericJoint {
863    fn from(val: GenericJointBuilder) -> GenericJoint {
864        val.0
865    }
866}
867
868#[cfg(all(test, feature = "alloc"))]
869mod test {
870    use super::JointMotor;
871    use crate::math::Real;
872
873    #[test]
874    fn infinite_motor_force_with_zero_dt_has_finite_impulse_bounds() {
875        let motor = JointMotor {
876            max_force: Real::INFINITY,
877            target_vel: 1.0,
878            damping: 1.0,
879            ..Default::default()
880        };
881        assert_eq!(motor.motor_params(0.0).max_impulse, 0.0);
882        assert_eq!(motor.motor_params(1.0 / 60.0).max_impulse, Real::INFINITY);
883
884        let motor = JointMotor::default();
885        assert_eq!(motor.motor_params(0.0).max_impulse, 0.0);
886        assert_eq!(motor.motor_params(0.5).max_impulse, Real::MAX * 0.5);
887    }
888}
rapier2d/dynamics/joint/generic_joint.rs

rapier2d/dynamics/joint/
generic_joint.rs
