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);
};
/**
* Calculates the Delaunay triangulation/tetrahedralization of a set of points.
* Note: right now, there can be only one active Delaunay triangulation at
* a given time.
*
* @param points [inout] Fortran array (ndim, npts)
* @param npts [in] number of points.
* @param ndim [in] number of dimensions
*/
int qhull_init_delaunay_(double points[], const int *npts, const int *ndim, const int *inode)
{
boolT ismalloc= False; /* don`t free points in qh_freeqhull() */
char flags[250];
FILE *outf=NULL;
FILE *errf=stderr;
int ierr;
_inode = *inode;
sprintf(flags, QHULL_STR);
ierr = qh_new_qhull(*ndim, *npts, points, ismalloc, flags, outf, errf);
if (ierr) {
qhull_error("could not create Delaunay triangulation (code %d)", ierr);
return ierr;
}
ierr = init_simplex_facet_array();
if (ierr) {
qhull_error("could not get simplicial facets (code %d)", ierr);
return ierr;
}
ierr = init_barycentric_transforms();
if (ierr) {
qhull_error("could not get barycentric transformations (code %d)", ierr);
return ierr;
}
return ierr;
}
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;
}
}
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;
}
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;
}
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;
}
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;
}
void gr_delaunay(int npoints, const double *x, const double *y,
int *ntri, int **triangles)
{
coordT *points = NULL;
facetT *facet;
vertexT *vertex, **vertexp;
int i, max_facet_id;
int *tri_indices = NULL;
int indices[3], *indicesp;
int curlong, totlong;
const int ndim = 2;
int *tri = NULL;
*ntri = 0;
*triangles = NULL;
points = (coordT *) malloc(npoints * ndim * sizeof(coordT));
if (points != NULL) {
for (i = 0; i < npoints; ++i) {
points[2*i ] = x[i];
points[2*i+1] = y[i];
}
/* Perform Delaunay triangulation */
if (qh_new_qhull(ndim, npoints, points, False,
"qhull d Qt QbB Qz", NULL, stderr) == qh_ERRnone) {
/* Split facets so that they only have 3 points each */
qh_triangulate();
/* Determine ntri and max_facet_id */
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) {
tri = (int *) malloc(*ntri * 3 * sizeof(int));
if (tri != NULL) {
*triangles = tri;
/* Determine triangles array and set tri_indices array */
i = 0;
FORALLfacets {
if (!facet->upperdelaunay) {
tri_indices[facet->id] = i++;
indicesp = indices;
FOREACHvertex_(facet->vertices)
*indicesp++ = qh_pointid(vertex->point);
*tri++ = (facet->toporient ? indices[0] : indices[2]);
*tri++ = indices[1];
*tri++ = (facet->toporient ? indices[2] : indices[0]);
}
else
tri_indices[facet->id] = -1;
}
xp = x;
yp = y;
qsort(*triangles, *ntri, 3 * sizeof(int), compar);
}
else
#if 0
{
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)
gInterface->HullPrintf (errfile, "qhull internal warning (main): did not free %d bytes of long memory (%d pieces)\n", totlong, curlong);
}
#endif
/*-<a href="qh-user.htm#TOC"
>-------------------------------</a><a name="new_qhull">-</a>
void
raytrace_1_4(int im, inputPars *par, struct grid *g, molData *m, image *img){
/*
This is an alternative raytracing algorithm which was implemented by
C Brinch in version 1.4 (the original parallelized version) of LIME.
I've adapted it slightly so it makes use of the function traceray(),
which was modified from the function tracerays() in v1.4. This algorithm
is not currently used, but may be useful as an option; that's why I have
kept it.
*/
int *counta, *countb,nlinetot;
int ichan,i,px,iline,tmptrans,count;
double size,xp,yp,minfreq,absDeltaFreq;
double cutoff;
gsl_rng *ran = gsl_rng_alloc(gsl_rng_ranlxs2); /* Random number generator */
#ifdef TEST
gsl_rng_set(ran,178490);
#else
gsl_rng_set(ran,time(0));
#endif
rayData *rays;
int sg,n;
double cx,cy;
double x1,x2,x3,y1,y2,y3,z1,z2,z3,xt[3],yt[3],di,p,d1,d2,d3,temp1;
int zt[3];
int c;
char flags[255];
boolT ismalloc = False;
facetT *facet, *neighbor, **neighborp;;
vertexT *vertex,**vertexp;
coordT *pt_array;
int id;
coordT point[3];
boolT isoutside;
realT bestdist;
size=img[im].distance*img[im].imgres;
/* Determine whether there are blended lines or not */
lineCount(par->nSpecies, m, &counta, &countb, &nlinetot);
if(img[im].doline==0) nlinetot=1;
/* Fix the image parameters */
if(img[im].freq < 0) img[im].freq=m[0].freq[img[im].trans];
if(img[im].nchan == 0 && img[im].bandwidth>0){
img[im].nchan=(int) (img[im].bandwidth/(img[im].velres/CLIGHT*img[im].freq));
} else if (img[im].velres<0 && img[im].bandwidth>0){
img[im].velres = img[im].bandwidth*CLIGHT/img[im].freq/img[im].nchan;
} else img[im].bandwidth = img[im].nchan*img[im].velres/CLIGHT * img[im].freq;
if(img[im].trans<0){
iline=0;
minfreq=fabs(img[im].freq-m[0].freq[iline]);
tmptrans=iline;
for(iline=1;iline<m[0].nline;iline++){
absDeltaFreq=fabs(img[im].freq-m[0].freq[iline]);
if(absDeltaFreq<minfreq){
minfreq=absDeltaFreq;
tmptrans=iline;
}
}
} else tmptrans=img[im].trans;
/* Allocate dynamical arrays */
rays = malloc(sizeof(rayData) * (par->pIntensity));
for(i=0;i<par->pIntensity;i++){
rays[i].x=g[i].x[0];
rays[i].y=g[i].x[1];
rays[i].tau=malloc(sizeof(double) * img[im].nchan);
rays[i].intensity=malloc(sizeof(double) * img[im].nchan);
for(ichan=0;ichan<img[im].nchan;ichan++) {
rays[i].tau[ichan]=0.0;
rays[i].intensity[ichan]=0.0;
}
}
/* Smooth out the distribution of rays */
for(sg=0;sg<20;sg++){
pt_array=malloc(2*sizeof(coordT)*par->pIntensity);
for(i=0;i<par->pIntensity;i++) {
pt_array[i*2+0]=rays[i].x;
pt_array[i*2+1]=rays[i].y;
}
sprintf(flags,"qhull v s Qbb T0");
if (!qh_new_qhull(2, par->pIntensity, pt_array, ismalloc, flags, NULL, NULL)) {
qh_setvoronoi_all();
FORALLvertices {
i=qh_pointid(vertex->point);
cx=0.;
cy=0.;
n=0;
FOREACHneighbor_(vertex) {
if (!neighbor->upperdelaunay) n++;
}
if(n>0){
} else {
if(!silent) bail_out("Qhull error");
exit(0);
}
FOREACHneighbor_(vertex) {
if (!neighbor->upperdelaunay) {
cx+=neighbor->center[0];
cy+=neighbor->center[1];
}
}
rays[i].x = rays[i].x - (rays[i].x-cx/ (double) n)*0.1;
rays[i].y = rays[i].y - (rays[i].y-cy/ (double) n)*0.1;
}
} else {
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;
}
CCLib::ReferenceCloud* qHPR::removeHiddenPoints(CCLib::GenericIndexedCloudPersist* theCloud, float viewPoint[], float fParam)
{
assert(theCloud);
unsigned i,nbPoints = theCloud->size();
if (nbPoints==0)
return 0;
CCLib::ReferenceCloud* newCloud = new CCLib::ReferenceCloud(theCloud);
//less than 4 points ? no need for calculation, we return the whole cloud
if (nbPoints<4)
{
if (!newCloud->reserve(nbPoints)) //well, we never know ;)
{
//not enough memory!
delete newCloud;
return 0;
}
newCloud->addPointIndex(0,nbPoints);
return newCloud;
}
//view point
coordT Cx = viewPoint[0];
coordT Cy = viewPoint[1];
coordT Cz = viewPoint[2];
float* radius = new float[nbPoints];
if (!radius)
{
//not enough memory!
delete newCloud;
return 0;
}
float r,maxRadius = 0.0;
//table of points
coordT* pt_array = new coordT[(nbPoints+1)*3];
coordT* _pt_array = pt_array;
theCloud->placeIteratorAtBegining();
//#define BACKUP_PROJECTED_CLOUDS
#ifdef BACKUP_PROJECTED_CLOUDS
FILE* fp = fopen("output_centered.asc","wt");
#endif
double x,y,z;
for (i=0;i<nbPoints;++i)
{
const CCVector3* P = theCloud->getNextPoint();
*(_pt_array++)=x=coordT(P->x)-Cx;
*(_pt_array++)=y=coordT(P->y)-Cy;
*(_pt_array++)=z=coordT(P->z)-Cz;
//we pre-compute the radius ...
r = (float)sqrt(x*x+y*y+z*z);
//in order to determine the max radius
if (maxRadius<r)
maxRadius = r;
radius[i] = r;
#ifdef BACKUP_PROJECTED_CLOUDS
fprintf(fp,"%f %f %f %f\n",x,y,z,r);
#endif
}
//we add the view point (Cf. HPR)
*(_pt_array++)=0.0;
*(_pt_array++)=0.0;
*(_pt_array++)=0.0;
#ifdef BACKUP_PROJECTED_CLOUDS
fprintf(fp,"%f %f %f %f\n",0,0,0,0);
fclose(fp);
#endif
maxRadius *= 2.0f*pow(10.0f,fParam);
_pt_array = pt_array;
#ifdef BACKUP_PROJECTED_CLOUDS
fp = fopen("output_transformed.asc","wt");
#endif
for (i=0;i<nbPoints;++i)
{
//Spherical flipping
r = maxRadius/radius[i]-1.0f;
#ifndef BACKUP_PROJECTED_CLOUDS
*(_pt_array++) *= double(r);
*(_pt_array++) *= double(r);
*(_pt_array++) *= double(r);
#else
x = *_pt_array * double(r);
*(_pt_array++) = x;
y = *_pt_array * double(r);
*(_pt_array++) = y;
z = *_pt_array * double(r);
*(_pt_array++) = z;
fprintf(fp,"%f %f %f %f\n",x,y,z,r);
#endif
}
#ifdef BACKUP_PROJECTED_CLOUDS
fclose(fp);
#endif
//we re-use the radius to record if each point belongs to the convex hull
//delete[] radius;
//uchar* pointBelongsToCvxHull = new uchar[nbPoints];
uchar* pointBelongsToCvxHull = (uchar*)radius;
memset(pointBelongsToCvxHull,0,sizeof(uchar)*(nbPoints+1));
if (!qh_new_qhull(3,nbPoints+1,pt_array,False,(char*)"qhull QJ s Qci Tcv",0,stderr))
{
vertexT *vertex,**vertexp;
facetT *facet;
setT *vertices;
int j, i = 0;
FORALLfacets
{
/*if (!facet->simplicial)
error("convhulln: non-simplicial facet"); // should never happen with QJ
*/
j = 0;
vertices = qh_facet3vertex (facet);
FOREACHvertex_(vertices)
{
pointBelongsToCvxHull[qh_pointid(vertex->point)]=1;
++j;
}
qh_settempfree(&vertices);
if (j < 3)
printf("Warning, facet %d only has %d vertices\n",i,j); // likewise but less fatal
i++;
}
}
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
{