use std::f32::consts::FRAC_PI_4;
use glam::{Mat3A, Quat, Vec3A};
const ANGULAR_MOTION_THRESHOLD: f32 = FRAC_PI_4;
#[must_use]
pub(crate) fn bullet_quat_from_basis_columns(fwd: Vec3A, right: Vec3A, up: Vec3A) -> Quat {
quat_from_basis(fwd, right, up)
}
#[must_use]
pub(crate) fn game_get_rotation_scalar(fwd: Vec3A, right: Vec3A, up: Vec3A) -> Quat {
quat_from_basis(fwd, right, up)
}
fn quat_from_basis(fwd: Vec3A, right: Vec3A, up: Vec3A) -> Quat {
if fwd.is_normalized() && right.is_normalized() && up.is_normalized() {
let quat = Quat::from_mat3a(&Mat3A::from_cols(fwd, right, up));
if quat.is_normalized() {
return quat;
}
return game_quat_normalize_scalar(quat, 0.0);
}
fallback_quat_from_basis(fwd, right, up)
}
fn fallback_quat_from_basis(fwd: Vec3A, right: Vec3A, up: Vec3A) -> Quat {
let axes = [Vec3A::X, Vec3A::Y, Vec3A::Z];
let mut columns = [fwd, right, up];
for (column, axis) in columns.iter_mut().zip(axes) {
*column = if column.is_finite() {
let unit = column.normalize_or_zero();
if unit == Vec3A::ZERO { axis } else { unit }
} else {
axis
};
}
let quat = Quat::from_mat3a(&Mat3A::from_cols(columns[0], columns[1], columns[2]));
if !quat.is_finite() {
return Quat::IDENTITY;
}
game_quat_normalize_scalar(quat, -1.0)
}
#[must_use]
pub(crate) fn game_set_rotation_scalar_columns(q: Quat) -> Mat3A {
Mat3A::from_quat(q)
}
#[allow(clippy::neg_cmp_op_on_partial_ord)]
#[must_use]
pub(crate) fn game_quat_normalize_scalar(q: Quat, min_len_sq: f32) -> Quat {
let len_sq = q.length_squared();
if len_sq == 0.0 || !(len_sq > min_len_sq) {
return Quat::IDENTITY;
}
q.normalize()
}
pub(crate) fn game_integrate_orientation_basis_columns(
fwd: &mut Vec3A,
right: &mut Vec3A,
up: &mut Vec3A,
q_mirror: &mut Quat,
ang_vel_world: Vec3A,
dt: f32,
basis_rot: Option<Quat>,
) {
let mut angle = ang_vel_world.length_squared().sqrt();
if angle * dt > ANGULAR_MOTION_THRESHOLD {
angle = ANGULAR_MOTION_THRESHOLD / dt;
}
let (sin_half, cos_half) = ((angle * dt) * 0.5).sin_cos();
let f = if angle < 0.001 {
dt * 0.5 - (((dt * dt) * dt) * 0.020_833_334 * angle) * angle
} else {
sin_half / angle
};
let rotation = Quat::from_xyzw(
ang_vel_world.x * f,
ang_vel_world.y * f,
ang_vel_world.z * f,
cos_half,
);
let o = basis_rot.unwrap_or_else(|| game_get_rotation_scalar(*fwd, *right, *up));
*q_mirror = game_quat_normalize_scalar(rotation * o, -1.0);
let columns = game_set_rotation_scalar_columns(*q_mirror);
*fwd = columns.x_axis;
*right = columns.y_axis;
*up = columns.z_axis;
}