polyhedrondistance.hh

Go to the documentation of this file.

// SPDX-FileCopyrightText: Copyright © DUNE-FUFEM Project contributors, see file AUTHORS.md
// SPDX-License-Identifier: LicenseRef-GPL-2.0-only-with-DUNE-exception OR LGPL-3.0-or-later
 
#ifndef DUNE_FUFEM_GEOMETRY_POLYHEDRONDISTANCE_HH
#define DUNE_FUFEM_GEOMETRY_POLYHEDRONDISTANCE_HH
 
// Based on the closest point projection from dune-contact
 
#include <dune/common/dynvector.hh>
#include <dune/common/dynmatrix.hh>
 
#include <dune/fufem/geometry/convexpolyhedron.hh>
 
template <class Coordinate, class Matrix>
void populateMatrix(ConvexPolyhedron<Coordinate> const &segment,
                    Matrix &matrix) {
  size_t const nCorners = segment.vertices.size();
  for (size_t i = 0; i < nCorners; i++)
    for (size_t j = 0; j <= i; j++)
      matrix[i][j] = segment.vertices[i] * segment.vertices[j];
  // make symmetric
  for (size_t i = 0; i < nCorners; i++)
    for (size_t j = i + 1; j < nCorners; j++)
      matrix[i][j] = matrix[j][i];
}
 
template <class Coordinate>
void approximate(Coordinate const &target, Dune::DynamicMatrix<double> const &A,
                 Dune::DynamicVector<double> const &l,
                 ConvexPolyhedron<Coordinate> const &segment,
                 Dune::DynamicVector<double> &sol) {
  size_t nCorners = segment.vertices.size();
 
  // compute the residual
  Dune::DynamicVector<double> rhs = l;
  // compute rhs -= A*sol_
  for (size_t k = 0; k < nCorners; k++)
    for (size_t j = 0; j < nCorners; j++)
      rhs[k] -= A[k][j] * sol[j];
 
  // use the edge vectors as search directions
  double alpha = 0.0;
  for (size_t j1 = 0; j1 < nCorners - 1; ++j1) {
    for (size_t j2 = j1 + 1; j2 < nCorners; ++j2) {
      // compute matrix entry and rhs for edge direction
      double a = A[j1][j1] - A[j1][j2] - A[j2][j1] + A[j2][j2];
      double b = rhs[j1] - rhs[j2];
 
      // compute minimizer for correction
      if (a > 0) {
        alpha = b / a;
 
        double const largestAlpha = sol[j2];
        double const smallestAlpha = -sol[j1];
 
        // project alpha such that we stay positive
        if (alpha > largestAlpha)
          alpha = largestAlpha;
        else if (alpha < smallestAlpha)
          alpha = smallestAlpha;
      } else {
        // if the quadratic term vanishes, we have a linear function, which
        // attains its extrema on the boundary
        double sum = sol[j1] + sol[j2];
 
        double lValue = 0.5 * A[j1][j1] * sum * sum - rhs[j1] * sum;
        double uValue = 0.5 * A[j2][j2] * sum * sum - rhs[j2] * sum;
 
        alpha = lValue < uValue ? sol[j2] : -sol[j1];
      }
      // apply correction
      sol[j1] += alpha;
      sol[j2] -= alpha;
 
      // update the local residual for corrections
      for (size_t p = 0; p < nCorners; p++)
        rhs[p] -= (A[p][j1] - A[p][j2]) * alpha;
    }
  }
}
 
// Point-to-Polyhedron
template <class Coordinate>
double distance(const Coordinate &target,
                const ConvexPolyhedron<Coordinate> &segment,
                double correctionTolerance,
                size_t maxIterations = 1000) {
  size_t nCorners = segment.vertices.size();
  if (nCorners == 0)
    return std::numeric_limits<double>::infinity();
  double const barycentricTolerance = 1e-14;
 
  Dune::DynamicMatrix<double> A(nCorners, nCorners);
  populateMatrix(segment, A);
 
  Dune::DynamicVector<double> l(nCorners);
  for (size_t i = 0; i < nCorners; i++)
    l[i] = target * segment.vertices[i];
 
  // choose a feasible initial solution
  Dune::DynamicVector<double> sol(nCorners);
  for (size_t i = 0; i < nCorners; i++)
    sol[i] = 1.0 / nCorners;
 
  Coordinate oldCoordinates = segment.barycentricToGlobal(sol);
  Coordinate newCoordinates;
 
  for (size_t iterations = 0; iterations < maxIterations; ++iterations) {
    approximate(target, A, l, segment, sol);
 
    newCoordinates = segment.barycentricToGlobal(sol);
    if ((oldCoordinates - newCoordinates).two_norm() < correctionTolerance)
      break;
 
    oldCoordinates = newCoordinates;
  }
  segment.sanityCheck(sol, barycentricTolerance);
  return (target - newCoordinates).two_norm();
}
 
