You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
189 lines
6.8 KiB
189 lines
6.8 KiB
#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<int N>
|
|
bool newtoncp_gepp(uvector<double,N*N>& A, uvector<double,N>& 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<double>::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 N, typename F>
|
|
int newtonCP(uvector<double,N>& x, const uvector<double,N>& ref, const F& phi, double r, double tolsqr, int maxsteps)
|
|
{
|
|
uvector<double,N> x0 = x;
|
|
double lambda = 0.0;
|
|
for (int step = 1; step <= maxsteps; ++step)
|
|
{
|
|
uvector<double,N> xold = x;
|
|
|
|
// Evaluate phi and its derivatives
|
|
double phival = phi(x);
|
|
uvector<double,N> phigrad = phi.grad(x);
|
|
uvector<double,N*(N+1)/2> 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<double,N+1> 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<double,(N+1)*(N+1)> 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<double,N> delta1 = (phival/magsqrgrad)*phigrad;
|
|
lambda = dot(ref - x, phigrad) / magsqrgrad;
|
|
uvector<double,N> 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 N, typename F>
|
|
int newtonIso(uvector<double,N>& 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<double,N> 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
|
|
|