Extras/ConvexDecomposition/bestfit.cpp

   1 #include "float_math.h"
   2 #include <stdio.h>
   3 #include <stdlib.h>
   4 #include <string.h>
   5 #include <assert.h>
   6 #include <math.h>
   7
   8 /*----------------------------------------------------------------------
   9                 Copyright (c) 2004 Open Dynamics Framework Group
  10                                         www.physicstools.org
  11                 All rights reserved.
  12
  13                 Redistribution and use in source and binary forms, with or without modification, are permitted provided
  14                 that the following conditions are met:
  15
  16                 Redistributions of source code must retain the above copyright notice, this list of conditions
  17                 and the following disclaimer.
  18
  19                 Redistributions in binary form must reproduce the above copyright notice,
  20                 this list of conditions and the following disclaimer in the documentation
  21                 and/or other materials provided with the distribution.
  22
  23                 Neither the name of the Open Dynamics Framework Group nor the names of its contributors may
  24                 be used to endorse or promote products derived from this software without specific prior written permission.
  25
  26                 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' AND ANY EXPRESS OR IMPLIED WARRANTIES,
  27                 INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  28                 DISCLAIMED. IN NO EVENT SHALL THE INTEL OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
  29                 EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  30                 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
  31                 IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
  32                 THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  33 -----------------------------------------------------------------------*/
  34
  35 // http://codesuppository.blogspot.com
  36 //
  37 // mailto: jratcliff@infiniplex.net
  38 //
  39 // http://www.amillionpixels.us
  40 //
  41 // Geometric Tools, Inc.
  42 // http://www.geometrictools.com
  43 // Copyright (c) 1998-2006.  All Rights Reserved
  44 //
  45 // The Wild Magic Library (WM3) source code is supplied under the terms of
  46 // the license agreement
  47 //     http://www.geometrictools.com/License/WildMagic3License.pdf
  48 // and may not be copied or disclosed except in accordance with the terms
  49 // of that agreement.
  50
  51 #include "bestfit.h"
  52
  53 namespace BestFit
  54 {
  55
  56 class Vec3
  57 {
  58 public:
  59   Vec3(void) { };
  60   Vec3(float _x,float _y,float _z) { x = _x; y = _y; z = _z; };
  61
  62
  63   float dot(const Vec3 &v)
  64   {
  65     return x*v.x + y*v.y + z*v.z; // the dot product
  66   }
  67
  68   float x;
  69   float y;
  70   float z;
  71 };
  72
  73
  74 class Eigen
  75 {
  76 public:
  77
  78
  79   void DecrSortEigenStuff(void)
  80   {
  81     Tridiagonal(); //diagonalize the matrix.
  82     QLAlgorithm(); //
  83     DecreasingSort();
  84     GuaranteeRotation();
  85   }
  86
  87   void Tridiagonal(void)
  88   {
  89     float fM00 = mElement[0][0];
  90     float fM01 = mElement[0][1];
  91     float fM02 = mElement[0][2];
  92     float fM11 = mElement[1][1];
  93     float fM12 = mElement[1][2];
  94     float fM22 = mElement[2][2];
  95
  96     m_afDiag[0] = fM00;
  97     m_afSubd[2] = 0;
  98     if (fM02 != (float)0.0)
  99     {
 100       float fLength = sqrtf(fM01*fM01+fM02*fM02);
 101       float fInvLength = ((float)1.0)/fLength;
 102       fM01 *= fInvLength;
 103       fM02 *= fInvLength;
 104       float fQ = ((float)2.0)*fM01*fM12+fM02*(fM22-fM11);
 105       m_afDiag[1] = fM11+fM02*fQ;
 106       m_afDiag[2] = fM22-fM02*fQ;
 107       m_afSubd[0] = fLength;
 108       m_afSubd[1] = fM12-fM01*fQ;
 109       mElement[0][0] = (float)1.0;
 110       mElement[0][1] = (float)0.0;
 111       mElement[0][2] = (float)0.0;
 112       mElement[1][0] = (float)0.0;
 113       mElement[1][1] = fM01;
 114       mElement[1][2] = fM02;
 115       mElement[2][0] = (float)0.0;
 116       mElement[2][1] = fM02;
 117       mElement[2][2] = -fM01;
 118       m_bIsRotation = false;
 119     }
 120     else
 121     {
 122       m_afDiag[1] = fM11;
 123       m_afDiag[2] = fM22;
 124       m_afSubd[0] = fM01;
 125       m_afSubd[1] = fM12;
 126       mElement[0][0] = (float)1.0;
 127       mElement[0][1] = (float)0.0;
 128       mElement[0][2] = (float)0.0;
 129       mElement[1][0] = (float)0.0;
 130       mElement[1][1] = (float)1.0;
 131       mElement[1][2] = (float)0.0;
 132       mElement[2][0] = (float)0.0;
 133       mElement[2][1] = (float)0.0;
 134       mElement[2][2] = (float)1.0;
 135       m_bIsRotation = true;
 136     }
 137   }
 138
 139   bool QLAlgorithm(void)
 140   {
 141     const int iMaxIter = 32;
 142
 143     for (int i0 = 0; i0 <3; i0++)
 144     {
 145       int i1;
 146       for (i1 = 0; i1 < iMaxIter; i1++)
 147       {
 148         int i2;
 149         for (i2 = i0; i2 <= (3-2); i2++)
 150         {
 151           float fTmp = fabsf(m_afDiag[i2]) + fabsf(m_afDiag[i2+1]);
 152           if ( fabsf(m_afSubd[i2]) + fTmp == fTmp )
 153             break;
 154         }
 155         if (i2 == i0)
 156         {
 157           break;
 158         }
 159
 160         float fG = (m_afDiag[i0+1] - m_afDiag[i0])/(((float)2.0) * m_afSubd[i0]);
 161         float fR = sqrtf(fG*fG+(float)1.0);
 162         if (fG < (float)0.0)
 163         {
 164           fG = m_afDiag[i2]-m_afDiag[i0]+m_afSubd[i0]/(fG-fR);
 165         }
 166         else
 167         {
 168           fG = m_afDiag[i2]-m_afDiag[i0]+m_afSubd[i0]/(fG+fR);
 169         }
 170         float fSin = (float)1.0, fCos = (float)1.0, fP = (float)0.0;
 171         for (int i3 = i2-1; i3 >= i0; i3--)
 172         {
 173           float fF = fSin*m_afSubd[i3];
 174           float fB = fCos*m_afSubd[i3];
 175           if (fabsf(fF) >= fabsf(fG))
 176           {
 177             fCos = fG/fF;
 178             fR = sqrtf(fCos*fCos+(float)1.0);
 179             m_afSubd[i3+1] = fF*fR;
 180             fSin = ((float)1.0)/fR;
 181             fCos *= fSin;
 182           }
 183           else
 184           {
 185             fSin = fF/fG;
 186             fR = sqrtf(fSin*fSin+(float)1.0);
 187             m_afSubd[i3+1] = fG*fR;
 188             fCos = ((float)1.0)/fR;
 189             fSin *= fCos;
 190           }
 191           fG = m_afDiag[i3+1]-fP;
 192           fR = (m_afDiag[i3]-fG)*fSin+((float)2.0)*fB*fCos;
 193           fP = fSin*fR;
 194           m_afDiag[i3+1] = fG+fP;
 195           fG = fCos*fR-fB;
 196           for (int i4 = 0; i4 < 3; i4++)
 197           {
 198             fF = mElement[i4][i3+1];
 199             mElement[i4][i3+1] = fSin*mElement[i4][i3]+fCos*fF;
 200             mElement[i4][i3] = fCos*mElement[i4][i3]-fSin*fF;
 201           }
 202         }
 203         m_afDiag[i0] -= fP;
 204         m_afSubd[i0] = fG;
 205         m_afSubd[i2] = (float)0.0;
 206       }
 207       if (i1 == iMaxIter)
 208       {
 209         return false;
 210       }
 211     }
 212     return true;
 213   }
 214
 215   void DecreasingSort(void)
 216   {
 217     //sort eigenvalues in decreasing order, e[0] >= ... >= e[iSize-1]
 218     for (int i0 = 0, i1; i0 <= 3-2; i0++)
 219     {
 220       // locate maximum eigenvalue
 221       i1 = i0;
 222       float fMax = m_afDiag[i1];
 223       int i2;
 224       for (i2 = i0+1; i2 < 3; i2++)
 225       {
 226         if (m_afDiag[i2] > fMax)
 227         {
 228           i1 = i2;
 229           fMax = m_afDiag[i1];
 230         }
 231       }
 232
 233       if (i1 != i0)
 234       {
 235         // swap eigenvalues
 236         m_afDiag[i1] = m_afDiag[i0];
 237         m_afDiag[i0] = fMax;
 238         // swap eigenvectors
 239         for (i2 = 0; i2 < 3; i2++)
 240         {
 241           float fTmp = mElement[i2][i0];
 242           mElement[i2][i0] = mElement[i2][i1];
 243           mElement[i2][i1] = fTmp;
 244           m_bIsRotation = !m_bIsRotation;
 245         }
 246       }
 247     }
 248   }
 249
 250
 251   void GuaranteeRotation(void)
 252   {
 253     if (!m_bIsRotation)
 254     {
 255       // change sign on the first column
 256       for (int iRow = 0; iRow <3; iRow++)
 257       {
 258         mElement[iRow][0] = -mElement[iRow][0];
 259       }
 260     }
 261   }
 262
 263   float mElement[3][3];
 264   float m_afDiag[3];
 265   float m_afSubd[3];
 266   bool m_bIsRotation;
 267 };
 268
 269 }
 270
 271
 272 using namespace BestFit;
 273
 274
 275 bool getBestFitPlane(unsigned int vcount,
 276                      const float *points,
 277                      unsigned int vstride,
 278                      const float *weights,
 279                      unsigned int wstride,
 280                      float *plane)
 281 {
 282   bool ret = false;
 283
 284   Vec3 kOrigin(0,0,0);
 285
 286   float wtotal = 0;
 287
 288   if ( 1 )
 289   {
 290     const char *source  = (const char *) points;
 291     const char *wsource = (const char *) weights;
 292
 293     for (unsigned int i=0; i<vcount; i++)
 294     {
 295
 296       const float *p = (const float *) source;
 297
 298       float w = 1;
 299
 300       if ( wsource )
 301       {
 302         const float *ws = (const float *) wsource;
 303         w = *ws; //
 304         wsource+=wstride;
 305       }
 306
 307       kOrigin.x+=p[0]*w;
 308       kOrigin.y+=p[1]*w;
 309       kOrigin.z+=p[2]*w;
 310
 311       wtotal+=w;
 312
 313       source+=vstride;
 314     }
 315   }
 316
 317   float recip = 1.0f / wtotal; // reciprocol of total weighting
 318
 319   kOrigin.x*=recip;
 320   kOrigin.y*=recip;
 321   kOrigin.z*=recip;
 322
 323
 324   float fSumXX=0;
 325   float fSumXY=0;
 326   float fSumXZ=0;
 327
 328   float fSumYY=0;
 329   float fSumYZ=0;
 330   float fSumZZ=0;
 331
 332
 333   if ( 1 )
 334   {
 335     const char *source  = (const char *) points;
 336     const char *wsource = (const char *) weights;
 337
 338     for (unsigned int i=0; i<vcount; i++)
 339     {
 340
 341       const float *p = (const float *) source;
 342
 343       float w = 1;
 344
 345       if ( wsource )
 346       {
 347         const float *ws = (const float *) wsource;
 348         w = *ws; //
 349         wsource+=wstride;
 350       }
 351
 352       Vec3 kDiff;
 353
 354       kDiff.x = w*(p[0] - kOrigin.x); // apply vertex weighting!
 355       kDiff.y = w*(p[1] - kOrigin.y);
 356       kDiff.z = w*(p[2] - kOrigin.z);
 357
 358       fSumXX+= kDiff.x * kDiff.x; // sume of the squares of the differences.
 359       fSumXY+= kDiff.x * kDiff.y; // sume of the squares of the differences.
 360       fSumXZ+= kDiff.x * kDiff.z; // sume of the squares of the differences.
 361
 362       fSumYY+= kDiff.y * kDiff.y;
 363       fSumYZ+= kDiff.y * kDiff.z;
 364       fSumZZ+= kDiff.z * kDiff.z;
 365
 366
 367       source+=vstride;
 368     }
 369   }
 370
 371   fSumXX *= recip;
 372   fSumXY *= recip;
 373   fSumXZ *= recip;
 374   fSumYY *= recip;
 375   fSumYZ *= recip;
 376   fSumZZ *= recip;
 377
 378   // setup the eigensolver
 379   Eigen kES;
 380
 381   kES.mElement[0][0] = fSumXX;
 382   kES.mElement[0][1] = fSumXY;
 383   kES.mElement[0][2] = fSumXZ;
 384
 385   kES.mElement[1][0] = fSumXY;
 386   kES.mElement[1][1] = fSumYY;
 387   kES.mElement[1][2] = fSumYZ;
 388
 389   kES.mElement[2][0] = fSumXZ;
 390   kES.mElement[2][1] = fSumYZ;
 391   kES.mElement[2][2] = fSumZZ;
 392
 393   // compute eigenstuff, smallest eigenvalue is in last position
 394   kES.DecrSortEigenStuff();
 395
 396   Vec3 kNormal;
 397
 398   kNormal.x = kES.mElement[0][2];
 399   kNormal.y = kES.mElement[1][2];
 400   kNormal.z = kES.mElement[2][2];
 401
 402   // the minimum energy
 403   plane[0] = kNormal.x;
 404   plane[1] = kNormal.y;
 405   plane[2] = kNormal.z;
 406
 407   plane[3] = 0 - kNormal.dot(kOrigin);
 408
 409   return ret;
 410 }
 411
 412
 413
 414 float getBoundingRegion(unsigned int vcount,const float *points,unsigned int pstride,float *bmin,float *bmax) // returns the diagonal distance
 415 {
 416
 417   const unsigned char *source = (const unsigned char *) points;
 418
 419         bmin[0] = points[0];
 420         bmin[1] = points[1];
 421         bmin[2] = points[2];
 422
 423         bmax[0] = points[0];
 424         bmax[1] = points[1];
 425         bmax[2] = points[2];
 426
 427
 428   for (unsigned int i=1; i<vcount; i++)
 429   {
 430         source+=pstride;
 431         const float *p = (const float *) source;
 432
 433         if ( p[0] < bmin[0] ) bmin[0] = p[0];
 434         if ( p[1] < bmin[1] ) bmin[1] = p[1];
 435         if ( p[2] < bmin[2] ) bmin[2] = p[2];
 436
 437                 if ( p[0] > bmax[0] ) bmax[0] = p[0];
 438                 if ( p[1] > bmax[1] ) bmax[1] = p[1];
 439                 if ( p[2] > bmax[2] ) bmax[2] = p[2];
 440
 441   }
 442
 443   float dx = bmax[0] - bmin[0];
 444   float dy = bmax[1] - bmin[1];
 445   float dz = bmax[2] - bmin[2];
 446
 447         return sqrtf( dx*dx + dy*dy + dz*dz );
 448
 449 }
 450
 451
 452 bool  overlapAABB(const float *bmin1,const float *bmax1,const float *bmin2,const float *bmax2) // return true if the two AABB's overlap.
 453 {
 454   if ( bmax2[0] < bmin1[0] ) return false; // if the maximum is less than our minimum on any axis
 455   if ( bmax2[1] < bmin1[1] ) return false;
 456   if ( bmax2[2] < bmin1[2] ) return false;
 457
 458   if ( bmin2[0] > bmax1[0] ) return false; // if the minimum is greater than our maximum on any axis
 459   if ( bmin2[1] > bmax1[1] ) return false; // if the minimum is greater than our maximum on any axis
 460   if ( bmin2[2] > bmax1[2] ) return false; // if the minimum is greater than our maximum on any axis
 461
 462
 463   return true; // the extents overlap
 464 }
 465
 466