WIP Interpolating high-to-low

radixmath/interpolate.cc

@@ -208,7 +208,8 @@ std::vector<T> interpolateToOtherBaseValues(
       size_t lowerBaseIndex = 0, higherBaseIndex = 0;
       for (size_t j = 0; j < baseValues.size(); ++j)
       {
-        if (baseValues[j] > newBaseValues[i])
+        if ((lowToHigh && (baseValues[j] >= newBaseValues[i])) ||
+            (!lowToHigh && (baseValues[j] <= newBaseValues[i])))
         {
           higherBaseIndex = j;
           break;
@@ -238,19 +239,44 @@ std::vector<T> interpolateToOtherBaseValues(
         }
       }
 
-      finalInterpolatedValues[i] =
-          lastGoodValue +
-          ((nextGoodValue - lastGoodValue) *
-           ((newBaseValues[i] - baseValues[lowerBaseIndex]) /
-            (baseValues[higherBaseIndex] - baseValues[lowerBaseIndex])));
+      // If the two values are the same, we are on an existing data point
+      // - don't interpolate
+      if (higherBaseIndex == lowerBaseIndex)
+      {
+        finalInterpolatedValues[i] = lastGoodValue;
+      }
+      else
+      {
+        finalInterpolatedValues[i] =
+            lastGoodValue +
+            ((nextGoodValue - lastGoodValue) *
+             ((newBaseValues[i] - baseValues[lowerBaseIndex]) /
+              (baseValues[higherBaseIndex] - baseValues[lowerBaseIndex])));
+      }
 
       if (circular)
       {
         finalInterpolatedValues[i] = fmod(finalInterpolatedValues[i], 360.0);
       }
+      //      radix_line("interp: previous " << lastGoodValue << ", next "
+      //                                     << nextGoodValue << ", result "
+      //                                     << finalInterpolatedValues[i]);
     }
   }
   radix_line("Interpolation to new base values complete");
+  radix_block(
+      radix("  Initial base values:         ");
+      for (float f
+           : baseValues) { radix(" " << f); } radix_line("");
+      radix("  Initial interpolated values: ");
+      for (float f
+           : initialInterpolatedValues) { radix(" " << f); } radix_line("");
+      radix("  New base values:             ");
+      for (float f
+           : newBaseValues) { radix(" " << f); } radix_line("");
+      radix("  Final interpolated values:   ");
+      for (float f
+           : finalInterpolatedValues) { radix(" " << f); } radix_line(""););
 
   return finalInterpolatedValues;
 }

radixmath/tests/tstInterpolate.cc

@@ -150,20 +150,18 @@ TEST(Interpolate, NewBaseValues)
                 expectValues1[i] * tolerance);
   }
 
-  // Uncomment if negative gradient base values required
-  //  std::vector<float> baseValues2{3000.f, 2000.f, 1000.f};
-  //  std::vector<float> newBaseValues2{3500.f, 2500.f, 1500.f, 500.f};
-  //  std::vector<float> interpValues2{30.5, 20.5, 10.5};
-  //  std::vector<float> expectValues2{30.5, 25.5, 15.5, 10.5};
-  //  std::vector<float> testValues2 =
-  //      interpolateToOtherBaseValues(baseValues2, newBaseValues2,
-  //      interpValues2);
-  //  ASSERT_EQ(expectValues2.size(), testValues2.size());
-  //  for (int i = 0; i < testValues2.size(); ++i)
-  //  {
-  //    EXPECT_NEAR(expectValues2[i], testValues2[i], expectValues2[i] *
-  //    tolerance);
-  //  }
+  // High-to-low base values
+  std::vector<float> baseValues2{3000.f, 2000.f, 1000.f};
+  std::vector<float> newBaseValues2{3500.f, 2500.f, 1500.f, 500.f};
+  std::vector<float> interpValues2{30.5, 20.5, 10.5};
+  std::vector<float> expectValues2{30.5, 25.5, 15.5, 10.5};
+  std::vector<float> testValues2 =
+      interpolateToOtherBaseValues(baseValues2, newBaseValues2, interpValues2);
+  ASSERT_EQ(expectValues2.size(), testValues2.size());
+  for (int i = 0; i < testValues2.size(); ++i)
+  {
+    EXPECT_NEAR(expectValues2[i], testValues2[i], expectValues2[i] * tolerance);
+  }
 }
 
 TEST(Interpolate, NewBaseValuesDoubles)