viam_rust_utils/ffi/spatialmath/
rotation_matrix.rs

1use ffi_helpers::null_pointer_check;
2use nalgebra::{Matrix3, Quaternion, Rotation3, UnitQuaternion};
3
4/// The FFI interface wrapper around the nalgebra crate for RotationMatrix functions
5/// and initialization. All public functions are meant to be called externally
6/// from other languages. These are 3D rotations (so members of SO(3))
7
8/// Allocates a copy of the rotation matrix to the heap with a stable memory address and
9/// returns the raw pointer (for use by the FFI interface)
10fn to_raw_pointer(rot: &Rotation3<f64>) -> *mut Rotation3<f64> {
11    Box::into_raw(Box::new(*rot))
12}
13
14/// Free memory at the address of the rotation matrix pointer. Outer processes
15/// that work with RotationMatrices via the FFI interface MUST remember
16/// to call this function when finished with a rotation matrix
17///
18/// # Safety
19#[no_mangle]
20pub unsafe extern "C" fn free_rotation_matrix_memory(ptr: *mut Rotation3<f64>) {
21    if ptr.is_null() {
22        return;
23    }
24    let _ = Box::from_raw(ptr);
25}
26
27/// Initialize a 3D rotation matrix from raw components and retrieve the C pointer
28/// to its address. This function DOES NOT check whether the matrix elements provided
29/// form a valid member of SO(3)
30///
31/// # Safety
32///
33/// When finished with the underlying rotation matrix initialized by this function
34/// the caller must remember to free the rotation matrix memory using the
35/// free_rotation_matrix_memory FFI function
36#[no_mangle]
37pub unsafe extern "C" fn new_rotation_matrix(elements: *const [f64; 9]) -> *mut Rotation3<f64> {
38    null_pointer_check!(elements);
39    let matrix = Matrix3::from_vec(Vec::from(*elements));
40    let rot = Rotation3::from_matrix_unchecked(matrix);
41    to_raw_pointer(&rot)
42}
43
44/// Converts a quaternion into a 3D rotation matrix (a Rotation<f64, 3>
45/// from the nalgebra crate)
46///
47/// # Safety
48///
49/// When finished with the underlying quaternion passed to this function
50/// the caller must remember to free the quaternion memory using the
51/// free_quaternion_memory FFI function and the rotation matrix memory using
52/// the free_rotation_matrix_memory function
53#[no_mangle]
54pub unsafe extern "C" fn rotation_matrix_from_quaternion(
55    quat: *const Quaternion<f64>,
56) -> *mut Rotation3<f64> {
57    null_pointer_check!(quat);
58    let unit_quat = UnitQuaternion::new_normalize(*quat);
59    let rot = unit_quat.to_rotation_matrix();
60    to_raw_pointer(&rot)
61}
viam_rust_utils/ffi/spatialmath/rotation_matrix.rs

viam_rust_utils/ffi/spatialmath/
rotation_matrix.rs
