use crate::convert::*;
fn mul(q: Quat, p: Quat) -> Quat {
Quat::from_xyzw(
p.w * q.x + p.x * q.w - p.y * q.z + p.z * q.y,
p.w * q.y + p.x * q.z + p.y * q.w - p.z * q.x,
p.w * q.z - p.x * q.y + p.y * q.x + p.z * q.w,
p.w * q.w - p.x * q.x - p.y * q.y - p.z * q.z,
)
}
fn divide(q: Quat, s: f32) -> Quat {
Quat::from_xyzw(q.x / s, q.y / s, q.z / s, q.w / s)
}
use glam::{Quat, Vec3};
pub fn quat_abs(x: Quat) -> Quat {
if x.w < 0.0 { -x } else { x }
}
pub fn quat_to_scaled_angle_axis(q: Quat, eps: f32) -> Vec3 {
let xyz = Vec3::new(q.x, q.y, q.z);
let length = (q.x * q.x + q.y * q.y + q.z * q.z).sqrt();
if length < eps {
xyz * 2.0
} else {
Vec3::new(xyz.x / length, xyz.y / length, xyz.z / length)
* q.w.clamp(-1.0, 1.0).acos()
* 2.0
}
}
pub fn quat_from_scaled_angle_axis(v: Vec3, eps: f32) -> Quat {
let v = v / 2.0;
let halfangle = (v.x * v.x + v.y * v.y + v.z * v.z).sqrt();
if halfangle < eps {
let q = Quat::from_xyzw(v.x, v.y, v.z, 1.0);
divide(q, (1.0 + v.x * v.x + v.y * v.y + v.z * v.z).sqrt() + eps)
} else {
let s = halfangle.sin() / halfangle;
Quat::from_xyzw(s * v.x, s * v.y, s * v.z, halfangle.cos())
}
}
fn shortest_approx(q: Quat, mut p: Quat, alpha: f32) -> Quat {
let ca = q.w * p.w + q.x * p.x + q.y * p.y + q.z * p.z;
if ca < 0.0 {
p = -p;
}
let d = ca.abs();
let a = 1.0904 + d * (-3.2452 + d * (3.55645 - d * 1.43519));
let b = 0.848013 + d * (-1.06021 + d * 0.215638);
let k = a * (alpha - 0.5) * (alpha - 0.5) + b;
let oalpha = alpha + alpha * (alpha - 0.5) * (alpha - 1.0) * k;
let r = q * (1.0 - oalpha) + p * oalpha;
divide(
r,
(r.w * r.w + r.x * r.x + r.y * r.y + r.z * r.z).sqrt() + 1e-8,
)
}
pub fn damper_exact_quat(x: Quat, g: Quat, halflife: f32, dt: f32, eps: f32) -> Quat {
if dt <= 0.0 {
return x;
}
shortest_approx(
x,
g,
1.0 - fast_negexp((0.69314718056 * dt) / (halflife + eps)),
)
}
pub fn damp_adjustment_exact_quat(g: Quat, halflife: f32, dt: f32, eps: f32) -> Quat {
damper_exact_quat(Quat::IDENTITY, g, halflife, dt, eps)
}
pub fn simple_spring_damper_exact_quat(
x: &mut Quat,
v: &mut Vec3,
x_goal: Quat,
halflife: f32,
dt: f32,
) {
if dt <= 0.0 {
return;
}
let y = halflife_to_damping(halflife, DEFAULT_EPS) / 2.0;
let j0 = quat_to_scaled_angle_axis(quat_abs(mul(*x, -x_goal.conjugate())), 1e-8);
let j1 = *v + j0 * y;
let eydt = fast_negexp(y * dt);
*x = mul(
quat_from_scaled_angle_axis((j0 + j1 * dt) * eydt, 1e-8),
x_goal,
);
*v = (*v - j1 * y * dt) * eydt;
}
pub fn decay_spring_damper_exact_quat(x: &mut Quat, v: &mut Vec3, halflife: f32, dt: f32) {
if dt <= 0.0 {
return;
}
let y = halflife_to_damping(halflife, DEFAULT_EPS) / 2.0;
let j0 = quat_to_scaled_angle_axis(*x, 1e-8);
let j1 = *v + j0 * y;
let eydt = fast_negexp(y * dt);
*x = quat_from_scaled_angle_axis((j0 + j1 * dt) * eydt, 1e-8);
*v = (*v - j1 * y * dt) * eydt;
}
pub fn inertialize_transition_quat(
off_x: &mut Quat,
off_v: &mut Vec3,
src_x: Quat,
src_v: Vec3,
dst_x: Quat,
dst_v: Vec3,
) {
*off_x = quat_abs(mul(mul(*off_x, src_x), -dst_x.conjugate()));
*off_v = (*off_v + src_v) - dst_v;
}
pub fn inertialize_update_quat(
out_x: &mut Quat,
out_v: &mut Vec3,
off_x: &mut Quat,
off_v: &mut Vec3,
in_x: Quat,
in_v: Vec3,
halflife: f32,
dt: f32,
) {
decay_spring_damper_exact_quat(off_x, off_v, halflife, dt);
*out_x = mul(*off_x, in_x);
*out_v = *off_v + *off_x * in_v;
}