Adds Rotation bindings and Rust API.

Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>

maliput-sys/src/api/api.h

@@ -121,5 +121,37 @@ std::unique_ptr<RoadPositionResult> RoadGeometry_ToRoadPosition(const RoadGeomet
   return std::make_unique<RoadPositionResult>(road_geometry.ToRoadPosition(inertial_pos));
 }
 
+std::unique_ptr<Rotation> Rotation_new() {
+  return std::make_unique<Rotation>();
+}
+
+std::unique_ptr<Rotation> Rotation_from_quat(const maliput::math::Quaternion& q) {
+  return std::make_unique<Rotation>(Rotation::FromQuat(q));
+}
+
+std::unique_ptr<Rotation> Rotation_from_rpy(const maliput::math::RollPitchYaw& rpy) {
+  return std::make_unique<Rotation>(Rotation::FromRpy(rpy.vector()));
+}
+
+void Rotation_set_quat(Rotation& rotation, const maliput::math::Quaternion& q) {
+  rotation.set_quat(q);
+}
+
+std::unique_ptr<maliput::math::RollPitchYaw> Rotation_rpy(const Rotation& rotation) {
+  return std::make_unique<maliput::math::RollPitchYaw>(rotation.rpy());
+}
+
+std::unique_ptr<maliput::math::Matrix3> Rotation_matrix(const Rotation& rotation) {
+  return std::make_unique<maliput::math::Matrix3>(rotation.matrix());
+}
+
+std::unique_ptr<InertialPosition> Rotation_Apply(const Rotation& rotation, const InertialPosition& inertial_pos) {
+  return std::make_unique<InertialPosition>(rotation.Apply(inertial_pos));
+}
+
+std::unique_ptr<Rotation> Rotation_Reverse(const Rotation& rotation) {
+  return std::make_unique<Rotation>(rotation.Reverse());
+}
+
 } // namespace api
 } // namespace maliput

maliput-sys/src/api/mod.rs

@@ -35,6 +35,12 @@ pub mod ffi {
 
         #[namespace = "maliput::math"]
         type Vector3 = crate::math::ffi::Vector3;
+        #[namespace = "maliput::math"]
+        type Quaternion = crate::math::ffi::Quaternion;
+        #[namespace = "maliput::math"]
+        type Matrix3 = crate::math::ffi::Matrix3;
+        #[namespace = "maliput::math"]
+        type RollPitchYaw = crate::math::ffi::RollPitchYaw;
 
         #[namespace = "maliput::api"]
         type RoadNetwork;
@@ -105,6 +111,23 @@ pub mod ffi {
         fn RoadPositionResult_road_position(result: &RoadPositionResult) -> UniquePtr<RoadPosition>;
         fn RoadPositionResult_nearest_position(result: &RoadPositionResult) -> UniquePtr<InertialPosition>;
         fn RoadPositionResult_distance(result: &RoadPositionResult) -> f64;
+
+        // Rotation bindings definitions
+        type Rotation;
+        fn Rotation_new() -> UniquePtr<Rotation>;
+        fn Rotation_from_quat(q: &Quaternion) -> UniquePtr<Rotation>;
+        fn Rotation_from_rpy(rpy: &RollPitchYaw) -> UniquePtr<Rotation>;
+        fn roll(self: &Rotation) -> f64;
+        fn pitch(self: &Rotation) -> f64;
+        fn yaw(self: &Rotation) -> f64;
+        fn quat(self: &Rotation) -> &Quaternion;
+        fn Distance(self: &Rotation, other: &Rotation) -> f64;
+        fn Rotation_set_quat(r: Pin<&mut Rotation>, q: &Quaternion);
+        fn Rotation_rpy(r: &Rotation) -> UniquePtr<RollPitchYaw>;
+        fn Rotation_matrix(r: &Rotation) -> UniquePtr<Matrix3>;
+        fn Rotation_Apply(r: &Rotation, ip: &InertialPosition) -> UniquePtr<InertialPosition>;
+        fn Rotation_Reverse(r: &Rotation) -> UniquePtr<Rotation>;
+
     }
     impl UniquePtr<RoadNetwork> {}
     impl UniquePtr<LanePosition> {}

maliput-sys/tests/api_tests.rs

@@ -185,4 +185,112 @@ mod api_test {
             assert_eq!(InertialPosition_to_str(&inertial_pos), "(x = 1, y = 2, z = 3)");
         }
     }
