curves.h

Go to the documentation of this file.

 
#pragma once
 
#include "src/system/utils/utils.h"
 
#include <algorithm>
#include <cmath>
#include <vector>
 
namespace lampda {
namespace common {
namespace curves {
 
template<typename T, typename U> struct Point
{
  T x; 
  U y; 
};
 
template<typename T, typename U> class LinearCurve
{
public:
  using point_t = Point<T, U>;
 
  LinearCurve(const std::vector<point_t>& points)
  {
    // first fail fast
    assert(points.size() >= 2 && "Linear curve must have more than 1 points");
 
    pts = points;
 
    for (const auto& p: pts)
    {
      // check A
      assert(not std::isnan(p.x) && "invalid value in curve parameters");
      assert(std::isfinite(p.x) && "invalid value in curve parameters");
      assert(not std::isnan(p.y) && "invalid value in curve parameters");
      assert(std::isfinite(p.y) && "invalid value in curve parameters");
    }
 
    // sort the vector by the x coordinate
    auto lbd = [](const point_t& a, const point_t& b) {
      return a.x < b.x;
    };
    std::sort(pts.begin(), pts.end(), lbd);
 
    // remove duplicates (after sorting)
    std::vector<point_t> nPoints;
    nPoints.reserve(pts.size());
 
    point_t lastPt = pts[0];
    nPoints.emplace_back(lastPt);
    for (size_t i = 1; i < pts.size(); i++)
    {
      const auto& p = pts[i];
      if (lastPt.x != p.x and lastPt.y != p.y)
        nPoints.emplace_back(p);
      lastPt = p;
    }
    // last check
    pts = nPoints;
    assert(pts.size() >= 2 && "Linear curve must have more than 1 points");
  }
 
  U sample(const T x) const
  {
    point_t lastPt = pts[0];
    // bounds failure
    if (std::isnan(x))
      return lastPt.y;
    if (x <= lastPt.x)
      return lastPt.y;
    if (x >= pts.back().x)
      return pts.back().y;
 
    for (size_t i = 1; i < pts.size(); ++i)
    {
      const point_t& pt = pts[i];
      // in this segment bound
      if (x >= lastPt.x and x <= pt.x)
      {
        return lmpd_map<U>(x, lastPt.x, pt.x, lastPt.y, pt.y);
      }
      // update last point
      lastPt = pt;
    }
 
    // highest bound failure
    return lastPt.y;
  }
 
private:
  std::vector<point_t> pts;
};
 
template<typename T, typename U> class ExponentialCurve
{
public:
  using point_t = Point<T, U>;
 
  ExponentialCurve(const point_t& pointA, const point_t& pointB, const double exponent = 15.0) :
    lowerBound(pointA.y),
    upperBound(pointB.y)
  {
    const double div = pointA.y / static_cast<double>(pointB.y);
    if (div <= -1.0)
    {
      // Error: linear approx...
      _exp = 1;
      _a = 0;
      _b = 1;
      return;
    }
    const double A = exp(log(div) / exponent);
    _exp = exponent;
    _a = (static_cast<double>(pointA.x) - static_cast<double>(pointB.x) * A) / (A - 1);
    _b = static_cast<double>(pointA.y) / pow(static_cast<double>(pointA.x + _a), exponent);
  }
 
  float sample(const T x) const
  {
    const float res = pow(static_cast<double>(x) + _a, _exp) * _b;
    return lmpd_constrain<float>(res, lowerBound, upperBound);
  }
 
private:
  const T lowerBound;
  const T upperBound;
 
  double _exp;
  double _a;
  double _b;
};
 
} // namespace curves
} // namespace common
} // namespace lampda