The ultimate union of CGAL::{Polyhedron_3,Nef_polyhedron_3} with fast-union optimizations and edge case detection for OpenSCAD

cgalutils-applyops.cc

	shared_ptr<const Geometry> applyUnion3DFastPolyhedron(
		Geometry::Geometries::iterator chbegin, Geometry::Geometries::iterator chend,
		const Tree* tree)
	{
    typedef std::pair<shared_ptr<Polyhedron>, int> QueueItem;
		struct QueueItemGreater {
			// stable sort for priority_queue by facets, then progress mark
			bool operator()(const QueueItem &lhs, const QueueItem& rhs) const
			{
				size_t l = lhs.first->numFacets();
				size_t r = rhs.first->numFacets();
				return (l > r) || (l == r && lhs.second > rhs.second);
			}
		};

		try {
			Geometry::Geometries children;
			children.insert(children.end(), chbegin, chend);

      // TODO: bucket things in the queue by rough edge count. Then pick non-overlaps from there!

			// // We could move the feature check around this call and make this the only
			// // applyUnion3D method (only other meaningful change should be the faster
			// // queue creation).
			// reduceFastUnionableGeometries(children, tree);

			// if (children.empty()) return nullptr;
			// if (children.size() == 1) {
			// 	// Fast-skip to avoid useless conversion to nef.
			// 	return children.begin()->second;
			// }

			// We'll fill the queue in one go to get linear time construction.
			std::vector<QueueItem> queueItems;
			queueItems.reserve(children.size());

      size_t total_facets = 0;
			for (auto &item : children) {
        auto chgeom = item.second;
        if (!chgeom || chgeom->isEmpty()) {
          continue;
        }
				// TODO(ochafik): Have that helper return futures, and wait for them in batch.
        shared_ptr<Polyhedron> poly;
        if (auto ps = dynamic_pointer_cast<const PolySet>(chgeom)) {
          poly = make_shared<Polyhedron>(*ps);
        } else if (auto nef = dynamic_pointer_cast<const CGAL_Nef_polyhedron>(chgeom)) {
          // assert(!"Unsupported: CGAL_Nef_polyhedron!");
          poly = make_shared<Polyhedron>(*nef);
        } else {
          assert(!"Unsupported format");
          continue;
        }


        total_facets += poly->numFacets();
        auto node_mark = item.first ? item.first->progress_mark : -1;
        queueItems.emplace_back(poly, node_mark);
			}
			// Build the queue in linear time (don't add items one by one!).
			std::priority_queue<QueueItem, std::vector<QueueItem>, QueueItemGreater>
				 q(queueItems.begin(), queueItems.end());


			LOG(message_group::Warning, getLocation(chbegin->first),
				"", "Union of %1$lu geometries (%2$lu total facets)", q.size(), total_facets);

			{
				ScopedTimer timer("Union of all Polyhedrons");
				progress_tick();
				while (q.size() > 1) {
					auto p1 = q.top();
					q.pop();
					auto p2 = q.top();
					q.pop();
					ScopedTimer timer("  Union of 2 Polyhedron");
			    LOG(message_group::Echo, getLocation(chbegin->first),
				    "", "  Polyhedron (%1$lu) += Polyhedron (%2$lu)", p1.first->numFacets(), p2.first->numFacets());
          *p1.first += *p2.first;
				  q.emplace(p1.first, -1);
					progress_tick();
				}
			}

			if (q.size() == 1) {
				return shared_ptr<const Geometry>(q.top().first->toGeometry());
			} else {
				return nullptr;
			}
		}
		catch (const CGAL::Failure_exception &e) {
			LOG(message_group::Error, Location::NONE, "", "CGAL error in CGALUtils::applyUnion3DFast: %1$s", e.what());
		}
		return nullptr;
	}

polyhedron.h

// Portions of this file are Copyright 2021 Google LLC, and licensed under GPL2+. See COPYING.
#pragma once

#include <CGAL/Polygon_mesh_processing/corefinement.h>
#include <CGAL/Polygon_mesh_processing/manifoldness.h>
#include <CGAL/Polygon_mesh_processing/orient_polygon_soup_extension.h>
#include <CGAL/Polygon_mesh_processing/orientation.h>
#include <CGAL/Polygon_mesh_processing/polygon_soup_to_polygon_mesh.h>
#include <CGAL/Polygon_mesh_processing/triangulate_faces.h>
#include <CGAL/Surface_mesh.h>
#include <unordered_set>