// Polyhedron-to-Polyhedron
template <class Coordinate>
double distance(const ConvexPolyhedron<Coordinate> &s1,
                const ConvexPolyhedron<Coordinate> &s2,
                double valueCorrectionTolerance,
                size_t maxIterations = 1000) {
  size_t nCorners1 = s1.vertices.size();
  size_t nCorners2 = s2.vertices.size();
  if (nCorners1 == 0 or nCorners2 == 0)
    return std::numeric_limits<double>::infinity();
  double const barycentricTolerance = 1e-14;
 
  // choose feasible initial solutions
  Dune::DynamicVector<double> sol1(nCorners1);
  for (size_t i = 0; i < s1.vertices.size(); i++)
    sol1[i] = 1.0 / nCorners1;
  auto c1 = s1.barycentricToGlobal(sol1);
 
  Dune::DynamicVector<double> sol2(nCorners2);
  for (size_t i = 0; i < nCorners2; i++)
    sol2[i] = 1.0 / nCorners2;
  auto c2 = s2.barycentricToGlobal(sol2);
 
  double oldDistance = (c2 - c1).two_norm();
 
  Dune::DynamicMatrix<double> A1(nCorners1, nCorners1);
  populateMatrix(s1, A1);
 
  Dune::DynamicMatrix<double> A2(nCorners2, nCorners2);
  populateMatrix(s2, A2);
 
  for (size_t iterations = 0; iterations < maxIterations; ++iterations) {
    Dune::DynamicVector<double> l2(nCorners2);
    for (size_t i = 0; i < nCorners2; i++)
      l2[i] = c1 * s2.vertices[i];
 
    approximate(c1, A2, l2, s2, sol2);
    c2 = s2.barycentricToGlobal(sol2);
 
    Dune::DynamicVector<double> l1(nCorners1);
    for (size_t i = 0; i < nCorners1; i++)
      l1[i] = c2 * s1.vertices[i];
    approximate(c2, A1, l1, s1, sol1);
    c1 = s1.barycentricToGlobal(sol1);
 
    double const newDistance = (c2 - c1).two_norm();
    if (oldDistance - newDistance <= valueCorrectionTolerance) {
      s1.sanityCheck(sol1, barycentricTolerance);
      s2.sanityCheck(sol2, barycentricTolerance);
      return newDistance;
    }
 
    oldDistance = newDistance;
  }
  return oldDistance;
}
 
// Point-to-Geometry convenience method
template <class Geometry>
double distance(typename Geometry::GlobalCoordinate const &point,
                Geometry const &geo, double valueCorrectionTolerance) {
  using Coordinate = typename Geometry::GlobalCoordinate;
 
  ConvexPolyhedron<Coordinate> polyhedron;
  polyhedron.vertices.resize(geo.corners());
  for (size_t i = 0; i < polyhedron.vertices.size(); ++i)
    polyhedron.vertices[i] = geo.corner(i);
 
  return distance(point, polyhedron, valueCorrectionTolerance);
}
 
// Polyhedron-to-Geometry convenience method
template <class Geometry>
double distance(ConvexPolyhedron<typename Geometry::GlobalCoordinate> const &p1,
                Geometry const &geo, double valueCorrectionTolerance,
                size_t maxIterations = 1000) {
  using Coordinate = typename Geometry::GlobalCoordinate;
 
  ConvexPolyhedron<Coordinate> p2;
  p2.vertices.resize(geo.corners());
  for (size_t i = 0; i < p2.vertices.size(); ++i)
    p2.vertices[i] = geo.corner(i);
 
  return distance(p2, p1, valueCorrectionTolerance, maxIterations);
}
 
#endif