From 65ea3564307d96d8508b9a737f60c0613cf55c1e Mon Sep 17 00:00:00 2001
From: Hector Sanjuan <code@hector.link>
Date: Mon, 14 Aug 2023 00:09:20 +0200
Subject: [PATCH] rotor effects: use rotor radius from rotor_new. Handle twin
 rotors.

I had added a rotor_diameter model option, but the models already define
rotors and their diameters using "rotor_new".

The only combination that serves to differentiate and count main rotors is
that they either have "follow controls" set or they are "tiltable" (ospray).

When more than 1 main rotor is identified (v22, ka-25), then torque and
Transverse Flow rotor effects are compensated and disabled.
---
 data/aircrafts/as350.3d |  3 --
 data/aircrafts/b47.3d   |  3 --
 data/aircrafts/ec145.3d |  3 --
 data/aircrafts/hh60.3d  |  3 --
 data/aircrafts/hh65.3d  |  3 --
 data/aircrafts/ka27.3d  |  3 --
 data/aircrafts/s64.3d   |  3 --
 data/aircrafts/uh1d.3d  |  3 --
 data/aircrafts/v22.3d   |  3 --
 src/cmdset.c            | 11 ------
 src/obj.h               |  3 --
 src/objio.c             | 13 -------
 src/sfmmodel.h          |  1 +
 src/sfmsimforce.c       | 86 +++++++++++++++++++++++++----------------
 src/simop.c             | 29 +++++++++++++-
 15 files changed, 81 insertions(+), 89 deletions(-)

diff --git a/data/aircrafts/as350.3d b/data/aircrafts/as350.3d
index 7e8b5d5..03aca39 100644
--- a/data/aircrafts/as350.3d
+++ b/data/aircrafts/as350.3d
@@ -66,9 +66,6 @@ belly_height 1.7
 # Length in meters
 length 10.92
 
-# Rotor diameter in meters
-rotor_diameter 10.8966
-
 # Gear (down position) to belly height in meters
 gear_height 0.35
 
diff --git a/data/aircrafts/b47.3d b/data/aircrafts/b47.3d
index 7cd9936..b0dd1cc 100644
--- a/data/aircrafts/b47.3d
+++ b/data/aircrafts/b47.3d
@@ -60,9 +60,6 @@ belly_height 0.5
 # Length in meters
 length 9.63
 
-# Rotor diameter in meters
-rotor_diameter 9.4742
-
 # Gear (down position) to belly height in meters
 gear_height 0.0
 
diff --git a/data/aircrafts/ec145.3d b/data/aircrafts/ec145.3d
index 711a3b6..dac6766 100644
--- a/data/aircrafts/ec145.3d
+++ b/data/aircrafts/ec145.3d
@@ -55,9 +55,6 @@ belly_height 1.55
 # Length, in meters
 length 13.03
 
-# Rotor diameter in meters
-rotor_diameter 11
-
 # Gear (down position) to belly height, in meters
 gear_height 0.44
 
diff --git a/data/aircrafts/hh60.3d b/data/aircrafts/hh60.3d
index 71bc94f..6c75eb7 100644
--- a/data/aircrafts/hh60.3d
+++ b/data/aircrafts/hh60.3d
@@ -71,9 +71,6 @@ belly_height 2.2
 # Length in meters
 length 19.81
 
-# Rotor diameter in meters
-rotor_diameter 16.4592
-
 # Gear (down position) to belly height in meters
 gear_height 0.5
 
diff --git a/data/aircrafts/hh65.3d b/data/aircrafts/hh65.3d
index 4f69cc6..545a0bb 100644
--- a/data/aircrafts/hh65.3d
+++ b/data/aircrafts/hh65.3d
@@ -69,9 +69,6 @@ belly_height 2.38
 # Length in meters
 length 13.54
 
-# Rotor diameter in meters
-rotor_diameter 11.938
-
 # Gear (down position) to belly height in meters
 gear_height 0.35
 
