bool getBestFitPlane(unsigned int vcount,
const float *points,
unsigned int vstride,
const float *weights,
unsigned int wstride,
float *plane)
{
bool ret = false;
Vec3 kOrigin(0,0,0);
float wtotal = 0;
if ( 1 )
{
const char *source = (const char *) points;
const char *wsource = (const char *) weights;
for (unsigned int i=0; i<vcount; i++)
{
const float *p = (const float *) source;
float w = 1;
if ( wsource )
{
const float *ws = (const float *) wsource;
w = *ws; //
wsource+=wstride;
}
kOrigin.x+=p[0]*w;
kOrigin.y+=p[1]*w;
kOrigin.z+=p[2]*w;
wtotal+=w;
source+=vstride;
}
}
float recip = 1.0f / wtotal; // reciprocol of total weighting
kOrigin.x*=recip;
kOrigin.y*=recip;
kOrigin.z*=recip;
float fSumXX=0;
float fSumXY=0;
float fSumXZ=0;
float fSumYY=0;
float fSumYZ=0;
float fSumZZ=0;
if ( 1 )
{
const char *source = (const char *) points;
const char *wsource = (const char *) weights;
for (unsigned int i=0; i<vcount; i++)
{
const float *p = (const float *) source;
float w = 1;
if ( wsource )
{
const float *ws = (const float *) wsource;
w = *ws; //
wsource+=wstride;
}
Vec3 kDiff;
kDiff.x = w*(p[0] - kOrigin.x); // apply vertex weighting!
kDiff.y = w*(p[1] - kOrigin.y);
kDiff.z = w*(p[2] - kOrigin.z);
fSumXX+= kDiff.x * kDiff.x; // sume of the squares of the differences.
fSumXY+= kDiff.x * kDiff.y; // sume of the squares of the differences.
fSumXZ+= kDiff.x * kDiff.z; // sume of the squares of the differences.
fSumYY+= kDiff.y * kDiff.y;
fSumYZ+= kDiff.y * kDiff.z;
fSumZZ+= kDiff.z * kDiff.z;
source+=vstride;
}
}
fSumXX *= recip;
fSumXY *= recip;
fSumXZ *= recip;
fSumYY *= recip;
fSumYZ *= recip;
fSumZZ *= recip;
// setup the eigensolver
Eigen 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();
Vec3 kNormal;
kNormal.x = kES.mElement[0][2];
kNormal.y = kES.mElement[1][2];
kNormal.z = kES.mElement[2][2];
// the minimum energy
plane[0] = kNormal.x;
plane[1] = kNormal.y;
plane[2] = kNormal.z;
plane[3] = 0 - kNormal.dot(kOrigin);
return ret;
}
