2 Copyright (c) 2011 Ole Kniemeyer, MAXON, www.maxon.net
4 This software is provided 'as-is', without any express or implied warranty.
5 In no event will the authors be held liable for any damages arising from the use of this software.
6 Permission is granted to anyone to use this software for any purpose,
7 including commercial applications, and to alter it and redistribute it freely,
8 subject to the following restrictions:
10 1. 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.
11 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
12 3. This notice may not be removed or altered from any source distribution.
17 #include "btConvexHullComputer.h"
18 #include "btAlignedObjectArray.h"
20 #include "btVector3.h"
24 #elif defined(_MSC_VER)
25 typedef __int32 int32_t;
26 typedef __int64 int64_t;
27 typedef unsigned __int32 uint32_t;
28 typedef unsigned __int64 uint64_t;
31 typedef long long int int64_t;
32 typedef unsigned int uint32_t;
33 typedef unsigned long long int uint64_t;
37 //The definition of USE_X86_64_ASM is moved into the build system. You can enable it manually by commenting out the following lines
38 //#if (defined(__GNUC__) && defined(__x86_64__) && !defined(__ICL)) // || (defined(__ICL) && defined(_M_X64)) bug in Intel compiler, disable inline assembly
39 // #define USE_X86_64_ASM
43 //#define DEBUG_CONVEX_HULL
44 //#define SHOW_ITERATIONS
46 #if defined(DEBUG_CONVEX_HULL) || defined(SHOW_ITERATIONS)
50 // Convex hull implementation based on Preparata and Hong
51 // Ole Kniemeyer, MAXON Computer GmbH
52 class btConvexHullInternal
63 Point64(int64_t x, int64_t y, int64_t z): x(x), y(y), z(z)
69 return (x == 0) && (y == 0) && (z == 0);
72 int64_t dot(const Point64& b) const
74 return x * b.x + y * b.y + z * b.z;
90 Point32(int32_t x, int32_t y, int32_t z): x(x), y(y), z(z), index(-1)
94 bool operator==(const Point32& b) const
96 return (x == b.x) && (y == b.y) && (z == b.z);
99 bool operator!=(const Point32& b) const
101 return (x != b.x) || (y != b.y) || (z != b.z);
106 return (x == 0) && (y == 0) && (z == 0);
109 Point64 cross(const Point32& b) const
111 return Point64(y * b.z - z * b.y, z * b.x - x * b.z, x * b.y - y * b.x);
114 Point64 cross(const Point64& b) const
116 return Point64(y * b.z - z * b.y, z * b.x - x * b.z, x * b.y - y * b.x);
119 int64_t dot(const Point32& b) const
121 return x * b.x + y * b.y + z * b.z;
124 int64_t dot(const Point64& b) const
126 return x * b.x + y * b.y + z * b.z;
129 Point32 operator+(const Point32& b) const
131 return Point32(x + b.x, y + b.y, z + b.z);
134 Point32 operator-(const Point32& b) const
136 return Point32(x - b.x, y - b.y, z - b.z);
150 Int128(uint64_t low, uint64_t high): low(low), high(high)
154 Int128(uint64_t low): low(low), high(0)
158 Int128(int64_t value): low(value), high((value >= 0) ? 0 : (uint64_t) -1LL)
162 static Int128 mul(int64_t a, int64_t b);
164 static Int128 mul(uint64_t a, uint64_t b);
166 Int128 operator-() const
168 return Int128((uint64_t) -(int64_t)low, ~high + (low == 0));
171 Int128 operator+(const Int128& b) const
173 #ifdef USE_X86_64_ASM
175 __asm__ ("addq %[bl], %[rl]\n\t"
176 "adcq %[bh], %[rh]\n\t"
177 : [rl] "=r" (result.low), [rh] "=r" (result.high)
178 : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high)
182 uint64_t lo = low + b.low;
183 return Int128(lo, high + b.high + (lo < low));
187 Int128 operator-(const Int128& b) const
189 #ifdef USE_X86_64_ASM
191 __asm__ ("subq %[bl], %[rl]\n\t"
192 "sbbq %[bh], %[rh]\n\t"
193 : [rl] "=r" (result.low), [rh] "=r" (result.high)
194 : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high)
202 Int128& operator+=(const Int128& b)
204 #ifdef USE_X86_64_ASM
205 __asm__ ("addq %[bl], %[rl]\n\t"
206 "adcq %[bh], %[rh]\n\t"
207 : [rl] "=r" (low), [rh] "=r" (high)
208 : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high)
211 uint64_t lo = low + b.low;
231 Int128 operator*(int64_t b) const;
233 btScalar toScalar() const
235 return ((int64_t) high >= 0) ? btScalar(high) * (btScalar(0x100000000LL) * btScalar(0x100000000LL)) + btScalar(low)
236 : -(-*this).toScalar();
241 return ((int64_t) high < 0) ? -1 : (high || low) ? 1 : 0;
244 bool operator<(const Int128& b) const
246 return (high < b.high) || ((high == b.high) && (low < b.low));
249 int ucmp(const Int128&b) const
275 uint64_t m_numerator;
276 uint64_t m_denominator;
280 Rational64(int64_t numerator, int64_t denominator)
285 m_numerator = (uint64_t) numerator;
287 else if (numerator < 0)
290 m_numerator = (uint64_t) -numerator;
299 m_denominator = (uint64_t) denominator;
301 else if (denominator < 0)
304 m_denominator = (uint64_t) -denominator;
312 bool isNegativeInfinity() const
314 return (sign < 0) && (m_denominator == 0);
319 return (sign == 0) && (m_denominator == 0);
322 int compare(const Rational64& b) const;
324 btScalar toScalar() const
326 return sign * ((m_denominator == 0) ? SIMD_INFINITY : (btScalar) m_numerator / m_denominator);
340 Rational128(int64_t value)
345 this->numerator = value;
350 this->numerator = -value;
355 this->numerator = (uint64_t) 0;
357 this->denominator = (uint64_t) 1;
361 Rational128(const Int128& numerator, const Int128& denominator)
363 sign = numerator.getSign();
366 this->numerator = numerator;
370 this->numerator = -numerator;
372 int dsign = denominator.getSign();
375 this->denominator = denominator;
380 this->denominator = -denominator;
385 int compare(const Rational128& b) const;
387 int compare(int64_t b) const;
389 btScalar toScalar() const
391 return sign * ((denominator.getSign() == 0) ? SIMD_INFINITY : numerator.toScalar() / denominator.toScalar());
407 PointR128(Int128 x, Int128 y, Int128 z, Int128 denominator): x(x), y(y), z(z), denominator(denominator)
411 btScalar xvalue() const
413 return x.toScalar() / denominator.toScalar();
416 btScalar yvalue() const
418 return y.toScalar() / denominator.toScalar();
421 btScalar zvalue() const
423 return z.toScalar() / denominator.toScalar();
437 Face* firstNearbyFace;
438 Face* lastNearbyFace;
443 Vertex(): next(NULL), prev(NULL), edges(NULL), firstNearbyFace(NULL), lastNearbyFace(NULL), copy(-1)
447 #ifdef DEBUG_CONVEX_HULL
450 printf("V%d (%d, %d, %d)", point.index, point.x, point.y, point.z);
456 Point32 operator-(const Vertex& b) const
458 return point - b.point;
461 Rational128 dot(const Point64& b) const
463 return (point.index >= 0) ? Rational128(point.dot(b))
464 : Rational128(point128.x * b.x + point128.y * b.y + point128.z * b.z, point128.denominator);
467 btScalar xvalue() const
469 return (point.index >= 0) ? btScalar(point.x) : point128.xvalue();
472 btScalar yvalue() const
474 return (point.index >= 0) ? btScalar(point.y) : point128.yvalue();
477 btScalar zvalue() const
479 return (point.index >= 0) ? btScalar(point.z) : point128.zvalue();
482 void receiveNearbyFaces(Vertex* src)
486 lastNearbyFace->nextWithSameNearbyVertex = src->firstNearbyFace;
490 firstNearbyFace = src->firstNearbyFace;
492 if (src->lastNearbyFace)
494 lastNearbyFace = src->lastNearbyFace;
496 for (Face* f = src->firstNearbyFace; f; f = f->nextWithSameNearbyVertex)
498 btAssert(f->nearbyVertex == src);
499 f->nearbyVertex = this;
501 src->firstNearbyFace = NULL;
502 src->lastNearbyFace = NULL;
528 btAssert(reverse->target == n->reverse->target);
533 #ifdef DEBUG_CONVEX_HULL
536 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,
537 reverse->target->point.x, reverse->target->point.y, reverse->target->point.z, target->point.x, target->point.y, target->point.z);
546 Vertex* nearbyVertex;
547 Face* nextWithSameNearbyVertex;
552 Face(): next(NULL), nearbyVertex(NULL), nextWithSameNearbyVertex(NULL)
556 void init(Vertex* a, Vertex* b, Vertex* c)
562 if (a->lastNearbyFace)
564 a->lastNearbyFace->nextWithSameNearbyVertex = this;
568 a->firstNearbyFace = this;
570 a->lastNearbyFace = this;
575 return dir0.cross(dir1);
579 template<typename UWord, typename UHWord> class DMul
582 static uint32_t high(uint64_t value)
584 return (uint32_t) (value >> 32);
587 static uint32_t low(uint64_t value)
589 return (uint32_t) value;
592 static uint64_t mul(uint32_t a, uint32_t b)
594 return (uint64_t) a * (uint64_t) b;
597 static void shlHalf(uint64_t& value)
602 static uint64_t high(Int128 value)
607 static uint64_t low(Int128 value)
612 static Int128 mul(uint64_t a, uint64_t b)
614 return Int128::mul(a, b);
617 static void shlHalf(Int128& value)
619 value.high = value.low;
625 static void mul(UWord a, UWord b, UWord& resLow, UWord& resHigh)
627 UWord p00 = mul(low(a), low(b));
628 UWord p01 = mul(low(a), high(b));
629 UWord p10 = mul(high(a), low(b));
630 UWord p11 = mul(high(a), high(b));
631 UWord p0110 = UWord(low(p01)) + UWord(low(p10));
648 class IntermediateHull
656 IntermediateHull(): minXy(NULL), maxXy(NULL), minYx(NULL), maxYx(NULL)
663 enum Orientation {NONE, CLOCKWISE, COUNTER_CLOCKWISE};
665 template <typename T> class PoolArray
674 PoolArray(int size): size(size), next(NULL)
676 array = (T*) btAlignedAlloc(sizeof(T) * size, 16);
681 btAlignedFree(array);
687 for (int i = 0; i < size; i++, o++)
689 o->next = (i+1 < size) ? o + 1 : NULL;
695 template <typename T> class Pool
698 PoolArray<T>* arrays;
699 PoolArray<T>* nextArray;
704 Pool(): arrays(NULL), nextArray(NULL), freeObjects(NULL), arraySize(256)
712 PoolArray<T>* p = arrays;
725 void setArraySize(int arraySize)
727 this->arraySize = arraySize;
735 PoolArray<T>* p = nextArray;
742 p = new(btAlignedAlloc(sizeof(PoolArray<T>), 16)) PoolArray<T>(arraySize);
748 freeObjects = o->next;
752 void freeObject(T* object)
755 object->next = freeObjects;
756 freeObjects = object;
762 Pool<Vertex> vertexPool;
765 btAlignedObjectArray<Vertex*> originalVertices;
771 int maxUsedEdgePairs;
773 static Orientation getOrientation(const Edge* prev, const Edge* next, const Point32& s, const Point32& t);
774 Edge* findMaxAngle(bool ccw, const Vertex* start, const Point32& s, const Point64& rxs, const Point64& sxrxs, Rational64& minCot);
775 void findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge*& e0, Edge*& e1, Vertex* stop0, Vertex* stop1);
777 Edge* newEdgePair(Vertex* from, Vertex* to);
779 void removeEdgePair(Edge* edge)
781 Edge* n = edge->next;
782 Edge* r = edge->reverse;
784 btAssert(edge->target && r->target);
788 n->prev = edge->prev;
789 edge->prev->next = n;
790 r->target->edges = n;
794 r->target->edges = NULL;
803 edge->target->edges = n;
807 edge->target->edges = NULL;
810 edgePool.freeObject(edge);
811 edgePool.freeObject(r);
815 void computeInternal(int start, int end, IntermediateHull& result);
817 bool mergeProjection(IntermediateHull& h0, IntermediateHull& h1, Vertex*& c0, Vertex*& c1);
819 void merge(IntermediateHull& h0, IntermediateHull& h1);
821 btVector3 toBtVector(const Point32& v);
823 btVector3 getBtNormal(Face* face);
825 bool shiftFace(Face* face, btScalar amount, btAlignedObjectArray<Vertex*> stack);
830 void compute(const void* coords, bool doubleCoords, int stride, int count);
832 btVector3 getCoordinates(const Vertex* v);
834 btScalar shrink(btScalar amount, btScalar clampAmount);
838 btConvexHullInternal::Int128 btConvexHullInternal::Int128::operator*(int64_t b) const
840 bool negative = (int64_t) high < 0;
841 Int128 a = negative ? -*this : *this;
844 negative = !negative;
847 Int128 result = mul(a.low, (uint64_t) b);
848 result.high += a.high * (uint64_t) b;
849 return negative ? -result : result;
852 btConvexHullInternal::Int128 btConvexHullInternal::Int128::mul(int64_t a, int64_t b)
856 #ifdef USE_X86_64_ASM
857 __asm__ ("imulq %[b]"
858 : "=a" (result.low), "=d" (result.high)
864 bool negative = a < 0;
871 negative = !negative;
874 DMul<uint64_t, uint32_t>::mul((uint64_t) a, (uint64_t) b, result.low, result.high);
875 return negative ? -result : result;
879 btConvexHullInternal::Int128 btConvexHullInternal::Int128::mul(uint64_t a, uint64_t b)
883 #ifdef USE_X86_64_ASM
885 : "=a" (result.low), "=d" (result.high)
890 DMul<uint64_t, uint32_t>::mul(a, b, result.low, result.high);
896 int btConvexHullInternal::Rational64::compare(const Rational64& b) const
900 return sign - b.sign;
907 // return (numerator * b.denominator > b.numerator * denominator) ? sign : (numerator * b.denominator < b.numerator * denominator) ? -sign : 0;
909 #ifdef USE_X86_64_ASM
914 __asm__ ("mulq %[bn]\n\t"
915 "movq %%rax, %[tmp]\n\t"
916 "movq %%rdx, %%rbx\n\t"
917 "movq %[tn], %%rax\n\t"
919 "subq %[tmp], %%rax\n\t"
920 "sbbq %%rbx, %%rdx\n\t" // rdx:rax contains 128-bit-difference "numerator*b.denominator - b.numerator*denominator"
921 "setnsb %%bh\n\t" // bh=1 if difference is non-negative, bh=0 otherwise
922 "orq %%rdx, %%rax\n\t"
923 "setnzb %%bl\n\t" // bl=1 if difference if non-zero, bl=0 if it is zero
924 "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)
925 "shll $16, %%ebx\n\t" // ebx has same sign as difference
926 : "=&b"(result), [tmp] "=&r"(tmp), "=a"(dummy)
927 : "a"(denominator), [bn] "g"(b.numerator), [tn] "g"(numerator), [bd] "g"(b.denominator)
929 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)
930 // if sign is -1, all bits of result are inverted, which changes the sign of result (and again cannot result in zero)
935 return sign * Int128::mul(m_numerator, b.m_denominator).ucmp(Int128::mul(m_denominator, b.m_numerator));
940 int btConvexHullInternal::Rational128::compare(const Rational128& b) const
944 return sign - b.sign;
952 return -b.compare(sign * (int64_t) numerator.low);
955 Int128 nbdLow, nbdHigh, dbnLow, dbnHigh;
956 DMul<Int128, uint64_t>::mul(numerator, b.denominator, nbdLow, nbdHigh);
957 DMul<Int128, uint64_t>::mul(denominator, b.numerator, dbnLow, dbnHigh);
959 int cmp = nbdHigh.ucmp(dbnHigh);
964 return nbdLow.ucmp(dbnLow) * sign;
967 int btConvexHullInternal::Rational128::compare(int64_t b) const
971 int64_t a = sign * (int64_t) numerator.low;
972 return (a > b) ? 1 : (a < b) ? -1 : 0;
994 return numerator.ucmp(denominator * b) * sign;
998 btConvexHullInternal::Edge* btConvexHullInternal::newEdgePair(Vertex* from, Vertex* to)
1000 btAssert(from && to);
1001 Edge* e = edgePool.newObject();
1002 Edge* r = edgePool.newObject();
1005 e->copy = mergeStamp;
1006 r->copy = mergeStamp;
1012 if (usedEdgePairs > maxUsedEdgePairs)
1014 maxUsedEdgePairs = usedEdgePairs;
1019 bool btConvexHullInternal::mergeProjection(IntermediateHull& h0, IntermediateHull& h1, Vertex*& c0, Vertex*& c1)
1021 Vertex* v0 = h0.maxYx;
1022 Vertex* v1 = h1.minYx;
1023 if ((v0->point.x == v1->point.x) && (v0->point.y == v1->point.y))
1025 btAssert(v0->point.z < v1->point.z);
1026 Vertex* v1p = v1->prev;
1032 btAssert(v1->edges->next == v1->edges);
1033 v1 = v1->edges->target;
1034 btAssert(v1->edges->next == v1->edges);
1039 Vertex* v1n = v1->next;
1044 if ((v1n->point.x < v1p->point.x) || ((v1n->point.x == v1p->point.x) && (v1n->point.y < v1p->point.y)))
1055 if ((v1n->point.x > v1p->point.x) || ((v1n->point.x == v1p->point.x) && (v1n->point.y > v1p->point.y)))
1072 for (int side = 0; side <= 1; side++)
1074 int32_t dx = (v1->point.x - v0->point.x) * sign;
1079 int32_t dy = v1->point.y - v0->point.y;
1081 Vertex* w0 = side ? v0->next : v0->prev;
1084 int32_t dx0 = (w0->point.x - v0->point.x) * sign;
1085 int32_t dy0 = w0->point.y - v0->point.y;
1086 if ((dy0 <= 0) && ((dx0 == 0) || ((dx0 < 0) && (dy0 * dx <= dy * dx0))))
1089 dx = (v1->point.x - v0->point.x) * sign;
1094 Vertex* w1 = side ? v1->next : v1->prev;
1097 int32_t dx1 = (w1->point.x - v1->point.x) * sign;
1098 int32_t dy1 = w1->point.y - v1->point.y;
1099 int32_t dxn = (w1->point.x - v0->point.x) * sign;
1100 if ((dxn > 0) && (dy1 < 0) && ((dx1 == 0) || ((dx1 < 0) && (dy1 * dx < dy * dx1))))
1115 int32_t dy = v1->point.y - v0->point.y;
1117 Vertex* w1 = side ? v1->prev : v1->next;
1120 int32_t dx1 = (w1->point.x - v1->point.x) * sign;
1121 int32_t dy1 = w1->point.y - v1->point.y;
1122 if ((dy1 >= 0) && ((dx1 == 0) || ((dx1 < 0) && (dy1 * dx <= dy * dx1))))
1125 dx = (v1->point.x - v0->point.x) * sign;
1130 Vertex* w0 = side ? v0->prev : v0->next;
1133 int32_t dx0 = (w0->point.x - v0->point.x) * sign;
1134 int32_t dy0 = w0->point.y - v0->point.y;
1135 int32_t dxn = (v1->point.x - w0->point.x) * sign;
1136 if ((dxn < 0) && (dy0 > 0) && ((dx0 == 0) || ((dx0 < 0) && (dy0 * dx < dy * dx0))))
1149 int32_t x = v0->point.x;
1150 int32_t y0 = v0->point.y;
1153 while (((t = side ? w0->next : w0->prev) != v0) && (t->point.x == x) && (t->point.y <= y0))
1160 int32_t y1 = v1->point.y;
1162 while (((t = side ? w1->prev : w1->next) != v1) && (t->point.x == x) && (t->point.y >= y1))
1187 if (h1.minXy->point.x < h0.minXy->point.x)
1189 h0.minXy = h1.minXy;
1191 if (h1.maxXy->point.x >= h0.maxXy->point.x)
1193 h0.maxXy = h1.maxXy;
1196 h0.maxYx = h1.maxYx;
1204 void btConvexHullInternal::computeInternal(int start, int end, IntermediateHull& result)
1206 int n = end - start;
1210 result.minXy = NULL;
1211 result.maxXy = NULL;
1212 result.minYx = NULL;
1213 result.maxYx = NULL;
1217 Vertex* v = originalVertices[start];
1219 if (v->point != w->point)
1221 int32_t dx = v->point.x - w->point.x;
1222 int32_t dy = v->point.y - w->point.y;
1224 if ((dx == 0) && (dy == 0))
1226 if (v->point.z > w->point.z)
1232 btAssert(v->point.z < w->point.z);
1247 if ((dx < 0) || ((dx == 0) && (dy < 0)))
1258 if ((dy < 0) || ((dy == 0) && (dx < 0)))
1270 Edge* e = newEdgePair(v, w);
1281 // lint -fallthrough
1284 Vertex* v = originalVertices[start];
1298 int split0 = start + n / 2;
1299 Point32 p = originalVertices[split0-1]->point;
1300 int split1 = split0;
1301 while ((split1 < end) && (originalVertices[split1]->point == p))
1305 computeInternal(start, split0, result);
1306 IntermediateHull hull1;
1307 computeInternal(split1, end, hull1);
1308 #ifdef DEBUG_CONVEX_HULL
1309 printf("\n\nMerge\n");
1313 merge(result, hull1);
1314 #ifdef DEBUG_CONVEX_HULL
1315 printf("\n Result\n");
1320 #ifdef DEBUG_CONVEX_HULL
1321 void btConvexHullInternal::IntermediateHull::print()
1324 for (Vertex* v = minXy; v; )
1340 if (v->next->prev != v)
1342 printf(" Inconsistency");
1353 minXy->copy = (minXy->copy == -1) ? -2 : -1;
1354 minXy->printGraph();
1358 void btConvexHullInternal::Vertex::printGraph()
1361 printf("\nEdges\n");
1370 } while (e != edges);
1373 Vertex* v = e->target;
1374 if (v->copy != copy)
1380 } while (e != edges);
1385 btConvexHullInternal::Orientation btConvexHullInternal::getOrientation(const Edge* prev, const Edge* next, const Point32& s, const Point32& t)
1387 btAssert(prev->reverse->target == next->reverse->target);
1388 if (prev->next == next)
1390 if (prev->prev == next)
1392 Point64 n = t.cross(s);
1393 Point64 m = (*prev->target - *next->reverse->target).cross(*next->target - *next->reverse->target);
1394 btAssert(!m.isZero());
1395 int64_t dot = n.dot(m);
1397 return (dot > 0) ? COUNTER_CLOCKWISE : CLOCKWISE;
1399 return COUNTER_CLOCKWISE;
1401 else if (prev->prev == next)
1411 btConvexHullInternal::Edge* btConvexHullInternal::findMaxAngle(bool ccw, const Vertex* start, const Point32& s, const Point64& rxs, const Point64& sxrxs, Rational64& minCot)
1413 Edge* minEdge = NULL;
1415 #ifdef DEBUG_CONVEX_HULL
1416 printf("find max edge for %d\n", start->point.index);
1418 Edge* e = start->edges;
1423 if (e->copy > mergeStamp)
1425 Point32 t = *e->target - *start;
1426 Rational64 cot(t.dot(sxrxs), t.dot(rxs));
1427 #ifdef DEBUG_CONVEX_HULL
1428 printf(" Angle is %f (%d) for ", (float) btAtan(cot.toScalar()), (int) cot.isNaN());
1433 btAssert(ccw ? (t.dot(s) < 0) : (t.dot(s) > 0));
1438 if (minEdge == NULL)
1443 else if ((cmp = cot.compare(minCot)) < 0)
1448 else if ((cmp == 0) && (ccw == (getOrientation(minEdge, e, s, t) == COUNTER_CLOCKWISE)))
1453 #ifdef DEBUG_CONVEX_HULL
1458 } while (e != start->edges);
1463 void btConvexHullInternal::findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge*& e0, Edge*& e1, Vertex* stop0, Vertex* stop1)
1467 Point32 et0 = start0 ? start0->target->point : c0->point;
1468 Point32 et1 = start1 ? start1->target->point : c1->point;
1469 Point32 s = c1->point - c0->point;
1470 Point64 normal = ((start0 ? start0 : start1)->target->point - c0->point).cross(s);
1471 int64_t dist = c0->point.dot(normal);
1472 btAssert(!start1 || (start1->target->point.dot(normal) == dist));
1473 Point64 perp = s.cross(normal);
1474 btAssert(!perp.isZero());
1476 #ifdef DEBUG_CONVEX_HULL
1477 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);
1480 int64_t maxDot0 = et0.dot(perp);
1483 while (e0->target != stop0)
1485 Edge* e = e0->reverse->prev;
1486 if (e->target->point.dot(normal) < dist)
1490 btAssert(e->target->point.dot(normal) == dist);
1491 if (e->copy == mergeStamp)
1495 int64_t dot = e->target->point.dot(perp);
1502 et0 = e->target->point;
1506 int64_t maxDot1 = et1.dot(perp);
1509 while (e1->target != stop1)
1511 Edge* e = e1->reverse->next;
1512 if (e->target->point.dot(normal) < dist)
1516 btAssert(e->target->point.dot(normal) == dist);
1517 if (e->copy == mergeStamp)
1521 int64_t dot = e->target->point.dot(perp);
1528 et1 = e->target->point;
1532 #ifdef DEBUG_CONVEX_HULL
1533 printf(" Starting at %d %d\n", et0.index, et1.index);
1536 int64_t dx = maxDot1 - maxDot0;
1541 int64_t dy = (et1 - et0).dot(s);
1543 if (e0 && (e0->target != stop0))
1545 Edge* f0 = e0->next->reverse;
1546 if (f0->copy > mergeStamp)
1548 int64_t dx0 = (f0->target->point - et0).dot(perp);
1549 int64_t dy0 = (f0->target->point - et0).dot(s);
1550 if ((dx0 == 0) ? (dy0 < 0) : ((dx0 < 0) && (Rational64(dy0, dx0).compare(Rational64(dy, dx)) >= 0)))
1552 et0 = f0->target->point;
1553 dx = (et1 - et0).dot(perp);
1554 e0 = (e0 == start0) ? NULL : f0;
1560 if (e1 && (e1->target != stop1))
1562 Edge* f1 = e1->reverse->next;
1563 if (f1->copy > mergeStamp)
1565 Point32 d1 = f1->target->point - et1;
1566 if (d1.dot(normal) == 0)
1568 int64_t dx1 = d1.dot(perp);
1569 int64_t dy1 = d1.dot(s);
1570 int64_t dxn = (f1->target->point - et0).dot(perp);
1571 if ((dxn > 0) && ((dx1 == 0) ? (dy1 < 0) : ((dx1 < 0) && (Rational64(dy1, dx1).compare(Rational64(dy, dx)) > 0))))
1574 et1 = e1->target->point;
1581 btAssert((e1 == start1) && (d1.dot(normal) < 0));
1593 int64_t dy = (et1 - et0).dot(s);
1595 if (e1 && (e1->target != stop1))
1597 Edge* f1 = e1->prev->reverse;
1598 if (f1->copy > mergeStamp)
1600 int64_t dx1 = (f1->target->point - et1).dot(perp);
1601 int64_t dy1 = (f1->target->point - et1).dot(s);
1602 if ((dx1 == 0) ? (dy1 > 0) : ((dx1 < 0) && (Rational64(dy1, dx1).compare(Rational64(dy, dx)) <= 0)))
1604 et1 = f1->target->point;
1605 dx = (et1 - et0).dot(perp);
1606 e1 = (e1 == start1) ? NULL : f1;
1612 if (e0 && (e0->target != stop0))
1614 Edge* f0 = e0->reverse->prev;
1615 if (f0->copy > mergeStamp)
1617 Point32 d0 = f0->target->point - et0;
1618 if (d0.dot(normal) == 0)
1620 int64_t dx0 = d0.dot(perp);
1621 int64_t dy0 = d0.dot(s);
1622 int64_t dxn = (et1 - f0->target->point).dot(perp);
1623 if ((dxn < 0) && ((dx0 == 0) ? (dy0 > 0) : ((dx0 < 0) && (Rational64(dy0, dx0).compare(Rational64(dy, dx)) < 0))))
1626 et0 = e0->target->point;
1633 btAssert((e0 == start0) && (d0.dot(normal) < 0));
1641 #ifdef DEBUG_CONVEX_HULL
1642 printf(" Advanced edges to %d %d\n", et0.index, et1.index);
1647 void btConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1)
1662 Edge* toPrev0 = NULL;
1663 Edge* firstNew0 = NULL;
1664 Edge* pendingHead0 = NULL;
1665 Edge* pendingTail0 = NULL;
1667 Edge* toPrev1 = NULL;
1668 Edge* firstNew1 = NULL;
1669 Edge* pendingHead1 = NULL;
1670 Edge* pendingTail1 = NULL;
1673 if (mergeProjection(h0, h1, c0, c1))
1675 Point32 s = *c1 - *c0;
1676 Point64 normal = Point32(0, 0, -1).cross(s);
1677 Point64 t = s.cross(normal);
1678 btAssert(!t.isZero());
1680 Edge* e = c0->edges;
1681 Edge* start0 = NULL;
1686 int64_t dot = (*e->target - *c0).dot(normal);
1688 if ((dot == 0) && ((*e->target - *c0).dot(t) > 0))
1690 if (!start0 || (getOrientation(start0, e, s, Point32(0, 0, -1)) == CLOCKWISE))
1696 } while (e != c0->edges);
1700 Edge* start1 = NULL;
1705 int64_t dot = (*e->target - *c1).dot(normal);
1707 if ((dot == 0) && ((*e->target - *c1).dot(t) > 0))
1709 if (!start1 || (getOrientation(start1, e, s, Point32(0, 0, -1)) == COUNTER_CLOCKWISE))
1715 } while (e != c1->edges);
1718 if (start0 || start1)
1720 findEdgeForCoplanarFaces(c0, c1, start0, start1, NULL, NULL);
1723 c0 = start0->target;
1727 c1 = start1->target;
1731 prevPoint = c1->point;
1736 prevPoint = c1->point;
1740 Vertex* first0 = c0;
1741 Vertex* first1 = c1;
1742 bool firstRun = true;
1746 Point32 s = *c1 - *c0;
1747 Point32 r = prevPoint - c0->point;
1748 Point64 rxs = r.cross(s);
1749 Point64 sxrxs = s.cross(rxs);
1751 #ifdef DEBUG_CONVEX_HULL
1752 printf("\n Checking %d %d\n", c0->point.index, c1->point.index);
1754 Rational64 minCot0(0, 0);
1755 Edge* min0 = findMaxAngle(false, c0, s, rxs, sxrxs, minCot0);
1756 Rational64 minCot1(0, 0);
1757 Edge* min1 = findMaxAngle(true, c1, s, rxs, sxrxs, minCot1);
1760 Edge* e = newEdgePair(c0, c1);
1771 int cmp = !min0 ? 1 : !min1 ? -1 : minCot0.compare(minCot1);
1772 #ifdef DEBUG_CONVEX_HULL
1773 printf(" -> Result %d\n", cmp);
1775 if (firstRun || ((cmp >= 0) ? !minCot1.isNegativeInfinity() : !minCot0.isNegativeInfinity()))
1777 Edge* e = newEdgePair(c0, c1);
1780 pendingTail0->prev = e;
1786 e->next = pendingTail0;
1792 pendingTail1->next = e;
1798 e->prev = pendingTail1;
1805 #ifdef DEBUG_CONVEX_HULL
1806 printf(" Found min edges to %d %d\n", e0 ? e0->target->point.index : -1, e1 ? e1->target->point.index : -1);
1811 findEdgeForCoplanarFaces(c0, c1, e0, e1, NULL, NULL);
1814 if ((cmp >= 0) && e1)
1818 for (Edge* e = toPrev1->next, *n = NULL; e != min1; e = n)
1829 toPrev1->link(pendingHead1);
1833 min1->prev->link(pendingHead1);
1834 firstNew1 = pendingHead1;
1836 pendingTail1->link(min1);
1837 pendingHead1 = NULL;
1838 pendingTail1 = NULL;
1845 prevPoint = c1->point;
1847 toPrev1 = e1->reverse;
1850 if ((cmp <= 0) && e0)
1854 for (Edge* e = toPrev0->prev, *n = NULL; e != min0; e = n)
1865 pendingHead0->link(toPrev0);
1869 pendingHead0->link(min0->next);
1870 firstNew0 = pendingHead0;
1872 min0->link(pendingTail0);
1873 pendingHead0 = NULL;
1874 pendingTail0 = NULL;
1881 prevPoint = c0->point;
1883 toPrev0 = e0->reverse;
1887 if ((c0 == first0) && (c1 == first1))
1889 if (toPrev0 == NULL)
1891 pendingHead0->link(pendingTail0);
1892 c0->edges = pendingTail0;
1896 for (Edge* e = toPrev0->prev, *n = NULL; e != firstNew0; e = n)
1903 pendingHead0->link(toPrev0);
1904 firstNew0->link(pendingTail0);
1908 if (toPrev1 == NULL)
1910 pendingTail1->link(pendingHead1);
1911 c1->edges = pendingTail1;
1915 for (Edge* e = toPrev1->next, *n = NULL; e != firstNew1; e = n)
1922 toPrev1->link(pendingHead1);
1923 pendingTail1->link(firstNew1);
1935 static bool pointCmp(const btConvexHullInternal::Point32& p, const btConvexHullInternal::Point32& q)
1937 return (p.y < q.y) || ((p.y == q.y) && ((p.x < q.x) || ((p.x == q.x) && (p.z < q.z))));
1940 void btConvexHullInternal::compute(const void* coords, bool doubleCoords, int stride, int count)
1942 btVector3 min(btScalar(1e30), btScalar(1e30), btScalar(1e30)), max(btScalar(-1e30), btScalar(-1e30), btScalar(-1e30));
1943 const char* ptr = (const char*) coords;
1946 for (int i = 0; i < count; i++)
1948 const double* v = (const double*) ptr;
1949 btVector3 p((btScalar) v[0], (btScalar) v[1], (btScalar) v[2]);
1957 for (int i = 0; i < count; i++)
1959 const float* v = (const float*) ptr;
1960 btVector3 p(v[0], v[1], v[2]);
1967 btVector3 s = max - min;
1968 maxAxis = s.maxAxis();
1969 minAxis = s.minAxis();
1970 if (minAxis == maxAxis)
1972 minAxis = (maxAxis + 1) % 3;
1974 medAxis = 3 - maxAxis - minAxis;
1976 s /= btScalar(10216);
1977 if (((medAxis + 1) % 3) != maxAxis)
1985 s[0] = btScalar(1) / s[0];
1989 s[1] = btScalar(1) / s[1];
1993 s[2] = btScalar(1) / s[2];
1996 center = (min + max) * btScalar(0.5);
1998 btAlignedObjectArray<Point32> points;
1999 points.resize(count);
2000 ptr = (const char*) coords;
2003 for (int i = 0; i < count; i++)
2005 const double* v = (const double*) ptr;
2006 btVector3 p((btScalar) v[0], (btScalar) v[1], (btScalar) v[2]);
2008 p = (p - center) * s;
2009 points[i].x = (int32_t) p[medAxis];
2010 points[i].y = (int32_t) p[maxAxis];
2011 points[i].z = (int32_t) p[minAxis];
2012 points[i].index = i;
2017 for (int i = 0; i < count; i++)
2019 const float* v = (const float*) ptr;
2020 btVector3 p(v[0], v[1], v[2]);
2022 p = (p - center) * s;
2023 points[i].x = (int32_t) p[medAxis];
2024 points[i].y = (int32_t) p[maxAxis];
2025 points[i].z = (int32_t) p[minAxis];
2026 points[i].index = i;
2029 points.quickSort(pointCmp);
2032 vertexPool.setArraySize(count);
2033 originalVertices.resize(count);
2034 for (int i = 0; i < count; i++)
2036 Vertex* v = vertexPool.newObject();
2038 v->point = points[i];
2040 originalVertices[i] = v;
2046 edgePool.setArraySize(6 * count);
2049 maxUsedEdgePairs = 0;
2053 IntermediateHull hull;
2054 computeInternal(0, count, hull);
2055 vertexList = hull.minXy;
2056 #ifdef DEBUG_CONVEX_HULL
2057 printf("max. edges %d (3v = %d)", maxUsedEdgePairs, 3 * count);
2061 btVector3 btConvexHullInternal::toBtVector(const Point32& v)
2064 p[medAxis] = btScalar(v.x);
2065 p[maxAxis] = btScalar(v.y);
2066 p[minAxis] = btScalar(v.z);
2070 btVector3 btConvexHullInternal::getBtNormal(Face* face)
2072 return toBtVector(face->dir0).cross(toBtVector(face->dir1)).normalized();
2075 btVector3 btConvexHullInternal::getCoordinates(const Vertex* v)
2078 p[medAxis] = v->xvalue();
2079 p[maxAxis] = v->yvalue();
2080 p[minAxis] = v->zvalue();
2081 return p * scaling + center;
2084 btScalar btConvexHullInternal::shrink(btScalar amount, btScalar clampAmount)
2090 int stamp = --mergeStamp;
2091 btAlignedObjectArray<Vertex*> stack;
2092 vertexList->copy = stamp;
2093 stack.push_back(vertexList);
2094 btAlignedObjectArray<Face*> faces;
2096 Point32 ref = vertexList->point;
2097 Int128 hullCenterX(0, 0);
2098 Int128 hullCenterY(0, 0);
2099 Int128 hullCenterZ(0, 0);
2100 Int128 volume(0, 0);
2102 while (stack.size() > 0)
2104 Vertex* v = stack[stack.size() - 1];
2111 if (e->target->copy != stamp)
2113 e->target->copy = stamp;
2114 stack.push_back(e->target);
2116 if (e->copy != stamp)
2118 Face* face = facePool.newObject();
2119 face->init(e->target, e->reverse->prev->target, v);
2120 faces.push_back(face);
2129 int64_t vol = (v->point - ref).dot((a->point - ref).cross(b->point - ref));
2131 Point32 c = v->point + a->point + b->point + ref;
2132 hullCenterX += vol * c.x;
2133 hullCenterY += vol * c.y;
2134 hullCenterZ += vol * c.z;
2138 btAssert(f->copy != stamp);
2145 f = f->reverse->prev;
2149 } while (e != v->edges);
2153 if (volume.getSign() <= 0)
2158 btVector3 hullCenter;
2159 hullCenter[medAxis] = hullCenterX.toScalar();
2160 hullCenter[maxAxis] = hullCenterY.toScalar();
2161 hullCenter[minAxis] = hullCenterZ.toScalar();
2162 hullCenter /= 4 * volume.toScalar();
2163 hullCenter *= scaling;
2165 int faceCount = faces.size();
2167 if (clampAmount > 0)
2169 btScalar minDist = SIMD_INFINITY;
2170 for (int i = 0; i < faceCount; i++)
2172 btVector3 normal = getBtNormal(faces[i]);
2173 btScalar dist = normal.dot(toBtVector(faces[i]->origin) - hullCenter);
2185 amount = btMin(amount, minDist * clampAmount);
2188 unsigned int seed = 243703;
2189 for (int i = 0; i < faceCount; i++, seed = 1664525 * seed + 1013904223)
2191 btSwap(faces[i], faces[seed % faceCount]);
2194 for (int i = 0; i < faceCount; i++)
2196 if (!shiftFace(faces[i], amount, stack))
2205 bool btConvexHullInternal::shiftFace(Face* face, btScalar amount, btAlignedObjectArray<Vertex*> stack)
2207 btVector3 origShift = getBtNormal(face) * -amount;
2208 if (scaling[0] != 0)
2210 origShift[0] /= scaling[0];
2212 if (scaling[1] != 0)
2214 origShift[1] /= scaling[1];
2216 if (scaling[2] != 0)
2218 origShift[2] /= scaling[2];
2220 Point32 shift((int32_t) origShift[medAxis], (int32_t) origShift[maxAxis], (int32_t) origShift[minAxis]);
2225 Point64 normal = face->getNormal();
2226 #ifdef DEBUG_CONVEX_HULL
2227 printf("\nShrinking face (%d %d %d) (%d %d %d) (%d %d %d) by (%d %d %d)\n",
2228 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);
2230 int64_t origDot = face->origin.dot(normal);
2231 Point32 shiftedOrigin = face->origin + shift;
2232 int64_t shiftedDot = shiftedOrigin.dot(normal);
2233 btAssert(shiftedDot <= origDot);
2234 if (shiftedDot >= origDot)
2239 Edge* intersection = NULL;
2241 Edge* startEdge = face->nearbyVertex->edges;
2242 #ifdef DEBUG_CONVEX_HULL
2243 printf("Start edge is ");
2245 printf(", normal is (%lld %lld %lld), shifted dot is %lld\n", normal.x, normal.y, normal.z, shiftedDot);
2247 Rational128 optDot = face->nearbyVertex->dot(normal);
2248 int cmp = optDot.compare(shiftedDot);
2249 #ifdef SHOW_ITERATIONS
2254 Edge* e = startEdge;
2257 #ifdef SHOW_ITERATIONS
2260 Rational128 dot = e->target->dot(normal);
2261 btAssert(dot.compare(origDot) <= 0);
2262 #ifdef DEBUG_CONVEX_HULL
2263 printf("Moving downwards, edge is ");
2265 printf(", dot is %f (%f %lld)\n", (float) dot.toScalar(), (float) optDot.toScalar(), shiftedDot);
2267 if (dot.compare(optDot) < 0)
2269 int c = dot.compare(shiftedDot);
2281 } while (e != startEdge);
2290 Edge* e = startEdge;
2293 #ifdef SHOW_ITERATIONS
2296 Rational128 dot = e->target->dot(normal);
2297 btAssert(dot.compare(origDot) <= 0);
2298 #ifdef DEBUG_CONVEX_HULL
2299 printf("Moving upwards, edge is ");
2301 printf(", dot is %f (%f %lld)\n", (float) dot.toScalar(), (float) optDot.toScalar(), shiftedDot);
2303 if (dot.compare(optDot) > 0)
2305 cmp = dot.compare(shiftedDot);
2316 } while (e != startEdge);
2324 #ifdef SHOW_ITERATIONS
2325 printf("Needed %d iterations to find initial intersection\n", n);
2330 Edge* e = intersection->reverse->next;
2331 #ifdef SHOW_ITERATIONS
2334 while (e->target->dot(normal).compare(shiftedDot) <= 0)
2336 #ifdef SHOW_ITERATIONS
2340 if (e == intersection->reverse)
2344 #ifdef DEBUG_CONVEX_HULL
2345 printf("Checking for outwards edge, current edge is ");
2350 #ifdef SHOW_ITERATIONS
2351 printf("Needed %d iterations to check for complete containment\n", n);
2355 Edge* firstIntersection = NULL;
2356 Edge* faceEdge = NULL;
2357 Edge* firstFaceEdge = NULL;
2359 #ifdef SHOW_ITERATIONS
2364 #ifdef SHOW_ITERATIONS
2367 #ifdef DEBUG_CONVEX_HULL
2368 printf("Intersecting edge is ");
2369 intersection->print();
2374 Edge* e = intersection->reverse->next;
2376 #ifdef SHOW_ITERATIONS
2381 #ifdef SHOW_ITERATIONS
2384 if (e->target->dot(normal).compare(shiftedDot) >= 0)
2388 intersection = e->reverse;
2395 #ifdef SHOW_ITERATIONS
2396 printf("Needed %d iterations to advance intersection\n", n);
2400 #ifdef DEBUG_CONVEX_HULL
2401 printf("Advanced intersecting edge to ");
2402 intersection->print();
2403 printf(", cmp = %d\n", cmp);
2406 if (!firstIntersection)
2408 firstIntersection = intersection;
2410 else if (intersection == firstIntersection)
2416 Edge* prevIntersection = intersection;
2417 Edge* prevFaceEdge = faceEdge;
2419 Edge* e = intersection->reverse;
2420 #ifdef SHOW_ITERATIONS
2425 #ifdef SHOW_ITERATIONS
2428 e = e->reverse->prev;
2429 btAssert(e != intersection->reverse);
2430 cmp = e->target->dot(normal).compare(shiftedDot);
2431 #ifdef DEBUG_CONVEX_HULL
2432 printf("Testing edge ");
2434 printf(" -> cmp = %d\n", cmp);
2442 #ifdef SHOW_ITERATIONS
2443 printf("Needed %d iterations to find other intersection of face\n", n);
2448 Vertex* removed = intersection->target;
2449 e = intersection->reverse;
2452 removed->edges = NULL;
2456 removed->edges = e->prev;
2457 e->prev->link(e->next);
2460 #ifdef DEBUG_CONVEX_HULL
2461 printf("1: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z);
2464 Point64 n0 = intersection->face->getNormal();
2465 Point64 n1 = intersection->reverse->face->getNormal();
2466 int64_t m00 = face->dir0.dot(n0);
2467 int64_t m01 = face->dir1.dot(n0);
2468 int64_t m10 = face->dir0.dot(n1);
2469 int64_t m11 = face->dir1.dot(n1);
2470 int64_t r0 = (intersection->face->origin - shiftedOrigin).dot(n0);
2471 int64_t r1 = (intersection->reverse->face->origin - shiftedOrigin).dot(n1);
2472 Int128 det = Int128::mul(m00, m11) - Int128::mul(m01, m10);
2473 btAssert(det.getSign() != 0);
2474 Vertex* v = vertexPool.newObject();
2475 v->point.index = -1;
2477 v->point128 = PointR128(Int128::mul(face->dir0.x * r0, m11) - Int128::mul(face->dir0.x * r1, m01)
2478 + Int128::mul(face->dir1.x * r1, m00) - Int128::mul(face->dir1.x * r0, m10) + det * shiftedOrigin.x,
2479 Int128::mul(face->dir0.y * r0, m11) - Int128::mul(face->dir0.y * r1, m01)
2480 + Int128::mul(face->dir1.y * r1, m00) - Int128::mul(face->dir1.y * r0, m10) + det * shiftedOrigin.y,
2481 Int128::mul(face->dir0.z * r0, m11) - Int128::mul(face->dir0.z * r1, m01)
2482 + Int128::mul(face->dir1.z * r1, m00) - Int128::mul(face->dir1.z * r0, m10) + det * shiftedOrigin.z,
2484 v->point.x = (int32_t) v->point128.xvalue();
2485 v->point.y = (int32_t) v->point128.yvalue();
2486 v->point.z = (int32_t) v->point128.zvalue();
2487 intersection->target = v;
2491 stack.push_back(removed);
2492 stack.push_back(NULL);
2495 if (cmp || prevCmp || (prevIntersection->reverse->next->target != intersection->target))
2497 faceEdge = newEdgePair(prevIntersection->target, intersection->target);
2500 faceEdge->link(prevIntersection->reverse->next);
2502 if ((prevCmp == 0) || prevFaceEdge)
2504 prevIntersection->reverse->link(faceEdge);
2508 intersection->reverse->prev->link(faceEdge->reverse);
2510 faceEdge->reverse->link(intersection->reverse);
2514 faceEdge = prevIntersection->reverse->next;
2521 faceEdge->link(prevFaceEdge->reverse);
2523 else if (faceEdge != prevFaceEdge->reverse)
2525 stack.push_back(prevFaceEdge->target);
2526 while (faceEdge->next != prevFaceEdge->reverse)
2528 Vertex* removed = faceEdge->next->target;
2529 removeEdgePair(faceEdge->next);
2530 stack.push_back(removed);
2531 #ifdef DEBUG_CONVEX_HULL
2532 printf("2: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z);
2535 stack.push_back(NULL);
2538 faceEdge->face = face;
2539 faceEdge->reverse->face = intersection->face;
2543 firstFaceEdge = faceEdge;
2546 #ifdef SHOW_ITERATIONS
2547 printf("Needed %d iterations to process all intersections\n", m);
2552 firstFaceEdge->reverse->target = faceEdge->target;
2553 firstIntersection->reverse->link(firstFaceEdge);
2554 firstFaceEdge->link(faceEdge->reverse);
2556 else if (firstFaceEdge != faceEdge->reverse)
2558 stack.push_back(faceEdge->target);
2559 while (firstFaceEdge->next != faceEdge->reverse)
2561 Vertex* removed = firstFaceEdge->next->target;
2562 removeEdgePair(firstFaceEdge->next);
2563 stack.push_back(removed);
2564 #ifdef DEBUG_CONVEX_HULL
2565 printf("3: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z);
2568 stack.push_back(NULL);
2571 btAssert(stack.size() > 0);
2572 vertexList = stack[0];
2574 #ifdef DEBUG_CONVEX_HULL
2575 printf("Removing part\n");
2577 #ifdef SHOW_ITERATIONS
2581 while (pos < stack.size())
2583 int end = stack.size();
2586 Vertex* kept = stack[pos++];
2587 #ifdef DEBUG_CONVEX_HULL
2590 bool deeper = false;
2592 while ((removed = stack[pos++]) != NULL)
2594 #ifdef SHOW_ITERATIONS
2597 kept->receiveNearbyFaces(removed);
2598 while (removed->edges)
2603 stack.push_back(kept);
2605 stack.push_back(removed->edges->target);
2606 removeEdgePair(removed->edges);
2611 stack.push_back(NULL);
2615 #ifdef SHOW_ITERATIONS
2616 printf("Needed %d iterations to remove part\n", n);
2620 face->origin = shiftedOrigin;
2626 static int getVertexCopy(btConvexHullInternal::Vertex* vertex, btAlignedObjectArray<btConvexHullInternal::Vertex*>& vertices)
2628 int index = vertex->copy;
2631 index = vertices.size();
2632 vertex->copy = index;
2633 vertices.push_back(vertex);
2634 #ifdef DEBUG_CONVEX_HULL
2635 printf("Vertex %d gets index *%d\n", vertex->point.index, index);
2641 btScalar btConvexHullComputer::compute(const void* coords, bool doubleCoords, int stride, int count, btScalar shrink, btScalar shrinkClamp)
2651 btConvexHullInternal hull;
2652 hull.compute(coords, doubleCoords, stride, count);
2655 if ((shrink > 0) && ((shift = hull.shrink(shrink, shrinkClamp)) < 0))
2667 btAlignedObjectArray<btConvexHullInternal::Vertex*> oldVertices;
2668 getVertexCopy(hull.vertexList, oldVertices);
2670 while (copied < oldVertices.size())
2672 btConvexHullInternal::Vertex* v = oldVertices[copied];
2673 vertices.push_back(hull.getCoordinates(v));
2674 btConvexHullInternal::Edge* firstEdge = v->edges;
2679 btConvexHullInternal::Edge* e = firstEdge;
2684 int s = edges.size();
2685 edges.push_back(Edge());
2686 edges.push_back(Edge());
2687 Edge* c = &edges[s];
2688 Edge* r = &edges[s + 1];
2690 e->reverse->copy = s + 1;
2693 c->targetVertex = getVertexCopy(e->target, oldVertices);
2694 r->targetVertex = copied;
2695 #ifdef DEBUG_CONVEX_HULL
2696 printf(" CREATE: Vertex *%d has edge to *%d\n", copied, c->getTargetVertex());
2701 edges[e->copy].next = prevCopy - e->copy;
2705 firstCopy = e->copy;
2709 } while (e != firstEdge);
2710 edges[firstCopy].next = prevCopy - firstCopy;
2715 for (int i = 0; i < copied; i++)
2717 btConvexHullInternal::Vertex* v = oldVertices[i];
2718 btConvexHullInternal::Edge* firstEdge = v->edges;
2721 btConvexHullInternal::Edge* e = firstEdge;
2726 #ifdef DEBUG_CONVEX_HULL
2727 printf("Vertex *%d has edge to *%d\n", i, edges[e->copy].getTargetVertex());
2729 faces.push_back(e->copy);
2730 btConvexHullInternal::Edge* f = e;
2733 #ifdef DEBUG_CONVEX_HULL
2734 printf(" Face *%d\n", edges[f->copy].getTargetVertex());
2737 f = f->reverse->prev;
2741 } while (e != firstEdge);