Ejemplo n.º 1
0
std::vector<PQP_Model *> Convexer::fromaiScene(const aiScene *scene) {
  // Get the triangles
  std::vector<PQP_Model *> ret_val;
  PQP_REAL v1[3], v2[3], v3[3], v4[3];
  for (unsigned int i = 0; i < scene->mNumMeshes; i++) {
    aiMesh *curr_mesh = scene->mMeshes[i];
    if (curr_mesh != NULL) {
      PQP_Model *mod = new PQP_Model();
      for (unsigned int k = 0; k < curr_mesh->mNumFaces; k++) {
	aiFace &curr_face = curr_mesh->mFaces[k];
	
	if (curr_face.mNumIndices == 3) {
	  // Triangualr faces!! --> ok (TODO: triangulate the non-triangular faces
	  v1[0] = curr_mesh->mVertices[curr_face.mIndices[0]].x;
	  v1[1] = curr_mesh->mVertices[curr_face.mIndices[0]].y;
	  v1[2] = curr_mesh->mVertices[curr_face.mIndices[0]].z;
	  v2[0] = curr_mesh->mVertices[curr_face.mIndices[1]].x;
	  v2[1] = curr_mesh->mVertices[curr_face.mIndices[1]].y;
	  v2[2] = curr_mesh->mVertices[curr_face.mIndices[1]].z;
	  v3[0] = curr_mesh->mVertices[curr_face.mIndices[2]].x;
	  v3[1] = curr_mesh->mVertices[curr_face.mIndices[2]].y;
	  v3[2] = curr_mesh->mVertices[curr_face.mIndices[2]].z;
	  
	  // Debug
// 	  std::cout << "Adding triangle: (" << v1[0] << ", " << v1[1] << ", " << v1[2] << ") ";
// 	  std::cout << "(" << v2[0] << ", " << v2[1] << ", " << v2[2] << ") ";
// 	  std::cout << "(" << v3[0] << ", " << v3[1] << ", " << v3[2] << ")" << std::endl;
	  
	  mod->AddTri(v1, v2, v3, k);
	} else if (curr_face.mNumIndices == 4) {
	  v1[0] = curr_mesh->mVertices[curr_face.mIndices[0]].x;
	  v1[1] = curr_mesh->mVertices[curr_face.mIndices[0]].y;
	  v1[2] = curr_mesh->mVertices[curr_face.mIndices[0]].z;
	  v2[0] = curr_mesh->mVertices[curr_face.mIndices[1]].x;
	  v2[1] = curr_mesh->mVertices[curr_face.mIndices[1]].y;
	  v2[2] = curr_mesh->mVertices[curr_face.mIndices[1]].z;
	  v3[0] = curr_mesh->mVertices[curr_face.mIndices[2]].x;
	  v3[1] = curr_mesh->mVertices[curr_face.mIndices[2]].y;
	  v3[2] = curr_mesh->mVertices[curr_face.mIndices[2]].z;
	  v4[0] = curr_mesh->mVertices[curr_face.mIndices[3]].x;
	  v4[1] = curr_mesh->mVertices[curr_face.mIndices[3]].y;
	  v4[2] = curr_mesh->mVertices[curr_face.mIndices[3]].z;
	  mod->AddTri(v1, v2, v3, k);
	  mod->AddTri(v1, v4, v3, k);
// 	  std::cout << "Adding quadrangle: (" << v1[0] << ", " << v1[1] << ", " << v1[2] << ") ";
// 	  std::cout << "(" << v2[0] << ", " << v2[1] << ", " << v2[2] << ") ";
// 	  std::cout << "(" << v3[0] << ", " << v3[1] << ", " << v3[2] << ")" << std::endl;
	} else {
	  std::cerr << "Warning faces with more than 4 edges are not allowed at this moment.\n";
	}
      }
      if (curr_mesh->mNumFaces > 0 && mod->num_tris > 0) {
	mod->EndModel();
	ret_val.push_back(mod);
      }
    }
  }
  
  return ret_val;
}
Ejemplo n.º 2
0
void Convexer::addModels(HACD::HACD *myHACD, std::vector<PQP_Model *> &mod_vec){
  
  
  PQP_REAL v[3][3];
  
  cout << "addModels --> Num clusters: " << myHACD->GetNClusters() << endl;
  
  myHACD->Save("/home/sinosuke/out.wrl", false);
  
  for(size_t c = 0; c < myHACD->GetNClusters(); ++c) {
    cout << "addModels --> cluster " << c << endl;
    PQP_Model *mod = new PQP_Model();
    unsigned int id = 0;
    
    // Get the cluster
    size_t nPoints = myHACD->GetNPointsCH(c);
    size_t nTriangles = myHACD->GetNTrianglesCH(c);
    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);
    
    for (unsigned int i = 0; i < nTriangles; i++) {
//       cout << "addModels --> Adding triangle: " ;
//       
//       for (int j = 0; j < 3; j++) {
// 	cout << "(";
// 	for (int k = 0; k < 3; k++) {
// 	  v[j][k] = pointsCH[trianglesCH[i][j]][k];
// 	  cout << v[j][k] << " ";
// 	}
// 	cout << ") ";
//       }
      mod->AddTri(v[0], v[1], v[2], id);
      
      id++;
    }
    cout <<endl;
    mod->EndModel();
    mod_vec.push_back(mod);
    delete[] pointsCH;
    delete[] trianglesCH;
  }
}
Ejemplo n.º 3
0
planner::Object* CreateObject(const ObjectData& data, const std::vector<Eigen::Vector3d>& dictionaryPoints, const T_PointReplacement &replacement)
{
    //create PQP MODEL
    PQP_Model* m = new PQP_Model;
    std::size_t currentIndex_(0);
    m->BeginModel();
    for(std::vector<ObjectTri>::const_iterator tit = data.triangles_.begin();
        tit != data.triangles_.end(); ++tit)
    {
        PQP_REAL points [3][3];
        for(int i=0; i< 3; ++i)
        {
            Vector3toArray(FindPointIndex(tit->points[i],dictionaryPoints, replacement),points[i]);
        }
        m->AddTri(points[0], points[1], points[2], currentIndex_++);
    }
    m->EndModel();
    return new planner::Object(m,data.normals_, data.name);
}
Ejemplo n.º 4
0
int main(int argc, char **argv)
{
  glutInit(&argc, argv);
  glutInitDisplayMode (GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH | GLUT_MULTISAMPLE);
  glutInitWindowSize(512, 512);

  // create the window

  glutCreateWindow("PQP Demo - Spinning");

  // set OpenGL graphics state -- material props, perspective, etc.

  InitViewerWindow();

  // set the callbacks

  glutDisplayFunc(DisplayCB);
  glutIdleFunc(IdleCB);
  glutMouseFunc(MouseCB);
  glutMotionFunc(MotionCB);  
  glutKeyboardFunc(KeyboardCB);

  // initialize the bunny

  FILE *fp;
  int i, ntris;

  bunny_to_draw = new Model("bunny.tris");

  fp = fopen("bunny.tris","r");
  if (fp == NULL) { fprintf(stderr,"Couldn't open bunny.tris\n"); exit(-1); }
  fscanf(fp,"%d",&ntris);

  bunny.BeginModel();
  for (i = 0; i < ntris; i++)
  {
    double p1x,p1y,p1z,p2x,p2y,p2z,p3x,p3y,p3z;
    fscanf(fp,"%lf %lf %lf %lf %lf %lf %lf %lf %lf", 
           &p1x,&p1y,&p1z,&p2x,&p2y,&p2z,&p3x,&p3y,&p3z);
    PQP_REAL p1[3],p2[3],p3[3];
    p1[0] = (PQP_REAL)p1x; p1[1] = (PQP_REAL)p1y; p1[2] = (PQP_REAL)p1z;
    p2[0] = (PQP_REAL)p2x; p2[1] = (PQP_REAL)p2y; p2[2] = (PQP_REAL)p2z;
    p3[0] = (PQP_REAL)p3x; p3[1] = (PQP_REAL)p3y; p3[2] = (PQP_REAL)p3z;
    bunny.AddTri(p1,p2,p3,i);
  }
  bunny.EndModel();
  fclose(fp);

  // initialize the torus

  torus_to_draw = new Model("torus.tris");

  fp = fopen("torus.tris","r");
  if (fp == NULL) { fprintf(stderr,"Couldn't open torus.tris\n"); exit(-1); }
  fscanf(fp,"%d",&ntris);

  torus.BeginModel();
  for (i = 0; i < ntris; i++)
  {
    double p1x,p1y,p1z,p2x,p2y,p2z,p3x,p3y,p3z;
    fscanf(fp,"%lf %lf %lf %lf %lf %lf %lf %lf %lf", 
           &p1x,&p1y,&p1z,&p2x,&p2y,&p2z,&p3x,&p3y,&p3z);
    PQP_REAL p1[3],p2[3],p3[3];
    p1[0] = (PQP_REAL)p1x; p1[1] = (PQP_REAL)p1y; p1[2] = (PQP_REAL)p1z;
    p2[0] = (PQP_REAL)p2x; p2[1] = (PQP_REAL)p2y; p2[2] = (PQP_REAL)p2z;
    p3[0] = (PQP_REAL)p3x; p3[1] = (PQP_REAL)p3y; p3[2] = (PQP_REAL)p3z;
    torus.AddTri(p1,p2,p3,i);
  }
  torus.EndModel();
  fclose(fp);

  // print instructions

  printf("PQP Demo - Spinning:\n"
         "Press 'q' to quit.\n"
         "Press any other key to toggle animation.\n"
         "Left-drag left & right to change angle of view.\n"
         "Left-drag up & down to change elevation of view.\n"
         "Right-drag up & down to change distance of view.\n");

  // Enter the main loop.

  glutMainLoop();

  return 0;
}
Ejemplo n.º 5
0
double ArizonaTest::getScale2Surface(double *dis)
{
	if (coords.empty() || indices.empty()) {
		DBGA("Unable to compute minimum force; hull not set");
		return 0;
	}
	if(mIsFlipped){
		dis[0] = - dis[0];
		dis[1] = - dis[1];
		dis[2] = - dis[2];
	}

	// compute the necessary force
	PQP_REAL v1[3],v2[3],v3[3];
	PQP_Model volume = PQP_Model();

	//model for the volumn
	volume.BeginModel();
	int triInd = 0;
	int numIndices = indices.size();
	for (int k = 0; k < numIndices - 3; k++) {
		if(indices[k+3] == -1) //  dealing with meaningful vertices.
		{
			v1[0]=coords[indices[k]].x();
			v1[1]=coords[indices[k]].y();
			v1[2]=coords[indices[k]].z();

			v2[0]=coords[indices[k+1]].x();
			v2[1]=coords[indices[k+1]].y();
			v2[2]=coords[indices[k+1]].z();

			v3[0]=coords[indices[k+2]].x();
			v3[1]=coords[indices[k+2]].y();
			v3[2]=coords[indices[k+2]].z();

			volume.AddTri(v1,v2,v3,triInd++);
		}
	}
	volume.EndModel();

	PQP_REAL pt[3];
	pt[0] = (double)(dis[0]*mGrasp->getMaxRadius());
	pt[1] = (double)(dis[1]*mGrasp->getMaxRadius());
	pt[2] = (double)(dis[2]*mGrasp->getMaxRadius());

	vec3 disturbance(pt[0], pt[1], pt[2]);
	double originalLength = disturbance.len();
	double startValue = 0, endValue = originalLength;

	int loops = 0;
	while (!isOutside(&volume, pt) ) {
		pt[0] *= 2; pt[1] *= 2; pt[2] *= 2;
		startValue = endValue;
		endValue *= 2;
		loops ++;
		if (loops > 20) {
			DBGA("Failed to find point outside of volume!!!!!");
			break;
		}
	}

	disturbance = normalise(disturbance);
	double length = binarySearch(disturbance, volume, startValue, endValue);

	return originalLength / length;
}