diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 270fc27..1bf85fc 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -8,6 +8,7 @@ on: pull_request: push: branches: + - noetic-devel - melodic-devel jobs: @@ -15,13 +16,13 @@ jobs: strategy: matrix: env: - - ROS_DISTRO: kinetic - ROS_REPO: main - ROS_DISTRO: melodic ROS_REPO: main + - ROS_DISTRO: noetic + ROS_REPO: main CLANG_TIDY: pedantic CATKIN_LINT: pedantic - - ROS_DISTRO: melodic + - ROS_DISTRO: noetic ROS_REPO: testing env: CCACHE_DIR: ${{ github.workspace }}/.ccache diff --git a/.github/workflows/prerelease.yaml b/.github/workflows/prerelease.yaml index bf2e944..c2a7121 100644 --- a/.github/workflows/prerelease.yaml +++ b/.github/workflows/prerelease.yaml @@ -11,7 +11,7 @@ jobs: strategy: fail-fast: false matrix: - distro: [melodic] + distro: [noetic] env: ROS_DISTRO: ${{ matrix.distro }} diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 2824ff4..d9fa186 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -34,7 +34,7 @@ repos: - id: trailing-whitespace - repo: https://github.com/psf/black - rev: 20.8b1 + rev: 22.3.0 hooks: - id: black diff --git a/CHANGELOG.rst b/CHANGELOG.rst index a5519a1..619a07c 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,27 +2,56 @@ Changelog for package geometric_shapes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -0.6.6 (2023-04-12) +0.7.6 (2024-05-07) ------------------ -* Limit indefinite growth of OBBs during merging (`#233 `_) +* Improve padding of meshes using weighted vertex normals (`#238 `_) +* Drop obsolete C++ standard definition (`#235 `_) +* Contributors: Kenji Brameld (TRACLabs), Michael Görner + +0.7.5 (2023-04-12) +------------------ +* Limit indefinite growth of OBBs during merging (`#232 `_) * Correctly initialize OBB with default constructor * Contributors: Martin Pecka -0.6.5 (2023-03-31) +0.7.4 (2023-03-31) ------------------ -* Add bodies::Body::computeBoundingBox (oriented box version) (`#210 `_) +* Body::getScaledDimensions(): avoid vtable lookup (`#225 `_) +* Add Body::computeBoundingBox (oriented box version) (`#210 `_) This adds the dependency on FCL, provide support for FCL 0.6 if available -* Add body operations constructShapeFromBody() and constructMarkerFromBody() (`#209 `_) -* Eigen < 3.3.0 needs explicit cast (`#224 `_) -* Contributors: Kei Okada, Martin Pecka, Robert Haschke +* Contributors: Martin Pecka, Robert Haschke -0.6.4 (2021-05-19) +0.7.3 (2021-05-19) ------------------ * [fix] Fix memory leak (`#168 `_) * [fix] Use proper Eigen alignment for make_shared calls (`#187 `_) -* [maint] Migrate from Travis to GitHub actions (`#172 `_) -* Contributors: Dave Coleman, Martin Pecka, Robert Haschke, Tyler Weaver +* [maint] Migrate from Travis to GitHub Actions (`#171 `_) +* Contributors: Robert Haschke, Tyler Weaver + +0.7.2 (2020-09-25) +------------------ +* [maint] Renamed SolidPrimitiveDimCount::value -> solidPrimitiveDimCount() (`#121 `_) +* [maint] cmake: Consistently use uppercase letters for QHULL dependency +* [maint] cmake: Fix assimp warning +* [maint] Update build badges for Noetic +* Contributors: Robert Haschke + +0.7.1 (2020-08-31) +------------------ +* [maint] Declare external includes as SYSTEM includes +* [maint] Migration to reentrant qhull (`#149 `_) +* [maint] Use soname version for library (`#157 `_) +* Contributors: Jochen Sprickerhof, Robert Haschke, Tyler Weaver + +0.7.0 (2020-05-25) +------------------ +* [feature] Added constructShapeFromBody() and constructMarkerFromBody() (`#138 `_) +* [maint] API cleanup + * Improve inlining + * ConvexMesh::MeshData as pimpl + * Reverted ABI compatibility fixups for Melodic: ed4cf1339cf3765ae9ffa6e6fd111a4e342c5fa2, d582479084a10cac53a7f17e29818b3d8be6161e +* Contributors: Martin Pecka, Robert Haschke 0.6.3 (2020-05-25) ------------------ diff --git a/CMakeLists.txt b/CMakeLists.txt index 95c28f9..ccc9864 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,8 +1,6 @@ cmake_minimum_required(VERSION 3.0.2) project(geometric_shapes) -add_compile_options(-std=c++11) - # Set compile options set(PROJECT_COMPILE_OPTIONS -Wall @@ -16,7 +14,7 @@ set(PROJECT_COMPILE_OPTIONS list(APPEND CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake/") -if (NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) +if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) message("${PROJECT_NAME}: You did not request a specific build type: Choosing 'Release' for maximum performance") set(CMAKE_BUILD_TYPE Release) endif() @@ -80,10 +78,7 @@ catkin_package( console_bridge ) -find_package(Qhull REQUIRED) -if (HAVE_QHULL_2011) - add_definitions(-DGEOMETRIC_SHAPES_HAVE_QHULL_2011) -endif() +find_package(QHULL REQUIRED) include_directories(include) include_directories(SYSTEM @@ -102,6 +97,7 @@ add_library(${PROJECT_NAME} src/shapes.cpp ) target_compile_options(${PROJECT_NAME} PRIVATE ${PROJECT_COMPILE_OPTIONS}) +set_target_properties(${PROJECT_NAME} PROPERTIES VERSION ${${PROJECT_NAME}_VERSION}) target_link_libraries(${PROJECT_NAME} ${ASSIMP_LIBRARIES} ${QHULL_LIBRARIES} ${catkin_LIBRARIES} ${console_bridge_LIBRARIES} ${Boost_LIBRARIES} ${LIBFCL_LIBRARIES}) diff --git a/README.md b/README.md index 22a5c7f..f405ce6 100644 --- a/README.md +++ b/README.md @@ -15,12 +15,16 @@ Note: Bodies for meshes compute the convex hull of those meshes in order to prov Note: [shape_tools](https://github.com/ros-planning/shape_tools) package was recently merged into this package +Note: `bodies::Box::corner1_` was renamed to `minCorner_` and `bodies::Box::corner2_` to `maxCorner_` in Noetic. + +Note: `bodies::ConvexMesh::MeshData` was made implementation-private in Noetic and is no longer accessible from the `.h` file. + ## Build Status GitHub Actions: -[![Format](https://github.com/ros-planning/geometric_shapes/actions/workflows/format.yaml/badge.svg?branch=melodic-devel)](https://github.com/ros-planning/geometric_shapes/actions/workflows/format.yaml?query=branch%3Amelodic-devel) -[![CI](https://github.com/ros-planning/geometric_shapes/actions/workflows/ci.yaml/badge.svg?branch=melodic-devel)](https://github.com/ros-planning/geometric_shapes/actions/workflows/ci.yaml?query=branch%3Amelodic-devel) +[![Format](https://github.com/ros-planning/geometric_shapes/actions/workflows/format.yaml/badge.svg?branch=noetic-devel)](https://github.com/ros-planning/geometric_shapes/actions/workflows/format.yaml?query=branch%3Anoetic-devel) +[![CI](https://github.com/ros-planning/geometric_shapes/actions/workflows/ci.yaml/badge.svg?branch=noetic-devel)](https://github.com/ros-planning/geometric_shapes/actions/workflows/ci.yaml?query=branch%3Anoetic-devel) -Devel Job: [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__geometric_shapes__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__geometric_shapes__ubuntu_bionic__source) +Devel Job: [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__geometric_shapes__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__geometric_shapes__ubuntu_focal__source) -Debian Job: [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__geometric_shapes__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__geometric_shapes__ubuntu_bionic_amd64__binary) +Debian Job: [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__geometric_shapes__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__geometric_shapes__ubuntu_focal_amd64__binary) diff --git a/cmake/FindQhull.cmake b/cmake/FindQHULL.cmake similarity index 59% rename from cmake/FindQhull.cmake rename to cmake/FindQHULL.cmake index 1253e16..e7cc66c 100644 --- a/cmake/FindQhull.cmake +++ b/cmake/FindQHULL.cmake @@ -5,37 +5,20 @@ # QHULL_FOUND - True if QHULL was found. # QHULL_INCLUDE_DIRS - Directories containing the QHULL include files. # QHULL_LIBRARIES - Libraries needed to use QHULL. -# If QHULL_USE_STATIC is specified then look for static libraries ONLY else -# look for shared ones -set(QHULL_MAJOR_VERSION 6) - -if(QHULL_USE_STATIC) - set(QHULL_RELEASE_NAME qhullstatic) - set(QHULL_DEBUG_NAME qhullstatic_d) -else(QHULL_USE_STATIC) - set(QHULL_RELEASE_NAME qhull_p qhull${QHULL_MAJOR_VERSION} qhull) - set(QHULL_DEBUG_NAME qhull_pd qhull${QHULL_MAJOR_VERSION}_d qhull_d${QHULL_MAJOR_VERSION} qhull_d) -endif(QHULL_USE_STATIC) +set(QHULL_RELEASE_NAME qhull_r) +set(QHULL_DEBUG_NAME qhull_rd) find_file(QHULL_HEADER - NAMES libqhull/libqhull.h qhull.h + NAMES libqhull_r.h HINTS "${QHULL_ROOT}" "$ENV{QHULL_ROOT}" "${QHULL_INCLUDE_DIR}" PATHS "$ENV{PROGRAMFILES}/QHull" "$ENV{PROGRAMW6432}/QHull" - PATH_SUFFIXES qhull src/libqhull libqhull include) + PATH_SUFFIXES libqhull_r) -set(QHULL_HEADER "${QHULL_HEADER}" CACHE INTERNAL "QHull header" FORCE ) +set(QHULL_HEADER "${QHULL_HEADER}" CACHE INTERNAL "QHull header" FORCE) if(QHULL_HEADER) - get_filename_component(qhull_header ${QHULL_HEADER} NAME_WE) - if("${qhull_header}" STREQUAL "qhull") - set(HAVE_QHULL_2011 OFF) - get_filename_component(QHULL_INCLUDE_DIR ${QHULL_HEADER} PATH) - elseif("${qhull_header}" STREQUAL "libqhull") - set(HAVE_QHULL_2011 ON) - get_filename_component(QHULL_INCLUDE_DIR ${QHULL_HEADER} PATH) - get_filename_component(QHULL_INCLUDE_DIR ${QHULL_INCLUDE_DIR} PATH) - endif() + get_filename_component(QHULL_INCLUDE_DIR ${QHULL_HEADER} PATH) else(QHULL_HEADER) set(QHULL_INCLUDE_DIR "QHULL_INCLUDE_DIR-NOTFOUND") endif(QHULL_HEADER) @@ -62,17 +45,11 @@ set(QHULL_INCLUDE_DIRS ${QHULL_INCLUDE_DIR}) set(QHULL_LIBRARIES optimized ${QHULL_LIBRARY} debug ${QHULL_LIBRARY_DEBUG}) include(FindPackageHandleStandardArgs) -find_package_handle_standard_args(Qhull DEFAULT_MSG QHULL_LIBRARY QHULL_INCLUDE_DIR) +find_package_handle_standard_args(QHULL DEFAULT_MSG QHULL_LIBRARY QHULL_INCLUDE_DIR) mark_as_advanced(QHULL_LIBRARY QHULL_LIBRARY_DEBUG QHULL_INCLUDE_DIR) if(QHULL_FOUND) set(HAVE_QHULL ON) - if(NOT QHULL_USE_STATIC) - add_definitions("-Dqh_QHpointer") - if(MSVC) - add_definitions("-Dqh_QHpointer_dllimport") - endif(MSVC) - endif(NOT QHULL_USE_STATIC) message(STATUS "QHULL found (include: ${QHULL_INCLUDE_DIRS}, lib: ${QHULL_LIBRARIES})") endif(QHULL_FOUND) diff --git a/codecov.yaml b/codecov.yaml new file mode 100644 index 0000000..3999cdb --- /dev/null +++ b/codecov.yaml @@ -0,0 +1,10 @@ +coverage: + precision: 2 + round: up + range: "45...70" + status: + project: + default: + target: auto + threshold: 5% + patch: off diff --git a/include/geometric_shapes/bodies.h b/include/geometric_shapes/bodies.h index a288965..f4ff6e2 100644 --- a/include/geometric_shapes/bodies.h +++ b/include/geometric_shapes/bodies.h @@ -94,12 +94,10 @@ class Body pose_.setIdentity(); } - virtual ~Body() - { - } + virtual ~Body() = default; /** \brief Get the type of shape this body represents */ - shapes::ShapeType getType() const + inline shapes::ShapeType getType() const { return type_; } @@ -112,21 +110,21 @@ class Body * internal structures after each call. When you are finished with the batch, call updateInternalData(). * \param scale The scale to set. 1.0 means no scaling. */ - void setScaleDirty(double scale) + inline void setScaleDirty(double scale) { scale_ = scale; } /** \brief If the dimension of the body should be scaled, this method sets the scale. Default is 1.0 */ - void setScale(double scale) + inline void setScale(double scale) { setScaleDirty(scale); updateInternalData(); } /** \brief Retrieve the current scale */ - double getScale() const + inline double getScale() const { return scale_; } @@ -139,21 +137,21 @@ class Body * internal structures after each call. When you are finished with the batch, call updateInternalData(). * \param padd The padding to set (in meters). 0.0 means no padding. */ - void setPaddingDirty(double padd) + inline void setPaddingDirty(double padd) { padding_ = padd; } /** \brief If constant padding should be added to the body, this method sets the padding. Default is 0.0 */ - void setPadding(double padd) + inline void setPadding(double padd) { setPaddingDirty(padd); updateInternalData(); } /** \brief Retrieve the current padding */ - double getPadding() const + inline double getPadding() const { return padding_; } @@ -166,20 +164,20 @@ class Body * internal structures after each call. When you are finished with the batch, call updateInternalData(). * \param pose The pose to set. Default is identity. */ - void setPoseDirty(const Eigen::Isometry3d& pose) + inline void setPoseDirty(const Eigen::Isometry3d& pose) { pose_ = pose; } /** \brief Set the pose of the body. Default is identity */ - void setPose(const Eigen::Isometry3d& pose) + inline void setPose(const Eigen::Isometry3d& pose) { setPoseDirty(pose); updateInternalData(); } /** \brief Retrieve the pose of the body */ - const Eigen::Isometry3d& getPose() const + inline const Eigen::Isometry3d& getPose() const { return pose_; } @@ -204,10 +202,14 @@ class Body virtual std::vector getScaledDimensions() const = 0; /** \brief Set the dimensions of the body (from corresponding shape) */ - void setDimensions(const shapes::Shape* shape); + inline void setDimensions(const shapes::Shape* shape) + { + setDimensionsDirty(shape); + updateInternalData(); + } /** \brief Check if a point is inside the body */ - bool containsPoint(double x, double y, double z, bool verbose = false) const + inline bool containsPoint(double x, double y, double z, bool verbose = false) const { Eigen::Vector3d pt(x, y, z); return containsPoint(pt, verbose); @@ -253,7 +255,7 @@ class Body virtual void computeBoundingBox(OBB& bbox) const = 0; /** \brief Get a clone of this body, but one that is located at the pose \e pose */ - BodyPtr cloneAt(const Eigen::Isometry3d& pose) const + inline BodyPtr cloneAt(const Eigen::Isometry3d& pose) const { return cloneAt(pose, padding_, scale_); } @@ -304,20 +306,9 @@ class Sphere : public Body setDimensions(shape); } - explicit Sphere(const BoundingSphere& sphere) : Body() - { - type_ = shapes::SPHERE; - shapes::Sphere shape(sphere.radius); - setDimensionsDirty(&shape); - - Eigen::Isometry3d pose = Eigen::Isometry3d::Identity(); - pose.translation() = sphere.center; - setPose(pose); - } + explicit Sphere(const BoundingSphere& sphere); - ~Sphere() override - { - } + ~Sphere() override = default; /** \brief Get the radius of the sphere */ std::vector getDimensions() const override; @@ -368,17 +359,9 @@ class Cylinder : public Body setDimensions(shape); } - explicit Cylinder(const BoundingCylinder& cylinder) : Body() - { - type_ = shapes::CYLINDER; - shapes::Cylinder shape(cylinder.radius, cylinder.length); - setDimensionsDirty(&shape); - setPose(cylinder.pose); - } + explicit Cylinder(const BoundingCylinder& cylinder); - ~Cylinder() override - { - } + ~Cylinder() override = default; /** \brief Get the radius & length of the cylinder */ std::vector getDimensions() const override; @@ -439,20 +422,9 @@ class Box : public Body setDimensions(shape); } - explicit Box(const AABB& aabb) : Body() - { - type_ = shapes::BOX; - shapes::Box shape(aabb.sizes()[0], aabb.sizes()[1], aabb.sizes()[2]); - setDimensionsDirty(&shape); - - Eigen::Isometry3d pose = Eigen::Isometry3d::Identity(); - pose.translation() = aabb.center(); - setPose(pose); - } + explicit Box(const AABB& aabb); - ~Box() override - { - } + ~Box() override = default; /** \brief Get the length & width & height (x, y, z) of the box */ std::vector getDimensions() const override; @@ -483,12 +455,10 @@ class Box : public Body // pose/padding/scaling-dependent values & values computed for convenience and fast upcoming computations Eigen::Vector3d center_; - Eigen::Vector3d normalL_; - Eigen::Vector3d normalW_; - Eigen::Vector3d normalH_; + Eigen::Matrix3d invRot_; - Eigen::Vector3d corner1_; //!< The translated, but not rotated min corner - Eigen::Vector3d corner2_; //!< The translated, but not rotated max corner + Eigen::Vector3d minCorner_; //!< The translated, but not rotated min corner + Eigen::Vector3d maxCorner_; //!< The translated, but not rotated max corner double length2_; double width2_; @@ -517,7 +487,7 @@ class ConvexMesh : public Body setDimensions(shape); } - ~ConvexMesh() override; + ~ConvexMesh() override = default; /** \brief Returns an empty vector */ std::vector getDimensions() const override; @@ -567,20 +537,8 @@ class ConvexMesh : public Body */ bool isPointInsidePlanes(const Eigen::Vector3d& point) const; - struct MeshData - { - EigenSTL::vector_Vector4d planes_; - EigenSTL::vector_Vector3d vertices_; - std::vector triangles_; - std::map plane_for_triangle_; - Eigen::Vector3d mesh_center_; - double mesh_radiusB_; - Eigen::Vector3d box_offset_; - Eigen::Vector3d box_size_; - BoundingCylinder bounding_cylinder_; - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; + // PIMPL structure + struct MeshData; // shape-dependent data; keep this in one struct so that a cheap pointer copy can be done in cloneAt() std::shared_ptr mesh_data_; diff --git a/include/geometric_shapes/obb.h b/include/geometric_shapes/obb.h index 9584bfa..ed462fe 100644 --- a/include/geometric_shapes/obb.h +++ b/include/geometric_shapes/obb.h @@ -53,7 +53,8 @@ class OBBPrivate; class OBB { public: - /** Initialize an oriented bounding box at position 0, with 0 extents and identity orientation. */ + /** \brief Initialize an oriented bounding box at position 0, with 0 extents and + * identity orientation. */ OBB(); OBB(const OBB& other); OBB(const Eigen::Isometry3d& pose, const Eigen::Vector3d& extents); diff --git a/include/geometric_shapes/shapes.h b/include/geometric_shapes/shapes.h index e527a38..f255fba 100644 --- a/include/geometric_shapes/shapes.h +++ b/include/geometric_shapes/shapes.h @@ -331,7 +331,8 @@ class Mesh : public Shape /** \brief Compute the normals of each triangle from its vertices via cross product. */ void computeTriangleNormals(); - /** \brief Compute vertex normals by averaging from adjacent triangle normals. + /** \brief Compute vertex normals by averaging from adjacent triangle normals, weighted using magnitude of + * angles formed by adjacent triangles at the vertex. Calls computeTriangleNormals() if needed. */ void computeVertexNormals(); diff --git a/include/geometric_shapes/solid_primitive_dims.h b/include/geometric_shapes/solid_primitive_dims.h index d89aed9..0b16567 100644 --- a/include/geometric_shapes/solid_primitive_dims.h +++ b/include/geometric_shapes/solid_primitive_dims.h @@ -38,49 +38,44 @@ namespace geometric_shapes { -/** \brief The number of dimensions of a particular shape */ -template -struct SolidPrimitiveDimCount -{ - enum - { - value = 0u - }; -}; +/** Get the number of dimensions of a particular shape */ +template +constexpr unsigned int solidPrimitiveDimCount(); template <> -struct SolidPrimitiveDimCount +constexpr unsigned int solidPrimitiveDimCount() { - enum - { - value = 1u - }; -}; + return 1u; +} template <> -struct SolidPrimitiveDimCount +constexpr unsigned int solidPrimitiveDimCount() { - enum - { - value = 3u - }; -}; + return 3u; +} template <> -struct SolidPrimitiveDimCount +constexpr unsigned int solidPrimitiveDimCount() { - enum - { - value = 2u - }; -}; + return 2u; +} template <> -struct SolidPrimitiveDimCount +constexpr unsigned int solidPrimitiveDimCount() +{ + return 2u; +} + +// clang-format off +template +struct [[deprecated("Replace SolidPrimitiveDimCount::value with solidPrimitiveDimCount()")]] +SolidPrimitiveDimCount +// clang-format on { enum { - value = 2u + value = solidPrimitiveDimCount() }; }; + } // namespace geometric_shapes diff --git a/package.xml b/package.xml index d4b5ee4..d611171 100644 --- a/package.xml +++ b/package.xml @@ -1,6 +1,6 @@ geometric_shapes - 0.6.6 + 0.7.6 Generic definitions of geometric shapes and bodies. Ioan Sucan diff --git a/src/aabb.cpp b/src/aabb.cpp index 64c2cc9..f119fd2 100644 --- a/src/aabb.cpp +++ b/src/aabb.cpp @@ -43,7 +43,7 @@ void bodies::AABB::extendWithTransformedBox(const Eigen::Isometry3d& transform, const Eigen::Vector3d& box) { -#if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION <= 5 +#if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5 // Method adapted from FCL src/shape/geometric_shapes_utility.cpp#computeBV(...) (BSD-licensed code): // https://github.com/flexible-collision-library/fcl/blob/fcl-0.4/src/shape/geometric_shapes_utility.cpp#L292 // We don't call their code because it would need creating temporary objects, and their method is in floats in FCL 0.5 diff --git a/src/bodies.cpp b/src/bodies.cpp index 3561d15..02c8734 100644 --- a/src/bodies.cpp +++ b/src/bodies.cpp @@ -41,25 +41,7 @@ #include extern "C" { -#ifdef GEOMETRIC_SHAPES_HAVE_QHULL_2011 -#include -#include -#include -#include -#include -#include -#include -#include -#else -#include -#include -#include -#include -#include -#include -#include -#include -#endif +#include } #include @@ -69,7 +51,6 @@ extern "C" { #include #include #include -#include namespace bodies { @@ -132,20 +113,6 @@ void filterIntersections(std::vector& ipts, EigenSTL::vector_Ve intersections->push_back(p.pt); } } - -// HACK: The global map g_triangle_for_plane_ is needed for ABI compatibility with the melodic version of -// geometric_shapes; in newer releases, it should instead be added as a member to ConvexMesh::MeshData. -std::unordered_map> g_triangle_for_plane_; -std::mutex g_triangle_for_plane_mutex; //!< Lock this mutex every time you work with g_triangle_for_plane_. -static std::map& getTriangleForPlane(const ConvexMesh* mesh) -{ - std::lock_guard lock(g_triangle_for_plane_mutex); - auto it = g_triangle_for_plane_.find(mesh); - if (it == detail::g_triangle_for_plane_.end()) - return detail::g_triangle_for_plane_.emplace(mesh, std::map()).first->second; - else - return it->second; -} } // namespace detail inline Eigen::Vector3d normalize(const Eigen::Vector3d& dir) @@ -154,17 +121,11 @@ inline Eigen::Vector3d normalize(const Eigen::Vector3d& dir) #if EIGEN_VERSION_AT_LEAST(3, 3, 0) return ((norm - 1) > 1e-9) ? (dir / Eigen::numext::sqrt(norm)) : dir; #else // used in kinetic - return ((norm - 1) > 1e-9) ? (Eigen::Vector3d)(dir / sqrt(norm)) : dir; + return ((norm - 1) > 1e-9) ? (dir / sqrt(norm)) : dir; #endif } } // namespace bodies -void bodies::Body::setDimensions(const shapes::Shape* shape) -{ - setDimensionsDirty(shape); - updateInternalData(); -} - bool bodies::Body::samplePointInside(random_numbers::RandomNumberGenerator& rng, unsigned int max_attempts, Eigen::Vector3d& result) const { @@ -344,6 +305,17 @@ bool bodies::Sphere::intersectsRay(const Eigen::Vector3d& origin, const Eigen::V return result; } +bodies::Sphere::Sphere(const bodies::BoundingSphere& sphere) : Body() +{ + type_ = shapes::SPHERE; + shapes::Sphere shape(sphere.radius); + setDimensionsDirty(&shape); + + Eigen::Isometry3d pose = Eigen::Isometry3d::Identity(); + pose.translation() = sphere.center; + setPose(pose); +} + bool bodies::Cylinder::containsPoint(const Eigen::Vector3d& p, bool /* verbose */) const { Eigen::Vector3d v = p - center_; @@ -581,6 +553,14 @@ bool bodies::Cylinder::intersectsRay(const Eigen::Vector3d& origin, const Eigen: return true; } +bodies::Cylinder::Cylinder(const bodies::BoundingCylinder& cylinder) : Body() +{ + type_ = shapes::CYLINDER; + shapes::Cylinder shape(cylinder.radius, cylinder.length); + setDimensionsDirty(&shape); + setPose(cylinder.pose); +} + bool bodies::Box::samplePointInside(random_numbers::RandomNumberGenerator& rng, unsigned int /* max_attempts */, Eigen::Vector3d& result) const { @@ -591,7 +571,7 @@ bool bodies::Box::samplePointInside(random_numbers::RandomNumberGenerator& rng, bool bodies::Box::containsPoint(const Eigen::Vector3d& p, bool /* verbose */) const { - const Eigen::Vector3d aligned = (pose_.linear().transpose() * (p - center_)).cwiseAbs(); + const Eigen::Vector3d aligned = (invRot_ * (p - center_)).cwiseAbs(); return aligned[0] <= length2_ && aligned[1] <= width2_ && aligned[2] <= height2_; } @@ -633,15 +613,12 @@ void bodies::Box::updateInternalData() radiusB_ = sqrt(radius2_); ASSERT_ISOMETRY(pose_); - Eigen::Matrix3d basis = pose_.linear(); - normalL_ = basis.col(0); - normalW_ = basis.col(1); - normalH_ = basis.col(2); + invRot_ = pose_.linear().transpose(); // rotation is intentionally not applied, the corners are used in intersectsRay() const Eigen::Vector3d tmp(length2_, width2_, height2_); - corner1_ = center_ - tmp; - corner2_ = center_ + tmp; + minCorner_ = center_ - tmp; + maxCorner_ = center_ + tmp; } std::shared_ptr bodies::Box::cloneAt(const Eigen::Isometry3d& pose, double padding, double scale) const @@ -720,17 +697,16 @@ bool bodies::Box::intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vect // Brian Smits. Efficient bounding box intersection. Ray tracing news 15(1), 2002 // The implemented method only works for axis-aligned boxes. So we treat ours as such, cancel its rotation, and - // rotate the origin and dir instead. corner1_ and corner2_ are corners with canceled rotation. - const Eigen::Matrix3d invRot = pose_.linear().transpose(); - const Eigen::Vector3d o(invRot * (origin - center_) + center_); - const Eigen::Vector3d d(invRot * dirNorm); + // rotate the origin and dir instead. minCorner_ and maxCorner_ are corners with canceled rotation. + const Eigen::Vector3d o(invRot_ * (origin - center_) + center_); + const Eigen::Vector3d d(invRot_ * dirNorm); Eigen::Vector3d tmpTmin, tmpTmax; - tmpTmin = (corner1_ - o).cwiseQuotient(d); - tmpTmax = (corner2_ - o).cwiseQuotient(d); + tmpTmin = (minCorner_ - o).cwiseQuotient(d); + tmpTmax = (maxCorner_ - o).cwiseQuotient(d); - // In projection to each axis, if the ray has positive direction, it goes from min corner (corner1_) to max corner - // (corner2_). If its direction is negative, the first intersection is at max corner and then at min corner. + // In projection to each axis, if the ray has positive direction, it goes from min corner (minCorner_) to max corner + // (maxCorner_). If its direction is negative, the first intersection is at max corner and then at min corner. for (size_t i = 0; i < 3; ++i) { if (d[i] < 0) @@ -785,6 +761,36 @@ bool bodies::Box::intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vect return true; } +bodies::Box::Box(const bodies::AABB& aabb) : Body() +{ + type_ = shapes::BOX; + shapes::Box shape(aabb.sizes()[0], aabb.sizes()[1], aabb.sizes()[2]); + setDimensionsDirty(&shape); + + Eigen::Isometry3d pose = Eigen::Isometry3d::Identity(); + pose.translation() = aabb.center(); + setPose(pose); +} + +namespace bodies +{ +struct ConvexMesh::MeshData +{ + EigenSTL::vector_Vector4d planes_; + EigenSTL::vector_Vector3d vertices_; + std::vector triangles_; + std::map plane_for_triangle_; + std::map triangle_for_plane_; + Eigen::Vector3d mesh_center_; + double mesh_radiusB_; + Eigen::Vector3d box_offset_; + Eigen::Vector3d box_size_; + BoundingCylinder bounding_cylinder_; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; +} // namespace bodies + bool bodies::ConvexMesh::containsPoint(const Eigen::Vector3d& p, bool /* verbose */) const { if (!mesh_data_) @@ -928,20 +934,24 @@ void bodies::ConvexMesh::useDimensions(const shapes::Shape* shape) static FILE* null = fopen("/dev/null", "w"); char flags[] = "qhull Tv Qt"; - int exitcode = qh_new_qhull(3, mesh->vertex_count, points, true, flags, null, null); + qhT qh_qh; + qhT* qh = &qh_qh; + QHULL_LIB_CHECK + qh_zero(qh, null); + int exitcode = qh_new_qhull(qh, 3, mesh->vertex_count, points, true, flags, null, null); if (exitcode != 0) { CONSOLE_BRIDGE_logWarn("Convex hull creation failed"); - qh_freeqhull(!qh_ALL); + qh_freeqhull(qh, !qh_ALL); int curlong, totlong; - qh_memfreeshort(&curlong, &totlong); + qh_memfreeshort(qh, &curlong, &totlong); return; } - int num_facets = qh num_facets; + int num_facets = qh->num_facets; - int num_vertices = qh num_vertices; + int num_vertices = qh->num_vertices; mesh_data_->vertices_.reserve(num_vertices); Eigen::Vector3d sum(0, 0, 0); @@ -967,10 +977,6 @@ void bodies::ConvexMesh::useDimensions(const shapes::Shape* shape) mesh_data_->mesh_radiusB_ = sqrt(mesh_data_->mesh_radiusB_); mesh_data_->triangles_.reserve(num_facets); - // HACK: only needed for ABI compatibility with melodic - std::map& triangle_for_plane = detail::getTriangleForPlane(this); - triangle_for_plane.clear(); - // neccessary for qhull macro facetT* facet; FORALLfacets @@ -989,17 +995,17 @@ void bodies::ConvexMesh::useDimensions(const shapes::Shape* shape) // Needed by FOREACHvertex_i_ int vertex_n, vertex_i; - FOREACHvertex_i_((*facet).vertices) + FOREACHvertex_i_(qh, (*facet).vertices) { mesh_data_->triangles_.push_back(qhull_vertex_table[vertex->id]); } mesh_data_->plane_for_triangle_[(mesh_data_->triangles_.size() - 1) / 3] = mesh_data_->planes_.size() - 1; - triangle_for_plane[mesh_data_->planes_.size() - 1] = (mesh_data_->triangles_.size() - 1) / 3; + mesh_data_->triangle_for_plane_[mesh_data_->planes_.size() - 1] = (mesh_data_->triangles_.size() - 1) / 3; } - qh_freeqhull(!qh_ALL); + qh_freeqhull(qh, !qh_ALL); int curlong, totlong; - qh_memfreeshort(&curlong, &totlong); + qh_memfreeshort(qh, &curlong, &totlong); } std::vector bodies::ConvexMesh::getDimensions() const @@ -1182,7 +1188,6 @@ void bodies::ConvexMesh::computeBoundingBox(bodies::OBB& bbox) const bool bodies::ConvexMesh::isPointInsidePlanes(const Eigen::Vector3d& point) const { unsigned int numplanes = mesh_data_->planes_.size(); - const std::map& triangle_for_plane = detail::getTriangleForPlane(this); for (unsigned int i = 0; i < numplanes; ++i) { const Eigen::Vector4d& plane = mesh_data_->planes_[i]; @@ -1190,7 +1195,8 @@ bool bodies::ConvexMesh::isPointInsidePlanes(const Eigen::Vector3d& point) const // w() needs to be recomputed from a scaled vertex as normally it refers to the unscaled plane // we also cannot simply subtract padding_ from it, because padding of the points on the plane causes a different // effect than adding padding along this plane's normal (padding effect is direction-dependent) - const auto scaled_point_on_plane = scaled_vertices_->at(mesh_data_->triangles_[3 * triangle_for_plane.at(i)]); + const auto scaled_point_on_plane = + scaled_vertices_->at(mesh_data_->triangles_[3 * mesh_data_->triangle_for_plane_[i]]); const double w_scaled_padded = -plane_vec.dot(scaled_point_on_plane); const double dist = plane_vec.dot(point) + w_scaled_padded - detail::ZERO; if (dist > 0.0) @@ -1324,15 +1330,6 @@ bool bodies::ConvexMesh::intersectsRay(const Eigen::Vector3d& origin, const Eige return result; } -bodies::ConvexMesh::~ConvexMesh() -{ - // HACK: only needed for ABI compatibility with melodic - { - std::lock_guard lock(detail::g_triangle_for_plane_mutex); - detail::g_triangle_for_plane_.erase(this); - } -} - bodies::BodyVector::BodyVector() { } diff --git a/src/obb.cpp b/src/obb.cpp index f161024..885e48e 100644 --- a/src/obb.cpp +++ b/src/obb.cpp @@ -2,7 +2,7 @@ #include -#if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION <= 5 +#if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5 #include typedef fcl::Vec3f FCL_Vec3; typedef fcl::OBB FCL_OBB; @@ -14,7 +14,7 @@ typedef fcl::OBB FCL_OBB; namespace bodies { -#if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION <= 5 +#if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5 inline FCL_Vec3 toFcl(const Eigen::Vector3d& eigenVec) { FCL_Vec3 result; @@ -25,7 +25,7 @@ inline FCL_Vec3 toFcl(const Eigen::Vector3d& eigenVec) #define toFcl #endif -#if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION <= 5 +#if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5 Eigen::Vector3d fromFcl(const FCL_Vec3& fclVec) { return Eigen::Map(fclVec.data.vs, 3, 1); @@ -78,7 +78,7 @@ void OBB::setPoseAndExtents(const Eigen::Isometry3d& pose, const Eigen::Vector3d { const auto rotation = pose.linear(); -#if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION <= 5 +#if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5 obb_->axis[0] = toFcl(rotation.col(0)); obb_->axis[1] = toFcl(rotation.col(1)); obb_->axis[2] = toFcl(rotation.col(2)); diff --git a/src/shape_extents.cpp b/src/shape_extents.cpp index 098c57d..b4974a8 100644 --- a/src/shape_extents.cpp +++ b/src/shape_extents.cpp @@ -43,13 +43,12 @@ void geometric_shapes::getShapeExtents(const shape_msgs::SolidPrimitive& shape_m if (shape_msg.type == shape_msgs::SolidPrimitive::SPHERE) { - if (shape_msg.dimensions.size() >= - geometric_shapes::SolidPrimitiveDimCount::value) + if (shape_msg.dimensions.size() >= geometric_shapes::solidPrimitiveDimCount()) x_extent = y_extent = z_extent = shape_msg.dimensions[shape_msgs::SolidPrimitive::SPHERE_RADIUS] * 2.0; } else if (shape_msg.type == shape_msgs::SolidPrimitive::BOX) { - if (shape_msg.dimensions.size() >= geometric_shapes::SolidPrimitiveDimCount::value) + if (shape_msg.dimensions.size() >= geometric_shapes::solidPrimitiveDimCount()) { x_extent = shape_msg.dimensions[shape_msgs::SolidPrimitive::BOX_X]; y_extent = shape_msg.dimensions[shape_msgs::SolidPrimitive::BOX_Y]; @@ -58,8 +57,7 @@ void geometric_shapes::getShapeExtents(const shape_msgs::SolidPrimitive& shape_m } else if (shape_msg.type == shape_msgs::SolidPrimitive::CYLINDER) { - if (shape_msg.dimensions.size() >= - geometric_shapes::SolidPrimitiveDimCount::value) + if (shape_msg.dimensions.size() >= geometric_shapes::solidPrimitiveDimCount()) { x_extent = y_extent = shape_msg.dimensions[shape_msgs::SolidPrimitive::CYLINDER_RADIUS] * 2.0; z_extent = shape_msg.dimensions[shape_msgs::SolidPrimitive::CYLINDER_HEIGHT]; @@ -67,8 +65,7 @@ void geometric_shapes::getShapeExtents(const shape_msgs::SolidPrimitive& shape_m } else if (shape_msg.type == shape_msgs::SolidPrimitive::CONE) { - if (shape_msg.dimensions.size() >= - geometric_shapes::SolidPrimitiveDimCount::value) + if (shape_msg.dimensions.size() >= geometric_shapes::solidPrimitiveDimCount()) { x_extent = y_extent = shape_msg.dimensions[shape_msgs::SolidPrimitive::CONE_RADIUS] * 2.0; z_extent = shape_msg.dimensions[shape_msgs::SolidPrimitive::CONE_HEIGHT]; diff --git a/src/shape_operations.cpp b/src/shape_operations.cpp index b7c303e..ada0fac 100644 --- a/src/shape_operations.cpp +++ b/src/shape_operations.cpp @@ -86,28 +86,25 @@ Shape* constructShapeFromMsg(const shape_msgs::SolidPrimitive& shape_msg) Shape* shape = nullptr; if (shape_msg.type == shape_msgs::SolidPrimitive::SPHERE) { - if (shape_msg.dimensions.size() >= - geometric_shapes::SolidPrimitiveDimCount::value) + if (shape_msg.dimensions.size() >= geometric_shapes::solidPrimitiveDimCount()) shape = new Sphere(shape_msg.dimensions[shape_msgs::SolidPrimitive::SPHERE_RADIUS]); } else if (shape_msg.type == shape_msgs::SolidPrimitive::BOX) { - if (shape_msg.dimensions.size() >= geometric_shapes::SolidPrimitiveDimCount::value) + if (shape_msg.dimensions.size() >= geometric_shapes::solidPrimitiveDimCount()) shape = new Box(shape_msg.dimensions[shape_msgs::SolidPrimitive::BOX_X], shape_msg.dimensions[shape_msgs::SolidPrimitive::BOX_Y], shape_msg.dimensions[shape_msgs::SolidPrimitive::BOX_Z]); } else if (shape_msg.type == shape_msgs::SolidPrimitive::CYLINDER) { - if (shape_msg.dimensions.size() >= - geometric_shapes::SolidPrimitiveDimCount::value) + if (shape_msg.dimensions.size() >= geometric_shapes::solidPrimitiveDimCount()) shape = new Cylinder(shape_msg.dimensions[shape_msgs::SolidPrimitive::CYLINDER_RADIUS], shape_msg.dimensions[shape_msgs::SolidPrimitive::CYLINDER_HEIGHT]); } else if (shape_msg.type == shape_msgs::SolidPrimitive::CONE) { - if (shape_msg.dimensions.size() >= - geometric_shapes::SolidPrimitiveDimCount::value) + if (shape_msg.dimensions.size() >= geometric_shapes::solidPrimitiveDimCount()) shape = new Cone(shape_msg.dimensions[shape_msgs::SolidPrimitive::CONE_RADIUS], shape_msg.dimensions[shape_msgs::SolidPrimitive::CONE_HEIGHT]); } @@ -352,7 +349,7 @@ bool constructMsgFromShape(const Shape* shape, ShapeMsg& shape_msg) { shape_msgs::SolidPrimitive s; s.type = shape_msgs::SolidPrimitive::SPHERE; - s.dimensions.resize(geometric_shapes::SolidPrimitiveDimCount::value); + s.dimensions.resize(geometric_shapes::solidPrimitiveDimCount()); s.dimensions[shape_msgs::SolidPrimitive::SPHERE_RADIUS] = static_cast(shape)->radius; shape_msg = s; } @@ -361,7 +358,7 @@ bool constructMsgFromShape(const Shape* shape, ShapeMsg& shape_msg) shape_msgs::SolidPrimitive s; s.type = shape_msgs::SolidPrimitive::BOX; const double* sz = static_cast(shape)->size; - s.dimensions.resize(geometric_shapes::SolidPrimitiveDimCount::value); + s.dimensions.resize(geometric_shapes::solidPrimitiveDimCount()); s.dimensions[shape_msgs::SolidPrimitive::BOX_X] = sz[0]; s.dimensions[shape_msgs::SolidPrimitive::BOX_Y] = sz[1]; s.dimensions[shape_msgs::SolidPrimitive::BOX_Z] = sz[2]; @@ -371,7 +368,7 @@ bool constructMsgFromShape(const Shape* shape, ShapeMsg& shape_msg) { shape_msgs::SolidPrimitive s; s.type = shape_msgs::SolidPrimitive::CYLINDER; - s.dimensions.resize(geometric_shapes::SolidPrimitiveDimCount::value); + s.dimensions.resize(geometric_shapes::solidPrimitiveDimCount()); s.dimensions[shape_msgs::SolidPrimitive::CYLINDER_RADIUS] = static_cast(shape)->radius; s.dimensions[shape_msgs::SolidPrimitive::CYLINDER_HEIGHT] = static_cast(shape)->length; shape_msg = s; @@ -380,7 +377,7 @@ bool constructMsgFromShape(const Shape* shape, ShapeMsg& shape_msg) { shape_msgs::SolidPrimitive s; s.type = shape_msgs::SolidPrimitive::CONE; - s.dimensions.resize(geometric_shapes::SolidPrimitiveDimCount::value); + s.dimensions.resize(geometric_shapes::solidPrimitiveDimCount()); s.dimensions[shape_msgs::SolidPrimitive::CONE_RADIUS] = static_cast(shape)->radius; s.dimensions[shape_msgs::SolidPrimitive::CONE_HEIGHT] = static_cast(shape)->length; shape_msg = s; diff --git a/src/shape_to_marker.cpp b/src/shape_to_marker.cpp index cac80ad..c663596 100644 --- a/src/shape_to_marker.cpp +++ b/src/shape_to_marker.cpp @@ -43,8 +43,7 @@ void geometric_shapes::constructMarkerFromShape(const shape_msgs::SolidPrimitive switch (shape_msg.type) { case shape_msgs::SolidPrimitive::SPHERE: - if (shape_msg.dimensions.size() < - geometric_shapes::SolidPrimitiveDimCount::value) + if (shape_msg.dimensions.size() < geometric_shapes::solidPrimitiveDimCount()) throw std::runtime_error("Insufficient dimensions in sphere definition"); else { @@ -53,8 +52,7 @@ void geometric_shapes::constructMarkerFromShape(const shape_msgs::SolidPrimitive } break; case shape_msgs::SolidPrimitive::BOX: - if (shape_msg.dimensions.size() < - geometric_shapes::SolidPrimitiveDimCount::value) + if (shape_msg.dimensions.size() < geometric_shapes::solidPrimitiveDimCount()) throw std::runtime_error("Insufficient dimensions in box definition"); else { @@ -65,8 +63,7 @@ void geometric_shapes::constructMarkerFromShape(const shape_msgs::SolidPrimitive } break; case shape_msgs::SolidPrimitive::CONE: - if (shape_msg.dimensions.size() < - geometric_shapes::SolidPrimitiveDimCount::value) + if (shape_msg.dimensions.size() < geometric_shapes::solidPrimitiveDimCount()) throw std::runtime_error("Insufficient dimensions in cone definition"); else { @@ -79,7 +76,7 @@ void geometric_shapes::constructMarkerFromShape(const shape_msgs::SolidPrimitive break; case shape_msgs::SolidPrimitive::CYLINDER: if (shape_msg.dimensions.size() < - geometric_shapes::SolidPrimitiveDimCount::value) + geometric_shapes::solidPrimitiveDimCount()) throw std::runtime_error("Insufficient dimensions in cylinder definition"); else { diff --git a/src/shapes.cpp b/src/shapes.cpp index b6f3ef5..3dbb447 100644 --- a/src/shapes.cpp +++ b/src/shapes.cpp @@ -400,23 +400,15 @@ void Mesh::scaleAndPadd(double scaleX, double scaleY, double scaleZ, double padd double dy = vertices[i3 + 1] - sy; double dz = vertices[i3 + 2] - sz; - // length of vector - double norm = sqrt(dx * dx + dy * dy + dz * dz); - if (norm > 1e-6) - { - vertices[i3] = sx + dx * (scaleX + paddX / norm); - vertices[i3 + 1] = sy + dy * (scaleY + paddY / norm); - vertices[i3 + 2] = sz + dz * (scaleZ + paddZ / norm); - } - else - { - double ndx = ((dx > 0) ? dx + paddX : dx - paddX); - double ndy = ((dy > 0) ? dy + paddY : dy - paddY); - double ndz = ((dz > 0) ? dz + paddZ : dz - paddZ); - vertices[i3] = sx + ndx; - vertices[i3 + 1] = sy + ndy; - vertices[i3 + 2] = sz + ndz; - } + // Scaled coordinate + double scaledX = sx + dx * scaleX; + double scaledY = sy + dy * scaleY; + double scaledZ = sz + dz * scaleZ; + + // Padding in each direction + vertices[i3] = scaledX + vertex_normals[i3] * paddX; + vertices[i3 + 1] = scaledY + vertex_normals[i3 + 1] * paddY; + vertices[i3 + 2] = scaledZ + vertex_normals[i3 + 2] * paddZ; } } @@ -529,7 +521,8 @@ void Mesh::computeVertexNormals() computeTriangleNormals(); if (vertex_count && !vertex_normals) vertex_normals = new double[vertex_count * 3]; - EigenSTL::vector_Vector3d avg_normals(vertex_count, Eigen::Vector3d(0, 0, 0)); + Eigen::Map> mapped_normals(vertex_normals, 3, vertex_count); + mapped_normals.setZero(); for (unsigned int tIdx = 0; tIdx < triangle_count; ++tIdx) { @@ -541,26 +534,43 @@ void Mesh::computeVertexNormals() unsigned int v2 = triangles[tIdx3_1]; unsigned int v3 = triangles[tIdx3_2]; - avg_normals[v1][0] += triangle_normals[tIdx3]; - avg_normals[v1][1] += triangle_normals[tIdx3_1]; - avg_normals[v1][2] += triangle_normals[tIdx3_2]; - - avg_normals[v2][0] += triangle_normals[tIdx3]; - avg_normals[v2][1] += triangle_normals[tIdx3_1]; - avg_normals[v2][2] += triangle_normals[tIdx3_2]; - - avg_normals[v3][0] += triangle_normals[tIdx3]; - avg_normals[v3][1] += triangle_normals[tIdx3_1]; - avg_normals[v3][2] += triangle_normals[tIdx3_2]; + // Get angles for each vertex at this triangle + Eigen::Map p1{ vertices + 3 * v1, 3 }; + Eigen::Map p2{ vertices + 3 * v2, 3 }; + Eigen::Map p3{ vertices + 3 * v3, 3 }; + + // Use re-arranged dot product equation to calculate angle between two vectors + auto angleBetweenVectors = [](const Eigen::Vector3d& vec1, const Eigen::Vector3d& vec2) -> double { + double vec1_norm = vec1.norm(); + double vec2_norm = vec2.norm(); + + // Handle the case where either vector has zero length, to prevent division-by-zero + if (vec1_norm == 0.0 || vec2_norm == 0.0) + return 0.0; + + return std::acos(vec1.dot(vec2) / (vec1_norm * vec2_norm)); + }; + + // Use law of cosines to compute angles + auto ang1 = angleBetweenVectors(p2 - p1, p3 - p1); + auto ang2 = angleBetweenVectors(p1 - p2, p3 - p2); + auto ang3 = angleBetweenVectors(p1 - p3, p2 - p3); + + // Weight normal with angle + Eigen::Map triangle_normal{ triangle_normals + tIdx3, 3 }; + mapped_normals.col(v1) += triangle_normal * ang1; + mapped_normals.col(v2) += triangle_normal * ang2; + mapped_normals.col(v3) += triangle_normal * ang3; } - for (std::size_t i = 0; i < avg_normals.size(); ++i) + + // Normalize each column of the matrix + for (int i = 0; i < mapped_normals.cols(); ++i) { - if (avg_normals[i].squaredNorm() > 0.0) - avg_normals[i].normalize(); - unsigned int i3 = i * 3; - vertex_normals[i3] = avg_normals[i][0]; - vertex_normals[i3 + 1] = avg_normals[i][1]; - vertex_normals[i3 + 2] = avg_normals[i][2]; + auto mapped_normal = mapped_normals.col(i); + if (mapped_normal.squaredNorm() != 0.0) + { + mapped_normal.normalize(); + } } } diff --git a/test/test_ray_intersection.cpp b/test/test_ray_intersection.cpp index 95d793b..368600c 100644 --- a/test/test_ray_intersection.cpp +++ b/test/test_ray_intersection.cpp @@ -1041,7 +1041,7 @@ TEST(ConvexMeshRayIntersection, SimpleRay1) shapes::Mesh* shape = shapes::createMeshFromShape(&box); bodies::ConvexMesh mesh(shape); delete shape; - mesh.setScale(0.95); + mesh.setScale(0.95); // NOLINT(performance-unnecessary-copy-initialization) CHECK_INTERSECTS_TWICE(mesh, (10, 0.449, 0), (-1, 0, 0), (0.475, 0.449, 0), (-0.475, 0.449, 0), 1e-4) } diff --git a/test/test_shapes.cpp b/test/test_shapes.cpp index dfa4fcf..1f985bf 100644 --- a/test/test_shapes.cpp +++ b/test/test_shapes.cpp @@ -276,10 +276,10 @@ TEST(Mesh, ScaleAndPadd) EXPECT_DOUBLE_EQ(mesh2->vertices[22], -2.0); EXPECT_DOUBLE_EQ(mesh2->vertices[23], 2.0); - // padding actually means extending each vertices' direction vector by the padding value, - // not extending it along each axis by the same amount + // for a right-angled corner, the vertex normal vector points away equally from the three sides, and hence + // padding is applied equally in x, y and z, such that the total distance the vertex moves is equal to 1.0. mesh2->padd(1.0); - const double pos = 2.0 * (1 + 1.0 / sqrt(12)); + const double pos = 2.0 + 1.0 / sqrt(3); EXPECT_DOUBLE_EQ(mesh2->vertices[0], pos); EXPECT_DOUBLE_EQ(mesh2->vertices[1], pos); @@ -314,7 +314,7 @@ TEST(Mesh, ScaleAndPadd) EXPECT_DOUBLE_EQ(mesh2->vertices[23], pos); mesh2->scaleAndPadd(2.0, 1.0); - const double pos2 = pos * (2.0 + 1.0 / sqrt(3 * pos * pos)); + const double pos2 = pos * 2.0 + 1.0 / sqrt(3); EXPECT_DOUBLE_EQ(mesh2->vertices[0], pos2); EXPECT_DOUBLE_EQ(mesh2->vertices[1], pos2); @@ -423,10 +423,9 @@ TEST(Mesh, ScaleAndPadd) EXPECT_DOUBLE_EQ(mesh2->vertices[23], pos4z); mesh2->padd(1.0, 2.0, 3.0); - const double norm5 = sqrt(pos4x * pos4x + pos4y * pos4y + pos4z * pos4z); - const double pos5x = pos4x * (1.0 + 1.0 / norm5); - const double pos5y = pos4y * (1.0 + 2.0 / norm5); - const double pos5z = pos4z * (1.0 + 3.0 / norm5); + const double pos5x = pos4x + (1.0 / sqrt(3)); + const double pos5y = pos4y + (2.0 / sqrt(3)); + const double pos5z = pos4z + (3.0 / sqrt(3)); EXPECT_DOUBLE_EQ(mesh2->vertices[0], pos5x); EXPECT_DOUBLE_EQ(mesh2->vertices[1], pos5y);