GCC Code Coverage Report


Directory: ./
File: src/collision_node.cpp
Date: 2025-04-01 09:23:31
Exec Total Coverage
Lines: 23 23 100.0%
Branches: 19 24 79.2%

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 #include <../src/collision_node.h>
39 #include "coal/internal/traversal_recurse.h"
40
41 namespace coal {
42
43 14062 void checkResultLowerBound(const CollisionResult& result,
44 Scalar sqrDistLowerBound) {
45 COAL_UNUSED_VARIABLE(result);
46 14062 const Scalar dummy_precision = std::sqrt(Eigen::NumTraits<Scalar>::epsilon());
47 COAL_UNUSED_VARIABLE(dummy_precision);
48
2/2
✓ Branch 0 taken 2516 times.
✓ Branch 1 taken 11546 times.
14062 if (sqrDistLowerBound == 0) {
49
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 2516 times.
2516 COAL_ASSERT(result.distance_lower_bound <= dummy_precision,
50 "Distance lower bound should not be positive.",
51 std::logic_error);
52 } else {
53
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 11546 times.
11546 COAL_ASSERT(result.distance_lower_bound * result.distance_lower_bound -
54 sqrDistLowerBound <
55 dummy_precision,
56 "Distance lower bound and sqrDistLowerBound should coincide.",
57 std::logic_error);
58 }
59 14062 }
60
61 14125 void collide(CollisionTraversalNodeBase* node, const CollisionRequest& request,
62 CollisionResult& result, BVHFrontList* front_list,
63 bool recursive) {
64
6/6
✓ Branch 0 taken 92 times.
✓ Branch 1 taken 14033 times.
✓ Branch 3 taken 46 times.
✓ Branch 4 taken 46 times.
✓ Branch 5 taken 46 times.
✓ Branch 6 taken 14079 times.
14125 if (front_list && front_list->size() > 0) {
65 46 propagateBVHFrontListCollisionRecurse(node, request, result, front_list);
66 } else {
67 14079 Scalar sqrDistLowerBound = 0;
68
2/2
✓ Branch 0 taken 14067 times.
✓ Branch 1 taken 12 times.
14079 if (recursive)
69
1/2
✓ Branch 1 taken 14067 times.
✗ Branch 2 not taken.
14067 collisionRecurse(node, 0, 0, front_list, sqrDistLowerBound);
70 else
71
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
12 collisionNonRecurse(node, front_list, sqrDistLowerBound);
72
2/2
✓ Branch 1 taken 14062 times.
✓ Branch 2 taken 17 times.
14079 if (!std::isnan(sqrDistLowerBound)) {
73
1/2
✓ Branch 1 taken 14062 times.
✗ Branch 2 not taken.
14062 checkResultLowerBound(result, sqrDistLowerBound);
74 }
75 }
76 14125 }
77
78 7289 void distance(DistanceTraversalNodeBase* node, BVHFrontList* front_list,
79 unsigned int qsize) {
80 7289 node->preprocess();
81
82
2/2
✓ Branch 0 taken 7268 times.
✓ Branch 1 taken 21 times.
7289 if (qsize <= 2)
83 7268 distanceRecurse(node, 0, 0, front_list);
84 else
85 21 distanceQueueRecurse(node, 0, 0, front_list, qsize);
86
87 7289 node->postprocess();
88 7289 }
89
90 } // namespace coal
91