Program Listing for File algorithm.hpp

↰ Return to documentation for file (src/navtk/utils/algorithm.hpp)

#pragma once

#include <algorithm>
#include <functional>
#include <type_traits>
#include <utility>

namespace navtk {
namespace utils {

template <typename X, typename Y>
Y trapezoidal_area(X x0, const Y& y0, X x1, const Y& y1) {
    static_assert(std::is_convertible<X, double>::value, "X must be convertible to a double");
    return (x1 - x0) * (y0 + y1) / 2.0;
}

template <typename Iterator, typename Compare = std::less<typename Iterator::value_type>>
struct InRange {
    using T = typename Iterator::value_type;

    std::pair<Iterator, Iterator> get(Iterator begin,
                                      Iterator end,
                                      const T& t0,
                                      const T& t1) const {
        Compare compare;
        auto lower = std::lower_bound(begin, end, t0, compare);
        auto upper = std::upper_bound(lower, end, t1, compare);
        if (lower == upper) lower = upper = end;
        return {lower, upper};
    }
};

template <typename Iterator, typename Compare = std::less<typename Iterator::value_type>>
struct NearestNeighbors {
    using T = typename Iterator::value_type;

    std::pair<Iterator, Iterator> get(Iterator begin, Iterator end, const T& t) const {
        if (begin == end) return {end, end};

        Compare compare;
        auto upper = std::upper_bound(begin, end, t, compare);
        if (upper == begin) {
            return {end, begin};
        }

        auto lower = upper - 1;
        if (!compare(*lower, t) && !compare(t, *lower)) {
            upper = lower;  // exact match
        }
        return {lower, upper};
    }
};

}  // namespace utils
}  // namespace navtk