| Line | Branch | Exec | Source |
|---|---|---|---|
| 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 | * Copyright (c) 2023, Inria | ||
| 7 | * All rights reserved. | ||
| 8 | * | ||
| 9 | * Redistribution and use in source and binary forms, with or without | ||
| 10 | * modification, are permitted provided that the following conditions | ||
| 11 | * are met: | ||
| 12 | * | ||
| 13 | * * Redistributions of source code must retain the above copyright | ||
| 14 | * notice, this list of conditions and the following disclaimer. | ||
| 15 | * * Redistributions in binary form must reproduce the above | ||
| 16 | * copyright notice, this list of conditions and the following | ||
| 17 | * disclaimer in the documentation and/or other materials provided | ||
| 18 | * with the distribution. | ||
| 19 | * * Neither the name of Open Source Robotics Foundation nor the names of its | ||
| 20 | * contributors may be used to endorse or promote products derived | ||
| 21 | * from this software without specific prior written permission. | ||
| 22 | * | ||
| 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | ||
| 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT | ||
| 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS | ||
| 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE | ||
| 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, | ||
| 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, | ||
| 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; | ||
| 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER | ||
| 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT | ||
| 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN | ||
| 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE | ||
| 34 | * POSSIBILITY OF SUCH DAMAGE. | ||
| 35 | */ | ||
| 36 | |||
| 37 | /** \author Jia Pan */ | ||
| 38 | |||
| 39 | #ifndef COAL_DATA_TYPES_H | ||
| 40 | #define COAL_DATA_TYPES_H | ||
| 41 | |||
| 42 | #include <Eigen/Core> | ||
| 43 | #include <Eigen/Geometry> | ||
| 44 | |||
| 45 | #include "coal/config.hh" | ||
| 46 | #include "coal/deprecated.hh" | ||
| 47 | |||
| 48 | #ifdef COAL_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 // COAL_HAS_OCTOMAP | ||
| 61 | |||
| 62 | namespace coal { | ||
| 63 | #ifdef COAL_USE_FLOAT_PRECISION | ||
| 64 | COAL_DEPRECATED typedef float CoalScalar; | ||
| 65 | typedef float Scalar; | ||
| 66 | #else | ||
| 67 | COAL_DEPRECATED typedef double CoalScalar; | ||
| 68 | typedef double Scalar; | ||
| 69 | #endif | ||
| 70 | typedef Eigen::Matrix<Scalar, 3, 1> Vec3s; | ||
| 71 | typedef Eigen::Matrix<Scalar, 2, 1> Vec2s; | ||
| 72 | typedef Eigen::Matrix<Scalar, 6, 1> Vec6s; | ||
| 73 | typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> VecXs; | ||
| 74 | typedef Eigen::Matrix<Scalar, 3, 3> Matrix3s; | ||
| 75 | typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 3, Eigen::RowMajor> MatrixX3s; | ||
| 76 | typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 2, Eigen::RowMajor> MatrixX2s; | ||
| 77 | typedef Eigen::Matrix<Eigen::DenseIndex, Eigen::Dynamic, 3, Eigen::RowMajor> | ||
| 78 | Matrixx3i; | ||
| 79 | typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixXs; | ||
| 80 | typedef Eigen::Vector2i support_func_guess_t; | ||
| 81 | |||
| 82 | typedef double SolverScalar; | ||
| 83 | typedef Eigen::Matrix<SolverScalar, 3, 1> Vec3ps; | ||
| 84 | |||
| 85 | #ifdef COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL | ||
| 86 | // We keep the FCL_REAL typedef and the Vec[..]f typedefs for backward | ||
| 87 | // compatibility. | ||
| 88 | typedef Scalar FCL_REAL; | ||
| 89 | typedef Vec3s Vec3f; | ||
| 90 | typedef Vec2s Vec2f; | ||
| 91 | typedef Vec6s Vec6f; | ||
| 92 | typedef VecXs VecXf; | ||
| 93 | typedef Matrix3s Matrix3f; | ||
| 94 | typedef MatrixX3s Matrixx3f; | ||
| 95 | typedef MatrixX2s Matrixx2f; | ||
| 96 | typedef MatrixXs MatrixXf; | ||
| 97 | #endif | ||
| 98 | |||
| 99 | /// @brief Initial guess to use for the GJK algorithm | ||
| 100 | /// DefaultGuess: Vec3s(1, 0, 0) | ||
| 101 | /// CachedGuess: previous vector found by GJK or guess cached by the user | ||
| 102 | /// BoundingVolumeGuess: guess using the centers of the shapes' AABB | ||
| 103 | /// WARNING: to use BoundingVolumeGuess, computeLocalAABB must have been called | ||
| 104 | /// on the two shapes. | ||
| 105 | enum GJKInitialGuess { DefaultGuess, CachedGuess, BoundingVolumeGuess }; | ||
| 106 | |||
| 107 | /// @brief Variant to use for the GJK algorithm | ||
| 108 | enum GJKVariant { DefaultGJK, PolyakAcceleration, NesterovAcceleration }; | ||
| 109 | |||
| 110 | /// @brief Which convergence criterion is used to stop the algorithm (when the | ||
| 111 | /// shapes are not in collision). (default) VDB: Van den Bergen (A Fast and | ||
| 112 | /// Robust GJK Implementation, 1999) DG: duality-gap, as used in the Frank-Wolfe | ||
| 113 | /// and the vanilla 1988 GJK algorithms Hybrid: a mix between VDB and DG. | ||
| 114 | enum GJKConvergenceCriterion { Default, DualityGap, Hybrid }; | ||
| 115 | |||
| 116 | /// @brief Wether the convergence criterion is scaled on the norm of the | ||
| 117 | /// solution or not | ||
| 118 | enum GJKConvergenceCriterionType { Relative, Absolute }; | ||
| 119 | |||
| 120 | /// @brief Triangle with 3 indices for points | ||
| 121 | template <typename _IndexType> | ||
| 122 | class TriangleTpl { | ||
| 123 | public: | ||
| 124 | // clang-format off | ||
| 125 | COAL_DEPRECATED_MESSAGE(Use IndexType instead.) typedef _IndexType index_type; | ||
| 126 | // clang-format on | ||
| 127 | typedef _IndexType IndexType; | ||
| 128 | typedef int size_type; | ||
| 129 | |||
| 130 | template <typename OtherIndexType> | ||
| 131 | friend class TriangleTpl; | ||
| 132 | |||
| 133 | /// @brief Default constructor | ||
| 134 | 2643374 | TriangleTpl() {} | |
| 135 | |||
| 136 | /// @brief Copy constructor | ||
| 137 | 5893795 | TriangleTpl(const TriangleTpl& other) { *this = other; } | |
| 138 | |||
| 139 | /// @brief Create a triangle with given vertex indices | ||
| 140 | 613888 | TriangleTpl(IndexType p1, IndexType p2, IndexType p3) { set(p1, p2, p3); } | |
| 141 | |||
| 142 | /// @brief Copy constructor from another vertex index type. | ||
| 143 | template <typename OtherIndexType> | ||
| 144 | TriangleTpl(const TriangleTpl<OtherIndexType>& other) { | ||
| 145 | *this = other; | ||
| 146 | } | ||
| 147 | |||
| 148 | /// @brief Copy operator | ||
| 149 | 6992874 | TriangleTpl& operator=(const TriangleTpl& other) { | |
| 150 | 6992874 | this->set(other.vids[0], other.vids[1], other.vids[2]); | |
| 151 | 6992874 | return *this; | |
| 152 | } | ||
| 153 | |||
| 154 | /// @brief Copy operator from another index type. | ||
| 155 | template <typename OtherIndexType> | ||
| 156 | TriangleTpl& operator=(const TriangleTpl<OtherIndexType>& other) { | ||
| 157 | *this = other.template cast<OtherIndexType>(); | ||
| 158 | return *this; | ||
| 159 | } | ||
| 160 | |||
| 161 | template <typename OtherIndexType> | ||
| 162 | 12 | TriangleTpl<OtherIndexType> cast() const { | |
| 163 | 12 | TriangleTpl<OtherIndexType> res; | |
| 164 | 12 | res.set(OtherIndexType(this->vids[0]), OtherIndexType(this->vids[1]), | |
| 165 | 12 | OtherIndexType(this->vids[2])); | |
| 166 | 12 | return res; | |
| 167 | } | ||
| 168 | |||
| 169 | /// @brief Set the vertex indices of the triangle | ||
| 170 | 9098849 | inline void set(IndexType p1, IndexType p2, IndexType p3) { | |
| 171 | 9098849 | vids[0] = p1; | |
| 172 | 9098849 | vids[1] = p2; | |
| 173 | 9098849 | vids[2] = p3; | |
| 174 | 9098849 | } | |
| 175 | |||
| 176 | /// @brief Access the triangle index | ||
| 177 | 338997532 | inline IndexType operator[](IndexType i) const { return vids[i]; } | |
| 178 | |||
| 179 | 12496582 | inline IndexType& operator[](IndexType i) { return vids[i]; } | |
| 180 | |||
| 181 | 2458970 | static inline size_type size() { return 3; } | |
| 182 | |||
| 183 | 44032 | bool operator==(const TriangleTpl& other) const { | |
| 184 |
2/4✓ Branch 0 taken 44032 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 44032 times.
✗ Branch 3 not taken.
|
88064 | return vids[0] == other.vids[0] && vids[1] == other.vids[1] && |
| 185 |
1/2✓ Branch 0 taken 44032 times.
✗ Branch 1 not taken.
|
88064 | vids[2] == other.vids[2]; |
| 186 | } | ||
| 187 | |||
| 188 | 41636 | bool operator!=(const TriangleTpl& other) const { return !(*this == other); } | |
| 189 | |||
| 190 | 87 | bool isValid() const { | |
| 191 |
1/2✓ Branch 1 taken 87 times.
✗ Branch 2 not taken.
|
174 | return vids[0] != (std::numeric_limits<IndexType>::max)() && |
| 192 |
2/4✓ Branch 0 taken 87 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 87 times.
✗ Branch 4 not taken.
|
174 | vids[1] != (std::numeric_limits<IndexType>::max)() && |
| 193 | 174 | vids[2] != (std::numeric_limits<IndexType>::max)(); | |
| 194 | } | ||
| 195 | |||
| 196 | protected: | ||
| 197 | /// @brief indices for each vertex of triangle | ||
| 198 | IndexType vids[3] = {(std::numeric_limits<IndexType>::max)(), | ||
| 199 | (std::numeric_limits<IndexType>::max)(), | ||
| 200 | (std::numeric_limits<IndexType>::max)()}; | ||
| 201 | }; | ||
| 202 | |||
| 203 | typedef TriangleTpl<std::uint16_t> Triangle16; | ||
| 204 | // | ||
| 205 | typedef TriangleTpl<std::uint32_t> Triangle32; | ||
| 206 | // | ||
| 207 | COAL_DEPRECATED_MESSAGE(Use Triangle32 instead.) | ||
| 208 | typedef Triangle32 Triangle; | ||
| 209 | |||
| 210 | /// @brief Quadrilateral with 4 indices for points | ||
| 211 | template <typename _IndexType> | ||
| 212 | struct QuadrilateralTpl { | ||
| 213 | // clang-format off | ||
| 214 | COAL_DEPRECATED_MESSAGE(Use IndexType instead.) typedef _IndexType index_type; | ||
| 215 | // clang-format on | ||
| 216 | typedef _IndexType IndexType; | ||
| 217 | typedef int size_type; | ||
| 218 | |||
| 219 | /// @brief Default constructor | ||
| 220 | 60 | QuadrilateralTpl() {} | |
| 221 | |||
| 222 | /// @brief Copy constructor | ||
| 223 | QuadrilateralTpl(const QuadrilateralTpl& other) { *this = other; } | ||
| 224 | |||
| 225 | /// @brief Copy constructor from another vertex index type. | ||
| 226 | template <typename OtherIndexType> | ||
| 227 | QuadrilateralTpl(const QuadrilateralTpl<OtherIndexType>& other) { | ||
| 228 | *this = other; | ||
| 229 | } | ||
| 230 | |||
| 231 | /// @brief Copy operator | ||
| 232 | 12 | QuadrilateralTpl& operator=(const QuadrilateralTpl& other) { | |
| 233 | 12 | this->set(other.vids[0], other.vids[1], other.vids[2], other.vids[3]); | |
| 234 | 12 | return *this; | |
| 235 | } | ||
| 236 | |||
| 237 | /// @brief Copy operator from another index type. | ||
| 238 | template <typename OtherIndexType> | ||
| 239 | QuadrilateralTpl& operator=(const QuadrilateralTpl<OtherIndexType>& other) { | ||
| 240 | *this = other.template cast<OtherIndexType>(); | ||
| 241 | return *this; | ||
| 242 | } | ||
| 243 | |||
| 244 | template <typename OtherIndexType> | ||
| 245 | 12 | QuadrilateralTpl<OtherIndexType> cast() const { | |
| 246 | 12 | QuadrilateralTpl<OtherIndexType> res; | |
| 247 | 12 | res.set(OtherIndexType(this->vids[0]), OtherIndexType(this->vids[1]), | |
| 248 | 12 | OtherIndexType(this->vids[2]), OtherIndexType(this->vids[3])); | |
| 249 | 12 | return res; | |
| 250 | } | ||
| 251 | |||
| 252 | QuadrilateralTpl(IndexType p0, IndexType p1, IndexType p2, IndexType p3) { | ||
| 253 | set(p0, p1, p2, p3); | ||
| 254 | } | ||
| 255 | |||
| 256 | /// @brief Set the vertex indices of the quadrilateral | ||
| 257 | 60 | inline void set(IndexType p0, IndexType p1, IndexType p2, IndexType p3) { | |
| 258 | 60 | vids[0] = p0; | |
| 259 | 60 | vids[1] = p1; | |
| 260 | 60 | vids[2] = p2; | |
| 261 | 60 | vids[3] = p3; | |
| 262 | 60 | } | |
| 263 | |||
| 264 | /// @access the quadrilateral index | ||
| 265 | 432 | inline IndexType operator[](IndexType i) const { return vids[i]; } | |
| 266 | |||
| 267 | ✗ | inline IndexType& operator[](IndexType i) { return vids[i]; } | |
| 268 | |||
| 269 | 216 | static inline size_type size() { return 4; } | |
| 270 | |||
| 271 | bool operator==(const QuadrilateralTpl& other) const { | ||
| 272 | return vids[0] == other.vids[0] && vids[1] == other.vids[1] && | ||
| 273 | vids[2] == other.vids[2] && vids[3] == other.vids[3]; | ||
| 274 | } | ||
| 275 | |||
| 276 | bool operator!=(const QuadrilateralTpl& other) const { | ||
| 277 | return !(*this == other); | ||
| 278 | } | ||
| 279 | |||
| 280 | protected: | ||
| 281 | IndexType vids[4]; | ||
| 282 | }; | ||
| 283 | |||
| 284 | typedef QuadrilateralTpl<std::uint16_t> Quadrilateral16; | ||
| 285 | // | ||
| 286 | typedef QuadrilateralTpl<std::uint32_t> Quadrilateral32; | ||
| 287 | // | ||
| 288 | COAL_DEPRECATED_MESSAGE(Use Quadrilateral32 instead.) | ||
| 289 | typedef Quadrilateral32 Quadrilateral; | ||
| 290 | |||
| 291 | } // namespace coal | ||
| 292 | |||
| 293 | #endif | ||
| 294 |