src/LinearMath/btConvexHullComputer.cpp

/*
Copyright (c) 2011 Ole Kniemeyer, MAXON, www.maxon.net

This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:

1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/

#include <string.h>

#include "btConvexHullComputer.h"
#include "btAlignedObjectArray.h"
#include "btMinMax.h"
#include "btVector3.h"

#ifdef __GNUC__
	#include <stdint.h>
#elif defined(_MSC_VER)
	typedef __int32 int32_t;
	typedef __int64 int64_t;
	typedef unsigned __int32 uint32_t;
	typedef unsigned __int64 uint64_t;
#else
	typedef int int32_t;
	typedef long long int int64_t;
	typedef unsigned int uint32_t;
	typedef unsigned long long int uint64_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>
#endif

// Convex hull implementation based on Preparata and Hong
// Ole Kniemeyer, MAXON Computer GmbH
class btConvexHullInternal
{
	public:

		class Point64
		{
			public:
				int64_t x;
				int64_t y;
				int64_t z;

				Point64(int64_t x, int64_t y, int64_t z): x(x), y(y), z(z)
				{
				}

				bool isZero()
				{
					return (x == 0) && (y == 0) && (z == 0);
				}

				int64_t dot(const Point64& b) const
				{
					return x * b.x + y * b.y + z * b.z;
				}
		};

		class Point32
		{
			public:
				int32_t x;
				int32_t y;
				int32_t z;
				int index;

				Point32()
				{
				}

				Point32(int32_t x, int32_t y, int32_t z): x(x), y(y), z(z), index(-1)
				{
				}

				bool operator==(const Point32& b) const
				{
					return (x == b.x) && (y == b.y) && (z == b.z);
				}

				bool operator!=(const Point32& b) const
				{
					return (x != b.x) || (y != b.y) || (z != b.z);
				}

				bool isZero()
				{
					return (x == 0) && (y == 0) && (z == 0);
				}

				Point64 cross(const Point32& b) const
				{
					return Point64(y * b.z - z * b.y, z * b.x - x * b.z, x * b.y - y * b.x);
				}

				Point64 cross(const Point64& b) const
				{
					return Point64(y * b.z - z * b.y, z * b.x - x * b.z, x * b.y - y * b.x);
				}

				int64_t dot(const Point32& b) const
				{
					return x * b.x + y * b.y + z * b.z;
				}

				int64_t dot(const Point64& b) const
				{
					return x * b.x + y * b.y + z * b.z;
				}

				Point32 operator+(const Point32& b) const
				{
					return Point32(x + b.x, y + b.y, z + b.z);
				}

				Point32 operator-(const Point32& b) const
				{
					return Point32(x - b.x, y - b.y, z - b.z);
				}
		};

		class Int128
		{
			public:
				uint64_t low;
				uint64_t high;

				Int128()
				{
				}

				Int128(uint64_t low, uint64_t high): low(low), high(high)
				{
				}

				Int128(uint64_t low): low(low), high(0)
				{
				}

				Int128(int64_t value): low(value), high((value >= 0) ? 0 : (uint64_t) -1LL)
				{
				}

				static Int128 mul(int64_t a, int64_t b);

				static Int128 mul(uint64_t a, uint64_t b);

				Int128 operator-() const
				{
					return Int128((uint64_t) -(int64_t)low, ~high + (low == 0));
				}

				Int128 operator+(const Int128& b) const
				{
#ifdef USE_X86_64_ASM
					Int128 result;
					__asm__ ("addq %[bl], %[rl]\n\t"
									 "adcq %[bh], %[rh]\n\t"
									 : [rl] "=r" (result.low), [rh] "=r" (result.high)
									 : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high)
									 : "cc" );
					return result;
#else
					uint64_t lo = low + b.low;
					return Int128(lo, high + b.high + (lo < low));
#endif
				}

				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;
#else
					return *this + -b;
#endif
				}

				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" );
#else
					uint64_t lo = low + b.low;
					if (lo < low)
					{
						++high;
					}
					low = lo;
					high += b.high;
#endif
					return *this;
				}

				Int128& operator++()
				{
					if (++low == 0)
					{
						++high;
					}
					return *this;
				}

				Int128 operator*(int64_t b) const;

				btScalar toScalar() const
				{
					return ((int64_t) high >= 0) ? btScalar(high) * (btScalar(0x100000000LL) * btScalar(0x100000000LL)) + btScalar(low)
						: -(-*this).toScalar();
				}

				int getSign() const
				{
					return ((int64_t) high < 0) ? -1 : (high || low) ? 1 : 0;
				}

				bool operator<(const Int128& b) const
				{
					return (high < b.high) || ((high == b.high) && (low < b.low));
				}

				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:
				uint64_t numerator;
				uint64_t denominator;
				int sign;

			public:
				Rational64(int64_t numerator, int64_t denominator)
				{
					if (numerator > 0)
					{
						sign = 1;
						this->numerator = (uint64_t) numerator;
					}
					else if (numerator < 0)
					{
						sign = -1;
						this->numerator = (uint64_t) -numerator;
					}
					else
					{
						sign = 0;
						this->numerator = 0;
					}
					if (denominator > 0)
					{
						this->denominator = (uint64_t) denominator;
					}
					else if (denominator < 0)
					{
						sign = -sign;
						this->denominator = (uint64_t) -denominator;
					}
					else
					{
						this->denominator = 0;
					}
				}

				bool isNegativeInfinity() const
				{
					return (sign < 0) && (denominator == 0);
				}

				bool isNaN() const
				{
					return (sign == 0) && (denominator == 0);
				}

				int compare(const Rational64& b) const;

				btScalar toScalar() const
				{
					return sign * ((denominator == 0) ? SIMD_INFINITY : (btScalar) numerator / denominator);
				}
		};


		class Rational128
		{
			private:
				Int128 numerator;
				Int128 denominator;
				int sign;
				bool isInt64;

			public:
				Rational128(int64_t value)
				{
					if (value > 0)
					{
						sign = 1;
						this->numerator = value;
					}
					else if (value < 0)
					{
						sign = -1;
						this->numerator = -value;
					}
					else
					{
						sign = 0;
						this->numerator = (uint64_t) 0;
					}
					this->denominator = (uint64_t) 1;
					isInt64 = true;
				}

				Rational128(const Int128& numerator, const Int128& denominator)
				{
					sign = numerator.getSign();
					if (sign >= 0)
					{
						this->numerator = numerator;
					}
					else
					{
						this->numerator = -numerator;
					}
					int dsign = denominator.getSign();
					if (dsign >= 0)
					{
						this->denominator = denominator;
					}
					else
					{
						sign = -sign;
						this->denominator = -denominator;
					}
					isInt64 = false;
				}

				int compare(const Rational128& b) const;

				int compare(int64_t b) const;

				btScalar toScalar() const
				{
					return sign * ((denominator.getSign() == 0) ? SIMD_INFINITY : numerator.toScalar() / denominator.toScalar());
				}
		};

		class PointR128
		{
			public:
				Int128 x;
				Int128 y;
				Int128 z;
				Int128 denominator;

				PointR128()
				{
				}

				PointR128(Int128 x, Int128 y, Int128 z, Int128 denominator): x(x), y(y), z(z), denominator(denominator)
				{
				}

				btScalar xvalue() const
				{
					return x.toScalar() / denominator.toScalar();
				}

				btScalar yvalue() const
				{
					return y.toScalar() / denominator.toScalar();
				}

				btScalar zvalue() const
				{
					return z.toScalar() / denominator.toScalar();
				}
		};


		class Edge;
		class Face;

		class Vertex
		{
			public:
				Vertex* next;
				Vertex* prev;
				Edge* edges;
				Face* firstNearbyFace;
				Face* lastNearbyFace;
				PointR128 point128;
				Point32 point;
				int copy;

				Vertex(): next(NULL), prev(NULL), edges(NULL), firstNearbyFace(NULL), lastNearbyFace(NULL), copy(-1)
				{
				}

#ifdef DEBUG_CONVEX_HULL
				void print()
				{
					printf("V%d (%d, %d, %d)", point.index, point.x, point.y, point.z);
				}

				void printGraph();
#endif

				Point32 operator-(const Vertex& b) const
				{
					return point - b.point;
				}

				Rational128 dot(const Point64& b) const
				{
					return (point.index >= 0) ? Rational128(point.dot(b))
						: Rational128(point128.x * b.x + point128.y * b.y + point128.z * b.z, point128.denominator);
				}

				btScalar xvalue() const
				{
					return (point.index >= 0) ? btScalar(point.x) : point128.xvalue();
				}

				btScalar yvalue() const
				{
					return (point.index >= 0) ? btScalar(point.y) : point128.yvalue();
				}

				btScalar zvalue() const
				{
					return (point.index >= 0) ? btScalar(point.z) : point128.zvalue();
				}

				void receiveNearbyFaces(Vertex* src)
				{
					if (lastNearbyFace)
					{
						lastNearbyFace->nextWithSameNearbyVertex = src->firstNearbyFace;
					}
					else
					{
						firstNearbyFace = src->firstNearbyFace;
					}
					if (src->lastNearbyFace)
					{
						lastNearbyFace = src->lastNearbyFace;
					}
					for (Face* f = src->firstNearbyFace; f; f = f->nextWithSameNearbyVertex)
					{
						btAssert(f->nearbyVertex == src);
						f->nearbyVertex = this;
					}
					src->firstNearbyFace = NULL;
					src->lastNearbyFace = NULL;
				}
		};


		class Edge
		{
			public:
				Edge* next;
				Edge* prev;
				Edge* reverse;
				Vertex* target;
				Face* face;
				int copy;

				~Edge()
				{
					next = NULL;
					prev = NULL;
					reverse = NULL;
					target = NULL;
					face = NULL;
				}

				void link(Edge* n)
				{
					btAssert(reverse->target == n->reverse->target);
					next = n;
					n->prev = this;
				}

#ifdef DEBUG_CONVEX_HULL
				void print()
				{
					printf("E%p : %d -> %d,  n=%p p=%p   (0 %d\t%d\t%d) -> (%d %d %d)", this, reverse->target->point.index, target->point.index, next, prev,
								 reverse->target->point.x, reverse->target->point.y, reverse->target->point.z, target->point.x, target->point.y, target->point.z);
				}
#endif
		};

		class Face
		{
			public:
				Face* next;
				Vertex* nearbyVertex;
				Face* nextWithSameNearbyVertex;
				Point32 origin;
				Point32 dir0;
				Point32 dir1;

				Face(): next(NULL), nearbyVertex(NULL), nextWithSameNearbyVertex(NULL)
				{
				}

				void init(Vertex* a, Vertex* b, Vertex* c)
				{
					nearbyVertex = a;
					origin = a->point;
					dir0 = *b - *a;
					dir1 = *c - *a;
					if (a->lastNearbyFace)
					{
						a->lastNearbyFace->nextWithSameNearbyVertex = this;
					}
					else
					{
						a->firstNearbyFace = this;
					}
					a->lastNearbyFace = this;
				}

				Point64 getNormal()
				{
					return dir0.cross(dir1);
				}
		};

		template<typename UWord, typename UHWord> class DMul
		{
			private:
				static uint32_t high(uint64_t value)
				{
					return (uint32_t) (value >> 32);
				}

				static uint32_t low(uint64_t value)
				{
					return (uint32_t) value;
				}

				static uint64_t mul(uint32_t a, uint32_t b)
				{
					return (uint64_t) a * (uint64_t) b;
				}

				static void shlHalf(uint64_t& value)
				{
					value <<= 32;
				}

				static uint64_t high(Int128 value)
				{
					return value.high;
				}

				static uint64_t low(Int128 value)
				{
					return value.low;
				}

				static Int128 mul(uint64_t a, uint64_t b)
				{
					return Int128::mul(a, b);
				}

				static void shlHalf(Int128& value)
				{
					value.high = value.low;
					value.low = 0;
				}

			public:

				static void mul(UWord a, UWord b, UWord& resLow, UWord& resHigh)
				{
					UWord p00 = mul(low(a), low(b));
					UWord p01 = mul(low(a), high(b));
					UWord p10 = mul(high(a), low(b));
					UWord p11 = mul(high(a), high(b));
					UWord p0110 = UWord(low(p01)) + UWord(low(p10));
					p11 += high(p01);
					p11 += high(p10);
					p11 += high(p0110);
					shlHalf(p0110);
					p00 += p0110;
					if (p00 < p0110)
					{
						++p11;
					}
					resLow = p00;
					resHigh = p11;
				}
		};

	private:

		class IntermediateHull
		{
			public:
				Vertex* minXy;
				Vertex* maxXy;
				Vertex* minYx;
				Vertex* maxYx;

				IntermediateHull(): minXy(NULL), maxXy(NULL), minYx(NULL), maxYx(NULL)
				{
				}

				void print();
		};

		enum Orientation {NONE, CLOCKWISE, COUNTER_CLOCKWISE};

		template <typename T> class PoolArray
		{
			private:
				T* array;
				int size;

			public:
				PoolArray<T>* next;

				PoolArray(int size): size(size), next(NULL)
				{
					array = (T*) btAlignedAlloc(sizeof(T) * size, 16);
				}

				~PoolArray()
				{
					btAlignedFree(array);
				}

				T* init()
				{
					T* o = array;
					for (int i = 0; i < size; i++, o++)
					{
						o->next = (i+1 < size) ? o + 1 : NULL;
					}
					return array;
				}
		};

		template <typename T> class Pool
		{
			private:
				PoolArray<T>* arrays;
				PoolArray<T>* nextArray;
				T* freeObjects;
				int arraySize;

			public:
				Pool(): arrays(NULL), nextArray(NULL), freeObjects(NULL), arraySize(256)
				{
				}

				~Pool()
				{
					while (arrays)
					{
						PoolArray<T>* p = arrays;
						arrays = p->next;
						p->~PoolArray<T>();
						btAlignedFree(p);
					}
				}

				void reset()
				{
					nextArray = arrays;
					freeObjects = NULL;
				}

				void setArraySize(int arraySize)
				{
					this->arraySize = arraySize;
				}

				T* newObject()
				{
					T* o = freeObjects;
					if (!o)
					{
						PoolArray<T>* p = nextArray;
						if (p)
						{
							nextArray = p->next;
						}
						else
						{
							p = new(btAlignedAlloc(sizeof(PoolArray<T>), 16)) PoolArray<T>(arraySize);
							p->next = arrays;
							arrays = p;
						}
						o = p->init();
					}
					freeObjects = o->next;
					return new(o) T();
				};

				void freeObject(T* object)
				{
					object->~T();
					object->next = freeObjects;
					freeObjects = object;
				}
		};

		btVector3 scaling;
		btVector3 center;
		Pool<Vertex> vertexPool;
		Pool<Edge> edgePool;
		Pool<Face> facePool;
		btAlignedObjectArray<Vertex*> originalVertices;
		int mergeStamp;
		int minAxis;
		int medAxis;
		int maxAxis;
		int usedEdgePairs;
		int maxUsedEdgePairs;

		static Orientation getOrientation(const Edge* prev, const Edge* next, const Point32& s, const Point32& t);
		Edge* findMaxAngle(bool ccw, const Vertex* start, const Point32& s, const Point64& rxs, const Point64& sxrxs, Rational64& minCot);
		void findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge*& e0, Edge*& e1, Vertex* stop0, Vertex* stop1);

		Edge* newEdgePair(Vertex* from, Vertex* to);

		void removeEdgePair(Edge* edge)
		{
			Edge* n = edge->next;
			Edge* r = edge->reverse;

			btAssert(edge->target && r->target);

			if (n != edge)
			{
				n->prev = edge->prev;
				edge->prev->next = n;
				r->target->edges = n;
			}
			else
			{
				r->target->edges = NULL;
			}

			n = r->next;

			if (n != r)
			{
				n->prev = r->prev;
				r->prev->next = n;
				edge->target->edges = n;
			}
			else
			{
				edge->target->edges = NULL;
			}

			edgePool.freeObject(edge);
			edgePool.freeObject(r);
			usedEdgePairs--;
		}

		void computeInternal(int start, int end, IntermediateHull& result);

		bool mergeProjection(IntermediateHull& h0, IntermediateHull& h1, Vertex*& c0, Vertex*& c1);

		void merge(IntermediateHull& h0, IntermediateHull& h1);

		btVector3 toBtVector(const Point32& v);

		btVector3 getBtNormal(Face* face);

		bool shiftFace(Face* face, btScalar amount, btAlignedObjectArray<Vertex*> stack);

	public:
		Vertex* vertexList;

		void compute(const void* coords, bool doubleCoords, int stride, int count);

		btVector3 getCoordinates(const Vertex* v);

		btScalar shrink(btScalar amount, btScalar clampAmount);
};


btConvexHullInternal::Int128 btConvexHullInternal::Int128::operator*(int64_t b) const
{
	bool negative = (int64_t) high < 0;
	Int128 a = negative ? -*this : *this;
	if (b < 0)
	{
		negative = !negative;
		b = -b;
	}
	Int128 result = mul(a.low, (uint64_t) b);
	result.high += a.high * (uint64_t) b;
	return negative ? -result : result;
}

btConvexHullInternal::Int128 btConvexHullInternal::Int128::mul(int64_t a, int64_t b)
{
	Int128 result;

#ifdef USE_X86_64_ASM
	__asm__ ("imulq %[b]"
					 : "=a" (result.low), "=d" (result.high)
					 : "0"(a), [b] "r"(b)
					 : "cc" );
	return result;

#else
	bool negative = a < 0;
	if (negative)
	{
		a = -a;
	}
	if (b < 0)
	{
		negative = !negative;
		b = -b;
	}
	DMul<uint64_t, uint32_t>::mul((uint64_t) a, (uint64_t) b, result.low, result.high);
	return negative ? -result : result;
#endif
}

btConvexHullInternal::Int128 btConvexHullInternal::Int128::mul(uint64_t a, uint64_t b)
{
	Int128 result;

#ifdef USE_X86_64_ASM
	__asm__ ("mulq %[b]"
					 : "=a" (result.low), "=d" (result.high)
					 : "0"(a), [b] "r"(b)
					 : "cc" );

#else
	DMul<uint64_t, uint32_t>::mul(a, b, result.low, result.high);
#endif

	return result;
}

int btConvexHullInternal::Rational64::compare(const Rational64& b) const
{
	if (sign != b.sign)
	{
		return sign - b.sign;
	}
	else if (sign == 0)
	{
		return 0;
	}

	//	return (numerator * b.denominator > b.numerator * denominator) ? sign : (numerator * b.denominator < b.numerator * denominator) ? -sign : 0;

#ifdef USE_X86_64_ASM

	int result;
	int64_t tmp;
	int64_t dummy;
	__asm__ ("mulq %[bn]\n\t"
					 "movq %%rax, %[tmp]\n\t"
					 "movq %%rdx, %%rbx\n\t"
					 "movq %[tn], %%rax\n\t"
					 "mulq %[bd]\n\t"
					 "subq %[tmp], %%rax\n\t"
					 "sbbq %%rbx, %%rdx\n\t" // rdx:rax contains 128-bit-difference "numerator*b.denominator - b.numerator*denominator"
					 "setnsb %%bh\n\t" // bh=1 if difference is non-negative, bh=0 otherwise
					 "orq %%rdx, %%rax\n\t"
					 "setnzb %%bl\n\t" // bl=1 if difference if non-zero, bl=0 if it is zero
					 "decb %%bh\n\t" // now bx=0x0000 if difference is zero, 0xff01 if it is negative, 0x0001 if it is positive (i.e., same sign as difference)
					 "shll $16, %%ebx\n\t" // ebx has same sign as difference
					 : "=&b"(result), [tmp] "=&r"(tmp), "=a"(dummy)
					 : "a"(denominator), [bn] "g"(b.numerator), [tn] "g"(numerator), [bd] "g"(b.denominator)
					 : "%rdx", "cc" );
	return result ? result ^ sign // if sign is +1, only bit 0 of result is inverted, which does not change the sign of result (and cannot result in zero)
																// if sign is -1, all bits of result are inverted, which changes the sign of result (and again cannot result in zero)
								: 0;

#else

	return sign * Int128::mul(numerator, b.denominator).ucmp(Int128::mul(denominator, b.numerator));

#endif
}

int btConvexHullInternal::Rational128::compare(const Rational128& b) const
{
	if (sign != b.sign)
	{
		return sign - b.sign;
	}
	else if (sign == 0)
	{
		return 0;
	}
	if (isInt64)
	{
		return -b.compare(sign * (int64_t) numerator.low);
	}

	Int128 nbdLow, nbdHigh, dbnLow, dbnHigh;
	DMul<Int128, uint64_t>::mul(numerator, b.denominator, nbdLow, nbdHigh);
	DMul<Int128, uint64_t>::mul(denominator, b.numerator, dbnLow, dbnHigh);

	int cmp = nbdHigh.ucmp(dbnHigh);
	if (cmp)
	{
		return cmp * sign;
	}
	return nbdLow.ucmp(dbnLow) * sign;
}

int btConvexHullInternal::Rational128::compare(int64_t b) const
{
	if (isInt64)
	{
		int64_t a = sign * (int64_t) numerator.low;
		return (a > b) ? 1 : (a < b) ? -1 : 0;
	}
	if (b > 0)
	{
		if (sign <= 0)
		{
			return -1;
		}
	}
	else if (b < 0)
	{
		if (sign >= 0)
		{
			return 1;
		}
		b = -b;
	}
	else
	{
		return sign;
	}

	return numerator.ucmp(denominator * b) * sign;
}


btConvexHullInternal::Edge* btConvexHullInternal::newEdgePair(Vertex* from, Vertex* to)
{
	btAssert(from && to);
	Edge* e = edgePool.newObject();
	Edge* r = edgePool.newObject();
	e->reverse = r;
	r->reverse = e;
	e->copy = mergeStamp;
	r->copy = mergeStamp;
	e->target = to;
	r->target = from;
	e->face = NULL;
	r->face = NULL;
	usedEdgePairs++;
	if (usedEdgePairs > maxUsedEdgePairs)
	{
		maxUsedEdgePairs = usedEdgePairs;
	}
	return e;
}

bool btConvexHullInternal::mergeProjection(IntermediateHull& h0, IntermediateHull& h1, Vertex*& c0, Vertex*& c1)
{
	Vertex* v0 = h0.maxYx;
	Vertex* v1 = h1.minYx;
	if ((v0->point.x == v1->point.x) && (v0->point.y == v1->point.y))
	{
		btAssert(v0->point.z < v1->point.z);
		Vertex* v1p = v1->prev;
		if (v1p == v1)
		{
			c0 = v0;
			if (v1->edges)
			{
				btAssert(v1->edges->next == v1->edges);
				v1 = v1->edges->target;
				btAssert(v1->edges->next == v1->edges);
			}
			c1 = v1;
			return false;
		}
		Vertex* v1n = v1->next;
		v1p->next = v1n;
		v1n->prev = v1p;
		if (v1 == h1.minXy)
		{
			if ((v1n->point.x < v1p->point.x) || ((v1n->point.x == v1p->point.x) && (v1n->point.y < v1p->point.y)))
			{
				h1.minXy = v1n;
			}
			else
			{
				h1.minXy = v1p;
			}
		}
		if (v1 == h1.maxXy)
		{
			if ((v1n->point.x > v1p->point.x) || ((v1n->point.x == v1p->point.x) && (v1n->point.y > v1p->point.y)))
			{
				h1.maxXy = v1n;
			}
			else
			{
				h1.maxXy = v1p;
			}
		}
	}

	v0 = h0.maxXy;
	v1 = h1.maxXy;
	Vertex* v00 = NULL;
	Vertex* v10 = NULL;
	int32_t sign = 1;

	for (int side = 0; side <= 1; side++)
	{
		int32_t dx = (v1->point.x - v0->point.x) * sign;
		if (dx > 0)
		{
			while (true)
			{
				int32_t dy = v1->point.y - v0->point.y;

				Vertex* w0 = side ? v0->next : v0->prev;
				if (w0 != v0)
				{
					int32_t dx0 = (w0->point.x - v0->point.x) * sign;
					int32_t dy0 = w0->point.y - v0->point.y;
					if ((dy0 <= 0) && ((dx0 == 0) || ((dx0 < 0) && (dy0 * dx <= dy * dx0))))
					{
						v0 = w0;
						dx = (v1->point.x - v0->point.x) * sign;
						continue;
					}
				}

				Vertex* w1 = side ? v1->next : v1->prev;
				if (w1 != v1)
				{
					int32_t dx1 = (w1->point.x - v1->point.x) * sign;
					int32_t dy1 = w1->point.y - v1->point.y;
					int32_t dxn = (w1->point.x - v0->point.x) * sign;
					if ((dxn > 0) && (dy1 < 0) && ((dx1 == 0) || ((dx1 < 0) && (dy1 * dx < dy * dx1))))
					{
						v1 = w1;
						dx = dxn;
						continue;
					}
				}

				break;
			}
		}
		else if (dx < 0)
		{
			while (true)
			{
				int32_t dy = v1->point.y - v0->point.y;

				Vertex* w1 = side ? v1->prev : v1->next;
				if (w1 != v1)
				{
					int32_t dx1 = (w1->point.x - v1->point.x) * sign;
					int32_t dy1 = w1->point.y - v1->point.y;
					if ((dy1 >= 0) && ((dx1 == 0) || ((dx1 < 0) && (dy1 * dx <= dy * dx1))))
					{
						v1 = w1;
						dx = (v1->point.x - v0->point.x) * sign;
						continue;
					}
				}

				Vertex* w0 = side ? v0->prev : v0->next;
				if (w0 != v0)
				{
					int32_t dx0 = (w0->point.x - v0->point.x) * sign;
					int32_t dy0 = w0->point.y - v0->point.y;
					int32_t dxn = (v1->point.x - w0->point.x) * sign;
					if ((dxn < 0) && (dy0 > 0) && ((dx0 == 0) || ((dx0 < 0) && (dy0 * dx < dy * dx0))))
					{
						v0 = w0;
						dx = dxn;
						continue;
					}
				}

				break;
			}
		}
		else
		{
			int32_t x = v0->point.x;
			int32_t y0 = v0->point.y;
			Vertex* w0 = v0;
			Vertex* t;
			while (((t = side ? w0->next : w0->prev) != v0) && (t->point.x == x) && (t->point.y <= y0))
			{
				w0 = t;
				y0 = t->point.y;
			}
			v0 = w0;

			int32_t y1 = v1->point.y;
			Vertex* w1 = v1;
			while (((t = side ? w1->prev : w1->next) != v1) && (t->point.x == x) && (t->point.y >= y1))
			{
				w1 = t;
				y1 = t->point.y;
			}
			v1 = w1;
		}

		if (side == 0)
		{
			v00 = v0;
			v10 = v1;

			v0 = h0.minXy;
			v1 = h1.minXy;
			sign = -1;
		}
	}

	v0->prev = v1;
	v1->next = v0;

	v00->next = v10;
	v10->prev = v00;

	if (h1.minXy->point.x < h0.minXy->point.x)
	{
		h0.minXy = h1.minXy;
	}
	if (h1.maxXy->point.x >= h0.maxXy->point.x)
	{
		h0.maxXy = h1.maxXy;
	}

	h0.maxYx = h1.maxYx;

	c0 = v00;
	c1 = v10;

	return true;
}

void btConvexHullInternal::computeInternal(int start, int end, IntermediateHull& result)
{
	int n = end - start;
	switch (n)
	{
		case 0:
			result.minXy = NULL;
			result.maxXy = NULL;
			result.minYx = NULL;
			result.maxYx = NULL;
			return;
		case 2:
		{
			Vertex* v = originalVertices[start];
			Vertex* w = v + 1;
			if (v->point != w->point)
			{
				int32_t dx = v->point.x - w->point.x;
				int32_t dy = v->point.y - w->point.y;

				if ((dx == 0) && (dy == 0))
				{
					if (v->point.z > w->point.z)
					{
						Vertex* t = w;
						w = v;
						v = t;
					}
					btAssert(v->point.z < w->point.z);
					v->next = v;
					v->prev = v;
					result.minXy = v;
					result.maxXy = v;
					result.minYx = v;
					result.maxYx = v;
				}
				else
				{
					v->next = w;
					v->prev = w;
					w->next = v;
					w->prev = v;

					if ((dx < 0) || ((dx == 0) && (dy < 0)))
					{
						result.minXy = v;
						result.maxXy = w;
					}
					else
					{
						result.minXy = w;
						result.maxXy = v;
					}

					if ((dy < 0) || ((dy == 0) && (dx < 0)))
					{
						result.minYx = v;
						result.maxYx = w;
					}
					else
					{
						result.minYx = w;
						result.maxYx = v;
					}
				}

				Edge* e = newEdgePair(v, w);
				e->link(e);
				v->edges = e;

				e = e->reverse;
				e->link(e);
				w->edges = e;

				return;
			}
		}
		// lint -fallthrough
		case 1:
		{
			Vertex* v = originalVertices[start];
			v->edges = NULL;
			v->next = v;
			v->prev = v;

			result.minXy = v;
			result.maxXy = v;
			result.minYx = v;
			result.maxYx = v;

			return;
		}
	}

	int split0 = start + n / 2;
	Point32 p = originalVertices[split0-1]->point;
	int split1 = split0;
	while ((split1 < end) && (originalVertices[split1]->point == p))
	{
		split1++;
	}
	computeInternal(start, split0, result);
	IntermediateHull hull1;
	computeInternal(split1, end, hull1);
#ifdef DEBUG_CONVEX_HULL
	printf("\n\nMerge\n");
	result.print();
	hull1.print();
#endif
	merge(result, hull1);
#ifdef DEBUG_CONVEX_HULL
	printf("\n  Result\n");
	result.print();
#endif
}

#ifdef DEBUG_CONVEX_HULL
void btConvexHullInternal::IntermediateHull::print()
{
	printf("    Hull\n");
	for (Vertex* v = minXy; v; )
	{
		printf("      ");
		v->print();
		if (v == maxXy)
		{
			printf(" maxXy");
		}
		if (v == minYx)
		{
			printf(" minYx");
		}
		if (v == maxYx)
		{
			printf(" maxYx");
		}
		if (v->next->prev != v)
		{
			printf(" Inconsistency");
		}
		printf("\n");
		v = v->next;
		if (v == minXy)
		{
			break;
		}
	}
	if (minXy)
	{
		minXy->copy = (minXy->copy == -1) ? -2 : -1;
		minXy->printGraph();
	}
}

void btConvexHullInternal::Vertex::printGraph()
{
	print();
	printf("\nEdges\n");
	Edge* e = edges;
	if (e)
	{
		do
		{
			e->print();
			printf("\n");
			e = e->next;
		} while (e != edges);
		do
		{
			Vertex* v = e->target;
			if (v->copy != copy)
			{
				v->copy = copy;
				v->printGraph();
			}
			e = e->next;
		} while (e != edges);
	}
}
#endif

btConvexHullInternal::Orientation btConvexHullInternal::getOrientation(const Edge* prev, const Edge* next, const Point32& s, const Point32& t)
{
	btAssert(prev->reverse->target == next->reverse->target);
	if (prev->next == next)
	{
		if (prev->prev == next)
		{
			Point64 n = t.cross(s);
			Point64 m = (*prev->target - *next->reverse->target).cross(*next->target - *next->reverse->target);
			btAssert(!m.isZero());
			int64_t dot = n.dot(m);
			btAssert(dot != 0);
			return (dot > 0) ? COUNTER_CLOCKWISE : CLOCKWISE;
		}
		return COUNTER_CLOCKWISE;
	}
	else if (prev->prev == next)
	{
		return CLOCKWISE;
	}
	else
	{
		return NONE;
	}
}

btConvexHullInternal::Edge* btConvexHullInternal::findMaxAngle(bool ccw, const Vertex* start, const Point32& s, const Point64& rxs, const Point64& sxrxs, Rational64& minCot)
{
	Edge* minEdge = NULL;

#ifdef DEBUG_CONVEX_HULL
	printf("find max edge for %d\n", start->point.index);
#endif
	Edge* e = start->edges;
	if (e)
	{
		do
		{
			if (e->copy > mergeStamp)
			{
				Point32 t = *e->target - *start;
				Rational64 cot(t.dot(sxrxs), t.dot(rxs));
#ifdef DEBUG_CONVEX_HULL
				printf("      Angle is %f (%d) for ", (float) btAtan(cot.toScalar()), (int) cot.isNaN());
				e->print();
#endif
				if (cot.isNaN())
				{
					btAssert(ccw ? (t.dot(s) < 0) : (t.dot(s) > 0));
				}
				else
				{
					int cmp;
					if (minEdge == NULL)
					{
						minCot = cot;
						minEdge = e;
					}
					else if ((cmp = cot.compare(minCot)) < 0)
					{
						minCot = cot;
						minEdge = e;
					}
					else if ((cmp == 0) && (ccw == (getOrientation(minEdge, e, s, t) == COUNTER_CLOCKWISE)))
					{
						minEdge = e;
					}
				}
#ifdef DEBUG_CONVEX_HULL
				printf("\n");
#endif
			}
			e = e->next;
		} while (e != start->edges);
	}
	return minEdge;
}

void btConvexHullInternal::findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge*& e0, Edge*& e1, Vertex* stop0, Vertex* stop1)
{
	Edge* start0 = e0;
	Edge* start1 = e1;
	Point32 et0 = start0 ? start0->target->point : c0->point;
	Point32 et1 = start1 ? start1->target->point : c1->point;
	Point32 s = c1->point - c0->point;
	Point64 normal = ((start0 ? start0 : start1)->target->point - c0->point).cross(s);
	int64_t dist = c0->point.dot(normal);
	btAssert(!start1 || (start1->target->point.dot(normal) == dist));
	Point64 perp = s.cross(normal);
	btAssert(!perp.isZero());

#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);
#endif

	int64_t maxDot0 = et0.dot(perp);
	if (e0)
	{
		while (e0->target != stop0)
		{
			Edge* e = e0->reverse->prev;
			if (e->target->point.dot(normal) < dist)
			{
				break;
			}
			btAssert(e->target->point.dot(normal) == dist);
			if (e->copy == mergeStamp)
			{
				break;
			}
			int64_t dot = e->target->point.dot(perp);
			if (dot <= maxDot0)
			{
				break;
			}
			maxDot0 = dot;
			e0 = e;
			et0 = e->target->point;
		}
	}

	int64_t maxDot1 = et1.dot(perp);
	if (e1)
	{
		while (e1->target != stop1)
		{
			Edge* e = e1->reverse->next;
			if (e->target->point.dot(normal) < dist)
			{
				break;
			}
			btAssert(e->target->point.dot(normal) == dist);
			if (e->copy == mergeStamp)
			{
				break;
			}
			int64_t dot = e->target->point.dot(perp);
			if (dot <= maxDot1)
			{
				break;
			}
			maxDot1 = dot;
			e1 = e;
			et1 = e->target->point;
		}
	}

#ifdef DEBUG_CONVEX_HULL
	printf("   Starting at %d %d\n", et0.index, et1.index);
#endif

	int64_t dx = maxDot1 - maxDot0;
	if (dx > 0)
	{
		while (true)
		{
			int64_t dy = (et1 - et0).dot(s);

			if (e0 && (e0->target != stop0))
			{
				Edge* f0 = e0->next->reverse;
				if (f0->copy > mergeStamp)
				{
					int64_t dx0 = (f0->target->point - et0).dot(perp);
					int64_t dy0 = (f0->target->point - et0).dot(s);
					if ((dx0 == 0) ? (dy0 < 0) : ((dx0 < 0) && (Rational64(dy0, dx0).compare(Rational64(dy, dx)) >= 0)))
					{
						et0 = f0->target->point;
						dx = (et1 - et0).dot(perp);
						e0 = (e0 == start0) ? NULL : f0;
						continue;
					}
				}
			}

			if (e1 && (e1->target != stop1))
			{
				Edge* f1 = e1->reverse->next;
				if (f1->copy > mergeStamp)
				{
					Point32 d1 = f1->target->point - et1;
					if (d1.dot(normal) == 0)
					{
						int64_t dx1 = d1.dot(perp);
						int64_t dy1 = d1.dot(s);
						int64_t dxn = (f1->target->point - et0).dot(perp);
						if ((dxn > 0) && ((dx1 == 0) ? (dy1 < 0) : ((dx1 < 0) && (Rational64(dy1, dx1).compare(Rational64(dy, dx)) > 0))))
						{
							e1 = f1;
							et1 = e1->target->point;
							dx = dxn;
							continue;
						}
					}
					else
					{
						btAssert((e1 == start1) && (d1.dot(normal) < 0));
					}
				}
			}

			break;
		}
	}
	else if (dx < 0)
	{
		while (true)
		{
			int64_t dy = (et1 - et0).dot(s);

			if (e1 && (e1->target != stop1))
			{
				Edge* f1 = e1->prev->reverse;
				if (f1->copy > mergeStamp)
				{
					int64_t dx1 = (f1->target->point - et1).dot(perp);
					int64_t dy1 = (f1->target->point - et1).dot(s);
					if ((dx1 == 0) ? (dy1 > 0) : ((dx1 < 0) && (Rational64(dy1, dx1).compare(Rational64(dy, dx)) <= 0)))
					{
						et1 = f1->target->point;
						dx = (et1 - et0).dot(perp);
						e1 = (e1 == start1) ? NULL : f1;
						continue;
					}
				}
			}

			if (e0 && (e0->target != stop0))
			{
				Edge* f0 = e0->reverse->prev;
				if (f0->copy > mergeStamp)
				{
					Point32 d0 = f0->target->point - et0;
					if (d0.dot(normal) == 0)
					{
						int64_t dx0 = d0.dot(perp);
						int64_t dy0 = d0.dot(s);
						int64_t dxn = (et1 - f0->target->point).dot(perp);
						if ((dxn < 0) && ((dx0 == 0) ? (dy0 > 0) : ((dx0 < 0) && (Rational64(dy0, dx0).compare(Rational64(dy, dx)) < 0))))
						{
							e0 = f0;
							et0 = e0->target->point;
							dx = dxn;
							continue;
						}
					}
					else
					{
						btAssert((e0 == start0) && (d0.dot(normal) < 0));
					}
				}
			}

			break;
		}
	}
#ifdef DEBUG_CONVEX_HULL
	printf("   Advanced edges to %d %d\n", et0.index, et1.index);
#endif
}


void btConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1)
{
	if (!h1.maxXy)
	{
		return;
	}
	if (!h0.maxXy)
	{
		h0 = h1;
		return;
	}

	mergeStamp--;

	Vertex* c0 = NULL;
	Edge* toPrev0 = NULL;
	Edge* firstNew0 = NULL;
	Edge* pendingHead0 = NULL;
	Edge* pendingTail0 = NULL;
	Vertex* c1 = NULL;
	Edge* toPrev1 = NULL;
	Edge* firstNew1 = NULL;
	Edge* pendingHead1 = NULL;
	Edge* pendingTail1 = NULL;
	Point32 prevPoint;

	if (mergeProjection(h0, h1, c0, c1))
	{
		Point32 s = *c1 - *c0;
		Point64 normal = Point32(0, 0, -1).cross(s);
		Point64 t = s.cross(normal);
		btAssert(!t.isZero());

		Edge* e = c0->edges;
		Edge* start0 = NULL;
		if (e)
		{
			do
			{
				int64_t dot = (*e->target - *c0).dot(normal);
				btAssert(dot <= 0);
				if ((dot == 0) && ((*e->target - *c0).dot(t) > 0))
				{
					if (!start0 || (getOrientation(start0, e, s, Point32(0, 0, -1)) == CLOCKWISE))
					{
						start0 = e;
					}
				}
				e = e->next;
			} while (e != c0->edges);
		}

		e = c1->edges;
		Edge* start1 = NULL;
		if (e)
		{
			do
			{
				int64_t dot = (*e->target - *c1).dot(normal);
				btAssert(dot <= 0);
				if ((dot == 0) && ((*e->target - *c1).dot(t) > 0))
				{
					if (!start1 || (getOrientation(start1, e, s, Point32(0, 0, -1)) == COUNTER_CLOCKWISE))
					{
						start1 = e;
					}
				}
				e = e->next;
			} while (e != c1->edges);
		}

		if (start0 || start1)
		{
			findEdgeForCoplanarFaces(c0, c1, start0, start1, NULL, NULL);
			if (start0)
			{
				c0 = start0->target;
			}
			if (start1)
			{
				c1 = start1->target;
			}
		}

		prevPoint = c1->point;
		prevPoint.z++;
	}
	else
	{
		prevPoint = c1->point;
		prevPoint.x++;
	}

	Vertex* first0 = c0;
	Vertex* first1 = c1;
	bool firstRun = true;

	while (true)
	{
		Point32 s = *c1 - *c0;
		Point32 r = prevPoint - c0->point;
		Point64 rxs = r.cross(s);
		Point64 sxrxs = s.cross(rxs);

#ifdef DEBUG_CONVEX_HULL
		printf("\n  Checking %d %d\n", c0->point.index, c1->point.index);
#endif
		Rational64 minCot0(0, 0);
		Edge* min0 = findMaxAngle(false, c0, s, rxs, sxrxs, minCot0);
		Rational64 minCot1(0, 0);
		Edge* min1 = findMaxAngle(true, c1, s, rxs, sxrxs, minCot1);
		if (!min0 && !min1)
		{
			Edge* e = newEdgePair(c0, c1);
			e->link(e);
			c0->edges = e;

			e = e->reverse;
			e->link(e);
			c1->edges = e;
			return;
		}
		else
		{
			int cmp = !min0 ? 1 : !min1 ? -1 : minCot0.compare(minCot1);
#ifdef DEBUG_CONVEX_HULL
			printf("    -> Result %d\n", cmp);
#endif
			if (firstRun || ((cmp >= 0) ? !minCot1.isNegativeInfinity() : !minCot0.isNegativeInfinity()))
			{
				Edge* e = newEdgePair(c0, c1);
				if (pendingTail0)
				{
					pendingTail0->prev = e;
				}
				else
				{
					pendingHead0 = e;
				}
				e->next = pendingTail0;
				pendingTail0 = e;

				e = e->reverse;
				if (pendingTail1)
				{
					pendingTail1->next = e;
				}
				else
				{
					pendingHead1 = e;
				}
				e->prev = pendingTail1;
				pendingTail1 = e;
			}

			Edge* e0 = min0;
			Edge* e1 = min1;

#ifdef DEBUG_CONVEX_HULL
			printf("   Found min edges to %d %d\n", e0 ? e0->target->point.index : -1, e1 ? e1->target->point.index : -1);
#endif

			if (cmp == 0)
			{
				findEdgeForCoplanarFaces(c0, c1, e0, e1, NULL, NULL);
			}

			if ((cmp >= 0) && e1)
			{
				if (toPrev1)
				{
					for (Edge* e = toPrev1->next, *n = NULL; e != min1; e = n)
					{
						n = e->next;
						removeEdgePair(e);
					}
				}

				if (pendingTail1)
				{
					if (toPrev1)
					{
						toPrev1->link(pendingHead1);
					}
					else
					{
						min1->prev->link(pendingHead1);
						firstNew1 = pendingHead1;
					}
					pendingTail1->link(min1);
					pendingHead1 = NULL;
					pendingTail1 = NULL;
				}
				else if (!toPrev1)
				{
					firstNew1 = min1;
				}

				prevPoint = c1->point;
				c1 = e1->target;
				toPrev1 = e1->reverse;
			}

			if ((cmp <= 0) && e0)
			{
				if (toPrev0)
				{
					for (Edge* e = toPrev0->prev, *n = NULL; e != min0; e = n)
					{
						n = e->prev;
						removeEdgePair(e);
					}
				}

				if (pendingTail0)
				{
					if (toPrev0)
					{
						pendingHead0->link(toPrev0);
					}
					else
					{
						pendingHead0->link(min0->next);
						firstNew0 = pendingHead0;
					}
					min0->link(pendingTail0);
					pendingHead0 = NULL;
					pendingTail0 = NULL;
				}
				else if (!toPrev0)
				{
					firstNew0 = min0;
				}

				prevPoint = c0->point;
				c0 = e0->target;
				toPrev0 = e0->reverse;
			}
		}

		if ((c0 == first0) && (c1 == first1))
		{
			if (toPrev0 == NULL)
			{
				pendingHead0->link(pendingTail0);
				c0->edges = pendingTail0;
			}
			else
			{
				for (Edge* e = toPrev0->prev, *n = NULL; e != firstNew0; e = n)
				{
					n = e->prev;
					removeEdgePair(e);
				}
				if (pendingTail0)
				{
					pendingHead0->link(toPrev0);
					firstNew0->link(pendingTail0);
				}
			}

			if (toPrev1 == NULL)
			{
				pendingTail1->link(pendingHead1);
				c1->edges = pendingTail1;
			}
			else
			{
				for (Edge* e = toPrev1->next, *n = NULL; e != firstNew1; e = n)
				{
					n = e->next;
					removeEdgePair(e);
				}
				if (pendingTail1)
				{
					toPrev1->link(pendingHead1);
					pendingTail1->link(firstNew1);
				}
			}

			return;
		}

		firstRun = false;
	}
}


static bool pointCmp(const btConvexHullInternal::Point32& p, const btConvexHullInternal::Point32& q)
{
	return (p.y < q.y) || ((p.y == q.y) && ((p.x < q.x) || ((p.x == q.x) && (p.z < q.z))));
}

void btConvexHullInternal::compute(const void* coords, bool doubleCoords, int stride, int count)
{
	btVector3 min(btScalar(1e30), btScalar(1e30), btScalar(1e30)), max(btScalar(-1e30), btScalar(-1e30), btScalar(-1e30));
	const char* ptr = (const char*) coords;
	if (doubleCoords)
	{
		for (int i = 0; i < count; i++)
		{
			const double* v = (const double*) ptr;
			btVector3 p((btScalar) v[0], (btScalar) v[1], (btScalar) v[2]);
			ptr += stride;
			min.setMin(p);
			max.setMax(p);
		}
	}
	else
	{
		for (int i = 0; i < count; i++)
		{
			const float* v = (const float*) ptr;
			btVector3 p(v[0], v[1], v[2]);
			ptr += stride;
			min.setMin(p);
			max.setMax(p);
		}
	}

	btVector3 s = max - min;
	maxAxis = s.maxAxis();
	minAxis = s.minAxis();
	if (minAxis == maxAxis)
	{
		minAxis = (maxAxis + 1) % 3;
	}
	medAxis = 3 - maxAxis - minAxis;

	s /= btScalar(10216);

	scaling = s;
	if (s[0] > 0)
	{
		s[0] = btScalar(1) / s[0];
	}
	if (s[1] > 0)
	{
		s[1] = btScalar(1) / s[1];
	}
	if (s[2] > 0)
	{
		s[2] = btScalar(1) / s[2];
	}

	center = (min + max) * btScalar(0.5);

	btAlignedObjectArray<Point32> points;
	points.resize(count);
	ptr = (const char*) coords;
	if (doubleCoords)
	{
		for (int i = 0; i < count; i++)
		{
			const double* v = (const double*) ptr;
			btVector3 p((btScalar) v[0], (btScalar) v[1], (btScalar) v[2]);
			ptr += stride;
			p = (p - center) * s;
			points[i].x = (int32_t) p[medAxis];
			points[i].y = (int32_t) p[maxAxis];
			points[i].z = (int32_t) p[minAxis];
			points[i].index = i;
		}
	}
	else
	{
		for (int i = 0; i < count; i++)
		{
			const float* v = (const float*) ptr;
			btVector3 p(v[0], v[1], v[2]);
			ptr += stride;
			p = (p - center) * s;
			points[i].x = (int32_t) p[medAxis];
			points[i].y = (int32_t) p[maxAxis];
			points[i].z = (int32_t) p[minAxis];
			points[i].index = i;
		}
	}
	points.quickSort(pointCmp);

	vertexPool.reset();
	vertexPool.setArraySize(count);
	originalVertices.resize(count);
	for (int i = 0; i < count; i++)
	{
		Vertex* v = vertexPool.newObject();
		v->edges = NULL;
		v->point = points[i];
		v->copy = -1;
		originalVertices[i] = v;
	}

	points.clear();

	edgePool.reset();
	edgePool.setArraySize(6 * count);

	usedEdgePairs = 0;
	maxUsedEdgePairs = 0;

	mergeStamp = -3;

	IntermediateHull hull;
	computeInternal(0, count, hull);
	vertexList = hull.minXy;
#ifdef DEBUG_CONVEX_HULL
	printf("max. edges %d (3v = %d)", maxUsedEdgePairs, 3 * count);
#endif
}

btVector3 btConvexHullInternal::toBtVector(const Point32& v)
{
	btVector3 p;
	p[medAxis] = btScalar(v.x);
	p[maxAxis] = btScalar(v.y);
	p[minAxis] = btScalar(v.z);
	return p * scaling;
}

btVector3 btConvexHullInternal::getBtNormal(Face* face)
{
	btVector3 normal = toBtVector(face->dir0).cross(toBtVector(face->dir1));
	normal /= ((medAxis + 1 == maxAxis) || (medAxis - 2 == maxAxis)) ? normal.length() : -normal.length();
	return normal;
}

btVector3 btConvexHullInternal::getCoordinates(const Vertex* v)
{
	btVector3 p;
	p[medAxis] = v->xvalue();
	p[maxAxis] = v->yvalue();
	p[minAxis] = v->zvalue();
	return p * scaling + center;
}

btScalar btConvexHullInternal::shrink(btScalar amount, btScalar clampAmount)
{
	if (!vertexList)
	{
		return 0;
	}
	int stamp = --mergeStamp;
	btAlignedObjectArray<Vertex*> stack;
	vertexList->copy = stamp;
	stack.push_back(vertexList);
	btAlignedObjectArray<Face*> faces;

	Point32 ref = vertexList->point;
	Int128 hullCenterX(0, 0);
	Int128 hullCenterY(0, 0);
	Int128 hullCenterZ(0, 0);
	Int128 volume(0, 0);

	while (stack.size() > 0)
	{
		Vertex* v = stack[stack.size() - 1];
		stack.pop_back();
		Edge* e = v->edges;
		if (e)
		{
			do
			{
				if (e->target->copy != stamp)
				{
					e->target->copy = stamp;
					stack.push_back(e->target);
				}
				if (e->copy != stamp)
				{
					Face* face = facePool.newObject();
					face->init(e->target, e->reverse->prev->target, v);
					faces.push_back(face);
					Edge* f = e;

					Vertex* a = NULL;
					Vertex* b = NULL;
					do
					{
						if (a && b)
						{
							int64_t vol = (v->point - ref).dot((a->point - ref).cross(b->point - ref));
							btAssert(vol >= 0);
							Point32 c = v->point + a->point + b->point + ref;
							hullCenterX += vol * c.x;
							hullCenterY += vol * c.y;
							hullCenterZ += vol * c.z;
							volume += vol;
						}

						btAssert(f->copy != stamp);
						f->copy = stamp;
						f->face = face;

						a = b;
						b = f->target;

						f = f->reverse->prev;
					} while (f != e);
				}
				e = e->next;
			} while (e != v->edges);
		}
	}

	if (volume.getSign() <= 0)
	{
		return 0;
	}

	btVector3 hullCenter;
	hullCenter[medAxis] = hullCenterX.toScalar();
	hullCenter[maxAxis] = hullCenterY.toScalar();
	hullCenter[minAxis] = hullCenterZ.toScalar();
	hullCenter /= 4 * volume.toScalar();
	hullCenter *= scaling;

	int faceCount = faces.size();

	if (clampAmount > 0)
	{
		btScalar minDist = SIMD_INFINITY;
		for (int i = 0; i < faceCount; i++)
		{
			btVector3 normal = getBtNormal(faces[i]);
			btScalar dist = normal.dot(toBtVector(faces[i]->origin) - hullCenter);
			if (dist < minDist)
			{
				minDist = dist;
			}
		}

		if (minDist <= 0)
		{
			return 0;
		}

		amount = btMin(amount, minDist * clampAmount);
	}

	unsigned int seed = 243703;
	for (int i = 0; i < faceCount; i++, seed = 1664525 * seed + 1013904223)
	{
		btSwap(faces[i], faces[seed % faceCount]);
	}

	for (int i = 0; i < faceCount; i++)
	{
		if (!shiftFace(faces[i], amount, stack))
		{
			return -amount;
		}
	}

	return amount;
}

bool btConvexHullInternal::shiftFace(Face* face, btScalar amount, btAlignedObjectArray<Vertex*> stack)
{
	btVector3 origShift = getBtNormal(face) * -amount;
	if (scaling[0] > 0)
	{
		origShift[0] /= scaling[0];
	}
	if (scaling[1] > 0)
	{
		origShift[1] /= scaling[1];
	}
	if (scaling[2] > 0)
	{
		origShift[2] /= scaling[2];
	}
	Point32 shift((int32_t) origShift[medAxis], (int32_t) origShift[maxAxis], (int32_t) origShift[minAxis]);
	if (shift.isZero())
	{
		return true;
	}
	Point64 normal = face->getNormal();
#ifdef DEBUG_CONVEX_HULL
	printf("\nShrinking face (%d %d %d) (%d %d %d) (%d %d %d) by (%d %d %d)\n",
				 face->origin.x, face->origin.y, face->origin.z, face->dir0.x, face->dir0.y, face->dir0.z, face->dir1.x, face->dir1.y, face->dir1.z, shift.x, shift.y, shift.z);
#endif
	int64_t origDot = face->origin.dot(normal);
	Point32 shiftedOrigin = face->origin + shift;
	int64_t shiftedDot = shiftedOrigin.dot(normal);
	btAssert(shiftedDot <= origDot);
	if (shiftedDot >= origDot)
	{
		return false;
	}

	Edge* intersection = NULL;

	Edge* startEdge = face->nearbyVertex->edges;
#ifdef DEBUG_CONVEX_HULL
	printf("Start edge is ");
	startEdge->print();
	printf(", normal is (%lld %lld %lld), shifted dot is %lld\n", normal.x, normal.y, normal.z, shiftedDot);
#endif
	Rational128 optDot = face->nearbyVertex->dot(normal);
	int cmp = optDot.compare(shiftedDot);
#ifdef SHOW_ITERATIONS
	int n = 0;
#endif
	if (cmp >= 0)
	{
		Edge* e = startEdge;
		do
		{
#ifdef SHOW_ITERATIONS
			n++;
#endif
			Rational128 dot = e->target->dot(normal);
			btAssert(dot.compare(origDot) <= 0);
#ifdef DEBUG_CONVEX_HULL
			printf("Moving downwards, edge is ");
			e->print();
			printf(", dot is %f (%f %lld)\n", (float) dot.toScalar(), (float) optDot.toScalar(), shiftedDot);
#endif
			if (dot.compare(optDot) < 0)
			{
				int c = dot.compare(shiftedDot);
				optDot = dot;
				e = e->reverse;
				startEdge = e;
				if (c < 0)
				{
					intersection = e;
					break;
				}
				cmp = c;
			}
			e = e->prev;
		} while (e != startEdge);

		if (!intersection)
		{
			return false;
		}
	}
	else
	{
		Edge* e = startEdge;
		do
		{
#ifdef SHOW_ITERATIONS
			n++;
#endif
			Rational128 dot = e->target->dot(normal);
			btAssert(dot.compare(origDot) <= 0);
#ifdef DEBUG_CONVEX_HULL
			printf("Moving upwards, edge is ");
			e->print();
			printf(", dot is %f (%f %lld)\n", (float) dot.toScalar(), (float) optDot.toScalar(), shiftedDot);
#endif
			if (dot.compare(optDot) > 0)
			{
				cmp = dot.compare(shiftedDot);
				if (cmp >= 0)
				{
					intersection = e;
					break;
				}
				optDot = dot;
				e = e->reverse;
				startEdge = e;
			}
			e = e->prev;
		} while (e != startEdge);

		if (!intersection)
		{
			return true;
		}
	}

#ifdef SHOW_ITERATIONS
	printf("Needed %d iterations to find initial intersection\n", n);
#endif

	if (cmp == 0)
	{
		Edge* e = intersection->reverse->next;
#ifdef SHOW_ITERATIONS
		n = 0;
#endif
		while (e->target->dot(normal).compare(shiftedDot) <= 0)
		{
#ifdef SHOW_ITERATIONS
			n++;
#endif
			e = e->next;
			if (e == intersection->reverse)
			{
				return true;
			}
#ifdef DEBUG_CONVEX_HULL
			printf("Checking for outwards edge, current edge is ");
			e->print();
			printf("\n");
#endif
		}
#ifdef SHOW_ITERATIONS
		printf("Needed %d iterations to check for complete containment\n", n);
#endif
	}

	Edge* firstIntersection = NULL;
	Edge* faceEdge = NULL;
	Edge* firstFaceEdge = NULL;

#ifdef SHOW_ITERATIONS
	int m = 0;
#endif
	while (true)
	{
#ifdef SHOW_ITERATIONS
		m++;
#endif
#ifdef DEBUG_CONVEX_HULL
		printf("Intersecting edge is ");
		intersection->print();
		printf("\n");
#endif
		if (cmp == 0)
		{
			Edge* e = intersection->reverse->next;
			startEdge = e;
#ifdef SHOW_ITERATIONS
			n = 0;
#endif
			while (true)
			{
#ifdef SHOW_ITERATIONS
				n++;
#endif
				if (e->target->dot(normal).compare(shiftedDot) >= 0)
				{
					break;
				}
				intersection = e->reverse;
				e = e->next;
				if (e == startEdge)
				{
					return true;
				}
			}
#ifdef SHOW_ITERATIONS
			printf("Needed %d iterations to advance intersection\n", n);
#endif
		}

#ifdef DEBUG_CONVEX_HULL
		printf("Advanced intersecting edge to ");
		intersection->print();
		printf(", cmp = %d\n", cmp);
#endif

		if (!firstIntersection)
		{
			firstIntersection = intersection;
		}
		else if (intersection == firstIntersection)
		{
			break;
		}

		int prevCmp = cmp;
		Edge* prevIntersection = intersection;
		Edge* prevFaceEdge = faceEdge;

		Edge* e = intersection->reverse;
#ifdef SHOW_ITERATIONS
		n = 0;
#endif
		while (true)
		{
#ifdef SHOW_ITERATIONS
			n++;
#endif
			e = e->reverse->prev;
			btAssert(e != intersection->reverse);
			cmp = e->target->dot(normal).compare(shiftedDot);
#ifdef DEBUG_CONVEX_HULL
			printf("Testing edge ");
			e->print();
			printf(" -> cmp = %d\n", cmp);
#endif
			if (cmp >= 0)
			{
				intersection = e;
				break;
			}
		}
#ifdef SHOW_ITERATIONS
		printf("Needed %d iterations to find other intersection of face\n", n);
#endif

		if (cmp > 0)
		{
			Vertex* removed = intersection->target;
			e = intersection->reverse;
			if (e->prev == e)
			{
				removed->edges = NULL;
			}
			else
			{
				removed->edges = e->prev;
				e->prev->link(e->next);
				e->link(e);
			}
#ifdef DEBUG_CONVEX_HULL
			printf("1: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z);
#endif

			Point64 n0 = intersection->face->getNormal();
			Point64 n1 = intersection->reverse->face->getNormal();
			int64_t m00 = face->dir0.dot(n0);
			int64_t m01 = face->dir1.dot(n0);
			int64_t m10 = face->dir0.dot(n1);
			int64_t m11 = face->dir1.dot(n1);
			int64_t r0 = (intersection->face->origin - shiftedOrigin).dot(n0);
			int64_t r1 = (intersection->reverse->face->origin - shiftedOrigin).dot(n1);
			Int128 det = Int128::mul(m00, m11) - Int128::mul(m01, m10);
			btAssert(det.getSign() != 0);
			Vertex* v = vertexPool.newObject();
			v->point.index = -1;
			v->copy = -1;
			v->point128 = PointR128(Int128::mul(face->dir0.x * r0, m11) - Int128::mul(face->dir0.x * r1, m01)
															+ Int128::mul(face->dir1.x * r1, m00) - Int128::mul(face->dir1.x * r0, m10) + det * shiftedOrigin.x,
															Int128::mul(face->dir0.y * r0, m11) - Int128::mul(face->dir0.y * r1, m01)
															+ Int128::mul(face->dir1.y * r1, m00) - Int128::mul(face->dir1.y * r0, m10) + det * shiftedOrigin.y,
															Int128::mul(face->dir0.z * r0, m11) - Int128::mul(face->dir0.z * r1, m01)
															+ Int128::mul(face->dir1.z * r1, m00) - Int128::mul(face->dir1.z * r0, m10) + det * shiftedOrigin.z,
															det);
			v->point.x = (int32_t) v->point128.xvalue();
			v->point.y = (int32_t) v->point128.yvalue();
			v->point.z = (int32_t) v->point128.zvalue();
			intersection->target = v;
			v->edges = e;

			stack.push_back(v);
			stack.push_back(removed);
			stack.push_back(NULL);
		}

		if (cmp || prevCmp || (prevIntersection->reverse->next->target != intersection->target))
		{
			faceEdge = newEdgePair(prevIntersection->target, intersection->target);
			if (prevCmp == 0)
			{
				faceEdge->link(prevIntersection->reverse->next);
			}
			if ((prevCmp == 0) || prevFaceEdge)
			{
				prevIntersection->reverse->link(faceEdge);
			}
			if (cmp == 0)
			{
				intersection->reverse->prev->link(faceEdge->reverse);
			}
			faceEdge->reverse->link(intersection->reverse);
		}
		else
		{
			faceEdge = prevIntersection->reverse->next;
		}

		if (prevFaceEdge)
		{
			if (prevCmp > 0)
			{
				faceEdge->link(prevFaceEdge->reverse);
			}
			else if (faceEdge != prevFaceEdge->reverse)
			{
				stack.push_back(prevFaceEdge->target);
				while (faceEdge->next != prevFaceEdge->reverse)
				{
					Vertex* removed = faceEdge->next->target;
					removeEdgePair(faceEdge->next);
					stack.push_back(removed);
#ifdef DEBUG_CONVEX_HULL
					printf("2: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z);
#endif
				}
				stack.push_back(NULL);
			}
		}
		faceEdge->face = face;
		faceEdge->reverse->face = intersection->face;

		if (!firstFaceEdge)
		{
			firstFaceEdge = faceEdge;
		}
	}
#ifdef SHOW_ITERATIONS
	printf("Needed %d iterations to process all intersections\n", m);
#endif

	if (cmp > 0)
	{
		firstFaceEdge->reverse->target = faceEdge->target;
		firstIntersection->reverse->link(firstFaceEdge);
		firstFaceEdge->link(faceEdge->reverse);
	}
	else if (firstFaceEdge != faceEdge->reverse)
	{
		stack.push_back(faceEdge->target);
		while (firstFaceEdge->next != faceEdge->reverse)
		{
			Vertex* removed = firstFaceEdge->next->target;
			removeEdgePair(firstFaceEdge->next);
			stack.push_back(removed);
#ifdef DEBUG_CONVEX_HULL
			printf("3: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z);
#endif
		}
		stack.push_back(NULL);
	}

	btAssert(stack.size() > 0);
	vertexList = stack[0];

#ifdef DEBUG_CONVEX_HULL
	printf("Removing part\n");
#endif
#ifdef SHOW_ITERATIONS
	n = 0;
#endif
	int pos = 0;
	while (pos < stack.size())
	{
		int end = stack.size();
		while (pos < end)
		{
			Vertex* kept = stack[pos++];
#ifdef DEBUG_CONVEX_HULL
			kept->print();
#endif
			bool deeper = false;
			Vertex* removed;
			while ((removed = stack[pos++]) != NULL)
			{
#ifdef SHOW_ITERATIONS
				n++;
#endif
				kept->receiveNearbyFaces(removed);
				while (removed->edges)
				{
					if (!deeper)
					{
						deeper = true;
						stack.push_back(kept);
					}
					stack.push_back(removed->edges->target);
					removeEdgePair(removed->edges);
				}
			}
			if (deeper)
			{
				stack.push_back(NULL);
			}
		}
	}
#ifdef SHOW_ITERATIONS
	printf("Needed %d iterations to remove part\n", n);
#endif

	stack.resize(0);
	face->origin = shiftedOrigin;

	return true;
}


static int getVertexCopy(btConvexHullInternal::Vertex* vertex, btAlignedObjectArray<btConvexHullInternal::Vertex*>& vertices)
{
	int index = vertex->copy;
	if (index < 0)
	{
		index = vertices.size();
		vertex->copy = index;
		vertices.push_back(vertex);
#ifdef DEBUG_CONVEX_HULL
		printf("Vertex %d gets index *%d\n", vertex->point.index, index);
#endif
	}
	return index;
}

btScalar btConvexHullComputer::compute(const void* coords, bool doubleCoords, int stride, int count, btScalar shrink, btScalar shrinkClamp)
{
	if (count <= 0)
	{
		vertices.clear();
		edges.clear();
		faces.clear();
		return 0;
	}

	btConvexHullInternal hull;
	hull.compute(coords, doubleCoords, stride, count);

	btScalar shift = 0;
	if ((shrink > 0) && ((shift = hull.shrink(shrink, shrinkClamp)) < 0))
	{
		vertices.clear();
		edges.clear();
		faces.clear();
		return shift;
	}

	vertices.resize(0);
	edges.resize(0);
	faces.resize(0);

	btAlignedObjectArray<btConvexHullInternal::Vertex*> oldVertices;
	getVertexCopy(hull.vertexList, oldVertices);
	int copied = 0;
	while (copied < oldVertices.size())
	{
		btConvexHullInternal::Vertex* v = oldVertices[copied];
		vertices.push_back(hull.getCoordinates(v));
		btConvexHullInternal::Edge* firstEdge = v->edges;
		if (firstEdge)
		{
			int firstCopy = -1;
			int prevCopy = -1;
			btConvexHullInternal::Edge* e = firstEdge;
			do
			{
				if (e->copy < 0)
				{
					int s = edges.size();
					edges.push_back(Edge());
					edges.push_back(Edge());
					Edge* c = &edges[s];
					Edge* r = &edges[s + 1];
					e->copy = s;
					e->reverse->copy = s + 1;
					c->reverse = 1;
					r->reverse = -1;
					c->targetVertex = getVertexCopy(e->target, oldVertices);
					r->targetVertex = copied;
#ifdef DEBUG_CONVEX_HULL
					printf("      CREATE: Vertex *%d has edge to *%d\n", copied, c->getTargetVertex());
#endif
				}
				if (prevCopy >= 0)
				{
					edges[e->copy].next = prevCopy - e->copy;
				}
				else
				{
					firstCopy = e->copy;
				}
				prevCopy = e->copy;
				e = e->next;
			} while (e != firstEdge);
			edges[firstCopy].next = prevCopy - firstCopy;
		}
		copied++;
	}

	for (int i = 0; i < copied; i++)
	{
		btConvexHullInternal::Vertex* v = oldVertices[i];
		btConvexHullInternal::Edge* firstEdge = v->edges;
		if (firstEdge)
		{
			btConvexHullInternal::Edge* e = firstEdge;
			do
			{
				if (e->copy >= 0)
				{
#ifdef DEBUG_CONVEX_HULL
					printf("Vertex *%d has edge to *%d\n", i, edges[e->copy].getTargetVertex());
#endif
					faces.push_back(e->copy);
					btConvexHullInternal::Edge* f = e;
					do
					{
#ifdef DEBUG_CONVEX_HULL
						printf("   Face *%d\n", edges[f->copy].getTargetVertex());
#endif
						f->copy = -1;
						f = f->reverse->prev;
					} while (f != e);
				}
				e = e->next;
			} while (e != firstEdge);
		}
	}

	return shift;
}