use glam::Vec3A;
#[cfg(feature = "native-math")]
use crate::shared::math::{cosf, sinf};
#[cfg(feature = "native-math")]
pub(super) fn steered_right_column(car_up: Vec3A, car_right: Vec3A, steer_angle: f32) -> Vec3A {
let up = car_up;
let half_angle = steer_angle * 0.5;
let sin_scale = if half_angle == 0.0 && up.length_squared() > 0.0 {
half_angle
} else {
sinf(half_angle) / up.length()
};
let cos_half = if half_angle == 0.0 {
1.0
} else {
cosf(half_angle)
};
let qx = up.x * sin_scale;
let qy = up.y * sin_scale;
let qz = up.z * sin_scale;
let qw = cos_half;
let s = 2.0 / (((qx * qx + qy * qy) + qz * qz) + qw * qw);
let xs = qx * s;
let ys = qy * s;
let zs = qz * s;
let wx = qw * xs;
let wy = qw * ys;
let wz = qw * zs;
let xx = qx * xs;
let xy = qx * ys;
let xz = qx * zs;
let yy = qy * ys;
let yz = qy * zs;
let zz = qz * zs;
let col0 = Vec3A::new(1.0 - (yy + zz), xy + wz, xz - wy);
let col1 = Vec3A::new(xy - wz, 1.0 - (xx + zz), yz + wx);
let col2 = Vec3A::new(xz + wy, yz - wx, 1.0 - (xx + yy));
let nr = car_right;
Vec3A::new(
col0.x * nr.x + col1.x * nr.y + col2.x * nr.z,
col0.y * nr.x + col1.y * nr.y + col2.y * nr.z,
col0.z * nr.x + col1.z * nr.y + col2.z * nr.z,
)
}
#[cfg(not(feature = "native-math"))]
#[inline]
pub(super) fn steered_right_column(car_up: Vec3A, car_right: Vec3A, steer_angle: f32) -> Vec3A {
glam::Quat::from_axis_angle(car_up.normalize().into(), steer_angle) * car_right
}