dali-physics/third-party/bullet3/src/LinearMath/btConvexHullComputer.cpp

   1 /*
   2 Copyright (c) 2011 Ole Kniemeyer, MAXON, www.maxon.net
   3
   4 This software is provided 'as-is', without any express or implied warranty.
   5 In no event will the authors be held liable for any damages arising from the use of this software.
   6 Permission is granted to anyone to use this software for any purpose,
   7 including commercial applications, and to alter it and redistribute it freely,
   8 subject to the following restrictions:
   9
  10 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.
  11 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  12 3. This notice may not be removed or altered from any source distribution.
  13 */
  14
  15 #include <string.h>
  16
  17 #include "btConvexHullComputer.h"
  18 #include "btAlignedObjectArray.h"
  19 #include "btMinMax.h"
  20 #include "btVector3.h"
  21
  22 #ifdef __GNUC__
  23 #include <stdint.h>
  24 #elif defined(_MSC_VER)
  25 typedef __int32 int32_t;
  26 typedef __int64 int64_t;
  27 typedef unsigned __int32 uint32_t;
  28 typedef unsigned __int64 uint64_t;
  29 #else
  30 typedef int int32_t;
  31 typedef long long int int64_t;
  32 typedef unsigned int uint32_t;
  33 typedef unsigned long long int uint64_t;
  34 #endif
  35
  36 //The definition of USE_X86_64_ASM is moved into the build system. You can enable it manually by commenting out the following lines
  37 //#if (defined(__GNUC__) && defined(__x86_64__) && !defined(__ICL))  // || (defined(__ICL) && defined(_M_X64))   bug in Intel compiler, disable inline assembly
  38 //      #define USE_X86_64_ASM
  39 //#endif
  40
  41 //#define DEBUG_CONVEX_HULL
  42 //#define SHOW_ITERATIONS
  43
  44 #if defined(DEBUG_CONVEX_HULL) || defined(SHOW_ITERATIONS)
  45 #include <stdio.h>
  46 #endif
  47
  48 // Convex hull implementation based on Preparata and Hong
  49 // Ole Kniemeyer, MAXON Computer GmbH
  50 class btConvexHullInternal
  51 {
  52 public:
  53         class Point64
  54         {
  55         public:
  56                 int64_t x;
  57                 int64_t y;
  58                 int64_t z;
  59
  60                 Point64(int64_t x, int64_t y, int64_t z) : x(x), y(y), z(z)
  61                 {
  62                 }
  63
  64                 bool isZero()
  65                 {
  66                         return (x == 0) && (y == 0) && (z == 0);
  67                 }
  68
  69                 int64_t dot(const Point64& b) const
  70                 {
  71                         return x * b.x + y * b.y + z * b.z;
  72                 }
  73         };
  74
  75         class Point32
  76         {
  77         public:
  78                 int32_t x;
  79                 int32_t y;
  80                 int32_t z;
  81                 int index;
  82
  83                 Point32()
  84                 {
  85                 }
  86
  87                 Point32(int32_t x, int32_t y, int32_t z) : x(x), y(y), z(z), index(-1)
  88                 {
  89                 }
  90
  91                 bool operator==(const Point32& b) const
  92                 {
  93                         return (x == b.x) && (y == b.y) && (z == b.z);
  94                 }
  95
  96                 bool operator!=(const Point32& b) const
  97                 {
  98                         return (x != b.x) || (y != b.y) || (z != b.z);
  99                 }
 100
 101                 bool isZero()
 102                 {
 103                         return (x == 0) && (y == 0) && (z == 0);
 104                 }
 105
 106                 Point64 cross(const Point32& b) const
 107                 {
 108                         return Point64(((int64_t)y) * b.z - ((int64_t)z) * b.y, ((int64_t)z) * b.x - ((int64_t)x) * b.z, ((int64_t)x) * b.y - ((int64_t)y) * b.x);
 109                 }
 110
 111                 Point64 cross(const Point64& b) const
 112                 {
 113                         return Point64(y * b.z - z * b.y, z * b.x - x * b.z, x * b.y - y * b.x);
 114                 }
 115
 116                 int64_t dot(const Point32& b) const
 117                 {
 118                         return ((int64_t)x) * b.x + ((int64_t)y) * b.y + ((int64_t)z) * b.z;
 119                 }
 120
 121                 int64_t dot(const Point64& b) const
 122                 {
 123                         return x * b.x + y * b.y + z * b.z;
 124                 }
 125
 126                 Point32 operator+(const Point32& b) const
 127                 {
 128                         return Point32(x + b.x, y + b.y, z + b.z);
 129                 }
 130
 131                 Point32 operator-(const Point32& b) const
 132                 {
 133                         return Point32(x - b.x, y - b.y, z - b.z);
 134                 }
 135         };
 136
 137         class Int128
 138         {
 139         public:
 140                 uint64_t low;
 141                 uint64_t high;
 142
 143                 Int128()
 144                 {
 145                 }
 146
 147                 Int128(uint64_t low, uint64_t high) : low(low), high(high)
 148                 {
 149                 }
 150
 151                 Int128(uint64_t low) : low(low), high(0)
 152                 {
 153                 }
 154
 155                 Int128(int64_t value) : low(value), high((value >= 0) ? 0 : (uint64_t)-1LL)
 156                 {
 157                 }
 158
 159                 static Int128 mul(int64_t a, int64_t b);
 160
 161                 static Int128 mul(uint64_t a, uint64_t b);
 162
 163                 Int128 operator-() const
 164                 {
 165                         return Int128((uint64_t) - (int64_t)low, ~high + (low == 0));
 166                 }
 167
 168                 Int128 operator+(const Int128& b) const
 169                 {
 170 #ifdef USE_X86_64_ASM
 171                         Int128 result;
 172                         __asm__(
 173                                 "addq %[bl], %[rl]\n\t"
 174                                 "adcq %[bh], %[rh]\n\t"
 175                                 : [rl] "=r"(result.low), [rh] "=r"(result.high)
 176                                 : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high)
 177                                 : "cc");
 178                         return result;
 179 #else
 180                         uint64_t lo = low + b.low;
 181                         return Int128(lo, high + b.high + (lo < low));
 182 #endif
 183                 }
 184
 185                 Int128 operator-(const Int128& b) const
 186                 {
 187 #ifdef USE_X86_64_ASM
 188                         Int128 result;
 189                         __asm__(
 190                                 "subq %[bl], %[rl]\n\t"
 191                                 "sbbq %[bh], %[rh]\n\t"
 192                                 : [rl] "=r"(result.low), [rh] "=r"(result.high)
 193                                 : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high)
 194                                 : "cc");
 195                         return result;
 196 #else
 197                         return *this + -b;
 198 #endif
 199                 }
 200
 201                 Int128& operator+=(const Int128& b)
 202                 {
 203 #ifdef USE_X86_64_ASM
 204                         __asm__(
 205                                 "addq %[bl], %[rl]\n\t"
 206                                 "adcq %[bh], %[rh]\n\t"
 207                                 : [rl] "=r"(low), [rh] "=r"(high)
 208                                 : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high)
 209                                 : "cc");
 210 #else
 211                         uint64_t lo = low + b.low;
 212                         if (lo < low)
 213                         {
 214                                 ++high;
 215                         }
 216                         low = lo;
 217                         high += b.high;
 218 #endif
 219                         return *this;
 220                 }
 221
 222                 Int128& operator++()
 223                 {
 224                         if (++low == 0)
 225                         {
 226                                 ++high;
 227                         }
 228                         return *this;
 229                 }
 230
 231                 Int128 operator*(int64_t b) const;
 232
 233                 btScalar toScalar() const
 234                 {
 235                         return ((int64_t)high >= 0) ? btScalar(high) * (btScalar(0x100000000LL) * btScalar(0x100000000LL)) + btScalar(low)
 236                                                                                 : -(-*this).toScalar();
 237                 }
 238
 239                 int getSign() const
 240                 {
 241                         return ((int64_t)high < 0) ? -1 : (high || low) ? 1 : 0;
 242                 }
 243
 244                 bool operator<(const Int128& b) const
 245                 {
 246                         return (high < b.high) || ((high == b.high) && (low < b.low));
 247                 }
 248
 249                 int ucmp(const Int128& b) const
 250                 {
 251                         if (high < b.high)
 252                         {
 253                                 return -1;
 254                         }
 255                         if (high > b.high)
 256                         {
 257                                 return 1;
 258                         }
 259                         if (low < b.low)
 260                         {
 261                                 return -1;
 262                         }
 263                         if (low > b.low)
 264                         {
 265                                 return 1;
 266                         }
 267                         return 0;
 268                 }
 269         };
 270
 271         class Rational64
 272         {
 273         private:
 274                 uint64_t m_numerator;
 275                 uint64_t m_denominator;
 276                 int sign;
 277
 278         public:
 279                 Rational64(int64_t numerator, int64_t denominator)
 280                 {
 281                         if (numerator > 0)
 282                         {
 283                                 sign = 1;
 284                                 m_numerator = (uint64_t)numerator;
 285                         }
 286                         else if (numerator < 0)
 287                         {
 288                                 sign = -1;
 289                                 m_numerator = (uint64_t)-numerator;
 290                         }
 291                         else
 292                         {
 293                                 sign = 0;
 294                                 m_numerator = 0;
 295                         }
 296                         if (denominator > 0)
 297                         {
 298                                 m_denominator = (uint64_t)denominator;
 299                         }
 300                         else if (denominator < 0)
 301                         {
 302                                 sign = -sign;
 303                                 m_denominator = (uint64_t)-denominator;
 304                         }
 305                         else
 306                         {
 307                                 m_denominator = 0;
 308                         }
 309                 }
 310
 311                 bool isNegativeInfinity() const
 312                 {
 313                         return (sign < 0) && (m_denominator == 0);
 314                 }
 315
 316                 bool isNaN() const
 317                 {
 318                         return (sign == 0) && (m_denominator == 0);
 319                 }
 320
 321                 int compare(const Rational64& b) const;
 322
 323                 btScalar toScalar() const
 324                 {
 325                         return sign * ((m_denominator == 0) ? SIMD_INFINITY : (btScalar)m_numerator / m_denominator);
 326                 }
 327         };
 328
 329         class Rational128
 330         {
 331         private:
 332                 Int128 numerator;
 333                 Int128 denominator;
 334                 int sign;
 335                 bool isInt64;
 336
 337         public:
 338                 Rational128(int64_t value)
 339                 {
 340                         if (value > 0)
 341                         {
 342                                 sign = 1;
 343                                 this->numerator = value;
 344                         }
 345                         else if (value < 0)
 346                         {
 347                                 sign = -1;
 348                                 this->numerator = -value;
 349                         }
 350                         else
 351                         {
 352                                 sign = 0;
 353                                 this->numerator = (uint64_t)0;
 354                         }
 355                         this->denominator = (uint64_t)1;
 356                         isInt64 = true;
 357                 }
 358
 359                 Rational128(const Int128& numerator, const Int128& denominator)
 360                 {
 361                         sign = numerator.getSign();
 362                         if (sign >= 0)
 363                         {
 364                                 this->numerator = numerator;
 365                         }
 366                         else
 367                         {
 368                                 this->numerator = -numerator;
 369                         }
 370                         int dsign = denominator.getSign();
 371                         if (dsign >= 0)
 372                         {
 373                                 this->denominator = denominator;
 374                         }
 375                         else
 376                         {
 377                                 sign = -sign;
 378                                 this->denominator = -denominator;
 379                         }
 380                         isInt64 = false;
 381                 }
 382
 383                 int compare(const Rational128& b) const;
 384
 385                 int compare(int64_t b) const;
 386
 387                 btScalar toScalar() const
 388                 {
 389                         return sign * ((denominator.getSign() == 0) ? SIMD_INFINITY : numerator.toScalar() / denominator.toScalar());
 390                 }
 391         };
 392
 393         class PointR128
 394         {
 395         public:
 396                 Int128 x;
 397                 Int128 y;
 398                 Int128 z;
 399                 Int128 denominator;
 400
 401                 PointR128()
 402                 {
 403                 }
 404
 405                 PointR128(Int128 x, Int128 y, Int128 z, Int128 denominator) : x(x), y(y), z(z), denominator(denominator)
 406                 {
 407                 }
 408
 409                 btScalar xvalue() const
 410                 {
 411                         return x.toScalar() / denominator.toScalar();
 412                 }
 413
 414                 btScalar yvalue() const
 415                 {
 416                         return y.toScalar() / denominator.toScalar();
 417                 }
 418
 419                 btScalar zvalue() const
 420                 {
 421                         return z.toScalar() / denominator.toScalar();
 422                 }
 423         };
 424
 425         class Edge;
 426         class Face;
 427
 428         class Vertex
 429         {
 430         public:
 431                 Vertex* next;
 432                 Vertex* prev;
 433                 Edge* edges;
 434                 Face* firstNearbyFace;
 435                 Face* lastNearbyFace;
 436                 PointR128 point128;
 437                 Point32 point;
 438                 int copy;
 439
 440                 Vertex() : next(NULL), prev(NULL), edges(NULL), firstNearbyFace(NULL), lastNearbyFace(NULL), copy(-1)
 441                 {
 442                 }
 443
 444 #ifdef DEBUG_CONVEX_HULL
 445                 void print()
 446                 {
 447                         printf("V%d (%d, %d, %d)", point.index, point.x, point.y, point.z);
 448                 }
 449
 450                 void printGraph();
 451 #endif
 452
 453                 Point32 operator-(const Vertex& b) const
 454                 {
 455                         return point - b.point;
 456                 }
 457
 458                 Rational128 dot(const Point64& b) const
 459                 {
 460                         return (point.index >= 0) ? Rational128(point.dot(b))
 461                                                                           : Rational128(point128.x * b.x + point128.y * b.y + point128.z * b.z, point128.denominator);
 462                 }
 463
 464                 btScalar xvalue() const
 465                 {
 466                         return (point.index >= 0) ? btScalar(point.x) : point128.xvalue();
 467                 }
 468
 469                 btScalar yvalue() const
 470                 {
 471                         return (point.index >= 0) ? btScalar(point.y) : point128.yvalue();
 472                 }
 473
 474                 btScalar zvalue() const
 475                 {
 476                         return (point.index >= 0) ? btScalar(point.z) : point128.zvalue();
 477                 }
 478
 479                 void receiveNearbyFaces(Vertex* src)
 480                 {
 481                         if (lastNearbyFace)
 482                         {
 483                                 lastNearbyFace->nextWithSameNearbyVertex = src->firstNearbyFace;
 484                         }
 485                         else
 486                         {
 487                                 firstNearbyFace = src->firstNearbyFace;
 488                         }
 489                         if (src->lastNearbyFace)
 490                         {
 491                                 lastNearbyFace = src->lastNearbyFace;
 492                         }
 493                         for (Face* f = src->firstNearbyFace; f; f = f->nextWithSameNearbyVertex)
 494                         {
 495                                 btAssert(f->nearbyVertex == src);
 496                                 f->nearbyVertex = this;
 497                         }
 498                         src->firstNearbyFace = NULL;
 499                         src->lastNearbyFace = NULL;
 500                 }
 501         };
 502
 503         class Edge
 504         {
 505         public:
 506                 Edge* next;
 507                 Edge* prev;
 508                 Edge* reverse;
 509                 Vertex* target;
 510                 Face* face;
 511                 int copy;
 512
 513                 ~Edge()
 514                 {
 515                         next = NULL;
 516                         prev = NULL;
 517                         reverse = NULL;
 518                         target = NULL;
 519                         face = NULL;
 520                 }
 521
 522                 void link(Edge* n)
 523                 {
 524                         btAssert(reverse->target == n->reverse->target);
 525                         next = n;
 526                         n->prev = this;
 527                 }
 528
 529 #ifdef DEBUG_CONVEX_HULL
 530                 void print()
 531                 {
 532                         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,
 533                                    reverse->target->point.x, reverse->target->point.y, reverse->target->point.z, target->point.x, target->point.y, target->point.z);
 534                 }
 535 #endif
 536         };
 537
 538         class Face
 539         {
 540         public:
 541                 Face* next;
 542                 Vertex* nearbyVertex;
 543                 Face* nextWithSameNearbyVertex;
 544                 Point32 origin;
 545                 Point32 dir0;
 546                 Point32 dir1;
 547
 548                 Face() : next(NULL), nearbyVertex(NULL), nextWithSameNearbyVertex(NULL)
 549                 {
 550                 }
 551
 552                 void init(Vertex* a, Vertex* b, Vertex* c)
 553                 {
 554                         nearbyVertex = a;
 555                         origin = a->point;
 556                         dir0 = *b - *a;
 557                         dir1 = *c - *a;
 558                         if (a->lastNearbyFace)
 559                         {
 560                                 a->lastNearbyFace->nextWithSameNearbyVertex = this;
 561                         }
 562                         else
 563                         {
 564                                 a->firstNearbyFace = this;
 565                         }
 566                         a->lastNearbyFace = this;
 567                 }
 568
 569                 Point64 getNormal()
 570                 {
 571                         return dir0.cross(dir1);
 572                 }
 573         };
 574
 575         template <typename UWord, typename UHWord>
 576         class DMul
 577         {
 578         private:
 579                 static uint32_t high(uint64_t value)
 580                 {
 581                         return (uint32_t)(value >> 32);
 582                 }
 583
 584                 static uint32_t low(uint64_t value)
 585                 {
 586                         return (uint32_t)value;
 587                 }
 588
 589                 static uint64_t mul(uint32_t a, uint32_t b)
 590                 {
 591                         return (uint64_t)a * (uint64_t)b;
 592                 }
 593
 594                 static void shlHalf(uint64_t& value)
 595                 {
 596                         value <<= 32;
 597                 }
 598
 599                 static uint64_t high(Int128 value)
 600                 {
 601                         return value.high;
 602                 }
 603
 604                 static uint64_t low(Int128 value)
 605                 {
 606                         return value.low;
 607                 }
 608
 609                 static Int128 mul(uint64_t a, uint64_t b)
 610                 {
 611                         return Int128::mul(a, b);
 612                 }
 613
 614                 static void shlHalf(Int128& value)
 615                 {
 616                         value.high = value.low;
 617                         value.low = 0;
 618                 }
 619
 620         public:
 621                 static void mul(UWord a, UWord b, UWord& resLow, UWord& resHigh)
 622                 {
 623                         UWord p00 = mul(low(a), low(b));
 624                         UWord p01 = mul(low(a), high(b));
 625                         UWord p10 = mul(high(a), low(b));
 626                         UWord p11 = mul(high(a), high(b));
 627                         UWord p0110 = UWord(low(p01)) + UWord(low(p10));
 628                         p11 += high(p01);
 629                         p11 += high(p10);
 630                         p11 += high(p0110);
 631                         shlHalf(p0110);
 632                         p00 += p0110;
 633                         if (p00 < p0110)
 634                         {
 635                                 ++p11;
 636                         }
 637                         resLow = p00;
 638                         resHigh = p11;
 639                 }
 640         };
 641
 642 private:
 643         class IntermediateHull
 644         {
 645         public:
 646                 Vertex* minXy;
 647                 Vertex* maxXy;
 648                 Vertex* minYx;
 649                 Vertex* maxYx;
 650
 651                 IntermediateHull() : minXy(NULL), maxXy(NULL), minYx(NULL), maxYx(NULL)
 652                 {
 653                 }
 654
 655                 void print();
 656         };
 657
 658         enum Orientation
 659         {
 660                 NONE,
 661                 CLOCKWISE,
 662                 COUNTER_CLOCKWISE
 663         };
 664
 665         template <typename T>
 666         class PoolArray
 667         {
 668         private:
 669                 T* array;
 670                 int size;
 671
 672         public:
 673                 PoolArray<T>* next;
 674
 675                 PoolArray(int size) : size(size), next(NULL)
 676                 {
 677                         array = (T*)btAlignedAlloc(sizeof(T) * size, 16);
 678                 }
 679
 680                 ~PoolArray()
 681                 {
 682                         btAlignedFree(array);
 683                 }
 684
 685                 T* init()
 686                 {
 687                         T* o = array;
 688                         for (int i = 0; i < size; i++, o++)
 689                         {
 690                                 o->next = (i + 1 < size) ? o + 1 : NULL;
 691                         }
 692                         return array;
 693                 }
 694         };
 695
 696         template <typename T>
 697         class Pool
 698         {
 699         private:
 700                 PoolArray<T>* arrays;
 701                 PoolArray<T>* nextArray;
 702                 T* freeObjects;
 703                 int arraySize;
 704
 705         public:
 706                 Pool() : arrays(NULL), nextArray(NULL), freeObjects(NULL), arraySize(256)
 707                 {
 708                 }
 709
 710                 ~Pool()
 711                 {
 712                         while (arrays)
 713                         {
 714                                 PoolArray<T>* p = arrays;
 715                                 arrays = p->next;
 716                                 p->~PoolArray<T>();
 717                                 btAlignedFree(p);
 718                         }
 719                 }
 720
 721                 void reset()
 722                 {
 723                         nextArray = arrays;
 724                         freeObjects = NULL;
 725                 }
 726
 727                 void setArraySize(int arraySize)
 728                 {
 729                         this->arraySize = arraySize;
 730                 }
 731
 732                 T* newObject()
 733                 {
 734                         T* o = freeObjects;
 735                         if (!o)
 736                         {
 737                                 PoolArray<T>* p = nextArray;
 738                                 if (p)
 739                                 {
 740                                         nextArray = p->next;
 741                                 }
 742                                 else
 743                                 {
 744                                         p = new (btAlignedAlloc(sizeof(PoolArray<T>), 16)) PoolArray<T>(arraySize);
 745                                         p->next = arrays;
 746                                         arrays = p;
 747                                 }
 748                                 o = p->init();
 749                         }
 750                         freeObjects = o->next;
 751                         return new (o) T();
 752                 };
 753
 754                 void freeObject(T* object)
 755                 {
 756                         object->~T();
 757                         object->next = freeObjects;
 758                         freeObjects = object;
 759                 }
 760         };
 761
 762         btVector3 scaling;
 763         btVector3 center;
 764         Pool<Vertex> vertexPool;
 765         Pool<Edge> edgePool;
 766         Pool<Face> facePool;
 767         btAlignedObjectArray<Vertex*> originalVertices;
 768         int mergeStamp;
 769         int minAxis;
 770         int medAxis;
 771         int maxAxis;
 772         int usedEdgePairs;
 773         int maxUsedEdgePairs;
 774
 775         static Orientation getOrientation(const Edge* prev, const Edge* next, const Point32& s, const Point32& t);
 776         Edge* findMaxAngle(bool ccw, const Vertex* start, const Point32& s, const Point64& rxs, const Point64& sxrxs, Rational64& minCot);
 777         void findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge*& e0, Edge*& e1, Vertex* stop0, Vertex* stop1);
 778
 779         Edge* newEdgePair(Vertex* from, Vertex* to);
 780
 781         void removeEdgePair(Edge* edge)
 782         {
 783                 Edge* n = edge->next;
 784                 Edge* r = edge->reverse;
 785
 786                 btAssert(edge->target && r->target);
 787
 788                 if (n != edge)
 789                 {
 790                         n->prev = edge->prev;
 791                         edge->prev->next = n;
 792                         r->target->edges = n;
 793                 }
 794                 else
 795                 {
 796                         r->target->edges = NULL;
 797                 }
 798
 799                 n = r->next;
 800
 801                 if (n != r)
 802                 {
 803                         n->prev = r->prev;
 804                         r->prev->next = n;
 805                         edge->target->edges = n;
 806                 }
 807                 else
 808                 {
 809                         edge->target->edges = NULL;
 810                 }
 811
 812                 edgePool.freeObject(edge);
 813                 edgePool.freeObject(r);
 814                 usedEdgePairs--;
 815         }
 816
 817         void computeInternal(int start, int end, IntermediateHull& result);
 818
 819         bool mergeProjection(IntermediateHull& h0, IntermediateHull& h1, Vertex*& c0, Vertex*& c1);
 820
 821         void merge(IntermediateHull& h0, IntermediateHull& h1);
 822
 823         btVector3 toBtVector(const Point32& v);
 824
 825         btVector3 getBtNormal(Face* face);
 826
 827         bool shiftFace(Face* face, btScalar amount, btAlignedObjectArray<Vertex*> stack);
 828
 829 public:
 830         Vertex* vertexList;
 831
 832         void compute(const void* coords, bool doubleCoords, int stride, int count);
 833
 834         btVector3 getCoordinates(const Vertex* v);
 835
 836         btScalar shrink(btScalar amount, btScalar clampAmount);
 837 };
 838
 839 btConvexHullInternal::Int128 btConvexHullInternal::Int128::operator*(int64_t b) const
 840 {
 841         bool negative = (int64_t)high < 0;
 842         Int128 a = negative ? -*this : *this;
 843         if (b < 0)
 844         {
 845                 negative = !negative;
 846                 b = -b;
 847         }
 848         Int128 result = mul(a.low, (uint64_t)b);
 849         result.high += a.high * (uint64_t)b;
 850         return negative ? -result : result;
 851 }
 852
 853 btConvexHullInternal::Int128 btConvexHullInternal::Int128::mul(int64_t a, int64_t b)
 854 {
 855         Int128 result;
 856
 857 #ifdef USE_X86_64_ASM
 858         __asm__("imulq %[b]"
 859                         : "=a"(result.low), "=d"(result.high)
 860                         : "0"(a), [b] "r"(b)
 861                         : "cc");
 862         return result;
 863
 864 #else
 865         bool negative = a < 0;
 866         if (negative)
 867         {
 868                 a = -a;
 869         }
 870         if (b < 0)
 871         {
 872                 negative = !negative;
 873                 b = -b;
 874         }
 875         DMul<uint64_t, uint32_t>::mul((uint64_t)a, (uint64_t)b, result.low, result.high);
 876         return negative ? -result : result;
 877 #endif
 878 }
 879
 880 btConvexHullInternal::Int128 btConvexHullInternal::Int128::mul(uint64_t a, uint64_t b)
 881 {
 882         Int128 result;
 883
 884 #ifdef USE_X86_64_ASM
 885         __asm__("mulq %[b]"
 886                         : "=a"(result.low), "=d"(result.high)
 887                         : "0"(a), [b] "r"(b)
 888                         : "cc");
 889
 890 #else
 891         DMul<uint64_t, uint32_t>::mul(a, b, result.low, result.high);
 892 #endif
 893
 894         return result;
 895 }
 896
 897 int btConvexHullInternal::Rational64::compare(const Rational64& b) const
 898 {
 899         if (sign != b.sign)
 900         {
 901                 return sign - b.sign;
 902         }
 903         else if (sign == 0)
 904         {
 905                 return 0;
 906         }
 907
 908         //      return (numerator * b.denominator > b.numerator * denominator) ? sign : (numerator * b.denominator < b.numerator * denominator) ? -sign : 0;
 909
 910 #ifdef USE_X86_64_ASM
 911
 912         int result;
 913         int64_t tmp;
 914         int64_t dummy;
 915         __asm__(
 916                 "mulq %[bn]\n\t"
 917                 "movq %%rax, %[tmp]\n\t"
 918                 "movq %%rdx, %%rbx\n\t"
 919                 "movq %[tn], %%rax\n\t"
 920                 "mulq %[bd]\n\t"
 921                 "subq %[tmp], %%rax\n\t"
 922                 "sbbq %%rbx, %%rdx\n\t"  // rdx:rax contains 128-bit-difference "numerator*b.denominator - b.numerator*denominator"
 923                 "setnsb %%bh\n\t"        // bh=1 if difference is non-negative, bh=0 otherwise
 924                 "orq %%rdx, %%rax\n\t"
 925                 "setnzb %%bl\n\t"      // bl=1 if difference if non-zero, bl=0 if it is zero
 926                 "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)
 927                 "shll $16, %%ebx\n\t"  // ebx has same sign as difference
 928                 : "=&b"(result), [tmp] "=&r"(tmp), "=a"(dummy)
 929                 : "a"(m_denominator), [bn] "g"(b.m_numerator), [tn] "g"(m_numerator), [bd] "g"(b.m_denominator)
 930                 : "%rdx", "cc");
 931         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)
 932                                                                    // if sign is -1, all bits of result are inverted, which changes the sign of result (and again cannot result in zero)
 933                                   : 0;
 934
 935 #else
 936
 937         return sign * Int128::mul(m_numerator, b.m_denominator).ucmp(Int128::mul(m_denominator, b.m_numerator));
 938
 939 #endif
 940 }
 941
 942 int btConvexHullInternal::Rational128::compare(const Rational128& b) const
 943 {
 944         if (sign != b.sign)
 945         {
 946                 return sign - b.sign;
 947         }
 948         else if (sign == 0)
 949         {
 950                 return 0;
 951         }
 952         if (isInt64)
 953         {
 954                 return -b.compare(sign * (int64_t)numerator.low);
 955         }
 956
 957         Int128 nbdLow, nbdHigh, dbnLow, dbnHigh;
 958         DMul<Int128, uint64_t>::mul(numerator, b.denominator, nbdLow, nbdHigh);
 959         DMul<Int128, uint64_t>::mul(denominator, b.numerator, dbnLow, dbnHigh);
 960
 961         int cmp = nbdHigh.ucmp(dbnHigh);
 962         if (cmp)
 963         {
 964                 return cmp * sign;
 965         }
 966         return nbdLow.ucmp(dbnLow) * sign;
 967 }
 968
 969 int btConvexHullInternal::Rational128::compare(int64_t b) const
 970 {
 971         if (isInt64)
 972         {
 973                 int64_t a = sign * (int64_t)numerator.low;
 974                 return (a > b) ? 1 : (a < b) ? -1 : 0;
 975         }
 976         if (b > 0)
 977         {
 978                 if (sign <= 0)
 979                 {
 980                         return -1;
 981                 }
 982         }
 983         else if (b < 0)
 984         {
 985                 if (sign >= 0)
 986                 {
 987                         return 1;
 988                 }
 989                 b = -b;
 990         }
 991         else
 992         {
 993                 return sign;
 994         }
 995
 996         return numerator.ucmp(denominator * b) * sign;
 997 }
 998
 999 btConvexHullInternal::Edge* btConvexHullInternal::newEdgePair(Vertex* from, Vertex* to)
