feat: add RevoluteJoint::angle to compute the revolute joint’s angle

3 files changed, 92 insertions, 10 deletions

diff --git a/CHANGELOG.md b/CHANGELOG.md
index eb6a330..dcd0ae9 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -23,6 +23,7 @@ This release introduces two new crates:
   OBB), a convex hull, or a convex decomposition.
 - Implement `Default` for `RigidBodyBuilder`. This is equivalent to `RigidBodyBuilder::dynamic()`.
 - Implement `Default` for `ColliderBuilder`. This is equivalent to `ColliderBuilder::ball(0.5)`.
+- Add `RevoluteJoint::angle` to compute the joint’s angle given the rotation of its attached rigid-bodies.
 
 ### Modified
 
diff --git a/examples3d/joint_motor_position3.rs b/examples3d/joint_motor_position3.rs
index 0a15e63..f89e880 100644
--- a/examples3d/joint_motor_position3.rs
+++ b/examples3d/joint_motor_position3.rs
@@ -15,6 +15,7 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let rigid_body = RigidBodyBuilder::fixed();
     let ground_handle = bodies.insert(rigid_body);
+    let mut target_angles = vec![];
 
     /*
      * A rectangle on a motor with target position.
@@ -28,15 +29,13 @@ pub fn init_world(testbed: &mut Testbed) {
         let collider = ColliderBuilder::cuboid(0.1, 0.5, 0.1);
         colliders.insert_with_parent(collider, handle, &mut bodies);
 
+        let target_angle = -std::f32::consts::PI + std::f32::consts::PI / 4.0 * num as f32;
         let joint = RevoluteJointBuilder::new(Vector::z_axis())
             .local_anchor1(point![x_pos, 1.5, 0.0])
             .local_anchor2(point![0.0, -0.5, 0.0])
-            .motor_position(
-                -std::f32::consts::PI + std::f32::consts::PI / 4.0 * num as f32,
-                1000.0,
-                150.0,
-            );
+            .motor_position(target_angle, 1000.0, 150.0);
         impulse_joints.insert(ground_handle, handle, joint, true);
+        target_angles.push(target_angle);
     }
 
     /*
@@ -52,18 +51,31 @@ pub fn init_world(testbed: &mut Testbed) {
         let collider = ColliderBuilder::cuboid(0.1, 0.5, 0.1);
         colliders.insert_with_parent(collider, handle, &mut bodies);
 
+        let max_angle_limit = -std::f32::consts::PI + std::f32::consts::PI / 4.0 * num as f32;
         let joint = RevoluteJointBuilder::new(Vector::z_axis())
             .local_anchor1(point![x_pos, 5.0, 0.0])
             .local_anchor2(point![0.0, -0.5, 0.0])
             .motor_velocity(1.5, 30.0)
             .motor_max_force(100.0)
-            .limits([
-                -std::f32::consts::PI,
-                -std::f32::consts::PI + std::f32::consts::PI / 4.0 * num as f32,
-            ]);
+            .limits([-std::f32::consts::PI, max_angle_limit]);
         impulse_joints.insert(ground_handle, handle, joint, true);
+        target_angles.push(max_angle_limit);
     }
 
+    testbed.add_callback(move |_, physics, _, state| {
+        for ((_, joint), target) in physics.impulse_joints.iter().zip(target_angles.iter()) {
+            let rb1 = &physics.bodies[joint.body1];
+            let rb2 = &physics.bodies[joint.body2];
+            let revolute = joint.data.as_revolute().unwrap();
+            println!(
+                "[Step {}] rev angle: {} (target = {})",
+                state.timestep_id,
+                revolute.angle(rb1.rotation(), rb2.rotation()),
+                target
+            );
+        }
+    });
+
     /*
      * Set up the testbed.
      */
