algorithm.hpp

Go to the documentation of this file.

#pragma once
 
// std
#include <utility>
#include <optional>
 
// Eigen
#include <Eigen/Dense>
 
namespace controlpp{
 
    template<class Itr, class T>
    std::optional<std::pair<Itr, Itr>> find_enclosing(Itr first, Itr last, const T& v){
        Itr itr_low = first;
        Itr itr_high = first;
        ++itr_high;
 
        for(; itr_high != last; (void)++itr_low, (void)++itr_high){
            if(*itr_low <= v && v <= *itr_high){
                std::pair<Itr, Itr> result(itr_low, itr_high);
                return result;
            }
        }
 
        return std::nullopt;
    }
 
    template<class T, int Size = Eigen::Dynamic>
    std::optional<std::pair<const T*, const T*>> find_enclosing(const Eigen::Vector<T, Size>& range, const T& v){
        const T* first = range.data();
        const T* last = range.data() + range.size();
        return find_enclosing(first, last, v);
    }
 
 
    template<class Iterator, class T>
    void shift_up(Iterator first, Iterator last, const T& v0 = T(0)){
        // the iterator that will be assigned to
        Iterator itr_to = last;
        --itr_to;
 
        // the iterator that will be read from
        Iterator itr_from = last;
        --itr_from;
        --itr_from;
        while(itr_to != first){
            *itr_to = std::move(*itr_from);
            --itr_to;
            --itr_from;
        }
        
        // assign the first value
        *first = v0;
    }
 
    template<class Iterator, class T>
    void shift_up(Iterator first, Iterator last, T&& v0 = T(0)){
        // the iterator that will be assigned to
        Iterator itr_to = last;
        --itr_to;
 
        // the iterator that will be read from
        Iterator itr_from = last;
        --itr_from;
        --itr_from;
 
        while(itr_to != first){
            *itr_to = std::move(*itr_from);
            --itr_to;
            --itr_from;
        }
        
        // assign the first value
        *first = std::move(v0);
    }
 
}