// flight_model.rs
use crate::aircraft::AircraftConfig;
use crate::consts::PHYSICS_DT;
#[derive(Clone)]
pub struct FlightModel {
// Airplane position in flight model coordinate system
// Note: airplane_x = North direction, airplane_y = East direction
// This differs from standard ENU where X=East, Y=North
pub airplane_x: f64, // North position (feet)
pub airplane_y: f64, // East position (feet)
pub airplane_z: f64, // Altitude (feet)
// Angles
pub side_tilt: f64, // sideTiltRadians in banks.c
pub forward_tilt: f64, // forwardTiltRadians in banks.c
pub compass: f64, // compassRadians in banks.c
// Flight parameters
pub speed: f64, // target speed (throttle setting)
pub speed_feet: f64, // current speed in feet/sec // User inputs
pub left_right: f64, // for turning
pub up_down: f64, // for pitch
// Autopilot system
pub autopilot_enabled: bool,
pub target_altitude: f64,
pub target_heading: f64,
// new state for the PD controller
pub autopilot_prev_hdg_error: f64,
pub autopilot_prev_alt_error: f64,
pub autopilot_filtered_hdg_deriv: f64, // Filtered derivative term to reduce noise
// Waypoint navigation
pub waypoint_enabled: bool,
pub waypoints: Vec,
pub current_waypoint_index: usize,
pub waypoint_target_heading: f64, // The actual bearing to waypoint (for HUD display) // HUD display options
pub show_coordinates: bool, // Toggle coordinate display with X key
pub show_scene_labels: bool, // Toggle scene origin labels with M key
// Global coordinate system (set by event loop during recentering)
pub current_lat: f64,
pub current_lon: f64,
// Reference coordinates for ENU conversion (set by event loop)
pub ref_lat: f64,
pub ref_lon: f64,
engine_count: u32,
power_hp: f64,
mass_slugs: f64,
cd0: f64,
cl_max: f64,
aspect_ratio: f64,
critical_aoa_rad: f64,
stall_pitch_down_rad: f64,
stall_speed_fps: f64,
thrust_lbf: f64,
// Rotation matrix (camera transform)
pub r11: f64, pub r12: f64, pub r13: f64,
pub r21: f64, pub r22: f64, pub r23: f64,
pub r31: f64, pub r32: f64, pub r33: f64,
// Additional physics variables from banks.c
d: f64,
f: f64,
p: f64,
t: f64,
v: f64,
w: f64,
m: f64,
e: f64,
i: f64,
h: f64,
x: f64,
gravity_accel: f64,
s: f64,
}
impl FlightModel {
pub fn new(config: AircraftConfig) -> Self {
let mut model = Self {
airplane_x: 0.0,
airplane_y: 0.0,
airplane_z: config.initial_altitude_ft,
side_tilt: 0.0,
forward_tilt: config.initial_forward_tilt_rad,
compass: 0.0,
speed: 8.0, // Still fixed throttle steps for now
speed_feet: config.initial_speed_fps,
left_right: 0.0,
up_down: 0.0, // Initialize autopilot system
autopilot_enabled: false,
target_altitude: 0.0,
target_heading: 0.0,
autopilot_prev_hdg_error: 0.0,
autopilot_prev_alt_error: 0.0,
autopilot_filtered_hdg_deriv: 0.0,
// Initialize waypoint navigation
waypoint_enabled: false,
waypoints: Vec::new(),
current_waypoint_index: 0,
waypoint_target_heading: 0.0,
// HUD display options
show_coordinates: true, // Enabled by default, toggle with X key
show_scene_labels: true, // Enabled by default, toggle with M key
// Global coordinate system (set by event loop during recentering)
current_lat: 37.6213, // Default SFO coordinates
current_lon: -122.3790,
// Reference coordinates for ENU conversion (set by event loop)
ref_lat: 37.6213, // Default SFO coordinates
ref_lon: -122.3790,
r11: 1.0, r12: 0.0, r13: 0.0,
r21: 0.0, r22: 1.0, r23: 0.0,
r31: 0.0, r32: 0.0, r33: 1.0,
d: 0.0,
f: 0.0,
p: 0.0,
t: 0.0,
v: 0.0,
w: 0.0,
m: 0.0,
e: 0.0,
i: 0.0,
h: 0.0,
x: config.initial_lift_param,
gravity_accel: config.gravity_accel,
s: config.wing_area_ft2,
mass_slugs: config.mass_lb / 32.174,
engine_count: config.engine_count,
// Aircraft config
power_hp: config.power_hp,
cd0: config.cd0,
cl_max: config.cl_max,
aspect_ratio: config.aspect_ratio,
critical_aoa_rad: config.critical_aoa_rad,
stall_pitch_down_rad: config.stall_pitch_down_rad,
stall_speed_fps: config.stall_speed_kts * 1.68781,
thrust_lbf: config.thrust_lbf,
};
model.calculate_angles();
model
}
// Throttle control - increases target speed (directly impacts acceleration)
pub fn increase_throttle(&mut self) {
self.speed += 1.0;
}
// Throttle control - decreases target speed
pub fn decrease_throttle(&mut self) {
self.speed -= 1.0;
if self.speed < 0.0 {
self.speed = 0.0; // Prevent negative speed
}
}
/// Calculate the new orientation angles and rotation matrix
pub fn calculate_angles(&mut self) {
// Trigonometric precomputations
let cos_fwd = self.forward_tilt.cos();
let sin_fwd = self.forward_tilt.sin();
let cos_cmp = self.compass.cos();
let cos_side = self.side_tilt.cos();
let sin_side = self.side_tilt.sin();
let sin_cmp = self.compass.sin();
// Update F (moment arm)
self.f += PHYSICS_DT * self.p;
// Yaw (compass heading)
self.compass += cos_side * PHYSICS_DT * self.f / cos_fwd
+ self.d / cos_fwd * sin_side * PHYSICS_DT;
// Pitch (forward/back tilt)
self.forward_tilt += self.d * PHYSICS_DT * cos_side
- PHYSICS_DT * self.f * sin_side;
// Optional: crude stall behavior (nose drop)
if self.speed_feet < self.stall_speed_fps && self.forward_tilt > self.critical_aoa_rad {
self.forward_tilt = self.stall_pitch_down_rad;
}
// Roll (sideways tilt)
self.side_tilt += (sin_side * self.d / cos_fwd * sin_fwd
+ self.v
+ sin_fwd / cos_fwd * self.f * cos_side) * PHYSICS_DT;
// Build rotation matrix
self.r11 = cos_fwd * cos_cmp;
self.r12 = cos_fwd * sin_cmp;
self.r13 = -sin_fwd;
self.r21 = cos_cmp * sin_side * sin_fwd - sin_cmp * cos_side;
self.r22 = cos_side * cos_cmp + sin_side * sin_cmp * sin_fwd;
self.r23 = sin_side * cos_fwd;
self.r31 = sin_cmp * sin_side + cos_side * sin_fwd * cos_cmp;
self.r32 = sin_fwd * sin_cmp * cos_side - sin_side * cos_cmp;
self.r33 = cos_side * cos_fwd;
}
/// Update physics: momentum, position, forces, and rates
pub fn update_physics(&mut self) {
// 1) Momentum
let local_momentum = self.h * PHYSICS_DT;
self.m += local_momentum;
// 2) Inertia
self.i = self.m / self.speed_feet;
// 3) Position update (world → airplane reference)
self.airplane_x += (self.r11 * self.speed_feet + self.r21 * self.m + self.r31 * self.x) * PHYSICS_DT;
self.airplane_y += (self.r12 * self.speed_feet + self.i * self.m + self.r32 * self.x) * PHYSICS_DT;
self.airplane_z += (-self.r13 * self.speed_feet - self.r23 * self.m - self.r33 * self.x) * PHYSICS_DT;
// 4) Intermediate values
let local_intermediate = 15.0 * self.f / self.speed_feet;
self.e = 0.1 + self.x * 4.9 / self.speed_feet;
self.t = self.x * self.x
+ self.speed_feet * self.speed_feet
+ self.m * self.m; // now using momentum squared
let t_temp = self.t * local_intermediate / 32.0
- self.i * self.t / 24.0;
// 5) H (for next momentum)
self.h = self.gravity_accel * self.r23
+ self.v * self.x
- self.f * self.speed_feet
+ t_temp / self.s;
// 6) Acceleration along fuselage
// Atmospheric constants
let rho = 0.0023769; // slugs/ft³ at sea level
let q = 0.5 * rho * self.speed_feet.powi(2); // dynamic pressure
// Approximate angle of attack from pitch angle (forward_tilt)
let aoa = self.forward_tilt;
// Clamp CL to critical AoA
let cl = if aoa.abs() > self.critical_aoa_rad {
self.cl_max * (1.0 - ((aoa.abs() - self.critical_aoa_rad) / self.critical_aoa_rad).min(1.0))
} else {
self.cl_max * aoa / self.critical_aoa_rad
};
// Drag = q * S * (cd0 + cl² / (π * AR))
let induced_drag = cl.powi(2) / (std::f64::consts::PI * self.aspect_ratio);
// Reduce drag coefficient to make it more realistic - small planes have less drag
let drag_coefficient = self.cd0 + induced_drag * 0.5; // Apply 50% factor to induced drag
let drag_force = q * self.s * drag_coefficient;
// Use the configured thrust directly instead of calculating from power
// Scale with throttle (range 0-10)
let throttle_ratio = (self.speed / 10.0).min(1.0).max(0.0);
// Use direct thrust with throttle control
let thrust_force = self.thrust_lbf * throttle_ratio * self.engine_count as f64;
// We can also calculate additional thrust from power for more realistic engine modeling
// Power = Force * Velocity, so Force = Power / Velocity
let power_thrust = if self.speed_feet > 20.0 { // Only apply at reasonable speeds
self.power_hp * 550.0 * throttle_ratio / self.speed_feet
} else {
0.0 // Avoid division by small numbers
};
// Use the greater of the two thrust values - at low speed use configured thrust,
// at higher speeds might transition to power-based thrust
let thrust_force = thrust_force.max(power_thrust);
// Gravity force in body X (forward) direction
let gravity_force = -self.mass_slugs * self.gravity_accel * self.forward_tilt.sin();
// Net force = thrust - drag + gravity component
let net_force = thrust_force - drag_force + gravity_force;
// F = ma ⇒ a = F / m
let accel = net_force / self.mass_slugs;
// 7) Speed update
self.speed_feet += accel * PHYSICS_DT;
if self.speed_feet < 0.1 {
self.speed_feet = 0.1;
}
// 8) Next‐step states
let a = 2.63 / self.speed_feet * self.d;
let m_momentum = self.m; // ← accumulate momentum from step 1
self.x += (self.d * self.speed_feet
- self.t / self.s * (0.19 * self.e + a * 0.64 + self.up_down / 1e3)
- m_momentum * self.v // now matches banks.c’s M * v
+ self.gravity_accel * self.r33) * PHYSICS_DT;
self.w = self.d;
self.d += self.t * (0.45 - 14.0 / self.speed_feet * self.x - a * 130.0 - self.up_down * 0.14)
* PHYSICS_DT / 125e2
+ self.f * PHYSICS_DT * self.v;
let d_param = self.v / self.speed_feet * 15.0;
// 9) Turn rate P
self.p = (self.t * (47.0 * self.i
- local_intermediate * 52.0
+ self.e * 94.0 * d_param
- t_temp * 0.38
+ self.left_right * 0.21 * self.e) / 1e2
+ self.w * 179.0 * self.v) / 2312.0;
// 10) Sideslip v
self.v -= (self.w * self.f
- self.t * (0.63 * local_intermediate
- self.i * 0.086
+ local_intermediate * self.e * 19.0
- d_param * 25.0
- 0.11 * self.left_right) / 107e2) * PHYSICS_DT;
}
}
