use std::f32::consts::FRAC_PI_4;
use glam::{Affine3A, Mat3A, Quat, Vec3A};
const ANGULAR_MOTION_THRESHOLD: f32 = FRAC_PI_4;
#[inline]
pub fn integrate_trans_no_rot(cur_trans: &mut Vec3A, lin_vel: Vec3A, time_step: f32) {
*cur_trans += lin_vel * time_step;
}
pub fn integrate_trans(
cur_trans: &mut Affine3A,
cur_rot: &mut Quat,
lin_vel: Vec3A,
ang_vel: Vec3A,
time_step: f32,
) {
integrate_trans_no_rot(&mut cur_trans.translation, lin_vel, time_step);
let mut angle = ang_vel.length();
if angle * time_step > ANGULAR_MOTION_THRESHOLD {
angle = ANGULAR_MOTION_THRESHOLD / time_step;
}
let half_angle = angle * time_step * 0.5;
let axis = ang_vel
* if angle < 0.001 {
0.5 * time_step - time_step * time_step * time_step * 0.020_833_334 * angle * angle
} else {
half_angle.sin() / angle
};
let dorn = Quat::from_vec4(axis.extend(half_angle.cos()));
*cur_rot = (dorn * *cur_rot).normalize();
cur_trans.matrix3 = Mat3A::from_quat(*cur_rot);
}