5276 lines
109 KiB
C++
5276 lines
109 KiB
C++
// 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
|