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
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080 namespace ConvexDecomposition
00081 {
00082
00083 #define BIGNUMBER 100000000.0
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
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);
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
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
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
00158 Copy(dia1,xmin);
00159 Copy(dia2,xmax);
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
00177
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
00183
00184 dx = dia2[0]-center[0];
00185 dy = dia2[1]-center[1];
00186 dz = dia2[2]-center[2];
00187
00188 mRadius2 = dx*dx + dy*dy + dz*dz;
00189 mRadius = double(sqrt(mRadius2));
00190
00191
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)
00205 {
00206 double old_to_p = double(sqrt(old_to_p_sq));
00207
00208 mRadius = (mRadius + old_to_p) * 0.5f;
00209 mRadius2 = mRadius*mRadius;
00210 double old_to_new = old_to_p - mRadius;
00211
00212
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
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);
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
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
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
00302 Copy(dia1,xmin);
00303 Copy(dia2,xmax);
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
00321
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
00327
00328 dx = dia2[0]-center[0];
00329 dy = dia2[1]-center[1];
00330 dz = dia2[2]-center[2];
00331
00332 mRadius2 = dx*dx + dy*dy + dz*dz;
00333 mRadius = float(sqrt(mRadius2));
00334
00335
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)
00349 {
00350 float old_to_p = float(sqrt(old_to_p_sq));
00351
00352 mRadius = (mRadius + old_to_p) * 0.5f;
00353 mRadius2 = mRadius*mRadius;
00354 float old_to_new = old_to_p - mRadius;
00355
00356
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;
00376 }
00377
00378 };