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