feat(flex): Add collisions with ground as an input for wing flex syst…

…em (flybywiresim#8812)


Co-authored-by: @Steveveepee

fbw-a380x/src/base/flybywire-aircraft-a380-842/SimObjects/AirPlanes/FlyByWire_A380_842/flight_model.cfg

@@ -77,7 +77,7 @@ static_pitch = -0.23 ; degrees, pitch when at rest on the ground (+=Up, -=Dn)
 static_cg_height = 8.89 ; feet, altitude of CG when at rest on the ground
 gear_system_type = 3 ; gear system type (betweeen 0 and 4) 0 = electrical, 1 = hydraulic, 2 = pneumatic, 3 = manual, 4 = none, 5 = undefined
 tailwheel_lock = 0 ; Is tailwheel lock available TRUE/FALSE
-max_number_of_points = 11 ; Number of contact points
+max_number_of_points = 23 ; Number of contact points
 gear_locked_on_ground = 0 ; Defines whether or not the landing gear handle is locked to down when the plane is on the ground.
 gear_locked_above_speed = -1 ; Defines the speed at which the landing gear handle becomes locked in the up position. (-1 = Disabled)==> Disabled is kept in favor of an XML-based solution
 allow_stopped_steering = 1 ; This can be used to enable (TRUE, 1) steering when the aircraft is stopped or not (FALSE, 0).
@@ -115,12 +115,29 @@ point.1 = 1, -5.7, -11.5, -15.4, 1200, 1, 2.356, 8, 1.3790921701152, 1.25, 0.5,
 point.2 = 1, -5.7, 11.5, -15.4, 1200, 2, 2.356, 8, 1.3790921701152, 1.25, 0.5, 21.1, 19.9, 3, 0, 0, 4
 point.3 = 1, 5.7, -23.0, -15.18, 1200, 1, 2.356, 0, 1.3146370499424, 1.25, 0.5, 21.1, 19.9, 2, 0, 0, 4
 point.4 = 1, 5.7, 23.0, -15.18, 1200, 2, 2.356, 0, 1.3146370499424, 1.25, 0.5, 21.1, 19.9, 3, 0, 0, 4
-point.5 = 2, -26, -60, 6, 100, 0, 0, 0, 0, 0, 0, 0, 0, 5, 0, 0, 1
-point.6 = 2, -26, 60, 6, 100, 0, 0, 0, 0, 0, 0, 0, 0, 6, 0, 0, 1
-point.7 = 2, -79.2, 0, 8, 100, 0, 0, 0, 0, 0, 0, 0, 0, 9, 0, 0, 1
-point.8 = 2, 44.07, 0, 2, 720, 0, 0, 0, 0, 0, 0, 0, 0, 4, 0, 0, 1
-point.9 = 2, -68, 0, 4.1, 100, 0, 0, 0, 0, 0, 0, 0, 0, 7, 0, 0, 1
-point.10 = 2, -75.2, 0, 32.3, 100, 0, 0, 0, 0, 0, 0, 0, 0, 8, 0, 0, 1
+
+; Next four points are set on max possible animation wingflex position allowing flex simulation to detect lower collisions
+point.5 = 2, 4, -84, -3, 100, 0, 0, 0, 0, 0, 0, 0, 0, 7, 0, 0, 1 ; Engine 1 max upflex position
+point.6 = 2, 4, 84, -3, 100, 0, 0, 0, 0, 0, 0, 0, 0, 7, 0, 0, 1 ; Engine 4 max upflex position
+point.7 = 2,-45,-130,25,100,0,0.0001,0,0.0001,0.0001,0,0,0,5,0,0,1 ; left wing tip max up flex
+point.8 = 2,-45,130,25,100,0,0.0001,0,0.0001,0.0001,0,0,0,6,0,0,1 ; right wing tip max up flex
+
+; Engines 2 and 3 are not handled by flex collision detection so just positioned on the model
+point.9 = 2,33.315075,-48.682236,-10.257178,100,0,0.0001,0,1,0.0001,0,0,0,0,0,0,1 ; #2 Engine Pod
+point.10 = 2,41.815704,-48.664852,-4.112969,100,0,0.0001,0,1,0.0001,0,0,0,0,0,0,1 ; #2 Engine cowl lip
+point.11 = 2,34.689693,48.535751,-10.585604,100,0,0.0001,0,0.0001,0.0001,0,0,0,8,0,0,1 ; #3 Engine Pod
+point.12 = 2,43.820728,49.039993,-3.867688,100,0,0.0001,0,1,0.0001,0,0,0,0,0,0,1 ; #3 Engine cowl lip
+
+point.13 = 2,115.108,0.0,2.452346,100,0,0.0001,0,1,0.0001,0,0,0,4,0,0,1 ; Nose radome
+point.14 = 2,-123.27,0.0,66.496658,100,0,0.0001,0,1,0.0001,0,0,0,0,0,0,1 ; Tail Rudder tip
+point.15 = 2,-123.13,-49.30,17.53,100,0,0.0001,0,1,0.0001,0,0,0,0,0,0,1 ; Tail LH Elevator tip
+point.16 = 2,-123.13,49.30,17.53,100,0,0.0001,0,1,0.0001,0,0,0,0,0,0,1 ; Tail RH Elevator tip
+point.17 = 2,-72.402222,0.0,0.002656,100,0,0.0001,0,1,0.0001,0,0,0,9,0,0,1 ; Body tailstrike location
+point.18 = 2,0.0,0.0,-7.930269,100,0,0.0001,0,1,0.0001,0,0,0,4,0,0,1 ; Body extents below reference point
+point.19 = 2,88.42083,0.0,-5.299597,100,0,0.0001,0,1,0.0001,0,0,0,4,0,0,1 ; Body extents behind nosewheel
+point.20 = 2,51.91,-16.86,0.96,100,0,0.0001,0,1,0.0001,0,0,0,0,0,0,1 ; LH wing root leading edge
+point.21 = 2,51.91,16.86,0.96,100,0,0.0001,0,1,0.0001,0,0,0,0,0,0,1 ; RH wing root leading edge
+point.22 = 2,79.979874,0.0,21.776155,100,0,0.0001,0,1,0.0001,0,0,0,4,0,0,1 ; Body fueslage top behind cockpit
 
 [FUEL]
 fuel_type = 2 ; 1 = OCTANE 100, 2 = JET_A, 3 = OCTANE 80, 4 = AUTO GAS, 5 = JET B

fbw-a380x/src/wasm/systems/a380_systems/src/structural_flex.rs

@@ -410,6 +410,18 @@ impl WingFlexA380 {
 
     const WING_NODES_X_COORDINATES: [f64; WING_FLEX_NODE_NUMBER] = [0., 11.5, 22.05, 29., 36.85];
 
+    // Section defining points for collision detection of wing tips and outter engines
+    const OUTTER_ENGINE_VERTICAL_OFFSET: f64 = -2.3;
+    const WING_TIP_VERTICAL_OFFSET: f64 = 2.2;
+
+    const RIGHT_OUTTER_ENGINE_COORDINATES: [f64; 3] =
+        [25.60, Self::OUTTER_ENGINE_VERTICAL_OFFSET, 1.22];
+    const LEFT_OUTTER_ENGINE_COORDINATES: [f64; 3] =
+        [-25.60, Self::OUTTER_ENGINE_VERTICAL_OFFSET, 1.22];
+
+    const RIGHT_WING_TIP_COORDINATES: [f64; 3] = [39.624, Self::WING_TIP_VERTICAL_OFFSET, -13.716];
+    const LEFT_WING_TIP_COORDINATES: [f64; 3] = [-39.624, Self::WING_TIP_VERTICAL_OFFSET, -13.716];
+
     pub fn new(context: &mut InitContext) -> Self {
         let empty_mass = Self::EMPTY_MASS_KG.map(Mass::new::<kilogram>);
 
@@ -437,12 +449,27 @@ impl WingFlexA380 {
             fuel_mapper: WingFuelNodeMapper::new(Self::FUEL_MAPPING),
             animation_mapper: WingAnimationMapper::new(Self::WING_NODES_X_COORDINATES),
 
-            flex_physics: [1, 2].map(|_| {
+            flex_physics: ['L', 'R'].map(|side| {
                 FlexPhysicsNG::new(
                     context,
                     empty_mass,
                     Self::FLEX_COEFFICIENTS,
                     Self::DAMPING_COEFFICIENTS,
+                    [
+                        None,
+                        None,
+                        Some(if side == 'L' {
+                            Vector3::from(Self::LEFT_OUTTER_ENGINE_COORDINATES)
+                        } else {
+                            Vector3::from(Self::RIGHT_OUTTER_ENGINE_COORDINATES)
+                        }),
+                        None,
+                        Some(if side == 'L' {
+                            Vector3::from(Self::LEFT_WING_TIP_COORDINATES)
+                        } else {
+                            Vector3::from(Self::RIGHT_WING_TIP_COORDINATES)
+                        }),
+                    ],
                 )
             }),
 
@@ -730,6 +757,7 @@ mod tests {
             Self {
                 test_bed: SimulationTestBed::new(WingFlexTestAircraft::new),
             }
+            .with_nominal_height()
         }
 
         fn left_wing_lift_per_node(&self) -> Vector5<f64> {
@@ -768,6 +796,11 @@ mod tests {
             self
         }
 
+        fn with_nominal_height(mut self) -> Self {
+            self.write_by_name("PLANE ALT ABOVE GROUND", 20.);
+            self
+        }
+
         fn with_max_fuel(mut self) -> Self {
             self.command(|a| {
                 a.set_fuel(A380FuelTankType::LeftInner, Mass::new::<kilogram>(32000.))
@@ -1440,4 +1473,74 @@ mod tests {
         assert!((53.1..=53.3).contains(&animation_position_outboard_mid));
         assert!((55. ..=57.).contains(&animation_position_outboard));
     }
+
+    #[test]
+    fn right_wing_is_higher_if_wing_strike_turning_right() {
+        let mut test_bed = WingFlexTestBed::new().with_nominal_weight().in_1g_flight();
+
+        test_bed = test_bed.run_waiting_for(Duration::from_secs(2));
+
+        let mut outboard_angle_left: Angle = test_bed.read_by_name("WING_FLEX_LEFT_OUTBOARD");
+        let mut outboard_angle_right: Angle = test_bed.read_by_name("WING_FLEX_RIGHT_OUTBOARD");
+
+        println!(
+            "ANGLES => LEFT TIP {:.1} RIGHT TIP {:.1}",
+            outboard_angle_left.get::<degree>(),
+            outboard_angle_right.get::<degree>(),
+        );
+
+        assert!(
+            (outboard_angle_left.get::<degree>() - outboard_angle_right.get::<degree>()).abs()
+                < 0.1
+        );
+
+        test_bed.write_by_name("PLANE BANK DEGREES", -45.);
+        test_bed = test_bed.run_waiting_for(Duration::from_secs(2));
+
+        outboard_angle_right = test_bed.read_by_name("WING_FLEX_RIGHT_OUTBOARD");
+        outboard_angle_left = test_bed.read_by_name("WING_FLEX_LEFT_OUTBOARD");
+
+        println!(
+            "!!!!WING STRIKE RIGHT!!!! ANGLES => LEFT TIP {:.1} RIGHT TIP {:.1}",
+            outboard_angle_left.get::<degree>(),
+            outboard_angle_right.get::<degree>(),
+        );
+
+        assert!(outboard_angle_right.get::<degree>() - outboard_angle_left.get::<degree>() > 5.);
+    }
+
+    #[test]
+    fn left_wing_is_higher_if_wing_strike_turning_left() {
+        let mut test_bed = WingFlexTestBed::new().with_nominal_weight().in_1g_flight();
+
+        test_bed = test_bed.run_waiting_for(Duration::from_secs(2));
+
+        let mut outboard_angle_left: Angle = test_bed.read_by_name("WING_FLEX_LEFT_OUTBOARD");
+        let mut outboard_angle_right: Angle = test_bed.read_by_name("WING_FLEX_RIGHT_OUTBOARD");
+
+        println!(
+            "ANGLES => LEFT TIP {:.1} RIGHT TIP {:.1}",
+            outboard_angle_left.get::<degree>(),
+            outboard_angle_right.get::<degree>(),
+        );
+
+        assert!(
+            (outboard_angle_left.get::<degree>() - outboard_angle_right.get::<degree>()).abs()
+                < 0.1
+        );
+
+        test_bed.write_by_name("PLANE BANK DEGREES", 45.);
+        test_bed = test_bed.run_waiting_for(Duration::from_secs(2));
+
+        outboard_angle_right = test_bed.read_by_name("WING_FLEX_RIGHT_OUTBOARD");
+        outboard_angle_left = test_bed.read_by_name("WING_FLEX_LEFT_OUTBOARD");
+
+        println!(
+            "!!!!WING STRIKE LEFT!!!! ANGLES => LEFT TIP {:.1} RIGHT TIP {:.1}",
+            outboard_angle_left.get::<degree>(),
+            outboard_angle_right.get::<degree>(),
+        );
+
+        assert!(outboard_angle_right.get::<degree>() - outboard_angle_left.get::<degree>() < 5.);
+    }
 }

fbw-common/src/wasm/systems/systems/src/structural_flex/wing_flex.rs

@@ -1,6 +1,6 @@
-use crate::shared::local_acceleration_at_plane_coordinate;
 use crate::shared::low_pass_filter::LowPassFilter;
 use crate::shared::update_iterator::MaxStepLoop;
+use crate::shared::{height_over_ground, local_acceleration_at_plane_coordinate};
 
 use crate::simulation::{
     InitContext, Read, SimulationElement, SimulationElementVisitor, SimulatorReader, UpdateContext,
@@ -425,25 +425,36 @@ struct WingSectionNode {
     position: Length,
     acceleration: Acceleration,
 
+    min_position: Length,
+
     external_position_offset: Length,
 
     sum_of_forces: Force,
 }
 impl WingSectionNode {
+    const ABSOLUTE_MIN_POSITION: f64 = -10.;
+    const ABSOLUTE_MAX_POSITION: f64 = 10.;
+
     fn new(empty_mass: Mass) -> Self {
         Self {
             empty_mass,
             fuel_mass: Mass::default(),
             speed: Velocity::default(),
             position: Length::default(),
             acceleration: Acceleration::default(),
+            min_position: Length::new::<meter>(Self::ABSOLUTE_MIN_POSITION),
 
             external_position_offset: Length::default(),
 
             sum_of_forces: Force::default(),
         }
     }
 
+    pub fn set_min_position(&mut self, min_pos: Length) {
+        let new_min = min_pos.max(Length::new::<meter>(Self::ABSOLUTE_MIN_POSITION));
+        self.min_position = new_min;
+    }
+
     fn update(&mut self, context: &UpdateContext) {
         self.apply_gravity_force(context);
         self.solve_physics(context);
@@ -477,6 +488,14 @@ impl WingSectionNode {
             self.speed += self.acceleration * context.delta_as_time();
 
             self.position += self.speed * context.delta_as_time();
+
+            if self.position < self.min_position {
+                self.position = self.min_position;
+                self.speed = Velocity::default();
+            } else if self.position > Length::new::<meter>(Self::ABSOLUTE_MAX_POSITION) {
+                self.position = Length::new::<meter>(Self::ABSOLUTE_MAX_POSITION);
+                self.speed = Velocity::default();
+            }
         }
 
         self.sum_of_forces = Force::default();
@@ -517,6 +536,8 @@ pub struct FlexPhysicsNG<const NODE_NUMBER: usize, const LINK_NUMBER: usize> {
     nodes: [WingSectionNode; NODE_NUMBER],
     flex_constraints: [FlexibleConstraint; LINK_NUMBER],
 
+    height_limit_absolute_coordinate: [Option<Vector3<f64>>; NODE_NUMBER],
+
     // DEV simvars to adjust parameters ingame
     wing_dev_spring_1_id: VariableIdentifier,
     wing_dev_spring_2_id: VariableIdentifier,
@@ -549,6 +570,7 @@ impl<const NODE_NUMBER: usize, const LINK_NUMBER: usize> FlexPhysicsNG<NODE_NUMB
         empty_mass: [Mass; NODE_NUMBER],
         springness: [f64; LINK_NUMBER],
         damping: [f64; LINK_NUMBER],
+        height_limit_absolute_coordinate: [Option<Vector3<f64>>; NODE_NUMBER],
     ) -> Self {
         let nodes_array = empty_mass.map(WingSectionNode::new);
 
@@ -573,6 +595,8 @@ impl<const NODE_NUMBER: usize, const LINK_NUMBER: usize> FlexPhysicsNG<NODE_NUMB
                 },
             ),
 
+            height_limit_absolute_coordinate,
+
             wing_dev_spring_1_id: context.get_identifier("WING_FLEX_DEV_SPRING_1".to_owned()),
             wing_dev_spring_2_id: context.get_identifier("WING_FLEX_DEV_SPRING_2".to_owned()),
             wing_dev_spring_3_id: context.get_identifier("WING_FLEX_DEV_SPRING_3".to_owned()),
@@ -589,6 +613,15 @@ impl<const NODE_NUMBER: usize, const LINK_NUMBER: usize> FlexPhysicsNG<NODE_NUMB
         }
     }
 
+    fn update_ground_collision_constraints(&mut self, context: &UpdateContext) {
+        for (node_idx, coordinate) in self.height_limit_absolute_coordinate.iter().enumerate() {
+            if coordinate.is_some() {
+                let min_height = -height_over_ground(context, coordinate.unwrap());
+                self.nodes[node_idx].set_min_position(min_height);
+            }
+        }
+    }
+
     pub fn update(
         &mut self,
         context: &UpdateContext,
@@ -598,6 +631,8 @@ impl<const NODE_NUMBER: usize, const LINK_NUMBER: usize> FlexPhysicsNG<NODE_NUMB
     ) {
         self.updater_max_step.update(context);
 
+        self.update_ground_collision_constraints(context);
+
         for cur_time_step in &mut self.updater_max_step {
             self.external_accelerations_filtered
                 .update(context.delta(), external_acceleration_from_plane_body);