polytri.cpp

来自「算断裂的」· C++ 代码 · 共 865 行 · 第 1/2 页

  vector<Real> slope_d;
  int numv = vtxlist.size();
  Triangulation tri;

  // Main loop: generate a triangle for each edge in the workstack.

  for (int eloop = 0; eloop < constraint.size() * 2; ++eloop) {
    int edgeind1 = eloop / 2;
    Edge e = constraint[edgeind1];
    if (eloop & 1) {
      if (top_done[edgeind1])
        continue;
      top_done[edgeind1] = true;
    }
    else {
      if (bottom_done[edgeind1])
        continue;
      bottom_done[edgeind1] = true;
      e = e.reverse();
    }

#ifdef POLYTRILOG
    *debug_str << "\n\n+++Main loop processing edge "<< e << endl;
#endif
    int nume = constraint.size();

    // Find a tranformation so that the two endpoints of the edge are at (-1,0) and (1,0).

    VertexIndex startv = e.endpoint(0);
    VertexIndex endv = e.endpoint(1);
    Real stx = vtxlist.coord(startv, 0);
    Real sty = vtxlist.coord(startv, 1);
    Real enx = vtxlist.coord(endv, 0);
    Real eny = vtxlist.coord(endv, 1);

    Real xdisp = enx - stx;
    Real ydisp = eny - sty;
    Real elength2 = xdisp * xdisp + ydisp * ydisp;

    if (elength2 == 0) continue;
    Real mult = 2.0 / elength2;

    transf(0,0) = xdisp * mult;
    transf(0,1) = ydisp * mult;
    transf(1,0) = -ydisp * mult;
    transf(1,1) = xdisp * mult;
    Real offsetx = (stx + enx) * 0.5;
    Real offsety = (sty + eny) * 0.5;

    tvtxlist.resize(0);
    {
      for (int j = 0; j < numv; ++j) {
        Real tmpx = vtxlist.coord(j,0) - offsetx;
        Real tmpy = vtxlist.coord(j,1) - offsety;
        Real newx = transf(0,0) * tmpx + transf(0,1) * tmpy;
        Real newy = transf(1,0) * tmpx + transf(1,1) * tmpy;
        if (j == startv || j == endv) {
          newy = 0.0;
          if (j == startv)
            newx = -1.0;
          else
            newx = 1.0;
        }
#ifdef POLYTRILOG
        *debug_str << " tvtx new vertex v" << j << "/ at "
          << newx << "," << newy << endl;
#endif
        tvtxlist.add_vertex(newx, newy);
      }
    }

    // For each vertex, compute its p-value and insert it into the
    // queue.

    vtxqueue.resize(0, null_labeled_dist);
    {
      for (int j = 0; j < numv; ++j) {
        Real y = tvtxlist.coord(j,1);
        if (y <= 0) continue;
        Real x = tvtxlist.coord(j,0);
        Real p = (x * x + y * y - 1) / (2 * y);
#ifdef POLYTRILOG
        *debug_str << " new vtxqueue entry v" << j << "/ with p = " << p << endl;
#endif
        vtxqueue.push_back(Labeled_dist(p,j));
      }
    }

    // For each constraint edge, compute its p-value and insert it into
    // the queue if its tangent point is in the segment's
    // relative interior.

    edgequeue.resize(0, null_labeled_dist);    
    tan_point_x.resize(nume);
    tan_point_y.resize(nume);
    slope_c.resize(nume);
    slope_d.resize(nume);
    {
      for (int j = 0; j < nume; ++j) {
#ifdef POLYTRILOG
        *debug_str << "Considering constraint " << constraint[j] << endl;
#endif
        VertexIndex startv1 = constraint[j].endpoint(0);
        VertexIndex endv1 = constraint[j].endpoint(1);
        if (startv == startv1 && endv == endv1) continue;
        if (startv == endv1 && endv == startv1) continue;
        Real stx1 = tvtxlist.coord(startv1,0);
        Real sty1 = tvtxlist.coord(startv1,1);
        Real enx1 = tvtxlist.coord(endv1,0);
        Real eny1 = tvtxlist.coord(endv1,1);
        if (startv == startv1 && eny1 <= 0.0) continue;
        if (endv == startv1 && eny1 <= 0.0) continue;
        if (startv == endv1 && sty1 <= 0.0) continue;
        if (endv == endv1 && sty1 <= 0.0) continue;
        smalleq(0,0) = stx1;
        smalleq(0,1) = sty1;
        smalleq(1,0) = enx1;
        smalleq(1,1) = eny1;
        smallrhs[0] = 1;
        smallrhs[1] = 1;
        Real condest = smalleq.linear_solve(smallrhs, smallsoln);
        if (condest == BIG_REAL) continue;
        Real c = smallsoln[0];
        Real d = smallsoln[1];
#ifdef POLYTRILOG
        *debug_str << " for this constraint, c, d = " << c << ',' <<  d << endl;
#endif
        Real cdlen = c * c + d * d;
        Real qa = c * c;
        Real qb = 2.0 * d;
        Real qc = cdlen - 1.0;
        Real discr;
        Real ynum;

        if (startv == startv1 || startv == endv1 || 
          endv == startv1 || endv == endv1) {
          discr = 0.0;
          ynum = 0;
        }
        else {
          ynum = 1.0 - c * c;
          discr = 4.0 * cdlen * ynum;
        }
#ifdef POLYTRILOG
        *debug_str << " for this constraint, ynum = " << ynum << endl;
#endif
        if (ynum < 0.0) continue;
        Real p;
        // numerically stable quadratic formula

        if (c == 0.0 && d < 0.0) continue;

        if (d <= 0.0)
          p = (-qb + sqrt(discr)) / (2 * qa);
        else
          p = 2.0 * qc / (-qb - sqrt(discr));
#ifdef POLYTRILOG
        *debug_str << " for this constraint p = " << p << endl;
#endif

        // compute y coord
        Real y = sqrt(ynum / cdlen);
        // Numerically stable way to recover x
        Real x;
        if (startv == startv1 || startv == endv1)
          x = -1.0;
        else if (endv == startv1 || endv == endv1)
          x = 1.0;
        else if (fabs(d) > fabs(c))
          x = (y - p) * c / d;
        else
          x = (1.0 - d * y) / c;

#ifdef POLYTRILOG
        *debug_str << " for this constraint tan point = " << x << ',' << y << " versus endpoints "
          << stx1 << ',' << sty1 << " and " << enx1 << ',' << eny1 << endl;
#endif

        //Check whether (x,y) is actually in the segment.

        if ((x - stx1) * (x - enx1) + (y - sty1) * (y - eny1) > 0) continue;

        // Put it in the queue.
        tan_point_x[j] = x;
        tan_point_y[j] = y;
        slope_c[j] = c;
        slope_d[j] = d;
        edgequeue.push_back(Labeled_dist(p,j));
#ifdef POLYTRILOG
        *debug_str << "new edgequeue entry " << constraint[j] << " tangent at point "
          << tan_point_x[j] << "," << tan_point_y[j] << " slope = " << slope_c[j] << ','
          << slope_d[j] << " p = " << p << endl;
#endif

      }
    }

    // Sort the two queues.
    
    sort(edgequeue.begin(), edgequeue.end());
    sort(vtxqueue.begin(), vtxqueue.end());
    
    // Now start growing the circle until we hit something.
    
    int vqptr = 0;
    int eqptr = 0;
    
    typedef multimap<Real,int> Front_type;
    typedef Front_type::iterator FTI;
    Front_type front;
#ifdef POLYTRILOG
    *debug_str << " \nbeginning front search " << endl;
#endif

    for (;;) {
      if (vqptr >= vtxqueue.size())
        throw_error("Unexpected end of vertex queue in polytri");
      
      // Check if the next thing to add is a vertex. 
      
      bool hit_on_vertex;
      Real x,y,p;
      VertexIndex thisvtxind;
      int thisedgeind;
      
      if (eqptr >= edgequeue.size() || 
        edgequeue[eqptr].dist >= vtxqueue[vqptr].dist) {
        hit_on_vertex = true;
        thisvtxind = vtxqueue[vqptr].label;
        p = vtxqueue[vqptr].dist;
        x = tvtxlist.coord(thisvtxind,0);
        y = tvtxlist.coord(thisvtxind,1);
#ifdef POLYTRILOG
        *debug_str << " processing front vertex v" << thisvtxind << "/ at x,y " << x << ','
          << y << " p = " << p << endl;
#endif
      }
      else {
        hit_on_vertex = false;
        thisedgeind = edgequeue[eqptr].label;
        p = edgequeue[eqptr].dist;
        x = tan_point_x[thisedgeind];
        y = tan_point_y[thisedgeind];
#ifdef POLYTRILOG
        *debug_str << " processing front edge" << constraint[thisedgeind] << " at x,y " << x << ','
          << y << " p = " << p << endl;
#endif
      }
      
      // Check if the point x,y is hidden by the front.
      // Find the angle that (x,y) makes w.r.t the center
      // of the circle at (0,p).
      
      Real ang = atan2(y - p,x);
#ifdef POLYTRILOG
      *debug_str << " ang = " << ang << endl;
#endif
      int checkcount;
      int checklist[2];
      if (front.size() == 0)
        checkcount = 0;
      else {
        FTI next = front.lower_bound(ang);
        FTI prev;
        if (next == front.begin()) {
          prev = front.end();
          advance(prev, -1);
        }
        else {
          prev = next;
          advance(prev, -1);
        }
        if (next == front.end()) {
          next = front.begin();
        }
        if (prev == next) {
          checkcount = 1;
          checklist[0] = prev -> second;
        }
        else {
          checkcount = 2;
          checklist[0] = prev -> second;
          checklist[1] = next -> second;
        }
      }
      bool hidden = false;
      for (int checki = 0; checki < checkcount; ++checki) {
        // Determine if (x,y) is hidden.
        // If the test point is a vertex of the edge we
        // are checking, then it is not hidden.
        
        int cedgeind = checklist[checki];
        if (hit_on_vertex &&
          (constraint[cedgeind].endpoint(0) == thisvtxind ||
          constraint[cedgeind].endpoint(1) == thisvtxind))
          continue;
#ifdef POLYTRILOG
        *debug_str << " checking against edge " << constraint[cedgeind] << endl;
#endif
        // Intersect the segment from the new point to the origin
        //  with the support line of the constraint.
        
        smalleq(0,0) = slope_c[cedgeind];
        smalleq(0,1) = slope_d[cedgeind];
        smallrhs[0] = 1.0;
        smalleq(1,0) = y;
        smalleq(1,1) = -x;
        smallrhs[1] = 0;
        Real condest = smalleq.linear_solve(smallrhs,smallsoln);
        if (condest > 1e16) continue;
#ifdef POLYTRILOG
        *debug_str << " smallsoln = " << smallsoln[0] <<',' << smallsoln[1]
          << " y = " << y << endl;
#endif
        if (fabs(y) > fabs(x)) {
          if (smallsoln[1] >= 0 && smallsoln[1] <= y && smallsoln[0]) {
            hidden = true;
          }
        }
        else {
          if (fabs(smallsoln[0]) <= fabs(x) && smallsoln[0] * x >= 0) {
            hidden = true;
          }
        }
        if (hidden) break;
      }
#ifdef POLYTRILOG
      *debug_str << "hidden = " << hidden << endl;
#endif
      if (hit_on_vertex) {
        if (!hidden) {
          // The hit is on a vertex, and the vertex is not hidden,
          // then we have found the sought-after triangle and we
          // can emit it.
          tri.add_triangle(startv, endv, thisvtxind);
#ifdef POLYTRILOG
          *debug_str << " adding triangle v" << startv << "/ v" << endv << "/ v"
            << thisvtxind << "/" << endl;
#endif
          // Add two new constraint edges, if not already present.
          for (int k = 0; k < 2; ++k) {
            VertexIndex ep1 = (k == 0)? endv : thisvtxind;
            VertexIndex ep2 = (k == 0)? thisvtxind : startv;
            Edge enew(ep1, ep2);
            if (checkedge.find(enew.sort()) == checkedge.end()) {
              int sz = constraint.size();
              constraint.push_back(enew);
              top_done.push_back(true);
              bottom_done.push_back(false);
              checkedge[enew.sort()] = sz;
#ifdef POLYTRILOG
              *debug_str << " adding constraint edge " << enew << " at position " << sz << endl;
#endif
            }
            else {
              int j1 = checkedge[enew.sort()];
              if (ep1 == constraint[j1].endpoint(0)) {
                if (top_done[j1]) 
                  throw_error("Top of edge done twice?");
                top_done[j1] = true;
              }
              else {
                if (bottom_done[j1])
                  throw_error("Bottom of edge done twice?");
                bottom_done[j1] = true;
              }
            }
          }
          break;  //end of front;
        }
        else { // hidden
          ++vqptr;
        }
      } 
      else { // hit is on edge
        if (!hidden) {
          // Add it to the front.
#ifdef POLYTRILOG
          *debug_str << " adding edge " << constraint[thisedgeind] << " to front" << endl;
#endif
          front.insert(pair<Real,int>(ang,thisedgeind));
        }
        ++eqptr;
      }
    } // Done with finding triangle 
  } // End of main work loop.
  return tri;
}






