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