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)
345 this->numerator = value;
350 this->numerator = -value;
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)))
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();
1371 void 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 1441 printf(
" Angle is %f (%d) for ", (
float)
btAtan(cot.toScalar()), (
int) cot.isNaN());
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);
2492 v->point.index = -1;
2501 v->point.x = (
int32_t) v->point128.xvalue();
2502 v->point.y = (
int32_t) v->point128.yvalue();
2503 v->point.z = (
int32_t) v->point128.zvalue();
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);
Rational128(int64_t value)
void push_back(const T &_Val)
void init(Vertex *a, Vertex *b, Vertex *c)
static Orientation getOrientation(const Edge *prev, const Edge *next, const Point32 &s, const Point32 &t)
Int128 operator-(const Int128 &b) const
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...
btScalar shrink(btScalar amount, btScalar clampAmount)
static Int128 mul(int64_t a, int64_t b)
int compare(const Rational128 &b) const
Point64 cross(const Point32 &b) const
Rational64(int64_t numerator, int64_t denominator)
int ucmp(const Int128 &b) const
static uint64_t mul(uint32_t a, uint32_t b)
void setArraySize(int arraySize)
Int128 operator+(const Int128 &b) const
int64_t dot(const Point32 &b) const
static Int128 mul(uint64_t a, uint64_t b)
static int getVertexCopy(btConvexHullInternal::Vertex *vertex, btAlignedObjectArray< btConvexHullInternal::Vertex * > &vertices)
PoolArray< T > * nextArray
btVector3 normalized() const
Return a normalized version of this vector.
bool operator<(const Int128 &b) const
void clear()
clear the array, deallocated memory. Generally it is better to use array.resize(0),...
static uint32_t high(uint64_t value)
void findEdgeForCoplanarFaces(Vertex *c0, Vertex *c1, Edge *&e0, Edge *&e1, Vertex *stop0, Vertex *stop1)
int64_t dot(const Point64 &b) const
int getTargetVertex() const
static void shlHalf(uint64_t &value)
static uint32_t low(uint64_t value)
int minAxis() const
Return the axis with the smallest value Note return values are 0,1,2 for x, y, or z.
btVector3 getCoordinates(const Vertex *v)
btScalar toScalar() const
void compute(const void *coords, bool doubleCoords, int stride, int count)
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
btScalar dot(const btVector3 &v) const
Return the dot product.
Edge * findMaxAngle(bool ccw, const Vertex *start, const Point32 &s, const Point64 &rxs, const Point64 &sxrxs, Rational64 &minCot)
#define btAlignedFree(ptr)
Edge * newEdgePair(Vertex *from, Vertex *to)
Int128 & operator+=(const Int128 &b)
static void mul(UWord a, UWord b, UWord &resLow, UWord &resHigh)
btVector3 can be used to represent 3D points and vectors.
Int128(uint64_t low, uint64_t high)
int size() const
return the number of elements in the array
bool mergeProjection(IntermediateHull &h0, IntermediateHull &h1, Vertex *&c0, Vertex *&c1)
btScalar btAtan(btScalar x)
btScalar toScalar() const
btAlignedObjectArray< Vertex * > originalVertices
bool shiftFace(Face *face, btScalar amount, btAlignedObjectArray< Vertex * > stack)
Int128 operator *(int64_t 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)
btScalar toScalar() const
btVector3 getBtNormal(Face *face)
bool operator!=(const Point32 &b) const
void computeInternal(int start, int end, IntermediateHull &result)
Point32(int32_t x, int32_t y, int32_t z)
Rational128 dot(const Point64 &b) const
Pool< Vertex > vertexPool
void receiveNearbyFaces(Vertex *src)
static uint64_t high(Int128 value)
void removeEdgePair(Edge *edge)
#define btAlignedAlloc(size, alignment)
int maxAxis() const
Return the axis with the largest value Note return values are 0,1,2 for x, y, or z.
Point32 operator-(const Vertex &b) const
btScalar compute(const void *coords, bool doubleCoords, int stride, int count, btScalar shrink, btScalar shrinkClamp)
Point64 cross(const Point64 &b) const
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
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)
int compare(const Rational64 &b) const
const T & btMin(const T &a, const T &b)
Point32 operator+(const Point32 &b) const
bool operator==(const Point32 &b) const
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
void quickSort(const L &CompareFunc)
static uint64_t low(Int128 value)
void merge(IntermediateHull &h0, IntermediateHull &h1)
int64_t dot(const Point64 &b) const
bool isNegativeInfinity() const
Point32 operator-(const Point32 &b) const