#include "bounding_boxes.h"
#include "cgal.h"
#include "cgalutils.h"
#include "linalg.h"
#include "polyset.h"
#include "Reindexer.h"
#include "scoped_timer.h"


namespace PMP = CGAL::Polygon_mesh_processing;
namespace params = PMP::parameters;

/*! A mutable polyhedron backed by a CGAL::Polyhedron_3 and fast Polygon Mesh
 * Processing (PMP) CSG functions when possible (manifold cases), or by a
 * CGAL::Nef_polyhedron_3 when it's not (non manifold cases).
 *
 * Note that even `cube(1); translate([1, 0, 0]) cube(1)` is considered
 * non-manifold because of shared vertices. PMP seems to be fine with edges
 * that share segments with others, so long as there's no shared vertex.
 *
 * Also, keeps track of the bounding boxes of past operations to fast-track
 * unions of disjoint bodies.
 */
class Polyhedron
{
  // https://doc.cgal.org/latest/Kernel_d/structCGAL_1_1Epeck__d.html
  typedef CGAL::Epeck kernel_t;
  // typedef CGAL::Simple_cartesian<CGAL::Gmpq> kernel_t;

  typedef CGAL::Point_3<kernel_t> point_t;
  typedef CGAL::Polyhedron_3<kernel_t> polyhedron_t;
  typedef CGAL::Nef_polyhedron_3<kernel_t> nef_polyhedron_t;

  // This contains data either as a polyhedron, or as a nef polyhedron.
  //
  // We stick to nef polyhedra in presence of non-manifold geometry (detected in
  // operations where the operands share any vertex), which breaks the
  // algorithms of Polygon Mesh Processing library that operate on normal
  // (non-nef) polyhedra.
  boost::variant<shared_ptr<polyhedron_t>, shared_ptr<nef_polyhedron_t>> data;
  BoundingBoxes bboxes;

  // Used to determine if two polyhedra share vertices. This is a no-no for
  // Polygon Mesh Processing boolean functions (in that case we use Nefs).
  std::unordered_set<polyhedron_t::Point_3> vertex_set;

public:

  /*! Builds a polyhedron using the provided, untrusted PolySet.
   * Face orientation is checked (and reversed if needed), faces are
   * triangulated (requirement of Polygon Mesh Processing functions),
   * and we check manifoldness (we use a nef polyhedra for non-manifold cases).
   */
  Polyhedron(const PolySet &ps)
  {
    ScopedTimer timer("Polyhedron(PolySet)");

    auto polyhedron = make_shared<polyhedron_t>();
    auto err = CGALUtils::createPolyhedronFromPolySet(ps, *polyhedron);
    assert(!err);
    data = polyhedron;
    bboxes.add(CGALUtils::boundingBox(ps));

    PMP::triangulate_faces(*polyhedron);
    if (!PMP::is_outward_oriented(*polyhedron, params::all_default())) {
      LOG(message_group::Warning, Location::NONE, "",
          "PolySet's %1$lu faces were oriented inwards and need inversion.",
          polyhedron->size_of_facets());
      PMP::reverse_face_orientations(polyhedron->facet_handles(),
                                                               *polyhedron);
    }

    size_t nonManifoldVertexCount = 0;
    for(auto &v : CGAL::vertices(*polyhedron))
    // for (auto it = polyhedron->vertices_begin(), end = polyhedron->vertices_end(); it != end; it++)
    {
      // auto &v = *it;
      if (PMP::is_non_manifold_vertex(v, *polyhedron)) {
        nonManifoldVertexCount++;
      }
    }

    if (!nonManifoldVertexCount) {
      LOG(message_group::Warning, Location::NONE, "",
          "PolySet had %1$lu non-manifold vertices. Converting polyhedron to nef polyhedron.",
          nonManifoldVertexCount);
      convertToNefPolyhedron();
    }
  }

  /*! Builds a polyhedron using a legacy nef polyhedron object.
   * This transitional method will disappear when this Polyhedron object is
   * fully integrated and replaces all of CGAL_Nef_polyhedron's uses.
   */
  Polyhedron(const CGAL_Nef_polyhedron3 &nef)
  {
    ScopedTimer timer("Polyhedron(CGAL_Nef_polyhedron3)");

    auto polyhedron = make_shared<polyhedron_t>();
    CGAL_Polyhedron poly;
    nef.convert_to_polyhedron(poly);
    CGALUtils::copyPolyhedron(poly, *polyhedron);
    data = polyhedron;
    bboxes.add(CGALUtils::boundingBox(nef));
  }

