$search
00001 #include <stdio.h> 00002 #include <stdlib.h> 00003 #include <string.h> 00004 #include <assert.h> 00005 #include <math.h> 00006 00007 // Geometric Tools, Inc. 00008 // http://www.geometrictools.com 00009 // Copyright (c) 1998-2006. All Rights Reserved 00010 // 00011 // The Wild Magic Library (WM3) source code is supplied under the terms of 00012 // the license agreement 00013 // http://www.geometrictools.com/License/WildMagic3License.pdf 00014 // and may not be copied or disclosed except in accordance with the terms 00015 // of that agreement. 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; // the dot product 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(); //diagonalize the matrix. 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 //sort eigenvalues in decreasing order, e[0] >= ... >= e[iSize-1] 00240 for (int i0 = 0, i1; i0 <= 3-2; i0++) 00241 { 00242 // locate maximum eigenvalue 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 // swap eigenvalues 00258 m_afDiag[i1] = m_afDiag[i0]; 00259 m_afDiag[i0] = fMax; 00260 // swap eigenvectors 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 // change sign on the first column 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; // reciprocol of total weighting 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); // apply vertex weighting! 00372 kDiff.y = w*(p[1] - kOrigin.y); 00373 kDiff.z = w*(p[2] - kOrigin.z); 00374 00375 fSumXX+= kDiff.x * kDiff.x; // sume of the squares of the differences. 00376 fSumXY+= kDiff.x * kDiff.y; // sume of the squares of the differences. 00377 fSumXZ+= kDiff.x * kDiff.z; // sume of the squares of the differences. 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 // setup the eigensolver 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 // compute eigenstuff, smallest eigenvalue is in last position 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 // the minimum energy 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) // returns the diagonal distance 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) // return true if the two AABB's overlap. 00470 { 00471 if ( bmax2[0] < bmin1[0] ) return false; // if the maximum is less than our minimum on any axis 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; // if the minimum is greater than our maximum on any axis 00476 if ( bmin2[1] > bmax1[1] ) return false; // if the minimum is greater than our maximum on any axis 00477 if ( bmin2[2] > bmax1[2] ) return false; // if the minimum is greater than our maximum on any axis 00478 00479 00480 return true; // the extents overlap 00481 } 00482 00483 }; // end of namespace