CHull * ConvexBuilder::canMerge(CHull *a,CHull *b)
{
if ( !a->overlap(*b) ) return 0; // if their AABB's (with a little slop) don't overlap, then return.
CHull *ret = 0;
// ok..we are going to combine both meshes into a single mesh
// and then we are going to compute the concavity...
VertexLookup vc = Vl_createVertexLookup();
UintVector indices;
getMesh( *a->mResult, vc, indices );
getMesh( *b->mResult, vc, indices );
unsigned int vcount = Vl_getVcount(vc);
const float *vertices = Vl_getVertices(vc);
unsigned int tcount = indices.size()/3;
//don't do anything if hull is empty
if (!tcount)
{
Vl_releaseVertexLookup (vc);
return 0;
}
HullResult hresult;
HullLibrary hl;
HullDesc desc;
desc.SetHullFlag(QF_TRIANGLES);
desc.mVcount = vcount;
desc.mVertices = vertices;
desc.mVertexStride = sizeof(float)*3;
HullError hret = hl.CreateConvexHull(desc,hresult);
if ( hret == QE_OK )
{
float combineVolume = computeMeshVolume( hresult.mOutputVertices, hresult.mNumFaces, hresult.mIndices );
float sumVolume = a->mVolume + b->mVolume;
float percent = (sumVolume*100) / combineVolume;
if ( percent >= (100.0f-MERGE_PERCENT) )
{
ConvexResult cr(hresult.mNumOutputVertices, hresult.mOutputVertices, hresult.mNumFaces, hresult.mIndices);
ret = new CHull(cr);
}
}
Vl_releaseVertexLookup(vc);
return ret;
}
void ChConvexHullLibraryWrapper::ComputeHull(const std::vector<ChVector<> >& points,
geometry::ChTriangleMeshConnected& vshape) {
HullLibrary hl;
HullResult hresult;
HullDesc desc;
desc.SetHullFlag(QF_TRIANGLES);
btVector3* btpoints = new btVector3[points.size()];
for (unsigned int ip = 0; ip < points.size(); ++ip) {
btpoints[ip].setX((btScalar)points[ip].x());
btpoints[ip].setY((btScalar)points[ip].y());
btpoints[ip].setZ((btScalar)points[ip].z());
}
desc.mVcount = (unsigned int)points.size();
desc.mVertices = btpoints;
desc.mVertexStride = sizeof(btVector3);
HullError hret = hl.CreateConvexHull(desc, hresult);
if (hret == QE_OK) {
vshape.Clear();
vshape.getIndicesVertexes().resize(hresult.mNumFaces);
for (unsigned int it = 0; it < hresult.mNumFaces; ++it) {
vshape.getIndicesVertexes()[it] = ChVector<int>(
hresult.m_Indices[it * 3 + 0], hresult.m_Indices[it * 3 + 1], hresult.m_Indices[it * 3 + 2]);
}
vshape.getCoordsVertices().resize(hresult.mNumOutputVertices);
for (unsigned int iv = 0; iv < hresult.mNumOutputVertices; ++iv) {
vshape.getCoordsVertices()[iv] = ChVector<>(
hresult.m_OutputVertices[iv].x(), hresult.m_OutputVertices[iv].y(), hresult.m_OutputVertices[iv].z());
}
}
delete[] btpoints;
hl.ReleaseResult(hresult);
}
HullError HullLibrary::CreateConvexHull(const HullDesc &desc, // describes the input request
HullResult &result) // contains the resulst
{
HullError ret = QE_FAIL;
PHullResult hr;
unsigned int vcount = desc.mVcount;
if ( vcount < 8 ) vcount = 8;
btAlignedObjectArray<btVector3> vertexSource;
vertexSource.resize(static_cast<int>(vcount));
btVector3 scale;
unsigned int ovcount;
bool ok = CleanupVertices(desc.mVcount,desc.mVertices, desc.mVertexStride, ovcount, &vertexSource[0], desc.mNormalEpsilon, scale ); // normalize point cloud, remove duplicates!
if ( ok )
{
// if ( 1 ) // scale vertices back to their original size.
{
for (unsigned int i=0; i<ovcount; i++)
{
btVector3& v = vertexSource[static_cast<int>(i)];
v[0]*=scale[0];
v[1]*=scale[1];
v[2]*=scale[2];
}
}
ok = ComputeHull(ovcount,&vertexSource[0],hr,desc.mMaxVertices);
if ( ok )
{
// re-index triangle mesh so it refers to only used vertices, rebuild a new vertex table.
btAlignedObjectArray<btVector3> vertexScratch;
vertexScratch.resize(static_cast<int>(hr.mVcount));
BringOutYourDead(hr.mVertices,hr.mVcount, &vertexScratch[0], ovcount, &hr.m_Indices[0], hr.mIndexCount );
ret = QE_OK;
if ( desc.HasHullFlag(QF_TRIANGLES) ) // if he wants the results as triangle!
{
result.mPolygons = false;
result.mNumOutputVertices = ovcount;
result.m_OutputVertices.resize(static_cast<int>(ovcount));
result.mNumFaces = hr.mFaceCount;
result.mNumIndices = hr.mIndexCount;
result.m_Indices.resize(static_cast<int>(hr.mIndexCount));
memcpy(&result.m_OutputVertices[0], &vertexScratch[0], sizeof(btVector3)*ovcount );
if ( desc.HasHullFlag(QF_REVERSE_ORDER) )
{
const unsigned int *source = &hr.m_Indices[0];
unsigned int *dest = &result.m_Indices[0];
for (unsigned int i=0; i<hr.mFaceCount; i++)
{
dest[0] = source[2];
dest[1] = source[1];
dest[2] = source[0];
dest+=3;
source+=3;
}
}
else
{
memcpy(&result.m_Indices[0], &hr.m_Indices[0], sizeof(unsigned int)*hr.mIndexCount);
}
}
else
{
result.mPolygons = true;
result.mNumOutputVertices = ovcount;
result.m_OutputVertices.resize(static_cast<int>(ovcount));
result.mNumFaces = hr.mFaceCount;
result.mNumIndices = hr.mIndexCount+hr.mFaceCount;
result.m_Indices.resize(static_cast<int>(result.mNumIndices));
memcpy(&result.m_OutputVertices[0], &vertexScratch[0], sizeof(btVector3)*ovcount );
// if ( 1 )
{
const unsigned int *source = &hr.m_Indices[0];
unsigned int *dest = &result.m_Indices[0];
for (unsigned int i=0; i<hr.mFaceCount; i++)
{
dest[0] = 3;
if ( desc.HasHullFlag(QF_REVERSE_ORDER) )
{
dest[1] = source[2];
dest[2] = source[1];
dest[3] = source[0];
}
else
{
dest[1] = source[0];
dest[2] = source[1];
dest[3] = source[2];
}
dest+=4;
source+=3;
}
}
}
ReleaseHull(hr);
}
}
return ret;
}
void calcConvexDecomposition(unsigned int vcount,
const float *vertices,
unsigned int tcount,
const unsigned int *indices,
ConvexDecompInterface *callback,
float masterVolume,
unsigned int depth)
{
float plane[4];
bool split = false;
if ( depth < MAXDEPTH )
{
float volume;
float c = computeConcavity( vcount, vertices, tcount, indices, callback, plane, volume );
if ( depth == 0 )
{
masterVolume = volume;
}
float percent = (c*100.0f)/masterVolume;
if ( percent > CONCAVE_PERCENT ) // if great than 5% of the total volume is concave, go ahead and keep splitting.
{
split = true;
}
}
if ( depth >= MAXDEPTH || !split )
{
#if 1
HullResult result;
HullLibrary hl;
HullDesc desc;
desc.SetHullFlag(QF_TRIANGLES);
desc.mVcount = vcount;
desc.mVertices = vertices;
desc.mVertexStride = sizeof(float)*3;
HullError ret = hl.CreateConvexHull(desc,result);
if ( ret == QE_OK )
{
ConvexResult r(result.mNumOutputVertices, result.mOutputVertices, result.mNumFaces, result.mIndices);
callback->ConvexDecompResult(r);
}
#else
static unsigned int colors[8] =
{
0xFF0000,
0x00FF00,
0x0000FF,
0xFFFF00,
0x00FFFF,
0xFF00FF,
0xFFFFFF,
0xFF8040
};
static int count = 0;
count++;
if ( count == 8 ) count = 0;
assert( count >= 0 && count < 8 );
unsigned int color = colors[count];
const unsigned int *source = indices;
for (unsigned int i=0; i<tcount; i++)
{
unsigned int i1 = *source++;
unsigned int i2 = *source++;
unsigned int i3 = *source++;
FaceTri t(vertices, i1, i2, i3 );
callback->ConvexDebugTri( t.mP1.Ptr(), t.mP2.Ptr(), t.mP3.Ptr(), color );
}
#endif
return;
}
UintVector ifront;
UintVector iback;
VertexLookup vfront = Vl_createVertexLookup();
VertexLookup vback = Vl_createVertexLookup();
bool showmesh = false;
#if SHOW_MESH
showmesh = true;
#endif
if ( 0 )
{
showmesh = true;
for (float x=-1; x<1; x+=0.10f)
{
for (float y=0; y<1; y+=0.10f)
{
for (float z=-1; z<1; z+=0.04f)
{
float d = x*plane[0] + y*plane[1] + z*plane[2] + plane[3];
Vector3d p(x,y,z);
if ( d >= 0 )
callback->ConvexDebugPoint(p.Ptr(), 0.02f, 0x00FF00);
else
callback->ConvexDebugPoint(p.Ptr(), 0.02f, 0xFF0000);
}
}
}
}
if ( 1 )
{
// ok..now we are going to 'split' all of the input triangles against this plane!
const unsigned int *source = indices;
for (unsigned int i=0; i<tcount; i++)
{
unsigned int i1 = *source++;
unsigned int i2 = *source++;
unsigned int i3 = *source++;
FaceTri t(vertices, i1, i2, i3 );
Vector3d front[4];
Vector3d back[4];
unsigned int fcount=0;
unsigned int bcount=0;
PlaneTriResult result;
result = planeTriIntersection(plane,t.mP1.Ptr(),sizeof(Vector3d),0.00001f,front[0].Ptr(),fcount,back[0].Ptr(),bcount );
if( fcount > 4 || bcount > 4 )
{
result = planeTriIntersection(plane,t.mP1.Ptr(),sizeof(Vector3d),0.00001f,front[0].Ptr(),fcount,back[0].Ptr(),bcount );
}
switch ( result )
{
case PTR_FRONT:
assert( fcount == 3 );
if ( showmesh )
callback->ConvexDebugTri( front[0].Ptr(), front[1].Ptr(), front[2].Ptr(), 0x00FF00 );
#if MAKE_MESH
addTri( vfront, ifront, front[0], front[1], front[2] );
#endif
break;
case PTR_BACK:
assert( bcount == 3 );
if ( showmesh )
callback->ConvexDebugTri( back[0].Ptr(), back[1].Ptr(), back[2].Ptr(), 0xFFFF00 );
#if MAKE_MESH
addTri( vback, iback, back[0], back[1], back[2] );
#endif
break;
case PTR_SPLIT:
assert( fcount >= 3 && fcount <= 4);
assert( bcount >= 3 && bcount <= 4);
#if MAKE_MESH
addTri( vfront, ifront, front[0], front[1], front[2] );
addTri( vback, iback, back[0], back[1], back[2] );
if ( fcount == 4 )
{
addTri( vfront, ifront, front[0], front[2], front[3] );
}
if ( bcount == 4 )
{
addTri( vback, iback, back[0], back[2], back[3] );
}
#endif
if ( showmesh )
{
callback->ConvexDebugTri( front[0].Ptr(), front[1].Ptr(), front[2].Ptr(), 0x00D000 );
callback->ConvexDebugTri( back[0].Ptr(), back[1].Ptr(), back[2].Ptr(), 0xD0D000 );
if ( fcount == 4 )
{
callback->ConvexDebugTri( front[0].Ptr(), front[2].Ptr(), front[3].Ptr(), 0x00D000 );
}
if ( bcount == 4 )
{
callback->ConvexDebugTri( back[0].Ptr(), back[2].Ptr(), back[3].Ptr(), 0xD0D000 );
}
}
break;
}
}
unsigned int fsize = ifront.size()/3;
unsigned int bsize = iback.size()/3;
// ok... here we recursively call
if ( ifront.size() )
{
unsigned int vcount = Vl_getVcount(vfront);
const float *vertices = Vl_getVertices(vfront);
unsigned int tcount = ifront.size()/3;
calcConvexDecomposition(vcount, vertices, tcount, &ifront[0], callback, masterVolume, depth+1);
}
ifront.clear();
Vl_releaseVertexLookup(vfront);
if ( iback.size() )
{
unsigned int vcount = Vl_getVcount(vback);
const float *vertices = Vl_getVertices(vback);
unsigned int tcount = iback.size()/3;
calcConvexDecomposition(vcount, vertices, tcount, &iback[0], callback, masterVolume, depth+1);
}
iback.clear();
Vl_releaseVertexLookup(vback);
}
}
unsigned int ConvexBuilder::process(const DecompDesc &desc)
{
unsigned int ret = 0;
MAXDEPTH = desc.mDepth;
CONCAVE_PERCENT = desc.mCpercent;
MERGE_PERCENT = desc.mPpercent;
calcConvexDecomposition(desc.mVcount, desc.mVertices, desc.mTcount, desc.mIndices,this,0,0);
while ( combineHulls() ); // keep combinging hulls until I can't combine any more...
int i;
for (i=0;i<mChulls.size();i++)
{
CHull *cr = mChulls[i];
// before we hand it back to the application, we need to regenerate the hull based on the
// limits given by the user.
const ConvexResult &c = *cr->mResult; // the high resolution hull...
HullResult result;
HullLibrary hl;
HullDesc hdesc;
hdesc.SetHullFlag(QF_TRIANGLES);
hdesc.mVcount = c.mHullVcount;
hdesc.mVertices = c.mHullVertices;
hdesc.mVertexStride = sizeof(float)*3;
hdesc.mMaxVertices = desc.mMaxVertices; // maximum number of vertices allowed in the output
if ( desc.mSkinWidth )
{
hdesc.mSkinWidth = desc.mSkinWidth;
hdesc.SetHullFlag(QF_SKIN_WIDTH); // do skin width computation.
}
HullError ret = hl.CreateConvexHull(hdesc,result);
if ( ret == QE_OK )
{
ConvexResult r(result.mNumOutputVertices, result.mOutputVertices, result.mNumFaces, result.mIndices);
r.mHullVolume = computeMeshVolume( result.mOutputVertices, result.mNumFaces, result.mIndices ); // the volume of the hull.
// compute the best fit OBB
computeBestFitOBB( result.mNumOutputVertices, result.mOutputVertices, sizeof(float)*3, r.mOBBSides, r.mOBBTransform );
r.mOBBVolume = r.mOBBSides[0] * r.mOBBSides[1] *r.mOBBSides[2]; // compute the OBB volume.
fm_getTranslation( r.mOBBTransform, r.mOBBCenter ); // get the translation component of the 4x4 matrix.
fm_matrixToQuat( r.mOBBTransform, r.mOBBOrientation ); // extract the orientation as a quaternion.
r.mSphereRadius = computeBoundingSphere( result.mNumOutputVertices, result.mOutputVertices, r.mSphereCenter );
r.mSphereVolume = fm_sphereVolume( r.mSphereRadius );
mCallback->ConvexDecompResult(r);
}
hl.ReleaseResult (result);
delete cr;
}
ret = mChulls.size();
mChulls.clear();
return ret;
}
float computeConcavity(unsigned int vcount,
const float *vertices,
unsigned int tcount,
const unsigned int *indices,
ConvexDecompInterface *callback,
float *plane, // plane equation to split on
float &volume)
{
float cret = 0;
volume = 1;
HullResult result;
HullLibrary hl;
HullDesc desc;
desc.mMaxFaces = 256;
desc.mMaxVertices = 256;
desc.SetHullFlag(QF_TRIANGLES);
desc.mVcount = vcount;
desc.mVertices = vertices;
desc.mVertexStride = sizeof(float)*3;
HullError ret = hl.CreateConvexHull(desc,result);
if ( ret == QE_OK )
{
#if 0
float bmin[3];
float bmax[3];
float dx = bmax[0] - bmin[0];
float dy = bmax[1] - bmin[1];
float dz = bmax[2] - bmin[2];
Vector3d center;
center.x = bmin[0] + dx*0.5f;
center.y = bmin[1] + dy*0.5f;
center.z = bmin[2] + dz*0.5f;
#endif
volume = computeMeshVolume2( result.mOutputVertices, result.mNumFaces, result.mIndices );
#if 1
// ok..now..for each triangle on the original mesh..
// we extrude the points to the nearest point on the hull.
const unsigned int *source = result.mIndices;
CTriVector tris;
for (unsigned int i=0; i<result.mNumFaces; i++)
{
unsigned int i1 = *source++;
unsigned int i2 = *source++;
unsigned int i3 = *source++;
const float *p1 = &result.mOutputVertices[i1*3];
const float *p2 = &result.mOutputVertices[i2*3];
const float *p3 = &result.mOutputVertices[i3*3];
// callback->ConvexDebugTri(p1,p2,p3,0xFFFFFF);
CTri t(p1,p2,p3,i1,i2,i3); //
tris.push_back(t);
}
// we have not pre-computed the plane equation for each triangle in the convex hull..
float totalVolume = 0;
CTriVector ftris; // 'feature' triangles.
const unsigned int *src = indices;
float maxc=0;
if ( 1 )
{
CTriVector input_mesh;
if ( 1 )
{
const unsigned int *src = indices;
for (unsigned int i=0; i<tcount; i++)
{
unsigned int i1 = *src++;
unsigned int i2 = *src++;
unsigned int i3 = *src++;
const float *p1 = &vertices[i1*3];
const float *p2 = &vertices[i2*3];
const float *p3 = &vertices[i3*3];
CTri t(p1,p2,p3,i1,i2,i3);
input_mesh.push_back(t);
}
}
CTri maxctri;
for (unsigned int i=0; i<tcount; i++)
{
unsigned int i1 = *src++;
unsigned int i2 = *src++;
unsigned int i3 = *src++;
const float *p1 = &vertices[i1*3];
const float *p2 = &vertices[i2*3];
const float *p3 = &vertices[i3*3];
CTri t(p1,p2,p3,i1,i2,i3);
featureMatch(t, tris, callback, input_mesh );
if ( t.mConcavity > CONCAVE_THRESH )
{
if ( t.mConcavity > maxc )
{
maxc = t.mConcavity;
maxctri = t;
}
float v = t.getVolume(0);
totalVolume+=v;
ftris.push_back(t);
}
}
}
if ( ftris.size() )
{
// ok..now we extract the triangles which form the maximum concavity.
CTriVector major_feature;
float maxarea = 0;
while ( maxc > CONCAVE_THRESH )
{
unsigned int color = getDebugColor(); //
CTriVector flist;
bool found;
float totalarea = 0;
do
{
found = false;
CTriVector::iterator i;
for (i=ftris.begin(); i!=ftris.end(); ++i)
{
CTri &t = (*i);
if ( isFeatureTri(t,flist,maxc,callback,color) )
{
found = true;
totalarea+=t.area();
}
}
} while ( found );
if ( totalarea > maxarea )
{
major_feature = flist;
maxarea = totalarea;
}
maxc = 0;
for (unsigned int i=0; i<ftris.size(); i++)
{
CTri &t = ftris[i];
if ( t.mProcessed != 2 )
{
t.mProcessed = 0;
if ( t.mConcavity > maxc )
{
maxc = t.mConcavity;
}
}
}
}
unsigned int color = getDebugColor();
WpointVector list;
for (unsigned int i=0; i<major_feature.size(); ++i)
{
major_feature[i].addWeighted(list,callback);
major_feature[i].debug(color,callback);
}
getBestFitPlane( list.size(), &list[0].mPoint.x, sizeof(Wpoint), &list[0].mWeight, sizeof(Wpoint), plane );
computeSplitPlane( vcount, vertices, tcount, indices, callback, plane );
}
else
{
computeSplitPlane( vcount, vertices, tcount, indices, callback, plane );
}
#endif
cret = totalVolume;
hl.ReleaseResult(result);
}
return cret;
}
//-----------------------------------------------------------------------
// T M e s h S h a p e
//-----------------------------------------------------------------------
TMeshShape::TMeshShape(IMesh* mesh, const matrix4& transform, bool isConvex) : TCollisionShape(),
m_baseCount(0),
m_hullCount(0)
{
TApplication* app = getApplication();
app->logMessage(LOG_INFO, "TMeshShape isConvex: %d", isConvex);
u32 vcount=0, tcount=0;
for(u32 i=0; i<mesh->getMeshBufferCount(); i++)
{
tcount += mesh->getMeshBuffer(i)->getIndexCount() / 3;
vcount += mesh->getMeshBuffer(i)->getVertexCount();
}
app->logMessage(LOG_INFO, " org vert count: %d", vcount);
app->logMessage(LOG_INFO, " org tri count: %d", tcount);
btQuaternion q(TMath::HALF_PI,0.f,0.f);
m_localTransform = transform;
m_localScale = transform.getScale();
m_triMesh = extractTriangles(mesh, true);
app->logMessage(LOG_INFO, " ext tri count: %d", m_triMesh->getNumTriangles());
if(isConvex)
{
if(1)
{ // using Bullet's btShapeHull class - faster, typically produces less verts/tris
btConvexShape* tmpConvexShape = new btConvexTriangleMeshShape(m_triMesh);
m_shape = tmpConvexShape;
btShapeHull* hull = new btShapeHull(tmpConvexShape);
btScalar margin = tmpConvexShape->getMargin();
hull->buildHull(margin);
tmpConvexShape->setUserPointer(hull);
app->logMessage(LOG_INFO, " hull vert count: %d", hull->numVertices());
app->logMessage(LOG_INFO, " hull tri count: %d", hull->numTriangles());
//btConvexHullShape* chShape = new btConvexHullShape((const btScalar *)hull->getVertexPointer(),hull->numVertices());
btConvexHullShape* chShape = new btConvexHullShape();
const btVector3* vp = hull->getVertexPointer();
const unsigned int* ip = hull->getIndexPointer();
for (int i=0;i<hull->numTriangles();i++)
{
chShape->addPoint(vp[ip[i*3]]);
chShape->addPoint(vp[ip[i*3+1]]);
chShape->addPoint(vp[ip[i*3+2]]);
}
m_shape = chShape;
delete hull;
delete tmpConvexShape;
}
else
{ // using Bullet's hull library directly
HullResult result;
HullLibrary hl;
HullDesc desc;
desc.mMaxFaces = 256;
desc.mMaxVertices = 256;
desc.SetHullFlag(QF_TRIANGLES);
PHY_ScalarType type, indicestype;
const unsigned char* indexbase;
int istride,numfaces;
m_triMesh->getLockedReadOnlyVertexIndexBase((const unsigned char**)&desc.mVertices, (int&)desc.mVcount, type,
(int&)desc.mVertexStride, &indexbase, istride, numfaces, indicestype);
HullError ret = hl.CreateConvexHull(desc,result);
if(ret == QE_OK)
{
app->logMessage(LOG_INFO, " hull vert count: %d", result.mNumOutputVertices);
app->logMessage(LOG_INFO, " hull tri count: %d", result.mNumFaces);
btConvexHullShape* chShape = new btConvexHullShape();
for (unsigned int i=0;i<result.mNumFaces;i++)
{
chShape->addPoint(result.m_OutputVertices[result.m_Indices[i*3]]);
chShape->addPoint(result.m_OutputVertices[result.m_Indices[i*3+1]]);
chShape->addPoint(result.m_OutputVertices[result.m_Indices[i*3+2]]);
}
m_shape = chShape;
}
else
{
m_shape = new btBvhTriangleMeshShape(m_triMesh,true,true);
}
hl.ReleaseResult(result);
}
}
else
{
//m_shape = _decomposeTriMesh();
m_shape = new btBvhTriangleMeshShape(m_triMesh,true,true);
}
btVector3 scale(m_localScale.X, m_localScale.Y, m_localScale.Z);
m_shape->setLocalScaling(scale);
}