use crate::trajectory::Trajectory;
use crate::vector::Vector3;
use std::f64::consts::PI;
const GRAVITY: f64 = 9.81; const FAIRWAY_FRICTION_COEFF: f64 = 0.18; const ROLL_EFFICIENCY: f64 = 0.85; const ROLL_SCALING_COEFF: f64 = 0.15;
pub fn get_landing_position(trajectory: &Trajectory) -> Vector3 {
trajectory
.points
.last()
.map(|p| p.position())
.unwrap_or(Vector3::new(f64::NAN, f64::NAN, f64::NAN))
}
pub fn get_landing_velocity(trajectory: &Trajectory) -> Vector3 {
trajectory
.points
.last()
.map(|p| p.velocity())
.unwrap_or(Vector3::new(f64::NAN, f64::NAN, f64::NAN))
}
pub fn get_hang_time(trajectory: &Trajectory) -> f64 {
trajectory.points.last().map(|p| p.t).unwrap_or(f64::NAN)
}
pub fn get_apex_position(trajectory: &Trajectory) -> Vector3 {
trajectory
.points
.iter()
.max_by(|a, b| a.z.partial_cmp(&b.z).unwrap())
.map(|p| p.position())
.unwrap_or(Vector3::new(f64::NAN, f64::NAN, f64::NAN))
}
pub fn get_time_to_apex(trajectory: &Trajectory) -> f64 {
trajectory
.points
.iter()
.max_by(|a, b| a.z.partial_cmp(&b.z).unwrap())
.map(|p| p.t)
.unwrap_or(f64::NAN)
}
pub fn get_peak_height(trajectory: &Trajectory) -> f64 {
trajectory
.points
.iter()
.map(|p| p.z)
.max_by(|a, b| a.partial_cmp(b).unwrap())
.unwrap_or(f64::NAN)
}
pub fn get_descent_angle(trajectory: &Trajectory) -> f64 {
let landing_vel = get_landing_velocity(trajectory);
let horizontal_speed = (landing_vel.x.powi(2) + landing_vel.y.powi(2)).sqrt();
let descent_angle_rad = (-landing_vel.z).atan2(horizontal_speed);
descent_angle_rad * 180.0 / PI
}
pub fn get_carry_distance(trajectory: &Trajectory) -> f64 {
let landing_pos = get_landing_position(trajectory);
landing_pos.magnitude()
}
pub fn get_offline_distance(trajectory: &Trajectory) -> f64 {
let landing_pos = get_landing_position(trajectory);
landing_pos.y
}
pub fn get_total_distance(trajectory: &Trajectory) -> f64 {
let carry = get_carry_distance(trajectory);
let landing_pos = get_landing_position(trajectory);
let landing_vel = get_landing_velocity(trajectory);
let horizontal_speed = (landing_vel.x.powi(2) + landing_vel.y.powi(2)).sqrt();
if horizontal_speed <= 0.1 {
return carry;
}
let descent_angle = get_descent_angle(trajectory).clamp(0.0, 90.0);
let descent_factor = ((90.0 - descent_angle) / 90.0).powf(1.4);
let base_roll = horizontal_speed.powi(2) / (2.0 * FAIRWAY_FRICTION_COEFF * GRAVITY);
let mut roll_distance = base_roll * descent_factor * ROLL_EFFICIENCY * ROLL_SCALING_COEFF;
roll_distance = roll_distance.max(0.0);
let mut heading = Vector3::new(landing_vel.x, landing_vel.y, 0.0);
if heading.magnitude() <= 0.01 {
heading = Vector3::new(landing_pos.x, landing_pos.y, 0.0);
}
let heading = heading.normalize();
let roll_vector = Vector3::new(heading.x * roll_distance, heading.y * roll_distance, 0.0);
let total_vector = landing_pos.add(&roll_vector);
total_vector.magnitude()
}