rapier 0.20 feature: RevoluteJoint::angle + more detailed joints enum (#530)

Co-authored-by: Sébastien Crozet <[email protected]>

Author: ThierryBerger

CHANGELOG.md

@@ -6,8 +6,17 @@
 and new features. Please have a look at the
 [0.20 and 0.21 changelogs](https://github.com/dimforge/rapier/blob/master/CHANGELOG.md) of Rapier.**
 
+### Modified
+
 - Update to rapier `0.21`.
 - Update to nalgebra `0.33`.
+- `ImpulseJoint::data` and `MultibodyJoint::data` are now a more detailed enum `TypedJoint` instead of a `GenericJoint`.
+You can still access its inner `GenericJoint` with `.as_ref()` or `as_mut()`.
+- `data` fields from all joints (`FixedJoint`, …) are now public, and their getters removed.
+
+### Added
+
+- Added `RapierContext::context.impulse_revolute_joint_angle` to compute the angle along a revolute joint’s principal axis.
 
 ## v0.27.0-rc.1 (18 June 2024)
 

bevy_rapier2d/examples/debug_despawn2.rs

@@ -174,8 +174,7 @@ fn spawn_cube(commands: &mut Commands, game: &mut Game) {
                         block_entities[*i],
                         RevoluteJointBuilder::new()
                             .local_anchor1(anchor_1)
-                            .local_anchor2(anchor_2)
-                            .build(),
+                            .local_anchor2(anchor_2),
                     ))
                     .id();
                 game.current_cube_joints.push(id);

bevy_rapier3d/examples/joints3.rs

@@ -1,4 +1,6 @@
-use bevy::prelude::*;
+use std::time::Duration;
+
+use bevy::{prelude::*, time::common_conditions::once_after_delay};
 use bevy_rapier3d::prelude::*;
 
 fn main() {
@@ -14,6 +16,11 @@ fn main() {
             RapierDebugRenderPlugin::default(),
         ))
         .add_systems(Startup, (setup_graphics, setup_physics))
+        .add_systems(
+            Last,
+            (print_impulse_revolute_joints,)
+                .run_if(once_after_delay(Duration::from_secs_f32(1f32))),
+        )
         .run();
 }
 
@@ -134,7 +141,6 @@ fn create_revolute_joints(commands: &mut Commands, origin: Vec3, num: usize) {
             RevoluteJointBuilder::new(z).local_anchor2(Vec3::new(0.0, 0.0, -shift)),
             RevoluteJointBuilder::new(x).local_anchor2(Vec3::new(shift, 0.0, 0.0)),
         ];
-
         commands
             .entity(handles[0])
             .insert(ImpulseJoint::new(curr_parent, revs[0]));
@@ -276,3 +282,21 @@ pub fn setup_physics(mut commands: Commands) {
     create_rope_joints(&mut commands, Vec3::new(30.0, 10.0, 0.0), 5);
     create_ball_joints(&mut commands, 15);
 }
+
+pub fn print_impulse_revolute_joints(
+    context: Res<RapierContext>,
+    joints: Query<(Entity, &ImpulseJoint)>,
+) {
+    for (entity, impulse_joint) in joints.iter() {
+        match &impulse_joint.data {
+            TypedJoint::RevoluteJoint(_revolute_joint) => {
+                println!(
+                    "angle for {}: {:?}",
+                    entity,
+                    context.impulse_revolute_joint_angle(entity),
+                );
+            }
+            _ => {}
+        }
+    }
+}

src/dynamics/fixed_joint.rs

@@ -2,11 +2,14 @@ use crate::dynamics::{GenericJoint, GenericJointBuilder};
 use crate::math::{Rot, Vect};
 use rapier::dynamics::JointAxesMask;
 
+use super::TypedJoint;
+
 #[derive(Copy, Clone, Debug, PartialEq)]
 #[repr(transparent)]
 /// A fixed joint, locks all relative motion between two bodies.
 pub struct FixedJoint {
-    data: GenericJoint,
+    /// The underlying joint data.
+    pub data: GenericJoint,
 }
 
 impl Default for FixedJoint {
@@ -134,8 +137,14 @@ impl FixedJointBuilder {
     }
 }
 