1000 {
1001         btAssert(from && to);
1002         Edge* e = edgePool.newObject();
1003         Edge* r = edgePool.newObject();
1004         e->reverse = r;
1005         r->reverse = e;
1006         e->copy = mergeStamp;
1007         r->copy = mergeStamp;
1008         e->target = to;
1009         r->target = from;
1010         e->face = NULL;
1011         r->face = NULL;
1012         usedEdgePairs++;
1013         if (usedEdgePairs > maxUsedEdgePairs)
1014         {
1015                 maxUsedEdgePairs = usedEdgePairs;
1016         }
1017         return e;
1018 }
1019
1020 bool btConvexHullInternal::mergeProjection(IntermediateHull& h0, IntermediateHull& h1, Vertex*& c0, Vertex*& c1)
1021 {
1022         Vertex* v0 = h0.maxYx;
1023         Vertex* v1 = h1.minYx;
1024         if ((v0->point.x == v1->point.x) && (v0->point.y == v1->point.y))
1025         {
1026                 btAssert(v0->point.z < v1->point.z);
1027                 Vertex* v1p = v1->prev;
1028                 if (v1p == v1)
1029                 {
1030                         c0 = v0;
1031                         if (v1->edges)
1032                         {
1033                                 btAssert(v1->edges->next == v1->edges);
1034                                 v1 = v1->edges->target;
1035                                 btAssert(v1->edges->next == v1->edges);
1036                         }
1037                         c1 = v1;
1038                         return false;
1039                 }
1040                 Vertex* v1n = v1->next;
1041                 v1p->next = v1n;
1042                 v1n->prev = v1p;
1043                 if (v1 == h1.minXy)
1044                 {
1045                         if ((v1n->point.x < v1p->point.x) || ((v1n->point.x == v1p->point.x) && (v1n->point.y < v1p->point.y)))
1046                         {
1047                                 h1.minXy = v1n;
1048                         }
1049                         else
1050                         {
1051                                 h1.minXy = v1p;
1052                         }
1053                 }
1054                 if (v1 == h1.maxXy)
1055                 {
1056                         if ((v1n->point.x > v1p->point.x) || ((v1n->point.x == v1p->point.x) && (v1n->point.y > v1p->point.y)))
1057                         {
1058                                 h1.maxXy = v1n;
1059                         }
1060                         else
1061                         {
1062                                 h1.maxXy = v1p;
1063                         }
1064                 }
1065         }
1066
1067         v0 = h0.maxXy;
1068         v1 = h1.maxXy;
1069         Vertex* v00 = NULL;
1070         Vertex* v10 = NULL;
1071         int32_t sign = 1;
1072
1073         for (int side = 0; side <= 1; side++)
1074         {
1075                 int32_t dx = (v1->point.x - v0->point.x) * sign;
1076                 if (dx > 0)
1077                 {
1078                         while (true)
1079                         {
1080                                 int32_t dy = v1->point.y - v0->point.y;
1081
1082                                 Vertex* w0 = side ? v0->next : v0->prev;
1083                                 if (w0 != v0)
1084                                 {
1085                                         int32_t dx0 = (w0->point.x - v0->point.x) * sign;
1086                                         int32_t dy0 = w0->point.y - v0->point.y;
1087                                         if ((dy0 <= 0) && ((dx0 == 0) || ((dx0 < 0) && (dy0 * dx <= dy * dx0))))
1088                                         {
1089                                                 v0 = w0;
1090                                                 dx = (v1->point.x - v0->point.x) * sign;
1091                                                 continue;
1092                                         }
1093                                 }
1094
1095                                 Vertex* w1 = side ? v1->next : v1->prev;
1096                                 if (w1 != v1)
1097                                 {
1098                                         int32_t dx1 = (w1->point.x - v1->point.x) * sign;
1099                                         int32_t dy1 = w1->point.y - v1->point.y;
1100                                         int32_t dxn = (w1->point.x - v0->point.x) * sign;
1101                                         if ((dxn > 0) && (dy1 < 0) && ((dx1 == 0) || ((dx1 < 0) && (dy1 * dx < dy * dx1))))
1102                                         {
1103                                                 v1 = w1;
1104                                                 dx = dxn;
1105                                                 continue;
1106                                         }
1107                                 }
1108
1109                                 break;
1110                         }
1111                 }
1112                 else if (dx < 0)
1113                 {
1114                         while (true)
1115                         {
1116                                 int32_t dy = v1->point.y - v0->point.y;
1117
1118                                 Vertex* w1 = side ? v1->prev : v1->next;
1119                                 if (w1 != v1)
1120                                 {
1121                                         int32_t dx1 = (w1->point.x - v1->point.x) * sign;
1122                                         int32_t dy1 = w1->point.y - v1->point.y;
1123                                         if ((dy1 >= 0) && ((dx1 == 0) || ((dx1 < 0) && (dy1 * dx <= dy * dx1))))
1124                                         {
1125                                                 v1 = w1;
1126                                                 dx = (v1->point.x - v0->point.x) * sign;
1127                                                 continue;
1128                                         }
1129                                 }
1130
1131                                 Vertex* w0 = side ? v0->prev : v0->next;
1132                                 if (w0 != v0)
1133                                 {
1134                                         int32_t dx0 = (w0->point.x - v0->point.x) * sign;
1135                                         int32_t dy0 = w0->point.y - v0->point.y;
1136                                         int32_t dxn = (v1->point.x - w0->point.x) * sign;
1137                                         if ((dxn < 0) && (dy0 > 0) && ((dx0 == 0) || ((dx0 < 0) && (dy0 * dx < dy * dx0))))
1138                                         {
1139                                                 v0 = w0;
1140                                                 dx = dxn;
1141                                                 continue;
1142                                         }
1143                                 }
1144
1145                                 break;
1146                         }
1147                 }
1148                 else
1149                 {
1150                         int32_t x = v0->point.x;
1151                         int32_t y0 = v0->point.y;
1152                         Vertex* w0 = v0;
1153                         Vertex* t;
1154                         while (((t = side ? w0->next : w0->prev) != v0) && (t->point.x == x) && (t->point.y <= y0))
1155                         {
1156                                 w0 = t;
1157                                 y0 = t->point.y;
1158                         }
1159                         v0 = w0;
1160
1161                         int32_t y1 = v1->point.y;
1162                         Vertex* w1 = v1;
1163                         while (((t = side ? w1->prev : w1->next) != v1) && (t->point.x == x) && (t->point.y >= y1))
1164                         {
1165                                 w1 = t;
1166                                 y1 = t->point.y;
1167                         }
1168                         v1 = w1;
1169                 }
1170
1171                 if (side == 0)
1172                 {
1173                         v00 = v0;
1174                         v10 = v1;
1175
1176                         v0 = h0.minXy;
1177                         v1 = h1.minXy;
1178                         sign = -1;
1179                 }
1180         }
1181
1182         v0->prev = v1;
1183         v1->next = v0;
1184
1185         v00->next = v10;
1186         v10->prev = v00;
1187
1188         if (h1.minXy->point.x < h0.minXy->point.x)
1189         {
1190                 h0.minXy = h1.minXy;
1191         }
1192         if (h1.maxXy->point.x >= h0.maxXy->point.x)
1193         {
1194                 h0.maxXy = h1.maxXy;
1195         }
1196
1197         h0.maxYx = h1.maxYx;
1198
1199         c0 = v00;
1200         c1 = v10;
1201
1202         return true;
1203 }
1204
1205 void btConvexHullInternal::computeInternal(int start, int end, IntermediateHull& result)
1206 {
1207         int n = end - start;
1208         switch (n)
1209         {
1210                 case 0:
1211                         result.minXy = NULL;
1212                         result.maxXy = NULL;
1213                         result.minYx = NULL;
1214                         result.maxYx = NULL;
1215                         return;
1216                 case 2:
1217                 {
1218                         Vertex* v = originalVertices[start];
1219                         Vertex* w = v + 1;
1220                         if (v->point != w->point)
1221                         {
1222                                 int32_t dx = v->point.x - w->point.x;
1223                                 int32_t dy = v->point.y - w->point.y;
1224
1225                                 if ((dx == 0) && (dy == 0))
1226                                 {
1227                                         if (v->point.z > w->point.z)
1228                                         {
1229                                                 Vertex* t = w;
1230                                                 w = v;
1231                                                 v = t;
1232                                         }
1233                                         btAssert(v->point.z < w->point.z);
1234                                         v->next = v;
1235                                         v->prev = v;
1236                                         result.minXy = v;
1237                                         result.maxXy = v;
1238                                         result.minYx = v;
1239                                         result.maxYx = v;
1240                                 }
1241                                 else
1242                                 {
1243                                         v->next = w;
1244                                         v->prev = w;
1245                                         w->next = v;
1246                                         w->prev = v;
1247
1248                                         if ((dx < 0) || ((dx == 0) && (dy < 0)))
1249                                         {
1250                                                 result.minXy = v;
1251                                                 result.maxXy = w;
1252                                         }
1253                                         else
1254                                         {
1255                                                 result.minXy = w;
1256                                                 result.maxXy = v;
1257                                         }
1258
1259                                         if ((dy < 0) || ((dy == 0) && (dx < 0)))
1260                                         {
1261                                                 result.minYx = v;
1262                                                 result.maxYx = w;
1263                                         }
1264                                         else
1265                                         {
1266                                                 result.minYx = w;
1267                                                 result.maxYx = v;
1268                                         }
1269                                 }
1270
1271                                 Edge* e = newEdgePair(v, w);
1272                                 e->link(e);
1273                                 v->edges = e;
1274
1275                                 e = e->reverse;
1276                                 e->link(e);
1277                                 w->edges = e;
1278
1279                                 return;
1280                         }
1281                         {
1282                                 Vertex* v = originalVertices[start];
1283                                 v->edges = NULL;
1284                                 v->next = v;
1285                                 v->prev = v;
1286
1287                                 result.minXy = v;
1288                                 result.maxXy = v;
1289                                 result.minYx = v;
1290                                 result.maxYx = v;
1291                         }
1292
1293                         return;
1294                 }
1295
1296                 case 1:
1297                 {
1298                         Vertex* v = originalVertices[start];
1299                         v->edges = NULL;
1300                         v->next = v;
1301                         v->prev = v;
1302
1303                         result.minXy = v;
1304                         result.maxXy = v;
1305                         result.minYx = v;
1306                         result.maxYx = v;
1307
1308                         return;
1309                 }
1310         }
1311
1312         int split0 = start + n / 2;
1313         Point32 p = originalVertices[split0 - 1]->point;
1314         int split1 = split0;
1315         while ((split1 < end) && (originalVertices[split1]->point == p))
1316         {
1317                 split1++;
1318         }
1319         computeInternal(start, split0, result);
1320         IntermediateHull hull1;
1321         computeInternal(split1, end, hull1);
1322 #ifdef DEBUG_CONVEX_HULL
1323         printf("\n\nMerge\n");
1324         result.print();
1325         hull1.print();
1326 #endif
1327         merge(result, hull1);
1328 #ifdef DEBUG_CONVEX_HULL
1329         printf("\n  Result\n");
1330         result.print();
1331 #endif
1332 }
1333
1334 #ifdef DEBUG_CONVEX_HULL
1335 void btConvexHullInternal::IntermediateHull::print()
1336 {
1337         printf("    Hull\n");
1338         for (Vertex* v = minXy; v;)
1339         {
1340                 printf("      ");
1341                 v->print();
1342                 if (v == maxXy)
1343                 {
1344                         printf(" maxXy");
1345                 }
1346                 if (v == minYx)
1347                 {
1348                         printf(" minYx");
1349                 }
1350                 if (v == maxYx)
1351                 {
1352                         printf(" maxYx");
1353                 }
1354                 if (v->next->prev != v)
1355                 {
1356                         printf(" Inconsistency");
1357                 }
1358                 printf("\n");
1359                 v = v->next;
1360                 if (v == minXy)
1361                 {
1362                         break;
1363                 }
1364         }
1365         if (minXy)
1366         {
1367                 minXy->copy = (minXy->copy == -1) ? -2 : -1;
1368                 minXy->printGraph();
1369         }
1370 }
1371
1372 void btConvexHullInternal::Vertex::printGraph()
1373 {
1374         print();
1375         printf("\nEdges\n");
1376         Edge* e = edges;
1377         if (e)
1378         {
1379                 do
1380                 {
1381                         e->print();
1382                         printf("\n");
1383                         e = e->next;
1384                 } while (e != edges);
1385                 do
1386                 {
1387                         Vertex* v = e->target;
1388                         if (v->copy != copy)
1389                         {
1390                                 v->copy = copy;
1391                                 v->printGraph();
1392                         }
1393                         e = e->next;
1394                 } while (e != edges);
1395         }
1396 }
1397 #endif
1398
1399 btConvexHullInternal::Orientation btConvexHullInternal::getOrientation(const Edge* prev, const Edge* next, const Point32& s, const Point32& t)
1400 {
1401         btAssert(prev->reverse->target == next->reverse->target);
1402         if (prev->next == next)
1403         {
1404                 if (prev->prev == next)
1405                 {
1406                         Point64 n = t.cross(s);
1407                         Point64 m = (*prev->target - *next->reverse->target).cross(*next->target - *next->reverse->target);
1408                         btAssert(!m.isZero());
1409                         int64_t dot = n.dot(m);
1410                         btAssert(dot != 0);
1411                         return (dot > 0) ? COUNTER_CLOCKWISE : CLOCKWISE;
1412                 }
1413                 return COUNTER_CLOCKWISE;
1414         }
1415         else if (prev->prev == next)
1416         {
1417                 return CLOCKWISE;
1418         }
1419         else
1420         {
1421                 return NONE;
1422         }
1423 }
1424
1425 btConvexHullInternal::Edge* btConvexHullInternal::findMaxAngle(bool ccw, const Vertex* start, const Point32& s, const Point64& rxs, const Point64& sxrxs, Rational64& minCot)
1426 {
1427         Edge* minEdge = NULL;
1428
1429 #ifdef DEBUG_CONVEX_HULL
1430         printf("find max edge for %d\n", start->point.index);
1431 #endif
1432         Edge* e = start->edges;
1433         if (e)
1434         {
1435                 do
1436                 {
1437                         if (e->copy > mergeStamp)
1438                         {
1439                                 Point32 t = *e->target - *start;
1440                                 Rational64 cot(t.dot(sxrxs), t.dot(rxs));
1441 #ifdef DEBUG_CONVEX_HULL
1442                                 printf("      Angle is %f (%d) for ", (float)btAtan(cot.toScalar()), (int)cot.isNaN());
1443                                 e->print();
1444 #endif
1445                                 if (cot.isNaN())
1446                                 {
1447                                         btAssert(ccw ? (t.dot(s) < 0) : (t.dot(s) > 0));
1448                                 }
1449                                 else
1450                                 {
1451                                         int cmp;
1452                                         if (minEdge == NULL)
1453                                         {
1454                                                 minCot = cot;
1455                                                 minEdge = e;
1456                                         }
1457                                         else if ((cmp = cot.compare(minCot)) < 0)
1458                                         {
1459                                                 minCot = cot;
1460                                                 minEdge = e;
1461                                         }
1462                                         else if ((cmp == 0) && (ccw == (getOrientation(minEdge, e, s, t) == COUNTER_CLOCKWISE)))
1463                                         {
1464                                                 minEdge = e;
1465                                         }
1466                                 }
1467 #ifdef DEBUG_CONVEX_HULL
1468                                 printf("\n");
1469 #endif
1470                         }
1471                         e = e->next;
1472                 } while (e != start->edges);
1473         }
1474         return minEdge;
1475 }
1476
1477 void btConvexHullInternal::findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge*& e0, Edge*& e1, Vertex* stop0, Vertex* stop1)
1478 {
1479         Edge* start0 = e0;
1480         Edge* start1 = e1;
1481         Point32 et0 = start0 ? start0->target->point : c0->point;
1482         Point32 et1 = start1 ? start1->target->point : c1->point;
1483         Point32 s = c1->point - c0->point;
1484         Point64 normal = ((start0 ? start0 : start1)->target->point - c0->point).cross(s);
1485         int64_t dist = c0->point.dot(normal);
1486         btAssert(!start1 || (start1->target->point.dot(normal) == dist));
1487         Point64 perp = s.cross(normal);
1488         btAssert(!perp.isZero());
1489
1490 #ifdef DEBUG_CONVEX_HULL
1491         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);
1492 #endif
1493
1494         int64_t maxDot0 = et0.dot(perp);
1495         if (e0)
1496         {
1497                 while (e0->target != stop0)
1498                 {
1499                         Edge* e = e0->reverse->prev;
1500                         if (e->target->point.dot(normal) < dist)
1501                         {
1502                                 break;
1503                         }
1504                         btAssert(e->target->point.dot(normal) == dist);
1505                         if (e->copy == mergeStamp)
1506                         {
1507                                 break;
1508                         }
1509                         int64_t dot = e->target->point.dot(perp);
1510                         if (dot <= maxDot0)
1511                         {
1512                                 break;
1513                         }
1514                         maxDot0 = dot;
1515                         e0 = e;
1516                         et0 = e->target->point;
1517                 }
1518         }
1519
1520         int64_t maxDot1 = et1.dot(perp);
1521         if (e1)
1522         {
1523                 while (e1->target != stop1)
1524                 {
1525                         Edge* e = e1->reverse->next;
1526                         if (e->target->point.dot(normal) < dist)
1527                         {
1528                                 break;
1529                         }
1530                         btAssert(e->target->point.dot(normal) == dist);
1531                         if (e->copy == mergeStamp)
1532                         {
1533                                 break;
1534                         }
1535                         int64_t dot = e->target->point.dot(perp);
1536                         if (dot <= maxDot1)
1537                         {
1538                                 break;
1539                         }
1540                         maxDot1 = dot;
1541                         e1 = e;
1542                         et1 = e->target->point;
1543                 }
1544         }
1545
1546 #ifdef DEBUG_CONVEX_HULL
1547         printf("   Starting at %d %d\n", et0.index, et1.index);
1548 #endif
1549
1550         int64_t dx = maxDot1 - maxDot0;
1551         if (dx > 0)
1552         {
1553                 while (true)
1554                 {
1555                         int64_t dy = (et1 - et0).dot(s);
1556
1557                         if (e0 && (e0->target != stop0))
1558                         {
1559                                 Edge* f0 = e0->next->reverse;
1560                                 if (f0->copy > mergeStamp)
1561                                 {
1562                                         int64_t dx0 = (f0->target->point - et0).dot(perp);
1563                                         int64_t dy0 = (f0->target->point - et0).dot(s);
1564                                         if ((dx0 == 0) ? (dy0 < 0) : ((dx0 < 0) && (Rational64(dy0, dx0).compare(Rational64(dy, dx)) >= 0)))
1565                                         {
1566                                                 et0 = f0->target->point;
1567                                                 dx = (et1 - et0).dot(perp);
1568                                                 e0 = (e0 == start0) ? NULL : f0;
1569                                                 continue;
1570                                         }
1571                                 }
1572                         }
1573
1574                         if (e1 && (e1->target != stop1))
1575                         {
1576                                 Edge* f1 = e1->reverse->next;
1577                                 if (f1->copy > mergeStamp)
1578                                 {
1579                                         Point32 d1 = f1->target->point - et1;
1580                                         if (d1.dot(normal) == 0)
1581                                         {
1582                                                 int64_t dx1 = d1.dot(perp);
1583                                                 int64_t dy1 = d1.dot(s);
1584                                                 int64_t dxn = (f1->target->point - et0).dot(perp);
1585                                                 if ((dxn > 0) && ((dx1 == 0) ? (dy1 < 0) : ((dx1 < 0) && (Rational64(dy1, dx1).compare(Rational64(dy, dx)) > 0))))
1586                                                 {
1587                                                         e1 = f1;
1588                                                         et1 = e1->target->point;
1589                                                         dx = dxn;
1590                                                         continue;
1591                                                 }
1592                                         }
1593                                         else
1594                                         {
1595                                                 btAssert((e1 == start1) && (d1.dot(normal) < 0));
1596                                         }
1597                                 }
1598                         }
1599
1600                         break;
1601                 }
1602         }
1603         else if (dx < 0)
1604         {
1605                 while (true)
1606                 {
1607                         int64_t dy = (et1 - et0).dot(s);
1608
1609                         if (e1 && (e1->target != stop1))
1610                         {
1611                                 Edge* f1 = e1->prev->reverse;
1612                                 if (f1->copy > mergeStamp)
1613                                 {
1614                                         int64_t dx1 = (f1->target->point - et1).dot(perp);
1615                                         int64_t dy1 = (f1->target->point - et1).dot(s);
1616                                         if ((dx1 == 0) ? (dy1 > 0) : ((dx1 < 0) && (Rational64(dy1, dx1).compare(Rational64(dy, dx)) <= 0)))
1617                                         {
1618                                                 et1 = f1->target->point;
1619                                                 dx = (et1 - et0).dot(perp);
1620                                                 e1 = (e1 == start1) ? NULL : f1;
1621                                                 continue;
1622                                         }
1623                                 }
1624                         }
1625
1626                         if (e0 && (e0->target != stop0))
1627                         {
1628                                 Edge* f0 = e0->reverse->prev;
1629                                 if (f0->copy > mergeStamp)
1630                                 {
1631                                         Point32 d0 = f0->target->point - et0;
1632                                         if (d0.dot(normal) == 0)
1633                                         {
1634                                                 int64_t dx0 = d0.dot(perp);
1635                                                 int64_t dy0 = d0.dot(s);
1636                                                 int64_t dxn = (et1 - f0->target->point).dot(perp);
1637                                                 if ((dxn < 0) && ((dx0 == 0) ? (dy0 > 0) : ((dx0 < 0) && (Rational64(dy0, dx0).compare(Rational64(dy, dx)) < 0))))
1638                                                 {
1639                                                         e0 = f0;
1640                                                         et0 = e0->target->point;
1641                                                         dx = dxn;
1642                                                         continue;
1643                                                 }
1644                                         }
1645                                         else
1646                                         {
1647                                                 btAssert((e0 == start0) && (d0.dot(normal) < 0));
1648                                         }
1649                                 }
1650                         }
1651
1652                         break;
1653                 }
1654         }
1655 #ifdef DEBUG_CONVEX_HULL
1656         printf("   Advanced edges to %d %d\n", et0.index, et1.index);
1657 #endif
1658 }
1659
1660 void btConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1)
1661 {
1662         if (!h1.maxXy)
1663         {
1664                 return;
1665         }
1666         if (!h0.maxXy)
1667         {
1668                 h0 = h1;
1669                 return;
1670         }
1671
1672         mergeStamp--;
1673
1674         Vertex* c0 = NULL;
1675         Edge* toPrev0 = NULL;
1676         Edge* firstNew0 = NULL;
1677         Edge* pendingHead0 = NULL;
1678         Edge* pendingTail0 = NULL;
1679         Vertex* c1 = NULL;
1680         Edge* toPrev1 = NULL;
1681         Edge* firstNew1 = NULL;
1682         Edge* pendingHead1 = NULL;
1683         Edge* pendingTail1 = NULL;
1684         Point32 prevPoint;
1685
1686         if (mergeProjection(h0, h1, c0, c1))
1687         {
1688                 Point32 s = *c1 - *c0;
1689                 Point64 normal = Point32(0, 0, -1).cross(s);
1690                 Point64 t = s.cross(normal);
1691                 btAssert(!t.isZero());
1692
1693                 Edge* e = c0->edges;
1694                 Edge* start0 = NULL;
1695                 if (e)
1696                 {
1697                         do
1698                         {
1699                                 int64_t dot = (*e->target - *c0).dot(normal);
1700                                 btAssert(dot <= 0);
1701                                 if ((dot == 0) && ((*e->target - *c0).dot(t) > 0))
1702                                 {
1703                                         if (!start0 || (getOrientation(start0, e, s, Point32(0, 0, -1)) == CLOCKWISE))
1704                                         {
1705                                                 start0 = e;
1706                                         }
1707                                 }
1708                                 e = e->next;
1709                         } while (e != c0->edges);
1710                 }
1711
1712                 e = c1->edges;
1713                 Edge* start1 = NULL;
1714                 if (e)
1715                 {
1716                         do
1717                         {
1718                                 int64_t dot = (*e->target - *c1).dot(normal);
1719                                 btAssert(dot <= 0);
1720                                 if ((dot == 0) && ((*e->target - *c1).dot(t) > 0))
1721                                 {
1722                                         if (!start1 || (getOrientation(start1, e, s, Point32(0, 0, -1)) == COUNTER_CLOCKWISE))
1723                                         {
1724                                                 start1 = e;
1725                                         }
1726                                 }
1727                                 e = e->next;
1728                         } while (e != c1->edges);
1729                 }
1730
1731                 if (start0 || start1)
1732                 {
1733                         findEdgeForCoplanarFaces(c0, c1, start0, start1, NULL, NULL);
1734                         if (start0)
1735                         {
1736                                 c0 = start0->target;
1737                         }
1738                         if (start1)
1739                         {
1740                                 c1 = start1->target;
1741                         }
1742                 }
1743
1744                 prevPoint = c1->point;
1745                 prevPoint.z++;
1746         }
1747         else
1748         {
1749                 prevPoint = c1->point;
1750                 prevPoint.x++;
1751         }
1752
1753         Vertex* first0 = c0;
1754         Vertex* first1 = c1;
1755         bool firstRun = true;
1756
1757         while (true)
1758         {
1759                 Point32 s = *c1 - *c0;
1760                 Point32 r = prevPoint - c0->point;
1761                 Point64 rxs = r.cross(s);
1762                 Point64 sxrxs = s.cross(rxs);
1763
1764 #ifdef DEBUG_CONVEX_HULL
1765                 printf("\n  Checking %d %d\n", c0->point.index, c1->point.index);
1766 #endif
1767                 Rational64 minCot0(0, 0);
1768                 Edge* min0 = findMaxAngle(false, c0, s, rxs, sxrxs, minCot0);
1769                 Rational64 minCot1(0, 0);
1770                 Edge* min1 = findMaxAngle(true, c1, s, rxs, sxrxs, minCot1);
1771                 if (!min0 && !min1)
1772                 {
1773                         Edge* e = newEdgePair(c0, c1);
1774                         e->link(e);
1775                         c0->edges = e;
1776
1777                         e = e->reverse;
1778                         e->link(e);
1779                         c1->edges = e;
1780                         return;
1781                 }
1782                 else
1783                 {
1784                         int cmp = !min0 ? 1 : !min1 ? -1 : minCot0.compare(minCot1);
1785 #ifdef DEBUG_CONVEX_HULL
1786                         printf("    -> Result %d\n", cmp);
1787 #endif
1788                         if (firstRun || ((cmp >= 0) ? !minCot1.isNegativeInfinity() : !minCot0.isNegativeInfinity()))
1789                         {
1790                                 Edge* e = newEdgePair(c0, c1);
1791                                 if (pendingTail0)
1792                                 {
1793                                         pendingTail0->prev = e;
1794                                 }
1795                                 else
1796                                 {
1797                                         pendingHead0 = e;
1798                                 }
1799                                 e->next = pendingTail0;
1800                                 pendingTail0 = e;
1801
1802                                 e = e->reverse;
1803                                 if (pendingTail1)
1804                                 {
1805                                         pendingTail1->next = e;
1806                                 }
1807                                 else
1808                                 {
1809                                         pendingHead1 = e;
1810                                 }
1811                                 e->prev = pendingTail1;
1812                                 pendingTail1 = e;
1813                         }
1814
1815                         Edge* e0 = min0;
1816                         Edge* e1 = min1;
1817
1818 #ifdef DEBUG_CONVEX_HULL
1819                         printf("   Found min edges to %d %d\n", e0 ? e0->target->point.index : -1, e1 ? e1->target->point.index : -1);
1820 #endif
1821
1822                         if (cmp == 0)
1823                         {
1824                                 findEdgeForCoplanarFaces(c0, c1, e0, e1, NULL, NULL);
1825                         }
1826
1827                         if ((cmp >= 0) && e1)
1828                         {
1829                                 if (toPrev1)
1830                                 {
1831                                         for (Edge *e = toPrev1->next, *n = NULL; e != min1; e = n)
1832                                         {
1833                                                 n = e->next;
1834                                                 removeEdgePair(e);
1835                                         }
1836                                 }
1837
1838                                 if (pendingTail1)
1839                                 {
1840                                         if (toPrev1)
1841                                         {
1842                                                 toPrev1->link(pendingHead1);
1843                                         }
1844                                         else
1845                                         {
1846                                                 min1->prev->link(pendingHead1);
1847                                                 firstNew1 = pendingHead1;
1848                                         }
1849                                         pendingTail1->link(min1);
1850                                         pendingHead1 = NULL;
1851                                         pendingTail1 = NULL;
1852                                 }
1853                                 else if (!toPrev1)
1854                                 {
1855                                         firstNew1 = min1;
1856                                 }
1857
1858                                 prevPoint = c1->point;
1859                                 c1 = e1->target;
1860                                 toPrev1 = e1->reverse;
1861                         }
1862
1863                         if ((cmp <= 0) && e0)
1864                         {
1865                                 if (toPrev0)
1866                                 {
1867                                         for (Edge *e = toPrev0->prev, *n = NULL; e != min0; e = n)
1868                                         {
1869                                                 n = e->prev;
1870                                                 removeEdgePair(e);
1871                                         }
1872                                 }
1873
1874                                 if (pendingTail0)
1875                                 {
1876                                         if (toPrev0)
1877                                         {
1878                                                 pendingHead0->link(toPrev0);
1879                                         }
1880                                         else
1881                                         {
1882                                                 pendingHead0->link(min0->next);
1883                                                 firstNew0 = pendingHead0;
1884                                         }
1885                                         min0->link(pendingTail0);
1886                                         pendingHead0 = NULL;
1887                                         pendingTail0 = NULL;
1888                                 }
1889                                 else if (!toPrev0)
1890                                 {
1891                                         firstNew0 = min0;
1892                                 }
1893
1894                                 prevPoint = c0->point;
1895                                 c0 = e0->target;
1896                                 toPrev0 = e0->reverse;
1897                         }
1898                 }
1899
1900                 if ((c0 == first0) && (c1 == first1))
1901                 {
1902                         if (toPrev0 == NULL)
1903                         {
1904                                 pendingHead0->link(pendingTail0);
1905                                 c0->edges = pendingTail0;
1906                         }
1907                         else
1908                         {
1909                                 for (Edge *e = toPrev0->prev, *n = NULL; e != firstNew0; e = n)
1910                                 {
1911                                         n = e->prev;
1912                                         removeEdgePair(e);
1913                                 }
1914                                 if (pendingTail0)
1915                                 {
1916                                         pendingHead0->link(toPrev0);
1917                                         firstNew0->link(pendingTail0);
1918                                 }
1919                         }
1920
1921                         if (toPrev1 == NULL)
1922                         {
1923                                 pendingTail1->link(pendingHead1);
1924                                 c1->edges = pendingTail1;
1925                         }
1926                         else
1927                         {
1928                                 for (Edge *e = toPrev1->next, *n = NULL; e != firstNew1; e = n)
1929                                 {
1930                                         n = e->next;
1931                                         removeEdgePair(e);
1932                                 }
1933                                 if (pendingTail1)
1934                                 {
1935                                         toPrev1->link(pendingHead1);
1936                                         pendingTail1->link(firstNew1);
1937                                 }
1938                         }
1939
1940                         return;
1941                 }
1942
1943                 firstRun = false;
1944         }
1945 }
1946
1947 class pointCmp
1948 {
1949 public:
1950         bool operator()(const btConvexHullInternal::Point32& p, const btConvexHullInternal::Point32& q) const
1951         {
1952                 return (p.y < q.y) || ((p.y == q.y) && ((p.x < q.x) || ((p.x == q.x) && (p.z < q.z))));
1953         }
1954 };
1955
1956 void btConvexHullInternal::compute(const void* coords, bool doubleCoords, int stride, int count)
1957 {
1958         btVector3 min(btScalar(1e30), btScalar(1e30), btScalar(1e30)), max(btScalar(-1e30), btScalar(-1e30), btScalar(-1e30));
1959         const char* ptr = (const char*)coords;
1960         if (doubleCoords)
1961         {
1962                 for (int i = 0; i < count; i++)
1963                 {
1964                         const double* v = (const double*)ptr;
1965                         btVector3 p((btScalar)v[0], (btScalar)v[1], (btScalar)v[2]);
1966                         ptr += stride;
1967                         min.setMin(p);
1968                         max.setMax(p);
1969                 }
1970         }
1971         else
1972         {
1973                 for (int i = 0; i < count; i++)
1974                 {
1975                         const float* v = (const float*)ptr;
1976                         btVector3 p(v[0], v[1], v[2]);
1977                         ptr += stride;
1978                         min.setMin(p);
1979                         max.setMax(p);
1980                 }
1981         }
1982
1983         btVector3 s = max - min;
1984         maxAxis = s.maxAxis();
1985         minAxis = s.minAxis();
1986         if (minAxis == maxAxis)
1987         {
1988                 minAxis = (maxAxis + 1) % 3;
1989         }
1990         medAxis = 3 - maxAxis - minAxis;
1991
1992         s /= btScalar(10216);
1993         if (((medAxis + 1) % 3) != maxAxis)
1994         {
1995                 s *= -1;
1996         }
1997         scaling = s;
1998
1999         if (s[0] != 0)
2000         {
2001                 s[0] = btScalar(1) / s[0];
2002         }
2003         if (s[1] != 0)
2004         {
2005                 s[1] = btScalar(1) / s[1];
2006         }
2007         if (s[2] != 0)
2008         {
2009                 s[2] = btScalar(1) / s[2];
2010         }
2011
2012         center = (min + max) * btScalar(0.5);
2013
2014         btAlignedObjectArray<Point32> points;
2015         points.resize(count);
2016         ptr = (const char*)coords;
2017         if (doubleCoords)
2018         {
2019                 for (int i = 0; i < count; i++)
2020                 {
2021                         const double* v = (const double*)ptr;
2022                         btVector3 p((btScalar)v[0], (btScalar)v[1], (btScalar)v[2]);
2023                         ptr += stride;
2024                         p = (p - center) * s;
2025                         points[i].x = (int32_t)p[medAxis];
2026                         points[i].y = (int32_t)p[maxAxis];
2027                         points[i].z = (int32_t)p[minAxis];
2028                         points[i].index = i;
2029                 }
2030         }
2031         else
2032         {
2033                 for (int i = 0; i < count; i++)
2034                 {
2035                         const float* v = (const float*)ptr;
2036                         btVector3 p(v[0], v[1], v[2]);
2037                         ptr += stride;
2038                         p = (p - center) * s;
2039                         points[i].x = (int32_t)p[medAxis];
2040                         points[i].y = (int32_t)p[maxAxis];
2041                         points[i].z = (int32_t)p[minAxis];
2042                         points[i].index = i;
2043                 }
2044         }
2045         points.quickSort(pointCmp());
2046
2047         vertexPool.reset();
2048         vertexPool.setArraySize(count);
2049         originalVertices.resize(count);
2050         for (int i = 0; i < count; i++)
2051         {
2052                 Vertex* v = vertexPool.newObject();
2053                 v->edges = NULL;
2054                 v->point = points[i];
2055                 v->copy = -1;
2056                 originalVertices[i] = v;
2057         }
2058
2059         points.clear();
2060
2061         edgePool.reset();
2062         edgePool.setArraySize(6 * count);
2063
2064         usedEdgePairs = 0;
2065         maxUsedEdgePairs = 0;
2066
2067         mergeStamp = -3;
2068
2069         IntermediateHull hull;
2070         computeInternal(0, count, hull);
2071         vertexList = hull.minXy;
2072 #ifdef DEBUG_CONVEX_HULL
2073         printf("max. edges %d (3v = %d)", maxUsedEdgePairs, 3 * count);
2074 #endif
2075 }
2076
2077 btVector3 btConvexHullInternal::toBtVector(const Point32& v)
2078 {
2079         btVector3 p;
2080         p[medAxis] = btScalar(v.x);
2081         p[maxAxis] = btScalar(v.y);
2082         p[minAxis] = btScalar(v.z);
2083         return p * scaling;
2084 }
2085
2086 btVector3 btConvexHullInternal::getBtNormal(Face* face)
2087 {
2088         return toBtVector(face->dir0).cross(toBtVector(face->dir1)).normalized();
2089 }
2090
2091 btVector3 btConvexHullInternal::getCoordinates(const Vertex* v)
2092 {
2093         btVector3 p;
2094         p[medAxis] = v->xvalue();
2095         p[maxAxis] = v->yvalue();
2096         p[minAxis] = v->zvalue();
2097         return p * scaling + center;
2098 }
2099
2100 btScalar btConvexHullInternal::shrink(btScalar amount, btScalar clampAmount)
2101 {
2102         if (!vertexList)
2103         {
2104                 return 0;
2105         }
2106         int stamp = --mergeStamp;
2107         btAlignedObjectArray<Vertex*> stack;
2108         vertexList->copy = stamp;
2109         stack.push_back(vertexList);
2110         btAlignedObjectArray<Face*> faces;
2111
2112         Point32 ref = vertexList->point;
2113         Int128 hullCenterX(0, 0);
2114         Int128 hullCenterY(0, 0);
2115         Int128 hullCenterZ(0, 0);
2116         Int128 volume(0, 0);
2117
2118         while (stack.size() > 0)
2119         {
2120                 Vertex* v = stack[stack.size() - 1];
2121                 stack.pop_back();
2122                 Edge* e = v->edges;
2123                 if (e)
2124                 {
2125                         do
2126                         {
2127                                 if (e->target->copy != stamp)
2128                                 {
2129                                         e->target->copy = stamp;
2130                                         stack.push_back(e->target);
2131                                 }
2132                                 if (e->copy != stamp)
2133                                 {
2134                                         Face* face = facePool.newObject();
2135                                         face->init(e->target, e->reverse->prev->target, v);
2136                                         faces.push_back(face);
2137                                         Edge* f = e;
2138
2139                                         Vertex* a = NULL;
2140                                         Vertex* b = NULL;
2141                                         do
2142                                         {
2143                                                 if (a && b)
2144                                                 {
2145                                                         int64_t vol = (v->point - ref).dot((a->point - ref).cross(b->point - ref));
2146                                                         btAssert(vol >= 0);
2147                                                         Point32 c = v->point + a->point + b->point + ref;
2148                                                         hullCenterX += vol * c.x;
2149                                                         hullCenterY += vol * c.y;
2150                                                         hullCenterZ += vol * c.z;
2151                                                         volume += vol;
2152                                                 }
2153
2154                                                 btAssert(f->copy != stamp);
2155                                                 f->copy = stamp;
2156                                                 f->face = face;
2157
2158                                                 a = b;
2159                                                 b = f->target;
2160
2161                                                 f = f->reverse->prev;
2162                                         } while (f != e);
2163                                 }
2164                                 e = e->next;
2165                         } while (e != v->edges);
2166                 }
2167         }
2168
2169         if (volume.getSign() <= 0)
2170         {
2171                 return 0;
2172         }
2173
2174         btVector3 hullCenter;
2175         hullCenter[medAxis] = hullCenterX.toScalar();
2176         hullCenter[maxAxis] = hullCenterY.toScalar();
2177         hullCenter[minAxis] = hullCenterZ.toScalar();
2178         hullCenter /= 4 * volume.toScalar();
2179         hullCenter *= scaling;
2180
2181         int faceCount = faces.size();
2182
2183         if (clampAmount > 0)
2184         {
2185                 btScalar minDist = SIMD_INFINITY;
2186                 for (int i = 0; i < faceCount; i++)
2187                 {
2188                         btVector3 normal = getBtNormal(faces[i]);
2189                         btScalar dist = normal.dot(toBtVector(faces[i]->origin) - hullCenter);
2190                         if (dist < minDist)
2191                         {
2192                                 minDist = dist;
2193                         }
2194                 }
2195
2196                 if (minDist <= 0)
2197                 {
2198                         return 0;
2199                 }
2200
2201                 amount = btMin(amount, minDist * clampAmount);
2202         }
2203
2204         unsigned int seed = 243703;
2205         for (int i = 0; i < faceCount; i++, seed = 1664525 * seed + 1013904223)
2206         {
2207                 btSwap(faces[i], faces[seed % faceCount]);
2208         }
2209
2210         for (int i = 0; i < faceCount; i++)
2211         {
2212                 if (!shiftFace(faces[i], amount, stack))
2213                 {
2214                         return -amount;
2215                 }
2216         }
2217
2218         return amount;
2219 }
2220
2221 bool btConvexHullInternal::shiftFace(Face* face, btScalar amount, btAlignedObjectArray<Vertex*> stack)
2222 {
2223         btVector3 origShift = getBtNormal(face) * -amount;
2224         if (scaling[0] != 0)
2225         {
2226                 origShift[0] /= scaling[0];
2227         }
2228         if (scaling[1] != 0)
2229         {
2230                 origShift[1] /= scaling[1];
2231         }
2232         if (scaling[2] != 0)
2233         {
2234                 origShift[2] /= scaling[2];
2235         }
2236         Point32 shift((int32_t)origShift[medAxis], (int32_t)origShift[maxAxis], (int32_t)origShift[minAxis]);
2237         if (shift.isZero())
2238         {
2239                 return true;
2240         }
2241         Point64 normal = face->getNormal();
2242 #ifdef DEBUG_CONVEX_HULL
2243         printf("\nShrinking face (%d %d %d) (%d %d %d) (%d %d %d) by (%d %d %d)\n",
2244                    face->origin.x, face->origin.y, face->origin.z, face->dir0.x, face->dir0.y, face->dir0.z, face->dir1.x, face->dir1.y, face->dir1.z, shift.x, shift.y, shift.z);
2245 #endif
2246         int64_t origDot = face->origin.dot(normal);
2247         Point32 shiftedOrigin = face->origin + shift;
2248         int64_t shiftedDot = shiftedOrigin.dot(normal);
2249         btAssert(shiftedDot <= origDot);
2250         if (shiftedDot >= origDot)
2251         {
2252                 return false;
2253         }
2254
2255         Edge* intersection = NULL;
2256
2257         Edge* startEdge = face->nearbyVertex->edges;
2258 #ifdef DEBUG_CONVEX_HULL
2259         printf("Start edge is ");
2260         startEdge->print();
2261         printf(", normal is (%lld %lld %lld), shifted dot is %lld\n", normal.x, normal.y, normal.z, shiftedDot);
2262 #endif
2263         Rational128 optDot = face->nearbyVertex->dot(normal);
2264         int cmp = optDot.compare(shiftedDot);
2265 #ifdef SHOW_ITERATIONS
2266         int n = 0;
2267 #endif
2268         if (cmp >= 0)
2269         {
2270                 Edge* e = startEdge;
2271                 do
2272                 {
2273 #ifdef SHOW_ITERATIONS
2274                         n++;
2275 #endif
2276                         Rational128 dot = e->target->dot(normal);
2277                         btAssert(dot.compare(origDot) <= 0);
2278 #ifdef DEBUG_CONVEX_HULL
2279                         printf("Moving downwards, edge is ");
2280                         e->print();
2281                         printf(", dot is %f (%f %lld)\n", (float)dot.toScalar(), (float)optDot.toScalar(), shiftedDot);
2282 #endif
2283                         if (dot.compare(optDot) < 0)
2284                         {
2285                                 int c = dot.compare(shiftedDot);
2286                                 optDot = dot;
2287                                 e = e->reverse;
2288                                 startEdge = e;
2289                                 if (c < 0)
2290                                 {
2291                                         intersection = e;
2292                                         break;
2293                                 }
2294                                 cmp = c;
2295                         }
2296                         e = e->prev;
2297                 } while (e != startEdge);
2298
2299                 if (!intersection)
2300                 {
2301                         return false;
2302                 }
2303         }
2304         else
2305         {
2306                 Edge* e = startEdge;
2307                 do
2308                 {
2309 #ifdef SHOW_ITERATIONS
2310                         n++;
2311 #endif
2312                         Rational128 dot = e->target->dot(normal);
2313                         btAssert(dot.compare(origDot) <= 0);
2314 #ifdef DEBUG_CONVEX_HULL
2315                         printf("Moving upwards, edge is ");
2316                         e->print();
2317                         printf(", dot is %f (%f %lld)\n", (float)dot.toScalar(), (float)optDot.toScalar(), shiftedDot);
2318 #endif
2319                         if (dot.compare(optDot) > 0)
2320                         {
2321                                 cmp = dot.compare(shiftedDot);
2322                                 if (cmp >= 0)
2323                                 {
2324                                         intersection = e;
2325                                         break;
2326                                 }
2327                                 optDot = dot;
2328                                 e = e->reverse;
2329                                 startEdge = e;
2330                         }
2331                         e = e->prev;
2332                 } while (e != startEdge);
2333
2334                 if (!intersection)
2335                 {
2336                         return true;
2337                 }
2338         }
2339
2340 #ifdef SHOW_ITERATIONS
2341         printf("Needed %d iterations to find initial intersection\n", n);
2342 #endif
2343
2344         if (cmp == 0)
2345         {
2346                 Edge* e = intersection->reverse->next;
2347 #ifdef SHOW_ITERATIONS
2348                 n = 0;
2349 #endif
2350                 while (e->target->dot(normal).compare(shiftedDot) <= 0)
2351                 {
2352 #ifdef SHOW_ITERATIONS
2353                         n++;
2354 #endif
2355                         e = e->next;
2356                         if (e == intersection->reverse)
2357                         {
2358                                 return true;
2359                         }
2360 #ifdef DEBUG_CONVEX_HULL
2361                         printf("Checking for outwards edge, current edge is ");
2362                         e->print();
2363                         printf("\n");
2364 #endif
2365                 }
2366 #ifdef SHOW_ITERATIONS
2367                 printf("Needed %d iterations to check for complete containment\n", n);
2368 #endif
2369         }
2370
2371         Edge* firstIntersection = NULL;
2372         Edge* faceEdge = NULL;
2373         Edge* firstFaceEdge = NULL;
2374
2375 #ifdef SHOW_ITERATIONS
2376         int m = 0;
2377 #endif
2378         while (true)
2379         {
2380 #ifdef SHOW_ITERATIONS
2381                 m++;
2382 #endif
2383 #ifdef DEBUG_CONVEX_HULL
2384                 printf("Intersecting edge is ");
2385                 intersection->print();
2386                 printf("\n");
2387 #endif
2388                 if (cmp == 0)
2389                 {
2390                         Edge* e = intersection->reverse->next;
2391                         startEdge = e;
2392 #ifdef SHOW_ITERATIONS
2393                         n = 0;
2394 #endif
2395                         while (true)
2396                         {
2397 #ifdef SHOW_ITERATIONS
2398                                 n++;
2399 #endif
2400                                 if (e->target->dot(normal).compare(shiftedDot) >= 0)
2401                                 {
2402                                         break;
2403                                 }
2404                                 intersection = e->reverse;
2405                                 e = e->next;
2406                                 if (e == startEdge)
2407                                 {
2408                                         return true;
2409                                 }
2410                         }
2411 #ifdef SHOW_ITERATIONS
2412                         printf("Needed %d iterations to advance intersection\n", n);
2413 #endif
2414                 }
2415
2416 #ifdef DEBUG_CONVEX_HULL
2417                 printf("Advanced intersecting edge to ");
2418                 intersection->print();
2419                 printf(", cmp = %d\n", cmp);
2420 #endif
2421
2422                 if (!firstIntersection)
2423                 {
2424                         firstIntersection = intersection;
2425                 }
2426                 else if (intersection == firstIntersection)
2427                 {
2428                         break;
2429                 }
2430
2431                 int prevCmp = cmp;
2432                 Edge* prevIntersection = intersection;
2433                 Edge* prevFaceEdge = faceEdge;
2434
2435                 Edge* e = intersection->reverse;
2436 #ifdef SHOW_ITERATIONS
2437                 n = 0;
2438 #endif
2439                 while (true)
2440                 {
2441 #ifdef SHOW_ITERATIONS
2442                         n++;
2443 #endif
2444                         e = e->reverse->prev;
2445                         btAssert(e != intersection->reverse);
2446                         cmp = e->target->dot(normal).compare(shiftedDot);
2447 #ifdef DEBUG_CONVEX_HULL
2448                         printf("Testing edge ");
2449                         e->print();
2450                         printf(" -> cmp = %d\n", cmp);
2451 #endif
2452                         if (cmp >= 0)
2453                         {
2454                                 intersection = e;
2455                                 break;
2456                         }
2457                 }
2458 #ifdef SHOW_ITERATIONS
2459                 printf("Needed %d iterations to find other intersection of face\n", n);
2460 #endif
2461
2462                 if (cmp > 0)
2463                 {
2464                         Vertex* removed = intersection->target;
2465                         e = intersection->reverse;
2466                         if (e->prev == e)
2467                         {
2468                                 removed->edges = NULL;
2469                         }
2470                         else
2471                         {
2472                                 removed->edges = e->prev;
2473                                 e->prev->link(e->next);
2474                                 e->link(e);
2475                         }
2476 #ifdef DEBUG_CONVEX_HULL
2477                         printf("1: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z);
2478 #endif
2479
2480                         Point64 n0 = intersection->face->getNormal();
2481                         Point64 n1 = intersection->reverse->face->getNormal();
2482                         int64_t m00 = face->dir0.dot(n0);
2483                         int64_t m01 = face->dir1.dot(n0);
2484                         int64_t m10 = face->dir0.dot(n1);
2485                         int64_t m11 = face->dir1.dot(n1);
2486                         int64_t r0 = (intersection->face->origin - shiftedOrigin).dot(n0);
2487                         int64_t r1 = (intersection->reverse->face->origin - shiftedOrigin).dot(n1);
2488                         Int128 det = Int128::mul(m00, m11) - Int128::mul(m01, m10);
2489                         btAssert(det.getSign() != 0);
2490                         Vertex* v = vertexPool.newObject();
2491                         v->point.index = -1;
2492                         v->copy = -1;
2493                         v->point128 = PointR128(Int128::mul(face->dir0.x * r0, m11) - Int128::mul(face->dir0.x * r1, m01) + Int128::mul(face->dir1.x * r1, m00) - Int128::mul(face->dir1.x * r0, m10) + det * shiftedOrigin.x,
2494                                                                         Int128::mul(face->dir0.y * r0, m11) - Int128::mul(face->dir0.y * r1, m01) + Int128::mul(face->dir1.y * r1, m00) - Int128::mul(face->dir1.y * r0, m10) + det * shiftedOrigin.y,
2495                                                                         Int128::mul(face->dir0.z * r0, m11) - Int128::mul(face->dir0.z * r1, m01) + Int128::mul(face->dir1.z * r1, m00) - Int128::mul(face->dir1.z * r0, m10) + det * shiftedOrigin.z,
2496                                                                         det);
2497                         v->point.x = (int32_t)v->point128.xvalue();
2498                         v->point.y = (int32_t)v->point128.yvalue();
2499                         v->point.z = (int32_t)v->point128.zvalue();
2500                         intersection->target = v;
2501                         v->edges = e;
2502
2503                         stack.push_back(v);
2504                         stack.push_back(removed);
2505                         stack.push_back(NULL);
2506                 }
2507
2508                 if (cmp || prevCmp || (prevIntersection->reverse->next->target != intersection->target))
2509                 {
2510                         faceEdge = newEdgePair(prevIntersection->target, intersection->target);
2511                         if (prevCmp == 0)
2512                         {
2513                                 faceEdge->link(prevIntersection->reverse->next);
2514                         }
2515                         if ((prevCmp == 0) || prevFaceEdge)
2516                         {
2517                                 prevIntersection->reverse->link(faceEdge);
2518                         }
2519                         if (cmp == 0)
2520                         {
2521                                 intersection->reverse->prev->link(faceEdge->reverse);
2522                         }
2523                         faceEdge->reverse->link(intersection->reverse);
2524                 }
2525                 else
2526                 {
2527                         faceEdge = prevIntersection->reverse->next;
2528                 }
2529
2530                 if (prevFaceEdge)
2531                 {
2532                         if (prevCmp > 0)
2533                         {
2534                                 faceEdge->link(prevFaceEdge->reverse);
2535                         }
2536                         else if (faceEdge != prevFaceEdge->reverse)
2537                         {
2538                                 stack.push_back(prevFaceEdge->target);
2539                                 while (faceEdge->next != prevFaceEdge->reverse)
2540                                 {
2541                                         Vertex* removed = faceEdge->next->target;
2542                                         removeEdgePair(faceEdge->next);
2543                                         stack.push_back(removed);
2544 #ifdef DEBUG_CONVEX_HULL
2545                                         printf("2: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z);
2546 #endif
2547                                 }
2548                                 stack.push_back(NULL);
2549                         }
2550                 }
2551                 faceEdge->face = face;
2552                 faceEdge->reverse->face = intersection->face;
2553
2554                 if (!firstFaceEdge)
2555                 {
2556                         firstFaceEdge = faceEdge;
2557                 }
2558         }
2559 #ifdef SHOW_ITERATIONS
2560         printf("Needed %d iterations to process all intersections\n", m);
2561 #endif
2562
2563         if (cmp > 0)
2564         {
2565                 firstFaceEdge->reverse->target = faceEdge->target;
2566                 firstIntersection->reverse->link(firstFaceEdge);
2567                 firstFaceEdge->link(faceEdge->reverse);
2568         }
2569         else if (firstFaceEdge != faceEdge->reverse)
2570         {
2571                 stack.push_back(faceEdge->target);
2572                 while (firstFaceEdge->next != faceEdge->reverse)
2573                 {
2574                         Vertex* removed = firstFaceEdge->next->target;
2575                         removeEdgePair(firstFaceEdge->next);
2576                         stack.push_back(removed);
2577 #ifdef DEBUG_CONVEX_HULL
2578                         printf("3: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z);
2579 #endif
2580                 }
2581                 stack.push_back(NULL);
2582         }
2583
2584         btAssert(stack.size() > 0);
2585         vertexList = stack[0];
2586
2587 #ifdef DEBUG_CONVEX_HULL
2588         printf("Removing part\n");
2589 #endif
2590 #ifdef SHOW_ITERATIONS
2591         n = 0;
2592 #endif
2593         int pos = 0;
2594         while (pos < stack.size())
2595         {
2596                 int end = stack.size();
2597                 while (pos < end)
2598                 {
2599                         Vertex* kept = stack[pos++];
2600 #ifdef DEBUG_CONVEX_HULL
2601                         kept->print();
2602 #endif
2603                         bool deeper = false;
2604                         Vertex* removed;
2605                         while ((removed = stack[pos++]) != NULL)
2606                         {
2607 #ifdef SHOW_ITERATIONS
2608                                 n++;
2609 #endif
2610                                 kept->receiveNearbyFaces(removed);
2611                                 while (removed->edges)
2612                                 {
2613                                         if (!deeper)
2614                                         {
2615                                                 deeper = true;
2616                                                 stack.push_back(kept);
2617                                         }
2618                                         stack.push_back(removed->edges->target);
2619                                         removeEdgePair(removed->edges);
2620                                 }
2621                         }
2622                         if (deeper)
2623                         {
2624                                 stack.push_back(NULL);
2625                         }
2626                 }
2627         }
2628 #ifdef SHOW_ITERATIONS
2629         printf("Needed %d iterations to remove part\n", n);
2630 #endif
2631
2632         stack.resize(0);
2633         face->origin = shiftedOrigin;
2634
2635         return true;
2636 }
2637
2638 static int getVertexCopy(btConvexHullInternal::Vertex* vertex, btAlignedObjectArray<btConvexHullInternal::Vertex*>& vertices)
2639 {
2640         int index = vertex->copy;
2641         if (index < 0)
2642         {
2643                 index = vertices.size();
2644                 vertex->copy = index;
2645                 vertices.push_back(vertex);
2646 #ifdef DEBUG_CONVEX_HULL
2647                 printf("Vertex %d gets index *%d\n", vertex->point.index, index);
2648 #endif
2649         }
2650         return index;
2651 }
2652
2653 btScalar btConvexHullComputer::compute(const void* coords, bool doubleCoords, int stride, int count, btScalar shrink, btScalar shrinkClamp)
2654 {
2655         if (count <= 0)
2656         {
2657                 vertices.clear();
2658                 edges.clear();
2659                 faces.clear();
2660                 return 0;
2661         }
2662
2663         btConvexHullInternal hull;
2664         hull.compute(coords, doubleCoords, stride, count);
2665
2666         btScalar shift = 0;
2667         if ((shrink > 0) && ((shift = hull.shrink(shrink, shrinkClamp)) < 0))
2668         {
2669                 vertices.clear();
2670                 edges.clear();
2671                 faces.clear();
2672                 return shift;
2673         }
2674
2675         vertices.resize(0);
2676         original_vertex_index.resize(0);
2677         edges.resize(0);
2678         faces.resize(0);
2679
2680         btAlignedObjectArray<btConvexHullInternal::Vertex*> oldVertices;
2681         getVertexCopy(hull.vertexList, oldVertices);
2682         int copied = 0;
2683         while (copied < oldVertices.size())
2684         {
2685                 btConvexHullInternal::Vertex* v = oldVertices[copied];
2686                 vertices.push_back(hull.getCoordinates(v));
2687                 original_vertex_index.push_back(v->point.index);
2688                 btConvexHullInternal::Edge* firstEdge = v->edges;
2689                 if (firstEdge)
2690                 {
2691                         int firstCopy = -1;
2692                         int prevCopy = -1;
2693                         btConvexHullInternal::Edge* e = firstEdge;
2694                         do
2695                         {
2696                                 if (e->copy < 0)
2697                                 {
2698                                         int s = edges.size();
2699                                         edges.push_back(Edge());
2700                                         edges.push_back(Edge());
2701                                         Edge* c = &edges[s];
2702                                         Edge* r = &edges[s + 1];
2703                                         e->copy = s;
2704                                         e->reverse->copy = s + 1;
2705                                         c->reverse = 1;
2706                                         r->reverse = -1;
2707                                         c->targetVertex = getVertexCopy(e->target, oldVertices);
2708                                         r->targetVertex = copied;
2709 #ifdef DEBUG_CONVEX_HULL
2710                                         printf("      CREATE: Vertex *%d has edge to *%d\n", copied, c->getTargetVertex());
2711 #endif
2712                                 }
2713                                 if (prevCopy >= 0)
2714                                 {
2715                                         edges[e->copy].next = prevCopy - e->copy;
2716                                 }
2717                                 else
2718                                 {
2719                                         firstCopy = e->copy;
2720                                 }
2721                                 prevCopy = e->copy;
2722                                 e = e->next;
2723                         } while (e != firstEdge);
2724                         edges[firstCopy].next = prevCopy - firstCopy;
2725                 }
2726                 copied++;
2727         }
2728
2729         for (int i = 0; i < copied; i++)
2730         {
2731                 btConvexHullInternal::Vertex* v = oldVertices[i];
2732                 btConvexHullInternal::Edge* firstEdge = v->edges;
2733                 if (firstEdge)
2734                 {
2735                         btConvexHullInternal::Edge* e = firstEdge;
2736                         do
2737                         {
2738                                 if (e->copy >= 0)
2739                                 {
2740 #ifdef DEBUG_CONVEX_HULL
2741                                         printf("Vertex *%d has edge to *%d\n", i, edges[e->copy].getTargetVertex());
2742 #endif
2743                                         faces.push_back(e->copy);
2744                                         btConvexHullInternal::Edge* f = e;
2745                                         do
2746                                         {
2747 #ifdef DEBUG_CONVEX_HULL
2748                                                 printf("   Face *%d\n", edges[f->copy].getTargetVertex());
2749 #endif
2750                                                 f->copy = -1;
2751                                                 f = f->reverse->prev;
2752                                         } while (f != e);
2753                                 }
2754                                 e = e->next;
2755                         } while (e != firstEdge);
2756                 }
2757         }
2758
2759         return shift;
2760 }