HACDCluster CreateConvexHullShapeHACD(HACD::HACD& hacd, size_t cluster, const btVector3& localScaling)
{
DKASSERT_DEBUG(cluster < hacd.GetNClusters());
// create convex-hull per cluster.
size_t numPoints = hacd.GetNPointsCH(cluster);
size_t numTriangles = hacd.GetNTrianglesCH(cluster);
HACDPoint* points = new HACDPoint[numPoints];
HACDTriangle* triangles = new HACDTriangle[numTriangles];
hacd.GetCH(cluster, points, triangles);
btVector3 centroid(0, 0, 0);
#ifdef HACD_CLUSTER_CALCULATE_CENTROID
for (size_t i = 0; i < numPoints; ++i)
{
btVector3 vertex( points[i].X(), points[i].Y(), points[i].Z() );
vertex *= localScaling;
centroid += vertex;
}
centroid *= 1.0f / static_cast<float>( numPoints );
#endif
btAlignedObjectArray<btVector3> vertices;
vertices.reserve(numPoints);
for (size_t i = 0; i < numPoints; ++i)
{
btVector3 vertex( points[i].X(), points[i].Y(), points[i].Z() );
vertex *= localScaling;
vertex -= centroid;
vertices.push_back(vertex);
}
delete[] points;
delete[] triangles;
HACDCluster result;
result.shape = new btConvexHullShape(&(vertices[0].getX()), vertices.size());
result.shape->setMargin(0.01f);
result.centroid = centroid;
return result;
}
void ConvexDecompositionDemo::initPhysics(const char* filename)
{
gContactAddedCallback = &MyContactCallback;
setupEmptyDynamicsWorld();
getDynamicsWorld()->setDebugDrawer(&gDebugDrawer);
setTexturing(true);
setShadows(true);
setCameraDistance(26.f);
#ifndef NO_OBJ_TO_BULLET
ConvexDecomposition::WavefrontObj wo;
tcount = 0;
const char* prefix[]={"./","../","../../","../../../","../../../../", "ConvexDecompositionDemo/", "Demos/ConvexDecompositionDemo/",
"../Demos/ConvexDecompositionDemo/","../../Demos/ConvexDecompositionDemo/"};
int numPrefixes = sizeof(prefix)/sizeof(const char*);
char relativeFileName[1024];
for (int i=0;i<numPrefixes;i++)
{
sprintf(relativeFileName,"%s%s",prefix[i],filename);
tcount = wo.loadObj(relativeFileName);
if (tcount)
break;
}
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(0,-4.5,0));
btCollisionShape* boxShape = new btBoxShape(btVector3(30,2,30));
m_collisionShapes.push_back(boxShape);
localCreateRigidBody(0.f,startTransform,boxShape);
class MyConvexDecomposition : public ConvexDecomposition::ConvexDecompInterface
{
ConvexDecompositionDemo* m_convexDemo;
public:
btAlignedObjectArray<btConvexHullShape*> m_convexShapes;
btAlignedObjectArray<btVector3> m_convexCentroids;
MyConvexDecomposition (FILE* outputFile,ConvexDecompositionDemo* demo)
:m_convexDemo(demo),
mBaseCount(0),
mHullCount(0),
mOutputFile(outputFile)
{
}
virtual void ConvexDecompResult(ConvexDecomposition::ConvexResult &result)
{
btTriangleMesh* trimesh = new btTriangleMesh();
m_convexDemo->m_trimeshes.push_back(trimesh);
btVector3 localScaling(6.f,6.f,6.f);
//export data to .obj
printf("ConvexResult. ");
if (mOutputFile)
{
fprintf(mOutputFile,"## Hull Piece %d with %d vertices and %d triangles.\r\n", mHullCount, result.mHullVcount, result.mHullTcount );
fprintf(mOutputFile,"usemtl Material%i\r\n",mBaseCount);
fprintf(mOutputFile,"o Object%i\r\n",mBaseCount);
for (unsigned int i=0; i<result.mHullVcount; i++)
{
const float *p = &result.mHullVertices[i*3];
fprintf(mOutputFile,"v %0.9f %0.9f %0.9f\r\n", p[0], p[1], p[2] );
}
//calc centroid, to shift vertices around center of mass
centroid.setValue(0,0,0);
btAlignedObjectArray<btVector3> vertices;
if ( 1 )
{
//const unsigned int *src = result.mHullIndices;
for (unsigned int i=0; i<result.mHullVcount; i++)
{
btVector3 vertex(result.mHullVertices[i*3],result.mHullVertices[i*3+1],result.mHullVertices[i*3+2]);
vertex *= localScaling;
centroid += vertex;
}
}
centroid *= 1.f/(float(result.mHullVcount) );
if ( 1 )
{
//const unsigned int *src = result.mHullIndices;
for (unsigned int i=0; i<result.mHullVcount; i++)
{
btVector3 vertex(result.mHullVertices[i*3],result.mHullVertices[i*3+1],result.mHullVertices[i*3+2]);
vertex *= localScaling;
vertex -= centroid ;
vertices.push_back(vertex);
}
}
if ( 1 )
{
const unsigned int *src = result.mHullIndices;
for (unsigned int i=0; i<result.mHullTcount; i++)
{
unsigned int index0 = *src++;
unsigned int index1 = *src++;
unsigned int index2 = *src++;
btVector3 vertex0(result.mHullVertices[index0*3], result.mHullVertices[index0*3+1],result.mHullVertices[index0*3+2]);
btVector3 vertex1(result.mHullVertices[index1*3], result.mHullVertices[index1*3+1],result.mHullVertices[index1*3+2]);
btVector3 vertex2(result.mHullVertices[index2*3], result.mHullVertices[index2*3+1],result.mHullVertices[index2*3+2]);
vertex0 *= localScaling;
vertex1 *= localScaling;
vertex2 *= localScaling;
vertex0 -= centroid;
vertex1 -= centroid;
vertex2 -= centroid;
trimesh->addTriangle(vertex0,vertex1,vertex2);
index0+=mBaseCount;
index1+=mBaseCount;
index2+=mBaseCount;
fprintf(mOutputFile,"f %d %d %d\r\n", index0+1, index1+1, index2+1 );
}
}
// float mass = 1.f;
//this is a tools issue: due to collision margin, convex objects overlap, compensate for it here:
//#define SHRINK_OBJECT_INWARDS 1
#ifdef SHRINK_OBJECT_INWARDS
float collisionMargin = 0.01f;
btAlignedObjectArray<btVector3> planeEquations;
btGeometryUtil::getPlaneEquationsFromVertices(vertices,planeEquations);
btAlignedObjectArray<btVector3> shiftedPlaneEquations;
for (int p=0;p<planeEquations.size();p++)
{
btVector3 plane = planeEquations[p];
plane[3] += collisionMargin;
shiftedPlaneEquations.push_back(plane);
}
btAlignedObjectArray<btVector3> shiftedVertices;
btGeometryUtil::getVerticesFromPlaneEquations(shiftedPlaneEquations,shiftedVertices);
btConvexHullShape* convexShape = new btConvexHullShape(&(shiftedVertices[0].getX()),shiftedVertices.size());
#else //SHRINK_OBJECT_INWARDS
btConvexHullShape* convexShape = new btConvexHullShape(&(vertices[0].getX()),vertices.size());
#endif
if (sEnableSAT)
convexShape->initializePolyhedralFeatures();
convexShape->setMargin(0.01f);
m_convexShapes.push_back(convexShape);
m_convexCentroids.push_back(centroid);
m_convexDemo->m_collisionShapes.push_back(convexShape);
mBaseCount+=result.mHullVcount; // advance the 'base index' counter.
}
}
int mBaseCount;
int mHullCount;
FILE* mOutputFile;
};
if (tcount)
{
btTriangleMesh* trimesh = new btTriangleMesh();
m_trimeshes.push_back(trimesh);
btVector3 localScaling(6.f,6.f,6.f);
int i;
for ( i=0;i<wo.mTriCount;i++)
{
int index0 = wo.mIndices[i*3];
int index1 = wo.mIndices[i*3+1];
int index2 = wo.mIndices[i*3+2];
btVector3 vertex0(wo.mVertices[index0*3], wo.mVertices[index0*3+1],wo.mVertices[index0*3+2]);
btVector3 vertex1(wo.mVertices[index1*3], wo.mVertices[index1*3+1],wo.mVertices[index1*3+2]);
btVector3 vertex2(wo.mVertices[index2*3], wo.mVertices[index2*3+1],wo.mVertices[index2*3+2]);
vertex0 *= localScaling;
vertex1 *= localScaling;
vertex2 *= localScaling;
trimesh->addTriangle(vertex0,vertex1,vertex2);
}
btConvexShape* tmpConvexShape = new btConvexTriangleMeshShape(trimesh);
printf("old numTriangles= %d\n",wo.mTriCount);
printf("old numIndices = %d\n",wo.mTriCount*3);
printf("old numVertices = %d\n",wo.mVertexCount);
printf("reducing vertices by creating a convex hull\n");
//create a hull approximation
btShapeHull* hull = new btShapeHull(tmpConvexShape);
btScalar margin = tmpConvexShape->getMargin();
hull->buildHull(margin);
tmpConvexShape->setUserPointer(hull);
printf("new numTriangles = %d\n", hull->numTriangles ());
printf("new numIndices = %d\n", hull->numIndices ());
printf("new numVertices = %d\n", hull->numVertices ());
btConvexHullShape* convexShape = new btConvexHullShape();
bool updateLocalAabb = false;
for (i=0;i<hull->numVertices();i++)
{
convexShape->addPoint(hull->getVertexPointer()[i],updateLocalAabb);
}
convexShape->recalcLocalAabb();
if (sEnableSAT)
convexShape->initializePolyhedralFeatures();
delete tmpConvexShape;
delete hull;
m_collisionShapes.push_back(convexShape);
float mass = 1.f;
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(0,2,14));
localCreateRigidBody(mass, startTransform,convexShape);
bool useQuantization = true;
btCollisionShape* concaveShape = new btBvhTriangleMeshShape(trimesh,useQuantization);
startTransform.setOrigin(convexDecompositionObjectOffset);
localCreateRigidBody(0.f,startTransform,concaveShape);
m_collisionShapes.push_back (concaveShape);
}
if (tcount)
{
//-----------------------------------
// Bullet Convex Decomposition
//-----------------------------------
char outputFileName[512];
strcpy(outputFileName,filename);
char *dot = strstr(outputFileName,".");
if ( dot )
*dot = 0;
strcat(outputFileName,"_convex.obj");
FILE* outputFile = fopen(outputFileName,"wb");
unsigned int depth = 5;
float cpercent = 5;
float ppercent = 15;
unsigned int maxv = 16;
float skinWidth = 0.0;
printf("WavefrontObj num triangles read %i\n",tcount);
ConvexDecomposition::DecompDesc desc;
desc.mVcount = wo.mVertexCount;
desc.mVertices = wo.mVertices;
desc.mTcount = wo.mTriCount;
desc.mIndices = (unsigned int *)wo.mIndices;
desc.mDepth = depth;
desc.mCpercent = cpercent;
desc.mPpercent = ppercent;
desc.mMaxVertices = maxv;
desc.mSkinWidth = skinWidth;
MyConvexDecomposition convexDecomposition(outputFile,this);
desc.mCallback = &convexDecomposition;
//-----------------------------------------------
// HACD
//-----------------------------------------------
std::vector< HACD::Vec3<HACD::Real> > points;
std::vector< HACD::Vec3<long> > triangles;
for(int i=0; i<wo.mVertexCount; i++ )
{
int index = i*3;
HACD::Vec3<HACD::Real> vertex(wo.mVertices[index], wo.mVertices[index+1],wo.mVertices[index+2]);
points.push_back(vertex);
}
for(int i=0;i<wo.mTriCount;i++)
{
int index = i*3;
HACD::Vec3<long> triangle(wo.mIndices[index], wo.mIndices[index+1], wo.mIndices[index+2]);
triangles.push_back(triangle);
}
HACD::HACD myHACD;
myHACD.SetPoints(&points[0]);
myHACD.SetNPoints(points.size());
myHACD.SetTriangles(&triangles[0]);
myHACD.SetNTriangles(triangles.size());
myHACD.SetCompacityWeight(0.1);
myHACD.SetVolumeWeight(0.0);
// HACD parameters
// Recommended parameters: 2 100 0 0 0 0
size_t nClusters = 2;
double concavity = 100;
bool invert = false;
bool addExtraDistPoints = false;
bool addNeighboursDistPoints = false;
bool addFacesPoints = false;
myHACD.SetNClusters(nClusters); // minimum number of clusters
myHACD.SetNVerticesPerCH(100); // max of 100 vertices per convex-hull
myHACD.SetConcavity(concavity); // maximum concavity
myHACD.SetAddExtraDistPoints(addExtraDistPoints);
myHACD.SetAddNeighboursDistPoints(addNeighboursDistPoints);
myHACD.SetAddFacesPoints(addFacesPoints);
myHACD.Compute();
nClusters = myHACD.GetNClusters();
myHACD.Save("output.wrl", false);
//convexDecomposition.performConvexDecomposition(desc);
// ConvexBuilder cb(desc.mCallback);
// cb.process(desc);
//now create some bodies
if (1)
{
btCompoundShape* compound = new btCompoundShape();
m_collisionShapes.push_back (compound);
btTransform trans;
trans.setIdentity();
for (int c=0;c<nClusters;c++)
{
//generate convex result
size_t nPoints = myHACD.GetNPointsCH(c);
size_t nTriangles = myHACD.GetNTrianglesCH(c);
float* vertices = new float[nPoints*3];
unsigned int* triangles = new unsigned int[nTriangles*3];
HACD::Vec3<HACD::Real> * pointsCH = new HACD::Vec3<HACD::Real>[nPoints];
HACD::Vec3<long> * trianglesCH = new HACD::Vec3<long>[nTriangles];
myHACD.GetCH(c, pointsCH, trianglesCH);
// points
for(size_t v = 0; v < nPoints; v++)
{
vertices[3*v] = pointsCH[v].X();
vertices[3*v+1] = pointsCH[v].Y();
vertices[3*v+2] = pointsCH[v].Z();
}
// triangles
for(size_t f = 0; f < nTriangles; f++)
{
triangles[3*f] = trianglesCH[f].X();
triangles[3*f+1] = trianglesCH[f].Y();
triangles[3*f+2] = trianglesCH[f].Z();
}
delete [] pointsCH;
delete [] trianglesCH;
ConvexResult r(nPoints, vertices, nTriangles, triangles);
convexDecomposition.ConvexDecompResult(r);
}
for (int i=0;i<convexDecomposition.m_convexShapes.size();i++)
{
btVector3 centroid = convexDecomposition.m_convexCentroids[i];
trans.setOrigin(centroid);
btConvexHullShape* convexShape = convexDecomposition.m_convexShapes[i];
compound->addChildShape(trans,convexShape);
btRigidBody* body;
body = localCreateRigidBody( 1.0, trans,convexShape);
}
/* for (int i=0;i<convexDecomposition.m_convexShapes.size();i++)
{
btVector3 centroid = convexDecomposition.m_convexCentroids[i];
trans.setOrigin(centroid);
btConvexHullShape* convexShape = convexDecomposition.m_convexShapes[i];
compound->addChildShape(trans,convexShape);
btRigidBody* body;
body = localCreateRigidBody( 1.0, trans,convexShape);
}*/
#if 1
btScalar mass=10.f;
trans.setOrigin(-convexDecompositionObjectOffset);
btRigidBody* body = localCreateRigidBody( mass, trans,compound);
body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
convexDecompositionObjectOffset.setZ(6);
trans.setOrigin(-convexDecompositionObjectOffset);
body = localCreateRigidBody( mass, trans,compound);
body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
convexDecompositionObjectOffset.setZ(-6);
trans.setOrigin(-convexDecompositionObjectOffset);
body = localCreateRigidBody( mass, trans,compound);
body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
#endif
}
if (outputFile)
fclose(outputFile);
}
#ifdef TEST_SERIALIZATION
//test serializing this
int maxSerializeBufferSize = 1024*1024*5;
btDefaultSerializer* serializer = new btDefaultSerializer(maxSerializeBufferSize);
m_dynamicsWorld->serialize(serializer);
FILE* f2 = fopen("testFile.bullet","wb");
fwrite(serializer->getBufferPointer(),serializer->getCurrentBufferSize(),1,f2);
fclose(f2);
exitPhysics();
//now try again from the loaded file
setupEmptyDynamicsWorld();
#endif //TEST_SERIALIZATION
#endif //NO_OBJ_TO_BULLET
#ifdef TEST_SERIALIZATION
btBulletWorldImporter* fileLoader = new btBulletWorldImporter(m_dynamicsWorld);
//fileLoader->setVerboseMode(true);
fileLoader->loadFile("testFile.bullet");
//fileLoader->loadFile("testFile64Double.bullet");
//fileLoader->loadFile("testFile64Single.bullet");
//fileLoader->loadFile("testFile32Single.bullet");
#endif //TEST_SERIALIZATION
}
void STLMesh::init(const QString stlpath, double mass, PositionData pos) {
if(mass == 0) {
QList<QVector<float>> retour = STLReader::readSTLTextFile(stlpath);
btTriangleMesh* trimesh = new btTriangleMesh();
for(int i=0; i<retour.size(); i++ )
{
QVector<float> s = retour.at(i);
trimesh->addTriangle(btVector3(s[0],s[1],s[2]),
btVector3(s[3],s[4],s[5]),
btVector3(s[6],s[7],s[8]));
}
buildRigidBody(new btBvhTriangleMeshShape(trimesh, true), mass, pos);
}
else if(s_stlshapes.contains(stlpath)) {
s_stlshapes[stlpath].second++;
buildRigidBody(s_stlshapes[stlpath].first, mass, pos);
}
else {
QList<QVector<float>> retour = STLReader::readSTLTextFile(stlpath);
//séparation points et faces
std::vector< HACD::Vec3<HACD::Real> > points;
std::vector< HACD::Vec3<long> > triangles;
for(int i=0; i<retour.size(); i++ )
{
int index = points.size();
int ii[3] = {-1,-1,-1};
for(int j=0; j<retour.at(i).size(); j+=3)
{
HACD::Vec3<HACD::Real> vertex(retour.at(i).at(j), retour.at(i).at(j+1),retour.at(i).at(j+2));
//recherche du point dans la list des points deja crée
for(int h=0; h < points.size();h++) {
if(points[h].X() == vertex.X()
&& points[h].Y() == vertex.Y()
&& points[h].Z() == vertex.Z())
ii[j/3] = h;
}
if(ii[j/3] == -1) {
ii[j/3] = points.size();
points.push_back(vertex);
}
}
HACD::Vec3<long> triangle(ii[0], ii[1], ii[2]);
triangles.push_back(triangle);
}
qDebug() << "Importation de " << points.size()<<" points pour " << triangles.size() << "faces.";
HACD::HACD myHACD;
myHACD.SetPoints(&points[0]);
myHACD.SetNPoints(points.size());
myHACD.SetTriangles(&triangles[0]);
myHACD.SetNTriangles(triangles.size());
myHACD.SetCompacityWeight(0.1);
myHACD.SetVolumeWeight(0.0);
// HACD parameters
// Recommended parameters: 2 100 0 0 0 0
size_t nClusters = 1;
double concavity = 100;
bool invert = false;
bool addExtraDistPoints = false;
bool addNeighboursDistPoints = false;
bool addFacesPoints = false;
myHACD.SetNClusters(nClusters); // minimum number of clusters
myHACD.SetNVerticesPerCH(200); // max of 100 vertices per convex-hull
myHACD.SetConcavity(concavity); // maximum concavity
myHACD.SetAddExtraDistPoints(addExtraDistPoints);
myHACD.SetAddNeighboursDistPoints(addNeighboursDistPoints);
myHACD.SetAddFacesPoints(addFacesPoints);
myHACD.Compute();
nClusters = myHACD.GetNClusters();
qDebug() << "nbcluster trouve " << nClusters;
//myHACD.Save("output.wrl", false);
/*******Création de la forme composé *******/
btCompoundShape* compound = new btCompoundShape();
btTransform trans;
trans.setIdentity();
for (unsigned int c=0;c<nClusters;c++)
{
//generate convex result
size_t nPoints = myHACD.GetNPointsCH(c);
size_t nTriangles = myHACD.GetNTrianglesCH(c);
float* vertices = new float[nPoints*3];
float* vertices2 = new float[nTriangles*9];
HACD::Vec3<HACD::Real> * pointsCH = new HACD::Vec3<HACD::Real>[nPoints];
HACD::Vec3<long> * trianglesCH = new HACD::Vec3<long>[nTriangles];
myHACD.GetCH(c, pointsCH, trianglesCH);
// points
for(size_t v = 0; v < nPoints; v++)
{
vertices[3*v] = pointsCH[v].X();
vertices[3*v+1] = pointsCH[v].Y();
vertices[3*v+2] = pointsCH[v].Z();
}
// triangles
for(size_t f = 0; f < nTriangles; f++)
{
memcpy(&vertices2[9*f], &vertices[3*trianglesCH[f].X()], sizeof(float)*3);
memcpy(&vertices2[9*f+3], &vertices[3*trianglesCH[f].Y()], sizeof(float)*3);
memcpy(&vertices2[9*f+6], &vertices[3*trianglesCH[f].Z()], sizeof(float)*3);
}
delete [] pointsCH;
delete [] trianglesCH;
btConvexHullShape* convexShape = new btConvexHullShape();
for(int k=0; k < nTriangles*9 ; k+=3) {
convexShape->addPoint(btVector3(vertices2[k],vertices2[k+1],vertices2[k+2]),true);
//std::cout << vertices2[k] << " " << vertices2[k+1]<< " " << vertices2[k+2] << std::endl;
}
//trans.setIdentity();
compound->addChildShape(trans,convexShape);
}
buildRigidBody(compound, mass, pos);
s_stlshapes.insert(stlpath, QPair<btCollisionShape*,unsigned int>(compound,1));
}
}
JNACluster* JNAConvexDecomposition(float* inputConcave, int num_triangles, int *pcount){
int float_count = 0;
std::map<std::string,HACD::Vec3<HACD::Real>* > my_map;
float*** triangles = (float ***) malloc (num_triangles * sizeof(float **));
for(int i = 0;i<num_triangles;i++){
triangles[i] = (float **) malloc (3 * sizeof(float *));
for(int k = 0;k<3;k++){
triangles[i][k] = (float *) malloc (3 * sizeof(float));
for(int j = 0;j<3;j++){
triangles[i][k][j] = inputConcave[float_count];
float_count++;
}
}
}
for(int i = 0;i<num_triangles;i++){
for(int j =0;j<3;j++){
HACD::Vec3<HACD::Real>* point = new HACD::Vec3<HACD::Real>(triangles[i][j][0],triangles[i][j][1],triangles[i][j][2]);
int tester = my_map.size();
double x = point->X();
double y = point->Y();
double z = point->Z();
char buffer[32];
snprintf(buffer, sizeof(buffer), "%010.5f/%010.5f/%010.5f", x, y, z);
my_map[buffer] = point;
}
}
std::vector<std::string> keys;
end_points = (HACD::Vec3<HACD::Real>*) malloc (my_map.size() * sizeof(HACD::Vec3<HACD::Real>));
int key_count = 0;
std::map<std::string,HACD::Vec3<HACD::Real>* >::iterator key_iter;
for(key_iter = my_map.begin();key_iter!=my_map.end();key_iter++){
std::string temp_str = key_iter->first;
keys.push_back(temp_str);
end_points[key_count] = *key_iter->second;
key_count++;
}
end_triangles = (HACD::Vec3<long>*) malloc (num_triangles * sizeof(HACD::Vec3<long>));
for(int i = 0;i<num_triangles;i++){
long index[3];
int index_count = 0;
for(int j =0;j<3;j++){
HACD::Vec3<HACD::Real>* point = new HACD::Vec3<HACD::Real>(triangles[i][j][0],triangles[i][j][1],triangles[i][j][2]);
double x = point->X();
double y = point->Y();
double z = point->Z();
char buffer[32];
snprintf(buffer, sizeof(buffer), "%010.5f/%010.5f/%010.5f", x, y, z);
for(int k = 0;k<keys.size();k++){
std::string key1_str = keys.at(k);
if(key1_str.compare(buffer)==0){
index[index_count] = (long)k;
index_count++;
}
}
}
HACD::Vec3<long>* triangle = new HACD::Vec3<long>(index[0],index[1],index[2]);
end_triangles[i] = *triangle;
int testert = 0;
}
for(int i =0;i<num_triangles;i++){
for(int j =0;j<3;j++){
delete[] triangles[i][j];
}
delete[] triangles[i];
}
my_hacd.SetNTriangles(num_triangles);
my_hacd.SetNPoints(my_map.size());
my_hacd.SetPoints(end_points);
my_hacd.SetTriangles(end_triangles);
my_hacd.Compute();
//
int num_clusters = my_hacd.GetNClusters();
pcount[0] = num_clusters;
clusters = (JNACluster*)malloc(num_clusters*sizeof(JNACluster));
int cluster_count =0;
for(size_t i = 0;i<num_clusters;i++){
size_t nPoints = my_hacd.GetNPointsCH(i);
size_t nTriangles = my_hacd.GetNTrianglesCH(i);
HACD::Vec3<HACD::Real>* cluster_points = new HACD::Vec3<HACD::Real>[nPoints];
HACD::Vec3<long>* cluster_triangles = new HACD::Vec3<long>[nTriangles];
my_hacd.GetCH(i,cluster_points,cluster_triangles);
float* end_cluster_points = (float*)malloc((nPoints*3)*sizeof(float));
for(size_t j = 0;j<nPoints;j++){
double x = cluster_points[j].X();
end_cluster_points[j*3]=x;
double y = cluster_points[j].Y();
end_cluster_points[j*3+1]=y;
double z = cluster_points[j].Z();
end_cluster_points[j*3+2]=z;
}
int n_points = my_hacd.GetNPointsCH(cluster_count);
clusters[cluster_count].floats = end_cluster_points;
clusters[cluster_count].size = n_points*3;
cluster_count++;
delete[] cluster_points;
delete[] cluster_triangles;
}
return clusters;
}
void JNASetAddFacesPoints(bool addFacePoints){
my_hacd.SetAddFacesPoints(addFacePoints);
}
void JNASetAddNeighboursDistPoints(bool addNeighboursDistPoints){
my_hacd.SetAddNeighboursDistPoints(addNeighboursDistPoints);
}
void JNASetAddExtraDistPoints(bool addDistPoints){
my_hacd.SetAddExtraDistPoints(addDistPoints);
}
vector<pair<btVector3, btConvexHullShape*> > ofxBulletConvexDecomposer::decompose(const ofMesh &meshToDecompose, btVector3 scale )
{
assert( meshToDecompose.getMode() == OF_TRIANGLES_MODE );
vector<pair<btVector3, btConvexHullShape*> > convexShapes;
int tcount = meshToDecompose.getNumIndices()/3;
if ( tcount == 0 )
// nothing to do
return convexShapes;
// adapted from bullet-2.81-rev2613/Demos/ConvexDecompositionDemo/ConvexDecompositionDemo.cpp
/*
unsigned int depth = 5;
float cpercent = 5;
float ppercent = 15;
unsigned int maxv = 16;
float skinWidth = 0.0;
// ConvexDecomposition::WavefrontObj wo;
ConvexDecomposition::DecompDesc desc;
desc.mVcount = meshToDecompose.getNumVertices();
desc.mVertices = (float*)(meshToDecompose.getVerticesPointer());
desc.mTcount = meshToDecompose.getNumIndices()/3;
desc.mIndices = meshToDecompose.getIndexPointer();
desc.mDepth = depth;
desc.mCpercent = cpercent;
desc.mPpercent = ppercent;
desc.mMaxVertices = maxv;
desc.mSkinWidth = skinWidth;
desc.mCallback = this;
*/
//-----------------------------------------------
// HACD
//-----------------------------------------------
std::vector< HACD::Vec3<HACD::Real> > points;
std::vector< HACD::Vec3<long> > triangles;
for(int i=0; i<meshToDecompose.getNumVertices(); i++ )
{
ofVec3f meshVert = meshToDecompose.getVertex(i);
HACD::Vec3<HACD::Real> vertex( meshVert.x, meshVert.y, meshVert.z );
points.push_back(vertex);
}
for(int i=0;i<meshToDecompose.getNumIndices(); i+=3 )
{
HACD::Vec3<long> triangle(meshToDecompose.getIndex(i), meshToDecompose.getIndex(i+1), meshToDecompose.getIndex(i+2) );
triangles.push_back(triangle);
}
assert(triangles.size()==tcount);
HACD::HACD myHACD;
myHACD.SetPoints(&points[0]);
myHACD.SetNPoints(points.size());
myHACD.SetTriangles(&triangles[0]);
myHACD.SetNTriangles(triangles.size());
myHACD.SetCompacityWeight(0.1);
myHACD.SetVolumeWeight(0.0);
// HACD parameters
// Recommended parameters: 2 100 0 0 0 0
size_t nClusters = 2;
double concavity = 100;
bool invert = false;
bool addExtraDistPoints = false;
bool addNeighboursDistPoints = false;
bool addFacesPoints = false;
myHACD.SetNClusters(nClusters); // minimum number of clusters
myHACD.SetNVerticesPerCH(100); // max of 100 vertices per convex-hull
myHACD.SetConcavity(concavity); // maximum concavity
myHACD.SetAddExtraDistPoints(addExtraDistPoints);
myHACD.SetAddNeighboursDistPoints(addNeighboursDistPoints);
myHACD.SetAddFacesPoints(addFacesPoints);
myHACD.SetCallBack( hacdCallback );
myHACD.Compute();
nClusters = myHACD.GetNClusters();
int totalTriangles = 0;
int totalPoints = 0;
for (int c=0;c<nClusters;c++)
{
//generate convex result
size_t nPoints = myHACD.GetNPointsCH(c);
size_t nTriangles = myHACD.GetNTrianglesCH(c);
ofLogVerbose("ofxBulletConvexDecomposer") << "cluster " << c <<"/" << nClusters << " points " << nPoints << " triangles " << nTriangles;
float* vertices = new float[nPoints*3];
unsigned int* triangles = new unsigned int[nTriangles*3];
HACD::Vec3<HACD::Real> * pointsCH = new HACD::Vec3<HACD::Real>[nPoints];
HACD::Vec3<long> * trianglesCH = new HACD::Vec3<long>[nTriangles];
myHACD.GetCH(c, pointsCH, trianglesCH);
// points
for(size_t v = 0; v < nPoints; v++)
{
vertices[3*v] = pointsCH[v].X();
vertices[3*v+1] = pointsCH[v].Y();
vertices[3*v+2] = pointsCH[v].Z();
}
// triangles
for(size_t f = 0; f < nTriangles; f++)
{
triangles[3*f] = trianglesCH[f].X();
triangles[3*f+1] = trianglesCH[f].Y();
triangles[3*f+2] = trianglesCH[f].Z();
}
ConvexResult r(nPoints, vertices, nTriangles, triangles);
convexShapes.push_back( createConvexHullShapeFromConvexResult(r, scale) );
delete [] pointsCH;
delete [] trianglesCH;
delete [] vertices;
delete [] triangles;
totalTriangles += nTriangles;
}
return convexShapes;
}
void JNASetNVerticesPerCH(int num_vertices){
my_hacd.SetNVerticesPerCH(num_vertices);
}
void JNASetNClusters(int num_clusters){
my_hacd.SetNClusters(num_clusters);
}
void JNASetVolumeWeight(float weight){
my_hacd.SetVolumeWeight(weight);
}
void JNASetCompacityWeight(float weight){
my_hacd.SetCompacityWeight(weight);
}
DKConvexHullShape::ConvexHullArray DKConvexHullShape::DecomposeTriangleMesh(
const DKVector3* verts,
size_t numVerts,
const long* indices,
size_t numIndices,
size_t minClusters,
size_t maxVertsPerCH,
double maxConcavity,
bool addExtraDistPoints,
bool addNeighboursDistPoints,
bool addFacesPoints)
{
ConvexHullArray result;
size_t numTriangles = numIndices / 3;
if (verts && numVerts > 0 && indices && numTriangles > 0)
{
DKArray<HACDPoint> points;
DKArray<HACDTriangle> triangles;
points.Reserve(numVerts);
triangles.Reserve(numTriangles);
for (size_t i = 0; i < numVerts; ++i)
{
points.Add(HACDPoint(verts[i].x, verts[i].y, verts[i].z));
}
for (size_t i = 0; i < numTriangles; ++i)
{
triangles.Add(HACDTriangle(indices[i*3], indices[i*3+1], indices[i*3+2]));
}
HACD::HACD hacd;
hacd.SetPoints(points);
hacd.SetNPoints(points.Count());
hacd.SetTriangles(triangles);
hacd.SetNTriangles(triangles.Count());
hacd.SetCompacityWeight(0.1);
hacd.SetVolumeWeight(0.0);
hacd.SetNClusters(minClusters);
hacd.SetNVerticesPerCH(maxVertsPerCH);
hacd.SetConcavity(maxConcavity);
hacd.SetAddExtraDistPoints(addExtraDistPoints);
hacd.SetAddNeighboursDistPoints(addNeighboursDistPoints);
hacd.SetAddFacesPoints(addFacesPoints);
hacd.Compute();
size_t numClusters = hacd.GetNClusters();
result.Reserve(numClusters);
for (size_t i = 0; i < numClusters; ++i)
{
HACDCluster cluster = CreateConvexHullShapeHACD(hacd, i, btVector3(1,1,1));
DKASSERT_DEBUG(cluster.shape);
ConvexHull res = {
DKOBJECT_NEW DKConvexHullShape(ShapeType::ConvexHull, cluster.shape),
BulletVector3(cluster.centroid)
};
result.Add(res);
}
}
return result;
}
void BulletCollisionDetectorImpl::addMesh(GeometryEx* model)
{
SgMesh* mesh = meshExtractor->currentMesh();
const Affine3& T = meshExtractor->currentTransform();
bool meshAdded = false;
if(mesh->primitiveType() != SgMesh::MESH){
bool doAddPrimitive = false;
Vector3 scale;
optional<Vector3> translation;
if(!meshExtractor->isCurrentScaled()){
scale.setOnes();
doAddPrimitive = true;
} else {
Affine3 S = meshExtractor->currentTransformWithoutScaling().inverse() *
meshExtractor->currentTransform();
if(S.linear().isDiagonal()){
if(!S.translation().isZero()){
translation = S.translation();
}
scale = S.linear().diagonal();
if(mesh->primitiveType() == SgMesh::BOX){
doAddPrimitive = true;
} else if(mesh->primitiveType() == SgMesh::SPHERE){
// check if the sphere is uniformly scaled for all the axes
if(scale.x() == scale.y() && scale.x() == scale.z()){
doAddPrimitive = true;
}
} else if(mesh->primitiveType() == SgMesh::CYLINDER){
// check if the bottom circle face is uniformly scaled
if(scale.x() == scale.z()){
doAddPrimitive = true;
}
} else if(mesh->primitiveType() == SgMesh::CONE){
if(scale.x() == scale.z()){
doAddPrimitive = true;
}
}
}
}
if(doAddPrimitive){
bool created = false;
btCollisionShape* primitiveShape;
switch(mesh->primitiveType()){
case SgMesh::BOX : {
const Vector3& s = mesh->primitive<SgMesh::Box>().size;
primitiveShape = new btBoxShape(btVector3(s.x() * scale.x()/2.0, s.y() * scale.y()/2.0, s.z() * scale.z()/2.0));
created = true;
break; }
case SgMesh::SPHERE : {
SgMesh::Sphere sphere = mesh->primitive<SgMesh::Sphere>();
primitiveShape = new btSphereShape(sphere.radius * scale.x());
created = true;
break; }
case SgMesh::CYLINDER : {
SgMesh::Cylinder cylinder = mesh->primitive<SgMesh::Cylinder>();
primitiveShape = new btCylinderShape(btVector3(cylinder.radius * scale.x(), cylinder.height * scale.y()/2.0, cylinder.radius * scale.x()));
created = true;
break; }
case SgMesh::CONE : {
SgMesh::Cone cone = mesh->primitive<SgMesh::Cone>();
primitiveShape = new btConeShape(cone.radius * scale.x(), cone.height * scale.y());
created = true;
break; }
default :
break;
}
if(created){
primitiveShape->setMargin(DEFAULT_COLLISION_MARGIN);
btCompoundShape* compoundShape = dynamic_cast<btCompoundShape*>(model->collisionShape);
if(!compoundShape){
model->collisionShape = new btCompoundShape();
model->collisionShape->setLocalScaling(btVector3(1.f,1.f,1.f));
compoundShape = dynamic_cast<btCompoundShape*>(model->collisionShape);
}
Affine3 T_ = meshExtractor->currentTransformWithoutScaling();
if(translation){
T_ *= Translation3(*translation);
}
btVector3 p(T_(0,3), T_(1,3), T_(2,3));
btMatrix3x3 R(T_(0,0), T_(0,1), T_(0,2),
T_(1,0), T_(1,1), T_(1,2),
T_(2,0), T_(2,1), T_(2,2));
btTransform btT(R, p);
compoundShape->addChildShape(btT, primitiveShape);
meshAdded = true;
}
}
}
if(!meshAdded){
if(!useHACD || model->isStatic){
const int vertexIndexTop = model->vertices.size() / 3;
const SgVertexArray& vertices_ = *mesh->vertices();
const int numVertices = vertices_.size();
for(int i=0; i < numVertices; ++i){
const Vector3 v = T * vertices_[i].cast<Position::Scalar>();
model->vertices.push_back((btScalar)v.x());
model->vertices.push_back((btScalar)v.y());
model->vertices.push_back((btScalar)v.z());
}
const int numTriangles = mesh->numTriangles();
for(int i=0; i < numTriangles; ++i){
SgMesh::TriangleRef tri = mesh->triangle(i);
model->triangles.push_back(vertexIndexTop + tri[0]);
model->triangles.push_back(vertexIndexTop + tri[1]);
model->triangles.push_back(vertexIndexTop + tri[2]);
}
}else{
btConvexHullShape* convexHullShape = dynamic_cast<btConvexHullShape*>(model->collisionShape);
if(convexHullShape){
btCompoundShape* compoundShape = new btCompoundShape();
compoundShape->setLocalScaling(btVector3(1.f,1.f,1.f));
btTransform T;
T.setIdentity();
compoundShape->addChildShape(T, convexHullShape);
model->collisionShape = compoundShape;
}
std::vector< HACD::Vec3<HACD::Real> > points;
std::vector< HACD::Vec3<long> > triangles;
const SgVertexArray& vertices_ = *mesh->vertices();
const int numVertices = vertices_.size();
for(int i=0; i < numVertices; ++i){
const Vector3 v = T * vertices_[i].cast<Position::Scalar>();
HACD::Vec3<HACD::Real> vertex(v.x(), v.y(), v.z());
points.push_back(vertex);
}
const int numTriangles = mesh->numTriangles();
for(int i=0; i < numTriangles; ++i){
SgMesh::TriangleRef tri = mesh->triangle(i);
HACD::Vec3<long> triangle(tri[0], tri[1], tri[2]);
triangles.push_back(triangle);
}
HACD::HACD hacd;
hacd.SetPoints(&points[0]);
hacd.SetNPoints(points.size());
hacd.SetTriangles(&triangles[0]);
hacd.SetNTriangles(triangles.size());
hacd.SetCompacityWeight(0.1);
hacd.SetVolumeWeight(0.0);
size_t nClusters = 1;
double concavity = 100;
bool invert = false;
bool addExtraDistPoints = false;
bool addNeighboursDistPoints = false;
bool addFacesPoints = false;
hacd.SetNClusters(nClusters); // minimum number of clusters
hacd.SetNVerticesPerCH(100); // max of 100 vertices per convex-hull
hacd.SetConcavity(concavity); // maximum concavity
hacd.SetAddExtraDistPoints(addExtraDistPoints);
hacd.SetAddNeighboursDistPoints(addNeighboursDistPoints);
hacd.SetAddFacesPoints(addFacesPoints);
hacd.Compute();
btTransform T;
T.setIdentity();
nClusters = hacd.GetNClusters();
if(nClusters>1){
btCompoundShape* compoundShape = dynamic_cast<btCompoundShape*>(model->collisionShape);
if(!compoundShape){
model->collisionShape = new btCompoundShape();
model->collisionShape->setLocalScaling(btVector3(1.f,1.f,1.f));
}
}
for(size_t i=0; i<nClusters; i++){
size_t nPoints = hacd.GetNPointsCH(i);
size_t nTriangles = hacd.GetNTrianglesCH(i);
HACD::Vec3<HACD::Real> * pointsCH = new HACD::Vec3<HACD::Real>[nPoints];
HACD::Vec3<long> * trianglesCH = new HACD::Vec3<long>[nTriangles];
hacd.GetCH(i, pointsCH, trianglesCH);
btAlignedObjectArray<btVector3> newVertices_;
for(size_t j=0; j<nTriangles; j++){
long index0 = trianglesCH[j].X();
long index1 = trianglesCH[j].Y();
long index2 = trianglesCH[j].Z();
btVector3 vertex0(pointsCH[index0].X(), pointsCH[index0].Y(), pointsCH[index0].Z());
btVector3 vertex1(pointsCH[index1].X(), pointsCH[index1].Y(), pointsCH[index1].Z());
btVector3 vertex2(pointsCH[index2].X(), pointsCH[index2].Y(), pointsCH[index2].Z());
newVertices_.push_back(vertex0);
newVertices_.push_back(vertex1);
newVertices_.push_back(vertex2);
}
delete [] pointsCH;
delete [] trianglesCH;
/*
float collisionMargin = 0.01f;
btAlignedObjectArray<btVector3> planeEquations;
btGeometryUtil::getPlaneEquationsFromVertices(newVertices_, planeEquations);
btAlignedObjectArray<btVector3> shiftedPlaneEquations;
for (int j=0; j<planeEquations.size(); j++){
btVector3 plane = planeEquations[j];
plane[3] += collisionMargin;
shiftedPlaneEquations.push_back(plane);
}
btAlignedObjectArray<btVector3> shiftedVertices;
btGeometryUtil::getVerticesFromPlaneEquations(shiftedPlaneEquations,shiftedVertices);
btConvexHullShape* convexHullShape_ = new btConvexHullShape(&(shiftedVertices[0].getX()),shiftedVertices.size());
*/
btConvexHullShape* convexHullShape_ = new btConvexHullShape(&(newVertices_[0].getX()), newVertices_.size());
convexHullShape_->setMargin(DEFAULT_COLLISION_MARGIN);
btCompoundShape* compoundShape = dynamic_cast<btCompoundShape*>(model->collisionShape);
if(compoundShape)
compoundShape->addChildShape(T, convexHullShape_);
else
model->collisionShape = convexHullShape_;
}
}
}
}
// From a previously created mesh shape, create a convex hull using the Bullet
// HACD hull creation code. The created hull will go into the hull collection
// so remember to delete it later.
// Returns the created collision shape or NULL if couldn't create
btCollisionShape* BulletSim::BuildHullShapeFromMesh2(btCollisionShape* mesh, HACDParams* parms)
{
#if defined(USEBULLETHACD)
// Get the triangle mesh data out of the passed mesh shape
int shapeType = mesh->getShapeType();
if (shapeType != TRIANGLE_MESH_SHAPE_PROXYTYPE)
{
// If the passed shape doesn't have a triangle mesh, we cannot hullify it.
m_worldData.BSLog("HACD: passed mesh not TRIANGLE_MESH_SHAPE"); // DEBUG DEBUG
return NULL;
}
btStridingMeshInterface* meshInfo = ((btTriangleMeshShape*)mesh)->getMeshInterface();
const unsigned char* vertexBase;
int numVerts;
PHY_ScalarType vertexType;
int vertexStride;
const unsigned char* indexBase;
int indexStride;
int numFaces;
PHY_ScalarType indicesType;
meshInfo->getLockedReadOnlyVertexIndexBase(&vertexBase, numVerts, vertexType, vertexStride, &indexBase, indexStride, numFaces, indicesType);
if (vertexType != PHY_FLOAT || indicesType != PHY_INTEGER)
{
// If an odd data structure, we cannot hullify
m_worldData.BSLog("HACD: triangle mesh not of right types"); // DEBUG DEBUG
return NULL;
}
// Create pointers to the vertices and indices as the PHY types that they are
float* tVertex = (float*)vertexBase;
int tVertexStride = vertexStride / sizeof(float);
int* tIndices = (int*) indexBase;
int tIndicesStride = indexStride / sizeof(int);
m_worldData.BSLog("HACD: nVertices=%d, nIndices=%d", numVerts, numFaces*3); // DEBUG DEBUG
// Copy the vertices/indices into the HACD data structures
std::vector< HACD::Vec3<HACD::Real> > points;
std::vector< HACD::Vec3<long> > triangles;
for (int ii=0; ii < (numVerts * tVertexStride); ii += tVertexStride)
{
HACD::Vec3<HACD::Real> vertex(tVertex[ii], tVertex[ii+1],tVertex[ii+2]);
points.push_back(vertex);
}
for(int ii=0; ii < (numFaces * tIndicesStride); ii += tIndicesStride )
{
HACD::Vec3<long> vertex( tIndices[ii], tIndices[ii+1], tIndices[ii+2]);
triangles.push_back(vertex);
}
meshInfo->unLockReadOnlyVertexBase(0);
m_worldData.BSLog("HACD: structures copied"); // DEBUG DEBUG
// Setup HACD parameters
HACD::HACD myHACD;
myHACD.SetPoints(&points[0]);
myHACD.SetNPoints(points.size());
myHACD.SetTriangles(&triangles[0]);
myHACD.SetNTriangles(triangles.size());
myHACD.SetCompacityWeight((double)parms->compacityWeight);
myHACD.SetVolumeWeight((double)parms->volumeWeight);
myHACD.SetNClusters((size_t)parms->minClusters);
myHACD.SetNVerticesPerCH((size_t)parms->maxVerticesPerHull);
myHACD.SetConcavity((double)parms->concavity);
myHACD.SetAddExtraDistPoints(parms->addExtraDistPoints == ParamTrue ? true : false);
myHACD.SetAddNeighboursDistPoints(parms->addNeighboursDistPoints == ParamTrue ? true : false);
myHACD.SetAddFacesPoints(parms->addFacesPoints == ParamTrue ? true : false);
m_worldData.BSLog("HACD: Before compute. nPoints=%d, nTriangles=%d, minClusters=%f, maxVerts=%f",
points.size(), triangles.size(), parms->minClusters, parms->maxVerticesPerHull); // DEBUG DEBUG
// Hullify the mesh
myHACD.Compute();
int nHulls = myHACD.GetNClusters();
m_worldData.BSLog("HACD: After compute. nHulls=%d", nHulls); // DEBUG DEBUG
// Create the compound shape all the hulls will be added to
btCompoundShape* compoundShape = new btCompoundShape(true);
compoundShape->setMargin(m_worldData.params->collisionMargin);
// Convert each of the built hulls into btConvexHullShape objects and add to the compoundShape
for (int hul=0; hul < nHulls; hul++)
{
size_t nPoints = myHACD.GetNPointsCH(hul);
size_t nTriangles = myHACD.GetNTrianglesCH(hul);
m_worldData.BSLog("HACD: Add hull %d. nPoints=%d, nTriangles=%d", hul, nPoints, nTriangles); // DEBUG DEBUG
// Get the vertices and indices for one hull
HACD::Vec3<HACD::Real> * pointsCH = new HACD::Vec3<HACD::Real>[nPoints];
HACD::Vec3<long> * trianglesCH = new HACD::Vec3<long>[nTriangles];
myHACD.GetCH(hul, pointsCH, trianglesCH);
// Average the location of all the vertices to create a centriod for the hull.
btAlignedObjectArray<btVector3> vertices;
btVector3 centroid;
centroid.setValue(0,0,0);
for (int ii=0; ii < nTriangles; ii++)
{
long tri = trianglesCH[ii].X();
btVector3 corner1(pointsCH[tri].X(), pointsCH[tri].Y(), pointsCH[tri].Z() );
vertices.push_back(corner1);
centroid += corner1;
tri = trianglesCH[ii].Y();
btVector3 corner2(pointsCH[tri].X(), pointsCH[tri].Y(), pointsCH[tri].Z() );
vertices.push_back(corner2);
centroid += corner2;
tri = trianglesCH[ii].Z();
btVector3 corner3(pointsCH[tri].X(), pointsCH[tri].Y(), pointsCH[tri].Z() );
vertices.push_back(corner3);
centroid += corner3;
}
centroid *= 1.f/((float)(nTriangles * 3));
for (int ii=0; ii < vertices.size(); ii++)
{
vertices[ii] -= centroid;
}
delete [] pointsCH;
delete [] trianglesCH;
btConvexHullShape* convexShape;
// Optionally compress the hull a little bit to account for the collision margin.
if (parms->shouldAdjustCollisionMargin == ParamTrue)
{
float collisionMargin = 0.01f;
btAlignedObjectArray<btVector3> planeEquations;
btGeometryUtil::getPlaneEquationsFromVertices(vertices, planeEquations);
btAlignedObjectArray<btVector3> shiftedPlaneEquations;
for (int p=0; p<planeEquations.size(); p++)
{
btVector3 plane = planeEquations[p];
plane[3] += collisionMargin;
shiftedPlaneEquations.push_back(plane);
}
btAlignedObjectArray<btVector3> shiftedVertices;
btGeometryUtil::getVerticesFromPlaneEquations(shiftedPlaneEquations,shiftedVertices);
convexShape = new btConvexHullShape(&(shiftedVertices[0].getX()),shiftedVertices.size());
}
else
{
convexShape = new btConvexHullShape(&(vertices[0].getX()),vertices.size());
}
convexShape->setMargin(m_worldData.params->collisionMargin);
// Add the hull shape to the compound shape
btTransform childTrans;
childTrans.setIdentity();
childTrans.setOrigin(centroid);
m_worldData.BSLog("HACD: Add child shape %d", hul); // DEBUG DEBUG
compoundShape->addChildShape(childTrans, convexShape);
}
return compoundShape;
#else
return NULL;
#endif
}
void JNASetConcavity(int concavity){
my_hacd.SetConcavity(concavity);
}
btCompoundShape* convex_decomposition_hacd::ConvexDecomp(int numVertices, float* vertices, int numIndices,const unsigned int* indices)
{
//-----------------------------------------------
// HACD
//-----------------------------------------------
std::vector< HACD::Vec3<HACD::Real> > points;
std::vector< HACD::Vec3<long> > triangles;
for(int i=0; i<numVertices; i++ )
{
int index = i*3;
HACD::Vec3<HACD::Real> vertex(vertices[index], vertices[index+1],vertices[index+2]);
points.push_back(vertex);
}
for(int i=0;i<numIndices/3;i++)
{
int index = i*3;
HACD::Vec3<long> triangle(indices[index], indices[index+1], indices[index+2]);
triangles.push_back(triangle);
}
HACD::HACD myHACD;
myHACD.SetPoints(&points[0]);
myHACD.SetNPoints(points.size());
myHACD.SetTriangles(&triangles[0]);
myHACD.SetNTriangles(triangles.size());
myHACD.SetCompacityWeight(0.1);
myHACD.SetVolumeWeight(0.0);
// HACD parameters
// Recommended parameters: 2 100 0 0 0 0
size_t nClusters = 2;
double concavity = 10;
bool invert = false;
bool addExtraDistPoints = true;//false;
bool addNeighboursDistPoints = true;//false;
bool addFacesPoints = false;
myHACD.SetNClusters(nClusters); // minimum number of clusters
myHACD.SetNVerticesPerCH(256); // max of 100 vertices per convex-hull
myHACD.SetConcavity(concavity); // maximum concavity
myHACD.SetAddExtraDistPoints(addExtraDistPoints);
myHACD.SetAddNeighboursDistPoints(addNeighboursDistPoints);
myHACD.SetAddFacesPoints(addFacesPoints);
myHACD.SetAddExtraDistPoints(true);
myHACD.SetAddFacesPoints(true);
MStatus stat = MS::kSuccess;
MString title = "Esc to stop";
MString sleeping = "Esc to stop";
int amount = 0;
int maxProgress = 1000;
// First reserve the progress window. If a progress window is already
// active (eg. through the mel "progressWindow" command), this command
// fails.
//
if (!MProgressWindow::reserve())
{
MGlobal::displayError("Progress window already in use.");
stat = MS::kFailure;
}
//
// Set up and print progress window state
//
CHECK_MSTATUS(MProgressWindow::setProgressRange(amount, maxProgress));
CHECK_MSTATUS(MProgressWindow::setTitle(title));
CHECK_MSTATUS(MProgressWindow::setInterruptable(true));
CHECK_MSTATUS(MProgressWindow::setProgress(amount));
MString progressWindowState = MString("Progress Window Info:") +
MString("\nMin: ") + MProgressWindow::progressMin() +
MString("\nMax: ") + MProgressWindow::progressMax() +
MString("\nTitle: ") + MProgressWindow::title() +
MString("\nInterruptible: ") + MProgressWindow::isInterruptable();
MGlobal::displayInfo(progressWindowState);
CHECK_MSTATUS(MProgressWindow::startProgress());
int i=1;
MString statusStr = sleeping;
statusStr += i;
CHECK_MSTATUS(MProgressWindow::setProgressStatus(statusStr));
CHECK_MSTATUS(MProgressWindow::advanceProgress(1));
MGlobal::displayInfo(MString("Current progress: ") + MProgressWindow::progress());
MGlobal::executeCommand("pause -sec 1", false,false);
// Count 10 seconds
//
/* for (int i = amount; i < maxProgress; i++)
{
if (i != 0 && MProgressWindow::isCancelled()) {
MGlobal::displayInfo("Progress interrupted!");
break;
}
MString statusStr = sleeping;
statusStr += i;
CHECK_MSTATUS(MProgressWindow::setProgressStatus(statusStr));
CHECK_MSTATUS(MProgressWindow::advanceProgress(1));
MGlobal::displayInfo(MString("Current progress: ") + MProgressWindow::progress());
MGlobal::executeCommand("pause -sec 1", false, false);
}
*/
// End the progress, unreserving the progress window so it can be used
// elsewhere.
//
myHACD.SetCallBack(mayaCallback);
bool result = myHACD.Compute();
if (!result)
{
nClusters = 0;
} else
{
nClusters = myHACD.GetNClusters();
}
CHECK_MSTATUS(MProgressWindow::endProgress());
// myHACD.Save("output.wrl", false);
btCompoundShape* compound = new btCompoundShape();
// mMergedTriangleMesh = new btTriangleMesh();
//now create some bodies
if (1)
{
btTransform trans;
trans.setIdentity();
for (int c=0;c<nClusters;c++)
{
//generate convex result
size_t nPoints = myHACD.GetNPointsCH(c);
size_t nTriangles = myHACD.GetNTrianglesCH(c);
float* vertices = new float[nPoints*3];
unsigned int* triangles = new unsigned int[nTriangles*3];
HACD::Vec3<HACD::Real> * pointsCH = new HACD::Vec3<HACD::Real>[nPoints];
HACD::Vec3<long> * trianglesCH = new HACD::Vec3<long>[nTriangles];
myHACD.GetCH(c, pointsCH, trianglesCH);
// points
for(size_t v = 0; v < nPoints; v++)
{
vertices[3*v] = pointsCH[v].X();
vertices[3*v+1] = pointsCH[v].Y();
vertices[3*v+2] = pointsCH[v].Z();
}
// triangles
for(size_t f = 0; f < nTriangles; f++)
{
triangles[3*f] = trianglesCH[f].X();
triangles[3*f+1] = trianglesCH[f].Y();
triangles[3*f+2] = trianglesCH[f].Z();
}
delete [] pointsCH;
delete [] trianglesCH;
ConvexDecompResult(nPoints, vertices, nTriangles, triangles);
}
for (int i=0;i<m_convexShapes.size();i++)
{
btVector3 centroid = m_convexCentroids[i];
trans.setOrigin(centroid);
btConvexHullShape* convexShape = m_convexShapes[i];
compound->addChildShape(trans,convexShape);
}
}
/* mMergedTriangleVertices = new float[mNumMergedTriangleVertices*3];
mMergedTriangleIndices = new int[mNumMergedTriangleIndices];
for(int i=0; i<m_trimeshes.size(); i++)
{
mMergedTriangleVertices[i] =
}*/
return compound;
}
boost::shared_ptr<btCompoundShape> ConvexDecomp::run(std::vector<boost::shared_ptr<btCollisionShape> > &shapeStorage) {
HACD::HACD hacd;
hacd.SetPoints(&points[0]);
hacd.SetNPoints(points.size());
hacd.SetTriangles(&triangles[0]);
hacd.SetNTriangles(triangles.size());
hacd.SetCompacityWeight(0.1);
hacd.SetVolumeWeight(0.0);
// HACD parameters
// Recommended parameters: 2 100 0 0 0 0
//size_t nClusters = 2;
size_t nClusters = 1;
double concavity = 100;
bool invert = false;
bool addExtraDistPoints = false;
bool addNeighboursDistPoints = false;
bool addFacesPoints = false;
hacd.SetNClusters(nClusters); // minimum number of clusters
hacd.SetNVerticesPerCH(100); // max of 100 vertices per convex-hull
hacd.SetConcavity(concavity); // maximum concavity
hacd.SetAddExtraDistPoints(addExtraDistPoints);
hacd.SetAddNeighboursDistPoints(addNeighboursDistPoints);
hacd.SetAddFacesPoints(addFacesPoints);
hacd.Compute();
nClusters = hacd.GetNClusters();
boost::shared_ptr<btCompoundShape> compound(new btCompoundShape());
for (int c = 0; c < nClusters; ++c) {
btVector3 centroid;
boost::shared_ptr<btConvexHullShape> shape(processCluster(hacd, c, centroid));
shapeStorage.push_back(shape);
compound->addChildShape(btTransform(btQuaternion(0, 0, 0, 1), centroid), shape.get());
}
return compound;
}
