24#elif defined(_MSC_VER)
46#if defined(DEBUG_CONVEX_HULL) || defined(SHOW_ITERATIONS)
69 return (
x == 0) && (
y == 0) && (
z == 0);
74 return x * b.
x +
y * b.
y +
z * b.
z;
96 return (
x == b.
x) && (
y == b.
y) && (
z == b.
z);
101 return (
x != b.
x) || (
y != b.
y) || (
z != b.
z);
106 return (
x == 0) && (
y == 0) && (
z == 0);
121 return x * b.
x +
y * b.
y +
z * b.
z;
126 return x * b.
x +
y * b.
y +
z * b.
z;
175 __asm__ (
"addq %[bl], %[rl]\n\t"
176 "adcq %[bh], %[rh]\n\t"
177 : [rl]
"=r" (result.
low), [rh]
"=r" (result.
high)
191 __asm__ (
"subq %[bl], %[rl]\n\t"
192 "sbbq %[bh], %[rh]\n\t"
193 : [rl]
"=r" (result.
low), [rh]
"=r" (result.
high)
205 __asm__ (
"addq %[bl], %[rl]\n\t"
206 "adcq %[bh], %[rh]\n\t"
207 : [rl]
"=r" (
low), [rh]
"=r" (
high)
287 else if (numerator < 0)
301 else if (denominator < 0)
345 this->numerator = value;
350 this->numerator = -value;
447#ifdef DEBUG_CONVEX_HULL
499 f->nearbyVertex =
this;
533#ifdef DEBUG_CONVEX_HULL
579 template<
typename UWord,
typename UHWord>
class DMul
625 static void mul(UWord a, UWord b, UWord& resLow, UWord& resHigh)
631 UWord p0110 = UWord(
low(p01)) + UWord(
low(p10));
687 for (
int i = 0; i <
size; i++, o++)
689 o->next = (i+1 <
size) ? o + 1 : NULL;
695 template <
typename T>
class Pool
817 bool mergeProjection(IntermediateHull& h0, IntermediateHull& h1, Vertex*& c0, Vertex*& c1);
819 void merge(IntermediateHull& h0, IntermediateHull& h1);
830 void compute(
const void* coords,
bool doubleCoords,
int stride,
int count);
841 Int128 a = negative ? -*this : *
this;
844 negative = !negative;
849 return negative ? -result : result;
857 __asm__ (
"imulq %[b]"
858 :
"=a" (result.
low),
"=d" (result.
high)
864 bool negative = a < 0;
871 negative = !negative;
875 return negative ? -result : result;
885 :
"=a" (result.
low),
"=d" (result.
high)
900 return sign - b.
sign;
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"
922 "orq %%rdx, %%rax\n\t"
925 "shll $16, %%ebx\n\t"
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
944 return sign - b.
sign;
955 Int128 nbdLow, nbdHigh, dbnLow, dbnHigh;
959 int cmp = nbdHigh.
ucmp(dbnHigh);
964 return nbdLow.
ucmp(dbnLow) * sign;
972 return (a > b) ? 1 : (a < b) ? -1 : 0;
994 return numerator.ucmp(denominator * b) * sign;
1072 for (
int side = 0; side <= 1; side++)
1086 if ((dy0 <= 0) && ((dx0 == 0) || ((dx0 < 0) && (dy0 * dx <= dy * dx0))))
1100 if ((dxn > 0) && (dy1 < 0) && ((dx1 == 0) || ((dx1 < 0) && (dy1 * dx < dy * dx1))))
1122 if ((dy1 >= 0) && ((dx1 == 0) || ((dx1 < 0) && (dy1 * dx <= dy * dx1))))
1136 if ((dxn < 0) && (dy0 > 0) && ((dx0 == 0) || ((dx0 < 0) && (dy0 * dx < dy * dx0))))
1206 int n = end - start;
1210 result.
minXy = NULL;
1211 result.
maxXy = NULL;
1212 result.
minYx = NULL;
1213 result.
maxYx = NULL;
1224 if ((dx == 0) && (dy == 0))
1247 if ((dx < 0) || ((dx == 0) && (dy < 0)))
1258 if ((dy < 0) || ((dy == 0) && (dx < 0)))
1311 int split0 = start + n / 2;
1313 int split1 = split0;
1321#ifdef DEBUG_CONVEX_HULL
1322 printf(
"\n\nMerge\n");
1326 merge(result, hull1);
1327#ifdef DEBUG_CONVEX_HULL
1328 printf(
"\n Result\n");
1333#ifdef DEBUG_CONVEX_HULL
1337 for (Vertex* v = minXy; v; )
1353 if (v->next->prev != v)
1355 printf(
" Inconsistency");
1366 minXy->copy = (minXy->copy == -1) ? -2 : -1;
1367 minXy->printGraph();
1371void btConvexHullInternal::Vertex::printGraph()
1374 printf(
"\nEdges\n");
1383 }
while (e != edges);
1386 Vertex* v = e->target;
1387 if (v->copy != copy)
1393 }
while (e != edges);
1401 if (prev->
next == next)
1403 if (prev->
prev == next)
1414 else if (prev->
prev == next)
1426 Edge* minEdge = NULL;
1428#ifdef DEBUG_CONVEX_HULL
1429 printf(
"find max edge for %d\n", start->
point.
index);
1440#ifdef DEBUG_CONVEX_HULL
1451 if (minEdge == NULL)
1456 else if ((cmp = cot.
compare(minCot)) < 0)
1466#ifdef DEBUG_CONVEX_HULL
1471 }
while (e != start->
edges);
1483 Point64 normal = ((start0 ? start0 : start1)->target->point - c0->
point).cross(s);
1489#ifdef DEBUG_CONVEX_HULL
1496 while (e0->
target != stop0)
1522 while (e1->
target != stop1)
1545#ifdef DEBUG_CONVEX_HULL
1546 printf(
" Starting at %d %d\n", et0.
index, et1.
index);
1549 int64_t dx = maxDot1 - maxDot0;
1556 if (e0 && (e0->
target != stop0))
1566 dx = (et1 - et0).
dot(perp);
1567 e0 = (e0 == start0) ? NULL : f0;
1573 if (e1 && (e1->
target != stop1))
1579 if (d1.
dot(normal) == 0)
1608 if (e1 && (e1->
target != stop1))
1618 dx = (et1 - et0).
dot(perp);
1619 e1 = (e1 == start1) ? NULL : f1;
1625 if (e0 && (e0->
target != stop0))
1631 if (d0.
dot(normal) == 0)
1654#ifdef DEBUG_CONVEX_HULL
1655 printf(
" Advanced edges to %d %d\n", et0.
index, et1.
index);
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;
1693 Edge* e = c0->edges;
1694 Edge* start0 = NULL;
1701 if ((
dot == 0) && ((*e->
target - *c0).dot(t) > 0))
1709 }
while (e != c0->edges);
1713 Edge* start1 = NULL;
1720 if ((
dot == 0) && ((*e->
target - *c1).dot(t) > 0))
1728 }
while (e != c1->
edges);
1731 if (start0 || start1)
1744 prevPoint = c1->
point;
1749 prevPoint = c1->
point;
1755 bool firstRun =
true;
1760 Point32 r = prevPoint - c0->point;
1764#ifdef DEBUG_CONVEX_HULL
1765 printf(
"\n Checking %d %d\n", c0->point.index, c1->
point.
index);
1784 int cmp = !min0 ? 1 : !min1 ? -1 : minCot0.
compare(minCot1);
1785#ifdef DEBUG_CONVEX_HULL
1786 printf(
" -> Result %d\n", cmp);
1793 pendingTail0->
prev = e;
1799 e->
next = pendingTail0;
1805 pendingTail1->
next = e;
1811 e->
prev = pendingTail1;
1818#ifdef DEBUG_CONVEX_HULL
1827 if ((cmp >= 0) && e1)
1831 for (
Edge* e = toPrev1->
next, *n = NULL; e != min1; e = n)
1842 toPrev1->
link(pendingHead1);
1847 firstNew1 = pendingHead1;
1849 pendingTail1->
link(min1);
1850 pendingHead1 = NULL;
1851 pendingTail1 = NULL;
1858 prevPoint = c1->
point;
1863 if ((cmp <= 0) && e0)
1867 for (
Edge* e = toPrev0->
prev, *n = NULL; e != min0; e = n)
1878 pendingHead0->
link(toPrev0);
1883 firstNew0 = pendingHead0;
1885 min0->
link(pendingTail0);
1886 pendingHead0 = NULL;
1887 pendingTail0 = NULL;
1894 prevPoint = c0->point;
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);
1953 return (p.
y < q.
y) || ((p.
y == q.
y) && ((p.
x < q.
x) || ((p.
x == q.
x) && (p.
z < q.
z))));
1960 const char* ptr = (
const char*) coords;
1963 for (
int i = 0; i < count; i++)
1965 const double* v = (
const double*) ptr;
1974 for (
int i = 0; i < count; i++)
1976 const float* v = (
const float*) ptr;
2017 ptr = (
const char*) coords;
2020 for (
int i = 0; i < count; i++)
2022 const double* v = (
const double*) ptr;
2029 points[i].index = i;
2034 for (
int i = 0; i < count; i++)
2036 const float* v = (
const float*) ptr;
2043 points[i].index = i;
2051 for (
int i = 0; i < count; i++)
2055 v->
point = points[i];
2073#ifdef DEBUG_CONVEX_HULL
2114 Int128 hullCenterX(0, 0);
2115 Int128 hullCenterY(0, 0);
2116 Int128 hullCenterZ(0, 0);
2119 while (stack.
size() > 0)
2133 if (e->
copy != stamp)
2149 hullCenterX += vol * c.
x;
2150 hullCenterY += vol * c.
y;
2151 hullCenterZ += vol * c.
z;
2166 }
while (e != v->
edges);
2179 hullCenter /= 4 * volume.
toScalar();
2182 int faceCount = faces.
size();
2184 if (clampAmount > 0)
2187 for (
int i = 0; i < faceCount; i++)
2202 amount =
btMin(amount, minDist * clampAmount);
2205 unsigned int seed = 243703;
2206 for (
int i = 0; i < faceCount; i++, seed = 1664525 * seed + 1013904223)
2208 btSwap(faces[i], faces[seed % faceCount]);
2211 for (
int i = 0; i < faceCount; i++)
2213 if (!
shiftFace(faces[i], amount, stack))
2243#ifdef DEBUG_CONVEX_HULL
2244 printf(
"\nShrinking face (%d %d %d) (%d %d %d) (%d %d %d) by (%d %d %d)\n",
2245 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);
2249 int64_t shiftedDot = shiftedOrigin.
dot(normal);
2251 if (shiftedDot >= origDot)
2256 Edge* intersection = NULL;
2259#ifdef DEBUG_CONVEX_HULL
2260 printf(
"Start edge is ");
2262 printf(
", normal is (%lld %lld %lld), shifted dot is %lld\n", normal.
x, normal.
y, normal.
z, shiftedDot);
2265 int cmp = optDot.
compare(shiftedDot);
2266#ifdef SHOW_ITERATIONS
2271 Edge* e = startEdge;
2274#ifdef SHOW_ITERATIONS
2279#ifdef DEBUG_CONVEX_HULL
2280 printf(
"Moving downwards, edge is ");
2282 printf(
", dot is %f (%f %lld)\n", (
float)
dot.toScalar(), (
float) optDot.
toScalar(), shiftedDot);
2284 if (
dot.compare(optDot) < 0)
2286 int c =
dot.compare(shiftedDot);
2298 }
while (e != startEdge);
2307 Edge* e = startEdge;
2310#ifdef SHOW_ITERATIONS
2315#ifdef DEBUG_CONVEX_HULL
2316 printf(
"Moving upwards, edge is ");
2318 printf(
", dot is %f (%f %lld)\n", (
float)
dot.toScalar(), (
float) optDot.
toScalar(), shiftedDot);
2320 if (
dot.compare(optDot) > 0)
2322 cmp =
dot.compare(shiftedDot);
2333 }
while (e != startEdge);
2341#ifdef SHOW_ITERATIONS
2342 printf(
"Needed %d iterations to find initial intersection\n", n);
2348#ifdef SHOW_ITERATIONS
2353#ifdef SHOW_ITERATIONS
2357 if (e == intersection->
reverse)
2361#ifdef DEBUG_CONVEX_HULL
2362 printf(
"Checking for outwards edge, current edge is ");
2367#ifdef SHOW_ITERATIONS
2368 printf(
"Needed %d iterations to check for complete containment\n", n);
2372 Edge* firstIntersection = NULL;
2373 Edge* faceEdge = NULL;
2374 Edge* firstFaceEdge = NULL;
2376#ifdef SHOW_ITERATIONS
2381#ifdef SHOW_ITERATIONS
2384#ifdef DEBUG_CONVEX_HULL
2385 printf(
"Intersecting edge is ");
2386 intersection->print();
2393#ifdef SHOW_ITERATIONS
2398#ifdef SHOW_ITERATIONS
2412#ifdef SHOW_ITERATIONS
2413 printf(
"Needed %d iterations to advance intersection\n", n);
2417#ifdef DEBUG_CONVEX_HULL
2418 printf(
"Advanced intersecting edge to ");
2419 intersection->print();
2420 printf(
", cmp = %d\n", cmp);
2423 if (!firstIntersection)
2425 firstIntersection = intersection;
2427 else if (intersection == firstIntersection)
2433 Edge* prevIntersection = intersection;
2434 Edge* prevFaceEdge = faceEdge;
2437#ifdef SHOW_ITERATIONS
2442#ifdef SHOW_ITERATIONS
2448#ifdef DEBUG_CONVEX_HULL
2449 printf(
"Testing edge ");
2451 printf(
" -> cmp = %d\n", cmp);
2459#ifdef SHOW_ITERATIONS
2460 printf(
"Needed %d iterations to find other intersection of face\n", n);
2469 removed->
edges = NULL;
2477#ifdef DEBUG_CONVEX_HULL
2478 printf(
"1: Removed part contains (%d %d %d)\n", removed->
point.
x, removed->
point.
y, removed->
point.
z);
2504 intersection->
target = v;
2519 if ((prevCmp == 0) || prevFaceEdge)
2540 else if (faceEdge != prevFaceEdge->
reverse)
2548#ifdef DEBUG_CONVEX_HULL
2549 printf(
"2: Removed part contains (%d %d %d)\n", removed->
point.
x, removed->
point.
y, removed->
point.
z);
2555 faceEdge->
face = face;
2560 firstFaceEdge = faceEdge;
2563#ifdef SHOW_ITERATIONS
2564 printf(
"Needed %d iterations to process all intersections\n", m);
2573 else if (firstFaceEdge != faceEdge->
reverse)
2581#ifdef DEBUG_CONVEX_HULL
2582 printf(
"3: Removed part contains (%d %d %d)\n", removed->
point.
x, removed->
point.
y, removed->
point.
z);
2591#ifdef DEBUG_CONVEX_HULL
2592 printf(
"Removing part\n");
2594#ifdef SHOW_ITERATIONS
2598 while (pos < stack.
size())
2600 int end = stack.
size();
2603 Vertex* kept = stack[pos++];
2604#ifdef DEBUG_CONVEX_HULL
2607 bool deeper =
false;
2609 while ((removed = stack[pos++]) != NULL)
2611#ifdef SHOW_ITERATIONS
2615 while (removed->
edges)
2632#ifdef SHOW_ITERATIONS
2633 printf(
"Needed %d iterations to remove part\n", n);
2637 face->
origin = shiftedOrigin;
2645 int index = vertex->
copy;
2648 index = vertices.
size();
2649 vertex->
copy = index;
2651#ifdef DEBUG_CONVEX_HULL
2652 printf(
"Vertex %d gets index *%d\n", vertex->
point.
index, index);
2669 hull.
compute(coords, doubleCoords, stride, count);
2687 while (copied < oldVertices.
size())
2701 int s = edges.size();
2702 edges.push_back(
Edge());
2703 edges.push_back(
Edge());
2704 Edge* c = &edges[s];
2705 Edge* r = &edges[s + 1];
2712#ifdef DEBUG_CONVEX_HULL
2713 printf(
" CREATE: Vertex *%d has edge to *%d\n", copied, c->
getTargetVertex());
2718 edges[e->
copy].next = prevCopy - e->
copy;
2722 firstCopy = e->
copy;
2726 }
while (e != firstEdge);
2727 edges[firstCopy].
next = prevCopy - firstCopy;
2732 for (
int i = 0; i < copied; i++)
2743#ifdef DEBUG_CONVEX_HULL
2744 printf(
"Vertex *%d has edge to *%d\n", i, edges[e->
copy].getTargetVertex());
2746 faces.push_back(e->
copy);
2750#ifdef DEBUG_CONVEX_HULL
2751 printf(
" Face *%d\n", edges[f->
copy].getTargetVertex());
2758 }
while (e != firstEdge);
#define btAlignedFree(ptr)
#define btAlignedAlloc(size, alignment)
static int getVertexCopy(btConvexHullInternal::Vertex *vertex, btAlignedObjectArray< btConvexHullInternal::Vertex * > &vertices)
unsigned long long int uint64_t
const T & btMin(const T &a, const T &b)
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
btScalar btAtan(btScalar x)
The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods It...
int size() const
return the number of elements in the array
void resize(int newsize, const T &fillData=T())
void clear()
clear the array, deallocated memory. Generally it is better to use array.resize(0),...
void quickSort(const L &CompareFunc)
void push_back(const T &_Val)
int getTargetVertex() const
btScalar compute(const void *coords, bool doubleCoords, int stride, int count, btScalar shrink, btScalar shrinkClamp)
static uint64_t high(Int128 value)
static void shlHalf(uint64_t &value)
static void shlHalf(Int128 &value)
static uint64_t low(Int128 value)
static uint32_t low(uint64_t value)
static void mul(UWord a, UWord b, UWord &resLow, UWord &resHigh)
static uint32_t high(uint64_t value)
static uint64_t mul(uint32_t a, uint32_t b)
static Int128 mul(uint64_t a, uint64_t b)
Face * nextWithSameNearbyVertex
void init(Vertex *a, Vertex *b, Vertex *c)
int ucmp(const Int128 &b) const
Int128 operator+(const Int128 &b) const
Int128 operator*(int64_t b) const
Int128(uint64_t low, uint64_t high)
bool operator<(const Int128 &b) const
Int128 operator-(const Int128 &b) const
btScalar toScalar() const
static Int128 mul(int64_t a, int64_t b)
Int128 & operator+=(const Int128 &b)
int64_t dot(const Point64 &b) const
Point32(int32_t x, int32_t y, int32_t z)
Point32 operator-(const Point32 &b) const
int64_t dot(const Point32 &b) const
bool operator!=(const Point32 &b) const
Point64 cross(const Point64 &b) const
Point32 operator+(const Point32 &b) const
Point64 cross(const Point32 &b) const
bool operator==(const Point32 &b) const
Point64(int64_t x, int64_t y, int64_t z)
int64_t dot(const Point64 &b) const
PointR128(Int128 x, Int128 y, Int128 z, Int128 denominator)
void setArraySize(int arraySize)
void freeObject(T *object)
PoolArray< T > * nextArray
btScalar toScalar() const
Rational128(const Int128 &numerator, const Int128 &denominator)
int compare(const Rational128 &b) const
Rational128(int64_t value)
bool isNegativeInfinity() const
btScalar toScalar() const
int compare(const Rational64 &b) const
Rational64(int64_t numerator, int64_t denominator)
Point32 operator-(const Vertex &b) const
Rational128 dot(const Point64 &b) const
void receiveNearbyFaces(Vertex *src)
bool mergeProjection(IntermediateHull &h0, IntermediateHull &h1, Vertex *&c0, Vertex *&c1)
btVector3 toBtVector(const Point32 &v)
void computeInternal(int start, int end, IntermediateHull &result)
void findEdgeForCoplanarFaces(Vertex *c0, Vertex *c1, Edge *&e0, Edge *&e1, Vertex *stop0, Vertex *stop1)
void compute(const void *coords, bool doubleCoords, int stride, int count)
bool shiftFace(Face *face, btScalar amount, btAlignedObjectArray< Vertex * > stack)
btVector3 getCoordinates(const Vertex *v)
btAlignedObjectArray< Vertex * > originalVertices
btScalar shrink(btScalar amount, btScalar clampAmount)
Edge * findMaxAngle(bool ccw, const Vertex *start, const Point32 &s, const Point64 &rxs, const Point64 &sxrxs, Rational64 &minCot)
static Orientation getOrientation(const Edge *prev, const Edge *next, const Point32 &s, const Point32 &t)
Pool< Vertex > vertexPool
Edge * newEdgePair(Vertex *from, Vertex *to)
void merge(IntermediateHull &h0, IntermediateHull &h1)
void removeEdgePair(Edge *edge)
btVector3 getBtNormal(Face *face)
btVector3 can be used to represent 3D points and vectors.
void setMax(const btVector3 &other)
Set each element to the max of the current values and the values of another btVector3.
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
int minAxis() const
Return the axis with the smallest value Note return values are 0,1,2 for x, y, or z.
btScalar dot(const btVector3 &v) const
Return the dot product.
btVector3 normalized() const
Return a normalized version of this vector.
int maxAxis() const
Return the axis with the largest value Note return values are 0,1,2 for x, y, or z.