1/**************************************************************************/
2/* convex_hull.cpp */
3/**************************************************************************/
4/* This file is part of: */
5/* GODOT ENGINE */
6/* https://godotengine.org */
7/**************************************************************************/
8/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
9/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
10/* */
11/* Permission is hereby granted, free of charge, to any person obtaining */
12/* a copy of this software and associated documentation files (the */
13/* "Software"), to deal in the Software without restriction, including */
14/* without limitation the rights to use, copy, modify, merge, publish, */
15/* distribute, sublicense, and/or sell copies of the Software, and to */
16/* permit persons to whom the Software is furnished to do so, subject to */
17/* the following conditions: */
18/* */
19/* The above copyright notice and this permission notice shall be */
20/* included in all copies or substantial portions of the Software. */
21/* */
22/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
23/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
24/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
25/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
26/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
27/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
28/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
29/**************************************************************************/
30
31/*
32 * Based on Godot's patched VHACD-version of Bullet's btConvexHullComputer.
33 * See /thirdparty/vhacd/btConvexHullComputer.cpp at 64403ddcab9f1dca2408f0a412a22d899708bbb1
34 * In turn, based on /src/LinearMath/btConvexHullComputer.cpp in <https://github.com/bulletphysics/bullet3>
35 * at 73b217fb07e7e3ce126caeb28ab3c9ddd0718467
36 *
37 * Changes:
38 * - int32_t is consistently used instead of int in some cases
39 * - integrated patch db0d6c92927f5a1358b887f2645c11f3014f0e8a from Bullet (CWE-190 integer overflow in btConvexHullComputer)
40 * - adapted to Godot's code style
41 * - replaced Bullet's types (e.g. vectors) with Godot's
42 * - replaced custom Pool implementation with PagedAllocator
43 */
44
45/*
46Copyright (c) 2011 Ole Kniemeyer, MAXON, www.maxon.net
47
48This software is provided 'as-is', without any express or implied warranty.
49In no event will the authors be held liable for any damages arising from the use of this software.
50Permission is granted to anyone to use this software for any purpose,
51including commercial applications, and to alter it and redistribute it freely,
52subject to the following restrictions:
53
541. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
552. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
563. This notice may not be removed or altered from any source distribution.
57*/
58
59#include "convex_hull.h"
60
61#include "core/error/error_macros.h"
62#include "core/math/aabb.h"
63#include "core/math/math_defs.h"
64#include "core/os/memory.h"
65#include "core/templates/oa_hash_map.h"
66#include "core/templates/paged_allocator.h"
67
68#include <string.h>
69
70//#define DEBUG_CONVEX_HULL
71//#define SHOW_ITERATIONS
72
73// -- GODOT start --
74// Assembly optimizations are not used at the moment.
75//#define USE_X86_64_ASM
76// -- GODOT end --
77
78#ifdef DEBUG_ENABLED
79#define CHULL_ASSERT(m_cond) \
80 do { \
81 if (unlikely(!(m_cond))) { \
82 ERR_PRINT("Assertion \"" _STR(m_cond) "\" failed."); \
83 } \
84 } while (0)
85#else
86#define CHULL_ASSERT(m_cond) \
87 do { \
88 } while (0)
89#endif
90
91#if defined(DEBUG_CONVEX_HULL) || defined(SHOW_ITERATIONS)
92#include <stdio.h>
93#endif
94
95// Convex hull implementation based on Preparata and Hong
96// Ole Kniemeyer, MAXON Computer GmbH
97class ConvexHullInternal {
98public:
99 class Point64 {
100 public:
101 int64_t x;
102 int64_t y;
103 int64_t z;
104
105 Point64(int64_t p_x, int64_t p_y, int64_t p_z) {
106 x = p_x;
107 y = p_y;
108 z = p_z;
109 }
110
111 bool is_zero() {
112 return (x == 0) && (y == 0) && (z == 0);
113 }
114
115 int64_t dot(const Point64 &b) const {
116 return x * b.x + y * b.y + z * b.z;
117 }
118 };
119
120 class Point32 {
121 public:
122 int32_t x = 0;
123 int32_t y = 0;
124 int32_t z = 0;
125 int32_t index = -1;
126
127 Point32() {
128 }
129
130 Point32(int32_t p_x, int32_t p_y, int32_t p_z) {
131 x = p_x;
132 y = p_y;
133 z = p_z;
134 }
135
136 bool operator==(const Point32 &b) const {
137 return (x == b.x) && (y == b.y) && (z == b.z);
138 }
139
140 bool operator!=(const Point32 &b) const {
141 return (x != b.x) || (y != b.y) || (z != b.z);
142 }
143
144 bool is_zero() {
145 return (x == 0) && (y == 0) && (z == 0);
146 }
147
148 Point64 cross(const Point32 &b) const {
149 return Point64((int64_t)y * b.z - (int64_t)z * b.y, (int64_t)z * b.x - (int64_t)x * b.z, (int64_t)x * b.y - (int64_t)y * b.x);
150 }
151
152 Point64 cross(const Point64 &b) const {
153 return Point64(y * b.z - z * b.y, z * b.x - x * b.z, x * b.y - y * b.x);
154 }
155
156 int64_t dot(const Point32 &b) const {
157 return (int64_t)x * b.x + (int64_t)y * b.y + (int64_t)z * b.z;
158 }
159
160 int64_t dot(const Point64 &b) const {
161 return x * b.x + y * b.y + z * b.z;
162 }
163
164 Point32 operator+(const Point32 &b) const {
165 return Point32(x + b.x, y + b.y, z + b.z);
166 }
167
168 Point32 operator-(const Point32 &b) const {
169 return Point32(x - b.x, y - b.y, z - b.z);
170 }
171 };
172
173 class Int128 {
174 public:
175 uint64_t low = 0;
176 uint64_t high = 0;
177
178 Int128() {
179 }
180
181 Int128(uint64_t p_low, uint64_t p_high) {
182 low = p_low;
183 high = p_high;
184 }
185
186 Int128(uint64_t p_low) {
187 low = p_low;
188 high = 0;
189 }
190
191 Int128(int64_t p_value) {
192 low = p_value;
193 if (p_value >= 0) {
194 high = 0;
195 } else {
196 high = (uint64_t)-1LL;
197 }
198 }
199
200 static Int128 mul(int64_t a, int64_t b);
201
202 static Int128 mul(uint64_t a, uint64_t b);
203
204 Int128 operator-() const {
205 return Int128((uint64_t) - (int64_t)low, ~high + (low == 0));
206 }
207
208 Int128 operator+(const Int128 &b) const {
209#ifdef USE_X86_64_ASM
210 Int128 result;
211 __asm__("addq %[bl], %[rl]\n\t"
212 "adcq %[bh], %[rh]\n\t"
213 : [rl] "=r"(result.low), [rh] "=r"(result.high)
214 : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high)
215 : "cc");
216 return result;
217#else
218 uint64_t lo = low + b.low;
219 return Int128(lo, high + b.high + (lo < low));
220#endif
221 }
222
223 Int128 operator-(const Int128 &b) const {
224#ifdef USE_X86_64_ASM
225 Int128 result;
226 __asm__("subq %[bl], %[rl]\n\t"
227 "sbbq %[bh], %[rh]\n\t"
228 : [rl] "=r"(result.low), [rh] "=r"(result.high)
229 : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high)
230 : "cc");
231 return result;
232#else
233 return *this + -b;
234#endif
235 }
236
237 Int128 &operator+=(const Int128 &b) {
238#ifdef USE_X86_64_ASM
239 __asm__("addq %[bl], %[rl]\n\t"
240 "adcq %[bh], %[rh]\n\t"
241 : [rl] "=r"(low), [rh] "=r"(high)
242 : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high)
243 : "cc");
244#else
245 uint64_t lo = low + b.low;
246 if (lo < low) {
247 ++high;
248 }
249 low = lo;
250 high += b.high;
251#endif
252 return *this;
253 }
254
255 Int128 &operator++() {
256 if (++low == 0) {
257 ++high;
258 }
259 return *this;
260 }
261
262 Int128 operator*(int64_t b) const;
263
264 real_t to_scalar() const {
265 return ((int64_t)high >= 0) ? real_t(high) * (real_t(0x100000000LL) * real_t(0x100000000LL)) + real_t(low) : -(-*this).to_scalar();
266 }
267
268 int32_t get_sign() const {
269 return ((int64_t)high < 0) ? -1 : ((high || low) ? 1 : 0);
270 }
271
272 bool operator<(const Int128 &b) const {
273 return (high < b.high) || ((high == b.high) && (low < b.low));
274 }
275
276 int32_t ucmp(const Int128 &b) const {
277 if (high < b.high) {
278 return -1;
279 }
280 if (high > b.high) {
281 return 1;
282 }
283 if (low < b.low) {
284 return -1;
285 }
286 if (low > b.low) {
287 return 1;
288 }
289 return 0;
290 }
291 };
292
293 class Rational64 {
294 private:
295 uint64_t numerator;
296 uint64_t denominator;
297 int32_t sign;
298
299 public:
300 Rational64(int64_t p_numerator, int64_t p_denominator) {
301 if (p_numerator > 0) {
302 sign = 1;
303 numerator = (uint64_t)p_numerator;
304 } else if (p_numerator < 0) {
305 sign = -1;
306 numerator = (uint64_t)-p_numerator;
307 } else {
308 sign = 0;
309 numerator = 0;
310 }
311 if (p_denominator > 0) {
312 denominator = (uint64_t)p_denominator;
313 } else if (p_denominator < 0) {
314 sign = -sign;
315 denominator = (uint64_t)-p_denominator;
316 } else {
317 denominator = 0;
318 }
319 }
320
321 bool is_negative_infinity() const {
322 return (sign < 0) && (denominator == 0);
323 }
324
325 bool is_nan() const {
326 return (sign == 0) && (denominator == 0);
327 }
328
329 int32_t compare(const Rational64 &b) const;
330
331 real_t to_scalar() const {
332 return sign * ((denominator == 0) ? FLT_MAX : (real_t)numerator / denominator);
333 }
334 };
335
336 class Rational128 {
337 private:
338 Int128 numerator;
339 Int128 denominator;
340 int32_t sign;
341 bool is_int_64;
342
343 public:
344 Rational128(int64_t p_value) {
345 if (p_value > 0) {
346 sign = 1;
347 this->numerator = p_value;
348 } else if (p_value < 0) {
349 sign = -1;
350 this->numerator = -p_value;
351 } else {
352 sign = 0;
353 this->numerator = (uint64_t)0;
354 }
355 this->denominator = (uint64_t)1;
356 is_int_64 = true;
357 }
358
359 Rational128(const Int128 &p_numerator, const Int128 &p_denominator) {
360 sign = p_numerator.get_sign();
361 if (sign >= 0) {
362 this->numerator = p_numerator;
363 } else {
364 this->numerator = -p_numerator;
365 }
366 int32_t dsign = p_denominator.get_sign();
367 if (dsign >= 0) {
368 this->denominator = p_denominator;
369 } else {
370 sign = -sign;
371 this->denominator = -p_denominator;
372 }
373 is_int_64 = false;
374 }
375
376 int32_t compare(const Rational128 &b) const;
377
378 int32_t compare(int64_t b) const;
379
380 real_t to_scalar() const {
381 return sign * ((denominator.get_sign() == 0) ? FLT_MAX : numerator.to_scalar() / denominator.to_scalar());
382 }
383 };
384
385 class PointR128 {
386 public:
387 Int128 x;
388 Int128 y;
389 Int128 z;
390 Int128 denominator;
391
392 PointR128() {
393 }
394
395 PointR128(Int128 p_x, Int128 p_y, Int128 p_z, Int128 p_denominator) {
396 x = p_x;
397 y = p_y;
398 z = p_z;
399 denominator = p_denominator;
400 }
401
402 real_t xvalue() const {
403 return x.to_scalar() / denominator.to_scalar();
404 }
405
406 real_t yvalue() const {
407 return y.to_scalar() / denominator.to_scalar();
408 }
409
410 real_t zvalue() const {
411 return z.to_scalar() / denominator.to_scalar();
412 }
413 };
414
415 class Edge;
416 class Face;
417
418 class Vertex {
419 public:
420 Vertex *next = nullptr;
421 Vertex *prev = nullptr;
422 Edge *edges = nullptr;
423 Face *first_nearby_face = nullptr;
424 Face *last_nearby_face = nullptr;
425 PointR128 point128;
426 Point32 point;
427 int32_t copy = -1;
428
429 Vertex() {
430 }
431
432#ifdef DEBUG_CONVEX_HULL
433 void print() {
434 printf("V%d (%d, %d, %d)", point.index, point.x, point.y, point.z);
435 }
436
437 void print_graph();
438#endif
439
440 Point32 operator-(const Vertex &b) const {
441 return point - b.point;
442 }
443
444 Rational128 dot(const Point64 &b) const {
445 return (point.index >= 0) ? Rational128(point.dot(b)) : Rational128(point128.x * b.x + point128.y * b.y + point128.z * b.z, point128.denominator);
446 }
447
448 real_t xvalue() const {
449 return (point.index >= 0) ? real_t(point.x) : point128.xvalue();
450 }
451
452 real_t yvalue() const {
453 return (point.index >= 0) ? real_t(point.y) : point128.yvalue();
454 }
455
456 real_t zvalue() const {
457 return (point.index >= 0) ? real_t(point.z) : point128.zvalue();
458 }
459
460 void receive_nearby_faces(Vertex *p_src) {
461 if (last_nearby_face) {
462 last_nearby_face->next_with_same_nearby_vertex = p_src->first_nearby_face;
463 } else {
464 first_nearby_face = p_src->first_nearby_face;
465 }
466 if (p_src->last_nearby_face) {
467 last_nearby_face = p_src->last_nearby_face;
468 }
469 for (Face *f = p_src->first_nearby_face; f; f = f->next_with_same_nearby_vertex) {
470 CHULL_ASSERT(f->nearby_vertex == p_src);
471 f->nearby_vertex = this;
472 }
473 p_src->first_nearby_face = nullptr;
474 p_src->last_nearby_face = nullptr;
475 }
476 };
477
478 class Edge {
479 public:
480 Edge *next = nullptr;
481 Edge *prev = nullptr;
482 Edge *reverse = nullptr;
483 Vertex *target = nullptr;
484 Face *face = nullptr;
485 int32_t copy = -1;
486
487 void link(Edge *n) {
488 CHULL_ASSERT(reverse->target == n->reverse->target);
489 next = n;
490 n->prev = this;
491 }
492
493#ifdef DEBUG_CONVEX_HULL
494 void print() {
495 printf("E%p : %d -> %d, n=%p p=%p (0 %d\t%d\t%d) -> (%d %d %d)", this, reverse->target->point.index, target->point.index, next, prev,
496 reverse->target->point.x, reverse->target->point.y, reverse->target->point.z, target->point.x, target->point.y, target->point.z);
497 }
498#endif
499 };
500
501 class Face {
502 public:
503 Face *next = nullptr;
504 Vertex *nearby_vertex = nullptr;
505 Face *next_with_same_nearby_vertex = nullptr;
506 Point32 origin;
507 Point32 dir0;
508 Point32 dir1;
509
510 Face() {
511 }
512
513 void init(Vertex *p_a, const Vertex *p_b, const Vertex *p_c) {
514 nearby_vertex = p_a;
515 origin = p_a->point;
516 dir0 = *p_b - *p_a;
517 dir1 = *p_c - *p_a;
518 if (p_a->last_nearby_face) {
519 p_a->last_nearby_face->next_with_same_nearby_vertex = this;
520 } else {
521 p_a->first_nearby_face = this;
522 }
523 p_a->last_nearby_face = this;
524 }
525
526 Point64 get_normal() {
527 return dir0.cross(dir1);
528 }
529 };
530
531 template <typename UWord, typename UHWord>
532 class DMul {
533 private:
534 static uint32_t high(uint64_t p_value) {
535 return (uint32_t)(p_value >> 32);
536 }
537
538 static uint32_t low(uint64_t p_value) {
539 return (uint32_t)p_value;
540 }
541
542 static uint64_t mul(uint32_t a, uint32_t b) {
543 return (uint64_t)a * (uint64_t)b;
544 }
545
546 static void shl_half(uint64_t &p_value) {
547 p_value <<= 32;
548 }
549
550 static uint64_t high(Int128 p_value) {
551 return p_value.high;
552 }
553
554 static uint64_t low(Int128 p_value) {
555 return p_value.low;
556 }
557
558 static Int128 mul(uint64_t a, uint64_t b) {
559 return Int128::mul(a, b);
560 }
561
562 static void shl_half(Int128 &p_value) {
563 p_value.high = p_value.low;
564 p_value.low = 0;
565 }
566
567 public:
568 static void mul(UWord p_a, UWord p_b, UWord &r_low, UWord &r_high) {
569 UWord p00 = mul(low(p_a), low(p_b));
570 UWord p01 = mul(low(p_a), high(p_b));
571 UWord p10 = mul(high(p_a), low(p_b));
572 UWord p11 = mul(high(p_a), high(p_b));
573 UWord p0110 = UWord(low(p01)) + UWord(low(p10));
574 p11 += high(p01);
575 p11 += high(p10);
576 p11 += high(p0110);
577 shl_half(p0110);
578 p00 += p0110;
579 if (p00 < p0110) {
580 ++p11;
581 }
582 r_low = p00;
583 r_high = p11;
584 }
585 };
586
587private:
588 class IntermediateHull {
589 public:
590 Vertex *min_xy = nullptr;
591 Vertex *max_xy = nullptr;
592 Vertex *min_yx = nullptr;
593 Vertex *max_yx = nullptr;
594
595 IntermediateHull() {
596 }
597 };
598
599 enum Orientation { ORIENTATION_NONE,
600 ORIENTATION_CLOCKWISE,
601 ORIENTATION_COUNTER_CLOCKWISE };
602
603 Vector3 scaling;
604 Vector3 center;
605 PagedAllocator<Vertex> vertex_pool;
606 PagedAllocator<Edge> edge_pool;
607 PagedAllocator<Face> face_pool;
608 LocalVector<Vertex *> original_vertices;
609 int32_t merge_stamp = 0;
610 Vector3::Axis min_axis = Vector3::Axis::AXIS_X;
611 Vector3::Axis med_axis = Vector3::Axis::AXIS_X;
612 Vector3::Axis max_axis = Vector3::Axis::AXIS_X;
613 int32_t used_edge_pairs = 0;
614 int32_t max_used_edge_pairs = 0;
615
616 static Orientation get_orientation(const Edge *p_prev, const Edge *p_next, const Point32 &p_s, const Point32 &p_t);
617 Edge *find_max_angle(bool p_ccw, const Vertex *p_start, const Point32 &p_s, const Point64 &p_rxs, const Point64 &p_ssxrxs, Rational64 &p_min_cot);
618 void find_edge_for_coplanar_faces(Vertex *p_c0, Vertex *p_c1, Edge *&p_e0, Edge *&p_e1, const Vertex *p_stop0, const Vertex *p_stop1);
619
620 Edge *new_edge_pair(Vertex *p_from, Vertex *p_to);
621
622 void remove_edge_pair(Edge *p_edge) {
623 Edge *n = p_edge->next;
624 Edge *r = p_edge->reverse;
625
626 CHULL_ASSERT(p_edge->target && r->target);
627
628 if (n != p_edge) {
629 n->prev = p_edge->prev;
630 p_edge->prev->next = n;
631 r->target->edges = n;
632 } else {
633 r->target->edges = nullptr;
634 }
635
636 n = r->next;
637
638 if (n != r) {
639 n->prev = r->prev;
640 r->prev->next = n;
641 p_edge->target->edges = n;
642 } else {
643 p_edge->target->edges = nullptr;
644 }
645
646 edge_pool.free(p_edge);
647 edge_pool.free(r);
648 used_edge_pairs--;
649 }
650
651 void compute_internal(int32_t p_start, int32_t p_end, IntermediateHull &r_result);
652
653 bool merge_projection(IntermediateHull &p_h0, IntermediateHull &p_h1, Vertex *&r_c0, Vertex *&r_c1);
654
655 void merge(IntermediateHull &p_h0, IntermediateHull &p_h1);
656
657 Vector3 to_gd_vector(const Point32 &p_v);
658
659 Vector3 get_gd_normal(Face *p_face);
660
661 bool shift_face(Face *p_face, real_t p_amount, LocalVector<Vertex *> &p_stack);
662
663public:
664 ~ConvexHullInternal() {
665 vertex_pool.reset(true);
666 edge_pool.reset(true);
667 face_pool.reset(true);
668 }
669
670 Vertex *vertex_list = nullptr;
671
672 void compute(const Vector3 *p_coords, int32_t p_count);
673
674 Vector3 get_coordinates(const Vertex *p_v);
675
676 real_t shrink(real_t amount, real_t p_clamp_amount);
677};
678
679ConvexHullInternal::Int128 ConvexHullInternal::Int128::operator*(int64_t b) const {
680 bool negative = (int64_t)high < 0;
681 Int128 a = negative ? -*this : *this;
682 if (b < 0) {
683 negative = !negative;
684 b = -b;
685 }
686 Int128 result = mul(a.low, (uint64_t)b);
687 result.high += a.high * (uint64_t)b;
688 return negative ? -result : result;
689}
690
691ConvexHullInternal::Int128 ConvexHullInternal::Int128::mul(int64_t a, int64_t b) {
692 Int128 result;
693
694#ifdef USE_X86_64_ASM
695 __asm__("imulq %[b]"
696 : "=a"(result.low), "=d"(result.high)
697 : "0"(a), [b] "r"(b)
698 : "cc");
699 return result;
700
701#else
702 bool negative = a < 0;
703 if (negative) {
704 a = -a;
705 }
706 if (b < 0) {
707 negative = !negative;
708 b = -b;
709 }
710 DMul<uint64_t, uint32_t>::mul((uint64_t)a, (uint64_t)b, result.low, result.high);
711 return negative ? -result : result;
712#endif
713}
714
715ConvexHullInternal::Int128 ConvexHullInternal::Int128::mul(uint64_t a, uint64_t b) {
716 Int128 result;
717
718#ifdef USE_X86_64_ASM
719 __asm__("mulq %[b]"
720 : "=a"(result.low), "=d"(result.high)
721 : "0"(a), [b] "r"(b)
722 : "cc");
723
724#else
725 DMul<uint64_t, uint32_t>::mul(a, b, result.low, result.high);
726#endif
727
728 return result;
729}
730
731int32_t ConvexHullInternal::Rational64::compare(const Rational64 &b) const {
732 if (sign != b.sign) {
733 return sign - b.sign;
734 } else if (sign == 0) {
735 return 0;
736 }
737
738#ifdef USE_X86_64_ASM
739
740 int32_t result;
741 int64_t tmp;
742 int64_t dummy;
743 __asm__("mulq %[bn]\n\t"
744 "movq %%rax, %[tmp]\n\t"
745 "movq %%rdx, %%rbx\n\t"
746 "movq %[tn], %%rax\n\t"
747 "mulq %[bd]\n\t"
748 "subq %[tmp], %%rax\n\t"
749 "sbbq %%rbx, %%rdx\n\t" // rdx:rax contains 128-bit-difference "numerator*b.denominator - b.numerator*denominator"
750 "setnsb %%bh\n\t" // bh=1 if difference is non-negative, bh=0 otherwise
751 "orq %%rdx, %%rax\n\t"
752 "setnzb %%bl\n\t" // bl=1 if difference if non-zero, bl=0 if it is zero
753 "decb %%bh\n\t" // now bx=0x0000 if difference is zero, 0xff01 if it is negative, 0x0001 if it is positive (i.e., same sign as difference)
754 "shll $16, %%ebx\n\t" // ebx has same sign as difference
755 : "=&b"(result), [tmp] "=&r"(tmp), "=a"(dummy)
756 : "a"(denominator), [bn] "g"(b.numerator), [tn] "g"(numerator), [bd] "g"(b.denominator)
757 : "%rdx", "cc");
758 // if sign is +1, only bit 0 of result is inverted, which does not change the sign of result (and cannot result in zero)
759 // if sign is -1, all bits of result are inverted, which changes the sign of result (and again cannot result in zero)
760 return result ? result ^ sign : 0;
761
762#else
763
764 return sign * Int128::mul(numerator, b.denominator).ucmp(Int128::mul(denominator, b.numerator));
765
766#endif
767}
768
769int32_t ConvexHullInternal::Rational128::compare(const Rational128 &b) const {
770 if (sign != b.sign) {
771 return sign - b.sign;
772 } else if (sign == 0) {
773 return 0;
774 }
775 if (is_int_64) {
776 return -b.compare(sign * (int64_t)numerator.low);
777 }
778
779 Int128 nbd_low, nbd_high, dbn_low, dbn_high;
780 DMul<Int128, uint64_t>::mul(numerator, b.denominator, nbd_low, nbd_high);
781 DMul<Int128, uint64_t>::mul(denominator, b.numerator, dbn_low, dbn_high);
782
783 int32_t cmp = nbd_high.ucmp(dbn_high);
784 if (cmp) {
785 return cmp * sign;
786 }
787 return nbd_low.ucmp(dbn_low) * sign;
788}
789
790int32_t ConvexHullInternal::Rational128::compare(int64_t b) const {
791 if (is_int_64) {
792 int64_t a = sign * (int64_t)numerator.low;
793 return (a > b) ? 1 : ((a < b) ? -1 : 0);
794 }
795 if (b > 0) {
796 if (sign <= 0) {
797 return -1;
798 }
799 } else if (b < 0) {
800 if (sign >= 0) {
801 return 1;
802 }
803 b = -b;
804 } else {
805 return sign;
806 }
807
808 return numerator.ucmp(denominator * b) * sign;
809}
810
811ConvexHullInternal::Edge *ConvexHullInternal::new_edge_pair(Vertex *p_from, Vertex *p_to) {
812 CHULL_ASSERT(p_from && p_to);
813 Edge *e = edge_pool.alloc();
814 Edge *r = edge_pool.alloc();
815 e->reverse = r;
816 r->reverse = e;
817 e->copy = merge_stamp;
818 r->copy = merge_stamp;
819 e->target = p_to;
820 r->target = p_from;
821 e->face = nullptr;
822 r->face = nullptr;
823 used_edge_pairs++;
824 if (used_edge_pairs > max_used_edge_pairs) {
825 max_used_edge_pairs = used_edge_pairs;
826 }
827 return e;
828}
829
830bool ConvexHullInternal::merge_projection(IntermediateHull &r_h0, IntermediateHull &r_h1, Vertex *&r_c0, Vertex *&r_c1) {
831 Vertex *v0 = r_h0.max_yx;
832 Vertex *v1 = r_h1.min_yx;
833 if ((v0->point.x == v1->point.x) && (v0->point.y == v1->point.y)) {
834 CHULL_ASSERT(v0->point.z < v1->point.z);
835 Vertex *v1p = v1->prev;
836 if (v1p == v1) {
837 r_c0 = v0;
838 if (v1->edges) {
839 CHULL_ASSERT(v1->edges->next == v1->edges);
840 v1 = v1->edges->target;
841 CHULL_ASSERT(v1->edges->next == v1->edges);
842 }
843 r_c1 = v1;
844 return false;
845 }
846 Vertex *v1n = v1->next;
847 v1p->next = v1n;
848 v1n->prev = v1p;
849 if (v1 == r_h1.min_xy) {
850 if ((v1n->point.x < v1p->point.x) || ((v1n->point.x == v1p->point.x) && (v1n->point.y < v1p->point.y))) {
851 r_h1.min_xy = v1n;
852 } else {
853 r_h1.min_xy = v1p;
854 }
855 }
856 if (v1 == r_h1.max_xy) {
857 if ((v1n->point.x > v1p->point.x) || ((v1n->point.x == v1p->point.x) && (v1n->point.y > v1p->point.y))) {
858 r_h1.max_xy = v1n;
859 } else {
860 r_h1.max_xy = v1p;
861 }
862 }
863 }
864
865 v0 = r_h0.max_xy;
866 v1 = r_h1.max_xy;
867 Vertex *v00 = nullptr;
868 Vertex *v10 = nullptr;
869 int32_t sign = 1;
870
871 for (int32_t side = 0; side <= 1; side++) {
872 int32_t dx = (v1->point.x - v0->point.x) * sign;
873 if (dx > 0) {
874 while (true) {
875 int32_t dy = v1->point.y - v0->point.y;
876
877 Vertex *w0 = side ? v0->next : v0->prev;
878 if (w0 != v0) {
879 int32_t dx0 = (w0->point.x - v0->point.x) * sign;
880 int32_t dy0 = w0->point.y - v0->point.y;
881 if ((dy0 <= 0) && ((dx0 == 0) || ((dx0 < 0) && (dy0 * dx <= dy * dx0)))) {
882 v0 = w0;
883 dx = (v1->point.x - v0->point.x) * sign;
884 continue;
885 }
886 }
887
888 Vertex *w1 = side ? v1->next : v1->prev;
889 if (w1 != v1) {
890 int32_t dx1 = (w1->point.x - v1->point.x) * sign;
891 int32_t dy1 = w1->point.y - v1->point.y;
892 int32_t dxn = (w1->point.x - v0->point.x) * sign;
893 if ((dxn > 0) && (dy1 < 0) && ((dx1 == 0) || ((dx1 < 0) && (dy1 * dx < dy * dx1)))) {
894 v1 = w1;
895 dx = dxn;
896 continue;
897 }
898 }
899
900 break;
901 }
902 } else if (dx < 0) {
903 while (true) {
904 int32_t dy = v1->point.y - v0->point.y;
905
906 Vertex *w1 = side ? v1->prev : v1->next;
907 if (w1 != v1) {
908 int32_t dx1 = (w1->point.x - v1->point.x) * sign;
909 int32_t dy1 = w1->point.y - v1->point.y;
910 if ((dy1 >= 0) && ((dx1 == 0) || ((dx1 < 0) && (dy1 * dx <= dy * dx1)))) {
911 v1 = w1;
912 dx = (v1->point.x - v0->point.x) * sign;
913 continue;
914 }
915 }
916
917 Vertex *w0 = side ? v0->prev : v0->next;
918 if (w0 != v0) {
919 int32_t dx0 = (w0->point.x - v0->point.x) * sign;
920 int32_t dy0 = w0->point.y - v0->point.y;
921 int32_t dxn = (v1->point.x - w0->point.x) * sign;
922 if ((dxn < 0) && (dy0 > 0) && ((dx0 == 0) || ((dx0 < 0) && (dy0 * dx < dy * dx0)))) {
923 v0 = w0;
924 dx = dxn;
925 continue;
926 }
927 }
928
929 break;
930 }
931 } else {
932 int32_t x = v0->point.x;
933 int32_t y0 = v0->point.y;
934 Vertex *w0 = v0;
935 Vertex *t;
936 while (((t = side ? w0->next : w0->prev) != v0) && (t->point.x == x) && (t->point.y <= y0)) {
937 w0 = t;
938 y0 = t->point.y;
939 }
940 v0 = w0;
941
942 int32_t y1 = v1->point.y;
943 Vertex *w1 = v1;
944 while (((t = side ? w1->prev : w1->next) != v1) && (t->point.x == x) && (t->point.y >= y1)) {
945 w1 = t;
946 y1 = t->point.y;
947 }
948 v1 = w1;
949 }
950
951 if (side == 0) {
952 v00 = v0;
953 v10 = v1;
954
955 v0 = r_h0.min_xy;
956 v1 = r_h1.min_xy;
957 sign = -1;
958 }
959 }
960
961 v0->prev = v1;
962 v1->next = v0;
963
964 v00->next = v10;
965 v10->prev = v00;
966
967 if (r_h1.min_xy->point.x < r_h0.min_xy->point.x) {
968 r_h0.min_xy = r_h1.min_xy;
969 }
970 if (r_h1.max_xy->point.x >= r_h0.max_xy->point.x) {
971 r_h0.max_xy = r_h1.max_xy;
972 }
973
974 r_h0.max_yx = r_h1.max_yx;
975
976 r_c0 = v00;
977 r_c1 = v10;
978
979 return true;
980}
981
982void ConvexHullInternal::compute_internal(int32_t p_start, int32_t p_end, IntermediateHull &r_result) {
983 int32_t n = p_end - p_start;
984 switch (n) {
985 case 0:
986 r_result.min_xy = nullptr;
987 r_result.max_xy = nullptr;
988 r_result.min_yx = nullptr;
989 r_result.max_yx = nullptr;
990 return;
991 case 2: {
992 Vertex *v = original_vertices[p_start];
993 Vertex *w = original_vertices[p_start + 1];
994 if (v->point != w->point) {
995 int32_t dx = v->point.x - w->point.x;
996 int32_t dy = v->point.y - w->point.y;
997
998 if ((dx == 0) && (dy == 0)) {
999 if (v->point.z > w->point.z) {
1000 Vertex *t = w;
1001 w = v;
1002 v = t;
1003 }
1004 CHULL_ASSERT(v->point.z < w->point.z);
1005 v->next = v;
1006 v->prev = v;
1007 r_result.min_xy = v;
1008 r_result.max_xy = v;
1009 r_result.min_yx = v;
1010 r_result.max_yx = v;
1011 } else {
1012 v->next = w;
1013 v->prev = w;
1014 w->next = v;
1015 w->prev = v;
1016
1017 if ((dx < 0) || ((dx == 0) && (dy < 0))) {
1018 r_result.min_xy = v;
1019 r_result.max_xy = w;
1020 } else {
1021 r_result.min_xy = w;
1022 r_result.max_xy = v;
1023 }
1024
1025 if ((dy < 0) || ((dy == 0) && (dx < 0))) {
1026 r_result.min_yx = v;
1027 r_result.max_yx = w;
1028 } else {
1029 r_result.min_yx = w;
1030 r_result.max_yx = v;
1031 }
1032 }
1033
1034 Edge *e = new_edge_pair(v, w);
1035 e->link(e);
1036 v->edges = e;
1037
1038 e = e->reverse;
1039 e->link(e);
1040 w->edges = e;
1041
1042 return;
1043 }
1044 [[fallthrough]];
1045 }
1046 case 1: {
1047 Vertex *v = original_vertices[p_start];
1048 v->edges = nullptr;
1049 v->next = v;
1050 v->prev = v;
1051
1052 r_result.min_xy = v;
1053 r_result.max_xy = v;
1054 r_result.min_yx = v;
1055 r_result.max_yx = v;
1056
1057 return;
1058 }
1059 }
1060
1061 int32_t split0 = p_start + n / 2;
1062 Point32 p = original_vertices[split0 - 1]->point;
1063 int32_t split1 = split0;
1064 while ((split1 < p_end) && (original_vertices[split1]->point == p)) {
1065 split1++;
1066 }
1067 compute_internal(p_start, split0, r_result);
1068 IntermediateHull hull1;
1069 compute_internal(split1, p_end, hull1);
1070#ifdef DEBUG_CONVEX_HULL
1071 printf("\n\nMerge\n");
1072 r_result.print();
1073 hull1.print();
1074#endif
1075 merge(r_result, hull1);
1076#ifdef DEBUG_CONVEX_HULL
1077 printf("\n Result\n");
1078 r_result.print();
1079#endif
1080}
1081
1082#ifdef DEBUG_CONVEX_HULL
1083void ConvexHullInternal::IntermediateHull::print() {
1084 printf(" Hull\n");
1085 for (Vertex *v = min_xy; v;) {
1086 printf(" ");
1087 v->print();
1088 if (v == max_xy) {
1089 printf(" max_xy");
1090 }
1091 if (v == min_yx) {
1092 printf(" min_yx");
1093 }
1094 if (v == max_yx) {
1095 printf(" max_yx");
1096 }
1097 if (v->next->prev != v) {
1098 printf(" Inconsistency");
1099 }
1100 printf("\n");
1101 v = v->next;
1102 if (v == min_xy) {
1103 break;
1104 }
1105 }
1106 if (min_xy) {
1107 min_xy->copy = (min_xy->copy == -1) ? -2 : -1;
1108 min_xy->print_graph();
1109 }
1110}
1111
1112void ConvexHullInternal::Vertex::print_graph() {
1113 print();
1114 printf("\nEdges\n");
1115 Edge *e = edges;
1116 if (e) {
1117 do {
1118 e->print();
1119 printf("\n");
1120 e = e->next;
1121 } while (e != edges);
1122 do {
1123 Vertex *v = e->target;
1124 if (v->copy != copy) {
1125 v->copy = copy;
1126 v->print_graph();
1127 }
1128 e = e->next;
1129 } while (e != edges);
1130 }
1131}
1132#endif
1133
1134ConvexHullInternal::Orientation ConvexHullInternal::get_orientation(const Edge *p_prev, const Edge *p_next, const Point32 &p_s, const Point32 &p_t) {
1135 CHULL_ASSERT(p_prev->reverse->target == p_next->reverse->target);
1136 if (p_prev->next == p_next) {
1137 if (p_prev->prev == p_next) {
1138 Point64 n = p_t.cross(p_s);
1139 Point64 m = (*p_prev->target - *p_next->reverse->target).cross(*p_next->target - *p_next->reverse->target);
1140 CHULL_ASSERT(!m.is_zero());
1141 int64_t dot = n.dot(m);
1142 CHULL_ASSERT(dot != 0);
1143 return (dot > 0) ? ORIENTATION_COUNTER_CLOCKWISE : ORIENTATION_CLOCKWISE;
1144 }
1145 return ORIENTATION_COUNTER_CLOCKWISE;
1146 } else if (p_prev->prev == p_next) {
1147 return ORIENTATION_CLOCKWISE;
1148 } else {
1149 return ORIENTATION_NONE;
1150 }
1151}
1152
1153ConvexHullInternal::Edge *ConvexHullInternal::find_max_angle(bool p_ccw, const Vertex *p_start, const Point32 &p_s, const Point64 &p_rxs, const Point64 &p_sxrxs, Rational64 &p_min_cot) {
1154 Edge *min_edge = nullptr;
1155
1156#ifdef DEBUG_CONVEX_HULL
1157 printf("find max edge for %d\n", p_start->point.index);
1158#endif
1159 Edge *e = p_start->edges;
1160 if (e) {
1161 do {
1162 if (e->copy > merge_stamp) {
1163 Point32 t = *e->target - *p_start;
1164 Rational64 cot(t.dot(p_sxrxs), t.dot(p_rxs));
1165#ifdef DEBUG_CONVEX_HULL
1166 printf(" Angle is %f (%d) for ", Math::atan(cot.to_scalar()), (int32_t)cot.is_nan());
1167 e->print();
1168#endif
1169 if (cot.is_nan()) {
1170 CHULL_ASSERT(p_ccw ? (t.dot(p_s) < 0) : (t.dot(p_s) > 0));
1171 } else {
1172 int32_t cmp;
1173 if (min_edge == nullptr) {
1174 p_min_cot = cot;
1175 min_edge = e;
1176 } else if ((cmp = cot.compare(p_min_cot)) < 0) {
1177 p_min_cot = cot;
1178 min_edge = e;
1179 } else if ((cmp == 0) && (p_ccw == (get_orientation(min_edge, e, p_s, t) == ORIENTATION_COUNTER_CLOCKWISE))) {
1180 min_edge = e;
1181 }
1182 }
1183#ifdef DEBUG_CONVEX_HULL
1184 printf("\n");
1185#endif
1186 }
1187 e = e->next;
1188 } while (e != p_start->edges);
1189 }
1190 return min_edge;
1191}
1192
1193void ConvexHullInternal::find_edge_for_coplanar_faces(Vertex *p_c0, Vertex *p_c1, Edge *&p_e0, Edge *&p_e1, const Vertex *p_stop0, const Vertex *p_stop1) {
1194 Edge *start0 = p_e0;
1195 Edge *start1 = p_e1;
1196 Point32 et0 = start0 ? start0->target->point : p_c0->point;
1197 Point32 et1 = start1 ? start1->target->point : p_c1->point;
1198 Point32 s = p_c1->point - p_c0->point;
1199 Point64 normal = ((start0 ? start0 : start1)->target->point - p_c0->point).cross(s);
1200 int64_t dist = p_c0->point.dot(normal);
1201 CHULL_ASSERT(!start1 || (start1->target->point.dot(normal) == dist));
1202 Point64 perp = s.cross(normal);
1203 CHULL_ASSERT(!perp.is_zero());
1204
1205#ifdef DEBUG_CONVEX_HULL
1206 printf(" Advancing %d %d (%p %p, %d %d)\n", p_c0->point.index, p_c1->point.index, start0, start1, start0 ? start0->target->point.index : -1, start1 ? start1->target->point.index : -1);
1207#endif
1208
1209 int64_t max_dot0 = et0.dot(perp);
1210 if (p_e0) {
1211 while (p_e0->target != p_stop0) {
1212 Edge *e = p_e0->reverse->prev;
1213 if (e->target->point.dot(normal) < dist) {
1214 break;
1215 }
1216 CHULL_ASSERT(e->target->point.dot(normal) == dist);
1217 if (e->copy == merge_stamp) {
1218 break;
1219 }
1220 int64_t dot = e->target->point.dot(perp);
1221 if (dot <= max_dot0) {
1222 break;
1223 }
1224 max_dot0 = dot;
1225 p_e0 = e;
1226 et0 = e->target->point;
1227 }
1228 }
1229
1230 int64_t max_dot1 = et1.dot(perp);
1231 if (p_e1) {
1232 while (p_e1->target != p_stop1) {
1233 Edge *e = p_e1->reverse->next;
1234 if (e->target->point.dot(normal) < dist) {
1235 break;
1236 }
1237 CHULL_ASSERT(e->target->point.dot(normal) == dist);
1238 if (e->copy == merge_stamp) {
1239 break;
1240 }
1241 int64_t dot = e->target->point.dot(perp);
1242 if (dot <= max_dot1) {
1243 break;
1244 }
1245 max_dot1 = dot;
1246 p_e1 = e;
1247 et1 = e->target->point;
1248 }
1249 }
1250
1251#ifdef DEBUG_CONVEX_HULL
1252 printf(" Starting at %d %d\n", et0.index, et1.index);
1253#endif
1254
1255 int64_t dx = max_dot1 - max_dot0;
1256 if (dx > 0) {
1257 while (true) {
1258 int64_t dy = (et1 - et0).dot(s);
1259
1260 if (p_e0 && (p_e0->target != p_stop0)) {
1261 Edge *f0 = p_e0->next->reverse;
1262 if (f0->copy > merge_stamp) {
1263 int64_t dx0 = (f0->target->point - et0).dot(perp);
1264 int64_t dy0 = (f0->target->point - et0).dot(s);
1265 if ((dx0 == 0) ? (dy0 < 0) : ((dx0 < 0) && (Rational64(dy0, dx0).compare(Rational64(dy, dx)) >= 0))) {
1266 et0 = f0->target->point;
1267 dx = (et1 - et0).dot(perp);
1268 p_e0 = (p_e0 == start0) ? nullptr : f0;
1269 continue;
1270 }
1271 }
1272 }
1273
1274 if (p_e1 && (p_e1->target != p_stop1)) {
1275 Edge *f1 = p_e1->reverse->next;
1276 if (f1->copy > merge_stamp) {
1277 Point32 d1 = f1->target->point - et1;
1278 if (d1.dot(normal) == 0) {
1279 int64_t dx1 = d1.dot(perp);
1280 int64_t dy1 = d1.dot(s);
1281 int64_t dxn = (f1->target->point - et0).dot(perp);
1282 if ((dxn > 0) && ((dx1 == 0) ? (dy1 < 0) : ((dx1 < 0) && (Rational64(dy1, dx1).compare(Rational64(dy, dx)) > 0)))) {
1283 p_e1 = f1;
1284 et1 = p_e1->target->point;
1285 dx = dxn;
1286 continue;
1287 }
1288 } else {
1289 CHULL_ASSERT((p_e1 == start1) && (d1.dot(normal) < 0));
1290 }
1291 }
1292 }
1293
1294 break;
1295 }
1296 } else if (dx < 0) {
1297 while (true) {
1298 int64_t dy = (et1 - et0).dot(s);
1299
1300 if (p_e1 && (p_e1->target != p_stop1)) {
1301 Edge *f1 = p_e1->prev->reverse;
1302 if (f1->copy > merge_stamp) {
1303 int64_t dx1 = (f1->target->point - et1).dot(perp);
1304 int64_t dy1 = (f1->target->point - et1).dot(s);
1305 if ((dx1 == 0) ? (dy1 > 0) : ((dx1 < 0) && (Rational64(dy1, dx1).compare(Rational64(dy, dx)) <= 0))) {
1306 et1 = f1->target->point;
1307 dx = (et1 - et0).dot(perp);
1308 p_e1 = (p_e1 == start1) ? nullptr : f1;
1309 continue;
1310 }
1311 }
1312 }
1313
1314 if (p_e0 && (p_e0->target != p_stop0)) {
1315 Edge *f0 = p_e0->reverse->prev;
1316 if (f0->copy > merge_stamp) {
1317 Point32 d0 = f0->target->point - et0;
1318 if (d0.dot(normal) == 0) {
1319 int64_t dx0 = d0.dot(perp);
1320 int64_t dy0 = d0.dot(s);
1321 int64_t dxn = (et1 - f0->target->point).dot(perp);
1322 if ((dxn < 0) && ((dx0 == 0) ? (dy0 > 0) : ((dx0 < 0) && (Rational64(dy0, dx0).compare(Rational64(dy, dx)) < 0)))) {
1323 p_e0 = f0;
1324 et0 = p_e0->target->point;
1325 dx = dxn;
1326 continue;
1327 }
1328 } else {
1329 CHULL_ASSERT((p_e0 == start0) && (d0.dot(normal) < 0));
1330 }
1331 }
1332 }
1333
1334 break;
1335 }
1336 }
1337#ifdef DEBUG_CONVEX_HULL
1338 printf(" Advanced edges to %d %d\n", et0.index, et1.index);
1339#endif
1340}
1341
1342void ConvexHullInternal::merge(IntermediateHull &p_h0, IntermediateHull &p_h1) {
1343 if (!p_h1.max_xy) {
1344 return;
1345 }
1346 if (!p_h0.max_xy) {
1347 p_h0 = p_h1;
1348 return;
1349 }
1350
1351 merge_stamp--;
1352
1353 Vertex *c0 = nullptr;
1354 Edge *to_prev0 = nullptr;
1355 Edge *first_new0 = nullptr;
1356 Edge *pending_head0 = nullptr;
1357 Edge *pending_tail0 = nullptr;
1358 Vertex *c1 = nullptr;
1359 Edge *to_prev1 = nullptr;
1360 Edge *first_new1 = nullptr;
1361 Edge *pending_head1 = nullptr;
1362 Edge *pending_tail1 = nullptr;
1363 Point32 prev_point;
1364
1365 if (merge_projection(p_h0, p_h1, c0, c1)) {
1366 Point32 s = *c1 - *c0;
1367 Point64 normal = Point32(0, 0, -1).cross(s);
1368 Point64 t = s.cross(normal);
1369 CHULL_ASSERT(!t.is_zero());
1370
1371 Edge *e = c0->edges;
1372 Edge *start0 = nullptr;
1373 if (e) {
1374 do {
1375 int64_t dot = (*e->target - *c0).dot(normal);
1376 CHULL_ASSERT(dot <= 0);
1377 if ((dot == 0) && ((*e->target - *c0).dot(t) > 0)) {
1378 if (!start0 || (get_orientation(start0, e, s, Point32(0, 0, -1)) == ORIENTATION_CLOCKWISE)) {
1379 start0 = e;
1380 }
1381 }
1382 e = e->next;
1383 } while (e != c0->edges);
1384 }
1385
1386 e = c1->edges;
1387 Edge *start1 = nullptr;
1388 if (e) {
1389 do {
1390 int64_t dot = (*e->target - *c1).dot(normal);
1391 CHULL_ASSERT(dot <= 0);
1392 if ((dot == 0) && ((*e->target - *c1).dot(t) > 0)) {
1393 if (!start1 || (get_orientation(start1, e, s, Point32(0, 0, -1)) == ORIENTATION_COUNTER_CLOCKWISE)) {
1394 start1 = e;
1395 }
1396 }
1397 e = e->next;
1398 } while (e != c1->edges);
1399 }
1400
1401 if (start0 || start1) {
1402 find_edge_for_coplanar_faces(c0, c1, start0, start1, nullptr, nullptr);
1403 if (start0) {
1404 c0 = start0->target;
1405 }
1406 if (start1) {
1407 c1 = start1->target;
1408 }
1409 }
1410
1411 prev_point = c1->point;
1412 prev_point.z++;
1413 } else {
1414 prev_point = c1->point;
1415 prev_point.x++;
1416 }
1417
1418 Vertex *first0 = c0;
1419 Vertex *first1 = c1;
1420 bool first_run = true;
1421
1422 while (true) {
1423 Point32 s = *c1 - *c0;
1424 Point32 r = prev_point - c0->point;
1425 Point64 rxs = r.cross(s);
1426 Point64 sxrxs = s.cross(rxs);
1427
1428#ifdef DEBUG_CONVEX_HULL
1429 printf("\n Checking %d %d\n", c0->point.index, c1->point.index);
1430#endif
1431 Rational64 min_cot0(0, 0);
1432 Edge *min0 = find_max_angle(false, c0, s, rxs, sxrxs, min_cot0);
1433 Rational64 min_cot1(0, 0);
1434 Edge *min1 = find_max_angle(true, c1, s, rxs, sxrxs, min_cot1);
1435 if (!min0 && !min1) {
1436 Edge *e = new_edge_pair(c0, c1);
1437 e->link(e);
1438 c0->edges = e;
1439
1440 e = e->reverse;
1441 e->link(e);
1442 c1->edges = e;
1443 return;
1444 } else {
1445 int32_t cmp = !min0 ? 1 : (!min1 ? -1 : min_cot0.compare(min_cot1));
1446#ifdef DEBUG_CONVEX_HULL
1447 printf(" -> Result %d\n", cmp);
1448#endif
1449 if (first_run || ((cmp >= 0) ? !min_cot1.is_negative_infinity() : !min_cot0.is_negative_infinity())) {
1450 Edge *e = new_edge_pair(c0, c1);
1451 if (pending_tail0) {
1452 pending_tail0->prev = e;
1453 } else {
1454 pending_head0 = e;
1455 }
1456 e->next = pending_tail0;
1457 pending_tail0 = e;
1458
1459 e = e->reverse;
1460 if (pending_tail1) {
1461 pending_tail1->next = e;
1462 } else {
1463 pending_head1 = e;
1464 }
1465 e->prev = pending_tail1;
1466 pending_tail1 = e;
1467 }
1468
1469 Edge *e0 = min0;
1470 Edge *e1 = min1;
1471
1472#ifdef DEBUG_CONVEX_HULL
1473 printf(" Found min edges to %d %d\n", e0 ? e0->target->point.index : -1, e1 ? e1->target->point.index : -1);
1474#endif
1475
1476 if (cmp == 0) {
1477 find_edge_for_coplanar_faces(c0, c1, e0, e1, nullptr, nullptr);
1478 }
1479
1480 if ((cmp >= 0) && e1) {
1481 if (to_prev1) {
1482 for (Edge *e = to_prev1->next, *n = nullptr; e != min1; e = n) {
1483 n = e->next;
1484 remove_edge_pair(e);
1485 }
1486 }
1487
1488 if (pending_tail1) {
1489 if (to_prev1) {
1490 to_prev1->link(pending_head1);
1491 } else {
1492 min1->prev->link(pending_head1);
1493 first_new1 = pending_head1;
1494 }
1495 pending_tail1->link(min1);
1496 pending_head1 = nullptr;
1497 pending_tail1 = nullptr;
1498 } else if (!to_prev1) {
1499 first_new1 = min1;
1500 }
1501
1502 prev_point = c1->point;
1503 c1 = e1->target;
1504 to_prev1 = e1->reverse;
1505 }
1506
1507 if ((cmp <= 0) && e0) {
1508 if (to_prev0) {
1509 for (Edge *e = to_prev0->prev, *n = nullptr; e != min0; e = n) {
1510 n = e->prev;
1511 remove_edge_pair(e);
1512 }
1513 }
1514
1515 if (pending_tail0) {
1516 if (to_prev0) {
1517 pending_head0->link(to_prev0);
1518 } else {
1519 pending_head0->link(min0->next);
1520 first_new0 = pending_head0;
1521 }
1522 min0->link(pending_tail0);
1523 pending_head0 = nullptr;
1524 pending_tail0 = nullptr;
1525 } else if (!to_prev0) {
1526 first_new0 = min0;
1527 }
1528
1529 prev_point = c0->point;
1530 c0 = e0->target;
1531 to_prev0 = e0->reverse;
1532 }
1533 }
1534
1535 if ((c0 == first0) && (c1 == first1)) {
1536 if (to_prev0 == nullptr) {
1537 pending_head0->link(pending_tail0);
1538 c0->edges = pending_tail0;
1539 } else {
1540 for (Edge *e = to_prev0->prev, *n = nullptr; e != first_new0; e = n) {
1541 n = e->prev;
1542 remove_edge_pair(e);
1543 }
1544 if (pending_tail0) {
1545 pending_head0->link(to_prev0);
1546 first_new0->link(pending_tail0);
1547 }
1548 }
1549
1550 if (to_prev1 == nullptr) {
1551 pending_tail1->link(pending_head1);
1552 c1->edges = pending_tail1;
1553 } else {
1554 for (Edge *e = to_prev1->next, *n = nullptr; e != first_new1; e = n) {
1555 n = e->next;
1556 remove_edge_pair(e);
1557 }
1558 if (pending_tail1) {
1559 to_prev1->link(pending_head1);
1560 pending_tail1->link(first_new1);
1561 }
1562 }
1563
1564 return;
1565 }
1566
1567 first_run = false;
1568 }
1569}
1570
1571struct PointComparator {
1572 _FORCE_INLINE_ bool operator()(const ConvexHullInternal::Point32 &p, const ConvexHullInternal::Point32 &q) const {
1573 return (p.y < q.y) || ((p.y == q.y) && ((p.x < q.x) || ((p.x == q.x) && (p.z < q.z))));
1574 }
1575};
1576
1577void ConvexHullInternal::compute(const Vector3 *p_coords, int32_t p_count) {
1578 AABB aabb;
1579 for (int32_t i = 0; i < p_count; i++) {
1580 Vector3 p = p_coords[i];
1581 if (i == 0) {
1582 aabb.position = p;
1583 } else {
1584 aabb.expand_to(p);
1585 }
1586 }
1587
1588 Vector3 s = aabb.size;
1589 max_axis = s.max_axis_index();
1590 min_axis = s.min_axis_index();
1591 if (min_axis == max_axis) {
1592 min_axis = Vector3::Axis((max_axis + 1) % 3);
1593 }
1594 med_axis = Vector3::Axis(3 - max_axis - min_axis);
1595
1596 s /= real_t(10216);
1597 if (((med_axis + 1) % 3) != max_axis) {
1598 s *= -1;
1599 }
1600 scaling = s;
1601
1602 if (s[0] != 0) {
1603 s[0] = real_t(1) / s[0];
1604 }
1605 if (s[1] != 0) {
1606 s[1] = real_t(1) / s[1];
1607 }
1608 if (s[2] != 0) {
1609 s[2] = real_t(1) / s[2];
1610 }
1611
1612 center = aabb.position;
1613
1614 LocalVector<Point32> points;
1615 points.resize(p_count);
1616 for (int32_t i = 0; i < p_count; i++) {
1617 Vector3 p = p_coords[i];
1618 p = (p - center) * s;
1619 points[i].x = (int32_t)p[med_axis];
1620 points[i].y = (int32_t)p[max_axis];
1621 points[i].z = (int32_t)p[min_axis];
1622 points[i].index = i;
1623 }
1624
1625 points.sort_custom<PointComparator>();
1626
1627 vertex_pool.reset(true);
1628 original_vertices.resize(p_count);
1629 for (int32_t i = 0; i < p_count; i++) {
1630 Vertex *v = vertex_pool.alloc();
1631 v->edges = nullptr;
1632 v->point = points[i];
1633 v->copy = -1;
1634 original_vertices[i] = v;
1635 }
1636
1637 points.clear();
1638
1639 edge_pool.reset(true);
1640
1641 used_edge_pairs = 0;
1642 max_used_edge_pairs = 0;
1643
1644 merge_stamp = -3;
1645
1646 IntermediateHull hull;
1647 compute_internal(0, p_count, hull);
1648 vertex_list = hull.min_xy;
1649#ifdef DEBUG_CONVEX_HULL
1650 printf("max. edges %d (3v = %d)", max_used_edge_pairs, 3 * p_count);
1651#endif
1652}
1653
1654Vector3 ConvexHullInternal::to_gd_vector(const Point32 &p_v) {
1655 Vector3 p;
1656 p[med_axis] = real_t(p_v.x);
1657 p[max_axis] = real_t(p_v.y);
1658 p[min_axis] = real_t(p_v.z);
1659 return p * scaling;
1660}
1661
1662Vector3 ConvexHullInternal::get_gd_normal(Face *p_face) {
1663 return to_gd_vector(p_face->dir0).cross(to_gd_vector(p_face->dir1)).normalized();
1664}
1665
1666Vector3 ConvexHullInternal::get_coordinates(const Vertex *p_v) {
1667 Vector3 p;
1668 p[med_axis] = p_v->xvalue();
1669 p[max_axis] = p_v->yvalue();
1670 p[min_axis] = p_v->zvalue();
1671 return p * scaling + center;
1672}
1673
1674real_t ConvexHullInternal::shrink(real_t p_amount, real_t p_clamp_amount) {
1675 if (!vertex_list) {
1676 return 0;
1677 }
1678 int32_t stamp = --merge_stamp;
1679 LocalVector<Vertex *> stack;
1680 vertex_list->copy = stamp;
1681 stack.push_back(vertex_list);
1682 LocalVector<Face *> faces;
1683
1684 Point32 ref = vertex_list->point;
1685 Int128 hull_center_x(0, 0);
1686 Int128 hull_center_y(0, 0);
1687 Int128 hull_center_z(0, 0);
1688 Int128 volume(0, 0);
1689
1690 while (stack.size() > 0) {
1691 Vertex *v = stack[stack.size() - 1];
1692 stack.remove_at(stack.size() - 1);
1693 Edge *e = v->edges;
1694 if (e) {
1695 do {
1696 if (e->target->copy != stamp) {
1697 e->target->copy = stamp;
1698 stack.push_back(e->target);
1699 }
1700 if (e->copy != stamp) {
1701 Face *face = face_pool.alloc();
1702 face->init(e->target, e->reverse->prev->target, v);
1703 faces.push_back(face);
1704 Edge *f = e;
1705
1706 Vertex *a = nullptr;
1707 Vertex *b = nullptr;
1708 do {
1709 if (a && b) {
1710 int64_t vol = (v->point - ref).dot((a->point - ref).cross(b->point - ref));
1711 CHULL_ASSERT(vol >= 0);
1712 Point32 c = v->point + a->point + b->point + ref;
1713 hull_center_x += vol * c.x;
1714 hull_center_y += vol * c.y;
1715 hull_center_z += vol * c.z;
1716 volume += vol;
1717 }
1718
1719 CHULL_ASSERT(f->copy != stamp);
1720 f->copy = stamp;
1721 f->face = face;
1722
1723 a = b;
1724 b = f->target;
1725
1726 f = f->reverse->prev;
1727 } while (f != e);
1728 }
1729 e = e->next;
1730 } while (e != v->edges);
1731 }
1732 }
1733
1734 if (volume.get_sign() <= 0) {
1735 return 0;
1736 }
1737
1738 Vector3 hull_center;
1739 hull_center[med_axis] = hull_center_x.to_scalar();
1740 hull_center[max_axis] = hull_center_y.to_scalar();
1741 hull_center[min_axis] = hull_center_z.to_scalar();
1742 hull_center /= 4 * volume.to_scalar();
1743 hull_center *= scaling;
1744
1745 int32_t face_count = faces.size();
1746
1747 if (p_clamp_amount > 0) {
1748 real_t min_dist = FLT_MAX;
1749 for (int32_t i = 0; i < face_count; i++) {
1750 Vector3 normal = get_gd_normal(faces[i]);
1751 real_t dist = normal.dot(to_gd_vector(faces[i]->origin) - hull_center);
1752 if (dist < min_dist) {
1753 min_dist = dist;
1754 }
1755 }
1756
1757 if (min_dist <= 0) {
1758 return 0;
1759 }
1760
1761 p_amount = MIN(p_amount, min_dist * p_clamp_amount);
1762 }
1763
1764 uint32_t seed = 243703;
1765 for (int32_t i = 0; i < face_count; i++, seed = 1664525 * seed + 1013904223) {
1766 SWAP(faces[i], faces[seed % face_count]);
1767 }
1768
1769 for (int32_t i = 0; i < face_count; i++) {
1770 if (!shift_face(faces[i], p_amount, stack)) {
1771 return -p_amount;
1772 }
1773 }
1774
1775 return p_amount;
1776}
1777
1778bool ConvexHullInternal::shift_face(Face *p_face, real_t p_amount, LocalVector<Vertex *> &p_stack) {
1779 Vector3 orig_shift = get_gd_normal(p_face) * -p_amount;
1780 if (scaling[0] != 0) {
1781 orig_shift[0] /= scaling[0];
1782 }
1783 if (scaling[1] != 0) {
1784 orig_shift[1] /= scaling[1];
1785 }
1786 if (scaling[2] != 0) {
1787 orig_shift[2] /= scaling[2];
1788 }
1789 Point32 shift((int32_t)orig_shift[med_axis], (int32_t)orig_shift[max_axis], (int32_t)orig_shift[min_axis]);
1790 if (shift.is_zero()) {
1791 return true;
1792 }
1793 Point64 normal = p_face->get_normal();
1794#ifdef DEBUG_CONVEX_HULL
1795 printf("\nShrinking p_face (%d %d %d) (%d %d %d) (%d %d %d) by (%d %d %d)\n",
1796 p_face->origin.x, p_face->origin.y, p_face->origin.z, p_face->dir0.x, p_face->dir0.y, p_face->dir0.z, p_face->dir1.x, p_face->dir1.y, p_face->dir1.z, shift.x, shift.y, shift.z);
1797#endif
1798 int64_t orig_dot = p_face->origin.dot(normal);
1799 Point32 shifted_origin = p_face->origin + shift;
1800 int64_t shifted_dot = shifted_origin.dot(normal);
1801 CHULL_ASSERT(shifted_dot <= orig_dot);
1802 if (shifted_dot >= orig_dot) {
1803 return false;
1804 }
1805
1806 Edge *intersection = nullptr;
1807
1808 Edge *start_edge = p_face->nearby_vertex->edges;
1809#ifdef DEBUG_CONVEX_HULL
1810 printf("Start edge is ");
1811 start_edge->print();
1812 printf(", normal is (%lld %lld %lld), shifted dot is %lld\n", normal.x, normal.y, normal.z, shifted_dot);
1813#endif
1814 Rational128 opt_dot = p_face->nearby_vertex->dot(normal);
1815 int32_t cmp = opt_dot.compare(shifted_dot);
1816#ifdef SHOW_ITERATIONS
1817 int32_t n = 0;
1818#endif
1819 if (cmp >= 0) {
1820 Edge *e = start_edge;
1821 do {
1822#ifdef SHOW_ITERATIONS
1823 n++;
1824#endif
1825 Rational128 dot = e->target->dot(normal);
1826 CHULL_ASSERT(dot.compare(orig_dot) <= 0);
1827#ifdef DEBUG_CONVEX_HULL
1828 printf("Moving downwards, edge is ");
1829 e->print();
1830 printf(", dot is %f (%f %lld)\n", (float)dot.to_scalar(), (float)opt_dot.to_scalar(), shifted_dot);
1831#endif
1832 if (dot.compare(opt_dot) < 0) {
1833 int32_t c = dot.compare(shifted_dot);
1834 opt_dot = dot;
1835 e = e->reverse;
1836 start_edge = e;
1837 if (c < 0) {
1838 intersection = e;
1839 break;
1840 }
1841 cmp = c;
1842 }
1843 e = e->prev;
1844 } while (e != start_edge);
1845
1846 if (!intersection) {
1847 return false;
1848 }
1849 } else {
1850 Edge *e = start_edge;
1851 do {
1852#ifdef SHOW_ITERATIONS
1853 n++;
1854#endif
1855 Rational128 dot = e->target->dot(normal);
1856 CHULL_ASSERT(dot.compare(orig_dot) <= 0);
1857#ifdef DEBUG_CONVEX_HULL
1858 printf("Moving upwards, edge is ");
1859 e->print();
1860 printf(", dot is %f (%f %lld)\n", (float)dot.to_scalar(), (float)opt_dot.to_scalar(), shifted_dot);
1861#endif
1862 if (dot.compare(opt_dot) > 0) {
1863 cmp = dot.compare(shifted_dot);
1864 if (cmp >= 0) {
1865 intersection = e;
1866 break;
1867 }
1868 opt_dot = dot;
1869 e = e->reverse;
1870 start_edge = e;
1871 }
1872 e = e->prev;
1873 } while (e != start_edge);
1874
1875 if (!intersection) {
1876 return true;
1877 }
1878 }
1879
1880#ifdef SHOW_ITERATIONS
1881 printf("Needed %d iterations to find initial intersection\n", n);
1882#endif
1883
1884 if (cmp == 0) {
1885 Edge *e = intersection->reverse->next;
1886#ifdef SHOW_ITERATIONS
1887 n = 0;
1888#endif
1889 while (e->target->dot(normal).compare(shifted_dot) <= 0) {
1890#ifdef SHOW_ITERATIONS
1891 n++;
1892#endif
1893 e = e->next;
1894 if (e == intersection->reverse) {
1895 return true;
1896 }
1897#ifdef DEBUG_CONVEX_HULL
1898 printf("Checking for outwards edge, current edge is ");
1899 e->print();
1900 printf("\n");
1901#endif
1902 }
1903#ifdef SHOW_ITERATIONS
1904 printf("Needed %d iterations to check for complete containment\n", n);
1905#endif
1906 }
1907
1908 Edge *first_intersection = nullptr;
1909 Edge *face_edge = nullptr;
1910 Edge *first_face_edge = nullptr;
1911
1912#ifdef SHOW_ITERATIONS
1913 int32_t m = 0;
1914#endif
1915 while (true) {
1916#ifdef SHOW_ITERATIONS
1917 m++;
1918#endif
1919#ifdef DEBUG_CONVEX_HULL
1920 printf("Intersecting edge is ");
1921 intersection->print();
1922 printf("\n");
1923#endif
1924 if (cmp == 0) {
1925 Edge *e = intersection->reverse->next;
1926 start_edge = e;
1927#ifdef SHOW_ITERATIONS
1928 n = 0;
1929#endif
1930 while (true) {
1931#ifdef SHOW_ITERATIONS
1932 n++;
1933#endif
1934 if (e->target->dot(normal).compare(shifted_dot) >= 0) {
1935 break;
1936 }
1937 intersection = e->reverse;
1938 e = e->next;
1939 if (e == start_edge) {
1940 return true;
1941 }
1942 }
1943#ifdef SHOW_ITERATIONS
1944 printf("Needed %d iterations to advance intersection\n", n);
1945#endif
1946 }
1947
1948#ifdef DEBUG_CONVEX_HULL
1949 printf("Advanced intersecting edge to ");
1950 intersection->print();
1951 printf(", cmp = %d\n", cmp);
1952#endif
1953
1954 if (!first_intersection) {
1955 first_intersection = intersection;
1956 } else if (intersection == first_intersection) {
1957 break;
1958 }
1959
1960 int32_t prev_cmp = cmp;
1961 Edge *prev_intersection = intersection;
1962 Edge *prev_face_edge = face_edge;
1963
1964 Edge *e = intersection->reverse;
1965#ifdef SHOW_ITERATIONS
1966 n = 0;
1967#endif
1968 while (true) {
1969#ifdef SHOW_ITERATIONS
1970 n++;
1971#endif
1972 e = e->reverse->prev;
1973 CHULL_ASSERT(e != intersection->reverse);
1974 cmp = e->target->dot(normal).compare(shifted_dot);
1975#ifdef DEBUG_CONVEX_HULL
1976 printf("Testing edge ");
1977 e->print();
1978 printf(" -> cmp = %d\n", cmp);
1979#endif
1980 if (cmp >= 0) {
1981 intersection = e;
1982 break;
1983 }
1984 }
1985#ifdef SHOW_ITERATIONS
1986 printf("Needed %d iterations to find other intersection of p_face\n", n);
1987#endif
1988
1989 if (cmp > 0) {
1990 Vertex *removed = intersection->target;
1991 e = intersection->reverse;
1992 if (e->prev == e) {
1993 removed->edges = nullptr;
1994 } else {
1995 removed->edges = e->prev;
1996 e->prev->link(e->next);
1997 e->link(e);
1998 }
1999#ifdef DEBUG_CONVEX_HULL
2000 printf("1: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z);
2001#endif
2002
2003 Point64 n0 = intersection->face->get_normal();
2004 Point64 n1 = intersection->reverse->face->get_normal();
2005 int64_t m00 = p_face->dir0.dot(n0);
2006 int64_t m01 = p_face->dir1.dot(n0);
2007 int64_t m10 = p_face->dir0.dot(n1);
2008 int64_t m11 = p_face->dir1.dot(n1);
2009 int64_t r0 = (intersection->face->origin - shifted_origin).dot(n0);
2010 int64_t r1 = (intersection->reverse->face->origin - shifted_origin).dot(n1);
2011 Int128 det = Int128::mul(m00, m11) - Int128::mul(m01, m10);
2012 CHULL_ASSERT(det.get_sign() != 0);
2013 Vertex *v = vertex_pool.alloc();
2014 v->point.index = -1;
2015 v->copy = -1;
2016 v->point128 = PointR128(Int128::mul(p_face->dir0.x * r0, m11) - Int128::mul(p_face->dir0.x * r1, m01) + Int128::mul(p_face->dir1.x * r1, m00) - Int128::mul(p_face->dir1.x * r0, m10) + det * shifted_origin.x,
2017 Int128::mul(p_face->dir0.y * r0, m11) - Int128::mul(p_face->dir0.y * r1, m01) + Int128::mul(p_face->dir1.y * r1, m00) - Int128::mul(p_face->dir1.y * r0, m10) + det * shifted_origin.y,
2018 Int128::mul(p_face->dir0.z * r0, m11) - Int128::mul(p_face->dir0.z * r1, m01) + Int128::mul(p_face->dir1.z * r1, m00) - Int128::mul(p_face->dir1.z * r0, m10) + det * shifted_origin.z,
2019 det);
2020 v->point.x = (int32_t)v->point128.xvalue();
2021 v->point.y = (int32_t)v->point128.yvalue();
2022 v->point.z = (int32_t)v->point128.zvalue();
2023 intersection->target = v;
2024 v->edges = e;
2025
2026 p_stack.push_back(v);
2027 p_stack.push_back(removed);
2028 p_stack.push_back(nullptr);
2029 }
2030
2031 if (cmp || prev_cmp || (prev_intersection->reverse->next->target != intersection->target)) {
2032 face_edge = new_edge_pair(prev_intersection->target, intersection->target);
2033 if (prev_cmp == 0) {
2034 face_edge->link(prev_intersection->reverse->next);
2035 }
2036 if ((prev_cmp == 0) || prev_face_edge) {
2037 prev_intersection->reverse->link(face_edge);
2038 }
2039 if (cmp == 0) {
2040 intersection->reverse->prev->link(face_edge->reverse);
2041 }
2042 face_edge->reverse->link(intersection->reverse);
2043 } else {
2044 face_edge = prev_intersection->reverse->next;
2045 }
2046
2047 if (prev_face_edge) {
2048 if (prev_cmp > 0) {
2049 face_edge->link(prev_face_edge->reverse);
2050 } else if (face_edge != prev_face_edge->reverse) {
2051 p_stack.push_back(prev_face_edge->target);
2052 while (face_edge->next != prev_face_edge->reverse) {
2053 Vertex *removed = face_edge->next->target;
2054 remove_edge_pair(face_edge->next);
2055 p_stack.push_back(removed);
2056#ifdef DEBUG_CONVEX_HULL
2057 printf("2: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z);
2058#endif
2059 }
2060 p_stack.push_back(nullptr);
2061 }
2062 }
2063 face_edge->face = p_face;
2064 face_edge->reverse->face = intersection->face;
2065
2066 if (!first_face_edge) {
2067 first_face_edge = face_edge;
2068 }
2069 }
2070#ifdef SHOW_ITERATIONS
2071 printf("Needed %d iterations to process all intersections\n", m);
2072#endif
2073
2074 if (cmp > 0) {
2075 first_face_edge->reverse->target = face_edge->target;
2076 first_intersection->reverse->link(first_face_edge);
2077 first_face_edge->link(face_edge->reverse);
2078 } else if (first_face_edge != face_edge->reverse) {
2079 p_stack.push_back(face_edge->target);
2080 while (first_face_edge->next != face_edge->reverse) {
2081 Vertex *removed = first_face_edge->next->target;
2082 remove_edge_pair(first_face_edge->next);
2083 p_stack.push_back(removed);
2084#ifdef DEBUG_CONVEX_HULL
2085 printf("3: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z);
2086#endif
2087 }
2088 p_stack.push_back(nullptr);
2089 }
2090
2091 CHULL_ASSERT(p_stack.size() > 0);
2092 vertex_list = p_stack[0];
2093
2094#ifdef DEBUG_CONVEX_HULL
2095 printf("Removing part\n");
2096#endif
2097#ifdef SHOW_ITERATIONS
2098 n = 0;
2099#endif
2100 uint32_t pos = 0;
2101 while (pos < p_stack.size()) {
2102 uint32_t end = p_stack.size();
2103 while (pos < end) {
2104 Vertex *kept = p_stack[pos++];
2105#ifdef DEBUG_CONVEX_HULL
2106 kept->print();
2107#endif
2108 bool deeper = false;
2109 Vertex *removed;
2110 while ((removed = p_stack[pos++]) != nullptr) {
2111#ifdef SHOW_ITERATIONS
2112 n++;
2113#endif
2114 kept->receive_nearby_faces(removed);
2115 while (removed->edges) {
2116 if (!deeper) {
2117 deeper = true;
2118 p_stack.push_back(kept);
2119 }
2120 p_stack.push_back(removed->edges->target);
2121 remove_edge_pair(removed->edges);
2122 }
2123 }
2124 if (deeper) {
2125 p_stack.push_back(nullptr);
2126 }
2127 }
2128 }
2129#ifdef SHOW_ITERATIONS
2130 printf("Needed %d iterations to remove part\n", n);
2131#endif
2132
2133 p_stack.clear();
2134 p_face->origin = shifted_origin;
2135
2136 return true;
2137}
2138
2139static int32_t get_vertex_copy(ConvexHullInternal::Vertex *p_vertex, LocalVector<ConvexHullInternal::Vertex *> &p_vertices) {
2140 int32_t index = p_vertex->copy;
2141 if (index < 0) {
2142 index = p_vertices.size();
2143 p_vertex->copy = index;
2144 p_vertices.push_back(p_vertex);
2145#ifdef DEBUG_CONVEX_HULL
2146 printf("Vertex %d gets index *%d\n", p_vertex->point.index, index);
2147#endif
2148 }
2149 return index;
2150}
2151
2152real_t ConvexHullComputer::compute(const Vector3 *p_coords, int32_t p_count, real_t p_shrink, real_t p_shrink_clamp) {
2153 if (p_count <= 0) {
2154 vertices.clear();
2155 edges.clear();
2156 faces.clear();
2157 return 0;
2158 }
2159
2160 ConvexHullInternal hull;
2161 hull.compute(p_coords, p_count);
2162
2163 real_t shift = 0;
2164 if ((p_shrink > 0) && ((shift = hull.shrink(p_shrink, p_shrink_clamp)) < 0)) {
2165 vertices.clear();
2166 edges.clear();
2167 faces.clear();
2168 return shift;
2169 }
2170
2171 vertices.clear();
2172 edges.clear();
2173 faces.clear();
2174
2175 LocalVector<ConvexHullInternal::Vertex *> old_vertices;
2176 get_vertex_copy(hull.vertex_list, old_vertices);
2177 int32_t copied = 0;
2178 while (copied < (int32_t)old_vertices.size()) {
2179 ConvexHullInternal::Vertex *v = old_vertices[copied];
2180 vertices.push_back(hull.get_coordinates(v));
2181 ConvexHullInternal::Edge *first_edge = v->edges;
2182 if (first_edge) {
2183 int32_t first_copy = -1;
2184 int32_t prev_copy = -1;
2185 ConvexHullInternal::Edge *e = first_edge;
2186 do {
2187 if (e->copy < 0) {
2188 int32_t s = edges.size();
2189 edges.push_back(Edge());
2190 edges.push_back(Edge());
2191 Edge *c = &edges[s];
2192 Edge *r = &edges[s + 1];
2193 e->copy = s;
2194 e->reverse->copy = s + 1;
2195 c->reverse = 1;
2196 r->reverse = -1;
2197 c->target_vertex = get_vertex_copy(e->target, old_vertices);
2198 r->target_vertex = copied;
2199#ifdef DEBUG_CONVEX_HULL
2200 printf(" CREATE: Vertex *%d has edge to *%d\n", copied, c->get_target_vertex());
2201#endif
2202 }
2203 if (prev_copy >= 0) {
2204 edges[e->copy].next = prev_copy - e->copy;
2205 } else {
2206 first_copy = e->copy;
2207 }
2208 prev_copy = e->copy;
2209 e = e->next;
2210 } while (e != first_edge);
2211 edges[first_copy].next = prev_copy - first_copy;
2212 }
2213 copied++;
2214 }
2215
2216 for (int32_t i = 0; i < copied; i++) {
2217 ConvexHullInternal::Vertex *v = old_vertices[i];
2218 ConvexHullInternal::Edge *first_edge = v->edges;
2219 if (first_edge) {
2220 ConvexHullInternal::Edge *e = first_edge;
2221 do {
2222 if (e->copy >= 0) {
2223#ifdef DEBUG_CONVEX_HULL
2224 printf("Vertex *%d has edge to *%d\n", i, edges[e->copy].get_target_vertex());
2225#endif
2226 faces.push_back(e->copy);
2227 ConvexHullInternal::Edge *f = e;
2228 do {
2229#ifdef DEBUG_CONVEX_HULL
2230 printf(" Face *%d\n", edges[f->copy].get_target_vertex());
2231#endif
2232 f->copy = -1;
2233 f = f->reverse->prev;
2234 } while (f != e);
2235 }
2236 e = e->next;
2237 } while (e != first_edge);
2238 }
2239 }
2240
2241 return shift;
2242}
2243
2244Error ConvexHullComputer::convex_hull(const Vector<Vector3> &p_points, Geometry3D::MeshData &r_mesh) {
2245 r_mesh = Geometry3D::MeshData(); // clear
2246
2247 if (p_points.size() == 0) {
2248 return FAILED; // matches QuickHull
2249 }
2250
2251 ConvexHullComputer ch;
2252 ch.compute(p_points.ptr(), p_points.size(), -1.0, -1.0);
2253
2254 r_mesh.vertices = ch.vertices;
2255
2256 // Tag which face each edge belongs to
2257 LocalVector<int32_t> edge_faces;
2258 edge_faces.resize(ch.edges.size());
2259
2260 for (uint32_t i = 0; i < ch.edges.size(); i++) {
2261 edge_faces[i] = -1;
2262 }
2263
2264 for (uint32_t i = 0; i < ch.faces.size(); i++) {
2265 const Edge *e_start = &ch.edges[ch.faces[i]];
2266 const Edge *e = e_start;
2267 do {
2268 int64_t ofs = e - ch.edges.ptr();
2269 edge_faces[ofs] = i;
2270
2271 e = e->get_next_edge_of_face();
2272 } while (e != e_start);
2273 }
2274
2275 // Copy the edges over. There's two "half-edges" for every edge, so we pick only one of them.
2276 r_mesh.edges.resize(ch.edges.size() / 2);
2277 OAHashMap<uint64_t, int32_t> edge_map(ch.edges.size() * 4); // The higher the capacity, the faster the insert
2278
2279 uint32_t edges_copied = 0;
2280 for (uint32_t i = 0; i < ch.edges.size(); i++) {
2281 ERR_CONTINUE(edge_faces[i] == -1); // Sanity check
2282
2283 uint32_t a = (&ch.edges[i])->get_source_vertex();
2284 uint32_t b = (&ch.edges[i])->get_target_vertex();
2285 if (a < b) { // Copy only the "canonical" edge. For the reverse edge, this will be false.
2286 ERR_BREAK(edges_copied >= (uint32_t)r_mesh.edges.size());
2287 r_mesh.edges[edges_copied].vertex_a = a;
2288 r_mesh.edges[edges_copied].vertex_b = b;
2289 r_mesh.edges[edges_copied].face_a = edge_faces[i];
2290 r_mesh.edges[edges_copied].face_b = -1;
2291
2292 uint64_t key = a;
2293 key <<= 32;
2294 key |= b;
2295 edge_map.insert(key, edges_copied);
2296
2297 edges_copied++;
2298 } else {
2299 uint64_t key = b;
2300 key <<= 32;
2301 key |= a;
2302 int32_t index;
2303 if (!edge_map.lookup(key, index)) {
2304 ERR_PRINT("Invalid edge");
2305 } else {
2306 r_mesh.edges[index].face_b = edge_faces[i];
2307 }
2308 }
2309 }
2310
2311 if (edges_copied != (uint32_t)r_mesh.edges.size()) {
2312 ERR_PRINT("Invalid edge count.");
2313 }
2314
2315 r_mesh.faces.resize(ch.faces.size());
2316 for (uint32_t i = 0; i < ch.faces.size(); i++) {
2317 const Edge *e_start = &ch.edges[ch.faces[i]];
2318 const Edge *e = e_start;
2319 Geometry3D::MeshData::Face &face = r_mesh.faces[i];
2320
2321 do {
2322 face.indices.push_back(e->get_target_vertex());
2323
2324 e = e->get_next_edge_of_face();
2325 } while (e != e_start);
2326
2327 // reverse indices: Godot wants clockwise, but this is counter-clockwise
2328 if (face.indices.size() > 2) {
2329 // reverse all but the first index.
2330 int *indices = face.indices.ptr();
2331 for (uint32_t c = 0; c < (face.indices.size() - 1) / 2; c++) {
2332 SWAP(indices[c + 1], indices[face.indices.size() - 1 - c]);
2333 }
2334 }
2335
2336 // compute normal
2337 if (face.indices.size() >= 3) {
2338 face.plane = Plane(r_mesh.vertices[face.indices[0]], r_mesh.vertices[face.indices[1]], r_mesh.vertices[face.indices[2]]);
2339 } else {
2340 WARN_PRINT("Too few vertices per face.");
2341 }
2342 }
2343
2344 return OK;
2345}
2346