btConvexHullComputer.cpp

Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2011 Ole Kniemeyer, MAXON, www.maxon.net
00003 
00004 This software is provided 'as-is', without any express or implied warranty.
00005 In no event will the authors be held liable for any damages arising from the use of this software.
00006 Permission is granted to anyone to use this software for any purpose, 
00007 including commercial applications, and to alter it and redistribute it freely, 
00008 subject to the following restrictions:
00009 
00010 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
00011 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00012 3. This notice may not be removed or altered from any source distribution.
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 //The definition of USE_X86_64_ASM is moved into the build system. You can enable it manually by commenting out the following lines
00038 //#if (defined(__GNUC__) && defined(__x86_64__) && !defined(__ICL))  // || (defined(__ICL) && defined(_M_X64))   bug in Intel compiler, disable inline assembly
00039 //      #define USE_X86_64_ASM
00040 //#endif
00041 
00042 
00043 //#define DEBUG_CONVEX_HULL
00044 //#define SHOW_ITERATIONS
00045 
00046 #if defined(DEBUG_CONVEX_HULL) || defined(SHOW_ITERATIONS)
00047         #include <stdio.h>
00048 #endif
00049 
00050 // Convex hull implementation based on Preparata and Hong
00051 // Ole Kniemeyer, MAXON Computer GmbH
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         //      return (numerator * b.denominator > b.numerator * denominator) ? sign : (numerator * b.denominator < b.numerator * denominator) ? -sign : 0;
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" // rdx:rax contains 128-bit-difference "numerator*b.denominator - b.numerator*denominator"
00921                                          "setnsb %%bh\n\t" // bh=1 if difference is non-negative, bh=0 otherwise
00922                                          "orq %%rdx, %%rax\n\t"
00923                                          "setnzb %%bl\n\t" // bl=1 if difference if non-zero, bl=0 if it is zero
00924                                          "decb %%bh\n\t" // now bx=0x0000 if difference is zero, 0xff01 if it is negative, 0x0001 if it is positive (i.e., same sign as difference)
00925                                          "shll $16, %%ebx\n\t" // ebx has same sign as difference
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 // if sign is +1, only bit 0 of result is inverted, which does not change the sign of result (and cannot result in zero)
00930                                                                                                                                 // if sign is -1, all bits of result are inverted, which changes the sign of result (and again cannot result in zero)
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                 // lint -fallthrough
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