diff options
Diffstat (limited to 'thirdparty/vhacd/src/btConvexHullComputer.cpp')
| -rw-r--r-- | thirdparty/vhacd/src/btConvexHullComputer.cpp | 4274 |
1 files changed, 2209 insertions, 2065 deletions
diff --git a/thirdparty/vhacd/src/btConvexHullComputer.cpp b/thirdparty/vhacd/src/btConvexHullComputer.cpp index 15956fc0c4..d3d749adbe 100644 --- a/thirdparty/vhacd/src/btConvexHullComputer.cpp +++ b/thirdparty/vhacd/src/btConvexHullComputer.cpp @@ -3,8 +3,8 @@ Copyright (c) 2011 Ole Kniemeyer, MAXON, www.maxon.net This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions: 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. @@ -34,7 +34,7 @@ typedef unsigned long long int32_t uint64_t; #endif #ifdef _MSC_VER -#pragma warning(disable : 4458) +#pragma warning(disable:4458) #endif //The definition of USE_X86_64_ASM is moved into the build system. You can enable it manually by commenting out the following lines @@ -49,2287 +49,2431 @@ typedef unsigned long long int32_t uint64_t; #include <stdio.h> #endif -//GODOT ADDITION -namespace VHACD { -// - // Convex hull implementation based on Preparata and Hong // Ole Kniemeyer, MAXON Computer GmbH class btConvexHullInternal { public: - class Point64 { - public: - int64_t x; - int64_t y; - int64_t z; - - Point64(int64_t x, int64_t y, int64_t z) : - x(x), - y(y), - z(z) { - } - - bool isZero() { - return (x == 0) && (y == 0) && (z == 0); - } - - int64_t dot(const Point64 &b) const { - return x * b.x + y * b.y + z * b.z; - } - }; - - class Point32 { - public: - int32_t x; - int32_t y; - int32_t z; - int32_t index; - - Point32() { - } - - Point32(int32_t x, int32_t y, int32_t z) : - x(x), - y(y), - z(z), - index(-1) { - } - - bool operator==(const Point32 &b) const { - return (x == b.x) && (y == b.y) && (z == b.z); - } - - bool operator!=(const Point32 &b) const { - return (x != b.x) || (y != b.y) || (z != b.z); - } - - bool isZero() { - return (x == 0) && (y == 0) && (z == 0); - } - - Point64 cross(const Point32 &b) const { - return Point64(y * b.z - z * b.y, z * b.x - x * b.z, x * b.y - y * b.x); - } - - Point64 cross(const Point64 &b) const { - return Point64(y * b.z - z * b.y, z * b.x - x * b.z, x * b.y - y * b.x); - } - - int64_t dot(const Point32 &b) const { - return x * b.x + y * b.y + z * b.z; - } - - int64_t dot(const Point64 &b) const { - return x * b.x + y * b.y + z * b.z; - } - - Point32 operator+(const Point32 &b) const { - return Point32(x + b.x, y + b.y, z + b.z); - } - - Point32 operator-(const Point32 &b) const { - return Point32(x - b.x, y - b.y, z - b.z); - } - }; - - class Int128 { - public: - uint64_t low; - uint64_t high; - - Int128() { - } - - Int128(uint64_t low, uint64_t high) : - low(low), - high(high) { - } - - Int128(uint64_t low) : - low(low), - high(0) { - } - - Int128(int64_t value) : - low(value), - high((value >= 0) ? 0 : (uint64_t)-1LL) { - } - - static Int128 mul(int64_t a, int64_t b); - - static Int128 mul(uint64_t a, uint64_t b); - - Int128 operator-() const { - return Int128((uint64_t) - (int64_t)low, ~high + (low == 0)); - } - - Int128 operator+(const Int128 &b) const { + class Point64 { + public: + int64_t x; + int64_t y; + int64_t z; + + Point64(int64_t x, int64_t y, int64_t z) + : x(x) + , y(y) + , z(z) + { + } + + bool isZero() + { + return (x == 0) && (y == 0) && (z == 0); + } + + int64_t dot(const Point64& b) const + { + return x * b.x + y * b.y + z * b.z; + } + }; + + class Point32 { + public: + int32_t x; + int32_t y; + int32_t z; + int32_t index; + + Point32() + { + } + + Point32(int32_t x, int32_t y, int32_t z) + : x(x) + , y(y) + , z(z) + , index(-1) + { + } + + bool operator==(const Point32& b) const + { + return (x == b.x) && (y == b.y) && (z == b.z); + } + + bool operator!=(const Point32& b) const + { + return (x != b.x) || (y != b.y) || (z != b.z); + } + + bool isZero() + { + return (x == 0) && (y == 0) && (z == 0); + } + + Point64 cross(const Point32& b) const + { + return Point64(y * b.z - z * b.y, z * b.x - x * b.z, x * b.y - y * b.x); + } + + Point64 cross(const Point64& b) const + { + return Point64(y * b.z - z * b.y, z * b.x - x * b.z, x * b.y - y * b.x); + } + + int64_t dot(const Point32& b) const + { + return x * b.x + y * b.y + z * b.z; + } + + int64_t dot(const Point64& b) const + { + return x * b.x + y * b.y + z * b.z; + } + + Point32 operator+(const Point32& b) const + { + return Point32(x + b.x, y + b.y, z + b.z); + } + + Point32 operator-(const Point32& b) const + { + return Point32(x - b.x, y - b.y, z - b.z); + } + }; + + class Int128 { + public: + uint64_t low; + uint64_t high; + + Int128() + { + } + + Int128(uint64_t low, uint64_t high) + : low(low) + , high(high) + { + } + + Int128(uint64_t low) + : low(low) + , high(0) + { + } + + Int128(int64_t value) + : low(value) + , high((value >= 0) ? 0 : (uint64_t)-1LL) + { + } + + static Int128 mul(int64_t a, int64_t b); + + static Int128 mul(uint64_t a, uint64_t b); + + Int128 operator-() const + { + return Int128((uint64_t) - (int64_t)low, ~high + (low == 0)); + } + + Int128 operator+(const Int128& b) const + { #ifdef USE_X86_64_ASM - Int128 result; - __asm__("addq %[bl], %[rl]\n\t" - "adcq %[bh], %[rh]\n\t" - : [rl] "=r"(result.low), [rh] "=r"(result.high) - : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high) - : "cc"); - return result; + Int128 result; + __asm__("addq %[bl], %[rl]\n\t" + "adcq %[bh], %[rh]\n\t" + : [rl] "=r"(result.low), [rh] "=r"(result.high) + : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high) + : "cc"); + return result; #else - uint64_t lo = low + b.low; - return Int128(lo, high + b.high + (lo < low)); + uint64_t lo = low + b.low; + return Int128(lo, high + b.high + (lo < low)); #endif - } + } - Int128 operator-(const Int128 &b) const { + Int128 operator-(const Int128& b) const + { #ifdef USE_X86_64_ASM - Int128 result; - __asm__("subq %[bl], %[rl]\n\t" - "sbbq %[bh], %[rh]\n\t" - : [rl] "=r"(result.low), [rh] "=r"(result.high) - : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high) - : "cc"); - return result; + Int128 result; + __asm__("subq %[bl], %[rl]\n\t" + "sbbq %[bh], %[rh]\n\t" + : [rl] "=r"(result.low), [rh] "=r"(result.high) + : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high) + : "cc"); + return result; #else - return *this + -b; + return *this + -b; #endif - } + } - Int128 &operator+=(const Int128 &b) { + Int128& operator+=(const Int128& b) + { #ifdef USE_X86_64_ASM - __asm__("addq %[bl], %[rl]\n\t" - "adcq %[bh], %[rh]\n\t" - : [rl] "=r"(low), [rh] "=r"(high) - : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high) - : "cc"); + __asm__("addq %[bl], %[rl]\n\t" + "adcq %[bh], %[rh]\n\t" + : [rl] "=r"(low), [rh] "=r"(high) + : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high) + : "cc"); #else - uint64_t lo = low + b.low; - if (lo < low) { - ++high; - } - low = lo; - high += b.high; + uint64_t lo = low + b.low; + if (lo < low) { + ++high; + } + low = lo; + high += b.high; #endif - return *this; - } - - Int128 &operator++() { - if (++low == 0) { - ++high; - } - return *this; - } - - Int128 operator*(int64_t b) const; - - btScalar toScalar() const { - return ((int64_t)high >= 0) ? btScalar(high) * (btScalar(0x100000000LL) * btScalar(0x100000000LL)) + btScalar(low) : -(-*this).toScalar(); - } - - int32_t getSign() const { - return ((int64_t)high < 0) ? -1 : (high || low) ? 1 : 0; - } - - bool operator<(const Int128 &b) const { - return (high < b.high) || ((high == b.high) && (low < b.low)); - } - - int32_t ucmp(const Int128 &b) const { - if (high < b.high) { - return -1; - } - if (high > b.high) { - return 1; - } - if (low < b.low) { - return -1; - } - if (low > b.low) { - return 1; - } - return 0; - } - }; - - class Rational64 { - private: - uint64_t m_numerator; - uint64_t m_denominator; - int32_t sign; - - public: - Rational64(int64_t numerator, int64_t denominator) { - if (numerator > 0) { - sign = 1; - m_numerator = (uint64_t)numerator; - } else if (numerator < 0) { - sign = -1; - m_numerator = (uint64_t)-numerator; - } else { - sign = 0; - m_numerator = 0; - } - if (denominator > 0) { - m_denominator = (uint64_t)denominator; - } else if (denominator < 0) { - sign = -sign; - m_denominator = (uint64_t)-denominator; - } else { - m_denominator = 0; - } - } - - bool isNegativeInfinity() const { - return (sign < 0) && (m_denominator == 0); - } - - bool isNaN() const { - return (sign == 0) && (m_denominator == 0); - } - - int32_t compare(const Rational64 &b) const; - - btScalar toScalar() const { - return sign * ((m_denominator == 0) ? SIMD_INFINITY : (btScalar)m_numerator / m_denominator); - } - }; - - class Rational128 { - private: - Int128 numerator; - Int128 denominator; - int32_t sign; - bool isInt64; - - public: - Rational128(int64_t value) { - if (value > 0) { - sign = 1; - this->numerator = value; - } else if (value < 0) { - sign = -1; - this->numerator = -value; - } else { - sign = 0; - this->numerator = (uint64_t)0; - } - this->denominator = (uint64_t)1; - isInt64 = true; - } - - Rational128(const Int128 &numerator, const Int128 &denominator) { - sign = numerator.getSign(); - if (sign >= 0) { - this->numerator = numerator; - } else { - this->numerator = -numerator; - } - int32_t dsign = denominator.getSign(); - if (dsign >= 0) { - this->denominator = denominator; - } else { - sign = -sign; - this->denominator = -denominator; - } - isInt64 = false; - } - - int32_t compare(const Rational128 &b) const; - - int32_t compare(int64_t b) const; - - btScalar toScalar() const { - return sign * ((denominator.getSign() == 0) ? SIMD_INFINITY : numerator.toScalar() / denominator.toScalar()); - } - }; - - class PointR128 { - public: - Int128 x; - Int128 y; - Int128 z; - Int128 denominator; - - PointR128() { - } - - PointR128(Int128 x, Int128 y, Int128 z, Int128 denominator) : - x(x), - y(y), - z(z), - denominator(denominator) { - } - - btScalar xvalue() const { - return x.toScalar() / denominator.toScalar(); - } - - btScalar yvalue() const { - return y.toScalar() / denominator.toScalar(); - } - - btScalar zvalue() const { - return z.toScalar() / denominator.toScalar(); - } - }; - - class Edge; - class Face; - - class Vertex { - public: - Vertex *next; - Vertex *prev; - Edge *edges; - Face *firstNearbyFace; - Face *lastNearbyFace; - PointR128 point128; - Point32 point; - int32_t copy; - - Vertex() : - next(NULL), - prev(NULL), - edges(NULL), - firstNearbyFace(NULL), - lastNearbyFace(NULL), - copy(-1) { - } + return *this; + } + + Int128& operator++() + { + if (++low == 0) { + ++high; + } + return *this; + } + + Int128 operator*(int64_t b) const; + + btScalar toScalar() const + { + return ((int64_t)high >= 0) ? btScalar(high) * (btScalar(0x100000000LL) * btScalar(0x100000000LL)) + btScalar(low) + : -(-*this).toScalar(); + } + + int32_t getSign() const + { + return ((int64_t)high < 0) ? -1 : (high || low) ? 1 : 0; + } + + bool operator<(const Int128& b) const + { + return (high < b.high) || ((high == b.high) && (low < b.low)); + } + + int32_t ucmp(const Int128& b) const + { + if (high < b.high) { + return -1; + } + if (high > b.high) { + return 1; + } + if (low < b.low) { + return -1; + } + if (low > b.low) { + return 1; + } + return 0; + } + }; + + class Rational64 { + private: + uint64_t m_numerator; + uint64_t m_denominator; + int32_t sign; + + public: + Rational64(int64_t numerator, int64_t denominator) + { + if (numerator > 0) { + sign = 1; + m_numerator = (uint64_t)numerator; + } + else if (numerator < 0) { + sign = -1; + m_numerator = (uint64_t)-numerator; + } + else { + sign = 0; + m_numerator = 0; + } + if (denominator > 0) { + m_denominator = (uint64_t)denominator; + } + else if (denominator < 0) { + sign = -sign; + m_denominator = (uint64_t)-denominator; + } + else { + m_denominator = 0; + } + } + + bool isNegativeInfinity() const + { + return (sign < 0) && (m_denominator == 0); + } + + bool isNaN() const + { + return (sign == 0) && (m_denominator == 0); + } + + int32_t compare(const Rational64& b) const; + + btScalar toScalar() const + { + return sign * ((m_denominator == 0) ? SIMD_INFINITY : (btScalar)m_numerator / m_denominator); + } + }; + + class Rational128 { + private: + Int128 numerator; + Int128 denominator; + int32_t sign; + bool isInt64; + + public: + Rational128(int64_t value) + { + if (value > 0) { + sign = 1; + this->numerator = value; + } + else if (value < 0) { + sign = -1; + this->numerator = -value; + } + else { + sign = 0; + this->numerator = (uint64_t)0; + } + this->denominator = (uint64_t)1; + isInt64 = true; + } + + Rational128(const Int128& numerator, const Int128& denominator) + { + sign = numerator.getSign(); + if (sign >= 0) { + this->numerator = numerator; + } + else { + this->numerator = -numerator; + } + int32_t dsign = denominator.getSign(); + if (dsign >= 0) { + this->denominator = denominator; + } + else { + sign = -sign; + this->denominator = -denominator; + } + isInt64 = false; + } + + int32_t compare(const Rational128& b) const; + + int32_t compare(int64_t b) const; + + btScalar toScalar() const + { + return sign * ((denominator.getSign() == 0) ? SIMD_INFINITY : numerator.toScalar() / denominator.toScalar()); + } + }; + + class PointR128 { + public: + Int128 x; + Int128 y; + Int128 z; + Int128 denominator; + + PointR128() + { + } + + PointR128(Int128 x, Int128 y, Int128 z, Int128 denominator) + : x(x) + , y(y) + , z(z) + , denominator(denominator) + { + } + + btScalar xvalue() const + { + return x.toScalar() / denominator.toScalar(); + } + + btScalar yvalue() const + { + return y.toScalar() / denominator.toScalar(); + } + + btScalar zvalue() const + { + return z.toScalar() / denominator.toScalar(); + } + }; + + class Edge; + class Face; + + class Vertex { + public: + Vertex* next; + Vertex* prev; + Edge* edges; + Face* firstNearbyFace; + Face* lastNearbyFace; + PointR128 point128; + Point32 point; + int32_t copy; + + Vertex() + : next(NULL) + , prev(NULL) + , edges(NULL) + , firstNearbyFace(NULL) + , lastNearbyFace(NULL) + , copy(-1) + { + } #ifdef DEBUG_CONVEX_HULL - void print() { - printf("V%d (%d, %d, %d)", point.index, point.x, point.y, point.z); - } + void print() + { + printf("V%d (%d, %d, %d)", point.index, point.x, point.y, point.z); + } - void printGraph(); + void printGraph(); #endif - Point32 operator-(const Vertex &b) const { - return point - b.point; - } - - Rational128 dot(const Point64 &b) const { - return (point.index >= 0) ? Rational128(point.dot(b)) : Rational128(point128.x * b.x + point128.y * b.y + point128.z * b.z, point128.denominator); - } - - btScalar xvalue() const { - return (point.index >= 0) ? btScalar(point.x) : point128.xvalue(); - } - - btScalar yvalue() const { - return (point.index >= 0) ? btScalar(point.y) : point128.yvalue(); - } - - btScalar zvalue() const { - return (point.index >= 0) ? btScalar(point.z) : point128.zvalue(); - } - - void receiveNearbyFaces(Vertex *src) { - if (lastNearbyFace) { - lastNearbyFace->nextWithSameNearbyVertex = src->firstNearbyFace; - } else { - firstNearbyFace = src->firstNearbyFace; - } - if (src->lastNearbyFace) { - lastNearbyFace = src->lastNearbyFace; - } - for (Face *f = src->firstNearbyFace; f; f = f->nextWithSameNearbyVertex) { - btAssert(f->nearbyVertex == src); - f->nearbyVertex = this; - } - src->firstNearbyFace = NULL; - src->lastNearbyFace = NULL; - } - }; - - class Edge { - public: - Edge *next; - Edge *prev; - Edge *reverse; - Vertex *target; - Face *face; - int32_t copy; - - ~Edge() { - next = NULL; - prev = NULL; - reverse = NULL; - target = NULL; - face = NULL; - } - - void link(Edge *n) { - btAssert(reverse->target == n->reverse->target); - next = n; - n->prev = this; - } + Point32 operator-(const Vertex& b) const + { + return point - b.point; + } + + Rational128 dot(const Point64& b) const + { + return (point.index >= 0) ? Rational128(point.dot(b)) + : Rational128(point128.x * b.x + point128.y * b.y + point128.z * b.z, point128.denominator); + } + + btScalar xvalue() const + { + return (point.index >= 0) ? btScalar(point.x) : point128.xvalue(); + } + + btScalar yvalue() const + { + return (point.index >= 0) ? btScalar(point.y) : point128.yvalue(); + } + + btScalar zvalue() const + { + return (point.index >= 0) ? btScalar(point.z) : point128.zvalue(); + } + + void receiveNearbyFaces(Vertex* src) + { + if (lastNearbyFace) { + lastNearbyFace->nextWithSameNearbyVertex = src->firstNearbyFace; + } + else { + firstNearbyFace = src->firstNearbyFace; + } + if (src->lastNearbyFace) { + lastNearbyFace = src->lastNearbyFace; + } + for (Face* f = src->firstNearbyFace; f; f = f->nextWithSameNearbyVertex) { + btAssert(f->nearbyVertex == src); + f->nearbyVertex = this; + } + src->firstNearbyFace = NULL; + src->lastNearbyFace = NULL; + } + }; + + class Edge { + public: + Edge* next; + Edge* prev; + Edge* reverse; + Vertex* target; + Face* face; + int32_t copy; + + ~Edge() + { + next = NULL; + prev = NULL; + reverse = NULL; + target = NULL; + face = NULL; + } + + void link(Edge* n) + { + btAssert(reverse->target == n->reverse->target); + next = n; + n->prev = this; + } #ifdef DEBUG_CONVEX_HULL - void print() { - 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, - reverse->target->point.x, reverse->target->point.y, reverse->target->point.z, target->point.x, target->point.y, target->point.z); - } + void print() + { + 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, + reverse->target->point.x, reverse->target->point.y, reverse->target->point.z, target->point.x, target->point.y, target->point.z); + } #endif - }; - - class Face { - public: - Face *next; - Vertex *nearbyVertex; - Face *nextWithSameNearbyVertex; - Point32 origin; - Point32 dir0; - Point32 dir1; - - Face() : - next(NULL), - nearbyVertex(NULL), - nextWithSameNearbyVertex(NULL) { - } - - void init(Vertex *a, Vertex *b, Vertex *c) { - nearbyVertex = a; - origin = a->point; - dir0 = *b - *a; - dir1 = *c - *a; - if (a->lastNearbyFace) { - a->lastNearbyFace->nextWithSameNearbyVertex = this; - } else { - a->firstNearbyFace = this; - } - a->lastNearbyFace = this; - } - - Point64 getNormal() { - return dir0.cross(dir1); - } - }; - - template <typename UWord, typename UHWord> - class DMul { - private: - static uint32_t high(uint64_t value) { - return (uint32_t)(value >> 32); - } - - static uint32_t low(uint64_t value) { - return (uint32_t)value; - } - - static uint64_t mul(uint32_t a, uint32_t b) { - return (uint64_t)a * (uint64_t)b; - } - - static void shlHalf(uint64_t &value) { - value <<= 32; - } - - static uint64_t high(Int128 value) { - return value.high; - } - - static uint64_t low(Int128 value) { - return value.low; - } - - static Int128 mul(uint64_t a, uint64_t b) { - return Int128::mul(a, b); - } - - static void shlHalf(Int128 &value) { - value.high = value.low; - value.low = 0; - } - - public: - static void mul(UWord a, UWord b, UWord &resLow, UWord &resHigh) { - UWord p00 = mul(low(a), low(b)); - UWord p01 = mul(low(a), high(b)); - UWord p10 = mul(high(a), low(b)); - UWord p11 = mul(high(a), high(b)); - UWord p0110 = UWord(low(p01)) + UWord(low(p10)); - p11 += high(p01); - p11 += high(p10); - p11 += high(p0110); - shlHalf(p0110); - p00 += p0110; - if (p00 < p0110) { - ++p11; - } - resLow = p00; - resHigh = p11; - } - }; + }; + + class Face { + public: + Face* next; + Vertex* nearbyVertex; + Face* nextWithSameNearbyVertex; + Point32 origin; + Point32 dir0; + Point32 dir1; + + Face() + : next(NULL) + , nearbyVertex(NULL) + , nextWithSameNearbyVertex(NULL) + { + } + + void init(Vertex* a, Vertex* b, Vertex* c) + { + nearbyVertex = a; + origin = a->point; + dir0 = *b - *a; + dir1 = *c - *a; + if (a->lastNearbyFace) { + a->lastNearbyFace->nextWithSameNearbyVertex = this; + } + else { + a->firstNearbyFace = this; + } + a->lastNearbyFace = this; + } + + Point64 getNormal() + { + return dir0.cross(dir1); + } + }; + + template <typename UWord, typename UHWord> + class DMul { + private: + static uint32_t high(uint64_t value) + { + return (uint32_t)(value >> 32); + } + + static uint32_t low(uint64_t value) + { + return (uint32_t)value; + } + + static uint64_t mul(uint32_t a, uint32_t b) + { + return (uint64_t)a * (uint64_t)b; + } + + static void shlHalf(uint64_t& value) + { + value <<= 32; + } + + static uint64_t high(Int128 value) + { + return value.high; + } + + static uint64_t low(Int128 value) + { + return value.low; + } + + static Int128 mul(uint64_t a, uint64_t b) + { + return Int128::mul(a, b); + } + + static void shlHalf(Int128& value) + { + value.high = value.low; + value.low = 0; + } + + public: + static void mul(UWord a, UWord b, UWord& resLow, UWord& resHigh) + { + UWord p00 = mul(low(a), low(b)); + UWord p01 = mul(low(a), high(b)); + UWord p10 = mul(high(a), low(b)); + UWord p11 = mul(high(a), high(b)); + UWord p0110 = UWord(low(p01)) + UWord(low(p10)); + p11 += high(p01); + p11 += high(p10); + p11 += high(p0110); + shlHalf(p0110); + p00 += p0110; + if (p00 < p0110) { + ++p11; + } + resLow = p00; + resHigh = p11; + } + }; private: - class IntermediateHull { - public: - Vertex *minXy; - Vertex *maxXy; - Vertex *minYx; - Vertex *maxYx; - - IntermediateHull() : - minXy(NULL), - maxXy(NULL), - minYx(NULL), - maxYx(NULL) { - } - - void print(); - }; - - enum Orientation { NONE, - CLOCKWISE, - COUNTER_CLOCKWISE }; - - template <typename T> - class PoolArray { - private: - T *array; - int32_t size; - - public: - PoolArray<T> *next; - - PoolArray(int32_t size) : - size(size), - next(NULL) { - array = (T *)btAlignedAlloc(sizeof(T) * size, 16); - } - - ~PoolArray() { - btAlignedFree(array); - } - - T *init() { - T *o = array; - for (int32_t i = 0; i < size; i++, o++) { - o->next = (i + 1 < size) ? o + 1 : NULL; - } - return array; - } - }; - - template <typename T> - class Pool { - private: - PoolArray<T> *arrays; - PoolArray<T> *nextArray; - T *freeObjects; - int32_t arraySize; - - public: - Pool() : - arrays(NULL), - nextArray(NULL), - freeObjects(NULL), - arraySize(256) { - } - - ~Pool() { - while (arrays) { - PoolArray<T> *p = arrays; - arrays = p->next; - p->~PoolArray<T>(); - btAlignedFree(p); - } - } - - void reset() { - nextArray = arrays; - freeObjects = NULL; - } - - void setArraySize(int32_t arraySize) { - this->arraySize = arraySize; - } - - T *newObject() { - T *o = freeObjects; - if (!o) { - PoolArray<T> *p = nextArray; - if (p) { - nextArray = p->next; - } else { - p = new (btAlignedAlloc(sizeof(PoolArray<T>), 16)) PoolArray<T>(arraySize); - p->next = arrays; - arrays = p; - } - o = p->init(); - } - freeObjects = o->next; - return new (o) T(); - }; - - void freeObject(T *object) { - object->~T(); - object->next = freeObjects; - freeObjects = object; - } - }; - - btVector3 scaling; - btVector3 center; - Pool<Vertex> vertexPool; - Pool<Edge> edgePool; - Pool<Face> facePool; - btAlignedObjectArray<Vertex *> originalVertices; - int32_t mergeStamp; - int32_t minAxis; - int32_t medAxis; - int32_t maxAxis; - int32_t usedEdgePairs; - int32_t maxUsedEdgePairs; - - static Orientation getOrientation(const Edge *prev, const Edge *next, const Point32 &s, const Point32 &t); - Edge *findMaxAngle(bool ccw, const Vertex *start, const Point32 &s, const Point64 &rxs, const Point64 &sxrxs, Rational64 &minCot); - void findEdgeForCoplanarFaces(Vertex *c0, Vertex *c1, Edge *&e0, Edge *&e1, Vertex *stop0, Vertex *stop1); - - Edge *newEdgePair(Vertex *from, Vertex *to); - - void removeEdgePair(Edge *edge) { - Edge *n = edge->next; - Edge *r = edge->reverse; - - btAssert(edge->target && r->target); - - if (n != edge) { - n->prev = edge->prev; - edge->prev->next = n; - r->target->edges = n; - } else { - r->target->edges = NULL; - } - - n = r->next; - - if (n != r) { - n->prev = r->prev; - r->prev->next = n; - edge->target->edges = n; - } else { - edge->target->edges = NULL; - } - - edgePool.freeObject(edge); - edgePool.freeObject(r); - usedEdgePairs--; - } - - void computeInternal(int32_t start, int32_t end, IntermediateHull &result); - - bool mergeProjection(IntermediateHull &h0, IntermediateHull &h1, Vertex *&c0, Vertex *&c1); - - void merge(IntermediateHull &h0, IntermediateHull &h1); - - btVector3 toBtVector(const Point32 &v); - - btVector3 getBtNormal(Face *face); - - bool shiftFace(Face *face, btScalar amount, btAlignedObjectArray<Vertex *> stack); + class IntermediateHull { + public: + Vertex* minXy; + Vertex* maxXy; + Vertex* minYx; + Vertex* maxYx; + + IntermediateHull() + : minXy(NULL) + , maxXy(NULL) + , minYx(NULL) + , maxYx(NULL) + { + } + + void print(); + }; + + enum Orientation { NONE, + CLOCKWISE, + COUNTER_CLOCKWISE }; + + template <typename T> + class PoolArray { + private: + T* array; + int32_t size; + + public: + PoolArray<T>* next; + + PoolArray(int32_t size) + : size(size) + , next(NULL) + { + array = (T*)btAlignedAlloc(sizeof(T) * size, 16); + } + + ~PoolArray() + { + btAlignedFree(array); + } + + T* init() + { + T* o = array; + for (int32_t i = 0; i < size; i++, o++) { + o->next = (i + 1 < size) ? o + 1 : NULL; + } + return array; + } + }; + + template <typename T> + class Pool { + private: + PoolArray<T>* arrays; + PoolArray<T>* nextArray; + T* freeObjects; + int32_t arraySize; + + public: + Pool() + : arrays(NULL) + , nextArray(NULL) + , freeObjects(NULL) + , arraySize(256) + { + } + + ~Pool() + { + while (arrays) { + PoolArray<T>* p = arrays; + arrays = p->next; + p->~PoolArray<T>(); + btAlignedFree(p); + } + } + + void reset() + { + nextArray = arrays; + freeObjects = NULL; + } + + void setArraySize(int32_t arraySize) + { + this->arraySize = arraySize; + } + + T* newObject() + { + T* o = freeObjects; + if (!o) { + PoolArray<T>* p = nextArray; + if (p) { + nextArray = p->next; + } + else { + p = new (btAlignedAlloc(sizeof(PoolArray<T>), 16)) PoolArray<T>(arraySize); + p->next = arrays; + arrays = p; + } + o = p->init(); + } + freeObjects = o->next; + return new (o) T(); + }; + + void freeObject(T* object) + { + object->~T(); + object->next = freeObjects; + freeObjects = object; + } + }; + + btVector3 scaling; + btVector3 center; + Pool<Vertex> vertexPool; + Pool<Edge> edgePool; + Pool<Face> facePool; + btAlignedObjectArray<Vertex*> originalVertices; + int32_t mergeStamp; + int32_t minAxis; + int32_t medAxis; + int32_t maxAxis; + int32_t usedEdgePairs; + int32_t maxUsedEdgePairs; + + static Orientation getOrientation(const Edge* prev, const Edge* next, const Point32& s, const Point32& t); + Edge* findMaxAngle(bool ccw, const Vertex* start, const Point32& s, const Point64& rxs, const Point64& sxrxs, Rational64& minCot); + void findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge*& e0, Edge*& e1, Vertex* stop0, Vertex* stop1); + + Edge* newEdgePair(Vertex* from, Vertex* to); + + void removeEdgePair(Edge* edge) + { + Edge* n = edge->next; + Edge* r = edge->reverse; + + btAssert(edge->target && r->target); + + if (n != edge) { + n->prev = edge->prev; + edge->prev->next = n; + r->target->edges = n; + } + else { + r->target->edges = NULL; + } + + n = r->next; + + if (n != r) { + n->prev = r->prev; + r->prev->next = n; + edge->target->edges = n; + } + else { + edge->target->edges = NULL; + } + + edgePool.freeObject(edge); + edgePool.freeObject(r); + usedEdgePairs--; + } + + void computeInternal(int32_t start, int32_t end, IntermediateHull& result); + + bool mergeProjection(IntermediateHull& h0, IntermediateHull& h1, Vertex*& c0, Vertex*& c1); + + void merge(IntermediateHull& h0, IntermediateHull& h1); + + btVector3 toBtVector(const Point32& v); + + btVector3 getBtNormal(Face* face); + + bool shiftFace(Face* face, btScalar amount, btAlignedObjectArray<Vertex*> stack); public: - Vertex *vertexList; + Vertex* vertexList; - void compute(const void *coords, bool doubleCoords, int32_t stride, int32_t count); + void compute(const void* coords, bool doubleCoords, int32_t stride, int32_t count); - btVector3 getCoordinates(const Vertex *v); + btVector3 getCoordinates(const Vertex* v); - btScalar shrink(btScalar amount, btScalar clampAmount); + btScalar shrink(btScalar amount, btScalar clampAmount); }; -btConvexHullInternal::Int128 btConvexHullInternal::Int128::operator*(int64_t b) const { - bool negative = (int64_t)high < 0; - Int128 a = negative ? -*this : *this; - if (b < 0) { - negative = !negative; - b = -b; - } - Int128 result = mul(a.low, (uint64_t)b); - result.high += a.high * (uint64_t)b; - return negative ? -result : result; +btConvexHullInternal::Int128 btConvexHullInternal::Int128::operator*(int64_t b) const +{ + bool negative = (int64_t)high < 0; + Int128 a = negative ? -*this : *this; + if (b < 0) { + negative = !negative; + b = -b; + } + Int128 result = mul(a.low, (uint64_t)b); + result.high += a.high * (uint64_t)b; + return negative ? -result : result; } -btConvexHullInternal::Int128 btConvexHullInternal::Int128::mul(int64_t a, int64_t b) { - Int128 result; +btConvexHullInternal::Int128 btConvexHullInternal::Int128::mul(int64_t a, int64_t b) +{ + Int128 result; #ifdef USE_X86_64_ASM - __asm__("imulq %[b]" - : "=a"(result.low), "=d"(result.high) - : "0"(a), [b] "r"(b) - : "cc"); - return result; + __asm__("imulq %[b]" + : "=a"(result.low), "=d"(result.high) + : "0"(a), [b] "r"(b) + : "cc"); + return result; #else - bool negative = a < 0; - if (negative) { - a = -a; - } - if (b < 0) { - negative = !negative; - b = -b; - } - DMul<uint64_t, uint32_t>::mul((uint64_t)a, (uint64_t)b, result.low, result.high); - return negative ? -result : result; + bool negative = a < 0; + if (negative) { + a = -a; + } + if (b < 0) { + negative = !negative; + b = -b; + } + DMul<uint64_t, uint32_t>::mul((uint64_t)a, (uint64_t)b, result.low, result.high); + return negative ? -result : result; #endif } -btConvexHullInternal::Int128 btConvexHullInternal::Int128::mul(uint64_t a, uint64_t b) { - Int128 result; +btConvexHullInternal::Int128 btConvexHullInternal::Int128::mul(uint64_t a, uint64_t b) +{ + Int128 result; #ifdef USE_X86_64_ASM - __asm__("mulq %[b]" - : "=a"(result.low), "=d"(result.high) - : "0"(a), [b] "r"(b) - : "cc"); + __asm__("mulq %[b]" + : "=a"(result.low), "=d"(result.high) + : "0"(a), [b] "r"(b) + : "cc"); #else - DMul<uint64_t, uint32_t>::mul(a, b, result.low, result.high); + DMul<uint64_t, uint32_t>::mul(a, b, result.low, result.high); #endif - return result; + return result; } -int32_t btConvexHullInternal::Rational64::compare(const Rational64 &b) const { - if (sign != b.sign) { - return sign - b.sign; - } else if (sign == 0) { - return 0; - } +int32_t btConvexHullInternal::Rational64::compare(const Rational64& b) const +{ + if (sign != b.sign) { + return sign - b.sign; + } + else if (sign == 0) { + return 0; + } - // return (numerator * b.denominator > b.numerator * denominator) ? sign : (numerator * b.denominator < b.numerator * denominator) ? -sign : 0; +// return (numerator * b.denominator > b.numerator * denominator) ? sign : (numerator * b.denominator < b.numerator * denominator) ? -sign : 0; #ifdef USE_X86_64_ASM - int32_t result; - int64_t tmp; - int64_t dummy; - __asm__("mulq %[bn]\n\t" - "movq %%rax, %[tmp]\n\t" - "movq %%rdx, %%rbx\n\t" - "movq %[tn], %%rax\n\t" - "mulq %[bd]\n\t" - "subq %[tmp], %%rax\n\t" - "sbbq %%rbx, %%rdx\n\t" // rdx:rax contains 128-bit-difference "numerator*b.denominator - b.numerator*denominator" - "setnsb %%bh\n\t" // bh=1 if difference is non-negative, bh=0 otherwise - "orq %%rdx, %%rax\n\t" - "setnzb %%bl\n\t" // bl=1 if difference if non-zero, bl=0 if it is zero - "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) - "shll $16, %%ebx\n\t" // ebx has same sign as difference - : "=&b"(result), [tmp] "=&r"(tmp), "=a"(dummy) - : "a"(denominator), [bn] "g"(b.numerator), [tn] "g"(numerator), [bd] "g"(b.denominator) - : "%rdx", "cc"); - 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) - // if sign is -1, all bits of result are inverted, which changes the sign of result (and again cannot result in zero) - : - 0; + int32_t result; + int64_t tmp; + int64_t dummy; + __asm__("mulq %[bn]\n\t" + "movq %%rax, %[tmp]\n\t" + "movq %%rdx, %%rbx\n\t" + "movq %[tn], %%rax\n\t" + "mulq %[bd]\n\t" + "subq %[tmp], %%rax\n\t" + "sbbq %%rbx, %%rdx\n\t" // rdx:rax contains 128-bit-difference "numerator*b.denominator - b.numerator*denominator" + "setnsb %%bh\n\t" // bh=1 if difference is non-negative, bh=0 otherwise + "orq %%rdx, %%rax\n\t" + "setnzb %%bl\n\t" // bl=1 if difference if non-zero, bl=0 if it is zero + "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) + "shll $16, %%ebx\n\t" // ebx has same sign as difference + : "=&b"(result), [tmp] "=&r"(tmp), "=a"(dummy) + : "a"(denominator), [bn] "g"(b.numerator), [tn] "g"(numerator), [bd] "g"(b.denominator) + : "%rdx", "cc"); + 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) + // if sign is -1, all bits of result are inverted, which changes the sign of result (and again cannot result in zero) + : 0; #else - return sign * Int128::mul(m_numerator, b.m_denominator).ucmp(Int128::mul(m_denominator, b.m_numerator)); + return sign * Int128::mul(m_numerator, b.m_denominator).ucmp(Int128::mul(m_denominator, b.m_numerator)); #endif } -int32_t btConvexHullInternal::Rational128::compare(const Rational128 &b) const { - if (sign != b.sign) { - return sign - b.sign; - } else if (sign == 0) { - return 0; - } - if (isInt64) { - return -b.compare(sign * (int64_t)numerator.low); - } - - Int128 nbdLow, nbdHigh, dbnLow, dbnHigh; - DMul<Int128, uint64_t>::mul(numerator, b.denominator, nbdLow, nbdHigh); - DMul<Int128, uint64_t>::mul(denominator, b.numerator, dbnLow, dbnHigh); - - int32_t cmp = nbdHigh.ucmp(dbnHigh); - if (cmp) { - return cmp * sign; - } - return nbdLow.ucmp(dbnLow) * sign; +int32_t btConvexHullInternal::Rational128::compare(const Rational128& b) const +{ + if (sign != b.sign) { + return sign - b.sign; + } + else if (sign == 0) { + return 0; + } + if (isInt64) { + return -b.compare(sign * (int64_t)numerator.low); + } + + Int128 nbdLow, nbdHigh, dbnLow, dbnHigh; + DMul<Int128, uint64_t>::mul(numerator, b.denominator, nbdLow, nbdHigh); + DMul<Int128, uint64_t>::mul(denominator, b.numerator, dbnLow, dbnHigh); + + int32_t cmp = nbdHigh.ucmp(dbnHigh); + if (cmp) { + return cmp * sign; + } + return nbdLow.ucmp(dbnLow) * sign; } -int32_t btConvexHullInternal::Rational128::compare(int64_t b) const { - if (isInt64) { - int64_t a = sign * (int64_t)numerator.low; - return (a > b) ? 1 : (a < b) ? -1 : 0; - } - if (b > 0) { - if (sign <= 0) { - return -1; - } - } else if (b < 0) { - if (sign >= 0) { - return 1; - } - b = -b; - } else { - return sign; - } - - return numerator.ucmp(denominator * b) * sign; +int32_t btConvexHullInternal::Rational128::compare(int64_t b) const +{ + if (isInt64) { + int64_t a = sign * (int64_t)numerator.low; + return (a > b) ? 1 : (a < b) ? -1 : 0; + } + if (b > 0) { + if (sign <= 0) { + return -1; + } + } + else if (b < 0) { + if (sign >= 0) { + return 1; + } + b = -b; + } + else { + return sign; + } + + return numerator.ucmp(denominator * b) * sign; } -btConvexHullInternal::Edge *btConvexHullInternal::newEdgePair(Vertex *from, Vertex *to) { - btAssert(from && to); - Edge *e = edgePool.newObject(); - Edge *r = edgePool.newObject(); - e->reverse = r; - r->reverse = e; - e->copy = mergeStamp; - r->copy = mergeStamp; - e->target = to; - r->target = from; - e->face = NULL; - r->face = NULL; - usedEdgePairs++; - if (usedEdgePairs > maxUsedEdgePairs) { - maxUsedEdgePairs = usedEdgePairs; - } - return e; +btConvexHullInternal::Edge* btConvexHullInternal::newEdgePair(Vertex* from, Vertex* to) +{ + btAssert(from && to); + Edge* e = edgePool.newObject(); + Edge* r = edgePool.newObject(); + e->reverse = r; + r->reverse = e; + e->copy = mergeStamp; + r->copy = mergeStamp; + e->target = to; + r->target = from; + e->face = NULL; + r->face = NULL; + usedEdgePairs++; + if (usedEdgePairs > maxUsedEdgePairs) { + maxUsedEdgePairs = usedEdgePairs; + } + return e; } -bool btConvexHullInternal::mergeProjection(IntermediateHull &h0, IntermediateHull &h1, Vertex *&c0, Vertex *&c1) { - Vertex *v0 = h0.maxYx; - Vertex *v1 = h1.minYx; - if ((v0->point.x == v1->point.x) && (v0->point.y == v1->point.y)) { - btAssert(v0->point.z < v1->point.z); - Vertex *v1p = v1->prev; - if (v1p == v1) { - c0 = v0; - if (v1->edges) { - btAssert(v1->edges->next == v1->edges); - v1 = v1->edges->target; - btAssert(v1->edges->next == v1->edges); - } - c1 = v1; - return false; - } - Vertex *v1n = v1->next; - v1p->next = v1n; - v1n->prev = v1p; - if (v1 == h1.minXy) { - if ((v1n->point.x < v1p->point.x) || ((v1n->point.x == v1p->point.x) && (v1n->point.y < v1p->point.y))) { - h1.minXy = v1n; - } else { - h1.minXy = v1p; - } - } - if (v1 == h1.maxXy) { - if ((v1n->point.x > v1p->point.x) || ((v1n->point.x == v1p->point.x) && (v1n->point.y > v1p->point.y))) { - h1.maxXy = v1n; - } else { - h1.maxXy = v1p; - } - } - } - - v0 = h0.maxXy; - v1 = h1.maxXy; - Vertex *v00 = NULL; - Vertex *v10 = NULL; - int32_t sign = 1; - - for (int32_t side = 0; side <= 1; side++) { - int32_t dx = (v1->point.x - v0->point.x) * sign; - if (dx > 0) { - while (true) { - int32_t dy = v1->point.y - v0->point.y; - - Vertex *w0 = side ? v0->next : v0->prev; - if (w0 != v0) { - int32_t dx0 = (w0->point.x - v0->point.x) * sign; - int32_t dy0 = w0->point.y - v0->point.y; - if ((dy0 <= 0) && ((dx0 == 0) || ((dx0 < 0) && (dy0 * dx <= dy * dx0)))) { - v0 = w0; - dx = (v1->point.x - v0->point.x) * sign; - continue; - } - } - - Vertex *w1 = side ? v1->next : v1->prev; - if (w1 != v1) { - int32_t dx1 = (w1->point.x - v1->point.x) * sign; - int32_t dy1 = w1->point.y - v1->point.y; - int32_t dxn = (w1->point.x - v0->point.x) * sign; - if ((dxn > 0) && (dy1 < 0) && ((dx1 == 0) || ((dx1 < 0) && (dy1 * dx < dy * dx1)))) { - v1 = w1; - dx = dxn; - continue; - } - } - - break; - } - } else if (dx < 0) { - while (true) { - int32_t dy = v1->point.y - v0->point.y; - - Vertex *w1 = side ? v1->prev : v1->next; - if (w1 != v1) { - int32_t dx1 = (w1->point.x - v1->point.x) * sign; - int32_t dy1 = w1->point.y - v1->point.y; - if ((dy1 >= 0) && ((dx1 == 0) || ((dx1 < 0) && (dy1 * dx <= dy * dx1)))) { - v1 = w1; - dx = (v1->point.x - v0->point.x) * sign; - continue; - } - } - - Vertex *w0 = side ? v0->prev : v0->next; - if (w0 != v0) { - int32_t dx0 = (w0->point.x - v0->point.x) * sign; - int32_t dy0 = w0->point.y - v0->point.y; - int32_t dxn = (v1->point.x - w0->point.x) * sign; - if ((dxn < 0) && (dy0 > 0) && ((dx0 == 0) || ((dx0 < 0) && (dy0 * dx < dy * dx0)))) { - v0 = w0; - dx = dxn; - continue; - } - } - - break; - } - } else { - int32_t x = v0->point.x; - int32_t y0 = v0->point.y; - Vertex *w0 = v0; - Vertex *t; - while (((t = side ? w0->next : w0->prev) != v0) && (t->point.x == x) && (t->point.y <= y0)) { - w0 = t; - y0 = t->point.y; - } - v0 = w0; - - int32_t y1 = v1->point.y; - Vertex *w1 = v1; - while (((t = side ? w1->prev : w1->next) != v1) && (t->point.x == x) && (t->point.y >= y1)) { - w1 = t; - y1 = t->point.y; - } - v1 = w1; - } - - if (side == 0) { - v00 = v0; - v10 = v1; - - v0 = h0.minXy; - v1 = h1.minXy; - sign = -1; - } - } - - v0->prev = v1; - v1->next = v0; - - v00->next = v10; - v10->prev = v00; - - if (h1.minXy->point.x < h0.minXy->point.x) { - h0.minXy = h1.minXy; - } - if (h1.maxXy->point.x >= h0.maxXy->point.x) { - h0.maxXy = h1.maxXy; - } - - h0.maxYx = h1.maxYx; - - c0 = v00; - c1 = v10; - - return true; +bool btConvexHullInternal::mergeProjection(IntermediateHull& h0, IntermediateHull& h1, Vertex*& c0, Vertex*& c1) +{ + Vertex* v0 = h0.maxYx; + Vertex* v1 = h1.minYx; + if ((v0->point.x == v1->point.x) && (v0->point.y == v1->point.y)) { + btAssert(v0->point.z < v1->point.z); + Vertex* v1p = v1->prev; + if (v1p == v1) { + c0 = v0; + if (v1->edges) { + btAssert(v1->edges->next == v1->edges); + v1 = v1->edges->target; + btAssert(v1->edges->next == v1->edges); + } + c1 = v1; + return false; + } + Vertex* v1n = v1->next; + v1p->next = v1n; + v1n->prev = v1p; + if (v1 == h1.minXy) { + if ((v1n->point.x < v1p->point.x) || ((v1n->point.x == v1p->point.x) && (v1n->point.y < v1p->point.y))) { + h1.minXy = v1n; + } + else { + h1.minXy = v1p; + } + } + if (v1 == h1.maxXy) { + if ((v1n->point.x > v1p->point.x) || ((v1n->point.x == v1p->point.x) && (v1n->point.y > v1p->point.y))) { + h1.maxXy = v1n; + } + else { + h1.maxXy = v1p; + } + } + } + + v0 = h0.maxXy; + v1 = h1.maxXy; + Vertex* v00 = NULL; + Vertex* v10 = NULL; + int32_t sign = 1; + + for (int32_t side = 0; side <= 1; side++) { + int32_t dx = (v1->point.x - v0->point.x) * sign; + if (dx > 0) { + while (true) { + int32_t dy = v1->point.y - v0->point.y; + + Vertex* w0 = side ? v0->next : v0->prev; + if (w0 != v0) { + int32_t dx0 = (w0->point.x - v0->point.x) * sign; + int32_t dy0 = w0->point.y - v0->point.y; + if ((dy0 <= 0) && ((dx0 == 0) || ((dx0 < 0) && (dy0 * dx <= dy * dx0)))) { + v0 = w0; + dx = (v1->point.x - v0->point.x) * sign; + continue; + } + } + + Vertex* w1 = side ? v1->next : v1->prev; + if (w1 != v1) { + int32_t dx1 = (w1->point.x - v1->point.x) * sign; + int32_t dy1 = w1->point.y - v1->point.y; + int32_t dxn = (w1->point.x - v0->point.x) * sign; + if ((dxn > 0) && (dy1 < 0) && ((dx1 == 0) || ((dx1 < 0) && (dy1 * dx < dy * dx1)))) { + v1 = w1; + dx = dxn; + continue; + } + } + + break; + } + } + else if (dx < 0) { + while (true) { + int32_t dy = v1->point.y - v0->point.y; + + Vertex* w1 = side ? v1->prev : v1->next; + if (w1 != v1) { + int32_t dx1 = (w1->point.x - v1->point.x) * sign; + int32_t dy1 = w1->point.y - v1->point.y; + if ((dy1 >= 0) && ((dx1 == 0) || ((dx1 < 0) && (dy1 * dx <= dy * dx1)))) { + v1 = w1; + dx = (v1->point.x - v0->point.x) * sign; + continue; + } + } + + Vertex* w0 = side ? v0->prev : v0->next; + if (w0 != v0) { + int32_t dx0 = (w0->point.x - v0->point.x) * sign; + int32_t dy0 = w0->point.y - v0->point.y; + int32_t dxn = (v1->point.x - w0->point.x) * sign; + if ((dxn < 0) && (dy0 > 0) && ((dx0 == 0) || ((dx0 < 0) && (dy0 * dx < dy * dx0)))) { + v0 = w0; + dx = dxn; + continue; + } + } + + break; + } + } + else { + int32_t x = v0->point.x; + int32_t y0 = v0->point.y; + Vertex* w0 = v0; + Vertex* t; + while (((t = side ? w0->next : w0->prev) != v0) && (t->point.x == x) && (t->point.y <= y0)) { + w0 = t; + y0 = t->point.y; + } + v0 = w0; + + int32_t y1 = v1->point.y; + Vertex* w1 = v1; + while (((t = side ? w1->prev : w1->next) != v1) && (t->point.x == x) && (t->point.y >= y1)) { + w1 = t; + y1 = t->point.y; + } + v1 = w1; + } + + if (side == 0) { + v00 = v0; + v10 = v1; + + v0 = h0.minXy; + v1 = h1.minXy; + sign = -1; + } + } + + v0->prev = v1; + v1->next = v0; + + v00->next = v10; + v10->prev = v00; + + if (h1.minXy->point.x < h0.minXy->point.x) { + h0.minXy = h1.minXy; + } + if (h1.maxXy->point.x >= h0.maxXy->point.x) { + h0.maxXy = h1.maxXy; + } + + h0.maxYx = h1.maxYx; + + c0 = v00; + c1 = v10; + + return true; } -void btConvexHullInternal::computeInternal(int32_t start, int32_t end, IntermediateHull &result) { - int32_t n = end - start; - switch (n) { - case 0: - result.minXy = NULL; - result.maxXy = NULL; - result.minYx = NULL; - result.maxYx = NULL; - return; - case 2: { - Vertex *v = originalVertices[start]; - Vertex *w = v + 1; - if (v->point != w->point) { - int32_t dx = v->point.x - w->point.x; - int32_t dy = v->point.y - w->point.y; - - if ((dx == 0) && (dy == 0)) { - if (v->point.z > w->point.z) { - Vertex *t = w; - w = v; - v = t; - } - btAssert(v->point.z < w->point.z); - v->next = v; - v->prev = v; - result.minXy = v; - result.maxXy = v; - result.minYx = v; - result.maxYx = v; - } else { - v->next = w; - v->prev = w; - w->next = v; - w->prev = v; - - if ((dx < 0) || ((dx == 0) && (dy < 0))) { - result.minXy = v; - result.maxXy = w; - } else { - result.minXy = w; - result.maxXy = v; - } - - if ((dy < 0) || ((dy == 0) && (dx < 0))) { - result.minYx = v; - result.maxYx = w; - } else { - result.minYx = w; - result.maxYx = v; - } - } - - Edge *e = newEdgePair(v, w); - e->link(e); - v->edges = e; - - e = e->reverse; - e->link(e); - w->edges = e; - - return; - } - } - // lint -fallthrough - case 1: { - Vertex *v = originalVertices[start]; - v->edges = NULL; - v->next = v; - v->prev = v; - - result.minXy = v; - result.maxXy = v; - result.minYx = v; - result.maxYx = v; - - return; - } - } - - int32_t split0 = start + n / 2; - Point32 p = originalVertices[split0 - 1]->point; - int32_t split1 = split0; - while ((split1 < end) && (originalVertices[split1]->point == p)) { - split1++; - } - computeInternal(start, split0, result); - IntermediateHull hull1; - computeInternal(split1, end, hull1); +void btConvexHullInternal::computeInternal(int32_t start, int32_t end, IntermediateHull& result) +{ + int32_t n = end - start; + switch (n) { + case 0: + result.minXy = NULL; + result.maxXy = NULL; + result.minYx = NULL; + result.maxYx = NULL; + return; + case 2: { + Vertex* v = originalVertices[start]; + Vertex* w = v + 1; + if (v->point != w->point) { + int32_t dx = v->point.x - w->point.x; + int32_t dy = v->point.y - w->point.y; + + if ((dx == 0) && (dy == 0)) { + if (v->point.z > w->point.z) { + Vertex* t = w; + w = v; + v = t; + } + btAssert(v->point.z < w->point.z); + v->next = v; + v->prev = v; + result.minXy = v; + result.maxXy = v; + result.minYx = v; + result.maxYx = v; + } + else { + v->next = w; + v->prev = w; + w->next = v; + w->prev = v; + + if ((dx < 0) || ((dx == 0) && (dy < 0))) { + result.minXy = v; + result.maxXy = w; + } + else { + result.minXy = w; + result.maxXy = v; + } + + if ((dy < 0) || ((dy == 0) && (dx < 0))) { + result.minYx = v; + result.maxYx = w; + } + else { + result.minYx = w; + result.maxYx = v; + } + } + + Edge* e = newEdgePair(v, w); + e->link(e); + v->edges = e; + + e = e->reverse; + e->link(e); + w->edges = e; + + return; + } + } + // lint -fallthrough + case 1: { + Vertex* v = originalVertices[start]; + v->edges = NULL; + v->next = v; + v->prev = v; + + result.minXy = v; + result.maxXy = v; + result.minYx = v; + result.maxYx = v; + + return; + } + } + + int32_t split0 = start + n / 2; + Point32 p = originalVertices[split0 - 1]->point; + int32_t split1 = split0; + while ((split1 < end) && (originalVertices[split1]->point == p)) { + split1++; + } + computeInternal(start, split0, result); + IntermediateHull hull1; + computeInternal(split1, end, hull1); #ifdef DEBUG_CONVEX_HULL - printf("\n\nMerge\n"); - result.print(); - hull1.print(); + printf("\n\nMerge\n"); + result.print(); + hull1.print(); #endif - merge(result, hull1); + merge(result, hull1); #ifdef DEBUG_CONVEX_HULL - printf("\n Result\n"); - result.print(); + printf("\n Result\n"); + result.print(); #endif } #ifdef DEBUG_CONVEX_HULL -void btConvexHullInternal::IntermediateHull::print() { - printf(" Hull\n"); - for (Vertex *v = minXy; v;) { - printf(" "); - v->print(); - if (v == maxXy) { - printf(" maxXy"); - } - if (v == minYx) { - printf(" minYx"); - } - if (v == maxYx) { - printf(" maxYx"); - } - if (v->next->prev != v) { - printf(" Inconsistency"); - } - printf("\n"); - v = v->next; - if (v == minXy) { - break; - } - } - if (minXy) { - minXy->copy = (minXy->copy == -1) ? -2 : -1; - minXy->printGraph(); - } +void btConvexHullInternal::IntermediateHull::print() +{ + printf(" Hull\n"); + for (Vertex* v = minXy; v;) { + printf(" "); + v->print(); + if (v == maxXy) { + printf(" maxXy"); + } + if (v == minYx) { + printf(" minYx"); + } + if (v == maxYx) { + printf(" maxYx"); + } + if (v->next->prev != v) { + printf(" Inconsistency"); + } + printf("\n"); + v = v->next; + if (v == minXy) { + break; + } + } + if (minXy) { + minXy->copy = (minXy->copy == -1) ? -2 : -1; + minXy->printGraph(); + } } -void btConvexHullInternal::Vertex::printGraph() { - print(); - printf("\nEdges\n"); - Edge *e = edges; - if (e) { - do { - e->print(); - printf("\n"); - e = e->next; - } while (e != edges); - do { - Vertex *v = e->target; - if (v->copy != copy) { - v->copy = copy; - v->printGraph(); - } - e = e->next; - } while (e != edges); - } +void btConvexHullInternal::Vertex::printGraph() +{ + print(); + printf("\nEdges\n"); + Edge* e = edges; + if (e) { + do { + e->print(); + printf("\n"); + e = e->next; + } while (e != edges); + do { + Vertex* v = e->target; + if (v->copy != copy) { + v->copy = copy; + v->printGraph(); + } + e = e->next; + } while (e != edges); + } } #endif -btConvexHullInternal::Orientation btConvexHullInternal::getOrientation(const Edge *prev, const Edge *next, const Point32 &s, const Point32 &t) { - btAssert(prev->reverse->target == next->reverse->target); - if (prev->next == next) { - if (prev->prev == next) { - Point64 n = t.cross(s); - Point64 m = (*prev->target - *next->reverse->target).cross(*next->target - *next->reverse->target); - btAssert(!m.isZero()); - int64_t dot = n.dot(m); - btAssert(dot != 0); - return (dot > 0) ? COUNTER_CLOCKWISE : CLOCKWISE; - } - return COUNTER_CLOCKWISE; - } else if (prev->prev == next) { - return CLOCKWISE; - } else { - return NONE; - } +btConvexHullInternal::Orientation btConvexHullInternal::getOrientation(const Edge* prev, const Edge* next, const Point32& s, const Point32& t) +{ + btAssert(prev->reverse->target == next->reverse->target); + if (prev->next == next) { + if (prev->prev == next) { + Point64 n = t.cross(s); + Point64 m = (*prev->target - *next->reverse->target).cross(*next->target - *next->reverse->target); + btAssert(!m.isZero()); + int64_t dot = n.dot(m); + btAssert(dot != 0); + return (dot > 0) ? COUNTER_CLOCKWISE : CLOCKWISE; + } + return COUNTER_CLOCKWISE; + } + else if (prev->prev == next) { + return CLOCKWISE; + } + else { + return NONE; + } } -btConvexHullInternal::Edge *btConvexHullInternal::findMaxAngle(bool ccw, const Vertex *start, const Point32 &s, const Point64 &rxs, const Point64 &sxrxs, Rational64 &minCot) { - Edge *minEdge = NULL; +btConvexHullInternal::Edge* btConvexHullInternal::findMaxAngle(bool ccw, const Vertex* start, const Point32& s, const Point64& rxs, const Point64& sxrxs, Rational64& minCot) +{ + Edge* minEdge = NULL; #ifdef DEBUG_CONVEX_HULL - printf("find max edge for %d\n", start->point.index); + printf("find max edge for %d\n", start->point.index); #endif - Edge *e = start->edges; - if (e) { - do { - if (e->copy > mergeStamp) { - Point32 t = *e->target - *start; - Rational64 cot(t.dot(sxrxs), t.dot(rxs)); + Edge* e = start->edges; + if (e) { + do { + if (e->copy > mergeStamp) { + Point32 t = *e->target - *start; + Rational64 cot(t.dot(sxrxs), t.dot(rxs)); #ifdef DEBUG_CONVEX_HULL - printf(" Angle is %f (%d) for ", (float)btAtan(cot.toScalar()), (int32_t)cot.isNaN()); - e->print(); + printf(" Angle is %f (%d) for ", (float)btAtan(cot.toScalar()), (int32_t)cot.isNaN()); + e->print(); #endif - if (cot.isNaN()) { - btAssert(ccw ? (t.dot(s) < 0) : (t.dot(s) > 0)); - } else { - int32_t cmp; - if (minEdge == NULL) { - minCot = cot; - minEdge = e; - } else if ((cmp = cot.compare(minCot)) < 0) { - minCot = cot; - minEdge = e; - } else if ((cmp == 0) && (ccw == (getOrientation(minEdge, e, s, t) == COUNTER_CLOCKWISE))) { - minEdge = e; - } - } + if (cot.isNaN()) { + btAssert(ccw ? (t.dot(s) < 0) : (t.dot(s) > 0)); + } + else { + int32_t cmp; + if (minEdge == NULL) { + minCot = cot; + minEdge = e; + } + else if ((cmp = cot.compare(minCot)) < 0) { + minCot = cot; + minEdge = e; + } + else if ((cmp == 0) && (ccw == (getOrientation(minEdge, e, s, t) == COUNTER_CLOCKWISE))) { + minEdge = e; + } + } #ifdef DEBUG_CONVEX_HULL - printf("\n"); + printf("\n"); #endif - } - e = e->next; - } while (e != start->edges); - } - return minEdge; + } + e = e->next; + } while (e != start->edges); + } + return minEdge; } -void btConvexHullInternal::findEdgeForCoplanarFaces(Vertex *c0, Vertex *c1, Edge *&e0, Edge *&e1, Vertex *stop0, Vertex *stop1) { - Edge *start0 = e0; - Edge *start1 = e1; - Point32 et0 = start0 ? start0->target->point : c0->point; - Point32 et1 = start1 ? start1->target->point : c1->point; - Point32 s = c1->point - c0->point; - Point64 normal = ((start0 ? start0 : start1)->target->point - c0->point).cross(s); - int64_t dist = c0->point.dot(normal); - btAssert(!start1 || (start1->target->point.dot(normal) == dist)); - Point64 perp = s.cross(normal); - btAssert(!perp.isZero()); +void btConvexHullInternal::findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge*& e0, Edge*& e1, Vertex* stop0, Vertex* stop1) +{ + Edge* start0 = e0; + Edge* start1 = e1; + Point32 et0 = start0 ? start0->target->point : c0->point; + Point32 et1 = start1 ? start1->target->point : c1->point; + Point32 s = c1->point - c0->point; + Point64 normal = ((start0 ? start0 : start1)->target->point - c0->point).cross(s); + int64_t dist = c0->point.dot(normal); + btAssert(!start1 || (start1->target->point.dot(normal) == dist)); + Point64 perp = s.cross(normal); + btAssert(!perp.isZero()); #ifdef DEBUG_CONVEX_HULL - 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); + 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); #endif - int64_t maxDot0 = et0.dot(perp); - if (e0) { - while (e0->target != stop0) { - Edge *e = e0->reverse->prev; - if (e->target->point.dot(normal) < dist) { - break; - } - btAssert(e->target->point.dot(normal) == dist); - if (e->copy == mergeStamp) { - break; - } - int64_t dot = e->target->point.dot(perp); - if (dot <= maxDot0) { - break; - } - maxDot0 = dot; - e0 = e; - et0 = e->target->point; - } - } - - int64_t maxDot1 = et1.dot(perp); - if (e1) { - while (e1->target != stop1) { - Edge *e = e1->reverse->next; - if (e->target->point.dot(normal) < dist) { - break; - } - btAssert(e->target->point.dot(normal) == dist); - if (e->copy == mergeStamp) { - break; - } - int64_t dot = e->target->point.dot(perp); - if (dot <= maxDot1) { - break; - } - maxDot1 = dot; - e1 = e; - et1 = e->target->point; - } - } + int64_t maxDot0 = et0.dot(perp); + if (e0) { + while (e0->target != stop0) { + Edge* e = e0->reverse->prev; + if (e->target->point.dot(normal) < dist) { + break; + } + btAssert(e->target->point.dot(normal) == dist); + if (e->copy == mergeStamp) { + break; + } + int64_t dot = e->target->point.dot(perp); + if (dot <= maxDot0) { + break; + } + maxDot0 = dot; + e0 = e; + et0 = e->target->point; + } + } + + int64_t maxDot1 = et1.dot(perp); + if (e1) { + while (e1->target != stop1) { + Edge* e = e1->reverse->next; + if (e->target->point.dot(normal) < dist) { + break; + } + btAssert(e->target->point.dot(normal) == dist); + if (e->copy == mergeStamp) { + break; + } + int64_t dot = e->target->point.dot(perp); + if (dot <= maxDot1) { + break; + } + maxDot1 = dot; + e1 = e; + et1 = e->target->point; + } + } #ifdef DEBUG_CONVEX_HULL - printf(" Starting at %d %d\n", et0.index, et1.index); + printf(" Starting at %d %d\n", et0.index, et1.index); #endif - int64_t dx = maxDot1 - maxDot0; - if (dx > 0) { - while (true) { - int64_t dy = (et1 - et0).dot(s); - - if (e0 && (e0->target != stop0)) { - Edge *f0 = e0->next->reverse; - if (f0->copy > mergeStamp) { - int64_t dx0 = (f0->target->point - et0).dot(perp); - int64_t dy0 = (f0->target->point - et0).dot(s); - if ((dx0 == 0) ? (dy0 < 0) : ((dx0 < 0) && (Rational64(dy0, dx0).compare(Rational64(dy, dx)) >= 0))) { - et0 = f0->target->point; - dx = (et1 - et0).dot(perp); - e0 = (e0 == start0) ? NULL : f0; - continue; - } - } - } - - if (e1 && (e1->target != stop1)) { - Edge *f1 = e1->reverse->next; - if (f1->copy > mergeStamp) { - Point32 d1 = f1->target->point - et1; - if (d1.dot(normal) == 0) { - int64_t dx1 = d1.dot(perp); - int64_t dy1 = d1.dot(s); - int64_t dxn = (f1->target->point - et0).dot(perp); - if ((dxn > 0) && ((dx1 == 0) ? (dy1 < 0) : ((dx1 < 0) && (Rational64(dy1, dx1).compare(Rational64(dy, dx)) > 0)))) { - e1 = f1; - et1 = e1->target->point; - dx = dxn; - continue; - } - } else { - btAssert((e1 == start1) && (d1.dot(normal) < 0)); - } - } - } - - break; - } - } else if (dx < 0) { - while (true) { - int64_t dy = (et1 - et0).dot(s); - - if (e1 && (e1->target != stop1)) { - Edge *f1 = e1->prev->reverse; - if (f1->copy > mergeStamp) { - int64_t dx1 = (f1->target->point - et1).dot(perp); - int64_t dy1 = (f1->target->point - et1).dot(s); - if ((dx1 == 0) ? (dy1 > 0) : ((dx1 < 0) && (Rational64(dy1, dx1).compare(Rational64(dy, dx)) <= 0))) { - et1 = f1->target->point; - dx = (et1 - et0).dot(perp); - e1 = (e1 == start1) ? NULL : f1; - continue; - } - } - } - - if (e0 && (e0->target != stop0)) { - Edge *f0 = e0->reverse->prev; - if (f0->copy > mergeStamp) { - Point32 d0 = f0->target->point - et0; - if (d0.dot(normal) == 0) { - int64_t dx0 = d0.dot(perp); - int64_t dy0 = d0.dot(s); - int64_t dxn = (et1 - f0->target->point).dot(perp); - if ((dxn < 0) && ((dx0 == 0) ? (dy0 > 0) : ((dx0 < 0) && (Rational64(dy0, dx0).compare(Rational64(dy, dx)) < 0)))) { - e0 = f0; - et0 = e0->target->point; - dx = dxn; - continue; - } - } else { - btAssert((e0 == start0) && (d0.dot(normal) < 0)); - } - } - } - - break; - } - } + int64_t dx = maxDot1 - maxDot0; + if (dx > 0) { + while (true) { + int64_t dy = (et1 - et0).dot(s); + + if (e0 && (e0->target != stop0)) { + Edge* f0 = e0->next->reverse; + if (f0->copy > mergeStamp) { + int64_t dx0 = (f0->target->point - et0).dot(perp); + int64_t dy0 = (f0->target->point - et0).dot(s); + if ((dx0 == 0) ? (dy0 < 0) : ((dx0 < 0) && (Rational64(dy0, dx0).compare(Rational64(dy, dx)) >= 0))) { + et0 = f0->target->point; + dx = (et1 - et0).dot(perp); + e0 = (e0 == start0) ? NULL : f0; + continue; + } + } + } + + if (e1 && (e1->target != stop1)) { + Edge* f1 = e1->reverse->next; + if (f1->copy > mergeStamp) { + Point32 d1 = f1->target->point - et1; + if (d1.dot(normal) == 0) { + int64_t dx1 = d1.dot(perp); + int64_t dy1 = d1.dot(s); + int64_t dxn = (f1->target->point - et0).dot(perp); + if ((dxn > 0) && ((dx1 == 0) ? (dy1 < 0) : ((dx1 < 0) && (Rational64(dy1, dx1).compare(Rational64(dy, dx)) > 0)))) { + e1 = f1; + et1 = e1->target->point; + dx = dxn; + continue; + } + } + else { + btAssert((e1 == start1) && (d1.dot(normal) < 0)); + } + } + } + + break; + } + } + else if (dx < 0) { + while (true) { + int64_t dy = (et1 - et0).dot(s); + + if (e1 && (e1->target != stop1)) { + Edge* f1 = e1->prev->reverse; + if (f1->copy > mergeStamp) { + int64_t dx1 = (f1->target->point - et1).dot(perp); + int64_t dy1 = (f1->target->point - et1).dot(s); + if ((dx1 == 0) ? (dy1 > 0) : ((dx1 < 0) && (Rational64(dy1, dx1).compare(Rational64(dy, dx)) <= 0))) { + et1 = f1->target->point; + dx = (et1 - et0).dot(perp); + e1 = (e1 == start1) ? NULL : f1; + continue; + } + } + } + + if (e0 && (e0->target != stop0)) { + Edge* f0 = e0->reverse->prev; + if (f0->copy > mergeStamp) { + Point32 d0 = f0->target->point - et0; + if (d0.dot(normal) == 0) { + int64_t dx0 = d0.dot(perp); + int64_t dy0 = d0.dot(s); + int64_t dxn = (et1 - f0->target->point).dot(perp); + if ((dxn < 0) && ((dx0 == 0) ? (dy0 > 0) : ((dx0 < 0) && (Rational64(dy0, dx0).compare(Rational64(dy, dx)) < 0)))) { + e0 = f0; + et0 = e0->target->point; + dx = dxn; + continue; + } + } + else { + btAssert((e0 == start0) && (d0.dot(normal) < 0)); + } + } + } + + break; + } + } #ifdef DEBUG_CONVEX_HULL - printf(" Advanced edges to %d %d\n", et0.index, et1.index); + printf(" Advanced edges to %d %d\n", et0.index, et1.index); #endif } -void btConvexHullInternal::merge(IntermediateHull &h0, IntermediateHull &h1) { - if (!h1.maxXy) { - return; - } - if (!h0.maxXy) { - h0 = h1; - return; - } - - mergeStamp--; - - Vertex *c0 = NULL; - Edge *toPrev0 = NULL; - Edge *firstNew0 = NULL; - Edge *pendingHead0 = NULL; - Edge *pendingTail0 = NULL; - Vertex *c1 = NULL; - Edge *toPrev1 = NULL; - Edge *firstNew1 = NULL; - Edge *pendingHead1 = NULL; - Edge *pendingTail1 = NULL; - Point32 prevPoint; - - if (mergeProjection(h0, h1, c0, c1)) { - Point32 s = *c1 - *c0; - Point64 normal = Point32(0, 0, -1).cross(s); - Point64 t = s.cross(normal); - btAssert(!t.isZero()); - - Edge *e = c0->edges; - Edge *start0 = NULL; - if (e) { - do { - int64_t dot = (*e->target - *c0).dot(normal); - btAssert(dot <= 0); - if ((dot == 0) && ((*e->target - *c0).dot(t) > 0)) { - if (!start0 || (getOrientation(start0, e, s, Point32(0, 0, -1)) == CLOCKWISE)) { - start0 = e; - } - } - e = e->next; - } while (e != c0->edges); - } - - e = c1->edges; - Edge *start1 = NULL; - if (e) { - do { - int64_t dot = (*e->target - *c1).dot(normal); - btAssert(dot <= 0); - if ((dot == 0) && ((*e->target - *c1).dot(t) > 0)) { - if (!start1 || (getOrientation(start1, e, s, Point32(0, 0, -1)) == COUNTER_CLOCKWISE)) { - start1 = e; - } - } - e = e->next; - } while (e != c1->edges); - } - - if (start0 || start1) { - findEdgeForCoplanarFaces(c0, c1, start0, start1, NULL, NULL); - if (start0) { - c0 = start0->target; - } - if (start1) { - c1 = start1->target; - } - } - - prevPoint = c1->point; - prevPoint.z++; - } else { - prevPoint = c1->point; - prevPoint.x++; - } - - Vertex *first0 = c0; - Vertex *first1 = c1; - bool firstRun = true; - - while (true) { - Point32 s = *c1 - *c0; - Point32 r = prevPoint - c0->point; - Point64 rxs = r.cross(s); - Point64 sxrxs = s.cross(rxs); +void btConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1) +{ + if (!h1.maxXy) { + return; + } + if (!h0.maxXy) { + h0 = h1; + return; + } + + mergeStamp--; + + Vertex* c0 = NULL; + Edge* toPrev0 = NULL; + Edge* firstNew0 = NULL; + Edge* pendingHead0 = NULL; + Edge* pendingTail0 = NULL; + Vertex* c1 = NULL; + Edge* toPrev1 = NULL; + Edge* firstNew1 = NULL; + Edge* pendingHead1 = NULL; + Edge* pendingTail1 = NULL; + Point32 prevPoint; + + if (mergeProjection(h0, h1, c0, c1)) { + Point32 s = *c1 - *c0; + Point64 normal = Point32(0, 0, -1).cross(s); + Point64 t = s.cross(normal); + btAssert(!t.isZero()); + + Edge* e = c0->edges; + Edge* start0 = NULL; + if (e) { + do { + int64_t dot = (*e->target - *c0).dot(normal); + btAssert(dot <= 0); + if ((dot == 0) && ((*e->target - *c0).dot(t) > 0)) { + if (!start0 || (getOrientation(start0, e, s, Point32(0, 0, -1)) == CLOCKWISE)) { + start0 = e; + } + } + e = e->next; + } while (e != c0->edges); + } + + e = c1->edges; + Edge* start1 = NULL; + if (e) { + do { + int64_t dot = (*e->target - *c1).dot(normal); + btAssert(dot <= 0); + if ((dot == 0) && ((*e->target - *c1).dot(t) > 0)) { + if (!start1 || (getOrientation(start1, e, s, Point32(0, 0, -1)) == COUNTER_CLOCKWISE)) { + start1 = e; + } + } + e = e->next; + } while (e != c1->edges); + } + + if (start0 || start1) { + findEdgeForCoplanarFaces(c0, c1, start0, start1, NULL, NULL); + if (start0) { + c0 = start0->target; + } + if (start1) { + c1 = start1->target; + } + } + + prevPoint = c1->point; + prevPoint.z++; + } + else { + prevPoint = c1->point; + prevPoint.x++; + } + + Vertex* first0 = c0; + Vertex* first1 = c1; + bool firstRun = true; + + while (true) { + Point32 s = *c1 - *c0; + Point32 r = prevPoint - c0->point; + Point64 rxs = r.cross(s); + Point64 sxrxs = s.cross(rxs); #ifdef DEBUG_CONVEX_HULL - printf("\n Checking %d %d\n", c0->point.index, c1->point.index); + printf("\n Checking %d %d\n", c0->point.index, c1->point.index); #endif - Rational64 minCot0(0, 0); - Edge *min0 = findMaxAngle(false, c0, s, rxs, sxrxs, minCot0); - Rational64 minCot1(0, 0); - Edge *min1 = findMaxAngle(true, c1, s, rxs, sxrxs, minCot1); - if (!min0 && !min1) { - Edge *e = newEdgePair(c0, c1); - e->link(e); - c0->edges = e; - - e = e->reverse; - e->link(e); - c1->edges = e; - return; - } else { - int32_t cmp = !min0 ? 1 : !min1 ? -1 : minCot0.compare(minCot1); + Rational64 minCot0(0, 0); + Edge* min0 = findMaxAngle(false, c0, s, rxs, sxrxs, minCot0); + Rational64 minCot1(0, 0); + Edge* min1 = findMaxAngle(true, c1, s, rxs, sxrxs, minCot1); + if (!min0 && !min1) { + Edge* e = newEdgePair(c0, c1); + e->link(e); + c0->edges = e; + + e = e->reverse; + e->link(e); + c1->edges = e; + return; + } + else { + int32_t cmp = !min0 ? 1 : !min1 ? -1 : minCot0.compare(minCot1); #ifdef DEBUG_CONVEX_HULL - printf(" -> Result %d\n", cmp); + printf(" -> Result %d\n", cmp); #endif - if (firstRun || ((cmp >= 0) ? !minCot1.isNegativeInfinity() : !minCot0.isNegativeInfinity())) { - Edge *e = newEdgePair(c0, c1); - if (pendingTail0) { - pendingTail0->prev = e; - } else { - pendingHead0 = e; - } - e->next = pendingTail0; - pendingTail0 = e; - - e = e->reverse; - if (pendingTail1) { - pendingTail1->next = e; - } else { - pendingHead1 = e; - } - e->prev = pendingTail1; - pendingTail1 = e; - } - - Edge *e0 = min0; - Edge *e1 = min1; + if (firstRun || ((cmp >= 0) ? !minCot1.isNegativeInfinity() : !minCot0.isNegativeInfinity())) { + Edge* e = newEdgePair(c0, c1); + if (pendingTail0) { + pendingTail0->prev = e; + } + else { + pendingHead0 = e; + } + e->next = pendingTail0; + pendingTail0 = e; + + e = e->reverse; + if (pendingTail1) { + pendingTail1->next = e; + } + else { + pendingHead1 = e; + } + e->prev = pendingTail1; + pendingTail1 = e; + } + + Edge* e0 = min0; + Edge* e1 = min1; #ifdef DEBUG_CONVEX_HULL - printf(" Found min edges to %d %d\n", e0 ? e0->target->point.index : -1, e1 ? e1->target->point.index : -1); + printf(" Found min edges to %d %d\n", e0 ? e0->target->point.index : -1, e1 ? e1->target->point.index : -1); #endif - if (cmp == 0) { - findEdgeForCoplanarFaces(c0, c1, e0, e1, NULL, NULL); - } - - if ((cmp >= 0) && e1) { - if (toPrev1) { - for (Edge *e = toPrev1->next, *n = NULL; e != min1; e = n) { - n = e->next; - removeEdgePair(e); - } - } - - if (pendingTail1) { - if (toPrev1) { - toPrev1->link(pendingHead1); - } else { - min1->prev->link(pendingHead1); - firstNew1 = pendingHead1; - } - pendingTail1->link(min1); - pendingHead1 = NULL; - pendingTail1 = NULL; - } else if (!toPrev1) { - firstNew1 = min1; - } - - prevPoint = c1->point; - c1 = e1->target; - toPrev1 = e1->reverse; - } - - if ((cmp <= 0) && e0) { - if (toPrev0) { - for (Edge *e = toPrev0->prev, *n = NULL; e != min0; e = n) { - n = e->prev; - removeEdgePair(e); - } - } - - if (pendingTail0) { - if (toPrev0) { - pendingHead0->link(toPrev0); - } else { - pendingHead0->link(min0->next); - firstNew0 = pendingHead0; - } - min0->link(pendingTail0); - pendingHead0 = NULL; - pendingTail0 = NULL; - } else if (!toPrev0) { - firstNew0 = min0; - } - - prevPoint = c0->point; - c0 = e0->target; - toPrev0 = e0->reverse; - } - } - - if ((c0 == first0) && (c1 == first1)) { - if (toPrev0 == NULL) { - pendingHead0->link(pendingTail0); - c0->edges = pendingTail0; - } else { - for (Edge *e = toPrev0->prev, *n = NULL; e != firstNew0; e = n) { - n = e->prev; - removeEdgePair(e); - } - if (pendingTail0) { - pendingHead0->link(toPrev0); - firstNew0->link(pendingTail0); - } - } - - if (toPrev1 == NULL) { - pendingTail1->link(pendingHead1); - c1->edges = pendingTail1; - } else { - for (Edge *e = toPrev1->next, *n = NULL; e != firstNew1; e = n) { - n = e->next; - removeEdgePair(e); - } - if (pendingTail1) { - toPrev1->link(pendingHead1); - pendingTail1->link(firstNew1); - } - } - - return; - } - - firstRun = false; - } + if (cmp == 0) { + findEdgeForCoplanarFaces(c0, c1, e0, e1, NULL, NULL); + } + + if ((cmp >= 0) && e1) { + if (toPrev1) { + for (Edge *e = toPrev1->next, *n = NULL; e != min1; e = n) { + n = e->next; + removeEdgePair(e); + } + } + + if (pendingTail1) { + if (toPrev1) { + toPrev1->link(pendingHead1); + } + else { + min1->prev->link(pendingHead1); + firstNew1 = pendingHead1; + } + pendingTail1->link(min1); + pendingHead1 = NULL; + pendingTail1 = NULL; + } + else if (!toPrev1) { + firstNew1 = min1; + } + + prevPoint = c1->point; + c1 = e1->target; + toPrev1 = e1->reverse; + } + + if ((cmp <= 0) && e0) { + if (toPrev0) { + for (Edge *e = toPrev0->prev, *n = NULL; e != min0; e = n) { + n = e->prev; + removeEdgePair(e); + } + } + + if (pendingTail0) { + if (toPrev0) { + pendingHead0->link(toPrev0); + } + else { + pendingHead0->link(min0->next); + firstNew0 = pendingHead0; + } + min0->link(pendingTail0); + pendingHead0 = NULL; + pendingTail0 = NULL; + } + else if (!toPrev0) { + firstNew0 = min0; + } + + prevPoint = c0->point; + c0 = e0->target; + toPrev0 = e0->reverse; + } + } + + if ((c0 == first0) && (c1 == first1)) { + if (toPrev0 == NULL) { + pendingHead0->link(pendingTail0); + c0->edges = pendingTail0; + } + else { + for (Edge *e = toPrev0->prev, *n = NULL; e != firstNew0; e = n) { + n = e->prev; + removeEdgePair(e); + } + if (pendingTail0) { + pendingHead0->link(toPrev0); + firstNew0->link(pendingTail0); + } + } + + if (toPrev1 == NULL) { + pendingTail1->link(pendingHead1); + c1->edges = pendingTail1; + } + else { + for (Edge *e = toPrev1->next, *n = NULL; e != firstNew1; e = n) { + n = e->next; + removeEdgePair(e); + } + if (pendingTail1) { + toPrev1->link(pendingHead1); + pendingTail1->link(firstNew1); + } + } + + return; + } + + firstRun = false; + } } -static bool pointCmp(const btConvexHullInternal::Point32 &p, const btConvexHullInternal::Point32 &q) { - return (p.y < q.y) || ((p.y == q.y) && ((p.x < q.x) || ((p.x == q.x) && (p.z < q.z)))); +static bool pointCmp(const btConvexHullInternal::Point32& p, const btConvexHullInternal::Point32& q) +{ + return (p.y < q.y) || ((p.y == q.y) && ((p.x < q.x) || ((p.x == q.x) && (p.z < q.z)))); } -void btConvexHullInternal::compute(const void *coords, bool doubleCoords, int32_t stride, int32_t count) { - btVector3 min(btScalar(1e30), btScalar(1e30), btScalar(1e30)), max(btScalar(-1e30), btScalar(-1e30), btScalar(-1e30)); - const char *ptr = (const char *)coords; - if (doubleCoords) { - for (int32_t i = 0; i < count; i++) { - const double *v = (const double *)ptr; - btVector3 p((btScalar)v[0], (btScalar)v[1], (btScalar)v[2]); - ptr += stride; - min.setMin(p); - max.setMax(p); - } - } else { - for (int32_t i = 0; i < count; i++) { - const float *v = (const float *)ptr; - btVector3 p(v[0], v[1], v[2]); - ptr += stride; - min.setMin(p); - max.setMax(p); - } - } - - btVector3 s = max - min; - maxAxis = s.maxAxis(); - minAxis = s.minAxis(); - if (minAxis == maxAxis) { - minAxis = (maxAxis + 1) % 3; - } - medAxis = 3 - maxAxis - minAxis; - - s /= btScalar(10216); - if (((medAxis + 1) % 3) != maxAxis) { - s *= -1; - } - scaling = s; - - if (s[0] != 0) { - s[0] = btScalar(1) / s[0]; - } - if (s[1] != 0) { - s[1] = btScalar(1) / s[1]; - } - if (s[2] != 0) { - s[2] = btScalar(1) / s[2]; - } - - center = (min + max) * btScalar(0.5); - - btAlignedObjectArray<Point32> points; - points.resize(count); - ptr = (const char *)coords; - if (doubleCoords) { - for (int32_t i = 0; i < count; i++) { - const double *v = (const double *)ptr; - btVector3 p((btScalar)v[0], (btScalar)v[1], (btScalar)v[2]); - ptr += stride; - p = (p - center) * s; - points[i].x = (int32_t)p[medAxis]; - points[i].y = (int32_t)p[maxAxis]; - points[i].z = (int32_t)p[minAxis]; - points[i].index = i; - } - } else { - for (int32_t i = 0; i < count; i++) { - const float *v = (const float *)ptr; - btVector3 p(v[0], v[1], v[2]); - ptr += stride; - p = (p - center) * s; - points[i].x = (int32_t)p[medAxis]; - points[i].y = (int32_t)p[maxAxis]; - points[i].z = (int32_t)p[minAxis]; - points[i].index = i; - } - } - points.quickSort(pointCmp); - - vertexPool.reset(); - vertexPool.setArraySize(count); - originalVertices.resize(count); - for (int32_t i = 0; i < count; i++) { - Vertex *v = vertexPool.newObject(); - v->edges = NULL; - v->point = points[i]; - v->copy = -1; - originalVertices[i] = v; - } - - points.clear(); - - edgePool.reset(); - edgePool.setArraySize(6 * count); - - usedEdgePairs = 0; - maxUsedEdgePairs = 0; - - mergeStamp = -3; - - IntermediateHull hull; - computeInternal(0, count, hull); - vertexList = hull.minXy; +void btConvexHullInternal::compute(const void* coords, bool doubleCoords, int32_t stride, int32_t count) +{ + btVector3 min(btScalar(1e30), btScalar(1e30), btScalar(1e30)), max(btScalar(-1e30), btScalar(-1e30), btScalar(-1e30)); + const char* ptr = (const char*)coords; + if (doubleCoords) { + for (int32_t i = 0; i < count; i++) { + const double* v = (const double*)ptr; + btVector3 p((btScalar)v[0], (btScalar)v[1], (btScalar)v[2]); + ptr += stride; + min.setMin(p); + max.setMax(p); + } + } + else { + for (int32_t i = 0; i < count; i++) { + const float* v = (const float*)ptr; + btVector3 p(v[0], v[1], v[2]); + ptr += stride; + min.setMin(p); + max.setMax(p); + } + } + + btVector3 s = max - min; + maxAxis = s.maxAxis(); + minAxis = s.minAxis(); + if (minAxis == maxAxis) { + minAxis = (maxAxis + 1) % 3; + } + medAxis = 3 - maxAxis - minAxis; + + s /= btScalar(10216); + if (((medAxis + 1) % 3) != maxAxis) { + s *= -1; + } + scaling = s; + + if (s[0] != 0) { + s[0] = btScalar(1) / s[0]; + } + if (s[1] != 0) { + s[1] = btScalar(1) / s[1]; + } + if (s[2] != 0) { + s[2] = btScalar(1) / s[2]; + } + + center = (min + max) * btScalar(0.5); + + btAlignedObjectArray<Point32> points; + points.resize(count); + ptr = (const char*)coords; + if (doubleCoords) { + for (int32_t i = 0; i < count; i++) { + const double* v = (const double*)ptr; + btVector3 p((btScalar)v[0], (btScalar)v[1], (btScalar)v[2]); + ptr += stride; + p = (p - center) * s; + points[i].x = (int32_t)p[medAxis]; + points[i].y = (int32_t)p[maxAxis]; + points[i].z = (int32_t)p[minAxis]; + points[i].index = i; + } + } + else { + for (int32_t i = 0; i < count; i++) { + const float* v = (const float*)ptr; + btVector3 p(v[0], v[1], v[2]); + ptr += stride; + p = (p - center) * s; + points[i].x = (int32_t)p[medAxis]; + points[i].y = (int32_t)p[maxAxis]; + points[i].z = (int32_t)p[minAxis]; + points[i].index = i; + } + } + points.quickSort(pointCmp); + + vertexPool.reset(); + vertexPool.setArraySize(count); + originalVertices.resize(count); + for (int32_t i = 0; i < count; i++) { + Vertex* v = vertexPool.newObject(); + v->edges = NULL; + v->point = points[i]; + v->copy = -1; + originalVertices[i] = v; + } + + points.clear(); + + edgePool.reset(); + edgePool.setArraySize(6 * count); + + usedEdgePairs = 0; + maxUsedEdgePairs = 0; + + mergeStamp = -3; + + IntermediateHull hull; + computeInternal(0, count, hull); + vertexList = hull.minXy; #ifdef DEBUG_CONVEX_HULL - printf("max. edges %d (3v = %d)", maxUsedEdgePairs, 3 * count); + printf("max. edges %d (3v = %d)", maxUsedEdgePairs, 3 * count); #endif } -btVector3 btConvexHullInternal::toBtVector(const Point32 &v) { - btVector3 p; - p[medAxis] = btScalar(v.x); - p[maxAxis] = btScalar(v.y); - p[minAxis] = btScalar(v.z); - return p * scaling; +btVector3 btConvexHullInternal::toBtVector(const Point32& v) +{ + btVector3 p; + p[medAxis] = btScalar(v.x); + p[maxAxis] = btScalar(v.y); + p[minAxis] = btScalar(v.z); + return p * scaling; } -btVector3 btConvexHullInternal::getBtNormal(Face *face) { - return toBtVector(face->dir0).cross(toBtVector(face->dir1)).normalized(); +btVector3 btConvexHullInternal::getBtNormal(Face* face) +{ + return toBtVector(face->dir0).cross(toBtVector(face->dir1)).normalized(); } -btVector3 btConvexHullInternal::getCoordinates(const Vertex *v) { - btVector3 p; - p[medAxis] = v->xvalue(); - p[maxAxis] = v->yvalue(); - p[minAxis] = v->zvalue(); - return p * scaling + center; +btVector3 btConvexHullInternal::getCoordinates(const Vertex* v) +{ + btVector3 p; + p[medAxis] = v->xvalue(); + p[maxAxis] = v->yvalue(); + p[minAxis] = v->zvalue(); + return p * scaling + center; } -btScalar btConvexHullInternal::shrink(btScalar amount, btScalar clampAmount) { - if (!vertexList) { - return 0; - } - int32_t stamp = --mergeStamp; - btAlignedObjectArray<Vertex *> stack; - vertexList->copy = stamp; - stack.push_back(vertexList); - btAlignedObjectArray<Face *> faces; - - Point32 ref = vertexList->point; - Int128 hullCenterX(0, 0); - Int128 hullCenterY(0, 0); - Int128 hullCenterZ(0, 0); - Int128 volume(0, 0); - - while (stack.size() > 0) { - Vertex *v = stack[stack.size() - 1]; - stack.pop_back(); - Edge *e = v->edges; - if (e) { - do { - if (e->target->copy != stamp) { - e->target->copy = stamp; - stack.push_back(e->target); - } - if (e->copy != stamp) { - Face *face = facePool.newObject(); - face->init(e->target, e->reverse->prev->target, v); - faces.push_back(face); - Edge *f = e; - - Vertex *a = NULL; - Vertex *b = NULL; - do { - if (a && b) { - int64_t vol = (v->point - ref).dot((a->point - ref).cross(b->point - ref)); - btAssert(vol >= 0); - Point32 c = v->point + a->point + b->point + ref; - hullCenterX += vol * c.x; - hullCenterY += vol * c.y; - hullCenterZ += vol * c.z; - volume += vol; - } - - btAssert(f->copy != stamp); - f->copy = stamp; - f->face = face; - - a = b; - b = f->target; - - f = f->reverse->prev; - } while (f != e); - } - e = e->next; - } while (e != v->edges); - } - } - - if (volume.getSign() <= 0) { - return 0; - } - - btVector3 hullCenter; - hullCenter[medAxis] = hullCenterX.toScalar(); - hullCenter[maxAxis] = hullCenterY.toScalar(); - hullCenter[minAxis] = hullCenterZ.toScalar(); - hullCenter /= 4 * volume.toScalar(); - hullCenter *= scaling; - - int32_t faceCount = faces.size(); - - if (clampAmount > 0) { - btScalar minDist = SIMD_INFINITY; - for (int32_t i = 0; i < faceCount; i++) { - btVector3 normal = getBtNormal(faces[i]); - btScalar dist = normal.dot(toBtVector(faces[i]->origin) - hullCenter); - if (dist < minDist) { - minDist = dist; - } - } - - if (minDist <= 0) { - return 0; - } - - amount = btMin(amount, minDist * clampAmount); - } - - uint32_t seed = 243703; - for (int32_t i = 0; i < faceCount; i++, seed = 1664525 * seed + 1013904223) { - btSwap(faces[i], faces[seed % faceCount]); - } - - for (int32_t i = 0; i < faceCount; i++) { - if (!shiftFace(faces[i], amount, stack)) { - return -amount; - } - } - - return amount; +btScalar btConvexHullInternal::shrink(btScalar amount, btScalar clampAmount) +{ + if (!vertexList) { + return 0; + } + int32_t stamp = --mergeStamp; + btAlignedObjectArray<Vertex*> stack; + vertexList->copy = stamp; + stack.push_back(vertexList); + btAlignedObjectArray<Face*> faces; + + Point32 ref = vertexList->point; + Int128 hullCenterX(0, 0); + Int128 hullCenterY(0, 0); + Int128 hullCenterZ(0, 0); + Int128 volume(0, 0); + + while (stack.size() > 0) { + Vertex* v = stack[stack.size() - 1]; + stack.pop_back(); + Edge* e = v->edges; + if (e) { + do { + if (e->target->copy != stamp) { + e->target->copy = stamp; + stack.push_back(e->target); + } + if (e->copy != stamp) { + Face* face = facePool.newObject(); + face->init(e->target, e->reverse->prev->target, v); + faces.push_back(face); + Edge* f = e; + + Vertex* a = NULL; + Vertex* b = NULL; + do { + if (a && b) { + int64_t vol = (v->point - ref).dot((a->point - ref).cross(b->point - ref)); + btAssert(vol >= 0); + Point32 c = v->point + a->point + b->point + ref; + hullCenterX += vol * c.x; + hullCenterY += vol * c.y; + hullCenterZ += vol * c.z; + volume += vol; + } + + btAssert(f->copy != stamp); + f->copy = stamp; + f->face = face; + + a = b; + b = f->target; + + f = f->reverse->prev; + } while (f != e); + } + e = e->next; + } while (e != v->edges); + } + } + + if (volume.getSign() <= 0) { + return 0; + } + + btVector3 hullCenter; + hullCenter[medAxis] = hullCenterX.toScalar(); + hullCenter[maxAxis] = hullCenterY.toScalar(); + hullCenter[minAxis] = hullCenterZ.toScalar(); + hullCenter /= 4 * volume.toScalar(); + hullCenter *= scaling; + + int32_t faceCount = faces.size(); + + if (clampAmount > 0) { + btScalar minDist = SIMD_INFINITY; + for (int32_t i = 0; i < faceCount; i++) { + btVector3 normal = getBtNormal(faces[i]); + btScalar dist = normal.dot(toBtVector(faces[i]->origin) - hullCenter); + if (dist < minDist) { + minDist = dist; + } + } + + if (minDist <= 0) { + return 0; + } + + amount = btMin(amount, minDist * clampAmount); + } + + uint32_t seed = 243703; + for (int32_t i = 0; i < faceCount; i++, seed = 1664525 * seed + 1013904223) { + btSwap(faces[i], faces[seed % faceCount]); + } + + for (int32_t i = 0; i < faceCount; i++) { + if (!shiftFace(faces[i], amount, stack)) { + return -amount; + } + } + + return amount; } -bool btConvexHullInternal::shiftFace(Face *face, btScalar amount, btAlignedObjectArray<Vertex *> stack) { - btVector3 origShift = getBtNormal(face) * -amount; - if (scaling[0] != 0) { - origShift[0] /= scaling[0]; - } - if (scaling[1] != 0) { - origShift[1] /= scaling[1]; - } - if (scaling[2] != 0) { - origShift[2] /= scaling[2]; - } - Point32 shift((int32_t)origShift[medAxis], (int32_t)origShift[maxAxis], (int32_t)origShift[minAxis]); - if (shift.isZero()) { - return true; - } - Point64 normal = face->getNormal(); +bool btConvexHullInternal::shiftFace(Face* face, btScalar amount, btAlignedObjectArray<Vertex*> stack) +{ + btVector3 origShift = getBtNormal(face) * -amount; + if (scaling[0] != 0) { + origShift[0] /= scaling[0]; + } + if (scaling[1] != 0) { + origShift[1] /= scaling[1]; + } + if (scaling[2] != 0) { + origShift[2] /= scaling[2]; + } + Point32 shift((int32_t)origShift[medAxis], (int32_t)origShift[maxAxis], (int32_t)origShift[minAxis]); + if (shift.isZero()) { + return true; + } + Point64 normal = face->getNormal(); #ifdef DEBUG_CONVEX_HULL - printf("\nShrinking face (%d %d %d) (%d %d %d) (%d %d %d) by (%d %d %d)\n", - 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); + printf("\nShrinking face (%d %d %d) (%d %d %d) (%d %d %d) by (%d %d %d)\n", + 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); #endif - int64_t origDot = face->origin.dot(normal); - Point32 shiftedOrigin = face->origin + shift; - int64_t shiftedDot = shiftedOrigin.dot(normal); - btAssert(shiftedDot <= origDot); - if (shiftedDot >= origDot) { - return false; - } + int64_t origDot = face->origin.dot(normal); + Point32 shiftedOrigin = face->origin + shift; + int64_t shiftedDot = shiftedOrigin.dot(normal); + btAssert(shiftedDot <= origDot); + if (shiftedDot >= origDot) { + return false; + } - Edge *intersection = NULL; + Edge* intersection = NULL; - Edge *startEdge = face->nearbyVertex->edges; + Edge* startEdge = face->nearbyVertex->edges; #ifdef DEBUG_CONVEX_HULL - printf("Start edge is "); - startEdge->print(); - printf(", normal is (%lld %lld %lld), shifted dot is %lld\n", normal.x, normal.y, normal.z, shiftedDot); + printf("Start edge is "); + startEdge->print(); + printf(", normal is (%lld %lld %lld), shifted dot is %lld\n", normal.x, normal.y, normal.z, shiftedDot); #endif - Rational128 optDot = face->nearbyVertex->dot(normal); - int32_t cmp = optDot.compare(shiftedDot); + Rational128 optDot = face->nearbyVertex->dot(normal); + int32_t cmp = optDot.compare(shiftedDot); #ifdef SHOW_ITERATIONS - int32_t n = 0; + int32_t n = 0; #endif - if (cmp >= 0) { - Edge *e = startEdge; - do { + if (cmp >= 0) { + Edge* e = startEdge; + do { #ifdef SHOW_ITERATIONS - n++; + n++; #endif - Rational128 dot = e->target->dot(normal); - btAssert(dot.compare(origDot) <= 0); + Rational128 dot = e->target->dot(normal); + btAssert(dot.compare(origDot) <= 0); #ifdef DEBUG_CONVEX_HULL - printf("Moving downwards, edge is "); - e->print(); - printf(", dot is %f (%f %lld)\n", (float)dot.toScalar(), (float)optDot.toScalar(), shiftedDot); + printf("Moving downwards, edge is "); + e->print(); + printf(", dot is %f (%f %lld)\n", (float)dot.toScalar(), (float)optDot.toScalar(), shiftedDot); #endif - if (dot.compare(optDot) < 0) { - int32_t c = dot.compare(shiftedDot); - optDot = dot; - e = e->reverse; - startEdge = e; - if (c < 0) { - intersection = e; - break; - } - cmp = c; - } - e = e->prev; - } while (e != startEdge); - - if (!intersection) { - return false; - } - } else { - Edge *e = startEdge; - do { + if (dot.compare(optDot) < 0) { + int32_t c = dot.compare(shiftedDot); + optDot = dot; + e = e->reverse; + startEdge = e; + if (c < 0) { + intersection = e; + break; + } + cmp = c; + } + e = e->prev; + } while (e != startEdge); + + if (!intersection) { + return false; + } + } + else { + Edge* e = startEdge; + do { #ifdef SHOW_ITERATIONS - n++; + n++; #endif - Rational128 dot = e->target->dot(normal); - btAssert(dot.compare(origDot) <= 0); + Rational128 dot = e->target->dot(normal); + btAssert(dot.compare(origDot) <= 0); #ifdef DEBUG_CONVEX_HULL - printf("Moving upwards, edge is "); - e->print(); - printf(", dot is %f (%f %lld)\n", (float)dot.toScalar(), (float)optDot.toScalar(), shiftedDot); + printf("Moving upwards, edge is "); + e->print(); + printf(", dot is %f (%f %lld)\n", (float)dot.toScalar(), (float)optDot.toScalar(), shiftedDot); #endif - if (dot.compare(optDot) > 0) { - cmp = dot.compare(shiftedDot); - if (cmp >= 0) { - intersection = e; - break; - } - optDot = dot; - e = e->reverse; - startEdge = e; - } - e = e->prev; - } while (e != startEdge); - - if (!intersection) { - return true; - } - } + if (dot.compare(optDot) > 0) { + cmp = dot.compare(shiftedDot); + if (cmp >= 0) { + intersection = e; + break; + } + optDot = dot; + e = e->reverse; + startEdge = e; + } + e = e->prev; + } while (e != startEdge); + + if (!intersection) { + return true; + } + } #ifdef SHOW_ITERATIONS - printf("Needed %d iterations to find initial intersection\n", n); + printf("Needed %d iterations to find initial intersection\n", n); #endif - if (cmp == 0) { - Edge *e = intersection->reverse->next; + if (cmp == 0) { + Edge* e = intersection->reverse->next; #ifdef SHOW_ITERATIONS - n = 0; + n = 0; #endif - while (e->target->dot(normal).compare(shiftedDot) <= 0) { + while (e->target->dot(normal).compare(shiftedDot) <= 0) { #ifdef SHOW_ITERATIONS - n++; + n++; #endif - e = e->next; - if (e == intersection->reverse) { - return true; - } + e = e->next; + if (e == intersection->reverse) { + return true; + } #ifdef DEBUG_CONVEX_HULL - printf("Checking for outwards edge, current edge is "); - e->print(); - printf("\n"); + printf("Checking for outwards edge, current edge is "); + e->print(); + printf("\n"); #endif - } + } #ifdef SHOW_ITERATIONS - printf("Needed %d iterations to check for complete containment\n", n); + printf("Needed %d iterations to check for complete containment\n", n); #endif - } + } - Edge *firstIntersection = NULL; - Edge *faceEdge = NULL; - Edge *firstFaceEdge = NULL; + Edge* firstIntersection = NULL; + Edge* faceEdge = NULL; + Edge* firstFaceEdge = NULL; #ifdef SHOW_ITERATIONS - int32_t m = 0; + int32_t m = 0; #endif - while (true) { + while (true) { #ifdef SHOW_ITERATIONS - m++; + m++; #endif #ifdef DEBUG_CONVEX_HULL - printf("Intersecting edge is "); - intersection->print(); - printf("\n"); + printf("Intersecting edge is "); + intersection->print(); + printf("\n"); #endif - if (cmp == 0) { - Edge *e = intersection->reverse->next; - startEdge = e; + if (cmp == 0) { + Edge* e = intersection->reverse->next; + startEdge = e; #ifdef SHOW_ITERATIONS - n = 0; + n = 0; #endif - while (true) { + while (true) { #ifdef SHOW_ITERATIONS - n++; + n++; #endif - if (e->target->dot(normal).compare(shiftedDot) >= 0) { - break; - } - intersection = e->reverse; - e = e->next; - if (e == startEdge) { - return true; - } - } + if (e->target->dot(normal).compare(shiftedDot) >= 0) { + break; + } + intersection = e->reverse; + e = e->next; + if (e == startEdge) { + return true; + } + } #ifdef SHOW_ITERATIONS - printf("Needed %d iterations to advance intersection\n", n); + printf("Needed %d iterations to advance intersection\n", n); #endif - } + } #ifdef DEBUG_CONVEX_HULL - printf("Advanced intersecting edge to "); - intersection->print(); - printf(", cmp = %d\n", cmp); + printf("Advanced intersecting edge to "); + intersection->print(); + printf(", cmp = %d\n", cmp); #endif - if (!firstIntersection) { - firstIntersection = intersection; - } else if (intersection == firstIntersection) { - break; - } + if (!firstIntersection) { + firstIntersection = intersection; + } + else if (intersection == firstIntersection) { + break; + } - int32_t prevCmp = cmp; - Edge *prevIntersection = intersection; - Edge *prevFaceEdge = faceEdge; + int32_t prevCmp = cmp; + Edge* prevIntersection = intersection; + Edge* prevFaceEdge = faceEdge; - Edge *e = intersection->reverse; + Edge* e = intersection->reverse; #ifdef SHOW_ITERATIONS - n = 0; + n = 0; #endif - while (true) { + while (true) { #ifdef SHOW_ITERATIONS - n++; + n++; #endif - e = e->reverse->prev; - btAssert(e != intersection->reverse); - cmp = e->target->dot(normal).compare(shiftedDot); + e = e->reverse->prev; + btAssert(e != intersection->reverse); + cmp = e->target->dot(normal).compare(shiftedDot); #ifdef DEBUG_CONVEX_HULL - printf("Testing edge "); - e->print(); - printf(" -> cmp = %d\n", cmp); + printf("Testing edge "); + e->print(); + printf(" -> cmp = %d\n", cmp); #endif - if (cmp >= 0) { - intersection = e; - break; - } - } + if (cmp >= 0) { + intersection = e; + break; + } + } #ifdef SHOW_ITERATIONS - printf("Needed %d iterations to find other intersection of face\n", n); + printf("Needed %d iterations to find other intersection of face\n", n); #endif - if (cmp > 0) { - Vertex *removed = intersection->target; - e = intersection->reverse; - if (e->prev == e) { - removed->edges = NULL; - } else { - removed->edges = e->prev; - e->prev->link(e->next); - e->link(e); - } + if (cmp > 0) { + Vertex* removed = intersection->target; + e = intersection->reverse; + if (e->prev == e) { + removed->edges = NULL; + } + else { + removed->edges = e->prev; + e->prev->link(e->next); + e->link(e); + } #ifdef DEBUG_CONVEX_HULL - printf("1: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z); + printf("1: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z); #endif - Point64 n0 = intersection->face->getNormal(); - Point64 n1 = intersection->reverse->face->getNormal(); - int64_t m00 = face->dir0.dot(n0); - int64_t m01 = face->dir1.dot(n0); - int64_t m10 = face->dir0.dot(n1); - int64_t m11 = face->dir1.dot(n1); - int64_t r0 = (intersection->face->origin - shiftedOrigin).dot(n0); - int64_t r1 = (intersection->reverse->face->origin - shiftedOrigin).dot(n1); - Int128 det = Int128::mul(m00, m11) - Int128::mul(m01, m10); - btAssert(det.getSign() != 0); - Vertex *v = vertexPool.newObject(); - v->point.index = -1; - v->copy = -1; - v->point128 = PointR128(Int128::mul(face->dir0.x * r0, m11) - Int128::mul(face->dir0.x * r1, m01) + Int128::mul(face->dir1.x * r1, m00) - Int128::mul(face->dir1.x * r0, m10) + det * shiftedOrigin.x, - Int128::mul(face->dir0.y * r0, m11) - Int128::mul(face->dir0.y * r1, m01) + Int128::mul(face->dir1.y * r1, m00) - Int128::mul(face->dir1.y * r0, m10) + det * shiftedOrigin.y, - Int128::mul(face->dir0.z * r0, m11) - Int128::mul(face->dir0.z * r1, m01) + Int128::mul(face->dir1.z * r1, m00) - Int128::mul(face->dir1.z * r0, m10) + det * shiftedOrigin.z, - det); - v->point.x = (int32_t)v->point128.xvalue(); - v->point.y = (int32_t)v->point128.yvalue(); - v->point.z = (int32_t)v->point128.zvalue(); - intersection->target = v; - v->edges = e; - - stack.push_back(v); - stack.push_back(removed); - stack.push_back(NULL); - } - - if (cmp || prevCmp || (prevIntersection->reverse->next->target != intersection->target)) { - faceEdge = newEdgePair(prevIntersection->target, intersection->target); - if (prevCmp == 0) { - faceEdge->link(prevIntersection->reverse->next); - } - if ((prevCmp == 0) || prevFaceEdge) { - prevIntersection->reverse->link(faceEdge); - } - if (cmp == 0) { - intersection->reverse->prev->link(faceEdge->reverse); - } - faceEdge->reverse->link(intersection->reverse); - } else { - faceEdge = prevIntersection->reverse->next; - } - - if (prevFaceEdge) { - if (prevCmp > 0) { - faceEdge->link(prevFaceEdge->reverse); - } else if (faceEdge != prevFaceEdge->reverse) { - stack.push_back(prevFaceEdge->target); - while (faceEdge->next != prevFaceEdge->reverse) { - Vertex *removed = faceEdge->next->target; - removeEdgePair(faceEdge->next); - stack.push_back(removed); + Point64 n0 = intersection->face->getNormal(); + Point64 n1 = intersection->reverse->face->getNormal(); + int64_t m00 = face->dir0.dot(n0); + int64_t m01 = face->dir1.dot(n0); + int64_t m10 = face->dir0.dot(n1); + int64_t m11 = face->dir1.dot(n1); + int64_t r0 = (intersection->face->origin - shiftedOrigin).dot(n0); + int64_t r1 = (intersection->reverse->face->origin - shiftedOrigin).dot(n1); + Int128 det = Int128::mul(m00, m11) - Int128::mul(m01, m10); + btAssert(det.getSign() != 0); + Vertex* v = vertexPool.newObject(); + v->point.index = -1; + v->copy = -1; + v->point128 = PointR128(Int128::mul(face->dir0.x * r0, m11) - Int128::mul(face->dir0.x * r1, m01) + + Int128::mul(face->dir1.x * r1, m00) - Int128::mul(face->dir1.x * r0, m10) + det * shiftedOrigin.x, + Int128::mul(face->dir0.y * r0, m11) - Int128::mul(face->dir0.y * r1, m01) + + Int128::mul(face->dir1.y * r1, m00) - Int128::mul(face->dir1.y * r0, m10) + det * shiftedOrigin.y, + Int128::mul(face->dir0.z * r0, m11) - Int128::mul(face->dir0.z * r1, m01) + + Int128::mul(face->dir1.z * r1, m00) - Int128::mul(face->dir1.z * r0, m10) + det * shiftedOrigin.z, + det); + v->point.x = (int32_t)v->point128.xvalue(); + v->point.y = (int32_t)v->point128.yvalue(); + v->point.z = (int32_t)v->point128.zvalue(); + intersection->target = v; + v->edges = e; + + stack.push_back(v); + stack.push_back(removed); + stack.push_back(NULL); + } + + if (cmp || prevCmp || (prevIntersection->reverse->next->target != intersection->target)) { + faceEdge = newEdgePair(prevIntersection->target, intersection->target); + if (prevCmp == 0) { + faceEdge->link(prevIntersection->reverse->next); + } + if ((prevCmp == 0) || prevFaceEdge) { + prevIntersection->reverse->link(faceEdge); + } + if (cmp == 0) { + intersection->reverse->prev->link(faceEdge->reverse); + } + faceEdge->reverse->link(intersection->reverse); + } + else { + faceEdge = prevIntersection->reverse->next; + } + + if (prevFaceEdge) { + if (prevCmp > 0) { + faceEdge->link(prevFaceEdge->reverse); + } + else if (faceEdge != prevFaceEdge->reverse) { + stack.push_back(prevFaceEdge->target); + while (faceEdge->next != prevFaceEdge->reverse) { + Vertex* removed = faceEdge->next->target; + removeEdgePair(faceEdge->next); + stack.push_back(removed); #ifdef DEBUG_CONVEX_HULL - printf("2: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z); + printf("2: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z); #endif - } - stack.push_back(NULL); - } - } - faceEdge->face = face; - faceEdge->reverse->face = intersection->face; - - if (!firstFaceEdge) { - firstFaceEdge = faceEdge; - } - } + } + stack.push_back(NULL); + } + } + faceEdge->face = face; + faceEdge->reverse->face = intersection->face; + + if (!firstFaceEdge) { + firstFaceEdge = faceEdge; + } + } #ifdef SHOW_ITERATIONS - printf("Needed %d iterations to process all intersections\n", m); + printf("Needed %d iterations to process all intersections\n", m); #endif - if (cmp > 0) { - firstFaceEdge->reverse->target = faceEdge->target; - firstIntersection->reverse->link(firstFaceEdge); - firstFaceEdge->link(faceEdge->reverse); - } else if (firstFaceEdge != faceEdge->reverse) { - stack.push_back(faceEdge->target); - while (firstFaceEdge->next != faceEdge->reverse) { - Vertex *removed = firstFaceEdge->next->target; - removeEdgePair(firstFaceEdge->next); - stack.push_back(removed); + if (cmp > 0) { + firstFaceEdge->reverse->target = faceEdge->target; + firstIntersection->reverse->link(firstFaceEdge); + firstFaceEdge->link(faceEdge->reverse); + } + else if (firstFaceEdge != faceEdge->reverse) { + stack.push_back(faceEdge->target); + while (firstFaceEdge->next != faceEdge->reverse) { + Vertex* removed = firstFaceEdge->next->target; + removeEdgePair(firstFaceEdge->next); + stack.push_back(removed); #ifdef DEBUG_CONVEX_HULL - printf("3: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z); + printf("3: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z); #endif - } - stack.push_back(NULL); - } + } + stack.push_back(NULL); + } - btAssert(stack.size() > 0); - vertexList = stack[0]; + btAssert(stack.size() > 0); + vertexList = stack[0]; #ifdef DEBUG_CONVEX_HULL - printf("Removing part\n"); + printf("Removing part\n"); #endif #ifdef SHOW_ITERATIONS - n = 0; + n = 0; #endif - int32_t pos = 0; - while (pos < stack.size()) { - int32_t end = stack.size(); - while (pos < end) { - Vertex *kept = stack[pos++]; + int32_t pos = 0; + while (pos < stack.size()) { + int32_t end = stack.size(); + while (pos < end) { + Vertex* kept = stack[pos++]; #ifdef DEBUG_CONVEX_HULL - kept->print(); + kept->print(); #endif - bool deeper = false; - Vertex *removed; - while ((removed = stack[pos++]) != NULL) { + bool deeper = false; + Vertex* removed; + while ((removed = stack[pos++]) != NULL) { #ifdef SHOW_ITERATIONS - n++; + n++; #endif - kept->receiveNearbyFaces(removed); - while (removed->edges) { - if (!deeper) { - deeper = true; - stack.push_back(kept); - } - stack.push_back(removed->edges->target); - removeEdgePair(removed->edges); - } - } - if (deeper) { - stack.push_back(NULL); - } - } - } + kept->receiveNearbyFaces(removed); + while (removed->edges) { + if (!deeper) { + deeper = true; + stack.push_back(kept); + } + stack.push_back(removed->edges->target); + removeEdgePair(removed->edges); + } + } + if (deeper) { + stack.push_back(NULL); + } + } + } #ifdef SHOW_ITERATIONS - printf("Needed %d iterations to remove part\n", n); + printf("Needed %d iterations to remove part\n", n); #endif - stack.resize(0); - face->origin = shiftedOrigin; + stack.resize(0); + face->origin = shiftedOrigin; - return true; + return true; } -static int32_t getVertexCopy(btConvexHullInternal::Vertex *vertex, btAlignedObjectArray<btConvexHullInternal::Vertex *> &vertices) { - int32_t index = vertex->copy; - if (index < 0) { - index = vertices.size(); - vertex->copy = index; - vertices.push_back(vertex); +static int32_t getVertexCopy(btConvexHullInternal::Vertex* vertex, btAlignedObjectArray<btConvexHullInternal::Vertex*>& vertices) +{ + int32_t index = vertex->copy; + if (index < 0) { + index = vertices.size(); + vertex->copy = index; + vertices.push_back(vertex); #ifdef DEBUG_CONVEX_HULL - printf("Vertex %d gets index *%d\n", vertex->point.index, index); + printf("Vertex %d gets index *%d\n", vertex->point.index, index); #endif - } - return index; + } + return index; } -btScalar btConvexHullComputer::compute(const void *coords, bool doubleCoords, int32_t stride, int32_t count, btScalar shrink, btScalar shrinkClamp) { - if (count <= 0) { - vertices.clear(); - edges.clear(); - faces.clear(); - return 0; - } - - btConvexHullInternal hull; - hull.compute(coords, doubleCoords, stride, count); - - btScalar shift = 0; - if ((shrink > 0) && ((shift = hull.shrink(shrink, shrinkClamp)) < 0)) { - vertices.clear(); - edges.clear(); - faces.clear(); - return shift; - } - - vertices.resize(0); - edges.resize(0); - faces.resize(0); - - btAlignedObjectArray<btConvexHullInternal::Vertex *> oldVertices; - getVertexCopy(hull.vertexList, oldVertices); - int32_t copied = 0; - while (copied < oldVertices.size()) { - btConvexHullInternal::Vertex *v = oldVertices[copied]; - vertices.push_back(hull.getCoordinates(v)); - btConvexHullInternal::Edge *firstEdge = v->edges; - if (firstEdge) { - int32_t firstCopy = -1; - int32_t prevCopy = -1; - btConvexHullInternal::Edge *e = firstEdge; - do { - if (e->copy < 0) { - int32_t s = edges.size(); - edges.push_back(Edge()); - edges.push_back(Edge()); - Edge *c = &edges[s]; - Edge *r = &edges[s + 1]; - e->copy = s; - e->reverse->copy = s + 1; - c->reverse = 1; - r->reverse = -1; - c->targetVertex = getVertexCopy(e->target, oldVertices); - r->targetVertex = copied; +btScalar btConvexHullComputer::compute(const void* coords, bool doubleCoords, int32_t stride, int32_t count, btScalar shrink, btScalar shrinkClamp) +{ + if (count <= 0) { + vertices.clear(); + edges.clear(); + faces.clear(); + return 0; + } + + btConvexHullInternal hull; + hull.compute(coords, doubleCoords, stride, count); + + btScalar shift = 0; + if ((shrink > 0) && ((shift = hull.shrink(shrink, shrinkClamp)) < 0)) { + vertices.clear(); + edges.clear(); + faces.clear(); + return shift; + } + + vertices.resize(0); + edges.resize(0); + faces.resize(0); + + btAlignedObjectArray<btConvexHullInternal::Vertex*> oldVertices; + getVertexCopy(hull.vertexList, oldVertices); + int32_t copied = 0; + while (copied < oldVertices.size()) { + btConvexHullInternal::Vertex* v = oldVertices[copied]; + vertices.push_back(hull.getCoordinates(v)); + btConvexHullInternal::Edge* firstEdge = v->edges; + if (firstEdge) { + int32_t firstCopy = -1; + int32_t prevCopy = -1; + btConvexHullInternal::Edge* e = firstEdge; + do { + if (e->copy < 0) { + int32_t s = edges.size(); + edges.push_back(Edge()); + edges.push_back(Edge()); + Edge* c = &edges[s]; + Edge* r = &edges[s + 1]; + e->copy = s; + e->reverse->copy = s + 1; + c->reverse = 1; + r->reverse = -1; + c->targetVertex = getVertexCopy(e->target, oldVertices); + r->targetVertex = copied; #ifdef DEBUG_CONVEX_HULL - printf(" CREATE: Vertex *%d has edge to *%d\n", copied, c->getTargetVertex()); + printf(" CREATE: Vertex *%d has edge to *%d\n", copied, c->getTargetVertex()); #endif - } - if (prevCopy >= 0) { - edges[e->copy].next = prevCopy - e->copy; - } else { - firstCopy = e->copy; - } - prevCopy = e->copy; - e = e->next; - } while (e != firstEdge); - edges[firstCopy].next = prevCopy - firstCopy; - } - copied++; - } - - for (int32_t i = 0; i < copied; i++) { - btConvexHullInternal::Vertex *v = oldVertices[i]; - btConvexHullInternal::Edge *firstEdge = v->edges; - if (firstEdge) { - btConvexHullInternal::Edge *e = firstEdge; - do { - if (e->copy >= 0) { + } + if (prevCopy >= 0) { + edges[e->copy].next = prevCopy - e->copy; + } + else { + firstCopy = e->copy; + } + prevCopy = e->copy; + e = e->next; + } while (e != firstEdge); + edges[firstCopy].next = prevCopy - firstCopy; + } + copied++; + } + + for (int32_t i = 0; i < copied; i++) { + btConvexHullInternal::Vertex* v = oldVertices[i]; + btConvexHullInternal::Edge* firstEdge = v->edges; + if (firstEdge) { + btConvexHullInternal::Edge* e = firstEdge; + do { + if (e->copy >= 0) { #ifdef DEBUG_CONVEX_HULL - printf("Vertex *%d has edge to *%d\n", i, edges[e->copy].getTargetVertex()); + printf("Vertex *%d has edge to *%d\n", i, edges[e->copy].getTargetVertex()); #endif - faces.push_back(e->copy); - btConvexHullInternal::Edge *f = e; - do { + faces.push_back(e->copy); + btConvexHullInternal::Edge* f = e; + do { #ifdef DEBUG_CONVEX_HULL - printf(" Face *%d\n", edges[f->copy].getTargetVertex()); + printf(" Face *%d\n", edges[f->copy].getTargetVertex()); #endif - f->copy = -1; - f = f->reverse->prev; - } while (f != e); - } - e = e->next; - } while (e != firstEdge); - } - } - - return shift; + f->copy = -1; + f = f->reverse->prev; + } while (f != e); + } + e = e->next; + } while (e != firstEdge); + } + } + + return shift; } - -//GODOT ADDITION -}; -// |