bezier eval and fit cubic bezier splin

include/igl/bezier.cpp

@@ -0,0 +1,74 @@
+// This file is part of libigl, a simple c++ geometry processing library.
+// 
+// Copyright (C) 2020 Alec Jacobson <[email protected]>
+// 
+// This Source Code Form is subject to the terms of the Mozilla Public License 
+// v. 2.0. If a copy of the MPL was not distributed with this file, You can 
+// obtain one at http://mozilla.org/MPL/2.0/.
+#include "bezier.h"
+#include <cassert>
+
+// Adapted from main.c accompanying
+// An Algorithm for Automatically Fitting Digitized Curves
+// by Philip J. Schneider
+// from "Graphics Gems", Academic Press, 1990
+template <typename DerivedV, typename DerivedP>
+IGL_INLINE void igl::bezier(
+  const Eigen::MatrixBase<DerivedV> & V,
+  const typename DerivedV::Scalar t,
+  Eigen::PlainObjectBase<DerivedP> & P)
+{
+  // working local copy
+  DerivedV Vtemp = V;
+  int degree = Vtemp.rows()-1;
+  /* Triangle computation	*/
+  for (int i = 1; i <= degree; i++)
+  {	
+    for (int j = 0; j <= degree-i; j++) 
+    {
+      Vtemp.row(j) = ((1.0 - t) * Vtemp.row(j) + t * Vtemp.row(j+1)).eval();
+    }
+  }
+  P = Vtemp.row(0);
+}
+
+template <typename DerivedV, typename DerivedT, typename DerivedP>
+IGL_INLINE void igl::bezier(
+  const Eigen::MatrixBase<DerivedV> & V,
+  const Eigen::MatrixBase<DerivedT> & T,
+  Eigen::PlainObjectBase<DerivedP> & P)
+{
+  P.resize(T.size(),V.cols());
+  for(int i = 0;i<T.size();i++)
+  {
+    Eigen::Matrix<typename DerivedV::Scalar,1,DerivedV::ColsAtCompileTime> Pi;
+    bezier(V,T(i),Pi);
+    P.row(i) = Pi;
+  }
+}
+
+template <typename VMat, typename DerivedT, typename DerivedP>
+IGL_INLINE void igl::bezier(
+  const std::vector<VMat> & spline,
+  const Eigen::MatrixBase<DerivedT> & T,
+  Eigen::PlainObjectBase<DerivedP> & P)
+{
+  if(spline.size() == 0) return;
+  const int m = T.rows();
+  const int dim = spline[0].cols();
+  P.resize(m*spline.size(),dim);
+  for(int c = 0;c<spline.size();c++)
+  {
+    assert(dim == spline[c].cols() && "All curves must have same dimension");
+    DerivedP Pc;
+    bezier(spline[c],T,Pc);
+    P.block(m*c,0,m,dim) = Pc;
+  }
+}
+
+#ifdef IGL_STATIC_LIBRARY
+// Explicit template instantiation
+template void igl::bezier<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > > const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
+template void igl::bezier<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
+template void igl::bezier<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, 1, -1, 1, 1, -1> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1>::Scalar, Eigen::PlainObjectBase<Eigen::Matrix<double, 1, -1, 1, 1, -1> >&);
+#endif

include/igl/bezier.h

