text: #include "math/Curve.h"
  #include <algorithm>
  
  NAMESPACE_SPH_BEGIN
  
  class Vector4 {
  private:
      Float v[4];
  
  public:
      Vector4(const Float x1, const Float x2, const Float x3, const Float x4)
          : v{ x1, x2, x3, x4 } {}
  
      INLINE Float operator[](const Size i) const {
          SPH_ASSERT(unsigned(i) < 4);
          return v[i];
      }
  };
  
  static Float dot(const Vector4& v1, const Vector4& v2) {
      return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2] + v1[3] * v2[3];
  }
  
  class Matrix4 {
  private:
      Vector4 rows[4];
  
  public:
      Matrix4(const Vector4& v1, const Vector4& v2, const Vector4& v3, const Vector4& v4)
          : rows{ v1, v2, v3, v4 } {}
  
      Vector4 row(const Size i) const {
          return rows[i];
      }
  
      Vector4 operator*(const Vector4& v) const {
          return Vector4(dot(rows[0], v), dot(rows[1], v), dot(rows[2], v), dot(rows[3], v));
      }
  };
  
  const Matrix4 CURVE_MATRIX{
      { 1.f, 0.f, 0.f, 0.f },
      { 0.f, 0.f, 1.f, 0.f },
      { -3.f, 3.f, -2.f, -1.f },
      { 2.f, -2.f, 1.f, 1.f },
  };
  
  static Float getDist(const CurvePoint& p1, const CurvePoint& p2) {
      return sqrt(sqr(p1.x - p2.x) + sqr(p1.y - p2.y));
  }
  
  static Float getDerivative(const CurvePoint& p1, const CurvePoint& p2) {
      return (p2.y - p1.y) / (p2.x - p1.x);
  }
  
  static Float getDerivative(const CurvePoint& p0, const CurvePoint& p1, const CurvePoint& p2) {
      // centripetal parametrization -> weight by the squared root of the distance
      const Float d1 = sqrt(getDist(p0, p1));
      const Float d2 = sqrt(getDist(p1, p2));
      return lerp(getDerivative(p0, p1), getDerivative(p1, p2), d1 / (d1 + d2));
  }
  
  
  Curve::Curve() {
      points = {
          { { 0.f, 0.f }, true },
          { { 1.f, 1.f }, true },
      };
  }
  
  Curve::Curve(const Interval& rangeX, const Interval& rangeY) {
      points = {
          { { float(rangeX.lower()), float(rangeY.lower()) }, true },
          { { float(rangeX.upper()), float(rangeY.upper()) }, true },
      };
  }
  
  Curve::Curve(Array<CurvePoint>&& curve) {
      points.resize(curve.size());
      for (Size i = 0; i < curve.size(); ++i) {
          points[i] = std::make_pair(curve[i], true);
      }
      this->sort();
  }
  
  Curve::Curve(const Curve& curve) {
      *this = curve;
  }
  
  Curve& Curve::operator=(const Curve& curve) {
      points = curve.points.clone();
      return *this;
  }
  
  Size Curve::getPointCnt() const {
      return points.size();
  }
  
  const CurvePoint& Curve::getPoint(const Size idx) const {
      return points[idx].first;
  }
  
  void Curve::setPoint(const Size idx, const CurvePoint& newPoint) {
      points[idx].first = newPoint;
      this->sort();
  }
  
  void Curve::addPoint(const CurvePoint& newPoint) {
      auto iter = std::lower_bound(points.begin(),
          points.end(),
          newPoint,
          [](const std::pair<CurvePoint, bool>& p, const CurvePoint& q) { return p.first.x < q.x; });
      if (iter == points.end()) {
          points.emplaceBack(newPoint, points[points.size() - 2].second);
      } else {
          points.insert(Size(iter - points.begin()), std::make_pair(newPoint, (iter - 1)->second));
      }
  }
  
  void Curve::deletePoint(const Size idx) {
      if (points.size() > 2) {
          points.remove(idx);
      }
  }
  
  bool Curve::getSegment(const Size idx) const {
      return points[idx].second;
  }
  
  void Curve::setSegment(const Size idx, const bool cubic) {
      points[idx].second = cubic;
  }
  
  Interval Curve::rangeX() const {
      Interval range;
      for (const auto& p : points) {
          range.extend(p.first.x);
      }
      return range;
  }
  
  Interval Curve::rangeY() const {
      Interval range;
      for (const auto& p : points) {
          range.extend(p.first.y);
      }
      return range;
  }
  
  Float Curve::operator()(const Float x) const {
      auto lowerBoundIter = std::lower_bound(
          points.begin(), points.end(), x, [](const std::pair<CurvePoint, bool>& p, const Float x) {
              return p.first.x < x;
          });
      Size idx;
      if (lowerBoundIter == points.end() || lowerBoundIter == points.begin()) {
          idx = 0;
      } else {
          idx = Size(lowerBoundIter - points.begin() - 1);
      }
  
      auto leftDy = [this, idx] {
          return points[idx - 1].second
                     ? getDerivative(points[idx - 1].first, points[idx].first, points[idx + 1].first)
                     : getDerivative(points[idx - 1].first, points[idx].first);
      };
      auto rightDy = [this, idx] {
          return points[idx + 1].second
                     ? getDerivative(points[idx].first, points[idx + 1].first, points[idx + 2].first)
                     : getDerivative(points[idx + 1].first, points[idx + 2].first);
      };
  
      if (points[idx].second && points.size() > 2) {
          if (idx == 0) {
              return cubic(points[idx].first,
                  points[idx + 1].first,
                  getDerivative(points[idx].first, points[idx + 1].first),
                  rightDy(),
                  x);
          } else if (idx == points.size() - 2) {
              return cubic(points[idx].first,
                  points[idx + 1].first,
                  leftDy(),
                  getDerivative(points[idx].first, points[idx + 1].first),
                  x);
          } else {
              return cubic(points[idx].first, points[idx + 1].first, leftDy(), rightDy(), x);
          }
      } else {
          return linear(points[idx].first, points[idx + 1].first, x);
      }
  }
  
  Float Curve::linear(const CurvePoint& p1, const CurvePoint& p2, const Float x) const {
      return p1.y + (x - p1.x) / (p2.x - p1.x) * (p2.y - p1.y);
  }
  
  Float Curve::cubic(const CurvePoint& p1,
      const CurvePoint& p2,
      const Float dy1,
      const Float dy2,
      const Float x) const {
      const Float d = p2.x - p1.x;
      const Float t = (x - p1.x) / d;
      const Vector4 ts(1._f, t, pow<2>(t), pow<3>(t));
      const Vector4 ys(p1.y, p2.y, dy1 * d, dy2 * d);
      return dot(ts, CURVE_MATRIX * ys);
  }
  
  void Curve::sort() {
      std::sort(points.begin(),
          points.end(),
          [](const std::pair<CurvePoint, bool>& p1, const std::pair<CurvePoint, bool>& p2) {
              return p1.first.x < p2.first.x;
          });
  }
  
  NAMESPACE_SPH_END
  