Ejemplo n.º 1
0
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;
}
Ejemplo n.º 2
0
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;
}
Ejemplo n.º 3
0
// 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
}
Ejemplo n.º 4
0
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

}
Ejemplo n.º 5
0
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)); 
  }
}
Ejemplo n.º 6
0
void JNASetAddExtraDistPoints(bool addDistPoints){
    my_hacd.SetAddExtraDistPoints(addDistPoints);
}
Ejemplo n.º 7
0
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_;
            }
        }
    }
}
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;
}