Handle FCL 0.6 (#873)

CHANGELOG.md

@@ -2,6 +2,10 @@
 
 ### DART 7.0.0 (201X-XX-XX)
 
+* Collision detection
+
+  * Added FCL 0.6 support: [#873](https://github.com/dartsim/dart/pull/873)
+
 * Optimizer
 
   * Changed Function::eval\[Gradient/Hessian\](...) to const function: [#928](https://github.com/dartsim/dart/pull/928)

CMakeLists.txt

@@ -189,6 +189,7 @@ elseif(CMAKE_COMPILER_IS_GNUCXX)
   set(CMAKE_CXX_FLAGS_DEBUG "-g -fno-omit-frame-pointer -fno-inline-functions -fno-inline-functions-called-once -fno-optimize-sibling-calls")
   set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "${CMAKE_CXX_FLAGS_RELEASE} ${CMAKE_CXX_FLAGS_DEBUG}")
   set(CMAKE_CXX_FLAGS_PROFILE "${CMAKE_CXX_FLAGS_DEBUG} -pg")
+  set(CMAKE_SHARED_LINKER_FLAGS "-Wl,--no-undefined")
 
 elseif("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang")
 

cmake/FindCCD.cmake

@@ -20,6 +20,14 @@ find_package(PkgConfig QUIET)
 pkg_check_modules(PC_CCD ccd QUIET)
 
 # Include directories
+# Give explicit precedence to ${PC_CCD_INCLUDEDIR}
+find_path(CCD_INCLUDE_DIRS
+    NAMES ccd/ccd.h
+    HINTS ${PC_CCD_INCLUDEDIR}
+    NO_DEFAULT_PATH
+    NO_CMAKE_PATH
+    NO_CMAKE_ENVIRONMENT_PATH
+    NO_SYSTEM_ENVIRONMENT_PATH)
 find_path(CCD_INCLUDE_DIRS
     NAMES ccd/ccd.h
     HINTS ${PC_CCD_INCLUDEDIR}
@@ -29,6 +37,14 @@ find_path(CCD_INCLUDE_DIRS
 if(MSVC)
   set(CCD_LIBRARIES optimized ccd debug ccdd)
 else()
+  # Give explicit precedence to ${PC_CCD_LIBDIR}
+  find_library(CCD_LIBRARIES
+      NAMES ccd
+      HINTS ${PC_CCD_LIBDIR}
+      NO_DEFAULT_PATH
+      NO_CMAKE_PATH
+      NO_CMAKE_ENVIRONMENT_PATH
+      NO_SYSTEM_ENVIRONMENT_PATH)
   find_library(CCD_LIBRARIES
       NAMES ccd
       HINTS ${PC_CCD_LIBDIR})

cmake/FindFCL.cmake

@@ -20,15 +20,30 @@ find_package(PkgConfig QUIET)
 pkg_check_modules(PC_FCL fcl QUIET)
 
 # Include directories
-find_path(FCL_INCLUDE_DIRS
-    NAMES fcl/collision.h
-    HINTS ${PC_FCL_INCLUDEDIR}
-    PATHS "${CMAKE_INSTALL_PREFIX}/include")
+if (${PC_FCL_VERSION} VERSION_LESS 0.6.0)
+  find_path(FCL_INCLUDE_DIRS
+      NAMES fcl/collision.h
+      HINTS ${PC_FCL_INCLUDEDIR}
+      PATHS "${CMAKE_INSTALL_PREFIX}/include")
+else()
+  find_path(FCL_INCLUDE_DIRS
+      NAMES fcl/narrowphase/collision.h
+      HINTS ${PC_FCL_INCLUDEDIR}
+      PATHS "${CMAKE_INSTALL_PREFIX}/include")
+endif()
 
 # Libraries
 if(MSVC)
   set(FCL_LIBRARIES optimized fcl debug fcld)
 else()
+  # Give explicit precedence to ${PC_FCL_LIBDIR}
+  find_library(FCL_LIBRARIES
+      NAMES fcl
+      HINTS ${PC_FCL_LIBDIR}
+      NO_DEFAULT_PATH
+      NO_CMAKE_PATH
+      NO_CMAKE_ENVIRONMENT_PATH
+      NO_SYSTEM_ENVIRONMENT_PATH)
   find_library(FCL_LIBRARIES
       NAMES fcl
       HINTS ${PC_FCL_LIBDIR})

dart/collision/fcl/BackwardCompatibility.cpp

@@ -0,0 +1,134 @@
+/*
+ * Copyright (c) 2011-2017, The DART development contributors
+ * All rights reserved.
+ *
+ * The list of contributors can be found at:
+ *   https://github.com/dartsim/dart/blob/master/LICENSE
+ *
+ * This file is provided under the following "BSD-style" License:
+ *   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.
+ *   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 HOLDER 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 "dart/collision/fcl/BackwardCompatibility.hpp"
+
+namespace dart {
+namespace collision {
+namespace fcl {
+
+//==============================================================================
+double length(const dart::collision::fcl::Vector3& t)
+{
+#if FCL_VERSION_AT_LEAST(0,6,0)
+  return t.norm();
+#else
+  return t.length();
+#endif
+}
+
+//==============================================================================
+dart::collision::fcl::Vector3 getTranslation(
+    const dart::collision::fcl::Transform3& T)
+{
+#if FCL_VERSION_AT_LEAST(0,6,0)
+  return T.translation();
+#else
+  return T.getTranslation();
+#endif
+}
+
+//==============================================================================
+void setTranslation(
+    dart::collision::fcl::Transform3& T, const dart::collision::fcl::Vector3& t)
+{
+#if FCL_VERSION_AT_LEAST(0,6,0)
+  T.translation() = t;
+#else
+  T.setTranslation(t);
+#endif
+}
+
+//==============================================================================
+dart::collision::fcl::Matrix3 getRotation(
+    const dart::collision::fcl::Transform3& T)
+{
+#if FCL_VERSION_AT_LEAST(0,6,0)
+  return T.linear();
+#else
+  return T.getRotation();
+#endif
+}
+
+//==============================================================================
+void setRotation(
+    dart::collision::fcl::Transform3& T, const dart::collision::fcl::Matrix3& R)
+{
+#if FCL_VERSION_AT_LEAST(0,6,0)
+  T.linear() = R;
+#else
+  T.setRotation(R);
+#endif
+}
+
+//==============================================================================
+void setEulerZYX(
+    dart::collision::fcl::Matrix3& rot,
+    double eulerX,
+    double eulerY,
+    double eulerZ)
+{
+#if FCL_VERSION_AT_LEAST(0,6,0)
+  double ci(cos(eulerX));
+  double cj(cos(eulerY));
+  double ch(cos(eulerZ));
+  double si(sin(eulerX));
+  double sj(sin(eulerY));
+  double sh(sin(eulerZ));
+  double cc = ci * ch;
+  double cs = ci * sh;
+  double sc = si * ch;
+  double ss = si * sh;
+
+  rot << cj * ch, sj * sc - cs, sj * cc + ss,
+         cj * sh, sj * ss + cc, sj * cs - sc,
+         -sj,     cj * si,      cj * ci;
+#else
+  rot.setEulerZYX(eulerX, eulerY, eulerZ);
+#endif
+}
+
+//==============================================================================
+dart::collision::fcl::Vector3 transform(
+      const dart::collision::fcl::Transform3 &t,
+    const dart::collision::fcl::Vector3 &v)
+{
+#if FCL_VERSION_AT_LEAST(0,6,0)
+  return t * v;
+#else
+  return t.transform(v);
+#endif
+}
+
+} // namespace fcl
+} // namespace collision
+} // namespace dart

dart/collision/fcl/BackwardCompatibility.hpp

@@ -0,0 +1,172 @@
+/*
+ * Copyright (c) 2011-2017, The DART development contributors
+ * All rights reserved.
+ *
+ * The list of contributors can be found at:
+ *   https://github.com/dartsim/dart/blob/master/LICENSE
+ *
+ * This file is provided under the following "BSD-style" License:
+ *   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.
+ *   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 HOLDER 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 DART_COLLISION_FCL_BACKWARDCOMPATIBILITY_HPP_
+#define DART_COLLISION_FCL_BACKWARDCOMPATIBILITY_HPP_
+
+#include <Eigen/Dense>
+
+#define FCL_VERSION_AT_LEAST(x,y,z) \
+  (FCL_MAJOR_VERSION > x || (FCL_MAJOR_VERSION >= x && \
+  (FCL_MINOR_VERSION > y || (FCL_MINOR_VERSION >= y && \
+  FCL_PATCH_VERSION >= z))))
+
+#define FCL_MAJOR_MINOR_VERSION_AT_MOST(x,y) \
+  (FCL_MAJOR_VERSION < x || (FCL_MAJOR_VERSION <= x && \
+  (FCL_MINOR_VERSION < y || (FCL_MINOR_VERSION <= y))))
+
+#include <fcl/config.h>
+
+#if FCL_VERSION_AT_LEAST(0,6,0)
+#include <fcl/math/geometry.h>
+
+#include <fcl/geometry/bvh/BVH_model.h>
+#include <fcl/geometry/geometric_shape_to_BVH_model.h>
+#include <fcl/math/bv/OBBRSS.h>
+#include <fcl/math/bv/utility.h>
+#include <fcl/narrowphase/collision.h>
+#include <fcl/narrowphase/collision_object.h>
+#include <fcl/narrowphase/distance.h>
+#else
+#include <fcl/math/vec_3f.h>
+#include <fcl/math/matrix_3f.h>
+#include <fcl/math/transform.h>
+
+#include <fcl/BV/OBBRSS.h>
+#include <fcl/BVH/BVH_model.h>
+#include <fcl/collision.h>
+#include <fcl/collision_data.h>
+#include <fcl/collision_object.h>
+#include <fcl/distance.h>
+#endif
+#include <fcl/broadphase/broadphase_dynamic_AABB_tree.h>
+
+#if FCL_VERSION_AT_LEAST(0,5,0)
+#include <memory>
+template <class T> using fcl_shared_ptr = std::shared_ptr<T>;
+template <class T> using fcl_weak_ptr = std::weak_ptr<T>;
+#else
+#include <boost/weak_ptr.hpp>
+template <class T> using fcl_shared_ptr = boost::shared_ptr<T>;
+template <class T> using fcl_weak_ptr = boost::weak_ptr<T>;
+#endif
+
+namespace dart {
+namespace collision {
+namespace fcl {
+
+#if FCL_VERSION_AT_LEAST(0,6,0)
+// Geometric fundamentals
+using Vector3 = ::fcl::Vector3<double>;
+using Matrix3 = ::fcl::Matrix3<double>;
+using Transform3 = ::fcl::Transform3<double>;
+// Geometric primitives
+using Box = ::fcl::Box<double>;
+using Cylinder = ::fcl::Cylinder<double>;
+using Ellipsoid = ::fcl::Ellipsoid<double>;
+using Halfspace = ::fcl::Halfspace<double>;
+using Sphere = ::fcl::Sphere<double>;
+// Collision objects
+using CollisionObject = ::fcl::CollisionObject<double>;
+using CollisionGeometry = ::fcl::CollisionGeometry<double>;
+using DynamicAABBTreeCollisionManager
+    = ::fcl::DynamicAABBTreeCollisionManager<double>;
+using OBBRSS = ::fcl::OBBRSS<double>;
+using CollisionRequest = ::fcl::CollisionRequest<double>;
+using CollisionResult = ::fcl::CollisionResult<double>;
+using DistanceRequest = ::fcl::DistanceRequest<double>;
+using DistanceResult = ::fcl::DistanceResult<double>;
+using Contact = ::fcl::Contact<double>;
+#else
+// Geometric fundamentals
+using Vector3 = ::fcl::Vec3f;
+using Matrix3 = ::fcl::Matrix3f;
+using Transform3 = ::fcl::Transform3f;
+// Geometric primitives
+using Box = ::fcl::Box;
+using Cylinder = ::fcl::Cylinder;
+using Halfspace = ::fcl::Halfspace;
+using Sphere = ::fcl::Sphere;
+// Collision objects
+using CollisionObject = ::fcl::CollisionObject;
+using CollisionGeometry = ::fcl::CollisionGeometry;
+using DynamicAABBTreeCollisionManager = ::fcl::DynamicAABBTreeCollisionManager;
+using OBBRSS = ::fcl::OBBRSS;
+using CollisionRequest = ::fcl::CollisionRequest;
+using CollisionResult = ::fcl::CollisionResult;
+using DistanceRequest = ::fcl::DistanceRequest;
+using DistanceResult = ::fcl::DistanceResult;
+using Contact = ::fcl::Contact;
+#endif
+
+#if FCL_VERSION_AT_LEAST(0,4,0) && !FCL_VERSION_AT_LEAST(0,6,0)
+using Ellipsoid = ::fcl::Ellipsoid;
+#endif
+
+/// Returns norm of a 3-dim vector
+double length(const dart::collision::fcl::Vector3& t);
+
+/// Returns translation component of a transform
+dart::collision::fcl::Vector3 getTranslation(
+    const dart::collision::fcl::Transform3& T);
+
+/// Sets translation component of a transform
+void setTranslation(
+    dart::collision::fcl::Transform3& T,
+    const dart::collision::fcl::Vector3& t);
+
+/// Returns rotation component of a transform
+dart::collision::fcl::Matrix3 getRotation(
+    const dart::collision::fcl::Transform3& T);
+
+/// Sets rotation component of a transform
+void setRotation(
+    dart::collision::fcl::Transform3& T,
+    const dart::collision::fcl::Matrix3& R);
+
+/// Sets a rotation matrix given Euler-XYZ angles
+void setEulerZYX(
+    dart::collision::fcl::Matrix3& rot,
+    double eulerX,
+    double eulerY,
+    double eulerZ);
+
+/// Transforms a 3-dim vector by a transform and returns the result
+dart::collision::fcl::Vector3 transform(
+    const dart::collision::fcl::Transform3& t,
+    const dart::collision::fcl::Vector3& v);
+
+} // namespace fcl
+} // namespace collision
+} // namespace dart
+
+#endif // DART_COLLISION_FCL_BACKWARDCOMPATIBILITY_HPP_