Bullet Collision Detection & Physics Library
btConvexHullComputer.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2011 Ole Kniemeyer, MAXON, www.maxon.net
3 
4 This software is provided 'as-is', without any express or implied warranty.
5 In no event will the authors be held liable for any damages arising from the use of this software.
6 Permission is granted to anyone to use this software for any purpose,
7 including commercial applications, and to alter it and redistribute it freely,
8 subject to the following restrictions:
9 
10 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
11 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
12 3. This notice may not be removed or altered from any source distribution.
13 */
14 
15 #include <string.h>
16 
17 #include "btConvexHullComputer.h"
18 #include "btAlignedObjectArray.h"
19 #include "btMinMax.h"
20 #include "btVector3.h"
21 
22 #ifdef __GNUC__
23  #include <stdint.h>
24 #elif defined(_MSC_VER)
25  typedef __int32 int32_t;
26  typedef __int64 int64_t;
27  typedef unsigned __int32 uint32_t;
28  typedef unsigned __int64 uint64_t;
29 #else
30  typedef int int32_t;
31  typedef long long int int64_t;
32  typedef unsigned int uint32_t;
33  typedef unsigned long long int uint64_t;
34 #endif
35 
36 
37 //The definition of USE_X86_64_ASM is moved into the build system. You can enable it manually by commenting out the following lines
38 //#if (defined(__GNUC__) && defined(__x86_64__) && !defined(__ICL)) // || (defined(__ICL) && defined(_M_X64)) bug in Intel compiler, disable inline assembly
39 // #define USE_X86_64_ASM
40 //#endif
41 
42 
43 //#define DEBUG_CONVEX_HULL
44 //#define SHOW_ITERATIONS
45 
46 #if defined(DEBUG_CONVEX_HULL) || defined(SHOW_ITERATIONS)
47  #include <stdio.h>
48 #endif
49 
50 // Convex hull implementation based on Preparata and Hong
51 // Ole Kniemeyer, MAXON Computer GmbH
53 {
54  public:
55 
56  class Point64
57  {
58  public:
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:
84  int index;
85 
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:
145 
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 
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 
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 
223  {
224  if (++low == 0)
225  {
226  ++high;
227  }
228  return *this;
229  }
230 
231  Int128 operator*(int64_t b) const;
232 
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 
273  {
274  private:
277  int sign;
278 
279  public:
280  Rational64(int64_t numerator, int64_t denominator)
281  {
282  if (numerator > 0)
283  {
284  sign = 1;
285  m_numerator = (uint64_t) numerator;
286  }
287  else if (numerator < 0)
288  {
289  sign = -1;
290  m_numerator = (uint64_t) -numerator;
291  }
292  else
293  {
294  sign = 0;
295  m_numerator = 0;
296  }
297  if (denominator > 0)
298  {
299  m_denominator = (uint64_t) denominator;
300  }
301  else if (denominator < 0)
302  {
303  sign = -sign;
304  m_denominator = (uint64_t) -denominator;
305  }
306  else
307  {
308  m_denominator = 0;
309  }
310  }
311 
312  bool isNegativeInfinity() const
313  {
314  return (sign < 0) && (m_denominator == 0);
315  }
316 
317  bool isNaN() const
318  {
319  return (sign == 0) && (m_denominator == 0);
320  }
321 
322  int compare(const Rational64& b) const;
323 
325  {
327  }
328  };
329 
330 
332  {
333  private:
336  int sign;
337  bool isInt64;
338 
339  public:
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 
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 
390  {
392  }
393  };
394 
395  class PointR128
396  {
397  public:
402 
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:
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))
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 
483  {
484  if (lastNearbyFace)
485  {
487  }
488  else
489  {
491  }
492  if (src->lastNearbyFace)
493  {
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:
515  int copy;
516 
518  {
519  next = NULL;
520  prev = NULL;
521  reverse = NULL;
522  target = NULL;
523  face = NULL;
524  }
525 
526  void link(Edge* n)
527  {
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,
538  }
539 #endif
540  };
541 
542  class Face
543  {
544  public:
551 
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  {
565  }
566  else
567  {
568  a->firstNearbyFace = this;
569  }
570  a->lastNearbyFace = this;
571  }
572 
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 
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 
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 
649  {
650  public:
655 
656  IntermediateHull(): minXy(NULL), maxXy(NULL), minYx(NULL), maxYx(NULL)
657  {
658  }
659 
660  void print();
661  };
662 
664 
665  template <typename T> class PoolArray
666  {
667  private:
668  T* array;
669  int size;
670 
671  public:
673 
674  PoolArray(int size): size(size), next(NULL)
675  {
676  array = (T*) btAlignedAlloc(sizeof(T) * size, 16);
677  }
678 
680  {
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:
702 
703  public:
704  Pool(): arrays(NULL), nextArray(NULL), freeObjects(NULL), arraySize(256)
705  {
706  }
707 
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 
726  {
727  this->arraySize = arraySize;
728  }
729 
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 
767  int minAxis;
768  int medAxis;
769  int maxAxis;
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:
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 
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 
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 
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 
897 {
898  if (sign != b.sign)
899  {
900  return sign - b.sign;
901  }
902  else if (sign == 0)
903  {
904  return 0;
905  }
906 
907  // return (numerator * b.denominator > b.numerator * denominator) ? sign : (numerator * b.denominator < b.numerator * denominator) ? -sign : 0;
908 
909 #ifdef USE_X86_64_ASM
910 
911  int result;
912  int64_t tmp;
913  int64_t dummy;
914  __asm__ ("mulq %[bn]\n\t"
915  "movq %%rax, %[tmp]\n\t"
916  "movq %%rdx, %%rbx\n\t"
917  "movq %[tn], %%rax\n\t"
918  "mulq %[bd]\n\t"
919  "subq %[tmp], %%rax\n\t"
920  "sbbq %%rbx, %%rdx\n\t" // rdx:rax contains 128-bit-difference "numerator*b.denominator - b.numerator*denominator"
921  "setnsb %%bh\n\t" // bh=1 if difference is non-negative, bh=0 otherwise
922  "orq %%rdx, %%rax\n\t"
923  "setnzb %%bl\n\t" // bl=1 if difference if non-zero, bl=0 if it is zero
924  "decb %%bh\n\t" // now bx=0x0000 if difference is zero, 0xff01 if it is negative, 0x0001 if it is positive (i.e., same sign as difference)
925  "shll $16, %%ebx\n\t" // ebx has same sign as difference
926  : "=&b"(result), [tmp] "=&r"(tmp), "=a"(dummy)
927  : "a"(denominator), [bn] "g"(b.numerator), [tn] "g"(numerator), [bd] "g"(b.denominator)
928  : "%rdx", "cc" );
929  return result ? result ^ sign // if sign is +1, only bit 0 of result is inverted, which does not change the sign of result (and cannot result in zero)
930  // if sign is -1, all bits of result are inverted, which changes the sign of result (and again cannot result in zero)
931  : 0;
932 
933 #else
934 
935  return sign * Int128::mul(m_numerator, b.m_denominator).ucmp(Int128::mul(m_denominator, b.m_numerator));
936 
937 #endif
938 }
939 
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 
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 
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++;
1013  {
1015  }
1016  return e;
1017 }
1018 
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 
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
1322 {
1323  printf(" Hull\n");
1324  for (Vertex* v = minXy; v; )
1325  {
1326  printf(" ");
1327  v->print();
1328  if (v == maxXy)
1329  {
1330  printf(" maxXy");
1331  }
1332  if (v == minYx)
1333  {
1334  printf(" minYx");
1335  }
1336  if (v == maxYx)
1337  {
1338  printf(" maxYx");
1339  }
1340  if (v->next->prev != v)
1341  {
1342  printf(" Inconsistency");
1343  }
1344  printf("\n");
1345  v = v->next;
1346  if (v == minXy)
1347  {
1348  break;
1349  }
1350  }
1351  if (minXy)
1352  {
1353  minXy->copy = (minXy->copy == -1) ? -2 : -1;
1354  minXy->printGraph();
1355  }
1356 }
1357 
1358 void btConvexHullInternal::Vertex::printGraph()
1359 {
1360  print();
1361  printf("\nEdges\n");
1362  Edge* e = edges;
1363  if (e)
1364  {
1365  do
1366  {
1367  e->print();
1368  printf("\n");
1369  e = e->next;
1370  } while (e != edges);
1371  do
1372  {
1373  Vertex* v = e->target;
1374  if (v->copy != copy)
1375  {
1376  v->copy = copy;
1377  v->printGraph();
1378  }
1379  e = e->next;
1380  } while (e != edges);
1381  }
1382 }
1383 #endif
1384 
1386 {
1387  btAssert(prev->reverse->target == next->reverse->target);
1388  if (prev->next == next)
1389  {
1390  if (prev->prev == next)
1391  {
1392  Point64 n = t.cross(s);
1393  Point64 m = (*prev->target - *next->reverse->target).cross(*next->target - *next->reverse->target);
1394  btAssert(!m.isZero());
1395  int64_t dot = n.dot(m);
1396  btAssert(dot != 0);
1397  return (dot > 0) ? COUNTER_CLOCKWISE : CLOCKWISE;
1398  }
1399  return COUNTER_CLOCKWISE;
1400  }
1401  else if (prev->prev == next)
1402  {
1403  return CLOCKWISE;
1404  }
1405  else
1406  {
1407  return NONE;
1408  }
1409 }
1410 
1411 btConvexHullInternal::Edge* btConvexHullInternal::findMaxAngle(bool ccw, const Vertex* start, const Point32& s, const Point64& rxs, const Point64& sxrxs, Rational64& minCot)
1412 {
1413  Edge* minEdge = NULL;
1414 
1415 #ifdef DEBUG_CONVEX_HULL
1416  printf("find max edge for %d\n", start->point.index);
1417 #endif
1418  Edge* e = start->edges;
1419  if (e)
1420  {
1421  do
1422  {
1423  if (e->copy > mergeStamp)
1424  {
1425  Point32 t = *e->target - *start;
1426  Rational64 cot(t.dot(sxrxs), t.dot(rxs));
1427 #ifdef DEBUG_CONVEX_HULL
1428  printf(" Angle is %f (%d) for ", (float) btAtan(cot.toScalar()), (int) cot.isNaN());
1429  e->print();
1430 #endif
1431  if (cot.isNaN())
1432  {
1433  btAssert(ccw ? (t.dot(s) < 0) : (t.dot(s) > 0));
1434  }
1435  else
1436  {
1437  int cmp;
1438  if (minEdge == NULL)
1439  {
1440  minCot = cot;
1441  minEdge = e;
1442  }
1443  else if ((cmp = cot.compare(minCot)) < 0)
1444  {
1445  minCot = cot;
1446  minEdge = e;
1447  }
1448  else if ((cmp == 0) && (ccw == (getOrientation(minEdge, e, s, t) == COUNTER_CLOCKWISE)))
1449  {
1450  minEdge = e;
1451  }
1452  }
1453 #ifdef DEBUG_CONVEX_HULL
1454  printf("\n");
1455 #endif
1456  }
1457  e = e->next;
1458  } while (e != start->edges);
1459  }
1460  return minEdge;
1461 }
1462 
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 
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 
1935 {
1936  public:
1937 
1938  bool operator() ( const btConvexHullInternal::Point32& p, const btConvexHullInternal::Point32& q ) const
1939  {
1940  return (p.y < q.y) || ((p.y == q.y) && ((p.x < q.x) || ((p.x == q.x) && (p.z < q.z))));
1941  }
1942 };
1943 
1944 void btConvexHullInternal::compute(const void* coords, bool doubleCoords, int stride, int count)
1945 {
1946  btVector3 min(btScalar(1e30), btScalar(1e30), btScalar(1e30)), max(btScalar(-1e30), btScalar(-1e30), btScalar(-1e30));
1947  const char* ptr = (const char*) coords;
1948  if (doubleCoords)
1949  {
1950  for (int i = 0; i < count; i++)
1951  {
1952  const double* v = (const double*) ptr;
1953  btVector3 p((btScalar) v[0], (btScalar) v[1], (btScalar) v[2]);
1954  ptr += stride;
1955  min.setMin(p);
1956  max.setMax(p);
1957  }
1958  }
1959  else
1960  {
1961  for (int i = 0; i < count; i++)
1962  {
1963  const float* v = (const float*) ptr;
1964  btVector3 p(v[0], v[1], v[2]);
1965  ptr += stride;
1966  min.setMin(p);
1967  max.setMax(p);
1968  }
1969  }
1970 
1971  btVector3 s = max - min;
1972  maxAxis = s.maxAxis();
1973  minAxis = s.minAxis();
1974  if (minAxis == maxAxis)
1975  {
1976  minAxis = (maxAxis + 1) % 3;
1977  }
1978  medAxis = 3 - maxAxis - minAxis;
1979 
1980  s /= btScalar(10216);
1981  if (((medAxis + 1) % 3) != maxAxis)
1982  {
1983  s *= -1;
1984  }
1985  scaling = s;
1986 
1987  if (s[0] != 0)
1988  {
1989  s[0] = btScalar(1) / s[0];
1990  }
1991  if (s[1] != 0)
1992  {
1993  s[1] = btScalar(1) / s[1];
1994  }
1995  if (s[2] != 0)
1996  {
1997  s[2] = btScalar(1) / s[2];
1998  }
1999 
2000  center = (min + max) * btScalar(0.5);
2001 
2003  points.resize(count);
2004  ptr = (const char*) coords;
2005  if (doubleCoords)
2006  {
2007  for (int i = 0; i < count; i++)
2008  {
2009  const double* v = (const double*) ptr;
2010  btVector3 p((btScalar) v[0], (btScalar) v[1], (btScalar) v[2]);
2011  ptr += stride;
2012  p = (p - center) * s;
2013  points[i].x = (int32_t) p[medAxis];
2014  points[i].y = (int32_t) p[maxAxis];
2015  points[i].z = (int32_t) p[minAxis];
2016  points[i].index = i;
2017  }
2018  }
2019  else
2020  {
2021  for (int i = 0; i < count; i++)
2022  {
2023  const float* v = (const float*) ptr;
2024  btVector3 p(v[0], v[1], v[2]);
2025  ptr += stride;
2026  p = (p - center) * s;
2027  points[i].x = (int32_t) p[medAxis];
2028  points[i].y = (int32_t) p[maxAxis];
2029  points[i].z = (int32_t) p[minAxis];
2030  points[i].index = i;
2031  }
2032  }
2033  points.quickSort(pointCmp());
2034 
2035  vertexPool.reset();
2036  vertexPool.setArraySize(count);
2037  originalVertices.resize(count);
2038  for (int i = 0; i < count; i++)
2039  {
2040  Vertex* v = vertexPool.newObject();
2041  v->edges = NULL;
2042  v->point = points[i];
2043  v->copy = -1;
2044  originalVertices[i] = v;
2045  }
2046 
2047  points.clear();
2048 
2049  edgePool.reset();
2050  edgePool.setArraySize(6 * count);
2051 
2052  usedEdgePairs = 0;
2053  maxUsedEdgePairs = 0;
2054 
2055  mergeStamp = -3;
2056 
2057  IntermediateHull hull;
2058  computeInternal(0, count, hull);
2059  vertexList = hull.minXy;
2060 #ifdef DEBUG_CONVEX_HULL
2061  printf("max. edges %d (3v = %d)", maxUsedEdgePairs, 3 * count);
2062 #endif
2063 }
2064 
2066 {
2067  btVector3 p;
2068  p[medAxis] = btScalar(v.x);
2069  p[maxAxis] = btScalar(v.y);
2070  p[minAxis] = btScalar(v.z);
2071  return p * scaling;
2072 }
2073 
2075 {
2076  return toBtVector(face->dir0).cross(toBtVector(face->dir1)).normalized();
2077 }
2078 
2080 {
2081  btVector3 p;
2082  p[medAxis] = v->xvalue();
2083  p[maxAxis] = v->yvalue();
2084  p[minAxis] = v->zvalue();
2085  return p * scaling + center;
2086 }
2087 
2089 {
2090  if (!vertexList)
2091  {
2092  return 0;
2093  }
2094  int stamp = --mergeStamp;
2096  vertexList->copy = stamp;
2097  stack.push_back(vertexList);
2099 
2100  Point32 ref = vertexList->point;
2101  Int128 hullCenterX(0, 0);
2102  Int128 hullCenterY(0, 0);
2103  Int128 hullCenterZ(0, 0);
2104  Int128 volume(0, 0);
2105 
2106  while (stack.size() > 0)
2107  {
2108  Vertex* v = stack[stack.size() - 1];
2109  stack.pop_back();
2110  Edge* e = v->edges;
2111  if (e)
2112  {
2113  do
2114  {
2115  if (e->target->copy != stamp)
2116  {
2117  e->target->copy = stamp;
2118  stack.push_back(e->target);
2119  }
2120  if (e->copy != stamp)
2121  {
2122  Face* face = facePool.newObject();
2123  face->init(e->target, e->reverse->prev->target, v);
2124  faces.push_back(face);
2125  Edge* f = e;
2126 
2127  Vertex* a = NULL;
2128  Vertex* b = NULL;
2129  do
2130  {
2131  if (a && b)
2132  {
2133  int64_t vol = (v->point - ref).dot((a->point - ref).cross(b->point - ref));
2134  btAssert(vol >= 0);
2135  Point32 c = v->point + a->point + b->point + ref;
2136  hullCenterX += vol * c.x;
2137  hullCenterY += vol * c.y;
2138  hullCenterZ += vol * c.z;
2139  volume += vol;
2140  }
2141 
2142  btAssert(f->copy != stamp);
2143  f->copy = stamp;
2144  f->face = face;
2145 
2146  a = b;
2147  b = f->target;
2148 
2149  f = f->reverse->prev;
2150  } while (f != e);
2151  }
2152  e = e->next;
2153  } while (e != v->edges);
2154  }
2155  }
2156 
2157  if (volume.getSign() <= 0)
2158  {
2159  return 0;
2160  }
2161 
2162  btVector3 hullCenter;
2163  hullCenter[medAxis] = hullCenterX.toScalar();
2164  hullCenter[maxAxis] = hullCenterY.toScalar();
2165  hullCenter[minAxis] = hullCenterZ.toScalar();
2166  hullCenter /= 4 * volume.toScalar();
2167  hullCenter *= scaling;
2168 
2169  int faceCount = faces.size();
2170 
2171  if (clampAmount > 0)
2172  {
2173  btScalar minDist = SIMD_INFINITY;
2174  for (int i = 0; i < faceCount; i++)
2175  {
2176  btVector3 normal = getBtNormal(faces[i]);
2177  btScalar dist = normal.dot(toBtVector(faces[i]->origin) - hullCenter);
2178  if (dist < minDist)
2179  {
2180  minDist = dist;
2181  }
2182  }
2183 
2184  if (minDist <= 0)
2185  {
2186  return 0;
2187  }
2188 
2189  amount = btMin(amount, minDist * clampAmount);
2190  }
2191 
2192  unsigned int seed = 243703;
2193  for (int i = 0; i < faceCount; i++, seed = 1664525 * seed + 1013904223)
2194  {
2195  btSwap(faces[i], faces[seed % faceCount]);
2196  }
2197 
2198  for (int i = 0; i < faceCount; i++)
2199  {
2200  if (!shiftFace(faces[i], amount, stack))
2201  {
2202  return -amount;
2203  }
2204  }
2205 
2206  return amount;
2207 }
2208 
2210 {
2211  btVector3 origShift = getBtNormal(face) * -amount;
2212  if (scaling[0] != 0)
2213  {
2214  origShift[0] /= scaling[0];
2215  }
2216  if (scaling[1] != 0)
2217  {
2218  origShift[1] /= scaling[1];
2219  }
2220  if (scaling[2] != 0)
2221  {
2222  origShift[2] /= scaling[2];
2223  }
2224  Point32 shift((int32_t) origShift[medAxis], (int32_t) origShift[maxAxis], (int32_t) origShift[minAxis]);
2225  if (shift.isZero())
2226  {
2227  return true;
2228  }
2229  Point64 normal = face->getNormal();
2230 #ifdef DEBUG_CONVEX_HULL
2231  printf("\nShrinking face (%d %d %d) (%d %d %d) (%d %d %d) by (%d %d %d)\n",
2232  face->origin.x, face->origin.y, face->origin.z, face->dir0.x, face->dir0.y, face->dir0.z, face->dir1.x, face->dir1.y, face->dir1.z, shift.x, shift.y, shift.z);
2233 #endif
2234  int64_t origDot = face->origin.dot(normal);
2235  Point32 shiftedOrigin = face->origin + shift;
2236  int64_t shiftedDot = shiftedOrigin.dot(normal);
2237  btAssert(shiftedDot <= origDot);
2238  if (shiftedDot >= origDot)
2239  {
2240  return false;
2241  }
2242 
2243  Edge* intersection = NULL;
2244 
2245  Edge* startEdge = face->nearbyVertex->edges;
2246 #ifdef DEBUG_CONVEX_HULL
2247  printf("Start edge is ");
2248  startEdge->print();
2249  printf(", normal is (%lld %lld %lld), shifted dot is %lld\n", normal.x, normal.y, normal.z, shiftedDot);
2250 #endif
2251  Rational128 optDot = face->nearbyVertex->dot(normal);
2252  int cmp = optDot.compare(shiftedDot);
2253 #ifdef SHOW_ITERATIONS
2254  int n = 0;
2255 #endif
2256  if (cmp >= 0)
2257  {
2258  Edge* e = startEdge;
2259  do
2260  {
2261 #ifdef SHOW_ITERATIONS
2262  n++;
2263 #endif
2264  Rational128 dot = e->target->dot(normal);
2265  btAssert(dot.compare(origDot) <= 0);
2266 #ifdef DEBUG_CONVEX_HULL
2267  printf("Moving downwards, edge is ");
2268  e->print();
2269  printf(", dot is %f (%f %lld)\n", (float) dot.toScalar(), (float) optDot.toScalar(), shiftedDot);
2270 #endif
2271  if (dot.compare(optDot) < 0)
2272  {
2273  int c = dot.compare(shiftedDot);
2274  optDot = dot;
2275  e = e->reverse;
2276  startEdge = e;
2277  if (c < 0)
2278  {
2279  intersection = e;
2280  break;
2281  }
2282  cmp = c;
2283  }
2284  e = e->prev;
2285  } while (e != startEdge);
2286 
2287  if (!intersection)
2288  {
2289  return false;
2290  }
2291  }
2292  else
2293  {
2294  Edge* e = startEdge;
2295  do
2296  {
2297 #ifdef SHOW_ITERATIONS
2298  n++;
2299 #endif
2300  Rational128 dot = e->target->dot(normal);
2301  btAssert(dot.compare(origDot) <= 0);
2302 #ifdef DEBUG_CONVEX_HULL
2303  printf("Moving upwards, edge is ");
2304  e->print();
2305  printf(", dot is %f (%f %lld)\n", (float) dot.toScalar(), (float) optDot.toScalar(), shiftedDot);
2306 #endif
2307  if (dot.compare(optDot) > 0)
2308  {
2309  cmp = dot.compare(shiftedDot);
2310  if (cmp >= 0)
2311  {
2312  intersection = e;
2313  break;
2314  }
2315  optDot = dot;
2316  e = e->reverse;
2317  startEdge = e;
2318  }
2319  e = e->prev;
2320  } while (e != startEdge);
2321 
2322  if (!intersection)
2323  {
2324  return true;
2325  }
2326  }
2327 
2328 #ifdef SHOW_ITERATIONS
2329  printf("Needed %d iterations to find initial intersection\n", n);
2330 #endif
2331 
2332  if (cmp == 0)
2333  {
2334  Edge* e = intersection->reverse->next;
2335 #ifdef SHOW_ITERATIONS
2336  n = 0;
2337 #endif
2338  while (e->target->dot(normal).compare(shiftedDot) <= 0)
2339  {
2340 #ifdef SHOW_ITERATIONS
2341  n++;
2342 #endif
2343  e = e->next;
2344  if (e == intersection->reverse)
2345  {
2346  return true;
2347  }
2348 #ifdef DEBUG_CONVEX_HULL
2349  printf("Checking for outwards edge, current edge is ");
2350  e->print();
2351  printf("\n");
2352 #endif
2353  }
2354 #ifdef SHOW_ITERATIONS
2355  printf("Needed %d iterations to check for complete containment\n", n);
2356 #endif
2357  }
2358 
2359  Edge* firstIntersection = NULL;
2360  Edge* faceEdge = NULL;
2361  Edge* firstFaceEdge = NULL;
2362 
2363 #ifdef SHOW_ITERATIONS
2364  int m = 0;
2365 #endif
2366  while (true)
2367  {
2368 #ifdef SHOW_ITERATIONS
2369  m++;
2370 #endif
2371 #ifdef DEBUG_CONVEX_HULL
2372  printf("Intersecting edge is ");
2373  intersection->print();
2374  printf("\n");
2375 #endif
2376  if (cmp == 0)
2377  {
2378  Edge* e = intersection->reverse->next;
2379  startEdge = e;
2380 #ifdef SHOW_ITERATIONS
2381  n = 0;
2382 #endif
2383  while (true)
2384  {
2385 #ifdef SHOW_ITERATIONS
2386  n++;
2387 #endif
2388  if (e->target->dot(normal).compare(shiftedDot) >= 0)
2389  {
2390  break;
2391  }
2392  intersection = e->reverse;
2393  e = e->next;
2394  if (e == startEdge)
2395  {
2396  return true;
2397  }
2398  }
2399 #ifdef SHOW_ITERATIONS
2400  printf("Needed %d iterations to advance intersection\n", n);
2401 #endif
2402  }
2403 
2404 #ifdef DEBUG_CONVEX_HULL
2405  printf("Advanced intersecting edge to ");
2406  intersection->print();
2407  printf(", cmp = %d\n", cmp);
2408 #endif
2409 
2410  if (!firstIntersection)
2411  {
2412  firstIntersection = intersection;
2413  }
2414  else if (intersection == firstIntersection)
2415  {
2416  break;
2417  }
2418 
2419  int prevCmp = cmp;
2420  Edge* prevIntersection = intersection;
2421  Edge* prevFaceEdge = faceEdge;
2422 
2423  Edge* e = intersection->reverse;
2424 #ifdef SHOW_ITERATIONS
2425  n = 0;
2426 #endif
2427  while (true)
2428  {
2429 #ifdef SHOW_ITERATIONS
2430  n++;
2431 #endif
2432  e = e->reverse->prev;
2433  btAssert(e != intersection->reverse);
2434  cmp = e->target->dot(normal).compare(shiftedDot);
2435 #ifdef DEBUG_CONVEX_HULL
2436  printf("Testing edge ");
2437  e->print();
2438  printf(" -> cmp = %d\n", cmp);
2439 #endif
2440  if (cmp >= 0)
2441  {
2442  intersection = e;
2443  break;
2444  }
2445  }
2446 #ifdef SHOW_ITERATIONS
2447  printf("Needed %d iterations to find other intersection of face\n", n);
2448 #endif
2449 
2450  if (cmp > 0)
2451  {
2452  Vertex* removed = intersection->target;
2453  e = intersection->reverse;
2454  if (e->prev == e)
2455  {
2456  removed->edges = NULL;
2457  }
2458  else
2459  {
2460  removed->edges = e->prev;
2461  e->prev->link(e->next);
2462  e->link(e);
2463  }
2464 #ifdef DEBUG_CONVEX_HULL
2465  printf("1: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z);
2466 #endif
2467 
2468  Point64 n0 = intersection->face->getNormal();
2469  Point64 n1 = intersection->reverse->face->getNormal();
2470  int64_t m00 = face->dir0.dot(n0);
2471  int64_t m01 = face->dir1.dot(n0);
2472  int64_t m10 = face->dir0.dot(n1);
2473  int64_t m11 = face->dir1.dot(n1);
2474  int64_t r0 = (intersection->face->origin - shiftedOrigin).dot(n0);
2475  int64_t r1 = (intersection->reverse->face->origin - shiftedOrigin).dot(n1);
2476  Int128 det = Int128::mul(m00, m11) - Int128::mul(m01, m10);
2477  btAssert(det.getSign() != 0);
2478  Vertex* v = vertexPool.newObject();
2479  v->point.index = -1;
2480  v->copy = -1;
2481  v->point128 = PointR128(Int128::mul(face->dir0.x * r0, m11) - Int128::mul(face->dir0.x * r1, m01)
2482  + Int128::mul(face->dir1.x * r1, m00) - Int128::mul(face->dir1.x * r0, m10) + det * shiftedOrigin.x,
2483  Int128::mul(face->dir0.y * r0, m11) - Int128::mul(face->dir0.y * r1, m01)
2484  + Int128::mul(face->dir1.y * r1, m00) - Int128::mul(face->dir1.y * r0, m10) + det * shiftedOrigin.y,
2485  Int128::mul(face->dir0.z * r0, m11) - Int128::mul(face->dir0.z * r1, m01)
2486  + Int128::mul(face->dir1.z * r1, m00) - Int128::mul(face->dir1.z * r0, m10) + det * shiftedOrigin.z,
2487  det);
2488  v->point.x = (int32_t) v->point128.xvalue();
2489  v->point.y = (int32_t) v->point128.yvalue();
2490  v->point.z = (int32_t) v->point128.zvalue();
2491  intersection->target = v;
2492  v->edges = e;
2493 
2494  stack.push_back(v);
2495  stack.push_back(removed);
2496  stack.push_back(NULL);
2497  }
2498 
2499  if (cmp || prevCmp || (prevIntersection->reverse->next->target != intersection->target))
2500  {
2501  faceEdge = newEdgePair(prevIntersection->target, intersection->target);
2502  if (prevCmp == 0)
2503  {
2504  faceEdge->link(prevIntersection->reverse->next);
2505  }
2506  if ((prevCmp == 0) || prevFaceEdge)
2507  {
2508  prevIntersection->reverse->link(faceEdge);
2509  }
2510  if (cmp == 0)
2511  {
2512  intersection->reverse->prev->link(faceEdge->reverse);
2513  }
2514  faceEdge->reverse->link(intersection->reverse);
2515  }
2516  else
2517  {
2518  faceEdge = prevIntersection->reverse->next;
2519  }
2520 
2521  if (prevFaceEdge)
2522  {
2523  if (prevCmp > 0)
2524  {
2525  faceEdge->link(prevFaceEdge->reverse);
2526  }
2527  else if (faceEdge != prevFaceEdge->reverse)
2528  {
2529  stack.push_back(prevFaceEdge->target);
2530  while (faceEdge->next != prevFaceEdge->reverse)
2531  {
2532  Vertex* removed = faceEdge->next->target;
2533  removeEdgePair(faceEdge->next);
2534  stack.push_back(removed);
2535 #ifdef DEBUG_CONVEX_HULL
2536  printf("2: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z);
2537 #endif
2538  }
2539  stack.push_back(NULL);
2540  }
2541  }
2542  faceEdge->face = face;
2543  faceEdge->reverse->face = intersection->face;
2544 
2545  if (!firstFaceEdge)
2546  {
2547  firstFaceEdge = faceEdge;
2548  }
2549  }
2550 #ifdef SHOW_ITERATIONS
2551  printf("Needed %d iterations to process all intersections\n", m);
2552 #endif
2553 
2554  if (cmp > 0)
2555  {
2556  firstFaceEdge->reverse->target = faceEdge->target;
2557  firstIntersection->reverse->link(firstFaceEdge);
2558  firstFaceEdge->link(faceEdge->reverse);
2559  }
2560  else if (firstFaceEdge != faceEdge->reverse)
2561  {
2562  stack.push_back(faceEdge->target);
2563  while (firstFaceEdge->next != faceEdge->reverse)
2564  {
2565  Vertex* removed = firstFaceEdge->next->target;
2566  removeEdgePair(firstFaceEdge->next);
2567  stack.push_back(removed);
2568 #ifdef DEBUG_CONVEX_HULL
2569  printf("3: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z);
2570 #endif
2571  }
2572  stack.push_back(NULL);
2573  }
2574 
2575  btAssert(stack.size() > 0);
2576  vertexList = stack[0];
2577 
2578 #ifdef DEBUG_CONVEX_HULL
2579  printf("Removing part\n");
2580 #endif
2581 #ifdef SHOW_ITERATIONS
2582  n = 0;
2583 #endif
2584  int pos = 0;
2585  while (pos < stack.size())
2586  {
2587  int end = stack.size();
2588  while (pos < end)
2589  {
2590  Vertex* kept = stack[pos++];
2591 #ifdef DEBUG_CONVEX_HULL
2592  kept->print();
2593 #endif
2594  bool deeper = false;
2595  Vertex* removed;
2596  while ((removed = stack[pos++]) != NULL)
2597  {
2598 #ifdef SHOW_ITERATIONS
2599  n++;
2600 #endif
2601  kept->receiveNearbyFaces(removed);
2602  while (removed->edges)
2603  {
2604  if (!deeper)
2605  {
2606  deeper = true;
2607  stack.push_back(kept);
2608  }
2609  stack.push_back(removed->edges->target);
2610  removeEdgePair(removed->edges);
2611  }
2612  }
2613  if (deeper)
2614  {
2615  stack.push_back(NULL);
2616  }
2617  }
2618  }
2619 #ifdef SHOW_ITERATIONS
2620  printf("Needed %d iterations to remove part\n", n);
2621 #endif
2622 
2623  stack.resize(0);
2624  face->origin = shiftedOrigin;
2625 
2626  return true;
2627 }
2628 
2629 
2631 {
2632  int index = vertex->copy;
2633  if (index < 0)
2634  {
2635  index = vertices.size();
2636  vertex->copy = index;
2637  vertices.push_back(vertex);
2638 #ifdef DEBUG_CONVEX_HULL
2639  printf("Vertex %d gets index *%d\n", vertex->point.index, index);
2640 #endif
2641  }
2642  return index;
2643 }
2644 
2645 btScalar btConvexHullComputer::compute(const void* coords, bool doubleCoords, int stride, int count, btScalar shrink, btScalar shrinkClamp)
2646 {
2647  if (count <= 0)
2648  {
2649  vertices.clear();
2650  edges.clear();
2651  faces.clear();
2652  return 0;
2653  }
2654 
2655  btConvexHullInternal hull;
2656  hull.compute(coords, doubleCoords, stride, count);
2657 
2658  btScalar shift = 0;
2659  if ((shrink > 0) && ((shift = hull.shrink(shrink, shrinkClamp)) < 0))
2660  {
2661  vertices.clear();
2662  edges.clear();
2663  faces.clear();
2664  return shift;
2665  }
2666 
2667  vertices.resize(0);
2668  edges.resize(0);
2669  faces.resize(0);
2670 
2672  getVertexCopy(hull.vertexList, oldVertices);
2673  int copied = 0;
2674  while (copied < oldVertices.size())
2675  {
2676  btConvexHullInternal::Vertex* v = oldVertices[copied];
2677  vertices.push_back(hull.getCoordinates(v));
2678  btConvexHullInternal::Edge* firstEdge = v->edges;
2679  if (firstEdge)
2680  {
2681  int firstCopy = -1;
2682  int prevCopy = -1;
2683  btConvexHullInternal::Edge* e = firstEdge;
2684  do
2685  {
2686  if (e->copy < 0)
2687  {
2688  int s = edges.size();
2689  edges.push_back(Edge());
2690  edges.push_back(Edge());
2691  Edge* c = &edges[s];
2692  Edge* r = &edges[s + 1];
2693  e->copy = s;
2694  e->reverse->copy = s + 1;
2695  c->reverse = 1;
2696  r->reverse = -1;
2697  c->targetVertex = getVertexCopy(e->target, oldVertices);
2698  r->targetVertex = copied;
2699 #ifdef DEBUG_CONVEX_HULL
2700  printf(" CREATE: Vertex *%d has edge to *%d\n", copied, c->getTargetVertex());
2701 #endif
2702  }
2703  if (prevCopy >= 0)
2704  {
2705  edges[e->copy].next = prevCopy - e->copy;
2706  }
2707  else
2708  {
2709  firstCopy = e->copy;
2710  }
2711  prevCopy = e->copy;
2712  e = e->next;
2713  } while (e != firstEdge);
2714  edges[firstCopy].next = prevCopy - firstCopy;
2715  }
2716  copied++;
2717  }
2718 
2719  for (int i = 0; i < copied; i++)
2720  {
2721  btConvexHullInternal::Vertex* v = oldVertices[i];
2722  btConvexHullInternal::Edge* firstEdge = v->edges;
2723  if (firstEdge)
2724  {
2725  btConvexHullInternal::Edge* e = firstEdge;
2726  do
2727  {
2728  if (e->copy >= 0)
2729  {
2730 #ifdef DEBUG_CONVEX_HULL
2731  printf("Vertex *%d has edge to *%d\n", i, edges[e->copy].getTargetVertex());
2732 #endif
2733  faces.push_back(e->copy);
2735  do
2736  {
2737 #ifdef DEBUG_CONVEX_HULL
2738  printf(" Face *%d\n", edges[f->copy].getTargetVertex());
2739 #endif
2740  f->copy = -1;
2741  f = f->reverse->prev;
2742  } while (f != e);
2743  }
2744  e = e->next;
2745  } while (e != firstEdge);
2746  }
2747  }
2748 
2749  return shift;
2750 }
2751 
2752 
2753 
2754 
2755 
Int128 operator-(const Int128 &b) const
void push_back(const T &_Val)
int64_t dot(const Point64 &b) const
void init(Vertex *a, Vertex *b, Vertex *c)
static Orientation getOrientation(const Edge *prev, const Edge *next, const Point32 &s, const Point32 &t)
unsigned long long int uint64_t
PointR128(Int128 x, Int128 y, Int128 z, Int128 denominator)
The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods It...
unsigned long int uint64_t
float dist(const Point3 &pnt0, const Point3 &pnt1)
btScalar shrink(btScalar amount, btScalar clampAmount)
static Int128 mul(int64_t a, int64_t b)
Int128 operator*(int64_t b) const
Rational64(int64_t numerator, int64_t denominator)
int compare(const Rational64 &b) const
static uint64_t mul(uint32_t a, uint32_t b)
#define btAssert(x)
Definition: btScalar.h:101
unsigned int uint32_t
int compare(const Rational128 &b) const
static Int128 mul(uint64_t a, uint64_t b)
static int getVertexCopy(btConvexHullInternal::Vertex *vertex, btAlignedObjectArray< btConvexHullInternal::Vertex * > &vertices)
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
void clear()
clear the array, deallocated memory. Generally it is better to use array.resize(0), to reduce performance overhead of run-time memory (de)allocations.
int int32_t
static uint32_t high(uint64_t value)
void findEdgeForCoplanarFaces(Vertex *c0, Vertex *c1, Edge *&e0, Edge *&e1, Vertex *stop0, Vertex *stop1)
static void shlHalf(uint64_t &value)
#define SIMD_INFINITY
Definition: btScalar.h:449
Point64 cross(const Point64 &b) const
static uint32_t low(uint64_t value)
bool operator<(const Int128 &b) const
int size() const
return the number of elements in the array
btVector3 getCoordinates(const Vertex *v)
void btSwap(T &a, T &b)
Definition: btScalar.h:535
Point32 operator-(const Point32 &b) const
void compute(const void *coords, bool doubleCoords, int stride, int count)
static float max(float a, float b)
bool operator==(const Point32 &b) const
int64_t dot(const Point64 &b) const
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:377
Edge * findMaxAngle(bool ccw, const Vertex *start, const Point32 &s, const Point64 &rxs, const Point64 &sxrxs, Rational64 &minCot)
#define btAlignedFree(ptr)
int64_t dot(const Point32 &b) const
unsigned int uint32_t
Edge * newEdgePair(Vertex *from, Vertex *to)
Int128 & operator+=(const Int128 &b)
static void mul(UWord a, UWord b, UWord &resLow, UWord &resHigh)
static float min(float a, float b)
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
Int128(uint64_t low, uint64_t high)
Point32 operator+(const Point32 &b) const
bool mergeProjection(IntermediateHull &h0, IntermediateHull &h1, Vertex *&c0, Vertex *&c1)
btScalar btAtan(btScalar x)
Definition: btScalar.h:425
Rational128 dot(const Point64 &b) const
btAlignedObjectArray< Vertex * > originalVertices
btVector3 normalized() const
Return a normalized version of this vector.
Definition: btVector3.h:951
bool shiftFace(Face *face, btScalar amount, btAlignedObjectArray< Vertex * > stack)
int ucmp(const Int128 &b) const
void resize(int newsize, const T &fillData=T())
Point64(int64_t x, int64_t y, int64_t z)
static void shlHalf(Int128 &value)
static float4 cross(const float4 &a, const float4 &b)
btVector3 getBtNormal(Face *face)
void computeInternal(int start, int end, IntermediateHull &result)
int minAxis() const
Return the axis with the smallest value Note return values are 0,1,2 for x, y, or z...
Definition: btVector3.h:468
Point32(int32_t x, int32_t y, int32_t z)
static uint64_t high(Int128 value)
void removeEdgePair(Edge *edge)
#define btAlignedAlloc(size, alignment)
btScalar compute(const void *coords, bool doubleCoords, int stride, int count, btScalar shrink, btScalar shrinkClamp)
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:827
void setMax(const btVector3 &other)
Set each element to the max of the current values and the values of another btVector3.
Definition: btVector3.h:609
btVector3 toBtVector(const Point32 &v)
Rational128(const Int128 &numerator, const Int128 &denominator)
Int128 operator+(const Int128 &b) const
const T & btMin(const T &a, const T &b)
Definition: btMinMax.h:23
long long int int64_t
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:266
void quickSort(const L &CompareFunc)
Point32 operator-(const Vertex &b) const
static uint64_t low(Int128 value)
void merge(IntermediateHull &h0, IntermediateHull &h1)
int maxAxis() const
Return the axis with the largest value Note return values are 0,1,2 for x, y, or z.
Definition: btVector3.h:475
bool operator!=(const Point32 &b) const
Point64 cross(const Point32 &b) const