convex_decomposition: convex_decomposition: fitsphere.cpp Source File

00001 #include <stdio.h>
00002 #include <stdlib.h>
00003 #include <string.h>
00004 #include <assert.h>
00005 #include <math.h>
00006 
00007 #include "fitsphere.h"
00008 
00009 
00067 /*
00068 An Efficient Bounding Sphere
00069 by Jack Ritter
00070 from "Graphics Gems", Academic Press, 1990
00071 */
00072 
00073 /* Routine to calculate tight bounding sphere over    */
00074 /* a set of points in 3D */
00075 /* This contains the routine find_bounding_sphere(), */
00076 /* the struct definition, and the globals used for parameters. */
00077 /* The abs() of all coordinates must be < BIGNUMBER */
00078 /* Code written by Jack Ritter and Lyle Rains. */
00079 
00080 namespace ConvexDecomposition
00081 {
00082 
00083 #define BIGNUMBER 100000000.0           /* hundred million */
00084 
00085 static inline void Set(double *n,double x,double y,double z)
00086 {
00087         n[0] = x;
00088         n[1] = y;
00089         n[2] = z;
00090 };
00091 
00092 static inline void Copy(double *dest,const double *source)
00093 {
00094         dest[0] = source[0];
00095         dest[1] = source[1];
00096         dest[2] = source[2];
00097 }
00098 
00099 double computeBoundingSphere(unsigned int vcount,const double *points,double *center)
00100 {
00101 
00102   double mRadius;
00103   double mRadius2;
00104 
00105         double xmin[3];
00106         double xmax[3];
00107         double ymin[3];
00108         double ymax[3];
00109         double zmin[3];
00110         double zmax[3];
00111         double dia1[3];
00112         double dia2[3];
00113 
00114   /* FIRST PASS: find 6 minima/maxima points */
00115   Set(xmin,BIGNUMBER,BIGNUMBER,BIGNUMBER);
00116   Set(xmax,-BIGNUMBER,-BIGNUMBER,-BIGNUMBER);
00117   Set(ymin,BIGNUMBER,BIGNUMBER,BIGNUMBER);
00118   Set(ymax,-BIGNUMBER,-BIGNUMBER,-BIGNUMBER);
00119   Set(zmin,BIGNUMBER,BIGNUMBER,BIGNUMBER);
00120   Set(zmax,-BIGNUMBER,-BIGNUMBER,-BIGNUMBER);
00121 
00122   for (unsigned i=0; i<vcount; i++)
00123         {
00124                 const double *caller_p = &points[i*3];
00125 
00126         if (caller_p[0]<xmin[0])
00127           Copy(xmin,caller_p); /* New xminimum point */
00128         if (caller_p[0]>xmax[0])
00129           Copy(xmax,caller_p);
00130         if (caller_p[1]<ymin[1])
00131           Copy(ymin,caller_p);
00132         if (caller_p[1]>ymax[1])
00133           Copy(ymax,caller_p);
00134         if (caller_p[2]<zmin[2])
00135           Copy(zmin,caller_p);
00136         if (caller_p[2]>zmax[2])
00137           Copy(zmax,caller_p);
00138         }
00139 
00140   /* Set xspan = distance between the 2 points xmin & xmax (squared) */
00141   double dx = xmax[0] - xmin[0];
00142   double dy = xmax[1] - xmin[1];
00143   double dz = xmax[2] - xmin[2];
00144   double xspan = dx*dx + dy*dy + dz*dz;
00145 
00146   /* Same for y & z spans */
00147   dx = ymax[0] - ymin[0];
00148   dy = ymax[1] - ymin[1];
00149   dz = ymax[2] - ymin[2];
00150   double yspan = dx*dx + dy*dy + dz*dz;
00151 
00152   dx = zmax[0] - zmin[0];
00153   dy = zmax[1] - zmin[1];
00154   dz = zmax[2] - zmin[2];
00155   double zspan = dx*dx + dy*dy + dz*dz;
00156 
00157   /* Set points dia1 & dia2 to the maximally separated pair */
00158   Copy(dia1,xmin);
00159   Copy(dia2,xmax); /* assume xspan biggest */
00160   double maxspan = xspan;
00161 
00162   if (yspan>maxspan)
00163         {
00164           maxspan = yspan;
00165         Copy(dia1,ymin);
00166         Copy(dia2,ymax);
00167         }
00168 
00169   if (zspan>maxspan)
00170         {
00171           Copy(dia1,zmin);
00172           Copy(dia2,zmax);
00173         }
00174 
00175 
00176   /* dia1,dia2 is a diameter of initial sphere */
00177   /* calc initial center */
00178   center[0] = (dia1[0]+dia2[0])*0.5f;
00179   center[1] = (dia1[1]+dia2[1])*0.5f;
00180   center[2] = (dia1[2]+dia2[2])*0.5f;
00181 
00182   /* calculate initial radius**2 and radius */
00183 
00184   dx = dia2[0]-center[0]; /* x component of radius vector */
00185   dy = dia2[1]-center[1]; /* y component of radius vector */
00186   dz = dia2[2]-center[2]; /* z component of radius vector */
00187 
00188   mRadius2 = dx*dx + dy*dy + dz*dz;
00189   mRadius = double(sqrt(mRadius2));
00190 
00191   /* SECOND PASS: increment current sphere */
00192   if ( 1 )
00193   {
00194           for (unsigned i=0; i<vcount; i++)
00195                 {
00196                         const double *caller_p = &points[i*3];
00197 
00198                 dx = caller_p[0]-center[0];
00199                   dy = caller_p[1]-center[1];
00200                 dz = caller_p[2]-center[2];
00201 
00202                   double old_to_p_sq = dx*dx + dy*dy + dz*dz;
00203 
00204                 if (old_to_p_sq > mRadius2)     /* do r**2 test first */
00205                         {       /* this point is outside of current sphere */
00206                         double old_to_p = double(sqrt(old_to_p_sq));
00207                           /* calc radius of new sphere */
00208                         mRadius = (mRadius + old_to_p) * 0.5f;
00209                         mRadius2 = mRadius*mRadius;     /* for next r**2 compare */
00210                         double old_to_new = old_to_p - mRadius;
00211 
00212                         /* calc center of new sphere */
00213 
00214                   double recip = 1.0f /old_to_p;
00215 
00216                         double cx = (mRadius*center[0] + old_to_new*caller_p[0]) * recip;
00217                         double cy = (mRadius*center[1] + old_to_new*caller_p[1]) * recip;
00218                           double cz = (mRadius*center[2] + old_to_new*caller_p[2]) * recip;
00219 
00220                   Set(center,cx,cy,cz);
00221                         }
00222                 }
00223   }
00224 
00225   return mRadius;
00226 }
00227 
00228 static inline void Set(float *n,float x,float y,float z)
00229 {
00230         n[0] = x;
00231         n[1] = y;
00232         n[2] = z;
00233 };
00234 
00235 static inline void Copy(float *dest,const float *source)
00236 {
00237         dest[0] = source[0];
00238         dest[1] = source[1];
00239         dest[2] = source[2];
00240 }
00241 
00242 
00243 
00244 float  computeBoundingSphere(unsigned int vcount,const float *points,float *center)
00245 {
00246   float mRadius;
00247   float mRadius2;
00248 
00249         float xmin[3];
00250         float xmax[3];
00251         float ymin[3];
00252         float ymax[3];
00253         float zmin[3];
00254         float zmax[3];
00255         float dia1[3];
00256         float dia2[3];
00257 
00258   /* FIRST PASS: find 6 minima/maxima points */
00259   Set(xmin,BIGNUMBER,BIGNUMBER,BIGNUMBER);
00260   Set(xmax,-BIGNUMBER,-BIGNUMBER,-BIGNUMBER);
00261   Set(ymin,BIGNUMBER,BIGNUMBER,BIGNUMBER);
00262   Set(ymax,-BIGNUMBER,-BIGNUMBER,-BIGNUMBER);
00263   Set(zmin,BIGNUMBER,BIGNUMBER,BIGNUMBER);
00264   Set(zmax,-BIGNUMBER,-BIGNUMBER,-BIGNUMBER);
00265 
00266   for (unsigned i=0; i<vcount; i++)
00267         {
00268                 const float *caller_p = &points[i*3];
00269 
00270         if (caller_p[0]<xmin[0])
00271           Copy(xmin,caller_p); /* New xminimum point */
00272         if (caller_p[0]>xmax[0])
00273           Copy(xmax,caller_p);
00274         if (caller_p[1]<ymin[1])
00275           Copy(ymin,caller_p);
00276         if (caller_p[1]>ymax[1])
00277           Copy(ymax,caller_p);
00278         if (caller_p[2]<zmin[2])
00279           Copy(zmin,caller_p);
00280         if (caller_p[2]>zmax[2])
00281           Copy(zmax,caller_p);
00282         }
00283 
00284   /* Set xspan = distance between the 2 points xmin & xmax (squared) */
00285   float dx = xmax[0] - xmin[0];
00286   float dy = xmax[1] - xmin[1];
00287   float dz = xmax[2] - xmin[2];
00288   float xspan = dx*dx + dy*dy + dz*dz;
00289 
00290   /* Same for y & z spans */
00291   dx = ymax[0] - ymin[0];
00292   dy = ymax[1] - ymin[1];
00293   dz = ymax[2] - ymin[2];
00294   float yspan = dx*dx + dy*dy + dz*dz;
00295 
00296   dx = zmax[0] - zmin[0];
00297   dy = zmax[1] - zmin[1];
00298   dz = zmax[2] - zmin[2];
00299   float zspan = dx*dx + dy*dy + dz*dz;
00300 
00301   /* Set points dia1 & dia2 to the maximally separated pair */
00302   Copy(dia1,xmin);
00303   Copy(dia2,xmax); /* assume xspan biggest */
00304   float maxspan = xspan;
00305 
00306   if (yspan>maxspan)
00307         {
00308           maxspan = yspan;
00309         Copy(dia1,ymin);
00310         Copy(dia2,ymax);
00311         }
00312 
00313   if (zspan>maxspan)
00314         {
00315           Copy(dia1,zmin);
00316           Copy(dia2,zmax);
00317         }
00318 
00319 
00320   /* dia1,dia2 is a diameter of initial sphere */
00321   /* calc initial center */
00322   center[0] = (dia1[0]+dia2[0])*0.5f;
00323   center[1] = (dia1[1]+dia2[1])*0.5f;
00324   center[2] = (dia1[2]+dia2[2])*0.5f;
00325 
00326   /* calculate initial radius**2 and radius */
00327 
00328   dx = dia2[0]-center[0]; /* x component of radius vector */
00329   dy = dia2[1]-center[1]; /* y component of radius vector */
00330   dz = dia2[2]-center[2]; /* z component of radius vector */
00331 
00332   mRadius2 = dx*dx + dy*dy + dz*dz;
00333   mRadius = float(sqrt(mRadius2));
00334 
00335   /* SECOND PASS: increment current sphere */
00336   if ( 1 )
00337   {
00338           for (unsigned i=0; i<vcount; i++)
00339                 {
00340                         const float *caller_p = &points[i*3];
00341 
00342                 dx = caller_p[0]-center[0];
00343                   dy = caller_p[1]-center[1];
00344                 dz = caller_p[2]-center[2];
00345 
00346                   float old_to_p_sq = dx*dx + dy*dy + dz*dz;
00347 
00348                 if (old_to_p_sq > mRadius2)     /* do r**2 test first */
00349                         {       /* this point is outside of current sphere */
00350                         float old_to_p = float(sqrt(old_to_p_sq));
00351                           /* calc radius of new sphere */
00352                         mRadius = (mRadius + old_to_p) * 0.5f;
00353                         mRadius2 = mRadius*mRadius;     /* for next r**2 compare */
00354                         float old_to_new = old_to_p - mRadius;
00355 
00356                         /* calc center of new sphere */
00357 
00358                   float recip = 1.0f /old_to_p;
00359 
00360                         float cx = (mRadius*center[0] + old_to_new*caller_p[0]) * recip;
00361                         float cy = (mRadius*center[1] + old_to_new*caller_p[1]) * recip;
00362                           float cz = (mRadius*center[2] + old_to_new*caller_p[2]) * recip;
00363 
00364                   Set(center,cx,cy,cz);
00365                         }
00366                 }
00367   }
00368 
00369   return mRadius;
00370 }
00371 
00372 
00373 double computeSphereVolume(double r)
00374 {
00375         return (4.0f*3.141592654f*r*r*r)/3.0f;  // 4/3 PI R cubed
00376 }
00377 
00378 };