From dd70e94143c56ef1c4bc5f1e9b4a8e5c681aeb2a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joachim=20Sch=C3=B6berl?= <joachim.schoeberl@tuwien.ac.at>
Date: Mon, 30 Sep 2019 18:14:46 +0200
Subject: [PATCH] searchtree in STLCharts

---
 libsrc/stlgeom/stlgeomchart.cpp |  4 +++
 libsrc/stlgeom/stlgeommesh.cpp  | 13 +++++--
 libsrc/stlgeom/stltool.cpp      | 61 +++++++++++++++++++++++++++++++++
 libsrc/stlgeom/stltool.hpp      |  3 ++
 libsrc/stlgeom/stltopology.hpp  |  2 ++
 5 files changed, 81 insertions(+), 2 deletions(-)

diff --git a/libsrc/stlgeom/stlgeomchart.cpp b/libsrc/stlgeom/stlgeomchart.cpp
index ca78b86a..972a6fa5 100644
--- a/libsrc/stlgeom/stlgeomchart.cpp
+++ b/libsrc/stlgeom/stlgeomchart.cpp
@@ -600,6 +600,10 @@ void STLGeometry :: MakeAtlas(Mesh & mesh, const MeshingParameters& mparam, cons
   
   PrintMessage(5,"Make Atlas finished");
 
+  
+  for (auto & chart : atlas)
+    chart->BuildInnerSearchTree();
+
   PopStatus();
 }
 
diff --git a/libsrc/stlgeom/stlgeommesh.cpp b/libsrc/stlgeom/stlgeommesh.cpp
index 5d47453a..ca1552e2 100644
--- a/libsrc/stlgeom/stlgeommesh.cpp
+++ b/libsrc/stlgeom/stlgeommesh.cpp
@@ -544,6 +544,13 @@ int STLGeometry :: Project(Point<3> & p3d) const
 
   const STLChart& chart = GetChart(meshchart);
 
+  STLTrigId trig = chart.ProjectNormal(p3d);
+  return trig;
+  // cout << "new, trig = " << trig << endl;
+
+  // #define MOVEDTOCHART
+#ifdef MOVEDTOCHART
+  
   int nt = chart.GetNT();
 
    QuadraticFunction3d quadfun(p3d, meshtrignv);
@@ -599,10 +606,12 @@ int STLGeometry :: Project(Point<3> & p3d) const
 	}
 
       if (inside)
-	break;
-
+        break;
     }
+#endif
 
+  // cout << "oldtrig = " << fi << endl;
+  
   //  if (cnt == 2) {(*testout) << "WARNING: found 2 triangles to project" << endl;}
   //if (cnt == 3) {(*testout) << "WARNING: found 3 triangles to project" << endl;}
   //if (cnt > 3) {(*testout) << "WARNING: found more than 3 triangles to project" << endl;}
diff --git a/libsrc/stlgeom/stltool.cpp b/libsrc/stlgeom/stltool.cpp
index bfaa7a13..c04b06b9 100644
--- a/libsrc/stlgeom/stltool.cpp
+++ b/libsrc/stlgeom/stltool.cpp
@@ -795,6 +795,67 @@ void STLChart :: SetNormal (const Point<3> & apref, const Vec<3> & anormal)
   t2 = Cross (normal, t1);
 }
 
+void STLChart :: BuildInnerSearchTree()
+{
+  Box<2> chart_bbox(Box<2>::EMPTY_BOX);
+  for (STLTrigId trigid : charttrigs)
+    {
+      for (STLPointId pi : (*geometry)[trigid].PNums())
+        {
+          Point<3> p = (*geometry)[pi];
+          Point<2> p2d = Project2d(p);
+          chart_bbox.Add(p2d);
+        }
+    }
+  chart_bbox.Increase (1e-2*chart_bbox.Diam());
+  inner_searchtree = make_unique<BoxTree<2,int>> (chart_bbox);
+  for (STLTrigId trigid : charttrigs)
+    {
+      Box<2> bbox(Box<2>::EMPTY_BOX);      
+      for (STLPointId pi : (*geometry)[trigid].PNums())
+        {
+          Point<3> p = (*geometry)[pi];
+          Point<2> p2d = Project2d(p);
+          bbox.Add(p2d);
+        }
+      inner_searchtree->Insert (bbox, trigid);
+    }
+}
+
+STLTrigId STLChart :: ProjectNormal (Point<3> & p3d) const
+{
+  
+  int nt = GetNT();
+  double lamtol = 1e-6;
+  QuadraticFunction3d quadfun(p3d, GetNormal());
+ 
+  for (int j = 1; j <= nt; j++)
+    {
+      STLTrigId i = GetTrig1(j);
+      auto & trig = geometry->GetTriangle(i);
+      const Point<3> & c = trig.center;
+
+      if (quadfun.Eval(c) > sqr (trig.rad))
+	continue;
+
+      Point<3> p = p3d;
+      Vec<3> lam;
+      int err = trig.ProjectInPlain(geometry->GetPoints(), GetNormal(), p, lam);      
+      bool inside = (err == 0 && lam(0) > -lamtol && 
+                     lam(1) > -lamtol && (1-lam(0)-lam(1)) > -lamtol);
+
+      if (inside)
+        {
+          p3d = p;
+          return i;
+        }
+    }
+
+  return 0;
+}
+
+
+
 /*
 Point<2> STLChart :: Project2d (const Point<3> & p3d) const
 {
diff --git a/libsrc/stlgeom/stltool.hpp b/libsrc/stlgeom/stltool.hpp
index 01ffd34d..8472f395 100644
--- a/libsrc/stlgeom/stltool.hpp
+++ b/libsrc/stlgeom/stltool.hpp
@@ -125,6 +125,7 @@ private:
   Vec<3> normal;
   Point<3> pref;
   Vec<3> t1, t2;
+  unique_ptr<BoxTree<2,int>> inner_searchtree;
 public:
   void SetNormal (const Point<3> & apref, const Vec<3> & anormal);
   const Vec<3> & GetNormal () const { return normal; }
@@ -133,6 +134,8 @@ public:
     Vec<3> v = p3d-pref;
     return Point<2> (t1 * v, t2 * v);
   }
+  void BuildInnerSearchTree();
+  STLTrigId ProjectNormal (Point<3> & p) const;
 };
 
 class STLBoundarySeg
diff --git a/libsrc/stlgeom/stltopology.hpp b/libsrc/stlgeom/stltopology.hpp
index 3e98535d..2cd65427 100644
--- a/libsrc/stlgeom/stltopology.hpp
+++ b/libsrc/stlgeom/stltopology.hpp
@@ -153,7 +153,9 @@ public:
   STLPointId & PNum(int i) { return pts[(i-1)]; }
   STLPointId PNumMod(int i) const { return pts[(i-1)%3]; }
   STLPointId & PNumMod(int i)  { return pts[(i-1)%3]; }
+  FlatArray<const STLPointId> PNums() const { return { 3, pts }; }
 
+  
   int EdgeNumMod(int i) const { return topedges[(i-1)%3]; }
   int & EdgeNumMod(int i)  { return topedges[(i-1)%3]; }