bestfit.cpp
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 // 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


convex_decomposition
Author(s): John W. Ratcliff
autogenerated on Sat Jun 8 2019 20:01:17