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;
36 //The definition of USE_X86_64_ASM is moved into the build system. You can enable it manually by commenting out the following lines
37 //#if (defined(__GNUC__) && defined(__x86_64__) && !defined(__ICL)) // || (defined(__ICL) && defined(_M_X64)) bug in Intel compiler, disable inline assembly
38 // #define USE_X86_64_ASM
41 //#define DEBUG_CONVEX_HULL
42 //#define SHOW_ITERATIONS
44 #if defined(DEBUG_CONVEX_HULL) || defined(SHOW_ITERATIONS)
48 // Convex hull implementation based on Preparata and Hong
49 // Ole Kniemeyer, MAXON Computer GmbH
50 class btConvexHullInternal
60 Point64(int64_t x, int64_t y, int64_t z) : x(x), y(y), z(z)
66 return (x == 0) && (y == 0) && (z == 0);
69 int64_t dot(const Point64& b) const
71 return x * b.x + y * b.y + z * b.z;
87 Point32(int32_t x, int32_t y, int32_t z) : x(x), y(y), z(z), index(-1)
91 bool operator==(const Point32& b) const
93 return (x == b.x) && (y == b.y) && (z == b.z);
96 bool operator!=(const Point32& b) const
98 return (x != b.x) || (y != b.y) || (z != b.z);
103 return (x == 0) && (y == 0) && (z == 0);
106 Point64 cross(const Point32& b) const
108 return Point64(((int64_t)y) * b.z - ((int64_t)z) * b.y, ((int64_t)z) * b.x - ((int64_t)x) * b.z, ((int64_t)x) * b.y - ((int64_t)y) * b.x);
111 Point64 cross(const Point64& b) const
113 return Point64(y * b.z - z * b.y, z * b.x - x * b.z, x * b.y - y * b.x);
116 int64_t dot(const Point32& b) const
118 return ((int64_t)x) * b.x + ((int64_t)y) * b.y + ((int64_t)z) * b.z;
121 int64_t dot(const Point64& b) const
123 return x * b.x + y * b.y + z * b.z;
126 Point32 operator+(const Point32& b) const
128 return Point32(x + b.x, y + b.y, z + b.z);
131 Point32 operator-(const Point32& b) const
133 return Point32(x - b.x, y - b.y, z - b.z);
147 Int128(uint64_t low, uint64_t high) : low(low), high(high)
151 Int128(uint64_t low) : low(low), high(0)
155 Int128(int64_t value) : low(value), high((value >= 0) ? 0 : (uint64_t)-1LL)
159 static Int128 mul(int64_t a, int64_t b);
161 static Int128 mul(uint64_t a, uint64_t b);
163 Int128 operator-() const
165 return Int128((uint64_t) - (int64_t)low, ~high + (low == 0));
168 Int128 operator+(const Int128& b) const
170 #ifdef USE_X86_64_ASM
173 "addq %[bl], %[rl]\n\t"
174 "adcq %[bh], %[rh]\n\t"
175 : [rl] "=r"(result.low), [rh] "=r"(result.high)
176 : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high)
180 uint64_t lo = low + b.low;
181 return Int128(lo, high + b.high + (lo < low));
185 Int128 operator-(const Int128& b) const
187 #ifdef USE_X86_64_ASM
190 "subq %[bl], %[rl]\n\t"
191 "sbbq %[bh], %[rh]\n\t"
192 : [rl] "=r"(result.low), [rh] "=r"(result.high)
193 : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high)
201 Int128& operator+=(const Int128& b)
203 #ifdef USE_X86_64_ASM
205 "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
274 uint64_t m_numerator;
275 uint64_t m_denominator;
279 Rational64(int64_t numerator, int64_t denominator)
284 m_numerator = (uint64_t)numerator;
286 else if (numerator < 0)
289 m_numerator = (uint64_t)-numerator;
298 m_denominator = (uint64_t)denominator;
300 else if (denominator < 0)
303 m_denominator = (uint64_t)-denominator;
311 bool isNegativeInfinity() const
313 return (sign < 0) && (m_denominator == 0);
318 return (sign == 0) && (m_denominator == 0);
321 int compare(const Rational64& b) const;
323 btScalar toScalar() const
325 return sign * ((m_denominator == 0) ? SIMD_INFINITY : (btScalar)m_numerator / m_denominator);
338 Rational128(int64_t value)
343 this->numerator = value;
348 this->numerator = -value;
353 this->numerator = (uint64_t)0;
355 this->denominator = (uint64_t)1;
359 Rational128(const Int128& numerator, const Int128& denominator)
361 sign = numerator.getSign();
364 this->numerator = numerator;
368 this->numerator = -numerator;
370 int dsign = denominator.getSign();
373 this->denominator = denominator;
378 this->denominator = -denominator;
383 int compare(const Rational128& b) const;
385 int compare(int64_t b) const;
387 btScalar toScalar() const
389 return sign * ((denominator.getSign() == 0) ? SIMD_INFINITY : numerator.toScalar() / denominator.toScalar());
405 PointR128(Int128 x, Int128 y, Int128 z, Int128 denominator) : x(x), y(y), z(z), denominator(denominator)
409 btScalar xvalue() const
411 return x.toScalar() / denominator.toScalar();
414 btScalar yvalue() const
416 return y.toScalar() / denominator.toScalar();
419 btScalar zvalue() const
421 return z.toScalar() / denominator.toScalar();
434 Face* firstNearbyFace;
435 Face* lastNearbyFace;
440 Vertex() : next(NULL), prev(NULL), edges(NULL), firstNearbyFace(NULL), lastNearbyFace(NULL), copy(-1)
444 #ifdef DEBUG_CONVEX_HULL
447 printf("V%d (%d, %d, %d)", point.index, point.x, point.y, point.z);
453 Point32 operator-(const Vertex& b) const
455 return point - b.point;
458 Rational128 dot(const Point64& b) const
460 return (point.index >= 0) ? Rational128(point.dot(b))
461 : Rational128(point128.x * b.x + point128.y * b.y + point128.z * b.z, point128.denominator);
464 btScalar xvalue() const
466 return (point.index >= 0) ? btScalar(point.x) : point128.xvalue();
469 btScalar yvalue() const
471 return (point.index >= 0) ? btScalar(point.y) : point128.yvalue();
474 btScalar zvalue() const
476 return (point.index >= 0) ? btScalar(point.z) : point128.zvalue();
479 void receiveNearbyFaces(Vertex* src)
483 lastNearbyFace->nextWithSameNearbyVertex = src->firstNearbyFace;
487 firstNearbyFace = src->firstNearbyFace;
489 if (src->lastNearbyFace)
491 lastNearbyFace = src->lastNearbyFace;
493 for (Face* f = src->firstNearbyFace; f; f = f->nextWithSameNearbyVertex)
495 btAssert(f->nearbyVertex == src);
496 f->nearbyVertex = this;
498 src->firstNearbyFace = NULL;
499 src->lastNearbyFace = NULL;
524 btAssert(reverse->target == n->reverse->target);
529 #ifdef DEBUG_CONVEX_HULL
532 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,
533 reverse->target->point.x, reverse->target->point.y, reverse->target->point.z, target->point.x, target->point.y, target->point.z);
542 Vertex* nearbyVertex;
543 Face* nextWithSameNearbyVertex;
548 Face() : next(NULL), nearbyVertex(NULL), nextWithSameNearbyVertex(NULL)
552 void init(Vertex* a, Vertex* b, Vertex* c)
558 if (a->lastNearbyFace)
560 a->lastNearbyFace->nextWithSameNearbyVertex = this;
564 a->firstNearbyFace = this;
566 a->lastNearbyFace = this;
571 return dir0.cross(dir1);
575 template <typename UWord, typename UHWord>
579 static uint32_t high(uint64_t value)
581 return (uint32_t)(value >> 32);
584 static uint32_t low(uint64_t value)
586 return (uint32_t)value;
589 static uint64_t mul(uint32_t a, uint32_t b)
591 return (uint64_t)a * (uint64_t)b;
594 static void shlHalf(uint64_t& value)
599 static uint64_t high(Int128 value)
604 static uint64_t low(Int128 value)
609 static Int128 mul(uint64_t a, uint64_t b)
611 return Int128::mul(a, b);
614 static void shlHalf(Int128& value)
616 value.high = value.low;
621 static void mul(UWord a, UWord b, UWord& resLow, UWord& resHigh)
623 UWord p00 = mul(low(a), low(b));
624 UWord p01 = mul(low(a), high(b));
625 UWord p10 = mul(high(a), low(b));
626 UWord p11 = mul(high(a), high(b));
627 UWord p0110 = UWord(low(p01)) + UWord(low(p10));
643 class IntermediateHull
651 IntermediateHull() : minXy(NULL), maxXy(NULL), minYx(NULL), maxYx(NULL)
665 template <typename T>
675 PoolArray(int size) : size(size), next(NULL)
677 array = (T*)btAlignedAlloc(sizeof(T) * size, 16);
682 btAlignedFree(array);
688 for (int i = 0; i < size; i++, o++)
690 o->next = (i + 1 < size) ? o + 1 : NULL;
696 template <typename T>
700 PoolArray<T>* arrays;
701 PoolArray<T>* nextArray;
706 Pool() : arrays(NULL), nextArray(NULL), freeObjects(NULL), arraySize(256)
714 PoolArray<T>* p = arrays;
727 void setArraySize(int arraySize)
729 this->arraySize = arraySize;
737 PoolArray<T>* p = nextArray;
744 p = new (btAlignedAlloc(sizeof(PoolArray<T>), 16)) PoolArray<T>(arraySize);
750 freeObjects = o->next;
754 void freeObject(T* object)
757 object->next = freeObjects;
758 freeObjects = object;
764 Pool<Vertex> vertexPool;
767 btAlignedObjectArray<Vertex*> originalVertices;
773 int maxUsedEdgePairs;
775 static Orientation getOrientation(const Edge* prev, const Edge* next, const Point32& s, const Point32& t);
776 Edge* findMaxAngle(bool ccw, const Vertex* start, const Point32& s, const Point64& rxs, const Point64& sxrxs, Rational64& minCot);
777 void findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge*& e0, Edge*& e1, Vertex* stop0, Vertex* stop1);
779 Edge* newEdgePair(Vertex* from, Vertex* to);
781 void removeEdgePair(Edge* edge)
783 Edge* n = edge->next;
784 Edge* r = edge->reverse;
786 btAssert(edge->target && r->target);
790 n->prev = edge->prev;
791 edge->prev->next = n;
792 r->target->edges = n;
796 r->target->edges = NULL;
805 edge->target->edges = n;
809 edge->target->edges = NULL;
812 edgePool.freeObject(edge);
813 edgePool.freeObject(r);
817 void computeInternal(int start, int end, IntermediateHull& result);
819 bool mergeProjection(IntermediateHull& h0, IntermediateHull& h1, Vertex*& c0, Vertex*& c1);
821 void merge(IntermediateHull& h0, IntermediateHull& h1);
823 btVector3 toBtVector(const Point32& v);
825 btVector3 getBtNormal(Face* face);
827 bool shiftFace(Face* face, btScalar amount, btAlignedObjectArray<Vertex*> stack);
832 void compute(const void* coords, bool doubleCoords, int stride, int count);
834 btVector3 getCoordinates(const Vertex* v);
836 btScalar shrink(btScalar amount, btScalar clampAmount);
839 btConvexHullInternal::Int128 btConvexHullInternal::Int128::operator*(int64_t b) const
841 bool negative = (int64_t)high < 0;
842 Int128 a = negative ? -*this : *this;
845 negative = !negative;
848 Int128 result = mul(a.low, (uint64_t)b);
849 result.high += a.high * (uint64_t)b;
850 return negative ? -result : result;
853 btConvexHullInternal::Int128 btConvexHullInternal::Int128::mul(int64_t a, int64_t b)
857 #ifdef USE_X86_64_ASM
859 : "=a"(result.low), "=d"(result.high)
865 bool negative = a < 0;
872 negative = !negative;
875 DMul<uint64_t, uint32_t>::mul((uint64_t)a, (uint64_t)b, result.low, result.high);
876 return negative ? -result : result;
880 btConvexHullInternal::Int128 btConvexHullInternal::Int128::mul(uint64_t a, uint64_t b)
884 #ifdef USE_X86_64_ASM
886 : "=a"(result.low), "=d"(result.high)
891 DMul<uint64_t, uint32_t>::mul(a, b, result.low, result.high);
897 int btConvexHullInternal::Rational64::compare(const Rational64& b) const
901 return sign - b.sign;
908 // return (numerator * b.denominator > b.numerator * denominator) ? sign : (numerator * b.denominator < b.numerator * denominator) ? -sign : 0;
910 #ifdef USE_X86_64_ASM
917 "movq %%rax, %[tmp]\n\t"
918 "movq %%rdx, %%rbx\n\t"
919 "movq %[tn], %%rax\n\t"
921 "subq %[tmp], %%rax\n\t"
922 "sbbq %%rbx, %%rdx\n\t" // rdx:rax contains 128-bit-difference "numerator*b.denominator - b.numerator*denominator"
923 "setnsb %%bh\n\t" // bh=1 if difference is non-negative, bh=0 otherwise
924 "orq %%rdx, %%rax\n\t"
925 "setnzb %%bl\n\t" // bl=1 if difference if non-zero, bl=0 if it is zero
926 "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)
927 "shll $16, %%ebx\n\t" // ebx has same sign as difference
928 : "=&b"(result), [tmp] "=&r"(tmp), "=a"(dummy)
929 : "a"(m_denominator), [bn] "g"(b.m_numerator), [tn] "g"(m_numerator), [bd] "g"(b.m_denominator)
931 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)
932 // if sign is -1, all bits of result are inverted, which changes the sign of result (and again cannot result in zero)
937 return sign * Int128::mul(m_numerator, b.m_denominator).ucmp(Int128::mul(m_denominator, b.m_numerator));
942 int btConvexHullInternal::Rational128::compare(const Rational128& b) const
946 return sign - b.sign;
954 return -b.compare(sign * (int64_t)numerator.low);
957 Int128 nbdLow, nbdHigh, dbnLow, dbnHigh;
958 DMul<Int128, uint64_t>::mul(numerator, b.denominator, nbdLow, nbdHigh);
959 DMul<Int128, uint64_t>::mul(denominator, b.numerator, dbnLow, dbnHigh);
961 int cmp = nbdHigh.ucmp(dbnHigh);
966 return nbdLow.ucmp(dbnLow) * sign;
969 int btConvexHullInternal::Rational128::compare(int64_t b) const
973 int64_t a = sign * (int64_t)numerator.low;
974 return (a > b) ? 1 : (a < b) ? -1 : 0;
996 return numerator.ucmp(denominator * b) * sign;
999 btConvexHullInternal::Edge* btConvexHullInternal::newEdgePair(Vertex* from, Vertex* to)
1001 btAssert(from && to);
1002 Edge* e = edgePool.newObject();
1003 Edge* r = edgePool.newObject();
1006 e->copy = mergeStamp;
1007 r->copy = mergeStamp;
1013 if (usedEdgePairs > maxUsedEdgePairs)
1015 maxUsedEdgePairs = usedEdgePairs;
1020 bool btConvexHullInternal::mergeProjection(IntermediateHull& h0, IntermediateHull& h1, Vertex*& c0, Vertex*& c1)
1022 Vertex* v0 = h0.maxYx;
1023 Vertex* v1 = h1.minYx;
1024 if ((v0->point.x == v1->point.x) && (v0->point.y == v1->point.y))
1026 btAssert(v0->point.z < v1->point.z);
1027 Vertex* v1p = v1->prev;
1033 btAssert(v1->edges->next == v1->edges);
1034 v1 = v1->edges->target;
1035 btAssert(v1->edges->next == v1->edges);
1040 Vertex* v1n = v1->next;
1045 if ((v1n->point.x < v1p->point.x) || ((v1n->point.x == v1p->point.x) && (v1n->point.y < v1p->point.y)))
1056 if ((v1n->point.x > v1p->point.x) || ((v1n->point.x == v1p->point.x) && (v1n->point.y > v1p->point.y)))
1073 for (int side = 0; side <= 1; side++)
1075 int32_t dx = (v1->point.x - v0->point.x) * sign;
1080 int32_t dy = v1->point.y - v0->point.y;
1082 Vertex* w0 = side ? v0->next : v0->prev;
1085 int32_t dx0 = (w0->point.x - v0->point.x) * sign;
1086 int32_t dy0 = w0->point.y - v0->point.y;
1087 if ((dy0 <= 0) && ((dx0 == 0) || ((dx0 < 0) && (dy0 * dx <= dy * dx0))))
1090 dx = (v1->point.x - v0->point.x) * sign;
1095 Vertex* w1 = side ? v1->next : v1->prev;
1098 int32_t dx1 = (w1->point.x - v1->point.x) * sign;
1099 int32_t dy1 = w1->point.y - v1->point.y;
1100 int32_t dxn = (w1->point.x - v0->point.x) * sign;
1101 if ((dxn > 0) && (dy1 < 0) && ((dx1 == 0) || ((dx1 < 0) && (dy1 * dx < dy * dx1))))
1116 int32_t dy = v1->point.y - v0->point.y;
1118 Vertex* w1 = side ? v1->prev : v1->next;
1121 int32_t dx1 = (w1->point.x - v1->point.x) * sign;
1122 int32_t dy1 = w1->point.y - v1->point.y;
1123 if ((dy1 >= 0) && ((dx1 == 0) || ((dx1 < 0) && (dy1 * dx <= dy * dx1))))
1126 dx = (v1->point.x - v0->point.x) * sign;
1131 Vertex* w0 = side ? v0->prev : v0->next;
1134 int32_t dx0 = (w0->point.x - v0->point.x) * sign;
1135 int32_t dy0 = w0->point.y - v0->point.y;
1136 int32_t dxn = (v1->point.x - w0->point.x) * sign;
1137 if ((dxn < 0) && (dy0 > 0) && ((dx0 == 0) || ((dx0 < 0) && (dy0 * dx < dy * dx0))))
1150 int32_t x = v0->point.x;
1151 int32_t y0 = v0->point.y;
1154 while (((t = side ? w0->next : w0->prev) != v0) && (t->point.x == x) && (t->point.y <= y0))
1161 int32_t y1 = v1->point.y;
1163 while (((t = side ? w1->prev : w1->next) != v1) && (t->point.x == x) && (t->point.y >= y1))
1188 if (h1.minXy->point.x < h0.minXy->point.x)
1190 h0.minXy = h1.minXy;
1192 if (h1.maxXy->point.x >= h0.maxXy->point.x)
1194 h0.maxXy = h1.maxXy;
1197 h0.maxYx = h1.maxYx;
1205 void btConvexHullInternal::computeInternal(int start, int end, IntermediateHull& result)
1207 int n = end - start;
1211 result.minXy = NULL;
1212 result.maxXy = NULL;
1213 result.minYx = NULL;
1214 result.maxYx = NULL;
1218 Vertex* v = originalVertices[start];
1220 if (v->point != w->point)
1222 int32_t dx = v->point.x - w->point.x;
1223 int32_t dy = v->point.y - w->point.y;
1225 if ((dx == 0) && (dy == 0))
1227 if (v->point.z > w->point.z)
1233 btAssert(v->point.z < w->point.z);
1248 if ((dx < 0) || ((dx == 0) && (dy < 0)))
1259 if ((dy < 0) || ((dy == 0) && (dx < 0)))
1271 Edge* e = newEdgePair(v, w);
1282 Vertex* v = originalVertices[start];
1298 Vertex* v = originalVertices[start];
1312 int split0 = start + n / 2;
1313 Point32 p = originalVertices[split0 - 1]->point;
1314 int split1 = split0;
1315 while ((split1 < end) && (originalVertices[split1]->point == p))
1319 computeInternal(start, split0, result);
1320 IntermediateHull hull1;
1321 computeInternal(split1, end, hull1);
1322 #ifdef DEBUG_CONVEX_HULL
1323 printf("\n\nMerge\n");
1327 merge(result, hull1);
1328 #ifdef DEBUG_CONVEX_HULL
1329 printf("\n Result\n");
1334 #ifdef DEBUG_CONVEX_HULL
1335 void btConvexHullInternal::IntermediateHull::print()
1338 for (Vertex* v = minXy; v;)
1354 if (v->next->prev != v)
1356 printf(" Inconsistency");
1367 minXy->copy = (minXy->copy == -1) ? -2 : -1;
1368 minXy->printGraph();
1372 void btConvexHullInternal::Vertex::printGraph()
1375 printf("\nEdges\n");
1384 } while (e != edges);
1387 Vertex* v = e->target;
1388 if (v->copy != copy)
1394 } while (e != edges);
1399 btConvexHullInternal::Orientation btConvexHullInternal::getOrientation(const Edge* prev, const Edge* next, const Point32& s, const Point32& t)
1401 btAssert(prev->reverse->target == next->reverse->target);
1402 if (prev->next == next)
1404 if (prev->prev == next)
1406 Point64 n = t.cross(s);
1407 Point64 m = (*prev->target - *next->reverse->target).cross(*next->target - *next->reverse->target);
1408 btAssert(!m.isZero());
1409 int64_t dot = n.dot(m);
1411 return (dot > 0) ? COUNTER_CLOCKWISE : CLOCKWISE;
1413 return COUNTER_CLOCKWISE;
1415 else if (prev->prev == next)
1425 btConvexHullInternal::Edge* btConvexHullInternal::findMaxAngle(bool ccw, const Vertex* start, const Point32& s, const Point64& rxs, const Point64& sxrxs, Rational64& minCot)
1427 Edge* minEdge = NULL;
1429 #ifdef DEBUG_CONVEX_HULL
1430 printf("find max edge for %d\n", start->point.index);
1432 Edge* e = start->edges;
1437 if (e->copy > mergeStamp)
1439 Point32 t = *e->target - *start;
1440 Rational64 cot(t.dot(sxrxs), t.dot(rxs));
1441 #ifdef DEBUG_CONVEX_HULL
1442 printf(" Angle is %f (%d) for ", (float)btAtan(cot.toScalar()), (int)cot.isNaN());
1447 btAssert(ccw ? (t.dot(s) < 0) : (t.dot(s) > 0));
1452 if (minEdge == NULL)
1457 else if ((cmp = cot.compare(minCot)) < 0)
1462 else if ((cmp == 0) && (ccw == (getOrientation(minEdge, e, s, t) == COUNTER_CLOCKWISE)))
1467 #ifdef DEBUG_CONVEX_HULL
1472 } while (e != start->edges);
1477 void btConvexHullInternal::findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge*& e0, Edge*& e1, Vertex* stop0, Vertex* stop1)
1481 Point32 et0 = start0 ? start0->target->point : c0->point;
1482 Point32 et1 = start1 ? start1->target->point : c1->point;
1483 Point32 s = c1->point - c0->point;
1484 Point64 normal = ((start0 ? start0 : start1)->target->point - c0->point).cross(s);
1485 int64_t dist = c0->point.dot(normal);
1486 btAssert(!start1 || (start1->target->point.dot(normal) == dist));
1487 Point64 perp = s.cross(normal);
1488 btAssert(!perp.isZero());
1490 #ifdef DEBUG_CONVEX_HULL
1491 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);
1494 int64_t maxDot0 = et0.dot(perp);
1497 while (e0->target != stop0)
1499 Edge* e = e0->reverse->prev;
1500 if (e->target->point.dot(normal) < dist)
1504 btAssert(e->target->point.dot(normal) == dist);
1505 if (e->copy == mergeStamp)
1509 int64_t dot = e->target->point.dot(perp);
1516 et0 = e->target->point;
1520 int64_t maxDot1 = et1.dot(perp);
1523 while (e1->target != stop1)
1525 Edge* e = e1->reverse->next;
1526 if (e->target->point.dot(normal) < dist)
1530 btAssert(e->target->point.dot(normal) == dist);
1531 if (e->copy == mergeStamp)
1535 int64_t dot = e->target->point.dot(perp);
1542 et1 = e->target->point;
1546 #ifdef DEBUG_CONVEX_HULL
1547 printf(" Starting at %d %d\n", et0.index, et1.index);
1550 int64_t dx = maxDot1 - maxDot0;
1555 int64_t dy = (et1 - et0).dot(s);
1557 if (e0 && (e0->target != stop0))
1559 Edge* f0 = e0->next->reverse;
1560 if (f0->copy > mergeStamp)
1562 int64_t dx0 = (f0->target->point - et0).dot(perp);
1563 int64_t dy0 = (f0->target->point - et0).dot(s);
1564 if ((dx0 == 0) ? (dy0 < 0) : ((dx0 < 0) && (Rational64(dy0, dx0).compare(Rational64(dy, dx)) >= 0)))
1566 et0 = f0->target->point;
1567 dx = (et1 - et0).dot(perp);
1568 e0 = (e0 == start0) ? NULL : f0;
1574 if (e1 && (e1->target != stop1))
1576 Edge* f1 = e1->reverse->next;
1577 if (f1->copy > mergeStamp)
1579 Point32 d1 = f1->target->point - et1;
1580 if (d1.dot(normal) == 0)
1582 int64_t dx1 = d1.dot(perp);
1583 int64_t dy1 = d1.dot(s);
1584 int64_t dxn = (f1->target->point - et0).dot(perp);
1585 if ((dxn > 0) && ((dx1 == 0) ? (dy1 < 0) : ((dx1 < 0) && (Rational64(dy1, dx1).compare(Rational64(dy, dx)) > 0))))
1588 et1 = e1->target->point;
1595 btAssert((e1 == start1) && (d1.dot(normal) < 0));
1607 int64_t dy = (et1 - et0).dot(s);
1609 if (e1 && (e1->target != stop1))
1611 Edge* f1 = e1->prev->reverse;
1612 if (f1->copy > mergeStamp)
1614 int64_t dx1 = (f1->target->point - et1).dot(perp);
1615 int64_t dy1 = (f1->target->point - et1).dot(s);
1616 if ((dx1 == 0) ? (dy1 > 0) : ((dx1 < 0) && (Rational64(dy1, dx1).compare(Rational64(dy, dx)) <= 0)))
1618 et1 = f1->target->point;
1619 dx = (et1 - et0).dot(perp);
1620 e1 = (e1 == start1) ? NULL : f1;
1626 if (e0 && (e0->target != stop0))
1628 Edge* f0 = e0->reverse->prev;
1629 if (f0->copy > mergeStamp)
1631 Point32 d0 = f0->target->point - et0;
1632 if (d0.dot(normal) == 0)
1634 int64_t dx0 = d0.dot(perp);
1635 int64_t dy0 = d0.dot(s);
1636 int64_t dxn = (et1 - f0->target->point).dot(perp);
1637 if ((dxn < 0) && ((dx0 == 0) ? (dy0 > 0) : ((dx0 < 0) && (Rational64(dy0, dx0).compare(Rational64(dy, dx)) < 0))))
1640 et0 = e0->target->point;
1647 btAssert((e0 == start0) && (d0.dot(normal) < 0));
1655 #ifdef DEBUG_CONVEX_HULL
1656 printf(" Advanced edges to %d %d\n", et0.index, et1.index);
1660 void btConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1)
1675 Edge* toPrev0 = NULL;
1676 Edge* firstNew0 = NULL;
1677 Edge* pendingHead0 = NULL;
1678 Edge* pendingTail0 = NULL;
1680 Edge* toPrev1 = NULL;
1681 Edge* firstNew1 = NULL;
1682 Edge* pendingHead1 = NULL;
1683 Edge* pendingTail1 = NULL;
1686 if (mergeProjection(h0, h1, c0, c1))
1688 Point32 s = *c1 - *c0;
1689 Point64 normal = Point32(0, 0, -1).cross(s);
1690 Point64 t = s.cross(normal);
1691 btAssert(!t.isZero());
1693 Edge* e = c0->edges;
1694 Edge* start0 = NULL;
1699 int64_t dot = (*e->target - *c0).dot(normal);
1701 if ((dot == 0) && ((*e->target - *c0).dot(t) > 0))
1703 if (!start0 || (getOrientation(start0, e, s, Point32(0, 0, -1)) == CLOCKWISE))
1709 } while (e != c0->edges);
1713 Edge* start1 = NULL;
1718 int64_t dot = (*e->target - *c1).dot(normal);
1720 if ((dot == 0) && ((*e->target - *c1).dot(t) > 0))
1722 if (!start1 || (getOrientation(start1, e, s, Point32(0, 0, -1)) == COUNTER_CLOCKWISE))
1728 } while (e != c1->edges);
1731 if (start0 || start1)
1733 findEdgeForCoplanarFaces(c0, c1, start0, start1, NULL, NULL);
1736 c0 = start0->target;
1740 c1 = start1->target;
1744 prevPoint = c1->point;
1749 prevPoint = c1->point;
1753 Vertex* first0 = c0;
1754 Vertex* first1 = c1;
1755 bool firstRun = true;
1759 Point32 s = *c1 - *c0;
1760 Point32 r = prevPoint - c0->point;
1761 Point64 rxs = r.cross(s);
1762 Point64 sxrxs = s.cross(rxs);
1764 #ifdef DEBUG_CONVEX_HULL
1765 printf("\n Checking %d %d\n", c0->point.index, c1->point.index);
1767 Rational64 minCot0(0, 0);
1768 Edge* min0 = findMaxAngle(false, c0, s, rxs, sxrxs, minCot0);
1769 Rational64 minCot1(0, 0);
1770 Edge* min1 = findMaxAngle(true, c1, s, rxs, sxrxs, minCot1);
1773 Edge* e = newEdgePair(c0, c1);
1784 int cmp = !min0 ? 1 : !min1 ? -1 : minCot0.compare(minCot1);
1785 #ifdef DEBUG_CONVEX_HULL
1786 printf(" -> Result %d\n", cmp);
1788 if (firstRun || ((cmp >= 0) ? !minCot1.isNegativeInfinity() : !minCot0.isNegativeInfinity()))
1790 Edge* e = newEdgePair(c0, c1);
1793 pendingTail0->prev = e;
1799 e->next = pendingTail0;
1805 pendingTail1->next = e;
1811 e->prev = pendingTail1;
1818 #ifdef DEBUG_CONVEX_HULL
1819 printf(" Found min edges to %d %d\n", e0 ? e0->target->point.index : -1, e1 ? e1->target->point.index : -1);
1824 findEdgeForCoplanarFaces(c0, c1, e0, e1, NULL, NULL);
1827 if ((cmp >= 0) && e1)
1831 for (Edge *e = toPrev1->next, *n = NULL; e != min1; e = n)
1842 toPrev1->link(pendingHead1);
1846 min1->prev->link(pendingHead1);
1847 firstNew1 = pendingHead1;
1849 pendingTail1->link(min1);
1850 pendingHead1 = NULL;
1851 pendingTail1 = NULL;
1858 prevPoint = c1->point;
1860 toPrev1 = e1->reverse;
1863 if ((cmp <= 0) && e0)
1867 for (Edge *e = toPrev0->prev, *n = NULL; e != min0; e = n)
1878 pendingHead0->link(toPrev0);
1882 pendingHead0->link(min0->next);
1883 firstNew0 = pendingHead0;
1885 min0->link(pendingTail0);
1886 pendingHead0 = NULL;
1887 pendingTail0 = NULL;
1894 prevPoint = c0->point;
1896 toPrev0 = e0->reverse;
1900 if ((c0 == first0) && (c1 == first1))
1902 if (toPrev0 == NULL)
1904 pendingHead0->link(pendingTail0);
1905 c0->edges = pendingTail0;
1909 for (Edge *e = toPrev0->prev, *n = NULL; e != firstNew0; e = n)
1916 pendingHead0->link(toPrev0);
1917 firstNew0->link(pendingTail0);
1921 if (toPrev1 == NULL)
1923 pendingTail1->link(pendingHead1);
1924 c1->edges = pendingTail1;
1928 for (Edge *e = toPrev1->next, *n = NULL; e != firstNew1; e = n)
1935 toPrev1->link(pendingHead1);
1936 pendingTail1->link(firstNew1);
1950 bool operator()(const btConvexHullInternal::Point32& p, const btConvexHullInternal::Point32& q) const
1952 return (p.y < q.y) || ((p.y == q.y) && ((p.x < q.x) || ((p.x == q.x) && (p.z < q.z))));
1956 void btConvexHullInternal::compute(const void* coords, bool doubleCoords, int stride, int count)
1958 btVector3 min(btScalar(1e30), btScalar(1e30), btScalar(1e30)), max(btScalar(-1e30), btScalar(-1e30), btScalar(-1e30));
1959 const char* ptr = (const char*)coords;
1962 for (int i = 0; i < count; i++)
1964 const double* v = (const double*)ptr;
1965 btVector3 p((btScalar)v[0], (btScalar)v[1], (btScalar)v[2]);
1973 for (int i = 0; i < count; i++)
1975 const float* v = (const float*)ptr;
1976 btVector3 p(v[0], v[1], v[2]);
1983 btVector3 s = max - min;
1984 maxAxis = s.maxAxis();
1985 minAxis = s.minAxis();
1986 if (minAxis == maxAxis)
1988 minAxis = (maxAxis + 1) % 3;
1990 medAxis = 3 - maxAxis - minAxis;
1992 s /= btScalar(10216);
1993 if (((medAxis + 1) % 3) != maxAxis)
2001 s[0] = btScalar(1) / s[0];
2005 s[1] = btScalar(1) / s[1];
2009 s[2] = btScalar(1) / s[2];
2012 center = (min + max) * btScalar(0.5);
2014 btAlignedObjectArray<Point32> points;
2015 points.resize(count);
2016 ptr = (const char*)coords;
2019 for (int i = 0; i < count; i++)
2021 const double* v = (const double*)ptr;
2022 btVector3 p((btScalar)v[0], (btScalar)v[1], (btScalar)v[2]);
2024 p = (p - center) * s;
2025 points[i].x = (int32_t)p[medAxis];
2026 points[i].y = (int32_t)p[maxAxis];
2027 points[i].z = (int32_t)p[minAxis];
2028 points[i].index = i;
2033 for (int i = 0; i < count; i++)
2035 const float* v = (const float*)ptr;
2036 btVector3 p(v[0], v[1], v[2]);
2038 p = (p - center) * s;
2039 points[i].x = (int32_t)p[medAxis];
2040 points[i].y = (int32_t)p[maxAxis];
2041 points[i].z = (int32_t)p[minAxis];
2042 points[i].index = i;
2045 points.quickSort(pointCmp());
2048 vertexPool.setArraySize(count);
2049 originalVertices.resize(count);
2050 for (int i = 0; i < count; i++)
2052 Vertex* v = vertexPool.newObject();
2054 v->point = points[i];
2056 originalVertices[i] = v;
2062 edgePool.setArraySize(6 * count);
2065 maxUsedEdgePairs = 0;
2069 IntermediateHull hull;
2070 computeInternal(0, count, hull);
2071 vertexList = hull.minXy;
2072 #ifdef DEBUG_CONVEX_HULL
2073 printf("max. edges %d (3v = %d)", maxUsedEdgePairs, 3 * count);
2077 btVector3 btConvexHullInternal::toBtVector(const Point32& v)
2080 p[medAxis] = btScalar(v.x);
2081 p[maxAxis] = btScalar(v.y);
2082 p[minAxis] = btScalar(v.z);
2086 btVector3 btConvexHullInternal::getBtNormal(Face* face)
2088 return toBtVector(face->dir0).cross(toBtVector(face->dir1)).normalized();
2091 btVector3 btConvexHullInternal::getCoordinates(const Vertex* v)
2094 p[medAxis] = v->xvalue();
2095 p[maxAxis] = v->yvalue();
2096 p[minAxis] = v->zvalue();
2097 return p * scaling + center;
2100 btScalar btConvexHullInternal::shrink(btScalar amount, btScalar clampAmount)
2106 int stamp = --mergeStamp;
2107 btAlignedObjectArray<Vertex*> stack;
2108 vertexList->copy = stamp;
2109 stack.push_back(vertexList);
2110 btAlignedObjectArray<Face*> faces;
2112 Point32 ref = vertexList->point;
2113 Int128 hullCenterX(0, 0);
2114 Int128 hullCenterY(0, 0);
2115 Int128 hullCenterZ(0, 0);
2116 Int128 volume(0, 0);
2118 while (stack.size() > 0)
2120 Vertex* v = stack[stack.size() - 1];
2127 if (e->target->copy != stamp)
2129 e->target->copy = stamp;
2130 stack.push_back(e->target);
2132 if (e->copy != stamp)
2134 Face* face = facePool.newObject();
2135 face->init(e->target, e->reverse->prev->target, v);
2136 faces.push_back(face);
2145 int64_t vol = (v->point - ref).dot((a->point - ref).cross(b->point - ref));
2147 Point32 c = v->point + a->point + b->point + ref;
2148 hullCenterX += vol * c.x;
2149 hullCenterY += vol * c.y;
2150 hullCenterZ += vol * c.z;
2154 btAssert(f->copy != stamp);
2161 f = f->reverse->prev;
2165 } while (e != v->edges);
2169 if (volume.getSign() <= 0)
2174 btVector3 hullCenter;
2175 hullCenter[medAxis] = hullCenterX.toScalar();
2176 hullCenter[maxAxis] = hullCenterY.toScalar();
2177 hullCenter[minAxis] = hullCenterZ.toScalar();
2178 hullCenter /= 4 * volume.toScalar();
2179 hullCenter *= scaling;
2181 int faceCount = faces.size();
2183 if (clampAmount > 0)
2185 btScalar minDist = SIMD_INFINITY;
2186 for (int i = 0; i < faceCount; i++)
2188 btVector3 normal = getBtNormal(faces[i]);
2189 btScalar dist = normal.dot(toBtVector(faces[i]->origin) - hullCenter);
2201 amount = btMin(amount, minDist * clampAmount);
2204 unsigned int seed = 243703;
2205 for (int i = 0; i < faceCount; i++, seed = 1664525 * seed + 1013904223)
2207 btSwap(faces[i], faces[seed % faceCount]);
2210 for (int i = 0; i < faceCount; i++)
2212 if (!shiftFace(faces[i], amount, stack))
2221 bool btConvexHullInternal::shiftFace(Face* face, btScalar amount, btAlignedObjectArray<Vertex*> stack)
2223 btVector3 origShift = getBtNormal(face) * -amount;
2224 if (scaling[0] != 0)
2226 origShift[0] /= scaling[0];
2228 if (scaling[1] != 0)
2230 origShift[1] /= scaling[1];
2232 if (scaling[2] != 0)
2234 origShift[2] /= scaling[2];
2236 Point32 shift((int32_t)origShift[medAxis], (int32_t)origShift[maxAxis], (int32_t)origShift[minAxis]);
2241 Point64 normal = face->getNormal();
2242 #ifdef DEBUG_CONVEX_HULL
2243 printf("\nShrinking face (%d %d %d) (%d %d %d) (%d %d %d) by (%d %d %d)\n",
2244 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);
2246 int64_t origDot = face->origin.dot(normal);
2247 Point32 shiftedOrigin = face->origin + shift;
2248 int64_t shiftedDot = shiftedOrigin.dot(normal);
2249 btAssert(shiftedDot <= origDot);
2250 if (shiftedDot >= origDot)
2255 Edge* intersection = NULL;
2257 Edge* startEdge = face->nearbyVertex->edges;
2258 #ifdef DEBUG_CONVEX_HULL
2259 printf("Start edge is ");
2261 printf(", normal is (%lld %lld %lld), shifted dot is %lld\n", normal.x, normal.y, normal.z, shiftedDot);
2263 Rational128 optDot = face->nearbyVertex->dot(normal);
2264 int cmp = optDot.compare(shiftedDot);
2265 #ifdef SHOW_ITERATIONS
2270 Edge* e = startEdge;
2273 #ifdef SHOW_ITERATIONS
2276 Rational128 dot = e->target->dot(normal);
2277 btAssert(dot.compare(origDot) <= 0);
2278 #ifdef DEBUG_CONVEX_HULL
2279 printf("Moving downwards, edge is ");
2281 printf(", dot is %f (%f %lld)\n", (float)dot.toScalar(), (float)optDot.toScalar(), shiftedDot);
2283 if (dot.compare(optDot) < 0)
2285 int c = dot.compare(shiftedDot);
2297 } while (e != startEdge);
2306 Edge* e = startEdge;
2309 #ifdef SHOW_ITERATIONS
2312 Rational128 dot = e->target->dot(normal);
2313 btAssert(dot.compare(origDot) <= 0);
2314 #ifdef DEBUG_CONVEX_HULL
2315 printf("Moving upwards, edge is ");
2317 printf(", dot is %f (%f %lld)\n", (float)dot.toScalar(), (float)optDot.toScalar(), shiftedDot);
2319 if (dot.compare(optDot) > 0)
2321 cmp = dot.compare(shiftedDot);
2332 } while (e != startEdge);
2340 #ifdef SHOW_ITERATIONS
2341 printf("Needed %d iterations to find initial intersection\n", n);
2346 Edge* e = intersection->reverse->next;
2347 #ifdef SHOW_ITERATIONS
2350 while (e->target->dot(normal).compare(shiftedDot) <= 0)
2352 #ifdef SHOW_ITERATIONS
2356 if (e == intersection->reverse)
2360 #ifdef DEBUG_CONVEX_HULL
2361 printf("Checking for outwards edge, current edge is ");
2366 #ifdef SHOW_ITERATIONS
2367 printf("Needed %d iterations to check for complete containment\n", n);
2371 Edge* firstIntersection = NULL;
2372 Edge* faceEdge = NULL;
2373 Edge* firstFaceEdge = NULL;
2375 #ifdef SHOW_ITERATIONS
2380 #ifdef SHOW_ITERATIONS
2383 #ifdef DEBUG_CONVEX_HULL
2384 printf("Intersecting edge is ");
2385 intersection->print();
2390 Edge* e = intersection->reverse->next;
2392 #ifdef SHOW_ITERATIONS
2397 #ifdef SHOW_ITERATIONS
2400 if (e->target->dot(normal).compare(shiftedDot) >= 0)
2404 intersection = e->reverse;
2411 #ifdef SHOW_ITERATIONS
2412 printf("Needed %d iterations to advance intersection\n", n);
2416 #ifdef DEBUG_CONVEX_HULL
2417 printf("Advanced intersecting edge to ");
2418 intersection->print();
2419 printf(", cmp = %d\n", cmp);
2422 if (!firstIntersection)
2424 firstIntersection = intersection;
2426 else if (intersection == firstIntersection)
2432 Edge* prevIntersection = intersection;
2433 Edge* prevFaceEdge = faceEdge;
2435 Edge* e = intersection->reverse;
2436 #ifdef SHOW_ITERATIONS
2441 #ifdef SHOW_ITERATIONS
2444 e = e->reverse->prev;
2445 btAssert(e != intersection->reverse);
2446 cmp = e->target->dot(normal).compare(shiftedDot);
2447 #ifdef DEBUG_CONVEX_HULL
2448 printf("Testing edge ");
2450 printf(" -> cmp = %d\n", cmp);
2458 #ifdef SHOW_ITERATIONS
2459 printf("Needed %d iterations to find other intersection of face\n", n);
2464 Vertex* removed = intersection->target;
2465 e = intersection->reverse;
2468 removed->edges = NULL;
2472 removed->edges = e->prev;
2473 e->prev->link(e->next);
2476 #ifdef DEBUG_CONVEX_HULL
2477 printf("1: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z);
2480 Point64 n0 = intersection->face->getNormal();
2481 Point64 n1 = intersection->reverse->face->getNormal();
2482 int64_t m00 = face->dir0.dot(n0);
2483 int64_t m01 = face->dir1.dot(n0);
2484 int64_t m10 = face->dir0.dot(n1);
2485 int64_t m11 = face->dir1.dot(n1);
2486 int64_t r0 = (intersection->face->origin - shiftedOrigin).dot(n0);
2487 int64_t r1 = (intersection->reverse->face->origin - shiftedOrigin).dot(n1);
2488 Int128 det = Int128::mul(m00, m11) - Int128::mul(m01, m10);
2489 btAssert(det.getSign() != 0);
2490 Vertex* v = vertexPool.newObject();
2491 v->point.index = -1;
2493 v->point128 = PointR128(Int128::mul(face->dir0.x * r0, m11) - Int128::mul(face->dir0.x * r1, m01) + Int128::mul(face->dir1.x * r1, m00) - Int128::mul(face->dir1.x * r0, m10) + det * shiftedOrigin.x,
2494 Int128::mul(face->dir0.y * r0, m11) - Int128::mul(face->dir0.y * r1, m01) + Int128::mul(face->dir1.y * r1, m00) - Int128::mul(face->dir1.y * r0, m10) + det * shiftedOrigin.y,
2495 Int128::mul(face->dir0.z * r0, m11) - Int128::mul(face->dir0.z * r1, m01) + Int128::mul(face->dir1.z * r1, m00) - Int128::mul(face->dir1.z * r0, m10) + det * shiftedOrigin.z,
2497 v->point.x = (int32_t)v->point128.xvalue();
2498 v->point.y = (int32_t)v->point128.yvalue();
2499 v->point.z = (int32_t)v->point128.zvalue();
2500 intersection->target = v;
2504 stack.push_back(removed);
2505 stack.push_back(NULL);
2508 if (cmp || prevCmp || (prevIntersection->reverse->next->target != intersection->target))
2510 faceEdge = newEdgePair(prevIntersection->target, intersection->target);
2513 faceEdge->link(prevIntersection->reverse->next);
2515 if ((prevCmp == 0) || prevFaceEdge)
2517 prevIntersection->reverse->link(faceEdge);
2521 intersection->reverse->prev->link(faceEdge->reverse);
2523 faceEdge->reverse->link(intersection->reverse);
2527 faceEdge = prevIntersection->reverse->next;
2534 faceEdge->link(prevFaceEdge->reverse);
2536 else if (faceEdge != prevFaceEdge->reverse)
2538 stack.push_back(prevFaceEdge->target);
2539 while (faceEdge->next != prevFaceEdge->reverse)
2541 Vertex* removed = faceEdge->next->target;
2542 removeEdgePair(faceEdge->next);
2543 stack.push_back(removed);
2544 #ifdef DEBUG_CONVEX_HULL
2545 printf("2: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z);
2548 stack.push_back(NULL);
2551 faceEdge->face = face;
2552 faceEdge->reverse->face = intersection->face;
2556 firstFaceEdge = faceEdge;
2559 #ifdef SHOW_ITERATIONS
2560 printf("Needed %d iterations to process all intersections\n", m);
2565 firstFaceEdge->reverse->target = faceEdge->target;
2566 firstIntersection->reverse->link(firstFaceEdge);
2567 firstFaceEdge->link(faceEdge->reverse);
2569 else if (firstFaceEdge != faceEdge->reverse)
2571 stack.push_back(faceEdge->target);
2572 while (firstFaceEdge->next != faceEdge->reverse)
2574 Vertex* removed = firstFaceEdge->next->target;
2575 removeEdgePair(firstFaceEdge->next);
2576 stack.push_back(removed);
2577 #ifdef DEBUG_CONVEX_HULL
2578 printf("3: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z);
2581 stack.push_back(NULL);
2584 btAssert(stack.size() > 0);
2585 vertexList = stack[0];
2587 #ifdef DEBUG_CONVEX_HULL
2588 printf("Removing part\n");
2590 #ifdef SHOW_ITERATIONS
2594 while (pos < stack.size())
2596 int end = stack.size();
2599 Vertex* kept = stack[pos++];
2600 #ifdef DEBUG_CONVEX_HULL
2603 bool deeper = false;
2605 while ((removed = stack[pos++]) != NULL)
2607 #ifdef SHOW_ITERATIONS
2610 kept->receiveNearbyFaces(removed);
2611 while (removed->edges)
2616 stack.push_back(kept);
2618 stack.push_back(removed->edges->target);
2619 removeEdgePair(removed->edges);
2624 stack.push_back(NULL);
2628 #ifdef SHOW_ITERATIONS
2629 printf("Needed %d iterations to remove part\n", n);
2633 face->origin = shiftedOrigin;
2638 static int getVertexCopy(btConvexHullInternal::Vertex* vertex, btAlignedObjectArray<btConvexHullInternal::Vertex*>& vertices)
2640 int index = vertex->copy;
2643 index = vertices.size();
2644 vertex->copy = index;
2645 vertices.push_back(vertex);
2646 #ifdef DEBUG_CONVEX_HULL
2647 printf("Vertex %d gets index *%d\n", vertex->point.index, index);
2653 btScalar btConvexHullComputer::compute(const void* coords, bool doubleCoords, int stride, int count, btScalar shrink, btScalar shrinkClamp)
2663 btConvexHullInternal hull;
2664 hull.compute(coords, doubleCoords, stride, count);
2667 if ((shrink > 0) && ((shift = hull.shrink(shrink, shrinkClamp)) < 0))
2676 original_vertex_index.resize(0);
2680 btAlignedObjectArray<btConvexHullInternal::Vertex*> oldVertices;
2681 getVertexCopy(hull.vertexList, oldVertices);
2683 while (copied < oldVertices.size())
2685 btConvexHullInternal::Vertex* v = oldVertices[copied];
2686 vertices.push_back(hull.getCoordinates(v));
2687 original_vertex_index.push_back(v->point.index);
2688 btConvexHullInternal::Edge* firstEdge = v->edges;
2693 btConvexHullInternal::Edge* e = firstEdge;
2698 int s = edges.size();
2699 edges.push_back(Edge());
2700 edges.push_back(Edge());
2701 Edge* c = &edges[s];
2702 Edge* r = &edges[s + 1];
2704 e->reverse->copy = s + 1;
2707 c->targetVertex = getVertexCopy(e->target, oldVertices);
2708 r->targetVertex = copied;
2709 #ifdef DEBUG_CONVEX_HULL
2710 printf(" CREATE: Vertex *%d has edge to *%d\n", copied, c->getTargetVertex());
2715 edges[e->copy].next = prevCopy - e->copy;
2719 firstCopy = e->copy;
2723 } while (e != firstEdge);
2724 edges[firstCopy].next = prevCopy - firstCopy;
2729 for (int i = 0; i < copied; i++)
2731 btConvexHullInternal::Vertex* v = oldVertices[i];
2732 btConvexHullInternal::Edge* firstEdge = v->edges;
2735 btConvexHullInternal::Edge* e = firstEdge;
2740 #ifdef DEBUG_CONVEX_HULL
2741 printf("Vertex *%d has edge to *%d\n", i, edges[e->copy].getTargetVertex());
2743 faces.push_back(e->copy);
2744 btConvexHullInternal::Edge* f = e;
2747 #ifdef DEBUG_CONVEX_HULL
2748 printf(" Face *%d\n", edges[f->copy].getTargetVertex());
2751 f = f->reverse->prev;
2755 } while (e != firstEdge);