  bool isEmpty() const { return numFacets() == 0; }

  size_t numFacets() const
  {
    if (auto poly = boost::get<shared_ptr<polyhedron_t>>(data)) {
      return poly->size_of_facets();
    }
    if (auto nef = boost::get<shared_ptr<nef_polyhedron_t>>(data)) {
      return nef->number_of_facets();
    }
    assert(!"Invalid Polyhedron.data state");
    return false;
  }

  size_t numVertices() const
  {
    if (auto poly = boost::get<shared_ptr<polyhedron_t>>(data)) {
      return poly->size_of_vertices();
    }
    if (auto nef = boost::get<shared_ptr<nef_polyhedron_t>>(data)) {
      return nef->number_of_vertices();
    }
    assert(!"Invalid Polyhedron.data state");
    return false;
  }

  bool isManifold() const
  {
    if (auto poly = boost::get<shared_ptr<polyhedron_t>>(data)) {
      // We don't keep simple polyhedra if they're not manifold (see contructor)
      return true;
    }
    if (auto nef = boost::get<shared_ptr<nef_polyhedron_t>>(data)) {
      return nef->is_simple();
    }
    assert(!"Invalid Polyhedron.data state");
    return false;
  }

  shared_ptr<const Geometry> toGeometry() const
  {
    if (auto nef = boost::get<shared_ptr<nef_polyhedron_t>>(data)) {
      auto ps = make_shared<PolySet>(3);
      auto err = CGALUtils::createPolySetFromNefPolyhedron3(*nef, *ps);
      assert(!err);
      return ps;
    }
    else if (auto poly = boost::get<shared_ptr<polyhedron_t>>(data)) {
      auto ps = make_shared<PolySet>(3);
      auto err = CGALUtils::createPolySetFromPolyhedron(*poly, *ps);
      assert(!err);
      return ps;
    }
    else {
      assert(!"Invalid Polyhedron.data state");
      return nullptr;
    }
  }

  void clear()
  {
    data = make_shared<polyhedron_t>();
    bboxes.clear();
  }

  /*! In-place union (this may also mutate/corefine the other polyhedron). */
  void operator+=(Polyhedron &other)
  {
    if (!bboxes.intersects(other.bboxes)) {
      polyBinOp("fast union", other,
                               [&](polyhedron_t &destinationPoly, polyhedron_t &otherPoly) {
                                 CGALUtils::appendToPolyhedron(otherPoly, destinationPoly);
                               });
    }
    else {
      if (!isManifold() || !other.isManifold() || sharesAnyVertexWith(other)) {
        nefPolyBinOp(
            "union", other, [&](nef_polyhedron_t &destinationNef, nef_polyhedron_t &otherNef) {
              destinationNef += otherNef;
            });
      }
      else {
        polyBinOp("union", other,
                                 [&](polyhedron_t &destinationPoly, polyhedron_t &otherPoly) {
                                   PMP::corefine_and_compute_union(
                                       destinationPoly, otherPoly, destinationPoly,
                                       params::all_default(), params::all_default());
                                 });
      }
    }
    bboxes += other.bboxes;
  }

  /*! In-place intersection (this may also mutate/corefine the other polyhedron). */
  void operator*=(Polyhedron &other)
  {
    if (!bboxes.intersects(other.bboxes)) {
      printf("Empty intersection difference!\n");
      clear();
      return;
    }

    if (!isManifold() || !other.isManifold() || sharesAnyVertexWith(other)) {
      nefPolyBinOp("intersection", other,
                                  [&](nef_polyhedron_t &destinationNef,
                                      nef_polyhedron_t &otherNef) { destinationNef *= otherNef; });
    }
    else {
      polyBinOp("intersection", other,
                               [&](polyhedron_t &destinationPoly, polyhedron_t &otherPoly) {
                                 PMP::corefine_and_compute_intersection(
                                     destinationPoly, otherPoly, destinationPoly,
                                     params::all_default(), params::all_default());
                               });
    }
    bboxes *= other.bboxes;
  }

