convex_decomposition: convex_decomposition: bestfitobb.cpp Source File

00001 #include <stdio.h>
00002 #include <stdlib.h>
00003 #include <string.h>
00004 #include <math.h>
00005 #include <assert.h>
00006 
00063 // compute the 'best fit' oriented bounding box of an input point cloud by doing an exhaustive search.
00064 // it spins the point cloud around searching for the minimal volume.  It keeps narrowing down until
00065 // it fails to find a better fit.  The only dependency is on 'double_math'
00066 //
00067 // The inputs are:
00068 //
00069 //         vcount    : number of input vertices in the point cloud.
00070 //         points    : a pointer to the first vertex.
00071 //         pstride   : The stride between each point measured in bytes.
00072 //
00073 // The outputs are:
00074 //
00075 //         sides     : The length of the sides of the OBB as X, Y, Z distance.
00076 //         matrix    : A pointer to a 4x4 matrix.  This will contain the 3x3 rotation and the translation component.
00077 //         pos       : The center of the OBB
00078 //         quat      : The orientation of the OBB expressed as quaternion in the form of X,Y,Z,W
00079 //
00080 //
00081 // Please email bug fixes or improvements to John W. Ratcliff at mailto:jratcliff@infiniplex.net
00082 //
00083 // If you find this source code useful donate a couple of bucks to my kid's fund raising website at
00084 //  www.amillionpixels.us
00085 //
00086 // More snippets at: www.codesuppository.com
00087 //
00088 
00089 
00090 #include "bestfitobb.h"
00091 #include "float_math.h"
00092 
00093 namespace ConvexDecomposition
00094 {
00095 
00096 // computes the OBB for this set of points relative to this transform matrix.
00097 void computeOBB(unsigned int vcount,const double *points,unsigned int pstride,double *sides,double *matrix)
00098 {
00099   const char *src = (const char *) points;
00100 
00101   double bmin[3] = { 1e9, 1e9, 1e9 };
00102   double bmax[3] = { -1e9, -1e9, -1e9 };
00103 
00104   for (unsigned int i=0; i<vcount; i++)
00105   {
00106     const double *p = (const double *) src;
00107     double t[3];
00108 
00109     fm_inverseRT(matrix, p, t ); // inverse rotate translate
00110 
00111     if ( t[0] < bmin[0] ) bmin[0] = t[0];
00112     if ( t[1] < bmin[1] ) bmin[1] = t[1];
00113     if ( t[2] < bmin[2] ) bmin[2] = t[2];
00114 
00115     if ( t[0] > bmax[0] ) bmax[0] = t[0];
00116     if ( t[1] > bmax[1] ) bmax[1] = t[1];
00117     if ( t[2] > bmax[2] ) bmax[2] = t[2];
00118 
00119     src+=pstride;
00120   }
00121 
00122   double center[3];
00123 
00124   sides[0] = bmax[0]-bmin[0];
00125   sides[1] = bmax[1]-bmin[1];
00126   sides[2] = bmax[2]-bmin[2];
00127 
00128   center[0] = sides[0]*0.5f+bmin[0];
00129   center[1] = sides[1]*0.5f+bmin[1];
00130   center[2] = sides[2]*0.5f+bmin[2];
00131 
00132   double ocenter[3];
00133 
00134   fm_rotate(matrix,center,ocenter);
00135 
00136   matrix[12]+=ocenter[0];
00137   matrix[13]+=ocenter[1];
00138   matrix[14]+=ocenter[2];
00139 
00140 }
00141 
00142 void computeBestFitOBB(unsigned int vcount,const double *points,unsigned int pstride,double *sides,double *matrix)
00143 {
00144 
00145   double bmin[3];
00146   double bmax[3];
00147 
00148   fm_getAABB(vcount,points,pstride,bmin,bmax);
00149 
00150   double center[3];
00151 
00152   center[0] = (bmax[0]-bmin[0])*0.5f + bmin[0];
00153   center[1] = (bmax[1]-bmin[1])*0.5f + bmin[1];
00154   center[2] = (bmax[2]-bmin[2])*0.5f + bmin[2];
00155 
00156   double ax = 0;
00157   double ay = 0;
00158   double az = 0;
00159 
00160   double sweep =  45.0f; // 180 degree sweep on all three axes.
00161   double steps =  7.0f; // 7 steps on each axis)
00162 
00163   double bestVolume = 1e9;
00164   double angle[3];
00165 
00166   while ( sweep >= 1 )
00167   {
00168 
00169     bool found = false;
00170 
00171     double stepsize = sweep / steps;
00172 
00173     for (double x=ax-sweep; x<=ax+sweep; x+=stepsize)
00174     {
00175       for (double y=ay-sweep; y<=ay+sweep; y+=stepsize)
00176       {
00177         for (double z=az-sweep; z<=az+sweep; z+=stepsize)
00178         {
00179           double pmatrix[16];
00180 
00181           fm_eulerMatrix( x*FM_DEG_TO_RAD, y*FM_DEG_TO_RAD, z*FM_DEG_TO_RAD, pmatrix );
00182 
00183           pmatrix[3*4+0] = center[0];
00184           pmatrix[3*4+1] = center[1];
00185           pmatrix[3*4+2] = center[2];
00186 
00187           double psides[3];
00188 
00189           computeOBB( vcount, points, pstride, psides, pmatrix );
00190 
00191           double volume = psides[0]*psides[1]*psides[2]; // the volume of the cube
00192 
00193           if ( volume < bestVolume )
00194           {
00195             bestVolume = volume;
00196 
00197             sides[0] = psides[0];
00198             sides[1] = psides[1];
00199             sides[2] = psides[2];
00200 
00201             angle[0] = ax;
00202             angle[1] = ay;
00203             angle[2] = az;
00204 
00205             memcpy(matrix,pmatrix,sizeof(double)*16);
00206             found = true; // yes, we found an improvement.
00207           }
00208         }
00209       }
00210     }
00211 
00212     if ( found )
00213     {
00214 
00215       ax = angle[0];
00216       ay = angle[1];
00217       az = angle[2];
00218 
00219       sweep*=0.5f; // sweep 1/2 the distance as the last time.
00220     }
00221     else
00222     {
00223       break; // no improvement, so just
00224     }
00225 
00226   }
00227 
00228 }
00229 
00230 
00231 void computeBestFitOBB(unsigned int vcount,const double *points,unsigned int pstride,double *sides,double *pos,double *quat)
00232 {
00233   double matrix[16];
00234 
00235   computeBestFitOBB(vcount,points,pstride,sides,matrix);
00236   fm_getTranslation(matrix,pos);
00237   fm_matrixToQuat(matrix,quat);
00238 
00239 }
00240 
00241 
00242 void computeBestFitABB(unsigned int vcount,const double *points,unsigned int pstride,double *sides,double *pos)
00243 {
00244         double bmin[3];
00245         double bmax[3];
00246 
00247   bmin[0] = points[0];
00248   bmin[1] = points[1];
00249   bmin[2] = points[2];
00250 
00251   bmax[0] = points[0];
00252   bmax[1] = points[1];
00253   bmax[2] = points[2];
00254 
00255         const char *cp = (const char *) points;
00256         for (unsigned int i=0; i<vcount; i++)
00257         {
00258                 const double *p = (const double *) cp;
00259 
00260                 if ( p[0] < bmin[0] ) bmin[0] = p[0];
00261                 if ( p[1] < bmin[1] ) bmin[1] = p[1];
00262                 if ( p[2] < bmin[2] ) bmin[2] = p[2];
00263 
00264     if ( p[0] > bmax[0] ) bmax[0] = p[0];
00265     if ( p[1] > bmax[1] ) bmax[1] = p[1];
00266     if ( p[2] > bmax[2] ) bmax[2] = p[2];
00267 
00268     cp+=pstride;
00269         }
00270 
00271 
00272         sides[0] = bmax[0] - bmin[0];
00273         sides[1] = bmax[1] - bmin[1];
00274         sides[2] = bmax[2] - bmin[2];
00275 
00276         pos[0] = bmin[0]+sides[0]*0.5f;
00277         pos[1] = bmin[1]+sides[1]*0.5f;
00278         pos[2] = bmin[2]+sides[2]*0.5f;
00279 
00280 }
00281 
00282 
00283 void computeBestFitOBB(unsigned int vcount,const float *points,unsigned int pstride,float *sides,float *pos,float *quat) // the float version of the routine.
00284 {
00285   double *temp = new double[vcount*3];
00286   const char *src = (const char *)points;
00287   double *dest     = temp;
00288   for (unsigned int i=0; i<vcount; i++)
00289   {
00290     const float *s = (const float *) src;
00291     temp[0] = s[0];
00292     temp[1] = s[1];
00293     temp[2] = s[2];
00294     temp+=3;
00295     s+=pstride;
00296   }
00297 
00298   double dsides[3];
00299   double dpos[3];
00300   double dquat[3];
00301 
00302   computeBestFitOBB(vcount,temp,sizeof(double)*3,dsides,dpos,dquat);
00303 
00304   if ( sides )
00305   {
00306     sides[0] = (float) dsides[0];
00307     sides[1] = (float) dsides[1];
00308     sides[2] = (float) dsides[2];
00309   }
00310   if ( pos )
00311   {
00312     pos[0] = (float) dpos[0];
00313     pos[1] = (float) dpos[1];
00314     pos[2] = (float) dpos[2];
00315   }
00316   if ( quat )
00317   {
00318     quat[0] = (float) dquat[0];
00319     quat[1] = (float) dquat[1];
00320     quat[2] = (float) dquat[2];
00321     quat[3] = (float) dquat[3];
00322   }
00323 
00324   delete temp;
00325 
00326 }
00327 
00328 void computeBestFitABB(unsigned int vcount,const float *points,unsigned int pstride,float *sides,float *pos)
00329 {
00330         float bmin[3];
00331         float bmax[3];
00332 
00333   bmin[0] = points[0];
00334   bmin[1] = points[1];
00335   bmin[2] = points[2];
00336 
00337   bmax[0] = points[0];
00338   bmax[1] = points[1];
00339   bmax[2] = points[2];
00340 
00341         const char *cp = (const char *) points;
00342         for (unsigned int i=0; i<vcount; i++)
00343         {
00344                 const float *p = (const float *) cp;
00345 
00346                 if ( p[0] < bmin[0] ) bmin[0] = p[0];
00347                 if ( p[1] < bmin[1] ) bmin[1] = p[1];
00348                 if ( p[2] < bmin[2] ) bmin[2] = p[2];
00349 
00350     if ( p[0] > bmax[0] ) bmax[0] = p[0];
00351     if ( p[1] > bmax[1] ) bmax[1] = p[1];
00352     if ( p[2] > bmax[2] ) bmax[2] = p[2];
00353 
00354     cp+=pstride;
00355         }
00356 
00357 
00358         sides[0] = bmax[0] - bmin[0];
00359         sides[1] = bmax[1] - bmin[1];
00360         sides[2] = bmax[2] - bmin[2];
00361 
00362         pos[0] = bmin[0]+sides[0]*0.5f;
00363         pos[1] = bmin[1]+sides[1]*0.5f;
00364         pos[2] = bmin[2]+sides[2]*0.5f;
00365 
00366 }
00367 
00368 };