use std::f32::consts::FRAC_PI_4;
use glam::{Mat3A, Quat, Vec3A};
use crate::shared::math::{cosf, sinf};
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 {
let m00 = fwd.x;
let m01 = right.x;
let m02 = up.x;
let m10 = fwd.y;
let m11 = right.y;
let m12 = up.y;
let m20 = fwd.z;
let m21 = right.z;
let m22 = up.z;
let trace = m00 + m11 + m22;
let (raw0, raw1, raw2, raw3, x);
if trace > 0.0 {
x = trace + 1.0;
raw0 = m21 - m12;
raw1 = m02 - m20;
raw2 = m10 - m01;
raw3 = x;
} else if m00 < m11 {
if m11 < m22 {
x = m22 - m00 - m11 + 1.0;
raw0 = m02 + m20;
raw1 = m12 + m21;
raw2 = x;
raw3 = m10 - m01;
} else {
x = m11 - m22 - m00 + 1.0;
raw0 = m01 + m10;
raw1 = x;
raw2 = m21 + m12;
raw3 = m02 - m20;
}
} else if m00 < m22 {
x = m22 - m00 - m11 + 1.0;
raw0 = m02 + m20;
raw1 = m12 + m21;
raw2 = x;
raw3 = m10 - m01;
} else {
x = m00 - m11 - m22 + 1.0;
raw0 = x;
raw1 = m10 + m01;
raw2 = m20 + m02;
raw3 = m21 - m12;
}
let s = x.sqrt();
let scale = 0.5 / s;
Quat::from_xyzw(raw0 * scale, raw1 * scale, raw2 * scale, raw3 * scale)
}
#[must_use]
pub(crate) fn game_get_rotation_scalar(fwd: Vec3A, right: Vec3A, up: Vec3A) -> Quat {
let m00 = fwd.x;
let m01 = right.x;
let m02 = up.x;
let m10 = fwd.y;
let m11 = right.y;
let m12 = up.y;
let m20 = fwd.z;
let m21 = right.z;
let m22 = up.z;
let trace = (m00 + m11) + m22;
if trace > 0.0 {
let mut s = (trace + 1.0).sqrt();
let w = s * 0.5;
s = 0.5 / s;
return Quat::from_xyzw((m21 - m12) * s, (m02 - m20) * s, (m10 - m01) * s, w);
}
if if m00 < m11 { m11 < m22 } else { m00 < m22 } {
let mut s = (((m22 - m00) - m11) + 1.0).sqrt();
let z = s * 0.5;
s = 0.5 / s;
return Quat::from_xyzw((m02 + m20) * s, (m12 + m21) * s, z, (m10 - m01) * s);
}
if m00 < m11 {
let mut s = (((m11 - m22) - m00) + 1.0).sqrt();
let y = s * 0.5;
s = 0.5 / s;
return Quat::from_xyzw((m01 + m10) * s, y, (m21 + m12) * s, (m02 - m20) * s);
}
let mut s = (((m00 - m11) - m22) + 1.0).sqrt();
let x = s * 0.5;
s = 0.5 / s;
Quat::from_xyzw(x, (m10 + m01) * s, (m20 + m02) * s, (m21 - m12) * s)
}
#[must_use]
pub(crate) fn game_set_rotation_scalar_columns(q: Quat) -> Mat3A {
let d = ((q.x * q.x + q.y * q.y) + q.z * q.z) + q.w * q.w;
let s = 2.0 / d;
let xs = q.x * s;
let ys = q.y * s;
let zs = q.z * s;
let wx = q.w * xs;
let wy = q.w * ys;
let wz = q.w * zs;
let xx = q.x * xs;
let xy = q.x * ys;
let xz = q.x * zs;
let yy = q.y * ys;
let yz = q.y * zs;
let zz = q.z * zs;
Mat3A::from_cols(
Vec3A::new(1.0 - (yy + zz), xy + wz, xz - wy),
Vec3A::new(xy - wz, 1.0 - (xx + zz), yz + wx),
Vec3A::new(xz + wy, yz - wx, 1.0 - (xx + yy)),
)
}
#[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.x * q.x + q.y * q.y) + q.z * q.z) + q.w * q.w;
if !(len_sq > min_len_sq) {
return Quat::IDENTITY;
}
let s = 1.0 / len_sq.sqrt();
Quat::from_xyzw(q.x * s, q.y * s, q.z * s, q.w * s)
}
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 angle_sq = (ang_vel_world.x * ang_vel_world.x + ang_vel_world.y * ang_vel_world.y)
+ ang_vel_world.z * ang_vel_world.z;
let mut angle = angle_sq.sqrt();
if angle * dt > ANGULAR_MOTION_THRESHOLD {
angle = ANGULAR_MOTION_THRESHOLD / dt;
}
let f = if angle < 0.001 {
dt * 0.5 - (((dt * dt) * dt) * 0.020_833_334 * angle) * angle
} else {
let half_angle = (angle * 0.5) * dt;
sinf(half_angle) / angle
};
let ax = ang_vel_world.x * f;
let ay = ang_vel_world.y * f;
let az = ang_vel_world.z * f;
let c = cosf((angle * dt) * 0.5);
let o = basis_rot.unwrap_or_else(|| game_get_rotation_scalar(*fwd, *right, *up));
let p = Quat::from_xyzw(
((ax * o.w + c * o.x) + ay * o.z) - az * o.y,
((c * o.y + ay * o.w) + az * o.x) - ax * o.z,
((c * o.z + az * o.w) + ax * o.y) - ay * o.x,
((c * o.w - ax * o.x) - ay * o.y) - az * o.z,
);
*q_mirror = game_quat_normalize_scalar(p, -1.0);
let columns = game_set_rotation_scalar_columns(*q_mirror);
*fwd = columns.x_axis;
*right = columns.y_axis;
*up = columns.z_axis;
}