@@ -0,0 +1,51 @@
+#ifndef BEZIER_H
+#define BEZIER_H
+#include "igl_inline.h"
+#include <Eigen/Core>
+#include <vector>
+namespace igl
+{
+  // Evaluate a polynomial Bezier Curve.
+  //
+  // Inputs:
+  //   V  #V by dim list of Bezier control points
+  //   t  evaluation parameter within [0,1]
+  // Outputs:
+  //   P  1 by dim output point 
+  template <typename DerivedV, typename DerivedP>
+  IGL_INLINE void bezier(
+    const Eigen::MatrixBase<DerivedV> & V,
+    const typename DerivedV::Scalar t,
+    Eigen::PlainObjectBase<DerivedP> & P);
+  // Evaluate a polynomial Bezier Curve.
+  //
+  // Inputs:
+  //   V  #V by dim list of Bezier control points
+  //   T  #T evaluation parameters within [0,1]
+  // Outputs:
+  //   P  #T  by dim output points
+  template <typename DerivedV, typename DerivedT, typename DerivedP>
+  IGL_INLINE void bezier(
+    const Eigen::MatrixBase<DerivedV> & V,
+    const Eigen::MatrixBase<DerivedT> & T,
+    Eigen::PlainObjectBase<DerivedP> & P);
+  // Evaluate a polynomial Bezier spline with a fixed parameter set for each
+  // sub-curve
+  //
+  // Inputs:
+  //   spline #curves list of lists of Bezier control points
+  //   T  #T evaluation parameters within [0,1] to use for each spline
+  // Outputs:
+  //   P  #curves*#T  by dim output points
+  template <typename VMat, typename DerivedT, typename DerivedP>
+  IGL_INLINE void bezier(
+    const std::vector<VMat> & spline,
+    const Eigen::MatrixBase<DerivedT> & T,
+    Eigen::PlainObjectBase<DerivedP> & P);
+}
+
+#ifndef IGL_STATIC_LIBRARY
+#  include "bezier.cpp"
+#endif
+
+#endif 

include/igl/fit_cubic_bezier.cpp

