Cansat
/
drone_controller
2
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

change apply rot and cmath in general

This commit is contained in:
franchioping 2026-03-18 15:47:28 +00:00
parent c3a3a356ed
commit 13e27fe725
1 changed files with 17 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
src/cmath.rs
View File

@ -36,16 +36,25 @@ impl From<na::Vector3<f32>> for Vec3C {
} }
} }
#[unsafe(no_mangle)] #[unsafe(no_mangle)]
pub extern "C" fn apply_rot(vec_ptr: *const Vec3C, quat_ptr: *const QuatC) -> Vec3C { pub extern "C" fn apply_rot(vec_ptr: *const Vec3C, quat_ptr: *const QuatC) -> Vec3C {
assert!(!vec_ptr.is_null()); if vec_ptr.is_null() || quat_ptr.is_null() {
assert!(!quat_ptr.is_null()); return Vec3C { x: 0.0, y: 0.0, z: 0.0 };
}
unsafe { unsafe {
let quat: na::UnitQuaternion<f32> = std::ptr::read(quat_ptr).into(); let v_raw = &*vec_ptr;
let v: na::Vector3<f32> = std::ptr::read(vec_ptr).into(); let q_raw = &*quat_ptr;
return quat.transform_vector(&v).into();
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 }
} }
} }