Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/trunk/src/external/bullet/LinearMath/btConvexHullComputer.cpp @ 8393

Last change on this file since 8393 was 8393, checked in by rgrieder, 13 years ago

Updated Bullet from v2.77 to v2.78.
(I'm not going to make a branch for that since the update from 2.74 to 2.77 hasn't been tested that much either).

You will HAVE to do a complete RECOMPILE! I tested with MSVC and MinGW and they both threw linker errors at me.

  • Property svn:eol-style set to native
File size: 56.0 KB
Line 
1/*
2Copyright (c) 2011 Ole Kniemeyer, MAXON, www.maxon.net
3
4This software is provided 'as-is', without any express or implied warranty.
5In no event will the authors be held liable for any damages arising from the use of this software.
6Permission is granted to anyone to use this software for any purpose,
7including commercial applications, and to alter it and redistribute it freely,
8subject to the following restrictions:
9
101. 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.
112. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
123. 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
52class 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 numerator;
276                                uint64_t 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                                                this->numerator = (uint64_t) numerator;
286                                        }
287                                        else if (numerator < 0)
288                                        {
289                                                sign = -1;
290                                                this->numerator = (uint64_t) -numerator;
291                                        }
292                                        else
293                                        {
294                                                sign = 0;
295                                                this->numerator = 0;
296                                        }
297                                        if (denominator > 0)
298                                        {
299                                                this->denominator = (uint64_t) denominator;
300                                        }
301                                        else if (denominator < 0)
302                                        {
303                                                sign = -sign;
304                                                this->denominator = (uint64_t) -denominator;
305                                        }
306                                        else
307                                        {
308                                                this->denominator = 0;
309                                        }
310                                }
311                               
312                                bool isNegativeInfinity() const
313                                {
314                                        return (sign < 0) && (denominator == 0);
315                                }
316                               
317                                bool isNaN() const
318                                {
319                                        return (sign == 0) && (denominator == 0);
320                                }
321                               
322                                int compare(const Rational64& b) const;
323                               
324                                btScalar toScalar() const
325                                {
326                                        return sign * ((denominator == 0) ? SIMD_INFINITY : (btScalar) numerator / 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
838btConvexHullInternal::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
852btConvexHullInternal::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
879btConvexHullInternal::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
896int 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(numerator, b.denominator).ucmp(Int128::mul(denominator, b.numerator));
936
937#endif
938}
939
940int 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
967int 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
998btConvexHullInternal::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
1019bool 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
1204void 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
1321void 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
1358void 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
1385btConvexHullInternal::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
1411btConvexHullInternal::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
1463void 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
1647void 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
1935static bool pointCmp(const btConvexHullInternal::Point32& p, const btConvexHullInternal::Point32& q)
1936{
1937        return (p.y < q.y) || ((p.y == q.y) && ((p.x < q.x) || ((p.x == q.x) && (p.z < q.z))));
1938}
1939
1940void btConvexHullInternal::compute(const void* coords, bool doubleCoords, int stride, int count)
1941{
1942        btVector3 min(btScalar(1e30), btScalar(1e30), btScalar(1e30)), max(btScalar(-1e30), btScalar(-1e30), btScalar(-1e30));
1943        const char* ptr = (const char*) coords;
1944        if (doubleCoords)
1945        {
1946                for (int i = 0; i < count; i++)
1947                {
1948                        const double* v = (const double*) ptr;
1949                        btVector3 p((btScalar) v[0], (btScalar) v[1], (btScalar) v[2]);
1950                        ptr += stride;
1951                        min.setMin(p);
1952                        max.setMax(p);
1953                }
1954        }
1955        else
1956        {
1957                for (int i = 0; i < count; i++)
1958                {
1959                        const float* v = (const float*) ptr;
1960                        btVector3 p(v[0], v[1], v[2]);
1961                        ptr += stride;
1962                        min.setMin(p);
1963                        max.setMax(p);
1964                }
1965        }
1966
1967        btVector3 s = max - min;
1968        maxAxis = s.maxAxis();
1969        minAxis = s.minAxis();
1970        if (minAxis == maxAxis)
1971        {
1972                minAxis = (maxAxis + 1) % 3;
1973        }
1974        medAxis = 3 - maxAxis - minAxis;
1975
1976        s /= btScalar(10216);
1977
1978        scaling = s;
1979        if (s[0] > 0)
1980        {
1981                s[0] = btScalar(1) / s[0];
1982        }
1983        if (s[1] > 0)
1984        {
1985                s[1] = btScalar(1) / s[1];
1986        }
1987        if (s[2] > 0)
1988        {
1989                s[2] = btScalar(1) / s[2];
1990        }
1991
1992        center = (min + max) * btScalar(0.5);
1993
1994        btAlignedObjectArray<Point32> points;
1995        points.resize(count);
1996        ptr = (const char*) coords;
1997        if (doubleCoords)
1998        {
1999                for (int i = 0; i < count; i++)
2000                {
2001                        const double* v = (const double*) ptr;
2002                        btVector3 p((btScalar) v[0], (btScalar) v[1], (btScalar) v[2]);
2003                        ptr += stride;
2004                        p = (p - center) * s;
2005                        points[i].x = (int32_t) p[medAxis];
2006                        points[i].y = (int32_t) p[maxAxis];
2007                        points[i].z = (int32_t) p[minAxis];
2008                        points[i].index = i;
2009                }
2010        }
2011        else
2012        {
2013                for (int i = 0; i < count; i++)
2014                {
2015                        const float* v = (const float*) ptr;
2016                        btVector3 p(v[0], v[1], v[2]);
2017                        ptr += stride;
2018                        p = (p - center) * s;
2019                        points[i].x = (int32_t) p[medAxis];
2020                        points[i].y = (int32_t) p[maxAxis];
2021                        points[i].z = (int32_t) p[minAxis];
2022                        points[i].index = i;
2023                }
2024        }
2025        points.quickSort(pointCmp);
2026
2027        vertexPool.reset();
2028        vertexPool.setArraySize(count);
2029        originalVertices.resize(count);
2030        for (int i = 0; i < count; i++)
2031        {
2032                Vertex* v = vertexPool.newObject();
2033                v->edges = NULL;
2034                v->point = points[i];
2035                v->copy = -1;
2036                originalVertices[i] = v;
2037        }
2038
2039        points.clear();
2040
2041        edgePool.reset();
2042        edgePool.setArraySize(6 * count);
2043
2044        usedEdgePairs = 0;
2045        maxUsedEdgePairs = 0;
2046
2047        mergeStamp = -3;
2048
2049        IntermediateHull hull;
2050        computeInternal(0, count, hull);
2051        vertexList = hull.minXy;
2052#ifdef DEBUG_CONVEX_HULL
2053        printf("max. edges %d (3v = %d)", maxUsedEdgePairs, 3 * count);
2054#endif
2055}
2056
2057btVector3 btConvexHullInternal::toBtVector(const Point32& v)
2058{
2059        btVector3 p;
2060        p[medAxis] = btScalar(v.x);
2061        p[maxAxis] = btScalar(v.y);
2062        p[minAxis] = btScalar(v.z);
2063        return p * scaling;
2064}
2065
2066btVector3 btConvexHullInternal::getBtNormal(Face* face)
2067{
2068        btVector3 normal = toBtVector(face->dir0).cross(toBtVector(face->dir1));
2069        normal /= ((medAxis + 1 == maxAxis) || (medAxis - 2 == maxAxis)) ? normal.length() : -normal.length();
2070        return normal;
2071}
2072
2073btVector3 btConvexHullInternal::getCoordinates(const Vertex* v)
2074{
2075        btVector3 p;
2076        p[medAxis] = v->xvalue();
2077        p[maxAxis] = v->yvalue();
2078        p[minAxis] = v->zvalue();
2079        return p * scaling + center;
2080}
2081
2082btScalar btConvexHullInternal::shrink(btScalar amount, btScalar clampAmount)
2083{
2084        if (!vertexList)
2085        {
2086                return 0;
2087        }
2088        int stamp = --mergeStamp;
2089        btAlignedObjectArray<Vertex*> stack;
2090        vertexList->copy = stamp;
2091        stack.push_back(vertexList);
2092        btAlignedObjectArray<Face*> faces;
2093
2094        Point32 ref = vertexList->point;
2095        Int128 hullCenterX(0, 0);
2096        Int128 hullCenterY(0, 0);
2097        Int128 hullCenterZ(0, 0);
2098        Int128 volume(0, 0);
2099
2100        while (stack.size() > 0)
2101        {
2102                Vertex* v = stack[stack.size() - 1];
2103                stack.pop_back();
2104                Edge* e = v->edges;
2105                if (e)
2106                {
2107                        do
2108                        {
2109                                if (e->target->copy != stamp)
2110                                {
2111                                        e->target->copy = stamp;
2112                                        stack.push_back(e->target);
2113                                }
2114                                if (e->copy != stamp)
2115                                {
2116                                        Face* face = facePool.newObject();
2117                                        face->init(e->target, e->reverse->prev->target, v);
2118                                        faces.push_back(face);
2119                                        Edge* f = e;
2120
2121                                        Vertex* a = NULL;
2122                                        Vertex* b = NULL;
2123                                        do
2124                                        {
2125                                                if (a && b)
2126                                                {
2127                                                        int64_t vol = (v->point - ref).dot((a->point - ref).cross(b->point - ref));
2128                                                        btAssert(vol >= 0);
2129                                                        Point32 c = v->point + a->point + b->point + ref;
2130                                                        hullCenterX += vol * c.x;
2131                                                        hullCenterY += vol * c.y;
2132                                                        hullCenterZ += vol * c.z;
2133                                                        volume += vol;
2134                                                }
2135
2136                                                btAssert(f->copy != stamp);
2137                                                f->copy = stamp;
2138                                                f->face = face;
2139
2140                                                a = b;
2141                                                b = f->target;
2142
2143                                                f = f->reverse->prev;
2144                                        } while (f != e);
2145                                }
2146                                e = e->next;
2147                        } while (e != v->edges);
2148                }
2149        }
2150
2151        if (volume.getSign() <= 0)
2152        {
2153                return 0;
2154        }
2155
2156        btVector3 hullCenter;
2157        hullCenter[medAxis] = hullCenterX.toScalar();
2158        hullCenter[maxAxis] = hullCenterY.toScalar();
2159        hullCenter[minAxis] = hullCenterZ.toScalar();
2160        hullCenter /= 4 * volume.toScalar();
2161        hullCenter *= scaling;
2162
2163        int faceCount = faces.size();
2164
2165        if (clampAmount > 0)
2166        {
2167                btScalar minDist = SIMD_INFINITY;
2168                for (int i = 0; i < faceCount; i++)
2169                {
2170                        btVector3 normal = getBtNormal(faces[i]);
2171                        btScalar dist = normal.dot(toBtVector(faces[i]->origin) - hullCenter);
2172                        if (dist < minDist)
2173                        {
2174                                minDist = dist;
2175                        }
2176                }
2177               
2178                if (minDist <= 0)
2179                {
2180                        return 0;
2181                }
2182
2183                amount = btMin(amount, minDist * clampAmount);
2184        }
2185
2186        unsigned int seed = 243703;
2187        for (int i = 0; i < faceCount; i++, seed = 1664525 * seed + 1013904223)
2188        {
2189                btSwap(faces[i], faces[seed % faceCount]);
2190        }
2191
2192        for (int i = 0; i < faceCount; i++)
2193        {
2194                if (!shiftFace(faces[i], amount, stack))
2195                {
2196                        return -amount;
2197                }
2198        }
2199
2200        return amount;
2201}
2202
2203bool btConvexHullInternal::shiftFace(Face* face, btScalar amount, btAlignedObjectArray<Vertex*> stack)
2204{
2205        btVector3 origShift = getBtNormal(face) * -amount;
2206        if (scaling[0] > 0)
2207        {
2208                origShift[0] /= scaling[0];
2209        }
2210        if (scaling[1] > 0)
2211        {
2212                origShift[1] /= scaling[1];
2213        }
2214        if (scaling[2] > 0)
2215        {
2216                origShift[2] /= scaling[2];
2217        }
2218        Point32 shift((int32_t) origShift[medAxis], (int32_t) origShift[maxAxis], (int32_t) origShift[minAxis]);
2219        if (shift.isZero())
2220        {
2221                return true;
2222        }
2223        Point64 normal = face->getNormal();
2224#ifdef DEBUG_CONVEX_HULL
2225        printf("\nShrinking face (%d %d %d) (%d %d %d) (%d %d %d) by (%d %d %d)\n",
2226                                 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);
2227#endif
2228        int64_t origDot = face->origin.dot(normal);
2229        Point32 shiftedOrigin = face->origin + shift;
2230        int64_t shiftedDot = shiftedOrigin.dot(normal);
2231        btAssert(shiftedDot <= origDot);
2232        if (shiftedDot >= origDot)
2233        {
2234                return false;
2235        }
2236
2237        Edge* intersection = NULL;
2238
2239        Edge* startEdge = face->nearbyVertex->edges;
2240#ifdef DEBUG_CONVEX_HULL
2241        printf("Start edge is ");
2242        startEdge->print();
2243        printf(", normal is (%lld %lld %lld), shifted dot is %lld\n", normal.x, normal.y, normal.z, shiftedDot);
2244#endif
2245        Rational128 optDot = face->nearbyVertex->dot(normal);
2246        int cmp = optDot.compare(shiftedDot);
2247#ifdef SHOW_ITERATIONS
2248        int n = 0;
2249#endif
2250        if (cmp >= 0)
2251        {
2252                Edge* e = startEdge;
2253                do
2254                {
2255#ifdef SHOW_ITERATIONS
2256                        n++;
2257#endif
2258                        Rational128 dot = e->target->dot(normal);
2259                        btAssert(dot.compare(origDot) <= 0);
2260#ifdef DEBUG_CONVEX_HULL
2261                        printf("Moving downwards, edge is ");
2262                        e->print();
2263                        printf(", dot is %f (%f %lld)\n", (float) dot.toScalar(), (float) optDot.toScalar(), shiftedDot);
2264#endif
2265                        if (dot.compare(optDot) < 0)
2266                        {
2267                                int c = dot.compare(shiftedDot);
2268                                optDot = dot;
2269                                e = e->reverse;
2270                                startEdge = e;
2271                                if (c < 0)
2272                                {
2273                                        intersection = e;
2274                                        break;
2275                                }
2276                                cmp = c;
2277                        }
2278                        e = e->prev;
2279                } while (e != startEdge);
2280
2281                if (!intersection)
2282                {
2283                        return false;
2284                }
2285        }
2286        else
2287        {
2288                Edge* e = startEdge;
2289                do
2290                {
2291#ifdef SHOW_ITERATIONS
2292                        n++;
2293#endif
2294                        Rational128 dot = e->target->dot(normal);
2295                        btAssert(dot.compare(origDot) <= 0);
2296#ifdef DEBUG_CONVEX_HULL
2297                        printf("Moving upwards, edge is ");
2298                        e->print();
2299                        printf(", dot is %f (%f %lld)\n", (float) dot.toScalar(), (float) optDot.toScalar(), shiftedDot);
2300#endif
2301                        if (dot.compare(optDot) > 0)
2302                        {
2303                                cmp = dot.compare(shiftedDot);
2304                                if (cmp >= 0)
2305                                {
2306                                        intersection = e;
2307                                        break;
2308                                }
2309                                optDot = dot;
2310                                e = e->reverse;
2311                                startEdge = e;
2312                        }
2313                        e = e->prev;
2314                } while (e != startEdge);
2315               
2316                if (!intersection)
2317                {
2318                        return true;
2319                }
2320        }
2321
2322#ifdef SHOW_ITERATIONS
2323        printf("Needed %d iterations to find initial intersection\n", n);
2324#endif
2325
2326        if (cmp == 0)
2327        {
2328                Edge* e = intersection->reverse->next;
2329#ifdef SHOW_ITERATIONS
2330                n = 0;
2331#endif
2332                while (e->target->dot(normal).compare(shiftedDot) <= 0)
2333                {
2334#ifdef SHOW_ITERATIONS
2335                        n++;
2336#endif
2337                        e = e->next;
2338                        if (e == intersection->reverse)
2339                        {
2340                                return true;
2341                        }
2342#ifdef DEBUG_CONVEX_HULL
2343                        printf("Checking for outwards edge, current edge is ");
2344                        e->print();
2345                        printf("\n");
2346#endif
2347                }
2348#ifdef SHOW_ITERATIONS
2349                printf("Needed %d iterations to check for complete containment\n", n);
2350#endif
2351        }
2352       
2353        Edge* firstIntersection = NULL;
2354        Edge* faceEdge = NULL;
2355        Edge* firstFaceEdge = NULL;
2356
2357#ifdef SHOW_ITERATIONS
2358        int m = 0;
2359#endif
2360        while (true)
2361        {
2362#ifdef SHOW_ITERATIONS
2363                m++;
2364#endif
2365#ifdef DEBUG_CONVEX_HULL
2366                printf("Intersecting edge is ");
2367                intersection->print();
2368                printf("\n");
2369#endif
2370                if (cmp == 0)
2371                {
2372                        Edge* e = intersection->reverse->next;
2373                        startEdge = e;
2374#ifdef SHOW_ITERATIONS
2375                        n = 0;
2376#endif
2377                        while (true)
2378                        {
2379#ifdef SHOW_ITERATIONS
2380                                n++;
2381#endif
2382                                if (e->target->dot(normal).compare(shiftedDot) >= 0)
2383                                {
2384                                        break;
2385                                }
2386                                intersection = e->reverse;
2387                                e = e->next;
2388                                if (e == startEdge)
2389                                {
2390                                        return true;
2391                                }
2392                        }
2393#ifdef SHOW_ITERATIONS
2394                        printf("Needed %d iterations to advance intersection\n", n);
2395#endif
2396                }
2397
2398#ifdef DEBUG_CONVEX_HULL
2399                printf("Advanced intersecting edge to ");
2400                intersection->print();
2401                printf(", cmp = %d\n", cmp);
2402#endif
2403
2404                if (!firstIntersection)
2405                {
2406                        firstIntersection = intersection;
2407                }
2408                else if (intersection == firstIntersection)
2409                {
2410                        break;
2411                }
2412
2413                int prevCmp = cmp;
2414                Edge* prevIntersection = intersection;
2415                Edge* prevFaceEdge = faceEdge;
2416
2417                Edge* e = intersection->reverse;
2418#ifdef SHOW_ITERATIONS
2419                n = 0;
2420#endif
2421                while (true)
2422                {
2423#ifdef SHOW_ITERATIONS
2424                        n++;
2425#endif
2426                        e = e->reverse->prev;
2427                        btAssert(e != intersection->reverse);
2428                        cmp = e->target->dot(normal).compare(shiftedDot);
2429#ifdef DEBUG_CONVEX_HULL
2430                        printf("Testing edge ");
2431                        e->print();
2432                        printf(" -> cmp = %d\n", cmp);
2433#endif
2434                        if (cmp >= 0)
2435                        {
2436                                intersection = e;
2437                                break;
2438                        }
2439                }
2440#ifdef SHOW_ITERATIONS
2441                printf("Needed %d iterations to find other intersection of face\n", n);
2442#endif
2443
2444                if (cmp > 0)
2445                {
2446                        Vertex* removed = intersection->target;
2447                        e = intersection->reverse;
2448                        if (e->prev == e)
2449                        {
2450                                removed->edges = NULL;
2451                        }
2452                        else
2453                        {
2454                                removed->edges = e->prev;
2455                                e->prev->link(e->next);
2456                                e->link(e);
2457                        }
2458#ifdef DEBUG_CONVEX_HULL
2459                        printf("1: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z);
2460#endif
2461                       
2462                        Point64 n0 = intersection->face->getNormal();
2463                        Point64 n1 = intersection->reverse->face->getNormal();
2464                        int64_t m00 = face->dir0.dot(n0);
2465                        int64_t m01 = face->dir1.dot(n0);
2466                        int64_t m10 = face->dir0.dot(n1);
2467                        int64_t m11 = face->dir1.dot(n1);
2468                        int64_t r0 = (intersection->face->origin - shiftedOrigin).dot(n0);
2469                        int64_t r1 = (intersection->reverse->face->origin - shiftedOrigin).dot(n1);
2470                        Int128 det = Int128::mul(m00, m11) - Int128::mul(m01, m10);
2471                        btAssert(det.getSign() != 0);
2472                        Vertex* v = vertexPool.newObject();
2473                        v->point.index = -1;
2474                        v->copy = -1;
2475                        v->point128 = PointR128(Int128::mul(face->dir0.x * r0, m11) - Int128::mul(face->dir0.x * r1, m01)
2476                                                                                                                        + Int128::mul(face->dir1.x * r1, m00) - Int128::mul(face->dir1.x * r0, m10) + det * shiftedOrigin.x,
2477                                                                                                                        Int128::mul(face->dir0.y * r0, m11) - Int128::mul(face->dir0.y * r1, m01)
2478                                                                                                                        + Int128::mul(face->dir1.y * r1, m00) - Int128::mul(face->dir1.y * r0, m10) + det * shiftedOrigin.y,
2479                                                                                                                        Int128::mul(face->dir0.z * r0, m11) - Int128::mul(face->dir0.z * r1, m01)
2480                                                                                                                        + Int128::mul(face->dir1.z * r1, m00) - Int128::mul(face->dir1.z * r0, m10) + det * shiftedOrigin.z,
2481                                                                                                                        det);
2482                        v->point.x = (int32_t) v->point128.xvalue();
2483                        v->point.y = (int32_t) v->point128.yvalue();
2484                        v->point.z = (int32_t) v->point128.zvalue();
2485                        intersection->target = v;
2486                        v->edges = e;
2487
2488                        stack.push_back(v);
2489                        stack.push_back(removed);
2490                        stack.push_back(NULL);
2491                }
2492
2493                if (cmp || prevCmp || (prevIntersection->reverse->next->target != intersection->target))
2494                {
2495                        faceEdge = newEdgePair(prevIntersection->target, intersection->target);
2496                        if (prevCmp == 0)
2497                        {
2498                                faceEdge->link(prevIntersection->reverse->next);
2499                        }
2500                        if ((prevCmp == 0) || prevFaceEdge)
2501                        {
2502                                prevIntersection->reverse->link(faceEdge);
2503                        }
2504                        if (cmp == 0)
2505                        {
2506                                intersection->reverse->prev->link(faceEdge->reverse);
2507                        }
2508                        faceEdge->reverse->link(intersection->reverse);
2509                }
2510                else
2511                {
2512                        faceEdge = prevIntersection->reverse->next;
2513                }
2514
2515                if (prevFaceEdge)
2516                {
2517                        if (prevCmp > 0)
2518                        {
2519                                faceEdge->link(prevFaceEdge->reverse);
2520                        }
2521                        else if (faceEdge != prevFaceEdge->reverse)
2522                        {
2523                                stack.push_back(prevFaceEdge->target);
2524                                while (faceEdge->next != prevFaceEdge->reverse)
2525                                {
2526                                        Vertex* removed = faceEdge->next->target;
2527                                        removeEdgePair(faceEdge->next);
2528                                        stack.push_back(removed);
2529#ifdef DEBUG_CONVEX_HULL
2530                                        printf("2: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z);
2531#endif
2532                                }
2533                                stack.push_back(NULL);
2534                        }
2535                }
2536                faceEdge->face = face;
2537                faceEdge->reverse->face = intersection->face;
2538
2539                if (!firstFaceEdge)
2540                {
2541                        firstFaceEdge = faceEdge;
2542                }
2543        }
2544#ifdef SHOW_ITERATIONS
2545        printf("Needed %d iterations to process all intersections\n", m);
2546#endif
2547
2548        if (cmp > 0)
2549        {
2550                firstFaceEdge->reverse->target = faceEdge->target;
2551                firstIntersection->reverse->link(firstFaceEdge);
2552                firstFaceEdge->link(faceEdge->reverse);
2553        }
2554        else if (firstFaceEdge != faceEdge->reverse)
2555        {
2556                stack.push_back(faceEdge->target);
2557                while (firstFaceEdge->next != faceEdge->reverse)
2558                {
2559                        Vertex* removed = firstFaceEdge->next->target;
2560                        removeEdgePair(firstFaceEdge->next);
2561                        stack.push_back(removed);
2562#ifdef DEBUG_CONVEX_HULL
2563                        printf("3: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z);
2564#endif
2565                }
2566                stack.push_back(NULL);
2567        }
2568
2569        btAssert(stack.size() > 0);
2570        vertexList = stack[0];
2571
2572#ifdef DEBUG_CONVEX_HULL
2573        printf("Removing part\n");
2574#endif
2575#ifdef SHOW_ITERATIONS
2576        n = 0;
2577#endif
2578        int pos = 0;
2579        while (pos < stack.size())
2580        {
2581                int end = stack.size();
2582                while (pos < end)
2583                {
2584                        Vertex* kept = stack[pos++];
2585#ifdef DEBUG_CONVEX_HULL
2586                        kept->print();
2587#endif
2588                        bool deeper = false;
2589                        Vertex* removed;
2590                        while ((removed = stack[pos++]) != NULL)
2591                        {
2592#ifdef SHOW_ITERATIONS
2593                                n++;
2594#endif
2595                                kept->receiveNearbyFaces(removed);
2596                                while (removed->edges)
2597                                {
2598                                        if (!deeper)
2599                                        {
2600                                                deeper = true;
2601                                                stack.push_back(kept);
2602                                        }
2603                                        stack.push_back(removed->edges->target);
2604                                        removeEdgePair(removed->edges);
2605                                }
2606                        }
2607                        if (deeper)
2608                        {
2609                                stack.push_back(NULL);
2610                        }
2611                }
2612        }
2613#ifdef SHOW_ITERATIONS
2614        printf("Needed %d iterations to remove part\n", n);
2615#endif
2616
2617        stack.resize(0);
2618        face->origin = shiftedOrigin;
2619
2620        return true;
2621}
2622
2623
2624static int getVertexCopy(btConvexHullInternal::Vertex* vertex, btAlignedObjectArray<btConvexHullInternal::Vertex*>& vertices)
2625{
2626        int index = vertex->copy;
2627        if (index < 0)
2628        {
2629                index = vertices.size();
2630                vertex->copy = index;
2631                vertices.push_back(vertex);
2632#ifdef DEBUG_CONVEX_HULL
2633                printf("Vertex %d gets index *%d\n", vertex->point.index, index);
2634#endif
2635        }
2636        return index;
2637}
2638
2639btScalar btConvexHullComputer::compute(const void* coords, bool doubleCoords, int stride, int count, btScalar shrink, btScalar shrinkClamp)
2640{
2641        if (count <= 0)
2642        {
2643                vertices.clear();
2644                edges.clear();
2645                faces.clear();
2646                return 0;
2647        }
2648
2649        btConvexHullInternal hull;
2650        hull.compute(coords, doubleCoords, stride, count);
2651
2652        btScalar shift = 0;
2653        if ((shrink > 0) && ((shift = hull.shrink(shrink, shrinkClamp)) < 0))
2654        {
2655                vertices.clear();
2656                edges.clear();
2657                faces.clear();
2658                return shift;
2659        }
2660
2661        vertices.resize(0);
2662        edges.resize(0);
2663        faces.resize(0);
2664
2665        btAlignedObjectArray<btConvexHullInternal::Vertex*> oldVertices;
2666        getVertexCopy(hull.vertexList, oldVertices);
2667        int copied = 0;
2668        while (copied < oldVertices.size())
2669        {
2670                btConvexHullInternal::Vertex* v = oldVertices[copied];
2671                vertices.push_back(hull.getCoordinates(v));
2672                btConvexHullInternal::Edge* firstEdge = v->edges;
2673                if (firstEdge)
2674                {
2675                        int firstCopy = -1;
2676                        int prevCopy = -1;
2677                        btConvexHullInternal::Edge* e = firstEdge;
2678                        do
2679                        {
2680                                if (e->copy < 0)
2681                                {
2682                                        int s = edges.size();
2683                                        edges.push_back(Edge());
2684                                        edges.push_back(Edge());
2685                                        Edge* c = &edges[s];
2686                                        Edge* r = &edges[s + 1];
2687                                        e->copy = s;
2688                                        e->reverse->copy = s + 1;
2689                                        c->reverse = 1;
2690                                        r->reverse = -1;
2691                                        c->targetVertex = getVertexCopy(e->target, oldVertices);
2692                                        r->targetVertex = copied;
2693#ifdef DEBUG_CONVEX_HULL
2694                                        printf("      CREATE: Vertex *%d has edge to *%d\n", copied, c->getTargetVertex());
2695#endif
2696                                }
2697                                if (prevCopy >= 0)
2698                                {
2699                                        edges[e->copy].next = prevCopy - e->copy;
2700                                }
2701                                else
2702                                {
2703                                        firstCopy = e->copy;
2704                                }
2705                                prevCopy = e->copy;
2706                                e = e->next;
2707                        } while (e != firstEdge);
2708                        edges[firstCopy].next = prevCopy - firstCopy;
2709                }
2710                copied++;
2711        }
2712
2713        for (int i = 0; i < copied; i++)
2714        {
2715                btConvexHullInternal::Vertex* v = oldVertices[i];
2716                btConvexHullInternal::Edge* firstEdge = v->edges;
2717                if (firstEdge)
2718                {
2719                        btConvexHullInternal::Edge* e = firstEdge;
2720                        do
2721                        {
2722                                if (e->copy >= 0)
2723                                {
2724#ifdef DEBUG_CONVEX_HULL
2725                                        printf("Vertex *%d has edge to *%d\n", i, edges[e->copy].getTargetVertex());
2726#endif
2727                                        faces.push_back(e->copy);
2728                                        btConvexHullInternal::Edge* f = e;
2729                                        do
2730                                        {
2731#ifdef DEBUG_CONVEX_HULL
2732                                                printf("   Face *%d\n", edges[f->copy].getTargetVertex());
2733#endif
2734                                                f->copy = -1;
2735                                                f = f->reverse->prev;
2736                                        } while (f != e);
2737                                }
2738                                e = e->next;
2739                        } while (e != firstEdge);
2740                }
2741        }
2742
2743        return shift;
2744}
2745
2746
2747
2748
2749
Note: See TracBrowser for help on using the repository browser.