shapes.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2011, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 /* Author: Ioan Sucan */
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   // find the center of the mesh
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   // scale the mesh
00286   for (unsigned int i = 0 ; i < vertex_count ; ++i)
00287   {
00288     unsigned int i3 = i * 3;
00289 
00290     // vector from center to the vertex
00291     double dx = vertices[i3] - sx;
00292     double dy = vertices[i3 + 1] - sy;
00293     double dz = vertices[i3 + 2] - sz;
00294 
00295     // length of vector
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   // compute normals
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   // redirect triangles to new vertices!
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 }


geometric_shapes
Author(s): Ioan Sucan , Gil Jones
autogenerated on Thu Jun 6 2019 20:13:53