98 Edge(
unsigned int i1,
unsigned int i2)
117 FaceTri(
const double *vertices,
unsigned int i1,
unsigned int i2,
unsigned int i3)
119 mP1.Set( &vertices[i1*3] );
120 mP2.Set( &vertices[i2*3] );
121 mP3.Set( &vertices[i3*3] );
143 double dx = mMax[0] - mMin[0];
144 double dy = mMax[1] - mMin[1];
145 double dz = mMax[2] - mMin[2];
199 mCallback = callback;
204 CHullVector::iterator i;
205 for (i=mChulls.begin(); i!=mChulls.end(); ++i)
213 unsigned int ci1,
unsigned int ci2,
unsigned int ci3)
215 unsigned int dcount = 0;
217 assert( i1 != i2 && i1 != i3 && i2 != i3 );
218 assert( ci1 != ci2 && ci1 != ci3 && ci2 != ci3 );
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++;
233 unsigned int i1 = *src++;
234 unsigned int i2 = *src++;
235 unsigned int i3 = *src++;
252 if ( !a->
overlap(*b) )
return 0;
290 double percent = (sumVolume*100) / combineVolume;
308 bool combine =
false;
315 CHullVector::iterator i;
317 for (i=mChulls.begin(); i!=mChulls.end() && !combine; ++i)
321 CHullVector::iterator j;
322 for (j=mChulls.begin(); j!=mChulls.end(); ++j)
329 CHull *merge = canMerge(cr,match);
334 output.push_back(merge);
338 while ( i != mChulls.end() )
343 output.push_back(cr);
362 output.push_back(cr);
381 unsigned int ret = 0;
391 while ( combineHulls() );
393 CHullVector::iterator i;
394 for (i=mChulls.begin(); i!=mChulls.end(); ++i)
428 mCallback->ConvexDecompResult(r);
435 ret = mChulls.size();
443 virtual void ConvexDebugTri(
const double *p1,
const double *p2,
const double *p3,
unsigned int color)
445 mCallback->ConvexDebugTri(p1,p2,p3,color);
448 virtual void ConvexDebugOBB(
const double *sides,
const double *matrix,
unsigned int color)
450 mCallback->ConvexDebugOBB(sides,matrix,color);
454 mCallback->ConvexDebugPoint(p,dist,color);
459 mCallback->ConvexDebugBound(bmin,bmax,color);
465 mChulls.push_back(ch);
470 std::sort( hulls.begin(), hulls.end(),
CHullSort() );
473 #define EPSILON 0.001f 492 if ( isEdge(p1,plane) && isEdge(p2,plane) )
499 for (
unsigned int i=0; i<edges.size(); i++)
502 if ( e.
mE1 == i1 && e.
mE2 == i2 )
507 if ( e.
mE1 == i2 && e.
mE2 == i1 )
538 if ( i1 != i2 && i1 != i3 && i2 != i3 )
545 addEdge(p1,p2,edges,split,plane);
546 addEdge(p2,p3,edges,split,plane);
547 addEdge(p3,p1,edges,split,plane);
560 sprintf(scratch,
"CD_Front%d.obj", fcount++);
565 sprintf(scratch,
"CD_Back%d.obj", bcount++);
568 FILE *fph = fopen(scratch,
"wb");
573 fprintf(fph,
"v 10 10 0\r\n");
574 for (
unsigned int i=0; i<vcount; i++)
576 fprintf(fph,
"v %0.9f %0.9f %0.9f\r\n", vertices[0], vertices[1], vertices[2] );
579 for (
unsigned int i=0; i<edges.size(); i++)
581 const Edge &e = edges[i];
582 fprintf(fph,
"f %d %d %d\r\n", e.
mE1+2, 1, e.
mE2+2);
595 sprintf(scratch,
"CD_Front%d.obj", fcount++);
600 sprintf(scratch,
"CD_Back%d.obj", bcount++);
603 FILE *fph = fopen(scratch,
"wb");
608 for (
unsigned int i=0; i<vcount; i++)
610 fprintf(fph,
"v %0.9f %0.9f %0.9f\r\n", vertices[0], vertices[1], vertices[2] );
613 for (
unsigned int i=0; i<indices.size()/3; i++)
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);
626 const double *vertices,
628 const unsigned int *indices,
646 double c =
computeConcavity( vcount, vertices, tcount, indices, callback, plane, volume );
650 masterVolume = volume;
653 double percent = (c*100.0f)/masterVolume;
698 EdgeVector frontEdges;
699 EdgeVector backEdges;
714 const unsigned int *source = indices;
716 for (
unsigned int i=0; i<tcount; i++)
718 unsigned int i1 = *source++;
719 unsigned int i2 = *source++;
720 unsigned int i3 = *source++;
722 FaceTri t(vertices, i1, i2, i3 );
727 unsigned int fcount=0;
728 unsigned int bcount=0;
734 if( fcount > 4 || bcount > 4 )
743 assert( fcount == 3 );
746 addTri( vfront, ifront, front[0], front[1], front[2], frontEdges, splitFront, plane );
751 assert( bcount == 3 );
754 addTri( vback, iback, back[0], back[1], back[2], backEdges, splitBack, plane );
760 if ( fcount >= 3 && fcount <= 4)
761 if ( bcount >= 3 && bcount <= 4)
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 );
769 addTri( vfront, ifront, front[0], front[2], front[3], frontEdges, splitFront, plane );
774 addTri( vback, iback, back[0], back[2], back[3], backEdges, splitBack, plane );
787 if ( frontEdges.size() )
791 bool ok = extractPolygon(frontEdges,polygon,splitFront);
797 unsigned int pcount = polygon.size();
798 unsigned int maxTri = pcount*3;
799 double *
tris =
new double[maxTri*9];
801 unsigned int tcount =
triangulate3d(pcount,(
const unsigned int *) &polygon[0], vertices, tris, maxTri, plane );
806 const double *source =
tris;
807 for (
unsigned int i=0; i<tcount; i++)
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] );
813 ifront.push_back(i1);
814 ifront.push_back(i2);
815 ifront.push_back(i3);
817 ifront.push_back(i3);
818 ifront.push_back(i2);
819 ifront.push_back(i1);
826 ok = extractPolygon(frontEdges,polygon,splitFront);
831 if ( backEdges.size() )
835 bool ok = extractPolygon(backEdges,polygon,splitBack);
841 unsigned int pcount = polygon.size();
842 unsigned int maxTri = pcount*3;
843 double *
tris =
new double[maxTri*9];
845 unsigned int tcount =
triangulate3d(pcount,(
const unsigned int *) &polygon[0], vertices, tris, maxTri, plane );
850 const double *source =
tris;
851 for (
unsigned int i=0; i<tcount; i++)
870 ok = extractPolygon(backEdges,polygon,splitBack);
875 saveObj(vfront,ifront,
true);
876 saveObj(vback,iback,
false);
882 unsigned int fsize = ifront.size()/3;
883 unsigned int bsize = iback.size()/3;
890 unsigned int tcount = ifront.size()/3;
892 doConvexDecomposition(vcount, vertices, tcount, &ifront[0], callback, masterVolume, depth+1);
904 unsigned int tcount = iback.size()/3;
906 doConvexDecomposition(vcount, vertices, tcount, &iback[0], callback, masterVolume, depth+1);
920 for (
int i=0; i<(int)edges.size(); i++)
922 if ( !edges[i].
mUsed )
924 edges[i].mUsed =
true;
926 printf(
"%d edges, found root at %d\r\n", (
int)edges.size(), ret );
931 for (
int i=0; i<(int)edges.size(); i++)
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 );
942 int findEdge(EdgeVector &edges,
unsigned int index)
const 946 for (
int i=0; i<(int)edges.size(); i++)
948 if ( !edges[i].
mUsed && edges[i].
mE1 == index )
950 edges[i].mUsed =
true;
951 printf(
"Found matching unused edge %d matching (%d)\r\n", i, index );
959 printf(
"Failed to find a match for edge '%d'\r\n", index );
971 const double *pos = &vertices[index*3];
972 double closest = 0.2f*0.2f;
974 for (
int i=0; i<(int)edges.size(); i++)
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 )
995 printf(
"Failed to find a match for edge '%d'\r\n", index );
999 edges[ret].mUsed =
true;
1012 int root = findFirstUnused(edges);
1016 Edge &e = edges[root];
1017 polygon.push_back(e.
mE1);
1022 link = findEdge(edges,e.
mE2);
1024 link = findNearestEdge(edges,e.
mE2,split);
1029 polygon.push_back(e.
mE1 );
1031 }
while ( link >= 0 );
1034 if ( polygon.size() >= 3 )
1051 unsigned int ret = 0;
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)
bool operator()(const CHull *a, const CHull *b) const
void Vl_releaseVertexLookup(VertexLookup vlook)
unsigned int mVertexStride
static Array< Tri * > tris
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)
bool computeSplitPlane(unsigned int vcount, const double *vertices, unsigned int tcount, const unsigned int *indices, ConvexDecompInterface *callback, double *plane)
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)
const double * Vl_getVertices(VertexLookup vlook)
unsigned int Vl_getIndex(VertexLookup vlook, const double *pos)
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)
HullError CreateConvexHull(const HullDesc &desc, HullResult &result)
unsigned int mNumOutputVertices
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)
static unsigned int MAXDEPTH
double computeMeshVolume(const double *vertices, unsigned int tcount, const unsigned int *indices)
Vector3d< double > mNormal
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)
virtual void ConvexDecompResult(ConvexResult &result)=0
void saveObj(VertexLookup vl, const UintVector &indices, bool front)
CHull * canMerge(CHull *a, CHull *b)
unsigned int * mHullIndices
unsigned int triangulate3d(unsigned int pcount, const double *vertices, double *triangles, unsigned int maxTri, const double *plane)
std::vector< Edge > EdgeVector
ConvexDecompInterface * mCallback
int findFirstUnused(EdgeVector &edges) const
virtual void ConvexDebugPoint(const double *p, double dist, unsigned int color)
static double CONCAVE_PERCENT
VertexLookup Vl_createVertexLookup(void)
unsigned int Vl_getVcount(VertexLookup vlook)
static const double EPSILON
ConvexDecompInterface * mCallback
unsigned int mMaxVertices
void saveEdges(VertexLookup vl, const EdgeVector &edges, bool front)
void sortChulls(CHullVector &hulls)
std::vector< unsigned int > UintVector
unsigned int mMaxVertices