00001 #include <stdio.h>
00002 #include <stdlib.h>
00003 #include <string.h>
00004 #include <math.h>
00005 #include <assert.h>
00006
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090 #include "bestfitobb.h"
00091 #include "float_math.h"
00092
00093 namespace ConvexDecomposition
00094 {
00095
00096
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 );
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;
00161 double steps = 7.0f;
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];
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;
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;
00220 }
00221 else
00222 {
00223 break;
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)
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 };