Go to the documentation of this file.00001 #include <stdio.h>
00002 #include <stdlib.h>
00003 #include <string.h>
00004 #include <assert.h>
00005 #include <math.h>
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00073 #include "bestfit.h"
00074
00075 namespace ConvexDecomposition
00076 {
00077
00078 class Vec3
00079 {
00080 public:
00081 Vec3(void) { };
00082 Vec3(double _x,double _y,double _z) { x = _x; y = _y; z = _z; };
00083
00084
00085 double dot(const Vec3 &v)
00086 {
00087 return x*v.x + y*v.y + z*v.z;
00088 }
00089
00090 double x;
00091 double y;
00092 double z;
00093 };
00094
00095
00096 class Eigen
00097 {
00098 public:
00099
00100
00101 void DecrSortEigenStuff(void)
00102 {
00103 Tridiagonal();
00104 QLAlgorithm();
00105 DecreasingSort();
00106 GuaranteeRotation();
00107 }
00108
00109 void Tridiagonal(void)
00110 {
00111 double fM00 = mElement[0][0];
00112 double fM01 = mElement[0][1];
00113 double fM02 = mElement[0][2];
00114 double fM11 = mElement[1][1];
00115 double fM12 = mElement[1][2];
00116 double fM22 = mElement[2][2];
00117
00118 m_afDiag[0] = fM00;
00119 m_afSubd[2] = 0;
00120 if (fM02 != (double)0.0)
00121 {
00122 double fLength = sqrt(fM01*fM01+fM02*fM02);
00123 double fInvLength = ((double)1.0)/fLength;
00124 fM01 *= fInvLength;
00125 fM02 *= fInvLength;
00126 double fQ = ((double)2.0)*fM01*fM12+fM02*(fM22-fM11);
00127 m_afDiag[1] = fM11+fM02*fQ;
00128 m_afDiag[2] = fM22-fM02*fQ;
00129 m_afSubd[0] = fLength;
00130 m_afSubd[1] = fM12-fM01*fQ;
00131 mElement[0][0] = (double)1.0;
00132 mElement[0][1] = (double)0.0;
00133 mElement[0][2] = (double)0.0;
00134 mElement[1][0] = (double)0.0;
00135 mElement[1][1] = fM01;
00136 mElement[1][2] = fM02;
00137 mElement[2][0] = (double)0.0;
00138 mElement[2][1] = fM02;
00139 mElement[2][2] = -fM01;
00140 m_bIsRotation = false;
00141 }
00142 else
00143 {
00144 m_afDiag[1] = fM11;
00145 m_afDiag[2] = fM22;
00146 m_afSubd[0] = fM01;
00147 m_afSubd[1] = fM12;
00148 mElement[0][0] = (double)1.0;
00149 mElement[0][1] = (double)0.0;
00150 mElement[0][2] = (double)0.0;
00151 mElement[1][0] = (double)0.0;
00152 mElement[1][1] = (double)1.0;
00153 mElement[1][2] = (double)0.0;
00154 mElement[2][0] = (double)0.0;
00155 mElement[2][1] = (double)0.0;
00156 mElement[2][2] = (double)1.0;
00157 m_bIsRotation = true;
00158 }
00159 }
00160
00161 bool QLAlgorithm(void)
00162 {
00163 const int iMaxIter = 32;
00164
00165 for (int i0 = 0; i0 <3; i0++)
00166 {
00167 int i1;
00168 for (i1 = 0; i1 < iMaxIter; i1++)
00169 {
00170 int i2;
00171 for (i2 = i0; i2 <= (3-2); i2++)
00172 {
00173 double fTmp = fabs(m_afDiag[i2]) + fabs(m_afDiag[i2+1]);
00174 if ( fabs(m_afSubd[i2]) + fTmp == fTmp )
00175 break;
00176 }
00177 if (i2 == i0)
00178 {
00179 break;
00180 }
00181
00182 double fG = (m_afDiag[i0+1] - m_afDiag[i0])/(((double)2.0) * m_afSubd[i0]);
00183 double fR = sqrt(fG*fG+(double)1.0);
00184 if (fG < (double)0.0)
00185 {
00186 fG = m_afDiag[i2]-m_afDiag[i0]+m_afSubd[i0]/(fG-fR);
00187 }
00188 else
00189 {
00190 fG = m_afDiag[i2]-m_afDiag[i0]+m_afSubd[i0]/(fG+fR);
00191 }
00192 double fSin = (double)1.0, fCos = (double)1.0, fP = (double)0.0;
00193 for (int i3 = i2-1; i3 >= i0; i3--)
00194 {
00195 double fF = fSin*m_afSubd[i3];
00196 double fB = fCos*m_afSubd[i3];
00197 if (fabs(fF) >= fabs(fG))
00198 {
00199 fCos = fG/fF;
00200 fR = sqrt(fCos*fCos+(double)1.0);
00201 m_afSubd[i3+1] = fF*fR;
00202 fSin = ((double)1.0)/fR;
00203 fCos *= fSin;
00204 }
00205 else
00206 {
00207 fSin = fF/fG;
00208 fR = sqrt(fSin*fSin+(double)1.0);
00209 m_afSubd[i3+1] = fG*fR;
00210 fCos = ((double)1.0)/fR;
00211 fSin *= fCos;
00212 }
00213 fG = m_afDiag[i3+1]-fP;
00214 fR = (m_afDiag[i3]-fG)*fSin+((double)2.0)*fB*fCos;
00215 fP = fSin*fR;
00216 m_afDiag[i3+1] = fG+fP;
00217 fG = fCos*fR-fB;
00218 for (int i4 = 0; i4 < 3; i4++)
00219 {
00220 fF = mElement[i4][i3+1];
00221 mElement[i4][i3+1] = fSin*mElement[i4][i3]+fCos*fF;
00222 mElement[i4][i3] = fCos*mElement[i4][i3]-fSin*fF;
00223 }
00224 }
00225 m_afDiag[i0] -= fP;
00226 m_afSubd[i0] = fG;
00227 m_afSubd[i2] = (double)0.0;
00228 }
00229 if (i1 == iMaxIter)
00230 {
00231 return false;
00232 }
00233 }
00234 return true;
00235 }
00236
00237 void DecreasingSort(void)
00238 {
00239
00240 for (int i0 = 0, i1; i0 <= 3-2; i0++)
00241 {
00242
00243 i1 = i0;
00244 double fMax = m_afDiag[i1];
00245 int i2;
00246 for (i2 = i0+1; i2 < 3; i2++)
00247 {
00248 if (m_afDiag[i2] > fMax)
00249 {
00250 i1 = i2;
00251 fMax = m_afDiag[i1];
00252 }
00253 }
00254
00255 if (i1 != i0)
00256 {
00257
00258 m_afDiag[i1] = m_afDiag[i0];
00259 m_afDiag[i0] = fMax;
00260
00261 for (i2 = 0; i2 < 3; i2++)
00262 {
00263 double fTmp = mElement[i2][i0];
00264 mElement[i2][i0] = mElement[i2][i1];
00265 mElement[i2][i1] = fTmp;
00266 m_bIsRotation = !m_bIsRotation;
00267 }
00268 }
00269 }
00270 }
00271
00272
00273 void GuaranteeRotation(void)
00274 {
00275 if (!m_bIsRotation)
00276 {
00277
00278 for (int iRow = 0; iRow <3; iRow++)
00279 {
00280 mElement[iRow][0] = -mElement[iRow][0];
00281 }
00282 }
00283 }
00284
00285 double mElement[3][3];
00286 double m_afDiag[3];
00287 double m_afSubd[3];
00288 bool m_bIsRotation;
00289 };
00290
00291
00292 bool getBestFitPlane(unsigned int vcount,
00293 const double *points,
00294 unsigned int vstride,
00295 const double *weights,
00296 unsigned int wstride,
00297 double *plane)
00298 {
00299 bool ret = false;
00300
00301 Vec3 kOrigin(0,0,0);
00302
00303 double wtotal = 0;
00304
00305 if ( 1 )
00306 {
00307 const char *source = (const char *) points;
00308 const char *wsource = (const char *) weights;
00309
00310 for (unsigned int i=0; i<vcount; i++)
00311 {
00312
00313 const double *p = (const double *) source;
00314
00315 double w = 1;
00316
00317 if ( wsource )
00318 {
00319 const double *ws = (const double *) wsource;
00320 w = *ws;
00321 wsource+=wstride;
00322 }
00323
00324 kOrigin.x+=p[0]*w;
00325 kOrigin.y+=p[1]*w;
00326 kOrigin.z+=p[2]*w;
00327
00328 wtotal+=w;
00329
00330 source+=vstride;
00331 }
00332 }
00333
00334 double recip = 1.0f / wtotal;
00335
00336 kOrigin.x*=recip;
00337 kOrigin.y*=recip;
00338 kOrigin.z*=recip;
00339
00340
00341 double fSumXX=0;
00342 double fSumXY=0;
00343 double fSumXZ=0;
00344
00345 double fSumYY=0;
00346 double fSumYZ=0;
00347 double fSumZZ=0;
00348
00349
00350 if ( 1 )
00351 {
00352 const char *source = (const char *) points;
00353 const char *wsource = (const char *) weights;
00354
00355 for (unsigned int i=0; i<vcount; i++)
00356 {
00357
00358 const double *p = (const double *) source;
00359
00360 double w = 1;
00361
00362 if ( wsource )
00363 {
00364 const double *ws = (const double *) wsource;
00365 w = *ws;
00366 wsource+=wstride;
00367 }
00368
00369 Vec3 kDiff;
00370
00371 kDiff.x = w*(p[0] - kOrigin.x);
00372 kDiff.y = w*(p[1] - kOrigin.y);
00373 kDiff.z = w*(p[2] - kOrigin.z);
00374
00375 fSumXX+= kDiff.x * kDiff.x;
00376 fSumXY+= kDiff.x * kDiff.y;
00377 fSumXZ+= kDiff.x * kDiff.z;
00378
00379 fSumYY+= kDiff.y * kDiff.y;
00380 fSumYZ+= kDiff.y * kDiff.z;
00381 fSumZZ+= kDiff.z * kDiff.z;
00382
00383
00384 source+=vstride;
00385 }
00386 }
00387
00388 fSumXX *= recip;
00389 fSumXY *= recip;
00390 fSumXZ *= recip;
00391 fSumYY *= recip;
00392 fSumYZ *= recip;
00393 fSumZZ *= recip;
00394
00395
00396 Eigen kES;
00397
00398 kES.mElement[0][0] = fSumXX;
00399 kES.mElement[0][1] = fSumXY;
00400 kES.mElement[0][2] = fSumXZ;
00401
00402 kES.mElement[1][0] = fSumXY;
00403 kES.mElement[1][1] = fSumYY;
00404 kES.mElement[1][2] = fSumYZ;
00405
00406 kES.mElement[2][0] = fSumXZ;
00407 kES.mElement[2][1] = fSumYZ;
00408 kES.mElement[2][2] = fSumZZ;
00409
00410
00411 kES.DecrSortEigenStuff();
00412
00413 Vec3 kNormal;
00414
00415 kNormal.x = kES.mElement[0][2];
00416 kNormal.y = kES.mElement[1][2];
00417 kNormal.z = kES.mElement[2][2];
00418
00419
00420 plane[0] = kNormal.x;
00421 plane[1] = kNormal.y;
00422 plane[2] = kNormal.z;
00423
00424 plane[3] = 0 - kNormal.dot(kOrigin);
00425
00426 return ret;
00427 }
00428
00429
00430
00431 double getBoundingRegion(unsigned int vcount,const double *points,unsigned int pstride,double *bmin,double *bmax)
00432 {
00433
00434 const unsigned char *source = (const unsigned char *) points;
00435
00436 bmin[0] = points[0];
00437 bmin[1] = points[1];
00438 bmin[2] = points[2];
00439
00440 bmax[0] = points[0];
00441 bmax[1] = points[1];
00442 bmax[2] = points[2];
00443
00444
00445 for (unsigned int i=1; i<vcount; i++)
00446 {
00447 source+=pstride;
00448 const double *p = (const double *) source;
00449
00450 if ( p[0] < bmin[0] ) bmin[0] = p[0];
00451 if ( p[1] < bmin[1] ) bmin[1] = p[1];
00452 if ( p[2] < bmin[2] ) bmin[2] = p[2];
00453
00454 if ( p[0] > bmax[0] ) bmax[0] = p[0];
00455 if ( p[1] > bmax[1] ) bmax[1] = p[1];
00456 if ( p[2] > bmax[2] ) bmax[2] = p[2];
00457
00458 }
00459
00460 double dx = bmax[0] - bmin[0];
00461 double dy = bmax[1] - bmin[1];
00462 double dz = bmax[2] - bmin[2];
00463
00464 return sqrt( dx*dx + dy*dy + dz*dz );
00465
00466 }
00467
00468
00469 bool overlapAABB(const double *bmin1,const double *bmax1,const double *bmin2,const double *bmax2)
00470 {
00471 if ( bmax2[0] < bmin1[0] ) return false;
00472 if ( bmax2[1] < bmin1[1] ) return false;
00473 if ( bmax2[2] < bmin1[2] ) return false;
00474
00475 if ( bmin2[0] > bmax1[0] ) return false;
00476 if ( bmin2[1] > bmax1[1] ) return false;
00477 if ( bmin2[2] > bmax1[2] ) return false;
00478
00479
00480 return true;
00481 }
00482
00483 };