Adds Vector3, Matrix3, Quaternion, RollPitchYaw and api::Rotation bin…

…dings (#43)

* Adds Vector4 bindings
* Adds FFI bindings for Matrix3.
* Adds Rust API for Matrix3
* Adds FFI bindings for Quaternion class.
* Adds Rust API of Quaternion class.
* Adds RollPitchYaw and Rotation bindings and Rust API (#44)

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

maliput-sys/src/api/api.h

@@ -143,5 +143,37 @@ const std::vector<ConstLanePtr>& RoadGeometry_GetLanes(const RoadGeometry& road_
   return lanes;
 }
 
+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

@@ -41,6 +41,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;
@@ -113,6 +119,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

@@ -30,9 +30,14 @@
 
 #pragma once
 
+#include <initializer_list>
 #include <memory>
+#include <sstream>
 
 #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>
 
@@ -62,12 +67,6 @@ std::unique_ptr<Vector3> Vector3_new(rust::f64 x, rust::f64 y, rust::f64 z) {
   return std::make_unique<Vector3>(x, y, z);
 }
 
-/// Returns the norm of the vector.
-/// Forwards to maliput::math::Vector3::norm() method.
-rust::f64 Vector3_norm(const Vector3& self) {
-  return self.norm();
-}
-
 /// Returns the dot product of two vectors.
 /// Forwards to maliput::math::Vector3::dot(const Vector3& other) method.
 rust::f64 Vector3_dot(const Vector3& self, const Vector3& other) {
@@ -92,5 +91,143 @@ rust::String Vector3_to_str(const Vector3& self) {
   return {self.to_str()};
 }
 
+/// Creates a new maliput::math::Vector4.
+/// Forwads to maliput::math::Vector4(double x, double y, double z, double w) constructor.
+std::unique_ptr<Vector4> Vector4_new(rust::f64 x, rust::f64 y, rust::f64 z, rust::f64 w) {
+  return std::make_unique<Vector4>(x, y, z, w);
+}
+
+rust::f64 Vector4_dot(const Vector4& self, const Vector4& other) {
+  return self.dot(other);
+}
+
+bool Vector4_equals(const Vector4& self, const Vector4& other) {
+  return self == other;
+}
+
+rust::String Vector4_to_str(const Vector4& self) {
+  return {self.to_str()};
+}
+
+std::unique_ptr<Matrix3> Matrix3_new(double m00, double m01, double m02,
+                                     double m10, double m11, double m12,
+                                     double m20, double m21, double m22) {
+  return std::make_unique<Matrix3>(std::initializer_list<double>{m00, m01, m02, m10, m11, m12, m20, m21, m22});
+}
+
+std::unique_ptr<Vector3> Matrix3_row(const Matrix3& self, rust::usize index) {
+  return std::make_unique<Vector3>(self.row(index));
+}
+
+std::unique_ptr<Vector3> Matrix3_col(const Matrix3& self, rust::usize index) {
+  return std::make_unique<Vector3>(self.col(index));
+}
+
+std::unique_ptr<Matrix3> Matrix3_cofactor_matrix(const Matrix3& self) {
+  return std::make_unique<Matrix3>(self.cofactor());
+}
+
+std::unique_ptr<Matrix3> Matrix3_transpose(const Matrix3& self) {
+  return std::make_unique<Matrix3>(self.transpose());
+}
+
+std::unique_ptr<Matrix3> Matrix3_adjoint(const Matrix3& self) {
+  return std::make_unique<Matrix3>(self.adjoint());
+}
+
+std::unique_ptr<Matrix3> Matrix3_inverse(const Matrix3& self) {
+  return std::make_unique<Matrix3>(self.inverse());
+}
+
+bool Matrix3_equals(const Matrix3& self, const Matrix3& other) {
+  return self == other;
+}
+
+std::unique_ptr<Matrix3> Matrix3_operator_mul(const Matrix3& self, const Matrix3& other) {
+  return std::make_unique<Matrix3>(self * other);
+}
+
+std::unique_ptr<Matrix3> Matrix3_operator_sum(const Matrix3& self, const Matrix3& other) {
+  return std::make_unique<Matrix3>(self + other);
+}
+
+std::unique_ptr<Matrix3> Matrix3_operator_sub(const Matrix3& self, const Matrix3& other) {
+  return std::make_unique<Matrix3>(self - other);
+}
+
+std::unique_ptr<Matrix3> Matrix3_operator_divide_by_scalar(const Matrix3& self, rust::f64 scalar) {
+  return std::make_unique<Matrix3>(self / scalar);
+}
+
+rust::String Matrix3_to_str(const Matrix3& self) {
+  return {self.to_str()};
+}
+
+std::unique_ptr<Quaternion> Quaternion_new(rust::f64 w, rust::f64 x, rust::f64 y, rust::f64 z) {
+  return std::make_unique<Quaternion>(w, x, y, z);
+}
+
+std::unique_ptr<Vector3> Quaternion_vec(const Quaternion& self) {
+  return std::make_unique<Vector3>(self.vec());
+}
+
+std::unique_ptr<Vector4> Quaternion_coeffs(const Quaternion& self) {
+  return std::make_unique<Vector4>(self.coeffs());
+}
+
+std::unique_ptr<Quaternion> Quaternion_Inverse(const Quaternion& self) {
+  return std::make_unique<Quaternion>(self.Inverse());
+}
+
+std::unique_ptr<Quaternion> Quaternion_conjugate(const Quaternion& self) {
+  return std::make_unique<Quaternion>(self.conjugate());
+}
+
+std::unique_ptr<Matrix3> Quaternion_ToRotationMatrix(const Quaternion& self) {
+  return std::make_unique<Matrix3>(self.ToRotationMatrix());
+}
+
+std::unique_ptr<Vector3> Quaternion_TransformVector(const Quaternion& self, const Vector3& vec) {
+  return std::make_unique<Vector3>(self.TransformVector(vec));
+}
+
+bool Quaternion_IsApprox(const Quaternion& self, const Quaternion& other, rust::f64 precision) {
+  return self.IsApprox(other, precision);
+}
+
+bool Quaternion_equals(const Quaternion& self, const Quaternion& other) {
+  return self == other;
+}
+
+rust::String Quaternion_to_str(const Quaternion& self) {
+  std::stringstream ss;
+  ss << 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

@@ -39,12 +39,85 @@ pub mod ffi {
         fn x(self: &Vector3) -> f64;
         fn y(self: &Vector3) -> f64;
         fn z(self: &Vector3) -> f64;
-
-        fn Vector3_norm(v: &Vector3) -> f64;
+        fn norm(self: &Vector3) -> f64;
+        fn normalize(self: Pin<&mut Vector3>);
         fn Vector3_dot(v: &Vector3, w: &Vector3) -> f64;
         fn Vector3_cross(v: &Vector3, w: &Vector3) -> UniquePtr<Vector3>;
-
         fn Vector3_equals(v: &Vector3, w: &Vector3) -> bool;
         fn Vector3_to_str(v: &Vector3) -> String;
+
+        type Vector4;
+        fn Vector4_new(x: f64, y: f64, z: f64, w: f64) -> UniquePtr<Vector4>;
+        fn x(self: &Vector4) -> f64;
+        fn y(self: &Vector4) -> f64;
+        fn z(self: &Vector4) -> f64;
+        fn w(self: &Vector4) -> f64;
+        fn norm(self: &Vector4) -> f64;
+        fn normalize(self: Pin<&mut Vector4>);
+        fn Vector4_dot(v: &Vector4, w: &Vector4) -> f64;
+        fn Vector4_equals(v: &Vector4, w: &Vector4) -> bool;
+        fn Vector4_to_str(v: &Vector4) -> String;
+
+        type Matrix3;
+        #[allow(clippy::too_many_arguments)]
+        fn Matrix3_new(
+            m00: f64,
+            m01: f64,
+            m02: f64,
+            m10: f64,
+            m11: f64,
+            m12: f64,
+            m20: f64,
+            m21: f64,
+            m22: f64,
+        ) -> UniquePtr<Matrix3>;
+        fn cofactor(self: &Matrix3, row: u64, col: u64) -> f64;
+        fn determinant(self: &Matrix3) -> f64;
+        fn is_singular(self: &Matrix3) -> bool;
+        fn Matrix3_row(m: &Matrix3, index: u64) -> UniquePtr<Vector3>;
+        fn Matrix3_col(m: &Matrix3, index: u64) -> UniquePtr<Vector3>;
+        fn Matrix3_cofactor_matrix(m: &Matrix3) -> UniquePtr<Matrix3>;
+        fn Matrix3_transpose(m: &Matrix3) -> UniquePtr<Matrix3>;
+        fn Matrix3_equals(m: &Matrix3, other: &Matrix3) -> bool;
+        fn Matrix3_adjoint(m: &Matrix3) -> UniquePtr<Matrix3>;
+        fn Matrix3_operator_mul(m: &Matrix3, other: &Matrix3) -> UniquePtr<Matrix3>;
+        fn Matrix3_inverse(m: &Matrix3) -> UniquePtr<Matrix3>;
+        fn Matrix3_operator_sum(m: &Matrix3, other: &Matrix3) -> UniquePtr<Matrix3>;
+        fn Matrix3_operator_sub(m: &Matrix3, other: &Matrix3) -> UniquePtr<Matrix3>;
+        fn Matrix3_operator_divide_by_scalar(m: &Matrix3, scalar: f64) -> UniquePtr<Matrix3>;
+        fn Matrix3_to_str(m: &Matrix3) -> String;
+
+        type Quaternion;
+        fn Quaternion_new(w: f64, x: f64, y: f64, z: f64) -> UniquePtr<Quaternion>;
+        fn w(self: &Quaternion) -> f64;
+        fn x(self: &Quaternion) -> f64;
+        fn y(self: &Quaternion) -> f64;
+        fn z(self: &Quaternion) -> f64;
+        fn dot(self: &Quaternion, other: &Quaternion) -> f64;
+        fn AngularDistance(self: &Quaternion, other: &Quaternion) -> f64;
+        fn norm(self: &Quaternion) -> f64;
+        fn normalize(self: Pin<&mut Quaternion>);
+        fn squared_norm(self: &Quaternion) -> f64;
+        fn Quaternion_vec(q: &Quaternion) -> UniquePtr<Vector3>;
+        fn Quaternion_coeffs(q: &Quaternion) -> UniquePtr<Vector4>;
+        fn Quaternion_Inverse(q: &Quaternion) -> UniquePtr<Quaternion>;
+        fn Quaternion_conjugate(q: &Quaternion) -> UniquePtr<Quaternion>;
+        fn Quaternion_ToRotationMatrix(q: &Quaternion) -> UniquePtr<Matrix3>;
+        fn Quaternion_TransformVector(q: &Quaternion, v: &Vector3) -> UniquePtr<Vector3>;
+        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>;
     }
 }