From 15eb6fe908ae484e88cf83f95ba9476618244f33 Mon Sep 17 00:00:00 2001
From: Gautham Varadarajan <gautham@viam.com>
Date: Mon, 31 Oct 2022 16:16:28 -0400
Subject: [PATCH 1/6] [RSDK-732] Create Quaternion and corresponding FFI
 namespace

---
 src/ffi/spatialmath/mod.rs        |   3 +-
 src/ffi/spatialmath/quaternion.rs | 127 ++++++++++
 src/spatialmath/mod.rs            |   3 +-
 src/spatialmath/quaternion.rs     | 371 ++++++++++++++++++++++++++++++
 src/spatialmath/vector3.rs        |   1 -
 5 files changed, 502 insertions(+), 3 deletions(-)
 create mode 100644 src/ffi/spatialmath/quaternion.rs
 create mode 100644 src/spatialmath/quaternion.rs

diff --git a/src/ffi/spatialmath/mod.rs b/src/ffi/spatialmath/mod.rs
index 9f3006e..0efc187 100644
--- a/src/ffi/spatialmath/mod.rs
+++ b/src/ffi/spatialmath/mod.rs
@@ -1 +1,2 @@
-pub mod vector3;
\ No newline at end of file
+pub mod vector3;
+pub mod quaternion;
\ No newline at end of file
diff --git a/src/ffi/spatialmath/quaternion.rs b/src/ffi/spatialmath/quaternion.rs
new file mode 100644
index 0000000..f738710
--- /dev/null
+++ b/src/ffi/spatialmath/quaternion.rs
@@ -0,0 +1,127 @@
+use ffi_helpers::null_pointer_check;
+use libc::c_double;
+
+use crate::spatialmath::{quaternion::Quaternion, vector3::Vector3};
+
+/// The FFI interface for Quaternion functions and initialization. All functions are
+/// meant to be called externally from other languages
+
+/// Initialize a quaternion from raw components and retrieve the C pointer
+/// to its address 
+#[no_mangle]
+pub extern "C" fn new_quaternion(real: f64, i: f64, j: f64, k: f64) -> *mut Quaternion {
+    Quaternion::new(real, i, j, k).to_raw_pointer()
+}
+
+/// Initialize a quaternion from a real part and a C pointer to a Vector3
+/// and retrieve the C pointer to its address 
+#[no_mangle]
+pub unsafe extern "C" fn new_quaternion_from_vector(
+    real: f64, imag_ptr: *const Vector3
+) -> *mut Quaternion {
+    null_pointer_check!(imag_ptr);
+    let imag = *imag_ptr;
+    Quaternion::new_from_vector(real, imag).to_raw_pointer()
+}
+
+/// Free memory at the address of the quaternion pointer. 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) {
+    if ptr.is_null() {
+        return;
+    }
+    Box::from_raw(ptr);
+}
+
+/// Get the components of a quaternion as a list of C doubles, the order of the
+/// components will be (real, i, j, k)
+#[no_mangle]
+pub unsafe extern "C" fn quaternion_get_components(quat_ptr: *const Quaternion) -> *const c_double {
+    null_pointer_check!(quat_ptr);
+    let quat = *quat_ptr;
+    let components: [c_double;4] = [quat.real, quat.i, quat.j, quat.k];
+    Box::into_raw(Box::new(components)) as *const _
+}
+
+/// Set the real component of an existing quaternion stored at the address
+/// of a pointer
+#[no_mangle]
+pub unsafe extern "C" fn quaternion_set_real(quat_ptr: *mut Quaternion, real: f64) {
+    null_pointer_check!(quat_ptr);
+    let quat = &mut*quat_ptr;
+    quat.real = real
+}
+
+/// Set the i component of an existing quaternion stored at the address
+/// of a pointer
+#[no_mangle]
+pub unsafe extern "C" fn quaternion_set_i(quat_ptr: *mut Quaternion, i: f64) {
+    null_pointer_check!(quat_ptr);
+    let quat = &mut*quat_ptr;
+    quat.i = i
+}
+
+/// Set the j component of an existing quaternion stored at the address
+/// of a pointer
+#[no_mangle]
+pub unsafe extern "C" fn quaternion_set_j(quat_ptr: *mut Quaternion, j: f64) {
+    null_pointer_check!(quat_ptr);
+    let quat = &mut*quat_ptr;
+    quat.j = j
+}
+
+/// Set the k component of an existing quaternion stored at the address
+/// of a pointer
+#[no_mangle]
+pub unsafe extern "C" fn quaternion_set_k(quat_ptr: *mut Quaternion, k: f64) {
+    null_pointer_check!(quat_ptr);
+    let quat = &mut*quat_ptr;
+    quat.k = k;
+}
+
+/// 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
+#[no_mangle]
+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);
+    let mut quat = *quat_ptr;
+    let imag = *vec_ptr;
+    quat.set_imag_from_vector(imag);
+}
+
+/// 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
+#[no_mangle]
+pub unsafe extern "C" fn quaternion_get_imaginary_vector(quat_ptr: *const Quaternion) -> *mut Vector3 {
+    null_pointer_check!(quat_ptr);
+    let quat = *quat_ptr;
+    let imag = quat.imag();
+    imag.to_raw_pointer()
+}
+
+/// Converts from euler angles 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)
+#[no_mangle]
+pub unsafe extern "C" fn quaternion_from_euler_angles(roll: f64, pitch: f64, yaw: f64) -> *mut Quaternion {
+    let quat = Quaternion::from_euler_angles(roll, pitch, yaw);
+    quat.to_raw_pointer()
+}
+
+/// Converts a quaternion into euler angles. 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). 
+/// The return value is a pointer to a list of [roll, pitch, yaw]
+/// as C doubles
+#[no_mangle]
+pub unsafe extern "C" fn quaternion_to_euler_angles(quat_ptr: *const Quaternion) -> *const c_double {
+    null_pointer_check!(quat_ptr);
+    let quat = *quat_ptr;
+    let euler_angles = quat.to_euler_angles();
+    Box::into_raw(Box::new(euler_angles)) as *const _
+}
\ No newline at end of file
diff --git a/src/spatialmath/mod.rs b/src/spatialmath/mod.rs
index 9f3006e..269e089 100644
--- a/src/spatialmath/mod.rs
+++ b/src/spatialmath/mod.rs
@@ -1 +1,2 @@
-pub mod vector3;
\ No newline at end of file
+pub mod vector3;
+pub mod quaternion;
diff --git a/src/spatialmath/quaternion.rs b/src/spatialmath/quaternion.rs
new file mode 100644
index 0000000..3c67e0a
--- /dev/null
+++ b/src/spatialmath/quaternion.rs
@@ -0,0 +1,371 @@
+use float_cmp::{ApproxEq, F64Margin, approx_eq};
+
+use super::vector3::Vector3;
+
+
+/// A Rust implementation of Quaternion, we use this instead of existing packages
+/// because we want a C-safe representational structure. These quaternions use
+/// the Real-I-J-K standard, so those using JPL or other standards should take
+/// care to convert before initializing a Quaternion struct from the library
+#[repr(C)]
+#[derive(Copy, Clone, Debug)]
+pub struct Quaternion {
+    pub real: f64,
+    pub i: f64,
+    pub j: f64,
+    pub k: f64,
+}
+
+impl Quaternion {
+    /// Initializes a quaternion from a real component and an
+    /// imaginary 3-vector
+    pub fn new_from_vector(real: f64, imag: Vector3) -> Self {
+        Self{real: real, i: imag.x, j: imag.y, k: imag.z}
+    }
+
+    /// Initializes a Quaternion from its raw components
+    pub fn new(real: f64, i: f64, j: f64, k: f64) -> Self {
+        Self{real, i, j, k}
+    }
+
+    /// Converts from euler angles 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)
+    pub fn from_euler_angles(roll: f64, pitch: f64, yaw: f64) -> Self {
+        let roll_cos = (roll * 0.5).cos();
+        let roll_sin = (roll * 0.5).sin();
+
+        let pitch_cos = (pitch * 0.5).cos();
+        let pitch_sin = (pitch * 0.5).sin();
+
+        let yaw_cos = (yaw * 0.5).cos();
+        let yaw_sin = (yaw * 0.5).sin();
+
+        let real = (roll_cos * pitch_cos * yaw_cos) + (roll_sin * pitch_sin * yaw_sin);
+        let i = (roll_sin * pitch_cos * yaw_cos) - (roll_cos * pitch_sin * yaw_sin);
+        let j = (roll_cos * pitch_sin * yaw_cos) + (roll_sin * pitch_cos * yaw_sin);
+        let k = (roll_cos * pitch_cos * yaw_sin) - (roll_sin * pitch_sin * yaw_cos);
+
+        Quaternion::new(real, i, j, k)
+    }
+
+    /// Converts a quaternion into euler angles. 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). 
+    /// The return value is a list of [roll, pitch, yaw]
+    pub fn to_euler_angles(&self) -> [f64;3] {
+        // get a normalized version of the quaternion
+        let quat = self.get_normalized();
+
+        // calculate yaw
+        let yaw_sin_pitch_cos = 2.0 * ((quat.real * quat.k) + (quat.i * quat.j));
+        let yaw_cos_pitch_cos = 1.0 - 2.0 * ((quat.j * quat.j) + (quat.k * quat.k));
+        let yaw = yaw_sin_pitch_cos.atan2(yaw_cos_pitch_cos);
+
+        // calculate pitch and roll
+        let pitch_sin = 2.0 * ((quat.real * quat.j) - (quat.k * quat.i));
+        let pitch: f64;
+        let roll: f64;
+        // for a pitch that is π / 2, we experience gimbal lock
+        // and must calculate roll based on the real rotation and yaw
+        if pitch_sin.abs() >= 1.0 {
+            pitch = (std::f64::consts::PI / 2.0).copysign(pitch_sin);
+            roll = (2.0 * quat.i.atan2(quat.real)) + yaw.copysign(pitch_sin);
+        } else {
+            pitch = pitch_sin.asin();
+            let roll_sin_pitch_cos = 2.0 * ((quat.real * quat.i) + (quat.j * quat.k));
+            let roll_cos_pitch_cos = 1.0 - 2.0 * ((quat.i * quat.i) + (quat.j * quat.j));
+            roll = roll_sin_pitch_cos.atan2(roll_cos_pitch_cos);
+        }
+
+
+        [roll, pitch, yaw]
+    }
+
+    /// Return the imaginary components of a quaternion as
+    /// a 3-vector
+    pub fn imag(&self) -> Vector3 {
+        Vector3 { x: self.i, y: self.j, z: self.k }
+    }
+
+    /// Sets the imaginary components of a quaternion from
+    /// a provided 3-vector
+    pub fn set_imag_from_vector(&mut self, imag: Vector3) {
+        self.i = imag.x;
+        self.j = imag.y;
+        self.k = imag.z;
+    }
+
+    /// Returns whether the quaternion is normalized 
+    /// (a four-vector on the unit sphere in quaternion space)
+    pub fn is_normalized(&self) -> bool {
+        approx_eq!(f64, self.norm2().sqrt(), 1.0)
+    }
+
+    pub fn norm2(&self) -> f64 {
+        (self.real * self.real) + (self.i * self.i) + (self.j * self.j) + (self.k * self.k)
+    }
+
+    /// Normalizes a quaternion
+    pub fn normalize(&mut self) {
+        self.scale(1.0 / self.norm2().sqrt())
+    }
+
+    /// Returns a normalized copy of the quaternion
+    pub fn get_normalized(&self) -> Self {
+        let mut copy_quat = self.clone();
+        copy_quat.normalize();
+        copy_quat
+    }
+
+    pub fn scale(&mut self, factor: f64) {
+        self.real = self.real * factor;
+        self.i = self.i * factor;
+        self.j = self.j * factor;
+        self.k = self.k * factor;
+    }
+
+    pub fn get_scaled(&self, factor: f64) -> Self {
+        let mut copy_quat = self.clone();
+        copy_quat.scale(factor);
+        copy_quat
+    }
+
+    pub fn conjugate(&self) -> Self {
+        Self { real: self.real, i: self.i * -1.0, j: self.j * -1.0, k: self.k * -1.0 }
+    }
+
+    /// Allocates the vector to the heap with a stable memory address and
+    /// returns the raw pointer (for use by the FFI interface)
+    pub(crate) fn to_raw_pointer(&self) -> *mut Self {
+        Box::into_raw(Box::new(*self))
+    }
+}
+
+impl std::ops::Add<Quaternion> for Quaternion {
+    type Output = Quaternion;
+
+    fn add(self, _rhs: Quaternion) -> Quaternion {
+        Self { 
+            real: self.real + _rhs.real, 
+            i: self.i + _rhs.i,
+            j: self.j + _rhs.j,
+            k: self.k + _rhs.k
+        }
+    }
+}
+
+impl std::ops::Sub<Quaternion> for Quaternion {
+    type Output = Quaternion;
+
+    fn sub(self, _rhs: Quaternion) -> Quaternion {
+        Self { 
+            real: self.real - _rhs.real, 
+            i: self.i - _rhs.i,
+            j: self.j - _rhs.j,
+            k: self.k - _rhs.k
+        }
+    }
+}
+
+/// Implements the Hamiltonian product for two quaternions
+impl std::ops::Mul<Quaternion> for Quaternion {
+    type Output = Quaternion;
+
+    fn mul(self, _rhs: Quaternion) -> Self::Output {
+        let real_0 = self.real;
+        let i_0 = self.i;
+        let j_0 = self.j;
+        let k_0 = self.k;
+
+        let real_1 = _rhs.real;
+        let i_1 = _rhs.i;
+        let j_1 = _rhs.j;
+        let k_1 = _rhs.k;
+        
+        let real = real_0*real_1 - i_0*i_1 - j_0*j_1 - k_0*k_1;
+        let i_part = real_0*i_1 + i_0*real_1 + j_0*k_1 - k_0*j_1;
+        let j_part = real_0*j_1 - i_0*k_1 + j_0*real_1 + k_0*i_1;
+        let k_part = real_0*k_1 + i_0*j_1 - j_0*i_1 + k_0*real_1;
+
+        Self::new(real, i_part, j_part, k_part)
+    }
+}
+
+impl ApproxEq for Quaternion {
+    type Margin = F64Margin;
+
+    fn approx_eq<M: Into<Self::Margin>>(self, other: Self, margin: M) -> bool {
+        let margin = margin.into();
+        let diff = other - self;
+        let diff_norm2 = (
+            diff.real * diff.real + diff.i * diff.i + diff.j * diff.j + diff.k * diff.k
+        ).sqrt();
+        diff_norm2.approx_eq(0.0, margin)
+    }
+}
+
+impl PartialEq for Quaternion {
+    fn eq(&self, other: &Self) -> bool {
+        (self.real == other.real) && (self.i == other.i) && (self.j == other.j) && (self.k == other.k)
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use float_cmp::{approx_eq};
+
+    use crate::spatialmath::vector3::Vector3;
+    use crate::spatialmath::quaternion::Quaternion;
+
+    #[test]
+    fn new_initializes_quaternion_successfully() {
+        let quat = Quaternion::new(1.0, 0.0, 0.5, 1.0);
+        assert_eq!(quat.real, 1.0);
+        assert_eq!(quat.i, 0.0);
+        assert_eq!(quat.j, 0.5);
+        assert_eq!(quat.k, 1.0)
+    }
+
+    #[test]
+    fn imag_returns_imaginary_components_as_vector() {
+        let quat = Quaternion::new(1.0, 0.0, 0.5, 1.0);
+        let expected_imag = Vector3::new(0.0, 0.5, 1.0);
+        assert_eq!(quat.imag(), expected_imag);
+    }
+
+    #[test]
+    fn set_imag_from_vector_works() {
+        let imag = Vector3::new(0.0, 0.5, 1.0);
+        let expected_quat = Quaternion::new(1.0, 0.0, 0.5, 1.0);
+        let quat = Quaternion::new_from_vector(1.0, imag);
+        assert_eq!(quat, expected_quat);
+    }
+
+    #[test]
+    fn quaternion_normalizes_successfully() {
+        let mut quat = Quaternion::new(
+            0.0,
+            (1.0_f64 / 3.0_f64).sqrt() * 0.5, 
+            (1.0_f64 / 3.0_f64).sqrt() * -0.5, 
+            (1.0_f64 / 3.0_f64).sqrt() * 0.5
+        );
+        let expected_quat = Quaternion::new(
+            0.0,
+            (1.0_f64 / 3.0_f64).sqrt(), 
+            (1.0_f64 / 3.0_f64).sqrt() * -1.0, 
+            (1.0_f64 / 3.0_f64).sqrt()
+        );
+        quat.normalize();
+        assert!(approx_eq!(Quaternion, quat, expected_quat));
+    }
+
+    #[test]
+    fn is_normalized_is_correct() {
+        let not_normalized = Quaternion::new(
+            0.3,
+            (1.0_f64 / 3.0_f64).sqrt() * 0.5, 
+            (1.0_f64 / 3.0_f64).sqrt() * -0.5, 
+            (1.0_f64 / 3.0_f64).sqrt() * 0.5
+        );
+        assert!(!not_normalized.is_normalized());
+        let normalized = Quaternion::new(
+            0.0,
+            (1.0_f64 / 3.0_f64).sqrt(), 
+            (1.0_f64 / 3.0_f64).sqrt() * -1.0, 
+            (1.0_f64 / 3.0_f64).sqrt()
+        );
+        assert!(normalized.is_normalized());
+    }
+
+    #[test]
+    fn add_subtract_works() {
+        let quat1 = Quaternion::new(0.1, 0.2, 0.3, 0.4);
+        let quat2 = Quaternion::new(0.2, 0.3, 0.4, 0.5);
+        let expected_add = Quaternion::new(0.3, 0.5, 0.7, 0.9);
+        let expected_sub = Quaternion::new(-0.1, -0.1, -0.1, -0.1);
+        assert!(approx_eq!(Quaternion, quat1 + quat2, expected_add));
+        assert!(approx_eq!(Quaternion, quat1 - quat2, expected_sub));
+    }
+
+    #[test]
+    fn multiply_works() {
+        let quat1 = Quaternion::new(0.1, 0.2, 0.3, 0.4);
+        let quat2 = Quaternion::new(0.2, 0.3, 0.4, 0.5);
+        let expected_mul = Quaternion::new(-0.36, 0.06, 0.12, 0.12);
+        let expected_rev_mul = Quaternion::new(-0.36, 0.08, 0.08, 0.14);
+        assert!(approx_eq!(Quaternion, quat1 * quat2, expected_mul));
+        assert!(approx_eq!(Quaternion, quat2 * quat1, expected_rev_mul));
+    }
+
+    #[test]
+    fn scale_works() {
+        let mut quat = Quaternion::new(0.1, 0.2, 0.3, 0.4);
+        quat.scale(2.0);
+        let expected_quat = Quaternion::new(0.2, 0.4, 0.6, 0.8);
+        assert!(approx_eq!(Quaternion, quat, expected_quat));
+    }
+
+    #[test]
+    fn get_scaled_returns_scaled_quaternion() {
+        let quat = Quaternion::new(0.1, 0.2, 0.3, 0.4);
+        let quat_orig = Quaternion::new(0.1, 0.2, 0.3, 0.4);
+        let scaled_quat = quat.get_scaled(2.0);
+        let expected_quat = Quaternion::new(0.2, 0.4, 0.6, 0.8);
+        assert_eq!(quat, quat_orig);
+        assert!(approx_eq!(Quaternion, scaled_quat, expected_quat));
+    }
+
+    #[test]
+    fn conjugate_works() {
+        let quat = Quaternion::new(0.1, 0.2, 0.3, 0.4);
+        let expected_quat = Quaternion::new(0.1, -0.2, -0.3, -0.4);
+        assert!(approx_eq!(Quaternion, quat.conjugate(), expected_quat));
+    }
+
+    #[test]
+    fn quaternion_initializes_from_euler_angles() {
+        let expected_quat = Quaternion::new(
+            0.2705980500730985, -0.6532814824381882, 0.27059805007309856, 0.6532814824381883
+        );
+        let roll = std::f64::consts::PI / 4.0;
+        let pitch = std::f64::consts::PI / 2.0;
+        let yaw = std::f64::consts::PI;
+        let quat = Quaternion::from_euler_angles(roll, pitch, yaw);
+        assert!(approx_eq!(Quaternion, quat, expected_quat));
+
+        let expected_quat2 = Quaternion::new(
+            0.4619397662556435, -0.19134171618254486, 0.4619397662556434, 0.7325378163287418
+        );
+        let roll2 = std::f64::consts::PI / 4.0;
+        let pitch2 = std::f64::consts::PI / 4.0;
+        let yaw2 = 3.0 * std::f64::consts::PI / 4.0;
+        let quat2 = Quaternion::from_euler_angles(roll2, pitch2, yaw2);
+        assert!(approx_eq!(Quaternion, quat2, expected_quat2));
+    }
+
+    #[test]
+    fn euler_angles_from_quaternion_works() {
+        let quat = Quaternion::new(
+            0.2705980500730985, -0.6532814824381882, 0.27059805007309856, 0.6532814824381883
+        );
+        let euler_angles = quat.to_euler_angles();
+        let roll = euler_angles[0];
+        let pitch = euler_angles[1];
+        let yaw = euler_angles[2];
+        assert!(approx_eq!(f64, pitch, std::f64::consts::PI / 2.0));
+        assert!(approx_eq!(f64, yaw, std::f64::consts::PI));
+        assert!(approx_eq!(f64, roll, std::f64::consts::PI / 4.0));
+
+        let quat2 = Quaternion::new(
+            0.4619397662556435, -0.19134171618254486, 0.4619397662556434, 0.7325378163287418
+        );
+        let euler_angles2 = quat2.to_euler_angles();
+        let roll2 = euler_angles2[0];
+        let pitch2 = euler_angles2[1];
+        let yaw2 = euler_angles2[2];
+        assert!(approx_eq!(f64, pitch2, std::f64::consts::PI / 4.0));
+        assert!(approx_eq!(f64, yaw2, 3.0 * std::f64::consts::PI / 4.0));
+        assert!(approx_eq!(f64, roll2, std::f64::consts::PI / 4.0));
+    }
+}
\ No newline at end of file
diff --git a/src/spatialmath/vector3.rs b/src/spatialmath/vector3.rs
index 3763914..289bfe5 100644
--- a/src/spatialmath/vector3.rs
+++ b/src/spatialmath/vector3.rs
@@ -111,7 +111,6 @@ mod tests {
         assert_eq!(vector.x, 1.0);
         assert_eq!(vector.y, 1.0);
         assert_eq!(vector.z, 1.0);
-        assert!(!vector.is_normalized())
     }
 
     #[test]

From efea9cc2d3086234a42b541ce85ba23aa654eabf Mon Sep 17 00:00:00 2001
From: Gautham Varadarajan <gautham@viam.com>
Date: Fri, 4 Nov 2022 15:19:51 -0400
Subject: [PATCH 2/6] address review comments

---
 src/ffi/mod.rs                    |  2 +-
 src/ffi/spatialmath/quaternion.rs | 28 ++++++++--
 src/spatialmath/quaternion.rs     | 85 +++++++++++++------------------
 src/spatialmath/vector3.rs        | 12 ++---
 4 files changed, 66 insertions(+), 61 deletions(-)

diff --git a/src/ffi/mod.rs b/src/ffi/mod.rs
index 47cc60b..c19fb24 100644
--- a/src/ffi/mod.rs
+++ b/src/ffi/mod.rs
@@ -1,2 +1,2 @@
 pub mod dial_ffi;
-pub mod spatialmath;
\ No newline at end of file
+pub mod spatialmath;
diff --git a/src/ffi/spatialmath/quaternion.rs b/src/ffi/spatialmath/quaternion.rs
index f738710..97f1b4d 100644
--- a/src/ffi/spatialmath/quaternion.rs
+++ b/src/ffi/spatialmath/quaternion.rs
@@ -6,11 +6,17 @@ use crate::spatialmath::{quaternion::Quaternion, vector3::Vector3};
 /// The FFI interface for Quaternion functions and initialization. All functions are
 /// meant to be called externally from other languages
 
+/// Allocates the quaternion to the heap with a stable memory address and
+/// returns the raw pointer (for use by the FFI interface)
+fn to_raw_pointer(quat: &Quaternion) -> *mut Quaternion {
+    Box::into_raw(Box::new(*quat))
+}
+
 /// Initialize a quaternion from raw components and retrieve the C pointer
 /// to its address 
 #[no_mangle]
 pub extern "C" fn new_quaternion(real: f64, i: f64, j: f64, k: f64) -> *mut Quaternion {
-    Quaternion::new(real, i, j, k).to_raw_pointer()
+    to_raw_pointer(&Quaternion::new(real, i, j, k))
 }
 
 /// Initialize a quaternion from a real part and a C pointer to a Vector3
@@ -21,7 +27,7 @@ pub unsafe extern "C" fn new_quaternion_from_vector(
 ) -> *mut Quaternion {
     null_pointer_check!(imag_ptr);
     let imag = *imag_ptr;
-    Quaternion::new_from_vector(real, imag).to_raw_pointer()
+    to_raw_pointer(&Quaternion::new_from_vector(real, imag))
 }
 
 /// Free memory at the address of the quaternion pointer. Outer processes
@@ -81,6 +87,20 @@ pub unsafe extern "C" fn quaternion_set_k(quat_ptr: *mut Quaternion, k: f64) {
     quat.k = k;
 }
 
+/// Set all of the components of an existing quaternion stored at the address
+/// of a pointer
+#[no_mangle]
+pub unsafe extern "C" fn quaternion_set_components(
+    quat_ptr: *mut Quaternion, real: f64, i: f64, j: f64, k: f64
+) {
+    null_pointer_check!(quat_ptr);
+    let quat = &mut*quat_ptr;
+    quat.real = real;
+    quat.i = i;
+    quat.j = j;
+    quat.k = k;
+}
+
 /// 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
@@ -110,7 +130,7 @@ pub unsafe extern "C" fn quaternion_get_imaginary_vector(quat_ptr: *const Quater
 #[no_mangle]
 pub unsafe extern "C" fn quaternion_from_euler_angles(roll: f64, pitch: f64, yaw: f64) -> *mut Quaternion {
     let quat = Quaternion::from_euler_angles(roll, pitch, yaw);
-    quat.to_raw_pointer()
+    to_raw_pointer(&quat)
 }
 
 /// Converts a quaternion into euler angles. The euler angles are 
@@ -124,4 +144,4 @@ pub unsafe extern "C" fn quaternion_to_euler_angles(quat_ptr: *const Quaternion)
     let quat = *quat_ptr;
     let euler_angles = quat.to_euler_angles();
     Box::into_raw(Box::new(euler_angles)) as *const _
-}
\ No newline at end of file
+}
diff --git a/src/spatialmath/quaternion.rs b/src/spatialmath/quaternion.rs
index 3c67e0a..5395e0d 100644
--- a/src/spatialmath/quaternion.rs
+++ b/src/spatialmath/quaternion.rs
@@ -2,7 +2,6 @@ use float_cmp::{ApproxEq, F64Margin, approx_eq};
 
 use super::vector3::Vector3;
 
