Adds RollPitchYaw and Rotation bindings and Rust API (#44)

Signed-off-by: Franco Cipollone <[email protected]>

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/src/math/math.h

@@ -36,6 +36,7 @@
 #include <maliput/math/vector.h>
 #include <maliput/math/matrix.h>
 #include <maliput/math/quaternion.h>
+#include <maliput/math/roll_pitch_yaw.h>
 
 #include <rust/cxx.h>
 
@@ -203,5 +204,29 @@ rust::String Quaternion_to_str(const Quaternion& self) {
   return {ss.str()};
 }
 
+std::unique_ptr<RollPitchYaw> RollPitchYaw_new(rust::f64 roll, rust::f64 pitch, rust::f64 yaw) {
+  return std::make_unique<RollPitchYaw>(roll, pitch, yaw);
+}
+
+void RollPitchYaw_set(RollPitchYaw& self, rust::f64 roll, rust::f64 pitch, rust::f64 yaw) {
+  self.set(roll, pitch, yaw);
+}
+
+void RollPitchYaw_SetFromQuaternion(RollPitchYaw& self, const Quaternion& q) {
+  self.SetFromQuaternion(q);
+}
+
+std::unique_ptr<Quaternion> RollPitchYaw_ToQuaternion(const RollPitchYaw& self) {
+  return std::make_unique<Quaternion>(self.ToQuaternion());
+}
+
+std::unique_ptr<Matrix3> RollPitchYaw_ToMatrix(const RollPitchYaw& self) {
+  return std::make_unique<Matrix3>(self.ToMatrix());
+}
+
+std::unique_ptr<Matrix3> RollPitchYaw_CalcRotationMatrixDt(const RollPitchYaw& self, const Vector3& w) {
+  return std::make_unique<Matrix3>(self.CalcRotationMatrixDt(w));
+}
+
 } // namespace math
 }  // namespace maliput

maliput-sys/src/math/mod.rs

@@ -107,5 +107,17 @@ pub mod ffi {
         fn Quaternion_IsApprox(q: &Quaternion, other: &Quaternion, precision: f64) -> bool;
         fn Quaternion_equals(q: &Quaternion, other: &Quaternion) -> bool;
         fn Quaternion_to_str(q: &Quaternion) -> String;
+
+        type RollPitchYaw;
+        fn RollPitchYaw_new(roll: f64, pitch: f64, yaw: f64) -> UniquePtr<RollPitchYaw>;
+        fn roll_angle(self: &RollPitchYaw) -> f64;
+        fn pitch_angle(self: &RollPitchYaw) -> f64;
+        fn yaw_angle(self: &RollPitchYaw) -> f64;
+        fn vector(self: &RollPitchYaw) -> &Vector3;
+        fn RollPitchYaw_set(rpy: Pin<&mut RollPitchYaw>, roll: f64, pitch: f64, yaw: f64);
+        fn RollPitchYaw_SetFromQuaternion(rpy: Pin<&mut RollPitchYaw>, q: &Quaternion);
+        fn RollPitchYaw_ToQuaternion(rpy: &RollPitchYaw) -> UniquePtr<Quaternion>;
+        fn RollPitchYaw_ToMatrix(rpy: &RollPitchYaw) -> UniquePtr<Matrix3>;
+        fn RollPitchYaw_CalcRotationMatrixDt(rpy: &RollPitchYaw, rpyDt: &Vector3) -> UniquePtr<Matrix3>;
     }
 }

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-sys/tests/math_test.rs

