1 |
|
|
/* |
2 |
|
|
* Software License Agreement (BSD License) |
3 |
|
|
* |
4 |
|
|
* Copyright (c) 2011-2014, Willow Garage, Inc. |
5 |
|
|
* Copyright (c) 2014-2015, Open Source Robotics Foundation |
6 |
|
|
* All rights reserved. |
7 |
|
|
* |
8 |
|
|
* Redistribution and use in source and binary forms, with or without |
9 |
|
|
* modification, are permitted provided that the following conditions |
10 |
|
|
* are met: |
11 |
|
|
* |
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 |
|
|
* * Neither the name of Open Source Robotics Foundation nor the names of its |
19 |
|
|
* contributors may be used to endorse or promote products derived |
20 |
|
|
* from this software without specific prior written permission. |
21 |
|
|
* |
22 |
|
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
23 |
|
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
24 |
|
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
25 |
|
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
26 |
|
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
27 |
|
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
28 |
|
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; |
29 |
|
|
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER |
30 |
|
|
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
31 |
|
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
32 |
|
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
33 |
|
|
* POSSIBILITY OF SUCH DAMAGE. |
34 |
|
|
*/ |
35 |
|
|
|
36 |
|
|
/** \author Jia Pan */ |
37 |
|
|
|
38 |
|
|
#ifndef HPP_FCL_DATA_TYPES_H |
39 |
|
|
#define HPP_FCL_DATA_TYPES_H |
40 |
|
|
|
41 |
|
|
#include <Eigen/Core> |
42 |
|
|
#include <Eigen/Geometry> |
43 |
|
|
|
44 |
|
|
#include <hpp/fcl/config.hh> |
45 |
|
|
|
46 |
|
|
namespace hpp { |
47 |
|
|
|
48 |
|
|
#ifdef HPP_FCL_HAS_OCTOMAP |
49 |
|
|
#define OCTOMAP_VERSION_AT_LEAST(x, y, z) \ |
50 |
|
|
(OCTOMAP_MAJOR_VERSION > x || \ |
51 |
|
|
(OCTOMAP_MAJOR_VERSION >= x && \ |
52 |
|
|
(OCTOMAP_MINOR_VERSION > y || \ |
53 |
|
|
(OCTOMAP_MINOR_VERSION >= y && OCTOMAP_PATCH_VERSION >= z)))) |
54 |
|
|
|
55 |
|
|
#define OCTOMAP_VERSION_AT_MOST(x, y, z) \ |
56 |
|
|
(OCTOMAP_MAJOR_VERSION < x || \ |
57 |
|
|
(OCTOMAP_MAJOR_VERSION <= x && \ |
58 |
|
|
(OCTOMAP_MINOR_VERSION < y || \ |
59 |
|
|
(OCTOMAP_MINOR_VERSION <= y && OCTOMAP_PATCH_VERSION <= z)))) |
60 |
|
|
#endif // HPP_FCL_HAS_OCTOMAP |
61 |
|
|
} // namespace hpp |
62 |
|
|
|
63 |
|
|
namespace hpp { |
64 |
|
|
namespace fcl { |
65 |
|
|
typedef double FCL_REAL; |
66 |
|
|
typedef Eigen::Matrix<FCL_REAL, 3, 1> Vec3f; |
67 |
|
|
typedef Eigen::Matrix<FCL_REAL, Eigen::Dynamic, 1> VecXf; |
68 |
|
|
typedef Eigen::Matrix<FCL_REAL, 3, 3> Matrix3f; |
69 |
|
|
typedef Eigen::Matrix<FCL_REAL, Eigen::Dynamic, 3> Matrixx3f; |
70 |
|
|
typedef Eigen::Matrix<Eigen::DenseIndex, Eigen::Dynamic, 3> Matrixx3i; |
71 |
|
|
typedef Eigen::Matrix<FCL_REAL, Eigen::Dynamic, Eigen::Dynamic> MatrixXf; |
72 |
|
|
typedef Eigen::Vector2i support_func_guess_t; |
73 |
|
|
|
74 |
|
|
/// @brief Initial guess to use for the GJK algorithm |
75 |
|
|
/// DefaultGuess: Vec3f(1, 0, 0) |
76 |
|
|
/// CachedGuess: previous vector found by GJK or guess cached by the user |
77 |
|
|
/// BoundingVolumeGuess: guess using the centers of the shapes' AABB |
78 |
|
|
/// WARNING: to use BoundingVolumeGuess, computeLocalAABB must have been called |
79 |
|
|
/// on the two shapes. |
80 |
|
|
enum GJKInitialGuess { DefaultGuess, CachedGuess, BoundingVolumeGuess }; |
81 |
|
|
|
82 |
|
|
/// @brief Variant to use for the GJK algorithm |
83 |
|
|
enum GJKVariant { DefaultGJK, NesterovAcceleration }; |
84 |
|
|
|
85 |
|
|
/// @brief Which convergence criterion is used to stop the algorithm (when the |
86 |
|
|
/// shapes are not in collision). (default) VDB: Van den Bergen (A Fast and |
87 |
|
|
/// Robust GJK Implementation, 1999) DG: duality-gap, as used in the Frank-Wolfe |
88 |
|
|
/// and the vanilla 1988 GJK algorithms Hybrid: a mix between VDB and DG. |
89 |
|
|
enum GJKConvergenceCriterion { VDB, DualityGap, Hybrid }; |
90 |
|
|
|
91 |
|
|
/// @brief Wether the convergence criterion is scaled on the norm of the |
92 |
|
|
/// solution or not |
93 |
|
|
enum GJKConvergenceCriterionType { Relative, Absolute }; |
94 |
|
|
|
95 |
|
|
/// @brief Triangle with 3 indices for points |
96 |
|
|
class HPP_FCL_DLLAPI Triangle { |
97 |
|
|
public: |
98 |
|
|
typedef std::size_t index_type; |
99 |
|
|
typedef int size_type; |
100 |
|
|
|
101 |
|
|
/// @brief Default constructor |
102 |
|
2557427 |
Triangle() {} |
103 |
|
|
|
104 |
|
|
/// @brief Create a triangle with given vertex indices |
105 |
|
609978 |
Triangle(index_type p1, index_type p2, index_type p3) { set(p1, p2, p3); } |
106 |
|
|
|
107 |
|
|
/// @brief Set the vertex indices of the triangle |
108 |
|
2050555 |
inline void set(index_type p1, index_type p2, index_type p3) { |
109 |
|
2050555 |
vids[0] = p1; |
110 |
|
2050555 |
vids[1] = p2; |
111 |
|
2050555 |
vids[2] = p3; |
112 |
|
2050555 |
} |
113 |
|
|
|
114 |
|
|
/// @brief Access the triangle index |
115 |
|
337043767 |
inline index_type operator[](index_type i) const { return vids[i]; } |
116 |
|
|
|
117 |
|
12372979 |
inline index_type& operator[](index_type i) { return vids[i]; } |
118 |
|
|
|
119 |
|
2227285 |
static inline size_type size() { return 3; } |
120 |
|
|
|
121 |
|
19836 |
bool operator==(const Triangle& other) const { |
122 |
✓✗✓✗
|
39672 |
return vids[0] == other.vids[0] && vids[1] == other.vids[1] && |
123 |
✓✗ |
39672 |
vids[2] == other.vids[2]; |
124 |
|
|
} |
125 |
|
|
|
126 |
|
19836 |
bool operator!=(const Triangle& other) const { return !(*this == other); } |
127 |
|
|
|
128 |
|
|
private: |
129 |
|
|
/// @brief indices for each vertex of triangle |
130 |
|
|
index_type vids[3]; |
131 |
|
|
}; |
132 |
|
|
|
133 |
|
|
/// @brief Quadrilateral with 4 indices for points |
134 |
|
|
struct HPP_FCL_DLLAPI Quadrilateral { |
135 |
|
|
typedef std::size_t index_type; |
136 |
|
|
typedef int size_type; |
137 |
|
|
|
138 |
|
12 |
Quadrilateral() {} |
139 |
|
|
|
140 |
|
|
Quadrilateral(index_type p0, index_type p1, index_type p2, index_type p3) { |
141 |
|
|
set(p0, p1, p2, p3); |
142 |
|
|
} |
143 |
|
|
|
144 |
|
|
/// @brief Set the vertex indices of the quadrilateral |
145 |
|
12 |
inline void set(index_type p0, index_type p1, index_type p2, index_type p3) { |
146 |
|
12 |
vids[0] = p0; |
147 |
|
12 |
vids[1] = p1; |
148 |
|
12 |
vids[2] = p2; |
149 |
|
12 |
vids[3] = p3; |
150 |
|
12 |
} |
151 |
|
|
|
152 |
|
|
/// @access the quadrilateral index |
153 |
|
144 |
inline index_type operator[](index_type i) const { return vids[i]; } |
154 |
|
|
|
155 |
|
|
inline index_type& operator[](index_type i) { return vids[i]; } |
156 |
|
|
|
157 |
|
72 |
static inline size_type size() { return 4; } |
158 |
|
|
|
159 |
|
|
bool operator==(const Quadrilateral& other) const { |
160 |
|
|
return vids[0] == other.vids[0] && vids[1] == other.vids[1] && |
161 |
|
|
vids[2] == other.vids[2] && vids[3] == other.vids[3]; |
162 |
|
|
} |
163 |
|
|
|
164 |
|
|
bool operator!=(const Quadrilateral& other) const { |
165 |
|
|
return !(*this == other); |
166 |
|
|
} |
167 |
|
|
|
168 |
|
|
private: |
169 |
|
|
index_type vids[4]; |
170 |
|
|
}; |
171 |
|
|
|
172 |
|
|
} // namespace fcl |
173 |
|
|
|
174 |
|
|
} // namespace hpp |
175 |
|
|
|
176 |
|
|
#endif |