1/*
2Copyright (c) 2011 Ole Kniemeyer, MAXON, www.maxon.net
3
4This software is provided 'as-is', without any express or implied warranty.
5In no event will the authors be held liable for any damages arising from the use of this software.
6Permission is granted to anyone to use this software for any purpose,
7including commercial applications, and to alter it and redistribute it freely,
8subject to the following restrictions:
9
101. 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.
112. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
123. This notice may not be removed or altered from any source distribution.
13*/
14
15#include <string.h>
16
17#include "btAlignedObjectArray.h"
18#include "btConvexHullComputer.h"
19#include "btMinMax.h"
20#include "btVector3.h"
21
22#ifdef __GNUC__
23#include <stdint.h>
24#elif defined(_MSC_VER)
25typedef __int32 int32_t;
26typedef __int64 int64_t;
27typedef unsigned __int32 uint32_t;
28typedef unsigned __int64 uint64_t;
29#else
30typedef int32_t int32_t;
31typedef long long int32_t int64_t;
32typedef uint32_t uint32_t;
33typedef unsigned long long int32_t uint64_t;
34#endif
35
36#ifdef _MSC_VER
37#pragma warning(disable:4458)
38#endif
39
40//The definition of USE_X86_64_ASM is moved into the build system. You can enable it manually by commenting out the following lines
41//#if (defined(__GNUC__) && defined(__x86_64__) && !defined(__ICL)) // || (defined(__ICL) && defined(_M_X64)) bug in Intel compiler, disable inline assembly
42// #define USE_X86_64_ASM
43//#endif
44
45//#define DEBUG_CONVEX_HULL
46//#define SHOW_ITERATIONS
47
48#if defined(DEBUG_CONVEX_HULL) || defined(SHOW_ITERATIONS)
49#include <stdio.h>
50#endif
51
52// -- GODOT start --
53namespace VHACD {
54// -- GODOT end --
55
56// Convex hull implementation based on Preparata and Hong
57// Ole Kniemeyer, MAXON Computer GmbH
58class btConvexHullInternal {
59public:
60 class Point64 {
61 public:
62 int64_t x;
63 int64_t y;
64 int64_t z;
65
66 Point64(int64_t x, int64_t y, int64_t z)
67 : x(x)
68 , y(y)
69 , z(z)
70 {
71 }
72
73 bool isZero()
74 {
75 return (x == 0) && (y == 0) && (z == 0);
76 }
77
78 int64_t dot(const Point64& b) const
79 {
80 return x * b.x + y * b.y + z * b.z;
81 }
82 };
83
84 class Point32 {
85 public:
86 int32_t x;
87 int32_t y;
88 int32_t z;
89 int32_t index;
90
91 Point32()
92 {
93 }
94
95 Point32(int32_t x, int32_t y, int32_t z)
96 : x(x)
97 , y(y)
98 , z(z)
99 , index(-1)
100 {
101 }
102
103 bool operator==(const Point32& b) const
104 {
105 return (x == b.x) && (y == b.y) && (z == b.z);
106 }
107
108 bool operator!=(const Point32& b) const
109 {
110 return (x != b.x) || (y != b.y) || (z != b.z);
111 }
112
113 bool isZero()
114 {
115 return (x == 0) && (y == 0) && (z == 0);
116 }
117
118 Point64 cross(const Point32& b) const
119 {
120 return Point64(y * b.z - z * b.y, z * b.x - x * b.z, x * b.y - y * b.x);
121 }
122
123 Point64 cross(const Point64& b) const
124 {
125 return Point64(y * b.z - z * b.y, z * b.x - x * b.z, x * b.y - y * b.x);
126 }
127
128 int64_t dot(const Point32& b) const
129 {
130 return x * b.x + y * b.y + z * b.z;
131 }
132
133 int64_t dot(const Point64& b) const
134 {
135 return x * b.x + y * b.y + z * b.z;
136 }
137
138 Point32 operator+(const Point32& b) const
139 {
140 return Point32(x + b.x, y + b.y, z + b.z);
141 }
142
143 Point32 operator-(const Point32& b) const
144 {
145 return Point32(x - b.x, y - b.y, z - b.z);
146 }
147 };
148
149 class Int128 {
150 public:
151 uint64_t low;
152 uint64_t high;
153
154 Int128()
155 {
156 }
157
158 Int128(uint64_t low, uint64_t high)
159 : low(low)
160 , high(high)
161 {
162 }
163
164 Int128(uint64_t low)
165 : low(low)
166 , high(0)
167 {
168 }
169
170 Int128(int64_t value)
171 : low(value)
172 , high((value >= 0) ? 0 : (uint64_t)-1LL)
173 {
174 }
175
176 static Int128 mul(int64_t a, int64_t b);
177
178 static Int128 mul(uint64_t a, uint64_t b);
179
180 Int128 operator-() const
181 {
182 return Int128((uint64_t) - (int64_t)low, ~high + (low == 0));
183 }
184
185 Int128 operator+(const Int128& b) const
186 {
187#ifdef USE_X86_64_ASM
188 Int128 result;
189 __asm__("addq %[bl], %[rl]\n\t"
190 "adcq %[bh], %[rh]\n\t"
191 : [rl] "=r"(result.low), [rh] "=r"(result.high)
192 : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high)
193 : "cc");
194 return result;
195#else
196 uint64_t lo = low + b.low;
197 return Int128(lo, high + b.high + (lo < low));
198#endif
199 }
200
201 Int128 operator-(const Int128& b) const
202 {
203#ifdef USE_X86_64_ASM
204 Int128 result;
205 __asm__("subq %[bl], %[rl]\n\t"
206 "sbbq %[bh], %[rh]\n\t"
207 : [rl] "=r"(result.low), [rh] "=r"(result.high)
208 : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high)
209 : "cc");
210 return result;
211#else
212 return *this + -b;
213#endif
214 }
215
216 Int128& operator+=(const Int128& b)
217 {
218#ifdef USE_X86_64_ASM
219 __asm__("addq %[bl], %[rl]\n\t"
220 "adcq %[bh], %[rh]\n\t"
221 : [rl] "=r"(low), [rh] "=r"(high)
222 : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high)
223 : "cc");
224#else
225 uint64_t lo = low + b.low;
226 if (lo < low) {
227 ++high;
228 }
229 low = lo;
230 high += b.high;
231#endif
232 return *this;
233 }
234
235 Int128& operator++()
236 {
237 if (++low == 0) {
238 ++high;
239 }
240 return *this;
241 }
242
243 Int128 operator*(int64_t b) const;
244
245 btScalar toScalar() const
246 {
247 return ((int64_t)high >= 0) ? btScalar(high) * (btScalar(0x100000000LL) * btScalar(0x100000000LL)) + btScalar(low)
248 : -(-*this).toScalar();
249 }
250
251 int32_t getSign() const
252 {
253 return ((int64_t)high < 0) ? -1 : (high || low) ? 1 : 0;
254 }
255
256 bool operator<(const Int128& b) const
257 {
258 return (high < b.high) || ((high == b.high) && (low < b.low));
259 }
260
261 int32_t ucmp(const Int128& b) const
262 {
263 if (high < b.high) {
264 return -1;
265 }
266 if (high > b.high) {
267 return 1;
268 }
269 if (low < b.low) {
270 return -1;
271 }
272 if (low > b.low) {
273 return 1;
274 }
275 return 0;
276 }
277 };
278
279 class Rational64 {
280 private:
281 uint64_t m_numerator;
282 uint64_t m_denominator;
283 int32_t sign;
284
285 public:
286 Rational64(int64_t numerator, int64_t denominator)
287 {
288 if (numerator > 0) {
289 sign = 1;
290 m_numerator = (uint64_t)numerator;
291 }
292 else if (numerator < 0) {
293 sign = -1;
294 m_numerator = (uint64_t)-numerator;
295 }
296 else {
297 sign = 0;
298 m_numerator = 0;
299 }
300 if (denominator > 0) {
301 m_denominator = (uint64_t)denominator;
302 }
303 else if (denominator < 0) {
304 sign = -sign;
305 m_denominator = (uint64_t)-denominator;
306 }
307 else {
308 m_denominator = 0;
309 }
310 }
311
312 bool isNegativeInfinity() const
313 {
314 return (sign < 0) && (m_denominator == 0);
315 }
316
317 bool isNaN() const
318 {
319 return (sign == 0) && (m_denominator == 0);
320 }
321
322 int32_t compare(const Rational64& b) const;
323
324 btScalar toScalar() const
325 {
326 return sign * ((m_denominator == 0) ? SIMD_INFINITY : (btScalar)m_numerator / m_denominator);
327 }
328 };
329
330 class Rational128 {
331 private:
332 Int128 numerator;
333 Int128 denominator;
334 int32_t sign;
335 bool isInt64;
336
337 public:
338 Rational128(int64_t value)
339 {
340 if (value > 0) {
341 sign = 1;
342 this->numerator = value;
343 }
344 else if (value < 0) {
345 sign = -1;
346 this->numerator = -value;
347 }
348 else {
349 sign = 0;
350 this->numerator = (uint64_t)0;
351 }
352 this->denominator = (uint64_t)1;
353 isInt64 = true;
354 }
355
356 Rational128(const Int128& numerator, const Int128& denominator)
357 {
358 sign = numerator.getSign();
359 if (sign >= 0) {
360 this->numerator = numerator;
361 }
362 else {
363 this->numerator = -numerator;
364 }
365 int32_t dsign = denominator.getSign();
366 if (dsign >= 0) {
367 this->denominator = denominator;
368 }
369 else {
370 sign = -sign;
371 this->denominator = -denominator;
372 }
373 isInt64 = false;
374 }
375
376 int32_t compare(const Rational128& b) const;
377
378 int32_t compare(int64_t b) const;
379
380 btScalar toScalar() const
381 {
382 return sign * ((denominator.getSign() == 0) ? SIMD_INFINITY : numerator.toScalar() / denominator.toScalar());
383 }
384 };
385
386 class PointR128 {
387 public:
388 Int128 x;
389 Int128 y;
390 Int128 z;
391 Int128 denominator;
392
393 PointR128()
394 {
395 }
396
397 PointR128(Int128 x, Int128 y, Int128 z, Int128 denominator)
398 : x(x)
399 , y(y)
400 , z(z)
401 , denominator(denominator)
402 {
403 }
404
405 btScalar xvalue() const
406 {
407 return x.toScalar() / denominator.toScalar();
408 }
409
410 btScalar yvalue() const
411 {
412 return y.toScalar() / denominator.toScalar();
413 }
414
415 btScalar zvalue() const
416 {
417 return z.toScalar() / denominator.toScalar();
418 }
419 };
420
421 class Edge;
422 class Face;
423
424 class Vertex {
425 public:
426 Vertex* next;
427 Vertex* prev;
428 Edge* edges;
429 Face* firstNearbyFace;
430 Face* lastNearbyFace;
431 PointR128 point128;
432 Point32 point;
433 int32_t copy;
434
435 Vertex()
436 : next(NULL)
437 , prev(NULL)
438 , edges(NULL)
439 , firstNearbyFace(NULL)
440 , lastNearbyFace(NULL)
441 , copy(-1)
442 {
443 }
444
445#ifdef DEBUG_CONVEX_HULL
446 void print()
447 {
448 printf("V%d (%d, %d, %d)", point.index, point.x, point.y, point.z);
449 }
450
451 void printGraph();
452#endif
453
454 Point32 operator-(const Vertex& b) const
455 {
456 return point - b.point;
457 }
458
459 Rational128 dot(const Point64& b) const
460 {
461 return (point.index >= 0) ? Rational128(point.dot(b))
462 : Rational128(point128.x * b.x + point128.y * b.y + point128.z * b.z, point128.denominator);
463 }
464
465 btScalar xvalue() const
466 {
467 return (point.index >= 0) ? btScalar(point.x) : point128.xvalue();
468 }
469
470 btScalar yvalue() const
471 {
472 return (point.index >= 0) ? btScalar(point.y) : point128.yvalue();
473 }
474
475 btScalar zvalue() const
476 {
477 return (point.index >= 0) ? btScalar(point.z) : point128.zvalue();
478 }
479
480 void receiveNearbyFaces(Vertex* src)
481 {
482 if (lastNearbyFace) {
483 lastNearbyFace->nextWithSameNearbyVertex = src->firstNearbyFace;
484 }
485 else {
486 firstNearbyFace = src->firstNearbyFace;
487 }
488 if (src->lastNearbyFace) {
489 lastNearbyFace = src->lastNearbyFace;
490 }
491 for (Face* f = src->firstNearbyFace; f; f = f->nextWithSameNearbyVertex) {
492 btAssert(f->nearbyVertex == src);
493 f->nearbyVertex = this;
494 }
495 src->firstNearbyFace = NULL;
496 src->lastNearbyFace = NULL;
497 }
498 };
499
500 class Edge {
501 public:
502 Edge* next;
503 Edge* prev;
504 Edge* reverse;
505 Vertex* target;
506 Face* face;
507 int32_t copy;
508
509 ~Edge()
510 {
511 next = NULL;
512 prev = NULL;
513 reverse = NULL;
514 target = NULL;
515 face = NULL;
516 }
517
518 void link(Edge* n)
519 {
520 btAssert(reverse->target == n->reverse->target);
521 next = n;
522 n->prev = this;
523 }
524
525#ifdef DEBUG_CONVEX_HULL
526 void print()
527 {
528 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,
529 reverse->target->point.x, reverse->target->point.y, reverse->target->point.z, target->point.x, target->point.y, target->point.z);
530 }
531#endif
532 };
533
534 class Face {
535 public:
536 Face* next;
537 Vertex* nearbyVertex;
538 Face* nextWithSameNearbyVertex;
539 Point32 origin;
540 Point32 dir0;
541 Point32 dir1;
542
543 Face()
544 : next(NULL)
545 , nearbyVertex(NULL)
546 , nextWithSameNearbyVertex(NULL)
547 {
548 }
549
550 void init(Vertex* a, Vertex* b, Vertex* c)
551 {
552 nearbyVertex = a;
553 origin = a->point;
554 dir0 = *b - *a;
555 dir1 = *c - *a;
556 if (a->lastNearbyFace) {
557 a->lastNearbyFace->nextWithSameNearbyVertex = this;
558 }
559 else {
560 a->firstNearbyFace = this;
561 }
562 a->lastNearbyFace = this;
563 }
564
565 Point64 getNormal()
566 {
567 return dir0.cross(dir1);
568 }
569 };
570
571 template <typename UWord, typename UHWord>
572 class DMul {
573 private:
574 static uint32_t high(uint64_t value)
575 {
576 return (uint32_t)(value >> 32);
577 }
578
579 static uint32_t low(uint64_t value)
580 {
581 return (uint32_t)value;
582 }
583
584 static uint64_t mul(uint32_t a, uint32_t b)
585 {
586 return (uint64_t)a * (uint64_t)b;
587 }
588
589 static void shlHalf(uint64_t& value)
590 {
591 value <<= 32;
592 }
593
594 static uint64_t high(Int128 value)
595 {
596 return value.high;
597 }
598
599 static uint64_t low(Int128 value)
600 {
601 return value.low;
602 }
603
604 static Int128 mul(uint64_t a, uint64_t b)
605 {
606 return Int128::mul(a, b);
607 }
608
609 static void shlHalf(Int128& value)
610 {
611 value.high = value.low;
612 value.low = 0;
613 }
614
615 public:
616 static void mul(UWord a, UWord b, UWord& resLow, UWord& resHigh)
617 {
618 UWord p00 = mul(low(a), low(b));
619 UWord p01 = mul(low(a), high(b));
620 UWord p10 = mul(high(a), low(b));
621 UWord p11 = mul(high(a), high(b));
622 UWord p0110 = UWord(low(p01)) + UWord(low(p10));
623 p11 += high(p01);
624 p11 += high(p10);
625 p11 += high(p0110);
626 shlHalf(p0110);
627 p00 += p0110;
628 if (p00 < p0110) {
629 ++p11;
630 }
631 resLow = p00;
632 resHigh = p11;
633 }
634 };
635
636private:
637 class IntermediateHull {
638 public:
639 Vertex* minXy;
640 Vertex* maxXy;
641 Vertex* minYx;
642 Vertex* maxYx;
643
644 IntermediateHull()
645 : minXy(NULL)
646 , maxXy(NULL)
647 , minYx(NULL)
648 , maxYx(NULL)
649 {
650 }
651
652 void print();
653 };
654
655 enum Orientation { NONE,
656 CLOCKWISE,
657 COUNTER_CLOCKWISE };
658
659 template <typename T>
660 class PoolArray {
661 private:
662 T* array;
663 int32_t size;
664
665 public:
666 PoolArray<T>* next;
667
668 PoolArray(int32_t size)
669 : size(size)
670 , next(NULL)
671 {
672 array = (T*)btAlignedAlloc(sizeof(T) * size, 16);
673 }
674
675 ~PoolArray()
676 {
677 btAlignedFree(array);
678 }
679
680 T* init()
681 {
682 T* o = array;
683 for (int32_t i = 0; i < size; i++, o++) {
684 o->next = (i + 1 < size) ? o + 1 : NULL;
685 }
686 return array;
687 }
688 };
689
690 template <typename T>
691 class Pool {
692 private:
693 PoolArray<T>* arrays;
694 PoolArray<T>* nextArray;
695 T* freeObjects;
696 int32_t arraySize;
697
698 public:
699 Pool()
700 : arrays(NULL)
701 , nextArray(NULL)
702 , freeObjects(NULL)
703 , arraySize(256)
704 {
705 }
706
707 ~Pool()
708 {
709 while (arrays) {
710 PoolArray<T>* p = arrays;
711 arrays = p->next;
712 p->~PoolArray<T>();
713 btAlignedFree(p);
714 }
715 }
716
717 void reset()
718 {
719 nextArray = arrays;
720 freeObjects = NULL;
721 }
722
723 void setArraySize(int32_t arraySize)
724 {
725 this->arraySize = arraySize;
726 }
727
728 T* newObject()
729 {
730 T* o = freeObjects;
731 if (!o) {
732 PoolArray<T>* p = nextArray;
733 if (p) {
734 nextArray = p->next;
735 }
736 else {
737 p = new (btAlignedAlloc(sizeof(PoolArray<T>), 16)) PoolArray<T>(arraySize);
738 p->next = arrays;
739 arrays = p;
740 }
741 o = p->init();
742 }
743 freeObjects = o->next;
744 return new (o) T();
745 };
746
747 void freeObject(T* object)
748 {
749 object->~T();
750 object->next = freeObjects;
751 freeObjects = object;
752 }
753 };
754
755 btVector3 scaling;
756 btVector3 center;
757 Pool<Vertex> vertexPool;
758 Pool<Edge> edgePool;
759 Pool<Face> facePool;
760 btAlignedObjectArray<Vertex*> originalVertices;
761 int32_t mergeStamp;
762 int32_t minAxis;
763 int32_t medAxis;
764 int32_t maxAxis;
765 int32_t usedEdgePairs;
766 int32_t maxUsedEdgePairs;
767
768 static Orientation getOrientation(const Edge* prev, const Edge* next, const Point32& s, const Point32& t);
769 Edge* findMaxAngle(bool ccw, const Vertex* start, const Point32& s, const Point64& rxs, const Point64& sxrxs, Rational64& minCot);
770 void findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge*& e0, Edge*& e1, Vertex* stop0, Vertex* stop1);
771
772 Edge* newEdgePair(Vertex* from, Vertex* to);
773
774 void removeEdgePair(Edge* edge)
775 {
776 Edge* n = edge->next;
777 Edge* r = edge->reverse;
778
779 btAssert(edge->target && r->target);
780
781 if (n != edge) {
782 n->prev = edge->prev;
783 edge->prev->next = n;
784 r->target->edges = n;
785 }
786 else {
787 r->target->edges = NULL;
788 }
789
790 n = r->next;
791
792 if (n != r) {
793 n->prev = r->prev;
794 r->prev->next = n;
795 edge->target->edges = n;
796 }
797 else {
798 edge->target->edges = NULL;
799 }
800
801 edgePool.freeObject(edge);
802 edgePool.freeObject(r);
803 usedEdgePairs--;
804 }
805
806 void computeInternal(int32_t start, int32_t end, IntermediateHull& result);
807
808 bool mergeProjection(IntermediateHull& h0, IntermediateHull& h1, Vertex*& c0, Vertex*& c1);
809
810 void merge(IntermediateHull& h0, IntermediateHull& h1);
811
812 btVector3 toBtVector(const Point32& v);
813
814 btVector3 getBtNormal(Face* face);
815
816 bool shiftFace(Face* face, btScalar amount, btAlignedObjectArray<Vertex*> stack);
817
818public:
819 Vertex* vertexList;
820
821 void compute(const void* coords, bool doubleCoords, int32_t stride, int32_t count);
822
823 btVector3 getCoordinates(const Vertex* v);
824
825 btScalar shrink(btScalar amount, btScalar clampAmount);
826};
827
828btConvexHullInternal::Int128 btConvexHullInternal::Int128::operator*(int64_t b) const
829{
830 bool negative = (int64_t)high < 0;
831 Int128 a = negative ? -*this : *this;
832 if (b < 0) {
833 negative = !negative;
834 b = -b;
835 }
836 Int128 result = mul(a.low, (uint64_t)b);
837 result.high += a.high * (uint64_t)b;
838 return negative ? -result : result;
839}
840
841btConvexHullInternal::Int128 btConvexHullInternal::Int128::mul(int64_t a, int64_t b)
842{
843 Int128 result;
844
845#ifdef USE_X86_64_ASM
846 __asm__("imulq %[b]"
847 : "=a"(result.low), "=d"(result.high)
848 : "0"(a), [b] "r"(b)
849 : "cc");
850 return result;
851
852#else
853 bool negative = a < 0;
854 if (negative) {
855 a = -a;
856 }
857 if (b < 0) {
858 negative = !negative;
859 b = -b;
860 }
861 DMul<uint64_t, uint32_t>::mul((uint64_t)a, (uint64_t)b, result.low, result.high);
862 return negative ? -result : result;
863#endif
864}
865
866btConvexHullInternal::Int128 btConvexHullInternal::Int128::mul(uint64_t a, uint64_t b)
867{
868 Int128 result;
869
870#ifdef USE_X86_64_ASM
871 __asm__("mulq %[b]"
872 : "=a"(result.low), "=d"(result.high)
873 : "0"(a), [b] "r"(b)
874 : "cc");
875
876#else
877 DMul<uint64_t, uint32_t>::mul(a, b, result.low, result.high);
878#endif
879
880 return result;
881}
882
883int32_t btConvexHullInternal::Rational64::compare(const Rational64& b) const
884{
885 if (sign != b.sign) {
886 return sign - b.sign;
887 }
888 else if (sign == 0) {
889 return 0;
890 }
891
892// return (numerator * b.denominator > b.numerator * denominator) ? sign : (numerator * b.denominator < b.numerator * denominator) ? -sign : 0;
893
894#ifdef USE_X86_64_ASM
895
896 int32_t result;
897 int64_t tmp;
898 int64_t dummy;
899 __asm__("mulq %[bn]\n\t"
900 "movq %%rax, %[tmp]\n\t"
901 "movq %%rdx, %%rbx\n\t"
902 "movq %[tn], %%rax\n\t"
903 "mulq %[bd]\n\t"
904 "subq %[tmp], %%rax\n\t"
905 "sbbq %%rbx, %%rdx\n\t" // rdx:rax contains 128-bit-difference "numerator*b.denominator - b.numerator*denominator"
906 "setnsb %%bh\n\t" // bh=1 if difference is non-negative, bh=0 otherwise
907 "orq %%rdx, %%rax\n\t"
908 "setnzb %%bl\n\t" // bl=1 if difference if non-zero, bl=0 if it is zero
909 "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)
910 "shll $16, %%ebx\n\t" // ebx has same sign as difference
911 : "=&b"(result), [tmp] "=&r"(tmp), "=a"(dummy)
912 : "a"(denominator), [bn] "g"(b.numerator), [tn] "g"(numerator), [bd] "g"(b.denominator)
913 : "%rdx", "cc");
914 return result ? result ^ sign // if sign is +1, only bit 0 of result is inverted, which does not change the sign of result (and cannot result in zero)
915 // if sign is -1, all bits of result are inverted, which changes the sign of result (and again cannot result in zero)
916 : 0;
917
918#else
919
920 return sign * Int128::mul(m_numerator, b.m_denominator).ucmp(Int128::mul(m_denominator, b.m_numerator));
921
922#endif
923}
924
925int32_t btConvexHullInternal::Rational128::compare(const Rational128& b) const
926{
927 if (sign != b.sign) {
928 return sign - b.sign;
929 }
930 else if (sign == 0) {
931 return 0;
932 }
933 if (isInt64) {
934 return -b.compare(sign * (int64_t)numerator.low);
935 }
936
937 Int128 nbdLow, nbdHigh, dbnLow, dbnHigh;
938 DMul<Int128, uint64_t>::mul(numerator, b.denominator, nbdLow, nbdHigh);
939 DMul<Int128, uint64_t>::mul(denominator, b.numerator, dbnLow, dbnHigh);
940
941 int32_t cmp = nbdHigh.ucmp(dbnHigh);
942 if (cmp) {
943 return cmp * sign;
944 }
945 return nbdLow.ucmp(dbnLow) * sign;
946}
947
948int32_t btConvexHullInternal::Rational128::compare(int64_t b) const
949{
950 if (isInt64) {
951 int64_t a = sign * (int64_t)numerator.low;
952 return (a > b) ? 1 : (a < b) ? -1 : 0;
953 }
954 if (b > 0) {
955 if (sign <= 0) {
956 return -1;
957 }
958 }
959 else if (b < 0) {
960 if (sign >= 0) {
961 return 1;
962 }
963 b = -b;
964 }
965 else {
966 return sign;
967 }
968
969 return numerator.ucmp(denominator * b) * sign;
970}
971
972btConvexHullInternal::Edge* btConvexHullInternal::newEdgePair(Vertex* from, Vertex* to)
973{
974 btAssert(from && to);
975 Edge* e = edgePool.newObject();
976 Edge* r = edgePool.newObject();
977 e->reverse = r;
978 r->reverse = e;
979 e->copy = mergeStamp;
980 r->copy = mergeStamp;
981 e->target = to;
982 r->target = from;
983 e->face = NULL;
984 r->face = NULL;
985 usedEdgePairs++;
986 if (usedEdgePairs > maxUsedEdgePairs) {
987 maxUsedEdgePairs = usedEdgePairs;
988 }
989 return e;
990}
991
992bool btConvexHullInternal::mergeProjection(IntermediateHull& h0, IntermediateHull& h1, Vertex*& c0, Vertex*& c1)
993{
994 Vertex* v0 = h0.maxYx;
995 Vertex* v1 = h1.minYx;
996 if ((v0->point.x == v1->point.x) && (v0->point.y == v1->point.y)) {
997 btAssert(v0->point.z < v1->point.z);
998 Vertex* v1p = v1->prev;
999 if (v1p == v1) {
1000 c0 = v0;
1001 if (v1->edges) {
1002 btAssert(v1->edges->next == v1->edges);
1003 v1 = v1->edges->target;
1004 btAssert(v1->edges->next == v1->edges);
1005 }
1006 c1 = v1;
1007 return false;
1008 }
1009 Vertex* v1n = v1->next;
1010 v1p->next = v1n;
1011 v1n->prev = v1p;
1012 if (v1 == h1.minXy) {
1013 if ((v1n->point.x < v1p->point.x) || ((v1n->point.x == v1p->point.x) && (v1n->point.y < v1p->point.y))) {
1014 h1.minXy = v1n;
1015 }
1016 else {
1017 h1.minXy = v1p;
1018 }
1019 }
1020 if (v1 == h1.maxXy) {
1021 if ((v1n->point.x > v1p->point.x) || ((v1n->point.x == v1p->point.x) && (v1n->point.y > v1p->point.y))) {
1022 h1.maxXy = v1n;
1023 }
1024 else {
1025 h1.maxXy = v1p;
1026 }
1027 }
1028 }
1029
1030 v0 = h0.maxXy;
1031 v1 = h1.maxXy;
1032 Vertex* v00 = NULL;
1033 Vertex* v10 = NULL;
1034 int32_t sign = 1;
1035
1036 for (int32_t side = 0; side <= 1; side++) {
1037 int32_t dx = (v1->point.x - v0->point.x) * sign;
1038 if (dx > 0) {
1039 while (true) {
1040 int32_t dy = v1->point.y - v0->point.y;
1041
1042 Vertex* w0 = side ? v0->next : v0->prev;
1043 if (w0 != v0) {
1044 int32_t dx0 = (w0->point.x - v0->point.x) * sign;
1045 int32_t dy0 = w0->point.y - v0->point.y;
1046 if ((dy0 <= 0) && ((dx0 == 0) || ((dx0 < 0) && (dy0 * dx <= dy * dx0)))) {
1047 v0 = w0;
1048 dx = (v1->point.x - v0->point.x) * sign;
1049 continue;
1050 }
1051 }
1052
1053 Vertex* w1 = side ? v1->next : v1->prev;
1054 if (w1 != v1) {
1055 int32_t dx1 = (w1->point.x - v1->point.x) * sign;
1056 int32_t dy1 = w1->point.y - v1->point.y;
1057 int32_t dxn = (w1->point.x - v0->point.x) * sign;
1058 if ((dxn > 0) && (dy1 < 0) && ((dx1 == 0) || ((dx1 < 0) && (dy1 * dx < dy * dx1)))) {
1059 v1 = w1;
1060 dx = dxn;
1061 continue;
1062 }
1063 }
1064
1065 break;
1066 }
1067 }
1068 else if (dx < 0) {
1069 while (true) {
1070 int32_t dy = v1->point.y - v0->point.y;
1071
1072 Vertex* w1 = side ? v1->prev : v1->next;
1073 if (w1 != v1) {
1074 int32_t dx1 = (w1->point.x - v1->point.x) * sign;
1075 int32_t dy1 = w1->point.y - v1->point.y;
1076 if ((dy1 >= 0) && ((dx1 == 0) || ((dx1 < 0) && (dy1 * dx <= dy * dx1)))) {
1077 v1 = w1;
1078 dx = (v1->point.x - v0->point.x) * sign;
1079 continue;
1080 }
1081 }
1082
1083 Vertex* w0 = side ? v0->prev : v0->next;
1084 if (w0 != v0) {
1085 int32_t dx0 = (w0->point.x - v0->point.x) * sign;
1086 int32_t dy0 = w0->point.y - v0->point.y;
1087 int32_t dxn = (v1->point.x - w0->point.x) * sign;
1088 if ((dxn < 0) && (dy0 > 0) && ((dx0 == 0) || ((dx0 < 0) && (dy0 * dx < dy * dx0)))) {
1089 v0 = w0;
1090 dx = dxn;
1091 continue;
1092 }
1093 }
1094
1095 break;
1096 }
1097 }
1098 else {
1099 int32_t x = v0->point.x;
1100 int32_t y0 = v0->point.y;
1101 Vertex* w0 = v0;
1102 Vertex* t;
1103 while (((t = side ? w0->next : w0->prev) != v0) && (t->point.x == x) && (t->point.y <= y0)) {
1104 w0 = t;
1105 y0 = t->point.y;
1106 }
1107 v0 = w0;
1108
1109 int32_t y1 = v1->point.y;
1110 Vertex* w1 = v1;
1111 while (((t = side ? w1->prev : w1->next) != v1) && (t->point.x == x) && (t->point.y >= y1)) {
1112 w1 = t;
1113 y1 = t->point.y;
1114 }
1115 v1 = w1;
1116 }
1117
1118 if (side == 0) {
1119 v00 = v0;
1120 v10 = v1;
1121
1122 v0 = h0.minXy;
1123 v1 = h1.minXy;
1124 sign = -1;
1125 }
1126 }
1127
1128 v0->prev = v1;
1129 v1->next = v0;
1130
1131 v00->next = v10;
1132 v10->prev = v00;
1133
1134 if (h1.minXy->point.x < h0.minXy->point.x) {
1135 h0.minXy = h1.minXy;
1136 }
1137 if (h1.maxXy->point.x >= h0.maxXy->point.x) {
1138 h0.maxXy = h1.maxXy;
1139 }
1140
1141 h0.maxYx = h1.maxYx;
1142
1143 c0 = v00;
1144 c1 = v10;
1145
1146 return true;
1147}
1148
1149void btConvexHullInternal::computeInternal(int32_t start, int32_t end, IntermediateHull& result)
1150{
1151 int32_t n = end - start;
1152 switch (n) {
1153 case 0:
1154 result.minXy = NULL;
1155 result.maxXy = NULL;
1156 result.minYx = NULL;
1157 result.maxYx = NULL;
1158 return;
1159 case 2: {
1160 Vertex* v = originalVertices[start];
1161 Vertex* w = v + 1;
1162 if (v->point != w->point) {
1163 int32_t dx = v->point.x - w->point.x;
1164 int32_t dy = v->point.y - w->point.y;
1165
1166 if ((dx == 0) && (dy == 0)) {
1167 if (v->point.z > w->point.z) {
1168 Vertex* t = w;
1169 w = v;
1170 v = t;
1171 }
1172 btAssert(v->point.z < w->point.z);
1173 v->next = v;
1174 v->prev = v;
1175 result.minXy = v;
1176 result.maxXy = v;
1177 result.minYx = v;
1178 result.maxYx = v;
1179 }
1180 else {
1181 v->next = w;
1182 v->prev = w;
1183 w->next = v;
1184 w->prev = v;
1185
1186 if ((dx < 0) || ((dx == 0) && (dy < 0))) {
1187 result.minXy = v;
1188 result.maxXy = w;
1189 }
1190 else {
1191 result.minXy = w;
1192 result.maxXy = v;
1193 }
1194
1195 if ((dy < 0) || ((dy == 0) && (dx < 0))) {
1196 result.minYx = v;
1197 result.maxYx = w;
1198 }
1199 else {
1200 result.minYx = w;
1201 result.maxYx = v;
1202 }
1203 }
1204
1205 Edge* e = newEdgePair(v, w);
1206 e->link(e);
1207 v->edges = e;
1208
1209 e = e->reverse;
1210 e->link(e);
1211 w->edges = e;
1212
1213 return;
1214 }
1215 }
1216 // lint -fallthrough
1217 case 1: {
1218 Vertex* v = originalVertices[start];
1219 v->edges = NULL;
1220 v->next = v;
1221 v->prev = v;
1222
1223 result.minXy = v;
1224 result.maxXy = v;
1225 result.minYx = v;
1226 result.maxYx = v;
1227
1228 return;
1229 }
1230 }
1231
1232 int32_t split0 = start + n / 2;
1233 Point32 p = originalVertices[split0 - 1]->point;
1234 int32_t split1 = split0;
1235 while ((split1 < end) && (originalVertices[split1]->point == p)) {
1236 split1++;
1237 }
1238 computeInternal(start, split0, result);
1239 IntermediateHull hull1;
1240 computeInternal(split1, end, hull1);
1241#ifdef DEBUG_CONVEX_HULL
1242 printf("\n\nMerge\n");
1243 result.print();
1244 hull1.print();
1245#endif
1246 merge(result, hull1);
1247#ifdef DEBUG_CONVEX_HULL
1248 printf("\n Result\n");
1249 result.print();
1250#endif
1251}
1252
1253#ifdef DEBUG_CONVEX_HULL
1254void btConvexHullInternal::IntermediateHull::print()
1255{
1256 printf(" Hull\n");
1257 for (Vertex* v = minXy; v;) {
1258 printf(" ");
1259 v->print();
1260 if (v == maxXy) {
1261 printf(" maxXy");
1262 }
1263 if (v == minYx) {
1264 printf(" minYx");
1265 }
1266 if (v == maxYx) {
1267 printf(" maxYx");
1268 }
1269 if (v->next->prev != v) {
1270 printf(" Inconsistency");
1271 }
1272 printf("\n");
1273 v = v->next;
1274 if (v == minXy) {
1275 break;
1276 }
1277 }
1278 if (minXy) {
1279 minXy->copy = (minXy->copy == -1) ? -2 : -1;
1280 minXy->printGraph();
1281 }
1282}
1283
1284void btConvexHullInternal::Vertex::printGraph()
1285{
1286 print();
1287 printf("\nEdges\n");
1288 Edge* e = edges;
1289 if (e) {
1290 do {
1291 e->print();
1292 printf("\n");
1293 e = e->next;
1294 } while (e != edges);
1295 do {
1296 Vertex* v = e->target;
1297 if (v->copy != copy) {
1298 v->copy = copy;
1299 v->printGraph();
1300 }
1301 e = e->next;
1302 } while (e != edges);
1303 }
1304}
1305#endif
1306
1307btConvexHullInternal::Orientation btConvexHullInternal::getOrientation(const Edge* prev, const Edge* next, const Point32& s, const Point32& t)
1308{
1309 btAssert(prev->reverse->target == next->reverse->target);
1310 if (prev->next == next) {
1311 if (prev->prev == next) {
1312 Point64 n = t.cross(s);
1313 Point64 m = (*prev->target - *next->reverse->target).cross(*next->target - *next->reverse->target);
1314 btAssert(!m.isZero());
1315 int64_t dot = n.dot(m);
1316 btAssert(dot != 0);
1317 return (dot > 0) ? COUNTER_CLOCKWISE : CLOCKWISE;
1318 }
1319 return COUNTER_CLOCKWISE;
1320 }
1321 else if (prev->prev == next) {
1322 return CLOCKWISE;
1323 }
1324 else {
1325 return NONE;
1326 }
1327}
1328
1329btConvexHullInternal::Edge* btConvexHullInternal::findMaxAngle(bool ccw, const Vertex* start, const Point32& s, const Point64& rxs, const Point64& sxrxs, Rational64& minCot)
1330{
1331 Edge* minEdge = NULL;
1332
1333#ifdef DEBUG_CONVEX_HULL
1334 printf("find max edge for %d\n", start->point.index);
1335#endif
1336 Edge* e = start->edges;
1337 if (e) {
1338 do {
1339 if (e->copy > mergeStamp) {
1340 Point32 t = *e->target - *start;
1341 Rational64 cot(t.dot(sxrxs), t.dot(rxs));
1342#ifdef DEBUG_CONVEX_HULL
1343 printf(" Angle is %f (%d) for ", (float)btAtan(cot.toScalar()), (int32_t)cot.isNaN());
1344 e->print();
1345#endif
1346 if (cot.isNaN()) {
1347 btAssert(ccw ? (t.dot(s) < 0) : (t.dot(s) > 0));
1348 }
1349 else {
1350 int32_t cmp;
1351 if (minEdge == NULL) {
1352 minCot = cot;
1353 minEdge = e;
1354 }
1355 else if ((cmp = cot.compare(minCot)) < 0) {
1356 minCot = cot;
1357 minEdge = e;
1358 }
1359 else if ((cmp == 0) && (ccw == (getOrientation(minEdge, e, s, t) == COUNTER_CLOCKWISE))) {
1360 minEdge = e;
1361 }
1362 }
1363#ifdef DEBUG_CONVEX_HULL
1364 printf("\n");
1365#endif
1366 }
1367 e = e->next;
1368 } while (e != start->edges);
1369 }
1370 return minEdge;
1371}
1372
1373void btConvexHullInternal::findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge*& e0, Edge*& e1, Vertex* stop0, Vertex* stop1)
1374{
1375 Edge* start0 = e0;
1376 Edge* start1 = e1;
1377 Point32 et0 = start0 ? start0->target->point : c0->point;
1378 Point32 et1 = start1 ? start1->target->point : c1->point;
1379 Point32 s = c1->point - c0->point;
1380 Point64 normal = ((start0 ? start0 : start1)->target->point - c0->point).cross(s);
1381 int64_t dist = c0->point.dot(normal);
1382 btAssert(!start1 || (start1->target->point.dot(normal) == dist));
1383 Point64 perp = s.cross(normal);
1384 btAssert(!perp.isZero());
1385
1386#ifdef DEBUG_CONVEX_HULL
1387 printf(" Advancing %d %d (%p %p, %d %d)\n", c0->point.index, c1->point.index, start0, start1, start0 ? start0->target->point.index : -1, start1 ? start1->target->point.index : -1);
1388#endif
1389
1390 int64_t maxDot0 = et0.dot(perp);
1391 if (e0) {
1392 while (e0->target != stop0) {
1393 Edge* e = e0->reverse->prev;
1394 if (e->target->point.dot(normal) < dist) {
1395 break;
1396 }
1397 btAssert(e->target->point.dot(normal) == dist);
1398 if (e->copy == mergeStamp) {
1399 break;
1400 }
1401 int64_t dot = e->target->point.dot(perp);
1402 if (dot <= maxDot0) {
1403 break;
1404 }
1405 maxDot0 = dot;
1406 e0 = e;
1407 et0 = e->target->point;
1408 }
1409 }
1410
1411 int64_t maxDot1 = et1.dot(perp);
1412 if (e1) {
1413 while (e1->target != stop1) {
1414 Edge* e = e1->reverse->next;
1415 if (e->target->point.dot(normal) < dist) {
1416 break;
1417 }
1418 btAssert(e->target->point.dot(normal) == dist);
1419 if (e->copy == mergeStamp) {
1420 break;
1421 }
1422 int64_t dot = e->target->point.dot(perp);
1423 if (dot <= maxDot1) {
1424 break;
1425 }
1426 maxDot1 = dot;
1427 e1 = e;
1428 et1 = e->target->point;
1429 }
1430 }
1431
1432#ifdef DEBUG_CONVEX_HULL
1433 printf(" Starting at %d %d\n", et0.index, et1.index);
1434#endif
1435
1436 int64_t dx = maxDot1 - maxDot0;
1437 if (dx > 0) {
1438 while (true) {
1439 int64_t dy = (et1 - et0).dot(s);
1440
1441 if (e0 && (e0->target != stop0)) {
1442 Edge* f0 = e0->next->reverse;
1443 if (f0->copy > mergeStamp) {
1444 int64_t dx0 = (f0->target->point - et0).dot(perp);
1445 int64_t dy0 = (f0->target->point - et0).dot(s);
1446 if ((dx0 == 0) ? (dy0 < 0) : ((dx0 < 0) && (Rational64(dy0, dx0).compare(Rational64(dy, dx)) >= 0))) {
1447 et0 = f0->target->point;
1448 dx = (et1 - et0).dot(perp);
1449 e0 = (e0 == start0) ? NULL : f0;
1450 continue;
1451 }
1452 }
1453 }
1454
1455 if (e1 && (e1->target != stop1)) {
1456 Edge* f1 = e1->reverse->next;
1457 if (f1->copy > mergeStamp) {
1458 Point32 d1 = f1->target->point - et1;
1459 if (d1.dot(normal) == 0) {
1460 int64_t dx1 = d1.dot(perp);
1461 int64_t dy1 = d1.dot(s);
1462 int64_t dxn = (f1->target->point - et0).dot(perp);
1463 if ((dxn > 0) && ((dx1 == 0) ? (dy1 < 0) : ((dx1 < 0) && (Rational64(dy1, dx1).compare(Rational64(dy, dx)) > 0)))) {
1464 e1 = f1;
1465 et1 = e1->target->point;
1466 dx = dxn;
1467 continue;
1468 }
1469 }
1470 else {
1471 btAssert((e1 == start1) && (d1.dot(normal) < 0));
1472 }
1473 }
1474 }
1475
1476 break;
1477 }
1478 }
1479 else if (dx < 0) {
1480 while (true) {
1481 int64_t dy = (et1 - et0).dot(s);
1482
1483 if (e1 && (e1->target != stop1)) {
1484 Edge* f1 = e1->prev->reverse;
1485 if (f1->copy > mergeStamp) {
1486 int64_t dx1 = (f1->target->point - et1).dot(perp);
1487 int64_t dy1 = (f1->target->point - et1).dot(s);
1488 if ((dx1 == 0) ? (dy1 > 0) : ((dx1 < 0) && (Rational64(dy1, dx1).compare(Rational64(dy, dx)) <= 0))) {
1489 et1 = f1->target->point;
1490 dx = (et1 - et0).dot(perp);
1491 e1 = (e1 == start1) ? NULL : f1;
1492 continue;
1493 }
1494 }
1495 }
1496
1497 if (e0 && (e0->target != stop0)) {
1498 Edge* f0 = e0->reverse->prev;
1499 if (f0->copy > mergeStamp) {
1500 Point32 d0 = f0->target->point - et0;
1501 if (d0.dot(normal) == 0) {
1502 int64_t dx0 = d0.dot(perp);
1503 int64_t dy0 = d0.dot(s);
1504 int64_t dxn = (et1 - f0->target->point).dot(perp);
1505 if ((dxn < 0) && ((dx0 == 0) ? (dy0 > 0) : ((dx0 < 0) && (Rational64(dy0, dx0).compare(Rational64(dy, dx)) < 0)))) {
1506 e0 = f0;
1507 et0 = e0->target->point;
1508 dx = dxn;
1509 continue;
1510 }
1511 }
1512 else {
1513 btAssert((e0 == start0) && (d0.dot(normal) < 0));
1514 }
1515 }
1516 }
1517
1518 break;
1519 }
1520 }
1521#ifdef DEBUG_CONVEX_HULL
1522 printf(" Advanced edges to %d %d\n", et0.index, et1.index);
1523#endif
1524}
1525
1526void btConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1)
1527{
1528 if (!h1.maxXy) {
1529 return;
1530 }
1531 if (!h0.maxXy) {
1532 h0 = h1;
1533 return;
1534 }
1535
1536 mergeStamp--;
1537
1538 Vertex* c0 = NULL;
1539 Edge* toPrev0 = NULL;
1540 Edge* firstNew0 = NULL;
1541 Edge* pendingHead0 = NULL;
1542 Edge* pendingTail0 = NULL;
1543 Vertex* c1 = NULL;
1544 Edge* toPrev1 = NULL;
1545 Edge* firstNew1 = NULL;
1546 Edge* pendingHead1 = NULL;
1547 Edge* pendingTail1 = NULL;
1548 Point32 prevPoint;
1549
1550 if (mergeProjection(h0, h1, c0, c1)) {
1551 Point32 s = *c1 - *c0;
1552 Point64 normal = Point32(0, 0, -1).cross(s);
1553 Point64 t = s.cross(normal);
1554 btAssert(!t.isZero());
1555
1556 Edge* e = c0->edges;
1557 Edge* start0 = NULL;
1558 if (e) {
1559 do {
1560 int64_t dot = (*e->target - *c0).dot(normal);
1561 btAssert(dot <= 0);
1562 if ((dot == 0) && ((*e->target - *c0).dot(t) > 0)) {
1563 if (!start0 || (getOrientation(start0, e, s, Point32(0, 0, -1)) == CLOCKWISE)) {
1564 start0 = e;
1565 }
1566 }
1567 e = e->next;
1568 } while (e != c0->edges);
1569 }
1570
1571 e = c1->edges;
1572 Edge* start1 = NULL;
1573 if (e) {
1574 do {
1575 int64_t dot = (*e->target - *c1).dot(normal);
1576 btAssert(dot <= 0);
1577 if ((dot == 0) && ((*e->target - *c1).dot(t) > 0)) {
1578 if (!start1 || (getOrientation(start1, e, s, Point32(0, 0, -1)) == COUNTER_CLOCKWISE)) {
1579 start1 = e;
1580 }
1581 }
1582 e = e->next;
1583 } while (e != c1->edges);
1584 }
1585
1586 if (start0 || start1) {
1587 findEdgeForCoplanarFaces(c0, c1, start0, start1, NULL, NULL);
1588 if (start0) {
1589 c0 = start0->target;
1590 }
1591 if (start1) {
1592 c1 = start1->target;
1593 }
1594 }
1595
1596 prevPoint = c1->point;
1597 prevPoint.z++;
1598 }
1599 else {
1600 prevPoint = c1->point;
1601 prevPoint.x++;
1602 }
1603
1604 Vertex* first0 = c0;
1605 Vertex* first1 = c1;
1606 bool firstRun = true;
1607
1608 while (true) {
1609 Point32 s = *c1 - *c0;
1610 Point32 r = prevPoint - c0->point;
1611 Point64 rxs = r.cross(s);
1612 Point64 sxrxs = s.cross(rxs);
1613
1614#ifdef DEBUG_CONVEX_HULL
1615 printf("\n Checking %d %d\n", c0->point.index, c1->point.index);
1616#endif
1617 Rational64 minCot0(0, 0);
1618 Edge* min0 = findMaxAngle(false, c0, s, rxs, sxrxs, minCot0);
1619 Rational64 minCot1(0, 0);
1620 Edge* min1 = findMaxAngle(true, c1, s, rxs, sxrxs, minCot1);
1621 if (!min0 && !min1) {
1622 Edge* e = newEdgePair(c0, c1);
1623 e->link(e);
1624 c0->edges = e;
1625
1626 e = e->reverse;
1627 e->link(e);
1628 c1->edges = e;
1629 return;
1630 }
1631 else {
1632 int32_t cmp = !min0 ? 1 : !min1 ? -1 : minCot0.compare(minCot1);
1633#ifdef DEBUG_CONVEX_HULL
1634 printf(" -> Result %d\n", cmp);
1635#endif
1636 if (firstRun || ((cmp >= 0) ? !minCot1.isNegativeInfinity() : !minCot0.isNegativeInfinity())) {
1637 Edge* e = newEdgePair(c0, c1);
1638 if (pendingTail0) {
1639 pendingTail0->prev = e;
1640 }
1641 else {
1642 pendingHead0 = e;
1643 }
1644 e->next = pendingTail0;
1645 pendingTail0 = e;
1646
1647 e = e->reverse;
1648 if (pendingTail1) {
1649 pendingTail1->next = e;
1650 }
1651 else {
1652 pendingHead1 = e;
1653 }
1654 e->prev = pendingTail1;
1655 pendingTail1 = e;
1656 }
1657
1658 Edge* e0 = min0;
1659 Edge* e1 = min1;
1660
1661#ifdef DEBUG_CONVEX_HULL
1662 printf(" Found min edges to %d %d\n", e0 ? e0->target->point.index : -1, e1 ? e1->target->point.index : -1);
1663#endif
1664
1665 if (cmp == 0) {
1666 findEdgeForCoplanarFaces(c0, c1, e0, e1, NULL, NULL);
1667 }
1668
1669 if ((cmp >= 0) && e1) {
1670 if (toPrev1) {
1671 for (Edge *e = toPrev1->next, *n = NULL; e != min1; e = n) {
1672 n = e->next;
1673 removeEdgePair(e);
1674 }
1675 }
1676
1677 if (pendingTail1) {
1678 if (toPrev1) {
1679 toPrev1->link(pendingHead1);
1680 }
1681 else {
1682 min1->prev->link(pendingHead1);
1683 firstNew1 = pendingHead1;
1684 }
1685 pendingTail1->link(min1);
1686 pendingHead1 = NULL;
1687 pendingTail1 = NULL;
1688 }
1689 else if (!toPrev1) {
1690 firstNew1 = min1;
1691 }
1692
1693 prevPoint = c1->point;
1694 c1 = e1->target;
1695 toPrev1 = e1->reverse;
1696 }
1697
1698 if ((cmp <= 0) && e0) {
1699 if (toPrev0) {
1700 for (Edge *e = toPrev0->prev, *n = NULL; e != min0; e = n) {
1701 n = e->prev;
1702 removeEdgePair(e);
1703 }
1704 }
1705
1706 if (pendingTail0) {
1707 if (toPrev0) {
1708 pendingHead0->link(toPrev0);
1709 }
1710 else {
1711 pendingHead0->link(min0->next);
1712 firstNew0 = pendingHead0;
1713 }
1714 min0->link(pendingTail0);
1715 pendingHead0 = NULL;
1716 pendingTail0 = NULL;
1717 }
1718 else if (!toPrev0) {
1719 firstNew0 = min0;
1720 }
1721
1722 prevPoint = c0->point;
1723 c0 = e0->target;
1724 toPrev0 = e0->reverse;
1725 }
1726 }
1727
1728 if ((c0 == first0) && (c1 == first1)) {
1729 if (toPrev0 == NULL) {
1730 pendingHead0->link(pendingTail0);
1731 c0->edges = pendingTail0;
1732 }
1733 else {
1734 for (Edge *e = toPrev0->prev, *n = NULL; e != firstNew0; e = n) {
1735 n = e->prev;
1736 removeEdgePair(e);
1737 }
1738 if (pendingTail0) {
1739 pendingHead0->link(toPrev0);
1740 firstNew0->link(pendingTail0);
1741 }
1742 }
1743
1744 if (toPrev1 == NULL) {
1745 pendingTail1->link(pendingHead1);
1746 c1->edges = pendingTail1;
1747 }
1748 else {
1749 for (Edge *e = toPrev1->next, *n = NULL; e != firstNew1; e = n) {
1750 n = e->next;
1751 removeEdgePair(e);
1752 }
1753 if (pendingTail1) {
1754 toPrev1->link(pendingHead1);
1755 pendingTail1->link(firstNew1);
1756 }
1757 }
1758
1759 return;
1760 }
1761
1762 firstRun = false;
1763 }
1764}
1765
1766static bool pointCmp(const btConvexHullInternal::Point32& p, const btConvexHullInternal::Point32& q)
1767{
1768 return (p.y < q.y) || ((p.y == q.y) && ((p.x < q.x) || ((p.x == q.x) && (p.z < q.z))));
1769}
1770
1771void btConvexHullInternal::compute(const void* coords, bool doubleCoords, int32_t stride, int32_t count)
1772{
1773 btVector3 min(btScalar(1e30), btScalar(1e30), btScalar(1e30)), max(btScalar(-1e30), btScalar(-1e30), btScalar(-1e30));
1774 const char* ptr = (const char*)coords;
1775 if (doubleCoords) {
1776 for (int32_t i = 0; i < count; i++) {
1777 const double* v = (const double*)ptr;
1778 btVector3 p((btScalar)v[0], (btScalar)v[1], (btScalar)v[2]);
1779 ptr += stride;
1780 min.setMin(p);
1781 max.setMax(p);
1782 }
1783 }
1784 else {
1785 for (int32_t i = 0; i < count; i++) {
1786 const float* v = (const float*)ptr;
1787 btVector3 p(v[0], v[1], v[2]);
1788 ptr += stride;
1789 min.setMin(p);
1790 max.setMax(p);
1791 }
1792 }
1793
1794 btVector3 s = max - min;
1795 maxAxis = s.maxAxis();
1796 minAxis = s.minAxis();
1797 if (minAxis == maxAxis) {
1798 minAxis = (maxAxis + 1) % 3;
1799 }
1800 medAxis = 3 - maxAxis - minAxis;
1801
1802 s /= btScalar(10216);
1803 if (((medAxis + 1) % 3) != maxAxis) {
1804 s *= -1;
1805 }
1806 scaling = s;
1807
1808 if (s[0] != 0) {
1809 s[0] = btScalar(1) / s[0];
1810 }
1811 if (s[1] != 0) {
1812 s[1] = btScalar(1) / s[1];
1813 }
1814 if (s[2] != 0) {
1815 s[2] = btScalar(1) / s[2];
1816 }
1817
1818 center = (min + max) * btScalar(0.5);
1819
1820 btAlignedObjectArray<Point32> points;
1821 points.resize(count);
1822 ptr = (const char*)coords;
1823 if (doubleCoords) {
1824 for (int32_t i = 0; i < count; i++) {
1825 const double* v = (const double*)ptr;
1826 btVector3 p((btScalar)v[0], (btScalar)v[1], (btScalar)v[2]);
1827 ptr += stride;
1828 p = (p - center) * s;
1829 points[i].x = (int32_t)p[medAxis];
1830 points[i].y = (int32_t)p[maxAxis];
1831 points[i].z = (int32_t)p[minAxis];
1832 points[i].index = i;
1833 }
1834 }
1835 else {
1836 for (int32_t i = 0; i < count; i++) {
1837 const float* v = (const float*)ptr;
1838 btVector3 p(v[0], v[1], v[2]);
1839 ptr += stride;
1840 p = (p - center) * s;
1841 points[i].x = (int32_t)p[medAxis];
1842 points[i].y = (int32_t)p[maxAxis];
1843 points[i].z = (int32_t)p[minAxis];
1844 points[i].index = i;
1845 }
1846 }
1847 points.quickSort(pointCmp);
1848
1849 vertexPool.reset();
1850 vertexPool.setArraySize(count);
1851 originalVertices.resize(count);
1852 for (int32_t i = 0; i < count; i++) {
1853 Vertex* v = vertexPool.newObject();
1854 v->edges = NULL;
1855 v->point = points[i];
1856 v->copy = -1;
1857 originalVertices[i] = v;
1858 }
1859
1860 points.clear();
1861
1862 edgePool.reset();
1863 edgePool.setArraySize(6 * count);
1864
1865 usedEdgePairs = 0;
1866 maxUsedEdgePairs = 0;
1867
1868 mergeStamp = -3;
1869
1870 IntermediateHull hull;
1871 computeInternal(0, count, hull);
1872 vertexList = hull.minXy;
1873#ifdef DEBUG_CONVEX_HULL
1874 printf("max. edges %d (3v = %d)", maxUsedEdgePairs, 3 * count);
1875#endif
1876}
1877
1878btVector3 btConvexHullInternal::toBtVector(const Point32& v)
1879{
1880 btVector3 p;
1881 p[medAxis] = btScalar(v.x);
1882 p[maxAxis] = btScalar(v.y);
1883 p[minAxis] = btScalar(v.z);
1884 return p * scaling;
1885}
1886
1887btVector3 btConvexHullInternal::getBtNormal(Face* face)
1888{
1889 return toBtVector(face->dir0).cross(toBtVector(face->dir1)).normalized();
1890}
1891
1892btVector3 btConvexHullInternal::getCoordinates(const Vertex* v)
1893{
1894 btVector3 p;
1895 p[medAxis] = v->xvalue();
1896 p[maxAxis] = v->yvalue();
1897 p[minAxis] = v->zvalue();
1898 return p * scaling + center;
1899}
1900
1901btScalar btConvexHullInternal::shrink(btScalar amount, btScalar clampAmount)
1902{
1903 if (!vertexList) {
1904 return 0;
1905 }
1906 int32_t stamp = --mergeStamp;
1907 btAlignedObjectArray<Vertex*> stack;
1908 vertexList->copy = stamp;
1909 stack.push_back(vertexList);
1910 btAlignedObjectArray<Face*> faces;
1911
1912 Point32 ref = vertexList->point;
1913 Int128 hullCenterX(0, 0);
1914 Int128 hullCenterY(0, 0);
1915 Int128 hullCenterZ(0, 0);
1916 Int128 volume(0, 0);
1917
1918 while (stack.size() > 0) {
1919 Vertex* v = stack[stack.size() - 1];
1920 stack.pop_back();
1921 Edge* e = v->edges;
1922 if (e) {
1923 do {
1924 if (e->target->copy != stamp) {
1925 e->target->copy = stamp;
1926 stack.push_back(e->target);
1927 }
1928 if (e->copy != stamp) {
1929 Face* face = facePool.newObject();
1930 face->init(e->target, e->reverse->prev->target, v);
1931 faces.push_back(face);
1932 Edge* f = e;
1933
1934 Vertex* a = NULL;
1935 Vertex* b = NULL;
1936 do {
1937 if (a && b) {
1938 int64_t vol = (v->point - ref).dot((a->point - ref).cross(b->point - ref));
1939 btAssert(vol >= 0);
1940 Point32 c = v->point + a->point + b->point + ref;
1941 hullCenterX += vol * c.x;
1942 hullCenterY += vol * c.y;
1943 hullCenterZ += vol * c.z;
1944 volume += vol;
1945 }
1946
1947 btAssert(f->copy != stamp);
1948 f->copy = stamp;
1949 f->face = face;
1950
1951 a = b;
1952 b = f->target;
1953
1954 f = f->reverse->prev;
1955 } while (f != e);
1956 }
1957 e = e->next;
1958 } while (e != v->edges);
1959 }
1960 }
1961
1962 if (volume.getSign() <= 0) {
1963 return 0;
1964 }
1965
1966 btVector3 hullCenter;
1967 hullCenter[medAxis] = hullCenterX.toScalar();
1968 hullCenter[maxAxis] = hullCenterY.toScalar();
1969 hullCenter[minAxis] = hullCenterZ.toScalar();
1970 hullCenter /= 4 * volume.toScalar();
1971 hullCenter *= scaling;
1972
1973 int32_t faceCount = faces.size();
1974
1975 if (clampAmount > 0) {
1976 btScalar minDist = SIMD_INFINITY;
1977 for (int32_t i = 0; i < faceCount; i++) {
1978 btVector3 normal = getBtNormal(faces[i]);
1979 btScalar dist = normal.dot(toBtVector(faces[i]->origin) - hullCenter);
1980 if (dist < minDist) {
1981 minDist = dist;
1982 }
1983 }
1984
1985 if (minDist <= 0) {
1986 return 0;
1987 }
1988
1989 amount = btMin(amount, minDist * clampAmount);
1990 }
1991
1992 uint32_t seed = 243703;
1993 for (int32_t i = 0; i < faceCount; i++, seed = 1664525 * seed + 1013904223) {
1994 btSwap(faces[i], faces[seed % faceCount]);
1995 }
1996
1997 for (int32_t i = 0; i < faceCount; i++) {
1998 if (!shiftFace(faces[i], amount, stack)) {
1999 return -amount;
2000 }
2001 }
2002
2003 return amount;
2004}
2005
2006bool btConvexHullInternal::shiftFace(Face* face, btScalar amount, btAlignedObjectArray<Vertex*> stack)
2007{
2008 btVector3 origShift = getBtNormal(face) * -amount;
2009 if (scaling[0] != 0) {
2010 origShift[0] /= scaling[0];
2011 }
2012 if (scaling[1] != 0) {
2013 origShift[1] /= scaling[1];
2014 }
2015 if (scaling[2] != 0) {
2016 origShift[2] /= scaling[2];
2017 }
2018 Point32 shift((int32_t)origShift[medAxis], (int32_t)origShift[maxAxis], (int32_t)origShift[minAxis]);
2019 if (shift.isZero()) {
2020 return true;
2021 }
2022 Point64 normal = face->getNormal();
2023#ifdef DEBUG_CONVEX_HULL
2024 printf("\nShrinking face (%d %d %d) (%d %d %d) (%d %d %d) by (%d %d %d)\n",
2025 face->origin.x, face->origin.y, face->origin.z, face->dir0.x, face->dir0.y, face->dir0.z, face->dir1.x, face->dir1.y, face->dir1.z, shift.x, shift.y, shift.z);
2026#endif
2027 int64_t origDot = face->origin.dot(normal);
2028 Point32 shiftedOrigin = face->origin + shift;
2029 int64_t shiftedDot = shiftedOrigin.dot(normal);
2030 btAssert(shiftedDot <= origDot);
2031 if (shiftedDot >= origDot) {
2032 return false;
2033 }
2034
2035 Edge* intersection = NULL;
2036
2037 Edge* startEdge = face->nearbyVertex->edges;
2038#ifdef DEBUG_CONVEX_HULL
2039 printf("Start edge is ");
2040 startEdge->print();
2041 printf(", normal is (%lld %lld %lld), shifted dot is %lld\n", normal.x, normal.y, normal.z, shiftedDot);
2042#endif
2043 Rational128 optDot = face->nearbyVertex->dot(normal);
2044 int32_t cmp = optDot.compare(shiftedDot);
2045#ifdef SHOW_ITERATIONS
2046 int32_t n = 0;
2047#endif
2048 if (cmp >= 0) {
2049 Edge* e = startEdge;
2050 do {
2051#ifdef SHOW_ITERATIONS
2052 n++;
2053#endif
2054 Rational128 dot = e->target->dot(normal);
2055 btAssert(dot.compare(origDot) <= 0);
2056#ifdef DEBUG_CONVEX_HULL
2057 printf("Moving downwards, edge is ");
2058 e->print();
2059 printf(", dot is %f (%f %lld)\n", (float)dot.toScalar(), (float)optDot.toScalar(), shiftedDot);
2060#endif
2061 if (dot.compare(optDot) < 0) {
2062 int32_t c = dot.compare(shiftedDot);
2063 optDot = dot;
2064 e = e->reverse;
2065 startEdge = e;
2066 if (c < 0) {
2067 intersection = e;
2068 break;
2069 }
2070 cmp = c;
2071 }
2072 e = e->prev;
2073 } while (e != startEdge);
2074
2075 if (!intersection) {
2076 return false;
2077 }
2078 }
2079 else {
2080 Edge* e = startEdge;
2081 do {
2082#ifdef SHOW_ITERATIONS
2083 n++;
2084#endif
2085 Rational128 dot = e->target->dot(normal);
2086 btAssert(dot.compare(origDot) <= 0);
2087#ifdef DEBUG_CONVEX_HULL
2088 printf("Moving upwards, edge is ");
2089 e->print();
2090 printf(", dot is %f (%f %lld)\n", (float)dot.toScalar(), (float)optDot.toScalar(), shiftedDot);
2091#endif
2092 if (dot.compare(optDot) > 0) {
2093 cmp = dot.compare(shiftedDot);
2094 if (cmp >= 0) {
2095 intersection = e;
2096 break;
2097 }
2098 optDot = dot;
2099 e = e->reverse;
2100 startEdge = e;
2101 }
2102 e = e->prev;
2103 } while (e != startEdge);
2104
2105 if (!intersection) {
2106 return true;
2107 }
2108 }
2109
2110#ifdef SHOW_ITERATIONS
2111 printf("Needed %d iterations to find initial intersection\n", n);
2112#endif
2113
2114 if (cmp == 0) {
2115 Edge* e = intersection->reverse->next;
2116#ifdef SHOW_ITERATIONS
2117 n = 0;
2118#endif
2119 while (e->target->dot(normal).compare(shiftedDot) <= 0) {
2120#ifdef SHOW_ITERATIONS
2121 n++;
2122#endif
2123 e = e->next;
2124 if (e == intersection->reverse) {
2125 return true;
2126 }
2127#ifdef DEBUG_CONVEX_HULL
2128 printf("Checking for outwards edge, current edge is ");
2129 e->print();
2130 printf("\n");
2131#endif
2132 }
2133#ifdef SHOW_ITERATIONS
2134 printf("Needed %d iterations to check for complete containment\n", n);
2135#endif
2136 }
2137
2138 Edge* firstIntersection = NULL;
2139 Edge* faceEdge = NULL;
2140 Edge* firstFaceEdge = NULL;
2141
2142#ifdef SHOW_ITERATIONS
2143 int32_t m = 0;
2144#endif
2145 while (true) {
2146#ifdef SHOW_ITERATIONS
2147 m++;
2148#endif
2149#ifdef DEBUG_CONVEX_HULL
2150 printf("Intersecting edge is ");
2151 intersection->print();
2152 printf("\n");
2153#endif
2154 if (cmp == 0) {
2155 Edge* e = intersection->reverse->next;
2156 startEdge = e;
2157#ifdef SHOW_ITERATIONS
2158 n = 0;
2159#endif
2160 while (true) {
2161#ifdef SHOW_ITERATIONS
2162 n++;
2163#endif
2164 if (e->target->dot(normal).compare(shiftedDot) >= 0) {
2165 break;
2166 }
2167 intersection = e->reverse;
2168 e = e->next;
2169 if (e == startEdge) {
2170 return true;
2171 }
2172 }
2173#ifdef SHOW_ITERATIONS
2174 printf("Needed %d iterations to advance intersection\n", n);
2175#endif
2176 }
2177
2178#ifdef DEBUG_CONVEX_HULL
2179 printf("Advanced intersecting edge to ");
2180 intersection->print();
2181 printf(", cmp = %d\n", cmp);
2182#endif
2183
2184 if (!firstIntersection) {
2185 firstIntersection = intersection;
2186 }
2187 else if (intersection == firstIntersection) {
2188 break;
2189 }
2190
2191 int32_t prevCmp = cmp;
2192 Edge* prevIntersection = intersection;
2193 Edge* prevFaceEdge = faceEdge;
2194
2195 Edge* e = intersection->reverse;
2196#ifdef SHOW_ITERATIONS
2197 n = 0;
2198#endif
2199 while (true) {
2200#ifdef SHOW_ITERATIONS
2201 n++;
2202#endif
2203 e = e->reverse->prev;
2204 btAssert(e != intersection->reverse);
2205 cmp = e->target->dot(normal).compare(shiftedDot);
2206#ifdef DEBUG_CONVEX_HULL
2207 printf("Testing edge ");
2208 e->print();
2209 printf(" -> cmp = %d\n", cmp);
2210#endif
2211 if (cmp >= 0) {
2212 intersection = e;
2213 break;
2214 }
2215 }
2216#ifdef SHOW_ITERATIONS
2217 printf("Needed %d iterations to find other intersection of face\n", n);
2218#endif
2219
2220 if (cmp > 0) {
2221 Vertex* removed = intersection->target;
2222 e = intersection->reverse;
2223 if (e->prev == e) {
2224 removed->edges = NULL;
2225 }
2226 else {
2227 removed->edges = e->prev;
2228 e->prev->link(e->next);
2229 e->link(e);
2230 }
2231#ifdef DEBUG_CONVEX_HULL
2232 printf("1: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z);
2233#endif
2234
2235 Point64 n0 = intersection->face->getNormal();
2236 Point64 n1 = intersection->reverse->face->getNormal();
2237 int64_t m00 = face->dir0.dot(n0);
2238 int64_t m01 = face->dir1.dot(n0);
2239 int64_t m10 = face->dir0.dot(n1);
2240 int64_t m11 = face->dir1.dot(n1);
2241 int64_t r0 = (intersection->face->origin - shiftedOrigin).dot(n0);
2242 int64_t r1 = (intersection->reverse->face->origin - shiftedOrigin).dot(n1);
2243 Int128 det = Int128::mul(m00, m11) - Int128::mul(m01, m10);
2244 btAssert(det.getSign() != 0);
2245 Vertex* v = vertexPool.newObject();
2246 v->point.index = -1;
2247 v->copy = -1;
2248 v->point128 = PointR128(Int128::mul(face->dir0.x * r0, m11) - Int128::mul(face->dir0.x * r1, m01)
2249 + Int128::mul(face->dir1.x * r1, m00) - Int128::mul(face->dir1.x * r0, m10) + det * shiftedOrigin.x,
2250 Int128::mul(face->dir0.y * r0, m11) - Int128::mul(face->dir0.y * r1, m01)
2251 + Int128::mul(face->dir1.y * r1, m00) - Int128::mul(face->dir1.y * r0, m10) + det * shiftedOrigin.y,
2252 Int128::mul(face->dir0.z * r0, m11) - Int128::mul(face->dir0.z * r1, m01)
2253 + Int128::mul(face->dir1.z * r1, m00) - Int128::mul(face->dir1.z * r0, m10) + det * shiftedOrigin.z,
2254 det);
2255 v->point.x = (int32_t)v->point128.xvalue();
2256 v->point.y = (int32_t)v->point128.yvalue();
2257 v->point.z = (int32_t)v->point128.zvalue();
2258 intersection->target = v;
2259 v->edges = e;
2260
2261 stack.push_back(v);
2262 stack.push_back(removed);
2263 stack.push_back(NULL);
2264 }
2265
2266 if (cmp || prevCmp || (prevIntersection->reverse->next->target != intersection->target)) {
2267 faceEdge = newEdgePair(prevIntersection->target, intersection->target);
2268 if (prevCmp == 0) {
2269 faceEdge->link(prevIntersection->reverse->next);
2270 }
2271 if ((prevCmp == 0) || prevFaceEdge) {
2272 prevIntersection->reverse->link(faceEdge);
2273 }
2274 if (cmp == 0) {
2275 intersection->reverse->prev->link(faceEdge->reverse);
2276 }
2277 faceEdge->reverse->link(intersection->reverse);
2278 }
2279 else {
2280 faceEdge = prevIntersection->reverse->next;
2281 }
2282
2283 if (prevFaceEdge) {
2284 if (prevCmp > 0) {
2285 faceEdge->link(prevFaceEdge->reverse);
2286 }
2287 else if (faceEdge != prevFaceEdge->reverse) {
2288 stack.push_back(prevFaceEdge->target);
2289 while (faceEdge->next != prevFaceEdge->reverse) {
2290 Vertex* removed = faceEdge->next->target;
2291 removeEdgePair(faceEdge->next);
2292 stack.push_back(removed);
2293#ifdef DEBUG_CONVEX_HULL
2294 printf("2: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z);
2295#endif
2296 }
2297 stack.push_back(NULL);
2298 }
2299 }
2300 faceEdge->face = face;
2301 faceEdge->reverse->face = intersection->face;
2302
2303 if (!firstFaceEdge) {
2304 firstFaceEdge = faceEdge;
2305 }
2306 }
2307#ifdef SHOW_ITERATIONS
2308 printf("Needed %d iterations to process all intersections\n", m);
2309#endif
2310
2311 if (cmp > 0) {
2312 firstFaceEdge->reverse->target = faceEdge->target;
2313 firstIntersection->reverse->link(firstFaceEdge);
2314 firstFaceEdge->link(faceEdge->reverse);
2315 }
2316 else if (firstFaceEdge != faceEdge->reverse) {
2317 stack.push_back(faceEdge->target);
2318 while (firstFaceEdge->next != faceEdge->reverse) {
2319 Vertex* removed = firstFaceEdge->next->target;
2320 removeEdgePair(firstFaceEdge->next);
2321 stack.push_back(removed);
2322#ifdef DEBUG_CONVEX_HULL
2323 printf("3: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z);
2324#endif
2325 }
2326 stack.push_back(NULL);
2327 }
2328
2329 btAssert(stack.size() > 0);
2330 vertexList = stack[0];
2331
2332#ifdef DEBUG_CONVEX_HULL
2333 printf("Removing part\n");
2334#endif
2335#ifdef SHOW_ITERATIONS
2336 n = 0;
2337#endif
2338 int32_t pos = 0;
2339 while (pos < stack.size()) {
2340 int32_t end = stack.size();
2341 while (pos < end) {
2342 Vertex* kept = stack[pos++];
2343#ifdef DEBUG_CONVEX_HULL
2344 kept->print();
2345#endif
2346 bool deeper = false;
2347 Vertex* removed;
2348 while ((removed = stack[pos++]) != NULL) {
2349#ifdef SHOW_ITERATIONS
2350 n++;
2351#endif
2352 kept->receiveNearbyFaces(removed);
2353 while (removed->edges) {
2354 if (!deeper) {
2355 deeper = true;
2356 stack.push_back(kept);
2357 }
2358 stack.push_back(removed->edges->target);
2359 removeEdgePair(removed->edges);
2360 }
2361 }
2362 if (deeper) {
2363 stack.push_back(NULL);
2364 }
2365 }
2366 }
2367#ifdef SHOW_ITERATIONS
2368 printf("Needed %d iterations to remove part\n", n);
2369#endif
2370
2371 stack.resize(0);
2372 face->origin = shiftedOrigin;
2373
2374 return true;
2375}
2376
2377static int32_t getVertexCopy(btConvexHullInternal::Vertex* vertex, btAlignedObjectArray<btConvexHullInternal::Vertex*>& vertices)
2378{
2379 int32_t index = vertex->copy;
2380 if (index < 0) {
2381 index = vertices.size();
2382 vertex->copy = index;
2383 vertices.push_back(vertex);
2384#ifdef DEBUG_CONVEX_HULL
2385 printf("Vertex %d gets index *%d\n", vertex->point.index, index);
2386#endif
2387 }
2388 return index;
2389}
2390
2391btScalar btConvexHullComputer::compute(const void* coords, bool doubleCoords, int32_t stride, int32_t count, btScalar shrink, btScalar shrinkClamp)
2392{
2393 if (count <= 0) {
2394 vertices.clear();
2395 edges.clear();
2396 faces.clear();
2397 return 0;
2398 }
2399
2400 btConvexHullInternal hull;
2401 hull.compute(coords, doubleCoords, stride, count);
2402
2403 btScalar shift = 0;
2404 if ((shrink > 0) && ((shift = hull.shrink(shrink, shrinkClamp)) < 0)) {
2405 vertices.clear();
2406 edges.clear();
2407 faces.clear();
2408 return shift;
2409 }
2410
2411 vertices.resize(0);
2412 edges.resize(0);
2413 faces.resize(0);
2414
2415 btAlignedObjectArray<btConvexHullInternal::Vertex*> oldVertices;
2416 getVertexCopy(hull.vertexList, oldVertices);
2417 int32_t copied = 0;
2418 while (copied < oldVertices.size()) {
2419 btConvexHullInternal::Vertex* v = oldVertices[copied];
2420 vertices.push_back(hull.getCoordinates(v));
2421 btConvexHullInternal::Edge* firstEdge = v->edges;
2422 if (firstEdge) {
2423 int32_t firstCopy = -1;
2424 int32_t prevCopy = -1;
2425 btConvexHullInternal::Edge* e = firstEdge;
2426 do {
2427 if (e->copy < 0) {
2428 int32_t s = edges.size();
2429 edges.push_back(Edge());
2430 edges.push_back(Edge());
2431 Edge* c = &edges[s];
2432 Edge* r = &edges[s + 1];
2433 e->copy = s;
2434 e->reverse->copy = s + 1;
2435 c->reverse = 1;
2436 r->reverse = -1;
2437 c->targetVertex = getVertexCopy(e->target, oldVertices);
2438 r->targetVertex = copied;
2439#ifdef DEBUG_CONVEX_HULL
2440 printf(" CREATE: Vertex *%d has edge to *%d\n", copied, c->getTargetVertex());
2441#endif
2442 }
2443 if (prevCopy >= 0) {
2444 edges[e->copy].next = prevCopy - e->copy;
2445 }
2446 else {
2447 firstCopy = e->copy;
2448 }
2449 prevCopy = e->copy;
2450 e = e->next;
2451 } while (e != firstEdge);
2452 edges[firstCopy].next = prevCopy - firstCopy;
2453 }
2454 copied++;
2455 }
2456
2457 for (int32_t i = 0; i < copied; i++) {
2458 btConvexHullInternal::Vertex* v = oldVertices[i];
2459 btConvexHullInternal::Edge* firstEdge = v->edges;
2460 if (firstEdge) {
2461 btConvexHullInternal::Edge* e = firstEdge;
2462 do {
2463 if (e->copy >= 0) {
2464#ifdef DEBUG_CONVEX_HULL
2465 printf("Vertex *%d has edge to *%d\n", i, edges[e->copy].getTargetVertex());
2466#endif
2467 faces.push_back(e->copy);
2468 btConvexHullInternal::Edge* f = e;
2469 do {
2470#ifdef DEBUG_CONVEX_HULL
2471 printf(" Face *%d\n", edges[f->copy].getTargetVertex());
2472#endif
2473 f->copy = -1;
2474 f = f->reverse->prev;
2475 } while (f != e);
2476 }
2477 e = e->next;
2478 } while (e != firstEdge);
2479 }
2480 }
2481
2482 return shift;
2483}
2484
2485// -- GODOT start --
2486}; // namespace VHACD
2487// -- GODOT end --
2488