  /*! In-place difference (this may also mutate/corefine the other polyhedron). */
  void operator-=(Polyhedron &other)
  {
    if (!bboxes.intersects(other.bboxes)) {
      printf("Non intersecting difference!\n");
      return;
    }

    // Note: we don't need to change the bbox.
    if (!isManifold() || !other.isManifold() || sharesAnyVertexWith(other)) {
      nefPolyBinOp("intersection", other,
                                  [&](nef_polyhedron_t &destinationNef,
                                      nef_polyhedron_t &otherNef) { destinationNef -= otherNef; });
    }
    else {
      polyBinOp("difference", other,
                               [&](polyhedron_t &destinationPoly, polyhedron_t &otherPoly) {
                                 PMP::corefine_and_compute_difference(
                                     destinationPoly, otherPoly, destinationPoly,
                                     params::all_default(), params::all_default());
                               });
    }
  }

  /*! In-place minkowksi operation. If the other polyhedron is non-convex,
   * it is modified during the computation, i.e., it is decomposed into convex pieces.
   */
  void minkowski(Polyhedron &other)
  {
    nefPolyBinOp("minkowski", other,
                                [&](nef_polyhedron_t &destinationNef, nef_polyhedron_t &otherNef) {
                                  destinationNef = CGAL::minkowski_sum_3(destinationNef, otherNef);
                                });
  }

private:

  /*! Templated so it can apply to a Nef Polyhedron or a normal Polyhedron. */
  template <typename T>
  static void foreachVertexUntilTrue(const T& poly, const std::function<bool(const point_t& pt)>& f) {
    for (auto fi = poly.facets_begin(); fi != poly.facets_end(); ++fi) {
      auto hc = fi->facet_begin();
      auto hc_end = hc;
      do {
        auto &v = *((hc++)->vertex());
        if (f(v.point())) return;
      } while (hc != hc_end);
    }
  }

  /*! Iterate over all vertices' points until the function returns true (for done). */
  void foreachVertexUntilTrue(const std::function<bool(const point_t& pt)>& f) const
  {
    if (auto poly = boost::get<shared_ptr<polyhedron_t>>(data))
    {
      polyhedron_t::Vertex_const_iterator vi;
      CGAL_forall_vertices(vi, *poly) {
        if (f(vi->point())) return;
      }
    }
    else if (auto nef = boost::get<shared_ptr<nef_polyhedron_t>>(data))
    {
      nef_polyhedron_t::Vertex_const_iterator vi;
      CGAL_forall_vertices(vi, *nef) {
        if (f(vi->point())) return;
      }
    }
    else {
      assert(!"Invalid Polyhedron.data state");
    }
  }

  bool sharesAnyVertexWith(const Polyhedron &other) const
  {
    if (other.numVertices() < numVertices()) {
      // The other has less vertices to index!
      return other.sharesAnyVertexWith(*this);
    }

    ScopedTimer timer("sharesAnyVertexWith");

    std::unordered_set<polyhedron_t::Point_3> vertices;
    foreachVertexUntilTrue([&](const auto &p) {
      vertices.insert(p);
      return false;
    });

    auto foundCollision = false;
    other.foreachVertexUntilTrue([&](const auto &p) {
      return foundCollision = vertices.find(p) != vertices.end();
    });

    return foundCollision;
  }

  /*! Runs a binary operation that operates on nef polyhedra, stores the result in
   * the first one and potentially mutates (e.g. corefines) the second. */
  void nefPolyBinOp(const std::string &opName, Polyhedron &other,
                                   const std::function<void(nef_polyhedron_t &destinationNef,
                                                            nef_polyhedron_t &otherNef)> &operation)
  {
    ScopedTimer timer(std::string("nef ") + opName);

    auto &destinationNef = convertToNefPolyhedron();
    auto &otherNef = other.convertToNefPolyhedron();

    auto manifoldBefore = isManifold() && other.isManifold();
    operation(destinationNef, otherNef);

    auto manifoldNow = isManifold();
    if (manifoldNow != manifoldBefore) {
      LOG(message_group::Echo, Location::NONE, "",
          "Nef operation got %1$s operands and produced a %2$s result",
          manifoldBefore ? "manifold" : "non-manifold", manifoldNow ? "manifold" : "non-manifold");
    }
  }

