use crate::convert::*;
use glam::{Vec2, Vec3, Vec3A, Vec4};
use std::ops::{Add, Mul, Sub};
pub trait SpringValue:
Copy + Add<Output = Self> + Sub<Output = Self> + Mul<f32, Output = Self> + Default
{
fn div_scalar(self, rhs: f32) -> Self {
self * (1.0 / rhs)
}
}
impl SpringValue for f32 {
fn div_scalar(self, rhs: f32) -> Self {
self / rhs
}
}
impl SpringValue for Vec2 {
fn div_scalar(self, rhs: f32) -> Self {
Self::from_array(self.to_array().map(|x| x / rhs))
}
}
impl SpringValue for Vec3 {
fn div_scalar(self, rhs: f32) -> Self {
Self::from_array(self.to_array().map(|x| x / rhs))
}
}
impl SpringValue for Vec3A {
fn div_scalar(self, rhs: f32) -> Self {
Self::from_array(self.to_array().map(|x| x / rhs))
}
}
impl SpringValue for Vec4 {
fn div_scalar(self, rhs: f32) -> Self {
Self::from_array(self.to_array().map(|x| x / rhs))
}
}
pub fn damper_exact_vec<T: SpringValue>(x: T, g: T, halflife: f32, dt: f32, eps: f32) -> T {
if dt <= 0.0 {
return x;
}
let a = 1.0 - fast_negexp((0.69314718056 * dt) / (halflife + eps));
x * (1.0 - a) + g * a
}
pub fn simple_spring_damper_exact_vec<T: SpringValue>(
x: &mut T,
v: &mut T,
x_goal: T,
halflife: f32,
dt: f32,
) {
if dt <= 0.0 {
return;
}
let y = halflife_to_damping(halflife, DEFAULT_EPS) / 2.0;
let j0 = *x - x_goal;
let j1 = *v + j0 * y;
let eydt = fast_negexp(y * dt);
*x = (j0 + j1 * dt) * eydt + x_goal;
*v = (*v - j1 * y * dt) * eydt;
}
pub fn critical_spring_damper_exact_vec<T: SpringValue>(
x: &mut T,
v: &mut T,
x_goal: T,
v_goal: T,
halflife: f32,
dt: f32,
) {
if dt <= 0.0 {
return;
}
let d = halflife_to_damping(halflife, DEFAULT_EPS);
let c = x_goal + (v_goal * d).div_scalar((d * d) / 4.0);
let y = d / 2.0;
let j0 = *x - c;
let j1 = *v + j0 * y;
let eydt = fast_negexp(y * dt);
*x = (j0 + j1 * dt) * eydt + c;
*v = (*v - j1 * y * dt) * eydt;
}
pub fn decay_spring_damper_exact_vec<T: SpringValue>(x: &mut T, v: &mut T, halflife: f32, dt: f32) {
if dt <= 0.0 {
return;
}
let y = halflife_to_damping(halflife, DEFAULT_EPS) / 2.0;
let j1 = *v + *x * y;
let eydt = fast_negexp(y * dt);
*x = (*x + j1 * dt) * eydt;
*v = (*v - j1 * y * dt) * eydt;
}
pub fn inertialize_transition_vec<T: SpringValue>(
off_x: &mut T,
off_v: &mut T,
src_x: T,
src_v: T,
dst_x: T,
dst_v: T,
) {
*off_x = (src_x + *off_x) - dst_x;
*off_v = (src_v + *off_v) - dst_v;
}
pub fn inertialize_update_vec<T: SpringValue>(
out_x: &mut T,
out_v: &mut T,
off_x: &mut T,
off_v: &mut T,
in_x: T,
in_v: T,
halflife: f32,
dt: f32,
) {
decay_spring_damper_exact_vec(off_x, off_v, halflife, dt);
*out_x = in_x + *off_x;
*out_v = in_v + *off_v;
}
pub fn extrapolate_vec<T: SpringValue>(x: &mut T, v: &mut T, dt: f32, halflife: f32, eps: f32) {
if dt <= 0.0 {
return;
}
let y = 0.69314718056 / (halflife + eps);
*x = *x + (*v).div_scalar(y + eps) * (1.0 - fast_negexp(y * dt));
*v = *v * fast_negexp(y * dt);
}