169e5a33805cdc9f64e076dd96c59564277f5546
[blender.git] / extern / carve / lib / triangulator.cpp
1 // Begin License:
2 // Copyright (C) 2006-2011 Tobias Sargeant (tobias.sargeant@gmail.com).
3 // All rights reserved.
4 //
5 // This file is part of the Carve CSG Library (http://carve-csg.com/)
6 //
7 // This file may be used under the terms of the GNU General Public
8 // License version 2.0 as published by the Free Software Foundation
9 // and appearing in the file LICENSE.GPL2 included in the packaging of
10 // this file.
11 //
12 // This file is provided "AS IS" with NO WARRANTY OF ANY KIND,
13 // INCLUDING THE WARRANTIES OF DESIGN, MERCHANTABILITY AND FITNESS FOR
14 // A PARTICULAR PURPOSE.
15 // End:
16
17
18 #if defined(HAVE_CONFIG_H)
19 #  include <carve_config.h>
20 #endif
21
22 #include <carve/csg.hpp>
23 #include <carve/triangulator.hpp>
24
25 #include <fstream>
26 #include <sstream>
27
28 #include <algorithm>
29
30
31 namespace {
32   // private code related to hole patching.
33
34   class order_h_loops_2d {
35     order_h_loops_2d &operator=(const order_h_loops_2d &);
36
37     const std::vector<std::vector<carve::geom2d::P2> > &poly;
38     int axis;
39     public:
40
41     order_h_loops_2d(const std::vector<std::vector<carve::geom2d::P2> > &_poly, int _axis) :
42       poly(_poly), axis(_axis) {
43       }
44
45     bool operator()(const std::pair<size_t, size_t> &a,
46         const std::pair<size_t, size_t> &b) const {
47       return carve::triangulate::detail::axisOrdering(poly[a.first][a.second], poly[b.first][b.second], axis);
48     }
49   };
50
51   class heap_ordering_2d {
52     heap_ordering_2d &operator=(const heap_ordering_2d &);
53
54     const std::vector<std::vector<carve::geom2d::P2> > &poly;
55     const std::vector<std::pair<size_t, size_t> > &loop;
56     const carve::geom2d::P2 p;
57     int axis;
58
59     public:
60
61     heap_ordering_2d(const std::vector<std::vector<carve::geom2d::P2> > &_poly,
62         const std::vector<std::pair<size_t, size_t> > &_loop,
63         const carve::geom2d::P2 _p,
64         int _axis) : poly(_poly), loop(_loop), p(_p), axis(_axis) {
65     }
66
67     bool operator()(size_t a, size_t b) const {
68       double da = carve::geom::distance2(p, poly[loop[a].first][loop[a].second]);
69       double db = carve::geom::distance2(p, poly[loop[b].first][loop[b].second]);
70       if (da > db) return true;
71       if (da < db) return false;
72       return carve::triangulate::detail::axisOrdering(poly[loop[a].first][loop[a].second], poly[loop[b].first][loop[b].second], axis);
73     }
74   };
75
76   static inline void patchHoleIntoPolygon_2d(std::vector<std::pair<size_t, size_t> > &f_loop,
77       size_t f_loop_attach,
78       size_t h_loop,
79       size_t h_loop_attach,
80       size_t h_loop_size) {
81     f_loop.insert(f_loop.begin() + f_loop_attach + 1, h_loop_size + 2, std::make_pair(h_loop, 0));
82     size_t f = f_loop_attach + 1;
83
84     for (size_t h = h_loop_attach; h != h_loop_size; ++h) {
85       f_loop[f++].second = h;
86     }
87
88     for (size_t h = 0; h <= h_loop_attach; ++h) {
89       f_loop[f++].second = h;
90     }
91
92     f_loop[f] = f_loop[f_loop_attach];
93   }
94
95   static inline const carve::geom2d::P2 &pvert(const std::vector<std::vector<carve::geom2d::P2> > &poly, const std::pair<size_t, size_t> &idx) {
96     return poly[idx.first][idx.second];
97   }
98 }
99
100
101 namespace {
102   // private code related to triangulation.
103
104   using carve::triangulate::detail::vertex_info;
105
106   struct vertex_info_ordering {
107     bool operator()(const vertex_info *a, const vertex_info *b) const {
108       return a->score < b->score;
109     }
110   };
111
112   struct vertex_info_l2norm_inc_ordering {
113     const vertex_info *v;
114     vertex_info_l2norm_inc_ordering(const vertex_info *_v) : v(_v) {
115     }
116     bool operator()(const vertex_info *a, const vertex_info *b) const {
117       return carve::geom::distance2(v->p, a->p) > carve::geom::distance2(v->p, b->p);
118     }
119   };
120
121   class EarQueue {
122     std::vector<vertex_info *> queue;
123
124     void checkheap() {
125 #ifdef __GNUC__
126       CARVE_ASSERT(std::__is_heap(queue.begin(), queue.end(), vertex_info_ordering()));
127 #endif
128     }
129
130   public:
131     EarQueue() {
132     }
133
134     size_t size() const {
135       return queue.size();
136     }
137
138     void push(vertex_info *v) {
139 #if defined(CARVE_DEBUG)
140       checkheap();
141 #endif
142       queue.push_back(v);
143       std::push_heap(queue.begin(), queue.end(), vertex_info_ordering());
144     }
145
146     vertex_info *pop() {
147 #if defined(CARVE_DEBUG)
148       checkheap();
149 #endif
150       std::pop_heap(queue.begin(), queue.end(), vertex_info_ordering());
151       vertex_info *v = queue.back();
152       queue.pop_back();
153       return v;
154     }
155
156     void remove(vertex_info *v) {
157 #if defined(CARVE_DEBUG)
158       checkheap();
159 #endif
160       CARVE_ASSERT(std::find(queue.begin(), queue.end(), v) != queue.end());
161       double score = v->score;
162       if (v != queue[0]) {
163         v->score = queue[0]->score + 1;
164         std::make_heap(queue.begin(), queue.end(), vertex_info_ordering());
165       }
166       CARVE_ASSERT(v == queue[0]);
167       std::pop_heap(queue.begin(), queue.end(), vertex_info_ordering());
168       CARVE_ASSERT(queue.back() == v);
169       queue.pop_back();
170       v->score = score;
171     }
172
173     void changeScore(vertex_info *v, double score) {
174 #if defined(CARVE_DEBUG)
175       checkheap();
176 #endif
177       CARVE_ASSERT(std::find(queue.begin(), queue.end(), v) != queue.end());
178       if (v->score != score) {
179         v->score = score;
180         std::make_heap(queue.begin(), queue.end(), vertex_info_ordering());
181       }
182     }
183
184     // 39% of execution time
185     void updateVertex(vertex_info *v) {
186       double spre = v->score;
187       bool qpre = v->isCandidate();
188       v->recompute();
189       bool qpost = v->isCandidate();
190       double spost = v->score;
191
192       v->score = spre;
193
194       if (qpre) {
195         if (qpost) {
196           if (v->score != spre) {
197             changeScore(v, spost);
198           }
199         } else {
200           remove(v);
201         }
202       } else {
203         if (qpost) {
204           push(v);
205         }
206       }
207     }
208   };
209
210
211
212   int windingNumber(vertex_info *begin, const carve::geom2d::P2 &point) {
213     int wn = 0;
214
215     vertex_info *v = begin;
216     do {
217       if (v->p.y <= point.y) {
218         if (v->next->p.y > point.y && carve::geom2d::orient2d(v->p, v->next->p, point) > 0.0) {
219           ++wn;
220         }
221       } else {
222         if (v->next->p.y <= point.y && carve::geom2d::orient2d(v->p, v->next->p, point) < 0.0) {
223           --wn;
224         }
225       }
226       v = v->next;
227     } while (v != begin);
228
229     return wn;
230   }
231
232
233
234   bool internalToAngle(const vertex_info *a,
235                        const vertex_info *b,
236                        const vertex_info *c,
237                        const carve::geom2d::P2 &p) {
238     return carve::geom2d::internalToAngle(a->p, b->p, c->p, p);
239   }
240
241
242
243   bool findDiagonal(vertex_info *begin, vertex_info *&v1, vertex_info *&v2) {
244     vertex_info *t;
245     std::vector<vertex_info *> heap;
246
247     v1 = begin;
248     do {
249       heap.clear();
250
251       for (v2 = v1->next->next; v2 != v1->prev; v2 = v2->next) {
252         if (!internalToAngle(v1->next, v1, v1->prev, v2->p) ||
253             !internalToAngle(v2->next, v2, v2->prev, v1->p)) continue;
254
255         heap.push_back(v2);
256         std::push_heap(heap.begin(), heap.end(), vertex_info_l2norm_inc_ordering(v1));
257       }
258
259       while (heap.size()) {
260         std::pop_heap(heap.begin(), heap.end(), vertex_info_l2norm_inc_ordering(v1));
261         v2 = heap.back(); heap.pop_back();
262
263 #if defined(CARVE_DEBUG)
264         std::cerr << "testing: " << v1 << " - " << v2 << std::endl;
265         std::cerr << "  length = " << (v2->p - v1->p).length() << std::endl;
266         std::cerr << "  pos: " << v1->p << " - " << v2->p << std::endl;
267 #endif
268         // test whether v1-v2 is a valid diagonal.
269         double v_min_x = std::min(v1->p.x, v2->p.x);
270         double v_max_x = std::max(v1->p.x, v2->p.x);
271
272         bool intersected = false;
273
274         for (t = v1->next; !intersected && t != v1->prev; t = t->next) {
275           vertex_info *u = t->next;
276           if (t == v2 || u == v2) continue;
277
278           double l1 = carve::geom2d::orient2d(v1->p, v2->p, t->p);
279           double l2 = carve::geom2d::orient2d(v1->p, v2->p, u->p);
280
281           if ((l1 > 0.0 && l2 > 0.0) || (l1 < 0.0 && l2 < 0.0)) {
282             // both on the same side; no intersection
283             continue;
284           }
285
286           double dx13 = v1->p.x - t->p.x;
287           double dy13 = v1->p.y - t->p.y;
288           double dx43 = u->p.x - t->p.x;
289           double dy43 = u->p.y - t->p.y;
290           double dx21 = v2->p.x - v1->p.x;
291           double dy21 = v2->p.y - v1->p.y;
292           double ua_n = dx43 * dy13 - dy43 * dx13;
293           double ub_n = dx21 * dy13 - dy21 * dx13;
294           double u_d  = dy43 * dx21 - dx43 * dy21;
295
296           if (carve::math::ZERO(u_d)) {
297             // parallel
298             if (carve::math::ZERO(ua_n)) {
299               // colinear
300               if (std::max(t->p.x, u->p.x) >= v_min_x && std::min(t->p.x, u->p.x) <= v_max_x) {
301                 // colinear and intersecting
302                 intersected = true;
303               }
304             }
305           } else {
306             // not parallel
307             double ua = ua_n / u_d;
308             double ub = ub_n / u_d;
309
310             if (0.0 <= ua && ua <= 1.0 && 0.0 <= ub && ub <= 1.0) {
311               intersected = true;
312             }
313           }
314 #if defined(CARVE_DEBUG)
315           if (intersected) {
316             std::cerr << "  failed on edge: " << t << " - " << u << std::endl;
317             std::cerr << "    pos: " << t->p << " - " << u->p << std::endl;
318           }
319 #endif
320         }
321
322         if (!intersected) {
323           // test whether midpoint winding == 1
324
325           carve::geom2d::P2 mid = (v1->p + v2->p) / 2;
326           if (windingNumber(begin, mid) == 1) {
327             // this diagonal is ok
328             return true;
329           }
330         }
331       }
332
333       // couldn't find a diagonal from v1 that was ok.
334       v1 = v1->next;
335     } while (v1 != begin);
336     return false;
337   }
338
339
340
341 #if defined(CARVE_DEBUG_WRITE_PLY_DATA)
342   void dumpPoly(const std::vector<carve::geom2d::P2> &points,
343       const std::vector<carve::triangulate::tri_idx> &result) {
344     static int step = 0;
345     std::ostringstream filename;
346     filename << "poly_" << step++ << ".svg";
347     std::cerr << "dumping to " << filename.str() << std::endl;
348     std::ofstream out(filename.str().c_str());
349
350     double minx = points[0].x, maxx = points[0].x;
351     double miny = points[0].y, maxy = points[0].y;
352
353     for (size_t i = 1; i < points.size(); ++i) {
354       minx = std::min(points[i].x, minx); maxx = std::max(points[i].x, maxx);
355       miny = std::min(points[i].y, miny); maxy = std::max(points[i].y, maxy);
356     }
357     double scale = 100 / std::max(maxx-minx, maxy-miny);
358
359     maxx *= scale; minx *= scale;
360     maxy *= scale; miny *= scale;
361
362     double width = maxx - minx + 10;
363     double height = maxy - miny + 10;
364
365     out << "\
366 <?xml version=\"1.0\"?>\n\
367 <!DOCTYPE svg PUBLIC \"-//W3C//DTD SVG 1.1//EN\" \"http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd\">\n\
368 <svg xmlns=\"http://www.w3.org/2000/svg\" version=\"1.1\" width=\"" << width << "\" height=\"" << height << "\">\n";
369
370     out << "<polygon fill=\"rgb(0,0,0)\" stroke=\"blue\" stroke-width=\"0.1\" points=\"";
371     for (size_t i = 0; i < points.size(); ++i) {
372       if (i) out << ' ';
373       double x, y;
374       x = scale * (points[i].x) - minx + 5;
375       y = scale * (points[i].y) - miny + 5;
376       out << x << ',' << y;
377     }
378     out << "\" />" << std::endl;
379
380     for (size_t i = 0; i < result.size(); ++i) {
381       out << "<polygon fill=\"rgb(255,255,255)\" stroke=\"black\" stroke-width=\"0.1\" points=\"";
382       double x, y;
383       x = scale * (points[result[i].a].x) - minx + 5;
384       y = scale * (points[result[i].a].y) - miny + 5;
385       out << x << ',' << y << ' ';        
386       x = scale * (points[result[i].b].x) - minx + 5;
387       y = scale * (points[result[i].b].y) - miny + 5;
388       out << x << ',' << y << ' ';        
389       x = scale * (points[result[i].c].x) - minx + 5;
390       y = scale * (points[result[i].c].y) - miny + 5;
391       out << x << ',' << y;
392       out << "\" />" << std::endl;
393     }
394
395     out << "</svg>" << std::endl;
396   }
397 #endif
398 }
399
400
401
402 double carve::triangulate::detail::vertex_info::triScore(const vertex_info *p, const vertex_info *v, const vertex_info *n) {
403
404   // different scoring functions.
405 #if 0
406   bool convex = isLeft(p, v, n);
407   if (!convex) return -1e-5;
408
409   double a1 = carve::geom2d::atan2(p->p - v->p) - carve::geom2d::atan2(n->p - v->p);
410   double a2 = carve::geom2d::atan2(v->p - n->p) - carve::geom2d::atan2(p->p - n->p);
411   if (a1 < 0) a1 += M_PI * 2;
412   if (a2 < 0) a2 += M_PI * 2;
413
414   return std::min(a1, std::min(a2, M_PI - a1 - a2)) / (M_PI / 3);
415 #endif
416
417 #if 1
418   // range: 0 - 1
419   double a, b, c;
420
421   bool convex = isLeft(p, v, n);
422   if (!convex) return -1e-5;
423
424   a = (n->p - v->p).length();
425   b = (p->p - n->p).length();
426   c = (v->p - p->p).length();
427
428   if (a < 1e-10 || b < 1e-10 || c < 1e-10) return 0.0;
429
430   return std::max(std::min((a+b)/c, std::min((a+c)/b, (b+c)/a)) - 1.0, 0.0);
431 #endif
432 }
433
434
435
436 double carve::triangulate::detail::vertex_info::calcScore() const {
437
438 #if 0
439   // examine only this triangle.
440   double this_tri = triScore(prev, this, next);
441   return this_tri;
442 #endif
443
444 #if 1
445   // attempt to look ahead in the neighbourhood to attempt to clip ears that have good neighbours.
446   double this_tri = triScore(prev, this, next);
447   double next_tri = triScore(prev, next, next->next);
448   double prev_tri = triScore(prev->prev, prev, next);
449
450   return this_tri + std::max(next_tri, prev_tri) * .2;
451 #endif
452
453 #if 0
454   // attempt to penalise ears that will require producing a sliver triangle.
455   double score = triScore(prev, this, next);
456
457   double a1, a2;
458   a1 = carve::geom2d::atan2(prev->p - next->p);
459   a2 = carve::geom2d::atan2(next->next->p - next->p);
460   if (fabs(a1 - a2) < 1e-5) score -= .5;
461
462   a1 = carve::geom2d::atan2(next->p - prev->p);
463   a2 = carve::geom2d::atan2(prev->prev->p - prev->p);
464   if (fabs(a1 - a2) < 1e-5) score -= .5;
465
466   return score;
467 #endif
468 }
469
470
471
472 bool carve::triangulate::detail::vertex_info::isClipable() const {
473   for (const vertex_info *v_test = next->next; v_test != prev; v_test = v_test->next) {
474     if (v_test->convex) {
475       continue;
476     }
477
478     if (v_test->p == prev->p ||
479         v_test->p == next->p) {
480       continue;
481     }
482
483     if (v_test->p == p) {
484       if (v_test->next->p == prev->p &&
485           v_test->prev->p == next->p) {
486         return false;
487       }
488       if (v_test->next->p == prev->p ||
489           v_test->prev->p == next->p) {
490         continue;
491       }
492     }
493
494     if (pointInTriangle(prev, this, next, v_test)) {
495       return false;
496     }
497   }
498   return true;
499 }
500
501
502
503 size_t carve::triangulate::detail::removeDegeneracies(vertex_info *&begin, std::vector<carve::triangulate::tri_idx> &result) {
504   vertex_info *v;
505   vertex_info *n;
506   size_t count = 0;
507   size_t remain = 0;
508
509   v = begin;
510   do {
511     v = v->next;
512     ++remain;
513   } while (v != begin);
514
515   v = begin;
516   do {
517     if (remain < 4) break;
518
519     bool remove = false;
520     if (v->p == v->next->p) {
521       remove = true;
522     } else if (v->p == v->next->next->p) {
523       if (v->next->p == v->next->next->next->p) {
524         // a 'z' in the loop: z (a) b a b c -> remove a-b-a -> z (a) a b c -> remove a-a-b (next loop) -> z a b c
525         // z --(a)-- b
526         //         /
527         //        /
528         //      a -- b -- d
529         remove = true;
530       } else {
531         // a 'shard' in the loop: z (a) b a c d -> remove a-b-a -> z (a) a b c d -> remove a-a-b (next loop) -> z a b c d
532         // z --(a)-- b
533         //         /
534         //        /
535         //      a -- c -- d
536         // n.b. can only do this if the shard is pointing out of the polygon. i.e. b is outside z-a-c
537         remove = !internalToAngle(v->next->next->next, v, v->prev, v->next->p);
538       }
539     }
540
541     if (remove) {
542       result.push_back(carve::triangulate::tri_idx(v->idx, v->next->idx, v->next->next->idx));
543       n = v->next;
544       if (n == begin) begin = n->next;
545       n->remove();
546       count++;
547       remain--;
548       delete n;
549     } else {
550       v = v->next;
551     }
552   } while (v != begin);
553   return count;
554 }
555
556
557
558 bool carve::triangulate::detail::splitAndResume(vertex_info *begin, std::vector<carve::triangulate::tri_idx> &result) {
559   vertex_info *v1, *v2;
560
561 #if defined(CARVE_DEBUG_WRITE_PLY_DATA)
562   {
563     std::vector<carve::triangulate::tri_idx> dummy;
564     std::vector<carve::geom2d::P2> dummy_p;
565     vertex_info *v = begin;
566     do {
567       dummy_p.push_back(v->p);
568       v = v->next;
569     } while (v != begin);
570     std::cerr << "input to splitAndResume:" << std::endl;
571     dumpPoly(dummy_p, dummy);
572   }
573 #endif
574
575
576   if (!findDiagonal(begin, v1, v2)) return false;
577
578   vertex_info *v1_copy = new vertex_info(*v1);
579   vertex_info *v2_copy = new vertex_info(*v2);
580
581   v1->next = v2;
582   v2->prev = v1;
583
584   v1_copy->next->prev = v1_copy;
585   v2_copy->prev->next = v2_copy;
586
587   v1_copy->prev = v2_copy;
588   v2_copy->next = v1_copy;
589
590   bool r1 = doTriangulate(v1, result);
591   bool r2 =  doTriangulate(v1_copy, result);
592   return r1 && r2;
593 }
594
595
596
597 bool carve::triangulate::detail::doTriangulate(vertex_info *begin, std::vector<carve::triangulate::tri_idx> &result) {
598 #if defined(CARVE_DEBUG)
599   std::cerr << "entering doTriangulate" << std::endl;
600 #endif
601
602 #if defined(CARVE_DEBUG_WRITE_PLY_DATA)
603   {
604     std::vector<carve::triangulate::tri_idx> dummy;
605     std::vector<carve::geom2d::P2> dummy_p;
606     vertex_info *v = begin;
607     do {
608       dummy_p.push_back(v->p);
609       v = v->next;
610     } while (v != begin);
611     dumpPoly(dummy_p, dummy);
612   }
613 #endif
614
615   EarQueue vq;
616
617   vertex_info *v = begin;
618   size_t remain = 0;
619   do {
620     if (v->isCandidate()) vq.push(v);
621     v = v->next;
622     remain++;
623   } while (v != begin);
624
625 #if defined(CARVE_DEBUG)
626   std::cerr << "remain = " << remain << std::endl;
627 #endif
628
629   while (remain > 3 && vq.size()) {
630     vertex_info *v = vq.pop();
631     if (!v->isClipable()) {
632       v->failed = true;
633       continue;
634     }
635
636   continue_clipping:
637     vertex_info *n = v->next;
638     vertex_info *p = v->prev;
639
640     result.push_back(carve::triangulate::tri_idx(v->prev->idx, v->idx, v->next->idx));
641
642 #if defined(CARVE_DEBUG)
643     {
644       std::vector<carve::geom2d::P2> temp;
645       temp.push_back(v->prev->p);
646       temp.push_back(v->p);
647       temp.push_back(v->next->p);
648       std::cerr << "clip " << v << " idx = " << v->idx << " score = " << v->score << " area = " << carve::geom2d::signedArea(temp) << " " << temp[0] << " " << temp[1] << " " << temp[2] << std::endl;
649     }
650 #endif
651
652     v->remove();
653     if (v == begin) begin = v->next;
654     delete v;
655
656     if (--remain == 3) break;
657
658     vq.updateVertex(n);
659     vq.updateVertex(p);
660
661     if (n->score < p->score) { std::swap(n, p); }
662
663     if (n->score > 0.25 && n->isCandidate() && n->isClipable()) {
664       vq.remove(n);
665       v = n;
666 #if defined(CARVE_DEBUG)
667       std::cerr << " continue clipping (n), score = " << n->score << std::endl;
668 #endif
669       goto continue_clipping;
670     }
671
672     if (p->score > 0.25 && p->isCandidate() && p->isClipable()) {
673       vq.remove(p);
674       v = p;
675 #if defined(CARVE_DEBUG)
676       std::cerr << " continue clipping (p), score = " << n->score << std::endl;
677 #endif
678       goto continue_clipping;
679     }
680
681 #if defined(CARVE_DEBUG)
682     std::cerr << "looking for new start point" << std::endl;
683     std::cerr << "remain = " << remain << std::endl;
684 #endif
685   }
686
687 #if defined(CARVE_DEBUG)
688   std::cerr << "doTriangulate complete; remain=" << remain << std::endl;
689 #endif
690
691   if (remain > 3) {
692 #if defined(CARVE_DEBUG)
693     std::cerr << "before removeDegeneracies: remain=" << remain << std::endl;
694 #endif
695     remain -= removeDegeneracies(begin, result);
696 #if defined(CARVE_DEBUG)
697     std::cerr << "after removeDegeneracies: remain=" << remain << std::endl;
698 #endif
699
700     if (remain > 3) {
701       return splitAndResume(begin, result);
702     }
703   }
704
705   if (remain == 3) {
706     result.push_back(carve::triangulate::tri_idx(begin->idx, begin->next->idx, begin->next->next->idx));
707   }
708
709   vertex_info *d = begin;
710   do {
711     vertex_info *n = d->next;
712     delete d;
713     d = n;
714   } while (d != begin);
715
716   return true;
717 }
718
719
720
721 bool testCandidateAttachment(const std::vector<std::vector<carve::geom2d::P2> > &poly,
722                              std::vector<std::pair<size_t, size_t> > &current_f_loop,
723                              size_t curr,
724                              carve::geom2d::P2 hole_min) {
725   const size_t SZ = current_f_loop.size();
726
727   if (!carve::geom2d::internalToAngle(pvert(poly, current_f_loop[(curr+1) % SZ]),
728                                       pvert(poly, current_f_loop[curr]),
729                                       pvert(poly, current_f_loop[(curr+SZ-1) % SZ]),
730                                       hole_min)) {
731     return false;
732   }
733
734   if (hole_min == pvert(poly, current_f_loop[curr])) {
735     return true;
736   }
737
738   carve::geom2d::LineSegment2 test(hole_min, pvert(poly, current_f_loop[curr]));
739
740   size_t v1 = current_f_loop.size() - 1;
741   size_t v2 = 0;
742   double v1_side = carve::geom2d::orient2d(test.v1, test.v2, pvert(poly, current_f_loop[v1]));
743   double v2_side = 0;
744
745   while (v2 != current_f_loop.size()) {
746     v2_side = carve::geom2d::orient2d(test.v1, test.v2, pvert(poly, current_f_loop[v2]));
747
748     if (v1_side != v2_side) {
749       // XXX: need to test vertices, not indices, because they may
750       // be duplicated.
751       if (pvert(poly, current_f_loop[v1]) != pvert(poly, current_f_loop[curr]) &&
752           pvert(poly, current_f_loop[v2]) != pvert(poly, current_f_loop[curr])) {
753         carve::geom2d::LineSegment2 test2(pvert(poly, current_f_loop[v1]), pvert(poly, current_f_loop[v2]));
754         if (carve::geom2d::lineSegmentIntersection_simple(test, test2)) {
755           // intersection; failed.
756           return false;
757         }
758       }
759     }
760
761     v1 = v2;
762     v1_side = v2_side;
763     ++v2;
764   }
765   return true;
766 }
767
768
769
770 void
771 carve::triangulate::incorporateHolesIntoPolygon(
772     const std::vector<std::vector<carve::geom2d::P2> > &poly,
773     std::vector<std::pair<size_t, size_t> > &result,
774     size_t poly_loop,
775     const std::vector<size_t> &hole_loops) {
776   typedef std::vector<carve::geom2d::P2> loop_t;
777
778   size_t N = poly[poly_loop].size();
779
780   // work out how much space to reserve for the patched in holes.
781   for (size_t i = 0; i < hole_loops.size(); i++) {
782     N += 2 + poly[hole_loops[i]].size();
783   }
784
785   // this is the vector that we will build the result in.
786   result.clear();
787   result.reserve(N);
788
789   // this is a heap of result indices that defines the vertex test order.
790   std::vector<size_t> f_loop_heap;
791   f_loop_heap.reserve(N);
792
793   // add the poly loop to result.
794   for (size_t i = 0; i < poly[poly_loop].size(); ++i) {
795     result.push_back(std::make_pair((size_t)poly_loop, i));
796   }
797
798   if (hole_loops.size() == 0) {
799     return;
800   }
801
802   std::vector<std::pair<size_t, size_t> > h_loop_min_vertex;
803
804   h_loop_min_vertex.reserve(hole_loops.size());
805
806   // find the major axis for the holes - this is the axis that we
807   // will sort on for finding vertices on the polygon to join
808   // holes up to.
809   //
810   // it might also be nice to also look for whether it is better
811   // to sort ascending or descending.
812   // 
813   // another trick that could be used is to modify the projection
814   // by 90 degree rotations or flipping about an axis. just as
815   // long as we keep the carve::geom3d::Vector pointers for the
816   // real data in sync, everything should be ok. then we wouldn't
817   // need to accomodate axes or sort order in the main loop.
818
819   // find the bounding box of all the holes.
820   carve::geom2d::P2 h_min, h_max;
821   h_min = h_max = poly[hole_loops[0]][0];
822   for (size_t i = 0; i < hole_loops.size(); ++i) {
823     const loop_t &hole = poly[hole_loops[i]];
824     for (size_t j = 0; j < hole.size(); ++j) {
825       assign_op(h_min, h_min, hole[j], carve::util::min_functor());
826       assign_op(h_max, h_max, hole[j], carve::util::max_functor());
827     }
828   }
829   // choose the axis for which the bbox is largest.
830   int axis = (h_max.x - h_min.x) > (h_max.y - h_min.y) ? 0 : 1;
831
832   // for each hole, find the minimum vertex in the chosen axis.
833   for (size_t i = 0; i < hole_loops.size(); ++i) {
834     const loop_t &hole = poly[hole_loops[i]];
835     size_t best, curr;
836     best = 0;
837     for (curr = 1; curr != hole.size(); ++curr) {
838       if (detail::axisOrdering(hole[curr], hole[best], axis)) {
839         best = curr;
840       }
841     }
842     h_loop_min_vertex.push_back(std::make_pair(hole_loops[i], best));
843   }
844
845   // sort the holes by the minimum vertex.
846   std::sort(h_loop_min_vertex.begin(), h_loop_min_vertex.end(), order_h_loops_2d(poly, axis));
847
848   // now, for each hole, find a vertex in the current polygon loop that it can be joined to.
849   for (unsigned i = 0; i < h_loop_min_vertex.size(); ++i) {
850     // the index of the vertex in the hole to connect.
851     size_t hole_i = h_loop_min_vertex[i].first;
852     size_t hole_i_connect = h_loop_min_vertex[i].second;
853
854     carve::geom2d::P2 hole_min = poly[hole_i][hole_i_connect];
855
856     f_loop_heap.clear();
857     // we order polygon loop vertices that may be able to be connected
858     // to the hole vertex by their distance to the hole vertex
859     heap_ordering_2d _heap_ordering(poly, result, hole_min, axis);
860
861     const size_t SZ = result.size();
862     for (size_t j = 0; j < SZ; ++j) {
863       // it is guaranteed that there exists a polygon vertex with
864       // coord < the min hole coord chosen, which can be joined to
865       // the min hole coord without crossing the polygon
866       // boundary. also, because we merge holes in ascending
867       // order, it is also true that this join can never cross
868       // another hole (and that doesn't need to be tested for).
869       if (pvert(poly, result[j]).v[axis] <= hole_min.v[axis]) {
870         f_loop_heap.push_back(j);
871         std::push_heap(f_loop_heap.begin(), f_loop_heap.end(), _heap_ordering);
872       }
873     }
874
875     // we are going to test each potential (according to the
876     // previous test) polygon vertex as a candidate join. we order
877     // by closeness to the hole vertex, so that the join we make
878     // is as small as possible. to test, we need to check the
879     // joining line segment does not cross any other line segment
880     // in the current polygon loop (excluding those that have the
881     // vertex that we are attempting to join with as an endpoint).
882     size_t attachment_point = result.size();
883
884     while (f_loop_heap.size()) {
885       std::pop_heap(f_loop_heap.begin(), f_loop_heap.end(), _heap_ordering);
886       size_t curr = f_loop_heap.back();
887       f_loop_heap.pop_back();
888       // test the candidate join from result[curr] to hole_min
889
890       if (!testCandidateAttachment(poly, result, curr, hole_min)) {
891         continue;
892       }
893
894       attachment_point = curr;
895       break;
896     }
897
898     if (attachment_point == result.size()) {
899       CARVE_FAIL("didn't manage to link up hole!");
900     }
901
902     patchHoleIntoPolygon_2d(result, attachment_point, hole_i, hole_i_connect, poly[hole_i].size());
903   }
904 }
905
906
907
908 std::vector<std::pair<size_t, size_t> >
909 carve::triangulate::incorporateHolesIntoPolygon(const std::vector<std::vector<carve::geom2d::P2> > &poly) {
910 #if 1
911   std::vector<std::pair<size_t, size_t> > result;
912   std::vector<size_t> hole_indices;
913   hole_indices.reserve(poly.size() - 1);
914   for (size_t i = 1; i < poly.size(); ++i) {
915     hole_indices.push_back(i);
916   }
917
918   incorporateHolesIntoPolygon(poly, result, 0, hole_indices);
919
920   return result;
921
922 #else
923   typedef std::vector<carve::geom2d::P2> loop_t;
924   size_t N = poly[0].size();
925   //
926   // work out how much space to reserve for the patched in holes.
927   for (size_t i = 0; i < poly.size(); i++) {
928     N += 2 + poly[i].size();
929   }
930
931   // this is the vector that we will build the result in.
932   std::vector<std::pair<size_t, size_t> > current_f_loop;
933   current_f_loop.reserve(N);
934
935   // this is a heap of current_f_loop indices that defines the vertex test order.
936   std::vector<size_t> f_loop_heap;
937   f_loop_heap.reserve(N);
938
939   // add the poly loop to current_f_loop.
940   for (size_t i = 0; i < poly[0].size(); ++i) {
941     current_f_loop.push_back(std::make_pair((size_t)0, i));
942   }
943
944   if (poly.size() == 1) {
945     return current_f_loop;
946   }
947
948   std::vector<std::pair<size_t, size_t> > h_loop_min_vertex;
949
950   h_loop_min_vertex.reserve(poly.size() - 1);
951
952   // find the major axis for the holes - this is the axis that we
953   // will sort on for finding vertices on the polygon to join
954   // holes up to.
955   //
956   // it might also be nice to also look for whether it is better
957   // to sort ascending or descending.
958   // 
959   // another trick that could be used is to modify the projection
960   // by 90 degree rotations or flipping about an axis. just as
961   // long as we keep the carve::geom3d::Vector pointers for the
962   // real data in sync, everything should be ok. then we wouldn't
963   // need to accomodate axes or sort order in the main loop.
964
965   // find the bounding box of all the holes.
966   double min_x, min_y, max_x, max_y;
967   min_x = max_x = poly[1][0].x;
968   min_y = max_y = poly[1][0].y;
969   for (size_t i = 1; i < poly.size(); ++i) {
970     const loop_t &hole = poly[i];
971     for (size_t j = 0; j < hole.size(); ++j) {
972       min_x = std::min(min_x, hole[j].x);
973       min_y = std::min(min_y, hole[j].y);
974       max_x = std::max(max_x, hole[j].x);
975       max_y = std::max(max_y, hole[j].y);
976     }
977   }
978
979   // choose the axis for which the bbox is largest.
980   int axis = (max_x - min_x) > (max_y - min_y) ? 0 : 1;
981
982   // for each hole, find the minimum vertex in the chosen axis.
983   for (size_t i = 1; i < poly.size(); ++i) {
984     const loop_t &hole = poly[i];
985     size_t best, curr;
986     best = 0;
987     for (curr = 1; curr != hole.size(); ++curr) {
988       if (detail::axisOrdering(hole[curr], hole[best], axis)) {
989         best = curr;
990       }
991     }
992     h_loop_min_vertex.push_back(std::make_pair(i, best));
993   }
994
995   // sort the holes by the minimum vertex.
996   std::sort(h_loop_min_vertex.begin(), h_loop_min_vertex.end(), order_h_loops_2d(poly, axis));
997
998   // now, for each hole, find a vertex in the current polygon loop that it can be joined to.
999   for (unsigned i = 0; i < h_loop_min_vertex.size(); ++i) {
1000     // the index of the vertex in the hole to connect.
1001     size_t hole_i = h_loop_min_vertex[i].first;
1002     size_t hole_i_connect = h_loop_min_vertex[i].second;
1003
1004     carve::geom2d::P2 hole_min = poly[hole_i][hole_i_connect];
1005
1006     f_loop_heap.clear();
1007     // we order polygon loop vertices that may be able to be connected
1008     // to the hole vertex by their distance to the hole vertex
1009     heap_ordering_2d _heap_ordering(poly, current_f_loop, hole_min, axis);
1010
1011     const size_t SZ = current_f_loop.size();
1012     for (size_t j = 0; j < SZ; ++j) {
1013       // it is guaranteed that there exists a polygon vertex with
1014       // coord < the min hole coord chosen, which can be joined to
1015       // the min hole coord without crossing the polygon
1016       // boundary. also, because we merge holes in ascending
1017       // order, it is also true that this join can never cross
1018       // another hole (and that doesn't need to be tested for).
1019       if (pvert(poly, current_f_loop[j]).v[axis] <= hole_min.v[axis]) {
1020         f_loop_heap.push_back(j);
1021         std::push_heap(f_loop_heap.begin(), f_loop_heap.end(), _heap_ordering);
1022       }
1023     }
1024
1025     // we are going to test each potential (according to the
1026     // previous test) polygon vertex as a candidate join. we order
1027     // by closeness to the hole vertex, so that the join we make
1028     // is as small as possible. to test, we need to check the
1029     // joining line segment does not cross any other line segment
1030     // in the current polygon loop (excluding those that have the
1031     // vertex that we are attempting to join with as an endpoint).
1032     size_t attachment_point = current_f_loop.size();
1033
1034     while (f_loop_heap.size()) {
1035       std::pop_heap(f_loop_heap.begin(), f_loop_heap.end(), _heap_ordering);
1036       size_t curr = f_loop_heap.back();
1037       f_loop_heap.pop_back();
1038       // test the candidate join from current_f_loop[curr] to hole_min
1039
1040       if (!testCandidateAttachment(poly, current_f_loop, curr, hole_min)) {
1041         continue;
1042       }
1043
1044       attachment_point = curr;
1045       break;
1046     }
1047
1048     if (attachment_point == current_f_loop.size()) {
1049       CARVE_FAIL("didn't manage to link up hole!");
1050     }
1051
1052     patchHoleIntoPolygon_2d(current_f_loop, attachment_point, hole_i, hole_i_connect, poly[hole_i].size());
1053   }
1054
1055   return current_f_loop;
1056 #endif
1057 }
1058
1059
1060
1061 std::vector<std::vector<std::pair<size_t, size_t> > >
1062 carve::triangulate::mergePolygonsAndHoles(const std::vector<std::vector<carve::geom2d::P2> > &poly) {
1063   std::vector<size_t> poly_indices, hole_indices;
1064
1065   poly_indices.reserve(poly.size());
1066   hole_indices.reserve(poly.size());
1067
1068   for (size_t i = 0; i < poly.size(); ++i) {
1069     if (carve::geom2d::signedArea(poly[i]) < 0) {
1070       poly_indices.push_back(i);
1071     } else {
1072       hole_indices.push_back(i);
1073     }
1074   }
1075
1076   std::vector<std::vector<std::pair<size_t, size_t> > > result;
1077   result.resize(poly_indices.size());
1078
1079   if (hole_indices.size() == 0) {
1080     for (size_t i = 0; i < poly.size(); ++i) {
1081       result[i].resize(poly[i].size());
1082       for (size_t j = 0; j < poly[i].size(); ++j) {
1083         result[i].push_back(std::make_pair(i, j));
1084       }
1085     }
1086     return result;
1087   }
1088
1089   if (poly_indices.size() == 1) {
1090     incorporateHolesIntoPolygon(poly, result[0], poly_indices[0], hole_indices);
1091
1092     return result;
1093   }
1094   
1095   throw carve::exception("not implemented");
1096 }
1097
1098
1099
1100 void carve::triangulate::triangulate(const std::vector<carve::geom2d::P2> &poly,
1101                                      std::vector<carve::triangulate::tri_idx> &result) {
1102   std::vector<detail::vertex_info *> vinfo;
1103   const size_t N = poly.size();
1104
1105 #if defined(CARVE_DEBUG)
1106   std::cerr << "TRIANGULATION BEGINS" << std::endl;
1107 #endif
1108
1109 #if defined(CARVE_DEBUG_WRITE_PLY_DATA)
1110   dumpPoly(poly, result);
1111 #endif
1112
1113   result.clear();
1114   if (N < 3) {
1115     return;
1116   }
1117
1118   result.reserve(poly.size() - 2);
1119
1120   if (N == 3) {
1121     result.push_back(tri_idx(0, 1, 2));
1122     return;
1123   }
1124
1125   vinfo.resize(N);
1126
1127   vinfo[0] = new detail::vertex_info(poly[0], 0);
1128   for (size_t i = 1; i < N-1; ++i) {
1129     vinfo[i] = new detail::vertex_info(poly[i], i);
1130     vinfo[i]->prev = vinfo[i-1];
1131     vinfo[i-1]->next = vinfo[i];
1132   }
1133   vinfo[N-1] = new detail::vertex_info(poly[N-1], N-1);
1134   vinfo[N-1]->prev = vinfo[N-2];
1135   vinfo[N-1]->next = vinfo[0];
1136   vinfo[0]->prev = vinfo[N-1];
1137   vinfo[N-2]->next = vinfo[N-1];
1138
1139   for (size_t i = 0; i < N; ++i) {
1140     vinfo[i]->recompute();
1141   }
1142
1143   detail::vertex_info *begin = vinfo[0];
1144
1145   removeDegeneracies(begin, result);
1146   doTriangulate(begin, result);
1147
1148 #if defined(CARVE_DEBUG)
1149   std::cerr << "TRIANGULATION ENDS" << std::endl;
1150 #endif
1151
1152 #if defined(CARVE_DEBUG_WRITE_PLY_DATA)
1153   dumpPoly(poly, result);
1154 #endif
1155 }
1156
1157
1158
1159 void carve::triangulate::detail::tri_pair_t::flip(vert_edge_t &old_edge,
1160                                                   vert_edge_t &new_edge,
1161                                                   vert_edge_t perim[4]) {
1162   unsigned ai, bi;
1163   unsigned cross_ai, cross_bi;
1164
1165   findSharedEdge(ai, bi);
1166   old_edge = ordered_vert_edge_t(a->v[ai], b->v[bi]);
1167
1168   cross_ai = P(ai);
1169   cross_bi = P(bi);
1170   new_edge = ordered_vert_edge_t(a->v[cross_ai], b->v[cross_bi]);
1171
1172   score = -score;
1173
1174   a->v[N(ai)] = b->v[cross_bi];
1175   b->v[N(bi)] = a->v[cross_ai];
1176
1177   perim[0] = ordered_vert_edge_t(a->v[P(ai)], a->v[ai]);
1178   perim[1] = ordered_vert_edge_t(a->v[N(ai)], a->v[ai]); // this edge was a b-edge
1179
1180   perim[2] = ordered_vert_edge_t(b->v[P(bi)], b->v[bi]);
1181   perim[3] = ordered_vert_edge_t(b->v[N(bi)], b->v[bi]); // this edge was an a-edge
1182 }
1183
1184
1185
1186 void carve::triangulate::detail::tri_pairs_t::insert(unsigned a, unsigned b, carve::triangulate::tri_idx *t) {
1187   tri_pair_t *tp;
1188   if (a < b) {
1189     tp = storage[vert_edge_t(a,b)];
1190     if (!tp) {
1191       tp = storage[vert_edge_t(a,b)] = new tri_pair_t;
1192     }
1193     tp->a = t;
1194   } else {
1195     tp = storage[vert_edge_t(b,a)];
1196     if (!tp) {
1197       tp = storage[vert_edge_t(b,a)] = new tri_pair_t;
1198     }
1199     tp->b = t;
1200   }
1201 }