use crate::as_entity::AsEntity;
pub fn soft_body<T>(p1: &T, p2: &T, stiffness: f64) -> (f64, f64, f64)
where
T: AsEntity,
{
let p1 = p1.as_entity();
let p2 = p2.as_entity();
let distance = p1.distance(&p2);
let radii_sum = p1.radius + p2.radius;
if distance >= radii_sum {
return (p1.vx, p1.vy, p1.vz);
}
let overlap = radii_sum - distance;
let force = stiffness * overlap;
let acceleration_scalar = force / p1.mass;
let (unit_x, unit_y, unit_z) = unit_vector(&p2.distance_vector(&p1));
(
unit_x * acceleration_scalar,
unit_y * acceleration_scalar,
unit_z * acceleration_scalar,
)
}
fn unit_vector(vec: &(f64, f64, f64)) -> (f64, f64, f64) {
let (x, y, z) = vec;
let length = f64::sqrt((x * x) + (y * y) + (z * z));
(x / length, y / length, z / length)
}