Пример #1
0
void Compute_Convex_Hull( const SWIFT_Real* vs, int vn, int*& fs, int& fn )
{
    int i;

    // qhull variables
    int exitcode;
    facetT *facet;
    vertexT *vertex;
    vertexT **vertexp;
    setT *vertices;
    coordT *qhv = (coordT*)malloc( sizeof(coordT)*vn*3 );
    coordT *p = qhv;
    const SWIFT_Real* vsp = vs;

    // Load the coordinates into the vertex array for qhull since SWIFT_Real
    // may not be the same type.
    for( i = 0; i < vn; i++ ) {
        *p++ = *vsp++;
        *p++ = *vsp++;
        *p++ = *vsp++;
    }

    qh_init_A( stdin, stdout, stderr, 0, NULL );
    if( (exitcode = setjmp (qh errexit)) ) exit(exitcode);
    qh_initflags( options );
    qh_init_B( qhv, vn, 3, True );
    qh_qhull();
#ifdef SWIFT_DEBUG
    qh_check_output();
#endif

    fs = new int[((vn<<1) + 4)*3];
    fn = 0;
    FORALLfacets {
        setT *vertices = qh_facet3vertex(facet);
        FOREACHvertex_( vertices ) {
            fs[fn++] = qh_pointid(vertex->point);
        }
        // Swap the face vertex indices back
        i = fs[fn-1]; fs[fn-1] = fs[fn-2]; fs[fn-2] = i;
        qh_settempfree(&vertices);
    }
    //qh NOerrexit = True;
    qh_freeqhull(qh_ALL);

    fn /= 3;
}
Пример #2
0
void
convex_hull_compute(double*         points,
	                  std::size_t     npoints,
	                  point_oiter_t   chull,
	                  tri_id_oiter_t  tri_ids,
	                  std::size_t     tri_id_offset)
{
  qh_init_A(0, 0, stderr, 0, 0);
  qh_init_B(points, int(npoints), int(N), false);
  qh_initflags((char*)"qhull Pp QJ");
  qh_qhull();
  qh_check_output();
  qh_triangulate();

  // set by FORALLfacets:
  facetT *facet;
  // set by FORALLvertices, FOREACHvertex_:
  vertexT *vertex;
  // set by FOREACHvertex_:
  vertexT **vertexp;

  coordT *point, *pointtemp;

  FORALLpoints {
    opoint_t p;
    std::copy(point, point + N, &p[0]);
    *chull = p;
  }

  FORALLfacets {
    setT const* const tri = qh_facet3vertex(facet);
    FOREACHvertex_(tri) {
      *tri_ids = qh_pointid(vertex->point) + (int)tri_id_offset;
    }
  }

  qh_freeqhull(!qh_ALL);

  int curlong, totlong;
  qh_memfreeshort(&curlong, &totlong);

  if(curlong || totlong) throw std::runtime_error("qhull memory was not freed");
}
Пример #3
0
int call_qhull (void) {
  int curlong, totlong, exitcode, numpoints, dim;
  coordT *points;
  boolT ismalloc;
  char *flags;

  qh_init_A (stdin, stdout, stderr, 0, NULL);
  exitcode= setjmp (qh errexit);
  if (!exitcode) {
    strcat (qh rbox_command, "user");            /* for documentation only */
    qh_initflags ("qhull Tc (your flags go here)"); /* 1st word ignored */

    /* define the input data */
    points= array;    		/* an array of point coordinates */
    ismalloc= False; 		/* True if qhull should 'free(points)' at end*/
    dim= 3;         		/* dimension of points */
    numpoints= 100; 		/* number of points */;
    /* For Delaunay triangulation ('d' or 'v'), set "qh PROJECTdelaunay= True" 
       to project points to a paraboloid.  You may project the points yourself.

       For halfspace intersection ('H'), compute the dual point array
       by "points= qh_sethalfspace_all (dim, numpoints, array, feasible);"
    */
    qh_init_B (points, numpoints, dim, ismalloc);

    qh_qhull();       		/* construct the hull */
    /*  For Voronoi centers ('v'), call qh_setvoronoi_all() */
    qh_check_output();     	/* optional */
    qh_produce_output();   	/* optional */
    if (qh VERIFYoutput && !qh STOPpoint && !qh STOPcone)
      qh_check_points();     	/* optional */
    /* exitcode == 0 */
  }
  qh NOerrexit= True;
  qh_freeqhull(!qh_ALL);
  qh_memfreeshort (&curlong, &totlong);
  if (curlong || totlong)  	/* optional */
    fprintf (stderr, "qhull internal warning (main): did not free %d bytes of long memory (%d pieces)\n",
       totlong, curlong);
  return exitcode;
} /* call_qhull */
Пример #4
0
void Compute_Convex_Hull( coordT* vs, int vn, int*& fs, int& fn )
{
    int i;

    // qhull variables
    int exitcode;
    facetT *facet;
    vertexT *vertex;
    vertexT **vertexp;
    setT *vertices;

    qh_init_A( stdin, stdout, stderr, 0, NULL );
    if( (exitcode = setjmp (qh errexit)) ) exit(exitcode);
    qh_initflags( options );
    qh_init_B( vs, vn, 3, True );
    qh_qhull();
#ifdef SWIFT_DEBUG
    qh_check_output();
#endif

    fs = new int[((vn<<1) + 4)*3];
    fn = 0;
    FORALLfacets {
        setT *vertices = qh_facet3vertex(facet);
        FOREACHvertex_( vertices ) {
            fs[fn++] = qh_pointid(vertex->point);
        }
        // Swap the face vertex indices back
        i = fs[fn-1]; fs[fn-1] = fs[fn-2]; fs[fn-2] = i;
        qh_settempfree(&vertices);
    }
    //qh_freeqhull(qh_ALL);
    qh_freeqhull(!qh_ALL);

    int i1, i2;
    qh_memfreeshort(&i1, &i2);

    fn /= 3;
}
Пример #5
0
static PyObject* py_qconvex(PyObject *self, PyObject *args) {
    const char *arg;
    const char *data;
    int curlong, totlong; /* used !qh_NOmem */
    int exitcode, numpoints, dim;
    coordT *points;
    boolT ismalloc;
    char* argv[10];
    int argc = 1;
    char *rest;
    char *token;
    /* Defensively copy the string first */
    char tempstr[30];
    char* ptr;
    char *bp;
    size_t size;
    FILE* fin;
    FILE* fout;

    if (!PyArg_ParseTuple(args, "ss", &arg, &data))
        return NULL;

    strcpy(tempstr, arg);
    ptr = tempstr;

    while(token = strtok_r(ptr, " ", &rest)) {
        argv[argc] = token;
        ptr = rest;
        argc  += 1;
    }
    argv[0] = "qconvex";

    /* Because qhull uses stdin and stdout streams for io, we need to create
    FILE* stream to simulate these io streams.*/
    fin = fmemopen(data, strlen(data), "r");
    fout = open_memstream(&bp, &size);

    if ((fin != NULL) && (fout != NULL))
    {
        /* Now do the usual qhull code (modified from qconvex.c). */
        qh_init_A(fin, fout, stderr, argc, argv);

        exitcode= setjmp(qh errexit);
        if (!exitcode) {
            qh_checkflags(qh qhull_command, hidden_options);
            qh_initflags(qh qhull_command);
            points= qh_readpoints(&numpoints, &dim, &ismalloc);
        if (dim >= 5) {
            qh_option("Qxact_merge", NULL, NULL);
            qh MERGEexact= True;
        }
        qh_init_B(points, numpoints, dim, ismalloc);
        qh_qhull();
        qh_check_output();
        qh_produce_output();
        if (qh VERIFYoutput && !qh FORCEoutput && !qh STOPpoint && !qh STOPcone)
            qh_check_points();
        exitcode= qh_ERRnone;
        }
        qh NOerrexit= True;  /* no more setjmp */
        #ifdef qh_NOmem
            qh_freeqhull( True);
        #else
            qh_freeqhull( False);
            qh_memfreeshort(&curlong, &totlong);
            if (curlong || totlong)
                fprintf(stderr, "qhull internal warning (main): did not free %d bytes of long memory(%d pieces)\n", totlong, curlong);
        #endif
        fclose(fin);
        fclose(fout);
        return Py_BuildValue("s", bp);
    }
    else
    {
        return NULL;
    }
}
Пример #6
0
void 
CollisionInterface::replaceContactSetWithPerimeter(ContactReport &contactSet)
{
	if (contactSet.size() < 2) return;
	
	double my_resabs = 1.0e-1;

	// first check for a colinear point set
	vec3 currLine, testLine;
	ContactReport::iterator endPt1 = contactSet.begin();
	ContactReport::iterator endPt2 = ++contactSet.begin();
	ContactReport::iterator cp;
	for (cp=contactSet.begin();cp!=contactSet.end();cp++) {    
		currLine = endPt2->b1_pos - endPt1->b1_pos;
		testLine = cp->b1_pos - endPt1->b1_pos;
		vec3 crossProd = testLine * currLine;
		if ( crossProd.len() > my_resabs) break;  // not colinear
		double dot = testLine % currLine;
		if (dot < 0) endPt1 = cp;
		if (dot > currLine % currLine) endPt2 = cp;
	}

	if (cp==contactSet.end()) {  
		// colinear points
		ContactReport tmpSet;
		tmpSet.push_back(*endPt1);
		tmpSet.push_back(*endPt2);
		contactSet.clear();
		contactSet = tmpSet;
		return;
	}

	// compute the origin of the projection frame
	vec3 contactNormal = contactSet.begin()->b1_normal;
	vec3 normal = normalise(testLine * currLine);
	double Soffset = contactSet.begin()->b1_pos % normal;
	vec3 origin_pr = Soffset * normal;

	// compute 2 other axes along the plane of S
	vec3 axis1 = normalise(testLine);
	vec3 axis2 = normal * axis1;

	coordT *array = new coordT[contactSet.size()*2];
	coordT *ptr = &array[0];
	volatile int ptCount = 0;
	for (cp=contactSet.begin(); cp!=contactSet.end(); cp++) {    
		*ptr++ = (cp->b1_pos - position::ORIGIN) % axis1;
		*ptr++ = (cp->b1_pos - position::ORIGIN) % axis2;
		ptCount++;
	}

	ContactReport tmpSet = contactSet;
	contactSet.clear();

	//qhull paramerers
	int exitcode,curlong,totlong;
	char options[200];

	//serialize access to qhull which is not thread-safe
	qhull_mutex.lock();

	bool ismalloc = False; 	// True if qh_freeqhull should 'free(array)'
	FILE *qhfp = fopen("logfile","w");
	if (!qhfp) {
		fprintf(stderr,"Could not open qhull logfile!\n");
		qh_init_A(NULL, stdout, stderr, 0, NULL);
	} else {
		qh_init_A(NULL, qhfp, qhfp, 0, NULL);
	}

	if((exitcode = setjmp(qh errexit))) {
		delete [] array;
		if (qhfp) fclose(qhfp);
		qhull_mutex.unlock();
		return;
	}

	sprintf(options, "qhull n Pp");
	try {
		qh_initflags(options);
		qh_init_B(&array[0],ptCount, 2, ismalloc);
		qh_qhull();
		qh_check_output();
	} catch(...) {
		//qhull has failed
		DBGA("QHull CompactSet failed!!!");
		//reinsert all original contacts
		contactSet.insert(contactSet.begin(), tmpSet.begin(), tmpSet.begin());
		delete [] array;
		fclose(qhfp);
		qhull_mutex.unlock();
		return;
	}
	fclose(qhfp);

	vertexT *vertex;
	double x,y;
	ContactData tmpContact;

	// keep only those vertices in the set that match the convex hull vertices
	FORALLvertices {
		x = vertex->point[0];
		y = vertex->point[1];
		tmpContact.b1_pos[0] = x * axis1[0] + y * axis2[0] + origin_pr[0];
		tmpContact.b1_pos[1] = x * axis1[1] + y * axis2[1] + origin_pr[1];
		tmpContact.b1_pos[2] = x * axis1[2] + y * axis2[2] + origin_pr[2];
		tmpContact.b1_normal = contactNormal;

		for (cp=tmpSet.begin(); cp!=tmpSet.end(); cp++) {
			if (fabs(cp->b1_pos[0] - tmpContact.b1_pos[0]) < my_resabs &&
				fabs(cp->b1_pos[1] - tmpContact.b1_pos[1]) < my_resabs &&
				fabs(cp->b1_pos[2] - tmpContact.b1_pos[2]) < my_resabs &&
				fabs(cp->b1_normal[0] - tmpContact.b1_normal[0]) < my_resabs &&
				fabs(cp->b1_normal[1] - tmpContact.b1_normal[1]) < my_resabs &&
				fabs(cp->b1_normal[2] - tmpContact.b1_normal[2]) < my_resabs)
				break;
		}
		if (cp==tmpSet.end()) {
		} else {
			contactSet.push_back(*cp);
		}
	}

	qh NOerrexit= True;
	qh_freeqhull(!qh_ALL);
	qh_memfreeshort (&curlong, &totlong);
	qhull_mutex.unlock();
	delete [] array;
}
/*! Uses the \a projCoords and \a fixedCoordSet (which identifies the indices
	of the fixed coordinates) to project the 6D hyperplanes of the Grasp Wrench 
	Space into 3D. The \a projCoords is an array of 6 values, but only 3 are used.  
	It then calls qhull to perform a halfspace intersection to get the vertices 
	of the 3D volume.  These vertices are stored in \a hullCoords, and indices 
	of the individual faces that make up the volume are stored in \a hullIndices 
	(an Indexed Face Set).
*/
int
GWS::projectTo3D(double *projCoords, std::set<int> fixedCoordSet,
				 std::vector<position> &hullCoords, std::vector<int> &hullIndices)
{
  int i,j,k,validPlanes,numCoords,numInLoop;
  double **planes;
  int freeCoord[3],fixedCoord[3];

  // qhull variables
  boolT ismalloc;
  int curlong,totlong,exitcode;
  char options[200];  
  facetT *facet;

  if (numHyperPlanes == 0) {
	  DBGP("No hyperplanes");
	  return SUCCESS;
  }

  planes = (double **) malloc(numHyperPlanes * sizeof(double *));
  if (!planes) {
#ifdef GRASPITDBG
    pr_error("GWS::ProjectTo3D,Out of memory allocating planes array");
    printf("NumHyperplanes: %d\n",numHyperPlanes);
#endif
    return FAILURE;
  }

  validPlanes = 0;

  // determine which dimensions are free and which are fixed
  // the set keeps things ordered
  for (i=0,j=0,k=0;i<6;i++) {
    if (fixedCoordSet.find(i) == fixedCoordSet.end()) {
      freeCoord[k++] = i;
	} else {
      fixedCoord[j++] = i;
	}
  }

  // project the hyperplanes to three dimensional planes
  for (i=0;i<numHyperPlanes;i++) {
    double len = sqrt(hyperPlanes[i][freeCoord[0]]*hyperPlanes[i][freeCoord[0]] +
                      hyperPlanes[i][freeCoord[1]]*hyperPlanes[i][freeCoord[1]] +
                      hyperPlanes[i][freeCoord[2]]*hyperPlanes[i][freeCoord[2]]);

    if (len>1e-11) {
      planes[validPlanes] = (double *) malloc(4 * sizeof(double));
      if (!planes[validPlanes]) {
		pr_error("Out of memory allocating planes array");
	    DBGP("Out of memory allocating planes array");
		return FAILURE;
	  }
      planes[validPlanes][0] = hyperPlanes[i][freeCoord[0]]/len;
      planes[validPlanes][1] = hyperPlanes[i][freeCoord[1]]/len;
      planes[validPlanes][2] = hyperPlanes[i][freeCoord[2]]/len;
      planes[validPlanes][3] = (hyperPlanes[i][6] + 
	                            hyperPlanes[i][fixedCoord[0]]*projCoords[fixedCoord[0]] +
		                        hyperPlanes[i][fixedCoord[1]]*projCoords[fixedCoord[1]] +
		                        hyperPlanes[i][fixedCoord[2]]*projCoords[fixedCoord[2]])/len;
      
      validPlanes++;
	}
  }

  if (validPlanes<numHyperPlanes) {
	  DBGP("Ignored " << numHyperPlanes-validPlanes << 
		   " hyperplanes which did not intersect this 3-space");
  }
  if (!validPlanes) {
	  DBGA("No valid planes in 3D projection!");
	  return FAILURE;
  }
	   

  //
  // call qhull to do the halfspace intersection
  //
  coordT *array = new coordT[validPlanes*3];
  coordT *p = &array[0];
  
  boolT zerodiv;
  coordT *point, *normp, *coordp, **pointp, *feasiblep;
  vertexT *vertex, **vertexp;

#ifdef GRASPITDBG
  printf("Calling qhull to perform a 3D halfspace intersection of %d planes...\n",validPlanes);
#endif

  ismalloc = False; 	// True if qh_freeqhull should 'free(array)'

  // I want to get rid of this but qh_init needs some sort of file pointer
  // for stdout and stderr
  FILE *qhfp = fopen("logfile","w");

  if (!qhfp) {
	fprintf(stderr,"Could not open qhull logfile!\n");
	qh_init_A(NULL, stdout, stderr, 0, NULL);
  }
  else
   qh_init_A(NULL, qhfp, qhfp, 0, NULL);

  if ((exitcode = setjmp(qh errexit))) {
    delete [] array;
	qh NOerrexit= True;
	qh_freeqhull(!qh_ALL);
	qh_memfreeshort (&curlong, &totlong);
	if (curlong || totlong)  	/* optional */
	   fprintf (stderr, "qhull internal warning (main): did not free %d bytes of long memory (%d pieces)\n",
		 totlong, curlong);
    for (i=0;i<validPlanes;i++)
      free(planes[i]);
    free(planes);
	if (qhfp) fclose(qhfp);
	DBGP("Qhull forces the exit; probably no valid intersection");
    return FAILURE; //exit(exitcode);
  }
  sprintf(options, "qhull -H0,0,0 Pp");
  qh_initflags(options);
  qh_setfeasible(3);
  if (!(qh feasible_point)) printf("why is qh_qh NULL?\n");
  for(i=0;i<validPlanes;i++) {
    qh_sethalfspace (3, p, &p, planes[i],&(planes[i][3]), qh feasible_point);
  }

  qh_init_B(&array[0], validPlanes, 3, ismalloc);
  qh_qhull();
  qh_check_output();
  if (qhfp) fclose(qhfp);

  //
  // Collect the vertices of the volume
  //
  hullCoords.clear();
  numCoords = qh num_facets;
  hullCoords.reserve(numCoords);
  int *indices = new int[numCoords];

  double scale = grasp->getMaxRadius(); // Hmm, is this right?

  point= (pointT*)qh_memalloc (qh normal_size);

  FORALLfacets {
    coordp = point;
	if (facet->offset > 0)
      goto LABELprintinfinite;
	
    normp= facet->normal;
    feasiblep= qh feasible_point;
    if (facet->offset < -qh MINdenom) {
      for (k= qh hull_dim; k--; )
        *(coordp++)= (*(normp++) / - facet->offset) + *(feasiblep++);
    }else {
      for (k= qh hull_dim; k--; ) {
        *(coordp++)= qh_divzero (*(normp++), facet->offset, qh MINdenom_1,&zerodiv) + *(feasiblep++);
        if (zerodiv) {
          goto LABELprintinfinite;
        }
      }
    }    
    hullCoords.push_back(position(point[0]*scale,point[1]*scale,
				  point[2]*scale));
    continue;
    LABELprintinfinite:
    hullCoords.push_back(position(qh_INFINITE,qh_INFINITE,qh_INFINITE));
    fprintf(stderr,"intersection at infinity!\n");
  }
  qh_memfree (point, qh normal_size);


  //
  // use adjacency information to build faces of the volume
  //
  double dot;
  vec3 testNormal, refNormal;

  int numfacets, numsimplicial, numridges, totneighbors, numneighbors, numcoplanars;
  setT *vertices, *vertex_points, *coplanar_points;
  int numpoints= qh num_points + qh_setsize (qh other_points);
  int vertex_i, vertex_n;
  facetT *neighbor, **neighborp;
  int unused_numnumtricoplanarsp; //added because countfacets takes more arguments in qhull 2012
								  //FIXME - understand what this argument does. 
  qh_countfacets (qh facet_list, NULL, !qh_ALL, &numfacets, &numsimplicial, 
      &totneighbors, &numridges, &numcoplanars, &unused_numnumtricoplanarsp);  /* sets facet->visitid */

  qh_vertexneighbors();
  vertices= qh_facetvertices (qh facet_list, NULL, !qh_ALL);
  vertex_points= qh_settemp (numpoints);
  coplanar_points= qh_settemp (numpoints);
  qh_setzero (vertex_points, 0, numpoints);
  qh_setzero (coplanar_points, 0, numpoints);
  FOREACHvertex_(vertices)
    qh_point_add (vertex_points, vertex->point, vertex);
  FORALLfacet_(qh facet_list) {
    FOREACHpoint_(facet->coplanarset)
      qh_point_add (coplanar_points, point, facet);
  }