diff --git a/data/aircrafts/ka27.3d b/data/aircrafts/ka27.3d
index 94490cd..dda95e8 100644
--- a/data/aircrafts/ka27.3d
+++ b/data/aircrafts/ka27.3d
@@ -67,9 +67,6 @@ belly_height 2.35
 # Length in meters
 length 10.49
 
-# Rotor diameter in meters
-rotor_diameter 16.637
-
 # Gear (down position) to belly height in meters
 gear_height 0.3
 
diff --git a/data/aircrafts/s64.3d b/data/aircrafts/s64.3d
index 64e3226..d5e855a 100644
--- a/data/aircrafts/s64.3d
+++ b/data/aircrafts/s64.3d
@@ -61,9 +61,6 @@ belly_height 3.55
 # Length in meters
 length 21.41
 
-# Rotor diameter in meters
-rotor_diameter 21.9456
-
 # Gear (down position) to belly height in meters
 gear_height 0.45
 
diff --git a/data/aircrafts/uh1d.3d b/data/aircrafts/uh1d.3d
index 74b3ea9..3493dd0 100644
--- a/data/aircrafts/uh1d.3d
+++ b/data/aircrafts/uh1d.3d
@@ -60,9 +60,6 @@ belly_height 1.7
 # Length in meters
 length 12.75
 
-# Rotor diameter in meters
-rotor_diameter 14.7828
-
 # Gear (down position) to belly height in meters
 gear_height 0.3
 
diff --git a/data/aircrafts/v22.3d b/data/aircrafts/v22.3d
index 827dd0c..d1dda1c 100644
--- a/data/aircrafts/v22.3d
+++ b/data/aircrafts/v22.3d
@@ -70,9 +70,6 @@ belly_height 2.8
 # Length in meters
 length 17.47
 
-# Rotor diameter in meters
-rotor_diameter 11.5824
-
 # Wingspan in meters
 wingspan 15.24
 
diff --git a/src/cmdset.c b/src/cmdset.c
index 3ea688e..c09f7e6 100644
--- a/src/cmdset.c
+++ b/src/cmdset.c
@@ -579,17 +579,6 @@ void SARCmdSet(SAR_CMD_PROTOTYPE)
 		got_match = True;
 	    }
 	}
