Extras/ConvexDecomposition/ConvexDecomposition.cpp

   1 #include "float_math.h"
   2
   3 #include <stdio.h>
   4 #include <stdlib.h>
   5 #include <string.h>
   6 #include <assert.h>
   7
   8
   9 /*----------------------------------------------------------------------
  10                 Copyright (c) 2004 Open Dynamics Framework Group
  11                                         www.physicstools.org
  12                 All rights reserved.
  13
  14                 Redistribution and use in source and binary forms, with or without modification, are permitted provided
  15                 that the following conditions are met:
  16
  17                 Redistributions of source code must retain the above copyright notice, this list of conditions
  18                 and the following disclaimer.
  19
  20                 Redistributions in binary form must reproduce the above copyright notice,
  21                 this list of conditions and the following disclaimer in the documentation
  22                 and/or other materials provided with the distribution.
  23
  24                 Neither the name of the Open Dynamics Framework Group nor the names of its contributors may
  25                 be used to endorse or promote products derived from this software without specific prior written permission.
  26
  27                 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' AND ANY EXPRESS OR IMPLIED WARRANTIES,
  28                 INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  29                 DISCLAIMED. IN NO EVENT SHALL THE INTEL OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
  30                 EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  31                 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
  32                 IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
  33                 THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  34 -----------------------------------------------------------------------*/
  35
  36 // http://codesuppository.blogspot.com
  37 //
  38 // mailto: jratcliff@infiniplex.net
  39 //
  40 // http://www.amillionpixels.us
  41 //
  42
  43 #include "ConvexDecomposition.h"
  44 #include "cd_vector.h"
  45 #include "cd_hull.h"
  46 #include "bestfit.h"
  47 #include "planetri.h"
  48 #include "vlookup.h"
  49 #include "splitplane.h"
  50 #include "meshvolume.h"
  51 #include "concavity.h"
  52 #include "bestfitobb.h"
  53 #include "float_math.h"
  54 #include "fitsphere.h"
  55
  56 #define SHOW_MESH 0
  57 #define MAKE_MESH 1
  58
  59
  60 using namespace ConvexDecomposition;
  61
  62
  63
  64 namespace ConvexDecomposition
  65 {
  66
  67 class FaceTri
  68 {
  69 public:
  70         FaceTri(void) { };
  71   FaceTri(const float *vertices,unsigned int i1,unsigned int i2,unsigned int i3)
  72   {
  73         mP1.Set( &vertices[i1*3] );
  74         mP2.Set( &vertices[i2*3] );
  75         mP3.Set( &vertices[i3*3] );
  76   }
  77
  78   Vector3d      mP1;
  79   Vector3d      mP2;
  80   Vector3d      mP3;
  81   Vector3d mNormal;
  82
  83 };
  84
  85
  86 void addTri(VertexLookup vl,UintVector &list,const Vector3d &p1,const Vector3d &p2,const Vector3d &p3)
  87 {
  88   unsigned int i1 = Vl_getIndex(vl, p1.Ptr() );
  89   unsigned int i2 = Vl_getIndex(vl, p2.Ptr() );
  90   unsigned int i3 = Vl_getIndex(vl, p3.Ptr() );
  91
  92   // do *not* process degenerate triangles!
  93
  94   if ( i1 != i2 && i1 != i3 && i2 != i3 )
  95   {
  96     list.push_back(i1);
  97     list.push_back(i2);
  98     list.push_back(i3);
  99   }
 100 }
 101
 102
 103 void calcConvexDecomposition(unsigned int           vcount,
 104                                 const float           *vertices,
 105                                 unsigned int           tcount,
 106                                 const unsigned int    *indices,
 107                                 ConvexDecompInterface *callback,
 108                                 float                  masterVolume,
 109                                 unsigned int           depth)
 110
 111 {
 112
 113   float plane[4];
 114
 115   bool split = false;
 116
 117
 118   if ( depth < MAXDEPTH )
 119   {
 120
 121                 float volume;
 122                 float c = computeConcavity( vcount, vertices, tcount, indices, callback, plane, volume );
 123
 124     if ( depth == 0 )
 125     {
 126       masterVolume = volume;
 127     }
 128
 129                 float percent = (c*100.0f)/masterVolume;
 130
 131                 if ( percent > CONCAVE_PERCENT ) // if great than 5% of the total volume is concave, go ahead and keep splitting.
 132                 {
 133       split = true;
 134     }
 135
 136   }
 137
 138   if ( depth >= MAXDEPTH || !split )
 139   {
 140
 141 #if 1
 142
 143     HullResult result;
 144     HullLibrary hl;
 145     HullDesc   desc;
 146
 147         desc.SetHullFlag(QF_TRIANGLES);
 148
 149     desc.mVcount       = vcount;
 150     desc.mVertices     = vertices;
 151     desc.mVertexStride = sizeof(float)*3;
 152
 153     HullError ret = hl.CreateConvexHull(desc,result);
 154
 155     if ( ret == QE_OK )
 156     {
 157
 158                         ConvexResult r(result.mNumOutputVertices, result.mOutputVertices, result.mNumFaces, result.mIndices);
 159
 160
 161                         callback->ConvexDecompResult(r);
 162     }
 163
 164
 165 #else
 166
 167                 static unsigned int colors[8] =
 168                 {
 169                         0xFF0000,
 170                   0x00FF00,
 171                         0x0000FF,
 172                         0xFFFF00,
 173                         0x00FFFF,
 174                         0xFF00FF,
 175                         0xFFFFFF,
 176                         0xFF8040
 177                 };
 178
 179                 static int count = 0;
 180
 181                 count++;
 182
 183                 if ( count == 8 ) count = 0;
 184
 185                 assert( count >= 0 && count < 8 );
 186
 187                 unsigned int color = colors[count];
 188
 189     const unsigned int *source = indices;
 190
 191     for (unsigned int i=0; i<tcount; i++)
 192     {
 193
 194       unsigned int i1 = *source++;
 195       unsigned int i2 = *source++;
 196       unsigned int i3 = *source++;
 197
 198                         FaceTri t(vertices, i1, i2, i3 );
 199
 200       callback->ConvexDebugTri( t.mP1.Ptr(), t.mP2.Ptr(), t.mP3.Ptr(), color );
 201
 202     }
 203 #endif
 204
 205     hl.ReleaseResult (result);
 206     return;
 207
 208   }
 209
 210   UintVector ifront;
 211   UintVector iback;
 212
 213   VertexLookup vfront = Vl_createVertexLookup();
 214   VertexLookup vback  = Vl_createVertexLookup();
 215
 216
 217         bool showmesh = false;
 218   #if SHOW_MESH
 219   showmesh = true;
 220   #endif
 221
 222         if ( 0 )
 223         {
 224                 showmesh = true;
 225           for (float x=-1; x<1; x+=0.10f)
 226                 {
 227                   for (float y=0; y<1; y+=0.10f)
 228                         {
 229                           for (float z=-1; z<1; z+=0.04f)
 230                                 {
 231                                   float d = x*plane[0] + y*plane[1] + z*plane[2] + plane[3];
 232                                         Vector3d p(x,y,z);
 233                                   if ( d >= 0 )
 234                                           callback->ConvexDebugPoint(p.Ptr(), 0.02f, 0x00FF00);
 235                                   else
 236                                           callback->ConvexDebugPoint(p.Ptr(), 0.02f, 0xFF0000);
 237                                 }
 238                         }
 239                 }
 240         }
 241
 242         if ( 1 )
 243         {
 244                 // ok..now we are going to 'split' all of the input triangles against this plane!
 245                 const unsigned int *source = indices;
 246                 for (unsigned int i=0; i<tcount; i++)
 247                 {
 248                         unsigned int i1 = *source++;
 249                         unsigned int i2 = *source++;
 250                         unsigned int i3 = *source++;
 251
 252                         FaceTri t(vertices, i1, i2, i3 );
 253
 254                         Vector3d front[4];
 255                         Vector3d back[4];
 256
 257                         unsigned int fcount=0;
 258                         unsigned int bcount=0;
 259
 260                         PlaneTriResult result;
 261
 262                   result = planeTriIntersection(plane,t.mP1.Ptr(),sizeof(Vector3d),0.00001f,front[0].Ptr(),fcount,back[0].Ptr(),bcount );
 263
 264                         if( fcount > 4 || bcount > 4 )
 265                         {
 266                     result = planeTriIntersection(plane,t.mP1.Ptr(),sizeof(Vector3d),0.00001f,front[0].Ptr(),fcount,back[0].Ptr(),bcount );
 267                         }
 268
 269                         switch ( result )
 270                         {
 271                                 case PTR_FRONT:
 272
 273                                         assert( fcount == 3 );
 274
 275           if ( showmesh )
 276             callback->ConvexDebugTri( front[0].Ptr(), front[1].Ptr(), front[2].Ptr(), 0x00FF00 );
 277
 278           #if MAKE_MESH
 279
 280           addTri( vfront, ifront, front[0], front[1], front[2] );
 281
 282
 283           #endif
 284
 285                                         break;
 286                                 case PTR_BACK:
 287                                         assert( bcount == 3 );
 288
 289           if ( showmesh )
 290                                         callback->ConvexDebugTri( back[0].Ptr(), back[1].Ptr(), back[2].Ptr(), 0xFFFF00 );
 291
 292           #if MAKE_MESH
 293
 294           addTri( vback, iback, back[0], back[1], back[2] );
 295
 296           #endif
 297
 298                                         break;
 299                                 case PTR_SPLIT:
 300
 301                                         assert( fcount >= 3 && fcount <= 4);
 302                                         assert( bcount >= 3 && bcount <= 4);
 303
 304           #if MAKE_MESH
 305
 306           addTri( vfront, ifront, front[0], front[1], front[2] );
 307           addTri( vback, iback, back[0], back[1], back[2] );
 308
 309
 310           if ( fcount == 4 )
 311           {
 312             addTri( vfront, ifront, front[0], front[2], front[3] );
 313           }
 314
 315           if ( bcount == 4  )
 316           {
 317             addTri( vback, iback, back[0], back[2], back[3] );
 318           }
 319
 320           #endif
 321
 322           if ( showmesh )
 323           {
 324                                         callback->ConvexDebugTri( front[0].Ptr(), front[1].Ptr(), front[2].Ptr(), 0x00D000 );
 325                                         callback->ConvexDebugTri( back[0].Ptr(), back[1].Ptr(), back[2].Ptr(), 0xD0D000 );
 326
 327                                         if ( fcount == 4 )
 328                                         {
 329                                                 callback->ConvexDebugTri( front[0].Ptr(), front[2].Ptr(), front[3].Ptr(), 0x00D000 );
 330                                         }
 331                                         if ( bcount == 4 )
 332                                         {
 333                                                 callback->ConvexDebugTri( back[0].Ptr(), back[2].Ptr(), back[3].Ptr(), 0xD0D000 );
 334                                         }
 335                                 }
 336
 337                                         break;
 338                         }
 339                 }
 340
 341     // ok... here we recursively call
 342     if ( ifront.size() )
 343     {
 344       unsigned int vcount   = Vl_getVcount(vfront);
 345       const float *vertices = Vl_getVertices(vfront);
 346       unsigned int tcount   = ifront.size()/3;
 347
 348       calcConvexDecomposition(vcount, vertices, tcount, &ifront[0], callback, masterVolume, depth+1);
 349
 350     }
 351
 352     ifront.clear();
 353
 354     Vl_releaseVertexLookup(vfront);
 355
 356     if ( iback.size() )
 357     {
 358       unsigned int vcount   = Vl_getVcount(vback);
 359       const float *vertices = Vl_getVertices(vback);
 360       unsigned int tcount   = iback.size()/3;
 361
 362       calcConvexDecomposition(vcount, vertices, tcount, &iback[0], callback, masterVolume, depth+1);
 363
 364     }
 365
 366     iback.clear();
 367     Vl_releaseVertexLookup(vback);
 368
 369         }
 370 }
 371
 372
 373
 374
 375 }