-
 /// A Rust implementation of Quaternion, we use this instead of existing packages
 /// because we want a C-safe representational structure. These quaternions use
 /// the Real-I-J-K standard, so those using JPL or other standards should take
@@ -77,8 +76,7 @@ impl Quaternion {
             let roll_cos_pitch_cos = 1.0 - 2.0 * ((quat.i * quat.i) + (quat.j * quat.j));
             roll = roll_sin_pitch_cos.atan2(roll_cos_pitch_cos);
         }
-
-
+        
         [roll, pitch, yaw]
     }
 
@@ -108,7 +106,8 @@ impl Quaternion {
 
     /// Normalizes a quaternion
     pub fn normalize(&mut self) {
-        self.scale(1.0 / self.norm2().sqrt())
+        // let inv_sq_dp = fast_inv_sqrt(self.norm2());
+        self.scale(self.norm2().sqrt().recip())
     }
 
     /// Returns a normalized copy of the quaternion
@@ -135,22 +134,17 @@ impl Quaternion {
         Self { real: self.real, i: self.i * -1.0, j: self.j * -1.0, k: self.k * -1.0 }
     }
 
-    /// Allocates the vector to the heap with a stable memory address and
-    /// returns the raw pointer (for use by the FFI interface)
-    pub(crate) fn to_raw_pointer(&self) -> *mut Self {
-        Box::into_raw(Box::new(*self))
-    }
 }
 
 impl std::ops::Add<Quaternion> for Quaternion {
     type Output = Quaternion;
 
-    fn add(self, _rhs: Quaternion) -> Quaternion {
+    fn add(self, rhs: Quaternion) -> Quaternion {
         Self { 
-            real: self.real + _rhs.real, 
-            i: self.i + _rhs.i,
-            j: self.j + _rhs.j,
-            k: self.k + _rhs.k
+            real: self.real + rhs.real, 
+            i: self.i + rhs.i,
+            j: self.j + rhs.j,
+            k: self.k + rhs.k
         }
     }
 }
@@ -158,12 +152,12 @@ impl std::ops::Add<Quaternion> for Quaternion {
 impl std::ops::Sub<Quaternion> for Quaternion {
     type Output = Quaternion;
 
-    fn sub(self, _rhs: Quaternion) -> Quaternion {
+    fn sub(self, rhs: Quaternion) -> Quaternion {
         Self { 
-            real: self.real - _rhs.real, 
-            i: self.i - _rhs.i,
-            j: self.j - _rhs.j,
-            k: self.k - _rhs.k
+            real: self.real - rhs.real, 
+            i: self.i - rhs.i,
+            j: self.j - rhs.j,
+            k: self.k - rhs.k
         }
     }
 }
@@ -213,7 +207,7 @@ impl PartialEq for Quaternion {
 
 #[cfg(test)]
 mod tests {
-    use float_cmp::{approx_eq};
+    use float_cmp::{assert_approx_eq};
 
     use crate::spatialmath::vector3::Vector3;
     use crate::spatialmath::quaternion::Quaternion;
@@ -244,20 +238,11 @@ mod tests {
 
     #[test]
     fn quaternion_normalizes_successfully() {
-        let mut quat = Quaternion::new(
-            0.0,
-            (1.0_f64 / 3.0_f64).sqrt() * 0.5, 
-            (1.0_f64 / 3.0_f64).sqrt() * -0.5, 
-            (1.0_f64 / 3.0_f64).sqrt() * 0.5
-        );
-        let expected_quat = Quaternion::new(
-            0.0,
-            (1.0_f64 / 3.0_f64).sqrt(), 
-            (1.0_f64 / 3.0_f64).sqrt() * -1.0, 
-            (1.0_f64 / 3.0_f64).sqrt()
-        );
+        let mut quat = Quaternion::new(1.0, 2.0, 3.0, 4.0);
         quat.normalize();
-        assert!(approx_eq!(Quaternion, quat, expected_quat));
+        let length = quat.norm2();
+        assert!(length <= 1.0);
+        assert_approx_eq!(f64, length, 1.0)
     }
 
     #[test]
@@ -284,8 +269,8 @@ mod tests {
         let quat2 = Quaternion::new(0.2, 0.3, 0.4, 0.5);
         let expected_add = Quaternion::new(0.3, 0.5, 0.7, 0.9);
         let expected_sub = Quaternion::new(-0.1, -0.1, -0.1, -0.1);
-        assert!(approx_eq!(Quaternion, quat1 + quat2, expected_add));
-        assert!(approx_eq!(Quaternion, quat1 - quat2, expected_sub));
+        assert_approx_eq!(Quaternion, quat1 + quat2, expected_add);
+        assert_approx_eq!(Quaternion, quat1 - quat2, expected_sub);
     }
 
     #[test]
@@ -294,8 +279,8 @@ mod tests {
         let quat2 = Quaternion::new(0.2, 0.3, 0.4, 0.5);
         let expected_mul = Quaternion::new(-0.36, 0.06, 0.12, 0.12);
         let expected_rev_mul = Quaternion::new(-0.36, 0.08, 0.08, 0.14);
-        assert!(approx_eq!(Quaternion, quat1 * quat2, expected_mul));
-        assert!(approx_eq!(Quaternion, quat2 * quat1, expected_rev_mul));
+        assert_approx_eq!(Quaternion, quat1 * quat2, expected_mul);
+        assert_approx_eq!(Quaternion, quat2 * quat1, expected_rev_mul);
     }
 
     #[test]
@@ -303,24 +288,24 @@ mod tests {
         let mut quat = Quaternion::new(0.1, 0.2, 0.3, 0.4);
         quat.scale(2.0);
         let expected_quat = Quaternion::new(0.2, 0.4, 0.6, 0.8);
-        assert!(approx_eq!(Quaternion, quat, expected_quat));
+        assert_approx_eq!(Quaternion, quat, expected_quat);
     }
 
     #[test]
     fn get_scaled_returns_scaled_quaternion() {
         let quat = Quaternion::new(0.1, 0.2, 0.3, 0.4);
-        let quat_orig = Quaternion::new(0.1, 0.2, 0.3, 0.4);
+        let quat_orig = quat.clone();
         let scaled_quat = quat.get_scaled(2.0);
         let expected_quat = Quaternion::new(0.2, 0.4, 0.6, 0.8);
         assert_eq!(quat, quat_orig);
-        assert!(approx_eq!(Quaternion, scaled_quat, expected_quat));
+        assert_approx_eq!(Quaternion, scaled_quat, expected_quat);
     }
 
     #[test]
     fn conjugate_works() {
         let quat = Quaternion::new(0.1, 0.2, 0.3, 0.4);
         let expected_quat = Quaternion::new(0.1, -0.2, -0.3, -0.4);
-        assert!(approx_eq!(Quaternion, quat.conjugate(), expected_quat));
+        assert_approx_eq!(Quaternion, quat.conjugate(), expected_quat);
     }
 
     #[test]
@@ -332,7 +317,7 @@ mod tests {
         let pitch = std::f64::consts::PI / 2.0;
         let yaw = std::f64::consts::PI;
         let quat = Quaternion::from_euler_angles(roll, pitch, yaw);
-        assert!(approx_eq!(Quaternion, quat, expected_quat));
+        assert_approx_eq!(Quaternion, quat, expected_quat);
 
         let expected_quat2 = Quaternion::new(
             0.4619397662556435, -0.19134171618254486, 0.4619397662556434, 0.7325378163287418
@@ -341,7 +326,7 @@ mod tests {
         let pitch2 = std::f64::consts::PI / 4.0;
         let yaw2 = 3.0 * std::f64::consts::PI / 4.0;
         let quat2 = Quaternion::from_euler_angles(roll2, pitch2, yaw2);
-        assert!(approx_eq!(Quaternion, quat2, expected_quat2));
+        assert_approx_eq!(Quaternion, quat2, expected_quat2);
     }
 
     #[test]
@@ -353,9 +338,9 @@ mod tests {
         let roll = euler_angles[0];
         let pitch = euler_angles[1];
         let yaw = euler_angles[2];
-        assert!(approx_eq!(f64, pitch, std::f64::consts::PI / 2.0));
-        assert!(approx_eq!(f64, yaw, std::f64::consts::PI));
-        assert!(approx_eq!(f64, roll, std::f64::consts::PI / 4.0));
+        assert_approx_eq!(f64, pitch, std::f64::consts::PI / 2.0);
+        assert_approx_eq!(f64, yaw, std::f64::consts::PI);
+        assert_approx_eq!(f64, roll, std::f64::consts::PI / 4.0);
 
         let quat2 = Quaternion::new(
             0.4619397662556435, -0.19134171618254486, 0.4619397662556434, 0.7325378163287418
@@ -364,8 +349,8 @@ mod tests {
         let roll2 = euler_angles2[0];
         let pitch2 = euler_angles2[1];
         let yaw2 = euler_angles2[2];
-        assert!(approx_eq!(f64, pitch2, std::f64::consts::PI / 4.0));
-        assert!(approx_eq!(f64, yaw2, 3.0 * std::f64::consts::PI / 4.0));
-        assert!(approx_eq!(f64, roll2, std::f64::consts::PI / 4.0));
+        assert_approx_eq!(f64, pitch2, std::f64::consts::PI / 4.0);
+        assert_approx_eq!(f64, yaw2, 3.0 * std::f64::consts::PI / 4.0);
+        assert_approx_eq!(f64, roll2, std::f64::consts::PI / 4.0);
     }
-}
\ No newline at end of file
+}
diff --git a/src/spatialmath/vector3.rs b/src/spatialmath/vector3.rs
index 289bfe5..efd09f6 100644
--- a/src/spatialmath/vector3.rs
+++ b/src/spatialmath/vector3.rs
@@ -103,7 +103,7 @@ impl PartialEq for Vector3 {
 #[cfg(test)]
 mod tests {
     use crate::spatialmath::vector3::Vector3;
-    use float_cmp::approx_eq;
+    use float_cmp::assert_approx_eq;
 
     #[test]
     fn new_initializes_vector_successfully() {
@@ -139,7 +139,7 @@ mod tests {
             (1.0_f64 / 3.0_f64).sqrt() * -1.0, 
             (1.0_f64 / 3.0_f64).sqrt());
         vector.normalize();
-        assert!(approx_eq!(Vector3, expected_vector, vector));
+        assert_approx_eq!(Vector3, expected_vector, vector);
     }
 
     #[test]
@@ -153,7 +153,7 @@ mod tests {
             (1.0_f64 / 3.0_f64).sqrt() * -1.0, 
             (1.0_f64 / 3.0_f64).sqrt());
         let result_vector = vector.get_normalized();
-        assert!(approx_eq!(Vector3, expected_vector, result_vector));
+        assert_approx_eq!(Vector3, expected_vector, result_vector);
         assert!(!vector.is_normalized());
         assert!(result_vector.is_normalized())
     }
@@ -202,7 +202,7 @@ mod tests {
         let v2 = Vector3::new(2.0, 4.0, 6.0);
         let zero_v = Vector3::new(0.0,0.0,0.0);
         let v1xv2 = v1.cross(&v2);
-        assert!(approx_eq!(Vector3, zero_v, v1xv2));
+        assert_approx_eq!(Vector3, zero_v, v1xv2);
     }
 
     #[test]
@@ -211,6 +211,6 @@ mod tests {
         let v2 = Vector3::new(3.0, 4.0, 5.0);
         let expected_vector = Vector3::new(-2.0, 4.0, -2.0);
         let v1xv2 = v1.cross(&v2);
-        assert!(approx_eq!(Vector3, expected_vector, v1xv2));
+        assert_approx_eq!(Vector3, expected_vector, v1xv2);
     }
-}
\ No newline at end of file
+}

From 973a78c1a157e1f7516a78742dd44904a17fcb7e Mon Sep 17 00:00:00 2001
From: Gautham Varadarajan <gautham@viam.com>
Date: Wed, 9 Nov 2022 10:22:37 -0500
Subject: [PATCH 3/6] address more review comments

---
 src/ffi/spatialmath/quaternion.rs | 73 ++++++++++++++++---------------
 src/ffi/spatialmath/vector3.rs    | 40 +++++------------
 src/spatialmath/quaternion.rs     | 51 ++++++++++++++-------
 src/spatialmath/vector3.rs        |  4 +-
 4 files changed, 87 insertions(+), 81 deletions(-)

diff --git a/src/ffi/spatialmath/quaternion.rs b/src/ffi/spatialmath/quaternion.rs
index 97f1b4d..41093bb 100644
--- a/src/ffi/spatialmath/quaternion.rs
+++ b/src/ffi/spatialmath/quaternion.rs
@@ -3,141 +3,144 @@ use libc::c_double;
 
 use crate::spatialmath::{quaternion::Quaternion, vector3::Vector3};
 
-/// The FFI interface for Quaternion functions and initialization. All functions are
-/// meant to be called externally from other languages
+/// The FFI interface for Quaternion functions and initialization. All public 
+/// functions are meant to be called externally from other languages
 
-/// Allocates the quaternion to the heap with a stable memory address and
+/// Allocates a copy of the quaternion to the heap with a stable memory address and
 /// returns the raw pointer (for use by the FFI interface)
 fn to_raw_pointer(quat: &Quaternion) -> *mut Quaternion {
     Box::into_raw(Box::new(*quat))
 }
 
 /// Initialize a quaternion from raw components and retrieve the C pointer
-/// to its address 
+/// to its address.
+/// # Safety
 #[no_mangle]
 pub extern "C" fn new_quaternion(real: f64, i: f64, j: f64, k: f64) -> *mut Quaternion {
     to_raw_pointer(&Quaternion::new(real, i, j, k))
 }
 
 /// Initialize a quaternion from a real part and a C pointer to a Vector3
-/// and retrieve the C pointer to its address 
+/// and retrieve the C pointer to its address.
+/// # Safety
 #[no_mangle]
 pub unsafe extern "C" fn new_quaternion_from_vector(
     real: f64, imag_ptr: *const Vector3
 ) -> *mut Quaternion {
     null_pointer_check!(imag_ptr);
-    let imag = *imag_ptr;
-    to_raw_pointer(&Quaternion::new_from_vector(real, imag))
+    to_raw_pointer(&Quaternion::new_from_vector(real, *imag_ptr))
 }
 
 /// Free memory at the address of the quaternion pointer. Outer processes
 /// that work with Quaternions via the FFI interface MUST remember 
 /// to call this function when finished with a quaternion
+/// # Safety
 #[no_mangle]
 pub unsafe extern "C" fn free_quaternion_memory(ptr: *mut Quaternion) {
     if ptr.is_null() {
         return;
     }
-    Box::from_raw(ptr);
+    let _ = Box::from_raw(ptr);
 }
 
 /// Get the components of a quaternion as a list of C doubles, the order of the
-/// components will be (real, i, j, k)
+/// components will be (real, i, j, k).
+/// # Safety
 #[no_mangle]
 pub unsafe extern "C" fn quaternion_get_components(quat_ptr: *const Quaternion) -> *const c_double {
     null_pointer_check!(quat_ptr);
-    let quat = *quat_ptr;
-    let components: [c_double;4] = [quat.real, quat.i, quat.j, quat.k];
+    let components: [c_double;4] = [(*quat_ptr).real, (*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
+/// of a pointer.
+/// # Safety
 #[no_mangle]
 pub unsafe extern "C" fn quaternion_set_real(quat_ptr: *mut Quaternion, real: f64) {
     null_pointer_check!(quat_ptr);
-    let quat = &mut*quat_ptr;
-    quat.real = real
+    (*quat_ptr).real = real;
 }
 
 /// Set the i component of an existing quaternion stored at the address
-/// of a pointer
+/// of a pointer.
+/// # Safety
 #[no_mangle]
 pub unsafe extern "C" fn quaternion_set_i(quat_ptr: *mut Quaternion, i: f64) {
     null_pointer_check!(quat_ptr);
-    let quat = &mut*quat_ptr;
-    quat.i = i
+    (*quat_ptr).i = i;
 }
 
 /// Set the j component of an existing quaternion stored at the address
-/// of a pointer
+/// of a pointer.
+/// # Safety
 #[no_mangle]
 pub unsafe extern "C" fn quaternion_set_j(quat_ptr: *mut Quaternion, j: f64) {
     null_pointer_check!(quat_ptr);
-    let quat = &mut*quat_ptr;
-    quat.j = j
+    (*quat_ptr).j = j;
 }
 
 /// Set the k component of an existing quaternion stored at the address
-/// of a pointer
+/// of a pointer.
+/// # Safety
 #[no_mangle]
 pub unsafe extern "C" fn quaternion_set_k(quat_ptr: *mut Quaternion, k: f64) {
     null_pointer_check!(quat_ptr);
-    let quat = &mut*quat_ptr;
-    quat.k = k;
+    (*quat_ptr).k = k;
 }
 
 /// Set all of the components of an existing quaternion stored at the address
 /// of a pointer
+/// # Safety
 #[no_mangle]
 pub unsafe extern "C" fn quaternion_set_components(
     quat_ptr: *mut Quaternion, real: f64, i: f64, j: f64, k: f64
 ) {
     null_pointer_check!(quat_ptr);
-    let quat = &mut*quat_ptr;
-    quat.real = real;
-    quat.i = i;
-    quat.j = j;
-    quat.k = k;
+    (*quat_ptr).real = real;
+    (*quat_ptr).i = i;
+    (*quat_ptr).j = j;
+    (*quat_ptr).k = k;
 }
 
 /// 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
 #[no_mangle]
 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);
-    let mut quat = *quat_ptr;
-    let imag = *vec_ptr;
-    quat.set_imag_from_vector(imag);
+    (*quat_ptr).set_imag_from_vector(*vec_ptr);
 }
 
 /// 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
 #[no_mangle]
 pub unsafe extern "C" fn quaternion_get_imaginary_vector(quat_ptr: *const Quaternion) -> *mut Vector3 {
     null_pointer_check!(quat_ptr);
-    let quat = *quat_ptr;
-    let imag = quat.imag();
+    let imag = (*quat_ptr).imag();
     imag.to_raw_pointer()
 }
 
-/// Converts from euler angles to a quaternion. The euler angles are expected to
+/// 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
 #[no_mangle]
 pub unsafe extern "C" fn quaternion_from_euler_angles(roll: f64, pitch: f64, yaw: f64) -> *mut Quaternion {
     let quat = Quaternion::from_euler_angles(roll, pitch, yaw);
     to_raw_pointer(&quat)
 }
 
-/// Converts a quaternion into euler angles. The euler angles are 
+/// 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). 
 /// The return value is a pointer to a list of [roll, pitch, yaw]
 /// as C doubles
+/// # Safety
 #[no_mangle]
 pub unsafe extern "C" fn quaternion_to_euler_angles(quat_ptr: *const Quaternion) -> *const c_double {
     null_pointer_check!(quat_ptr);
diff --git a/src/ffi/spatialmath/vector3.rs b/src/ffi/spatialmath/vector3.rs
index 77e015b..41503ab 100644
--- a/src/ffi/spatialmath/vector3.rs
+++ b/src/ffi/spatialmath/vector3.rs
@@ -19,59 +19,51 @@ pub unsafe extern "C" fn free_vector_memory(ptr: *mut Vector3) {
 #[no_mangle]
 pub unsafe extern "C" fn vector_get_components(vec_ptr: *const Vector3) -> *const c_double {
     null_pointer_check!(vec_ptr);
-    let vec = *vec_ptr;
-    let components: [c_double; 3] = [vec.x, vec.y, vec.z];
+    let components: [c_double; 3] = [(*vec_ptr).x, (*vec_ptr).y, (*vec_ptr).z];
     Box::into_raw(Box::new(components)) as *const _
 }
 
 #[no_mangle]
 pub unsafe extern "C" fn vector_set_x(vec_ptr: *mut Vector3, x_val: f64) {
     null_pointer_check!(vec_ptr);
-    let vec = &mut *vec_ptr;
-    vec.x = x_val
+    (*vec_ptr).x = x_val;
 }
 
 #[no_mangle]
 pub unsafe extern "C" fn vector_set_y(vec_ptr: *mut Vector3, y_val: f64) {
     null_pointer_check!(vec_ptr);
-    let vec = &mut *vec_ptr;
-    vec.y = y_val
+    (*vec_ptr).y = y_val;
 }
 
 #[no_mangle]
 pub unsafe extern "C" fn vector_set_z(vec_ptr: *mut Vector3, z_val: f64) {
     null_pointer_check!(vec_ptr);
-    let vec = &mut *vec_ptr;
-    vec.z = z_val
+    (*vec_ptr).z = z_val;
 }
 
 #[no_mangle]
 pub unsafe extern "C" fn normalize_vector(vec_ptr: *mut Vector3) {
     null_pointer_check!(vec_ptr);
-    let vec = &mut *vec_ptr;
-    vec.normalize();
+    (*vec_ptr).normalize();
 }
 
 #[no_mangle]
 pub unsafe extern "C" fn vector_get_normalized(vec_ptr: *const Vector3) -> *mut Vector3 {
     null_pointer_check!(vec_ptr);
-    let vec1 = &*vec_ptr;
-    let vec = vec1.get_normalized();
+    let vec = (*vec_ptr).get_normalized();
     vec.to_raw_pointer()
 }
 
 #[no_mangle]
 pub unsafe extern "C" fn scale_vector(vec_ptr: *mut Vector3, factor: f64) {
     null_pointer_check!(vec_ptr);
-    let vec = &mut *vec_ptr;
-    vec.scale(factor);
+    (*vec_ptr).scale(factor);
 }
 
 #[no_mangle]
 pub unsafe extern "C" fn vector_get_scaled(vec_ptr: *const Vector3, factor: f64) -> *mut Vector3 {
     null_pointer_check!(vec_ptr);
-    let vec1 = &*vec_ptr;
-    let vec = vec1.get_scaled(factor);
+    let vec = (*vec_ptr).get_scaled(factor);
     vec.to_raw_pointer()
 }
 
@@ -82,9 +74,7 @@ pub unsafe extern "C" fn vector_add(
 ) -> *mut Vector3 {
     null_pointer_check!(vec_ptr_1);
     null_pointer_check!(vec_ptr_2);
-    let vec1 = &*vec_ptr_1;
-    let vec2 = &*vec_ptr_2;
-    (*vec1 + *vec2).to_raw_pointer()
+    ((*vec_ptr_1) + (*vec_ptr_2)).to_raw_pointer()
 }
 
 #[no_mangle]
@@ -94,9 +84,7 @@ pub unsafe extern "C" fn vector_subtract(
 ) -> *mut Vector3 {
     null_pointer_check!(vec_ptr_1);
     null_pointer_check!(vec_ptr_2);
-    let vec1 = &*vec_ptr_1;
-    let vec2 = &*vec_ptr_2;
-    (*vec1 - *vec2).to_raw_pointer()
+    ((*vec_ptr_1) - (*vec_ptr_2)).to_raw_pointer()
 }
 
 #[no_mangle]
@@ -106,9 +94,7 @@ pub unsafe extern "C" fn vector_dot_product(
 ) -> f64 {
     null_pointer_check!(vec_ptr_1, f64::NAN);
     null_pointer_check!(vec_ptr_2, f64::NAN);
-    let vec1 = &*vec_ptr_1;
-    let vec2 = &*vec_ptr_2;
-    vec1.dot(vec2)
+    (*vec_ptr_1).dot(&*vec_ptr_2)
 }
 
 #[no_mangle]
@@ -118,8 +104,6 @@ pub unsafe extern "C" fn vector_cross_product(
 ) -> *mut Vector3 {
     null_pointer_check!(vec_ptr_1);
     null_pointer_check!(vec_ptr_2);
-    let vec1 = &mut *vec_ptr_1;
-    let vec2 = &mut *vec_ptr_2;
-    let vec = vec1.cross(vec2);
+    let vec = (*vec_ptr_1).cross(&*vec_ptr_2);
     vec.to_raw_pointer()
 }
diff --git a/src/spatialmath/quaternion.rs b/src/spatialmath/quaternion.rs
index 5395e0d..0f6b3ca 100644
--- a/src/spatialmath/quaternion.rs
+++ b/src/spatialmath/quaternion.rs
@@ -19,7 +19,7 @@ impl Quaternion {
     /// Initializes a quaternion from a real component and an
     /// imaginary 3-vector
     pub fn new_from_vector(real: f64, imag: Vector3) -> Self {
-        Self{real: real, i: imag.x, j: imag.y, k: imag.z}
+        Self{real, i: imag.x, j: imag.y, k: imag.z}
     }
 
     /// Initializes a Quaternion from its raw components
@@ -27,9 +27,9 @@ impl Quaternion {
         Self{real, i, j, k}
     }
 
-    /// Converts from euler angles 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)
+    /// 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).
     pub fn from_euler_angles(roll: f64, pitch: f64, yaw: f64) -> Self {
         let roll_cos = (roll * 0.5).cos();
         let roll_sin = (roll * 0.5).sin();
@@ -48,7 +48,7 @@ impl Quaternion {
         Quaternion::new(real, i, j, k)
     }
 
-    /// Converts a quaternion into euler angles. The euler angles are 
+    /// 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). 
     /// The return value is a list of [roll, pitch, yaw]
@@ -76,7 +76,7 @@ impl Quaternion {
             let roll_cos_pitch_cos = 1.0 - 2.0 * ((quat.i * quat.i) + (quat.j * quat.j));
             roll = roll_sin_pitch_cos.atan2(roll_cos_pitch_cos);
         }
-        
+
         [roll, pitch, yaw]
     }
 
@@ -111,21 +111,20 @@ impl Quaternion {
     }
 
     /// Returns a normalized copy of the quaternion
-    pub fn get_normalized(&self) -> Self {
-        let mut copy_quat = self.clone();
-        copy_quat.normalize();
-        copy_quat
+    pub fn get_normalized(mut self) -> Self {
+        self.normalize();
+        return self
     }
 
     pub fn scale(&mut self, factor: f64) {
-        self.real = self.real * factor;
-        self.i = self.i * factor;
-        self.j = self.j * factor;
-        self.k = self.k * factor;
+        self.real *= factor;
+        self.i *= factor;
+        self.j *= factor;
+        self.k *= factor;
     }
 
     pub fn get_scaled(&self, factor: f64) -> Self {
-        let mut copy_quat = self.clone();
+        let mut copy_quat = *self;
         copy_quat.scale(factor);
         copy_quat
     }
@@ -245,6 +244,26 @@ mod tests {
         assert_approx_eq!(f64, length, 1.0)
     }
 
+    #[test]
+    fn get_normalized_returns_a_normalized_copy() {
+        let quat = Quaternion::new(
+            0.0,
+            (1.0_f64 / 3.0_f64).sqrt() * 0.5, 
+            (1.0_f64 / 3.0_f64).sqrt() * -0.5, 
+            (1.0_f64 / 3.0_f64).sqrt() * 0.5
+        );
+        let expected_quat = Quaternion::new(
+            0.0,
+            (1.0_f64 / 3.0_f64).sqrt(), 
+            (1.0_f64 / 3.0_f64).sqrt() * -1.0, 
+            (1.0_f64 / 3.0_f64).sqrt()
+        );
+        let unchanged_quat = quat;
+        let normalized_quat = quat.get_normalized();
+        assert_approx_eq!(Quaternion, normalized_quat, expected_quat);
+        assert_approx_eq!(Quaternion, quat, unchanged_quat);
+    }
+
     #[test]
     fn is_normalized_is_correct() {
         let not_normalized = Quaternion::new(
@@ -294,7 +313,7 @@ mod tests {
     #[test]
     fn get_scaled_returns_scaled_quaternion() {
         let quat = Quaternion::new(0.1, 0.2, 0.3, 0.4);
-        let quat_orig = quat.clone();
+        let quat_orig = quat;
         let scaled_quat = quat.get_scaled(2.0);
         let expected_quat = Quaternion::new(0.2, 0.4, 0.6, 0.8);
         assert_eq!(quat, quat_orig);
diff --git a/src/spatialmath/vector3.rs b/src/spatialmath/vector3.rs
index efd09f6..940cc5a 100644
--- a/src/spatialmath/vector3.rs
+++ b/src/spatialmath/vector3.rs
@@ -18,7 +18,7 @@ impl Vector3 {
     }
 
     pub fn get_normalized(&self) -> Self {
-        let mut copy_vec = self.clone();
+        let mut copy_vec = *self;
         copy_vec.normalize();
         copy_vec
     }
@@ -57,7 +57,7 @@ impl Vector3 {
     }
 
     pub fn get_scaled(&self, factor: f64) -> Self {
-        let mut copy_vec = self.clone();
+        let mut copy_vec = *self;
         copy_vec.scale(factor);
         copy_vec
     }

From 4f46b30292ba798d4429bf2b7ee4b48fa7996246 Mon Sep 17 00:00:00 2001
From: Gautham Varadarajan <gautham@viam.com>
Date: Wed, 9 Nov 2022 14:26:01 -0500
Subject: [PATCH 4/6] add more documentation + satisfy clippy

---
 src/ffi/spatialmath/quaternion.rs |  91 +++++++++++++++++++++++++++
 src/ffi/spatialmath/vector3.rs    | 100 ++++++++++++++++++++++++++++++
 src/spatialmath/quaternion.rs     |   2 +-
 src/spatialmath/vector3.rs        |  16 ++---
 4 files changed, 200 insertions(+), 9 deletions(-)

diff --git a/src/ffi/spatialmath/quaternion.rs b/src/ffi/spatialmath/quaternion.rs
index 41093bb..261ae11 100644
--- a/src/ffi/spatialmath/quaternion.rs
+++ b/src/ffi/spatialmath/quaternion.rs
@@ -14,7 +14,12 @@ 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 
+/// free_quaternion_memory FFI function
 #[no_mangle]
 pub extern "C" fn new_quaternion(real: f64, i: f64, j: f64, k: f64) -> *mut Quaternion {
     to_raw_pointer(&Quaternion::new(real, i, j, k))
@@ -22,7 +27,12 @@ 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 
+/// free_quaternion_memory FFI function
 #[no_mangle]
 pub unsafe extern "C" fn new_quaternion_from_vector(
     real: f64, imag_ptr: *const Vector3
@@ -34,6 +44,7 @@ pub unsafe extern "C" fn new_quaternion_from_vector(
 /// Free memory at the address of the quaternion pointer. Outer processes
 /// that work with Quaternions via the FFI interface MUST remember 
 /// to call this function when finished with a quaternion
+/// 
 /// # Safety
 #[no_mangle]
 pub unsafe extern "C" fn free_quaternion_memory(ptr: *mut Quaternion) {
@@ -45,7 +56,12 @@ 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 
+/// free_quaternion_memory FFI function
 #[no_mangle]
 pub unsafe extern "C" fn quaternion_get_components(quat_ptr: *const Quaternion) -> *const c_double {
     null_pointer_check!(quat_ptr);
@@ -55,7 +71,12 @@ pub unsafe extern "C" fn quaternion_get_components(quat_ptr: *const Quaternion)
 
 /// 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 
+/// free_quaternion_memory FFI function
 #[no_mangle]
 pub unsafe extern "C" fn quaternion_set_real(quat_ptr: *mut Quaternion, real: f64) {
     null_pointer_check!(quat_ptr);
@@ -64,7 +85,12 @@ pub unsafe extern "C" fn quaternion_set_real(quat_ptr: *mut Quaternion, real: f6
 
 /// 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 
+/// free_quaternion_memory FFI function
 #[no_mangle]
 pub unsafe extern "C" fn quaternion_set_i(quat_ptr: *mut Quaternion, i: f64) {
     null_pointer_check!(quat_ptr);
@@ -73,7 +99,12 @@ 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 
+/// free_quaternion_memory FFI function
 #[no_mangle]
 pub unsafe extern "C" fn quaternion_set_j(quat_ptr: *mut Quaternion, j: f64) {
     null_pointer_check!(quat_ptr);
@@ -82,7 +113,12 @@ 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 
+/// free_quaternion_memory FFI function
 #[no_mangle]
 pub unsafe extern "C" fn quaternion_set_k(quat_ptr: *mut Quaternion, k: f64) {
     null_pointer_check!(quat_ptr);
@@ -91,7 +127,12 @@ 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 
+/// 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
@@ -106,7 +147,13 @@ 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 
+/// 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) {
     null_pointer_check!(quat_ptr);
@@ -117,7 +164,12 @@ 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 
+/// free_quaternion_memory FFI function
 #[no_mangle]
 pub unsafe extern "C" fn quaternion_get_imaginary_vector(quat_ptr: *const Quaternion) -> *mut Vector3 {
     null_pointer_check!(quat_ptr);
@@ -125,10 +177,44 @@ pub unsafe extern "C" fn quaternion_get_imaginary_vector(quat_ptr: *const Quater
     imag.to_raw_pointer()
 }
 
+/// Normalizes an existing quaternion stored at the address of 
+/// a pointer (quat_ptr)
+/// 
+/// # Safety
+/// 
+/// When finished with the underlying quaternion initialized by this function
+/// 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) {
+    null_pointer_check!(quat_ptr);
+    (*quat_ptr).normalize()
+}
+
+/// 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 
+/// using the free_quaternion_memory FFI function
+#[no_mangle]
+pub unsafe extern "C" fn quaternion_get_normalized(quat_ptr: *const Quaternion) -> *mut Quaternion {
+    null_pointer_check!(quat_ptr);
+    to_raw_pointer(&(*quat_ptr).get_normalized())
+}
+
 /// 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 
+/// free_quaternion_memory FFI function
 #[no_mangle]
 pub unsafe extern "C" fn quaternion_from_euler_angles(roll: f64, pitch: f64, yaw: f64) -> *mut Quaternion {
     let quat = Quaternion::from_euler_angles(roll, pitch, yaw);
@@ -140,7 +226,12 @@ pub unsafe extern "C" fn quaternion_from_euler_angles(roll: f64, pitch: f64, yaw
 /// in the Z-Y'-X" order (where Z -> yaw, Y -> pitch, X -> roll). 
 /// The return value is a pointer to a list of [roll, pitch, yaw]
 /// as C doubles
+/// 
 /// # Safety
+/// 
+/// When finished with the underlying quaternion passed to this function
+/// the caller must remember to free the quaternion memory using the 
+/// free_quaternion_memory FFI function
 #[no_mangle]
 pub unsafe extern "C" fn quaternion_to_euler_angles(quat_ptr: *const Quaternion) -> *const c_double {
     null_pointer_check!(quat_ptr);
diff --git a/src/ffi/spatialmath/vector3.rs b/src/ffi/spatialmath/vector3.rs
index 41503ab..75a2d93 100644
--- a/src/ffi/spatialmath/vector3.rs
+++ b/src/ffi/spatialmath/vector3.rs
@@ -3,11 +3,26 @@ use libc::c_double;
 
 use crate::spatialmath::vector3::Vector3;
 
+/// The FFI interface for Vector functions and initialization. All public 
+/// functions are meant to be called externally from other languages
+
+/// 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 
+/// free_vector_memory FFI function
 #[no_mangle]
 pub extern "C" fn new_vector3(x: f64, y: f64, z: f64) -> *mut Vector3 {
     Vector3::new(x, y, z).to_raw_pointer()
 }
 
+/// Free memory at the address of the vector pointer. Outer processes
+/// that work with Vectors via the FFI interface MUST remember 
+/// to call this function when finished with a vector
+/// # Safety
 #[no_mangle]
 pub unsafe extern "C" fn free_vector_memory(ptr: *mut Vector3) {
     if ptr.is_null() {
@@ -16,6 +31,13 @@ pub unsafe extern "C" fn free_vector_memory(ptr: *mut Vector3) {
     let _ = Box::from_raw(ptr);
 }
 
+/// 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 
+/// 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 {
     null_pointer_check!(vec_ptr);
@@ -23,30 +45,66 @@ pub unsafe extern "C" fn vector_get_components(vec_ptr: *const Vector3) -> *cons
     Box::into_raw(Box::new(components)) as *const _
 }
 
+/// 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 
+/// 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) {
     null_pointer_check!(vec_ptr);
     (*vec_ptr).x = x_val;
 }
 
+/// 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 
+/// 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) {
     null_pointer_check!(vec_ptr);
     (*vec_ptr).y = y_val;
 }
 
+/// 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 
+/// 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) {
     null_pointer_check!(vec_ptr);
     (*vec_ptr).z = z_val;
 }
 
+/// 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 
+/// free the vector memory using the free_vector_memory FFI function
 #[no_mangle]
 pub unsafe extern "C" fn normalize_vector(vec_ptr: *mut Vector3) {
     null_pointer_check!(vec_ptr);
     (*vec_ptr).normalize();
 }
 
+/// 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]
 pub unsafe extern "C" fn vector_get_normalized(vec_ptr: *const Vector3) -> *mut Vector3 {
     null_pointer_check!(vec_ptr);
@@ -54,12 +112,27 @@ pub unsafe extern "C" fn vector_get_normalized(vec_ptr: *const Vector3) -> *mut
     vec.to_raw_pointer()
 }
 
+/// 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 
+/// 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) {
     null_pointer_check!(vec_ptr);
     (*vec_ptr).scale(factor);
 }
 
+/// 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 {
     null_pointer_check!(vec_ptr);
@@ -67,6 +140,13 @@ pub unsafe extern "C" fn vector_get_scaled(vec_ptr: *const Vector3, factor: f64)
     vec.to_raw_pointer()
 }
 
+/// 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]
 pub unsafe extern "C" fn vector_add(
     vec_ptr_1: *const Vector3,
@@ -77,6 +157,13 @@ pub unsafe extern "C" fn vector_add(
     ((*vec_ptr_1) + (*vec_ptr_2)).to_raw_pointer()
 }
 
+/// 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]
 pub unsafe extern "C" fn vector_subtract(
     vec_ptr_1: *const Vector3,
@@ -87,6 +174,12 @@ pub unsafe extern "C" fn vector_subtract(
     ((*vec_ptr_1) - (*vec_ptr_2)).to_raw_pointer()
 }
 
+/// Computes the dot product of two vectors
+/// 
+/// # Safety
+/// 
+/// 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(
     vec_ptr_1: *const Vector3,
@@ -97,6 +190,13 @@ pub unsafe extern "C" fn vector_dot_product(
     (*vec_ptr_1).dot(&*vec_ptr_2)
 }
 
+/// 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]
 pub unsafe extern "C" fn vector_cross_product(
     vec_ptr_1: *mut Vector3,
diff --git a/src/spatialmath/quaternion.rs b/src/spatialmath/quaternion.rs
index 0f6b3ca..3ec7ec3 100644
--- a/src/spatialmath/quaternion.rs
+++ b/src/spatialmath/quaternion.rs
@@ -113,7 +113,7 @@ impl Quaternion {
     /// Returns a normalized copy of the quaternion
     pub fn get_normalized(mut self) -> Self {
         self.normalize();
-        return self
+        self
     }
 
     pub fn scale(&mut self, factor: f64) {
diff --git a/src/spatialmath/vector3.rs b/src/spatialmath/vector3.rs
index 940cc5a..2467359 100644
--- a/src/spatialmath/vector3.rs
+++ b/src/spatialmath/vector3.rs
@@ -27,9 +27,9 @@ impl Vector3 {
         if !self.is_normalized() {
             let norm = self.norm2().sqrt();
             if !approx_eq!(f64, norm, 0.0) {
-                self.x = self.x / norm;
-                self.y = self.y / norm;
-                self.z = self.z / norm;
+                self.x /= norm;
+                self.y /= norm;
+                self.z /= norm;
             }
         }
     }
@@ -51,9 +51,9 @@ impl Vector3 {
     }
 
     pub fn scale(&mut self, factor: f64) {
-        self.x = self.x * factor;
-        self.y = self.y * factor;
-        self.z = self.z * factor;
+        self.x *= factor;
+        self.y *= factor;
+        self.z *= factor;
     }
 
     pub fn get_scaled(&self, factor: f64) -> Self {
@@ -64,8 +64,8 @@ impl Vector3 {
 
     /// Allocates the vector to the heap with a stable memory address and
     /// returns the raw pointer (for use by the FFI interface)
-    pub(crate) fn to_raw_pointer(&self) -> *mut Self {
-        Box::into_raw(Box::new(*self))
+    pub(crate) fn to_raw_pointer(self) -> *mut Self {
+        Box::into_raw(Box::new(self))
     }
 
 }

From 7ea69124998dd5182b78bab2eb75c744c4fc24ea Mon Sep 17 00:00:00 2001
From: Gautham Varadarajan <gautham@viam.com>
Date: Wed, 9 Nov 2022 14:44:42 -0500
Subject: [PATCH 5/6] add a couple more useful functions to the ffi

---
 src/ffi/spatialmath/quaternion.rs | 104 ++++++++++++++++++++++++++++--
 1 file changed, 100 insertions(+), 4 deletions(-)

diff --git a/src/ffi/spatialmath/quaternion.rs b/src/ffi/spatialmath/quaternion.rs
index 261ae11..ee84ae1 100644
--- a/src/ffi/spatialmath/quaternion.rs
+++ b/src/ffi/spatialmath/quaternion.rs
@@ -4,7 +4,10 @@ use libc::c_double;
 use crate::spatialmath::{quaternion::Quaternion, vector3::Vector3};
 
 /// The FFI interface for Quaternion functions and initialization. All public 
-/// functions are meant to be called externally from other languages
+/// 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
+/// from this library
 
 /// Allocates a copy of the quaternion to the heap with a stable memory address and
 /// returns the raw pointer (for use by the FFI interface)
@@ -182,7 +185,7 @@ pub unsafe extern "C" fn quaternion_get_imaginary_vector(quat_ptr: *const Quater
 /// 
 /// # Safety
 /// 
-/// When finished with the underlying quaternion initialized by this function
+/// When finished with the underlying quaternion passed to this function
 /// the caller must remember to free the quaternion memory using the 
 /// free_quaternion_memory FFI function
 #[no_mangle]
@@ -235,7 +238,100 @@ pub unsafe extern "C" fn quaternion_from_euler_angles(roll: f64, pitch: f64, yaw
 #[no_mangle]
 pub unsafe extern "C" fn quaternion_to_euler_angles(quat_ptr: *const Quaternion) -> *const c_double {
     null_pointer_check!(quat_ptr);
-    let quat = *quat_ptr;
-    let euler_angles = quat.to_euler_angles();
+    let euler_angles = (*quat_ptr).to_euler_angles();
     Box::into_raw(Box::new(euler_angles)) as *const _
 }
+
+/// 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 
+/// free_quaternion_memory FFI function
+#[no_mangle]
+pub unsafe extern "C" fn scale_quaternion(quat_ptr: *mut Quaternion, factor: f64) {
+    null_pointer_check!(quat_ptr);
+    (*quat_ptr).scale(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 
+/// 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 {
+    null_pointer_check!(quat_ptr);
+    to_raw_pointer(&(*quat_ptr).get_scaled(factor))
+}
+
+/// 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 
+/// using the free_quaternion_memory FFI function
+#[no_mangle]
+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 
+/// 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]
+pub unsafe extern "C" fn quaternion_add(
+    quat_ptr_1: *const Quaternion,
+    quat_ptr_2: *const Quaternion,
+) -> *mut Quaternion {
+    null_pointer_check!(quat_ptr_1);
+    null_pointer_check!(quat_ptr_2);
+    to_raw_pointer(&((*quat_ptr_1) + (*quat_ptr_2)))
+}
+
+/// 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]
+pub unsafe extern "C" fn quaternion_subtract(
+    quat_ptr_1: *const Quaternion,
+    quat_ptr_2: *const Quaternion,
+) -> *mut Quaternion {
+    null_pointer_check!(quat_ptr_1);
+    null_pointer_check!(quat_ptr_2);
+    to_raw_pointer(&((*quat_ptr_1) - (*quat_ptr_2)))
+}
+
+/// 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]
+pub unsafe extern "C" fn quaternion_hamiltonian_product(
+    quat_ptr_1: *const Quaternion,
+    quat_ptr_2: *const Quaternion,
+) -> *mut Quaternion {
+    null_pointer_check!(quat_ptr_1);
+    null_pointer_check!(quat_ptr_2);
+    to_raw_pointer(&((*quat_ptr_1) * (*quat_ptr_2)))
+}

From 7daf22a89ae85edac87b0326204e0eff8fb97227 Mon Sep 17 00:00:00 2001
From: Gautham Varadarajan <gautham@viam.com>
Date: Thu, 10 Nov 2022 15:21:05 -0500
Subject: [PATCH 6/6] adjust comment and add free function for euler angles

---
 src/ffi/spatialmath/quaternion.rs | 20 +++++++++++++++++---
 src/spatialmath/quaternion.rs     |  3 ++-
 2 files changed, 19 insertions(+), 4 deletions(-)

diff --git a/src/ffi/spatialmath/quaternion.rs b/src/ffi/spatialmath/quaternion.rs
index ee84ae1..8af7eec 100644
--- a/src/ffi/spatialmath/quaternion.rs
+++ b/src/ffi/spatialmath/quaternion.rs
@@ -57,6 +57,19 @@ pub unsafe extern "C" fn free_quaternion_memory(ptr: *mut Quaternion) {
     let _ = Box::from_raw(ptr);
 }
 
+/// Free memory at the address of the euler angles pointer. Outer processes
+/// that work with euler angles returned by this interface MUST remember 
+/// to call this function when finished with the list of doubles
+/// 
+/// # Safety
+#[no_mangle]
+pub unsafe extern "C" fn free_euler_angles_memory(ptr: *mut c_double) {
+    if ptr.is_null() {
+        return;
+    }
+    let _ = Box::from_raw(ptr);
+}
+
 /// Get the components of a quaternion as a list of C doubles, the order of the
 /// components will be (real, i, j, k).
 /// 
@@ -234,12 +247,13 @@ pub unsafe extern "C" fn quaternion_from_euler_angles(roll: f64, pitch: f64, yaw
 /// 
 /// When finished with the underlying quaternion passed to this function
 /// the caller must remember to free the quaternion memory using the 
-/// free_quaternion_memory FFI function
+/// free_quaternion_memory FFI function and the euler angles memory using
+/// the free_euler_angles memory function
 #[no_mangle]
-pub unsafe extern "C" fn quaternion_to_euler_angles(quat_ptr: *const Quaternion) -> *const c_double {
+pub unsafe extern "C" fn quaternion_to_euler_angles(quat_ptr: *const Quaternion) -> *mut c_double {
     null_pointer_check!(quat_ptr);
     let euler_angles = (*quat_ptr).to_euler_angles();
-    Box::into_raw(Box::new(euler_angles)) as *const _
+    Box::into_raw(Box::new(euler_angles)) as *mut _
 }
 
 /// Scales an existing quaternion stored at the address of 
diff --git a/src/spatialmath/quaternion.rs b/src/spatialmath/quaternion.rs
index 3ec7ec3..4afa660 100644
--- a/src/spatialmath/quaternion.rs
+++ b/src/spatialmath/quaternion.rs
@@ -51,7 +51,8 @@ impl Quaternion {
     /// 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). 
-    /// The return value is a list of [roll, pitch, yaw]
+    /// The return value is a list of [roll, pitch, yaw] and all returned angles
+    /// are in the domain of -π/2 to π/2
     pub fn to_euler_angles(&self) -> [f64;3] {
         // get a normalized version of the quaternion
         let quat = self.get_normalized();