@@ -0,0 +1,294 @@
+// This file is part of libigl, a simple c++ geometry processing library.
+// 
+// Copyright (C) 2020 Alec Jacobson <[email protected]>
+// 
+// This Source Code Form is subject to the terms of the Mozilla Public License 
+// v. 2.0. If a copy of the MPL was not distributed with this file, You can 
+// obtain one at http://mozilla.org/MPL/2.0/.
+#include "fit_cubic_bezier.h"
+#include "bezier.h"
+#include "EPS.h"
+
+// Adapted from main.c accompanying
+// An Algorithm for Automatically Fitting Digitized Curves
+// by Philip J. Schneider
+// from "Graphics Gems", Academic Press, 1990
+IGL_INLINE void igl::fit_cubic_bezier(
+  const Eigen::MatrixXd & d,
+  const double error,
+  std::vector<Eigen::MatrixXd> & cubics)
+{
+  const int nPts = d.rows();
+  // Don't attempt to fit curve to single point
+  if(nPts==1) { return; }
+  // Avoid using zero tangent
+  const static auto tangent = [&](const int i,const int dir)->Eigen::RowVectorXd
+  {
+    int j = i;
+    Eigen::RowVectorXd t;
+    while(true)
+    {
+      // look at next point
+      j += dir;
+      if(j < 0 || j>=nPts)
+      {
+        // All points are coincident?
+        // give up and use zero tangent...
+        return Eigen::RowVectorXd::Zero(1,d.cols());
+      }
+      t = d.row(j)-d.row(i);
+      if(t.squaredNorm() > igl::DOUBLE_EPS)
+      {
+        break;
+      }
+    }
+    return t.normalized();
+  };
+  const Eigen::RowVectorXd tHat1 = tangent(0,+1);
+  const Eigen::RowVectorXd tHat2 = tangent(nPts-1,-1);
+  cubics.clear();
+  fit_cubic_bezier_substring(d,0,nPts-1,tHat1,tHat2,error,cubics);
+};
+
+IGL_INLINE void igl::fit_cubic_bezier_substring(
+  const Eigen::MatrixXd & d,
+  const int first,
+  const int last,
+  const Eigen::RowVectorXd & tHat1,
+  const Eigen::RowVectorXd & tHat2,
+  const double error,
+  std::vector<Eigen::MatrixXd> & cubics)
+{
+  // Helper functions
+  // Evaluate a Bezier curve at a particular parameter value
+  const static auto bezier_eval = [](const Eigen::MatrixXd & V, const double t)
+    { Eigen::RowVectorXd P; bezier(V,t,P); return P; };
+  //
+  // Use Newton-Raphson iteration to find better root.
+  const static auto NewtonRaphsonRootFind = [](
+    const Eigen::MatrixXd & Q,
+    const Eigen::RowVectorXd & P,
+    const double u)->double
+  {
+    /* Compute Q(u)	*/
+    Eigen::RowVectorXd Q_u = bezier_eval(Q, u);
+    Eigen::MatrixXd Q1(3,Q.cols());
+    Eigen::MatrixXd Q2(2,Q.cols());
+    /* Generate control vertices for Q'	*/
+    for (int i = 0; i <= 2; i++) 
+    {
+      Q1.row(i) = (Q.row(i+1) - Q.row(i)) * 3.0;
+    }
+    /* Generate control vertices for Q'' */
+    for (int i = 0; i <= 1; i++) 
+    {
+      Q2.row(i) = (Q1.row(i+1) - Q1.row(i)) * 2.0;
+    }
+    /* Compute Q'(u) and Q''(u)	*/
+    const Eigen::RowVectorXd Q1_u = bezier_eval(Q1, u);
+    const Eigen::RowVectorXd Q2_u = bezier_eval(Q2, u);
+    /* Compute f(u)/f'(u) */
+    const double numerator = ((Q_u-P).array() * Q1_u.array()).array().sum();
+    const double denominator = 
+      Q1_u.squaredNorm() + ((Q_u-P).array() * Q2_u.array()).array().sum();
+    /* u = u - f(u)/f'(u) */
+    return u - (numerator/denominator);
+  };
+  const static auto ComputeMaxError = [](
+    const Eigen::MatrixXd & d,
+    const int first,
+    const int last,
+    const Eigen::MatrixXd & bezCurve,
+    const Eigen::VectorXd & u,
+    int & splitPoint)->double
+  {
+    Eigen::VectorXd E(last - (first+1));
+    splitPoint = (last-first + 1)/2;
+    double maxDist = 0.0;
+    for (int i = first + 1; i < last; i++) 
+    {
+      Eigen::RowVectorXd P = bezier_eval(bezCurve, u(i-first));
+      const double dist = (P-d.row(i)).squaredNorm();
+      E(i-(first+1)) = dist;
+      if (dist >= maxDist)
+      {
+        maxDist = dist;
+        // Worst offender
+        splitPoint = i;
+      }
+    }
+    //const double half_total = E.array().sum()/2;
+    //double run = 0;
+    //for (int i = first + 1; i < last; i++) 
+    //{
+    //  run += E(i-(first+1));
+    //  if(run>half_total)
+    //  {
+    //    // When accumulated ½ the error --> more symmetric, but requires more
+    //    // curves
+    //    splitPoint = i;
+    //    break;
+    //  }
+    //}
+    return maxDist;
+  };
+  const static auto Straight = [](
+    const Eigen::MatrixXd & d,
+    const int first,
+    const int last,
+    const Eigen::RowVectorXd & tHat1,
+    const Eigen::RowVectorXd & tHat2,
+    Eigen::MatrixXd & bezCurve)
+  {
+    bezCurve.resize(4,d.cols());
+    const double dist = (d.row(last)-d.row(first)).norm()/3.0;
+    bezCurve.row(0) = d.row(first);
+    bezCurve.row(1) = d.row(first) + tHat1*dist;
+    bezCurve.row(2) = d.row(last) + tHat2*dist;
+    bezCurve.row(3) = d.row(last);
+  };
+  const static auto GenerateBezier = [](
+    const Eigen::MatrixXd & d,
+    const int first,
+    const int last,
+    const Eigen::VectorXd & uPrime,
+    const Eigen::RowVectorXd & tHat1,
+    const Eigen::RowVectorXd & tHat2,
+    Eigen::MatrixXd & bezCurve)
+  {
+    bezCurve.resize(4,d.cols());
+    const int nPts = last - first + 1;
+    const static auto B0 = [](const double u)->double
+      { double tmp = 1.0 - u; return (tmp * tmp * tmp);};
+    const static auto B1 = [](const double u)->double
+      { double tmp = 1.0 - u; return (3 * u * (tmp * tmp));};
+    const static auto B2 = [](const double u)->double
+      { double tmp = 1.0 - u; return (3 * u * u * tmp); };
+    const static auto B3 = [](const double u)->double
+      { return (u * u * u); };
+    /* Compute the A's	*/
+    std::vector<std::vector<Eigen::RowVectorXd> > A(nPts);
+    for (int i = 0; i < nPts; i++)
+    {
+      Eigen::RowVectorXd v1 = tHat1*B1(uPrime(i));
+      Eigen::RowVectorXd v2 = tHat2*B2(uPrime(i));
+      A[i] = {v1,v2};
+    }
+    /* Create the C and X matrices	*/
+    Eigen::MatrixXd C(2,2);
+    Eigen::VectorXd X(2);
+    C(0,0) = 0.0;
+    C(0,1) = 0.0;
+    C(1,0) = 0.0;
+    C(1,1) = 0.0;
+    X(0)    = 0.0;
+    X(1)    = 0.0;
+    for( int i = 0; i < nPts; i++) 
+    {
+      C(0,0) += A[i][0].dot(A[i][0]);
+      C(0,1) += A[i][0].dot(A[i][1]);
+      C(1,0) = C(0,1);
+      C(1,1) += A[i][1].dot(A[i][1]);
+      const Eigen::RowVectorXd tmp = 
+        d.row(first+i)-(
+              d.row(first)*B0(uPrime(i))+
+              d.row(first)*B1(uPrime(i))+
+              d.row(last)*B2(uPrime(i))+
+              d.row(last)*B3(uPrime(i)));
+  	X(0) += A[i][0].dot(tmp);
+  	X(1) += A[i][1].dot(tmp);
+      }
+    /* Compute the determinants of C and X	*/
+    double det_C0_C1 = C(0,0) * C(1,1) - C(1,0) * C(0,1);
+    const double det_C0_X  = C(0,0) * X(1)    - C(0,1) * X(0);
+    const double det_X_C1  = X(0)    * C(1,1) - X(1)    * C(0,1);
+    /* Finally, derive alpha values	*/
+    if (det_C0_C1 == 0.0) 
+    {
+      det_C0_C1 = (C(0,0) * C(1,1)) * 10e-12;
+    }
+    const double alpha_l = det_X_C1 / det_C0_C1;
+    const double alpha_r = det_C0_X / det_C0_C1;
+    /*  If alpha negative, use the Wu/Barsky heuristic (see text) */
+        /* (if alpha is 0, you get coincident control points that lead to
+         * divide by zero in any subsequent NewtonRaphsonRootFind() call. */
+    if (alpha_l < 1.0e-6 || alpha_r < 1.0e-6) 
+    {
+      return Straight(d,first,last,tHat1,tHat2,bezCurve);
+    }
+    bezCurve.row(0) = d.row(first);
+    bezCurve.row(1) = d.row(first) + tHat1*alpha_l;
+    bezCurve.row(2) = d.row(last) + tHat2*alpha_r;
+    bezCurve.row(3) = d.row(last);
+  };
+
+  const int maxIterations = 4;
+  // This is a bad idea if error<1 ...
+  //const double iterationError = error * error;
+  const double iterationError = 100 * error;
+  const int nPts = last - first + 1;
+  /*  Use heuristic if region only has two points in it */
+  if(nPts == 2)
+  {
+    Eigen::MatrixXd bezCurve;
+    Straight(d,first,last,tHat1,tHat2,bezCurve);
+    cubics.push_back(bezCurve);
+    return;
+  }
+  // ChordLengthParameterize
+  Eigen::VectorXd u(last-first+1);
+  u(0) = 0;
+  for (int i = first+1; i <= last; i++)
+  {
+    u(i-first) = u(i-first-1) + (d.row(i)-d.row(i-1)).norm();
+  }
+  for (int i = first + 1; i <= last; i++) 
+  {
+    u(i-first) = u(i-first) / u(last-first);
+  }
+  Eigen::MatrixXd bezCurve;
+  GenerateBezier(d, first, last, u, tHat1, tHat2, bezCurve);
+
+
+  int splitPoint;
+  double maxError = ComputeMaxError(d, first, last, bezCurve, u, splitPoint);
+  if (maxError < error)
+  {
+    cubics.push_back(bezCurve);
+    return;
+  }
+  /*  If error not too large, try some reparameterization  */
+  /*  and iteration */
+  if (maxError < iterationError)
+  {
+    for (int i = 0; i < maxIterations; i++) 
+    {
+      Eigen::VectorXd uPrime;
+      // Reparameterize
+      uPrime.resize(last-first+1);
+      for (int i = first; i <= last; i++) 
+      {
+        uPrime(i-first) = NewtonRaphsonRootFind(bezCurve, d.row(i), u(i- first));
+      }
+      GenerateBezier(d, first, last, uPrime, tHat1, tHat2, bezCurve);
+      maxError = ComputeMaxError(d, first, last, bezCurve, uPrime, splitPoint);
+      if (maxError < error) {
+        cubics.push_back(bezCurve);
+        return;
+      }
+      u = uPrime;
+    }
+  }
+
+  /* Fitting failed -- split at max error point and fit recursively */
+  const Eigen::RowVectorXd tHatCenter = 
+    (d.row(splitPoint-1)-d.row(splitPoint+1)).normalized();
+  //foobar
+  fit_cubic_bezier_substring(d,first,splitPoint,tHat1,tHatCenter,error,cubics);
+  fit_cubic_bezier_substring(d,splitPoint,last,(-tHatCenter).eval(),tHat2,error,cubics);
+}
+
+
+#ifdef IGL_STATIC_LIBRARY
+// Explicit template instantiation
+#endif

