File indexing completed on 2024-12-15 04:01:12

 /*
  * SPDX-FileCopyrightText: 2019-2023 Mattia Basaglia <dev@dragon.best>
  *
  * SPDX-License-Identifier: GPL-3.0-or-later
  */
 
 #pragma once
 
 #include <array>
 
 #include "math/vector.hpp"
 #include "math/polynomial.hpp"
 
 #include <QPointF>
 
 namespace glaxnimate::math::bezier {
 
 template<class Vec>
 class CubicBezierSolver
 {
 public:
     using vector_type = Vec;
     using scalar = scalar_type<Vec>;
     using argument_type = scalar;
     using bounding_box_type = std::pair<Vec, Vec>;
 
 
     constexpr CubicBezierSolver(const std::array<Vec, 4>& points) noexcept
     : points_(points)
     {
         rebuild_coeff();
     }
 
     constexpr CubicBezierSolver(Vec p0, Vec p1, Vec p2, Vec p3) noexcept
     : CubicBezierSolver{{p0, p1, p2, p3}} {}
 
     /**
      * \brief Finds the point along the bezier curve
      * \param t between 0 and 1
      */
     constexpr Vec solve(argument_type t) const noexcept
     {
         return ((a_ * t + b_ ) * t +  c_ ) * t + d_;
     }
 
     constexpr scalar solve_component(argument_type t, int component) const noexcept
     {
         scalar a = detail::get(a_, component);
         scalar b = detail::get(b_, component);
         scalar c = detail::get(c_, component);
         scalar d = detail::get(d_, component);
         return ((a * t + b ) * t +  c ) * t + d;
     }
 
     constexpr scalar derivative(argument_type factor, int component) const noexcept
     {
         scalar a = detail::get(a_, component);
         scalar b = detail::get(b_, component);
         scalar c = detail::get(c_, component);
         return (3 * a * factor + 2 * b) * factor + c;
     }
 
     /**
      * \brief Finds the tangent of the point on the bezier
      * \param factor between 0 and 1
      * \return Angle in radians
      */
     scalar tangent_angle(argument_type factor) const
     {
         return std::atan2(derivative(factor, 1), derivative(factor, 0));
     }
 
     /**
      * \brief Finds the normal of the point on the bezier
      * \param factor between 0 and 1
      * \return Angle in radians
      */
     scalar normal_angle(argument_type factor) const
     {
         return std::atan2(derivative(factor, 0), derivative(factor, 1));
     }
 
     constexpr const std::array<Vec, 4>& points() const noexcept
     {
         return points_;
     }
 
 //     constexpr std::array<Vec, 4>& points() noexcept
 //     {
 //         return points_;
 //     }
 
     template<int i>
     constexpr void set(const Vec& v) noexcept
     {
         points_[i] = v;
         rebuild_coeff();
     }
 
     /**
      * \brief Splits a bezier
      * \param factor value between 0 and 1 determining the split position
      * \return Two vectors for the two resulting cubic beziers
      */
     std::pair<std::array<Vec, 4>, std::array<Vec, 4>> split(argument_type factor) const
     {
         // linear
         if ( points_[0] == points_[1] && points_[2] == points_[3] )
         {
             Vec mid = lerp(points_[0], points_[3], factor);
             return {
                 {points_[0], points_[0], mid, mid},
                 {mid, mid, points_[3], points_[3]},
             };
         }
 
         Vec p01 = lerp(points_[0], points_[1], factor);
         Vec p12 = lerp(points_[1], points_[2], factor);
         Vec p23 = lerp(points_[2], points_[3], factor);
 
         Vec p012 = lerp(p01, p12, factor);
         Vec p123 = lerp(p12, p23, factor);
 
         Vec p0123 = lerp(p012, p123, factor);
 
         return {
             {points_[0], p01, p012, p0123},
             {p0123, p123, p23, points_[3]}
         };
     }
 
     bounding_box_type bounds() const
     {
         std::vector<scalar> solutions;
         for ( int i = 0; i < detail::VecSize<Vec>::value; i++ )
         {
             bounds_solve(3 * detail::get(a_, i), 2 * detail::get(b_, i), detail::get(c_, i), solutions);
         }
 
         std::vector<Vec> boundary_points;
         //Add Begin and end point not the control points!
         boundary_points.push_back(points_[0]);
         boundary_points.push_back(points_[3]);
 
         for ( scalar e : solutions )
             boundary_points.push_back(solve(e));
 
         Vec min;
         Vec max;
         for ( int i = 0; i < detail::VecSize<Vec>::value; i++ )
         {
             scalar cmin = std::numeric_limits<scalar>::max();
             scalar cmax = std::numeric_limits<scalar>::lowest();
             for ( const Vec& p : boundary_points )
             {
                if ( detail::get(p, i) < cmin )
                    cmin = detail::get(p, i);
                if ( detail::get(p, i) > cmax )
                    cmax = detail::get(p, i);
             }
             detail::get(max, i) = cmax;
             detail::get(min, i) = cmin;
         }
 
         return {min, max};
     }
 
 
     /**
      * \brief Returns the \p t for the min/max points for the given component
      *
      * \note The returned value is sorted so that \p first <= \p second
      */
     std::pair<argument_type, argument_type> extrema(int component) const
     {
         std::vector<scalar> solutions;
         bounds_solve(3 * detail::get(a_, component), 2 * detail::get(b_, component), detail::get(c_, component), solutions);
 
         // No solution: end points have the extrema
         if ( solutions.size() == 0 )
             return {0, 1};
 
         // One solution: one of the end points has an extreme
         if ( solutions.size() == 1 )
         {
             auto val = solve_component(solutions[0], component);
             // The end point is a minimum
             if ( detail::get(points_[0], component) < val )
             {
                 // Last point is min
                 if ( detail::get(points_[3], component) < detail::get(points_[0], component) )
                     return {solutions[0], 1};
 
                 // First point is min
                 return {0, solutions[0]};
             }
             // Last point is min
             else if ( detail::get(points_[3], component) < val )
             {
                 return {solutions[0], 1};
             }
             // end point is a maximum
             else
             {
                 // First point is max
                 if ( detail::get(points_[0], component) > val )
                     return {0, solutions[0]};
 
                 // Last point is max
                 return {solutions[0], 1};
             }
         }
 
         if ( solutions[0] > solutions[1] )
             return {solutions[1], solutions[0]};
 
         return {solutions[0], solutions[1]};
     }
 
     /**
      * \brief Return inflection points for a 2D bezier
      */
     std::vector<argument_type> inflection_points() const
     {
         auto denom = detail::get(a_, 1) * detail::get(b_, 0) - detail::get(a_, 0) * detail::get(b_, 1);
         if ( qFuzzyIsNull(denom) )
             return {};
 
         auto t_cusp = -0.5 * (detail::get(a_, 1) * detail::get(c_, 0) - detail::get(a_, 0) * detail::get(c_, 1)) / denom;
 
         auto square = t_cusp * t_cusp - 1./3. * (detail::get(b_, 1) * detail::get(c_, 0) - detail::get(b_, 0) * detail::get(c_, 1)) / denom;
 
         if ( square < 0 )
             return {};
 
         auto root = std::sqrt(square);
         if ( qFuzzyIsNull(root) )
         {
             if ( is_valid_inflection(t_cusp) )
                 return {t_cusp};
             return {};
         }
 
         std::vector<argument_type> roots;
         roots.reserve(2);
 
         if ( is_valid_inflection(t_cusp - root) )
             roots.push_back(t_cusp - root);
 
 
         if ( is_valid_inflection(t_cusp + root) )
             roots.push_back(t_cusp + root);
 
         return roots;
     }
 
 
     std::vector<std::pair<argument_type, argument_type>> intersections(
         const CubicBezierSolver& other, int max_count = 1, scalar tolerance = 2, int max_recursion = 10
     ) const
     {
         std::vector<std::pair<argument_type, argument_type>> intersections;
         intersects_impl(IntersectData(*this), IntersectData(other), max_count, tolerance, intersections, 0, max_recursion);
         return intersections;
     }
 
     /**
      * \brief Returns the t corresponding to the given value for a component.
      * \returns `t` or -1 in case of no root has been found
      */
     scalar t_at_value(scalar value, int comp = 0) const
     {
         auto roots = math::cubic_roots(
             detail::get(a_, comp),
             detail::get(b_, comp),
             detail::get(c_, comp),
             detail::get(d_, comp) - value
         );
 
         for ( auto root : roots )
         {
             if ( valid_solution(root) )
                 return root;
         }
 
         return -1;
     }
 
 private:
     static constexpr bool is_valid_inflection(scalar root) noexcept
     {
         return root > 0 && root < 1;
     }
 
     // You get these terms by expanding the Bezier definition and re-arranging them as a polynomial in t
     // B(t) = a t^3 + b t^2 + c t + d
     static constexpr scalar a(const scalar& k0, const scalar& k1, const scalar& k2, const scalar& k3) noexcept
     {
         return -k0 + k1*3 + k2 * -3 + k3;
     }
     static constexpr scalar b(const scalar& k0, const scalar& k1, const scalar& k2) noexcept
     {
         return k0 * 3 + k1 * -6 + k2 * 3;
     }
     static constexpr scalar c(const scalar& k0, const scalar& k1) noexcept
     {
         return k0 * -3 + k1 * 3;
     }
     static constexpr scalar d(const scalar& k0) noexcept
     {
         return k0;
     }
 
     static void bounds_solve(scalar a, scalar b, scalar c, std::vector<argument_type>& solutions)
     {
         scalar d = b * b - 4. * a * c;
 
         // no real solution
         if ( d < 0 )
             return;
 
         if ( qFuzzyIsNull(a) )
         {
             // linear case
             add_bounds_solution(-c / b, solutions);
         }
         else
         {
             add_bounds_solution((-b + std::sqrt(d)) / (2 * a), solutions);
 
             if ( d != 0 )
                 add_bounds_solution((-b - std::sqrt(d)) / (2 * a), solutions);
         }
     }
 
     static bool valid_solution(scalar& root)
     {
         if ( root >= 0 && root <= 1 )
             return true;
 
         if ( qFuzzyIsNull(root) )
         {
             root = 0;
             return true;
         }
 
         if ( qFuzzyCompare(root, 1) )
         {
             root = 1;
             return true;
         }
 
         return false;
     }
 
     static void add_bounds_solution(scalar solution, std::vector<argument_type>& solutions)
     {
         if ( valid_solution(solution) )
             solutions.push_back(solution);
     }
 
     struct IntersectData
     {
         IntersectData(
             const CubicBezierSolver& solver,
             const bounding_box_type& box,
             argument_type t1,
             argument_type t2
         ) : bez(solver),
             width(box.second.x() - box.first.x()),
             height(box.second.y() - box.first.y()),
             center((box.first + box.second) / 2),
             t1(t1),
             t2(t2),
             t((t1 + t2) / 2)
         {}
 
         IntersectData(const CubicBezierSolver& solver)
             : IntersectData(solver, solver.bounds(), 0, 1)
         {}
 
         std::pair<IntersectData, IntersectData> split() const
         {
             auto split = bez.split(0.5);
             return {
                 IntersectData(split.first),
                 IntersectData(split.second)
             };
         }
 
         bool intersects(const IntersectData& other) const
         {
             return std::abs(center.x() - other.center.x()) * 2 < width + other.width &&
                    std::abs(center.y() - other.center.y()) * 2 < height + other.height;
         }
 
         CubicBezierSolver bez;
         scalar width;
         scalar height;
         Vec center;
         argument_type t1, t2, t;
     };
 
     static void intersects_impl(
         const IntersectData& d1,
         const IntersectData& d2,
         std::size_t max_count,
         scalar tolerance,
         std::vector<std::pair<argument_type, argument_type>>& intersections,
         int depth,
         int max_recursion
     )
     {
         if ( !d1.intersects(d2) )
             return;
 
         if ( depth >= max_recursion || (d1.width <= tolerance && d1.height <= tolerance && d2.width <= tolerance && d2.height <= tolerance) )
         {
             intersections.emplace_back(d1.t, d2.t);
             return;
         }
 
         auto d1s = d1.split();
         auto d2s = d2.split();
         std::array<std::pair<const IntersectData&, const IntersectData&>, 4> splits = {{
             {d1s.first,  d2s.first },
             {d1s.first,  d2s.second},
             {d1s.second, d2s.first },
             {d1s.second, d2s.second}
         }};
 
         for ( const auto& p : splits )
         {
             intersects_impl(p.first, p.second, max_count, tolerance, intersections, depth + 1, max_recursion);
             if ( intersections.size() >= max_count )
                 return;
         }
     }
 
     void rebuild_coeff()
     {
         for ( int component = 0; component < detail::VecSize<Vec>::value; component++ )
         {
             detail::get(a_, component) = a(
                 detail::get(points_[0], component),
                 detail::get(points_[1], component),
                 detail::get(points_[2], component),
                 detail::get(points_[3], component)
             );
             detail::get(b_, component) = b(
                 detail::get(points_[0], component),
                 detail::get(points_[1], component),
                 detail::get(points_[2], component)
             );
             detail::get(c_, component) = c(
                 detail::get(points_[0], component),
                 detail::get(points_[1], component)
             );
             detail::get(d_, component) = d(
                 detail::get(points_[0], component)
             );
         }
     }
 
     std::array<Vec, 4> points_;
     // Polynomial coefficients (a t^3 + b t^2 + c t + d = 0)
     Vec a_, b_, c_, d_;
 };
 
 } // namespace glaxnimate::math::bezier