| 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_TRAVERSAL_NODE_MESHES_H | ||
| 39 | #define COAL_TRAVERSAL_NODE_MESHES_H | ||
| 40 | |||
| 41 | /// @cond INTERNAL | ||
| 42 | |||
| 43 | #include "coal/collision_data.h" | ||
| 44 | #include "coal/internal/traversal_node_base.h" | ||
| 45 | #include "coal/BV/BV_node.h" | ||
| 46 | #include "coal/BV/BV.h" | ||
| 47 | #include "coal/BVH/BVH_model.h" | ||
| 48 | #include "coal/internal/intersect.h" | ||
| 49 | #include "coal/shape/geometric_shapes.h" | ||
| 50 | #include "coal/narrowphase/narrowphase.h" | ||
| 51 | #include "coal/internal/traversal.h" | ||
| 52 | #include "coal/internal/shape_shape_func.h" | ||
| 53 | |||
| 54 | #include <cassert> | ||
| 55 | |||
| 56 | namespace coal { | ||
| 57 | |||
| 58 | /// @addtogroup Traversal_For_Collision | ||
| 59 | /// @{ | ||
| 60 | |||
| 61 | /// @brief Traversal node for collision between BVH models | ||
| 62 | template <typename BV> | ||
| 63 | class BVHCollisionTraversalNode : public CollisionTraversalNodeBase { | ||
| 64 | public: | ||
| 65 | 21595 | BVHCollisionTraversalNode(const CollisionRequest& request) | |
| 66 | 21595 | : CollisionTraversalNodeBase(request) { | |
| 67 | 21595 | model1 = NULL; | |
| 68 | 21595 | model2 = NULL; | |
| 69 | |||
| 70 | 21595 | num_bv_tests = 0; | |
| 71 | 21595 | num_leaf_tests = 0; | |
| 72 | 21595 | query_time_seconds = 0.0; | |
| 73 | 21595 | } | |
| 74 | |||
| 75 | /// @brief Whether the BV node in the first BVH tree is leaf | ||
| 76 | 116998931 | bool isFirstNodeLeaf(unsigned int b) const { | |
| 77 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 58499498 times.
|
116998931 | assert(model1 != NULL && "model1 is NULL"); |
| 78 | 116998931 | return model1->getBV(b).isLeaf(); | |
| 79 | } | ||
| 80 | |||
| 81 | /// @brief Whether the BV node in the second BVH tree is leaf | ||
| 82 | 116998931 | bool isSecondNodeLeaf(unsigned int b) const { | |
| 83 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 58499498 times.
|
116998931 | assert(model2 != NULL && "model2 is NULL"); |
| 84 | 116998931 | return model2->getBV(b).isLeaf(); | |
| 85 | } | ||
| 86 | |||
| 87 | /// @brief Determine the traversal order, is the first BVTT subtree better | ||
| 88 | 41583777 | bool firstOverSecond(unsigned int b1, unsigned int b2) const { | |
| 89 | 41583777 | Scalar sz1 = model1->getBV(b1).bv.size(); | |
| 90 | 41583777 | Scalar sz2 = model2->getBV(b2).bv.size(); | |
| 91 | |||
| 92 | 41583777 | bool l1 = model1->getBV(b1).isLeaf(); | |
| 93 | 41583777 | bool l2 = model2->getBV(b2).isLeaf(); | |
| 94 | |||
| 95 |
6/6✓ Branch 0 taken 9579863 times.
✓ Branch 1 taken 11212045 times.
✓ Branch 2 taken 3851809 times.
✓ Branch 3 taken 5728054 times.
✓ Branch 4 taken 1543658 times.
✓ Branch 5 taken 2308151 times.
|
41583777 | if (l2 || (!l1 && (sz1 > sz2))) return true; |
| 96 | 16072382 | return false; | |
| 97 | } | ||
| 98 | |||
| 99 | /// @brief Obtain the left child of BV node in the first BVH | ||
| 100 | 25511395 | int getFirstLeftChild(unsigned int b) const { | |
| 101 | 25511395 | return model1->getBV(b).leftChild(); | |
| 102 | } | ||
| 103 | |||
| 104 | /// @brief Obtain the right child of BV node in the first BVH | ||
| 105 | 25511395 | int getFirstRightChild(unsigned int b) const { | |
| 106 | 25511395 | return model1->getBV(b).rightChild(); | |
| 107 | } | ||
| 108 | |||
| 109 | /// @brief Obtain the left child of BV node in the second BVH | ||
| 110 | 16072382 | int getSecondLeftChild(unsigned int b) const { | |
| 111 | 16072382 | return model2->getBV(b).leftChild(); | |
| 112 | } | ||
| 113 | |||
| 114 | /// @brief Obtain the right child of BV node in the second BVH | ||
| 115 | 16072382 | int getSecondRightChild(unsigned int b) const { | |
| 116 | 16072382 | return model2->getBV(b).rightChild(); | |
| 117 | } | ||
| 118 | |||
| 119 | /// @brief The first BVH model | ||
| 120 | const BVHModel<BV>* model1; | ||
| 121 | /// @brief The second BVH model | ||
| 122 | const BVHModel<BV>* model2; | ||
| 123 | |||
| 124 | /// @brief statistical information | ||
| 125 | mutable int num_bv_tests; | ||
| 126 | mutable int num_leaf_tests; | ||
| 127 | mutable Scalar query_time_seconds; | ||
| 128 | }; | ||
| 129 | |||
| 130 | /// @brief Traversal node for collision between two meshes | ||
| 131 | template <typename BV, int _Options = RelativeTransformationIsIdentity> | ||
| 132 | class MeshCollisionTraversalNode : public BVHCollisionTraversalNode<BV> { | ||
| 133 | public: | ||
| 134 | enum { | ||
| 135 | Options = _Options, | ||
| 136 | RTIsIdentity = _Options & RelativeTransformationIsIdentity | ||
| 137 | }; | ||
| 138 | |||
| 139 | 21595 | MeshCollisionTraversalNode(const CollisionRequest& request) | |
| 140 |
1/2✓ Branch 2 taken 10697 times.
✗ Branch 3 not taken.
|
21595 | : BVHCollisionTraversalNode<BV>(request) { |
| 141 | 21595 | vertices1 = NULL; | |
| 142 | 21595 | vertices2 = NULL; | |
| 143 | 21595 | tri_indices1 = NULL; | |
| 144 | 21595 | tri_indices2 = NULL; | |
| 145 | 21595 | } | |
| 146 | |||
| 147 | /// BV test between b1 and b2 | ||
| 148 | /// @param b1, b2 Bounding volumes to test, | ||
| 149 | /// @retval sqrDistLowerBound square of a lower bound of the minimal | ||
| 150 | /// distance between bounding volumes. | ||
| 151 | 41745725 | bool BVDisjoints(unsigned int b1, unsigned int b2, | |
| 152 | Scalar& sqrDistLowerBound) const { | ||
| 153 |
2/2✓ Branch 0 taken 3869604 times.
✓ Branch 1 taken 17003287 times.
|
41745725 | if (this->enable_statistics) this->num_bv_tests++; |
| 154 | bool disjoint; | ||
| 155 | if (RTIsIdentity) | ||
| 156 | 83175980 | disjoint = !this->model1->getBV(b1).overlap( | |
| 157 | 41587990 | this->model2->getBV(b2), this->request, sqrDistLowerBound); | |
| 158 | else { | ||
| 159 | 157735 | disjoint = !overlap(RT._R(), RT._T(), this->model2->getBV(b2).bv, | |
| 160 | 157735 | this->model1->getBV(b1).bv, this->request, | |
| 161 | sqrDistLowerBound); | ||
| 162 | } | ||
| 163 |
2/2✓ Branch 0 taken 80983 times.
✓ Branch 1 taken 20791908 times.
|
41745725 | if (disjoint) |
| 164 | 161948 | internal::updateDistanceLowerBoundFromBV(this->request, *this->result, | |
| 165 | sqrDistLowerBound); | ||
| 166 | 41745725 | return disjoint; | |
| 167 | } | ||
| 168 | |||
| 169 | /// Intersection testing between leaves (two triangles) | ||
| 170 | /// | ||
| 171 | /// @param b1, b2 id of primitive in bounding volume hierarchy | ||
| 172 | /// @retval sqrDistLowerBound squared lower bound of distance between | ||
| 173 | /// primitives if they are not in collision. | ||
| 174 | /// | ||
| 175 | /// This method supports a security margin. If the distance between | ||
| 176 | /// the primitives is less than the security margin, the objects are | ||
| 177 | /// considered as in collision. in this case a contact point is | ||
| 178 | /// returned in the CollisionResult. | ||
| 179 | /// | ||
| 180 | /// @note If the distance between objects is less than the security margin, | ||
| 181 | /// and the object are not colliding, the penetration depth is | ||
| 182 | /// negative. | ||
| 183 | 58359872 | void leafCollides(unsigned int b1, unsigned int b2, | |
| 184 | Scalar& sqrDistLowerBound) const { | ||
| 185 |
2/2✓ Branch 0 taken 3846921 times.
✓ Branch 1 taken 25333019 times.
|
58359872 | if (this->enable_statistics) this->num_leaf_tests++; |
| 186 | |||
| 187 | 58359872 | const BVNode<BV>& node1 = this->model1->getBV(b1); | |
| 188 | 58359872 | const BVNode<BV>& node2 = this->model2->getBV(b2); | |
| 189 | |||
| 190 | 58359872 | int primitive_id1 = node1.primitiveId(); | |
| 191 | 58359872 | int primitive_id2 = node2.primitiveId(); | |
| 192 | |||
| 193 | 58359872 | const Triangle32& tri_id1 = tri_indices1[primitive_id1]; | |
| 194 | 58359872 | const Triangle32& tri_id2 = tri_indices2[primitive_id2]; | |
| 195 | |||
| 196 | 58359872 | const Vec3s& P1 = vertices1[tri_id1[0]]; | |
| 197 | 58359872 | const Vec3s& P2 = vertices1[tri_id1[1]]; | |
| 198 | 58359872 | const Vec3s& P3 = vertices1[tri_id1[2]]; | |
| 199 | 58359872 | const Vec3s& Q1 = vertices2[tri_id2[0]]; | |
| 200 | 58359872 | const Vec3s& Q2 = vertices2[tri_id2[1]]; | |
| 201 | 58359872 | const Vec3s& Q3 = vertices2[tri_id2[2]]; | |
| 202 | |||
| 203 |
1/2✓ Branch 1 taken 29179940 times.
✗ Branch 2 not taken.
|
58359872 | TriangleP tri1(P1, P2, P3); |
| 204 |
1/2✓ Branch 1 taken 29179940 times.
✗ Branch 2 not taken.
|
58359872 | TriangleP tri2(Q1, Q2, Q3); |
| 205 | |||
| 206 | // TODO(louis): MeshCollisionTraversalNode should have its own GJKSolver. | ||
| 207 |
1/2✓ Branch 1 taken 29179940 times.
✗ Branch 2 not taken.
|
58359872 | GJKSolver solver(this->request); |
| 208 | |||
| 209 | 58359872 | const bool compute_penetration = | |
| 210 |
3/4✓ Branch 0 taken 25329507 times.
✓ Branch 1 taken 3850433 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 25329507 times.
|
58359872 | this->request.enable_contact || (this->request.security_margin < 0); |
| 211 |
3/6✓ Branch 1 taken 29179940 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 29179940 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 29179940 times.
✗ Branch 8 not taken.
|
58359872 | Vec3s p1, p2, normal; |
| 212 |
1/2✓ Branch 2 taken 29179940 times.
✗ Branch 3 not taken.
|
58359872 | DistanceResult distanceResult; |
| 213 | 116719744 | Scalar distance = internal::ShapeShapeDistance<TriangleP, TriangleP>( | |
| 214 |
1/2✓ Branch 1 taken 29179940 times.
✗ Branch 2 not taken.
|
58359872 | &tri1, this->tf1, &tri2, this->tf2, &solver, compute_penetration, p1, |
| 215 | p2, normal); | ||
| 216 | |||
| 217 | 58359872 | const Scalar distToCollision = distance - this->request.security_margin; | |
| 218 | |||
| 219 |
1/2✓ Branch 1 taken 29179940 times.
✗ Branch 2 not taken.
|
58359872 | internal::updateDistanceLowerBoundFromLeaf(this->request, *(this->result), |
| 220 | distToCollision, p1, p2, normal); | ||
| 221 | |||
| 222 | 58359872 | if (distToCollision <= | |
| 223 |
2/2✓ Branch 0 taken 15561 times.
✓ Branch 1 taken 29164379 times.
|
58359872 | this->request.collision_distance_threshold) { // collision |
| 224 | 31121 | sqrDistLowerBound = 0; | |
| 225 |
1/2✓ Branch 1 taken 15561 times.
✗ Branch 2 not taken.
|
31121 | if (this->result->numContacts() < this->request.num_max_contacts) { |
| 226 |
2/4✓ Branch 1 taken 15561 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15561 times.
✗ Branch 5 not taken.
|
31121 | this->result->addContact(Contact(this->model1, this->model2, |
| 227 | primitive_id1, primitive_id2, p1, p2, | ||
| 228 | normal, distance)); | ||
| 229 | } | ||
| 230 | } else | ||
| 231 | 58328751 | sqrDistLowerBound = distToCollision * distToCollision; | |
| 232 | 58359872 | } | |
| 233 | |||
| 234 | Vec3s* vertices1; | ||
| 235 | Vec3s* vertices2; | ||
| 236 | |||
| 237 | Triangle32* tri_indices1; | ||
| 238 | Triangle32* tri_indices2; | ||
| 239 | |||
| 240 | details::RelativeTransformation<!bool(RTIsIdentity)> RT; | ||
| 241 | }; | ||
| 242 | |||
| 243 | /// @brief Traversal node for collision between two meshes if their underlying | ||
| 244 | /// BVH node is oriented node (OBB, RSS, OBBRSS, kIOS) | ||
| 245 | typedef MeshCollisionTraversalNode<OBB, 0> MeshCollisionTraversalNodeOBB; | ||
| 246 | typedef MeshCollisionTraversalNode<RSS, 0> MeshCollisionTraversalNodeRSS; | ||
| 247 | typedef MeshCollisionTraversalNode<kIOS, 0> MeshCollisionTraversalNodekIOS; | ||
| 248 | typedef MeshCollisionTraversalNode<OBBRSS, 0> MeshCollisionTraversalNodeOBBRSS; | ||
| 249 | |||
| 250 | /// @} | ||
| 251 | |||
| 252 | namespace details { | ||
| 253 | template <typename BV> | ||
| 254 | struct DistanceTraversalBVDistanceLowerBound_impl { | ||
| 255 | 5556 | static Scalar run(const BVNode<BV>& b1, const BVNode<BV>& b2) { | |
| 256 | 5556 | return b1.distance(b2); | |
| 257 | } | ||
| 258 | 3160964 | static Scalar run(const Matrix3s& R, const Vec3s& T, const BVNode<BV>& b1, | |
| 259 | const BVNode<BV>& b2) { | ||
| 260 | 3160964 | return distance(R, T, b1.bv, b2.bv); | |
| 261 | } | ||
| 262 | }; | ||
| 263 | |||
| 264 | template <> | ||
| 265 | struct DistanceTraversalBVDistanceLowerBound_impl<OBB> { | ||
| 266 | 15564 | static Scalar run(const BVNode<OBB>& b1, const BVNode<OBB>& b2) { | |
| 267 | Scalar sqrDistLowerBound; | ||
| 268 |
1/2✓ Branch 1 taken 15564 times.
✗ Branch 2 not taken.
|
15564 | CollisionRequest request(DISTANCE_LOWER_BOUND, 0); |
| 269 | // request.break_distance = ? | ||
| 270 |
3/4✓ Branch 1 taken 15564 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 10494 times.
✓ Branch 4 taken 5070 times.
|
15564 | if (b1.overlap(b2, request, sqrDistLowerBound)) { |
| 271 | // TODO A penetration upper bound should be computed. | ||
| 272 | 10494 | return -1; | |
| 273 | } | ||
| 274 | 5070 | return std::sqrt(sqrDistLowerBound); | |
| 275 | } | ||
| 276 | static Scalar run(const Matrix3s& R, const Vec3s& T, const BVNode<OBB>& b1, | ||
| 277 | const BVNode<OBB>& b2) { | ||
| 278 | Scalar sqrDistLowerBound; | ||
| 279 | CollisionRequest request(DISTANCE_LOWER_BOUND, 0); | ||
| 280 | // request.break_distance = ? | ||
| 281 | if (overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) { | ||
| 282 | // TODO A penetration upper bound should be computed. | ||
| 283 | return -1; | ||
| 284 | } | ||
| 285 | return std::sqrt(sqrDistLowerBound); | ||
| 286 | } | ||
| 287 | }; | ||
| 288 | |||
| 289 | template <> | ||
| 290 | struct DistanceTraversalBVDistanceLowerBound_impl<AABB> { | ||
| 291 | ✗ | static Scalar run(const BVNode<AABB>& b1, const BVNode<AABB>& b2) { | |
| 292 | Scalar sqrDistLowerBound; | ||
| 293 | ✗ | CollisionRequest request(DISTANCE_LOWER_BOUND, 0); | |
| 294 | // request.break_distance = ? | ||
| 295 | ✗ | if (b1.overlap(b2, request, sqrDistLowerBound)) { | |
| 296 | // TODO A penetration upper bound should be computed. | ||
| 297 | ✗ | return -1; | |
| 298 | } | ||
| 299 | ✗ | return std::sqrt(sqrDistLowerBound); | |
| 300 | } | ||
| 301 | static Scalar run(const Matrix3s& R, const Vec3s& T, const BVNode<AABB>& b1, | ||
| 302 | const BVNode<AABB>& b2) { | ||
| 303 | Scalar sqrDistLowerBound; | ||
| 304 | CollisionRequest request(DISTANCE_LOWER_BOUND, 0); | ||
| 305 | // request.break_distance = ? | ||
| 306 | if (overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) { | ||
| 307 | // TODO A penetration upper bound should be computed. | ||
| 308 | return -1; | ||
| 309 | } | ||
| 310 | return std::sqrt(sqrDistLowerBound); | ||
| 311 | } | ||
| 312 | }; | ||
| 313 | } // namespace details | ||
| 314 | |||
| 315 | /// @addtogroup Traversal_For_Distance | ||
| 316 | /// @{ | ||
| 317 | |||
| 318 | /// @brief Traversal node for distance computation between BVH models | ||
| 319 | template <typename BV> | ||
| 320 | class BVHDistanceTraversalNode : public DistanceTraversalNodeBase { | ||
| 321 | public: | ||
| 322 | 14364 | BVHDistanceTraversalNode() : DistanceTraversalNodeBase() { | |
| 323 | 14364 | model1 = NULL; | |
| 324 | 14364 | model2 = NULL; | |
| 325 | |||
| 326 | 14364 | num_bv_tests = 0; | |
| 327 | 14364 | num_leaf_tests = 0; | |
| 328 | 14364 | query_time_seconds = 0.0; | |
| 329 | 14364 | } | |
| 330 | |||
| 331 | /// @brief Whether the BV node in the first BVH tree is leaf | ||
| 332 | 2263638 | bool isFirstNodeLeaf(unsigned int b) const { | |
| 333 | 2263638 | return model1->getBV(b).isLeaf(); | |
| 334 | } | ||
| 335 | |||
| 336 | /// @brief Whether the BV node in the second BVH tree is leaf | ||
| 337 | 2263638 | bool isSecondNodeLeaf(unsigned int b) const { | |
| 338 | 2263638 | return model2->getBV(b).isLeaf(); | |
| 339 | } | ||
| 340 | |||
| 341 | /// @brief Determine the traversal order, is the first BVTT subtree better | ||
| 342 | 1598824 | bool firstOverSecond(unsigned int b1, unsigned int b2) const { | |
| 343 | 1598824 | Scalar sz1 = model1->getBV(b1).bv.size(); | |
| 344 | 1598824 | Scalar sz2 = model2->getBV(b2).bv.size(); | |
| 345 | |||
| 346 | 1598824 | bool l1 = model1->getBV(b1).isLeaf(); | |
| 347 | 1598824 | bool l2 = model2->getBV(b2).isLeaf(); | |
| 348 | |||
| 349 |
6/6✓ Branch 0 taken 348307 times.
✓ Branch 1 taken 451105 times.
✓ Branch 2 taken 276501 times.
✓ Branch 3 taken 71806 times.
✓ Branch 4 taken 110821 times.
✓ Branch 5 taken 165680 times.
|
1598824 | if (l2 || (!l1 && (sz1 > sz2))) return true; |
| 350 | 474972 | return false; | |
| 351 | } | ||
| 352 | |||
| 353 | /// @brief Obtain the left child of BV node in the first BVH | ||
| 354 | 1123852 | int getFirstLeftChild(unsigned int b) const { | |
| 355 | 1123852 | return model1->getBV(b).leftChild(); | |
| 356 | } | ||
| 357 | |||
| 358 | /// @brief Obtain the right child of BV node in the first BVH | ||
| 359 | 1123852 | int getFirstRightChild(unsigned int b) const { | |
| 360 | 1123852 | return model1->getBV(b).rightChild(); | |
| 361 | } | ||
| 362 | |||
| 363 | /// @brief Obtain the left child of BV node in the second BVH | ||
| 364 | 474972 | int getSecondLeftChild(unsigned int b) const { | |
| 365 | 474972 | return model2->getBV(b).leftChild(); | |
| 366 | } | ||
| 367 | |||
| 368 | /// @brief Obtain the right child of BV node in the second BVH | ||
| 369 | 474972 | int getSecondRightChild(unsigned int b) const { | |
| 370 | 474972 | return model2->getBV(b).rightChild(); | |
| 371 | } | ||
| 372 | |||
| 373 | /// @brief The first BVH model | ||
| 374 | const BVHModel<BV>* model1; | ||
| 375 | /// @brief The second BVH model | ||
| 376 | const BVHModel<BV>* model2; | ||
| 377 | |||
| 378 | /// @brief statistical information | ||
| 379 | mutable int num_bv_tests; | ||
| 380 | mutable int num_leaf_tests; | ||
| 381 | mutable Scalar query_time_seconds; | ||
| 382 | }; | ||
| 383 | |||
| 384 | /// @brief Traversal node for distance computation between two meshes | ||
| 385 | template <typename BV, int _Options = RelativeTransformationIsIdentity> | ||
| 386 | class MeshDistanceTraversalNode : public BVHDistanceTraversalNode<BV> { | ||
| 387 | public: | ||
| 388 | enum { | ||
| 389 | Options = _Options, | ||
| 390 | RTIsIdentity = _Options & RelativeTransformationIsIdentity | ||
| 391 | }; | ||
| 392 | |||
| 393 | using BVHDistanceTraversalNode<BV>::enable_statistics; | ||
| 394 | using BVHDistanceTraversalNode<BV>::request; | ||
| 395 | using BVHDistanceTraversalNode<BV>::result; | ||
| 396 | using BVHDistanceTraversalNode<BV>::tf1; | ||
| 397 | using BVHDistanceTraversalNode<BV>::model1; | ||
| 398 | using BVHDistanceTraversalNode<BV>::model2; | ||
| 399 | using BVHDistanceTraversalNode<BV>::num_bv_tests; | ||
| 400 | using BVHDistanceTraversalNode<BV>::num_leaf_tests; | ||
| 401 | |||
| 402 |
1/2✓ Branch 2 taken 7158 times.
✗ Branch 3 not taken.
|
14364 | MeshDistanceTraversalNode() : BVHDistanceTraversalNode<BV>() { |
| 403 | 14364 | vertices1 = NULL; | |
| 404 | 14364 | vertices2 = NULL; | |
| 405 | 14364 | tri_indices1 = NULL; | |
| 406 | 14364 | tri_indices2 = NULL; | |
| 407 | |||
| 408 | 14364 | rel_err = this->request.rel_err; | |
| 409 | 14364 | abs_err = this->request.abs_err; | |
| 410 | 14364 | } | |
| 411 | |||
| 412 | 14364 | void preprocess() { | |
| 413 | 14316 | if (!RTIsIdentity) preprocessOrientedNode(); | |
| 414 | 14364 | } | |
| 415 | |||
| 416 | 14364 | void postprocess() { | |
| 417 | 14316 | if (!RTIsIdentity) postprocessOrientedNode(); | |
| 418 | 14364 | } | |
| 419 | |||
| 420 | /// @brief BV culling test in one BVTT node | ||
| 421 | 3197648 | Scalar BVDistanceLowerBound(unsigned int b1, unsigned int b2) const { | |
| 422 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 1598824 times.
|
3197648 | if (enable_statistics) num_bv_tests++; |
| 423 | if (RTIsIdentity) | ||
| 424 | 36684 | return details::DistanceTraversalBVDistanceLowerBound_impl<BV>::run( | |
| 425 | 73368 | model1->getBV(b1), model2->getBV(b2)); | |
| 426 | else | ||
| 427 | 3160964 | return details::DistanceTraversalBVDistanceLowerBound_impl<BV>::run( | |
| 428 | 6321928 | RT._R(), RT._T(), model1->getBV(b1), model2->getBV(b2)); | |
| 429 | } | ||
| 430 | |||
| 431 | /// @brief Distance testing between leaves (two triangles) | ||
| 432 | 663638 | void leafComputeDistance(unsigned int b1, unsigned int b2) const { | |
| 433 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 331819 times.
|
663638 | if (this->enable_statistics) this->num_leaf_tests++; |
| 434 | |||
| 435 | 663638 | const BVNode<BV>& node1 = this->model1->getBV(b1); | |
| 436 | 663638 | const BVNode<BV>& node2 = this->model2->getBV(b2); | |
| 437 | |||
| 438 | 663638 | int primitive_id1 = node1.primitiveId(); | |
| 439 | 663638 | int primitive_id2 = node2.primitiveId(); | |
| 440 | |||
| 441 | 663638 | const Triangle32& tri_id1 = tri_indices1[primitive_id1]; | |
| 442 | 663638 | const Triangle32& tri_id2 = tri_indices2[primitive_id2]; | |
| 443 | |||
| 444 | 663638 | const Vec3s& t11 = vertices1[tri_id1[0]]; | |
| 445 | 663638 | const Vec3s& t12 = vertices1[tri_id1[1]]; | |
| 446 | 663638 | const Vec3s& t13 = vertices1[tri_id1[2]]; | |
| 447 | |||
| 448 | 663638 | const Vec3s& t21 = vertices2[tri_id2[0]]; | |
| 449 | 663638 | const Vec3s& t22 = vertices2[tri_id2[1]]; | |
| 450 | 663638 | const Vec3s& t23 = vertices2[tri_id2[2]]; | |
| 451 | |||
| 452 | // nearest point pair | ||
| 453 |
3/6✓ Branch 1 taken 331819 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 331819 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 331819 times.
✗ Branch 8 not taken.
|
663638 | Vec3s P1, P2, normal; |
| 454 | |||
| 455 | Scalar d2; | ||
| 456 | if (RTIsIdentity) | ||
| 457 |
1/2✓ Branch 1 taken 3601 times.
✗ Branch 2 not taken.
|
7202 | d2 = TriangleDistance::sqrTriDistance(t11, t12, t13, t21, t22, t23, P1, |
| 458 | P2); | ||
| 459 | else | ||
| 460 |
1/2✓ Branch 3 taken 328218 times.
✗ Branch 4 not taken.
|
656436 | d2 = TriangleDistance::sqrTriDistance(t11, t12, t13, t21, t22, t23, |
| 461 | RT._R(), RT._T(), P1, P2); | ||
| 462 | 663638 | Scalar d = sqrt(d2); | |
| 463 | |||
| 464 |
1/2✓ Branch 1 taken 331819 times.
✗ Branch 2 not taken.
|
663638 | this->result->update(d, this->model1, this->model2, primitive_id1, |
| 465 | primitive_id2, P1, P2, normal); | ||
| 466 | 663638 | } | |
| 467 | |||
| 468 | /// @brief Whether the traversal process can stop early | ||
| 469 | 3191550 | bool canStop(Scalar c) const { | |
| 470 |
2/2✓ Branch 0 taken 471726 times.
✓ Branch 1 taken 1124049 times.
|
3191550 | if ((c >= this->result->min_distance - abs_err) && |
| 471 |
1/2✓ Branch 0 taken 471726 times.
✗ Branch 1 not taken.
|
943452 | (c * (1 + rel_err) >= this->result->min_distance)) |
| 472 | 943452 | return true; | |
| 473 | 2248098 | return false; | |
| 474 | } | ||
| 475 | |||
| 476 | Vec3s* vertices1; | ||
| 477 | Vec3s* vertices2; | ||
| 478 | |||
| 479 | Triangle32* tri_indices1; | ||
| 480 | Triangle32* tri_indices2; | ||
| 481 | |||
| 482 | /// @brief relative and absolute error, default value is 0.01 for both terms | ||
| 483 | Scalar rel_err; | ||
| 484 | Scalar abs_err; | ||
| 485 | |||
| 486 | details::RelativeTransformation<!bool(RTIsIdentity)> RT; | ||
| 487 | |||
| 488 | private: | ||
| 489 | 14316 | void preprocessOrientedNode() { | |
| 490 | 14316 | const int init_tri_id1 = 0, init_tri_id2 = 0; | |
| 491 | 14316 | const Triangle32& init_tri1 = tri_indices1[init_tri_id1]; | |
| 492 | 14316 | const Triangle32& init_tri2 = tri_indices2[init_tri_id2]; | |
| 493 | |||
| 494 |
3/4✓ Branch 1 taken 21474 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 21474 times.
✓ Branch 4 taken 7158 times.
|
57264 | Vec3s init_tri1_points[3]; |
| 495 |
3/4✓ Branch 1 taken 21474 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 21474 times.
✓ Branch 4 taken 7158 times.
|
57264 | Vec3s init_tri2_points[3]; |
| 496 | |||
| 497 |
1/2✓ Branch 2 taken 7158 times.
✗ Branch 3 not taken.
|
14316 | init_tri1_points[0] = vertices1[init_tri1[0]]; |
| 498 |
1/2✓ Branch 2 taken 7158 times.
✗ Branch 3 not taken.
|
14316 | init_tri1_points[1] = vertices1[init_tri1[1]]; |
| 499 |
1/2✓ Branch 2 taken 7158 times.
✗ Branch 3 not taken.
|
14316 | init_tri1_points[2] = vertices1[init_tri1[2]]; |
| 500 | |||
| 501 |
1/2✓ Branch 2 taken 7158 times.
✗ Branch 3 not taken.
|
14316 | init_tri2_points[0] = vertices2[init_tri2[0]]; |
| 502 |
1/2✓ Branch 2 taken 7158 times.
✗ Branch 3 not taken.
|
14316 | init_tri2_points[1] = vertices2[init_tri2[1]]; |
| 503 |
1/2✓ Branch 2 taken 7158 times.
✗ Branch 3 not taken.
|
14316 | init_tri2_points[2] = vertices2[init_tri2[2]]; |
| 504 | |||
| 505 |
3/6✓ Branch 1 taken 7158 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7158 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7158 times.
✗ Branch 8 not taken.
|
14316 | Vec3s p1, p2, normal; |
| 506 |
1/2✓ Branch 3 taken 7158 times.
✗ Branch 4 not taken.
|
14316 | Scalar distance = sqrt(TriangleDistance::sqrTriDistance( |
| 507 | init_tri1_points[0], init_tri1_points[1], init_tri1_points[2], | ||
| 508 | init_tri2_points[0], init_tri2_points[1], init_tri2_points[2], RT._R(), | ||
| 509 | RT._T(), p1, p2)); | ||
| 510 | |||
| 511 |
1/2✓ Branch 1 taken 7158 times.
✗ Branch 2 not taken.
|
14316 | result->update(distance, model1, model2, init_tri_id1, init_tri_id2, p1, p2, |
| 512 | normal); | ||
| 513 | 14316 | } | |
| 514 | 14316 | void postprocessOrientedNode() { | |
| 515 | /// the points obtained by triDistance are not in world space: both are in | ||
| 516 | /// object1's local coordinate system, so we need to convert them into the | ||
| 517 | /// world space. | ||
| 518 |
3/4✓ Branch 0 taken 7158 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 1896 times.
✓ Branch 3 taken 5262 times.
|
14316 | if (request.enable_nearest_points && (result->o1 == model1) && |
| 519 |
2/2✓ Branch 0 taken 1667 times.
✓ Branch 1 taken 229 times.
|
3792 | (result->o2 == model2)) { |
| 520 |
1/2✓ Branch 2 taken 1667 times.
✗ Branch 3 not taken.
|
3334 | result->nearest_points[0] = tf1.transform(result->nearest_points[0]); |
| 521 |
1/2✓ Branch 2 taken 1667 times.
✗ Branch 3 not taken.
|
3334 | result->nearest_points[1] = tf1.transform(result->nearest_points[1]); |
| 522 | } | ||
| 523 | 14316 | } | |
| 524 | }; | ||
| 525 | |||
| 526 | /// @brief Traversal node for distance computation between two meshes if their | ||
| 527 | /// underlying BVH node is oriented node (RSS, OBBRSS, kIOS) | ||
| 528 | typedef MeshDistanceTraversalNode<RSS, 0> MeshDistanceTraversalNodeRSS; | ||
| 529 | typedef MeshDistanceTraversalNode<kIOS, 0> MeshDistanceTraversalNodekIOS; | ||
| 530 | typedef MeshDistanceTraversalNode<OBBRSS, 0> MeshDistanceTraversalNodeOBBRSS; | ||
| 531 | |||
| 532 | /// @} | ||
| 533 | |||
| 534 | /// @brief for OBB and RSS, there is local coordinate of BV, so normal need to | ||
| 535 | /// be transformed | ||
| 536 | namespace details { | ||
| 537 | |||
| 538 | template <typename BV> | ||
| 539 | inline const Matrix3s& getBVAxes(const BV& bv) { | ||
| 540 | return bv.axes; | ||
| 541 | } | ||
| 542 | |||
| 543 | template <> | ||
| 544 | inline const Matrix3s& getBVAxes<OBBRSS>(const OBBRSS& bv) { | ||
| 545 | return bv.obb.axes; | ||
| 546 | } | ||
| 547 | |||
| 548 | } // namespace details | ||
| 549 | |||
| 550 | } // namespace coal | ||
| 551 | |||
| 552 | /// @endcond | ||
| 553 | |||
| 554 | #endif | ||
| 555 |