diff --git a/cmake b/cmake index f9f195355..b0a77e2ea 160000 --- a/cmake +++ b/cmake @@ -1 +1 @@ -Subproject commit f9f19535583faa2bb2ff21d97cb33f8ab7624d36 +Subproject commit b0a77e2ead3f8dbb201b05e42048bb1cfc6a518e diff --git a/include/hpp/fcl/octree.h b/include/hpp/fcl/octree.h index 3884e6278..ddca1da5b 100644 --- a/include/hpp/fcl/octree.h +++ b/include/hpp/fcl/octree.h @@ -303,9 +303,8 @@ static inline void computeChildBV(const AABB& root_bv, unsigned int i, AABB& chi /// /// \returns An OcTree that can be used for collision checking and more. /// - HPP_FCL_DLLAPI OcTreePtr_t makeOctree - (const Eigen::Matrix & point_cloud, - const FCL_REAL & resolution); + HPP_FCL_DLLAPI OcTreePtr_t makeOctree(const Eigen::Matrix & point_cloud, + const FCL_REAL resolution); } diff --git a/src/octree.cpp b/src/octree.cpp index 87712e5b5..00a8fa7fd 100644 --- a/src/octree.cpp +++ b/src/octree.cpp @@ -1,36 +1,36 @@ /* -* Software License Agreement (BSD License) -* -* Copyright (c) 2020, INRIA -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Open Source Robotics Foundation nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, INRIA + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ #include @@ -40,165 +40,200 @@ namespace fcl { namespace internal { - struct Neighbors - { - char value; - Neighbors() : value(0){} - bool minusX() const{ - return value & 0x1; - } - bool plusX() const{ - return value & 0x2; - } - bool minusY() const{ - return value & 0x4; - } - bool plusY() const{ - return value & 0x8; - } - bool minusZ() const{ - return value & 0x10; - } - bool plusZ() const{ - return value & 0x20; - } - void hasNeighboordMinusX(){ - value |= 0x1; - } - void hasNeighboordPlusX(){ - value |= 0x2; - } - void hasNeighboordMinusY(){ - value |= 0x4; - } - void hasNeighboordPlusY(){ - value |= 0x8; - } - void hasNeighboordMinusZ(){ - value |= 0x10; - } - void hasNeighboordPlusZ(){ - value |= 0x20; - } - }; //struct neighbors - - void computeNeighbors - (const std::vector >& boxes, - std::vector& neighbors) - { - FCL_REAL fixedSize = -1; - FCL_REAL e(1e-8); - for (std::size_t i=0; i < boxes.size(); ++i){ - const boost::array& box(boxes[i]); - Neighbors& n(neighbors[i]); - FCL_REAL x(box[0]); - FCL_REAL y(box[1]); - FCL_REAL z(box[2]); - FCL_REAL s(box[3]); - if (fixedSize == -1) - fixedSize = s; - else - assert(s == fixedSize); +struct Neighbors +{ + char value; + Neighbors() : value(0){} + bool minusX() const{ + return value & 0x1; + } + bool plusX() const{ + return value & 0x2; + } + bool minusY() const{ + return value & 0x4; + } + bool plusY() const{ + return value & 0x8; + } + bool minusZ() const{ + return value & 0x10; + } + bool plusZ() const{ + return value & 0x20; + } + void hasNeighboordMinusX(){ + value |= 0x1; + } + void hasNeighboordPlusX(){ + value |= 0x2; + } + void hasNeighboordMinusY(){ + value |= 0x4; + } + void hasNeighboordPlusY(){ + value |= 0x8; + } + void hasNeighboordMinusZ(){ + value |= 0x10; + } + void hasNeighboordPlusZ(){ + value |= 0x20; + } +}; //struct neighbors - for (auto otherBox : boxes){ - FCL_REAL xo(otherBox[0]); - FCL_REAL yo(otherBox[1]); - FCL_REAL zo(otherBox[2]); - // if (fabs(x-xo) < e && fabs(y-yo) < e && fabs(z-zo) < e){ - // continue; - // } - if ((fabs(x-xo-s) < e)&&(fabs(y-yo) < e)&&(fabs(z-zo) < e)){ - n.hasNeighboordMinusX(); - } else if((fabs(x-xo+s) < e)&&(fabs(y-yo) < e)&&(fabs(z-zo) < e)){ - n.hasNeighboordPlusX(); - } else if ((fabs(x-xo) < e)&&(fabs(y-yo-s) < e)&&(fabs(z-zo) < e)){ - n.hasNeighboordMinusY(); - } else if((fabs(x-xo) < e)&&(fabs(y-yo+s) < e)&&(fabs(z-zo) < e)){ - n.hasNeighboordPlusY(); - } else if ((fabs(x-xo) < e)&&(fabs(y-yo) < e)&&(fabs(z-zo-s) < e)){ - n.hasNeighboordMinusZ(); - } else if((fabs(x-xo) < e)&&(fabs(y-yo) < e)&&(fabs(z-zo+s) < e)){ - n.hasNeighboordPlusZ(); - } +void computeNeighbors(const std::vector >& boxes, + std::vector& neighbors) +{ + typedef std::vector > VectorArray6; + FCL_REAL fixedSize = -1; + FCL_REAL e(1e-8); + for (std::size_t i=0; i < boxes.size(); ++i){ + const boost::array& box(boxes[i]); + Neighbors& n(neighbors[i]); + FCL_REAL x(box[0]); + FCL_REAL y(box[1]); + FCL_REAL z(box[2]); + FCL_REAL s(box[3]); + if (fixedSize == -1) + fixedSize = s; + else + assert(s == fixedSize); + + for(VectorArray6::const_iterator it = boxes.begin(); + it != boxes.end(); ++it) + { + const boost::array & otherBox = *it; + FCL_REAL xo(otherBox[0]); + FCL_REAL yo(otherBox[1]); + FCL_REAL zo(otherBox[2]); + // if (fabs(x-xo) < e && fabs(y-yo) < e && fabs(z-zo) < e){ + // continue; + // } + if ((fabs(x-xo-s) < e)&&(fabs(y-yo) < e)&&(fabs(z-zo) < e)){ + n.hasNeighboordMinusX(); + } else if((fabs(x-xo+s) < e)&&(fabs(y-yo) < e)&&(fabs(z-zo) < e)){ + n.hasNeighboordPlusX(); + } else if ((fabs(x-xo) < e)&&(fabs(y-yo-s) < e)&&(fabs(z-zo) < e)){ + n.hasNeighboordMinusY(); + } else if((fabs(x-xo) < e)&&(fabs(y-yo+s) < e)&&(fabs(z-zo) < e)){ + n.hasNeighboordPlusY(); + } else if ((fabs(x-xo) < e)&&(fabs(y-yo) < e)&&(fabs(z-zo-s) < e)){ + n.hasNeighboordMinusZ(); + } else if((fabs(x-xo) < e)&&(fabs(y-yo) < e)&&(fabs(z-zo+s) < e)){ + n.hasNeighboordPlusZ(); } } } +} } // namespace internal - void OcTree::exportAsObjFile(const std::string& filename) const +void OcTree::exportAsObjFile(const std::string& filename) const +{ + std::vector > boxes(this->toBoxes()); + std::vector neighbors(boxes.size()); + internal::computeNeighbors(boxes, neighbors); + // compute list of vertices and faces + + typedef std::vector VectorVec3f; + std::vector vertices; + + typedef boost::array Array4; + typedef std::vector VectorArray4; + std::vector faces; + + for (std::size_t i=0; i > boxes(this->toBoxes()); - std::vector neighbors(boxes.size()); - internal::computeNeighbors(boxes, neighbors); - // compute list of vertices and faces - std::vector > vertices; - std::vector > faces; - for (std::size_t i=0; i& box(boxes[i]); - internal::Neighbors& n(neighbors[i]); - FCL_REAL x(box[0]); - FCL_REAL y(box[1]); - FCL_REAL z(box[2]); - FCL_REAL size(box[3]); - vertices.push_back({x-.5*size, y-.5*size, z-.5*size}); - vertices.push_back({x+.5*size, y-.5*size, z-.5*size}); - vertices.push_back({x-.5*size, y+.5*size, z-.5*size}); - vertices.push_back({x+.5*size, y+.5*size, z-.5*size}); - vertices.push_back({x-.5*size, y-.5*size, z+.5*size}); - vertices.push_back({x+.5*size, y-.5*size, z+.5*size}); - vertices.push_back({x-.5*size, y+.5*size, z+.5*size}); - vertices.push_back({x+.5*size, y+.5*size, z+.5*size}); - // Add face only if box has no neighbor with the same face - if (!n.minusX()) - faces.push_back({8*i + 1, 8*i + 5, 8*i + 7, 8*i + 3}); - if (!n.plusX()) - faces.push_back({8*i + 2, 8*i + 4, 8*i + 8, 8*i + 6}); - if (!n.minusY()) - faces.push_back({8*i + 1, 8*i + 2, 8*i + 6, 8*i + 5}); - if (!n.plusY()) - faces.push_back({8*i + 4, 8*i + 3, 8*i + 7, 8*i + 8}); - if (!n.minusZ()) - faces.push_back({8*i + 1, 8*i + 2, 8*i + 4, 8*i + 3}); - if (!n.plusZ()) - faces.push_back({8*i + 5, 8*i + 6, 8*i + 8, 8*i + 7}); - + const boost::array& box(boxes[i]); + internal::Neighbors& n(neighbors[i]); + + FCL_REAL x(box[0]); + FCL_REAL y(box[1]); + FCL_REAL z(box[2]); + FCL_REAL size(box[3]); + + vertices.push_back(Vec3f(x-.5*size, y-.5*size, z-.5*size)); + vertices.push_back(Vec3f(x+.5*size, y-.5*size, z-.5*size)); + vertices.push_back(Vec3f(x-.5*size, y+.5*size, z-.5*size)); + vertices.push_back(Vec3f(x+.5*size, y+.5*size, z-.5*size)); + vertices.push_back(Vec3f(x-.5*size, y-.5*size, z+.5*size)); + vertices.push_back(Vec3f(x+.5*size, y-.5*size, z+.5*size)); + vertices.push_back(Vec3f(x-.5*size, y+.5*size, z+.5*size)); + vertices.push_back(Vec3f(x+.5*size, y+.5*size, z+.5*size)); + + // Add face only if box has no neighbor with the same face + if (!n.minusX()) + { + Array4 a = {{8*i + 1, 8*i + 5, 8*i + 7, 8*i + 3}}; + faces.push_back(a); } - // write obj in a file - std::ofstream os; - os.open(filename); - if (!os.is_open()) - throw std::runtime_error(std::string("failed to open file \"")+ - filename + std::string("\"")); - // write vertices - os << "# list of vertices" << std::endl; - for (auto v : vertices){ - os << "v " << v[0] << " " << v[1] << " " << v[2] << std::endl; + if (!n.plusX()) + { + Array4 a = {{8*i + 2, 8*i + 4, 8*i + 8, 8*i + 6}}; + faces.push_back(a); } - os << std::endl << "# list of faces" << std::endl; - for (auto f : faces) { - os << "f " << f[0] << " " << f[1] << " " << f[2] << " " << f[3] - << std::endl; + if (!n.minusY()) + { + Array4 a = {{8*i + 1, 8*i + 2, 8*i + 6, 8*i + 5}}; + faces.push_back(a); } - } - - OcTreePtr_t makeOctree - (const Eigen::Matrix& point_cloud, - const FCL_REAL & resolution) - { - typedef Eigen::Matrix InputType; - typedef InputType::ConstRowXpr RowType; - - boost::shared_ptr octree(new octomap::OcTree(resolution)); - for(Eigen::DenseIndex row_id = 0; row_id < point_cloud.rows(); ++row_id) + if (!n.plusY()) { - RowType row = point_cloud.row(row_id); - octomap::point3d p(static_cast(row[0]),static_cast(row[1]),static_cast(row[2])); - octree->updateNode(p, true); + Array4 a = {{8*i + 4, 8*i + 3, 8*i + 7, 8*i + 8}}; + faces.push_back(a); + } + if (!n.minusZ()) + { + Array4 a = {{8*i + 1, 8*i + 2, 8*i + 4, 8*i + 3}}; + faces.push_back(a); + } + if (!n.plusZ()) + { + Array4 a = {{8*i + 5, 8*i + 6, 8*i + 8, 8*i + 7}}; + faces.push_back(a); } - octree->updateInnerOccupancy(); - return OcTreePtr_t (new OcTree(octree)); } + // write obj in a file + std::ofstream os; + os.open(filename); + if (!os.is_open()) + throw std::runtime_error(std::string("failed to open file \"")+ + filename + std::string("\"")); + // write vertices + os << "# list of vertices\n"; + for (VectorVec3f::const_iterator it = vertices.begin(); + it != vertices.end(); ++it) + { + const Vec3f & v = *it; + os << "v " << v[0] << " " << v[1] << " " << v[2] << '\n'; + } + os << "\n# list of faces\n"; + for (VectorArray4::const_iterator it = faces.begin(); + it != faces.end(); ++it) + { + const Array4 & f = *it; + os << "f " << f[0] << " " << f[1] << " " << f[2] << " " << f[3] << '\n'; + } +} + +OcTreePtr_t makeOctree(const Eigen::Matrix& point_cloud, + const FCL_REAL resolution) +{ + typedef Eigen::Matrix InputType; + typedef InputType::ConstRowXpr RowType; + + boost::shared_ptr octree(new octomap::OcTree(resolution)); + for(Eigen::DenseIndex row_id = 0; row_id < point_cloud.rows(); ++row_id) + { + RowType row = point_cloud.row(row_id); + octomap::point3d p(static_cast(row[0]),static_cast(row[1]),static_cast(row[2])); + octree->updateNode(p, true); + } + octree->updateInnerOccupancy(); + + return OcTreePtr_t (new OcTree(octree)); +} } }