  /*! Runs a binary operation that operates on polyhedra, stores the result in
   * the first one and potentially mutates (e.g. corefines) the second. */
  void polyBinOp(
      const std::string &opName, Polyhedron &other,
      const std::function<void(polyhedron_t &destinationPoly, polyhedron_t &otherPoly)> &operation)
  {
    ScopedTimer timer(std::string("mesh ") + opName);

    auto &destinationPoly = convertToPolyhedron();
    auto &otherPoly = other.convertToPolyhedron();

    operation(destinationPoly, otherPoly);
  }

  nef_polyhedron_t &convertToNefPolyhedron()
  {
    if (auto poly = boost::get<shared_ptr<polyhedron_t>>(data)) {
      ScopedTimer timer("Polyhedron -> Nef");

      auto nef = make_shared<nef_polyhedron_t>(*poly);
      data = nef;
      return *nef;
    }
    else if (auto nef = boost::get<shared_ptr<nef_polyhedron_t>>(data)) {
      return *nef;
    }
    else {
      throw "Bad data state";
    }
  }

  polyhedron_t &convertToPolyhedron()
  {
    if (auto nef = boost::get<shared_ptr<nef_polyhedron_t>>(data)) {
      ScopedTimer timer("Nef -> Polyhedron");

      auto poly = make_shared<polyhedron_t>();
      nef->convert_to_polyhedron(*poly);
      data = poly;
      return *poly;
    }
    else if (auto poly = boost::get<shared_ptr<polyhedron_t>>(data)) {
      return *poly;
    }
    else {
      throw "Bad data state";
    }
  }

  /*
    void check()
    {
      std::cout << "Using parallel mode? "
                << std::is_same<CGAL::Parallel_if_available_tag, CGAL::Parallel_tag>::value
                << std::endl;
      bool intersecting = PMP::does_self_intersect<CGAL::Parallel_if_available_tag>(
          polyhedron, CGAL::parameters::vertex_point_map(get(CGAL::vertex_point, polyhedron)));
      std::cout << (intersecting ? "There are self-intersections." : "There is no
    self-intersection.")
                << std::endl;
    }

    void fix_manifoldness()
    {
      std::cout << "Before stitching : " << std::endl;
      std::cout << "\t Number of vertices  :\t" << polyhedron.size_of_vertices() << std::endl;
      std::cout << "\t Number of halfedges :\t" << polyhedron.size_of_halfedges() << std::endl;
      std::cout << "\t Number of facets    :\t" << polyhedron.size_of_facets() << std::endl;
      PMP::stitch_borders(polyhedron);
      std::cout << "Stitching done : " << std::endl;
      std::cout << "\t Number of vertices  :\t" << polyhedron.size_of_vertices() << std::endl;
      std::cout << "\t Number of halfedges :\t" << polyhedron.size_of_halfedges() << std::endl;
      std::cout << "\t Number of facets    :\t" << polyhedron.size_of_facets() << std::endl;

      // Fix manifoldness by splitting non-manifold vertices
      std::size_t new_vertices_nb =
          PMP::duplicate_non_manifold_vertices(polyhedron, params::all_default());
      std::cout << new_vertices_nb << " vertices have been added to fix mesh manifoldness"
                << std::endl;

      Reindexer<polyhedron_t::Point_3> vertices;
      std::vector<std::vector<std::size_t>> polygons;
      for (auto fi = polyhedron.facets_begin(); fi != polyhedron.facets_end(); ++fi) {
        auto hc = fi->facet_begin();
        auto hc_end = hc;

        std::vector<std::size_t> polygon;
        do {
          auto &v = *((hc++)->vertex());
          size_t idx = vertices.lookup(v.point());
          polygon.push_back(idx);
        } while (hc != hc_end);

        polygons.emplace_back(polygon);
      }

      auto points = vertices.getArray();
      auto numpoints_before = points.size();
      auto numpolygons_before = polygons.size();
      PMP::duplicate_non_manifold_edges_in_polygon_soup(points, polygons);

      auto numpoints_after = points.size();
      auto numpolygons_after = polygons.size();
      printf("POINTS:: BEFORE: %lu; AFTER: %lu\n", numpoints_before, numpoints_after);
      printf("POLYS: BEFORE: %lu; AFTER: %lu\n", numpolygons_before, numpolygons_after);

      polyhedron_t repaired;
      PMP::polygon_soup_to_polygon_mesh(points, polygons, repaired);

      polyhedron = repaired;
    }
  */
};