drone_controller/src/cmath.rs

use nalgebra as na;

#[repr(C)]
pub struct Vec3C {
    pub x: f32,
    pub y: f32,
    pub z: f32,
}

#[repr(C)]
pub struct QuatC {
    pub i: f32,
    pub j: f32,
    pub k: f32,
    pub w: f32,
}

impl Into<na::UnitQuaternion<f32>> for QuatC {
    fn into(self) -> na::UnitQuaternion<f32> {
        na::UnitQuaternion::from_quaternion(na::Quaternion::from_parts(
            self.w,
            na::vector![self.i, self.j, self.k],
        ))
    }
}

impl Into<na::Vector3<f32>> for Vec3C {
    fn into(self) -> na::Vector3<f32> {
        na::vector![self.x, self.y, self.z]
    }
}

impl From<na::Vector3<f32>> for Vec3C {
    fn from(value: na::Vector3<f32>) -> Self {
        Vec3C { x: value.x, y: value.y, z: value.z }
    }
}

#[unsafe(no_mangle)]
pub extern "C" fn apply_rot(vec_ptr: *const Vec3C, quat_ptr: *const QuatC) -> Vec3C {
    if vec_ptr.is_null() || quat_ptr.is_null() {
        return Vec3C { x: 0.0, y: 0.0, z: 0.0 };
    }

    unsafe {
        let v_raw = &*vec_ptr;
        let q_raw = &*quat_ptr;

        let v = na::Vector3::new(v_raw.x, v_raw.y, v_raw.z);

        let q = na::UnitQuaternion::from_quaternion(
            na::Quaternion::new(q_raw.w , q_raw.i, q_raw.j, q_raw.k)
        );

        let rotated = q.transform_vector(&v);

        Vec3C { x: rotated.x, y: rotated.y, z: rotated.z }
    }
}

#[unsafe(no_mangle)]
pub extern "C" fn multiply_vec_by(vec_ptr: *mut Vec3C, k: f32) {
    assert!(!vec_ptr.is_null());

    unsafe {
        (*vec_ptr).x *= k;
        (*vec_ptr).y *= k;
        (*vec_ptr).z *= k;
    }
}


#[unsafe(no_mangle)]
pub extern "C" fn add_to_vec(vec_ptr: *mut Vec3C, to_add_ptr: *const Vec3C){
    assert!(!vec_ptr.is_null());
    assert!(!to_add_ptr.is_null());
    unsafe {
        (*vec_ptr).x += (*to_add_ptr).x;
        (*vec_ptr).y += (*to_add_ptr).y;
        (*vec_ptr).z += (*to_add_ptr).z;
    }
}