GCC Code Coverage Report


Directory: ./
File: src/BV/AABB.cpp
Date: 2025-04-01 09:23:31
Exec Total Coverage
Lines: 38 92 41.3%
Branches: 29 156 18.6%

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 "coal/BV/AABB.h"
39 #include "coal/shape/geometric_shapes.h"
40 #include "coal/collision_data.h"
41
42 #include <limits>
43
44 namespace coal {
45
46 117705326 AABB::AABB()
47
2/4
✓ Branch 2 taken 117705326 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 117705326 times.
✗ Branch 6 not taken.
117705326 : min_(Vec3s::Constant((std::numeric_limits<Scalar>::max)())),
48
2/4
✓ Branch 2 taken 117705326 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 117705326 times.
✗ Branch 6 not taken.
117705326 max_(Vec3s::Constant(-(std::numeric_limits<Scalar>::max)())) {}
49
50 59166 bool AABB::overlap(const AABB& other, const CollisionRequest& request,
51 Scalar& sqrDistLowerBound) const {
52 59166 const Scalar break_distance_squared =
53 59166 request.break_distance * request.break_distance;
54
55 59166 sqrDistLowerBound =
56
2/4
✓ Branch 2 taken 59166 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 59166 times.
✗ Branch 6 not taken.
59166 (min_ - other.max_ - Vec3s::Constant(request.security_margin))
57
1/2
✓ Branch 1 taken 59166 times.
✗ Branch 2 not taken.
59166 .array()
58
1/2
✓ Branch 1 taken 59166 times.
✗ Branch 2 not taken.
59166 .max(Scalar(0))
59
1/2
✓ Branch 1 taken 59166 times.
✗ Branch 2 not taken.
59166 .matrix()
60
1/2
✓ Branch 1 taken 59166 times.
✗ Branch 2 not taken.
59166 .squaredNorm();
61
2/2
✓ Branch 0 taken 1827 times.
✓ Branch 1 taken 57339 times.
59166 if (sqrDistLowerBound > break_distance_squared) return false;
62
63 57339 sqrDistLowerBound =
64
2/4
✓ Branch 2 taken 57339 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 57339 times.
✗ Branch 6 not taken.
57339 (other.min_ - max_ - Vec3s::Constant(request.security_margin))
65
1/2
✓ Branch 1 taken 57339 times.
✗ Branch 2 not taken.
57339 .array()
66
1/2
✓ Branch 1 taken 57339 times.
✗ Branch 2 not taken.
57339 .max(Scalar(0))
67
1/2
✓ Branch 1 taken 57339 times.
✗ Branch 2 not taken.
57339 .matrix()
68
1/2
✓ Branch 1 taken 57339 times.
✗ Branch 2 not taken.
57339 .squaredNorm();
69
2/2
✓ Branch 0 taken 2982 times.
✓ Branch 1 taken 54357 times.
57339 if (sqrDistLowerBound > break_distance_squared) return false;
70
71 54357 return true;
72 }
73
74 Scalar AABB::distance(const AABB& other, Vec3s* P, Vec3s* Q) const {
75 Scalar result = 0;
76 for (Eigen::DenseIndex i = 0; i < 3; ++i) {
77 const Scalar& amin = min_[i];
78 const Scalar& amax = max_[i];
79 const Scalar& bmin = other.min_[i];
80 const Scalar& bmax = other.max_[i];
81
82 if (amin > bmax) {
83 Scalar delta = bmax - amin;
84 result += delta * delta;
85 if (P && Q) {
86 (*P)[i] = amin;
87 (*Q)[i] = bmax;
88 }
89 } else if (bmin > amax) {
90 Scalar delta = amax - bmin;
91 result += delta * delta;
92 if (P && Q) {
93 (*P)[i] = amax;
94 (*Q)[i] = bmin;
95 }
96 } else {
97 if (P && Q) {
98 if (bmin >= amin) {
99 Scalar t = Scalar(0.5) * (amax + bmin);
100 (*P)[i] = t;
101 (*Q)[i] = t;
102 } else {
103 Scalar t = Scalar(0.5) * (amin + bmax);
104 (*P)[i] = t;
105 (*Q)[i] = t;
106 }
107 }
108 }
109 }
110
111 return std::sqrt(result);
112 }
113
114 2742415 Scalar AABB::distance(const AABB& other) const {
115 2742415 Scalar result = 0;
116
2/2
✓ Branch 0 taken 8227245 times.
✓ Branch 1 taken 2742415 times.
10969660 for (Eigen::DenseIndex i = 0; i < 3; ++i) {
117 8227245 const Scalar& amin = min_[i];
118 8227245 const Scalar& amax = max_[i];
119 8227245 const Scalar& bmin = other.min_[i];
120 8227245 const Scalar& bmax = other.max_[i];
121
122
2/2
✓ Branch 0 taken 3885219 times.
✓ Branch 1 taken 4342026 times.
8227245 if (amin > bmax) {
123 3885219 Scalar delta = bmax - amin;
124 3885219 result += delta * delta;
125
2/2
✓ Branch 0 taken 3999451 times.
✓ Branch 1 taken 342575 times.
4342026 } else if (bmin > amax) {
126 3999451 Scalar delta = amax - bmin;
127 3999451 result += delta * delta;
128 }
129 }
130
131 2742415 return std::sqrt(result);
132 }
133
134 bool overlap(const Matrix3s& R0, const Vec3s& T0, const AABB& b1,
135 const AABB& b2) {
136 AABB bb1(translate(rotate(b1, R0), T0));
137 return bb1.overlap(b2);
138 }
139
140 31643 bool overlap(const Matrix3s& R0, const Vec3s& T0, const AABB& b1,
141 const AABB& b2, const CollisionRequest& request,
142 Scalar& sqrDistLowerBound) {
143
2/4
✓ Branch 1 taken 31643 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 31643 times.
✗ Branch 5 not taken.
31643 AABB bb1(translate(rotate(b1, R0), T0));
144
1/2
✓ Branch 1 taken 31643 times.
✗ Branch 2 not taken.
63286 return bb1.overlap(b2, request, sqrDistLowerBound);
145 }
146
147 bool AABB::overlap(const Plane& p) const {
148 // Convert AABB to a (box, transform) representation and compute the support
149 // points in the directions normal and -normal.
150 // If both points lie on different sides of the plane, there is an overlap
151 // between the AABB and the plane. Otherwise, there is no overlap.
152 const Vec3s halfside = (this->max_ - this->min_) / 2;
153 const Vec3s center = (this->max_ + this->min_) / 2;
154
155 const Vec3s support1 = (p.n.array() > 0).select(halfside, -halfside) + center;
156 const Vec3s support2 =
157 ((-p.n).array() > 0).select(halfside, -halfside) + center;
158
159 const Scalar dist1 = p.n.dot(support1) - p.d;
160 const Scalar dist2 = p.n.dot(support2) - p.d;
161 const int sign1 = (dist1 > 0) ? 1 : -1;
162 const int sign2 = (dist2 > 0) ? 1 : -1;
163
164 if (p.getSweptSphereRadius() > 0) {
165 if (sign1 != sign2) {
166 // Supports are on different sides of the plane. There is an overlap.
167 return true;
168 }
169 // Both supports are on the same side of the plane.
170 // We now need to check if they are on the same side of the plane inflated
171 // by the swept-sphere radius.
172 const Scalar ssr_dist1 = std::abs(dist1) - p.getSweptSphereRadius();
173 const Scalar ssr_dist2 = std::abs(dist2) - p.getSweptSphereRadius();
174 const int ssr_sign1 = (ssr_dist1 > 0) ? 1 : -1;
175 const int ssr_sign2 = (ssr_dist2 > 0) ? 1 : -1;
176 return ssr_sign1 != ssr_sign2;
177 }
178
179 return (sign1 != sign2);
180 }
181
182 bool AABB::overlap(const Halfspace& hs) const {
183 // Convert AABB to a (box, transform) representation and compute the support
184 // points in the direction -normal.
185 // If the support is below the plane defined by the halfspace, there is an
186 // overlap between the AABB and the halfspace. Otherwise, there is no
187 // overlap.
188 Vec3s halfside = (this->max_ - this->min_) / 2;
189 Vec3s center = (this->max_ + this->min_) / 2;
190 Vec3s support = ((-hs.n).array() > 0).select(halfside, -halfside) + center;
191 return (hs.signedDistance(support) < 0);
192 }
193
194 } // namespace coal
195