| Directory: | ./ |
|---|---|
| File: | include/coal/BV/kDOP.h |
| Date: | 2025-04-13 14:25:19 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 7 | 14 | 50.0% |
| Branches: | 5 | 14 | 35.7% |
| 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 | * 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 COAL_KDOP_H | ||
| 39 | #define COAL_KDOP_H | ||
| 40 | |||
| 41 | #include "coal/fwd.hh" | ||
| 42 | #include "coal/data_types.h" | ||
| 43 | |||
| 44 | namespace coal { | ||
| 45 | |||
| 46 | struct CollisionRequest; | ||
| 47 | |||
| 48 | /// @addtogroup Bounding_Volume | ||
| 49 | /// @{ | ||
| 50 | |||
| 51 | /// @brief KDOP class describes the KDOP collision structures. K is set as the | ||
| 52 | /// template parameter, which should be 16, 18, or 24 | ||
| 53 | /// The KDOP structure is defined by some pairs of parallel planes defined by | ||
| 54 | /// some axes. | ||
| 55 | /// For K = 16, the planes are 6 AABB planes and 10 diagonal planes that cut off | ||
| 56 | /// some space of the edges: | ||
| 57 | /// (-1,0,0) and (1,0,0) -> indices 0 and 8 | ||
| 58 | /// (0,-1,0) and (0,1,0) -> indices 1 and 9 | ||
| 59 | /// (0,0,-1) and (0,0,1) -> indices 2 and 10 | ||
| 60 | /// (-1,-1,0) and (1,1,0) -> indices 3 and 11 | ||
| 61 | /// (-1,0,-1) and (1,0,1) -> indices 4 and 12 | ||
| 62 | /// (0,-1,-1) and (0,1,1) -> indices 5 and 13 | ||
| 63 | /// (-1,1,0) and (1,-1,0) -> indices 6 and 14 | ||
| 64 | /// (-1,0,1) and (1,0,-1) -> indices 7 and 15 | ||
| 65 | /// For K = 18, the planes are 6 AABB planes and 12 diagonal planes that cut off | ||
| 66 | /// some space of the edges: | ||
| 67 | /// (-1,0,0) and (1,0,0) -> indices 0 and 9 | ||
| 68 | /// (0,-1,0) and (0,1,0) -> indices 1 and 10 | ||
| 69 | /// (0,0,-1) and (0,0,1) -> indices 2 and 11 | ||
| 70 | /// (-1,-1,0) and (1,1,0) -> indices 3 and 12 | ||
| 71 | /// (-1,0,-1) and (1,0,1) -> indices 4 and 13 | ||
| 72 | /// (0,-1,-1) and (0,1,1) -> indices 5 and 14 | ||
| 73 | /// (-1,1,0) and (1,-1,0) -> indices 6 and 15 | ||
| 74 | /// (-1,0,1) and (1,0,-1) -> indices 7 and 16 | ||
| 75 | /// (0,-1,1) and (0,1,-1) -> indices 8 and 17 | ||
| 76 | /// For K = 18, the planes are 6 AABB planes and 18 diagonal planes that cut off | ||
| 77 | /// some space of the edges: | ||
| 78 | /// (-1,0,0) and (1,0,0) -> indices 0 and 12 | ||
| 79 | /// (0,-1,0) and (0,1,0) -> indices 1 and 13 | ||
| 80 | /// (0,0,-1) and (0,0,1) -> indices 2 and 14 | ||
| 81 | /// (-1,-1,0) and (1,1,0) -> indices 3 and 15 | ||
| 82 | /// (-1,0,-1) and (1,0,1) -> indices 4 and 16 | ||
| 83 | /// (0,-1,-1) and (0,1,1) -> indices 5 and 17 | ||
| 84 | /// (-1,1,0) and (1,-1,0) -> indices 6 and 18 | ||
| 85 | /// (-1,0,1) and (1,0,-1) -> indices 7 and 19 | ||
| 86 | /// (0,-1,1) and (0,1,-1) -> indices 8 and 20 | ||
| 87 | /// (-1, -1, 1) and (1, 1, -1) --> indices 9 and 21 | ||
| 88 | /// (-1, 1, -1) and (1, -1, 1) --> indices 10 and 22 | ||
| 89 | /// (1, -1, -1) and (-1, 1, 1) --> indices 11 and 23 | ||
| 90 | template <short N> | ||
| 91 | class COAL_DLLAPI KDOP { | ||
| 92 | protected: | ||
| 93 | /// @brief Origin's distances to N KDOP planes | ||
| 94 | Eigen::Array<Scalar, N, 1> dist_; | ||
| 95 | |||
| 96 | public: | ||
| 97 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 98 | |||
| 99 | /// @brief Creating kDOP containing nothing | ||
| 100 | KDOP(); | ||
| 101 | |||
| 102 | /// @brief Creating kDOP containing only one point | ||
| 103 | KDOP(const Vec3s& v); | ||
| 104 | |||
| 105 | /// @brief Creating kDOP containing two points | ||
| 106 | KDOP(const Vec3s& a, const Vec3s& b); | ||
| 107 | |||
| 108 | /// @brief Equality operator | ||
| 109 | ✗ | bool operator==(const KDOP& other) const { | |
| 110 | ✗ | return (dist_ == other.dist_).all(); | |
| 111 | } | ||
| 112 | |||
| 113 | /// @brief Difference operator | ||
| 114 | ✗ | bool operator!=(const KDOP& other) const { | |
| 115 | ✗ | return (dist_ != other.dist_).any(); | |
| 116 | } | ||
| 117 | |||
| 118 | /// @brief Check whether two KDOPs overlap. | ||
| 119 | bool overlap(const KDOP<N>& other) const; | ||
| 120 | |||
| 121 | /// @brief Check whether two KDOPs overlap. | ||
| 122 | /// @return true if collision happens. | ||
| 123 | /// @retval sqrDistLowerBound squared lower bound on distance between boxes if | ||
| 124 | /// they do not overlap. | ||
| 125 | bool overlap(const KDOP<N>& other, const CollisionRequest& request, | ||
| 126 | Scalar& sqrDistLowerBound) const; | ||
| 127 | |||
| 128 | /// @brief The distance between two KDOP<N>. Not implemented. | ||
| 129 | Scalar distance(const KDOP<N>& other, Vec3s* P = NULL, Vec3s* Q = NULL) const; | ||
| 130 | |||
| 131 | /// @brief Merge the point and the KDOP | ||
| 132 | KDOP<N>& operator+=(const Vec3s& p); | ||
| 133 | |||
| 134 | /// @brief Merge two KDOPs | ||
| 135 | KDOP<N>& operator+=(const KDOP<N>& other); | ||
| 136 | |||
| 137 | /// @brief Create a KDOP by mergin two KDOPs | ||
| 138 | KDOP<N> operator+(const KDOP<N>& other) const; | ||
| 139 | |||
| 140 | /// @brief Size of the kDOP (used in BV_Splitter to order two kDOPs) | ||
| 141 | 82809620 | inline Scalar size() const { | |
| 142 | 82809620 | return width() * width() + height() * height() + depth() * depth(); | |
| 143 | } | ||
| 144 | |||
| 145 | /// @brief The (AABB) center | ||
| 146 | 253488 | inline Vec3s center() const { | |
| 147 |
5/10✓ Branch 1 taken 126744 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 126744 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 126744 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 126744 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 126744 times.
✗ Branch 14 not taken.
|
253488 | return (dist_.template head<3>() + dist_.template segment<3>(N / 2)) / 2; |
| 148 | } | ||
| 149 | |||
| 150 | /// @brief The (AABB) width | ||
| 151 | 167461054 | inline Scalar width() const { return dist_[N / 2] - dist_[0]; } | |
| 152 | |||
| 153 | /// @brief The (AABB) height | ||
| 154 | 167437564 | inline Scalar height() const { return dist_[N / 2 + 1] - dist_[1]; } | |
| 155 | |||
| 156 | /// @brief The (AABB) depth | ||
| 157 | 166441546 | inline Scalar depth() const { return dist_[N / 2 + 2] - dist_[2]; } | |
| 158 | |||
| 159 | /// @brief The (AABB) volume | ||
| 160 | ✗ | inline Scalar volume() const { return width() * height() * depth(); } | |
| 161 | |||
| 162 | ✗ | inline Scalar dist(short i) const { return dist_[i]; } | |
| 163 | |||
| 164 | ✗ | inline Scalar& dist(short i) { return dist_[i]; } | |
| 165 | |||
| 166 | //// @brief Check whether one point is inside the KDOP | ||
| 167 | bool inside(const Vec3s& p) const; | ||
| 168 | }; | ||
| 169 | |||
| 170 | template <short N> | ||
| 171 | bool overlap(const Matrix3s& /*R0*/, const Vec3s& /*T0*/, const KDOP<N>& /*b1*/, | ||
| 172 | const KDOP<N>& /*b2*/) { | ||
| 173 | COAL_THROW_PRETTY("not implemented", std::logic_error); | ||
| 174 | } | ||
| 175 | |||
| 176 | template <short N> | ||
| 177 | bool overlap(const Matrix3s& /*R0*/, const Vec3s& /*T0*/, const KDOP<N>& /*b1*/, | ||
| 178 | const KDOP<N>& /*b2*/, const CollisionRequest& /*request*/, | ||
| 179 | Scalar& /*sqrDistLowerBound*/) { | ||
| 180 | COAL_THROW_PRETTY("not implemented", std::logic_error); | ||
| 181 | } | ||
| 182 | |||
| 183 | /// @brief translate the KDOP BV | ||
| 184 | template <short N> | ||
| 185 | COAL_DLLAPI KDOP<N> translate(const KDOP<N>& bv, const Vec3s& t); | ||
| 186 | |||
| 187 | } // namespace coal | ||
| 188 | |||
| 189 | #endif | ||
| 190 |