ConvexDecomposition.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 
62 #include <algorithm>
63 #include <vector>
64 
65 #include "ConvexDecomposition.h"
66 #include "cd_vector.h"
67 #include "cd_hull.h"
68 #include "bestfit.h"
69 #include "planetri.h"
70 #include "vlookup.h"
71 #include "splitplane.h"
72 #include "meshvolume.h"
73 #include "concavity.h"
74 #include "bestfitobb.h"
75 #include "fitsphere.h"
76 #include "triangulate.h"
77 #include "float_math.h"
78 
79 #define MAKE_MESH 1
80 #define CLOSE_FACE 0
81 
82 static unsigned int MAXDEPTH=8;
83 static double CONCAVE_PERCENT=1.0f;
84 static double MERGE_PERCENT=2.0f;
85 
86 
87 using namespace ConvexDecomposition;
88 
89 typedef std::vector< unsigned int > UintVector;
90 
91 namespace ConvexDecomposition
92 {
93 
94 class Edge
95 {
96 public:
97 
98  Edge(unsigned int i1,unsigned int i2)
99  {
100  mE1 = i1;
101  mE2 = i2;
102  mUsed = false;
103  }
104 
105  unsigned int mE1;
106  unsigned int mE2;
107  bool mUsed;
108 };
109 
110 typedef std::vector< Edge > EdgeVector;
111 
112 class FaceTri
113 {
114 public:
115  FaceTri(void) { };
116 
117  FaceTri(const double *vertices,unsigned int i1,unsigned int i2,unsigned int i3)
118  {
119  mP1.Set( &vertices[i1*3] );
120  mP2.Set( &vertices[i2*3] );
121  mP3.Set( &vertices[i3*3] );
122  }
123 
128 
129 };
130 
131 
132 
133 class CHull
134 {
135 public:
136  CHull(const ConvexResult &result)
137  {
138  mResult = new ConvexResult(result);
139  mVolume = computeMeshVolume( result.mHullVertices, result.mHullTcount, result.mHullIndices );
140 
141  mDiagonal = getBoundingRegion( result.mHullVcount, result.mHullVertices, sizeof(double)*3, mMin, mMax );
142 
143  double dx = mMax[0] - mMin[0];
144  double dy = mMax[1] - mMin[1];
145  double dz = mMax[2] - mMin[2];
146 
147  dx*=0.1f; // inflate 1/10th on each edge
148  dy*=0.1f; // inflate 1/10th on each edge
149  dz*=0.1f; // inflate 1/10th on each edge
150 
151  mMin[0]-=dx;
152  mMin[1]-=dy;
153  mMin[2]-=dz;
154 
155  mMax[0]+=dx;
156  mMax[1]+=dy;
157  mMax[2]+=dz;
158 
159 
160  }
161 
162  ~CHull(void)
163  {
164  delete mResult;
165  }
166 
167  bool overlap(const CHull &h) const
168  {
169  return overlapAABB(mMin,mMax, h.mMin, h.mMax );
170  }
171 
172  double mMin[3];
173  double mMax[3];
174  double mVolume;
175  double mDiagonal; // long edge..
177 };
178 
179 // Usage: std::sort( list.begin(), list.end(), StringSortRef() );
181 {
182  public:
183 
184  bool operator()(const CHull *a,const CHull *b) const
185  {
186  return a->mVolume < b->mVolume;
187  }
188 };
189 
190 
191 typedef std::vector< CHull * > CHullVector;
192 
193 
195 {
196 public:
198  {
199  mCallback = callback;
200  };
201 
203  {
204  CHullVector::iterator i;
205  for (i=mChulls.begin(); i!=mChulls.end(); ++i)
206  {
207  CHull *cr = (*i);
208  delete cr;
209  }
210  }
211 
212  bool isDuplicate(unsigned int i1,unsigned int i2,unsigned int i3,
213  unsigned int ci1,unsigned int ci2,unsigned int ci3)
214  {
215  unsigned int dcount = 0;
216 
217  assert( i1 != i2 && i1 != i3 && i2 != i3 );
218  assert( ci1 != ci2 && ci1 != ci3 && ci2 != ci3 );
219 
220  if ( i1 == ci1 || i1 == ci2 || i1 == ci3 ) dcount++;
221  if ( i2 == ci1 || i2 == ci2 || i2 == ci3 ) dcount++;
222  if ( i3 == ci1 || i3 == ci2 || i3 == ci3 ) dcount++;
223 
224  return dcount == 3;
225  }
226 
227  void getMesh(const ConvexResult &cr,VertexLookup vc)
228  {
229  unsigned int *src = cr.mHullIndices;
230 
231  for (unsigned int i=0; i<cr.mHullTcount; i++)
232  {
233  unsigned int i1 = *src++;
234  unsigned int i2 = *src++;
235  unsigned int i3 = *src++;
236 
237  const double *p1 = &cr.mHullVertices[i1*3];
238  const double *p2 = &cr.mHullVertices[i2*3];
239  const double *p3 = &cr.mHullVertices[i3*3];
240 
241  i1 = Vl_getIndex(vc,p1);
242  i2 = Vl_getIndex(vc,p2);
243  i3 = Vl_getIndex(vc,p3);
244 
245 
246  }
247  }
248 
250  {
251 
252  if ( !a->overlap(*b) ) return 0; // if their AABB's (with a little slop) don't overlap, then return.
253 
254  if ( MERGE_PERCENT < 0 ) return 0;
255 
256  assert( a->mVolume > 0 );
257  assert( b->mVolume > 0 );
258 
259  CHull *ret = 0;
260 
261  // ok..we are going to combine both meshes into a single mesh
262  // and then we are going to compute the concavity...
263 
265 
266  getMesh( *a->mResult, vc);
267  getMesh( *b->mResult, vc);
268 
269  unsigned int vcount = Vl_getVcount(vc);
270  const double *vertices = Vl_getVertices(vc);
271 
272  HullResult hresult;
273  HullLibrary hl;
274  HullDesc desc;
275 
277 
278  desc.mVcount = vcount;
279  desc.mVertices = vertices;
280  desc.mVertexStride = sizeof(double)*3;
281 
282  HullError hret = hl.CreateConvexHull(desc,hresult);
283 
284  if ( hret == QE_OK )
285  {
286 
287  double combineVolume = computeMeshVolume( hresult.mOutputVertices, hresult.mNumFaces, hresult.mIndices );
288  double sumVolume = a->mVolume + b->mVolume;
289 
290  double percent = (sumVolume*100) / combineVolume;
291 
292  if ( percent >= (100.0f-MERGE_PERCENT) )
293  {
294  ConvexResult cr(hresult.mNumOutputVertices, hresult.mOutputVertices, hresult.mNumFaces, hresult.mIndices);
295  ret = new CHull(cr);
296  }
297  }
298 
299 
301 
302  return ret;
303  }
304 
305  bool combineHulls(void)
306  {
307 
308  bool combine = false;
309 
310  sortChulls(mChulls); // sort the convex hulls, largest volume to least...
311 
312  CHullVector output; // the output hulls...
313 
314 
315  CHullVector::iterator i;
316 
317  for (i=mChulls.begin(); i!=mChulls.end() && !combine; ++i)
318  {
319  CHull *cr = (*i);
320 
321  CHullVector::iterator j;
322  for (j=mChulls.begin(); j!=mChulls.end(); ++j)
323  {
324  CHull *match = (*j);
325 
326  if ( cr != match ) // don't try to merge a hull with itself, that be stoopid
327  {
328 
329  CHull *merge = canMerge(cr,match); // if we can merge these two....
330 
331  if ( merge )
332  {
333 
334  output.push_back(merge);
335 
336 
337  ++i;
338  while ( i != mChulls.end() )
339  {
340  CHull *cr = (*i);
341  if ( cr != match )
342  {
343  output.push_back(cr);
344  }
345  i++;
346  }
347 
348  delete cr;
349  delete match;
350  combine = true;
351  break;
352  }
353  }
354  }
355 
356  if ( combine )
357  {
358  break;
359  }
360  else
361  {
362  output.push_back(cr);
363  }
364 
365  }
366 
367  if ( combine )
368  {
369  mChulls.clear();
370  mChulls = output;
371  output.clear();
372  }
373 
374 
375  return combine;
376  }
377 
378  unsigned int process(const DecompDesc &desc)
379  {
380 
381  unsigned int ret = 0;
382 
383  MAXDEPTH = desc.mDepth;
384  CONCAVE_PERCENT = desc.mCpercent;
385  MERGE_PERCENT = desc.mPpercent;
386 
387 
388  doConvexDecomposition(desc.mVcount, desc.mVertices, desc.mTcount, desc.mIndices,this,0,0);
389 
390 
391  while ( combineHulls() ); // keep combinging hulls until I can't combine any more...
392 
393  CHullVector::iterator i;
394  for (i=mChulls.begin(); i!=mChulls.end(); ++i)
395  {
396  CHull *cr = (*i);
397 
398  // before we hand it back to the application, we need to regenerate the hull based on the
399  // limits given by the user.
400 
401  const ConvexResult &c = *cr->mResult; // the high resolution hull...
402 
403  HullResult result;
404  HullLibrary hl;
405  HullDesc hdesc;
406 
407  hdesc.SetHullFlag(QF_TRIANGLES);
408 
409  hdesc.mVcount = c.mHullVcount;
410  hdesc.mVertices = c.mHullVertices;
411  hdesc.mVertexStride = sizeof(double)*3;
412  hdesc.mMaxVertices = desc.mMaxVertices; // maximum number of vertices allowed in the output
413 
414  if ( desc.mSkinWidth > 0 )
415  {
416  hdesc.mSkinWidth = desc.mSkinWidth;
417  hdesc.SetHullFlag(QF_SKIN_WIDTH); // do skin width computation.
418  }
419 
420  HullError ret = hl.CreateConvexHull(hdesc,result);
421 
422  if ( ret == QE_OK )
423  {
424  ConvexResult r(result.mNumOutputVertices, result.mOutputVertices, result.mNumFaces, result.mIndices);
425 
426  r.mHullVolume = computeMeshVolume( result.mOutputVertices, result.mNumFaces, result.mIndices ); // the volume of the hull.
427 
428  mCallback->ConvexDecompResult(r);
429  }
430 
431 
432  delete cr;
433  }
434 
435  ret = mChulls.size();
436 
437  mChulls.clear();
438 
439  return ret;
440  }
441 
442 
443  virtual void ConvexDebugTri(const double *p1,const double *p2,const double *p3,unsigned int color)
444  {
445  mCallback->ConvexDebugTri(p1,p2,p3,color);
446  }
447 
448  virtual void ConvexDebugOBB(const double *sides, const double *matrix,unsigned int color)
449  {
450  mCallback->ConvexDebugOBB(sides,matrix,color);
451  }
452  virtual void ConvexDebugPoint(const double *p,double dist,unsigned int color)
453  {
454  mCallback->ConvexDebugPoint(p,dist,color);
455  }
456 
457  virtual void ConvexDebugBound(const double *bmin,const double *bmax,unsigned int color)
458  {
459  mCallback->ConvexDebugBound(bmin,bmax,color);
460  }
461 
462  virtual void ConvexDecompResult(ConvexResult &result)
463  {
464  CHull *ch = new CHull(result);
465  mChulls.push_back(ch);
466  }
467 
468  void sortChulls(CHullVector &hulls)
469  {
470  std::sort( hulls.begin(), hulls.end(), CHullSort() );
471  }
472 
473 #define EPSILON 0.001f
474 
475  bool isEdge(const Vector3d<double> &p,const double *plane)
476  {
477  bool ret = false;
478 
479  double dist = fabs(fm_distToPlane(plane,p.Ptr()));
480 
481  if ( dist < EPSILON )
482  {
483  ret = true;
484  }
485 
486 
487  return ret;
488  }
489 
490  void addEdge(const Vector3d<double> &p1,const Vector3d<double> &p2,EdgeVector &edges,VertexLookup split,const double *plane)
491  {
492  if ( isEdge(p1,plane) && isEdge(p2,plane) )
493  {
494  unsigned int i1 = Vl_getIndex(split,p1.Ptr());
495  unsigned int i2 = Vl_getIndex(split,p2.Ptr());
496 
497  bool found = false;
498 
499  for (unsigned int i=0; i<edges.size(); i++)
500  {
501  Edge &e = edges[i];
502  if ( e.mE1 == i1 && e.mE2 == i2 )
503  {
504  found = true;
505  break;
506  }
507  if ( e.mE1 == i2 && e.mE2 == i1 )
508  {
509  found = true;
510  break;
511  }
512  }
513  if ( !found )
514  {
515  Edge e(i1,i2);
516  edges.push_back(e);
517  }
518  }
519  }
520 
522  UintVector &list,
523  const Vector3d<double> &p1,
524  const Vector3d<double> &p2,
525  const Vector3d<double> &p3,
526  EdgeVector &edges,
527  VertexLookup split,
528  const double *plane)
529  {
530  bool ret = false;
531 
532  unsigned int i1 = Vl_getIndex(vl, p1.Ptr() );
533  unsigned int i2 = Vl_getIndex(vl, p2.Ptr() );
534  unsigned int i3 = Vl_getIndex(vl, p3.Ptr() );
535 
536  // do *not* process degenerate triangles!
537 
538  if ( i1 != i2 && i1 != i3 && i2 != i3 )
539  {
540 
541  list.push_back(i1);
542  list.push_back(i2);
543  list.push_back(i3);
544 #if CLOSE_FACE
545  addEdge(p1,p2,edges,split,plane);
546  addEdge(p2,p3,edges,split,plane);
547  addEdge(p3,p1,edges,split,plane);
548 #endif
549  ret = true;
550  }
551  return ret;
552  }
553 
554  void saveEdges(VertexLookup vl,const EdgeVector &edges,bool front)
555  {
556  char scratch[512];
557  if ( front )
558  {
559  static int fcount=1;
560  sprintf(scratch,"CD_Front%d.obj", fcount++);
561  }
562  else
563  {
564  static int bcount=1;
565  sprintf(scratch,"CD_Back%d.obj", bcount++);
566  }
567 
568  FILE *fph = fopen(scratch,"wb");
569  if (fph)
570  {
571  unsigned int vcount = Vl_getVcount(vl);
572  const double *vertices = Vl_getVertices(vl);
573  fprintf(fph,"v 10 10 0\r\n");
574  for (unsigned int i=0; i<vcount; i++)
575  {
576  fprintf(fph,"v %0.9f %0.9f %0.9f\r\n", vertices[0], vertices[1], vertices[2] );
577  vertices+=3;
578  }
579  for (unsigned int i=0; i<edges.size(); i++)
580  {
581  const Edge &e = edges[i];
582  fprintf(fph,"f %d %d %d\r\n", e.mE1+2, 1, e.mE2+2);
583  }
584  fclose(fph);
585  }
586 
587  }
588 
589  void saveObj(VertexLookup vl,const UintVector &indices,bool front)
590  {
591  char scratch[512];
592  if ( front )
593  {
594  static int fcount=1;
595  sprintf(scratch,"CD_Front%d.obj", fcount++);
596  }
597  else
598  {
599  static int bcount=1;
600  sprintf(scratch,"CD_Back%d.obj", bcount++);
601  }
602 
603  FILE *fph = fopen(scratch,"wb");
604  if (fph)
605  {
606  unsigned int vcount = Vl_getVcount(vl);
607  const double *vertices = Vl_getVertices(vl);
608  for (unsigned int i=0; i<vcount; i++)
609  {
610  fprintf(fph,"v %0.9f %0.9f %0.9f\r\n", vertices[0], vertices[1], vertices[2] );
611  vertices+=3;
612  }
613  for (unsigned int i=0; i<indices.size()/3; i++)
614  {
615  unsigned int i1 = indices[i*3+0];
616  unsigned int i2 = indices[i*3+1];
617  unsigned int i3 = indices[i*3+2];
618  fprintf(fph,"f %d %d %d\r\n", i1+1, i2+1, i3+1);
619  }
620  fclose(fph);
621  }
622 
623  };
624 
625  void doConvexDecomposition(unsigned int vcount,
626  const double *vertices,
627  unsigned int tcount,
628  const unsigned int *indices,
629  ConvexDecompInterface *callback,
630  double masterVolume,
631  unsigned int depth)
632 
633  {
634 
635  double plane[4];
636 
637  bool split = false;
638 
639 
640  if ( depth < MAXDEPTH )
641  {
642  if ( CONCAVE_PERCENT >= 0 )
643  {
644  double volume;
645 
646  double c = computeConcavity( vcount, vertices, tcount, indices, callback, plane, volume );
647 
648  if ( depth == 0 )
649  {
650  masterVolume = volume;
651  }
652 
653  double percent = (c*100.0f)/masterVolume;
654 
655  if ( percent > CONCAVE_PERCENT ) // if great than 5% of the total volume is concave, go ahead and keep splitting.
656  {
657  split = true;
658  }
659  }
660  else
661  {
662  split = computeSplitPlane(vcount,vertices,tcount,indices,callback,plane);
663  }
664 
665  }
666 
667  if ( depth >= MAXDEPTH || !split )
668  {
669 
670  HullResult result;
671  HullLibrary hl;
672  HullDesc desc;
673 
675 
676  desc.mVcount = vcount;
677  desc.mVertices = vertices;
678  desc.mVertexStride = sizeof(double)*3;
679 
680  HullError ret = hl.CreateConvexHull(desc,result);
681 
682  if ( ret == QE_OK )
683  {
684 
685  ConvexResult r(result.mNumOutputVertices, result.mOutputVertices, result.mNumFaces, result.mIndices);
686 
687 
688  callback->ConvexDecompResult(r);
689  }
690 
691 
692  return;
693  }
694 
695  UintVector ifront;
696  UintVector iback;
697 
698  EdgeVector frontEdges;
699  EdgeVector backEdges;
700 
703 
704  VertexLookup splitFront = Vl_createVertexLookup();
705  VertexLookup splitBack = Vl_createVertexLookup();
706 
707 
708 
709  if ( 1 )
710  {
711 
712  // ok..now we are going to 'split' all of the input triangles against this plane!
713 
714  const unsigned int *source = indices;
715 
716  for (unsigned int i=0; i<tcount; i++)
717  {
718  unsigned int i1 = *source++;
719  unsigned int i2 = *source++;
720  unsigned int i3 = *source++;
721 
722  FaceTri t(vertices, i1, i2, i3 );
723 
724  Vector3d<double> front[4];
725  Vector3d<double> back[4];
726 
727  unsigned int fcount=0;
728  unsigned int bcount=0;
729 
730  PlaneTriResult result;
731 
732  result = planeTriIntersection(plane,t.mP1.Ptr(),sizeof(Vector3d<double>),0.00001f,front[0].Ptr(),fcount,back[0].Ptr(),bcount );
733 
734  if( fcount > 4 || bcount > 4 )
735  {
736  result = planeTriIntersection(plane,t.mP1.Ptr(),sizeof(Vector3d<double>),0.00001f,front[0].Ptr(),fcount,back[0].Ptr(),bcount );
737  }
738 
739  switch ( result )
740  {
741  case PTR_FRONT:
742 
743  assert( fcount == 3 );
744 
745  #if MAKE_MESH
746  addTri( vfront, ifront, front[0], front[1], front[2], frontEdges, splitFront, plane );
747  #endif
748 
749  break;
750  case PTR_BACK:
751  assert( bcount == 3 );
752 
753  #if MAKE_MESH
754  addTri( vback, iback, back[0], back[1], back[2], backEdges, splitBack, plane );
755  #endif
756 
757  break;
758  case PTR_SPLIT:
759 
760  if ( fcount >= 3 && fcount <= 4)
761  if ( bcount >= 3 && bcount <= 4)
762  {
763  #if MAKE_MESH
764  addTri( vfront, ifront, front[0], front[1], front[2], frontEdges, splitFront, plane );
765  addTri( vback, iback, back[0], back[1], back[2], backEdges, splitBack, plane );
766 
767  if ( fcount == 4 )
768  {
769  addTri( vfront, ifront, front[0], front[2], front[3], frontEdges, splitFront, plane );
770  }
771 
772  if ( bcount == 4 )
773  {
774  addTri( vback, iback, back[0], back[2], back[3], backEdges, splitBack, plane );
775  }
776  #endif
777  }
778  break;
779  }
780  }
781 
782 
783 // saveEdges(vfront,frontEdges,true);
784 // saveEdges(vback,backEdges,false);
785 
786  // Triangulate the front surface...
787  if ( frontEdges.size() ) // extract polygons for the front
788  {
790 
791  bool ok = extractPolygon(frontEdges,polygon,splitFront);
792 
793  while ( ok )
794  {
795 
796  const double *vertices = Vl_getVertices(splitFront);
797  unsigned int pcount = polygon.size();
798  unsigned int maxTri = pcount*3;
799  double *tris = new double[maxTri*9];
800 
801  unsigned int tcount = triangulate3d(pcount,(const unsigned int *) &polygon[0], vertices, tris, maxTri, plane );
802 
803  if ( tcount )
804  {
805  // cool! now add these triangles to the frong..
806  const double *source = tris;
807  for (unsigned int i=0; i<tcount; i++)
808  {
809  unsigned int i1 = Vl_getIndex(vfront, &source[0] );
810  unsigned int i2 = Vl_getIndex(vfront, &source[3] );
811  unsigned int i3 = Vl_getIndex(vfront, &source[6] );
812 
813  ifront.push_back(i1);
814  ifront.push_back(i2);
815  ifront.push_back(i3);
816 
817  ifront.push_back(i3);
818  ifront.push_back(i2);
819  ifront.push_back(i1);
820 
821 
822  source+=9;
823  }
824  }
825  delete tris;
826  ok = extractPolygon(frontEdges,polygon,splitFront);
827  }
828  }
829 
830  // Triangulate the back surface...
831  if ( backEdges.size() ) // extract polygons for the front
832  {
834 
835  bool ok = extractPolygon(backEdges,polygon,splitBack);
836 
837  while ( ok )
838  {
839 
840  const double *vertices = Vl_getVertices(splitBack);
841  unsigned int pcount = polygon.size();
842  unsigned int maxTri = pcount*3;
843  double *tris = new double[maxTri*9];
844 
845  unsigned int tcount = triangulate3d(pcount,(const unsigned int *) &polygon[0], vertices, tris, maxTri, plane );
846 
847  if ( tcount )
848  {
849  // cool! now add these triangles to the frong..
850  const double *source = tris;
851  for (unsigned int i=0; i<tcount; i++)
852  {
853  unsigned int i1 = Vl_getIndex(vback, &source[0] );
854  unsigned int i2 = Vl_getIndex(vback, &source[3] );
855  unsigned int i3 = Vl_getIndex(vback, &source[6] );
856 
857  iback.push_back(i1);
858  iback.push_back(i2);
859  iback.push_back(i3);
860 
861  iback.push_back(i3);
862  iback.push_back(i2);
863  iback.push_back(i1);
864 
865 
866  source+=9;
867  }
868  }
869  delete tris;
870  ok = extractPolygon(backEdges,polygon,splitBack);
871  }
872  }
873 
874 #if CLOSE_FACE
875  saveObj(vfront,ifront,true);
876  saveObj(vback,iback,false);
877 #endif
878 
879  Vl_releaseVertexLookup(splitFront);
880  Vl_releaseVertexLookup(splitBack);
881 
882  unsigned int fsize = ifront.size()/3;
883  unsigned int bsize = iback.size()/3;
884 
885  // ok... here we recursively call
886  if ( ifront.size() )
887  {
888  unsigned int vcount = Vl_getVcount(vfront);
889  const double *vertices = Vl_getVertices(vfront);
890  unsigned int tcount = ifront.size()/3;
891 
892  doConvexDecomposition(vcount, vertices, tcount, &ifront[0], callback, masterVolume, depth+1);
893 
894  }
895 
896  ifront.clear();
897 
898  Vl_releaseVertexLookup(vfront);
899 
900  if ( iback.size() )
901  {
902  unsigned int vcount = Vl_getVcount(vback);
903  const double *vertices = Vl_getVertices(vback);
904  unsigned int tcount = iback.size()/3;
905 
906  doConvexDecomposition(vcount, vertices, tcount, &iback[0], callback, masterVolume, depth+1);
907 
908  }
909 
910  iback.clear();
911  Vl_releaseVertexLookup(vback);
912 
913  }
914  }
915 
916  int findFirstUnused(EdgeVector &edges) const
917  {
918  int ret = -1;
919 
920  for (int i=0; i<(int)edges.size(); i++)
921  {
922  if ( !edges[i].mUsed )
923  {
924  edges[i].mUsed = true;
925  ret = i;
926  printf("%d edges, found root at %d\r\n", (int)edges.size(), ret );
927  break;
928  }
929  }
930 
931  for (int i=0; i<(int)edges.size(); i++)
932  {
933  const char *used = "false";
934  if ( edges[i].mUsed ) used = "true";
935  printf("Edge%d : %d to %d used: %s\r\n", i, edges[i].mE1, edges[i].mE2, used );
936  }
937 
938 
939  return ret;
940  }
941 
942  int findEdge(EdgeVector &edges,unsigned int index) const
943  {
944  int ret = -1;
945 
946  for (int i=0; i<(int)edges.size(); i++)
947  {
948  if ( !edges[i].mUsed && edges[i].mE1 == index )
949  {
950  edges[i].mUsed = true;
951  printf("Found matching unused edge %d matching (%d)\r\n", i, index );
952  ret = i;
953  break;
954  }
955  }
956 
957  if ( ret == -1 )
958  {
959  printf("Failed to find a match for edge '%d'\r\n", index );
960  }
961 
962  return ret;
963  }
964 
965  int findNearestEdge(EdgeVector &edges,unsigned int index,VertexLookup verts) const
966  {
967  int ret = -1;
968 
969 
970  const double *vertices = Vl_getVertices(verts);
971  const double *pos = &vertices[index*3];
972  double closest = 0.2f*0.2f;
973 
974  for (int i=0; i<(int)edges.size(); i++)
975  {
976  Edge &e = edges[i];
977 
978  if ( !e.mUsed )
979  {
980  const double *dpos = &vertices[ e.mE1*3 ];
981  double dx = pos[0] - dpos[0];
982  double dy = pos[1] - dpos[1];
983  double dz = pos[2] - dpos[2];
984  double dist = dx*dx+dy*dy+dz*dz;
985  if ( dist < closest )
986  {
987  closest = dist;
988  ret = i;
989  }
990  }
991  }
992 
993  if ( ret == -1 )
994  {
995  printf("Failed to find a match for edge '%d'\r\n", index );
996  }
997  else
998  {
999  edges[ret].mUsed = true;
1000  }
1001 
1002  return ret;
1003  }
1004 
1005  bool extractPolygon(EdgeVector &edges,UintVector &polygon,VertexLookup split)
1006  {
1007  bool ret = false;
1008 
1009 
1010  polygon.clear();
1011 
1012  int root = findFirstUnused(edges);
1013 
1014  if ( root >= 0 )
1015  {
1016  Edge &e = edges[root];
1017  polygon.push_back(e.mE1);
1018  int link;
1019 
1020  do
1021  {
1022  link = findEdge(edges,e.mE2);
1023  if ( link < 0 )
1024  link = findNearestEdge(edges,e.mE2,split);
1025 
1026  if ( link >= 0 )
1027  {
1028  e = edges[link];
1029  polygon.push_back(e.mE1 );
1030  }
1031  } while ( link >= 0 );
1032 
1033 
1034  if ( polygon.size() >= 3 )
1035  {
1036  ret = true;
1037  }
1038 
1039  }
1040 
1041  return ret;
1042  }
1043 
1044 CHullVector mChulls;
1046 
1047 };
1048 
1049 unsigned int performConvexDecomposition(const DecompDesc &desc)
1050 {
1051  unsigned int ret = 0;
1052 
1053  if ( desc.mCallback )
1054  {
1055  ConvexBuilder cb(desc.mCallback);
1056 
1057  ret = cb.process(desc);
1058  }
1059 
1060  return ret;
1061 }
1062 
1063 
1064 
1065 };
void getMesh(const ConvexResult &cr, VertexLookup vc)
virtual void ConvexDebugBound(const double *bmin, const double *bmax, unsigned int color)
FaceTri(const double *vertices, unsigned int i1, unsigned int i2, unsigned int i3)
virtual void ConvexDecompResult(ConvexResult &result)
void SetHullFlag(HullFlag flag)
Definition: cd_hull.h:178
bool operator()(const CHull *a, const CHull *b) const
void Vl_releaseVertexLookup(VertexLookup vlook)
Definition: vlookup.cpp:314
unsigned int mVertexStride
Definition: cd_hull.h:191
static Array< Tri * > tris
Definition: cd_hull.cpp:2390
unsigned int process(const DecompDesc &desc)
bool extractPolygon(EdgeVector &edges, UintVector &polygon, VertexLookup split)
unsigned int performConvexDecomposition(const DecompDesc &desc)
void addEdge(const Vector3d< double > &p1, const Vector3d< double > &p2, EdgeVector &edges, VertexLookup split, const double *plane)
double computeConcavity(unsigned int vcount, const double *vertices, unsigned int tcount, const unsigned int *indices, ConvexDecompInterface *callback, double *plane, double &volume)
Definition: concavity.cpp:594
bool computeSplitPlane(unsigned int vcount, const double *vertices, unsigned int tcount, const unsigned int *indices, ConvexDecompInterface *callback, double *plane)
Definition: splitplane.cpp:204
bool isEdge(const Vector3d< double > &p, const double *plane)
int findNearestEdge(EdgeVector &edges, unsigned int index, VertexLookup verts) const
bool overlap(const CHull &h) const
double getBoundingRegion(unsigned int vcount, const double *points, unsigned int pstride, double *bmin, double *bmax)
Definition: bestfit.cpp:431
const double * Vl_getVertices(VertexLookup vlook)
Definition: vlookup.cpp:327
unsigned int Vl_getIndex(VertexLookup vlook, const double *pos)
Definition: vlookup.cpp:320
static double MERGE_PERCENT
std::vector< CHull *> CHullVector
int findEdge(EdgeVector &edges, unsigned int index) const
virtual void ConvexDebugOBB(const double *sides, const double *matrix, unsigned int color)
virtual void ConvexDebugTri(const double *p1, const double *p2, const double *p3, unsigned int color)
Edge(unsigned int i1, unsigned int i2)
ConvexBuilder(ConvexDecompInterface *callback)
bool addTri(VertexLookup vl, UintVector &list, const Vector3d< double > &p1, const Vector3d< double > &p2, const Vector3d< double > &p3, EdgeVector &edges, VertexLookup split, const double *plane)
CHull(const ConvexResult &result)
bool overlapAABB(const double *bmin1, const double *bmax1, const double *bmin2, const double *bmax2)
Definition: bestfit.cpp:469
HullError CreateConvexHull(const HullDesc &desc, HullResult &result)
Definition: cd_hull.cpp:2948
const double * mVertices
Definition: cd_hull.h:190
unsigned int mNumOutputVertices
Definition: cd_hull.h:78
const Type * Ptr() const
Definition: cd_vector.h:291
bool isDuplicate(unsigned int i1, unsigned int i2, unsigned int i3, unsigned int ci1, unsigned int ci2, unsigned int ci3)
PlaneTriResult planeTriIntersection(const double *_plane, const double *triangle, unsigned int tstride, double epsilon, double *front, unsigned int &fcount, double *back, unsigned int &bcount)
Definition: planetri.cpp:244
static unsigned int MAXDEPTH
double computeMeshVolume(const double *vertices, unsigned int tcount, const unsigned int *indices)
Definition: meshvolume.cpp:67
void doConvexDecomposition(unsigned int vcount, const double *vertices, unsigned int tcount, const unsigned int *indices, ConvexDecompInterface *callback, double masterVolume, unsigned int depth)
double fm_distToPlane(const double *plane, const double *p)
Definition: float_math.cpp:397
virtual void ConvexDecompResult(ConvexResult &result)=0
void saveObj(VertexLookup vl, const UintVector &indices, bool front)
CHull * canMerge(CHull *a, CHull *b)
unsigned int triangulate3d(unsigned int pcount, const double *vertices, double *triangles, unsigned int maxTri, const double *plane)
std::vector< Edge > EdgeVector
int findFirstUnused(EdgeVector &edges) const
virtual void ConvexDebugPoint(const double *p, double dist, unsigned int color)
static double CONCAVE_PERCENT
VertexLookup Vl_createVertexLookup(void)
Definition: vlookup.cpp:308
unsigned int Vl_getVcount(VertexLookup vlook)
Definition: vlookup.cpp:334
void * VertexLookup
Definition: vlookup.h:130
static const double EPSILON
ConvexDecompInterface * mCallback
void saveEdges(VertexLookup vl, const EdgeVector &edges, bool front)
std::vector< unsigned int > UintVector


convex_decomposition
Author(s): John W. Ratcliff
autogenerated on Mon Feb 28 2022 22:06:34