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;
}
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;
}
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++;
}
}
