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