Add methods for comparisons within tolerance

fbw-a32nx/src/systems/fmgc/src/guidance/vnav/VnavDriver.ts

@@ -32,6 +32,7 @@ import {
 } from './profile/NavGeometryProfile';
 import { VMLeg } from '@fmgc/guidance/lnav/legs/VM';
 import { FMLeg } from '@fmgc/guidance/lnav/legs/FM';
+import { MathUtils } from '@flybywiresim/fbw-sdk';
 
 export class VnavDriver implements GuidanceComponent {
   version: number = 0;
@@ -288,11 +289,10 @@ export class VnavDriver implements GuidanceComponent {
 
       const isPastCstrDeceleration =
         checkpoint.reason === VerticalCheckpointReason.StartDecelerationToConstraint &&
-        currentDistanceFromStart - checkpoint.distanceFromStart > -1e-4;
+        MathUtils.isCloseToGreaterThan(currentDistanceFromStart, checkpoint.distanceFromStart);
       const isPastLimitDeceleration =
         checkpoint.reason === VerticalCheckpointReason.StartDecelerationToLimit &&
-        currentAltitude - checkpoint.altitude < 1e-4;
-
+        MathUtils.isCloseToLessThan(currentAltitude, checkpoint.altitude);
       if (
         isSpeedChangePoint(checkpoint) &&
         checkpoint.targetSpeed >= decelPointSpeed &&

fbw-a32nx/src/systems/fmgc/src/guidance/vnav/descent/ApproachPathBuilder.ts

@@ -147,6 +147,8 @@ export class ApproachPathBuilder {
 
   private readonly configuration = new AircraftConfigurationRegister();
 
+  private static readonly DISTANCE_EPSILON = 1e-4;
+
   constructor(
     private observer: VerticalProfileComputationParametersObserver,
     atmosphericConditions: AtmosphericConditions,
@@ -227,7 +229,7 @@ export class ApproachPathBuilder {
     }
 
     const speedTarget = speedProfile.getTarget(
-      sequence.lastCheckpoint.distanceFromStart - 1e-4,
+      sequence.lastCheckpoint.distanceFromStart - ApproachPathBuilder.DISTANCE_EPSILON,
       sequence.lastCheckpoint.altitude,
       ManagedSpeedType.Descent,
     );
@@ -347,7 +349,7 @@ export class ApproachPathBuilder {
     const finalError = distanceTraveled - desiredDistanceToCover;
 
     // Even without decelerating and only descending, we cannot make the constraint
-    if (solution < 1e-4 && finalError > 1e-4) {
+    if (MathUtils.isCloseToNegative(solution) && !MathUtils.isCloseToNegative(finalError)) {
       // Try with speedbrakes
       const solutionWithSpeedbrakes = BisectionMethod.findZero(
         (distance) => tryDecelDistance(distance, true),
@@ -360,7 +362,10 @@ export class ApproachPathBuilder {
       const distanceTraveledWithSpeedbrakes = solutionWithSpeedbrakes + -descentSegment.distanceTraveled;
       const finalErrorWithSpeedbrakes = distanceTraveledWithSpeedbrakes - desiredDistanceToCover;
 
-      if (solutionWithSpeedbrakes < 1e-4 && finalErrorWithSpeedbrakes > 1e-4) {
+      if (
+        MathUtils.isCloseToNegative(solutionWithSpeedbrakes) &&
+        !MathUtils.isCloseToNegative(finalErrorWithSpeedbrakes)
+      ) {
         // Insert TOO STEEP PATH
         const scaling = desiredDistanceToCover / -descentSegment.distanceTraveled;
         this.scaleStepBasedOnLastCheckpoint(sequence.lastCheckpoint, descentSegment, scaling);
@@ -426,12 +431,14 @@ export class ApproachPathBuilder {
 
     const isDoneDeclerating = () =>
       decelerationSequence.lastCheckpoint.reason === VerticalCheckpointReason.Decel ||
-      decelerationSequence.lastCheckpoint.distanceFromStart - targetDistanceFromStart <= 1e-4; // We really only want to prevent floating point errors here
+      MathUtils.isCloseToLessThan(decelerationSequence.lastCheckpoint.distanceFromStart, targetDistanceFromStart); // We really only want to prevent floating point errors here
 
     for (let i = 0; i < 10 && !isDoneDeclerating(); i++) {
       const { distanceFromStart, altitude, speed, remainingFuelOnBoard } = decelerationSequence.lastCheckpoint;
 
-      const speedConstraint = speedProfile.getMaxDescentSpeedConstraint(distanceFromStart - 1e-4);
+      const speedConstraint = speedProfile.getMaxDescentSpeedConstraint(
+        distanceFromStart - ApproachPathBuilder.DISTANCE_EPSILON,
+      );
       const flapTargetSpeed = FlapConfigurationProfile.findNextExtensionSpeed(speed, parameters);
 
       // This is the managed descent speed, or the speed limit speed.
@@ -477,7 +484,7 @@ export class ApproachPathBuilder {
 
         lastAccelerationCheckpointIndex = decelerationSequence.length;
 
-        if (decelerationStep.distanceTraveled < 1e-4) {
+        if (MathUtils.isCloseToNegative(decelerationStep.distanceTraveled)) {
           // We tried to declerate, but it took us beyond targetDistanceFromStart, so we scale down the step
           const scaling = Math.min(1, remainingDistance / decelerationStep.distanceTraveled);
           this.scaleStepBasedOnLastCheckpoint(decelerationSequence.lastCheckpoint, decelerationStep, scaling);

fbw-a32nx/src/systems/fmgc/src/guidance/vnav/descent/IdlePathBuilder.ts

@@ -1,3 +1,4 @@
+import { MathUtils } from '@flybywiresim/fbw-sdk';
 import { AircraftConfig } from '@fmgc/flightplanning/AircraftConfigTypes';
 import { AtmosphericConditions } from '@fmgc/guidance/vnav/AtmosphericConditions';
 import { SpeedProfile, ManagedSpeedType } from '@fmgc/guidance/vnav/climb/SpeedProfile';
@@ -16,6 +17,12 @@ import { HeadwindProfile } from '@fmgc/guidance/vnav/wind/HeadwindProfile';
 export class IdlePathBuilder {
   private readonly idleDescentStrategy: DescentStrategy;
 
+  /**
+   * Sometimes we want to check if two distances are close enough to each other to be considered equal, or sometimes we
+   * want to use a distance "just before" or "just after" another distance.
+   */
+  private static readonly DISTANCE_EPSILON = 1e-4;
+
   constructor(
     private computationParametersObserver: VerticalProfileComputationParametersObserver,
     private atmosphericConditions: AtmosphericConditions,
@@ -85,16 +92,20 @@ export class IdlePathBuilder {
     for (
       let i = 0;
       i < 100 &&
-      sequence.lastCheckpoint.distanceFromStart - toDistance > 1e-4 &&
-      topOfDescentAltitude - sequence.lastCheckpoint.altitude > 1;
+      !MathUtils.isCloseToLessThan(sequence.lastCheckpoint.distanceFromStart, toDistance) &&
+      !MathUtils.isCloseToLessThan(topOfDescentAltitude, sequence.lastCheckpoint.altitude, 1);
       i++
     ) {
       const { distanceFromStart, altitude, speed, remainingFuelOnBoard } = sequence.lastCheckpoint;
       const headwind = windProfile.getHeadwindComponent(distanceFromStart, altitude);
       const isUnderSpeedLimitAltitude =
         speedProfile.shouldTakeDescentSpeedLimitIntoAccount() && altitude < descentSpeedLimit.underAltitude;
 
-      const casTarget = speedProfile.getTarget(distanceFromStart - 1e-4, altitude + 1e-4, ManagedSpeedType.Descent);
+      const casTarget = speedProfile.getTarget(
+        distanceFromStart - IdlePathBuilder.DISTANCE_EPSILON,
+        altitude + IdlePathBuilder.DISTANCE_EPSILON,
+        ManagedSpeedType.Descent,
+      );
       const currentSpeedTarget = Math.min(
         casTarget,
         this.atmosphericConditions.computeCasFromMach(managedDescentSpeedMach, altitude),
@@ -237,7 +248,7 @@ export class IdlePathBuilder {
   ): Generator<MaxSpeedConstraint> {
     for (let i = constraints.length - 1; i >= 0; ) {
       // Small tolerance here, so we don't get stuck on a constraint
-      if (constraints[i].distanceFromStart - sequence.lastCheckpoint.distanceFromStart > -1e-4) {
+      if (MathUtils.isCloseToGreaterThan(constraints[i].distanceFromStart, sequence.lastCheckpoint.distanceFromStart)) {
         i--;
         continue;
       }

fbw-common/src/systems/shared/src/MathUtils.ts

@@ -595,4 +595,57 @@ export class MathUtils {
 
     return MathUtils.interpolate(j, table[0][c1], table[0][c2], interpolatedRowAtC1, interpolatedRowAtC2);
   }
+
+  /**
+   * Checks whether two numbers are within a certain epsilon of each other.
+   * @param a
+   * @param b
+   * @param epsilon the absolute tolerance
+   * @returns true if the numbers are within epsilon of each other
+   */
+  public static isAboutEqual(a: number, b: number, epsilon = 1e-4): boolean {
+    return Math.abs(a - b) < epsilon;
+  }
+
+  /**
+   * Checks whether a number is positive and within a certain epsilon of zero.
+   * @param num
+   * @param epsilon the absolute tolerance
+   * @returns true if the number is positive and within epsilon of zero
+   */
+  public static isCloseToPositive(num: number, epsilon = 1e-4): boolean {
+    return num > -Math.abs(epsilon);
+  }
+
+  /**
+   * Checks whether a number is negative and within a certain epsilon of zero.
+   * @param num
+   * @param epsilon the absolute tolerance
+   * @returns true if the number is negative and within epsilon of zero
+   */
+  public static isCloseToNegative(num: number, epsilon = 1e-4): boolean {
+    return this.isCloseToPositive(-num, epsilon);
+  }
+
+  /**
+   * Checks whether a > b or a is within a certain epsilon of b.
+   * @param a
+   * @param b
+   * @param epsilon the absolute tolerance
+   * @returns true if the number is greater than or within epsilon of the other
+   */
+  public static isCloseToGreaterThan(a: number, b: number, epsilon = 1e-4): boolean {
+    return this.isCloseToPositive(a - b, epsilon);
+  }
+
+  /**
+   * Checks whether a < b or a is within a certain epsilon of b.
+   * @param a
+   * @param b
+   * @param epsilon the absolute tolerance
+   * @returns true if the number is less than or within epsilon of the other
+   */
+  public static isCloseToLessThan(a: number, b: number, epsilon = 1e-4): boolean {
+    return this.isCloseToNegative(a - b, epsilon);
+  }
 }