From 94001b69334960200d5863429033e3462ba5225f Mon Sep 17 00:00:00 2001 From: jckras <99275379+jckras@users.noreply.github.com> Date: Mon, 10 Jul 2023 16:53:50 -0400 Subject: [PATCH] RSDK-3833 Dial takes type and payload as parameters (#59) --- examples/src/ffi/cpp/ffi_echo.cc | 2 +- examples/src/ffi/cpp/ffi_robot.cc | 4 +- src/ffi/dial_ffi.rs | 60 +++--- src/ffi/spatialmath/axis_angle.rs | 24 +-- src/ffi/spatialmath/euler_angles.rs | 32 ++-- src/ffi/spatialmath/mod.rs | 2 +- src/ffi/spatialmath/quaternion.rs | 242 +++++++++++++----------- src/ffi/spatialmath/rotation_matrix.rs | 28 +-- src/ffi/spatialmath/vector3.rs | 94 +++++----- src/spatialmath/utils.rs | 243 +++++++++++-------------- 10 files changed, 380 insertions(+), 351 deletions(-) diff --git a/examples/src/ffi/cpp/ffi_echo.cc b/examples/src/ffi/cpp/ffi_echo.cc index c958a6d..a104904 100644 --- a/examples/src/ffi/cpp/ffi_echo.cc +++ b/examples/src/ffi/cpp/ffi_echo.cc @@ -19,7 +19,7 @@ using proto::rpc::examples::echo::v1::EchoService; extern "C" void *init_rust_runtime(); extern "C" int free_rust_runtime(void *ptr); extern "C" void free_string(char* s); -extern "C" char *dial(const char *uri, const char *payload, +extern "C" char *dial(const char *uri, const char *type, const char *payload, bool allow_insecure, void *ptr); class EchoServiceClient { diff --git a/examples/src/ffi/cpp/ffi_robot.cc b/examples/src/ffi/cpp/ffi_robot.cc index a43e20f..ade33ea 100644 --- a/examples/src/ffi/cpp/ffi_robot.cc +++ b/examples/src/ffi/cpp/ffi_robot.cc @@ -19,7 +19,7 @@ using viam::robot::v1::ResourceNamesResponse; extern "C" void *init_rust_runtime(); extern "C" int free_rust_runtime(void *ptr); extern "C" void free_string(char* s); -extern "C" char *dial(const char *uri, const char *payload, +extern "C" char *dial(const char *uri, const char *type, const char *payload, bool allow_insecure, void *ptr); class RobotServiceClient { @@ -51,7 +51,7 @@ class RobotServiceClient { int main(int argc, char *argv[]) { void *ptr = init_rust_runtime(); - char *path = dial("", "", false, ptr); + char *path = dial("", "", "", false, ptr); if(path == NULL){ free_rust_runtime(ptr); return 1; diff --git a/src/ffi/dial_ffi.rs b/src/ffi/dial_ffi.rs index afd79e2..7f8e2bb 100644 --- a/src/ffi/dial_ffi.rs +++ b/src/ffi/dial_ffi.rs @@ -87,15 +87,12 @@ fn dial_without_cred( fn dial_with_cred( uri: String, + r#type: &str, payload: &str, allow_insec: bool, disable_webrtc: bool, ) -> Result> { - let creds = RPCCredentials::new( - None, - String::from("robot-location-secret"), - String::from(payload), - ); + let creds = RPCCredentials::new(None, String::from(r#type), String::from(payload)); let c = DialOptions::builder().uri(&uri).with_credentials(creds); let c = if disable_webrtc { c.disable_webrtc() @@ -113,12 +110,14 @@ fn dial_with_cred( /// When falling to dial it will return a NULL pointer /// # Arguments /// * `c_uri` a C-style string representing the address of robot you want to connect to +/// * `c_type` a C-style string representing the type of robot's secret you want to use, set to NULL if you don't need authentication /// * `c_payload` a C-style string that is the robot's secret, set to NULL if you don't need authentication /// * `c_allow_insecure` a bool, set to true when allowing insecure connection to your robot /// * `rt_ptr` a pointer to a rust runtime previously obtained with init_rust_runtime #[no_mangle] pub unsafe extern "C" fn dial( c_uri: *const c_char, + c_type: *const c_char, c_payload: *const c_char, c_allow_insec: bool, rt_ptr: Option<&mut DialFfi>, @@ -130,19 +129,13 @@ pub unsafe extern "C" fn dial( let ur = match Uri::from_maybe_shared(CStr::from_ptr(c_uri).to_bytes()) { Ok(ur) => ur, Err(e) => { - println!("Sorry {e:?} is not a valid URI"); + log::error!("Sorry {e:?} is not a valid URI"); return ptr::null_mut(); } }; ur }; let allow_insec = c_allow_insec; - let payload = { - match c_payload.is_null() { - true => None, - false => Some(CStr::from_ptr(c_payload)), - } - }; let ctx = match rt_ptr { Some(rt) => rt, None => { @@ -158,19 +151,20 @@ pub unsafe extern "C" fn dial( let conn = match runtime.block_on(async { proxy::uds::UDSConnector::new_random() }) { Ok(conn) => conn, Err(e) => { - println!("Error creating the UDS proxy {e:?}"); + log::error!("Error creating the UDS proxy {e:?}"); return ptr::null_mut(); } }; let path = match CString::new(conn.get_path()) { Ok(s) => s, Err(e) => { - println!("Error getting the path {e:?}"); + log::error!("Error getting the path {e:?}"); return ptr::null_mut(); } }; let (tx, rx) = oneshot::channel::<()>(); let uri_str = uri.to_string(); + // if the uri is local then we can connect directly. let disable_webrtc; if let Some(host) = uri.host() { @@ -178,18 +172,38 @@ pub unsafe extern "C" fn dial( } else { disable_webrtc = uri_str.contains(".local") || uri_str.contains("localhost"); } + let r#type = { + match c_type.is_null() { + true => None, + false => Some(CStr::from_ptr(c_type)), + } + }; + let payload = { + match c_payload.is_null() { + true => None, + false => Some(CStr::from_ptr(c_payload)), + } + }; let (server, channel) = match runtime.block_on(async move { - let channel = match payload { - Some(p) => { - dial_with_cred(uri_str, p.to_str()?, allow_insec, disable_webrtc)? - .connect() - .await? + let channel = match (r#type, payload) { + (Some(t), Some(p)) => { + dial_with_cred( + uri_str, + t.to_str()?, + p.to_str()?, + allow_insec, + disable_webrtc, + )? + .connect() + .await } - None => { + (None, None) => { let c = dial_without_cred(uri_str, allow_insec, disable_webrtc)?; - c.connect().await? + c.connect().await } - }; + (None, Some(_)) => Err(anyhow::anyhow!("Error missing credential: type")), + (Some(_), None) => Err(anyhow::anyhow!("Error missing credential: payload")), + }?; let dial = channel.clone(); let g = GRPCProxy::new(dial, uri); let service = ServiceBuilder::new() @@ -211,7 +225,7 @@ pub unsafe extern "C" fn dial( }) { Ok(s) => s, Err(e) => { - println!("Error building GRPC proxy reason : {e:?}"); + log::error!("Error building GRPC proxy reason : {e:?}"); return ptr::null_mut(); } }; diff --git a/src/ffi/spatialmath/axis_angle.rs b/src/ffi/spatialmath/axis_angle.rs index 723f343..66b6d5a 100644 --- a/src/ffi/spatialmath/axis_angle.rs +++ b/src/ffi/spatialmath/axis_angle.rs @@ -1,5 +1,5 @@ use ffi_helpers::null_pointer_check; -use nalgebra::{Quaternion}; +use nalgebra::Quaternion; use crate::spatialmath::utils::AxisAngle; @@ -15,10 +15,10 @@ fn to_raw_pointer(aa: &AxisAngle) -> *mut AxisAngle { } /// Free memory at the address of the axis angle pointer. -/// +/// /// # Safety -/// -/// Outer processes that work with axis angles via the FFI interface MUST remember +/// +/// Outer processes that work with axis angles via the FFI interface MUST remember /// to call this function when finished with an axis angle instance #[no_mangle] pub unsafe extern "C" fn free_axis_angles_memory(ptr: *mut AxisAngle) { @@ -30,11 +30,11 @@ pub unsafe extern "C" fn free_axis_angles_memory(ptr: *mut AxisAngle) { /// Initialize axis angle from raw components and retrieve the C pointer /// to its address. -/// +/// /// # Safety -/// +/// /// When finished with the underlying axis angle initialized by this function -/// the caller must remember to free the axis angle memory using the +/// the caller must remember to free the axis angle memory using the /// free_axis_angles_memory FFI function #[no_mangle] pub extern "C" fn new_axis_angle(x: f64, y: f64, z: f64, theta: f64) -> *mut AxisAngle { @@ -46,21 +46,21 @@ pub extern "C" fn new_axis_angle(x: f64, y: f64, z: f64, theta: f64) -> *mut Axi /// and theta is the rotation about the axis in radians. A zero quaternion returns /// a zero axis angle. In the event of an error from the nalgebra crate, a zero /// axis angle is also returned. -/// +/// /// # Safety -/// +/// /// When finished with the underlying quaternion passed to this function -/// the caller must remember to free the quaternion memory using the +/// the caller must remember to free the quaternion memory using the /// free_quaternion_memory FFI function and the axis angle memory using /// the free_array_memory function #[no_mangle] pub unsafe extern "C" fn axis_angle_from_quaternion( - quat: *const Quaternion + quat: *const Quaternion, ) -> *mut AxisAngle { null_pointer_check!(quat); let axis_angle = match (*quat).try_into() { Ok(aa) => aa, - Err(_err) => AxisAngle::new(0.0, 0.0, 0.0, 0.0) + Err(_err) => AxisAngle::new(0.0, 0.0, 0.0, 0.0), }; to_raw_pointer(&axis_angle) } diff --git a/src/ffi/spatialmath/euler_angles.rs b/src/ffi/spatialmath/euler_angles.rs index 21e5ba6..e7b52fc 100644 --- a/src/ffi/spatialmath/euler_angles.rs +++ b/src/ffi/spatialmath/euler_angles.rs @@ -4,9 +4,9 @@ use nalgebra::Quaternion; use crate::spatialmath::utils::EulerAngles; /// The FFI interface for initializing euler angles. Our euler angles -/// follow the Tait-Bryan formalism and are applied in the Z-Y'-X" order +/// follow the Tait-Bryan formalism and are applied in the Z-Y'-X" order /// (where Z -> yaw, Y -> pitch, X -> roll). -/// +/// /// It is highly recommended not to attempt any mathematics with the euler /// angles directly and to convert to quaternions via the FFI interface instead @@ -16,11 +16,11 @@ fn to_raw_pointer(ea: &EulerAngles) -> *mut EulerAngles { Box::into_raw(Box::new(*ea)) } -/// Free memory at the address of the euler angles pointer. -/// +/// Free memory at the address of the euler angles pointer. +/// /// # Safety -/// -/// Outer processes that work with EulerAngles via the FFI interface MUST remember +/// +/// Outer processes that work with EulerAngles via the FFI interface MUST remember /// to call this function when finished with a euler angles instance #[no_mangle] pub unsafe extern "C" fn free_euler_angles_memory(ptr: *mut EulerAngles) { @@ -32,29 +32,31 @@ pub unsafe extern "C" fn free_euler_angles_memory(ptr: *mut EulerAngles) { /// Initialize euler angles from raw components and retrieve the C pointer /// to its address. -/// +/// /// # Safety -/// +/// /// When finished with the underlying euler angles initialized by this function -/// the caller must remember to free the euler angles memory using the +/// the caller must remember to free the euler angles memory using the /// free_euler_angles_memory FFI function #[no_mangle] pub extern "C" fn new_euler_angles(roll: f64, pitch: f64, yaw: f64) -> *mut EulerAngles { to_raw_pointer(&EulerAngles::new(roll, pitch, yaw)) } -/// Converts a quaternion into euler angles (in radians). The euler angles are -/// represented according to the Tait-Bryan formalism and applied +/// Converts a quaternion into euler angles (in radians). The euler angles are +/// represented according to the Tait-Bryan formalism and applied /// in the Z-Y'-X" order (where Z -> yaw, Y -> pitch, X -> roll). -/// +/// /// # Safety -/// +/// /// When finished with the underlying quaternion passed to this function -/// the caller must remember to free the quaternion memory using the +/// the caller must remember to free the quaternion memory using the /// free_quaternion_memory FFI function and the euler angles memory using /// the free_euler_angles_memory function #[no_mangle] -pub unsafe extern "C" fn euler_angles_from_quaternion(quat_ptr: *const Quaternion) -> *mut EulerAngles { +pub unsafe extern "C" fn euler_angles_from_quaternion( + quat_ptr: *const Quaternion, +) -> *mut EulerAngles { null_pointer_check!(quat_ptr); let euler_angles: EulerAngles = (*quat_ptr).into(); to_raw_pointer(&euler_angles) diff --git a/src/ffi/spatialmath/mod.rs b/src/ffi/spatialmath/mod.rs index e0cb7a3..d50b5e1 100644 --- a/src/ffi/spatialmath/mod.rs +++ b/src/ffi/spatialmath/mod.rs @@ -1,6 +1,6 @@ pub mod axis_angle; pub mod euler_angles; pub mod orientation_vector; -pub mod rotation_matrix; pub mod quaternion; +pub mod rotation_matrix; pub mod vector3; diff --git a/src/ffi/spatialmath/quaternion.rs b/src/ffi/spatialmath/quaternion.rs index 4db4513..fe1f148 100644 --- a/src/ffi/spatialmath/quaternion.rs +++ b/src/ffi/spatialmath/quaternion.rs @@ -1,11 +1,14 @@ use ffi_helpers::null_pointer_check; use libc::c_double; -use nalgebra::{Quaternion, Vector3, UnitQuaternion, Normed, UnitVector3, Rotation3}; +use nalgebra::{Normed, Quaternion, Rotation3, UnitQuaternion, UnitVector3, Vector3}; -use crate::{ffi::spatialmath::vector3::to_raw_pointer as vec_to_raw_pointer, spatialmath::utils::{OrientationVector, rotate_vector_by_quaternion}}; +use crate::{ + ffi::spatialmath::vector3::to_raw_pointer as vec_to_raw_pointer, + spatialmath::utils::{rotate_vector_by_quaternion, OrientationVector}, +}; -/// The FFI interface wrapper around the nalgebra crate for Quaternion functions -/// and initialization. All public functions are meant to be called externally +/// The FFI interface wrapper around the nalgebra crate for Quaternion functions +/// and initialization. All public functions are meant to be called externally /// from other languages. Quaternions /// use the Real-I-J-K standard, so quaternions in other standards should be /// converted in the native language before being used to initialize quaternions @@ -19,11 +22,11 @@ fn to_raw_pointer(quat: &Quaternion) -> *mut Quaternion { /// Initialize a quaternion from raw components and retrieve the C pointer /// to its address. -/// +/// /// # Safety -/// +/// /// When finished with the underlying quaternion initialized by this function -/// the caller must remember to free the quaternion memory using the +/// the caller must remember to free the quaternion memory using the /// free_quaternion_memory FFI function #[no_mangle] pub extern "C" fn new_quaternion(real: f64, i: f64, j: f64, k: f64) -> *mut Quaternion { @@ -32,25 +35,31 @@ pub extern "C" fn new_quaternion(real: f64, i: f64, j: f64, k: f64) -> *mut Quat /// Initialize a quaternion from a real part and a C pointer to a Vector3 /// and retrieve the C pointer to its address. -/// +/// /// # Safety -/// +/// /// When finished with the underlying quaternion initialized by this function -/// the caller must remember to free the quaternion memory using the +/// the caller must remember to free the quaternion memory using the /// free_quaternion_memory FFI function #[no_mangle] pub unsafe extern "C" fn new_quaternion_from_vector( - real: f64, imag_ptr: *const Vector3 + real: f64, + imag_ptr: *const Vector3, ) -> *mut Quaternion { null_pointer_check!(imag_ptr); - to_raw_pointer(&Quaternion::new(real, (*imag_ptr).x, (*imag_ptr).y, (*imag_ptr).z)) + to_raw_pointer(&Quaternion::new( + real, + (*imag_ptr).x, + (*imag_ptr).y, + (*imag_ptr).z, + )) } -/// Free memory at the address of the quaternion pointer. -/// +/// Free memory at the address of the quaternion pointer. +/// /// # Safety -/// -/// Outer processes that work with Quaternions via the FFI interface MUST remember +/// +/// Outer processes that work with Quaternions via the FFI interface MUST remember /// to call this function when finished with a quaternion #[no_mangle] pub unsafe extern "C" fn free_quaternion_memory(ptr: *mut Quaternion) { @@ -62,26 +71,28 @@ pub unsafe extern "C" fn free_quaternion_memory(ptr: *mut Quaternion) { /// Get the components of a quaternion as a list of C doubles, the order of the /// components will be (real, i, j, k). -/// +/// /// # Safety -/// +/// /// When finished with the underlying quaternion passed to this function -/// the caller must remember to free the quaternion memory using the +/// the caller must remember to free the quaternion memory using the /// free_quaternion_memory FFI function #[no_mangle] -pub unsafe extern "C" fn quaternion_get_components(quat_ptr: *const Quaternion) -> *const c_double { +pub unsafe extern "C" fn quaternion_get_components( + quat_ptr: *const Quaternion, +) -> *const c_double { null_pointer_check!(quat_ptr); - let components: [c_double;4] = [(*quat_ptr).w, (*quat_ptr).i, (*quat_ptr).j, (*quat_ptr).k]; + let components: [c_double; 4] = [(*quat_ptr).w, (*quat_ptr).i, (*quat_ptr).j, (*quat_ptr).k]; Box::into_raw(Box::new(components)) as *const _ } /// Set the real component of an existing quaternion stored at the address /// of a pointer. -/// +/// /// # Safety -/// +/// /// When finished with the underlying quaternion passed to this function -/// the caller must remember to free the quaternion memory using the +/// the caller must remember to free the quaternion memory using the /// free_quaternion_memory FFI function #[no_mangle] pub unsafe extern "C" fn quaternion_set_real(quat_ptr: *mut Quaternion, real: f64) { @@ -91,11 +102,11 @@ pub unsafe extern "C" fn quaternion_set_real(quat_ptr: *mut Quaternion, rea /// Set the i component of an existing quaternion stored at the address /// of a pointer. -/// +/// /// # Safety -/// +/// /// When finished with the underlying quaternion passed to this function -/// the caller must remember to free the quaternion memory using the +/// the caller must remember to free the quaternion memory using the /// free_quaternion_memory FFI function #[no_mangle] pub unsafe extern "C" fn quaternion_set_i(quat_ptr: *mut Quaternion, i: f64) { @@ -105,11 +116,11 @@ pub unsafe extern "C" fn quaternion_set_i(quat_ptr: *mut Quaternion, i: f64 /// Set the j component of an existing quaternion stored at the address /// of a pointer. -/// +/// /// # Safety -/// +/// /// When finished with the underlying quaternion passed to this function -/// the caller must remember to free the quaternion memory using the +/// the caller must remember to free the quaternion memory using the /// free_quaternion_memory FFI function #[no_mangle] pub unsafe extern "C" fn quaternion_set_j(quat_ptr: *mut Quaternion, j: f64) { @@ -119,11 +130,11 @@ pub unsafe extern "C" fn quaternion_set_j(quat_ptr: *mut Quaternion, j: f64 /// Set the k component of an existing quaternion stored at the address /// of a pointer. -/// +/// /// # Safety -/// +/// /// When finished with the underlying quaternion passed to this function -/// the caller must remember to free the quaternion memory using the +/// the caller must remember to free the quaternion memory using the /// free_quaternion_memory FFI function #[no_mangle] pub unsafe extern "C" fn quaternion_set_k(quat_ptr: *mut Quaternion, k: f64) { @@ -133,15 +144,19 @@ pub unsafe extern "C" fn quaternion_set_k(quat_ptr: *mut Quaternion, k: f64 /// Set all of the components of an existing quaternion stored at the address /// of a pointer -/// +/// /// # Safety -/// +/// /// When finished with the underlying quaternion passed to this function -/// the caller must remember to free the quaternion memory using the +/// the caller must remember to free the quaternion memory using the /// free_quaternion_memory FFI function #[no_mangle] pub unsafe extern "C" fn quaternion_set_components( - quat_ptr: *mut Quaternion, real: f64, i: f64, j: f64, k: f64 + quat_ptr: *mut Quaternion, + real: f64, + i: f64, + j: f64, + k: f64, ) { null_pointer_check!(quat_ptr); (*quat_ptr).w = real; @@ -153,15 +168,18 @@ pub unsafe extern "C" fn quaternion_set_components( /// Set the imaginary components of an existing quaternion stored at /// the address of a pointer (quat_ptr) from the components of a 3-vector /// (stored at vec_ptr). The convention is x -> i, y -> j, z -> k -/// +/// /// # Safety -/// +/// /// When finished with the underlying quaternion passed to this function -/// the caller must remember to free the quaternion memory using the +/// the caller must remember to free the quaternion memory using the /// free_quaternion_memory FFI function (the same applies for the vector /// stored at vec_ptr) #[no_mangle] -pub unsafe extern "C" fn quaternion_set_imag_from_vector(quat_ptr: *mut Quaternion, vec_ptr: *const Vector3) { +pub unsafe extern "C" fn quaternion_set_imag_from_vector( + quat_ptr: *mut Quaternion, + vec_ptr: *const Vector3, +) { null_pointer_check!(quat_ptr); null_pointer_check!(vec_ptr); (*quat_ptr).i = (*vec_ptr).x; @@ -172,27 +190,29 @@ pub unsafe extern "C" fn quaternion_set_imag_from_vector(quat_ptr: *mut Quaterni /// Copies the imaginary components to a 3-vector (using x -> i, y -> j /// z -> k) and returns a pointer to the memory address of the resulting /// vector -/// +/// /// # Safety -/// +/// /// When finished with the underlying quaternion initialized by this function -/// the caller must remember to free the quaternion memory using the +/// the caller must remember to free the quaternion memory using the /// free_quaternion_memory FFI function #[no_mangle] -pub unsafe extern "C" fn quaternion_get_imaginary_vector(quat_ptr: *const Quaternion) -> *mut Vector3 { +pub unsafe extern "C" fn quaternion_get_imaginary_vector( + quat_ptr: *const Quaternion, +) -> *mut Vector3 { null_pointer_check!(quat_ptr); let imag = (*quat_ptr).vector(); let imag_vec = Vector3::new(imag[0], imag[1], imag[2]); vec_to_raw_pointer(imag_vec) } -/// Normalizes an existing quaternion stored at the address of +/// Normalizes an existing quaternion stored at the address of /// a pointer (quat_ptr) -/// +/// /// # Safety -/// +/// /// When finished with the underlying quaternion passed to this function -/// the caller must remember to free the quaternion memory using the +/// the caller must remember to free the quaternion memory using the /// free_quaternion_memory FFI function #[no_mangle] pub unsafe extern "C" fn normalize_quaternion(quat_ptr: *mut Quaternion) { @@ -203,28 +223,31 @@ pub unsafe extern "C" fn normalize_quaternion(quat_ptr: *mut Quaternion) { /// Initializes a normalized copy of a quaternion stored at the /// address of a pointer (quat_ptr) and returns a pointer to the /// memory of the result -/// +/// /// # Safety -/// -/// The caller must remember to free the quaternion memory of -/// *both* the input and output quaternions when finished with them +/// +/// The caller must remember to free the quaternion memory of +/// *both* the input and output quaternions when finished with them /// using the free_quaternion_memory FFI function #[no_mangle] -pub unsafe extern "C" fn quaternion_get_normalized(quat_ptr: *const Quaternion) -> *mut Quaternion { +pub unsafe extern "C" fn quaternion_get_normalized( + quat_ptr: *const Quaternion, +) -> *mut Quaternion { null_pointer_check!(quat_ptr); to_raw_pointer(&(*quat_ptr).normalize()) } /// Returns the result of rotating a vector by a quaternion -/// +/// /// # Safety -/// +/// /// The caller must remember to free the quaternion memory and /// the memory of both vectors when finished with them using the /// free_quaternion_memory and free_vector_memory FFI functions #[no_mangle] pub unsafe extern "C" fn quaternion_rotate_vector( - quat_ptr: *const Quaternion, vec_ptr: *const Vector3 + quat_ptr: *const Quaternion, + vec_ptr: *const Vector3, ) -> *mut Vector3 { null_pointer_check!(quat_ptr); null_pointer_check!(vec_ptr); @@ -235,14 +258,18 @@ pub unsafe extern "C" fn quaternion_rotate_vector( /// Converts from euler angles (in radians) to a quaternion. The euler angles are expected to /// be represented according to the Tait-Bryan formalism and applied in the Z-Y'-X" /// order (where Z -> yaw, Y -> pitch, X -> roll) -/// +/// /// # Safety -/// +/// /// When finished with the underlying quaternion initialized by this function -/// the caller must remember to free the quaternion memory using the +/// the caller must remember to free the quaternion memory using the /// free_quaternion_memory FFI function #[no_mangle] -pub unsafe extern "C" fn quaternion_from_euler_angles(roll: f64, pitch: f64, yaw: f64) -> *mut Quaternion { +pub unsafe extern "C" fn quaternion_from_euler_angles( + roll: f64, + pitch: f64, + yaw: f64, +) -> *mut Quaternion { let unit_quat = UnitQuaternion::from_euler_angles(roll, pitch, yaw); let quat = unit_quat.quaternion(); to_raw_pointer(&quat) @@ -250,15 +277,18 @@ pub unsafe extern "C" fn quaternion_from_euler_angles(roll: f64, pitch: f64, yaw /// Converts from an axis angle given by a vector's x, y, z components /// and a rotation theta (in radians) about the vector into a quaternion -/// +/// /// # Safety -/// +/// /// When finished with the underlying quaternion initialized by this function -/// the caller must remember to free the quaternion memory using the +/// the caller must remember to free the quaternion memory using the /// free_quaternion_memory FFI function #[no_mangle] pub unsafe extern "C" fn quaternion_from_axis_angle( - x: f64, y: f64, z: f64, theta: f64 + x: f64, + y: f64, + z: f64, + theta: f64, ) -> *mut Quaternion { let axis_angle_vec = Vector3::new(x, y, z); let axis_angle_vec_normed = UnitVector3::new_normalize(axis_angle_vec); @@ -266,19 +296,20 @@ pub unsafe extern "C" fn quaternion_from_axis_angle( to_raw_pointer(unit_quat.quaternion()) } -/// Converts from an axis angle whose vector is given by a pointer +/// Converts from an axis angle whose vector is given by a pointer /// to a nalgebra::Vector3 instance and a rotation theta (in radians) /// about the vector to a quaternion -/// +/// /// # Safety -/// +/// /// When finished with the underlying quaternion initialized by this function -/// the caller must remember to free the quaternion memory using the +/// the caller must remember to free the quaternion memory using the /// free_quaternion_memory FFI function. Similarly the free_vector_memory should /// be called when finished with the axis angle vector #[no_mangle] pub unsafe extern "C" fn quaternion_from_axis_angle_vector( - theta: f64, axis_angle_vec_ptr: *const Vector3 + theta: f64, + axis_angle_vec_ptr: *const Vector3, ) -> *mut Quaternion { null_pointer_check!(axis_angle_vec_ptr); let axis_angle_vec_normed = UnitVector3::new_normalize(*axis_angle_vec_ptr); @@ -287,15 +318,15 @@ pub unsafe extern "C" fn quaternion_from_axis_angle_vector( } /// Converts from a pointer to a Rotation3 to a quaternion -/// +/// /// # Safety -/// +/// /// When finished with the underlying quaternion initialized by this function -/// the caller must remember to free the quaternion memory using the +/// the caller must remember to free the quaternion memory using the /// free_quaternion_memory FFI function #[no_mangle] pub unsafe extern "C" fn quaternion_from_rotation_matrix( - rot_ptr: *const Rotation3 + rot_ptr: *const Rotation3, ) -> *mut Quaternion { null_pointer_check!(rot_ptr); let unit_quat = UnitQuaternion::from_rotation_matrix(&*rot_ptr); @@ -303,27 +334,27 @@ pub unsafe extern "C" fn quaternion_from_rotation_matrix( } /// Converts from a pointer to an OrientationVector to a quaternion -/// +/// /// # Safety -/// +/// /// When finished with the underlying quaternion initialized by this function -/// the caller must remember to free the quaternion memory using the +/// the caller must remember to free the quaternion memory using the /// free_quaternion_memory FFI function #[no_mangle] pub unsafe extern "C" fn quaternion_from_orientation_vector( - o_vec_ptr: *const OrientationVector + o_vec_ptr: *const OrientationVector, ) -> *mut Quaternion { null_pointer_check!(o_vec_ptr); to_raw_pointer(&(*o_vec_ptr).to_quaternion()) } -/// Scales an existing quaternion stored at the address of +/// Scales an existing quaternion stored at the address of /// a pointer (quat_ptr) by a factor (float) -/// +/// /// # Safety -/// +/// /// When finished with the underlying quaternion passed to this function -/// the caller must remember to free the quaternion memory using the +/// the caller must remember to free the quaternion memory using the /// free_quaternion_memory FFI function #[no_mangle] pub unsafe extern "C" fn scale_quaternion(quat_ptr: *mut Quaternion, factor: f64) { @@ -333,40 +364,45 @@ pub unsafe extern "C" fn scale_quaternion(quat_ptr: *mut Quaternion, factor /// Initializes a copy of the quaternion stored at the address of a pointer (quat_ptr) /// scaled by a factor (float) and returns a pointer to the memory of the result -/// +/// /// # Safety -/// -/// The caller must remember to free the quaternion memory of -/// *both* the input and output quaternions when finished with them +/// +/// The caller must remember to free the quaternion memory of +/// *both* the input and output quaternions when finished with them /// using the free_quaternion_memory FFI function #[no_mangle] -pub unsafe extern "C" fn quaternion_get_scaled(quat_ptr: *const Quaternion, factor: f64) -> *mut Quaternion { +pub unsafe extern "C" fn quaternion_get_scaled( + quat_ptr: *const Quaternion, + factor: f64, +) -> *mut Quaternion { null_pointer_check!(quat_ptr); let mut copy_quat = *quat_ptr; copy_quat.scale_mut(factor); to_raw_pointer(©_quat) } -/// Initializes a quaternion that is the conjugate of one stored -/// at the address of a pointer (quat_ptr)and returns a pointer +/// Initializes a quaternion that is the conjugate of one stored +/// at the address of a pointer (quat_ptr)and returns a pointer /// to the memory of the result -/// +/// /// # Safety -/// -/// The caller must remember to free the quaternion memory of -/// *both* the input and output quaternions when finished with them +/// +/// The caller must remember to free the quaternion memory of +/// *both* the input and output quaternions when finished with them /// using the free_quaternion_memory FFI function #[no_mangle] -pub unsafe extern "C" fn quaternion_get_conjugate(quat_ptr: *const Quaternion) -> *mut Quaternion { +pub unsafe extern "C" fn quaternion_get_conjugate( + quat_ptr: *const Quaternion, +) -> *mut Quaternion { null_pointer_check!(quat_ptr); to_raw_pointer(&(*quat_ptr).conjugate()) } -/// Adds two quaternions and returns a pointer to the +/// Adds two quaternions and returns a pointer to the /// memory of the result -/// +/// /// # Safety -/// +/// /// The caller must remember to free the quaternion memory of *both* the input and /// output quaternions when finished with them using the free_quaternion_memory FFI function #[no_mangle] @@ -379,11 +415,11 @@ pub unsafe extern "C" fn quaternion_add( to_raw_pointer(&((*quat_ptr_1) + (*quat_ptr_2))) } -/// Subtracts two quaternions and returns a pointer to the +/// Subtracts two quaternions and returns a pointer to the /// memory of the result -/// +/// /// # Safety -/// +/// /// The caller must remember to free the quaternion memory of *both* the input and /// output quaternions when finished with them using the free_quaternion_memory FFI function #[no_mangle] @@ -396,11 +432,11 @@ pub unsafe extern "C" fn quaternion_subtract( to_raw_pointer(&((*quat_ptr_1) - (*quat_ptr_2))) } -/// Computes the Hamiltonian product of two quaternions and +/// Computes the Hamiltonian product of two quaternions and /// returns a pointer to the memory of the result -/// +/// /// # Safety -/// +/// /// The caller must remember to free the quaternion memory of *both* the input and /// output quaternions when finished with them using the free_quaternion_memory FFI function #[no_mangle] diff --git a/src/ffi/spatialmath/rotation_matrix.rs b/src/ffi/spatialmath/rotation_matrix.rs index 75f0865..aff41d2 100644 --- a/src/ffi/spatialmath/rotation_matrix.rs +++ b/src/ffi/spatialmath/rotation_matrix.rs @@ -1,8 +1,8 @@ use ffi_helpers::null_pointer_check; -use nalgebra::{Rotation3, Quaternion, UnitQuaternion, Matrix3}; +use nalgebra::{Matrix3, Quaternion, Rotation3, UnitQuaternion}; -/// The FFI interface wrapper around the nalgebra crate for RotationMatrix functions -/// and initialization. All public functions are meant to be called externally +/// The FFI interface wrapper around the nalgebra crate for RotationMatrix functions +/// and initialization. All public functions are meant to be called externally /// from other languages. These are 3D rotations (so members of SO(3)) /// Allocates a copy of the rotation matrix to the heap with a stable memory address and @@ -12,9 +12,9 @@ fn to_raw_pointer(rot: &Rotation3) -> *mut Rotation3 { } /// Free memory at the address of the rotation matrix pointer. Outer processes -/// that work with RotationMatrices via the FFI interface MUST remember +/// that work with RotationMatrices via the FFI interface MUST remember /// to call this function when finished with a rotation matrix -/// +/// /// # Safety #[no_mangle] pub unsafe extern "C" fn free_rotation_matrix_memory(ptr: *mut Rotation3) { @@ -27,14 +27,14 @@ pub unsafe extern "C" fn free_rotation_matrix_memory(ptr: *mut Rotation3) { /// Initialize a 3D rotation matrix from raw components and retrieve the C pointer /// to its address. This function DOES NOT check whether the matrix elements provided /// form a valid member of SO(3) -/// +/// /// # Safety -/// +/// /// When finished with the underlying rotation matrix initialized by this function -/// the caller must remember to free the rotation matrix memory using the +/// the caller must remember to free the rotation matrix memory using the /// free_rotation_matrix_memory FFI function #[no_mangle] -pub unsafe extern "C" fn new_rotation_matrix(elements: *const [f64;9]) -> *mut Rotation3 { +pub unsafe extern "C" fn new_rotation_matrix(elements: *const [f64; 9]) -> *mut Rotation3 { null_pointer_check!(elements); let matrix = Matrix3::from_vec(Vec::from(*elements)); let rot = Rotation3::from_matrix_unchecked(matrix); @@ -43,15 +43,17 @@ pub unsafe extern "C" fn new_rotation_matrix(elements: *const [f64;9]) -> *mut R /// Converts a quaternion into a 3D rotation matrix (a Rotation /// from the nalgebra crate) -/// +/// /// # Safety -/// +/// /// When finished with the underlying quaternion passed to this function -/// the caller must remember to free the quaternion memory using the +/// the caller must remember to free the quaternion memory using the /// free_quaternion_memory FFI function and the rotation matrix memory using /// the free_rotation_matrix_memory function #[no_mangle] -pub unsafe extern "C" fn rotation_matrix_from_quaternion(quat: *const Quaternion) -> *mut Rotation3 { +pub unsafe extern "C" fn rotation_matrix_from_quaternion( + quat: *const Quaternion, +) -> *mut Rotation3 { null_pointer_check!(quat); let unit_quat = UnitQuaternion::new_normalize(*quat); let rot = unit_quat.to_rotation_matrix(); diff --git a/src/ffi/spatialmath/vector3.rs b/src/ffi/spatialmath/vector3.rs index 59e4ede..123ad01 100644 --- a/src/ffi/spatialmath/vector3.rs +++ b/src/ffi/spatialmath/vector3.rs @@ -3,9 +3,8 @@ use libc::c_double; use nalgebra::Vector3; - -/// The FFI interface wrapping the nalgebra crate for Vector functions and -/// initialization. All public functions are meant to be called externally +/// The FFI interface wrapping the nalgebra crate for Vector functions and +/// initialization. All public functions are meant to be called externally /// from other languages /// Allocates the vector to the heap with a stable memory address and @@ -16,11 +15,11 @@ pub(crate) fn to_raw_pointer(vec: Vector3) -> *mut Vector3 { /// Initialize a 3-vector from raw components and retrieve the C pointer /// to its address. -/// +/// /// # Safety -/// +/// /// When finished with the underlying vector initialized by this function -/// the caller must remember to free the vector memory using the +/// the caller must remember to free the vector memory using the /// free_vector_memory FFI function #[no_mangle] pub extern "C" fn new_vector3(x: f64, y: f64, z: f64) -> *mut Vector3 { @@ -28,10 +27,10 @@ pub extern "C" fn new_vector3(x: f64, y: f64, z: f64) -> *mut Vector3 { to_raw_pointer(new_vec) } -/// Free memory at the address of the vector pointer. -/// +/// Free memory at the address of the vector pointer. +/// /// # Safety -/// Outer processes that work with Vectors via the FFI interface MUST remember +/// Outer processes that work with Vectors via the FFI interface MUST remember /// to call this function when finished with a vector #[no_mangle] pub unsafe extern "C" fn free_vector_memory(ptr: *mut Vector3) { @@ -43,10 +42,10 @@ pub unsafe extern "C" fn free_vector_memory(ptr: *mut Vector3) { /// Get the components of a vector as a list of C doubles, the order of the /// components will be (x, y, z). -/// +/// /// # Safety -/// -/// When finished with the underlying vector, the caller must remember to +/// +/// When finished with the underlying vector, the caller must remember to /// free the vector memory using the free_vector_memory FFI function #[no_mangle] pub unsafe extern "C" fn vector_get_components(vec_ptr: *const Vector3) -> *const c_double { @@ -57,10 +56,10 @@ pub unsafe extern "C" fn vector_get_components(vec_ptr: *const Vector3) -> /// Set the x component of an existing vector stored at the address /// of a pointer. -/// +/// /// # Safety -/// -/// When finished with the underlying vector, the caller must remember to +/// +/// When finished with the underlying vector, the caller must remember to /// free the vector memory using the free_vector_memory FFI function #[no_mangle] pub unsafe extern "C" fn vector_set_x(vec_ptr: *mut Vector3, x_val: f64) { @@ -70,10 +69,10 @@ pub unsafe extern "C" fn vector_set_x(vec_ptr: *mut Vector3, x_val: f64) { /// Set the y component of an existing vector stored at the address /// of a pointer. -/// +/// /// # Safety -/// -/// When finished with the underlying vector, the caller must remember to +/// +/// When finished with the underlying vector, the caller must remember to /// free the vector memory using the free_vector_memory FFI function #[no_mangle] pub unsafe extern "C" fn vector_set_y(vec_ptr: *mut Vector3, y_val: f64) { @@ -83,10 +82,10 @@ pub unsafe extern "C" fn vector_set_y(vec_ptr: *mut Vector3, y_val: f64) { /// Set the z component of an existing vector stored at the address /// of a pointer. -/// +/// /// # Safety -/// -/// When finished with the underlying vector, the caller must remember to +/// +/// When finished with the underlying vector, the caller must remember to /// free the vector memory using the free_vector_memory FFI function #[no_mangle] pub unsafe extern "C" fn vector_set_z(vec_ptr: *mut Vector3, z_val: f64) { @@ -94,12 +93,12 @@ pub unsafe extern "C" fn vector_set_z(vec_ptr: *mut Vector3, z_val: f64) { (*vec_ptr)[2] = z_val; } -/// Normalizes an existing vector stored at the address of +/// Normalizes an existing vector stored at the address of /// a pointer (vec_ptr) -/// +/// /// # Safety -/// -/// When finished with the underlying vector, the caller must remember to +/// +/// When finished with the underlying vector, the caller must remember to /// free the vector memory using the free_vector_memory FFI function #[no_mangle] pub unsafe extern "C" fn normalize_vector(vec_ptr: *mut Vector3) { @@ -110,9 +109,9 @@ pub unsafe extern "C" fn normalize_vector(vec_ptr: *mut Vector3) { /// Initializes a normalized copy of a vector stored at the /// address of a pointer (vec_ptr) and returns a pointer to the /// memory of the result -/// +/// /// # Safety -/// +/// /// The caller must remember to free the vector memory of *both* the input and /// output vectors when finished with them using the free_vector_memory FFI function #[no_mangle] @@ -122,12 +121,12 @@ pub unsafe extern "C" fn vector_get_normalized(vec_ptr: *const Vector3) -> to_raw_pointer(vec) } -/// Scales an existing vector stored at the address of +/// Scales an existing vector stored at the address of /// a pointer (vec_ptr) by a float factor -/// +/// /// # Safety -/// -/// When finished with the underlying vector, the caller must remember to +/// +/// When finished with the underlying vector, the caller must remember to /// free the vector memory using the free_vector_memory FFI function #[no_mangle] pub unsafe extern "C" fn scale_vector(vec_ptr: *mut Vector3, factor: f64) { @@ -138,23 +137,26 @@ pub unsafe extern "C" fn scale_vector(vec_ptr: *mut Vector3, factor: f64) { /// Initializes a scaled copy of a vector stored at the /// address of a pointer (vec_ptr) and returns a pointer to the /// memory of the result -/// +/// /// # Safety -/// +/// /// The caller must remember to free the vector memory of *both* the input and /// output vectors when finished with them using the free_vector_memory FFI function #[no_mangle] -pub unsafe extern "C" fn vector_get_scaled(vec_ptr: *const Vector3, factor: f64) -> *mut Vector3 { +pub unsafe extern "C" fn vector_get_scaled( + vec_ptr: *const Vector3, + factor: f64, +) -> *mut Vector3 { null_pointer_check!(vec_ptr); let vec = (*vec_ptr).scale(factor); to_raw_pointer(vec) } -/// Adds two vectors and returns a pointer to the +/// Adds two vectors and returns a pointer to the /// memory of the result -/// +/// /// # Safety -/// +/// /// The caller must remember to free the vector memory of *both* the input and /// output vectors when finished with them using the free_vector_memory FFI function #[no_mangle] @@ -167,11 +169,11 @@ pub unsafe extern "C" fn vector_add( to_raw_pointer((*vec_ptr_1) + (*vec_ptr_2)) } -/// Subtracts two vectors and returns a pointer to the +/// Subtracts two vectors and returns a pointer to the /// memory of the result -/// +/// /// # Safety -/// +/// /// The caller must remember to free the vector memory of *both* the input and /// output vectors when finished with them using the free_vector_memory FFI function #[no_mangle] @@ -185,10 +187,10 @@ pub unsafe extern "C" fn vector_subtract( } /// Computes the dot product of two vectors -/// +/// /// # Safety -/// -/// The caller must remember to free the vector memory of the input vectors +/// +/// The caller must remember to free the vector memory of the input vectors /// when finished with them using the free_vector_memory FFI function #[no_mangle] pub unsafe extern "C" fn vector_dot_product( @@ -200,11 +202,11 @@ pub unsafe extern "C" fn vector_dot_product( (*vec_ptr_1).dot(&*vec_ptr_2) } -/// Computes the cross product of two vectors and returns +/// Computes the cross product of two vectors and returns /// a pointer to the memory of the result -/// +/// /// # Safety -/// +/// /// The caller must remember to free the vector memory of *both* the input and /// output vectors when finished with them using the free_vector_memory FFI function #[no_mangle] diff --git a/src/spatialmath/utils.rs b/src/spatialmath/utils.rs index 5e39ea4..a9a22e1 100644 --- a/src/spatialmath/utils.rs +++ b/src/spatialmath/utils.rs @@ -1,5 +1,5 @@ use float_cmp::{ApproxEq, F64Margin}; -use nalgebra::{Quaternion, Vector3, UnitQuaternion, UnitVector3}; +use nalgebra::{Quaternion, UnitQuaternion, UnitVector3, Vector3}; const ANGLE_ACCEPTANCE: f64 = 0.0001; @@ -8,7 +8,7 @@ const ANGLE_ACCEPTANCE: f64 = 0.0001; pub struct EulerAngles { pub roll: f64, pub pitch: f64, - pub yaw: f64 + pub yaw: f64, } impl EulerAngles { @@ -16,16 +16,18 @@ impl EulerAngles { EulerAngles { roll, pitch, yaw } } - /// Converts a quaternion into euler angles (in radians). The euler angles are - /// represented according to the Tait-Bryan formalism and applied + /// Converts a quaternion into euler angles (in radians). The euler angles are + /// represented according to the Tait-Bryan formalism and applied /// in the Z-Y'-X" order (where Z -> yaw, Y -> pitch, X -> roll). pub fn from_quaternion(quat: &Quaternion) -> Self { // get a normalized version of the quaternion let norm_quat = quat.normalize(); // calculate yaw - let yaw_sin_pitch_cos: f64 = 2.0 * ((norm_quat.w * norm_quat.k) + (norm_quat.i * norm_quat.j)); - let yaw_cos_pitch_cos: f64 = 1.0 - 2.0 * ((norm_quat.j * norm_quat.j) + (norm_quat.k * norm_quat.k)); + let yaw_sin_pitch_cos: f64 = + 2.0 * ((norm_quat.w * norm_quat.k) + (norm_quat.i * norm_quat.j)); + let yaw_cos_pitch_cos: f64 = + 1.0 - 2.0 * ((norm_quat.j * norm_quat.j) + (norm_quat.k * norm_quat.k)); let yaw = yaw_sin_pitch_cos.atan2(yaw_cos_pitch_cos); // calculate pitch and roll @@ -39,8 +41,10 @@ impl EulerAngles { roll = (2.0 * norm_quat.i.atan2(norm_quat.w)) + yaw.copysign(pitch_sin); } else { pitch = pitch_sin.asin(); - let roll_sin_pitch_cos = 2.0 * ((norm_quat.w * norm_quat.i) + (norm_quat.j * norm_quat.k)); - let roll_cos_pitch_cos = 1.0 - 2.0 * ((norm_quat.i * norm_quat.i) + (norm_quat.j * norm_quat.j)); + let roll_sin_pitch_cos = + 2.0 * ((norm_quat.w * norm_quat.i) + (norm_quat.j * norm_quat.k)); + let roll_cos_pitch_cos = + 1.0 - 2.0 * ((norm_quat.i * norm_quat.i) + (norm_quat.j * norm_quat.j)); roll = roll_sin_pitch_cos.atan2(roll_cos_pitch_cos); } @@ -54,8 +58,10 @@ impl From> for EulerAngles { let norm_quat = quat.normalize(); // calculate yaw - let yaw_sin_pitch_cos: f64 = 2.0 * ((norm_quat.w * norm_quat.k) + (norm_quat.i * norm_quat.j)); - let yaw_cos_pitch_cos: f64 = 1.0 - 2.0 * ((norm_quat.j * norm_quat.j) + (norm_quat.k * norm_quat.k)); + let yaw_sin_pitch_cos: f64 = + 2.0 * ((norm_quat.w * norm_quat.k) + (norm_quat.i * norm_quat.j)); + let yaw_cos_pitch_cos: f64 = + 1.0 - 2.0 * ((norm_quat.j * norm_quat.j) + (norm_quat.k * norm_quat.k)); let yaw = yaw_sin_pitch_cos.atan2(yaw_cos_pitch_cos); // calculate pitch and roll @@ -69,8 +75,10 @@ impl From> for EulerAngles { roll = (2.0 * norm_quat.i.atan2(norm_quat.w)) + yaw.copysign(pitch_sin); } else { pitch = pitch_sin.asin(); - let roll_sin_pitch_cos = 2.0 * ((norm_quat.w * norm_quat.i) + (norm_quat.j * norm_quat.k)); - let roll_cos_pitch_cos = 1.0 - 2.0 * ((norm_quat.i * norm_quat.i) + (norm_quat.j * norm_quat.j)); + let roll_sin_pitch_cos = + 2.0 * ((norm_quat.w * norm_quat.i) + (norm_quat.j * norm_quat.k)); + let roll_cos_pitch_cos = + 1.0 - 2.0 * ((norm_quat.i * norm_quat.i) + (norm_quat.j * norm_quat.j)); roll = roll_sin_pitch_cos.atan2(roll_cos_pitch_cos); } @@ -82,12 +90,15 @@ impl From> for EulerAngles { #[derive(Clone, Copy, Debug)] pub struct AxisAngle { pub axis: Vector3, - pub theta: f64 + pub theta: f64, } impl AxisAngle { pub fn new(x: f64, y: f64, z: f64, theta: f64) -> Self { - AxisAngle { axis: Vector3::new(x, y, z), theta } + AxisAngle { + axis: Vector3::new(x, y, z), + theta, + } } } @@ -99,12 +110,8 @@ impl TryFrom> for AxisAngle { let axis_opt = unit_quat.axis(); let angle = unit_quat.angle(); match axis_opt { - Some(value) => { - Ok(Self::new(value[0], value[1], value[2], angle)) - }, - None => { - Err(()) - }, + Some(value) => Ok(Self::new(value[0], value[1], value[2], angle)), + None => Err(()), } } } @@ -113,40 +120,40 @@ impl TryFrom> for AxisAngle { #[derive(Clone, Copy, Debug)] pub struct OrientationVector { pub o_vector: UnitVector3, - pub theta: f64 + pub theta: f64, } impl OrientationVector { pub fn new(o_x: f64, o_y: f64, o_z: f64, theta: f64) -> Self { let o_vector = UnitVector3::new_normalize(Vector3::new(o_x, o_y, o_z)); - OrientationVector{o_vector, theta} + OrientationVector { o_vector, theta } } pub fn to_quaternion(&self) -> Quaternion { let lat = self.o_vector.z.acos(); let lon = match self.o_vector.z { val if 1.0 - val > ANGLE_ACCEPTANCE => self.o_vector.y.atan2(self.o_vector.x), - _ => 0.0 + _ => 0.0, }; // convert angles as euler angles (lon, lat, theta) to quaternion // using the zyz rotational order - let s: [f64;3] = [ + let s: [f64; 3] = [ (lon / 2.0).sin(), (lat / 2.0).sin(), - (self.theta / 2.0).sin() + (self.theta / 2.0).sin(), ]; - let c: [f64;3] = [ + let c: [f64; 3] = [ (lon / 2.0).cos(), (lat / 2.0).cos(), - (self.theta / 2.0).cos() + (self.theta / 2.0).cos(), ]; - let real = c[0]*c[1]*c[2] - s[0]*c[1]*s[2]; - let i = c[0]*s[1]*s[2] - s[0]*s[1]*c[2]; - let j = c[0]*s[1]*c[2] + s[0]*s[1]*s[2]; - let k = s[0]*c[1]*c[2] + c[0]*c[1]*s[2]; + let real = c[0] * c[1] * c[2] - s[0] * c[1] * s[2]; + let i = c[0] * s[1] * s[2] - s[0] * s[1] * c[2]; + let j = c[0] * s[1] * c[2] + s[0] * s[1] * s[2]; + let k = s[0] * c[1] * c[2] + c[0] * c[1] * s[2]; Quaternion::new(real, i, j, k) } @@ -185,53 +192,55 @@ impl From> for OrientationVector { let cos_theta = match cos_theta_cand { val if val < -1.0 => -1.0, val if val > 1.0 => 1.0, - _ => cos_theta_cand + _ => cos_theta_cand, }; match cos_theta.acos() { val if val > ANGLE_ACCEPTANCE => { let new_z_imag_unit = UnitVector3::new_normalize(new_z_imag); - let rot_quat_unit = UnitQuaternion::from_axis_angle(&new_z_imag_unit, -1.0 * val); + let rot_quat_unit = + UnitQuaternion::from_axis_angle(&new_z_imag_unit, -1.0 * val); let rot_quat = rot_quat_unit.quaternion(); let z_axis_quat = Quaternion::new(0.0, 0.0, 0.0, 1.0); let test_z = (rot_quat * z_axis_quat) * rot_quat.conjugate(); let test_z_imag = test_z.imag(); let normal_3 = new_z_imag.cross(&test_z_imag); - let cos_test = normal_1.dot(&normal_3) / (normal_3.norm() * normal_1.norm()); + let cos_test = + normal_1.dot(&normal_3) / (normal_3.norm() * normal_1.norm()); match cos_test { - val2 if (1.0 - val2) < (ANGLE_ACCEPTANCE * ANGLE_ACCEPTANCE) => -1.0 * val, - _ => val + val2 if (1.0 - val2) < (ANGLE_ACCEPTANCE * ANGLE_ACCEPTANCE) => { + -1.0 * val + } + _ => val, } - }, - _ => 0.0 + } + _ => 0.0, } - }, + } _ => match new_z.k { val if val < 0.0 => -1.0 * new_x.j.atan2(new_x.i), - _ => -1.0 * new_x.j.atan2(new_x.i * -1.0) - } + _ => -1.0 * new_x.j.atan2(new_x.i * -1.0), + }, }; Self { o_vector, theta } } } -pub fn rotate_vector_by_quaternion( - quat: &Quaternion, vector: &Vector3 -) -> Vector3 { +pub fn rotate_vector_by_quaternion(quat: &Quaternion, vector: &Vector3) -> Vector3 { let quat_vec = Vector3::new(quat.i, quat.j, quat.k); let quat_real = quat.w; (2.0 * quat_vec.dot(vector) * quat_vec) - + ((quat_real * quat_real - quat_vec.norm_squared()) * vector) - + (2.0 * quat_real) * quat_vec.cross(vector) + + ((quat_real * quat_real - quat_vec.norm_squared()) * vector) + + (2.0 * quat_real) * quat_vec.cross(vector) } #[cfg(test)] mod tests { - use float_cmp::{assert_approx_eq}; + use float_cmp::assert_approx_eq; use nalgebra::{Quaternion, Vector3}; - use super::{EulerAngles, OrientationVector, rotate_vector_by_quaternion}; + use super::{rotate_vector_by_quaternion, EulerAngles, OrientationVector}; fn get_quaternion_diff_norm(quat1: &Quaternion, quat2: &Quaternion) -> f64 { let quat_diff = quat1.coords - quat2.coords; @@ -245,164 +254,126 @@ mod tests { #[test] fn quaternion_to_orientation_vector_works() { - let quat = Quaternion::new( - 0.7071067811865476, 0.7071067811865476, 0.0, 0.0 - ); - let expected_ov = OrientationVector::new( - 0.0, -1.0, 0.0, 1.5707963267948966 - ); + let quat = Quaternion::new(0.7071067811865476, 0.7071067811865476, 0.0, 0.0); + let expected_ov = OrientationVector::new(0.0, -1.0, 0.0, 1.5707963267948966); let calc_ov: OrientationVector = quat.into(); assert_approx_eq!(OrientationVector, calc_ov, expected_ov); - let quat2 = Quaternion::new( - 0.7071067811865476, -0.7071067811865476, 0.0, 0.0 - ); - let expected_ov2 = OrientationVector::new( - 0.0, 1.0, 0.0, -1.5707963267948966 - ); + let quat2 = Quaternion::new(0.7071067811865476, -0.7071067811865476, 0.0, 0.0); + let expected_ov2 = OrientationVector::new(0.0, 1.0, 0.0, -1.5707963267948966); let calc_ov2: OrientationVector = quat2.into(); assert_approx_eq!(OrientationVector, calc_ov2, expected_ov2); - let quat3 = Quaternion::new( - 0.96, 0.0, -0.28, 0.0 - ); - let expected_ov3 = OrientationVector::new( - -0.5376, 0.0, 0.8432, -1.0 * std::f64::consts::PI - ); + let quat3 = Quaternion::new(0.96, 0.0, -0.28, 0.0); + let expected_ov3 = + OrientationVector::new(-0.5376, 0.0, 0.8432, -1.0 * std::f64::consts::PI); let calc_ov3: OrientationVector = quat3.into(); assert_approx_eq!(OrientationVector, calc_ov3, expected_ov3); - let quat4 = Quaternion::new( - 0.96, 0.0, 0.0, -0.28 - ); - let expected_ov4 = OrientationVector::new( - 0.0, 0.0, 1.0, -0.5675882184166557 - ); + let quat4 = Quaternion::new(0.96, 0.0, 0.0, -0.28); + let expected_ov4 = OrientationVector::new(0.0, 0.0, 1.0, -0.5675882184166557); let calc_ov4: OrientationVector = quat4.into(); assert_approx_eq!(OrientationVector, calc_ov4, expected_ov4); - let quat5 = Quaternion::new( - 0.96, -0.28, 0.0, 0.0 - ); - let expected_ov5 = OrientationVector::new( - 0.0, 0.5376, 0.8432, -1.5707963267948966 - ); + let quat5 = Quaternion::new(0.96, -0.28, 0.0, 0.0); + let expected_ov5 = OrientationVector::new(0.0, 0.5376, 0.8432, -1.5707963267948966); let calc_ov5: OrientationVector = quat5.into(); assert_approx_eq!(OrientationVector, calc_ov5, expected_ov5); - let quat6 = Quaternion::new( - 0.96, 0.28, 0.0, 0.0 - ); - let expected_ov6 = OrientationVector::new( - 0.0, -0.5376, 0.8432, 1.5707963267948966 - ); + let quat6 = Quaternion::new(0.96, 0.28, 0.0, 0.0); + let expected_ov6 = OrientationVector::new(0.0, -0.5376, 0.8432, 1.5707963267948966); let calc_ov6: OrientationVector = quat6.into(); assert_approx_eq!(OrientationVector, calc_ov6, expected_ov6); let quat7 = Quaternion::new(0.5, -0.5, -0.5, -0.5); - let expected_ov7 = OrientationVector::new( - 0.0, 1.0, 0.0, -1.0 * std::f64::consts::PI - ); + let expected_ov7 = OrientationVector::new(0.0, 1.0, 0.0, -1.0 * std::f64::consts::PI); let calc_ov7: OrientationVector = quat7.into(); assert_approx_eq!(OrientationVector, calc_ov7, expected_ov7); let quat8 = Quaternion::new( - 0.816632212270443, -0.17555966025413142, 0.39198397193979817, 0.3855375485164001 + 0.816632212270443, + -0.17555966025413142, + 0.39198397193979817, + 0.3855375485164001, ); let expected_ov8 = OrientationVector::new( - 0.5048437942940054, 0.5889844266763397, 0.631054742867507, 0.02 + 0.5048437942940054, + 0.5889844266763397, + 0.631054742867507, + 0.02, ); let calc_ov8: OrientationVector = quat8.into(); assert_approx_eq!(OrientationVector, calc_ov8, expected_ov8, epsilon = 0.0001); - } #[test] fn orientation_vector_to_quaternion_works() { - let ov = OrientationVector::new( - 0.0, -1.0, 0.0, 1.5707963267948966 - ); - let expected_quat = Quaternion::new( - 0.7071067811865476, 0.7071067811865476, 0.0, 0.0 - ); + let ov = OrientationVector::new(0.0, -1.0, 0.0, 1.5707963267948966); + let expected_quat = Quaternion::new(0.7071067811865476, 0.7071067811865476, 0.0, 0.0); let calc_quat = ov.to_quaternion(); let mut diff = get_quaternion_diff_norm(&expected_quat, &calc_quat); assert_approx_eq!(f64, diff, 0.0); - let ov2 = OrientationVector::new( - 0.0, 1.0, 0.0, -1.5707963267948966 - ); - let expected_quat2 = Quaternion::new( - 0.7071067811865476, -0.7071067811865476, 0.0, 0.0 - ); + let ov2 = OrientationVector::new(0.0, 1.0, 0.0, -1.5707963267948966); + let expected_quat2 = Quaternion::new(0.7071067811865476, -0.7071067811865476, 0.0, 0.0); let calc_quat2 = ov2.to_quaternion(); diff = get_quaternion_diff_norm(&expected_quat2, &calc_quat2); assert_approx_eq!(f64, diff, 0.0); - let ov3 = OrientationVector::new( - -0.5376, 0.0, 0.8432, -1.0 * std::f64::consts::PI - ); - let expected_quat3 = Quaternion::new( - 0.96, 0.0, -0.28, 0.0 - ); + let ov3 = OrientationVector::new(-0.5376, 0.0, 0.8432, -1.0 * std::f64::consts::PI); + let expected_quat3 = Quaternion::new(0.96, 0.0, -0.28, 0.0); let calc_quat3 = ov3.to_quaternion(); diff = get_quaternion_diff_norm(&expected_quat3, &calc_quat3); assert_approx_eq!(f64, diff, 0.0); - let ov4 = OrientationVector::new( - 0.0, 0.0, 1.0, -0.5675882184166557 - ); - let expected_quat4 = Quaternion::new( - 0.96, 0.0, 0.0, -0.28 - ); + let ov4 = OrientationVector::new(0.0, 0.0, 1.0, -0.5675882184166557); + let expected_quat4 = Quaternion::new(0.96, 0.0, 0.0, -0.28); let calc_quat4 = ov4.to_quaternion(); diff = get_quaternion_diff_norm(&expected_quat4, &calc_quat4); assert_approx_eq!(f64, diff, 0.0); - let ov5 = OrientationVector::new( - 0.0, 0.5376, 0.8432, -1.5707963267948966 - ); - let expected_quat5 = Quaternion::new( - 0.96, -0.28, 0.0, 0.0 - ); + let ov5 = OrientationVector::new(0.0, 0.5376, 0.8432, -1.5707963267948966); + let expected_quat5 = Quaternion::new(0.96, -0.28, 0.0, 0.0); let calc_quat5 = ov5.to_quaternion(); diff = get_quaternion_diff_norm(&expected_quat5, &calc_quat5); assert_approx_eq!(f64, diff, 0.0); - let ov6 = OrientationVector::new( - 0.0, -0.5376, 0.8432, 1.5707963267948966 - ); - let expected_quat6 = Quaternion::new( - 0.96, 0.28, 0.0, 0.0 - ); + let ov6 = OrientationVector::new(0.0, -0.5376, 0.8432, 1.5707963267948966); + let expected_quat6 = Quaternion::new(0.96, 0.28, 0.0, 0.0); let calc_quat6 = ov6.to_quaternion(); diff = get_quaternion_diff_norm(&expected_quat6, &calc_quat6); assert_approx_eq!(f64, diff, 0.0); - let ov7 = OrientationVector::new( - 0.0, 1.0, 0.0, -1.0 * std::f64::consts::PI - ); + let ov7 = OrientationVector::new(0.0, 1.0, 0.0, -1.0 * std::f64::consts::PI); let expected_quat7 = Quaternion::new(0.5, -0.5, -0.5, -0.5); let calc_quat7 = ov7.to_quaternion(); diff = get_quaternion_diff_norm(&expected_quat7, &calc_quat7); assert_approx_eq!(f64, diff, 0.0); let ov8 = OrientationVector::new( - 0.5048437942940054, 0.5889844266763397, 0.631054742867507, 0.02 + 0.5048437942940054, + 0.5889844266763397, + 0.631054742867507, + 0.02, ); let expected_quat8 = Quaternion::new( - 0.816632212270443, -0.17555966025413142, 0.39198397193979817, 0.3855375485164001 + 0.816632212270443, + -0.17555966025413142, + 0.39198397193979817, + 0.3855375485164001, ); let calc_quat8 = ov8.to_quaternion(); diff = get_quaternion_diff_norm(&expected_quat8, &calc_quat8); assert_approx_eq!(f64, diff, 0.0); - } #[test] fn euler_angles_from_quaternion_works() { let quat = Quaternion::new( - 0.2705980500730985, -0.6532814824381882, 0.27059805007309856, 0.6532814824381883 + 0.2705980500730985, + -0.6532814824381882, + 0.27059805007309856, + 0.6532814824381883, ); let euler_angles: EulerAngles = quat.into(); assert_approx_eq!(f64, euler_angles.pitch, std::f64::consts::PI / 2.0); @@ -410,7 +381,10 @@ mod tests { assert_approx_eq!(f64, euler_angles.roll, std::f64::consts::PI / 4.0); let quat2 = Quaternion::new( - 0.4619397662556435, -0.19134171618254486, 0.4619397662556434, 0.7325378163287418 + 0.4619397662556435, + -0.19134171618254486, + 0.4619397662556434, + 0.7325378163287418, ); let euler_angles2: EulerAngles = quat2.into(); assert_approx_eq!(f64, euler_angles2.pitch, std::f64::consts::PI / 4.0); @@ -436,5 +410,4 @@ mod tests { let diff = get_vector_diff_norm(&expected_vector2, &rotated_vector2); assert_approx_eq!(f64, diff, 0.0, epsilon = 0.0001); } - }