+    mod rotation_test {
+        use std::f64::consts::PI;
+
+        use maliput_sys::api::ffi::InertialPosition_new;
+        use maliput_sys::api::ffi::Rotation;
+        use maliput_sys::api::ffi::Rotation_Apply;
+        use maliput_sys::api::ffi::Rotation_Reverse;
+        use maliput_sys::api::ffi::Rotation_from_quat;
+        use maliput_sys::api::ffi::Rotation_from_rpy;
+        use maliput_sys::api::ffi::Rotation_matrix;
+        use maliput_sys::api::ffi::Rotation_new;
+        use maliput_sys::api::ffi::Rotation_set_quat;
+
+        use maliput_sys::math::ffi::Matrix3;
+        use maliput_sys::math::ffi::Matrix3_new;
+        use maliput_sys::math::ffi::Matrix3_row;
+        use maliput_sys::math::ffi::Quaternion_new;
+        use maliput_sys::math::ffi::RollPitchYaw_new;
+        use maliput_sys::math::ffi::Vector3_equals;
+
+        #[allow(clippy::too_many_arguments)]
+        fn check_all_rotation_accessors(
+            rotation: &Rotation,
+            roll: f64,
+            pitch: f64,
+            yaw: f64,
+            w: f64,
+            x: f64,
+            y: f64,
+            z: f64,
+            matrix: &Matrix3,
+        ) {
+            assert_eq!(rotation.roll(), roll);
+            assert_eq!(rotation.pitch(), pitch);
+            assert_eq!(rotation.yaw(), yaw);
+            assert_eq!(rotation.quat().w(), w);
+            assert_eq!(rotation.quat().x(), x);
+            assert_eq!(rotation.quat().y(), y);
+            assert_eq!(rotation.quat().z(), z);
+            let rot_matrix = Rotation_matrix(rotation);
+            assert!(Vector3_equals(&Matrix3_row(&rot_matrix, 0), &Matrix3_row(matrix, 0)));
+        }
+
+        #[test]
+        fn rotation_constructors() {
+            let rotation = Rotation_new();
+            check_all_rotation_accessors(
+                &rotation,
+                0.,
+                0.,
+                0.,
+                1.,
+                0.,
+                0.,
+                0.,
+                &Matrix3_new(1., 0., 0., 0., 1., 0., 0., 0., 1.),
+            );
+            let rpy = RollPitchYaw_new(1.0, 0.0, 0.0);
+            let rotation_from_rpy = Rotation_from_rpy(&rpy);
+            assert_eq!(rotation_from_rpy.roll(), 1.);
+            let quat = Quaternion_new(1.0, 0.0, 0.0, 0.0);
+            let rotation_from_quat = Rotation_from_quat(&quat);
+            assert_eq!(rotation_from_quat.roll(), 0.);
+        }
+
+        #[test]
+        fn rotation_set_quat() {
+            let mut rotation = Rotation_new();
+            let quat = Quaternion_new(1.0, 0.0, 0.0, 0.0);
+            Rotation_set_quat(rotation.pin_mut(), &quat);
+            check_all_rotation_accessors(
+                &rotation,
+                0.,
+                0.,
+                0.,
+                1.,
+                0.,
+                0.,
+                0.,
+                &Matrix3_new(1., 0., 0., 0., 1., 0., 0., 0., 1.),
+            );
+        }
+
+        #[test]
+        fn rotation_distance() {
+            let rotation1 = Rotation_new();
+            let rotation2 = Rotation_new();
+            assert_eq!(rotation1.Distance(&rotation2), 0.);
+        }
+
+        #[test]
+        fn rotation_apply() {
+            let rotation = Rotation_new();
+            let inertial_pos = InertialPosition_new(1., 0., 0.);
+            let rotated_vector = Rotation_Apply(&rotation, &inertial_pos);
+            assert_eq!(rotated_vector.x(), 1.);
+            assert_eq!(rotated_vector.y(), 0.);
+            assert_eq!(rotated_vector.z(), 0.);
+        }
+
+        #[test]
+        fn rotation_reverse() {
+            let tol = 1e-10;
+            let rotation = Rotation_new();
+            let reversed_rotation = Rotation_Reverse(&rotation);
+            assert!((reversed_rotation.yaw() - PI).abs() < tol);
+        }
+    }
 }