include/igl/fit_cubic_bezier.h

@@ -0,0 +1,54 @@
+// This file is part of libigl, a simple c++ geometry processing library.
+// 
+// Copyright (C) 2020 Alec Jacobson <[email protected]>
+// 
+// This Source Code Form is subject to the terms of the Mozilla Public License 
+// v. 2.0. If a copy of the MPL was not distributed with this file, You can 
+// obtain one at http://mozilla.org/MPL/2.0/.
+#ifndef FIT_CUBIC_BEZIER_H
+#define FIT_CUBIC_BEZIER_H
+#include "igl_inline.h"
+#include <Eigen/Core>
+#include <vector>
+namespace igl
+{
+  // Fit a cubic bezier spline (G1 continuous) to an ordered list of input
+  // points in any dimension, according to "An algorithm for automatically
+  // fitting digitized curves" [Schneider 1990].
+  //
+  // Inputs:
+  //   d  #d by dim list of points along a curve to be fit with a cubic bezier
+  //     spline (should probably be roughly uniformly spaced)
+  //   error  maximum squared distance allowed
+  // Output:
+  //   cubics #cubics list of 4 by dim lists of cubic control points
+  IGL_INLINE void fit_cubic_bezier(
+    const Eigen::MatrixXd & d,
+    const double error,
+    std::vector<Eigen::MatrixXd> & cubics);
+  // Recursive helper function. 
+  //
+  // Inputs:
+  //    first  index of first point in d of substring
+  //    last  index of last point in d of substring
+  //    tHat1  tangent to use at beginning of spline
+  //    tHat2  tangent to use at end of spline
+  //    error  see above
+  //    cubics  running list of cubics so far
+  // Outputs
+  //    cubics  running list of cubics so far (new cubics appended)
+  IGL_INLINE void fit_cubic_bezier_substring(
+    const Eigen::MatrixXd & d,
+    const int first,
+    const int last,
+    const Eigen::RowVectorXd & tHat1,
+    const Eigen::RowVectorXd & tHat2,
+    const double error,
+    std::vector<Eigen::MatrixXd> & cubics);
+}
+
+#ifndef IGL_STATIC_LIBRARY
+#include "fit_cubic_bezier.cpp"
+#endif
+
+#endif 

