cs_Math.h

Go to the documentation of this file.

/*
 * Author: Crownstone Team
 * Copyright: Crownstone (https://crownstone.rocks)
 * Date: Okt 18, 2019
 * License: LGPLv3+, Apache License 2.0, and/or MIT (triple-licensed)
 */
#pragma once
 
#include <algorithm>
 
#ifdef HOST_TARGET
#include <limits>
#endif
 
namespace CsMath {
 
template <class T, class U>
constexpr inline T SafeAdd(T lhs, U rhs) {
    T min = std::numeric_limits<T>::lowest();
    T max = std::numeric_limits<T>::max();
 
    if (rhs > 0) {
        return (lhs > max - rhs) ? max : T(lhs + rhs);
    }
    if (rhs < 0) {
        return (lhs < min - rhs) ? min : T(lhs + rhs);
    }
    return lhs;
}
 
template <class T, class U = int>
constexpr inline T& Increase(T& obj, U diff = 1) {
    obj = SafeAdd(obj, diff);
    return obj;
}
 
template <class T, class U = int>
constexpr inline T& Decrease(T& obj, U diff = 1) {
    obj = SafeAdd(obj, -diff);
    return obj;
}
 
template <class T, class M, class U = int>
constexpr inline decltype(auto) DecreaseMember(T& obj, M member, U diff = 1) {
    obj.*member = SafeAdd(obj.*member, -diff);
    return obj.*member;
}
 
template <class T, class S>
auto mod(T v, S m) -> decltype(v % m) {
    if (m == 0) {
        return v;
    }
    if (m < 0) {
        // flip [m] doesn't change the value.
        return mod(v, -m);
    }
    if (v < 0) {
        // flip [v] twice won't change it either, but be careful not to return
        // [m] instead of 0 when val is a multiple of m.
        if (auto neg_v = mod(-v, m)) {
            // hidden recursion in if clause will only go one level deep ;)
            return m - neg_v;
        }
        else {
            return 0;
        }
    }
    // m positive, v non-negative, yay C++ actually works in that case.
    return v % m;
}
 
template <class T, class S>
auto min(T l, S r) {
    return l < r ? l : r;
}
 
template <class T, class S>
auto max(T l, S r) {
    return l > r ? l : r;
}
 
template <class V, class L, class U>
auto clamp(V value, L lower, U upper) {
    return min(max(value, lower), upper);
}
 
template <class T, class S = T>
class Interval {
private:
    T low, high;
    bool invert;
 
public:
    Interval(T base, S diff, bool inv = false) : low(base), high(base + (inv ? -diff : diff)), invert(inv) {
        // notes:
        // - addition base+diff is allowed to overflow/underflow.
        // - having a different type S for diff allows negative
        //   values without conflicting type resolutions.
        if ((diff < 0) ^ invert) {
            std::swap(low, high);
        }
    }
 
    T lowerbound() { return low; }
    T upperbound() { return high; }
 
    // E.g.
    // Interval<uint8_t> i(200,100)
    // i.contains(0) == true.
    // considers this interval as half open (lower closed) so that the return value is
    // true if [val] is enclosed in the interval or equal to the lower boundary.
    bool contains(T val) {
        return low < high                             // reversed interval?
                       ? (low <= val && val < high)   // nope, both must hold
                       : (low <= val || val < high);  // yup, only one can hold
    }
 
    // considers this interval as open ended so that the return value is
    // true if [val] is enclosed in the interval or on the boundary.
    bool ClosureContains(T val) {
        return low < high                              // reversed interval?
                       ? (low <= val && val <= high)   // nope, both must hold
                       : (low <= val || val <= high);  // yup, only one can hold
    }
 
    // considers this interval as open ended so that the return value is
    // true if [val] is strictly enclosed in the interval.
    bool InteriorContains(T val) {
        return low < high                            // reversed interval?
                       ? (low < val && val < high)   // nope, both must hold
                       : (low < val || val < high);  // yup, only one can hold
    }
};
 
template <class T>
T round(float val) {
    // since numeric types are implicitly converted from float by flooring them,
    // adding 0.5 results in conventional rounding.
    return T(val + 0.5);
}
}  // namespace CsMath