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