#ifndef ALGOIM_NEWTONCP_HPP #define ALGOIM_NEWTONCP_HPP /* Newton's method for the constrained minimum-distance optimisation problem applied to finding closest points on the zero level set of a function (typically a polynomial) For more information, refer to the paper R. I. Saye, High-order methods for computing distances to implicitly defined surfaces, Communications in Applied Mathematics and Computational Science, 9(1), 107-141 (2014), http://dx.doi.org/10.2140/camcos.2014.9.107 */ #include "uvector.hpp" #include "utility.hpp" namespace algoim { namespace detail { // Gaussian elimination with partial pivoting, solving the system Ax = b, such that // b is overwritten with the solution x, and A is overwritten with LU decomposition // (modulo permutations). Returns false if and only if a "small" pivot is detected, // assuming largest singular value is O(1). template bool newtoncp_gepp(uvector& A, uvector& b) { for (int i = 0; i < N; ++i) { int j = i; for (int k = i + 1; k < N; ++k) if (std::abs(A(k*N+i)) > std::abs(A(j*N+i))) j = k; if (j != i) { for (int k = 0; k < N; ++k) std::swap(A(i*N+k), A(j*N+k)); std::swap(b(i), b(j)); } if (std::abs(A(i*N+i)) < 1.0e4*std::numeric_limits::epsilon()) return false; double fac = 1.0 / A(i*N+i); for (int j = i + 1; j < N; ++j) A(j*N+i) *= fac; for (int j = i + 1; j < N; ++j) { for (int k = i + 1; k < N; ++k) A(j*N+k) -= A(j*N+i)*A(i*N+k); b(j) -= A(j*N+i)*b(i); } } for (int i = N - 1; i >= 0; --i) { double sum = 0.0; for (int j = i + 1; j < N; ++j) sum += A(i*N+j)*b(j); b(i) = (b(i) - sum) / A(i*N+i); } return true; } } /* Newton's method for the constrained minimum-distance optimisation problem: - x: initial guess of the closest point - ref: query point for which argmin ||x - ref|| is sought - phi: level set function whose zero level set defines the surface - r: radius of the bounding ball - tolsqr: squared tolerance that determines convergence criterion - maxsteps: maximum number of steps in the iterative method */ template int newtonCP(uvector& x, const uvector& ref, const F& phi, double r, double tolsqr, int maxsteps) { uvector x0 = x; double lambda = 0.0; for (int step = 1; step <= maxsteps; ++step) { uvector xold = x; // Evaluate phi and its derivatives double phival = phi(x); uvector phigrad = phi.grad(x); uvector phiHessian = phi.hessian(x); // Since x is on or very near the zero level set of phi, it is unlikely that |nabla(phi)| = 0. If it is, // then the closest point problem is ill-posed, so terminate with x being the closest point. double magsqrgrad = sqrnorm(phigrad); if (magsqrgrad < 1e-4*tolsqr) return step; // Initialise lambda at step 0 assuming near closest point if (step == 1) lambda = dot(ref - x, phigrad) / magsqrgrad; // Calculate gradient of functional uvector gradf; for (int i = 0; i < N; ++i) gradf(i) = x(i) - ref(i) + lambda*phigrad(i); gradf(N) = phival; // Calculate Hessian of functional uvector Hf; int k = 0; for (int i = 0; i < N; ++i) { Hf(i*(N+1)+i) = 1.0 + lambda*phiHessian(k++); for (int j = i + 1; j < N; ++j) { double Hij = lambda*phiHessian(k++); Hf(i*(N+1)+j) = Hij; Hf(j*(N+1)+i) = Hij; } Hf(i*(N+1)+N) = phigrad(i); Hf(N*(N+1)+i) = phigrad(i); } Hf(N*(N+1)+N) = 0.0; // Apply Newton's method if (detail::newtoncp_gepp(Hf, gradf)) { // Clamp update to ensure do not move too far double msqr = 0.0; for (int i = 0; i < N; ++i) msqr += util::sqr(gradf(i)); if (msqr > util::sqr(0.5*r)) gradf *= 0.5*r/sqrt(msqr); // Update for (int i = 0; i < N; ++i) x(i) -= gradf(i); lambda -= gradf(N); } else { // Newton's method failed, since Hessian was detected to be approximately singular. This generally // only occurs when x is (very) near the centre of curvature of the surface. In this case, revert to // a type of gradient descent uvector delta1 = (phival/magsqrgrad)*phigrad; lambda = dot(ref - x, phigrad) / magsqrgrad; uvector delta2 = x - ref + lambda*phigrad; // Clamp delta2, the tangential direction, necessary when interface undergoes high curvature double msqr = sqrnorm(delta2); if (msqr > util::sqr(0.1*r)) delta2 *= 0.1*r/sqrt(msqr); x -= delta1 + delta2; } if (sqrnorm(x - x0) > util::sqr(r)) { // Restore x to the last iterate inside the bounding ball x = xold; return -1; } if (sqrnorm(x - xold) < tolsqr) return step; } return -2; } // Simple Newton-style procedure for projecting a point x onto the zero level set of f template int newtonIso(uvector& x, const F& f, double tolsqr, int maxsteps) { for (int step = 1; step <= maxsteps; ++step) { // Evaluate f and its gradient double val = f(x); uvector g = f.grad(x); // Compute delta and update double msqr = sqrnorm(g); if (msqr > 0.0) g *= -val/msqr; x += g; // Terminate if converged to within tolerance if (sqrnorm(g) < tolsqr) return step; } return -1; } } // namespace algoim #endif