tests/include/igl/bezier.cpp

@@ -0,0 +1,33 @@
+// This file is part of libigl, a simple c++ geometry processing library.
+// 
+// Copyright (C) 2020 Alec Jacobson <[email protected]>
+// 
+// This Source Code Form is subject to the terms of the Mozilla Public License 
+// v. 2.0. If a copy of the MPL was not distributed with this file, You can 
+// obtain one at http://mozilla.org/MPL/2.0/.
+#include <test_common.h>
+#include <igl/bezier.h>
+
+TEST_CASE("bezier: ease", "[igl]")
+{
+  // Ease curve
+  const Eigen::MatrixXd C = 
+    (Eigen::MatrixXd(4,2)<<0,0,0.5,0,0.5,1,1,1).finished();
+  const Eigen::VectorXd T = 
+    (Eigen::VectorXd(11,1)<<0,0.1,0.2,0.3,0.4,0.5,0.6,0.7,0.8,0.9,1).finished();
+  Eigen::MatrixXd P;
+  igl::bezier(C,T,P);
+  const Eigen::MatrixXd Pexact = (Eigen::MatrixXd(11,2)<<
+    0.00000000000000000,0.00000000000000000,
+    0.13600000000000001,0.02800000000000001,
+    0.24800000000000008,0.10400000000000004,
+    0.34199999999999997,0.21599999999999997,
+    0.42400000000000004,0.35200000000000004,
+    0.50000000000000000,0.50000000000000000,
+    0.57600000000000007,0.64800000000000002,
+    0.65799999999999992,0.78399999999999992,
+    0.75200000000000011,0.89600000000000013,
+    0.86400000000000010,0.97200000000000009,
+    1.00000000000000000,1.00000000000000000).finished();
+  test_common::assert_near(P,Pexact,1e-15);
+}

