diff --git a/cxx/isce3/core/Utilities.h b/cxx/isce3/core/Utilities.h index 5c29cb221..1cc96bd0e 100644 --- a/cxx/isce3/core/Utilities.h +++ b/cxx/isce3/core/Utilities.h @@ -16,6 +16,8 @@ #include #include #include +#include +#include // Macro wrappers to check vector lengths // (adds calling function and variable name information to the exception) @@ -332,27 +334,80 @@ namespace isce3 { namespace core { } } - /** Searches array for index closest to provided value */ - inline int binarySearch(const std::valarray & array, double value) { - - // Do the binary search + /** Searches array for index closest to provided value + * + * Assumes the array is sorted in ascending order and contains no NaNs. + * + * @param array The input array, which must be sorted in ascending order + * and contain no NaNs. + * @param value The value to search for. + * @param always_pick_left If true, and `value` is strictly between two + * array elements, the function will return the lower index (left). + * If false, the function returns the closest index (left or right). + * + * @return The index of the array element closest to `value`. + */ + inline int binarySearch(const std::valarray & array, double value, + const bool always_pick_left = false) { + + if (array.size() == 0) { + throw std::invalid_argument("input array must contain at least 1 element"); + } + if (array.size() == 1) { + return 0; + } + int left = 0; int right = array.size() - 1; - int index; - while (left <= right) { - const int middle = static_cast(std::round(0.5 * (left + right))); - if (left == (right - 1)) { - index = left; - return index; + + // Test extreme values + if (value <= array[left]) { + return left; + } + if (array[right] <= value) { + return right; + } + + std::size_t count = 0; + + // `max_iter` provides a safeguard against unexpected behavior. + // The loop is guaranteed to converge with correct input, but we include + // this check as a safe measure for unexpected edge cases. + constexpr std::size_t padding = 2; + std::size_t max_iter = std::ceil(std::log2(array.size())) + padding; + while (left + 1 < right) { + const int middle = left + (right - left) / 2; + const auto middle_value = array[middle]; + if (std::isnan(middle_value)) { + throw std::invalid_argument("input array may not contain NaN values"); } - if (array[middle] <= value) { + if (middle_value < value) { left = middle; - } else if (array[middle] > value) { + } else { right = middle; } + if (++count > max_iter) { + throw std::runtime_error( + "Binary search failed to converge within the allowed iterations."); + } } - index = left; - return index; + + // Test for NaNs + if (std::isnan(array[left]) || std::isnan(array[right])) { + throw std::invalid_argument("input array may not contain NaN values"); + } + + // If always pick left and the `right` index is not pointing at value + if (always_pick_left && value < array[right]) { + return left; + } + + // Return the closest of left and right + if (std::abs(array[left] - value) <= std::abs(array[right] - value)) { + return left; + } + + return right; } /** Clip a number between an upper and lower range (implements std::clamp for older GCC) */ diff --git a/cxx/isce3/geometry/Topo.cpp b/cxx/isce3/geometry/Topo.cpp index 813425690..5bdbdfb4d 100644 --- a/cxx/isce3/geometry/Topo.cpp +++ b/cxx/isce3/geometry/Topo.cpp @@ -812,7 +812,9 @@ setLayoverShadow(TopoLayers& layers, DEMInterpolator& demInterp, // Compute nearest ctrack index for current ctrackGrid value const double crossTrack = ctrackGrid[i]; - int k = isce3::core::binarySearch(ctrack, crossTrack); + const bool always_pick_left = true; + int k = isce3::core::binarySearch(ctrack, crossTrack, always_pick_left); + // Adjust edges if necessary if (k == (width - 1)) { k = width - 2; diff --git a/tests/cxx/isce3/Sources.cmake b/tests/cxx/isce3/Sources.cmake index 80c5f37cb..aae2006be 100644 --- a/tests/cxx/isce3/Sources.cmake +++ b/tests/cxx/isce3/Sources.cmake @@ -23,6 +23,7 @@ core/projections/utm.cpp core/serialization/serializeAttitude.cpp core/serialization/serializeDoppler.cpp core/serialization/serializeOrbit.cpp +core/utilities/binarySearch.cpp fft/fft.cpp fft/fftplan.cpp fft/fftutil.cpp diff --git a/tests/cxx/isce3/core/utilities/binarySearch.cpp b/tests/cxx/isce3/core/utilities/binarySearch.cpp new file mode 100644 index 000000000..93f2dfea6 --- /dev/null +++ b/tests/cxx/isce3/core/utilities/binarySearch.cpp @@ -0,0 +1,100 @@ +#include + +#include + +using isce3::core::binarySearch; + +TEST(BinarySearchTest, SingleElementArray) { + std::valarray arr = {2.0}; + EXPECT_EQ(binarySearch(arr, 0.0), 0); + EXPECT_EQ(binarySearch(arr, 100.0), 0); +} + +TEST(BinarySearchTest, SingleElementArrayPickLeft) { + const bool always_pick_left = true; + std::valarray arr = {2.0}; + EXPECT_EQ(binarySearch(arr, 0.0, always_pick_left), 0); + EXPECT_EQ(binarySearch(arr, 100.0, always_pick_left), 0); +} + + +TEST(BinarySearchTest, ClosestMatchExact) { + std::valarray arr = {1.0, 3.0, 5.0, 7.0}; + EXPECT_EQ(binarySearch(arr, 3.0), 1); + EXPECT_EQ(binarySearch(arr, 7.0), 3); +} + +TEST(BinarySearchTest, ClosestMatchExactPickLeft) { + const bool always_pick_left = true; + std::valarray arr = {1.0, 3.0, 5.0, 7.0}; + EXPECT_EQ(binarySearch(arr, 3.0, always_pick_left), 1); + EXPECT_EQ(binarySearch(arr, 7.0, always_pick_left), 3); +} + +TEST(BinarySearchTest, ClosestMatchInBetween) { + std::valarray arr = {1.0, 3.0, 5.0, 7.0}; + EXPECT_EQ(binarySearch(arr, 2.1), 1); // closer to `3` + EXPECT_EQ(binarySearch(arr, 4.1), 2); // closer to `5` + EXPECT_EQ(binarySearch(arr, 6.4), 3); // closer to `7` +} + +TEST(BinarySearchTest, ClosestMatchInBetweenPickLeft) { + const bool always_pick_left = true; + std::valarray arr = {1.0, 3.0, 5.0, 7.0}; + EXPECT_EQ(binarySearch(arr, 2.1, always_pick_left), 0); + EXPECT_EQ(binarySearch(arr, 4.1, always_pick_left), 1); + EXPECT_EQ(binarySearch(arr, 6.4, always_pick_left), 2); +} + +TEST(BinarySearchTest, ValueBeforeFirstElement) { + std::valarray arr = {1.0, 2.0, 3.0}; + EXPECT_EQ(binarySearch(arr, -1.0), 0); // `-1` closest to `1` +} + +TEST(BinarySearchTest, ValueBeforeFirstElementPickLeft) { + const bool always_pick_left = true; + std::valarray arr = {1.0, 2.0, 3.0}; + EXPECT_EQ(binarySearch(arr, -1.0, always_pick_left), 0); +} + +TEST(BinarySearchTest, ValueAfterLastElement) { + std::valarray arr = {1.0, 2.0, 3.0}; + EXPECT_EQ(binarySearch(arr, 5.0), 2); // `5` closest to `3` +} + +TEST(BinarySearchTest, ValueAfterLastElementPickLeft) { + const bool always_pick_left = true; + std::valarray arr = {1.0, 2.0, 3.0}; + EXPECT_EQ(binarySearch(arr, 5.0, always_pick_left), 2); +} + +TEST(BinarySearchTest, EqualDistanceChoosesLeft) { + std::valarray arr = {1.0, 2.0, 3.0, 4.0}; + + // `2` and `3` equally distant; picks left (`2`) + EXPECT_EQ(binarySearch(arr, 2.5), 1); +} + +TEST(BinarySearchTest, EqualDistanceChoosesLeftPickLeft) { + const bool always_pick_left = true; + std::valarray arr = {1.0, 2.0, 3.0, 4.0}; + + // `2` and `3` equally distant; picks left (`2`) + EXPECT_EQ(binarySearch(arr, 2.5, always_pick_left), 1); +} + +TEST(BinarySearchTest, ThrowsOnEmptyArray) { + std::valarray arr; + EXPECT_THROW(binarySearch(arr, 0.0), std::invalid_argument); +} + +TEST(BinarySearchTest, ThrowsOnNaN) { + std::valarray arr = {1.0, 2.0, NAN, 4.0}; + EXPECT_THROW(binarySearch(arr, 3.0), std::invalid_argument); +} + +int main(int argc, char * argv[]) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}