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