@@ -53,6 +53,13 @@ mod math_test {
     use maliput_sys::math::ffi::Matrix3_to_str;
     use maliput_sys::math::ffi::Matrix3_transpose;
 
+    use maliput_sys::math::ffi::RollPitchYaw_CalcRotationMatrixDt;
+    use maliput_sys::math::ffi::RollPitchYaw_SetFromQuaternion;
+    use maliput_sys::math::ffi::RollPitchYaw_ToMatrix;
+    use maliput_sys::math::ffi::RollPitchYaw_ToQuaternion;
+    use maliput_sys::math::ffi::RollPitchYaw_new;
+    use maliput_sys::math::ffi::RollPitchYaw_set;
+
     use maliput_sys::math::ffi::Quaternion_Inverse;
     use maliput_sys::math::ffi::Quaternion_IsApprox;
     use maliput_sys::math::ffi::Quaternion_ToRotationMatrix;
@@ -426,4 +433,69 @@ mod math_test {
         let q = Quaternion_new(1.0, 2.0, 3.0, 4.0);
         assert_eq!(Quaternion_to_str(&q), "(w: 1, x: 2, y: 3, z: 4)");
     }
+
+    #[test]
+    fn roll_pitch_yaw_new() {
+        let rpy = RollPitchYaw_new(1.0, 2.0, 3.0);
+        assert_eq!(rpy.roll_angle(), 1.0);
+        assert_eq!(rpy.pitch_angle(), 2.0);
+        assert_eq!(rpy.yaw_angle(), 3.0);
+    }
+
+    #[test]
+    fn roll_pitch_yaw_vector() {
+        let rpy = RollPitchYaw_new(1.0, 2.0, 3.0);
+        let v = rpy.vector();
+        assert_eq!(v.x(), 1.0);
+        assert_eq!(v.y(), 2.0);
+        assert_eq!(v.z(), 3.0);
+    }
+
+    #[test]
+    fn roll_pitch_yaw_set() {
+        let mut rpy = RollPitchYaw_new(1.0, 2.0, 3.0);
+        RollPitchYaw_set(rpy.as_mut().expect(""), 4.0, 5.0, 6.0);
+        assert_eq!(rpy.roll_angle(), 4.0);
+        assert_eq!(rpy.pitch_angle(), 5.0);
+        assert_eq!(rpy.yaw_angle(), 6.0);
+    }
+
+    #[test]
+    fn roll_pitch_yaw_set_from_quaternion() {
+        let q = Quaternion_new(1.0, 0.0, 0.0, 0.0);
+        let mut rpy = RollPitchYaw_new(1.0, 2.0, 3.0);
+        RollPitchYaw_SetFromQuaternion(rpy.as_mut().expect(""), &q);
+        assert_eq!(rpy.roll_angle(), 0.0);
+        assert_eq!(rpy.pitch_angle(), 0.0);
+        assert_eq!(rpy.yaw_angle(), 0.0);
+    }
+
+    #[test]
+    fn roll_pitch_yaw_to_quaternion() {
+        let rpy = RollPitchYaw_new(0.0, 0.0, 0.0);
+        let q = RollPitchYaw_ToQuaternion(&rpy);
+        assert_eq!(q.w(), 1.);
+        assert_eq!(q.x(), 0.);
+        assert_eq!(q.y(), 0.);
+        assert_eq!(q.z(), 0.);
+    }
+
+    #[test]
+    fn roll_pitch_yaw_to_matrix() {
+        let rpy = RollPitchYaw_new(0.0, 0.0, 0.0);
+        let m = RollPitchYaw_ToMatrix(&rpy);
+        assert_eq!(Matrix3_row(&m, 0).x(), 1.0);
+        assert_eq!(Matrix3_row(&m, 1).y(), 1.0);
+        assert_eq!(Matrix3_row(&m, 2).z(), 1.0);
+    }
+
+    #[test]
+    fn roll_pitch_yaw_calc_rotation_matrix_dt() {
+        let rpy = RollPitchYaw_new(0.0, 0.0, 0.0);
+        let v = Vector3_new(1.0, 1.0, 1.0);
+        let m = RollPitchYaw_CalcRotationMatrixDt(&rpy, &v);
+        assert_eq!(Matrix3_row(&m, 0).x(), 0.0);
+        assert_eq!(Matrix3_row(&m, 1).y(), 0.0);
+        assert_eq!(Matrix3_row(&m, 2).z(), 0.0);
+    }
 }