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  {
326  return sign * ((m_denominator == 0) ? SIMD_INFINITY : (btScalar) m_numerator / m_denominator);
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 
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 
390  {
391  return sign * ((denominator.getSign() == 0) ? SIMD_INFINITY : numerator.toScalar() / denominator.toScalar());
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))
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 
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:
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  {
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:
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  {
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  {
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:
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 
725  void setArraySize(int arraySize)
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 
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  Vertex* v = originalVertices[start];
1282  v->edges = NULL;
1283  v->next = v;
1284  v->prev = v;
1285 
1286  result.minXy = v;
1287  result.maxXy = v;
1288  result.minYx = v;
1289  result.maxYx = v;
1290  }
1291 
1292  return;
1293  }
1294 
1295  case 1:
1296  {
1297  Vertex* v = originalVertices[start];
1298  v->edges = NULL;
1299  v->next = v;
1300  v->prev = v;
1301 
1302  result.minXy = v;
1303  result.maxXy = v;
1304  result.minYx = v;
1305  result.maxYx = v;
1306 
1307  return;
1308  }
1309  }
1310 
1311  int split0 = start + n / 2;
1312  Point32 p = originalVertices[split0-1]->point;
1313  int split1 = split0;
1314  while ((split1 < end) && (originalVertices[split1]->point == p))
1315  {
1316  split1++;
1317  }
1318  computeInternal(start, split0, result);
1319  IntermediateHull hull1;
1320  computeInternal(split1, end, hull1);
1321 #ifdef DEBUG_CONVEX_HULL
1322  printf("\n\nMerge\n");
1323  result.print();
1324  hull1.print();
1325 #endif
1326  merge(result, hull1);
1327 #ifdef DEBUG_CONVEX_HULL
1328  printf("\n Result\n");
1329  result.print();
1330 #endif
1331 }
1332 
1333 #ifdef DEBUG_CONVEX_HULL
1335 {
1336  printf(" Hull\n");
1337  for (Vertex* v = minXy; v; )
1338  {
1339  printf(" ");
1340  v->print();
1341  if (v == maxXy)
1342  {
1343  printf(" maxXy");
1344  }
1345  if (v == minYx)
1346  {
1347  printf(" minYx");
1348  }
1349  if (v == maxYx)
1350  {
1351  printf(" maxYx");
1352  }
1353  if (v->next->prev != v)
1354  {
1355  printf(" Inconsistency");
1356  }
1357  printf("\n");
1358  v = v->next;
1359  if (v == minXy)
1360  {
1361  break;
1362  }
1363  }
1364  if (minXy)
1365  {
1366  minXy->copy = (minXy->copy == -1) ? -2 : -1;
1367  minXy->printGraph();
1368  }
1369 }
1370 
1371 void btConvexHullInternal::Vertex::printGraph()
1372 {
1373  print();
1374  printf("\nEdges\n");
1375  Edge* e = edges;
1376  if (e)
1377  {
1378  do
1379  {
1380  e->print();
1381  printf("\n");
1382  e = e->next;
1383  } while (e != edges);
1384  do
1385  {
1386  Vertex* v = e->target;
1387  if (v->copy != copy)
1388  {
1389  v->copy = copy;
1390  v->printGraph();
1391  }
1392  e = e->next;
1393  } while (e != edges);
1394  }
1395 }
1396 #endif
1397 
1399 {
1400  btAssert(prev->reverse->target == next->reverse->target);
1401  if (prev->next == next)
1402  {
1403  if (prev->prev == next)
1404  {
1405  Point64 n = t.cross(s);
1406  Point64 m = (*prev->target - *next->reverse->target).cross(*next->target - *next->reverse->target);
1407  btAssert(!m.isZero());
1408  int64_t dot = n.dot(m);
1409  btAssert(dot != 0);
1410  return (dot > 0) ? COUNTER_CLOCKWISE : CLOCKWISE;
1411  }
1412  return COUNTER_CLOCKWISE;
1413  }
1414  else if (prev->prev == next)
1415  {
1416  return CLOCKWISE;
1417  }
1418  else
1419  {
1420  return NONE;
1421  }
1422 }
1423 
1424 btConvexHullInternal::Edge* btConvexHullInternal::findMaxAngle(bool ccw, const Vertex* start, const Point32& s, const Point64& rxs, const Point64& sxrxs, Rational64& minCot)
1425 {
1426  Edge* minEdge = NULL;
1427 
1428 #ifdef DEBUG_CONVEX_HULL
1429  printf("find max edge for %d\n", start->point.index);
1430 #endif
1431  Edge* e = start->edges;
1432  if (e)
1433  {
1434  do
1435  {
1436  if (e->copy > mergeStamp)
1437  {
1438  Point32 t = *e->target - *start;
1439  Rational64 cot(t.dot(sxrxs), t.dot(rxs));
1440 #ifdef DEBUG_CONVEX_HULL
1441  printf(" Angle is %f (%d) for ", (float) btAtan(cot.toScalar()), (int) cot.isNaN());
1442  e->print();
1443 #endif
1444  if (cot.isNaN())
1445  {
1446  btAssert(ccw ? (t.dot(s) < 0) : (t.dot(s) > 0));
1447  }
1448  else
1449  {
1450  int cmp;
1451  if (minEdge == NULL)
1452  {
1453  minCot = cot;
1454  minEdge = e;
1455  }
1456  else if ((cmp = cot.compare(minCot)) < 0)
1457  {
1458  minCot = cot;
1459  minEdge = e;
1460  }
1461  else if ((cmp == 0) && (ccw == (getOrientation(minEdge, e, s, t) == COUNTER_CLOCKWISE)))
1462  {
1463  minEdge = e;
1464  }
1465  }
1466 #ifdef DEBUG_CONVEX_HULL
1467  printf("\n");
1468 #endif
1469  }
1470  e = e->next;
1471  } while (e != start->edges);
1472  }
1473  return minEdge;
1474 }
1475 
1477 {
1478  Edge* start0 = e0;
1479  Edge* start1 = e1;
1480  Point32 et0 = start0 ? start0->target->point : c0->point;
1481  Point32 et1 = start1 ? start1->target->point : c1->point;
1482  Point32 s = c1->point - c0->point;
1483  Point64 normal = ((start0 ? start0 : start1)->target->point - c0->point).cross(s);
1484  int64_t dist = c0->point.dot(normal);
1485  btAssert(!start1 || (start1->target->point.dot(normal) == dist));
1486  Point64 perp = s.cross(normal);
1487  btAssert(!perp.isZero());
1488 
1489 #ifdef DEBUG_CONVEX_HULL
1490  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);
1491 #endif
1492 
1493  int64_t maxDot0 = et0.dot(perp);
1494  if (e0)
1495  {
1496  while (e0->target != stop0)
1497  {
1498  Edge* e = e0->reverse->prev;
1499  if (e->target->point.dot(normal) < dist)
1500  {
1501  break;
1502  }
1503  btAssert(e->target->point.dot(normal) == dist);
1504  if (e->copy == mergeStamp)
1505  {
1506  break;
1507  }
1508  int64_t dot = e->target->point.dot(perp);
1509  if (dot <= maxDot0)
1510  {
1511  break;
1512  }
1513  maxDot0 = dot;
1514  e0 = e;
1515  et0 = e->target->point;
1516  }
1517  }
1518 
1519  int64_t maxDot1 = et1.dot(perp);
1520  if (e1)
1521  {
1522  while (e1->target != stop1)
1523  {
1524  Edge* e = e1->reverse->next;
1525  if (e->target->point.dot(normal) < dist)
1526  {
1527  break;
1528  }
1529  btAssert(e->target->point.dot(normal) == dist);
1530  if (e->copy == mergeStamp)
1531  {
1532  break;
1533  }
1534  int64_t dot = e->target->point.dot(perp);
1535  if (dot <= maxDot1)
1536  {
1537  break;
1538  }
1539  maxDot1 = dot;
1540  e1 = e;
1541  et1 = e->target->point;
1542  }
1543  }
1544 
1545 #ifdef DEBUG_CONVEX_HULL
1546  printf(" Starting at %d %d\n", et0.index, et1.index);
1547 #endif
1548 
1549  int64_t dx = maxDot1 - maxDot0;
1550  if (dx > 0)
1551  {
1552  while (true)
1553  {
1554  int64_t dy = (et1 - et0).dot(s);
1555 
1556  if (e0 && (e0->target != stop0))
1557  {
1558  Edge* f0 = e0->next->reverse;
1559  if (f0->copy > mergeStamp)
1560  {
1561  int64_t dx0 = (f0->target->point - et0).dot(perp);
1562  int64_t dy0 = (f0->target->point - et0).dot(s);
1563  if ((dx0 == 0) ? (dy0 < 0) : ((dx0 < 0) && (Rational64(dy0, dx0).compare(Rational64(dy, dx)) >= 0)))
1564  {
1565  et0 = f0->target->point;
1566  dx = (et1 - et0).dot(perp);
1567  e0 = (e0 == start0) ? NULL : f0;
1568  continue;
1569  }
1570  }
1571  }
1572 
1573  if (e1 && (e1->target != stop1))
1574  {
1575  Edge* f1 = e1->reverse->next;
1576  if (f1->copy > mergeStamp)
1577  {
1578  Point32 d1 = f1->target->point - et1;
1579  if (d1.dot(normal) == 0)
1580  {
1581  int64_t dx1 = d1.dot(perp);
1582  int64_t dy1 = d1.dot(s);
1583  int64_t dxn = (f1->target->point - et0).dot(perp);
1584  if ((dxn > 0) && ((dx1 == 0) ? (dy1 < 0) : ((dx1 < 0) && (Rational64(dy1, dx1).compare(Rational64(dy, dx)) > 0))))
1585  {
1586  e1 = f1;
1587  et1 = e1->target->point;
1588  dx = dxn;
1589  continue;
1590  }
1591  }
1592  else
1593  {
1594  btAssert((e1 == start1) && (d1.dot(normal) < 0));
1595  }
1596  }
1597  }
1598 
1599  break;
1600  }
1601  }
1602  else if (dx < 0)
1603  {
1604  while (true)
1605  {
1606  int64_t dy = (et1 - et0).dot(s);
1607 
1608  if (e1 && (e1->target != stop1))
1609  {
1610  Edge* f1 = e1->prev->reverse;
1611  if (f1->copy > mergeStamp)
1612  {
1613  int64_t dx1 = (f1->target->point - et1).dot(perp);
1614  int64_t dy1 = (f1->target->point - et1).dot(s);
1615  if ((dx1 == 0) ? (dy1 > 0) : ((dx1 < 0) && (Rational64(dy1, dx1).compare(Rational64(dy, dx)) <= 0)))
1616  {
1617  et1 = f1->target->point;
1618  dx = (et1 - et0).dot(perp);
1619  e1 = (e1 == start1) ? NULL : f1;
1620  continue;
1621  }
1622  }
1623  }
1624 
1625  if (e0 && (e0->target != stop0))
1626  {
1627  Edge* f0 = e0->reverse->prev;
1628  if (f0->copy > mergeStamp)
1629  {
1630  Point32 d0 = f0->target->point - et0;
1631  if (d0.dot(normal) == 0)
1632  {
1633  int64_t dx0 = d0.dot(perp);
1634  int64_t dy0 = d0.dot(s);
1635  int64_t dxn = (et1 - f0->target->point).dot(perp);
1636  if ((dxn < 0) && ((dx0 == 0) ? (dy0 > 0) : ((dx0 < 0) && (Rational64(dy0, dx0).compare(Rational64(dy, dx)) < 0))))
1637  {
1638  e0 = f0;
1639  et0 = e0->target->point;
1640  dx = dxn;
1641  continue;
1642  }
1643  }
1644  else
1645  {
1646  btAssert((e0 == start0) && (d0.dot(normal) < 0));
1647  }
1648  }
1649  }
1650 
1651  break;
1652  }
1653  }
1654 #ifdef DEBUG_CONVEX_HULL
1655  printf(" Advanced edges to %d %d\n", et0.index, et1.index);
1656 #endif
1657 }
1658 
1659 
1661 {
1662  if (!h1.maxXy)
1663  {
1664  return;
1665  }
1666  if (!h0.maxXy)
1667  {
1668  h0 = h1;
1669  return;
1670  }
1671 
1672  mergeStamp--;
1673 
1674  Vertex* c0 = NULL;
1675  Edge* toPrev0 = NULL;
1676  Edge* firstNew0 = NULL;
1677  Edge* pendingHead0 = NULL;
1678  Edge* pendingTail0 = NULL;
1679  Vertex* c1 = NULL;
1680  Edge* toPrev1 = NULL;
1681  Edge* firstNew1 = NULL;
1682  Edge* pendingHead1 = NULL;
1683  Edge* pendingTail1 = NULL;
1684  Point32 prevPoint;
1685 
1686  if (mergeProjection(h0, h1, c0, c1))
1687  {
1688  Point32 s = *c1 - *c0;
1689  Point64 normal = Point32(0, 0, -1).cross(s);
1690  Point64 t = s.cross(normal);
1691  btAssert(!t.isZero());
1692 
1693  Edge* e = c0->edges;
1694  Edge* start0 = NULL;
1695  if (e)
1696  {
1697  do
1698  {
1699  int64_t dot = (*e->target - *c0).dot(normal);
1700  btAssert(dot <= 0);
1701  if ((dot == 0) && ((*e->target - *c0).dot(t) > 0))
1702  {
1703  if (!start0 || (getOrientation(start0, e, s, Point32(0, 0, -1)) == CLOCKWISE))
1704  {
1705  start0 = e;
1706  }
1707  }
1708  e = e->next;
1709  } while (e != c0->edges);
1710  }
1711 
1712  e = c1->edges;
1713  Edge* start1 = NULL;
1714  if (e)
1715  {
1716  do
1717  {
1718  int64_t dot = (*e->target - *c1).dot(normal);
1719  btAssert(dot <= 0);
1720  if ((dot == 0) && ((*e->target - *c1).dot(t) > 0))
1721  {
1722  if (!start1 || (getOrientation(start1, e, s, Point32(0, 0, -1)) == COUNTER_CLOCKWISE))
1723  {
1724  start1 = e;
1725  }
1726  }
1727  e = e->next;
1728  } while (e != c1->edges);
1729  }
1730 
1731  if (start0 || start1)
1732  {
1733  findEdgeForCoplanarFaces(c0, c1, start0, start1, NULL, NULL);
1734  if (start0)
1735  {
1736  c0 = start0->target;
1737  }
1738  if (start1)
1739  {
1740  c1 = start1->target;
1741  }
1742  }
1743 
1744  prevPoint = c1->point;
1745  prevPoint.z++;
1746  }
1747  else
1748  {
1749  prevPoint = c1->point;
1750  prevPoint.x++;
1751  }
1752 
1753  Vertex* first0 = c0;
1754  Vertex* first1 = c1;
1755  bool firstRun = true;
1756 
1757  while (true)
1758  {
1759  Point32 s = *c1 - *c0;
1760  Point32 r = prevPoint - c0->point;
1761  Point64 rxs = r.cross(s);
1762  Point64 sxrxs = s.cross(rxs);
1763 
1764 #ifdef DEBUG_CONVEX_HULL
1765  printf("\n Checking %d %d\n", c0->point.index, c1->point.index);
1766 #endif
1767  Rational64 minCot0(0, 0);
1768  Edge* min0 = findMaxAngle(false, c0, s, rxs, sxrxs, minCot0);
1769  Rational64 minCot1(0, 0);
1770  Edge* min1 = findMaxAngle(true, c1, s, rxs, sxrxs, minCot1);
1771  if (!min0 && !min1)
1772  {
1773  Edge* e = newEdgePair(c0, c1);
1774  e->link(e);
1775  c0->edges = e;
1776 
1777  e = e->reverse;
1778  e->link(e);
1779  c1->edges = e;
1780  return;
1781  }
1782  else
1783  {
1784  int cmp = !min0 ? 1 : !min1 ? -1 : minCot0.compare(minCot1);
1785 #ifdef DEBUG_CONVEX_HULL
1786  printf(" -> Result %d\n", cmp);
1787 #endif
1788  if (firstRun || ((cmp >= 0) ? !minCot1.isNegativeInfinity() : !minCot0.isNegativeInfinity()))
1789  {
1790  Edge* e = newEdgePair(c0, c1);
1791  if (pendingTail0)
1792  {
1793  pendingTail0->prev = e;
1794  }
1795  else
1796  {
1797  pendingHead0 = e;
1798  }
1799  e->next = pendingTail0;
1800  pendingTail0 = e;
1801 
1802  e = e->reverse;
1803  if (pendingTail1)
1804  {
1805  pendingTail1->next = e;
1806  }
1807  else
1808  {
1809  pendingHead1 = e;
1810  }
1811  e->prev = pendingTail1;
1812  pendingTail1 = e;
1813  }
1814 
1815  Edge* e0 = min0;
1816  Edge* e1 = min1;
1817 
1818 #ifdef DEBUG_CONVEX_HULL
1819  printf(" Found min edges to %d %d\n", e0 ? e0->target->point.index : -1, e1 ? e1->target->point.index : -1);
1820 #endif
1821 
1822  if (cmp == 0)
1823  {
1824  findEdgeForCoplanarFaces(c0, c1, e0, e1, NULL, NULL);
1825  }
1826 
1827  if ((cmp >= 0) && e1)
1828  {
1829  if (toPrev1)
1830  {
1831  for (Edge* e = toPrev1->next, *n = NULL; e != min1; e = n)
1832  {
1833  n = e->next;
1834  removeEdgePair(e);
1835  }
1836  }
1837 
1838  if (pendingTail1)
1839  {
1840  if (toPrev1)
1841  {
1842  toPrev1->link(pendingHead1);
1843  }
1844  else
1845  {
1846  min1->prev->link(pendingHead1);
1847  firstNew1 = pendingHead1;
1848  }
1849  pendingTail1->link(min1);
1850  pendingHead1 = NULL;
1851  pendingTail1 = NULL;
1852  }
1853  else if (!toPrev1)
1854  {
1855  firstNew1 = min1;
1856  }
1857 
1858  prevPoint = c1->point;
1859  c1 = e1->target;
1860  toPrev1 = e1->reverse;
1861  }
1862 
1863  if ((cmp <= 0) && e0)
1864  {
1865  if (toPrev0)
1866  {
1867  for (Edge* e = toPrev0->prev, *n = NULL; e != min0; e = n)
1868  {
1869  n = e->prev;
1870  removeEdgePair(e);
1871  }
1872  }
1873 
1874  if (pendingTail0)
1875  {
1876  if (toPrev0)
1877  {
1878  pendingHead0->link(toPrev0);
1879  }
1880  else
1881  {
1882  pendingHead0->link(min0->next);
1883  firstNew0 = pendingHead0;
1884  }
1885  min0->link(pendingTail0);
1886  pendingHead0 = NULL;
1887  pendingTail0 = NULL;
1888  }
1889  else if (!toPrev0)
1890  {
1891  firstNew0 = min0;
1892  }
1893 
1894  prevPoint = c0->point;
1895  c0 = e0->target;
1896  toPrev0 = e0->reverse;
1897  }
1898  }
1899 
1900  if ((c0 == first0) && (c1 == first1))
1901  {
1902  if (toPrev0 == NULL)
1903  {
1904  pendingHead0->link(pendingTail0);
1905  c0->edges = pendingTail0;
1906  }
1907  else
1908  {
1909  for (Edge* e = toPrev0->prev, *n = NULL; e != firstNew0; e = n)
1910  {
1911  n = e->prev;
1912  removeEdgePair(e);
1913  }
1914  if (pendingTail0)
1915  {
1916  pendingHead0->link(toPrev0);
1917  firstNew0->link(pendingTail0);
1918  }
1919  }
1920 
1921  if (toPrev1 == NULL)
1922  {
1923  pendingTail1->link(pendingHead1);
1924  c1->edges = pendingTail1;
1925  }
1926  else
1927  {
1928  for (Edge* e = toPrev1->next, *n = NULL; e != firstNew1; e = n)
1929  {
1930  n = e->next;
1931  removeEdgePair(e);
1932  }
1933  if (pendingTail1)
1934  {
1935  toPrev1->link(pendingHead1);
1936  pendingTail1->link(firstNew1);
1937  }
1938  }
1939 
1940  return;
1941  }
1942 
1943  firstRun = false;
1944  }
1945 }
1946 
1948 {
1949  public:
1950 
1951  bool operator() ( const btConvexHullInternal::Point32& p, const btConvexHullInternal::Point32& q ) const
1952  {
1953  return (p.y < q.y) || ((p.y == q.y) && ((p.x < q.x) || ((p.x == q.x) && (p.z < q.z))));
1954  }
1955 };
1956 
1957 void btConvexHullInternal::compute(const void* coords, bool doubleCoords, int stride, int count)
1958 {
1959  btVector3 min(btScalar(1e30), btScalar(1e30), btScalar(1e30)), max(btScalar(-1e30), btScalar(-1e30), btScalar(-1e30));
1960  const char* ptr = (const char*) coords;
1961  if (doubleCoords)
1962  {
1963  for (int i = 0; i < count; i++)
1964  {
1965  const double* v = (const double*) ptr;
1966  btVector3 p((btScalar) v[0], (btScalar) v[1], (btScalar) v[2]);
1967  ptr += stride;
1968  min.setMin(p);
1969  max.setMax(p);
1970  }
1971  }
1972  else
1973  {
1974  for (int i = 0; i < count; i++)
1975  {
1976  const float* v = (const float*) ptr;
1977  btVector3 p(v[0], v[1], v[2]);
1978  ptr += stride;
1979  min.setMin(p);
1980  max.setMax(p);
1981  }
1982  }
1983 
1984  btVector3 s = max - min;
1985  maxAxis = s.maxAxis();
1986  minAxis = s.minAxis();
1987  if (minAxis == maxAxis)
1988  {
1989  minAxis = (maxAxis + 1) % 3;
1990  }
1991  medAxis = 3 - maxAxis - minAxis;
1992 
1993  s /= btScalar(10216);
1994  if (((medAxis + 1) % 3) != maxAxis)
1995  {
1996  s *= -1;
1997  }
1998  scaling = s;
1999 
2000  if (s[0] != 0)
2001  {
2002  s[0] = btScalar(1) / s[0];
2003  }
2004  if (s[1] != 0)
2005  {
2006  s[1] = btScalar(1) / s[1];
2007  }
2008  if (s[2] != 0)
2009  {
2010  s[2] = btScalar(1) / s[2];
2011  }
2012 
2013  center = (min + max) * btScalar(0.5);
2014 
2016  points.resize(count);
2017  ptr = (const char*) coords;
2018  if (doubleCoords)
2019  {
2020  for (int i = 0; i < count; i++)
2021  {
2022  const double* v = (const double*) ptr;
2023  btVector3 p((btScalar) v[0], (btScalar) v[1], (btScalar) v[2]);
2024  ptr += stride;
2025  p = (p - center) * s;
2026  points[i].x = (int32_t) p[medAxis];
2027  points[i].y = (int32_t) p[maxAxis];
2028  points[i].z = (int32_t) p[minAxis];
2029  points[i].index = i;
2030  }
2031  }
2032  else
2033  {
2034  for (int i = 0; i < count; i++)
2035  {
2036  const float* v = (const float*) ptr;
2037  btVector3 p(v[0], v[1], v[2]);
2038  ptr += stride;
2039  p = (p - center) * s;
2040  points[i].x = (int32_t) p[medAxis];
2041  points[i].y = (int32_t) p[maxAxis];
2042  points[i].z = (int32_t) p[minAxis];
2043  points[i].index = i;
2044  }
2045  }
2046  points.quickSort(pointCmp());
2047 
2048  vertexPool.reset();
2049  vertexPool.setArraySize(count);
2050  originalVertices.resize(count);
2051  for (int i = 0; i < count; i++)
2052  {
2053  Vertex* v = vertexPool.newObject();
2054  v->edges = NULL;
2055  v->point = points[i];
2056  v->copy = -1;
2057  originalVertices[i] = v;
2058  }
2059 
2060  points.clear();
2061 
2062  edgePool.reset();
2063  edgePool.setArraySize(6 * count);
2064 
2065  usedEdgePairs = 0;
2066  maxUsedEdgePairs = 0;
2067 
2068  mergeStamp = -3;
2069 
2070  IntermediateHull hull;
2071  computeInternal(0, count, hull);
2072  vertexList = hull.minXy;
2073 #ifdef DEBUG_CONVEX_HULL
2074  printf("max. edges %d (3v = %d)", maxUsedEdgePairs, 3 * count);
2075 #endif
2076 }
2077 
2079 {
2080  btVector3 p;
2081  p[medAxis] = btScalar(v.x);
2082  p[maxAxis] = btScalar(v.y);
2083  p[minAxis] = btScalar(v.z);
2084  return p * scaling;
2085 }
2086 
2088 {
2089  return toBtVector(face->dir0).cross(toBtVector(face->dir1)).normalized();
2090 }
2091 
2093 {
2094  btVector3 p;
2095  p[medAxis] = v->xvalue();
2096  p[maxAxis] = v->yvalue();
2097  p[minAxis] = v->zvalue();
2098  return p * scaling + center;
2099 }
2100 
2102 {
2103  if (!vertexList)
2104  {
2105  return 0;
2106  }
2107  int stamp = --mergeStamp;
2109  vertexList->copy = stamp;
2110  stack.push_back(vertexList);
2112 
2113  Point32 ref = vertexList->point;
2114  Int128 hullCenterX(0, 0);
2115  Int128 hullCenterY(0, 0);
2116  Int128 hullCenterZ(0, 0);
2117  Int128 volume(0, 0);
2118 
2119  while (stack.size() > 0)
2120  {
2121  Vertex* v = stack[stack.size() - 1];
2122  stack.pop_back();
2123  Edge* e = v->edges;
2124  if (e)
2125  {
2126  do
2127  {
2128  if (e->target->copy != stamp)
2129  {
2130  e->target->copy = stamp;
2131  stack.push_back(e->target);
2132  }
2133  if (e->copy != stamp)
2134  {
2135  Face* face = facePool.newObject();
2136  face->init(e->target, e->reverse->prev->target, v);
2137  faces.push_back(face);
2138  Edge* f = e;
2139 
2140  Vertex* a = NULL;
2141  Vertex* b = NULL;
2142  do
2143  {
2144  if (a && b)
2145  {
2146  int64_t vol = (v->point - ref).dot((a->point - ref).cross(b->point - ref));
2147  btAssert(vol >= 0);
2148  Point32 c = v->point + a->point + b->point + ref;
2149  hullCenterX += vol * c.x;
2150  hullCenterY += vol * c.y;
2151  hullCenterZ += vol * c.z;
2152  volume += vol;
2153  }
2154 
2155  btAssert(f->copy != stamp);
2156  f->copy = stamp;
2157  f->face = face;
2158 
2159  a = b;
2160  b = f->target;
2161 
2162  f = f->reverse->prev;
2163  } while (f != e);
2164  }
2165  e = e->next;
2166  } while (e != v->edges);
2167  }
2168  }
2169 
2170  if (volume.getSign() <= 0)
2171  {
2172  return 0;
2173  }
2174 
2175  btVector3 hullCenter;
2176  hullCenter[medAxis] = hullCenterX.toScalar();
2177  hullCenter[maxAxis] = hullCenterY.toScalar();
2178  hullCenter[minAxis] = hullCenterZ.toScalar();
2179  hullCenter /= 4 * volume.toScalar();
2180  hullCenter *= scaling;
2181 
2182  int faceCount = faces.size();
2183 
2184  if (clampAmount > 0)
2185  {
2186  btScalar minDist = SIMD_INFINITY;
2187  for (int i = 0; i < faceCount; i++)
2188  {
2189  btVector3 normal = getBtNormal(faces[i]);
2190  btScalar dist = normal.dot(toBtVector(faces[i]->origin) - hullCenter);
2191  if (dist < minDist)
2192  {
2193  minDist = dist;
2194  }
2195  }
2196 
2197  if (minDist <= 0)
2198  {
2199  return 0;
2200  }
2201 
2202  amount = btMin(amount, minDist * clampAmount);
2203  }
2204 
2205  unsigned int seed = 243703;
2206  for (int i = 0; i < faceCount; i++, seed = 1664525 * seed + 1013904223)
2207  {
2208  btSwap(faces[i], faces[seed % faceCount]);
2209  }
2210 
2211  for (int i = 0; i < faceCount; i++)
2212  {
2213  if (!shiftFace(faces[i], amount, stack))
2214  {
2215  return -amount;
2216  }
2217  }
2218 
2219  return amount;
2220 }
2221 
2223 {
2224  btVector3 origShift = getBtNormal(face) * -amount;
2225  if (scaling[0] != 0)
2226  {
2227  origShift[0] /= scaling[0];
2228  }
2229  if (scaling[1] != 0)
2230  {
2231  origShift[1] /= scaling[1];
2232  }
2233  if (scaling[2] != 0)
2234  {
2235  origShift[2] /= scaling[2];
2236  }
2237  Point32 shift((int32_t) origShift[medAxis], (int32_t) origShift[maxAxis], (int32_t) origShift[minAxis]);
2238  if (shift.isZero())
2239  {
2240  return true;
2241  }
2242  Point64 normal = face->getNormal();
2243 #ifdef DEBUG_CONVEX_HULL
2244  printf("\nShrinking face (%d %d %d) (%d %d %d) (%d %d %d) by (%d %d %d)\n",
2245  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);
2246 #endif
2247  int64_t origDot = face->origin.dot(normal);
2248  Point32 shiftedOrigin = face->origin + shift;
2249  int64_t shiftedDot = shiftedOrigin.dot(normal);
2250  btAssert(shiftedDot <= origDot);
2251  if (shiftedDot >= origDot)
2252  {
2253  return false;
2254  }
2255 
2256  Edge* intersection = NULL;
2257 
2258  Edge* startEdge = face->nearbyVertex->edges;
2259 #ifdef DEBUG_CONVEX_HULL
2260  printf("Start edge is ");
2261  startEdge->print();
2262  printf(", normal is (%lld %lld %lld), shifted dot is %lld\n", normal.x, normal.y, normal.z, shiftedDot);
2263 #endif
2264  Rational128 optDot = face->nearbyVertex->dot(normal);
2265  int cmp = optDot.compare(shiftedDot);
2266 #ifdef SHOW_ITERATIONS
2267  int n = 0;
2268 #endif
2269  if (cmp >= 0)
2270  {
2271  Edge* e = startEdge;
2272  do
2273  {
2274 #ifdef SHOW_ITERATIONS
2275  n++;
2276 #endif
2277  Rational128 dot = e->target->dot(normal);
2278  btAssert(dot.compare(origDot) <= 0);
2279 #ifdef DEBUG_CONVEX_HULL
2280  printf("Moving downwards, edge is ");
2281  e->print();
2282  printf(", dot is %f (%f %lld)\n", (float) dot.toScalar(), (float) optDot.toScalar(), shiftedDot);
2283 #endif
2284  if (dot.compare(optDot) < 0)
2285  {
2286  int c = dot.compare(shiftedDot);
2287  optDot = dot;
2288  e = e->reverse;
2289  startEdge = e;
2290  if (c < 0)
2291  {
2292  intersection = e;
2293  break;
2294  }
2295  cmp = c;
2296  }
2297  e = e->prev;
2298  } while (e != startEdge);
2299 
2300  if (!intersection)
2301  {
2302  return false;
2303  }
2304  }
2305  else
2306  {
2307  Edge* e = startEdge;
2308  do
2309  {
2310 #ifdef SHOW_ITERATIONS
2311  n++;
2312 #endif
2313  Rational128 dot = e->target->dot(normal);
2314  btAssert(dot.compare(origDot) <= 0);
2315 #ifdef DEBUG_CONVEX_HULL
2316  printf("Moving upwards, edge is ");
2317  e->print();
2318  printf(", dot is %f (%f %lld)\n", (float) dot.toScalar(), (float) optDot.toScalar(), shiftedDot);
2319 #endif
2320  if (dot.compare(optDot) > 0)
2321  {
2322  cmp = dot.compare(shiftedDot);
2323  if (cmp >= 0)
2324  {
2325  intersection = e;
2326  break;
2327  }
2328  optDot = dot;
2329  e = e->reverse;
2330  startEdge = e;
2331  }
2332  e = e->prev;
2333  } while (e != startEdge);
2334 
2335  if (!intersection)
2336  {
2337  return true;
2338  }
2339  }
2340 
2341 #ifdef SHOW_ITERATIONS
2342  printf("Needed %d iterations to find initial intersection\n", n);
2343 #endif
2344 
2345  if (cmp == 0)
2346  {
2347  Edge* e = intersection->reverse->next;
2348 #ifdef SHOW_ITERATIONS
2349  n = 0;
2350 #endif
2351  while (e->target->dot(normal).compare(shiftedDot) <= 0)
2352  {
2353 #ifdef SHOW_ITERATIONS
2354  n++;
2355 #endif
2356  e = e->next;
2357  if (e == intersection->reverse)
2358  {
2359  return true;
2360  }
2361 #ifdef DEBUG_CONVEX_HULL
2362  printf("Checking for outwards edge, current edge is ");
2363  e->print();
2364  printf("\n");
2365 #endif
2366  }
2367 #ifdef SHOW_ITERATIONS
2368  printf("Needed %d iterations to check for complete containment\n", n);
2369 #endif
2370  }
2371 
2372  Edge* firstIntersection = NULL;
2373  Edge* faceEdge = NULL;
2374  Edge* firstFaceEdge = NULL;
2375 
2376 #ifdef SHOW_ITERATIONS
2377  int m = 0;
2378 #endif
2379  while (true)
2380  {
2381 #ifdef SHOW_ITERATIONS
2382  m++;
2383 #endif
2384 #ifdef DEBUG_CONVEX_HULL
2385  printf("Intersecting edge is ");
2386  intersection->print();
2387  printf("\n");
2388 #endif
2389  if (cmp == 0)
2390  {
2391  Edge* e = intersection->reverse->next;
2392  startEdge = e;
2393 #ifdef SHOW_ITERATIONS
2394  n = 0;
2395 #endif
2396  while (true)
2397  {
2398 #ifdef SHOW_ITERATIONS
2399  n++;
2400 #endif
2401  if (e->target->dot(normal).compare(shiftedDot) >= 0)
2402  {
2403  break;
2404  }
2405  intersection = e->reverse;
2406  e = e->next;
2407  if (e == startEdge)
2408  {
2409  return true;
2410  }
2411  }
2412 #ifdef SHOW_ITERATIONS
2413  printf("Needed %d iterations to advance intersection\n", n);
2414 #endif
2415  }
2416 
2417 #ifdef DEBUG_CONVEX_HULL
2418  printf("Advanced intersecting edge to ");
2419  intersection->print();
2420  printf(", cmp = %d\n", cmp);
2421 #endif
2422 
2423  if (!firstIntersection)
2424  {
2425  firstIntersection = intersection;
2426  }
2427  else if (intersection == firstIntersection)
2428  {
2429  break;
2430  }
2431 
2432  int prevCmp = cmp;
2433  Edge* prevIntersection = intersection;
2434  Edge* prevFaceEdge = faceEdge;
2435 
2436  Edge* e = intersection->reverse;
2437 #ifdef SHOW_ITERATIONS
2438  n = 0;
2439 #endif
2440  while (true)
2441  {
2442 #ifdef SHOW_ITERATIONS
2443  n++;
2444 #endif
2445  e = e->reverse->prev;
2446  btAssert(e != intersection->reverse);
2447  cmp = e->target->dot(normal).compare(shiftedDot);
2448 #ifdef DEBUG_CONVEX_HULL
2449  printf("Testing edge ");
2450  e->print();
2451  printf(" -> cmp = %d\n", cmp);
2452 #endif
2453  if (cmp >= 0)
2454  {
2455  intersection = e;
2456  break;
2457  }
2458  }
2459 #ifdef SHOW_ITERATIONS
2460  printf("Needed %d iterations to find other intersection of face\n", n);
2461 #endif
2462 
2463  if (cmp > 0)
2464  {
2465  Vertex* removed = intersection->target;
2466  e = intersection->reverse;
2467  if (e->prev == e)
2468  {
2469  removed->edges = NULL;
2470  }
2471  else
2472  {
2473  removed->edges = e->prev;
2474  e->prev->link(e->next);
2475  e->link(e);
2476  }
2477 #ifdef DEBUG_CONVEX_HULL
2478  printf("1: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z);
2479 #endif
2480 
2481  Point64 n0 = intersection->face->getNormal();
2482  Point64 n1 = intersection->reverse->face->getNormal();
2483  int64_t m00 = face->dir0.dot(n0);
2484  int64_t m01 = face->dir1.dot(n0);
2485  int64_t m10 = face->dir0.dot(n1);
2486  int64_t m11 = face->dir1.dot(n1);
2487  int64_t r0 = (intersection->face->origin - shiftedOrigin).dot(n0);
2488  int64_t r1 = (intersection->reverse->face->origin - shiftedOrigin).dot(n1);
2489  Int128 det = Int128::mul(m00, m11) - Int128::mul(m01, m10);
2490  btAssert(det.getSign() != 0);
2491  Vertex* v = vertexPool.newObject();
2492  v->point.index = -1;
2493  v->copy = -1;
2494  v->point128 = PointR128(Int128::mul(face->dir0.x * r0, m11) - Int128::mul(face->dir0.x * r1, m01)
2495  + Int128::mul(face->dir1.x * r1, m00) - Int128::mul(face->dir1.x * r0, m10) + det * shiftedOrigin.x,
2496  Int128::mul(face->dir0.y * r0, m11) - Int128::mul(face->dir0.y * r1, m01)
2497  + Int128::mul(face->dir1.y * r1, m00) - Int128::mul(face->dir1.y * r0, m10) + det * shiftedOrigin.y,
2498  Int128::mul(face->dir0.z * r0, m11) - Int128::mul(face->dir0.z * r1, m01)
2499  + Int128::mul(face->dir1.z * r1, m00) - Int128::mul(face->dir1.z * r0, m10) + det * shiftedOrigin.z,
2500  det);
2501  v->point.x = (int32_t) v->point128.xvalue();
2502  v->point.y = (int32_t) v->point128.yvalue();
2503  v->point.z = (int32_t) v->point128.zvalue();
2504  intersection->target = v;
2505  v->edges = e;
2506 
2507  stack.push_back(v);
2508  stack.push_back(removed);
2509  stack.push_back(NULL);
2510  }
2511 
2512  if (cmp || prevCmp || (prevIntersection->reverse->next->target != intersection->target))
2513  {
2514  faceEdge = newEdgePair(prevIntersection->target, intersection->target);
2515  if (prevCmp == 0)
2516  {
2517  faceEdge->link(prevIntersection->reverse->next);
2518  }
2519  if ((prevCmp == 0) || prevFaceEdge)
2520  {
2521  prevIntersection->reverse->link(faceEdge);
2522  }
2523  if (cmp == 0)
2524  {
2525  intersection->reverse->prev->link(faceEdge->reverse);
2526  }
2527  faceEdge->reverse->link(intersection->reverse);
2528  }
2529  else
2530  {
2531  faceEdge = prevIntersection->reverse->next;
2532  }
2533 
2534  if (prevFaceEdge)
2535  {
2536  if (prevCmp > 0)
2537  {
2538  faceEdge->link(prevFaceEdge->reverse);
2539  }
2540  else if (faceEdge != prevFaceEdge->reverse)
2541  {
2542  stack.push_back(prevFaceEdge->target);
2543  while (faceEdge->next != prevFaceEdge->reverse)
2544  {
2545  Vertex* removed = faceEdge->next->target;
2546  removeEdgePair(faceEdge->next);
2547  stack.push_back(removed);
2548 #ifdef DEBUG_CONVEX_HULL
2549  printf("2: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z);
2550 #endif
2551  }
2552  stack.push_back(NULL);
2553  }
2554  }
2555  faceEdge->face = face;
2556  faceEdge->reverse->face = intersection->face;
2557 
2558  if (!firstFaceEdge)
2559  {
2560  firstFaceEdge = faceEdge;
2561  }
2562  }
2563 #ifdef SHOW_ITERATIONS
2564  printf("Needed %d iterations to process all intersections\n", m);
2565 #endif
2566 
2567  if (cmp > 0)
2568  {
2569  firstFaceEdge->reverse->target = faceEdge->target;
2570  firstIntersection->reverse->link(firstFaceEdge);
2571  firstFaceEdge->link(faceEdge->reverse);
2572  }
2573  else if (firstFaceEdge != faceEdge->reverse)
2574  {
2575  stack.push_back(faceEdge->target);
2576  while (firstFaceEdge->next != faceEdge->reverse)
2577  {
2578  Vertex* removed = firstFaceEdge->next->target;
2579  removeEdgePair(firstFaceEdge->next);
2580  stack.push_back(removed);
2581 #ifdef DEBUG_CONVEX_HULL
2582  printf("3: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z);
2583 #endif
2584  }
2585  stack.push_back(NULL);
2586  }
2587 
2588  btAssert(stack.size() > 0);
2589  vertexList = stack[0];
2590 
2591 #ifdef DEBUG_CONVEX_HULL
2592  printf("Removing part\n");
2593 #endif
2594 #ifdef SHOW_ITERATIONS
2595  n = 0;
2596 #endif
2597  int pos = 0;
2598  while (pos < stack.size())
2599  {
2600  int end = stack.size();
2601  while (pos < end)
2602  {
2603  Vertex* kept = stack[pos++];
2604 #ifdef DEBUG_CONVEX_HULL
2605  kept->print();
2606 #endif
2607  bool deeper = false;
2608  Vertex* removed;
2609  while ((removed = stack[pos++]) != NULL)
2610  {
2611 #ifdef SHOW_ITERATIONS
2612  n++;
2613 #endif
2614  kept->receiveNearbyFaces(removed);
2615  while (removed->edges)
2616  {
2617  if (!deeper)
2618  {
2619  deeper = true;
2620  stack.push_back(kept);
2621  }
2622  stack.push_back(removed->edges->target);
2623  removeEdgePair(removed->edges);
2624  }
2625  }
2626  if (deeper)
2627  {
2628  stack.push_back(NULL);
2629  }
2630  }
2631  }
2632 #ifdef SHOW_ITERATIONS
2633  printf("Needed %d iterations to remove part\n", n);
2634 #endif
2635 
2636  stack.resize(0);
2637  face->origin = shiftedOrigin;
2638 
2639  return true;
2640 }
2641 
2642 
2644 {
2645  int index = vertex->copy;
2646  if (index < 0)
2647  {
2648  index = vertices.size();
2649  vertex->copy = index;
2650  vertices.push_back(vertex);
2651 #ifdef DEBUG_CONVEX_HULL
2652  printf("Vertex %d gets index *%d\n", vertex->point.index, index);
2653 #endif
2654  }
2655  return index;
2656 }
2657 
2658 btScalar btConvexHullComputer::compute(const void* coords, bool doubleCoords, int stride, int count, btScalar shrink, btScalar shrinkClamp)
2659 {
2660  if (count <= 0)
2661  {
2662  vertices.clear();
2663  edges.clear();
2664  faces.clear();
2665  return 0;
2666  }
2667 
2668  btConvexHullInternal hull;
2669  hull.compute(coords, doubleCoords, stride, count);
2670 
2671  btScalar shift = 0;
2672  if ((shrink > 0) && ((shift = hull.shrink(shrink, shrinkClamp)) < 0))
2673  {
2674  vertices.clear();
2675  edges.clear();
2676  faces.clear();
2677  return shift;
2678  }
2679 
2680  vertices.resize(0);
2681  edges.resize(0);
2682  faces.resize(0);
2683 
2685  getVertexCopy(hull.vertexList, oldVertices);
2686  int copied = 0;
2687  while (copied < oldVertices.size())
2688  {
2689  btConvexHullInternal::Vertex* v = oldVertices[copied];
2690  vertices.push_back(hull.getCoordinates(v));
2691  btConvexHullInternal::Edge* firstEdge = v->edges;
2692  if (firstEdge)
2693  {
2694  int firstCopy = -1;
2695  int prevCopy = -1;
2696  btConvexHullInternal::Edge* e = firstEdge;
2697  do
2698  {
2699  if (e->copy < 0)
2700  {
2701  int s = edges.size();
2702  edges.push_back(Edge());
2703  edges.push_back(Edge());
2704  Edge* c = &edges[s];
2705  Edge* r = &edges[s + 1];
2706  e->copy = s;
2707  e->reverse->copy = s + 1;
2708  c->reverse = 1;
2709  r->reverse = -1;
2710  c->targetVertex = getVertexCopy(e->target, oldVertices);
2711  r->targetVertex = copied;
2712 #ifdef DEBUG_CONVEX_HULL
2713  printf(" CREATE: Vertex *%d has edge to *%d\n", copied, c->getTargetVertex());
2714 #endif
2715  }
2716  if (prevCopy >= 0)
2717  {
2718  edges[e->copy].next = prevCopy - e->copy;
2719  }
2720  else
2721  {
2722  firstCopy = e->copy;
2723  }
2724  prevCopy = e->copy;
2725  e = e->next;
2726  } while (e != firstEdge);
2727  edges[firstCopy].next = prevCopy - firstCopy;
2728  }
2729  copied++;
2730  }
2731 
2732  for (int i = 0; i < copied; i++)
2733  {
2734  btConvexHullInternal::Vertex* v = oldVertices[i];
2735  btConvexHullInternal::Edge* firstEdge = v->edges;
2736  if (firstEdge)
2737  {
2738  btConvexHullInternal::Edge* e = firstEdge;
2739  do
2740  {
2741  if (e->copy >= 0)
2742  {
2743 #ifdef DEBUG_CONVEX_HULL
2744  printf("Vertex *%d has edge to *%d\n", i, edges[e->copy].getTargetVertex());
2745 #endif
2746  faces.push_back(e->copy);
2748  do
2749  {
2750 #ifdef DEBUG_CONVEX_HULL
2751  printf(" Face *%d\n", edges[f->copy].getTargetVertex());
2752 #endif
2753  f->copy = -1;
2754  f = f->reverse->prev;
2755  } while (f != e);
2756  }
2757  e = e->next;
2758  } while (e != firstEdge);
2759  }
2760  }
2761 
2762  return shift;
2763 }
2764 
2765 
2766 
2767 
2768 
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...
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)
static DBVT_INLINE btScalar size(const btDbvtVolume &a)
Definition: btDbvt.cpp:52
#define btAssert(x)
Definition: btScalar.h:131
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:522
Point64 cross(const Point64 &b) const
static uint32_t low(uint64_t value)
bool operator<(const Int128 &b) const
static void mul(btAlignedObjectArray< T > &items, const Q &value)
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:621
Point32 operator-(const Point32 &b) const
void compute(const void *coords, bool doubleCoords, int stride, int count)
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:389
Edge * findMaxAngle(bool ccw, const Vertex *start, const Point32 &s, const Point64 &rxs, const Point64 &sxrxs, Rational64 &minCot)
#define btAlignedFree(ptr)
btMatrix3x3 operator*(const btMatrix3x3 &m, const btScalar &k)
Definition: btMatrix3x3.h:908
int64_t dot(const Point32 &b) const
Edge * newEdgePair(Vertex *from, Vertex *to)
Int128 & operator+=(const Int128 &b)
static void mul(UWord a, UWord b, UWord &resLow, UWord &resHigh)
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:495
Rational128 dot(const Point64 &b) const
btAlignedObjectArray< Vertex * > originalVertices
btVector3 normalized() const
Return a normalized version of this vector.
Definition: btVector3.h:966
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)
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:480
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)
void setMax(const btVector3 &other)
Set each element to the max of the current values and the values of another btVector3.
Definition: btVector3.h:621
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:292
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:487
bool operator!=(const Point32 &b) const
Point64 cross(const Point32 &b) const