fitsphere.cpp
Go to the documentation of this file.
1 #include <stdio.h>
2 #include <stdlib.h>
3 #include <string.h>
4 #include <assert.h>
5 #include <math.h>
6 
7 #include "fitsphere.h"
8 
9 
67 /*
68 An Efficient Bounding Sphere
69 by Jack Ritter
70 from "Graphics Gems", Academic Press, 1990
71 */
72 
73 /* Routine to calculate tight bounding sphere over */
74 /* a set of points in 3D */
75 /* This contains the routine find_bounding_sphere(), */
76 /* the struct definition, and the globals used for parameters. */
77 /* The abs() of all coordinates must be < BIGNUMBER */
78 /* Code written by Jack Ritter and Lyle Rains. */
79 
80 namespace ConvexDecomposition
81 {
82 
83 #define BIGNUMBER 100000000.0 /* hundred million */
84 
85 static inline void Set(double *n,double x,double y,double z)
86 {
87  n[0] = x;
88  n[1] = y;
89  n[2] = z;
90 };
91 
92 static inline void Copy(double *dest,const double *source)
93 {
94  dest[0] = source[0];
95  dest[1] = source[1];
96  dest[2] = source[2];
97 }
98 
99 double computeBoundingSphere(unsigned int vcount,const double *points,double *center)
100 {
101 
102  double mRadius;
103  double mRadius2;
104 
105  double xmin[3];
106  double xmax[3];
107  double ymin[3];
108  double ymax[3];
109  double zmin[3];
110  double zmax[3];
111  double dia1[3];
112  double dia2[3];
113 
114  /* FIRST PASS: find 6 minima/maxima points */
115  Set(xmin,BIGNUMBER,BIGNUMBER,BIGNUMBER);
116  Set(xmax,-BIGNUMBER,-BIGNUMBER,-BIGNUMBER);
117  Set(ymin,BIGNUMBER,BIGNUMBER,BIGNUMBER);
118  Set(ymax,-BIGNUMBER,-BIGNUMBER,-BIGNUMBER);
119  Set(zmin,BIGNUMBER,BIGNUMBER,BIGNUMBER);
120  Set(zmax,-BIGNUMBER,-BIGNUMBER,-BIGNUMBER);
121 
122  for (unsigned i=0; i<vcount; i++)
123  {
124  const double *caller_p = &points[i*3];
125 
126  if (caller_p[0]<xmin[0])
127  Copy(xmin,caller_p); /* New xminimum point */
128  if (caller_p[0]>xmax[0])
129  Copy(xmax,caller_p);
130  if (caller_p[1]<ymin[1])
131  Copy(ymin,caller_p);
132  if (caller_p[1]>ymax[1])
133  Copy(ymax,caller_p);
134  if (caller_p[2]<zmin[2])
135  Copy(zmin,caller_p);
136  if (caller_p[2]>zmax[2])
137  Copy(zmax,caller_p);
138  }
139 
140  /* Set xspan = distance between the 2 points xmin & xmax (squared) */
141  double dx = xmax[0] - xmin[0];
142  double dy = xmax[1] - xmin[1];
143  double dz = xmax[2] - xmin[2];
144  double xspan = dx*dx + dy*dy + dz*dz;
145 
146  /* Same for y & z spans */
147  dx = ymax[0] - ymin[0];
148  dy = ymax[1] - ymin[1];
149  dz = ymax[2] - ymin[2];
150  double yspan = dx*dx + dy*dy + dz*dz;
151 
152  dx = zmax[0] - zmin[0];
153  dy = zmax[1] - zmin[1];
154  dz = zmax[2] - zmin[2];
155  double zspan = dx*dx + dy*dy + dz*dz;
156 
157  /* Set points dia1 & dia2 to the maximally separated pair */
158  Copy(dia1,xmin);
159  Copy(dia2,xmax); /* assume xspan biggest */
160  double maxspan = xspan;
161 
162  if (yspan>maxspan)
163  {
164  maxspan = yspan;
165  Copy(dia1,ymin);
166  Copy(dia2,ymax);
167  }
168 
169  if (zspan>maxspan)
170  {
171  Copy(dia1,zmin);
172  Copy(dia2,zmax);
173  }
174 
175 
176  /* dia1,dia2 is a diameter of initial sphere */
177  /* calc initial center */
178  center[0] = (dia1[0]+dia2[0])*0.5f;
179  center[1] = (dia1[1]+dia2[1])*0.5f;
180  center[2] = (dia1[2]+dia2[2])*0.5f;
181 
182  /* calculate initial radius**2 and radius */
183 
184  dx = dia2[0]-center[0]; /* x component of radius vector */
185  dy = dia2[1]-center[1]; /* y component of radius vector */
186  dz = dia2[2]-center[2]; /* z component of radius vector */
187 
188  mRadius2 = dx*dx + dy*dy + dz*dz;
189  mRadius = double(sqrt(mRadius2));
190 
191  /* SECOND PASS: increment current sphere */
192  if ( 1 )
193  {
194  for (unsigned i=0; i<vcount; i++)
195  {
196  const double *caller_p = &points[i*3];
197 
198  dx = caller_p[0]-center[0];
199  dy = caller_p[1]-center[1];
200  dz = caller_p[2]-center[2];
201 
202  double old_to_p_sq = dx*dx + dy*dy + dz*dz;
203 
204  if (old_to_p_sq > mRadius2) /* do r**2 test first */
205  { /* this point is outside of current sphere */
206  double old_to_p = double(sqrt(old_to_p_sq));
207  /* calc radius of new sphere */
208  mRadius = (mRadius + old_to_p) * 0.5f;
209  mRadius2 = mRadius*mRadius; /* for next r**2 compare */
210  double old_to_new = old_to_p - mRadius;
211 
212  /* calc center of new sphere */
213 
214  double recip = 1.0f /old_to_p;
215 
216  double cx = (mRadius*center[0] + old_to_new*caller_p[0]) * recip;
217  double cy = (mRadius*center[1] + old_to_new*caller_p[1]) * recip;
218  double cz = (mRadius*center[2] + old_to_new*caller_p[2]) * recip;
219 
220  Set(center,cx,cy,cz);
221  }
222  }
223  }
224 
225  return mRadius;
226 }
227 
228 static inline void Set(float *n,float x,float y,float z)
229 {
230  n[0] = x;
231  n[1] = y;
232  n[2] = z;
233 };
234 
235 static inline void Copy(float *dest,const float *source)
236 {
237  dest[0] = source[0];
238  dest[1] = source[1];
239  dest[2] = source[2];
240 }
241 
242 
243 
244 float computeBoundingSphere(unsigned int vcount,const float *points,float *center)
245 {
246  float mRadius;
247  float mRadius2;
248 
249  float xmin[3];
250  float xmax[3];
251  float ymin[3];
252  float ymax[3];
253  float zmin[3];
254  float zmax[3];
255  float dia1[3];
256  float dia2[3];
257 
258  /* FIRST PASS: find 6 minima/maxima points */
259  Set(xmin,BIGNUMBER,BIGNUMBER,BIGNUMBER);
260  Set(xmax,-BIGNUMBER,-BIGNUMBER,-BIGNUMBER);
261  Set(ymin,BIGNUMBER,BIGNUMBER,BIGNUMBER);
262  Set(ymax,-BIGNUMBER,-BIGNUMBER,-BIGNUMBER);
263  Set(zmin,BIGNUMBER,BIGNUMBER,BIGNUMBER);
264  Set(zmax,-BIGNUMBER,-BIGNUMBER,-BIGNUMBER);
265 
266  for (unsigned i=0; i<vcount; i++)
267  {
268  const float *caller_p = &points[i*3];
269 
270  if (caller_p[0]<xmin[0])
271  Copy(xmin,caller_p); /* New xminimum point */
272  if (caller_p[0]>xmax[0])
273  Copy(xmax,caller_p);
274  if (caller_p[1]<ymin[1])
275  Copy(ymin,caller_p);
276  if (caller_p[1]>ymax[1])
277  Copy(ymax,caller_p);
278  if (caller_p[2]<zmin[2])
279  Copy(zmin,caller_p);
280  if (caller_p[2]>zmax[2])
281  Copy(zmax,caller_p);
282  }
283 
284  /* Set xspan = distance between the 2 points xmin & xmax (squared) */
285  float dx = xmax[0] - xmin[0];
286  float dy = xmax[1] - xmin[1];
287  float dz = xmax[2] - xmin[2];
288  float xspan = dx*dx + dy*dy + dz*dz;
289 
290  /* Same for y & z spans */
291  dx = ymax[0] - ymin[0];
292  dy = ymax[1] - ymin[1];
293  dz = ymax[2] - ymin[2];
294  float yspan = dx*dx + dy*dy + dz*dz;
295 
296  dx = zmax[0] - zmin[0];
297  dy = zmax[1] - zmin[1];
298  dz = zmax[2] - zmin[2];
299  float zspan = dx*dx + dy*dy + dz*dz;
300 
301  /* Set points dia1 & dia2 to the maximally separated pair */
302  Copy(dia1,xmin);
303  Copy(dia2,xmax); /* assume xspan biggest */
304  float maxspan = xspan;
305 
306  if (yspan>maxspan)
307  {
308  maxspan = yspan;
309  Copy(dia1,ymin);
310  Copy(dia2,ymax);
311  }
312 
313  if (zspan>maxspan)
314  {
315  Copy(dia1,zmin);
316  Copy(dia2,zmax);
317  }
318 
319 
320  /* dia1,dia2 is a diameter of initial sphere */
321  /* calc initial center */
322  center[0] = (dia1[0]+dia2[0])*0.5f;
323  center[1] = (dia1[1]+dia2[1])*0.5f;
324  center[2] = (dia1[2]+dia2[2])*0.5f;
325 
326  /* calculate initial radius**2 and radius */
327 
328  dx = dia2[0]-center[0]; /* x component of radius vector */
329  dy = dia2[1]-center[1]; /* y component of radius vector */
330  dz = dia2[2]-center[2]; /* z component of radius vector */
331 
332  mRadius2 = dx*dx + dy*dy + dz*dz;
333  mRadius = float(sqrt(mRadius2));
334 
335  /* SECOND PASS: increment current sphere */
336  if ( 1 )
337  {
338  for (unsigned i=0; i<vcount; i++)
339  {
340  const float *caller_p = &points[i*3];
341 
342  dx = caller_p[0]-center[0];
343  dy = caller_p[1]-center[1];
344  dz = caller_p[2]-center[2];
345 
346  float old_to_p_sq = dx*dx + dy*dy + dz*dz;
347 
348  if (old_to_p_sq > mRadius2) /* do r**2 test first */
349  { /* this point is outside of current sphere */
350  float old_to_p = float(sqrt(old_to_p_sq));
351  /* calc radius of new sphere */
352  mRadius = (mRadius + old_to_p) * 0.5f;
353  mRadius2 = mRadius*mRadius; /* for next r**2 compare */
354  float old_to_new = old_to_p - mRadius;
355 
356  /* calc center of new sphere */
357 
358  float recip = 1.0f /old_to_p;
359 
360  float cx = (mRadius*center[0] + old_to_new*caller_p[0]) * recip;
361  float cy = (mRadius*center[1] + old_to_new*caller_p[1]) * recip;
362  float cz = (mRadius*center[2] + old_to_new*caller_p[2]) * recip;
363 
364  Set(center,cx,cy,cz);
365  }
366  }
367  }
368 
369  return mRadius;
370 }
371 
372 
373 double computeSphereVolume(double r)
374 {
375  return (4.0f*3.141592654f*r*r*r)/3.0f; // 4/3 PI R cubed
376 }
377 
378 };
ConvexDecomposition::Set
static void Set(double *n, double x, double y, double z)
Definition: fitsphere.cpp:85
ConvexDecomposition::Copy
static void Copy(double *dest, const double *source)
Definition: fitsphere.cpp:92
BIGNUMBER
#define BIGNUMBER
Definition: fitsphere.cpp:83
ConvexDecomposition
Definition: bestfit.cpp:75
ConvexDecomposition::computeBoundingSphere
double computeBoundingSphere(unsigned int vcount, const double *points, double *center)
Definition: fitsphere.cpp:99
ConvexDecomposition::computeSphereVolume
double computeSphereVolume(double r)
Definition: fitsphere.cpp:373
fitsphere.h

convex_decomposition
Author(s): John W. Ratcliff
autogenerated on Wed Mar 2 2022 00:04:59