diff --git a/CHANGELOG.md b/CHANGELOG.md index 80094cd090ee4..5b24084c9b037 100644 --- a/CHANGELOG.md +++ b/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) diff --git a/CMakeLists.txt b/CMakeLists.txt index 1f22e99583223..89c692c883550 100644 --- a/CMakeLists.txt +++ b/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") diff --git a/cmake/FindCCD.cmake b/cmake/FindCCD.cmake index 0938dd7a5fd6e..dbb217fdbac65 100644 --- a/cmake/FindCCD.cmake +++ b/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}) diff --git a/cmake/FindFCL.cmake b/cmake/FindFCL.cmake index 1c4b0ae86dc71..3fffc51e7c78d 100644 --- a/cmake/FindFCL.cmake +++ b/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}) diff --git a/dart/collision/fcl/BackwardCompatibility.cpp b/dart/collision/fcl/BackwardCompatibility.cpp new file mode 100644 index 0000000000000..5616fc537b5a7 --- /dev/null +++ b/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 diff --git a/dart/collision/fcl/BackwardCompatibility.hpp b/dart/collision/fcl/BackwardCompatibility.hpp new file mode 100644 index 0000000000000..c3b19ff8d32b6 --- /dev/null +++ b/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 + +#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 + +#if FCL_VERSION_AT_LEAST(0,6,0) +#include + +#include +#include +#include +#include +#include +#include +#include +#else +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#endif +#include + +#if FCL_VERSION_AT_LEAST(0,5,0) +#include +template using fcl_shared_ptr = std::shared_ptr; +template using fcl_weak_ptr = std::weak_ptr; +#else +#include +template using fcl_shared_ptr = boost::shared_ptr; +template using fcl_weak_ptr = boost::weak_ptr; +#endif + +namespace dart { +namespace collision { +namespace fcl { + +#if FCL_VERSION_AT_LEAST(0,6,0) +// Geometric fundamentals +using Vector3 = ::fcl::Vector3; +using Matrix3 = ::fcl::Matrix3; +using Transform3 = ::fcl::Transform3; +// Geometric primitives +using Box = ::fcl::Box; +using Cylinder = ::fcl::Cylinder; +using Ellipsoid = ::fcl::Ellipsoid; +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; +#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_ diff --git a/dart/collision/fcl/CollisionShapes.hpp b/dart/collision/fcl/CollisionShapes.hpp index 8afd4e9cbad48..6f236fc703cc2 100644 --- a/dart/collision/fcl/CollisionShapes.hpp +++ b/dart/collision/fcl/CollisionShapes.hpp @@ -33,32 +33,33 @@ #ifndef DART_COLLISION_FCL_MESH_COLLISIONSHAPES_HPP_ #define DART_COLLISION_FCL_MESH_COLLISIONSHAPES_HPP_ +#include "dart/collision/fcl/FCLTypes.hpp" + #include #include -#include namespace dart { namespace collision { template -fcl::BVHModel* createMesh(float _scaleX, float _scaleY, float _scaleZ, - const aiScene* _mesh, - const fcl::Transform3f& _transform) { +::fcl::BVHModel* createMesh(float _scaleX, float _scaleY, float _scaleZ, + const aiScene* _mesh, + const dart::collision::fcl::Transform3& _transform) { assert(_mesh); - fcl::BVHModel* model = new fcl::BVHModel; + ::fcl::BVHModel* model = new ::fcl::BVHModel; model->beginModel(); for (unsigned int i = 0; i < _mesh->mNumMeshes; i++) { for (unsigned int j = 0; j < _mesh->mMeshes[i]->mNumFaces; j++) { - fcl::Vec3f vertices[3]; + dart::collision::fcl::Vector3 vertices[3]; for (unsigned int k = 0; k < 3; k++) { const aiVector3D& vertex = _mesh->mMeshes[i]->mVertices[ _mesh->mMeshes[i]->mFaces[j].mIndices[k]]; - vertices[k] = fcl::Vec3f(vertex.x * _scaleX, - vertex.y * _scaleY, - vertex.z * _scaleZ); - vertices[k] = _transform.transform(vertices[k]); + vertices[k] = dart::collision::fcl::Vector3(vertex.x * _scaleX, + vertex.y * _scaleY, + vertex.z * _scaleZ); + vertices[k] = dart::collision::fcl::transform(_transform, vertices[k]); } model->addTriangle(vertices[0], vertices[1], vertices[2]); } @@ -69,8 +70,8 @@ fcl::BVHModel* createMesh(float _scaleX, float _scaleY, float _scaleZ, } template -fcl::BVHModel* createEllipsoid(float _sizeX, float _sizeY, float _sizeZ, - const fcl::Transform3f& _transform) { +::fcl::BVHModel* createEllipsoid(float _sizeX, float _sizeY, float _sizeZ, + const dart::collision::fcl::Transform3& _transform) { float v[59][3] = { {0, 0, 0}, {0.135299, -0.461940, -0.135299}, @@ -247,23 +248,23 @@ fcl::BVHModel* createEllipsoid(float _sizeX, float _sizeY, float _sizeZ, {56, 49, 58} }; - fcl::BVHModel* model = new fcl::BVHModel; - fcl::Vec3f p1, p2, p3; + ::fcl::BVHModel* model = new ::fcl::BVHModel; + dart::collision::fcl::Vector3 p1, p2, p3; model->beginModel(); for (int i = 0; i < 112; i++) { - p1 = fcl::Vec3f(v[f[i][0]][0] * _sizeX, - v[f[i][0]][1] * _sizeY, - v[f[i][0]][2] * _sizeZ); - p2 = fcl::Vec3f(v[f[i][1]][0] * _sizeX, - v[f[i][1]][1] * _sizeY, - v[f[i][1]][2] * _sizeZ); - p3 = fcl::Vec3f(v[f[i][2]][0] * _sizeX, - v[f[i][2]][1] * _sizeY, - v[f[i][2]][2] * _sizeZ); - p1 = _transform.transform(p1); - p2 = _transform.transform(p2); - p3 = _transform.transform(p3); + p1 = dart::collision::fcl::Vector3(v[f[i][0]][0] * _sizeX, + v[f[i][0]][1] * _sizeY, + v[f[i][0]][2] * _sizeZ); + p2 = dart::collision::fcl::Vector3(v[f[i][1]][0] * _sizeX, + v[f[i][1]][1] * _sizeY, + v[f[i][1]][2] * _sizeZ); + p3 = dart::collision::fcl::Vector3(v[f[i][2]][0] * _sizeX, + v[f[i][2]][1] * _sizeY, + v[f[i][2]][2] * _sizeZ); + p1 = dart::collision::fcl::transform(_transform, p1); + p2 = dart::collision::fcl::transform(_transform, p2); + p3 = dart::collision::fcl::transform(_transform, p3); model->addTriangle(p1, p2, p3); } model->endModel(); @@ -272,8 +273,8 @@ fcl::BVHModel* createEllipsoid(float _sizeX, float _sizeY, float _sizeZ, // Create a cube mesh for collision detection template -fcl::BVHModel* createCube(float _sizeX, float _sizeY, float _sizeZ, - const fcl::Transform3f& _transform) { +::fcl::BVHModel* createCube(float _sizeX, float _sizeY, float _sizeZ, + const dart::collision::fcl::Transform3& _transform) { // float n[6][3] = { // {-1.0, 0.0, 0.0}, // {0.0, 1.0, 0.0}, @@ -299,24 +300,24 @@ fcl::BVHModel* createCube(float _sizeX, float _sizeY, float _sizeZ, v[0][2] = v[3][2] = v[4][2] = v[7][2] = -_sizeZ / 2; v[1][2] = v[2][2] = v[5][2] = v[6][2] = _sizeZ / 2; - fcl::BVHModel* model = new fcl::BVHModel; - fcl::Vec3f p1, p2, p3; + ::fcl::BVHModel* model = new ::fcl::BVHModel; + dart::collision::fcl::Vector3 p1, p2, p3; model->beginModel(); for (int i = 0; i < 6; i++) { - p1 = fcl::Vec3f(v[faces[i][0]][0], v[faces[i][0]][1], v[faces[i][0]][2]); - p2 = fcl::Vec3f(v[faces[i][1]][0], v[faces[i][1]][1], v[faces[i][1]][2]); - p3 = fcl::Vec3f(v[faces[i][2]][0], v[faces[i][2]][1], v[faces[i][2]][2]); - p1 = _transform.transform(p1); - p2 = _transform.transform(p2); - p3 = _transform.transform(p3); + p1 = dart::collision::fcl::Vector3(v[faces[i][0]][0], v[faces[i][0]][1], v[faces[i][0]][2]); + p2 = dart::collision::fcl::Vector3(v[faces[i][1]][0], v[faces[i][1]][1], v[faces[i][1]][2]); + p3 = dart::collision::fcl::Vector3(v[faces[i][2]][0], v[faces[i][2]][1], v[faces[i][2]][2]); + p1 = dart::collision::fcl::transform(_transform, p1); + p2 = dart::collision::fcl::transform(_transform, p2); + p3 = dart::collision::fcl::transform(_transform, p3); model->addTriangle(p1, p2, p3); - p1 = fcl::Vec3f(v[faces[i][0]][0], v[faces[i][0]][1], v[faces[i][0]][2]); - p2 = fcl::Vec3f(v[faces[i][2]][0], v[faces[i][2]][1], v[faces[i][2]][2]); - p3 = fcl::Vec3f(v[faces[i][3]][0], v[faces[i][3]][1], v[faces[i][3]][2]); - p1 = _transform.transform(p1); - p2 = _transform.transform(p2); - p3 = _transform.transform(p3); + p1 = dart::collision::fcl::Vector3(v[faces[i][0]][0], v[faces[i][0]][1], v[faces[i][0]][2]); + p2 = dart::collision::fcl::Vector3(v[faces[i][2]][0], v[faces[i][2]][1], v[faces[i][2]][2]); + p3 = dart::collision::fcl::Vector3(v[faces[i][3]][0], v[faces[i][3]][1], v[faces[i][3]][2]); + p1 = dart::collision::fcl::transform(_transform, p1); + p2 = dart::collision::fcl::transform(_transform, p2); + p3 = dart::collision::fcl::transform(_transform, p3); model->addTriangle(p1, p2, p3); } model->endModel(); @@ -324,9 +325,9 @@ fcl::BVHModel* createCube(float _sizeX, float _sizeY, float _sizeZ, } template -fcl::BVHModel* createCylinder(double _baseRadius, double _topRadius, +::fcl::BVHModel* createCylinder(double _baseRadius, double _topRadius, double _height, int _slices, int _stacks, - const fcl::Transform3f& _transform) { + const dart::collision::fcl::Transform3& _transform) { const int CACHE_SIZE = 240; int i, j; @@ -362,8 +363,8 @@ fcl::BVHModel* createCylinder(double _baseRadius, double _topRadius, sinCache[_slices] = sinCache[0]; cosCache[_slices] = cosCache[0]; - fcl::BVHModel* model = new fcl::BVHModel; - fcl::Vec3f p1, p2, p3, p4; + ::fcl::BVHModel* model = new ::fcl::BVHModel; + dart::collision::fcl::Vector3 p1, p2, p3, p4; model->beginModel(); @@ -372,13 +373,13 @@ fcl::BVHModel* createCylinder(double _baseRadius, double _topRadius, costemp = cosCache[0]; radiusLow = _baseRadius; zLow = zBase; - p1 = fcl::Vec3f(radiusLow * sintemp, radiusLow * costemp, zLow); - p1 = _transform.transform(p1); + p1 = dart::collision::fcl::Vector3(radiusLow * sintemp, radiusLow * costemp, zLow); + p1 = dart::collision::fcl::transform(_transform, p1); for (i = 1; i < _slices; i++) { - p2 = fcl::Vec3f(radiusLow * sinCache[i], radiusLow * cosCache[i], zLow); - p3 = fcl::Vec3f(radiusLow * sinCache[i+1], radiusLow * cosCache[i+1], zLow); - p2 = _transform.transform(p2); - p3 = _transform.transform(p3); + p2 = dart::collision::fcl::Vector3(radiusLow * sinCache[i], radiusLow * cosCache[i], zLow); + p3 = dart::collision::fcl::Vector3(radiusLow * sinCache[i+1], radiusLow * cosCache[i+1], zLow); + p2 = dart::collision::fcl::transform(_transform, p2); + p3 = dart::collision::fcl::transform(_transform, p3); model->addTriangle(p1, p2, p3); Eigen::Vector3d v(radiusLow * sinCache[i], radiusLow * cosCache[i], zLow); @@ -394,18 +395,18 @@ fcl::BVHModel* createCylinder(double _baseRadius, double _topRadius, radiusHigh = _baseRadius - deltaRadius * (static_cast(j + 1) / _stacks); - p1 = fcl::Vec3f(radiusLow * sinCache[i], radiusLow * cosCache[i], - zLow); - p2 = fcl::Vec3f(radiusLow * sinCache[i+1], radiusLow * cosCache[i+1], - zLow); - p3 = fcl::Vec3f(radiusHigh * sinCache[i], radiusHigh * cosCache[i], - zHigh); - p4 = fcl::Vec3f(radiusHigh * sinCache[i+1], radiusHigh * cosCache[i+1], - zHigh); - p1 = _transform.transform(p1); - p2 = _transform.transform(p2); - p3 = _transform.transform(p3); - p4 = _transform.transform(p4); + p1 = dart::collision::fcl::Vector3(radiusLow * sinCache[i], radiusLow * cosCache[i], + zLow); + p2 = dart::collision::fcl::Vector3(radiusLow * sinCache[i+1], radiusLow * cosCache[i+1], + zLow); + p3 = dart::collision::fcl::Vector3(radiusHigh * sinCache[i], radiusHigh * cosCache[i], + zHigh); + p4 = dart::collision::fcl::Vector3(radiusHigh * sinCache[i+1], radiusHigh * cosCache[i+1], + zHigh); + p1 = dart::collision::fcl::transform(_transform, p1); + p2 = dart::collision::fcl::transform(_transform, p2); + p3 = dart::collision::fcl::transform(_transform, p3); + p4 = dart::collision::fcl::transform(_transform, p4); model->addTriangle(p1, p2, p3); model->addTriangle(p2, p3, p4); @@ -417,13 +418,13 @@ fcl::BVHModel* createCylinder(double _baseRadius, double _topRadius, costemp = cosCache[0]; radiusLow = _topRadius; zLow = zBase + _height; - p1 = fcl::Vec3f(radiusLow * sintemp, radiusLow * costemp, zLow); - p1 = _transform.transform(p1); + p1 = dart::collision::fcl::Vector3(radiusLow * sintemp, radiusLow * costemp, zLow); + p1 = dart::collision::fcl::transform(_transform, p1); for (i = 1; i < _slices; i++) { - p2 = fcl::Vec3f(radiusLow * sinCache[i], radiusLow * cosCache[i], zLow); - p3 = fcl::Vec3f(radiusLow * sinCache[i+1], radiusLow * cosCache[i+1], zLow); - p2 = _transform.transform(p2); - p3 = _transform.transform(p3); + p2 = dart::collision::fcl::Vector3(radiusLow * sinCache[i], radiusLow * cosCache[i], zLow); + p3 = dart::collision::fcl::Vector3(radiusLow * sinCache[i+1], radiusLow * cosCache[i+1], zLow); + p2 = dart::collision::fcl::transform(_transform, p2); + p3 = dart::collision::fcl::transform(_transform, p3); model->addTriangle(p1, p2, p3); } diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp index f1540fada1266..16ac49e61ee9c 100644 --- a/dart/collision/fcl/FCLCollisionDetector.cpp +++ b/dart/collision/fcl/FCLCollisionDetector.cpp @@ -34,15 +34,6 @@ #include -#include -#include -#include -#include -#include -#include -#include -#include - #include "dart/common/Console.hpp" #include "dart/collision/CollisionObject.hpp" #include "dart/collision/CollisionFilter.hpp" @@ -67,45 +58,49 @@ namespace collision { namespace { bool collisionCallback( - fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* cdata); + dart::collision::fcl::CollisionObject* o1, + dart::collision::fcl::CollisionObject* o2, + void* cdata); bool distanceCallback( - fcl::CollisionObject* o1, - fcl::CollisionObject* o2, + dart::collision::fcl::CollisionObject* o1, + dart::collision::fcl::CollisionObject* o2, void* cdata, - fcl::FCL_REAL& dist); + double& dist); void postProcessFCL( - const fcl::CollisionResult& fclResult, - fcl::CollisionObject* o1, - fcl::CollisionObject* o2, + const dart::collision::fcl::CollisionResult& fclResult, + dart::collision::fcl::CollisionObject* o1, + dart::collision::fcl::CollisionObject* o2, const CollisionOption& option, CollisionResult& result); void postProcessDART( - const fcl::CollisionResult& fclResult, - fcl::CollisionObject* o1, - fcl::CollisionObject* o2, + const dart::collision::fcl::CollisionResult& fclResult, + dart::collision::fcl::CollisionObject* o1, + dart::collision::fcl::CollisionObject* o2, const CollisionOption& option, CollisionResult& result); void interpreteDistanceResult( - const fcl::DistanceResult& fclResult, - fcl::CollisionObject* o1, - fcl::CollisionObject* o2, + const dart::collision::fcl::DistanceResult& fclResult, + dart::collision::fcl::CollisionObject* o1, + dart::collision::fcl::CollisionObject* o2, const DistanceOption& option, DistanceResult& result); -int evalContactPosition(const fcl::Contact& fclContact, - const fcl::BVHModel* mesh1, - const fcl::BVHModel* mesh2, - const fcl::Transform3f& transform1, - const fcl::Transform3f& transform2, +int evalContactPosition(const dart::collision::fcl::Contact& fclContact, + const ::fcl::BVHModel* mesh1, + const ::fcl::BVHModel* mesh2, + const dart::collision::fcl::Transform3& transform1, + const dart::collision::fcl::Transform3& transform2, Eigen::Vector3d& contactPosition1, Eigen::Vector3d& contactPosition2); Eigen::Vector3d getDiff(const Contact& contact1, const Contact& contact2); -fcl::Vec3f getDiff(const fcl::Contact& contact1, const fcl::Contact& contact2); +dart::collision::fcl::Vector3 getDiff( + const dart::collision::fcl::Contact& contact1, + const dart::collision::fcl::Contact& contact2); bool isColinear( const Contact& contact1, @@ -114,31 +109,31 @@ bool isColinear( double tol); bool isColinear( - const fcl::Contact& contact1, - const fcl::Contact& contact2, - const fcl::Contact& contact3, + const dart::collision::fcl::Contact& contact1, + const dart::collision::fcl::Contact& contact2, + const dart::collision::fcl::Contact& contact3, double tol); template bool isColinear(const T& pos1, const T& pos2, const T& pos3, double tol); int FFtest( - const fcl::Vec3f& r1, const fcl::Vec3f& r2, const fcl::Vec3f& r3, - const fcl::Vec3f& R1, const fcl::Vec3f& R2, const fcl::Vec3f& R3, - fcl::Vec3f* res1, fcl::Vec3f* res2); + const dart::collision::fcl::Vector3& r1, const dart::collision::fcl::Vector3& r2, const dart::collision::fcl::Vector3& r3, + const dart::collision::fcl::Vector3& R1, const dart::collision::fcl::Vector3& R2, const dart::collision::fcl::Vector3& R3, + dart::collision::fcl::Vector3* res1, dart::collision::fcl::Vector3* res2); -double triArea(fcl::Vec3f& p1, fcl::Vec3f& p2, fcl::Vec3f& p3); +double triArea(dart::collision::fcl::Vector3& p1, dart::collision::fcl::Vector3& p2, dart::collision::fcl::Vector3& p3); void convertOption( - const CollisionOption& option, fcl::CollisionRequest& request); + const CollisionOption& option, dart::collision::fcl::CollisionRequest& request); void convertOption( - const DistanceOption& option, fcl::DistanceRequest& request); + const DistanceOption& option, dart::collision::fcl::DistanceRequest& request); Contact convertContact( - const fcl::Contact& fclContact, - fcl::CollisionObject* o1, - fcl::CollisionObject* o2, + const dart::collision::fcl::Contact& fclContact, + dart::collision::fcl::CollisionObject* o1, + dart::collision::fcl::CollisionObject* o2, const CollisionOption& option); /// Collision data stores the collision request and the result given by @@ -146,10 +141,10 @@ Contact convertContact( struct FCLCollisionCallbackData { /// FCL collision request - fcl::CollisionRequest fclRequest; + dart::collision::fcl::CollisionRequest fclRequest; /// FCL collision result - fcl::CollisionResult fclResult; + dart::collision::fcl::CollisionResult fclResult; /// Collision option of DART const CollisionOption& option; @@ -204,10 +199,10 @@ struct FCLCollisionCallbackData struct FCLDistanceCallbackData { /// FCL distance request - fcl::DistanceRequest fclRequest; + dart::collision::fcl::DistanceRequest fclRequest; /// FCL distance result - fcl::DistanceResult fclResult; + dart::collision::fcl::DistanceResult fclResult; /// Distance option of DART const DistanceOption& option; @@ -235,7 +230,7 @@ struct FCLDistanceCallbackData //============================================================================== // Create a cube mesh for collision detection template -fcl::BVHModel* createCube(float _sizeX, float _sizeY, float _sizeZ) +::fcl::BVHModel* createCube(float _sizeX, float _sizeY, float _sizeZ) { int faces[6][4] = { @@ -255,20 +250,20 @@ fcl::BVHModel* createCube(float _sizeX, float _sizeY, float _sizeZ) v[0][2] = v[3][2] = v[4][2] = v[7][2] = -_sizeZ / 2; v[1][2] = v[2][2] = v[5][2] = v[6][2] = _sizeZ / 2; - fcl::BVHModel* model = new fcl::BVHModel; - fcl::Vec3f p1, p2, p3; + ::fcl::BVHModel* model = new ::fcl::BVHModel; + dart::collision::fcl::Vector3 p1, p2, p3; model->beginModel(); for (int i = 0; i < 6; i++) { - p1 = fcl::Vec3f(v[faces[i][0]][0], v[faces[i][0]][1], v[faces[i][0]][2]); - p2 = fcl::Vec3f(v[faces[i][1]][0], v[faces[i][1]][1], v[faces[i][1]][2]); - p3 = fcl::Vec3f(v[faces[i][2]][0], v[faces[i][2]][1], v[faces[i][2]][2]); + p1 = dart::collision::fcl::Vector3(v[faces[i][0]][0], v[faces[i][0]][1], v[faces[i][0]][2]); + p2 = dart::collision::fcl::Vector3(v[faces[i][1]][0], v[faces[i][1]][1], v[faces[i][1]][2]); + p3 = dart::collision::fcl::Vector3(v[faces[i][2]][0], v[faces[i][2]][1], v[faces[i][2]][2]); model->addTriangle(p1, p2, p3); - p1 = fcl::Vec3f(v[faces[i][0]][0], v[faces[i][0]][1], v[faces[i][0]][2]); - p2 = fcl::Vec3f(v[faces[i][2]][0], v[faces[i][2]][1], v[faces[i][2]][2]); - p3 = fcl::Vec3f(v[faces[i][3]][0], v[faces[i][3]][1], v[faces[i][3]][2]); + p1 = dart::collision::fcl::Vector3(v[faces[i][0]][0], v[faces[i][0]][1], v[faces[i][0]][2]); + p2 = dart::collision::fcl::Vector3(v[faces[i][2]][0], v[faces[i][2]][1], v[faces[i][2]][2]); + p3 = dart::collision::fcl::Vector3(v[faces[i][3]][0], v[faces[i][3]][1], v[faces[i][3]][2]); model->addTriangle(p1, p2, p3); } model->endModel(); @@ -277,7 +272,7 @@ fcl::BVHModel* createCube(float _sizeX, float _sizeY, float _sizeZ) //============================================================================== template -fcl::BVHModel* createEllipsoid(float _sizeX, float _sizeY, float _sizeZ) +::fcl::BVHModel* createEllipsoid(float _sizeX, float _sizeY, float _sizeZ) { float v[59][3] = { @@ -458,21 +453,21 @@ fcl::BVHModel* createEllipsoid(float _sizeX, float _sizeY, float _sizeZ) {56, 49, 58} }; - fcl::BVHModel* model = new fcl::BVHModel; - fcl::Vec3f p1, p2, p3; + ::fcl::BVHModel* model = new ::fcl::BVHModel; + dart::collision::fcl::Vector3 p1, p2, p3; model->beginModel(); for (int i = 0; i < 112; i++) { - p1 = fcl::Vec3f( + p1 = dart::collision::fcl::Vector3( v[f[i][0]][0] * _sizeX, v[f[i][0]][1] * _sizeY, v[f[i][0]][2] * _sizeZ); - p2 = fcl::Vec3f( + p2 = dart::collision::fcl::Vector3( v[f[i][1]][0] * _sizeX, v[f[i][1]][1] * _sizeY, v[f[i][1]][2] * _sizeZ); - p3 = fcl::Vec3f( + p3 = dart::collision::fcl::Vector3( v[f[i][2]][0] * _sizeX, v[f[i][2]][1] * _sizeY, v[f[i][2]][2] * _sizeZ); @@ -487,8 +482,8 @@ fcl::BVHModel* createEllipsoid(float _sizeX, float _sizeY, float _sizeZ) //============================================================================== template -fcl::BVHModel* createCylinder(double _baseRadius, double _topRadius, - double _height, int _slices, int _stacks) +::fcl::BVHModel* createCylinder(double _baseRadius, double _topRadius, + double _height, int _slices, int _stacks) { const int CACHE_SIZE = 240; @@ -527,8 +522,8 @@ fcl::BVHModel* createCylinder(double _baseRadius, double _topRadius, sinCache[_slices] = sinCache[0]; cosCache[_slices] = cosCache[0]; - fcl::BVHModel* model = new fcl::BVHModel; - fcl::Vec3f p1, p2, p3, p4; + ::fcl::BVHModel* model = new ::fcl::BVHModel; + dart::collision::fcl::Vector3 p1, p2, p3, p4; model->beginModel(); @@ -537,11 +532,11 @@ fcl::BVHModel* createCylinder(double _baseRadius, double _topRadius, costemp = cosCache[0]; radiusLow = _baseRadius; zLow = zBase; - p1 = fcl::Vec3f(radiusLow * sintemp, radiusLow * costemp, zLow); + p1 = dart::collision::fcl::Vector3(radiusLow * sintemp, radiusLow * costemp, zLow); for (i = 1; i < _slices; i++) { - p2 = fcl::Vec3f(radiusLow * sinCache[i], radiusLow * cosCache[i], zLow); - p3 = fcl::Vec3f(radiusLow * sinCache[i+1], radiusLow * cosCache[i+1], zLow); + p2 = dart::collision::fcl::Vector3(radiusLow * sinCache[i], radiusLow * cosCache[i], zLow); + p3 = dart::collision::fcl::Vector3(radiusLow * sinCache[i+1], radiusLow * cosCache[i+1], zLow); model->addTriangle(p1, p2, p3); } @@ -557,14 +552,14 @@ fcl::BVHModel* createCylinder(double _baseRadius, double _topRadius, radiusHigh = _baseRadius - deltaRadius * (static_cast(j + 1) / _stacks); - p1 = fcl::Vec3f(radiusLow * sinCache[i], radiusLow * cosCache[i], - zLow); - p2 = fcl::Vec3f(radiusLow * sinCache[i+1], radiusLow * cosCache[i+1], - zLow); - p3 = fcl::Vec3f(radiusHigh * sinCache[i], radiusHigh * cosCache[i], - zHigh); - p4 = fcl::Vec3f(radiusHigh * sinCache[i+1], radiusHigh * cosCache[i+1], - zHigh); + p1 = dart::collision::fcl::Vector3(radiusLow * sinCache[i], radiusLow * cosCache[i], + zLow); + p2 = dart::collision::fcl::Vector3(radiusLow * sinCache[i+1], radiusLow * cosCache[i+1], + zLow); + p3 = dart::collision::fcl::Vector3(radiusHigh * sinCache[i], radiusHigh * cosCache[i], + zHigh); + p4 = dart::collision::fcl::Vector3(radiusHigh * sinCache[i+1], radiusHigh * cosCache[i+1], + zHigh); model->addTriangle(p1, p2, p3); model->addTriangle(p2, p3, p4); @@ -576,11 +571,11 @@ fcl::BVHModel* createCylinder(double _baseRadius, double _topRadius, costemp = cosCache[0]; radiusLow = _topRadius; zLow = zBase + _height; - p1 = fcl::Vec3f(radiusLow * sintemp, radiusLow * costemp, zLow); + p1 = dart::collision::fcl::Vector3(radiusLow * sintemp, radiusLow * costemp, zLow); for (i = 1; i < _slices; i++) { - p2 = fcl::Vec3f(radiusLow * sinCache[i], radiusLow * cosCache[i], zLow); - p3 = fcl::Vec3f(radiusLow * sinCache[i+1], radiusLow * cosCache[i+1], zLow); + p2 = dart::collision::fcl::Vector3(radiusLow * sinCache[i], radiusLow * cosCache[i], zLow); + p3 = dart::collision::fcl::Vector3(radiusLow * sinCache[i+1], radiusLow * cosCache[i+1], zLow); model->addTriangle(p1, p2, p3); } @@ -590,27 +585,27 @@ fcl::BVHModel* createCylinder(double _baseRadius, double _topRadius, //============================================================================== template -fcl::BVHModel* createMesh(float _scaleX, float _scaleY, float _scaleZ, - const aiScene* _mesh) +::fcl::BVHModel* createMesh(float _scaleX, float _scaleY, float _scaleZ, + const aiScene* _mesh) { // Create FCL mesh from Assimp mesh assert(_mesh); - fcl::BVHModel* model = new fcl::BVHModel; + ::fcl::BVHModel* model = new ::fcl::BVHModel; model->beginModel(); for (std::size_t i = 0; i < _mesh->mNumMeshes; i++) { for (std::size_t j = 0; j < _mesh->mMeshes[i]->mNumFaces; j++) { - fcl::Vec3f vertices[3]; + dart::collision::fcl::Vector3 vertices[3]; for (std::size_t k = 0; k < 3; k++) { const aiVector3D& vertex = _mesh->mMeshes[i]->mVertices[ _mesh->mMeshes[i]->mFaces[j].mIndices[k]]; - vertices[k] = fcl::Vec3f(vertex.x * _scaleX, - vertex.y * _scaleY, - vertex.z * _scaleZ); + vertices[k] = dart::collision::fcl::Vector3(vertex.x * _scaleX, + vertex.y * _scaleY, + vertex.z * _scaleZ); } model->addTriangle(vertices[0], vertices[1], vertices[2]); } @@ -621,21 +616,21 @@ fcl::BVHModel* createMesh(float _scaleX, float _scaleY, float _scaleZ, //============================================================================== template -fcl::BVHModel* createSoftMesh(const aiMesh* _mesh) +::fcl::BVHModel* createSoftMesh(const aiMesh* _mesh) { // Create FCL mesh from Assimp mesh assert(_mesh); - fcl::BVHModel* model = new fcl::BVHModel; + ::fcl::BVHModel* model = new ::fcl::BVHModel; model->beginModel(); for (std::size_t i = 0; i < _mesh->mNumFaces; i++) { - fcl::Vec3f vertices[3]; + dart::collision::fcl::Vector3 vertices[3]; for (std::size_t j = 0; j < 3; j++) { const aiVector3D& vertex = _mesh->mVertices[_mesh->mFaces[i].mIndices[j]]; - vertices[j] = fcl::Vec3f(vertex.x, vertex.y, vertex.z); + vertices[j] = dart::collision::fcl::Vector3(vertex.x, vertex.y, vertex.z); } model->addTriangle(vertices[0], vertices[1], vertices[2]); } @@ -891,7 +886,7 @@ std::unique_ptr FCLCollisionDetector::createCollisionObject( } //============================================================================== -fcl_shared_ptr +fcl_shared_ptr FCLCollisionDetector::claimFCLCollisionGeometry( const dynamics::ConstShapePtr& shape) { @@ -914,7 +909,7 @@ FCLCollisionDetector::claimFCLCollisionGeometry( } //============================================================================== -fcl_shared_ptr +fcl_shared_ptr FCLCollisionDetector::createFCLCollisionGeometry( const dynamics::ConstShapePtr& shape, FCLCollisionDetector::PrimitiveShape type, @@ -929,7 +924,7 @@ FCLCollisionDetector::createFCLCollisionGeometry( using dynamics::MeshShape; using dynamics::SoftMeshShape; - fcl::CollisionGeometry* geom = nullptr; + dart::collision::fcl::CollisionGeometry* geom = nullptr; const auto& shapeType = shape->getType(); if (SphereShape::getStaticType() == shapeType) @@ -940,9 +935,9 @@ FCLCollisionDetector::createFCLCollisionGeometry( const auto radius = sphere->getRadius(); if (FCLCollisionDetector::PRIMITIVE == type) - geom = new fcl::Sphere(radius); + geom = new dart::collision::fcl::Sphere(radius); else - geom = createEllipsoid(radius*2.0, radius*2.0, radius*2.0); + geom = createEllipsoid(radius*2.0, radius*2.0, radius*2.0); } else if (BoxShape::getStaticType() == shapeType) { @@ -952,9 +947,9 @@ FCLCollisionDetector::createFCLCollisionGeometry( const Eigen::Vector3d& size = box->getSize(); if (FCLCollisionDetector::PRIMITIVE == type) - geom = new fcl::Box(size[0], size[1], size[2]); + geom = new dart::collision::fcl::Box(size[0], size[1], size[2]); else - geom = createCube(size[0], size[1], size[2]); + geom = createCube(size[0], size[1], size[2]); } else if (EllipsoidShape::getStaticType() == shapeType) { @@ -966,15 +961,15 @@ FCLCollisionDetector::createFCLCollisionGeometry( if (FCLCollisionDetector::PRIMITIVE == type) { #if FCL_VERSION_AT_LEAST(0,4,0) - geom = new fcl::Ellipsoid(FCLTypes::convertVector3(radii)); + geom = new dart::collision::fcl::Ellipsoid(FCLTypes::convertVector3(radii)); #else - geom = createEllipsoid( + geom = createEllipsoid( radii[0]*2.0, radii[1]*2.0, radii[2]*2.0); #endif } else { - geom = createEllipsoid( + geom = createEllipsoid( radii[0]*2.0, radii[1]*2.0, radii[2]*2.0); } } @@ -988,15 +983,15 @@ FCLCollisionDetector::createFCLCollisionGeometry( if (FCLCollisionDetector::PRIMITIVE == type) { - geom = createCylinder(radius, radius, height, 16, 16); + geom = createCylinder(radius, radius, height, 16, 16); // TODO(JS): We still need to use mesh for cylinder because FCL 0.4.0 // returns single contact point for cylinder yet. Once FCL support // multiple contact points then above code will be replaced by: - // fclCollGeom.reset(new fcl::Cylinder(radius, height)); + // fclCollGeom.reset(new dart::collision::fcl::Cylinder(radius, height)); } else { - geom = createCylinder(radius, radius, height, 16, 16); + geom = createCylinder(radius, radius, height, 16, 16); } } else if (PlaneShape::getStaticType() == shapeType) @@ -1008,11 +1003,11 @@ FCLCollisionDetector::createFCLCollisionGeometry( const Eigen::Vector3d normal = plane->getNormal(); const double offset = plane->getOffset(); - geom = new fcl::Halfspace(FCLTypes::convertVector3(normal), offset); + geom = new dart::collision::fcl::Halfspace(FCLTypes::convertVector3(normal), offset); } else { - geom = createCube(1000.0, 0.0, 1000.0); + geom = createCube(1000.0, 0.0, 1000.0); dtwarn << "[FCLCollisionDetector] PlaneShape is not supported by " << "FCLCollisionDetector. We create a thin box mesh insted, where " << "the size is [1000 0 1000].\n"; @@ -1026,7 +1021,7 @@ FCLCollisionDetector::createFCLCollisionGeometry( const Eigen::Vector3d& scale = shapeMesh->getScale(); auto aiScene = shapeMesh->getMesh(); - geom = createMesh(scale[0], scale[1], scale[2], aiScene); + geom = createMesh(scale[0], scale[1], scale[2], aiScene); } else if (SoftMeshShape::getStaticType() == shapeType) { @@ -1035,7 +1030,7 @@ FCLCollisionDetector::createFCLCollisionGeometry( auto softMeshShape = static_cast(shape.get()); auto aiMesh = softMeshShape->getAssimpMesh(); - geom = createSoftMesh(aiMesh); + geom = createSoftMesh(aiMesh); } else { @@ -1044,10 +1039,10 @@ FCLCollisionDetector::createFCLCollisionGeometry( << shapeType << "]. Creating a sphere with 0.1 radius " << "instead.\n"; - geom = createEllipsoid(0.1, 0.1, 0.1); + geom = createEllipsoid(0.1, 0.1, 0.1); } - return fcl_shared_ptr(geom, deleter); + return fcl_shared_ptr(geom, deleter); } //============================================================================== @@ -1063,7 +1058,7 @@ FCLCollisionDetector::FCLCollisionGeometryDeleter::FCLCollisionGeometryDeleter( //============================================================================== void FCLCollisionDetector::FCLCollisionGeometryDeleter::operator()( - fcl::CollisionGeometry* geom) const + dart::collision::fcl::CollisionGeometry* geom) const { mFCLCollisionDetector->mShapeMap.erase(mShape); @@ -1076,7 +1071,7 @@ namespace { //============================================================================== bool collisionCallback( - fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* cdata) + dart::collision::fcl::CollisionObject* o1, dart::collision::fcl::CollisionObject* o2, void* cdata) { // Return true if you don't want more narrow phase collision checking after // this callback function returns, return false otherwise. @@ -1108,7 +1103,7 @@ bool collisionCallback( fclResult.clear(); // Perform narrow-phase detection - fcl::collide(o1, o2, fclRequest, fclResult); + ::fcl::collide(o1, o2, fclRequest, fclResult); if (result) { @@ -1142,10 +1137,10 @@ bool collisionCallback( //============================================================================== bool distanceCallback( - fcl::CollisionObject* o1, - fcl::CollisionObject* o2, + dart::collision::fcl::CollisionObject* o1, + dart::collision::fcl::CollisionObject* o2, void* ddata, - fcl::FCL_REAL& dist) + double& dist) { auto* distData = static_cast(ddata); @@ -1177,7 +1172,7 @@ bool distanceCallback( fclResult.clear(); // Perform narrow-phase check - fcl::distance(o1, o2, fclRequest, fclResult); + ::fcl::distance(o1, o2, fclRequest, fclResult); // Store the minimum distance just in case result is nullptr. distData->unclampedMinDistance = fclResult.min_distance; @@ -1198,7 +1193,7 @@ Eigen::Vector3d getDiff(const Contact& contact1, const Contact& contact2) } //============================================================================== -fcl::Vec3f getDiff(const fcl::Contact& contact1, const fcl::Contact& contact2) +dart::collision::fcl::Vector3 getDiff(const dart::collision::fcl::Contact& contact1, const dart::collision::fcl::Contact& contact2) { return contact1.pos - contact2.pos; } @@ -1280,9 +1275,9 @@ void markColinearPoints( //============================================================================== void postProcessFCL( - const fcl::CollisionResult& fclResult, - fcl::CollisionObject* o1, - fcl::CollisionObject* o2, + const dart::collision::fcl::CollisionResult& fclResult, + dart::collision::fcl::CollisionObject* o1, + dart::collision::fcl::CollisionObject* o2, const CollisionOption& option, CollisionResult& result) { @@ -1307,15 +1302,15 @@ void postProcessFCL( // mark all the repeated points markRepeatedPoints< - fcl::CollisionResult, - fcl::Contact, - &fcl::CollisionResult::getContact>(markForDeletion, fclResult, tol3); + dart::collision::fcl::CollisionResult, + dart::collision::fcl::Contact, + &dart::collision::fcl::CollisionResult::getContact>(markForDeletion, fclResult, tol3); // remove all the co-linear contact points markColinearPoints< - fcl::CollisionResult, - fcl::Contact, - &fcl::CollisionResult::getContact>(markForDeletion, fclResult, tol); + dart::collision::fcl::CollisionResult, + dart::collision::fcl::Contact, + &dart::collision::fcl::CollisionResult::getContact>(markForDeletion, fclResult, tol); for (auto i = 0u; i < numContacts; ++i) { @@ -1331,9 +1326,9 @@ void postProcessFCL( //============================================================================== void postProcessDART( - const fcl::CollisionResult& fclResult, - fcl::CollisionObject* o1, - fcl::CollisionObject* o2, + const dart::collision::fcl::CollisionResult& fclResult, + dart::collision::fcl::CollisionObject* o1, + dart::collision::fcl::CollisionObject* o2, const CollisionOption& option, CollisionResult& result) { @@ -1368,8 +1363,8 @@ void postProcessDART( auto contactResult = evalContactPosition( c, - static_cast*>(c.o1), - static_cast*>(c.o2), + static_cast*>(c.o1), + static_cast*>(c.o2), FCLTypes::convertTransform(pair1.collisionObject1->getTransform()), FCLTypes::convertTransform(pair1.collisionObject2->getTransform()), pair1.point, @@ -1436,9 +1431,9 @@ void postProcessDART( //============================================================================== void interpreteDistanceResult( - const fcl::DistanceResult& fclResult, - fcl::CollisionObject* o1, - fcl::CollisionObject* o2, + const dart::collision::fcl::DistanceResult& fclResult, + dart::collision::fcl::CollisionObject* o1, + dart::collision::fcl::CollisionObject* o2, const DistanceOption& option, DistanceResult& result) { @@ -1462,11 +1457,11 @@ void interpreteDistanceResult( //============================================================================== int evalContactPosition( - const fcl::Contact& fclContact, - const fcl::BVHModel* mesh1, - const fcl::BVHModel* mesh2, - const fcl::Transform3f& transform1, - const fcl::Transform3f& transform2, + const dart::collision::fcl::Contact& fclContact, + const ::fcl::BVHModel* mesh1, + const ::fcl::BVHModel* mesh2, + const dart::collision::fcl::Transform3& transform1, + const dart::collision::fcl::Transform3& transform2, Eigen::Vector3d& contactPosition1, Eigen::Vector3d& contactPosition2) { @@ -1475,7 +1470,7 @@ int evalContactPosition( auto tri1 = mesh1->tri_indices[id1]; auto tri2 = mesh2->tri_indices[id2]; - fcl::Vec3f v1, v2, v3, p1, p2, p3; + dart::collision::fcl::Vector3 v1, v2, v3, p1, p2, p3; v1 = mesh1->vertices[tri1[0]]; v2 = mesh1->vertices[tri1[1]]; v3 = mesh1->vertices[tri1[2]]; @@ -1484,13 +1479,13 @@ int evalContactPosition( p2 = mesh2->vertices[tri2[1]]; p3 = mesh2->vertices[tri2[2]]; - fcl::Vec3f contact1, contact2; - v1 = transform1.transform(v1); - v2 = transform1.transform(v2); - v3 = transform1.transform(v3); - p1 = transform2.transform(p1); - p2 = transform2.transform(p2); - p3 = transform2.transform(p3); + dart::collision::fcl::Vector3 contact1, contact2; + v1 = dart::collision::fcl::transform(transform1, v1); + v2 = dart::collision::fcl::transform(transform1, v2); + v3 = dart::collision::fcl::transform(transform1, v3); + p1 = dart::collision::fcl::transform(transform2, p1); + p2 = dart::collision::fcl::transform(transform2, p2); + p3 = dart::collision::fcl::transform(transform2, p3); auto testRes = FFtest(v1, v2, v3, p1, p2, p3, &contact1, &contact2); if (testRes == COPLANAR_CONTACT) @@ -1517,9 +1512,9 @@ int evalContactPosition( //============================================================================== int FFtest( - const fcl::Vec3f& r1, const fcl::Vec3f& r2, const fcl::Vec3f& r3, - const fcl::Vec3f& R1, const fcl::Vec3f& R2, const fcl::Vec3f& R3, - fcl::Vec3f* res1, fcl::Vec3f* res2) + const dart::collision::fcl::Vector3& r1, const dart::collision::fcl::Vector3& r2, const dart::collision::fcl::Vector3& r3, + const dart::collision::fcl::Vector3& R1, const dart::collision::fcl::Vector3& R2, const dart::collision::fcl::Vector3& R3, + dart::collision::fcl::Vector3* res1, dart::collision::fcl::Vector3* res2) { float U0[3], U1[3], U2[3], V0[3], V1[3], V2[3], RES1[3], RES2[3]; SET(U0, r1); @@ -1538,10 +1533,10 @@ int FFtest( } //============================================================================== -double triArea(fcl::Vec3f& p1, fcl::Vec3f& p2, fcl::Vec3f& p3) +double triArea(dart::collision::fcl::Vector3& p1, dart::collision::fcl::Vector3& p2, dart::collision::fcl::Vector3& p3) { - fcl::Vec3f a = p2 - p1; - fcl::Vec3f b = p3 - p1; + dart::collision::fcl::Vector3 a = p2 - p1; + dart::collision::fcl::Vector3 b = p3 - p1; double aMag = a[0] * a[0] + a[1] * a[1] + a[2] * a[2]; double bMag = b[0] * b[0] + b[1] * b[1] + b[2] * b[2]; double dp = a[0] * b[0] + a[1] * b[1] + a[2] * b[2]; @@ -1562,9 +1557,9 @@ bool isColinear( //============================================================================== bool isColinear( - const fcl::Contact& contact1, - const fcl::Contact& contact2, - const fcl::Contact& contact3, + const dart::collision::fcl::Contact& contact1, + const dart::collision::fcl::Contact& contact2, + const dart::collision::fcl::Contact& contact3, double tol) { return isColinear(contact1.pos, contact2.pos, contact3.pos, tol); @@ -1585,25 +1580,25 @@ bool isColinear(const T& pos1, const T& pos2, const T& pos3, double tol) } //============================================================================== -void convertOption(const CollisionOption& option, fcl::CollisionRequest& request) +void convertOption(const CollisionOption& option, dart::collision::fcl::CollisionRequest& request) { request.num_max_contacts = option.maxNumContacts; request.enable_contact = option.enableContact; #if FCL_VERSION_AT_LEAST(0,3,0) - request.gjk_solver_type = fcl::GST_LIBCCD; + request.gjk_solver_type = ::fcl::GST_LIBCCD; #endif } //============================================================================== -void convertOption(const DistanceOption& option, fcl::DistanceRequest& request) +void convertOption(const DistanceOption& option, dart::collision::fcl::DistanceRequest& request) { request.enable_nearest_points = option.enableNearestPoints; } //============================================================================== -Contact convertContact(const fcl::Contact& fclContact, - fcl::CollisionObject* o1, - fcl::CollisionObject* o2, +Contact convertContact(const dart::collision::fcl::Contact& fclContact, + dart::collision::fcl::CollisionObject* o1, + dart::collision::fcl::CollisionObject* o2, const CollisionOption& option) { Contact contact; diff --git a/dart/collision/fcl/FCLCollisionDetector.hpp b/dart/collision/fcl/FCLCollisionDetector.hpp index 81cd63a583de8..0521bfbfa2796 100644 --- a/dart/collision/fcl/FCLCollisionDetector.hpp +++ b/dart/collision/fcl/FCLCollisionDetector.hpp @@ -34,7 +34,6 @@ #define DART_COLLISION_FCL_FCLCOLLISIONDETECTOR_HPP_ #include -#include #include "dart/collision/CollisionDetector.hpp" #include "dart/collision/fcl/FCLTypes.hpp" @@ -144,7 +143,7 @@ class FCLCollisionDetector : public CollisionDetector /// Return fcl::CollisionGeometry associated with give Shape. New /// fcl::CollisionGeome will be created if it hasn't created yet. - fcl_shared_ptr claimFCLCollisionGeometry( + fcl_shared_ptr claimFCLCollisionGeometry( const dynamics::ConstShapePtr& shape); protected: @@ -164,7 +163,7 @@ class FCLCollisionDetector : public CollisionDetector FCLCollisionGeometryDeleter(FCLCollisionDetector* cd, const dynamics::ConstShapePtr& shape); - void operator()(fcl::CollisionGeometry* geom) const; + void operator()(dart::collision::fcl::CollisionGeometry* geom) const; private: @@ -176,7 +175,7 @@ class FCLCollisionDetector : public CollisionDetector /// Create fcl::CollisionGeometry with the custom deleter /// FCLCollisionGeometryDeleter - fcl_shared_ptr createFCLCollisionGeometry( + fcl_shared_ptr createFCLCollisionGeometry( const dynamics::ConstShapePtr& shape, FCLCollisionDetector::PrimitiveShape type, const FCLCollisionGeometryDeleter& deleter); @@ -184,7 +183,7 @@ class FCLCollisionDetector : public CollisionDetector private: using ShapeMap = std::map>; + fcl_weak_ptr>; ShapeMap mShapeMap; diff --git a/dart/collision/fcl/FCLCollisionGroup.cpp b/dart/collision/fcl/FCLCollisionGroup.cpp index 516b8805d789c..0e410a58acdbb 100644 --- a/dart/collision/fcl/FCLCollisionGroup.cpp +++ b/dart/collision/fcl/FCLCollisionGroup.cpp @@ -42,7 +42,7 @@ namespace collision { FCLCollisionGroup::FCLCollisionGroup( const CollisionDetectorPtr& collisionDetector) : CollisionGroup(collisionDetector), - mBroadPhaseAlg(new fcl::DynamicAABBTreeCollisionManager()) + mBroadPhaseAlg(new dart::collision::fcl::DynamicAABBTreeCollisionManager()) { // Do nothing } diff --git a/dart/collision/fcl/FCLCollisionGroup.hpp b/dart/collision/fcl/FCLCollisionGroup.hpp index e2d088c29bb67..b23fe48a44335 100644 --- a/dart/collision/fcl/FCLCollisionGroup.hpp +++ b/dart/collision/fcl/FCLCollisionGroup.hpp @@ -33,9 +33,8 @@ #ifndef DART_COLLISION_FCL_FCLCOLLISIONGROUP_HPP_ #define DART_COLLISION_FCL_FCLCOLLISIONGROUP_HPP_ -#include - #include "dart/collision/CollisionGroup.hpp" +#include "dart/collision/fcl/FCLTypes.hpp" namespace dart { namespace collision { @@ -49,7 +48,7 @@ class FCLCollisionGroup : public CollisionGroup friend class FCLCollisionDetector; - using FCLCollisionManager = fcl::DynamicAABBTreeCollisionManager; + using FCLCollisionManager = dart::collision::fcl::DynamicAABBTreeCollisionManager; /// Constructor FCLCollisionGroup(const CollisionDetectorPtr& collisionDetector); diff --git a/dart/collision/fcl/FCLCollisionObject.cpp b/dart/collision/fcl/FCLCollisionObject.cpp index 7d92c8eb5dfa0..88752e45e584e 100644 --- a/dart/collision/fcl/FCLCollisionObject.cpp +++ b/dart/collision/fcl/FCLCollisionObject.cpp @@ -32,8 +32,6 @@ #include "dart/collision/fcl/FCLCollisionObject.hpp" -#include - #include "dart/collision/fcl/FCLTypes.hpp" #include "dart/dynamics/SoftMeshShape.hpp" #include "dart/dynamics/ShapeFrame.hpp" @@ -42,13 +40,13 @@ namespace dart { namespace collision { //============================================================================== -fcl::CollisionObject* FCLCollisionObject::getFCLCollisionObject() +dart::collision::fcl::CollisionObject* FCLCollisionObject::getFCLCollisionObject() { return mFCLCollisionObject.get(); } //============================================================================== -const fcl::CollisionObject* FCLCollisionObject::getFCLCollisionObject() const +const dart::collision::fcl::CollisionObject* FCLCollisionObject::getFCLCollisionObject() const { return mFCLCollisionObject.get(); } @@ -57,9 +55,9 @@ const fcl::CollisionObject* FCLCollisionObject::getFCLCollisionObject() const FCLCollisionObject::FCLCollisionObject( CollisionDetector* collisionDetector, const dynamics::ShapeFrame* shapeFrame, - const fcl_shared_ptr& fclCollGeom) + const fcl_shared_ptr& fclCollGeom) : CollisionObject(collisionDetector, shapeFrame), - mFCLCollisionObject(new fcl::CollisionObject(fclCollGeom)) + mFCLCollisionObject(new dart::collision::fcl::CollisionObject(fclCollGeom)) { mFCLCollisionObject->setUserData(this); } @@ -84,24 +82,24 @@ void FCLCollisionObject::updateEngineData() // TODO(JS): update function be called by somewhere out of here. #if FCL_VERSION_AT_LEAST(0,3,0) - auto collGeom = const_cast( + auto collGeom = const_cast( mFCLCollisionObject->collisionGeometry().get()); #else - fcl::CollisionGeometry* collGeom - = const_cast( + dart::collision::fcl::CollisionGeometry* collGeom + = const_cast( mFCLCollisionObject->getCollisionGeometry()); #endif - assert(dynamic_cast*>(collGeom)); - auto bvhModel = static_cast*>(collGeom); + assert(dynamic_cast<::fcl::BVHModel*>(collGeom)); + auto bvhModel = static_cast<::fcl::BVHModel*>(collGeom); bvhModel->beginUpdateModel(); for (auto i = 0u; i < mesh->mNumFaces; ++i) { - fcl::Vec3f vertices[3]; + dart::collision::fcl::Vector3 vertices[3]; for (auto j = 0u; j < 3; ++j) { const auto& vertex = mesh->mVertices[mesh->mFaces[i].mIndices[j]]; - vertices[j] = fcl::Vec3f(vertex.x, vertex.y, vertex.z); + vertices[j] = dart::collision::fcl::Vector3(vertex.x, vertex.y, vertex.z); } bvhModel->updateTriangle(vertices[0], vertices[1], vertices[2]); } diff --git a/dart/collision/fcl/FCLCollisionObject.hpp b/dart/collision/fcl/FCLCollisionObject.hpp index 1ad768a488527..e531f7a253cb7 100644 --- a/dart/collision/fcl/FCLCollisionObject.hpp +++ b/dart/collision/fcl/FCLCollisionObject.hpp @@ -33,7 +33,6 @@ #ifndef DART_COLLISION_FCL_FCLCOLLISIONOBJECT_HPP_ #define DART_COLLISION_FCL_FCLCOLLISIONOBJECT_HPP_ -#include #include "dart/collision/CollisionObject.hpp" #include "dart/collision/fcl/FCLTypes.hpp" @@ -47,17 +46,17 @@ class FCLCollisionObject : public CollisionObject friend class FCLCollisionDetector; /// Return FCL collision object - fcl::CollisionObject* getFCLCollisionObject(); + dart::collision::fcl::CollisionObject* getFCLCollisionObject(); /// Return FCL collision object - const fcl::CollisionObject* getFCLCollisionObject() const; + const dart::collision::fcl::CollisionObject* getFCLCollisionObject() const; protected: /// Constructor FCLCollisionObject(CollisionDetector* collisionDetector, const dynamics::ShapeFrame* shapeFrame, - const fcl_shared_ptr& fclCollGeom); + const fcl_shared_ptr& fclCollGeom); // Documentation inherited void updateEngineData() override; @@ -65,7 +64,7 @@ class FCLCollisionObject : public CollisionObject protected: /// FCL collision object - std::unique_ptr mFCLCollisionObject; + std::unique_ptr mFCLCollisionObject; }; diff --git a/dart/collision/fcl/FCLTypes.cpp b/dart/collision/fcl/FCLTypes.cpp index 03ec5f8aaa4ad..f6b70da4ec47f 100644 --- a/dart/collision/fcl/FCLTypes.cpp +++ b/dart/collision/fcl/FCLTypes.cpp @@ -35,36 +35,56 @@ namespace dart { namespace collision { +#if FCL_VERSION_AT_LEAST(0,6,0) //============================================================================== -Eigen::Vector3d FCLTypes::convertVector3(const fcl::Vec3f& _vec) +Eigen::Vector3d FCLTypes::convertVector3(const dart::collision::fcl::Vector3& _vec) +{ + return _vec; +} + +//============================================================================== +dart::collision::fcl::Matrix3 FCLTypes::convertMatrix3x3(const Eigen::Matrix3d& _R) +{ + return _R; +} + +//============================================================================== +dart::collision::fcl::Transform3 FCLTypes::convertTransform(const Eigen::Isometry3d& _T) +{ + return _T; +} + +#else +//============================================================================== +Eigen::Vector3d FCLTypes::convertVector3(const dart::collision::fcl::Vector3& _vec) { return Eigen::Vector3d(_vec[0], _vec[1], _vec[2]); } //============================================================================== -fcl::Vec3f FCLTypes::convertVector3(const Eigen::Vector3d& _vec) +dart::collision::fcl::Vector3 FCLTypes::convertVector3(const Eigen::Vector3d& _vec) { - return fcl::Vec3f(_vec[0], _vec[1], _vec[2]); + return dart::collision::fcl::Vector3(_vec[0], _vec[1], _vec[2]); } //============================================================================== -fcl::Matrix3f FCLTypes::convertMatrix3x3(const Eigen::Matrix3d& _R) +dart::collision::fcl::Matrix3 FCLTypes::convertMatrix3x3(const Eigen::Matrix3d& _R) { - return fcl::Matrix3f(_R(0, 0), _R(0, 1), _R(0, 2), - _R(1, 0), _R(1, 1), _R(1, 2), - _R(2, 0), _R(2, 1), _R(2, 2)); + return dart::collision::fcl::Matrix3(_R(0, 0), _R(0, 1), _R(0, 2), + _R(1, 0), _R(1, 1), _R(1, 2), + _R(2, 0), _R(2, 1), _R(2, 2)); } //============================================================================== -fcl::Transform3f FCLTypes::convertTransform(const Eigen::Isometry3d& _T) +dart::collision::fcl::Transform3 FCLTypes::convertTransform(const Eigen::Isometry3d& _T) { - fcl::Transform3f trans; + dart::collision::fcl::Transform3 trans; trans.setTranslation(convertVector3(_T.translation())); trans.setRotation(convertMatrix3x3(_T.linear())); return trans; } - +#endif } // namespace collision } // namespace dart diff --git a/dart/collision/fcl/FCLTypes.hpp b/dart/collision/fcl/FCLTypes.hpp index de037d2ad8187..7f534b7062acd 100644 --- a/dart/collision/fcl/FCLTypes.hpp +++ b/dart/collision/fcl/FCLTypes.hpp @@ -34,28 +34,7 @@ #define DART_COLLISION_FCL_FCLTTYPES_HPP_ #include -#include -#include -#include - -#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)))) - -#if FCL_VERSION_AT_LEAST(0,5,0) -#include -template using fcl_shared_ptr = std::shared_ptr; -template using fcl_weak_ptr = std::weak_ptr; -#else -#include -template using fcl_shared_ptr = boost::shared_ptr; -template using fcl_weak_ptr = boost::weak_ptr; -#endif +#include "dart/collision/fcl/BackwardCompatibility.hpp" namespace dart { namespace collision { @@ -63,20 +42,21 @@ namespace collision { class FCLTypes { public: - /// Convert FCL vector3 type to Eigen vector3 type - static Eigen::Vector3d convertVector3(const fcl::Vec3f& _vec); - +#if !FCL_VERSION_AT_LEAST(0,6,0) /// Convert Eigen vector3 type to FCL vector3 type - static fcl::Vec3f convertVector3(const Eigen::Vector3d& _vec); + static dart::collision::fcl::Vector3 convertVector3(const Eigen::Vector3d& _vec); +#endif + /// Convert FCL vector3 type to Eigen vector3 type + static Eigen::Vector3d convertVector3(const dart::collision::fcl::Vector3& _vec); /// Convert FCL matrix3x3 type to Eigen matrix3x3 type - static fcl::Matrix3f convertMatrix3x3(const Eigen::Matrix3d& _R); + static dart::collision::fcl::Matrix3 convertMatrix3x3(const Eigen::Matrix3d& _R); /// Convert FCL transformation type to Eigen transformation type - static fcl::Transform3f convertTransform(const Eigen::Isometry3d& _T); + static dart::collision::fcl::Transform3 convertTransform(const Eigen::Isometry3d& _T); }; -} // namespace collision -} // namespace dart +} // namespace collision +} // namespace dart -#endif // DART_COLLISION_FCL_FCLTTYPES_HPP_ +#endif // DART_COLLISION_FCL_FCLTTYPES_HPP_ diff --git a/dart/constraint/ContactConstraint.hpp b/dart/constraint/ContactConstraint.hpp index 89eeb75295256..65d5722bd77bb 100644 --- a/dart/constraint/ContactConstraint.hpp +++ b/dart/constraint/ContactConstraint.hpp @@ -168,10 +168,10 @@ class ContactConstraint : public ConstraintBase double mRestitutionCoeff; /// Local body jacobians for mBodyNode1 - Eigen::aligned_vector mJacobians1; + dart::aligned_vector mJacobians1; /// Local body jacobians for mBodyNode2 - Eigen::aligned_vector mJacobians2; + dart::aligned_vector mJacobians2; /// bool mIsFrictionOn; diff --git a/dart/constraint/SoftContactConstraint.hpp b/dart/constraint/SoftContactConstraint.hpp index 7b07a04664085..cfa5e9832aef5 100644 --- a/dart/constraint/SoftContactConstraint.hpp +++ b/dart/constraint/SoftContactConstraint.hpp @@ -205,10 +205,10 @@ class SoftContactConstraint : public ConstraintBase double mRestitutionCoeff; /// Local body jacobians for mBodyNode1 - Eigen::aligned_vector mJacobians1; + dart::aligned_vector mJacobians1; /// Local body jacobians for mBodyNode2 - Eigen::aligned_vector mJacobians2; + dart::aligned_vector mJacobians2; /// Contact normal expressed in body frame of the first body node Eigen::Vector3d mBodyDirection1; diff --git a/dart/dynamics/LineSegmentShape.cpp b/dart/dynamics/LineSegmentShape.cpp index 71687e12f88c2..60a601ca35e59 100644 --- a/dart/dynamics/LineSegmentShape.cpp +++ b/dart/dynamics/LineSegmentShape.cpp @@ -239,7 +239,7 @@ void LineSegmentShape::addConnection(std::size_t _idx1, std::size_t _idx2) void LineSegmentShape::removeConnection(std::size_t _vertexIdx1, std::size_t _vertexIdx2) { // Search through all connections to remove any that match the request - Eigen::aligned_vector::iterator it = mConnections.begin(); + dart::aligned_vector::iterator it = mConnections.begin(); while(it != mConnections.end()) { const Eigen::Vector2i c = (*it); @@ -278,7 +278,7 @@ void LineSegmentShape::removeConnection(std::size_t _connectionIdx) } //============================================================================== -const Eigen::aligned_vector& +const dart::aligned_vector& LineSegmentShape::getConnections() const { return mConnections; diff --git a/dart/dynamics/LineSegmentShape.hpp b/dart/dynamics/LineSegmentShape.hpp index fd239fa50c196..6733f890c6b75 100644 --- a/dart/dynamics/LineSegmentShape.hpp +++ b/dart/dynamics/LineSegmentShape.hpp @@ -100,7 +100,7 @@ class LineSegmentShape : public Shape void removeConnection(std::size_t _connectionIdx); /// Get all the connections - const Eigen::aligned_vector& getConnections() const; + const dart::aligned_vector& getConnections() const; /// The returned inertia matrix will be like a very thin cylinder. The _mass /// will be evenly distributed across all lines. @@ -120,7 +120,7 @@ class LineSegmentShape : public Shape std::vector mVertices; /// Vector of connections - Eigen::aligned_vector mConnections; + dart::aligned_vector mConnections; /// A dummy vertex that can be returned when an out-of-bounds vertex is /// requested diff --git a/dart/dynamics/Skeleton.hpp b/dart/dynamics/Skeleton.hpp index aba8d1a5334dc..9d7d8320aa4d9 100644 --- a/dart/dynamics/Skeleton.hpp +++ b/dart/dynamics/Skeleton.hpp @@ -1251,7 +1251,7 @@ class Skeleton : EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; - mutable Eigen::aligned_vector mTreeCache; + mutable dart::aligned_vector mTreeCache; mutable DataCache mSkelCache; diff --git a/dart/gui/OpenGLRenderInterface.cpp b/dart/gui/OpenGLRenderInterface.cpp index cc6a45aabd7eb..f1aeda54bc0a1 100644 --- a/dart/gui/OpenGLRenderInterface.cpp +++ b/dart/gui/OpenGLRenderInterface.cpp @@ -630,7 +630,7 @@ void OpenGLRenderInterface::drawList(GLuint index) { void OpenGLRenderInterface::drawLineSegments( const std::vector& _vertices, - const Eigen::aligned_vector& _connections) + const dart::aligned_vector& _connections) { glBegin(GL_LINES); for(const Eigen::Vector2i& c : _connections) diff --git a/dart/gui/OpenGLRenderInterface.hpp b/dart/gui/OpenGLRenderInterface.hpp index 2200edfba42f4..f3b1decf2beb6 100644 --- a/dart/gui/OpenGLRenderInterface.hpp +++ b/dart/gui/OpenGLRenderInterface.hpp @@ -93,7 +93,7 @@ class OpenGLRenderInterface : public RenderInterface { void drawSoftMesh(const aiMesh* mesh) override; void drawList(GLuint index) override; void drawLineSegments(const std::vector& _vertices, - const Eigen::aligned_vector& _connections) override; + const dart::aligned_vector& _connections) override; void setPenColor(const Eigen::Vector4d& _col) override; void setPenColor(const Eigen::Vector3d& _col) override; diff --git a/dart/gui/RenderInterface.cpp b/dart/gui/RenderInterface.cpp index 836c6b0695e3f..a3102879a3547 100644 --- a/dart/gui/RenderInterface.cpp +++ b/dart/gui/RenderInterface.cpp @@ -129,7 +129,7 @@ void RenderInterface::drawList(unsigned int /*indeX*/) } void RenderInterface::drawLineSegments(const std::vector&, - const Eigen::aligned_vector&) + const dart::aligned_vector&) { } diff --git a/dart/gui/RenderInterface.hpp b/dart/gui/RenderInterface.hpp index f541ba56573c9..c44b1b0438043 100644 --- a/dart/gui/RenderInterface.hpp +++ b/dart/gui/RenderInterface.hpp @@ -103,7 +103,7 @@ class RenderInterface { virtual void drawSoftMesh(const aiMesh* mesh); virtual void drawList(unsigned int index); virtual void drawLineSegments(const std::vector& _vertices, - const Eigen::aligned_vector& _connections); + const dart::aligned_vector& _connections); virtual unsigned int compileDisplayList(const Eigen::Vector3d& _size, const aiScene* _mesh); diff --git a/dart/gui/osg/render/LineSegmentShapeNode.cpp b/dart/gui/osg/render/LineSegmentShapeNode.cpp index 0ae8a81d72638..caa653e4eba4d 100644 --- a/dart/gui/osg/render/LineSegmentShapeNode.cpp +++ b/dart/gui/osg/render/LineSegmentShapeNode.cpp @@ -205,7 +205,7 @@ void LineSegmentShapeDrawable::refresh(bool firstTime) const std::vector& vertices = mLineSegmentShape->getVertices(); - const Eigen::aligned_vector& connections = + const dart::aligned_vector& connections = mLineSegmentShape->getConnections(); if( mLineSegmentShape->checkDataVariance(dart::dynamics::Shape::DYNAMIC_ELEMENTS) diff --git a/dart/io/SkelParser.cpp b/dart/io/SkelParser.cpp index 50c352f870ef3..356cc13ddfa65 100644 --- a/dart/io/SkelParser.cpp +++ b/dart/io/SkelParser.cpp @@ -113,7 +113,7 @@ struct SkelJoint }; // first: BodyNode name | second: BodyNode information -using BodyMap = Eigen::aligned_map; +using BodyMap = dart::aligned_map; // first: Child BodyNode name | second: Joint information using JointMap = std::map; diff --git a/dart/io/sdf/SdfParser.cpp b/dart/io/sdf/SdfParser.cpp index 81cda0b6ec8fa..75dc8ee993e14 100644 --- a/dart/io/sdf/SdfParser.cpp +++ b/dart/io/sdf/SdfParser.cpp @@ -94,7 +94,7 @@ struct SDFJoint }; // Maps the name of a BodyNode to its properties -using BodyMap = Eigen::aligned_map; +using BodyMap = dart::aligned_map; // Maps a child BodyNode to the properties of its parent Joint using JointMap = std::map; diff --git a/dart/math/Geometry.hpp b/dart/math/Geometry.hpp index 5d8845cfaa529..a0c3848df585f 100644 --- a/dart/math/Geometry.hpp +++ b/dart/math/Geometry.hpp @@ -496,7 +496,7 @@ void computeNullSpace(const MatrixType& _M, ReturnType& _NS) typedef std::vector SupportGeometry; -typedef Eigen::aligned_vector SupportPolygon; +typedef dart::aligned_vector SupportPolygon; /// Project the support geometry points onto a plane with the given axes /// and then compute their convex hull, which will take the form of a polgyon. diff --git a/dart/math/MathTypes.hpp b/dart/math/MathTypes.hpp index 6e8f25b52be34..2ed41629752fb 100644 --- a/dart/math/MathTypes.hpp +++ b/dart/math/MathTypes.hpp @@ -60,6 +60,10 @@ inline Vector6d compose(const Eigen::Vector3d& _angular, typedef std::vector EIGEN_V_VEC3D; typedef std::vector > EIGEN_VV_VEC3D; +} // namespace Eigen + +namespace dart { + #if EIGEN_VERSION_AT_LEAST(3,2,1) && EIGEN_VERSION_AT_MOST(3,2,8) template @@ -89,9 +93,6 @@ std::shared_ptr<_Tp> make_aligned_shared(_Args&&... __args) std::forward<_Args>(__args)...); } -} // namespace Eigen - -namespace dart { namespace math { typedef Eigen::Matrix6d Inertia; diff --git a/examples/osgExamples/osgAtlasPuppet/osgAtlasPuppet.cpp b/examples/osgExamples/osgAtlasPuppet/osgAtlasPuppet.cpp index 73161ce4d06d7..4d8f4cbe160b7 100644 --- a/examples/osgExamples/osgAtlasPuppet/osgAtlasPuppet.cpp +++ b/examples/osgExamples/osgAtlasPuppet/osgAtlasPuppet.cpp @@ -477,7 +477,7 @@ class InputHandler : public ::osgGA::GUIEventHandler std::vector< std::pair > mDefaultBounds; - Eigen::aligned_vector mDefaultTargetTf; + dart::aligned_vector mDefaultTargetTf; std::shared_ptr mPosture; diff --git a/examples/osgExamples/osgHuboPuppet/osgHuboPuppet.cpp b/examples/osgExamples/osgHuboPuppet/osgHuboPuppet.cpp index f7bb517b8558e..07f1ea65b3fcf 100644 --- a/examples/osgExamples/osgHuboPuppet/osgHuboPuppet.cpp +++ b/examples/osgExamples/osgHuboPuppet/osgHuboPuppet.cpp @@ -1049,7 +1049,7 @@ class InputHandler : public ::osgGA::GUIEventHandler std::vector< std::pair > mDefaultBounds; - Eigen::aligned_vector mDefaultTargetTf; + dart::aligned_vector mDefaultTargetTf; std::shared_ptr mPosture; diff --git a/examples/osgExamples/osgWamIkFast/Helpers.cpp b/examples/osgExamples/osgWamIkFast/Helpers.cpp index 61c6729b5b2c1..7a603f3c4dbb5 100644 --- a/examples/osgExamples/osgWamIkFast/Helpers.cpp +++ b/examples/osgExamples/osgWamIkFast/Helpers.cpp @@ -32,7 +32,7 @@ #include "Helpers.hpp" -#include +#include //============================================================================== dart::dynamics::SkeletonPtr createGround() @@ -62,7 +62,7 @@ dart::dynamics::SkeletonPtr createGround() //============================================================================== dart::dynamics::SkeletonPtr createWam() { - dart::utils::DartLoader urdfParser; + dart::io::DartLoader urdfParser; urdfParser.addPackageDirectory("herb_description", DART_DATA_PATH"/urdf/wam"); dart::dynamics::SkeletonPtr wam = urdfParser.parseSkeleton(DART_DATA_PATH"/urdf/wam/wam.urdf"); diff --git a/examples/osgExamples/osgWamIkFast/InputHandler.hpp b/examples/osgExamples/osgWamIkFast/InputHandler.hpp index 3de416f660011..78600d74767bd 100644 --- a/examples/osgExamples/osgWamIkFast/InputHandler.hpp +++ b/examples/osgExamples/osgWamIkFast/InputHandler.hpp @@ -68,5 +68,5 @@ class InputHandler : public ::osgGA::GUIEventHandler std::vector< std::pair > mDefaultBounds; - Eigen::aligned_vector mDefaultTargetTf; + dart::aligned_vector mDefaultTargetTf; }; diff --git a/examples/osgExamples/osgWamIkFast/WamWorld.hpp b/examples/osgExamples/osgWamIkFast/WamWorld.hpp index e7304bd61a294..90cb8f8bcc449 100644 --- a/examples/osgExamples/osgWamIkFast/WamWorld.hpp +++ b/examples/osgExamples/osgWamIkFast/WamWorld.hpp @@ -32,8 +32,8 @@ #include #include -#include -#include +#include +#include using namespace dart::dynamics; using namespace dart::simulation; diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt index 028c35563345a..e6a7a6bbd40f8 100644 --- a/unittests/CMakeLists.txt +++ b/unittests/CMakeLists.txt @@ -50,9 +50,9 @@ set_target_properties( # - DART_${test_type}_TESTS # # Usage: -# add_add_test("unit" test_UnitTestA) # assumed source is test_UnitTestA.cpp -# add_add_test("unit" test_UnitTestB test_SourceB1.cpp) -# add_add_test("unit" test_UnitTestA test_SourceC1.cpp test_SourceC2.cpp) +# dart_add_test("unit" test_UnitTestA) # assumed source is test_UnitTestA.cpp +# dart_add_test("unit" test_UnitTestB test_SourceB1.cpp) +# dart_add_test("unit" test_UnitTestA test_SourceC1.cpp test_SourceC2.cpp) #=============================================================================== function(dart_add_test test_type target_name) # ARGN for source files diff --git a/unittests/comprehensive/test_Collision.cpp b/unittests/comprehensive/test_Collision.cpp index f3135f81f2289..4e7a03650c796 100644 --- a/unittests/comprehensive/test_Collision.cpp +++ b/unittests/comprehensive/test_Collision.cpp @@ -33,15 +33,12 @@ #include #include -#include -#include -#include - #include "dart/config.hpp" #include "dart/common/common.hpp" #include "dart/math/math.hpp" #include "dart/dynamics/dynamics.hpp" #include "dart/collision/collision.hpp" +#include "dart/collision/fcl/fcl.hpp" #if HAVE_ODE #include "dart/collision/ode/ode.hpp" #endif @@ -63,28 +60,28 @@ using namespace io; class COLLISION : public testing::Test { public: - void unrotatedTest(fcl::CollisionGeometry* _coll1, - fcl::CollisionGeometry* _coll2, + void unrotatedTest(dart::collision::fcl::CollisionGeometry* _coll1, + dart::collision::fcl::CollisionGeometry* _coll2, double expectedContactPoint, int _idxAxis); - void dropWithRotation(fcl::CollisionGeometry* _object, + void dropWithRotation(dart::collision::fcl::CollisionGeometry* _object, double EulerZ, double EulerY, double EulerX); - void printResult(const fcl::CollisionResult& _result); + void printResult(const dart::collision::fcl::CollisionResult& _result); }; -void COLLISION::unrotatedTest(fcl::CollisionGeometry* _coll1, - fcl::CollisionGeometry* _coll2, +void COLLISION::unrotatedTest(dart::collision::fcl::CollisionGeometry* _coll1, + dart::collision::fcl::CollisionGeometry* _coll2, double expectedContactPoint, int _idxAxis) { - fcl::CollisionResult result; - fcl::CollisionRequest request; + dart::collision::fcl::CollisionResult result; + dart::collision::fcl::CollisionRequest request; request.enable_contact = true; request.num_max_contacts = 100; - fcl::Vec3f position(0, 0, 0); + dart::collision::fcl::Vector3 position(0, 0, 0); - fcl::Transform3f coll1_transform; - fcl::Transform3f coll2_transform; + dart::collision::fcl::Transform3 coll1_transform; + dart::collision::fcl::Transform3 coll2_transform; //========================================================================== // Approaching test @@ -94,17 +91,17 @@ void COLLISION::unrotatedTest(fcl::CollisionGeometry* _coll1, double pos = 10.0; coll1_transform.setIdentity(); - coll1_transform.setTranslation(fcl::Vec3f(0, 0, 0)); + dart::collision::fcl::setTranslation(coll1_transform, dart::collision::fcl::Vector3(0, 0, 0)); coll2_transform.setIdentity(); // Let's drop box2 until it collide with box1 do { position[_idxAxis] = pos; - coll2_transform.setTranslation(position); + dart::collision::fcl::setTranslation(coll2_transform, position); - fcl::collide(_coll1, coll1_transform, - _coll2, coll2_transform, - request, result); + ::fcl::collide(_coll1, coll1_transform, + _coll2, coll2_transform, + request, result); pos += dpos; } @@ -124,36 +121,34 @@ void COLLISION::unrotatedTest(fcl::CollisionGeometry* _coll1, { EXPECT_GE(result.getContact(i).penetration_depth, 0.0); // EXPECT_NEAR(result.getContact(i).normal[_idxAxis], -1.0); - EXPECT_EQ(result.getContact(i).normal.length(), 1.0); + EXPECT_EQ(dart::collision::fcl::length(result.getContact(i).normal), 1.0); EXPECT_NEAR(result.getContact(i).pos[_idxAxis], expectedContactPoint, -dpos*2.0); } } -void COLLISION::dropWithRotation(fcl::CollisionGeometry* _object, +void COLLISION::dropWithRotation(dart::collision::fcl::CollisionGeometry* _object, double EulerZ, double EulerY, double EulerX) { // Collision test setting - fcl::CollisionResult result; - fcl::CollisionRequest request; + dart::collision::fcl::CollisionResult result; + dart::collision::fcl::CollisionRequest request; request.enable_contact = true; request.num_max_contacts = 100; // Ground like box setting - fcl::Box groundObject(100, 100, 0.1); - fcl::Transform3f groundTransf; + dart::collision::fcl::Box groundObject(100, 100, 0.1); + dart::collision::fcl::Transform3 groundTransf; groundTransf.setIdentity(); - fcl::Vec3f ground_position(0, 0, 0); - groundTransf.setTranslation(ground_position); + dart::collision::fcl::Vector3 ground_position(0, 0, 0); + dart::collision::fcl::setTranslation(groundTransf, ground_position); // Dropping object setting - fcl::Transform3f objectTransf; - fcl::Matrix3f rot; - rot.setEulerZYX(EulerZ, - EulerY, - EulerX); - objectTransf.setRotation(rot); - fcl::Vec3f dropping_position(0, 0, 0); - objectTransf.setTranslation(dropping_position); + dart::collision::fcl::Transform3 objectTransf; + dart::collision::fcl::Matrix3 rot; + dart::collision::fcl::setEulerZYX(rot, EulerZ, EulerY, EulerX); + dart::collision::fcl::setRotation(objectTransf, rot); + dart::collision::fcl::Vector3 dropping_position(0, 0, 0); + dart::collision::fcl::setTranslation(objectTransf, dropping_position); //========================================================================== // Dropping test in x, y, z aixs each. @@ -162,44 +157,44 @@ void COLLISION::dropWithRotation(fcl::CollisionGeometry* _object, { result.clear(); - groundObject.side = fcl::Vec3f(100, 100, 100); + groundObject.side = dart::collision::fcl::Vector3(100, 100, 100); groundObject.side[_idxAxis] = 0.1; - ground_position.setValue(0, 0, 0); + ground_position = dart::collision::fcl::Vector3(0, 0, 0); ground_position[_idxAxis] = -0.05; - groundTransf.setTranslation(ground_position); + dart::collision::fcl::setTranslation(groundTransf, ground_position); // Let's drop the object until it collide with ground double posDelta = -0.0001; double initPos = 10.0; - dropping_position.setValue(0, 0, 0); + dropping_position = dart::collision::fcl::Vector3(0, 0, 0); do { dropping_position[_idxAxis] = initPos; - objectTransf.setTranslation(dropping_position); + dart::collision::fcl::setTranslation(objectTransf, dropping_position); - fcl::collide(_object, objectTransf, - &groundObject, groundTransf, - request, result); + ::fcl::collide(_object, objectTransf, + &groundObject, groundTransf, + request, result); initPos += posDelta; } while (result.numContacts() == 0); std::cout << "Current position of the object: " - << objectTransf.getTranslation() + << dart::collision::fcl::getTranslation(objectTransf) << std::endl << "Number of contacts: " << result.numContacts() << std::endl; - fcl::Transform3f objectTransfInv = objectTransf; + dart::collision::fcl::Transform3 objectTransfInv = objectTransf; objectTransfInv.inverse(); for (std::size_t i = 0; i < result.numContacts(); ++i) { - fcl::Vec3f posWorld = objectTransfInv.transform(result.getContact(i).pos); + dart::collision::fcl::Vector3 posWorld = dart::collision::fcl::transform(objectTransfInv, result.getContact(i).pos); std::cout << "----- CONTACT " << i << " --------" << std::endl; std::cout << "contact_points: " << result.getContact(i).pos << std::endl; std::cout << "contact_points(w): " << posWorld << std::endl; - std::cout << "norm: " << result.getContact(i).pos.length() << std::endl; + std::cout << "norm: " << dart::collision::fcl::length(result.getContact(i).pos) << std::endl; std::cout << "penetration_depth: " << result.getContact(i).penetration_depth << std::endl; std::cout << "normal: " << result.getContact(i).normal << std::endl; } @@ -208,7 +203,7 @@ void COLLISION::dropWithRotation(fcl::CollisionGeometry* _object, } } -void COLLISION::printResult(const fcl::CollisionResult& _result) +void COLLISION::printResult(const dart::collision::fcl::CollisionResult& _result) { std::cout << "====== [ RESULT ] ======" << std::endl; std::cout << "The number of contacts: " << _result.numContacts() << std::endl; @@ -227,164 +222,164 @@ void COLLISION::printResult(const fcl::CollisionResult& _result) /* ********************************************************************************************* */ //TEST_F(COLLISION, BOX_BOX_X) { -// fcl::Box box1(2, 2, 2); -// fcl::Box box2(1, 1, 1); +// dart::collision::fcl::Box box1(2, 2, 2); +// dart::collision::fcl::Box box2(1, 1, 1); // unrotatedTest(&box1, &box2, 1.0, 0); // x-axis //} //TEST_F(COLLISION, BOX_BOX_Y) { -// fcl::Box box1(2, 2, 2); -// fcl::Box box2(1, 1, 1); +// dart::collision::fcl::Box box1(2, 2, 2); +// dart::collision::fcl::Box box2(1, 1, 1); // unrotatedTest(&box1, &box2, 1.0, 1); // y-axis //} //TEST_F(COLLISION, BOX_BOX_Z) { -// fcl::Box box1(2, 2, 2); -// fcl::Box box2(1, 1, 1); +// dart::collision::fcl::Box box1(2, 2, 2); +// dart::collision::fcl::Box box2(1, 1, 1); // unrotatedTest(&box1, &box2, 1.0, 2); // z-axis //} //TEST_F(COLLISION, BOX_SPHERE_X) { -// fcl::Box box1(2, 2, 2); -// fcl::Sphere sphere(0.5); +// dart::collision::fcl::Box box1(2, 2, 2); +// dart::collision::fcl::Sphere sphere(0.5); // unrotatedTest(&box1, &sphere, 1.0, 0); // x-axis //} //TEST_F(COLLISION, BOX_SPHERE_Y) { -// fcl::Box box1(2, 2, 2); -// fcl::Sphere sphere(0.5); +// dart::collision::fcl::Box box1(2, 2, 2); +// dart::collision::fcl::Sphere sphere(0.5); // unrotatedTest(&box1, &sphere, 1.0, 1); // y-axis //} //TEST_F(COLLISION, BOX_SPHERE_Z) { -// fcl::Box box1(2, 2, 2); -// fcl::Sphere sphere(0.5); +// dart::collision::fcl::Box box1(2, 2, 2); +// dart::collision::fcl::Sphere sphere(0.5); // unrotatedTest(&box1, &sphere, 1.0, 2); // z-axis //} //TEST_F(COLLISION, SPHERE_BOX_X) { -// fcl::Sphere obj1(0.5); -// fcl::Box obj2(2, 2, 2); +// dart::collision::fcl::Sphere obj1(0.5); +// dart::collision::fcl::Box obj2(2, 2, 2); // unrotatedTest(&obj1, &obj2, 0.5, 0); // x-axis //} //TEST_F(COLLISION, SPHERE_BOX_Y) { -// fcl::Sphere obj1(0.5); -// fcl::Box obj2(2, 2, 2); +// dart::collision::fcl::Sphere obj1(0.5); +// dart::collision::fcl::Box obj2(2, 2, 2); // unrotatedTest(&obj1, &obj2, 0.5, 1); // y-axis //} //TEST_F(COLLISION, SPHERE_BOX_Z) { -// fcl::Sphere obj1(0.5); -// fcl::Box obj2(2, 2, 2); +// dart::collision::fcl::Sphere obj1(0.5); +// dart::collision::fcl::Box obj2(2, 2, 2); // unrotatedTest(&obj1, &obj2, 0.5, 2); // z-axis //} //TEST_F(COLLISION, SPHERE_SPHERE_X) { -// fcl::Sphere sphere1(1); -// fcl::Sphere sphere2(0.5); +// dart::collision::fcl::Sphere sphere1(1); +// dart::collision::fcl::Sphere sphere2(0.5); // unrotatedTest(&sphere1, &sphere2, 1.0, 0); // x-axis //} //TEST_F(COLLISION, SPHERE_SPHERE_Y) { -// fcl::Sphere sphere1(1); -// fcl::Sphere sphere2(0.5); +// dart::collision::fcl::Sphere sphere1(1); +// dart::collision::fcl::Sphere sphere2(0.5); // unrotatedTest(&sphere1, &sphere2, 1.0, 1); // y-axis //} //TEST_F(COLLISION, SPHERE_SPHERE_Z) { -// fcl::Sphere sphere1(1); -// fcl::Sphere sphere2(0.5); +// dart::collision::fcl::Sphere sphere1(1); +// dart::collision::fcl::Sphere sphere2(0.5); // unrotatedTest(&sphere1, &sphere2, 1.0, 2); // z-axis //} //TEST_F(COLLISION, PLANE_BOX_X) { -// fcl::Plane obj1(1, 0, 0, 0); -// fcl::Box obj2(1, 1, 1); +// dart::collision::fcl::Plane obj1(1, 0, 0, 0); +// dart::collision::fcl::Box obj2(1, 1, 1); // unrotatedTest(&obj1, &obj2, 0.0, 0); // x-axis //} //TEST_F(COLLISION, PLANE_BOX_Y) { -// fcl::Plane obj1(0, 1, 0, 0); -// fcl::Box obj2(1, 1, 1); +// dart::collision::fcl::Plane obj1(0, 1, 0, 0); +// dart::collision::fcl::Box obj2(1, 1, 1); // unrotatedTest(&obj1, &obj2, 0.0, 1); // x-axis //} //TEST_F(COLLISION, PLANE_BOX_Z) { -// fcl::Plane obj1(0, 0, 1, 0); -// fcl::Box obj2(1, 1, 1); +// dart::collision::fcl::Plane obj1(0, 0, 1, 0); +// dart::collision::fcl::Box obj2(1, 1, 1); // unrotatedTest(&obj1, &obj2, 0.0, 2); // x-axis //} //TEST_F(COLLISION, PLANE_SPHERE_X) { -// fcl::Plane obj1(1, 0, 0, 0); -// fcl::Sphere obj2(0.5); +// dart::collision::fcl::Plane obj1(1, 0, 0, 0); +// dart::collision::fcl::Sphere obj2(0.5); // unrotatedTest(&obj1, &obj2, 0.0, 0); // x-axis //} //TEST_F(COLLISION, PLANE_SPHERE_Y) { -// fcl::Plane obj1(0, 1, 0, 0); -// fcl::Sphere obj2(0.5); +// dart::collision::fcl::Plane obj1(0, 1, 0, 0); +// dart::collision::fcl::Sphere obj2(0.5); // unrotatedTest(&obj1, &obj2, 0.0, 1); // x-axis //} //TEST_F(COLLISION, PLANE_SPHERE_Z) { -// fcl::Plane obj1(0, 0, 1, 0); -// fcl::Sphere obj2(0.5); +// dart::collision::fcl::Plane obj1(0, 0, 1, 0); +// dart::collision::fcl::Sphere obj2(0.5); // unrotatedTest(&obj1, &obj2, 0.0, 2); // x-axis //} //TEST_F(COLLISION, PLANE_CYLINDER_X) { -// fcl::Plane obj1(1, 0, 0, 0); -// fcl::Cylinder obj2(0.5, 1); +// dart::collision::fcl::Plane obj1(1, 0, 0, 0); +// dart::collision::fcl::Cylinder obj2(0.5, 1); // unrotatedTest(&obj1, &obj2, 0.0, 0); // x-axis //} //TEST_F(COLLISION, PLANE_CYLINDER_Y) { -// fcl::Plane obj1(0, 1, 0, 0); -// fcl::Cylinder obj2(0.5, 1); +// dart::collision::fcl::Plane obj1(0, 1, 0, 0); +// dart::collision::fcl::Cylinder obj2(0.5, 1); // unrotatedTest(&obj1, &obj2, 0.0, 1); // x-axis //} //TEST_F(COLLISION, PLANE_CYLINDER_Z) { -// fcl::Plane obj1(0, 0, 1, 0); -// fcl::Cylinder obj2(0.5, 1); +// dart::collision::fcl::Plane obj1(0, 0, 1, 0); +// dart::collision::fcl::Cylinder obj2(0.5, 1); // unrotatedTest(&obj1, &obj2, 0.0, 2); // x-axis //} //TEST_F(COLLISION, PLANE_CAPSULE_X) { -// fcl::Plane obj1(1, 0, 0, 0); -// fcl::Capsule obj2(0.5, 2); +// dart::collision::fcl::Plane obj1(1, 0, 0, 0); +// dart::collision::fcl::Capsule obj2(0.5, 2); // unrotatedTest(&obj1, &obj2, 0.0, 0); // x-axis //} //TEST_F(COLLISION, PLANE_CAPSULE_Y) { -// fcl::Plane obj1(0, 1, 0, 0); -// fcl::Capsule obj2(0.5, 2); +// dart::collision::fcl::Plane obj1(0, 1, 0, 0); +// dart::collision::fcl::Capsule obj2(0.5, 2); // unrotatedTest(&obj1, &obj2, 0.0, 1); // x-axis //} //TEST_F(COLLISION, PLANE_CAPSULE_Z) { -// fcl::Plane obj1(0, 0, 1, 0); -// fcl::Capsule obj2(0.5, 2); +// dart::collision::fcl::Plane obj1(0, 0, 1, 0); +// dart::collision::fcl::Capsule obj2(0.5, 2); // unrotatedTest(&obj1, &obj2, 0.0, 2); // x-axis //} //TEST_F(COLLISION, PLANE_CONE_X) { -// fcl::Plane obj1(1, 0, 0, 0); -// fcl::Cone obj2(0.5, 1); +// dart::collision::fcl::Plane obj1(1, 0, 0, 0); +// dart::collision::fcl::Cone obj2(0.5, 1); // unrotatedTest(&obj1, &obj2, 0.0, 0); // x-axis //} //TEST_F(COLLISION, PLANE_CONE_Y) { -// fcl::Plane obj1(0, 1, 0, 0); -// fcl::Cone obj2(0.5, 1); +// dart::collision::fcl::Plane obj1(0, 1, 0, 0); +// dart::collision::fcl::Cone obj2(0.5, 1); // unrotatedTest(&obj1, &obj2, 0.0, 1); // x-axis //} //TEST_F(COLLISION, PLANE_CONE_Z) { -// fcl::Plane obj1(0, 0, 1, 0); -// fcl::Cone obj2(0.5, 1); +// dart::collision::fcl::Plane obj1(0, 0, 1, 0); +// dart::collision::fcl::Cone obj2(0.5, 1); // unrotatedTest(&obj1, &obj2, 0.0, 2); // x-axis //} @@ -392,11 +387,11 @@ void COLLISION::printResult(const fcl::CollisionResult& _result) TEST_F(COLLISION, DROP) { dtdbg << "Unrotated box\n"; - fcl::Box box1(0.5, 0.5, 0.5); + dart::collision::fcl::Box box1(0.5, 0.5, 0.5); dropWithRotation(&box1, 0, 0, 0); dtdbg << "Rotated box\n"; - fcl::Box box2(0.5, 0.5, 0.5); + dart::collision::fcl::Box box2(0.5, 0.5, 0.5); dropWithRotation(&box2, dart::math::random(-3.14, 3.14), dart::math::random(-3.14, 3.14), @@ -415,39 +410,39 @@ TEST_F(COLLISION, FCL_BOX_BOX) double EulerX = 3; // Collision test setting - fcl::CollisionResult result; - fcl::CollisionRequest request; + dart::collision::fcl::CollisionResult result; + dart::collision::fcl::CollisionRequest request; request.enable_contact = true; request.num_max_contacts = 100; // Ground like box setting - fcl::Box groundObject(100, 100, 0.1); - fcl::Transform3f groundTransf; + dart::collision::fcl::Box groundObject(100, 100, 0.1); + dart::collision::fcl::Transform3 groundTransf; groundTransf.setIdentity(); - fcl::Vec3f ground_position(0.0, 0.0, -0.05); - groundTransf.setTranslation(ground_position); + dart::collision::fcl::Vector3 ground_position(0.0, 0.0, -0.05); + dart::collision::fcl::setTranslation(groundTransf, ground_position); // Dropping box object setting - fcl::Box box(0.5, 0.5, 0.5); - fcl::Transform3f objectTransf; - fcl::Matrix3f rot; - rot.setEulerZYX(EulerZ, EulerY, EulerX); - objectTransf.setRotation(rot); - fcl::Vec3f dropping_position(0.0, 0.0, 5.0); - objectTransf.setTranslation(dropping_position); + dart::collision::fcl::Box box(0.5, 0.5, 0.5); + dart::collision::fcl::Transform3 objectTransf; + dart::collision::fcl::Matrix3 rot; + dart::collision::fcl::setEulerZYX(rot, EulerZ, EulerY, EulerX); + dart::collision::fcl::setRotation(objectTransf, rot); + dart::collision::fcl::Vector3 dropping_position(0.0, 0.0, 5.0); + dart::collision::fcl::setTranslation(objectTransf, dropping_position); // Let's drop the object until it collide with ground do { - objectTransf.setTranslation(dropping_position); + dart::collision::fcl::setTranslation(objectTransf, dropping_position); - fcl::collide(&box, objectTransf, &groundObject, groundTransf, request, result); + ::fcl::collide(&box, objectTransf, &groundObject, groundTransf, request, result); dropping_position[2] -= 0.00001; } while (result.numContacts() == 0); std::cout << "Current position of the object: " - << objectTransf.getTranslation() + << dart::collision::fcl::getTranslation(objectTransf) << std::endl << "Number of contacts: " << result.numContacts() @@ -491,7 +486,7 @@ TEST_F(COLLISION, FCL_BOX_BOX) // while (result.size() == 0); // std::cout //<< "Current position of the object: " -// //<< objectTransf.getTranslation() +// //<< dart::collision::fcl::getTranslation(objectTransf) // //<< std::endl // << "Number of contacts: " // << result.size() diff --git a/unittests/comprehensive/test_Distance.cpp b/unittests/comprehensive/test_Distance.cpp index fe0e79cac7fcd..48d06a13a146a 100644 --- a/unittests/comprehensive/test_Distance.cpp +++ b/unittests/comprehensive/test_Distance.cpp @@ -31,8 +31,8 @@ */ #include -#include #include "dart/dart.hpp" +#include "dart/collision/fcl/fcl.hpp" #if HAVE_BULLET #include "dart/collision/bullet/bullet.hpp" #endif diff --git a/unittests/comprehensive/test_Dynamics.cpp b/unittests/comprehensive/test_Dynamics.cpp index f6067c5a525c9..82d52a1db942b 100644 --- a/unittests/comprehensive/test_Dynamics.cpp +++ b/unittests/comprehensive/test_Dynamics.cpp @@ -833,7 +833,7 @@ void DynamicsTest::testFiniteDifferenceBodyNodeVelocity(const common::Uri& uri) skeleton->setVelocities(dq); skeleton->setAccelerations(ddq); - Eigen::aligned_map Tmap; + dart::aligned_map Tmap; for (auto k = 0u; k < numBodies; ++k) { auto body = skeleton->getBodyNode(k); @@ -1057,9 +1057,9 @@ void testForwardKinematicsSkeleton(const dynamics::SkeletonPtr& skel) Eigen::VectorXd dq; Eigen::VectorXd ddq; - Eigen::aligned_map Tmap; - Eigen::aligned_map Vmap; - Eigen::aligned_map dVmap; + dart::aligned_map Tmap; + dart::aligned_map Vmap; + dart::aligned_map dVmap; for (auto j = 0u; j < numBodies; ++j) { diff --git a/unittests/comprehensive/test_Frames.cpp b/unittests/comprehensive/test_Frames.cpp index 6585f7aee240e..039b695f4171f 100644 --- a/unittests/comprehensive/test_Frames.cpp +++ b/unittests/comprehensive/test_Frames.cpp @@ -60,7 +60,7 @@ void randomize_transform(Eigen::Isometry3d& tf, tf.rotate(Eigen::AngleAxisd(theta.norm(), theta.normalized())); } -void randomize_transforms(Eigen::aligned_vector& tfs) +void randomize_transforms(dart::aligned_vector& tfs) { for(std::size_t i=0; i tfs; + dart::aligned_vector tfs; tfs.resize(frames.size(), Eigen::Isometry3d::Identity()); randomize_transforms(tfs); @@ -189,8 +189,8 @@ TEST(FRAMES, FORWARD_KINEMATICS_CHAIN) // Basic forward spatial velocity propagation { // The brackets are to allow reusing variable names - Eigen::aligned_vector v_rels(frames.size()); - Eigen::aligned_vector v_total(frames.size()); + dart::aligned_vector v_rels(frames.size()); + dart::aligned_vector v_total(frames.size()); for(std::size_t i=0; i v_rels(frames.size()); - Eigen::aligned_vector a_rels(frames.size()); + dart::aligned_vector v_rels(frames.size()); + dart::aligned_vector a_rels(frames.size()); - Eigen::aligned_vector v_total(frames.size()); - Eigen::aligned_vector a_total(frames.size()); + dart::aligned_vector v_total(frames.size()); + dart::aligned_vector a_total(frames.size()); for(std::size_t i=0; i joints; std::vector bns; - Eigen::aligned_vector desired_tfs; - Eigen::aligned_vector actual_tfs; + dart::aligned_vector desired_tfs; + dart::aligned_vector actual_tfs; joints.push_back(freejoint); bns.push_back(freejoint_bn); diff --git a/unittests/unit/test_IkFast.cpp b/unittests/unit/test_IkFast.cpp index 20f39334b16df..b55ff7be3da21 100644 --- a/unittests/unit/test_IkFast.cpp +++ b/unittests/unit/test_IkFast.cpp @@ -32,7 +32,7 @@ #include #include -#include +#include #include "TestHelpers.hpp" using namespace dart; @@ -59,7 +59,7 @@ TEST(IkFast, FailedToLoadSharedLibrary) //============================================================================== TEST(IkFast, LoadWamArmIk) { - utils::DartLoader urdfParser; + io::DartLoader urdfParser; urdfParser.addPackageDirectory("herb_description", DART_DATA_PATH"/urdf/wam"); auto wam = urdfParser.parseSkeleton(DART_DATA_PATH"/urdf/wam/wam.urdf"); EXPECT_NE(wam, nullptr);