diff --git a/maliput-sys/src/api/api.h b/maliput-sys/src/api/api.h index 2feb52c..3230933 100644 --- a/maliput-sys/src/api/api.h +++ b/maliput-sys/src/api/api.h @@ -121,5 +121,37 @@ std::unique_ptr RoadGeometry_ToRoadPosition(const RoadGeomet return std::make_unique(road_geometry.ToRoadPosition(inertial_pos)); } +std::unique_ptr Rotation_new() { + return std::make_unique(); +} + +std::unique_ptr Rotation_from_quat(const maliput::math::Quaternion& q) { + return std::make_unique(Rotation::FromQuat(q)); +} + +std::unique_ptr Rotation_from_rpy(const maliput::math::RollPitchYaw& rpy) { + return std::make_unique(Rotation::FromRpy(rpy.vector())); +} + +void Rotation_set_quat(Rotation& rotation, const maliput::math::Quaternion& q) { + rotation.set_quat(q); +} + +std::unique_ptr Rotation_rpy(const Rotation& rotation) { + return std::make_unique(rotation.rpy()); +} + +std::unique_ptr Rotation_matrix(const Rotation& rotation) { + return std::make_unique(rotation.matrix()); +} + +std::unique_ptr Rotation_Apply(const Rotation& rotation, const InertialPosition& inertial_pos) { + return std::make_unique(rotation.Apply(inertial_pos)); +} + +std::unique_ptr Rotation_Reverse(const Rotation& rotation) { + return std::make_unique(rotation.Reverse()); +} + } // namespace api } // namespace maliput diff --git a/maliput-sys/src/api/mod.rs b/maliput-sys/src/api/mod.rs index cb6bc4f..55376c0 100644 --- a/maliput-sys/src/api/mod.rs +++ b/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; fn RoadPositionResult_nearest_position(result: &RoadPositionResult) -> UniquePtr; fn RoadPositionResult_distance(result: &RoadPositionResult) -> f64; + + // Rotation bindings definitions + type Rotation; + fn Rotation_new() -> UniquePtr; + fn Rotation_from_quat(q: &Quaternion) -> UniquePtr; + fn Rotation_from_rpy(rpy: &RollPitchYaw) -> UniquePtr; + 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; + fn Rotation_matrix(r: &Rotation) -> UniquePtr; + fn Rotation_Apply(r: &Rotation, ip: &InertialPosition) -> UniquePtr; + fn Rotation_Reverse(r: &Rotation) -> UniquePtr; + } impl UniquePtr {} impl UniquePtr {} diff --git a/maliput-sys/src/math/math.h b/maliput-sys/src/math/math.h index 017cd0f..f9e65a1 100644 --- a/maliput-sys/src/math/math.h +++ b/maliput-sys/src/math/math.h @@ -36,6 +36,7 @@ #include #include #include +#include #include @@ -203,5 +204,29 @@ rust::String Quaternion_to_str(const Quaternion& self) { return {ss.str()}; } +std::unique_ptr RollPitchYaw_new(rust::f64 roll, rust::f64 pitch, rust::f64 yaw) { + return std::make_unique(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 RollPitchYaw_ToQuaternion(const RollPitchYaw& self) { + return std::make_unique(self.ToQuaternion()); +} + +std::unique_ptr RollPitchYaw_ToMatrix(const RollPitchYaw& self) { + return std::make_unique(self.ToMatrix()); +} + +std::unique_ptr RollPitchYaw_CalcRotationMatrixDt(const RollPitchYaw& self, const Vector3& w) { + return std::make_unique(self.CalcRotationMatrixDt(w)); +} + } // namespace math } // namespace maliput diff --git a/maliput-sys/src/math/mod.rs b/maliput-sys/src/math/mod.rs index 5758018..728690e 100644 --- a/maliput-sys/src/math/mod.rs +++ b/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; + 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; + fn RollPitchYaw_ToMatrix(rpy: &RollPitchYaw) -> UniquePtr; + fn RollPitchYaw_CalcRotationMatrixDt(rpy: &RollPitchYaw, rpyDt: &Vector3) -> UniquePtr; } } diff --git a/maliput-sys/tests/api_tests.rs b/maliput-sys/tests/api_tests.rs index 14bb7f9..c2582e9 100644 --- a/maliput-sys/tests/api_tests.rs +++ b/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); + } + } } diff --git a/maliput-sys/tests/math_test.rs b/maliput-sys/tests/math_test.rs index 633bfba..40a69f6 100644 --- a/maliput-sys/tests/math_test.rs +++ b/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); + } } diff --git a/maliput/src/api/mod.rs b/maliput/src/api/mod.rs index 38c9521..8a062bf 100644 --- a/maliput/src/api/mod.rs +++ b/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, } -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, +} + +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::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)); + } + } } diff --git a/maliput/src/math/mod.rs b/maliput/src/math/mod.rs index f1afdbb..43e49b1 100644 --- a/maliput/src/math/mod.rs +++ b/maliput/src/math/mod.rs @@ -416,6 +416,91 @@ impl std::fmt::Debug for Quaternion { } } +/// This class represents the orientation between two arbitrary frames A and D +/// associated with a Space-fixed (extrinsic) X-Y-Z rotation by "roll-pitch-yaw" +/// angles `[r, p, y]`, which is equivalent to a Body-fixed (intrinsic) Z-Y-X +/// rotation by "yaw-pitch-roll" angles `[y, p, r]`. The rotation matrix `R_AD` +/// associated with this roll-pitch-yaw `[r, p, y]` rotation sequence is equal +/// to the matrix multiplication shown below. +/// ```md, no_run +/// ⎡cos(y) -sin(y) 0⎤ ⎡ cos(p) 0 sin(p)⎤ ⎡1 0 0 ⎤ +/// R_AD = ⎢sin(y) cos(y) 0⎥ * ⎢ 0 1 0 ⎥ * ⎢0 cos(r) -sin(r)⎥ +/// ⎣ 0 0 1⎦ ⎣-sin(p) 0 cos(p)⎦ ⎣0 sin(r) cos(r)⎦ +/// = R_AB * R_BC * R_CD +/// ``` +/// +/// Wrapper of C++ implementation `maliput::math::RollPitchYaw`. +pub struct RollPitchYaw { + rpy: cxx::UniquePtr, +} + +impl RollPitchYaw { + /// Create a new `RollPitchYaw` with the given `roll`, `pitch`, and `yaw` angles. + pub fn new(roll: f64, pitch: f64, yaw: f64) -> RollPitchYaw { + RollPitchYaw { + rpy: maliput_sys::math::ffi::RollPitchYaw_new(roll, pitch, yaw), + } + } + /// Get the roll angle of the `RollPitchYaw`. + pub fn roll_angle(&self) -> f64 { + self.rpy.roll_angle() + } + /// Get the pitch angle of the `RollPitchYaw`. + pub fn pitch_angle(&self) -> f64 { + self.rpy.pitch_angle() + } + /// Get the yaw angle of the `RollPitchYaw`. + pub fn yaw_angle(&self) -> f64 { + self.rpy.yaw_angle() + } + /// Get the vector of the `RollPitchYaw`. + pub fn vector(&self) -> Vector3 { + Vector3::new(self.rpy.vector().x(), self.rpy.vector().y(), self.rpy.vector().z()) + } + /// Set the `RollPitchYaw` with the given `roll`, `pitch`, and `yaw` angles. + pub fn set(&mut self, roll: f64, pitch: f64, yaw: f64) { + maliput_sys::math::ffi::RollPitchYaw_set(self.rpy.as_mut().expect("Unexpected Error"), roll, pitch, yaw); + } + /// Set the `RollPitchYaw` from the given `Quaternion`. + pub fn set_from_quaternion(&mut self, q: &Quaternion) { + maliput_sys::math::ffi::RollPitchYaw_SetFromQuaternion(self.rpy.as_mut().expect("Unexpected Error"), &q.q); + } + /// Get the `Quaternion` representation of the `RollPitchYaw`. + pub fn to_quaternion(&self) -> Quaternion { + Quaternion { + q: maliput_sys::math::ffi::RollPitchYaw_ToQuaternion(&self.rpy), + } + } + /// Get the rotation matrix representation of the `RollPitchYaw`. + pub fn to_matrix(&self) -> Matrix3 { + Matrix3 { + m: maliput_sys::math::ffi::RollPitchYaw_ToMatrix(&self.rpy), + } + } + /// Get the rotation matrix representation of the `RollPitchYaw` with the given `rpy_dt`. + /// ## Description + /// Forms Ṙ, the ordinary derivative of the RotationMatrix `R` with respect + /// to an independent variable `t` (`t` usually denotes time) and `R` is the + /// RotationMatrix formed by `this` `RollPitchYaw`. The roll-pitch-yaw angles + /// r, p, y are regarded as functions of `t` [i.e., r(t), p(t), y(t)]. + /// The param `rpy_dt` Ordinary derivative of rpy with respect to an independent + /// variable `t` (`t` usually denotes time, but not necessarily). + /// Returns `Ṙ``, the ordinary derivative of `R` with respect to `t`, calculated + /// as `Ṙ = ∂R/∂r * ṙ + ∂R/∂p * ṗ + ∂R/∂y * ẏ`. In other words, the returned + /// (i, j) element is `∂Rij/∂r * ṙ + ∂Rij/∂p * ṗ + ∂Rij/∂y * ẏ``. + /// + /// Note: Docs are taken from the C++ implementation. + /// ## Args + /// - `rpy_dt` Ordinary derivative of rpy with respect to an independent variable `t`. + /// ## Returns + /// Ordinary derivative of the RotationMatrix `R` with respect to an independent variable `t`. + pub fn calc_rotation_matrix_dt(&self, rpy_dt: &Vector3) -> Matrix3 { + Matrix3 { + m: maliput_sys::math::ffi::RollPitchYaw_CalcRotationMatrixDt(&self.rpy, &rpy_dt.v), + } + } +} + mod tests { #[test] fn vector3_new() { @@ -532,4 +617,13 @@ mod tests { assert_eq!(q.z(), 4.0); // TODO(francocipollone): Add tests for the rest of the API. } + + #[test] + fn roll_pitch_yaw_tests() { + let rpy = super::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); + // TODO(francocipollone): Add tests for the rest of the API. + } }