00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015 #include <string.h>
00016
00017 #include "btConvexHullComputer.h"
00018 #include "btAlignedObjectArray.h"
00019 #include "btMinMax.h"
00020 #include "btVector3.h"
00021
00022 #ifdef __GNUC__
00023 #include <stdint.h>
00024 #elif defined(_MSC_VER)
00025 typedef __int32 int32_t;
00026 typedef __int64 int64_t;
00027 typedef unsigned __int32 uint32_t;
00028 typedef unsigned __int64 uint64_t;
00029 #else
00030 typedef int int32_t;
00031 typedef long long int int64_t;
00032 typedef unsigned int uint32_t;
00033 typedef unsigned long long int uint64_t;
00034 #endif
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046 #if defined(DEBUG_CONVEX_HULL) || defined(SHOW_ITERATIONS)
00047 #include <stdio.h>
00048 #endif
00049
00050
00051
00052 class btConvexHullInternal
00053 {
00054 public:
00055
00056 class Point64
00057 {
00058 public:
00059 int64_t x;
00060 int64_t y;
00061 int64_t z;
00062
00063 Point64(int64_t x, int64_t y, int64_t z): x(x), y(y), z(z)
00064 {
00065 }
00066
00067 bool isZero()
00068 {
00069 return (x == 0) && (y == 0) && (z == 0);
00070 }
00071
00072 int64_t dot(const Point64& b) const
00073 {
00074 return x * b.x + y * b.y + z * b.z;
00075 }
00076 };
00077
00078 class Point32
00079 {
00080 public:
00081 int32_t x;
00082 int32_t y;
00083 int32_t z;
00084 int index;
00085
00086 Point32()
00087 {
00088 }
00089
00090 Point32(int32_t x, int32_t y, int32_t z): x(x), y(y), z(z), index(-1)
00091 {
00092 }
00093
00094 bool operator==(const Point32& b) const
00095 {
00096 return (x == b.x) && (y == b.y) && (z == b.z);
00097 }
00098
00099 bool operator!=(const Point32& b) const
00100 {
00101 return (x != b.x) || (y != b.y) || (z != b.z);
00102 }
00103
00104 bool isZero()
00105 {
00106 return (x == 0) && (y == 0) && (z == 0);
00107 }
00108
00109 Point64 cross(const Point32& b) const
00110 {
00111 return Point64(y * b.z - z * b.y, z * b.x - x * b.z, x * b.y - y * b.x);
00112 }
00113
00114 Point64 cross(const Point64& b) const
00115 {
00116 return Point64(y * b.z - z * b.y, z * b.x - x * b.z, x * b.y - y * b.x);
00117 }
00118
00119 int64_t dot(const Point32& b) const
00120 {
00121 return x * b.x + y * b.y + z * b.z;
00122 }
00123
00124 int64_t dot(const Point64& b) const
00125 {
00126 return x * b.x + y * b.y + z * b.z;
00127 }
00128
00129 Point32 operator+(const Point32& b) const
00130 {
00131 return Point32(x + b.x, y + b.y, z + b.z);
00132 }
00133
00134 Point32 operator-(const Point32& b) const
00135 {
00136 return Point32(x - b.x, y - b.y, z - b.z);
00137 }
00138 };
00139
00140 class Int128
00141 {
00142 public:
00143 uint64_t low;
00144 uint64_t high;
00145
00146 Int128()
00147 {
00148 }
00149
00150 Int128(uint64_t low, uint64_t high): low(low), high(high)
00151 {
00152 }
00153
00154 Int128(uint64_t low): low(low), high(0)
00155 {
00156 }
00157
00158 Int128(int64_t value): low(value), high((value >= 0) ? 0 : (uint64_t) -1LL)
00159 {
00160 }
00161
00162 static Int128 mul(int64_t a, int64_t b);
00163
00164 static Int128 mul(uint64_t a, uint64_t b);
00165
00166 Int128 operator-() const
00167 {
00168 return Int128((uint64_t) -(int64_t)low, ~high + (low == 0));
00169 }
00170
00171 Int128 operator+(const Int128& b) const
00172 {
00173 #ifdef USE_X86_64_ASM
00174 Int128 result;
00175 __asm__ ("addq %[bl], %[rl]\n\t"
00176 "adcq %[bh], %[rh]\n\t"
00177 : [rl] "=r" (result.low), [rh] "=r" (result.high)
00178 : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high)
00179 : "cc" );
00180 return result;
00181 #else
00182 uint64_t lo = low + b.low;
00183 return Int128(lo, high + b.high + (lo < low));
00184 #endif
00185 }
00186
00187 Int128 operator-(const Int128& b) const
00188 {
00189 #ifdef USE_X86_64_ASM
00190 Int128 result;
00191 __asm__ ("subq %[bl], %[rl]\n\t"
00192 "sbbq %[bh], %[rh]\n\t"
00193 : [rl] "=r" (result.low), [rh] "=r" (result.high)
00194 : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high)
00195 : "cc" );
00196 return result;
00197 #else
00198 return *this + -b;
00199 #endif
00200 }
00201
00202 Int128& operator+=(const Int128& b)
00203 {
00204 #ifdef USE_X86_64_ASM
00205 __asm__ ("addq %[bl], %[rl]\n\t"
00206 "adcq %[bh], %[rh]\n\t"
00207 : [rl] "=r" (low), [rh] "=r" (high)
00208 : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high)
00209 : "cc" );
00210 #else
00211 uint64_t lo = low + b.low;
00212 if (lo < low)
00213 {
00214 ++high;
00215 }
00216 low = lo;
00217 high += b.high;
00218 #endif
00219 return *this;
00220 }
00221
00222 Int128& operator++()
00223 {
00224 if (++low == 0)
00225 {
00226 ++high;
00227 }
00228 return *this;
00229 }
00230
00231 Int128 operator*(int64_t b) const;
00232
00233 btScalar toScalar() const
00234 {
00235 return ((int64_t) high >= 0) ? btScalar(high) * (btScalar(0x100000000LL) * btScalar(0x100000000LL)) + btScalar(low)
00236 : -(-*this).toScalar();
00237 }
00238
00239 int getSign() const
00240 {
00241 return ((int64_t) high < 0) ? -1 : (high || low) ? 1 : 0;
00242 }
00243
00244 bool operator<(const Int128& b) const
00245 {
00246 return (high < b.high) || ((high == b.high) && (low < b.low));
00247 }
00248
00249 int ucmp(const Int128&b) const
00250 {
00251 if (high < b.high)
00252 {
00253 return -1;
00254 }
00255 if (high > b.high)
00256 {
00257 return 1;
00258 }
00259 if (low < b.low)
00260 {
00261 return -1;
00262 }
00263 if (low > b.low)
00264 {
00265 return 1;
00266 }
00267 return 0;
00268 }
00269 };
00270
00271
00272 class Rational64
00273 {
00274 private:
00275 uint64_t m_numerator;
00276 uint64_t m_denominator;
00277 int sign;
00278
00279 public:
00280 Rational64(int64_t numerator, int64_t denominator)
00281 {
00282 if (numerator > 0)
00283 {
00284 sign = 1;
00285 m_numerator = (uint64_t) numerator;
00286 }
00287 else if (numerator < 0)
00288 {
00289 sign = -1;
00290 m_numerator = (uint64_t) -numerator;
00291 }
00292 else
00293 {
00294 sign = 0;
00295 m_numerator = 0;
00296 }
00297 if (denominator > 0)
00298 {
00299 m_denominator = (uint64_t) denominator;
00300 }
00301 else if (denominator < 0)
00302 {
00303 sign = -sign;
00304 m_denominator = (uint64_t) -denominator;
00305 }
00306 else
00307 {
00308 m_denominator = 0;
00309 }
00310 }
00311
00312 bool isNegativeInfinity() const
00313 {
00314 return (sign < 0) && (m_denominator == 0);
00315 }
00316
00317 bool isNaN() const
00318 {
00319 return (sign == 0) && (m_denominator == 0);
00320 }
00321
00322 int compare(const Rational64& b) const;
00323
00324 btScalar toScalar() const
00325 {
00326 return sign * ((m_denominator == 0) ? SIMD_INFINITY : (btScalar) m_numerator / m_denominator);
00327 }
00328 };
00329
00330
00331 class Rational128
00332 {
00333 private:
00334 Int128 numerator;
00335 Int128 denominator;
00336 int sign;
00337 bool isInt64;
00338
00339 public:
00340 Rational128(int64_t value)
00341 {
00342 if (value > 0)
00343 {
00344 sign = 1;
00345 this->numerator = value;
00346 }
00347 else if (value < 0)
00348 {
00349 sign = -1;
00350 this->numerator = -value;
00351 }
00352 else
00353 {
00354 sign = 0;
00355 this->numerator = (uint64_t) 0;
00356 }
00357 this->denominator = (uint64_t) 1;
00358 isInt64 = true;
00359 }
00360
00361 Rational128(const Int128& numerator, const Int128& denominator)
00362 {
00363 sign = numerator.getSign();
00364 if (sign >= 0)
00365 {
00366 this->numerator = numerator;
00367 }
00368 else
00369 {
00370 this->numerator = -numerator;
00371 }
00372 int dsign = denominator.getSign();
00373 if (dsign >= 0)
00374 {
00375 this->denominator = denominator;
00376 }
00377 else
00378 {
00379 sign = -sign;
00380 this->denominator = -denominator;
00381 }
00382 isInt64 = false;
00383 }
00384
00385 int compare(const Rational128& b) const;
00386
00387 int compare(int64_t b) const;
00388
00389 btScalar toScalar() const
00390 {
00391 return sign * ((denominator.getSign() == 0) ? SIMD_INFINITY : numerator.toScalar() / denominator.toScalar());
00392 }
00393 };
00394
00395 class PointR128
00396 {
00397 public:
00398 Int128 x;
00399 Int128 y;
00400 Int128 z;
00401 Int128 denominator;
00402
00403 PointR128()
00404 {
00405 }
00406
00407 PointR128(Int128 x, Int128 y, Int128 z, Int128 denominator): x(x), y(y), z(z), denominator(denominator)
00408 {
00409 }
00410
00411 btScalar xvalue() const
00412 {
00413 return x.toScalar() / denominator.toScalar();
00414 }
00415
00416 btScalar yvalue() const
00417 {
00418 return y.toScalar() / denominator.toScalar();
00419 }
00420
00421 btScalar zvalue() const
00422 {
00423 return z.toScalar() / denominator.toScalar();
00424 }
00425 };
00426
00427
00428 class Edge;
00429 class Face;
00430
00431 class Vertex
00432 {
00433 public:
00434 Vertex* next;
00435 Vertex* prev;
00436 Edge* edges;
00437 Face* firstNearbyFace;
00438 Face* lastNearbyFace;
00439 PointR128 point128;
00440 Point32 point;
00441 int copy;
00442
00443 Vertex(): next(NULL), prev(NULL), edges(NULL), firstNearbyFace(NULL), lastNearbyFace(NULL), copy(-1)
00444 {
00445 }
00446
00447 #ifdef DEBUG_CONVEX_HULL
00448 void print()
00449 {
00450 printf("V%d (%d, %d, %d)", point.index, point.x, point.y, point.z);
00451 }
00452
00453 void printGraph();
00454 #endif
00455
00456 Point32 operator-(const Vertex& b) const
00457 {
00458 return point - b.point;
00459 }
00460
00461 Rational128 dot(const Point64& b) const
00462 {
00463 return (point.index >= 0) ? Rational128(point.dot(b))
00464 : Rational128(point128.x * b.x + point128.y * b.y + point128.z * b.z, point128.denominator);
00465 }
00466
00467 btScalar xvalue() const
00468 {
00469 return (point.index >= 0) ? btScalar(point.x) : point128.xvalue();
00470 }
00471
00472 btScalar yvalue() const
00473 {
00474 return (point.index >= 0) ? btScalar(point.y) : point128.yvalue();
00475 }
00476
00477 btScalar zvalue() const
00478 {
00479 return (point.index >= 0) ? btScalar(point.z) : point128.zvalue();
00480 }
00481
00482 void receiveNearbyFaces(Vertex* src)
00483 {
00484 if (lastNearbyFace)
00485 {
00486 lastNearbyFace->nextWithSameNearbyVertex = src->firstNearbyFace;
00487 }
00488 else
00489 {
00490 firstNearbyFace = src->firstNearbyFace;
00491 }
00492 if (src->lastNearbyFace)
00493 {
00494 lastNearbyFace = src->lastNearbyFace;
00495 }
00496 for (Face* f = src->firstNearbyFace; f; f = f->nextWithSameNearbyVertex)
00497 {
00498 btAssert(f->nearbyVertex == src);
00499 f->nearbyVertex = this;
00500 }
00501 src->firstNearbyFace = NULL;
00502 src->lastNearbyFace = NULL;
00503 }
00504 };
00505
00506
00507 class Edge
00508 {
00509 public:
00510 Edge* next;
00511 Edge* prev;
00512 Edge* reverse;
00513 Vertex* target;
00514 Face* face;
00515 int copy;
00516
00517 ~Edge()
00518 {
00519 next = NULL;
00520 prev = NULL;
00521 reverse = NULL;
00522 target = NULL;
00523 face = NULL;
00524 }
00525
00526 void link(Edge* n)
00527 {
00528 btAssert(reverse->target == n->reverse->target);
00529 next = n;
00530 n->prev = this;
00531 }
00532
00533 #ifdef DEBUG_CONVEX_HULL
00534 void print()
00535 {
00536 printf("E%p : %d -> %d, n=%p p=%p (0 %d\t%d\t%d) -> (%d %d %d)", this, reverse->target->point.index, target->point.index, next, prev,
00537 reverse->target->point.x, reverse->target->point.y, reverse->target->point.z, target->point.x, target->point.y, target->point.z);
00538 }
00539 #endif
00540 };
00541
00542 class Face
00543 {
00544 public:
00545 Face* next;
00546 Vertex* nearbyVertex;
00547 Face* nextWithSameNearbyVertex;
00548 Point32 origin;
00549 Point32 dir0;
00550 Point32 dir1;
00551
00552 Face(): next(NULL), nearbyVertex(NULL), nextWithSameNearbyVertex(NULL)
00553 {
00554 }
00555
00556 void init(Vertex* a, Vertex* b, Vertex* c)
00557 {
00558 nearbyVertex = a;
00559 origin = a->point;
00560 dir0 = *b - *a;
00561 dir1 = *c - *a;
00562 if (a->lastNearbyFace)
00563 {
00564 a->lastNearbyFace->nextWithSameNearbyVertex = this;
00565 }
00566 else
00567 {
00568 a->firstNearbyFace = this;
00569 }
00570 a->lastNearbyFace = this;
00571 }
00572
00573 Point64 getNormal()
00574 {
00575 return dir0.cross(dir1);
00576 }
00577 };
00578
00579 template<typename UWord, typename UHWord> class DMul
00580 {
00581 private:
00582 static uint32_t high(uint64_t value)
00583 {
00584 return (uint32_t) (value >> 32);
00585 }
00586
00587 static uint32_t low(uint64_t value)
00588 {
00589 return (uint32_t) value;
00590 }
00591
00592 static uint64_t mul(uint32_t a, uint32_t b)
00593 {
00594 return (uint64_t) a * (uint64_t) b;
00595 }
00596
00597 static void shlHalf(uint64_t& value)
00598 {
00599 value <<= 32;
00600 }
00601
00602 static uint64_t high(Int128 value)
00603 {
00604 return value.high;
00605 }
00606
00607 static uint64_t low(Int128 value)
00608 {
00609 return value.low;
00610 }
00611
00612 static Int128 mul(uint64_t a, uint64_t b)
00613 {
00614 return Int128::mul(a, b);
00615 }
00616
00617 static void shlHalf(Int128& value)
00618 {
00619 value.high = value.low;
00620 value.low = 0;
00621 }
00622
00623 public:
00624
00625 static void mul(UWord a, UWord b, UWord& resLow, UWord& resHigh)
00626 {
00627 UWord p00 = mul(low(a), low(b));
00628 UWord p01 = mul(low(a), high(b));
00629 UWord p10 = mul(high(a), low(b));
00630 UWord p11 = mul(high(a), high(b));
00631 UWord p0110 = UWord(low(p01)) + UWord(low(p10));
00632 p11 += high(p01);
00633 p11 += high(p10);
00634 p11 += high(p0110);
00635 shlHalf(p0110);
00636 p00 += p0110;
00637 if (p00 < p0110)
00638 {
00639 ++p11;
00640 }
00641 resLow = p00;
00642 resHigh = p11;
00643 }
00644 };
00645
00646 private:
00647
00648 class IntermediateHull
00649 {
00650 public:
00651 Vertex* minXy;
00652 Vertex* maxXy;
00653 Vertex* minYx;
00654 Vertex* maxYx;
00655
00656 IntermediateHull(): minXy(NULL), maxXy(NULL), minYx(NULL), maxYx(NULL)
00657 {
00658 }
00659
00660 void print();
00661 };
00662
00663 enum Orientation {NONE, CLOCKWISE, COUNTER_CLOCKWISE};
00664
00665 template <typename T> class PoolArray
00666 {
00667 private:
00668 T* array;
00669 int size;
00670
00671 public:
00672 PoolArray<T>* next;
00673
00674 PoolArray(int size): size(size), next(NULL)
00675 {
00676 array = (T*) btAlignedAlloc(sizeof(T) * size, 16);
00677 }
00678
00679 ~PoolArray()
00680 {
00681 btAlignedFree(array);
00682 }
00683
00684 T* init()
00685 {
00686 T* o = array;
00687 for (int i = 0; i < size; i++, o++)
00688 {
00689 o->next = (i+1 < size) ? o + 1 : NULL;
00690 }
00691 return array;
00692 }
00693 };
00694
00695 template <typename T> class Pool
00696 {
00697 private:
00698 PoolArray<T>* arrays;
00699 PoolArray<T>* nextArray;
00700 T* freeObjects;
00701 int arraySize;
00702
00703 public:
00704 Pool(): arrays(NULL), nextArray(NULL), freeObjects(NULL), arraySize(256)
00705 {
00706 }
00707
00708 ~Pool()
00709 {
00710 while (arrays)
00711 {
00712 PoolArray<T>* p = arrays;
00713 arrays = p->next;
00714 p->~PoolArray<T>();
00715 btAlignedFree(p);
00716 }
00717 }
00718
00719 void reset()
00720 {
00721 nextArray = arrays;
00722 freeObjects = NULL;
00723 }
00724
00725 void setArraySize(int arraySize)
00726 {
00727 this->arraySize = arraySize;
00728 }
00729
00730 T* newObject()
00731 {
00732 T* o = freeObjects;
00733 if (!o)
00734 {
00735 PoolArray<T>* p = nextArray;
00736 if (p)
00737 {
00738 nextArray = p->next;
00739 }
00740 else
00741 {
00742 p = new(btAlignedAlloc(sizeof(PoolArray<T>), 16)) PoolArray<T>(arraySize);
00743 p->next = arrays;
00744 arrays = p;
00745 }
00746 o = p->init();
00747 }
00748 freeObjects = o->next;
00749 return new(o) T();
00750 };
00751
00752 void freeObject(T* object)
00753 {
00754 object->~T();
00755 object->next = freeObjects;
00756 freeObjects = object;
00757 }
00758 };
00759
00760 btVector3 scaling;
00761 btVector3 center;
00762 Pool<Vertex> vertexPool;
00763 Pool<Edge> edgePool;
00764 Pool<Face> facePool;
00765 btAlignedObjectArray<Vertex*> originalVertices;
00766 int mergeStamp;
00767 int minAxis;
00768 int medAxis;
00769 int maxAxis;
00770 int usedEdgePairs;
00771 int maxUsedEdgePairs;
00772
00773 static Orientation getOrientation(const Edge* prev, const Edge* next, const Point32& s, const Point32& t);
00774 Edge* findMaxAngle(bool ccw, const Vertex* start, const Point32& s, const Point64& rxs, const Point64& sxrxs, Rational64& minCot);
00775 void findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge*& e0, Edge*& e1, Vertex* stop0, Vertex* stop1);
00776
00777 Edge* newEdgePair(Vertex* from, Vertex* to);
00778
00779 void removeEdgePair(Edge* edge)
00780 {
00781 Edge* n = edge->next;
00782 Edge* r = edge->reverse;
00783
00784 btAssert(edge->target && r->target);
00785
00786 if (n != edge)
00787 {
00788 n->prev = edge->prev;
00789 edge->prev->next = n;
00790 r->target->edges = n;
00791 }
00792 else
00793 {
00794 r->target->edges = NULL;
00795 }
00796
00797 n = r->next;
00798
00799 if (n != r)
00800 {
00801 n->prev = r->prev;
00802 r->prev->next = n;
00803 edge->target->edges = n;
00804 }
00805 else
00806 {
00807 edge->target->edges = NULL;
00808 }
00809
00810 edgePool.freeObject(edge);
00811 edgePool.freeObject(r);
00812 usedEdgePairs--;
00813 }
00814
00815 void computeInternal(int start, int end, IntermediateHull& result);
00816
00817 bool mergeProjection(IntermediateHull& h0, IntermediateHull& h1, Vertex*& c0, Vertex*& c1);
00818
00819 void merge(IntermediateHull& h0, IntermediateHull& h1);
00820
00821 btVector3 toBtVector(const Point32& v);
00822
00823 btVector3 getBtNormal(Face* face);
00824
00825 bool shiftFace(Face* face, btScalar amount, btAlignedObjectArray<Vertex*> stack);
00826
00827 public:
00828 Vertex* vertexList;
00829
00830 void compute(const void* coords, bool doubleCoords, int stride, int count);
00831
00832 btVector3 getCoordinates(const Vertex* v);
00833
00834 btScalar shrink(btScalar amount, btScalar clampAmount);
00835 };
00836
00837
00838 btConvexHullInternal::Int128 btConvexHullInternal::Int128::operator*(int64_t b) const
00839 {
00840 bool negative = (int64_t) high < 0;
00841 Int128 a = negative ? -*this : *this;
00842 if (b < 0)
00843 {
00844 negative = !negative;
00845 b = -b;
00846 }
00847 Int128 result = mul(a.low, (uint64_t) b);
00848 result.high += a.high * (uint64_t) b;
00849 return negative ? -result : result;
00850 }
00851
00852 btConvexHullInternal::Int128 btConvexHullInternal::Int128::mul(int64_t a, int64_t b)
00853 {
00854 Int128 result;
00855
00856 #ifdef USE_X86_64_ASM
00857 __asm__ ("imulq %[b]"
00858 : "=a" (result.low), "=d" (result.high)
00859 : "0"(a), [b] "r"(b)
00860 : "cc" );
00861 return result;
00862
00863 #else
00864 bool negative = a < 0;
00865 if (negative)
00866 {
00867 a = -a;
00868 }
00869 if (b < 0)
00870 {
00871 negative = !negative;
00872 b = -b;
00873 }
00874 DMul<uint64_t, uint32_t>::mul((uint64_t) a, (uint64_t) b, result.low, result.high);
00875 return negative ? -result : result;
00876 #endif
00877 }
00878
00879 btConvexHullInternal::Int128 btConvexHullInternal::Int128::mul(uint64_t a, uint64_t b)
00880 {
00881 Int128 result;
00882
00883 #ifdef USE_X86_64_ASM
00884 __asm__ ("mulq %[b]"
00885 : "=a" (result.low), "=d" (result.high)
00886 : "0"(a), [b] "r"(b)
00887 : "cc" );
00888
00889 #else
00890 DMul<uint64_t, uint32_t>::mul(a, b, result.low, result.high);
00891 #endif
00892
00893 return result;
00894 }
00895
00896 int btConvexHullInternal::Rational64::compare(const Rational64& b) const
00897 {
00898 if (sign != b.sign)
00899 {
00900 return sign - b.sign;
00901 }
00902 else if (sign == 0)
00903 {
00904 return 0;
00905 }
00906
00907
00908
00909 #ifdef USE_X86_64_ASM
00910
00911 int result;
00912 int64_t tmp;
00913 int64_t dummy;
00914 __asm__ ("mulq %[bn]\n\t"
00915 "movq %%rax, %[tmp]\n\t"
00916 "movq %%rdx, %%rbx\n\t"
00917 "movq %[tn], %%rax\n\t"
00918 "mulq %[bd]\n\t"
00919 "subq %[tmp], %%rax\n\t"
00920 "sbbq %%rbx, %%rdx\n\t"
00921 "setnsb %%bh\n\t"
00922 "orq %%rdx, %%rax\n\t"
00923 "setnzb %%bl\n\t"
00924 "decb %%bh\n\t"
00925 "shll $16, %%ebx\n\t"
00926 : "=&b"(result), [tmp] "=&r"(tmp), "=a"(dummy)
00927 : "a"(denominator), [bn] "g"(b.numerator), [tn] "g"(numerator), [bd] "g"(b.denominator)
00928 : "%rdx", "cc" );
00929 return result ? result ^ sign
00930
00931 : 0;
00932
00933 #else
00934
00935 return sign * Int128::mul(m_numerator, b.m_denominator).ucmp(Int128::mul(m_denominator, b.m_numerator));
00936
00937 #endif
00938 }
00939
00940 int btConvexHullInternal::Rational128::compare(const Rational128& b) const
00941 {
00942 if (sign != b.sign)
00943 {
00944 return sign - b.sign;
00945 }
00946 else if (sign == 0)
00947 {
00948 return 0;
00949 }
00950 if (isInt64)
00951 {
00952 return -b.compare(sign * (int64_t) numerator.low);
00953 }
00954
00955 Int128 nbdLow, nbdHigh, dbnLow, dbnHigh;
00956 DMul<Int128, uint64_t>::mul(numerator, b.denominator, nbdLow, nbdHigh);
00957 DMul<Int128, uint64_t>::mul(denominator, b.numerator, dbnLow, dbnHigh);
00958
00959 int cmp = nbdHigh.ucmp(dbnHigh);
00960 if (cmp)
00961 {
00962 return cmp * sign;
00963 }
00964 return nbdLow.ucmp(dbnLow) * sign;
00965 }
00966
00967 int btConvexHullInternal::Rational128::compare(int64_t b) const
00968 {
00969 if (isInt64)
00970 {
00971 int64_t a = sign * (int64_t) numerator.low;
00972 return (a > b) ? 1 : (a < b) ? -1 : 0;
00973 }
00974 if (b > 0)
00975 {
00976 if (sign <= 0)
00977 {
00978 return -1;
00979 }
00980 }
00981 else if (b < 0)
00982 {
00983 if (sign >= 0)
00984 {
00985 return 1;
00986 }
00987 b = -b;
00988 }
00989 else
00990 {
00991 return sign;
00992 }
00993
00994 return numerator.ucmp(denominator * b) * sign;
00995 }
00996
00997
00998 btConvexHullInternal::Edge* btConvexHullInternal::newEdgePair(Vertex* from, Vertex* to)
00999 {
01000 btAssert(from && to);
01001 Edge* e = edgePool.newObject();
01002 Edge* r = edgePool.newObject();
01003 e->reverse = r;
01004 r->reverse = e;
01005 e->copy = mergeStamp;
01006 r->copy = mergeStamp;
01007 e->target = to;
01008 r->target = from;
01009 e->face = NULL;
01010 r->face = NULL;
01011 usedEdgePairs++;
01012 if (usedEdgePairs > maxUsedEdgePairs)
01013 {
01014 maxUsedEdgePairs = usedEdgePairs;
01015 }
01016 return e;
01017 }
01018
01019 bool btConvexHullInternal::mergeProjection(IntermediateHull& h0, IntermediateHull& h1, Vertex*& c0, Vertex*& c1)
01020 {
01021 Vertex* v0 = h0.maxYx;
01022 Vertex* v1 = h1.minYx;
01023 if ((v0->point.x == v1->point.x) && (v0->point.y == v1->point.y))
01024 {
01025 btAssert(v0->point.z < v1->point.z);
01026 Vertex* v1p = v1->prev;
01027 if (v1p == v1)
01028 {
01029 c0 = v0;
01030 if (v1->edges)
01031 {
01032 btAssert(v1->edges->next == v1->edges);
01033 v1 = v1->edges->target;
01034 btAssert(v1->edges->next == v1->edges);
01035 }
01036 c1 = v1;
01037 return false;
01038 }
01039 Vertex* v1n = v1->next;
01040 v1p->next = v1n;
01041 v1n->prev = v1p;
01042 if (v1 == h1.minXy)
01043 {
01044 if ((v1n->point.x < v1p->point.x) || ((v1n->point.x == v1p->point.x) && (v1n->point.y < v1p->point.y)))
01045 {
01046 h1.minXy = v1n;
01047 }
01048 else
01049 {
01050 h1.minXy = v1p;
01051 }
01052 }
01053 if (v1 == h1.maxXy)
01054 {
01055 if ((v1n->point.x > v1p->point.x) || ((v1n->point.x == v1p->point.x) && (v1n->point.y > v1p->point.y)))
01056 {
01057 h1.maxXy = v1n;
01058 }
01059 else
01060 {
01061 h1.maxXy = v1p;
01062 }
01063 }
01064 }
01065
01066 v0 = h0.maxXy;
01067 v1 = h1.maxXy;
01068 Vertex* v00 = NULL;
01069 Vertex* v10 = NULL;
01070 int32_t sign = 1;
01071
01072 for (int side = 0; side <= 1; side++)
01073 {
01074 int32_t dx = (v1->point.x - v0->point.x) * sign;
01075 if (dx > 0)
01076 {
01077 while (true)
01078 {
01079 int32_t dy = v1->point.y - v0->point.y;
01080
01081 Vertex* w0 = side ? v0->next : v0->prev;
01082 if (w0 != v0)
01083 {
01084 int32_t dx0 = (w0->point.x - v0->point.x) * sign;
01085 int32_t dy0 = w0->point.y - v0->point.y;
01086 if ((dy0 <= 0) && ((dx0 == 0) || ((dx0 < 0) && (dy0 * dx <= dy * dx0))))
01087 {
01088 v0 = w0;
01089 dx = (v1->point.x - v0->point.x) * sign;
01090 continue;
01091 }
01092 }
01093
01094 Vertex* w1 = side ? v1->next : v1->prev;
01095 if (w1 != v1)
01096 {
01097 int32_t dx1 = (w1->point.x - v1->point.x) * sign;
01098 int32_t dy1 = w1->point.y - v1->point.y;
01099 int32_t dxn = (w1->point.x - v0->point.x) * sign;
01100 if ((dxn > 0) && (dy1 < 0) && ((dx1 == 0) || ((dx1 < 0) && (dy1 * dx < dy * dx1))))
01101 {
01102 v1 = w1;
01103 dx = dxn;
01104 continue;
01105 }
01106 }
01107
01108 break;
01109 }
01110 }
01111 else if (dx < 0)
01112 {
01113 while (true)
01114 {
01115 int32_t dy = v1->point.y - v0->point.y;
01116
01117 Vertex* w1 = side ? v1->prev : v1->next;
01118 if (w1 != v1)
01119 {
01120 int32_t dx1 = (w1->point.x - v1->point.x) * sign;
01121 int32_t dy1 = w1->point.y - v1->point.y;
01122 if ((dy1 >= 0) && ((dx1 == 0) || ((dx1 < 0) && (dy1 * dx <= dy * dx1))))
01123 {
01124 v1 = w1;
01125 dx = (v1->point.x - v0->point.x) * sign;
01126 continue;
01127 }
01128 }
01129
01130 Vertex* w0 = side ? v0->prev : v0->next;
01131 if (w0 != v0)
01132 {
01133 int32_t dx0 = (w0->point.x - v0->point.x) * sign;
01134 int32_t dy0 = w0->point.y - v0->point.y;
01135 int32_t dxn = (v1->point.x - w0->point.x) * sign;
01136 if ((dxn < 0) && (dy0 > 0) && ((dx0 == 0) || ((dx0 < 0) && (dy0 * dx < dy * dx0))))
01137 {
01138 v0 = w0;
01139 dx = dxn;
01140 continue;
01141 }
01142 }
01143
01144 break;
01145 }
01146 }
01147 else
01148 {
01149 int32_t x = v0->point.x;
01150 int32_t y0 = v0->point.y;
01151 Vertex* w0 = v0;
01152 Vertex* t;
01153 while (((t = side ? w0->next : w0->prev) != v0) && (t->point.x == x) && (t->point.y <= y0))
01154 {
01155 w0 = t;
01156 y0 = t->point.y;
01157 }
01158 v0 = w0;
01159
01160 int32_t y1 = v1->point.y;
01161 Vertex* w1 = v1;
01162 while (((t = side ? w1->prev : w1->next) != v1) && (t->point.x == x) && (t->point.y >= y1))
01163 {
01164 w1 = t;
01165 y1 = t->point.y;
01166 }
01167 v1 = w1;
01168 }
01169
01170 if (side == 0)
01171 {
01172 v00 = v0;
01173 v10 = v1;
01174
01175 v0 = h0.minXy;
01176 v1 = h1.minXy;
01177 sign = -1;
01178 }
01179 }
01180
01181 v0->prev = v1;
01182 v1->next = v0;
01183
01184 v00->next = v10;
01185 v10->prev = v00;
01186
01187 if (h1.minXy->point.x < h0.minXy->point.x)
01188 {
01189 h0.minXy = h1.minXy;
01190 }
01191 if (h1.maxXy->point.x >= h0.maxXy->point.x)
01192 {
01193 h0.maxXy = h1.maxXy;
01194 }
01195
01196 h0.maxYx = h1.maxYx;
01197
01198 c0 = v00;
01199 c1 = v10;
01200
01201 return true;
01202 }
01203
01204 void btConvexHullInternal::computeInternal(int start, int end, IntermediateHull& result)
01205 {
01206 int n = end - start;
01207 switch (n)
01208 {
01209 case 0:
01210 result.minXy = NULL;
01211 result.maxXy = NULL;
01212 result.minYx = NULL;
01213 result.maxYx = NULL;
01214 return;
01215 case 2:
01216 {
01217 Vertex* v = originalVertices[start];
01218 Vertex* w = v + 1;
01219 if (v->point != w->point)
01220 {
01221 int32_t dx = v->point.x - w->point.x;
01222 int32_t dy = v->point.y - w->point.y;
01223
01224 if ((dx == 0) && (dy == 0))
01225 {
01226 if (v->point.z > w->point.z)
01227 {
01228 Vertex* t = w;
01229 w = v;
01230 v = t;
01231 }
01232 btAssert(v->point.z < w->point.z);
01233 v->next = v;
01234 v->prev = v;
01235 result.minXy = v;
01236 result.maxXy = v;
01237 result.minYx = v;
01238 result.maxYx = v;
01239 }
01240 else
01241 {
01242 v->next = w;
01243 v->prev = w;
01244 w->next = v;
01245 w->prev = v;
01246
01247 if ((dx < 0) || ((dx == 0) && (dy < 0)))
01248 {
01249 result.minXy = v;
01250 result.maxXy = w;
01251 }
01252 else
01253 {
01254 result.minXy = w;
01255 result.maxXy = v;
01256 }
01257
01258 if ((dy < 0) || ((dy == 0) && (dx < 0)))
01259 {
01260 result.minYx = v;
01261 result.maxYx = w;
01262 }
01263 else
01264 {
01265 result.minYx = w;
01266 result.maxYx = v;
01267 }
01268 }
01269
01270 Edge* e = newEdgePair(v, w);
01271 e->link(e);
01272 v->edges = e;
01273
01274 e = e->reverse;
01275 e->link(e);
01276 w->edges = e;
01277
01278 return;
01279 }
01280 }
01281
01282 case 1:
01283 {
01284 Vertex* v = originalVertices[start];
01285 v->edges = NULL;
01286 v->next = v;
01287 v->prev = v;
01288
01289 result.minXy = v;
01290 result.maxXy = v;
01291 result.minYx = v;
01292 result.maxYx = v;
01293
01294 return;
01295 }
01296 }
01297
01298 int split0 = start + n / 2;
01299 Point32 p = originalVertices[split0-1]->point;
01300 int split1 = split0;
01301 while ((split1 < end) && (originalVertices[split1]->point == p))
01302 {
01303 split1++;
01304 }
01305 computeInternal(start, split0, result);
01306 IntermediateHull hull1;
01307 computeInternal(split1, end, hull1);
01308 #ifdef DEBUG_CONVEX_HULL
01309 printf("\n\nMerge\n");
01310 result.print();
01311 hull1.print();
01312 #endif
01313 merge(result, hull1);
01314 #ifdef DEBUG_CONVEX_HULL
01315 printf("\n Result\n");
01316 result.print();
01317 #endif
01318 }
01319
01320 #ifdef DEBUG_CONVEX_HULL
01321 void btConvexHullInternal::IntermediateHull::print()
01322 {
01323 printf(" Hull\n");
01324 for (Vertex* v = minXy; v; )
01325 {
01326 printf(" ");
01327 v->print();
01328 if (v == maxXy)
01329 {
01330 printf(" maxXy");
01331 }
01332 if (v == minYx)
01333 {
01334 printf(" minYx");
01335 }
01336 if (v == maxYx)
01337 {
01338 printf(" maxYx");
01339 }
01340 if (v->next->prev != v)
01341 {
01342 printf(" Inconsistency");
01343 }
01344 printf("\n");
01345 v = v->next;
01346 if (v == minXy)
01347 {
01348 break;
01349 }
01350 }
01351 if (minXy)
01352 {
01353 minXy->copy = (minXy->copy == -1) ? -2 : -1;
01354 minXy->printGraph();
01355 }
01356 }
01357
01358 void btConvexHullInternal::Vertex::printGraph()
01359 {
01360 print();
01361 printf("\nEdges\n");
01362 Edge* e = edges;
01363 if (e)
01364 {
01365 do
01366 {
01367 e->print();
01368 printf("\n");
01369 e = e->next;
01370 } while (e != edges);
01371 do
01372 {
01373 Vertex* v = e->target;
01374 if (v->copy != copy)
01375 {
01376 v->copy = copy;
01377 v->printGraph();
01378 }
01379 e = e->next;
01380 } while (e != edges);
01381 }
01382 }
01383 #endif
01384
01385 btConvexHullInternal::Orientation btConvexHullInternal::getOrientation(const Edge* prev, const Edge* next, const Point32& s, const Point32& t)
01386 {
01387 btAssert(prev->reverse->target == next->reverse->target);
01388 if (prev->next == next)
01389 {
01390 if (prev->prev == next)
01391 {
01392 Point64 n = t.cross(s);
01393 Point64 m = (*prev->target - *next->reverse->target).cross(*next->target - *next->reverse->target);
01394 btAssert(!m.isZero());
01395 int64_t dot = n.dot(m);
01396 btAssert(dot != 0);
01397 return (dot > 0) ? COUNTER_CLOCKWISE : CLOCKWISE;
01398 }
01399 return COUNTER_CLOCKWISE;
01400 }
01401 else if (prev->prev == next)
01402 {
01403 return CLOCKWISE;
01404 }
01405 else
01406 {
01407 return NONE;
01408 }
01409 }
01410
01411 btConvexHullInternal::Edge* btConvexHullInternal::findMaxAngle(bool ccw, const Vertex* start, const Point32& s, const Point64& rxs, const Point64& sxrxs, Rational64& minCot)
01412 {
01413 Edge* minEdge = NULL;
01414
01415 #ifdef DEBUG_CONVEX_HULL
01416 printf("find max edge for %d\n", start->point.index);
01417 #endif
01418 Edge* e = start->edges;
01419 if (e)
01420 {
01421 do
01422 {
01423 if (e->copy > mergeStamp)
01424 {
01425 Point32 t = *e->target - *start;
01426 Rational64 cot(t.dot(sxrxs), t.dot(rxs));
01427 #ifdef DEBUG_CONVEX_HULL
01428 printf(" Angle is %f (%d) for ", (float) btAtan(cot.toScalar()), (int) cot.isNaN());
01429 e->print();
01430 #endif
01431 if (cot.isNaN())
01432 {
01433 btAssert(ccw ? (t.dot(s) < 0) : (t.dot(s) > 0));
01434 }
01435 else
01436 {
01437 int cmp;
01438 if (minEdge == NULL)
01439 {
01440 minCot = cot;
01441 minEdge = e;
01442 }
01443 else if ((cmp = cot.compare(minCot)) < 0)
01444 {
01445 minCot = cot;
01446 minEdge = e;
01447 }
01448 else if ((cmp == 0) && (ccw == (getOrientation(minEdge, e, s, t) == COUNTER_CLOCKWISE)))
01449 {
01450 minEdge = e;
01451 }
01452 }
01453 #ifdef DEBUG_CONVEX_HULL
01454 printf("\n");
01455 #endif
01456 }
01457 e = e->next;
01458 } while (e != start->edges);
01459 }
01460 return minEdge;
01461 }
01462
01463 void btConvexHullInternal::findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge*& e0, Edge*& e1, Vertex* stop0, Vertex* stop1)
01464 {
01465 Edge* start0 = e0;
01466 Edge* start1 = e1;
01467 Point32 et0 = start0 ? start0->target->point : c0->point;
01468 Point32 et1 = start1 ? start1->target->point : c1->point;
01469 Point32 s = c1->point - c0->point;
01470 Point64 normal = ((start0 ? start0 : start1)->target->point - c0->point).cross(s);
01471 int64_t dist = c0->point.dot(normal);
01472 btAssert(!start1 || (start1->target->point.dot(normal) == dist));
01473 Point64 perp = s.cross(normal);
01474 btAssert(!perp.isZero());
01475
01476 #ifdef DEBUG_CONVEX_HULL
01477 printf(" Advancing %d %d (%p %p, %d %d)\n", c0->point.index, c1->point.index, start0, start1, start0 ? start0->target->point.index : -1, start1 ? start1->target->point.index : -1);
01478 #endif
01479
01480 int64_t maxDot0 = et0.dot(perp);
01481 if (e0)
01482 {
01483 while (e0->target != stop0)
01484 {
01485 Edge* e = e0->reverse->prev;
01486 if (e->target->point.dot(normal) < dist)
01487 {
01488 break;
01489 }
01490 btAssert(e->target->point.dot(normal) == dist);
01491 if (e->copy == mergeStamp)
01492 {
01493 break;
01494 }
01495 int64_t dot = e->target->point.dot(perp);
01496 if (dot <= maxDot0)
01497 {
01498 break;
01499 }
01500 maxDot0 = dot;
01501 e0 = e;
01502 et0 = e->target->point;
01503 }
01504 }
01505
01506 int64_t maxDot1 = et1.dot(perp);
01507 if (e1)
01508 {
01509 while (e1->target != stop1)
01510 {
01511 Edge* e = e1->reverse->next;
01512 if (e->target->point.dot(normal) < dist)
01513 {
01514 break;
01515 }
01516 btAssert(e->target->point.dot(normal) == dist);
01517 if (e->copy == mergeStamp)
01518 {
01519 break;
01520 }
01521 int64_t dot = e->target->point.dot(perp);
01522 if (dot <= maxDot1)
01523 {
01524 break;
01525 }
01526 maxDot1 = dot;
01527 e1 = e;
01528 et1 = e->target->point;
01529 }
01530 }
01531
01532 #ifdef DEBUG_CONVEX_HULL
01533 printf(" Starting at %d %d\n", et0.index, et1.index);
01534 #endif
01535
01536 int64_t dx = maxDot1 - maxDot0;
01537 if (dx > 0)
01538 {
01539 while (true)
01540 {
01541 int64_t dy = (et1 - et0).dot(s);
01542
01543 if (e0 && (e0->target != stop0))
01544 {
01545 Edge* f0 = e0->next->reverse;
01546 if (f0->copy > mergeStamp)
01547 {
01548 int64_t dx0 = (f0->target->point - et0).dot(perp);
01549 int64_t dy0 = (f0->target->point - et0).dot(s);
01550 if ((dx0 == 0) ? (dy0 < 0) : ((dx0 < 0) && (Rational64(dy0, dx0).compare(Rational64(dy, dx)) >= 0)))
01551 {
01552 et0 = f0->target->point;
01553 dx = (et1 - et0).dot(perp);
01554 e0 = (e0 == start0) ? NULL : f0;
01555 continue;
01556 }
01557 }
01558 }
01559
01560 if (e1 && (e1->target != stop1))
01561 {
01562 Edge* f1 = e1->reverse->next;
01563 if (f1->copy > mergeStamp)
01564 {
01565 Point32 d1 = f1->target->point - et1;
01566 if (d1.dot(normal) == 0)
01567 {
01568 int64_t dx1 = d1.dot(perp);
01569 int64_t dy1 = d1.dot(s);
01570 int64_t dxn = (f1->target->point - et0).dot(perp);
01571 if ((dxn > 0) && ((dx1 == 0) ? (dy1 < 0) : ((dx1 < 0) && (Rational64(dy1, dx1).compare(Rational64(dy, dx)) > 0))))
01572 {
01573 e1 = f1;
01574 et1 = e1->target->point;
01575 dx = dxn;
01576 continue;
01577 }
01578 }
01579 else
01580 {
01581 btAssert((e1 == start1) && (d1.dot(normal) < 0));
01582 }
01583 }
01584 }
01585
01586 break;
01587 }
01588 }
01589 else if (dx < 0)
01590 {
01591 while (true)
01592 {
01593 int64_t dy = (et1 - et0).dot(s);
01594
01595 if (e1 && (e1->target != stop1))
01596 {
01597 Edge* f1 = e1->prev->reverse;
01598 if (f1->copy > mergeStamp)
01599 {
01600 int64_t dx1 = (f1->target->point - et1).dot(perp);
01601 int64_t dy1 = (f1->target->point - et1).dot(s);
01602 if ((dx1 == 0) ? (dy1 > 0) : ((dx1 < 0) && (Rational64(dy1, dx1).compare(Rational64(dy, dx)) <= 0)))
01603 {
01604 et1 = f1->target->point;
01605 dx = (et1 - et0).dot(perp);
01606 e1 = (e1 == start1) ? NULL : f1;
01607 continue;
01608 }
01609 }
01610 }
01611
01612 if (e0 && (e0->target != stop0))
01613 {
01614 Edge* f0 = e0->reverse->prev;
01615 if (f0->copy > mergeStamp)
01616 {
01617 Point32 d0 = f0->target->point - et0;
01618 if (d0.dot(normal) == 0)
01619 {
01620 int64_t dx0 = d0.dot(perp);
01621 int64_t dy0 = d0.dot(s);
01622 int64_t dxn = (et1 - f0->target->point).dot(perp);
01623 if ((dxn < 0) && ((dx0 == 0) ? (dy0 > 0) : ((dx0 < 0) && (Rational64(dy0, dx0).compare(Rational64(dy, dx)) < 0))))
01624 {
01625 e0 = f0;
01626 et0 = e0->target->point;
01627 dx = dxn;
01628 continue;
01629 }
01630 }
01631 else
01632 {
01633 btAssert((e0 == start0) && (d0.dot(normal) < 0));
01634 }
01635 }
01636 }
01637
01638 break;
01639 }
01640 }
01641 #ifdef DEBUG_CONVEX_HULL
01642 printf(" Advanced edges to %d %d\n", et0.index, et1.index);
01643 #endif
01644 }
01645
01646
01647 void btConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1)
01648 {
01649 if (!h1.maxXy)
01650 {
01651 return;
01652 }
01653 if (!h0.maxXy)
01654 {
01655 h0 = h1;
01656 return;
01657 }
01658
01659 mergeStamp--;
01660
01661 Vertex* c0 = NULL;
01662 Edge* toPrev0 = NULL;
01663 Edge* firstNew0 = NULL;
01664 Edge* pendingHead0 = NULL;
01665 Edge* pendingTail0 = NULL;
01666 Vertex* c1 = NULL;
01667 Edge* toPrev1 = NULL;
01668 Edge* firstNew1 = NULL;
01669 Edge* pendingHead1 = NULL;
01670 Edge* pendingTail1 = NULL;
01671 Point32 prevPoint;
01672
01673 if (mergeProjection(h0, h1, c0, c1))
01674 {
01675 Point32 s = *c1 - *c0;
01676 Point64 normal = Point32(0, 0, -1).cross(s);
01677 Point64 t = s.cross(normal);
01678 btAssert(!t.isZero());
01679
01680 Edge* e = c0->edges;
01681 Edge* start0 = NULL;
01682 if (e)
01683 {
01684 do
01685 {
01686 int64_t dot = (*e->target - *c0).dot(normal);
01687 btAssert(dot <= 0);
01688 if ((dot == 0) && ((*e->target - *c0).dot(t) > 0))
01689 {
01690 if (!start0 || (getOrientation(start0, e, s, Point32(0, 0, -1)) == CLOCKWISE))
01691 {
01692 start0 = e;
01693 }
01694 }
01695 e = e->next;
01696 } while (e != c0->edges);
01697 }
01698
01699 e = c1->edges;
01700 Edge* start1 = NULL;
01701 if (e)
01702 {
01703 do
01704 {
01705 int64_t dot = (*e->target - *c1).dot(normal);
01706 btAssert(dot <= 0);
01707 if ((dot == 0) && ((*e->target - *c1).dot(t) > 0))
01708 {
01709 if (!start1 || (getOrientation(start1, e, s, Point32(0, 0, -1)) == COUNTER_CLOCKWISE))
01710 {
01711 start1 = e;
01712 }
01713 }
01714 e = e->next;
01715 } while (e != c1->edges);
01716 }
01717
01718 if (start0 || start1)
01719 {
01720 findEdgeForCoplanarFaces(c0, c1, start0, start1, NULL, NULL);
01721 if (start0)
01722 {
01723 c0 = start0->target;
01724 }
01725 if (start1)
01726 {
01727 c1 = start1->target;
01728 }
01729 }
01730
01731 prevPoint = c1->point;
01732 prevPoint.z++;
01733 }
01734 else
01735 {
01736 prevPoint = c1->point;
01737 prevPoint.x++;
01738 }
01739
01740 Vertex* first0 = c0;
01741 Vertex* first1 = c1;
01742 bool firstRun = true;
01743
01744 while (true)
01745 {
01746 Point32 s = *c1 - *c0;
01747 Point32 r = prevPoint - c0->point;
01748 Point64 rxs = r.cross(s);
01749 Point64 sxrxs = s.cross(rxs);
01750
01751 #ifdef DEBUG_CONVEX_HULL
01752 printf("\n Checking %d %d\n", c0->point.index, c1->point.index);
01753 #endif
01754 Rational64 minCot0(0, 0);
01755 Edge* min0 = findMaxAngle(false, c0, s, rxs, sxrxs, minCot0);
01756 Rational64 minCot1(0, 0);
01757 Edge* min1 = findMaxAngle(true, c1, s, rxs, sxrxs, minCot1);
01758 if (!min0 && !min1)
01759 {
01760 Edge* e = newEdgePair(c0, c1);
01761 e->link(e);
01762 c0->edges = e;
01763
01764 e = e->reverse;
01765 e->link(e);
01766 c1->edges = e;
01767 return;
01768 }
01769 else
01770 {
01771 int cmp = !min0 ? 1 : !min1 ? -1 : minCot0.compare(minCot1);
01772 #ifdef DEBUG_CONVEX_HULL
01773 printf(" -> Result %d\n", cmp);
01774 #endif
01775 if (firstRun || ((cmp >= 0) ? !minCot1.isNegativeInfinity() : !minCot0.isNegativeInfinity()))
01776 {
01777 Edge* e = newEdgePair(c0, c1);
01778 if (pendingTail0)
01779 {
01780 pendingTail0->prev = e;
01781 }
01782 else
01783 {
01784 pendingHead0 = e;
01785 }
01786 e->next = pendingTail0;
01787 pendingTail0 = e;
01788
01789 e = e->reverse;
01790 if (pendingTail1)
01791 {
01792 pendingTail1->next = e;
01793 }
01794 else
01795 {
01796 pendingHead1 = e;
01797 }
01798 e->prev = pendingTail1;
01799 pendingTail1 = e;
01800 }
01801
01802 Edge* e0 = min0;
01803 Edge* e1 = min1;
01804
01805 #ifdef DEBUG_CONVEX_HULL
01806 printf(" Found min edges to %d %d\n", e0 ? e0->target->point.index : -1, e1 ? e1->target->point.index : -1);
01807 #endif
01808
01809 if (cmp == 0)
01810 {
01811 findEdgeForCoplanarFaces(c0, c1, e0, e1, NULL, NULL);
01812 }
01813
01814 if ((cmp >= 0) && e1)
01815 {
01816 if (toPrev1)
01817 {
01818 for (Edge* e = toPrev1->next, *n = NULL; e != min1; e = n)
01819 {
01820 n = e->next;
01821 removeEdgePair(e);
01822 }
01823 }
01824
01825 if (pendingTail1)
01826 {
01827 if (toPrev1)
01828 {
01829 toPrev1->link(pendingHead1);
01830 }
01831 else
01832 {
01833 min1->prev->link(pendingHead1);
01834 firstNew1 = pendingHead1;
01835 }
01836 pendingTail1->link(min1);
01837 pendingHead1 = NULL;
01838 pendingTail1 = NULL;
01839 }
01840 else if (!toPrev1)
01841 {
01842 firstNew1 = min1;
01843 }
01844
01845 prevPoint = c1->point;
01846 c1 = e1->target;
01847 toPrev1 = e1->reverse;
01848 }
01849
01850 if ((cmp <= 0) && e0)
01851 {
01852 if (toPrev0)
01853 {
01854 for (Edge* e = toPrev0->prev, *n = NULL; e != min0; e = n)
01855 {
01856 n = e->prev;
01857 removeEdgePair(e);
01858 }
01859 }
01860
01861 if (pendingTail0)
01862 {
01863 if (toPrev0)
01864 {
01865 pendingHead0->link(toPrev0);
01866 }
01867 else
01868 {
01869 pendingHead0->link(min0->next);
01870 firstNew0 = pendingHead0;
01871 }
01872 min0->link(pendingTail0);
01873 pendingHead0 = NULL;
01874 pendingTail0 = NULL;
01875 }
01876 else if (!toPrev0)
01877 {
01878 firstNew0 = min0;
01879 }
01880
01881 prevPoint = c0->point;
01882 c0 = e0->target;
01883 toPrev0 = e0->reverse;
01884 }
01885 }
01886
01887 if ((c0 == first0) && (c1 == first1))
01888 {
01889 if (toPrev0 == NULL)
01890 {
01891 pendingHead0->link(pendingTail0);
01892 c0->edges = pendingTail0;
01893 }
01894 else
01895 {
01896 for (Edge* e = toPrev0->prev, *n = NULL; e != firstNew0; e = n)
01897 {
01898 n = e->prev;
01899 removeEdgePair(e);
01900 }
01901 if (pendingTail0)
01902 {
01903 pendingHead0->link(toPrev0);
01904 firstNew0->link(pendingTail0);
01905 }
01906 }
01907
01908 if (toPrev1 == NULL)
01909 {
01910 pendingTail1->link(pendingHead1);
01911 c1->edges = pendingTail1;
01912 }
01913 else
01914 {
01915 for (Edge* e = toPrev1->next, *n = NULL; e != firstNew1; e = n)
01916 {
01917 n = e->next;
01918 removeEdgePair(e);
01919 }
01920 if (pendingTail1)
01921 {
01922 toPrev1->link(pendingHead1);
01923 pendingTail1->link(firstNew1);
01924 }
01925 }
01926
01927 return;
01928 }
01929
01930 firstRun = false;
01931 }
01932 }
01933
01934
01935 static bool pointCmp(const btConvexHullInternal::Point32& p, const btConvexHullInternal::Point32& q)
01936 {
01937 return (p.y < q.y) || ((p.y == q.y) && ((p.x < q.x) || ((p.x == q.x) && (p.z < q.z))));
01938 }
01939
01940 void btConvexHullInternal::compute(const void* coords, bool doubleCoords, int stride, int count)
01941 {
01942 btVector3 min(btScalar(1e30), btScalar(1e30), btScalar(1e30)), max(btScalar(-1e30), btScalar(-1e30), btScalar(-1e30));
01943 const char* ptr = (const char*) coords;
01944 if (doubleCoords)
01945 {
01946 for (int i = 0; i < count; i++)
01947 {
01948 const double* v = (const double*) ptr;
01949 btVector3 p((btScalar) v[0], (btScalar) v[1], (btScalar) v[2]);
01950 ptr += stride;
01951 min.setMin(p);
01952 max.setMax(p);
01953 }
01954 }
01955 else
01956 {
01957 for (int i = 0; i < count; i++)
01958 {
01959 const float* v = (const float*) ptr;
01960 btVector3 p(v[0], v[1], v[2]);
01961 ptr += stride;
01962 min.setMin(p);
01963 max.setMax(p);
01964 }
01965 }
01966
01967 btVector3 s = max - min;
01968 maxAxis = s.maxAxis();
01969 minAxis = s.minAxis();
01970 if (minAxis == maxAxis)
01971 {
01972 minAxis = (maxAxis + 1) % 3;
01973 }
01974 medAxis = 3 - maxAxis - minAxis;
01975
01976 s /= btScalar(10216);
01977 if (((medAxis + 1) % 3) != maxAxis)
01978 {
01979 s *= -1;
01980 }
01981 scaling = s;
01982
01983 if (s[0] != 0)
01984 {
01985 s[0] = btScalar(1) / s[0];
01986 }
01987 if (s[1] != 0)
01988 {
01989 s[1] = btScalar(1) / s[1];
01990 }
01991 if (s[2] != 0)
01992 {
01993 s[2] = btScalar(1) / s[2];
01994 }
01995
01996 center = (min + max) * btScalar(0.5);
01997
01998 btAlignedObjectArray<Point32> points;
01999 points.resize(count);
02000 ptr = (const char*) coords;
02001 if (doubleCoords)
02002 {
02003 for (int i = 0; i < count; i++)
02004 {
02005 const double* v = (const double*) ptr;
02006 btVector3 p((btScalar) v[0], (btScalar) v[1], (btScalar) v[2]);
02007 ptr += stride;
02008 p = (p - center) * s;
02009 points[i].x = (int32_t) p[medAxis];
02010 points[i].y = (int32_t) p[maxAxis];
02011 points[i].z = (int32_t) p[minAxis];
02012 points[i].index = i;
02013 }
02014 }
02015 else
02016 {
02017 for (int i = 0; i < count; i++)
02018 {
02019 const float* v = (const float*) ptr;
02020 btVector3 p(v[0], v[1], v[2]);
02021 ptr += stride;
02022 p = (p - center) * s;
02023 points[i].x = (int32_t) p[medAxis];
02024 points[i].y = (int32_t) p[maxAxis];
02025 points[i].z = (int32_t) p[minAxis];
02026 points[i].index = i;
02027 }
02028 }
02029 points.quickSort(pointCmp);
02030
02031 vertexPool.reset();
02032 vertexPool.setArraySize(count);
02033 originalVertices.resize(count);
02034 for (int i = 0; i < count; i++)
02035 {
02036 Vertex* v = vertexPool.newObject();
02037 v->edges = NULL;
02038 v->point = points[i];
02039 v->copy = -1;
02040 originalVertices[i] = v;
02041 }
02042
02043 points.clear();
02044
02045 edgePool.reset();
02046 edgePool.setArraySize(6 * count);
02047
02048 usedEdgePairs = 0;
02049 maxUsedEdgePairs = 0;
02050
02051 mergeStamp = -3;
02052
02053 IntermediateHull hull;
02054 computeInternal(0, count, hull);
02055 vertexList = hull.minXy;
02056 #ifdef DEBUG_CONVEX_HULL
02057 printf("max. edges %d (3v = %d)", maxUsedEdgePairs, 3 * count);
02058 #endif
02059 }
02060
02061 btVector3 btConvexHullInternal::toBtVector(const Point32& v)
02062 {
02063 btVector3 p;
02064 p[medAxis] = btScalar(v.x);
02065 p[maxAxis] = btScalar(v.y);
02066 p[minAxis] = btScalar(v.z);
02067 return p * scaling;
02068 }
02069
02070 btVector3 btConvexHullInternal::getBtNormal(Face* face)
02071 {
02072 return toBtVector(face->dir0).cross(toBtVector(face->dir1)).normalized();
02073 }
02074
02075 btVector3 btConvexHullInternal::getCoordinates(const Vertex* v)
02076 {
02077 btVector3 p;
02078 p[medAxis] = v->xvalue();
02079 p[maxAxis] = v->yvalue();
02080 p[minAxis] = v->zvalue();
02081 return p * scaling + center;
02082 }
02083
02084 btScalar btConvexHullInternal::shrink(btScalar amount, btScalar clampAmount)
02085 {
02086 if (!vertexList)
02087 {
02088 return 0;
02089 }
02090 int stamp = --mergeStamp;
02091 btAlignedObjectArray<Vertex*> stack;
02092 vertexList->copy = stamp;
02093 stack.push_back(vertexList);
02094 btAlignedObjectArray<Face*> faces;
02095
02096 Point32 ref = vertexList->point;
02097 Int128 hullCenterX(0, 0);
02098 Int128 hullCenterY(0, 0);
02099 Int128 hullCenterZ(0, 0);
02100 Int128 volume(0, 0);
02101
02102 while (stack.size() > 0)
02103 {
02104 Vertex* v = stack[stack.size() - 1];
02105 stack.pop_back();
02106 Edge* e = v->edges;
02107 if (e)
02108 {
02109 do
02110 {
02111 if (e->target->copy != stamp)
02112 {
02113 e->target->copy = stamp;
02114 stack.push_back(e->target);
02115 }
02116 if (e->copy != stamp)
02117 {
02118 Face* face = facePool.newObject();
02119 face->init(e->target, e->reverse->prev->target, v);
02120 faces.push_back(face);
02121 Edge* f = e;
02122
02123 Vertex* a = NULL;
02124 Vertex* b = NULL;
02125 do
02126 {
02127 if (a && b)
02128 {
02129 int64_t vol = (v->point - ref).dot((a->point - ref).cross(b->point - ref));
02130 btAssert(vol >= 0);
02131 Point32 c = v->point + a->point + b->point + ref;
02132 hullCenterX += vol * c.x;
02133 hullCenterY += vol * c.y;
02134 hullCenterZ += vol * c.z;
02135 volume += vol;
02136 }
02137
02138 btAssert(f->copy != stamp);
02139 f->copy = stamp;
02140 f->face = face;
02141
02142 a = b;
02143 b = f->target;
02144
02145 f = f->reverse->prev;
02146 } while (f != e);
02147 }
02148 e = e->next;
02149 } while (e != v->edges);
02150 }
02151 }
02152
02153 if (volume.getSign() <= 0)
02154 {
02155 return 0;
02156 }
02157
02158 btVector3 hullCenter;
02159 hullCenter[medAxis] = hullCenterX.toScalar();
02160 hullCenter[maxAxis] = hullCenterY.toScalar();
02161 hullCenter[minAxis] = hullCenterZ.toScalar();
02162 hullCenter /= 4 * volume.toScalar();
02163 hullCenter *= scaling;
02164
02165 int faceCount = faces.size();
02166
02167 if (clampAmount > 0)
02168 {
02169 btScalar minDist = SIMD_INFINITY;
02170 for (int i = 0; i < faceCount; i++)
02171 {
02172 btVector3 normal = getBtNormal(faces[i]);
02173 btScalar dist = normal.dot(toBtVector(faces[i]->origin) - hullCenter);
02174 if (dist < minDist)
02175 {
02176 minDist = dist;
02177 }
02178 }
02179
02180 if (minDist <= 0)
02181 {
02182 return 0;
02183 }
02184
02185 amount = btMin(amount, minDist * clampAmount);
02186 }
02187
02188 unsigned int seed = 243703;
02189 for (int i = 0; i < faceCount; i++, seed = 1664525 * seed + 1013904223)
02190 {
02191 btSwap(faces[i], faces[seed % faceCount]);
02192 }
02193
02194 for (int i = 0; i < faceCount; i++)
02195 {
02196 if (!shiftFace(faces[i], amount, stack))
02197 {
02198 return -amount;
02199 }
02200 }
02201
02202 return amount;
02203 }
02204
02205 bool btConvexHullInternal::shiftFace(Face* face, btScalar amount, btAlignedObjectArray<Vertex*> stack)
02206 {
02207 btVector3 origShift = getBtNormal(face) * -amount;
02208 if (scaling[0] != 0)
02209 {
02210 origShift[0] /= scaling[0];
02211 }
02212 if (scaling[1] != 0)
02213 {
02214 origShift[1] /= scaling[1];
02215 }
02216 if (scaling[2] != 0)
02217 {
02218 origShift[2] /= scaling[2];
02219 }
02220 Point32 shift((int32_t) origShift[medAxis], (int32_t) origShift[maxAxis], (int32_t) origShift[minAxis]);
02221 if (shift.isZero())
02222 {
02223 return true;
02224 }
02225 Point64 normal = face->getNormal();
02226 #ifdef DEBUG_CONVEX_HULL
02227 printf("\nShrinking face (%d %d %d) (%d %d %d) (%d %d %d) by (%d %d %d)\n",
02228 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);
02229 #endif
02230 int64_t origDot = face->origin.dot(normal);
02231 Point32 shiftedOrigin = face->origin + shift;
02232 int64_t shiftedDot = shiftedOrigin.dot(normal);
02233 btAssert(shiftedDot <= origDot);
02234 if (shiftedDot >= origDot)
02235 {
02236 return false;
02237 }
02238
02239 Edge* intersection = NULL;
02240
02241 Edge* startEdge = face->nearbyVertex->edges;
02242 #ifdef DEBUG_CONVEX_HULL
02243 printf("Start edge is ");
02244 startEdge->print();
02245 printf(", normal is (%lld %lld %lld), shifted dot is %lld\n", normal.x, normal.y, normal.z, shiftedDot);
02246 #endif
02247 Rational128 optDot = face->nearbyVertex->dot(normal);
02248 int cmp = optDot.compare(shiftedDot);
02249 #ifdef SHOW_ITERATIONS
02250 int n = 0;
02251 #endif
02252 if (cmp >= 0)
02253 {
02254 Edge* e = startEdge;
02255 do
02256 {
02257 #ifdef SHOW_ITERATIONS
02258 n++;
02259 #endif
02260 Rational128 dot = e->target->dot(normal);
02261 btAssert(dot.compare(origDot) <= 0);
02262 #ifdef DEBUG_CONVEX_HULL
02263 printf("Moving downwards, edge is ");
02264 e->print();
02265 printf(", dot is %f (%f %lld)\n", (float) dot.toScalar(), (float) optDot.toScalar(), shiftedDot);
02266 #endif
02267 if (dot.compare(optDot) < 0)
02268 {
02269 int c = dot.compare(shiftedDot);
02270 optDot = dot;
02271 e = e->reverse;
02272 startEdge = e;
02273 if (c < 0)
02274 {
02275 intersection = e;
02276 break;
02277 }
02278 cmp = c;
02279 }
02280 e = e->prev;
02281 } while (e != startEdge);
02282
02283 if (!intersection)
02284 {
02285 return false;
02286 }
02287 }
02288 else
02289 {
02290 Edge* e = startEdge;
02291 do
02292 {
02293 #ifdef SHOW_ITERATIONS
02294 n++;
02295 #endif
02296 Rational128 dot = e->target->dot(normal);
02297 btAssert(dot.compare(origDot) <= 0);
02298 #ifdef DEBUG_CONVEX_HULL
02299 printf("Moving upwards, edge is ");
02300 e->print();
02301 printf(", dot is %f (%f %lld)\n", (float) dot.toScalar(), (float) optDot.toScalar(), shiftedDot);
02302 #endif
02303 if (dot.compare(optDot) > 0)
02304 {
02305 cmp = dot.compare(shiftedDot);
02306 if (cmp >= 0)
02307 {
02308 intersection = e;
02309 break;
02310 }
02311 optDot = dot;
02312 e = e->reverse;
02313 startEdge = e;
02314 }
02315 e = e->prev;
02316 } while (e != startEdge);
02317
02318 if (!intersection)
02319 {
02320 return true;
02321 }
02322 }
02323
02324 #ifdef SHOW_ITERATIONS
02325 printf("Needed %d iterations to find initial intersection\n", n);
02326 #endif
02327
02328 if (cmp == 0)
02329 {
02330 Edge* e = intersection->reverse->next;
02331 #ifdef SHOW_ITERATIONS
02332 n = 0;
02333 #endif
02334 while (e->target->dot(normal).compare(shiftedDot) <= 0)
02335 {
02336 #ifdef SHOW_ITERATIONS
02337 n++;
02338 #endif
02339 e = e->next;
02340 if (e == intersection->reverse)
02341 {
02342 return true;
02343 }
02344 #ifdef DEBUG_CONVEX_HULL
02345 printf("Checking for outwards edge, current edge is ");
02346 e->print();
02347 printf("\n");
02348 #endif
02349 }
02350 #ifdef SHOW_ITERATIONS
02351 printf("Needed %d iterations to check for complete containment\n", n);
02352 #endif
02353 }
02354
02355 Edge* firstIntersection = NULL;
02356 Edge* faceEdge = NULL;
02357 Edge* firstFaceEdge = NULL;
02358
02359 #ifdef SHOW_ITERATIONS
02360 int m = 0;
02361 #endif
02362 while (true)
02363 {
02364 #ifdef SHOW_ITERATIONS
02365 m++;
02366 #endif
02367 #ifdef DEBUG_CONVEX_HULL
02368 printf("Intersecting edge is ");
02369 intersection->print();
02370 printf("\n");
02371 #endif
02372 if (cmp == 0)
02373 {
02374 Edge* e = intersection->reverse->next;
02375 startEdge = e;
02376 #ifdef SHOW_ITERATIONS
02377 n = 0;
02378 #endif
02379 while (true)
02380 {
02381 #ifdef SHOW_ITERATIONS
02382 n++;
02383 #endif
02384 if (e->target->dot(normal).compare(shiftedDot) >= 0)
02385 {
02386 break;
02387 }
02388 intersection = e->reverse;
02389 e = e->next;
02390 if (e == startEdge)
02391 {
02392 return true;
02393 }
02394 }
02395 #ifdef SHOW_ITERATIONS
02396 printf("Needed %d iterations to advance intersection\n", n);
02397 #endif
02398 }
02399
02400 #ifdef DEBUG_CONVEX_HULL
02401 printf("Advanced intersecting edge to ");
02402 intersection->print();
02403 printf(", cmp = %d\n", cmp);
02404 #endif
02405
02406 if (!firstIntersection)
02407 {
02408 firstIntersection = intersection;
02409 }
02410 else if (intersection == firstIntersection)
02411 {
02412 break;
02413 }
02414
02415 int prevCmp = cmp;
02416 Edge* prevIntersection = intersection;
02417 Edge* prevFaceEdge = faceEdge;
02418
02419 Edge* e = intersection->reverse;
02420 #ifdef SHOW_ITERATIONS
02421 n = 0;
02422 #endif
02423 while (true)
02424 {
02425 #ifdef SHOW_ITERATIONS
02426 n++;
02427 #endif
02428 e = e->reverse->prev;
02429 btAssert(e != intersection->reverse);
02430 cmp = e->target->dot(normal).compare(shiftedDot);
02431 #ifdef DEBUG_CONVEX_HULL
02432 printf("Testing edge ");
02433 e->print();
02434 printf(" -> cmp = %d\n", cmp);
02435 #endif
02436 if (cmp >= 0)
02437 {
02438 intersection = e;
02439 break;
02440 }
02441 }
02442 #ifdef SHOW_ITERATIONS
02443 printf("Needed %d iterations to find other intersection of face\n", n);
02444 #endif
02445
02446 if (cmp > 0)
02447 {
02448 Vertex* removed = intersection->target;
02449 e = intersection->reverse;
02450 if (e->prev == e)
02451 {
02452 removed->edges = NULL;
02453 }
02454 else
02455 {
02456 removed->edges = e->prev;
02457 e->prev->link(e->next);
02458 e->link(e);
02459 }
02460 #ifdef DEBUG_CONVEX_HULL
02461 printf("1: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z);
02462 #endif
02463
02464 Point64 n0 = intersection->face->getNormal();
02465 Point64 n1 = intersection->reverse->face->getNormal();
02466 int64_t m00 = face->dir0.dot(n0);
02467 int64_t m01 = face->dir1.dot(n0);
02468 int64_t m10 = face->dir0.dot(n1);
02469 int64_t m11 = face->dir1.dot(n1);
02470 int64_t r0 = (intersection->face->origin - shiftedOrigin).dot(n0);
02471 int64_t r1 = (intersection->reverse->face->origin - shiftedOrigin).dot(n1);
02472 Int128 det = Int128::mul(m00, m11) - Int128::mul(m01, m10);
02473 btAssert(det.getSign() != 0);
02474 Vertex* v = vertexPool.newObject();
02475 v->point.index = -1;
02476 v->copy = -1;
02477 v->point128 = PointR128(Int128::mul(face->dir0.x * r0, m11) - Int128::mul(face->dir0.x * r1, m01)
02478 + Int128::mul(face->dir1.x * r1, m00) - Int128::mul(face->dir1.x * r0, m10) + det * shiftedOrigin.x,
02479 Int128::mul(face->dir0.y * r0, m11) - Int128::mul(face->dir0.y * r1, m01)
02480 + Int128::mul(face->dir1.y * r1, m00) - Int128::mul(face->dir1.y * r0, m10) + det * shiftedOrigin.y,
02481 Int128::mul(face->dir0.z * r0, m11) - Int128::mul(face->dir0.z * r1, m01)
02482 + Int128::mul(face->dir1.z * r1, m00) - Int128::mul(face->dir1.z * r0, m10) + det * shiftedOrigin.z,
02483 det);
02484 v->point.x = (int32_t) v->point128.xvalue();
02485 v->point.y = (int32_t) v->point128.yvalue();
02486 v->point.z = (int32_t) v->point128.zvalue();
02487 intersection->target = v;
02488 v->edges = e;
02489
02490 stack.push_back(v);
02491 stack.push_back(removed);
02492 stack.push_back(NULL);
02493 }
02494
02495 if (cmp || prevCmp || (prevIntersection->reverse->next->target != intersection->target))
02496 {
02497 faceEdge = newEdgePair(prevIntersection->target, intersection->target);
02498 if (prevCmp == 0)
02499 {
02500 faceEdge->link(prevIntersection->reverse->next);
02501 }
02502 if ((prevCmp == 0) || prevFaceEdge)
02503 {
02504 prevIntersection->reverse->link(faceEdge);
02505 }
02506 if (cmp == 0)
02507 {
02508 intersection->reverse->prev->link(faceEdge->reverse);
02509 }
02510 faceEdge->reverse->link(intersection->reverse);
02511 }
02512 else
02513 {
02514 faceEdge = prevIntersection->reverse->next;
02515 }
02516
02517 if (prevFaceEdge)
02518 {
02519 if (prevCmp > 0)
02520 {
02521 faceEdge->link(prevFaceEdge->reverse);
02522 }
02523 else if (faceEdge != prevFaceEdge->reverse)
02524 {
02525 stack.push_back(prevFaceEdge->target);
02526 while (faceEdge->next != prevFaceEdge->reverse)
02527 {
02528 Vertex* removed = faceEdge->next->target;
02529 removeEdgePair(faceEdge->next);
02530 stack.push_back(removed);
02531 #ifdef DEBUG_CONVEX_HULL
02532 printf("2: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z);
02533 #endif
02534 }
02535 stack.push_back(NULL);
02536 }
02537 }
02538 faceEdge->face = face;
02539 faceEdge->reverse->face = intersection->face;
02540
02541 if (!firstFaceEdge)
02542 {
02543 firstFaceEdge = faceEdge;
02544 }
02545 }
02546 #ifdef SHOW_ITERATIONS
02547 printf("Needed %d iterations to process all intersections\n", m);
02548 #endif
02549
02550 if (cmp > 0)
02551 {
02552 firstFaceEdge->reverse->target = faceEdge->target;
02553 firstIntersection->reverse->link(firstFaceEdge);
02554 firstFaceEdge->link(faceEdge->reverse);
02555 }
02556 else if (firstFaceEdge != faceEdge->reverse)
02557 {
02558 stack.push_back(faceEdge->target);
02559 while (firstFaceEdge->next != faceEdge->reverse)
02560 {
02561 Vertex* removed = firstFaceEdge->next->target;
02562 removeEdgePair(firstFaceEdge->next);
02563 stack.push_back(removed);
02564 #ifdef DEBUG_CONVEX_HULL
02565 printf("3: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z);
02566 #endif
02567 }
02568 stack.push_back(NULL);
02569 }
02570
02571 btAssert(stack.size() > 0);
02572 vertexList = stack[0];
02573
02574 #ifdef DEBUG_CONVEX_HULL
02575 printf("Removing part\n");
02576 #endif
02577 #ifdef SHOW_ITERATIONS
02578 n = 0;
02579 #endif
02580 int pos = 0;
02581 while (pos < stack.size())
02582 {
02583 int end = stack.size();
02584 while (pos < end)
02585 {
02586 Vertex* kept = stack[pos++];
02587 #ifdef DEBUG_CONVEX_HULL
02588 kept->print();
02589 #endif
02590 bool deeper = false;
02591 Vertex* removed;
02592 while ((removed = stack[pos++]) != NULL)
02593 {
02594 #ifdef SHOW_ITERATIONS
02595 n++;
02596 #endif
02597 kept->receiveNearbyFaces(removed);
02598 while (removed->edges)
02599 {
02600 if (!deeper)
02601 {
02602 deeper = true;
02603 stack.push_back(kept);
02604 }
02605 stack.push_back(removed->edges->target);
02606 removeEdgePair(removed->edges);
02607 }
02608 }
02609 if (deeper)
02610 {
02611 stack.push_back(NULL);
02612 }
02613 }
02614 }
02615 #ifdef SHOW_ITERATIONS
02616 printf("Needed %d iterations to remove part\n", n);
02617 #endif
02618
02619 stack.resize(0);
02620 face->origin = shiftedOrigin;
02621
02622 return true;
02623 }
02624
02625
02626 static int getVertexCopy(btConvexHullInternal::Vertex* vertex, btAlignedObjectArray<btConvexHullInternal::Vertex*>& vertices)
02627 {
02628 int index = vertex->copy;
02629 if (index < 0)
02630 {
02631 index = vertices.size();
02632 vertex->copy = index;
02633 vertices.push_back(vertex);
02634 #ifdef DEBUG_CONVEX_HULL
02635 printf("Vertex %d gets index *%d\n", vertex->point.index, index);
02636 #endif
02637 }
02638 return index;
02639 }
02640
02641 btScalar btConvexHullComputer::compute(const void* coords, bool doubleCoords, int stride, int count, btScalar shrink, btScalar shrinkClamp)
02642 {
02643 if (count <= 0)
02644 {
02645 vertices.clear();
02646 edges.clear();
02647 faces.clear();
02648 return 0;
02649 }
02650
02651 btConvexHullInternal hull;
02652 hull.compute(coords, doubleCoords, stride, count);
02653
02654 btScalar shift = 0;
02655 if ((shrink > 0) && ((shift = hull.shrink(shrink, shrinkClamp)) < 0))
02656 {
02657 vertices.clear();
02658 edges.clear();
02659 faces.clear();
02660 return shift;
02661 }
02662
02663 vertices.resize(0);
02664 edges.resize(0);
02665 faces.resize(0);
02666
02667 btAlignedObjectArray<btConvexHullInternal::Vertex*> oldVertices;
02668 getVertexCopy(hull.vertexList, oldVertices);
02669 int copied = 0;
02670 while (copied < oldVertices.size())
02671 {
02672 btConvexHullInternal::Vertex* v = oldVertices[copied];
02673 vertices.push_back(hull.getCoordinates(v));
02674 btConvexHullInternal::Edge* firstEdge = v->edges;
02675 if (firstEdge)
02676 {
02677 int firstCopy = -1;
02678 int prevCopy = -1;
02679 btConvexHullInternal::Edge* e = firstEdge;
02680 do
02681 {
02682 if (e->copy < 0)
02683 {
02684 int s = edges.size();
02685 edges.push_back(Edge());
02686 edges.push_back(Edge());
02687 Edge* c = &edges[s];
02688 Edge* r = &edges[s + 1];
02689 e->copy = s;
02690 e->reverse->copy = s + 1;
02691 c->reverse = 1;
02692 r->reverse = -1;
02693 c->targetVertex = getVertexCopy(e->target, oldVertices);
02694 r->targetVertex = copied;
02695 #ifdef DEBUG_CONVEX_HULL
02696 printf(" CREATE: Vertex *%d has edge to *%d\n", copied, c->getTargetVertex());
02697 #endif
02698 }
02699 if (prevCopy >= 0)
02700 {
02701 edges[e->copy].next = prevCopy - e->copy;
02702 }
02703 else
02704 {
02705 firstCopy = e->copy;
02706 }
02707 prevCopy = e->copy;
02708 e = e->next;
02709 } while (e != firstEdge);
02710 edges[firstCopy].next = prevCopy - firstCopy;
02711 }
02712 copied++;
02713 }
02714
02715 for (int i = 0; i < copied; i++)
02716 {
02717 btConvexHullInternal::Vertex* v = oldVertices[i];
02718 btConvexHullInternal::Edge* firstEdge = v->edges;
02719 if (firstEdge)
02720 {
02721 btConvexHullInternal::Edge* e = firstEdge;
02722 do
02723 {
02724 if (e->copy >= 0)
02725 {
02726 #ifdef DEBUG_CONVEX_HULL
02727 printf("Vertex *%d has edge to *%d\n", i, edges[e->copy].getTargetVertex());
02728 #endif
02729 faces.push_back(e->copy);
02730 btConvexHullInternal::Edge* f = e;
02731 do
02732 {
02733 #ifdef DEBUG_CONVEX_HULL
02734 printf(" Face *%d\n", edges[f->copy].getTargetVertex());
02735 #endif
02736 f->copy = -1;
02737 f = f->reverse->prev;
02738 } while (f != e);
02739 }
02740 e = e->next;
02741 } while (e != firstEdge);
02742 }
02743 }
02744
02745 return shift;
02746 }
02747
02748
02749
02750
02751