// ------------------------------------------------------------------
// poly_cdt computes the CDT of a polygon.  It first computes the
// CDT of the corresponding PSLG, then it calls searchtri to 
// get rid of unused triangles.

QMG::PolyTri::Triangulation 
QMG::PolyTri::poly_cdt(const Matrix& vtxlist, 
                       const vector<Edge>& elist,
                       double tol) {
  VertexList vl;
  make_2d_vertices(vtxlist, vl, tol);
  Triangulation init_tri = pslg_constrained_delaunay_triangulate(vl, elist);
  int nt = init_tri.numtri();
  vector<Triangle_Label> disposition(nt);
  {
    for (int k = 0; k < nt; ++k) {
      disposition[k] = NOT_YET_SEARCHED;
      for (int l = 0; l < 3; ++l) {
        if (init_tri.tri_vertex(k,l) >= vtxlist.numrows())
          disposition[k] = DELETE;
      }
    }
  }
  search_tri(elist, init_tri, disposition);
  Triangulation result;
  {
    for (int k = 0; k < nt; ++k) {
      if (disposition[k] == KEEP) {
        result.add_triangle(init_tri.tri_vertex(k,0), 
          init_tri.tri_vertex(k,1),
          init_tri.tri_vertex(k,2));
      }
    }
  }
  return result;
}