viam_rust_utils/ffi/spatialmath/
orientation_vector.rs

1use ffi_helpers::null_pointer_check;
2use libc::c_double;
3use nalgebra::Quaternion;
4
5use crate::spatialmath::utils::OrientationVector;
6
7/// The FFI Interface for initialization of Viam's Orientation Vector format.
8/// Like an axis angle, the format involves a vector axis and a rotation
9/// theta (in radians). However, unlike an axis-angle, an orientation vector alters
10/// the axes of the given frame of reference by rotating the z-axis to the
11/// vector axis provided. The new x-axis is the vector that is both orthogonal to
12/// the vector axis provided and co-planar with both the old
13/// z-axis and the vector axis (this leaves two choices for the y-axis,
14/// but the canonical "right-hand rule" is used to select one consistently). Then,
15/// a clockwise-rotation of theta is applied about the new-z axis
16///
17/// It is highly recommended not to attempt any mathematics with the orientation
18/// vector directly and to convert to quaternions via the FFI interface instead
19
20/// Allocates a copy of the orientation vector to the heap with a stable memory address and
21/// returns the raw pointer (for use by the FFI interface)
22fn to_raw_pointer(o_vec: &OrientationVector) -> *mut OrientationVector {
23    Box::into_raw(Box::new(*o_vec))
24}
25
26/// Free memory at the address of the orientation vector pointer. Outer processes
27/// that work with OrientationVectors via the FFI interface MUST remember
28/// to call this function when finished with a OrientationVector instance
29///
30/// # Safety
31#[no_mangle]
32pub unsafe extern "C" fn free_orientation_vector_memory(ptr: *mut OrientationVector) {
33    if ptr.is_null() {
34        return;
35    }
36    let _ = Box::from_raw(ptr);
37}
38
39/// Initialize an orientation vector from raw components and retrieve the C pointer
40/// to its address.
41///
42/// # Safety
43///
44/// When finished with the underlying orientation vector initialized by this function
45/// the caller must remember to free the orientation vector memory using the
46/// free_orientation_vector_memory FFI function
47#[no_mangle]
48pub unsafe extern "C" fn new_orientation_vector(
49    o_x: f64,
50    o_y: f64,
51    o_z: f64,
52    theta: f64,
53) -> *mut OrientationVector {
54    let o_vec = OrientationVector::new(o_x, o_y, o_z, theta);
55    to_raw_pointer(&o_vec)
56}
57
58/// Get the components of an orientation vector as a list of C doubles, the order of the
59/// components will be (o_x, o_y, o_z, theta).
60///
61/// # Safety
62///
63/// When finished with the underlying orientation_vector passed to this function
64/// the caller must remember to free the orientation_vector memory using the
65/// free_orientation_vector_memory FFI function
66#[no_mangle]
67pub unsafe extern "C" fn orientation_vector_get_components(
68    ov_ptr: *const OrientationVector,
69) -> *const c_double {
70    null_pointer_check!(ov_ptr);
71    let components: [c_double; 4] = [
72        (&(*ov_ptr)).o_vector.x,
73        (&(*ov_ptr)).o_vector.y,
74        (&(*ov_ptr)).o_vector.z,
75        (&(*ov_ptr)).theta,
76    ];
77    Box::into_raw(Box::new(components)) as *const _
78}
79
80/// Converts a quaternion into an orientation vector.
81///
82/// # Safety
83///
84/// When finished with the underlying quaternion passed to this function
85/// the caller must remember to free the quaternion memory using the
86/// free_quaternion_memory FFI function and the orientation-vector memory using
87/// the free_orientation_vector_memory function
88#[no_mangle]
89pub unsafe extern "C" fn orientation_vector_from_quaternion(
90    quat_ptr: *const Quaternion<f64>,
91) -> *mut OrientationVector {
92    null_pointer_check!(quat_ptr);
93    let o_vec: OrientationVector = (*quat_ptr).into();
94    to_raw_pointer(&o_vec)
95}
96
97/// Free memory of an array of orientation vector components at the given address.
98///
99/// # Safety
100///
101/// Outer processes that request the components of a orientation vector should call this function
102/// to free the memory allocated to the array once finished
103#[no_mangle]
104pub unsafe extern "C" fn free_orientation_vector_components(ptr: *mut c_double) {
105    if ptr.is_null() {
106        return;
107    }
108
109    let ptr = ptr as *mut [c_double; 4];
110    let _: Box<[c_double; 4]> = Box::from_raw(ptr);
111}
viam_rust_utils/ffi/spatialmath/orientation_vector.rs

viam_rust_utils/ffi/spatialmath/
orientation_vector.rs
