BVutil.cpp
Go to the documentation of this file.
1 #include <iostream>
2 extern "C" {
3 #if (defined __APPLE__)
4 #include <pcl/surface/qhull.h>
5 #else
6 #include <qhull/qhull_a.h>
7 #endif
8 }
9 #include <hrpModel/Link.h>
10 #include "BVutil.h"
11 
12 using namespace hrp;
13 
15 {
16  for (unsigned int i=0; i<i_body->numLinks(); i++) convertToAABB(i_body->link(i));
17 }
18 
19 void convertToAABB(hrp::Link *i_link)
20 {
21  if (!i_link->coldetModel || !i_link->coldetModel->getNumVertices()) return;
22 
23  int ptype = i_link->coldetModel->getPrimitiveType();
24  if (ptype == ColdetModel::SP_PLANE || ptype == ColdetModel::SP_SPHERE){
25  return;
26  }
27 
28  std::vector<Vector3> boundingBoxData;
29  i_link->coldetModel->getBoundingBoxData(0, boundingBoxData);
30  if (boundingBoxData.size() != 2){
31  std::cerr << "unexpected bounding box data size(" << i_link->name
32  << ", " << boundingBoxData.size() << ")" << std::endl;
33  return;
34  }
35  const Vector3 &c = boundingBoxData[0];
36  const Vector3 &s = boundingBoxData[1];
37  ColdetModelPtr coldetModel(new ColdetModel());
38  coldetModel->setName(std::string(i_link->name.c_str()));
39  coldetModel->setPrimitiveType(ColdetModel::SP_BOX);
40  coldetModel->setNumPrimitiveParams(3);
41  for (int i=0; i<3; i++){
42  coldetModel->setPrimitiveParam(i, s[i]);
43  }
44  double R[] = {1,0,0,0,1,0,0,0,1};
45  coldetModel->setPrimitivePosition(R, c.data());
46  coldetModel->setNumVertices(8);
47  coldetModel->setVertex(0, c[0]+s[0], c[1]+s[1], c[2]+s[2]);
48  coldetModel->setVertex(1, c[0]-s[0], c[1]+s[1], c[2]+s[2]);
49  coldetModel->setVertex(2, c[0]-s[0], c[1]-s[1], c[2]+s[2]);
50  coldetModel->setVertex(3, c[0]+s[0], c[1]-s[1], c[2]+s[2]);
51  coldetModel->setVertex(4, c[0]+s[0], c[1]+s[1], c[2]-s[2]);
52  coldetModel->setVertex(5, c[0]-s[0], c[1]+s[1], c[2]-s[2]);
53  coldetModel->setVertex(6, c[0]-s[0], c[1]-s[1], c[2]-s[2]);
54  coldetModel->setVertex(7, c[0]+s[0], c[1]-s[1], c[2]-s[2]);
55  int numTriangles = 12;
56  coldetModel->setNumTriangles(numTriangles);
57  int triangles[] = {0,2,3,
58  0,1,2,
59  4,3,7,
60  4,0,3,
61  0,4,5,
62  5,1,0,
63  1,5,6,
64  1,6,2,
65  2,6,7,
66  2,7,3,
67  7,6,4,
68  5,4,6};
69  for(int j=0; j < numTriangles; ++j){
70  int t0 = triangles[j*3];
71  int t1 = triangles[j*3+1];
72  int t2 = triangles[j*3+2];
73  coldetModel->setTriangle(j, t0, t1, t2);
74  }
75  coldetModel->build();
76  i_link->coldetModel = coldetModel;
77 }
78 
80 {
81  for (unsigned int i=0; i<i_body->numLinks(); i++){
82  convertToConvexHull(i_body->link(i));
83  }
84 }
85 
87 {
88  if (!i_link->coldetModel || !i_link->coldetModel->getNumVertices()) return;
89 
90  int ptype = i_link->coldetModel->getPrimitiveType();
91  if (ptype == ColdetModel::SP_PLANE || ptype == ColdetModel::SP_SPHERE){
92  return;
93  }
94 
95  ColdetModelPtr coldetModel(new ColdetModel());
96  coldetModel->setName(i_link->name.c_str());
97  coldetModel->setPrimitiveType(ColdetModel::SP_MESH);
98  // qhull
99  int numVertices = i_link->coldetModel->getNumVertices();
100  double points[numVertices*3];
101  float v[3];
102  for (int i=0; i<numVertices; i++){
103  i_link->coldetModel->getVertex(i, v[0],v[1],v[2]);
104  points[i*3+0] = v[0];
105  points[i*3+1] = v[1];
106  points[i*3+2] = v[2];
107  }
108  char flags[250];
109  boolT ismalloc = False;
110  sprintf(flags,"qhull Qt Tc");
111  if (qh_new_qhull (3,numVertices,points,ismalloc,flags,NULL,stderr)) return;
112 
113  qh_triangulate();
114  qh_vertexneighbors();
115 
116  coldetModel->setNumVertices(qh num_vertices);
117  coldetModel->setNumTriangles(qh num_facets);
118  int index[numVertices];
119  int vertexIndex = 0;
120  vertexT *vertex;
121  FORALLvertices {
122  int p = qh_pointid(vertex->point);
123  index[p] = vertexIndex;
124  coldetModel->setVertex(vertexIndex++, points[p*3+0], points[p*3+1], points[p*3+2]);
125  }
126  facetT *facet;
127  int num = qh num_facets;
128  int triangleIndex = 0;
129  FORALLfacets {
130  int j = 0, p[3];
131  setT *vertices = qh_facet3vertex (facet);
132  vertexT **vertexp;
133  FOREACHvertexreverse12_ (vertices) {
134  if (j<3) {
135  p[j] = index[qh_pointid(vertex->point)];
136  } else {
137  fprintf(stderr, "extra vertex %d\n",j);
138  }
139  j++;
140  }
141  coldetModel->setTriangle(triangleIndex++, p[0], p[1], p[2]);
142  }
143 
144  coldetModel->build();
145  i_link->coldetModel = coldetModel;
146 
147  qh_freeqhull(!qh_ALL);
148  int curlong, totlong;
149  qh_memfreeshort (&curlong, &totlong);
150  if (curlong || totlong) {
151  fprintf(stderr, "convhulln: did not free %d bytes of long memory (%d pieces)\n", totlong, curlong);
152  }
153  //
154  //std::cerr << i_link->name << " reduce triangles from " << numTriangles << " to " << num << std::endl;
155 }
void convertToAABB(hrp::BodyPtr i_body)
Definition: BVutil.cpp:14
png_uint_32 i
png_uint_32 int flags
OpenHRP::vector3 Vector3
void convertToConvexHull(hrp::BodyPtr i_body)
Definition: BVutil.cpp:79
list index
std::string sprintf(char const *__restrict fmt,...)
int num
boost::intrusive_ptr< ColdetModel > ColdetModelPtr


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Sat Dec 17 2022 03:52:20