-impl From<FixedJointBuilder> for GenericJoint {
-    fn from(joint: FixedJointBuilder) -> GenericJoint {
+impl From<FixedJointBuilder> for TypedJoint {
+    fn from(joint: FixedJointBuilder) -> TypedJoint {
         joint.0.into()
     }
 }
+
+impl From<FixedJoint> for TypedJoint {
+    fn from(joint: FixedJoint) -> TypedJoint {
+        TypedJoint::FixedJoint(joint)
+    }
+}

src/dynamics/joint.rs

@@ -1,9 +1,63 @@
-use crate::dynamics::GenericJoint;
 use bevy::prelude::*;
 use rapier::dynamics::{ImpulseJointHandle, MultibodyJointHandle};
 
 pub use rapier::dynamics::{JointAxesMask, JointAxis, MotorModel};
 
+use super::{FixedJoint, GenericJoint, PrismaticJoint, RevoluteJoint, RopeJoint, SpringJoint};
+
+#[cfg(feature = "dim3")]
+use super::SphericalJoint;
+
+/// Wrapper enum over a specific joint.
+#[derive(Clone, Copy, Debug, PartialEq)]
+pub enum TypedJoint {
+    /// See [`FixedJoint`]
+    FixedJoint(FixedJoint),
+    /// See [`GenericJoint`]
+    GenericJoint(GenericJoint),
+    /// See [`PrismaticJoint`]
+    PrismaticJoint(PrismaticJoint),
+    /// See [`RevoluteJoint`]
+    RevoluteJoint(RevoluteJoint),
+    /// See [`RopeJoint`]
+    RopeJoint(RopeJoint),
+    /// See [`SphericalJoint`]
+    #[cfg(feature = "dim3")]
+    SphericalJoint(SphericalJoint),
+    /// See [`SpringJoint`]
+    SpringJoint(SpringJoint),
+}
+
+impl AsMut<GenericJoint> for TypedJoint {
+    fn as_mut(&mut self) -> &mut GenericJoint {
+        match self {
+            TypedJoint::FixedJoint(ref mut j) => &mut j.data,
+            TypedJoint::GenericJoint(ref mut j) => j,
+            TypedJoint::PrismaticJoint(ref mut j) => &mut j.data,
+            TypedJoint::RevoluteJoint(ref mut j) => &mut j.data,
+            TypedJoint::RopeJoint(ref mut j) => &mut j.data,
+            #[cfg(feature = "dim3")]
+            TypedJoint::SphericalJoint(ref mut j) => &mut j.data,
+            TypedJoint::SpringJoint(ref mut j) => &mut j.data,
+        }
+    }
+}
+
+impl AsRef<GenericJoint> for TypedJoint {
+    fn as_ref(&self) -> &GenericJoint {
+        match self {
+            TypedJoint::FixedJoint(j) => &j.data,
+            TypedJoint::GenericJoint(j) => j,
+            TypedJoint::PrismaticJoint(j) => &j.data,
+            TypedJoint::RevoluteJoint(j) => &j.data,
+            TypedJoint::RopeJoint(j) => &j.data,
+            #[cfg(feature = "dim3")]
+            TypedJoint::SphericalJoint(j) => &j.data,
+            TypedJoint::SpringJoint(j) => &j.data,
+        }
+    }
+}
+
 /// The handle of an impulse joint added to the physics scene.
 #[derive(Copy, Clone, Debug, Component)]
 pub struct RapierImpulseJointHandle(pub ImpulseJointHandle);
@@ -28,12 +82,12 @@ pub struct ImpulseJoint {
     /// The entity containing the rigid-body used as the first endpoint of this joint.
     pub parent: Entity,
     /// The joint’s description.
-    pub data: GenericJoint,
+    pub data: TypedJoint,
 }
 
 impl ImpulseJoint {
     /// Initializes an impulse-based joint from its first endpoint and the joint description.
-    pub fn new(parent: Entity, data: impl Into<GenericJoint>) -> Self {
+    pub fn new(parent: Entity, data: impl Into<TypedJoint>) -> Self {
         Self {
             parent,
             data: data.into(),
@@ -55,16 +109,13 @@ pub struct MultibodyJoint {
     /// The entity containing the rigid-body used as the first endpoint of this joint.
     pub parent: Entity,
     /// The joint’s description.
-    pub data: GenericJoint,
+    pub data: TypedJoint,
 }
 
 impl MultibodyJoint {
     /// Initializes an joint based on reduced coordinates from its first endpoint and
     /// the joint description.
-    pub fn new(parent: Entity, data: impl Into<GenericJoint>) -> Self {
-        Self {
-            parent,
-            data: data.into(),
-        }
+    pub fn new(parent: Entity, data: TypedJoint) -> Self {
+        Self { parent, data }
     }
 }

src/dynamics/prismatic_joint.rs

@@ -2,12 +2,15 @@ use crate::dynamics::{GenericJoint, GenericJointBuilder};
 use crate::math::{Real, Vect};
 use rapier::dynamics::{JointAxesMask, JointAxis, JointLimits, JointMotor, MotorModel};
 
+use super::TypedJoint;
+
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[derive(Copy, Clone, Debug, PartialEq)]
 #[repr(transparent)]
 /// A prismatic joint, locks all relative motion between two bodies except for translation along the joint’s principal axis.
 pub struct PrismaticJoint {
-    data: GenericJoint,
+    /// The underlying joint data.
+    pub data: GenericJoint,
 }
 
 impl PrismaticJoint {
@@ -22,11 +25,6 @@ impl PrismaticJoint {
         Self { data }
     }
 
-    /// The underlying generic joint.
-    pub fn data(&self) -> &GenericJoint {
-        &self.data
-    }
-
     /// Are contacts between the attached rigid-bodies enabled?
     pub fn contacts_enabled(&self) -> bool {
         self.data.contacts_enabled()
@@ -253,8 +251,14 @@ impl PrismaticJointBuilder {
     }
 }
 
-impl From<PrismaticJointBuilder> for GenericJoint {
-    fn from(joint: PrismaticJointBuilder) -> GenericJoint {
+impl From<PrismaticJointBuilder> for TypedJoint {
+    fn from(joint: PrismaticJointBuilder) -> TypedJoint {
         joint.0.into()
     }
 }
+
+impl From<PrismaticJoint> for TypedJoint {
+    fn from(joint: PrismaticJoint) -> TypedJoint {
+        TypedJoint::PrismaticJoint(joint)
+    }
+}

src/dynamics/revolute_joint.rs

@@ -1,13 +1,20 @@
 use crate::dynamics::{GenericJoint, GenericJointBuilder};
 use crate::math::{Real, Vect};
-use rapier::dynamics::{JointAxesMask, JointAxis, JointLimits, JointMotor, MotorModel};
+use crate::plugin::RapierContext;
+use bevy::prelude::Entity;
+use rapier::dynamics::{
+    JointAxesMask, JointAxis, JointLimits, JointMotor, MotorModel, RigidBodyHandle, RigidBodySet,
+};
+
+use super::TypedJoint;
 
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[derive(Copy, Clone, Debug, PartialEq)]
 #[repr(transparent)]
 /// A revolute joint, locks all relative motion except for rotation along the joint’s principal axis.
 pub struct RevoluteJoint {
-    data: GenericJoint,
+    /// The underlying joint data.
+    pub data: GenericJoint,
 }
 
 #[cfg(feature = "dim2")]
@@ -37,11 +44,6 @@ impl RevoluteJoint {
         Self { data }
     }
 
-    /// The underlying generic joint.
-    pub fn data(&self) -> &GenericJoint {
-        &self.data
-    }
-
     /// Are contacts between the attached rigid-bodies enabled?
     pub fn contacts_enabled(&self) -> bool {
         self.data.contacts_enabled()
@@ -138,6 +140,41 @@ impl RevoluteJoint {
         self.data.set_limits(JointAxis::AngX, limits);
         self
     }
+
+    /// The angle along the free degree of freedom of this revolute joint in `[-π, π]`.
+    ///
+    /// See also [`Self::angle`] for a version of this method taking entities instead of rigid-body handles.
+    /// Similarly [`RapierContext::impulse_revolute_joint_angle`] only takes a single entity as argument to compute that angle.
+    ///
+    /// # Parameters
+    /// - `bodies` : the rigid body set from [`RapierContext`]
+    /// - `body1`: the first rigid-body attached to this revolute joint, obtained through [`rapier::dynamics::ImpulseJoint`] or [`rapier::dynamics::MultibodyJoint`].
+    /// - `body2`: the second rigid-body attached to this revolute joint, obtained through [`rapier::dynamics::ImpulseJoint`] or [`rapier::dynamics::MultibodyJoint`].
+    pub fn angle_from_handles(
+        &self,
+        bodies: &RigidBodySet,
+        body1: RigidBodyHandle,
+        body2: RigidBodyHandle,
+    ) -> f32 {
+        // NOTE: unwrap will always succeed since `Self` is known to be a revolute joint.
+        let joint = self.data.raw.as_revolute().unwrap();
+
+        let rb1 = &bodies[body1];
+        let rb2 = &bodies[body2];
+        joint.angle(rb1.rotation(), rb2.rotation())
+    }
+
+    /// The angle along the free degree of freedom of this revolute joint in `[-π, π]`.
+    ///
+    /// # Parameters
+    /// - `bodies` : the rigid body set from [`super::super::RapierContext`]
+    /// - `body1`: the first rigid-body attached to this revolute joint.
+    /// - `body2`: the second rigid-body attached to this revolute joint.
+    pub fn angle(&self, context: &RapierContext, body1: Entity, body2: Entity) -> f32 {
+        let rb1 = context.entity2body().get(&body1).unwrap();
+        let rb2 = context.entity2body().get(&body2).unwrap();
+        self.angle_from_handles(&context.bodies, *rb1, *rb2)
+    }
 }
 
 impl From<RevoluteJoint> for GenericJoint {
@@ -244,8 +281,14 @@ impl RevoluteJointBuilder {
     }
 }
 
-impl From<RevoluteJointBuilder> for GenericJoint {
-    fn from(joint: RevoluteJointBuilder) -> GenericJoint {
+impl From<RevoluteJointBuilder> for TypedJoint {
+    fn from(joint: RevoluteJointBuilder) -> TypedJoint {
         joint.0.into()
     }
 }
+
+impl From<RevoluteJoint> for TypedJoint {
+    fn from(joint: RevoluteJoint) -> TypedJoint {
+        TypedJoint::RevoluteJoint(joint)
+    }
+}

src/dynamics/rope_joint.rs

@@ -2,12 +2,15 @@ use crate::dynamics::{GenericJoint, GenericJointBuilder};
 use crate::math::{Real, Vect};
 use rapier::dynamics::{JointAxesMask, JointAxis, JointLimits, JointMotor, MotorModel};
 
+use super::TypedJoint;
+
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[derive(Copy, Clone, Debug, PartialEq)]
 #[repr(transparent)]
 /// A rope joint, limits the maximum distance between two bodies
 pub struct RopeJoint {
-    data: GenericJoint,
+    /// The underlying joint data.
+    pub data: GenericJoint,
 }
 
 impl RopeJoint {
@@ -21,11 +24,6 @@ impl RopeJoint {
         result
     }
 
-    /// The underlying generic joint.
-    pub fn data(&self) -> &GenericJoint {
-        &self.data
-    }
-
     /// Are contacts between the attached rigid-bodies enabled?
     pub fn contacts_enabled(&self) -> bool {
         self.data.contacts_enabled()
@@ -262,8 +260,14 @@ impl RopeJointBuilder {
     }
 }
 
-impl From<RopeJointBuilder> for GenericJoint {
-    fn from(joint: RopeJointBuilder) -> GenericJoint {
+impl From<RopeJointBuilder> for TypedJoint {
+    fn from(joint: RopeJointBuilder) -> TypedJoint {
         joint.0.into()
     }
 }
+
+impl From<RopeJoint> for TypedJoint {
+    fn from(joint: RopeJoint) -> TypedJoint {
+        TypedJoint::RopeJoint(joint)
+    }
+}