summaryrefslogtreecommitdiff
path: root/thirdparty/bullet/Bullet3Geometry/b3ConvexHullComputer.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'thirdparty/bullet/Bullet3Geometry/b3ConvexHullComputer.cpp')
-rw-r--r--thirdparty/bullet/Bullet3Geometry/b3ConvexHullComputer.cpp1554
1 files changed, 772 insertions, 782 deletions
diff --git a/thirdparty/bullet/Bullet3Geometry/b3ConvexHullComputer.cpp b/thirdparty/bullet/Bullet3Geometry/b3ConvexHullComputer.cpp
index 18835c38d5..b37652456e 100644
--- a/thirdparty/bullet/Bullet3Geometry/b3ConvexHullComputer.cpp
+++ b/thirdparty/bullet/Bullet3Geometry/b3ConvexHullComputer.cpp
@@ -20,850 +20,851 @@ subject to the following restrictions:
#include "Bullet3Common/b3Vector3.h"
#ifdef __GNUC__
- #include <stdint.h>
- typedef int32_t btInt32_t;
- typedef int64_t btInt64_t;
- typedef uint32_t btUint32_t;
- typedef uint64_t btUint64_t;
+#include <stdint.h>
+typedef int32_t btInt32_t;
+typedef int64_t btInt64_t;
+typedef uint32_t btUint32_t;
+typedef uint64_t btUint64_t;
#elif defined(_MSC_VER)
- typedef __int32 btInt32_t;
- typedef __int64 btInt64_t;
- typedef unsigned __int32 btUint32_t;
- typedef unsigned __int64 btUint64_t;
+typedef __int32 btInt32_t;
+typedef __int64 btInt64_t;
+typedef unsigned __int32 btUint32_t;
+typedef unsigned __int64 btUint64_t;
#else
- typedef int btInt32_t;
- typedef long long int btInt64_t;
- typedef unsigned int btUint32_t;
- typedef unsigned long long int btUint64_t;
+typedef int btInt32_t;
+typedef long long int btInt64_t;
+typedef unsigned int btUint32_t;
+typedef unsigned long long int btUint64_t;
#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
//#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
//#endif
-
//#define DEBUG_CONVEX_HULL
//#define SHOW_ITERATIONS
#if defined(DEBUG_CONVEX_HULL) || defined(SHOW_ITERATIONS)
- #include <stdio.h>
+#include <stdio.h>
#endif
// Convex hull implementation based on Preparata and Hong
// Ole Kniemeyer, MAXON Computer GmbH
class b3ConvexHullInternal
{
+public:
+ class Point64
+ {
public:
-
- class Point64
- {
- public:
- btInt64_t x;
- btInt64_t y;
- btInt64_t z;
-
- Point64(btInt64_t x, btInt64_t y, btInt64_t z): x(x), y(y), z(z)
- {
- }
+ btInt64_t x;
+ btInt64_t y;
+ btInt64_t z;
- bool isZero()
- {
- return (x == 0) && (y == 0) && (z == 0);
- }
+ Point64(btInt64_t x, btInt64_t y, btInt64_t z) : x(x), y(y), z(z)
+ {
+ }
- btInt64_t dot(const Point64& b) const
- {
- return x * b.x + y * b.y + z * b.z;
- }
- };
-
- class Point32
- {
- public:
- btInt32_t x;
- btInt32_t y;
- btInt32_t z;
- int index;
-
- Point32()
- {
- }
-
- Point32(btInt32_t x, btInt32_t y, btInt32_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 isZero()
+ {
+ return (x == 0) && (y == 0) && (z == 0);
+ }
- bool operator!=(const Point32& b) const
- {
- return (x != b.x) || (y != b.y) || (z != b.z);
- }
+ btInt64_t dot(const Point64& b) const
+ {
+ return x * b.x + y * b.y + z * b.z;
+ }
+ };
- bool isZero()
- {
- return (x == 0) && (y == 0) && (z == 0);
- }
+ class Point32
+ {
+ public:
+ btInt32_t x;
+ btInt32_t y;
+ btInt32_t z;
+ int index;
- 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);
- }
+ Point32()
+ {
+ }
- 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);
- }
+ Point32(btInt32_t x, btInt32_t y, btInt32_t z) : x(x), y(y), z(z), index(-1)
+ {
+ }
- btInt64_t dot(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);
+ }
- btInt64_t dot(const Point64& 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);
+ }
- Point32 operator+(const Point32& b) const
- {
- return Point32(x + b.x, y + b.y, z + b.z);
- }
+ bool isZero()
+ {
+ return (x == 0) && (y == 0) && (z == 0);
+ }
- Point32 operator-(const Point32& b) const
- {
- return Point32(x - b.x, y - b.y, z - b.z);
- }
- };
+ 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);
+ }
- class Int128
+ Point64 cross(const Point64& b) const
{
- public:
- btUint64_t low;
- btUint64_t high;
+ return Point64(y * b.z - z * b.y, z * b.x - x * b.z, x * b.y - y * b.x);
+ }
- Int128()
- {
- }
+ btInt64_t dot(const Point32& b) const
+ {
+ return x * b.x + y * b.y + z * b.z;
+ }
- Int128(btUint64_t low, btUint64_t high): low(low), high(high)
- {
- }
+ btInt64_t dot(const Point64& b) const
+ {
+ return x * b.x + y * b.y + z * b.z;
+ }
- Int128(btUint64_t low): low(low), high(0)
- {
- }
+ Point32 operator+(const Point32& b) const
+ {
+ return Point32(x + b.x, y + b.y, z + b.z);
+ }
- Int128(btInt64_t value): low(value), high((value >= 0) ? 0 : (btUint64_t) -1LL)
- {
- }
+ Point32 operator-(const Point32& b) const
+ {
+ return Point32(x - b.x, y - b.y, z - b.z);
+ }
+ };
- static Int128 mul(btInt64_t a, btInt64_t b);
+ class Int128
+ {
+ public:
+ btUint64_t low;
+ btUint64_t high;
- static Int128 mul(btUint64_t a, btUint64_t b);
+ Int128()
+ {
+ }
- Int128 operator-() const
- {
- return Int128((btUint64_t) -(btInt64_t)low, ~high + (low == 0));
- }
+ Int128(btUint64_t low, btUint64_t high) : low(low), high(high)
+ {
+ }
- Int128 operator+(const Int128& b) const
- {
+ Int128(btUint64_t low) : low(low), high(0)
+ {
+ }
+
+ Int128(btInt64_t value) : low(value), high((value >= 0) ? 0 : (btUint64_t)-1LL)
+ {
+ }
+
+ static Int128 mul(btInt64_t a, btInt64_t b);
+
+ static Int128 mul(btUint64_t a, btUint64_t b);
+
+ Int128 operator-() const
+ {
+ return Int128((btUint64_t) - (btInt64_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
- btUint64_t lo = low + b.low;
- return Int128(lo, high + b.high + (lo < low));
+ btUint64_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
- btUint64_t lo = low + b.low;
- if (lo < low)
- {
- ++high;
- }
- low = lo;
- high += b.high;
+ btUint64_t lo = low + b.low;
+ if (lo < low)
+ {
+ ++high;
+ }
+ low = lo;
+ high += b.high;
#endif
- return *this;
- }
+ return *this;
+ }
- Int128& operator++()
- {
- if (++low == 0)
- {
- ++high;
- }
- return *this;
- }
+ Int128& operator++()
+ {
+ if (++low == 0)
+ {
+ ++high;
+ }
+ return *this;
+ }
- Int128 operator*(btInt64_t b) const;
+ Int128 operator*(btInt64_t b) const;
- b3Scalar toScalar() const
- {
- return ((btInt64_t) high >= 0) ? b3Scalar(high) * (b3Scalar(0x100000000LL) * b3Scalar(0x100000000LL)) + b3Scalar(low)
- : -(-*this).toScalar();
- }
+ b3Scalar toScalar() const
+ {
+ return ((btInt64_t)high >= 0) ? b3Scalar(high) * (b3Scalar(0x100000000LL) * b3Scalar(0x100000000LL)) + b3Scalar(low)
+ : -(-*this).toScalar();
+ }
- int getSign() const
- {
- return ((btInt64_t) high < 0) ? -1 : (high || low) ? 1 : 0;
- }
+ int getSign() const
+ {
+ return ((btInt64_t)high < 0) ? -1 : (high || low) ? 1 : 0;
+ }
- bool operator<(const Int128& b) const
- {
- return (high < b.high) || ((high == b.high) && (low < b.low));
- }
+ bool operator<(const Int128& b) const
+ {
+ return (high < b.high) || ((high == b.high) && (low < b.low));
+ }
- int 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;
- }
- };
+ int 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:
+ btUint64_t m_numerator;
+ btUint64_t m_denominator;
+ int sign;
- class Rational64
+ public:
+ Rational64(btInt64_t numerator, btInt64_t denominator)
{
- private:
- btUint64_t m_numerator;
- btUint64_t m_denominator;
- int sign;
-
- public:
- Rational64(btInt64_t numerator, btInt64_t denominator)
- {
- if (numerator > 0)
- {
- sign = 1;
- m_numerator = (btUint64_t) numerator;
- }
- else if (numerator < 0)
- {
- sign = -1;
- m_numerator = (btUint64_t) -numerator;
- }
- else
- {
- sign = 0;
- m_numerator = 0;
- }
- if (denominator > 0)
- {
- m_denominator = (btUint64_t) denominator;
- }
- else if (denominator < 0)
- {
- sign = -sign;
- m_denominator = (btUint64_t) -denominator;
- }
- else
- {
- m_denominator = 0;
- }
- }
-
- bool isNegativeInfinity() const
- {
- return (sign < 0) && (m_denominator == 0);
- }
-
- bool isNaN() const
- {
- return (sign == 0) && (m_denominator == 0);
- }
-
- int compare(const Rational64& b) const;
-
- b3Scalar toScalar() const
- {
- return sign * ((m_denominator == 0) ? B3_INFINITY : (b3Scalar) m_numerator / m_denominator);
- }
- };
-
+ if (numerator > 0)
+ {
+ sign = 1;
+ m_numerator = (btUint64_t)numerator;
+ }
+ else if (numerator < 0)
+ {
+ sign = -1;
+ m_numerator = (btUint64_t)-numerator;
+ }
+ else
+ {
+ sign = 0;
+ m_numerator = 0;
+ }
+ if (denominator > 0)
+ {
+ m_denominator = (btUint64_t)denominator;
+ }
+ else if (denominator < 0)
+ {
+ sign = -sign;
+ m_denominator = (btUint64_t)-denominator;
+ }
+ else
+ {
+ m_denominator = 0;
+ }
+ }
- class Rational128
+ bool isNegativeInfinity() const
{
- private:
- Int128 numerator;
- Int128 denominator;
- int sign;
- bool isInt64;
+ return (sign < 0) && (m_denominator == 0);
+ }
- public:
- Rational128(btInt64_t value)
- {
- if (value > 0)
- {
- sign = 1;
- this->numerator = value;
- }
- else if (value < 0)
- {
- sign = -1;
- this->numerator = -value;
- }
- else
- {
- sign = 0;
- this->numerator = (btUint64_t) 0;
- }
- this->denominator = (btUint64_t) 1;
- isInt64 = true;
- }
+ bool isNaN() const
+ {
+ return (sign == 0) && (m_denominator == 0);
+ }
- Rational128(const Int128& numerator, const Int128& denominator)
- {
- sign = numerator.getSign();
- if (sign >= 0)
- {
- this->numerator = numerator;
- }
- else
- {
- this->numerator = -numerator;
- }
- int dsign = denominator.getSign();
- if (dsign >= 0)
- {
- this->denominator = denominator;
- }
- else
- {
- sign = -sign;
- this->denominator = -denominator;
- }
- isInt64 = false;
- }
+ int compare(const Rational64& b) const;
- int compare(const Rational128& b) const;
+ b3Scalar toScalar() const
+ {
+ return sign * ((m_denominator == 0) ? B3_INFINITY : (b3Scalar)m_numerator / m_denominator);
+ }
+ };
- int compare(btInt64_t b) const;
+ class Rational128
+ {
+ private:
+ Int128 numerator;
+ Int128 denominator;
+ int sign;
+ bool isInt64;
- b3Scalar toScalar() const
- {
- return sign * ((denominator.getSign() == 0) ? B3_INFINITY : numerator.toScalar() / denominator.toScalar());
- }
- };
+ public:
+ Rational128(btInt64_t value)
+ {
+ if (value > 0)
+ {
+ sign = 1;
+ this->numerator = value;
+ }
+ else if (value < 0)
+ {
+ sign = -1;
+ this->numerator = -value;
+ }
+ else
+ {
+ sign = 0;
+ this->numerator = (btUint64_t)0;
+ }
+ this->denominator = (btUint64_t)1;
+ isInt64 = true;
+ }
- class PointR128
+ Rational128(const Int128& numerator, const Int128& denominator)
{
- public:
- Int128 x;
- Int128 y;
- Int128 z;
- Int128 denominator;
+ sign = numerator.getSign();
+ if (sign >= 0)
+ {
+ this->numerator = numerator;
+ }
+ else
+ {
+ this->numerator = -numerator;
+ }
+ int dsign = denominator.getSign();
+ if (dsign >= 0)
+ {
+ this->denominator = denominator;
+ }
+ else
+ {
+ sign = -sign;
+ this->denominator = -denominator;
+ }
+ isInt64 = false;
+ }
- PointR128()
- {
- }
+ int compare(const Rational128& b) const;
- PointR128(Int128 x, Int128 y, Int128 z, Int128 denominator): x(x), y(y), z(z), denominator(denominator)
- {
- }
+ int compare(btInt64_t b) const;
- b3Scalar xvalue() const
- {
- return x.toScalar() / denominator.toScalar();
- }
+ b3Scalar toScalar() const
+ {
+ return sign * ((denominator.getSign() == 0) ? B3_INFINITY : numerator.toScalar() / denominator.toScalar());
+ }
+ };
- b3Scalar yvalue() const
- {
- return y.toScalar() / denominator.toScalar();
- }
+ class PointR128
+ {
+ public:
+ Int128 x;
+ Int128 y;
+ Int128 z;
+ Int128 denominator;
- b3Scalar zvalue() const
- {
- return z.toScalar() / denominator.toScalar();
- }
- };
+ PointR128()
+ {
+ }
+ PointR128(Int128 x, Int128 y, Int128 z, Int128 denominator) : x(x), y(y), z(z), denominator(denominator)
+ {
+ }
- class Edge;
- class Face;
+ b3Scalar xvalue() const
+ {
+ return x.toScalar() / denominator.toScalar();
+ }
- class Vertex
+ b3Scalar yvalue() const
{
- public:
- Vertex* next;
- Vertex* prev;
- Edge* edges;
- Face* firstNearbyFace;
- Face* lastNearbyFace;
- PointR128 point128;
- Point32 point;
- int copy;
-
- Vertex(): next(NULL), prev(NULL), edges(NULL), firstNearbyFace(NULL), lastNearbyFace(NULL), copy(-1)
- {
- }
+ return y.toScalar() / denominator.toScalar();
+ }
-#ifdef DEBUG_CONVEX_HULL
- void print()
- {
- b3Printf("V%d (%d, %d, %d)", point.index, point.x, point.y, point.z);
- }
+ b3Scalar zvalue() const
+ {
+ return z.toScalar() / denominator.toScalar();
+ }
+ };
- void printGraph();
-#endif
+ class Edge;
+ class Face;
- Point32 operator-(const Vertex& b) const
- {
- return point - b.point;
- }
+ class Vertex
+ {
+ public:
+ Vertex* next;
+ Vertex* prev;
+ Edge* edges;
+ Face* firstNearbyFace;
+ Face* lastNearbyFace;
+ PointR128 point128;
+ Point32 point;
+ int copy;
- 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);
- }
+ Vertex() : next(NULL), prev(NULL), edges(NULL), firstNearbyFace(NULL), lastNearbyFace(NULL), copy(-1)
+ {
+ }
- b3Scalar xvalue() const
- {
- return (point.index >= 0) ? b3Scalar(point.x) : point128.xvalue();
- }
+#ifdef DEBUG_CONVEX_HULL
+ void print()
+ {
+ b3Printf("V%d (%d, %d, %d)", point.index, point.x, point.y, point.z);
+ }
- b3Scalar yvalue() const
- {
- return (point.index >= 0) ? b3Scalar(point.y) : point128.yvalue();
- }
+ void printGraph();
+#endif
- b3Scalar zvalue() const
- {
- return (point.index >= 0) ? b3Scalar(point.z) : point128.zvalue();
- }
+ Point32 operator-(const Vertex& b) const
+ {
+ return point - b.point;
+ }
- 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)
- {
- b3Assert(f->nearbyVertex == src);
- f->nearbyVertex = this;
- }
- src->firstNearbyFace = NULL;
- src->lastNearbyFace = NULL;
- }
- };
+ 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);
+ }
+ b3Scalar xvalue() const
+ {
+ return (point.index >= 0) ? b3Scalar(point.x) : point128.xvalue();
+ }
- class Edge
+ b3Scalar yvalue() const
{
- public:
- Edge* next;
- Edge* prev;
- Edge* reverse;
- Vertex* target;
- Face* face;
- int copy;
+ return (point.index >= 0) ? b3Scalar(point.y) : point128.yvalue();
+ }
- ~Edge()
- {
- next = NULL;
- prev = NULL;
- reverse = NULL;
- target = NULL;
- face = NULL;
- }
+ b3Scalar zvalue() const
+ {
+ return (point.index >= 0) ? b3Scalar(point.z) : point128.zvalue();
+ }
- void link(Edge* n)
- {
- b3Assert(reverse->target == n->reverse->target);
- next = n;
- n->prev = this;
- }
+ 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)
+ {
+ b3Assert(f->nearbyVertex == src);
+ f->nearbyVertex = this;
+ }
+ src->firstNearbyFace = NULL;
+ src->lastNearbyFace = NULL;
+ }
+ };
-#ifdef DEBUG_CONVEX_HULL
- void print()
- {
- b3Printf("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 Edge
+ {
+ public:
+ Edge* next;
+ Edge* prev;
+ Edge* reverse;
+ Vertex* target;
+ Face* face;
+ int copy;
+
+ ~Edge()
+ {
+ next = NULL;
+ prev = NULL;
+ reverse = NULL;
+ target = NULL;
+ face = NULL;
+ }
- class Face
+ void link(Edge* n)
{
- public:
- Face* next;
- Vertex* nearbyVertex;
- Face* nextWithSameNearbyVertex;
- Point32 origin;
- Point32 dir0;
- Point32 dir1;
+ b3Assert(reverse->target == n->reverse->target);
+ next = n;
+ n->prev = this;
+ }
- Face(): next(NULL), nearbyVertex(NULL), nextWithSameNearbyVertex(NULL)
- {
- }
+#ifdef DEBUG_CONVEX_HULL
+ void print()
+ {
+ b3Printf("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
+ };
- 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;
- }
+ class Face
+ {
+ public:
+ Face* next;
+ Vertex* nearbyVertex;
+ Face* nextWithSameNearbyVertex;
+ Point32 origin;
+ Point32 dir0;
+ Point32 dir1;
- Point64 getNormal()
- {
- return dir0.cross(dir1);
- }
- };
+ Face() : next(NULL), nearbyVertex(NULL), nextWithSameNearbyVertex(NULL)
+ {
+ }
- template<typename UWord, typename UHWord> class DMul
+ void init(Vertex* a, Vertex* b, Vertex* c)
{
- private:
- static btUint32_t high(btUint64_t value)
- {
- return (btUint32_t) (value >> 32);
- }
-
- static btUint32_t low(btUint64_t value)
- {
- return (btUint32_t) value;
- }
-
- static btUint64_t mul(btUint32_t a, btUint32_t b)
- {
- return (btUint64_t) a * (btUint64_t) b;
- }
-
- static void shlHalf(btUint64_t& value)
- {
- value <<= 32;
- }
-
- static btUint64_t high(Int128 value)
- {
- return value.high;
- }
-
- static btUint64_t low(Int128 value)
- {
- return value.low;
- }
-
- static Int128 mul(btUint64_t a, btUint64_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:
+ 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;
+ }
- class IntermediateHull
+ Point64 getNormal()
{
- 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};
+ return dir0.cross(dir1);
+ }
+ };
- template <typename T> class PoolArray
+ template <typename UWord, typename UHWord>
+ class DMul
+ {
+ private:
+ static btUint32_t high(btUint64_t value)
{
- private:
- T* array;
- int size;
+ return (btUint32_t)(value >> 32);
+ }
- public:
- PoolArray<T>* next;
+ static btUint32_t low(btUint64_t value)
+ {
+ return (btUint32_t)value;
+ }
- PoolArray(int size): size(size), next(NULL)
- {
- array = (T*) b3AlignedAlloc(sizeof(T) * size, 16);
- }
+ static btUint64_t mul(btUint32_t a, btUint32_t b)
+ {
+ return (btUint64_t)a * (btUint64_t)b;
+ }
- ~PoolArray()
- {
- b3AlignedFree(array);
- }
+ static void shlHalf(btUint64_t& value)
+ {
+ value <<= 32;
+ }
- T* init()
- {
- T* o = array;
- for (int i = 0; i < size; i++, o++)
- {
- o->next = (i+1 < size) ? o + 1 : NULL;
- }
- return array;
- }
- };
+ static btUint64_t high(Int128 value)
+ {
+ return value.high;
+ }
- template <typename T> class Pool
+ static btUint64_t low(Int128 value)
{
- private:
- PoolArray<T>* arrays;
- PoolArray<T>* nextArray;
- T* freeObjects;
- int arraySize;
+ return value.low;
+ }
- public:
- Pool(): arrays(NULL), nextArray(NULL), freeObjects(NULL), arraySize(256)
- {
- }
+ static Int128 mul(btUint64_t a, btUint64_t b)
+ {
+ return Int128::mul(a, b);
+ }
- ~Pool()
- {
- while (arrays)
- {
- PoolArray<T>* p = arrays;
- arrays = p->next;
- p->~PoolArray<T>();
- b3AlignedFree(p);
- }
- }
+ static void shlHalf(Int128& value)
+ {
+ value.high = value.low;
+ value.low = 0;
+ }
- void reset()
- {
- nextArray = arrays;
- freeObjects = NULL;
- }
+ 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;
+ }
+ };
- void setArraySize(int arraySize)
- {
- this->arraySize = arraySize;
- }
+private:
+ class IntermediateHull
+ {
+ public:
+ Vertex* minXy;
+ Vertex* maxXy;
+ Vertex* minYx;
+ Vertex* maxYx;
- T* newObject()
- {
- T* o = freeObjects;
- if (!o)
- {
- PoolArray<T>* p = nextArray;
- if (p)
- {
- nextArray = p->next;
- }
- else
- {
- p = new(b3AlignedAlloc(sizeof(PoolArray<T>), 16)) PoolArray<T>(arraySize);
- p->next = arrays;
- arrays = p;
- }
- o = p->init();
- }
- freeObjects = o->next;
- return new(o) T();
- };
+ IntermediateHull() : minXy(NULL), maxXy(NULL), minYx(NULL), maxYx(NULL)
+ {
+ }
- void freeObject(T* object)
- {
- object->~T();
- object->next = freeObjects;
- freeObjects = object;
- }
- };
+ void print();
+ };
- b3Vector3 scaling;
- b3Vector3 center;
- Pool<Vertex> vertexPool;
- Pool<Edge> edgePool;
- Pool<Face> facePool;
- b3AlignedObjectArray<Vertex*> originalVertices;
- int mergeStamp;
- int minAxis;
- int medAxis;
- int maxAxis;
- int usedEdgePairs;
- int maxUsedEdgePairs;
+ enum Orientation
+ {
+ NONE,
+ CLOCKWISE,
+ COUNTER_CLOCKWISE
+ };
- 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);
+ template <typename T>
+ class PoolArray
+ {
+ private:
+ T* array;
+ int size;
- Edge* newEdgePair(Vertex* from, Vertex* to);
+ public:
+ PoolArray<T>* next;
- void removeEdgePair(Edge* edge)
+ PoolArray(int size) : size(size), next(NULL)
{
- Edge* n = edge->next;
- Edge* r = edge->reverse;
+ array = (T*)b3AlignedAlloc(sizeof(T) * size, 16);
+ }
- b3Assert(edge->target && r->target);
+ ~PoolArray()
+ {
+ b3AlignedFree(array);
+ }
- if (n != edge)
- {
- n->prev = edge->prev;
- edge->prev->next = n;
- r->target->edges = n;
- }
- else
+ T* init()
+ {
+ T* o = array;
+ for (int i = 0; i < size; i++, o++)
{
- r->target->edges = NULL;
+ o->next = (i + 1 < size) ? o + 1 : NULL;
}
-
- n = r->next;
-
- if (n != r)
+ return array;
+ }
+ };
+
+ template <typename T>
+ class Pool
+ {
+ private:
+ PoolArray<T>* arrays;
+ PoolArray<T>* nextArray;
+ T* freeObjects;
+ int arraySize;
+
+ public:
+ Pool() : arrays(NULL), nextArray(NULL), freeObjects(NULL), arraySize(256)
+ {
+ }
+
+ ~Pool()
+ {
+ while (arrays)
{
- n->prev = r->prev;
- r->prev->next = n;
- edge->target->edges = n;
+ PoolArray<T>* p = arrays;
+ arrays = p->next;
+ p->~PoolArray<T>();
+ b3AlignedFree(p);
}
- else
+ }
+
+ void reset()
+ {
+ nextArray = arrays;
+ freeObjects = NULL;
+ }
+
+ void setArraySize(int arraySize)
+ {
+ this->arraySize = arraySize;
+ }
+
+ T* newObject()
+ {
+ T* o = freeObjects;
+ if (!o)
{
- edge->target->edges = NULL;
+ PoolArray<T>* p = nextArray;
+ if (p)
+ {
+ nextArray = p->next;
+ }
+ else
+ {
+ p = new (b3AlignedAlloc(sizeof(PoolArray<T>), 16)) PoolArray<T>(arraySize);
+ p->next = arrays;
+ arrays = p;
+ }
+ o = p->init();
}
+ freeObjects = o->next;
+ return new (o) T();
+ };
- edgePool.freeObject(edge);
- edgePool.freeObject(r);
- usedEdgePairs--;
+ void freeObject(T* object)
+ {
+ object->~T();
+ object->next = freeObjects;
+ freeObjects = object;
}
-
- void computeInternal(int start, int end, IntermediateHull& result);
-
- bool mergeProjection(IntermediateHull& h0, IntermediateHull& h1, Vertex*& c0, Vertex*& c1);
-
- void merge(IntermediateHull& h0, IntermediateHull& h1);
+ };
- b3Vector3 toBtVector(const Point32& v);
+ b3Vector3 scaling;
+ b3Vector3 center;
+ Pool<Vertex> vertexPool;
+ Pool<Edge> edgePool;
+ Pool<Face> facePool;
+ b3AlignedObjectArray<Vertex*> originalVertices;
+ int mergeStamp;
+ int minAxis;
+ int medAxis;
+ int maxAxis;
+ int usedEdgePairs;
+ int maxUsedEdgePairs;
- b3Vector3 getBtNormal(Face* face);
+ 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);
- bool shiftFace(Face* face, b3Scalar amount, b3AlignedObjectArray<Vertex*> stack);
+ Edge* newEdgePair(Vertex* from, Vertex* to);
- public:
- Vertex* vertexList;
+ void removeEdgePair(Edge* edge)
+ {
+ Edge* n = edge->next;
+ Edge* r = edge->reverse;
- void compute(const void* coords, bool doubleCoords, int stride, int count);
+ b3Assert(edge->target && r->target);
- b3Vector3 getCoordinates(const Vertex* v);
+ if (n != edge)
+ {
+ n->prev = edge->prev;
+ edge->prev->next = n;
+ r->target->edges = n;
+ }
+ else
+ {
+ r->target->edges = NULL;
+ }
- b3Scalar shrink(b3Scalar amount, b3Scalar clampAmount);
-};
+ 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(int start, int end, IntermediateHull& result);
+
+ bool mergeProjection(IntermediateHull& h0, IntermediateHull& h1, Vertex*& c0, Vertex*& c1);
+
+ void merge(IntermediateHull& h0, IntermediateHull& h1);
+ b3Vector3 toBtVector(const Point32& v);
+
+ b3Vector3 getBtNormal(Face* face);
+
+ bool shiftFace(Face* face, b3Scalar amount, b3AlignedObjectArray<Vertex*> stack);
+
+public:
+ Vertex* vertexList;
+
+ void compute(const void* coords, bool doubleCoords, int stride, int count);
+
+ b3Vector3 getCoordinates(const Vertex* v);
+
+ b3Scalar shrink(b3Scalar amount, b3Scalar clampAmount);
+};
b3ConvexHullInternal::Int128 b3ConvexHullInternal::Int128::operator*(btInt64_t b) const
{
- bool negative = (btInt64_t) high < 0;
+ bool negative = (btInt64_t)high < 0;
Int128 a = negative ? -*this : *this;
if (b < 0)
{
negative = !negative;
b = -b;
}
- Int128 result = mul(a.low, (btUint64_t) b);
- result.high += a.high * (btUint64_t) b;
+ Int128 result = mul(a.low, (btUint64_t)b);
+ result.high += a.high * (btUint64_t)b;
return negative ? -result : result;
}
b3ConvexHullInternal::Int128 b3ConvexHullInternal::Int128::mul(btInt64_t a, btInt64_t b)
{
Int128 result;
-
+
#ifdef USE_X86_64_ASM
- __asm__ ("imulq %[b]"
- : "=a" (result.low), "=d" (result.high)
- : "0"(a), [b] "r"(b)
- : "cc" );
+ __asm__("imulq %[b]"
+ : "=a"(result.low), "=d"(result.high)
+ : "0"(a), [b] "r"(b)
+ : "cc");
return result;
-
+
#else
bool negative = a < 0;
if (negative)
@@ -875,7 +876,7 @@ b3ConvexHullInternal::Int128 b3ConvexHullInternal::Int128::mul(btInt64_t a, btIn
negative = !negative;
b = -b;
}
- DMul<btUint64_t, btUint32_t>::mul((btUint64_t) a, (btUint64_t) b, result.low, result.high);
+ DMul<btUint64_t, btUint32_t>::mul((btUint64_t)a, (btUint64_t)b, result.low, result.high);
return negative ? -result : result;
#endif
}
@@ -885,10 +886,10 @@ b3ConvexHullInternal::Int128 b3ConvexHullInternal::Int128::mul(btUint64_t a, btU
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<btUint64_t, btUint32_t>::mul(a, b, result.low, result.high);
@@ -915,24 +916,25 @@ int b3ConvexHullInternal::Rational64::compare(const Rational64& b) const
int result;
btInt64_t tmp;
btInt64_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;
+ __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
@@ -953,7 +955,7 @@ int b3ConvexHullInternal::Rational128::compare(const Rational128& b) const
}
if (isInt64)
{
- return -b.compare(sign * (btInt64_t) numerator.low);
+ return -b.compare(sign * (btInt64_t)numerator.low);
}
Int128 nbdLow, nbdHigh, dbnLow, dbnHigh;
@@ -972,7 +974,7 @@ int b3ConvexHullInternal::Rational128::compare(btInt64_t b) const
{
if (isInt64)
{
- btInt64_t a = sign * (btInt64_t) numerator.low;
+ btInt64_t a = sign * (btInt64_t)numerator.low;
return (a > b) ? 1 : (a < b) ? -1 : 0;
}
if (b > 0)
@@ -998,7 +1000,6 @@ int b3ConvexHullInternal::Rational128::compare(btInt64_t b) const
return numerator.ucmp(denominator * b) * sign;
}
-
b3ConvexHullInternal::Edge* b3ConvexHullInternal::newEdgePair(Vertex* from, Vertex* to)
{
b3Assert(from && to);
@@ -1066,7 +1067,7 @@ bool b3ConvexHullInternal::mergeProjection(IntermediateHull& h0, IntermediateHul
}
}
}
-
+
v0 = h0.maxXy;
v1 = h1.maxXy;
Vertex* v00 = NULL;
@@ -1074,7 +1075,7 @@ bool b3ConvexHullInternal::mergeProjection(IntermediateHull& h0, IntermediateHul
btInt32_t sign = 1;
for (int side = 0; side <= 1; side++)
- {
+ {
btInt32_t dx = (v1->point.x - v0->point.x) * sign;
if (dx > 0)
{
@@ -1117,7 +1118,7 @@ bool b3ConvexHullInternal::mergeProjection(IntermediateHull& h0, IntermediateHul
while (true)
{
btInt32_t dy = v1->point.y - v0->point.y;
-
+
Vertex* w1 = side ? v1->prev : v1->next;
if (w1 != v1)
{
@@ -1130,7 +1131,7 @@ bool b3ConvexHullInternal::mergeProjection(IntermediateHull& h0, IntermediateHul
continue;
}
}
-
+
Vertex* w0 = side ? v0->prev : v0->next;
if (w0 != v0)
{
@@ -1144,7 +1145,7 @@ bool b3ConvexHullInternal::mergeProjection(IntermediateHull& h0, IntermediateHul
continue;
}
}
-
+
break;
}
}
@@ -1170,7 +1171,7 @@ bool b3ConvexHullInternal::mergeProjection(IntermediateHull& h0, IntermediateHul
}
v1 = w1;
}
-
+
if (side == 0)
{
v00 = v0;
@@ -1196,7 +1197,7 @@ bool b3ConvexHullInternal::mergeProjection(IntermediateHull& h0, IntermediateHul
{
h0.maxXy = h1.maxXy;
}
-
+
h0.maxYx = h1.maxYx;
c0 = v00;
@@ -1300,7 +1301,7 @@ void b3ConvexHullInternal::computeInternal(int start, int end, IntermediateHull&
}
int split0 = start + n / 2;
- Point32 p = originalVertices[split0-1]->point;
+ Point32 p = originalVertices[split0 - 1]->point;
int split1 = split0;
while ((split1 < end) && (originalVertices[split1]->point == p))
{
@@ -1325,7 +1326,7 @@ void b3ConvexHullInternal::computeInternal(int start, int end, IntermediateHull&
void b3ConvexHullInternal::IntermediateHull::print()
{
b3Printf(" Hull\n");
- for (Vertex* v = minXy; v; )
+ for (Vertex* v = minXy; v;)
{
b3Printf(" ");
v->print();
@@ -1353,7 +1354,7 @@ void b3ConvexHullInternal::IntermediateHull::print()
}
}
if (minXy)
- {
+ {
minXy->copy = (minXy->copy == -1) ? -2 : -1;
minXy->printGraph();
}
@@ -1429,7 +1430,7 @@ b3ConvexHullInternal::Edge* b3ConvexHullInternal::findMaxAngle(bool ccw, const V
Point32 t = *e->target - *start;
Rational64 cot(t.dot(sxrxs), t.dot(rxs));
#ifdef DEBUG_CONVEX_HULL
- b3Printf(" Angle is %f (%d) for ", (float) b3Atan(cot.toScalar()), (int) cot.isNaN());
+ b3Printf(" Angle is %f (%d) for ", (float)b3Atan(cot.toScalar()), (int)cot.isNaN());
e->print();
#endif
if (cot.isNaN())
@@ -1476,7 +1477,7 @@ void b3ConvexHullInternal::findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge
b3Assert(!start1 || (start1->target->point.dot(normal) == dist));
Point64 perp = s.cross(normal);
b3Assert(!perp.isZero());
-
+
#ifdef DEBUG_CONVEX_HULL
b3Printf(" 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
@@ -1506,7 +1507,7 @@ void b3ConvexHullInternal::findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge
et0 = e->target->point;
}
}
-
+
btInt64_t maxDot1 = et1.dot(perp);
if (e1)
{
@@ -1543,7 +1544,7 @@ void b3ConvexHullInternal::findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge
while (true)
{
btInt64_t dy = (et1 - et0).dot(s);
-
+
if (e0 && (e0->target != stop0))
{
Edge* f0 = e0->next->reverse;
@@ -1560,7 +1561,7 @@ void b3ConvexHullInternal::findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge
}
}
}
-
+
if (e1 && (e1->target != stop1))
{
Edge* f1 = e1->reverse->next;
@@ -1595,7 +1596,7 @@ void b3ConvexHullInternal::findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge
while (true)
{
btInt64_t dy = (et1 - et0).dot(s);
-
+
if (e1 && (e1->target != stop1))
{
Edge* f1 = e1->prev->reverse;
@@ -1612,7 +1613,7 @@ void b3ConvexHullInternal::findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge
}
}
}
-
+
if (e0 && (e0->target != stop0))
{
Edge* f0 = e0->reverse->prev;
@@ -1647,7 +1648,6 @@ void b3ConvexHullInternal::findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge
#endif
}
-
void b3ConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1)
{
if (!h1.maxXy)
@@ -1659,7 +1659,7 @@ void b3ConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1)
h0 = h1;
return;
}
-
+
mergeStamp--;
Vertex* c0 = NULL;
@@ -1699,7 +1699,7 @@ void b3ConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1)
e = e->next;
} while (e != c0->edges);
}
-
+
e = c1->edges;
Edge* start1 = NULL;
if (e)
@@ -1751,7 +1751,7 @@ void b3ConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1)
Point32 r = prevPoint - c0->point;
Point64 rxs = r.cross(s);
Point64 sxrxs = s.cross(rxs);
-
+
#ifdef DEBUG_CONVEX_HULL
b3Printf("\n Checking %d %d\n", c0->point.index, c1->point.index);
#endif
@@ -1802,7 +1802,7 @@ void b3ConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1)
e->prev = pendingTail1;
pendingTail1 = e;
}
-
+
Edge* e0 = min0;
Edge* e1 = min1;
@@ -1819,7 +1819,7 @@ void b3ConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1)
{
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;
removeEdgePair(e);
@@ -1855,7 +1855,7 @@ void b3ConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1)
{
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;
removeEdgePair(e);
@@ -1897,7 +1897,7 @@ void b3ConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1)
}
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;
removeEdgePair(e);
@@ -1916,7 +1916,7 @@ void b3ConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1)
}
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;
removeEdgePair(e);
@@ -1927,7 +1927,7 @@ void b3ConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1)
pendingTail1->link(firstNew1);
}
}
-
+
return;
}
@@ -1935,7 +1935,6 @@ void b3ConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1)
}
}
-
static bool b3PointCmp(const b3ConvexHullInternal::Point32& p, const b3ConvexHullInternal::Point32& q)
{
return (p.y < q.y) || ((p.y == q.y) && ((p.x < q.x) || ((p.x == q.x) && (p.z < q.z))));
@@ -1943,14 +1942,14 @@ static bool b3PointCmp(const b3ConvexHullInternal::Point32& p, const b3ConvexHul
void b3ConvexHullInternal::compute(const void* coords, bool doubleCoords, int stride, int count)
{
- b3Vector3 min = b3MakeVector3(b3Scalar(1e30), b3Scalar(1e30), b3Scalar(1e30)), max = b3MakeVector3(b3Scalar(-1e30), b3Scalar(-1e30), b3Scalar(-1e30));
- const char* ptr = (const char*) coords;
+ b3Vector3 min = b3MakeVector3(b3Scalar(1e30), b3Scalar(1e30), b3Scalar(1e30)), max = b3MakeVector3(b3Scalar(-1e30), b3Scalar(-1e30), b3Scalar(-1e30));
+ const char* ptr = (const char*)coords;
if (doubleCoords)
{
for (int i = 0; i < count; i++)
{
- const double* v = (const double*) ptr;
- b3Vector3 p = b3MakeVector3((b3Scalar) v[0], (b3Scalar) v[1], (b3Scalar) v[2]);
+ const double* v = (const double*)ptr;
+ b3Vector3 p = b3MakeVector3((b3Scalar)v[0], (b3Scalar)v[1], (b3Scalar)v[2]);
ptr += stride;
min.setMin(p);
max.setMax(p);
@@ -1960,7 +1959,7 @@ void b3ConvexHullInternal::compute(const void* coords, bool doubleCoords, int st
{
for (int i = 0; i < count; i++)
{
- const float* v = (const float*) ptr;
+ const float* v = (const float*)ptr;
b3Vector3 p = b3MakeVector3(v[0], v[1], v[2]);
ptr += stride;
min.setMin(p);
@@ -2001,18 +2000,18 @@ void b3ConvexHullInternal::compute(const void* coords, bool doubleCoords, int st
b3AlignedObjectArray<Point32> points;
points.resize(count);
- ptr = (const char*) coords;
+ ptr = (const char*)coords;
if (doubleCoords)
{
for (int i = 0; i < count; i++)
{
- const double* v = (const double*) ptr;
- b3Vector3 p = b3MakeVector3((b3Scalar) v[0], (b3Scalar) v[1], (b3Scalar) v[2]);
+ const double* v = (const double*)ptr;
+ b3Vector3 p = b3MakeVector3((b3Scalar)v[0], (b3Scalar)v[1], (b3Scalar)v[2]);
ptr += stride;
p = (p - center) * s;
- points[i].x = (btInt32_t) p[medAxis];
- points[i].y = (btInt32_t) p[maxAxis];
- points[i].z = (btInt32_t) p[minAxis];
+ points[i].x = (btInt32_t)p[medAxis];
+ points[i].y = (btInt32_t)p[maxAxis];
+ points[i].z = (btInt32_t)p[minAxis];
points[i].index = i;
}
}
@@ -2020,13 +2019,13 @@ void b3ConvexHullInternal::compute(const void* coords, bool doubleCoords, int st
{
for (int i = 0; i < count; i++)
{
- const float* v = (const float*) ptr;
+ const float* v = (const float*)ptr;
b3Vector3 p = b3MakeVector3(v[0], v[1], v[2]);
ptr += stride;
p = (p - center) * s;
- points[i].x = (btInt32_t) p[medAxis];
- points[i].y = (btInt32_t) p[maxAxis];
- points[i].z = (btInt32_t) p[minAxis];
+ points[i].x = (btInt32_t)p[medAxis];
+ points[i].y = (btInt32_t)p[maxAxis];
+ points[i].z = (btInt32_t)p[minAxis];
points[i].index = i;
}
}
@@ -2180,7 +2179,7 @@ b3Scalar b3ConvexHullInternal::shrink(b3Scalar amount, b3Scalar clampAmount)
minDist = dist;
}
}
-
+
if (minDist <= 0)
{
return 0;
@@ -2221,7 +2220,7 @@ bool b3ConvexHullInternal::shiftFace(Face* face, b3Scalar amount, b3AlignedObjec
{
origShift[2] /= scaling[2];
}
- Point32 shift((btInt32_t) origShift[medAxis], (btInt32_t) origShift[maxAxis], (btInt32_t) origShift[minAxis]);
+ Point32 shift((btInt32_t)origShift[medAxis], (btInt32_t)origShift[maxAxis], (btInt32_t)origShift[minAxis]);
if (shift.isZero())
{
return true;
@@ -2229,7 +2228,7 @@ bool b3ConvexHullInternal::shiftFace(Face* face, b3Scalar amount, b3AlignedObjec
Point64 normal = face->getNormal();
#ifdef DEBUG_CONVEX_HULL
b3Printf("\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
btInt64_t origDot = face->origin.dot(normal);
Point32 shiftedOrigin = face->origin + shift;
@@ -2266,7 +2265,7 @@ bool b3ConvexHullInternal::shiftFace(Face* face, b3Scalar amount, b3AlignedObjec
#ifdef DEBUG_CONVEX_HULL
b3Printf("Moving downwards, edge is ");
e->print();
- b3Printf(", dot is %f (%f %lld)\n", (float) dot.toScalar(), (float) optDot.toScalar(), shiftedDot);
+ b3Printf(", dot is %f (%f %lld)\n", (float)dot.toScalar(), (float)optDot.toScalar(), shiftedDot);
#endif
if (dot.compare(optDot) < 0)
{
@@ -2302,7 +2301,7 @@ bool b3ConvexHullInternal::shiftFace(Face* face, b3Scalar amount, b3AlignedObjec
#ifdef DEBUG_CONVEX_HULL
b3Printf("Moving upwards, edge is ");
e->print();
- b3Printf(", dot is %f (%f %lld)\n", (float) dot.toScalar(), (float) optDot.toScalar(), shiftedDot);
+ b3Printf(", dot is %f (%f %lld)\n", (float)dot.toScalar(), (float)optDot.toScalar(), shiftedDot);
#endif
if (dot.compare(optDot) > 0)
{
@@ -2318,7 +2317,7 @@ bool b3ConvexHullInternal::shiftFace(Face* face, b3Scalar amount, b3AlignedObjec
}
e = e->prev;
} while (e != startEdge);
-
+
if (!intersection)
{
return true;
@@ -2355,7 +2354,7 @@ bool b3ConvexHullInternal::shiftFace(Face* face, b3Scalar amount, b3AlignedObjec
b3Printf("Needed %d iterations to check for complete containment\n", n);
#endif
}
-
+
Edge* firstIntersection = NULL;
Edge* faceEdge = NULL;
Edge* firstFaceEdge = NULL;
@@ -2464,7 +2463,7 @@ bool b3ConvexHullInternal::shiftFace(Face* face, b3Scalar amount, b3AlignedObjec
#ifdef DEBUG_CONVEX_HULL
b3Printf("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();
btInt64_t m00 = face->dir0.dot(n0);
@@ -2478,16 +2477,13 @@ bool b3ConvexHullInternal::shiftFace(Face* face, b3Scalar amount, b3AlignedObjec
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 = (btInt32_t) v->point128.xvalue();
- v->point.y = (btInt32_t) v->point128.yvalue();
- v->point.z = (btInt32_t) v->point128.zvalue();
+ 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 = (btInt32_t)v->point128.xvalue();
+ v->point.y = (btInt32_t)v->point128.yvalue();
+ v->point.z = (btInt32_t)v->point128.zvalue();
intersection->target = v;
v->edges = e;
@@ -2626,7 +2622,6 @@ bool b3ConvexHullInternal::shiftFace(Face* face, b3Scalar amount, b3AlignedObjec
return true;
}
-
static int getVertexCopy(b3ConvexHullInternal::Vertex* vertex, b3AlignedObjectArray<b3ConvexHullInternal::Vertex*>& vertices)
{
int index = vertex->copy;
@@ -2748,8 +2743,3 @@ b3Scalar b3ConvexHullComputer::compute(const void* coords, bool doubleCoords, in
return shift;
}
-
-
-
-
-
generated by cgit v1.2.3 (git 2.25.1) at 2024-12-28 09:20:38 +0000