Extras/ConvexDecomposition/fitsphere.cpp

   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
  10 /*----------------------------------------------------------------------
  11                 Copyright (c) 2004 Open Dynamics Framework Group
  12                                         www.physicstools.org
  13                 All rights reserved.
  14
  15                 Redistribution and use in source and binary forms, with or without modification, are permitted provided
  16                 that the following conditions are met:
  17
  18                 Redistributions of source code must retain the above copyright notice, this list of conditions
  19                 and the following disclaimer.
  20
  21                 Redistributions in binary form must reproduce the above copyright notice,
  22                 this list of conditions and the following disclaimer in the documentation
  23                 and/or other materials provided with the distribution.
  24
  25                 Neither the name of the Open Dynamics Framework Group nor the names of its contributors may
  26                 be used to endorse or promote products derived from this software without specific prior written permission.
  27
  28                 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' AND ANY EXPRESS OR IMPLIED WARRANTIES,
  29                 INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  30                 DISCLAIMED. IN NO EVENT SHALL THE INTEL OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
  31                 EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  32                 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
  33                 IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
  34                 THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  35 -----------------------------------------------------------------------*/
  36
  37 // http://codesuppository.blogspot.com
  38 //
  39 // mailto: jratcliff@infiniplex.net
  40 //
  41 // http://www.amillionpixels.us
  42 //
  43 /*
  44 An Efficient Bounding Sphere
  45 by Jack Ritter
  46 from "Graphics Gems", Academic Press, 1990
  47 */
  48
  49 /* Routine to calculate tight bounding sphere over    */
  50 /* a set of points in 3D */
  51 /* This contains the routine find_bounding_sphere(), */
  52 /* the struct definition, and the globals used for parameters. */
  53 /* The abs() of all coordinates must be < BIGNUMBER */
  54 /* Code written by Jack Ritter and Lyle Rains. */
  55
  56 #define BIGNUMBER 100000000.0           /* hundred million */
  57
  58 static inline void Set(float *n,float x,float y,float z)
  59 {
  60         n[0] = x;
  61         n[1] = y;
  62         n[2] = z;
  63 }
  64
  65 static inline void Copy(float *dest,const float *source)
  66 {
  67         dest[0] = source[0];
  68         dest[1] = source[1];
  69         dest[2] = source[2];
  70 }
  71
  72 float computeBoundingSphere(unsigned int vcount,const float *points,float *center)
  73 {
  74
  75   float mRadius;
  76   float mRadius2;
  77
  78         float xmin[3];
  79         float xmax[3];
  80         float ymin[3];
  81         float ymax[3];
  82         float zmin[3];
  83         float zmax[3];
  84         float dia1[3];
  85         float dia2[3];
  86
  87   /* FIRST PASS: find 6 minima/maxima points */
  88   Set(xmin,BIGNUMBER,BIGNUMBER,BIGNUMBER);
  89   Set(xmax,-BIGNUMBER,-BIGNUMBER,-BIGNUMBER);
  90   Set(ymin,BIGNUMBER,BIGNUMBER,BIGNUMBER);
  91   Set(ymax,-BIGNUMBER,-BIGNUMBER,-BIGNUMBER);
  92   Set(zmin,BIGNUMBER,BIGNUMBER,BIGNUMBER);
  93   Set(zmax,-BIGNUMBER,-BIGNUMBER,-BIGNUMBER);
  94
  95   for (unsigned i=0; i<vcount; i++)
  96         {
  97                 const float *caller_p = &points[i*3];
  98
  99         if (caller_p[0]<xmin[0])
 100           Copy(xmin,caller_p); /* New xminimum point */
 101         if (caller_p[0]>xmax[0])
 102           Copy(xmax,caller_p);
 103         if (caller_p[1]<ymin[1])
 104           Copy(ymin,caller_p);
 105         if (caller_p[1]>ymax[1])
 106           Copy(ymax,caller_p);
 107         if (caller_p[2]<zmin[2])
 108           Copy(zmin,caller_p);
 109         if (caller_p[2]>zmax[2])
 110           Copy(zmax,caller_p);
 111         }
 112
 113   /* Set xspan = distance between the 2 points xmin & xmax (squared) */
 114   float dx = xmax[0] - xmin[0];
 115   float dy = xmax[1] - xmin[1];
 116   float dz = xmax[2] - xmin[2];
 117   float xspan = dx*dx + dy*dy + dz*dz;
 118
 119   /* Same for y & z spans */
 120   dx = ymax[0] - ymin[0];
 121   dy = ymax[1] - ymin[1];
 122   dz = ymax[2] - ymin[2];
 123   float yspan = dx*dx + dy*dy + dz*dz;
 124
 125   dx = zmax[0] - zmin[0];
 126   dy = zmax[1] - zmin[1];
 127   dz = zmax[2] - zmin[2];
 128   float zspan = dx*dx + dy*dy + dz*dz;
 129
 130   /* Set points dia1 & dia2 to the maximally separated pair */
 131   Copy(dia1,xmin);
 132   Copy(dia2,xmax); /* assume xspan biggest */
 133   float maxspan = xspan;
 134
 135   if (yspan>maxspan)
 136         {
 137           maxspan = yspan;
 138         Copy(dia1,ymin);
 139         Copy(dia2,ymax);
 140         }
 141
 142   if (zspan>maxspan)
 143         {
 144           Copy(dia1,zmin);
 145           Copy(dia2,zmax);
 146         }
 147
 148
 149   /* dia1,dia2 is a diameter of initial sphere */
 150   /* calc initial center */
 151   center[0] = (dia1[0]+dia2[0])*0.5f;
 152   center[1] = (dia1[1]+dia2[1])*0.5f;
 153   center[2] = (dia1[2]+dia2[2])*0.5f;
 154
 155   /* calculate initial radius**2 and radius */
 156
 157   dx = dia2[0]-center[0]; /* x component of radius vector */
 158   dy = dia2[1]-center[1]; /* y component of radius vector */
 159   dz = dia2[2]-center[2]; /* z component of radius vector */
 160
 161   mRadius2 = dx*dx + dy*dy + dz*dz;
 162   mRadius = float(sqrt(mRadius2));
 163
 164   /* SECOND PASS: increment current sphere */
 165
 166         if ( 1 )
 167         {
 168                 for (unsigned i=0; i<vcount; i++)
 169                 {
 170                         const float *caller_p = &points[i*3];
 171
 172                 dx = caller_p[0]-center[0];
 173                         dy = caller_p[1]-center[1];
 174                 dz = caller_p[2]-center[2];
 175
 176                         float old_to_p_sq = dx*dx + dy*dy + dz*dz;
 177
 178                 if (old_to_p_sq > mRadius2)     /* do r**2 test first */
 179                         {       /* this point is outside of current sphere */
 180                         float old_to_p = float(sqrt(old_to_p_sq));
 181                                 /* calc radius of new sphere */
 182                         mRadius = (mRadius + old_to_p) * 0.5f;
 183                         mRadius2 = mRadius*mRadius;     /* for next r**2 compare */
 184                         float old_to_new = old_to_p - mRadius;
 185
 186                         /* calc center of new sphere */
 187
 188                                 float recip = 1.0f /old_to_p;
 189
 190                         float cx = (mRadius*center[0] + old_to_new*caller_p[0]) * recip;
 191                         float cy = (mRadius*center[1] + old_to_new*caller_p[1]) * recip;
 192                                 float cz = (mRadius*center[2] + old_to_new*caller_p[2]) * recip;
 193
 194                                 Set(center,cx,cy,cz);
 195                         }
 196                 }
 197         }
 198
 199   return mRadius;
 200 }
 201
 202