Add polyhedron distance minimisation functions

dune/fufem/CMakeLists.txt

@@ -22,6 +22,7 @@ install(FILES
     boundingbox.hh
     box.hh
     concept.hh
+    convexpolyhedron.hh
     dataio.hh
     dgindexset.hh
     dgpqkindexset.hh
@@ -42,6 +43,7 @@ install(FILES
     mappedmatrix.hh
     matlab_io.hh
     orientedsubface.hh
+    polyhedrondistance.hh
     prolongboundarypatch.hh
     readbitfield.hh
     referenceelementhelper.hh

dune/fufem/convexpolyhedron.hh

+#ifndef DUNE_FUFEM_CONVEXPOLYHEDRON_HH
+#define DUNE_FUFEM_CONVEXPOLYHEDRON_HH
+
+#include <dune/fufem/arithmetic.hh>
+
+template <class Coordinate> struct ConvexPolyhedron {
+  std::vector<Coordinate> vertices;
+
+  template <class DynamicVector>
+  Coordinate barycentricToGlobal(DynamicVector const &b) const {
+    assert(b.size() == vertices.size());
+    Coordinate r(0);
+    for (size_t i = 0; i < b.size(); i++)
+      Arithmetic::addProduct(r, b[i], vertices[i]);
+    return r;
+  }
+
+  template <class DynamicVector>
+  void sanityCheck(DynamicVector const &b, double tol) const {
+    double sum = 0;
+    for (size_t i = 0; i < b.size(); i++) {
+      if (b[i] < -tol)
+        DUNE_THROW(Dune::Exception, "No barycentric coords " << b[1] << " nr. "
+                                                             << i);
+      sum += b[i];
+    }
+    if (sum > 1 + tol)
+      DUNE_THROW(Dune::Exception, "No barycentric coords, sum: " << sum);
+  }
+};
+#endif

dune/fufem/polyhedrondistance.hh

+#ifndef DUNE_FUFEM_POLYHEDRONDISTANCE_HH
+#define DUNE_FUFEM_POLYHEDRONDISTANCE_HH
+
+// Based on the closest point projection from dune-contact
+
+#include <dune/common/dynvector.hh>
+#include <dune/common/dynmatrix.hh>
+
+#include <dune/fufem/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];
+}
+
+/**
+ * The functional to be minimized is
+ *
+ *   0.5*norm(\sum_i lambda_i corner_i - target)^2
+ *
+ * where lambda_i are barycentric coordinates, i.e.
+ *
+ *   0 \le lambda_i \le 1 and \sum_i lambda_i=1
+ *
+ * The resulting quadratic minimization problem is given by
+ *
+ *   0.5 lambda^T*A*lambda - l^T*lambda
+ *
+ * with A_ij = (corner_i,corner_j)  and l_i = (corner_i, target)
+ */
+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 nCorners = segment.vertices.size();
+  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;
+
+  size_t maxIterations = 1000;
+  size_t iterations;
+  for (iterations = 0; iterations < maxIterations; ++iterations) {
+    approximate(target, A, l, segment, sol);
+
+    newCoordinates = segment.barycentricToGlobal(sol);
+    if ((oldCoordinates - newCoordinates).two_norm() < correctionTolerance)
+      break;
+
+    oldCoordinates = newCoordinates;
+  }
+  assert(iterations < maxIterations);
+
+  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 nCorners1 = s1.vertices.size();
+  size_t nCorners2 = s2.vertices.size();
+  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 / s1.vertices.size();
+  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);
+
+  size_t maxIterations = 1000;
+  size_t iterations;
+  for (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();
+    assert(newDistance - oldDistance <= 1e-12); // debugging
+    if (oldDistance - newDistance <= valueCorrectionTolerance) {
+      s1.sanityCheck(sol1, barycentricTolerance);
+      s2.sanityCheck(sol2, barycentricTolerance);
+      return newDistance;
+    }
+
+    oldDistance = newDistance;
+  }
+  assert(false);
+}
+
+// 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) {
+  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);
+}
+
+#endif

dune/fufem/test/CMakeLists.txt

@@ -34,6 +34,7 @@ set(OTHER_TESTS
   pgmtest
   ppmtest
   serializationtest
+  test-polyhedral-minimisation
   )
 
 set(TESTS ${OTHER_TESTS} ${GRID_BASED_TESTS})

dune/fufem/test/test-polyhedral-minimisation.cc

+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <dune/common/fvector.hh>
+
+#include <dune/fufem/polyhedrondistance.hh>
+
+int main() {
+  using LocalVector = Dune::FieldVector<double, 2>;
+
+  auto const test =
+      [&](ConvexPolyhedron<LocalVector> const &p1,
+          ConvexPolyhedron<LocalVector> const &p2, double analyticalDistance) {
+        LocalVector target;
+        {
+          double const error =
+              std::abs(analyticalDistance - distance(p1, p2, 1e-12));
+          std::cout << "error: " << error << std::endl;
+          assert(error < 1e-12);
+        }
+      };
+  {
+    /*
+     * Calculate the distance between two triangles, where it is attained at a
+     * face-vertex pair
+     *
+     *    O
+     *    |\
+     *    | \
+     * O  O--O
+     * |\
+     * | \
+     * O--O
+     */
+    double const analyticalDistance = std::sqrt(2.0);
+    ConvexPolyhedron<LocalVector> const bs1 = {
+      { { 0, 0 }, { 0, 2 }, { 2, 0 } }
+    };
+    ConvexPolyhedron<LocalVector> const bs2 = {
+      { { 2, 2 }, { 2, 4 }, { 4, 2 } }
+    };
+    test(bs1, bs2, analyticalDistance);
+  }
+
+  {
+    /*
+     * Calculate the distance between two triangles, where it is
+     * attained in a face-face pair
+     *
+     *    O--O
+     *     \ |
+     *      \|
+     * O     O
+     * |\
+     * | \
+     * O--O
+     */
+    double const analyticalDistance = 2.0 * std::sqrt(2.0);
+    ConvexPolyhedron<LocalVector> const bs1 = {
+      { { 0, 0 }, { 0, 2 }, { 2, 0 } }
+    };
+    ConvexPolyhedron<LocalVector> const bs2 = {
+      { { 4, 4 }, { 2, 4 }, { 4, 2 } }
+    };
+    test(bs1, bs2, analyticalDistance);
+  }
+
+  {
+    /*
+     * Calculate the distance between two triangles, where it is
+     * attained in a vertex-vertex pair
+     *
+     *       O
+     *       |\
+     *       | \
+     * O--O  O--O
+     *  \ |
+     *   \|
+     *    O
+     */
+    double const analyticalDistance = 2.0;
+    ConvexPolyhedron<LocalVector> const bs1 = {
+      { { 2, 2 }, { 0, 2 }, { 2, 0 } }
+    };
+    ConvexPolyhedron<LocalVector> const bs2 = {
+      { { 4, 2 }, { 4, 4 }, { 6, 2 } }
+    };
+    test(bs1, bs2, analyticalDistance);
+  }
+}