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);
}
示例#2
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;
}
示例#3
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;
}