1 /*
2 * Copyright (c) 2011-2021, The DART development contributors
3 * All rights reserved.
4 *
5 * The list of contributors can be found at:
6 * https://github.com/dartsim/dart/blob/master/LICENSE
7 *
8 * This file is provided under the following "BSD-style" License:
9 * Redistribution and use in source and binary forms, with or
10 * without modification, are permitted provided that the following
11 * conditions are met:
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
19 * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
20 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
23 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
24 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
25 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
26 * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30 * POSSIBILITY OF SUCH DAMAGE.
31 */
32
33 #ifndef DART_COLLISION_FCL_BACKWARDCOMPATIBILITY_HPP_
34 #define DART_COLLISION_FCL_BACKWARDCOMPATIBILITY_HPP_
35
36 #include "dart/config.hpp"
37
38 #include <Eigen/Dense>
39
40 // clang-format off
41 #define FCL_VERSION_AT_LEAST(x,y,z) \
42 (FCL_MAJOR_VERSION > x || (FCL_MAJOR_VERSION >= x && \
43 (FCL_MINOR_VERSION > y || (FCL_MINOR_VERSION >= y && \
44 FCL_PATCH_VERSION >= z))))
45
46 #define FCL_MAJOR_MINOR_VERSION_AT_MOST(x,y) \
47 (FCL_MAJOR_VERSION < x || (FCL_MAJOR_VERSION <= x && \
48 (FCL_MINOR_VERSION < y || (FCL_MINOR_VERSION <= y))))
49 // clang-format on
50
51 #include <fcl/config.h>
52
53 #if FCL_VERSION_AT_LEAST(0, 6, 0)
54
55 # include <fcl/math/geometry.h>
56
57 # include <fcl/geometry/bvh/BVH_model.h>
58 # include <fcl/geometry/geometric_shape_to_BVH_model.h>
59 # include <fcl/math/bv/OBBRSS.h>
60 # include <fcl/math/bv/utility.h>
61 # include <fcl/narrowphase/collision.h>
62 # include <fcl/narrowphase/collision_object.h>
63 # include <fcl/narrowphase/distance.h>
64
65 #else
66
67 # include <fcl/math/matrix_3f.h>
68 # include <fcl/math/transform.h>
69 # include <fcl/math/vec_3f.h>
70
71 # include <fcl/BV/OBBRSS.h>
72 # include <fcl/BVH/BVH_model.h>
73 # include <fcl/shape/geometric_shape_to_BVH_model.h>
74 # include <fcl/collision.h>
75 # include <fcl/collision_data.h>
76 # include <fcl/collision_object.h>
77 # include <fcl/distance.h>
78
79 #endif // FCL_VERSION_AT_LEAST(0,6,0)
80
81 #include <fcl/broadphase/broadphase_dynamic_AABB_tree.h>
82
83 #if HAVE_OCTOMAP && FCL_HAVE_OCTOMAP
84 # if FCL_VERSION_AT_LEAST(0, 6, 0)
85 # include <fcl/geometry/octree/octree.h>
86 # else
87 # include <fcl/octree.h>
88 # endif // FCL_VERSION_AT_LEAST(0,6,0)
89 #endif // HAVE_OCTOMAP && FCL_HAVE_OCTOMAP
90
91 #if FCL_VERSION_AT_LEAST(0, 5, 0)
92 # include <memory>
93 template <class T>
94 using fcl_shared_ptr = std::shared_ptr<T>;
95 template <class T>
96 using fcl_weak_ptr = std::weak_ptr<T>;
97 template <class T, class... Args>
fcl_make_shared(Args &&...args)98 fcl_shared_ptr<T> fcl_make_shared(Args&&... args)
99 {
100 return std::make_shared<T>(std::forward<Args>(args)...);
101 }
102 #else
103 # include <boost/make_shared.hpp>
104 # include <boost/weak_ptr.hpp>
105 template <class T>
106 using fcl_shared_ptr = boost::shared_ptr<T>;
107 template <class T>
108 using fcl_weak_ptr = boost::weak_ptr<T>;
109 template <class T, class... Args>
fcl_make_shared(Args &&...args)110 fcl_shared_ptr<T> fcl_make_shared(Args&&... args)
111 {
112 return boost::make_shared<T>(std::forward<Args>(args)...);
113 }
114 #endif // FCL_VERSION_AT_LEAST(0,5,0)
115
116 namespace dart {
117 namespace collision {
118 namespace fcl {
119
120 #if FCL_VERSION_AT_LEAST(0, 6, 0)
121 // Geometric fundamentals
122 using Vector3 = ::fcl::Vector3<double>;
123 using Matrix3 = ::fcl::Matrix3<double>;
124 using Transform3 = ::fcl::Transform3<double>;
125 // Geometric primitives
126 using Box = ::fcl::Box<double>;
127 using Cylinder = ::fcl::Cylinder<double>;
128 using Cone = ::fcl::Cone<double>;
129 using Ellipsoid = ::fcl::Ellipsoid<double>;
130 using Halfspace = ::fcl::Halfspace<double>;
131 using Sphere = ::fcl::Sphere<double>;
132 # if HAVE_OCTOMAP && FCL_HAVE_OCTOMAP
133 using OcTree = ::fcl::OcTree<double>;
134 # endif // HAVE_OCTOMAP && FCL_HAVE_OCTOMAP
135 // Collision objects
136 using CollisionObject = ::fcl::CollisionObject<double>;
137 using CollisionGeometry = ::fcl::CollisionGeometry<double>;
138 using DynamicAABBTreeCollisionManager
139 = ::fcl::DynamicAABBTreeCollisionManager<double>;
140 using OBBRSS = ::fcl::OBBRSS<double>;
141 using CollisionRequest = ::fcl::CollisionRequest<double>;
142 using CollisionResult = ::fcl::CollisionResult<double>;
143 using DistanceRequest = ::fcl::DistanceRequest<double>;
144 using DistanceResult = ::fcl::DistanceResult<double>;
145 using Contact = ::fcl::Contact<double>;
146 #else
147 // Geometric fundamentals
148 using Vector3 = ::fcl::Vec3f;
149 using Matrix3 = ::fcl::Matrix3f;
150 using Transform3 = ::fcl::Transform3f;
151 // Geometric primitives
152 using Box = ::fcl::Box;
153 using Cylinder = ::fcl::Cylinder;
154 using Cone = ::fcl::Cone;
155 using Halfspace = ::fcl::Halfspace;
156 using Sphere = ::fcl::Sphere;
157 # if HAVE_OCTOMAP && FCL_HAVE_OCTOMAP
158 using OcTree = ::fcl::OcTree;
159 # endif // HAVE_OCTOMAP && FCL_HAVE_OCTOMAP
160 // Collision objects
161 using CollisionObject = ::fcl::CollisionObject;
162 using CollisionGeometry = ::fcl::CollisionGeometry;
163 using DynamicAABBTreeCollisionManager = ::fcl::DynamicAABBTreeCollisionManager;
164 using OBBRSS = ::fcl::OBBRSS;
165 using CollisionRequest = ::fcl::CollisionRequest;
166 using CollisionResult = ::fcl::CollisionResult;
167 using DistanceRequest = ::fcl::DistanceRequest;
168 using DistanceResult = ::fcl::DistanceResult;
169 using Contact = ::fcl::Contact;
170 #endif
171
172 #if FCL_VERSION_AT_LEAST(0, 4, 0) && !FCL_VERSION_AT_LEAST(0, 6, 0)
173 using Ellipsoid = ::fcl::Ellipsoid;
174 #endif
175
176 /// Returns norm of a 3-dim vector
177 double length(const dart::collision::fcl::Vector3& t);
178
179 /// Returns squared norm of a 3-dim vector
180 double length2(const dart::collision::fcl::Vector3& t);
181
182 /// Returns translation component of a transform
183 dart::collision::fcl::Vector3 getTranslation(
184 const dart::collision::fcl::Transform3& T);
185
186 /// Sets translation component of a transform
187 void setTranslation(
188 dart::collision::fcl::Transform3& T,
189 const dart::collision::fcl::Vector3& t);
190
191 /// Returns rotation component of a transform
192 dart::collision::fcl::Matrix3 getRotation(
193 const dart::collision::fcl::Transform3& T);
194
195 /// Sets rotation component of a transform
196 void setRotation(
197 dart::collision::fcl::Transform3& T,
198 const dart::collision::fcl::Matrix3& R);
199
200 /// Sets a rotation matrix given Euler-XYZ angles
201 void setEulerZYX(
202 dart::collision::fcl::Matrix3& rot,
203 double eulerX,
204 double eulerY,
205 double eulerZ);
206
207 /// Transforms a 3-dim vector by a transform and returns the result
208 dart::collision::fcl::Vector3 transform(
209 const dart::collision::fcl::Transform3& t,
210 const dart::collision::fcl::Vector3& v);
211
212 } // namespace fcl
213 } // namespace collision
214 } // namespace dart
215
216 #endif // DART_COLLISION_FCL_BACKWARDCOMPATIBILITY_HPP_
217