1
2 #include "delaunator.hpp"
3
4 #include <algorithm>
5 #include <cmath>
6 #include <limits>
7 #include <stdexcept>
8 #include <tuple>
9
10 namespace delaunator {
11
12 //@see https://stackoverflow.com/questions/33333363/built-in-mod-vs-custom-mod-function-improve-the-performance-of-modulus-op/33333636#33333636
fast_mod(const size_t i,const size_t c)13 inline size_t fast_mod(const size_t i, const size_t c) {
14 return i >= c ? i % c : i;
15 }
16
17 // Kahan and Babuska summation, Neumaier variant; accumulates less FP error
sum(const std::vector<double> & x)18 inline double sum(const std::vector<double>& x) {
19 double sum = x[0];
20 double err = 0.0;
21
22 for (size_t i = 1; i < x.size(); i++) {
23 const double k = x[i];
24 const double m = sum + k;
25 err += std::fabs(sum) >= std::fabs(k) ? sum - m + k : k - m + sum;
26 sum = m;
27 }
28 return sum + err;
29 }
30
dist(const double ax,const double ay,const double bx,const double by)31 inline double dist(
32 const double ax,
33 const double ay,
34 const double bx,
35 const double by) {
36 const double dx = ax - bx;
37 const double dy = ay - by;
38 return dx * dx + dy * dy;
39 }
40
circumradius(const double ax,const double ay,const double bx,const double by,const double cx,const double cy)41 inline double circumradius(
42 const double ax,
43 const double ay,
44 const double bx,
45 const double by,
46 const double cx,
47 const double cy) {
48 const double dx = bx - ax;
49 const double dy = by - ay;
50 const double ex = cx - ax;
51 const double ey = cy - ay;
52
53 const double bl = dx * dx + dy * dy;
54 const double cl = ex * ex + ey * ey;
55 const double d = dx * ey - dy * ex;
56
57 const double x = (ey * bl - dy * cl) * 0.5 / d;
58 const double y = (dx * cl - ex * bl) * 0.5 / d;
59
60 if ((bl > 0.0 || bl < 0.0) && (cl > 0.0 || cl < 0.0) && (d > 0.0 || d < 0.0)) {
61 return x * x + y * y;
62 } else {
63 return (std::numeric_limits<double>::max)();
64 }
65 }
66
orient(const double px,const double py,const double qx,const double qy,const double rx,const double ry)67 inline bool orient(
68 const double px,
69 const double py,
70 const double qx,
71 const double qy,
72 const double rx,
73 const double ry) {
74 return (qy - py) * (rx - qx) - (qx - px) * (ry - qy) < 0.0;
75 }
76
circumcenter(const double ax,const double ay,const double bx,const double by,const double cx,const double cy)77 inline Point circumcenter(
78 const double ax,
79 const double ay,
80 const double bx,
81 const double by,
82 const double cx,
83 const double cy) {
84 const double dx = bx - ax;
85 const double dy = by - ay;
86 const double ex = cx - ax;
87 const double ey = cy - ay;
88
89 const double bl = dx * dx + dy * dy;
90 const double cl = ex * ex + ey * ey;
91 const double d = dx * ey - dy * ex;
92
93 const double x = ax + (ey * bl - dy * cl) * 0.5 / d;
94 const double y = ay + (dx * cl - ex * bl) * 0.5 / d;
95
96 return Point(x, y);
97 }
98
99
in_circle(const double ax,const double ay,const double bx,const double by,const double cx,const double cy,const double px,const double py)100 inline bool in_circle(
101 const double ax,
102 const double ay,
103 const double bx,
104 const double by,
105 const double cx,
106 const double cy,
107 const double px,
108 const double py) {
109 const double dx = ax - px;
110 const double dy = ay - py;
111 const double ex = bx - px;
112 const double ey = by - py;
113 const double fx = cx - px;
114 const double fy = cy - py;
115
116 const double ap = dx * dx + dy * dy;
117 const double bp = ex * ex + ey * ey;
118 const double cp = fx * fx + fy * fy;
119
120 return (dx * (ey * cp - bp * fy) -
121 dy * (ex * cp - bp * fx) +
122 ap * (ex * fy - ey * fx)) < 0.0;
123 }
124
125 constexpr double EPSILON = std::numeric_limits<double>::epsilon();
126
check_pts_equal(double x1,double y1,double x2,double y2)127 inline bool check_pts_equal(double x1, double y1, double x2, double y2) {
128 return std::fabs(x1 - x2) <= EPSILON &&
129 std::fabs(y1 - y2) <= EPSILON;
130 }
131
132 // monotonically increases with real angle, but doesn't need expensive trigonometry
pseudo_angle(const double dx,const double dy)133 inline double pseudo_angle(const double dx, const double dy) {
134 const double p = dx / (std::abs(dx) + std::abs(dy));
135 return (dy > 0.0 ? 3.0 - p : 1.0 + p) / 4.0; // [0..1)
136 }
137
138 struct DelaunatorPoint {
139 std::size_t i;
140 double x;
141 double y;
142 std::size_t t;
143 std::size_t prev;
144 std::size_t next;
145 bool removed;
146 };
147
Delaunator(std::vector<double> const & in_coords)148 Delaunator::Delaunator(std::vector<double> const& in_coords)
149 : coords(in_coords),
150 triangles(),
151 halfedges(),
152 hull_prev(),
153 hull_next(),
154 hull_tri(),
155 hull_start(),
156 m_hash(),
157 m_hash_size(),
158 m_edge_stack() {
159 std::size_t n = coords.size() >> 1;
160
161 double max_x = (std::numeric_limits<double>::min)();
162 double max_y = (std::numeric_limits<double>::min)();
163 double min_x = (std::numeric_limits<double>::max)();
164 double min_y = (std::numeric_limits<double>::max)();
165 std::vector<std::size_t> ids;
166 ids.reserve(n);
167
168 for (std::size_t i = 0; i < n; i++) {
169 const double x = coords[2 * i];
170 const double y = coords[2 * i + 1];
171
172 if (x < min_x) min_x = x;
173 if (y < min_y) min_y = y;
174 if (x > max_x) max_x = x;
175 if (y > max_y) max_y = y;
176
177 ids.push_back(i);
178 }
179 const double cx = (min_x + max_x) / 2;
180 const double cy = (min_y + max_y) / 2;
181 double min_dist = (std::numeric_limits<double>::max)();
182
183 std::size_t i0 = INVALID_INDEX;
184 std::size_t i1 = INVALID_INDEX;
185 std::size_t i2 = INVALID_INDEX;
186
187 // pick a seed point close to the centroid
188 for (std::size_t i = 0; i < n; i++) {
189 const double d = dist(cx, cy, coords[2 * i], coords[2 * i + 1]);
190 if (d < min_dist) {
191 i0 = i;
192 min_dist = d;
193 }
194 }
195
196 const double i0x = coords[2 * i0];
197 const double i0y = coords[2 * i0 + 1];
198
199 min_dist = (std::numeric_limits<double>::max)();
200
201 // find the point closest to the seed
202 for (std::size_t i = 0; i < n; i++) {
203 if (i == i0) continue;
204 const double d = dist(i0x, i0y, coords[2 * i], coords[2 * i + 1]);
205 if (d < min_dist && d > 0.0) {
206 i1 = i;
207 min_dist = d;
208 }
209 }
210
211 double i1x = coords[2 * i1];
212 double i1y = coords[2 * i1 + 1];
213
214 double min_radius = (std::numeric_limits<double>::max)();
215
216 // find the third point which forms the smallest circumcircle with the first two
217 for (std::size_t i = 0; i < n; i++) {
218 if (i == i0 || i == i1) continue;
219
220 const double r = circumradius(
221 i0x, i0y, i1x, i1y, coords[2 * i], coords[2 * i + 1]);
222
223 if (r < min_radius) {
224 i2 = i;
225 min_radius = r;
226 }
227 }
228
229 if (!(min_radius < (std::numeric_limits<double>::max()))) {
230 throw std::runtime_error("All points collinear");
231 }
232
233 double i2x = coords[2 * i2];
234 double i2y = coords[2 * i2 + 1];
235
236 if (orient(i0x, i0y, i1x, i1y, i2x, i2y)) {
237 std::swap(i1, i2);
238 std::swap(i1x, i2x);
239 std::swap(i1y, i2y);
240 }
241
242 m_center = circumcenter(i0x, i0y, i1x, i1y, i2x, i2y);
243
244 // Calculate the distances from the center once to avoid having to
245 // calculate for each compare. This used to be done in the comparator,
246 // but GCC 7.5+ would copy the comparator to iterators used in the
247 // sort, and this was excruciatingly slow when there were many points
248 // because you had to copy the vector of distances.
249 std::vector<double> dists;
250 dists.reserve(n);
251 for (size_t i = 0; i < n; i++)
252 {
253 const double& x = coords[2 * i];
254 const double& y = coords[2 * i + 1];
255 dists.push_back(dist(x, y, m_center.x(), m_center.y()));
256 }
257
258 // sort the points by distance from the seed triangle circumcenter
259 std::sort(ids.begin(), ids.end(),
260 [&dists](std::size_t i, std::size_t j)
261 { return dists[i] < dists[j]; });
262
263 // initialize a hash table for storing edges of the advancing convex hull
264 m_hash_size = static_cast<std::size_t>(std::llround(std::ceil(std::sqrt(n))));
265 m_hash.resize(m_hash_size);
266 std::fill(m_hash.begin(), m_hash.end(), INVALID_INDEX);
267
268 // initialize arrays for tracking the edges of the advancing convex hull
269 hull_prev.resize(n);
270 hull_next.resize(n);
271 hull_tri.resize(n);
272
273 hull_start = i0;
274
275 size_t hull_size = 3;
276
277 hull_next[i0] = hull_prev[i2] = i1;
278 hull_next[i1] = hull_prev[i0] = i2;
279 hull_next[i2] = hull_prev[i1] = i0;
280
281 hull_tri[i0] = 0;
282 hull_tri[i1] = 1;
283 hull_tri[i2] = 2;
284
285 m_hash[hash_key(i0x, i0y)] = i0;
286 m_hash[hash_key(i1x, i1y)] = i1;
287 m_hash[hash_key(i2x, i2y)] = i2;
288
289 std::size_t max_triangles = n < 3 ? 1 : 2 * n - 5;
290 triangles.reserve(max_triangles * 3);
291 halfedges.reserve(max_triangles * 3);
292 add_triangle(i0, i1, i2, INVALID_INDEX, INVALID_INDEX, INVALID_INDEX);
293 double xp = std::numeric_limits<double>::quiet_NaN();
294 double yp = std::numeric_limits<double>::quiet_NaN();
295 for (std::size_t k = 0; k < n; k++) {
296 const std::size_t i = ids[k];
297 const double x = coords[2 * i];
298 const double y = coords[2 * i + 1];
299
300 // skip near-duplicate points
301 if (k > 0 && check_pts_equal(x, y, xp, yp)) continue;
302 xp = x;
303 yp = y;
304
305 // skip seed triangle points
306 if (
307 check_pts_equal(x, y, i0x, i0y) ||
308 check_pts_equal(x, y, i1x, i1y) ||
309 check_pts_equal(x, y, i2x, i2y)) continue;
310
311 // find a visible edge on the convex hull using edge hash
312 std::size_t start = 0;
313
314 size_t key = hash_key(x, y);
315 for (size_t j = 0; j < m_hash_size; j++) {
316 start = m_hash[fast_mod(key + j, m_hash_size)];
317 if (start != INVALID_INDEX && start != hull_next[start]) break;
318 }
319
320 start = hull_prev[start];
321 size_t e = start;
322 size_t q;
323
324 while (q = hull_next[e], !orient(x, y, coords[2 * e], coords[2 * e + 1], coords[2 * q], coords[2 * q + 1])) { //TODO: does it works in a same way as in JS
325 e = q;
326 if (e == start) {
327 e = INVALID_INDEX;
328 break;
329 }
330 }
331
332 if (e == INVALID_INDEX) continue; // likely a near-duplicate point; skip it
333
334 // add the first triangle from the point
335 std::size_t t = add_triangle(
336 e,
337 i,
338 hull_next[e],
339 INVALID_INDEX,
340 INVALID_INDEX,
341 hull_tri[e]);
342
343 hull_tri[i] = legalize(t + 2);
344 hull_tri[e] = t;
345 hull_size++;
346
347 // walk forward through the hull, adding more triangles and flipping recursively
348 std::size_t next = hull_next[e];
349 while (
350 q = hull_next[next],
351 orient(x, y, coords[2 * next], coords[2 * next + 1], coords[2 * q], coords[2 * q + 1])) {
352 t = add_triangle(next, i, q, hull_tri[i], INVALID_INDEX, hull_tri[next]);
353 hull_tri[i] = legalize(t + 2);
354 hull_next[next] = next; // mark as removed
355 hull_size--;
356 next = q;
357 }
358
359 // walk backward from the other side, adding more triangles and flipping
360 if (e == start) {
361 while (
362 q = hull_prev[e],
363 orient(x, y, coords[2 * q], coords[2 * q + 1], coords[2 * e], coords[2 * e + 1])) {
364 t = add_triangle(q, i, e, INVALID_INDEX, hull_tri[e], hull_tri[q]);
365 legalize(t + 2);
366 hull_tri[q] = t;
367 hull_next[e] = e; // mark as removed
368 hull_size--;
369 e = q;
370 }
371 }
372
373 // update the hull indices
374 hull_prev[i] = e;
375 hull_start = e;
376 hull_prev[next] = i;
377 hull_next[e] = i;
378 hull_next[i] = next;
379
380 m_hash[hash_key(x, y)] = i;
381 m_hash[hash_key(coords[2 * e], coords[2 * e + 1])] = e;
382 }
383 }
384
get_hull_area()385 double Delaunator::get_hull_area() {
386 std::vector<double> hull_area;
387 size_t e = hull_start;
388 do {
389 hull_area.push_back((coords[2 * e] - coords[2 * hull_prev[e]]) * (coords[2 * e + 1] + coords[2 * hull_prev[e] + 1]));
390 e = hull_next[e];
391 } while (e != hull_start);
392 return sum(hull_area);
393 }
394
legalize(std::size_t a)395 std::size_t Delaunator::legalize(std::size_t a) {
396 std::size_t i = 0;
397 std::size_t ar = 0;
398 m_edge_stack.clear();
399
400 // recursion eliminated with a fixed-size stack
401 while (true) {
402 const size_t b = halfedges[a];
403
404 /* if the pair of triangles doesn't satisfy the Delaunay condition
405 * (p1 is inside the circumcircle of [p0, pl, pr]), flip them,
406 * then do the same check/flip recursively for the new pair of triangles
407 *
408 * pl pl
409 * /||\ / \
410 * al/ || \bl al/ \a
411 * / || \ / \
412 * / a||b \ flip /___ar___\
413 * p0\ || /p1 => p0\---bl---/p1
414 * \ || / \ /
415 * ar\ || /br b\ /br
416 * \||/ \ /
417 * pr pr
418 */
419 const size_t a0 = 3 * (a / 3);
420 ar = a0 + (a + 2) % 3;
421
422 if (b == INVALID_INDEX) {
423 if (i > 0) {
424 i--;
425 a = m_edge_stack[i];
426 continue;
427 } else {
428 //i = INVALID_INDEX;
429 break;
430 }
431 }
432
433 const size_t b0 = 3 * (b / 3);
434 const size_t al = a0 + (a + 1) % 3;
435 const size_t bl = b0 + (b + 2) % 3;
436
437 const std::size_t p0 = triangles[ar];
438 const std::size_t pr = triangles[a];
439 const std::size_t pl = triangles[al];
440 const std::size_t p1 = triangles[bl];
441
442 const bool illegal = in_circle(
443 coords[2 * p0],
444 coords[2 * p0 + 1],
445 coords[2 * pr],
446 coords[2 * pr + 1],
447 coords[2 * pl],
448 coords[2 * pl + 1],
449 coords[2 * p1],
450 coords[2 * p1 + 1]);
451
452 if (illegal) {
453 triangles[a] = p1;
454 triangles[b] = p0;
455
456 auto hbl = halfedges[bl];
457
458 // edge swapped on the other side of the hull (rare); fix the halfedge reference
459 if (hbl == INVALID_INDEX) {
460 std::size_t e = hull_start;
461 do {
462 if (hull_tri[e] == bl) {
463 hull_tri[e] = a;
464 break;
465 }
466 e = hull_prev[e];
467 } while (e != hull_start);
468 }
469 link(a, hbl);
470 link(b, halfedges[ar]);
471 link(ar, bl);
472 std::size_t br = b0 + (b + 1) % 3;
473
474 if (i < m_edge_stack.size()) {
475 m_edge_stack[i] = br;
476 } else {
477 m_edge_stack.push_back(br);
478 }
479 i++;
480
481 } else {
482 if (i > 0) {
483 i--;
484 a = m_edge_stack[i];
485 continue;
486 } else {
487 break;
488 }
489 }
490 }
491 return ar;
492 }
493
hash_key(const double x,const double y) const494 std::size_t Delaunator::hash_key(const double x, const double y) const {
495 const double dx = x - m_center.x();
496 const double dy = y - m_center.y();
497 return fast_mod(
498 static_cast<std::size_t>(std::llround(std::floor(pseudo_angle(dx, dy) * static_cast<double>(m_hash_size)))),
499 m_hash_size);
500 }
501
add_triangle(std::size_t i0,std::size_t i1,std::size_t i2,std::size_t a,std::size_t b,std::size_t c)502 std::size_t Delaunator::add_triangle(
503 std::size_t i0,
504 std::size_t i1,
505 std::size_t i2,
506 std::size_t a,
507 std::size_t b,
508 std::size_t c) {
509 std::size_t t = triangles.size();
510 triangles.push_back(i0);
511 triangles.push_back(i1);
512 triangles.push_back(i2);
513 link(t, a);
514 link(t + 1, b);
515 link(t + 2, c);
516 return t;
517 }
518
link(const std::size_t a,const std::size_t b)519 void Delaunator::link(const std::size_t a, const std::size_t b) {
520 std::size_t s = halfedges.size();
521 if (a == s) {
522 halfedges.push_back(b);
523 } else if (a < s) {
524 halfedges[a] = b;
525 } else {
526 throw std::runtime_error("Cannot link edge");
527 }
528 if (b != INVALID_INDEX) {
529 std::size_t s2 = halfedges.size();
530 if (b == s2) {
531 halfedges.push_back(a);
532 } else if (b < s2) {
533 halfedges[b] = a;
534 } else {
535 throw std::runtime_error("Cannot link edge");
536 }
537 }
538 }
539
540 } //namespace delaunator
541