diff -Nru fcl-0.3.0/CMakeLists.txt fcl-0.3.2/CMakeLists.txt --- fcl-0.3.0/CMakeLists.txt 2014-02-16 04:36:54.000000000 +0000 +++ fcl-0.3.2/CMakeLists.txt 2015-11-12 16:53:37.000000000 +0000 @@ -65,22 +65,21 @@ if (FLANN_FOUND) set(FCL_HAVE_FLANN 1) include_directories(${FLANN_INCLUDE_DIRS}) - link_directories(${FLANN_LIBRARY_DIRS}) message(STATUS "FCL uses Flann") else() message(STATUS "FCL does not use Flann") endif() -find_package(tinyxml QUIET) -if (TINYXML_FOUND) - set(FCL_HAVE_TINYXML 1) - include_directories(${TINYXML_INCLUDE_DIRS}) - link_directories(${TINYXML_LIBRARY_DIRS}) - message(STATUS "FCL uses tinyxml") -else() - message(STATUS "FCL does not use tinyxml") -endif() +# find_package(tinyxml QUIET) +# if (TINYXML_FOUND) +# set(FCL_HAVE_TINYXML 1) +# include_directories(${TINYXML_INCLUDE_DIRS}) +# link_directories(${TINYXML_LIBRARY_DIRS}) +# message(STATUS "FCL uses tinyxml") +# else() +# message(STATUS "FCL does not use tinyxml") +# endif() find_package(Boost COMPONENTS thread date_time filesystem system unit_test_framework REQUIRED) diff -Nru fcl-0.3.0/CMakeModules/FCLVersion.cmake fcl-0.3.2/CMakeModules/FCLVersion.cmake --- fcl-0.3.0/CMakeModules/FCLVersion.cmake 2014-02-16 04:36:54.000000000 +0000 +++ fcl-0.3.2/CMakeModules/FCLVersion.cmake 2015-11-12 16:53:37.000000000 +0000 @@ -1,8 +1,8 @@ # set the version in a way CMake can use set(FCL_MAJOR_VERSION 0) set(FCL_MINOR_VERSION 3) -set(FCL_PATCH_VERSION 0) +set(FCL_PATCH_VERSION 2) set(FCL_VERSION "${FCL_MAJOR_VERSION}.${FCL_MINOR_VERSION}.${FCL_PATCH_VERSION}") # increment this when we have ABI changes -set(FCL_ABI_VERSION 4) +set(FCL_ABI_VERSION 5) diff -Nru fcl-0.3.0/debian/changelog fcl-0.3.2/debian/changelog --- fcl-0.3.0/debian/changelog 2015-08-28 12:53:36.000000000 +0000 +++ fcl-0.3.2/debian/changelog 2015-11-13 01:30:39.000000000 +0000 @@ -1,8 +1,17 @@ -fcl (0.3.0-1build1) wily; urgency=medium +fcl (0.3.2-1) unstable; urgency=medium - * No-change rebuild using boost 1.58. + * Remove already merged patch + * Update Standards version + * Properly define several uploaders + * Patch 0.3.0 version to use libccd 2.0 currently in SID + * Missing patch in series files + * Implement autopkgtest for checking the build + * Fix priorities + * Added Testsuite autopkgtest + * Added watch file + * Imported Upstream version 0.3.2 - -- Matthias Klose Fri, 28 Aug 2015 12:53:36 +0000 + -- Jose Luis Rivero Fri, 13 Nov 2015 01:30:09 +0000 fcl (0.3.0-1) unstable; urgency=medium diff -Nru fcl-0.3.0/debian/control fcl-0.3.2/debian/control --- fcl-0.3.0/debian/control 2014-05-08 18:38:20.000000000 +0000 +++ fcl-0.3.2/debian/control 2015-11-13 01:29:46.000000000 +0000 @@ -1,9 +1,10 @@ Source: fcl Maintainer: Debian Science Maintainers Uploaders: Jose Luis Rivero , Leopold Palomo-Avellaneda -Standards-Version: 3.9.5 +Standards-Version: 3.9.6 Section: science -Priority: extra +Priority: optional +Testsuite: autopkgtest Build-Depends: debhelper (>= 9), cmake, pkg-config, @@ -62,6 +63,7 @@ Package: libfcl0-dbg Architecture: any Section: debug +Priority: extra Depends: libfcl0 (= ${binary:Version}), ${misc:Depends} Description: Flexible Collision Library - debug files diff -Nru fcl-0.3.0/debian/patches/0003-fix-simplex-libccd.patch fcl-0.3.2/debian/patches/0003-fix-simplex-libccd.patch --- fcl-0.3.0/debian/patches/0003-fix-simplex-libccd.patch 2014-05-07 18:43:57.000000000 +0000 +++ fcl-0.3.2/debian/patches/0003-fix-simplex-libccd.patch 1970-01-01 00:00:00.000000000 +0000 @@ -1,189 +0,0 @@ -From: Jose Luis Rivero -Date: Mon, 5 May 2014 18:45 +0200 -Subject: Fix problem with libccd version 2 -Bug: https://github.com/flexible-collision-library/fcl/issues/28 - -diff --git a/include/fcl/ccd/simplex.h b/include/fcl/ccd/simplex.h -new file mode 100644 -index 0000000..8ae09c7 ---- /dev/null -+++ b/include/fcl/ccd/simplex.h -@@ -0,0 +1,104 @@ -+/*** -+ * libccd -+ * --------------------------------- -+ * Copyright (c)2010 Daniel Fiser -+ * -+ * -+ * This file is part of libccd. -+ * -+ * Distributed under the OSI-approved BSD License (the "License"); -+ * see accompanying file BDS-LICENSE for details or see -+ * . -+ * -+ * This software is distributed WITHOUT ANY WARRANTY; without even the -+ * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. -+ * See the License for more information. -+ */ -+ -+#ifndef __CCD_SIMPLEX_H__ -+#define __CCD_SIMPLEX_H__ -+ -+#include -+#include "support.h" -+ -+#ifdef __cplusplus -+extern "C" { -+#endif /* __cplusplus */ -+ -+struct _ccd_simplex_t { -+ ccd_support_t ps[4]; -+ int last; //!< index of last added point -+}; -+typedef struct _ccd_simplex_t ccd_simplex_t; -+ -+ -+_ccd_inline void ccdSimplexInit(ccd_simplex_t *s); -+_ccd_inline int ccdSimplexSize(const ccd_simplex_t *s); -+_ccd_inline const ccd_support_t *ccdSimplexLast(const ccd_simplex_t *s); -+_ccd_inline const ccd_support_t *ccdSimplexPoint(const ccd_simplex_t *s, int idx); -+_ccd_inline ccd_support_t *ccdSimplexPointW(ccd_simplex_t *s, int idx); -+ -+_ccd_inline void ccdSimplexAdd(ccd_simplex_t *s, const ccd_support_t *v); -+_ccd_inline void ccdSimplexSet(ccd_simplex_t *s, size_t pos, const ccd_support_t *a); -+_ccd_inline void ccdSimplexSetSize(ccd_simplex_t *s, int size); -+_ccd_inline void ccdSimplexSwap(ccd_simplex_t *s, size_t pos1, size_t pos2); -+ -+ -+/**** INLINES ****/ -+ -+_ccd_inline void ccdSimplexInit(ccd_simplex_t *s) -+{ -+ s->last = -1; -+} -+ -+_ccd_inline int ccdSimplexSize(const ccd_simplex_t *s) -+{ -+ return s->last + 1; -+} -+ -+_ccd_inline const ccd_support_t *ccdSimplexLast(const ccd_simplex_t *s) -+{ -+ return ccdSimplexPoint(s, s->last); -+} -+ -+_ccd_inline const ccd_support_t *ccdSimplexPoint(const ccd_simplex_t *s, int idx) -+{ -+ // here is no check on boundaries -+ return &s->ps[idx]; -+} -+_ccd_inline ccd_support_t *ccdSimplexPointW(ccd_simplex_t *s, int idx) -+{ -+ return &s->ps[idx]; -+} -+ -+_ccd_inline void ccdSimplexAdd(ccd_simplex_t *s, const ccd_support_t *v) -+{ -+ // here is no check on boundaries in sake of speed -+ ++s->last; -+ ccdSupportCopy(s->ps + s->last, v); -+} -+ -+_ccd_inline void ccdSimplexSet(ccd_simplex_t *s, size_t pos, const ccd_support_t *a) -+{ -+ ccdSupportCopy(s->ps + pos, a); -+} -+ -+_ccd_inline void ccdSimplexSetSize(ccd_simplex_t *s, int size) -+{ -+ s->last = size - 1; -+} -+ -+_ccd_inline void ccdSimplexSwap(ccd_simplex_t *s, size_t pos1, size_t pos2) -+{ -+ ccd_support_t supp; -+ -+ ccdSupportCopy(&supp, &s->ps[pos1]); -+ ccdSupportCopy(&s->ps[pos1], &s->ps[pos2]); -+ ccdSupportCopy(&s->ps[pos2], &supp); -+} -+ -+#ifdef __cplusplus -+} /* extern "C" */ -+#endif /* __cplusplus */ -+ -+#endif /* __CCD_SIMPLEX_H__ */ -diff --git a/include/fcl/ccd/support.h b/include/fcl/ccd/support.h -new file mode 100644 -index 0000000..3372f5e ---- /dev/null -+++ b/include/fcl/ccd/support.h -@@ -0,0 +1,55 @@ -+/*** -+ * libccd -+ * --------------------------------- -+ * Copyright (c)2010 Daniel Fiser -+ * -+ * -+ * This file is part of libccd. -+ * -+ * Distributed under the OSI-approved BSD License (the "License"); -+ * see accompanying file BDS-LICENSE for details or see -+ * . -+ * -+ * This software is distributed WITHOUT ANY WARRANTY; without even the -+ * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. -+ * See the License for more information. -+ */ -+ -+#ifndef __CCD_SUPPORT_H__ -+#define __CCD_SUPPORT_H__ -+ -+#include -+ -+#ifdef __cplusplus -+extern "C" { -+#endif /* __cplusplus */ -+ -+struct _ccd_support_t { -+ ccd_vec3_t v; //!< Support point in minkowski sum -+ ccd_vec3_t v1; //!< Support point in obj1 -+ ccd_vec3_t v2; //!< Support point in obj2 -+}; -+typedef struct _ccd_support_t ccd_support_t; -+ -+_ccd_inline void ccdSupportCopy(ccd_support_t *, const ccd_support_t *s); -+ -+/** -+ * Computes support point of obj1 and obj2 in direction dir. -+ * Support point is returned via supp. -+ */ -+void __ccdSupport(const void *obj1, const void *obj2, -+ const ccd_vec3_t *dir, const ccd_t *ccd, -+ ccd_support_t *supp); -+ -+ -+/**** INLINES ****/ -+_ccd_inline void ccdSupportCopy(ccd_support_t *d, const ccd_support_t *s) -+{ -+ *d = *s; -+} -+ -+#ifdef __cplusplus -+} /* extern "C" */ -+#endif /* __cplusplus */ -+ -+#endif /* __CCD_SUPPORT_H__ */ -diff --git a/src/narrowphase/gjk_libccd.cpp b/src/narrowphase/gjk_libccd.cpp -index d46689a..0d76b06 100644 ---- a/src/narrowphase/gjk_libccd.cpp -+++ b/src/narrowphase/gjk_libccd.cpp -@@ -36,7 +36,7 @@ - - - #include "fcl/narrowphase/gjk_libccd.h" --#include -+#include "fcl/ccd/simplex.h" - #include - #include - diff -Nru fcl-0.3.0/debian/patches/series fcl-0.3.2/debian/patches/series --- fcl-0.3.0/debian/patches/series 2014-05-08 18:38:20.000000000 +0000 +++ fcl-0.3.2/debian/patches/series 2015-11-13 01:04:39.000000000 +0000 @@ -1,3 +1,2 @@ 0001-no-rpath-in-libs.patch 0002-define-proper-soname.patch -0003-fix-simplex-libccd.patch diff -Nru fcl-0.3.0/debian/watch fcl-0.3.2/debian/watch --- fcl-0.3.0/debian/watch 1970-01-01 00:00:00.000000000 +0000 +++ fcl-0.3.2/debian/watch 2015-11-12 16:49:50.000000000 +0000 @@ -0,0 +1,3 @@ +version=3 +opts=dversionmangle=s/\+dfsg// \ + https://github.com/flexible-collision-library/fcl/tags .*/v?(\d\S*)\.tar\.gz diff -Nru fcl-0.3.0/include/fcl/ccd/interval.h fcl-0.3.2/include/fcl/ccd/interval.h --- fcl-0.3.0/include/fcl/ccd/interval.h 2014-02-16 04:36:54.000000000 +0000 +++ fcl-0.3.2/include/fcl/ccd/interval.h 2015-11-12 16:53:37.000000000 +0000 @@ -32,6 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ +// This code is based on code developed by Stephane Redon at UNC and Inria for the CATCH library: http://graphics.ewha.ac.kr/CATCH/ /** \author Jia Pan */ diff -Nru fcl-0.3.0/include/fcl/ccd/interval_matrix.h fcl-0.3.2/include/fcl/ccd/interval_matrix.h --- fcl-0.3.0/include/fcl/ccd/interval_matrix.h 2014-02-16 04:36:54.000000000 +0000 +++ fcl-0.3.2/include/fcl/ccd/interval_matrix.h 2015-11-12 16:53:37.000000000 +0000 @@ -31,7 +31,7 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ - +// This code is based on code developed by Stephane Redon at UNC and Inria for the CATCH library: http://graphics.ewha.ac.kr/CATCH/ /** \author Jia Pan */ diff -Nru fcl-0.3.0/include/fcl/ccd/interval_vector.h fcl-0.3.2/include/fcl/ccd/interval_vector.h --- fcl-0.3.0/include/fcl/ccd/interval_vector.h 2014-02-16 04:36:54.000000000 +0000 +++ fcl-0.3.2/include/fcl/ccd/interval_vector.h 2015-11-12 16:53:37.000000000 +0000 @@ -31,7 +31,7 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ - +// This code is based on code developed by Stephane Redon at UNC and Inria for the CATCH library: http://graphics.ewha.ac.kr/CATCH/ /** \author Jia Pan */ diff -Nru fcl-0.3.0/include/fcl/ccd/simplex.h fcl-0.3.2/include/fcl/ccd/simplex.h --- fcl-0.3.0/include/fcl/ccd/simplex.h 1970-01-01 00:00:00.000000000 +0000 +++ fcl-0.3.2/include/fcl/ccd/simplex.h 2015-11-12 16:53:37.000000000 +0000 @@ -0,0 +1,104 @@ +/*** + * libccd + * --------------------------------- + * Copyright (c)2010 Daniel Fiser + * + * + * This file is part of libccd. + * + * Distributed under the OSI-approved BSD License (the "License"); + * see accompanying file BDS-LICENSE for details or see + * . + * + * This software is distributed WITHOUT ANY WARRANTY; without even the + * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the License for more information. + */ + +#ifndef __CCD_SIMPLEX_H__ +#define __CCD_SIMPLEX_H__ + +#include +#include "support.h" + +#ifdef __cplusplus +extern "C" { +#endif /* __cplusplus */ + +struct _ccd_simplex_t { + ccd_support_t ps[4]; + int last; //!< index of last added point +}; +typedef struct _ccd_simplex_t ccd_simplex_t; + + +_ccd_inline void ccdSimplexInit(ccd_simplex_t *s); +_ccd_inline int ccdSimplexSize(const ccd_simplex_t *s); +_ccd_inline const ccd_support_t *ccdSimplexLast(const ccd_simplex_t *s); +_ccd_inline const ccd_support_t *ccdSimplexPoint(const ccd_simplex_t *s, int idx); +_ccd_inline ccd_support_t *ccdSimplexPointW(ccd_simplex_t *s, int idx); + +_ccd_inline void ccdSimplexAdd(ccd_simplex_t *s, const ccd_support_t *v); +_ccd_inline void ccdSimplexSet(ccd_simplex_t *s, size_t pos, const ccd_support_t *a); +_ccd_inline void ccdSimplexSetSize(ccd_simplex_t *s, int size); +_ccd_inline void ccdSimplexSwap(ccd_simplex_t *s, size_t pos1, size_t pos2); + + +/**** INLINES ****/ + +_ccd_inline void ccdSimplexInit(ccd_simplex_t *s) +{ + s->last = -1; +} + +_ccd_inline int ccdSimplexSize(const ccd_simplex_t *s) +{ + return s->last + 1; +} + +_ccd_inline const ccd_support_t *ccdSimplexLast(const ccd_simplex_t *s) +{ + return ccdSimplexPoint(s, s->last); +} + +_ccd_inline const ccd_support_t *ccdSimplexPoint(const ccd_simplex_t *s, int idx) +{ + // here is no check on boundaries + return &s->ps[idx]; +} +_ccd_inline ccd_support_t *ccdSimplexPointW(ccd_simplex_t *s, int idx) +{ + return &s->ps[idx]; +} + +_ccd_inline void ccdSimplexAdd(ccd_simplex_t *s, const ccd_support_t *v) +{ + // here is no check on boundaries in sake of speed + ++s->last; + ccdSupportCopy(s->ps + s->last, v); +} + +_ccd_inline void ccdSimplexSet(ccd_simplex_t *s, size_t pos, const ccd_support_t *a) +{ + ccdSupportCopy(s->ps + pos, a); +} + +_ccd_inline void ccdSimplexSetSize(ccd_simplex_t *s, int size) +{ + s->last = size - 1; +} + +_ccd_inline void ccdSimplexSwap(ccd_simplex_t *s, size_t pos1, size_t pos2) +{ + ccd_support_t supp; + + ccdSupportCopy(&supp, &s->ps[pos1]); + ccdSupportCopy(&s->ps[pos1], &s->ps[pos2]); + ccdSupportCopy(&s->ps[pos2], &supp); +} + +#ifdef __cplusplus +} /* extern "C" */ +#endif /* __cplusplus */ + +#endif /* __CCD_SIMPLEX_H__ */ diff -Nru fcl-0.3.0/include/fcl/ccd/support.h fcl-0.3.2/include/fcl/ccd/support.h --- fcl-0.3.0/include/fcl/ccd/support.h 1970-01-01 00:00:00.000000000 +0000 +++ fcl-0.3.2/include/fcl/ccd/support.h 2015-11-12 16:53:37.000000000 +0000 @@ -0,0 +1,55 @@ +/*** + * libccd + * --------------------------------- + * Copyright (c)2010 Daniel Fiser + * + * + * This file is part of libccd. + * + * Distributed under the OSI-approved BSD License (the "License"); + * see accompanying file BDS-LICENSE for details or see + * . + * + * This software is distributed WITHOUT ANY WARRANTY; without even the + * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the License for more information. + */ + +#ifndef __CCD_SUPPORT_H__ +#define __CCD_SUPPORT_H__ + +#include + +#ifdef __cplusplus +extern "C" { +#endif /* __cplusplus */ + +struct _ccd_support_t { + ccd_vec3_t v; //!< Support point in minkowski sum + ccd_vec3_t v1; //!< Support point in obj1 + ccd_vec3_t v2; //!< Support point in obj2 +}; +typedef struct _ccd_support_t ccd_support_t; + +_ccd_inline void ccdSupportCopy(ccd_support_t *, const ccd_support_t *s); + +/** + * Computes support point of obj1 and obj2 in direction dir. + * Support point is returned via supp. + */ +void __ccdSupport(const void *obj1, const void *obj2, + const ccd_vec3_t *dir, const ccd_t *ccd, + ccd_support_t *supp); + + +/**** INLINES ****/ +_ccd_inline void ccdSupportCopy(ccd_support_t *d, const ccd_support_t *s) +{ + *d = *s; +} + +#ifdef __cplusplus +} /* extern "C" */ +#endif /* __cplusplus */ + +#endif /* __CCD_SUPPORT_H__ */ diff -Nru fcl-0.3.0/include/fcl/ccd/taylor_matrix.h fcl-0.3.2/include/fcl/ccd/taylor_matrix.h --- fcl-0.3.0/include/fcl/ccd/taylor_matrix.h 2014-02-16 04:36:54.000000000 +0000 +++ fcl-0.3.2/include/fcl/ccd/taylor_matrix.h 2015-11-12 16:53:37.000000000 +0000 @@ -31,7 +31,7 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ - +// This code is based on code developed by Stephane Redon at UNC and Inria for the CATCH library: http://graphics.ewha.ac.kr/CATCH/ /** \author Jia Pan */ #ifndef FCL_CCD_TAYLOR_MATRIX_H diff -Nru fcl-0.3.0/include/fcl/ccd/taylor_model.h fcl-0.3.2/include/fcl/ccd/taylor_model.h --- fcl-0.3.0/include/fcl/ccd/taylor_model.h 2014-02-16 04:36:54.000000000 +0000 +++ fcl-0.3.2/include/fcl/ccd/taylor_model.h 2015-11-12 16:53:37.000000000 +0000 @@ -31,7 +31,7 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ - +// This code is based on code developed by Stephane Redon at UNC and Inria for the CATCH library: http://graphics.ewha.ac.kr/CATCH/ /** \author Jia Pan */ #ifndef FCL_CCD_TAYLOR_MODEL_H diff -Nru fcl-0.3.0/include/fcl/ccd/taylor_vector.h fcl-0.3.2/include/fcl/ccd/taylor_vector.h --- fcl-0.3.0/include/fcl/ccd/taylor_vector.h 2014-02-16 04:36:54.000000000 +0000 +++ fcl-0.3.2/include/fcl/ccd/taylor_vector.h 2015-11-12 16:53:37.000000000 +0000 @@ -31,7 +31,7 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ - +// This code is based on code developed by Stephane Redon at UNC and Inria for the CATCH library: http://graphics.ewha.ac.kr/CATCH/ /** \author Jia Pan */ #ifndef FCL_CCD_TAYLOR_VECTOR_H diff -Nru fcl-0.3.0/include/fcl/collision_object.h fcl-0.3.2/include/fcl/collision_object.h --- fcl-0.3.0/include/fcl/collision_object.h 2014-02-16 04:36:54.000000000 +0000 +++ fcl-0.3.2/include/fcl/collision_object.h 2015-11-12 16:53:37.000000000 +0000 @@ -69,7 +69,7 @@ /// @brief get the type of the object virtual OBJECT_TYPE getObjectType() const { return OT_UNKNOWN; } - /// @brief get the node type + /// @brief get the node type virtual NODE_TYPE getNodeType() const { return BV_UNKNOWN; } /// @brief compute the AABB for object in local coordinate @@ -150,29 +150,30 @@ class CollisionObject { public: - CollisionObject(const boost::shared_ptr &cgeom_) : cgeom(cgeom_) + CollisionObject(const boost::shared_ptr &cgeom_) : + cgeom(cgeom_), cgeom_const(cgeom_) { - cgeom->computeLocalAABB(); - computeAABB(); + if (cgeom) + { + cgeom->computeLocalAABB(); + computeAABB(); + } } - CollisionObject(const boost::shared_ptr &cgeom_, const Transform3f& tf) : cgeom(cgeom_), t(tf) + CollisionObject(const boost::shared_ptr &cgeom_, const Transform3f& tf) : + cgeom(cgeom_), cgeom_const(cgeom_), t(tf) { cgeom->computeLocalAABB(); computeAABB(); } CollisionObject(const boost::shared_ptr &cgeom_, const Matrix3f& R, const Vec3f& T): - cgeom(cgeom_), t(Transform3f(R, T)) + cgeom(cgeom_), cgeom_const(cgeom_), t(Transform3f(R, T)) { cgeom->computeLocalAABB(); computeAABB(); } - CollisionObject() - { - } - ~CollisionObject() { } @@ -302,9 +303,9 @@ } /// @brief get geometry from the object instance - boost::shared_ptr collisionGeometry() const + const boost::shared_ptr& collisionGeometry() const { - return cgeom; + return cgeom_const; } /// @brief get object's cost density @@ -340,6 +341,7 @@ protected: boost::shared_ptr cgeom; + boost::shared_ptr cgeom_const; Transform3f t; @@ -355,16 +357,16 @@ class ContinuousCollisionObject { public: - ContinuousCollisionObject(const boost::shared_ptr& cgeom_) : cgeom(cgeom_) + ContinuousCollisionObject(const boost::shared_ptr& cgeom_) : + cgeom(cgeom_), cgeom_const(cgeom_) { } - ContinuousCollisionObject(const boost::shared_ptr& cgeom_, const boost::shared_ptr& motion_) : cgeom(cgeom_), motion(motion_) + ContinuousCollisionObject(const boost::shared_ptr& cgeom_, const boost::shared_ptr& motion_) : + cgeom(cgeom_), cgeom_const(cgeom), motion(motion_) { } - ContinuousCollisionObject() {} - ~ContinuousCollisionObject() {} /// @brief get the type of the object @@ -450,14 +452,15 @@ } /// @brief get geometry from the object instance - inline boost::shared_ptr collisionGeometry() const + inline const boost::shared_ptr& collisionGeometry() const { - return cgeom; + return cgeom_const; } protected: boost::shared_ptr cgeom; + boost::shared_ptr cgeom_const; boost::shared_ptr motion; diff -Nru fcl-0.3.0/include/fcl/math/matrix_3f.h fcl-0.3.2/include/fcl/math/matrix_3f.h --- fcl-0.3.0/include/fcl/math/matrix_3f.h 2014-02-16 04:36:54.000000000 +0000 +++ fcl-0.3.2/include/fcl/math/matrix_3f.h 2015-11-12 16:53:37.000000000 +0000 @@ -171,6 +171,14 @@ data.setIdentity(); } + inline bool isIdentity () const + { + return + data (0,0) == 1 && data (0,1) == 0 && data (0,2) == 0 && + data (0,0) == 0 && data (0,1) == 1 && data (0,2) == 0 && + data (0,0) == 0 && data (0,1) == 0 && data (0,2) == 1; + } + inline void setZero() { data.setZero(); diff -Nru fcl-0.3.0/include/fcl/math/vec_3f.h fcl-0.3.2/include/fcl/math/vec_3f.h --- fcl-0.3.0/include/fcl/math/vec_3f.h 2014-02-16 04:36:54.000000000 +0000 +++ fcl-0.3.2/include/fcl/math/vec_3f.h 2015-11-12 16:53:37.000000000 +0000 @@ -125,9 +125,12 @@ } inline U length() const { return sqrt(details::dot_prod3(data, data)); } + inline U norm() const { return sqrt(details::dot_prod3(data, data)); } inline U sqrLength() const { return details::dot_prod3(data, data); } + inline U squaredNorm() const { return details::dot_prod3(data, data); } inline void setValue(U x, U y, U z) { data.setValue(x, y, z); } inline void setValue(U x) { data.setValue(x); } + inline void setZero () {data.setValue (0); } inline bool equal(const Vec3fX& other, U epsilon = std::numeric_limits::epsilon() * 100) const { return details::equal(data, other.data, epsilon); } inline Vec3fX& negate() { data.negate(); return *this; } @@ -241,6 +244,13 @@ return o; } + template + inline Vec3fX operator * (const typename Vec3fX ::U& t, + const Vec3fX & v) + { + return Vec3fX (v.data * t); + } + } diff -Nru fcl-0.3.0/include/fcl/math/vec_nf.h fcl-0.3.2/include/fcl/math/vec_nf.h --- fcl-0.3.0/include/fcl/math/vec_nf.h 2014-02-16 04:36:54.000000000 +0000 +++ fcl-0.3.2/include/fcl/math/vec_nf.h 2015-11-12 16:53:37.000000000 +0000 @@ -167,7 +167,7 @@ void repack(const Vec_n& input, Vec_n& output) { - int n = std::min(N1, N2); + std::size_t n = std::min(N1, N2); for(std::size_t i = 0; i < n; ++i) output[i] = input[i]; } diff -Nru fcl-0.3.0/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp fcl-0.3.2/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp --- fcl-0.3.0/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp 2014-02-16 04:36:54.000000000 +0000 +++ fcl-0.3.2/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp 2015-11-12 16:53:37.000000000 +0000 @@ -764,7 +764,7 @@ void DynamicAABBTreeCollisionManager_Array::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const { if(size() == 0) return; - switch(obj->getCollisionGeometry()->getNodeType()) + switch(obj->collisionGeometry()->getNodeType()) { #if FCL_HAVE_OCTOMAP case GEOM_OCTREE: @@ -788,7 +788,7 @@ { if(size() == 0) return; FCL_REAL min_dist = std::numeric_limits::max(); - switch(obj->getCollisionGeometry()->getNodeType()) + switch(obj->collisionGeometry()->getNodeType()) { #if FCL_HAVE_OCTOMAP case GEOM_OCTREE: diff -Nru fcl-0.3.0/src/broadphase/broadphase_dynamic_AABB_tree.cpp fcl-0.3.2/src/broadphase/broadphase_dynamic_AABB_tree.cpp --- fcl-0.3.0/src/broadphase/broadphase_dynamic_AABB_tree.cpp 2014-02-16 04:36:54.000000000 +0000 +++ fcl-0.3.2/src/broadphase/broadphase_dynamic_AABB_tree.cpp 2015-11-12 16:53:37.000000000 +0000 @@ -739,7 +739,7 @@ void DynamicAABBTreeCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const { if(size() == 0) return; - switch(obj->getCollisionGeometry()->getNodeType()) + switch(obj->collisionGeometry()->getNodeType()) { #if FCL_HAVE_OCTOMAP case GEOM_OCTREE: @@ -763,7 +763,7 @@ { if(size() == 0) return; FCL_REAL min_dist = std::numeric_limits::max(); - switch(obj->getCollisionGeometry()->getNodeType()) + switch(obj->collisionGeometry()->getNodeType()) { #if FCL_HAVE_OCTOMAP case GEOM_OCTREE: diff -Nru fcl-0.3.0/src/broadphase/broadphase_SSaP.cpp fcl-0.3.2/src/broadphase/broadphase_SSaP.cpp --- fcl-0.3.0/src/broadphase/broadphase_SSaP.cpp 2014-02-16 04:36:54.000000000 +0000 +++ fcl-0.3.2/src/broadphase/broadphase_SSaP.cpp 2015-11-12 16:53:37.000000000 +0000 @@ -78,7 +78,7 @@ class DummyCollisionObject : public CollisionObject { public: - DummyCollisionObject(const AABB& aabb_) : CollisionObject() + DummyCollisionObject(const AABB& aabb_) : CollisionObject(boost::shared_ptr()) { aabb = aabb_; } @@ -432,7 +432,7 @@ if(size() == 0) return; std::vector::const_iterator it, it_end; - // size_t axis = selectOptimalAxis(objs_x, objs_y, objs_z, it, it_end); + selectOptimalAxis(objs_x, objs_y, objs_z, it, it_end); FCL_REAL min_dist = std::numeric_limits::max(); for(; it != it_end; ++it) diff -Nru fcl-0.3.0/src/collision.cpp fcl-0.3.2/src/collision.cpp --- fcl-0.3.0/src/collision.cpp 2014-02-16 04:36:54.000000000 +0000 +++ fcl-0.3.2/src/collision.cpp 2015-11-12 16:53:37.000000000 +0000 @@ -57,7 +57,7 @@ const CollisionRequest& request, CollisionResult& result) { - return collide(o1->getCollisionGeometry(), o1->getTransform(), o2->getCollisionGeometry(), o2->getTransform(), + return collide(o1->collisionGeometry().get(), o1->getTransform(), o2->collisionGeometry().get(), o2->getTransform(), nsolver, request, result); } diff -Nru fcl-0.3.0/src/continuous_collision.cpp fcl-0.3.2/src/continuous_collision.cpp --- fcl-0.3.0/src/continuous_collision.cpp 2014-02-16 04:36:54.000000000 +0000 +++ fcl-0.3.2/src/continuous_collision.cpp 2015-11-12 16:53:37.000000000 +0000 @@ -316,8 +316,8 @@ const ContinuousCollisionRequest& request, ContinuousCollisionResult& result) { - return continuousCollide(o1->getCollisionGeometry(), o1->getTransform(), tf1_end, - o2->getCollisionGeometry(), o2->getTransform(), tf2_end, + return continuousCollide(o1->collisionGeometry().get(), o1->getTransform(), tf1_end, + o2->collisionGeometry().get(), o2->getTransform(), tf2_end, request, result); } @@ -326,8 +326,8 @@ const ContinuousCollisionRequest& request, ContinuousCollisionResult& result) { - return continuousCollide(o1->getCollisionGeometry(), o1->getMotion(), - o2->getCollisionGeometry(), o2->getMotion(), + return continuousCollide(o1->collisionGeometry().get(), o1->getMotion(), + o2->collisionGeometry().get(), o2->getMotion(), request, result); } diff -Nru fcl-0.3.0/src/distance.cpp fcl-0.3.2/src/distance.cpp --- fcl-0.3.0/src/distance.cpp 2014-02-16 04:36:54.000000000 +0000 +++ fcl-0.3.2/src/distance.cpp 2015-11-12 16:53:37.000000000 +0000 @@ -54,7 +54,7 @@ FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const NarrowPhaseSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { - return distance(o1->getCollisionGeometry(), o1->getTransform(), o2->getCollisionGeometry(), o2->getTransform(), nsolver, + return distance(o1->collisionGeometry().get(), o1->getTransform(), o2->collisionGeometry().get(), o2->getTransform(), nsolver, request, result); } diff -Nru fcl-0.3.0/src/math/transform.cpp fcl-0.3.2/src/math/transform.cpp --- fcl-0.3.0/src/math/transform.cpp 2014-02-16 04:36:54.000000000 +0000 +++ fcl-0.3.2/src/math/transform.cpp 2015-11-12 16:53:37.000000000 +0000 @@ -85,6 +85,10 @@ void Quaternion3f::toRotation(Matrix3f& R) const { + assert (.99 < data [0]*data [0] + data [1]*data [1] + + data [2]*data [2] + data [3]*data [3]); + assert (data [0]*data [0] + data [1]*data [1] + + data [2]*data [2] + data [3]*data [3] < 1.01); FCL_REAL twoX = 2.0*data[1]; FCL_REAL twoY = 2.0*data[2]; FCL_REAL twoZ = 2.0*data[3]; @@ -312,8 +316,10 @@ Vec3f Quaternion3f::transform(const Vec3f& v) const { - Quaternion3f r = (*this) * Quaternion3f(0, v[0], v[1], v[2]) * (fcl::conj(*this)); - return Vec3f(r.data[1], r.data[2], r.data[3]); + Vec3f u(getX(), getY(), getZ()); + double s = getW(); + Vec3f vprime = 2*u.dot(v)*u + (s*s - u.dot(u))*v + 2*s*u.cross(v); + return vprime; } Quaternion3f conj(const Quaternion3f& q) diff -Nru fcl-0.3.0/src/narrowphase/gjk_libccd.cpp fcl-0.3.2/src/narrowphase/gjk_libccd.cpp --- fcl-0.3.0/src/narrowphase/gjk_libccd.cpp 2014-02-16 04:36:54.000000000 +0000 +++ fcl-0.3.2/src/narrowphase/gjk_libccd.cpp 2015-11-12 16:53:37.000000000 +0000 @@ -36,7 +36,7 @@ #include "fcl/narrowphase/gjk_libccd.h" -#include +#include "fcl/ccd/simplex.h" #include #include @@ -579,11 +579,12 @@ ccdVec3Set(&pos2, CCD_ZERO, CCD_ZERO, -o->height); ccdVec3Copy(v, &dir); + ccdVec3Normalize (v); ccdVec3Scale(v, o->radius); ccdVec3Add(&pos1, v); ccdVec3Add(&pos2, v); - if(ccdVec3Dot(&dir, &pos1) > ccdVec3Dot(&dir, &pos2)) + if(ccdVec3Z (&dir) > 0) ccdVec3Copy(v, &pos1); else ccdVec3Copy(v, &pos2); diff -Nru fcl-0.3.0/src/narrowphase/narrowphase.cpp fcl-0.3.2/src/narrowphase/narrowphase.cpp --- fcl-0.3.0/src/narrowphase/narrowphase.cpp 2014-02-16 04:36:54.000000000 +0000 +++ fcl-0.3.2/src/narrowphase/narrowphase.cpp 2015-11-12 16:53:37.000000000 +0000 @@ -388,7 +388,7 @@ Vec3f contact_point; if(is_inside_contact_plane) { - if(projectInTriangle(P1, P2, P3, center, normal)) + if(projectInTriangle(P1, P2, P3, normal, center)) { has_contact = true; contact_point = center - normal * distance_from_plane; @@ -703,7 +703,7 @@ result = Project::projectTriangle(P1, P2, P3, o); if(result.sqr_distance > sp.radius * sp.radius) { - if(dist) *dist = std::sqrt(result.sqr_distance); + if(dist) *dist = std::sqrt(result.sqr_distance) - sp.radius; Vec3f project_p = P1 * result.parameterization[0] + P2 * result.parameterization[1] + P3 * result.parameterization[2]; Vec3f dir = o - project_p; dir.normalize(); @@ -725,7 +725,7 @@ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, FCL_REAL* dist, Vec3f* p1, Vec3f* p2) { - bool res = details::sphereTriangleDistance(sp, tf1, tf2.transform(P1), tf2.transform(P2), tf2.transform(P2), dist, p1, p2); + bool res = details::sphereTriangleDistance(sp, tf1, tf2.transform(P1), tf2.transform(P2), tf2.transform(P3), dist, p1, p2); if(p2) *p2 = inverse(tf2).transform(*p2); return res; diff -Nru fcl-0.3.0/test/CMakeLists.txt fcl-0.3.2/test/CMakeLists.txt --- fcl-0.3.0/test/CMakeLists.txt 2014-02-16 04:36:54.000000000 +0000 +++ fcl-0.3.2/test/CMakeLists.txt 2015-11-12 16:53:37.000000000 +0000 @@ -30,6 +30,8 @@ add_fcl_test(test_fcl_sphere_capsule test_fcl_sphere_capsule.cpp) add_fcl_test(test_fcl_capsule_capsule test_fcl_capsule_capsule.cpp) add_fcl_test(test_fcl_simple test_fcl_simple.cpp) +add_fcl_test(test_fcl_capsule_box_1 test_fcl_capsule_box_1.cpp) +add_fcl_test(test_fcl_capsule_box_2 test_fcl_capsule_box_2.cpp) #add_fcl_test(test_fcl_global_penetration test_fcl_global_penetration.cpp libsvm/svm.cpp test_fcl_utility.cpp) if (FCL_HAVE_OCTOMAP) diff -Nru fcl-0.3.0/test/fcl_resources/config.h fcl-0.3.2/test/fcl_resources/config.h --- fcl-0.3.0/test/fcl_resources/config.h 2014-02-16 04:36:54.000000000 +0000 +++ fcl-0.3.2/test/fcl_resources/config.h 1970-01-01 00:00:00.000000000 +0000 @@ -1,41 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2010, Rice University. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Rice University nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -/* Author: Mark Moll */ - -#ifndef FCL_TEST_RESOURCES_CONFIG_ -#define FCL_TEST_RESOURCES_CONFIG_ - -#define TEST_RESOURCES_DIR "/home/jia/Workspace/fcl/test/fcl_resources" -#endif diff -Nru fcl-0.3.0/test/test_fcl_capsule_box_1.cpp fcl-0.3.2/test/test_fcl_capsule_box_1.cpp --- fcl-0.3.0/test/test_fcl_capsule_box_1.cpp 1970-01-01 00:00:00.000000000 +0000 +++ fcl-0.3.2/test/test_fcl_capsule_box_1.cpp 2015-11-12 16:53:37.000000000 +0000 @@ -0,0 +1,115 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2014, CNRS-LAAS + * 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 CNRS-LAAS 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 Florent Lamiraux */ + + +#define BOOST_TEST_MODULE "FCL_GEOMETRIC_SHAPES" +#define CHECK_CLOSE_TO_0(x, eps) BOOST_CHECK_CLOSE ((x + 1.0), (1.0), (eps)) +#include + +#include +#include +#include +#include +#include +#include + +BOOST_AUTO_TEST_CASE(distance_capsule_box) +{ + typedef boost::shared_ptr CollisionGeometryPtr_t; + // Capsule of radius 2 and of height 4 + CollisionGeometryPtr_t capsuleGeometry (new fcl::Capsule (2., 4.)); + // Box of size 1 by 2 by 4 + CollisionGeometryPtr_t boxGeometry (new fcl::Box (1., 2., 4.)); + + // Enable computation of nearest points + fcl::DistanceRequest distanceRequest (true); + fcl::DistanceResult distanceResult; + + fcl::Transform3f tf1 (fcl::Vec3f (3., 0, 0)); + fcl::Transform3f tf2; + fcl::CollisionObject capsule (capsuleGeometry, tf1); + fcl::CollisionObject box (boxGeometry, tf2); + + // test distance + fcl::distance (&capsule, &box, distanceRequest, distanceResult); + // Nearest point on capsule + fcl::Vec3f o1 (distanceResult.nearest_points [0]); + // Nearest point on box + fcl::Vec3f o2 (distanceResult.nearest_points [1]); + BOOST_CHECK_CLOSE (distanceResult.min_distance, 0.5, 1e-4); + BOOST_CHECK_CLOSE (o1 [0], -2.0, 1e-4); + CHECK_CLOSE_TO_0 (o1 [1], 1e-4); + BOOST_CHECK_CLOSE (o2 [0], 0.5, 1e-4); + BOOST_CHECK_CLOSE (o1 [1], 0.0, 1e-4); + + // Move capsule above box + tf1 = fcl::Transform3f (fcl::Vec3f (0., 0., 8.)); + capsule.setTransform (tf1); + + // test distance + distanceResult.clear (); + fcl::distance (&capsule, &box, distanceRequest, distanceResult); + o1 = distanceResult.nearest_points [0]; + o2 = distanceResult.nearest_points [1]; + + BOOST_CHECK_CLOSE (distanceResult.min_distance, 2.0, 1e-4); + CHECK_CLOSE_TO_0 (o1 [0], 1e-4); + CHECK_CLOSE_TO_0 (o1 [1], 1e-4); + BOOST_CHECK_CLOSE (o1 [2], -4.0, 1e-4); + + CHECK_CLOSE_TO_0 (o2 [0], 1e-4); + CHECK_CLOSE_TO_0 (o2 [1], 1e-4); + BOOST_CHECK_CLOSE (o2 [2], 2.0, 1e-4); + + // Rotate capsule around y axis by pi/2 and move it behind box + tf1.setTranslation (fcl::Vec3f (-10., 0., 0.)); + tf1.setQuatRotation (fcl::Quaternion3f (sqrt(2)/2, 0, sqrt(2)/2, 0)); + capsule.setTransform (tf1); + + // test distance + distanceResult.clear (); + fcl::distance (&capsule, &box, distanceRequest, distanceResult); + o1 = distanceResult.nearest_points [0]; + o2 = distanceResult.nearest_points [1]; + + BOOST_CHECK_CLOSE (distanceResult.min_distance, 5.5, 1e-4); + CHECK_CLOSE_TO_0 (o1 [0], 1e-4); + CHECK_CLOSE_TO_0 (o1 [1], 1e-4); + BOOST_CHECK_CLOSE (o1 [2], 4.0, 1e-4); + BOOST_CHECK_CLOSE (o2 [0], -0.5, 1e-4); + CHECK_CLOSE_TO_0 (o2 [1], 1e-4); + CHECK_CLOSE_TO_0 (o2 [2], 1e-4); +} diff -Nru fcl-0.3.0/test/test_fcl_capsule_box_2.cpp fcl-0.3.2/test/test_fcl_capsule_box_2.cpp --- fcl-0.3.0/test/test_fcl_capsule_box_2.cpp 1970-01-01 00:00:00.000000000 +0000 +++ fcl-0.3.2/test/test_fcl_capsule_box_2.cpp 2015-11-12 16:53:37.000000000 +0000 @@ -0,0 +1,81 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2014, CNRS-LAAS + * 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 CNRS-LAAS 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 Florent Lamiraux */ + + +#define BOOST_TEST_MODULE "FCL_GEOMETRIC_SHAPES" +#define CHECK_CLOSE_TO_0(x, eps) BOOST_CHECK_CLOSE ((x + 1.0), (1.0), (eps)) +#include + +#include +#include +#include +#include +#include +#include + +BOOST_AUTO_TEST_CASE(distance_capsule_box) +{ + typedef boost::shared_ptr CollisionGeometryPtr_t; + // Capsule of radius 2 and of height 4 + CollisionGeometryPtr_t capsuleGeometry (new fcl::Capsule (2., 4.)); + // Box of size 1 by 2 by 4 + CollisionGeometryPtr_t boxGeometry (new fcl::Box (1., 2., 4.)); + + // Enable computation of nearest points + fcl::DistanceRequest distanceRequest (true); + fcl::DistanceResult distanceResult; + + // Rotate capsule around y axis by pi/2 and move it behind box + fcl::Transform3f tf1 (fcl::Quaternion3f (sqrt(2)/2, 0, sqrt(2)/2, 0), + fcl::Vec3f (-10., 0.8, 1.5)); + fcl::Transform3f tf2; + fcl::CollisionObject capsule (capsuleGeometry, tf1); + fcl::CollisionObject box (boxGeometry, tf2); + + // test distance + distanceResult.clear (); + fcl::distance (&capsule, &box, distanceRequest, distanceResult); + fcl::Vec3f o1 = distanceResult.nearest_points [0]; + fcl::Vec3f o2 = distanceResult.nearest_points [1]; + + BOOST_CHECK_CLOSE (distanceResult.min_distance, 5.5, 1e-4); + CHECK_CLOSE_TO_0 (o1 [0], 1e-4); + CHECK_CLOSE_TO_0 (o1 [1], 1e-4); + BOOST_CHECK_CLOSE (o1 [2], 4.0, 1e-4); + BOOST_CHECK_CLOSE (o2 [0], -0.5, 1e-4); + BOOST_CHECK_CLOSE (o2 [1], 0.8, 1e-4); + BOOST_CHECK_CLOSE (o2 [2], 1.5, 1e-4); +} diff -Nru fcl-0.3.0/test/test_fcl_geometric_shapes.cpp fcl-0.3.2/test/test_fcl_geometric_shapes.cpp --- fcl-0.3.0/test/test_fcl_geometric_shapes.cpp 2014-02-16 04:36:54.000000000 +0000 +++ fcl-0.3.2/test/test_fcl_geometric_shapes.cpp 2015-11-12 16:53:37.000000000 +0000 @@ -1938,25 +1938,20 @@ BOOST_CHECK(dist < 0); BOOST_CHECK_FALSE(res); - std::cerr << " SOVLER NUMBER 1" << std::endl; res = solver1.shapeDistance(s2, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), &dist, &closest_p1, &closest_p2); - std::cerr << "computed points in box to box" << closest_p1 << " & " << closest_p2 << "with dist: " << dist<< std::endl; BOOST_CHECK(fabs(dist - 0.1) < 0.001); BOOST_CHECK(res); res = solver1.shapeDistance(s2, Transform3f(), s2, Transform3f(Vec3f(20.1, 0, 0)), &dist, &closest_p1, &closest_p2); - std::cerr << "computed points in box to box" << closest_p1 << " & " << closest_p2 << "with dist: " << dist<< std::endl; - BOOST_CHECK(fabs(dist - 0.1) < 0.001); + BOOST_CHECK(fabs(dist - 10.1) < 0.001); BOOST_CHECK(res); res = solver1.shapeDistance(s2, Transform3f(), s2, Transform3f(Vec3f(0, 20.2, 0)), &dist, &closest_p1, &closest_p2); - std::cerr << "computed points in box to box" << closest_p1 << " & " << closest_p2 << "with dist: " << dist<< std::endl; - BOOST_CHECK(fabs(dist - 0.1) < 0.001); + BOOST_CHECK(fabs(dist - 10.2) < 0.001); BOOST_CHECK(res); res = solver1.shapeDistance(s2, Transform3f(), s2, Transform3f(Vec3f(10.1, 10.1, 0)), &dist, &closest_p1, &closest_p2); - std::cerr << "computed points in box to box" << closest_p1 << " & " << closest_p2 << "with dist: " << dist<< std::endl; - BOOST_CHECK(fabs(dist - 0.1) < 0.001); + BOOST_CHECK(fabs(dist - 0.1 * 1.414) < 0.001); BOOST_CHECK(res); res = solver2.shapeDistance(s2, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), &dist, &closest_p1, &closest_p2); @@ -1964,15 +1959,15 @@ BOOST_CHECK(res); res = solver2.shapeDistance(s2, Transform3f(), s2, Transform3f(Vec3f(20.1, 0, 0)), &dist, &closest_p1, &closest_p2); - BOOST_CHECK(fabs(dist - 0.1) < 0.001); + BOOST_CHECK(fabs(dist - 10.1) < 0.001); BOOST_CHECK(res); res = solver2.shapeDistance(s2, Transform3f(), s2, Transform3f(Vec3f(0, 20.1, 0)), &dist, &closest_p1, &closest_p2); - BOOST_CHECK(fabs(dist - 0.1) < 0.001); + BOOST_CHECK(fabs(dist - 10.1) < 0.001); BOOST_CHECK(res); res = solver2.shapeDistance(s2, Transform3f(), s2, Transform3f(Vec3f(10.1, 10.1, 0)), &dist, &closest_p1, &closest_p2); - BOOST_CHECK(fabs(dist - 0.1) < 0.001); + BOOST_CHECK(fabs(dist - 0.1 * 1.414) < 0.001); BOOST_CHECK(res); diff -Nru fcl-0.3.0/test/test_fcl_shape_mesh_consistency.cpp fcl-0.3.2/test/test_fcl_shape_mesh_consistency.cpp --- fcl-0.3.0/test/test_fcl_shape_mesh_consistency.cpp 2014-02-16 04:36:54.000000000 +0000 +++ fcl-0.3.2/test/test_fcl_shape_mesh_consistency.cpp 2015-11-12 16:53:37.000000000 +0000 @@ -890,19 +890,19 @@ BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); - BOOST_CHECK(res); + BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); - BOOST_CHECK(res); + BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); - BOOST_CHECK(res); + BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); @@ -987,19 +987,19 @@ BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); - BOOST_CHECK(res); + BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); - BOOST_CHECK(res); + BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); - BOOST_CHECK(res); + BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); @@ -1590,19 +1590,19 @@ BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); - BOOST_CHECK(res); + BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); - BOOST_CHECK(res); + BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); - BOOST_CHECK(res); + BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); @@ -1687,19 +1687,19 @@ BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); - BOOST_CHECK(res); + BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); - BOOST_CHECK(res); + BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); - BOOST_CHECK(res); + BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); diff -Nru fcl-0.3.0/test/test_fcl_sphere_capsule.cpp fcl-0.3.2/test/test_fcl_sphere_capsule.cpp --- fcl-0.3.0/test/test_fcl_sphere_capsule.cpp 2014-02-16 04:36:54.000000000 +0000 +++ fcl-0.3.2/test/test_fcl_sphere_capsule.cpp 2015-11-12 16:53:37.000000000 +0000 @@ -189,27 +189,3 @@ BOOST_CHECK (is_separated); BOOST_CHECK (distance == 25.); } - -BOOST_AUTO_TEST_CASE(Capsule_Capsule_Distance_test_separated) -{ - GJKSolver_libccd solver; - - Capsule sphere1 (50, 0); - Transform3f sphere1_transform(Vec3f (0., 0., 0)); - - Capsule capsule (50, 00.); - Transform3f capsule_transform (Vec3f (150., 0., 0)); - - FCL_REAL distance = 0.; - Vec3f p1; - Vec3f p2; - bool is_separated = solver.shapeDistance(sphere1, sphere1_transform, capsule, capsule_transform, &distance, &p1, &p2); - - std::cerr << "computed distance: " << distance << std::endl; - std::cerr << "computed p1: " << p1 << std::endl; - std::cerr << "computed p2: " << p2 << std::endl; - - - BOOST_CHECK (is_separated); - BOOST_CHECK (distance == 25.); -}