-	/* Rotor diameter */
-	else if(!strcasecmp(parm, "rotor_diameter"))
-	{
-	    sar_object_aircraft_struct *obj_aircraft_ptr =
-		SAR_OBJ_GET_AIRCRAFT(obj_ptr);
-	    if(obj_aircraft_ptr != NULL)
-	    {
-		obj_aircraft_ptr->rotor_diameter = (float)ATOF(val);
-		got_match = True;
-	    }
-	}
 	/* Gear height */
 	else if(!strcasecmp(parm, "gear_height"))
 	{
diff --git a/src/obj.h b/src/obj.h
index c8c5ea2..c8978cd 100644
--- a/src/obj.h
+++ b/src/obj.h
@@ -861,9 +861,6 @@ typedef struct {
 	/* Wingspan of aircraft in meters */
 	float		wingspan;
 
-	/* Rotor diameter in meters */
-	float		rotor_diameter;
-
 	/* Height of landing gear in meters */
 	float		gear_height;
 
diff --git a/src/objio.c b/src/objio.c
index 163f6ca..a3cb161 100644
--- a/src/objio.c
+++ b/src/objio.c
@@ -3245,19 +3245,6 @@ static void SARObjLoadLine(
 		if(aircraft != NULL)
 		    aircraft->wingspan = wingspan;
 	    }
-	    else if(!strcasecmp(parm, "rotor_diameter"))
-	    {
-		/* Arguments:
-		 *
-		 * <wingspan>
-		 */
-		float	rotor_diameter = 0.0f;
-
-		arg = GET_ARG_F(arg, &rotor_diameter);
-
-		if(aircraft != NULL)
-		    aircraft->rotor_diameter = rotor_diameter;
-	    }
 	    /* Landing Gear Height */
 	    else if(!strcasecmp(parm, "gear_height"))
 	    {
diff --git a/src/sfmmodel.h b/src/sfmmodel.h
index e1b4d15..03e42fd 100644
--- a/src/sfmmodel.h
+++ b/src/sfmmodel.h
@@ -79,6 +79,7 @@
 #define SFMFlagLength			((SFMFlags)1 << 42)
 #define SFMFlagWingspan			((SFMFlags)1 << 43)
 #define SFMFlagRotorDiameter		((SFMFlags)1 << 44)
+#define SFMFlagSingleMainRotor		((SFMFlags)1 << 45)
 
 /*
  *	Flight model types:
diff --git a/src/sfmsimforce.c b/src/sfmsimforce.c
index bf0f15f..749ab25 100644
--- a/src/sfmsimforce.c
+++ b/src/sfmsimforce.c
@@ -1524,6 +1524,42 @@ int SFMForceApplyArtificial(
 			sin_bank = sin(dir->bank),
 			cos_bank = cos(dir->bank);
 
+		/* Rotor effects */
+
+		/* IGE effect (In-Ground-Effect) adds more lift force when close to the ground.
+		 * We effectively give thrust_output a maximum of 28% bonus in that region.
+		 *
+		 * Effect starts at a rotor height of 1.25 rotor diameter.
+		 * http://www.copters.com/aero/ground_effect.html
+		 */
+		if(flags & SFMFlagRotorDiameter)
+		{
+		    // Twin-rotor aircrafts note:
+		    //   - Coaxial are assumed to experience normal IGE since its a single
+		    //     engine after all.
+		    //   - Transverse (V22 Ospray): the model engine is already sized at 2x
+		    //     I think, so we should be good.
+
+		    // horizontallity_coeff dampens IGE based on how horizontal to the
+		    // ground the aircraft is. It can be played with.
+		    double horizontallity_coeff = POW(ABS(cos_pitch) * ABS(cos_bank),2);
+		    double ige_height = 1.25 * model->rotor_diameter;
+		    // The rotor is as high as the center of the aircraft plus
+		    // the belly_height (assume distance from center to belly
+		    // is the same as from center to rotor. This saves having
+		    // to carry exact rotor elevation to the FSM model,
+		    // although that could be done.
+		    double rotor_height = ABS(pos->z - model->ground_elevation_msl + model->belly_height);
+		    // Increases towards 1 when ground_elevation is lower. Non
+		    // linear, approximate by the square.
+		    double ige_coeff = (1 - POW(CLIP(rotor_height, 0, ige_height) / ige_height, 2));
+		    // Increase thrust output 28% at most.
+		    // It is always assumed that the ground is horizontal and effect happens
+		    // when we are parallel to ground.
+		    thrust_output = (1 + 0.28 * ige_coeff * horizontallity_coeff) * thrust_output;
+		    // fprintf(stderr, "IGE. hor_coeff: %.2f, coeff: %.2f, thrust_bonus: %.2f\n", horizontallity_coeff, ige_coeff, 1 + 0.28 * ige_coeff);
+		}
+
 		/* This is the speed vector relative to the rotor blades.
 		   Used to calculate pitch and bank changes. It should match
 		   airspeed vector when aircraft horizontal */
@@ -1533,6 +1569,8 @@ int SFMForceApplyArtificial(
 		// heading of the aircraft so we don't need to rotate
 		// heading. A rotor moving forward will just see y_speed and
 		// z_speed (because pitched).
+		// FIXME: rotor follows control so it may not be perpendicular to
+		// aircraft as assumed here, but close enough.
 		double a[3 *1], r[3 * 1];
 		a[0] = airspeed->x;
 		a[1] = airspeed->y;
@@ -1546,31 +1584,7 @@ int SFMForceApplyArtificial(
 		double airspeed_rotor_2d = SFMHypot2(airspeed_rotor.x, airspeed_rotor.y);
 		// fprintf(stderr, "airspeed_rotor. b: %.2f, p: %.2f, h: %.2f x: %.2f, y: %.2f, z: %.2f, 2d: %.2f\n", dir->bank, dir->pitch, dir->heading, a[0], a[1], a[2], airspeed_rotor_2d);
 
-		/* IGE effect (In-Ground-Effect) adds more lift force when close to the ground.
-		 * We effectively give thrust_output a maximum of 28% bonus in that region.
-		 *
-		 * Effect starts at a rotor height of 1.25 rotor diameters.
-		 * http://www.copters.com/aero/ground_effect.html
-		 */
-		if(flags & SFMFlagRotorDiameter && model->rotor_diameter > 0)
-		{
-		    // horizontallity_coeff dampens IGE based on how horizontal to the
-		    // ground the aircraft is. It can be played with.
-		    double horizontallity_coeff = POW(ABS(cos_pitch) * ABS(cos_bank),2);
-		    double ige_height = 1.25 * model->rotor_diameter;
-		    // The rotor is as high as the center of the aircraft plus the
-		    // belly_height (assume distance from center to belly is the
-		    // same as from center to rotor.
-		    double rotor_height = ABS(pos->z - model->ground_elevation_msl + model->belly_height);
-		    // Increases towards 1 when ground_elevation is lower. Non
-		    // linear, approximate by the square.
-		    double ige_coeff = (1 - POW(CLIP(rotor_height, 0, ige_height) / ige_height, 2));
-		    // Increase thrust output 28% at most.
-		    // It is always assumed that the ground is horizontal and effect happens
-		    // when we are parallel to ground.
-		    thrust_output = (1 + 0.28 * ige_coeff * horizontallity_coeff) * thrust_output;
-		    // fprintf(stderr, "IGE. hor_coeff: %.2f, coeff: %.2f, thrust_bonus: %.2f\n", horizontallity_coeff, ige_coeff, 1 + 0.28 * ige_coeff);
-		}
+
 
 		/* Transverse Flow Effect (TF) happens from around 5 knots,
 		 * reaches max magnitude at 15 knots and dissapears by 25
@@ -1581,10 +1595,13 @@ int SFMForceApplyArtificial(
 		 * https://en.wikipedia.org/wiki/Transverse_flow_effect
 		 */
 
-		// The effect starts at SFMTFStart and follows a sin wave
-		// incidence until it is 0 again at SFMTFEnd. Otherwise 0.
-		// sin((speed-effect_start)*PI / effect_speed_range)
-		if (!model->landed_state && airspeed_rotor_2d > 0) {
+		if (flags & SFMFlagSingleMainRotor && // does not affect twin as they compensate.
+		    !model->landed_state &&
+		    airspeed_rotor_2d > 0
+		    ) {
+		    // The effect starts at SFMTFStart and follows a sin wave
+		    // incidence until it is 0 again at SFMTFEnd. Otherwise 0.
+		    // sin((speed-effect_start)*PI / effect_speed_range)
 		    double tf_coeff = sin(
 			(CLIP(airspeed_rotor_2d, SFMTFStart, SFMTFEnd) - SFMTFStart) * PI / (SFMTFEnd - SFMTFStart));
 
@@ -1598,7 +1615,7 @@ int SFMForceApplyArtificial(
 		/* Effective Transactional Lift (ETL): as airspeed increases, air
 		 * vortexes at the tip of the rotor blades dissappear,
 		 * providing a bonus lift effect. Fully effective at
-		 * SFMETLEnd (24 knots). Without ETL, we suffer a thrust
+		 * SFMETLSpeed (24 knots). Without ETL, we suffer a thrust
 		 * penalty of up to 25%.
 		 *
 		 * ETL effect on the advancing side vs retreating side
@@ -1614,7 +1631,9 @@ int SFMForceApplyArtificial(
 		double etl_thrust_coeff = 1 - POW(CLIP(airspeed_rotor_2d / SFMETLSpeed, 0, 1),2);
 		thrust_output = (1 - 0.25 * etl_thrust_coeff) * thrust_output;
 
-		if (!model->landed_state && airspeed_rotor_2d > 0) {
+		if (!model->landed_state &&
+		    airspeed_rotor_2d > 0
+		    ) {
 		    // Similar to TF, we add some pitch/bank changes while
 		    // entering ETL. The difference here is that nose rises
 		    // when going forward, rather than causing a roll.  By end
@@ -1639,8 +1658,9 @@ int SFMForceApplyArtificial(
 		 * there.
 		 */
 
-		if(!model->landed_state)
-		{
+		if(flags & SFMFlagSingleMainRotor && // does not affect twin rotors
+		   !model->landed_state
+		    ) {
 		    // torque_coeff: 1 at 0-speed, 0 at SFMETLEnd and negative
 		    // above that so that torque acceleration dissappears.
 		    double torque_coeff = 1 - airspeed_2d / SFMETLSpeed;
diff --git a/src/simop.c b/src/simop.c
index 4d9c73e..f0a12cd 100644
--- a/src/simop.c
+++ b/src/simop.c
@@ -1054,7 +1054,7 @@ void SARSimSetSFMValues(
 			      SFMFlagCrashContactShape | SFMFlagCrashableSizeRadius |
 			      SFMFlagCrashableSizeZMin | SFMFlagCrashableSizeZMax |
 			      SFMFlagTouchDownCrashResistance | SFMFlagCollisionCrashResistance |
-			      SFMFlagStopped | SFMFlagLength | SFMFlagWingspan | SFMFlagRotorDiameter
+			      SFMFlagStopped | SFMFlagLength | SFMFlagWingspan
 		);
 
 	    /* Update flight model type only if SFM not in slew mode */
@@ -1110,7 +1110,32 @@ void SARSimSetSFMValues(
 	    TAR_PTR->belly_height = SRC_PTR->belly_height;
 	    TAR_PTR->length = SRC_PTR->length;
 	    TAR_PTR->wingspan = SRC_PTR->wingspan;
-	    TAR_PTR->rotor_diameter = SRC_PTR->rotor_diameter;
+	    {
+		double rotor_diameter = 0.0;
+		int main_rotor_count = 0;
+		// Calculate effective rotor diameter
+		for (i = 0; i<SRC_PTR->total_rotors; i++) {
+		    sar_obj_rotor_struct *rotor = SRC_PTR->rotor[i];
+
+		    // Identify main rotors as rotors with blades that follow
+		    // controls or can pitch.
+		    if (rotor->total_blades > 0 &&
+			(rotor->flags & SAR_ROTOR_FLAG_FOLLOW_CONTROLS ||
+			 rotor->flags & SAR_ROTOR_FLAG_CAN_PITCH)
+			) {
+			main_rotor_count++;
+			// Effective diameter will come from the biggest rotor.
+			rotor_diameter = MAX(rotor_diameter, rotor->radius*2);
+			}
+		}
+		if (rotor_diameter > 0) {
+		    TAR_PTR->rotor_diameter = rotor_diameter;
+		    TAR_PTR->flags |= SFMFlagRotorDiameter;
+		}
+		if (main_rotor_count == 1)
+		    TAR_PTR->flags |= SFMFlagSingleMainRotor;
+	    }
+
 	    TAR_PTR->ground_elevation_msl = obj_ptr->ground_elevation_msl;
 	    TAR_PTR->gear_state = (SFMBoolean)((lgear_ptr != NULL) ?
 		(lgear_ptr->flags & SAR_OBJ_PART_FLAG_STATE) : False