00001 #include <stdio.h>
00002 #include <stdlib.h>
00003 #include <string.h>
00004 #include <assert.h>
00005
00062 #include <vector>
00063
00064 #include "concavity.h"
00065 #include "raytri.h"
00066 #include "bestfit.h"
00067 #include "cd_hull.h"
00068 #include "meshvolume.h"
00069 #include "cd_vector.h"
00070 #include "splitplane.h"
00071 #include "ConvexDecomposition.h"
00072
00073
00074 #define WSCALE 4
00075 #define CONCAVE_THRESH 0.05f
00076
00077 namespace ConvexDecomposition
00078 {
00079
00080 unsigned int getDebugColor(void)
00081 {
00082 static unsigned int colors[8] =
00083 {
00084 0xFF0000,
00085 0x00FF00,
00086 0x0000FF,
00087 0xFFFF00,
00088 0x00FFFF,
00089 0xFF00FF,
00090 0xFFFFFF,
00091 0xFF8040
00092 };
00093
00094 static int count = 0;
00095
00096 count++;
00097
00098 if ( count == 8 ) count = 0;
00099
00100 assert( count >= 0 && count < 8 );
00101
00102 unsigned int color = colors[count];
00103
00104 return color;
00105
00106 }
00107
00108 class Wpoint
00109 {
00110 public:
00111 Wpoint(const Vector3d<double> &p,double w)
00112 {
00113 mPoint = p;
00114 mWeight = w;
00115 }
00116
00117 Vector3d<double> mPoint;
00118 double mWeight;
00119 };
00120
00121 typedef std::vector< Wpoint > WpointVector;
00122
00123
00124 static inline double DistToPt(const double *p,const double *plane)
00125 {
00126 double x = p[0];
00127 double y = p[1];
00128 double z = p[2];
00129 double d = x*plane[0] + y*plane[1] + z*plane[2] + plane[3];
00130 return d;
00131 }
00132
00133
00134 static void intersect(const double *p1,const double *p2,double *split,const double *plane)
00135 {
00136
00137 double dp1 = DistToPt(p1,plane);
00138 double dp2 = DistToPt(p2,plane);
00139
00140 double dir[3];
00141
00142 dir[0] = p2[0] - p1[0];
00143 dir[1] = p2[1] - p1[1];
00144 dir[2] = p2[2] - p1[2];
00145
00146 double dot1 = dir[0]*plane[0] + dir[1]*plane[1] + dir[2]*plane[2];
00147 double dot2 = dp1 - plane[3];
00148
00149 double t = -(plane[3] + dot2 ) / dot1;
00150
00151 split[0] = (dir[0]*t)+p1[0];
00152 split[1] = (dir[1]*t)+p1[1];
00153 split[2] = (dir[2]*t)+p1[2];
00154
00155 }
00156
00157
00158 class CTri
00159 {
00160 public:
00161 CTri(void) { };
00162
00163 CTri(const double *p1,const double *p2,const double *p3,unsigned int i1,unsigned int i2,unsigned int i3)
00164 {
00165 mProcessed = 0;
00166 mI1 = i1;
00167 mI2 = i2;
00168 mI3 = i3;
00169
00170 mP1.Set(p1);
00171 mP2.Set(p2);
00172 mP3.Set(p3);
00173
00174 mPlaneD = mNormal.ComputePlane(mP1,mP2,mP3);
00175 }
00176
00177 double Facing(const CTri &t)
00178 {
00179 double d = mNormal.Dot(t.mNormal);
00180 return d;
00181 }
00182
00183
00184 bool clip(const Vector3d<double> &start,Vector3d<double> &end) const
00185 {
00186 Vector3d<double> sect;
00187
00188 bool hit = lineIntersectsTriangle(start.Ptr(), end.Ptr(), mP1.Ptr(), mP2.Ptr(), mP3.Ptr(), sect.Ptr() );
00189
00190 if ( hit )
00191 {
00192 end = sect;
00193 }
00194 return hit;
00195 }
00196
00197 bool Concave(const Vector3d<double> &p,double &distance,Vector3d<double> &n) const
00198 {
00199 n.NearestPointInTriangle(p,mP1,mP2,mP3);
00200 distance = p.Distance(n);
00201 return true;
00202 }
00203
00204 void addTri(unsigned int *indices,unsigned int i1,unsigned int i2,unsigned int i3,unsigned int &tcount) const
00205 {
00206 indices[tcount*3+0] = i1;
00207 indices[tcount*3+1] = i2;
00208 indices[tcount*3+2] = i3;
00209 tcount++;
00210 }
00211
00212 double getVolume(ConvexDecompInterface *callback) const
00213 {
00214 unsigned int indices[8*3];
00215
00216
00217 unsigned int tcount = 0;
00218
00219 addTri(indices,0,1,2,tcount);
00220 addTri(indices,3,4,5,tcount);
00221
00222 addTri(indices,0,3,4,tcount);
00223 addTri(indices,0,4,1,tcount);
00224
00225 addTri(indices,1,4,5,tcount);
00226 addTri(indices,1,5,2,tcount);
00227
00228 addTri(indices,0,3,5,tcount);
00229 addTri(indices,0,5,2,tcount);
00230
00231 const double *vertices = mP1.Ptr();
00232
00233 if ( callback )
00234 {
00235 unsigned int color = getDebugColor();
00236
00237 #if 0
00238 Vector3d<double> d1 = mNear1;
00239 Vector3d<double> d2 = mNear2;
00240 Vector3d<double> d3 = mNear3;
00241
00242 callback->ConvexDebugPoint(mP1.Ptr(),0.01f,0x00FF00);
00243 callback->ConvexDebugPoint(mP2.Ptr(),0.01f,0x00FF00);
00244 callback->ConvexDebugPoint(mP3.Ptr(),0.01f,0x00FF00);
00245 callback->ConvexDebugPoint(d1.Ptr(),0.01f,0xFF0000);
00246 callback->ConvexDebugPoint(d2.Ptr(),0.01f,0xFF0000);
00247 callback->ConvexDebugPoint(d3.Ptr(),0.01f,0xFF0000);
00248
00249 callback->ConvexDebugTri(mP1.Ptr(), d1.Ptr(), d1.Ptr(),0x00FF00);
00250 callback->ConvexDebugTri(mP2.Ptr(), d2.Ptr(), d2.Ptr(),0x00FF00);
00251 callback->ConvexDebugTri(mP3.Ptr(), d3.Ptr(), d3.Ptr(),0x00FF00);
00252
00253 #else
00254 for (unsigned int i=0; i<tcount; i++)
00255 {
00256 unsigned int i1 = indices[i*3+0];
00257 unsigned int i2 = indices[i*3+1];
00258 unsigned int i3 = indices[i*3+2];
00259
00260 const double *p1 = &vertices[ i1*3 ];
00261 const double *p2 = &vertices[ i2*3 ];
00262 const double *p3 = &vertices[ i3*3 ];
00263
00264 callback->ConvexDebugTri(p1,p2,p3,color);
00265
00266 }
00267 #endif
00268 }
00269
00270 double v = computeMeshVolume(mP1.Ptr(), tcount, indices );
00271
00272 return v;
00273
00274 }
00275
00276 double raySect(const Vector3d<double> &p,const Vector3d<double> &dir,Vector3d<double> §) const
00277 {
00278 double plane[4];
00279
00280 plane[0] = mNormal.x;
00281 plane[1] = mNormal.y;
00282 plane[2] = mNormal.z;
00283 plane[3] = mPlaneD;
00284
00285 Vector3d<double> dest = p+dir*100000;
00286
00287 intersect( p.Ptr(), dest.Ptr(), sect.Ptr(), plane );
00288
00289 return sect.Distance(p);
00290
00291 }
00292
00293 double planeDistance(const Vector3d<double> &p) const
00294 {
00295 double plane[4];
00296
00297 plane[0] = mNormal.x;
00298 plane[1] = mNormal.y;
00299 plane[2] = mNormal.z;
00300 plane[3] = mPlaneD;
00301
00302 return DistToPt( p.Ptr(), plane );
00303
00304 }
00305
00306 bool samePlane(const CTri &t) const
00307 {
00308 const double THRESH = 0.001f;
00309 double dd = fabs( t.mPlaneD - mPlaneD );
00310 if ( dd > THRESH ) return false;
00311 dd = fabs( t.mNormal.x - mNormal.x );
00312 if ( dd > THRESH ) return false;
00313 dd = fabs( t.mNormal.y - mNormal.y );
00314 if ( dd > THRESH ) return false;
00315 dd = fabs( t.mNormal.z - mNormal.z );
00316 if ( dd > THRESH ) return false;
00317 return true;
00318 }
00319
00320 bool hasIndex(unsigned int i) const
00321 {
00322 if ( i == mI1 || i == mI2 || i == mI3 ) return true;
00323 return false;
00324 }
00325
00326 bool sharesEdge(const CTri &t) const
00327 {
00328 bool ret = false;
00329 unsigned int count = 0;
00330
00331 if ( t.hasIndex(mI1) ) count++;
00332 if ( t.hasIndex(mI2) ) count++;
00333 if ( t.hasIndex(mI3) ) count++;
00334
00335 if ( count >= 2 ) ret = true;
00336
00337 return ret;
00338 }
00339
00340 void debug(unsigned int color,ConvexDecompInterface *callback)
00341 {
00342 callback->ConvexDebugTri( mP1.Ptr(), mP2.Ptr(), mP3.Ptr(), color );
00343 callback->ConvexDebugTri( mP1.Ptr(), mP1.Ptr(), mNear1.Ptr(), 0xFF0000 );
00344 callback->ConvexDebugTri( mP2.Ptr(), mP2.Ptr(), mNear2.Ptr(), 0xFF0000 );
00345 callback->ConvexDebugTri( mP2.Ptr(), mP3.Ptr(), mNear3.Ptr(), 0xFF0000 );
00346 callback->ConvexDebugPoint( mNear1.Ptr(), 0.01f, 0xFF0000 );
00347 callback->ConvexDebugPoint( mNear2.Ptr(), 0.01f, 0xFF0000 );
00348 callback->ConvexDebugPoint( mNear3.Ptr(), 0.01f, 0xFF0000 );
00349 }
00350
00351 double area(void)
00352 {
00353 double a = mConcavity*mP1.Area(mP2,mP3);
00354 return a;
00355 }
00356
00357 void addWeighted(WpointVector &list,ConvexDecompInterface *callback)
00358 {
00359
00360 Wpoint p1(mP1,mC1);
00361 Wpoint p2(mP2,mC2);
00362 Wpoint p3(mP3,mC3);
00363
00364 Vector3d<double> d1 = mNear1 - mP1;
00365 Vector3d<double> d2 = mNear2 - mP2;
00366 Vector3d<double> d3 = mNear3 - mP3;
00367
00368 d1*=WSCALE;
00369 d2*=WSCALE;
00370 d3*=WSCALE;
00371
00372 d1 = d1 + mP1;
00373 d2 = d2 + mP2;
00374 d3 = d3 + mP3;
00375
00376 Wpoint p4(d1,mC1);
00377 Wpoint p5(d2,mC2);
00378 Wpoint p6(d3,mC3);
00379
00380 list.push_back(p1);
00381 list.push_back(p2);
00382 list.push_back(p3);
00383
00384 list.push_back(p4);
00385 list.push_back(p5);
00386 list.push_back(p6);
00387
00388 #if 0
00389 callback->ConvexDebugPoint(mP1.Ptr(),0.01f,0x00FF00);
00390 callback->ConvexDebugPoint(mP2.Ptr(),0.01f,0x00FF00);
00391 callback->ConvexDebugPoint(mP3.Ptr(),0.01f,0x00FF00);
00392 callback->ConvexDebugPoint(d1.Ptr(),0.01f,0xFF0000);
00393 callback->ConvexDebugPoint(d2.Ptr(),0.01f,0xFF0000);
00394 callback->ConvexDebugPoint(d3.Ptr(),0.01f,0xFF0000);
00395
00396 callback->ConvexDebugTri(mP1.Ptr(), d1.Ptr(), d1.Ptr(),0x00FF00);
00397 callback->ConvexDebugTri(mP2.Ptr(), d2.Ptr(), d2.Ptr(),0x00FF00);
00398 callback->ConvexDebugTri(mP3.Ptr(), d3.Ptr(), d3.Ptr(),0x00FF00);
00399
00400 Vector3d<double> np1 = mP1 + mNormal*0.05f;
00401 Vector3d<double> np2 = mP2 + mNormal*0.05f;
00402 Vector3d<double> np3 = mP3 + mNormal*0.05f;
00403
00404 callback->ConvexDebugTri(mP1.Ptr(), np1.Ptr(), np1.Ptr(), 0xFF00FF );
00405 callback->ConvexDebugTri(mP2.Ptr(), np2.Ptr(), np2.Ptr(), 0xFF00FF );
00406 callback->ConvexDebugTri(mP3.Ptr(), np3.Ptr(), np3.Ptr(), 0xFF00FF );
00407
00408 callback->ConvexDebugPoint( np1.Ptr(), 0.01F, 0XFF00FF );
00409 callback->ConvexDebugPoint( np2.Ptr(), 0.01F, 0XFF00FF );
00410 callback->ConvexDebugPoint( np3.Ptr(), 0.01F, 0XFF00FF );
00411
00412 #endif
00413
00414
00415
00416 }
00417
00418 Vector3d<double> mP1;
00419 Vector3d<double> mP2;
00420 Vector3d<double> mP3;
00421 Vector3d<double> mNear1;
00422 Vector3d<double> mNear2;
00423 Vector3d<double> mNear3;
00424 Vector3d<double> mNormal;
00425 double mPlaneD;
00426 double mConcavity;
00427 double mC1;
00428 double mC2;
00429 double mC3;
00430 unsigned int mI1;
00431 unsigned int mI2;
00432 unsigned int mI3;
00433 int mProcessed;
00434 };
00435
00436 typedef std::vector< CTri > CTriVector;
00437
00438 bool featureMatch(CTri &m,const CTriVector &tris,ConvexDecompInterface *callback,const CTriVector &input_mesh)
00439 {
00440
00441 bool ret = false;
00442
00443 double neardot = 0.707f;
00444
00445 m.mConcavity = 0;
00446
00447
00448
00449
00450
00451 CTriVector::const_iterator i;
00452
00453 CTri nearest;
00454
00455 double near[3] = { 1e9, 1e9, 1e9 };
00456
00457 for (i=tris.begin(); i!=tris.end(); ++i)
00458 {
00459 const CTri &t = (*i);
00460
00461
00462
00463
00464 if ( t.samePlane(m) )
00465 {
00466
00467 ret = false;
00468 break;
00469 }
00470
00471 double dot = t.mNormal.Dot(m.mNormal);
00472
00473 if ( dot > neardot )
00474 {
00475
00476 double d1 = t.planeDistance( m.mP1 );
00477 double d2 = t.planeDistance( m.mP2 );
00478 double d3 = t.planeDistance( m.mP3 );
00479
00480 if ( d1 > 0.001f || d2 > 0.001f || d3 > 0.001f )
00481 {
00482
00483 neardot = dot;
00484
00485 Vector3d<double> n1,n2,n3;
00486
00487 t.raySect( m.mP1, m.mNormal, m.mNear1 );
00488 t.raySect( m.mP2, m.mNormal, m.mNear2 );
00489 t.raySect( m.mP3, m.mNormal, m.mNear3 );
00490
00491 nearest = t;
00492
00493 ret = true;
00494 }
00495
00496 }
00497 }
00498
00499 if ( ret )
00500 {
00501 if ( 0 )
00502 {
00503 CTriVector::const_iterator i;
00504 for (i=input_mesh.begin(); i!=input_mesh.end(); ++i)
00505 {
00506 const CTri &c = (*i);
00507 if ( c.mI1 != m.mI1 && c.mI2 != m.mI2 && c.mI3 != m.mI3 )
00508 {
00509 c.clip( m.mP1, m.mNear1 );
00510 c.clip( m.mP2, m.mNear2 );
00511 c.clip( m.mP3, m.mNear3 );
00512 }
00513 }
00514 }
00515
00516
00517
00518
00519 m.mC1 = m.mP1.Distance( m.mNear1 );
00520 m.mC2 = m.mP2.Distance( m.mNear2 );
00521 m.mC3 = m.mP3.Distance( m.mNear3 );
00522
00523 m.mConcavity = m.mC1;
00524
00525 if ( m.mC2 > m.mConcavity ) m.mConcavity = m.mC2;
00526 if ( m.mC3 > m.mConcavity ) m.mConcavity = m.mC3;
00527
00528 #if 0
00529 callback->ConvexDebugTri( m.mP1.Ptr(), m.mP2.Ptr(), m.mP3.Ptr(), 0x00FF00 );
00530 callback->ConvexDebugTri( m.mNear1.Ptr(), m.mNear2.Ptr(), m.mNear3.Ptr(), 0xFF0000 );
00531
00532 callback->ConvexDebugTri( m.mP1.Ptr(), m.mP1.Ptr(), m.mNear1.Ptr(), 0xFFFF00 );
00533 callback->ConvexDebugTri( m.mP2.Ptr(), m.mP2.Ptr(), m.mNear2.Ptr(), 0xFFFF00 );
00534 callback->ConvexDebugTri( m.mP3.Ptr(), m.mP3.Ptr(), m.mNear3.Ptr(), 0xFFFF00 );
00535 #endif
00536
00537 }
00538 else
00539 {
00540
00541 }
00542
00543
00544 return ret;
00545 }
00546
00547 bool isFeatureTri(CTri &t,CTriVector &flist,double fc,ConvexDecompInterface *callback,unsigned int color)
00548 {
00549 bool ret = false;
00550
00551 if ( t.mProcessed == 0 )
00552 {
00553
00554 double c = t.mConcavity / fc;
00555
00556 if ( c > 0.85f )
00557 {
00558
00559
00560
00561 if ( flist.size() )
00562 {
00563 CTriVector::iterator i;
00564 for (i=flist.begin(); i!=flist.end(); ++i)
00565 {
00566 CTri &ftri = (*i);
00567 if ( ftri.sharesEdge(t) )
00568 {
00569 t.mProcessed = 2;
00570 flist.push_back(t);
00571
00572 ret = true;
00573 break;
00574 }
00575 }
00576 }
00577 else
00578 {
00579 t.mProcessed = 2;
00580 flist.push_back(t);
00581
00582 ret = true;
00583 }
00584 }
00585 else
00586 {
00587 t.mProcessed = 1;
00588 }
00589
00590 }
00591 return ret;
00592 }
00593
00594 double computeConcavity(unsigned int vcount,
00595 const double *vertices,
00596 unsigned int tcount,
00597 const unsigned int *indices,
00598 ConvexDecompInterface *callback,
00599 double *plane,
00600 double &volume)
00601 {
00602
00603
00604 double cret = 0;
00605 volume = 1;
00606
00607 HullResult result;
00608 HullLibrary hl;
00609 HullDesc desc;
00610
00611 desc.mMaxVertices = 256;
00612 desc.SetHullFlag(QF_TRIANGLES);
00613
00614
00615 desc.mVcount = vcount;
00616 desc.mVertices = vertices;
00617 desc.mVertexStride = sizeof(double)*3;
00618
00619 HullError ret = hl.CreateConvexHull(desc,result);
00620
00621 if ( ret == QE_OK )
00622 {
00623
00624 double bmin[3];
00625 double bmax[3];
00626
00627 double diagonal = getBoundingRegion( result.mNumOutputVertices, result.mOutputVertices, sizeof(double)*3, bmin, bmax );
00628
00629 double dx = bmax[0] - bmin[0];
00630 double dy = bmax[1] - bmin[1];
00631 double dz = bmax[2] - bmin[2];
00632
00633 Vector3d<double> center;
00634
00635 center.x = bmin[0] + dx*0.5f;
00636 center.y = bmin[1] + dy*0.5f;
00637 center.z = bmin[2] + dz*0.5f;
00638
00639 double boundVolume = dx*dy*dz;
00640
00641 volume = computeMeshVolume2( result.mOutputVertices, result.mNumFaces, result.mIndices );
00642
00643 #if 1
00644
00645
00646 const unsigned int *source = result.mIndices;
00647
00648 CTriVector tris;
00649
00650 for (unsigned int i=0; i<result.mNumFaces; i++)
00651 {
00652 unsigned int i1 = *source++;
00653 unsigned int i2 = *source++;
00654 unsigned int i3 = *source++;
00655
00656 const double *p1 = &result.mOutputVertices[i1*3];
00657 const double *p2 = &result.mOutputVertices[i2*3];
00658 const double *p3 = &result.mOutputVertices[i3*3];
00659
00660
00661
00662 CTri t(p1,p2,p3,i1,i2,i3);
00663 tris.push_back(t);
00664 }
00665
00666
00667
00668 double totalVolume = 0;
00669
00670 CTriVector ftris;
00671
00672 const unsigned int *src = indices;
00673
00674
00675 double maxc=0;
00676
00677
00678 if ( 1 )
00679 {
00680 CTriVector input_mesh;
00681 if ( 1 )
00682 {
00683 const unsigned int *src = indices;
00684 for (unsigned int i=0; i<tcount; i++)
00685 {
00686
00687 unsigned int i1 = *src++;
00688 unsigned int i2 = *src++;
00689 unsigned int i3 = *src++;
00690
00691 const double *p1 = &vertices[i1*3];
00692 const double *p2 = &vertices[i2*3];
00693 const double *p3 = &vertices[i3*3];
00694
00695 CTri t(p1,p2,p3,i1,i2,i3);
00696 input_mesh.push_back(t);
00697 }
00698 }
00699
00700 CTri maxctri;
00701
00702 for (unsigned int i=0; i<tcount; i++)
00703 {
00704
00705 unsigned int i1 = *src++;
00706 unsigned int i2 = *src++;
00707 unsigned int i3 = *src++;
00708
00709 const double *p1 = &vertices[i1*3];
00710 const double *p2 = &vertices[i2*3];
00711 const double *p3 = &vertices[i3*3];
00712
00713 CTri t(p1,p2,p3,i1,i2,i3);
00714
00715 featureMatch(t, tris, callback, input_mesh );
00716
00717 if ( t.mConcavity > CONCAVE_THRESH )
00718 {
00719
00720 if ( t.mConcavity > maxc )
00721 {
00722 maxc = t.mConcavity;
00723 maxctri = t;
00724 }
00725
00726 double v = t.getVolume(0);
00727 totalVolume+=v;
00728 ftris.push_back(t);
00729 }
00730
00731 }
00732 }
00733
00734 if ( ftris.size() && 0 )
00735 {
00736
00737
00738 CTriVector major_feature;
00739 double maxarea = 0;
00740
00741 while ( maxc > CONCAVE_THRESH )
00742 {
00743
00744 unsigned int color = getDebugColor();
00745
00746 CTriVector flist;
00747
00748 bool found;
00749
00750 double totalarea = 0;
00751
00752 do
00753 {
00754 found = false;
00755 CTriVector::iterator i;
00756 for (i=ftris.begin(); i!=ftris.end(); ++i)
00757 {
00758 CTri &t = (*i);
00759 if ( isFeatureTri(t,flist,maxc,callback,color) )
00760 {
00761 found = true;
00762 totalarea+=t.area();
00763 }
00764 }
00765 } while ( found );
00766
00767
00768 if ( totalarea > maxarea )
00769 {
00770 major_feature = flist;
00771 maxarea = totalarea;
00772 }
00773
00774 maxc = 0;
00775
00776 for (unsigned int i=0; i<ftris.size(); i++)
00777 {
00778 CTri &t = ftris[i];
00779 if ( t.mProcessed != 2 )
00780 {
00781 t.mProcessed = 0;
00782 if ( t.mConcavity > maxc )
00783 {
00784 maxc = t.mConcavity;
00785 }
00786 }
00787 }
00788 }
00789
00790 unsigned int color = getDebugColor();
00791
00792 WpointVector list;
00793 for (unsigned int i=0; i<major_feature.size(); ++i)
00794 {
00795 major_feature[i].addWeighted(list,callback);
00796 major_feature[i].debug(color,callback);
00797 }
00798
00799 getBestFitPlane( list.size(), &list[0].mPoint.x, sizeof(Wpoint), &list[0].mWeight, sizeof(Wpoint), plane );
00800
00801 computeSplitPlane( vcount, vertices, tcount, indices, callback, plane );
00802
00803
00804 }
00805 else
00806 {
00807 computeSplitPlane( vcount, vertices, tcount, indices, callback, plane );
00808 }
00809 #endif
00810
00811 cret = totalVolume;
00812
00813 hl.ReleaseResult(result);
00814 }
00815
00816
00817 return cret;
00818 }
00819
00820
00821 };