int main(int argc, char **argv) {
int *intarray= NULL;
int numInts;
int checkEvery= MAXint;
int curlong, totlong;
int traceLevel= 4; /* 4 normally, no tracing since qset does not log. 5 for memory tracing */
readOptions(argc, argv, prompt, &numInts, &checkEvery, &traceLevel);
setupMemory(traceLevel, numInts, &intarray);
testSetappendSettruncate(numInts, intarray, checkEvery);
testSetdelSetadd(numInts, intarray, checkEvery);
testSetappendSet(numInts, intarray, checkEvery);
testSetcompactCopy(numInts, intarray, checkEvery);
testSetequalInEtc(numInts, intarray, checkEvery);
testSettemp(numInts, intarray, checkEvery);
testSetlastEtc(numInts, intarray, checkEvery);
testSetdelsortedEtc(numInts, intarray, checkEvery);
printf("\n\nNot testing qh_setduplicate and qh_setfree2.\n These routines use heap-allocated set contents. See qhull tests.\n");
qh_memstatistics(stdout);
qh_memfreeshort(&curlong, &totlong);
if (curlong || totlong){
qh_fprintf(stderr, 8043, "qh_memfreeshort: did not free %d bytes of long memory(%d pieces)\n", totlong, curlong);
error_count++;
}
if(error_count){
qh_fprintf(stderr, 8012, "testqset: %d errors\n\n", error_count);
exit(1);
}else{
printf("testqset: OK\n\n");
}
return 0;
}/*main*/
void compute_convex_hull(void)
{
int dim; /* dimension of points */
int numpoints; /* number of points */
coordT *points; /* array of coordinates for each point */
boolT ismalloc; /* True if qhull should free points in qh_freeqhull() or reallocation */
char flags[]= "qhull Tv"; /* option flags for qhull, see qh_opt.htm */
FILE *outfile= stdout; /* output from qh_produce_output()
use NULL to skip qh_produce_output() */
FILE *errfile= stderr; /* error messages from qhull code */
int exitcode; /* 0 if no error from qhull */
facetT *facet; /* set by FORALLfacets */
int curlong, totlong; /* memory remaining after qh_memfreeshort */
/* initialize dim, numpoints, points[], ismalloc here */
exitcode= qh_new_qhull(dim, numpoints, points, ismalloc,
flags, outfile, errfile);
if (!exitcode) { /* if no error */
/* 'qh facet_list' contains the convex hull */
FORALLfacets {
/* ... your code ... */
}
}
qh_freeqhull(!qh_ALL);
qh_memfreeshort(&curlong, &totlong);
if (curlong || totlong)
fprintf(errfile, "qhull internal warning (main): did not free %d bytes of long memory(%d pieces)\n",
totlong, curlong);
};
/**
* Frees buffers associated to the Delaunay triangulation.
*/
int qhull_free_delaunay_()
{
int curlong, totlong; /* mem remaining after qh_memfreeshort */
qh_freeqhull(!qh_ALL); /* long mem */
qh_memfreeshort (&curlong, &totlong); /* short mem/mem allocator */
if (curlong || totlong) {
qhull_warn("did not free %d bytes of long memory (%d pieces)\n",
totlong, curlong);
}
free_simplex_facet_array();
free_barycentric_transforms();
return curlong || totlong;
}
//! Check memory for internal consistency
//! Free global memory used by qh_initbuild and qh_buildhull
//! Zero the qhT data structure, except for memory (qhmemT) and statistics (qhstatT)
//! Check and free short memory (e.g., facetT)
//! Zero the qhmemT data structure
void QhullQh::
checkAndFreeQhullMemory()
{
#ifdef qh_NOmem
qh_freeqhull(this, qh_ALL);
#else
qh_memcheck(this);
qh_freeqhull(this, !qh_ALL);
countT curlong;
countT totlong;
qh_memfreeshort(this, &curlong, &totlong);
if (curlong || totlong)
throw QhullError(10026, "Qhull error: qhull did not free %d bytes of long memory (%d pieces).", totlong, curlong);
#endif
}//checkAndFreeQhullMemory
bool QhullCalc::Calc(
const std::vector<v3d> &pin,
std::vector<v3d> &pout,
std::vector<std::vector<int> > &fout)const
{
if(pin.size() < 4)
return false;
try
{
char cmd[]="qhull ";
if(qh_new_qhull(3,int(pin.size()),const_cast<double*>(&pin[0].x),false,cmd,NULL,NULL))
return false;
pointT *point, *pointtemp;
FORALLpoints
{
pout.push_back(v3d(point[0],point[1],point[2]));
}
facetT *facet;
vertexT *vertex, **vertexp;
FORALLfacets
{
std::vector<int> idx;
setT *vertices=qh_facet3vertex(facet);
qh_setsize(vertices);
FOREACHvertex_(vertices)
{
idx.push_back(qh_pointid(vertex->point));
}
fout.push_back(idx);
qh_settempfree(&vertices);
}
qh_freeqhull(!qh_ALL);
int curlong, totlong;
qh_memfreeshort(&curlong, &totlong);
if(curlong || totlong)
{
return false;
}
return true;
}
catch(...)
{
return false;
}
}
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");
}
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 */
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;
}
RTC::ReturnCode_t setupCollisionModel(hrp::BodyPtr m_robot, const char *url, OpenHRP::BodyInfo_var binfo) {
// do qhull
OpenHRP::ShapeInfoSequence_var sis = binfo->shapes();
OpenHRP::LinkInfoSequence_var lis = binfo->links();
for(int i = 0; i < m_robot->numLinks(); i++ ) {
const OpenHRP::LinkInfo& i_li = lis[i];
const OpenHRP::TransformedShapeIndexSequence& tsis = i_li.shapeIndices;
// setup
int numTriangles = 0;
for (unsigned int l=0; l<tsis.length(); l++) {
OpenHRP::ShapeInfo& si = sis[tsis[l].shapeIndex];
const OpenHRP::LongSequence& triangles = si.triangles;
numTriangles += triangles.length();
}
double points[numTriangles*3];
int points_i = 0;
hrp::Matrix44 Rs44; // inv
hrp::Matrix33 Rs33 = m_robot->link(i)->Rs;
Rs44 << Rs33(0,0),Rs33(0,1), Rs33(0,2), 0,
Rs33(1,0),Rs33(1,1), Rs33(1,2), 0,
Rs33(2,0),Rs33(2,1), Rs33(2,2), 0,
0.0, 0.0, 0.0, 1.0;
for (unsigned int l=0; l<tsis.length(); l++) {
const OpenHRP::DblArray12& M = tsis[l].transformMatrix;
hrp::Matrix44 T0;
T0 << M[0], M[1], M[2], M[3],
M[4], M[5], M[6], M[7],
M[8], M[9], M[10], M[11],
0.0, 0.0, 0.0, 1.0;
hrp::Matrix44 T(Rs44 * T0);
const OpenHRP::ShapeInfo& si = sis[tsis[l].shapeIndex];
const OpenHRP::LongSequence& triangles = si.triangles;
const float *vertices = si.vertices.get_buffer();
for(unsigned int j=0; j < triangles.length() / 3; ++j){
for(int k=0; k < 3; ++k){
long orgVertexIndex = si.triangles[j * 3 + k];
int p = orgVertexIndex * 3;
hrp::Vector4 v(T * hrp::Vector4(vertices[p+0], vertices[p+1], vertices[p+2], 1.0));
points[points_i++] = v[0];
points[points_i++] = v[1];
points[points_i++] = v[2];
}
}
}
hrp::ColdetModelPtr coldetModel(new hrp::ColdetModel());
coldetModel->setName(std::string(i_li.name));
// qhull
int vertexIndex = 0;
int triangleIndex = 0;
int num = 0;
char flags[250];
boolT ismalloc = False;
sprintf(flags,"qhull Qt Tc");
if (! qh_new_qhull (3,numTriangles,points,ismalloc,flags,NULL,stderr) ) {
qh_triangulate();
qh_vertexneighbors();
vertexT *vertex,**vertexp;
coldetModel->setNumVertices(qh num_vertices);
coldetModel->setNumTriangles(qh num_facets);
int index[qh num_vertices];
FORALLvertices {
int p = qh_pointid(vertex->point);
index[p] = vertexIndex;
coldetModel->setVertex(vertexIndex++, points[p*3+0], points[p*3+1], points[p*3+2]);
}
facetT *facet;
num = qh num_facets;;
{
FORALLfacets {
int j = 0, p[3];
setT *vertices = qh_facet3vertex (facet);
FOREACHvertexreverse12_ (vertices) {
if (j<3) {
p[j] = index[qh_pointid(vertex->point)];
} else {
fprintf(stderr, "extra vertex %d\n",j);
}
j++;
}
coldetModel->setTriangle(triangleIndex++, p[0], p[1], p[2]);
}
}
} // qh_new_qhull
coldetModel->build();
m_robot->link(i)->coldetModel = coldetModel;
qh_freeqhull(!qh_ALL);
int curlong, totlong;
qh_memfreeshort (&curlong, &totlong);
if (curlong || totlong) {
fprintf(stderr, "convhulln: did not free %d bytes of long memory (%d pieces)\n", totlong, curlong);
}
//
std::cerr << std::setw(16) << i_li.name << " reduce triangles from " << std::setw(5) << numTriangles << " to " << std::setw(4) << num << std::endl;
}
/* Delaunay implementation methyod. If hide_qhull_errors is 1 then qhull error
* messages are discarded; if it is 0 then they are written to stderr. */
static PyObject*
delaunay_impl(int npoints, const double* x, const double* y,
int hide_qhull_errors)
{
coordT* points = NULL;
facetT* facet;
int i, ntri, max_facet_id;
FILE* error_file = NULL; /* qhull expects a FILE* to write errors to. */
int exitcode; /* Value returned from qh_new_qhull(). */
int* tri_indices = NULL; /* Maps qhull facet id to triangle index. */
int indices[3];
int curlong, totlong; /* Memory remaining after qh_memfreeshort. */
PyObject* tuple; /* Return tuple (triangles, neighbors). */
const int ndim = 2;
npy_intp dims[2];
PyArrayObject* triangles = NULL;
PyArrayObject* neighbors = NULL;
int* triangles_ptr;
int* neighbors_ptr;
/* Allocate points. */
points = (coordT*)malloc(npoints*ndim*sizeof(coordT));
if (points == NULL) {
PyErr_SetString(PyExc_MemoryError,
"Could not allocate points array in qhull.delaunay");
goto error_before_qhull;
}
/* Prepare points array to pass to qhull. */
for (i = 0; i < npoints; ++i) {
points[2*i ] = x[i];
points[2*i+1] = y[i];
}
/* qhull expects a FILE* to write errors to. */
if (hide_qhull_errors) {
/* qhull errors are ignored by writing to OS-equivalent of /dev/null.
* Rather than have OS-specific code here, instead it is determined by
* setupext.py and passed in via the macro MPL_DEVNULL. */
error_file = fopen(STRINGIFY(MPL_DEVNULL), "w");
if (error_file == NULL) {
PyErr_SetString(PyExc_RuntimeError,
"Could not open devnull in qhull.delaunay");
goto error_before_qhull;
}
}
else {
/* qhull errors written to stderr. */
error_file = stderr;
}
/* Perform Delaunay triangulation. */
exitcode = qh_new_qhull(ndim, npoints, points, False,
"qhull d Qt Qbb Qc Qz", NULL, error_file);
if (exitcode != qh_ERRnone) {
PyErr_Format(PyExc_RuntimeError,
"Error in qhull Delaunay triangulation calculation: %s (exitcode=%d)%s",
qhull_error_msg[exitcode], exitcode,
hide_qhull_errors ? "; use python verbose option (-v) to see original qhull error." : "");
goto error;
}
/* Split facets so that they only have 3 points each. */
qh_triangulate();
/* Determine ntri and max_facet_id.
Note that libqhull uses macros to iterate through collections. */
ntri = 0;
FORALLfacets {
if (!facet->upperdelaunay)
++ntri;
}
max_facet_id = qh facet_id - 1;
/* Create array to map facet id to triangle index. */
tri_indices = (int*)malloc((max_facet_id+1)*sizeof(int));
if (tri_indices == NULL) {
PyErr_SetString(PyExc_MemoryError,
"Could not allocate triangle map in qhull.delaunay");
goto error;
}
/* Allocate python arrays to return. */
dims[0] = ntri;
dims[1] = 3;
triangles = (PyArrayObject*)PyArray_SimpleNew(ndim, dims, NPY_INT);
if (triangles == NULL) {
PyErr_SetString(PyExc_MemoryError,
"Could not allocate triangles array in qhull.delaunay");
goto error;
}
neighbors = (PyArrayObject*)PyArray_SimpleNew(ndim, dims, NPY_INT);
if (neighbors == NULL) {
PyErr_SetString(PyExc_MemoryError,
"Could not allocate neighbors array in qhull.delaunay");
goto error;
}
triangles_ptr = (int*)PyArray_DATA(triangles);
neighbors_ptr = (int*)PyArray_DATA(neighbors);
/* Determine triangles array and set tri_indices array. */
i = 0;
FORALLfacets {
if (!facet->upperdelaunay) {
tri_indices[facet->id] = i++;
get_facet_vertices(facet, indices);
*triangles_ptr++ = (facet->toporient ? indices[0] : indices[2]);
*triangles_ptr++ = indices[1];
*triangles_ptr++ = (facet->toporient ? indices[2] : indices[0]);
}
else
tri_indices[facet->id] = -1;
}
/* Determine neighbors array. */
FORALLfacets {
if (!facet->upperdelaunay) {
get_facet_neighbours(facet, tri_indices, indices);
*neighbors_ptr++ = (facet->toporient ? indices[2] : indices[0]);
*neighbors_ptr++ = (facet->toporient ? indices[0] : indices[2]);
*neighbors_ptr++ = indices[1];
}
}
/* Clean up. */
qh_freeqhull(!qh_ALL);
qh_memfreeshort(&curlong, &totlong);
if (curlong || totlong)
PyErr_WarnEx(PyExc_RuntimeWarning,
"Qhull could not free all allocated memory", 1);
if (hide_qhull_errors)
fclose(error_file);
free(tri_indices);
free(points);
tuple = PyTuple_New(2);
PyTuple_SetItem(tuple, 0, (PyObject*)triangles);
PyTuple_SetItem(tuple, 1, (PyObject*)neighbors);
return tuple;
error:
/* Clean up. */
Py_XDECREF(triangles);
Py_XDECREF(neighbors);
qh_freeqhull(!qh_ALL);
qh_memfreeshort(&curlong, &totlong);
/* Don't bother checking curlong and totlong as raising error anyway. */
if (hide_qhull_errors)
fclose(error_file);
free(tri_indices);
error_before_qhull:
free(points);
return NULL;
}
/*! 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);
}
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;
}
}
JNIEXPORT jboolean JNICALL Java_qhull_QHull_voronoi_1
(JNIEnv *env, jclass jc, jobject jresult, jobjectArray points)
{
jclass vresultsCls=(*env)->GetObjectClass(env,jresult);
jint np=(*env)->GetArrayLength(env,points);
//Convert points to pt_array, suitable for input
double pt_array[np*3];
int dim=3;
for(int i=0;i<np;i++)
{
jobject pv=(*env)->GetObjectArrayElement(env,points,i);
jclass vecclass=(*env)->GetObjectClass(env,pv);
jfieldID fieldX=(*env)->GetFieldID(env, vecclass, "x", "D");
jfieldID fieldY=(*env)->GetFieldID(env, vecclass, "y", "D");
jfieldID fieldZ=(*env)->GetFieldID(env, vecclass, "z", "D");
pt_array[i*3+0]=(*env)->GetDoubleField(env,pv,fieldX);
pt_array[i*3+1]=(*env)->GetDoubleField(env,pv,fieldY);
pt_array[i*3+2]=(*env)->GetDoubleField(env,pv,fieldZ);
}
char options[]="";
char flags[250];
sprintf (flags, "qhull v Fv T0 %s", options);
////////////////////////////////
int ismalloc = false;
FILE *outfile = stdout;
// FILE *errfile = stderr;
FILE *errfile = 0;
if(!qh_new_qhull (dim, np, pt_array, ismalloc, flags, outfile, errfile))
{
facetT *facet;
vertexT *vertex;
int n = 1, k = 0, m = 0, fidx = 0, j = 0;//, r = 0;
int ni[np];
for (int i = 0; i < np; i++) ni[i] = 0;
qh_setvoronoi_all ();
return JNI_FALSE; ///////
int infinity_seen = false;
facetT *neighbor, **neighborp;
coordT *voronoi_vertex;
FORALLfacets
{
facet->seen = false;
}
FORALLvertices
{
if (qh hull_dim == 3)
qh_order_vertexneighbors (vertex);
infinity_seen = false;
FOREACHneighbor_ (vertex)
{
if (! neighbor->upperdelaunay)
{
if (! neighbor->seen)
{
n++;
neighbor->seen = true;
}
ni[k]++;
}
else if (! infinity_seen)
{
infinity_seen = true;
ni[k]++;
}
}
k++;
}
double v[n][dim];
// Matrix v (n, dim);
for (int d = 0; d < dim; d++)
v[0][d] = 99999999; //infinity
int AtInf[np]; //need to malloc
for (int i = 0; i < np; i++)
AtInf[i] = 0;
jobject *faceArray=NULL;
int faceArraySize=0;
// octave_value_list F;
k = 0;
int newi = 0;
FORALLfacets
{
facet->seen = false;
}
FORALLvertices
{
if (qh hull_dim == 3)
qh_order_vertexneighbors(vertex);
infinity_seen = false;
jintArray facet_list=(*env)->NewIntArray(env, ni[k++]);
m = 0;
FOREACHneighbor_(vertex)
{
if (neighbor->upperdelaunay)
{
if (! infinity_seen)
{
infinity_seen = true;
jint one=1;
(*env)->SetIntArrayRegion(env, facet_list, m++, 1, (jint *)(&one));
// m++;
// facet_list(m++) = 1;
AtInf[j] = true;
}
}
else
{
if(!neighbor->seen)
{
voronoi_vertex = neighbor->center;
fidx = neighbor->id;
newi++;
for (int d = 0; d < dim; d++)
{
v[newi][d] = *(voronoi_vertex++);
}
neighbor->seen = true;
neighbor->visitid = newi;
}
jint pos=neighbor->visitid + 1;
(*env)->SetIntArrayRegion(env, facet_list, m++, 1, (jint *)(&pos));
// facet_list(m++) = neighbor->visitid + 1;
}
}
faceArraySize++;
faceArray=realloc(faceArray,sizeof(jobject*)*faceArraySize);
faceArray[faceArraySize-1]=facet_list;
// F(r++) = facet_list;
j++;
}
// *outv=v;
// *outC=C;
// *outAtInf=AtInf;
//Convert C faces *[I to [[I
jclass intArrCls = (*env)->FindClass(env,"[I") ;
jobjectArray faceJavaArray=(*env)->NewObjectArray(env, faceArraySize, intArrCls, NULL);
for(int i=0;i<faceArraySize;i++)
(*env)->SetObjectArrayElement(env, faceJavaArray, faceArraySize, faceArray[i]);
free(faceArray);
//Convert *vertex to [Vector3d
//TODO
// jobject pv=(*env)->GetObjectArrayElement(env,points,i);
// pt_array[i*3+0]=(*env)->GetDoubleField(env,pv,fieldX);
//Set result field faces
jfieldID fieldFaces=(*env)->GetFieldID(env, vresultsCls, "faces", "[[I");
(*env)->SetObjectField(env, jresult, fieldFaces, faceJavaArray);
//Set result field vector
jfieldID fieldVectors=(*env)->GetFieldID(env, vresultsCls, "vectors", "[javax/vecmath/Vector3d");
// free long memory
qh_freeqhull (! qh_ALL);
// free short memory and memory allocator
int curlong, totlong;
qh_memfreeshort (&curlong, &totlong);
if (curlong || totlong)
printf ("voronoi: did not free %d bytes of long memory (%d pieces)", totlong, curlong);
return JNI_TRUE;
}
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;
}
template <typename PointInT> void
pcl::ConvexHull<PointInT>::performReconstruction (PointCloud &hull, std::vector<pcl::Vertices> &polygons,
bool fill_polygon_data)
{
// FInd the principal directions
EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
Eigen::Vector4f xyz_centroid;
compute3DCentroid (*input_, *indices_, xyz_centroid);
computeCovarianceMatrix (*input_, *indices_, xyz_centroid, covariance_matrix);
EIGEN_ALIGN16 Eigen::Vector3f eigen_values;
EIGEN_ALIGN16 Eigen::Matrix3f eigen_vectors;
pcl::eigen33 (covariance_matrix, eigen_vectors, eigen_values);
Eigen::Affine3f transform1;
transform1.setIdentity ();
int dim = 3;
if (eigen_values[0] / eigen_values[2] < 1.0e-5)
{
// We have points laying on a plane, using 2d convex hull
// Compute transformation bring eigen_vectors.col(i) to z-axis
eigen_vectors.col (2) = eigen_vectors.col (0).cross (eigen_vectors.col (1));
eigen_vectors.col (1) = eigen_vectors.col (2).cross (eigen_vectors.col (0));
transform1 (0, 2) = eigen_vectors (0, 0);
transform1 (1, 2) = eigen_vectors (1, 0);
transform1 (2, 2) = eigen_vectors (2, 0);
transform1 (0, 1) = eigen_vectors (0, 1);
transform1 (1, 1) = eigen_vectors (1, 1);
transform1 (2, 1) = eigen_vectors (2, 1);
transform1 (0, 0) = eigen_vectors (0, 2);
transform1 (1, 0) = eigen_vectors (1, 2);
transform1 (2, 0) = eigen_vectors (2, 2);
transform1 = transform1.inverse ();
dim = 2;
}
else
transform1.setIdentity ();
PointCloud cloud_transformed;
pcl::demeanPointCloud (*input_, *indices_, xyz_centroid, cloud_transformed);
pcl::transformPointCloud (cloud_transformed, cloud_transformed, transform1);
// True if qhull should free points in qh_freeqhull() or reallocation
boolT ismalloc = True;
// option flags for qhull, see qh_opt.htm
char flags[] = "qhull Tc";
// output from qh_produce_output(), use NULL to skip qh_produce_output()
FILE *outfile = NULL;
// error messages from qhull code
FILE *errfile = stderr;
// 0 if no error from qhull
int exitcode;
// Array of coordinates for each point
coordT *points = (coordT *)calloc (cloud_transformed.points.size () * dim, sizeof(coordT));
for (size_t i = 0; i < cloud_transformed.points.size (); ++i)
{
points[i * dim + 0] = (coordT)cloud_transformed.points[i].x;
points[i * dim + 1] = (coordT)cloud_transformed.points[i].y;
if (dim > 2)
points[i * dim + 2] = (coordT)cloud_transformed.points[i].z;
}
// Compute convex hull
exitcode = qh_new_qhull (dim, cloud_transformed.points.size (), points, ismalloc, flags, outfile, errfile);
if (exitcode != 0)
{
PCL_ERROR ("[pcl::%s::performReconstrution] ERROR: qhull was unable to compute a convex hull for the given point cloud (%lu)!\n",
getClassName ().c_str (), (unsigned long) input_->points.size ());
//check if it fails because of NaN values...
if (!cloud_transformed.is_dense)
{
bool NaNvalues = false;
for (size_t i = 0; i < cloud_transformed.size (); ++i)
{
if (!pcl_isfinite (cloud_transformed.points[i].x) ||
!pcl_isfinite (cloud_transformed.points[i].y) ||
!pcl_isfinite (cloud_transformed.points[i].z))
{
NaNvalues = true;
break;
}
}
if (NaNvalues)
PCL_ERROR ("[pcl::%s::performReconstruction] ERROR: point cloud contains NaN values, consider running pcl::PassThrough filter first to remove NaNs!\n", getClassName ().c_str ());
}
hull.points.resize (0);
hull.width = hull.height = 0;
polygons.resize (0);
qh_freeqhull (!qh_ALL);
int curlong, totlong;
qh_memfreeshort (&curlong, &totlong);
return;
}
qh_triangulate ();
int num_facets = qh num_facets;
int num_vertices = qh num_vertices;
hull.points.resize (num_vertices);
vertexT * vertex;
int i = 0;
// Max vertex id
int max_vertex_id = -1;
FORALLvertices
{
if ((int)vertex->id > max_vertex_id)
max_vertex_id = vertex->id;
}
++max_vertex_id;
std::vector<int> qhid_to_pcidx (max_vertex_id);
FORALLvertices
{
// Add vertices to hull point_cloud
hull.points[i].x = vertex->point[0];
hull.points[i].y = vertex->point[1];
if (dim>2)
hull.points[i].z = vertex->point[2];
else
hull.points[i].z = 0;
qhid_to_pcidx[vertex->id] = i; // map the vertex id of qhull to the point cloud index
++i;
}
if (fill_polygon_data)
{
if (dim == 3)
{
polygons.resize (num_facets);
int dd = 0;
facetT * facet;
FORALLfacets
{
polygons[dd].vertices.resize (3);
// Needed by FOREACHvertex_i_
int vertex_n, vertex_i;
FOREACHvertex_i_((*facet).vertices)
//facet_vertices.vertices.push_back (qhid_to_pcidx[vertex->id]);
polygons[dd].vertices[vertex_i] = qhid_to_pcidx[vertex->id];
++dd;
}
}
else
{
template <typename PointInT> void
pcl::ConvexHull<PointInT>::performReconstruction3D (
PointCloud &hull, std::vector<pcl::Vertices> &polygons, bool fill_polygon_data)
{
int dimension = 3;
// True if qhull should free points in qh_freeqhull() or reallocation
boolT ismalloc = True;
// output from qh_produce_output(), use NULL to skip qh_produce_output()
FILE *outfile = NULL;
if (compute_area_)
outfile = stderr;
// option flags for qhull, see qh_opt.htm
const char *flags = qhull_flags.c_str ();
// error messages from qhull code
FILE *errfile = stderr;
// Array of coordinates for each point
coordT *points = reinterpret_cast<coordT*> (calloc (indices_->size () * dimension, sizeof (coordT)));
int j = 0;
for (size_t i = 0; i < indices_->size (); ++i, j+=dimension)
{
points[j + 0] = static_cast<coordT> (input_->points[(*indices_)[i]].x);
points[j + 1] = static_cast<coordT> (input_->points[(*indices_)[i]].y);
points[j + 2] = static_cast<coordT> (input_->points[(*indices_)[i]].z);
}
// Compute convex hull
int exitcode = qh_new_qhull (dimension, static_cast<int> (indices_->size ()), points, ismalloc, const_cast<char*> (flags), outfile, errfile);
// 0 if no error from qhull
if (exitcode != 0)
{
PCL_ERROR ("[pcl::%s::performReconstrution3D] ERROR: qhull was unable to compute a convex hull for the given point cloud (%zu)!\n", getClassName ().c_str (), input_->points.size ());
hull.points.resize (0);
hull.width = hull.height = 0;
polygons.resize (0);
qh_freeqhull (!qh_ALL);
int curlong, totlong;
qh_memfreeshort (&curlong, &totlong);
return;
}
qh_triangulate ();
int num_facets = qh num_facets;
int num_vertices = qh num_vertices;
hull.points.resize (num_vertices);
vertexT * vertex;
int i = 0;
// Max vertex id
int max_vertex_id = 0;
FORALLvertices
{
if (vertex->id + 1 > max_vertex_id)
max_vertex_id = vertex->id + 1;
}
++max_vertex_id;
std::vector<int> qhid_to_pcidx (max_vertex_id);
FORALLvertices
{
// Add vertices to hull point_cloud
hull.points[i] = input_->points[(*indices_)[qh_pointid (vertex->point)]];
qhid_to_pcidx[vertex->id] = i; // map the vertex id of qhull to the point cloud index
++i;
}
if (compute_area_)
{
total_area_ = qh totarea;
total_volume_ = qh totvol;
}
if (fill_polygon_data)
{
polygons.resize (num_facets);
int dd = 0;
facetT * facet;
FORALLfacets
{
polygons[dd].vertices.resize (3);
// Needed by FOREACHvertex_i_
int vertex_n, vertex_i;
FOREACHvertex_i_ ((*facet).vertices)
//facet_vertices.vertices.push_back (qhid_to_pcidx[vertex->id]);
polygons[dd].vertices[vertex_i] = qhid_to_pcidx[vertex->id];
++dd;
}
}
// Deallocates memory (also the points)
qh_freeqhull (!qh_ALL);
int curlong, totlong;
qh_memfreeshort (&curlong, &totlong);
hull.width = static_cast<uint32_t> (hull.points.size ());
hull.height = 1;
hull.is_dense = false;
}
template <typename PointInT> void
pcl::ConvexHull<PointInT>::performReconstruction2D (PointCloud &hull, std::vector<pcl::Vertices> &polygons,
bool)
{
int dimension = 2;
bool xy_proj_safe = true;
bool yz_proj_safe = true;
bool xz_proj_safe = true;
// Check the input's normal to see which projection to use
PointInT p0 = input_->points[0];
PointInT p1 = input_->points[indices_->size () - 1];
PointInT p2 = input_->points[indices_->size () / 2];
Eigen::Array4f dy1dy2 = (p1.getArray4fMap () - p0.getArray4fMap ()) / (p2.getArray4fMap () - p0.getArray4fMap ());
while (!( (dy1dy2[0] != dy1dy2[1]) || (dy1dy2[2] != dy1dy2[1]) ) )
{
p0 = input_->points[rand () % indices_->size ()];
p1 = input_->points[rand () % indices_->size ()];
p2 = input_->points[rand () % indices_->size ()];
dy1dy2 = (p1.getArray4fMap () - p0.getArray4fMap ()) / (p2.getArray4fMap () - p0.getArray4fMap ());
}
pcl::PointCloud<PointInT> normal_calc_cloud;
normal_calc_cloud.points.resize (3);
normal_calc_cloud.points[0] = p0;
normal_calc_cloud.points[1] = p1;
normal_calc_cloud.points[2] = p2;
Eigen::Vector4f normal_calc_centroid;
Eigen::Matrix3f normal_calc_covariance;
pcl::computeMeanAndCovarianceMatrix (normal_calc_cloud, normal_calc_covariance, normal_calc_centroid);
// Need to set -1 here. See eigen33 for explanations.
Eigen::Vector3f::Scalar eigen_value;
Eigen::Vector3f plane_params;
pcl::eigen33 (normal_calc_covariance, eigen_value, plane_params);
float theta_x = fabsf (plane_params.dot (x_axis_));
float theta_y = fabsf (plane_params.dot (y_axis_));
float theta_z = fabsf (plane_params.dot (z_axis_));
// Check for degenerate cases of each projection
// We must avoid projections in which the plane projects as a line
if (theta_z > projection_angle_thresh_)
{
xz_proj_safe = false;
yz_proj_safe = false;
}
if (theta_x > projection_angle_thresh_)
{
xz_proj_safe = false;
xy_proj_safe = false;
}
if (theta_y > projection_angle_thresh_)
{
xy_proj_safe = false;
yz_proj_safe = false;
}
// True if qhull should free points in qh_freeqhull() or reallocation
boolT ismalloc = True;
// output from qh_produce_output(), use NULL to skip qh_produce_output()
FILE *outfile = NULL;
if (compute_area_)
outfile = stderr;
// option flags for qhull, see qh_opt.htm
const char* flags = qhull_flags.c_str ();
// error messages from qhull code
FILE *errfile = stderr;
// Array of coordinates for each point
coordT *points = reinterpret_cast<coordT*> (calloc (indices_->size () * dimension, sizeof (coordT)));
// Build input data, using appropriate projection
int j = 0;
if (xy_proj_safe)
{
for (size_t i = 0; i < indices_->size (); ++i, j+=dimension)
{
points[j + 0] = static_cast<coordT> (input_->points[(*indices_)[i]].x);
points[j + 1] = static_cast<coordT> (input_->points[(*indices_)[i]].y);
}
}
else if (yz_proj_safe)
{
for (size_t i = 0; i < input_->points.size (); ++i, j+=dimension)
{
points[j + 0] = static_cast<coordT> (input_->points[(*indices_)[i]].y);
points[j + 1] = static_cast<coordT> (input_->points[(*indices_)[i]].z);
}
}
else if (xz_proj_safe)
{
for (size_t i = 0; i < input_->points.size (); ++i, j+=dimension)
{
points[j + 0] = static_cast<coordT> (input_->points[(*indices_)[i]].x);
points[j + 1] = static_cast<coordT> (input_->points[(*indices_)[i]].z);
}
}
else
{
// This should only happen if we had invalid input
PCL_ERROR ("[pcl::%s::performReconstruction2D] Invalid input!\n", getClassName ().c_str ());
}
// Compute convex hull
int exitcode = qh_new_qhull (dimension, static_cast<int> (indices_->size ()), points, ismalloc, const_cast<char*> (flags), outfile, errfile);
// 0 if no error from qhull
if (exitcode != 0)
{
PCL_ERROR ("[pcl::%s::performReconstrution2D] ERROR: qhull was unable to compute a convex hull for the given point cloud (%zu)!\n", getClassName ().c_str (), indices_->size ());
hull.points.resize (0);
hull.width = hull.height = 0;
polygons.resize (0);
qh_freeqhull (!qh_ALL);
int curlong, totlong;
qh_memfreeshort (&curlong, &totlong);
return;
}
// Qhull returns the area in volume for 2D
if (compute_area_)
{
total_area_ = qh totvol;
total_volume_ = 0.0;
}
int num_vertices = qh num_vertices;
hull.points.resize (num_vertices);
memset (&hull.points[0], static_cast<int> (hull.points.size ()), sizeof (PointInT));
vertexT * vertex;
int i = 0;
std::vector<std::pair<int, Eigen::Vector4f>, Eigen::aligned_allocator<std::pair<int, Eigen::Vector4f> > > idx_points (num_vertices);
idx_points.resize (hull.points.size ());
memset (&idx_points[0], static_cast<int> (hull.points.size ()), sizeof (std::pair<int, Eigen::Vector4f>));
FORALLvertices
{
hull.points[i] = input_->points[(*indices_)[qh_pointid (vertex->point)]];
idx_points[i].first = qh_pointid (vertex->point);
++i;
}
// Sort
Eigen::Vector4f centroid;
pcl::compute3DCentroid (hull, centroid);
if (xy_proj_safe)
{
for (size_t j = 0; j < hull.points.size (); j++)
{
idx_points[j].second[0] = hull.points[j].x - centroid[0];
idx_points[j].second[1] = hull.points[j].y - centroid[1];
}
}
else if (yz_proj_safe)
{
for (size_t j = 0; j < hull.points.size (); j++)
{
idx_points[j].second[0] = hull.points[j].y - centroid[1];
idx_points[j].second[1] = hull.points[j].z - centroid[2];
}
}
else if (xz_proj_safe)
{
for (size_t j = 0; j < hull.points.size (); j++)
{
idx_points[j].second[0] = hull.points[j].x - centroid[0];
idx_points[j].second[1] = hull.points[j].z - centroid[2];
}
}
std::sort (idx_points.begin (), idx_points.end (), comparePoints2D);
polygons.resize (1);
polygons[0].vertices.resize (hull.points.size () + 1);
for (int j = 0; j < static_cast<int> (hull.points.size ()); j++)
{
hull.points[j] = input_->points[(*indices_)[idx_points[j].first]];
polygons[0].vertices[j] = static_cast<unsigned int> (j);
}
polygons[0].vertices[hull.points.size ()] = 0;
qh_freeqhull (!qh_ALL);
int curlong, totlong;
qh_memfreeshort (&curlong, &totlong);
hull.width = static_cast<uint32_t> (hull.points.size ());
hull.height = 1;
hull.is_dense = false;
return;
}
CCLib::ReferenceCloud* qHPR::removeHiddenPoints(CCLib::GenericIndexedCloudPersist* theCloud, const CCVector3d& viewPoint, double fParam)
{
assert(theCloud);
unsigned nbPoints = theCloud->size();
if (nbPoints == 0)
return 0;
//less than 4 points? no need for calculation, we return the whole cloud
if (nbPoints < 4)
{
CCLib::ReferenceCloud* visiblePoints = new CCLib::ReferenceCloud(theCloud);
if (!visiblePoints->addPointIndex(0,nbPoints)) //well even for less than 4 points we never know ;)
{
//not enough memory!
delete visiblePoints;
visiblePoints = 0;
}
return visiblePoints;
}
double maxRadius = 0;
//convert point cloud to an array of double triplets (for qHull)
coordT* pt_array = new coordT[(nbPoints+1)*3];
{
coordT* _pt_array = pt_array;
for (unsigned i=0; i<nbPoints; ++i)
{
CCVector3d P = CCVector3d::fromArray(theCloud->getPoint(i)->u) - viewPoint;
*_pt_array++ = static_cast<coordT>(P.x);
*_pt_array++ = static_cast<coordT>(P.y);
*_pt_array++ = static_cast<coordT>(P.z);
//we keep track of the highest 'radius'
double r2 = P.norm2();
if (maxRadius < r2)
maxRadius = r2;
}
//we add the view point (Cf. HPR)
*_pt_array++ = 0;
*_pt_array++ = 0;
*_pt_array++ = 0;
maxRadius = sqrt(maxRadius);
}
//apply spherical flipping
{
maxRadius *= pow(10.0,fParam) * 2;
coordT* _pt_array = pt_array;
for (unsigned i=0; i<nbPoints; ++i)
{
CCVector3d P = CCVector3d::fromArray(theCloud->getPoint(i)->u) - viewPoint;
double r = (maxRadius/P.norm()) - 1.0;
*_pt_array++ *= r;
*_pt_array++ *= r;
*_pt_array++ *= r;
}
}
//array to flag points on the convex hull
std::vector<bool> pointBelongsToCvxHull;
static char qHullCommand[] = "qhull QJ Qci";
if (!qh_new_qhull(3,nbPoints+1,pt_array,False,qHullCommand,0,stderr))
{
try
{
pointBelongsToCvxHull.resize(nbPoints+1,false);
}
catch (const std::bad_alloc&)
{
//not enough memory!
delete[] pt_array;
return 0;
}
vertexT *vertex = 0,**vertexp = 0;
facetT *facet = 0;
FORALLfacets
{
//if (!facet->simplicial)
// error("convhulln: non-simplicial facet"); // should never happen with QJ
setT* vertices = qh_facet3vertex(facet);
FOREACHvertex_(vertices)
{
pointBelongsToCvxHull[qh_pointid(vertex->point)] = true;
}
qh_settempfree(&vertices);
}
}
delete[] pt_array;
pt_array = 0;
qh_freeqhull(!qh_ALL);
//free long memory
int curlong, totlong;
qh_memfreeshort (&curlong, &totlong);
//free short memory and memory allocator
if (!pointBelongsToCvxHull.empty())
{
//compute the number of points belonging to the convex hull
unsigned cvxHullSize = 0;
{
for (unsigned i=0; i<nbPoints; ++i)
if (pointBelongsToCvxHull[i])
++cvxHullSize;
}
CCLib::ReferenceCloud* visiblePoints = new CCLib::ReferenceCloud(theCloud);
if (cvxHullSize!=0 && visiblePoints->reserve(cvxHullSize))
{
for (unsigned i=0; i<nbPoints; ++i)
if (pointBelongsToCvxHull[i])
visiblePoints->addPointIndex(i); //can't fail, see above
return visiblePoints;
}
else //not enough memory
{
delete visiblePoints;
visiblePoints = 0;
}
}
return 0;
}