maliput/src/api/mod.rs

@@ -28,6 +28,11 @@
 // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
+use crate::math::Matrix3;
+use crate::math::Quaternion;
+use crate::math::RollPitchYaw;
+use crate::math::Vector3;
+
 /// A RoadGeometry.
 /// Wrapper around C++ implementation `maliput::api::RoadGeometry`.
 /// See RoadNetwork for an example of how to get a RoadGeometry.
@@ -137,8 +142,6 @@ pub struct LanePosition {
     lp: cxx::UniquePtr<maliput_sys::api::ffi::LanePosition>,
 }
 
-use crate::math::Vector3;
-
 impl LanePosition {
     /// Create a new `LanePosition` with the given `s`, `r`, and `h` components.
     pub fn new(s: f64, r: f64, h: f64) -> LanePosition {
@@ -445,6 +448,97 @@ impl RoadPositionResult {
     }
 }
 
+/// A maliput::api::Rotation
+/// A wrapper around C++ implementation `maliput::api::Rotation`.
+pub struct Rotation {
+    r: cxx::UniquePtr<maliput_sys::api::ffi::Rotation>,
+}
+
+impl Default for Rotation {
+    fn default() -> Self {
+        Self::new()
+    }
+}
+
+impl Rotation {
+    /// Create a new `Rotation`.
+    pub fn new() -> Rotation {
+        Rotation {
+            r: maliput_sys::api::ffi::Rotation_new(),
+        }
+    }
+    /// Create a new `Rotation` from a `Quaternion`.
+    pub fn from_quat(q: &Quaternion) -> Rotation {
+        let q_ffi = maliput_sys::math::ffi::Quaternion_new(q.w(), q.x(), q.y(), q.z());
+        Rotation {
+            r: maliput_sys::api::ffi::Rotation_from_quat(&q_ffi),
+        }
+    }
+    /// Create a new `Rotation` from a `RollPitchYaw`.
+    pub fn from_rpy(rpy: &RollPitchYaw) -> Rotation {
+        let rpy_ffi = maliput_sys::math::ffi::RollPitchYaw_new(rpy.roll_angle(), rpy.pitch_angle(), rpy.yaw_angle());
+        Rotation {
+            r: maliput_sys::api::ffi::Rotation_from_rpy(&rpy_ffi),
+        }
+    }
+    /// Get the roll of the `Rotation`.
+    pub fn roll(&self) -> f64 {
+        self.r.roll()
+    }
+    /// Get the pitch of the `Rotation`.
+    pub fn pitch(&self) -> f64 {
+        self.r.pitch()
+    }
+    /// Get the yaw of the `Rotation`.
+    pub fn yaw(&self) -> f64 {
+        self.r.yaw()
+    }
+    /// Get a quaternion representation of the `Rotation`.
+    pub fn quat(&self) -> Quaternion {
+        let q_ffi = self.r.quat();
+        Quaternion::new(q_ffi.w(), q_ffi.x(), q_ffi.y(), q_ffi.z())
+    }
+    /// Get a roll-pitch-yaw representation of the `Rotation`.
+    pub fn rpy(&self) -> RollPitchYaw {
+        let rpy_ffi = maliput_sys::api::ffi::Rotation_rpy(&self.r);
+        RollPitchYaw::new(rpy_ffi.roll_angle(), rpy_ffi.pitch_angle(), rpy_ffi.yaw_angle())
+    }
+    /// Set the `Rotation` from a `Quaternion`.
+    pub fn set_quat(&mut self, q: &Quaternion) {
+        let q_ffi = maliput_sys::math::ffi::Quaternion_new(q.w(), q.x(), q.y(), q.z());
+        maliput_sys::api::ffi::Rotation_set_quat(self.r.pin_mut(), &q_ffi);
+    }
+    /// Get the matrix representation of the `Rotation`.
+    pub fn matrix(&self) -> Matrix3 {
+        let matrix_ffi: cxx::UniquePtr<maliput_sys::math::ffi::Matrix3> =
+            maliput_sys::api::ffi::Rotation_matrix(&self.r);
+        let row_0 = maliput_sys::math::ffi::Matrix3_row(matrix_ffi.as_ref().expect(""), 0);
+        let row_1 = maliput_sys::math::ffi::Matrix3_row(matrix_ffi.as_ref().expect(""), 1);
+        let row_2 = maliput_sys::math::ffi::Matrix3_row(matrix_ffi.as_ref().expect(""), 2);
+        Matrix3::new(
+            Vector3::new(row_0.x(), row_0.y(), row_0.z()),
+            Vector3::new(row_1.x(), row_1.y(), row_1.z()),
+            Vector3::new(row_2.x(), row_2.y(), row_2.z()),
+        )
+    }
+    /// Get the distance between two `Rotation`.
+    pub fn distance(&self, other: &Rotation) -> f64 {
+        self.r.Distance(&other.r)
+    }
+    /// Apply the `Rotation` to an `InertialPosition`.
+    pub fn apply(&self, v: &InertialPosition) -> InertialPosition {
+        InertialPosition {
+            ip: maliput_sys::api::ffi::Rotation_Apply(&self.r, &v.ip),
+        }
+    }
+    /// Get the reverse of the `Rotation`.
+    pub fn reverse(&self) -> Rotation {
+        Rotation {
+            r: maliput_sys::api::ffi::Rotation_Reverse(&self.r),
+        }
+    }
+}
+
 mod tests {
     mod lane_position {
         #[test]
@@ -602,4 +696,50 @@ mod tests {
             assert_eq!(inertial_pos2.z(), 6.0);
         }
     }
+    mod rotation {
+        #[test]
+        fn rotation_new() {
+            let rotation = crate::api::Rotation::new();
+            assert_eq!(rotation.roll(), 0.0);
+            assert_eq!(rotation.pitch(), 0.0);
+            assert_eq!(rotation.yaw(), 0.0);
+        }
+
+        #[test]
+        fn from_quat() {
+            let quat = crate::math::Quaternion::new(1.0, 0.0, 0.0, 0.0);
+            let rotation = crate::api::Rotation::from_quat(&quat);
+            assert_eq!(rotation.roll(), 0.0);
+            assert_eq!(rotation.pitch(), 0.0);
+            assert_eq!(rotation.yaw(), 0.0);
+        }
+
+        #[test]
+        fn from_rpy() {
+            let rpy = crate::math::RollPitchYaw::new(0.0, 0.0, 0.0);
+            let rotation = crate::api::Rotation::from_rpy(&rpy);
+            assert_eq!(rotation.roll(), 0.0);
+            assert_eq!(rotation.pitch(), 0.0);
+            assert_eq!(rotation.yaw(), 0.0);
+        }
+
+        #[test]
+        fn set_quat() {
+            let mut rotation = crate::api::Rotation::new();
+            let quat = crate::math::Quaternion::new(1.0, 0.0, 0.0, 0.0);
+            rotation.set_quat(&quat);
+            assert_eq!(rotation.roll(), 0.0);
+            assert_eq!(rotation.pitch(), 0.0);
+            assert_eq!(rotation.yaw(), 0.0);
+        }
+
+        #[test]
+        fn matrix() {
+            let rotation = crate::api::Rotation::new();
+            let matrix = rotation.matrix();
+            assert_eq!(matrix.row(0), crate::math::Vector3::new(1.0, 0.0, 0.0));
+            assert_eq!(matrix.row(1), crate::math::Vector3::new(0.0, 1.0, 0.0));
+            assert_eq!(matrix.row(2), crate::math::Vector3::new(0.0, 0.0, 1.0));
+        }
+    }
 }