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;
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)
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)
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)
287 else if (numerator < 0)
301 else if (denominator < 0)
372 int dsign = denominator.
getSign();
447 #ifdef DEBUG_CONVEX_HULL
499 f->nearbyVertex =
this;
533 #ifdef DEBUG_CONVEX_HULL
562 if (a->lastNearbyFace)
568 a->firstNearbyFace =
this;
570 a->lastNearbyFace =
this;
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;
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;
875 return negative ? -result : result;
883 #ifdef USE_X86_64_ASM
885 :
"=a" (result.
low),
"=d" (result.
high)
900 return sign - b.
sign;
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"
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)))
1298 int split0 = start + n / 2;
1300 int split1 = split0;
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
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);
1388 if (prev->
next == next)
1390 if (prev->
prev == next)
1401 else if (prev->
prev == next)
1413 Edge* minEdge = NULL;
1415 #ifdef DEBUG_CONVEX_HULL
1416 printf(
"find max edge for %d\n", start->
point.
index);
1427 #ifdef DEBUG_CONVEX_HULL
1428 printf(
" Angle is %f (%d) for ", (
float)
btAtan(cot.toScalar()), (
int) cot.isNaN());
1438 if (minEdge == NULL)
1443 else if ((cmp = cot.compare(minCot)) < 0)
1453 #ifdef DEBUG_CONVEX_HULL
1458 }
while (e != start->
edges);
1470 Point64 normal = ((start0 ? start0 : start1)->target->point - c0->
point).cross(s);
1476 #ifdef DEBUG_CONVEX_HULL
1483 while (e0->
target != stop0)
1509 while (e1->
target != stop1)
1532 #ifdef DEBUG_CONVEX_HULL
1533 printf(
" Starting at %d %d\n", et0.
index, et1.
index);
1536 int64_t dx = maxDot1 - maxDot0;
1543 if (e0 && (e0->
target != stop0))
1553 dx = (et1 - et0).
dot(perp);
1554 e0 = (e0 == start0) ? NULL : f0;
1560 if (e1 && (e1->
target != stop1))
1566 if (d1.
dot(normal) == 0)
1595 if (e1 && (e1->
target != stop1))
1605 dx = (et1 - et0).
dot(perp);
1606 e1 = (e1 == start1) ? NULL : f1;
1612 if (e0 && (e0->
target != stop0))
1618 if (d0.
dot(normal) == 0)
1641 #ifdef DEBUG_CONVEX_HULL
1642 printf(
" Advanced edges to %d %d\n", et0.
index, et1.
index);
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;
1680 Edge* e = c0->edges;
1681 Edge* start0 = NULL;
1688 if ((dot == 0) && ((*e->target - *c0).dot(t) > 0))
1696 }
while (e != c0->edges);
1700 Edge* start1 = NULL;
1707 if ((dot == 0) && ((*e->target - *c1).dot(t) > 0))
1715 }
while (e != c1->
edges);
1718 if (start0 || start1)
1731 prevPoint = c1->
point;
1736 prevPoint = c1->
point;
1742 bool firstRun =
true;
1747 Point32 r = prevPoint - c0->point;
1751 #ifdef DEBUG_CONVEX_HULL
1752 printf(
"\n Checking %d %d\n", c0->point.index, c1->
point.
index);
1771 int cmp = !min0 ? 1 : !min1 ? -1 : minCot0.
compare(minCot1);
1772 #ifdef DEBUG_CONVEX_HULL
1773 printf(
" -> Result %d\n", cmp);
1780 pendingTail0->
prev = e;
1786 e->
next = pendingTail0;
1792 pendingTail1->
next = e;
1798 e->
prev = pendingTail1;
1805 #ifdef DEBUG_CONVEX_HULL
1814 if ((cmp >= 0) && e1)
1818 for (
Edge* e = toPrev1->
next, *n = NULL; e != min1; e = n)
1829 toPrev1->
link(pendingHead1);
1834 firstNew1 = pendingHead1;
1836 pendingTail1->
link(min1);
1837 pendingHead1 = NULL;
1838 pendingTail1 = NULL;
1845 prevPoint = c1->
point;
1850 if ((cmp <= 0) && e0)
1854 for (
Edge* e = toPrev0->
prev, *n = NULL; e != min0; e = n)
1865 pendingHead0->
link(toPrev0);
1870 firstNew0 = pendingHead0;
1872 min0->
link(pendingTail0);
1873 pendingHead0 = NULL;
1874 pendingTail0 = NULL;
1881 prevPoint = c0->point;
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);
1940 return (p.
y < q.
y) || ((p.
y == q.
y) && ((p.
x < q.
x) || ((p.
x == q.
x) && (p.
z < q.
z))));
1947 const char* ptr = (
const char*) coords;
1950 for (
int i = 0; i < count; i++)
1952 const double* v = (
const double*) ptr;
1961 for (
int i = 0; i < count; i++)
1963 const float* v = (
const float*) ptr;
2004 ptr = (
const char*) coords;
2007 for (
int i = 0; i < count; i++)
2009 const double* v = (
const double*) ptr;
2015 points[i].z = (
int32_t) p[minAxis];
2016 points[i].index = i;
2021 for (
int i = 0; i < count; i++)
2023 const float* v = (
const float*) ptr;
2029 points[i].z = (
int32_t) p[minAxis];
2030 points[i].index = i;
2038 for (
int i = 0; i < count; i++)
2042 v->
point = points[i];
2060 #ifdef DEBUG_CONVEX_HULL
2101 Int128 hullCenterX(0, 0);
2102 Int128 hullCenterY(0, 0);
2103 Int128 hullCenterZ(0, 0);
2106 while (stack.
size() > 0)
2120 if (e->
copy != stamp)
2136 hullCenterX += vol * c.
x;
2137 hullCenterY += vol * c.
y;
2138 hullCenterZ += vol * c.
z;
2153 }
while (e != v->
edges);
2166 hullCenter /= 4 * volume.
toScalar();
2169 int faceCount = faces.
size();
2171 if (clampAmount > 0)
2174 for (
int i = 0; i < faceCount; i++)
2189 amount =
btMin(amount, minDist * clampAmount);
2192 unsigned int seed = 243703;
2193 for (
int i = 0; i < faceCount; i++, seed = 1664525 * seed + 1013904223)
2195 btSwap(faces[i], faces[seed % faceCount]);
2198 for (
int i = 0; i < faceCount; i++)
2200 if (!
shiftFace(faces[i], amount, stack))
2230 #ifdef DEBUG_CONVEX_HULL
2231 printf(
"\nShrinking face (%d %d %d) (%d %d %d) (%d %d %d) by (%d %d %d)\n",
2232 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);
2236 int64_t shiftedDot = shiftedOrigin.
dot(normal);
2238 if (shiftedDot >= origDot)
2243 Edge* intersection = NULL;
2246 #ifdef DEBUG_CONVEX_HULL
2247 printf(
"Start edge is ");
2249 printf(
", normal is (%lld %lld %lld), shifted dot is %lld\n", normal.
x, normal.
y, normal.
z, shiftedDot);
2252 int cmp = optDot.
compare(shiftedDot);
2253 #ifdef SHOW_ITERATIONS
2258 Edge* e = startEdge;
2261 #ifdef SHOW_ITERATIONS
2266 #ifdef DEBUG_CONVEX_HULL
2267 printf(
"Moving downwards, edge is ");
2269 printf(
", dot is %f (%f %lld)\n", (
float) dot.
toScalar(), (float) optDot.
toScalar(), shiftedDot);
2273 int c = dot.
compare(shiftedDot);
2285 }
while (e != startEdge);
2294 Edge* e = startEdge;
2297 #ifdef SHOW_ITERATIONS
2302 #ifdef DEBUG_CONVEX_HULL
2303 printf(
"Moving upwards, edge is ");
2305 printf(
", dot is %f (%f %lld)\n", (
float) dot.
toScalar(), (float) optDot.
toScalar(), shiftedDot);
2309 cmp = dot.
compare(shiftedDot);
2320 }
while (e != startEdge);
2328 #ifdef SHOW_ITERATIONS
2329 printf(
"Needed %d iterations to find initial intersection\n", n);
2335 #ifdef SHOW_ITERATIONS
2340 #ifdef SHOW_ITERATIONS
2344 if (e == intersection->
reverse)
2348 #ifdef DEBUG_CONVEX_HULL
2349 printf(
"Checking for outwards edge, current edge is ");
2354 #ifdef SHOW_ITERATIONS
2355 printf(
"Needed %d iterations to check for complete containment\n", n);
2359 Edge* firstIntersection = NULL;
2360 Edge* faceEdge = NULL;
2361 Edge* firstFaceEdge = NULL;
2363 #ifdef SHOW_ITERATIONS
2368 #ifdef SHOW_ITERATIONS
2371 #ifdef DEBUG_CONVEX_HULL
2372 printf(
"Intersecting edge is ");
2373 intersection->print();
2380 #ifdef SHOW_ITERATIONS
2385 #ifdef SHOW_ITERATIONS
2399 #ifdef SHOW_ITERATIONS
2400 printf(
"Needed %d iterations to advance intersection\n", n);
2404 #ifdef DEBUG_CONVEX_HULL
2405 printf(
"Advanced intersecting edge to ");
2406 intersection->print();
2407 printf(
", cmp = %d\n", cmp);
2410 if (!firstIntersection)
2412 firstIntersection = intersection;
2414 else if (intersection == firstIntersection)
2420 Edge* prevIntersection = intersection;
2421 Edge* prevFaceEdge = faceEdge;
2424 #ifdef SHOW_ITERATIONS
2429 #ifdef SHOW_ITERATIONS
2435 #ifdef DEBUG_CONVEX_HULL
2436 printf(
"Testing edge ");
2438 printf(
" -> cmp = %d\n", cmp);
2446 #ifdef SHOW_ITERATIONS
2447 printf(
"Needed %d iterations to find other intersection of face\n", n);
2456 removed->
edges = NULL;
2464 #ifdef DEBUG_CONVEX_HULL
2465 printf(
"1: Removed part contains (%d %d %d)\n", removed->
point.
x, removed->
point.
y, removed->
point.
z);
2479 v->point.index = -1;
2488 v->point.x = (
int32_t) v->point128.xvalue();
2489 v->point.y = (
int32_t) v->point128.yvalue();
2490 v->point.z = (
int32_t) v->point128.zvalue();
2491 intersection->
target = v;
2506 if ((prevCmp == 0) || prevFaceEdge)
2527 else if (faceEdge != prevFaceEdge->
reverse)
2535 #ifdef DEBUG_CONVEX_HULL
2536 printf(
"2: Removed part contains (%d %d %d)\n", removed->
point.
x, removed->
point.
y, removed->
point.
z);
2542 faceEdge->
face = face;
2547 firstFaceEdge = faceEdge;
2550 #ifdef SHOW_ITERATIONS
2551 printf(
"Needed %d iterations to process all intersections\n", m);
2560 else if (firstFaceEdge != faceEdge->
reverse)
2568 #ifdef DEBUG_CONVEX_HULL
2569 printf(
"3: Removed part contains (%d %d %d)\n", removed->
point.
x, removed->
point.
y, removed->
point.
z);
2578 #ifdef DEBUG_CONVEX_HULL
2579 printf(
"Removing part\n");
2581 #ifdef SHOW_ITERATIONS
2585 while (pos < stack.
size())
2587 int end = stack.
size();
2590 Vertex* kept = stack[pos++];
2591 #ifdef DEBUG_CONVEX_HULL
2594 bool deeper =
false;
2596 while ((removed = stack[pos++]) != NULL)
2598 #ifdef SHOW_ITERATIONS
2602 while (removed->
edges)
2619 #ifdef SHOW_ITERATIONS
2620 printf(
"Needed %d iterations to remove part\n", n);
2624 face->
origin = shiftedOrigin;
2632 int index = vertex->
copy;
2635 index = vertices.
size();
2636 vertex->
copy = index;
2638 #ifdef DEBUG_CONVEX_HULL
2639 printf(
"Vertex %d gets index *%d\n", vertex->
point.
index, index);
2656 hull.
compute(coords, doubleCoords, stride, count);
2659 if ((shrink > 0) && ((shift = hull.
shrink(shrink, shrinkClamp)) < 0))
2674 while (copied < oldVertices.
size())
2688 int s = edges.size();
2689 edges.push_back(
Edge());
2690 edges.push_back(
Edge());
2691 Edge* c = &edges[s];
2692 Edge* r = &edges[s + 1];
2699 #ifdef DEBUG_CONVEX_HULL
2700 printf(
" CREATE: Vertex *%d has edge to *%d\n", copied, c->
getTargetVertex());
2705 edges[e->
copy].next = prevCopy - e->
copy;
2709 firstCopy = e->
copy;
2713 }
while (e != firstEdge);
2714 edges[firstCopy].
next = prevCopy - firstCopy;
2719 for (
int i = 0; i < copied; i++)
2730 #ifdef DEBUG_CONVEX_HULL
2731 printf(
"Vertex *%d has edge to *%d\n", i, edges[e->
copy].getTargetVertex());
2733 faces.push_back(e->
copy);
2737 #ifdef DEBUG_CONVEX_HULL
2738 printf(
" Face *%d\n", edges[f->
copy].getTargetVertex());
2745 }
while (e != firstEdge);
Rational128(int64_t value)
Int128 operator-(const Int128 &b) const
void push_back(const T &_Val)
int64_t dot(const Point64 &b) const
void init(Vertex *a, Vertex *b, Vertex *c)
static Orientation getOrientation(const Edge *prev, const Edge *next, const Point32 &s, const Point32 &t)
Face * nextWithSameNearbyVertex
unsigned long long int uint64_t
PointR128(Int128 x, Int128 y, Int128 z, Int128 denominator)
The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods It...
float dist(const Point3 &pnt0, const Point3 &pnt1)
btScalar shrink(btScalar amount, btScalar clampAmount)
static Int128 mul(int64_t a, int64_t b)
Int128 operator*(int64_t b) const
int getTargetVertex() const
Rational64(int64_t numerator, int64_t denominator)
int compare(const Rational64 &b) const
static uint64_t mul(uint32_t a, uint32_t b)
void setArraySize(int arraySize)
int compare(const Rational128 &b) const
static Int128 mul(uint64_t a, uint64_t b)
static int getVertexCopy(btConvexHullInternal::Vertex *vertex, btAlignedObjectArray< btConvexHullInternal::Vertex * > &vertices)
PoolArray< T > * nextArray
btScalar dot(const btVector3 &v) const
Return the dot product.
btScalar toScalar() const
void clear()
clear the array, deallocated memory. Generally it is better to use array.resize(0), to reduce performance overhead of run-time memory (de)allocations.
static uint32_t high(uint64_t value)
void findEdgeForCoplanarFaces(Vertex *c0, Vertex *c1, Edge *&e0, Edge *&e1, Vertex *stop0, Vertex *stop1)
static void shlHalf(uint64_t &value)
Point64 cross(const Point64 &b) const
static uint32_t low(uint64_t value)
bool operator<(const Int128 &b) const
int size() const
return the number of elements in the array
btVector3 getCoordinates(const Vertex *v)
Point32 operator-(const Point32 &b) const
void compute(const void *coords, bool doubleCoords, int stride, int count)
static float max(float a, float b)
bool operator==(const Point32 &b) const
int64_t dot(const Point64 &b) const
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Edge * findMaxAngle(bool ccw, const Vertex *start, const Point32 &s, const Point64 &rxs, const Point64 &sxrxs, Rational64 &minCot)
#define btAlignedFree(ptr)
int64_t dot(const Point32 &b) const
Edge * newEdgePair(Vertex *from, Vertex *to)
Int128 & operator+=(const Int128 &b)
static void mul(UWord a, UWord b, UWord &resLow, UWord &resHigh)
static float min(float a, float b)
btVector3 can be used to represent 3D points and vectors.
Int128(uint64_t low, uint64_t high)
btScalar toScalar() const
Point32 operator+(const Point32 &b) const
bool mergeProjection(IntermediateHull &h0, IntermediateHull &h1, Vertex *&c0, Vertex *&c1)
btScalar btAtan(btScalar x)
Rational128 dot(const Point64 &b) const
btAlignedObjectArray< Vertex * > originalVertices
btVector3 normalized() const
Return a normalized version of this vector.
bool shiftFace(Face *face, btScalar amount, btAlignedObjectArray< Vertex * > stack)
int ucmp(const Int128 &b) const
void resize(int newsize, const T &fillData=T())
Point64(int64_t x, int64_t y, int64_t z)
static void shlHalf(Int128 &value)
void freeObject(T *object)
static float4 cross(const float4 &a, const float4 &b)
btVector3 getBtNormal(Face *face)
void computeInternal(int start, int end, IntermediateHull &result)
int minAxis() const
Return the axis with the smallest value Note return values are 0,1,2 for x, y, or z...
Point32(int32_t x, int32_t y, int32_t z)
Pool< Vertex > vertexPool
void receiveNearbyFaces(Vertex *src)
static uint64_t high(Int128 value)
void removeEdgePair(Edge *edge)
#define btAlignedAlloc(size, alignment)
btScalar compute(const void *coords, bool doubleCoords, int stride, int count, btScalar shrink, btScalar shrinkClamp)
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
btScalar toScalar() const
void setMax(const btVector3 &other)
Set each element to the max of the current values and the values of another btVector3.
btVector3 toBtVector(const Point32 &v)
Rational128(const Int128 &numerator, const Int128 &denominator)
Int128 operator+(const Int128 &b) const
const T & btMin(const T &a, const T &b)
bool isNegativeInfinity() const
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
void quickSort(const L &CompareFunc)
Point32 operator-(const Vertex &b) const
static uint64_t low(Int128 value)
void merge(IntermediateHull &h0, IntermediateHull &h1)
int maxAxis() const
Return the axis with the largest value Note return values are 0,1,2 for x, y, or z.
bool operator!=(const Point32 &b) const
Point64 cross(const Point32 &b) const