use libm::sqrtf;
use crate::basic_types::Vector3WithYaw;
#[must_use]
pub fn travel_time_for_distance(distance: f32, max_speed: f32, acceleration: f32) -> f32 {
if distance < 0.0 || max_speed <= 0.0 || acceleration <= 0.0 {
return f32::INFINITY;
}
if distance == 0.0 {
return 0.0;
}
if acceleration == f32::INFINITY {
return distance / max_speed;
}
let t1 = max_speed / acceleration;
let s1 = acceleration / 2.0 * t1 * t1;
if distance >= 2.0 * s1 {
let t2 = (distance - 2.0 * s1) / max_speed;
2.0 * t1 + t2
} else {
2.0 * sqrtf(distance / acceleration)
}
}
#[must_use]
pub fn travel_velocity_for_distance(distance: f32, time: f32, acceleration: f32) -> f32 {
if distance < 0.0 || time <= 0.0 || acceleration <= 0.0 {
return f32::INFINITY;
}
if distance == 0.0 {
return 0.0;
}
if acceleration == f32::INFINITY {
return distance / time;
}
let d = time * time - 4.0 * distance / acceleration;
if d >= 0.0 {
-(sqrtf(d) - time) / 2.0 * acceleration
} else {
f32::INFINITY
}
}
#[must_use]
pub fn cubic_bezier_from_velocity_constraints(
start: Vector3WithYaw,
start_vel: Vector3WithYaw,
end: Vector3WithYaw,
end_vel: Vector3WithYaw,
duration_sec: f32,
) -> Option<(Vector3WithYaw, Vector3WithYaw)> {
if duration_sec <= 0.0 {
return None;
}
let scale = duration_sec / 3.0;
Some((start + scale * start_vel, end - scale * end_vel))
}