00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include "geometric_shapes/shapes.h"
00038 #include <eigen_stl_containers/eigen_stl_containers.h>
00039 #include <octomap/octomap.h>
00040 #include <console_bridge/console.h>
00041
00042 const std::string shapes::Sphere::STRING_NAME = "sphere";
00043 const std::string shapes::Box::STRING_NAME = "box";
00044 const std::string shapes::Cylinder::STRING_NAME = "cylinder";
00045 const std::string shapes::Cone::STRING_NAME = "cone";
00046 const std::string shapes::Mesh::STRING_NAME = "mesh";
00047 const std::string shapes::Plane::STRING_NAME = "plane";
00048 const std::string shapes::OcTree::STRING_NAME = "octree";
00049
00050 shapes::Shape::Shape()
00051 {
00052 type = UNKNOWN_SHAPE;
00053 }
00054
00055 shapes::Shape::~Shape()
00056 {
00057 }
00058
00059 shapes::Sphere::Sphere() : Shape()
00060 {
00061 type = SPHERE;
00062 radius = 0.0;
00063 }
00064
00065 shapes::Sphere::Sphere(double r) : Shape()
00066 {
00067 type = SPHERE;
00068 radius = r;
00069 }
00070
00071 shapes::Cylinder::Cylinder() : Shape()
00072 {
00073 type = CYLINDER;
00074 length = radius = 0.0;
00075 }
00076
00077 shapes::Cylinder::Cylinder(double r, double l) : Shape()
00078 {
00079 type = CYLINDER;
00080 length = l;
00081 radius = r;
00082 }
00083
00084 shapes::Cone::Cone() : Shape()
00085 {
00086 type = CONE;
00087 length = radius = 0.0;
00088 }
00089
00090 shapes::Cone::Cone(double r, double l) : Shape()
00091 {
00092 type = CONE;
00093 length = l;
00094 radius = r;
00095 }
00096
00097 shapes::Box::Box() : Shape()
00098 {
00099 type = BOX;
00100 size[0] = size[1] = size[2] = 0.0;
00101 }
00102
00103 shapes::Box::Box(double x, double y, double z) : Shape()
00104 {
00105 type = BOX;
00106 size[0] = x;
00107 size[1] = y;
00108 size[2] = z;
00109 }
00110
00111 shapes::Mesh::Mesh() : Shape()
00112 {
00113 type = MESH;
00114 vertex_count = 0;
00115 vertices = NULL;
00116 triangle_count = 0;
00117 triangles = NULL;
00118 triangle_normals = NULL;
00119 vertex_normals = NULL;
00120 }
00121
00122 shapes::Mesh::Mesh(unsigned int v_count, unsigned int t_count) : Shape()
00123 {
00124 type = MESH;
00125 vertex_count = v_count;
00126 vertices = new double[v_count * 3];
00127 triangle_count = t_count;
00128 triangles = new unsigned int[t_count * 3];
00129 triangle_normals = new double[t_count * 3];
00130 vertex_normals = new double[v_count * 3];
00131 }
00132
00133 shapes::Mesh::~Mesh()
00134 {
00135 if (vertices)
00136 delete[] vertices;
00137 if (triangles)
00138 delete[] triangles;
00139 if (triangle_normals)
00140 delete[] triangle_normals;
00141 if (vertex_normals)
00142 delete[] vertex_normals;
00143 }
00144
00145 shapes::Plane::Plane() : Shape()
00146 {
00147 type = PLANE;
00148 a = b = c = d = 0.0;
00149 }
00150
00151 shapes::Plane::Plane(double pa, double pb, double pc, double pd) : Shape()
00152 {
00153 type = PLANE;
00154 a = pa; b = pb; c = pc; d = pd;
00155 }
00156
00157 shapes::OcTree::OcTree() : Shape()
00158 {
00159 type = OCTREE;
00160 }
00161
00162 shapes::OcTree::OcTree(const boost::shared_ptr<const octomap::OcTree> &t) : octree(t)
00163 {
00164 type = OCTREE;
00165 }
00166
00167 shapes::Shape* shapes::Sphere::clone() const
00168 {
00169 return new Sphere(radius);
00170 }
00171
00172 shapes::Shape* shapes::Cylinder::clone() const
00173 {
00174 return new Cylinder(radius, length);
00175 }
00176
00177 shapes::Shape* shapes::Cone::clone() const
00178 {
00179 return new Cone(radius, length);
00180 }
00181
00182 shapes::Shape* shapes::Box::clone() const
00183 {
00184 return new Box(size[0], size[1], size[2]);
00185 }
00186
00187 shapes::Shape* shapes::Mesh::clone() const
00188 {
00189 Mesh *dest = new Mesh(vertex_count, triangle_count);
00190 unsigned int n = 3 * vertex_count;
00191 for (unsigned int i = 0 ; i < n ; ++i)
00192 dest->vertices[i] = vertices[i];
00193 if (vertex_normals)
00194 for (unsigned int i = 0 ; i < n ; ++i)
00195 dest->vertex_normals[i] = vertex_normals[i];
00196 else
00197 {
00198 delete[] dest->vertex_normals;
00199 dest->vertex_normals = NULL;
00200 }
00201 n = 3 * triangle_count;
00202 for (unsigned int i = 0 ; i < n ; ++i)
00203 dest->triangles[i] = triangles[i];
00204 if (triangle_normals)
00205 for (unsigned int i = 0 ; i < n ; ++i)
00206 dest->triangle_normals[i] = triangle_normals[i];
00207 else
00208 {
00209 delete[] dest->triangle_normals;
00210 dest->triangle_normals = NULL;
00211 }
00212 return dest;
00213 }
00214
00215 shapes::Shape* shapes::Plane::clone() const
00216 {
00217 return new Plane(a, b, c, d);
00218 }
00219
00220 shapes::Shape* shapes::OcTree::clone() const
00221 {
00222 return new OcTree(octree);
00223 }
00224
00225 void shapes::OcTree::scaleAndPadd(double scale, double padd)
00226 {
00227 logWarn("OcTrees cannot be scaled or padded");
00228 }
00229
00230 void shapes::Plane::scaleAndPadd(double scale, double padding)
00231 {
00232 logWarn("Planes cannot be scaled or padded");
00233 }
00234
00235 void shapes::Shape::scale(double scale)
00236 {
00237 scaleAndPadd(scale, 0.0);
00238 }
00239
00240 void shapes::Shape::padd(double padding)
00241 {
00242 scaleAndPadd(1.0, padding);
00243 }
00244
00245 void shapes::Sphere::scaleAndPadd(double scale, double padding)
00246 {
00247 radius = radius * scale + padding;
00248 }
00249
00250 void shapes::Cylinder::scaleAndPadd(double scale, double padding)
00251 {
00252 radius = radius * scale + padding;
00253 length = length * scale + 2.0 * padding;
00254 }
00255
00256 void shapes::Cone::scaleAndPadd(double scale, double padding)
00257 {
00258 radius = radius * scale + padding;
00259 length = length * scale + 2.0 * padding;
00260 }
00261
00262 void shapes::Box::scaleAndPadd(double scale, double padding)
00263 {
00264 double p2 = padding * 2.0;
00265 size[0] = size[0] * scale + p2;
00266 size[1] = size[1] * scale + p2;
00267 size[2] = size[2] * scale + p2;
00268 }
00269
00270 void shapes::Mesh::scaleAndPadd(double scale, double padding)
00271 {
00272
00273 double sx = 0.0, sy = 0.0, sz = 0.0;
00274 for (unsigned int i = 0 ; i < vertex_count ; ++i)
00275 {
00276 unsigned int i3 = i * 3;
00277 sx += vertices[i3];
00278 sy += vertices[i3 + 1];
00279 sz += vertices[i3 + 2];
00280 }
00281 sx /= (double)vertex_count;
00282 sy /= (double)vertex_count;
00283 sz /= (double)vertex_count;
00284
00285
00286 for (unsigned int i = 0 ; i < vertex_count ; ++i)
00287 {
00288 unsigned int i3 = i * 3;
00289
00290
00291 double dx = vertices[i3] - sx;
00292 double dy = vertices[i3 + 1] - sy;
00293 double dz = vertices[i3 + 2] - sz;
00294
00295
00296 double norm = sqrt(dx * dx + dy * dy + dz * dz);
00297 if (norm > 1e-6)
00298 {
00299 double fact = scale + padding/norm;
00300 vertices[i3] = sx + dx * fact;
00301 vertices[i3 + 1] = sy + dy * fact;
00302 vertices[i3 + 2] = sz + dz * fact;
00303 }
00304 else
00305 {
00306 double ndx = ((dx > 0) ? dx+padding : dx-padding);
00307 double ndy = ((dy > 0) ? dy+padding : dy-padding);
00308 double ndz = ((dz > 0) ? dz+padding : dz-padding);
00309 vertices[i3] = sx + ndx;
00310 vertices[i3 + 1] = sy + ndy;
00311 vertices[i3 + 2] = sz + ndz;
00312 }
00313 }
00314 }
00315
00316 void shapes::Shape::print(std::ostream &out) const
00317 {
00318 out << this << std::endl;
00319 }
00320
00321 void shapes::Sphere::print(std::ostream &out) const
00322 {
00323 out << "Sphere[radius=" << radius << "]" << std::endl;
00324 }
00325
00326 void shapes::Cylinder::print(std::ostream &out) const
00327 {
00328 out << "Cylinder[radius=" << radius << ", length=" << length << "]" << std::endl;
00329 }
00330
00331 void shapes::Cone::print(std::ostream &out) const
00332 {
00333 out << "Cone[radius=" << radius << ", length=" << length << "]" << std::endl;
00334 }
00335
00336 void shapes::Box::print(std::ostream &out) const
00337 {
00338 out << "Box[x=length=" << size[0] << ", y=width=" << size[1] << "z=height=" << size[2] << "]" << std::endl;
00339 }
00340
00341 void shapes::Mesh::print(std::ostream &out) const
00342 {
00343 out << "Mesh[vertices=" << vertex_count << ", triangles=" << triangle_count << "]" << std::endl;
00344 }
00345
00346 void shapes::Plane::print(std::ostream &out) const
00347 {
00348 out << "Plane[a=" << a << ", b=" << b << ", c=" << c << ", d=" << d << "]" << std::endl;
00349 }
00350
00351 void shapes::OcTree::print(std::ostream &out) const
00352 {
00353 if (octree)
00354 {
00355 double minx, miny, minz, maxx, maxy, maxz;
00356 octree->getMetricMin(minx, miny, minz);
00357 octree->getMetricMax(maxx, maxy, maxz);
00358 out << "OcTree[depth = " << octree->getTreeDepth() << ", resolution = " << octree->getResolution()
00359 << " inside box (minx=" << minx << ", miny=" << miny << ", minz=" << minz << ", maxx=" << maxx
00360 << ", maxy=" << maxy << ", maxz=" << maxz << ")]" << std::endl;
00361 }
00362 else
00363 out << "OcTree[NULL]" << std::endl;
00364 }
00365
00366 bool shapes::Shape::isFixed() const
00367 {
00368 return false;
00369 }
00370
00371 bool shapes::OcTree::isFixed() const
00372 {
00373 return true;
00374 }
00375
00376 bool shapes::Plane::isFixed() const
00377 {
00378 return true;
00379 }
00380
00381 void shapes::Mesh::computeTriangleNormals()
00382 {
00383 if (triangle_count && !triangle_normals)
00384 triangle_normals = new double[triangle_count * 3];
00385
00386
00387 for (unsigned int i = 0 ; i < triangle_count ; ++i)
00388 {
00389 unsigned int i3 = i * 3;
00390 Eigen::Vector3d s1(vertices[triangles[i3] * 3] - vertices[triangles[i3 + 1] * 3],
00391 vertices[triangles[i3] * 3 + 1] - vertices[triangles[i3 + 1] * 3 + 1],
00392 vertices[triangles[i3] * 3 + 2] - vertices[triangles[i3 + 1] * 3 + 2]);
00393 Eigen::Vector3d s2(vertices[triangles[i3 + 1] * 3] - vertices[triangles[i3 + 2] * 3],
00394 vertices[triangles[i3 + 1] * 3 + 1] - vertices[triangles[i3 + 2] * 3 + 1],
00395 vertices[triangles[i3 + 1] * 3 + 2] - vertices[triangles[i3 + 2] * 3 + 2]);
00396 Eigen::Vector3d normal = s1.cross(s2);
00397 normal.normalize();
00398 triangle_normals[i3 ] = normal.x();
00399 triangle_normals[i3 + 1] = normal.y();
00400 triangle_normals[i3 + 2] = normal.z();
00401 }
00402 }
00403
00404 void shapes::Mesh::computeVertexNormals()
00405 {
00406 if (!triangle_normals)
00407 computeTriangleNormals();
00408 if (vertex_count && !vertex_normals)
00409 vertex_normals = new double[vertex_count * 3];
00410 EigenSTL::vector_Vector3d avg_normals(vertex_count, Eigen::Vector3d(0, 0, 0));
00411
00412 for (unsigned int tIdx = 0; tIdx < triangle_count; ++tIdx)
00413 {
00414 unsigned int tIdx3 = 3 * tIdx;
00415 unsigned int tIdx3_1 = tIdx3 + 1;
00416 unsigned int tIdx3_2 = tIdx3 + 2;
00417
00418 unsigned int v1 = triangles [tIdx3];
00419 unsigned int v2 = triangles [tIdx3_1];
00420 unsigned int v3 = triangles [tIdx3_2];
00421
00422 avg_normals[v1][0] += triangle_normals [tIdx3];
00423 avg_normals[v1][1] += triangle_normals [tIdx3_1];
00424 avg_normals[v1][2] += triangle_normals [tIdx3_2];
00425
00426 avg_normals[v2][0] += triangle_normals [tIdx3];
00427 avg_normals[v2][1] += triangle_normals [tIdx3_1];
00428 avg_normals[v2][2] += triangle_normals [tIdx3_2];
00429
00430 avg_normals[v3][0] += triangle_normals [tIdx3];
00431 avg_normals[v3][1] += triangle_normals [tIdx3_1];
00432 avg_normals[v3][2] += triangle_normals [tIdx3_2];
00433 }
00434 for (std::size_t i = 0 ; i < avg_normals.size() ; ++i)
00435 {
00436 if (avg_normals[i].squaredNorm () > 0.0)
00437 avg_normals[i].normalize();
00438 unsigned int i3 = i * 3;
00439 vertex_normals[i3] = avg_normals[i][0];
00440 vertex_normals[i3 + 1] = avg_normals[i][1];
00441 vertex_normals[i3 + 2] = avg_normals[i][2];
00442 }
00443 }
00444
00445 void shapes::Mesh::mergeVertices(double threshold)
00446 {
00447 const double thresholdSQR = threshold * threshold;
00448
00449 std::vector<unsigned int> vertex_map(vertex_count);
00450 EigenSTL::vector_Vector3d orig_vertices(vertex_count);
00451 EigenSTL::vector_Vector3d compressed_vertices;
00452
00453 for (unsigned int vIdx = 0; vIdx < vertex_count ; ++vIdx)
00454 {
00455 orig_vertices[vIdx][0] = vertices[3 * vIdx];
00456 orig_vertices[vIdx][1] = vertices[3 * vIdx + 1];
00457 orig_vertices[vIdx][2] = vertices[3 * vIdx + 2];
00458 vertex_map[vIdx] = vIdx;
00459 }
00460
00461 for (unsigned int vIdx1 = 0; vIdx1 < vertex_count; ++vIdx1)
00462 {
00463 if (vertex_map[vIdx1] != vIdx1)
00464 continue;
00465
00466 vertex_map[vIdx1] = compressed_vertices.size();
00467 compressed_vertices.push_back(orig_vertices[vIdx1]);
00468
00469 for (unsigned int vIdx2 = vIdx1 + 1 ; vIdx2 < vertex_count ; ++vIdx2)
00470 {
00471 double distanceSQR = (orig_vertices[vIdx1] - orig_vertices[vIdx2]).squaredNorm();
00472 if (distanceSQR <= thresholdSQR)
00473 vertex_map[vIdx2] = vertex_map[vIdx1];
00474 }
00475 }
00476
00477 if (compressed_vertices.size() == orig_vertices.size())
00478 return;
00479
00480
00481 for (unsigned int tIdx = 0; tIdx < triangle_count; ++tIdx)
00482 {
00483 unsigned int i3 = 3 * tIdx;
00484 triangles[i3] = vertex_map[triangles [i3]];
00485 triangles[i3 + 1] = vertex_map[triangles [i3 + 1]];
00486 triangles[i3 + 2] = vertex_map[triangles [i3 + 2]];
00487 }
00488
00489 vertex_count = compressed_vertices.size();
00490 delete[] vertices;
00491 vertices = new double[vertex_count * 3];
00492
00493 for (unsigned int vIdx = 0; vIdx < vertex_count ; ++vIdx)
00494 {
00495 unsigned int i3 = 3 * vIdx;
00496 vertices[i3] = compressed_vertices[vIdx][0];
00497 vertices[i3 + 1] = compressed_vertices[vIdx][1];
00498 vertices[i3 + 2] = compressed_vertices[vIdx][2];
00499 }
00500
00501 if (triangle_normals)
00502 computeTriangleNormals();
00503 if (vertex_normals)
00504 computeVertexNormals();
00505 }