/* Return the indices of the 3 vertices that comprise the specified facet (i.e.
* triangle). */
static void
get_facet_vertices(const facetT* facet, int indices[3])
{
vertexT *vertex, **vertexp;
FOREACHvertex_(facet->vertices)
*indices++ = qh_pointid(vertex->point);
}
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;
}
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");
}
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;
}
//! If qhRundID undefined uses QhullPoint::s_points_begin and dimension
int QhullPoint::
id(int qhRunId, int dimension, const coordT *c)
{
QHULL_UNUSED(dimension);
if(UsingLibQhull::hasPoints()){
if(qhRunId==UsingLibQhull::NOqhRunId){
const coordT *pointsEnd;
int dimension;
const coordT *points= UsingLibQhull::globalPoints(&dimension, &pointsEnd);
if(c>=points && c<pointsEnd){
int offset= (int)(c-points); // WARN64
return offset/dimension;
}
}else{
UsingLibQhull q(qhRunId);
// NOerrors from qh_pointid or qh_setindex
return qh_pointid(const_cast<coordT *>(c));
}
}
long long i=(long long)c;
return (int)i; // WARN64
}//id
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;
}
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
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 {
/*-------------------------------------------------
-qhull- hull_dim convex hull of num_points starting at first_point
returns:
returns facet_list, numfacets, etc.
*/
void qh_qhull (void) {
setT *maxpoints, *vertices;
facetT *facet;
int numpart, i, numoutside;
realT dist;
boolT isoutside;
qh hulltime= qh_CPUclock;
if (qh DELAUNAY && qh upper_threshold[qh hull_dim-1] > REALmax/2
&& qh lower_threshold[qh hull_dim-1] < -REALmax/2) {
for (i= qh_PRINTEND; i--; ) {
if (qh PRINTout[i] == qh_PRINTgeom && qh DROPdim < 0
&& !qh GOODthreshold && !qh SPLITthresholds)
break; /* in this case, don't set upper_threshold */
}
if (i < 0) {
if (qh UPPERdelaunay)
qh lower_threshold[qh hull_dim-1]= 0.0;
else
qh upper_threshold[qh hull_dim-1]= 0.0;
if (!qh GOODthreshold)
qh SPLITthresholds= True; /* build upper-convex hull even if Qg */
/* qh_initqhull_globals errors if Qg without Pdk/etc. */
}
}
maxpoints= qh_maxmin(qh first_point, qh num_points, qh hull_dim);
/* qh_maxmin sets DISTround and other precision constants */
vertices= qh_initialvertices(qh hull_dim, maxpoints, qh first_point, qh num_points);
qh_initialhull (vertices); /* initial qh facet_list */
qh_partitionall (vertices, qh first_point, qh num_points);
qh_resetlists (False /*qh visible_list newvertex_list newfacet_list */);
qh facet_next= qh facet_list;
qh_furthestnext (/* qh facet_list */);
if (qh PREmerge) {
qh cos_max= qh premerge_cos;
qh centrum_radius= qh premerge_centrum;
}
if (qh ONLYgood) {
if (qh GOODvertex > 0 && qh MERGING) {
fprintf (qh ferr, "qhull input error: 'Qg QVn' (only good vertex) does not work with merging. Use 'Q0'\n");
qh_errexit (qh_ERRinput, NULL, NULL);
}
if (!(qh GOODthreshold || qh GOODpoint)) {
fprintf (qh ferr, "qhull input error: 'Qg' (ONLYgood) needs a good threshold ('Pd0D0'), a\n\
good point (QGn or QG-n), or a good vertex with 'Q0' (QVn).\n");
qh_errexit (qh_ERRinput, NULL, NULL);
}
if (qh GOODvertex > 0 && !qh MERGING /* matches qh_partitionall */
&& !qh_isvertex (qh GOODvertexp, vertices)) {
facet= qh_findbestnew (qh GOODvertexp, qh facet_list,
&dist, &isoutside, &numpart);
zadd_(Zdistgood, numpart);
if (!isoutside) {
fprintf (qh ferr, "qhull input error: point for QV%d is inside initial simplex\n",
qh_pointid(qh GOODvertexp));
qh_errexit (qh_ERRinput, NULL, NULL);
}
if (!qh_addpoint (qh GOODvertexp, facet, False)) {
qh_settempfree(&vertices);
qh_settempfree(&maxpoints);
return;
}
}
qh_findgood (qh facet_list, 0);
}
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++;
}
}
