Changeset View
Changeset View
Standalone View
Standalone View
extern/bullet2/src/LinearMath/btConvexHullComputer.cpp
| Show All 14 Lines | |||||
| #include <string.h> | #include <string.h> | ||||
| #include "btConvexHullComputer.h" | #include "btConvexHullComputer.h" | ||||
| #include "btAlignedObjectArray.h" | #include "btAlignedObjectArray.h" | ||||
| #include "btMinMax.h" | #include "btMinMax.h" | ||||
| #include "btVector3.h" | #include "btVector3.h" | ||||
| #ifdef __GNUC__ | #ifdef __GNUC__ | ||||
| #include <stdint.h> | #include <stdint.h> | ||||
| #elif defined(_MSC_VER) | #elif defined(_MSC_VER) | ||||
| typedef __int32 int32_t; | typedef __int32 int32_t; | ||||
| typedef __int64 int64_t; | typedef __int64 int64_t; | ||||
| typedef unsigned __int32 uint32_t; | typedef unsigned __int32 uint32_t; | ||||
| typedef unsigned __int64 uint64_t; | typedef unsigned __int64 uint64_t; | ||||
| #else | #else | ||||
| typedef int int32_t; | typedef int int32_t; | ||||
| typedef long long int int64_t; | typedef long long int int64_t; | ||||
| typedef unsigned int uint32_t; | typedef unsigned int uint32_t; | ||||
| typedef unsigned long long int uint64_t; | typedef unsigned long long int uint64_t; | ||||
| #endif | #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 | //The definition of USE_X86_64_ASM is moved into the build system. You can enable it manually by commenting out the following lines | ||||
| //#if (defined(__GNUC__) && defined(__x86_64__) && !defined(__ICL)) // || (defined(__ICL) && defined(_M_X64)) bug in Intel compiler, disable inline assembly | //#if (defined(__GNUC__) && defined(__x86_64__) && !defined(__ICL)) // || (defined(__ICL) && defined(_M_X64)) bug in Intel compiler, disable inline assembly | ||||
| // #define USE_X86_64_ASM | // #define USE_X86_64_ASM | ||||
| //#endif | //#endif | ||||
| //#define DEBUG_CONVEX_HULL | //#define DEBUG_CONVEX_HULL | ||||
| //#define SHOW_ITERATIONS | //#define SHOW_ITERATIONS | ||||
| #if defined(DEBUG_CONVEX_HULL) || defined(SHOW_ITERATIONS) | #if defined(DEBUG_CONVEX_HULL) || defined(SHOW_ITERATIONS) | ||||
| #include <stdio.h> | #include <stdio.h> | ||||
| #endif | #endif | ||||
| // Convex hull implementation based on Preparata and Hong | // Convex hull implementation based on Preparata and Hong | ||||
| // Ole Kniemeyer, MAXON Computer GmbH | // Ole Kniemeyer, MAXON Computer GmbH | ||||
| class btConvexHullInternal | class btConvexHullInternal | ||||
| { | { | ||||
| public: | public: | ||||
| class Point64 | class Point64 | ||||
| { | { | ||||
| public: | public: | ||||
| int64_t x; | int64_t x; | ||||
| int64_t y; | int64_t y; | ||||
| int64_t z; | int64_t z; | ||||
| Point64(int64_t x, int64_t y, int64_t z): x(x), y(y), z(z) | Point64(int64_t x, int64_t y, int64_t z) : x(x), y(y), z(z) | ||||
| { | { | ||||
| } | } | ||||
| bool isZero() | bool isZero() | ||||
| { | { | ||||
| return (x == 0) && (y == 0) && (z == 0); | return (x == 0) && (y == 0) && (z == 0); | ||||
| } | } | ||||
| int64_t dot(const Point64& b) const | int64_t dot(const Point64& b) const | ||||
| { | { | ||||
| return x * b.x + y * b.y + z * b.z; | return x * b.x + y * b.y + z * b.z; | ||||
| } | } | ||||
| }; | }; | ||||
| class Point32 | class Point32 | ||||
| { | { | ||||
| public: | public: | ||||
| int32_t x; | int32_t x; | ||||
| int32_t y; | int32_t y; | ||||
| int32_t z; | int32_t z; | ||||
| int index; | int index; | ||||
| Point32() | Point32() | ||||
| { | { | ||||
| } | } | ||||
| Point32(int32_t x, int32_t y, int32_t z): x(x), y(y), z(z), index(-1) | Point32(int32_t x, int32_t y, int32_t z) : x(x), y(y), z(z), index(-1) | ||||
| { | { | ||||
| } | } | ||||
| bool operator==(const Point32& b) const | bool operator==(const Point32& b) const | ||||
| { | { | ||||
| return (x == b.x) && (y == b.y) && (z == b.z); | return (x == b.x) && (y == b.y) && (z == b.z); | ||||
| } | } | ||||
| bool operator!=(const Point32& b) const | bool operator!=(const Point32& b) const | ||||
| { | { | ||||
| return (x != b.x) || (y != b.y) || (z != b.z); | return (x != b.x) || (y != b.y) || (z != b.z); | ||||
| } | } | ||||
| bool isZero() | bool isZero() | ||||
| { | { | ||||
| return (x == 0) && (y == 0) && (z == 0); | return (x == 0) && (y == 0) && (z == 0); | ||||
| } | } | ||||
| Point64 cross(const Point32& b) const | 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); | 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 | 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); | 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 | int64_t dot(const Point32& b) const | ||||
| { | { | ||||
| return x * b.x + y * b.y + z * b.z; | return x * b.x + y * b.y + z * b.z; | ||||
| } | } | ||||
| int64_t dot(const Point64& b) const | int64_t dot(const Point64& b) const | ||||
| { | { | ||||
| return x * b.x + y * b.y + z * b.z; | return x * b.x + y * b.y + z * b.z; | ||||
| } | } | ||||
| Point32 operator+(const Point32& b) const | Point32 operator+(const Point32& b) const | ||||
| { | { | ||||
| return Point32(x + b.x, y + b.y, z + b.z); | return Point32(x + b.x, y + b.y, z + b.z); | ||||
| } | } | ||||
| Point32 operator-(const Point32& b) const | Point32 operator-(const Point32& b) const | ||||
| { | { | ||||
| return Point32(x - b.x, y - b.y, z - b.z); | return Point32(x - b.x, y - b.y, z - b.z); | ||||
| } | } | ||||
| }; | }; | ||||
| class Int128 | class Int128 | ||||
| { | { | ||||
| public: | public: | ||||
| uint64_t low; | uint64_t low; | ||||
| uint64_t high; | uint64_t high; | ||||
| Int128() | Int128() | ||||
| { | { | ||||
| } | } | ||||
| Int128(uint64_t low, uint64_t high): low(low), high(high) | Int128(uint64_t low, uint64_t high) : low(low), high(high) | ||||
| { | { | ||||
| } | } | ||||
| Int128(uint64_t low): low(low), high(0) | Int128(uint64_t low) : low(low), high(0) | ||||
| { | { | ||||
| } | } | ||||
| Int128(int64_t value): low(value), high((value >= 0) ? 0 : (uint64_t) -1LL) | 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(int64_t a, int64_t b); | ||||
| static Int128 mul(uint64_t a, uint64_t b); | static Int128 mul(uint64_t a, uint64_t b); | ||||
| Int128 operator-() const | Int128 operator-() const | ||||
| { | { | ||||
| return Int128((uint64_t) -(int64_t)low, ~high + (low == 0)); | return Int128((uint64_t) - (int64_t)low, ~high + (low == 0)); | ||||
| } | } | ||||
| Int128 operator+(const Int128& b) const | Int128 operator+(const Int128& b) const | ||||
| { | { | ||||
| #ifdef USE_X86_64_ASM | #ifdef USE_X86_64_ASM | ||||
| Int128 result; | Int128 result; | ||||
| __asm__ ("addq %[bl], %[rl]\n\t" | __asm__( | ||||
| "addq %[bl], %[rl]\n\t" | |||||
| "adcq %[bh], %[rh]\n\t" | "adcq %[bh], %[rh]\n\t" | ||||
| : [rl] "=r" (result.low), [rh] "=r" (result.high) | : [rl] "=r"(result.low), [rh] "=r"(result.high) | ||||
| : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high) | : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high) | ||||
| : "cc" ); | : "cc"); | ||||
| return result; | return result; | ||||
| #else | #else | ||||
| uint64_t lo = low + b.low; | uint64_t lo = low + b.low; | ||||
| return Int128(lo, high + b.high + (lo < low)); | return Int128(lo, high + b.high + (lo < low)); | ||||
| #endif | #endif | ||||
| } | } | ||||
| Int128 operator-(const Int128& b) const | Int128 operator-(const Int128& b) const | ||||
| { | { | ||||
| #ifdef USE_X86_64_ASM | #ifdef USE_X86_64_ASM | ||||
| Int128 result; | Int128 result; | ||||
| __asm__ ("subq %[bl], %[rl]\n\t" | __asm__( | ||||
| "subq %[bl], %[rl]\n\t" | |||||
| "sbbq %[bh], %[rh]\n\t" | "sbbq %[bh], %[rh]\n\t" | ||||
| : [rl] "=r" (result.low), [rh] "=r" (result.high) | : [rl] "=r"(result.low), [rh] "=r"(result.high) | ||||
| : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high) | : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high) | ||||
| : "cc" ); | : "cc"); | ||||
| return result; | return result; | ||||
| #else | #else | ||||
| return *this + -b; | return *this + -b; | ||||
| #endif | #endif | ||||
| } | } | ||||
| Int128& operator+=(const Int128& b) | Int128& operator+=(const Int128& b) | ||||
| { | { | ||||
| #ifdef USE_X86_64_ASM | #ifdef USE_X86_64_ASM | ||||
| __asm__ ("addq %[bl], %[rl]\n\t" | __asm__( | ||||
| "addq %[bl], %[rl]\n\t" | |||||
| "adcq %[bh], %[rh]\n\t" | "adcq %[bh], %[rh]\n\t" | ||||
| : [rl] "=r" (low), [rh] "=r" (high) | : [rl] "=r"(low), [rh] "=r"(high) | ||||
| : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high) | : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high) | ||||
| : "cc" ); | : "cc"); | ||||
| #else | #else | ||||
| uint64_t lo = low + b.low; | uint64_t lo = low + b.low; | ||||
| if (lo < low) | if (lo < low) | ||||
| { | { | ||||
| ++high; | ++high; | ||||
| } | } | ||||
| low = lo; | low = lo; | ||||
| high += b.high; | high += b.high; | ||||
| #endif | #endif | ||||
| return *this; | return *this; | ||||
| } | } | ||||
| Int128& operator++() | Int128& operator++() | ||||
| { | { | ||||
| if (++low == 0) | if (++low == 0) | ||||
| { | { | ||||
| ++high; | ++high; | ||||
| } | } | ||||
| return *this; | return *this; | ||||
| } | } | ||||
| Int128 operator*(int64_t b) const; | Int128 operator*(int64_t b) const; | ||||
| btScalar toScalar() const | btScalar toScalar() const | ||||
| { | { | ||||
| return ((int64_t) high >= 0) ? btScalar(high) * (btScalar(0x100000000LL) * btScalar(0x100000000LL)) + btScalar(low) | return ((int64_t)high >= 0) ? btScalar(high) * (btScalar(0x100000000LL) * btScalar(0x100000000LL)) + btScalar(low) | ||||
| : -(-*this).toScalar(); | : -(-*this).toScalar(); | ||||
| } | } | ||||
| int getSign() const | int getSign() const | ||||
| { | { | ||||
| return ((int64_t) high < 0) ? -1 : (high || low) ? 1 : 0; | return ((int64_t)high < 0) ? -1 : (high || low) ? 1 : 0; | ||||
| } | } | ||||
| bool operator<(const Int128& b) const | bool operator<(const Int128& b) const | ||||
| { | { | ||||
| return (high < b.high) || ((high == b.high) && (low < b.low)); | return (high < b.high) || ((high == b.high) && (low < b.low)); | ||||
| } | } | ||||
| int ucmp(const Int128&b) const | int ucmp(const Int128& b) const | ||||
| { | { | ||||
| if (high < b.high) | if (high < b.high) | ||||
| { | { | ||||
| return -1; | return -1; | ||||
| } | } | ||||
| if (high > b.high) | if (high > b.high) | ||||
| { | { | ||||
| return 1; | return 1; | ||||
| } | } | ||||
| if (low < b.low) | if (low < b.low) | ||||
| { | { | ||||
| return -1; | return -1; | ||||
| } | } | ||||
| if (low > b.low) | if (low > b.low) | ||||
| { | { | ||||
| return 1; | return 1; | ||||
| } | } | ||||
| return 0; | return 0; | ||||
| } | } | ||||
| }; | }; | ||||
| class Rational64 | class Rational64 | ||||
| { | { | ||||
| private: | private: | ||||
| uint64_t m_numerator; | uint64_t m_numerator; | ||||
| uint64_t m_denominator; | uint64_t m_denominator; | ||||
| int sign; | int sign; | ||||
| public: | public: | ||||
| Rational64(int64_t numerator, int64_t denominator) | Rational64(int64_t numerator, int64_t denominator) | ||||
| { | { | ||||
| if (numerator > 0) | if (numerator > 0) | ||||
| { | { | ||||
| sign = 1; | sign = 1; | ||||
| m_numerator = (uint64_t) numerator; | m_numerator = (uint64_t)numerator; | ||||
| } | } | ||||
| else if (numerator < 0) | else if (numerator < 0) | ||||
| { | { | ||||
| sign = -1; | sign = -1; | ||||
| m_numerator = (uint64_t) -numerator; | m_numerator = (uint64_t)-numerator; | ||||
| } | } | ||||
| else | else | ||||
| { | { | ||||
| sign = 0; | sign = 0; | ||||
| m_numerator = 0; | m_numerator = 0; | ||||
| } | } | ||||
| if (denominator > 0) | if (denominator > 0) | ||||
| { | { | ||||
| m_denominator = (uint64_t) denominator; | m_denominator = (uint64_t)denominator; | ||||
| } | } | ||||
| else if (denominator < 0) | else if (denominator < 0) | ||||
| { | { | ||||
| sign = -sign; | sign = -sign; | ||||
| m_denominator = (uint64_t) -denominator; | m_denominator = (uint64_t)-denominator; | ||||
| } | } | ||||
| else | else | ||||
| { | { | ||||
| m_denominator = 0; | m_denominator = 0; | ||||
| } | } | ||||
| } | } | ||||
| bool isNegativeInfinity() const | bool isNegativeInfinity() const | ||||
| { | { | ||||
| return (sign < 0) && (m_denominator == 0); | return (sign < 0) && (m_denominator == 0); | ||||
| } | } | ||||
| bool isNaN() const | bool isNaN() const | ||||
| { | { | ||||
| return (sign == 0) && (m_denominator == 0); | return (sign == 0) && (m_denominator == 0); | ||||
| } | } | ||||
| int compare(const Rational64& b) const; | int compare(const Rational64& b) const; | ||||
| btScalar toScalar() const | btScalar toScalar() const | ||||
| { | { | ||||
| return sign * ((m_denominator == 0) ? SIMD_INFINITY : (btScalar) m_numerator / m_denominator); | return sign * ((m_denominator == 0) ? SIMD_INFINITY : (btScalar)m_numerator / m_denominator); | ||||
| } | } | ||||
| }; | }; | ||||
| class Rational128 | class Rational128 | ||||
| { | { | ||||
| private: | private: | ||||
| Int128 numerator; | Int128 numerator; | ||||
| Int128 denominator; | Int128 denominator; | ||||
| int sign; | int sign; | ||||
| bool isInt64; | bool isInt64; | ||||
| public: | public: | ||||
| Rational128(int64_t value) | Rational128(int64_t value) | ||||
| { | { | ||||
| if (value > 0) | if (value > 0) | ||||
| { | { | ||||
| sign = 1; | sign = 1; | ||||
| this->numerator = value; | this->numerator = value; | ||||
| } | } | ||||
| else if (value < 0) | else if (value < 0) | ||||
| { | { | ||||
| sign = -1; | sign = -1; | ||||
| this->numerator = -value; | this->numerator = -value; | ||||
| } | } | ||||
| else | else | ||||
| { | { | ||||
| sign = 0; | sign = 0; | ||||
| this->numerator = (uint64_t) 0; | this->numerator = (uint64_t)0; | ||||
| } | } | ||||
| this->denominator = (uint64_t) 1; | this->denominator = (uint64_t)1; | ||||
| isInt64 = true; | isInt64 = true; | ||||
| } | } | ||||
| Rational128(const Int128& numerator, const Int128& denominator) | Rational128(const Int128& numerator, const Int128& denominator) | ||||
| { | { | ||||
| sign = numerator.getSign(); | sign = numerator.getSign(); | ||||
| if (sign >= 0) | if (sign >= 0) | ||||
| { | { | ||||
| this->numerator = numerator; | this->numerator = numerator; | ||||
| } | } | ||||
| else | else | ||||
| { | { | ||||
| this->numerator = -numerator; | this->numerator = -numerator; | ||||
| } | } | ||||
| int dsign = denominator.getSign(); | int dsign = denominator.getSign(); | ||||
| if (dsign >= 0) | if (dsign >= 0) | ||||
| { | { | ||||
| this->denominator = denominator; | this->denominator = denominator; | ||||
| } | } | ||||
| else | else | ||||
| { | { | ||||
| sign = -sign; | sign = -sign; | ||||
| this->denominator = -denominator; | this->denominator = -denominator; | ||||
| } | } | ||||
| isInt64 = false; | isInt64 = false; | ||||
| } | } | ||||
| int compare(const Rational128& b) const; | int compare(const Rational128& b) const; | ||||
| int compare(int64_t b) const; | int compare(int64_t b) const; | ||||
| btScalar toScalar() const | btScalar toScalar() const | ||||
| { | { | ||||
| return sign * ((denominator.getSign() == 0) ? SIMD_INFINITY : numerator.toScalar() / denominator.toScalar()); | return sign * ((denominator.getSign() == 0) ? SIMD_INFINITY : numerator.toScalar() / denominator.toScalar()); | ||||
| } | } | ||||
| }; | }; | ||||
| class PointR128 | class PointR128 | ||||
| { | { | ||||
| public: | public: | ||||
| Int128 x; | Int128 x; | ||||
| Int128 y; | Int128 y; | ||||
| Int128 z; | Int128 z; | ||||
| Int128 denominator; | Int128 denominator; | ||||
| PointR128() | PointR128() | ||||
| { | { | ||||
| } | } | ||||
| PointR128(Int128 x, Int128 y, Int128 z, Int128 denominator): x(x), y(y), z(z), denominator(denominator) | PointR128(Int128 x, Int128 y, Int128 z, Int128 denominator) : x(x), y(y), z(z), denominator(denominator) | ||||
| { | { | ||||
| } | } | ||||
| btScalar xvalue() const | btScalar xvalue() const | ||||
| { | { | ||||
| return x.toScalar() / denominator.toScalar(); | return x.toScalar() / denominator.toScalar(); | ||||
| } | } | ||||
| btScalar yvalue() const | btScalar yvalue() const | ||||
| { | { | ||||
| return y.toScalar() / denominator.toScalar(); | return y.toScalar() / denominator.toScalar(); | ||||
| } | } | ||||
| btScalar zvalue() const | btScalar zvalue() const | ||||
| { | { | ||||
| return z.toScalar() / denominator.toScalar(); | return z.toScalar() / denominator.toScalar(); | ||||
| } | } | ||||
| }; | }; | ||||
| class Edge; | class Edge; | ||||
| class Face; | class Face; | ||||
| class Vertex | class Vertex | ||||
| { | { | ||||
| public: | public: | ||||
| Vertex* next; | Vertex* next; | ||||
| Vertex* prev; | Vertex* prev; | ||||
| Edge* edges; | Edge* edges; | ||||
| Face* firstNearbyFace; | Face* firstNearbyFace; | ||||
| Face* lastNearbyFace; | Face* lastNearbyFace; | ||||
| PointR128 point128; | PointR128 point128; | ||||
| Point32 point; | Point32 point; | ||||
| int copy; | int copy; | ||||
| Vertex(): next(NULL), prev(NULL), edges(NULL), firstNearbyFace(NULL), lastNearbyFace(NULL), copy(-1) | Vertex() : next(NULL), prev(NULL), edges(NULL), firstNearbyFace(NULL), lastNearbyFace(NULL), copy(-1) | ||||
| { | { | ||||
| } | } | ||||
| #ifdef DEBUG_CONVEX_HULL | #ifdef DEBUG_CONVEX_HULL | ||||
| void print() | void print() | ||||
| { | { | ||||
| printf("V%d (%d, %d, %d)", point.index, point.x, point.y, point.z); | printf("V%d (%d, %d, %d)", point.index, point.x, point.y, point.z); | ||||
| } | } | ||||
| void printGraph(); | void printGraph(); | ||||
| #endif | #endif | ||||
| Point32 operator-(const Vertex& b) const | Point32 operator-(const Vertex& b) const | ||||
| { | { | ||||
| return point - b.point; | return point - b.point; | ||||
| } | } | ||||
| Rational128 dot(const Point64& b) const | Rational128 dot(const Point64& b) const | ||||
| { | { | ||||
| return (point.index >= 0) ? Rational128(point.dot(b)) | return (point.index >= 0) ? Rational128(point.dot(b)) | ||||
| : Rational128(point128.x * b.x + point128.y * b.y + point128.z * b.z, point128.denominator); | : Rational128(point128.x * b.x + point128.y * b.y + point128.z * b.z, point128.denominator); | ||||
| } | } | ||||
| btScalar xvalue() const | btScalar xvalue() const | ||||
| { | { | ||||
| return (point.index >= 0) ? btScalar(point.x) : point128.xvalue(); | return (point.index >= 0) ? btScalar(point.x) : point128.xvalue(); | ||||
| } | } | ||||
| btScalar yvalue() const | btScalar yvalue() const | ||||
| { | { | ||||
| return (point.index >= 0) ? btScalar(point.y) : point128.yvalue(); | return (point.index >= 0) ? btScalar(point.y) : point128.yvalue(); | ||||
| } | } | ||||
| btScalar zvalue() const | btScalar zvalue() const | ||||
| { | { | ||||
| return (point.index >= 0) ? btScalar(point.z) : point128.zvalue(); | return (point.index >= 0) ? btScalar(point.z) : point128.zvalue(); | ||||
| } | } | ||||
| void receiveNearbyFaces(Vertex* src) | void receiveNearbyFaces(Vertex* src) | ||||
| { | { | ||||
| if (lastNearbyFace) | if (lastNearbyFace) | ||||
| { | { | ||||
| lastNearbyFace->nextWithSameNearbyVertex = src->firstNearbyFace; | lastNearbyFace->nextWithSameNearbyVertex = src->firstNearbyFace; | ||||
| } | } | ||||
| else | else | ||||
| { | { | ||||
| firstNearbyFace = src->firstNearbyFace; | firstNearbyFace = src->firstNearbyFace; | ||||
| } | } | ||||
| if (src->lastNearbyFace) | if (src->lastNearbyFace) | ||||
| { | { | ||||
| lastNearbyFace = src->lastNearbyFace; | lastNearbyFace = src->lastNearbyFace; | ||||
| } | } | ||||
| for (Face* f = src->firstNearbyFace; f; f = f->nextWithSameNearbyVertex) | for (Face* f = src->firstNearbyFace; f; f = f->nextWithSameNearbyVertex) | ||||
| { | { | ||||
| btAssert(f->nearbyVertex == src); | btAssert(f->nearbyVertex == src); | ||||
| f->nearbyVertex = this; | f->nearbyVertex = this; | ||||
| } | } | ||||
| src->firstNearbyFace = NULL; | src->firstNearbyFace = NULL; | ||||
| src->lastNearbyFace = NULL; | src->lastNearbyFace = NULL; | ||||
| } | } | ||||
| }; | }; | ||||
| class Edge | class Edge | ||||
| { | { | ||||
| public: | public: | ||||
| Edge* next; | Edge* next; | ||||
| Edge* prev; | Edge* prev; | ||||
| Edge* reverse; | Edge* reverse; | ||||
| Vertex* target; | Vertex* target; | ||||
| Face* face; | Face* face; | ||||
| int copy; | int copy; | ||||
| ~Edge() | ~Edge() | ||||
| { | { | ||||
| next = NULL; | next = NULL; | ||||
| prev = NULL; | prev = NULL; | ||||
| reverse = NULL; | reverse = NULL; | ||||
| target = NULL; | target = NULL; | ||||
| face = NULL; | face = NULL; | ||||
| } | } | ||||
| void link(Edge* n) | void link(Edge* n) | ||||
| { | { | ||||
| btAssert(reverse->target == n->reverse->target); | btAssert(reverse->target == n->reverse->target); | ||||
| next = n; | next = n; | ||||
| n->prev = this; | n->prev = this; | ||||
| } | } | ||||
| #ifdef DEBUG_CONVEX_HULL | #ifdef DEBUG_CONVEX_HULL | ||||
| void print() | 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, | 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); | reverse->target->point.x, reverse->target->point.y, reverse->target->point.z, target->point.x, target->point.y, target->point.z); | ||||
| } | } | ||||
| #endif | #endif | ||||
| }; | }; | ||||
| class Face | class Face | ||||
| { | { | ||||
| public: | public: | ||||
| Face* next; | Face* next; | ||||
| Vertex* nearbyVertex; | Vertex* nearbyVertex; | ||||
| Face* nextWithSameNearbyVertex; | Face* nextWithSameNearbyVertex; | ||||
| Point32 origin; | Point32 origin; | ||||
| Point32 dir0; | Point32 dir0; | ||||
| Point32 dir1; | Point32 dir1; | ||||
| Face(): next(NULL), nearbyVertex(NULL), nextWithSameNearbyVertex(NULL) | Face() : next(NULL), nearbyVertex(NULL), nextWithSameNearbyVertex(NULL) | ||||
| { | { | ||||
| } | } | ||||
| void init(Vertex* a, Vertex* b, Vertex* c) | void init(Vertex* a, Vertex* b, Vertex* c) | ||||
| { | { | ||||
| nearbyVertex = a; | nearbyVertex = a; | ||||
| origin = a->point; | origin = a->point; | ||||
| dir0 = *b - *a; | dir0 = *b - *a; | ||||
| dir1 = *c - *a; | dir1 = *c - *a; | ||||
| if (a->lastNearbyFace) | if (a->lastNearbyFace) | ||||
| { | { | ||||
| a->lastNearbyFace->nextWithSameNearbyVertex = this; | a->lastNearbyFace->nextWithSameNearbyVertex = this; | ||||
| } | } | ||||
| else | else | ||||
| { | { | ||||
| a->firstNearbyFace = this; | a->firstNearbyFace = this; | ||||
| } | } | ||||
| a->lastNearbyFace = this; | a->lastNearbyFace = this; | ||||
| } | } | ||||
| Point64 getNormal() | Point64 getNormal() | ||||
| { | { | ||||
| return dir0.cross(dir1); | return dir0.cross(dir1); | ||||
| } | } | ||||
| }; | }; | ||||
| template<typename UWord, typename UHWord> class DMul | template <typename UWord, typename UHWord> | ||||
| class DMul | |||||
| { | { | ||||
| private: | private: | ||||
| static uint32_t high(uint64_t value) | static uint32_t high(uint64_t value) | ||||
| { | { | ||||
| return (uint32_t) (value >> 32); | return (uint32_t)(value >> 32); | ||||
| } | } | ||||
| static uint32_t low(uint64_t value) | static uint32_t low(uint64_t value) | ||||
| { | { | ||||
| return (uint32_t) value; | return (uint32_t)value; | ||||
| } | } | ||||
| static uint64_t mul(uint32_t a, uint32_t b) | static uint64_t mul(uint32_t a, uint32_t b) | ||||
| { | { | ||||
| return (uint64_t) a * (uint64_t) b; | return (uint64_t)a * (uint64_t)b; | ||||
| } | } | ||||
| static void shlHalf(uint64_t& value) | static void shlHalf(uint64_t& value) | ||||
| { | { | ||||
| value <<= 32; | value <<= 32; | ||||
| } | } | ||||
| static uint64_t high(Int128 value) | static uint64_t high(Int128 value) | ||||
| { | { | ||||
| return value.high; | return value.high; | ||||
| } | } | ||||
| static uint64_t low(Int128 value) | static uint64_t low(Int128 value) | ||||
| { | { | ||||
| return value.low; | return value.low; | ||||
| } | } | ||||
| static Int128 mul(uint64_t a, uint64_t b) | static Int128 mul(uint64_t a, uint64_t b) | ||||
| { | { | ||||
| return Int128::mul(a, b); | return Int128::mul(a, b); | ||||
| } | } | ||||
| static void shlHalf(Int128& value) | static void shlHalf(Int128& value) | ||||
| { | { | ||||
| value.high = value.low; | value.high = value.low; | ||||
| value.low = 0; | value.low = 0; | ||||
| } | } | ||||
| public: | public: | ||||
| static void mul(UWord a, UWord b, UWord& resLow, UWord& resHigh) | static void mul(UWord a, UWord b, UWord& resLow, UWord& resHigh) | ||||
| { | { | ||||
| UWord p00 = mul(low(a), low(b)); | UWord p00 = mul(low(a), low(b)); | ||||
| UWord p01 = mul(low(a), high(b)); | UWord p01 = mul(low(a), high(b)); | ||||
| UWord p10 = mul(high(a), low(b)); | UWord p10 = mul(high(a), low(b)); | ||||
| UWord p11 = mul(high(a), high(b)); | UWord p11 = mul(high(a), high(b)); | ||||
| UWord p0110 = UWord(low(p01)) + UWord(low(p10)); | UWord p0110 = UWord(low(p01)) + UWord(low(p10)); | ||||
| p11 += high(p01); | p11 += high(p01); | ||||
| p11 += high(p10); | p11 += high(p10); | ||||
| p11 += high(p0110); | p11 += high(p0110); | ||||
| shlHalf(p0110); | shlHalf(p0110); | ||||
| p00 += p0110; | p00 += p0110; | ||||
| if (p00 < p0110) | if (p00 < p0110) | ||||
| { | { | ||||
| ++p11; | ++p11; | ||||
| } | } | ||||
| resLow = p00; | resLow = p00; | ||||
| resHigh = p11; | resHigh = p11; | ||||
| } | } | ||||
| }; | }; | ||||
| private: | private: | ||||
| class IntermediateHull | class IntermediateHull | ||||
| { | { | ||||
| public: | public: | ||||
| Vertex* minXy; | Vertex* minXy; | ||||
| Vertex* maxXy; | Vertex* maxXy; | ||||
| Vertex* minYx; | Vertex* minYx; | ||||
| Vertex* maxYx; | Vertex* maxYx; | ||||
| IntermediateHull(): minXy(NULL), maxXy(NULL), minYx(NULL), maxYx(NULL) | IntermediateHull() : minXy(NULL), maxXy(NULL), minYx(NULL), maxYx(NULL) | ||||
| { | { | ||||
| } | } | ||||
| void print(); | void print(); | ||||
| }; | }; | ||||
| enum Orientation {NONE, CLOCKWISE, COUNTER_CLOCKWISE}; | enum Orientation | ||||
| { | |||||
| NONE, | |||||
| CLOCKWISE, | |||||
| COUNTER_CLOCKWISE | |||||
| }; | |||||
| template <typename T> class PoolArray | template <typename T> | ||||
| class PoolArray | |||||
| { | { | ||||
| private: | private: | ||||
| T* array; | T* array; | ||||
| int size; | int size; | ||||
| public: | public: | ||||
| PoolArray<T>* next; | PoolArray<T>* next; | ||||
| PoolArray(int size): size(size), next(NULL) | PoolArray(int size) : size(size), next(NULL) | ||||
| { | { | ||||
| array = (T*) btAlignedAlloc(sizeof(T) * size, 16); | array = (T*)btAlignedAlloc(sizeof(T) * size, 16); | ||||
| } | } | ||||
| ~PoolArray() | ~PoolArray() | ||||
| { | { | ||||
| btAlignedFree(array); | btAlignedFree(array); | ||||
| } | } | ||||
| T* init() | T* init() | ||||
| { | { | ||||
| T* o = array; | T* o = array; | ||||
| for (int i = 0; i < size; i++, o++) | for (int i = 0; i < size; i++, o++) | ||||
| { | { | ||||
| o->next = (i+1 < size) ? o + 1 : NULL; | o->next = (i + 1 < size) ? o + 1 : NULL; | ||||
| } | } | ||||
| return array; | return array; | ||||
| } | } | ||||
| }; | }; | ||||
| template <typename T> class Pool | template <typename T> | ||||
| class Pool | |||||
| { | { | ||||
| private: | private: | ||||
| PoolArray<T>* arrays; | PoolArray<T>* arrays; | ||||
| PoolArray<T>* nextArray; | PoolArray<T>* nextArray; | ||||
| T* freeObjects; | T* freeObjects; | ||||
| int arraySize; | int arraySize; | ||||
| public: | public: | ||||
| Pool(): arrays(NULL), nextArray(NULL), freeObjects(NULL), arraySize(256) | Pool() : arrays(NULL), nextArray(NULL), freeObjects(NULL), arraySize(256) | ||||
| { | { | ||||
| } | } | ||||
| ~Pool() | ~Pool() | ||||
| { | { | ||||
| while (arrays) | while (arrays) | ||||
| { | { | ||||
| PoolArray<T>* p = arrays; | PoolArray<T>* p = arrays; | ||||
| arrays = p->next; | arrays = p->next; | ||||
| p->~PoolArray<T>(); | p->~PoolArray<T>(); | ||||
| btAlignedFree(p); | btAlignedFree(p); | ||||
| } | } | ||||
| } | } | ||||
| void reset() | void reset() | ||||
| { | { | ||||
| nextArray = arrays; | nextArray = arrays; | ||||
| freeObjects = NULL; | freeObjects = NULL; | ||||
| } | } | ||||
| void setArraySize(int arraySize) | void setArraySize(int arraySize) | ||||
| { | { | ||||
| this->arraySize = arraySize; | this->arraySize = arraySize; | ||||
| } | } | ||||
| T* newObject() | T* newObject() | ||||
| { | { | ||||
| T* o = freeObjects; | T* o = freeObjects; | ||||
| if (!o) | if (!o) | ||||
| { | { | ||||
| PoolArray<T>* p = nextArray; | PoolArray<T>* p = nextArray; | ||||
| if (p) | if (p) | ||||
| { | { | ||||
| nextArray = p->next; | nextArray = p->next; | ||||
| } | } | ||||
| else | else | ||||
| { | { | ||||
| p = new(btAlignedAlloc(sizeof(PoolArray<T>), 16)) PoolArray<T>(arraySize); | p = new (btAlignedAlloc(sizeof(PoolArray<T>), 16)) PoolArray<T>(arraySize); | ||||
| p->next = arrays; | p->next = arrays; | ||||
| arrays = p; | arrays = p; | ||||
| } | } | ||||
| o = p->init(); | o = p->init(); | ||||
| } | } | ||||
| freeObjects = o->next; | freeObjects = o->next; | ||||
| return new(o) T(); | return new (o) T(); | ||||
| }; | }; | ||||
| void freeObject(T* object) | void freeObject(T* object) | ||||
| { | { | ||||
| object->~T(); | object->~T(); | ||||
| object->next = freeObjects; | object->next = freeObjects; | ||||
| freeObjects = object; | freeObjects = object; | ||||
| } | } | ||||
| }; | }; | ||||
| btVector3 scaling; | btVector3 scaling; | ||||
| btVector3 center; | btVector3 center; | ||||
| Pool<Vertex> vertexPool; | Pool<Vertex> vertexPool; | ||||
| Pool<Edge> edgePool; | Pool<Edge> edgePool; | ||||
| Pool<Face> facePool; | Pool<Face> facePool; | ||||
| btAlignedObjectArray<Vertex*> originalVertices; | btAlignedObjectArray<Vertex*> originalVertices; | ||||
| int mergeStamp; | int mergeStamp; | ||||
| int minAxis; | int minAxis; | ||||
| int medAxis; | int medAxis; | ||||
| int maxAxis; | int maxAxis; | ||||
| int usedEdgePairs; | int usedEdgePairs; | ||||
| int maxUsedEdgePairs; | int maxUsedEdgePairs; | ||||
| static Orientation getOrientation(const Edge* prev, const Edge* next, const Point32& s, const Point32& t); | 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); | 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); | void findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge*& e0, Edge*& e1, Vertex* stop0, Vertex* stop1); | ||||
| Edge* newEdgePair(Vertex* from, Vertex* to); | Edge* newEdgePair(Vertex* from, Vertex* to); | ||||
| void removeEdgePair(Edge* edge) | void removeEdgePair(Edge* edge) | ||||
| { | { | ||||
| Edge* n = edge->next; | Edge* n = edge->next; | ||||
| Edge* r = edge->reverse; | Edge* r = edge->reverse; | ||||
| btAssert(edge->target && r->target); | btAssert(edge->target && r->target); | ||||
| if (n != edge) | if (n != edge) | ||||
| { | { | ||||
| n->prev = edge->prev; | n->prev = edge->prev; | ||||
| edge->prev->next = n; | edge->prev->next = n; | ||||
| r->target->edges = n; | r->target->edges = n; | ||||
| } | } | ||||
| else | else | ||||
| { | { | ||||
| r->target->edges = NULL; | r->target->edges = NULL; | ||||
| } | } | ||||
| n = r->next; | n = r->next; | ||||
| if (n != r) | if (n != r) | ||||
| { | { | ||||
| n->prev = r->prev; | n->prev = r->prev; | ||||
| r->prev->next = n; | r->prev->next = n; | ||||
| edge->target->edges = n; | edge->target->edges = n; | ||||
| } | } | ||||
| else | else | ||||
| { | { | ||||
| edge->target->edges = NULL; | edge->target->edges = NULL; | ||||
| } | } | ||||
| edgePool.freeObject(edge); | edgePool.freeObject(edge); | ||||
| edgePool.freeObject(r); | edgePool.freeObject(r); | ||||
| usedEdgePairs--; | usedEdgePairs--; | ||||
| } | } | ||||
| void computeInternal(int start, int end, IntermediateHull& result); | void computeInternal(int start, int end, IntermediateHull& result); | ||||
| bool mergeProjection(IntermediateHull& h0, IntermediateHull& h1, Vertex*& c0, Vertex*& c1); | bool mergeProjection(IntermediateHull& h0, IntermediateHull& h1, Vertex*& c0, Vertex*& c1); | ||||
| void merge(IntermediateHull& h0, IntermediateHull& h1); | void merge(IntermediateHull& h0, IntermediateHull& h1); | ||||
| btVector3 toBtVector(const Point32& v); | btVector3 toBtVector(const Point32& v); | ||||
| btVector3 getBtNormal(Face* face); | btVector3 getBtNormal(Face* face); | ||||
| bool shiftFace(Face* face, btScalar amount, btAlignedObjectArray<Vertex*> stack); | bool shiftFace(Face* face, btScalar amount, btAlignedObjectArray<Vertex*> stack); | ||||
| public: | public: | ||||
| Vertex* vertexList; | Vertex* vertexList; | ||||
| void compute(const void* coords, bool doubleCoords, int stride, int count); | void compute(const void* coords, bool doubleCoords, int stride, int 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 | btConvexHullInternal::Int128 btConvexHullInternal::Int128::operator*(int64_t b) const | ||||
| { | { | ||||
| bool negative = (int64_t) high < 0; | bool negative = (int64_t)high < 0; | ||||
| Int128 a = negative ? -*this : *this; | Int128 a = negative ? -*this : *this; | ||||
| if (b < 0) | if (b < 0) | ||||
| { | { | ||||
| negative = !negative; | negative = !negative; | ||||
| b = -b; | b = -b; | ||||
| } | } | ||||
| Int128 result = mul(a.low, (uint64_t) b); | Int128 result = mul(a.low, (uint64_t)b); | ||||
| result.high += a.high * (uint64_t) b; | result.high += a.high * (uint64_t)b; | ||||
| return negative ? -result : result; | return negative ? -result : result; | ||||
| } | } | ||||
| btConvexHullInternal::Int128 btConvexHullInternal::Int128::mul(int64_t a, int64_t b) | btConvexHullInternal::Int128 btConvexHullInternal::Int128::mul(int64_t a, int64_t b) | ||||
| { | { | ||||
| Int128 result; | Int128 result; | ||||
| #ifdef USE_X86_64_ASM | #ifdef USE_X86_64_ASM | ||||
| __asm__ ("imulq %[b]" | __asm__("imulq %[b]" | ||||
| : "=a" (result.low), "=d" (result.high) | : "=a"(result.low), "=d"(result.high) | ||||
| : "0"(a), [b] "r"(b) | : "0"(a), [b] "r"(b) | ||||
| : "cc" ); | : "cc"); | ||||
| return result; | return result; | ||||
| #else | #else | ||||
| bool negative = a < 0; | bool negative = a < 0; | ||||
| if (negative) | if (negative) | ||||
| { | { | ||||
| a = -a; | a = -a; | ||||
| } | } | ||||
| if (b < 0) | if (b < 0) | ||||
| { | { | ||||
| negative = !negative; | negative = !negative; | ||||
| b = -b; | b = -b; | ||||
| } | } | ||||
| DMul<uint64_t, uint32_t>::mul((uint64_t) a, (uint64_t) b, result.low, result.high); | DMul<uint64_t, uint32_t>::mul((uint64_t)a, (uint64_t)b, result.low, result.high); | ||||
| return negative ? -result : result; | return negative ? -result : result; | ||||
| #endif | #endif | ||||
| } | } | ||||
| btConvexHullInternal::Int128 btConvexHullInternal::Int128::mul(uint64_t a, uint64_t b) | btConvexHullInternal::Int128 btConvexHullInternal::Int128::mul(uint64_t a, uint64_t b) | ||||
| { | { | ||||
| Int128 result; | Int128 result; | ||||
| #ifdef USE_X86_64_ASM | #ifdef USE_X86_64_ASM | ||||
| __asm__ ("mulq %[b]" | __asm__("mulq %[b]" | ||||
| : "=a" (result.low), "=d" (result.high) | : "=a"(result.low), "=d"(result.high) | ||||
| : "0"(a), [b] "r"(b) | : "0"(a), [b] "r"(b) | ||||
| : "cc" ); | : "cc"); | ||||
| #else | #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 | #endif | ||||
| return result; | return result; | ||||
| } | } | ||||
| Show All 10 Lines | int btConvexHullInternal::Rational64::compare(const Rational64& b) const | ||||
| // 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 | #ifdef USE_X86_64_ASM | ||||
| int result; | int result; | ||||
| int64_t tmp; | int64_t tmp; | ||||
| int64_t dummy; | int64_t dummy; | ||||
| __asm__ ("mulq %[bn]\n\t" | __asm__( | ||||
| "mulq %[bn]\n\t" | |||||
| "movq %%rax, %[tmp]\n\t" | "movq %%rax, %[tmp]\n\t" | ||||
| "movq %%rdx, %%rbx\n\t" | "movq %%rdx, %%rbx\n\t" | ||||
| "movq %[tn], %%rax\n\t" | "movq %[tn], %%rax\n\t" | ||||
| "mulq %[bd]\n\t" | "mulq %[bd]\n\t" | ||||
| "subq %[tmp], %%rax\n\t" | "subq %[tmp], %%rax\n\t" | ||||
| "sbbq %%rbx, %%rdx\n\t" // rdx:rax contains 128-bit-difference "numerator*b.denominator - b.numerator*denominator" | "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 | "setnsb %%bh\n\t" // bh=1 if difference is non-negative, bh=0 otherwise | ||||
| "orq %%rdx, %%rax\n\t" | "orq %%rdx, %%rax\n\t" | ||||
| "setnzb %%bl\n\t" // bl=1 if difference if non-zero, bl=0 if it is zero | "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) | "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 | "shll $16, %%ebx\n\t" // ebx has same sign as difference | ||||
| : "=&b"(result), [tmp] "=&r"(tmp), "=a"(dummy) | : "=&b"(result), [tmp] "=&r"(tmp), "=a"(dummy) | ||||
| : "a"(denominator), [bn] "g"(b.numerator), [tn] "g"(numerator), [bd] "g"(b.denominator) | : "a"(m_denominator), [bn] "g"(b.m_numerator), [tn] "g"(m_numerator), [bd] "g"(b.m_denominator) | ||||
| : "%rdx", "cc" ); | : "%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) | 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) | // if sign is -1, all bits of result are inverted, which changes the sign of result (and again cannot result in zero) | ||||
| : 0; | : 0; | ||||
| #else | #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 | #endif | ||||
| } | } | ||||
| int btConvexHullInternal::Rational128::compare(const Rational128& b) const | int btConvexHullInternal::Rational128::compare(const Rational128& b) const | ||||
| { | { | ||||
| if (sign != b.sign) | if (sign != b.sign) | ||||
| { | { | ||||
| return sign - b.sign; | return sign - b.sign; | ||||
| } | } | ||||
| else if (sign == 0) | else if (sign == 0) | ||||
| { | { | ||||
| return 0; | return 0; | ||||
| } | } | ||||
| if (isInt64) | if (isInt64) | ||||
| { | { | ||||
| return -b.compare(sign * (int64_t) numerator.low); | return -b.compare(sign * (int64_t)numerator.low); | ||||
| } | } | ||||
| Int128 nbdLow, nbdHigh, dbnLow, dbnHigh; | Int128 nbdLow, nbdHigh, dbnLow, dbnHigh; | ||||
| DMul<Int128, uint64_t>::mul(numerator, b.denominator, nbdLow, nbdHigh); | DMul<Int128, uint64_t>::mul(numerator, b.denominator, nbdLow, nbdHigh); | ||||
| DMul<Int128, uint64_t>::mul(denominator, b.numerator, dbnLow, dbnHigh); | DMul<Int128, uint64_t>::mul(denominator, b.numerator, dbnLow, dbnHigh); | ||||
| int cmp = nbdHigh.ucmp(dbnHigh); | int cmp = nbdHigh.ucmp(dbnHigh); | ||||
| if (cmp) | if (cmp) | ||||
| { | { | ||||
| return cmp * sign; | return cmp * sign; | ||||
| } | } | ||||
| return nbdLow.ucmp(dbnLow) * sign; | return nbdLow.ucmp(dbnLow) * sign; | ||||
| } | } | ||||
| int btConvexHullInternal::Rational128::compare(int64_t b) const | int btConvexHullInternal::Rational128::compare(int64_t b) const | ||||
| { | { | ||||
| if (isInt64) | if (isInt64) | ||||
| { | { | ||||
| int64_t a = sign * (int64_t) numerator.low; | int64_t a = sign * (int64_t)numerator.low; | ||||
| return (a > b) ? 1 : (a < b) ? -1 : 0; | return (a > b) ? 1 : (a < b) ? -1 : 0; | ||||
| } | } | ||||
| if (b > 0) | if (b > 0) | ||||
| { | { | ||||
| if (sign <= 0) | if (sign <= 0) | ||||
| { | { | ||||
| return -1; | return -1; | ||||
| } | } | ||||
| Show All 9 Lines | int btConvexHullInternal::Rational128::compare(int64_t b) const | ||||
| else | else | ||||
| { | { | ||||
| return sign; | return sign; | ||||
| } | } | ||||
| return numerator.ucmp(denominator * b) * sign; | return numerator.ucmp(denominator * b) * sign; | ||||
| } | } | ||||
| btConvexHullInternal::Edge* btConvexHullInternal::newEdgePair(Vertex* from, Vertex* to) | btConvexHullInternal::Edge* btConvexHullInternal::newEdgePair(Vertex* from, Vertex* to) | ||||
| { | { | ||||
| btAssert(from && to); | btAssert(from && to); | ||||
| Edge* e = edgePool.newObject(); | Edge* e = edgePool.newObject(); | ||||
| Edge* r = edgePool.newObject(); | Edge* r = edgePool.newObject(); | ||||
| e->reverse = r; | e->reverse = r; | ||||
| r->reverse = e; | r->reverse = e; | ||||
| e->copy = mergeStamp; | e->copy = mergeStamp; | ||||
| ▲ Show 20 Lines • Show All 51 Lines • ▼ Show 20 Lines | if (v1 == h1.maxXy) | ||||
| h1.maxXy = v1n; | h1.maxXy = v1n; | ||||
| } | } | ||||
| else | else | ||||
| { | { | ||||
| h1.maxXy = v1p; | h1.maxXy = v1p; | ||||
| } | } | ||||
| } | } | ||||
| } | } | ||||
| v0 = h0.maxXy; | v0 = h0.maxXy; | ||||
| v1 = h1.maxXy; | v1 = h1.maxXy; | ||||
| Vertex* v00 = NULL; | Vertex* v00 = NULL; | ||||
| Vertex* v10 = NULL; | Vertex* v10 = NULL; | ||||
| int32_t sign = 1; | int32_t sign = 1; | ||||
| for (int side = 0; side <= 1; side++) | for (int side = 0; side <= 1; side++) | ||||
| { | { | ||||
| int32_t dx = (v1->point.x - v0->point.x) * sign; | int32_t dx = (v1->point.x - v0->point.x) * sign; | ||||
| if (dx > 0) | if (dx > 0) | ||||
| { | { | ||||
| while (true) | while (true) | ||||
| { | { | ||||
| int32_t dy = v1->point.y - v0->point.y; | int32_t dy = v1->point.y - v0->point.y; | ||||
| Vertex* w0 = side ? v0->next : v0->prev; | Vertex* w0 = side ? v0->next : v0->prev; | ||||
| Show All 26 Lines | if (dx > 0) | ||||
| break; | break; | ||||
| } | } | ||||
| } | } | ||||
| else if (dx < 0) | else if (dx < 0) | ||||
| { | { | ||||
| while (true) | while (true) | ||||
| { | { | ||||
| int32_t dy = v1->point.y - v0->point.y; | int32_t dy = v1->point.y - v0->point.y; | ||||
| Vertex* w1 = side ? v1->prev : v1->next; | Vertex* w1 = side ? v1->prev : v1->next; | ||||
| if (w1 != v1) | if (w1 != v1) | ||||
| { | { | ||||
| int32_t dx1 = (w1->point.x - v1->point.x) * sign; | int32_t dx1 = (w1->point.x - v1->point.x) * sign; | ||||
| int32_t dy1 = w1->point.y - v1->point.y; | int32_t dy1 = w1->point.y - v1->point.y; | ||||
| if ((dy1 >= 0) && ((dx1 == 0) || ((dx1 < 0) && (dy1 * dx <= dy * dx1)))) | if ((dy1 >= 0) && ((dx1 == 0) || ((dx1 < 0) && (dy1 * dx <= dy * dx1)))) | ||||
| { | { | ||||
| v1 = w1; | v1 = w1; | ||||
| dx = (v1->point.x - v0->point.x) * sign; | dx = (v1->point.x - v0->point.x) * sign; | ||||
| continue; | continue; | ||||
| } | } | ||||
| } | } | ||||
| Vertex* w0 = side ? v0->prev : v0->next; | Vertex* w0 = side ? v0->prev : v0->next; | ||||
| if (w0 != v0) | if (w0 != v0) | ||||
| { | { | ||||
| int32_t dx0 = (w0->point.x - v0->point.x) * sign; | int32_t dx0 = (w0->point.x - v0->point.x) * sign; | ||||
| int32_t dy0 = w0->point.y - v0->point.y; | int32_t dy0 = w0->point.y - v0->point.y; | ||||
| int32_t dxn = (v1->point.x - w0->point.x) * sign; | int32_t dxn = (v1->point.x - w0->point.x) * sign; | ||||
| if ((dxn < 0) && (dy0 > 0) && ((dx0 == 0) || ((dx0 < 0) && (dy0 * dx < dy * dx0)))) | if ((dxn < 0) && (dy0 > 0) && ((dx0 == 0) || ((dx0 < 0) && (dy0 * dx < dy * dx0)))) | ||||
| { | { | ||||
| v0 = w0; | v0 = w0; | ||||
| dx = dxn; | dx = dxn; | ||||
| continue; | continue; | ||||
| } | } | ||||
| } | } | ||||
| break; | break; | ||||
| } | } | ||||
| } | } | ||||
| else | else | ||||
| { | { | ||||
| int32_t x = v0->point.x; | int32_t x = v0->point.x; | ||||
| int32_t y0 = v0->point.y; | int32_t y0 = v0->point.y; | ||||
| Vertex* w0 = v0; | Vertex* w0 = v0; | ||||
| Show All 9 Lines | else | ||||
| Vertex* w1 = v1; | Vertex* w1 = v1; | ||||
| while (((t = side ? w1->prev : w1->next) != v1) && (t->point.x == x) && (t->point.y >= y1)) | while (((t = side ? w1->prev : w1->next) != v1) && (t->point.x == x) && (t->point.y >= y1)) | ||||
| { | { | ||||
| w1 = t; | w1 = t; | ||||
| y1 = t->point.y; | y1 = t->point.y; | ||||
| } | } | ||||
| v1 = w1; | v1 = w1; | ||||
| } | } | ||||
| if (side == 0) | if (side == 0) | ||||
| { | { | ||||
| v00 = v0; | v00 = v0; | ||||
| v10 = v1; | v10 = v1; | ||||
| v0 = h0.minXy; | v0 = h0.minXy; | ||||
| v1 = h1.minXy; | v1 = h1.minXy; | ||||
| sign = -1; | sign = -1; | ||||
| Show All 9 Lines | bool btConvexHullInternal::mergeProjection(IntermediateHull& h0, IntermediateHull& h1, Vertex*& c0, Vertex*& c1) | ||||
| if (h1.minXy->point.x < h0.minXy->point.x) | if (h1.minXy->point.x < h0.minXy->point.x) | ||||
| { | { | ||||
| h0.minXy = h1.minXy; | h0.minXy = h1.minXy; | ||||
| } | } | ||||
| if (h1.maxXy->point.x >= h0.maxXy->point.x) | if (h1.maxXy->point.x >= h0.maxXy->point.x) | ||||
| { | { | ||||
| h0.maxXy = h1.maxXy; | h0.maxXy = h1.maxXy; | ||||
| } | } | ||||
| h0.maxYx = h1.maxYx; | h0.maxYx = h1.maxYx; | ||||
| c0 = v00; | c0 = v00; | ||||
| c1 = v10; | c1 = v10; | ||||
| return true; | return true; | ||||
| } | } | ||||
| ▲ Show 20 Lines • Show All 68 Lines • ▼ Show 20 Lines | case 2: | ||||
| v->edges = e; | v->edges = e; | ||||
| e = e->reverse; | e = e->reverse; | ||||
| e->link(e); | e->link(e); | ||||
| w->edges = e; | w->edges = e; | ||||
| return; | return; | ||||
| } | } | ||||
| { | |||||
| 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; | |||||
| } | } | ||||
| // lint -fallthrough | |||||
| return; | |||||
| } | |||||
| case 1: | case 1: | ||||
| { | { | ||||
| Vertex* v = originalVertices[start]; | Vertex* v = originalVertices[start]; | ||||
| v->edges = NULL; | v->edges = NULL; | ||||
| v->next = v; | v->next = v; | ||||
| v->prev = v; | v->prev = v; | ||||
| result.minXy = v; | result.minXy = v; | ||||
| result.maxXy = v; | result.maxXy = v; | ||||
| result.minYx = v; | result.minYx = v; | ||||
| result.maxYx = v; | result.maxYx = v; | ||||
| return; | return; | ||||
| } | } | ||||
| } | } | ||||
| int split0 = start + n / 2; | int split0 = start + n / 2; | ||||
| Point32 p = originalVertices[split0-1]->point; | Point32 p = originalVertices[split0 - 1]->point; | ||||
| int split1 = split0; | int split1 = split0; | ||||
| while ((split1 < end) && (originalVertices[split1]->point == p)) | while ((split1 < end) && (originalVertices[split1]->point == p)) | ||||
| { | { | ||||
| split1++; | split1++; | ||||
| } | } | ||||
| computeInternal(start, split0, result); | computeInternal(start, split0, result); | ||||
| IntermediateHull hull1; | IntermediateHull hull1; | ||||
| computeInternal(split1, end, hull1); | computeInternal(split1, end, hull1); | ||||
| #ifdef DEBUG_CONVEX_HULL | #ifdef DEBUG_CONVEX_HULL | ||||
| printf("\n\nMerge\n"); | printf("\n\nMerge\n"); | ||||
| result.print(); | result.print(); | ||||
| hull1.print(); | hull1.print(); | ||||
| #endif | #endif | ||||
| merge(result, hull1); | merge(result, hull1); | ||||
| #ifdef DEBUG_CONVEX_HULL | #ifdef DEBUG_CONVEX_HULL | ||||
| printf("\n Result\n"); | printf("\n Result\n"); | ||||
| result.print(); | result.print(); | ||||
| #endif | #endif | ||||
| } | } | ||||
| #ifdef DEBUG_CONVEX_HULL | #ifdef DEBUG_CONVEX_HULL | ||||
| void btConvexHullInternal::IntermediateHull::print() | void btConvexHullInternal::IntermediateHull::print() | ||||
| { | { | ||||
| printf(" Hull\n"); | printf(" Hull\n"); | ||||
| for (Vertex* v = minXy; v; ) | for (Vertex* v = minXy; v;) | ||||
| { | { | ||||
| printf(" "); | printf(" "); | ||||
| v->print(); | v->print(); | ||||
| if (v == maxXy) | if (v == maxXy) | ||||
| { | { | ||||
| printf(" maxXy"); | printf(" maxXy"); | ||||
| } | } | ||||
| if (v == minYx) | if (v == minYx) | ||||
| Show All 11 Lines | for (Vertex* v = minXy; v;) | ||||
| printf("\n"); | printf("\n"); | ||||
| v = v->next; | v = v->next; | ||||
| if (v == minXy) | if (v == minXy) | ||||
| { | { | ||||
| break; | break; | ||||
| } | } | ||||
| } | } | ||||
| if (minXy) | if (minXy) | ||||
| { | { | ||||
| minXy->copy = (minXy->copy == -1) ? -2 : -1; | minXy->copy = (minXy->copy == -1) ? -2 : -1; | ||||
| minXy->printGraph(); | minXy->printGraph(); | ||||
| } | } | ||||
| } | } | ||||
| void btConvexHullInternal::Vertex::printGraph() | void btConvexHullInternal::Vertex::printGraph() | ||||
| { | { | ||||
| print(); | print(); | ||||
| ▲ Show 20 Lines • Show All 59 Lines • ▼ Show 20 Lines | #endif | ||||
| { | { | ||||
| do | do | ||||
| { | { | ||||
| if (e->copy > mergeStamp) | if (e->copy > mergeStamp) | ||||
| { | { | ||||
| Point32 t = *e->target - *start; | Point32 t = *e->target - *start; | ||||
| Rational64 cot(t.dot(sxrxs), t.dot(rxs)); | Rational64 cot(t.dot(sxrxs), t.dot(rxs)); | ||||
| #ifdef DEBUG_CONVEX_HULL | #ifdef DEBUG_CONVEX_HULL | ||||
| printf(" Angle is %f (%d) for ", (float) btAtan(cot.toScalar()), (int) cot.isNaN()); | printf(" Angle is %f (%d) for ", (float)btAtan(cot.toScalar()), (int)cot.isNaN()); | ||||
| e->print(); | e->print(); | ||||
| #endif | #endif | ||||
| if (cot.isNaN()) | if (cot.isNaN()) | ||||
| { | { | ||||
| btAssert(ccw ? (t.dot(s) < 0) : (t.dot(s) > 0)); | btAssert(ccw ? (t.dot(s) < 0) : (t.dot(s) > 0)); | ||||
| } | } | ||||
| else | else | ||||
| { | { | ||||
| Show All 30 Lines | void btConvexHullInternal::findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge*& e0, Edge*& e1, Vertex* stop0, Vertex* stop1) | ||||
| Point32 et0 = start0 ? start0->target->point : c0->point; | Point32 et0 = start0 ? start0->target->point : c0->point; | ||||
| Point32 et1 = start1 ? start1->target->point : c1->point; | Point32 et1 = start1 ? start1->target->point : c1->point; | ||||
| Point32 s = c1->point - c0->point; | Point32 s = c1->point - c0->point; | ||||
| Point64 normal = ((start0 ? start0 : start1)->target->point - c0->point).cross(s); | Point64 normal = ((start0 ? start0 : start1)->target->point - c0->point).cross(s); | ||||
| int64_t dist = c0->point.dot(normal); | int64_t dist = c0->point.dot(normal); | ||||
| btAssert(!start1 || (start1->target->point.dot(normal) == dist)); | btAssert(!start1 || (start1->target->point.dot(normal) == dist)); | ||||
| Point64 perp = s.cross(normal); | Point64 perp = s.cross(normal); | ||||
| btAssert(!perp.isZero()); | btAssert(!perp.isZero()); | ||||
| #ifdef DEBUG_CONVEX_HULL | #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 | #endif | ||||
| int64_t maxDot0 = et0.dot(perp); | int64_t maxDot0 = et0.dot(perp); | ||||
| if (e0) | if (e0) | ||||
| { | { | ||||
| while (e0->target != stop0) | while (e0->target != stop0) | ||||
| Show All 13 Lines | while (e0->target != stop0) | ||||
| { | { | ||||
| break; | break; | ||||
| } | } | ||||
| maxDot0 = dot; | maxDot0 = dot; | ||||
| e0 = e; | e0 = e; | ||||
| et0 = e->target->point; | et0 = e->target->point; | ||||
| } | } | ||||
| } | } | ||||
| int64_t maxDot1 = et1.dot(perp); | int64_t maxDot1 = et1.dot(perp); | ||||
| if (e1) | if (e1) | ||||
| { | { | ||||
| while (e1->target != stop1) | while (e1->target != stop1) | ||||
| { | { | ||||
| Edge* e = e1->reverse->next; | Edge* e = e1->reverse->next; | ||||
| if (e->target->point.dot(normal) < dist) | if (e->target->point.dot(normal) < dist) | ||||
| { | { | ||||
| Show All 20 Lines | |||||
| #endif | #endif | ||||
| int64_t dx = maxDot1 - maxDot0; | int64_t dx = maxDot1 - maxDot0; | ||||
| if (dx > 0) | if (dx > 0) | ||||
| { | { | ||||
| while (true) | while (true) | ||||
| { | { | ||||
| int64_t dy = (et1 - et0).dot(s); | int64_t dy = (et1 - et0).dot(s); | ||||
| if (e0 && (e0->target != stop0)) | if (e0 && (e0->target != stop0)) | ||||
| { | { | ||||
| Edge* f0 = e0->next->reverse; | Edge* f0 = e0->next->reverse; | ||||
| if (f0->copy > mergeStamp) | if (f0->copy > mergeStamp) | ||||
| { | { | ||||
| int64_t dx0 = (f0->target->point - et0).dot(perp); | int64_t dx0 = (f0->target->point - et0).dot(perp); | ||||
| int64_t dy0 = (f0->target->point - et0).dot(s); | int64_t dy0 = (f0->target->point - et0).dot(s); | ||||
| if ((dx0 == 0) ? (dy0 < 0) : ((dx0 < 0) && (Rational64(dy0, dx0).compare(Rational64(dy, dx)) >= 0))) | if ((dx0 == 0) ? (dy0 < 0) : ((dx0 < 0) && (Rational64(dy0, dx0).compare(Rational64(dy, dx)) >= 0))) | ||||
| { | { | ||||
| et0 = f0->target->point; | et0 = f0->target->point; | ||||
| dx = (et1 - et0).dot(perp); | dx = (et1 - et0).dot(perp); | ||||
| e0 = (e0 == start0) ? NULL : f0; | e0 = (e0 == start0) ? NULL : f0; | ||||
| continue; | continue; | ||||
| } | } | ||||
| } | } | ||||
| } | } | ||||
| if (e1 && (e1->target != stop1)) | if (e1 && (e1->target != stop1)) | ||||
| { | { | ||||
| Edge* f1 = e1->reverse->next; | Edge* f1 = e1->reverse->next; | ||||
| if (f1->copy > mergeStamp) | if (f1->copy > mergeStamp) | ||||
| { | { | ||||
| Point32 d1 = f1->target->point - et1; | Point32 d1 = f1->target->point - et1; | ||||
| if (d1.dot(normal) == 0) | if (d1.dot(normal) == 0) | ||||
| { | { | ||||
| Show All 18 Lines | while (true) | ||||
| break; | break; | ||||
| } | } | ||||
| } | } | ||||
| else if (dx < 0) | else if (dx < 0) | ||||
| { | { | ||||
| while (true) | while (true) | ||||
| { | { | ||||
| int64_t dy = (et1 - et0).dot(s); | int64_t dy = (et1 - et0).dot(s); | ||||
| if (e1 && (e1->target != stop1)) | if (e1 && (e1->target != stop1)) | ||||
| { | { | ||||
| Edge* f1 = e1->prev->reverse; | Edge* f1 = e1->prev->reverse; | ||||
| if (f1->copy > mergeStamp) | if (f1->copy > mergeStamp) | ||||
| { | { | ||||
| int64_t dx1 = (f1->target->point - et1).dot(perp); | int64_t dx1 = (f1->target->point - et1).dot(perp); | ||||
| int64_t dy1 = (f1->target->point - et1).dot(s); | int64_t dy1 = (f1->target->point - et1).dot(s); | ||||
| if ((dx1 == 0) ? (dy1 > 0) : ((dx1 < 0) && (Rational64(dy1, dx1).compare(Rational64(dy, dx)) <= 0))) | if ((dx1 == 0) ? (dy1 > 0) : ((dx1 < 0) && (Rational64(dy1, dx1).compare(Rational64(dy, dx)) <= 0))) | ||||
| { | { | ||||
| et1 = f1->target->point; | et1 = f1->target->point; | ||||
| dx = (et1 - et0).dot(perp); | dx = (et1 - et0).dot(perp); | ||||
| e1 = (e1 == start1) ? NULL : f1; | e1 = (e1 == start1) ? NULL : f1; | ||||
| continue; | continue; | ||||
| } | } | ||||
| } | } | ||||
| } | } | ||||
| if (e0 && (e0->target != stop0)) | if (e0 && (e0->target != stop0)) | ||||
| { | { | ||||
| Edge* f0 = e0->reverse->prev; | Edge* f0 = e0->reverse->prev; | ||||
| if (f0->copy > mergeStamp) | if (f0->copy > mergeStamp) | ||||
| { | { | ||||
| Point32 d0 = f0->target->point - et0; | Point32 d0 = f0->target->point - et0; | ||||
| if (d0.dot(normal) == 0) | if (d0.dot(normal) == 0) | ||||
| { | { | ||||
| Show All 18 Lines | while (true) | ||||
| break; | break; | ||||
| } | } | ||||
| } | } | ||||
| #ifdef DEBUG_CONVEX_HULL | #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 | #endif | ||||
| } | } | ||||
| void btConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1) | void btConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1) | ||||
| { | { | ||||
| if (!h1.maxXy) | if (!h1.maxXy) | ||||
| { | { | ||||
| return; | return; | ||||
| } | } | ||||
| if (!h0.maxXy) | if (!h0.maxXy) | ||||
| { | { | ||||
| h0 = h1; | h0 = h1; | ||||
| return; | return; | ||||
| } | } | ||||
| mergeStamp--; | mergeStamp--; | ||||
| Vertex* c0 = NULL; | Vertex* c0 = NULL; | ||||
| Edge* toPrev0 = NULL; | Edge* toPrev0 = NULL; | ||||
| Edge* firstNew0 = NULL; | Edge* firstNew0 = NULL; | ||||
| Edge* pendingHead0 = NULL; | Edge* pendingHead0 = NULL; | ||||
| Edge* pendingTail0 = NULL; | Edge* pendingTail0 = NULL; | ||||
| Vertex* c1 = NULL; | Vertex* c1 = NULL; | ||||
| Show All 23 Lines | if (e) | ||||
| if (!start0 || (getOrientation(start0, e, s, Point32(0, 0, -1)) == CLOCKWISE)) | if (!start0 || (getOrientation(start0, e, s, Point32(0, 0, -1)) == CLOCKWISE)) | ||||
| { | { | ||||
| start0 = e; | start0 = e; | ||||
| } | } | ||||
| } | } | ||||
| e = e->next; | e = e->next; | ||||
| } while (e != c0->edges); | } while (e != c0->edges); | ||||
| } | } | ||||
| e = c1->edges; | e = c1->edges; | ||||
| Edge* start1 = NULL; | Edge* start1 = NULL; | ||||
| if (e) | if (e) | ||||
| { | { | ||||
| do | do | ||||
| { | { | ||||
| int64_t dot = (*e->target - *c1).dot(normal); | int64_t dot = (*e->target - *c1).dot(normal); | ||||
| btAssert(dot <= 0); | btAssert(dot <= 0); | ||||
| Show All 35 Lines | void btConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1) | ||||
| bool firstRun = true; | bool firstRun = true; | ||||
| while (true) | while (true) | ||||
| { | { | ||||
| Point32 s = *c1 - *c0; | Point32 s = *c1 - *c0; | ||||
| Point32 r = prevPoint - c0->point; | Point32 r = prevPoint - c0->point; | ||||
| Point64 rxs = r.cross(s); | Point64 rxs = r.cross(s); | ||||
| Point64 sxrxs = s.cross(rxs); | Point64 sxrxs = s.cross(rxs); | ||||
| #ifdef DEBUG_CONVEX_HULL | #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 | #endif | ||||
| Rational64 minCot0(0, 0); | Rational64 minCot0(0, 0); | ||||
| Edge* min0 = findMaxAngle(false, c0, s, rxs, sxrxs, minCot0); | Edge* min0 = findMaxAngle(false, c0, s, rxs, sxrxs, minCot0); | ||||
| Rational64 minCot1(0, 0); | Rational64 minCot1(0, 0); | ||||
| Edge* min1 = findMaxAngle(true, c1, s, rxs, sxrxs, minCot1); | Edge* min1 = findMaxAngle(true, c1, s, rxs, sxrxs, minCot1); | ||||
| if (!min0 && !min1) | if (!min0 && !min1) | ||||
| Show All 34 Lines | #endif | ||||
| } | } | ||||
| else | else | ||||
| { | { | ||||
| pendingHead1 = e; | pendingHead1 = e; | ||||
| } | } | ||||
| e->prev = pendingTail1; | e->prev = pendingTail1; | ||||
| pendingTail1 = e; | pendingTail1 = e; | ||||
| } | } | ||||
| Edge* e0 = min0; | Edge* e0 = min0; | ||||
| Edge* e1 = min1; | Edge* e1 = min1; | ||||
| #ifdef DEBUG_CONVEX_HULL | #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 | #endif | ||||
| if (cmp == 0) | if (cmp == 0) | ||||
| { | { | ||||
| findEdgeForCoplanarFaces(c0, c1, e0, e1, NULL, NULL); | findEdgeForCoplanarFaces(c0, c1, e0, e1, NULL, NULL); | ||||
| } | } | ||||
| if ((cmp >= 0) && e1) | if ((cmp >= 0) && e1) | ||||
| { | { | ||||
| if (toPrev1) | if (toPrev1) | ||||
| { | { | ||||
| for (Edge* e = toPrev1->next, *n = NULL; e != min1; e = n) | for (Edge *e = toPrev1->next, *n = NULL; e != min1; e = n) | ||||
| { | { | ||||
| n = e->next; | n = e->next; | ||||
| removeEdgePair(e); | removeEdgePair(e); | ||||
| } | } | ||||
| } | } | ||||
| if (pendingTail1) | if (pendingTail1) | ||||
| { | { | ||||
| Show All 19 Lines | #endif | ||||
| c1 = e1->target; | c1 = e1->target; | ||||
| toPrev1 = e1->reverse; | toPrev1 = e1->reverse; | ||||
| } | } | ||||
| if ((cmp <= 0) && e0) | if ((cmp <= 0) && e0) | ||||
| { | { | ||||
| if (toPrev0) | if (toPrev0) | ||||
| { | { | ||||
| for (Edge* e = toPrev0->prev, *n = NULL; e != min0; e = n) | for (Edge *e = toPrev0->prev, *n = NULL; e != min0; e = n) | ||||
| { | { | ||||
| n = e->prev; | n = e->prev; | ||||
| removeEdgePair(e); | removeEdgePair(e); | ||||
| } | } | ||||
| } | } | ||||
| if (pendingTail0) | if (pendingTail0) | ||||
| { | { | ||||
| Show All 25 Lines | #endif | ||||
| { | { | ||||
| if (toPrev0 == NULL) | if (toPrev0 == NULL) | ||||
| { | { | ||||
| pendingHead0->link(pendingTail0); | pendingHead0->link(pendingTail0); | ||||
| c0->edges = pendingTail0; | c0->edges = pendingTail0; | ||||
| } | } | ||||
| else | else | ||||
| { | { | ||||
| for (Edge* e = toPrev0->prev, *n = NULL; e != firstNew0; e = n) | for (Edge *e = toPrev0->prev, *n = NULL; e != firstNew0; e = n) | ||||
| { | { | ||||
| n = e->prev; | n = e->prev; | ||||
| removeEdgePair(e); | removeEdgePair(e); | ||||
| } | } | ||||
| if (pendingTail0) | if (pendingTail0) | ||||
| { | { | ||||
| pendingHead0->link(toPrev0); | pendingHead0->link(toPrev0); | ||||
| firstNew0->link(pendingTail0); | firstNew0->link(pendingTail0); | ||||
| } | } | ||||
| } | } | ||||
| if (toPrev1 == NULL) | if (toPrev1 == NULL) | ||||
| { | { | ||||
| pendingTail1->link(pendingHead1); | pendingTail1->link(pendingHead1); | ||||
| c1->edges = pendingTail1; | c1->edges = pendingTail1; | ||||
| } | } | ||||
| else | else | ||||
| { | { | ||||
| for (Edge* e = toPrev1->next, *n = NULL; e != firstNew1; e = n) | for (Edge *e = toPrev1->next, *n = NULL; e != firstNew1; e = n) | ||||
| { | { | ||||
| n = e->next; | n = e->next; | ||||
| removeEdgePair(e); | removeEdgePair(e); | ||||
| } | } | ||||
| if (pendingTail1) | if (pendingTail1) | ||||
| { | { | ||||
| toPrev1->link(pendingHead1); | toPrev1->link(pendingHead1); | ||||
| pendingTail1->link(firstNew1); | pendingTail1->link(firstNew1); | ||||
| } | } | ||||
| } | } | ||||
| return; | return; | ||||
| } | } | ||||
| firstRun = false; | firstRun = false; | ||||
| } | } | ||||
| } | } | ||||
| class pointCmp | class pointCmp | ||||
| { | { | ||||
| public: | public: | ||||
| bool operator() ( const btConvexHullInternal::Point32& p, const btConvexHullInternal::Point32& q ) const | bool operator()(const btConvexHullInternal::Point32& p, const btConvexHullInternal::Point32& q) const | ||||
| { | { | ||||
| return (p.y < q.y) || ((p.y == q.y) && ((p.x < q.x) || ((p.x == q.x) && (p.z < q.z)))); | 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, int stride, int count) | void btConvexHullInternal::compute(const void* coords, bool doubleCoords, int stride, int count) | ||||
| { | { | ||||
| btVector3 min(btScalar(1e30), btScalar(1e30), btScalar(1e30)), max(btScalar(-1e30), btScalar(-1e30), btScalar(-1e30)); | btVector3 min(btScalar(1e30), btScalar(1e30), btScalar(1e30)), max(btScalar(-1e30), btScalar(-1e30), btScalar(-1e30)); | ||||
| const char* ptr = (const char*) coords; | const char* ptr = (const char*)coords; | ||||
| if (doubleCoords) | if (doubleCoords) | ||||
| { | { | ||||
| for (int i = 0; i < count; i++) | for (int i = 0; i < count; i++) | ||||
| { | { | ||||
| const double* v = (const double*) ptr; | const double* v = (const double*)ptr; | ||||
| btVector3 p((btScalar) v[0], (btScalar) v[1], (btScalar) v[2]); | btVector3 p((btScalar)v[0], (btScalar)v[1], (btScalar)v[2]); | ||||
| ptr += stride; | ptr += stride; | ||||
| min.setMin(p); | min.setMin(p); | ||||
| max.setMax(p); | max.setMax(p); | ||||
| } | } | ||||
| } | } | ||||
| else | else | ||||
| { | { | ||||
| for (int i = 0; i < count; i++) | for (int i = 0; i < count; i++) | ||||
| { | { | ||||
| const float* v = (const float*) ptr; | const float* v = (const float*)ptr; | ||||
| btVector3 p(v[0], v[1], v[2]); | btVector3 p(v[0], v[1], v[2]); | ||||
| ptr += stride; | ptr += stride; | ||||
| min.setMin(p); | min.setMin(p); | ||||
| max.setMax(p); | max.setMax(p); | ||||
| } | } | ||||
| } | } | ||||
| btVector3 s = max - min; | btVector3 s = max - min; | ||||
| Show All 24 Lines | void btConvexHullInternal::compute(const void* coords, bool doubleCoords, int stride, int count) | ||||
| { | { | ||||
| s[2] = btScalar(1) / s[2]; | s[2] = btScalar(1) / s[2]; | ||||
| } | } | ||||
| center = (min + max) * btScalar(0.5); | center = (min + max) * btScalar(0.5); | ||||
| btAlignedObjectArray<Point32> points; | btAlignedObjectArray<Point32> points; | ||||
| points.resize(count); | points.resize(count); | ||||
| ptr = (const char*) coords; | ptr = (const char*)coords; | ||||
| if (doubleCoords) | if (doubleCoords) | ||||
| { | { | ||||
| for (int i = 0; i < count; i++) | for (int i = 0; i < count; i++) | ||||
| { | { | ||||
| const double* v = (const double*) ptr; | const double* v = (const double*)ptr; | ||||
| btVector3 p((btScalar) v[0], (btScalar) v[1], (btScalar) v[2]); | btVector3 p((btScalar)v[0], (btScalar)v[1], (btScalar)v[2]); | ||||
| ptr += stride; | ptr += stride; | ||||
| p = (p - center) * s; | p = (p - center) * s; | ||||
| points[i].x = (int32_t) p[medAxis]; | points[i].x = (int32_t)p[medAxis]; | ||||
| points[i].y = (int32_t) p[maxAxis]; | points[i].y = (int32_t)p[maxAxis]; | ||||
| points[i].z = (int32_t) p[minAxis]; | points[i].z = (int32_t)p[minAxis]; | ||||
| points[i].index = i; | points[i].index = i; | ||||
| } | } | ||||
| } | } | ||||
| else | else | ||||
| { | { | ||||
| for (int i = 0; i < count; i++) | for (int i = 0; i < count; i++) | ||||
| { | { | ||||
| const float* v = (const float*) ptr; | const float* v = (const float*)ptr; | ||||
| btVector3 p(v[0], v[1], v[2]); | btVector3 p(v[0], v[1], v[2]); | ||||
| ptr += stride; | ptr += stride; | ||||
| p = (p - center) * s; | p = (p - center) * s; | ||||
| points[i].x = (int32_t) p[medAxis]; | points[i].x = (int32_t)p[medAxis]; | ||||
| points[i].y = (int32_t) p[maxAxis]; | points[i].y = (int32_t)p[maxAxis]; | ||||
| points[i].z = (int32_t) p[minAxis]; | points[i].z = (int32_t)p[minAxis]; | ||||
| points[i].index = i; | points[i].index = i; | ||||
| } | } | ||||
| } | } | ||||
| points.quickSort(pointCmp()); | points.quickSort(pointCmp()); | ||||
| vertexPool.reset(); | vertexPool.reset(); | ||||
| vertexPool.setArraySize(count); | vertexPool.setArraySize(count); | ||||
| originalVertices.resize(count); | originalVertices.resize(count); | ||||
| ▲ Show 20 Lines • Show All 137 Lines • ▼ Show 20 Lines | if (clampAmount > 0) | ||||
| { | { | ||||
| btVector3 normal = getBtNormal(faces[i]); | btVector3 normal = getBtNormal(faces[i]); | ||||
| btScalar dist = normal.dot(toBtVector(faces[i]->origin) - hullCenter); | btScalar dist = normal.dot(toBtVector(faces[i]->origin) - hullCenter); | ||||
| if (dist < minDist) | if (dist < minDist) | ||||
| { | { | ||||
| minDist = dist; | minDist = dist; | ||||
| } | } | ||||
| } | } | ||||
| if (minDist <= 0) | if (minDist <= 0) | ||||
| { | { | ||||
| return 0; | return 0; | ||||
| } | } | ||||
| amount = btMin(amount, minDist * clampAmount); | amount = btMin(amount, minDist * clampAmount); | ||||
| } | } | ||||
| Show All 24 Lines | bool btConvexHullInternal::shiftFace(Face* face, btScalar amount, btAlignedObjectArray<Vertex*> stack) | ||||
| if (scaling[1] != 0) | if (scaling[1] != 0) | ||||
| { | { | ||||
| origShift[1] /= scaling[1]; | origShift[1] /= scaling[1]; | ||||
| } | } | ||||
| if (scaling[2] != 0) | if (scaling[2] != 0) | ||||
| { | { | ||||
| origShift[2] /= scaling[2]; | origShift[2] /= scaling[2]; | ||||
| } | } | ||||
| Point32 shift((int32_t) origShift[medAxis], (int32_t) origShift[maxAxis], (int32_t) origShift[minAxis]); | Point32 shift((int32_t)origShift[medAxis], (int32_t)origShift[maxAxis], (int32_t)origShift[minAxis]); | ||||
| if (shift.isZero()) | if (shift.isZero()) | ||||
| { | { | ||||
| return true; | return true; | ||||
| } | } | ||||
| Point64 normal = face->getNormal(); | Point64 normal = face->getNormal(); | ||||
| #ifdef DEBUG_CONVEX_HULL | #ifdef DEBUG_CONVEX_HULL | ||||
| printf("\nShrinking face (%d %d %d) (%d %d %d) (%d %d %d) by (%d %d %d)\n", | 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); | 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 | #endif | ||||
| int64_t origDot = face->origin.dot(normal); | int64_t origDot = face->origin.dot(normal); | ||||
| Point32 shiftedOrigin = face->origin + shift; | Point32 shiftedOrigin = face->origin + shift; | ||||
| int64_t shiftedDot = shiftedOrigin.dot(normal); | int64_t shiftedDot = shiftedOrigin.dot(normal); | ||||
| btAssert(shiftedDot <= origDot); | btAssert(shiftedDot <= origDot); | ||||
| if (shiftedDot >= origDot) | if (shiftedDot >= origDot) | ||||
| { | { | ||||
| return false; | return false; | ||||
| Show All 20 Lines | |||||
| #ifdef SHOW_ITERATIONS | #ifdef SHOW_ITERATIONS | ||||
| n++; | n++; | ||||
| #endif | #endif | ||||
| Rational128 dot = e->target->dot(normal); | Rational128 dot = e->target->dot(normal); | ||||
| btAssert(dot.compare(origDot) <= 0); | btAssert(dot.compare(origDot) <= 0); | ||||
| #ifdef DEBUG_CONVEX_HULL | #ifdef DEBUG_CONVEX_HULL | ||||
| printf("Moving downwards, edge is "); | printf("Moving downwards, edge is "); | ||||
| e->print(); | e->print(); | ||||
| printf(", dot is %f (%f %lld)\n", (float) dot.toScalar(), (float) optDot.toScalar(), shiftedDot); | printf(", dot is %f (%f %lld)\n", (float)dot.toScalar(), (float)optDot.toScalar(), shiftedDot); | ||||
| #endif | #endif | ||||
| if (dot.compare(optDot) < 0) | if (dot.compare(optDot) < 0) | ||||
| { | { | ||||
| int c = dot.compare(shiftedDot); | int c = dot.compare(shiftedDot); | ||||
| optDot = dot; | optDot = dot; | ||||
| e = e->reverse; | e = e->reverse; | ||||
| startEdge = e; | startEdge = e; | ||||
| if (c < 0) | if (c < 0) | ||||
| Show All 19 Lines | |||||
| #ifdef SHOW_ITERATIONS | #ifdef SHOW_ITERATIONS | ||||
| n++; | n++; | ||||
| #endif | #endif | ||||
| Rational128 dot = e->target->dot(normal); | Rational128 dot = e->target->dot(normal); | ||||
| btAssert(dot.compare(origDot) <= 0); | btAssert(dot.compare(origDot) <= 0); | ||||
| #ifdef DEBUG_CONVEX_HULL | #ifdef DEBUG_CONVEX_HULL | ||||
| printf("Moving upwards, edge is "); | printf("Moving upwards, edge is "); | ||||
| e->print(); | e->print(); | ||||
| printf(", dot is %f (%f %lld)\n", (float) dot.toScalar(), (float) optDot.toScalar(), shiftedDot); | printf(", dot is %f (%f %lld)\n", (float)dot.toScalar(), (float)optDot.toScalar(), shiftedDot); | ||||
| #endif | #endif | ||||
| if (dot.compare(optDot) > 0) | if (dot.compare(optDot) > 0) | ||||
| { | { | ||||
| cmp = dot.compare(shiftedDot); | cmp = dot.compare(shiftedDot); | ||||
| if (cmp >= 0) | if (cmp >= 0) | ||||
| { | { | ||||
| intersection = e; | intersection = e; | ||||
| break; | break; | ||||
| } | } | ||||
| optDot = dot; | optDot = dot; | ||||
| e = e->reverse; | e = e->reverse; | ||||
| startEdge = e; | startEdge = e; | ||||
| } | } | ||||
| e = e->prev; | e = e->prev; | ||||
| } while (e != startEdge); | } while (e != startEdge); | ||||
| if (!intersection) | if (!intersection) | ||||
| { | { | ||||
| return true; | return true; | ||||
| } | } | ||||
| } | } | ||||
| #ifdef SHOW_ITERATIONS | #ifdef SHOW_ITERATIONS | ||||
| printf("Needed %d iterations to find initial intersection\n", n); | printf("Needed %d iterations to find initial intersection\n", n); | ||||
| Show All 20 Lines | #ifdef DEBUG_CONVEX_HULL | ||||
| e->print(); | e->print(); | ||||
| printf("\n"); | printf("\n"); | ||||
| #endif | #endif | ||||
| } | } | ||||
| #ifdef SHOW_ITERATIONS | #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 | #endif | ||||
| } | } | ||||
| Edge* firstIntersection = NULL; | Edge* firstIntersection = NULL; | ||||
| Edge* faceEdge = NULL; | Edge* faceEdge = NULL; | ||||
| Edge* firstFaceEdge = NULL; | Edge* firstFaceEdge = NULL; | ||||
| #ifdef SHOW_ITERATIONS | #ifdef SHOW_ITERATIONS | ||||
| int m = 0; | int m = 0; | ||||
| #endif | #endif | ||||
| while (true) | while (true) | ||||
| ▲ Show 20 Lines • Show All 92 Lines • ▼ Show 20 Lines | if (cmp > 0) | ||||
| { | { | ||||
| removed->edges = e->prev; | removed->edges = e->prev; | ||||
| e->prev->link(e->next); | e->prev->link(e->next); | ||||
| e->link(e); | e->link(e); | ||||
| } | } | ||||
| #ifdef DEBUG_CONVEX_HULL | #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 | #endif | ||||
| Point64 n0 = intersection->face->getNormal(); | Point64 n0 = intersection->face->getNormal(); | ||||
| Point64 n1 = intersection->reverse->face->getNormal(); | Point64 n1 = intersection->reverse->face->getNormal(); | ||||
| int64_t m00 = face->dir0.dot(n0); | int64_t m00 = face->dir0.dot(n0); | ||||
| int64_t m01 = face->dir1.dot(n0); | int64_t m01 = face->dir1.dot(n0); | ||||
| int64_t m10 = face->dir0.dot(n1); | int64_t m10 = face->dir0.dot(n1); | ||||
| int64_t m11 = face->dir1.dot(n1); | int64_t m11 = face->dir1.dot(n1); | ||||
| int64_t r0 = (intersection->face->origin - shiftedOrigin).dot(n0); | int64_t r0 = (intersection->face->origin - shiftedOrigin).dot(n0); | ||||
| int64_t r1 = (intersection->reverse->face->origin - shiftedOrigin).dot(n1); | int64_t r1 = (intersection->reverse->face->origin - shiftedOrigin).dot(n1); | ||||
| Int128 det = Int128::mul(m00, m11) - Int128::mul(m01, m10); | Int128 det = Int128::mul(m00, m11) - Int128::mul(m01, m10); | ||||
| btAssert(det.getSign() != 0); | btAssert(det.getSign() != 0); | ||||
| Vertex* v = vertexPool.newObject(); | Vertex* v = vertexPool.newObject(); | ||||
| v->point.index = -1; | v->point.index = -1; | ||||
| v->copy = -1; | v->copy = -1; | ||||
| v->point128 = PointR128(Int128::mul(face->dir0.x * r0, m11) - Int128::mul(face->dir0.x * r1, m01) | 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->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.y * r0, m11) - Int128::mul(face->dir0.y * r1, m01) | 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, | ||||
| + 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); | det); | ||||
| v->point.x = (int32_t) v->point128.xvalue(); | v->point.x = (int32_t)v->point128.xvalue(); | ||||
| v->point.y = (int32_t) v->point128.yvalue(); | v->point.y = (int32_t)v->point128.yvalue(); | ||||
| v->point.z = (int32_t) v->point128.zvalue(); | v->point.z = (int32_t)v->point128.zvalue(); | ||||
| intersection->target = v; | intersection->target = v; | ||||
| v->edges = e; | v->edges = e; | ||||
| stack.push_back(v); | stack.push_back(v); | ||||
| stack.push_back(removed); | stack.push_back(removed); | ||||
| stack.push_back(NULL); | stack.push_back(NULL); | ||||
| } | } | ||||
| ▲ Show 20 Lines • Show All 122 Lines • ▼ Show 20 Lines | |||||
| #endif | #endif | ||||
| stack.resize(0); | stack.resize(0); | ||||
| face->origin = shiftedOrigin; | face->origin = shiftedOrigin; | ||||
| return true; | return true; | ||||
| } | } | ||||
| static int getVertexCopy(btConvexHullInternal::Vertex* vertex, btAlignedObjectArray<btConvexHullInternal::Vertex*>& vertices) | static int getVertexCopy(btConvexHullInternal::Vertex* vertex, btAlignedObjectArray<btConvexHullInternal::Vertex*>& vertices) | ||||
| { | { | ||||
| int index = vertex->copy; | int index = vertex->copy; | ||||
| if (index < 0) | if (index < 0) | ||||
| { | { | ||||
| index = vertices.size(); | index = vertices.size(); | ||||
| vertex->copy = index; | vertex->copy = index; | ||||
| vertices.push_back(vertex); | vertices.push_back(vertex); | ||||
| ▲ Show 20 Lines • Show All 107 Lines • ▼ Show 20 Lines | #endif | ||||
| } | } | ||||
| e = e->next; | e = e->next; | ||||
| } while (e != firstEdge); | } while (e != firstEdge); | ||||
| } | } | ||||
| } | } | ||||
| return shift; | return shift; | ||||
| } | } | ||||