1 #include "float_math.h"
9 /*----------------------------------------------------------------------
10 Copyright (c) 2004 Open Dynamics Framework Group
14 Redistribution and use in source and binary forms, with or without modification, are permitted provided
15 that the following conditions are met:
17 Redistributions of source code must retain the above copyright notice, this list of conditions
18 and the following disclaimer.
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.
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.
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 -----------------------------------------------------------------------*/
36 // http://codesuppository.blogspot.com
38 // mailto: jratcliff@infiniplex.net
40 // http://www.amillionpixels.us
43 #include "concavity.h"
47 #include "meshvolume.h"
48 #include "cd_vector.h"
49 #include "splitplane.h"
50 #include "ConvexDecomposition.h"
54 #define CONCAVE_THRESH 0.05f
56 namespace ConvexDecomposition
59 unsigned int getDebugColor(void)
61 static unsigned int colors[8] =
77 if ( count == 8 ) count = 0;
79 assert( count >= 0 && count < 8 );
81 unsigned int color = colors[count];
90 Wpoint(const Vector3d &p,float w)
100 typedef std::vector< Wpoint > WpointVector;
103 static inline float DistToPt(const float *p,const float *plane)
108 float d = x*plane[0] + y*plane[1] + z*plane[2] + plane[3];
113 static void intersect(const float *p1,const float *p2,float *split,const float *plane)
116 float dp1 = DistToPt(p1,plane);
120 dir[0] = p2[0] - p1[0];
121 dir[1] = p2[1] - p1[1];
122 dir[2] = p2[2] - p1[2];
124 float dot1 = dir[0]*plane[0] + dir[1]*plane[1] + dir[2]*plane[2];
125 float dot2 = dp1 - plane[3];
127 float t = -(plane[3] + dot2 ) / dot1;
129 split[0] = (dir[0]*t)+p1[0];
130 split[1] = (dir[1]*t)+p1[1];
131 split[2] = (dir[2]*t)+p1[2];
140 CTri(const float *p1,const float *p2,const float *p3,unsigned int i1,unsigned int i2,unsigned int i3)
151 mPlaneD = mNormal.ComputePlane(mP1,mP2,mP3);
154 float Facing(const CTri &t)
156 float d = mNormal.Dot(t.mNormal);
160 // clip this line segment against this triangle.
161 bool clip(const Vector3d &start,Vector3d &end) const
165 bool hit = lineIntersectsTriangle(start.Ptr(), end.Ptr(), mP1.Ptr(), mP2.Ptr(), mP3.Ptr(), sect.Ptr() );
174 bool Concave(const Vector3d &p,float &distance,Vector3d &n) const
176 n.NearestPointInTriangle(p,mP1,mP2,mP3);
177 distance = p.Distance(n);
181 void addTri(unsigned int *indices,unsigned int i1,unsigned int i2,unsigned int i3,unsigned int &tcount) const
183 indices[tcount*3+0] = i1;
184 indices[tcount*3+1] = i2;
185 indices[tcount*3+2] = i3;
189 float getVolume(ConvexDecompInterface *callback) const
191 unsigned int indices[8*3];
194 unsigned int tcount = 0;
196 addTri(indices,0,1,2,tcount);
197 addTri(indices,3,4,5,tcount);
199 addTri(indices,0,3,4,tcount);
200 addTri(indices,0,4,1,tcount);
202 addTri(indices,1,4,5,tcount);
203 addTri(indices,1,5,2,tcount);
205 addTri(indices,0,3,5,tcount);
206 addTri(indices,0,5,2,tcount);
208 const float *vertices = mP1.Ptr();
212 unsigned int color = getDebugColor();
215 Vector3d d1 = mNear1;
216 Vector3d d2 = mNear2;
217 Vector3d d3 = mNear3;
219 callback->ConvexDebugPoint(mP1.Ptr(),0.01f,0x00FF00);
220 callback->ConvexDebugPoint(mP2.Ptr(),0.01f,0x00FF00);
221 callback->ConvexDebugPoint(mP3.Ptr(),0.01f,0x00FF00);
222 callback->ConvexDebugPoint(d1.Ptr(),0.01f,0xFF0000);
223 callback->ConvexDebugPoint(d2.Ptr(),0.01f,0xFF0000);
224 callback->ConvexDebugPoint(d3.Ptr(),0.01f,0xFF0000);
226 callback->ConvexDebugTri(mP1.Ptr(), d1.Ptr(), d1.Ptr(),0x00FF00);
227 callback->ConvexDebugTri(mP2.Ptr(), d2.Ptr(), d2.Ptr(),0x00FF00);
228 callback->ConvexDebugTri(mP3.Ptr(), d3.Ptr(), d3.Ptr(),0x00FF00);
231 for (unsigned int i=0; i<tcount; i++)
233 unsigned int i1 = indices[i*3+0];
234 unsigned int i2 = indices[i*3+1];
235 unsigned int i3 = indices[i*3+2];
237 const float *p1 = &vertices[ i1*3 ];
238 const float *p2 = &vertices[ i2*3 ];
239 const float *p3 = &vertices[ i3*3 ];
241 callback->ConvexDebugTri(p1,p2,p3,color);
247 float v = computeMeshVolume(mP1.Ptr(), tcount, indices );
253 float raySect(const Vector3d &p,const Vector3d &dir,Vector3d §) const
257 plane[0] = mNormal.x;
258 plane[1] = mNormal.y;
259 plane[2] = mNormal.z;
262 Vector3d dest = p+dir*100000;
264 intersect( p.Ptr(), dest.Ptr(), sect.Ptr(), plane );
266 return sect.Distance(p); // return the intersection distance.
270 float planeDistance(const Vector3d &p) const
274 plane[0] = mNormal.x;
275 plane[1] = mNormal.y;
276 plane[2] = mNormal.z;
279 return DistToPt( p.Ptr(), plane );
283 bool samePlane(const CTri &t) const
285 const float THRESH = 0.001f;
286 float dd = fabsf( t.mPlaneD - mPlaneD );
287 if ( dd > THRESH ) return false;
288 dd = fabsf( t.mNormal.x - mNormal.x );
289 if ( dd > THRESH ) return false;
290 dd = fabsf( t.mNormal.y - mNormal.y );
291 if ( dd > THRESH ) return false;
292 dd = fabsf( t.mNormal.z - mNormal.z );
293 if ( dd > THRESH ) return false;
297 bool hasIndex(unsigned int i) const
299 if ( i == mI1 || i == mI2 || i == mI3 ) return true;
303 bool sharesEdge(const CTri &t) const
306 unsigned int count = 0;
308 if ( t.hasIndex(mI1) ) count++;
309 if ( t.hasIndex(mI2) ) count++;
310 if ( t.hasIndex(mI3) ) count++;
312 if ( count >= 2 ) ret = true;
317 void debug(unsigned int color,ConvexDecompInterface *callback)
319 callback->ConvexDebugTri( mP1.Ptr(), mP2.Ptr(), mP3.Ptr(), color );
320 callback->ConvexDebugTri( mP1.Ptr(), mP1.Ptr(), mNear1.Ptr(), 0xFF0000 );
321 callback->ConvexDebugTri( mP2.Ptr(), mP2.Ptr(), mNear2.Ptr(), 0xFF0000 );
322 callback->ConvexDebugTri( mP2.Ptr(), mP3.Ptr(), mNear3.Ptr(), 0xFF0000 );
323 callback->ConvexDebugPoint( mNear1.Ptr(), 0.01f, 0xFF0000 );
324 callback->ConvexDebugPoint( mNear2.Ptr(), 0.01f, 0xFF0000 );
325 callback->ConvexDebugPoint( mNear3.Ptr(), 0.01f, 0xFF0000 );
330 float a = mConcavity*mP1.Area(mP2,mP3);
334 void addWeighted(WpointVector &list,ConvexDecompInterface *callback)
341 Vector3d d1 = mNear1 - mP1;
342 Vector3d d2 = mNear2 - mP2;
343 Vector3d d3 = mNear3 - mP3;
366 callback->ConvexDebugPoint(mP1.Ptr(),0.01f,0x00FF00);
367 callback->ConvexDebugPoint(mP2.Ptr(),0.01f,0x00FF00);
368 callback->ConvexDebugPoint(mP3.Ptr(),0.01f,0x00FF00);
369 callback->ConvexDebugPoint(d1.Ptr(),0.01f,0xFF0000);
370 callback->ConvexDebugPoint(d2.Ptr(),0.01f,0xFF0000);
371 callback->ConvexDebugPoint(d3.Ptr(),0.01f,0xFF0000);
373 callback->ConvexDebugTri(mP1.Ptr(), d1.Ptr(), d1.Ptr(),0x00FF00);
374 callback->ConvexDebugTri(mP2.Ptr(), d2.Ptr(), d2.Ptr(),0x00FF00);
375 callback->ConvexDebugTri(mP3.Ptr(), d3.Ptr(), d3.Ptr(),0x00FF00);
377 Vector3d np1 = mP1 + mNormal*0.05f;
378 Vector3d np2 = mP2 + mNormal*0.05f;
379 Vector3d np3 = mP3 + mNormal*0.05f;
381 callback->ConvexDebugTri(mP1.Ptr(), np1.Ptr(), np1.Ptr(), 0xFF00FF );
382 callback->ConvexDebugTri(mP2.Ptr(), np2.Ptr(), np2.Ptr(), 0xFF00FF );
383 callback->ConvexDebugTri(mP3.Ptr(), np3.Ptr(), np3.Ptr(), 0xFF00FF );
385 callback->ConvexDebugPoint( np1.Ptr(), 0.01F, 0XFF00FF );
386 callback->ConvexDebugPoint( np2.Ptr(), 0.01F, 0XFF00FF );
387 callback->ConvexDebugPoint( np3.Ptr(), 0.01F, 0XFF00FF );
410 int mProcessed; // already been added...
413 typedef std::vector< CTri > CTriVector;
415 bool featureMatch(CTri &m,const CTriVector &tris,ConvexDecompInterface *callback,const CTriVector &input_mesh)
420 float neardot = 0.707f;
424 //gLog->Display("*********** FEATURE MATCH *************\r\n");
425 //gLog->Display("Plane: %0.4f,%0.4f,%0.4f %0.4f\r\n", m.mNormal.x, m.mNormal.y, m.mNormal.z, m.mPlaneD );
426 //gLog->Display("*********************************************\r\n");
428 CTriVector::const_iterator i;
433 for (i=tris.begin(); i!=tris.end(); ++i)
435 const CTri &t = (*i);
438 //gLog->Display(" HullPlane: %0.4f,%0.4f,%0.4f %0.4f\r\n", t.mNormal.x, t.mNormal.y, t.mNormal.z, t.mPlaneD );
440 if ( t.samePlane(m) )
442 //gLog->Display("*** PLANE MATCH!!!\r\n");
447 float dot = t.mNormal.Dot(m.mNormal);
452 float d1 = t.planeDistance( m.mP1 );
453 float d2 = t.planeDistance( m.mP2 );
454 float d3 = t.planeDistance( m.mP3 );
456 if ( d1 > 0.001f || d2 > 0.001f || d3 > 0.001f ) // can't be near coplaner!
463 t.raySect( m.mP1, m.mNormal, m.mNear1 );
464 t.raySect( m.mP2, m.mNormal, m.mNear2 );
465 t.raySect( m.mP3, m.mNormal, m.mNear3 );
479 CTriVector::const_iterator i;
480 for (i=input_mesh.begin(); i!=input_mesh.end(); ++i)
482 const CTri &c = (*i);
483 if ( c.mI1 != m.mI1 && c.mI2 != m.mI2 && c.mI3 != m.mI3 )
485 c.clip( m.mP1, m.mNear1 );
486 c.clip( m.mP2, m.mNear2 );
487 c.clip( m.mP3, m.mNear3 );
492 //gLog->Display("*********************************************\r\n");
493 //gLog->Display(" HullPlaneNearest: %0.4f,%0.4f,%0.4f %0.4f\r\n", nearest.mNormal.x, nearest.mNormal.y, nearest.mNormal.z, nearest.mPlaneD );
495 m.mC1 = m.mP1.Distance( m.mNear1 );
496 m.mC2 = m.mP2.Distance( m.mNear2 );
497 m.mC3 = m.mP3.Distance( m.mNear3 );
499 m.mConcavity = m.mC1;
501 if ( m.mC2 > m.mConcavity ) m.mConcavity = m.mC2;
502 if ( m.mC3 > m.mConcavity ) m.mConcavity = m.mC3;
505 callback->ConvexDebugTri( m.mP1.Ptr(), m.mP2.Ptr(), m.mP3.Ptr(), 0x00FF00 );
506 callback->ConvexDebugTri( m.mNear1.Ptr(), m.mNear2.Ptr(), m.mNear3.Ptr(), 0xFF0000 );
508 callback->ConvexDebugTri( m.mP1.Ptr(), m.mP1.Ptr(), m.mNear1.Ptr(), 0xFFFF00 );
509 callback->ConvexDebugTri( m.mP2.Ptr(), m.mP2.Ptr(), m.mNear2.Ptr(), 0xFFFF00 );
510 callback->ConvexDebugTri( m.mP3.Ptr(), m.mP3.Ptr(), m.mNear3.Ptr(), 0xFFFF00 );
516 //gLog->Display("No match\r\n");
519 //gLog->Display("*********************************************\r\n");
523 bool isFeatureTri(CTri &t,CTriVector &flist,float fc,ConvexDecompInterface *callback,unsigned int color)
527 if ( t.mProcessed == 0 ) // if not already processed
530 float c = t.mConcavity / fc; // must be within 80% of the concavity of the parent.
534 // see if this triangle is a 'feature' triangle. Meaning it shares an
535 // edge with any existing feature triangle and is within roughly the same
536 // concavity of the parent.
539 CTriVector::iterator i;
540 for (i=flist.begin(); i!=flist.end(); ++i)
543 if ( ftri.sharesEdge(t) )
545 t.mProcessed = 2; // it is now part of a feature.
546 flist.push_back(t); // add it to the feature list.
547 // callback->ConvexDebugTri( t.mP1.Ptr(), t.mP2.Ptr(),t.mP3.Ptr(), color );
556 flist.push_back(t); // add it to the feature list.
557 // callback->ConvexDebugTri( t.mP1.Ptr(), t.mP2.Ptr(),t.mP3.Ptr(), color );
563 t.mProcessed = 1; // eliminated for this feature, but might be valid for the next one..
570 float computeConcavity(unsigned int vcount,
571 const float *vertices,
573 const unsigned int *indices,
574 ConvexDecompInterface *callback,
575 float *plane, // plane equation to split on
587 desc.mMaxFaces = 256;
588 desc.mMaxVertices = 256;
589 desc.SetHullFlag(QF_TRIANGLES);
592 desc.mVcount = vcount;
593 desc.mVertices = vertices;
594 desc.mVertexStride = sizeof(float)*3;
596 HullError ret = hl.CreateConvexHull(desc,result);
604 float dx = bmax[0] - bmin[0];
605 float dy = bmax[1] - bmin[1];
606 float dz = bmax[2] - bmin[2];
610 center.x = bmin[0] + dx*0.5f;
611 center.y = bmin[1] + dy*0.5f;
612 center.z = bmin[2] + dz*0.5f;
615 volume = computeMeshVolume2( result.mOutputVertices, result.mNumFaces, result.mIndices );
618 // ok..now..for each triangle on the original mesh..
619 // we extrude the points to the nearest point on the hull.
620 const unsigned int *source = result.mIndices;
624 for (unsigned int i=0; i<result.mNumFaces; i++)
626 unsigned int i1 = *source++;
627 unsigned int i2 = *source++;
628 unsigned int i3 = *source++;
630 const float *p1 = &result.mOutputVertices[i1*3];
631 const float *p2 = &result.mOutputVertices[i2*3];
632 const float *p3 = &result.mOutputVertices[i3*3];
634 // callback->ConvexDebugTri(p1,p2,p3,0xFFFFFF);
636 CTri t(p1,p2,p3,i1,i2,i3); //
640 // we have not pre-computed the plane equation for each triangle in the convex hull..
642 float totalVolume = 0;
644 CTriVector ftris; // 'feature' triangles.
646 const unsigned int *src = indices;
654 CTriVector input_mesh;
657 const unsigned int *src = indices;
658 for (unsigned int i=0; i<tcount; i++)
661 unsigned int i1 = *src++;
662 unsigned int i2 = *src++;
663 unsigned int i3 = *src++;
665 const float *p1 = &vertices[i1*3];
666 const float *p2 = &vertices[i2*3];
667 const float *p3 = &vertices[i3*3];
669 CTri t(p1,p2,p3,i1,i2,i3);
670 input_mesh.push_back(t);
676 for (unsigned int i=0; i<tcount; i++)
679 unsigned int i1 = *src++;
680 unsigned int i2 = *src++;
681 unsigned int i3 = *src++;
683 const float *p1 = &vertices[i1*3];
684 const float *p2 = &vertices[i2*3];
685 const float *p3 = &vertices[i3*3];
687 CTri t(p1,p2,p3,i1,i2,i3);
689 featureMatch(t, tris, callback, input_mesh );
691 if ( t.mConcavity > CONCAVE_THRESH )
694 if ( t.mConcavity > maxc )
700 float v = t.getVolume(0);
708 if ( ftris.size() && 0 )
711 // ok..now we extract the triangles which form the maximum concavity.
712 CTriVector major_feature;
715 while ( maxc > CONCAVE_THRESH )
718 unsigned int color = getDebugColor(); //
729 CTriVector::iterator i;
730 for (i=ftris.begin(); i!=ftris.end(); ++i)
733 if ( isFeatureTri(t,flist,maxc,callback,color) )
742 if ( totalarea > maxarea )
744 major_feature = flist;
750 for (unsigned int i=0; i<ftris.size(); i++)
753 if ( t.mProcessed != 2 )
756 if ( t.mConcavity > maxc )
764 unsigned int color = getDebugColor();
767 for (unsigned int i=0; i<major_feature.size(); ++i)
769 major_feature[i].addWeighted(list,callback);
770 major_feature[i].debug(color,callback);
773 getBestFitPlane( list.size(), &list[0].mPoint.x, sizeof(Wpoint), &list[0].mWeight, sizeof(Wpoint), plane );
775 computeSplitPlane( vcount, vertices, tcount, indices, callback, plane );
781 computeSplitPlane( vcount, vertices, tcount, indices, callback, plane );
787 hl.ReleaseResult(result);