Optimize triangulator with collider (elalish#830)

* implemented - weird linker error

* fix linker

* fixed empty collider

* improved perf and cleanup

* add comments

* more docs

src/collider/include/collider.h

@@ -30,6 +30,7 @@ class Collider {
   template <const bool selfCollision = false, const bool inverted = false,
             typename T>
   SparseIndices Collisions(const VecView<const T>& queriesIn) const;
+  static uint32_t MortonCode(glm::vec3 position, Box bBox);
 
  private:
   Vec<Box> nodeBBox_;

src/collider/src/collider.cpp

@@ -265,6 +265,14 @@ struct TransformBox {
   const glm::mat4x3 transform;
   void operator()(Box& box) { box = box.Transform(transform); }
 };
+
+uint32_t SpreadBits3(uint32_t v) {
+  v = 0xFF0000FFu & (v * 0x00010001u);
+  v = 0x0F00F00Fu & (v * 0x00000101u);
+  v = 0xC30C30C3u & (v * 0x00000011u);
+  v = 0x49249249u & (v * 0x00000005u);
+  return v;
+}
 }  // namespace
 
 namespace manifold {
@@ -374,6 +382,18 @@ bool Collider::Transform(glm::mat4x3 transform) {
   return axisAligned;
 }
 
+/**
+ * Calculate the 32-bit Morton code of a position within a bounding box.
+ */
+uint32_t Collider::MortonCode(glm::vec3 position, Box bBox) {
+  glm::vec3 xyz = (position - bBox.min) / (bBox.max - bBox.min);
+  xyz = glm::min(glm::vec3(1023.0f), glm::max(glm::vec3(0.0f), 1024.0f * xyz));
+  uint32_t x = SpreadBits3(static_cast<uint32_t>(xyz.x));
+  uint32_t y = SpreadBits3(static_cast<uint32_t>(xyz.y));
+  uint32_t z = SpreadBits3(static_cast<uint32_t>(xyz.z));
+  return x * 4 + y * 2 + z;
+}
+
 template SparseIndices Collider::Collisions<true, false, Box>(
     const VecView<const Box>&) const;
 
@@ -391,5 +411,4 @@ template SparseIndices Collider::Collisions<false, true, Box>(
 
 template SparseIndices Collider::Collisions<false, true, glm::vec3>(
     const VecView<const glm::vec3>&) const;
-
 }  // namespace manifold

src/manifold/src/sort.cpp

@@ -48,25 +48,12 @@ struct Extrema : public thrust::binary_function<Halfedge, Halfedge, Halfedge> {
   }
 };
 
-uint32_t SpreadBits3(uint32_t v) {
-  v = 0xFF0000FFu & (v * 0x00010001u);
-  v = 0x0F00F00Fu & (v * 0x00000101u);
-  v = 0xC30C30C3u & (v * 0x00000011u);
-  v = 0x49249249u & (v * 0x00000005u);
-  return v;
-}
-
 uint32_t MortonCode(glm::vec3 position, Box bBox) {
   // Unreferenced vertices are marked NaN, and this will sort them to the end
   // (the Morton code only uses the first 30 of 32 bits).
   if (isnan(position.x)) return kNoCode;
 
-  glm::vec3 xyz = (position - bBox.min) / (bBox.max - bBox.min);
-  xyz = glm::min(glm::vec3(1023.0f), glm::max(glm::vec3(0.0f), 1024.0f * xyz));
-  uint32_t x = SpreadBits3(static_cast<uint32_t>(xyz.x));
-  uint32_t y = SpreadBits3(static_cast<uint32_t>(xyz.y));
-  uint32_t z = SpreadBits3(static_cast<uint32_t>(xyz.z));
-  return x * 4 + y * 2 + z;
+  return Collider::MortonCode(position, bBox);
 }
 
 struct Morton {

src/polygon/CMakeLists.txt

@@ -15,11 +15,14 @@
 project (polygon)
 
 file(GLOB_RECURSE SOURCE_FILES CONFIGURE_DEPENDS *.cpp)
-add_library(${PROJECT_NAME} OBJECT ${SOURCE_FILES})
+add_library(${PROJECT_NAME} ${SOURCE_FILES})
 target_include_directories(${PROJECT_NAME} PUBLIC
     $<INSTALL_INTERFACE:include/${CMAKE_PROJECT_NAME}>
     $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>)
-target_link_libraries(${PROJECT_NAME} PUBLIC utilities)
+target_link_libraries(${PROJECT_NAME} 
+    PUBLIC utilities 
+    PRIVATE collider
+)
 
 target_compile_options(${PROJECT_NAME} PRIVATE ${MANIFOLD_FLAGS})
 target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17)

src/polygon/src/polygon.cpp

@@ -18,6 +18,7 @@
 #include <map>
 #include <set>
 
+#include "collider.h"
 #include "optional_assert.h"
 #include "utils.h"
 
@@ -211,6 +212,7 @@ class EarClip {
  private:
   struct Vert;
   typedef std::vector<Vert>::iterator VertItr;
+  typedef std::vector<Vert>::const_iterator VertItrC;
   struct MaxX {
     bool operator()(const VertItr &a, const VertItr &b) const {
       return a->pos.x > b->pos.x;
@@ -237,9 +239,16 @@ class EarClip {
   std::multiset<VertItr, MinCost> earsQueue_;
   // The output triangulation.
   std::vector<glm::ivec3> triangles_;
+  // Bounding box of the entire set of polygons
+  Rect bBox_;
   // Working precision: max of float error and input value.
   float precision_;
 
+  struct IdxCollider {
+    Collider collider;
+    Vec<VertItr> itr;
+  };
+
   // A circularly-linked list representing the polygon(s) that still need to be
   // triangulated. This gets smaller as ears are clipped until it degenerates to
   // two points and terminates.
@@ -400,47 +409,54 @@ class EarClip {
     // For verts outside the ear, apply a cost based on the Delaunay condition
     // to aid in prioritization and produce cleaner triangulations. This doesn't
     // affect robustness, but may be adjusted to improve output.
-    float DelaunayCost(glm::vec2 diff, float scale, float precision) const {
+    static float DelaunayCost(glm::vec2 diff, float scale, float precision) {
       return -precision - scale * glm::dot(diff, diff);
     }
 
-    // This is the O(n^2) part of the algorithm, checking this ear against every
-    // Vert to ensure none are inside. It may be possible to improve performance
-    // by using the Collider to get it down to nlogn or doing some
-    // parallelization, but that may be more trouble than it's worth.
+    // This is the expensive part of the algorithm, checking this ear against
+    // every Vert to ensure none are inside. The Collider brings the total
+    // triangulator cost down from O(n^2) to O(nlogn) for most large polygons.
     //
     // Think of a cost as vaguely a distance metric - 0 is right on the edge of
     // being invalid. cost > precision is definitely invalid. Cost < -precision
     // is definitely valid, so all improvement costs are designed to always give
     // values < -precision so they will never affect validity. The first
     // totalCost is designed to give priority to sharper angles. Any cost < (-1
     // - precision) has satisfied the Delaunay condition.
-    float EarCost(float precision) const {
+    float EarCost(float precision, const IdxCollider &collider) const {
       glm::vec2 openSide = left->pos - right->pos;
       const glm::vec2 center = 0.5f * (left->pos + right->pos);
       const float scale = 4 / glm::dot(openSide, openSide);
+      const float radius = glm::length(openSide) / 2;
       openSide = glm::normalize(openSide);
 
       float totalCost = glm::dot(left->rightDir, rightDir) - 1 - precision;
       if (CCW(pos, left->pos, right->pos, precision) == 0) {
         // Clip folded ears first
         return totalCost < -1 ? kBest : 0;
       }
-      VertItr test = right->right;
-      auto lid = left->mesh_idx;
-      auto rid = right->mesh_idx;
-      while (test != left) {
-        if (test->mesh_idx != mesh_idx && test->mesh_idx != lid &&
+
+      Vec<Box> earBox;
+      earBox.push_back({glm::vec3(center.x - radius, center.y - radius, 0),
+                        glm::vec3(center.x + radius, center.y + radius, 0)});
+      earBox.back().Union(glm::vec3(pos, 0));
+      const SparseIndices toTest = collider.collider.Collisions(earBox.cview());
+
+      const int lid = left->mesh_idx;
+      const int rid = right->mesh_idx;
+      for (int i = 0; i < toTest.size(); ++i) {
+        const VertItr test = collider.itr[toTest.Get(i, true)];
+        if (!Clipped(test) && test->mesh_idx != mesh_idx &&
+            test->mesh_idx != lid &&
             test->mesh_idx != rid) {  // Skip duplicated verts
           float cost = Cost(test, openSide, precision);
           if (cost < -precision) {
             cost = DelaunayCost(test->pos - center, scale, precision);
           }
           totalCost = glm::max(totalCost, cost);
         }
-
-        test = test->right;
       }
+
       return totalCost;
     }
 
@@ -454,26 +470,26 @@ class EarClip {
     }
   };
 
-  glm::vec2 SafeNormalize(glm::vec2 v) const {
+  static glm::vec2 SafeNormalize(glm::vec2 v) {
     glm::vec2 n = glm::normalize(v);
     return glm::isfinite(n.x) ? n : glm::vec2(0, 0);
   }
 
   // This function and JoinPolygons are the only functions that affect the
   // circular list data structure. This helps ensure it remains circular.
-  void Link(VertItr left, VertItr right) const {
+  static void Link(VertItr left, VertItr right) {
     left->right = right;
     right->left = left;
     left->rightDir = SafeNormalize(right->pos - left->pos);
   }
 
   // When an ear vert is clipped, its neighbors get linked, so they get unlinked
   // from it, but it is still linked to them.
-  bool Clipped(VertItr v) const { return v->right->left != v; }
+  static bool Clipped(VertItr v) { return v->right->left != v; }
 
   // Apply func to each un-clipped vert in a polygon and return an un-clipped
   // vert.
-  VertItr Loop(VertItr first, std::function<void(VertItr)> func) {
+  VertItrC Loop(VertItr first, std::function<void(VertItr)> func) const {
     VertItr v = first;
     do {
       if (Clipped(v)) {
@@ -500,7 +516,7 @@ class EarClip {
 
   // Remove this vert from the circular list and output a corresponding
   // triangle.
-  void ClipEar(VertItr ear) {
+  void ClipEar(VertItrC ear) {
     Link(ear->left, ear->right);
     if (ear->left->mesh_idx != ear->mesh_idx &&
         ear->mesh_idx != ear->right->mesh_idx &&
@@ -545,22 +561,19 @@ class EarClip {
   // Build the circular list polygon structures.
   std::vector<VertItr> Initialize(const PolygonsIdx &polys) {
     std::vector<VertItr> starts;
-    float bound = 0;
     for (const SimplePolygonIdx &poly : polys) {
       auto vert = poly.begin();
       polygon_.push_back({vert->idx, 0.0f, earsQueue_.end(), vert->pos});
       const VertItr first = std::prev(polygon_.end());
 
-      bound = glm::max(
-          bound, glm::max(std::abs(first->pos.x), std::abs(first->pos.y)));
+      bBox_.Union(first->pos);
       VertItr last = first;
       // This is not the real rightmost start, but just an arbitrary vert for
       // now to identify each polygon.
       starts.push_back(first);
 
       for (++vert; vert != poly.end(); ++vert) {
-        bound = glm::max(
-            bound, glm::max(std::abs(vert->pos.x), std::abs(vert->pos.y)));
+        bBox_.Union(vert->pos);
 
         polygon_.push_back({vert->idx, 0.0f, earsQueue_.end(), vert->pos});
         VertItr next = std::prev(polygon_.end());
@@ -571,7 +584,7 @@ class EarClip {
       Link(last, first);
     }
 
-    if (precision_ < 0) precision_ = bound * kTolerance;
+    if (precision_ < 0) precision_ = bBox_.Scale() * kTolerance;
 
     // Slightly more than enough, since each hole can cause two extra triangles.
     triangles_.reserve(polygon_.size() + 2 * starts.size());
@@ -730,7 +743,7 @@ class EarClip {
 
   // Recalculate the cost of the Vert v ear, updating it in the queue by
   // removing and reinserting it.
-  void ProcessEar(VertItr v) {
+  void ProcessEar(VertItr v, const IdxCollider &collider) {
     if (v->ear != earsQueue_.end()) {
       earsQueue_.erase(v->ear);
       v->ear = earsQueue_.end();
@@ -739,27 +752,65 @@ class EarClip {
       v->cost = kBest;
       v->ear = earsQueue_.insert(v);
     } else if (v->IsConvex(precision_)) {
-      v->cost = v->EarCost(precision_);
+      v->cost = v->EarCost(precision_, collider);
       v->ear = earsQueue_.insert(v);
     }
   }
 
+  // Create a collider of all vertices in this polygon, each expanded by
+  // precision_. Each ear uses this BVH to quickly find a subset of vertices to
+  // check for cost.
+  IdxCollider VertCollider(VertItr start) const {
+    Vec<Box> vertBox;
+    Vec<uint32_t> vertMorton;
+    Vec<VertItr> itr;
+    const Box box(glm::vec3(bBox_.min, 0), glm::vec3(bBox_.max, 0));
+
+    Loop(start, [&vertBox, &vertMorton, &itr, &box, this](VertItr v) {
+      itr.push_back(v);
+      const glm::vec3 pos(v->pos, 0);
+      vertBox.push_back({pos - precision_, pos + precision_});
+      vertMorton.push_back(Collider::MortonCode(pos, box));
+    });
+
+    if (itr.empty()) {
+      return {Collider(), itr};
+    }
+
+    stable_sort(autoPolicy(itr.size()),
+                zip(vertMorton.begin(), vertBox.begin(), itr.begin()),
+                zip(vertMorton.end(), vertBox.end(), itr.end()),
+                [](const thrust::tuple<uint32_t, Box, VertItr> &a,
+                   const thrust::tuple<uint32_t, Box, VertItr> &b) {
+                  return thrust::get<0>(a) < thrust::get<0>(b);
+                });
+
+    return {Collider(vertBox, vertMorton), itr};
+  }
+
   // The main ear-clipping loop. This is called once for each simple polygon -
   // all holes have already been key-holed and joined to an outer polygon.
   void TriangulatePoly(VertItr start) {
     ZoneScoped;
 
+    const IdxCollider vertCollider = VertCollider(start);
+
+    if (vertCollider.itr.empty()) {
+      PRINT("Empty poly");
+      return;
+    }
+
     // A simple polygon always creates two fewer triangles than it has verts.
     int numTri = -2;
     earsQueue_.clear();
 
     auto QueueVert = [&](VertItr v) {
-      ProcessEar(v);
+      ProcessEar(v, vertCollider);
       ++numTri;
       v->PrintVert();
     };
 
-    VertItr v = Loop(start, QueueVert);
+    VertItrC v = Loop(start, QueueVert);
     if (v == polygon_.end()) return;
     Dump(v);
 
@@ -777,8 +828,8 @@ class EarClip {
       ClipEar(v);
       --numTri;
 
-      ProcessEar(v->left);
-      ProcessEar(v->right);
+      ProcessEar(v->left, vertCollider);
+      ProcessEar(v->right, vertCollider);
       // This is a backup vert that is used if the queue is empty (geometrically
       // invalid polygon), to ensure manifoldness.
       v = v->right;
@@ -788,10 +839,10 @@ class EarClip {
     PRINT("Finished poly");
   }
 
-  void Dump(VertItr start) const {
+  void Dump(VertItrC start) const {
 #ifdef MANIFOLD_DEBUG
     if (!params.verbose) return;
-    VertItr v = start;
+    VertItrC v = start;
     std::cout << "show(array([" << std::endl;
     do {
       std::cout << "  [" << v->pos.x << ", " << v->pos.y << "],# "