summaryrefslogtreecommitdiff
path: root/thirdparty/bullet/Bullet3Geometry
diff options
context:
space:
mode:
Diffstat (limited to 'thirdparty/bullet/Bullet3Geometry')
-rw-r--r--thirdparty/bullet/Bullet3Geometry/b3AabbUtil.h189
-rw-r--r--thirdparty/bullet/Bullet3Geometry/b3ConvexHullComputer.cpp1554
-rw-r--r--thirdparty/bullet/Bullet3Geometry/b3ConvexHullComputer.h102
-rw-r--r--thirdparty/bullet/Bullet3Geometry/b3GeometryUtil.cpp113
-rw-r--r--thirdparty/bullet/Bullet3Geometry/b3GeometryUtil.h22
-rw-r--r--thirdparty/bullet/Bullet3Geometry/b3GrahamScan2dConvexHull.h63
6 files changed, 998 insertions, 1045 deletions
diff --git a/thirdparty/bullet/Bullet3Geometry/b3AabbUtil.h b/thirdparty/bullet/Bullet3Geometry/b3AabbUtil.h
index 4c72d5bbfc..396a401450 100644
--- a/thirdparty/bullet/Bullet3Geometry/b3AabbUtil.h
+++ b/thirdparty/bullet/Bullet3Geometry/b3AabbUtil.h
@@ -12,8 +12,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
-
-
#ifndef B3_AABB_UTIL2
#define B3_AABB_UTIL2
@@ -21,20 +19,18 @@ subject to the following restrictions:
#include "Bullet3Common/b3Vector3.h"
#include "Bullet3Common/b3MinMax.h"
-
-
-B3_FORCE_INLINE void b3AabbExpand (b3Vector3& aabbMin,
- b3Vector3& aabbMax,
- const b3Vector3& expansionMin,
- const b3Vector3& expansionMax)
+B3_FORCE_INLINE void b3AabbExpand(b3Vector3& aabbMin,
+ b3Vector3& aabbMax,
+ const b3Vector3& expansionMin,
+ const b3Vector3& expansionMax)
{
aabbMin = aabbMin + expansionMin;
aabbMax = aabbMax + expansionMax;
}
/// conservative test for overlap between two aabbs
-B3_FORCE_INLINE bool b3TestPointAgainstAabb2(const b3Vector3 &aabbMin1, const b3Vector3 &aabbMax1,
- const b3Vector3 &point)
+B3_FORCE_INLINE bool b3TestPointAgainstAabb2(const b3Vector3& aabbMin1, const b3Vector3& aabbMax1,
+ const b3Vector3& point)
{
bool overlap = true;
overlap = (aabbMin1.getX() > point.getX() || aabbMax1.getX() < point.getX()) ? false : overlap;
@@ -43,10 +39,9 @@ B3_FORCE_INLINE bool b3TestPointAgainstAabb2(const b3Vector3 &aabbMin1, const b3
return overlap;
}
-
/// conservative test for overlap between two aabbs
-B3_FORCE_INLINE bool b3TestAabbAgainstAabb2(const b3Vector3 &aabbMin1, const b3Vector3 &aabbMax1,
- const b3Vector3 &aabbMin2, const b3Vector3 &aabbMax2)
+B3_FORCE_INLINE bool b3TestAabbAgainstAabb2(const b3Vector3& aabbMin1, const b3Vector3& aabbMax1,
+ const b3Vector3& aabbMin2, const b3Vector3& aabbMax2)
{
bool overlap = true;
overlap = (aabbMin1.getX() > aabbMax2.getX() || aabbMax1.getX() < aabbMin2.getX()) ? false : overlap;
@@ -56,52 +51,49 @@ B3_FORCE_INLINE bool b3TestAabbAgainstAabb2(const b3Vector3 &aabbMin1, const b3V
}
/// conservative test for overlap between triangle and aabb
-B3_FORCE_INLINE bool b3TestTriangleAgainstAabb2(const b3Vector3 *vertices,
- const b3Vector3 &aabbMin, const b3Vector3 &aabbMax)
+B3_FORCE_INLINE bool b3TestTriangleAgainstAabb2(const b3Vector3* vertices,
+ const b3Vector3& aabbMin, const b3Vector3& aabbMax)
{
- const b3Vector3 &p1 = vertices[0];
- const b3Vector3 &p2 = vertices[1];
- const b3Vector3 &p3 = vertices[2];
+ const b3Vector3& p1 = vertices[0];
+ const b3Vector3& p2 = vertices[1];
+ const b3Vector3& p3 = vertices[2];
if (b3Min(b3Min(p1[0], p2[0]), p3[0]) > aabbMax[0]) return false;
if (b3Max(b3Max(p1[0], p2[0]), p3[0]) < aabbMin[0]) return false;
if (b3Min(b3Min(p1[2], p2[2]), p3[2]) > aabbMax[2]) return false;
if (b3Max(b3Max(p1[2], p2[2]), p3[2]) < aabbMin[2]) return false;
-
+
if (b3Min(b3Min(p1[1], p2[1]), p3[1]) > aabbMax[1]) return false;
if (b3Max(b3Max(p1[1], p2[1]), p3[1]) < aabbMin[1]) return false;
return true;
}
-
-B3_FORCE_INLINE int b3Outcode(const b3Vector3& p,const b3Vector3& halfExtent)
+B3_FORCE_INLINE int b3Outcode(const b3Vector3& p, const b3Vector3& halfExtent)
{
- return (p.getX() < -halfExtent.getX() ? 0x01 : 0x0) |
- (p.getX() > halfExtent.getX() ? 0x08 : 0x0) |
- (p.getY() < -halfExtent.getY() ? 0x02 : 0x0) |
- (p.getY() > halfExtent.getY() ? 0x10 : 0x0) |
- (p.getZ() < -halfExtent.getZ() ? 0x4 : 0x0) |
- (p.getZ() > halfExtent.getZ() ? 0x20 : 0x0);
+ return (p.getX() < -halfExtent.getX() ? 0x01 : 0x0) |
+ (p.getX() > halfExtent.getX() ? 0x08 : 0x0) |
+ (p.getY() < -halfExtent.getY() ? 0x02 : 0x0) |
+ (p.getY() > halfExtent.getY() ? 0x10 : 0x0) |
+ (p.getZ() < -halfExtent.getZ() ? 0x4 : 0x0) |
+ (p.getZ() > halfExtent.getZ() ? 0x20 : 0x0);
}
-
-
B3_FORCE_INLINE bool b3RayAabb2(const b3Vector3& rayFrom,
- const b3Vector3& rayInvDirection,
- const unsigned int raySign[3],
- const b3Vector3 bounds[2],
- b3Scalar& tmin,
- b3Scalar lambda_min,
- b3Scalar lambda_max)
+ const b3Vector3& rayInvDirection,
+ const unsigned int raySign[3],
+ const b3Vector3 bounds[2],
+ b3Scalar& tmin,
+ b3Scalar lambda_min,
+ b3Scalar lambda_max)
{
b3Scalar tmax, tymin, tymax, tzmin, tzmax;
tmin = (bounds[raySign[0]].getX() - rayFrom.getX()) * rayInvDirection.getX();
- tmax = (bounds[1-raySign[0]].getX() - rayFrom.getX()) * rayInvDirection.getX();
+ tmax = (bounds[1 - raySign[0]].getX() - rayFrom.getX()) * rayInvDirection.getX();
tymin = (bounds[raySign[1]].getY() - rayFrom.getY()) * rayInvDirection.getY();
- tymax = (bounds[1-raySign[1]].getY() - rayFrom.getY()) * rayInvDirection.getY();
+ tymax = (bounds[1 - raySign[1]].getY() - rayFrom.getY()) * rayInvDirection.getY();
- if ( (tmin > tymax) || (tymin > tmax) )
+ if ((tmin > tymax) || (tymin > tmax))
return false;
if (tymin > tmin)
@@ -111,59 +103,59 @@ B3_FORCE_INLINE bool b3RayAabb2(const b3Vector3& rayFrom,
tmax = tymax;
tzmin = (bounds[raySign[2]].getZ() - rayFrom.getZ()) * rayInvDirection.getZ();
- tzmax = (bounds[1-raySign[2]].getZ() - rayFrom.getZ()) * rayInvDirection.getZ();
+ tzmax = (bounds[1 - raySign[2]].getZ() - rayFrom.getZ()) * rayInvDirection.getZ();
- if ( (tmin > tzmax) || (tzmin > tmax) )
+ if ((tmin > tzmax) || (tzmin > tmax))
return false;
if (tzmin > tmin)
tmin = tzmin;
if (tzmax < tmax)
tmax = tzmax;
- return ( (tmin < lambda_max) && (tmax > lambda_min) );
+ return ((tmin < lambda_max) && (tmax > lambda_min));
}
-B3_FORCE_INLINE bool b3RayAabb(const b3Vector3& rayFrom,
- const b3Vector3& rayTo,
- const b3Vector3& aabbMin,
- const b3Vector3& aabbMax,
- b3Scalar& param, b3Vector3& normal)
+B3_FORCE_INLINE bool b3RayAabb(const b3Vector3& rayFrom,
+ const b3Vector3& rayTo,
+ const b3Vector3& aabbMin,
+ const b3Vector3& aabbMax,
+ b3Scalar& param, b3Vector3& normal)
{
- b3Vector3 aabbHalfExtent = (aabbMax-aabbMin)* b3Scalar(0.5);
- b3Vector3 aabbCenter = (aabbMax+aabbMin)* b3Scalar(0.5);
- b3Vector3 source = rayFrom - aabbCenter;
- b3Vector3 target = rayTo - aabbCenter;
- int sourceOutcode = b3Outcode(source,aabbHalfExtent);
- int targetOutcode = b3Outcode(target,aabbHalfExtent);
+ b3Vector3 aabbHalfExtent = (aabbMax - aabbMin) * b3Scalar(0.5);
+ b3Vector3 aabbCenter = (aabbMax + aabbMin) * b3Scalar(0.5);
+ b3Vector3 source = rayFrom - aabbCenter;
+ b3Vector3 target = rayTo - aabbCenter;
+ int sourceOutcode = b3Outcode(source, aabbHalfExtent);
+ int targetOutcode = b3Outcode(target, aabbHalfExtent);
if ((sourceOutcode & targetOutcode) == 0x0)
{
b3Scalar lambda_enter = b3Scalar(0.0);
- b3Scalar lambda_exit = param;
+ b3Scalar lambda_exit = param;
b3Vector3 r = target - source;
int i;
- b3Scalar normSign = 1;
- b3Vector3 hitNormal = b3MakeVector3(0,0,0);
- int bit=1;
+ b3Scalar normSign = 1;
+ b3Vector3 hitNormal = b3MakeVector3(0, 0, 0);
+ int bit = 1;
- for (int j=0;j<2;j++)
+ for (int j = 0; j < 2; j++)
{
for (i = 0; i != 3; ++i)
{
if (sourceOutcode & bit)
{
- b3Scalar lambda = (-source[i] - aabbHalfExtent[i]*normSign) / r[i];
+ b3Scalar lambda = (-source[i] - aabbHalfExtent[i] * normSign) / r[i];
if (lambda_enter <= lambda)
{
lambda_enter = lambda;
- hitNormal.setValue(0,0,0);
+ hitNormal.setValue(0, 0, 0);
hitNormal[i] = normSign;
}
}
- else if (targetOutcode & bit)
+ else if (targetOutcode & bit)
{
- b3Scalar lambda = (-source[i] - aabbHalfExtent[i]*normSign) / r[i];
+ b3Scalar lambda = (-source[i] - aabbHalfExtent[i] * normSign) / r[i];
b3SetMin(lambda_exit, lambda);
}
- bit<<=1;
+ bit <<= 1;
}
normSign = b3Scalar(-1.);
}
@@ -177,56 +169,49 @@ B3_FORCE_INLINE bool b3RayAabb(const b3Vector3& rayFrom,
return false;
}
-
-
-B3_FORCE_INLINE void b3TransformAabb(const b3Vector3& halfExtents, b3Scalar margin,const b3Transform& t,b3Vector3& aabbMinOut,b3Vector3& aabbMaxOut)
+B3_FORCE_INLINE void b3TransformAabb(const b3Vector3& halfExtents, b3Scalar margin, const b3Transform& t, b3Vector3& aabbMinOut, b3Vector3& aabbMaxOut)
{
- b3Vector3 halfExtentsWithMargin = halfExtents+b3MakeVector3(margin,margin,margin);
- b3Matrix3x3 abs_b = t.getBasis().absolute();
+ b3Vector3 halfExtentsWithMargin = halfExtents + b3MakeVector3(margin, margin, margin);
+ b3Matrix3x3 abs_b = t.getBasis().absolute();
b3Vector3 center = t.getOrigin();
- b3Vector3 extent = halfExtentsWithMargin.dot3( abs_b[0], abs_b[1], abs_b[2] );
+ b3Vector3 extent = halfExtentsWithMargin.dot3(abs_b[0], abs_b[1], abs_b[2]);
aabbMinOut = center - extent;
aabbMaxOut = center + extent;
}
-
-B3_FORCE_INLINE void b3TransformAabb(const b3Vector3& localAabbMin,const b3Vector3& localAabbMax, b3Scalar margin,const b3Transform& trans,b3Vector3& aabbMinOut,b3Vector3& aabbMaxOut)
+B3_FORCE_INLINE void b3TransformAabb(const b3Vector3& localAabbMin, const b3Vector3& localAabbMax, b3Scalar margin, const b3Transform& trans, b3Vector3& aabbMinOut, b3Vector3& aabbMaxOut)
{
- //b3Assert(localAabbMin.getX() <= localAabbMax.getX());
- //b3Assert(localAabbMin.getY() <= localAabbMax.getY());
- //b3Assert(localAabbMin.getZ() <= localAabbMax.getZ());
- b3Vector3 localHalfExtents = b3Scalar(0.5)*(localAabbMax-localAabbMin);
- localHalfExtents+=b3MakeVector3(margin,margin,margin);
-
- b3Vector3 localCenter = b3Scalar(0.5)*(localAabbMax+localAabbMin);
- b3Matrix3x3 abs_b = trans.getBasis().absolute();
- b3Vector3 center = trans(localCenter);
- b3Vector3 extent = localHalfExtents.dot3( abs_b[0], abs_b[1], abs_b[2] );
- aabbMinOut = center-extent;
- aabbMaxOut = center+extent;
+ //b3Assert(localAabbMin.getX() <= localAabbMax.getX());
+ //b3Assert(localAabbMin.getY() <= localAabbMax.getY());
+ //b3Assert(localAabbMin.getZ() <= localAabbMax.getZ());
+ b3Vector3 localHalfExtents = b3Scalar(0.5) * (localAabbMax - localAabbMin);
+ localHalfExtents += b3MakeVector3(margin, margin, margin);
+
+ b3Vector3 localCenter = b3Scalar(0.5) * (localAabbMax + localAabbMin);
+ b3Matrix3x3 abs_b = trans.getBasis().absolute();
+ b3Vector3 center = trans(localCenter);
+ b3Vector3 extent = localHalfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]);
+ aabbMinOut = center - extent;
+ aabbMaxOut = center + extent;
}
#define B3_USE_BANCHLESS 1
#ifdef B3_USE_BANCHLESS
- //This block replaces the block below and uses no branches, and replaces the 8 bit return with a 32 bit return for improved performance (~3x on XBox 360)
- B3_FORCE_INLINE unsigned b3TestQuantizedAabbAgainstQuantizedAabb(const unsigned short int* aabbMin1,const unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2)
- {
- return static_cast<unsigned int>(b3Select((unsigned)((aabbMin1[0] <= aabbMax2[0]) & (aabbMax1[0] >= aabbMin2[0])
- & (aabbMin1[2] <= aabbMax2[2]) & (aabbMax1[2] >= aabbMin2[2])
- & (aabbMin1[1] <= aabbMax2[1]) & (aabbMax1[1] >= aabbMin2[1])),
- 1, 0));
- }
+//This block replaces the block below and uses no branches, and replaces the 8 bit return with a 32 bit return for improved performance (~3x on XBox 360)
+B3_FORCE_INLINE unsigned b3TestQuantizedAabbAgainstQuantizedAabb(const unsigned short int* aabbMin1, const unsigned short int* aabbMax1, const unsigned short int* aabbMin2, const unsigned short int* aabbMax2)
+{
+ return static_cast<unsigned int>(b3Select((unsigned)((aabbMin1[0] <= aabbMax2[0]) & (aabbMax1[0] >= aabbMin2[0]) & (aabbMin1[2] <= aabbMax2[2]) & (aabbMax1[2] >= aabbMin2[2]) & (aabbMin1[1] <= aabbMax2[1]) & (aabbMax1[1] >= aabbMin2[1])),
+ 1, 0));
+}
#else
- B3_FORCE_INLINE bool b3TestQuantizedAabbAgainstQuantizedAabb(const unsigned short int* aabbMin1,const unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2)
- {
- bool overlap = true;
- overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap;
- overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap;
- overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap;
- return overlap;
- }
-#endif //B3_USE_BANCHLESS
-
-#endif //B3_AABB_UTIL2
-
+B3_FORCE_INLINE bool b3TestQuantizedAabbAgainstQuantizedAabb(const unsigned short int* aabbMin1, const unsigned short int* aabbMax1, const unsigned short int* aabbMin2, const unsigned short int* aabbMax2)
+{
+ bool overlap = true;
+ overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap;
+ overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap;
+ overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap;
+ return overlap;
+}
+#endif //B3_USE_BANCHLESS
+#endif //B3_AABB_UTIL2
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;
}
-
-
-
-
-
diff --git a/thirdparty/bullet/Bullet3Geometry/b3ConvexHullComputer.h b/thirdparty/bullet/Bullet3Geometry/b3ConvexHullComputer.h
index 6dcc931a78..8852c5a524 100644
--- a/thirdparty/bullet/Bullet3Geometry/b3ConvexHullComputer.h
+++ b/thirdparty/bullet/Bullet3Geometry/b3ConvexHullComputer.h
@@ -23,58 +23,56 @@ subject to the following restrictions:
/// Ole Kniemeyer, MAXON Computer GmbH
class b3ConvexHullComputer
{
+private:
+ b3Scalar compute(const void* coords, bool doubleCoords, int stride, int count, b3Scalar shrink, b3Scalar shrinkClamp);
+
+public:
+ class Edge
+ {
private:
- b3Scalar compute(const void* coords, bool doubleCoords, int stride, int count, b3Scalar shrink, b3Scalar shrinkClamp);
+ int next;
+ int reverse;
+ int targetVertex;
- public:
+ friend class b3ConvexHullComputer;
- class Edge
+ public:
+ int getSourceVertex() const
{
- private:
- int next;
- int reverse;
- int targetVertex;
-
- friend class b3ConvexHullComputer;
-
- public:
- int getSourceVertex() const
- {
- return (this + reverse)->targetVertex;
- }
-
- int getTargetVertex() const
- {
- return targetVertex;
- }
+ return (this + reverse)->targetVertex;
+ }
- const Edge* getNextEdgeOfVertex() const // clockwise list of all edges of a vertex
- {
- return this + next;
- }
+ int getTargetVertex() const
+ {
+ return targetVertex;
+ }
- const Edge* getNextEdgeOfFace() const // counter-clockwise list of all edges of a face
- {
- return (this + reverse)->getNextEdgeOfVertex();
- }
+ const Edge* getNextEdgeOfVertex() const // clockwise list of all edges of a vertex
+ {
+ return this + next;
+ }
- const Edge* getReverseEdge() const
- {
- return this + reverse;
- }
- };
+ const Edge* getNextEdgeOfFace() const // counter-clockwise list of all edges of a face
+ {
+ return (this + reverse)->getNextEdgeOfVertex();
+ }
+ const Edge* getReverseEdge() const
+ {
+ return this + reverse;
+ }
+ };
- // Vertices of the output hull
- b3AlignedObjectArray<b3Vector3> vertices;
+ // Vertices of the output hull
+ b3AlignedObjectArray<b3Vector3> vertices;
- // Edges of the output hull
- b3AlignedObjectArray<Edge> edges;
+ // Edges of the output hull
+ b3AlignedObjectArray<Edge> edges;
- // Faces of the convex hull. Each entry is an index into the "edges" array pointing to an edge of the face. Faces are planar n-gons
- b3AlignedObjectArray<int> faces;
+ // Faces of the convex hull. Each entry is an index into the "edges" array pointing to an edge of the face. Faces are planar n-gons
+ b3AlignedObjectArray<int> faces;
- /*
+ /*
Compute convex hull of "count" vertices stored in "coords". "stride" is the difference in bytes
between the addresses of consecutive vertices. If "shrink" is positive, the convex hull is shrunken
by that amount (each face is moved by "shrink" length units towards the center along its normal).
@@ -86,18 +84,16 @@ class b3ConvexHullComputer
The output convex hull can be found in the member variables "vertices", "edges", "faces".
*/
- b3Scalar compute(const float* coords, int stride, int count, b3Scalar shrink, b3Scalar shrinkClamp)
- {
- return compute(coords, false, stride, count, shrink, shrinkClamp);
- }
-
- // same as above, but double precision
- b3Scalar compute(const double* coords, int stride, int count, b3Scalar shrink, b3Scalar shrinkClamp)
- {
- return compute(coords, true, stride, count, shrink, shrinkClamp);
- }
+ b3Scalar compute(const float* coords, int stride, int count, b3Scalar shrink, b3Scalar shrinkClamp)
+ {
+ return compute(coords, false, stride, count, shrink, shrinkClamp);
+ }
+
+ // same as above, but double precision
+ b3Scalar compute(const double* coords, int stride, int count, b3Scalar shrink, b3Scalar shrinkClamp)
+ {
+ return compute(coords, true, stride, count, shrink, shrinkClamp);
+ }
};
-
-#endif //B3_CONVEX_HULL_COMPUTER_H
-
+#endif //B3_CONVEX_HULL_COMPUTER_H
diff --git a/thirdparty/bullet/Bullet3Geometry/b3GeometryUtil.cpp b/thirdparty/bullet/Bullet3Geometry/b3GeometryUtil.cpp
index dd80fed6bd..c4041003ca 100644
--- a/thirdparty/bullet/Bullet3Geometry/b3GeometryUtil.cpp
+++ b/thirdparty/bullet/Bullet3Geometry/b3GeometryUtil.cpp
@@ -12,49 +12,43 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
-
-
#include "b3GeometryUtil.h"
-
/*
Make sure this dummy function never changes so that it
can be used by probes that are checking whether the
library is actually installed.
*/
extern "C"
-{
- void b3BulletMathProbe ();
+{
+ void b3BulletMathProbe();
- void b3BulletMathProbe () {}
+ void b3BulletMathProbe() {}
}
-
-bool b3GeometryUtil::isPointInsidePlanes(const b3AlignedObjectArray<b3Vector3>& planeEquations, const b3Vector3& point, b3Scalar margin)
+bool b3GeometryUtil::isPointInsidePlanes(const b3AlignedObjectArray<b3Vector3>& planeEquations, const b3Vector3& point, b3Scalar margin)
{
int numbrushes = planeEquations.size();
- for (int i=0;i<numbrushes;i++)
+ for (int i = 0; i < numbrushes; i++)
{
const b3Vector3& N1 = planeEquations[i];
- b3Scalar dist = b3Scalar(N1.dot(point))+b3Scalar(N1[3])-margin;
- if (dist>b3Scalar(0.))
+ b3Scalar dist = b3Scalar(N1.dot(point)) + b3Scalar(N1[3]) - margin;
+ if (dist > b3Scalar(0.))
{
return false;
}
}
return true;
-
}
-
-bool b3GeometryUtil::areVerticesBehindPlane(const b3Vector3& planeNormal, const b3AlignedObjectArray<b3Vector3>& vertices, b3Scalar margin)
+bool b3GeometryUtil::areVerticesBehindPlane(const b3Vector3& planeNormal, const b3AlignedObjectArray<b3Vector3>& vertices, b3Scalar margin)
{
int numvertices = vertices.size();
- for (int i=0;i<numvertices;i++)
+ for (int i = 0; i < numvertices; i++)
{
const b3Vector3& N1 = vertices[i];
- b3Scalar dist = b3Scalar(planeNormal.dot(N1))+b3Scalar(planeNormal[3])-margin;
- if (dist>b3Scalar(0.))
+ b3Scalar dist = b3Scalar(planeNormal.dot(N1)) + b3Scalar(planeNormal[3]) - margin;
+ if (dist > b3Scalar(0.))
{
return false;
}
@@ -62,102 +56,98 @@ bool b3GeometryUtil::areVerticesBehindPlane(const b3Vector3& planeNormal, const
return true;
}
-bool notExist(const b3Vector3& planeEquation,const b3AlignedObjectArray<b3Vector3>& planeEquations);
+bool notExist(const b3Vector3& planeEquation, const b3AlignedObjectArray<b3Vector3>& planeEquations);
-bool notExist(const b3Vector3& planeEquation,const b3AlignedObjectArray<b3Vector3>& planeEquations)
+bool notExist(const b3Vector3& planeEquation, const b3AlignedObjectArray<b3Vector3>& planeEquations)
{
int numbrushes = planeEquations.size();
- for (int i=0;i<numbrushes;i++)
+ for (int i = 0; i < numbrushes; i++)
{
const b3Vector3& N1 = planeEquations[i];
if (planeEquation.dot(N1) > b3Scalar(0.999))
{
return false;
- }
+ }
}
return true;
}
-void b3GeometryUtil::getPlaneEquationsFromVertices(b3AlignedObjectArray<b3Vector3>& vertices, b3AlignedObjectArray<b3Vector3>& planeEquationsOut )
+void b3GeometryUtil::getPlaneEquationsFromVertices(b3AlignedObjectArray<b3Vector3>& vertices, b3AlignedObjectArray<b3Vector3>& planeEquationsOut)
{
- const int numvertices = vertices.size();
+ const int numvertices = vertices.size();
// brute force:
- for (int i=0;i<numvertices;i++)
+ for (int i = 0; i < numvertices; i++)
{
const b3Vector3& N1 = vertices[i];
-
- for (int j=i+1;j<numvertices;j++)
+ for (int j = i + 1; j < numvertices; j++)
{
const b3Vector3& N2 = vertices[j];
-
- for (int k=j+1;k<numvertices;k++)
- {
+ for (int k = j + 1; k < numvertices; k++)
+ {
const b3Vector3& N3 = vertices[k];
- b3Vector3 planeEquation,edge0,edge1;
- edge0 = N2-N1;
- edge1 = N3-N1;
+ b3Vector3 planeEquation, edge0, edge1;
+ edge0 = N2 - N1;
+ edge1 = N3 - N1;
b3Scalar normalSign = b3Scalar(1.);
- for (int ww=0;ww<2;ww++)
+ for (int ww = 0; ww < 2; ww++)
{
planeEquation = normalSign * edge0.cross(edge1);
if (planeEquation.length2() > b3Scalar(0.0001))
{
planeEquation.normalize();
- if (notExist(planeEquation,planeEquationsOut))
+ if (notExist(planeEquation, planeEquationsOut))
{
planeEquation[3] = -planeEquation.dot(N1);
-
- //check if inside, and replace supportingVertexOut if needed
- if (areVerticesBehindPlane(planeEquation,vertices,b3Scalar(0.01)))
- {
- planeEquationsOut.push_back(planeEquation);
- }
+
+ //check if inside, and replace supportingVertexOut if needed
+ if (areVerticesBehindPlane(planeEquation, vertices, b3Scalar(0.01)))
+ {
+ planeEquationsOut.push_back(planeEquation);
+ }
}
}
normalSign = b3Scalar(-1.);
}
-
}
}
}
-
}
-void b3GeometryUtil::getVerticesFromPlaneEquations(const b3AlignedObjectArray<b3Vector3>& planeEquations , b3AlignedObjectArray<b3Vector3>& verticesOut )
+void b3GeometryUtil::getVerticesFromPlaneEquations(const b3AlignedObjectArray<b3Vector3>& planeEquations, b3AlignedObjectArray<b3Vector3>& verticesOut)
{
const int numbrushes = planeEquations.size();
// brute force:
- for (int i=0;i<numbrushes;i++)
+ for (int i = 0; i < numbrushes; i++)
{
const b3Vector3& N1 = planeEquations[i];
-
- for (int j=i+1;j<numbrushes;j++)
+ for (int j = i + 1; j < numbrushes; j++)
{
const b3Vector3& N2 = planeEquations[j];
-
- for (int k=j+1;k<numbrushes;k++)
- {
+ for (int k = j + 1; k < numbrushes; k++)
+ {
const b3Vector3& N3 = planeEquations[k];
- b3Vector3 n2n3; n2n3 = N2.cross(N3);
- b3Vector3 n3n1; n3n1 = N3.cross(N1);
- b3Vector3 n1n2; n1n2 = N1.cross(N2);
-
- if ( ( n2n3.length2() > b3Scalar(0.0001) ) &&
- ( n3n1.length2() > b3Scalar(0.0001) ) &&
- ( n1n2.length2() > b3Scalar(0.0001) ) )
+ b3Vector3 n2n3;
+ n2n3 = N2.cross(N3);
+ b3Vector3 n3n1;
+ n3n1 = N3.cross(N1);
+ b3Vector3 n1n2;
+ n1n2 = N1.cross(N2);
+
+ if ((n2n3.length2() > b3Scalar(0.0001)) &&
+ (n3n1.length2() > b3Scalar(0.0001)) &&
+ (n1n2.length2() > b3Scalar(0.0001)))
{
//point P out of 3 plane equations:
- // d1 ( N2 * N3 ) + d2 ( N3 * N1 ) + d3 ( N1 * N2 )
- //P = -------------------------------------------------------------------------
- // N1 . ( N2 * N3 )
-
+ // d1 ( N2 * N3 ) + d2 ( N3 * N1 ) + d3 ( N1 * N2 )
+ //P = -------------------------------------------------------------------------
+ // N1 . ( N2 * N3 )
b3Scalar quotient = (N1.dot(n2n3));
if (b3Fabs(quotient) > b3Scalar(0.000001))
@@ -172,7 +162,7 @@ void b3GeometryUtil::getVerticesFromPlaneEquations(const b3AlignedObjectArray<b3
potentialVertex *= quotient;
//check if inside, and replace supportingVertexOut if needed
- if (isPointInsidePlanes(planeEquations,potentialVertex,b3Scalar(0.01)))
+ if (isPointInsidePlanes(planeEquations, potentialVertex, b3Scalar(0.01)))
{
verticesOut.push_back(potentialVertex);
}
@@ -182,4 +172,3 @@ void b3GeometryUtil::getVerticesFromPlaneEquations(const b3AlignedObjectArray<b3
}
}
}
-
diff --git a/thirdparty/bullet/Bullet3Geometry/b3GeometryUtil.h b/thirdparty/bullet/Bullet3Geometry/b3GeometryUtil.h
index 8b5fd7ad62..967c8d67e9 100644
--- a/thirdparty/bullet/Bullet3Geometry/b3GeometryUtil.h
+++ b/thirdparty/bullet/Bullet3Geometry/b3GeometryUtil.h
@@ -12,7 +12,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
-
#ifndef B3_GEOMETRY_UTIL_H
#define B3_GEOMETRY_UTIL_H
@@ -22,21 +21,16 @@ subject to the following restrictions:
///The b3GeometryUtil helper class provides a few methods to convert between plane equations and vertices.
class b3GeometryUtil
{
- public:
-
-
- static void getPlaneEquationsFromVertices(b3AlignedObjectArray<b3Vector3>& vertices, b3AlignedObjectArray<b3Vector3>& planeEquationsOut );
-
- static void getVerticesFromPlaneEquations(const b3AlignedObjectArray<b3Vector3>& planeEquations , b3AlignedObjectArray<b3Vector3>& verticesOut );
-
- static bool isInside(const b3AlignedObjectArray<b3Vector3>& vertices, const b3Vector3& planeNormal, b3Scalar margin);
-
- static bool isPointInsidePlanes(const b3AlignedObjectArray<b3Vector3>& planeEquations, const b3Vector3& point, b3Scalar margin);
+public:
+ static void getPlaneEquationsFromVertices(b3AlignedObjectArray<b3Vector3>& vertices, b3AlignedObjectArray<b3Vector3>& planeEquationsOut);
- static bool areVerticesBehindPlane(const b3Vector3& planeNormal, const b3AlignedObjectArray<b3Vector3>& vertices, b3Scalar margin);
+ static void getVerticesFromPlaneEquations(const b3AlignedObjectArray<b3Vector3>& planeEquations, b3AlignedObjectArray<b3Vector3>& verticesOut);
-};
+ static bool isInside(const b3AlignedObjectArray<b3Vector3>& vertices, const b3Vector3& planeNormal, b3Scalar margin);
+ static bool isPointInsidePlanes(const b3AlignedObjectArray<b3Vector3>& planeEquations, const b3Vector3& point, b3Scalar margin);
-#endif //B3_GEOMETRY_UTIL_H
+ static bool areVerticesBehindPlane(const b3Vector3& planeNormal, const b3AlignedObjectArray<b3Vector3>& vertices, b3Scalar margin);
+};
+#endif //B3_GEOMETRY_UTIL_H
diff --git a/thirdparty/bullet/Bullet3Geometry/b3GrahamScan2dConvexHull.h b/thirdparty/bullet/Bullet3Geometry/b3GrahamScan2dConvexHull.h
index 1b933c5264..8881c9a638 100644
--- a/thirdparty/bullet/Bullet3Geometry/b3GrahamScan2dConvexHull.h
+++ b/thirdparty/bullet/Bullet3Geometry/b3GrahamScan2dConvexHull.h
@@ -13,41 +13,40 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
-
#ifndef B3_GRAHAM_SCAN_2D_CONVEX_HULL_H
#define B3_GRAHAM_SCAN_2D_CONVEX_HULL_H
-
#include "Bullet3Common/b3Vector3.h"
#include "Bullet3Common/b3AlignedObjectArray.h"
struct b3GrahamVector3 : public b3Vector3
{
b3GrahamVector3(const b3Vector3& org, int orgIndex)
- :b3Vector3(org),
- m_orgIndex(orgIndex)
+ : b3Vector3(org),
+ m_orgIndex(orgIndex)
{
}
- b3Scalar m_angle;
+ b3Scalar m_angle;
int m_orgIndex;
};
-
-struct b3AngleCompareFunc {
+struct b3AngleCompareFunc
+{
b3Vector3 m_anchor;
b3AngleCompareFunc(const b3Vector3& anchor)
- : m_anchor(anchor)
+ : m_anchor(anchor)
{
}
- bool operator()(const b3GrahamVector3& a, const b3GrahamVector3& b) const {
+ bool operator()(const b3GrahamVector3& a, const b3GrahamVector3& b) const
+ {
if (a.m_angle != b.m_angle)
return a.m_angle < b.m_angle;
else
{
- b3Scalar al = (a-m_anchor).length2();
- b3Scalar bl = (b-m_anchor).length2();
+ b3Scalar al = (a - m_anchor).length2();
+ b3Scalar bl = (b - m_anchor).length2();
if (al != bl)
- return al < bl;
+ return al < bl;
else
{
return a.m_orgIndex < b.m_orgIndex;
@@ -58,60 +57,60 @@ struct b3AngleCompareFunc {
inline void b3GrahamScanConvexHull2D(b3AlignedObjectArray<b3GrahamVector3>& originalPoints, b3AlignedObjectArray<b3GrahamVector3>& hull, const b3Vector3& normalAxis)
{
- b3Vector3 axis0,axis1;
- b3PlaneSpace1(normalAxis,axis0,axis1);
-
+ b3Vector3 axis0, axis1;
+ b3PlaneSpace1(normalAxis, axis0, axis1);
- if (originalPoints.size()<=1)
+ if (originalPoints.size() <= 1)
{
- for (int i=0;i<originalPoints.size();i++)
+ for (int i = 0; i < originalPoints.size(); i++)
hull.push_back(originalPoints[0]);
return;
}
//step1 : find anchor point with smallest projection on axis0 and move it to first location
- for (int i=0;i<originalPoints.size();i++)
+ for (int i = 0; i < originalPoints.size(); i++)
{
-// const b3Vector3& left = originalPoints[i];
-// const b3Vector3& right = originalPoints[0];
+ // const b3Vector3& left = originalPoints[i];
+ // const b3Vector3& right = originalPoints[0];
b3Scalar projL = originalPoints[i].dot(axis0);
b3Scalar projR = originalPoints[0].dot(axis0);
if (projL < projR)
{
- originalPoints.swap(0,i);
+ originalPoints.swap(0, i);
}
}
//also precompute angles
originalPoints[0].m_angle = -1e30f;
- for (int i=1;i<originalPoints.size();i++)
+ for (int i = 1; i < originalPoints.size(); i++)
{
b3Vector3 xvec = axis0;
- b3Vector3 ar = originalPoints[i]-originalPoints[0];
+ b3Vector3 ar = originalPoints[i] - originalPoints[0];
originalPoints[i].m_angle = b3Cross(xvec, ar).dot(normalAxis) / ar.length();
}
//step 2: sort all points, based on 'angle' with this anchor
b3AngleCompareFunc comp(originalPoints[0]);
- originalPoints.quickSortInternal(comp,1,originalPoints.size()-1);
+ originalPoints.quickSortInternal(comp, 1, originalPoints.size() - 1);
int i;
- for (i = 0; i<2; i++)
+ for (i = 0; i < 2; i++)
hull.push_back(originalPoints[i]);
//step 3: keep all 'convex' points and discard concave points (using back tracking)
- for (; i != originalPoints.size(); i++)
+ for (; i != originalPoints.size(); i++)
{
bool isConvex = false;
- while (!isConvex&& hull.size()>1) {
- b3Vector3& a = hull[hull.size()-2];
- b3Vector3& b = hull[hull.size()-1];
- isConvex = b3Cross(a-b,a-originalPoints[i]).dot(normalAxis)> 0;
+ while (!isConvex && hull.size() > 1)
+ {
+ b3Vector3& a = hull[hull.size() - 2];
+ b3Vector3& b = hull[hull.size() - 1];
+ isConvex = b3Cross(a - b, a - originalPoints[i]).dot(normalAxis) > 0;
if (!isConvex)
hull.pop_back();
- else
+ else
hull.push_back(originalPoints[i]);
}
}
}
-#endif //B3_GRAHAM_SCAN_2D_CONVEX_HULL_H
+#endif //B3_GRAHAM_SCAN_2D_CONVEX_HULL_H
generated by cgit v1.2.3 (git 2.25.1) at 2024-12-16 11:33:09 +0000