pax_global_header00006660000000000000000000000064130020232020014472gustar00rootroot0000000000000052 comment=2ccd23ad59fdee7e91383b001a8d0fd8187930ce geometric_shapes-0.5.2/000077500000000000000000000000001300202320200150175ustar00rootroot00000000000000geometric_shapes-0.5.2/.gitignore000066400000000000000000000000031300202320200170000ustar00rootroot00000000000000*~ geometric_shapes-0.5.2/.travis.yml000066400000000000000000000013101300202320200171230ustar00rootroot00000000000000# This config file for Travis CI utilizes https://github.com/ros-planning/moveit_ci/ package. sudo: required dist: trusty services: - docker language: generic compiler: - gcc notifications: email: recipients: - 130s@2000.jukuin.keio.ac.jp env: matrix: - ROS_DISTRO=kinetic ROS_REPO=ros UPSTREAM_WORKSPACE=https://raw.githubusercontent.com/ros-planning/moveit/kinetic-devel/moveit.rosinstall - ROS_DISTRO=kinetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=https://raw.githubusercontent.com/ros-planning/moveit/kinetic-devel/moveit.rosinstall before_script: - git clone -q https://github.com/ros-planning/moveit_ci.git .moveit_ci script: - source .moveit_ci/travis.shgeometric_shapes-0.5.2/CHANGELOG.rst000066400000000000000000000113231300202320200170400ustar00rootroot00000000000000^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package geometric_shapes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 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.5.2/CMakeLists.txt000066400000000000000000000040771300202320200175670ustar00rootroot00000000000000cmake_minimum_required(VERSION 2.8.12) project(geometric_shapes) add_compile_options(-std=c++11) 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() find_package(ASSIMP QUIET) if (NOT ASSIMP_FOUND) find_package(PkgConfig REQUIRED) # assimp is required, so REQUIRE the second attempt pkg_check_modules(ASSIMP REQUIRED assimp) endif() 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) if (HAVE_QHULL_2011) add_definitions(-DGEOMETRIC_SHAPES_HAVE_QHULL_2011) endif() include_directories(include) include_directories(${EIGEN3_INCLUDE_DIR} ${Boost_INCLUDE_DIR} ${ASSIMP_INCLUDE_DIRS} ${OCTOMAP_INCLUDE_DIRS}) include_directories(${catkin_INCLUDE_DIRS} ${console_bridge_INCLUDE_DIRS}) add_library(${PROJECT_NAME} 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_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}) install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) geometric_shapes-0.5.2/README.md000066400000000000000000000022751300202320200163040ustar00rootroot00000000000000# 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 ## Build Status Travis CI: [![Build Status](https://travis-ci.org/ros-planning/geometric_shapes.svg?branch=kinetic-devel)](https://travis-ci.org/ros-planning/geometric_shapes) Devel Job: [![Build Status](http://build.ros.org/buildStatus/icon?job=Jbin_uT64__geometric_shapes__ubuntu_trusty_amd64__binary)](http://build.ros.org/view/Ibin_uT64/job/Jbin_uT64__geometric_shapes__ubuntu_trusty_amd64__binary/) Debian Job: [![Build Status](http://build.ros.org/buildStatus/icon?job=Jdev__geometric_shapes__ubuntu_trusty_amd64)](http://build.ros.org/view/Idev/job/Jdev__geometric_shapes__ubuntu_trusty_amd64/) geometric_shapes-0.5.2/cmake/000077500000000000000000000000001300202320200160775ustar00rootroot00000000000000geometric_shapes-0.5.2/cmake/FindQhull.cmake000066400000000000000000000056341300202320200207770ustar00rootroot00000000000000############################################################################### # 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. # If QHULL_USE_STATIC is specified then look for static libraries ONLY else # look for shared ones set(QHULL_MAJOR_VERSION 6) if(QHULL_USE_STATIC) set(QHULL_RELEASE_NAME qhullstatic) set(QHULL_DEBUG_NAME qhullstatic_d) else(QHULL_USE_STATIC) set(QHULL_RELEASE_NAME qhull_p qhull${QHULL_MAJOR_VERSION} qhull) set(QHULL_DEBUG_NAME qhull_pd qhull${QHULL_MAJOR_VERSION}_d qhull_d${QHULL_MAJOR_VERSION} qhull_d) endif(QHULL_USE_STATIC) find_file(QHULL_HEADER NAMES libqhull/libqhull.h qhull.h HINTS "${QHULL_ROOT}" "$ENV{QHULL_ROOT}" "${QHULL_INCLUDE_DIR}" PATHS "$ENV{PROGRAMFILES}/QHull" "$ENV{PROGRAMW6432}/QHull" PATH_SUFFIXES qhull src/libqhull libqhull include) set(QHULL_HEADER "${QHULL_HEADER}" CACHE INTERNAL "QHull header" FORCE ) if(QHULL_HEADER) get_filename_component(qhull_header ${QHULL_HEADER} NAME_WE) if("${qhull_header}" STREQUAL "qhull") set(HAVE_QHULL_2011 OFF) get_filename_component(QHULL_INCLUDE_DIR ${QHULL_HEADER} PATH) elseif("${qhull_header}" STREQUAL "libqhull") set(HAVE_QHULL_2011 ON) get_filename_component(QHULL_INCLUDE_DIR ${QHULL_HEADER} PATH) get_filename_component(QHULL_INCLUDE_DIR ${QHULL_INCLUDE_DIR} PATH) endif() 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) if(NOT QHULL_USE_STATIC) add_definitions("-Dqh_QHpointer") if(MSVC) add_definitions("-Dqh_QHpointer_dllimport") endif(MSVC) endif(NOT QHULL_USE_STATIC) message(STATUS "QHULL found (include: ${QHULL_INCLUDE_DIRS}, lib: ${QHULL_LIBRARIES})") endif(QHULL_FOUND) geometric_shapes-0.5.2/include/000077500000000000000000000000001300202320200164425ustar00rootroot00000000000000geometric_shapes-0.5.2/include/geometric_shapes/000077500000000000000000000000001300202320200217635ustar00rootroot00000000000000geometric_shapes-0.5.2/include/geometric_shapes/bodies.h000066400000000000000000000427021300202320200234060ustar00rootroot00000000000000/********************************************************************* * 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/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::Affine3d 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() { } /** \brief Get the type of shape this body represents */ shapes::ShapeType getType() const { return type_; } /** \brief If the dimension of the body should be scaled, this method sets the scale. Default is 1.0 */ void setScale(double scale) { scale_ = scale; updateInternalData(); } /** \brief Retrieve the current scale */ double getScale() const { return scale_; } /** \brief If constant padding should be added to the body, this method sets the padding. Default is 0.0 */ void setPadding(double padd) { padding_ = padd; updateInternalData(); } /** \brief Retrieve the current padding */ double getPadding() const { return padding_; } /** \brief Set the pose of the body. Default is identity */ void setPose(const Eigen::Affine3d &pose) { pose_ = pose; updateInternalData(); } /** \brief Retrieve the pose of the body */ const Eigen::Affine3d& getPose() const { return pose_; } /** \brief Get the dimensions associated to this body (as read from corresponding shape) */ virtual std::vector getDimensions() const = 0; /** \brief Set the dimensions of the body (from corresponding shape) */ void setDimensions(const shapes::Shape *shape); /** \brief Check if a point is inside the body */ 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 */ 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 */ virtual bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d &dir, EigenSTL::vector_Vector3d *intersections = NULL, 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); /** \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 Get a clone of this body, but one that is located at the pose \e pose */ BodyPtr cloneAt(const Eigen::Affine3d &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::Affine3d &pose, double padding, double scaling) const = 0; protected: /** \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; /** \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::Affine3d 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); } virtual ~Sphere() { } /** \brief Get the radius of the sphere */ virtual std::vector getDimensions() const; virtual bool containsPoint(const Eigen::Vector3d &p, bool verbose = false) const; virtual double computeVolume() const; virtual bool samplePointInside(random_numbers::RandomNumberGenerator &rng, unsigned int max_attempts, Eigen::Vector3d &result); virtual void computeBoundingSphere(BoundingSphere &sphere) const; virtual void computeBoundingCylinder(BoundingCylinder &cylinder) const; virtual bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d &dir, EigenSTL::vector_Vector3d *intersections = NULL, unsigned int count = 0) const; virtual BodyPtr cloneAt(const Eigen::Affine3d &pose, double padding, double scale) const; protected: virtual void useDimensions(const shapes::Shape *shape); virtual void updateInternalData(); // 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); } virtual ~Cylinder() { } /** \brief Get the radius & length of the cylinder */ virtual std::vector getDimensions() const; virtual bool containsPoint(const Eigen::Vector3d &p, bool verbose = false) const; virtual double computeVolume() const; virtual bool samplePointInside(random_numbers::RandomNumberGenerator &rng, unsigned int max_attempts, Eigen::Vector3d &result); virtual void computeBoundingSphere(BoundingSphere &sphere) const; virtual void computeBoundingCylinder(BoundingCylinder &cylinder) const; virtual bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d &dir, EigenSTL::vector_Vector3d *intersections = NULL, unsigned int count = 0) const; virtual BodyPtr cloneAt(const Eigen::Affine3d &pose, double padding, double scale) const; protected: virtual void useDimensions(const shapes::Shape *shape); virtual void updateInternalData(); // 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); } virtual ~Box() { } /** \brief Get the length & width & height (x, y, z) of the box */ virtual std::vector getDimensions() const; virtual bool containsPoint(const Eigen::Vector3d &p, bool verbose = false) const; virtual double computeVolume() const; virtual bool samplePointInside(random_numbers::RandomNumberGenerator &rng, unsigned int max_attempts, Eigen::Vector3d &result); virtual void computeBoundingSphere(BoundingSphere &sphere) const; virtual void computeBoundingCylinder(BoundingCylinder &cylinder) const; virtual bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d &dir, EigenSTL::vector_Vector3d *intersections = NULL, unsigned int count = 0) const; virtual BodyPtr cloneAt(const Eigen::Affine3d &pose, double padding, double scale) const; protected: virtual void useDimensions(const shapes::Shape *shape); // (x, y, z) = (length, width, height) virtual void updateInternalData(); // 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::Vector3d normalL_; Eigen::Vector3d normalW_; Eigen::Vector3d normalH_; Eigen::Vector3d corner1_; Eigen::Vector3d corner2_; 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_ = NULL; } ConvexMesh(const shapes::Shape *shape) : Body() { type_ = shapes::MESH; scaled_vertices_ = NULL; setDimensions(shape); } virtual ~ConvexMesh() { } /** \brief Returns an empty vector */ virtual std::vector getDimensions() const; virtual bool containsPoint(const Eigen::Vector3d &p, bool verbose = false) const; virtual double computeVolume() const; virtual void computeBoundingSphere(BoundingSphere &sphere) const; virtual void computeBoundingCylinder(BoundingCylinder &cylinder) const; virtual bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d &dir, EigenSTL::vector_Vector3d *intersections = NULL, unsigned int count = 0) const; const std::vector& getTriangles() const; const EigenSTL::vector_Vector3d& getVertices() const; const EigenSTL::vector_Vector3d& getScaledVertices() const; virtual BodyPtr cloneAt(const Eigen::Affine3d &pose, double padding, double scale) const; /// Project the original vertex to the scaled and padded planes and average. void computeScaledVerticesFromPlaneProjections(); void correctVertexOrderFromPlanes(); protected: virtual void useDimensions(const shapes::Shape *shape); virtual void updateInternalData(); /** \brief (Used mainly for debugging) Count the number of vertices behind a plane*/ unsigned int countVerticesBehindPlane(const Eigen::Vector4f& planeNormal) const; /** \brief Check if a point is inside a set of planes that make up a convex mesh*/ bool isPointInsidePlanes(const Eigen::Vector3d& point) const; struct MeshData { EigenSTL::vector_Vector4f planes_; EigenSTL::vector_Vector3d vertices_; std::vector triangles_; std::map plane_for_triangle_; Eigen::Vector3d mesh_center_; double mesh_radiusB_; Eigen::Vector3d box_offset_; Eigen::Vector3d box_size_; BoundingCylinder bounding_cylinder_; EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; // 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::Affine3d 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_Affine3d& 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::Affine3d& 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::Affine3d& pose); /** \brief Get the number of bodies in this vector*/ std::size_t getCount() const; /** \brief Check if any of the bodies in the vector contains the input point*/ bool containsPoint(const Eigen::Vector3d &p, bool verbose = false) const; /** \brief Check if any of the bodies in the vector contains the input point, and report the index at which the first body that contains the point was found. */ 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 = NULL, 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; } #endif geometric_shapes-0.5.2/include/geometric_shapes/body_operations.h000066400000000000000000000057151300202320200253440ustar00rootroot00000000000000/********************************************************************* * 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 namespace bodies { /** \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 Compute a bounding sphere to enclose a set of bounding spheres */ void mergeBoundingSpheres(const std::vector &spheres, BoundingSphere &mergedSphere); /** \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); } #endif geometric_shapes-0.5.2/include/geometric_shapes/mesh_operations.h000066400000000000000000000111151300202320200253320ustar00rootroot00000000000000/********************************************************************* * 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); } #endif geometric_shapes-0.5.2/include/geometric_shapes/shape_extents.h000066400000000000000000000046051300202320200250130ustar00rootroot00000000000000/********************************************************************* * 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); } #endif geometric_shapes-0.5.2/include/geometric_shapes/shape_messages.h000066400000000000000000000045321300202320200251270ustar00rootroot00000000000000/********************************************************************* * 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; } #endif geometric_shapes-0.5.2/include/geometric_shapes/shape_operations.h000066400000000000000000000073741300202320200255120ustar00rootroot00000000000000/********************************************************************* * 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); } #endif geometric_shapes-0.5.2/include/geometric_shapes/shape_to_marker.h000066400000000000000000000053701300202320200253040ustar00rootroot00000000000000/********************************************************************* * 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); } #endif geometric_shapes-0.5.2/include/geometric_shapes/shapes.h000066400000000000000000000205101300202320200234150ustar00rootroot00000000000000/********************************************************************* * 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 }; /** \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 Scale this shape by a factor */ void scale(double scale); /** \brief Add padding to this shape */ void padd(double padding); /** \brief 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; virtual void scaleAndPadd(double scale, double padd); virtual Shape* clone() const; virtual void print(std::ostream &out = std::cout) const; /** \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: 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; virtual void scaleAndPadd(double scale, double padd); virtual Shape* clone() const; virtual void print(std::ostream &out = std::cout) const; /** \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: Cone(); Cone(double r, double l); /** \brief The type of the shape, as a string */ static const std::string STRING_NAME; virtual void scaleAndPadd(double scale, double padd); virtual Shape* clone() const; virtual void print(std::ostream &out = std::cout) const; /** \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: Box(); Box(double x, double y, double z); /** \brief The type of the shape, as a string */ static const std::string STRING_NAME; virtual void scaleAndPadd(double scale, double padd); virtual Shape* clone() const; virtual void print(std::ostream &out = std::cout) const; /** \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. * */ class Mesh : public Shape { public: Mesh(); Mesh(unsigned int v_count, unsigned int t_count); virtual ~Mesh(); /** \brief The type of the shape, as a string */ static const std::string STRING_NAME; virtual void scaleAndPadd(double scale, double padd); virtual Shape* clone() const; virtual void print(std::ostream &out = std::cout) const; /** \brief The normals to each triangle can be computed from the vertices using cross products. This function performs this computation and allocates memory for normals if needed */ void computeTriangleNormals(); /** \brief The normals to each vertex, averaged from the triangle normals. computeTriangleNormals() is automatically called 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; virtual Shape* clone() const; virtual void print(std::ostream &out = std::cout) const; virtual void scaleAndPadd(double scale, double padd); virtual bool isFixed() const; /** \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; virtual Shape* clone() const; virtual void print(std::ostream &out = std::cout) const; virtual void scaleAndPadd(double scale, double padd); virtual bool isFixed() const; 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; } #endif geometric_shapes-0.5.2/include/geometric_shapes/solid_primitive_dims.h000066400000000000000000000077611300202320200263650ustar00rootroot00000000000000/********************************************************************* * 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_SOLID_PRIMITIVE_DIMS_ #define GEOMETRIC_SHAPES_SOLID_PRIMITIVE_DIMS_ #include namespace geometric_shapes { /** \brief The number of dimensions of a particular shape */ template struct SolidPrimitiveDimCount { enum { value = 0 }; }; template<> struct SolidPrimitiveDimCount { enum { value = static_cast(shape_msgs::SolidPrimitive::SPHERE_RADIUS) + 1 }; }; template<> struct SolidPrimitiveDimCount { enum { value = (static_cast(shape_msgs::SolidPrimitive::BOX_X) >= static_cast(shape_msgs::SolidPrimitive::BOX_Y) && static_cast(shape_msgs::SolidPrimitive::BOX_X) >= static_cast(shape_msgs::SolidPrimitive::BOX_Z)) ? static_cast(shape_msgs::SolidPrimitive::BOX_X) : (((static_cast(shape_msgs::SolidPrimitive::BOX_Y) >= static_cast(shape_msgs::SolidPrimitive::BOX_X) && static_cast(shape_msgs::SolidPrimitive::BOX_Y) >= static_cast(shape_msgs::SolidPrimitive::BOX_Z))) ? static_cast(shape_msgs::SolidPrimitive::BOX_Y) : ((static_cast(shape_msgs::SolidPrimitive::BOX_Z) >= static_cast(shape_msgs::SolidPrimitive::BOX_X) && static_cast(shape_msgs::SolidPrimitive::BOX_Z) >= static_cast(shape_msgs::SolidPrimitive::BOX_Y)) ? static_cast(shape_msgs::SolidPrimitive::BOX_Z) : 0)) + 1 }; }; template<> struct SolidPrimitiveDimCount { enum { value = (static_cast(shape_msgs::SolidPrimitive::CONE_RADIUS) >= static_cast(shape_msgs::SolidPrimitive::CONE_HEIGHT) ? static_cast(shape_msgs::SolidPrimitive::CONE_RADIUS) : static_cast(shape_msgs::SolidPrimitive::CONE_HEIGHT)) + 1 }; }; template<> struct SolidPrimitiveDimCount { enum { value = (static_cast(shape_msgs::SolidPrimitive::CYLINDER_RADIUS) >= static_cast(shape_msgs::SolidPrimitive::CYLINDER_HEIGHT) ? static_cast(shape_msgs::SolidPrimitive::CYLINDER_RADIUS) : static_cast(shape_msgs::SolidPrimitive::CYLINDER_HEIGHT)) + 1 }; }; } #endif geometric_shapes-0.5.2/package.xml000066400000000000000000000030521300202320200171340ustar00rootroot00000000000000 geometric_shapes 0.5.2 This package contains generic definitions of geometric shapes and bodies. Ioan Sucan Gil Jones Ioan Sucan Dave Coleman 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.5.2/src/000077500000000000000000000000001300202320200156065ustar00rootroot00000000000000geometric_shapes-0.5.2/src/bodies.cpp000066400000000000000000001071561300202320200175710ustar00rootroot00000000000000/********************************************************************* * 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 extern "C" { #ifdef GEOMETRIC_SHAPES_HAVE_QHULL_2011 #include #include #include #include #include #include #include #include #else #include #include #include #include #include #include #include #include #endif } #include #include #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.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; } }; } } void bodies::Body::setDimensions(const shapes::Shape *shape) { useDimensions(shape); updateInternalData(); } bool bodies::Body::samplePointInside(random_numbers::RandomNumberGenerator &rng, unsigned int max_attempts, Eigen::Vector3d &result) { 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 { std::vector d(1, radius_); return d; } void bodies::Sphere::updateInternalData() { radiusU_ = radius_ * scale_ + padding_; radius2_ = radiusU_ * radiusU_; center_ = pose_.translation(); } std::shared_ptr bodies::Sphere::cloneAt(const Eigen::Affine3d &pose, double padding, double scale) const { Sphere *s = new Sphere(); s->radius_ = radius_; s->padding_ = padding; s->scale_ = scale; s->pose_ = pose; s->updateInternalData(); return std::shared_ptr(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 = radiusU_; } bool bodies::Sphere::samplePointInside(random_numbers::RandomNumberGenerator &rng, unsigned int max_attempts, Eigen::Vector3d &result) { 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 { if (detail::distanceSQR(center_, origin, dir) > radius2_) return false; bool result = false; Eigen::Vector3d cp = origin - center_; double dpcpv = cp.dot(dir); Eigen::Vector3d w = cp - dpcpv * dir; Eigen::Vector3d Q = center_ + w; double x = radius2_ - w.squaredNorm(); if (fabs(x) < detail::ZERO) { w = Q - origin; double dpQv = w.dot(dir); if (dpQv > detail::ZERO) { if (intersections) intersections->push_back(Q); result = true; } } else if (x > 0.0) { x = sqrt(x); w = dir * x; Eigen::Vector3d A = Q - w; Eigen::Vector3d B = Q + w; w = A - origin; double dpAv = w.dot(dir); w = B - origin; double dpBv = w.dot(dir); 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; } 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 { std::vector d(2); d[0] = radius_; d[1] = length_; return d; } void bodies::Cylinder::updateInternalData() { radiusU_ = radius_ * scale_ + padding_; radius2_ = radiusU_ * radiusU_; length2_ = scale_ * length_ / 2.0 + padding_; center_ = pose_.translation(); radiusBSqr_ = length2_ * length2_ + radius2_; radiusB_ = sqrt(radiusBSqr_); Eigen::Matrix3d basis = pose_.rotation(); 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) { // 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 = Eigen::Vector3d(x, y, z); return true; } std::shared_ptr bodies::Cylinder::cloneAt(const Eigen::Affine3d &pose, double padding, double scale) const { Cylinder *c = new Cylinder(); c->length_ = length_; c->radius_ = radius_; c->padding_ = padding; c->scale_ = scale; c->pose_ = pose; c->updateInternalData(); return std::shared_ptr(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 = scale_*length_+padding_; } bool bodies::Cylinder::intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir, EigenSTL::vector_Vector3d *intersections, unsigned int count) const { if (detail::distanceSQR(center_, origin, dir) > radiusBSqr_) return false; std::vector ipts; // intersect bases double tmp = normalH_.dot(dir); if (fabs(tmp) > detail::ZERO) { double tmp2 = -normalH_.dot(origin); double t1 = (tmp2 - d1_) / tmp; if (t1 > 0.0) { Eigen::Vector3d p1(origin + dir * t1); Eigen::Vector3d v1(p1 - center_); v1 = v1 - normalH_.dot(v1) * normalH_; if (v1.squaredNorm() < radius2_ + detail::ZERO) { if (intersections == NULL) return true; detail::intersc ip(p1, t1); ipts.push_back(ip); } } double t2 = (tmp2 - d2_) / tmp; if (t2 > 0.0) { Eigen::Vector3d p2(origin + dir * t2); Eigen::Vector3d v2(p2 - center_); v2 = v2 - normalH_.dot(v2) * normalH_; if (v2.squaredNorm() < radius2_ + detail::ZERO) { if (intersections == NULL) return true; detail::intersc ip(p2, t2); ipts.push_back(ip); } } } if (ipts.size() < 2) { // intersect with infinite cylinder Eigen::Vector3d VD(normalH_.cross(dir)); 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 + dir * t1); Eigen::Vector3d v1(center_ - p1); if (fabs(normalH_.dot(v1)) < length2_ + detail::ZERO) { if (intersections == NULL) return true; detail::intersc ip(p1, t1); ipts.push_back(ip); } } if (t2 > 0.0) { Eigen::Vector3d p2(origin + dir * t2); Eigen::Vector3d v2(center_ - p2); if (fabs(normalH_.dot(v2)) < length2_ + detail::ZERO) { if (intersections == NULL) return true; detail::intersc ip(p2, t2); ipts.push_back(ip); } } } } if (ipts.empty()) return false; std::sort(ipts.begin(), ipts.end(), detail::interscOrder()); const unsigned int n = count > 0 ? std::min(count, ipts.size()) : ipts.size(); for (unsigned int i = 0 ; i < n ; ++i) intersections->push_back(ipts[i].pt); return true; } bool bodies::Box::samplePointInside(random_numbers::RandomNumberGenerator &rng, unsigned int /* max_attempts */, Eigen::Vector3d &result) { 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 { Eigen::Vector3d v = p - center_; double pL = v.dot(normalL_); if (fabs(pL) > length2_) return false; double pW = v.dot(normalW_); if (fabs(pW) > width2_) return false; double pH = v.dot(normalH_); if (fabs(pH) > height2_) return false; return true; } 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 { std::vector d(3); d[0] = length_; d[1] = width_; d[2] = height_; return d; } void bodies::Box::updateInternalData() { double s2 = scale_ / 2.0; length2_ = length_ * s2 + padding_; width2_ = width_ * s2 + padding_; height2_ = height_ * s2 + padding_; center_ = pose_.translation(); radius2_ = length2_ * length2_ + width2_ * width2_ + height2_ * height2_; radiusB_ = sqrt(radius2_); Eigen::Matrix3d basis = pose_.rotation(); normalL_ = basis.col(0); normalW_ = basis.col(1); normalH_ = basis.col(2); const Eigen::Vector3d tmp(normalL_ * length2_ + normalW_ * width2_ + normalH_ * height2_); corner1_ = center_ - tmp; corner2_ = center_ + tmp; } std::shared_ptr bodies::Box::cloneAt(const Eigen::Affine3d &pose, double padding, double scale) const { Box *b = new Box(); b->length_ = length_; b->width_ = width_; b->height_ = height_; b->padding_ = padding; b->scale_ = scale; b->pose_ = pose; b->updateInternalData(); return std::shared_ptr(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::Affine3d 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::Affine3d 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); } bool bodies::Box::intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir, EigenSTL::vector_Vector3d *intersections, unsigned int count) const { if (detail::distanceSQR(center_, origin, dir) > radius2_) return false; double t_near = -std::numeric_limits::infinity(); double t_far = std::numeric_limits::infinity(); for (int i = 0; i < 3; i++) { const Eigen::Vector3d &vN = i == 0 ? normalL_ : (i == 1 ? normalW_ : normalH_); double dp = vN.dot(dir); if (fabs(dp) > detail::ZERO) { double t1 = vN.dot(corner1_ - origin) / dp; double t2 = vN.dot(corner2_ - origin) / dp; if (t1 > t2) std::swap(t1, t2); if (t1 > t_near) t_near = t1; if (t2 < t_far) t_far = t2; if (t_near > t_far) return false; if (t_far < 0.0) return false; } else { if (i == 0) { if ((std::min(corner1_.y(), corner2_.y()) > origin.y() || std::max(corner1_.y(), corner2_.y()) < origin.y()) && (std::min(corner1_.z(), corner2_.z()) > origin.z() || std::max(corner1_.z(), corner2_.z()) < origin.z())) return false; } else { if (i == 1) { if ((std::min(corner1_.x(), corner2_.x()) > origin.x() || std::max(corner1_.x(), corner2_.x()) < origin.x()) && (std::min(corner1_.z(), corner2_.z()) > origin.z() || std::max(corner1_.z(), corner2_.z()) < origin.z())) return false; } else if ((std::min(corner1_.x(), corner2_.x()) > origin.x() || std::max(corner1_.x(), corner2_.x()) < origin.x()) && (std::min(corner1_.y(), corner2_.y()) > origin.y() || std::max(corner1_.y(), corner2_.y()) < origin.y())) return false; } } } if (intersections) { if (t_far - t_near > detail::ZERO) { intersections->push_back(t_near * dir + origin); if (count > 1) intersections->push_back(t_far * dir + origin); } else intersections->push_back(t_far * dir + origin); } return true; } bool bodies::ConvexMesh::containsPoint(const Eigen::Vector3d &p, bool verbose) const { if (!mesh_data_) return false; if (bounding_box_.containsPoint(p)) { Eigen::Vector3d ip(i_pose_ * p); ip = mesh_data_->mesh_center_ + (ip - mesh_data_->mesh_center_) / scale_; 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_.reset(new MeshData()); 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"; int exitcode = qh_new_qhull(3, mesh->vertex_count, points, true, flags, null, null); if (exitcode != 0) { logWarn("Convex hull creation failed"); qh_freeqhull (!qh_ALL); int curlong, totlong; qh_memfreeshort (&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::Vector4f 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_ ((*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; } qh_freeqhull(!qh_ALL); int curlong, totlong; qh_memfreeshort (&curlong, &totlong); } std::vector bodies::ConvexMesh::getDimensions() const { return std::vector(); } 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 = 0; t < vertex_to_tris[i].size(); ++ t) { const Eigen::Vector4f & plane = mesh_data_->planes_[mesh_data_->plane_for_triangle_[vertex_to_tris[i][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(unsigned int v = 0; v < projected_vertices.size(); ++ v) { sum += projected_vertices[v]; } sum /= projected_vertices.size(); scaled_vertices_storage_->at(i) = sum; } } } void bodies::ConvexMesh::updateInternalData() { if (!mesh_data_) return; Eigen::Affine3d pose = pose_; pose.translation() = Eigen::Vector3d(pose_ * mesh_data_->box_offset_); std::unique_ptr box_shape(new shapes::Box(mesh_data_->box_size_.x(), mesh_data_->box_size_.y(), mesh_data_->box_size_.z())); bounding_box_.setDimensions(box_shape.get()); bounding_box_.setPose(pose); bounding_box_.setPadding(padding_); bounding_box_.setScale(scale_); 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(); } std::shared_ptr bodies::ConvexMesh::cloneAt(const Eigen::Affine3d &pose, double padding, double scale) const { ConvexMesh *m = new ConvexMesh(); m->mesh_data_ = mesh_data_; m->padding_ = padding; m->scale_ = scale; m->pose_ = pose; m->updateInternalData(); return std::shared_ptr(m); } void bodies::ConvexMesh::computeBoundingSphere(BoundingSphere &sphere) const { sphere.center = center_; sphere.radius = radiusB_; } void bodies::ConvexMesh::computeBoundingCylinder(BoundingCylinder &cylinder) const { cylinder.length = mesh_data_ ? mesh_data_->bounding_cylinder_.length : 0.0; cylinder.radius = mesh_data_ ? mesh_data_->bounding_cylinder_.radius : 0.0; //need to do rotation correctly to get pose, which bounding box does BoundingCylinder cyl; bounding_box_.computeBoundingCylinder(cyl); cylinder.pose = cyl.pose; } 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::Vector4f& plane = mesh_data_->planes_[i]; Eigen::Vector3d plane_vec(plane.x(), plane.y(), plane.z()); double dist = plane_vec.dot(point) + plane.w() - padding_ - 1e-6; 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 { if (!mesh_data_) return false; if (detail::distanceSQR(center_, origin, dir) > radiusBSqr_) return false; if (!bounding_box_.intersectsRay(origin, dir)) return false; // transform the ray into the coordinate frame of the mesh Eigen::Vector3d orig(i_pose_ * origin); Eigen::Vector3d dr(i_pose_ * dir); std::vector ipts; bool result = false; // for each triangle const unsigned int nt = mesh_data_->triangles_.size() / 3; for (unsigned int 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()); double tmp = vec.dot(dr); if (fabs(tmp) > detail::ZERO) { double t = -(vec.dot(orig) + mesh_data_->planes_[mesh_data_->plane_for_triangle_[i]].w()) / tmp; if (t > 0.0) { const int i3 = 3 * i; const int v1 = mesh_data_->triangles_[i3 + 0]; const int v2 = mesh_data_->triangles_[i3 + 1]; const int 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 + dir * t, t); ipts.push_back(ip); } else break; } } } if (intersections) { std::sort(ipts.begin(), ipts.end(), detail::interscOrder()); const unsigned int n = count > 0 ? std::min(count, ipts.size()) : ipts.size(); for (unsigned int i = 0 ; i < n ; ++i) intersections->push_back(ipts[i].pt); } return result; } bodies::BodyVector::BodyVector() { } bodies::BodyVector::BodyVector(const std::vector& shapes, const EigenSTL::vector_Affine3d& 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(unsigned int i = 0; i < bodies_.size(); i++) delete bodies_[i]; 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::Affine3d& pose, double padding) { bodies::Body* body = bodies::createBodyFromShape(shape); body->setPose(pose); body->setPadding(padding); addBody(body); } std::size_t bodies::BodyVector::getCount() const { return bodies_.size(); } void bodies::BodyVector::setPose(unsigned int i, const Eigen::Affine3d& pose) { if (i >= bodies_.size()) { 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()) { logError("There is no body at index %u", i); return NULL; } 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.5.2/src/body_operations.cpp000066400000000000000000000135001300202320200215110ustar00rootroot00000000000000/********************************************************************* * 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 bodies::Body* bodies::createBodyFromShape(const shapes::Shape *shape) { Body *body = NULL; if (shape) switch (shape->type) { case shapes::BOX: body = new bodies::Box(shape); break; case shapes::SPHERE: body = new bodies::Sphere(shape); break; case shapes::CYLINDER: body = new bodies::Cylinder(shape); break; case shapes::MESH: body = new bodies::ConvexMesh(shape); break; default: logError("Creating body from shape: Unknown shape type %d", (int)shape->type); break; } return body; } 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::Shape *shape = shapes::constructShapeFromMsg(shape_msg); if (shape) { Body *body = createBodyFromShape(shape); 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) { logError("Quaternion is not normalized. Assuming identity."); q = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0); } Eigen::Affine3d af(Eigen::Translation3d(pose.position.x, pose.position.y, pose.position.z) * q.toRotationMatrix()); body->setPose(af); return body; } } return NULL; } } 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(unsigned int i = 0; i < bodies.size(); i++) { const bodies::ConvexMesh* conv = dynamic_cast(bodies[i]); if(!conv) continue; 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(unsigned int i = 0; i < bodies.size(); i++) { const bodies::ConvexMesh* conv = dynamic_cast(bodies[i]); if(!conv) continue; 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); } geometric_shapes-0.5.2/src/mesh_operations.cpp000066400000000000000000000532061300202320200215170ustar00rootroot00000000000000/********************************************************************* * 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; } }; } } 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 NULL; if (source.size() % 3 != 0) 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) { logWarn("Cannot construct mesh from empty binary buffer"); return NULL; } // 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 NULL; // 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) { logError("%s", e.what()); return NULL; } if (res.size == 0) { logWarn("Retrieved empty mesh for resource '%s'", resource.c_str()); return NULL; } Mesh *m = createMeshFromBinary(reinterpret_cast(res.data.get()), res.size, scale, resource); if (!m) logWarn("Assimp reports no scene in %s", resource.c_str()); 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); } } 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()) { logWarn("Assimp reports scene in %s has no meshes", resource_name.c_str()); return NULL; } EigenSTL::vector_Vector3d vertices; std::vector triangles; extractMeshData(scene, scene->mRootNode, aiMatrix4x4(), scale, vertices, triangles); if (vertices.empty()) { logWarn("There are no vertices in the scene %s", resource_name.c_str()); return NULL; } if (triangles.empty()) { logWarn("There are no triangles in the scene %s", resource_name.c_str()); return NULL; } 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 logError("Conversion of shape of type '%s' to a mesh is not known", shapeStringName(shape).c_str()); return NULL; } 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 = ceil(tot_for_unit_cylinder * 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 < 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 , float data) { memcpy(ptr, &data, sizeof(float)); ptr += sizeof(float); } inline void writeFloatToSTL(char *&ptr , double datad) { float data = datad; memcpy(ptr, &data, sizeof(float)); ptr += sizeof(float); } } 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; } } } geometric_shapes-0.5.2/src/shape_extents.cpp000066400000000000000000000113571300202320200211730ustar00rootroot00000000000000/********************************************************************* * 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 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() > shape_msgs::SolidPrimitive::SPHERE_RADIUS) 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() > shape_msgs::SolidPrimitive::BOX_X && shape_msg.dimensions.size() > shape_msgs::SolidPrimitive::BOX_Y && shape_msg.dimensions.size() > shape_msgs::SolidPrimitive::BOX_Z) { 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() > shape_msgs::SolidPrimitive::CYLINDER_RADIUS && shape_msg.dimensions.size() > shape_msgs::SolidPrimitive::CYLINDER_HEIGHT) { 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() > shape_msgs::SolidPrimitive::CONE_RADIUS && shape_msg.dimensions.size() > shape_msgs::SolidPrimitive::CONE_HEIGHT) { 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.size() > 0) { 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 (std::size_t i = 0; i < shape_msg.vertices.size() ; ++i) { if (shape_msg.vertices[i].x > xmax) xmax = shape_msg.vertices[i].x; if (shape_msg.vertices[i].x < xmin) xmin = shape_msg.vertices[i].x; if (shape_msg.vertices[i].y > ymax) ymax = shape_msg.vertices[i].y; if (shape_msg.vertices[i].y < ymin) ymin = shape_msg.vertices[i].y; if (shape_msg.vertices[i].z > zmax) zmax = shape_msg.vertices[i].z; if (shape_msg.vertices[i].z < zmin) zmin = shape_msg.vertices[i].z; } x_extent = xmax-xmin; y_extent = ymax-ymin; z_extent = zmax-zmin; } } geometric_shapes-0.5.2/src/shape_operations.cpp000066400000000000000000000473701300202320200216700ustar00rootroot00000000000000/********************************************************************* * 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()) { logWarn("Mesh definition is empty"); return NULL; } 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 = NULL; if (shape_msg.type == shape_msgs::SolidPrimitive::SPHERE) { if (shape_msg.dimensions.size() > shape_msgs::SolidPrimitive::SPHERE_RADIUS) 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() > shape_msgs::SolidPrimitive::BOX_X && shape_msg.dimensions.size() > shape_msgs::SolidPrimitive::BOX_Y && shape_msg.dimensions.size() > shape_msgs::SolidPrimitive::BOX_Z) 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() > shape_msgs::SolidPrimitive::CYLINDER_RADIUS && shape_msg.dimensions.size() > shape_msgs::SolidPrimitive::CYLINDER_HEIGHT) 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() > shape_msgs::SolidPrimitive::CONE_RADIUS && shape_msg.dimensions.size() > shape_msgs::SolidPrimitive::CONE_HEIGHT) shape = new Cone(shape_msg.dimensions[shape_msgs::SolidPrimitive::CONE_RADIUS], shape_msg.dimensions[shape_msgs::SolidPrimitive::CONE_HEIGHT]); } if (shape == NULL) 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); } }; } 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_; }; } 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) { 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; } }; } 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::value); 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::value); 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::value); 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::value); 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 { 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 { logError("Unable to save shape of type %d", (int)shape->type); } } Shape* constructShapeFromText(std::istream &in) { Shape *result = NULL; 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 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; } } } geometric_shapes-0.5.2/src/shape_to_marker.cpp000066400000000000000000000135531300202320200214640ustar00rootroot00000000000000/********************************************************************* * 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::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() <= shape_msgs::SolidPrimitive::SPHERE_RADIUS) 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() <= shape_msgs::SolidPrimitive::BOX_X || shape_msg.dimensions.size() <= shape_msgs::SolidPrimitive::BOX_Y || shape_msg.dimensions.size() <= shape_msgs::SolidPrimitive::BOX_Z) 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() <= shape_msgs::SolidPrimitive::CONE_RADIUS || shape_msg.dimensions.size() <= shape_msgs::SolidPrimitive::CONE_HEIGHT) 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() <= shape_msgs::SolidPrimitive::CYLINDER_RADIUS || shape_msg.dimensions.size() <= shape_msgs::SolidPrimitive::CYLINDER_HEIGHT) 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.5.2/src/shapes.cpp000066400000000000000000000324721300202320200176050ustar00rootroot00000000000000/********************************************************************* * 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 const std::string shapes::Sphere::STRING_NAME = "sphere"; const std::string shapes::Box::STRING_NAME = "box"; const std::string shapes::Cylinder::STRING_NAME = "cylinder"; const std::string shapes::Cone::STRING_NAME = "cone"; const std::string shapes::Mesh::STRING_NAME = "mesh"; const std::string shapes::Plane::STRING_NAME = "plane"; const std::string shapes::OcTree::STRING_NAME = "octree"; shapes::Shape::Shape() { type = UNKNOWN_SHAPE; } shapes::Shape::~Shape() { } shapes::Sphere::Sphere() : Shape() { type = SPHERE; radius = 0.0; } shapes::Sphere::Sphere(double r) : Shape() { type = SPHERE; radius = r; } shapes::Cylinder::Cylinder() : Shape() { type = CYLINDER; length = radius = 0.0; } shapes::Cylinder::Cylinder(double r, double l) : Shape() { type = CYLINDER; length = l; radius = r; } shapes::Cone::Cone() : Shape() { type = CONE; length = radius = 0.0; } shapes::Cone::Cone(double r, double l) : Shape() { type = CONE; length = l; radius = r; } shapes::Box::Box() : Shape() { type = BOX; size[0] = size[1] = size[2] = 0.0; } shapes::Box::Box(double x, double y, double z) : Shape() { type = BOX; size[0] = x; size[1] = y; size[2] = z; } shapes::Mesh::Mesh() : Shape() { type = MESH; vertex_count = 0; vertices = NULL; triangle_count = 0; triangles = NULL; triangle_normals = NULL; vertex_normals = NULL; } shapes::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]; } shapes::Mesh::~Mesh() { if (vertices) delete[] vertices; if (triangles) delete[] triangles; if (triangle_normals) delete[] triangle_normals; if (vertex_normals) delete[] vertex_normals; } shapes::Plane::Plane() : Shape() { type = PLANE; a = b = c = d = 0.0; } shapes::Plane::Plane(double pa, double pb, double pc, double pd) : Shape() { type = PLANE; a = pa; b = pb; c = pc; d = pd; } shapes::OcTree::OcTree() : Shape() { type = OCTREE; } shapes::OcTree::OcTree(const std::shared_ptr &t) : octree(t) { type = OCTREE; } shapes::Shape* shapes::Sphere::clone() const { return new Sphere(radius); } shapes::Shape* shapes::Cylinder::clone() const { return new Cylinder(radius, length); } shapes::Shape* shapes::Cone::clone() const { return new Cone(radius, length); } shapes::Shape* shapes::Box::clone() const { return new Box(size[0], size[1], size[2]); } shapes::Shape* shapes::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 = NULL; } 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 = NULL; } return dest; } shapes::Shape* shapes::Plane::clone() const { return new Plane(a, b, c, d); } shapes::Shape* shapes::OcTree::clone() const { return new OcTree(octree); } void shapes::OcTree::scaleAndPadd(double scale, double padd) { logWarn("OcTrees cannot be scaled or padded"); } void shapes::Plane::scaleAndPadd(double scale, double padding) { logWarn("Planes cannot be scaled or padded"); } void shapes::Shape::scale(double scale) { scaleAndPadd(scale, 0.0); } void shapes::Shape::padd(double padding) { scaleAndPadd(1.0, padding); } void shapes::Sphere::scaleAndPadd(double scale, double padding) { radius = radius * scale + padding; } void shapes::Cylinder::scaleAndPadd(double scale, double padding) { radius = radius * scale + padding; length = length * scale + 2.0 * padding; } void shapes::Cone::scaleAndPadd(double scale, double padding) { radius = radius * scale + padding; length = length * scale + 2.0 * padding; } void shapes::Box::scaleAndPadd(double scale, double padding) { double p2 = padding * 2.0; size[0] = size[0] * scale + p2; size[1] = size[1] * scale + p2; size[2] = size[2] * scale + p2; } void shapes::Mesh::scaleAndPadd(double scale, double padding) { // 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) { double fact = scale + padding/norm; vertices[i3] = sx + dx * fact; vertices[i3 + 1] = sy + dy * fact; vertices[i3 + 2] = sz + dz * fact; } else { double ndx = ((dx > 0) ? dx+padding : dx-padding); double ndy = ((dy > 0) ? dy+padding : dy-padding); double ndz = ((dz > 0) ? dz+padding : dz-padding); vertices[i3] = sx + ndx; vertices[i3 + 1] = sy + ndy; vertices[i3 + 2] = sz + ndz; } } } void shapes::Shape::print(std::ostream &out) const { out << this << std::endl; } void shapes::Sphere::print(std::ostream &out) const { out << "Sphere[radius=" << radius << "]" << std::endl; } void shapes::Cylinder::print(std::ostream &out) const { out << "Cylinder[radius=" << radius << ", length=" << length << "]" << std::endl; } void shapes::Cone::print(std::ostream &out) const { out << "Cone[radius=" << radius << ", length=" << length << "]" << std::endl; } void shapes::Box::print(std::ostream &out) const { out << "Box[x=length=" << size[0] << ", y=width=" << size[1] << "z=height=" << size[2] << "]" << std::endl; } void shapes::Mesh::print(std::ostream &out) const { out << "Mesh[vertices=" << vertex_count << ", triangles=" << triangle_count << "]" << std::endl; } void shapes::Plane::print(std::ostream &out) const { out << "Plane[a=" << a << ", b=" << b << ", c=" << c << ", d=" << d << "]" << std::endl; } void shapes::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 shapes::Shape::isFixed() const { return false; } bool shapes::OcTree::isFixed() const { return true; } bool shapes::Plane::isFixed() const { return true; } void shapes::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 shapes::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 shapes::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(); } geometric_shapes-0.5.2/test/000077500000000000000000000000001300202320200157765ustar00rootroot00000000000000geometric_shapes-0.5.2/test/CMakeLists.txt000066400000000000000000000017411300202320200205410ustar00rootroot00000000000000file(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_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_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}) geometric_shapes-0.5.2/test/resources/000077500000000000000000000000001300202320200200105ustar00rootroot00000000000000geometric_shapes-0.5.2/test/resources/config.h.in000066400000000000000000000002321300202320200220300ustar00rootroot00000000000000#ifndef GEOMETRIC_SHAPES_TEST_RESOURCES_CONFIG_ #define GEOMETRIC_SHAPES_TEST_RESOURCES_CONFIG_ #define TEST_RESOURCES_DIR "@TEST_RESOURCES_DIR@" #endif geometric_shapes-0.5.2/test/resources/cube.stl000066400000000000000000000012541300202320200214540ustar00rootroot00000000000000Exported from Blender-2.77 (sub 0) >>> 3>>>3??>?>????>??>?? >>?>?>??4>?>>>>>??3?>4?>?4>?> `4?4?>?>>>4?3>>??>?geometric_shapes-0.5.2/test/resources/forearm_roll.stl000066400000000000000000001411061300202320200232220ustar00rootroot00000000000000VCG ?%>? ?=2$= =O#'=%=!j*%=5?!?9>=`=Y;=G=纇=\;=+9m4?jn4?=qٻn<=B>軸<=¾ջ#q<?#N=cuO=;=LN=c̼K?= ==Hr@=佼 ^?=j++>Ҿ)V=8|WȾ?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.5.2/test/resources/triangle_1m.dae000066400000000000000000000023721300202320200226710ustar00rootroot00000000000000 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.5.2/test/resources/triangle_no_unit.dae000066400000000000000000000023271300202320200240270ustar00rootroot00000000000000 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.5.2/test/resources/triangle_no_up.dae000066400000000000000000000023721300202320200234740ustar00rootroot00000000000000 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.5.2/test/resources/triangle_x_up.dae000066400000000000000000000024261300202320200233270ustar00rootroot00000000000000 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.5.2/test/resources/triangle_y_up.dae000066400000000000000000000024261300202320200233300ustar00rootroot00000000000000 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.5.2/test/resources/triangle_z_up.dae000066400000000000000000000024261300202320200233310ustar00rootroot00000000000000 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.5.2/test/test_bounding_sphere.cpp000066400000000000000000000163141300202320200227210ustar00rootroot00000000000000/********************************************************************* * 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); shapes::Mesh *m = dynamic_cast(shape); EXPECT_TRUE(m); // 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.5.2/test/test_create_mesh.cpp000066400000000000000000000067271300202320200220340ustar00rootroot00000000000000/********************************************************************* * 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 != NULL); ASSERT_EQ(3, mesh->vertex_count); ASSERT_EQ(1, mesh->triangle_count); ASSERT_EQ(0, mesh->triangles[0]); ASSERT_EQ(1, mesh->triangles[1]); ASSERT_EQ(2, 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); } } 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.5.2/test/test_loaded_meshes.cpp000066400000000000000000000162441300202320200223440ustar00rootroot00000000000000/********************************************************************* * 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() { // 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() { for (int 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() { } 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 (int i = 0; i < shape_meshes.size(); ++i) { shapes::Mesh *shape_ms = shape_meshes[i]; shapes::Mesh *loaded_ms = loaded_meshes[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 (int i = 0; i < shape_meshes.size(); ++i) { shapes::Mesh *shape_ms = shape_meshes[i]; shapes::Mesh *loaded_ms = loaded_meshes[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.size() > 0 && vi2.size() > 0) { 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 (int i = 0; i < shape_meshes.size(); ++i) { shapes::Mesh *shape_ms = shape_meshes[i]; shapes::Mesh *loaded_ms = loaded_meshes[i]; bodies::Body *shape_cms = shape_convex_meshes[i]; bodies::Body *loaded_cms = loaded_convex_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.5.2/test/test_point_inclusion.cpp000066400000000000000000000221441300202320200227600ustar00rootroot00000000000000/********************************************************************* * 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" TEST(SpherePointContainment, SimpleInside) { shapes::Sphere shape(1.0); bodies::Body* sphere = new bodies::Sphere(&shape); sphere->setScale(1.05); bool contains = sphere->containsPoint(0,0,1.0); random_numbers::RandomNumberGenerator r; Eigen::Vector3d p; EXPECT_TRUE(sphere->samplePointInside(r, 100, p)); EXPECT_TRUE(sphere->containsPoint(p)); EXPECT_TRUE(contains); 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::Affine3d 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::Affine3d 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(SphereRayIntersection, SimpleRay1) { shapes::Sphere shape(1.0); bodies::Body* sphere = new bodies::Sphere(&shape); sphere->setScale(1.05); Eigen::Vector3d ray_o(5, 0, 0); Eigen::Vector3d ray_d(-1, 0, 0); EigenSTL::vector_Vector3d p; bool intersect = sphere->intersectsRay(ray_o, ray_d, &p); delete sphere; EXPECT_TRUE(intersect); EXPECT_EQ(2, (int)p.size()); EXPECT_NEAR(p[0].x(), 1.05, 1e-6); EXPECT_NEAR(p[1].x(), -1.05, 1e-6); } TEST(SphereRayIntersection, SimpleRay2) { shapes::Sphere shape(1.0); bodies::Body* sphere = new bodies::Sphere(&shape); sphere->setScale(1.05); Eigen::Vector3d ray_o(5, 0, 0); Eigen::Vector3d ray_d(1, 0, 0); EigenSTL::vector_Vector3d p; bool intersect = sphere->intersectsRay(ray_o, ray_d, &p); delete sphere; EXPECT_FALSE(intersect); EXPECT_EQ(0, (int)p.size()); } 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::Affine3d 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); random_numbers::RandomNumberGenerator r; Eigen::Vector3d p; for (int i = 0 ; i < 100 ; ++i) { EXPECT_TRUE(box->samplePointInside(r, 100, p)); EXPECT_TRUE(box->containsPoint(p)); } delete box; } TEST(BoxPointContainment, ComplexOutside) { shapes::Box shape(1.0, 1.0, 1.0); bodies::Body* box = new bodies::Box(&shape); box->setScale(1.01); Eigen::Affine3d 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(BoxRayIntersection, SimpleRay1) { shapes::Box shape(1.0, 1.0, 3.0); bodies::Body* box = new bodies::Box(&shape); box->setScale(0.95); Eigen::Vector3d ray_o(10, 0.449, 0); Eigen::Vector3d ray_d(-1, 0, 0); EigenSTL::vector_Vector3d p; bool intersect = box->intersectsRay(ray_o, ray_d, &p); // for (unsigned int i = 0; i < p.size() ; ++i) // printf("intersection at %f, %f, %f\n", p[i].x(), p[i].y(), p[i].z()); delete box; EXPECT_TRUE(intersect); } 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); random_numbers::RandomNumberGenerator r; Eigen::Vector3d p; for (int i = 0 ; i < 1000 ; ++i) { EXPECT_TRUE(cylinder->samplePointInside(r, 100, p)); EXPECT_TRUE(cylinder->containsPoint(p)); } delete cylinder; } TEST(MeshPointContainment, Pr2Forearm) { shapes::Mesh *ms = shapes::createMeshFromResource("file://" + (boost::filesystem::path(TEST_RESOURCES_DIR) / "/forearm_roll.stl").string()); ASSERT_TRUE(ms != NULL); bodies::Body *m = new bodies::ConvexMesh(ms); ASSERT_TRUE(m != NULL); Eigen::Affine3d t(Eigen::Affine3d::Identity()); t.translation().x() = 1.0; EXPECT_FALSE(m->cloneAt(t)->containsPoint(-1.0, 0.0, 0.0)); random_numbers::RandomNumberGenerator r; 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(); }