Directory: | ./ |
---|---|
File: | include/coal/internal/traversal_node_octree.h |
Date: | 2025-04-01 09:23:31 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 249 | 560 | 44.5% |
Branches: | 250 | 898 | 27.8% |
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) 2022-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_TRAVERSAL_NODE_OCTREE_H | ||
40 | #define COAL_TRAVERSAL_NODE_OCTREE_H | ||
41 | |||
42 | /// @cond INTERNAL | ||
43 | |||
44 | #include "coal/collision_data.h" | ||
45 | #include "coal/internal/traversal_node_base.h" | ||
46 | #include "coal/internal/traversal_node_hfield_shape.h" | ||
47 | #include "coal/narrowphase/narrowphase.h" | ||
48 | #include "coal/hfield.h" | ||
49 | #include "coal/octree.h" | ||
50 | #include "coal/BVH/BVH_model.h" | ||
51 | #include "coal/shape/geometric_shapes_utility.h" | ||
52 | #include "coal/internal/shape_shape_func.h" | ||
53 | |||
54 | namespace coal { | ||
55 | |||
56 | /// @brief Algorithms for collision related with octree | ||
57 | class COAL_DLLAPI OcTreeSolver { | ||
58 | private: | ||
59 | const GJKSolver* solver; | ||
60 | |||
61 | mutable const CollisionRequest* crequest; | ||
62 | mutable const DistanceRequest* drequest; | ||
63 | |||
64 | mutable CollisionResult* cresult; | ||
65 | mutable DistanceResult* dresult; | ||
66 | |||
67 | public: | ||
68 | 3100 | OcTreeSolver(const GJKSolver* solver_) | |
69 | 3100 | : solver(solver_), | |
70 | 3100 | crequest(NULL), | |
71 | 3100 | drequest(NULL), | |
72 | 3100 | cresult(NULL), | |
73 | 3100 | dresult(NULL) {} | |
74 | |||
75 | /// @brief collision between two octrees | ||
76 | ✗ | void OcTreeIntersect(const OcTree* tree1, const OcTree* tree2, | |
77 | const Transform3s& tf1, const Transform3s& tf2, | ||
78 | const CollisionRequest& request_, | ||
79 | CollisionResult& result_) const { | ||
80 | ✗ | crequest = &request_; | |
81 | ✗ | cresult = &result_; | |
82 | |||
83 | ✗ | OcTreeIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), tree2, | |
84 | ✗ | tree2->getRoot(), tree2->getRootBV(), tf1, tf2); | |
85 | } | ||
86 | |||
87 | /// @brief distance between two octrees | ||
88 | ✗ | void OcTreeDistance(const OcTree* tree1, const OcTree* tree2, | |
89 | const Transform3s& tf1, const Transform3s& tf2, | ||
90 | const DistanceRequest& request_, | ||
91 | DistanceResult& result_) const { | ||
92 | ✗ | drequest = &request_; | |
93 | ✗ | dresult = &result_; | |
94 | |||
95 | ✗ | OcTreeDistanceRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), tree2, | |
96 | ✗ | tree2->getRoot(), tree2->getRootBV(), tf1, tf2); | |
97 | } | ||
98 | |||
99 | /// @brief collision between octree and mesh | ||
100 | template <typename BV> | ||
101 | 200 | void OcTreeMeshIntersect(const OcTree* tree1, const BVHModel<BV>* tree2, | |
102 | const Transform3s& tf1, const Transform3s& tf2, | ||
103 | const CollisionRequest& request_, | ||
104 | CollisionResult& result_) const { | ||
105 | 200 | crequest = &request_; | |
106 | 200 | cresult = &result_; | |
107 | |||
108 |
2/4✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
|
200 | OcTreeMeshIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), |
109 | tree2, 0, tf1, tf2); | ||
110 | } | ||
111 | |||
112 | /// @brief distance between octree and mesh | ||
113 | template <typename BV> | ||
114 | ✗ | void OcTreeMeshDistance(const OcTree* tree1, const BVHModel<BV>* tree2, | |
115 | const Transform3s& tf1, const Transform3s& tf2, | ||
116 | const DistanceRequest& request_, | ||
117 | DistanceResult& result_) const { | ||
118 | ✗ | drequest = &request_; | |
119 | ✗ | dresult = &result_; | |
120 | |||
121 | ✗ | OcTreeMeshDistanceRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), | |
122 | tree2, 0, tf1, tf2); | ||
123 | } | ||
124 | |||
125 | /// @brief collision between mesh and octree | ||
126 | template <typename BV> | ||
127 | void MeshOcTreeIntersect(const BVHModel<BV>* tree1, const OcTree* tree2, | ||
128 | const Transform3s& tf1, const Transform3s& tf2, | ||
129 | const CollisionRequest& request_, | ||
130 | CollisionResult& result_) const | ||
131 | |||
132 | { | ||
133 | crequest = &request_; | ||
134 | cresult = &result_; | ||
135 | |||
136 | OcTreeMeshIntersectRecurse(tree2, tree2->getRoot(), tree2->getRootBV(), | ||
137 | tree1, 0, tf2, tf1); | ||
138 | } | ||
139 | |||
140 | /// @brief distance between mesh and octree | ||
141 | template <typename BV> | ||
142 | void MeshOcTreeDistance(const BVHModel<BV>* tree1, const OcTree* tree2, | ||
143 | const Transform3s& tf1, const Transform3s& tf2, | ||
144 | const DistanceRequest& request_, | ||
145 | DistanceResult& result_) const { | ||
146 | drequest = &request_; | ||
147 | dresult = &result_; | ||
148 | |||
149 | OcTreeMeshDistanceRecurse(tree1, 0, tree2, tree2->getRoot(), | ||
150 | tree2->getRootBV(), tf1, tf2); | ||
151 | } | ||
152 | |||
153 | template <typename BV> | ||
154 | 2000 | void OcTreeHeightFieldIntersect( | |
155 | const OcTree* tree1, const HeightField<BV>* tree2, const Transform3s& tf1, | ||
156 | const Transform3s& tf2, const CollisionRequest& request_, | ||
157 | CollisionResult& result_, Scalar& sqrDistLowerBound) const { | ||
158 | 2000 | crequest = &request_; | |
159 | 2000 | cresult = &result_; | |
160 | |||
161 |
2/4✓ Branch 2 taken 1000 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1000 times.
✗ Branch 6 not taken.
|
2000 | OcTreeHeightFieldIntersectRecurse(tree1, tree1->getRoot(), |
162 | tree1->getRootBV(), tree2, 0, tf1, tf2, | ||
163 | sqrDistLowerBound); | ||
164 | } | ||
165 | |||
166 | template <typename BV> | ||
167 | 2000 | void HeightFieldOcTreeIntersect(const HeightField<BV>* tree1, | |
168 | const OcTree* tree2, const Transform3s& tf1, | ||
169 | const Transform3s& tf2, | ||
170 | const CollisionRequest& request_, | ||
171 | CollisionResult& result_, | ||
172 | Scalar& sqrDistLowerBound) const { | ||
173 | 2000 | crequest = &request_; | |
174 | 2000 | cresult = &result_; | |
175 | |||
176 |
2/4✓ Branch 2 taken 1000 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1000 times.
✗ Branch 6 not taken.
|
2000 | HeightFieldOcTreeIntersectRecurse(tree1, 0, tree2, tree2->getRoot(), |
177 | tree2->getRootBV(), tf1, tf2, | ||
178 | sqrDistLowerBound); | ||
179 | } | ||
180 | |||
181 | /// @brief collision between octree and shape | ||
182 | template <typename S> | ||
183 | 2000 | void OcTreeShapeIntersect(const OcTree* tree, const S& s, | |
184 | const Transform3s& tf1, const Transform3s& tf2, | ||
185 | const CollisionRequest& request_, | ||
186 | CollisionResult& result_) const { | ||
187 | 2000 | crequest = &request_; | |
188 | 2000 | cresult = &result_; | |
189 | |||
190 |
1/2✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
|
2000 | AABB bv2; |
191 |
2/4✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1000 times.
✗ Branch 5 not taken.
|
2000 | computeBV<AABB>(s, Transform3s(), bv2); |
192 |
1/2✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
|
2000 | OBB obb2; |
193 |
1/2✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
|
2000 | convertBV(bv2, tf2, obb2); |
194 |
3/6✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1000 times.
✗ Branch 8 not taken.
|
2000 | OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(), s, |
195 | obb2, tf1, tf2); | ||
196 | } | ||
197 | |||
198 | /// @brief collision between shape and octree | ||
199 | template <typename S> | ||
200 | void ShapeOcTreeIntersect(const S& s, const OcTree* tree, | ||
201 | const Transform3s& tf1, const Transform3s& tf2, | ||
202 | const CollisionRequest& request_, | ||
203 | CollisionResult& result_) const { | ||
204 | crequest = &request_; | ||
205 | cresult = &result_; | ||
206 | |||
207 | AABB bv1; | ||
208 | computeBV<AABB>(s, Transform3s(), bv1); | ||
209 | OBB obb1; | ||
210 | convertBV(bv1, tf1, obb1); | ||
211 | OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(), s, | ||
212 | obb1, tf2, tf1); | ||
213 | } | ||
214 | |||
215 | /// @brief distance between octree and shape | ||
216 | template <typename S> | ||
217 | ✗ | void OcTreeShapeDistance(const OcTree* tree, const S& s, | |
218 | const Transform3s& tf1, const Transform3s& tf2, | ||
219 | const DistanceRequest& request_, | ||
220 | DistanceResult& result_) const { | ||
221 | ✗ | drequest = &request_; | |
222 | ✗ | dresult = &result_; | |
223 | |||
224 | ✗ | AABB aabb2; | |
225 | ✗ | computeBV<AABB>(s, tf2, aabb2); | |
226 | ✗ | OcTreeShapeDistanceRecurse(tree, tree->getRoot(), tree->getRootBV(), s, | |
227 | aabb2, tf1, tf2); | ||
228 | } | ||
229 | |||
230 | /// @brief distance between shape and octree | ||
231 | template <typename S> | ||
232 | void ShapeOcTreeDistance(const S& s, const OcTree* tree, | ||
233 | const Transform3s& tf1, const Transform3s& tf2, | ||
234 | const DistanceRequest& request_, | ||
235 | DistanceResult& result_) const { | ||
236 | drequest = &request_; | ||
237 | dresult = &result_; | ||
238 | |||
239 | AABB aabb1; | ||
240 | computeBV<AABB>(s, tf1, aabb1); | ||
241 | OcTreeShapeDistanceRecurse(tree, tree->getRoot(), tree->getRootBV(), s, | ||
242 | aabb1, tf2, tf1); | ||
243 | } | ||
244 | |||
245 | private: | ||
246 | template <typename S> | ||
247 | ✗ | bool OcTreeShapeDistanceRecurse(const OcTree* tree1, | |
248 | const OcTree::OcTreeNode* root1, | ||
249 | const AABB& bv1, const S& s, | ||
250 | const AABB& aabb2, const Transform3s& tf1, | ||
251 | const Transform3s& tf2) const { | ||
252 | ✗ | if (!tree1->nodeHasChildren(root1)) { | |
253 | ✗ | if (tree1->isNodeOccupied(root1)) { | |
254 | ✗ | Box box; | |
255 | ✗ | Transform3s box_tf; | |
256 | ✗ | constructBox(bv1, tf1, box, box_tf); | |
257 | |||
258 | ✗ | if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) { | |
259 | ✗ | box.computeLocalAABB(); | |
260 | } | ||
261 | |||
262 | ✗ | Vec3s p1, p2, normal; | |
263 | ✗ | const Scalar distance = internal::ShapeShapeDistance<Box, S>( | |
264 | ✗ | &box, box_tf, &s, tf2, this->solver, | |
265 | ✗ | this->drequest->enable_signed_distance, p1, p2, normal); | |
266 | |||
267 | ✗ | this->dresult->update(distance, tree1, &s, | |
268 | ✗ | (int)(root1 - tree1->getRoot()), | |
269 | DistanceResult::NONE, p1, p2, normal); | ||
270 | |||
271 | ✗ | return drequest->isSatisfied(*dresult); | |
272 | ✗ | } else | |
273 | ✗ | return false; | |
274 | } | ||
275 | |||
276 | ✗ | if (!tree1->isNodeOccupied(root1)) return false; | |
277 | |||
278 | ✗ | for (unsigned int i = 0; i < 8; ++i) { | |
279 | ✗ | if (tree1->nodeChildExists(root1, i)) { | |
280 | ✗ | const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); | |
281 | ✗ | AABB child_bv; | |
282 | ✗ | computeChildBV(bv1, i, child_bv); | |
283 | |||
284 | ✗ | AABB aabb1; | |
285 | ✗ | convertBV(child_bv, tf1, aabb1); | |
286 | ✗ | Scalar d = aabb1.distance(aabb2); | |
287 | ✗ | if (d < dresult->min_distance) { | |
288 | ✗ | if (OcTreeShapeDistanceRecurse(tree1, child, child_bv, s, aabb2, tf1, | |
289 | tf2)) | ||
290 | ✗ | return true; | |
291 | } | ||
292 | } | ||
293 | } | ||
294 | |||
295 | ✗ | return false; | |
296 | } | ||
297 | |||
298 | template <typename S> | ||
299 | 56052 | bool OcTreeShapeIntersectRecurse(const OcTree* tree1, | |
300 | const OcTree::OcTreeNode* root1, | ||
301 | const AABB& bv1, const S& s, const OBB& obb2, | ||
302 | const Transform3s& tf1, | ||
303 | const Transform3s& tf2) const { | ||
304 | // Empty OcTree is considered free. | ||
305 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 28026 times.
|
56052 | if (!root1) return false; |
306 | |||
307 | /// stop when 1) bounding boxes of two objects not overlap; OR | ||
308 | /// 2) at least of one the nodes is free; OR | ||
309 | /// 2) (two uncertain nodes or one node occupied and one node | ||
310 | /// uncertain) AND cost not required | ||
311 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 28026 times.
|
56052 | if (tree1->isNodeFree(root1)) |
312 | ✗ | return false; | |
313 |
3/6✓ Branch 1 taken 28026 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 28026 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 28026 times.
|
56052 | else if ((tree1->isNodeUncertain(root1) || s.isUncertain())) |
314 | ✗ | return false; | |
315 | else { | ||
316 |
1/2✓ Branch 1 taken 28026 times.
✗ Branch 2 not taken.
|
56052 | OBB obb1; |
317 |
1/2✓ Branch 1 taken 28026 times.
✗ Branch 2 not taken.
|
56052 | convertBV(bv1, tf1, obb1); |
318 | Scalar sqrDistLowerBound; | ||
319 |
3/4✓ Branch 1 taken 28026 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 17739 times.
✓ Branch 4 taken 10287 times.
|
56052 | if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound)) { |
320 | 35478 | internal::updateDistanceLowerBoundFromBV(*crequest, *cresult, | |
321 | sqrDistLowerBound); | ||
322 | 35478 | return false; | |
323 | } | ||
324 | } | ||
325 | |||
326 |
2/2✓ Branch 1 taken 16 times.
✓ Branch 2 taken 10271 times.
|
20574 | if (!tree1->nodeHasChildren(root1)) { |
327 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 16 times.
|
32 | assert(tree1->isNodeOccupied(root1)); // it isn't free nor uncertain. |
328 | |||
329 |
1/2✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
|
32 | Box box; |
330 |
1/2✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
|
32 | Transform3s box_tf; |
331 |
1/2✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
|
32 | constructBox(bv1, tf1, box, box_tf); |
332 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 16 times.
|
32 | if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) { |
333 | ✗ | box.computeLocalAABB(); | |
334 | } | ||
335 | |||
336 | 32 | bool contactNotAdded = | |
337 | 32 | (cresult->numContacts() >= crequest->num_max_contacts); | |
338 | 64 | std::size_t ncontact = ShapeShapeCollide<Box, S>( | |
339 |
1/2✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
|
32 | &box, box_tf, &s, tf2, solver, *crequest, *cresult); |
340 |
2/4✓ Branch 0 taken 16 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 16 times.
|
32 | assert(ncontact == 0 || ncontact == 1); |
341 |
2/4✓ Branch 0 taken 16 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
|
32 | if (!contactNotAdded && ncontact == 1) { |
342 | // Update contact information. | ||
343 |
1/2✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
|
32 | const Contact& c = cresult->getContact(cresult->numContacts() - 1); |
344 |
1/2✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
|
32 | cresult->setContact( |
345 | 32 | cresult->numContacts() - 1, | |
346 |
1/2✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
|
64 | Contact(tree1, c.o2, static_cast<int>(root1 - tree1->getRoot()), |
347 |
1/2✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
|
32 | c.b2, c.pos, c.normal, c.penetration_depth)); |
348 | } | ||
349 | |||
350 | // no need to call `internal::updateDistanceLowerBoundFromLeaf` here | ||
351 | // as it is already done internally in `ShapeShapeCollide` above. | ||
352 |
1/2✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
|
32 | return crequest->isSatisfied(*cresult); |
353 | } | ||
354 | |||
355 |
2/2✓ Branch 0 taken 81389 times.
✓ Branch 1 taken 10025 times.
|
182828 | for (unsigned int i = 0; i < 8; ++i) { |
356 |
2/2✓ Branch 1 taken 27026 times.
✓ Branch 2 taken 54363 times.
|
162778 | if (tree1->nodeChildExists(root1, i)) { |
357 |
1/2✓ Branch 1 taken 27026 times.
✗ Branch 2 not taken.
|
54052 | const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); |
358 |
1/2✓ Branch 1 taken 27026 times.
✗ Branch 2 not taken.
|
54052 | AABB child_bv; |
359 |
1/2✓ Branch 1 taken 27026 times.
✗ Branch 2 not taken.
|
54052 | computeChildBV(bv1, i, child_bv); |
360 | |||
361 |
3/4✓ Branch 1 taken 27026 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 246 times.
✓ Branch 4 taken 26780 times.
|
54052 | if (OcTreeShapeIntersectRecurse(tree1, child, child_bv, s, obb2, tf1, |
362 | tf2)) | ||
363 | 492 | return true; | |
364 | } | ||
365 | } | ||
366 | |||
367 | 20050 | return false; | |
368 | } | ||
369 | |||
370 | template <typename BV> | ||
371 | ✗ | bool OcTreeMeshDistanceRecurse(const OcTree* tree1, | |
372 | const OcTree::OcTreeNode* root1, | ||
373 | const AABB& bv1, const BVHModel<BV>* tree2, | ||
374 | unsigned int root2, const Transform3s& tf1, | ||
375 | const Transform3s& tf2) const { | ||
376 | ✗ | if (!tree1->nodeHasChildren(root1) && tree2->getBV(root2).isLeaf()) { | |
377 | ✗ | if (tree1->isNodeOccupied(root1)) { | |
378 | ✗ | Box box; | |
379 | ✗ | Transform3s box_tf; | |
380 | ✗ | constructBox(bv1, tf1, box, box_tf); | |
381 | |||
382 | ✗ | if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) { | |
383 | ✗ | box.computeLocalAABB(); | |
384 | } | ||
385 | |||
386 | ✗ | size_t primitive_id = | |
387 | ✗ | static_cast<size_t>(tree2->getBV(root2).primitiveId()); | |
388 | ✗ | const Triangle& tri_id = (*(tree2->tri_indices))[primitive_id]; | |
389 | ✗ | const TriangleP tri((*(tree2->vertices))[tri_id[0]], | |
390 | ✗ | (*(tree2->vertices))[tri_id[1]], | |
391 | ✗ | (*(tree2->vertices))[tri_id[2]]); | |
392 | |||
393 | ✗ | Vec3s p1, p2, normal; | |
394 | ✗ | const Scalar distance = internal::ShapeShapeDistance<Box, TriangleP>( | |
395 | ✗ | &box, box_tf, &tri, tf2, this->solver, | |
396 | ✗ | this->drequest->enable_signed_distance, p1, p2, normal); | |
397 | |||
398 | ✗ | this->dresult->update(distance, tree1, tree2, | |
399 | ✗ | (int)(root1 - tree1->getRoot()), | |
400 | static_cast<int>(primitive_id), p1, p2, normal); | ||
401 | |||
402 | ✗ | return this->drequest->isSatisfied(*dresult); | |
403 | ✗ | } else | |
404 | ✗ | return false; | |
405 | } | ||
406 | |||
407 | ✗ | if (!tree1->isNodeOccupied(root1)) return false; | |
408 | |||
409 | ✗ | if (tree2->getBV(root2).isLeaf() || | |
410 | ✗ | (tree1->nodeHasChildren(root1) && | |
411 | ✗ | (bv1.size() > tree2->getBV(root2).bv.size()))) { | |
412 | ✗ | for (unsigned int i = 0; i < 8; ++i) { | |
413 | ✗ | if (tree1->nodeChildExists(root1, i)) { | |
414 | ✗ | const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); | |
415 | ✗ | AABB child_bv; | |
416 | ✗ | computeChildBV(bv1, i, child_bv); | |
417 | |||
418 | Scalar d; | ||
419 | ✗ | AABB aabb1, aabb2; | |
420 | ✗ | convertBV(child_bv, tf1, aabb1); | |
421 | ✗ | convertBV(tree2->getBV(root2).bv, tf2, aabb2); | |
422 | ✗ | d = aabb1.distance(aabb2); | |
423 | |||
424 | ✗ | if (d < dresult->min_distance) { | |
425 | ✗ | if (OcTreeMeshDistanceRecurse(tree1, child, child_bv, tree2, root2, | |
426 | tf1, tf2)) | ||
427 | ✗ | return true; | |
428 | } | ||
429 | } | ||
430 | } | ||
431 | } else { | ||
432 | Scalar d; | ||
433 | ✗ | AABB aabb1, aabb2; | |
434 | ✗ | convertBV(bv1, tf1, aabb1); | |
435 | ✗ | unsigned int child = (unsigned int)tree2->getBV(root2).leftChild(); | |
436 | ✗ | convertBV(tree2->getBV(child).bv, tf2, aabb2); | |
437 | ✗ | d = aabb1.distance(aabb2); | |
438 | |||
439 | ✗ | if (d < dresult->min_distance) { | |
440 | ✗ | if (OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1, | |
441 | tf2)) | ||
442 | ✗ | return true; | |
443 | } | ||
444 | |||
445 | ✗ | child = (unsigned int)tree2->getBV(root2).rightChild(); | |
446 | ✗ | convertBV(tree2->getBV(child).bv, tf2, aabb2); | |
447 | ✗ | d = aabb1.distance(aabb2); | |
448 | |||
449 | ✗ | if (d < dresult->min_distance) { | |
450 | ✗ | if (OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1, | |
451 | tf2)) | ||
452 | ✗ | return true; | |
453 | } | ||
454 | } | ||
455 | |||
456 | ✗ | return false; | |
457 | } | ||
458 | |||
459 | /// \return True if the request is satisfied. | ||
460 | template <typename BV> | ||
461 | 57964 | bool OcTreeMeshIntersectRecurse(const OcTree* tree1, | |
462 | const OcTree::OcTreeNode* root1, | ||
463 | const AABB& bv1, const BVHModel<BV>* tree2, | ||
464 | unsigned int root2, const Transform3s& tf1, | ||
465 | const Transform3s& tf2) const { | ||
466 | // FIXME(jmirabel) I do not understand why the BVHModel was traversed. The | ||
467 | // code in this if(!root1) did not output anything so the empty OcTree is | ||
468 | // considered free. Should an empty OcTree be considered free ? | ||
469 | |||
470 | // Empty OcTree is considered free. | ||
471 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 28982 times.
|
57964 | if (!root1) return false; |
472 | 57964 | BVNode<BV> const& bvn2 = tree2->getBV(root2); | |
473 | |||
474 | /// stop when 1) bounding boxes of two objects not overlap; OR | ||
475 | /// 2) at least one of the nodes is free; OR | ||
476 | /// 2) (two uncertain nodes OR one node occupied and one node | ||
477 | /// uncertain) AND cost not required | ||
478 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 28982 times.
|
57964 | if (tree1->isNodeFree(root1)) |
479 | ✗ | return false; | |
480 |
3/6✓ Branch 1 taken 28982 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 28982 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 28982 times.
|
57964 | else if ((tree1->isNodeUncertain(root1) || tree2->isUncertain())) |
481 | ✗ | return false; | |
482 | else { | ||
483 |
2/4✓ Branch 1 taken 28982 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 28982 times.
✗ Branch 5 not taken.
|
57964 | OBB obb1, obb2; |
484 |
1/2✓ Branch 1 taken 28982 times.
✗ Branch 2 not taken.
|
57964 | convertBV(bv1, tf1, obb1); |
485 |
1/2✓ Branch 1 taken 28982 times.
✗ Branch 2 not taken.
|
57964 | convertBV(bvn2.bv, tf2, obb2); |
486 | Scalar sqrDistLowerBound; | ||
487 |
3/4✓ Branch 1 taken 28982 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 17430 times.
✓ Branch 4 taken 11552 times.
|
57964 | if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound)) { |
488 | 34860 | internal::updateDistanceLowerBoundFromBV(*crequest, *cresult, | |
489 | sqrDistLowerBound); | ||
490 | 34860 | return false; | |
491 | } | ||
492 | } | ||
493 | |||
494 | // Check if leaf collides. | ||
495 |
5/6✓ Branch 1 taken 539 times.
✓ Branch 2 taken 11013 times.
✓ Branch 4 taken 539 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 539 times.
✓ Branch 7 taken 11013 times.
|
23104 | if (!tree1->nodeHasChildren(root1) && bvn2.isLeaf()) { |
496 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 539 times.
|
1078 | assert(tree1->isNodeOccupied(root1)); // it isn't free nor uncertain. |
497 |
1/2✓ Branch 1 taken 539 times.
✗ Branch 2 not taken.
|
1078 | Box box; |
498 |
1/2✓ Branch 1 taken 539 times.
✗ Branch 2 not taken.
|
1078 | Transform3s box_tf; |
499 |
1/2✓ Branch 1 taken 539 times.
✗ Branch 2 not taken.
|
1078 | constructBox(bv1, tf1, box, box_tf); |
500 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 539 times.
|
1078 | if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) { |
501 | ✗ | box.computeLocalAABB(); | |
502 | } | ||
503 | |||
504 | 1078 | size_t primitive_id = static_cast<size_t>(bvn2.primitiveId()); | |
505 | 1078 | const Triangle& tri_id = (*(tree2->tri_indices))[primitive_id]; | |
506 |
1/2✓ Branch 4 taken 539 times.
✗ Branch 5 not taken.
|
1078 | const TriangleP tri((*(tree2->vertices))[tri_id[0]], |
507 | 1078 | (*(tree2->vertices))[tri_id[1]], | |
508 | 1078 | (*(tree2->vertices))[tri_id[2]]); | |
509 | |||
510 | // When reaching this point, `this->solver` has already been set up | ||
511 | // by the CollisionRequest `this->crequest`. | ||
512 | // The only thing we need to (and can) pass to `ShapeShapeDistance` is | ||
513 | // whether or not penetration information is should be computed in case of | ||
514 | // collision. | ||
515 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 539 times.
|
1078 | const bool compute_penetration = this->crequest->enable_contact || |
516 | ✗ | (this->crequest->security_margin < 0); | |
517 |
3/6✓ Branch 1 taken 539 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 539 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 539 times.
✗ Branch 8 not taken.
|
1078 | Vec3s c1, c2, normal; |
518 | 2156 | const Scalar distance = internal::ShapeShapeDistance<Box, TriangleP>( | |
519 |
1/2✓ Branch 1 taken 539 times.
✗ Branch 2 not taken.
|
1078 | &box, box_tf, &tri, tf2, this->solver, compute_penetration, c1, c2, |
520 | normal); | ||
521 | 1078 | const Scalar distToCollision = distance - this->crequest->security_margin; | |
522 | |||
523 | 1078 | internal::updateDistanceLowerBoundFromLeaf( | |
524 |
1/2✓ Branch 1 taken 539 times.
✗ Branch 2 not taken.
|
1078 | *(this->crequest), *(this->cresult), distToCollision, c1, c2, normal); |
525 | |||
526 |
1/2✓ Branch 1 taken 539 times.
✗ Branch 2 not taken.
|
1078 | if (cresult->numContacts() < crequest->num_max_contacts) { |
527 |
2/2✓ Branch 0 taken 53 times.
✓ Branch 1 taken 486 times.
|
1078 | if (distToCollision <= crequest->collision_distance_threshold) { |
528 |
1/2✓ Branch 1 taken 53 times.
✗ Branch 2 not taken.
|
106 | cresult->addContact(Contact( |
529 |
2/4✓ Branch 1 taken 53 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 53 times.
✗ Branch 5 not taken.
|
106 | tree1, tree2, (int)(root1 - tree1->getRoot()), |
530 | static_cast<int>(primitive_id), c1, c2, normal, distance)); | ||
531 | } | ||
532 | } | ||
533 |
1/2✓ Branch 1 taken 539 times.
✗ Branch 2 not taken.
|
1078 | return crequest->isSatisfied(*cresult); |
534 | } | ||
535 | |||
536 | // Determine which tree to traverse first. | ||
537 |
4/4✓ Branch 1 taken 8970 times.
✓ Branch 2 taken 2043 times.
✓ Branch 3 taken 5600 times.
✓ Branch 4 taken 5413 times.
|
39966 | if (bvn2.isLeaf() || |
538 |
3/4✓ Branch 1 taken 8970 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3557 times.
✓ Branch 6 taken 5413 times.
|
17940 | (tree1->nodeHasChildren(root1) && (bv1.size() > bvn2.bv.size()))) { |
539 |
2/2✓ Branch 0 taken 42996 times.
✓ Branch 1 taken 4790 times.
|
95572 | for (unsigned int i = 0; i < 8; ++i) { |
540 |
2/2✓ Branch 1 taken 18305 times.
✓ Branch 2 taken 24691 times.
|
85992 | if (tree1->nodeChildExists(root1, i)) { |
541 |
1/2✓ Branch 1 taken 18305 times.
✗ Branch 2 not taken.
|
36610 | const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); |
542 |
1/2✓ Branch 1 taken 18305 times.
✗ Branch 2 not taken.
|
36610 | AABB child_bv; |
543 |
1/2✓ Branch 1 taken 18305 times.
✗ Branch 2 not taken.
|
36610 | computeChildBV(bv1, i, child_bv); |
544 | |||
545 |
3/4✓ Branch 1 taken 18305 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 810 times.
✓ Branch 4 taken 17495 times.
|
36610 | if (OcTreeMeshIntersectRecurse(tree1, child, child_bv, tree2, root2, |
546 | tf1, tf2)) | ||
547 | 1620 | return true; | |
548 | } | ||
549 | } | ||
550 | } else { | ||
551 |
2/2✓ Branch 1 taken 249 times.
✓ Branch 2 taken 5164 times.
|
10826 | if (OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, |
552 | 10826 | (unsigned int)bvn2.leftChild(), tf1, tf2)) | |
553 | 498 | return true; | |
554 | |||
555 |
2/2✓ Branch 1 taken 147 times.
✓ Branch 2 taken 5017 times.
|
10328 | if (OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, |
556 | 10328 | (unsigned int)bvn2.rightChild(), tf1, tf2)) | |
557 | 294 | return true; | |
558 | } | ||
559 | |||
560 | 19614 | return false; | |
561 | } | ||
562 | |||
563 | /// \return True if the request is satisfied. | ||
564 | template <typename BV> | ||
565 | 56584 | bool OcTreeHeightFieldIntersectRecurse( | |
566 | const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1, | ||
567 | const HeightField<BV>* tree2, unsigned int root2, const Transform3s& tf1, | ||
568 | const Transform3s& tf2, Scalar& sqrDistLowerBound) const { | ||
569 | // FIXME(jmirabel) I do not understand why the BVHModel was traversed. The | ||
570 | // code in this if(!root1) did not output anything so the empty OcTree is | ||
571 | // considered free. Should an empty OcTree be considered free ? | ||
572 | |||
573 | // Empty OcTree is considered free. | ||
574 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 28292 times.
|
56584 | if (!root1) return false; |
575 | 56584 | HFNode<BV> const& bvn2 = tree2->getBV(root2); | |
576 | |||
577 | /// stop when 1) bounding boxes of two objects not overlap; OR | ||
578 | /// 2) at least one of the nodes is free; OR | ||
579 | /// 2) (two uncertain nodes OR one node occupied and one node | ||
580 | /// uncertain) AND cost not required | ||
581 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 28292 times.
|
56584 | if (tree1->isNodeFree(root1)) |
582 | ✗ | return false; | |
583 |
3/6✓ Branch 1 taken 28292 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 28292 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 28292 times.
|
56584 | else if ((tree1->isNodeUncertain(root1) || tree2->isUncertain())) |
584 | ✗ | return false; | |
585 | else { | ||
586 |
2/4✓ Branch 1 taken 28292 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 28292 times.
✗ Branch 5 not taken.
|
56584 | OBB obb1, obb2; |
587 |
1/2✓ Branch 1 taken 28292 times.
✗ Branch 2 not taken.
|
56584 | convertBV(bv1, tf1, obb1); |
588 |
1/2✓ Branch 1 taken 28292 times.
✗ Branch 2 not taken.
|
56584 | convertBV(bvn2.bv, tf2, obb2); |
589 | Scalar sqrDistLowerBound_; | ||
590 |
3/4✓ Branch 1 taken 28292 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 17800 times.
✓ Branch 4 taken 10492 times.
|
56584 | if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound_)) { |
591 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 17800 times.
|
35600 | if (sqrDistLowerBound_ < sqrDistLowerBound) |
592 | ✗ | sqrDistLowerBound = sqrDistLowerBound_; | |
593 | 35600 | internal::updateDistanceLowerBoundFromBV(*crequest, *cresult, | |
594 | sqrDistLowerBound); | ||
595 | 35600 | return false; | |
596 | } | ||
597 | } | ||
598 | |||
599 | // Check if leaf collides. | ||
600 |
6/6✓ Branch 1 taken 221 times.
✓ Branch 2 taken 10271 times.
✓ Branch 4 taken 16 times.
✓ Branch 5 taken 205 times.
✓ Branch 6 taken 16 times.
✓ Branch 7 taken 10476 times.
|
20984 | if (!tree1->nodeHasChildren(root1) && bvn2.isLeaf()) { |
601 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 16 times.
|
32 | assert(tree1->isNodeOccupied(root1)); // it isn't free nor uncertain. |
602 |
1/2✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
|
32 | Box box; |
603 |
1/2✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
|
32 | Transform3s box_tf; |
604 |
1/2✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
|
32 | constructBox(bv1, tf1, box, box_tf); |
605 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 16 times.
|
32 | if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) { |
606 | ✗ | box.computeLocalAABB(); | |
607 | } | ||
608 | |||
609 | typedef Convex<Triangle> ConvexTriangle; | ||
610 |
2/4✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
|
32 | ConvexTriangle convex1, convex2; |
611 | int convex1_active_faces, convex2_active_faces; | ||
612 |
1/2✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
|
32 | details::buildConvexTriangles(bvn2, *tree2, convex1, convex1_active_faces, |
613 | convex2, convex2_active_faces); | ||
614 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 16 times.
|
32 | if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) { |
615 | ✗ | convex1.computeLocalAABB(); | |
616 | ✗ | convex2.computeLocalAABB(); | |
617 | } | ||
618 | |||
619 |
4/8✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 16 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 16 times.
✗ Branch 11 not taken.
|
32 | Vec3s c1, c2, normal, normal_top; |
620 | Scalar distance; | ||
621 | bool hfield_witness_is_on_bin_side; | ||
622 | |||
623 | 64 | bool collision = details::shapeDistance<Triangle, Box, 0>( | |
624 |
1/2✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
|
32 | solver, *crequest, convex1, convex1_active_faces, convex2, |
625 | convex2_active_faces, tf2, box, box_tf, distance, c2, c1, normal, | ||
626 | normal_top, hfield_witness_is_on_bin_side); | ||
627 | |||
628 | 32 | Scalar distToCollision = | |
629 |
1/2✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
|
32 | distance - crequest->security_margin * (normal_top.dot(normal)); |
630 | |||
631 |
1/2✓ Branch 0 taken 16 times.
✗ Branch 1 not taken.
|
32 | if (distToCollision <= crequest->collision_distance_threshold) { |
632 | 32 | sqrDistLowerBound = 0; | |
633 |
1/2✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
|
32 | if (crequest->num_max_contacts > cresult->numContacts()) { |
634 |
4/8✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 16 times.
✓ Branch 8 taken 16 times.
✗ Branch 9 not taken.
|
32 | if (normal_top.isApprox(normal) && |
635 | ✗ | (collision || !hfield_witness_is_on_bin_side)) { | |
636 |
1/2✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
|
32 | cresult->addContact( |
637 |
4/8✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 16 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 16 times.
✗ Branch 11 not taken.
|
64 | Contact(tree1, tree2, (int)(root1 - tree1->getRoot()), |
638 | (int)Contact::NONE, c1, c2, -normal, distance)); | ||
639 | } | ||
640 | } | ||
641 | } else | ||
642 | ✗ | sqrDistLowerBound = distToCollision * distToCollision; | |
643 | |||
644 | // const Vec3s c1 = contact_point - distance * 0.5 * normal; | ||
645 | // const Vec3s c2 = contact_point + distance * 0.5 * normal; | ||
646 |
2/4✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
|
32 | internal::updateDistanceLowerBoundFromLeaf( |
647 |
1/2✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
|
32 | *crequest, *cresult, distToCollision, c1, c2, -normal); |
648 | |||
649 |
1/4✗ Branch 1 not taken.
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
|
32 | assert(cresult->isCollision() || sqrDistLowerBound > 0); |
650 |
1/2✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
|
32 | return crequest->isSatisfied(*cresult); |
651 | } | ||
652 | |||
653 | // Determine which tree to traverse first. | ||
654 |
3/4✓ Branch 1 taken 10476 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 10271 times.
✓ Branch 4 taken 205 times.
|
41904 | if (bvn2.isLeaf() || |
655 |
3/4✓ Branch 1 taken 10271 times.
✓ Branch 2 taken 205 times.
✓ Branch 5 taken 10271 times.
✗ Branch 6 not taken.
|
20952 | (tree1->nodeHasChildren(root1) && (bv1.size() > bvn2.bv.size()))) { |
656 |
2/2✓ Branch 0 taken 81389 times.
✓ Branch 1 taken 10025 times.
|
182828 | for (unsigned int i = 0; i < 8; ++i) { |
657 |
2/2✓ Branch 1 taken 27026 times.
✓ Branch 2 taken 54363 times.
|
162778 | if (tree1->nodeChildExists(root1, i)) { |
658 |
1/2✓ Branch 1 taken 27026 times.
✗ Branch 2 not taken.
|
54052 | const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); |
659 |
1/2✓ Branch 1 taken 27026 times.
✗ Branch 2 not taken.
|
54052 | AABB child_bv; |
660 |
1/2✓ Branch 1 taken 27026 times.
✗ Branch 2 not taken.
|
54052 | computeChildBV(bv1, i, child_bv); |
661 | |||
662 |
3/4✓ Branch 1 taken 27026 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 246 times.
✓ Branch 4 taken 26780 times.
|
54052 | if (OcTreeHeightFieldIntersectRecurse(tree1, child, child_bv, tree2, |
663 | root2, tf1, tf2, | ||
664 | sqrDistLowerBound)) | ||
665 | 492 | return true; | |
666 | } | ||
667 | } | ||
668 | } else { | ||
669 |
2/2✓ Branch 1 taken 144 times.
✓ Branch 2 taken 61 times.
|
410 | if (OcTreeHeightFieldIntersectRecurse(tree1, root1, bv1, tree2, |
670 | 410 | (unsigned int)bvn2.leftChild(), tf1, | |
671 | tf2, sqrDistLowerBound)) | ||
672 | 288 | return true; | |
673 | |||
674 |
1/2✓ Branch 1 taken 61 times.
✗ Branch 2 not taken.
|
122 | if (OcTreeHeightFieldIntersectRecurse(tree1, root1, bv1, tree2, |
675 | 122 | (unsigned int)bvn2.rightChild(), | |
676 | tf1, tf2, sqrDistLowerBound)) | ||
677 | 122 | return true; | |
678 | } | ||
679 | |||
680 | 20050 | return false; | |
681 | } | ||
682 | |||
683 | /// \return True if the request is satisfied. | ||
684 | template <typename BV> | ||
685 | 56584 | bool HeightFieldOcTreeIntersectRecurse( | |
686 | const HeightField<BV>* tree1, unsigned int root1, const OcTree* tree2, | ||
687 | const OcTree::OcTreeNode* root2, const AABB& bv2, const Transform3s& tf1, | ||
688 | const Transform3s& tf2, Scalar& sqrDistLowerBound) const { | ||
689 | // FIXME(jmirabel) I do not understand why the BVHModel was traversed. The | ||
690 | // code in this if(!root1) did not output anything so the empty OcTree is | ||
691 | // considered free. Should an empty OcTree be considered free ? | ||
692 | |||
693 | // Empty OcTree is considered free. | ||
694 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 28292 times.
|
56584 | if (!root2) return false; |
695 | 56584 | HFNode<BV> const& bvn1 = tree1->getBV(root1); | |
696 | |||
697 | /// stop when 1) bounding boxes of two objects not overlap; OR | ||
698 | /// 2) at least one of the nodes is free; OR | ||
699 | /// 2) (two uncertain nodes OR one node occupied and one node | ||
700 | /// uncertain) AND cost not required | ||
701 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 28292 times.
|
56584 | if (tree2->isNodeFree(root2)) |
702 | ✗ | return false; | |
703 |
3/6✓ Branch 1 taken 28292 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 28292 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 28292 times.
|
56584 | else if ((tree2->isNodeUncertain(root2) || tree1->isUncertain())) |
704 | ✗ | return false; | |
705 | else { | ||
706 |
2/4✓ Branch 1 taken 28292 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 28292 times.
✗ Branch 5 not taken.
|
56584 | OBB obb1, obb2; |
707 |
1/2✓ Branch 1 taken 28292 times.
✗ Branch 2 not taken.
|
56584 | convertBV(bvn1.bv, tf1, obb1); |
708 |
1/2✓ Branch 1 taken 28292 times.
✗ Branch 2 not taken.
|
56584 | convertBV(bv2, tf2, obb2); |
709 | Scalar sqrDistLowerBound_; | ||
710 |
3/4✓ Branch 1 taken 28292 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 17800 times.
✓ Branch 4 taken 10492 times.
|
56584 | if (!obb2.overlap(obb1, *crequest, sqrDistLowerBound_)) { |
711 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 17800 times.
|
35600 | if (sqrDistLowerBound_ < sqrDistLowerBound) |
712 | ✗ | sqrDistLowerBound = sqrDistLowerBound_; | |
713 | 35600 | internal::updateDistanceLowerBoundFromBV(*crequest, *cresult, | |
714 | sqrDistLowerBound); | ||
715 | 35600 | return false; | |
716 | } | ||
717 | } | ||
718 | |||
719 | // Check if leaf collides. | ||
720 |
6/6✓ Branch 1 taken 221 times.
✓ Branch 2 taken 10271 times.
✓ Branch 4 taken 16 times.
✓ Branch 5 taken 205 times.
✓ Branch 6 taken 16 times.
✓ Branch 7 taken 10476 times.
|
20984 | if (!tree2->nodeHasChildren(root2) && bvn1.isLeaf()) { |
721 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 16 times.
|
32 | assert(tree2->isNodeOccupied(root2)); // it isn't free nor uncertain. |
722 |
1/2✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
|
32 | Box box; |
723 |
1/2✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
|
32 | Transform3s box_tf; |
724 |
1/2✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
|
32 | constructBox(bv2, tf2, box, box_tf); |
725 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 16 times.
|
32 | if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) { |
726 | ✗ | box.computeLocalAABB(); | |
727 | } | ||
728 | |||
729 | typedef Convex<Triangle> ConvexTriangle; | ||
730 |
2/4✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
|
32 | ConvexTriangle convex1, convex2; |
731 | int convex1_active_faces, convex2_active_faces; | ||
732 |
1/2✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
|
32 | details::buildConvexTriangles(bvn1, *tree1, convex1, convex1_active_faces, |
733 | convex2, convex2_active_faces); | ||
734 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 16 times.
|
32 | if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) { |
735 | ✗ | convex1.computeLocalAABB(); | |
736 | ✗ | convex2.computeLocalAABB(); | |
737 | } | ||
738 | |||
739 |
4/8✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 16 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 16 times.
✗ Branch 11 not taken.
|
32 | Vec3s c1, c2, normal, normal_top; |
740 | Scalar distance; | ||
741 | bool hfield_witness_is_on_bin_side; | ||
742 | |||
743 | 64 | bool collision = details::shapeDistance<Triangle, Box, 0>( | |
744 |
1/2✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
|
32 | solver, *crequest, convex1, convex1_active_faces, convex2, |
745 | convex2_active_faces, tf1, box, box_tf, distance, c1, c2, normal, | ||
746 | normal_top, hfield_witness_is_on_bin_side); | ||
747 | |||
748 | 32 | Scalar distToCollision = | |
749 |
1/2✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
|
32 | distance - crequest->security_margin * (normal_top.dot(normal)); |
750 | |||
751 |
1/2✓ Branch 0 taken 16 times.
✗ Branch 1 not taken.
|
32 | if (distToCollision <= crequest->collision_distance_threshold) { |
752 | 32 | sqrDistLowerBound = 0; | |
753 |
1/2✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
|
32 | if (crequest->num_max_contacts > cresult->numContacts()) { |
754 |
4/8✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 16 times.
✓ Branch 8 taken 16 times.
✗ Branch 9 not taken.
|
32 | if (normal_top.isApprox(normal) && |
755 | ✗ | (collision || !hfield_witness_is_on_bin_side)) { | |
756 |
1/2✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
|
32 | cresult->addContact(Contact(tree1, tree2, (int)Contact::NONE, |
757 |
2/4✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
|
32 | (int)(root2 - tree2->getRoot()), c1, c2, |
758 | normal, distance)); | ||
759 | } | ||
760 | } | ||
761 | } else | ||
762 | ✗ | sqrDistLowerBound = distToCollision * distToCollision; | |
763 | |||
764 | // const Vec3s c1 = contact_point - distance * 0.5 * normal; | ||
765 | // const Vec3s c2 = contact_point + distance * 0.5 * normal; | ||
766 | 32 | internal::updateDistanceLowerBoundFromLeaf( | |
767 |
1/2✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
|
32 | *crequest, *cresult, distToCollision, c1, c2, normal); |
768 | |||
769 |
1/4✗ Branch 1 not taken.
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
|
32 | assert(cresult->isCollision() || sqrDistLowerBound > 0); |
770 |
1/2✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
|
32 | return crequest->isSatisfied(*cresult); |
771 | } | ||
772 | |||
773 | // Determine which tree to traverse first. | ||
774 |
3/4✓ Branch 1 taken 10476 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 10271 times.
✓ Branch 4 taken 205 times.
|
41904 | if (bvn1.isLeaf() || |
775 |
3/4✓ Branch 1 taken 10271 times.
✓ Branch 2 taken 205 times.
✓ Branch 5 taken 10271 times.
✗ Branch 6 not taken.
|
20952 | (tree2->nodeHasChildren(root2) && (bv2.size() > bvn1.bv.size()))) { |
776 |
2/2✓ Branch 0 taken 81389 times.
✓ Branch 1 taken 10025 times.
|
182828 | for (unsigned int i = 0; i < 8; ++i) { |
777 |
2/2✓ Branch 1 taken 27026 times.
✓ Branch 2 taken 54363 times.
|
162778 | if (tree2->nodeChildExists(root2, i)) { |
778 |
1/2✓ Branch 1 taken 27026 times.
✗ Branch 2 not taken.
|
54052 | const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); |
779 |
1/2✓ Branch 1 taken 27026 times.
✗ Branch 2 not taken.
|
54052 | AABB child_bv; |
780 |
1/2✓ Branch 1 taken 27026 times.
✗ Branch 2 not taken.
|
54052 | computeChildBV(bv2, i, child_bv); |
781 | |||
782 |
3/4✓ Branch 1 taken 27026 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 246 times.
✓ Branch 4 taken 26780 times.
|
54052 | if (HeightFieldOcTreeIntersectRecurse(tree1, root1, tree2, child, |
783 | child_bv, tf1, tf2, | ||
784 | sqrDistLowerBound)) | ||
785 | 492 | return true; | |
786 | } | ||
787 | } | ||
788 | } else { | ||
789 |
2/2✓ Branch 1 taken 144 times.
✓ Branch 2 taken 61 times.
|
410 | if (HeightFieldOcTreeIntersectRecurse( |
790 | 410 | tree1, (unsigned int)bvn1.leftChild(), tree2, root2, bv2, tf1, | |
791 | tf2, sqrDistLowerBound)) | ||
792 | 288 | return true; | |
793 | |||
794 |
1/2✓ Branch 1 taken 61 times.
✗ Branch 2 not taken.
|
122 | if (HeightFieldOcTreeIntersectRecurse( |
795 | 122 | tree1, (unsigned int)bvn1.rightChild(), tree2, root2, bv2, tf1, | |
796 | tf2, sqrDistLowerBound)) | ||
797 | 122 | return true; | |
798 | } | ||
799 | |||
800 | 20050 | return false; | |
801 | } | ||
802 | |||
803 | ✗ | bool OcTreeDistanceRecurse(const OcTree* tree1, | |
804 | const OcTree::OcTreeNode* root1, const AABB& bv1, | ||
805 | const OcTree* tree2, | ||
806 | const OcTree::OcTreeNode* root2, const AABB& bv2, | ||
807 | const Transform3s& tf1, | ||
808 | const Transform3s& tf2) const { | ||
809 | ✗ | if (!tree1->nodeHasChildren(root1) && !tree2->nodeHasChildren(root2)) { | |
810 | ✗ | if (tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2)) { | |
811 | ✗ | Box box1, box2; | |
812 | ✗ | Transform3s box1_tf, box2_tf; | |
813 | ✗ | constructBox(bv1, tf1, box1, box1_tf); | |
814 | ✗ | constructBox(bv2, tf2, box2, box2_tf); | |
815 | |||
816 | ✗ | if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) { | |
817 | ✗ | box1.computeLocalAABB(); | |
818 | ✗ | box2.computeLocalAABB(); | |
819 | } | ||
820 | |||
821 | ✗ | Vec3s p1, p2, normal; | |
822 | ✗ | const Scalar distance = internal::ShapeShapeDistance<Box, Box>( | |
823 | ✗ | &box1, box1_tf, &box2, box2_tf, this->solver, | |
824 | ✗ | this->drequest->enable_signed_distance, p1, p2, normal); | |
825 | |||
826 | ✗ | this->dresult->update(distance, tree1, tree2, | |
827 | ✗ | (int)(root1 - tree1->getRoot()), | |
828 | ✗ | (int)(root2 - tree2->getRoot()), p1, p2, normal); | |
829 | |||
830 | ✗ | return drequest->isSatisfied(*dresult); | |
831 | ✗ | } else | |
832 | ✗ | return false; | |
833 | } | ||
834 | |||
835 | ✗ | if (!tree1->isNodeOccupied(root1) || !tree2->isNodeOccupied(root2)) | |
836 | ✗ | return false; | |
837 | |||
838 | ✗ | if (!tree2->nodeHasChildren(root2) || | |
839 | ✗ | (tree1->nodeHasChildren(root1) && (bv1.size() > bv2.size()))) { | |
840 | ✗ | for (unsigned int i = 0; i < 8; ++i) { | |
841 | ✗ | if (tree1->nodeChildExists(root1, i)) { | |
842 | ✗ | const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); | |
843 | ✗ | AABB child_bv; | |
844 | ✗ | computeChildBV(bv1, i, child_bv); | |
845 | |||
846 | Scalar d; | ||
847 | ✗ | AABB aabb1, aabb2; | |
848 | ✗ | convertBV(bv1, tf1, aabb1); | |
849 | ✗ | convertBV(bv2, tf2, aabb2); | |
850 | ✗ | d = aabb1.distance(aabb2); | |
851 | |||
852 | ✗ | if (d < dresult->min_distance) { | |
853 | ✗ | if (OcTreeDistanceRecurse(tree1, child, child_bv, tree2, root2, bv2, | |
854 | tf1, tf2)) | ||
855 | ✗ | return true; | |
856 | } | ||
857 | } | ||
858 | } | ||
859 | } else { | ||
860 | ✗ | for (unsigned int i = 0; i < 8; ++i) { | |
861 | ✗ | if (tree2->nodeChildExists(root2, i)) { | |
862 | ✗ | const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); | |
863 | ✗ | AABB child_bv; | |
864 | ✗ | computeChildBV(bv2, i, child_bv); | |
865 | |||
866 | Scalar d; | ||
867 | ✗ | AABB aabb1, aabb2; | |
868 | ✗ | convertBV(bv1, tf1, aabb1); | |
869 | ✗ | convertBV(bv2, tf2, aabb2); | |
870 | ✗ | d = aabb1.distance(aabb2); | |
871 | |||
872 | ✗ | if (d < dresult->min_distance) { | |
873 | ✗ | if (OcTreeDistanceRecurse(tree1, root1, bv1, tree2, child, child_bv, | |
874 | tf1, tf2)) | ||
875 | ✗ | return true; | |
876 | } | ||
877 | } | ||
878 | } | ||
879 | } | ||
880 | |||
881 | ✗ | return false; | |
882 | } | ||
883 | |||
884 | ✗ | bool OcTreeIntersectRecurse(const OcTree* tree1, | |
885 | const OcTree::OcTreeNode* root1, const AABB& bv1, | ||
886 | const OcTree* tree2, | ||
887 | const OcTree::OcTreeNode* root2, const AABB& bv2, | ||
888 | const Transform3s& tf1, | ||
889 | const Transform3s& tf2) const { | ||
890 | // Empty OcTree is considered free. | ||
891 | ✗ | if (!root1) return false; | |
892 | ✗ | if (!root2) return false; | |
893 | |||
894 | /// stop when 1) bounding boxes of two objects not overlap; OR | ||
895 | /// 2) at least one of the nodes is free; OR | ||
896 | /// 2) (two uncertain nodes OR one node occupied and one node | ||
897 | /// uncertain) AND cost not required | ||
898 | ✗ | if (tree1->isNodeFree(root1) || tree2->isNodeFree(root2)) | |
899 | ✗ | return false; | |
900 | ✗ | else if ((tree1->isNodeUncertain(root1) || tree2->isNodeUncertain(root2))) | |
901 | ✗ | return false; | |
902 | |||
903 | bool bothAreLeaves = | ||
904 | ✗ | (!tree1->nodeHasChildren(root1) && !tree2->nodeHasChildren(root2)); | |
905 | ✗ | if (!bothAreLeaves || !crequest->enable_contact) { | |
906 | ✗ | OBB obb1, obb2; | |
907 | ✗ | convertBV(bv1, tf1, obb1); | |
908 | ✗ | convertBV(bv2, tf2, obb2); | |
909 | Scalar sqrDistLowerBound; | ||
910 | ✗ | if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound)) { | |
911 | ✗ | if (cresult->distance_lower_bound > 0 && | |
912 | ✗ | sqrDistLowerBound < | |
913 | ✗ | cresult->distance_lower_bound * cresult->distance_lower_bound) | |
914 | ✗ | cresult->distance_lower_bound = | |
915 | ✗ | sqrt(sqrDistLowerBound) - crequest->security_margin; | |
916 | ✗ | return false; | |
917 | } | ||
918 | ✗ | if (crequest->enable_contact) { // Overlap | |
919 | ✗ | if (cresult->numContacts() < crequest->num_max_contacts) | |
920 | ✗ | cresult->addContact( | |
921 | ✗ | Contact(tree1, tree2, static_cast<int>(root1 - tree1->getRoot()), | |
922 | ✗ | static_cast<int>(root2 - tree2->getRoot()))); | |
923 | ✗ | return crequest->isSatisfied(*cresult); | |
924 | } | ||
925 | } | ||
926 | |||
927 | // Both node are leaves | ||
928 | ✗ | if (bothAreLeaves) { | |
929 | ✗ | assert(tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2)); | |
930 | |||
931 | ✗ | Box box1, box2; | |
932 | ✗ | Transform3s box1_tf, box2_tf; | |
933 | ✗ | constructBox(bv1, tf1, box1, box1_tf); | |
934 | ✗ | constructBox(bv2, tf2, box2, box2_tf); | |
935 | |||
936 | ✗ | if (this->solver->gjk_initial_guess == | |
937 | GJKInitialGuess::BoundingVolumeGuess) { | ||
938 | ✗ | box1.computeLocalAABB(); | |
939 | ✗ | box2.computeLocalAABB(); | |
940 | } | ||
941 | |||
942 | // When reaching this point, `this->solver` has already been set up | ||
943 | // by the CollisionRequest `this->request`. | ||
944 | // The only thing we need to (and can) pass to `ShapeShapeDistance` is | ||
945 | // whether or not penetration information is should be computed in case of | ||
946 | // collision. | ||
947 | ✗ | const bool compute_penetration = (this->crequest->enable_contact || | |
948 | ✗ | (this->crequest->security_margin < 0)); | |
949 | ✗ | Vec3s c1, c2, normal; | |
950 | ✗ | Scalar distance = internal::ShapeShapeDistance<Box, Box>( | |
951 | ✗ | &box1, box1_tf, &box2, box2_tf, this->solver, compute_penetration, c1, | |
952 | c2, normal); | ||
953 | |||
954 | ✗ | const Scalar distToCollision = distance - this->crequest->security_margin; | |
955 | |||
956 | ✗ | internal::updateDistanceLowerBoundFromLeaf( | |
957 | ✗ | *(this->crequest), *(this->cresult), distToCollision, c1, c2, normal); | |
958 | |||
959 | ✗ | if (this->cresult->numContacts() < this->crequest->num_max_contacts) { | |
960 | ✗ | if (distToCollision <= this->crequest->collision_distance_threshold) | |
961 | ✗ | this->cresult->addContact( | |
962 | ✗ | Contact(tree1, tree2, static_cast<int>(root1 - tree1->getRoot()), | |
963 | ✗ | static_cast<int>(root2 - tree2->getRoot()), c1, c2, | |
964 | normal, distance)); | ||
965 | } | ||
966 | |||
967 | ✗ | return crequest->isSatisfied(*cresult); | |
968 | } | ||
969 | |||
970 | // Determine which tree to traverse first. | ||
971 | ✗ | if (!tree2->nodeHasChildren(root2) || | |
972 | ✗ | (tree1->nodeHasChildren(root1) && (bv1.size() > bv2.size()))) { | |
973 | ✗ | for (unsigned int i = 0; i < 8; ++i) { | |
974 | ✗ | if (tree1->nodeChildExists(root1, i)) { | |
975 | ✗ | const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); | |
976 | ✗ | AABB child_bv; | |
977 | ✗ | computeChildBV(bv1, i, child_bv); | |
978 | |||
979 | ✗ | if (OcTreeIntersectRecurse(tree1, child, child_bv, tree2, root2, bv2, | |
980 | tf1, tf2)) | ||
981 | ✗ | return true; | |
982 | } | ||
983 | } | ||
984 | } else { | ||
985 | ✗ | for (unsigned int i = 0; i < 8; ++i) { | |
986 | ✗ | if (tree2->nodeChildExists(root2, i)) { | |
987 | ✗ | const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); | |
988 | ✗ | AABB child_bv; | |
989 | ✗ | computeChildBV(bv2, i, child_bv); | |
990 | |||
991 | ✗ | if (OcTreeIntersectRecurse(tree1, root1, bv1, tree2, child, child_bv, | |
992 | tf1, tf2)) | ||
993 | ✗ | return true; | |
994 | } | ||
995 | } | ||
996 | } | ||
997 | |||
998 | ✗ | return false; | |
999 | } | ||
1000 | }; | ||
1001 | |||
1002 | /// @addtogroup Traversal_For_Collision | ||
1003 | /// @{ | ||
1004 | |||
1005 | /// @brief Traversal node for octree collision | ||
1006 | class COAL_DLLAPI OcTreeCollisionTraversalNode | ||
1007 | : public CollisionTraversalNodeBase { | ||
1008 | public: | ||
1009 | ✗ | OcTreeCollisionTraversalNode(const CollisionRequest& request) | |
1010 | ✗ | : CollisionTraversalNodeBase(request) { | |
1011 | ✗ | model1 = NULL; | |
1012 | ✗ | model2 = NULL; | |
1013 | |||
1014 | ✗ | otsolver = NULL; | |
1015 | } | ||
1016 | |||
1017 | ✗ | bool BVDisjoints(unsigned, unsigned, Scalar&) const { return false; } | |
1018 | |||
1019 | ✗ | void leafCollides(unsigned, unsigned, Scalar& sqrDistLowerBound) const { | |
1020 | ✗ | otsolver->OcTreeIntersect(model1, model2, tf1, tf2, request, *result); | |
1021 | ✗ | sqrDistLowerBound = std::max((Scalar)0, result->distance_lower_bound); | |
1022 | ✗ | sqrDistLowerBound *= sqrDistLowerBound; | |
1023 | } | ||
1024 | |||
1025 | const OcTree* model1; | ||
1026 | const OcTree* model2; | ||
1027 | |||
1028 | Transform3s tf1, tf2; | ||
1029 | |||
1030 | const OcTreeSolver* otsolver; | ||
1031 | }; | ||
1032 | |||
1033 | /// @brief Traversal node for shape-octree collision | ||
1034 | template <typename S> | ||
1035 | class COAL_DLLAPI ShapeOcTreeCollisionTraversalNode | ||
1036 | : public CollisionTraversalNodeBase { | ||
1037 | public: | ||
1038 | ✗ | ShapeOcTreeCollisionTraversalNode(const CollisionRequest& request) | |
1039 | ✗ | : CollisionTraversalNodeBase(request) { | |
1040 | ✗ | model1 = NULL; | |
1041 | ✗ | model2 = NULL; | |
1042 | |||
1043 | ✗ | otsolver = NULL; | |
1044 | } | ||
1045 | |||
1046 | ✗ | bool BVDisjoints(unsigned int, unsigned int, Scalar&) const { return false; } | |
1047 | |||
1048 | ✗ | void leafCollides(unsigned int, unsigned int, | |
1049 | Scalar& sqrDistLowerBound) const { | ||
1050 | ✗ | otsolver->OcTreeShapeIntersect(model2, *model1, tf2, tf1, request, *result); | |
1051 | ✗ | sqrDistLowerBound = std::max((Scalar)0, result->distance_lower_bound); | |
1052 | ✗ | sqrDistLowerBound *= sqrDistLowerBound; | |
1053 | } | ||
1054 | |||
1055 | const S* model1; | ||
1056 | const OcTree* model2; | ||
1057 | |||
1058 | Transform3s tf1, tf2; | ||
1059 | |||
1060 | const OcTreeSolver* otsolver; | ||
1061 | }; | ||
1062 | |||
1063 | /// @brief Traversal node for octree-shape collision | ||
1064 | |||
1065 | template <typename S> | ||
1066 | class COAL_DLLAPI OcTreeShapeCollisionTraversalNode | ||
1067 | : public CollisionTraversalNodeBase { | ||
1068 | public: | ||
1069 | 2000 | OcTreeShapeCollisionTraversalNode(const CollisionRequest& request) | |
1070 |
2/4✓ Branch 2 taken 1000 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1000 times.
✗ Branch 6 not taken.
|
2000 | : CollisionTraversalNodeBase(request) { |
1071 | 2000 | model1 = NULL; | |
1072 | 2000 | model2 = NULL; | |
1073 | |||
1074 | 2000 | otsolver = NULL; | |
1075 | } | ||
1076 | |||
1077 | ✗ | bool BVDisjoints(unsigned int, unsigned int, coal::Scalar&) const { | |
1078 | ✗ | return false; | |
1079 | } | ||
1080 | |||
1081 | 2000 | void leafCollides(unsigned int, unsigned int, | |
1082 | Scalar& sqrDistLowerBound) const { | ||
1083 | 2000 | otsolver->OcTreeShapeIntersect(model1, *model2, tf1, tf2, request, *result); | |
1084 | 2000 | sqrDistLowerBound = std::max((Scalar)0, result->distance_lower_bound); | |
1085 | 2000 | sqrDistLowerBound *= sqrDistLowerBound; | |
1086 | } | ||
1087 | |||
1088 | const OcTree* model1; | ||
1089 | const S* model2; | ||
1090 | |||
1091 | Transform3s tf1, tf2; | ||
1092 | |||
1093 | const OcTreeSolver* otsolver; | ||
1094 | }; | ||
1095 | |||
1096 | /// @brief Traversal node for mesh-octree collision | ||
1097 | template <typename BV> | ||
1098 | class COAL_DLLAPI MeshOcTreeCollisionTraversalNode | ||
1099 | : public CollisionTraversalNodeBase { | ||
1100 | public: | ||
1101 | 200 | MeshOcTreeCollisionTraversalNode(const CollisionRequest& request) | |
1102 |
2/4✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
|
200 | : CollisionTraversalNodeBase(request) { |
1103 | 200 | model1 = NULL; | |
1104 | 200 | model2 = NULL; | |
1105 | |||
1106 | 200 | otsolver = NULL; | |
1107 | } | ||
1108 | |||
1109 | ✗ | bool BVDisjoints(unsigned int, unsigned int, Scalar&) const { return false; } | |
1110 | |||
1111 | 200 | void leafCollides(unsigned int, unsigned int, | |
1112 | Scalar& sqrDistLowerBound) const { | ||
1113 | 200 | otsolver->OcTreeMeshIntersect(model2, model1, tf2, tf1, request, *result); | |
1114 | 200 | sqrDistLowerBound = std::max((Scalar)0, result->distance_lower_bound); | |
1115 | 200 | sqrDistLowerBound *= sqrDistLowerBound; | |
1116 | } | ||
1117 | |||
1118 | const BVHModel<BV>* model1; | ||
1119 | const OcTree* model2; | ||
1120 | |||
1121 | Transform3s tf1, tf2; | ||
1122 | |||
1123 | const OcTreeSolver* otsolver; | ||
1124 | }; | ||
1125 | |||
1126 | /// @brief Traversal node for octree-mesh collision | ||
1127 | template <typename BV> | ||
1128 | class COAL_DLLAPI OcTreeMeshCollisionTraversalNode | ||
1129 | : public CollisionTraversalNodeBase { | ||
1130 | public: | ||
1131 | ✗ | OcTreeMeshCollisionTraversalNode(const CollisionRequest& request) | |
1132 | ✗ | : CollisionTraversalNodeBase(request) { | |
1133 | ✗ | model1 = NULL; | |
1134 | ✗ | model2 = NULL; | |
1135 | |||
1136 | ✗ | otsolver = NULL; | |
1137 | } | ||
1138 | |||
1139 | ✗ | bool BVDisjoints(unsigned int, unsigned int, Scalar&) const { return false; } | |
1140 | |||
1141 | ✗ | void leafCollides(unsigned int, unsigned int, | |
1142 | Scalar& sqrDistLowerBound) const { | ||
1143 | ✗ | otsolver->OcTreeMeshIntersect(model1, model2, tf1, tf2, request, *result); | |
1144 | ✗ | sqrDistLowerBound = std::max((Scalar)0, result->distance_lower_bound); | |
1145 | ✗ | sqrDistLowerBound *= sqrDistLowerBound; | |
1146 | } | ||
1147 | |||
1148 | const OcTree* model1; | ||
1149 | const BVHModel<BV>* model2; | ||
1150 | |||
1151 | Transform3s tf1, tf2; | ||
1152 | |||
1153 | const OcTreeSolver* otsolver; | ||
1154 | }; | ||
1155 | |||
1156 | /// @brief Traversal node for octree-height-field collision | ||
1157 | template <typename BV> | ||
1158 | class COAL_DLLAPI OcTreeHeightFieldCollisionTraversalNode | ||
1159 | : public CollisionTraversalNodeBase { | ||
1160 | public: | ||
1161 | 2000 | OcTreeHeightFieldCollisionTraversalNode(const CollisionRequest& request) | |
1162 |
2/4✓ Branch 2 taken 1000 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1000 times.
✗ Branch 6 not taken.
|
2000 | : CollisionTraversalNodeBase(request) { |
1163 | 2000 | model1 = NULL; | |
1164 | 2000 | model2 = NULL; | |
1165 | |||
1166 | 2000 | otsolver = NULL; | |
1167 | } | ||
1168 | |||
1169 | ✗ | bool BVDisjoints(unsigned int, unsigned int, Scalar&) const { return false; } | |
1170 | |||
1171 | 2000 | void leafCollides(unsigned int, unsigned int, | |
1172 | Scalar& sqrDistLowerBound) const { | ||
1173 | 2000 | otsolver->OcTreeHeightFieldIntersect(model1, model2, tf1, tf2, request, | |
1174 | 2000 | *result, sqrDistLowerBound); | |
1175 | } | ||
1176 | |||
1177 | const OcTree* model1; | ||
1178 | const HeightField<BV>* model2; | ||
1179 | |||
1180 | Transform3s tf1, tf2; | ||
1181 | |||
1182 | const OcTreeSolver* otsolver; | ||
1183 | }; | ||
1184 | |||
1185 | /// @brief Traversal node for octree-height-field collision | ||
1186 | template <typename BV> | ||
1187 | class COAL_DLLAPI HeightFieldOcTreeCollisionTraversalNode | ||
1188 | : public CollisionTraversalNodeBase { | ||
1189 | public: | ||
1190 | 2000 | HeightFieldOcTreeCollisionTraversalNode(const CollisionRequest& request) | |
1191 |
2/4✓ Branch 2 taken 1000 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1000 times.
✗ Branch 6 not taken.
|
2000 | : CollisionTraversalNodeBase(request) { |
1192 | 2000 | model1 = NULL; | |
1193 | 2000 | model2 = NULL; | |
1194 | |||
1195 | 2000 | otsolver = NULL; | |
1196 | } | ||
1197 | |||
1198 | ✗ | bool BVDisjoints(unsigned int, unsigned int, Scalar&) const { return false; } | |
1199 | |||
1200 | 2000 | void leafCollides(unsigned int, unsigned int, | |
1201 | Scalar& sqrDistLowerBound) const { | ||
1202 | 2000 | otsolver->HeightFieldOcTreeIntersect(model1, model2, tf1, tf2, request, | |
1203 | 2000 | *result, sqrDistLowerBound); | |
1204 | } | ||
1205 | |||
1206 | const HeightField<BV>* model1; | ||
1207 | const OcTree* model2; | ||
1208 | |||
1209 | Transform3s tf1, tf2; | ||
1210 | |||
1211 | const OcTreeSolver* otsolver; | ||
1212 | }; | ||
1213 | |||
1214 | /// @} | ||
1215 | |||
1216 | /// @addtogroup Traversal_For_Distance | ||
1217 | /// @{ | ||
1218 | |||
1219 | /// @brief Traversal node for octree distance | ||
1220 | class COAL_DLLAPI OcTreeDistanceTraversalNode | ||
1221 | : public DistanceTraversalNodeBase { | ||
1222 | public: | ||
1223 | ✗ | OcTreeDistanceTraversalNode() { | |
1224 | ✗ | model1 = NULL; | |
1225 | ✗ | model2 = NULL; | |
1226 | |||
1227 | ✗ | otsolver = NULL; | |
1228 | } | ||
1229 | |||
1230 | ✗ | Scalar BVDistanceLowerBound(unsigned, unsigned) const { return -1; } | |
1231 | |||
1232 | bool BVDistanceLowerBound(unsigned, unsigned, Scalar&) const { return false; } | ||
1233 | |||
1234 | ✗ | void leafComputeDistance(unsigned, unsigned int) const { | |
1235 | ✗ | otsolver->OcTreeDistance(model1, model2, tf1, tf2, request, *result); | |
1236 | } | ||
1237 | |||
1238 | const OcTree* model1; | ||
1239 | const OcTree* model2; | ||
1240 | |||
1241 | const OcTreeSolver* otsolver; | ||
1242 | }; | ||
1243 | |||
1244 | /// @brief Traversal node for shape-octree distance | ||
1245 | template <typename Shape> | ||
1246 | class COAL_DLLAPI ShapeOcTreeDistanceTraversalNode | ||
1247 | : public DistanceTraversalNodeBase { | ||
1248 | public: | ||
1249 | ✗ | ShapeOcTreeDistanceTraversalNode() { | |
1250 | ✗ | model1 = NULL; | |
1251 | ✗ | model2 = NULL; | |
1252 | |||
1253 | ✗ | otsolver = NULL; | |
1254 | } | ||
1255 | |||
1256 | ✗ | Scalar BVDistanceLowerBound(unsigned int, unsigned int) const { return -1; } | |
1257 | |||
1258 | ✗ | void leafComputeDistance(unsigned int, unsigned int) const { | |
1259 | ✗ | otsolver->OcTreeShapeDistance(model2, *model1, tf2, tf1, request, *result); | |
1260 | } | ||
1261 | |||
1262 | const Shape* model1; | ||
1263 | const OcTree* model2; | ||
1264 | |||
1265 | const OcTreeSolver* otsolver; | ||
1266 | }; | ||
1267 | |||
1268 | /// @brief Traversal node for octree-shape distance | ||
1269 | template <typename Shape> | ||
1270 | class COAL_DLLAPI OcTreeShapeDistanceTraversalNode | ||
1271 | : public DistanceTraversalNodeBase { | ||
1272 | public: | ||
1273 | ✗ | OcTreeShapeDistanceTraversalNode() { | |
1274 | ✗ | model1 = NULL; | |
1275 | ✗ | model2 = NULL; | |
1276 | |||
1277 | ✗ | otsolver = NULL; | |
1278 | } | ||
1279 | |||
1280 | ✗ | Scalar BVDistanceLowerBound(unsigned int, unsigned int) const { return -1; } | |
1281 | |||
1282 | ✗ | void leafComputeDistance(unsigned int, unsigned int) const { | |
1283 | ✗ | otsolver->OcTreeShapeDistance(model1, *model2, tf1, tf2, request, *result); | |
1284 | } | ||
1285 | |||
1286 | const OcTree* model1; | ||
1287 | const Shape* model2; | ||
1288 | |||
1289 | const OcTreeSolver* otsolver; | ||
1290 | }; | ||
1291 | |||
1292 | /// @brief Traversal node for mesh-octree distance | ||
1293 | template <typename BV> | ||
1294 | class COAL_DLLAPI MeshOcTreeDistanceTraversalNode | ||
1295 | : public DistanceTraversalNodeBase { | ||
1296 | public: | ||
1297 | ✗ | MeshOcTreeDistanceTraversalNode() { | |
1298 | ✗ | model1 = NULL; | |
1299 | ✗ | model2 = NULL; | |
1300 | |||
1301 | ✗ | otsolver = NULL; | |
1302 | } | ||
1303 | |||
1304 | ✗ | Scalar BVDistanceLowerBound(unsigned int, unsigned int) const { return -1; } | |
1305 | |||
1306 | ✗ | void leafComputeDistance(unsigned int, unsigned int) const { | |
1307 | ✗ | otsolver->OcTreeMeshDistance(model2, model1, tf2, tf1, request, *result); | |
1308 | } | ||
1309 | |||
1310 | const BVHModel<BV>* model1; | ||
1311 | const OcTree* model2; | ||
1312 | |||
1313 | const OcTreeSolver* otsolver; | ||
1314 | }; | ||
1315 | |||
1316 | /// @brief Traversal node for octree-mesh distance | ||
1317 | template <typename BV> | ||
1318 | class COAL_DLLAPI OcTreeMeshDistanceTraversalNode | ||
1319 | : public DistanceTraversalNodeBase { | ||
1320 | public: | ||
1321 | ✗ | OcTreeMeshDistanceTraversalNode() { | |
1322 | ✗ | model1 = NULL; | |
1323 | ✗ | model2 = NULL; | |
1324 | |||
1325 | ✗ | otsolver = NULL; | |
1326 | } | ||
1327 | |||
1328 | ✗ | Scalar BVDistanceLowerBound(unsigned int, unsigned int) const { return -1; } | |
1329 | |||
1330 | ✗ | void leafComputeDistance(unsigned int, unsigned int) const { | |
1331 | ✗ | otsolver->OcTreeMeshDistance(model1, model2, tf1, tf2, request, *result); | |
1332 | } | ||
1333 | |||
1334 | const OcTree* model1; | ||
1335 | const BVHModel<BV>* model2; | ||
1336 | |||
1337 | const OcTreeSolver* otsolver; | ||
1338 | }; | ||
1339 | |||
1340 | /// @} | ||
1341 | |||
1342 | } // namespace coal | ||
1343 | |||
1344 | /// @endcond | ||
1345 | |||
1346 | #endif | ||
1347 |