#include "optimizer.hh" #include void optimize(BSplineCurve &curve, const VectorVector &directions, size_t n_samples, size_t fix) { size_t n = curve.n + 1, p = curve.p, m = n_samples; size_t nv = n - fix * 2; double knot_start = curve.knots.front(); double knot_length = curve.knots.back() - curve.knots.front(); Eigen::MatrixXd A = Eigen::MatrixXd::Zero(m * nv, nv); Eigen::VectorXd b = Eigen::VectorXd::Zero(m * nv); for (size_t i = 0; i < m; ++i) { // sampled data point at u_i // Compute the basis function derivatives double u = (double)i / (m - 1); u = knot_start + u * knot_length; size_t span = curve.findSpan(u); DoubleMatrix der; curve.basisFunctionDerivatives(span, u, 2, der); for (size_t j = fix; j < n - fix; ++j) { // derivative by w_j size_t jv = j - fix; // Outside of the span? if (span - p > j || j > span) continue; auto vj = directions[j]; for (size_t k = 0; k < n; ++k) // coefficient of w_k if (span - p <= k && k <= span) { if (k >= fix && k < n - fix) A(i * nv + jv, k - fix) = der[2][k+p-span] * der[2][j+p-span]; b(i * nv + jv) -= curve.cp[k] * vj * der[2][k+p-span] * der[2][j+p-span]; } } } // Solve the system Eigen::VectorXd x = A.colPivHouseholderQr().solve(b); for (size_t i = fix; i < n - fix; ++i) curve.cp[i] += directions[i] * x(i - fix); }