rapier3d/dynamics/joint/
revolute_joint.rs

1use crate::dynamics::joint::{GenericJoint, GenericJointBuilder, JointAxesMask};
2use crate::dynamics::{JointAxis, JointLimits, JointMotor, MotorModel};
3use crate::math::{Point, Real, Rotation};
4
5#[cfg(feature = "dim3")]
6use crate::math::UnitVector;
7
8#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
9#[derive(Copy, Clone, Debug, PartialEq)]
10#[repr(transparent)]
11/// A revolute joint, locks all relative motion except for rotation along the joint’s principal axis.
12pub struct RevoluteJoint {
13    /// The underlying joint data.
14    pub data: GenericJoint,
15}
16
17impl RevoluteJoint {
18    /// Creates a new revolute joint allowing only relative rotations.
19    #[cfg(feature = "dim2")]
20    #[allow(clippy::new_without_default)] // For symmetry with 3D which can’t have a Default impl.
21    pub fn new() -> Self {
22        let data = GenericJointBuilder::new(JointAxesMask::LOCKED_REVOLUTE_AXES);
23        Self { data: data.build() }
24    }
25
26    /// Creates a new revolute joint allowing only relative rotations along the specified axis.
27    ///
28    /// This axis is expressed in the local-space of both rigid-bodies.
29    #[cfg(feature = "dim3")]
30    pub fn new(axis: UnitVector<Real>) -> Self {
31        let data = GenericJointBuilder::new(JointAxesMask::LOCKED_REVOLUTE_AXES)
32            .local_axis1(axis)
33            .local_axis2(axis)
34            .build();
35        Self { data }
36    }
37
38    /// The underlying generic joint.
39    pub fn data(&self) -> &GenericJoint {
40        &self.data
41    }
42
43    /// Are contacts between the attached rigid-bodies enabled?
44    pub fn contacts_enabled(&self) -> bool {
45        self.data.contacts_enabled
46    }
47
48    /// Sets whether contacts between the attached rigid-bodies are enabled.
49    pub fn set_contacts_enabled(&mut self, enabled: bool) -> &mut Self {
50        self.data.set_contacts_enabled(enabled);
51        self
52    }
53
54    /// The joint’s anchor, expressed in the local-space of the first rigid-body.
55    #[must_use]
56    pub fn local_anchor1(&self) -> Point<Real> {
57        self.data.local_anchor1()
58    }
59
60    /// Sets the joint’s anchor, expressed in the local-space of the first rigid-body.
61    pub fn set_local_anchor1(&mut self, anchor1: Point<Real>) -> &mut Self {
62        self.data.set_local_anchor1(anchor1);
63        self
64    }
65
66    /// The joint’s anchor, expressed in the local-space of the second rigid-body.
67    #[must_use]
68    pub fn local_anchor2(&self) -> Point<Real> {
69        self.data.local_anchor2()
70    }
71
72    /// Sets the joint’s anchor, expressed in the local-space of the second rigid-body.
73    pub fn set_local_anchor2(&mut self, anchor2: Point<Real>) -> &mut Self {
74        self.data.set_local_anchor2(anchor2);
75        self
76    }
77
78    /// The angle along the free degree of freedom of this revolute joint in `[-π, π]`.
79    ///
80    /// # Parameters
81    /// - `rb_rot1`: the rotation of the first rigid-body attached to this revolute joint.
82    /// - `rb_rot2`: the rotation of the second rigid-body attached to this revolute joint.
83    pub fn angle(&self, rb_rot1: &Rotation<Real>, rb_rot2: &Rotation<Real>) -> Real {
84        let joint_rot1 = rb_rot1 * self.data.local_frame1.rotation;
85        let joint_rot2 = rb_rot2 * self.data.local_frame2.rotation;
86        let ang_err = joint_rot1.inverse() * joint_rot2;
87
88        #[cfg(feature = "dim3")]
89        if joint_rot1.dot(&joint_rot2) < 0.0 {
90            -ang_err.i.clamp(-1.0, 1.0).asin() * 2.0
91        } else {
92            ang_err.i.clamp(-1.0, 1.0).asin() * 2.0
93        }
94
95        #[cfg(feature = "dim2")]
96        {
97            ang_err.angle()
98        }
99    }
100
101    /// The motor affecting the joint’s rotational degree of freedom.
102    #[must_use]
103    pub fn motor(&self) -> Option<&JointMotor> {
104        self.data.motor(JointAxis::AngX)
105    }
106
107    /// Set the spring-like model used by the motor to reach the desired target velocity and position.
108    pub fn set_motor_model(&mut self, model: MotorModel) -> &mut Self {
109        self.data.set_motor_model(JointAxis::AngX, model);
110        self
111    }
112
113    /// Sets the target velocity this motor needs to reach.
114    pub fn set_motor_velocity(&mut self, target_vel: Real, factor: Real) -> &mut Self {
115        self.data
116            .set_motor_velocity(JointAxis::AngX, target_vel, factor);
117        self
118    }
119
120    /// Sets the target angle this motor needs to reach.
121    pub fn set_motor_position(
122        &mut self,
123        target_pos: Real,
124        stiffness: Real,
125        damping: Real,
126    ) -> &mut Self {
127        self.data
128            .set_motor_position(JointAxis::AngX, target_pos, stiffness, damping);
129        self
130    }
131
132    /// Configure both the target angle and target velocity of the motor.
133    pub fn set_motor(
134        &mut self,
135        target_pos: Real,
136        target_vel: Real,
137        stiffness: Real,
138        damping: Real,
139    ) -> &mut Self {
140        self.data
141            .set_motor(JointAxis::AngX, target_pos, target_vel, stiffness, damping);
142        self
143    }
144
145    /// Sets the maximum force the motor can deliver.
146    pub fn set_motor_max_force(&mut self, max_force: Real) -> &mut Self {
147        self.data.set_motor_max_force(JointAxis::AngX, max_force);
148        self
149    }
150
151    /// The limit angle attached bodies can translate along the joint’s principal axis.
152    #[must_use]
153    pub fn limits(&self) -> Option<&JointLimits<Real>> {
154        self.data.limits(JointAxis::AngX)
155    }
156
157    /// Sets the `[min,max]` limit angle attached bodies can translate along the joint’s principal axis.
158    pub fn set_limits(&mut self, limits: [Real; 2]) -> &mut Self {
159        self.data.set_limits(JointAxis::AngX, limits);
160        self
161    }
162}
163
164impl From<RevoluteJoint> for GenericJoint {
165    fn from(val: RevoluteJoint) -> GenericJoint {
166        val.data
167    }
168}
169
170/// Create revolute joints using the builder pattern.
171///
172/// A revolute joint locks all relative motion except for rotations along the joint’s principal axis.
173#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
174#[derive(Copy, Clone, Debug, PartialEq)]
175pub struct RevoluteJointBuilder(pub RevoluteJoint);
176
177impl RevoluteJointBuilder {
178    /// Creates a new revolute joint builder.
179    #[cfg(feature = "dim2")]
180    #[allow(clippy::new_without_default)] // For symmetry with 3D which can’t have a Default impl.
181    pub fn new() -> Self {
182        Self(RevoluteJoint::new())
183    }
184
185    /// Creates a new revolute joint builder, allowing only relative rotations along the specified axis.
186    ///
187    /// This axis is expressed in the local-space of both rigid-bodies.
188    #[cfg(feature = "dim3")]
189    pub fn new(axis: UnitVector<Real>) -> Self {
190        Self(RevoluteJoint::new(axis))
191    }
192
193    /// Sets whether contacts between the attached rigid-bodies are enabled.
194    #[must_use]
195    pub fn contacts_enabled(mut self, enabled: bool) -> Self {
196        self.0.set_contacts_enabled(enabled);
197        self
198    }
199
200    /// Sets the joint’s anchor, expressed in the local-space of the first rigid-body.
201    #[must_use]
202    pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {
203        self.0.set_local_anchor1(anchor1);
204        self
205    }
206
207    /// Sets the joint’s anchor, expressed in the local-space of the second rigid-body.
208    #[must_use]
209    pub fn local_anchor2(mut self, anchor2: Point<Real>) -> Self {
210        self.0.set_local_anchor2(anchor2);
211        self
212    }
213
214    /// Set the spring-like model used by the motor to reach the desired target velocity and position.
215    #[must_use]
216    pub fn motor_model(mut self, model: MotorModel) -> Self {
217        self.0.set_motor_model(model);
218        self
219    }
220
221    /// Sets the target velocity this motor needs to reach.
222    #[must_use]
223    pub fn motor_velocity(mut self, target_vel: Real, factor: Real) -> Self {
224        self.0.set_motor_velocity(target_vel, factor);
225        self
226    }
227
228    /// Sets the target angle this motor needs to reach.
229    #[must_use]
230    pub fn motor_position(mut self, target_pos: Real, stiffness: Real, damping: Real) -> Self {
231        self.0.set_motor_position(target_pos, stiffness, damping);
232        self
233    }
234
235    /// Configure both the target angle and target velocity of the motor.
236    #[must_use]
237    pub fn motor(
238        mut self,
239        target_pos: Real,
240        target_vel: Real,
241        stiffness: Real,
242        damping: Real,
243    ) -> Self {
244        self.0.set_motor(target_pos, target_vel, stiffness, damping);
245        self
246    }
247
248    /// Sets the maximum force the motor can deliver.
249    #[must_use]
250    pub fn motor_max_force(mut self, max_force: Real) -> Self {
251        self.0.set_motor_max_force(max_force);
252        self
253    }
254
255    /// Sets the `[min,max]` limit angles attached bodies can rotate along the joint’s principal axis.
256    #[must_use]
257    pub fn limits(mut self, limits: [Real; 2]) -> Self {
258        self.0.set_limits(limits);
259        self
260    }
261
262    /// Builds the revolute joint.
263    #[must_use]
264    pub fn build(self) -> RevoluteJoint {
265        self.0
266    }
267}
268
269impl From<RevoluteJointBuilder> for GenericJoint {
270    fn from(val: RevoluteJointBuilder) -> GenericJoint {
271        val.0.into()
272    }
273}
274
275#[cfg(test)]
276mod test {
277    #[test]
278    fn test_revolute_joint_angle() {
279        use crate::math::{Real, Rotation};
280        use crate::na::RealField;
281        #[cfg(feature = "dim3")]
282        use crate::{math::Vector, na::vector};
283
284        #[cfg(feature = "dim2")]
285        let revolute = super::RevoluteJointBuilder::new().build();
286        #[cfg(feature = "dim2")]
287        let rot1 = Rotation::new(1.0);
288        #[cfg(feature = "dim3")]
289        let revolute = super::RevoluteJointBuilder::new(Vector::y_axis()).build();
290        #[cfg(feature = "dim3")]
291        let rot1 = Rotation::new(vector![0.0, 1.0, 0.0]);
292
293        let steps = 100;
294
295        // The -pi and pi values will be checked later.
296        for i in 1..steps {
297            let delta = -Real::pi() + i as Real * Real::two_pi() / steps as Real;
298            #[cfg(feature = "dim2")]
299            let rot2 = Rotation::new(1.0 + delta);
300            #[cfg(feature = "dim3")]
301            let rot2 = Rotation::new(vector![0.0, 1.0 + delta, 0.0]);
302            approx::assert_relative_eq!(revolute.angle(&rot1, &rot2), delta, epsilon = 1.0e-5);
303        }
304
305        // Check the special case for -pi and pi that may return an angle with a flipped sign
306        // (because they are equivalent).
307        for delta in [-Real::pi(), Real::pi()] {
308            #[cfg(feature = "dim2")]
309            let rot2 = Rotation::new(1.0 + delta);
310            #[cfg(feature = "dim3")]
311            let rot2 = Rotation::new(vector![0.0, 1.0 + delta, 0.0]);
312            approx::assert_relative_eq!(
313                revolute.angle(&rot1, &rot2).abs(),
314                delta.abs(),
315                epsilon = 1.0e-2
316            );
317        }
318    }
319}
rapier3d/dynamics/joint/revolute_joint.rs

rapier3d/dynamics/joint/
revolute_joint.rs