tests/include/igl/fit_cubic_bezier.cpp

@@ -0,0 +1,38 @@
+// This file is part of libigl, a simple c++ geometry processing library.
+// 
+// Copyright (C) 2020 Alec Jacobson <[email protected]>
+// 
+// This Source Code Form is subject to the terms of the Mozilla Public License 
+// v. 2.0. If a copy of the MPL was not distributed with this file, You can 
+// obtain one at http://mozilla.org/MPL/2.0/.
+#include <test_common.h>
+#include <igl/fit_cubic_bezier.h>
+#include <igl/bezier.h>
+#include <igl/PI.h>
+
+TEST_CASE("fit_cubic_bezier: hemicircle", "[igl]")
+{
+  // Create a hemicircle
+  Eigen::MatrixXd d(101,2);
+  for(int i=0;i<d.rows();i++)
+  {
+    const double th = double(i)/double(d.rows()-1)*igl::PI;
+    d(i,0) = cos(th);
+    d(i,1) = sin(th);
+  }
+  const double error = 0.000001;
+  std::vector<Eigen::MatrixXd> cubics;
+  igl::fit_cubic_bezier(d,error,cubics);
+  REQUIRE(cubics.size()>1);
+  REQUIRE(cubics.size()<10);
+  // Generate a dense sampling
+  const Eigen::VectorXd T = Eigen::VectorXd::LinSpaced(1000,0,1);
+  Eigen::MatrixXd X;
+  igl::bezier(cubics,T,X);
+  for(int j=0;j<d.rows();j++)
+  {
+    const double sd = 
+      ((X.rowwise()-d.row(j)).rowwise().squaredNorm()).minCoeff();
+    REQUIRE(sd < error);
+  }
+}