BVutil.cpp
Go to the documentation of this file.
00001 #include <iostream>
00002 extern "C" {
00003 #if (defined __APPLE__)
00004 #include <pcl/surface/qhull.h>
00005 #else
00006 #include <qhull/qhull_a.h>
00007 #endif
00008 }
00009 #include <hrpModel/Link.h>
00010 #include "BVutil.h"
00011 
00012 using namespace hrp;
00013 
00014 void convertToAABB(hrp::BodyPtr i_body)
00015 {
00016     for (unsigned int i=0; i<i_body->numLinks(); i++) convertToAABB(i_body->link(i));
00017 }
00018 
00019 void convertToAABB(hrp::Link *i_link)
00020 {
00021     if (!i_link->coldetModel || !i_link->coldetModel->getNumVertices()) return;
00022 
00023     int ptype = i_link->coldetModel->getPrimitiveType();
00024     if (ptype == ColdetModel::SP_PLANE || ptype == ColdetModel::SP_SPHERE){
00025         return;
00026     }
00027 
00028     std::vector<Vector3> boundingBoxData;
00029     i_link->coldetModel->getBoundingBoxData(0, boundingBoxData);
00030     if (boundingBoxData.size() != 2){
00031         std::cerr << "unexpected bounding box data size(" << i_link->name 
00032                   << ", " << boundingBoxData.size() << ")" << std::endl;
00033         return;
00034     }
00035     const Vector3 &c = boundingBoxData[0];
00036     const Vector3 &s = boundingBoxData[1];
00037     ColdetModelPtr coldetModel(new ColdetModel());
00038     coldetModel->setName(std::string(i_link->name.c_str()));
00039     coldetModel->setPrimitiveType(ColdetModel::SP_BOX);
00040     coldetModel->setNumPrimitiveParams(3);
00041     for (int i=0; i<3; i++){
00042         coldetModel->setPrimitiveParam(i, s[i]);
00043     }
00044     double R[] = {1,0,0,0,1,0,0,0,1};
00045     coldetModel->setPrimitivePosition(R, c.data());
00046     coldetModel->setNumVertices(8);
00047     coldetModel->setVertex(0, c[0]+s[0], c[1]+s[1], c[2]+s[2]);
00048     coldetModel->setVertex(1, c[0]-s[0], c[1]+s[1], c[2]+s[2]);
00049     coldetModel->setVertex(2, c[0]-s[0], c[1]-s[1], c[2]+s[2]);
00050     coldetModel->setVertex(3, c[0]+s[0], c[1]-s[1], c[2]+s[2]);
00051     coldetModel->setVertex(4, c[0]+s[0], c[1]+s[1], c[2]-s[2]);
00052     coldetModel->setVertex(5, c[0]-s[0], c[1]+s[1], c[2]-s[2]);
00053     coldetModel->setVertex(6, c[0]-s[0], c[1]-s[1], c[2]-s[2]);
00054     coldetModel->setVertex(7, c[0]+s[0], c[1]-s[1], c[2]-s[2]);
00055     int numTriangles = 12;
00056     coldetModel->setNumTriangles(numTriangles);
00057     int triangles[] = {0,2,3,
00058                        0,1,2,
00059                        4,3,7,
00060                        4,0,3,
00061                        0,4,5,
00062                        5,1,0,
00063                        1,5,6,
00064                        1,6,2,
00065                        2,6,7,
00066                        2,7,3,
00067                        7,6,4,
00068                        5,4,6};
00069     for(int j=0; j < numTriangles; ++j){
00070         int t0 = triangles[j*3];
00071         int t1 = triangles[j*3+1];
00072         int t2 = triangles[j*3+2];
00073         coldetModel->setTriangle(j, t0, t1, t2);
00074     }
00075     coldetModel->build();
00076     i_link->coldetModel = coldetModel;
00077 }
00078 
00079 void convertToConvexHull(hrp::BodyPtr i_body)
00080 {
00081     for (unsigned int i=0; i<i_body->numLinks(); i++){
00082         convertToConvexHull(i_body->link(i));
00083     }
00084 }
00085 
00086 void convertToConvexHull(hrp::Link *i_link)
00087 {
00088     if (!i_link->coldetModel || !i_link->coldetModel->getNumVertices()) return;
00089 
00090     int ptype = i_link->coldetModel->getPrimitiveType();
00091     if (ptype == ColdetModel::SP_PLANE || ptype == ColdetModel::SP_SPHERE){
00092         return;
00093     }
00094 
00095     ColdetModelPtr coldetModel(new ColdetModel());
00096     coldetModel->setName(i_link->name.c_str());
00097     coldetModel->setPrimitiveType(ColdetModel::SP_MESH);
00098     // qhull
00099     int numVertices = i_link->coldetModel->getNumVertices();
00100     double points[numVertices*3];
00101     float v[3];
00102     for (int i=0; i<numVertices; i++){
00103         i_link->coldetModel->getVertex(i, v[0],v[1],v[2]);
00104         points[i*3+0] = v[0];
00105         points[i*3+1] = v[1];
00106         points[i*3+2] = v[2];
00107     }
00108     char flags[250];
00109     boolT ismalloc = False;
00110     sprintf(flags,"qhull Qt Tc");
00111     if (qh_new_qhull (3,numVertices,points,ismalloc,flags,NULL,stderr)) return;
00112 
00113     qh_triangulate();
00114     qh_vertexneighbors();
00115     
00116     coldetModel->setNumVertices(qh num_vertices);
00117     coldetModel->setNumTriangles(qh num_facets);
00118     int index[numVertices];
00119     int vertexIndex = 0;
00120     vertexT *vertex;
00121     FORALLvertices {
00122         int p = qh_pointid(vertex->point);
00123         index[p] = vertexIndex;
00124         coldetModel->setVertex(vertexIndex++, points[p*3+0], points[p*3+1], points[p*3+2]);
00125     }
00126     facetT *facet;
00127     int num = qh num_facets;
00128     int triangleIndex = 0;
00129     FORALLfacets {
00130         int j = 0, p[3];
00131         setT *vertices = qh_facet3vertex (facet);
00132         vertexT **vertexp;
00133         FOREACHvertexreverse12_ (vertices) {
00134             if (j<3) {
00135                 p[j] = index[qh_pointid(vertex->point)];
00136             } else {
00137                 fprintf(stderr, "extra vertex %d\n",j);
00138             }
00139             j++;
00140         }
00141         coldetModel->setTriangle(triangleIndex++, p[0], p[1], p[2]);
00142     }
00143 
00144     coldetModel->build();
00145     i_link->coldetModel =  coldetModel;
00146     
00147     qh_freeqhull(!qh_ALL);
00148     int curlong, totlong;
00149     qh_memfreeshort (&curlong, &totlong);
00150     if (curlong || totlong) {
00151         fprintf(stderr, "convhulln: did not free %d bytes of long memory (%d pieces)\n", totlong, curlong);
00152     }
00153     //
00154     //std::cerr << i_link->name << " reduce triangles from " << numTriangles << " to " << num << std::endl;
00155 }


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed May 15 2019 05:02:17