24 #elif defined(_MSC_VER) 44 #if defined(DEBUG_CONVEX_HULL) || defined(SHOW_ITERATIONS) 66 return (
x == 0) && (
y == 0) && (
z == 0);
71 return x * b.
x +
y * b.
y +
z * b.
z;
93 return (
x == b.
x) && (
y == b.
y) && (
z == b.
z);
98 return (
x != b.
x) || (
y != b.
y) || (
z != b.
z);
103 return (
x == 0) && (
y == 0) && (
z == 0);
118 return x * b.
x +
y * b.
y +
z * b.
z;
123 return x * b.
x +
y * b.
y +
z * b.
z;
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)
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)
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)
286 else if (numerator < 0)
300 else if (denominator < 0)
343 this->numerator = value;
348 this->numerator = -value;
444 #ifdef DEBUG_CONVEX_HULL 496 f->nearbyVertex =
this;
529 #ifdef DEBUG_CONVEX_HULL 558 if (a->lastNearbyFace)
564 a->firstNearbyFace =
this;
566 a->lastNearbyFace =
this;
575 template <
typename UWord,
typename UHWord>
621 static void mul(UWord a, UWord b, UWord& resLow, UWord& resHigh)
627 UWord p0110 = UWord(
low(p01)) + UWord(
low(p10));
665 template <
typename T>
688 for (
int i = 0; i <
size; i++, o++)
690 o->next = (i + 1 <
size) ? o + 1 : NULL;
696 template <
typename T>
819 bool mergeProjection(IntermediateHull& h0, IntermediateHull& h1, Vertex*& c0, Vertex*& c1);
821 void merge(IntermediateHull& h0, IntermediateHull& h1);
832 void compute(
const void* coords,
bool doubleCoords,
int stride,
int count);
842 Int128 a = negative ? -*this : *
this;
845 negative = !negative;
850 return negative ? -result : result;
857 #ifdef USE_X86_64_ASM 859 :
"=a"(result.
low),
"=d"(result.
high)
865 bool negative = a < 0;
872 negative = !negative;
876 return negative ? -result : result;
884 #ifdef USE_X86_64_ASM 886 :
"=a"(result.
low),
"=d"(result.
high)
901 return sign - b.
sign;
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" 924 "orq %%rdx, %%rax\n\t" 927 "shll $16, %%ebx\n\t" 928 :
"=&b"(result), [tmp]
"=&r"(tmp),
"=a"(dummy)
931 return result ? result ^ sign
946 return sign - b.
sign;
957 Int128 nbdLow, nbdHigh, dbnLow, dbnHigh;
961 int cmp = nbdHigh.
ucmp(dbnHigh);
966 return nbdLow.
ucmp(dbnLow) * sign;
974 return (a > b) ? 1 : (a < b) ? -1 : 0;
996 return numerator.ucmp(denominator * b) * sign;
1073 for (
int side = 0; side <= 1; side++)
1087 if ((dy0 <= 0) && ((dx0 == 0) || ((dx0 < 0) && (dy0 * dx <= dy * dx0))))
1101 if ((dxn > 0) && (dy1 < 0) && ((dx1 == 0) || ((dx1 < 0) && (dy1 * dx < dy * dx1))))
1123 if ((dy1 >= 0) && ((dx1 == 0) || ((dx1 < 0) && (dy1 * dx <= dy * dx1))))
1137 if ((dxn < 0) && (dy0 > 0) && ((dx0 == 0) || ((dx0 < 0) && (dy0 * dx < dy * dx0))))
1207 int n = end - start;
1211 result.
minXy = NULL;
1212 result.
maxXy = NULL;
1213 result.
minYx = NULL;
1214 result.
maxYx = NULL;
1225 if ((dx == 0) && (dy == 0))
1248 if ((dx < 0) || ((dx == 0) && (dy < 0)))
1259 if ((dy < 0) || ((dy == 0) && (dx < 0)))
1312 int split0 = start + n / 2;
1314 int split1 = split0;
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 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);
1402 if (prev->
next == next)
1404 if (prev->
prev == next)
1415 else if (prev->
prev == next)
1427 Edge* minEdge = NULL;
1429 #ifdef DEBUG_CONVEX_HULL 1430 printf(
"find max edge for %d\n", start->
point.
index);
1441 #ifdef DEBUG_CONVEX_HULL 1442 printf(
" Angle is %f (%d) for ", (
float)
btAtan(cot.toScalar()), (
int)cot.isNaN());
1452 if (minEdge == NULL)
1457 else if ((cmp = cot.compare(minCot)) < 0)
1467 #ifdef DEBUG_CONVEX_HULL 1472 }
while (e != start->
edges);
1484 Point64 normal = ((start0 ? start0 : start1)->target->point - c0->
point).cross(s);
1490 #ifdef DEBUG_CONVEX_HULL 1497 while (e0->
target != stop0)
1523 while (e1->
target != stop1)
1546 #ifdef DEBUG_CONVEX_HULL 1547 printf(
" Starting at %d %d\n", et0.
index, et1.
index);
1550 int64_t dx = maxDot1 - maxDot0;
1557 if (e0 && (e0->
target != stop0))
1567 dx = (et1 - et0).
dot(perp);
1568 e0 = (e0 == start0) ? NULL : f0;
1574 if (e1 && (e1->
target != stop1))
1580 if (d1.
dot(normal) == 0)
1609 if (e1 && (e1->
target != stop1))
1619 dx = (et1 - et0).
dot(perp);
1620 e1 = (e1 == start1) ? NULL : f1;
1626 if (e0 && (e0->
target != stop0))
1632 if (d0.
dot(normal) == 0)
1655 #ifdef DEBUG_CONVEX_HULL 1656 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);
1952 return (p.
y < q.
y) || ((p.
y == q.
y) && ((p.
x < q.
x) || ((p.
x == q.
x) && (p.
z < q.
z))));
1959 const char* ptr = (
const char*)coords;
1962 for (
int i = 0; i < count; i++)
1964 const double* v = (
const double*)ptr;
1973 for (
int i = 0; i < count; i++)
1975 const float* v = (
const float*)ptr;
2016 ptr = (
const char*)coords;
2019 for (
int i = 0; i < count; i++)
2021 const double* v = (
const double*)ptr;
2028 points[i].index = i;
2033 for (
int i = 0; i < count; i++)
2035 const float* v = (
const float*)ptr;
2042 points[i].index = i;
2050 for (
int i = 0; i < count; i++)
2054 v->
point = points[i];
2072 #ifdef DEBUG_CONVEX_HULL 2113 Int128 hullCenterX(0, 0);
2114 Int128 hullCenterY(0, 0);
2115 Int128 hullCenterZ(0, 0);
2118 while (stack.
size() > 0)
2132 if (e->
copy != stamp)
2148 hullCenterX += vol * c.
x;
2149 hullCenterY += vol * c.
y;
2150 hullCenterZ += vol * c.
z;
2165 }
while (e != v->
edges);
2178 hullCenter /= 4 * volume.
toScalar();
2181 int faceCount = faces.
size();
2183 if (clampAmount > 0)
2186 for (
int i = 0; i < faceCount; i++)
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))
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);
2248 int64_t shiftedDot = shiftedOrigin.
dot(normal);
2250 if (shiftedDot >= origDot)
2255 Edge* intersection = NULL;
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);
2264 int cmp = optDot.
compare(shiftedDot);
2265 #ifdef SHOW_ITERATIONS 2270 Edge* e = startEdge;
2273 #ifdef SHOW_ITERATIONS 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 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);
2347 #ifdef SHOW_ITERATIONS 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();
2392 #ifdef SHOW_ITERATIONS 2397 #ifdef SHOW_ITERATIONS 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;
2436 #ifdef SHOW_ITERATIONS 2441 #ifdef SHOW_ITERATIONS 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);
2468 removed->
edges = NULL;
2476 #ifdef DEBUG_CONVEX_HULL 2477 printf(
"1: Removed part contains (%d %d %d)\n", removed->
point.
x, removed->
point.
y, removed->
point.
z);
2491 v->point.index = -1;
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;
2515 if ((prevCmp == 0) || prevFaceEdge)
2536 else if (faceEdge != prevFaceEdge->
reverse)
2544 #ifdef DEBUG_CONVEX_HULL 2545 printf(
"2: Removed part contains (%d %d %d)\n", removed->
point.
x, removed->
point.
y, removed->
point.
z);
2551 faceEdge->
face = face;
2556 firstFaceEdge = faceEdge;
2559 #ifdef SHOW_ITERATIONS 2560 printf(
"Needed %d iterations to process all intersections\n", m);
2569 else if (firstFaceEdge != faceEdge->
reverse)
2577 #ifdef DEBUG_CONVEX_HULL 2578 printf(
"3: Removed part contains (%d %d %d)\n", removed->
point.
x, removed->
point.
y, removed->
point.
z);
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 2611 while (removed->
edges)
2628 #ifdef SHOW_ITERATIONS 2629 printf(
"Needed %d iterations to remove part\n", n);
2633 face->
origin = shiftedOrigin;
2640 int index = vertex->
copy;
2643 index = vertices.
size();
2644 vertex->
copy = index;
2646 #ifdef DEBUG_CONVEX_HULL 2647 printf(
"Vertex %d gets index *%d\n", vertex->
point.
index, index);
2664 hull.
compute(coords, doubleCoords, stride, count);
2682 while (copied < oldVertices.
size())
2696 int s = edges.size();
2697 edges.push_back(
Edge());
2698 edges.push_back(
Edge());
2699 Edge* c = &edges[s];
2700 Edge* r = &edges[s + 1];
2707 #ifdef DEBUG_CONVEX_HULL 2708 printf(
" CREATE: Vertex *%d has edge to *%d\n", copied, c->
getTargetVertex());
2713 edges[e->
copy].next = prevCopy - e->
copy;
2717 firstCopy = e->
copy;
2721 }
while (e != firstEdge);
2722 edges[firstCopy].
next = prevCopy - firstCopy;
2727 for (
int i = 0; i < copied; i++)
2738 #ifdef DEBUG_CONVEX_HULL 2739 printf(
"Vertex *%d has edge to *%d\n", i, edges[e->
copy].getTargetVertex());
2741 faces.push_back(e->
copy);
2745 #ifdef DEBUG_CONVEX_HULL 2746 printf(
" Face *%d\n", edges[f->
copy].getTargetVertex());
2753 }
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)
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), to reduce performance overhead of run-time memory (de)allocations.
bool shiftFace(Face *face, btScalar amount, btAlignedObjectArray< Vertex *> stack)
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
Int128 operator*(int64_t b) 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
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.
bool operator()(const btConvexHullInternal::Point32 &p, const btConvexHullInternal::Point32 &q) const
static int getVertexCopy(btConvexHullInternal::Vertex *vertex, btAlignedObjectArray< btConvexHullInternal::Vertex *> &vertices)
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