pax_global_header00006660000000000000000000000064140511574610014516gustar00rootroot0000000000000052 comment=d147f0371afbece0b8c93a2d2d55149a284d5192 geometric_shapes-0.7.3/000077500000000000000000000000001405115746100150465ustar00rootroot00000000000000geometric_shapes-0.7.3/.clang-format000066400000000000000000000036101405115746100174210ustar00rootroot00000000000000--- BasedOnStyle: Google ColumnLimit: 120 MaxEmptyLinesToKeep: 1 SortIncludes: false Standard: Auto IndentWidth: 2 TabWidth: 2 UseTab: Never AccessModifierOffset: -2 ConstructorInitializerIndentWidth: 2 NamespaceIndentation: None ContinuationIndentWidth: 4 IndentCaseLabels: true IndentFunctionDeclarationAfterType: false AlignEscapedNewlinesLeft: false AlignTrailingComments: true AllowAllParametersOfDeclarationOnNextLine: false ExperimentalAutoDetectBinPacking: false ObjCSpaceBeforeProtocolList: true Cpp11BracedListStyle: false AllowShortBlocksOnASingleLine: true AllowShortIfStatementsOnASingleLine: false AllowShortLoopsOnASingleLine: false AllowShortFunctionsOnASingleLine: None AllowShortCaseLabelsOnASingleLine: false AlwaysBreakTemplateDeclarations: true AlwaysBreakBeforeMultilineStrings: false BreakBeforeBinaryOperators: false BreakBeforeTernaryOperators: false BreakConstructorInitializersBeforeComma: true BinPackParameters: true ConstructorInitializerAllOnOneLineOrOnePerLine: true DerivePointerBinding: false PointerBindsToType: true PenaltyExcessCharacter: 75 PenaltyBreakBeforeFirstCallParameter: 30 PenaltyBreakComment: 1000 PenaltyBreakFirstLessLess: 10 PenaltyBreakString: 100 PenaltyReturnTypeOnItsOwnLine: 50 SpacesBeforeTrailingComments: 2 SpacesInParentheses: false SpacesInAngles: false SpaceInEmptyParentheses: false SpacesInCStyleCastParentheses: false SpaceAfterCStyleCast: false SpaceAfterControlStatementKeyword: true SpaceBeforeAssignmentOperators: true # Configure each individual brace in BraceWrapping BreakBeforeBraces: Custom # Control of individual brace wrapping cases BraceWrapping: AfterCaseLabel: true AfterClass: true AfterControlStatement: true AfterEnum: true AfterFunction: true AfterNamespace: true AfterStruct: true AfterUnion: true BeforeCatch: true BeforeElse: true IndentBraces: false ... geometric_shapes-0.7.3/.clang-tidy000066400000000000000000000016621405115746100171070ustar00rootroot00000000000000--- Checks: '-*, performance-*, llvm-namespace-comment, modernize-redundant-void-arg, modernize-use-nullptr, modernize-use-default, modernize-use-override, modernize-loop-convert, readability-named-parameter, readability-redundant-smartptr-get, readability-redundant-string-cstr, readability-simplify-boolean-expr, readability-container-size-empty, ' HeaderFilterRegex: '' AnalyzeTemporaryDtors: false CheckOptions: - key: llvm-namespace-comment.ShortNamespaceLines value: '10' - key: llvm-namespace-comment.SpacesBeforeComments value: '2' - key: readability-braces-around-statements.ShortStatementLines value: '2' ... geometric_shapes-0.7.3/.github/000077500000000000000000000000001405115746100164065ustar00rootroot00000000000000geometric_shapes-0.7.3/.github/workflows/000077500000000000000000000000001405115746100204435ustar00rootroot00000000000000geometric_shapes-0.7.3/.github/workflows/ci.yaml000066400000000000000000000051261405115746100217260ustar00rootroot00000000000000# This config uses industrial_ci (https://github.com/ros-industrial/industrial_ci.git). # For troubleshooting, see readme (https://github.com/ros-industrial/industrial_ci/blob/master/README.rst) name: CI on: workflow_dispatch: pull_request: push: branches: - noetic-devel - melodic-devel jobs: default: strategy: matrix: env: - ROS_DISTRO: melodic ROS_REPO: main - ROS_DISTRO: noetic ROS_REPO: main CLANG_TIDY: pedantic CATKIN_LINT: pedantic - ROS_DISTRO: noetic ROS_REPO: testing env: CCACHE_DIR: ${{ github.workspace }}/.ccache BASEDIR: ${{ github.workspace }}/.work CACHE_PREFIX: ${{ matrix.env.ROS_DISTRO }}-${{ matrix.env.ROS_REPO }} CLANG_TIDY_BASE_REF: ${{ github.base_ref || github.ref }} name: ${{ matrix.env.ROS_DISTRO }}-${{ matrix.env.ROS_REPO }}${{ matrix.env.CATKIN_LINT && ' + catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' + clang-tidy' || ''}} runs-on: ubuntu-latest steps: - uses: actions/checkout@v2 # The target directory cache doesn't include the source directory because # that comes from the checkout. See "prepare target_ws for cache" task below - name: cache target_ws uses: pat-s/always-upload-cache@v2.1.3 with: path: ${{ env.BASEDIR }}/target_ws key: target_ws-${{ env.CACHE_PREFIX }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml') }}-${{ github.run_id }} restore-keys: | target_ws-${{ env.CACHE_PREFIX }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml') }} - name: cache ccache uses: pat-s/always-upload-cache@v2.1.3 with: path: ${{ env.CCACHE_DIR }} key: ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }}-${{ github.run_id }} restore-keys: | ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }} ccache-${{ env.CACHE_PREFIX }} - name: industrial_ci uses: ros-industrial/industrial_ci@master env: ${{ matrix.env }} - name: upload test artifacts (on failure) uses: actions/upload-artifact@v2 if: failure() with: name: test-results path: ${{ env.BASEDIR }}/target_ws/**/test_results/**/*.xml - name: prepare target_ws for cache if: always() run: | du -sh ${{ env.BASEDIR }}/target_ws sudo find ${{ env.BASEDIR }}/target_ws -wholename '*/test_results/*' -delete sudo rm -rf ${{ env.BASEDIR }}/target_ws/src du -sh ${{ env.BASEDIR }}/target_ws geometric_shapes-0.7.3/.github/workflows/format.yaml000066400000000000000000000010051405115746100226130ustar00rootroot00000000000000# This is a format job. Pre-commit has a first-party GitHub action, so we use # that: https://github.com/pre-commit/action name: Format on: workflow_dispatch: pull_request: push: jobs: pre-commit: name: Format runs-on: ubuntu-latest steps: - uses: actions/checkout@v2 - uses: actions/setup-python@v2 - name: Install clang-format-10 run: sudo apt-get install clang-format-10 - uses: rhaschke/install-catkin_lint-action@v1.0 - uses: pre-commit/action@v2.0.0 geometric_shapes-0.7.3/.gitignore000066400000000000000000000000031405115746100170270ustar00rootroot00000000000000*~ geometric_shapes-0.7.3/.pre-commit-config.yaml000066400000000000000000000031441405115746100213310ustar00rootroot00000000000000# To use: # # pre-commit run -a # # Or: # # pre-commit install # (runs every time you commit in git) # # To update this file: # # pre-commit autoupdate # # See https://github.com/pre-commit/pre-commit repos: # Standard hooks - repo: https://github.com/pre-commit/pre-commit-hooks rev: v3.4.0 hooks: - id: check-added-large-files - id: check-case-conflict - id: check-json - id: check-merge-conflict - id: check-symlinks - id: check-toml - id: check-xml - id: check-yaml - id: debug-statements - id: destroyed-symlinks - id: detect-private-key - id: end-of-file-fixer - id: mixed-line-ending - id: pretty-format-json - id: trailing-whitespace - repo: https://github.com/psf/black rev: 20.8b1 hooks: - id: black - repo: local hooks: - id: clang-format name: clang-format description: Format files with ClangFormat. entry: clang-format-10 language: system files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$ args: ['-fallback-style=none', '-i'] - repo: local hooks: - id: misspelled-moveit name: misspelled-moveit description: MoveIt should be spelled exactly as MoveIt language: pygrep entry: Moveit|MoveIt! exclude: .pre-commit-config.yaml|README.md - id: catkin_lint name: catkin_lint description: Check package.xml and cmake files entry: catkin_lint . language: system always_run: true pass_filenames: false geometric_shapes-0.7.3/CHANGELOG.rst000066400000000000000000000246671405115746100171060ustar00rootroot00000000000000^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package geometric_shapes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 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 (`#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) ------------------ * [maint] Provide checkIsometry() helper function (`#144 `_) * [maint] Remove dynamic casts (`#143 `_) * [feature] Added createEmptyBodyFromShapeType() (`#137 `_) This allows more efficient body construction when scale, padding or pose should also be set during the construction. * Contributors: Martin Pecka, Michael Görner 0.6.2 (2020-05-02) ------------------ * [maint] clang-tidy fixes in headers (`#139 `_) * [fix] Various fixes + performance improvements (`#109 `_, `#126 `_, `#107 `_, `#108 `_) * Use Eigen::Isometry3d::linear() instead of rotation() * Normalize the direction vector passed to Body::intersectsRay() (`#115 `_) * Improved test coverage * [feature] Added support for non-uniform scaling and padding of shapes. (`#103 `_) * [maint] Made bodies::samplePointInside() const. (`#133 `_) * [fix] Throw runtime exception when a shape or body should have a negative dimension. (`#106 `_) * [maint] Prefer std::make_shared (`#116 `_) * [maint] clang-tidy fixes (`#114 `_) * [fix] Use covariant returns for clone() (`#102 `_) * [feature] Added bodies::Body::computeBoundingBox (aligned box version). (`#104 `_) * [maint] Windows compatibility: fix ASSIMP libraries path (`#101 `_) * [fix] Body::containsPoint(): always include surface points (`#97 `_) * Contributors: Martin Pecka, Alejandro Hernández Cordero, Bryce Willey, Michael Görner, Mike Lautman, Robert Haschke, RoboticsYY, Sean Yen, Tyler Weaver 0.6.1 (2018-12-09) ------------------ * Limit minimum number of cylinder vertices (on circumference) to 6 (`#92 `_) * Eigen::Affine3d -> Eigen::Isometry3d (`#88 `_) * Contributors: Robert Haschke, eisoku9618 0.6.0 (2018-05-14) ------------------ * Add method getPlanes and use double precision for planes (`#82 `_) * Contributors: Bence Magyar 0.5.4 (2018-04-06) ------------------ * gracefully handle negative cylinder height: `#64 `_, `#80 `_ * clang-formatting of whole repo: `#79 `_ * operator<< for ShapeType: `#80 `_ * adaption to new CONSOLE_BRIDGE_logXXX API: `#75 `_, `#72 `_ * [fix] box-ray intersection: `#73 `_ * Contributors: Dave Coleman, Leroy Rügemer, Malcolm Mielle, Mike Purvis, Robert Haschke, Michael Goerner 0.5.3 (2017-11-26) ------------------ * [enhance] Add warning about common Assimp bug (`#63 `_) * [maintenance] Update maintainers (`#66 `_) * Contributors: Dave Coleman 0.5.2 (2016-10-20) ------------------ * [fix] mesh with too many vertices (`#39 `_) (`#60 `_) * [fix] gcc6 build error (`#56 `_) * [fix] Clear root transformation on imported Collada meshes. `#52 `_ * [improve] relax mesh containment test (`#58 `_) * [maintenance] Switch boost::shared_ptr to std::shared_ptr. `#57 `_ * Contributors: Dave Coleman, Isaac I.Y. Saito, Lukas Bulwahn, Maarten de Vries, Michael Goerner 0.5.1 (2016-08-23) ------------------ * add c++11 safe-guards to the respective headers (`#51 `_) This is, to be polite and point problems that might arise it out to users. * Fix incorrect hint always sent to Assimp, improved STL reading (`#24 `_) * Contributors: Dave Coleman, Michael Görner 0.5.0 (2016-07-31) ------------------ * [fix] append cmake module path instead of prepending (`#22 `_) * [fix] FindQhull with non-debian systems (`#30 `_). See https://github.com/PointCloudLibrary/pcl/pull/852 * [sys] Use std::shared_ptr for compatibility with FCL 0.5. `#47 `_ * [sys] Switch to eigen 3 (`#46 `_) * [sys] Switched to C++11 `#44 `_ * [sys] add notice that project will be built in Release mode * [sys] Remove link_directories, deprecated assimp code * Contributors: Dave Coleman, Ioan A Sucan, Jochen Sprickerhof, Maarten de Vries, Michael Goerner 0.4.4 (2016-03-06) ------------------ * Merge pull request `#37 `_ from corot/indigo-devel Fix issue `#28 `_ on small radius cylinders * Contributors: Dave Coleman, Jorge Santos Simon 0.4.3 (2015-04-30) ------------------ * add functions for better display of convex meshes * produce actual triangles for qhull mesh * Fixed inverted scale for convex meshes inside check * Contributors: Christian Dornhege, Michael Ferguson 0.4.2 (2015-04-22) ------------------ * PR `#32 `_ Merge shape_tools package into geometric shapes * PR `#33 `_ Add run_depend on visualization_msgs * PR `#26 `_ Prevent every mesh generation opening a new file handle. * Contributors: Christian Dornhege, Dave Coleman, Jochen Sprickerhof, Michael Ferguson, Steven Peters 0.4.1 (2014-07-07) ------------------ * update distro for travis testing. precise:=trusty * update to use debian console_bridge dependency. https://github.com/ros/rosdistro/issues/4633 * Contributors: Ioan A Sucan, Tully Foote 0.4.0 (2014-06-24) ------------------ * update usage of console_bridge to deal with version in Trusty * Merge pull request `#13 `_ from ros-planning/testing-in-travis Run local and moveit_core tests in Travis builds. * Merge pull request `#18 `_ from dirk-thomas/hydro-devel fix configure config.h.in when paths contain spaces fix `#9 `_ * Run local and moveit_core tests in Travis builds. * Contributors: Acorn, Dave Hershberger, Dirk Thomas, Ioan A Sucan, William Woodall 0.3.8 (2014-02-25) ------------------ * fix how we find eigen * Contributors: Ioan Sucan 0.3.7 (2014-02-23) ------------------ * add build dep so we can find eigen, build fixes * Contributors: Ioan A Sucan, Scott K Logan 0.3.6 (2014-01-31) ------------------ * Use assimp-dev dep for building * Remove stray IS_ASSIMP3 define * Invert Assimp version detect logic for greater accuracy * Better feature detection for assimp version * added travis support * check for CATKIN_ENABLE_TESTING * Contributors: Dave Hershberger, Ioan A Sucan, Lukas Bulwahn, Scott K Logan 0.3.5 (2013-09-23) ------------------ * Fix syntax error. * white space fixes (tabs are now spaces) * add comments for shape definitions geometric_shapes-0.7.3/CMakeLists.txt000066400000000000000000000060121405115746100176050ustar00rootroot00000000000000cmake_minimum_required(VERSION 3.0.2) project(geometric_shapes) add_compile_options(-std=c++11) # Set compile options set(PROJECT_COMPILE_OPTIONS -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wno-unused-parameter ) list(APPEND CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake/") 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() # TODO(henningkayser): Remove policy fix when assimp 5.1 is available # Suppress policy warning in assimp (https://github.com/assimp/assimp/pull/2722) set(CMAKE_POLICY_DEFAULT_CMP0012 NEW) find_package(ASSIMP QUIET) if(NOT ASSIMP_FOUND) find_package(PkgConfig REQUIRED) # assimp is required, so REQUIRE the second attempt pkg_check_modules(ASSIMP_PC REQUIRED assimp) set(ASSIMP_INCLUDE_DIRS ${ASSIMP_PC_INCLUDE_DIRS}) endif() # find *absolute* paths to ASSIMP_LIBRARIES # Both, pkg-config and assimp's cmake-config don't provide an absolute library path. # For, pkg-config the path is in ASSIMP_PC_LIBRARY_DIRS, for cmake in ASSIMP_LIBRARY_DIRS. find_library(ASSIMP_ABS_LIBRARIES NAMES ${ASSIMP_LIBRARIES} assimp HINTS ${ASSIMP_LIBRARY_DIRS} ${ASSIMP_PC_LIBRARY_DIRS}) set(ASSIMP_LIBRARIES "${ASSIMP_ABS_LIBRARIES}") find_package(Boost REQUIRED system filesystem) find_package(console_bridge REQUIRED) find_package(Eigen3 REQUIRED) find_package(octomap REQUIRED) find_package(catkin REQUIRED COMPONENTS eigen_stl_containers random_numbers resource_retriever shape_msgs visualization_msgs ) catkin_package( INCLUDE_DIRS include LIBRARIES ${PROJECT_NAME} ${OCTOMAP_LIBRARIES} CATKIN_DEPENDS eigen_stl_containers random_numbers shape_msgs visualization_msgs DEPENDS EIGEN3 OCTOMAP console_bridge ) find_package(QHULL REQUIRED) include_directories(include) include_directories(SYSTEM ${EIGEN3_INCLUDE_DIR} ${Boost_INCLUDE_DIR} ${ASSIMP_INCLUDE_DIRS} ${OCTOMAP_INCLUDE_DIRS} ${QHULL_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} ${console_bridge_INCLUDE_DIRS}) add_library(${PROJECT_NAME} src/aabb.cpp src/bodies.cpp src/body_operations.cpp src/mesh_operations.cpp src/shape_extents.cpp src/shape_operations.cpp src/shape_to_marker.cpp 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}) if(CATKIN_ENABLE_TESTING) # Unit tests add_subdirectory(test) endif() install(TARGETS ${PROJECT_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) geometric_shapes-0.7.3/README.md000066400000000000000000000033461405115746100163330ustar00rootroot00000000000000# Geometric Shapes This package contains generic definitions of geometric shapes and bodies, as well as tools for operating on shape messages. Shapes represent only the form of an object. Bodies are shapes at a particular pose. Routines such as point containment and ray intersections are provided. Supported shapes: - sphere - box - cone - cylinder - mesh Note: Bodies for meshes compute the convex hull of those meshes in order to provide the point containment / ray intersection routines. 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=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=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=Nbin_uF64__geometric_shapes__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__geometric_shapes__ubuntu_focal_amd64__binary) geometric_shapes-0.7.3/cmake/000077500000000000000000000000001405115746100161265ustar00rootroot00000000000000geometric_shapes-0.7.3/cmake/FindQHULL.cmake000066400000000000000000000037361405115746100206270ustar00rootroot00000000000000############################################################################### # Find QHULL # # This sets the following variables: # QHULL_FOUND - True if QHULL was found. # QHULL_INCLUDE_DIRS - Directories containing the QHULL include files. # QHULL_LIBRARIES - Libraries needed to use QHULL. set(QHULL_RELEASE_NAME qhull_r) set(QHULL_DEBUG_NAME qhull_rd) find_file(QHULL_HEADER NAMES libqhull_r.h HINTS "${QHULL_ROOT}" "$ENV{QHULL_ROOT}" "${QHULL_INCLUDE_DIR}" PATHS "$ENV{PROGRAMFILES}/QHull" "$ENV{PROGRAMW6432}/QHull" PATH_SUFFIXES libqhull_r) set(QHULL_HEADER "${QHULL_HEADER}" CACHE INTERNAL "QHull header" FORCE) if(QHULL_HEADER) get_filename_component(QHULL_INCLUDE_DIR ${QHULL_HEADER} PATH) else(QHULL_HEADER) set(QHULL_INCLUDE_DIR "QHULL_INCLUDE_DIR-NOTFOUND") endif(QHULL_HEADER) set(QHULL_INCLUDE_DIR "${QHULL_INCLUDE_DIR}" CACHE PATH "QHull include dir." FORCE) find_library(QHULL_LIBRARY NAMES ${QHULL_RELEASE_NAME} HINTS "${QHULL_ROOT}" "$ENV{QHULL_ROOT}" PATHS "$ENV{PROGRAMFILES}/QHull" "$ENV{PROGRAMW6432}/QHull" PATH_SUFFIXES project build bin lib) find_library(QHULL_LIBRARY_DEBUG NAMES ${QHULL_DEBUG_NAME} ${QHULL_RELEASE_NAME} HINTS "${QHULL_ROOT}" "$ENV{QHULL_ROOT}" PATHS "$ENV{PROGRAMFILES}/QHull" "$ENV{PROGRAMW6432}/QHull" PATH_SUFFIXES project build bin lib) if(NOT QHULL_LIBRARY_DEBUG) set(QHULL_LIBRARY_DEBUG ${QHULL_LIBRARY}) endif(NOT QHULL_LIBRARY_DEBUG) 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) mark_as_advanced(QHULL_LIBRARY QHULL_LIBRARY_DEBUG QHULL_INCLUDE_DIR) if(QHULL_FOUND) set(HAVE_QHULL ON) message(STATUS "QHULL found (include: ${QHULL_INCLUDE_DIRS}, lib: ${QHULL_LIBRARIES})") endif(QHULL_FOUND) geometric_shapes-0.7.3/codecov.yaml000066400000000000000000000002301405115746100173470ustar00rootroot00000000000000coverage: precision: 2 round: up range: "45...70" status: project: default: target: auto threshold: 5% patch: off geometric_shapes-0.7.3/include/000077500000000000000000000000001405115746100164715ustar00rootroot00000000000000geometric_shapes-0.7.3/include/geometric_shapes/000077500000000000000000000000001405115746100220125ustar00rootroot00000000000000geometric_shapes-0.7.3/include/geometric_shapes/aabb.h000066400000000000000000000045341405115746100230560ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2019, Open Robotics * 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 Robotics 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. *********************************************************************/ /* Author: Martin Pecka */ #ifndef GEOMETRIC_SHAPES_AABB_H #define GEOMETRIC_SHAPES_AABB_H #include namespace bodies { /** \brief Represents an axis-aligned bounding box. */ class AABB : public Eigen::AlignedBox3d { EIGEN_MAKE_ALIGNED_OPERATOR_NEW // inherit parent class constructors (since C++11) using Eigen::AlignedBox3d::AlignedBox; public: /** \brief Extend with a box transformed by the given transform. */ void extendWithTransformedBox(const Eigen::Isometry3d& transform, const Eigen::Vector3d& box); }; } // namespace bodies #endif // GEOMETRIC_SHAPES_AABB_H geometric_shapes-0.7.3/include/geometric_shapes/bodies.h000066400000000000000000000531061405115746100234350ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * 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 the Willow Garage 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. *********************************************************************/ /* Author: Ioan Sucan, E. Gil Jones */ #ifndef GEOMETRIC_SHAPES_BODIES_ #define GEOMETRIC_SHAPES_BODIES_ #if __cplusplus <= 199711L #error This header requires at least C++11 #endif #include "geometric_shapes/aabb.h" #include "geometric_shapes/shapes.h" #include #include #include #include #include #include /** \brief This set of classes allows quickly detecting whether a given point is inside an object or not. This capability is useful when removing points from inside the robot (when the robot sees its arms, for example). */ namespace bodies { /** \brief Definition of a sphere that bounds another object */ struct BoundingSphere { Eigen::Vector3d center; double radius; EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; /** \brief Definition of a cylinder */ struct BoundingCylinder { Eigen::Isometry3d pose; double radius; double length; EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; class Body; /** \brief Shared pointer to a Body */ typedef std::shared_ptr BodyPtr; /** \brief Shared pointer to a const Body */ typedef std::shared_ptr BodyConstPtr; /** \brief A body is a shape + its pose. Point inclusion, ray intersection can be tested, volumes and bounding spheres can be computed.*/ class Body { public: Body() : scale_(1.0), padding_(0.0), type_(shapes::UNKNOWN_SHAPE) { pose_.setIdentity(); } virtual ~Body() = default; /** \brief Get the type of shape this body represents */ inline shapes::ShapeType getType() const { return type_; } /** * \brief If the dimension of the body should be scaled, this method sets the scale. * \note This is the dirty version of the function which does not update internal data that depend on the scale. * In the general case, you should call setScale() instead. Only call this function if you have a series of * calls like setScale/setPadding/setPose/setDimensions and you want to avoid the overhead of updating the * 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. */ 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 */ inline void setScale(double scale) { setScaleDirty(scale); updateInternalData(); } /** \brief Retrieve the current scale */ inline double getScale() const { return scale_; } /** * \brief If the dimension of the body should be padded, this method sets the pading. * \note This is the dirty version of the function which does not update internal data that depend on the scale. * In the general case, you should call setPadding() instead. Only call this function if you have a series of * calls like setScale/setPadding/setPose/setDimensions and you want to avoid the overhead of updating the * 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. */ 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 */ inline void setPadding(double padd) { setPaddingDirty(padd); updateInternalData(); } /** \brief Retrieve the current padding */ inline double getPadding() const { return padding_; } /** * \brief Set the pose of the body. * \note This is the dirty version of the function which does not update internal data that depend on the pose. * In the general case, you should call setPose() instead. Only call this function if you have a series of * calls like setScale/setPadding/setPose/setDimensions and you want to avoid the overhead of updating the * internal structures after each call. When you are finished with the batch, call updateInternalData(). * \param pose The pose to set. Default is identity. */ inline void setPoseDirty(const Eigen::Isometry3d& pose) { pose_ = pose; } /** \brief Set the pose of the body. Default is identity */ inline void setPose(const Eigen::Isometry3d& pose) { setPoseDirty(pose); updateInternalData(); } /** \brief Retrieve the pose of the body */ inline const Eigen::Isometry3d& getPose() const { return pose_; } /** * \brief Set the dimensions of the body (from corresponding shape). * \note This is the dirty version of the function which does not update internal data that depend on the dimensions. * In the general case, you should call setDimensions() instead. Only call this function if you have a series of * calls like setScale/setPadding/setPose/setDimensions and you want to avoid the overhead of updating the * internal structures after each call. When you are finished with the batch, call updateInternalData(). * \param shape The shape whose dimensions should be assumed. After the function finishes, the pointer can be deleted. */ inline void setDimensionsDirty(const shapes::Shape* shape) { useDimensions(shape); } /** \brief Get the dimensions associated to this body (as read from corresponding shape) */ virtual std::vector getDimensions() const = 0; /** \brief Get the dimensions associated to this body (scaled and padded) */ virtual std::vector getScaledDimensions() const = 0; /** \brief Set the dimensions of the body (from corresponding shape) */ inline void setDimensions(const shapes::Shape* shape) { setDimensionsDirty(shape); updateInternalData(); } /** \brief Check if a point is inside the body */ inline bool containsPoint(double x, double y, double z, bool verbose = false) const { Eigen::Vector3d pt(x, y, z); return containsPoint(pt, verbose); } /** \brief Check if a point is inside the body. Surface points are included. */ virtual bool containsPoint(const Eigen::Vector3d& p, bool verbose = false) const = 0; /** \brief Check if a ray intersects the body, and find the set of intersections, in order, along the ray. A maximum number of intersections can be specified as well. If that number is 0, all intersections are returned. Passing dir as a unit vector will result in faster computation. */ virtual bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir, EigenSTL::vector_Vector3d* intersections = nullptr, unsigned int count = 0) const = 0; /** \brief Compute the volume of the body. This method includes changes induced by scaling and padding */ virtual double computeVolume() const = 0; /** \brief Sample a point that is included in the body using a given random number generator. Sometimes multiple attempts need to be generated. The function terminates with failure (returns false) after \e max_attempts attempts. If the call is successful (returns true) the point is written to \e result */ virtual bool samplePointInside(random_numbers::RandomNumberGenerator& rng, unsigned int max_attempts, Eigen::Vector3d& result) const; /** \brief Compute the bounding radius for the body, in its current pose. Scaling and padding are accounted for. */ virtual void computeBoundingSphere(BoundingSphere& sphere) const = 0; /** \brief Compute the bounding cylinder for the body, in its current pose. Scaling and padding are accounted for. */ virtual void computeBoundingCylinder(BoundingCylinder& cylinder) const = 0; /** \brief Compute the axis-aligned bounding box for the body, in its current pose. Scaling and padding are accounted for. */ virtual void computeBoundingBox(AABB& bbox) const = 0; /** \brief Get a clone of this body, but one that is located at the pose \e pose */ inline BodyPtr cloneAt(const Eigen::Isometry3d& pose) const { return cloneAt(pose, padding_, scale_); } /** \brief Get a clone of this body, but one that is located at the pose \e pose and has possibly different passing and scaling: \e padding and \e scaling. This function is useful to implement thread safety, when bodies need to be moved around. */ virtual BodyPtr cloneAt(const Eigen::Isometry3d& pose, double padding, double scaling) const = 0; /** \brief This function is called every time a change to the body is made, so that intermediate values stored for efficiency reasons are kept up to date. */ virtual void updateInternalData() = 0; protected: /** \brief Depending on the shape, this function copies the relevant data to the body. */ virtual void useDimensions(const shapes::Shape* shape) = 0; /** \brief The scale that was set for this body */ double scale_; /** \brief The scale that was set for this body */ double padding_; /** \brief The type of shape this body was constructed from */ shapes::ShapeType type_; /** \brief The location of the body (position and orientation) */ Eigen::Isometry3d pose_; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; /** \brief Definition of a sphere */ class Sphere : public Body { public: Sphere() : Body() { type_ = shapes::SPHERE; } Sphere(const shapes::Shape* shape) : Body() { type_ = shapes::SPHERE; setDimensions(shape); } explicit Sphere(const BoundingSphere& sphere); ~Sphere() override = default; /** \brief Get the radius of the sphere */ std::vector getDimensions() const override; std::vector getScaledDimensions() const override; bool containsPoint(const Eigen::Vector3d& p, bool verbose = false) const override; double computeVolume() const override; bool samplePointInside(random_numbers::RandomNumberGenerator& rng, unsigned int max_attempts, Eigen::Vector3d& result) const override; void computeBoundingSphere(BoundingSphere& sphere) const override; void computeBoundingCylinder(BoundingCylinder& cylinder) const override; void computeBoundingBox(AABB& bbox) const override; bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir, EigenSTL::vector_Vector3d* intersections = nullptr, unsigned int count = 0) const override; BodyPtr cloneAt(const Eigen::Isometry3d& pose, double padding, double scale) const override; void updateInternalData() override; protected: void useDimensions(const shapes::Shape* shape) override; // shape-dependent data double radius_; // pose/padding/scaling-dependent values & values computed for convenience and fast upcoming computations Eigen::Vector3d center_; double radiusU_; double radius2_; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; /** \brief Definition of a cylinder */ class Cylinder : public Body { public: Cylinder() : Body() { type_ = shapes::CYLINDER; } Cylinder(const shapes::Shape* shape) : Body() { type_ = shapes::CYLINDER; setDimensions(shape); } explicit Cylinder(const BoundingCylinder& cylinder); ~Cylinder() override = default; /** \brief Get the radius & length of the cylinder */ std::vector getDimensions() const override; std::vector getScaledDimensions() const override; bool containsPoint(const Eigen::Vector3d& p, bool verbose = false) const override; double computeVolume() const override; bool samplePointInside(random_numbers::RandomNumberGenerator& rng, unsigned int max_attempts, Eigen::Vector3d& result) const override; void computeBoundingSphere(BoundingSphere& sphere) const override; void computeBoundingCylinder(BoundingCylinder& cylinder) const override; void computeBoundingBox(AABB& bbox) const override; bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir, EigenSTL::vector_Vector3d* intersections = nullptr, unsigned int count = 0) const override; BodyPtr cloneAt(const Eigen::Isometry3d& pose, double padding, double scale) const override; void updateInternalData() override; protected: void useDimensions(const shapes::Shape* shape) override; // shape-dependent data double length_; double radius_; // pose/padding/scaling-dependent values & values computed for convenience and fast upcoming computations Eigen::Vector3d center_; Eigen::Vector3d normalH_; Eigen::Vector3d normalB1_; Eigen::Vector3d normalB2_; double length2_; double radiusU_; double radiusB_; double radiusBSqr_; double radius2_; double d1_; double d2_; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; /** \brief Definition of a box */ class Box : public Body { public: Box() : Body() { type_ = shapes::BOX; } Box(const shapes::Shape* shape) : Body() { type_ = shapes::BOX; setDimensions(shape); } explicit Box(const AABB& aabb); ~Box() override = default; /** \brief Get the length & width & height (x, y, z) of the box */ std::vector getDimensions() const override; std::vector getScaledDimensions() const override; bool containsPoint(const Eigen::Vector3d& p, bool verbose = false) const override; double computeVolume() const override; bool samplePointInside(random_numbers::RandomNumberGenerator& rng, unsigned int max_attempts, Eigen::Vector3d& result) const override; void computeBoundingSphere(BoundingSphere& sphere) const override; void computeBoundingCylinder(BoundingCylinder& cylinder) const override; void computeBoundingBox(AABB& bbox) const override; bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir, EigenSTL::vector_Vector3d* intersections = nullptr, unsigned int count = 0) const override; BodyPtr cloneAt(const Eigen::Isometry3d& pose, double padding, double scale) const override; void updateInternalData() override; protected: void useDimensions(const shapes::Shape* shape) override; // (x, y, z) = (length, width, height) // shape-dependent data double length_; double width_; double height_; // pose/padding/scaling-dependent values & values computed for convenience and fast upcoming computations Eigen::Vector3d center_; Eigen::Matrix3d invRot_; Eigen::Vector3d minCorner_; //!< The translated, but not rotated min corner Eigen::Vector3d maxCorner_; //!< The translated, but not rotated max corner double length2_; double width2_; double height2_; double radiusB_; double radius2_; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; /** \brief Definition of a convex mesh. Convex hull is computed for a given shape::Mesh */ class ConvexMesh : public Body { public: ConvexMesh() : Body() { type_ = shapes::MESH; scaled_vertices_ = nullptr; } ConvexMesh(const shapes::Shape* shape) : Body() { type_ = shapes::MESH; scaled_vertices_ = nullptr; setDimensions(shape); } ~ConvexMesh() override = default; /** \brief Returns an empty vector */ std::vector getDimensions() const override; /** \brief Returns an empty vector */ std::vector getScaledDimensions() const override; bool containsPoint(const Eigen::Vector3d& p, bool verbose = false) const override; double computeVolume() const override; void computeBoundingSphere(BoundingSphere& sphere) const override; void computeBoundingCylinder(BoundingCylinder& cylinder) const override; void computeBoundingBox(AABB& bbox) const override; bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir, EigenSTL::vector_Vector3d* intersections = nullptr, unsigned int count = 0) const override; const std::vector& getTriangles() const; const EigenSTL::vector_Vector3d& getVertices() const; const EigenSTL::vector_Vector3d& getScaledVertices() const; /** * @brief Get the planes that define the convex shape. * @return A list of Vector4d(nx, ny, nz, d). */ const EigenSTL::vector_Vector4d& getPlanes() const; BodyPtr cloneAt(const Eigen::Isometry3d& pose, double padding, double scale) const override; /// Project the original vertex to the scaled and padded planes and average. void computeScaledVerticesFromPlaneProjections(); void correctVertexOrderFromPlanes(); void updateInternalData() override; protected: void useDimensions(const shapes::Shape* shape) override; /** \brief (Used mainly for debugging) Count the number of vertices behind a plane*/ unsigned int countVerticesBehindPlane(const Eigen::Vector4f& planeNormal) const; /** \brief Check if the point is inside all halfspaces this mesh consists of (mesh_data_->planes_). * * \note The point is expected to have pose_ "cancelled" (have inverse pose of this mesh applied to it). * \note Scale and padding of the mesh are taken into account. * \note There is a 1e-9 margin "outside" the planes where points are still considered to be inside. */ bool isPointInsidePlanes(const Eigen::Vector3d& point) const; // 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_; // pose/padding/scaling-dependent values & values computed for convenience and fast upcoming computations Eigen::Isometry3d i_pose_; Eigen::Vector3d center_; double radiusB_; double radiusBSqr_; Box bounding_box_; // pointer to an array of scaled vertices // If the padding is 0 & scaling is 1, then there is no need to have scaled vertices; // we can just point to the vertices in mesh_data_. // Otherwise, point to scaled_vertices_storage_ EigenSTL::vector_Vector3d* scaled_vertices_; private: std::unique_ptr scaled_vertices_storage_; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; /** @class BodyVector * @brief A vector of Body objects */ class BodyVector { public: BodyVector(); /** \brief Construct a body vector from a vector of shapes, a vector of poses and a padding */ BodyVector(const std::vector& shapes, const EigenSTL::vector_Isometry3d& poses, double padding = 0.0); ~BodyVector(); /** \brief Add a body*/ void addBody(Body* body); /** \brief Add a body from a shape, a pose for the body and a padding */ void addBody(const shapes::Shape* shape, const Eigen::Isometry3d& pose, double padding = 0.0); /** \brief Clear all bodies from the vector*/ void clear(); /** \brief Set the pose of a particular body in the vector of bodies */ void setPose(unsigned int i, const Eigen::Isometry3d& pose); /** \brief Get the number of bodies in this vector*/ std::size_t getCount() const; /** \brief Check if any body in the vector contains the input point */ bool containsPoint(const Eigen::Vector3d& p, bool verbose = false) const; /** \brief Check if any body contains the input point, and report the first body's index if so */ bool containsPoint(const Eigen::Vector3d& p, std::size_t& index, bool verbose = false) const; /** \brief Check if any of the bodies intersects the ray defined by \e origin and \e dir. When the first intersection is found, this function terminates. The index of the body that does intersect the ray is set to \e index (unset if no intersections were found). Optionally, the intersection points are computed and set to \e intersections (only for the first body that is found to intersect the ray) */ bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir, std::size_t& index, EigenSTL::vector_Vector3d* intersections = nullptr, unsigned int count = 0) const; /** \brief Get the \e ith body in the vector*/ const Body* getBody(unsigned int i) const; private: std::vector bodies_; }; /** \brief Shared pointer to a Body */ typedef std::shared_ptr BodyPtr; /** \brief Shared pointer to a const Body */ typedef std::shared_ptr BodyConstPtr; } // namespace bodies #endif geometric_shapes-0.7.3/include/geometric_shapes/body_operations.h000066400000000000000000000072151405115746100253700ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * 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 the Willow Garage 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. *********************************************************************/ /* Author: Ioan Sucan, E. Gil Jones */ #ifndef GEOMETRIC_SHAPES_BODY_OPERATIONS_ #define GEOMETRIC_SHAPES_BODY_OPERATIONS_ #include "geometric_shapes/shapes.h" #include "geometric_shapes/bodies.h" #include "geometric_shapes/shape_messages.h" #include #include #include namespace bodies { /** \brief Create a body from a given shape */ Body* createEmptyBodyFromShapeType(const shapes::ShapeType& shapeType); /** \brief Create a body from a given shape */ Body* createBodyFromShape(const shapes::Shape* shape); /** \brief Create a body from a given shape */ Body* constructBodyFromMsg(const shape_msgs::Mesh& shape, const geometry_msgs::Pose& pose); /** \brief Create a body from a given shape */ Body* constructBodyFromMsg(const shape_msgs::SolidPrimitive& shape, const geometry_msgs::Pose& pose); /** \brief Create a body from a given shape */ Body* constructBodyFromMsg(const shapes::ShapeMsg& shape, const geometry_msgs::Pose& pose); /** \brief Get a shape that corresponds to this (scaled and padded) body. */ shapes::ShapeConstPtr constructShapeFromBody(const bodies::Body* body); /** \brief Construct a Marker message that corresponds to this (scaled and padded) body. */ void constructMarkerFromBody(const bodies::Body* body, visualization_msgs::Marker& msg); /** \brief Compute a bounding sphere to enclose a set of bounding spheres */ void mergeBoundingSpheres(const std::vector& spheres, BoundingSphere& mergedSphere); /** \brief Compute an axis-aligned bounding box to enclose a set of bounding boxes. */ void mergeBoundingBoxes(const std::vector& boxes, AABB& mergedBox); /** \brief Compute the bounding sphere for a set of \e bodies and store the resulting sphere in \e mergedSphere */ void computeBoundingSphere(const std::vector& bodies, BoundingSphere& mergedSphere); } // namespace bodies #endif geometric_shapes-0.7.3/include/geometric_shapes/check_isometry.h000066400000000000000000000134351405115746100252010ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2020, Martin Pecka * 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 the Willow Garage 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. *********************************************************************/ /* Author: Martin Pecka, Robert Haschke */ #ifndef GEOMETRIC_SHAPES_CHECK_ISOMETRY_H #define GEOMETRIC_SHAPES_CHECK_ISOMETRY_H #include #include #include #include #include #include /** \brief This file provides functions and macros that can be used to verify that an Eigen::Isometry3d is really an * isometry. Eigen itself doesn't do the checks because they're expensive. If the isometry object is constructed in * a wrong way (e.g. from an AngleAxisd with non-unit axis), it can represent a non-isometry. But some methods in the * Isometry3d class perform isometry-specific operations which return wrong results when called on a non-isometry. E.g. * for isometries, .linear() and .rotation() should be the same, but for non-isometries, the result of .linear() * contains also the scaling factor, whether .rotation() is only the rotation part. * * These checks are primarily meant to be performed only in debug mode (via the ASSERT_ISOMETRY macro), but you can call * checkIsometry() even in release mode. This check should be mainly performed on transforms input by the user. */ #ifndef CHECK_ISOMETRY_PRECISION /** \brief The default precision to which the transform has to correspond to an isometry. */ #define CHECK_ISOMETRY_PRECISION Eigen::NumTraits::dummy_precision() #endif /** * \brief Check whether the given transform is really (mathematically) an isometry. * \param transform The possibly non-isometric transform. * \param precision Precision to which the transform has to correspond to an isometry (element-wise). * \param printError Whether an error should be printed to std::cerr with details about the false isometry. * \return Whether the given transform is close to an isometry or not. */ inline bool checkIsometry(const Eigen::Isometry3d& transform, const double precision = CHECK_ISOMETRY_PRECISION, const bool printError = true) { if (!transform.matrix().row(3).isApprox(Eigen::Vector4d::UnitW().transpose(), precision)) { if (printError) { std::cerr << "The given transform is not an isometry! Its last row deviates from [0 0 0 1] by [" << (transform.matrix().row(3) - Eigen::Vector4d::UnitW().transpose()) << "] but the required precision is " << precision << "." << std::endl; } return false; } Eigen::Isometry3d::LinearMatrixType scale; transform.computeRotationScaling((Eigen::Isometry3d::LinearMatrixType*)nullptr, &scale); if (!scale.isApprox(Eigen::Matrix3d::Identity(), precision)) { if (printError) { std::cerr << "The given transform is not an isometry! Its linear part involves non-unit scaling. The scaling " "matrix diagonal differs from [1 1 1] by [" << (scale.diagonal().transpose() - Eigen::Vector3d::Ones().transpose()) << "] but the required precision is " << precision << "." << std::endl; } return false; } return true; } // To be able to use the more efficient Eigen::Transform::linear() instead of rotation(), // we need to check the user has really passed an isometry. To avoid runtime costs, // this check is only done as assert, which gets compiled-out in release builds #ifdef NDEBUG /** \brief Assert that the given transform is an isometry (no-op in release mode). */ #define ASSERT_ISOMETRY(transform) (void)sizeof(transform); // this is a no-op, but prevents unused variable warnings #else /** \brief Assert that the given transform is an isometry. */ #define ASSERT_ISOMETRY(transform) \ { \ if (!::checkIsometry(transform, CHECK_ISOMETRY_PRECISION)) \ assert(!"Invalid isometry transform"); \ } #endif #endif // GEOMETRIC_SHAPES_CHECK_ISOMETRY_H geometric_shapes-0.7.3/include/geometric_shapes/mesh_operations.h000066400000000000000000000110441405115746100253620ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * 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 the Willow Garage 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. *********************************************************************/ /* Author: Ioan Sucan */ #ifndef GEOMETRIC_SHAPES_MESH_OPERATIONS_ #define GEOMETRIC_SHAPES_MESH_OPERATIONS_ #include "geometric_shapes/shapes.h" #include #include // forward declaration of aiScene (caller needs to include assimp if aiScene is used) class aiScene; namespace shapes { /** \brief Load a mesh from a set of vertices. Triangles are constructed using index values from the triangles vector. Triangle k has vertices at index values triangles[3k], triangles[3k+1], triangles[3k+2] */ Mesh* createMeshFromVertices(const EigenSTL::vector_Vector3d& vertices, const std::vector& triangles); /** \brief Load a mesh from a set of vertices. Every 3 vertices are considered a triangle. Repeating vertices are identified and the set of triangle indices is constructed. The normal at each triangle is also computed */ Mesh* createMeshFromVertices(const EigenSTL::vector_Vector3d& source); /** \brief Load a mesh from a resource that contains a mesh that can be loaded by assimp */ Mesh* createMeshFromResource(const std::string& resource); /** \brief Load a mesh from a resource that contains a mesh that can be loaded by assimp */ Mesh* createMeshFromResource(const std::string& resource, const Eigen::Vector3d& scale); /** \brief Load a mesh from a binary stream that contains a mesh that can be loaded by assimp */ Mesh* createMeshFromBinary(const char* buffer, std::size_t size, const std::string& assimp_hint = std::string()); /** \brief Load a mesh from a resource that contains a mesh that can be loaded by assimp */ Mesh* createMeshFromBinary(const char* buffer, std::size_t size, const Eigen::Vector3d& scale, const std::string& assimp_hint = std::string()); /** \brief Load a mesh from an assimp datastructure */ Mesh* createMeshFromAsset(const aiScene* scene, const Eigen::Vector3d& scale, const std::string& assimp_hint = std::string()); /** \brief Load a mesh from an assimp datastructure */ Mesh* createMeshFromAsset(const aiScene* scene, const std::string& assimp_hint = std::string()); /** \brief Construct a mesh from a primitive shape that is NOT already a mesh. This call allocates a new object. */ Mesh* createMeshFromShape(const Shape* shape); /** \brief Construct a mesh from a box */ Mesh* createMeshFromShape(const Box& box); /** \brief Construct a mesh from a sphere */ Mesh* createMeshFromShape(const Sphere& sphere); /** \brief Construct a mesh from a cylinder */ Mesh* createMeshFromShape(const Cylinder& cylinder); /** \brief Construct a mesh from a cone */ Mesh* createMeshFromShape(const Cone& cone); /** \brief Write the mesh to a buffer in STL format */ void writeSTLBinary(const Mesh* mesh, std::vector& buffer); } // namespace shapes #endif geometric_shapes-0.7.3/include/geometric_shapes/shape_extents.h000066400000000000000000000046421405115746100250430ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2012, Willow Garage, Inc. * 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 the Willow Garage 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. *********************************************************************/ #ifndef GEOMETRIC_SHAPES_SHAPE_EXTENTS_ #define GEOMETRIC_SHAPES_SHAPE_EXTENTS_ #include #include namespace geometric_shapes { /** \brief Get the dimensions of an axis-aligned bounding box for the shape described by \e shape_msg */ void getShapeExtents(const shape_msgs::SolidPrimitive& shape_msg, double& x_extent, double& y_extent, double& z_extent); /** \brief Get the dimensions of an axis-aligned bounding box for the shape described by \e shape_msg */ void getShapeExtents(const shape_msgs::Mesh& shape_msg, double& x_extent, double& y_extent, double& z_extent); } // namespace geometric_shapes #endif geometric_shapes-0.7.3/include/geometric_shapes/shape_messages.h000066400000000000000000000045551405115746100251630ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * 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 the Willow Garage 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. *********************************************************************/ /* Author: Ioan Sucan */ #ifndef GEOMETRIC_SHAPES_SHAPE_MESSAGES_ #define GEOMETRIC_SHAPES_SHAPE_MESSAGES_ #include #include #include #include #if __cplusplus <= 199711L #error This header requires at least C++11 (boost::variant is incompatible between c++98 and c++11 and we enforce 11) #endif namespace shapes { /** \brief Type that can hold any of the desired shape message types */ typedef boost::variant ShapeMsg; } // namespace shapes #endif geometric_shapes-0.7.3/include/geometric_shapes/shape_operations.h000066400000000000000000000074171405115746100255370ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * 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 the Willow Garage 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. *********************************************************************/ /* Author: Ioan Sucan */ #ifndef GEOMETRIC_SHAPES_SHAPE_OPERATIONS_ #define GEOMETRIC_SHAPES_SHAPE_OPERATIONS_ #include "geometric_shapes/shapes.h" #include "geometric_shapes/shape_messages.h" #include "geometric_shapes/mesh_operations.h" #include #include namespace shapes { /** \brief Construct the shape that corresponds to the message. Return NULL on failure. */ Shape* constructShapeFromMsg(const shape_msgs::SolidPrimitive& shape_msg); /** \brief Construct the shape that corresponds to the message. Return NULL on failure. */ Shape* constructShapeFromMsg(const shape_msgs::Plane& shape_msg); /** \brief Construct the shape that corresponds to the message. Return NULL on failure. */ Shape* constructShapeFromMsg(const shape_msgs::Mesh& shape_msg); /** \brief Construct the shape that corresponds to the message. Return NULL on failure. */ Shape* constructShapeFromMsg(const ShapeMsg& shape_msg); /** \brief Construct the message that corresponds to the shape. Return false on failure. */ bool constructMsgFromShape(const Shape* shape, ShapeMsg& shape_msg); /** \brief Construct the marker that corresponds to the shape. Return false on failure. */ bool constructMarkerFromShape(const Shape* shape, visualization_msgs::Marker& mk, bool use_mesh_triangle_list = false); /** \brief Compute the extents of a shape */ Eigen::Vector3d computeShapeExtents(const ShapeMsg& shape_msg); /** \brief Compute the extents of a shape */ Eigen::Vector3d computeShapeExtents(const Shape* shape); /** \brief Compute a sphere bounding a shape */ void computeShapeBoundingSphere(const Shape* shape, Eigen::Vector3d& center, double& radius); /** \brief Get the string name of the shape */ const std::string& shapeStringName(const Shape* shape); /** \brief Save all the information about this shape as plain text */ void saveAsText(const Shape* shape, std::ostream& out); /** \brief Construct a shape from plain text description */ Shape* constructShapeFromText(std::istream& in); } // namespace shapes #endif geometric_shapes-0.7.3/include/geometric_shapes/shape_to_marker.h000066400000000000000000000054511405115746100253330ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2012, Willow Garage, Inc. * 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 the Willow Garage 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. *********************************************************************/ #ifndef GEOMETRIC_SHAPES_SHAPE_TO_MARKER_ #define GEOMETRIC_SHAPES_SHAPE_TO_MARKER_ #include #include #include namespace geometric_shapes { /** \brief Convert a shape_msgs::Mesh \e shape_msg to a visualization_msgs::Marker \e marker. The corresponding marker will be constructed as a LINE_LIST (if \e use_mesh_triangle_list is false) or as a TRIANGLE_LIST (if \e use_mesh_triangle_list is true). On incorrect input, this function throws a std::runtime_error. */ void constructMarkerFromShape(const shape_msgs::Mesh& shape_msg, visualization_msgs::Marker& marker, bool use_mesh_triangle_list = true); /** \brief Convert a shape_msgs::SolidPrimitive \e shape_msg to a visualization_msgs::Marker \e marker. On incorrect input, this function throws a std::runtime_error. */ void constructMarkerFromShape(const shape_msgs::SolidPrimitive& shape_msg, visualization_msgs::Marker& marker); } // namespace geometric_shapes #endif geometric_shapes-0.7.3/include/geometric_shapes/shapes.h000066400000000000000000000314651405115746100234570ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * 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 the Willow Garage 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. *********************************************************************/ /* Author: Ioan Sucan */ #ifndef GEOMETRIC_SHAPES_SHAPES_ #define GEOMETRIC_SHAPES_SHAPES_ #if __cplusplus <= 199711L #error This header requires at least C++11 #endif #include #include #include #include #include namespace octomap { class OcTree; } /** \brief Definition of various shapes. No properties such as position are included. These are simply the descriptions and dimensions of shapes. */ namespace shapes { /** \brief A list of known shape types */ enum ShapeType { UNKNOWN_SHAPE, SPHERE, CYLINDER, CONE, BOX, PLANE, MESH, OCTREE }; /* convert above enum to printable */ std::ostream& operator<<(std::ostream& ss, ShapeType type); /** \brief A basic definition of a shape. Shapes are considered centered at origin */ class Shape { public: Shape(); virtual ~Shape(); /** \brief Create a copy of this shape */ virtual Shape* clone() const = 0; /** \brief Print information about this shape */ virtual void print(std::ostream& out = std::cout) const; /** \brief Uniformly scale this shape by a factor */ void scale(double scale); /** \brief Add uniform padding to this shape */ void padd(double padding); /** \brief Uniformly scale and padd this shape */ virtual void scaleAndPadd(double scale, double padd) = 0; /** \brief Return a flag indicating whether this shape can be scaled and/or padded */ virtual bool isFixed() const; /** \brief The type of the shape */ ShapeType type; }; /** \brief Definition of a sphere */ class Sphere : public Shape { public: Sphere(); /** \brief The radius of the shpere */ Sphere(double r); /** \brief The type of the shape, as a string */ static const std::string STRING_NAME; void scaleAndPadd(double scale, double padd) override; Sphere* clone() const override; void print(std::ostream& out = std::cout) const override; /** \brief The radius of the sphere */ double radius; }; /** \brief Definition of a cylinder * Length is along z axis. Origin is at center of mass. */ class Cylinder : public Shape { public: using Shape::padd; using Shape::scale; Cylinder(); /** \brief The radius and the length of the cylinder */ Cylinder(double r, double l); /** \brief The type of the shape, as a string */ static const std::string STRING_NAME; /** * \brief Scale this shape by a non-uniform factor. * \param scaleRadius Radius scaling factor. * \param scaleLength Cylinder length scaling factor. */ void scale(double scaleRadius, double scaleLength); /** * \brief Add non-uniform padding to this shape. * \param paddRadius Radius padding (in meters). * \param paddLength Cylinder length padding (in meters). */ void padd(double paddRadius, double paddLength); /** * \brief Scale this shape by a non-uniform factor and then add non-uniform padding. * \param scaleRadius Radius scaling factor. * \param scaleLength Cylinder length scaling factor. * \param paddRadius Radius padding (in meters). * \param paddLength Cylinder length padding (in meters). */ void scaleAndPadd(double scaleRadius, double scaleLength, double paddRadius, double paddLength); void scaleAndPadd(double scale, double padd) override; Cylinder* clone() const override; void print(std::ostream& out = std::cout) const override; /** \brief The length of the cylinder */ double length; /** \brief The radius of the cylinder */ double radius; }; /** \brief Definition of a cone * Tip is on positive z axis. Center of base is on negative z axis. Origin is * halway between tip and center of base. */ class Cone : public Shape { public: using Shape::padd; using Shape::scale; Cone(); Cone(double r, double l); /** \brief The type of the shape, as a string */ static const std::string STRING_NAME; /** * \brief Scale this shape by a non-uniform factor. * \param scaleRadius Radius scaling factor. * \param scaleLength Cone length scaling factor. */ void scale(double scaleRadius, double scaleLength); /** * \brief Add non-uniform padding to this shape. * \param paddRadius Radius padding (in meters). * \param paddLength Cone length padding (in meters). */ void padd(double paddRadius, double paddLength); /** * \brief Scale this shape by a non-uniform factor and then add non-uniform padding. * \param scaleRadius Radius scaling factor. * \param scaleLength Cone length scaling factor. * \param paddRadius Radius padding (in meters). * \param paddLength Cone length padding (in meters). */ void scaleAndPadd(double scaleRadius, double scaleLength, double paddRadius, double paddLength); void scaleAndPadd(double scale, double padd) override; Cone* clone() const override; void print(std::ostream& out = std::cout) const override; /** \brief The length (height) of the cone */ double length; /** \brief The radius of the cone */ double radius; }; /** \brief Definition of a box * Aligned with the XYZ axes. */ class Box : public Shape { public: using Shape::padd; using Shape::scale; Box(); Box(double x, double y, double z); /** \brief The type of the shape, as a string */ static const std::string STRING_NAME; /** * \brief Scale this shape by a non-uniform factor. * \param scaleX Scale in x-dimension. * \param scaleY Scale in y-dimension. * \param scaleZ Scale in z-dimension. */ void scale(double scaleX, double scaleY, double scaleZ); /** * \brief Add non-uniform padding to this shape. * \param paddX Padding in x-dimension (in meters). * \param paddY Padding in y-dimension (in meters). * \param paddZ Padding in z-dimension (in meters). */ void padd(double paddX, double paddY, double paddZ); /** * \brief Scale this shape by a non-uniform factor and then add non-uniform padding. * \param scaleX Scale in x-dimension. * \param scaleY Scale in y-dimension. * \param scaleZ Scale in z-dimension. * \param paddX Padding in x-dimension (in meters). * \param paddY Padding in y-dimension (in meters). * \param paddZ Padding in z-dimension (in meters). */ void scaleAndPadd(double scaleX, double scaleY, double scaleZ, double paddX, double paddY, double paddZ); void scaleAndPadd(double scale, double padd) override; Box* clone() const override; void print(std::ostream& out = std::cout) const override; /** \brief x, y, z dimensions of the box (axis-aligned) */ double size[3]; }; /** \brief Definition of a triangle mesh * By convention the "center" of the shape is at the origin. For a mesh this * implies that the AABB of the mesh is centered at the origin. Some methods * may not work with arbitrary meshes whose AABB is not centered at the origin. * Padding is not applied to vertices plainly coordinate-wise, but instead the * padding value is added to the length of the direction vector between centroid * and each vertex. * */ class Mesh : public Shape { public: using Shape::padd; using Shape::scale; Mesh(); Mesh(unsigned int v_count, unsigned int t_count); ~Mesh() override; /** \brief The type of the shape, as a string */ static const std::string STRING_NAME; /** * \brief Scale this shape by a non-uniform factor. * \param scaleX Scale in x-dimension. * \param scaleY Scale in y-dimension. * \param scaleZ Scale in z-dimension. */ void scale(double scaleX, double scaleY, double scaleZ); /** * \brief Add non-uniform padding to this shape. * \param paddX Padding in x-dimension (in meters). * \param paddY Padding in y-dimension (in meters). * \param paddZ Padding in z-dimension (in meters). * \note Padding is not applied to vertices plainly coordinate-wise, but instead the * padding value is added to the length of the direction vector between centroid * and each vertex. */ void padd(double paddX, double paddY, double paddZ); /** * \brief Scale this shape by a non-uniform factor and then add non-uniform padding. * \param scaleX Scale in x-dimension. * \param scaleY Scale in y-dimension. * \param scaleZ Scale in z-dimension. * \param paddX Padding in x-dimension (in meters). * \param paddY Padding in y-dimension (in meters). * \param paddZ Padding in z-dimension (in meters). * \note Padding is not applied to vertices plainly coordinate-wise, but instead the * padding value is added to the length of the direction vector between centroid * and each vertex. */ void scaleAndPadd(double scaleX, double scaleY, double scaleZ, double paddX, double paddY, double paddZ); void scaleAndPadd(double scale, double padd) override; Mesh* clone() const override; void print(std::ostream& out = std::cout) const override; /** \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. Calls computeTriangleNormals() if needed. */ void computeVertexNormals(); /** \brief Merge vertices that are very close to each other, up to a threshold*/ void mergeVertices(double threshold); /** \brief The number of available vertices */ unsigned int vertex_count; /** \brief The position for each vertex vertex k has values at * index (3k, 3k+1, 3k+2) = (x,y,z) */ double* vertices; /** \brief The number of triangles formed with the vertices */ unsigned int triangle_count; /** \brief The vertex indices for each triangle * triangle k has vertices at index (3k, 3k+1, 3k+2) = (v1, v2, v3) */ unsigned int* triangles; /** \brief The normal to each triangle; unit vector represented as (x,y,z); If missing from the mesh, these vectors can be computed using computeTriangleNormals() */ double* triangle_normals; /** \brief The normal to each vertex; unit vector represented as (x,y,z); If missing from the mesh, these vectors can be computed using computeVertexNormals() */ double* vertex_normals; }; /** \brief Definition of a plane with equation ax + by + cz + d = 0 */ class Plane : public Shape { public: Plane(); Plane(double pa, double pb, double pc, double pd); /** \brief The type of the shape, as a string */ static const std::string STRING_NAME; Plane* clone() const override; void print(std::ostream& out = std::cout) const override; void scaleAndPadd(double scale, double padd) override; bool isFixed() const override; /** \brief The plane equation is ax + by + cz + d = 0 */ double a, b, c, d; }; /** \brief Representation of an octomap::OcTree as a Shape */ class OcTree : public Shape { public: OcTree(); OcTree(const std::shared_ptr& t); /** \brief The type of the shape, as a string */ static const std::string STRING_NAME; OcTree* clone() const override; void print(std::ostream& out = std::cout) const override; void scaleAndPadd(double scale, double padd) override; bool isFixed() const override; std::shared_ptr octree; }; /** \brief Shared pointer to a Shape */ typedef std::shared_ptr ShapePtr; /** \brief Shared pointer to a const Shape */ typedef std::shared_ptr ShapeConstPtr; } // namespace shapes #endif geometric_shapes-0.7.3/include/geometric_shapes/solid_primitive_dims.h000066400000000000000000000053051405115746100264040ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2012, Willow Garage, Inc. * 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 the Willow Garage 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. *********************************************************************/ #pragma once #include namespace geometric_shapes { /** Get the number of dimensions of a particular shape */ template constexpr unsigned int solidPrimitiveDimCount(); template <> constexpr unsigned int solidPrimitiveDimCount() { return 1u; } template <> constexpr unsigned int solidPrimitiveDimCount() { return 3u; } template <> constexpr unsigned int solidPrimitiveDimCount() { return 2u; } template <> constexpr unsigned int solidPrimitiveDimCount() { return 2u; } // clang-format off template struct [[deprecated("Replace SolidPrimitiveDimCount::value with solidPrimitiveDimCount()")]] SolidPrimitiveDimCount // clang-format on { enum { value = solidPrimitiveDimCount() }; }; } // namespace geometric_shapes geometric_shapes-0.7.3/package.xml000066400000000000000000000030571405115746100171700ustar00rootroot00000000000000 geometric_shapes 0.7.3 Generic definitions of geometric shapes and bodies. Ioan Sucan Gil Jones Tyler Weaver Robert Haschke BSD http://ros.org/wiki/geometric_shapes catkin assimp-dev boost eigen eigen_stl_containers libconsole-bridge-dev libqhull octomap pkg-config random_numbers resource_retriever shape_msgs visualization_msgs assimp boost eigen eigen_stl_containers libconsole-bridge-dev libqhull octomap random_numbers resource_retriever shape_msgs visualization_msgs rosunit geometric_shapes-0.7.3/src/000077500000000000000000000000001405115746100156355ustar00rootroot00000000000000geometric_shapes-0.7.3/src/aabb.cpp000066400000000000000000000056031405115746100172320ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2019, Open Robotics * 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 the Open Robotics 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. *********************************************************************/ /* Author: Martin Pecka */ #include void bodies::AABB::extendWithTransformedBox(const Eigen::Isometry3d& transform, const Eigen::Vector3d& box) { // 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. // // Here's a nice explanation why it works: https://zeuxcg.org/2010/10/17/aabb-from-obb-with-component-wise-abs/ const Eigen::Matrix3d& r = transform.rotation(); const Eigen::Vector3d& t = transform.translation(); double x_range = 0.5 * (fabs(r(0, 0) * box[0]) + fabs(r(0, 1) * box[1]) + fabs(r(0, 2) * box[2])); double y_range = 0.5 * (fabs(r(1, 0) * box[0]) + fabs(r(1, 1) * box[1]) + fabs(r(1, 2) * box[2])); double z_range = 0.5 * (fabs(r(2, 0) * box[0]) + fabs(r(2, 1) * box[1]) + fabs(r(2, 2) * box[2])); const Eigen::Vector3d v_delta(x_range, y_range, z_range); extend(t + v_delta); extend(t - v_delta); } geometric_shapes-0.7.3/src/bodies.cpp000066400000000000000000001253211405115746100176120ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * 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 the Willow Garage 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. *********************************************************************/ /* Author: Ioan Sucan */ #include "geometric_shapes/bodies.h" #include "geometric_shapes/body_operations.h" #include "geometric_shapes/check_isometry.h" #include extern "C" { #include } #include #include #include #include // std::fmin, std::fmax #include #include #include namespace bodies { namespace detail { static const double ZERO = 1e-9; /** \brief Compute the square of the distance between a ray and a point Note: this requires 'dir' to be normalized */ static inline double distanceSQR(const Eigen::Vector3d& p, const Eigen::Vector3d& origin, const Eigen::Vector3d& dir) { Eigen::Vector3d a = p - origin; double d = dir.normalized().dot(a); return a.squaredNorm() - d * d; } // temp structure for intersection points (used for ordering them) struct intersc { intersc(const Eigen::Vector3d& _pt, const double _tm) : pt(_pt), time(_tm) { } Eigen::Vector3d pt; double time; EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; // define order on intersection points struct interscOrder { bool operator()(const intersc& a, const intersc& b) const { return a.time < b.time; } }; /** * \brief Take intersections points in ipts and add them to intersections, filtering duplicates. * \param ipts The source list of intersections (will be modified (sorted)). * \param intersections The output list of intersection points. * \param count The maximum count of returned intersection points. 0 = return all points. */ void filterIntersections(std::vector& ipts, EigenSTL::vector_Vector3d* intersections, const size_t count) { if (intersections == nullptr || ipts.empty()) return; std::sort(ipts.begin(), ipts.end(), interscOrder()); const auto n = count > 0 ? std::min(count, ipts.size()) : ipts.size(); for (const auto& p : ipts) { if (intersections->size() == n) break; if (!intersections->empty() && p.pt.isApprox(intersections->back(), ZERO)) continue; intersections->push_back(p.pt); } } } // namespace detail inline Eigen::Vector3d normalize(const Eigen::Vector3d& dir) { const double norm = dir.squaredNorm(); #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) ? (dir / sqrt(norm)) : dir; #endif } } // namespace bodies bool bodies::Body::samplePointInside(random_numbers::RandomNumberGenerator& rng, unsigned int max_attempts, Eigen::Vector3d& result) const { BoundingSphere bs; computeBoundingSphere(bs); for (unsigned int i = 0; i < max_attempts; ++i) { result = Eigen::Vector3d(rng.uniformReal(bs.center.x() - bs.radius, bs.center.x() + bs.radius), rng.uniformReal(bs.center.y() - bs.radius, bs.center.y() + bs.radius), rng.uniformReal(bs.center.z() - bs.radius, bs.center.z() + bs.radius)); if (containsPoint(result)) return true; } return false; } bool bodies::Sphere::containsPoint(const Eigen::Vector3d& p, bool /* verbose */) const { return (center_ - p).squaredNorm() <= radius2_; } void bodies::Sphere::useDimensions(const shapes::Shape* shape) // radius { radius_ = static_cast(shape)->radius; } std::vector bodies::Sphere::getDimensions() const { return { radius_ }; } std::vector bodies::Sphere::getScaledDimensions() const { return { radiusU_ }; } void bodies::Sphere::updateInternalData() { const auto tmpRadiusU = radius_ * scale_ + padding_; if (tmpRadiusU < 0) throw std::runtime_error("Sphere radius must be non-negative."); radiusU_ = tmpRadiusU; radius2_ = radiusU_ * radiusU_; center_ = pose_.translation(); } std::shared_ptr bodies::Sphere::cloneAt(const Eigen::Isometry3d& pose, double padding, double scale) const { auto s = std::allocate_shared(Eigen::aligned_allocator()); s->radius_ = radius_; s->padding_ = padding; s->scale_ = scale; s->pose_ = pose; s->updateInternalData(); return s; } double bodies::Sphere::computeVolume() const { return 4.0 * boost::math::constants::pi() * radiusU_ * radiusU_ * radiusU_ / 3.0; } void bodies::Sphere::computeBoundingSphere(BoundingSphere& sphere) const { sphere.center = center_; sphere.radius = radiusU_; } void bodies::Sphere::computeBoundingCylinder(BoundingCylinder& cylinder) const { cylinder.pose = pose_; cylinder.radius = radiusU_; cylinder.length = 2.0 * radiusU_; } void bodies::Sphere::computeBoundingBox(bodies::AABB& bbox) const { bbox.setEmpty(); // it's a sphere, so we do not rotate the bounding box Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); transform.translation() = getPose().translation(); bbox.extendWithTransformedBox(transform, Eigen::Vector3d(2 * radiusU_, 2 * radiusU_, 2 * radiusU_)); } bool bodies::Sphere::samplePointInside(random_numbers::RandomNumberGenerator& rng, unsigned int max_attempts, Eigen::Vector3d& result) const { for (unsigned int i = 0; i < max_attempts; ++i) { const double minX = center_.x() - radiusU_; const double maxX = center_.x() + radiusU_; const double minY = center_.y() - radiusU_; const double maxY = center_.y() + radiusU_; const double minZ = center_.z() - radiusU_; const double maxZ = center_.z() + radiusU_; // we are sampling in a box; the probability of success after 20 attempts is 99.99996% given the ratio of box volume // to sphere volume for (int j = 0; j < 20; ++j) { result = Eigen::Vector3d(rng.uniformReal(minX, maxX), rng.uniformReal(minY, maxY), rng.uniformReal(minZ, maxZ)); if (containsPoint(result)) return true; } } return false; } bool bodies::Sphere::intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir, EigenSTL::vector_Vector3d* intersections, unsigned int count) const { // this is faster than always calling dir.normalized() in case the vector is already unit const Eigen::Vector3d dirNorm = normalize(dir); if (detail::distanceSQR(center_, origin, dirNorm) > radius2_) return false; bool result = false; Eigen::Vector3d cp = origin - center_; double dpcpv = cp.dot(dirNorm); Eigen::Vector3d w = cp - dpcpv * dirNorm; Eigen::Vector3d Q = center_ + w; double x = radius2_ - w.squaredNorm(); if (fabs(x) < detail::ZERO) { w = Q - origin; double dpQv = w.dot(dirNorm); if (dpQv > detail::ZERO) { if (intersections) intersections->push_back(Q); result = true; } } else if (x > 0.0) { x = sqrt(x); w = dirNorm * x; Eigen::Vector3d A = Q - w; Eigen::Vector3d B = Q + w; w = A - origin; double dpAv = w.dot(dirNorm); w = B - origin; double dpBv = w.dot(dirNorm); if (dpAv > detail::ZERO) { result = true; if (intersections) { intersections->push_back(A); if (count == 1) return result; } } if (dpBv > detail::ZERO) { result = true; if (intersections) intersections->push_back(B); } } 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_; double pH = v.dot(normalH_); if (fabs(pH) > length2_) return false; double pB1 = v.dot(normalB1_); double remaining = radius2_ - pB1 * pB1; if (remaining < 0.0) return false; else { double pB2 = v.dot(normalB2_); return pB2 * pB2 <= remaining; } } void bodies::Cylinder::useDimensions(const shapes::Shape* shape) // (length, radius) { length_ = static_cast(shape)->length; radius_ = static_cast(shape)->radius; } std::vector bodies::Cylinder::getDimensions() const { return { radius_, length_ }; } std::vector bodies::Cylinder::getScaledDimensions() const { return { radiusU_, 2 * length2_ }; } void bodies::Cylinder::updateInternalData() { const auto tmpRadiusU = radius_ * scale_ + padding_; if (tmpRadiusU < 0) throw std::runtime_error("Cylinder radius must be non-negative."); const auto tmpLength2 = scale_ * length_ / 2.0 + padding_; if (tmpLength2 < 0) throw std::runtime_error("Cylinder length must be non-negative."); radiusU_ = tmpRadiusU; length2_ = tmpLength2; radius2_ = radiusU_ * radiusU_; center_ = pose_.translation(); radiusBSqr_ = length2_ * length2_ + radius2_; radiusB_ = sqrt(radiusBSqr_); ASSERT_ISOMETRY(pose_); Eigen::Matrix3d basis = pose_.linear(); normalB1_ = basis.col(0); normalB2_ = basis.col(1); normalH_ = basis.col(2); double tmp = -normalH_.dot(center_); d1_ = tmp + length2_; d2_ = tmp - length2_; } bool bodies::Cylinder::samplePointInside(random_numbers::RandomNumberGenerator& rng, unsigned int /* max_attempts */, Eigen::Vector3d& result) const { // sample a point on the base disc of the cylinder double a = rng.uniformReal(-boost::math::constants::pi(), boost::math::constants::pi()); double r = rng.uniformReal(-radiusU_, radiusU_); double x = cos(a) * r; double y = sin(a) * r; // sample e height double z = rng.uniformReal(-length2_, length2_); result = pose_ * Eigen::Vector3d(x, y, z); return true; } std::shared_ptr bodies::Cylinder::cloneAt(const Eigen::Isometry3d& pose, double padding, double scale) const { auto c = std::allocate_shared(Eigen::aligned_allocator()); c->length_ = length_; c->radius_ = radius_; c->padding_ = padding; c->scale_ = scale; c->pose_ = pose; c->updateInternalData(); return c; } double bodies::Cylinder::computeVolume() const { return 2.0 * boost::math::constants::pi() * radius2_ * length2_; } void bodies::Cylinder::computeBoundingSphere(BoundingSphere& sphere) const { sphere.center = center_; sphere.radius = radiusB_; } void bodies::Cylinder::computeBoundingCylinder(BoundingCylinder& cylinder) const { cylinder.pose = pose_; cylinder.radius = radiusU_; cylinder.length = 2 * length2_; } void bodies::Cylinder::computeBoundingBox(bodies::AABB& bbox) const { bbox.setEmpty(); // method taken from http://www.iquilezles.org/www/articles/diskbbox/diskbbox.htm const auto a = normalH_; const auto e = radiusU_ * (Eigen::Vector3d::Ones() - a.cwiseProduct(a) / a.dot(a)).cwiseSqrt(); const auto pa = center_ + length2_ * normalH_; const auto pb = center_ - length2_ * normalH_; bbox.extend(pa - e); bbox.extend(pa + e); bbox.extend(pb - e); bbox.extend(pb + e); } bool bodies::Cylinder::intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir, EigenSTL::vector_Vector3d* intersections, unsigned int count) const { // this is faster than always calling dir.normalized() in case the vector is already unit const Eigen::Vector3d dirNorm = normalize(dir); if (detail::distanceSQR(center_, origin, dirNorm) > radiusBSqr_) return false; std::vector ipts; // intersect bases double tmp = normalH_.dot(dirNorm); if (fabs(tmp) > detail::ZERO) { double tmp2 = -normalH_.dot(origin); double t1 = (tmp2 - d1_) / tmp; if (t1 > 0.0) { Eigen::Vector3d p1(origin + dirNorm * t1); Eigen::Vector3d v1(p1 - center_); v1 = v1 - normalH_.dot(v1) * normalH_; if (v1.squaredNorm() < radius2_ + detail::ZERO) { if (intersections == nullptr) return true; detail::intersc ip(p1, t1); ipts.push_back(ip); } } double t2 = (tmp2 - d2_) / tmp; if (t2 > 0.0) { Eigen::Vector3d p2(origin + dirNorm * t2); Eigen::Vector3d v2(p2 - center_); v2 = v2 - normalH_.dot(v2) * normalH_; if (v2.squaredNorm() < radius2_ + detail::ZERO) { if (intersections == nullptr) return true; detail::intersc ip(p2, t2); ipts.push_back(ip); } } } if (ipts.size() < 2) { // intersect with infinite cylinder Eigen::Vector3d VD(normalH_.cross(dirNorm)); Eigen::Vector3d ROD(normalH_.cross(origin - center_)); double a = VD.squaredNorm(); double b = 2.0 * ROD.dot(VD); double c = ROD.squaredNorm() - radius2_; double d = b * b - 4.0 * a * c; if (d >= 0.0 && fabs(a) > detail::ZERO) { d = sqrt(d); double e = -a * 2.0; double t1 = (b + d) / e; double t2 = (b - d) / e; if (t1 > 0.0) { Eigen::Vector3d p1(origin + dirNorm * t1); Eigen::Vector3d v1(center_ - p1); if (fabs(normalH_.dot(v1)) < length2_ + detail::ZERO) { if (intersections == nullptr) return true; detail::intersc ip(p1, t1); ipts.push_back(ip); } } if (t2 > 0.0) { Eigen::Vector3d p2(origin + dirNorm * t2); Eigen::Vector3d v2(center_ - p2); if (fabs(normalH_.dot(v2)) < length2_ + detail::ZERO) { if (intersections == nullptr) return true; detail::intersc ip(p2, t2); ipts.push_back(ip); } } } } if (ipts.empty()) return false; // If a ray hits exactly the boundary between a side and a base, it is reported twice. // We want to only return the intersection once, thus we need to filter them. detail::filterIntersections(ipts, intersections, count); 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 { result = pose_ * Eigen::Vector3d(rng.uniformReal(-length2_, length2_), rng.uniformReal(-width2_, width2_), rng.uniformReal(-height2_, height2_)); return true; } bool bodies::Box::containsPoint(const Eigen::Vector3d& p, bool /* verbose */) const { const Eigen::Vector3d aligned = (invRot_ * (p - center_)).cwiseAbs(); return aligned[0] <= length2_ && aligned[1] <= width2_ && aligned[2] <= height2_; } void bodies::Box::useDimensions(const shapes::Shape* shape) // (x, y, z) = (length, width, height) { const double* size = static_cast(shape)->size; length_ = size[0]; width_ = size[1]; height_ = size[2]; } std::vector bodies::Box::getDimensions() const { return { length_, width_, height_ }; } std::vector bodies::Box::getScaledDimensions() const { return { 2 * length2_, 2 * width2_, 2 * height2_ }; } void bodies::Box::updateInternalData() { double s2 = scale_ / 2.0; const auto tmpLength2 = length_ * s2 + padding_; const auto tmpWidth2 = width_ * s2 + padding_; const auto tmpHeight2 = height_ * s2 + padding_; if (tmpLength2 < 0 || tmpWidth2 < 0 || tmpHeight2 < 0) throw std::runtime_error("Box dimensions must be non-negative."); length2_ = tmpLength2; width2_ = tmpWidth2; height2_ = tmpHeight2; center_ = pose_.translation(); radius2_ = length2_ * length2_ + width2_ * width2_ + height2_ * height2_; radiusB_ = sqrt(radius2_); ASSERT_ISOMETRY(pose_); invRot_ = pose_.linear().transpose(); // rotation is intentionally not applied, the corners are used in intersectsRay() const Eigen::Vector3d tmp(length2_, width2_, height2_); minCorner_ = center_ - tmp; maxCorner_ = center_ + tmp; } std::shared_ptr bodies::Box::cloneAt(const Eigen::Isometry3d& pose, double padding, double scale) const { auto b = std::allocate_shared(Eigen::aligned_allocator()); b->length_ = length_; b->width_ = width_; b->height_ = height_; b->padding_ = padding; b->scale_ = scale; b->pose_ = pose; b->updateInternalData(); return b; } double bodies::Box::computeVolume() const { return 8.0 * length2_ * width2_ * height2_; } void bodies::Box::computeBoundingSphere(BoundingSphere& sphere) const { sphere.center = center_; sphere.radius = radiusB_; } void bodies::Box::computeBoundingCylinder(BoundingCylinder& cylinder) const { double a, b; if (length2_ > width2_ && length2_ > height2_) { cylinder.length = length2_ * 2.0; a = width2_; b = height2_; Eigen::Isometry3d rot(Eigen::AngleAxisd(90.0f * (M_PI / 180.0f), Eigen::Vector3d::UnitY())); cylinder.pose = pose_ * rot; } else if (width2_ > height2_) { cylinder.length = width2_ * 2.0; a = height2_; b = length2_; cylinder.radius = sqrt(height2_ * height2_ + length2_ * length2_); Eigen::Isometry3d rot(Eigen::AngleAxisd(90.0f * (M_PI / 180.0f), Eigen::Vector3d::UnitX())); cylinder.pose = pose_ * rot; } else { cylinder.length = height2_ * 2.0; a = width2_; b = length2_; cylinder.pose = pose_; } cylinder.radius = sqrt(a * a + b * b); } void bodies::Box::computeBoundingBox(bodies::AABB& bbox) const { bbox.setEmpty(); bbox.extendWithTransformedBox(getPose(), 2 * Eigen::Vector3d(length2_, width2_, height2_)); } bool bodies::Box::intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir, EigenSTL::vector_Vector3d* intersections, unsigned int count) const { // this is faster than always calling dir.normalized() in case the vector is already unit const Eigen::Vector3d dirNorm = normalize(dir); // 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. 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 = (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 (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) std::swap(tmpTmin[i], tmpTmax[i]); } // tmin and tmax are such values of t in "p = o + t * d" in which the line intersects the box faces. // The box is viewed projected from all three directions, values of t are computed for each of the projections, // and a final constraint on tmin and tmax is updated by each of these projections. If tmin > tmax, there is no // intersection between the line and the box. double tmin, tmax; // use fmax/fmin to handle NaNs which can sneak in when dividing by d in tmpTmin and tmpTmax tmin = std::fmax(tmpTmin.x(), std::fmax(tmpTmin.y(), tmpTmin.z())); tmax = std::fmin(tmpTmax.x(), std::fmin(tmpTmax.y(), tmpTmax.z())); // tmin > tmax, there is no intersection between the line and the box if (tmax - tmin < -detail::ZERO) return false; // As we're doing intersections with a ray and not a line, cases where tmax is negative mean that the intersection is // with the opposite ray and not the one we are working with. if (tmax < 0) return false; if (intersections) { if (tmax - tmin > detail::ZERO) { // tmax > tmin, we have two distinct intersection points if (tmin > detail::ZERO) { // tmin > 0, both intersections lie on the ray intersections->push_back(tmin * dirNorm + origin); if (count == 0 || count > 1) intersections->push_back(tmax * dirNorm + origin); } else { // tmin <= 0 && tmax >= 0, the first intersection point is on the opposite ray and the second one on the correct // ray - this means origin of the ray lies inside the box and we should only report one intersection. intersections->push_back(tmax * dirNorm + origin); } } else { // tmax == tmin, there is exactly one intersection at a corner or edge intersections->push_back(tmax * dirNorm + origin); } } 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_) return false; if (bounding_box_.containsPoint(p)) { // Transform the point to the "base space" of this mesh Eigen::Vector3d ip(i_pose_ * p); return isPointInsidePlanes(ip); } else return false; } void bodies::ConvexMesh::correctVertexOrderFromPlanes() { for (unsigned int i = 0; i < mesh_data_->triangles_.size(); i += 3) { Eigen::Vector3d d1 = mesh_data_->vertices_[mesh_data_->triangles_[i]] - mesh_data_->vertices_[mesh_data_->triangles_[i + 1]]; Eigen::Vector3d d2 = mesh_data_->vertices_[mesh_data_->triangles_[i]] - mesh_data_->vertices_[mesh_data_->triangles_[i + 2]]; // expected computed normal from triangle vertex order Eigen::Vector3d tri_normal = d1.cross(d2); tri_normal.normalize(); // actual plane normal Eigen::Vector3d normal(mesh_data_->planes_[mesh_data_->plane_for_triangle_[i / 3]].x(), mesh_data_->planes_[mesh_data_->plane_for_triangle_[i / 3]].y(), mesh_data_->planes_[mesh_data_->plane_for_triangle_[i / 3]].z()); bool same_dir = tri_normal.dot(normal) > 0; if (!same_dir) { std::swap(mesh_data_->triangles_[i], mesh_data_->triangles_[i + 1]); } } } void bodies::ConvexMesh::useDimensions(const shapes::Shape* shape) { mesh_data_ = std::allocate_shared(Eigen::aligned_allocator()); const shapes::Mesh* mesh = static_cast(shape); double maxX = -std::numeric_limits::infinity(), maxY = -std::numeric_limits::infinity(), maxZ = -std::numeric_limits::infinity(); double minX = std::numeric_limits::infinity(), minY = std::numeric_limits::infinity(), minZ = std::numeric_limits::infinity(); for (unsigned int i = 0; i < mesh->vertex_count; ++i) { double vx = mesh->vertices[3 * i]; double vy = mesh->vertices[3 * i + 1]; double vz = mesh->vertices[3 * i + 2]; if (maxX < vx) maxX = vx; if (maxY < vy) maxY = vy; if (maxZ < vz) maxZ = vz; if (minX > vx) minX = vx; if (minY > vy) minY = vy; if (minZ > vz) minZ = vz; } if (maxX < minX) maxX = minX = 0.0; if (maxY < minY) maxY = minY = 0.0; if (maxZ < minZ) maxZ = minZ = 0.0; mesh_data_->box_size_ = Eigen::Vector3d(maxX - minX, maxY - minY, maxZ - minZ); mesh_data_->box_offset_ = Eigen::Vector3d((minX + maxX) / 2.0, (minY + maxY) / 2.0, (minZ + maxZ) / 2.0); mesh_data_->planes_.clear(); mesh_data_->triangles_.clear(); mesh_data_->vertices_.clear(); mesh_data_->mesh_radiusB_ = 0.0; mesh_data_->mesh_center_ = Eigen::Vector3d(); double xdim = maxX - minX; double ydim = maxY - minY; double zdim = maxZ - minZ; double pose1; double pose2; unsigned int off1; unsigned int off2; /* compute bounding cylinder */ double cyl_length; double maxdist = -std::numeric_limits::infinity(); if (xdim > ydim && xdim > zdim) { off1 = 1; off2 = 2; pose1 = mesh_data_->box_offset_.y(); pose2 = mesh_data_->box_offset_.z(); cyl_length = xdim; } else if (ydim > zdim) { off1 = 0; off2 = 2; pose1 = mesh_data_->box_offset_.x(); pose2 = mesh_data_->box_offset_.z(); cyl_length = ydim; } else { off1 = 0; off2 = 1; pose1 = mesh_data_->box_offset_.x(); pose2 = mesh_data_->box_offset_.y(); cyl_length = zdim; } /* compute convex hull */ coordT* points = (coordT*)calloc(mesh->vertex_count * 3, sizeof(coordT)); for (unsigned int i = 0; i < mesh->vertex_count; ++i) { points[3 * i + 0] = (coordT)mesh->vertices[3 * i + 0]; points[3 * i + 1] = (coordT)mesh->vertices[3 * i + 1]; points[3 * i + 2] = (coordT)mesh->vertices[3 * i + 2]; double dista = mesh->vertices[3 * i + off1] - pose1; double distb = mesh->vertices[3 * i + off2] - pose2; double dist = sqrt(((dista * dista) + (distb * distb))); if (dist > maxdist) maxdist = dist; } mesh_data_->bounding_cylinder_.radius = maxdist; mesh_data_->bounding_cylinder_.length = cyl_length; static FILE* null = fopen("/dev/null", "w"); char flags[] = "qhull Tv Qt"; 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, !qh_ALL); int curlong, totlong; qh_memfreeshort(qh, &curlong, &totlong); return; } int num_facets = qh->num_facets; int num_vertices = qh->num_vertices; mesh_data_->vertices_.reserve(num_vertices); Eigen::Vector3d sum(0, 0, 0); // necessary for FORALLvertices std::map qhull_vertex_table; vertexT* vertex; FORALLvertices { Eigen::Vector3d vert(vertex->point[0], vertex->point[1], vertex->point[2]); qhull_vertex_table[vertex->id] = mesh_data_->vertices_.size(); sum += vert; mesh_data_->vertices_.push_back(vert); } mesh_data_->mesh_center_ = sum / (double)(num_vertices); for (unsigned int j = 0; j < mesh_data_->vertices_.size(); ++j) { double dist = (mesh_data_->vertices_[j] - mesh_data_->mesh_center_).squaredNorm(); if (dist > mesh_data_->mesh_radiusB_) mesh_data_->mesh_radiusB_ = dist; } mesh_data_->mesh_radiusB_ = sqrt(mesh_data_->mesh_radiusB_); mesh_data_->triangles_.reserve(num_facets); // neccessary for qhull macro facetT* facet; FORALLfacets { Eigen::Vector4d planeEquation(facet->normal[0], facet->normal[1], facet->normal[2], facet->offset); if (!mesh_data_->planes_.empty()) { // filter equal planes - assuming same ones follow each other if ((planeEquation - mesh_data_->planes_.back()).cwiseAbs().maxCoeff() > 1e-6) // max diff to last mesh_data_->planes_.push_back(planeEquation); } else { mesh_data_->planes_.push_back(planeEquation); } // Needed by FOREACHvertex_i_ int vertex_n, vertex_i; 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; mesh_data_->triangle_for_plane_[mesh_data_->planes_.size() - 1] = (mesh_data_->triangles_.size() - 1) / 3; } qh_freeqhull(qh, !qh_ALL); int curlong, totlong; qh_memfreeshort(qh, &curlong, &totlong); } std::vector bodies::ConvexMesh::getDimensions() const { return {}; } std::vector bodies::ConvexMesh::getScaledDimensions() const { return {}; } void bodies::ConvexMesh::computeScaledVerticesFromPlaneProjections() { // compute the scaled vertices, if needed if (padding_ == 0.0 && scale_ == 1.0) { scaled_vertices_ = &mesh_data_->vertices_; return; } if (!scaled_vertices_storage_) scaled_vertices_storage_.reset(new EigenSTL::vector_Vector3d()); scaled_vertices_ = scaled_vertices_storage_.get(); scaled_vertices_storage_->resize(mesh_data_->vertices_.size()); // project vertices along the vertex - center line to the scaled and padded plane // take the average of all tri's planes around that vertex as the result // is not unique // First figure out, which tris are connected to each vertex std::map> vertex_to_tris; for (unsigned int i = 0; i < mesh_data_->triangles_.size() / 3; ++i) { vertex_to_tris[mesh_data_->triangles_[3 * i + 0]].push_back(i); vertex_to_tris[mesh_data_->triangles_[3 * i + 1]].push_back(i); vertex_to_tris[mesh_data_->triangles_[3 * i + 2]].push_back(i); } for (unsigned int i = 0; i < mesh_data_->vertices_.size(); ++i) { Eigen::Vector3d v(mesh_data_->vertices_[i] - mesh_data_->mesh_center_); EigenSTL::vector_Vector3d projected_vertices; for (unsigned int t : vertex_to_tris[i]) { const Eigen::Vector4d& plane = mesh_data_->planes_[mesh_data_->plane_for_triangle_[t]]; Eigen::Vector3d plane_normal(plane.x(), plane.y(), plane.z()); double d_scaled_padded = scale_ * plane.w() - (1 - scale_) * mesh_data_->mesh_center_.dot(plane_normal) - padding_; // intersect vert - center with scaled/padded plane equation double denom = v.dot(plane_normal); if (fabs(denom) < 1e-3) continue; double lambda = (-mesh_data_->mesh_center_.dot(plane_normal) - d_scaled_padded) / denom; Eigen::Vector3d vert_on_plane = v * lambda + mesh_data_->mesh_center_; projected_vertices.push_back(vert_on_plane); } if (projected_vertices.empty()) { double l = v.norm(); scaled_vertices_storage_->at(i) = mesh_data_->mesh_center_ + v * (scale_ + (l > detail::ZERO ? padding_ / l : 0.0)); } else { Eigen::Vector3d sum(0, 0, 0); for (const Eigen::Vector3d& vertex : projected_vertices) { sum += vertex; } sum /= projected_vertices.size(); scaled_vertices_storage_->at(i) = sum; } } } void bodies::ConvexMesh::updateInternalData() { if (!mesh_data_) return; Eigen::Isometry3d pose = pose_; pose.translation() = Eigen::Vector3d(pose_ * mesh_data_->box_offset_); shapes::Box box_shape(mesh_data_->box_size_.x(), mesh_data_->box_size_.y(), mesh_data_->box_size_.z()); bounding_box_.setPoseDirty(pose); // The real effect of padding will most likely be smaller due to the mesh padding algorithm, but in "worst case" it // can inflate the primitive bounding box by the padding_ value. bounding_box_.setPaddingDirty(padding_); bounding_box_.setScaleDirty(scale_); bounding_box_.setDimensionsDirty(&box_shape); bounding_box_.updateInternalData(); i_pose_ = pose_.inverse(); center_ = pose_ * mesh_data_->mesh_center_; radiusB_ = mesh_data_->mesh_radiusB_ * scale_ + padding_; radiusBSqr_ = radiusB_ * radiusB_; // compute the scaled vertices, if needed if (padding_ == 0.0 && scale_ == 1.0) scaled_vertices_ = &mesh_data_->vertices_; else { if (!scaled_vertices_storage_) scaled_vertices_storage_.reset(new EigenSTL::vector_Vector3d()); scaled_vertices_ = scaled_vertices_storage_.get(); scaled_vertices_storage_->resize(mesh_data_->vertices_.size()); for (unsigned int i = 0; i < mesh_data_->vertices_.size(); ++i) { Eigen::Vector3d v(mesh_data_->vertices_[i] - mesh_data_->mesh_center_); double l = v.norm(); scaled_vertices_storage_->at(i) = mesh_data_->mesh_center_ + v * (scale_ + (l > detail::ZERO ? padding_ / l : 0.0)); } } } const std::vector& bodies::ConvexMesh::getTriangles() const { static const std::vector empty; return mesh_data_ ? mesh_data_->triangles_ : empty; } const EigenSTL::vector_Vector3d& bodies::ConvexMesh::getVertices() const { static const EigenSTL::vector_Vector3d empty; return mesh_data_ ? mesh_data_->vertices_ : empty; } const EigenSTL::vector_Vector3d& bodies::ConvexMesh::getScaledVertices() const { return scaled_vertices_ ? *scaled_vertices_ : getVertices(); } const EigenSTL::vector_Vector4d& bodies::ConvexMesh::getPlanes() const { static const EigenSTL::vector_Vector4d empty; return mesh_data_ ? mesh_data_->planes_ : empty; } std::shared_ptr bodies::ConvexMesh::cloneAt(const Eigen::Isometry3d& pose, double padding, double scale) const { auto m = std::allocate_shared(Eigen::aligned_allocator()); m->mesh_data_ = mesh_data_; m->padding_ = padding; m->scale_ = scale; m->pose_ = pose; m->updateInternalData(); return m; } void bodies::ConvexMesh::computeBoundingSphere(BoundingSphere& sphere) const { sphere.center = center_; sphere.radius = radiusB_; } void bodies::ConvexMesh::computeBoundingCylinder(BoundingCylinder& cylinder) const { // the padding contibution might be smaller in reality, but we want to get it right for the worst case cylinder.length = mesh_data_ ? mesh_data_->bounding_cylinder_.length * scale_ + 2 * padding_ : 0.0; cylinder.radius = mesh_data_ ? mesh_data_->bounding_cylinder_.radius * scale_ + padding_ : 0.0; // need to do rotation correctly to get pose, which bounding box does BoundingCylinder cyl; bounding_box_.computeBoundingCylinder(cyl); cylinder.pose = cyl.pose; } void bodies::ConvexMesh::computeBoundingBox(bodies::AABB& bbox) const { bbox.setEmpty(); bounding_box_.computeBoundingBox(bbox); } bool bodies::ConvexMesh::isPointInsidePlanes(const Eigen::Vector3d& point) const { unsigned int numplanes = mesh_data_->planes_.size(); for (unsigned int i = 0; i < numplanes; ++i) { const Eigen::Vector4d& plane = mesh_data_->planes_[i]; Eigen::Vector3d plane_vec(plane.x(), plane.y(), plane.z()); // 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 * 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) return false; } return true; } unsigned int bodies::ConvexMesh::countVerticesBehindPlane(const Eigen::Vector4f& planeNormal) const { unsigned int numvertices = mesh_data_->vertices_.size(); unsigned int result = 0; for (unsigned int i = 0; i < numvertices; ++i) { Eigen::Vector3d plane_vec(planeNormal.x(), planeNormal.y(), planeNormal.z()); double dist = plane_vec.dot(mesh_data_->vertices_[i]) + planeNormal.w() - 1e-6; if (dist > 0.0) result++; } return result; } double bodies::ConvexMesh::computeVolume() const { double volume = 0.0; if (mesh_data_) for (unsigned int i = 0; i < mesh_data_->triangles_.size() / 3; ++i) { const Eigen::Vector3d& v1 = mesh_data_->vertices_[mesh_data_->triangles_[3 * i + 0]]; const Eigen::Vector3d& v2 = mesh_data_->vertices_[mesh_data_->triangles_[3 * i + 1]]; const Eigen::Vector3d& v3 = mesh_data_->vertices_[mesh_data_->triangles_[3 * i + 2]]; volume += v1.x() * v2.y() * v3.z() + v2.x() * v3.y() * v1.z() + v3.x() * v1.y() * v2.z() - v1.x() * v3.y() * v2.z() - v2.x() * v1.y() * v3.z() - v3.x() * v2.y() * v1.z(); } return fabs(volume) / 6.0; } bool bodies::ConvexMesh::intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir, EigenSTL::vector_Vector3d* intersections, unsigned int count) const { // this is faster than always calling dir.normalized() in case the vector is already unit const Eigen::Vector3d dirNorm = normalize(dir); if (!mesh_data_) return false; if (detail::distanceSQR(center_, origin, dirNorm) > radiusBSqr_) return false; if (!bounding_box_.intersectsRay(origin, dirNorm)) return false; // transform the ray into the coordinate frame of the mesh Eigen::Vector3d orig(i_pose_ * origin); Eigen::Vector3d dr(i_pose_.linear() * dirNorm); std::vector ipts; bool result = false; // for each triangle const auto nt = mesh_data_->triangles_.size() / 3; for (size_t i = 0; i < nt; ++i) { Eigen::Vector3d vec(mesh_data_->planes_[mesh_data_->plane_for_triangle_[i]].x(), mesh_data_->planes_[mesh_data_->plane_for_triangle_[i]].y(), mesh_data_->planes_[mesh_data_->plane_for_triangle_[i]].z()); const double tmp = vec.dot(dr); if (fabs(tmp) > detail::ZERO) { // planes_[...].w() corresponds to the unscaled mesh, so we need to compute it ourselves const double w_scaled_padded = vec.dot(scaled_vertices_->at(mesh_data_->triangles_[3 * i])); const double t = -(vec.dot(orig) + w_scaled_padded) / tmp; if (t > 0.0) { const auto i3 = 3 * i; const auto v1 = mesh_data_->triangles_[i3 + 0]; const auto v2 = mesh_data_->triangles_[i3 + 1]; const auto v3 = mesh_data_->triangles_[i3 + 2]; const Eigen::Vector3d& a = scaled_vertices_->at(v1); const Eigen::Vector3d& b = scaled_vertices_->at(v2); const Eigen::Vector3d& c = scaled_vertices_->at(v3); Eigen::Vector3d cb(c - b); Eigen::Vector3d ab(a - b); // intersection of the plane defined by the triangle and the ray Eigen::Vector3d P(orig + dr * t); // check if it is inside the triangle Eigen::Vector3d pb(P - b); Eigen::Vector3d c1(cb.cross(pb)); Eigen::Vector3d c2(cb.cross(ab)); if (c1.dot(c2) < 0.0) continue; Eigen::Vector3d ca(c - a); Eigen::Vector3d pa(P - a); Eigen::Vector3d ba(-ab); c1 = ca.cross(pa); c2 = ca.cross(ba); if (c1.dot(c2) < 0.0) continue; c1 = ba.cross(pa); c2 = ba.cross(ca); if (c1.dot(c2) < 0.0) continue; result = true; if (intersections) { detail::intersc ip(origin + dirNorm * t, t); ipts.push_back(ip); } else break; } } } if (result && intersections) { // If a ray hits exactly the boundary between two triangles, it is reported twice; // We only want return the intersection once; thus we need to filter them. detail::filterIntersections(ipts, intersections, count); } return result; } bodies::BodyVector::BodyVector() { } bodies::BodyVector::BodyVector(const std::vector& shapes, const EigenSTL::vector_Isometry3d& poses, double padding) { for (unsigned int i = 0; i < shapes.size(); i++) addBody(shapes[i], poses[i], padding); } bodies::BodyVector::~BodyVector() { clear(); } void bodies::BodyVector::clear() { for (auto& body : bodies_) delete body; bodies_.clear(); } void bodies::BodyVector::addBody(Body* body) { bodies_.push_back(body); BoundingSphere sphere; body->computeBoundingSphere(sphere); } void bodies::BodyVector::addBody(const shapes::Shape* shape, const Eigen::Isometry3d& pose, double padding) { bodies::Body* body = bodies::createBodyFromShape(shape); body->setPoseDirty(pose); body->setPaddingDirty(padding); body->updateInternalData(); addBody(body); } std::size_t bodies::BodyVector::getCount() const { return bodies_.size(); } void bodies::BodyVector::setPose(unsigned int i, const Eigen::Isometry3d& pose) { if (i >= bodies_.size()) { CONSOLE_BRIDGE_logError("There is no body at index %u", i); return; } bodies_[i]->setPose(pose); } const bodies::Body* bodies::BodyVector::getBody(unsigned int i) const { if (i >= bodies_.size()) { CONSOLE_BRIDGE_logError("There is no body at index %u", i); return nullptr; } else return bodies_[i]; } bool bodies::BodyVector::containsPoint(const Eigen::Vector3d& p, std::size_t& index, bool verbose) const { for (std::size_t i = 0; i < bodies_.size(); ++i) if (bodies_[i]->containsPoint(p, verbose)) { index = i; return true; } return false; } bool bodies::BodyVector::containsPoint(const Eigen::Vector3d& p, bool verbose) const { std::size_t dummy; return containsPoint(p, dummy, verbose); } bool bodies::BodyVector::intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir, std::size_t& index, EigenSTL::vector_Vector3d* intersections, unsigned int count) const { for (std::size_t i = 0; i < bodies_.size(); ++i) if (bodies_[i]->intersectsRay(origin, dir, intersections, count)) { index = i; return true; } return false; } geometric_shapes-0.7.3/src/body_operations.cpp000066400000000000000000000211131405115746100215370ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * 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 the Willow Garage 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. *********************************************************************/ /** \author Ioan Sucan, E. Gil Jones */ #include #include #include #include #include bodies::Body* bodies::createEmptyBodyFromShapeType(const shapes::ShapeType& shapeType) { Body* body = nullptr; switch (shapeType) { case shapes::BOX: body = new bodies::Box(); break; case shapes::SPHERE: body = new bodies::Sphere(); break; case shapes::CYLINDER: body = new bodies::Cylinder(); break; case shapes::MESH: body = new bodies::ConvexMesh(); break; default: CONSOLE_BRIDGE_logError("Creating body from shape: Unknown shape type %d", (int)shapeType); break; } return body; } bodies::Body* bodies::createBodyFromShape(const shapes::Shape* shape) { Body* body = nullptr; if (shape) { body = createEmptyBodyFromShapeType(shape->type); body->setDimensions(shape); } return body; } shapes::ShapeConstPtr bodies::constructShapeFromBody(const bodies::Body* body) { shapes::ShapePtr result; switch (body->getType()) { case shapes::SPHERE: { const auto& dims = static_cast(body)->getScaledDimensions(); result.reset(new shapes::Sphere(dims[0])); break; } case shapes::BOX: { const auto& dims = static_cast(body)->getScaledDimensions(); result.reset(new shapes::Box(dims[0], dims[1], dims[2])); break; } case shapes::CYLINDER: { const auto& dims = static_cast(body)->getScaledDimensions(); result.reset(new shapes::Cylinder(dims[0], dims[1])); break; } case shapes::MESH: { const auto mesh = static_cast(body); const auto& scaledVertices = mesh->getScaledVertices(); // createMeshFromVertices requires an "expanded" list of triangles where each triangle is // represented by its three vertex positions EigenSTL::vector_Vector3d vertexList; vertexList.reserve(3 * mesh->getTriangles().size()); for (const auto& triangle : mesh->getTriangles()) vertexList.push_back(scaledVertices[triangle]); result.reset(shapes::createMeshFromVertices(vertexList)); break; } default: { CONSOLE_BRIDGE_logError("Unknown body type: %d", (int)body->getType()); break; } } return result; } void bodies::constructMarkerFromBody(const bodies::Body* body, visualization_msgs::Marker& msg) { auto shape = bodies::constructShapeFromBody(body); shapes::constructMarkerFromShape(shape.get(), msg, true); const auto& pose = body->getPose(); msg.pose.position.x = pose.translation().x(); msg.pose.position.y = pose.translation().y(); msg.pose.position.z = pose.translation().z(); ASSERT_ISOMETRY(pose); Eigen::Quaterniond quat(pose.linear().matrix()); msg.pose.orientation.x = quat.x(); msg.pose.orientation.y = quat.y(); msg.pose.orientation.z = quat.z(); msg.pose.orientation.w = quat.w(); } void bodies::mergeBoundingSpheres(const std::vector& spheres, BoundingSphere& mergedSphere) { if (spheres.empty()) { mergedSphere.center = Eigen::Vector3d(0.0, 0.0, 0.0); mergedSphere.radius = 0.0; } else { mergedSphere = spheres[0]; for (unsigned int i = 1; i < spheres.size(); ++i) { if (spheres[i].radius <= 0.0) continue; Eigen::Vector3d diff = spheres[i].center - mergedSphere.center; double d = diff.norm(); if (d + mergedSphere.radius <= spheres[i].radius) { mergedSphere.center = spheres[i].center; mergedSphere.radius = spheres[i].radius; } else if (d + spheres[i].radius > mergedSphere.radius) { Eigen::Vector3d delta = mergedSphere.center - spheres[i].center; mergedSphere.radius = (delta.norm() + spheres[i].radius + mergedSphere.radius) / 2.0; mergedSphere.center = delta.normalized() * (mergedSphere.radius - spheres[i].radius) + spheres[i].center; } } } } namespace bodies { template Body* constructBodyFromMsgHelper(const T& shape_msg, const geometry_msgs::Pose& pose) { shapes::ShapePtr shape(shapes::constructShapeFromMsg(shape_msg)); if (shape) { Body* body = createEmptyBodyFromShapeType(shape->type); if (body) { Eigen::Quaterniond q(pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z); if (fabs(q.squaredNorm() - 1.0) > 1e-3) { CONSOLE_BRIDGE_logError("Quaternion is not normalized. Assuming identity."); q = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0); } Eigen::Isometry3d af(Eigen::Translation3d(pose.position.x, pose.position.y, pose.position.z) * q); body->setPoseDirty(af); body->setDimensionsDirty(shape.get()); body->updateInternalData(); return body; } } return nullptr; } } // namespace bodies bodies::Body* bodies::constructBodyFromMsg(const shapes::ShapeMsg& shape_msg, const geometry_msgs::Pose& pose) { return constructBodyFromMsgHelper(shape_msg, pose); } bodies::Body* bodies::constructBodyFromMsg(const shape_msgs::Mesh& shape_msg, const geometry_msgs::Pose& pose) { return constructBodyFromMsgHelper(shape_msg, pose); } bodies::Body* bodies::constructBodyFromMsg(const shape_msgs::SolidPrimitive& shape_msg, const geometry_msgs::Pose& pose) { return constructBodyFromMsgHelper(shape_msg, pose); } void bodies::computeBoundingSphere(const std::vector& bodies, bodies::BoundingSphere& sphere) { Eigen::Vector3d sum(0.0, 0.0, 0.0); // TODO - expand to all body types unsigned int vertex_count = 0; for (auto body : bodies) { if (!body || body->getType() != shapes::MESH) continue; // MESH type implies bodies::ConvexMesh const bodies::ConvexMesh* conv = static_cast(body); for (unsigned int j = 0; j < conv->getScaledVertices().size(); j++, vertex_count++) { sum += conv->getPose() * conv->getScaledVertices()[j]; } } sphere.center = sum / (double)vertex_count; double max_dist_squared = 0.0; for (auto body : bodies) { if (!body || body->getType() != shapes::MESH) continue; // MESH type implies bodies::ConvexMesh const bodies::ConvexMesh* conv = static_cast(body); for (unsigned int j = 0; j < conv->getScaledVertices().size(); j++) { double dist = (conv->getPose() * conv->getScaledVertices()[j] - sphere.center).squaredNorm(); if (dist > max_dist_squared) { max_dist_squared = dist; } } } sphere.radius = sqrt(max_dist_squared); } void bodies::mergeBoundingBoxes(const std::vector& boxes, bodies::AABB& mergedBox) { for (const auto& box : boxes) mergedBox.extend(box); } geometric_shapes-0.7.3/src/mesh_operations.cpp000066400000000000000000000534041405115746100215460ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * 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 the Willow Garage 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. *********************************************************************/ /* Author: Ioan Sucan */ #include "geometric_shapes/mesh_operations.h" #include "geometric_shapes/shape_operations.h" #include #include #include #include #include #include #include #include #include #include #include #include namespace shapes { namespace detail { namespace { /// Local representation of a vertex that knows its position in an array (used for sorting) struct LocalVertexType { LocalVertexType() : x(0.0), y(0.0), z(0.0) { } LocalVertexType(const Eigen::Vector3d& v) : x(v.x()), y(v.y()), z(v.z()) { } double x, y, z; unsigned int index; }; /// Sorting operator by point value struct ltLocalVertexValue { bool operator()(const LocalVertexType& p1, const LocalVertexType& p2) const { if (p1.x < p2.x) return true; if (p1.x > p2.x) return false; if (p1.y < p2.y) return true; if (p1.y > p2.y) return false; if (p1.z < p2.z) return true; return false; } }; /// Sorting operator by point index struct ltLocalVertexIndex { bool operator()(const LocalVertexType& p1, const LocalVertexType& p2) const { return p1.index < p2.index; } }; } // namespace } // namespace detail Mesh* createMeshFromVertices(const EigenSTL::vector_Vector3d& vertices, const std::vector& triangles) { unsigned int nt = triangles.size() / 3; Mesh* mesh = new Mesh(vertices.size(), nt); for (unsigned int i = 0; i < vertices.size(); ++i) { mesh->vertices[3 * i] = vertices[i].x(); mesh->vertices[3 * i + 1] = vertices[i].y(); mesh->vertices[3 * i + 2] = vertices[i].z(); } std::copy(triangles.begin(), triangles.end(), mesh->triangles); mesh->computeTriangleNormals(); mesh->computeVertexNormals(); return mesh; } Mesh* createMeshFromVertices(const EigenSTL::vector_Vector3d& source) { if (source.size() < 3) return nullptr; if (source.size() % 3 != 0) CONSOLE_BRIDGE_logError("The number of vertices to construct a mesh from is not divisible by 3. Probably " "constructed triangles will not make sense."); std::set vertices; std::vector triangles; unsigned int n = source.size() / 3; for (unsigned int i = 0; i < n; ++i) { // check if we have new vertices unsigned int i3 = i * 3; detail::LocalVertexType vt1(source[i3]); std::set::iterator p1 = vertices.find(vt1); if (p1 == vertices.end()) { vt1.index = vertices.size(); vertices.insert(vt1); } else vt1.index = p1->index; triangles.push_back(vt1.index); detail::LocalVertexType vt2(source[++i3]); std::set::iterator p2 = vertices.find(vt2); if (p2 == vertices.end()) { vt2.index = vertices.size(); vertices.insert(vt2); } else vt2.index = p2->index; triangles.push_back(vt2.index); detail::LocalVertexType vt3(source[++i3]); std::set::iterator p3 = vertices.find(vt3); if (p3 == vertices.end()) { vt3.index = vertices.size(); vertices.insert(vt3); } else vt3.index = p3->index; triangles.push_back(vt3.index); } // sort our vertices std::vector vt; vt.insert(vt.end(), vertices.begin(), vertices.end()); std::sort(vt.begin(), vt.end(), detail::ltLocalVertexIndex()); // copy the data to a mesh structure unsigned int nt = triangles.size() / 3; Mesh* mesh = new Mesh(vt.size(), nt); for (unsigned int i = 0; i < vt.size(); ++i) { unsigned int i3 = i * 3; mesh->vertices[i3] = vt[i].x; mesh->vertices[i3 + 1] = vt[i].y; mesh->vertices[i3 + 2] = vt[i].z; } std::copy(triangles.begin(), triangles.end(), mesh->triangles); mesh->computeTriangleNormals(); mesh->computeVertexNormals(); return mesh; } Mesh* createMeshFromResource(const std::string& resource) { static const Eigen::Vector3d one(1.0, 1.0, 1.0); return createMeshFromResource(resource, one); } Mesh* createMeshFromBinary(const char* buffer, std::size_t size, const std::string& assimp_hint) { static const Eigen::Vector3d one(1.0, 1.0, 1.0); return createMeshFromBinary(buffer, size, one, assimp_hint); } Mesh* createMeshFromBinary(const char* buffer, std::size_t size, const Eigen::Vector3d& scale, const std::string& assimp_hint) { if (!buffer || size < 1) { CONSOLE_BRIDGE_logWarn("Cannot construct mesh from empty binary buffer"); return nullptr; } // try to get a file extension std::string hint; std::size_t pos = assimp_hint.find_last_of('.'); if (pos != std::string::npos) { hint = assimp_hint.substr(pos + 1); std::transform(hint.begin(), hint.end(), hint.begin(), ::tolower); } if (hint.empty()) hint = assimp_hint; // send entire file name as hint if no extension was found // Create an instance of the Importer class Assimp::Importer importer; // Issue #38 fix: as part of the post-processing, we remove all other components in file but // the meshes, as anyway the resulting shapes:Mesh object just receives vertices and triangles. importer.SetPropertyInteger(AI_CONFIG_PP_RVC_FLAGS, aiComponent_NORMALS | aiComponent_TANGENTS_AND_BITANGENTS | aiComponent_COLORS | aiComponent_TEXCOORDS | aiComponent_BONEWEIGHTS | aiComponent_ANIMATIONS | aiComponent_TEXTURES | aiComponent_LIGHTS | aiComponent_CAMERAS | aiComponent_MATERIALS); // And have it read the given file with some post-processing const aiScene* scene = importer.ReadFileFromMemory(reinterpret_cast(buffer), size, aiProcess_Triangulate | aiProcess_JoinIdenticalVertices | aiProcess_SortByPType | aiProcess_RemoveComponent, hint.c_str()); if (!scene) return nullptr; // Assimp enforces Y_UP convention by rotating models with different conventions. // However, that behaviour is confusing and doesn't match the ROS convention // where the Z axis is pointing up. // Hopefully this doesn't undo legit use of the root node transformation... // Note that this is also what RViz does internally. scene->mRootNode->mTransformation = aiMatrix4x4(); // These post processing steps flatten the root node transformation into child nodes, // so they must be delayed until after clearing the root node transform above. importer.ApplyPostProcessing(aiProcess_OptimizeMeshes | aiProcess_OptimizeGraph); return createMeshFromAsset(scene, scale, hint); } Mesh* createMeshFromResource(const std::string& resource, const Eigen::Vector3d& scale) { resource_retriever::Retriever retriever; resource_retriever::MemoryResource res; try { res = retriever.get(resource); } catch (resource_retriever::Exception& e) { CONSOLE_BRIDGE_logError("%s", e.what()); return nullptr; } if (res.size == 0) { CONSOLE_BRIDGE_logWarn("Retrieved empty mesh for resource '%s'", resource.c_str()); return nullptr; } Mesh* m = createMeshFromBinary(reinterpret_cast(res.data.get()), res.size, scale, resource); if (!m) { CONSOLE_BRIDGE_logWarn("Assimp reports no scene in %s.", resource.c_str()); CONSOLE_BRIDGE_logWarn("This could be because of a resource filename that is too long for the Assimp library, a " "known bug. As a workaround shorten the filename/path."); } return m; } namespace { void extractMeshData(const aiScene* scene, const aiNode* node, const aiMatrix4x4& parent_transform, const Eigen::Vector3d& scale, EigenSTL::vector_Vector3d& vertices, std::vector& triangles) { aiMatrix4x4 transform = parent_transform; transform *= node->mTransformation; for (unsigned int j = 0; j < node->mNumMeshes; ++j) { const aiMesh* a = scene->mMeshes[node->mMeshes[j]]; unsigned int offset = vertices.size(); for (unsigned int i = 0; i < a->mNumVertices; ++i) { aiVector3D v = transform * a->mVertices[i]; vertices.push_back(Eigen::Vector3d(v.x * scale.x(), v.y * scale.y(), v.z * scale.z())); } for (unsigned int i = 0; i < a->mNumFaces; ++i) if (a->mFaces[i].mNumIndices == 3) { triangles.push_back(offset + a->mFaces[i].mIndices[0]); triangles.push_back(offset + a->mFaces[i].mIndices[1]); triangles.push_back(offset + a->mFaces[i].mIndices[2]); } } for (unsigned int n = 0; n < node->mNumChildren; ++n) extractMeshData(scene, node->mChildren[n], transform, scale, vertices, triangles); } } // namespace Mesh* createMeshFromAsset(const aiScene* scene, const std::string& resource_name) { static const Eigen::Vector3d one(1.0, 1.0, 1.0); return createMeshFromAsset(scene, one, resource_name); } Mesh* createMeshFromAsset(const aiScene* scene, const Eigen::Vector3d& scale, const std::string& resource_name) { if (!scene->HasMeshes()) { CONSOLE_BRIDGE_logWarn("Assimp reports scene in %s has no meshes", resource_name.c_str()); return nullptr; } EigenSTL::vector_Vector3d vertices; std::vector triangles; extractMeshData(scene, scene->mRootNode, aiMatrix4x4(), scale, vertices, triangles); if (vertices.empty()) { CONSOLE_BRIDGE_logWarn("There are no vertices in the scene %s", resource_name.c_str()); return nullptr; } if (triangles.empty()) { CONSOLE_BRIDGE_logWarn("There are no triangles in the scene %s", resource_name.c_str()); return nullptr; } return createMeshFromVertices(vertices, triangles); } Mesh* createMeshFromShape(const Shape* shape) { if (shape->type == shapes::SPHERE) return shapes::createMeshFromShape(static_cast(*shape)); else if (shape->type == shapes::BOX) return shapes::createMeshFromShape(static_cast(*shape)); else if (shape->type == shapes::CYLINDER) return shapes::createMeshFromShape(static_cast(*shape)); else if (shape->type == shapes::CONE) return shapes::createMeshFromShape(static_cast(*shape)); else CONSOLE_BRIDGE_logError("Conversion of shape of type '%s' to a mesh is not known", shapeStringName(shape).c_str()); return nullptr; } Mesh* createMeshFromShape(const Box& box) { double x = box.size[0] / 2.0; double y = box.size[1] / 2.0; double z = box.size[2] / 2.0; // define vertices of box mesh Mesh* result = new Mesh(8, 12); result->vertices[0] = -x; result->vertices[1] = -y; result->vertices[2] = -z; result->vertices[3] = x; result->vertices[4] = -y; result->vertices[5] = -z; result->vertices[6] = x; result->vertices[7] = -y; result->vertices[8] = z; result->vertices[9] = -x; result->vertices[10] = -y; result->vertices[11] = z; result->vertices[12] = -x; result->vertices[13] = y; result->vertices[14] = z; result->vertices[15] = -x; result->vertices[16] = y; result->vertices[17] = -z; result->vertices[18] = x; result->vertices[19] = y; result->vertices[20] = z; result->vertices[21] = x; result->vertices[22] = y; result->vertices[23] = -z; static const unsigned int tri[] = { 0, 1, 2, 2, 3, 0, 4, 3, 2, 2, 6, 4, 7, 6, 2, 2, 1, 7, 3, 4, 5, 5, 0, 3, 0, 5, 7, 7, 1, 0, 7, 5, 4, 4, 6, 7 }; memcpy(result->triangles, tri, sizeof(unsigned int) * 36); result->computeTriangleNormals(); result->computeVertexNormals(); return result; } Mesh* createMeshFromShape(const Sphere& sphere) { // this code is adapted from FCL EigenSTL::vector_Vector3d vertices; std::vector triangles; const double r = sphere.radius; const double pi = boost::math::constants::pi(); const unsigned int seg = std::max(6, 0.5 + 2.0 * pi * r / 0.01); // split the sphere longitudinally up // to a resolution of 1 cm at the // ecuator, or a minimum of 6 segments const unsigned int ring = std::max(6, 2.0 * r / 0.01); // split the sphere into rings along latitude, // up to a height of at most 1 cm, or a minimum // of 6 rings double phi, phid; phid = pi * 2.0 / seg; phi = 0.0; double theta, thetad; thetad = pi / (ring + 1); theta = 0; for (unsigned int i = 0; i < ring; ++i) { double theta_ = theta + thetad * (i + 1); for (unsigned int j = 0; j < seg; ++j) vertices.push_back(Eigen::Vector3d(r * sin(theta_) * cos(phi + j * phid), r * sin(theta_) * sin(phi + j * phid), r * cos(theta_))); } vertices.push_back(Eigen::Vector3d(0.0, 0.0, r)); vertices.push_back(Eigen::Vector3d(0.0, 0.0, -r)); for (unsigned int i = 0; i < ring - 1; ++i) { for (unsigned int j = 0; j < seg; ++j) { unsigned int a, b, c, d; a = i * seg + j; b = (j == seg - 1) ? (i * seg) : (i * seg + j + 1); c = (i + 1) * seg + j; d = (j == seg - 1) ? ((i + 1) * seg) : ((i + 1) * seg + j + 1); triangles.push_back(a); triangles.push_back(c); triangles.push_back(b); triangles.push_back(b); triangles.push_back(c); triangles.push_back(d); } } for (unsigned int j = 0; j < seg; ++j) { unsigned int a, b; a = j; b = (j == seg - 1) ? 0 : (j + 1); triangles.push_back(ring * seg); triangles.push_back(a); triangles.push_back(b); a = (ring - 1) * seg + j; b = (j == seg - 1) ? (ring - 1) * seg : ((ring - 1) * seg + j + 1); triangles.push_back(a); triangles.push_back(ring * seg + 1); triangles.push_back(b); } return createMeshFromVertices(vertices, triangles); } Mesh* createMeshFromShape(const Cylinder& cylinder) { // this code is adapted from FCL EigenSTL::vector_Vector3d vertices; std::vector triangles; // magic number defining how many triangles to construct for the unit cylinder; perhaps this should be a param static unsigned int tot_for_unit_cylinder = 100; double r = cylinder.radius; double h = cylinder.length; const double pi = boost::math::constants::pi(); unsigned int tot = std::max(6, ceil(tot_for_unit_cylinder * r)); double phid = pi * 2 / tot; double circle_edge = phid * r; unsigned int h_num = ceil(std::abs(h) / circle_edge); double phi = 0; double hd = h / h_num; for (unsigned int i = 0; i < tot; ++i) vertices.push_back(Eigen::Vector3d(r * cos(phi + phid * i), r * sin(phi + phid * i), h / 2)); for (unsigned int i = 0; i < h_num - 1; ++i) for (unsigned int j = 0; j < tot; ++j) vertices.push_back(Eigen::Vector3d(r * cos(phi + phid * j), r * sin(phi + phid * j), h / 2 - (i + 1) * hd)); for (unsigned int i = 0; i < tot; ++i) vertices.push_back(Eigen::Vector3d(r * cos(phi + phid * i), r * sin(phi + phid * i), -h / 2)); vertices.push_back(Eigen::Vector3d(0, 0, h / 2)); vertices.push_back(Eigen::Vector3d(0, 0, -h / 2)); for (unsigned int i = 0; i < tot; ++i) { triangles.push_back((h_num + 1) * tot); triangles.push_back(i); triangles.push_back((i == tot - 1) ? 0 : (i + 1)); } for (unsigned int i = 0; i < tot; ++i) { triangles.push_back((h_num + 1) * tot + 1); triangles.push_back(h_num * tot + ((i == tot - 1) ? 0 : (i + 1))); triangles.push_back(h_num * tot + i); } for (unsigned int i = 0; i < h_num; ++i) { for (unsigned int j = 0; j < tot; ++j) { int a, b, c, d; a = j; b = (j == tot - 1) ? 0 : (j + 1); c = j + tot; d = (j == tot - 1) ? tot : (j + 1 + tot); int start = i * tot; triangles.push_back(start + b); triangles.push_back(start + a); triangles.push_back(start + c); triangles.push_back(start + b); triangles.push_back(start + c); triangles.push_back(start + d); } } return createMeshFromVertices(vertices, triangles); } Mesh* createMeshFromShape(const Cone& cone) { // this code is adapted from FCL EigenSTL::vector_Vector3d vertices; std::vector triangles; // magic number defining how many triangles to construct for the unit cylinder; perhaps this should be a param static unsigned int tot_for_unit_cone = 100; double r = cone.radius; double h = cone.length; const double pi = boost::math::constants::pi(); unsigned int tot = tot_for_unit_cone * r; double phid = pi * 2 / tot; double circle_edge = phid * r; unsigned int h_num = ceil(h / circle_edge); double phi = 0; double hd = h / h_num; for (unsigned int i = 0; i < h_num - 1; ++i) { double h_i = h / 2 - (i + 1) * hd; double rh = r * (0.5 - h_i / h); for (unsigned int j = 0; j < tot; ++j) vertices.push_back(Eigen::Vector3d(rh * cos(phi + phid * j), rh * sin(phi + phid * j), h_i)); } for (unsigned int i = 0; i < tot; ++i) vertices.push_back(Eigen::Vector3d(r * cos(phi + phid * i), r * sin(phi + phid * i), -h / 2)); vertices.push_back(Eigen::Vector3d(0, 0, h / 2)); vertices.push_back(Eigen::Vector3d(0, 0, -h / 2)); for (unsigned int i = 0; i < tot; ++i) { triangles.push_back(h_num * tot); triangles.push_back(i); triangles.push_back((i == tot - 1) ? 0 : (i + 1)); } for (unsigned int i = 0; i < tot; ++i) { triangles.push_back(h_num * tot + 1); triangles.push_back((h_num - 1) * tot + ((i == tot - 1) ? 0 : (i + 1))); triangles.push_back((h_num - 1) * tot + i); } for (unsigned int i = 0; i < h_num - 1; ++i) for (unsigned int j = 0; j < tot; ++j) { int a, b, c, d; a = j; b = (j == tot - 1) ? 0 : (j + 1); c = j + tot; d = (j == tot - 1) ? tot : (j + 1 + tot); int start = i * tot; triangles.push_back(start + b); triangles.push_back(start + a); triangles.push_back(start + c); triangles.push_back(start + b); triangles.push_back(start + c); triangles.push_back(start + d); } return createMeshFromVertices(vertices, triangles); } namespace { inline void writeFloatToSTL(char*& ptr, const float data) { memcpy(ptr, &data, sizeof(float)); ptr += sizeof(float); } } // namespace void writeSTLBinary(const Mesh* mesh, std::vector& buffer) { buffer.resize(84 + mesh->triangle_count * 50); memset(&buffer[0], 0, 80); char* ptr = &buffer[80]; uint32_t nt = mesh->triangle_count; memcpy(ptr, &nt, sizeof(uint32_t)); ptr += sizeof(uint32_t); for (unsigned int i = 0; i < mesh->triangle_count; ++i) { unsigned int i3 = i * 3; if (mesh->triangle_normals) { writeFloatToSTL(ptr, mesh->triangle_normals[i3]); writeFloatToSTL(ptr, mesh->triangle_normals[i3 + 1]); writeFloatToSTL(ptr, mesh->triangle_normals[i3 + 2]); } else { memset(ptr, 0, sizeof(float) * 3); ptr += sizeof(float) * 3; } unsigned int index = mesh->triangles[i3] * 3; writeFloatToSTL(ptr, mesh->vertices[index]); writeFloatToSTL(ptr, mesh->vertices[index + 1]); writeFloatToSTL(ptr, mesh->vertices[index + 2]); index = mesh->triangles[i3 + 1] * 3; writeFloatToSTL(ptr, mesh->vertices[index]); writeFloatToSTL(ptr, mesh->vertices[index + 1]); writeFloatToSTL(ptr, mesh->vertices[index + 2]); index = mesh->triangles[i3 + 2] * 3; writeFloatToSTL(ptr, mesh->vertices[index]); writeFloatToSTL(ptr, mesh->vertices[index + 1]); writeFloatToSTL(ptr, mesh->vertices[index + 2]); memset(ptr, 0, 2); ptr += 2; } } } // namespace shapes geometric_shapes-0.7.3/src/shape_extents.cpp000066400000000000000000000110641405115746100212150ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2012, Willow Garage, Inc. * 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 the Willow Garage 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 #include #include void geometric_shapes::getShapeExtents(const shape_msgs::SolidPrimitive& shape_msg, double& x_extent, double& y_extent, double& z_extent) { x_extent = y_extent = z_extent = 0.0; if (shape_msg.type == shape_msgs::SolidPrimitive::SPHERE) { 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()) { x_extent = shape_msg.dimensions[shape_msgs::SolidPrimitive::BOX_X]; y_extent = shape_msg.dimensions[shape_msgs::SolidPrimitive::BOX_Y]; z_extent = 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()) { x_extent = y_extent = shape_msg.dimensions[shape_msgs::SolidPrimitive::CYLINDER_RADIUS] * 2.0; z_extent = 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()) { x_extent = y_extent = shape_msg.dimensions[shape_msgs::SolidPrimitive::CONE_RADIUS] * 2.0; z_extent = shape_msg.dimensions[shape_msgs::SolidPrimitive::CONE_HEIGHT]; } } } void geometric_shapes::getShapeExtents(const shape_msgs::Mesh& shape_msg, double& x_extent, double& y_extent, double& z_extent) { x_extent = y_extent = z_extent = 0.0; if (!shape_msg.vertices.empty()) { double xmin = std::numeric_limits::max(), ymin = std::numeric_limits::max(), zmin = std::numeric_limits::max(); double xmax = -std::numeric_limits::max(), ymax = -std::numeric_limits::max(), zmax = -std::numeric_limits::max(); for (const geometry_msgs::Point& vertex : shape_msg.vertices) { if (vertex.x > xmax) xmax = vertex.x; if (vertex.x < xmin) xmin = vertex.x; if (vertex.y > ymax) ymax = vertex.y; if (vertex.y < ymin) ymin = vertex.y; if (vertex.z > zmax) zmax = vertex.z; if (vertex.z < zmin) zmin = vertex.z; } x_extent = xmax - xmin; y_extent = ymax - ymin; z_extent = zmax - zmin; } } geometric_shapes-0.7.3/src/shape_operations.cpp000066400000000000000000000441571405115746100217170ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * 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 the Willow Garage 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. *********************************************************************/ /* Author: Ioan Sucan */ #include "geometric_shapes/shape_operations.h" #include #include #include #include #include #include #include #include #include #include namespace shapes { Shape* constructShapeFromMsg(const shape_msgs::Plane& shape_msg) { return new Plane(shape_msg.coef[0], shape_msg.coef[1], shape_msg.coef[2], shape_msg.coef[3]); } Shape* constructShapeFromMsg(const shape_msgs::Mesh& shape_msg) { if (shape_msg.triangles.empty() || shape_msg.vertices.empty()) { CONSOLE_BRIDGE_logWarn("Mesh definition is empty"); return nullptr; } else { EigenSTL::vector_Vector3d vertices(shape_msg.vertices.size()); std::vector triangles(shape_msg.triangles.size() * 3); for (unsigned int i = 0; i < shape_msg.vertices.size(); ++i) vertices[i] = Eigen::Vector3d(shape_msg.vertices[i].x, shape_msg.vertices[i].y, shape_msg.vertices[i].z); for (unsigned int i = 0; i < shape_msg.triangles.size(); ++i) { unsigned int i3 = i * 3; triangles[i3++] = shape_msg.triangles[i].vertex_indices[0]; triangles[i3++] = shape_msg.triangles[i].vertex_indices[1]; triangles[i3] = shape_msg.triangles[i].vertex_indices[2]; } return createMeshFromVertices(vertices, triangles); } } 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()) 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()) 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()) 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()) shape = new Cone(shape_msg.dimensions[shape_msgs::SolidPrimitive::CONE_RADIUS], shape_msg.dimensions[shape_msgs::SolidPrimitive::CONE_HEIGHT]); } if (shape == nullptr) CONSOLE_BRIDGE_logError("Unable to construct shape corresponding to shape_msg of type %d", (int)shape_msg.type); return shape; } namespace { class ShapeVisitorAlloc : public boost::static_visitor { public: Shape* operator()(const shape_msgs::Plane& shape_msg) const { return constructShapeFromMsg(shape_msg); } Shape* operator()(const shape_msgs::Mesh& shape_msg) const { return constructShapeFromMsg(shape_msg); } Shape* operator()(const shape_msgs::SolidPrimitive& shape_msg) const { return constructShapeFromMsg(shape_msg); } }; } // namespace Shape* constructShapeFromMsg(const ShapeMsg& shape_msg) { return boost::apply_visitor(ShapeVisitorAlloc(), shape_msg); } namespace { class ShapeVisitorMarker : public boost::static_visitor { public: ShapeVisitorMarker(visualization_msgs::Marker* marker, bool use_mesh_triangle_list) : boost::static_visitor(), use_mesh_triangle_list_(use_mesh_triangle_list), marker_(marker) { } void operator()(const shape_msgs::Plane& /* shape_msg */) const { throw std::runtime_error("No visual markers can be constructed for planes"); } void operator()(const shape_msgs::Mesh& shape_msg) const { geometric_shapes::constructMarkerFromShape(shape_msg, *marker_, use_mesh_triangle_list_); } void operator()(const shape_msgs::SolidPrimitive& shape_msg) const { geometric_shapes::constructMarkerFromShape(shape_msg, *marker_); } private: bool use_mesh_triangle_list_; visualization_msgs::Marker* marker_; }; } // namespace bool constructMarkerFromShape(const Shape* shape, visualization_msgs::Marker& marker, bool use_mesh_triangle_list) { ShapeMsg shape_msg; if (constructMsgFromShape(shape, shape_msg)) { bool ok = false; try { boost::apply_visitor(ShapeVisitorMarker(&marker, use_mesh_triangle_list), shape_msg); ok = true; } catch (std::runtime_error& ex) { CONSOLE_BRIDGE_logError("%s", ex.what()); } if (ok) return true; } return false; } namespace { class ShapeVisitorComputeExtents : public boost::static_visitor { public: Eigen::Vector3d operator()(const shape_msgs::Plane& /* shape_msg */) const { Eigen::Vector3d e(0.0, 0.0, 0.0); return e; } Eigen::Vector3d operator()(const shape_msgs::Mesh& shape_msg) const { double x_extent, y_extent, z_extent; geometric_shapes::getShapeExtents(shape_msg, x_extent, y_extent, z_extent); Eigen::Vector3d e(x_extent, y_extent, z_extent); return e; } Eigen::Vector3d operator()(const shape_msgs::SolidPrimitive& shape_msg) const { double x_extent, y_extent, z_extent; geometric_shapes::getShapeExtents(shape_msg, x_extent, y_extent, z_extent); Eigen::Vector3d e(x_extent, y_extent, z_extent); return e; } }; } // namespace Eigen::Vector3d computeShapeExtents(const ShapeMsg& shape_msg) { return boost::apply_visitor(ShapeVisitorComputeExtents(), shape_msg); } Eigen::Vector3d computeShapeExtents(const Shape* shape) { if (shape->type == SPHERE) { double d = static_cast(shape)->radius * 2.0; return Eigen::Vector3d(d, d, d); } else if (shape->type == BOX) { const double* sz = static_cast(shape)->size; return Eigen::Vector3d(sz[0], sz[1], sz[2]); } else if (shape->type == CYLINDER) { double d = static_cast(shape)->radius * 2.0; return Eigen::Vector3d(d, d, static_cast(shape)->length); } else if (shape->type == CONE) { double d = static_cast(shape)->radius * 2.0; return Eigen::Vector3d(d, d, static_cast(shape)->length); } else if (shape->type == MESH) { const Mesh* mesh = static_cast(shape); if (mesh->vertex_count > 1) { std::vector vmin(3, std::numeric_limits::max()); std::vector vmax(3, -std::numeric_limits::max()); for (unsigned int i = 0; i < mesh->vertex_count; ++i) { unsigned int i3 = i * 3; for (unsigned int k = 0; k < 3; ++k) { unsigned int i3k = i3 + k; if (mesh->vertices[i3k] > vmax[k]) vmax[k] = mesh->vertices[i3k]; if (mesh->vertices[i3k] < vmin[k]) vmin[k] = mesh->vertices[i3k]; } } return Eigen::Vector3d(vmax[0] - vmin[0], vmax[1] - vmin[1], vmax[2] - vmin[2]); } else return Eigen::Vector3d(0.0, 0.0, 0.0); } else return Eigen::Vector3d(0.0, 0.0, 0.0); } void computeShapeBoundingSphere(const Shape* shape, Eigen::Vector3d& center, double& radius) { center.x() = 0.0; center.y() = 0.0; center.z() = 0.0; radius = 0.0; if (shape->type == SPHERE) { radius = static_cast(shape)->radius; } else if (shape->type == BOX) { const double* sz = static_cast(shape)->size; double half_width = sz[0] * 0.5; double half_height = sz[1] * 0.5; double half_depth = sz[2] * 0.5; radius = std::sqrt(half_width * half_width + half_height * half_height + half_depth * half_depth); } else if (shape->type == CYLINDER) { double cyl_radius = static_cast(shape)->radius; double half_len = static_cast(shape)->length * 0.5; radius = std::sqrt(cyl_radius * cyl_radius + half_len * half_len); } else if (shape->type == CONE) { double cone_radius = static_cast(shape)->radius; double cone_height = static_cast(shape)->length; if (cone_height > cone_radius) { // center of sphere is intersection of perpendicular bisectors: double z = (cone_height - (cone_radius * cone_radius / cone_height)) * 0.5; center.z() = z - (cone_height * 0.5); radius = cone_height - z; } else { // short cone. Bounding sphere touches base only. center.z() = -(cone_height * 0.5); radius = cone_radius; } } else if (shape->type == MESH) { const Mesh* mesh = static_cast(shape); if (mesh->vertex_count > 1) { double mx = std::numeric_limits::max(); Eigen::Vector3d min(mx, mx, mx); Eigen::Vector3d max(-mx, -mx, -mx); unsigned int cnt = mesh->vertex_count * 3; for (unsigned int i = 0; i < cnt; i += 3) { Eigen::Vector3d v(mesh->vertices[i + 0], mesh->vertices[i + 1], mesh->vertices[i + 2]); min = min.cwiseMin(v); max = max.cwiseMax(v); } center = (min + max) * 0.5; radius = (max - min).norm() * 0.5; } } } bool constructMsgFromShape(const Shape* shape, ShapeMsg& shape_msg) { if (shape->type == SPHERE) { shape_msgs::SolidPrimitive s; s.type = shape_msgs::SolidPrimitive::SPHERE; s.dimensions.resize(geometric_shapes::solidPrimitiveDimCount()); s.dimensions[shape_msgs::SolidPrimitive::SPHERE_RADIUS] = static_cast(shape)->radius; shape_msg = s; } else if (shape->type == BOX) { shape_msgs::SolidPrimitive s; s.type = shape_msgs::SolidPrimitive::BOX; const double* sz = static_cast(shape)->size; 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]; shape_msg = s; } else if (shape->type == CYLINDER) { shape_msgs::SolidPrimitive s; s.type = shape_msgs::SolidPrimitive::CYLINDER; 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; } else if (shape->type == CONE) { shape_msgs::SolidPrimitive s; s.type = shape_msgs::SolidPrimitive::CONE; 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; } else if (shape->type == PLANE) { shape_msgs::Plane s; const Plane* p = static_cast(shape); s.coef[0] = p->a; s.coef[1] = p->b; s.coef[2] = p->c; s.coef[3] = p->d; shape_msg = s; } else if (shape->type == MESH) { shape_msgs::Mesh s; const Mesh* mesh = static_cast(shape); s.vertices.resize(mesh->vertex_count); s.triangles.resize(mesh->triangle_count); for (unsigned int i = 0; i < mesh->vertex_count; ++i) { unsigned int i3 = i * 3; s.vertices[i].x = mesh->vertices[i3]; s.vertices[i].y = mesh->vertices[i3 + 1]; s.vertices[i].z = mesh->vertices[i3 + 2]; } for (unsigned int i = 0; i < s.triangles.size(); ++i) { unsigned int i3 = i * 3; s.triangles[i].vertex_indices[0] = mesh->triangles[i3]; s.triangles[i].vertex_indices[1] = mesh->triangles[i3 + 1]; s.triangles[i].vertex_indices[2] = mesh->triangles[i3 + 2]; } shape_msg = s; } else { CONSOLE_BRIDGE_logError("Unable to construct shape message for shape of type %d", (int)shape->type); return false; } return true; } void saveAsText(const Shape* shape, std::ostream& out) { if (shape->type == SPHERE) { out << Sphere::STRING_NAME << std::endl; out << static_cast(shape)->radius << std::endl; } else if (shape->type == BOX) { out << Box::STRING_NAME << std::endl; const double* sz = static_cast(shape)->size; out << sz[0] << " " << sz[1] << " " << sz[2] << std::endl; } else if (shape->type == CYLINDER) { out << Cylinder::STRING_NAME << std::endl; out << static_cast(shape)->radius << " " << static_cast(shape)->length << std::endl; } else if (shape->type == CONE) { out << Cone::STRING_NAME << std::endl; out << static_cast(shape)->radius << " " << static_cast(shape)->length << std::endl; } else if (shape->type == PLANE) { out << Plane::STRING_NAME << std::endl; const Plane* p = static_cast(shape); out << p->a << " " << p->b << " " << p->c << " " << p->d << std::endl; } else if (shape->type == MESH) { out << Mesh::STRING_NAME << std::endl; const Mesh* mesh = static_cast(shape); out << mesh->vertex_count << " " << mesh->triangle_count << std::endl; for (unsigned int i = 0; i < mesh->vertex_count; ++i) { unsigned int i3 = i * 3; out << mesh->vertices[i3] << " " << mesh->vertices[i3 + 1] << " " << mesh->vertices[i3 + 2] << std::endl; } for (unsigned int i = 0; i < mesh->triangle_count; ++i) { unsigned int i3 = i * 3; out << mesh->triangles[i3] << " " << mesh->triangles[i3 + 1] << " " << mesh->triangles[i3 + 2] << std::endl; } } else { CONSOLE_BRIDGE_logError("Unable to save shape of type %d", (int)shape->type); } } Shape* constructShapeFromText(std::istream& in) { Shape* result = nullptr; if (in.good() && !in.eof()) { std::string type; in >> type; if (in.good() && !in.eof()) { if (type == Sphere::STRING_NAME) { double radius; in >> radius; result = new Sphere(radius); } else if (type == Box::STRING_NAME) { double x, y, z; in >> x >> y >> z; result = new Box(x, y, z); } else if (type == Cylinder::STRING_NAME) { double r, l; in >> r >> l; result = new Cylinder(r, l); } else if (type == Cone::STRING_NAME) { double r, l; in >> r >> l; result = new Cone(r, l); } else if (type == Plane::STRING_NAME) { double a, b, c, d; in >> a >> b >> c >> d; result = new Plane(a, b, c, d); } else if (type == Mesh::STRING_NAME) { unsigned int v, t; in >> v >> t; Mesh* m = new Mesh(v, t); result = m; for (unsigned int i = 0; i < m->vertex_count; ++i) { unsigned int i3 = i * 3; in >> m->vertices[i3] >> m->vertices[i3 + 1] >> m->vertices[i3 + 2]; } for (unsigned int i = 0; i < m->triangle_count; ++i) { unsigned int i3 = i * 3; in >> m->triangles[i3] >> m->triangles[i3 + 1] >> m->triangles[i3 + 2]; } m->computeTriangleNormals(); m->computeVertexNormals(); } else CONSOLE_BRIDGE_logError("Unknown shape type: '%s'", type.c_str()); } } return result; } const std::string& shapeStringName(const Shape* shape) { static const std::string unknown = "unknown"; if (shape) switch (shape->type) { case SPHERE: return Sphere::STRING_NAME; case CYLINDER: return Cylinder::STRING_NAME; case CONE: return Cone::STRING_NAME; case BOX: return Box::STRING_NAME; case PLANE: return Plane::STRING_NAME; case MESH: return Mesh::STRING_NAME; case OCTREE: return OcTree::STRING_NAME; default: return unknown; } else { static const std::string empty; return empty; } } } // namespace shapes geometric_shapes-0.7.3/src/shape_to_marker.cpp000066400000000000000000000136471405115746100215170ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2012, Willow Garage, Inc. * 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 the Willow Garage 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 #include #include #include void geometric_shapes::constructMarkerFromShape(const shape_msgs::SolidPrimitive& shape_msg, visualization_msgs::Marker& mk) { switch (shape_msg.type) { case shape_msgs::SolidPrimitive::SPHERE: if (shape_msg.dimensions.size() < geometric_shapes::solidPrimitiveDimCount()) throw std::runtime_error("Insufficient dimensions in sphere definition"); else { mk.type = visualization_msgs::Marker::SPHERE; mk.scale.x = mk.scale.y = mk.scale.z = shape_msg.dimensions[shape_msgs::SolidPrimitive::SPHERE_RADIUS] * 2.0; } break; case shape_msgs::SolidPrimitive::BOX: if (shape_msg.dimensions.size() < geometric_shapes::solidPrimitiveDimCount()) throw std::runtime_error("Insufficient dimensions in box definition"); else { mk.type = visualization_msgs::Marker::CUBE; mk.scale.x = shape_msg.dimensions[shape_msgs::SolidPrimitive::BOX_X]; mk.scale.y = shape_msg.dimensions[shape_msgs::SolidPrimitive::BOX_Y]; mk.scale.z = shape_msg.dimensions[shape_msgs::SolidPrimitive::BOX_Z]; } break; case shape_msgs::SolidPrimitive::CONE: if (shape_msg.dimensions.size() < geometric_shapes::solidPrimitiveDimCount()) throw std::runtime_error("Insufficient dimensions in cone definition"); else { // there is no CONE marker, so this produces a cylinder marker as well mk.type = visualization_msgs::Marker::CYLINDER; mk.scale.x = shape_msg.dimensions[shape_msgs::SolidPrimitive::CONE_RADIUS] * 2.0; mk.scale.y = mk.scale.x; mk.scale.z = shape_msg.dimensions[shape_msgs::SolidPrimitive::CONE_HEIGHT]; } break; case shape_msgs::SolidPrimitive::CYLINDER: if (shape_msg.dimensions.size() < geometric_shapes::solidPrimitiveDimCount()) throw std::runtime_error("Insufficient dimensions in cylinder definition"); else { mk.type = visualization_msgs::Marker::CYLINDER; mk.scale.x = shape_msg.dimensions[shape_msgs::SolidPrimitive::CYLINDER_RADIUS] * 2.0; mk.scale.y = mk.scale.x; mk.scale.z = shape_msg.dimensions[shape_msgs::SolidPrimitive::CYLINDER_HEIGHT]; } break; default: { std::stringstream ss; ss << shape_msg.type; throw std::runtime_error("Unknown shape type: " + ss.str()); } } } void geometric_shapes::constructMarkerFromShape(const shape_msgs::Mesh& shape_msg, visualization_msgs::Marker& mk, bool use_mesh_triangle_list) { if (shape_msg.triangles.empty() || shape_msg.vertices.empty()) throw std::runtime_error("Mesh definition is empty"); if (use_mesh_triangle_list) { mk.type = visualization_msgs::Marker::TRIANGLE_LIST; mk.scale.x = mk.scale.y = mk.scale.z = 1.0; for (std::size_t i = 0; i < shape_msg.triangles.size(); ++i) { mk.points.push_back(shape_msg.vertices[shape_msg.triangles[i].vertex_indices[0]]); mk.points.push_back(shape_msg.vertices[shape_msg.triangles[i].vertex_indices[1]]); mk.points.push_back(shape_msg.vertices[shape_msg.triangles[i].vertex_indices[2]]); } } else { mk.type = visualization_msgs::Marker::LINE_LIST; mk.scale.x = mk.scale.y = mk.scale.z = 0.01; for (std::size_t i = 0; i < shape_msg.triangles.size(); ++i) { mk.points.push_back(shape_msg.vertices[shape_msg.triangles[i].vertex_indices[0]]); mk.points.push_back(shape_msg.vertices[shape_msg.triangles[i].vertex_indices[1]]); mk.points.push_back(shape_msg.vertices[shape_msg.triangles[i].vertex_indices[0]]); mk.points.push_back(shape_msg.vertices[shape_msg.triangles[i].vertex_indices[2]]); mk.points.push_back(shape_msg.vertices[shape_msg.triangles[i].vertex_indices[1]]); mk.points.push_back(shape_msg.vertices[shape_msg.triangles[i].vertex_indices[2]]); } } } geometric_shapes-0.7.3/src/shapes.cpp000066400000000000000000000403021405115746100176230ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2011, Willow Garage, Inc. * 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 the Willow Garage 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. *********************************************************************/ /* Author: Ioan Sucan */ #include "geometric_shapes/shapes.h" #include #include #include namespace shapes { const std::string Sphere::STRING_NAME = "sphere"; const std::string Box::STRING_NAME = "box"; const std::string Cylinder::STRING_NAME = "cylinder"; const std::string Cone::STRING_NAME = "cone"; const std::string Mesh::STRING_NAME = "mesh"; const std::string Plane::STRING_NAME = "plane"; const std::string OcTree::STRING_NAME = "octree"; std::ostream& operator<<(std::ostream& ss, ShapeType type) { switch (type) { case UNKNOWN_SHAPE: ss << "unknown"; break; case SPHERE: ss << Sphere::STRING_NAME; break; case CYLINDER: ss << Cylinder::STRING_NAME; break; case CONE: ss << Cone::STRING_NAME; break; case BOX: ss << Box::STRING_NAME; break; case PLANE: ss << Plane::STRING_NAME; break; case MESH: ss << Mesh::STRING_NAME; break; case OCTREE: ss << OcTree::STRING_NAME; break; default: ss << "impossible"; break; } return ss; } Shape::Shape() { type = UNKNOWN_SHAPE; } Shape::~Shape() { } Sphere::Sphere() : Shape() { type = SPHERE; radius = 0.0; } Sphere::Sphere(double r) : Shape() { if (r < 0) throw std::runtime_error("Sphere radius must be non-negative."); type = SPHERE; radius = r; } Cylinder::Cylinder() : Shape() { type = CYLINDER; length = radius = 0.0; } Cylinder::Cylinder(double r, double l) : Shape() { if (r < 0 || l < 0) throw std::runtime_error("Cylinder dimensions must be non-negative."); type = CYLINDER; length = l; radius = r; } Cone::Cone() : Shape() { type = CONE; length = radius = 0.0; } Cone::Cone(double r, double l) : Shape() { if (r < 0 || l < 0) throw std::runtime_error("Cone dimensions must be non-negative."); type = CONE; length = l; radius = r; } Box::Box() : Shape() { type = BOX; size[0] = size[1] = size[2] = 0.0; } Box::Box(double x, double y, double z) : Shape() { if (x < 0 || y < 0 || z < 0) throw std::runtime_error("Box dimensions must be non-negative."); type = BOX; size[0] = x; size[1] = y; size[2] = z; } Mesh::Mesh() : Shape() { type = MESH; vertex_count = 0; vertices = nullptr; triangle_count = 0; triangles = nullptr; triangle_normals = nullptr; vertex_normals = nullptr; } Mesh::Mesh(unsigned int v_count, unsigned int t_count) : Shape() { type = MESH; vertex_count = v_count; vertices = new double[v_count * 3]; triangle_count = t_count; triangles = new unsigned int[t_count * 3]; triangle_normals = new double[t_count * 3]; vertex_normals = new double[v_count * 3]; } Mesh::~Mesh() { if (vertices) delete[] vertices; if (triangles) delete[] triangles; if (triangle_normals) delete[] triangle_normals; if (vertex_normals) delete[] vertex_normals; } Plane::Plane() : Shape() { type = PLANE; a = b = c = d = 0.0; } Plane::Plane(double pa, double pb, double pc, double pd) : Shape() { type = PLANE; a = pa; b = pb; c = pc; d = pd; } OcTree::OcTree() : Shape() { type = OCTREE; } OcTree::OcTree(const std::shared_ptr& t) : octree(t) { type = OCTREE; } Sphere* Sphere::clone() const { return new Sphere(radius); } Cylinder* Cylinder::clone() const { return new Cylinder(radius, length); } Cone* Cone::clone() const { return new Cone(radius, length); } Box* Box::clone() const { return new Box(size[0], size[1], size[2]); } Mesh* Mesh::clone() const { Mesh* dest = new Mesh(vertex_count, triangle_count); unsigned int n = 3 * vertex_count; for (unsigned int i = 0; i < n; ++i) dest->vertices[i] = vertices[i]; if (vertex_normals) for (unsigned int i = 0; i < n; ++i) dest->vertex_normals[i] = vertex_normals[i]; else { delete[] dest->vertex_normals; dest->vertex_normals = nullptr; } n = 3 * triangle_count; for (unsigned int i = 0; i < n; ++i) dest->triangles[i] = triangles[i]; if (triangle_normals) for (unsigned int i = 0; i < n; ++i) dest->triangle_normals[i] = triangle_normals[i]; else { delete[] dest->triangle_normals; dest->triangle_normals = nullptr; } return dest; } Plane* Plane::clone() const { return new Plane(a, b, c, d); } OcTree* OcTree::clone() const { return new OcTree(octree); } void OcTree::scaleAndPadd(double /* scale */, double /* padd */) { CONSOLE_BRIDGE_logWarn("OcTrees cannot be scaled or padded"); } void Plane::scaleAndPadd(double /* scale */, double /* padd */) { CONSOLE_BRIDGE_logWarn("Planes cannot be scaled or padded"); } void Shape::scale(double scale) { scaleAndPadd(scale, 0.0); } void Shape::padd(double padding) { scaleAndPadd(1.0, padding); } void Sphere::scaleAndPadd(double scale, double padding) { const auto tmpRadius = radius * scale + padding; if (tmpRadius < 0) throw std::runtime_error("Sphere radius must be non-negative."); radius = tmpRadius; } void Cylinder::scaleAndPadd(double scaleRadius, double scaleLength, double paddRadius, double paddLength) { const auto tmpRadius = radius * scaleRadius + paddRadius; const auto tmpLength = length * scaleLength + 2.0 * paddLength; if (tmpRadius < 0 || tmpLength < 0) throw std::runtime_error("Cylinder dimensions must be non-negative."); radius = tmpRadius; length = tmpLength; } void Cylinder::scale(double scaleRadius, double scaleLength) { scaleAndPadd(scaleRadius, scaleLength, 0.0, 0.0); } void Cylinder::padd(double paddRadius, double paddLength) { scaleAndPadd(1.0, 1.0, paddRadius, paddLength); } void Cylinder::scaleAndPadd(double scale, double padd) { scaleAndPadd(scale, scale, padd, padd); } void Cone::scaleAndPadd(double scaleRadius, double scaleLength, double paddRadius, double paddLength) { const auto tmpRadius = radius * scaleRadius + paddRadius; const auto tmpLength = length * scaleLength + 2.0 * paddLength; if (tmpRadius < 0 || tmpLength < 0) throw std::runtime_error("Cone dimensions must be non-negative."); radius = tmpRadius; length = tmpLength; } void Cone::scale(double scaleRadius, double scaleLength) { scaleAndPadd(scaleRadius, scaleLength, 0.0, 0.0); } void Cone::padd(double paddRadius, double paddLength) { scaleAndPadd(1.0, 1.0, paddRadius, paddLength); } void Cone::scaleAndPadd(double scale, double padd) { scaleAndPadd(scale, scale, padd, padd); } void Box::scaleAndPadd(double scaleX, double scaleY, double scaleZ, double paddX, double paddY, double paddZ) { const auto tmpSize0 = size[0] * scaleX + paddX * 2.0; const auto tmpSize1 = size[1] * scaleY + paddY * 2.0; const auto tmpSize2 = size[2] * scaleZ + paddZ * 2.0; if (tmpSize0 < 0 || tmpSize1 < 0 || tmpSize2 < 0) throw std::runtime_error("Box dimensions must be non-negative."); size[0] = tmpSize0; size[1] = tmpSize1; size[2] = tmpSize2; } void Box::scale(double scaleX, double scaleY, double scaleZ) { scaleAndPadd(scaleX, scaleY, scaleZ, 0.0, 0.0, 0.0); } void Box::padd(double paddX, double paddY, double paddZ) { scaleAndPadd(1.0, 1.0, 1.0, paddX, paddY, paddZ); } void Box::scaleAndPadd(double scale, double padd) { scaleAndPadd(scale, scale, scale, padd, padd, padd); } void Mesh::scaleAndPadd(double scaleX, double scaleY, double scaleZ, double paddX, double paddY, double paddZ) { // find the center of the mesh double sx = 0.0, sy = 0.0, sz = 0.0; for (unsigned int i = 0; i < vertex_count; ++i) { unsigned int i3 = i * 3; sx += vertices[i3]; sy += vertices[i3 + 1]; sz += vertices[i3 + 2]; } sx /= (double)vertex_count; sy /= (double)vertex_count; sz /= (double)vertex_count; // scale the mesh for (unsigned int i = 0; i < vertex_count; ++i) { unsigned int i3 = i * 3; // vector from center to the vertex double dx = vertices[i3] - sx; 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; } } } void Mesh::scale(double scaleX, double scaleY, double scaleZ) { scaleAndPadd(scaleX, scaleY, scaleZ, 0.0, 0.0, 0.0); } void Mesh::padd(double paddX, double paddY, double paddZ) { scaleAndPadd(1.0, 1.0, 1.0, paddX, paddY, paddZ); } void Mesh::scaleAndPadd(double scale, double padd) { scaleAndPadd(scale, scale, scale, padd, padd, padd); } void Shape::print(std::ostream& out) const { out << this << std::endl; } void Sphere::print(std::ostream& out) const { out << "Sphere[radius=" << radius << "]" << std::endl; } void Cylinder::print(std::ostream& out) const { out << "Cylinder[radius=" << radius << ", length=" << length << "]" << std::endl; } void Cone::print(std::ostream& out) const { out << "Cone[radius=" << radius << ", length=" << length << "]" << std::endl; } void Box::print(std::ostream& out) const { out << "Box[x=length=" << size[0] << ", y=width=" << size[1] << "z=height=" << size[2] << "]" << std::endl; } void Mesh::print(std::ostream& out) const { out << "Mesh[vertices=" << vertex_count << ", triangles=" << triangle_count << "]" << std::endl; } void Plane::print(std::ostream& out) const { out << "Plane[a=" << a << ", b=" << b << ", c=" << c << ", d=" << d << "]" << std::endl; } void OcTree::print(std::ostream& out) const { if (octree) { double minx, miny, minz, maxx, maxy, maxz; octree->getMetricMin(minx, miny, minz); octree->getMetricMax(maxx, maxy, maxz); out << "OcTree[depth = " << octree->getTreeDepth() << ", resolution = " << octree->getResolution() << " inside box (minx=" << minx << ", miny=" << miny << ", minz=" << minz << ", maxx=" << maxx << ", maxy=" << maxy << ", maxz=" << maxz << ")]" << std::endl; } else out << "OcTree[NULL]" << std::endl; } bool Shape::isFixed() const { return false; } bool OcTree::isFixed() const { return true; } bool Plane::isFixed() const { return true; } void Mesh::computeTriangleNormals() { if (triangle_count && !triangle_normals) triangle_normals = new double[triangle_count * 3]; // compute normals for (unsigned int i = 0; i < triangle_count; ++i) { unsigned int i3 = i * 3; Eigen::Vector3d s1(vertices[triangles[i3] * 3] - vertices[triangles[i3 + 1] * 3], vertices[triangles[i3] * 3 + 1] - vertices[triangles[i3 + 1] * 3 + 1], vertices[triangles[i3] * 3 + 2] - vertices[triangles[i3 + 1] * 3 + 2]); Eigen::Vector3d s2(vertices[triangles[i3 + 1] * 3] - vertices[triangles[i3 + 2] * 3], vertices[triangles[i3 + 1] * 3 + 1] - vertices[triangles[i3 + 2] * 3 + 1], vertices[triangles[i3 + 1] * 3 + 2] - vertices[triangles[i3 + 2] * 3 + 2]); Eigen::Vector3d normal = s1.cross(s2); normal.normalize(); triangle_normals[i3] = normal.x(); triangle_normals[i3 + 1] = normal.y(); triangle_normals[i3 + 2] = normal.z(); } } void Mesh::computeVertexNormals() { if (!triangle_normals) 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)); for (unsigned int tIdx = 0; tIdx < triangle_count; ++tIdx) { unsigned int tIdx3 = 3 * tIdx; unsigned int tIdx3_1 = tIdx3 + 1; unsigned int tIdx3_2 = tIdx3 + 2; unsigned int v1 = triangles[tIdx3]; 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]; } for (std::size_t i = 0; i < avg_normals.size(); ++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]; } } void Mesh::mergeVertices(double threshold) { const double thresholdSQR = threshold * threshold; std::vector vertex_map(vertex_count); EigenSTL::vector_Vector3d orig_vertices(vertex_count); EigenSTL::vector_Vector3d compressed_vertices; for (unsigned int vIdx = 0; vIdx < vertex_count; ++vIdx) { orig_vertices[vIdx][0] = vertices[3 * vIdx]; orig_vertices[vIdx][1] = vertices[3 * vIdx + 1]; orig_vertices[vIdx][2] = vertices[3 * vIdx + 2]; vertex_map[vIdx] = vIdx; } for (unsigned int vIdx1 = 0; vIdx1 < vertex_count; ++vIdx1) { if (vertex_map[vIdx1] != vIdx1) continue; vertex_map[vIdx1] = compressed_vertices.size(); compressed_vertices.push_back(orig_vertices[vIdx1]); for (unsigned int vIdx2 = vIdx1 + 1; vIdx2 < vertex_count; ++vIdx2) { double distanceSQR = (orig_vertices[vIdx1] - orig_vertices[vIdx2]).squaredNorm(); if (distanceSQR <= thresholdSQR) vertex_map[vIdx2] = vertex_map[vIdx1]; } } if (compressed_vertices.size() == orig_vertices.size()) return; // redirect triangles to new vertices! for (unsigned int tIdx = 0; tIdx < triangle_count; ++tIdx) { unsigned int i3 = 3 * tIdx; triangles[i3] = vertex_map[triangles[i3]]; triangles[i3 + 1] = vertex_map[triangles[i3 + 1]]; triangles[i3 + 2] = vertex_map[triangles[i3 + 2]]; } vertex_count = compressed_vertices.size(); delete[] vertices; vertices = new double[vertex_count * 3]; for (unsigned int vIdx = 0; vIdx < vertex_count; ++vIdx) { unsigned int i3 = 3 * vIdx; vertices[i3] = compressed_vertices[vIdx][0]; vertices[i3 + 1] = compressed_vertices[vIdx][1]; vertices[i3 + 2] = compressed_vertices[vIdx][2]; } if (triangle_normals) computeTriangleNormals(); if (vertex_normals) computeVertexNormals(); } } /* namespace shapes */ geometric_shapes-0.7.3/test/000077500000000000000000000000001405115746100160255ustar00rootroot00000000000000geometric_shapes-0.7.3/test/CMakeLists.txt000066400000000000000000000035751405115746100205770ustar00rootroot00000000000000file(TO_NATIVE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/resources" TEST_RESOURCES_DIR) if(WIN32) # Correct directory separator for Windows string(REPLACE "\\" "\\\\" TEST_RESOURCES_DIR "${TEST_RESOURCES_DIR}") endif() configure_file(resources/config.h.in "${CMAKE_CURRENT_BINARY_DIR}/resources/config.h") include_directories(${CMAKE_CURRENT_BINARY_DIR}) catkin_add_gtest(test_basics test_basics.cpp) # target_link_libraries(test_basics ) # only depends on Eigen, which is header-only catkin_add_gtest(test_point_inclusion test_point_inclusion.cpp) target_link_libraries(test_point_inclusion ${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) catkin_add_gtest(test_bounding_sphere test_bounding_sphere.cpp) target_link_libraries(test_bounding_sphere ${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) catkin_add_gtest(test_bounding_box test_bounding_box.cpp) target_link_libraries(test_bounding_box ${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) catkin_add_gtest(test_bounding_cylinder test_bounding_cylinder.cpp) target_link_libraries(test_bounding_cylinder ${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) catkin_add_gtest(test_create_mesh test_create_mesh.cpp) target_link_libraries(test_create_mesh ${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) catkin_add_gtest(test_loaded_meshes test_loaded_meshes.cpp) target_link_libraries(test_loaded_meshes ${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) catkin_add_gtest(test_shapes test_shapes.cpp) target_link_libraries(test_shapes ${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) catkin_add_gtest(test_ray_intersection test_ray_intersection.cpp) target_link_libraries(test_ray_intersection ${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) catkin_add_gtest(test_body_operations test_body_operations.cpp) target_link_libraries(test_body_operations ${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) geometric_shapes-0.7.3/test/resources/000077500000000000000000000000001405115746100200375ustar00rootroot00000000000000geometric_shapes-0.7.3/test/resources/box.dae000066400000000000000000000050331405115746100213030ustar00rootroot00000000000000 Blender User Blender 2.79.0 2018-11-20T19:03:54 2018-11-20T19:03:54 Z_UP 1.0 1.0 -1.0 1.0 -1.0 -1.0 -1.0 -1.0 -1.0 -1.0 1.0 -1.0 1.0 1.0 1.0 -1.0 1.0 1.0 -1.0 -1.0 1.0 1.0 -1.0 1.0 0 0 -1 0 0 -1 0 0 1 0 0 1 1 0 0 1 0 0 0 -1 0 0 -1 0 -1 0 0 -1 0 0 0 1 0 0 1 0

0 0 1 0 2 0 2 1 3 1 0 1 4 2 5 2 6 2 6 3 7 3 4 3 0 4 4 4 7 4 7 5 1 5 0 5 1 6 7 6 6 6 6 7 2 7 1 7 2 8 6 8 5 8 5 9 3 9 2 9 4 10 0 10 3 10 3 11 5 11 4 11

1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1
geometric_shapes-0.7.3/test/resources/config.h.in000066400000000000000000000002321405115746100220570ustar00rootroot00000000000000#ifndef GEOMETRIC_SHAPES_TEST_RESOURCES_CONFIG_ #define GEOMETRIC_SHAPES_TEST_RESOURCES_CONFIG_ #define TEST_RESOURCES_DIR "@TEST_RESOURCES_DIR@" #endif geometric_shapes-0.7.3/test/resources/cube.stl000066400000000000000000000012541405115746100215030ustar00rootroot00000000000000Exported from Blender-2.77 (sub 0) >>> 3>>>3??>?>????>??>?? >>?>?>??4>?>>>>>??3?>4?>?4>?> `4?4?>?>>>4?3>>??>?geometric_shapes-0.7.3/test/resources/forearm_roll.stl000066400000000000000000001411061405115746100232510ustar00rootroot00000000000000VCG ?%>? ?=2$= =O#'=%=!j*%=5?!?9>=`=Y;=G=纇=\;=+9m4?jn4?=qٻn<=B>軸<=¾ջ#q<?#N=cuO=;=LN=c̼K?= ==Hr@=佼 ^?=j++>Ҿ)V=8|?VK=*y?H=.<=N=<=N=c<B=f#F|?H==H==4===?ç%=𘗾WOc>6=,a=$<=f`S=6=E=5?W5==}=<}==o= 3>?6=E=$<=f`S=6=|=435=>jk?N=B>M== <c'?,Q= @A=4`<=M]Y< =yXk<?8:7N=Z2w?=B>M<=A<<=q'<?=j9m4=IH%<;=<^ռ7<=41*)H=41*)7<=s(i;ĠƜrL=4t{>^?=j++SF=w% 5=H=41*)7<=41*)7<=;W(@3G%q>9<==4=6=,a===?ç%=GC3?Z?N=ҬB=N==H==N=B^=XQ=f===ۏ q=D#?9<==4=H==4=$<=f`S=V炶b56=3 '6=|;496=.;W$J?2=Y?=#<=۵;n<=h ;<Y=f>]Ly,=$(-Vy=gף<=5^+=N=5^+=2r=?=d=8==cA==ۏ q=<=9sڣB?$<=pF?!(=6=8Kd+&=3=2Rvh$=s}K >2r=N=>.=>.E4>i}%?==4< #=$<=<`S=6=)<|=?T6iGN=޿-O$N=wS;ōN=C弅ʀX?3Ǽ=5M=I(u;N='-/:N=LQa<HYulI=)r!QV>=[%X7=(*?5˵5=zx"==8==d</Ȏ [ZN=BFD$?=)<|==<`S==SW< =v==ϩsM=iP%L=4t{>SF=w%5?a5=uY==s ==s<p;>Uu?':=pM<'=|H=;<2(==;<2(=C=H?)?J=6</?N=$F=, =d-(=H=7~>-(=LX? ḥN=<=#==ff<گ<=6n.?=B>軸<=B>M<=K :<a`?u>N==+===+=m==?=G=纭=.<͹===>.D?? >N=%=;5='=;N=x=i<┊&GK?T *J= =i5=547GyuO=;=弃D=bⱬ;=мIR>UFT;WL=4t{>LN=c̼Kr@=佼 P_+>ȓD?K?I=8<ҫ$=N=$=fէگ<=Fg<=e<`>A,?G_==en=}uN=*=='N=c=Pt?=…<=XڼJ<=#¼\< @E~>v?TI=0<4*=|H=;<2(=7=B,;-=?qH N=hYN=B`eN=p;$J5?L5=Z>.=n=B`e4?\Hu,?=7~>-(==zx"==HX[)1=-.===B`e<Q;>%w?S>LJ=JZ(=xHgJ=""=ۇ8E=̈$=t8N{Q$D=bⱬ7=(*;=мf>YmrOL=4t{>N=| LN=c̼KU [w?^I=*=$<=pF?!(=H=7~>-(=q'>ml?E=̈$=t8B=M=ډ@==4\&M3?.N3?X =ܼh9=.ἅ͹=ݼ͹D>=y7>5M=I(u;A=#>=[%X?ˍ7N=%=;N=#J=>.N=%=̍(@?> ` ?H=<=N==6< ==ùq3= ^< 6=.;W$6=H<?=us<=d<=uY=Jd>I?6=^6)>a`=M]Y<=b`C<=;u<m}6=9u q< 6=. [פ<;=<aA?>#(N=C弅ʀN= ޼7r=ѝ|5?K5=<==6 ='b+2==f==B^=XQ=[ #VC= */ 6=<$f~V>=[%XjEt(N=wqqyW#=- xN=B`e?SіBN=ϖE<<(c=~#?M??=<`S==Y<,r==<=C2{ds>-?':=pM<'===4< #=6=^6F=, =d.!#SHa?e=uF_8-=;u<=b`CS?N=f=l'<^< =Y =s.?=.ἅ͹=Z>.=ZB`e?=.B`e=.ἅ͹=ZB`eh »o^yBG=<N=S?%=!j*%= =O#'==Z5)=;J¾RlN=;#=-(=3=2Rvh$=<;>Xs?=vZ<[w&=':=pM<'==;<2(=9<2Y$SF=w%^?=j++6=__%~ý=#<'R7N=w<͹=w<͹5?W5=H}=|o=Hos8?b=<4N=ff<گ ='b+2<=fէگ<=s<?ݴ4mN=cq =?H=<=7<=<=H=.<=?N=d?$<=<`S=G5=`><(c=6=)<|=N=w<=w6=' ;6=J<6=|;494J.#=C4?7=B,;-=6=4B-=$<=4O/= fAd?=<[&=?=•"<4(==.<[&)=?N=#J=>.=#J=B`eN=#J=B`e?N=ؼ:N=ܼh9N=ݼ͹?jp7췛=v<=b`C<=e<u 8?>6=)<(c=#H}?6=8Kd+&=8=jN-=6=[֫T,=?w6󗶬=zx"==|==8=6??N=Wjdj="<=e.=ݞN===}==+==G=S?>v}>=>.=t=5^+=\>(>D=g,=2<6=g=`<6=`=I[%<9=<=d<P*?$<=4O/=B=K.=H=ⷸO/=? FsN=hYN=M:aN=wqqye?3*[f`= =%=!j*%==HѤ{?N=Y;@=ff<گ<=6=c ?6="< =6=)<|=G5=`><(c=?FT^=׫}?=.<[&)=|H=;<2(=m=W2<)=?7N=y=6YN=#J=>.N=#J=B`eF:Z?z?G5=`><(c=e.=ݞN=5^+=N=>.2r=?r?ס>=ff<گN==+=m==N=#J=>.?-G7=K :<=b`C<=M]Y<?z.)N=c輥N=޿-O$N=C弅ʀs}?>m===#J=>.N=#J=>.U>4V?,>=}<<=6=[%Xۼb*?$e>I=fuT!O=7)WV>=[%XI=)r!Q=QW| ?I=^Jy]? B=M=ډ*J= =i@==4\Bdc:wp=5M=I(u;m4=IH%K=U=wL= <%H=41*)7<=;W(A>IBy7<=s(H=41*)SF=w%G>B?e*J= =iI=f<꼋5=54wz?<5='=;LJ=JZ(=xH6=(=7rŵ$A%?6=9u qB8= =X<[I==2$u<2=%=d<c>HdɾD=bⱬI=)r!Q7=(*ɵ/>|s?dz>[I==2$u<5='=;2=%=d<=pPK?K=z:5\ =Y =< ==<&=ċ=H<2>$=mZ|9,>N=< ==< =ċ=H<2>$=*a[>O 6=!7=B,;-=6=^6?v}>==}=#J=>.==+={ ?Q??I=8<ҫ==H==4=#=Y=H66=,a=6=|=6=8Kd+&=6=)?օ6N=DQ;.=#J=B`eN=#J=>.?۹5}tN=Wjd-w =NEN=HoN=C弅ʀ?N=ff<گ6=Hɼp=6=8<@={` <rnNR1? =`N{B$=18:>}>=sN=Wjdxr4K=v< =?ɋ< =Vt([< E? ?N=(h<K=]Y=ɊoZN=]z8&k=;u<< WN=7<= D| =yXk<,=$(-VsL= <%7<=;W(3= ^< iq>D>slY=ɊL>I=^J[I==2$u 9?N={ϼ?=H==K=z:5\<:?YMSB'M=iP%N=޿-O$L=4t{>F?y{PN=޿-O$M=iP%H=41*)c4WƾZ?7<==6=ҬAB=6=Hɼp=d3?6q>'?H=.<=N=c<B=H=<=h?:>H==#=Y=H==4=ASƲ 6=Ȏ;6=__%^?=j++j5?7<==6=Hɼp=@={` <B4185ҷ6=8<6=J< 6=. [פ<+N?YK>6=g=`f===Y==ۏ q=4? ^=4H=41*)N=;N$N=4';I?S>sN=<{KY=Ɋ-(==7~>-(=6uigҾ+=%<p9軸<=K :<=M]Y<KUx^ @6=' ; 6=<$f~C= */ |Q5ᄚi>6=8< 6=. [פ<6=9u q<hN?(=5M=I(u;N=LQa<"fEڳ?N=4j< =`N{B<=4j<?)8N=ϖE<p>6=cm?e>== <=yA‚<=A<<6 W}?m=Z5)=H=7~>-(==27b$)=ۇ>[?\>F=, =d<[I==2$uN=Z=…<=Fg<=Em <?яML6N=c輥N=HoN=I6=cD=g,=22=%=d<6=$=;B8= =X<`>lg*?HN=x=i<5='=;[I==2$u<,?_ 㾃D=bⱬuO=;=弪N=cN^>6=J\Y=Ɋ^>=q'<`=I[%<=K :<?36N=B<13K=z:5\(y?=27b$)=N=(&}?(=m=Z5)=d;yx\> = <.?N=w-(==Z5)=9;>t? =O#'=P= !w'==27b$)=?N=ݼ])N=ؼ:N=ݼ͹2:}?} >H=<<4=1n= O<=#=j="<==<==ow?=Z5)= =O#'==27b$)=94?b2=4?H=NL;18.=H=ⷸO/=N=ⷸB-=?oY6Rq4N=ҬB=N={ϼ?=N=s<I:=\W~?=۵;n<=B;ME?=<&=?=•"<4(=ċ=H<2>$=?փ@N=1n= O<=j="<=#=|?=h ;<=۵;nqJ|?N=Y;@<=h ;==+==#J=>.m===)=5^+=2r==>.=n=>.N=>.;@?ʌ(? =O#'==2$=P= !w'=D}N=.=jP~?e>== M<IMɰs]㛾=@= =N= =N=M%=NI0Is?䛾=ܼh9==ݼ7N=ܼh9Li?Ҿ=XB=2$== =N=M%=p:b?>y=gף<=us<=s<=fէگ<?N=w<͹N=@=e<=Fg<=…<#4?$4j==Y==uY==F=3?;>-?=<`S==)<|==Y<,r=4?{%?=XB<=O6<=#¼\<01=LZ?B=ݼB`e=V(缲ځ=ѝ|=-mz?^I=*=B=K.=8=jN-=v>1>1yy?TI=0<4*=7=B,;-=H=NL;18.=H?yx>p=M]Y<=;u< =yXk<@W=ބ?H=ⷸO/=H=NL;18.=7=B,;-=?=8==uY==d<R>b;jo>A=#>d'*]?!+gJ=""=ۇ8LJ=JZ(=xHN=%=̍?N=B<136p?=;<2(===4< #==vZ<[w&=u4?$?=|= =-:"==f`S=M?K>:?=;<2(==)<|==SW< =-?0li2? =-:"==zx"==7~>-(=z m?b=XBCi?=SW< =$<=<`S===4< #=$x^C?8=jN-=$<=4O/=6=[֫T,=w*?b?>8?m=W2<)=|H=;<2(=TI=0<4*=T*?>8?m=Z5)=^I=*=H=7~>-(=4?&4?B=K.=N=ⷸB-=H=ⷸO/=?=ݼ7@N=ݼB`eN=ݼ])t?N=(&}?(==27b$)=P= !w'=;D8{N=;iN=F}y =yXk<=;u<,=$(-V<4c*56=3 '7<=41*)6=__%u,l2U>ċ=H<2>$==<[&=N=$e.=ݞ.=.ἅ͹=Ⱥ ?8TJ=;E<=4`<=:BM<6=|=6=,a=6=E=7-."6=J=ļ6=|;496=L%=CgѶ6=Y<=6=4B-=6=^6M<=qٻn<=A<<;j]=Ğ?=B;M[o?=q'<=A<<=yA‚<?׶_6N=]z.N='-/:N=p;$J,W{$s#=l;V<}=9ĭ;'F=b`C< =Vt([1>I=8<ҫ.N=5^+=?64N=Wjd==B^=XQ=v=sY=3|k=U*N=|o=|o_}?;N= ޼7rN=ݼB`e=ݼB`eX4K?۫57? =`N{B<=¾ջ#q<=#<4?]q=e3?=;#q<=۵;n<=#<綟<>e?=yA‚== -(=$<=pF?!(=S;v>m/x?|H=;<2(=':=pM<'=7=B,;-=y m2b=@Kic<=1v=8w==oy=BN=[<E?]C"?=yA‚<9=<`=I[%<ƺ\B?Q&?=yA‚<7=[<9=<Xo?(=bm<-(==H=t=Ⱥ纇=5^+=?}r+o=׫N==N=d<=d<9=<dyg?=qٻn== <=A<<?F78N=ͼqN=C弅ʀN=wS;ō?J07j=b`C<=d<=us<#w;45=L͈<N==#=Y=H==l5?O?¼>#==軸<=M]Y<=¾ջ#q<?5N=HoN=|oN=I_`:I>XJ?6=^6H=<<4=G5=`><(c=1n= O<= 3a >v=sY=H==4===?l7XN=F=< ==6< ==<&=4?/f/67=]v=B`e=x`/?2a/}=.T"=Z=r==}<=1v=8wWU*?L>V*==}==o<=1v=8w< ?tD@=|o=]v=x ?OtR?L2>`=I[%<=d<=K :<#t $DW#=- xk=U*=x ?#:W =NE=ѝ|=V(缲ځG>KM}==oN==oN=*=='?N=we8(?М@y=BN=N=*=='=en=}u^:T->G|y=BN===oN=*=='?vŵs6N=x=i6OL?ȿ=[Hy=V(缲ځ=ݼB`e8?,?5'>%=!j*%== ==2$=# ?AoC=<5?'?=z%:=ܼh9=XB<?& 6N=f=;u<6=d#lN=+p¾<6=d#*w,=$(-V<=;urW|=V(缲ځ=Ho =NE1?>01?=}<<=B;M<=B;<XB>i\3= ^< 7<=;W(6=.;W$?N=fէگ.?%=!j*%==Z5)==X5'=.?.?(=.B`e=[Hy=ݼB`eO.?Kq.=|}=]v=|o@?"?V-===B`e=Z=r<=1v=8w1?T=X1=6=ZJ2V>=[%X 6=<$f~}(C@?H====?ç%=7<==˞*?D>?H=.<=7<=<=?<=.<=g@U>==?ç%=H==4=9<==4=d?7>H=<<4=H=.<=;<=<<4=d?7>H=.<=?<=.<=;<=<<4=1?S1?CB>= == =%=!j*%=?as9N=Nq?':=pM<'==vZ<[w&===4< #=~3#?;>;<=<<4=6=)<(c=m4?_n4?=B>M<=B>軸<=qٻn<B>kFh?$<=<`S==SW< ==<`S=+ؾEh?=f`S= =-:"=$<=f`S= ??4D=bⱬN=IN=M:a?N=ݼ])N=ͼqN=wS;ō0* .m8:?$<=pF?!(=8=jN-=6=8Kd+&=VϤ=6=J<6=' ;m4=IH%<;>:,q?=;<2(==SW< ===4< #=? I6cN=LQa7L= <%N=<{KN=;N$?n桶n{U7=׫=d<=d<=K :<+4?=.,4?=j9 =Y >N=<{KL= <%Y=Ɋ7=[N=c`2Z=I=)r!QN=p;$J!O=7)Wr5?(?9>=`=Y;=\;=+9=X<.6<?a+ 7N=c輼N=C弅ʀN=Ho5??9>=\;=+9=IN=(h<I=f<꼪N=K =zi8?41?Y*J= =iB=M=ډN=K =zN=|oN=Ho=Ho?Թb8N=*=='N==oN=K =z?kY6LN=ⷸB-=N=}*=N=m=?N=$=Em <=XڼJ<=…<VR9>叾@==4\6=J=ļ6==?c/õN==+=N=#J=>.N=%=;3Df==N==N=s<L?N=<ü׊<=fէگpھ)1=-}X>A=#>.=B`ejt^x?m=W2<)=N=$.=B`e=n=< =N=< ==<=Y?@>>=F==8==Y=6=?'?!>=.=#J=B`e=#J=>.5?M5=Z>.=>.=nJ[G166= $6=|;49 6=Ȏ;+K?P?=ù?7<=<=7= <=6=c6=,a=9<==4=$<=f`S=00?^10Dj>=e<=us<=fէگ<4?B|>t)?=j9 =?ɋ=Y==8==ۏ q=5?L5=.<͹=w<͹=wa1=H}=Ho=[Hym4?#=pn4?=B;M<=۵;n<=;#q<4?4=:BM<=4`<=4<3?3Г==Z>.=t=>.S1?1+D=ZB`e=B`e=]v3?3?ϊ====>.=#J=>.==}3?3?"===B`e<=1v=8w=#J=B`e5??9>==+==\;=+9=G=BM3?N3b = <$:=@ 6=. [פ<6=J<;=<DW#?y ?7= <=D=g,=2<6=R?&S=v<=#¼\=Em <=5^+==Ⱥ:~vZS >}?=W2<)==;<2(==.<[&)=?=ݼ7@N=ݼ])=ݼ͹?N=ݼ])N=ݼ͹=ݼ͹N=w<͹N=w|}=HoN=Ho =NE{:9=w3/??<=.<=7<=<=6=c?<=.<=6=c|i>`?=K :<=B>M<=q'<5? 9>=XڼJ<=Em <=Ⱥ47=w 57<=;W(7<=41*)6=3 '?=`=Y;= <$:=.<͹?r98%ŊN=!#)p.N=p;$J5??9>=X<.6<=I?4 ?ʾN=ID=bⱬN=c?x2:N=c=PtN=y=6YN=#J=B`e`ޖ} mb=y;<=<<4=G5=`><(c=H=<<4=?y8$N=B>M=fէگ< ='b+2|{s{q=fu<;Z}㻏?=fu<;Z}㻭= =9?1T?U=fu<;Z};6=;;=;;9?0T?9ȼfu<;Z};S;fu<;Z};9ȼ;;{G>/X9ȼ;;qq{9fu<;Z}5Ta{9fu<;Z}(x;v"499ȼԡ;=Tp==k< =1T9tT=i={=Z}fu<{G{=Z}fu(6=;;d-6=Z};fu<;6=;;{?G9ȼZ};fu<9ȼ;;XH)a0=;B?V6=;t!6=Z}fu<;{GԦ=Z}fu<6=;t!a0=;B?V0T?96=Z};fu<6=;z4=;1T95<]{?8~<;;S;fu<;Z};9ȼ!;;=T=k<㻭=-{=}֭T =;=m#<5<G>{?S;fu<;Z};8~<;;U=fu<;Z};9?0T?S;fu<;Z};U=fu<;Z};=;;7;T=k<6=fuz{5T9ȼZ}fu<;d<; /X{?9ȼfu<;Z};9ȼ!;;S;fu<;Z};91T?6=;6=fu{9ȼfu<;Z}㻄sfu<;Z}9ȼԡ;G>{?6= #!;;6=fu<;Z};U=fu<;Z};9?0T9ȼ;sfu<;Z}9ȼfu<;Z}0T9:9ȼZ}fu<{G>9ȼZ}fu<;9ȼ;B`e!d<; 1T99ȼZ}fu<9ȼ*N{G9ȼ;B`e!9ȼZ}fu{6=Ġ;= =?=fu<;Z}1T9:YZ}fu<1T9W]{(x;v"495Ta0=;B?V/X{sfu<;Z}㻰9fu<;Z}9ȼԡ;G>{{#=fu<;Z}s{q=fu<;Z}㻭= =1T?9?q{?=fu<;Z}6=fu<;Z}6=Ġ;{?G>9ȼZ};fu<;q{?8~<;;6= #!;;U=fu<;Z};0T9WYZ}fu<1T9p={=Z}fu<+=0T9g{;6D<] 2016-08-28T22:17:50 2016-08-28T22:17:50 0 0 1 0 1 0 1 0 0

0 1 2

geometric_shapes-0.7.3/test/resources/triangle_1m.dae000066400000000000000000000023721405115746100227200ustar00rootroot00000000000000 2016-08-28T22:17:50 2016-08-28T22:17:50 0 0 1 0 1 0 1 0 0

0 1 2

geometric_shapes-0.7.3/test/resources/triangle_no_unit.dae000066400000000000000000000023271405115746100240560ustar00rootroot00000000000000 2016-08-28T22:17:50 2016-08-28T22:17:50 0 0 1 0 1 0 1 0 0

0 1 2

geometric_shapes-0.7.3/test/resources/triangle_no_up.dae000066400000000000000000000023721405115746100235230ustar00rootroot00000000000000 2016-08-28T22:17:50 2016-08-28T22:17:50 0 0 1 0 1 0 1 0 0

0 1 2

geometric_shapes-0.7.3/test/resources/triangle_x_up.dae000066400000000000000000000024261405115746100233560ustar00rootroot00000000000000 2016-08-28T22:17:50 2016-08-28T22:17:50 X_UP 0 0 1 0 1 0 1 0 0

0 1 2

geometric_shapes-0.7.3/test/resources/triangle_y_up.dae000066400000000000000000000024261405115746100233570ustar00rootroot00000000000000 2016-08-28T22:17:50 2016-08-28T22:17:50 Y_UP 0 0 1 0 1 0 1 0 0

0 1 2

geometric_shapes-0.7.3/test/resources/triangle_z_up.dae000066400000000000000000000024261405115746100233600ustar00rootroot00000000000000 2016-08-28T22:17:50 2016-08-28T22:17:50 Z_UP 0 0 1 0 1 0 1 0 0

0 1 2

geometric_shapes-0.7.3/test/test_basics.cpp000066400000000000000000000063261405115746100210430ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2019, Bielefeld University * 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 the Bielefeld University 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 #include #include #include TEST(Utils, checkIsometry) { Eigen::Isometry3d t(Eigen::AngleAxisd(0.42, Eigen::Vector3d(1, 1, 1).normalized())); ASSERT_TRUE(::checkIsometry(t)); const Eigen::Vector3d oldDiagonal = t.linear().diagonal(); t.linear() = t.linear() * Eigen::DiagonalMatrix(1.0, 2.0, 3.0); ASSERT_FALSE(::checkIsometry(t)); t.matrix().row(3) = Eigen::Vector4d(1e-6, 1e-6, 1e-6, 1 - 1e-6); ASSERT_FALSE(::checkIsometry(t, CHECK_ISOMETRY_PRECISION)); t.linear().diagonal() = oldDiagonal; ASSERT_TRUE(::checkIsometry(t, 1e-0, false)); ASSERT_FALSE(::checkIsometry(t, 1e-1)); ASSERT_FALSE(::checkIsometry(t, 1e-2, false)); std::cerr.flush(); } TEST(Utils, assertIsometry) { Eigen::Isometry3d t(Eigen::AngleAxisd(0.42, Eigen::Vector3d(1, 1, 1).normalized())); ASSERT_ISOMETRY(t); t.linear() = t.linear() * Eigen::DiagonalMatrix(1.0, 2.0, 3.0); #ifdef NDEBUG ASSERT_ISOMETRY(t) // noop in release mode #else ASSERT_DEATH(ASSERT_ISOMETRY(t), "Invalid isometry transform"); #endif t.matrix().row(3) = Eigen::Vector4d(1e-6, 1e-6, 1e-6, 1 - 1e-6); #ifdef NDEBUG ASSERT_ISOMETRY(t) // noop in release mode #else ASSERT_DEATH(ASSERT_ISOMETRY(t), "Invalid isometry transform"); #endif std::cerr.flush(); } int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } geometric_shapes-0.7.3/test/test_body_operations.cpp000066400000000000000000000256771405115746100230110ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2019, Bielefeld University * 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 the Bielefeld University 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 #include #include #include "resources/config.h" #include using namespace bodies; void expectVector3dSetsEqual(EigenSTL::vector_Vector3d vec1, EigenSTL::vector_Vector3d vec2, const double upToError = 1e-6) { ASSERT_EQ(vec1.size(), vec2.size()); auto vecCompare = [upToError](const Eigen::Vector3d& a, const Eigen::Vector3d& b) -> bool { if (a.x() < b.x() - upToError) return true; if (a.x() > b.x() + upToError) return false; if (a.y() < b.y() - upToError) return true; if (a.y() > b.y() + upToError) return false; if (a.z() < b.z() - upToError) return true; if (a.z() > b.z() + upToError) return false; return false; }; std::sort(vec1.begin(), vec1.end(), vecCompare); std::sort(vec2.begin(), vec2.end(), vecCompare); for (size_t i = 0; i < vec1.size(); ++i) { EXPECT_NEAR(vec1[i].x(), vec2[i].x(), upToError); EXPECT_NEAR(vec1[i].y(), vec2[i].y(), upToError); EXPECT_NEAR(vec1[i].z(), vec2[i].z(), upToError); } } TEST(Bodies, ConstructShapeFromBodySphere) { const shapes::Shape* shape = new shapes::Sphere(2.0); const auto body = new Sphere(shape); const auto constructedShape = constructShapeFromBody(body); const auto constructedSphere = std::dynamic_pointer_cast(constructedShape); EXPECT_EQ(shape->type, constructedShape->type); ASSERT_NE(nullptr, constructedSphere); EXPECT_EQ(2.0, constructedSphere->radius); } TEST(Bodies, ConstructShapeFromBodyBox) { const shapes::Shape* shape = new shapes::Box(1.0, 2.0, 3.0); const auto body = new Box(shape); const auto constructedShape = constructShapeFromBody(body); const auto constructedBox = std::dynamic_pointer_cast(constructedShape); EXPECT_EQ(shape->type, constructedShape->type); ASSERT_NE(nullptr, constructedBox); EXPECT_EQ(1.0, constructedBox->size[0]); EXPECT_EQ(2.0, constructedBox->size[1]); EXPECT_EQ(3.0, constructedBox->size[2]); } TEST(Bodies, ConstructShapeFromBodyCylinder) { const shapes::Shape* shape = new shapes::Cylinder(1.0, 2.0); const auto body = new Cylinder(shape); const auto constructedShape = constructShapeFromBody(body); const auto constructedCylinder = std::dynamic_pointer_cast(constructedShape); EXPECT_EQ(shape->type, constructedShape->type); ASSERT_NE(nullptr, constructedCylinder); EXPECT_EQ(1.0, constructedCylinder->radius); EXPECT_EQ(2.0, constructedCylinder->length); } TEST(Bodies, ConstructShapeFromBodyMesh) { shapes::Mesh* shape = shapes::createMeshFromResource("file://" + (boost::filesystem::path(TEST_RESOURCES_DIR) / "/box.dae").string()); const auto body = new ConvexMesh(shape); const auto constructedShape = constructShapeFromBody(body); const auto constructedMesh = std::dynamic_pointer_cast(constructedShape); EXPECT_EQ(shape->type, constructedShape->type); ASSERT_NE(nullptr, constructedMesh); ASSERT_EQ(shape->vertex_count, constructedMesh->vertex_count); ASSERT_EQ(shape->triangle_count, constructedMesh->triangle_count); // Compare the vertices and triangle normals of the constructed mesh // Triangle indices cannot be checked because the vertex IDs could change in the constructed mesh EigenSTL::vector_Vector3d verticesOrig, verticesConstructed; for (size_t i = 0; i < shape->vertex_count * 3; i += 3) verticesOrig.push_back({ shape->vertices[i], shape->vertices[i + 1], shape->vertices[i + 2] }); for (size_t i = 0; i < constructedMesh->vertex_count * 3; i += 3) verticesConstructed.push_back( { constructedMesh->vertices[i], constructedMesh->vertices[i + 1], constructedMesh->vertices[i + 2] }); expectVector3dSetsEqual(verticesOrig, verticesConstructed); EigenSTL::vector_Vector3d normalsOrig, normalsConstructed; shape->computeTriangleNormals(); // constructedMesh->computeTriangleNormals(); // is done during construction for (size_t i = 0; i < shape->triangle_count * 3; i += 3) normalsOrig.push_back( { shape->triangle_normals[i], shape->triangle_normals[i + 1], shape->triangle_normals[i + 2] }); for (size_t i = 0; i < constructedMesh->triangle_count * 3; i += 3) normalsConstructed.push_back({ constructedMesh->triangle_normals[i], constructedMesh->triangle_normals[i + 1], constructedMesh->triangle_normals[i + 2] }); expectVector3dSetsEqual(normalsOrig, normalsConstructed); } TEST(Bodies, ConstructMarkerFromBodySphere) { const Eigen::Isometry3d pose = Eigen::Translation3d(1.0, 2.0, 3.0) * Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d(0.0, 1.0, 0.0)); const shapes::Shape* shape = new shapes::Sphere(2.0); const auto body = new Sphere(shape); body->setPose(pose); visualization_msgs::Marker marker; constructMarkerFromBody(body, marker); EXPECT_EQ(visualization_msgs::Marker::SPHERE, marker.type); EXPECT_DOUBLE_EQ(4.0, marker.scale.x); EXPECT_DOUBLE_EQ(4.0, marker.scale.y); EXPECT_DOUBLE_EQ(4.0, marker.scale.z); EXPECT_DOUBLE_EQ(1.0, marker.pose.position.x); EXPECT_DOUBLE_EQ(2.0, marker.pose.position.y); EXPECT_DOUBLE_EQ(3.0, marker.pose.position.z); EXPECT_DOUBLE_EQ(0.0, marker.pose.orientation.x); EXPECT_DOUBLE_EQ(M_SQRT1_2, marker.pose.orientation.y); EXPECT_DOUBLE_EQ(0.0, marker.pose.orientation.z); EXPECT_DOUBLE_EQ(M_SQRT1_2, marker.pose.orientation.w); } TEST(Bodies, ConstructMarkerFromBodyBox) { const Eigen::Isometry3d pose = Eigen::Translation3d(1.0, 2.0, 3.0) * Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d(0.0, 1.0, 0.0)); const shapes::Shape* shape = new shapes::Box(1.0, 2.0, 3.0); const auto body = new Box(shape); body->setPose(pose); visualization_msgs::Marker marker; constructMarkerFromBody(body, marker); EXPECT_EQ(visualization_msgs::Marker::CUBE, marker.type); EXPECT_DOUBLE_EQ(1.0, marker.scale.x); EXPECT_DOUBLE_EQ(2.0, marker.scale.y); EXPECT_DOUBLE_EQ(3.0, marker.scale.z); EXPECT_DOUBLE_EQ(1.0, marker.pose.position.x); EXPECT_DOUBLE_EQ(2.0, marker.pose.position.y); EXPECT_DOUBLE_EQ(3.0, marker.pose.position.z); EXPECT_DOUBLE_EQ(0.0, marker.pose.orientation.x); EXPECT_DOUBLE_EQ(M_SQRT1_2, marker.pose.orientation.y); EXPECT_DOUBLE_EQ(0.0, marker.pose.orientation.z); EXPECT_DOUBLE_EQ(M_SQRT1_2, marker.pose.orientation.w); } TEST(Bodies, ConstructMarkerFromBodyCylinder) { const Eigen::Isometry3d pose = Eigen::Translation3d(1.0, 2.0, 3.0) * Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d(0.0, 1.0, 0.0)); const shapes::Shape* shape = new shapes::Cylinder(3.0, 2.0); const auto body = new Cylinder(shape); body->setPose(pose); visualization_msgs::Marker marker; constructMarkerFromBody(body, marker); EXPECT_EQ(visualization_msgs::Marker::CYLINDER, marker.type); EXPECT_DOUBLE_EQ(6.0, marker.scale.x); EXPECT_DOUBLE_EQ(6.0, marker.scale.y); EXPECT_DOUBLE_EQ(2.0, marker.scale.z); EXPECT_DOUBLE_EQ(1.0, marker.pose.position.x); EXPECT_DOUBLE_EQ(2.0, marker.pose.position.y); EXPECT_DOUBLE_EQ(3.0, marker.pose.position.z); EXPECT_DOUBLE_EQ(0.0, marker.pose.orientation.x); EXPECT_DOUBLE_EQ(M_SQRT1_2, marker.pose.orientation.y); EXPECT_DOUBLE_EQ(0.0, marker.pose.orientation.z); EXPECT_DOUBLE_EQ(M_SQRT1_2, marker.pose.orientation.w); } TEST(Bodies, ConstructMarkerFromBodyMesh) { const Eigen::Isometry3d pose = Eigen::Translation3d(1.0, 2.0, 3.0) * Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d(0.0, 1.0, 0.0)); shapes::Mesh* shape = shapes::createMeshFromResource("file://" + (boost::filesystem::path(TEST_RESOURCES_DIR) / "/box.dae").string()); const auto body = new ConvexMesh(shape); body->setPose(pose); visualization_msgs::Marker marker; constructMarkerFromBody(body, marker); EXPECT_EQ(visualization_msgs::Marker::TRIANGLE_LIST, marker.type); EXPECT_DOUBLE_EQ(1.0, marker.scale.x); EXPECT_DOUBLE_EQ(1.0, marker.scale.y); EXPECT_DOUBLE_EQ(1.0, marker.scale.z); EXPECT_DOUBLE_EQ(1.0, marker.pose.position.x); EXPECT_DOUBLE_EQ(2.0, marker.pose.position.y); EXPECT_DOUBLE_EQ(3.0, marker.pose.position.z); EXPECT_DOUBLE_EQ(0.0, marker.pose.orientation.x); EXPECT_DOUBLE_EQ(M_SQRT1_2, marker.pose.orientation.y); EXPECT_DOUBLE_EQ(0.0, marker.pose.orientation.z); EXPECT_DOUBLE_EQ(M_SQRT1_2, marker.pose.orientation.w); // We can't directly use shape->triangles because after passing it to the body constructor, // it can "optimize" the vertices (i.e. swap normals etc.) or reorder them const auto shapeFromBody = std::dynamic_pointer_cast(constructShapeFromBody(body)); EigenSTL::vector_Vector3d shapeVertices, markerVertices; for (size_t t = 0; t < shapeFromBody->triangle_count * 3; ++t) { const auto vertexId = shapeFromBody->triangles[t]; shapeVertices.push_back({ shapeFromBody->vertices[3 * vertexId + 0], shapeFromBody->vertices[3 * vertexId + 1], shapeFromBody->vertices[3 * vertexId + 2] }); } for (auto& point : marker.points) markerVertices.push_back({ point.x, point.y, point.z }); expectVector3dSetsEqual(shapeVertices, markerVertices); } int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } geometric_shapes-0.7.3/test/test_bounding_box.cpp000066400000000000000000000274531405115746100222600ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2019, Open Robotics. * 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 the Open Robotics 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. *********************************************************************/ /** \Author Martin Pecka */ #include #include #include #include // The magic numbers in this test were verified visually using Blender TEST(SphereBoundingBox, Sphere1) { shapes::Sphere shape(1.0); bodies::Sphere body(&shape); bodies::AABB bbox; body.computeBoundingBox(bbox); EXPECT_NEAR(-1.0, bbox.min().x(), 1e-4); EXPECT_NEAR(-1.0, bbox.min().y(), 1e-4); EXPECT_NEAR(-1.0, bbox.min().z(), 1e-4); EXPECT_NEAR(1.0, bbox.max().x(), 1e-4); EXPECT_NEAR(1.0, bbox.max().y(), 1e-4); EXPECT_NEAR(1.0, bbox.max().z(), 1e-4); } TEST(SphereBoundingBox, Sphere2) { shapes::Sphere shape(2.0); bodies::Sphere body(&shape); Eigen::Isometry3d pose; pose.setIdentity(); pose.translation() = Eigen::Vector3d(1, 2, 3); body.setPose(pose); bodies::AABB bbox; body.computeBoundingBox(bbox); EXPECT_NEAR(-1.0, bbox.min().x(), 1e-4); EXPECT_NEAR(0.0, bbox.min().y(), 1e-4); EXPECT_NEAR(1.0, bbox.min().z(), 1e-4); EXPECT_NEAR(3.0, bbox.max().x(), 1e-4); EXPECT_NEAR(4.0, bbox.max().y(), 1e-4); EXPECT_NEAR(5.0, bbox.max().z(), 1e-4); pose *= Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d(1, 1, 1).normalized()); body.setPose(pose); body.computeBoundingBox(bbox); EXPECT_NEAR(-1.0, bbox.min().x(), 1e-4); EXPECT_NEAR(0.0, bbox.min().y(), 1e-4); EXPECT_NEAR(1.0, bbox.min().z(), 1e-4); EXPECT_NEAR(3.0, bbox.max().x(), 1e-4); EXPECT_NEAR(4.0, bbox.max().y(), 1e-4); EXPECT_NEAR(5.0, bbox.max().z(), 1e-4); // verify the bounding box is rotation-invariant random_numbers::RandomNumberGenerator gen; double quatData[4]; Eigen::Quaterniond quat; for (size_t i = 0; i < 10; ++i) { gen.quaternion(quatData); quat.x() = quatData[0]; quat.y() = quatData[1]; quat.z() = quatData[2]; quat.w() = quatData[3]; pose.linear() = quat.toRotationMatrix(); body.setPose(pose); bodies::AABB bbox2; body.computeBoundingBox(bbox2); EXPECT_NEAR(bbox2.min().x(), bbox.min().x(), 1e-4); EXPECT_NEAR(bbox2.min().y(), bbox.min().y(), 1e-4); EXPECT_NEAR(bbox2.min().z(), bbox.min().z(), 1e-4); EXPECT_NEAR(bbox2.max().x(), bbox.max().x(), 1e-4); EXPECT_NEAR(bbox2.max().y(), bbox.max().y(), 1e-4); EXPECT_NEAR(bbox2.max().z(), bbox.max().z(), 1e-4); } } TEST(BoxBoundingBox, Box1) { shapes::Box shape(1.0, 2.0, 3.0); bodies::Box body(&shape); bodies::AABB bbox; body.computeBoundingBox(bbox); EXPECT_NEAR(-0.5, bbox.min().x(), 1e-4); EXPECT_NEAR(-1.0, bbox.min().y(), 1e-4); EXPECT_NEAR(-1.5, bbox.min().z(), 1e-4); EXPECT_NEAR(0.5, bbox.max().x(), 1e-4); EXPECT_NEAR(1.0, bbox.max().y(), 1e-4); EXPECT_NEAR(1.5, bbox.max().z(), 1e-4); } TEST(BoxBoundingBox, Box2) { shapes::Box shape(1.0, 2.0, 3.0); bodies::Box body(&shape); Eigen::Isometry3d pose; pose.setIdentity(); pose.translation() = Eigen::Vector3d(1, 2, 3); body.setPose(pose); bodies::AABB bbox; body.computeBoundingBox(bbox); EXPECT_NEAR(0.5, bbox.min().x(), 1e-4); EXPECT_NEAR(1.0, bbox.min().y(), 1e-4); EXPECT_NEAR(1.5, bbox.min().z(), 1e-4); EXPECT_NEAR(1.5, bbox.max().x(), 1e-4); EXPECT_NEAR(3.0, bbox.max().y(), 1e-4); EXPECT_NEAR(4.5, bbox.max().z(), 1e-4); pose *= Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d(1, 1, 1).normalized()); body.setPose(pose); body.computeBoundingBox(bbox); EXPECT_NEAR(-0.7767, bbox.min().x(), 1e-4); EXPECT_NEAR(0.8452, bbox.min().y(), 1e-4); EXPECT_NEAR(1.4673, bbox.min().z(), 1e-4); EXPECT_NEAR(2.7767, bbox.max().x(), 1e-4); EXPECT_NEAR(3.1547, bbox.max().y(), 1e-4); EXPECT_NEAR(4.5326, bbox.max().z(), 1e-4); } TEST(CylinderBoundingBox, Cylinder1) { shapes::Cylinder shape(1.0, 2.0); bodies::Cylinder body(&shape); bodies::AABB bbox; body.computeBoundingBox(bbox); EXPECT_NEAR(-1.0, bbox.min().x(), 1e-4); EXPECT_NEAR(-1.0, bbox.min().y(), 1e-4); EXPECT_NEAR(-1.0, bbox.min().z(), 1e-4); EXPECT_NEAR(1.0, bbox.max().x(), 1e-4); EXPECT_NEAR(1.0, bbox.max().y(), 1e-4); EXPECT_NEAR(1.0, bbox.max().z(), 1e-4); } TEST(CylinderBoundingBox, Cylinder2) { shapes::Cylinder shape(1.0, 2.0); bodies::Cylinder body(&shape); Eigen::Isometry3d pose; pose.setIdentity(); pose.translation() = Eigen::Vector3d(1, 2, 3); body.setPose(pose); bodies::AABB bbox; body.computeBoundingBox(bbox); EXPECT_NEAR(0.0, bbox.min().x(), 1e-4); EXPECT_NEAR(1.0, bbox.min().y(), 1e-4); EXPECT_NEAR(2.0, bbox.min().z(), 1e-4); EXPECT_NEAR(2.0, bbox.max().x(), 1e-4); EXPECT_NEAR(3.0, bbox.max().y(), 1e-4); EXPECT_NEAR(4.0, bbox.max().z(), 1e-4); pose *= Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d(1, 1, 1).normalized()); body.setPose(pose); body.computeBoundingBox(bbox); EXPECT_NEAR(-0.3238, bbox.min().x(), 1e-4); EXPECT_NEAR(0.7862, bbox.min().y(), 1e-4); EXPECT_NEAR(1.7239, bbox.min().z(), 1e-4); EXPECT_NEAR(2.3238, bbox.max().x(), 1e-4); EXPECT_NEAR(3.2138, bbox.max().y(), 1e-4); EXPECT_NEAR(4.2761, bbox.max().z(), 1e-4); // verify the bounding box is yaw-invariant random_numbers::RandomNumberGenerator gen(0); const auto rollPitch = Eigen::AngleAxisd(1.0, Eigen::Vector3d::UnitX()) * Eigen::AngleAxisd(1.0, Eigen::Vector3d::UnitY()); pose.linear() = rollPitch.toRotationMatrix(); body.setPose(pose); body.computeBoundingBox(bbox); bodies::AABB bbox2; for (size_t i = 0; i < 10; ++i) { const auto angle = gen.uniformReal(-M_PI, M_PI); const auto yaw = Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitZ()); pose.linear() = (rollPitch * yaw).toRotationMatrix(); body.setPose(pose); body.computeBoundingBox(bbox2); EXPECT_NEAR(bbox2.min().x(), bbox.min().x(), 1e-4); EXPECT_NEAR(bbox2.min().y(), bbox.min().y(), 1e-4); EXPECT_NEAR(bbox2.min().z(), bbox.min().z(), 1e-4); EXPECT_NEAR(bbox2.max().x(), bbox.max().x(), 1e-4); EXPECT_NEAR(bbox2.max().y(), bbox.max().y(), 1e-4); EXPECT_NEAR(bbox2.max().z(), bbox.max().z(), 1e-4); } } shapes::Mesh* createBoxMesh(const Eigen::Vector3d& min, const Eigen::Vector3d& max) { shapes::Mesh* m = new shapes::Mesh(8, 12); m->vertices[3 * 0 + 0] = min.x(); m->vertices[3 * 0 + 1] = min.y(); m->vertices[3 * 0 + 2] = min.z(); m->vertices[3 * 1 + 0] = max.x(); m->vertices[3 * 1 + 1] = min.y(); m->vertices[3 * 1 + 2] = min.z(); m->vertices[3 * 2 + 0] = min.x(); m->vertices[3 * 2 + 1] = max.y(); m->vertices[3 * 2 + 2] = min.z(); m->vertices[3 * 3 + 0] = max.x(); m->vertices[3 * 3 + 1] = max.y(); m->vertices[3 * 3 + 2] = min.z(); m->vertices[3 * 4 + 0] = min.x(); m->vertices[3 * 4 + 1] = min.y(); m->vertices[3 * 4 + 2] = max.z(); m->vertices[3 * 5 + 0] = max.x(); m->vertices[3 * 5 + 1] = min.y(); m->vertices[3 * 5 + 2] = max.z(); m->vertices[3 * 6 + 0] = min.x(); m->vertices[3 * 6 + 1] = max.y(); m->vertices[3 * 6 + 2] = max.z(); m->vertices[3 * 7 + 0] = max.x(); m->vertices[3 * 7 + 1] = max.y(); m->vertices[3 * 7 + 2] = max.z(); m->triangles[3 * 0 + 0] = 0; m->triangles[3 * 0 + 1] = 1; m->triangles[3 * 0 + 2] = 2; m->triangles[3 * 1 + 0] = 1; m->triangles[3 * 1 + 1] = 3; m->triangles[3 * 1 + 2] = 2; m->triangles[3 * 2 + 0] = 5; m->triangles[3 * 2 + 1] = 4; m->triangles[3 * 2 + 2] = 6; m->triangles[3 * 3 + 0] = 5; m->triangles[3 * 3 + 1] = 6; m->triangles[3 * 3 + 2] = 7; m->triangles[3 * 4 + 0] = 1; m->triangles[3 * 4 + 1] = 5; m->triangles[3 * 4 + 2] = 3; m->triangles[3 * 5 + 0] = 5; m->triangles[3 * 5 + 1] = 7; m->triangles[3 * 5 + 2] = 3; m->triangles[3 * 6 + 0] = 4; m->triangles[3 * 6 + 1] = 0; m->triangles[3 * 6 + 2] = 2; m->triangles[3 * 7 + 0] = 4; m->triangles[3 * 7 + 1] = 2; m->triangles[3 * 7 + 2] = 6; m->triangles[3 * 8 + 0] = 2; m->triangles[3 * 8 + 1] = 3; m->triangles[3 * 8 + 2] = 6; m->triangles[3 * 9 + 0] = 3; m->triangles[3 * 9 + 1] = 7; m->triangles[3 * 9 + 2] = 6; m->triangles[3 * 10 + 0] = 1; m->triangles[3 * 10 + 1] = 0; m->triangles[3 * 10 + 2] = 4; m->triangles[3 * 11 + 0] = 1; m->triangles[3 * 11 + 1] = 4; m->triangles[3 * 11 + 2] = 5; return m; } TEST(MeshBoundingBox, Mesh1) { shapes::Mesh* m = createBoxMesh({ -1, -1, -1 }, { 1, 1, 1 }); bodies::ConvexMesh body(m); bodies::AABB bbox; body.computeBoundingBox(bbox); EXPECT_NEAR(-1.0, bbox.min().x(), 1e-4); EXPECT_NEAR(-1.0, bbox.min().y(), 1e-4); EXPECT_NEAR(-1.0, bbox.min().z(), 1e-4); EXPECT_NEAR(1.0, bbox.max().x(), 1e-4); EXPECT_NEAR(1.0, bbox.max().y(), 1e-4); EXPECT_NEAR(1.0, bbox.max().z(), 1e-4); delete m; } TEST(MeshBoundingBox, Mesh2) { shapes::Mesh* m = createBoxMesh({ -0.5, -1.0, -1.5 }, { 0.5, 1.0, 1.5 }); bodies::ConvexMesh body(m); Eigen::Isometry3d pose; pose.setIdentity(); pose.translation() = Eigen::Vector3d(1, 2, 3); body.setPose(pose); bodies::AABB bbox; body.computeBoundingBox(bbox); EXPECT_NEAR(0.5, bbox.min().x(), 1e-4); EXPECT_NEAR(1.0, bbox.min().y(), 1e-4); EXPECT_NEAR(1.5, bbox.min().z(), 1e-4); EXPECT_NEAR(1.5, bbox.max().x(), 1e-4); EXPECT_NEAR(3.0, bbox.max().y(), 1e-4); EXPECT_NEAR(4.5, bbox.max().z(), 1e-4); pose *= Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d(1, 1, 1).normalized()); body.setPose(pose); body.computeBoundingBox(bbox); EXPECT_NEAR(-0.7767, bbox.min().x(), 1e-4); EXPECT_NEAR(0.8452, bbox.min().y(), 1e-4); EXPECT_NEAR(1.4673, bbox.min().z(), 1e-4); EXPECT_NEAR(2.7767, bbox.max().x(), 1e-4); EXPECT_NEAR(3.1547, bbox.max().y(), 1e-4); EXPECT_NEAR(4.5326, bbox.max().z(), 1e-4); delete m; } TEST(MergeBoundingBoxes, Merge1) { std::vector boxes; boxes.emplace_back(Eigen::Vector3d(-1, -1, -1), Eigen::Vector3d(0, 0, 0)); boxes.emplace_back(Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(1, 1, 1)); bodies::AABB bbox; bodies::mergeBoundingBoxes(boxes, bbox); EXPECT_NEAR(-1.0, bbox.min().x(), 1e-4); EXPECT_NEAR(-1.0, bbox.min().y(), 1e-4); EXPECT_NEAR(-1.0, bbox.min().z(), 1e-4); EXPECT_NEAR(1.0, bbox.max().x(), 1e-4); EXPECT_NEAR(1.0, bbox.max().y(), 1e-4); EXPECT_NEAR(1.0, bbox.max().z(), 1e-4); } int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } geometric_shapes-0.7.3/test/test_bounding_cylinder.cpp000066400000000000000000000535071405115746100233000ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2019, Open Robotics. * 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 the Open Robotics 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. *********************************************************************/ /** \Author Martin Pecka */ #include #include #include #include // The magic numbers in this test were verified visually using Blender TEST(SphereBoundingCylinder, Sphere1) { shapes::Sphere shape(1.0); bodies::Sphere body(&shape); bodies::BoundingCylinder bcyl; body.computeBoundingCylinder(bcyl); EXPECT_NEAR(0.0, bcyl.pose.translation().x(), 1e-4); EXPECT_NEAR(0.0, bcyl.pose.translation().y(), 1e-4); EXPECT_NEAR(0.0, bcyl.pose.translation().z(), 1e-4); // orientation can be any EXPECT_NEAR(1.0, bcyl.radius, 1e-4); EXPECT_NEAR(2.0, bcyl.length, 1e-4); body.setScale(2.0); body.setPadding(1.0); body.computeBoundingCylinder(bcyl); EXPECT_NEAR(0.0, bcyl.pose.translation().x(), 1e-4); EXPECT_NEAR(0.0, bcyl.pose.translation().y(), 1e-4); EXPECT_NEAR(0.0, bcyl.pose.translation().z(), 1e-4); // orientation can be any EXPECT_NEAR(3.0, bcyl.radius, 1e-4); EXPECT_NEAR(6.0, bcyl.length, 1e-4); } TEST(SphereBoundingCylinder, Sphere2) { shapes::Sphere shape(2.0); bodies::Sphere body(&shape); Eigen::Isometry3d pose(Eigen::Isometry3d::TranslationType(1, 2, 3)); body.setPose(pose); bodies::BoundingCylinder bcyl; body.computeBoundingCylinder(bcyl); EXPECT_NEAR(1.0, bcyl.pose.translation().x(), 1e-4); EXPECT_NEAR(2.0, bcyl.pose.translation().y(), 1e-4); EXPECT_NEAR(3.0, bcyl.pose.translation().z(), 1e-4); // orientation can be any EXPECT_NEAR(2.0, bcyl.radius, 1e-4); EXPECT_NEAR(4.0, bcyl.length, 1e-4); // verify the bounding cylinder is rotation-invariant pose *= Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d(1, 1, 1).normalized()); body.setPose(pose); body.computeBoundingCylinder(bcyl); EXPECT_NEAR(1.0, bcyl.pose.translation().x(), 1e-4); EXPECT_NEAR(2.0, bcyl.pose.translation().y(), 1e-4); EXPECT_NEAR(3.0, bcyl.pose.translation().z(), 1e-4); // orientation can be any EXPECT_NEAR(2.0, bcyl.radius, 1e-4); EXPECT_NEAR(4.0, bcyl.length, 1e-4); random_numbers::RandomNumberGenerator gen; double quatData[4]; Eigen::Quaterniond quat; for (size_t i = 0; i < 10; ++i) { gen.quaternion(quatData); quat.x() = quatData[0]; quat.y() = quatData[1]; quat.z() = quatData[2]; quat.w() = quatData[3]; pose.linear() = quat.toRotationMatrix(); body.setPose(pose); bodies::BoundingCylinder bcyl2; body.computeBoundingCylinder(bcyl2); EXPECT_NEAR(1.0, bcyl2.pose.translation().x(), 1e-4); EXPECT_NEAR(2.0, bcyl2.pose.translation().y(), 1e-4); EXPECT_NEAR(3.0, bcyl2.pose.translation().z(), 1e-4); // orientation can be any EXPECT_NEAR(2.0, bcyl2.radius, 1e-4); EXPECT_NEAR(4.0, bcyl2.length, 1e-4); } } TEST(BoxBoundingCylinder, Box1) { shapes::Box shape(1.0, 2.0, 3.0); bodies::Box body(&shape); bodies::BoundingCylinder bcyl; body.computeBoundingCylinder(bcyl); EXPECT_NEAR(0.0, bcyl.pose.translation().x(), 1e-4); EXPECT_NEAR(0.0, bcyl.pose.translation().y(), 1e-4); EXPECT_NEAR(0.0, bcyl.pose.translation().z(), 1e-4); EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4); EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4); EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4); EXPECT_NEAR(1.0, Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4); EXPECT_NEAR(sqrt(0.5 * 0.5 + 1 * 1), bcyl.radius, 1e-4); EXPECT_NEAR(3.0, bcyl.length, 1e-4); body.setScale(2.0); body.setPadding(1.0); body.computeBoundingCylinder(bcyl); EXPECT_NEAR(0.0, bcyl.pose.translation().x(), 1e-4); EXPECT_NEAR(0.0, bcyl.pose.translation().y(), 1e-4); EXPECT_NEAR(0.0, bcyl.pose.translation().z(), 1e-4); EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4); EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4); EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4); EXPECT_NEAR(1.0, Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4); EXPECT_NEAR(sqrt(2 * 2 + 3 * 3), bcyl.radius, 1e-4); EXPECT_NEAR(8.0, bcyl.length, 1e-4); // test that the rotational axis of the cylinder sticks with the longest dimension shape = shapes::Box(2.0, 3.0, 1.0); body = bodies::Box(&shape); body.computeBoundingCylinder(bcyl); EXPECT_NEAR(0.0, bcyl.pose.translation().x(), 1e-4); EXPECT_NEAR(0.0, bcyl.pose.translation().y(), 1e-4); EXPECT_NEAR(0.0, bcyl.pose.translation().z(), 1e-4); EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4); EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4); EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4); EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4); EXPECT_NEAR(sqrt(0.5 * 0.5 + 1 * 1), bcyl.radius, 1e-4); EXPECT_NEAR(3.0, bcyl.length, 1e-4); body.setScale(2.0); body.setPadding(1.0); body.computeBoundingCylinder(bcyl); EXPECT_NEAR(0.0, bcyl.pose.translation().x(), 1e-4); EXPECT_NEAR(0.0, bcyl.pose.translation().y(), 1e-4); EXPECT_NEAR(0.0, bcyl.pose.translation().z(), 1e-4); EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4); EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4); EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4); EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4); EXPECT_NEAR(sqrt(2 * 2 + 3 * 3), bcyl.radius, 1e-4); EXPECT_NEAR(8.0, bcyl.length, 1e-4); shape = shapes::Box(3.0, 1.0, 2.0); body = bodies::Box(&shape); body.computeBoundingCylinder(bcyl); EXPECT_NEAR(0.0, bcyl.pose.translation().x(), 1e-4); EXPECT_NEAR(0.0, bcyl.pose.translation().y(), 1e-4); EXPECT_NEAR(0.0, bcyl.pose.translation().z(), 1e-4); EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4); EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4); EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4); EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4); EXPECT_NEAR(sqrt(0.5 * 0.5 + 1 * 1), bcyl.radius, 1e-4); EXPECT_NEAR(3.0, bcyl.length, 1e-4); body.setScale(2.0); body.setPadding(1.0); body.computeBoundingCylinder(bcyl); EXPECT_NEAR(0.0, bcyl.pose.translation().x(), 1e-4); EXPECT_NEAR(0.0, bcyl.pose.translation().y(), 1e-4); EXPECT_NEAR(0.0, bcyl.pose.translation().z(), 1e-4); EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4); EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4); EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4); EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4); EXPECT_NEAR(sqrt(2 * 2 + 3 * 3), bcyl.radius, 1e-4); EXPECT_NEAR(8.0, bcyl.length, 1e-4); } TEST(BoxBoundingCylinder, Box2) { shapes::Box shape(1.0, 2.0, 3.0); bodies::Box body(&shape); Eigen::Isometry3d pose; pose.setIdentity(); pose.translation() = Eigen::Vector3d(1, 2, 3); body.setPose(pose); bodies::BoundingCylinder bcyl; body.computeBoundingCylinder(bcyl); EXPECT_NEAR(1.0, bcyl.pose.translation().x(), 1e-4); EXPECT_NEAR(2.0, bcyl.pose.translation().y(), 1e-4); EXPECT_NEAR(3.0, bcyl.pose.translation().z(), 1e-4); EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4); EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4); EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4); EXPECT_NEAR(1.0, Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4); EXPECT_NEAR(sqrt(0.5 * 0.5 + 1 * 1), bcyl.radius, 1e-4); EXPECT_NEAR(3.0, bcyl.length, 1e-4); Eigen::AngleAxisd rot(M_PI_2, Eigen::Vector3d(1, 1, 1).normalized()); pose *= rot; body.setPose(pose); body.computeBoundingCylinder(bcyl); EXPECT_NEAR(1.0, bcyl.pose.translation().x(), 1e-4); EXPECT_NEAR(2.0, bcyl.pose.translation().y(), 1e-4); EXPECT_NEAR(3.0, bcyl.pose.translation().z(), 1e-4); EXPECT_NEAR(Eigen::Quaterniond(rot).x(), Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4); EXPECT_NEAR(Eigen::Quaterniond(rot).y(), Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4); EXPECT_NEAR(Eigen::Quaterniond(rot).z(), Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4); EXPECT_NEAR(Eigen::Quaterniond(rot).w(), Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4); EXPECT_NEAR(sqrt(0.5 * 0.5 + 1 * 1), bcyl.radius, 1e-4); EXPECT_NEAR(3.0, bcyl.length, 1e-4); } TEST(CylinderBoundingCylinder, Cylinder1) { shapes::Cylinder shape(1.0, 2.0); bodies::Cylinder body(&shape); bodies::BoundingCylinder bcyl; body.computeBoundingCylinder(bcyl); EXPECT_NEAR(0.0, bcyl.pose.translation().x(), 1e-4); EXPECT_NEAR(0.0, bcyl.pose.translation().y(), 1e-4); EXPECT_NEAR(0.0, bcyl.pose.translation().z(), 1e-4); EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4); EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4); EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4); EXPECT_NEAR(1.0, Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4); EXPECT_NEAR(1.0, bcyl.radius, 1e-4); EXPECT_NEAR(2.0, bcyl.length, 1e-4); body.setScale(2.0); body.setPadding(1.0); body.computeBoundingCylinder(bcyl); EXPECT_NEAR(0.0, bcyl.pose.translation().x(), 1e-4); EXPECT_NEAR(0.0, bcyl.pose.translation().y(), 1e-4); EXPECT_NEAR(0.0, bcyl.pose.translation().z(), 1e-4); EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4); EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4); EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4); EXPECT_NEAR(1.0, Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4); EXPECT_NEAR(3.0, bcyl.radius, 1e-4); EXPECT_NEAR(6.0, bcyl.length, 1e-4); } TEST(CylinderBoundingCylinder, Cylinder2) { shapes::Cylinder shape(1.0, 2.0); bodies::Cylinder body(&shape); Eigen::Isometry3d pose; pose.setIdentity(); pose.translation() = Eigen::Vector3d(1, 2, 3); body.setPose(pose); bodies::BoundingCylinder bcyl; body.computeBoundingCylinder(bcyl); EXPECT_NEAR(1.0, bcyl.pose.translation().x(), 1e-4); EXPECT_NEAR(2.0, bcyl.pose.translation().y(), 1e-4); EXPECT_NEAR(3.0, bcyl.pose.translation().z(), 1e-4); EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4); EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4); EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4); EXPECT_NEAR(1.0, Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4); EXPECT_NEAR(1.0, bcyl.radius, 1e-4); EXPECT_NEAR(2.0, bcyl.length, 1e-4); pose.linear() = Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d(1, 1, 1).normalized()).toRotationMatrix(); body.setPose(pose); body.computeBoundingCylinder(bcyl); EXPECT_NEAR(1.0, bcyl.pose.translation().x(), 1e-4); EXPECT_NEAR(2.0, bcyl.pose.translation().y(), 1e-4); EXPECT_NEAR(3.0, bcyl.pose.translation().z(), 1e-4); EXPECT_NEAR(Eigen::Quaterniond(pose.linear()).x(), Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4); EXPECT_NEAR(Eigen::Quaterniond(pose.linear()).y(), Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4); EXPECT_NEAR(Eigen::Quaterniond(pose.linear()).z(), Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4); EXPECT_NEAR(Eigen::Quaterniond(pose.linear()).w(), Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4); EXPECT_NEAR(1.0, bcyl.radius, 1e-4); EXPECT_NEAR(2.0, bcyl.length, 1e-4); } shapes::Mesh* createBoxMesh(const Eigen::Vector3d& min, const Eigen::Vector3d& max) { shapes::Mesh* m = new shapes::Mesh(8, 12); m->vertices[3 * 0 + 0] = min.x(); m->vertices[3 * 0 + 1] = min.y(); m->vertices[3 * 0 + 2] = min.z(); m->vertices[3 * 1 + 0] = max.x(); m->vertices[3 * 1 + 1] = min.y(); m->vertices[3 * 1 + 2] = min.z(); m->vertices[3 * 2 + 0] = min.x(); m->vertices[3 * 2 + 1] = max.y(); m->vertices[3 * 2 + 2] = min.z(); m->vertices[3 * 3 + 0] = max.x(); m->vertices[3 * 3 + 1] = max.y(); m->vertices[3 * 3 + 2] = min.z(); m->vertices[3 * 4 + 0] = min.x(); m->vertices[3 * 4 + 1] = min.y(); m->vertices[3 * 4 + 2] = max.z(); m->vertices[3 * 5 + 0] = max.x(); m->vertices[3 * 5 + 1] = min.y(); m->vertices[3 * 5 + 2] = max.z(); m->vertices[3 * 6 + 0] = min.x(); m->vertices[3 * 6 + 1] = max.y(); m->vertices[3 * 6 + 2] = max.z(); m->vertices[3 * 7 + 0] = max.x(); m->vertices[3 * 7 + 1] = max.y(); m->vertices[3 * 7 + 2] = max.z(); m->triangles[3 * 0 + 0] = 0; m->triangles[3 * 0 + 1] = 1; m->triangles[3 * 0 + 2] = 2; m->triangles[3 * 1 + 0] = 1; m->triangles[3 * 1 + 1] = 3; m->triangles[3 * 1 + 2] = 2; m->triangles[3 * 2 + 0] = 5; m->triangles[3 * 2 + 1] = 4; m->triangles[3 * 2 + 2] = 6; m->triangles[3 * 3 + 0] = 5; m->triangles[3 * 3 + 1] = 6; m->triangles[3 * 3 + 2] = 7; m->triangles[3 * 4 + 0] = 1; m->triangles[3 * 4 + 1] = 5; m->triangles[3 * 4 + 2] = 3; m->triangles[3 * 5 + 0] = 5; m->triangles[3 * 5 + 1] = 7; m->triangles[3 * 5 + 2] = 3; m->triangles[3 * 6 + 0] = 4; m->triangles[3 * 6 + 1] = 0; m->triangles[3 * 6 + 2] = 2; m->triangles[3 * 7 + 0] = 4; m->triangles[3 * 7 + 1] = 2; m->triangles[3 * 7 + 2] = 6; m->triangles[3 * 8 + 0] = 2; m->triangles[3 * 8 + 1] = 3; m->triangles[3 * 8 + 2] = 6; m->triangles[3 * 9 + 0] = 3; m->triangles[3 * 9 + 1] = 7; m->triangles[3 * 9 + 2] = 6; m->triangles[3 * 10 + 0] = 1; m->triangles[3 * 10 + 1] = 0; m->triangles[3 * 10 + 2] = 4; m->triangles[3 * 11 + 0] = 1; m->triangles[3 * 11 + 1] = 4; m->triangles[3 * 11 + 2] = 5; return m; } TEST(MeshBoundingCylinder, Mesh1) { shapes::Mesh* m = createBoxMesh({ -0.5, -1.0, -1.5 }, { 0.5, 1.0, 1.5 }); double radiusScaled; double lengthScaled; bodies::ConvexMesh body(m); bodies::BoundingCylinder bcyl; body.computeBoundingCylinder(bcyl); EXPECT_NEAR(0.0, bcyl.pose.translation().x(), 1e-4); EXPECT_NEAR(0.0, bcyl.pose.translation().y(), 1e-4); EXPECT_NEAR(0.0, bcyl.pose.translation().z(), 1e-4); EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4); EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4); EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4); EXPECT_NEAR(1.0, Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4); // for meshes without padding, the bounding cylinder should be tight EXPECT_NEAR(sqrt(0.5 * 0.5 + 1 * 1), bcyl.radius, 1e-4); EXPECT_NEAR(3.0, bcyl.length, 1e-4); body.setScale(2.0); body.setPadding(1.0); body.computeBoundingCylinder(bcyl); EXPECT_NEAR(0.0, bcyl.pose.translation().x(), 1e-4); EXPECT_NEAR(0.0, bcyl.pose.translation().y(), 1e-4); EXPECT_NEAR(0.0, bcyl.pose.translation().z(), 1e-4); EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4); EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4); EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4); EXPECT_NEAR(1.0, Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4); // computation of bounding cylinder with padding is only approximate; the approximation error in both radius and // length is anywhere from zero to padding_ (or 2*padding_ for length) radiusScaled = 2 * sqrt(0.5 * 0.5 + 1 * 1); EXPECT_LE(radiusScaled, bcyl.radius); EXPECT_GE(radiusScaled + 1.0, bcyl.radius); lengthScaled = 6.0; EXPECT_LE(lengthScaled, bcyl.length); EXPECT_GE(lengthScaled + 2 * 1.0, bcyl.length); delete m; // test that the rotational axis of the cylinder sticks with the longest dimension m = createBoxMesh({ -1.0, -1.5, -0.5 }, { 1.0, 1.5, 0.5 }); bodies::ConvexMesh body2(m); body2.computeBoundingCylinder(bcyl); EXPECT_NEAR(0.0, bcyl.pose.translation().x(), 1e-4); EXPECT_NEAR(0.0, bcyl.pose.translation().y(), 1e-4); EXPECT_NEAR(0.0, bcyl.pose.translation().z(), 1e-4); EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4); EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4); EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4); EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4); // for meshes without padding, the bounding cylinder should be tight EXPECT_NEAR(sqrt(0.5 * 0.5 + 1 * 1), bcyl.radius, 1e-4); EXPECT_NEAR(3.0, bcyl.length, 1e-4); body2.setScale(2.0); body2.setPadding(1.0); body2.computeBoundingCylinder(bcyl); EXPECT_NEAR(0.0, bcyl.pose.translation().x(), 1e-4); EXPECT_NEAR(0.0, bcyl.pose.translation().y(), 1e-4); EXPECT_NEAR(0.0, bcyl.pose.translation().z(), 1e-4); EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4); EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4); EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4); EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4); radiusScaled = 2 * sqrt(0.5 * 0.5 + 1 * 1); EXPECT_LE(radiusScaled, bcyl.radius); EXPECT_GE(radiusScaled + 1.0, bcyl.radius); lengthScaled = 6.0; EXPECT_LE(lengthScaled, bcyl.length); EXPECT_GE(lengthScaled + 2 * 1.0, bcyl.length); delete m; m = createBoxMesh({ -1.5, -0.5, -1.0 }, { 1.5, 0.5, 1.0 }); bodies::ConvexMesh body3(m); body3.computeBoundingCylinder(bcyl); EXPECT_NEAR(0.0, bcyl.pose.translation().x(), 1e-4); EXPECT_NEAR(0.0, bcyl.pose.translation().y(), 1e-4); EXPECT_NEAR(0.0, bcyl.pose.translation().z(), 1e-4); EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4); EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4); EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4); EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4); // for meshes without padding, the bounding cylinder should be tight EXPECT_NEAR(sqrt(0.5 * 0.5 + 1 * 1), bcyl.radius, 1e-4); EXPECT_NEAR(3.0, bcyl.length, 1e-4); body3.setScale(2.0); body3.setPadding(1.0); body3.computeBoundingCylinder(bcyl); EXPECT_NEAR(0.0, bcyl.pose.translation().x(), 1e-4); EXPECT_NEAR(0.0, bcyl.pose.translation().y(), 1e-4); EXPECT_NEAR(0.0, bcyl.pose.translation().z(), 1e-4); EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4); EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4); EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4); EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4); radiusScaled = 2 * sqrt(0.5 * 0.5 + 1 * 1); EXPECT_LE(radiusScaled, bcyl.radius); EXPECT_GE(radiusScaled + 1.0, bcyl.radius); lengthScaled = 6.0; EXPECT_LE(lengthScaled, bcyl.length); EXPECT_GE(lengthScaled + 2 * 1.0, bcyl.length); delete m; } TEST(MeshBoundingCylinder, Mesh2) { shapes::Mesh* m = createBoxMesh({ -0.5, -1.0, -1.5 }, { 0.5, 1.0, 1.5 }); bodies::ConvexMesh body(m); Eigen::Isometry3d pose; pose.setIdentity(); pose.translation() = Eigen::Vector3d(1, 2, 3); body.setPose(pose); bodies::BoundingCylinder bcyl; body.computeBoundingCylinder(bcyl); EXPECT_NEAR(1.0, bcyl.pose.translation().x(), 1e-4); EXPECT_NEAR(2.0, bcyl.pose.translation().y(), 1e-4); EXPECT_NEAR(3.0, bcyl.pose.translation().z(), 1e-4); EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4); EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4); EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4); EXPECT_NEAR(1.0, Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4); // for meshes without padding, the bounding cylinder should be tight EXPECT_NEAR(sqrt(0.5 * 0.5 + 1 * 1), bcyl.radius, 1e-4); EXPECT_NEAR(3.0, bcyl.length, 1e-4); Eigen::AngleAxisd rot(M_PI_2, Eigen::Vector3d(1, 1, 1).normalized()); pose *= rot; body.setPose(pose); body.computeBoundingCylinder(bcyl); EXPECT_NEAR(1.0, bcyl.pose.translation().x(), 1e-4); EXPECT_NEAR(2.0, bcyl.pose.translation().y(), 1e-4); EXPECT_NEAR(3.0, bcyl.pose.translation().z(), 1e-4); EXPECT_NEAR(Eigen::Quaterniond(rot).x(), Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4); EXPECT_NEAR(Eigen::Quaterniond(rot).y(), Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4); EXPECT_NEAR(Eigen::Quaterniond(rot).z(), Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4); EXPECT_NEAR(Eigen::Quaterniond(rot).w(), Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4); // for meshes without padding, the bounding cylinder should be tight EXPECT_NEAR(sqrt(0.5 * 0.5 + 1 * 1), bcyl.radius, 1e-4); EXPECT_NEAR(3.0, bcyl.length, 1e-4); delete m; } int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } geometric_shapes-0.7.3/test/test_bounding_sphere.cpp000066400000000000000000000166001405115746100227460ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2013, Willow Garage, Inc. * 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 the Willow Garage 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. *********************************************************************/ /** \Author Acorn Pooley */ #include #include #include #include #include TEST(SphereBoundingSphere, Sphere1) { shapes::Sphere shape(1.0); Eigen::Vector3d center; double radius; computeShapeBoundingSphere(&shape, center, radius); EXPECT_EQ(1.0, radius); EXPECT_EQ(0.0, center.x()); EXPECT_EQ(0.0, center.y()); EXPECT_EQ(0.0, center.z()); } TEST(SphereBoundingSphere, Sphere2) { shapes::Shape* shape = new shapes::Sphere(2.0); Eigen::Vector3d center; double radius; computeShapeBoundingSphere(shape, center, radius); EXPECT_EQ(2.0, radius); EXPECT_EQ(0.0, center.x()); EXPECT_EQ(0.0, center.y()); EXPECT_EQ(0.0, center.z()); delete shape; } TEST(BoxBoundingSphere, Box1) { shapes::Shape* shape = new shapes::Box(2.0, 4.0, 6.0); Eigen::Vector3d center; double radius; computeShapeBoundingSphere(shape, center, radius); EXPECT_EQ(sqrt(14.0), radius); EXPECT_EQ(0.0, center.x()); EXPECT_EQ(0.0, center.y()); EXPECT_EQ(0.0, center.z()); } TEST(BoxBoundingSphere, Box2) { shapes::Shape* shape = new shapes::Box(2.0, 2.0, 2.0); Eigen::Vector3d center; double radius; computeShapeBoundingSphere(shape, center, radius); EXPECT_EQ(sqrt(3.0), radius); EXPECT_EQ(0.0, center.x()); EXPECT_EQ(0.0, center.y()); EXPECT_EQ(0.0, center.z()); } TEST(CylBoundingSphere, Cyl1) { shapes::Shape* shape = new shapes::Cylinder(1.0, 4.0); Eigen::Vector3d center; double radius; computeShapeBoundingSphere(shape, center, radius); EXPECT_EQ(sqrt(1 + 4), radius); EXPECT_EQ(0.0, center.x()); EXPECT_EQ(0.0, center.y()); EXPECT_EQ(0.0, center.z()); } TEST(CylBoundingSphere, Cyl2) { shapes::Shape* shape = new shapes::Cylinder(2.0, 20.0); Eigen::Vector3d center; double radius; computeShapeBoundingSphere(shape, center, radius); EXPECT_EQ(sqrt(4 + 100), radius); EXPECT_EQ(0.0, center.x()); EXPECT_EQ(0.0, center.y()); EXPECT_EQ(0.0, center.z()); } TEST(ConeBoundingSphere, Cone1) { shapes::Shape* shape = new shapes::Cone(20.0, 2.0); Eigen::Vector3d center; double radius; computeShapeBoundingSphere(shape, center, radius); EXPECT_EQ(20.0, radius); EXPECT_EQ(0.0, center.x()); EXPECT_EQ(0.0, center.y()); EXPECT_EQ(-1.0, center.z()); } TEST(ConeBoundingSphere, Cone2) { shapes::Shape* shape = new shapes::Cone(5.0, 5.0); Eigen::Vector3d center; double radius; computeShapeBoundingSphere(shape, center, radius); EXPECT_EQ(5.0, radius); EXPECT_EQ(0.0, center.x()); EXPECT_EQ(0.0, center.y()); EXPECT_EQ(-2.5, center.z()); } TEST(ConeBoundingSphere, Cone3) { double height = 1.0 + 1.0 / sqrt(2); shapes::Shape* shape = new shapes::Cone(1 / sqrt(2), height); Eigen::Vector3d center; double radius; computeShapeBoundingSphere(shape, center, radius); EXPECT_EQ(1.0, radius); EXPECT_EQ(0.0, center.x()); EXPECT_EQ(0.0, center.y()); EXPECT_EQ(height / 2 - 1, center.z()); } TEST(ConeBoundingSphere, Cone4) { shapes::Shape* shape = new shapes::Cone(3, 10); Eigen::Vector3d center; double radius; computeShapeBoundingSphere(shape, center, radius); EXPECT_EQ(109.0 / 20, radius); EXPECT_EQ(0.0, center.x()); EXPECT_EQ(0.0, center.y()); EXPECT_EQ(5 - (109.0 / 20), center.z()); } TEST(MeshBoundingSphere, Mesh1) { shapes::Shape* shape = new shapes::Mesh(8, 12); EXPECT_EQ(shape->type, shapes::MESH); shapes::Mesh* m = static_cast(shape); // box mesh m->vertices[3 * 0 + 0] = 0; m->vertices[3 * 0 + 1] = 0; m->vertices[3 * 0 + 2] = 0; m->vertices[3 * 1 + 0] = 1; m->vertices[3 * 1 + 1] = 0; m->vertices[3 * 1 + 2] = 0; m->vertices[3 * 2 + 0] = 0; m->vertices[3 * 2 + 1] = 1; m->vertices[3 * 2 + 2] = 0; m->vertices[3 * 3 + 0] = 1; m->vertices[3 * 3 + 1] = 1; m->vertices[3 * 3 + 2] = 0; m->vertices[3 * 4 + 0] = 0; m->vertices[3 * 4 + 1] = 0; m->vertices[3 * 4 + 2] = 1; m->vertices[3 * 5 + 0] = 1; m->vertices[3 * 5 + 1] = 0; m->vertices[3 * 5 + 2] = 1; m->vertices[3 * 6 + 0] = 0; m->vertices[3 * 6 + 1] = 1; m->vertices[3 * 6 + 2] = 1; m->vertices[3 * 7 + 0] = 1; m->vertices[3 * 7 + 1] = 1; m->vertices[3 * 7 + 2] = 1; m->triangles[3 * 0 + 0] = 0; m->triangles[3 * 0 + 1] = 1; m->triangles[3 * 0 + 2] = 2; m->triangles[3 * 1 + 0] = 1; m->triangles[3 * 1 + 1] = 3; m->triangles[3 * 1 + 2] = 2; m->triangles[3 * 2 + 0] = 5; m->triangles[3 * 2 + 1] = 4; m->triangles[3 * 2 + 2] = 6; m->triangles[3 * 3 + 0] = 5; m->triangles[3 * 3 + 1] = 6; m->triangles[3 * 3 + 2] = 7; m->triangles[3 * 4 + 0] = 1; m->triangles[3 * 4 + 1] = 5; m->triangles[3 * 4 + 2] = 3; m->triangles[3 * 5 + 0] = 5; m->triangles[3 * 5 + 1] = 7; m->triangles[3 * 5 + 2] = 3; m->triangles[3 * 6 + 0] = 4; m->triangles[3 * 6 + 1] = 0; m->triangles[3 * 6 + 2] = 2; m->triangles[3 * 7 + 0] = 4; m->triangles[3 * 7 + 1] = 2; m->triangles[3 * 7 + 2] = 6; m->triangles[3 * 8 + 0] = 2; m->triangles[3 * 8 + 1] = 3; m->triangles[3 * 8 + 2] = 6; m->triangles[3 * 9 + 0] = 3; m->triangles[3 * 9 + 1] = 7; m->triangles[3 * 9 + 2] = 6; m->triangles[3 * 10 + 0] = 1; m->triangles[3 * 10 + 1] = 0; m->triangles[3 * 10 + 2] = 4; m->triangles[3 * 11 + 0] = 1; m->triangles[3 * 11 + 1] = 4; m->triangles[3 * 11 + 2] = 5; Eigen::Vector3d center; double radius; computeShapeBoundingSphere(shape, center, radius); EXPECT_EQ(sqrt(0.75), radius); EXPECT_EQ(0.5, center.x()); EXPECT_EQ(0.5, center.y()); EXPECT_EQ(0.5, center.z()); } int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } geometric_shapes-0.7.3/test/test_create_mesh.cpp000066400000000000000000000067321405115746100220570ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2016, Delft Robotics * 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 the copyright holder 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. *********************************************************************/ /** \Author Maarten de Vries */ #include "resources/config.h" #include #include #include namespace { void assertMesh(shapes::Mesh* mesh) { ASSERT_TRUE(mesh != nullptr); ASSERT_EQ(3u, mesh->vertex_count); ASSERT_EQ(1u, mesh->triangle_count); ASSERT_EQ(0u, mesh->triangles[0]); ASSERT_EQ(1u, mesh->triangles[1]); ASSERT_EQ(2u, mesh->triangles[2]); ASSERT_FLOAT_EQ(0, mesh->vertices[0 + 0]); ASSERT_FLOAT_EQ(0, mesh->vertices[0 + 1]); ASSERT_FLOAT_EQ(1, mesh->vertices[0 + 2]); ASSERT_FLOAT_EQ(0, mesh->vertices[3 + 0]); ASSERT_FLOAT_EQ(1, mesh->vertices[3 + 1]); ASSERT_FLOAT_EQ(0, mesh->vertices[3 + 2]); ASSERT_FLOAT_EQ(1, mesh->vertices[6 + 0]); ASSERT_FLOAT_EQ(0, mesh->vertices[6 + 1]); ASSERT_FLOAT_EQ(0, mesh->vertices[6 + 2]); } shapes::Mesh* loadMesh(const std::string& mesh) { std::string path = "file://" + std::string(TEST_RESOURCES_DIR) + "/" + mesh; return shapes::createMeshFromResource(path); } } // namespace TEST(CreateMesh, stl) { assertMesh(loadMesh("triangle.stl")); } TEST(CreateMesh, daeNoUp) { assertMesh(loadMesh("triangle_no_up.dae")); } TEST(CreateMesh, daeYUp) { assertMesh(loadMesh("triangle_y_up.dae")); } TEST(CreateMesh, daeZUp) { assertMesh(loadMesh("triangle_z_up.dae")); } TEST(CreateMesh, daeXUp) { assertMesh(loadMesh("triangle_x_up.dae")); } TEST(CreateMesh, daeNoUnit) { assertMesh(loadMesh("triangle_no_unit.dae")); } TEST(CreateMesh, dae1M) { assertMesh(loadMesh("triangle_1m.dae")); } TEST(CreateMesh, dae10M) { assertMesh(loadMesh("triangle_10m.dae")); } int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } geometric_shapes-0.7.3/test/test_loaded_meshes.cpp000066400000000000000000000155221405115746100223710ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2016, Jorge Santos * 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 the Willow Garage 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. *********************************************************************/ /** \Author Jorge Santos */ #include #include #include #include #include #include "resources/config.h" /** * Test fixture that generates meshes from the primitive shapes SPHERE, CYLINDER, CONE and BOX, * and load their twins from STL files. All the following tests are intended to verify that both * procedures produce equivalent meshes, and particularly that changes related to issue #38 don't * break mesh loading. */ class CompareMeshVsPrimitive : public ::testing::Test { public: CompareMeshVsPrimitive() { } void SetUp() override { // BOX shapes::Box box(1.0, 1.0, 1.0); shape_meshes.push_back(shapes::createMeshFromShape(&box)); loaded_meshes.push_back(shapes::createMeshFromResource( "file://" + (boost::filesystem::path(TEST_RESOURCES_DIR) / "/cube.stl").string())); shape_convex_meshes.push_back(new bodies::ConvexMesh(shape_meshes.back())); loaded_convex_meshes.push_back(new bodies::ConvexMesh(loaded_meshes.back())); } void TearDown() override { for (size_t i = 0; i < shape_meshes.size(); ++i) { delete shape_meshes[i]; delete loaded_meshes[i]; delete shape_convex_meshes[i]; delete loaded_convex_meshes[i]; } } ~CompareMeshVsPrimitive() override { } protected: random_numbers::RandomNumberGenerator rng; std::vector shape_meshes; std::vector loaded_meshes; std::vector shape_convex_meshes; std::vector loaded_convex_meshes; }; TEST_F(CompareMeshVsPrimitive, ContainsPoint) { // Any point inside a mesh must be inside the other too for (size_t i = 0; i < shape_meshes.size(); ++i) { bodies::Body* shape_cms = shape_convex_meshes[i]; bodies::Body* loaded_cms = loaded_convex_meshes[i]; Eigen::Vector3d p; bool found = false; for (int i = 0; i < 100; ++i) { if ((shape_cms->samplePointInside(rng, 10000, p)) || (loaded_cms->samplePointInside(rng, 10000, p))) { found = true; EXPECT_EQ(shape_cms->containsPoint(p), loaded_cms->containsPoint(p)); } } EXPECT_TRUE(found) << "No point inside the meshes was found (very unlikely)"; } } TEST_F(CompareMeshVsPrimitive, IntersectsRay) { // Random rays must intersect both meshes nearly at the same points for (size_t i = 0; i < shape_meshes.size(); ++i) { bodies::Body* shape_cms = shape_convex_meshes[i]; bodies::Body* loaded_cms = loaded_convex_meshes[i]; bool intersects = false; for (int i = 0; i < 100; ++i) { Eigen::Vector3d ray_o(rng.uniformReal(-1.0, +1.0), rng.uniformReal(-1.0, +1.0), rng.uniformReal(-1.0, +1.0)); Eigen::Vector3d ray_d(rng.uniformReal(-1.0, +1.0), rng.uniformReal(-1.0, +1.0), rng.uniformReal(-1.0, +1.0)); EigenSTL::vector_Vector3d vi1, vi2; shape_cms->intersectsRay(ray_o, ray_d, &vi1); loaded_cms->intersectsRay(ray_o, ray_d, &vi2); // DEBUG printing // if (vi1.size() != vi2.size() && vi1.size() > 0 && vi2.size() > 0) // { // std::cout << vi1.size() << " " << vi2.size() << "\n"; // std::cout << ray_o.x() << " "<< ray_o.y() << " "<< ray_o.z() // << "\n" << ray_d.x() << " "<< ray_d.y() << " "<< ray_d.z() << "\n"; // } EXPECT_EQ(vi1.size(), vi2.size()); if (!vi1.empty() && !vi2.empty()) { EXPECT_NEAR(vi1[0].x(), vi2[0].x(), 0.01); EXPECT_NEAR(vi1[0].y(), vi2[0].y(), 0.01); EXPECT_NEAR(vi1[0].z(), vi2[0].z(), 0.01); intersects = true; } } EXPECT_TRUE(intersects) << "No ray intersects the meshes (very unlikely)"; } } TEST_F(CompareMeshVsPrimitive, BoundingSphere) { // Bounding spheres must be nearly identical for (size_t i = 0; i < shape_meshes.size(); ++i) { shapes::Mesh* shape_ms = shape_meshes[i]; shapes::Mesh* loaded_ms = loaded_meshes[i]; shapes::Sphere shape(1.0); Eigen::Vector3d center1, center2; double radius1, radius2; computeShapeBoundingSphere(shape_ms, center1, radius1); computeShapeBoundingSphere(loaded_ms, center2, radius2); EXPECT_NEAR(radius1, radius2, 0.001); EXPECT_NEAR(center1.x(), center2.x(), 0.001); EXPECT_NEAR(center1.y(), center2.y(), 0.001); EXPECT_NEAR(center1.z(), center2.z(), 0.001); } } TEST_F(CompareMeshVsPrimitive, BoxVertexCount) { // For a simple shape as a cube, we expect that both meshes have the same number of vertex and triangles // But that was not the case before fixing issue #38! // These tests don't apply to curve shapes because the number of elements depends on how smooth they where // created. So ensure that "back()" gives a pointer to box meshes! EXPECT_EQ(shape_meshes.back()->vertex_count, loaded_meshes.back()->vertex_count); } TEST_F(CompareMeshVsPrimitive, BoxTriangleCount) { EXPECT_EQ(shape_meshes.back()->triangle_count, loaded_meshes.back()->triangle_count); } int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } geometric_shapes-0.7.3/test/test_point_inclusion.cpp000066400000000000000000000521631405115746100230130ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * 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 the Willow Garage 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. *********************************************************************/ /** \Author Ioan Sucan */ #include #include #include #include #include #include "resources/config.h" // We expect surface points are counted inside. #define EXPECT_SURF EXPECT_TRUE // split length into the largest number elem, such that sqrt(elem^2 + elem^2) <= length double largestComponentForLength2D(const double length) { // HACK: sqrt(2) / 2 is a problem due to rounding errors since the distance // is computed as 1.0000000000000002 . In such case we subtract an // epsilon. double sq2 = sqrt(length / 2); while (sq2 * sq2 + sq2 * sq2 > length) sq2 -= std::numeric_limits::epsilon(); return sq2; } Eigen::Isometry3d getRandomPose(random_numbers::RandomNumberGenerator& g) { const Eigen::Vector3d t(g.uniformReal(-100, 100), g.uniformReal(-100, 100), g.uniformReal(-100, 100)); double quat[4]; g.quaternion(quat); const Eigen::Quaterniond r({ quat[3], quat[0], quat[1], quat[2] }); return Eigen::Isometry3d::TranslationType(t) * r; } TEST(SpherePointContainment, Basic) { shapes::Sphere shape(1.0); bodies::Sphere sphere(&shape); // clang-format off // zero EXPECT_TRUE( sphere.containsPoint(Eigen::Vector3d(0.00, 0.00, 0.00))); // general point outside EXPECT_FALSE(sphere.containsPoint(Eigen::Vector3d(1.00, 1.00, 1.00))); // near single-axis maximum EXPECT_TRUE( sphere.containsPoint(Eigen::Vector3d(0.99, 0.00, 0.00))); EXPECT_SURF( sphere.containsPoint(Eigen::Vector3d(1.00, 0.00, 0.00))); EXPECT_FALSE(sphere.containsPoint(Eigen::Vector3d(1.01, 0.00, 0.00))); EXPECT_TRUE( sphere.containsPoint(Eigen::Vector3d(0.00, 0.99, 0.00))); EXPECT_SURF( sphere.containsPoint(Eigen::Vector3d(0.00, 1.00, 0.00))); EXPECT_FALSE(sphere.containsPoint(Eigen::Vector3d(0.00, 1.01, 0.00))); EXPECT_TRUE( sphere.containsPoint(Eigen::Vector3d(0.00, 0.00, 0.99))); EXPECT_SURF( sphere.containsPoint(Eigen::Vector3d(0.00, 0.00, 1.00))); EXPECT_FALSE(sphere.containsPoint(Eigen::Vector3d(0.00, 0.00, 1.01))); // near two-axis maximum const double sq2e = largestComponentForLength2D(1.0); EXPECT_TRUE( sphere.containsPoint(Eigen::Vector3d(0.70, 0.70, 0.00))); EXPECT_SURF( sphere.containsPoint(Eigen::Vector3d(sq2e, sq2e, 0.00))); EXPECT_FALSE(sphere.containsPoint(Eigen::Vector3d(0.71, 0.71, 0.00))); EXPECT_TRUE( sphere.containsPoint(Eigen::Vector3d(0.70, 0.00, 0.70))); EXPECT_SURF( sphere.containsPoint(Eigen::Vector3d(sq2e, 0.00, sq2e))); EXPECT_FALSE(sphere.containsPoint(Eigen::Vector3d(0.71, 0.00, 0.71))); EXPECT_TRUE( sphere.containsPoint(Eigen::Vector3d(0.00, 0.70, 0.70))); EXPECT_SURF( sphere.containsPoint(Eigen::Vector3d(0.00, sq2e, sq2e))); EXPECT_FALSE(sphere.containsPoint(Eigen::Vector3d(0.00, 0.71, 0.71))); // near three-axis maximum const double sq3 = sqrt(3) / 3; EXPECT_TRUE( sphere.containsPoint(Eigen::Vector3d(0.57, 0.57, 0.57))); EXPECT_SURF( sphere.containsPoint(Eigen::Vector3d(sq3 , sq3 , sq3 ))); EXPECT_FALSE(sphere.containsPoint(Eigen::Vector3d(0.58, 0.58, 0.58))); // near three-axis maximum with translation Eigen::Isometry3d pose; pose.setIdentity(); pose.translation() = Eigen::Vector3d(1.0, 0.0, 0.0); sphere.setPose(pose); EXPECT_TRUE( sphere.containsPoint(Eigen::Vector3d(1.57, 0.57, 0.57))); EXPECT_SURF( sphere.containsPoint(Eigen::Vector3d(1+sq3,sq3 , sq3 ))); EXPECT_FALSE(sphere.containsPoint(Eigen::Vector3d(1.58, 0.58, 0.58))); pose.translation() = Eigen::Vector3d(0.0, 1.0, 0.0); sphere.setPose(pose); EXPECT_TRUE( sphere.containsPoint(Eigen::Vector3d(0.57, 1.57, 0.57))); EXPECT_SURF( sphere.containsPoint(Eigen::Vector3d(sq3 ,1+sq3, sq3 ))); EXPECT_FALSE(sphere.containsPoint(Eigen::Vector3d(0.58, 1.58, 0.58))); pose.translation() = Eigen::Vector3d(0.0, 0.0, 1.0); sphere.setPose(pose); EXPECT_TRUE( sphere.containsPoint(Eigen::Vector3d(0.57, 0.57, 1.57))); EXPECT_SURF( sphere.containsPoint(Eigen::Vector3d(sq3 , sq3, 1+sq3))); EXPECT_FALSE(sphere.containsPoint(Eigen::Vector3d(0.58, 0.58, 1.58))); // clang-format on } TEST(SpherePointContainment, SimpleInside) { shapes::Sphere shape(1.0); bodies::Body* sphere = new bodies::Sphere(&shape); sphere->setScale(1.05); EXPECT_TRUE(sphere->containsPoint(0, 0, 1.0)); random_numbers::RandomNumberGenerator r(0); Eigen::Vector3d p; for (int i = 0; i < 1000; ++i) { const Eigen::Isometry3d pos = getRandomPose(r); sphere->setPose(pos); sphere->setScale(r.uniformReal(0.1, 100.0)); sphere->setPadding(r.uniformReal(-0.001, 10.0)); EXPECT_TRUE(sphere->samplePointInside(r, 100, p)); EXPECT_TRUE(sphere->containsPoint(p)); } delete sphere; } TEST(SpherePointContainment, SimpleOutside) { shapes::Sphere shape(1.0); bodies::Body* sphere = new bodies::Sphere(&shape); sphere->setScale(0.95); bool contains = sphere->containsPoint(0, 0, 1.0); delete sphere; EXPECT_FALSE(contains); } TEST(SpherePointContainment, ComplexInside) { shapes::Sphere shape(1.0); bodies::Body* sphere = new bodies::Sphere(&shape); sphere->setScale(0.95); Eigen::Isometry3d pose; pose.setIdentity(); pose.translation() = Eigen::Vector3d(1.0, 1.0, 1.0); sphere->setPose(pose); bool contains = sphere->containsPoint(0.5, 1, 1.0); delete sphere; EXPECT_TRUE(contains); } TEST(SpherePointContainment, ComplexOutside) { shapes::Sphere shape(1.0); bodies::Body* sphere = new bodies::Sphere(&shape); sphere->setScale(0.95); Eigen::Isometry3d pose; pose.setIdentity(); pose.translation() = Eigen::Vector3d(1.0, 1.0, 1.0); sphere->setPose(pose); bool contains = sphere->containsPoint(0.5, 0.0, 0.0); delete sphere; EXPECT_FALSE(contains); } TEST(BoxPointContainment, Basic) { shapes::Box shape(2.0, 2.0, 2.0); bodies::Box box(&shape); // clang-format off // zero EXPECT_TRUE( box.containsPoint(Eigen::Vector3d(0.00, 0.00, 0.00))); // general point outside EXPECT_FALSE(box.containsPoint(Eigen::Vector3d(2.00, 2.00, 2.00))); // near single-axis maximum EXPECT_TRUE( box.containsPoint(Eigen::Vector3d(0.99, 0.00, 0.00))); EXPECT_SURF( box.containsPoint(Eigen::Vector3d(1.00, 0.00, 0.00))); EXPECT_FALSE(box.containsPoint(Eigen::Vector3d(1.01, 0.00, 0.00))); EXPECT_TRUE( box.containsPoint(Eigen::Vector3d(0.00, 0.99, 0.00))); EXPECT_SURF( box.containsPoint(Eigen::Vector3d(0.00, 1.00, 0.00))); EXPECT_FALSE(box.containsPoint(Eigen::Vector3d(0.00, 1.01, 0.00))); EXPECT_TRUE( box.containsPoint(Eigen::Vector3d(0.00, 0.00, 0.99))); EXPECT_SURF( box.containsPoint(Eigen::Vector3d(0.00, 0.00, 1.00))); EXPECT_FALSE(box.containsPoint(Eigen::Vector3d(0.00, 0.00, 1.01))); // near two-axis maximum EXPECT_TRUE( box.containsPoint(Eigen::Vector3d(0.99, 0.99, 0.00))); EXPECT_SURF( box.containsPoint(Eigen::Vector3d(1.00, 1.00, 0.00))); EXPECT_FALSE(box.containsPoint(Eigen::Vector3d(1.01, 1.01, 0.00))); EXPECT_TRUE( box.containsPoint(Eigen::Vector3d(0.99, 0.00, 0.99))); EXPECT_SURF( box.containsPoint(Eigen::Vector3d(1.00, 0.00, 1.00))); EXPECT_FALSE(box.containsPoint(Eigen::Vector3d(1.01, 0.00, 1.01))); EXPECT_TRUE( box.containsPoint(Eigen::Vector3d(0.00, 0.99, 0.99))); EXPECT_SURF( box.containsPoint(Eigen::Vector3d(0.00, 1.00, 1.00))); EXPECT_FALSE(box.containsPoint(Eigen::Vector3d(0.00, 1.01, 1.01))); // near three-axis maximum EXPECT_TRUE( box.containsPoint(Eigen::Vector3d(0.99, 0.99, 0.99))); EXPECT_SURF( box.containsPoint(Eigen::Vector3d(1.00, 1.00, 1.00))); EXPECT_FALSE(box.containsPoint(Eigen::Vector3d(1.01, 1.01, 1.01))); // near three-axis maximum with translation Eigen::Isometry3d pose; pose.setIdentity(); pose.translation() = Eigen::Vector3d(1.0, 0.0, 0.0); box.setPose(pose); EXPECT_TRUE( box.containsPoint(Eigen::Vector3d(1.99, 0.99, 0.99))); EXPECT_SURF( box.containsPoint(Eigen::Vector3d(2.00, 1.00, 1.00))); EXPECT_FALSE(box.containsPoint(Eigen::Vector3d(2.01, 1.01, 1.01))); pose.translation() = Eigen::Vector3d(0.0, 1.0, 0.0); box.setPose(pose); EXPECT_TRUE( box.containsPoint(Eigen::Vector3d(0.99, 1.99, 0.99))); EXPECT_SURF( box.containsPoint(Eigen::Vector3d(1.00, 2.00, 1.00))); EXPECT_FALSE(box.containsPoint(Eigen::Vector3d(1.01, 2.01, 1.01))); pose.translation() = Eigen::Vector3d(0.0, 0.0, 1.0); box.setPose(pose); EXPECT_TRUE( box.containsPoint(Eigen::Vector3d(0.99, 0.99, 1.99))); EXPECT_SURF( box.containsPoint(Eigen::Vector3d(1.00, 1.00, 2.00))); EXPECT_FALSE(box.containsPoint(Eigen::Vector3d(1.01, 1.01, 2.01))); // clang-format on } TEST(BoxPointContainment, SimpleInside) { shapes::Box shape(1.0, 2.0, 3.0); bodies::Body* box = new bodies::Box(&shape); box->setScale(0.95); bool contains = box->containsPoint(0, 0, 1.0); EXPECT_TRUE(contains); random_numbers::RandomNumberGenerator r; Eigen::Vector3d p; EXPECT_TRUE(box->samplePointInside(r, 100, p)); EXPECT_TRUE(box->containsPoint(p)); delete box; } TEST(BoxPointContainment, SimpleOutside) { shapes::Box shape(1.0, 2.0, 3.0); bodies::Body* box = new bodies::Box(&shape); box->setScale(0.95); bool contains = box->containsPoint(0, 0, 3.0); delete box; EXPECT_FALSE(contains); } TEST(BoxPointContainment, ComplexInside) { shapes::Box shape(1.0, 1.0, 1.0); bodies::Body* box = new bodies::Box(&shape); box->setScale(1.01); Eigen::Isometry3d pose(Eigen::AngleAxisd(M_PI / 3.0, Eigen::Vector3d::UnitX())); pose.translation() = Eigen::Vector3d(1.0, 1.0, 1.0); box->setPose(pose); bool contains = box->containsPoint(1.5, 1.0, 1.5); EXPECT_TRUE(contains); delete box; } TEST(BoxPointContainment, Sampled) { shapes::Box shape(1.0, 2.0, 3.0); bodies::Box box(&shape); random_numbers::RandomNumberGenerator r(0); Eigen::Vector3d p; for (int i = 0; i < 1000; ++i) { const Eigen::Isometry3d pos = getRandomPose(r); box.setPose(pos); box.setScale(r.uniformReal(0.1, 100.0)); box.setPadding(r.uniformReal(-0.001, 10.0)); EXPECT_TRUE(box.samplePointInside(r, 100, p)); EXPECT_TRUE(box.containsPoint(p)); } } TEST(BoxPointContainment, ComplexOutside) { shapes::Box shape(1.0, 1.0, 1.0); bodies::Body* box = new bodies::Box(&shape); box->setScale(1.01); Eigen::Isometry3d pose(Eigen::AngleAxisd(M_PI / 3.0, Eigen::Vector3d::UnitX())); pose.translation() = Eigen::Vector3d(1.0, 1.0, 1.0); box->setPose(pose); bool contains = box->containsPoint(1.5, 1.5, 1.5); delete box; EXPECT_FALSE(contains); } TEST(CylinderPointContainment, Basic) { shapes::Cylinder shape(1.0, 4.0); bodies::Cylinder cylinder(&shape); // clang-format off // zero EXPECT_TRUE( cylinder.containsPoint(Eigen::Vector3d(0.00, 0.00, 0.00))); // general point outside EXPECT_FALSE(cylinder.containsPoint(Eigen::Vector3d(1.00, 1.00, 4.00))); // near single-axis maximum EXPECT_TRUE( cylinder.containsPoint(Eigen::Vector3d(0.99, 0.00, 0.00))); EXPECT_SURF( cylinder.containsPoint(Eigen::Vector3d(1.00, 0.00, 0.00))); EXPECT_FALSE(cylinder.containsPoint(Eigen::Vector3d(1.01, 0.00, 0.00))); EXPECT_TRUE( cylinder.containsPoint(Eigen::Vector3d(0.00, 0.99, 0.00))); EXPECT_SURF( cylinder.containsPoint(Eigen::Vector3d(0.00, 1.00, 0.00))); EXPECT_FALSE(cylinder.containsPoint(Eigen::Vector3d(0.00, 1.01, 0.00))); EXPECT_TRUE( cylinder.containsPoint(Eigen::Vector3d(0.00, 0.00, 1.99))); EXPECT_SURF( cylinder.containsPoint(Eigen::Vector3d(0.00, 0.00, 2.00))); EXPECT_FALSE(cylinder.containsPoint(Eigen::Vector3d(0.00, 0.00, 2.01))); // near two-axis maximum const double sq2 = sqrt(2) / 2; const double sq2e = largestComponentForLength2D(1); EXPECT_TRUE( cylinder.containsPoint(Eigen::Vector3d(0.70, 0.70, 0.00))); EXPECT_SURF( cylinder.containsPoint(Eigen::Vector3d(sq2e, sq2e, 0.00))); EXPECT_FALSE(cylinder.containsPoint(Eigen::Vector3d(0.71, 0.71, 0.00))); EXPECT_TRUE( cylinder.containsPoint(Eigen::Vector3d(0.99, 0.00, 1.99))); EXPECT_SURF( cylinder.containsPoint(Eigen::Vector3d(1.00, 0.00, 2.00))); EXPECT_FALSE(cylinder.containsPoint(Eigen::Vector3d(1.01, 0.00, 2.01))); EXPECT_TRUE( cylinder.containsPoint(Eigen::Vector3d(0.00, 0.99, 1.99))); EXPECT_SURF( cylinder.containsPoint(Eigen::Vector3d(0.00, 1.00, 2.00))); EXPECT_FALSE(cylinder.containsPoint(Eigen::Vector3d(0.00, 1.01, 2.01))); // near three-axis maximum EXPECT_TRUE( cylinder.containsPoint(Eigen::Vector3d(0.70, 0.70, 1.99))); EXPECT_SURF( cylinder.containsPoint(Eigen::Vector3d(sq2e, sq2e, 2.00))); EXPECT_FALSE(cylinder.containsPoint(Eigen::Vector3d(0.71, 0.71, 2.01))); // near three-axis maximum with translation Eigen::Isometry3d pose; pose.setIdentity(); pose.translation() = Eigen::Vector3d(1.0, 0.0, 0.0); cylinder.setPose(pose); EXPECT_TRUE( cylinder.containsPoint(Eigen::Vector3d(1.70, 0.70, 1.99))); EXPECT_SURF( cylinder.containsPoint(Eigen::Vector3d(1+sq2,sq2 , 2.00))); EXPECT_FALSE(cylinder.containsPoint(Eigen::Vector3d(1.71, 0.71, 2.01))); pose.translation() = Eigen::Vector3d(0.0, 1.0, 0.0); cylinder.setPose(pose); EXPECT_TRUE( cylinder.containsPoint(Eigen::Vector3d(0.70, 1.70, 1.99))); EXPECT_SURF( cylinder.containsPoint(Eigen::Vector3d(sq2 ,1+sq2, 2.00))); EXPECT_FALSE(cylinder.containsPoint(Eigen::Vector3d(0.71, 1.71, 2.01))); pose.translation() = Eigen::Vector3d(0.0, 0.0, 1.0); cylinder.setPose(pose); EXPECT_TRUE( cylinder.containsPoint(Eigen::Vector3d(0.70, 0.70, 2.99))); EXPECT_SURF( cylinder.containsPoint(Eigen::Vector3d(sq2e, sq2e, 3.00))); EXPECT_FALSE(cylinder.containsPoint(Eigen::Vector3d(0.71, 0.71, 3.01))); // clang-format on } TEST(CylinderPointContainment, SimpleInside) { shapes::Cylinder shape(1.0, 4.0); bodies::Body* cylinder = new bodies::Cylinder(&shape); cylinder->setScale(1.05); bool contains = cylinder->containsPoint(0, 0, 2.0); delete cylinder; EXPECT_TRUE(contains); } TEST(CylinderPointContainment, SimpleOutside) { shapes::Cylinder shape(1.0, 4.0); bodies::Body* cylinder = new bodies::Cylinder(&shape); cylinder->setScale(0.95); bool contains = cylinder->containsPoint(0, 0, 2.0); delete cylinder; EXPECT_FALSE(contains); } TEST(CylinderPointContainment, CylinderPadding) { shapes::Cylinder shape(1.0, 4.0); bodies::Body* cylinder = new bodies::Cylinder(&shape); bool contains = cylinder->containsPoint(0, 1.01, 0); EXPECT_FALSE(contains); cylinder->setPadding(.02); contains = cylinder->containsPoint(0, 1.01, 0); EXPECT_TRUE(contains); cylinder->setPadding(0.0); bodies::BoundingSphere bsphere; cylinder->computeBoundingSphere(bsphere); EXPECT_TRUE(bsphere.radius > 2.0); delete cylinder; } TEST(CylinderPointContainment, Sampled) { shapes::Cylinder shape(1.0, 4.0); bodies::Cylinder cylinder(&shape); random_numbers::RandomNumberGenerator r(0); Eigen::Vector3d p; for (int i = 0; i < 1000; ++i) { const Eigen::Isometry3d pos = getRandomPose(r); cylinder.setPose(pos); cylinder.setScale(r.uniformReal(0.1, 100.0)); cylinder.setPadding(r.uniformReal(-0.001, 10.0)); EXPECT_TRUE(cylinder.samplePointInside(r, 100, p)); EXPECT_TRUE(cylinder.containsPoint(p)); } } TEST(MeshPointContainment, Basic) { // clang-format off shapes::Mesh *ms = shapes::createMeshFromResource("file://" + (boost::filesystem::path(TEST_RESOURCES_DIR) / "/box.dae").string()); ASSERT_TRUE(ms != nullptr); bodies::ConvexMesh cubeMesh(ms); cubeMesh.setScale(1.5); cubeMesh.setPadding(0.5 * sqrt(3)); // zero EXPECT_TRUE( cubeMesh.containsPoint(Eigen::Vector3d(0.00, 0.00, 0.00))); // general point outside EXPECT_FALSE(cubeMesh.containsPoint(Eigen::Vector3d(3.00, 3.00, 3.00))); // near single-axis maximum EXPECT_TRUE( cubeMesh.containsPoint(Eigen::Vector3d(1.99, 0.00, 0.00))); EXPECT_SURF( cubeMesh.containsPoint(Eigen::Vector3d(2.00, 0.00, 0.00))); EXPECT_FALSE(cubeMesh.containsPoint(Eigen::Vector3d(2.01, 0.00, 0.00))); EXPECT_TRUE( cubeMesh.containsPoint(Eigen::Vector3d(0.00, 1.99, 0.00))); EXPECT_SURF( cubeMesh.containsPoint(Eigen::Vector3d(0.00, 2.00, 0.00))); EXPECT_FALSE(cubeMesh.containsPoint(Eigen::Vector3d(0.00, 2.01, 0.00))); EXPECT_TRUE( cubeMesh.containsPoint(Eigen::Vector3d(0.00, 0.00, 1.99))); EXPECT_SURF( cubeMesh.containsPoint(Eigen::Vector3d(0.00, 0.00, 2.00))); EXPECT_FALSE(cubeMesh.containsPoint(Eigen::Vector3d(0.00, 0.00, 2.01))); // near two-axis maximum EXPECT_TRUE( cubeMesh.containsPoint(Eigen::Vector3d(1.99, 1.99, 0.00))); EXPECT_SURF( cubeMesh.containsPoint(Eigen::Vector3d(2.00, 2.00, 0.00))); EXPECT_FALSE(cubeMesh.containsPoint(Eigen::Vector3d(2.01, 2.01, 0.00))); EXPECT_TRUE( cubeMesh.containsPoint(Eigen::Vector3d(1.99, 0.00, 1.99))); EXPECT_SURF( cubeMesh.containsPoint(Eigen::Vector3d(2.00, 0.00, 2.00))); EXPECT_FALSE(cubeMesh.containsPoint(Eigen::Vector3d(2.01, 0.00, 2.01))); EXPECT_TRUE( cubeMesh.containsPoint(Eigen::Vector3d(0.00, 1.99, 1.99))); EXPECT_SURF( cubeMesh.containsPoint(Eigen::Vector3d(0.00, 2.00, 2.00))); EXPECT_FALSE(cubeMesh.containsPoint(Eigen::Vector3d(0.00, 2.01, 2.01))); // near three-axis maximum EXPECT_TRUE( cubeMesh.containsPoint(Eigen::Vector3d(1.99, 1.99, 1.99))); EXPECT_SURF( cubeMesh.containsPoint(Eigen::Vector3d(2.00, 2.00, 2.00))); EXPECT_FALSE(cubeMesh.containsPoint(Eigen::Vector3d(2.01, 2.01, 2.01))); // near three-axis maximum with translation Eigen::Isometry3d pose; pose.setIdentity(); pose.translation() = Eigen::Vector3d(1.0, 0.0, 0.0); cubeMesh.setPose(pose); EXPECT_TRUE( cubeMesh.containsPoint(Eigen::Vector3d(2.99, 1.99, 1.99))); EXPECT_SURF( cubeMesh.containsPoint(Eigen::Vector3d(3.00, 2.00, 2.00))); EXPECT_FALSE(cubeMesh.containsPoint(Eigen::Vector3d(3.01, 2.01, 2.01))); pose.translation() = Eigen::Vector3d(0.0, 1.0, 0.0); cubeMesh.setPose(pose); EXPECT_TRUE( cubeMesh.containsPoint(Eigen::Vector3d(1.99, 2.99, 1.99))); EXPECT_SURF( cubeMesh.containsPoint(Eigen::Vector3d(2.00, 3.00, 2.00))); EXPECT_FALSE(cubeMesh.containsPoint(Eigen::Vector3d(2.01, 3.01, 2.01))); pose.translation() = Eigen::Vector3d(0.0, 0.0, 1.0); cubeMesh.setPose(pose); EXPECT_TRUE( cubeMesh.containsPoint(Eigen::Vector3d(1.99, 1.99, 2.99))); EXPECT_SURF( cubeMesh.containsPoint(Eigen::Vector3d(2.00, 2.00, 3.00))); EXPECT_FALSE(cubeMesh.containsPoint(Eigen::Vector3d(2.01, 2.01, 3.01))); // clang-format on delete ms; } TEST(MeshPointContainment, Pr2Forearm) { shapes::Mesh* ms = shapes::createMeshFromResource( "file://" + (boost::filesystem::path(TEST_RESOURCES_DIR) / "/forearm_roll.stl").string()); ASSERT_TRUE(ms != nullptr); bodies::Body* m = new bodies::ConvexMesh(ms); ASSERT_TRUE(m != nullptr); Eigen::Isometry3d t(Eigen::Isometry3d::Identity()); t.translation().x() = 1.0; EXPECT_FALSE(m->cloneAt(t)->containsPoint(-1.0, 0.0, 0.0)); random_numbers::RandomNumberGenerator r(0); Eigen::Vector3d p; bool found = true; for (int i = 0; i < 10; ++i) { if (m->samplePointInside(r, 10000, p)) { found = true; EXPECT_TRUE(m->containsPoint(p)); } } EXPECT_TRUE(found); delete m; delete ms; } TEST(MergeBoundingSpheres, MergeTwoSpheres) { std::vector spheres; bodies::BoundingSphere s1; s1.center = Eigen::Vector3d(5.0, 0.0, 0.0); s1.radius = 1.0; bodies::BoundingSphere s2; s2.center = Eigen::Vector3d(-5.1, 0.0, 0.0); s2.radius = 1.0; spheres.push_back(s1); spheres.push_back(s2); bodies::BoundingSphere merged_sphere; bodies::mergeBoundingSpheres(spheres, merged_sphere); EXPECT_NEAR(merged_sphere.center[0], -.05, .00001); EXPECT_EQ(merged_sphere.radius, 6.05); } int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } geometric_shapes-0.7.3/test/test_ray_intersection.cpp000066400000000000000000002030641405115746100231560ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2019, Open Robotics * 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 the Willow Garage 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. *********************************************************************/ /** \Author Ioan Sucan */ /** \Author Martin Pecka */ #include #include #include #include #include #include #include "resources/config.h" Eigen::Isometry3d getRandomPose(random_numbers::RandomNumberGenerator& g) { const Eigen::Vector3d t(g.uniformReal(-100, 100), g.uniformReal(-100, 100), g.uniformReal(-100, 100)); double quat[4]; g.quaternion(quat); const Eigen::Quaterniond r({ quat[3], quat[0], quat[1], quat[2] }); return Eigen::Isometry3d::TranslationType(t) * r; } #define EXPECT_VECTORS_EQUAL(v1, v2, error) \ EXPECT_NEAR((v1)[0], (v2)[0], (error)); \ EXPECT_NEAR((v1)[1], (v2)[1], (error)); \ EXPECT_NEAR((v1)[2], (v2)[2], (error)); #define CHECK_NO_INTERSECTION(body, origin, direction) \ { \ EigenSTL::vector_Vector3d intersections; \ Eigen::Vector3d o origin; \ Eigen::Vector3d d direction; \ const auto result = (body).intersectsRay(o, d, &intersections, 2); \ EXPECT_FALSE(result); \ EXPECT_EQ(0u, intersections.size()); \ } #define CHECK_INTERSECTS(body, origin, direction, numIntersections) \ { \ EigenSTL::vector_Vector3d intersections; \ Eigen::Vector3d o origin; \ Eigen::Vector3d d direction; \ const auto result = (body).intersectsRay(o, d, &intersections, 2); \ EXPECT_TRUE(result); \ EXPECT_EQ(static_cast((numIntersections)), intersections.size()); \ } #define CHECK_INTERSECTS_ONCE(body, origin, direction, intersection, error) \ { \ EigenSTL::vector_Vector3d intersections; \ Eigen::Vector3d o origin; \ Eigen::Vector3d d direction; \ Eigen::Vector3d i intersection; \ const auto result = (body).intersectsRay(o, d, &intersections, 2); \ EXPECT_TRUE(result); \ EXPECT_EQ(1u, intersections.size()); \ if (intersections.size() == 1u) \ { \ EXPECT_VECTORS_EQUAL(intersections.at(0), i, (error)); \ } \ } #define CHECK_INTERSECTS_TWICE(body, origin, direction, intersc1, intersc2, error) \ { \ EigenSTL::vector_Vector3d intersections; \ Eigen::Vector3d o origin; \ Eigen::Vector3d d direction; \ Eigen::Vector3d i1 intersc1; \ Eigen::Vector3d i2 intersc2; \ const auto result = (body).intersectsRay(o, d, &intersections, 2); \ EXPECT_TRUE(result); \ EXPECT_EQ(2u, intersections.size()); \ if (intersections.size() == 2u) \ { \ if (fabs(static_cast((intersections.at(0) - i1).norm())) < (error)) \ { \ EXPECT_VECTORS_EQUAL(intersections.at(0), i1, (error)); \ EXPECT_VECTORS_EQUAL(intersections.at(1), i2, (error)); \ } \ else \ { \ EXPECT_VECTORS_EQUAL(intersections.at(0), i2, (error)); \ EXPECT_VECTORS_EQUAL(intersections.at(1), i1, (error)); \ } \ } \ } TEST(SphereRayIntersection, OriginInside) { shapes::Sphere shape(1.0); bodies::Sphere sphere(&shape); // clang-format off CHECK_INTERSECTS_ONCE(sphere, (0, 0, 0), ( 1, 0, 0), ( 1, 0, 0), 1e-6) CHECK_INTERSECTS_ONCE(sphere, (0, 0, 0), (-1, 0, 0), (-1, 0, 0), 1e-6) CHECK_INTERSECTS_ONCE(sphere, (0, 0, 0), ( 0, 1, 0), ( 0, 1, 0), 1e-6) CHECK_INTERSECTS_ONCE(sphere, (0, 0, 0), ( 0, -1, 0), ( 0, -1, 0), 1e-6) CHECK_INTERSECTS_ONCE(sphere, (0, 0, 0), ( 0, 0, 1), ( 0, 0, 1), 1e-6) CHECK_INTERSECTS_ONCE(sphere, (0, 0, 0), ( 0, 0, -1), ( 0, 0, -1), 1e-6) // clang-format on // scaling sphere.setScale(1.1); // clang-format off CHECK_INTERSECTS_ONCE(sphere, (0, 0, 0), ( 1, 0, 0), ( 1.1, 0, 0), 1e-6) CHECK_INTERSECTS_ONCE(sphere, (0, 0, 0), (-1, 0, 0), (-1.1, 0, 0), 1e-6) CHECK_INTERSECTS_ONCE(sphere, (0, 0, 0), ( 0, 1, 0), ( 0, 1.1, 0), 1e-6) CHECK_INTERSECTS_ONCE(sphere, (0, 0, 0), ( 0, -1, 0), ( 0, -1.1, 0), 1e-6) CHECK_INTERSECTS_ONCE(sphere, (0, 0, 0), ( 0, 0, 1), ( 0, 0, 1.1), 1e-6) CHECK_INTERSECTS_ONCE(sphere, (0, 0, 0), ( 0, 0, -1), ( 0, 0, -1.1), 1e-6) // clang-format on // move origin within the sphere // clang-format off CHECK_INTERSECTS_ONCE(sphere, (0.5, 0 , 0 ), ( 1, 0, 0), ( 1.1, 0, 0), 1e-6) CHECK_INTERSECTS_ONCE(sphere, (0.5, 0 , 0 ), (-1, 0, 0), (-1.1, 0, 0), 1e-6) CHECK_INTERSECTS_ONCE(sphere, (0 , 0.5, 0 ), ( 0, 1, 0), ( 0, 1.1, 0), 1e-6) CHECK_INTERSECTS_ONCE(sphere, (0 , 0.5, 0 ), ( 0, -1, 0), ( 0, -1.1, 0), 1e-6) CHECK_INTERSECTS_ONCE(sphere, (0 , 0 , 0.5), ( 0, 0, 1), ( 0, 0, 1.1), 1e-6) CHECK_INTERSECTS_ONCE(sphere, (0 , 0 , 0.5), ( 0, 0, -1), ( 0, 0, -1.1), 1e-6) // clang-format on // move sphere Eigen::Isometry3d pose = Eigen::Translation3d(0.5, 0, 0) * Eigen::Quaterniond::Identity(); sphere.setPose(pose); // clang-format off CHECK_INTERSECTS_ONCE(sphere, (0, 0, 0), ( 1, 0, 0), ( 1.6, 0, 0), 1e-6) CHECK_INTERSECTS_ONCE(sphere, (0, 0, 0), (-1, 0, 0), (-0.6, 0, 0), 1e-6) // clang-format on pose = Eigen::Translation3d(0, 0.5, 0) * Eigen::Quaterniond::Identity(); sphere.setPose(pose); // clang-format off CHECK_INTERSECTS_ONCE(sphere, (0, 0, 0), ( 0, 1, 0), (0, 1.6, 0), 1e-6) CHECK_INTERSECTS_ONCE(sphere, (0, 0, 0), ( 0, -1, 0), (0, -0.6, 0), 1e-6) // clang-format on pose = Eigen::Translation3d(0, 0, 0.5) * Eigen::Quaterniond::Identity(); sphere.setPose(pose); // clang-format off CHECK_INTERSECTS_ONCE(sphere, (0, 0, 0), ( 0, 0, 1), (0, 0, 1.6), 1e-6) CHECK_INTERSECTS_ONCE(sphere, (0, 0, 0), ( 0, 0, -1), (0, 0, -0.6), 1e-6) // clang-format on // 3D diagonal sphere.setPose(Eigen::Isometry3d::Identity()); sphere.setScale(1.0); sphere.setPadding(0.1); const auto sq3 = sqrt(pow(1 + 0.1, 2) / 3); const auto dir3 = Eigen::Vector3d::Ones().normalized(); // clang-format off CHECK_INTERSECTS_ONCE(sphere, ( 0, 0, 0), ( dir3), ( sq3, sq3, sq3), 1e-4) // NOLINT(performance-unnecessary-copy-initialization) CHECK_INTERSECTS_ONCE(sphere, ( 0.5, 0.5, 0.5), ( dir3), ( sq3, sq3, sq3), 1e-4) // NOLINT(performance-unnecessary-copy-initialization) CHECK_INTERSECTS_ONCE(sphere, (-0.5, -0.5, -0.5), ( dir3), ( sq3, sq3, sq3), 1e-4) // NOLINT(performance-unnecessary-copy-initialization) CHECK_INTERSECTS_ONCE(sphere, ( 0, 0, 0), (-dir3), (-sq3, -sq3, -sq3), 1e-4) CHECK_INTERSECTS_ONCE(sphere, ( 0.5, 0.5, 0.5), (-dir3), (-sq3, -sq3, -sq3), 1e-4) CHECK_INTERSECTS_ONCE(sphere, (-0.5, -0.5, -0.5), (-dir3), (-sq3, -sq3, -sq3), 1e-4) // clang-format on // 2D diagonal const auto sq2 = sqrt(pow(1 + 0.1, 2) / 2); const auto dir2 = 1 / sqrt(2); // clang-format off CHECK_INTERSECTS_ONCE(sphere, ( 0, 0, 0), ( dir2, dir2, 0), ( sq2, sq2, 0), 1e-4) CHECK_INTERSECTS_ONCE(sphere, ( 0, 0, 0), (-dir2, -dir2, 0), (-sq2, -sq2, 0), 1e-4) CHECK_INTERSECTS_ONCE(sphere, ( 0.5, 0.5, 0), ( dir2, dir2, 0), ( sq2, sq2, 0), 1e-4) CHECK_INTERSECTS_ONCE(sphere, ( 0.5, 0.5, 0), (-dir2, -dir2, 0), (-sq2, -sq2, 0), 1e-4) CHECK_INTERSECTS_ONCE(sphere, (-0.5, -0.5, 0), ( dir2, dir2, 0), ( sq2, sq2, 0), 1e-4) CHECK_INTERSECTS_ONCE(sphere, (-0.5, -0.5, 0), (-dir2, -dir2, 0), (-sq2, -sq2, 0), 1e-4) CHECK_INTERSECTS_ONCE(sphere, ( 0, 0, 0), ( 0, dir2, dir2), ( 0, sq2, sq2), 1e-4) CHECK_INTERSECTS_ONCE(sphere, ( 0, 0, 0), ( 0, -dir2, -dir2), ( 0, -sq2, -sq2), 1e-4) CHECK_INTERSECTS_ONCE(sphere, ( 0, 0.5, 0.5), ( 0, dir2, dir2), ( 0, sq2, sq2), 1e-4) CHECK_INTERSECTS_ONCE(sphere, ( 0, 0.5, 0.5), ( 0, -dir2, -dir2), ( 0, -sq2, -sq2), 1e-4) CHECK_INTERSECTS_ONCE(sphere, ( 0, -0.5, -0.5), ( 0, dir2, dir2), ( 0, sq2, sq2), 1e-4) CHECK_INTERSECTS_ONCE(sphere, ( 0, -0.5, -0.5), ( 0, -dir2, -dir2), ( 0, -sq2, -sq2), 1e-4) CHECK_INTERSECTS_ONCE(sphere, ( 0, 0, 0), ( dir2, 0, dir2), ( sq2, 0, sq2), 1e-4) CHECK_INTERSECTS_ONCE(sphere, ( 0, 0, 0), (-dir2, 0, -dir2), (-sq2, 0, -sq2), 1e-4) CHECK_INTERSECTS_ONCE(sphere, ( 0.5, 0, 0.5), ( dir2, 0, dir2), ( sq2, 0, sq2), 1e-4) CHECK_INTERSECTS_ONCE(sphere, ( 0.5, 0, 0.5), (-dir2, 0, -dir2), (-sq2, 0, -sq2), 1e-4) CHECK_INTERSECTS_ONCE(sphere, (-0.5, 0, -0.5), ( dir2, 0, dir2), ( sq2, 0, sq2), 1e-4) CHECK_INTERSECTS_ONCE(sphere, (-0.5, 0, -0.5), (-dir2, 0, -dir2), (-sq2, 0, -sq2), 1e-4) // clang-format on // any random rays that start inside the sphere should have exactly one intersection with it random_numbers::RandomNumberGenerator g(0u); for (size_t i = 0; i < 1000; ++i) { const Eigen::Isometry3d pos = getRandomPose(g); sphere.setPose(pos); sphere.setScale(g.uniformReal(0.5, 100.0)); sphere.setPadding(g.uniformReal(-0.1, 100.0)); Eigen::Vector3d origin; sphere.samplePointInside(g, 10, origin); // get the scaled sphere bodies::BoundingSphere s; sphere.computeBoundingSphere(s); const Eigen::Vector3d dir = Eigen::Vector3d::Random().normalized(); EigenSTL::vector_Vector3d intersections; if (sphere.intersectsRay(origin, dir, &intersections, 2)) { EXPECT_EQ(1u, intersections.size()); if (intersections.size() == 1) { EXPECT_NEAR(s.radius, (s.center - intersections[0]).norm(), 1e-6); // verify the point lies on the ray EXPECT_NEAR(0.0, (Eigen::ParametrizedLine(origin, dir).distance(intersections[0])), 1e-6); EXPECT_LT(0.0, (intersections[0] - origin).normalized().dot(dir)); } } else { GTEST_NONFATAL_FAILURE_(("No intersection in iteration " + std::to_string(i)).c_str()); } } } TEST(SphereRayIntersection, OriginOutside) { shapes::Sphere shape(1.0); bodies::Sphere sphere(&shape); // clang-format off CHECK_INTERSECTS_TWICE(sphere, (-2, 0, 0), ( 1, 0, 0), ( 1, 0, 0), (-1, 0, 0), 1e-6) CHECK_INTERSECTS_TWICE(sphere, ( 2, 0, 0), (-1, 0, 0), (-1, 0, 0), ( 1, 0, 0), 1e-6) CHECK_INTERSECTS_TWICE(sphere, ( 0, -2, 0), ( 0, 1, 0), ( 0, 1, 0), ( 0, -1, 0), 1e-6) CHECK_INTERSECTS_TWICE(sphere, ( 0, 2, 0), ( 0, -1, 0), ( 0, -1, 0), ( 0, 1, 0), 1e-6) CHECK_INTERSECTS_TWICE(sphere, ( 0, 0, -2), ( 0, 0, 1), ( 0, 0, 1), ( 0, 0, -1), 1e-6) CHECK_INTERSECTS_TWICE(sphere, ( 0, 0, 2), ( 0, 0, -1), ( 0, 0, -1), ( 0, 0, 1), 1e-6) // clang-format on // test hitting the surface // clang-format off CHECK_INTERSECTS_ONCE(sphere, (-1, -1, 0), ( 0, 1, 0), (-1, 0, 0), 1e-6) CHECK_INTERSECTS_ONCE(sphere, (-1, 1, 0), ( 0, -1, 0), (-1, 0, 0), 1e-6) CHECK_INTERSECTS_ONCE(sphere, ( 1, -1, 0), ( 0, 1, 0), ( 1, 0, 0), 1e-6) CHECK_INTERSECTS_ONCE(sphere, ( 1, 1, 0), ( 0, -1, 0), ( 1, 0, 0), 1e-6) CHECK_INTERSECTS_ONCE(sphere, ( 0, -1, -1), ( 0, 0, 1), ( 0, -1, 0), 1e-6) CHECK_INTERSECTS_ONCE(sphere, ( 0, -1, 1), ( 0, 0, -1), ( 0, -1, 0), 1e-6) CHECK_INTERSECTS_ONCE(sphere, ( 0, 1, -1), ( 0, 0, 1), ( 0, 1, 0), 1e-6) CHECK_INTERSECTS_ONCE(sphere, ( 0, 1, 1), ( 0, 0, -1), ( 0, 1, 0), 1e-6) CHECK_INTERSECTS_ONCE(sphere, (-1, 0, -1), ( 1, 0, 0), ( 0, 0, -1), 1e-6) CHECK_INTERSECTS_ONCE(sphere, ( 1, 0, -1), (-1, 0, 0), ( 0, 0, -1), 1e-6) CHECK_INTERSECTS_ONCE(sphere, (-1, 0, 1), ( 1, 0, 0), ( 0, 0, 1), 1e-6) CHECK_INTERSECTS_ONCE(sphere, ( 1, 0, 1), (-1, 0, 0), ( 0, 0, 1), 1e-6) // clang-format on // test missing the surface // clang-format off CHECK_NO_INTERSECTION(sphere, (-1.1, -1, 0), ( 0, 1, 0)) CHECK_NO_INTERSECTION(sphere, (-1.1, 1, 0), ( 0, -1, 0)) CHECK_NO_INTERSECTION(sphere, ( 1.1, -1, 0), ( 0, 1, 0)) CHECK_NO_INTERSECTION(sphere, ( 1.1, 1, 0), ( 0, -1, 0)) CHECK_NO_INTERSECTION(sphere, ( 0, -1.1, -1), ( 0, 0, 1)) CHECK_NO_INTERSECTION(sphere, ( 0, -1.1, 1), ( 0, 0, -1)) CHECK_NO_INTERSECTION(sphere, ( 0, 1.1, -1), ( 0, 0, 1)) CHECK_NO_INTERSECTION(sphere, ( 0, 1.1, 1), ( 0, 0, -1)) CHECK_NO_INTERSECTION(sphere, ( -1, 0, -1.1), ( 1, 0, 0)) CHECK_NO_INTERSECTION(sphere, ( 1, 0, -1.1), (-1, 0, 0)) CHECK_NO_INTERSECTION(sphere, ( -1, 0, 1.1), ( 1, 0, 0)) CHECK_NO_INTERSECTION(sphere, ( 1, 0, 1.1), (-1, 0, 0)) // clang-format on // generate some random rays outside the sphere and test them random_numbers::RandomNumberGenerator g(0u); for (size_t i = 0; i < 1000; ++i) { const Eigen::Isometry3d pos = getRandomPose(g); sphere.setPose(pos); sphere.setScale(g.uniformReal(0.5, 100.0)); sphere.setPadding(g.uniformReal(-0.1, 100.0)); // choose a random direction const Eigen::Vector3d dir = Eigen::Vector3d::Random().normalized(); // get the scaled dimensions of the sphere bodies::BoundingSphere s; sphere.computeBoundingSphere(s); // create origin outside the sphere in the opposite direction than the chosen one const Eigen::Vector3d origin = s.center + -dir * 1.5 * s.radius; // a ray constructed in this way should intersect twice (through the sphere center) EigenSTL::vector_Vector3d intersections; if (sphere.intersectsRay(origin, dir, &intersections, 2)) { EXPECT_EQ(2u, intersections.size()); if (intersections.size() == 2) { EXPECT_NEAR(s.radius, (s.center - intersections[0]).norm(), 1e-4); EXPECT_NEAR(s.radius, (s.center - intersections[1]).norm(), 1e-4); // verify the point lies on the ray EXPECT_NEAR(0.0, (Eigen::ParametrizedLine(origin, dir).distance(intersections[0])), 1e-6); EXPECT_LT(0.0, (intersections[0] - origin).normalized().dot(dir)); EXPECT_NEAR(0.0, (Eigen::ParametrizedLine(origin, dir).distance(intersections[1])), 1e-6); EXPECT_LT(0.0, (intersections[1] - origin).normalized().dot(dir)); } } else { GTEST_NONFATAL_FAILURE_(("No intersection in iteration " + std::to_string(i)).c_str()); } // check that the opposite ray misses CHECK_NO_INTERSECTION(sphere, (origin), (-dir)) // NOLINT(performance-unnecessary-copy-initialization) // shift the ray a bit sideways // choose a perpendicular direction Eigen::Vector3d perpDir = dir.cross(Eigen::Vector3d({ dir.z(), dir.z(), -dir.x() - dir.y() })); if (perpDir.norm() < 1e-6) perpDir = dir.cross(Eigen::Vector3d({ -dir.y() - dir.z(), dir.x(), dir.x() })); perpDir.normalize(); // now move origin "sideways" but still only so much that the ray will hit the sphere const Eigen::Vector3d origin2 = origin + g.uniformReal(1e-6, s.radius - 1e-4) * perpDir; intersections.clear(); if (sphere.intersectsRay(origin2, dir, &intersections, 2)) { EXPECT_EQ(2u, intersections.size()); if (intersections.size() == 2) { EXPECT_NEAR(s.radius, (s.center - intersections[0]).norm(), 1e-4); EXPECT_NEAR(s.radius, (s.center - intersections[1]).norm(), 1e-4); // verify the point lies on the ray EXPECT_NEAR(0.0, (Eigen::ParametrizedLine(origin2, dir).distance(intersections[0])), 1e-6); EXPECT_LT(0.0, (intersections[0] - origin2).normalized().dot(dir)); EXPECT_NEAR(0.0, (Eigen::ParametrizedLine(origin2, dir).distance(intersections[1])), 1e-6); EXPECT_LT(0.0, (intersections[1] - origin2).normalized().dot(dir)); } } else { GTEST_NONFATAL_FAILURE_(("No intersection in iteration " + std::to_string(i)).c_str()); } } } TEST(SphereRayIntersection, SimpleRay1) { shapes::Sphere shape(1.0); bodies::Sphere sphere(&shape); sphere.setScale(1.05); CHECK_INTERSECTS_TWICE(sphere, (5, 0, 0), (-1, 0, 0), (1.05, 0, 0), (-1.05, 0, 0), 1e-6) } TEST(SphereRayIntersection, SimpleRay2) { shapes::Sphere shape(1.0); bodies::Sphere sphere(&shape); sphere.setScale(1.05); CHECK_NO_INTERSECTION(sphere, (5, 0, 0), (1, 0, 0)) } TEST(CylinderRayIntersection, OriginInside) { shapes::Cylinder shape(1.0, 2.0); bodies::Cylinder cylinder(&shape); // clang-format off CHECK_INTERSECTS_ONCE(cylinder, (0, 0, 0), ( 1, 0, 0), ( 1, 0, 0), 1e-6) CHECK_INTERSECTS_ONCE(cylinder, (0, 0, 0), (-1, 0, 0), (-1, 0, 0), 1e-6) CHECK_INTERSECTS_ONCE(cylinder, (0, 0, 0), ( 0, 1, 0), ( 0, 1, 0), 1e-6) CHECK_INTERSECTS_ONCE(cylinder, (0, 0, 0), ( 0, -1, 0), ( 0, -1, 0), 1e-6) CHECK_INTERSECTS_ONCE(cylinder, (0, 0, 0), ( 0, 0, 1), ( 0, 0, 1), 1e-6) CHECK_INTERSECTS_ONCE(cylinder, (0, 0, 0), ( 0, 0, -1), ( 0, 0, -1), 1e-6) // clang-format on // scaling cylinder.setScale(1.1); // clang-format off CHECK_INTERSECTS_ONCE(cylinder, (0, 0, 0), ( 1, 0, 0), ( 1.1, 0, 0), 1e-6) CHECK_INTERSECTS_ONCE(cylinder, (0, 0, 0), (-1, 0, 0), (-1.1, 0, 0), 1e-6) CHECK_INTERSECTS_ONCE(cylinder, (0, 0, 0), ( 0, 1, 0), ( 0, 1.1, 0), 1e-6) CHECK_INTERSECTS_ONCE(cylinder, (0, 0, 0), ( 0, -1, 0), ( 0, -1.1, 0), 1e-6) CHECK_INTERSECTS_ONCE(cylinder, (0, 0, 0), ( 0, 0, 1), ( 0, 0, 1.1), 1e-6) CHECK_INTERSECTS_ONCE(cylinder, (0, 0, 0), ( 0, 0, -1), ( 0, 0, -1.1), 1e-6) // clang-format on // move origin within the cylinder // clang-format off CHECK_INTERSECTS_ONCE(cylinder, (0.5, 0 , 0 ), ( 1, 0, 0), ( 1.1, 0, 0), 1e-6) CHECK_INTERSECTS_ONCE(cylinder, (0.5, 0 , 0 ), (-1, 0, 0), (-1.1, 0, 0), 1e-6) CHECK_INTERSECTS_ONCE(cylinder, (0 , 0.5, 0 ), ( 0, 1, 0), ( 0, 1.1, 0), 1e-6) CHECK_INTERSECTS_ONCE(cylinder, (0 , 0.5, 0 ), ( 0, -1, 0), ( 0, -1.1, 0), 1e-6) CHECK_INTERSECTS_ONCE(cylinder, (0 , 0 , 0.5), ( 0, 0, 1), ( 0, 0, 1.1), 1e-6) CHECK_INTERSECTS_ONCE(cylinder, (0 , 0 , 0.5), ( 0, 0, -1), ( 0, 0, -1.1), 1e-6) // clang-format on // move cylinder Eigen::Isometry3d pose = Eigen::Translation3d(0.5, 0, 0) * Eigen::Quaterniond::Identity(); cylinder.setPose(pose); // clang-format off CHECK_INTERSECTS_ONCE(cylinder, (0, 0, 0), ( 1, 0, 0), ( 1.6, 0, 0), 1e-6) CHECK_INTERSECTS_ONCE(cylinder, (0, 0, 0), (-1, 0, 0), (-0.6, 0, 0), 1e-6) // clang-format on pose = Eigen::Translation3d(0, 0.5, 0) * Eigen::Quaterniond::Identity(); cylinder.setPose(pose); // clang-format off CHECK_INTERSECTS_ONCE(cylinder, (0, 0, 0), ( 0, 1, 0), (0, 1.6, 0), 1e-6) CHECK_INTERSECTS_ONCE(cylinder, (0, 0, 0), ( 0, -1, 0), (0, -0.6, 0), 1e-6) // clang-format on pose = Eigen::Translation3d(0, 0, 0.5) * Eigen::Quaterniond::Identity(); cylinder.setPose(pose); // clang-format off CHECK_INTERSECTS_ONCE(cylinder, (0, 0, 0), ( 0, 0, 1), (0, 0, 1.6), 1e-6) CHECK_INTERSECTS_ONCE(cylinder, (0, 0, 0), ( 0, 0, -1), (0, 0, -0.6), 1e-6) // clang-format on // 3D diagonal cylinder.setPose(Eigen::Isometry3d::Identity()); cylinder.setScale(1.0); cylinder.setPadding(0.1); // diagonal distance to the base boundary const auto sq2 = sqrt(pow(1 + 0.1, 2) / 2); // direction towards the very "corner" of the cylinder const auto dir3 = Eigen::Vector3d({ sq2, sq2, 1.1 }).normalized(); // clang-format off CHECK_INTERSECTS_ONCE(cylinder, ( 0, 0, 0), ( dir3), ( sq2, sq2, 1.1), 1e-4) // NOLINT(performance-unnecessary-copy-initialization) CHECK_INTERSECTS_ONCE(cylinder, ( dir3 / 2), ( dir3), ( sq2, sq2, 1.1), 1e-4) // NOLINT(performance-unnecessary-copy-initialization) CHECK_INTERSECTS_ONCE(cylinder, ( -dir3 / 2), ( dir3), ( sq2, sq2, 1.1), 1e-4) // NOLINT(performance-unnecessary-copy-initialization) CHECK_INTERSECTS_ONCE(cylinder, ( 0, 0, 0), (-dir3), (-sq2, -sq2, -1.1), 1e-4) CHECK_INTERSECTS_ONCE(cylinder, ( dir3 / 2), (-dir3), (-sq2, -sq2, -1.1), 1e-4) CHECK_INTERSECTS_ONCE(cylinder, ( -dir3 / 2), (-dir3), (-sq2, -sq2, -1.1), 1e-4) // clang-format on // 2D diagonal // coordinate of the diagonal direction in the base const auto dir2 = 1 / sqrt(2); // clang-format off CHECK_INTERSECTS_ONCE(cylinder, ( 0, 0, 0), ( dir2, dir2, 0), ( sq2, sq2, 0), 1e-4) CHECK_INTERSECTS_ONCE(cylinder, ( 0, 0, 0), (-dir2, -dir2, 0), (-sq2, -sq2, 0), 1e-4) CHECK_INTERSECTS_ONCE(cylinder, ( 0.5, 0.5, 0), ( dir2, dir2, 0), ( sq2, sq2, 0), 1e-4) CHECK_INTERSECTS_ONCE(cylinder, ( 0.5, 0.5, 0), (-dir2, -dir2, 0), (-sq2, -sq2, 0), 1e-4) CHECK_INTERSECTS_ONCE(cylinder, (-0.5, -0.5, 0), ( dir2, dir2, 0), ( sq2, sq2, 0), 1e-4) CHECK_INTERSECTS_ONCE(cylinder, (-0.5, -0.5, 0), (-dir2, -dir2, 0), (-sq2, -sq2, 0), 1e-4) CHECK_INTERSECTS_ONCE(cylinder, ( 0, 0, 0), ( 0, dir2, dir2), ( 0, 1.1, 1.1), 1e-4) CHECK_INTERSECTS_ONCE(cylinder, ( 0, 0, 0), ( 0, -dir2, -dir2), ( 0, -1.1, -1.1), 1e-4) CHECK_INTERSECTS_ONCE(cylinder, ( 0, 0.5, 0.5), ( 0, dir2, dir2), ( 0, 1.1, 1.1), 1e-4) CHECK_INTERSECTS_ONCE(cylinder, ( 0, 0.5, 0.5), ( 0, -dir2, -dir2), ( 0, -1.1, -1.1), 1e-4) CHECK_INTERSECTS_ONCE(cylinder, ( 0, -0.5, -0.5), ( 0, dir2, dir2), ( 0, 1.1, 1.1), 1e-4) CHECK_INTERSECTS_ONCE(cylinder, ( 0, -0.5, -0.5), ( 0, -dir2, -dir2), ( 0, -1.1, -1.1), 1e-4) CHECK_INTERSECTS_ONCE(cylinder, ( 0, 0, 0), ( dir2, 0, dir2), ( 1.1, 0, 1.1), 1e-4) CHECK_INTERSECTS_ONCE(cylinder, ( 0, 0, 0), (-dir2, 0, -dir2), (-1.1, 0, -1.1), 1e-4) CHECK_INTERSECTS_ONCE(cylinder, ( 0.5, 0, 0.5), ( dir2, 0, dir2), ( 1.1, 0, 1.1), 1e-4) CHECK_INTERSECTS_ONCE(cylinder, ( 0.5, 0, 0.5), (-dir2, 0, -dir2), (-1.1, 0, -1.1), 1e-4) CHECK_INTERSECTS_ONCE(cylinder, (-0.5, 0, -0.5), ( dir2, 0, dir2), ( 1.1, 0, 1.1), 1e-4) CHECK_INTERSECTS_ONCE(cylinder, (-0.5, 0, -0.5), (-dir2, 0, -dir2), (-1.1, 0, -1.1), 1e-4) // clang-format on // any random rays that start inside the cylinder should have exactly one intersection with it random_numbers::RandomNumberGenerator g(0u); for (size_t i = 0; i < 1000; ++i) { const Eigen::Isometry3d pos = getRandomPose(g); cylinder.setPose(pos); cylinder.setScale(g.uniformReal(0.5, 100.0)); cylinder.setPadding(g.uniformReal(-0.1, 100.0)); Eigen::Vector3d origin; cylinder.samplePointInside(g, 10, origin); // get the bounding sphere of the scaled cylinder bodies::BoundingSphere s; cylinder.computeBoundingSphere(s); const Eigen::Vector3d dir = Eigen::Vector3d::Random().normalized(); EigenSTL::vector_Vector3d intersections; if (cylinder.intersectsRay(origin, dir, &intersections, 2)) { EXPECT_EQ(1u, intersections.size()); if (intersections.size() == 1) { EXPECT_GE(s.radius, (s.center - intersections[0]).norm()); // verify the point lies on the ray EXPECT_NEAR(0.0, (Eigen::ParametrizedLine(origin, dir).distance(intersections[0])), 1e-6); EXPECT_LT(0.0, (intersections[0] - origin).normalized().dot(dir)); } } else { GTEST_NONFATAL_FAILURE_(("No intersection in iteration " + std::to_string(i)).c_str()); } } } TEST(CylinderRayIntersection, OriginOutside) { shapes::Cylinder shape(1.0, 2.0); bodies::Cylinder cylinder(&shape); cylinder.setScale(1.5); cylinder.setPadding(0.5); // clang-format off CHECK_INTERSECTS_TWICE(cylinder, (-4, 0, 0), ( 1, 0, 0), ( 2, 0, 0), (-2, 0, 0), 1e-6) CHECK_INTERSECTS_TWICE(cylinder, ( 4, 0, 0), (-1, 0, 0), (-2, 0, 0), ( 2, 0, 0), 1e-6) CHECK_INTERSECTS_TWICE(cylinder, ( 0, -4, 0), ( 0, 1, 0), ( 0, 2, 0), ( 0, -2, 0), 1e-6) CHECK_INTERSECTS_TWICE(cylinder, ( 0, 4, 0), ( 0, -1, 0), ( 0, -2, 0), ( 0, 2, 0), 1e-6) CHECK_INTERSECTS_TWICE(cylinder, ( 0, 0, -4), ( 0, 0, 1), ( 0, 0, 2), ( 0, 0, -2), 1e-6) CHECK_INTERSECTS_TWICE(cylinder, ( 0, 0, 4), ( 0, 0, -1), ( 0, 0, -2), ( 0, 0, 2), 1e-6) // clang-format on // test hitting the surface // clang-format off CHECK_INTERSECTS_ONCE(cylinder, (-2, -2, 0), ( 0, 1, 0), (-2, 0, 0), 1e-6) CHECK_INTERSECTS_ONCE(cylinder, (-2, 2, 0), ( 0, -1, 0), (-2, 0, 0), 1e-6) CHECK_INTERSECTS_ONCE(cylinder, ( 2, -2, 0), ( 0, 1, 0), ( 2, 0, 0), 1e-6) CHECK_INTERSECTS_ONCE(cylinder, ( 2, 2, 0), ( 0, -1, 0), ( 2, 0, 0), 1e-6) CHECK_INTERSECTS_ONCE(cylinder, (-2, -2, 0), ( 1, 0, 0), ( 0, -2, 0), 1e-6) CHECK_INTERSECTS_ONCE(cylinder, ( 2, -2, 0), (-1, 0, 0), ( 0, -2, 0), 1e-6) CHECK_INTERSECTS_ONCE(cylinder, (-2, 2, 0), ( 1, 0, 0), ( 0, 2, 0), 1e-6) CHECK_INTERSECTS_ONCE(cylinder, ( 2, 2, 0), (-1, 0, 0), ( 0, 2, 0), 1e-6) CHECK_INTERSECTS_TWICE(cylinder, ( 0, -2, -3), ( 0, 0, 1), ( 0, -2, -2), ( 0, -2, 2), 1e-6) CHECK_INTERSECTS_TWICE(cylinder, ( 0, -2, 3), ( 0, 0, -1), ( 0, -2, 2), ( 0, -2, -2), 1e-6) CHECK_INTERSECTS_TWICE(cylinder, ( 0, 2, -3), ( 0, 0, 1), ( 0, 2, -2), ( 0, 2, 2), 1e-6) CHECK_INTERSECTS_TWICE(cylinder, ( 0, 2, 3), ( 0, 0, -1), ( 0, 2, 2), ( 0, 2, -2), 1e-6) CHECK_INTERSECTS_TWICE(cylinder, (-2, 0, -3), ( 0, 0, 1), (-2, 0, -2), (-2, 0, 2), 1e-6) CHECK_INTERSECTS_TWICE(cylinder, (-2, 0, 3), ( 0, 0, -1), (-2, 0, 2), (-2, 0, -2), 1e-6) CHECK_INTERSECTS_TWICE(cylinder, ( 2, 0, -3), ( 0, 0, 1), ( 2, 0, -2), ( 2, 0, 2), 1e-6) CHECK_INTERSECTS_TWICE(cylinder, ( 2, 0, 3), ( 0, 0, -1), ( 2, 0, 2), ( 2, 0, -2), 1e-6) // clang-format on // test missing the surface // clang-format off CHECK_NO_INTERSECTION(cylinder, (-2.1, -1, 0), ( 0, 1, 0)) CHECK_NO_INTERSECTION(cylinder, (-2.1, 1, 0), ( 0, -1, 0)) CHECK_NO_INTERSECTION(cylinder, ( 2.1, -1, 0), ( 0, 1, 0)) CHECK_NO_INTERSECTION(cylinder, ( 2.1, 1, 0), ( 0, -1, 0)) CHECK_NO_INTERSECTION(cylinder, ( 0, -2.1, -2), ( 0, 0, 1)) CHECK_NO_INTERSECTION(cylinder, ( 0, -2.1, 2), ( 0, 0, -1)) CHECK_NO_INTERSECTION(cylinder, ( 0, 2.1, -2), ( 0, 0, 1)) CHECK_NO_INTERSECTION(cylinder, ( 0, 2.1, 2), ( 0, 0, -1)) CHECK_NO_INTERSECTION(cylinder, ( -2, 0, -2.1), ( 1, 0, 0)) CHECK_NO_INTERSECTION(cylinder, ( 2, 0, -2.1), (-1, 0, 0)) CHECK_NO_INTERSECTION(cylinder, ( -2, 0, 2.1), ( 1, 0, 0)) CHECK_NO_INTERSECTION(cylinder, ( 2, 0, 2.1), (-1, 0, 0)) // clang-format on // generate some random rays outside the cylinder and test them random_numbers::RandomNumberGenerator g(0u); for (size_t i = 0; i < 1000; ++i) { const Eigen::Isometry3d pos = getRandomPose(g); cylinder.setPose(pos); cylinder.setScale(g.uniformReal(0.5, 100.0)); cylinder.setPadding(g.uniformReal(-0.1, 100.0)); // choose a random direction const Eigen::Vector3d dir = Eigen::Vector3d::Random().normalized(); // get the bounding sphere of the cylinder bodies::BoundingSphere s; cylinder.computeBoundingSphere(s); // create origin outside the cylinder in the opposite direction than the chosen one const Eigen::Vector3d origin = s.center + -dir * 1.5 * s.radius; // a ray constructed in this way should intersect twice (through the cylinder center) EigenSTL::vector_Vector3d intersections; if (cylinder.intersectsRay(origin, dir, &intersections, 2)) { EXPECT_EQ(2u, intersections.size()); if (intersections.size() == 2) { EXPECT_GE(s.radius, (s.center - intersections[0]).norm()); EXPECT_GE(s.radius, (s.center - intersections[1]).norm()); // verify the point lies on the ray EXPECT_NEAR(0.0, (Eigen::ParametrizedLine(origin, dir).distance(intersections[0])), 1e-6); EXPECT_LT(0.0, (intersections[0] - origin).normalized().dot(dir)); EXPECT_NEAR(0.0, (Eigen::ParametrizedLine(origin, dir).distance(intersections[1])), 1e-6); EXPECT_LT(0.0, (intersections[1] - origin).normalized().dot(dir)); } } else { GTEST_NONFATAL_FAILURE_(("No intersection in iteration " + std::to_string(i)).c_str()); } // check that the opposite ray misses CHECK_NO_INTERSECTION(cylinder, (origin), (-dir)) // NOLINT(performance-unnecessary-copy-initialization) // shift the ray a bit sideways // choose a perpendicular direction Eigen::Vector3d perpDir = dir.cross(Eigen::Vector3d({ dir.z(), dir.z(), -dir.x() - dir.y() })); if (perpDir.norm() < 1e-6) perpDir = dir.cross(Eigen::Vector3d({ -dir.y() - dir.z(), dir.x(), dir.x() })); perpDir.normalize(); // get the scaled cylinder bodies::BoundingCylinder c; cylinder.computeBoundingCylinder(c); // now move origin "sideways" but still only so much that the ray will hit the cylinder auto minRadius = (std::min)(c.radius, c.length); const Eigen::Vector3d origin2 = origin + g.uniformReal(1e-6, minRadius - 1e-4) * perpDir; intersections.clear(); if (cylinder.intersectsRay(origin2, dir, &intersections, 2)) { EXPECT_EQ(2u, intersections.size()); if (intersections.size() == 2) { EXPECT_GT(s.radius, (s.center - intersections[0]).norm()); EXPECT_GT(s.radius, (s.center - intersections[1]).norm()); EXPECT_LT(minRadius, (s.center - intersections[0]).norm()); EXPECT_LT(minRadius, (s.center - intersections[1]).norm()); // verify the point lies on the ray EXPECT_NEAR(0.0, (Eigen::ParametrizedLine(origin2, dir).distance(intersections[0])), 1e-6); EXPECT_LT(0.0, (intersections[0] - origin2).normalized().dot(dir)); EXPECT_NEAR(0.0, (Eigen::ParametrizedLine(origin2, dir).distance(intersections[1])), 1e-6); EXPECT_LT(0.0, (intersections[1] - origin2).normalized().dot(dir)); } } else { GTEST_NONFATAL_FAILURE_(("No intersection in iteration " + std::to_string(i)).c_str()); } } } TEST(CylinderRayIntersection, SimpleRay1) { shapes::Cylinder shape(1.0, 2.0); bodies::Cylinder cylinder(&shape); cylinder.setScale(1.05); CHECK_INTERSECTS_TWICE(cylinder, (5, 0, 0), (-1, 0, 0), (1.05, 0, 0), (-1.05, 0, 0), 1e-6) } TEST(CylinderRayIntersection, SimpleRay2) { shapes::Cylinder shape(1.0, 2.0); bodies::Cylinder cylinder(&shape); cylinder.setScale(1.05); CHECK_NO_INTERSECTION(cylinder, (5, 0, 0), (1, 0, 0)) } TEST(BoxRayIntersection, SimpleRay1) { shapes::Box shape(1.0, 1.0, 3.0); bodies::Box box(&shape); box.setScale(0.95); CHECK_INTERSECTS_TWICE(box, (10, 0.449, 0), (-1, 0, 0), (0.475, 0.449, 0), (-0.475, 0.449, 0), 1e-4) } TEST(BoxRayIntersection, SimpleRay2) { shapes::Box shape(0.9, 0.01, 1.2); bodies::Box box(&shape); Eigen::Isometry3d pose(Eigen::AngleAxisd(0, Eigen::Vector3d::UnitX())); pose.translation() = Eigen::Vector3d(0, 0.005, 0.6); box.setPose(pose); const Eigen::Vector3d ray_d(0, -5.195, -0.77); CHECK_INTERSECTS(box, (0, 5, 1.6), (ray_d.normalized()), 2) } TEST(BoxRayIntersection, SimpleRay3) { shapes::Box shape(0.02, 0.4, 1.2); bodies::Box box(&shape); Eigen::Isometry3d pose(Eigen::AngleAxisd(0, Eigen::Vector3d::UnitX())); pose.translation() = Eigen::Vector3d(0.45, -0.195, 0.6); box.setPose(pose); const Eigen::Vector3d ray_d(0, 1.8, -0.669); CHECK_NO_INTERSECTION(box, (0, -2, 1.11), (ray_d.normalized())) } TEST(BoxRayIntersection, Regression109) { shapes::Box shape(1.0, 1.0, 1.0); bodies::Box box(&shape); // rotates the box so that the original (0.5, 0.5, 0.5) corner gets elsewhere and is no longer the // maximal corner const auto rot = Eigen::AngleAxisd(M_PI * 2 / 3, Eigen::Vector3d(1.0, -1.0, 1.0).normalized()); box.setPose(Eigen::Isometry3d(rot)); CHECK_INTERSECTS_TWICE(box, (-2, 0, 0), (1, 0, 0), (0.5, 0, 0), (-0.5, 0, 0), 1e-6) } TEST(BoxRayIntersection, OriginInside) { shapes::Box shape(2.0, 2.0, 2.0); bodies::Box box(&shape); // clang-format off CHECK_INTERSECTS_ONCE(box, (0, 0, 0), ( 1, 0, 0), ( 1, 0, 0), 1e-6) CHECK_INTERSECTS_ONCE(box, (0, 0, 0), (-1, 0, 0), (-1, 0, 0), 1e-6) CHECK_INTERSECTS_ONCE(box, (0, 0, 0), ( 0, 1, 0), ( 0, 1, 0), 1e-6) CHECK_INTERSECTS_ONCE(box, (0, 0, 0), ( 0, -1, 0), ( 0, -1, 0), 1e-6) CHECK_INTERSECTS_ONCE(box, (0, 0, 0), ( 0, 0, 1), ( 0, 0, 1), 1e-6) CHECK_INTERSECTS_ONCE(box, (0, 0, 0), ( 0, 0, -1), ( 0, 0, -1), 1e-6) // clang-format on // scaling box.setScale(1.1); // clang-format off CHECK_INTERSECTS_ONCE(box, (0, 0, 0), ( 1, 0, 0), ( 1.1, 0, 0), 1e-6) CHECK_INTERSECTS_ONCE(box, (0, 0, 0), (-1, 0, 0), (-1.1, 0, 0), 1e-6) CHECK_INTERSECTS_ONCE(box, (0, 0, 0), ( 0, 1, 0), ( 0, 1.1, 0), 1e-6) CHECK_INTERSECTS_ONCE(box, (0, 0, 0), ( 0, -1, 0), ( 0, -1.1, 0), 1e-6) CHECK_INTERSECTS_ONCE(box, (0, 0, 0), ( 0, 0, 1), ( 0, 0, 1.1), 1e-6) CHECK_INTERSECTS_ONCE(box, (0, 0, 0), ( 0, 0, -1), ( 0, 0, -1.1), 1e-6) // clang-format on // move origin within the box // clang-format off CHECK_INTERSECTS_ONCE(box, (0.5, 0 , 0 ), ( 1, 0, 0), ( 1.1, 0, 0), 1e-6) CHECK_INTERSECTS_ONCE(box, (0.5, 0 , 0 ), (-1, 0, 0), (-1.1, 0, 0), 1e-6) CHECK_INTERSECTS_ONCE(box, (0 , 0.5, 0 ), ( 0, 1, 0), ( 0, 1.1, 0), 1e-6) CHECK_INTERSECTS_ONCE(box, (0 , 0.5, 0 ), ( 0, -1, 0), ( 0, -1.1, 0), 1e-6) CHECK_INTERSECTS_ONCE(box, (0 , 0 , 0.5), ( 0, 0, 1), ( 0, 0, 1.1), 1e-6) CHECK_INTERSECTS_ONCE(box, (0 , 0 , 0.5), ( 0, 0, -1), ( 0, 0, -1.1), 1e-6) // clang-format on // move box Eigen::Isometry3d pose(Eigen::Translation3d(0.5, 0, 0)); box.setPose(pose); // clang-format off CHECK_INTERSECTS_ONCE(box, (0, 0, 0), ( 1, 0, 0), ( 1.6, 0, 0), 1e-6) CHECK_INTERSECTS_ONCE(box, (0, 0, 0), (-1, 0, 0), (-0.6, 0, 0), 1e-6) // clang-format on pose = Eigen::Translation3d(0, 0.5, 0); box.setPose(pose); // clang-format off CHECK_INTERSECTS_ONCE(box, (0, 0, 0), ( 0, 1, 0), (0, 1.6, 0), 1e-6) CHECK_INTERSECTS_ONCE(box, (0, 0, 0), ( 0, -1, 0), (0, -0.6, 0), 1e-6) // clang-format on pose = Eigen::Translation3d(0, 0, 0.5); box.setPose(pose); // clang-format off CHECK_INTERSECTS_ONCE(box, (0, 0, 0), ( 0, 0, 1), (0, 0, 1.6), 1e-6) CHECK_INTERSECTS_ONCE(box, (0, 0, 0), ( 0, 0, -1), (0, 0, -0.6), 1e-6) // clang-format on // 3D diagonal box.setPose(Eigen::Isometry3d::Identity()); box.setScale(1.0); box.setPadding(0.1); const auto dir3 = Eigen::Vector3d::Ones().normalized(); // clang-format off CHECK_INTERSECTS_ONCE(box, ( 0, 0, 0), ( dir3), ( 1.1, 1.1, 1.1), 1e-4) // NOLINT(performance-unnecessary-copy-initialization) CHECK_INTERSECTS_ONCE(box, ( 0.5, 0.5, 0.5), ( dir3), ( 1.1, 1.1, 1.1), 1e-4) // NOLINT(performance-unnecessary-copy-initialization) CHECK_INTERSECTS_ONCE(box, (-0.5, -0.5, -0.5), ( dir3), ( 1.1, 1.1, 1.1), 1e-4) // NOLINT(performance-unnecessary-copy-initialization) CHECK_INTERSECTS_ONCE(box, ( 0, 0, 0), (-dir3), (-1.1, -1.1, -1.1), 1e-4) CHECK_INTERSECTS_ONCE(box, ( 0.5, 0.5, 0.5), (-dir3), (-1.1, -1.1, -1.1), 1e-4) CHECK_INTERSECTS_ONCE(box, (-0.5, -0.5, -0.5), (-dir3), (-1.1, -1.1, -1.1), 1e-4) // clang-format on // 2D diagonal const auto dir2 = 1 / sqrt(2); // clang-format off CHECK_INTERSECTS_ONCE(box, ( 0, 0, 0), ( dir2, dir2, 0), ( 1.1, 1.1, 0), 1e-4) CHECK_INTERSECTS_ONCE(box, ( 0, 0, 0), (-dir2, -dir2, 0), (-1.1, -1.1, 0), 1e-4) CHECK_INTERSECTS_ONCE(box, ( 0.5, 0.5, 0), ( dir2, dir2, 0), ( 1.1, 1.1, 0), 1e-4) CHECK_INTERSECTS_ONCE(box, ( 0.5, 0.5, 0), (-dir2, -dir2, 0), (-1.1, -1.1, 0), 1e-4) CHECK_INTERSECTS_ONCE(box, (-0.5, -0.5, 0), ( dir2, dir2, 0), ( 1.1, 1.1, 0), 1e-4) CHECK_INTERSECTS_ONCE(box, (-0.5, -0.5, 0), (-dir2, -dir2, 0), (-1.1, -1.1, 0), 1e-4) CHECK_INTERSECTS_ONCE(box, ( 0, 0, 0), ( 0, dir2, dir2), ( 0, 1.1, 1.1), 1e-4) CHECK_INTERSECTS_ONCE(box, ( 0, 0, 0), ( 0, -dir2, -dir2), ( 0, -1.1, -1.1), 1e-4) CHECK_INTERSECTS_ONCE(box, ( 0, 0.5, 0.5), ( 0, dir2, dir2), ( 0, 1.1, 1.1), 1e-4) CHECK_INTERSECTS_ONCE(box, ( 0, 0.5, 0.5), ( 0, -dir2, -dir2), ( 0, -1.1, -1.1), 1e-4) CHECK_INTERSECTS_ONCE(box, ( 0, -0.5, -0.5), ( 0, dir2, dir2), ( 0, 1.1, 1.1), 1e-4) CHECK_INTERSECTS_ONCE(box, ( 0, -0.5, -0.5), ( 0, -dir2, -dir2), ( 0, -1.1, -1.1), 1e-4) CHECK_INTERSECTS_ONCE(box, ( 0, 0, 0), ( dir2, 0, dir2), ( 1.1, 0, 1.1), 1e-4) CHECK_INTERSECTS_ONCE(box, ( 0, 0, 0), (-dir2, 0, -dir2), (-1.1, 0, -1.1), 1e-4) CHECK_INTERSECTS_ONCE(box, ( 0.5, 0, 0.5), ( dir2, 0, dir2), ( 1.1, 0, 1.1), 1e-4) CHECK_INTERSECTS_ONCE(box, ( 0.5, 0, 0.5), (-dir2, 0, -dir2), (-1.1, 0, -1.1), 1e-4) CHECK_INTERSECTS_ONCE(box, (-0.5, 0, -0.5), ( dir2, 0, dir2), ( 1.1, 0, 1.1), 1e-4) CHECK_INTERSECTS_ONCE(box, (-0.5, 0, -0.5), (-dir2, 0, -dir2), (-1.1, 0, -1.1), 1e-4) // clang-format on // any random rays that start inside the box should have exactly one intersection with it random_numbers::RandomNumberGenerator g(0u); for (size_t i = 0; i < 1000; ++i) { const Eigen::Isometry3d pos = getRandomPose(g); box.setPose(pos); box.setScale(g.uniformReal(0.5, 100.0)); box.setPadding(g.uniformReal(-0.1, 100.0)); // get the scaled dimensions of the box Eigen::Vector3d sizes = { box.getDimensions()[0], box.getDimensions()[1], box.getDimensions()[2] }; sizes *= box.getScale(); sizes += 2 * box.getPadding() * Eigen::Vector3d::Ones(); // radii of the inscribed and bounding spheres const auto minRadius = sizes.minCoeff() / 2; const auto maxRadius = (sizes / 2).norm(); const Eigen::Vector3d boxCenter = box.getPose().translation(); Eigen::Vector3d origin; box.samplePointInside(g, 10, origin); const Eigen::Vector3d dir = Eigen::Vector3d::Random().normalized(); EigenSTL::vector_Vector3d intersections; if (box.intersectsRay(origin, dir, &intersections, 2)) { EXPECT_EQ(1u, intersections.size()); if (intersections.size() == 1) { // just approximate verification that the point is between inscribed and bounding sphere EXPECT_GE(maxRadius, (boxCenter - intersections[0]).norm()); EXPECT_LE(minRadius, (boxCenter - intersections[0]).norm()); // verify the point lies on the ray EXPECT_NEAR(0.0, (Eigen::ParametrizedLine(origin, dir).distance(intersections[0])), 1e-6); EXPECT_LT(0.0, (intersections[0] - origin).normalized().dot(dir)); } } else { GTEST_NONFATAL_FAILURE_(("No intersection in iteration " + std::to_string(i)).c_str()); } } } TEST(BoxRayIntersection, OriginOutsideIntersects) { shapes::Box shape(2.0, 2.0, 2.0); bodies::Box box(&shape); // clang-format off CHECK_INTERSECTS_TWICE(box, (-2, 0, 0), ( 1, 0, 0), ( 1, 0, 0), (-1, 0, 0), 1e-6) CHECK_INTERSECTS_TWICE(box, ( 2, 0, 0), (-1, 0, 0), (-1, 0, 0), ( 1, 0, 0), 1e-6) CHECK_INTERSECTS_TWICE(box, (0, -2, 0), ( 0, 1, 0), ( 0, 1, 0), ( 0, -1, 0), 1e-6) CHECK_INTERSECTS_TWICE(box, (0, 2, 0), ( 0, -1, 0), ( 0, -1, 0), ( 0, 1, 0), 1e-6) CHECK_INTERSECTS_TWICE(box, (0, 0, -2), ( 0, 0, 1), ( 0, 0, 1), ( 0, 0, -1), 1e-6) CHECK_INTERSECTS_TWICE(box, (0, 0, 2), ( 0, 0, -1), ( 0, 0, -1), ( 0, 0, 1), 1e-6) // clang-format on // test hitting the surface // clang-format off CHECK_INTERSECTS_TWICE(box, (-1, -2, 0), ( 0, 1, 0), (-1, -1, 0), (-1, 1, 0), 1e-6) CHECK_INTERSECTS_TWICE(box, (-1, 2, 0), ( 0, -1, 0), (-1, 1, 0), (-1, -1, 0), 1e-6) CHECK_INTERSECTS_TWICE(box, ( 1, -2, 0), ( 0, 1, 0), ( 1, -1, 0), ( 1, 1, 0), 1e-6) CHECK_INTERSECTS_TWICE(box, ( 1, 2, 0), ( 0, -1, 0), ( 1, 1, 0), ( 1, -1, 0), 1e-6) CHECK_INTERSECTS_TWICE(box, ( 0, -1, -2), ( 0, 0, 1), ( 0, -1, -1), ( 0, -1, 1), 1e-6) CHECK_INTERSECTS_TWICE(box, ( 0, -1, 2), ( 0, 0, -1), ( 0, -1, 1), ( 0, -1, -1), 1e-6) CHECK_INTERSECTS_TWICE(box, ( 0, 1, -2), ( 0, 0, 1), ( 0, 1, -1), ( 0, 1, 1), 1e-6) CHECK_INTERSECTS_TWICE(box, ( 0, 1, 2), ( 0, 0, -1), ( 0, 1, 1), ( 0, 1, -1), 1e-6) CHECK_INTERSECTS_TWICE(box, (-2, 0, -1), ( 1, 0, 0), (-1, 0, -1), ( 1, 0, -1), 1e-6) CHECK_INTERSECTS_TWICE(box, ( 2, 0, -1), (-1, 0, 0), ( 1, 0, -1), (-1, 0, -1), 1e-6) CHECK_INTERSECTS_TWICE(box, (-2, 0, 1), ( 1, 0, 0), (-1, 0, 1), ( 1, 0, 1), 1e-6) CHECK_INTERSECTS_TWICE(box, ( 2, 0, 1), (-1, 0, 0), ( 1, 0, 1), (-1, 0, 1), 1e-6) // clang-format on // test missing the surface // clang-format off CHECK_NO_INTERSECTION(box, (-1.1, -2, 0), ( 0, 1, 0)) CHECK_NO_INTERSECTION(box, (-1.1, 2, 0), ( 0, -1, 0)) CHECK_NO_INTERSECTION(box, ( 1.1, -2, 0), ( 0, 1, 0)) CHECK_NO_INTERSECTION(box, ( 1.1, 2, 0), ( 0, -1, 0)) CHECK_NO_INTERSECTION(box, ( 0, -1.1, -2), ( 0, 0, 1)) CHECK_NO_INTERSECTION(box, ( 0, -1.1, 2), ( 0, 0, -1)) CHECK_NO_INTERSECTION(box, ( 0, 1.1, -2), ( 0, 0, 1)) CHECK_NO_INTERSECTION(box, ( 0, 1.1, 2), ( 0, 0, -1)) CHECK_NO_INTERSECTION(box, ( -2, 0, -1.1), ( 1, 0, 0)) CHECK_NO_INTERSECTION(box, ( 2, 0, -1.1), (-1, 0, 0)) CHECK_NO_INTERSECTION(box, ( -2, 0, 1.1), ( 1, 0, 0)) CHECK_NO_INTERSECTION(box, ( 2, 0, 1.1), (-1, 0, 0)) // clang-format on // generate some random rays outside the box and test them random_numbers::RandomNumberGenerator g(0u); for (size_t i = 0; i < 1000; ++i) { const Eigen::Isometry3d pos = getRandomPose(g); box.setPose(pos); box.setScale(g.uniformReal(0.5, 100.0)); box.setPadding(g.uniformReal(-0.1, 100.0)); // choose a random direction const Eigen::Vector3d dir = Eigen::Vector3d::Random().normalized(); // get the scaled dimensions of the box Eigen::Vector3d sizes = { box.getDimensions()[0], box.getDimensions()[1], box.getDimensions()[2] }; sizes *= box.getScale(); sizes += 2 * box.getPadding() * Eigen::Vector3d::Ones(); // radii of the inscribed and bounding spheres const auto minRadius = sizes.minCoeff() / 2; const auto maxRadius = (sizes / 2).norm(); const Eigen::Vector3d boxCenter = box.getPose().translation(); // create origin outside the box in the opposite direction than the chosen one const Eigen::Vector3d origin = boxCenter + -dir * 1.5 * maxRadius; // a ray constructed in this way should intersect twice (through the box center) EigenSTL::vector_Vector3d intersections; if (box.intersectsRay(origin, dir, &intersections, 2)) { EXPECT_EQ(2u, intersections.size()); if (intersections.size() == 2) { // just approximate verification that the point is between inscribed and bounding sphere EXPECT_GE(maxRadius, (boxCenter - intersections[0]).norm()); EXPECT_LE(minRadius, (boxCenter - intersections[0]).norm()); EXPECT_GE(maxRadius, (boxCenter - intersections[1]).norm()); EXPECT_LE(minRadius, (boxCenter - intersections[1]).norm()); // verify the point lies on the ray EXPECT_NEAR(0.0, (Eigen::ParametrizedLine(origin, dir).distance(intersections[0])), 1e-6); EXPECT_LT(0.0, (intersections[0] - origin).normalized().dot(dir)); EXPECT_NEAR(0.0, (Eigen::ParametrizedLine(origin, dir).distance(intersections[1])), 1e-6); EXPECT_LT(0.0, (intersections[1] - origin).normalized().dot(dir)); } } else { GTEST_NONFATAL_FAILURE_(("No intersection in iteration " + std::to_string(i)).c_str()); } // check that the opposite ray misses CHECK_NO_INTERSECTION(box, (origin), (-dir)) // NOLINT(performance-unnecessary-copy-initialization) // shift the ray a bit sideways // choose a perpendicular direction Eigen::Vector3d perpDir = dir.cross(Eigen::Vector3d({ dir.z(), dir.z(), -dir.x() - dir.y() })); if (perpDir.norm() < 1e-6) perpDir = dir.cross(Eigen::Vector3d({ -dir.y() - dir.z(), dir.x(), dir.x() })); perpDir.normalize(); // now move origin "sideways" but still only so much that the ray will hit the box const Eigen::Vector3d origin2 = origin + g.uniformReal(1e-6, minRadius - 1e-4) * perpDir; intersections.clear(); if (box.intersectsRay(origin2, dir, &intersections, 2)) { EXPECT_EQ(2u, intersections.size()); if (intersections.size() == 2) { // just approximate verification that the point is between inscribed and bounding sphere EXPECT_GE(maxRadius, (boxCenter - intersections[0]).norm()); EXPECT_LE(minRadius, (boxCenter - intersections[0]).norm()); EXPECT_GE(maxRadius, (boxCenter - intersections[1]).norm()); EXPECT_LE(minRadius, (boxCenter - intersections[1]).norm()); // verify the point lies on the ray EXPECT_NEAR(0.0, (Eigen::ParametrizedLine(origin2, dir).distance(intersections[0])), 1e-6); EXPECT_LT(0.0, (intersections[0] - origin2).normalized().dot(dir)); EXPECT_NEAR(0.0, (Eigen::ParametrizedLine(origin2, dir).distance(intersections[1])), 1e-6); EXPECT_LT(0.0, (intersections[1] - origin2).normalized().dot(dir)); } } else { GTEST_NONFATAL_FAILURE_(("No intersection in iteration " + std::to_string(i)).c_str()); } } } TEST(ConvexMeshRayIntersection, SimpleRay1) { shapes::Box box(1.0, 1.0, 3.0); shapes::Mesh* shape = shapes::createMeshFromShape(&box); bodies::ConvexMesh mesh(shape); delete shape; 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) } TEST(ConvexMeshRayIntersection, SimpleRay2) { shapes::Box box(0.9, 0.01, 1.2); shapes::Mesh* shape = shapes::createMeshFromShape(&box); bodies::ConvexMesh mesh(shape); delete shape; Eigen::Isometry3d pose(Eigen::AngleAxisd(0, Eigen::Vector3d::UnitX())); pose.translation() = Eigen::Vector3d(0, 0.005, 0.6); mesh.setPose(pose); const Eigen::Vector3d ray_d(0, -5.195, -0.77); CHECK_INTERSECTS(mesh, (0, 5, 1.6), (ray_d.normalized()), 2) } TEST(ConvexMeshRayIntersection, SimpleRay3) { shapes::Box box(0.02, 0.4, 1.2); shapes::Mesh* shape = shapes::createMeshFromShape(&box); bodies::ConvexMesh mesh(shape); delete shape; Eigen::Isometry3d pose(Eigen::AngleAxisd(0, Eigen::Vector3d::UnitX())); pose.translation() = Eigen::Vector3d(0.45, -0.195, 0.6); mesh.setPose(pose); const Eigen::Vector3d ray_d(0, 1.8, -0.669); CHECK_NO_INTERSECTION(mesh, (0, -2, 1.11), (ray_d.normalized())) } TEST(ConvexMeshRayIntersection, OriginInside) { shapes::Box box(2.0, 2.0, 2.0); shapes::Mesh* shape = shapes::createMeshFromShape(&box); bodies::ConvexMesh mesh(shape); delete shape; // clang-format off CHECK_INTERSECTS_ONCE(mesh, (0, 0, 0), ( 1, 0, 0), ( 1, 0, 0), 1e-6) CHECK_INTERSECTS_ONCE(mesh, (0, 0, 0), (-1, 0, 0), (-1, 0, 0), 1e-6) CHECK_INTERSECTS_ONCE(mesh, (0, 0, 0), ( 0, 1, 0), ( 0, 1, 0), 1e-6) CHECK_INTERSECTS_ONCE(mesh, (0, 0, 0), ( 0, -1, 0), ( 0, -1, 0), 1e-6) CHECK_INTERSECTS_ONCE(mesh, (0, 0, 0), ( 0, 0, 1), ( 0, 0, 1), 1e-6) CHECK_INTERSECTS_ONCE(mesh, (0, 0, 0), ( 0, 0, -1), ( 0, 0, -1), 1e-6) // clang-format on // scaling mesh.setScale(1.1); // clang-format off CHECK_INTERSECTS_ONCE(mesh, (0, 0, 0), ( 1, 0, 0), ( 1.1, 0, 0), 1e-6) CHECK_INTERSECTS_ONCE(mesh, (0, 0, 0), (-1, 0, 0), (-1.1, 0, 0), 1e-6) CHECK_INTERSECTS_ONCE(mesh, (0, 0, 0), ( 0, 1, 0), ( 0, 1.1, 0), 1e-6) CHECK_INTERSECTS_ONCE(mesh, (0, 0, 0), ( 0, -1, 0), ( 0, -1.1, 0), 1e-6) CHECK_INTERSECTS_ONCE(mesh, (0, 0, 0), ( 0, 0, 1), ( 0, 0, 1.1), 1e-6) CHECK_INTERSECTS_ONCE(mesh, (0, 0, 0), ( 0, 0, -1), ( 0, 0, -1.1), 1e-6) // clang-format on // move origin within the mesh // clang-format off CHECK_INTERSECTS_ONCE(mesh, (0.5, 0 , 0 ), ( 1, 0, 0), ( 1.1, 0, 0), 1e-6) CHECK_INTERSECTS_ONCE(mesh, (0.5, 0 , 0 ), (-1, 0, 0), (-1.1, 0, 0), 1e-6) CHECK_INTERSECTS_ONCE(mesh, (0 , 0.5, 0 ), ( 0, 1, 0), ( 0, 1.1, 0), 1e-6) CHECK_INTERSECTS_ONCE(mesh, (0 , 0.5, 0 ), ( 0, -1, 0), ( 0, -1.1, 0), 1e-6) CHECK_INTERSECTS_ONCE(mesh, (0 , 0 , 0.5), ( 0, 0, 1), ( 0, 0, 1.1), 1e-6) CHECK_INTERSECTS_ONCE(mesh, (0 , 0 , 0.5), ( 0, 0, -1), ( 0, 0, -1.1), 1e-6) // clang-format on // move mesh Eigen::Isometry3d pose(Eigen::Translation3d(0.5, 0, 0)); mesh.setPose(pose); // clang-format off CHECK_INTERSECTS_ONCE(mesh, (0, 0, 0), ( 1, 0, 0), ( 1.6, 0, 0), 1e-6) CHECK_INTERSECTS_ONCE(mesh, (0, 0, 0), (-1, 0, 0), (-0.6, 0, 0), 1e-6) // clang-format on pose = Eigen::Translation3d(0, 0.5, 0); mesh.setPose(pose); // clang-format off CHECK_INTERSECTS_ONCE(mesh, (0, 0, 0), ( 0, 1, 0), (0, 1.6, 0), 1e-6) CHECK_INTERSECTS_ONCE(mesh, (0, 0, 0), ( 0, -1, 0), (0, -0.6, 0), 1e-6) // clang-format on pose = Eigen::Translation3d(0, 0, 0.5); mesh.setPose(pose); // clang-format off CHECK_INTERSECTS_ONCE(mesh, (0, 0, 0), ( 0, 0, 1), (0, 0, 1.6), 1e-6) CHECK_INTERSECTS_ONCE(mesh, (0, 0, 0), ( 0, 0, -1), (0, 0, -0.6), 1e-6) // clang-format on // 3D diagonal mesh.setPose(Eigen::Isometry3d::Identity()); mesh.setScale(1.0); mesh.setPadding(0.1); // half-size of the scaled and padded mesh (mesh scaling isn't "linear" in padding) const double s = 1.0 + 0.1 / sqrt(3); const auto dir3 = Eigen::Vector3d::Ones().normalized(); // clang-format off CHECK_INTERSECTS_ONCE(mesh, ( 0, 0, 0), ( dir3), ( s, s, s), 1e-4) // NOLINT(performance-unnecessary-copy-initialization) CHECK_INTERSECTS_ONCE(mesh, ( 0.5, 0.5, 0.5), ( dir3), ( s, s, s), 1e-4) // NOLINT(performance-unnecessary-copy-initialization) CHECK_INTERSECTS_ONCE(mesh, (-0.5, -0.5, -0.5), ( dir3), ( s, s, s), 1e-4) // NOLINT(performance-unnecessary-copy-initialization) CHECK_INTERSECTS_ONCE(mesh, ( 0, 0, 0), (-dir3), (-s, -s, -s), 1e-4) CHECK_INTERSECTS_ONCE(mesh, ( 0.5, 0.5, 0.5), (-dir3), (-s, -s, -s), 1e-4) CHECK_INTERSECTS_ONCE(mesh, (-0.5, -0.5, -0.5), (-dir3), (-s, -s, -s), 1e-4) // clang-format on // 2D diagonal const auto dir2 = 1 / sqrt(2); // clang-format off CHECK_INTERSECTS_ONCE(mesh, ( 0, 0, 0), ( dir2, dir2, 0), ( s, s, 0), 1e-4) CHECK_INTERSECTS_ONCE(mesh, ( 0, 0, 0), (-dir2, -dir2, 0), (-s, -s, 0), 1e-4) CHECK_INTERSECTS_ONCE(mesh, ( 0.5, 0.5, 0), ( dir2, dir2, 0), ( s, s, 0), 1e-4) CHECK_INTERSECTS_ONCE(mesh, ( 0.5, 0.5, 0), (-dir2, -dir2, 0), (-s, -s, 0), 1e-4) CHECK_INTERSECTS_ONCE(mesh, (-0.5, -0.5, 0), ( dir2, dir2, 0), ( s, s, 0), 1e-4) CHECK_INTERSECTS_ONCE(mesh, (-0.5, -0.5, 0), (-dir2, -dir2, 0), (-s, -s, 0), 1e-4) CHECK_INTERSECTS_ONCE(mesh, ( 0, 0, 0), ( 0, dir2, dir2), ( 0, s, s), 1e-4) CHECK_INTERSECTS_ONCE(mesh, ( 0, 0, 0), ( 0, -dir2, -dir2), ( 0, -s, -s), 1e-4) CHECK_INTERSECTS_ONCE(mesh, ( 0, 0.5, 0.5), ( 0, dir2, dir2), ( 0, s, s), 1e-4) CHECK_INTERSECTS_ONCE(mesh, ( 0, 0.5, 0.5), ( 0, -dir2, -dir2), ( 0, -s, -s), 1e-4) CHECK_INTERSECTS_ONCE(mesh, ( 0, -0.5, -0.5), ( 0, dir2, dir2), ( 0, s, s), 1e-4) CHECK_INTERSECTS_ONCE(mesh, ( 0, -0.5, -0.5), ( 0, -dir2, -dir2), ( 0, -s, -s), 1e-4) CHECK_INTERSECTS_ONCE(mesh, ( 0, 0, 0), ( dir2, 0, dir2), ( s, 0, s), 1e-4) CHECK_INTERSECTS_ONCE(mesh, ( 0, 0, 0), (-dir2, 0, -dir2), (-s, 0, -s), 1e-4) CHECK_INTERSECTS_ONCE(mesh, ( 0.5, 0, 0.5), ( dir2, 0, dir2), ( s, 0, s), 1e-4) CHECK_INTERSECTS_ONCE(mesh, ( 0.5, 0, 0.5), (-dir2, 0, -dir2), (-s, 0, -s), 1e-4) CHECK_INTERSECTS_ONCE(mesh, (-0.5, 0, -0.5), ( dir2, 0, dir2), ( s, 0, s), 1e-4) CHECK_INTERSECTS_ONCE(mesh, (-0.5, 0, -0.5), (-dir2, 0, -dir2), (-s, 0, -s), 1e-4) // clang-format on // any random rays that start inside the mesh should have exactly one intersection with it random_numbers::RandomNumberGenerator g(0u); for (size_t i = 0; i < 1000; ++i) { const Eigen::Isometry3d pos = getRandomPose(g); mesh.setPose(pos); mesh.setScale(g.uniformReal(0.5, 100.0)); mesh.setPadding(g.uniformReal(-0.1, 100.0)); // get the scaled dimensions of the mesh Eigen::Vector3d sizes = { box.size[0], box.size[1], box.size[2] }; sizes *= mesh.getScale(); sizes += 2 * mesh.getPadding() / sqrt(3) * Eigen::Vector3d::Ones(); // radii of the inscribed and bounding spheres const auto minRadius = sizes.minCoeff() / 2; const auto maxRadius = (sizes / 2).norm(); const Eigen::Vector3d meshCenter = mesh.getPose().translation(); Eigen::Vector3d origin; mesh.samplePointInside(g, 10000, origin); const Eigen::Vector3d dir = Eigen::Vector3d::Random().normalized(); EigenSTL::vector_Vector3d intersections; if (mesh.intersectsRay(origin, dir, &intersections, 2)) { EXPECT_EQ(1u, intersections.size()); if (intersections.size() == 1) { // just approximate verification that the point is between inscribed and bounding sphere EXPECT_GE(maxRadius, (meshCenter - intersections[0]).norm()); EXPECT_LE(minRadius, (meshCenter - intersections[0]).norm()); // verify the point lies on the ray EXPECT_NEAR(0.0, (Eigen::ParametrizedLine(origin, dir).distance(intersections[0])), 1e-6); EXPECT_LT(0.0, (intersections[0] - origin).normalized().dot(dir)); } } else { GTEST_NONFATAL_FAILURE_(("No intersection in iteration " + std::to_string(i)).c_str()); } } } TEST(ConvexMeshRayIntersection, OriginOutsideIntersects) { shapes::Box box(2.0, 2.0, 2.0); shapes::Mesh* shape = shapes::createMeshFromShape(&box); bodies::ConvexMesh mesh(shape); delete shape; // clang-format off CHECK_INTERSECTS_TWICE(mesh, (-2, 0, 0), ( 1, 0, 0), ( 1, 0, 0), (-1, 0, 0), 1e-6) CHECK_INTERSECTS_TWICE(mesh, ( 2, 0, 0), (-1, 0, 0), (-1, 0, 0), ( 1, 0, 0), 1e-6) CHECK_INTERSECTS_TWICE(mesh, (0, -2, 0), ( 0, 1, 0), ( 0, 1, 0), ( 0, -1, 0), 1e-6) CHECK_INTERSECTS_TWICE(mesh, (0, 2, 0), ( 0, -1, 0), ( 0, -1, 0), ( 0, 1, 0), 1e-6) CHECK_INTERSECTS_TWICE(mesh, (0, 0, -2), ( 0, 0, 1), ( 0, 0, 1), ( 0, 0, -1), 1e-6) CHECK_INTERSECTS_TWICE(mesh, (0, 0, 2), ( 0, 0, -1), ( 0, 0, -1), ( 0, 0, 1), 1e-6) // clang-format on // test hitting the surface // clang-format off CHECK_INTERSECTS_TWICE(mesh, (-1, -2, 0), ( 0, 1, 0), (-1, -1, 0), (-1, 1, 0), 1e-6) CHECK_INTERSECTS_TWICE(mesh, (-1, 2, 0), ( 0, -1, 0), (-1, 1, 0), (-1, -1, 0), 1e-6) CHECK_INTERSECTS_TWICE(mesh, ( 1, -2, 0), ( 0, 1, 0), ( 1, -1, 0), ( 1, 1, 0), 1e-6) CHECK_INTERSECTS_TWICE(mesh, ( 1, 2, 0), ( 0, -1, 0), ( 1, 1, 0), ( 1, -1, 0), 1e-6) CHECK_INTERSECTS_TWICE(mesh, ( 0, -1, -2), ( 0, 0, 1), ( 0, -1, -1), ( 0, -1, 1), 1e-6) CHECK_INTERSECTS_TWICE(mesh, ( 0, -1, 2), ( 0, 0, -1), ( 0, -1, 1), ( 0, -1, -1), 1e-6) CHECK_INTERSECTS_TWICE(mesh, ( 0, 1, -2), ( 0, 0, 1), ( 0, 1, -1), ( 0, 1, 1), 1e-6) CHECK_INTERSECTS_TWICE(mesh, ( 0, 1, 2), ( 0, 0, -1), ( 0, 1, 1), ( 0, 1, -1), 1e-6) CHECK_INTERSECTS_TWICE(mesh, (-2, 0, -1), ( 1, 0, 0), (-1, 0, -1), ( 1, 0, -1), 1e-6) CHECK_INTERSECTS_TWICE(mesh, ( 2, 0, -1), (-1, 0, 0), ( 1, 0, -1), (-1, 0, -1), 1e-6) CHECK_INTERSECTS_TWICE(mesh, (-2, 0, 1), ( 1, 0, 0), (-1, 0, 1), ( 1, 0, 1), 1e-6) CHECK_INTERSECTS_TWICE(mesh, ( 2, 0, 1), (-1, 0, 0), ( 1, 0, 1), (-1, 0, 1), 1e-6) // clang-format on // test missing the surface // clang-format off CHECK_NO_INTERSECTION(mesh, (-1.1, -2, 0), ( 0, 1, 0)) CHECK_NO_INTERSECTION(mesh, (-1.1, 2, 0), ( 0, -1, 0)) CHECK_NO_INTERSECTION(mesh, ( 1.1, -2, 0), ( 0, 1, 0)) CHECK_NO_INTERSECTION(mesh, ( 1.1, 2, 0), ( 0, -1, 0)) CHECK_NO_INTERSECTION(mesh, ( 0, -1.1, -2), ( 0, 0, 1)) CHECK_NO_INTERSECTION(mesh, ( 0, -1.1, 2), ( 0, 0, -1)) CHECK_NO_INTERSECTION(mesh, ( 0, 1.1, -2), ( 0, 0, 1)) CHECK_NO_INTERSECTION(mesh, ( 0, 1.1, 2), ( 0, 0, -1)) CHECK_NO_INTERSECTION(mesh, ( -2, 0, -1.1), ( 1, 0, 0)) CHECK_NO_INTERSECTION(mesh, ( 2, 0, -1.1), (-1, 0, 0)) CHECK_NO_INTERSECTION(mesh, ( -2, 0, 1.1), ( 1, 0, 0)) CHECK_NO_INTERSECTION(mesh, ( 2, 0, 1.1), (-1, 0, 0)) // clang-format on // generate some random rays outside the mesh and test them random_numbers::RandomNumberGenerator g(0u); for (size_t i = 0; i < 1000; ++i) { const Eigen::Isometry3d pos = getRandomPose(g); mesh.setPose(pos); mesh.setScale(g.uniformReal(0.5, 100.0)); mesh.setPadding(g.uniformReal(-0.1, 100.0)); // choose a random direction const Eigen::Vector3d dir = Eigen::Vector3d::Random().normalized(); // get the scaled dimensions of the mesh Eigen::Vector3d sizes = { box.size[0], box.size[1], box.size[2] }; sizes *= mesh.getScale(); sizes += 2 * mesh.getPadding() / sqrt(3) * Eigen::Vector3d::Ones(); // radii of the inscribed and bounding spheres const auto minRadius = sizes.minCoeff() / 2; const auto maxRadius = (sizes / 2).norm(); const Eigen::Vector3d meshCenter = mesh.getPose().translation(); // create origin outside the mesh in the opposite direction than the chosen one const Eigen::Vector3d origin = meshCenter + -dir * 1.5 * maxRadius; // a ray constructed in this way should intersect twice (through the mesh center) EigenSTL::vector_Vector3d intersections; if (mesh.intersectsRay(origin, dir, &intersections, 2)) { EXPECT_EQ(2u, intersections.size()); if (intersections.size() == 2) { // just approximate verification that the point is between inscribed and bounding sphere EXPECT_GE(maxRadius, (meshCenter - intersections[0]).norm()); EXPECT_LE(minRadius, (meshCenter - intersections[0]).norm()); EXPECT_GE(maxRadius, (meshCenter - intersections[1]).norm()); EXPECT_LE(minRadius, (meshCenter - intersections[1]).norm()); // verify the point lies on the ray EXPECT_NEAR(0.0, (Eigen::ParametrizedLine(origin, dir).distance(intersections[0])), 1e-6); EXPECT_LT(0.0, (intersections[0] - origin).normalized().dot(dir)); EXPECT_NEAR(0.0, (Eigen::ParametrizedLine(origin, dir).distance(intersections[1])), 1e-6); EXPECT_LT(0.0, (intersections[1] - origin).normalized().dot(dir)); } } else { GTEST_NONFATAL_FAILURE_(("No intersection in iteration " + std::to_string(i)).c_str()); } // check that the opposite ray misses CHECK_NO_INTERSECTION(mesh, (origin), (-dir)) // NOLINT(performance-unnecessary-copy-initialization) // shift the ray a bit sideways // choose a perpendicular direction Eigen::Vector3d perpDir = dir.cross(Eigen::Vector3d({ dir.z(), dir.z(), -dir.x() - dir.y() })); if (perpDir.norm() < 1e-6) perpDir = dir.cross(Eigen::Vector3d({ -dir.y() - dir.z(), dir.x(), dir.x() })); perpDir.normalize(); // now move origin "sideways" but still only so much that the ray will hit the mesh const Eigen::Vector3d origin2 = origin + g.uniformReal(1e-6, minRadius - 1e-4) * perpDir; intersections.clear(); if (mesh.intersectsRay(origin2, dir, &intersections, 2)) { EXPECT_EQ(2u, intersections.size()); if (intersections.size() == 2) { // just approximate verification that the point is between inscribed and bounding sphere EXPECT_GE(maxRadius, (meshCenter - intersections[0]).norm()); EXPECT_LE(minRadius, (meshCenter - intersections[0]).norm()); EXPECT_GE(maxRadius, (meshCenter - intersections[1]).norm()); EXPECT_LE(minRadius, (meshCenter - intersections[1]).norm()); // verify the point lies on the ray EXPECT_NEAR(0.0, (Eigen::ParametrizedLine(origin2, dir).distance(intersections[0])), 1e-6); EXPECT_LT(0.0, (intersections[0] - origin2).normalized().dot(dir)); EXPECT_NEAR(0.0, (Eigen::ParametrizedLine(origin2, dir).distance(intersections[1])), 1e-6); EXPECT_LT(0.0, (intersections[1] - origin2).normalized().dot(dir)); } } else { GTEST_NONFATAL_FAILURE_(("No intersection in iteration " + std::to_string(i)).c_str()); } } } int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } geometric_shapes-0.7.3/test/test_shapes.cpp000066400000000000000000000357361405115746100210710ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2019, Czech Technical University * 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 the copyright holder 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. *********************************************************************/ /** \Author Martin Pecka */ #include "resources/config.h" #include #include #include using namespace shapes; TEST(Plane, ScaleAndPadd) { const Plane plane(1., 1., 1., 1.0); auto plane2 = plane; EXPECT_EQ(plane2.a, plane.a); EXPECT_EQ(plane2.b, plane.b); EXPECT_EQ(plane2.c, plane.c); EXPECT_EQ(plane2.d, plane.d); plane2.scale(2.0); EXPECT_EQ(plane2.a, plane.a); EXPECT_EQ(plane2.b, plane.b); EXPECT_EQ(plane2.c, plane.c); EXPECT_EQ(plane2.d, plane.d); plane2.padd(1.0); EXPECT_EQ(plane2.a, plane.a); EXPECT_EQ(plane2.b, plane.b); EXPECT_EQ(plane2.c, plane.c); EXPECT_EQ(plane2.d, plane.d); plane2.scaleAndPadd(2.0, 1.0); EXPECT_EQ(plane2.a, plane.a); EXPECT_EQ(plane2.b, plane.b); EXPECT_EQ(plane2.c, plane.c); EXPECT_EQ(plane2.d, plane.d); } TEST(OcTree, ScaleAndPaddEmpty) { // just test that scaling/padding an empty octree doesn't throw OcTree octree; octree.scale(2.0); octree.padd(1.0); octree.scaleAndPadd(2.0, 1.0); } TEST(Sphere, ScaleAndPadd) { const Sphere sphere(1.); auto sphere2 = sphere; EXPECT_EQ(sphere2.radius, sphere.radius); sphere2.scale(2.0); EXPECT_DOUBLE_EQ(sphere2.radius, 2.0); sphere2.padd(1.0); EXPECT_DOUBLE_EQ(sphere2.radius, 3.0); sphere2.scaleAndPadd(2.0, 1.0); EXPECT_DOUBLE_EQ(sphere2.radius, 7.0); } TEST(Cylinder, ScaleAndPadd) { const Cylinder cylinder(1., 2.); auto cylinder2 = cylinder; EXPECT_EQ(cylinder2.radius, cylinder.radius); EXPECT_EQ(cylinder2.length, cylinder.length); cylinder2.scale(2.0); EXPECT_DOUBLE_EQ(cylinder2.radius, 2.0); EXPECT_DOUBLE_EQ(cylinder2.length, 4.0); cylinder2.padd(1.0); EXPECT_DOUBLE_EQ(cylinder2.radius, 3.0); EXPECT_DOUBLE_EQ(cylinder2.length, 6.0); cylinder2.scaleAndPadd(2.0, 1.0); EXPECT_DOUBLE_EQ(cylinder2.radius, 7.0); EXPECT_DOUBLE_EQ(cylinder2.length, 14.0); cylinder2.scaleAndPadd(1.0, 3.0, 1.0, 3.0); EXPECT_DOUBLE_EQ(cylinder2.radius, 8.0); EXPECT_DOUBLE_EQ(cylinder2.length, 48.0); cylinder2.scale(2.0, 1.5); EXPECT_DOUBLE_EQ(cylinder2.radius, 16.0); EXPECT_DOUBLE_EQ(cylinder2.length, 72.0); cylinder2.padd(2.0, 3.0); EXPECT_DOUBLE_EQ(cylinder2.radius, 18.0); EXPECT_DOUBLE_EQ(cylinder2.length, 78.0); } TEST(Cone, ScaleAndPadd) { const Cone cone(1., 2.); auto cone2 = cone; EXPECT_EQ(cone2.radius, cone.radius); EXPECT_EQ(cone2.length, cone.length); cone2.scale(2.0); EXPECT_DOUBLE_EQ(cone2.radius, 2.0); EXPECT_DOUBLE_EQ(cone2.length, 4.0); cone2.padd(1.0); EXPECT_DOUBLE_EQ(cone2.radius, 3.0); EXPECT_DOUBLE_EQ(cone2.length, 6.0); cone2.scaleAndPadd(2.0, 1.0); EXPECT_DOUBLE_EQ(cone2.radius, 7.0); EXPECT_DOUBLE_EQ(cone2.length, 14.0); cone2.scaleAndPadd(1.0, 3.0, 1.0, 3.0); EXPECT_DOUBLE_EQ(cone2.radius, 8.0); EXPECT_DOUBLE_EQ(cone2.length, 48.0); cone2.scale(2.0, 1.5); EXPECT_DOUBLE_EQ(cone2.radius, 16.0); EXPECT_DOUBLE_EQ(cone2.length, 72.0); cone2.padd(2.0, 3.0); EXPECT_DOUBLE_EQ(cone2.radius, 18.0); EXPECT_DOUBLE_EQ(cone2.length, 78.0); } TEST(Box, ScaleAndPadd) { const Box box(1., 2., 3.0); auto box2 = box; EXPECT_EQ(box2.size[0], box.size[0]); EXPECT_EQ(box2.size[1], box.size[1]); EXPECT_EQ(box2.size[2], box.size[2]); box2.scale(2.0); EXPECT_DOUBLE_EQ(box2.size[0], 2.0); EXPECT_DOUBLE_EQ(box2.size[1], 4.0); EXPECT_DOUBLE_EQ(box2.size[2], 6.0); box2.padd(1.0); EXPECT_DOUBLE_EQ(box2.size[0], 4.0); EXPECT_DOUBLE_EQ(box2.size[1], 6.0); EXPECT_DOUBLE_EQ(box2.size[2], 8.0); box2.scaleAndPadd(2.0, 1.0); EXPECT_DOUBLE_EQ(box2.size[0], 10.0); EXPECT_DOUBLE_EQ(box2.size[1], 14.0); EXPECT_DOUBLE_EQ(box2.size[2], 18.0); box2.scaleAndPadd(1.0, 2.0, 3.0, 1.0, 2.0, 3.0); EXPECT_DOUBLE_EQ(box2.size[0], 12.0); EXPECT_DOUBLE_EQ(box2.size[1], 32.0); EXPECT_DOUBLE_EQ(box2.size[2], 60.0); box2.scale(1.0, 2.0, 3.0); EXPECT_DOUBLE_EQ(box2.size[0], 12.0); EXPECT_DOUBLE_EQ(box2.size[1], 64.0); EXPECT_DOUBLE_EQ(box2.size[2], 180.0); box2.padd(1.0, 2.0, 3.0); EXPECT_DOUBLE_EQ(box2.size[0], 14.0); EXPECT_DOUBLE_EQ(box2.size[1], 68.0); EXPECT_DOUBLE_EQ(box2.size[2], 186.0); } TEST(Mesh, ScaleAndPadd) { std::string path = "file://" + std::string(TEST_RESOURCES_DIR) + "/box.dae"; const auto& mesh = shapes::createMeshFromResource(path); ASSERT_EQ(mesh->vertex_count, 8u); auto mesh2 = mesh; EXPECT_DOUBLE_EQ(mesh2->vertices[0], 1.0); EXPECT_DOUBLE_EQ(mesh2->vertices[1], 1.0); EXPECT_DOUBLE_EQ(mesh2->vertices[2], -1.0); EXPECT_DOUBLE_EQ(mesh2->vertices[3], 1.0); EXPECT_DOUBLE_EQ(mesh2->vertices[4], -1.0); EXPECT_DOUBLE_EQ(mesh2->vertices[5], -1.0); EXPECT_DOUBLE_EQ(mesh2->vertices[6], -1.0); EXPECT_DOUBLE_EQ(mesh2->vertices[7], -1.0); EXPECT_DOUBLE_EQ(mesh2->vertices[8], -1.0); EXPECT_DOUBLE_EQ(mesh2->vertices[9], -1.0); EXPECT_DOUBLE_EQ(mesh2->vertices[10], 1.0); EXPECT_DOUBLE_EQ(mesh2->vertices[11], -1.0); EXPECT_DOUBLE_EQ(mesh2->vertices[12], 1.0); EXPECT_DOUBLE_EQ(mesh2->vertices[13], 1.0); EXPECT_DOUBLE_EQ(mesh2->vertices[14], 1.0); EXPECT_DOUBLE_EQ(mesh2->vertices[15], -1.0); EXPECT_DOUBLE_EQ(mesh2->vertices[16], 1.0); EXPECT_DOUBLE_EQ(mesh2->vertices[17], 1.0); EXPECT_DOUBLE_EQ(mesh2->vertices[18], -1.0); EXPECT_DOUBLE_EQ(mesh2->vertices[19], -1.0); EXPECT_DOUBLE_EQ(mesh2->vertices[20], 1.0); EXPECT_DOUBLE_EQ(mesh2->vertices[21], 1.0); EXPECT_DOUBLE_EQ(mesh2->vertices[22], -1.0); EXPECT_DOUBLE_EQ(mesh2->vertices[23], 1.0); mesh2->scale(2.0); EXPECT_DOUBLE_EQ(mesh2->vertices[0], 2.0); EXPECT_DOUBLE_EQ(mesh2->vertices[1], 2.0); EXPECT_DOUBLE_EQ(mesh2->vertices[2], -2.0); EXPECT_DOUBLE_EQ(mesh2->vertices[3], 2.0); EXPECT_DOUBLE_EQ(mesh2->vertices[4], -2.0); EXPECT_DOUBLE_EQ(mesh2->vertices[5], -2.0); EXPECT_DOUBLE_EQ(mesh2->vertices[6], -2.0); EXPECT_DOUBLE_EQ(mesh2->vertices[7], -2.0); EXPECT_DOUBLE_EQ(mesh2->vertices[8], -2.0); EXPECT_DOUBLE_EQ(mesh2->vertices[9], -2.0); EXPECT_DOUBLE_EQ(mesh2->vertices[10], 2.0); EXPECT_DOUBLE_EQ(mesh2->vertices[11], -2.0); EXPECT_DOUBLE_EQ(mesh2->vertices[12], 2.0); EXPECT_DOUBLE_EQ(mesh2->vertices[13], 2.0); EXPECT_DOUBLE_EQ(mesh2->vertices[14], 2.0); EXPECT_DOUBLE_EQ(mesh2->vertices[15], -2.0); EXPECT_DOUBLE_EQ(mesh2->vertices[16], 2.0); EXPECT_DOUBLE_EQ(mesh2->vertices[17], 2.0); EXPECT_DOUBLE_EQ(mesh2->vertices[18], -2.0); EXPECT_DOUBLE_EQ(mesh2->vertices[19], -2.0); EXPECT_DOUBLE_EQ(mesh2->vertices[20], 2.0); EXPECT_DOUBLE_EQ(mesh2->vertices[21], 2.0); 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 mesh2->padd(1.0); const double pos = 2.0 * (1 + 1.0 / sqrt(12)); EXPECT_DOUBLE_EQ(mesh2->vertices[0], pos); EXPECT_DOUBLE_EQ(mesh2->vertices[1], pos); EXPECT_DOUBLE_EQ(mesh2->vertices[2], -pos); EXPECT_DOUBLE_EQ(mesh2->vertices[3], pos); EXPECT_DOUBLE_EQ(mesh2->vertices[4], -pos); EXPECT_DOUBLE_EQ(mesh2->vertices[5], -pos); EXPECT_DOUBLE_EQ(mesh2->vertices[6], -pos); EXPECT_DOUBLE_EQ(mesh2->vertices[7], -pos); EXPECT_DOUBLE_EQ(mesh2->vertices[8], -pos); EXPECT_DOUBLE_EQ(mesh2->vertices[9], -pos); EXPECT_DOUBLE_EQ(mesh2->vertices[10], pos); EXPECT_DOUBLE_EQ(mesh2->vertices[11], -pos); EXPECT_DOUBLE_EQ(mesh2->vertices[12], pos); EXPECT_DOUBLE_EQ(mesh2->vertices[13], pos); EXPECT_DOUBLE_EQ(mesh2->vertices[14], pos); EXPECT_DOUBLE_EQ(mesh2->vertices[15], -pos); EXPECT_DOUBLE_EQ(mesh2->vertices[16], pos); EXPECT_DOUBLE_EQ(mesh2->vertices[17], pos); EXPECT_DOUBLE_EQ(mesh2->vertices[18], -pos); EXPECT_DOUBLE_EQ(mesh2->vertices[19], -pos); EXPECT_DOUBLE_EQ(mesh2->vertices[20], pos); EXPECT_DOUBLE_EQ(mesh2->vertices[21], pos); EXPECT_DOUBLE_EQ(mesh2->vertices[22], -pos); 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)); EXPECT_DOUBLE_EQ(mesh2->vertices[0], pos2); EXPECT_DOUBLE_EQ(mesh2->vertices[1], pos2); EXPECT_DOUBLE_EQ(mesh2->vertices[2], -pos2); EXPECT_DOUBLE_EQ(mesh2->vertices[3], pos2); EXPECT_DOUBLE_EQ(mesh2->vertices[4], -pos2); EXPECT_DOUBLE_EQ(mesh2->vertices[5], -pos2); EXPECT_DOUBLE_EQ(mesh2->vertices[6], -pos2); EXPECT_DOUBLE_EQ(mesh2->vertices[7], -pos2); EXPECT_DOUBLE_EQ(mesh2->vertices[8], -pos2); EXPECT_DOUBLE_EQ(mesh2->vertices[9], -pos2); EXPECT_DOUBLE_EQ(mesh2->vertices[10], pos2); EXPECT_DOUBLE_EQ(mesh2->vertices[11], -pos2); EXPECT_DOUBLE_EQ(mesh2->vertices[12], pos2); EXPECT_DOUBLE_EQ(mesh2->vertices[13], pos2); EXPECT_DOUBLE_EQ(mesh2->vertices[14], pos2); EXPECT_DOUBLE_EQ(mesh2->vertices[15], -pos2); EXPECT_DOUBLE_EQ(mesh2->vertices[16], pos2); EXPECT_DOUBLE_EQ(mesh2->vertices[17], pos2); EXPECT_DOUBLE_EQ(mesh2->vertices[18], -pos2); EXPECT_DOUBLE_EQ(mesh2->vertices[19], -pos2); EXPECT_DOUBLE_EQ(mesh2->vertices[20], pos2); EXPECT_DOUBLE_EQ(mesh2->vertices[21], pos2); EXPECT_DOUBLE_EQ(mesh2->vertices[22], -pos2); EXPECT_DOUBLE_EQ(mesh2->vertices[23], pos2); mesh2->scaleAndPadd(1.0, 2.0, 3.0, 1.0, 2.0, 3.0); const double pos3x = pos2 * (1.0 + 1.0 / sqrt(3 * pos2 * pos2)); const double pos3y = pos2 * (2.0 + 2.0 / sqrt(3 * pos2 * pos2)); const double pos3z = pos2 * (3.0 + 3.0 / sqrt(3 * pos2 * pos2)); EXPECT_DOUBLE_EQ(mesh2->vertices[0], pos3x); EXPECT_DOUBLE_EQ(mesh2->vertices[1], pos3y); EXPECT_DOUBLE_EQ(mesh2->vertices[2], -pos3z); EXPECT_DOUBLE_EQ(mesh2->vertices[3], pos3x); EXPECT_DOUBLE_EQ(mesh2->vertices[4], -pos3y); EXPECT_DOUBLE_EQ(mesh2->vertices[5], -pos3z); EXPECT_DOUBLE_EQ(mesh2->vertices[6], -pos3x); EXPECT_DOUBLE_EQ(mesh2->vertices[7], -pos3y); EXPECT_DOUBLE_EQ(mesh2->vertices[8], -pos3z); EXPECT_DOUBLE_EQ(mesh2->vertices[9], -pos3x); EXPECT_DOUBLE_EQ(mesh2->vertices[10], pos3y); EXPECT_DOUBLE_EQ(mesh2->vertices[11], -pos3z); EXPECT_DOUBLE_EQ(mesh2->vertices[12], pos3x); EXPECT_DOUBLE_EQ(mesh2->vertices[13], pos3y); EXPECT_DOUBLE_EQ(mesh2->vertices[14], pos3z); EXPECT_DOUBLE_EQ(mesh2->vertices[15], -pos3x); EXPECT_DOUBLE_EQ(mesh2->vertices[16], pos3y); EXPECT_DOUBLE_EQ(mesh2->vertices[17], pos3z); EXPECT_DOUBLE_EQ(mesh2->vertices[18], -pos3x); EXPECT_DOUBLE_EQ(mesh2->vertices[19], -pos3y); EXPECT_DOUBLE_EQ(mesh2->vertices[20], pos3z); EXPECT_DOUBLE_EQ(mesh2->vertices[21], pos3x); EXPECT_DOUBLE_EQ(mesh2->vertices[22], -pos3y); EXPECT_DOUBLE_EQ(mesh2->vertices[23], pos3z); mesh2->scale(1.0, 2.0, 3.0); const double pos4x = pos3x; const double pos4y = 2 * pos3y; const double pos4z = 3 * pos3z; EXPECT_DOUBLE_EQ(mesh2->vertices[0], pos4x); EXPECT_DOUBLE_EQ(mesh2->vertices[1], pos4y); EXPECT_DOUBLE_EQ(mesh2->vertices[2], -pos4z); EXPECT_DOUBLE_EQ(mesh2->vertices[3], pos4x); EXPECT_DOUBLE_EQ(mesh2->vertices[4], -pos4y); EXPECT_DOUBLE_EQ(mesh2->vertices[5], -pos4z); EXPECT_DOUBLE_EQ(mesh2->vertices[6], -pos4x); EXPECT_DOUBLE_EQ(mesh2->vertices[7], -pos4y); EXPECT_DOUBLE_EQ(mesh2->vertices[8], -pos4z); EXPECT_DOUBLE_EQ(mesh2->vertices[9], -pos4x); EXPECT_DOUBLE_EQ(mesh2->vertices[10], pos4y); EXPECT_DOUBLE_EQ(mesh2->vertices[11], -pos4z); EXPECT_DOUBLE_EQ(mesh2->vertices[12], pos4x); EXPECT_DOUBLE_EQ(mesh2->vertices[13], pos4y); EXPECT_DOUBLE_EQ(mesh2->vertices[14], pos4z); EXPECT_DOUBLE_EQ(mesh2->vertices[15], -pos4x); EXPECT_DOUBLE_EQ(mesh2->vertices[16], pos4y); EXPECT_DOUBLE_EQ(mesh2->vertices[17], pos4z); EXPECT_DOUBLE_EQ(mesh2->vertices[18], -pos4x); EXPECT_DOUBLE_EQ(mesh2->vertices[19], -pos4y); EXPECT_DOUBLE_EQ(mesh2->vertices[20], pos4z); EXPECT_DOUBLE_EQ(mesh2->vertices[21], pos4x); EXPECT_DOUBLE_EQ(mesh2->vertices[22], -pos4y); 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); EXPECT_DOUBLE_EQ(mesh2->vertices[0], pos5x); EXPECT_DOUBLE_EQ(mesh2->vertices[1], pos5y); EXPECT_DOUBLE_EQ(mesh2->vertices[2], -pos5z); EXPECT_DOUBLE_EQ(mesh2->vertices[3], pos5x); EXPECT_DOUBLE_EQ(mesh2->vertices[4], -pos5y); EXPECT_DOUBLE_EQ(mesh2->vertices[5], -pos5z); EXPECT_DOUBLE_EQ(mesh2->vertices[6], -pos5x); EXPECT_DOUBLE_EQ(mesh2->vertices[7], -pos5y); EXPECT_DOUBLE_EQ(mesh2->vertices[8], -pos5z); EXPECT_DOUBLE_EQ(mesh2->vertices[9], -pos5x); EXPECT_DOUBLE_EQ(mesh2->vertices[10], pos5y); EXPECT_DOUBLE_EQ(mesh2->vertices[11], -pos5z); EXPECT_DOUBLE_EQ(mesh2->vertices[12], pos5x); EXPECT_DOUBLE_EQ(mesh2->vertices[13], pos5y); EXPECT_DOUBLE_EQ(mesh2->vertices[14], pos5z); EXPECT_DOUBLE_EQ(mesh2->vertices[15], -pos5x); EXPECT_DOUBLE_EQ(mesh2->vertices[16], pos5y); EXPECT_DOUBLE_EQ(mesh2->vertices[17], pos5z); EXPECT_DOUBLE_EQ(mesh2->vertices[18], -pos5x); EXPECT_DOUBLE_EQ(mesh2->vertices[19], -pos5y); EXPECT_DOUBLE_EQ(mesh2->vertices[20], pos5z); EXPECT_DOUBLE_EQ(mesh2->vertices[21], pos5x); EXPECT_DOUBLE_EQ(mesh2->vertices[22], -pos5y); EXPECT_DOUBLE_EQ(mesh2->vertices[23], pos5z); } int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); }