Bullet Collision Detection & Physics Library
|
Go to the documentation of this file.
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);
Pool< Vertex > vertexPool
static uint64_t high(Int128 value)
static void shlHalf(uint64_t &value)
PointR128(Int128 x, Int128 y, Int128 z, Int128 denominator)
bool operator==(const Point32 &b) const
static uint32_t low(uint64_t value)
#define btAlignedFree(ptr)
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
static void shlHalf(Int128 &value)
static Orientation getOrientation(const Edge *prev, const Edge *next, const Point32 &s, const Point32 &t)
Int128 operator-(const Int128 &b) const
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Point64 cross(const Point64 &b) const
Point64(int64_t x, int64_t y, int64_t z)
int getTargetVertex() const
void merge(IntermediateHull &h0, IntermediateHull &h1)
Int128(uint64_t low, uint64_t high)
Rational128(int64_t value)
void quickSort(const L &CompareFunc)
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
void setArraySize(int arraySize)
Int128 operator*(int64_t b) const
void clear()
clear the array, deallocated memory. Generally it is better to use array.resize(0),...
int maxAxis() const
Return the axis with the largest value Note return values are 0,1,2 for x, y, or z.
btScalar toScalar() const
btScalar dot(const btVector3 &v) const
Return the dot product.
bool shiftFace(Face *face, btScalar amount, btAlignedObjectArray< Vertex * > stack)
#define btAlignedAlloc(size, alignment)
Rational128 dot(const Point64 &b) const
bool operator()(const btConvexHullInternal::Point32 &p, const btConvexHullInternal::Point32 &q) const
int64_t dot(const Point64 &b) const
btVector3 getBtNormal(Face *face)
void computeInternal(int start, int end, IntermediateHull &result)
static void mul(UWord a, UWord b, UWord &resLow, UWord &resHigh)
int compare(const Rational128 &b) const
const T & btMin(const T &a, const T &b)
void findEdgeForCoplanarFaces(Vertex *c0, Vertex *c1, Edge *&e0, Edge *&e1, Vertex *stop0, Vertex *stop1)
int compare(const Rational64 &b) const
btScalar toScalar() const
unsigned long long int uint64_t
Point32 operator-(const Vertex &b) const
bool operator<(const Int128 &b) const
void resize(int newsize, const T &fillData=T())
void setMax(const btVector3 &other)
Set each element to the max of the current values and the values of another btVector3.
Point64 cross(const Point32 &b) const
Point32(int32_t x, int32_t y, int32_t z)
void compute(const void *coords, bool doubleCoords, int stride, int count)
bool mergeProjection(IntermediateHull &h0, IntermediateHull &h1, Vertex *&c0, Vertex *&c1)
void init(Vertex *a, Vertex *b, Vertex *c)
bool isNegativeInfinity() const
btVector3 getCoordinates(const Vertex *v)
Edge * newEdgePair(Vertex *from, Vertex *to)
int ucmp(const Int128 &b) const
btVector3 can be used to represent 3D points and vectors.
static Int128 mul(int64_t a, int64_t b)
Point32 operator+(const Point32 &b) const
Int128 operator+(const Int128 &b) const
btScalar toScalar() const
PoolArray< T > * nextArray
Face * nextWithSameNearbyVertex
static uint64_t low(Int128 value)
Edge * findMaxAngle(bool ccw, const Vertex *start, const Point32 &s, const Point64 &rxs, const Point64 &sxrxs, Rational64 &minCot)
static uint32_t high(uint64_t value)
The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods It...
Point32 operator-(const Point32 &b) const
int64_t dot(const Point32 &b) const
Rational64(int64_t numerator, int64_t denominator)
static int getVertexCopy(btConvexHullInternal::Vertex *vertex, btAlignedObjectArray< btConvexHullInternal::Vertex * > &vertices)
Int128 & operator+=(const Int128 &b)
void freeObject(T *object)
btScalar shrink(btScalar amount, btScalar clampAmount)
btAlignedObjectArray< Vertex * > originalVertices
static uint64_t mul(uint32_t a, uint32_t b)
int64_t dot(const Point64 &b) const
int minAxis() const
Return the axis with the smallest value Note return values are 0,1,2 for x, y, or z.
bool operator!=(const Point32 &b) const
btScalar compute(const void *coords, bool doubleCoords, int stride, int count, btScalar shrink, btScalar shrinkClamp)
void push_back(const T &_Val)
btVector3 toBtVector(const Point32 &v)
static Int128 mul(uint64_t a, uint64_t b)
btScalar btAtan(btScalar x)
Rational128(const Int128 &numerator, const Int128 &denominator)
void removeEdgePair(Edge *edge)
btVector3 normalized() const
Return a normalized version of this vector.
int size() const
return the number of elements in the array
void receiveNearbyFaces(Vertex *src)