diff --git a/src/dynamics/joint/revolute_joint.rs b/src/dynamics/joint/revolute_joint.rs
index ba27351..4dfed5c 100644
--- a/src/dynamics/joint/revolute_joint.rs
+++ b/src/dynamics/joint/revolute_joint.rs
@@ -1,6 +1,6 @@
 use crate::dynamics::joint::{GenericJoint, GenericJointBuilder, JointAxesMask};
 use crate::dynamics::{JointAxis, JointLimits, JointMotor, MotorModel};
-use crate::math::{Point, Real};
+use crate::math::{Point, Real, Rotation};
 
 #[cfg(feature = "dim3")]
 use crate::math::UnitVector;
@@ -75,6 +75,29 @@ impl RevoluteJoint {
         self
     }
 
+    /// The angle along the free degree of freedom of this revolute joint in `[-π, π]`.
+    ///
+    /// # Parameters
+    /// - `rb_rot1`: the rotation of the first rigid-body attached to this revolute joint.
+    /// - `rb_rot2`: the rotation of the second rigid-body attached to this revolute joint.
+    pub fn angle(&self, rb_rot1: &Rotation<Real>, rb_rot2: &Rotation<Real>) -> Real {
+        let joint_rot1 = rb_rot1 * self.data.local_frame1.rotation;
+        let joint_rot2 = rb_rot2 * self.data.local_frame2.rotation;
+        let ang_err = joint_rot1.inverse() * joint_rot2;
+
+        #[cfg(feature = "dim3")]
+        if joint_rot1.dot(&joint_rot2) < 0.0 {
+            -ang_err.i.asin() * 2.0
+        } else {
+            ang_err.i.asin() * 2.0
+        }
+
+        #[cfg(feature = "dim2")]
+        {
+            ang_err.angle()
+        }
+    }
+
     /// The motor affecting the joint’s rotational degree of freedom.
     #[must_use]
     pub fn motor(&self) -> Option<&JointMotor> {
@@ -248,3 +271,49 @@ impl From<RevoluteJointBuilder> for GenericJoint {
         val.0.into()
     }
 }
+
+#[cfg(test)]
+mod test {
+    #[test]
+    fn test_revolute_joint_angle() {
+        use crate::math::{Real, Rotation};
+        use crate::na::RealField;
+        #[cfg(feature = "dim3")]
+        use crate::{math::Vector, na::vector};
+
+        #[cfg(feature = "dim2")]
+        let revolute = super::RevoluteJointBuilder::new().build();
+        #[cfg(feature = "dim2")]
+        let rot1 = Rotation::new(1.0);
+        #[cfg(feature = "dim3")]
+        let revolute = super::RevoluteJointBuilder::new(Vector::y_axis()).build();
+        #[cfg(feature = "dim3")]
+        let rot1 = Rotation::new(vector![0.0, 1.0, 0.0]);
+
+        let steps = 100;
+
+        // The -pi and pi values will be checked later.
+        for i in 1..steps {
+            let delta = -Real::pi() + i as Real * Real::two_pi() / steps as Real;
+            #[cfg(feature = "dim2")]
+            let rot2 = Rotation::new(1.0 + delta);
+            #[cfg(feature = "dim3")]
+            let rot2 = Rotation::new(vector![0.0, 1.0 + delta, 0.0]);
+            approx::assert_relative_eq!(revolute.angle(&rot1, &rot2), delta, epsilon = 1.0e-5);
+        }
+
+        // Check the special case for -pi and pi that may return an angle with a flipped sign
+        // (because they are equivalent).
+        for delta in [-Real::pi(), Real::pi()] {
+            #[cfg(feature = "dim2")]
+            let rot2 = Rotation::new(1.0 + delta);
+            #[cfg(feature = "dim3")]
+            let rot2 = Rotation::new(vector![0.0, 1.0 + delta, 0.0]);
+            approx::assert_relative_eq!(
+                revolute.angle(&rot1, &rot2).abs(),
+                delta.abs(),
+                epsilon = 1.0e-2
+            );
+        }
+    }
+}