I'm currently trying to port a flight dynamics model that I've implemented in Rust to Unity. The original FDM utilized the Hecs ECS, and I plan on implementing this same model in Unity using the ECS framework provided by the engine.
The main issue I keep running into is how to access state data (a struct containing locations, velocities, and various other components like bank angle, throttle and flap) associated with my aircraft entity. In the rust version of this model, my main function begins by instantiating this state struct (among other variables) with the necessary starting values, and continues into an infinite loop (the "game" loop, so to speak) in which keyboard states are checked, the state struct is updated according to the various inputs, and the entity is updated with Rust's equivalent of a "ForEach" function.
However, when I create a system in Unity, the equivalent of the game loop seems to be the "OnUpdate" function. This becomes an issue because I need to be able to instantiate (and access) the state struct outside of the "ForEach" function. I thought about placing the instantiation of the state struct inside of a "Start" function, but that would require some sort of variable local to the class (which, from what I recall, is not ideal when creating a system, which shouldn't contain data, but rather the logic).
I guess what I'm trying to ask is this: is it a good idea (or even possible within a class that uses SystemBase, for that matter) to create a variable within the class itself (and not within a function, for instance) that can be used by both my "Start" and "OnUpdate" functions? If not, is there a workaround for this? Or, since the state struct is not attached to the actual entity (since the only data associated with the aircraft within the world is its translation data), and only serves to help in the helper functions for the equations of motion, would it be acceptable?
Thanks in advance! I'll include the code snippets of the rust implementation below. If I omitted anything important, I apologize. I've been trying to figure out how to explain this problem as concisely as I can, so I may have missed some important detail.
use std::{process, thread, time::Duration};
use gphys::palmer::fdm::{Properties, State, eom};
use hecs::*;
use console::Term;
use device_query::{DeviceQuery, DeviceState, Keycode};
#[derive(Debug)]
struct Position {
x: i32,
y: i32,
z: i32
}
#[derive(Debug)]
struct Velocity {
vx: i32,
vy: i32,
vz: i32
}
fn check_ranges(state: &mut State) {
// throttle range bounds enforcement
if state.throttle > 1.0 { state.throttle = 1.0; }
if state.throttle < 0.0 { state.throttle = 0.0; }
// angle of attack bounds enforcement
if state.alpha > 20.0 { state.alpha = 20.0; }
if state.alpha < 0.0 { state.alpha = 0.0; }
// bank/roll angle bounds enforcement
if state.bank > 20.0 { state.bank = 20.0; }
if state.bank < -20.0 { state.bank = -20.0; }
// flap deflection bounds enforcement
if state.flap > 40.0 { state.flap = 40.0; }
if state.flap < 0.0 { state.flap = 0.0; }
}
#[allow(dead_code)]
fn print_state(term: &Term, state: &State) {
// velocity information
let vx = state.q[0];
let vy = state.q[2];
let vz = state.q[4];
// position information
//let x = state.q[1];
//let y = state.q[3];
let z = state.q[5];
let heading = vy.atan2(vx).to_degrees();
let vh: f64 = (vx * vx + vy * vy).sqrt();
let climb_angle = (vz / vh).atan().to_degrees();
let air_speed = (vx * vx + vy * vy + vz * vz).sqrt();
let s = format!("Throttle: {:>3.1}% | Angle of Attack: {:>3.1} deg | Bank Angle: {:>3.1} deg | Flap Deflection: {:>3.1} deg | Heading: {:>3.1} deg | Climb Angle: {:>3.1} deg | Air Speed: {:>3.1} | Climb Rate: {:>3.1} | Altitude: {:>3.1}",
(state.throttle * 100.0).round(),
state.alpha,
state.bank,
state.flap,
heading,
climb_angle,
air_speed,
vz,
z);
term.clear_line().unwrap();
term.write_str(&s).unwrap();
}
fn spawn_entity(world: &mut World, state: &State) {
let pos = Position {
x: state.q[1] as i32,
y: state.q[3] as i32,
z: state.q[5] as i32
};
let velocity = Velocity {
vx: state.q[0] as i32,
vy: state.q[2] as i32,
vz: state.q[4] as i32
};
world.spawn((pos, velocity));
}
fn update_entity(world: &mut World, state: &State) {
for (_id, (pos, velocity)) in &mut world.query::<(&mut Position, &mut Velocity)>() {
// Update positions
pos.x = state.q[1] as i32;
pos.y = state.q[3] as i32;
pos.z = state.q[5] as i32;
// Update velocities
velocity.vx = state.q[0] as i32;
velocity.vy = state.q[2] as i32;
velocity.vz = state.q[4] as i32;
}
}
#[allow(dead_code)]
fn print_entity_debug_tool(term: &Term, world: &mut World) {
// Function I used when testing this file to make sure that the entity is being written to properly.
// Uncomment this function wherever it appears in the main loop to see the results.
// Also recommended to comment out the fdm console term output to better see these results.
// Doing so might generate a few "function is never used" errors for the omitted functions.
for (id, (pos, velocity)) in &mut world.query::<(&Position, &Velocity)>() {
let s = format!("Entity ID: {:?} || {:?} || {:?}",
id,
pos,
velocity);
term.clear_line().unwrap();
term.write_str(&s).unwrap();
}
}
fn main() {
// Cessna 172 properties
let prop = Properties {
wing_area: 16.2,
wing_span: 10.9,
tail_area: 2.0,
cl_slope0: 0.0889, // slope of Cl-alpha curve
cl0: 0.178, // intercept of Cl-alpha curve
cl_slope1: -0.1, // post-stall slope of Cl-alpha curve
cl1: 3.2, // post-stall intercept of Cl-alpha curve
alpha_cl_max: 16.0, // alpha when Cl=Clmax
cdp: 0.034, // parasite drag coefficient
eff: 0.77, // induced drag efficiency coefficient
mass: 1114.0,
engine_power: 119310.0,
engine_rps: 40.0, // revolutions per second
prop_diameter: 1.905,
a: 1.83, // propeller efficiency coefficient
b: -1.32, // propeller efficiency coefficient
};
// simulation data (inputs/outputs)
let mut state = State {
time: 0.0, // time
q: [0.0; 6], // ODE results
bank: 0.0, // roll angle
alpha: 4.0, // pitch angle
throttle: 0.0, // throttle percentage
flap: 0.0, // flap deflection
};
let mut world = World::new();
spawn_entity(&mut world, &state);
let device_state = DeviceState::new();
let mut prev_keys = vec![];
let term = Term::stdout();
const FRAME_RATE : u64 = 60; // hertz
const DT: f64 = 1.0/FRAME_RATE as f64; // seconds
loop {
// compute equations of motion
eom(&prop, &mut state, DT);
// Comment out this print function if "print_entity_debug_tool" is uncommented below for better readability
print_state(&term, &state);
// Update entity after eom
update_entity(&mut world, &state);
// Considered adding an "update_entity()" function call here, but figured just one call after the range check should be necessary
// update state
let keys = device_state.get_keys();
if keys != prev_keys {
if keys.contains(&Keycode::E) {
state.throttle += 0.1;
} else if keys.contains(&Keycode::D) {
state.throttle -= 0.1;
} else if keys.contains(&Keycode::Up) {
state.alpha += 1.0;
} else if keys.contains(&Keycode::Down) {
state.alpha -= 1.0;
} else if keys.contains(&Keycode::Left) {
state.bank -= 1.0;
} else if keys.contains(&Keycode::Right) {
state.bank += 1.0;
} else if keys.contains(&Keycode::L) {
state.flap -= 1.0;
} else if keys.contains(&Keycode::K) {
state.flap += 1.0;
} else if keys.contains(&Keycode::Q) {
process::exit(0);
}
check_ranges(&mut state);
}
// Update entity after range check to ensure entity is also within bounds
update_entity(&mut world, &state);
// Remove the commented-out function to show entity values being written to
//print_entity_debug_tool(&term, &mut world);
prev_keys = keys;
thread::sleep(Duration::from_millis((DT*100.0) as u64));
}
}
And here's the function containing the equation of motion helper functions:
use std::f64::consts::PI;
static G: f64 = -9.81;
#[derive(Debug, Default)]
pub struct Properties
{
pub wing_area: f64,
pub wing_span: f64,
pub tail_area: f64,
pub cl_slope0: f64, // slope of Cl-alpha curve
pub cl0: f64, // intercept of Cl-alpha curve
pub cl_slope1: f64, // post-stall slope of Cl-alpha curve
pub cl1: f64, // post-stall intercept of Cl-alpha curve
pub alpha_cl_max: f64, // alpha when Cl=Clmax
pub cdp: f64, // parasite drag coefficient
pub eff: f64, // induced drag efficiency coefficient
pub mass: f64,
pub engine_power: f64,
pub engine_rps: f64, // revolutions per second
pub prop_diameter: f64,
pub a: f64, // propeller efficiency coefficient
pub b: f64, // propeller efficiency coefficient
}
#[derive(Debug, Default)]
pub struct State
{
pub time: f64, // time
pub q: [f64; 6], // ODE results
pub bank: f64, // roll angle
pub alpha: f64, // pitch angle
pub throttle: f64, // throttle percentage
pub flap: f64, // flap deflection
}
//-----------------------------------------------------
// calculates the forces associated with an aircraft
// given a set of properties and current state
//-----------------------------------------------------
fn plane_rhs(prop: &Properties, state: &State,
q: &[f64; 6], delta_q: &[f64; 6],
dt: f64, q_scale: f64,
dq: &mut [f64; 6])
{
// property convenience variables
let wing_area = prop.wing_area;
let wing_span = prop.wing_span;
// let tail_area = plane.tail_area;
let cl_slope0 = prop.cl_slope0;
let cl0 = prop.cl0;
let cl_slope1 = prop.cl_slope1;
let cl1 = prop.cl1;
let alpha_cl_max = prop.alpha_cl_max;
let cdp = prop.cdp;
let eff = prop.eff;
let mass = prop.mass;
let engine_power = prop.engine_power;
let engine_rps = prop.engine_rps;
let prop_diameter = prop.prop_diameter;
let a = prop.a;
let b = prop.b;
// state convenience variables
let alpha = state.alpha;
let throttle = state.throttle;
let flap = state.flap;
// convert bank angle from degrees to radians
// angle of attack is not converted because the
// Cl-alpha curve is defined in terms of degrees
let bank = state.bank.to_radians();
// compute the intermediate values of the dependent variables
let mut new_q : [f64; 6] = [0.0; 6];
for i in 0..6 {
new_q[i] = q[i] + q_scale * delta_q[i];
}
// assign convenenience variables to the intermediate
// values of the locations and velocities
let vx: f64 = new_q[0];
let vy: f64 = new_q[2];
let vz: f64 = new_q[4];
let _x: f64 = new_q[1];
let _y: f64 = new_q[3];
let z: f64 = new_q[5];
let vh: f64 = (vx * vx + vy * vy).sqrt();
let vtotal: f64 = (vx * vx + vy * vy + vz * vz).sqrt();
// compute the air density
let temperature: f64 = 288.15 - 0.0065 * z;
let grp: f64 = 1.0 - 0.0065 * z / 288.15;
let pressure: f64 = 101325.0 * (grp.powf(5.25));
let density: f64 = 0.00348 * pressure / temperature;
// compute power drop-off factor
let omega: f64 = density / 1.225;
let factor: f64 = (omega - 0.12)/ 0.88;
// compute thrust
let advance_ratio: f64 = vtotal / (engine_rps * prop_diameter);
let thrust: f64 = throttle * factor * engine_power * (a + b * advance_ratio * advance_ratio) / (engine_rps * prop_diameter);
// compute lift coefficient - the Cl curve is modeled using two straight lines
let mut cl: f64;
if alpha < alpha_cl_max {
cl = cl_slope0 * alpha + cl0;
} else {
cl = cl_slope1 * alpha + cl1;
}
// include effects of flaps and ground effects
// -- ground effects are present if the plane is within 5 meters of the ground
if flap == 20.0 {
cl += 0.25;
}
if flap == 40.0 {
cl += 0.5;
}
if z < 5.0 {
cl += 0.25;
}
// compute lift
let lift: f64 = 0.5 * cl * density * vtotal * vtotal * wing_area;
// compute drag coefficient
let aspect_ratio: f64 = wing_span * wing_span / wing_area;
let cd = cdp + cl * cl / (PI * aspect_ratio * eff);
// compute drag force
let drag: f64 = 0.5 * cd * density * vtotal * vtotal * wing_area;
// define some shorthand convenience variables for use with the rotation matrix
// compute the sine and cosines of the climb angle, bank angle, and heading angle
let cos_w: f64 = bank.cos();
let sin_w: f64 = bank.sin();
let cos_p: f64; // climb angle
let sin_p: f64; // climb angle
let cos_t: f64; // heading angle
let sin_t: f64; // heading angle
if vtotal == 0.0 {
cos_p = 1.0;
sin_p = 0.0;
} else {
cos_p = vh / vtotal;
sin_p = vz / vtotal;
}
if vh == 0.0 {
cos_t = 1.0;
sin_t = 0.0;
} else {
cos_t = vx / vh;
sin_t = vy / vh;
}
// convert the thrust, drag, and lift forces into x-, y-, and z-components using the rotation matrix
let fx: f64 = cos_t * cos_p * (thrust - drag) + (sin_t * sin_w - cos_t * sin_p * cos_w) * lift;
let fy: f64 = sin_t * cos_p * (thrust - drag) + (-cos_t * sin_w - sin_t * sin_p * cos_w) * lift;
let mut fz: f64 = sin_p * (thrust - drag) + cos_p * cos_w * lift;
// add the gravity force to the z-direction force.
fz = fz + mass * G;
// since the plane can't sink into the ground, if the altitude is less than or equal to zero and the z-component
// of force is less than zero, set the z-force to be zero
if z <= 0.0 && fz <= 0.0 {
fz = 0.0;
}
// load the right-hand sides of the ODE's
dq[0] = dt * (fx / mass);
dq[1] = dt * vx;
dq[2] = dt * (fy / mass);
dq[3] = dt * vy;
dq[4] = dt * (fz / mass);
dq[5] = dt * vz;
}
//-----------------------------------------------------
// solves the equations of motion using the Runge-Kutta
// integration method
//-----------------------------------------------------
pub fn eom(prop: &Properties, state: &mut State, dt: f64)
{
let mut q : [f64; 6] = [0.0; 6];
let mut dq1 : [f64; 6] = [0.0; 6];
let mut dq2 : [f64; 6] = [0.0; 6];
let mut dq3 : [f64; 6] = [0.0; 6];
let mut dq4 : [f64; 6] = [0.0; 6];
// retrieve the current values of the dependent and independent variables
for i in 0..6 {
q[i] = state.q[i];
}
// compute the four Runge-Kutta steps, then return
// value of planeRightHandSide method is an array
// of delta-q values for each of the four steps
plane_rhs(&prop, &state, &q, &q, dt, 0.0, &mut dq1);
plane_rhs(&prop, &state, &q, &dq1, dt, 0.5, &mut dq2);
plane_rhs(&prop, &state, &q, &dq2, dt, 0.5, &mut dq3);
plane_rhs(&prop, &state, &q, &dq3, dt, 1.0, &mut dq4);
// update simulation time
state.time += dt;
// update the dependent and independent variable values
// at the new dependent variable location and store the
// values in the ODE object arrays
for i in 0..6 {
q[i] = q[i] + (dq1[i] + 2.0 * dq2[i] + 2.0 * dq3[i] + dq4[i]) / 6.0;
state.q[i] = q[i];
}
}
