diff options
Diffstat (limited to 'thirdparty/vhacd/src')
| -rw-r--r-- | thirdparty/vhacd/src/FloatMath.cpp | 17 | ||||
| -rw-r--r-- | thirdparty/vhacd/src/FloatMath.inl | 5276 | ||||
| -rw-r--r-- | thirdparty/vhacd/src/VHACD-ASYNC.cpp | 334 | ||||
| -rw-r--r-- | thirdparty/vhacd/src/VHACD.cpp | 1589 | ||||
| -rw-r--r-- | thirdparty/vhacd/src/btAlignedAllocator.cpp | 192 | ||||
| -rw-r--r-- | thirdparty/vhacd/src/btConvexHullComputer.cpp | 2487 | ||||
| -rw-r--r-- | thirdparty/vhacd/src/vhacdICHull.cpp | 731 | ||||
| -rw-r--r-- | thirdparty/vhacd/src/vhacdManifoldMesh.cpp | 202 | ||||
| -rw-r--r-- | thirdparty/vhacd/src/vhacdMesh.cpp | 376 | ||||
| -rw-r--r-- | thirdparty/vhacd/src/vhacdRaycastMesh.cpp | 208 | ||||
| -rw-r--r-- | thirdparty/vhacd/src/vhacdVolume.cpp | 1626 |
11 files changed, 13038 insertions, 0 deletions
diff --git a/thirdparty/vhacd/src/FloatMath.cpp b/thirdparty/vhacd/src/FloatMath.cpp new file mode 100644 index 0000000000..481e875104 --- /dev/null +++ b/thirdparty/vhacd/src/FloatMath.cpp @@ -0,0 +1,17 @@ +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <assert.h> +#include <math.h> +#include <float.h> +#include "FloatMath.h" +#include <vector> + +#define REAL float + +#include "FloatMath.inl" + +#undef REAL +#define REAL double + +#include "FloatMath.inl" diff --git a/thirdparty/vhacd/src/FloatMath.inl b/thirdparty/vhacd/src/FloatMath.inl new file mode 100644 index 0000000000..ce529e6f71 --- /dev/null +++ b/thirdparty/vhacd/src/FloatMath.inl @@ -0,0 +1,5276 @@ +// a set of routines that let you do common 3d math +// operations without any vector, matrix, or quaternion +// classes or templates. +// +// a vector (or point) is a 'float *' to 3 floating point numbers. +// a matrix is a 'float *' to an array of 16 floating point numbers representing a 4x4 transformation matrix compatible with D3D or OGL +// a quaternion is a 'float *' to 4 floats representing a quaternion x,y,z,w +// + +namespace FLOAT_MATH +{ + +void fm_inverseRT(const REAL matrix[16],const REAL pos[3],REAL t[3]) // inverse rotate translate the point. +{ + + REAL _x = pos[0] - matrix[3*4+0]; + REAL _y = pos[1] - matrix[3*4+1]; + REAL _z = pos[2] - matrix[3*4+2]; + + // Multiply inverse-translated source vector by inverted rotation transform + + t[0] = (matrix[0*4+0] * _x) + (matrix[0*4+1] * _y) + (matrix[0*4+2] * _z); + t[1] = (matrix[1*4+0] * _x) + (matrix[1*4+1] * _y) + (matrix[1*4+2] * _z); + t[2] = (matrix[2*4+0] * _x) + (matrix[2*4+1] * _y) + (matrix[2*4+2] * _z); + +} + +REAL fm_getDeterminant(const REAL matrix[16]) +{ + REAL tempv[3]; + REAL p0[3]; + REAL p1[3]; + REAL p2[3]; + + + p0[0] = matrix[0*4+0]; + p0[1] = matrix[0*4+1]; + p0[2] = matrix[0*4+2]; + + p1[0] = matrix[1*4+0]; + p1[1] = matrix[1*4+1]; + p1[2] = matrix[1*4+2]; + + p2[0] = matrix[2*4+0]; + p2[1] = matrix[2*4+1]; + p2[2] = matrix[2*4+2]; + + fm_cross(tempv,p1,p2); + + return fm_dot(p0,tempv); + +} + +REAL fm_squared(REAL x) { return x*x; }; + +void fm_decomposeTransform(const REAL local_transform[16],REAL trans[3],REAL rot[4],REAL scale[3]) +{ + + trans[0] = local_transform[12]; + trans[1] = local_transform[13]; + trans[2] = local_transform[14]; + + scale[0] = (REAL)sqrt(fm_squared(local_transform[0*4+0]) + fm_squared(local_transform[0*4+1]) + fm_squared(local_transform[0*4+2])); + scale[1] = (REAL)sqrt(fm_squared(local_transform[1*4+0]) + fm_squared(local_transform[1*4+1]) + fm_squared(local_transform[1*4+2])); + scale[2] = (REAL)sqrt(fm_squared(local_transform[2*4+0]) + fm_squared(local_transform[2*4+1]) + fm_squared(local_transform[2*4+2])); + + REAL m[16]; + memcpy(m,local_transform,sizeof(REAL)*16); + + REAL sx = 1.0f / scale[0]; + REAL sy = 1.0f / scale[1]; + REAL sz = 1.0f / scale[2]; + + m[0*4+0]*=sx; + m[0*4+1]*=sx; + m[0*4+2]*=sx; + + m[1*4+0]*=sy; + m[1*4+1]*=sy; + m[1*4+2]*=sy; + + m[2*4+0]*=sz; + m[2*4+1]*=sz; + m[2*4+2]*=sz; + + fm_matrixToQuat(m,rot); + +} + +void fm_getSubMatrix(int32_t ki,int32_t kj,REAL pDst[16],const REAL matrix[16]) +{ + int32_t row, col; + int32_t dstCol = 0, dstRow = 0; + + for ( col = 0; col < 4; col++ ) + { + if ( col == kj ) + { + continue; + } + for ( dstRow = 0, row = 0; row < 4; row++ ) + { + if ( row == ki ) + { + continue; + } + pDst[dstCol*4+dstRow] = matrix[col*4+row]; + dstRow++; + } + dstCol++; + } +} + +void fm_inverseTransform(const REAL matrix[16],REAL inverse_matrix[16]) +{ + REAL determinant = fm_getDeterminant(matrix); + determinant = 1.0f / determinant; + for (int32_t i = 0; i < 4; i++ ) + { + for (int32_t j = 0; j < 4; j++ ) + { + int32_t sign = 1 - ( ( i + j ) % 2 ) * 2; + REAL subMat[16]; + fm_identity(subMat); + fm_getSubMatrix( i, j, subMat, matrix ); + REAL subDeterminant = fm_getDeterminant(subMat); + inverse_matrix[i*4+j] = ( subDeterminant * sign ) * determinant; + } + } +} + +void fm_identity(REAL matrix[16]) // set 4x4 matrix to identity. +{ + matrix[0*4+0] = 1; + matrix[1*4+1] = 1; + matrix[2*4+2] = 1; + matrix[3*4+3] = 1; + + matrix[1*4+0] = 0; + matrix[2*4+0] = 0; + matrix[3*4+0] = 0; + + matrix[0*4+1] = 0; + matrix[2*4+1] = 0; + matrix[3*4+1] = 0; + + matrix[0*4+2] = 0; + matrix[1*4+2] = 0; + matrix[3*4+2] = 0; + + matrix[0*4+3] = 0; + matrix[1*4+3] = 0; + matrix[2*4+3] = 0; + +} + +void fm_quatToEuler(const REAL quat[4],REAL &ax,REAL &ay,REAL &az) +{ + REAL x = quat[0]; + REAL y = quat[1]; + REAL z = quat[2]; + REAL w = quat[3]; + + REAL sint = (2.0f * w * y) - (2.0f * x * z); + REAL cost_temp = 1.0f - (sint * sint); + REAL cost = 0; + + if ( (REAL)fabs(cost_temp) > 0.001f ) + { + cost = (REAL)sqrt( cost_temp ); + } + + REAL sinv, cosv, sinf, cosf; + if ( (REAL)fabs(cost) > 0.001f ) + { + cost = 1.0f / cost; + sinv = ((2.0f * y * z) + (2.0f * w * x)) * cost; + cosv = (1.0f - (2.0f * x * x) - (2.0f * y * y)) * cost; + sinf = ((2.0f * x * y) + (2.0f * w * z)) * cost; + cosf = (1.0f - (2.0f * y * y) - (2.0f * z * z)) * cost; + } + else + { + sinv = (2.0f * w * x) - (2.0f * y * z); + cosv = 1.0f - (2.0f * x * x) - (2.0f * z * z); + sinf = 0; + cosf = 1.0f; + } + + // compute output rotations + ax = (REAL)atan2( sinv, cosv ); + ay = (REAL)atan2( sint, cost ); + az = (REAL)atan2( sinf, cosf ); + +} + +void fm_eulerToMatrix(REAL ax,REAL ay,REAL az,REAL *matrix) // convert euler (in radians) to a dest 4x4 matrix (translation set to zero) +{ + REAL quat[4]; + fm_eulerToQuat(ax,ay,az,quat); + fm_quatToMatrix(quat,matrix); +} + +void fm_getAABB(uint32_t vcount,const REAL *points,uint32_t pstride,REAL *bmin,REAL *bmax) +{ + + const uint8_t *source = (const uint8_t *) points; + + bmin[0] = points[0]; + bmin[1] = points[1]; + bmin[2] = points[2]; + + bmax[0] = points[0]; + bmax[1] = points[1]; + bmax[2] = points[2]; + + + for (uint32_t i=1; i<vcount; i++) + { + source+=pstride; + const REAL *p = (const REAL *) source; + + if ( p[0] < bmin[0] ) bmin[0] = p[0]; + if ( p[1] < bmin[1] ) bmin[1] = p[1]; + if ( p[2] < bmin[2] ) bmin[2] = p[2]; + + if ( p[0] > bmax[0] ) bmax[0] = p[0]; + if ( p[1] > bmax[1] ) bmax[1] = p[1]; + if ( p[2] > bmax[2] ) bmax[2] = p[2]; + + } +} + +void fm_eulerToQuat(const REAL *euler,REAL *quat) // convert euler angles to quaternion. +{ + fm_eulerToQuat(euler[0],euler[1],euler[2],quat); +} + +void fm_eulerToQuat(REAL roll,REAL pitch,REAL yaw,REAL *quat) // convert euler angles to quaternion. +{ + roll *= 0.5f; + pitch *= 0.5f; + yaw *= 0.5f; + + REAL cr = (REAL)cos(roll); + REAL cp = (REAL)cos(pitch); + REAL cy = (REAL)cos(yaw); + + REAL sr = (REAL)sin(roll); + REAL sp = (REAL)sin(pitch); + REAL sy = (REAL)sin(yaw); + + REAL cpcy = cp * cy; + REAL spsy = sp * sy; + REAL spcy = sp * cy; + REAL cpsy = cp * sy; + + quat[0] = ( sr * cpcy - cr * spsy); + quat[1] = ( cr * spcy + sr * cpsy); + quat[2] = ( cr * cpsy - sr * spcy); + quat[3] = cr * cpcy + sr * spsy; +} + +void fm_quatToMatrix(const REAL *quat,REAL *matrix) // convert quaterinion rotation to matrix, zeros out the translation component. +{ + + REAL xx = quat[0]*quat[0]; + REAL yy = quat[1]*quat[1]; + REAL zz = quat[2]*quat[2]; + REAL xy = quat[0]*quat[1]; + REAL xz = quat[0]*quat[2]; + REAL yz = quat[1]*quat[2]; + REAL wx = quat[3]*quat[0]; + REAL wy = quat[3]*quat[1]; + REAL wz = quat[3]*quat[2]; + + matrix[0*4+0] = 1 - 2 * ( yy + zz ); + matrix[1*4+0] = 2 * ( xy - wz ); + matrix[2*4+0] = 2 * ( xz + wy ); + + matrix[0*4+1] = 2 * ( xy + wz ); + matrix[1*4+1] = 1 - 2 * ( xx + zz ); + matrix[2*4+1] = 2 * ( yz - wx ); + + matrix[0*4+2] = 2 * ( xz - wy ); + matrix[1*4+2] = 2 * ( yz + wx ); + matrix[2*4+2] = 1 - 2 * ( xx + yy ); + + matrix[3*4+0] = matrix[3*4+1] = matrix[3*4+2] = (REAL) 0.0f; + matrix[0*4+3] = matrix[1*4+3] = matrix[2*4+3] = (REAL) 0.0f; + matrix[3*4+3] =(REAL) 1.0f; + +} + + +void fm_quatRotate(const REAL *quat,const REAL *v,REAL *r) // rotate a vector directly by a quaternion. +{ + REAL left[4]; + + left[0] = quat[3]*v[0] + quat[1]*v[2] - v[1]*quat[2]; + left[1] = quat[3]*v[1] + quat[2]*v[0] - v[2]*quat[0]; + left[2] = quat[3]*v[2] + quat[0]*v[1] - v[0]*quat[1]; + left[3] = - quat[0]*v[0] - quat[1]*v[1] - quat[2]*v[2]; + + r[0] = (left[3]*-quat[0]) + (quat[3]*left[0]) + (left[1]*-quat[2]) - (-quat[1]*left[2]); + r[1] = (left[3]*-quat[1]) + (quat[3]*left[1]) + (left[2]*-quat[0]) - (-quat[2]*left[0]); + r[2] = (left[3]*-quat[2]) + (quat[3]*left[2]) + (left[0]*-quat[1]) - (-quat[0]*left[1]); + +} + + +void fm_getTranslation(const REAL *matrix,REAL *t) +{ + t[0] = matrix[3*4+0]; + t[1] = matrix[3*4+1]; + t[2] = matrix[3*4+2]; +} + +void fm_matrixToQuat(const REAL *matrix,REAL *quat) // convert the 3x3 portion of a 4x4 matrix into a quaterion as x,y,z,w +{ + + REAL tr = matrix[0*4+0] + matrix[1*4+1] + matrix[2*4+2]; + + // check the diagonal + + if (tr > 0.0f ) + { + REAL s = (REAL) sqrt ( (double) (tr + 1.0f) ); + quat[3] = s * 0.5f; + s = 0.5f / s; + quat[0] = (matrix[1*4+2] - matrix[2*4+1]) * s; + quat[1] = (matrix[2*4+0] - matrix[0*4+2]) * s; + quat[2] = (matrix[0*4+1] - matrix[1*4+0]) * s; + + } + else + { + // diagonal is negative + int32_t nxt[3] = {1, 2, 0}; + REAL qa[4]; + + int32_t i = 0; + + if (matrix[1*4+1] > matrix[0*4+0]) i = 1; + if (matrix[2*4+2] > matrix[i*4+i]) i = 2; + + int32_t j = nxt[i]; + int32_t k = nxt[j]; + + REAL s = (REAL)sqrt ( ((matrix[i*4+i] - (matrix[j*4+j] + matrix[k*4+k])) + 1.0f) ); + + qa[i] = s * 0.5f; + + if (s != 0.0f ) s = 0.5f / s; + + qa[3] = (matrix[j*4+k] - matrix[k*4+j]) * s; + qa[j] = (matrix[i*4+j] + matrix[j*4+i]) * s; + qa[k] = (matrix[i*4+k] + matrix[k*4+i]) * s; + + quat[0] = qa[0]; + quat[1] = qa[1]; + quat[2] = qa[2]; + quat[3] = qa[3]; + } +// fm_normalizeQuat(quat); +} + + +REAL fm_sphereVolume(REAL radius) // return's the volume of a sphere of this radius (4/3 PI * R cubed ) +{ + return (4.0f / 3.0f ) * FM_PI * radius * radius * radius; +} + + +REAL fm_cylinderVolume(REAL radius,REAL h) +{ + return FM_PI * radius * radius *h; +} + +REAL fm_capsuleVolume(REAL radius,REAL h) +{ + REAL volume = fm_sphereVolume(radius); // volume of the sphere portion. + REAL ch = h-radius*2; // this is the cylinder length + if ( ch > 0 ) + { + volume+=fm_cylinderVolume(radius,ch); + } + return volume; +} + +void fm_transform(const REAL matrix[16],const REAL v[3],REAL t[3]) // rotate and translate this point +{ + if ( matrix ) + { + REAL tx = (matrix[0*4+0] * v[0]) + (matrix[1*4+0] * v[1]) + (matrix[2*4+0] * v[2]) + matrix[3*4+0]; + REAL ty = (matrix[0*4+1] * v[0]) + (matrix[1*4+1] * v[1]) + (matrix[2*4+1] * v[2]) + matrix[3*4+1]; + REAL tz = (matrix[0*4+2] * v[0]) + (matrix[1*4+2] * v[1]) + (matrix[2*4+2] * v[2]) + matrix[3*4+2]; + t[0] = tx; + t[1] = ty; + t[2] = tz; + } + else + { + t[0] = v[0]; + t[1] = v[1]; + t[2] = v[2]; + } +} + +void fm_rotate(const REAL matrix[16],const REAL v[3],REAL t[3]) // rotate and translate this point +{ + if ( matrix ) + { + REAL tx = (matrix[0*4+0] * v[0]) + (matrix[1*4+0] * v[1]) + (matrix[2*4+0] * v[2]); + REAL ty = (matrix[0*4+1] * v[0]) + (matrix[1*4+1] * v[1]) + (matrix[2*4+1] * v[2]); + REAL tz = (matrix[0*4+2] * v[0]) + (matrix[1*4+2] * v[1]) + (matrix[2*4+2] * v[2]); + t[0] = tx; + t[1] = ty; + t[2] = tz; + } + else + { + t[0] = v[0]; + t[1] = v[1]; + t[2] = v[2]; + } +} + + +REAL fm_distance(const REAL *p1,const REAL *p2) +{ + REAL dx = p1[0] - p2[0]; + REAL dy = p1[1] - p2[1]; + REAL dz = p1[2] - p2[2]; + + return (REAL)sqrt( dx*dx + dy*dy + dz *dz ); +} + +REAL fm_distanceSquared(const REAL *p1,const REAL *p2) +{ + REAL dx = p1[0] - p2[0]; + REAL dy = p1[1] - p2[1]; + REAL dz = p1[2] - p2[2]; + + return dx*dx + dy*dy + dz *dz; +} + + +REAL fm_distanceSquaredXZ(const REAL *p1,const REAL *p2) +{ + REAL dx = p1[0] - p2[0]; + REAL dz = p1[2] - p2[2]; + + return dx*dx + dz *dz; +} + + +REAL fm_computePlane(const REAL *A,const REAL *B,const REAL *C,REAL *n) // returns D +{ + REAL vx = (B[0] - C[0]); + REAL vy = (B[1] - C[1]); + REAL vz = (B[2] - C[2]); + + REAL wx = (A[0] - B[0]); + REAL wy = (A[1] - B[1]); + REAL wz = (A[2] - B[2]); + + REAL vw_x = vy * wz - vz * wy; + REAL vw_y = vz * wx - vx * wz; + REAL vw_z = vx * wy - vy * wx; + + REAL mag = (REAL)sqrt((vw_x * vw_x) + (vw_y * vw_y) + (vw_z * vw_z)); + + if ( mag < 0.000001f ) + { + mag = 0; + } + else + { + mag = 1.0f/mag; + } + + REAL x = vw_x * mag; + REAL y = vw_y * mag; + REAL z = vw_z * mag; + + + REAL D = 0.0f - ((x*A[0])+(y*A[1])+(z*A[2])); + + n[0] = x; + n[1] = y; + n[2] = z; + + return D; +} + +REAL fm_distToPlane(const REAL *plane,const REAL *p) // computes the distance of this point from the plane. +{ + return p[0]*plane[0]+p[1]*plane[1]+p[2]*plane[2]+plane[3]; +} + +REAL fm_dot(const REAL *p1,const REAL *p2) +{ + return p1[0]*p2[0]+p1[1]*p2[1]+p1[2]*p2[2]; +} + +void fm_cross(REAL *cross,const REAL *a,const REAL *b) +{ + cross[0] = a[1]*b[2] - a[2]*b[1]; + cross[1] = a[2]*b[0] - a[0]*b[2]; + cross[2] = a[0]*b[1] - a[1]*b[0]; +} + +REAL fm_computeNormalVector(REAL *n,const REAL *p1,const REAL *p2) +{ + n[0] = p2[0] - p1[0]; + n[1] = p2[1] - p1[1]; + n[2] = p2[2] - p1[2]; + return fm_normalize(n); +} + +bool fm_computeWindingOrder(const REAL *p1,const REAL *p2,const REAL *p3) // returns true if the triangle is clockwise. +{ + bool ret = false; + + REAL v1[3]; + REAL v2[3]; + + fm_computeNormalVector(v1,p1,p2); // p2-p1 (as vector) and then normalized + fm_computeNormalVector(v2,p1,p3); // p3-p1 (as vector) and then normalized + + REAL cross[3]; + + fm_cross(cross, v1, v2 ); + REAL ref[3] = { 1, 0, 0 }; + + REAL d = fm_dot( cross, ref ); + + + if ( d <= 0 ) + ret = false; + else + ret = true; + + return ret; +} + +REAL fm_normalize(REAL *n) // normalize this vector +{ + REAL dist = (REAL)sqrt(n[0]*n[0] + n[1]*n[1] + n[2]*n[2]); + if ( dist > 0.0000001f ) + { + REAL mag = 1.0f / dist; + n[0]*=mag; + n[1]*=mag; + n[2]*=mag; + } + else + { + n[0] = 1; + n[1] = 0; + n[2] = 0; + } + + return dist; +} + + +void fm_matrixMultiply(const REAL *pA,const REAL *pB,REAL *pM) +{ +#if 1 + + REAL a = pA[0*4+0] * pB[0*4+0] + pA[0*4+1] * pB[1*4+0] + pA[0*4+2] * pB[2*4+0] + pA[0*4+3] * pB[3*4+0]; + REAL b = pA[0*4+0] * pB[0*4+1] + pA[0*4+1] * pB[1*4+1] + pA[0*4+2] * pB[2*4+1] + pA[0*4+3] * pB[3*4+1]; + REAL c = pA[0*4+0] * pB[0*4+2] + pA[0*4+1] * pB[1*4+2] + pA[0*4+2] * pB[2*4+2] + pA[0*4+3] * pB[3*4+2]; + REAL d = pA[0*4+0] * pB[0*4+3] + pA[0*4+1] * pB[1*4+3] + pA[0*4+2] * pB[2*4+3] + pA[0*4+3] * pB[3*4+3]; + + REAL e = pA[1*4+0] * pB[0*4+0] + pA[1*4+1] * pB[1*4+0] + pA[1*4+2] * pB[2*4+0] + pA[1*4+3] * pB[3*4+0]; + REAL f = pA[1*4+0] * pB[0*4+1] + pA[1*4+1] * pB[1*4+1] + pA[1*4+2] * pB[2*4+1] + pA[1*4+3] * pB[3*4+1]; + REAL g = pA[1*4+0] * pB[0*4+2] + pA[1*4+1] * pB[1*4+2] + pA[1*4+2] * pB[2*4+2] + pA[1*4+3] * pB[3*4+2]; + REAL h = pA[1*4+0] * pB[0*4+3] + pA[1*4+1] * pB[1*4+3] + pA[1*4+2] * pB[2*4+3] + pA[1*4+3] * pB[3*4+3]; + + REAL i = pA[2*4+0] * pB[0*4+0] + pA[2*4+1] * pB[1*4+0] + pA[2*4+2] * pB[2*4+0] + pA[2*4+3] * pB[3*4+0]; + REAL j = pA[2*4+0] * pB[0*4+1] + pA[2*4+1] * pB[1*4+1] + pA[2*4+2] * pB[2*4+1] + pA[2*4+3] * pB[3*4+1]; + REAL k = pA[2*4+0] * pB[0*4+2] + pA[2*4+1] * pB[1*4+2] + pA[2*4+2] * pB[2*4+2] + pA[2*4+3] * pB[3*4+2]; + REAL l = pA[2*4+0] * pB[0*4+3] + pA[2*4+1] * pB[1*4+3] + pA[2*4+2] * pB[2*4+3] + pA[2*4+3] * pB[3*4+3]; + + REAL m = pA[3*4+0] * pB[0*4+0] + pA[3*4+1] * pB[1*4+0] + pA[3*4+2] * pB[2*4+0] + pA[3*4+3] * pB[3*4+0]; + REAL n = pA[3*4+0] * pB[0*4+1] + pA[3*4+1] * pB[1*4+1] + pA[3*4+2] * pB[2*4+1] + pA[3*4+3] * pB[3*4+1]; + REAL o = pA[3*4+0] * pB[0*4+2] + pA[3*4+1] * pB[1*4+2] + pA[3*4+2] * pB[2*4+2] + pA[3*4+3] * pB[3*4+2]; + REAL p = pA[3*4+0] * pB[0*4+3] + pA[3*4+1] * pB[1*4+3] + pA[3*4+2] * pB[2*4+3] + pA[3*4+3] * pB[3*4+3]; + + pM[0] = a; + pM[1] = b; + pM[2] = c; + pM[3] = d; + + pM[4] = e; + pM[5] = f; + pM[6] = g; + pM[7] = h; + + pM[8] = i; + pM[9] = j; + pM[10] = k; + pM[11] = l; + + pM[12] = m; + pM[13] = n; + pM[14] = o; + pM[15] = p; + + +#else + memset(pM, 0, sizeof(REAL)*16); + for(int32_t i=0; i<4; i++ ) + for(int32_t j=0; j<4; j++ ) + for(int32_t k=0; k<4; k++ ) + pM[4*i+j] += pA[4*i+k] * pB[4*k+j]; +#endif +} + + +void fm_eulerToQuatDX(REAL x,REAL y,REAL z,REAL *quat) // convert euler angles to quaternion using the fucked up DirectX method +{ + REAL matrix[16]; + fm_eulerToMatrix(x,y,z,matrix); + fm_matrixToQuat(matrix,quat); +} + +// implementation copied from: http://blogs.msdn.com/mikepelton/archive/2004/10/29/249501.aspx +void fm_eulerToMatrixDX(REAL x,REAL y,REAL z,REAL *matrix) // convert euler angles to quaternion using the fucked up DirectX method. +{ + fm_identity(matrix); + matrix[0*4+0] = (REAL)(cos(z)*cos(y) + sin(z)*sin(x)*sin(y)); + matrix[0*4+1] = (REAL)(sin(z)*cos(x)); + matrix[0*4+2] = (REAL)(cos(z)*-sin(y) + sin(z)*sin(x)*cos(y)); + + matrix[1*4+0] = (REAL)(-sin(z)*cos(y)+cos(z)*sin(x)*sin(y)); + matrix[1*4+1] = (REAL)(cos(z)*cos(x)); + matrix[1*4+2] = (REAL)(sin(z)*sin(y) +cos(z)*sin(x)*cos(y)); + + matrix[2*4+0] = (REAL)(cos(x)*sin(y)); + matrix[2*4+1] = (REAL)(-sin(x)); + matrix[2*4+2] = (REAL)(cos(x)*cos(y)); +} + + +void fm_scale(REAL x,REAL y,REAL z,REAL *fscale) // apply scale to the matrix. +{ + fscale[0*4+0] = x; + fscale[1*4+1] = y; + fscale[2*4+2] = z; +} + + +void fm_composeTransform(const REAL *position,const REAL *quat,const REAL *scale,REAL *matrix) +{ + fm_identity(matrix); + fm_quatToMatrix(quat,matrix); + + if ( scale && ( scale[0] != 1 || scale[1] != 1 || scale[2] != 1 ) ) + { + REAL work[16]; + memcpy(work,matrix,sizeof(REAL)*16); + REAL mscale[16]; + fm_identity(mscale); + fm_scale(scale[0],scale[1],scale[2],mscale); + fm_matrixMultiply(work,mscale,matrix); + } + + matrix[12] = position[0]; + matrix[13] = position[1]; + matrix[14] = position[2]; +} + + +void fm_setTranslation(const REAL *translation,REAL *matrix) +{ + matrix[12] = translation[0]; + matrix[13] = translation[1]; + matrix[14] = translation[2]; +} + +static REAL enorm0_3d ( REAL x0, REAL y0, REAL z0, REAL x1, REAL y1, REAL z1 ) + +/**********************************************************************/ + +/* +Purpose: + +ENORM0_3D computes the Euclidean norm of (P1-P0) in 3D. + +Modified: + +18 April 1999 + +Author: + +John Burkardt + +Parameters: + +Input, REAL X0, Y0, Z0, X1, Y1, Z1, the coordinates of the points +P0 and P1. + +Output, REAL ENORM0_3D, the Euclidean norm of (P1-P0). +*/ +{ + REAL value; + + value = (REAL)sqrt ( + ( x1 - x0 ) * ( x1 - x0 ) + + ( y1 - y0 ) * ( y1 - y0 ) + + ( z1 - z0 ) * ( z1 - z0 ) ); + + return value; +} + + +static REAL triangle_area_3d ( REAL x1, REAL y1, REAL z1, REAL x2,REAL y2, REAL z2, REAL x3, REAL y3, REAL z3 ) + + /**********************************************************************/ + + /* + Purpose: + + TRIANGLE_AREA_3D computes the area of a triangle in 3D. + + Modified: + + 22 April 1999 + + Author: + + John Burkardt + + Parameters: + + Input, REAL X1, Y1, Z1, X2, Y2, Z2, X3, Y3, Z3, the (X,Y,Z) + coordinates of the corners of the triangle. + + Output, REAL TRIANGLE_AREA_3D, the area of the triangle. + */ +{ + REAL a; + REAL alpha; + REAL area; + REAL b; + REAL base; + REAL c; + REAL dot; + REAL height; + /* + Find the projection of (P3-P1) onto (P2-P1). + */ + dot = + ( x2 - x1 ) * ( x3 - x1 ) + + ( y2 - y1 ) * ( y3 - y1 ) + + ( z2 - z1 ) * ( z3 - z1 ); + + base = enorm0_3d ( x1, y1, z1, x2, y2, z2 ); + /* + The height of the triangle is the length of (P3-P1) after its + projection onto (P2-P1) has been subtracted. + */ + if ( base == 0.0 ) { + + height = 0.0; + + } + else { + + alpha = dot / ( base * base ); + + a = x3 - x1 - alpha * ( x2 - x1 ); + b = y3 - y1 - alpha * ( y2 - y1 ); + c = z3 - z1 - alpha * ( z2 - z1 ); + + height = (REAL)sqrt ( a * a + b * b + c * c ); + + } + + area = 0.5f * base * height; + + return area; +} + + +REAL fm_computeArea(const REAL *p1,const REAL *p2,const REAL *p3) +{ + REAL ret = 0; + + ret = triangle_area_3d(p1[0],p1[1],p1[2],p2[0],p2[1],p2[2],p3[0],p3[1],p3[2]); + + return ret; +} + + +void fm_lerp(const REAL *p1,const REAL *p2,REAL *dest,REAL lerpValue) +{ + dest[0] = ((p2[0] - p1[0])*lerpValue) + p1[0]; + dest[1] = ((p2[1] - p1[1])*lerpValue) + p1[1]; + dest[2] = ((p2[2] - p1[2])*lerpValue) + p1[2]; +} + +bool fm_pointTestXZ(const REAL *p,const REAL *i,const REAL *j) +{ + bool ret = false; + + if (((( i[2] <= p[2] ) && ( p[2] < j[2] )) || (( j[2] <= p[2] ) && ( p[2] < i[2] ))) && ( p[0] < (j[0] - i[0]) * (p[2] - i[2]) / (j[2] - i[2]) + i[0])) + ret = true; + + return ret; +}; + + +bool fm_insideTriangleXZ(const REAL *p,const REAL *p1,const REAL *p2,const REAL *p3) +{ + bool ret = false; + + int32_t c = 0; + if ( fm_pointTestXZ(p,p1,p2) ) c = !c; + if ( fm_pointTestXZ(p,p2,p3) ) c = !c; + if ( fm_pointTestXZ(p,p3,p1) ) c = !c; + if ( c ) ret = true; + + return ret; +} + +bool fm_insideAABB(const REAL *pos,const REAL *bmin,const REAL *bmax) +{ + bool ret = false; + + if ( pos[0] >= bmin[0] && pos[0] <= bmax[0] && + pos[1] >= bmin[1] && pos[1] <= bmax[1] && + pos[2] >= bmin[2] && pos[2] <= bmax[2] ) + ret = true; + + return ret; +} + + +uint32_t fm_clipTestPoint(const REAL *bmin,const REAL *bmax,const REAL *pos) +{ + uint32_t ret = 0; + + if ( pos[0] < bmin[0] ) + ret|=FMCS_XMIN; + else if ( pos[0] > bmax[0] ) + ret|=FMCS_XMAX; + + if ( pos[1] < bmin[1] ) + ret|=FMCS_YMIN; + else if ( pos[1] > bmax[1] ) + ret|=FMCS_YMAX; + + if ( pos[2] < bmin[2] ) + ret|=FMCS_ZMIN; + else if ( pos[2] > bmax[2] ) + ret|=FMCS_ZMAX; + + return ret; +} + +uint32_t fm_clipTestPointXZ(const REAL *bmin,const REAL *bmax,const REAL *pos) // only tests X and Z, not Y +{ + uint32_t ret = 0; + + if ( pos[0] < bmin[0] ) + ret|=FMCS_XMIN; + else if ( pos[0] > bmax[0] ) + ret|=FMCS_XMAX; + + if ( pos[2] < bmin[2] ) + ret|=FMCS_ZMIN; + else if ( pos[2] > bmax[2] ) + ret|=FMCS_ZMAX; + + return ret; +} + +uint32_t fm_clipTestAABB(const REAL *bmin,const REAL *bmax,const REAL *p1,const REAL *p2,const REAL *p3,uint32_t &andCode) +{ + uint32_t orCode = 0; + + andCode = FMCS_XMIN | FMCS_XMAX | FMCS_YMIN | FMCS_YMAX | FMCS_ZMIN | FMCS_ZMAX; + + uint32_t c = fm_clipTestPoint(bmin,bmax,p1); + orCode|=c; + andCode&=c; + + c = fm_clipTestPoint(bmin,bmax,p2); + orCode|=c; + andCode&=c; + + c = fm_clipTestPoint(bmin,bmax,p3); + orCode|=c; + andCode&=c; + + return orCode; +} + +bool intersect(const REAL *si,const REAL *ei,const REAL *bmin,const REAL *bmax,REAL *time) +{ + REAL st,et,fst = 0,fet = 1; + + for (int32_t i = 0; i < 3; i++) + { + if (*si < *ei) + { + if (*si > *bmax || *ei < *bmin) + return false; + REAL di = *ei - *si; + st = (*si < *bmin)? (*bmin - *si) / di: 0; + et = (*ei > *bmax)? (*bmax - *si) / di: 1; + } + else + { + if (*ei > *bmax || *si < *bmin) + return false; + REAL di = *ei - *si; + st = (*si > *bmax)? (*bmax - *si) / di: 0; + et = (*ei < *bmin)? (*bmin - *si) / di: 1; + } + + if (st > fst) fst = st; + if (et < fet) fet = et; + if (fet < fst) + return false; + bmin++; bmax++; + si++; ei++; + } + + *time = fst; + return true; +} + + + +bool fm_lineTestAABB(const REAL *p1,const REAL *p2,const REAL *bmin,const REAL *bmax,REAL &time) +{ + bool sect = intersect(p1,p2,bmin,bmax,&time); + return sect; +} + + +bool fm_lineTestAABBXZ(const REAL *p1,const REAL *p2,const REAL *bmin,const REAL *bmax,REAL &time) +{ + REAL _bmin[3]; + REAL _bmax[3]; + + _bmin[0] = bmin[0]; + _bmin[1] = -1e9; + _bmin[2] = bmin[2]; + + _bmax[0] = bmax[0]; + _bmax[1] = 1e9; + _bmax[2] = bmax[2]; + + bool sect = intersect(p1,p2,_bmin,_bmax,&time); + + return sect; +} + +void fm_minmax(const REAL *p,REAL *bmin,REAL *bmax) // accmulate to a min-max value +{ + + if ( p[0] < bmin[0] ) bmin[0] = p[0]; + if ( p[1] < bmin[1] ) bmin[1] = p[1]; + if ( p[2] < bmin[2] ) bmin[2] = p[2]; + + if ( p[0] > bmax[0] ) bmax[0] = p[0]; + if ( p[1] > bmax[1] ) bmax[1] = p[1]; + if ( p[2] > bmax[2] ) bmax[2] = p[2]; + +} + +REAL fm_solveX(const REAL *plane,REAL y,REAL z) // solve for X given this plane equation and the other two components. +{ + REAL x = (y*plane[1]+z*plane[2]+plane[3]) / -plane[0]; + return x; +} + +REAL fm_solveY(const REAL *plane,REAL x,REAL z) // solve for Y given this plane equation and the other two components. +{ + REAL y = (x*plane[0]+z*plane[2]+plane[3]) / -plane[1]; + return y; +} + + +REAL fm_solveZ(const REAL *plane,REAL x,REAL y) // solve for Y given this plane equation and the other two components. +{ + REAL z = (x*plane[0]+y*plane[1]+plane[3]) / -plane[2]; + return z; +} + + +void fm_getAABBCenter(const REAL *bmin,const REAL *bmax,REAL *center) +{ + center[0] = (bmax[0]-bmin[0])*0.5f+bmin[0]; + center[1] = (bmax[1]-bmin[1])*0.5f+bmin[1]; + center[2] = (bmax[2]-bmin[2])*0.5f+bmin[2]; +} + +FM_Axis fm_getDominantAxis(const REAL normal[3]) +{ + FM_Axis ret = FM_XAXIS; + + REAL x = (REAL)fabs(normal[0]); + REAL y = (REAL)fabs(normal[1]); + REAL z = (REAL)fabs(normal[2]); + + if ( y > x && y > z ) + ret = FM_YAXIS; + else if ( z > x && z > y ) + ret = FM_ZAXIS; + + return ret; +} + + +bool fm_lineSphereIntersect(const REAL *center,REAL radius,const REAL *p1,const REAL *p2,REAL *intersect) +{ + bool ret = false; + + REAL dir[3]; + + dir[0] = p2[0]-p1[0]; + dir[1] = p2[1]-p1[1]; + dir[2] = p2[2]-p1[2]; + + REAL distance = (REAL)sqrt( dir[0]*dir[0]+dir[1]*dir[1]+dir[2]*dir[2]); + + if ( distance > 0 ) + { + REAL recip = 1.0f / distance; + dir[0]*=recip; + dir[1]*=recip; + dir[2]*=recip; + ret = fm_raySphereIntersect(center,radius,p1,dir,distance,intersect); + } + else + { + dir[0] = center[0]-p1[0]; + dir[1] = center[1]-p1[1]; + dir[2] = center[2]-p1[2]; + REAL d2 = dir[0]*dir[0]+dir[1]*dir[1]+dir[2]*dir[2]; + REAL r2 = radius*radius; + if ( d2 < r2 ) + { + ret = true; + if ( intersect ) + { + intersect[0] = p1[0]; + intersect[1] = p1[1]; + intersect[2] = p1[2]; + } + } + } + return ret; +} + +#define DOT(p1,p2) (p1[0]*p2[0]+p1[1]*p2[1]+p1[2]*p2[2]) + +bool fm_raySphereIntersect(const REAL *center,REAL radius,const REAL *pos,const REAL *dir,REAL distance,REAL *intersect) +{ + bool ret = false; + + REAL E0[3]; + + E0[0] = center[0] - pos[0]; + E0[1] = center[1] - pos[1]; + E0[2] = center[2] - pos[2]; + + REAL V[3]; + + V[0] = dir[0]; + V[1] = dir[1]; + V[2] = dir[2]; + + + REAL dist2 = E0[0]*E0[0] + E0[1]*E0[1] + E0[2] * E0[2]; + REAL radius2 = radius*radius; // radius squared.. + + // Bug Fix For Gem, if origin is *inside* the sphere, invert the + // direction vector so that we get a valid intersection location. + if ( dist2 < radius2 ) + { + V[0]*=-1; + V[1]*=-1; + V[2]*=-1; + } + + + REAL v = DOT(E0,V); + + REAL disc = radius2 - (dist2 - v*v); + + if (disc > 0.0f) + { + if ( intersect ) + { + REAL d = (REAL)sqrt(disc); + REAL diff = v-d; + if ( diff < distance ) + { + intersect[0] = pos[0]+V[0]*diff; + intersect[1] = pos[1]+V[1]*diff; + intersect[2] = pos[2]+V[2]*diff; + ret = true; + } + } + } + + return ret; +} + + +void fm_catmullRom(REAL *out_vector,const REAL *p1,const REAL *p2,const REAL *p3,const REAL *p4, const REAL s) +{ + REAL s_squared = s * s; + REAL s_cubed = s_squared * s; + + REAL coefficient_p1 = -s_cubed + 2*s_squared - s; + REAL coefficient_p2 = 3 * s_cubed - 5 * s_squared + 2; + REAL coefficient_p3 = -3 * s_cubed +4 * s_squared + s; + REAL coefficient_p4 = s_cubed - s_squared; + + out_vector[0] = (coefficient_p1 * p1[0] + coefficient_p2 * p2[0] + coefficient_p3 * p3[0] + coefficient_p4 * p4[0])*0.5f; + out_vector[1] = (coefficient_p1 * p1[1] + coefficient_p2 * p2[1] + coefficient_p3 * p3[1] + coefficient_p4 * p4[1])*0.5f; + out_vector[2] = (coefficient_p1 * p1[2] + coefficient_p2 * p2[2] + coefficient_p3 * p3[2] + coefficient_p4 * p4[2])*0.5f; +} + +bool fm_intersectAABB(const REAL *bmin1,const REAL *bmax1,const REAL *bmin2,const REAL *bmax2) +{ + if ((bmin1[0] > bmax2[0]) || (bmin2[0] > bmax1[0])) return false; + if ((bmin1[1] > bmax2[1]) || (bmin2[1] > bmax1[1])) return false; + if ((bmin1[2] > bmax2[2]) || (bmin2[2] > bmax1[2])) return false; + return true; + +} + +bool fm_insideAABB(const REAL *obmin,const REAL *obmax,const REAL *tbmin,const REAL *tbmax) // test if bounding box tbmin/tmbax is fully inside obmin/obmax +{ + bool ret = false; + + if ( tbmax[0] <= obmax[0] && + tbmax[1] <= obmax[1] && + tbmax[2] <= obmax[2] && + tbmin[0] >= obmin[0] && + tbmin[1] >= obmin[1] && + tbmin[2] >= obmin[2] ) ret = true; + + return ret; +} + + +// Reference, from Stan Melax in Game Gems I +// Quaternion q; +// vector3 c = CrossProduct(v0,v1); +// REAL d = DotProduct(v0,v1); +// REAL s = (REAL)sqrt((1+d)*2); +// q.x = c.x / s; +// q.y = c.y / s; +// q.z = c.z / s; +// q.w = s /2.0f; +// return q; +void fm_rotationArc(const REAL *v0,const REAL *v1,REAL *quat) +{ + REAL cross[3]; + + fm_cross(cross,v0,v1); + REAL d = fm_dot(v0,v1); + + if( d<= -0.99999f ) // 180 about x axis + { + if ( fabsf((float)v0[0]) < 0.1f ) + { + quat[0] = 0; + quat[1] = v0[2]; + quat[2] = -v0[1]; + quat[3] = 0; + } + else + { + quat[0] = v0[1]; + quat[1] = -v0[0]; + quat[2] = 0; + quat[3] = 0; + } + REAL magnitudeSquared = quat[0]*quat[0] + quat[1]*quat[1] + quat[2]*quat[2] + quat[3]*quat[3]; + REAL magnitude = sqrtf((float)magnitudeSquared); + REAL recip = 1.0f / magnitude; + quat[0]*=recip; + quat[1]*=recip; + quat[2]*=recip; + quat[3]*=recip; + } + else + { + REAL s = (REAL)sqrt((1+d)*2); + REAL recip = 1.0f / s; + + quat[0] = cross[0] * recip; + quat[1] = cross[1] * recip; + quat[2] = cross[2] * recip; + quat[3] = s * 0.5f; + } +} + + +REAL fm_distancePointLineSegment(const REAL *Point,const REAL *LineStart,const REAL *LineEnd,REAL *intersection,LineSegmentType &type,REAL epsilon) +{ + REAL ret; + + REAL LineMag = fm_distance( LineEnd, LineStart ); + + if ( LineMag > 0 ) + { + REAL U = ( ( ( Point[0] - LineStart[0] ) * ( LineEnd[0] - LineStart[0] ) ) + ( ( Point[1] - LineStart[1] ) * ( LineEnd[1] - LineStart[1] ) ) + ( ( Point[2] - LineStart[2] ) * ( LineEnd[2] - LineStart[2] ) ) ) / ( LineMag * LineMag ); + if( U < 0.0f || U > 1.0f ) + { + REAL d1 = fm_distanceSquared(Point,LineStart); + REAL d2 = fm_distanceSquared(Point,LineEnd); + if ( d1 <= d2 ) + { + ret = (REAL)sqrt(d1); + intersection[0] = LineStart[0]; + intersection[1] = LineStart[1]; + intersection[2] = LineStart[2]; + type = LS_START; + } + else + { + ret = (REAL)sqrt(d2); + intersection[0] = LineEnd[0]; + intersection[1] = LineEnd[1]; + intersection[2] = LineEnd[2]; + type = LS_END; + } + } + else + { + intersection[0] = LineStart[0] + U * ( LineEnd[0] - LineStart[0] ); + intersection[1] = LineStart[1] + U * ( LineEnd[1] - LineStart[1] ); + intersection[2] = LineStart[2] + U * ( LineEnd[2] - LineStart[2] ); + + ret = fm_distance(Point,intersection); + + REAL d1 = fm_distanceSquared(intersection,LineStart); + REAL d2 = fm_distanceSquared(intersection,LineEnd); + REAL mag = (epsilon*2)*(epsilon*2); + + if ( d1 < mag ) // if less than 1/100th the total distance, treat is as the 'start' + { + type = LS_START; + } + else if ( d2 < mag ) + { + type = LS_END; + } + else + { + type = LS_MIDDLE; + } + + } + } + else + { + ret = LineMag; + intersection[0] = LineEnd[0]; + intersection[1] = LineEnd[1]; + intersection[2] = LineEnd[2]; + type = LS_END; + } + + return ret; +} + + +#ifndef BEST_FIT_PLANE_H + +#define BEST_FIT_PLANE_H + +template <class Type> class Eigen +{ +public: + + + void DecrSortEigenStuff(void) + { + Tridiagonal(); //diagonalize the matrix. + QLAlgorithm(); // + DecreasingSort(); + GuaranteeRotation(); + } + + void Tridiagonal(void) + { + Type fM00 = mElement[0][0]; + Type fM01 = mElement[0][1]; + Type fM02 = mElement[0][2]; + Type fM11 = mElement[1][1]; + Type fM12 = mElement[1][2]; + Type fM22 = mElement[2][2]; + + m_afDiag[0] = fM00; + m_afSubd[2] = 0; + if (fM02 != (Type)0.0) + { + Type fLength = (REAL)sqrt(fM01*fM01+fM02*fM02); + Type fInvLength = ((Type)1.0)/fLength; + fM01 *= fInvLength; + fM02 *= fInvLength; + Type fQ = ((Type)2.0)*fM01*fM12+fM02*(fM22-fM11); + m_afDiag[1] = fM11+fM02*fQ; + m_afDiag[2] = fM22-fM02*fQ; + m_afSubd[0] = fLength; + m_afSubd[1] = fM12-fM01*fQ; + mElement[0][0] = (Type)1.0; + mElement[0][1] = (Type)0.0; + mElement[0][2] = (Type)0.0; + mElement[1][0] = (Type)0.0; + mElement[1][1] = fM01; + mElement[1][2] = fM02; + mElement[2][0] = (Type)0.0; + mElement[2][1] = fM02; + mElement[2][2] = -fM01; + m_bIsRotation = false; + } + else + { + m_afDiag[1] = fM11; + m_afDiag[2] = fM22; + m_afSubd[0] = fM01; + m_afSubd[1] = fM12; + mElement[0][0] = (Type)1.0; + mElement[0][1] = (Type)0.0; + mElement[0][2] = (Type)0.0; + mElement[1][0] = (Type)0.0; + mElement[1][1] = (Type)1.0; + mElement[1][2] = (Type)0.0; + mElement[2][0] = (Type)0.0; + mElement[2][1] = (Type)0.0; + mElement[2][2] = (Type)1.0; + m_bIsRotation = true; + } + } + + bool QLAlgorithm(void) + { + const int32_t iMaxIter = 32; + + for (int32_t i0 = 0; i0 <3; i0++) + { + int32_t i1; + for (i1 = 0; i1 < iMaxIter; i1++) + { + int32_t i2; + for (i2 = i0; i2 <= (3-2); i2++) + { + Type fTmp = fabs(m_afDiag[i2]) + fabs(m_afDiag[i2+1]); + if ( fabs(m_afSubd[i2]) + fTmp == fTmp ) + break; + } + if (i2 == i0) + { + break; + } + + Type fG = (m_afDiag[i0+1] - m_afDiag[i0])/(((Type)2.0) * m_afSubd[i0]); + Type fR = (REAL)sqrt(fG*fG+(Type)1.0); + if (fG < (Type)0.0) + { + fG = m_afDiag[i2]-m_afDiag[i0]+m_afSubd[i0]/(fG-fR); + } + else + { + fG = m_afDiag[i2]-m_afDiag[i0]+m_afSubd[i0]/(fG+fR); + } + Type fSin = (Type)1.0, fCos = (Type)1.0, fP = (Type)0.0; + for (int32_t i3 = i2-1; i3 >= i0; i3--) + { + Type fF = fSin*m_afSubd[i3]; + Type fB = fCos*m_afSubd[i3]; + if (fabs(fF) >= fabs(fG)) + { + fCos = fG/fF; + fR = (REAL)sqrt(fCos*fCos+(Type)1.0); + m_afSubd[i3+1] = fF*fR; + fSin = ((Type)1.0)/fR; + fCos *= fSin; + } + else + { + fSin = fF/fG; + fR = (REAL)sqrt(fSin*fSin+(Type)1.0); + m_afSubd[i3+1] = fG*fR; + fCos = ((Type)1.0)/fR; + fSin *= fCos; + } + fG = m_afDiag[i3+1]-fP; + fR = (m_afDiag[i3]-fG)*fSin+((Type)2.0)*fB*fCos; + fP = fSin*fR; + m_afDiag[i3+1] = fG+fP; + fG = fCos*fR-fB; + for (int32_t i4 = 0; i4 < 3; i4++) + { + fF = mElement[i4][i3+1]; + mElement[i4][i3+1] = fSin*mElement[i4][i3]+fCos*fF; + mElement[i4][i3] = fCos*mElement[i4][i3]-fSin*fF; + } + } + m_afDiag[i0] -= fP; + m_afSubd[i0] = fG; + m_afSubd[i2] = (Type)0.0; + } + if (i1 == iMaxIter) + { + return false; + } + } + return true; + } + + void DecreasingSort(void) + { + //sort eigenvalues in decreasing order, e[0] >= ... >= e[iSize-1] + for (int32_t i0 = 0, i1; i0 <= 3-2; i0++) + { + // locate maximum eigenvalue + i1 = i0; + Type fMax = m_afDiag[i1]; + int32_t i2; + for (i2 = i0+1; i2 < 3; i2++) + { + if (m_afDiag[i2] > fMax) + { + i1 = i2; + fMax = m_afDiag[i1]; + } + } + + if (i1 != i0) + { + // swap eigenvalues + m_afDiag[i1] = m_afDiag[i0]; + m_afDiag[i0] = fMax; + // swap eigenvectors + for (i2 = 0; i2 < 3; i2++) + { + Type fTmp = mElement[i2][i0]; + mElement[i2][i0] = mElement[i2][i1]; + mElement[i2][i1] = fTmp; + m_bIsRotation = !m_bIsRotation; + } + } + } + } + + + void GuaranteeRotation(void) + { + if (!m_bIsRotation) + { + // change sign on the first column + for (int32_t iRow = 0; iRow <3; iRow++) + { + mElement[iRow][0] = -mElement[iRow][0]; + } + } + } + + Type mElement[3][3]; + Type m_afDiag[3]; + Type m_afSubd[3]; + bool m_bIsRotation; +}; + +#endif + +bool fm_computeBestFitPlane(uint32_t vcount, + const REAL *points, + uint32_t vstride, + const REAL *weights, + uint32_t wstride, + REAL *plane, + REAL *center) +{ + bool ret = false; + + REAL kOrigin[3] = { 0, 0, 0 }; + + REAL wtotal = 0; + + { + const char *source = (const char *) points; + const char *wsource = (const char *) weights; + + for (uint32_t i=0; i<vcount; i++) + { + + const REAL *p = (const REAL *) source; + + REAL w = 1; + + if ( wsource ) + { + const REAL *ws = (const REAL *) wsource; + w = *ws; // + wsource+=wstride; + } + + kOrigin[0]+=p[0]*w; + kOrigin[1]+=p[1]*w; + kOrigin[2]+=p[2]*w; + + wtotal+=w; + + source+=vstride; + } + } + + REAL recip = 1.0f / wtotal; // reciprocol of total weighting + + kOrigin[0]*=recip; + kOrigin[1]*=recip; + kOrigin[2]*=recip; + + center[0] = kOrigin[0]; + center[1] = kOrigin[1]; + center[2] = kOrigin[2]; + + + REAL fSumXX=0; + REAL fSumXY=0; + REAL fSumXZ=0; + + REAL fSumYY=0; + REAL fSumYZ=0; + REAL fSumZZ=0; + + + { + const char *source = (const char *) points; + const char *wsource = (const char *) weights; + + for (uint32_t i=0; i<vcount; i++) + { + + const REAL *p = (const REAL *) source; + + REAL w = 1; + + if ( wsource ) + { + const REAL *ws = (const REAL *) wsource; + w = *ws; // + wsource+=wstride; + } + + REAL kDiff[3]; + + kDiff[0] = w*(p[0] - kOrigin[0]); // apply vertex weighting! + kDiff[1] = w*(p[1] - kOrigin[1]); + kDiff[2] = w*(p[2] - kOrigin[2]); + + fSumXX+= kDiff[0] * kDiff[0]; // sume of the squares of the differences. + fSumXY+= kDiff[0] * kDiff[1]; // sume of the squares of the differences. + fSumXZ+= kDiff[0] * kDiff[2]; // sume of the squares of the differences. + + fSumYY+= kDiff[1] * kDiff[1]; + fSumYZ+= kDiff[1] * kDiff[2]; + fSumZZ+= kDiff[2] * kDiff[2]; + + + source+=vstride; + } + } + + fSumXX *= recip; + fSumXY *= recip; + fSumXZ *= recip; + fSumYY *= recip; + fSumYZ *= recip; + fSumZZ *= recip; + + // setup the eigensolver + Eigen<REAL> kES; + + kES.mElement[0][0] = fSumXX; + kES.mElement[0][1] = fSumXY; + kES.mElement[0][2] = fSumXZ; + + kES.mElement[1][0] = fSumXY; + kES.mElement[1][1] = fSumYY; + kES.mElement[1][2] = fSumYZ; + + kES.mElement[2][0] = fSumXZ; + kES.mElement[2][1] = fSumYZ; + kES.mElement[2][2] = fSumZZ; + + // compute eigenstuff, smallest eigenvalue is in last position + kES.DecrSortEigenStuff(); + + REAL kNormal[3]; + + kNormal[0] = kES.mElement[0][2]; + kNormal[1] = kES.mElement[1][2]; + kNormal[2] = kES.mElement[2][2]; + + // the minimum energy + plane[0] = kNormal[0]; + plane[1] = kNormal[1]; + plane[2] = kNormal[2]; + + plane[3] = 0 - fm_dot(kNormal,kOrigin); + + ret = true; + + return ret; +} + + +bool fm_colinear(const REAL a1[3],const REAL a2[3],const REAL b1[3],const REAL b2[3],REAL epsilon) // true if these two line segments are co-linear. +{ + bool ret = false; + + REAL dir1[3]; + REAL dir2[3]; + + dir1[0] = (a2[0] - a1[0]); + dir1[1] = (a2[1] - a1[1]); + dir1[2] = (a2[2] - a1[2]); + + dir2[0] = (b2[0]-a1[0]) - (b1[0]-a1[0]); + dir2[1] = (b2[1]-a1[1]) - (b1[1]-a1[1]); + dir2[2] = (b2[2]-a2[2]) - (b1[2]-a2[2]); + + fm_normalize(dir1); + fm_normalize(dir2); + + REAL dot = fm_dot(dir1,dir2); + + if ( dot >= epsilon ) + { + ret = true; + } + + + return ret; +} + +bool fm_colinear(const REAL *p1,const REAL *p2,const REAL *p3,REAL epsilon) +{ + bool ret = false; + + REAL dir1[3]; + REAL dir2[3]; + + dir1[0] = p2[0] - p1[0]; + dir1[1] = p2[1] - p1[1]; + dir1[2] = p2[2] - p1[2]; + + dir2[0] = p3[0] - p2[0]; + dir2[1] = p3[1] - p2[1]; + dir2[2] = p3[2] - p2[2]; + + fm_normalize(dir1); + fm_normalize(dir2); + + REAL dot = fm_dot(dir1,dir2); + + if ( dot >= epsilon ) + { + ret = true; + } + + + return ret; +} + +void fm_initMinMax(const REAL *p,REAL *bmin,REAL *bmax) +{ + bmax[0] = bmin[0] = p[0]; + bmax[1] = bmin[1] = p[1]; + bmax[2] = bmin[2] = p[2]; +} + +IntersectResult fm_intersectLineSegments2d(const REAL *a1,const REAL *a2,const REAL *b1,const REAL *b2,REAL *intersection) +{ + IntersectResult ret; + + REAL denom = ((b2[1] - b1[1])*(a2[0] - a1[0])) - ((b2[0] - b1[0])*(a2[1] - a1[1])); + REAL nume_a = ((b2[0] - b1[0])*(a1[1] - b1[1])) - ((b2[1] - b1[1])*(a1[0] - b1[0])); + REAL nume_b = ((a2[0] - a1[0])*(a1[1] - b1[1])) - ((a2[1] - a1[1])*(a1[0] - b1[0])); + if (denom == 0 ) + { + if(nume_a == 0 && nume_b == 0) + { + ret = IR_COINCIDENT; + } + else + { + ret = IR_PARALLEL; + } + } + else + { + + REAL recip = 1 / denom; + REAL ua = nume_a * recip; + REAL ub = nume_b * recip; + + if(ua >= 0 && ua <= 1 && ub >= 0 && ub <= 1 ) + { + // Get the intersection point. + intersection[0] = a1[0] + ua*(a2[0] - a1[0]); + intersection[1] = a1[1] + ua*(a2[1] - a1[1]); + ret = IR_DO_INTERSECT; + } + else + { + ret = IR_DONT_INTERSECT; + } + } + return ret; +} + +IntersectResult fm_intersectLineSegments2dTime(const REAL *a1,const REAL *a2,const REAL *b1,const REAL *b2,REAL &t1,REAL &t2) +{ + IntersectResult ret; + + REAL denom = ((b2[1] - b1[1])*(a2[0] - a1[0])) - ((b2[0] - b1[0])*(a2[1] - a1[1])); + REAL nume_a = ((b2[0] - b1[0])*(a1[1] - b1[1])) - ((b2[1] - b1[1])*(a1[0] - b1[0])); + REAL nume_b = ((a2[0] - a1[0])*(a1[1] - b1[1])) - ((a2[1] - a1[1])*(a1[0] - b1[0])); + if (denom == 0 ) + { + if(nume_a == 0 && nume_b == 0) + { + ret = IR_COINCIDENT; + } + else + { + ret = IR_PARALLEL; + } + } + else + { + + REAL recip = 1 / denom; + REAL ua = nume_a * recip; + REAL ub = nume_b * recip; + + if(ua >= 0 && ua <= 1 && ub >= 0 && ub <= 1 ) + { + t1 = ua; + t2 = ub; + ret = IR_DO_INTERSECT; + } + else + { + ret = IR_DONT_INTERSECT; + } + } + return ret; +} + +//**** Plane Triangle Intersection + + + + + +// assumes that the points are on opposite sides of the plane! +bool fm_intersectPointPlane(const REAL *p1,const REAL *p2,REAL *split,const REAL *plane) +{ + + REAL dp1 = fm_distToPlane(plane,p1); + REAL dp2 = fm_distToPlane(plane, p2); + if (dp1 <= 0 && dp2 <= 0) + { + return false; + } + if (dp1 >= 0 && dp2 >= 0) + { + return false; + } + + REAL dir[3]; + + dir[0] = p2[0] - p1[0]; + dir[1] = p2[1] - p1[1]; + dir[2] = p2[2] - p1[2]; + + REAL dot1 = dir[0]*plane[0] + dir[1]*plane[1] + dir[2]*plane[2]; + REAL dot2 = dp1 - plane[3]; + + REAL t = -(plane[3] + dot2 ) / dot1; + + split[0] = (dir[0]*t)+p1[0]; + split[1] = (dir[1]*t)+p1[1]; + split[2] = (dir[2]*t)+p1[2]; + + return true; +} + +PlaneTriResult fm_getSidePlane(const REAL *p,const REAL *plane,REAL epsilon) +{ + PlaneTriResult ret = PTR_ON_PLANE; + + REAL d = fm_distToPlane(plane,p); + + if ( d < -epsilon || d > epsilon ) + { + if ( d > 0 ) + ret = PTR_FRONT; // it is 'in front' within the provided epsilon value. + else + ret = PTR_BACK; + } + + return ret; +} + + + +#ifndef PLANE_TRIANGLE_INTERSECTION_H + +#define PLANE_TRIANGLE_INTERSECTION_H + +#define MAXPTS 256 + +template <class Type> class point +{ +public: + + void set(const Type *p) + { + x = p[0]; + y = p[1]; + z = p[2]; + } + + Type x; + Type y; + Type z; +}; + +template <class Type> class plane +{ +public: + plane(const Type *p) + { + normal.x = p[0]; + normal.y = p[1]; + normal.z = p[2]; + D = p[3]; + } + + Type Classify_Point(const point<Type> &p) + { + return p.x*normal.x + p.y*normal.y + p.z*normal.z + D; + } + + point<Type> normal; + Type D; +}; + +template <class Type> class polygon +{ +public: + polygon(void) + { + mVcount = 0; + } + + polygon(const Type *p1,const Type *p2,const Type *p3) + { + mVcount = 3; + mVertices[0].set(p1); + mVertices[1].set(p2); + mVertices[2].set(p3); + } + + + int32_t NumVertices(void) const { return mVcount; }; + + const point<Type>& Vertex(int32_t index) + { + if ( index < 0 ) index+=mVcount; + return mVertices[index]; + }; + + + void set(const point<Type> *pts,int32_t count) + { + for (int32_t i=0; i<count; i++) + { + mVertices[i] = pts[i]; + } + mVcount = count; + } + + + void Split_Polygon(polygon<Type> *poly,plane<Type> *part, polygon<Type> &front, polygon<Type> &back) + { + int32_t count = poly->NumVertices (); + int32_t out_c = 0, in_c = 0; + point<Type> ptA, ptB,outpts[MAXPTS],inpts[MAXPTS]; + Type sideA, sideB; + ptA = poly->Vertex (count - 1); + sideA = part->Classify_Point (ptA); + for (int32_t i = -1; ++i < count;) + { + ptB = poly->Vertex(i); + sideB = part->Classify_Point(ptB); + if (sideB > 0) + { + if (sideA < 0) + { + point<Type> v; + fm_intersectPointPlane(&ptB.x, &ptA.x, &v.x, &part->normal.x ); + outpts[out_c++] = inpts[in_c++] = v; + } + outpts[out_c++] = ptB; + } + else if (sideB < 0) + { + if (sideA > 0) + { + point<Type> v; + fm_intersectPointPlane(&ptB.x, &ptA.x, &v.x, &part->normal.x ); + outpts[out_c++] = inpts[in_c++] = v; + } + inpts[in_c++] = ptB; + } + else + outpts[out_c++] = inpts[in_c++] = ptB; + ptA = ptB; + sideA = sideB; + } + + front.set(&outpts[0], out_c); + back.set(&inpts[0], in_c); + } + + int32_t mVcount; + point<Type> mVertices[MAXPTS]; +}; + + + +#endif + +static inline void add(const REAL *p,REAL *dest,uint32_t tstride,uint32_t &pcount) +{ + char *d = (char *) dest; + d = d + pcount*tstride; + dest = (REAL *) d; + dest[0] = p[0]; + dest[1] = p[1]; + dest[2] = p[2]; + pcount++; + assert( pcount <= 4 ); +} + + +PlaneTriResult fm_planeTriIntersection(const REAL *_plane, // the plane equation in Ax+By+Cz+D format + const REAL *triangle, // the source triangle. + uint32_t tstride, // stride in bytes of the input and output *vertices* + REAL epsilon, // the co-planar epsilon value. + REAL *front, // the triangle in front of the + uint32_t &fcount, // number of vertices in the 'front' triangle + REAL *back, // the triangle in back of the plane + uint32_t &bcount) // the number of vertices in the 'back' triangle. +{ + + fcount = 0; + bcount = 0; + + const char *tsource = (const char *) triangle; + + // get the three vertices of the triangle. + const REAL *p1 = (const REAL *) (tsource); + const REAL *p2 = (const REAL *) (tsource+tstride); + const REAL *p3 = (const REAL *) (tsource+tstride*2); + + + PlaneTriResult r1 = fm_getSidePlane(p1,_plane,epsilon); // compute the side of the plane each vertex is on + PlaneTriResult r2 = fm_getSidePlane(p2,_plane,epsilon); + PlaneTriResult r3 = fm_getSidePlane(p3,_plane,epsilon); + + // If any of the points lay right *on* the plane.... + if ( r1 == PTR_ON_PLANE || r2 == PTR_ON_PLANE || r3 == PTR_ON_PLANE ) + { + // If the triangle is completely co-planar, then just treat it as 'front' and return! + if ( r1 == PTR_ON_PLANE && r2 == PTR_ON_PLANE && r3 == PTR_ON_PLANE ) + { + add(p1,front,tstride,fcount); + add(p2,front,tstride,fcount); + add(p3,front,tstride,fcount); + return PTR_FRONT; + } + // Decide to place the co-planar points on the same side as the co-planar point. + PlaneTriResult r= PTR_ON_PLANE; + if ( r1 != PTR_ON_PLANE ) + r = r1; + else if ( r2 != PTR_ON_PLANE ) + r = r2; + else if ( r3 != PTR_ON_PLANE ) + r = r3; + + if ( r1 == PTR_ON_PLANE ) r1 = r; + if ( r2 == PTR_ON_PLANE ) r2 = r; + if ( r3 == PTR_ON_PLANE ) r3 = r; + + } + + if ( r1 == r2 && r1 == r3 ) // if all three vertices are on the same side of the plane. + { + if ( r1 == PTR_FRONT ) // if all three are in front of the plane, then copy to the 'front' output triangle. + { + add(p1,front,tstride,fcount); + add(p2,front,tstride,fcount); + add(p3,front,tstride,fcount); + } + else + { + add(p1,back,tstride,bcount); // if all three are in 'back' then copy to the 'back' output triangle. + add(p2,back,tstride,bcount); + add(p3,back,tstride,bcount); + } + return r1; // if all three points are on the same side of the plane return result + } + + + polygon<REAL> pi(p1,p2,p3); + polygon<REAL> pfront,pback; + + plane<REAL> part(_plane); + + pi.Split_Polygon(&pi,&part,pfront,pback); + + for (int32_t i=0; i<pfront.mVcount; i++) + { + add( &pfront.mVertices[i].x, front, tstride, fcount ); + } + + for (int32_t i=0; i<pback.mVcount; i++) + { + add( &pback.mVertices[i].x, back, tstride, bcount ); + } + + PlaneTriResult ret = PTR_SPLIT; + + if ( fcount < 3 ) fcount = 0; + if ( bcount < 3 ) bcount = 0; + + if ( fcount == 0 && bcount ) + ret = PTR_BACK; + + if ( bcount == 0 && fcount ) + ret = PTR_FRONT; + + + return ret; +} + +// computes the OBB for this set of points relative to this transform matrix. +void computeOBB(uint32_t vcount,const REAL *points,uint32_t pstride,REAL *sides,REAL *matrix) +{ + const char *src = (const char *) points; + + REAL bmin[3] = { 1e9, 1e9, 1e9 }; + REAL bmax[3] = { -1e9, -1e9, -1e9 }; + + for (uint32_t i=0; i<vcount; i++) + { + const REAL *p = (const REAL *) src; + REAL t[3]; + + fm_inverseRT(matrix, p, t ); // inverse rotate translate + + if ( t[0] < bmin[0] ) bmin[0] = t[0]; + if ( t[1] < bmin[1] ) bmin[1] = t[1]; + if ( t[2] < bmin[2] ) bmin[2] = t[2]; + + if ( t[0] > bmax[0] ) bmax[0] = t[0]; + if ( t[1] > bmax[1] ) bmax[1] = t[1]; + if ( t[2] > bmax[2] ) bmax[2] = t[2]; + + src+=pstride; + } + + REAL center[3]; + + sides[0] = bmax[0]-bmin[0]; + sides[1] = bmax[1]-bmin[1]; + sides[2] = bmax[2]-bmin[2]; + + center[0] = sides[0]*0.5f+bmin[0]; + center[1] = sides[1]*0.5f+bmin[1]; + center[2] = sides[2]*0.5f+bmin[2]; + + REAL ocenter[3]; + + fm_rotate(matrix,center,ocenter); + + matrix[12]+=ocenter[0]; + matrix[13]+=ocenter[1]; + matrix[14]+=ocenter[2]; + +} + +void fm_computeBestFitOBB(uint32_t vcount,const REAL *points,uint32_t pstride,REAL *sides,REAL *matrix,bool bruteForce) +{ + REAL plane[4]; + REAL center[3]; + fm_computeBestFitPlane(vcount,points,pstride,0,0,plane,center); + fm_planeToMatrix(plane,matrix); + computeOBB( vcount, points, pstride, sides, matrix ); + + REAL refmatrix[16]; + memcpy(refmatrix,matrix,16*sizeof(REAL)); + + REAL volume = sides[0]*sides[1]*sides[2]; + if ( bruteForce ) + { + for (REAL a=10; a<180; a+=10) + { + REAL quat[4]; + fm_eulerToQuat(0,a*FM_DEG_TO_RAD,0,quat); + REAL temp[16]; + REAL pmatrix[16]; + fm_quatToMatrix(quat,temp); + fm_matrixMultiply(temp,refmatrix,pmatrix); + REAL psides[3]; + computeOBB( vcount, points, pstride, psides, pmatrix ); + REAL v = psides[0]*psides[1]*psides[2]; + if ( v < volume ) + { + volume = v; + memcpy(matrix,pmatrix,sizeof(REAL)*16); + sides[0] = psides[0]; + sides[1] = psides[1]; + sides[2] = psides[2]; + } + } + } +} + +void fm_computeBestFitOBB(uint32_t vcount,const REAL *points,uint32_t pstride,REAL *sides,REAL *pos,REAL *quat,bool bruteForce) +{ + REAL matrix[16]; + fm_computeBestFitOBB(vcount,points,pstride,sides,matrix,bruteForce); + fm_getTranslation(matrix,pos); + fm_matrixToQuat(matrix,quat); +} + +void fm_computeBestFitABB(uint32_t vcount,const REAL *points,uint32_t pstride,REAL *sides,REAL *pos) +{ + REAL bmin[3]; + REAL bmax[3]; + + bmin[0] = points[0]; + bmin[1] = points[1]; + bmin[2] = points[2]; + + bmax[0] = points[0]; + bmax[1] = points[1]; + bmax[2] = points[2]; + + const char *cp = (const char *) points; + for (uint32_t i=0; i<vcount; i++) + { + const REAL *p = (const REAL *) cp; + + if ( p[0] < bmin[0] ) bmin[0] = p[0]; + if ( p[1] < bmin[1] ) bmin[1] = p[1]; + if ( p[2] < bmin[2] ) bmin[2] = p[2]; + + if ( p[0] > bmax[0] ) bmax[0] = p[0]; + if ( p[1] > bmax[1] ) bmax[1] = p[1]; + if ( p[2] > bmax[2] ) bmax[2] = p[2]; + + cp+=pstride; + } + + + sides[0] = bmax[0] - bmin[0]; + sides[1] = bmax[1] - bmin[1]; + sides[2] = bmax[2] - bmin[2]; + + pos[0] = bmin[0]+sides[0]*0.5f; + pos[1] = bmin[1]+sides[1]*0.5f; + pos[2] = bmin[2]+sides[2]*0.5f; + +} + + +void fm_planeToMatrix(const REAL *plane,REAL *matrix) // convert a plane equation to a 4x4 rotation matrix +{ + REAL ref[3] = { 0, 1, 0 }; + REAL quat[4]; + fm_rotationArc(ref,plane,quat); + fm_quatToMatrix(quat,matrix); + REAL origin[3] = { 0, -plane[3], 0 }; + REAL center[3]; + fm_transform(matrix,origin,center); + fm_setTranslation(center,matrix); +} + +void fm_planeToQuat(const REAL *plane,REAL *quat,REAL *pos) // convert a plane equation to a quaternion and translation +{ + REAL ref[3] = { 0, 1, 0 }; + REAL matrix[16]; + fm_rotationArc(ref,plane,quat); + fm_quatToMatrix(quat,matrix); + REAL origin[3] = { 0, plane[3], 0 }; + fm_transform(matrix,origin,pos); +} + +void fm_eulerMatrix(REAL ax,REAL ay,REAL az,REAL *matrix) // convert euler (in radians) to a dest 4x4 matrix (translation set to zero) +{ + REAL quat[4]; + fm_eulerToQuat(ax,ay,az,quat); + fm_quatToMatrix(quat,matrix); +} + + +//********************************************************** +//********************************************************** +//**** Vertex Welding +//********************************************************** +//********************************************************** + +#ifndef VERTEX_INDEX_H + +#define VERTEX_INDEX_H + +namespace VERTEX_INDEX +{ + +class KdTreeNode; + +typedef std::vector< KdTreeNode * > KdTreeNodeVector; + +enum Axes +{ + X_AXIS = 0, + Y_AXIS = 1, + Z_AXIS = 2 +}; + +class KdTreeFindNode +{ +public: + KdTreeFindNode(void) + { + mNode = 0; + mDistance = 0; + } + KdTreeNode *mNode; + double mDistance; +}; + +class KdTreeInterface +{ +public: + virtual const double * getPositionDouble(uint32_t index) const = 0; + virtual const float * getPositionFloat(uint32_t index) const = 0; +}; + +class KdTreeNode +{ +public: + KdTreeNode(void) + { + mIndex = 0; + mLeft = 0; + mRight = 0; + } + + KdTreeNode(uint32_t index) + { + mIndex = index; + mLeft = 0; + mRight = 0; + }; + + ~KdTreeNode(void) + { + } + + + void addDouble(KdTreeNode *node,Axes dim,const KdTreeInterface *iface) + { + const double *nodePosition = iface->getPositionDouble( node->mIndex ); + const double *position = iface->getPositionDouble( mIndex ); + switch ( dim ) + { + case X_AXIS: + if ( nodePosition[0] <= position[0] ) + { + if ( mLeft ) + mLeft->addDouble(node,Y_AXIS,iface); + else + mLeft = node; + } + else + { + if ( mRight ) + mRight->addDouble(node,Y_AXIS,iface); + else + mRight = node; + } + break; + case Y_AXIS: + if ( nodePosition[1] <= position[1] ) + { + if ( mLeft ) + mLeft->addDouble(node,Z_AXIS,iface); + else + mLeft = node; + } + else + { + if ( mRight ) + mRight->addDouble(node,Z_AXIS,iface); + else + mRight = node; + } + break; + case Z_AXIS: + if ( nodePosition[2] <= position[2] ) + { + if ( mLeft ) + mLeft->addDouble(node,X_AXIS,iface); + else + mLeft = node; + } + else + { + if ( mRight ) + mRight->addDouble(node,X_AXIS,iface); + else + mRight = node; + } + break; + } + + } + + + void addFloat(KdTreeNode *node,Axes dim,const KdTreeInterface *iface) + { + const float *nodePosition = iface->getPositionFloat( node->mIndex ); + const float *position = iface->getPositionFloat( mIndex ); + switch ( dim ) + { + case X_AXIS: + if ( nodePosition[0] <= position[0] ) + { + if ( mLeft ) + mLeft->addFloat(node,Y_AXIS,iface); + else + mLeft = node; + } + else + { + if ( mRight ) + mRight->addFloat(node,Y_AXIS,iface); + else + mRight = node; + } + break; + case Y_AXIS: + if ( nodePosition[1] <= position[1] ) + { + if ( mLeft ) + mLeft->addFloat(node,Z_AXIS,iface); + else + mLeft = node; + } + else + { + if ( mRight ) + mRight->addFloat(node,Z_AXIS,iface); + else + mRight = node; + } + break; + case Z_AXIS: + if ( nodePosition[2] <= position[2] ) + { + if ( mLeft ) + mLeft->addFloat(node,X_AXIS,iface); + else + mLeft = node; + } + else + { + if ( mRight ) + mRight->addFloat(node,X_AXIS,iface); + else + mRight = node; + } + break; + } + + } + + + uint32_t getIndex(void) const { return mIndex; }; + + void search(Axes axis,const double *pos,double radius,uint32_t &count,uint32_t maxObjects,KdTreeFindNode *found,const KdTreeInterface *iface) + { + + const double *position = iface->getPositionDouble(mIndex); + + double dx = pos[0] - position[0]; + double dy = pos[1] - position[1]; + double dz = pos[2] - position[2]; + + KdTreeNode *search1 = 0; + KdTreeNode *search2 = 0; + + switch ( axis ) + { + case X_AXIS: + if ( dx <= 0 ) // JWR if we are to the left + { + search1 = mLeft; // JWR then search to the left + if ( -dx < radius ) // JWR if distance to the right is less than our search radius, continue on the right as well. + search2 = mRight; + } + else + { + search1 = mRight; // JWR ok, we go down the left tree + if ( dx < radius ) // JWR if the distance from the right is less than our search radius + search2 = mLeft; + } + axis = Y_AXIS; + break; + case Y_AXIS: + if ( dy <= 0 ) + { + search1 = mLeft; + if ( -dy < radius ) + search2 = mRight; + } + else + { + search1 = mRight; + if ( dy < radius ) + search2 = mLeft; + } + axis = Z_AXIS; + break; + case Z_AXIS: + if ( dz <= 0 ) + { + search1 = mLeft; + if ( -dz < radius ) + search2 = mRight; + } + else + { + search1 = mRight; + if ( dz < radius ) + search2 = mLeft; + } + axis = X_AXIS; + break; + } + + double r2 = radius*radius; + double m = dx*dx+dy*dy+dz*dz; + + if ( m < r2 ) + { + switch ( count ) + { + case 0: + found[count].mNode = this; + found[count].mDistance = m; + break; + case 1: + if ( m < found[0].mDistance ) + { + if ( maxObjects == 1 ) + { + found[0].mNode = this; + found[0].mDistance = m; + } + else + { + found[1] = found[0]; + found[0].mNode = this; + found[0].mDistance = m; + } + } + else if ( maxObjects > 1) + { + found[1].mNode = this; + found[1].mDistance = m; + } + break; + default: + { + bool inserted = false; + + for (uint32_t i=0; i<count; i++) + { + if ( m < found[i].mDistance ) // if this one is closer than a pre-existing one... + { + // insertion sort... + uint32_t scan = count; + if ( scan >= maxObjects ) scan=maxObjects-1; + for (uint32_t j=scan; j>i; j--) + { + found[j] = found[j-1]; + } + found[i].mNode = this; + found[i].mDistance = m; + inserted = true; + break; + } + } + + if ( !inserted && count < maxObjects ) + { + found[count].mNode = this; + found[count].mDistance = m; + } + } + break; + } + count++; + if ( count > maxObjects ) + { + count = maxObjects; + } + } + + + if ( search1 ) + search1->search( axis, pos,radius, count, maxObjects, found, iface); + + if ( search2 ) + search2->search( axis, pos,radius, count, maxObjects, found, iface); + + } + + void search(Axes axis,const float *pos,float radius,uint32_t &count,uint32_t maxObjects,KdTreeFindNode *found,const KdTreeInterface *iface) + { + + const float *position = iface->getPositionFloat(mIndex); + + float dx = pos[0] - position[0]; + float dy = pos[1] - position[1]; + float dz = pos[2] - position[2]; + + KdTreeNode *search1 = 0; + KdTreeNode *search2 = 0; + + switch ( axis ) + { + case X_AXIS: + if ( dx <= 0 ) // JWR if we are to the left + { + search1 = mLeft; // JWR then search to the left + if ( -dx < radius ) // JWR if distance to the right is less than our search radius, continue on the right as well. + search2 = mRight; + } + else + { + search1 = mRight; // JWR ok, we go down the left tree + if ( dx < radius ) // JWR if the distance from the right is less than our search radius + search2 = mLeft; + } + axis = Y_AXIS; + break; + case Y_AXIS: + if ( dy <= 0 ) + { + search1 = mLeft; + if ( -dy < radius ) + search2 = mRight; + } + else + { + search1 = mRight; + if ( dy < radius ) + search2 = mLeft; + } + axis = Z_AXIS; + break; + case Z_AXIS: + if ( dz <= 0 ) + { + search1 = mLeft; + if ( -dz < radius ) + search2 = mRight; + } + else + { + search1 = mRight; + if ( dz < radius ) + search2 = mLeft; + } + axis = X_AXIS; + break; + } + + float r2 = radius*radius; + float m = dx*dx+dy*dy+dz*dz; + + if ( m < r2 ) + { + switch ( count ) + { + case 0: + found[count].mNode = this; + found[count].mDistance = m; + break; + case 1: + if ( m < found[0].mDistance ) + { + if ( maxObjects == 1 ) + { + found[0].mNode = this; + found[0].mDistance = m; + } + else + { + found[1] = found[0]; + found[0].mNode = this; + found[0].mDistance = m; + } + } + else if ( maxObjects > 1) + { + found[1].mNode = this; + found[1].mDistance = m; + } + break; + default: + { + bool inserted = false; + + for (uint32_t i=0; i<count; i++) + { + if ( m < found[i].mDistance ) // if this one is closer than a pre-existing one... + { + // insertion sort... + uint32_t scan = count; + if ( scan >= maxObjects ) scan=maxObjects-1; + for (uint32_t j=scan; j>i; j--) + { + found[j] = found[j-1]; + } + found[i].mNode = this; + found[i].mDistance = m; + inserted = true; + break; + } + } + + if ( !inserted && count < maxObjects ) + { + found[count].mNode = this; + found[count].mDistance = m; + } + } + break; + } + count++; + if ( count > maxObjects ) + { + count = maxObjects; + } + } + + + if ( search1 ) + search1->search( axis, pos,radius, count, maxObjects, found, iface); + + if ( search2 ) + search2->search( axis, pos,radius, count, maxObjects, found, iface); + + } + +private: + + void setLeft(KdTreeNode *left) { mLeft = left; }; + void setRight(KdTreeNode *right) { mRight = right; }; + + KdTreeNode *getLeft(void) { return mLeft; } + KdTreeNode *getRight(void) { return mRight; } + + uint32_t mIndex; + KdTreeNode *mLeft; + KdTreeNode *mRight; +}; + + +#define MAX_BUNDLE_SIZE 1024 // 1024 nodes at a time, to minimize memory allocation and guarantee that pointers are persistent. + +class KdTreeNodeBundle +{ +public: + + KdTreeNodeBundle(void) + { + mNext = 0; + mIndex = 0; + } + + bool isFull(void) const + { + return (bool)( mIndex == MAX_BUNDLE_SIZE ); + } + + KdTreeNode * getNextNode(void) + { + assert(mIndex<MAX_BUNDLE_SIZE); + KdTreeNode *ret = &mNodes[mIndex]; + mIndex++; + return ret; + } + + KdTreeNodeBundle *mNext; + uint32_t mIndex; + KdTreeNode mNodes[MAX_BUNDLE_SIZE]; +}; + + +typedef std::vector< double > DoubleVector; +typedef std::vector< float > FloatVector; + +class KdTree : public KdTreeInterface +{ +public: + KdTree(void) + { + mRoot = 0; + mBundle = 0; + mVcount = 0; + mUseDouble = false; + } + + virtual ~KdTree(void) + { + reset(); + } + + const double * getPositionDouble(uint32_t index) const + { + assert( mUseDouble ); + assert ( index < mVcount ); + return &mVerticesDouble[index*3]; + } + + const float * getPositionFloat(uint32_t index) const + { + assert( !mUseDouble ); + assert ( index < mVcount ); + return &mVerticesFloat[index*3]; + } + + uint32_t search(const double *pos,double radius,uint32_t maxObjects,KdTreeFindNode *found) const + { + assert( mUseDouble ); + if ( !mRoot ) return 0; + uint32_t count = 0; + mRoot->search(X_AXIS,pos,radius,count,maxObjects,found,this); + return count; + } + + uint32_t search(const float *pos,float radius,uint32_t maxObjects,KdTreeFindNode *found) const + { + assert( !mUseDouble ); + if ( !mRoot ) return 0; + uint32_t count = 0; + mRoot->search(X_AXIS,pos,radius,count,maxObjects,found,this); + return count; + } + + void reset(void) + { + mRoot = 0; + mVerticesDouble.clear(); + mVerticesFloat.clear(); + KdTreeNodeBundle *bundle = mBundle; + while ( bundle ) + { + KdTreeNodeBundle *next = bundle->mNext; + delete bundle; + bundle = next; + } + mBundle = 0; + mVcount = 0; + } + + uint32_t add(double x,double y,double z) + { + assert(mUseDouble); + uint32_t ret = mVcount; + mVerticesDouble.push_back(x); + mVerticesDouble.push_back(y); + mVerticesDouble.push_back(z); + mVcount++; + KdTreeNode *node = getNewNode(ret); + if ( mRoot ) + { + mRoot->addDouble(node,X_AXIS,this); + } + else + { + mRoot = node; + } + return ret; + } + + uint32_t add(float x,float y,float z) + { + assert(!mUseDouble); + uint32_t ret = mVcount; + mVerticesFloat.push_back(x); + mVerticesFloat.push_back(y); + mVerticesFloat.push_back(z); + mVcount++; + KdTreeNode *node = getNewNode(ret); + if ( mRoot ) + { + mRoot->addFloat(node,X_AXIS,this); + } + else + { + mRoot = node; + } + return ret; + } + + KdTreeNode * getNewNode(uint32_t index) + { + if ( mBundle == 0 ) + { + mBundle = new KdTreeNodeBundle; + } + if ( mBundle->isFull() ) + { + KdTreeNodeBundle *bundle = new KdTreeNodeBundle; + mBundle->mNext = bundle; + mBundle = bundle; + } + KdTreeNode *node = mBundle->getNextNode(); + new ( node ) KdTreeNode(index); + return node; + } + + uint32_t getNearest(const double *pos,double radius,bool &_found) const // returns the nearest possible neighbor's index. + { + assert( mUseDouble ); + uint32_t ret = 0; + + _found = false; + KdTreeFindNode found[1]; + uint32_t count = search(pos,radius,1,found); + if ( count ) + { + KdTreeNode *node = found[0].mNode; + ret = node->getIndex(); + _found = true; + } + return ret; + } + + uint32_t getNearest(const float *pos,float radius,bool &_found) const // returns the nearest possible neighbor's index. + { + assert( !mUseDouble ); + uint32_t ret = 0; + + _found = false; + KdTreeFindNode found[1]; + uint32_t count = search(pos,radius,1,found); + if ( count ) + { + KdTreeNode *node = found[0].mNode; + ret = node->getIndex(); + _found = true; + } + return ret; + } + + const double * getVerticesDouble(void) const + { + assert( mUseDouble ); + const double *ret = 0; + if ( !mVerticesDouble.empty() ) + { + ret = &mVerticesDouble[0]; + } + return ret; + } + + const float * getVerticesFloat(void) const + { + assert( !mUseDouble ); + const float * ret = 0; + if ( !mVerticesFloat.empty() ) + { + ret = &mVerticesFloat[0]; + } + return ret; + } + + uint32_t getVcount(void) const { return mVcount; }; + + void setUseDouble(bool useDouble) + { + mUseDouble = useDouble; + } + +private: + bool mUseDouble; + KdTreeNode *mRoot; + KdTreeNodeBundle *mBundle; + uint32_t mVcount; + DoubleVector mVerticesDouble; + FloatVector mVerticesFloat; +}; + +}; // end of namespace VERTEX_INDEX + +class MyVertexIndex : public fm_VertexIndex +{ +public: + MyVertexIndex(double granularity,bool snapToGrid) + { + mDoubleGranularity = granularity; + mFloatGranularity = (float)granularity; + mSnapToGrid = snapToGrid; + mUseDouble = true; + mKdTree.setUseDouble(true); + } + + MyVertexIndex(float granularity,bool snapToGrid) + { + mDoubleGranularity = granularity; + mFloatGranularity = (float)granularity; + mSnapToGrid = snapToGrid; + mUseDouble = false; + mKdTree.setUseDouble(false); + } + + virtual ~MyVertexIndex(void) + { + + } + + + double snapToGrid(double p) + { + double m = fmod(p,mDoubleGranularity); + p-=m; + return p; + } + + float snapToGrid(float p) + { + float m = fmodf(p,mFloatGranularity); + p-=m; + return p; + } + + uint32_t getIndex(const float *_p,bool &newPos) // get index for a vector float + { + uint32_t ret; + + if ( mUseDouble ) + { + double p[3]; + p[0] = _p[0]; + p[1] = _p[1]; + p[2] = _p[2]; + return getIndex(p,newPos); + } + + newPos = false; + + float p[3]; + + if ( mSnapToGrid ) + { + p[0] = snapToGrid(_p[0]); + p[1] = snapToGrid(_p[1]); + p[2] = snapToGrid(_p[2]); + } + else + { + p[0] = _p[0]; + p[1] = _p[1]; + p[2] = _p[2]; + } + + bool found; + ret = mKdTree.getNearest(p,mFloatGranularity,found); + if ( !found ) + { + newPos = true; + ret = mKdTree.add(p[0],p[1],p[2]); + } + + + return ret; + } + + uint32_t getIndex(const double *_p,bool &newPos) // get index for a vector double + { + uint32_t ret; + + if ( !mUseDouble ) + { + float p[3]; + p[0] = (float)_p[0]; + p[1] = (float)_p[1]; + p[2] = (float)_p[2]; + return getIndex(p,newPos); + } + + newPos = false; + + double p[3]; + + if ( mSnapToGrid ) + { + p[0] = snapToGrid(_p[0]); + p[1] = snapToGrid(_p[1]); + p[2] = snapToGrid(_p[2]); + } + else + { + p[0] = _p[0]; + p[1] = _p[1]; + p[2] = _p[2]; + } + + bool found; + ret = mKdTree.getNearest(p,mDoubleGranularity,found); + if ( !found ) + { + newPos = true; + ret = mKdTree.add(p[0],p[1],p[2]); + } + + + return ret; + } + + const float * getVerticesFloat(void) const + { + const float * ret = 0; + + assert( !mUseDouble ); + + ret = mKdTree.getVerticesFloat(); + + return ret; + } + + const double * getVerticesDouble(void) const + { + const double * ret = 0; + + assert( mUseDouble ); + + ret = mKdTree.getVerticesDouble(); + + return ret; + } + + const float * getVertexFloat(uint32_t index) const + { + const float * ret = 0; + assert( !mUseDouble ); +#ifdef _DEBUG + uint32_t vcount = mKdTree.getVcount(); + assert( index < vcount ); +#endif + ret = mKdTree.getVerticesFloat(); + ret = &ret[index*3]; + return ret; + } + + const double * getVertexDouble(uint32_t index) const + { + const double * ret = 0; + assert( mUseDouble ); +#ifdef _DEBUG + uint32_t vcount = mKdTree.getVcount(); + assert( index < vcount ); +#endif + ret = mKdTree.getVerticesDouble(); + ret = &ret[index*3]; + + return ret; + } + + uint32_t getVcount(void) const + { + return mKdTree.getVcount(); + } + + bool isDouble(void) const + { + return mUseDouble; + } + + + bool saveAsObj(const char *fname,uint32_t tcount,uint32_t *indices) + { + bool ret = false; + + + FILE *fph = fopen(fname,"wb"); + if ( fph ) + { + ret = true; + + uint32_t vcount = getVcount(); + if ( mUseDouble ) + { + const double *v = getVerticesDouble(); + for (uint32_t i=0; i<vcount; i++) + { + fprintf(fph,"v %0.9f %0.9f %0.9f\r\n", (float)v[0], (float)v[1], (float)v[2] ); + v+=3; + } + } + else + { + const float *v = getVerticesFloat(); + for (uint32_t i=0; i<vcount; i++) + { + fprintf(fph,"v %0.9f %0.9f %0.9f\r\n", v[0], v[1], v[2] ); + v+=3; + } + } + + for (uint32_t i=0; i<tcount; i++) + { + uint32_t i1 = *indices++; + uint32_t i2 = *indices++; + uint32_t i3 = *indices++; + fprintf(fph,"f %d %d %d\r\n", i1+1, i2+1, i3+1 ); + } + fclose(fph); + } + + return ret; + } + +private: + bool mUseDouble:1; + bool mSnapToGrid:1; + double mDoubleGranularity; + float mFloatGranularity; + VERTEX_INDEX::KdTree mKdTree; +}; + +fm_VertexIndex * fm_createVertexIndex(double granularity,bool snapToGrid) // create an indexed vertex system for doubles +{ + MyVertexIndex *ret = new MyVertexIndex(granularity,snapToGrid); + return static_cast< fm_VertexIndex *>(ret); +} + +fm_VertexIndex * fm_createVertexIndex(float granularity,bool snapToGrid) // create an indexed vertext system for floats +{ + MyVertexIndex *ret = new MyVertexIndex(granularity,snapToGrid); + return static_cast< fm_VertexIndex *>(ret); +} + +void fm_releaseVertexIndex(fm_VertexIndex *vindex) +{ + MyVertexIndex *m = static_cast< MyVertexIndex *>(vindex); + delete m; +} + +#endif // END OF VERTEX WELDING CODE + + +REAL fm_computeBestFitAABB(uint32_t vcount,const REAL *points,uint32_t pstride,REAL *bmin,REAL *bmax) // returns the diagonal distance +{ + + const uint8_t *source = (const uint8_t *) points; + + bmin[0] = points[0]; + bmin[1] = points[1]; + bmin[2] = points[2]; + + bmax[0] = points[0]; + bmax[1] = points[1]; + bmax[2] = points[2]; + + + for (uint32_t i=1; i<vcount; i++) + { + source+=pstride; + const REAL *p = (const REAL *) source; + + if ( p[0] < bmin[0] ) bmin[0] = p[0]; + if ( p[1] < bmin[1] ) bmin[1] = p[1]; + if ( p[2] < bmin[2] ) bmin[2] = p[2]; + + if ( p[0] > bmax[0] ) bmax[0] = p[0]; + if ( p[1] > bmax[1] ) bmax[1] = p[1]; + if ( p[2] > bmax[2] ) bmax[2] = p[2]; + + } + + REAL dx = bmax[0] - bmin[0]; + REAL dy = bmax[1] - bmin[1]; + REAL dz = bmax[2] - bmin[2]; + + return (REAL) sqrt( dx*dx + dy*dy + dz*dz ); + +} + + + +/* a = b - c */ +#define vector(a,b,c) \ + (a)[0] = (b)[0] - (c)[0]; \ + (a)[1] = (b)[1] - (c)[1]; \ + (a)[2] = (b)[2] - (c)[2]; + + + +#define innerProduct(v,q) \ + ((v)[0] * (q)[0] + \ + (v)[1] * (q)[1] + \ + (v)[2] * (q)[2]) + +#define crossProduct(a,b,c) \ + (a)[0] = (b)[1] * (c)[2] - (c)[1] * (b)[2]; \ + (a)[1] = (b)[2] * (c)[0] - (c)[2] * (b)[0]; \ + (a)[2] = (b)[0] * (c)[1] - (c)[0] * (b)[1]; + + +bool fm_lineIntersectsTriangle(const REAL *rayStart,const REAL *rayEnd,const REAL *p1,const REAL *p2,const REAL *p3,REAL *sect) +{ + REAL dir[3]; + + dir[0] = rayEnd[0] - rayStart[0]; + dir[1] = rayEnd[1] - rayStart[1]; + dir[2] = rayEnd[2] - rayStart[2]; + + REAL d = (REAL)sqrt(dir[0]*dir[0] + dir[1]*dir[1] + dir[2]*dir[2]); + REAL r = 1.0f / d; + + dir[0]*=r; + dir[1]*=r; + dir[2]*=r; + + + REAL t; + + bool ret = fm_rayIntersectsTriangle(rayStart, dir, p1, p2, p3, t ); + + if ( ret ) + { + if ( t > d ) + { + sect[0] = rayStart[0] + dir[0]*t; + sect[1] = rayStart[1] + dir[1]*t; + sect[2] = rayStart[2] + dir[2]*t; + } + else + { + ret = false; + } + } + + return ret; +} + + + +bool fm_rayIntersectsTriangle(const REAL *p,const REAL *d,const REAL *v0,const REAL *v1,const REAL *v2,REAL &t) +{ + REAL e1[3],e2[3],h[3],s[3],q[3]; + REAL a,f,u,v; + + vector(e1,v1,v0); + vector(e2,v2,v0); + crossProduct(h,d,e2); + a = innerProduct(e1,h); + + if (a > -0.00001 && a < 0.00001) + return(false); + + f = 1/a; + vector(s,p,v0); + u = f * (innerProduct(s,h)); + + if (u < 0.0 || u > 1.0) + return(false); + + crossProduct(q,s,e1); + v = f * innerProduct(d,q); + if (v < 0.0 || u + v > 1.0) + return(false); + // at this stage we can compute t to find out where + // the intersection point is on the line + t = f * innerProduct(e2,q); + if (t > 0) // ray intersection + return(true); + else // this means that there is a line intersection + // but not a ray intersection + return (false); +} + + +inline REAL det(const REAL *p1,const REAL *p2,const REAL *p3) +{ + return p1[0]*p2[1]*p3[2] + p2[0]*p3[1]*p1[2] + p3[0]*p1[1]*p2[2] -p1[0]*p3[1]*p2[2] - p2[0]*p1[1]*p3[2] - p3[0]*p2[1]*p1[2]; +} + + +REAL fm_computeMeshVolume(const REAL *vertices,uint32_t tcount,const uint32_t *indices) +{ + REAL volume = 0; + + for (uint32_t i=0; i<tcount; i++,indices+=3) + { + const REAL *p1 = &vertices[ indices[0]*3 ]; + const REAL *p2 = &vertices[ indices[1]*3 ]; + const REAL *p3 = &vertices[ indices[2]*3 ]; + volume+=det(p1,p2,p3); // compute the volume of the tetrahedran relative to the origin. + } + + volume*=(1.0f/6.0f); + if ( volume < 0 ) + volume*=-1; + return volume; +} + + +const REAL * fm_getPoint(const REAL *points,uint32_t pstride,uint32_t index) +{ + const uint8_t *scan = (const uint8_t *)points; + scan+=(index*pstride); + return (REAL *)scan; +} + + +bool fm_insideTriangle(REAL Ax, REAL Ay, + REAL Bx, REAL By, + REAL Cx, REAL Cy, + REAL Px, REAL Py) + +{ + REAL ax, ay, bx, by, cx, cy, apx, apy, bpx, bpy, cpx, cpy; + REAL cCROSSap, bCROSScp, aCROSSbp; + + ax = Cx - Bx; ay = Cy - By; + bx = Ax - Cx; by = Ay - Cy; + cx = Bx - Ax; cy = By - Ay; + apx= Px - Ax; apy= Py - Ay; + bpx= Px - Bx; bpy= Py - By; + cpx= Px - Cx; cpy= Py - Cy; + + aCROSSbp = ax*bpy - ay*bpx; + cCROSSap = cx*apy - cy*apx; + bCROSScp = bx*cpy - by*cpx; + + return ((aCROSSbp >= 0.0f) && (bCROSScp >= 0.0f) && (cCROSSap >= 0.0f)); +} + + +REAL fm_areaPolygon2d(uint32_t pcount,const REAL *points,uint32_t pstride) +{ + int32_t n = (int32_t)pcount; + + REAL A=0.0f; + for(int32_t p=n-1,q=0; q<n; p=q++) + { + const REAL *p1 = fm_getPoint(points,pstride,p); + const REAL *p2 = fm_getPoint(points,pstride,q); + A+= p1[0]*p2[1] - p2[0]*p1[1]; + } + return A*0.5f; +} + + +bool fm_pointInsidePolygon2d(uint32_t pcount,const REAL *points,uint32_t pstride,const REAL *point,uint32_t xindex,uint32_t yindex) +{ + uint32_t j = pcount-1; + int32_t oddNodes = 0; + + REAL x = point[xindex]; + REAL y = point[yindex]; + + for (uint32_t i=0; i<pcount; i++) + { + const REAL *p1 = fm_getPoint(points,pstride,i); + const REAL *p2 = fm_getPoint(points,pstride,j); + + REAL x1 = p1[xindex]; + REAL y1 = p1[yindex]; + + REAL x2 = p2[xindex]; + REAL y2 = p2[yindex]; + + if ( (y1 < y && y2 >= y) || (y2 < y && y1 >= y) ) + { + if (x1+(y-y1)/(y2-y1)*(x2-x1)<x) + { + oddNodes = 1-oddNodes; + } + } + j = i; + } + + return oddNodes ? true : false; +} + + +uint32_t fm_consolidatePolygon(uint32_t pcount,const REAL *points,uint32_t pstride,REAL *_dest,REAL epsilon) // collapses co-linear edges. +{ + uint32_t ret = 0; + + + if ( pcount >= 3 ) + { + const REAL *prev = fm_getPoint(points,pstride,pcount-1); + const REAL *current = points; + const REAL *next = fm_getPoint(points,pstride,1); + REAL *dest = _dest; + + for (uint32_t i=0; i<pcount; i++) + { + + next = (i+1)==pcount ? points : next; + + if ( !fm_colinear(prev,current,next,epsilon) ) + { + dest[0] = current[0]; + dest[1] = current[1]; + dest[2] = current[2]; + + dest+=3; + ret++; + } + + prev = current; + current+=3; + next+=3; + + } + } + + return ret; +} + + +#ifndef RECT3D_TEMPLATE + +#define RECT3D_TEMPLATE + +template <class T> class Rect3d +{ +public: + Rect3d(void) { }; + + Rect3d(const T *bmin,const T *bmax) + { + + mMin[0] = bmin[0]; + mMin[1] = bmin[1]; + mMin[2] = bmin[2]; + + mMax[0] = bmax[0]; + mMax[1] = bmax[1]; + mMax[2] = bmax[2]; + + } + + void SetMin(const T *bmin) + { + mMin[0] = bmin[0]; + mMin[1] = bmin[1]; + mMin[2] = bmin[2]; + } + + void SetMax(const T *bmax) + { + mMax[0] = bmax[0]; + mMax[1] = bmax[1]; + mMax[2] = bmax[2]; + } + + void SetMin(T x,T y,T z) + { + mMin[0] = x; + mMin[1] = y; + mMin[2] = z; + } + + void SetMax(T x,T y,T z) + { + mMax[0] = x; + mMax[1] = y; + mMax[2] = z; + } + + T mMin[3]; + T mMax[3]; +}; + +#endif + +void splitRect(uint32_t axis, + const Rect3d<REAL> &source, + Rect3d<REAL> &b1, + Rect3d<REAL> &b2, + const REAL *midpoint) +{ + switch ( axis ) + { + case 0: + b1.SetMin(source.mMin); + b1.SetMax( midpoint[0], source.mMax[1], source.mMax[2] ); + + b2.SetMin( midpoint[0], source.mMin[1], source.mMin[2] ); + b2.SetMax(source.mMax); + + break; + case 1: + b1.SetMin(source.mMin); + b1.SetMax( source.mMax[0], midpoint[1], source.mMax[2] ); + + b2.SetMin( source.mMin[0], midpoint[1], source.mMin[2] ); + b2.SetMax(source.mMax); + + break; + case 2: + b1.SetMin(source.mMin); + b1.SetMax( source.mMax[0], source.mMax[1], midpoint[2] ); + + b2.SetMin( source.mMin[0], source.mMin[1], midpoint[2] ); + b2.SetMax(source.mMax); + + break; + } +} + +bool fm_computeSplitPlane(uint32_t vcount, + const REAL *vertices, + uint32_t /* tcount */, + const uint32_t * /* indices */, + REAL *plane) +{ + + REAL sides[3]; + REAL matrix[16]; + + fm_computeBestFitOBB( vcount, vertices, sizeof(REAL)*3, sides, matrix ); + + REAL bmax[3]; + REAL bmin[3]; + + bmax[0] = sides[0]*0.5f; + bmax[1] = sides[1]*0.5f; + bmax[2] = sides[2]*0.5f; + + bmin[0] = -bmax[0]; + bmin[1] = -bmax[1]; + bmin[2] = -bmax[2]; + + + REAL dx = sides[0]; + REAL dy = sides[1]; + REAL dz = sides[2]; + + + uint32_t axis = 0; + + if ( dy > dx ) + { + axis = 1; + } + + if ( dz > dx && dz > dy ) + { + axis = 2; + } + + REAL p1[3]; + REAL p2[3]; + REAL p3[3]; + + p3[0] = p2[0] = p1[0] = bmin[0] + dx*0.5f; + p3[1] = p2[1] = p1[1] = bmin[1] + dy*0.5f; + p3[2] = p2[2] = p1[2] = bmin[2] + dz*0.5f; + + Rect3d<REAL> b(bmin,bmax); + + Rect3d<REAL> b1,b2; + + splitRect(axis,b,b1,b2,p1); + + + switch ( axis ) + { + case 0: + p2[1] = bmin[1]; + p2[2] = bmin[2]; + + if ( dz > dy ) + { + p3[1] = bmax[1]; + p3[2] = bmin[2]; + } + else + { + p3[1] = bmin[1]; + p3[2] = bmax[2]; + } + + break; + case 1: + p2[0] = bmin[0]; + p2[2] = bmin[2]; + + if ( dx > dz ) + { + p3[0] = bmax[0]; + p3[2] = bmin[2]; + } + else + { + p3[0] = bmin[0]; + p3[2] = bmax[2]; + } + + break; + case 2: + p2[0] = bmin[0]; + p2[1] = bmin[1]; + + if ( dx > dy ) + { + p3[0] = bmax[0]; + p3[1] = bmin[1]; + } + else + { + p3[0] = bmin[0]; + p3[1] = bmax[1]; + } + + break; + } + + REAL tp1[3]; + REAL tp2[3]; + REAL tp3[3]; + + fm_transform(matrix,p1,tp1); + fm_transform(matrix,p2,tp2); + fm_transform(matrix,p3,tp3); + + plane[3] = fm_computePlane(tp1,tp2,tp3,plane); + + return true; + +} + +#pragma warning(disable:4100) + +void fm_nearestPointInTriangle(const REAL * /*nearestPoint*/,const REAL * /*p1*/,const REAL * /*p2*/,const REAL * /*p3*/,REAL * /*nearest*/) +{ + +} + +static REAL Partial(const REAL *a,const REAL *p) +{ + return (a[0]*p[1]) - (p[0]*a[1]); +} + +REAL fm_areaTriangle(const REAL *p0,const REAL *p1,const REAL *p2) +{ + REAL A = Partial(p0,p1); + A+= Partial(p1,p2); + A+= Partial(p2,p0); + return A*0.5f; +} + +void fm_subtract(const REAL *A,const REAL *B,REAL *diff) // compute A-B and store the result in 'diff' +{ + diff[0] = A[0]-B[0]; + diff[1] = A[1]-B[1]; + diff[2] = A[2]-B[2]; +} + + +void fm_multiplyTransform(const REAL *pA,const REAL *pB,REAL *pM) +{ + + REAL a = pA[0*4+0] * pB[0*4+0] + pA[0*4+1] * pB[1*4+0] + pA[0*4+2] * pB[2*4+0] + pA[0*4+3] * pB[3*4+0]; + REAL b = pA[0*4+0] * pB[0*4+1] + pA[0*4+1] * pB[1*4+1] + pA[0*4+2] * pB[2*4+1] + pA[0*4+3] * pB[3*4+1]; + REAL c = pA[0*4+0] * pB[0*4+2] + pA[0*4+1] * pB[1*4+2] + pA[0*4+2] * pB[2*4+2] + pA[0*4+3] * pB[3*4+2]; + REAL d = pA[0*4+0] * pB[0*4+3] + pA[0*4+1] * pB[1*4+3] + pA[0*4+2] * pB[2*4+3] + pA[0*4+3] * pB[3*4+3]; + + REAL e = pA[1*4+0] * pB[0*4+0] + pA[1*4+1] * pB[1*4+0] + pA[1*4+2] * pB[2*4+0] + pA[1*4+3] * pB[3*4+0]; + REAL f = pA[1*4+0] * pB[0*4+1] + pA[1*4+1] * pB[1*4+1] + pA[1*4+2] * pB[2*4+1] + pA[1*4+3] * pB[3*4+1]; + REAL g = pA[1*4+0] * pB[0*4+2] + pA[1*4+1] * pB[1*4+2] + pA[1*4+2] * pB[2*4+2] + pA[1*4+3] * pB[3*4+2]; + REAL h = pA[1*4+0] * pB[0*4+3] + pA[1*4+1] * pB[1*4+3] + pA[1*4+2] * pB[2*4+3] + pA[1*4+3] * pB[3*4+3]; + + REAL i = pA[2*4+0] * pB[0*4+0] + pA[2*4+1] * pB[1*4+0] + pA[2*4+2] * pB[2*4+0] + pA[2*4+3] * pB[3*4+0]; + REAL j = pA[2*4+0] * pB[0*4+1] + pA[2*4+1] * pB[1*4+1] + pA[2*4+2] * pB[2*4+1] + pA[2*4+3] * pB[3*4+1]; + REAL k = pA[2*4+0] * pB[0*4+2] + pA[2*4+1] * pB[1*4+2] + pA[2*4+2] * pB[2*4+2] + pA[2*4+3] * pB[3*4+2]; + REAL l = pA[2*4+0] * pB[0*4+3] + pA[2*4+1] * pB[1*4+3] + pA[2*4+2] * pB[2*4+3] + pA[2*4+3] * pB[3*4+3]; + + REAL m = pA[3*4+0] * pB[0*4+0] + pA[3*4+1] * pB[1*4+0] + pA[3*4+2] * pB[2*4+0] + pA[3*4+3] * pB[3*4+0]; + REAL n = pA[3*4+0] * pB[0*4+1] + pA[3*4+1] * pB[1*4+1] + pA[3*4+2] * pB[2*4+1] + pA[3*4+3] * pB[3*4+1]; + REAL o = pA[3*4+0] * pB[0*4+2] + pA[3*4+1] * pB[1*4+2] + pA[3*4+2] * pB[2*4+2] + pA[3*4+3] * pB[3*4+2]; + REAL p = pA[3*4+0] * pB[0*4+3] + pA[3*4+1] * pB[1*4+3] + pA[3*4+2] * pB[2*4+3] + pA[3*4+3] * pB[3*4+3]; + + pM[0] = a; pM[1] = b; pM[2] = c; pM[3] = d; + + pM[4] = e; pM[5] = f; pM[6] = g; pM[7] = h; + + pM[8] = i; pM[9] = j; pM[10] = k; pM[11] = l; + + pM[12] = m; pM[13] = n; pM[14] = o; pM[15] = p; +} + +void fm_multiply(REAL *A,REAL scaler) +{ + A[0]*=scaler; + A[1]*=scaler; + A[2]*=scaler; +} + +void fm_add(const REAL *A,const REAL *B,REAL *sum) +{ + sum[0] = A[0]+B[0]; + sum[1] = A[1]+B[1]; + sum[2] = A[2]+B[2]; +} + +void fm_copy3(const REAL *source,REAL *dest) +{ + dest[0] = source[0]; + dest[1] = source[1]; + dest[2] = source[2]; +} + + +uint32_t fm_copyUniqueVertices(uint32_t vcount,const REAL *input_vertices,REAL *output_vertices,uint32_t tcount,const uint32_t *input_indices,uint32_t *output_indices) +{ + uint32_t ret = 0; + + REAL *vertices = (REAL *)malloc(sizeof(REAL)*vcount*3); + memcpy(vertices,input_vertices,sizeof(REAL)*vcount*3); + REAL *dest = output_vertices; + + uint32_t *reindex = (uint32_t *)malloc(sizeof(uint32_t)*vcount); + memset(reindex,0xFF,sizeof(uint32_t)*vcount); + + uint32_t icount = tcount*3; + + for (uint32_t i=0; i<icount; i++) + { + uint32_t index = *input_indices++; + + assert( index < vcount ); + + if ( reindex[index] == 0xFFFFFFFF ) + { + *output_indices++ = ret; + reindex[index] = ret; + const REAL *pos = &vertices[index*3]; + dest[0] = pos[0]; + dest[1] = pos[1]; + dest[2] = pos[2]; + dest+=3; + ret++; + } + else + { + *output_indices++ = reindex[index]; + } + } + free(vertices); + free(reindex); + return ret; +} + +bool fm_isMeshCoplanar(uint32_t tcount,const uint32_t *indices,const REAL *vertices,bool doubleSided) // returns true if this collection of indexed triangles are co-planar! +{ + bool ret = true; + + if ( tcount > 0 ) + { + uint32_t i1 = indices[0]; + uint32_t i2 = indices[1]; + uint32_t i3 = indices[2]; + const REAL *p1 = &vertices[i1*3]; + const REAL *p2 = &vertices[i2*3]; + const REAL *p3 = &vertices[i3*3]; + REAL plane[4]; + plane[3] = fm_computePlane(p1,p2,p3,plane); + const uint32_t *scan = &indices[3]; + for (uint32_t i=1; i<tcount; i++) + { + i1 = *scan++; + i2 = *scan++; + i3 = *scan++; + p1 = &vertices[i1*3]; + p2 = &vertices[i2*3]; + p3 = &vertices[i3*3]; + REAL _plane[4]; + _plane[3] = fm_computePlane(p1,p2,p3,_plane); + if ( !fm_samePlane(plane,_plane,0.01f,0.001f,doubleSided) ) + { + ret = false; + break; + } + } + } + return ret; +} + + +bool fm_samePlane(const REAL p1[4],const REAL p2[4],REAL normalEpsilon,REAL dEpsilon,bool doubleSided) +{ + bool ret = false; + +#if 0 + if (p1[0] == p2[0] && + p1[1] == p2[1] && + p1[2] == p2[2] && + p1[3] == p2[3]) + { + ret = true; + } +#else + REAL diff = (REAL) fabs(p1[3]-p2[3]); + if ( diff < dEpsilon ) // if the plane -d co-efficient is within our epsilon + { + REAL dot = fm_dot(p1,p2); // compute the dot-product of the vector normals. + if ( doubleSided ) dot = (REAL)fabs(dot); + REAL dmin = 1 - normalEpsilon; + REAL dmax = 1 + normalEpsilon; + if ( dot >= dmin && dot <= dmax ) + { + ret = true; // then the plane equation is for practical purposes identical. + } + } +#endif + return ret; +} + + +void fm_initMinMax(REAL bmin[3],REAL bmax[3]) +{ + bmin[0] = FLT_MAX; + bmin[1] = FLT_MAX; + bmin[2] = FLT_MAX; + + bmax[0] = -FLT_MAX; + bmax[1] = -FLT_MAX; + bmax[2] = -FLT_MAX; +} + +void fm_inflateMinMax(REAL bmin[3], REAL bmax[3], REAL ratio) +{ + REAL inflate = fm_distance(bmin, bmax)*0.5f*ratio; + + bmin[0] -= inflate; + bmin[1] -= inflate; + bmin[2] -= inflate; + + bmax[0] += inflate; + bmax[1] += inflate; + bmax[2] += inflate; +} + +#ifndef TESSELATE_H + +#define TESSELATE_H + +typedef std::vector< uint32_t > UintVector; + +class Myfm_Tesselate : public fm_Tesselate +{ +public: + virtual ~Myfm_Tesselate(void) + { + + } + + const uint32_t * tesselate(fm_VertexIndex *vindex,uint32_t tcount,const uint32_t *indices,float longEdge,uint32_t maxDepth,uint32_t &outcount) + { + const uint32_t *ret = 0; + + mMaxDepth = maxDepth; + mLongEdge = longEdge*longEdge; + mLongEdgeD = mLongEdge; + mVertices = vindex; + + if ( mVertices->isDouble() ) + { + uint32_t vcount = mVertices->getVcount(); + double *vertices = (double *)malloc(sizeof(double)*vcount*3); + memcpy(vertices,mVertices->getVerticesDouble(),sizeof(double)*vcount*3); + + for (uint32_t i=0; i<tcount; i++) + { + uint32_t i1 = *indices++; + uint32_t i2 = *indices++; + uint32_t i3 = *indices++; + + const double *p1 = &vertices[i1*3]; + const double *p2 = &vertices[i2*3]; + const double *p3 = &vertices[i3*3]; + + tesselate(p1,p2,p3,0); + + } + free(vertices); + } + else + { + uint32_t vcount = mVertices->getVcount(); + float *vertices = (float *)malloc(sizeof(float)*vcount*3); + memcpy(vertices,mVertices->getVerticesFloat(),sizeof(float)*vcount*3); + + + for (uint32_t i=0; i<tcount; i++) + { + uint32_t i1 = *indices++; + uint32_t i2 = *indices++; + uint32_t i3 = *indices++; + + const float *p1 = &vertices[i1*3]; + const float *p2 = &vertices[i2*3]; + const float *p3 = &vertices[i3*3]; + + tesselate(p1,p2,p3,0); + + } + free(vertices); + } + + outcount = (uint32_t)(mIndices.size()/3); + ret = &mIndices[0]; + + + return ret; + } + + void tesselate(const float *p1,const float *p2,const float *p3,uint32_t recurse) + { + bool split = false; + float l1,l2,l3; + + l1 = l2 = l3 = 0; + + if ( recurse < mMaxDepth ) + { + l1 = fm_distanceSquared(p1,p2); + l2 = fm_distanceSquared(p2,p3); + l3 = fm_distanceSquared(p3,p1); + + if ( l1 > mLongEdge || l2 > mLongEdge || l3 > mLongEdge ) + split = true; + + } + + if ( split ) + { + uint32_t edge; + + if ( l1 >= l2 && l1 >= l3 ) + edge = 0; + else if ( l2 >= l1 && l2 >= l3 ) + edge = 1; + else + edge = 2; + + float splits[3]; + + switch ( edge ) + { + case 0: + { + fm_lerp(p1,p2,splits,0.5f); + tesselate(p1,splits,p3, recurse+1 ); + tesselate(splits,p2,p3, recurse+1 ); + } + break; + case 1: + { + fm_lerp(p2,p3,splits,0.5f); + tesselate(p1,p2,splits, recurse+1 ); + tesselate(p1,splits,p3, recurse+1 ); + } + break; + case 2: + { + fm_lerp(p3,p1,splits,0.5f); + tesselate(p1,p2,splits, recurse+1 ); + tesselate(splits,p2,p3, recurse+1 ); + } + break; + } + } + else + { + bool newp; + + uint32_t i1 = mVertices->getIndex(p1,newp); + uint32_t i2 = mVertices->getIndex(p2,newp); + uint32_t i3 = mVertices->getIndex(p3,newp); + + mIndices.push_back(i1); + mIndices.push_back(i2); + mIndices.push_back(i3); + } + + } + + void tesselate(const double *p1,const double *p2,const double *p3,uint32_t recurse) + { + bool split = false; + double l1,l2,l3; + + l1 = l2 = l3 = 0; + + if ( recurse < mMaxDepth ) + { + l1 = fm_distanceSquared(p1,p2); + l2 = fm_distanceSquared(p2,p3); + l3 = fm_distanceSquared(p3,p1); + + if ( l1 > mLongEdgeD || l2 > mLongEdgeD || l3 > mLongEdgeD ) + split = true; + + } + + if ( split ) + { + uint32_t edge; + + if ( l1 >= l2 && l1 >= l3 ) + edge = 0; + else if ( l2 >= l1 && l2 >= l3 ) + edge = 1; + else + edge = 2; + + double splits[3]; + + switch ( edge ) + { + case 0: + { + fm_lerp(p1,p2,splits,0.5); + tesselate(p1,splits,p3, recurse+1 ); + tesselate(splits,p2,p3, recurse+1 ); + } + break; + case 1: + { + fm_lerp(p2,p3,splits,0.5); + tesselate(p1,p2,splits, recurse+1 ); + tesselate(p1,splits,p3, recurse+1 ); + } + break; + case 2: + { + fm_lerp(p3,p1,splits,0.5); + tesselate(p1,p2,splits, recurse+1 ); + tesselate(splits,p2,p3, recurse+1 ); + } + break; + } + } + else + { + bool newp; + + uint32_t i1 = mVertices->getIndex(p1,newp); + uint32_t i2 = mVertices->getIndex(p2,newp); + uint32_t i3 = mVertices->getIndex(p3,newp); + + mIndices.push_back(i1); + mIndices.push_back(i2); + mIndices.push_back(i3); + } + + } + +private: + float mLongEdge; + double mLongEdgeD; + fm_VertexIndex *mVertices; + UintVector mIndices; + uint32_t mMaxDepth; +}; + +fm_Tesselate * fm_createTesselate(void) +{ + Myfm_Tesselate *m = new Myfm_Tesselate; + return static_cast< fm_Tesselate * >(m); +} + +void fm_releaseTesselate(fm_Tesselate *t) +{ + Myfm_Tesselate *m = static_cast< Myfm_Tesselate *>(t); + delete m; +} + +#endif + + +#ifndef RAY_ABB_INTERSECT + +#define RAY_ABB_INTERSECT + +//! Integer representation of a floating-point value. +#define IR(x) ((uint32_t&)x) + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** +* A method to compute a ray-AABB intersection. +* Original code by Andrew Woo, from "Graphics Gems", Academic Press, 1990 +* Optimized code by Pierre Terdiman, 2000 (~20-30% faster on my Celeron 500) +* Epsilon value added by Klaus Hartmann. (discarding it saves a few cycles only) +* +* Hence this version is faster as well as more robust than the original one. +* +* Should work provided: +* 1) the integer representation of 0.0f is 0x00000000 +* 2) the sign bit of the float is the most significant one +* +* Report bugs: p.terdiman@codercorner.com +* +* \param aabb [in] the axis-aligned bounding box +* \param origin [in] ray origin +* \param dir [in] ray direction +* \param coord [out] impact coordinates +* \return true if ray intersects AABB +*/ +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +#define RAYAABB_EPSILON 0.00001f +bool fm_intersectRayAABB(const float MinB[3],const float MaxB[3],const float origin[3],const float dir[3],float coord[3]) +{ + bool Inside = true; + float MaxT[3]; + MaxT[0]=MaxT[1]=MaxT[2]=-1.0f; + + // Find candidate planes. + for(uint32_t i=0;i<3;i++) + { + if(origin[i] < MinB[i]) + { + coord[i] = MinB[i]; + Inside = false; + + // Calculate T distances to candidate planes + if(IR(dir[i])) MaxT[i] = (MinB[i] - origin[i]) / dir[i]; + } + else if(origin[i] > MaxB[i]) + { + coord[i] = MaxB[i]; + Inside = false; + + // Calculate T distances to candidate planes + if(IR(dir[i])) MaxT[i] = (MaxB[i] - origin[i]) / dir[i]; + } + } + + // Ray origin inside bounding box + if(Inside) + { + coord[0] = origin[0]; + coord[1] = origin[1]; + coord[2] = origin[2]; + return true; + } + + // Get largest of the maxT's for final choice of intersection + uint32_t WhichPlane = 0; + if(MaxT[1] > MaxT[WhichPlane]) WhichPlane = 1; + if(MaxT[2] > MaxT[WhichPlane]) WhichPlane = 2; + + // Check final candidate actually inside box + if(IR(MaxT[WhichPlane])&0x80000000) return false; + + for(uint32_t i=0;i<3;i++) + { + if(i!=WhichPlane) + { + coord[i] = origin[i] + MaxT[WhichPlane] * dir[i]; +#ifdef RAYAABB_EPSILON + if(coord[i] < MinB[i] - RAYAABB_EPSILON || coord[i] > MaxB[i] + RAYAABB_EPSILON) return false; +#else + if(coord[i] < MinB[i] || coord[i] > MaxB[i]) return false; +#endif + } + } + return true; // ray hits box +} + +bool fm_intersectLineSegmentAABB(const float bmin[3],const float bmax[3],const float p1[3],const float p2[3],float intersect[3]) +{ + bool ret = false; + + float dir[3]; + dir[0] = p2[0] - p1[0]; + dir[1] = p2[1] - p1[1]; + dir[2] = p2[2] - p1[2]; + float dist = fm_normalize(dir); + if ( dist > RAYAABB_EPSILON ) + { + ret = fm_intersectRayAABB(bmin,bmax,p1,dir,intersect); + if ( ret ) + { + float d = fm_distanceSquared(p1,intersect); + if ( d > (dist*dist) ) + { + ret = false; + } + } + } + return ret; +} + +#endif + +#ifndef OBB_TO_AABB + +#define OBB_TO_AABB + +#pragma warning(disable:4100) + +void fm_OBBtoAABB(const float /*obmin*/[3],const float /*obmax*/[3],const float /*matrix*/[16],float /*abmin*/[3],float /*abmax*/[3]) +{ + assert(0); // not yet implemented. +} + + +const REAL * computePos(uint32_t index,const REAL *vertices,uint32_t vstride) +{ + const char *tmp = (const char *)vertices; + tmp+=(index*vstride); + return (const REAL*)tmp; +} + +void computeNormal(uint32_t index,REAL *normals,uint32_t nstride,const REAL *normal) +{ + char *tmp = (char *)normals; + tmp+=(index*nstride); + REAL *dest = (REAL *)tmp; + dest[0]+=normal[0]; + dest[1]+=normal[1]; + dest[2]+=normal[2]; +} + +void fm_computeMeanNormals(uint32_t vcount, // the number of vertices + const REAL *vertices, // the base address of the vertex position data. + uint32_t vstride, // the stride between position data. + REAL *normals, // the base address of the destination for mean vector normals + uint32_t nstride, // the stride between normals + uint32_t tcount, // the number of triangles + const uint32_t *indices) // the triangle indices +{ + + // Step #1 : Zero out the vertex normals + char *dest = (char *)normals; + for (uint32_t i=0; i<vcount; i++) + { + REAL *n = (REAL *)dest; + n[0] = 0; + n[1] = 0; + n[2] = 0; + dest+=nstride; + } + + // Step #2 : Compute the face normals and accumulate them + const uint32_t *scan = indices; + for (uint32_t i=0; i<tcount; i++) + { + + uint32_t i1 = *scan++; + uint32_t i2 = *scan++; + uint32_t i3 = *scan++; + + const REAL *p1 = computePos(i1,vertices,vstride); + const REAL *p2 = computePos(i2,vertices,vstride); + const REAL *p3 = computePos(i3,vertices,vstride); + + REAL normal[3]; + fm_computePlane(p3,p2,p1,normal); + + computeNormal(i1,normals,nstride,normal); + computeNormal(i2,normals,nstride,normal); + computeNormal(i3,normals,nstride,normal); + } + + + // Normalize the accumulated normals + dest = (char *)normals; + for (uint32_t i=0; i<vcount; i++) + { + REAL *n = (REAL *)dest; + fm_normalize(n); + dest+=nstride; + } + +} + +#endif + + +#define BIGNUMBER 100000000.0 /* hundred million */ + +static inline void Set(REAL *n,REAL x,REAL y,REAL z) +{ + n[0] = x; + n[1] = y; + n[2] = z; +}; + +static inline void Copy(REAL *dest,const REAL *source) +{ + dest[0] = source[0]; + dest[1] = source[1]; + dest[2] = source[2]; +} + + +REAL fm_computeBestFitSphere(uint32_t vcount,const REAL *points,uint32_t pstride,REAL *center) +{ + REAL radius; + REAL radius2; + + REAL xmin[3]; + REAL xmax[3]; + REAL ymin[3]; + REAL ymax[3]; + REAL zmin[3]; + REAL zmax[3]; + REAL dia1[3]; + REAL dia2[3]; + + /* FIRST PASS: find 6 minima/maxima points */ + Set(xmin,BIGNUMBER,BIGNUMBER,BIGNUMBER); + Set(xmax,-BIGNUMBER,-BIGNUMBER,-BIGNUMBER); + Set(ymin,BIGNUMBER,BIGNUMBER,BIGNUMBER); + Set(ymax,-BIGNUMBER,-BIGNUMBER,-BIGNUMBER); + Set(zmin,BIGNUMBER,BIGNUMBER,BIGNUMBER); + Set(zmax,-BIGNUMBER,-BIGNUMBER,-BIGNUMBER); + + { + const char *scan = (const char *)points; + for (uint32_t i=0; i<vcount; i++) + { + const REAL *caller_p = (const REAL *)scan; + if (caller_p[0]<xmin[0]) + Copy(xmin,caller_p); /* New xminimum point */ + if (caller_p[0]>xmax[0]) + Copy(xmax,caller_p); + if (caller_p[1]<ymin[1]) + Copy(ymin,caller_p); + if (caller_p[1]>ymax[1]) + Copy(ymax,caller_p); + if (caller_p[2]<zmin[2]) + Copy(zmin,caller_p); + if (caller_p[2]>zmax[2]) + Copy(zmax,caller_p); + scan+=pstride; + } + } + + /* Set xspan = distance between the 2 points xmin & xmax (squared) */ + REAL dx = xmax[0] - xmin[0]; + REAL dy = xmax[1] - xmin[1]; + REAL dz = xmax[2] - xmin[2]; + REAL xspan = dx*dx + dy*dy + dz*dz; + +/* Same for y & z spans */ + dx = ymax[0] - ymin[0]; + dy = ymax[1] - ymin[1]; + dz = ymax[2] - ymin[2]; + REAL yspan = dx*dx + dy*dy + dz*dz; + + dx = zmax[0] - zmin[0]; + dy = zmax[1] - zmin[1]; + dz = zmax[2] - zmin[2]; + REAL zspan = dx*dx + dy*dy + dz*dz; + + /* Set points dia1 & dia2 to the maximally separated pair */ + Copy(dia1,xmin); + Copy(dia2,xmax); /* assume xspan biggest */ + REAL maxspan = xspan; + + if (yspan>maxspan) + { + maxspan = yspan; + Copy(dia1,ymin); + Copy(dia2,ymax); + } + + if (zspan>maxspan) + { + maxspan = zspan; + Copy(dia1,zmin); + Copy(dia2,zmax); + } + + + /* dia1,dia2 is a diameter of initial sphere */ + /* calc initial center */ + center[0] = (dia1[0]+dia2[0])*0.5f; + center[1] = (dia1[1]+dia2[1])*0.5f; + center[2] = (dia1[2]+dia2[2])*0.5f; + + /* calculate initial radius**2 and radius */ + + dx = dia2[0]-center[0]; /* x component of radius vector */ + dy = dia2[1]-center[1]; /* y component of radius vector */ + dz = dia2[2]-center[2]; /* z component of radius vector */ + + radius2 = dx*dx + dy*dy + dz*dz; + radius = REAL(sqrt(radius2)); + + /* SECOND PASS: increment current sphere */ + { + const char *scan = (const char *)points; + for (uint32_t i=0; i<vcount; i++) + { + const REAL *caller_p = (const REAL *)scan; + dx = caller_p[0]-center[0]; + dy = caller_p[1]-center[1]; + dz = caller_p[2]-center[2]; + REAL old_to_p_sq = dx*dx + dy*dy + dz*dz; + if (old_to_p_sq > radius2) /* do r**2 test first */ + { /* this point is outside of current sphere */ + REAL old_to_p = REAL(sqrt(old_to_p_sq)); + /* calc radius of new sphere */ + radius = (radius + old_to_p) * 0.5f; + radius2 = radius*radius; /* for next r**2 compare */ + REAL old_to_new = old_to_p - radius; + /* calc center of new sphere */ + REAL recip = 1.0f /old_to_p; + REAL cx = (radius*center[0] + old_to_new*caller_p[0]) * recip; + REAL cy = (radius*center[1] + old_to_new*caller_p[1]) * recip; + REAL cz = (radius*center[2] + old_to_new*caller_p[2]) * recip; + Set(center,cx,cy,cz); + scan+=pstride; + } + } + } + return radius; +} + + +void fm_computeBestFitCapsule(uint32_t vcount,const REAL *points,uint32_t pstride,REAL &radius,REAL &height,REAL matrix[16],bool bruteForce) +{ + REAL sides[3]; + REAL omatrix[16]; + fm_computeBestFitOBB(vcount,points,pstride,sides,omatrix,bruteForce); + + int32_t axis = 0; + if ( sides[0] > sides[1] && sides[0] > sides[2] ) + axis = 0; + else if ( sides[1] > sides[0] && sides[1] > sides[2] ) + axis = 1; + else + axis = 2; + + REAL localTransform[16]; + + REAL maxDist = 0; + REAL maxLen = 0; + + switch ( axis ) + { + case 0: + { + fm_eulerMatrix(0,0,FM_PI/2,localTransform); + fm_matrixMultiply(localTransform,omatrix,matrix); + + const uint8_t *scan = (const uint8_t *)points; + for (uint32_t i=0; i<vcount; i++) + { + const REAL *p = (const REAL *)scan; + REAL t[3]; + fm_inverseRT(omatrix,p,t); + REAL dist = t[1]*t[1]+t[2]*t[2]; + if ( dist > maxDist ) + { + maxDist = dist; + } + REAL l = (REAL) fabs(t[0]); + if ( l > maxLen ) + { + maxLen = l; + } + scan+=pstride; + } + } + height = sides[0]; + break; + case 1: + { + fm_eulerMatrix(0,FM_PI/2,0,localTransform); + fm_matrixMultiply(localTransform,omatrix,matrix); + + const uint8_t *scan = (const uint8_t *)points; + for (uint32_t i=0; i<vcount; i++) + { + const REAL *p = (const REAL *)scan; + REAL t[3]; + fm_inverseRT(omatrix,p,t); + REAL dist = t[0]*t[0]+t[2]*t[2]; + if ( dist > maxDist ) + { + maxDist = dist; + } + REAL l = (REAL) fabs(t[1]); + if ( l > maxLen ) + { + maxLen = l; + } + scan+=pstride; + } + } + height = sides[1]; + break; + case 2: + { + fm_eulerMatrix(FM_PI/2,0,0,localTransform); + fm_matrixMultiply(localTransform,omatrix,matrix); + + const uint8_t *scan = (const uint8_t *)points; + for (uint32_t i=0; i<vcount; i++) + { + const REAL *p = (const REAL *)scan; + REAL t[3]; + fm_inverseRT(omatrix,p,t); + REAL dist = t[0]*t[0]+t[1]*t[1]; + if ( dist > maxDist ) + { + maxDist = dist; + } + REAL l = (REAL) fabs(t[2]); + if ( l > maxLen ) + { + maxLen = l; + } + scan+=pstride; + } + } + height = sides[2]; + break; + } + radius = (REAL)sqrt(maxDist); + height = (maxLen*2)-(radius*2); +} + + +//************* Triangulation + +#ifndef TRIANGULATE_H + +#define TRIANGULATE_H + +typedef uint32_t TU32; + +class TVec +{ +public: + TVec(double _x,double _y,double _z) { x = _x; y = _y; z = _z; }; + TVec(void) { }; + + double x; + double y; + double z; +}; + +typedef std::vector< TVec > TVecVector; +typedef std::vector< TU32 > TU32Vector; + +class CTriangulator +{ +public: + /// Default constructor + CTriangulator(); + + /// Default destructor + virtual ~CTriangulator(); + + /// Triangulates the contour + void triangulate(TU32Vector &indices); + + /// Returns the given point in the triangulator array + inline TVec get(const TU32 id) { return mPoints[id]; } + + virtual void reset(void) + { + mInputPoints.clear(); + mPoints.clear(); + mIndices.clear(); + } + + virtual void addPoint(double x,double y,double z) + { + TVec v(x,y,z); + // update bounding box... + if ( mInputPoints.empty() ) + { + mMin = v; + mMax = v; + } + else + { + if ( x < mMin.x ) mMin.x = x; + if ( y < mMin.y ) mMin.y = y; + if ( z < mMin.z ) mMin.z = z; + + if ( x > mMax.x ) mMax.x = x; + if ( y > mMax.y ) mMax.y = y; + if ( z > mMax.z ) mMax.z = z; + } + mInputPoints.push_back(v); + } + + // Triangulation happens in 2d. We could inverse transform the polygon around the normal direction, or we just use the two most signficant axes + // Here we find the two longest axes and use them to triangulate. Inverse transforming them would introduce more doubleing point error and isn't worth it. + virtual uint32_t * triangulate(uint32_t &tcount,double epsilon) + { + uint32_t *ret = 0; + tcount = 0; + mEpsilon = epsilon; + + if ( !mInputPoints.empty() ) + { + mPoints.clear(); + + double dx = mMax.x - mMin.x; // locate the first, second and third longest edges and store them in i1, i2, i3 + double dy = mMax.y - mMin.y; + double dz = mMax.z - mMin.z; + + uint32_t i1,i2,i3; + + if ( dx > dy && dx > dz ) + { + i1 = 0; + if ( dy > dz ) + { + i2 = 1; + i3 = 2; + } + else + { + i2 = 2; + i3 = 1; + } + } + else if ( dy > dx && dy > dz ) + { + i1 = 1; + if ( dx > dz ) + { + i2 = 0; + i3 = 2; + } + else + { + i2 = 2; + i3 = 0; + } + } + else + { + i1 = 2; + if ( dx > dy ) + { + i2 = 0; + i3 = 1; + } + else + { + i2 = 1; + i3 = 0; + } + } + + uint32_t pcount = (uint32_t)mInputPoints.size(); + const double *points = &mInputPoints[0].x; + for (uint32_t i=0; i<pcount; i++) + { + TVec v( points[i1], points[i2], points[i3] ); + mPoints.push_back(v); + points+=3; + } + + mIndices.clear(); + triangulate(mIndices); + tcount = (uint32_t)mIndices.size()/3; + if ( tcount ) + { + ret = &mIndices[0]; + } + } + return ret; + } + + virtual const double * getPoint(uint32_t index) + { + return &mInputPoints[index].x; + } + + +private: + double mEpsilon; + TVec mMin; + TVec mMax; + TVecVector mInputPoints; + TVecVector mPoints; + TU32Vector mIndices; + + /// Tests if a point is inside the given triangle + bool _insideTriangle(const TVec& A, const TVec& B, const TVec& C,const TVec& P); + + /// Returns the area of the contour + double _area(); + + bool _snip(int32_t u, int32_t v, int32_t w, int32_t n, int32_t *V); + + /// Processes the triangulation + void _process(TU32Vector &indices); + +}; + +/// Default constructor +CTriangulator::CTriangulator(void) +{ +} + +/// Default destructor +CTriangulator::~CTriangulator() +{ +} + +/// Triangulates the contour +void CTriangulator::triangulate(TU32Vector &indices) +{ + _process(indices); +} + +/// Processes the triangulation +void CTriangulator::_process(TU32Vector &indices) +{ + const int32_t n = (const int32_t)mPoints.size(); + if (n < 3) + return; + int32_t *V = (int32_t *)malloc(sizeof(int32_t)*n); + + bool flipped = false; + + if (0.0f < _area()) + { + for (int32_t v = 0; v < n; v++) + V[v] = v; + } + else + { + flipped = true; + for (int32_t v = 0; v < n; v++) + V[v] = (n - 1) - v; + } + + int32_t nv = n; + int32_t count = 2 * nv; + for (int32_t m = 0, v = nv - 1; nv > 2;) + { + if (0 >= (count--)) + return; + + int32_t u = v; + if (nv <= u) + u = 0; + v = u + 1; + if (nv <= v) + v = 0; + int32_t w = v + 1; + if (nv <= w) + w = 0; + + if (_snip(u, v, w, nv, V)) + { + int32_t a, b, c, s, t; + a = V[u]; + b = V[v]; + c = V[w]; + if ( flipped ) + { + indices.push_back(a); + indices.push_back(b); + indices.push_back(c); + } + else + { + indices.push_back(c); + indices.push_back(b); + indices.push_back(a); + } + m++; + for (s = v, t = v + 1; t < nv; s++, t++) + V[s] = V[t]; + nv--; + count = 2 * nv; + } + } + + free(V); +} + +/// Returns the area of the contour +double CTriangulator::_area() +{ + int32_t n = (uint32_t)mPoints.size(); + double A = 0.0f; + for (int32_t p = n - 1, q = 0; q < n; p = q++) + { + const TVec &pval = mPoints[p]; + const TVec &qval = mPoints[q]; + A += pval.x * qval.y - qval.x * pval.y; + } + A*=0.5f; + return A; +} + +bool CTriangulator::_snip(int32_t u, int32_t v, int32_t w, int32_t n, int32_t *V) +{ + int32_t p; + + const TVec &A = mPoints[ V[u] ]; + const TVec &B = mPoints[ V[v] ]; + const TVec &C = mPoints[ V[w] ]; + + if (mEpsilon > (((B.x - A.x) * (C.y - A.y)) - ((B.y - A.y) * (C.x - A.x))) ) + return false; + + for (p = 0; p < n; p++) + { + if ((p == u) || (p == v) || (p == w)) + continue; + const TVec &P = mPoints[ V[p] ]; + if (_insideTriangle(A, B, C, P)) + return false; + } + return true; +} + +/// Tests if a point is inside the given triangle +bool CTriangulator::_insideTriangle(const TVec& A, const TVec& B, const TVec& C,const TVec& P) +{ + double ax, ay, bx, by, cx, cy, apx, apy, bpx, bpy, cpx, cpy; + double cCROSSap, bCROSScp, aCROSSbp; + + ax = C.x - B.x; ay = C.y - B.y; + bx = A.x - C.x; by = A.y - C.y; + cx = B.x - A.x; cy = B.y - A.y; + apx = P.x - A.x; apy = P.y - A.y; + bpx = P.x - B.x; bpy = P.y - B.y; + cpx = P.x - C.x; cpy = P.y - C.y; + + aCROSSbp = ax * bpy - ay * bpx; + cCROSSap = cx * apy - cy * apx; + bCROSScp = bx * cpy - by * cpx; + + return ((aCROSSbp >= 0.0f) && (bCROSScp >= 0.0f) && (cCROSSap >= 0.0f)); +} + +class Triangulate : public fm_Triangulate +{ +public: + Triangulate(void) + { + mPointsFloat = 0; + mPointsDouble = 0; + } + + virtual ~Triangulate(void) + { + reset(); + } + void reset(void) + { + free(mPointsFloat); + free(mPointsDouble); + mPointsFloat = 0; + mPointsDouble = 0; + } + + virtual const double * triangulate3d(uint32_t pcount, + const double *_points, + uint32_t vstride, + uint32_t &tcount, + bool consolidate, + double epsilon) + { + reset(); + + double *points = (double *)malloc(sizeof(double)*pcount*3); + if ( consolidate ) + { + pcount = fm_consolidatePolygon(pcount,_points,vstride,points,1-epsilon); + } + else + { + double *dest = points; + for (uint32_t i=0; i<pcount; i++) + { + const double *src = fm_getPoint(_points,vstride,i); + dest[0] = src[0]; + dest[1] = src[1]; + dest[2] = src[2]; + dest+=3; + } + vstride = sizeof(double)*3; + } + + if ( pcount >= 3 ) + { + CTriangulator ct; + for (uint32_t i=0; i<pcount; i++) + { + const double *src = fm_getPoint(points,vstride,i); + ct.addPoint( src[0], src[1], src[2] ); + } + uint32_t _tcount; + uint32_t *indices = ct.triangulate(_tcount,epsilon); + if ( indices ) + { + tcount = _tcount; + mPointsDouble = (double *)malloc(sizeof(double)*tcount*3*3); + double *dest = mPointsDouble; + for (uint32_t i=0; i<tcount; i++) + { + uint32_t i1 = indices[i*3+0]; + uint32_t i2 = indices[i*3+1]; + uint32_t i3 = indices[i*3+2]; + const double *p1 = ct.getPoint(i1); + const double *p2 = ct.getPoint(i2); + const double *p3 = ct.getPoint(i3); + + dest[0] = p1[0]; + dest[1] = p1[1]; + dest[2] = p1[2]; + + dest[3] = p2[0]; + dest[4] = p2[1]; + dest[5] = p2[2]; + + dest[6] = p3[0]; + dest[7] = p3[1]; + dest[8] = p3[2]; + dest+=9; + } + } + } + free(points); + + return mPointsDouble; + } + + virtual const float * triangulate3d(uint32_t pcount, + const float *points, + uint32_t vstride, + uint32_t &tcount, + bool consolidate, + float epsilon) + { + reset(); + + double *temp = (double *)malloc(sizeof(double)*pcount*3); + double *dest = temp; + for (uint32_t i=0; i<pcount; i++) + { + const float *p = fm_getPoint(points,vstride,i); + dest[0] = p[0]; + dest[1] = p[1]; + dest[2] = p[2]; + dest+=3; + } + const double *results = triangulate3d(pcount,temp,sizeof(double)*3,tcount,consolidate,epsilon); + if ( results ) + { + uint32_t fcount = tcount*3*3; + mPointsFloat = (float *)malloc(sizeof(float)*tcount*3*3); + for (uint32_t i=0; i<fcount; i++) + { + mPointsFloat[i] = (float) results[i]; + } + free(mPointsDouble); + mPointsDouble = 0; + } + free(temp); + + return mPointsFloat; + } + +private: + float *mPointsFloat; + double *mPointsDouble; +}; + +fm_Triangulate * fm_createTriangulate(void) +{ + Triangulate *t = new Triangulate; + return static_cast< fm_Triangulate *>(t); +} + +void fm_releaseTriangulate(fm_Triangulate *t) +{ + Triangulate *tt = static_cast< Triangulate *>(t); + delete tt; +} + +#endif + +bool validDistance(const REAL *p1,const REAL *p2,REAL epsilon) +{ + bool ret = true; + + REAL dx = p1[0] - p2[0]; + REAL dy = p1[1] - p2[1]; + REAL dz = p1[2] - p2[2]; + REAL dist = dx*dx+dy*dy+dz*dz; + if ( dist < (epsilon*epsilon) ) + { + ret = false; + } + return ret; +} + +bool fm_isValidTriangle(const REAL *p1,const REAL *p2,const REAL *p3,REAL epsilon) +{ + bool ret = false; + + if ( validDistance(p1,p2,epsilon) && + validDistance(p1,p3,epsilon) && + validDistance(p2,p3,epsilon) ) + { + + REAL area = fm_computeArea(p1,p2,p3); + if ( area > epsilon ) + { + REAL _vertices[3*3],vertices[64*3]; + + _vertices[0] = p1[0]; + _vertices[1] = p1[1]; + _vertices[2] = p1[2]; + + _vertices[3] = p2[0]; + _vertices[4] = p2[1]; + _vertices[5] = p2[2]; + + _vertices[6] = p3[0]; + _vertices[7] = p3[1]; + _vertices[8] = p3[2]; + + uint32_t pcount = fm_consolidatePolygon(3,_vertices,sizeof(REAL)*3,vertices,1-epsilon); + if ( pcount == 3 ) + { + ret = true; + } + } + } + return ret; +} + + +void fm_multiplyQuat(const REAL *left,const REAL *right,REAL *quat) +{ + REAL a,b,c,d; + + a = left[3]*right[3] - left[0]*right[0] - left[1]*right[1] - left[2]*right[2]; + b = left[3]*right[0] + right[3]*left[0] + left[1]*right[2] - right[1]*left[2]; + c = left[3]*right[1] + right[3]*left[1] + left[2]*right[0] - right[2]*left[0]; + d = left[3]*right[2] + right[3]*left[2] + left[0]*right[1] - right[0]*left[1]; + + quat[3] = a; + quat[0] = b; + quat[1] = c; + quat[2] = d; +} + +bool fm_computeCentroid(uint32_t vcount, // number of input data points + const REAL *points, // starting address of points array. + REAL *center) + +{ + bool ret = false; + if ( vcount ) + { + center[0] = 0; + center[1] = 0; + center[2] = 0; + const REAL *p = points; + for (uint32_t i=0; i<vcount; i++) + { + center[0]+=p[0]; + center[1]+=p[1]; + center[2]+=p[2]; + p += 3; + } + REAL recip = 1.0f / (REAL)vcount; + center[0]*=recip; + center[1]*=recip; + center[2]*=recip; + ret = true; + } + return ret; +} + +bool fm_computeCentroid(uint32_t vcount, // number of input data points + const REAL *points, // starting address of points array. + uint32_t triCount, + const uint32_t *indices, + REAL *center) + +{ + bool ret = false; + if (vcount) + { + center[0] = 0; + center[1] = 0; + center[2] = 0; + + REAL numerator[3] = { 0, 0, 0 }; + REAL denomintaor = 0; + + for (uint32_t i = 0; i < triCount; i++) + { + uint32_t i1 = indices[i * 3 + 0]; + uint32_t i2 = indices[i * 3 + 1]; + uint32_t i3 = indices[i * 3 + 2]; + + const REAL *p1 = &points[i1 * 3]; + const REAL *p2 = &points[i2 * 3]; + const REAL *p3 = &points[i3 * 3]; + + // Compute the sum of the three positions + REAL sum[3]; + sum[0] = p1[0] + p2[0] + p3[0]; + sum[1] = p1[1] + p2[1] + p3[1]; + sum[2] = p1[2] + p2[2] + p3[2]; + + // Compute the average of the three positions + sum[0] = sum[0] / 3; + sum[1] = sum[1] / 3; + sum[2] = sum[2] / 3; + + // Compute the area of this triangle + REAL area = fm_computeArea(p1, p2, p3); + + numerator[0]+= (sum[0] * area); + numerator[1]+= (sum[1] * area); + numerator[2]+= (sum[2] * area); + + denomintaor += area; + + } + REAL recip = 1 / denomintaor; + center[0] = numerator[0] * recip; + center[1] = numerator[1] * recip; + center[2] = numerator[2] * recip; + ret = true; + } + return ret; +} + + +#ifndef TEMPLATE_VEC3 +#define TEMPLATE_VEC3 +template <class Type> class Vec3 +{ +public: + Vec3(void) + { + + } + Vec3(Type _x,Type _y,Type _z) + { + x = _x; + y = _y; + z = _z; + } + Type x; + Type y; + Type z; +}; +#endif + +void fm_transformAABB(const REAL bmin[3],const REAL bmax[3],const REAL matrix[16],REAL tbmin[3],REAL tbmax[3]) +{ + Vec3<REAL> box[8]; + box[0] = Vec3< REAL >( bmin[0], bmin[1], bmin[2] ); + box[1] = Vec3< REAL >( bmax[0], bmin[1], bmin[2] ); + box[2] = Vec3< REAL >( bmax[0], bmax[1], bmin[2] ); + box[3] = Vec3< REAL >( bmin[0], bmax[1], bmin[2] ); + box[4] = Vec3< REAL >( bmin[0], bmin[1], bmax[2] ); + box[5] = Vec3< REAL >( bmax[0], bmin[1], bmax[2] ); + box[6] = Vec3< REAL >( bmax[0], bmax[1], bmax[2] ); + box[7] = Vec3< REAL >( bmin[0], bmax[1], bmax[2] ); + // transform all 8 corners of the box and then recompute a new AABB + for (unsigned int i=0; i<8; i++) + { + Vec3< REAL > &p = box[i]; + fm_transform(matrix,&p.x,&p.x); + if ( i == 0 ) + { + tbmin[0] = tbmax[0] = p.x; + tbmin[1] = tbmax[1] = p.y; + tbmin[2] = tbmax[2] = p.z; + } + else + { + if ( p.x < tbmin[0] ) tbmin[0] = p.x; + if ( p.y < tbmin[1] ) tbmin[1] = p.y; + if ( p.z < tbmin[2] ) tbmin[2] = p.z; + if ( p.x > tbmax[0] ) tbmax[0] = p.x; + if ( p.y > tbmax[1] ) tbmax[1] = p.y; + if ( p.z > tbmax[2] ) tbmax[2] = p.z; + } + } +} + +REAL fm_normalizeQuat(REAL n[4]) // normalize this quat +{ + REAL dx = n[0]*n[0]; + REAL dy = n[1]*n[1]; + REAL dz = n[2]*n[2]; + REAL dw = n[3]*n[3]; + + REAL dist = dx*dx+dy*dy+dz*dz+dw*dw; + + dist = (REAL)sqrt(dist); + + REAL recip = 1.0f / dist; + + n[0]*=recip; + n[1]*=recip; + n[2]*=recip; + n[3]*=recip; + + return dist; +} + + +}; // end of namespace diff --git a/thirdparty/vhacd/src/VHACD-ASYNC.cpp b/thirdparty/vhacd/src/VHACD-ASYNC.cpp new file mode 100644 index 0000000000..baf309d6a1 --- /dev/null +++ b/thirdparty/vhacd/src/VHACD-ASYNC.cpp @@ -0,0 +1,334 @@ +#include "../public/VHACD.h" +#include <stdlib.h> +#include <string.h> +#include <stdarg.h> +#include <thread> +#include <atomic> +#include <mutex> +#include <string> +#include <float.h> + +#define ENABLE_ASYNC 1 + +#define HACD_ALLOC(x) malloc(x) +#define HACD_FREE(x) free(x) +#define HACD_ASSERT(x) assert(x) + +namespace VHACD +{ + +class MyHACD_API : public VHACD::IVHACD, public VHACD::IVHACD::IUserCallback, VHACD::IVHACD::IUserLogger +{ +public: + MyHACD_API(void) + { + mVHACD = VHACD::CreateVHACD(); + } + + virtual ~MyHACD_API(void) + { + releaseHACD(); + Cancel(); + mVHACD->Release(); + } + + + virtual bool Compute(const double* const _points, + const uint32_t countPoints, + const uint32_t* const _triangles, + const uint32_t countTriangles, + const Parameters& _desc) final + { +#if ENABLE_ASYNC + Cancel(); // if we previously had a solution running; cancel it. + releaseHACD(); + + // We need to copy the input vertices and triangles into our own buffers so we can operate + // on them safely from the background thread. + mVertices = (double *)HACD_ALLOC(sizeof(double)*countPoints * 3); + mIndices = (uint32_t *)HACD_ALLOC(sizeof(uint32_t)*countTriangles * 3); + memcpy(mVertices, _points, sizeof(double)*countPoints * 3); + memcpy(mIndices, _triangles, sizeof(uint32_t)*countTriangles * 3); + mRunning = true; + mThread = new std::thread([this, countPoints, countTriangles, _desc]() + { + ComputeNow(mVertices, countPoints, mIndices, countTriangles, _desc); + mRunning = false; + }); +#else + releaseHACD(); + ComputeNow(_points, countPoints, _triangles, countTriangles, _desc); +#endif + return true; + } + + bool ComputeNow(const double* const points, + const uint32_t countPoints, + const uint32_t* const triangles, + const uint32_t countTriangles, + const Parameters& _desc) + { + uint32_t ret = 0; + + mHullCount = 0; + mCallback = _desc.m_callback; + mLogger = _desc.m_logger; + + IVHACD::Parameters desc = _desc; + // Set our intercepting callback interfaces if non-null + desc.m_callback = desc.m_callback ? this : nullptr; + desc.m_logger = desc.m_logger ? this : nullptr; + + if ( countPoints ) + { + bool ok = mVHACD->Compute(points, countPoints, triangles, countTriangles, desc); + if (ok) + { + ret = mVHACD->GetNConvexHulls(); + mHulls = new IVHACD::ConvexHull[ret]; + for (uint32_t i = 0; i < ret; i++) + { + VHACD::IVHACD::ConvexHull vhull; + mVHACD->GetConvexHull(i, vhull); + VHACD::IVHACD::ConvexHull h; + h.m_nPoints = vhull.m_nPoints; + h.m_points = (double *)HACD_ALLOC(sizeof(double) * 3 * h.m_nPoints); + memcpy(h.m_points, vhull.m_points, sizeof(double) * 3 * h.m_nPoints); + h.m_nTriangles = vhull.m_nTriangles; + h.m_triangles = (uint32_t *)HACD_ALLOC(sizeof(uint32_t) * 3 * h.m_nTriangles); + memcpy(h.m_triangles, vhull.m_triangles, sizeof(uint32_t) * 3 * h.m_nTriangles); + h.m_volume = vhull.m_volume; + h.m_center[0] = vhull.m_center[0]; + h.m_center[1] = vhull.m_center[1]; + h.m_center[2] = vhull.m_center[2]; + mHulls[i] = h; + if (mCancel) + { + ret = 0; + break; + } + } + } + } + + mHullCount = ret; + return ret ? true : false; + } + + void releaseHull(VHACD::IVHACD::ConvexHull &h) + { + HACD_FREE((void *)h.m_triangles); + HACD_FREE((void *)h.m_points); + h.m_triangles = nullptr; + h.m_points = nullptr; + } + + virtual void GetConvexHull(const uint32_t index, VHACD::IVHACD::ConvexHull& ch) const final + { + if ( index < mHullCount ) + { + ch = mHulls[index]; + } + } + + void releaseHACD(void) // release memory associated with the last HACD request + { + for (uint32_t i=0; i<mHullCount; i++) + { + releaseHull(mHulls[i]); + } + delete[]mHulls; + mHulls = nullptr; + mHullCount = 0; + HACD_FREE(mVertices); + mVertices = nullptr; + HACD_FREE(mIndices); + mIndices = nullptr; + } + + + virtual void release(void) // release the HACD_API interface + { + delete this; + } + + virtual uint32_t getHullCount(void) + { + return mHullCount; + } + + virtual void Cancel() final + { + if (mRunning) + { + mVHACD->Cancel(); // Set the cancel signal to the base VHACD + } + if (mThread) + { + mThread->join(); // Wait for the thread to fully exit before we delete the instance + delete mThread; + mThread = nullptr; + Log("Convex Decomposition thread canceled\n"); + } + mCancel = false; // clear the cancel semaphore + } + + virtual bool Compute(const float* const points, + const uint32_t countPoints, + const uint32_t* const triangles, + const uint32_t countTriangles, + const Parameters& params) final + { + + double *vertices = (double *)HACD_ALLOC(sizeof(double)*countPoints * 3); + const float *source = points; + double *dest = vertices; + for (uint32_t i = 0; i < countPoints; i++) + { + dest[0] = source[0]; + dest[1] = source[1]; + dest[2] = source[2]; + dest += 3; + source += 3; + } + + bool ret = Compute(vertices, countPoints, triangles, countTriangles, params); + HACD_FREE(vertices); + return ret; + } + + virtual uint32_t GetNConvexHulls() const final + { + processPendingMessages(); + return mHullCount; + } + + virtual void Clean(void) final // release internally allocated memory + { + Cancel(); + releaseHACD(); + mVHACD->Clean(); + } + + virtual void Release(void) final // release IVHACD + { + delete this; + } + + virtual bool OCLInit(void* const oclDevice, + IVHACD::IUserLogger* const logger = 0) final + { + return mVHACD->OCLInit(oclDevice, logger); + } + + virtual bool OCLRelease(IVHACD::IUserLogger* const logger = 0) final + { + return mVHACD->OCLRelease(logger); + } + + virtual void Update(const double overallProgress, + const double stageProgress, + const double operationProgress, + const char* const stage, + const char* const operation) final + { + mMessageMutex.lock(); + mHaveUpdateMessage = true; + mOverallProgress = overallProgress; + mStageProgress = stageProgress; + mOperationProgress = operationProgress; + mStage = std::string(stage); + mOperation = std::string(operation); + mMessageMutex.unlock(); + } + + virtual void Log(const char* const msg) final + { + mMessageMutex.lock(); + mHaveLogMessage = true; + mMessage = std::string(msg); + mMessageMutex.unlock(); + } + + virtual bool IsReady(void) const final + { + processPendingMessages(); + return !mRunning; + } + + // As a convenience for the calling application we only send it update and log messages from it's own main + // thread. This reduces the complexity burden on the caller by making sure it only has to deal with log + // messages in it's main application thread. + void processPendingMessages(void) const + { + // If we have a new update message and the user has specified a callback we send the message and clear the semaphore + if (mHaveUpdateMessage && mCallback) + { + mMessageMutex.lock(); + mCallback->Update(mOverallProgress, mStageProgress, mOperationProgress, mStage.c_str(), mOperation.c_str()); + mHaveUpdateMessage = false; + mMessageMutex.unlock(); + } + // If we have a new log message and the user has specified a callback we send the message and clear the semaphore + if (mHaveLogMessage && mLogger) + { + mMessageMutex.lock(); + mLogger->Log(mMessage.c_str()); + mHaveLogMessage = false; + mMessageMutex.unlock(); + } + } + + // Will compute the center of mass of the convex hull decomposition results and return it + // in 'centerOfMass'. Returns false if the center of mass could not be computed. + virtual bool ComputeCenterOfMass(double centerOfMass[3]) const + { + bool ret = false; + + centerOfMass[0] = 0; + centerOfMass[1] = 0; + centerOfMass[2] = 0; + + if (mVHACD && IsReady() ) + { + ret = mVHACD->ComputeCenterOfMass(centerOfMass); + } + return ret; + } + +private: + double *mVertices{ nullptr }; + uint32_t *mIndices{ nullptr }; + std::atomic< uint32_t> mHullCount{ 0 }; + VHACD::IVHACD::ConvexHull *mHulls{ nullptr }; + VHACD::IVHACD::IUserCallback *mCallback{ nullptr }; + VHACD::IVHACD::IUserLogger *mLogger{ nullptr }; + VHACD::IVHACD *mVHACD{ nullptr }; + std::thread *mThread{ nullptr }; + std::atomic< bool > mRunning{ false }; + std::atomic<bool> mCancel{ false }; + + // Thread safe caching mechanism for messages and update status. + // This is so that caller always gets messages in his own thread + // Member variables are marked as 'mutable' since the message dispatch function + // is called from const query methods. + mutable std::mutex mMessageMutex; + mutable std::atomic< bool > mHaveUpdateMessage{ false }; + mutable std::atomic< bool > mHaveLogMessage{ false }; + mutable double mOverallProgress{ 0 }; + mutable double mStageProgress{ 0 }; + mutable double mOperationProgress{ 0 }; + mutable std::string mStage; + mutable std::string mOperation; + mutable std::string mMessage; +}; + +IVHACD* CreateVHACD_ASYNC(void) +{ + MyHACD_API *m = new MyHACD_API; + return static_cast<IVHACD *>(m); +} + + +}; // end of VHACD namespace + diff --git a/thirdparty/vhacd/src/VHACD.cpp b/thirdparty/vhacd/src/VHACD.cpp new file mode 100644 index 0000000000..54cc6e3761 --- /dev/null +++ b/thirdparty/vhacd/src/VHACD.cpp @@ -0,0 +1,1589 @@ +/* Copyright (c) 2011 Khaled Mamou (kmamou at gmail dot com) + All rights reserved. + + + Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + + 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + + 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. + + 3. The names of the contributors may not be used to endorse or promote products derived from this software without specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef _CRT_SECURE_NO_WARNINGS +#define _CRT_SECURE_NO_WARNINGS +#endif + +#include <algorithm> +#include <fstream> +#include <iomanip> +#include <limits> +#include <sstream> +#if _OPENMP +#include <omp.h> +#endif // _OPENMP + +#include "../public/VHACD.h" +#include "btConvexHullComputer.h" +#include "vhacdICHull.h" +#include "vhacdMesh.h" +#include "vhacdSArray.h" +#include "vhacdTimer.h" +#include "vhacdVHACD.h" +#include "vhacdVector.h" +#include "vhacdVolume.h" +#include "FloatMath.h" + +#define MAX(a, b) (((a) > (b)) ? (a) : (b)) +#define MIN(a, b) (((a) < (b)) ? (a) : (b)) +#define ABS(a) (((a) < 0) ? -(a) : (a)) +#define ZSGN(a) (((a) < 0) ? -1 : (a) > 0 ? 1 : 0) +#define MAX_DOUBLE (1.79769e+308) + +#ifdef _MSC_VER +#pragma warning(disable:4267 4100 4244 4456) +#endif + +#ifdef USE_SSE +#include <immintrin.h> + +const int32_t SIMD_WIDTH = 4; +inline int32_t FindMinimumElement(const float* const d, float* const _, const int32_t n) +{ + // Min within vectors + __m128 min_i = _mm_set1_ps(-1.0f); + __m128 min_v = _mm_set1_ps(std::numeric_limits<float>::max()); + for (int32_t i = 0; i <= n - SIMD_WIDTH; i += SIMD_WIDTH) { + const __m128 data = _mm_load_ps(&d[i]); + const __m128 pred = _mm_cmplt_ps(data, min_v); + + min_i = _mm_blendv_ps(min_i, _mm_set1_ps(i), pred); + min_v = _mm_min_ps(data, min_v); + } + + /* Min within vector */ + const __m128 min1 = _mm_shuffle_ps(min_v, min_v, _MM_SHUFFLE(1, 0, 3, 2)); + const __m128 min2 = _mm_min_ps(min_v, min1); + const __m128 min3 = _mm_shuffle_ps(min2, min2, _MM_SHUFFLE(0, 1, 0, 1)); + const __m128 min4 = _mm_min_ps(min2, min3); + float min_d = _mm_cvtss_f32(min4); + + // Min index + const int32_t min_idx = __builtin_ctz(_mm_movemask_ps(_mm_cmpeq_ps(min_v, min4))); + int32_t ret = min_i[min_idx] + min_idx; + + // Trailing elements + for (int32_t i = (n & ~(SIMD_WIDTH - 1)); i < n; ++i) { + if (d[i] < min_d) { + min_d = d[i]; + ret = i; + } + } + + *m = min_d; + return ret; +} + +inline int32_t FindMinimumElement(const float* const d, float* const m, const int32_t begin, const int32_t end) +{ + // Leading elements + int32_t min_i = -1; + float min_d = std::numeric_limits<float>::max(); + const int32_t aligned = (begin & ~(SIMD_WIDTH - 1)) + ((begin & (SIMD_WIDTH - 1)) ? SIMD_WIDTH : 0); + for (int32_t i = begin; i < std::min(end, aligned); ++i) { + if (d[i] < min_d) { + min_d = d[i]; + min_i = i; + } + } + + // Middle and trailing elements + float r_m = std::numeric_limits<float>::max(); + const int32_t n = end - aligned; + const int32_t r_i = (n > 0) ? FindMinimumElement(&d[aligned], &r_m, n) : 0; + + // Pick the lowest + if (r_m < min_d) { + *m = r_m; + return r_i + aligned; + } + else { + *m = min_d; + return min_i; + } +} +#else +inline int32_t FindMinimumElement(const float* const d, float* const m, const int32_t begin, const int32_t end) +{ + int32_t idx = -1; + float min = (std::numeric_limits<float>::max)(); + for (size_t i = begin; i < size_t(end); ++i) { + if (d[i] < min) { + idx = i; + min = d[i]; + } + } + + *m = min; + return idx; +} +#endif + +//#define OCL_SOURCE_FROM_FILE +#ifndef OCL_SOURCE_FROM_FILE +const char* oclProgramSource = "\ +__kernel void ComputePartialVolumes(__global short4 * voxels, \ + const int numVoxels, \ + const float4 plane, \ + const float4 minBB, \ + const float4 scale, \ + __local uint4 * localPartialVolumes, \ + __global uint4 * partialVolumes) \ +{ \ + int localId = get_local_id(0); \ + int groupSize = get_local_size(0); \ + int i0 = get_global_id(0) << 2; \ + float4 voxel; \ + uint4 v; \ + voxel = convert_float4(voxels[i0]); \ + v.s0 = (dot(plane, mad(scale, voxel, minBB)) >= 0.0f) * (i0 < numVoxels);\ + voxel = convert_float4(voxels[i0 + 1]); \ + v.s1 = (dot(plane, mad(scale, voxel, minBB)) >= 0.0f) * (i0 + 1 < numVoxels);\ + voxel = convert_float4(voxels[i0 + 2]); \ + v.s2 = (dot(plane, mad(scale, voxel, minBB)) >= 0.0f) * (i0 + 2 < numVoxels);\ + voxel = convert_float4(voxels[i0 + 3]); \ + v.s3 = (dot(plane, mad(scale, voxel, minBB)) >= 0.0f) * (i0 + 3 < numVoxels);\ + localPartialVolumes[localId] = v; \ + barrier(CLK_LOCAL_MEM_FENCE); \ + for (int i = groupSize >> 1; i > 0; i >>= 1) \ + { \ + if (localId < i) \ + { \ + localPartialVolumes[localId] += localPartialVolumes[localId + i]; \ + } \ + barrier(CLK_LOCAL_MEM_FENCE); \ + } \ + if (localId == 0) \ + { \ + partialVolumes[get_group_id(0)] = localPartialVolumes[0]; \ + } \ +} \ +__kernel void ComputePartialSums(__global uint4 * data, \ + const int dataSize, \ + __local uint4 * partialSums) \ +{ \ + int globalId = get_global_id(0); \ + int localId = get_local_id(0); \ + int groupSize = get_local_size(0); \ + int i; \ + if (globalId < dataSize) \ + { \ + partialSums[localId] = data[globalId]; \ + } \ + else \ + { \ + partialSums[localId] = (0, 0, 0, 0); \ + } \ + barrier(CLK_LOCAL_MEM_FENCE); \ + for (i = groupSize >> 1; i > 0; i >>= 1) \ + { \ + if (localId < i) \ + { \ + partialSums[localId] += partialSums[localId + i]; \ + } \ + barrier(CLK_LOCAL_MEM_FENCE); \ + } \ + if (localId == 0) \ + { \ + data[get_group_id(0)] = partialSums[0]; \ + } \ +}"; +#endif //OCL_SOURCE_FROM_FILE + +namespace VHACD { +IVHACD* CreateVHACD(void) +{ + return new VHACD(); +} +bool VHACD::OCLInit(void* const oclDevice, IUserLogger* const logger) +{ +#ifdef CL_VERSION_1_1 + m_oclDevice = (cl_device_id*)oclDevice; + cl_int error; + m_oclContext = clCreateContext(NULL, 1, m_oclDevice, NULL, NULL, &error); + if (error != CL_SUCCESS) { + if (logger) { + logger->Log("Couldn't create context\n"); + } + return false; + } + +#ifdef OCL_SOURCE_FROM_FILE + std::string cl_files = OPENCL_CL_FILES; +// read kernal from file +#ifdef _WIN32 + std::replace(cl_files.begin(), cl_files.end(), '/', '\\'); +#endif // _WIN32 + + FILE* program_handle = fopen(cl_files.c_str(), "rb"); + fseek(program_handle, 0, SEEK_END); + size_t program_size = ftell(program_handle); + rewind(program_handle); + char* program_buffer = new char[program_size + 1]; + program_buffer[program_size] = '\0'; + fread(program_buffer, sizeof(char), program_size, program_handle); + fclose(program_handle); + // create program + m_oclProgram = clCreateProgramWithSource(m_oclContext, 1, (const char**)&program_buffer, &program_size, &error); + delete[] program_buffer; +#else + size_t program_size = strlen(oclProgramSource); + m_oclProgram = clCreateProgramWithSource(m_oclContext, 1, (const char**)&oclProgramSource, &program_size, &error); +#endif + if (error != CL_SUCCESS) { + if (logger) { + logger->Log("Couldn't create program\n"); + } + return false; + } + + /* Build program */ + error = clBuildProgram(m_oclProgram, 1, m_oclDevice, "-cl-denorms-are-zero", NULL, NULL); + if (error != CL_SUCCESS) { + size_t log_size; + /* Find Size of log and print to std output */ + clGetProgramBuildInfo(m_oclProgram, *m_oclDevice, CL_PROGRAM_BUILD_LOG, 0, NULL, &log_size); + char* program_log = new char[log_size + 2]; + program_log[log_size] = '\n'; + program_log[log_size + 1] = '\0'; + clGetProgramBuildInfo(m_oclProgram, *m_oclDevice, CL_PROGRAM_BUILD_LOG, log_size + 1, program_log, NULL); + if (logger) { + logger->Log("Couldn't build program\n"); + logger->Log(program_log); + } + delete[] program_log; + return false; + } + + delete[] m_oclQueue; + delete[] m_oclKernelComputePartialVolumes; + delete[] m_oclKernelComputeSum; + m_oclQueue = new cl_command_queue[m_ompNumProcessors]; + m_oclKernelComputePartialVolumes = new cl_kernel[m_ompNumProcessors]; + m_oclKernelComputeSum = new cl_kernel[m_ompNumProcessors]; + + const char nameKernelComputePartialVolumes[] = "ComputePartialVolumes"; + const char nameKernelComputeSum[] = "ComputePartialSums"; + for (int32_t k = 0; k < m_ompNumProcessors; ++k) { + m_oclKernelComputePartialVolumes[k] = clCreateKernel(m_oclProgram, nameKernelComputePartialVolumes, &error); + if (error != CL_SUCCESS) { + if (logger) { + logger->Log("Couldn't create kernel\n"); + } + return false; + } + m_oclKernelComputeSum[k] = clCreateKernel(m_oclProgram, nameKernelComputeSum, &error); + if (error != CL_SUCCESS) { + if (logger) { + logger->Log("Couldn't create kernel\n"); + } + return false; + } + } + + error = clGetKernelWorkGroupInfo(m_oclKernelComputePartialVolumes[0], + *m_oclDevice, + CL_KERNEL_WORK_GROUP_SIZE, + sizeof(size_t), + &m_oclWorkGroupSize, + NULL); + size_t workGroupSize = 0; + error = clGetKernelWorkGroupInfo(m_oclKernelComputeSum[0], + *m_oclDevice, + CL_KERNEL_WORK_GROUP_SIZE, + sizeof(size_t), + &workGroupSize, + NULL); + if (error != CL_SUCCESS) { + if (logger) { + logger->Log("Couldn't query work group info\n"); + } + return false; + } + + if (workGroupSize < m_oclWorkGroupSize) { + m_oclWorkGroupSize = workGroupSize; + } + + for (int32_t k = 0; k < m_ompNumProcessors; ++k) { + m_oclQueue[k] = clCreateCommandQueue(m_oclContext, *m_oclDevice, 0 /*CL_QUEUE_PROFILING_ENABLE*/, &error); + if (error != CL_SUCCESS) { + if (logger) { + logger->Log("Couldn't create queue\n"); + } + return false; + } + } + return true; +#else //CL_VERSION_1_1 + return false; +#endif //CL_VERSION_1_1 +} +bool VHACD::OCLRelease(IUserLogger* const logger) +{ +#ifdef CL_VERSION_1_1 + cl_int error; + if (m_oclKernelComputePartialVolumes) { + for (int32_t k = 0; k < m_ompNumProcessors; ++k) { + error = clReleaseKernel(m_oclKernelComputePartialVolumes[k]); + if (error != CL_SUCCESS) { + if (logger) { + logger->Log("Couldn't release kernal\n"); + } + return false; + } + } + delete[] m_oclKernelComputePartialVolumes; + } + if (m_oclKernelComputeSum) { + for (int32_t k = 0; k < m_ompNumProcessors; ++k) { + error = clReleaseKernel(m_oclKernelComputeSum[k]); + if (error != CL_SUCCESS) { + if (logger) { + logger->Log("Couldn't release kernal\n"); + } + return false; + } + } + delete[] m_oclKernelComputeSum; + } + if (m_oclQueue) { + for (int32_t k = 0; k < m_ompNumProcessors; ++k) { + error = clReleaseCommandQueue(m_oclQueue[k]); + if (error != CL_SUCCESS) { + if (logger) { + logger->Log("Couldn't release queue\n"); + } + return false; + } + } + delete[] m_oclQueue; + } + error = clReleaseProgram(m_oclProgram); + if (error != CL_SUCCESS) { + if (logger) { + logger->Log("Couldn't release program\n"); + } + return false; + } + error = clReleaseContext(m_oclContext); + if (error != CL_SUCCESS) { + if (logger) { + logger->Log("Couldn't release context\n"); + } + return false; + } + + return true; +#else //CL_VERSION_1_1 + return false; +#endif //CL_VERSION_1_1 +} +void VHACD::ComputePrimitiveSet(const Parameters& params) +{ + if (GetCancel()) { + return; + } + m_timer.Tic(); + + m_stage = "Compute primitive set"; + m_operation = "Convert volume to pset"; + + std::ostringstream msg; + if (params.m_logger) { + msg << "+ " << m_stage << std::endl; + params.m_logger->Log(msg.str().c_str()); + } + + Update(0.0, 0.0, params); + if (params.m_mode == 0) { + VoxelSet* vset = new VoxelSet; + m_volume->Convert(*vset); + m_pset = vset; + } + else { + TetrahedronSet* tset = new TetrahedronSet; + m_volume->Convert(*tset); + m_pset = tset; + } + + delete m_volume; + m_volume = 0; + + if (params.m_logger) { + msg.str(""); + msg << "\t # primitives " << m_pset->GetNPrimitives() << std::endl; + msg << "\t # inside surface " << m_pset->GetNPrimitivesInsideSurf() << std::endl; + msg << "\t # on surface " << m_pset->GetNPrimitivesOnSurf() << std::endl; + params.m_logger->Log(msg.str().c_str()); + } + + m_overallProgress = 15.0; + Update(100.0, 100.0, params); + m_timer.Toc(); + if (params.m_logger) { + msg.str(""); + msg << "\t time " << m_timer.GetElapsedTime() / 1000.0 << "s" << std::endl; + params.m_logger->Log(msg.str().c_str()); + } +} +bool VHACD::Compute(const double* const points, const uint32_t nPoints, + const uint32_t* const triangles,const uint32_t nTriangles, const Parameters& params) +{ + return ComputeACD(points, nPoints, triangles, nTriangles, params); +} +bool VHACD::Compute(const float* const points,const uint32_t nPoints, + const uint32_t* const triangles,const uint32_t nTriangles, const Parameters& params) +{ + return ComputeACD(points, nPoints, triangles, nTriangles, params); +} +double ComputePreferredCuttingDirection(const PrimitiveSet* const tset, Vec3<double>& dir) +{ + double ex = tset->GetEigenValue(AXIS_X); + double ey = tset->GetEigenValue(AXIS_Y); + double ez = tset->GetEigenValue(AXIS_Z); + double vx = (ey - ez) * (ey - ez); + double vy = (ex - ez) * (ex - ez); + double vz = (ex - ey) * (ex - ey); + if (vx < vy && vx < vz) { + double e = ey * ey + ez * ez; + dir[0] = 1.0; + dir[1] = 0.0; + dir[2] = 0.0; + return (e == 0.0) ? 0.0 : 1.0 - vx / e; + } + else if (vy < vx && vy < vz) { + double e = ex * ex + ez * ez; + dir[0] = 0.0; + dir[1] = 1.0; + dir[2] = 0.0; + return (e == 0.0) ? 0.0 : 1.0 - vy / e; + } + else { + double e = ex * ex + ey * ey; + dir[0] = 0.0; + dir[1] = 0.0; + dir[2] = 1.0; + return (e == 0.0) ? 0.0 : 1.0 - vz / e; + } +} +void ComputeAxesAlignedClippingPlanes(const VoxelSet& vset, const short downsampling, SArray<Plane>& planes) +{ + const Vec3<short> minV = vset.GetMinBBVoxels(); + const Vec3<short> maxV = vset.GetMaxBBVoxels(); + Vec3<double> pt; + Plane plane; + const short i0 = minV[0]; + const short i1 = maxV[0]; + plane.m_a = 1.0; + plane.m_b = 0.0; + plane.m_c = 0.0; + plane.m_axis = AXIS_X; + for (short i = i0; i <= i1; i += downsampling) { + pt = vset.GetPoint(Vec3<double>(i + 0.5, 0.0, 0.0)); + plane.m_d = -pt[0]; + plane.m_index = i; + planes.PushBack(plane); + } + const short j0 = minV[1]; + const short j1 = maxV[1]; + plane.m_a = 0.0; + plane.m_b = 1.0; + plane.m_c = 0.0; + plane.m_axis = AXIS_Y; + for (short j = j0; j <= j1; j += downsampling) { + pt = vset.GetPoint(Vec3<double>(0.0, j + 0.5, 0.0)); + plane.m_d = -pt[1]; + plane.m_index = j; + planes.PushBack(plane); + } + const short k0 = minV[2]; + const short k1 = maxV[2]; + plane.m_a = 0.0; + plane.m_b = 0.0; + plane.m_c = 1.0; + plane.m_axis = AXIS_Z; + for (short k = k0; k <= k1; k += downsampling) { + pt = vset.GetPoint(Vec3<double>(0.0, 0.0, k + 0.5)); + plane.m_d = -pt[2]; + plane.m_index = k; + planes.PushBack(plane); + } +} +void ComputeAxesAlignedClippingPlanes(const TetrahedronSet& tset, const short downsampling, SArray<Plane>& planes) +{ + const Vec3<double> minV = tset.GetMinBB(); + const Vec3<double> maxV = tset.GetMaxBB(); + const double scale = tset.GetSacle(); + const short i0 = 0; + const short j0 = 0; + const short k0 = 0; + const short i1 = static_cast<short>((maxV[0] - minV[0]) / scale + 0.5); + const short j1 = static_cast<short>((maxV[1] - minV[1]) / scale + 0.5); + const short k1 = static_cast<short>((maxV[2] - minV[2]) / scale + 0.5); + + Plane plane; + plane.m_a = 1.0; + plane.m_b = 0.0; + plane.m_c = 0.0; + plane.m_axis = AXIS_X; + for (short i = i0; i <= i1; i += downsampling) { + double x = minV[0] + scale * i; + plane.m_d = -x; + plane.m_index = i; + planes.PushBack(plane); + } + plane.m_a = 0.0; + plane.m_b = 1.0; + plane.m_c = 0.0; + plane.m_axis = AXIS_Y; + for (short j = j0; j <= j1; j += downsampling) { + double y = minV[1] + scale * j; + plane.m_d = -y; + plane.m_index = j; + planes.PushBack(plane); + } + plane.m_a = 0.0; + plane.m_b = 0.0; + plane.m_c = 1.0; + plane.m_axis = AXIS_Z; + for (short k = k0; k <= k1; k += downsampling) { + double z = minV[2] + scale * k; + plane.m_d = -z; + plane.m_index = k; + planes.PushBack(plane); + } +} +void RefineAxesAlignedClippingPlanes(const VoxelSet& vset, const Plane& bestPlane, const short downsampling, + SArray<Plane>& planes) +{ + const Vec3<short> minV = vset.GetMinBBVoxels(); + const Vec3<short> maxV = vset.GetMaxBBVoxels(); + Vec3<double> pt; + Plane plane; + + if (bestPlane.m_axis == AXIS_X) { + const short i0 = MAX(minV[0], bestPlane.m_index - downsampling); + const short i1 = MIN(maxV[0], bestPlane.m_index + downsampling); + plane.m_a = 1.0; + plane.m_b = 0.0; + plane.m_c = 0.0; + plane.m_axis = AXIS_X; + for (short i = i0; i <= i1; ++i) { + pt = vset.GetPoint(Vec3<double>(i + 0.5, 0.0, 0.0)); + plane.m_d = -pt[0]; + plane.m_index = i; + planes.PushBack(plane); + } + } + else if (bestPlane.m_axis == AXIS_Y) { + const short j0 = MAX(minV[1], bestPlane.m_index - downsampling); + const short j1 = MIN(maxV[1], bestPlane.m_index + downsampling); + plane.m_a = 0.0; + plane.m_b = 1.0; + plane.m_c = 0.0; + plane.m_axis = AXIS_Y; + for (short j = j0; j <= j1; ++j) { + pt = vset.GetPoint(Vec3<double>(0.0, j + 0.5, 0.0)); + plane.m_d = -pt[1]; + plane.m_index = j; + planes.PushBack(plane); + } + } + else { + const short k0 = MAX(minV[2], bestPlane.m_index - downsampling); + const short k1 = MIN(maxV[2], bestPlane.m_index + downsampling); + plane.m_a = 0.0; + plane.m_b = 0.0; + plane.m_c = 1.0; + plane.m_axis = AXIS_Z; + for (short k = k0; k <= k1; ++k) { + pt = vset.GetPoint(Vec3<double>(0.0, 0.0, k + 0.5)); + plane.m_d = -pt[2]; + plane.m_index = k; + planes.PushBack(plane); + } + } +} +void RefineAxesAlignedClippingPlanes(const TetrahedronSet& tset, const Plane& bestPlane, const short downsampling, + SArray<Plane>& planes) +{ + const Vec3<double> minV = tset.GetMinBB(); + const Vec3<double> maxV = tset.GetMaxBB(); + const double scale = tset.GetSacle(); + Plane plane; + + if (bestPlane.m_axis == AXIS_X) { + const short i0 = MAX(0, bestPlane.m_index - downsampling); + const short i1 = static_cast<short>(MIN((maxV[0] - minV[0]) / scale + 0.5, bestPlane.m_index + downsampling)); + plane.m_a = 1.0; + plane.m_b = 0.0; + plane.m_c = 0.0; + plane.m_axis = AXIS_X; + for (short i = i0; i <= i1; ++i) { + double x = minV[0] + scale * i; + plane.m_d = -x; + plane.m_index = i; + planes.PushBack(plane); + } + } + else if (bestPlane.m_axis == AXIS_Y) { + const short j0 = MAX(0, bestPlane.m_index - downsampling); + const short j1 = static_cast<short>(MIN((maxV[1] - minV[1]) / scale + 0.5, bestPlane.m_index + downsampling)); + plane.m_a = 0.0; + plane.m_b = 1.0; + plane.m_c = 0.0; + plane.m_axis = AXIS_Y; + for (short j = j0; j <= j1; ++j) { + double y = minV[1] + scale * j; + plane.m_d = -y; + plane.m_index = j; + planes.PushBack(plane); + } + } + else { + const short k0 = MAX(0, bestPlane.m_index - downsampling); + const short k1 = static_cast<short>(MIN((maxV[2] - minV[2]) / scale + 0.5, bestPlane.m_index + downsampling)); + plane.m_a = 0.0; + plane.m_b = 0.0; + plane.m_c = 1.0; + plane.m_axis = AXIS_Z; + for (short k = k0; k <= k1; ++k) { + double z = minV[2] + scale * k; + plane.m_d = -z; + plane.m_index = k; + planes.PushBack(plane); + } + } +} +inline double ComputeLocalConcavity(const double volume, const double volumeCH) +{ + return fabs(volumeCH - volume) / volumeCH; +} +inline double ComputeConcavity(const double volume, const double volumeCH, const double volume0) +{ + return fabs(volumeCH - volume) / volume0; +} + +//#define DEBUG_TEMP +void VHACD::ComputeBestClippingPlane(const PrimitiveSet* inputPSet, const double volume, const SArray<Plane>& planes, + const Vec3<double>& preferredCuttingDirection, const double w, const double alpha, const double beta, + const int32_t convexhullDownsampling, const double progress0, const double progress1, Plane& bestPlane, + double& minConcavity, const Parameters& params) +{ + if (GetCancel()) { + return; + } + char msg[256]; + size_t nPrimitives = inputPSet->GetNPrimitives(); + bool oclAcceleration = (nPrimitives > OCL_MIN_NUM_PRIMITIVES && params.m_oclAcceleration && params.m_mode == 0) ? true : false; + int32_t iBest = -1; + int32_t nPlanes = static_cast<int32_t>(planes.Size()); + bool cancel = false; + int32_t done = 0; + double minTotal = MAX_DOUBLE; + double minBalance = MAX_DOUBLE; + double minSymmetry = MAX_DOUBLE; + minConcavity = MAX_DOUBLE; + + SArray<Vec3<double> >* chPts = new SArray<Vec3<double> >[2 * m_ompNumProcessors]; + Mesh* chs = new Mesh[2 * m_ompNumProcessors]; + PrimitiveSet* onSurfacePSet = inputPSet->Create(); + inputPSet->SelectOnSurface(onSurfacePSet); + + PrimitiveSet** psets = 0; + if (!params.m_convexhullApproximation) { + psets = new PrimitiveSet*[2 * m_ompNumProcessors]; + for (int32_t i = 0; i < 2 * m_ompNumProcessors; ++i) { + psets[i] = inputPSet->Create(); + } + } + +#ifdef CL_VERSION_1_1 + // allocate OpenCL data structures + cl_mem voxels; + cl_mem* partialVolumes = 0; + size_t globalSize = 0; + size_t nWorkGroups = 0; + double unitVolume = 0.0; + if (oclAcceleration) { + VoxelSet* vset = (VoxelSet*)inputPSet; + const Vec3<double> minBB = vset->GetMinBB(); + const float fMinBB[4] = { (float)minBB[0], (float)minBB[1], (float)minBB[2], 1.0f }; + const float fSclae[4] = { (float)vset->GetScale(), (float)vset->GetScale(), (float)vset->GetScale(), 0.0f }; + const int32_t nVoxels = (int32_t)nPrimitives; + unitVolume = vset->GetUnitVolume(); + nWorkGroups = (nPrimitives + 4 * m_oclWorkGroupSize - 1) / (4 * m_oclWorkGroupSize); + globalSize = nWorkGroups * m_oclWorkGroupSize; + cl_int error; + voxels = clCreateBuffer(m_oclContext, + CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR, + sizeof(Voxel) * nPrimitives, + vset->GetVoxels(), + &error); + if (error != CL_SUCCESS) { + if (params.m_logger) { + params.m_logger->Log("Couldn't create buffer\n"); + } + SetCancel(true); + } + + partialVolumes = new cl_mem[m_ompNumProcessors]; + for (int32_t i = 0; i < m_ompNumProcessors; ++i) { + partialVolumes[i] = clCreateBuffer(m_oclContext, + CL_MEM_WRITE_ONLY, + sizeof(uint32_t) * 4 * nWorkGroups, + NULL, + &error); + if (error != CL_SUCCESS) { + if (params.m_logger) { + params.m_logger->Log("Couldn't create buffer\n"); + } + SetCancel(true); + break; + } + error = clSetKernelArg(m_oclKernelComputePartialVolumes[i], 0, sizeof(cl_mem), &voxels); + error |= clSetKernelArg(m_oclKernelComputePartialVolumes[i], 1, sizeof(uint32_t), &nVoxels); + error |= clSetKernelArg(m_oclKernelComputePartialVolumes[i], 3, sizeof(float) * 4, fMinBB); + error |= clSetKernelArg(m_oclKernelComputePartialVolumes[i], 4, sizeof(float) * 4, &fSclae); + error |= clSetKernelArg(m_oclKernelComputePartialVolumes[i], 5, sizeof(uint32_t) * 4 * m_oclWorkGroupSize, NULL); + error |= clSetKernelArg(m_oclKernelComputePartialVolumes[i], 6, sizeof(cl_mem), &(partialVolumes[i])); + error |= clSetKernelArg(m_oclKernelComputeSum[i], 0, sizeof(cl_mem), &(partialVolumes[i])); + error |= clSetKernelArg(m_oclKernelComputeSum[i], 2, sizeof(uint32_t) * 4 * m_oclWorkGroupSize, NULL); + if (error != CL_SUCCESS) { + if (params.m_logger) { + params.m_logger->Log("Couldn't kernel arguments \n"); + } + SetCancel(true); + } + } + } +#else // CL_VERSION_1_1 + oclAcceleration = false; +#endif // CL_VERSION_1_1 + +#ifdef DEBUG_TEMP + Timer timerComputeCost; + timerComputeCost.Tic(); +#endif // DEBUG_TEMP + +#if USE_THREAD == 1 && _OPENMP +#pragma omp parallel for +#endif + for (int32_t x = 0; x < nPlanes; ++x) { + int32_t threadID = 0; +#if USE_THREAD == 1 && _OPENMP + threadID = omp_get_thread_num(); +#pragma omp flush(cancel) +#endif + if (!cancel) { + //Update progress + if (GetCancel()) { + cancel = true; +#if USE_THREAD == 1 && _OPENMP +#pragma omp flush(cancel) +#endif + } + Plane plane = planes[x]; + + if (oclAcceleration) { +#ifdef CL_VERSION_1_1 + const float fPlane[4] = { (float)plane.m_a, (float)plane.m_b, (float)plane.m_c, (float)plane.m_d }; + cl_int error = clSetKernelArg(m_oclKernelComputePartialVolumes[threadID], 2, sizeof(float) * 4, fPlane); + if (error != CL_SUCCESS) { + if (params.m_logger) { + params.m_logger->Log("Couldn't kernel atguments \n"); + } + SetCancel(true); + } + + error = clEnqueueNDRangeKernel(m_oclQueue[threadID], m_oclKernelComputePartialVolumes[threadID], + 1, NULL, &globalSize, &m_oclWorkGroupSize, 0, NULL, NULL); + if (error != CL_SUCCESS) { + if (params.m_logger) { + params.m_logger->Log("Couldn't run kernel \n"); + } + SetCancel(true); + } + int32_t nValues = (int32_t)nWorkGroups; + while (nValues > 1) { + error = clSetKernelArg(m_oclKernelComputeSum[threadID], 1, sizeof(int32_t), &nValues); + if (error != CL_SUCCESS) { + if (params.m_logger) { + params.m_logger->Log("Couldn't kernel atguments \n"); + } + SetCancel(true); + } + size_t nWorkGroups = (nValues + m_oclWorkGroupSize - 1) / m_oclWorkGroupSize; + size_t globalSize = nWorkGroups * m_oclWorkGroupSize; + error = clEnqueueNDRangeKernel(m_oclQueue[threadID], m_oclKernelComputeSum[threadID], + 1, NULL, &globalSize, &m_oclWorkGroupSize, 0, NULL, NULL); + if (error != CL_SUCCESS) { + if (params.m_logger) { + params.m_logger->Log("Couldn't run kernel \n"); + } + SetCancel(true); + } + nValues = (int32_t)nWorkGroups; + } +#endif // CL_VERSION_1_1 + } + + Mesh& leftCH = chs[threadID]; + Mesh& rightCH = chs[threadID + m_ompNumProcessors]; + rightCH.ResizePoints(0); + leftCH.ResizePoints(0); + rightCH.ResizeTriangles(0); + leftCH.ResizeTriangles(0); + +// compute convex-hulls +#ifdef TEST_APPROX_CH + double volumeLeftCH1; + double volumeRightCH1; +#endif //TEST_APPROX_CH + if (params.m_convexhullApproximation) { + SArray<Vec3<double> >& leftCHPts = chPts[threadID]; + SArray<Vec3<double> >& rightCHPts = chPts[threadID + m_ompNumProcessors]; + rightCHPts.Resize(0); + leftCHPts.Resize(0); + onSurfacePSet->Intersect(plane, &rightCHPts, &leftCHPts, convexhullDownsampling * 32); + inputPSet->GetConvexHull().Clip(plane, rightCHPts, leftCHPts); + rightCH.ComputeConvexHull((double*)rightCHPts.Data(), rightCHPts.Size()); + leftCH.ComputeConvexHull((double*)leftCHPts.Data(), leftCHPts.Size()); +#ifdef TEST_APPROX_CH + Mesh leftCH1; + Mesh rightCH1; + VoxelSet right; + VoxelSet left; + onSurfacePSet->Clip(plane, &right, &left); + right.ComputeConvexHull(rightCH1, convexhullDownsampling); + left.ComputeConvexHull(leftCH1, convexhullDownsampling); + + volumeLeftCH1 = leftCH1.ComputeVolume(); + volumeRightCH1 = rightCH1.ComputeVolume(); +#endif //TEST_APPROX_CH + } + else { + PrimitiveSet* const right = psets[threadID]; + PrimitiveSet* const left = psets[threadID + m_ompNumProcessors]; + onSurfacePSet->Clip(plane, right, left); + right->ComputeConvexHull(rightCH, convexhullDownsampling); + left->ComputeConvexHull(leftCH, convexhullDownsampling); + } + double volumeLeftCH = leftCH.ComputeVolume(); + double volumeRightCH = rightCH.ComputeVolume(); + + // compute clipped volumes + double volumeLeft = 0.0; + double volumeRight = 0.0; + if (oclAcceleration) { +#ifdef CL_VERSION_1_1 + uint32_t volumes[4]; + cl_int error = clEnqueueReadBuffer(m_oclQueue[threadID], partialVolumes[threadID], CL_TRUE, + 0, sizeof(uint32_t) * 4, volumes, 0, NULL, NULL); + size_t nPrimitivesRight = volumes[0] + volumes[1] + volumes[2] + volumes[3]; + size_t nPrimitivesLeft = nPrimitives - nPrimitivesRight; + volumeRight = nPrimitivesRight * unitVolume; + volumeLeft = nPrimitivesLeft * unitVolume; + if (error != CL_SUCCESS) { + if (params.m_logger) { + params.m_logger->Log("Couldn't read buffer \n"); + } + SetCancel(true); + } +#endif // CL_VERSION_1_1 + } + else { + inputPSet->ComputeClippedVolumes(plane, volumeRight, volumeLeft); + } + double concavityLeft = ComputeConcavity(volumeLeft, volumeLeftCH, m_volumeCH0); + double concavityRight = ComputeConcavity(volumeRight, volumeRightCH, m_volumeCH0); + double concavity = (concavityLeft + concavityRight); + + // compute cost + double balance = alpha * fabs(volumeLeft - volumeRight) / m_volumeCH0; + double d = w * (preferredCuttingDirection[0] * plane.m_a + preferredCuttingDirection[1] * plane.m_b + preferredCuttingDirection[2] * plane.m_c); + double symmetry = beta * d; + double total = concavity + balance + symmetry; + +#if USE_THREAD == 1 && _OPENMP +#pragma omp critical +#endif + { + if (total < minTotal || (total == minTotal && x < iBest)) { + minConcavity = concavity; + minBalance = balance; + minSymmetry = symmetry; + bestPlane = plane; + minTotal = total; + iBest = x; + } + ++done; + if (!(done & 127)) // reduce update frequency + { + double progress = done * (progress1 - progress0) / nPlanes + progress0; + Update(m_stageProgress, progress, params); + } + } + } + } + +#ifdef DEBUG_TEMP + timerComputeCost.Toc(); + printf_s("Cost[%i] = %f\n", nPlanes, timerComputeCost.GetElapsedTime()); +#endif // DEBUG_TEMP + +#ifdef CL_VERSION_1_1 + if (oclAcceleration) { + clReleaseMemObject(voxels); + for (int32_t i = 0; i < m_ompNumProcessors; ++i) { + clReleaseMemObject(partialVolumes[i]); + } + delete[] partialVolumes; + } +#endif // CL_VERSION_1_1 + + if (psets) { + for (int32_t i = 0; i < 2 * m_ompNumProcessors; ++i) { + delete psets[i]; + } + delete[] psets; + } + delete onSurfacePSet; + delete[] chPts; + delete[] chs; + if (params.m_logger) { + sprintf(msg, "\n\t\t\t Best %04i T=%2.6f C=%2.6f B=%2.6f S=%2.6f (%1.1f, %1.1f, %1.1f, %3.3f)\n\n", iBest, minTotal, minConcavity, minBalance, minSymmetry, bestPlane.m_a, bestPlane.m_b, bestPlane.m_c, bestPlane.m_d); + params.m_logger->Log(msg); + } +} +void VHACD::ComputeACD(const Parameters& params) +{ + if (GetCancel()) { + return; + } + m_timer.Tic(); + + m_stage = "Approximate Convex Decomposition"; + m_stageProgress = 0.0; + std::ostringstream msg; + if (params.m_logger) { + msg << "+ " << m_stage << std::endl; + params.m_logger->Log(msg.str().c_str()); + } + + SArray<PrimitiveSet*> parts; + SArray<PrimitiveSet*> inputParts; + SArray<PrimitiveSet*> temp; + inputParts.PushBack(m_pset); + m_pset = 0; + SArray<Plane> planes; + SArray<Plane> planesRef; + uint32_t sub = 0; + bool firstIteration = true; + m_volumeCH0 = 1.0; + + // Compute the decomposition depth based on the number of convex hulls being requested.. + uint32_t hullCount = 2; + uint32_t depth = 1; + while (params.m_maxConvexHulls > hullCount) + { + depth++; + hullCount *= 2; + } + // We must always increment the decomposition depth one higher than the maximum number of hulls requested. + // The reason for this is as follows. + // Say, for example, the user requests 32 convex hulls exactly. This would be a decomposition depth of 5. + // However, when we do that, we do *not* necessarily get 32 hulls as a result. This is because, during + // the recursive descent of the binary tree, one or more of the leaf nodes may have no concavity and + // will not be split. So, in this way, even with a decomposition depth of 5, you can produce fewer than + // 32 hulls. So, in this case, we would set the decomposition depth to 6 (producing up to as high as 64 convex hulls). + // Then, the merge step which combines over-described hulls down to the user requested amount, we will end up + // getting exactly 32 convex hulls as a result. + // We could just allow the artist to directly control the decomposition depth directly, but this would be a bit + // too complex and the preference is simply to let them specify how many hulls they want and derive the solution + // from that. + depth++; + + + while (sub++ < depth && inputParts.Size() > 0 && !m_cancel) { + msg.str(""); + msg << "Subdivision level " << sub; + m_operation = msg.str(); + + if (params.m_logger) { + msg.str(""); + msg << "\t Subdivision level " << sub << std::endl; + params.m_logger->Log(msg.str().c_str()); + } + + double maxConcavity = 0.0; + const size_t nInputParts = inputParts.Size(); + Update(m_stageProgress, 0.0, params); + for (size_t p = 0; p < nInputParts && !m_cancel; ++p) { + const double progress0 = p * 100.0 / nInputParts; + const double progress1 = (p + 0.75) * 100.0 / nInputParts; + const double progress2 = (p + 1.00) * 100.0 / nInputParts; + + Update(m_stageProgress, progress0, params); + + PrimitiveSet* pset = inputParts[p]; + inputParts[p] = 0; + double volume = pset->ComputeVolume(); + pset->ComputeBB(); + pset->ComputePrincipalAxes(); + if (params.m_pca) { + pset->AlignToPrincipalAxes(); + } + + pset->ComputeConvexHull(pset->GetConvexHull()); + double volumeCH = fabs(pset->GetConvexHull().ComputeVolume()); + if (firstIteration) { + m_volumeCH0 = volumeCH; + } + + double concavity = ComputeConcavity(volume, volumeCH, m_volumeCH0); + double error = 1.01 * pset->ComputeMaxVolumeError() / m_volumeCH0; + + if (firstIteration) { + firstIteration = false; + } + + if (params.m_logger) { + msg.str(""); + msg << "\t -> Part[" << p + << "] C = " << concavity + << ", E = " << error + << ", VS = " << pset->GetNPrimitivesOnSurf() + << ", VI = " << pset->GetNPrimitivesInsideSurf() + << std::endl; + params.m_logger->Log(msg.str().c_str()); + } + + if (concavity > params.m_concavity && concavity > error) { + Vec3<double> preferredCuttingDirection; + double w = ComputePreferredCuttingDirection(pset, preferredCuttingDirection); + planes.Resize(0); + if (params.m_mode == 0) { + VoxelSet* vset = (VoxelSet*)pset; + ComputeAxesAlignedClippingPlanes(*vset, params.m_planeDownsampling, planes); + } + else { + TetrahedronSet* tset = (TetrahedronSet*)pset; + ComputeAxesAlignedClippingPlanes(*tset, params.m_planeDownsampling, planes); + } + + if (params.m_logger) { + msg.str(""); + msg << "\t\t [Regular sampling] Number of clipping planes " << planes.Size() << std::endl; + params.m_logger->Log(msg.str().c_str()); + } + + Plane bestPlane; + double minConcavity = MAX_DOUBLE; + ComputeBestClippingPlane(pset, + volume, + planes, + preferredCuttingDirection, + w, + concavity * params.m_alpha, + concavity * params.m_beta, + params.m_convexhullDownsampling, + progress0, + progress1, + bestPlane, + minConcavity, + params); + if (!m_cancel && (params.m_planeDownsampling > 1 || params.m_convexhullDownsampling > 1)) { + planesRef.Resize(0); + + if (params.m_mode == 0) { + VoxelSet* vset = (VoxelSet*)pset; + RefineAxesAlignedClippingPlanes(*vset, bestPlane, params.m_planeDownsampling, planesRef); + } + else { + TetrahedronSet* tset = (TetrahedronSet*)pset; + RefineAxesAlignedClippingPlanes(*tset, bestPlane, params.m_planeDownsampling, planesRef); + } + + if (params.m_logger) { + msg.str(""); + msg << "\t\t [Refining] Number of clipping planes " << planesRef.Size() << std::endl; + params.m_logger->Log(msg.str().c_str()); + } + ComputeBestClippingPlane(pset, + volume, + planesRef, + preferredCuttingDirection, + w, + concavity * params.m_alpha, + concavity * params.m_beta, + 1, // convexhullDownsampling = 1 + progress1, + progress2, + bestPlane, + minConcavity, + params); + } + if (GetCancel()) { + delete pset; // clean up + break; + } + else { + if (maxConcavity < minConcavity) { + maxConcavity = minConcavity; + } + PrimitiveSet* bestLeft = pset->Create(); + PrimitiveSet* bestRight = pset->Create(); + temp.PushBack(bestLeft); + temp.PushBack(bestRight); + pset->Clip(bestPlane, bestRight, bestLeft); + if (params.m_pca) { + bestRight->RevertAlignToPrincipalAxes(); + bestLeft->RevertAlignToPrincipalAxes(); + } + delete pset; + } + } + else { + if (params.m_pca) { + pset->RevertAlignToPrincipalAxes(); + } + parts.PushBack(pset); + } + } + + Update(95.0 * (1.0 - maxConcavity) / (1.0 - params.m_concavity), 100.0, params); + if (GetCancel()) { + const size_t nTempParts = temp.Size(); + for (size_t p = 0; p < nTempParts; ++p) { + delete temp[p]; + } + temp.Resize(0); + } + else { + inputParts = temp; + temp.Resize(0); + } + } + const size_t nInputParts = inputParts.Size(); + for (size_t p = 0; p < nInputParts; ++p) { + parts.PushBack(inputParts[p]); + } + + if (GetCancel()) { + const size_t nParts = parts.Size(); + for (size_t p = 0; p < nParts; ++p) { + delete parts[p]; + } + return; + } + + m_overallProgress = 90.0; + Update(m_stageProgress, 100.0, params); + + msg.str(""); + msg << "Generate convex-hulls"; + m_operation = msg.str(); + size_t nConvexHulls = parts.Size(); + if (params.m_logger) { + msg.str(""); + msg << "+ Generate " << nConvexHulls << " convex-hulls " << std::endl; + params.m_logger->Log(msg.str().c_str()); + } + + Update(m_stageProgress, 0.0, params); + m_convexHulls.Resize(0); + for (size_t p = 0; p < nConvexHulls && !m_cancel; ++p) { + Update(m_stageProgress, p * 100.0 / nConvexHulls, params); + m_convexHulls.PushBack(new Mesh); + parts[p]->ComputeConvexHull(*m_convexHulls[p]); + size_t nv = m_convexHulls[p]->GetNPoints(); + double x, y, z; + for (size_t i = 0; i < nv; ++i) { + Vec3<double>& pt = m_convexHulls[p]->GetPoint(i); + x = pt[0]; + y = pt[1]; + z = pt[2]; + pt[0] = m_rot[0][0] * x + m_rot[0][1] * y + m_rot[0][2] * z + m_barycenter[0]; + pt[1] = m_rot[1][0] * x + m_rot[1][1] * y + m_rot[1][2] * z + m_barycenter[1]; + pt[2] = m_rot[2][0] * x + m_rot[2][1] * y + m_rot[2][2] * z + m_barycenter[2]; + } + } + + const size_t nParts = parts.Size(); + for (size_t p = 0; p < nParts; ++p) { + delete parts[p]; + parts[p] = 0; + } + parts.Resize(0); + + if (GetCancel()) { + const size_t nConvexHulls = m_convexHulls.Size(); + for (size_t p = 0; p < nConvexHulls; ++p) { + delete m_convexHulls[p]; + } + m_convexHulls.Clear(); + return; + } + + m_overallProgress = 95.0; + Update(100.0, 100.0, params); + m_timer.Toc(); + if (params.m_logger) { + msg.str(""); + msg << "\t time " << m_timer.GetElapsedTime() / 1000.0 << "s" << std::endl; + params.m_logger->Log(msg.str().c_str()); + } +} +void AddPoints(const Mesh* const mesh, SArray<Vec3<double> >& pts) +{ + const int32_t n = (int32_t)mesh->GetNPoints(); + for (int32_t i = 0; i < n; ++i) { + pts.PushBack(mesh->GetPoint(i)); + } +} +void ComputeConvexHull(const Mesh* const ch1, const Mesh* const ch2, SArray<Vec3<double> >& pts, Mesh* const combinedCH) +{ + pts.Resize(0); + AddPoints(ch1, pts); + AddPoints(ch2, pts); + + btConvexHullComputer ch; + ch.compute((double*)pts.Data(), 3 * sizeof(double), (int32_t)pts.Size(), -1.0, -1.0); + combinedCH->ResizePoints(0); + combinedCH->ResizeTriangles(0); + for (int32_t v = 0; v < ch.vertices.size(); v++) { + combinedCH->AddPoint(Vec3<double>(ch.vertices[v].getX(), ch.vertices[v].getY(), ch.vertices[v].getZ())); + } + const int32_t nt = ch.faces.size(); + for (int32_t t = 0; t < nt; ++t) { + const btConvexHullComputer::Edge* sourceEdge = &(ch.edges[ch.faces[t]]); + int32_t a = sourceEdge->getSourceVertex(); + int32_t b = sourceEdge->getTargetVertex(); + const btConvexHullComputer::Edge* edge = sourceEdge->getNextEdgeOfFace(); + int32_t c = edge->getTargetVertex(); + while (c != a) { + combinedCH->AddTriangle(Vec3<int32_t>(a, b, c)); + edge = edge->getNextEdgeOfFace(); + b = c; + c = edge->getTargetVertex(); + } + } +} +void VHACD::MergeConvexHulls(const Parameters& params) +{ + if (GetCancel()) { + return; + } + m_timer.Tic(); + + m_stage = "Merge Convex Hulls"; + + std::ostringstream msg; + if (params.m_logger) { + msg << "+ " << m_stage << std::endl; + params.m_logger->Log(msg.str().c_str()); + } + + // Get the current number of convex hulls + size_t nConvexHulls = m_convexHulls.Size(); + // Iteration counter + int32_t iteration = 0; + // While we have more than at least one convex hull and the user has not asked us to cancel the operation + if (nConvexHulls > 1 && !m_cancel) + { + // Get the gamma error threshold for when to exit + SArray<Vec3<double> > pts; + Mesh combinedCH; + + // Populate the cost matrix + size_t idx = 0; + SArray<float> costMatrix; + costMatrix.Resize(((nConvexHulls * nConvexHulls) - nConvexHulls) >> 1); + for (size_t p1 = 1; p1 < nConvexHulls; ++p1) + { + const float volume1 = m_convexHulls[p1]->ComputeVolume(); + for (size_t p2 = 0; p2 < p1; ++p2) + { + ComputeConvexHull(m_convexHulls[p1], m_convexHulls[p2], pts, &combinedCH); + costMatrix[idx++] = ComputeConcavity(volume1 + m_convexHulls[p2]->ComputeVolume(), combinedCH.ComputeVolume(), m_volumeCH0); + } + } + + // Until we cant merge below the maximum cost + size_t costSize = m_convexHulls.Size(); + while (!m_cancel) + { + msg.str(""); + msg << "Iteration " << iteration++; + m_operation = msg.str(); + + // Search for lowest cost + float bestCost = (std::numeric_limits<float>::max)(); + const size_t addr = FindMinimumElement(costMatrix.Data(), &bestCost, 0, costMatrix.Size()); + if ( (costSize-1) < params.m_maxConvexHulls) + { + break; + } + const size_t addrI = (static_cast<int32_t>(sqrt(1 + (8 * addr))) - 1) >> 1; + const size_t p1 = addrI + 1; + const size_t p2 = addr - ((addrI * (addrI + 1)) >> 1); + assert(p1 >= 0); + assert(p2 >= 0); + assert(p1 < costSize); + assert(p2 < costSize); + + if (params.m_logger) + { + msg.str(""); + msg << "\t\t Merging (" << p1 << ", " << p2 << ") " << bestCost << std::endl + << std::endl; + params.m_logger->Log(msg.str().c_str()); + } + + // Make the lowest cost row and column into a new hull + Mesh* cch = new Mesh; + ComputeConvexHull(m_convexHulls[p1], m_convexHulls[p2], pts, cch); + delete m_convexHulls[p2]; + m_convexHulls[p2] = cch; + + delete m_convexHulls[p1]; + std::swap(m_convexHulls[p1], m_convexHulls[m_convexHulls.Size() - 1]); + m_convexHulls.PopBack(); + + costSize = costSize - 1; + + // Calculate costs versus the new hull + size_t rowIdx = ((p2 - 1) * p2) >> 1; + const float volume1 = m_convexHulls[p2]->ComputeVolume(); + for (size_t i = 0; (i < p2) && (!m_cancel); ++i) + { + ComputeConvexHull(m_convexHulls[p2], m_convexHulls[i], pts, &combinedCH); + costMatrix[rowIdx++] = ComputeConcavity(volume1 + m_convexHulls[i]->ComputeVolume(), combinedCH.ComputeVolume(), m_volumeCH0); + } + + rowIdx += p2; + for (size_t i = p2 + 1; (i < costSize) && (!m_cancel); ++i) + { + ComputeConvexHull(m_convexHulls[p2], m_convexHulls[i], pts, &combinedCH); + costMatrix[rowIdx] = ComputeConcavity(volume1 + m_convexHulls[i]->ComputeVolume(), combinedCH.ComputeVolume(), m_volumeCH0); + rowIdx += i; + assert(rowIdx >= 0); + } + + // Move the top column in to replace its space + const size_t erase_idx = ((costSize - 1) * costSize) >> 1; + if (p1 < costSize) { + rowIdx = (addrI * p1) >> 1; + size_t top_row = erase_idx; + for (size_t i = 0; i < p1; ++i) { + if (i != p2) { + costMatrix[rowIdx] = costMatrix[top_row]; + } + ++rowIdx; + ++top_row; + } + + ++top_row; + rowIdx += p1; + for (size_t i = p1 + 1; i < (costSize + 1); ++i) { + costMatrix[rowIdx] = costMatrix[top_row++]; + rowIdx += i; + assert(rowIdx >= 0); + } + } + costMatrix.Resize(erase_idx); + } + } + m_overallProgress = 99.0; + Update(100.0, 100.0, params); + m_timer.Toc(); + if (params.m_logger) { + msg.str(""); + msg << "\t time " << m_timer.GetElapsedTime() / 1000.0 << "s" << std::endl; + params.m_logger->Log(msg.str().c_str()); + } +} +void VHACD::SimplifyConvexHull(Mesh* const ch, const size_t nvertices, const double minVolume) +{ + if (nvertices <= 4) { + return; + } + ICHull icHull; + if (mRaycastMesh) + { + // We project these points onto the original source mesh to increase precision + // The voxelization process drops floating point precision so returned data points are not exactly lying on the + // surface of the original source mesh. + // The first step is we need to compute the bounding box of the mesh we are trying to build a convex hull for. + // From this bounding box, we compute the length of the diagonal to get a relative size and center for point projection + uint32_t nPoints = ch->GetNPoints(); + Vec3<double> *inputPoints = ch->GetPointsBuffer(); + Vec3<double> bmin(inputPoints[0]); + Vec3<double> bmax(inputPoints[1]); + for (uint32_t i = 1; i < nPoints; i++) + { + const Vec3<double> &p = inputPoints[i]; + p.UpdateMinMax(bmin, bmax); + } + Vec3<double> center; + double diagonalLength = center.GetCenter(bmin, bmax); // Get the center of the bounding box + // This is the error threshold for determining if we should use the raycast result data point vs. the voxelized result. + double pointDistanceThreshold = diagonalLength * 0.05; + // If a new point is within 1/100th the diagonal length of the bounding volume we do not add it. To do so would create a + // thin sliver in the resulting convex hull + double snapDistanceThreshold = diagonalLength * 0.01; + double snapDistanceThresholdSquared = snapDistanceThreshold*snapDistanceThreshold; + + // Allocate buffer for projected vertices + Vec3<double> *outputPoints = new Vec3<double>[nPoints]; + uint32_t outCount = 0; + for (uint32_t i = 0; i < nPoints; i++) + { + Vec3<double> &inputPoint = inputPoints[i]; + Vec3<double> &outputPoint = outputPoints[outCount]; + // Compute the direction vector from the center of this mesh to the vertex + Vec3<double> dir = inputPoint - center; + // Normalize the direction vector. + dir.Normalize(); + // Multiply times the diagonal length of the mesh + dir *= diagonalLength; + // Add the center back in again to get the destination point + dir += center; + // By default the output point is equal to the input point + outputPoint = inputPoint; + double pointDistance; + if (mRaycastMesh->raycast(center.GetData(), dir.GetData(), inputPoint.GetData(), outputPoint.GetData(),&pointDistance) ) + { + // If the nearest intersection point is too far away, we keep the original source data point. + // Not all points lie directly on the original mesh surface + if (pointDistance > pointDistanceThreshold) + { + outputPoint = inputPoint; + } + } + // Ok, before we add this point, we do not want to create points which are extremely close to each other. + // This will result in tiny sliver triangles which are really bad for collision detection. + bool foundNearbyPoint = false; + for (uint32_t j = 0; j < outCount; j++) + { + // If this new point is extremely close to an existing point, we do not add it! + double squaredDistance = outputPoints[j].GetDistanceSquared(outputPoint); + if (squaredDistance < snapDistanceThresholdSquared ) + { + foundNearbyPoint = true; + break; + } + } + if (!foundNearbyPoint) + { + outCount++; + } + } + icHull.AddPoints(outputPoints, outCount); + delete[]outputPoints; + } + else + { + icHull.AddPoints(ch->GetPointsBuffer(), ch->GetNPoints()); + } + icHull.Process((uint32_t)nvertices, minVolume); + TMMesh& mesh = icHull.GetMesh(); + const size_t nT = mesh.GetNTriangles(); + const size_t nV = mesh.GetNVertices(); + ch->ResizePoints(nV); + ch->ResizeTriangles(nT); + mesh.GetIFS(ch->GetPointsBuffer(), ch->GetTrianglesBuffer()); +} +void VHACD::SimplifyConvexHulls(const Parameters& params) +{ + if (m_cancel || params.m_maxNumVerticesPerCH < 4) { + return; + } + m_timer.Tic(); + + m_stage = "Simplify convex-hulls"; + m_operation = "Simplify convex-hulls"; + + std::ostringstream msg; + const size_t nConvexHulls = m_convexHulls.Size(); + if (params.m_logger) { + msg << "+ Simplify " << nConvexHulls << " convex-hulls " << std::endl; + params.m_logger->Log(msg.str().c_str()); + } + + Update(0.0, 0.0, params); + for (size_t i = 0; i < nConvexHulls && !m_cancel; ++i) { + if (params.m_logger) { + msg.str(""); + msg << "\t\t Simplify CH[" << std::setfill('0') << std::setw(5) << i << "] " << m_convexHulls[i]->GetNPoints() << " V, " << m_convexHulls[i]->GetNTriangles() << " T" << std::endl; + params.m_logger->Log(msg.str().c_str()); + } + SimplifyConvexHull(m_convexHulls[i], params.m_maxNumVerticesPerCH, m_volumeCH0 * params.m_minVolumePerCH); + } + + m_overallProgress = 100.0; + Update(100.0, 100.0, params); + m_timer.Toc(); + if (params.m_logger) { + msg.str(""); + msg << "\t time " << m_timer.GetElapsedTime() / 1000.0 << "s" << std::endl; + params.m_logger->Log(msg.str().c_str()); + } +} + +bool VHACD::ComputeCenterOfMass(double centerOfMass[3]) const +{ + bool ret = false; + + centerOfMass[0] = 0; + centerOfMass[1] = 0; + centerOfMass[2] = 0; + // Get number of convex hulls in the result + uint32_t hullCount = GetNConvexHulls(); + if (hullCount) // if we have results + { + ret = true; + double totalVolume = 0; + // Initialize the center of mass to zero + centerOfMass[0] = 0; + centerOfMass[1] = 0; + centerOfMass[2] = 0; + // Compute the total volume of all convex hulls + for (uint32_t i = 0; i < hullCount; i++) + { + ConvexHull ch; + GetConvexHull(i, ch); + totalVolume += ch.m_volume; + } + // compute the reciprocal of the total volume + double recipVolume = 1.0 / totalVolume; + // Add in the weighted by volume average of the center point of each convex hull + for (uint32_t i = 0; i < hullCount; i++) + { + ConvexHull ch; + GetConvexHull(i, ch); + double ratio = ch.m_volume*recipVolume; + centerOfMass[0] += ch.m_center[0] * ratio; + centerOfMass[1] += ch.m_center[1] * ratio; + centerOfMass[2] += ch.m_center[2] * ratio; + } + } + return ret; +} + +} // end of VHACD namespace diff --git a/thirdparty/vhacd/src/btAlignedAllocator.cpp b/thirdparty/vhacd/src/btAlignedAllocator.cpp new file mode 100644 index 0000000000..8dee31e7e3 --- /dev/null +++ b/thirdparty/vhacd/src/btAlignedAllocator.cpp @@ -0,0 +1,192 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +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 "btAlignedAllocator.h" + +// -- GODOT start -- +namespace VHACD { +// -- GODOT end -- + +#ifdef _MSC_VER +#pragma warning(disable:4311 4302) +#endif + +int32_t gNumAlignedAllocs = 0; +int32_t gNumAlignedFree = 0; +int32_t gTotalBytesAlignedAllocs = 0; //detect memory leaks + +static void* btAllocDefault(size_t size) +{ + return malloc(size); +} + +static void btFreeDefault(void* ptr) +{ + free(ptr); +} + +static btAllocFunc* sAllocFunc = btAllocDefault; +static btFreeFunc* sFreeFunc = btFreeDefault; + +#if defined(BT_HAS_ALIGNED_ALLOCATOR) +#include <malloc.h> +static void* btAlignedAllocDefault(size_t size, int32_t alignment) +{ + return _aligned_malloc(size, (size_t)alignment); +} + +static void btAlignedFreeDefault(void* ptr) +{ + _aligned_free(ptr); +} +#elif defined(__CELLOS_LV2__) +#include <stdlib.h> + +static inline void* btAlignedAllocDefault(size_t size, int32_t alignment) +{ + return memalign(alignment, size); +} + +static inline void btAlignedFreeDefault(void* ptr) +{ + free(ptr); +} +#else +static inline void* btAlignedAllocDefault(size_t size, int32_t alignment) +{ + void* ret; + char* real; + unsigned long offset; + + real = (char*)sAllocFunc(size + sizeof(void*) + (alignment - 1)); + if (real) { + // -- GODOT start -- + // Synced with Bullet 2.88 to fix GH-27926 + //offset = (alignment - (unsigned long)(real + sizeof(void*))) & (alignment - 1); + //ret = (void*)((real + sizeof(void*)) + offset); + ret = btAlignPointer(real + sizeof(void *), alignment); + // -- GODOT end -- + *((void**)(ret)-1) = (void*)(real); + } + else { + ret = (void*)(real); + } + return (ret); +} + +static inline void btAlignedFreeDefault(void* ptr) +{ + void* real; + + if (ptr) { + real = *((void**)(ptr)-1); + sFreeFunc(real); + } +} +#endif + +static btAlignedAllocFunc* sAlignedAllocFunc = btAlignedAllocDefault; +static btAlignedFreeFunc* sAlignedFreeFunc = btAlignedFreeDefault; + +void btAlignedAllocSetCustomAligned(btAlignedAllocFunc* allocFunc, btAlignedFreeFunc* freeFunc) +{ + sAlignedAllocFunc = allocFunc ? allocFunc : btAlignedAllocDefault; + sAlignedFreeFunc = freeFunc ? freeFunc : btAlignedFreeDefault; +} + +void btAlignedAllocSetCustom(btAllocFunc* allocFunc, btFreeFunc* freeFunc) +{ + sAllocFunc = allocFunc ? allocFunc : btAllocDefault; + sFreeFunc = freeFunc ? freeFunc : btFreeDefault; +} + +#ifdef BT_DEBUG_MEMORY_ALLOCATIONS +//this generic allocator provides the total allocated number of bytes +#include <stdio.h> + +void* btAlignedAllocInternal(size_t size, int32_t alignment, int32_t line, char* filename) +{ + void* ret; + char* real; + unsigned long offset; + + gTotalBytesAlignedAllocs += size; + gNumAlignedAllocs++; + + real = (char*)sAllocFunc(size + 2 * sizeof(void*) + (alignment - 1)); + if (real) { + offset = (alignment - (unsigned long)(real + 2 * sizeof(void*))) & (alignment - 1); + ret = (void*)((real + 2 * sizeof(void*)) + offset); + *((void**)(ret)-1) = (void*)(real); + *((int32_t*)(ret)-2) = size; + } + else { + ret = (void*)(real); //?? + } + + printf("allocation#%d at address %x, from %s,line %d, size %d\n", gNumAlignedAllocs, real, filename, line, size); + + int32_t* ptr = (int32_t*)ret; + *ptr = 12; + return (ret); +} + +void btAlignedFreeInternal(void* ptr, int32_t line, char* filename) +{ + + void* real; + gNumAlignedFree++; + + if (ptr) { + real = *((void**)(ptr)-1); + int32_t size = *((int32_t*)(ptr)-2); + gTotalBytesAlignedAllocs -= size; + + printf("free #%d at address %x, from %s,line %d, size %d\n", gNumAlignedFree, real, filename, line, size); + + sFreeFunc(real); + } + else { + printf("NULL ptr\n"); + } +} + +#else //BT_DEBUG_MEMORY_ALLOCATIONS + +void* btAlignedAllocInternal(size_t size, int32_t alignment) +{ + gNumAlignedAllocs++; + void* ptr; + ptr = sAlignedAllocFunc(size, alignment); + // printf("btAlignedAllocInternal %d, %x\n",size,ptr); + return ptr; +} + +void btAlignedFreeInternal(void* ptr) +{ + if (!ptr) { + return; + } + + gNumAlignedFree++; + // printf("btAlignedFreeInternal %x\n",ptr); + sAlignedFreeFunc(ptr); +} + +// -- GODOT start -- +}; // namespace VHACD +// -- GODOT end -- + +#endif //BT_DEBUG_MEMORY_ALLOCATIONS diff --git a/thirdparty/vhacd/src/btConvexHullComputer.cpp b/thirdparty/vhacd/src/btConvexHullComputer.cpp new file mode 100644 index 0000000000..8ab34af2ab --- /dev/null +++ b/thirdparty/vhacd/src/btConvexHullComputer.cpp @@ -0,0 +1,2487 @@ +/* +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 "btAlignedObjectArray.h" +#include "btConvexHullComputer.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 int32_t int32_t; +typedef long long int32_t int64_t; +typedef uint32_t uint32_t; +typedef unsigned long long int32_t uint64_t; +#endif + +#ifdef _MSC_VER +#pragma warning(disable:4458) +#endif + +//The definition of USE_X86_64_ASM is moved into the build system. You can enable it manually by commenting out the following lines +//#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 + +// -- GODOT start -- +namespace VHACD { +// -- GODOT end -- + +// Convex hull implementation based on Preparata and Hong +// Ole Kniemeyer, MAXON Computer GmbH +class btConvexHullInternal { +public: + class Point64 { + public: + int64_t x; + int64_t y; + int64_t z; + + Point64(int64_t x, int64_t y, int64_t z) + : x(x) + , y(y) + , z(z) + { + } + + bool isZero() + { + return (x == 0) && (y == 0) && (z == 0); + } + + int64_t dot(const Point64& b) const + { + return x * b.x + y * b.y + z * b.z; + } + }; + + class Point32 { + public: + int32_t x; + int32_t y; + int32_t z; + int32_t index; + + Point32() + { + } + + Point32(int32_t x, int32_t y, int32_t z) + : x(x) + , y(y) + , z(z) + , index(-1) + { + } + + bool operator==(const Point32& b) const + { + return (x == b.x) && (y == b.y) && (z == b.z); + } + + bool operator!=(const Point32& b) const + { + return (x != b.x) || (y != b.y) || (z != b.z); + } + + bool isZero() + { + return (x == 0) && (y == 0) && (z == 0); + } + + Point64 cross(const Point32& b) const + { + return Point64(y * b.z - z * b.y, z * b.x - x * b.z, x * b.y - y * b.x); + } + + Point64 cross(const Point64& b) const + { + return Point64(y * b.z - z * b.y, z * b.x - x * b.z, x * b.y - y * b.x); + } + + int64_t dot(const Point32& b) const + { + return x * b.x + y * b.y + z * b.z; + } + + int64_t dot(const Point64& b) const + { + return x * b.x + y * b.y + z * b.z; + } + + Point32 operator+(const Point32& b) const + { + return Point32(x + b.x, y + b.y, z + b.z); + } + + Point32 operator-(const Point32& b) const + { + return Point32(x - b.x, y - b.y, z - b.z); + } + }; + + class Int128 { + public: + uint64_t low; + uint64_t high; + + Int128() + { + } + + Int128(uint64_t low, uint64_t high) + : low(low) + , high(high) + { + } + + Int128(uint64_t low) + : low(low) + , high(0) + { + } + + Int128(int64_t value) + : low(value) + , high((value >= 0) ? 0 : (uint64_t)-1LL) + { + } + + static Int128 mul(int64_t a, int64_t b); + + static Int128 mul(uint64_t a, uint64_t b); + + Int128 operator-() const + { + return Int128((uint64_t) - (int64_t)low, ~high + (low == 0)); + } + + Int128 operator+(const Int128& b) const + { +#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(); + } + + int32_t getSign() const + { + return ((int64_t)high < 0) ? -1 : (high || low) ? 1 : 0; + } + + bool operator<(const Int128& b) const + { + return (high < b.high) || ((high == b.high) && (low < b.low)); + } + + int32_t ucmp(const Int128& b) const + { + if (high < b.high) { + return -1; + } + if (high > b.high) { + return 1; + } + if (low < b.low) { + return -1; + } + if (low > b.low) { + return 1; + } + return 0; + } + }; + + class Rational64 { + private: + uint64_t m_numerator; + uint64_t m_denominator; + int32_t sign; + + public: + Rational64(int64_t numerator, int64_t denominator) + { + if (numerator > 0) { + sign = 1; + m_numerator = (uint64_t)numerator; + } + else if (numerator < 0) { + sign = -1; + m_numerator = (uint64_t)-numerator; + } + else { + sign = 0; + m_numerator = 0; + } + if (denominator > 0) { + m_denominator = (uint64_t)denominator; + } + else if (denominator < 0) { + sign = -sign; + m_denominator = (uint64_t)-denominator; + } + else { + m_denominator = 0; + } + } + + bool isNegativeInfinity() const + { + return (sign < 0) && (m_denominator == 0); + } + + bool isNaN() const + { + return (sign == 0) && (m_denominator == 0); + } + + int32_t compare(const Rational64& b) const; + + btScalar toScalar() const + { + return sign * ((m_denominator == 0) ? SIMD_INFINITY : (btScalar)m_numerator / m_denominator); + } + }; + + class Rational128 { + private: + Int128 numerator; + Int128 denominator; + int32_t sign; + bool isInt64; + + public: + Rational128(int64_t value) + { + if (value > 0) { + sign = 1; + this->numerator = value; + } + else if (value < 0) { + sign = -1; + this->numerator = -value; + } + else { + sign = 0; + this->numerator = (uint64_t)0; + } + this->denominator = (uint64_t)1; + isInt64 = true; + } + + Rational128(const Int128& numerator, const Int128& denominator) + { + sign = numerator.getSign(); + if (sign >= 0) { + this->numerator = numerator; + } + else { + this->numerator = -numerator; + } + int32_t dsign = denominator.getSign(); + if (dsign >= 0) { + this->denominator = denominator; + } + else { + sign = -sign; + this->denominator = -denominator; + } + isInt64 = false; + } + + int32_t compare(const Rational128& b) const; + + int32_t compare(int64_t b) const; + + btScalar toScalar() const + { + return sign * ((denominator.getSign() == 0) ? SIMD_INFINITY : numerator.toScalar() / denominator.toScalar()); + } + }; + + class PointR128 { + public: + Int128 x; + Int128 y; + Int128 z; + Int128 denominator; + + PointR128() + { + } + + PointR128(Int128 x, Int128 y, Int128 z, Int128 denominator) + : x(x) + , y(y) + , z(z) + , denominator(denominator) + { + } + + btScalar xvalue() const + { + return x.toScalar() / denominator.toScalar(); + } + + btScalar yvalue() const + { + return y.toScalar() / denominator.toScalar(); + } + + btScalar zvalue() const + { + return z.toScalar() / denominator.toScalar(); + } + }; + + class Edge; + class Face; + + class Vertex { + public: + Vertex* next; + Vertex* prev; + Edge* edges; + Face* firstNearbyFace; + Face* lastNearbyFace; + PointR128 point128; + Point32 point; + int32_t copy; + + Vertex() + : next(NULL) + , prev(NULL) + , edges(NULL) + , firstNearbyFace(NULL) + , lastNearbyFace(NULL) + , copy(-1) + { + } + +#ifdef DEBUG_CONVEX_HULL + void print() + { + printf("V%d (%d, %d, %d)", point.index, point.x, point.y, point.z); + } + + void printGraph(); +#endif + + Point32 operator-(const Vertex& b) const + { + return point - b.point; + } + + Rational128 dot(const Point64& b) const + { + return (point.index >= 0) ? Rational128(point.dot(b)) + : Rational128(point128.x * b.x + point128.y * b.y + point128.z * b.z, point128.denominator); + } + + btScalar xvalue() const + { + return (point.index >= 0) ? btScalar(point.x) : point128.xvalue(); + } + + btScalar yvalue() const + { + return (point.index >= 0) ? btScalar(point.y) : point128.yvalue(); + } + + btScalar zvalue() const + { + return (point.index >= 0) ? btScalar(point.z) : point128.zvalue(); + } + + void receiveNearbyFaces(Vertex* src) + { + if (lastNearbyFace) { + lastNearbyFace->nextWithSameNearbyVertex = src->firstNearbyFace; + } + else { + firstNearbyFace = src->firstNearbyFace; + } + if (src->lastNearbyFace) { + lastNearbyFace = src->lastNearbyFace; + } + for (Face* f = src->firstNearbyFace; f; f = f->nextWithSameNearbyVertex) { + btAssert(f->nearbyVertex == src); + f->nearbyVertex = this; + } + src->firstNearbyFace = NULL; + src->lastNearbyFace = NULL; + } + }; + + class Edge { + public: + Edge* next; + Edge* prev; + Edge* reverse; + Vertex* target; + Face* face; + int32_t copy; + + ~Edge() + { + next = NULL; + prev = NULL; + reverse = NULL; + target = NULL; + face = NULL; + } + + void link(Edge* n) + { + btAssert(reverse->target == n->reverse->target); + next = n; + n->prev = this; + } + +#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; + int32_t size; + + public: + PoolArray<T>* next; + + PoolArray(int32_t size) + : size(size) + , next(NULL) + { + array = (T*)btAlignedAlloc(sizeof(T) * size, 16); + } + + ~PoolArray() + { + btAlignedFree(array); + } + + T* init() + { + T* o = array; + for (int32_t i = 0; i < size; i++, o++) { + o->next = (i + 1 < size) ? o + 1 : NULL; + } + return array; + } + }; + + template <typename T> + class Pool { + private: + PoolArray<T>* arrays; + PoolArray<T>* nextArray; + T* freeObjects; + int32_t arraySize; + + public: + Pool() + : arrays(NULL) + , nextArray(NULL) + , freeObjects(NULL) + , arraySize(256) + { + } + + ~Pool() + { + while (arrays) { + PoolArray<T>* p = arrays; + arrays = p->next; + p->~PoolArray<T>(); + btAlignedFree(p); + } + } + + void reset() + { + nextArray = arrays; + freeObjects = NULL; + } + + void setArraySize(int32_t arraySize) + { + this->arraySize = arraySize; + } + + T* newObject() + { + T* o = freeObjects; + if (!o) { + PoolArray<T>* p = nextArray; + if (p) { + nextArray = p->next; + } + else { + p = new (btAlignedAlloc(sizeof(PoolArray<T>), 16)) PoolArray<T>(arraySize); + p->next = arrays; + arrays = p; + } + o = p->init(); + } + freeObjects = o->next; + return new (o) T(); + }; + + void freeObject(T* object) + { + object->~T(); + object->next = freeObjects; + freeObjects = object; + } + }; + + btVector3 scaling; + btVector3 center; + Pool<Vertex> vertexPool; + Pool<Edge> edgePool; + Pool<Face> facePool; + btAlignedObjectArray<Vertex*> originalVertices; + int32_t mergeStamp; + int32_t minAxis; + int32_t medAxis; + int32_t maxAxis; + int32_t usedEdgePairs; + int32_t maxUsedEdgePairs; + + static Orientation getOrientation(const Edge* prev, const Edge* next, const Point32& s, const Point32& t); + Edge* findMaxAngle(bool ccw, const Vertex* start, const Point32& s, const Point64& rxs, const Point64& sxrxs, Rational64& minCot); + void findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge*& e0, Edge*& e1, Vertex* stop0, Vertex* stop1); + + Edge* newEdgePair(Vertex* from, Vertex* to); + + void removeEdgePair(Edge* edge) + { + Edge* n = edge->next; + Edge* r = edge->reverse; + + btAssert(edge->target && r->target); + + if (n != edge) { + n->prev = edge->prev; + edge->prev->next = n; + r->target->edges = n; + } + else { + r->target->edges = NULL; + } + + n = r->next; + + if (n != r) { + n->prev = r->prev; + r->prev->next = n; + edge->target->edges = n; + } + else { + edge->target->edges = NULL; + } + + edgePool.freeObject(edge); + edgePool.freeObject(r); + usedEdgePairs--; + } + + void computeInternal(int32_t start, int32_t end, IntermediateHull& result); + + bool mergeProjection(IntermediateHull& h0, IntermediateHull& h1, Vertex*& c0, Vertex*& c1); + + void merge(IntermediateHull& h0, IntermediateHull& h1); + + btVector3 toBtVector(const Point32& v); + + btVector3 getBtNormal(Face* face); + + bool shiftFace(Face* face, btScalar amount, btAlignedObjectArray<Vertex*> stack); + +public: + Vertex* vertexList; + + void compute(const void* coords, bool doubleCoords, int32_t stride, int32_t 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; +} + +int32_t btConvexHullInternal::Rational64::compare(const Rational64& b) const +{ + if (sign != b.sign) { + return sign - b.sign; + } + else if (sign == 0) { + return 0; + } + +// return (numerator * b.denominator > b.numerator * denominator) ? sign : (numerator * b.denominator < b.numerator * denominator) ? -sign : 0; + +#ifdef USE_X86_64_ASM + + int32_t result; + int64_t tmp; + int64_t dummy; + __asm__("mulq %[bn]\n\t" + "movq %%rax, %[tmp]\n\t" + "movq %%rdx, %%rbx\n\t" + "movq %[tn], %%rax\n\t" + "mulq %[bd]\n\t" + "subq %[tmp], %%rax\n\t" + "sbbq %%rbx, %%rdx\n\t" // rdx:rax contains 128-bit-difference "numerator*b.denominator - b.numerator*denominator" + "setnsb %%bh\n\t" // bh=1 if difference is non-negative, bh=0 otherwise + "orq %%rdx, %%rax\n\t" + "setnzb %%bl\n\t" // bl=1 if difference if non-zero, bl=0 if it is zero + "decb %%bh\n\t" // now bx=0x0000 if difference is zero, 0xff01 if it is negative, 0x0001 if it is positive (i.e., same sign as difference) + "shll $16, %%ebx\n\t" // ebx has same sign as difference + : "=&b"(result), [tmp] "=&r"(tmp), "=a"(dummy) + : "a"(denominator), [bn] "g"(b.numerator), [tn] "g"(numerator), [bd] "g"(b.denominator) + : "%rdx", "cc"); + return result ? result ^ sign // if sign is +1, only bit 0 of result is inverted, which does not change the sign of result (and cannot result in zero) + // if sign is -1, all bits of result are inverted, which changes the sign of result (and again cannot result in zero) + : 0; + +#else + + return sign * Int128::mul(m_numerator, b.m_denominator).ucmp(Int128::mul(m_denominator, b.m_numerator)); + +#endif +} + +int32_t btConvexHullInternal::Rational128::compare(const Rational128& b) const +{ + if (sign != b.sign) { + return sign - b.sign; + } + else if (sign == 0) { + return 0; + } + if (isInt64) { + return -b.compare(sign * (int64_t)numerator.low); + } + + Int128 nbdLow, nbdHigh, dbnLow, dbnHigh; + DMul<Int128, uint64_t>::mul(numerator, b.denominator, nbdLow, nbdHigh); + DMul<Int128, uint64_t>::mul(denominator, b.numerator, dbnLow, dbnHigh); + + int32_t cmp = nbdHigh.ucmp(dbnHigh); + if (cmp) { + return cmp * sign; + } + return nbdLow.ucmp(dbnLow) * sign; +} + +int32_t btConvexHullInternal::Rational128::compare(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 (int32_t side = 0; side <= 1; side++) { + int32_t dx = (v1->point.x - v0->point.x) * sign; + if (dx > 0) { + while (true) { + int32_t dy = v1->point.y - v0->point.y; + + Vertex* w0 = side ? v0->next : v0->prev; + if (w0 != v0) { + int32_t dx0 = (w0->point.x - v0->point.x) * sign; + int32_t dy0 = w0->point.y - v0->point.y; + if ((dy0 <= 0) && ((dx0 == 0) || ((dx0 < 0) && (dy0 * dx <= dy * dx0)))) { + v0 = w0; + dx = (v1->point.x - v0->point.x) * sign; + continue; + } + } + + Vertex* w1 = side ? v1->next : v1->prev; + if (w1 != v1) { + int32_t dx1 = (w1->point.x - v1->point.x) * sign; + int32_t dy1 = w1->point.y - v1->point.y; + int32_t dxn = (w1->point.x - v0->point.x) * sign; + if ((dxn > 0) && (dy1 < 0) && ((dx1 == 0) || ((dx1 < 0) && (dy1 * dx < dy * dx1)))) { + v1 = w1; + dx = dxn; + continue; + } + } + + break; + } + } + else if (dx < 0) { + while (true) { + int32_t dy = v1->point.y - v0->point.y; + + Vertex* w1 = side ? v1->prev : v1->next; + if (w1 != v1) { + int32_t dx1 = (w1->point.x - v1->point.x) * sign; + int32_t dy1 = w1->point.y - v1->point.y; + if ((dy1 >= 0) && ((dx1 == 0) || ((dx1 < 0) && (dy1 * dx <= dy * dx1)))) { + v1 = w1; + dx = (v1->point.x - v0->point.x) * sign; + continue; + } + } + + Vertex* w0 = side ? v0->prev : v0->next; + if (w0 != v0) { + int32_t dx0 = (w0->point.x - v0->point.x) * sign; + int32_t dy0 = w0->point.y - v0->point.y; + int32_t dxn = (v1->point.x - w0->point.x) * sign; + if ((dxn < 0) && (dy0 > 0) && ((dx0 == 0) || ((dx0 < 0) && (dy0 * dx < dy * dx0)))) { + v0 = w0; + dx = dxn; + continue; + } + } + + break; + } + } + else { + int32_t x = v0->point.x; + int32_t y0 = v0->point.y; + Vertex* w0 = v0; + Vertex* t; + while (((t = side ? w0->next : w0->prev) != v0) && (t->point.x == x) && (t->point.y <= y0)) { + w0 = t; + y0 = t->point.y; + } + v0 = w0; + + int32_t y1 = v1->point.y; + Vertex* w1 = v1; + while (((t = side ? w1->prev : w1->next) != v1) && (t->point.x == x) && (t->point.y >= y1)) { + w1 = t; + y1 = t->point.y; + } + v1 = w1; + } + + if (side == 0) { + v00 = v0; + v10 = v1; + + v0 = h0.minXy; + v1 = h1.minXy; + sign = -1; + } + } + + v0->prev = v1; + v1->next = v0; + + v00->next = v10; + v10->prev = v00; + + if (h1.minXy->point.x < h0.minXy->point.x) { + h0.minXy = h1.minXy; + } + if (h1.maxXy->point.x >= h0.maxXy->point.x) { + h0.maxXy = h1.maxXy; + } + + h0.maxYx = h1.maxYx; + + c0 = v00; + c1 = v10; + + return true; +} + +void btConvexHullInternal::computeInternal(int32_t start, int32_t end, IntermediateHull& result) +{ + int32_t n = end - start; + switch (n) { + case 0: + result.minXy = NULL; + result.maxXy = NULL; + result.minYx = NULL; + result.maxYx = NULL; + return; + case 2: { + Vertex* v = originalVertices[start]; + Vertex* w = v + 1; + if (v->point != w->point) { + int32_t dx = v->point.x - w->point.x; + int32_t dy = v->point.y - w->point.y; + + if ((dx == 0) && (dy == 0)) { + if (v->point.z > w->point.z) { + Vertex* t = w; + w = v; + v = t; + } + btAssert(v->point.z < w->point.z); + v->next = v; + v->prev = v; + result.minXy = v; + result.maxXy = v; + result.minYx = v; + result.maxYx = v; + } + else { + v->next = w; + v->prev = w; + w->next = v; + w->prev = v; + + if ((dx < 0) || ((dx == 0) && (dy < 0))) { + result.minXy = v; + result.maxXy = w; + } + else { + result.minXy = w; + result.maxXy = v; + } + + if ((dy < 0) || ((dy == 0) && (dx < 0))) { + result.minYx = v; + result.maxYx = w; + } + else { + result.minYx = w; + result.maxYx = v; + } + } + + Edge* e = newEdgePair(v, w); + e->link(e); + v->edges = e; + + e = e->reverse; + e->link(e); + w->edges = e; + + return; + } + } + // lint -fallthrough + case 1: { + Vertex* v = originalVertices[start]; + v->edges = NULL; + v->next = v; + v->prev = v; + + result.minXy = v; + result.maxXy = v; + result.minYx = v; + result.maxYx = v; + + return; + } + } + + int32_t split0 = start + n / 2; + Point32 p = originalVertices[split0 - 1]->point; + int32_t split1 = split0; + while ((split1 < end) && (originalVertices[split1]->point == p)) { + split1++; + } + computeInternal(start, split0, result); + IntermediateHull hull1; + computeInternal(split1, end, hull1); +#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()), (int32_t)cot.isNaN()); + e->print(); +#endif + if (cot.isNaN()) { + btAssert(ccw ? (t.dot(s) < 0) : (t.dot(s) > 0)); + } + else { + int32_t cmp; + if (minEdge == NULL) { + minCot = cot; + minEdge = e; + } + else if ((cmp = cot.compare(minCot)) < 0) { + minCot = cot; + minEdge = e; + } + else if ((cmp == 0) && (ccw == (getOrientation(minEdge, e, s, t) == COUNTER_CLOCKWISE))) { + minEdge = e; + } + } +#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 { + int32_t 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, int32_t stride, int32_t count) +{ + btVector3 min(btScalar(1e30), btScalar(1e30), btScalar(1e30)), max(btScalar(-1e30), btScalar(-1e30), btScalar(-1e30)); + const char* ptr = (const char*)coords; + if (doubleCoords) { + for (int32_t i = 0; i < count; i++) { + const double* v = (const double*)ptr; + btVector3 p((btScalar)v[0], (btScalar)v[1], (btScalar)v[2]); + ptr += stride; + min.setMin(p); + max.setMax(p); + } + } + else { + for (int32_t i = 0; i < count; i++) { + const float* v = (const float*)ptr; + btVector3 p(v[0], v[1], v[2]); + ptr += stride; + min.setMin(p); + max.setMax(p); + } + } + + btVector3 s = max - min; + maxAxis = s.maxAxis(); + minAxis = s.minAxis(); + if (minAxis == maxAxis) { + minAxis = (maxAxis + 1) % 3; + } + medAxis = 3 - maxAxis - minAxis; + + s /= btScalar(10216); + if (((medAxis + 1) % 3) != maxAxis) { + s *= -1; + } + scaling = s; + + if (s[0] != 0) { + s[0] = btScalar(1) / s[0]; + } + if (s[1] != 0) { + s[1] = btScalar(1) / s[1]; + } + if (s[2] != 0) { + s[2] = btScalar(1) / s[2]; + } + + center = (min + max) * btScalar(0.5); + + btAlignedObjectArray<Point32> points; + points.resize(count); + ptr = (const char*)coords; + if (doubleCoords) { + for (int32_t i = 0; i < count; i++) { + const double* v = (const double*)ptr; + btVector3 p((btScalar)v[0], (btScalar)v[1], (btScalar)v[2]); + ptr += stride; + p = (p - center) * s; + points[i].x = (int32_t)p[medAxis]; + points[i].y = (int32_t)p[maxAxis]; + points[i].z = (int32_t)p[minAxis]; + points[i].index = i; + } + } + else { + for (int32_t i = 0; i < count; i++) { + const float* v = (const float*)ptr; + btVector3 p(v[0], v[1], v[2]); + ptr += stride; + p = (p - center) * s; + points[i].x = (int32_t)p[medAxis]; + points[i].y = (int32_t)p[maxAxis]; + points[i].z = (int32_t)p[minAxis]; + points[i].index = i; + } + } + points.quickSort(pointCmp); + + vertexPool.reset(); + vertexPool.setArraySize(count); + originalVertices.resize(count); + for (int32_t i = 0; i < count; i++) { + Vertex* v = vertexPool.newObject(); + v->edges = NULL; + v->point = points[i]; + v->copy = -1; + originalVertices[i] = v; + } + + points.clear(); + + edgePool.reset(); + edgePool.setArraySize(6 * count); + + usedEdgePairs = 0; + maxUsedEdgePairs = 0; + + mergeStamp = -3; + + IntermediateHull hull; + computeInternal(0, count, hull); + vertexList = hull.minXy; +#ifdef DEBUG_CONVEX_HULL + printf("max. edges %d (3v = %d)", maxUsedEdgePairs, 3 * count); +#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) +{ + return toBtVector(face->dir0).cross(toBtVector(face->dir1)).normalized(); +} + +btVector3 btConvexHullInternal::getCoordinates(const Vertex* v) +{ + btVector3 p; + p[medAxis] = v->xvalue(); + p[maxAxis] = v->yvalue(); + p[minAxis] = v->zvalue(); + return p * scaling + center; +} + +btScalar btConvexHullInternal::shrink(btScalar amount, btScalar clampAmount) +{ + if (!vertexList) { + return 0; + } + int32_t stamp = --mergeStamp; + btAlignedObjectArray<Vertex*> stack; + vertexList->copy = stamp; + stack.push_back(vertexList); + btAlignedObjectArray<Face*> faces; + + Point32 ref = vertexList->point; + Int128 hullCenterX(0, 0); + Int128 hullCenterY(0, 0); + Int128 hullCenterZ(0, 0); + Int128 volume(0, 0); + + while (stack.size() > 0) { + Vertex* v = stack[stack.size() - 1]; + stack.pop_back(); + Edge* e = v->edges; + if (e) { + do { + if (e->target->copy != stamp) { + e->target->copy = stamp; + stack.push_back(e->target); + } + if (e->copy != stamp) { + Face* face = facePool.newObject(); + face->init(e->target, e->reverse->prev->target, v); + faces.push_back(face); + Edge* f = e; + + Vertex* a = NULL; + Vertex* b = NULL; + do { + if (a && b) { + int64_t vol = (v->point - ref).dot((a->point - ref).cross(b->point - ref)); + btAssert(vol >= 0); + Point32 c = v->point + a->point + b->point + ref; + hullCenterX += vol * c.x; + hullCenterY += vol * c.y; + hullCenterZ += vol * c.z; + volume += vol; + } + + btAssert(f->copy != stamp); + f->copy = stamp; + f->face = face; + + a = b; + b = f->target; + + f = f->reverse->prev; + } while (f != e); + } + e = e->next; + } while (e != v->edges); + } + } + + if (volume.getSign() <= 0) { + return 0; + } + + btVector3 hullCenter; + hullCenter[medAxis] = hullCenterX.toScalar(); + hullCenter[maxAxis] = hullCenterY.toScalar(); + hullCenter[minAxis] = hullCenterZ.toScalar(); + hullCenter /= 4 * volume.toScalar(); + hullCenter *= scaling; + + int32_t faceCount = faces.size(); + + if (clampAmount > 0) { + btScalar minDist = SIMD_INFINITY; + for (int32_t i = 0; i < faceCount; i++) { + btVector3 normal = getBtNormal(faces[i]); + btScalar dist = normal.dot(toBtVector(faces[i]->origin) - hullCenter); + if (dist < minDist) { + minDist = dist; + } + } + + if (minDist <= 0) { + return 0; + } + + amount = btMin(amount, minDist * clampAmount); + } + + uint32_t seed = 243703; + for (int32_t i = 0; i < faceCount; i++, seed = 1664525 * seed + 1013904223) { + btSwap(faces[i], faces[seed % faceCount]); + } + + for (int32_t i = 0; i < faceCount; i++) { + if (!shiftFace(faces[i], amount, stack)) { + return -amount; + } + } + + return amount; +} + +bool btConvexHullInternal::shiftFace(Face* face, btScalar amount, btAlignedObjectArray<Vertex*> stack) +{ + btVector3 origShift = getBtNormal(face) * -amount; + if (scaling[0] != 0) { + origShift[0] /= scaling[0]; + } + if (scaling[1] != 0) { + origShift[1] /= scaling[1]; + } + if (scaling[2] != 0) { + origShift[2] /= scaling[2]; + } + Point32 shift((int32_t)origShift[medAxis], (int32_t)origShift[maxAxis], (int32_t)origShift[minAxis]); + if (shift.isZero()) { + return true; + } + Point64 normal = face->getNormal(); +#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); + int32_t cmp = optDot.compare(shiftedDot); +#ifdef SHOW_ITERATIONS + int32_t 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) { + int32_t c = dot.compare(shiftedDot); + optDot = dot; + e = e->reverse; + startEdge = e; + if (c < 0) { + intersection = e; + break; + } + cmp = c; + } + e = e->prev; + } while (e != startEdge); + + if (!intersection) { + return false; + } + } + else { + Edge* e = startEdge; + do { +#ifdef SHOW_ITERATIONS + n++; +#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 + int32_t 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; + } + + int32_t 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 + int32_t pos = 0; + while (pos < stack.size()) { + int32_t end = stack.size(); + while (pos < end) { + Vertex* kept = stack[pos++]; +#ifdef DEBUG_CONVEX_HULL + kept->print(); +#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 int32_t getVertexCopy(btConvexHullInternal::Vertex* vertex, btAlignedObjectArray<btConvexHullInternal::Vertex*>& vertices) +{ + int32_t index = vertex->copy; + if (index < 0) { + index = vertices.size(); + vertex->copy = index; + vertices.push_back(vertex); +#ifdef DEBUG_CONVEX_HULL + printf("Vertex %d gets index *%d\n", vertex->point.index, index); +#endif + } + return index; +} + +btScalar btConvexHullComputer::compute(const void* coords, bool doubleCoords, int32_t stride, int32_t count, btScalar shrink, btScalar shrinkClamp) +{ + if (count <= 0) { + vertices.clear(); + edges.clear(); + faces.clear(); + return 0; + } + + btConvexHullInternal hull; + hull.compute(coords, doubleCoords, stride, count); + + btScalar shift = 0; + if ((shrink > 0) && ((shift = hull.shrink(shrink, shrinkClamp)) < 0)) { + vertices.clear(); + edges.clear(); + faces.clear(); + return shift; + } + + vertices.resize(0); + edges.resize(0); + faces.resize(0); + + btAlignedObjectArray<btConvexHullInternal::Vertex*> oldVertices; + getVertexCopy(hull.vertexList, oldVertices); + int32_t copied = 0; + while (copied < oldVertices.size()) { + btConvexHullInternal::Vertex* v = oldVertices[copied]; + vertices.push_back(hull.getCoordinates(v)); + btConvexHullInternal::Edge* firstEdge = v->edges; + if (firstEdge) { + int32_t firstCopy = -1; + int32_t prevCopy = -1; + btConvexHullInternal::Edge* e = firstEdge; + do { + if (e->copy < 0) { + int32_t s = edges.size(); + edges.push_back(Edge()); + edges.push_back(Edge()); + Edge* c = &edges[s]; + Edge* r = &edges[s + 1]; + e->copy = s; + e->reverse->copy = s + 1; + c->reverse = 1; + r->reverse = -1; + c->targetVertex = getVertexCopy(e->target, oldVertices); + r->targetVertex = copied; +#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 (int32_t i = 0; i < copied; i++) { + btConvexHullInternal::Vertex* v = oldVertices[i]; + btConvexHullInternal::Edge* firstEdge = v->edges; + if (firstEdge) { + btConvexHullInternal::Edge* e = firstEdge; + do { + if (e->copy >= 0) { +#ifdef DEBUG_CONVEX_HULL + printf("Vertex *%d has edge to *%d\n", i, edges[e->copy].getTargetVertex()); +#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; +} + +// -- GODOT start -- +}; // namespace VHACD +// -- GODOT end -- diff --git a/thirdparty/vhacd/src/vhacdICHull.cpp b/thirdparty/vhacd/src/vhacdICHull.cpp new file mode 100644 index 0000000000..989587c124 --- /dev/null +++ b/thirdparty/vhacd/src/vhacdICHull.cpp @@ -0,0 +1,731 @@ +/* Copyright (c) 2011 Khaled Mamou (kmamou at gmail dot com) + All rights reserved. + + + Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + + 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + + 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. + + 3. The names of the contributors may not be used to endorse or promote products derived from this software without specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ +#include "vhacdICHull.h" +#include <limits> + +#ifdef _MSC_VER +#pragma warning(disable:4456 4706) +#endif + + +namespace VHACD { +const double ICHull::sc_eps = 1.0e-15; +const int32_t ICHull::sc_dummyIndex = std::numeric_limits<int32_t>::max(); +ICHull::ICHull() +{ + m_isFlat = false; +} +bool ICHull::AddPoints(const Vec3<double>* points, size_t nPoints) +{ + if (!points) { + return false; + } + CircularListElement<TMMVertex>* vertex = NULL; + for (size_t i = 0; i < nPoints; i++) { + vertex = m_mesh.AddVertex(); + vertex->GetData().m_pos.X() = points[i].X(); + vertex->GetData().m_pos.Y() = points[i].Y(); + vertex->GetData().m_pos.Z() = points[i].Z(); + vertex->GetData().m_name = static_cast<int32_t>(i); + } + return true; +} +bool ICHull::AddPoint(const Vec3<double>& point, int32_t id) +{ + if (AddPoints(&point, 1)) { + m_mesh.m_vertices.GetData().m_name = id; + return true; + } + return false; +} + +ICHullError ICHull::Process() +{ + uint32_t addedPoints = 0; + if (m_mesh.GetNVertices() < 3) { + return ICHullErrorNotEnoughPoints; + } + if (m_mesh.GetNVertices() == 3) { + m_isFlat = true; + CircularListElement<TMMTriangle>* t1 = m_mesh.AddTriangle(); + CircularListElement<TMMTriangle>* t2 = m_mesh.AddTriangle(); + CircularListElement<TMMVertex>* v0 = m_mesh.m_vertices.GetHead(); + CircularListElement<TMMVertex>* v1 = v0->GetNext(); + CircularListElement<TMMVertex>* v2 = v1->GetNext(); + // Compute the normal to the plane + Vec3<double> p0 = v0->GetData().m_pos; + Vec3<double> p1 = v1->GetData().m_pos; + Vec3<double> p2 = v2->GetData().m_pos; + m_normal = (p1 - p0) ^ (p2 - p0); + m_normal.Normalize(); + t1->GetData().m_vertices[0] = v0; + t1->GetData().m_vertices[1] = v1; + t1->GetData().m_vertices[2] = v2; + t2->GetData().m_vertices[0] = v1; + t2->GetData().m_vertices[1] = v2; + t2->GetData().m_vertices[2] = v2; + return ICHullErrorOK; + } + if (m_isFlat) { + m_mesh.m_edges.Clear(); + m_mesh.m_triangles.Clear(); + m_isFlat = false; + } + if (m_mesh.GetNTriangles() == 0) // we have to create the first polyhedron + { + ICHullError res = DoubleTriangle(); + if (res != ICHullErrorOK) { + return res; + } + else { + addedPoints += 3; + } + } + CircularList<TMMVertex>& vertices = m_mesh.GetVertices(); + // go to the first added and not processed vertex + while (!(vertices.GetHead()->GetPrev()->GetData().m_tag)) { + vertices.Prev(); + } + while (!vertices.GetData().m_tag) // not processed + { + vertices.GetData().m_tag = true; + if (ProcessPoint()) { + addedPoints++; + CleanUp(addedPoints); + vertices.Next(); + if (!GetMesh().CheckConsistancy()) { + size_t nV = m_mesh.GetNVertices(); + CircularList<TMMVertex>& vertices = m_mesh.GetVertices(); + for (size_t v = 0; v < nV; ++v) { + if (vertices.GetData().m_name == sc_dummyIndex) { + vertices.Delete(); + break; + } + vertices.Next(); + } + return ICHullErrorInconsistent; + } + } + } + if (m_isFlat) { + SArray<CircularListElement<TMMTriangle>*> trianglesToDuplicate; + size_t nT = m_mesh.GetNTriangles(); + for (size_t f = 0; f < nT; f++) { + TMMTriangle& currentTriangle = m_mesh.m_triangles.GetHead()->GetData(); + if (currentTriangle.m_vertices[0]->GetData().m_name == sc_dummyIndex || currentTriangle.m_vertices[1]->GetData().m_name == sc_dummyIndex || currentTriangle.m_vertices[2]->GetData().m_name == sc_dummyIndex) { + m_trianglesToDelete.PushBack(m_mesh.m_triangles.GetHead()); + for (int32_t k = 0; k < 3; k++) { + for (int32_t h = 0; h < 2; h++) { + if (currentTriangle.m_edges[k]->GetData().m_triangles[h] == m_mesh.m_triangles.GetHead()) { + currentTriangle.m_edges[k]->GetData().m_triangles[h] = 0; + break; + } + } + } + } + else { + trianglesToDuplicate.PushBack(m_mesh.m_triangles.GetHead()); + } + m_mesh.m_triangles.Next(); + } + size_t nE = m_mesh.GetNEdges(); + for (size_t e = 0; e < nE; e++) { + TMMEdge& currentEdge = m_mesh.m_edges.GetHead()->GetData(); + if (currentEdge.m_triangles[0] == 0 && currentEdge.m_triangles[1] == 0) { + m_edgesToDelete.PushBack(m_mesh.m_edges.GetHead()); + } + m_mesh.m_edges.Next(); + } + size_t nV = m_mesh.GetNVertices(); + CircularList<TMMVertex>& vertices = m_mesh.GetVertices(); + for (size_t v = 0; v < nV; ++v) { + if (vertices.GetData().m_name == sc_dummyIndex) { + vertices.Delete(); + } + else { + vertices.GetData().m_tag = false; + vertices.Next(); + } + } + CleanEdges(); + CleanTriangles(); + CircularListElement<TMMTriangle>* newTriangle; + for (size_t t = 0; t < trianglesToDuplicate.Size(); t++) { + newTriangle = m_mesh.AddTriangle(); + newTriangle->GetData().m_vertices[0] = trianglesToDuplicate[t]->GetData().m_vertices[1]; + newTriangle->GetData().m_vertices[1] = trianglesToDuplicate[t]->GetData().m_vertices[0]; + newTriangle->GetData().m_vertices[2] = trianglesToDuplicate[t]->GetData().m_vertices[2]; + } + } + return ICHullErrorOK; +} +ICHullError ICHull::Process(const uint32_t nPointsCH, + const double minVolume) +{ + uint32_t addedPoints = 0; + if (nPointsCH < 3 || m_mesh.GetNVertices() < 3) { + return ICHullErrorNotEnoughPoints; + } + if (m_mesh.GetNVertices() == 3) { + m_isFlat = true; + CircularListElement<TMMTriangle>* t1 = m_mesh.AddTriangle(); + CircularListElement<TMMTriangle>* t2 = m_mesh.AddTriangle(); + CircularListElement<TMMVertex>* v0 = m_mesh.m_vertices.GetHead(); + CircularListElement<TMMVertex>* v1 = v0->GetNext(); + CircularListElement<TMMVertex>* v2 = v1->GetNext(); + // Compute the normal to the plane + Vec3<double> p0 = v0->GetData().m_pos; + Vec3<double> p1 = v1->GetData().m_pos; + Vec3<double> p2 = v2->GetData().m_pos; + m_normal = (p1 - p0) ^ (p2 - p0); + m_normal.Normalize(); + t1->GetData().m_vertices[0] = v0; + t1->GetData().m_vertices[1] = v1; + t1->GetData().m_vertices[2] = v2; + t2->GetData().m_vertices[0] = v1; + t2->GetData().m_vertices[1] = v0; + t2->GetData().m_vertices[2] = v2; + return ICHullErrorOK; + } + + if (m_isFlat) { + m_mesh.m_triangles.Clear(); + m_mesh.m_edges.Clear(); + m_isFlat = false; + } + + if (m_mesh.GetNTriangles() == 0) // we have to create the first polyhedron + { + ICHullError res = DoubleTriangle(); + if (res != ICHullErrorOK) { + return res; + } + else { + addedPoints += 3; + } + } + CircularList<TMMVertex>& vertices = m_mesh.GetVertices(); + while (!vertices.GetData().m_tag && addedPoints < nPointsCH) // not processed + { + if (!FindMaxVolumePoint((addedPoints > 4) ? minVolume : 0.0)) { + break; + } + vertices.GetData().m_tag = true; + if (ProcessPoint()) { + addedPoints++; + CleanUp(addedPoints); + if (!GetMesh().CheckConsistancy()) { + size_t nV = m_mesh.GetNVertices(); + CircularList<TMMVertex>& vertices = m_mesh.GetVertices(); + for (size_t v = 0; v < nV; ++v) { + if (vertices.GetData().m_name == sc_dummyIndex) { + vertices.Delete(); + break; + } + vertices.Next(); + } + return ICHullErrorInconsistent; + } + vertices.Next(); + } + } + // delete remaining points + while (!vertices.GetData().m_tag) { + vertices.Delete(); + } + if (m_isFlat) { + SArray<CircularListElement<TMMTriangle>*> trianglesToDuplicate; + size_t nT = m_mesh.GetNTriangles(); + for (size_t f = 0; f < nT; f++) { + TMMTriangle& currentTriangle = m_mesh.m_triangles.GetHead()->GetData(); + if (currentTriangle.m_vertices[0]->GetData().m_name == sc_dummyIndex || currentTriangle.m_vertices[1]->GetData().m_name == sc_dummyIndex || currentTriangle.m_vertices[2]->GetData().m_name == sc_dummyIndex) { + m_trianglesToDelete.PushBack(m_mesh.m_triangles.GetHead()); + for (int32_t k = 0; k < 3; k++) { + for (int32_t h = 0; h < 2; h++) { + if (currentTriangle.m_edges[k]->GetData().m_triangles[h] == m_mesh.m_triangles.GetHead()) { + currentTriangle.m_edges[k]->GetData().m_triangles[h] = 0; + break; + } + } + } + } + else { + trianglesToDuplicate.PushBack(m_mesh.m_triangles.GetHead()); + } + m_mesh.m_triangles.Next(); + } + size_t nE = m_mesh.GetNEdges(); + for (size_t e = 0; e < nE; e++) { + TMMEdge& currentEdge = m_mesh.m_edges.GetHead()->GetData(); + if (currentEdge.m_triangles[0] == 0 && currentEdge.m_triangles[1] == 0) { + m_edgesToDelete.PushBack(m_mesh.m_edges.GetHead()); + } + m_mesh.m_edges.Next(); + } + size_t nV = m_mesh.GetNVertices(); + CircularList<TMMVertex>& vertices = m_mesh.GetVertices(); + for (size_t v = 0; v < nV; ++v) { + if (vertices.GetData().m_name == sc_dummyIndex) { + vertices.Delete(); + } + else { + vertices.GetData().m_tag = false; + vertices.Next(); + } + } + CleanEdges(); + CleanTriangles(); + CircularListElement<TMMTriangle>* newTriangle; + for (size_t t = 0; t < trianglesToDuplicate.Size(); t++) { + newTriangle = m_mesh.AddTriangle(); + newTriangle->GetData().m_vertices[0] = trianglesToDuplicate[t]->GetData().m_vertices[1]; + newTriangle->GetData().m_vertices[1] = trianglesToDuplicate[t]->GetData().m_vertices[0]; + newTriangle->GetData().m_vertices[2] = trianglesToDuplicate[t]->GetData().m_vertices[2]; + } + } + return ICHullErrorOK; +} +bool ICHull::FindMaxVolumePoint(const double minVolume) +{ + CircularList<TMMVertex>& vertices = m_mesh.GetVertices(); + CircularListElement<TMMVertex>* vMaxVolume = 0; + CircularListElement<TMMVertex>* vHeadPrev = vertices.GetHead()->GetPrev(); + + double maxVolume = minVolume; + double volume = 0.0; + while (!vertices.GetData().m_tag) // not processed + { + if (ComputePointVolume(volume, false)) { + if (maxVolume < volume) { + maxVolume = volume; + vMaxVolume = vertices.GetHead(); + } + vertices.Next(); + } + } + CircularListElement<TMMVertex>* vHead = vHeadPrev->GetNext(); + vertices.GetHead() = vHead; + if (!vMaxVolume) { + return false; + } + if (vMaxVolume != vHead) { + Vec3<double> pos = vHead->GetData().m_pos; + int32_t id = vHead->GetData().m_name; + vHead->GetData().m_pos = vMaxVolume->GetData().m_pos; + vHead->GetData().m_name = vMaxVolume->GetData().m_name; + vMaxVolume->GetData().m_pos = pos; + vHead->GetData().m_name = id; + } + return true; +} +ICHullError ICHull::DoubleTriangle() +{ + // find three non colinear points + m_isFlat = false; + CircularList<TMMVertex>& vertices = m_mesh.GetVertices(); + CircularListElement<TMMVertex>* v0 = vertices.GetHead(); + while (Colinear(v0->GetData().m_pos, + v0->GetNext()->GetData().m_pos, + v0->GetNext()->GetNext()->GetData().m_pos)) { + if ((v0 = v0->GetNext()) == vertices.GetHead()) { + return ICHullErrorCoplanarPoints; + } + } + CircularListElement<TMMVertex>* v1 = v0->GetNext(); + CircularListElement<TMMVertex>* v2 = v1->GetNext(); + // mark points as processed + v0->GetData().m_tag = v1->GetData().m_tag = v2->GetData().m_tag = true; + + // create two triangles + CircularListElement<TMMTriangle>* f0 = MakeFace(v0, v1, v2, 0); + MakeFace(v2, v1, v0, f0); + + // find a fourth non-coplanar point to form tetrahedron + CircularListElement<TMMVertex>* v3 = v2->GetNext(); + vertices.GetHead() = v3; + + double vol = ComputeVolume4(v0->GetData().m_pos, v1->GetData().m_pos, v2->GetData().m_pos, v3->GetData().m_pos); + while (fabs(vol) < sc_eps && !v3->GetNext()->GetData().m_tag) { + v3 = v3->GetNext(); + vol = ComputeVolume4(v0->GetData().m_pos, v1->GetData().m_pos, v2->GetData().m_pos, v3->GetData().m_pos); + } + if (fabs(vol) < sc_eps) { + // compute the barycenter + Vec3<double> bary(0.0, 0.0, 0.0); + CircularListElement<TMMVertex>* vBary = v0; + do { + bary += vBary->GetData().m_pos; + } while ((vBary = vBary->GetNext()) != v0); + bary /= static_cast<double>(vertices.GetSize()); + + // Compute the normal to the plane + Vec3<double> p0 = v0->GetData().m_pos; + Vec3<double> p1 = v1->GetData().m_pos; + Vec3<double> p2 = v2->GetData().m_pos; + m_normal = (p1 - p0) ^ (p2 - p0); + m_normal.Normalize(); + // add dummy vertex placed at (bary + normal) + vertices.GetHead() = v2; + Vec3<double> newPt = bary + m_normal; + AddPoint(newPt, sc_dummyIndex); + m_isFlat = true; + return ICHullErrorOK; + } + else if (v3 != vertices.GetHead()) { + TMMVertex temp; + temp.m_name = v3->GetData().m_name; + temp.m_pos = v3->GetData().m_pos; + v3->GetData().m_name = vertices.GetHead()->GetData().m_name; + v3->GetData().m_pos = vertices.GetHead()->GetData().m_pos; + vertices.GetHead()->GetData().m_name = temp.m_name; + vertices.GetHead()->GetData().m_pos = temp.m_pos; + } + return ICHullErrorOK; +} +CircularListElement<TMMTriangle>* ICHull::MakeFace(CircularListElement<TMMVertex>* v0, + CircularListElement<TMMVertex>* v1, + CircularListElement<TMMVertex>* v2, + CircularListElement<TMMTriangle>* fold) +{ + CircularListElement<TMMEdge>* e0; + CircularListElement<TMMEdge>* e1; + CircularListElement<TMMEdge>* e2; + int32_t index = 0; + if (!fold) // if first face to be created + { + e0 = m_mesh.AddEdge(); // create the three edges + e1 = m_mesh.AddEdge(); + e2 = m_mesh.AddEdge(); + } + else // otherwise re-use existing edges (in reverse order) + { + e0 = fold->GetData().m_edges[2]; + e1 = fold->GetData().m_edges[1]; + e2 = fold->GetData().m_edges[0]; + index = 1; + } + e0->GetData().m_vertices[0] = v0; + e0->GetData().m_vertices[1] = v1; + e1->GetData().m_vertices[0] = v1; + e1->GetData().m_vertices[1] = v2; + e2->GetData().m_vertices[0] = v2; + e2->GetData().m_vertices[1] = v0; + // create the new face + CircularListElement<TMMTriangle>* f = m_mesh.AddTriangle(); + f->GetData().m_edges[0] = e0; + f->GetData().m_edges[1] = e1; + f->GetData().m_edges[2] = e2; + f->GetData().m_vertices[0] = v0; + f->GetData().m_vertices[1] = v1; + f->GetData().m_vertices[2] = v2; + // link edges to face f + e0->GetData().m_triangles[index] = e1->GetData().m_triangles[index] = e2->GetData().m_triangles[index] = f; + return f; +} +CircularListElement<TMMTriangle>* ICHull::MakeConeFace(CircularListElement<TMMEdge>* e, CircularListElement<TMMVertex>* p) +{ + // create two new edges if they don't already exist + CircularListElement<TMMEdge>* newEdges[2]; + for (int32_t i = 0; i < 2; ++i) { + if (!(newEdges[i] = e->GetData().m_vertices[i]->GetData().m_duplicate)) { // if the edge doesn't exits add it and mark the vertex as duplicated + newEdges[i] = m_mesh.AddEdge(); + newEdges[i]->GetData().m_vertices[0] = e->GetData().m_vertices[i]; + newEdges[i]->GetData().m_vertices[1] = p; + e->GetData().m_vertices[i]->GetData().m_duplicate = newEdges[i]; + } + } + // make the new face + CircularListElement<TMMTriangle>* newFace = m_mesh.AddTriangle(); + newFace->GetData().m_edges[0] = e; + newFace->GetData().m_edges[1] = newEdges[0]; + newFace->GetData().m_edges[2] = newEdges[1]; + MakeCCW(newFace, e, p); + for (int32_t i = 0; i < 2; ++i) { + for (int32_t j = 0; j < 2; ++j) { + if (!newEdges[i]->GetData().m_triangles[j]) { + newEdges[i]->GetData().m_triangles[j] = newFace; + break; + } + } + } + return newFace; +} +bool ICHull::ComputePointVolume(double& totalVolume, bool markVisibleFaces) +{ + // mark visible faces + CircularListElement<TMMTriangle>* fHead = m_mesh.GetTriangles().GetHead(); + CircularListElement<TMMTriangle>* f = fHead; + CircularList<TMMVertex>& vertices = m_mesh.GetVertices(); + CircularListElement<TMMVertex>* vertex0 = vertices.GetHead(); + bool visible = false; + Vec3<double> pos0 = Vec3<double>(vertex0->GetData().m_pos.X(), + vertex0->GetData().m_pos.Y(), + vertex0->GetData().m_pos.Z()); + double vol = 0.0; + totalVolume = 0.0; + Vec3<double> ver0, ver1, ver2; + do { + ver0.X() = f->GetData().m_vertices[0]->GetData().m_pos.X(); + ver0.Y() = f->GetData().m_vertices[0]->GetData().m_pos.Y(); + ver0.Z() = f->GetData().m_vertices[0]->GetData().m_pos.Z(); + ver1.X() = f->GetData().m_vertices[1]->GetData().m_pos.X(); + ver1.Y() = f->GetData().m_vertices[1]->GetData().m_pos.Y(); + ver1.Z() = f->GetData().m_vertices[1]->GetData().m_pos.Z(); + ver2.X() = f->GetData().m_vertices[2]->GetData().m_pos.X(); + ver2.Y() = f->GetData().m_vertices[2]->GetData().m_pos.Y(); + ver2.Z() = f->GetData().m_vertices[2]->GetData().m_pos.Z(); + vol = ComputeVolume4(ver0, ver1, ver2, pos0); + if (vol < -sc_eps) { + vol = fabs(vol); + totalVolume += vol; + if (markVisibleFaces) { + f->GetData().m_visible = true; + m_trianglesToDelete.PushBack(f); + } + visible = true; + } + f = f->GetNext(); + } while (f != fHead); + + if (m_trianglesToDelete.Size() == m_mesh.m_triangles.GetSize()) { + for (size_t i = 0; i < m_trianglesToDelete.Size(); i++) { + m_trianglesToDelete[i]->GetData().m_visible = false; + } + visible = false; + } + // if no faces visible from p then p is inside the hull + if (!visible && markVisibleFaces) { + vertices.Delete(); + m_trianglesToDelete.Resize(0); + return false; + } + return true; +} +bool ICHull::ProcessPoint() +{ + double totalVolume = 0.0; + if (!ComputePointVolume(totalVolume, true)) { + return false; + } + // Mark edges in interior of visible region for deletion. + // Create a new face based on each border edge + CircularListElement<TMMVertex>* v0 = m_mesh.GetVertices().GetHead(); + CircularListElement<TMMEdge>* eHead = m_mesh.GetEdges().GetHead(); + CircularListElement<TMMEdge>* e = eHead; + CircularListElement<TMMEdge>* tmp = 0; + int32_t nvisible = 0; + m_edgesToDelete.Resize(0); + m_edgesToUpdate.Resize(0); + do { + tmp = e->GetNext(); + nvisible = 0; + for (int32_t k = 0; k < 2; k++) { + if (e->GetData().m_triangles[k]->GetData().m_visible) { + nvisible++; + } + } + if (nvisible == 2) { + m_edgesToDelete.PushBack(e); + } + else if (nvisible == 1) { + e->GetData().m_newFace = MakeConeFace(e, v0); + m_edgesToUpdate.PushBack(e); + } + e = tmp; + } while (e != eHead); + return true; +} +bool ICHull::MakeCCW(CircularListElement<TMMTriangle>* f, + CircularListElement<TMMEdge>* e, + CircularListElement<TMMVertex>* v) +{ + // the visible face adjacent to e + CircularListElement<TMMTriangle>* fv; + if (e->GetData().m_triangles[0]->GetData().m_visible) { + fv = e->GetData().m_triangles[0]; + } + else { + fv = e->GetData().m_triangles[1]; + } + + // set vertex[0] and vertex[1] to have the same orientation as the corresponding vertices of fv. + int32_t i; // index of e->m_vertices[0] in fv + CircularListElement<TMMVertex>* v0 = e->GetData().m_vertices[0]; + CircularListElement<TMMVertex>* v1 = e->GetData().m_vertices[1]; + for (i = 0; fv->GetData().m_vertices[i] != v0; i++) + ; + + if (fv->GetData().m_vertices[(i + 1) % 3] != e->GetData().m_vertices[1]) { + f->GetData().m_vertices[0] = v1; + f->GetData().m_vertices[1] = v0; + } + else { + f->GetData().m_vertices[0] = v0; + f->GetData().m_vertices[1] = v1; + // swap edges + CircularListElement<TMMEdge>* tmp = f->GetData().m_edges[0]; + f->GetData().m_edges[0] = f->GetData().m_edges[1]; + f->GetData().m_edges[1] = tmp; + } + f->GetData().m_vertices[2] = v; + return true; +} +bool ICHull::CleanUp(uint32_t& addedPoints) +{ + bool r0 = CleanEdges(); + bool r1 = CleanTriangles(); + bool r2 = CleanVertices(addedPoints); + return r0 && r1 && r2; +} +bool ICHull::CleanEdges() +{ + // integrate the new faces into the data structure + CircularListElement<TMMEdge>* e; + const size_t ne_update = m_edgesToUpdate.Size(); + for (size_t i = 0; i < ne_update; ++i) { + e = m_edgesToUpdate[i]; + if (e->GetData().m_newFace) { + if (e->GetData().m_triangles[0]->GetData().m_visible) { + e->GetData().m_triangles[0] = e->GetData().m_newFace; + } + else { + e->GetData().m_triangles[1] = e->GetData().m_newFace; + } + e->GetData().m_newFace = 0; + } + } + // delete edges maked for deletion + CircularList<TMMEdge>& edges = m_mesh.GetEdges(); + const size_t ne_delete = m_edgesToDelete.Size(); + for (size_t i = 0; i < ne_delete; ++i) { + edges.Delete(m_edgesToDelete[i]); + } + m_edgesToDelete.Resize(0); + m_edgesToUpdate.Resize(0); + return true; +} +bool ICHull::CleanTriangles() +{ + CircularList<TMMTriangle>& triangles = m_mesh.GetTriangles(); + const size_t nt_delete = m_trianglesToDelete.Size(); + for (size_t i = 0; i < nt_delete; ++i) { + triangles.Delete(m_trianglesToDelete[i]); + } + m_trianglesToDelete.Resize(0); + return true; +} +bool ICHull::CleanVertices(uint32_t& addedPoints) +{ + // mark all vertices incident to some undeleted edge as on the hull + CircularList<TMMEdge>& edges = m_mesh.GetEdges(); + CircularListElement<TMMEdge>* e = edges.GetHead(); + size_t nE = edges.GetSize(); + for (size_t i = 0; i < nE; i++) { + e->GetData().m_vertices[0]->GetData().m_onHull = true; + e->GetData().m_vertices[1]->GetData().m_onHull = true; + e = e->GetNext(); + } + // delete all the vertices that have been processed but are not on the hull + CircularList<TMMVertex>& vertices = m_mesh.GetVertices(); + CircularListElement<TMMVertex>* vHead = vertices.GetHead(); + CircularListElement<TMMVertex>* v = vHead; + v = v->GetPrev(); + do { + if (v->GetData().m_tag && !v->GetData().m_onHull) { + CircularListElement<TMMVertex>* tmp = v->GetPrev(); + vertices.Delete(v); + v = tmp; + addedPoints--; + } + else { + v->GetData().m_duplicate = 0; + v->GetData().m_onHull = false; + v = v->GetPrev(); + } + } while (v->GetData().m_tag && v != vHead); + return true; +} +void ICHull::Clear() +{ + m_mesh.Clear(); + m_edgesToDelete.Resize(0); + m_edgesToUpdate.Resize(0); + m_trianglesToDelete.Resize(0); + m_isFlat = false; +} +const ICHull& ICHull::operator=(ICHull& rhs) +{ + if (&rhs != this) { + m_mesh.Copy(rhs.m_mesh); + m_edgesToDelete = rhs.m_edgesToDelete; + m_edgesToUpdate = rhs.m_edgesToUpdate; + m_trianglesToDelete = rhs.m_trianglesToDelete; + m_isFlat = rhs.m_isFlat; + } + return (*this); +} +bool ICHull::IsInside(const Vec3<double>& pt0, const double eps) +{ + const Vec3<double> pt(pt0.X(), pt0.Y(), pt0.Z()); + if (m_isFlat) { + size_t nT = m_mesh.m_triangles.GetSize(); + Vec3<double> ver0, ver1, ver2, a, b, c; + double u, v; + for (size_t t = 0; t < nT; t++) { + ver0.X() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[0]->GetData().m_pos.X(); + ver0.Y() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[0]->GetData().m_pos.Y(); + ver0.Z() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[0]->GetData().m_pos.Z(); + ver1.X() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[1]->GetData().m_pos.X(); + ver1.Y() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[1]->GetData().m_pos.Y(); + ver1.Z() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[1]->GetData().m_pos.Z(); + ver2.X() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[2]->GetData().m_pos.X(); + ver2.Y() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[2]->GetData().m_pos.Y(); + ver2.Z() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[2]->GetData().m_pos.Z(); + a = ver1 - ver0; + b = ver2 - ver0; + c = pt - ver0; + u = c * a; + v = c * b; + if (u >= 0.0 && u <= 1.0 && v >= 0.0 && u + v <= 1.0) { + return true; + } + m_mesh.m_triangles.Next(); + } + return false; + } + else { + size_t nT = m_mesh.m_triangles.GetSize(); + Vec3<double> ver0, ver1, ver2; + double vol; + for (size_t t = 0; t < nT; t++) { + ver0.X() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[0]->GetData().m_pos.X(); + ver0.Y() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[0]->GetData().m_pos.Y(); + ver0.Z() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[0]->GetData().m_pos.Z(); + ver1.X() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[1]->GetData().m_pos.X(); + ver1.Y() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[1]->GetData().m_pos.Y(); + ver1.Z() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[1]->GetData().m_pos.Z(); + ver2.X() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[2]->GetData().m_pos.X(); + ver2.Y() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[2]->GetData().m_pos.Y(); + ver2.Z() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[2]->GetData().m_pos.Z(); + vol = ComputeVolume4(ver0, ver1, ver2, pt); + if (vol < eps) { + return false; + } + m_mesh.m_triangles.Next(); + } + return true; + } +} +} diff --git a/thirdparty/vhacd/src/vhacdManifoldMesh.cpp b/thirdparty/vhacd/src/vhacdManifoldMesh.cpp new file mode 100644 index 0000000000..7aac9c0d17 --- /dev/null +++ b/thirdparty/vhacd/src/vhacdManifoldMesh.cpp @@ -0,0 +1,202 @@ +/* Copyright (c) 2011 Khaled Mamou (kmamou at gmail dot com) + All rights reserved. + + + Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + + 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + + 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. + + 3. The names of the contributors may not be used to endorse or promote products derived from this software without specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ +#include "vhacdManifoldMesh.h" +namespace VHACD { +TMMVertex::TMMVertex(void) +{ + Initialize(); +} +void TMMVertex::Initialize() +{ + m_name = 0; + m_id = 0; + m_duplicate = 0; + m_onHull = false; + m_tag = false; +} + +TMMVertex::~TMMVertex(void) +{ +} +TMMEdge::TMMEdge(void) +{ + Initialize(); +} +void TMMEdge::Initialize() +{ + m_id = 0; + m_triangles[0] = m_triangles[1] = m_newFace = 0; + m_vertices[0] = m_vertices[1] = 0; +} +TMMEdge::~TMMEdge(void) +{ +} +void TMMTriangle::Initialize() +{ + m_id = 0; + for (int32_t i = 0; i < 3; i++) { + m_edges[i] = 0; + m_vertices[0] = 0; + } + m_visible = false; +} +TMMTriangle::TMMTriangle(void) +{ + Initialize(); +} +TMMTriangle::~TMMTriangle(void) +{ +} +TMMesh::TMMesh() +{ +} +TMMesh::~TMMesh(void) +{ +} +void TMMesh::GetIFS(Vec3<double>* const points, Vec3<int32_t>* const triangles) +{ + size_t nV = m_vertices.GetSize(); + size_t nT = m_triangles.GetSize(); + + for (size_t v = 0; v < nV; v++) { + points[v] = m_vertices.GetData().m_pos; + m_vertices.GetData().m_id = v; + m_vertices.Next(); + } + for (size_t f = 0; f < nT; f++) { + TMMTriangle& currentTriangle = m_triangles.GetData(); + triangles[f].X() = static_cast<int32_t>(currentTriangle.m_vertices[0]->GetData().m_id); + triangles[f].Y() = static_cast<int32_t>(currentTriangle.m_vertices[1]->GetData().m_id); + triangles[f].Z() = static_cast<int32_t>(currentTriangle.m_vertices[2]->GetData().m_id); + m_triangles.Next(); + } +} +void TMMesh::Clear() +{ + m_vertices.Clear(); + m_edges.Clear(); + m_triangles.Clear(); +} +void TMMesh::Copy(TMMesh& mesh) +{ + Clear(); + // updating the id's + size_t nV = mesh.m_vertices.GetSize(); + size_t nE = mesh.m_edges.GetSize(); + size_t nT = mesh.m_triangles.GetSize(); + for (size_t v = 0; v < nV; v++) { + mesh.m_vertices.GetData().m_id = v; + mesh.m_vertices.Next(); + } + for (size_t e = 0; e < nE; e++) { + mesh.m_edges.GetData().m_id = e; + mesh.m_edges.Next(); + } + for (size_t f = 0; f < nT; f++) { + mesh.m_triangles.GetData().m_id = f; + mesh.m_triangles.Next(); + } + // copying data + m_vertices = mesh.m_vertices; + m_edges = mesh.m_edges; + m_triangles = mesh.m_triangles; + + // generate mapping + CircularListElement<TMMVertex>** vertexMap = new CircularListElement<TMMVertex>*[nV]; + CircularListElement<TMMEdge>** edgeMap = new CircularListElement<TMMEdge>*[nE]; + CircularListElement<TMMTriangle>** triangleMap = new CircularListElement<TMMTriangle>*[nT]; + for (size_t v = 0; v < nV; v++) { + vertexMap[v] = m_vertices.GetHead(); + m_vertices.Next(); + } + for (size_t e = 0; e < nE; e++) { + edgeMap[e] = m_edges.GetHead(); + m_edges.Next(); + } + for (size_t f = 0; f < nT; f++) { + triangleMap[f] = m_triangles.GetHead(); + m_triangles.Next(); + } + + // updating pointers + for (size_t v = 0; v < nV; v++) { + if (vertexMap[v]->GetData().m_duplicate) { + vertexMap[v]->GetData().m_duplicate = edgeMap[vertexMap[v]->GetData().m_duplicate->GetData().m_id]; + } + } + for (size_t e = 0; e < nE; e++) { + if (edgeMap[e]->GetData().m_newFace) { + edgeMap[e]->GetData().m_newFace = triangleMap[edgeMap[e]->GetData().m_newFace->GetData().m_id]; + } + if (nT > 0) { + for (int32_t f = 0; f < 2; f++) { + if (edgeMap[e]->GetData().m_triangles[f]) { + edgeMap[e]->GetData().m_triangles[f] = triangleMap[edgeMap[e]->GetData().m_triangles[f]->GetData().m_id]; + } + } + } + for (int32_t v = 0; v < 2; v++) { + if (edgeMap[e]->GetData().m_vertices[v]) { + edgeMap[e]->GetData().m_vertices[v] = vertexMap[edgeMap[e]->GetData().m_vertices[v]->GetData().m_id]; + } + } + } + for (size_t f = 0; f < nT; f++) { + if (nE > 0) { + for (int32_t e = 0; e < 3; e++) { + if (triangleMap[f]->GetData().m_edges[e]) { + triangleMap[f]->GetData().m_edges[e] = edgeMap[triangleMap[f]->GetData().m_edges[e]->GetData().m_id]; + } + } + } + for (int32_t v = 0; v < 3; v++) { + if (triangleMap[f]->GetData().m_vertices[v]) { + triangleMap[f]->GetData().m_vertices[v] = vertexMap[triangleMap[f]->GetData().m_vertices[v]->GetData().m_id]; + } + } + } + delete[] vertexMap; + delete[] edgeMap; + delete[] triangleMap; +} +bool TMMesh::CheckConsistancy() +{ + size_t nE = m_edges.GetSize(); + size_t nT = m_triangles.GetSize(); + for (size_t e = 0; e < nE; e++) { + for (int32_t f = 0; f < 2; f++) { + if (!m_edges.GetHead()->GetData().m_triangles[f]) { + return false; + } + } + m_edges.Next(); + } + for (size_t f = 0; f < nT; f++) { + for (int32_t e = 0; e < 3; e++) { + int32_t found = 0; + for (int32_t k = 0; k < 2; k++) { + if (m_triangles.GetHead()->GetData().m_edges[e]->GetData().m_triangles[k] == m_triangles.GetHead()) { + found++; + } + } + if (found != 1) { + return false; + } + } + m_triangles.Next(); + } + return true; +} +}
\ No newline at end of file diff --git a/thirdparty/vhacd/src/vhacdMesh.cpp b/thirdparty/vhacd/src/vhacdMesh.cpp new file mode 100644 index 0000000000..5d03989fda --- /dev/null +++ b/thirdparty/vhacd/src/vhacdMesh.cpp @@ -0,0 +1,376 @@ +/* Copyright (c) 2011 Khaled Mamou (kmamou at gmail dot com) + All rights reserved. + + + Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + + 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + + 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. + + 3. The names of the contributors may not be used to endorse or promote products derived from this software without specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef _CRT_SECURE_NO_WARNINGS +#define _CRT_SECURE_NO_WARNINGS +#endif + +#include "btConvexHullComputer.h" +#include "vhacdMesh.h" +#include "FloatMath.h" +#include <fstream> +#include <iosfwd> +#include <iostream> +#include <stdio.h> +#include <stdlib.h> +#include <string> + +namespace VHACD { +Mesh::Mesh() +{ + m_diag = 1.0; +} +Mesh::~Mesh() +{ +} + +Vec3<double>& Mesh::ComputeCenter(void) +{ + const size_t nV = GetNPoints(); + if (nV) + { + double center[3]; + uint32_t pcount = uint32_t(GetNPoints()); + const double *points = GetPoints(); + uint32_t tcount = uint32_t(GetNTriangles()); + const uint32_t *indices = (const uint32_t *)GetTriangles(); + FLOAT_MATH::fm_computeCentroid(pcount, points, tcount, indices, center); + m_center.X() = center[0]; + m_center.Y() = center[1]; + m_center.Z() = center[2]; + m_minBB = GetPoint(0); + m_maxBB = GetPoint(0); + for (size_t v = 1; v < nV; v++) + { + Vec3<double> p = GetPoint(v); + if (p.X() < m_minBB.X()) + { + m_minBB.X() = p.X(); + } + if (p.Y() < m_minBB.Y()) + { + m_minBB.Y() = p.Y(); + } + if (p.Z() < m_minBB.Z()) + { + m_minBB.Z() = p.Z(); + } + if (p.X() > m_maxBB.X()) + { + m_maxBB.X() = p.X(); + } + if (p.Y() > m_maxBB.Y()) + { + m_maxBB.Y() = p.Y(); + } + if (p.Z() > m_maxBB.Z()) + { + m_maxBB.Z() = p.Z(); + } + } + } + return m_center; +} + +double Mesh::ComputeVolume() const +{ + const size_t nV = GetNPoints(); + const size_t nT = GetNTriangles(); + if (nV == 0 || nT == 0) { + return 0.0; + } + + Vec3<double> bary(0.0, 0.0, 0.0); + for (size_t v = 0; v < nV; v++) { + bary += GetPoint(v); + } + bary /= static_cast<double>(nV); + + Vec3<double> ver0, ver1, ver2; + double totalVolume = 0.0; + for (int32_t t = 0; t < int32_t(nT); t++) { + const Vec3<int32_t>& tri = GetTriangle(t); + ver0 = GetPoint(tri[0]); + ver1 = GetPoint(tri[1]); + ver2 = GetPoint(tri[2]); + totalVolume += ComputeVolume4(ver0, ver1, ver2, bary); + } + return totalVolume / 6.0; +} + +void Mesh::ComputeConvexHull(const double* const pts, + const size_t nPts) +{ + ResizePoints(0); + ResizeTriangles(0); + btConvexHullComputer ch; + ch.compute(pts, 3 * sizeof(double), (int32_t)nPts, -1.0, -1.0); + for (int32_t v = 0; v < ch.vertices.size(); v++) { + AddPoint(Vec3<double>(ch.vertices[v].getX(), ch.vertices[v].getY(), ch.vertices[v].getZ())); + } + const int32_t nt = ch.faces.size(); + for (int32_t t = 0; t < nt; ++t) { + const btConvexHullComputer::Edge* sourceEdge = &(ch.edges[ch.faces[t]]); + int32_t a = sourceEdge->getSourceVertex(); + int32_t b = sourceEdge->getTargetVertex(); + const btConvexHullComputer::Edge* edge = sourceEdge->getNextEdgeOfFace(); + int32_t c = edge->getTargetVertex(); + while (c != a) { + AddTriangle(Vec3<int32_t>(a, b, c)); + edge = edge->getNextEdgeOfFace(); + b = c; + c = edge->getTargetVertex(); + } + } +} +void Mesh::Clip(const Plane& plane, + SArray<Vec3<double> >& positivePart, + SArray<Vec3<double> >& negativePart) const +{ + const size_t nV = GetNPoints(); + if (nV == 0) { + return; + } + double d; + for (size_t v = 0; v < nV; v++) { + const Vec3<double>& pt = GetPoint(v); + d = plane.m_a * pt[0] + plane.m_b * pt[1] + plane.m_c * pt[2] + plane.m_d; + if (d > 0.0) { + positivePart.PushBack(pt); + } + else if (d < 0.0) { + negativePart.PushBack(pt); + } + else { + positivePart.PushBack(pt); + negativePart.PushBack(pt); + } + } +} +bool Mesh::IsInside(const Vec3<double>& pt) const +{ + const size_t nV = GetNPoints(); + const size_t nT = GetNTriangles(); + if (nV == 0 || nT == 0) { + return false; + } + Vec3<double> ver0, ver1, ver2; + double volume; + for (int32_t t = 0; t < int32_t(nT); t++) { + const Vec3<int32_t>& tri = GetTriangle(t); + ver0 = GetPoint(tri[0]); + ver1 = GetPoint(tri[1]); + ver2 = GetPoint(tri[2]); + volume = ComputeVolume4(ver0, ver1, ver2, pt); + if (volume < 0.0) { + return false; + } + } + return true; +} +double Mesh::ComputeDiagBB() +{ + const size_t nPoints = GetNPoints(); + if (nPoints == 0) + return 0.0; + Vec3<double> minBB = m_points[0]; + Vec3<double> maxBB = m_points[0]; + double x, y, z; + for (size_t v = 1; v < nPoints; v++) { + x = m_points[v][0]; + y = m_points[v][1]; + z = m_points[v][2]; + if (x < minBB[0]) + minBB[0] = x; + else if (x > maxBB[0]) + maxBB[0] = x; + if (y < minBB[1]) + minBB[1] = y; + else if (y > maxBB[1]) + maxBB[1] = y; + if (z < minBB[2]) + minBB[2] = z; + else if (z > maxBB[2]) + maxBB[2] = z; + } + return (m_diag = (maxBB - minBB).GetNorm()); +} + +#ifdef VHACD_DEBUG_MESH +bool Mesh::SaveVRML2(const std::string& fileName) const +{ + std::ofstream fout(fileName.c_str()); + if (fout.is_open()) { + const Material material; + + if (SaveVRML2(fout, material)) { + fout.close(); + return true; + } + return false; + } + return false; +} +bool Mesh::SaveVRML2(std::ofstream& fout, const Material& material) const +{ + if (fout.is_open()) { + fout.setf(std::ios::fixed, std::ios::floatfield); + fout.setf(std::ios::showpoint); + fout.precision(6); + size_t nV = m_points.Size(); + size_t nT = m_triangles.Size(); + fout << "#VRML V2.0 utf8" << std::endl; + fout << "" << std::endl; + fout << "# Vertices: " << nV << std::endl; + fout << "# Triangles: " << nT << std::endl; + fout << "" << std::endl; + fout << "Group {" << std::endl; + fout << " children [" << std::endl; + fout << " Shape {" << std::endl; + fout << " appearance Appearance {" << std::endl; + fout << " material Material {" << std::endl; + fout << " diffuseColor " << material.m_diffuseColor[0] << " " + << material.m_diffuseColor[1] << " " + << material.m_diffuseColor[2] << std::endl; + fout << " ambientIntensity " << material.m_ambientIntensity << std::endl; + fout << " specularColor " << material.m_specularColor[0] << " " + << material.m_specularColor[1] << " " + << material.m_specularColor[2] << std::endl; + fout << " emissiveColor " << material.m_emissiveColor[0] << " " + << material.m_emissiveColor[1] << " " + << material.m_emissiveColor[2] << std::endl; + fout << " shininess " << material.m_shininess << std::endl; + fout << " transparency " << material.m_transparency << std::endl; + fout << " }" << std::endl; + fout << " }" << std::endl; + fout << " geometry IndexedFaceSet {" << std::endl; + fout << " ccw TRUE" << std::endl; + fout << " solid TRUE" << std::endl; + fout << " convex TRUE" << std::endl; + if (nV > 0) { + fout << " coord DEF co Coordinate {" << std::endl; + fout << " point [" << std::endl; + for (size_t v = 0; v < nV; v++) { + fout << " " << m_points[v][0] << " " + << m_points[v][1] << " " + << m_points[v][2] << "," << std::endl; + } + fout << " ]" << std::endl; + fout << " }" << std::endl; + } + if (nT > 0) { + fout << " coordIndex [ " << std::endl; + for (size_t f = 0; f < nT; f++) { + fout << " " << m_triangles[f][0] << ", " + << m_triangles[f][1] << ", " + << m_triangles[f][2] << ", -1," << std::endl; + } + fout << " ]" << std::endl; + } + fout << " }" << std::endl; + fout << " }" << std::endl; + fout << " ]" << std::endl; + fout << "}" << std::endl; + return true; + } + return false; +} +bool Mesh::SaveOFF(const std::string& fileName) const +{ + std::ofstream fout(fileName.c_str()); + if (fout.is_open()) { + size_t nV = m_points.Size(); + size_t nT = m_triangles.Size(); + fout << "OFF" << std::endl; + fout << nV << " " << nT << " " << 0 << std::endl; + for (size_t v = 0; v < nV; v++) { + fout << m_points[v][0] << " " + << m_points[v][1] << " " + << m_points[v][2] << std::endl; + } + for (size_t f = 0; f < nT; f++) { + fout << "3 " << m_triangles[f][0] << " " + << m_triangles[f][1] << " " + << m_triangles[f][2] << std::endl; + } + fout.close(); + return true; + } + return false; +} + +bool Mesh::LoadOFF(const std::string& fileName, bool invert) +{ + FILE* fid = fopen(fileName.c_str(), "r"); + if (fid) { + const std::string strOFF("OFF"); + char temp[1024]; + fscanf(fid, "%s", temp); + if (std::string(temp) != strOFF) { + fclose(fid); + return false; + } + else { + int32_t nv = 0; + int32_t nf = 0; + int32_t ne = 0; + fscanf(fid, "%i", &nv); + fscanf(fid, "%i", &nf); + fscanf(fid, "%i", &ne); + m_points.Resize(nv); + m_triangles.Resize(nf); + Vec3<double> coord; + float x, y, z; + for (int32_t p = 0; p < nv; p++) { + fscanf(fid, "%f", &x); + fscanf(fid, "%f", &y); + fscanf(fid, "%f", &z); + m_points[p][0] = x; + m_points[p][1] = y; + m_points[p][2] = z; + } + int32_t i, j, k, s; + for (int32_t t = 0; t < nf; ++t) { + fscanf(fid, "%i", &s); + if (s == 3) { + fscanf(fid, "%i", &i); + fscanf(fid, "%i", &j); + fscanf(fid, "%i", &k); + m_triangles[t][0] = i; + if (invert) { + m_triangles[t][1] = k; + m_triangles[t][2] = j; + } + else { + m_triangles[t][1] = j; + m_triangles[t][2] = k; + } + } + else // Fix me: support only triangular meshes + { + for (int32_t h = 0; h < s; ++h) + fscanf(fid, "%i", &s); + } + } + fclose(fid); + } + } + else { + return false; + } + return true; +} +#endif // VHACD_DEBUG_MESH +} diff --git a/thirdparty/vhacd/src/vhacdRaycastMesh.cpp b/thirdparty/vhacd/src/vhacdRaycastMesh.cpp new file mode 100644 index 0000000000..e8b9435d5f --- /dev/null +++ b/thirdparty/vhacd/src/vhacdRaycastMesh.cpp @@ -0,0 +1,208 @@ +#include "vhacdRaycastMesh.h" +#include <math.h> +#include <assert.h> + +namespace RAYCAST_MESH +{ + +/* a = b - c */ +#define vector(a,b,c) \ + (a)[0] = (b)[0] - (c)[0]; \ + (a)[1] = (b)[1] - (c)[1]; \ + (a)[2] = (b)[2] - (c)[2]; + +#define innerProduct(v,q) \ + ((v)[0] * (q)[0] + \ + (v)[1] * (q)[1] + \ + (v)[2] * (q)[2]) + +#define crossProduct(a,b,c) \ + (a)[0] = (b)[1] * (c)[2] - (c)[1] * (b)[2]; \ + (a)[1] = (b)[2] * (c)[0] - (c)[2] * (b)[0]; \ + (a)[2] = (b)[0] * (c)[1] - (c)[0] * (b)[1]; + + +static inline bool rayIntersectsTriangle(const double *p,const double *d,const double *v0,const double *v1,const double *v2,double &t) +{ + double e1[3],e2[3],h[3],s[3],q[3]; + double a,f,u,v; + + vector(e1,v1,v0); + vector(e2,v2,v0); + crossProduct(h,d,e2); + a = innerProduct(e1,h); + + if (a > -0.00001 && a < 0.00001) + return(false); + + f = 1/a; + vector(s,p,v0); + u = f * (innerProduct(s,h)); + + if (u < 0.0 || u > 1.0) + return(false); + + crossProduct(q,s,e1); + v = f * innerProduct(d,q); + if (v < 0.0 || u + v > 1.0) + return(false); + // at this stage we can compute t to find out where + // the intersection point is on the line + t = f * innerProduct(e2,q); + if (t > 0) // ray intersection + return(true); + else // this means that there is a line intersection + // but not a ray intersection + return (false); +} + +static double getPointDistance(const double *p1, const double *p2) +{ + double dx = p1[0] - p2[0]; + double dy = p1[1] - p2[1]; + double dz = p1[2] - p2[2]; + return sqrt(dx*dx + dy*dy + dz*dz); +} + +class MyRaycastMesh : public VHACD::RaycastMesh +{ +public: + + template <class T> + MyRaycastMesh(uint32_t vcount, + const T *vertices, + uint32_t tcount, + const uint32_t *indices) + { + mVcount = vcount; + mVertices = new double[mVcount * 3]; + for (uint32_t i = 0; i < mVcount; i++) + { + mVertices[i * 3 + 0] = vertices[0]; + mVertices[i * 3 + 1] = vertices[1]; + mVertices[i * 3 + 2] = vertices[2]; + vertices += 3; + } + mTcount = tcount; + mIndices = new uint32_t[mTcount * 3]; + for (uint32_t i = 0; i < mTcount; i++) + { + mIndices[i * 3 + 0] = indices[0]; + mIndices[i * 3 + 1] = indices[1]; + mIndices[i * 3 + 2] = indices[2]; + indices += 3; + } + } + + + ~MyRaycastMesh(void) + { + delete[]mVertices; + delete[]mIndices; + } + + virtual void release(void) + { + delete this; + } + + virtual bool raycast(const double *from, // The starting point of the raycast + const double *to, // The ending point of the raycast + const double *closestToPoint, // The point to match the nearest hit location (can just be the 'from' location of no specific point) + double *hitLocation, // The point where the ray hit nearest to the 'closestToPoint' location + double *hitDistance) final // The distance the ray traveled to the hit location + { + bool ret = false; + + double dir[3]; + + dir[0] = to[0] - from[0]; + dir[1] = to[1] - from[1]; + dir[2] = to[2] - from[2]; + + double distance = sqrt( dir[0]*dir[0] + dir[1]*dir[1]+dir[2]*dir[2] ); + if ( distance < 0.0000000001f ) return false; + double recipDistance = 1.0f / distance; + dir[0]*=recipDistance; + dir[1]*=recipDistance; + dir[2]*=recipDistance; + const uint32_t *indices = mIndices; + const double *vertices = mVertices; + double nearestDistance = distance; + + for (uint32_t tri=0; tri<mTcount; tri++) + { + uint32_t i1 = indices[tri*3+0]; + uint32_t i2 = indices[tri*3+1]; + uint32_t i3 = indices[tri*3+2]; + + const double *p1 = &vertices[i1*3]; + const double *p2 = &vertices[i2*3]; + const double *p3 = &vertices[i3*3]; + + double t; + if ( rayIntersectsTriangle(from,dir,p1,p2,p3,t)) + { + double hitPos[3]; + + hitPos[0] = from[0] + dir[0] * t; + hitPos[1] = from[1] + dir[1] * t; + hitPos[2] = from[2] + dir[2] * t; + + double pointDistance = getPointDistance(hitPos, closestToPoint); + + if (pointDistance < nearestDistance ) + { + nearestDistance = pointDistance; + if ( hitLocation ) + { + hitLocation[0] = hitPos[0]; + hitLocation[1] = hitPos[1]; + hitLocation[2] = hitPos[2]; + } + if ( hitDistance ) + { + *hitDistance = pointDistance; + } + ret = true; + } + } + } + return ret; + } + + uint32_t mVcount; + double *mVertices; + uint32_t mTcount; + uint32_t *mIndices; +}; + +}; + + + +using namespace RAYCAST_MESH; + +namespace VHACD +{ + + RaycastMesh * RaycastMesh::createRaycastMesh(uint32_t vcount, // The number of vertices in the source triangle mesh + const double *vertices, // The array of vertex positions in the format x1,y1,z1..x2,y2,z2.. etc. + uint32_t tcount, // The number of triangles in the source triangle mesh + const uint32_t *indices) // The triangle indices in the format of i1,i2,i3 ... i4,i5,i6, ... + { + MyRaycastMesh *m = new MyRaycastMesh(vcount, vertices, tcount, indices); + return static_cast<RaycastMesh *>(m); + } + + RaycastMesh * RaycastMesh::createRaycastMesh(uint32_t vcount, // The number of vertices in the source triangle mesh + const float *vertices, // The array of vertex positions in the format x1,y1,z1..x2,y2,z2.. etc. + uint32_t tcount, // The number of triangles in the source triangle mesh + const uint32_t *indices) // The triangle indices in the format of i1,i2,i3 ... i4,i5,i6, ... + { + MyRaycastMesh *m = new MyRaycastMesh(vcount, vertices, tcount, indices); + return static_cast<RaycastMesh *>(m); + } + + +} // end of VHACD namespace
\ No newline at end of file diff --git a/thirdparty/vhacd/src/vhacdVolume.cpp b/thirdparty/vhacd/src/vhacdVolume.cpp new file mode 100644 index 0000000000..a250e5acaa --- /dev/null +++ b/thirdparty/vhacd/src/vhacdVolume.cpp @@ -0,0 +1,1626 @@ +/* Copyright (c) 2011 Khaled Mamou (kmamou at gmail dot com) + All rights reserved. + + + Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + + 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + + 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. + + 3. The names of the contributors may not be used to endorse or promote products derived from this software without specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef _CRT_SECURE_NO_WARNINGS +#define _CRT_SECURE_NO_WARNINGS +#endif + +#include "btConvexHullComputer.h" +#include "vhacdVolume.h" +#include <algorithm> +#include <float.h> +#include <math.h> +#include <queue> +#include <string.h> + +#ifdef _MSC_VER +#pragma warning(disable:4458 4100) +#endif + + +namespace VHACD { +/********************************************************/ +/* AABB-triangle overlap test code */ +/* by Tomas Akenine-M�ller */ +/* Function: int32_t triBoxOverlap(float boxcenter[3], */ +/* float boxhalfsize[3],float triverts[3][3]); */ +/* History: */ +/* 2001-03-05: released the code in its first version */ +/* 2001-06-18: changed the order of the tests, faster */ +/* */ +/* Acknowledgement: Many thanks to Pierre Terdiman for */ +/* suggestions and discussions on how to optimize code. */ +/* Thanks to David Hunt for finding a ">="-bug! */ +/********************************************************/ + +#define X 0 +#define Y 1 +#define Z 2 +#define FINDMINMAX(x0, x1, x2, min, max) \ + min = max = x0; \ + if (x1 < min) \ + min = x1; \ + if (x1 > max) \ + max = x1; \ + if (x2 < min) \ + min = x2; \ + if (x2 > max) \ + max = x2; + +#define AXISTEST_X01(a, b, fa, fb) \ + p0 = a * v0[Y] - b * v0[Z]; \ + p2 = a * v2[Y] - b * v2[Z]; \ + if (p0 < p2) { \ + min = p0; \ + max = p2; \ + } \ + else { \ + min = p2; \ + max = p0; \ + } \ + rad = fa * boxhalfsize[Y] + fb * boxhalfsize[Z]; \ + if (min > rad || max < -rad) \ + return 0; + +#define AXISTEST_X2(a, b, fa, fb) \ + p0 = a * v0[Y] - b * v0[Z]; \ + p1 = a * v1[Y] - b * v1[Z]; \ + if (p0 < p1) { \ + min = p0; \ + max = p1; \ + } \ + else { \ + min = p1; \ + max = p0; \ + } \ + rad = fa * boxhalfsize[Y] + fb * boxhalfsize[Z]; \ + if (min > rad || max < -rad) \ + return 0; + +#define AXISTEST_Y02(a, b, fa, fb) \ + p0 = -a * v0[X] + b * v0[Z]; \ + p2 = -a * v2[X] + b * v2[Z]; \ + if (p0 < p2) { \ + min = p0; \ + max = p2; \ + } \ + else { \ + min = p2; \ + max = p0; \ + } \ + rad = fa * boxhalfsize[X] + fb * boxhalfsize[Z]; \ + if (min > rad || max < -rad) \ + return 0; + +#define AXISTEST_Y1(a, b, fa, fb) \ + p0 = -a * v0[X] + b * v0[Z]; \ + p1 = -a * v1[X] + b * v1[Z]; \ + if (p0 < p1) { \ + min = p0; \ + max = p1; \ + } \ + else { \ + min = p1; \ + max = p0; \ + } \ + rad = fa * boxhalfsize[X] + fb * boxhalfsize[Z]; \ + if (min > rad || max < -rad) \ + return 0; + +#define AXISTEST_Z12(a, b, fa, fb) \ + p1 = a * v1[X] - b * v1[Y]; \ + p2 = a * v2[X] - b * v2[Y]; \ + if (p2 < p1) { \ + min = p2; \ + max = p1; \ + } \ + else { \ + min = p1; \ + max = p2; \ + } \ + rad = fa * boxhalfsize[X] + fb * boxhalfsize[Y]; \ + if (min > rad || max < -rad) \ + return 0; + +#define AXISTEST_Z0(a, b, fa, fb) \ + p0 = a * v0[X] - b * v0[Y]; \ + p1 = a * v1[X] - b * v1[Y]; \ + if (p0 < p1) { \ + min = p0; \ + max = p1; \ + } \ + else { \ + min = p1; \ + max = p0; \ + } \ + rad = fa * boxhalfsize[X] + fb * boxhalfsize[Y]; \ + if (min > rad || max < -rad) \ + return 0; + +int32_t PlaneBoxOverlap(const Vec3<double>& normal, + const Vec3<double>& vert, + const Vec3<double>& maxbox) +{ + int32_t q; + Vec3<double> vmin, vmax; + double v; + for (q = X; q <= Z; q++) { + v = vert[q]; + if (normal[q] > 0.0) { + vmin[q] = -maxbox[q] - v; + vmax[q] = maxbox[q] - v; + } + else { + vmin[q] = maxbox[q] - v; + vmax[q] = -maxbox[q] - v; + } + } + if (normal * vmin > 0.0) + return 0; + if (normal * vmax >= 0.0) + return 1; + return 0; +} + +int32_t TriBoxOverlap(const Vec3<double>& boxcenter, + const Vec3<double>& boxhalfsize, + const Vec3<double>& triver0, + const Vec3<double>& triver1, + const Vec3<double>& triver2) +{ + /* use separating axis theorem to test overlap between triangle and box */ + /* need to test for overlap in these directions: */ + /* 1) the {x,y,z}-directions (actually, since we use the AABB of the triangle */ + /* we do not even need to test these) */ + /* 2) normal of the triangle */ + /* 3) crossproduct(edge from tri, {x,y,z}-directin) */ + /* this gives 3x3=9 more tests */ + + Vec3<double> v0, v1, v2; + double min, max, p0, p1, p2, rad, fex, fey, fez; // -NJMP- "d" local variable removed + Vec3<double> normal, e0, e1, e2; + + /* This is the fastest branch on Sun */ + /* move everything so that the boxcenter is in (0,0,0) */ + + v0 = triver0 - boxcenter; + v1 = triver1 - boxcenter; + v2 = triver2 - boxcenter; + + /* compute triangle edges */ + e0 = v1 - v0; /* tri edge 0 */ + e1 = v2 - v1; /* tri edge 1 */ + e2 = v0 - v2; /* tri edge 2 */ + + /* Bullet 3: */ + /* test the 9 tests first (this was faster) */ + fex = fabs(e0[X]); + fey = fabs(e0[Y]); + fez = fabs(e0[Z]); + + AXISTEST_X01(e0[Z], e0[Y], fez, fey); + AXISTEST_Y02(e0[Z], e0[X], fez, fex); + AXISTEST_Z12(e0[Y], e0[X], fey, fex); + + fex = fabs(e1[X]); + fey = fabs(e1[Y]); + fez = fabs(e1[Z]); + + AXISTEST_X01(e1[Z], e1[Y], fez, fey); + AXISTEST_Y02(e1[Z], e1[X], fez, fex); + AXISTEST_Z0(e1[Y], e1[X], fey, fex); + + fex = fabs(e2[X]); + fey = fabs(e2[Y]); + fez = fabs(e2[Z]); + + AXISTEST_X2(e2[Z], e2[Y], fez, fey); + AXISTEST_Y1(e2[Z], e2[X], fez, fex); + AXISTEST_Z12(e2[Y], e2[X], fey, fex); + + /* Bullet 1: */ + /* first test overlap in the {x,y,z}-directions */ + /* find min, max of the triangle each direction, and test for overlap in */ + /* that direction -- this is equivalent to testing a minimal AABB around */ + /* the triangle against the AABB */ + + /* test in X-direction */ + FINDMINMAX(v0[X], v1[X], v2[X], min, max); + if (min > boxhalfsize[X] || max < -boxhalfsize[X]) + return 0; + + /* test in Y-direction */ + FINDMINMAX(v0[Y], v1[Y], v2[Y], min, max); + if (min > boxhalfsize[Y] || max < -boxhalfsize[Y]) + return 0; + + /* test in Z-direction */ + FINDMINMAX(v0[Z], v1[Z], v2[Z], min, max); + if (min > boxhalfsize[Z] || max < -boxhalfsize[Z]) + return 0; + + /* Bullet 2: */ + /* test if the box intersects the plane of the triangle */ + /* compute plane equation of triangle: normal*x+d=0 */ + normal = e0 ^ e1; + + if (!PlaneBoxOverlap(normal, v0, boxhalfsize)) + return 0; + return 1; /* box and triangle overlaps */ +} + +// Slightly modified version of Stan Melax's code for 3x3 matrix diagonalization (Thanks Stan!) +// source: http://www.melax.com/diag.html?attredirects=0 +void Diagonalize(const double (&A)[3][3], double (&Q)[3][3], double (&D)[3][3]) +{ + // A must be a symmetric matrix. + // returns Q and D such that + // Diagonal matrix D = QT * A * Q; and A = Q*D*QT + const int32_t maxsteps = 24; // certainly wont need that many. + int32_t k0, k1, k2; + double o[3], m[3]; + double q[4] = { 0.0, 0.0, 0.0, 1.0 }; + double jr[4]; + double sqw, sqx, sqy, sqz; + double tmp1, tmp2, mq; + double AQ[3][3]; + double thet, sgn, t, c; + for (int32_t i = 0; i < maxsteps; ++i) { + // quat to matrix + sqx = q[0] * q[0]; + sqy = q[1] * q[1]; + sqz = q[2] * q[2]; + sqw = q[3] * q[3]; + Q[0][0] = (sqx - sqy - sqz + sqw); + Q[1][1] = (-sqx + sqy - sqz + sqw); + Q[2][2] = (-sqx - sqy + sqz + sqw); + tmp1 = q[0] * q[1]; + tmp2 = q[2] * q[3]; + Q[1][0] = 2.0 * (tmp1 + tmp2); + Q[0][1] = 2.0 * (tmp1 - tmp2); + tmp1 = q[0] * q[2]; + tmp2 = q[1] * q[3]; + Q[2][0] = 2.0 * (tmp1 - tmp2); + Q[0][2] = 2.0 * (tmp1 + tmp2); + tmp1 = q[1] * q[2]; + tmp2 = q[0] * q[3]; + Q[2][1] = 2.0 * (tmp1 + tmp2); + Q[1][2] = 2.0 * (tmp1 - tmp2); + + // AQ = A * Q + AQ[0][0] = Q[0][0] * A[0][0] + Q[1][0] * A[0][1] + Q[2][0] * A[0][2]; + AQ[0][1] = Q[0][1] * A[0][0] + Q[1][1] * A[0][1] + Q[2][1] * A[0][2]; + AQ[0][2] = Q[0][2] * A[0][0] + Q[1][2] * A[0][1] + Q[2][2] * A[0][2]; + AQ[1][0] = Q[0][0] * A[0][1] + Q[1][0] * A[1][1] + Q[2][0] * A[1][2]; + AQ[1][1] = Q[0][1] * A[0][1] + Q[1][1] * A[1][1] + Q[2][1] * A[1][2]; + AQ[1][2] = Q[0][2] * A[0][1] + Q[1][2] * A[1][1] + Q[2][2] * A[1][2]; + AQ[2][0] = Q[0][0] * A[0][2] + Q[1][0] * A[1][2] + Q[2][0] * A[2][2]; + AQ[2][1] = Q[0][1] * A[0][2] + Q[1][1] * A[1][2] + Q[2][1] * A[2][2]; + AQ[2][2] = Q[0][2] * A[0][2] + Q[1][2] * A[1][2] + Q[2][2] * A[2][2]; + // D = Qt * AQ + D[0][0] = AQ[0][0] * Q[0][0] + AQ[1][0] * Q[1][0] + AQ[2][0] * Q[2][0]; + D[0][1] = AQ[0][0] * Q[0][1] + AQ[1][0] * Q[1][1] + AQ[2][0] * Q[2][1]; + D[0][2] = AQ[0][0] * Q[0][2] + AQ[1][0] * Q[1][2] + AQ[2][0] * Q[2][2]; + D[1][0] = AQ[0][1] * Q[0][0] + AQ[1][1] * Q[1][0] + AQ[2][1] * Q[2][0]; + D[1][1] = AQ[0][1] * Q[0][1] + AQ[1][1] * Q[1][1] + AQ[2][1] * Q[2][1]; + D[1][2] = AQ[0][1] * Q[0][2] + AQ[1][1] * Q[1][2] + AQ[2][1] * Q[2][2]; + D[2][0] = AQ[0][2] * Q[0][0] + AQ[1][2] * Q[1][0] + AQ[2][2] * Q[2][0]; + D[2][1] = AQ[0][2] * Q[0][1] + AQ[1][2] * Q[1][1] + AQ[2][2] * Q[2][1]; + D[2][2] = AQ[0][2] * Q[0][2] + AQ[1][2] * Q[1][2] + AQ[2][2] * Q[2][2]; + o[0] = D[1][2]; + o[1] = D[0][2]; + o[2] = D[0][1]; + m[0] = fabs(o[0]); + m[1] = fabs(o[1]); + m[2] = fabs(o[2]); + + k0 = (m[0] > m[1] && m[0] > m[2]) ? 0 : (m[1] > m[2]) ? 1 : 2; // index of largest element of offdiag + k1 = (k0 + 1) % 3; + k2 = (k0 + 2) % 3; + if (o[k0] == 0.0) { + break; // diagonal already + } + thet = (D[k2][k2] - D[k1][k1]) / (2.0 * o[k0]); + sgn = (thet > 0.0) ? 1.0 : -1.0; + thet *= sgn; // make it positive + t = sgn / (thet + ((thet < 1.E6) ? sqrt(thet * thet + 1.0) : thet)); // sign(T)/(|T|+sqrt(T^2+1)) + c = 1.0 / sqrt(t * t + 1.0); // c= 1/(t^2+1) , t=s/c + if (c == 1.0) { + break; // no room for improvement - reached machine precision. + } + jr[0] = jr[1] = jr[2] = jr[3] = 0.0; + jr[k0] = sgn * sqrt((1.0 - c) / 2.0); // using 1/2 angle identity sin(a/2) = sqrt((1-cos(a))/2) + jr[k0] *= -1.0; // since our quat-to-matrix convention was for v*M instead of M*v + jr[3] = sqrt(1.0 - jr[k0] * jr[k0]); + if (jr[3] == 1.0) { + break; // reached limits of floating point precision + } + q[0] = (q[3] * jr[0] + q[0] * jr[3] + q[1] * jr[2] - q[2] * jr[1]); + q[1] = (q[3] * jr[1] - q[0] * jr[2] + q[1] * jr[3] + q[2] * jr[0]); + q[2] = (q[3] * jr[2] + q[0] * jr[1] - q[1] * jr[0] + q[2] * jr[3]); + q[3] = (q[3] * jr[3] - q[0] * jr[0] - q[1] * jr[1] - q[2] * jr[2]); + mq = sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); + q[0] /= mq; + q[1] /= mq; + q[2] /= mq; + q[3] /= mq; + } +} +const double TetrahedronSet::EPS = 0.0000000000001; +VoxelSet::VoxelSet() +{ + m_minBB[0] = m_minBB[1] = m_minBB[2] = 0.0; + m_minBBVoxels[0] = m_minBBVoxels[1] = m_minBBVoxels[2] = 0; + m_maxBBVoxels[0] = m_maxBBVoxels[1] = m_maxBBVoxels[2] = 1; + m_minBBPts[0] = m_minBBPts[1] = m_minBBPts[2] = 0; + m_maxBBPts[0] = m_maxBBPts[1] = m_maxBBPts[2] = 1; + m_barycenter[0] = m_barycenter[1] = m_barycenter[2] = 0; + m_barycenterPCA[0] = m_barycenterPCA[1] = m_barycenterPCA[2] = 0.0; + m_scale = 1.0; + m_unitVolume = 1.0; + m_numVoxelsOnSurface = 0; + m_numVoxelsInsideSurface = 0; + memset(m_Q, 0, sizeof(double) * 9); + memset(m_D, 0, sizeof(double) * 9); +} +VoxelSet::~VoxelSet(void) +{ +} +void VoxelSet::ComputeBB() +{ + const size_t nVoxels = m_voxels.Size(); + if (nVoxels == 0) + return; + for (int32_t h = 0; h < 3; ++h) { + m_minBBVoxels[h] = m_voxels[0].m_coord[h]; + m_maxBBVoxels[h] = m_voxels[0].m_coord[h]; + } + Vec3<double> bary(0.0); + for (size_t p = 0; p < nVoxels; ++p) { + for (int32_t h = 0; h < 3; ++h) { + bary[h] += m_voxels[p].m_coord[h]; + if (m_minBBVoxels[h] > m_voxels[p].m_coord[h]) + m_minBBVoxels[h] = m_voxels[p].m_coord[h]; + if (m_maxBBVoxels[h] < m_voxels[p].m_coord[h]) + m_maxBBVoxels[h] = m_voxels[p].m_coord[h]; + } + } + bary /= (double)nVoxels; + for (int32_t h = 0; h < 3; ++h) { + m_minBBPts[h] = m_minBBVoxels[h] * m_scale + m_minBB[h]; + m_maxBBPts[h] = m_maxBBVoxels[h] * m_scale + m_minBB[h]; + m_barycenter[h] = (short)(bary[h] + 0.5); + } +} +void VoxelSet::ComputeConvexHull(Mesh& meshCH, const size_t sampling) const +{ + const size_t CLUSTER_SIZE = 65536; + const size_t nVoxels = m_voxels.Size(); + if (nVoxels == 0) + return; + + SArray<Vec3<double> > cpoints; + + Vec3<double>* points = new Vec3<double>[CLUSTER_SIZE]; + size_t p = 0; + size_t s = 0; + short i, j, k; + while (p < nVoxels) { + size_t q = 0; + while (q < CLUSTER_SIZE && p < nVoxels) { + if (m_voxels[p].m_data == PRIMITIVE_ON_SURFACE) { + ++s; + if (s == sampling) { + s = 0; + i = m_voxels[p].m_coord[0]; + j = m_voxels[p].m_coord[1]; + k = m_voxels[p].m_coord[2]; + Vec3<double> p0((i - 0.5) * m_scale, (j - 0.5) * m_scale, (k - 0.5) * m_scale); + Vec3<double> p1((i + 0.5) * m_scale, (j - 0.5) * m_scale, (k - 0.5) * m_scale); + Vec3<double> p2((i + 0.5) * m_scale, (j + 0.5) * m_scale, (k - 0.5) * m_scale); + Vec3<double> p3((i - 0.5) * m_scale, (j + 0.5) * m_scale, (k - 0.5) * m_scale); + Vec3<double> p4((i - 0.5) * m_scale, (j - 0.5) * m_scale, (k + 0.5) * m_scale); + Vec3<double> p5((i + 0.5) * m_scale, (j - 0.5) * m_scale, (k + 0.5) * m_scale); + Vec3<double> p6((i + 0.5) * m_scale, (j + 0.5) * m_scale, (k + 0.5) * m_scale); + Vec3<double> p7((i - 0.5) * m_scale, (j + 0.5) * m_scale, (k + 0.5) * m_scale); + points[q++] = p0 + m_minBB; + points[q++] = p1 + m_minBB; + points[q++] = p2 + m_minBB; + points[q++] = p3 + m_minBB; + points[q++] = p4 + m_minBB; + points[q++] = p5 + m_minBB; + points[q++] = p6 + m_minBB; + points[q++] = p7 + m_minBB; + } + } + ++p; + } + btConvexHullComputer ch; + ch.compute((double*)points, 3 * sizeof(double), (int32_t)q, -1.0, -1.0); + for (int32_t v = 0; v < ch.vertices.size(); v++) { + cpoints.PushBack(Vec3<double>(ch.vertices[v].getX(), ch.vertices[v].getY(), ch.vertices[v].getZ())); + } + } + delete[] points; + + points = cpoints.Data(); + btConvexHullComputer ch; + ch.compute((double*)points, 3 * sizeof(double), (int32_t)cpoints.Size(), -1.0, -1.0); + meshCH.ResizePoints(0); + meshCH.ResizeTriangles(0); + for (int32_t v = 0; v < ch.vertices.size(); v++) { + meshCH.AddPoint(Vec3<double>(ch.vertices[v].getX(), ch.vertices[v].getY(), ch.vertices[v].getZ())); + } + const int32_t nt = ch.faces.size(); + for (int32_t t = 0; t < nt; ++t) { + const btConvexHullComputer::Edge* sourceEdge = &(ch.edges[ch.faces[t]]); + int32_t a = sourceEdge->getSourceVertex(); + int32_t b = sourceEdge->getTargetVertex(); + const btConvexHullComputer::Edge* edge = sourceEdge->getNextEdgeOfFace(); + int32_t c = edge->getTargetVertex(); + while (c != a) { + meshCH.AddTriangle(Vec3<int32_t>(a, b, c)); + edge = edge->getNextEdgeOfFace(); + b = c; + c = edge->getTargetVertex(); + } + } +} +void VoxelSet::GetPoints(const Voxel& voxel, + Vec3<double>* const pts) const +{ + short i = voxel.m_coord[0]; + short j = voxel.m_coord[1]; + short k = voxel.m_coord[2]; + pts[0][0] = (i - 0.5) * m_scale + m_minBB[0]; + pts[1][0] = (i + 0.5) * m_scale + m_minBB[0]; + pts[2][0] = (i + 0.5) * m_scale + m_minBB[0]; + pts[3][0] = (i - 0.5) * m_scale + m_minBB[0]; + pts[4][0] = (i - 0.5) * m_scale + m_minBB[0]; + pts[5][0] = (i + 0.5) * m_scale + m_minBB[0]; + pts[6][0] = (i + 0.5) * m_scale + m_minBB[0]; + pts[7][0] = (i - 0.5) * m_scale + m_minBB[0]; + pts[0][1] = (j - 0.5) * m_scale + m_minBB[1]; + pts[1][1] = (j - 0.5) * m_scale + m_minBB[1]; + pts[2][1] = (j + 0.5) * m_scale + m_minBB[1]; + pts[3][1] = (j + 0.5) * m_scale + m_minBB[1]; + pts[4][1] = (j - 0.5) * m_scale + m_minBB[1]; + pts[5][1] = (j - 0.5) * m_scale + m_minBB[1]; + pts[6][1] = (j + 0.5) * m_scale + m_minBB[1]; + pts[7][1] = (j + 0.5) * m_scale + m_minBB[1]; + pts[0][2] = (k - 0.5) * m_scale + m_minBB[2]; + pts[1][2] = (k - 0.5) * m_scale + m_minBB[2]; + pts[2][2] = (k - 0.5) * m_scale + m_minBB[2]; + pts[3][2] = (k - 0.5) * m_scale + m_minBB[2]; + pts[4][2] = (k + 0.5) * m_scale + m_minBB[2]; + pts[5][2] = (k + 0.5) * m_scale + m_minBB[2]; + pts[6][2] = (k + 0.5) * m_scale + m_minBB[2]; + pts[7][2] = (k + 0.5) * m_scale + m_minBB[2]; +} +void VoxelSet::Intersect(const Plane& plane, + SArray<Vec3<double> >* const positivePts, + SArray<Vec3<double> >* const negativePts, + const size_t sampling) const +{ + const size_t nVoxels = m_voxels.Size(); + if (nVoxels == 0) + return; + const double d0 = m_scale; + double d; + Vec3<double> pts[8]; + Vec3<double> pt; + Voxel voxel; + size_t sp = 0; + size_t sn = 0; + for (size_t v = 0; v < nVoxels; ++v) { + voxel = m_voxels[v]; + pt = GetPoint(voxel); + d = plane.m_a * pt[0] + plane.m_b * pt[1] + plane.m_c * pt[2] + plane.m_d; + // if (d >= 0.0 && d <= d0) positivePts->PushBack(pt); + // else if (d < 0.0 && -d <= d0) negativePts->PushBack(pt); + if (d >= 0.0) { + if (d <= d0) { + GetPoints(voxel, pts); + for (int32_t k = 0; k < 8; ++k) { + positivePts->PushBack(pts[k]); + } + } + else { + if (++sp == sampling) { + // positivePts->PushBack(pt); + GetPoints(voxel, pts); + for (int32_t k = 0; k < 8; ++k) { + positivePts->PushBack(pts[k]); + } + sp = 0; + } + } + } + else { + if (-d <= d0) { + GetPoints(voxel, pts); + for (int32_t k = 0; k < 8; ++k) { + negativePts->PushBack(pts[k]); + } + } + else { + if (++sn == sampling) { + // negativePts->PushBack(pt); + GetPoints(voxel, pts); + for (int32_t k = 0; k < 8; ++k) { + negativePts->PushBack(pts[k]); + } + sn = 0; + } + } + } + } +} +void VoxelSet::ComputeExteriorPoints(const Plane& plane, + const Mesh& mesh, + SArray<Vec3<double> >* const exteriorPts) const +{ + const size_t nVoxels = m_voxels.Size(); + if (nVoxels == 0) + return; + double d; + Vec3<double> pt; + Vec3<double> pts[8]; + Voxel voxel; + for (size_t v = 0; v < nVoxels; ++v) { + voxel = m_voxels[v]; + pt = GetPoint(voxel); + d = plane.m_a * pt[0] + plane.m_b * pt[1] + plane.m_c * pt[2] + plane.m_d; + if (d >= 0.0) { + if (!mesh.IsInside(pt)) { + GetPoints(voxel, pts); + for (int32_t k = 0; k < 8; ++k) { + exteriorPts->PushBack(pts[k]); + } + } + } + } +} +void VoxelSet::ComputeClippedVolumes(const Plane& plane, + double& positiveVolume, + double& negativeVolume) const +{ + negativeVolume = 0.0; + positiveVolume = 0.0; + const size_t nVoxels = m_voxels.Size(); + if (nVoxels == 0) + return; + double d; + Vec3<double> pt; + size_t nPositiveVoxels = 0; + for (size_t v = 0; v < nVoxels; ++v) { + pt = GetPoint(m_voxels[v]); + d = plane.m_a * pt[0] + plane.m_b * pt[1] + plane.m_c * pt[2] + plane.m_d; + nPositiveVoxels += (d >= 0.0); + } + size_t nNegativeVoxels = nVoxels - nPositiveVoxels; + positiveVolume = m_unitVolume * nPositiveVoxels; + negativeVolume = m_unitVolume * nNegativeVoxels; +} +void VoxelSet::SelectOnSurface(PrimitiveSet* const onSurfP) const +{ + VoxelSet* const onSurf = (VoxelSet*)onSurfP; + const size_t nVoxels = m_voxels.Size(); + if (nVoxels == 0) + return; + + for (int32_t h = 0; h < 3; ++h) { + onSurf->m_minBB[h] = m_minBB[h]; + } + onSurf->m_voxels.Resize(0); + onSurf->m_scale = m_scale; + onSurf->m_unitVolume = m_unitVolume; + onSurf->m_numVoxelsOnSurface = 0; + onSurf->m_numVoxelsInsideSurface = 0; + Voxel voxel; + for (size_t v = 0; v < nVoxels; ++v) { + voxel = m_voxels[v]; + if (voxel.m_data == PRIMITIVE_ON_SURFACE) { + onSurf->m_voxels.PushBack(voxel); + ++onSurf->m_numVoxelsOnSurface; + } + } +} +void VoxelSet::Clip(const Plane& plane, + PrimitiveSet* const positivePartP, + PrimitiveSet* const negativePartP) const +{ + VoxelSet* const positivePart = (VoxelSet*)positivePartP; + VoxelSet* const negativePart = (VoxelSet*)negativePartP; + const size_t nVoxels = m_voxels.Size(); + if (nVoxels == 0) + return; + + for (int32_t h = 0; h < 3; ++h) { + negativePart->m_minBB[h] = positivePart->m_minBB[h] = m_minBB[h]; + } + positivePart->m_voxels.Resize(0); + negativePart->m_voxels.Resize(0); + positivePart->m_voxels.Allocate(nVoxels); + negativePart->m_voxels.Allocate(nVoxels); + negativePart->m_scale = positivePart->m_scale = m_scale; + negativePart->m_unitVolume = positivePart->m_unitVolume = m_unitVolume; + negativePart->m_numVoxelsOnSurface = positivePart->m_numVoxelsOnSurface = 0; + negativePart->m_numVoxelsInsideSurface = positivePart->m_numVoxelsInsideSurface = 0; + + double d; + Vec3<double> pt; + Voxel voxel; + const double d0 = m_scale; + for (size_t v = 0; v < nVoxels; ++v) { + voxel = m_voxels[v]; + pt = GetPoint(voxel); + d = plane.m_a * pt[0] + plane.m_b * pt[1] + plane.m_c * pt[2] + plane.m_d; + if (d >= 0.0) { + if (voxel.m_data == PRIMITIVE_ON_SURFACE || d <= d0) { + voxel.m_data = PRIMITIVE_ON_SURFACE; + positivePart->m_voxels.PushBack(voxel); + ++positivePart->m_numVoxelsOnSurface; + } + else { + positivePart->m_voxels.PushBack(voxel); + ++positivePart->m_numVoxelsInsideSurface; + } + } + else { + if (voxel.m_data == PRIMITIVE_ON_SURFACE || -d <= d0) { + voxel.m_data = PRIMITIVE_ON_SURFACE; + negativePart->m_voxels.PushBack(voxel); + ++negativePart->m_numVoxelsOnSurface; + } + else { + negativePart->m_voxels.PushBack(voxel); + ++negativePart->m_numVoxelsInsideSurface; + } + } + } +} +void VoxelSet::Convert(Mesh& mesh, const VOXEL_VALUE value) const +{ + const size_t nVoxels = m_voxels.Size(); + if (nVoxels == 0) + return; + Voxel voxel; + Vec3<double> pts[8]; + for (size_t v = 0; v < nVoxels; ++v) { + voxel = m_voxels[v]; + if (voxel.m_data == value) { + GetPoints(voxel, pts); + int32_t s = (int32_t)mesh.GetNPoints(); + for (int32_t k = 0; k < 8; ++k) { + mesh.AddPoint(pts[k]); + } + mesh.AddTriangle(Vec3<int32_t>(s + 0, s + 2, s + 1)); + mesh.AddTriangle(Vec3<int32_t>(s + 0, s + 3, s + 2)); + mesh.AddTriangle(Vec3<int32_t>(s + 4, s + 5, s + 6)); + mesh.AddTriangle(Vec3<int32_t>(s + 4, s + 6, s + 7)); + mesh.AddTriangle(Vec3<int32_t>(s + 7, s + 6, s + 2)); + mesh.AddTriangle(Vec3<int32_t>(s + 7, s + 2, s + 3)); + mesh.AddTriangle(Vec3<int32_t>(s + 4, s + 1, s + 5)); + mesh.AddTriangle(Vec3<int32_t>(s + 4, s + 0, s + 1)); + mesh.AddTriangle(Vec3<int32_t>(s + 6, s + 5, s + 1)); + mesh.AddTriangle(Vec3<int32_t>(s + 6, s + 1, s + 2)); + mesh.AddTriangle(Vec3<int32_t>(s + 7, s + 0, s + 4)); + mesh.AddTriangle(Vec3<int32_t>(s + 7, s + 3, s + 0)); + } + } +} +void VoxelSet::ComputePrincipalAxes() +{ + const size_t nVoxels = m_voxels.Size(); + if (nVoxels == 0) + return; + m_barycenterPCA[0] = m_barycenterPCA[1] = m_barycenterPCA[2] = 0.0; + for (size_t v = 0; v < nVoxels; ++v) { + Voxel& voxel = m_voxels[v]; + m_barycenterPCA[0] += voxel.m_coord[0]; + m_barycenterPCA[1] += voxel.m_coord[1]; + m_barycenterPCA[2] += voxel.m_coord[2]; + } + m_barycenterPCA /= (double)nVoxels; + + double covMat[3][3] = { { 0.0, 0.0, 0.0 }, + { 0.0, 0.0, 0.0 }, + { 0.0, 0.0, 0.0 } }; + double x, y, z; + for (size_t v = 0; v < nVoxels; ++v) { + Voxel& voxel = m_voxels[v]; + x = voxel.m_coord[0] - m_barycenter[0]; + y = voxel.m_coord[1] - m_barycenter[1]; + z = voxel.m_coord[2] - m_barycenter[2]; + covMat[0][0] += x * x; + covMat[1][1] += y * y; + covMat[2][2] += z * z; + covMat[0][1] += x * y; + covMat[0][2] += x * z; + covMat[1][2] += y * z; + } + covMat[0][0] /= nVoxels; + covMat[1][1] /= nVoxels; + covMat[2][2] /= nVoxels; + covMat[0][1] /= nVoxels; + covMat[0][2] /= nVoxels; + covMat[1][2] /= nVoxels; + covMat[1][0] = covMat[0][1]; + covMat[2][0] = covMat[0][2]; + covMat[2][1] = covMat[1][2]; + Diagonalize(covMat, m_Q, m_D); +} +Volume::Volume() +{ + m_dim[0] = m_dim[1] = m_dim[2] = 0; + m_minBB[0] = m_minBB[1] = m_minBB[2] = 0.0; + m_maxBB[0] = m_maxBB[1] = m_maxBB[2] = 1.0; + m_numVoxelsOnSurface = 0; + m_numVoxelsInsideSurface = 0; + m_numVoxelsOutsideSurface = 0; + m_scale = 1.0; + m_data = 0; +} +Volume::~Volume(void) +{ + delete[] m_data; +} +void Volume::Allocate() +{ + delete[] m_data; + size_t size = m_dim[0] * m_dim[1] * m_dim[2]; + m_data = new unsigned char[size]; + memset(m_data, PRIMITIVE_UNDEFINED, sizeof(unsigned char) * size); +} +void Volume::Free() +{ + delete[] m_data; + m_data = 0; +} +void Volume::FillOutsideSurface(const size_t i0, + const size_t j0, + const size_t k0, + const size_t i1, + const size_t j1, + const size_t k1) +{ + const short neighbours[6][3] = { { 1, 0, 0 }, + { 0, 1, 0 }, + { 0, 0, 1 }, + { -1, 0, 0 }, + { 0, -1, 0 }, + { 0, 0, -1 } }; + std::queue<Vec3<short> > fifo; + Vec3<short> current; + short a, b, c; + for (size_t i = i0; i < i1; ++i) { + for (size_t j = j0; j < j1; ++j) { + for (size_t k = k0; k < k1; ++k) { + + if (GetVoxel(i, j, k) == PRIMITIVE_UNDEFINED) { + current[0] = (short)i; + current[1] = (short)j; + current[2] = (short)k; + fifo.push(current); + GetVoxel(current[0], current[1], current[2]) = PRIMITIVE_OUTSIDE_SURFACE; + ++m_numVoxelsOutsideSurface; + while (fifo.size() > 0) { + current = fifo.front(); + fifo.pop(); + for (int32_t h = 0; h < 6; ++h) { + a = current[0] + neighbours[h][0]; + b = current[1] + neighbours[h][1]; + c = current[2] + neighbours[h][2]; + if (a < 0 || a >= (int32_t)m_dim[0] || b < 0 || b >= (int32_t)m_dim[1] || c < 0 || c >= (int32_t)m_dim[2]) { + continue; + } + unsigned char& v = GetVoxel(a, b, c); + if (v == PRIMITIVE_UNDEFINED) { + v = PRIMITIVE_OUTSIDE_SURFACE; + ++m_numVoxelsOutsideSurface; + fifo.push(Vec3<short>(a, b, c)); + } + } + } + } + } + } + } +} +void Volume::FillInsideSurface() +{ + const size_t i0 = m_dim[0]; + const size_t j0 = m_dim[1]; + const size_t k0 = m_dim[2]; + for (size_t i = 0; i < i0; ++i) { + for (size_t j = 0; j < j0; ++j) { + for (size_t k = 0; k < k0; ++k) { + unsigned char& v = GetVoxel(i, j, k); + if (v == PRIMITIVE_UNDEFINED) { + v = PRIMITIVE_INSIDE_SURFACE; + ++m_numVoxelsInsideSurface; + } + } + } + } +} +void Volume::Convert(Mesh& mesh, const VOXEL_VALUE value) const +{ + const size_t i0 = m_dim[0]; + const size_t j0 = m_dim[1]; + const size_t k0 = m_dim[2]; + for (size_t i = 0; i < i0; ++i) { + for (size_t j = 0; j < j0; ++j) { + for (size_t k = 0; k < k0; ++k) { + const unsigned char& voxel = GetVoxel(i, j, k); + if (voxel == value) { + Vec3<double> p0((i - 0.5) * m_scale, (j - 0.5) * m_scale, (k - 0.5) * m_scale); + Vec3<double> p1((i + 0.5) * m_scale, (j - 0.5) * m_scale, (k - 0.5) * m_scale); + Vec3<double> p2((i + 0.5) * m_scale, (j + 0.5) * m_scale, (k - 0.5) * m_scale); + Vec3<double> p3((i - 0.5) * m_scale, (j + 0.5) * m_scale, (k - 0.5) * m_scale); + Vec3<double> p4((i - 0.5) * m_scale, (j - 0.5) * m_scale, (k + 0.5) * m_scale); + Vec3<double> p5((i + 0.5) * m_scale, (j - 0.5) * m_scale, (k + 0.5) * m_scale); + Vec3<double> p6((i + 0.5) * m_scale, (j + 0.5) * m_scale, (k + 0.5) * m_scale); + Vec3<double> p7((i - 0.5) * m_scale, (j + 0.5) * m_scale, (k + 0.5) * m_scale); + int32_t s = (int32_t)mesh.GetNPoints(); + mesh.AddPoint(p0 + m_minBB); + mesh.AddPoint(p1 + m_minBB); + mesh.AddPoint(p2 + m_minBB); + mesh.AddPoint(p3 + m_minBB); + mesh.AddPoint(p4 + m_minBB); + mesh.AddPoint(p5 + m_minBB); + mesh.AddPoint(p6 + m_minBB); + mesh.AddPoint(p7 + m_minBB); + mesh.AddTriangle(Vec3<int32_t>(s + 0, s + 2, s + 1)); + mesh.AddTriangle(Vec3<int32_t>(s + 0, s + 3, s + 2)); + mesh.AddTriangle(Vec3<int32_t>(s + 4, s + 5, s + 6)); + mesh.AddTriangle(Vec3<int32_t>(s + 4, s + 6, s + 7)); + mesh.AddTriangle(Vec3<int32_t>(s + 7, s + 6, s + 2)); + mesh.AddTriangle(Vec3<int32_t>(s + 7, s + 2, s + 3)); + mesh.AddTriangle(Vec3<int32_t>(s + 4, s + 1, s + 5)); + mesh.AddTriangle(Vec3<int32_t>(s + 4, s + 0, s + 1)); + mesh.AddTriangle(Vec3<int32_t>(s + 6, s + 5, s + 1)); + mesh.AddTriangle(Vec3<int32_t>(s + 6, s + 1, s + 2)); + mesh.AddTriangle(Vec3<int32_t>(s + 7, s + 0, s + 4)); + mesh.AddTriangle(Vec3<int32_t>(s + 7, s + 3, s + 0)); + } + } + } + } +} +void Volume::Convert(VoxelSet& vset) const +{ + for (int32_t h = 0; h < 3; ++h) { + vset.m_minBB[h] = m_minBB[h]; + } + vset.m_voxels.Allocate(m_numVoxelsInsideSurface + m_numVoxelsOnSurface); + vset.m_scale = m_scale; + vset.m_unitVolume = m_scale * m_scale * m_scale; + const short i0 = (short)m_dim[0]; + const short j0 = (short)m_dim[1]; + const short k0 = (short)m_dim[2]; + Voxel voxel; + vset.m_numVoxelsOnSurface = 0; + vset.m_numVoxelsInsideSurface = 0; + for (short i = 0; i < i0; ++i) { + for (short j = 0; j < j0; ++j) { + for (short k = 0; k < k0; ++k) { + const unsigned char& value = GetVoxel(i, j, k); + if (value == PRIMITIVE_INSIDE_SURFACE) { + voxel.m_coord[0] = i; + voxel.m_coord[1] = j; + voxel.m_coord[2] = k; + voxel.m_data = PRIMITIVE_INSIDE_SURFACE; + vset.m_voxels.PushBack(voxel); + ++vset.m_numVoxelsInsideSurface; + } + else if (value == PRIMITIVE_ON_SURFACE) { + voxel.m_coord[0] = i; + voxel.m_coord[1] = j; + voxel.m_coord[2] = k; + voxel.m_data = PRIMITIVE_ON_SURFACE; + vset.m_voxels.PushBack(voxel); + ++vset.m_numVoxelsOnSurface; + } + } + } + } +} + +void Volume::Convert(TetrahedronSet& tset) const +{ + tset.m_tetrahedra.Allocate(5 * (m_numVoxelsInsideSurface + m_numVoxelsOnSurface)); + tset.m_scale = m_scale; + const short i0 = (short)m_dim[0]; + const short j0 = (short)m_dim[1]; + const short k0 = (short)m_dim[2]; + tset.m_numTetrahedraOnSurface = 0; + tset.m_numTetrahedraInsideSurface = 0; + Tetrahedron tetrahedron; + for (short i = 0; i < i0; ++i) { + for (short j = 0; j < j0; ++j) { + for (short k = 0; k < k0; ++k) { + const unsigned char& value = GetVoxel(i, j, k); + if (value == PRIMITIVE_INSIDE_SURFACE || value == PRIMITIVE_ON_SURFACE) { + tetrahedron.m_data = value; + Vec3<double> p1((i - 0.5) * m_scale + m_minBB[0], (j - 0.5) * m_scale + m_minBB[1], (k - 0.5) * m_scale + m_minBB[2]); + Vec3<double> p2((i + 0.5) * m_scale + m_minBB[0], (j - 0.5) * m_scale + m_minBB[1], (k - 0.5) * m_scale + m_minBB[2]); + Vec3<double> p3((i + 0.5) * m_scale + m_minBB[0], (j + 0.5) * m_scale + m_minBB[1], (k - 0.5) * m_scale + m_minBB[2]); + Vec3<double> p4((i - 0.5) * m_scale + m_minBB[0], (j + 0.5) * m_scale + m_minBB[1], (k - 0.5) * m_scale + m_minBB[2]); + Vec3<double> p5((i - 0.5) * m_scale + m_minBB[0], (j - 0.5) * m_scale + m_minBB[1], (k + 0.5) * m_scale + m_minBB[2]); + Vec3<double> p6((i + 0.5) * m_scale + m_minBB[0], (j - 0.5) * m_scale + m_minBB[1], (k + 0.5) * m_scale + m_minBB[2]); + Vec3<double> p7((i + 0.5) * m_scale + m_minBB[0], (j + 0.5) * m_scale + m_minBB[1], (k + 0.5) * m_scale + m_minBB[2]); + Vec3<double> p8((i - 0.5) * m_scale + m_minBB[0], (j + 0.5) * m_scale + m_minBB[1], (k + 0.5) * m_scale + m_minBB[2]); + + tetrahedron.m_pts[0] = p2; + tetrahedron.m_pts[1] = p4; + tetrahedron.m_pts[2] = p7; + tetrahedron.m_pts[3] = p5; + tset.m_tetrahedra.PushBack(tetrahedron); + + tetrahedron.m_pts[0] = p6; + tetrahedron.m_pts[1] = p2; + tetrahedron.m_pts[2] = p7; + tetrahedron.m_pts[3] = p5; + tset.m_tetrahedra.PushBack(tetrahedron); + + tetrahedron.m_pts[0] = p3; + tetrahedron.m_pts[1] = p4; + tetrahedron.m_pts[2] = p7; + tetrahedron.m_pts[3] = p2; + tset.m_tetrahedra.PushBack(tetrahedron); + + tetrahedron.m_pts[0] = p1; + tetrahedron.m_pts[1] = p4; + tetrahedron.m_pts[2] = p2; + tetrahedron.m_pts[3] = p5; + tset.m_tetrahedra.PushBack(tetrahedron); + + tetrahedron.m_pts[0] = p8; + tetrahedron.m_pts[1] = p5; + tetrahedron.m_pts[2] = p7; + tetrahedron.m_pts[3] = p4; + tset.m_tetrahedra.PushBack(tetrahedron); + if (value == PRIMITIVE_INSIDE_SURFACE) { + tset.m_numTetrahedraInsideSurface += 5; + } + else { + tset.m_numTetrahedraOnSurface += 5; + } + } + } + } + } +} + +void Volume::AlignToPrincipalAxes(double (&rot)[3][3]) const +{ + const short i0 = (short)m_dim[0]; + const short j0 = (short)m_dim[1]; + const short k0 = (short)m_dim[2]; + Vec3<double> barycenter(0.0); + size_t nVoxels = 0; + for (short i = 0; i < i0; ++i) { + for (short j = 0; j < j0; ++j) { + for (short k = 0; k < k0; ++k) { + const unsigned char& value = GetVoxel(i, j, k); + if (value == PRIMITIVE_INSIDE_SURFACE || value == PRIMITIVE_ON_SURFACE) { + barycenter[0] += i; + barycenter[1] += j; + barycenter[2] += k; + ++nVoxels; + } + } + } + } + barycenter /= (double)nVoxels; + + double covMat[3][3] = { { 0.0, 0.0, 0.0 }, + { 0.0, 0.0, 0.0 }, + { 0.0, 0.0, 0.0 } }; + double x, y, z; + for (short i = 0; i < i0; ++i) { + for (short j = 0; j < j0; ++j) { + for (short k = 0; k < k0; ++k) { + const unsigned char& value = GetVoxel(i, j, k); + if (value == PRIMITIVE_INSIDE_SURFACE || value == PRIMITIVE_ON_SURFACE) { + x = i - barycenter[0]; + y = j - barycenter[1]; + z = k - barycenter[2]; + covMat[0][0] += x * x; + covMat[1][1] += y * y; + covMat[2][2] += z * z; + covMat[0][1] += x * y; + covMat[0][2] += x * z; + covMat[1][2] += y * z; + } + } + } + } + covMat[1][0] = covMat[0][1]; + covMat[2][0] = covMat[0][2]; + covMat[2][1] = covMat[1][2]; + double D[3][3]; + Diagonalize(covMat, rot, D); +} +TetrahedronSet::TetrahedronSet() +{ + m_minBB[0] = m_minBB[1] = m_minBB[2] = 0.0; + m_maxBB[0] = m_maxBB[1] = m_maxBB[2] = 1.0; + m_barycenter[0] = m_barycenter[1] = m_barycenter[2] = 0.0; + m_scale = 1.0; + m_numTetrahedraOnSurface = 0; + m_numTetrahedraInsideSurface = 0; + memset(m_Q, 0, sizeof(double) * 9); + memset(m_D, 0, sizeof(double) * 9); +} +TetrahedronSet::~TetrahedronSet(void) +{ +} +void TetrahedronSet::ComputeBB() +{ + const size_t nTetrahedra = m_tetrahedra.Size(); + if (nTetrahedra == 0) + return; + + for (int32_t h = 0; h < 3; ++h) { + m_minBB[h] = m_maxBB[h] = m_tetrahedra[0].m_pts[0][h]; + m_barycenter[h] = 0.0; + } + for (size_t p = 0; p < nTetrahedra; ++p) { + for (int32_t i = 0; i < 4; ++i) { + for (int32_t h = 0; h < 3; ++h) { + if (m_minBB[h] > m_tetrahedra[p].m_pts[i][h]) + m_minBB[h] = m_tetrahedra[p].m_pts[i][h]; + if (m_maxBB[h] < m_tetrahedra[p].m_pts[i][h]) + m_maxBB[h] = m_tetrahedra[p].m_pts[i][h]; + m_barycenter[h] += m_tetrahedra[p].m_pts[i][h]; + } + } + } + m_barycenter /= (double)(4 * nTetrahedra); +} +void TetrahedronSet::ComputeConvexHull(Mesh& meshCH, const size_t sampling) const +{ + const size_t CLUSTER_SIZE = 65536; + const size_t nTetrahedra = m_tetrahedra.Size(); + if (nTetrahedra == 0) + return; + + SArray<Vec3<double> > cpoints; + + Vec3<double>* points = new Vec3<double>[CLUSTER_SIZE]; + size_t p = 0; + while (p < nTetrahedra) { + size_t q = 0; + size_t s = 0; + while (q < CLUSTER_SIZE && p < nTetrahedra) { + if (m_tetrahedra[p].m_data == PRIMITIVE_ON_SURFACE) { + ++s; + if (s == sampling) { + s = 0; + for (int32_t a = 0; a < 4; ++a) { + points[q++] = m_tetrahedra[p].m_pts[a]; + for (int32_t xx = 0; xx < 3; ++xx) { + assert(m_tetrahedra[p].m_pts[a][xx] + EPS >= m_minBB[xx]); + assert(m_tetrahedra[p].m_pts[a][xx] <= m_maxBB[xx] + EPS); + } + } + } + } + ++p; + } + btConvexHullComputer ch; + ch.compute((double*)points, 3 * sizeof(double), (int32_t)q, -1.0, -1.0); + for (int32_t v = 0; v < ch.vertices.size(); v++) { + cpoints.PushBack(Vec3<double>(ch.vertices[v].getX(), ch.vertices[v].getY(), ch.vertices[v].getZ())); + } + } + delete[] points; + + points = cpoints.Data(); + btConvexHullComputer ch; + ch.compute((double*)points, 3 * sizeof(double), (int32_t)cpoints.Size(), -1.0, -1.0); + meshCH.ResizePoints(0); + meshCH.ResizeTriangles(0); + for (int32_t v = 0; v < ch.vertices.size(); v++) { + meshCH.AddPoint(Vec3<double>(ch.vertices[v].getX(), ch.vertices[v].getY(), ch.vertices[v].getZ())); + } + const int32_t nt = ch.faces.size(); + for (int32_t t = 0; t < nt; ++t) { + const btConvexHullComputer::Edge* sourceEdge = &(ch.edges[ch.faces[t]]); + int32_t a = sourceEdge->getSourceVertex(); + int32_t b = sourceEdge->getTargetVertex(); + const btConvexHullComputer::Edge* edge = sourceEdge->getNextEdgeOfFace(); + int32_t c = edge->getTargetVertex(); + while (c != a) { + meshCH.AddTriangle(Vec3<int32_t>(a, b, c)); + edge = edge->getNextEdgeOfFace(); + b = c; + c = edge->getTargetVertex(); + } + } +} +inline bool TetrahedronSet::Add(Tetrahedron& tetrahedron) +{ + double v = ComputeVolume4(tetrahedron.m_pts[0], tetrahedron.m_pts[1], tetrahedron.m_pts[2], tetrahedron.m_pts[3]); + + const double EPS = 0.0000000001; + if (fabs(v) < EPS) { + return false; + } + else if (v < 0.0) { + Vec3<double> tmp = tetrahedron.m_pts[0]; + tetrahedron.m_pts[0] = tetrahedron.m_pts[1]; + tetrahedron.m_pts[1] = tmp; + } + + for (int32_t a = 0; a < 4; ++a) { + for (int32_t xx = 0; xx < 3; ++xx) { + assert(tetrahedron.m_pts[a][xx] + EPS >= m_minBB[xx]); + assert(tetrahedron.m_pts[a][xx] <= m_maxBB[xx] + EPS); + } + } + m_tetrahedra.PushBack(tetrahedron); + return true; +} + +void TetrahedronSet::AddClippedTetrahedra(const Vec3<double> (&pts)[10], const int32_t nPts) +{ + const int32_t tetF[4][3] = { { 0, 1, 2 }, { 2, 1, 3 }, { 3, 1, 0 }, { 3, 0, 2 } }; + if (nPts < 4) { + return; + } + else if (nPts == 4) { + Tetrahedron tetrahedron; + tetrahedron.m_data = PRIMITIVE_ON_SURFACE; + tetrahedron.m_pts[0] = pts[0]; + tetrahedron.m_pts[1] = pts[1]; + tetrahedron.m_pts[2] = pts[2]; + tetrahedron.m_pts[3] = pts[3]; + if (Add(tetrahedron)) { + ++m_numTetrahedraOnSurface; + } + } + else if (nPts == 5) { + const int32_t tet[15][4] = { + { 0, 1, 2, 3 }, { 1, 2, 3, 4 }, { 0, 2, 3, 4 }, { 0, 1, 3, 4 }, { 0, 1, 2, 4 }, + }; + const int32_t rem[5] = { 4, 0, 1, 2, 3 }; + double maxVol = 0.0; + int32_t h0 = -1; + Tetrahedron tetrahedron0; + tetrahedron0.m_data = PRIMITIVE_ON_SURFACE; + for (int32_t h = 0; h < 5; ++h) { + double v = ComputeVolume4(pts[tet[h][0]], pts[tet[h][1]], pts[tet[h][2]], pts[tet[h][3]]); + if (v > maxVol) { + h0 = h; + tetrahedron0.m_pts[0] = pts[tet[h][0]]; + tetrahedron0.m_pts[1] = pts[tet[h][1]]; + tetrahedron0.m_pts[2] = pts[tet[h][2]]; + tetrahedron0.m_pts[3] = pts[tet[h][3]]; + maxVol = v; + } + else if (-v > maxVol) { + h0 = h; + tetrahedron0.m_pts[0] = pts[tet[h][1]]; + tetrahedron0.m_pts[1] = pts[tet[h][0]]; + tetrahedron0.m_pts[2] = pts[tet[h][2]]; + tetrahedron0.m_pts[3] = pts[tet[h][3]]; + maxVol = -v; + } + } + if (h0 == -1) + return; + if (Add(tetrahedron0)) { + ++m_numTetrahedraOnSurface; + } + else { + return; + } + int32_t a = rem[h0]; + maxVol = 0.0; + int32_t h1 = -1; + Tetrahedron tetrahedron1; + tetrahedron1.m_data = PRIMITIVE_ON_SURFACE; + for (int32_t h = 0; h < 4; ++h) { + double v = ComputeVolume4(pts[a], tetrahedron0.m_pts[tetF[h][0]], tetrahedron0.m_pts[tetF[h][1]], tetrahedron0.m_pts[tetF[h][2]]); + if (v > maxVol) { + h1 = h; + tetrahedron1.m_pts[0] = pts[a]; + tetrahedron1.m_pts[1] = tetrahedron0.m_pts[tetF[h][0]]; + tetrahedron1.m_pts[2] = tetrahedron0.m_pts[tetF[h][1]]; + tetrahedron1.m_pts[3] = tetrahedron0.m_pts[tetF[h][2]]; + maxVol = v; + } + } + if (h1 == -1 && Add(tetrahedron1)) { + ++m_numTetrahedraOnSurface; + } + } + else if (nPts == 6) { + + const int32_t tet[15][4] = { { 2, 3, 4, 5 }, { 1, 3, 4, 5 }, { 1, 2, 4, 5 }, { 1, 2, 3, 5 }, { 1, 2, 3, 4 }, + { 0, 3, 4, 5 }, { 0, 2, 4, 5 }, { 0, 2, 3, 5 }, { 0, 2, 3, 4 }, { 0, 1, 4, 5 }, + { 0, 1, 3, 5 }, { 0, 1, 3, 4 }, { 0, 1, 2, 5 }, { 0, 1, 2, 4 }, { 0, 1, 2, 3 } }; + const int32_t rem[15][2] = { { 0, 1 }, { 0, 2 }, { 0, 3 }, { 0, 4 }, { 0, 5 }, + { 1, 2 }, { 1, 3 }, { 1, 4 }, { 1, 5 }, { 2, 3 }, + { 2, 4 }, { 2, 5 }, { 3, 4 }, { 3, 5 }, { 4, 5 } }; + double maxVol = 0.0; + int32_t h0 = -1; + Tetrahedron tetrahedron0; + tetrahedron0.m_data = PRIMITIVE_ON_SURFACE; + for (int32_t h = 0; h < 15; ++h) { + double v = ComputeVolume4(pts[tet[h][0]], pts[tet[h][1]], pts[tet[h][2]], pts[tet[h][3]]); + if (v > maxVol) { + h0 = h; + tetrahedron0.m_pts[0] = pts[tet[h][0]]; + tetrahedron0.m_pts[1] = pts[tet[h][1]]; + tetrahedron0.m_pts[2] = pts[tet[h][2]]; + tetrahedron0.m_pts[3] = pts[tet[h][3]]; + maxVol = v; + } + else if (-v > maxVol) { + h0 = h; + tetrahedron0.m_pts[0] = pts[tet[h][1]]; + tetrahedron0.m_pts[1] = pts[tet[h][0]]; + tetrahedron0.m_pts[2] = pts[tet[h][2]]; + tetrahedron0.m_pts[3] = pts[tet[h][3]]; + maxVol = -v; + } + } + if (h0 == -1) + return; + if (Add(tetrahedron0)) { + ++m_numTetrahedraOnSurface; + } + else { + return; + } + + int32_t a0 = rem[h0][0]; + int32_t a1 = rem[h0][1]; + int32_t h1 = -1; + Tetrahedron tetrahedron1; + tetrahedron1.m_data = PRIMITIVE_ON_SURFACE; + maxVol = 0.0; + for (int32_t h = 0; h < 4; ++h) { + double v = ComputeVolume4(pts[a0], tetrahedron0.m_pts[tetF[h][0]], tetrahedron0.m_pts[tetF[h][1]], tetrahedron0.m_pts[tetF[h][2]]); + if (v > maxVol) { + h1 = h; + tetrahedron1.m_pts[0] = pts[a0]; + tetrahedron1.m_pts[1] = tetrahedron0.m_pts[tetF[h][0]]; + tetrahedron1.m_pts[2] = tetrahedron0.m_pts[tetF[h][1]]; + tetrahedron1.m_pts[3] = tetrahedron0.m_pts[tetF[h][2]]; + maxVol = v; + } + } + if (h1 != -1 && Add(tetrahedron1)) { + ++m_numTetrahedraOnSurface; + } + else { + h1 = -1; + } + maxVol = 0.0; + int32_t h2 = -1; + Tetrahedron tetrahedron2; + tetrahedron2.m_data = PRIMITIVE_ON_SURFACE; + for (int32_t h = 0; h < 4; ++h) { + double v = ComputeVolume4(pts[a0], tetrahedron0.m_pts[tetF[h][0]], tetrahedron0.m_pts[tetF[h][1]], tetrahedron0.m_pts[tetF[h][2]]); + if (h == h1) + continue; + if (v > maxVol) { + h2 = h; + tetrahedron2.m_pts[0] = pts[a1]; + tetrahedron2.m_pts[1] = tetrahedron0.m_pts[tetF[h][0]]; + tetrahedron2.m_pts[2] = tetrahedron0.m_pts[tetF[h][1]]; + tetrahedron2.m_pts[3] = tetrahedron0.m_pts[tetF[h][2]]; + maxVol = v; + } + } + if (h1 != -1) { + for (int32_t h = 0; h < 4; ++h) { + double v = ComputeVolume4(pts[a1], tetrahedron1.m_pts[tetF[h][0]], tetrahedron1.m_pts[tetF[h][1]], tetrahedron1.m_pts[tetF[h][2]]); + if (h == 1) + continue; + if (v > maxVol) { + h2 = h; + tetrahedron2.m_pts[0] = pts[a1]; + tetrahedron2.m_pts[1] = tetrahedron1.m_pts[tetF[h][0]]; + tetrahedron2.m_pts[2] = tetrahedron1.m_pts[tetF[h][1]]; + tetrahedron2.m_pts[3] = tetrahedron1.m_pts[tetF[h][2]]; + maxVol = v; + } + } + } + if (h2 != -1 && Add(tetrahedron2)) { + ++m_numTetrahedraOnSurface; + } + } + else { + assert(0); + } +} + +void TetrahedronSet::Intersect(const Plane& plane, + SArray<Vec3<double> >* const positivePts, + SArray<Vec3<double> >* const negativePts, + const size_t sampling) const +{ + const size_t nTetrahedra = m_tetrahedra.Size(); + if (nTetrahedra == 0) + return; +} +void TetrahedronSet::ComputeExteriorPoints(const Plane& plane, + const Mesh& mesh, + SArray<Vec3<double> >* const exteriorPts) const +{ +} +void TetrahedronSet::ComputeClippedVolumes(const Plane& plane, + double& positiveVolume, + double& negativeVolume) const +{ + const size_t nTetrahedra = m_tetrahedra.Size(); + if (nTetrahedra == 0) + return; +} + +void TetrahedronSet::SelectOnSurface(PrimitiveSet* const onSurfP) const +{ + TetrahedronSet* const onSurf = (TetrahedronSet*)onSurfP; + const size_t nTetrahedra = m_tetrahedra.Size(); + if (nTetrahedra == 0) + return; + onSurf->m_tetrahedra.Resize(0); + onSurf->m_scale = m_scale; + onSurf->m_numTetrahedraOnSurface = 0; + onSurf->m_numTetrahedraInsideSurface = 0; + onSurf->m_barycenter = m_barycenter; + onSurf->m_minBB = m_minBB; + onSurf->m_maxBB = m_maxBB; + for (int32_t i = 0; i < 3; ++i) { + for (int32_t j = 0; j < 3; ++j) { + onSurf->m_Q[i][j] = m_Q[i][j]; + onSurf->m_D[i][j] = m_D[i][j]; + } + } + Tetrahedron tetrahedron; + for (size_t v = 0; v < nTetrahedra; ++v) { + tetrahedron = m_tetrahedra[v]; + if (tetrahedron.m_data == PRIMITIVE_ON_SURFACE) { + onSurf->m_tetrahedra.PushBack(tetrahedron); + ++onSurf->m_numTetrahedraOnSurface; + } + } +} +void TetrahedronSet::Clip(const Plane& plane, + PrimitiveSet* const positivePartP, + PrimitiveSet* const negativePartP) const +{ + TetrahedronSet* const positivePart = (TetrahedronSet*)positivePartP; + TetrahedronSet* const negativePart = (TetrahedronSet*)negativePartP; + const size_t nTetrahedra = m_tetrahedra.Size(); + if (nTetrahedra == 0) + return; + positivePart->m_tetrahedra.Resize(0); + negativePart->m_tetrahedra.Resize(0); + positivePart->m_tetrahedra.Allocate(nTetrahedra); + negativePart->m_tetrahedra.Allocate(nTetrahedra); + negativePart->m_scale = positivePart->m_scale = m_scale; + negativePart->m_numTetrahedraOnSurface = positivePart->m_numTetrahedraOnSurface = 0; + negativePart->m_numTetrahedraInsideSurface = positivePart->m_numTetrahedraInsideSurface = 0; + negativePart->m_barycenter = m_barycenter; + positivePart->m_barycenter = m_barycenter; + negativePart->m_minBB = m_minBB; + positivePart->m_minBB = m_minBB; + negativePart->m_maxBB = m_maxBB; + positivePart->m_maxBB = m_maxBB; + for (int32_t i = 0; i < 3; ++i) { + for (int32_t j = 0; j < 3; ++j) { + negativePart->m_Q[i][j] = positivePart->m_Q[i][j] = m_Q[i][j]; + negativePart->m_D[i][j] = positivePart->m_D[i][j] = m_D[i][j]; + } + } + + Tetrahedron tetrahedron; + double delta, alpha; + int32_t sign[4]; + int32_t npos, nneg; + Vec3<double> posPts[10]; + Vec3<double> negPts[10]; + Vec3<double> P0, P1, M; + const Vec3<double> n(plane.m_a, plane.m_b, plane.m_c); + const int32_t edges[6][2] = { { 0, 1 }, { 0, 2 }, { 0, 3 }, { 1, 2 }, { 1, 3 }, { 2, 3 } }; + double dist; + for (size_t v = 0; v < nTetrahedra; ++v) { + tetrahedron = m_tetrahedra[v]; + npos = nneg = 0; + for (int32_t i = 0; i < 4; ++i) { + dist = plane.m_a * tetrahedron.m_pts[i][0] + plane.m_b * tetrahedron.m_pts[i][1] + plane.m_c * tetrahedron.m_pts[i][2] + plane.m_d; + if (dist > 0.0) { + sign[i] = 1; + posPts[npos] = tetrahedron.m_pts[i]; + ++npos; + } + else { + sign[i] = -1; + negPts[nneg] = tetrahedron.m_pts[i]; + ++nneg; + } + } + + if (npos == 4) { + positivePart->Add(tetrahedron); + if (tetrahedron.m_data == PRIMITIVE_ON_SURFACE) { + ++positivePart->m_numTetrahedraOnSurface; + } + else { + ++positivePart->m_numTetrahedraInsideSurface; + } + } + else if (nneg == 4) { + negativePart->Add(tetrahedron); + if (tetrahedron.m_data == PRIMITIVE_ON_SURFACE) { + ++negativePart->m_numTetrahedraOnSurface; + } + else { + ++negativePart->m_numTetrahedraInsideSurface; + } + } + else { + int32_t nnew = 0; + for (int32_t j = 0; j < 6; ++j) { + if (sign[edges[j][0]] * sign[edges[j][1]] == -1) { + P0 = tetrahedron.m_pts[edges[j][0]]; + P1 = tetrahedron.m_pts[edges[j][1]]; + delta = (P0 - P1) * n; + alpha = -(plane.m_d + (n * P1)) / delta; + assert(alpha >= 0.0 && alpha <= 1.0); + M = alpha * P0 + (1 - alpha) * P1; + for (int32_t xx = 0; xx < 3; ++xx) { + assert(M[xx] + EPS >= m_minBB[xx]); + assert(M[xx] <= m_maxBB[xx] + EPS); + } + posPts[npos++] = M; + negPts[nneg++] = M; + ++nnew; + } + } + negativePart->AddClippedTetrahedra(negPts, nneg); + positivePart->AddClippedTetrahedra(posPts, npos); + } + } +} +void TetrahedronSet::Convert(Mesh& mesh, const VOXEL_VALUE value) const +{ + const size_t nTetrahedra = m_tetrahedra.Size(); + if (nTetrahedra == 0) + return; + for (size_t v = 0; v < nTetrahedra; ++v) { + const Tetrahedron& tetrahedron = m_tetrahedra[v]; + if (tetrahedron.m_data == value) { + int32_t s = (int32_t)mesh.GetNPoints(); + mesh.AddPoint(tetrahedron.m_pts[0]); + mesh.AddPoint(tetrahedron.m_pts[1]); + mesh.AddPoint(tetrahedron.m_pts[2]); + mesh.AddPoint(tetrahedron.m_pts[3]); + mesh.AddTriangle(Vec3<int32_t>(s + 0, s + 1, s + 2)); + mesh.AddTriangle(Vec3<int32_t>(s + 2, s + 1, s + 3)); + mesh.AddTriangle(Vec3<int32_t>(s + 3, s + 1, s + 0)); + mesh.AddTriangle(Vec3<int32_t>(s + 3, s + 0, s + 2)); + } + } +} +const double TetrahedronSet::ComputeVolume() const +{ + const size_t nTetrahedra = m_tetrahedra.Size(); + if (nTetrahedra == 0) + return 0.0; + double volume = 0.0; + for (size_t v = 0; v < nTetrahedra; ++v) { + const Tetrahedron& tetrahedron = m_tetrahedra[v]; + volume += fabs(ComputeVolume4(tetrahedron.m_pts[0], tetrahedron.m_pts[1], tetrahedron.m_pts[2], tetrahedron.m_pts[3])); + } + return volume / 6.0; +} +const double TetrahedronSet::ComputeMaxVolumeError() const +{ + const size_t nTetrahedra = m_tetrahedra.Size(); + if (nTetrahedra == 0) + return 0.0; + double volume = 0.0; + for (size_t v = 0; v < nTetrahedra; ++v) { + const Tetrahedron& tetrahedron = m_tetrahedra[v]; + if (tetrahedron.m_data == PRIMITIVE_ON_SURFACE) { + volume += fabs(ComputeVolume4(tetrahedron.m_pts[0], tetrahedron.m_pts[1], tetrahedron.m_pts[2], tetrahedron.m_pts[3])); + } + } + return volume / 6.0; +} +void TetrahedronSet::RevertAlignToPrincipalAxes() +{ + const size_t nTetrahedra = m_tetrahedra.Size(); + if (nTetrahedra == 0) + return; + double x, y, z; + for (size_t v = 0; v < nTetrahedra; ++v) { + Tetrahedron& tetrahedron = m_tetrahedra[v]; + for (int32_t i = 0; i < 4; ++i) { + x = tetrahedron.m_pts[i][0] - m_barycenter[0]; + y = tetrahedron.m_pts[i][1] - m_barycenter[1]; + z = tetrahedron.m_pts[i][2] - m_barycenter[2]; + tetrahedron.m_pts[i][0] = m_Q[0][0] * x + m_Q[0][1] * y + m_Q[0][2] * z + m_barycenter[0]; + tetrahedron.m_pts[i][1] = m_Q[1][0] * x + m_Q[1][1] * y + m_Q[1][2] * z + m_barycenter[1]; + tetrahedron.m_pts[i][2] = m_Q[2][0] * x + m_Q[2][1] * y + m_Q[2][2] * z + m_barycenter[2]; + } + } + ComputeBB(); +} +void TetrahedronSet::ComputePrincipalAxes() +{ + const size_t nTetrahedra = m_tetrahedra.Size(); + if (nTetrahedra == 0) + return; + double covMat[3][3] = { { 0.0, 0.0, 0.0 }, + { 0.0, 0.0, 0.0 }, + { 0.0, 0.0, 0.0 } }; + double x, y, z; + for (size_t v = 0; v < nTetrahedra; ++v) { + Tetrahedron& tetrahedron = m_tetrahedra[v]; + for (int32_t i = 0; i < 4; ++i) { + x = tetrahedron.m_pts[i][0] - m_barycenter[0]; + y = tetrahedron.m_pts[i][1] - m_barycenter[1]; + z = tetrahedron.m_pts[i][2] - m_barycenter[2]; + covMat[0][0] += x * x; + covMat[1][1] += y * y; + covMat[2][2] += z * z; + covMat[0][1] += x * y; + covMat[0][2] += x * z; + covMat[1][2] += y * z; + } + } + double n = nTetrahedra * 4.0; + covMat[0][0] /= n; + covMat[1][1] /= n; + covMat[2][2] /= n; + covMat[0][1] /= n; + covMat[0][2] /= n; + covMat[1][2] /= n; + covMat[1][0] = covMat[0][1]; + covMat[2][0] = covMat[0][2]; + covMat[2][1] = covMat[1][2]; + Diagonalize(covMat, m_Q, m_D); +} +void TetrahedronSet::AlignToPrincipalAxes() +{ + const size_t nTetrahedra = m_tetrahedra.Size(); + if (nTetrahedra == 0) + return; + double x, y, z; + for (size_t v = 0; v < nTetrahedra; ++v) { + Tetrahedron& tetrahedron = m_tetrahedra[v]; + for (int32_t i = 0; i < 4; ++i) { + x = tetrahedron.m_pts[i][0] - m_barycenter[0]; + y = tetrahedron.m_pts[i][1] - m_barycenter[1]; + z = tetrahedron.m_pts[i][2] - m_barycenter[2]; + tetrahedron.m_pts[i][0] = m_Q[0][0] * x + m_Q[1][0] * y + m_Q[2][0] * z + m_barycenter[0]; + tetrahedron.m_pts[i][1] = m_Q[0][1] * x + m_Q[1][1] * y + m_Q[2][1] * z + m_barycenter[1]; + tetrahedron.m_pts[i][2] = m_Q[0][2] * x + m_Q[1][2] * y + m_Q[2][2] * z + m_barycenter[2]; + } + } + ComputeBB(); +} +} |