use glam::Vec3A;
use crate::CarState;
#[cfg(feature = "native-math")]
#[inline]
#[must_use]
pub(super) fn body_axes(state: &CarState) -> (Vec3A, Vec3A, Vec3A) {
fixed::axes(state.phys.rot_mat)
}
#[cfg(not(feature = "native-math"))]
#[inline]
#[must_use]
pub(super) fn body_axes(state: &CarState) -> (Vec3A, Vec3A, Vec3A) {
let m = state.phys.rot_mat;
(m.x_axis, m.y_axis, m.z_axis)
}
#[cfg(feature = "native-math")]
mod fixed {
use glam::{Mat3A, Vec3A};
use crate::shared::{math::atan2f, round_ties_even, ucrt_math::ucrt_sinf};
const PI_F32: f32 = f32::from_bits(0x4049_0fdb);
const QUARTER_TURN: i32 = 0x4000;
#[inline]
fn rotator_sin(angle: i32) -> f32 {
let j = ((angle >> 2) & 0x3fff) as f32;
ucrt_sinf(((j + j) * PI_F32) * (1.0 / 16384.0))
}
#[inline]
fn rotator_units(y: f32, x: f32) -> i32 {
let units = (atan2f(y, x) * 32768.0) / PI_F32;
(round_ties_even(units + units + 0.5) as i32) >> 1
}
#[must_use]
pub(super) fn units(m: Mat3A) -> (i32, i32, i32) {
let forward = m.x_axis;
let right = m.y_axis;
let up = m.z_axis;
let yaw = rotator_units(forward.y, forward.x);
let forward_xy = f64::from(forward.y * forward.y + forward.x * forward.x).sqrt() as f32;
let pitch = rotator_units(forward.z, forward_xy);
let sy = rotator_sin(yaw);
let cy = rotator_sin(yaw + QUARTER_TURN);
let z_dot = (up.y * cy + up.x * -sy) + up.z * -0.0;
let y_dot = (right.y * cy + right.x * -sy) + right.z * -0.0;
let roll = rotator_units(z_dot, y_dot);
(pitch, yaw, roll)
}
#[must_use]
pub(super) fn axes_from_units(pitch: i32, yaw: i32, roll: i32) -> (Vec3A, Vec3A, Vec3A) {
let sp = rotator_sin(pitch);
let cp = rotator_sin(pitch + QUARTER_TURN);
let sy = rotator_sin(yaw);
let cy = rotator_sin(yaw + QUARTER_TURN);
let sr = rotator_sin(roll);
let cr = rotator_sin(roll + QUARTER_TURN);
let sp_sr = sp * sr;
let cr_sp = cr * sp;
let x = Vec3A::new(cy * cp, cp * sy, sp);
let y = Vec3A::new(sp_sr * cy - cr * sy, cy * cr + sp_sr * sy, -sr * cp);
let z = Vec3A::new(-(cr_sp * cy + sy * sr), cy * sr - cr_sp * sy, cp * cr);
(x, y, z)
}
#[inline]
#[must_use]
pub(super) fn axes(m: Mat3A) -> (Vec3A, Vec3A, Vec3A) {
let (pitch, yaw, roll) = units(m);
axes_from_units(pitch, yaw, roll)
}
}