GCC Code Coverage Report

Directory: ./
File: src/BV/kIOS.cpp
Date: 2025-04-01 09:23:31
Exec Total Coverage
Lines: 45 89 50.6%
Branches: 41 142 28.9%
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/kIOS.h"
39 #include "coal/BVH/BVH_utility.h"
40 #include "coal/math/transform.h"
41
42 #include <iostream>
43 #include <limits>
44
45 namespace coal {
46
47 ✗ bool kIOS::overlap(const kIOS& other) const {
48 ✗ for (unsigned int i = 0; i < num_spheres; ++i) {
49 ✗ for (unsigned int j = 0; j < other.num_spheres; ++j) {
50 ✗ Scalar o_dist = (spheres[i].o - other.spheres[j].o).squaredNorm();
51 ✗ Scalar sum_r = spheres[i].r + other.spheres[j].r;
52 ✗ if (o_dist > sum_r * sum_r) return false;
53 }
54 }
55
56 ✗ return obb.overlap(other.obb);
57 }
58
59 2974 bool kIOS::overlap(const kIOS& other, const CollisionRequest& request,
60 Scalar& sqrDistLowerBound) const {
61
2/2
✓ Branch 0 taken 10093 times.
✓ Branch 1 taken 2763 times.
 12856 for (unsigned int i = 0; i < num_spheres; ++i) {
62
2/2
✓ Branch 0 taken 42592 times.
✓ Branch 1 taken 9882 times.
 52474 for (unsigned int j = 0; j < other.num_spheres; ++j) {
63
1/2
✓ Branch 2 taken 42592 times.
✗ Branch 3 not taken.
 42592 Scalar o_dist = (spheres[i].o - other.spheres[j].o).squaredNorm();
64 42592 Scalar sum_r = spheres[i].r + other.spheres[j].r;
65
2/2
✓ Branch 0 taken 211 times.
✓ Branch 1 taken 42381 times.
 42592 if (o_dist > sum_r * sum_r) {
66 211 o_dist = sqrt(o_dist) - sum_r;
67 211 sqrDistLowerBound = o_dist * o_dist;
68 211 return false;
69 }
70 }
71 }
72
73 2763 return obb.overlap(other.obb, request, sqrDistLowerBound);
74 }
75
76 ✗ bool kIOS::contain(const Vec3s& p) const {
77 ✗ for (unsigned int i = 0; i < num_spheres; ++i) {
78 ✗ Scalar r = spheres[i].r;
79 ✗ if ((spheres[i].o - p).squaredNorm() > r * r) return false;
80 }
81
82 ✗ return true;
83 }
84
85 ✗ kIOS& kIOS::operator+=(const Vec3s& p) {
86 ✗ for (unsigned int i = 0; i < num_spheres; ++i) {
87 ✗ Scalar r = spheres[i].r;
88 ✗ Scalar new_r_sqr = (p - spheres[i].o).squaredNorm();
89 ✗ if (new_r_sqr > r * r) {
90 ✗ spheres[i].r = sqrt(new_r_sqr);
91 }
92 }
93
94 ✗ obb += p;
95 ✗ return *this;
96 }
97
98 6537 kIOS kIOS::operator+(const kIOS& other) const {
99 6537 kIOS result;
100 6537 unsigned int new_num_spheres = std::min(num_spheres, other.num_spheres);
101
2/2
✓ Branch 0 taken 19611 times.
✓ Branch 1 taken 6537 times.
 26148 for (unsigned int i = 0; i < new_num_spheres; ++i) {
102 19611 result.spheres[i] = encloseSphere(spheres[i], other.spheres[i]);
103 }
104
105 6537 result.num_spheres = new_num_spheres;
106
107 6537 result.obb = obb + other.obb;
108
109 6537 return result;
110 }
111
112 ✗ Scalar kIOS::width() const { return obb.width(); }
113
114 ✗ Scalar kIOS::height() const { return obb.height(); }
115
116 ✗ Scalar kIOS::depth() const { return obb.depth(); }
117
118 7592 Scalar kIOS::volume() const { return obb.volume(); }
119
120 7592 Scalar kIOS::size() const { return volume(); }
121
122 3106 Scalar kIOS::distance(const kIOS& other, Vec3s* P, Vec3s* Q) const {
123 3106 Scalar d_max = 0;
124 3106 long id_a = -1, id_b = -1;
125
2/2
✓ Branch 0 taken 14932 times.
✓ Branch 1 taken 3106 times.
 18038 for (unsigned int i = 0; i < num_spheres; ++i) {
126
2/2
✓ Branch 0 taken 64232 times.
✓ Branch 1 taken 14932 times.
 79164 for (unsigned int j = 0; j < other.num_spheres; ++j) {
127
1/2
✓ Branch 2 taken 64232 times.
✗ Branch 3 not taken.
 64232 Scalar d = (spheres[i].o - other.spheres[j].o).norm() -
128 64232 (spheres[i].r + other.spheres[j].r);
129
2/2
✓ Branch 0 taken 2271 times.
✓ Branch 1 taken 61961 times.
 64232 if (d_max < d) {
130 2271 d_max = d;
131
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 2271 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 2271 if (P && Q) {
132 ✗ id_a = (long)i;
133 ✗ id_b = (long)j;
134 }
135 }
136 }
137 }
138
139
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 3106 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 3106 if (P && Q) {
140 ✗ if (id_a != -1 && id_b != -1) {
141 ✗ const Vec3s v = spheres[id_a].o - spheres[id_b].o;
142 ✗ Scalar len_v = v.norm();
143 ✗ *P = spheres[id_a].o - v * (spheres[id_a].r / len_v);
144 ✗ *Q = spheres[id_b].o + v * (spheres[id_b].r / len_v);
145 }
146 }
147
148 3106 return d_max;
149 }
150
151 ✗ bool overlap(const Matrix3s& R0, const Vec3s& T0, const kIOS& b1,
152 const kIOS& b2) {
153 ✗ kIOS b2_temp = b2;
154 ✗ for (unsigned int i = 0; i < b2_temp.num_spheres; ++i) {
155 ✗ b2_temp.spheres[i].o.noalias() =
156 ✗ R0.transpose() * (b2_temp.spheres[i].o - T0);
157 }
158
159 ✗ b2_temp.obb.To.noalias() = R0.transpose() * (b2_temp.obb.To - T0);
160 ✗ b2_temp.obb.axes.applyOnTheLeft(R0.transpose());
161
162 ✗ return b1.overlap(b2_temp);
163 }
164
165 1648 bool overlap(const Matrix3s& R0, const Vec3s& T0, const kIOS& b1,
166 const kIOS& b2, const CollisionRequest& request,
167 Scalar& sqrDistLowerBound) {
168
1/2
✓ Branch 1 taken 1648 times.
✗ Branch 2 not taken.
 1648 kIOS b2_temp = b2;
169
2/2
✓ Branch 0 taken 7720 times.
✓ Branch 1 taken 1648 times.
 9368 for (unsigned int i = 0; i < b2_temp.num_spheres; ++i) {
170
1/2
✓ Branch 1 taken 7720 times.
✗ Branch 2 not taken.
 7720 b2_temp.spheres[i].o.noalias() =
171
4/8
✓ Branch 1 taken 7720 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7720 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7720 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7720 times.
✗ Branch 11 not taken.
 15440 R0.transpose() * (b2_temp.spheres[i].o - T0);
172 }
173
174
5/10
✓ Branch 1 taken 1648 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1648 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1648 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1648 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1648 times.
✗ Branch 14 not taken.
 1648 b2_temp.obb.To.noalias() = R0.transpose() * (b2_temp.obb.To - T0);
175
2/4
✓ Branch 1 taken 1648 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1648 times.
✗ Branch 5 not taken.
 1648 b2_temp.obb.axes.applyOnTheLeft(R0.transpose());
176
177
1/2
✓ Branch 1 taken 1648 times.
✗ Branch 2 not taken.
 3296 return b1.overlap(b2_temp, request, sqrDistLowerBound);
178 }
179
180 1292 Scalar distance(const Matrix3s& R0, const Vec3s& T0, const kIOS& b1,
181 const kIOS& b2, Vec3s* P, Vec3s* Q) {
182
1/2
✓ Branch 1 taken 1292 times.
✗ Branch 2 not taken.
 1292 kIOS b2_temp = b2;
183
2/2
✓ Branch 0 taken 5106 times.
✓ Branch 1 taken 1292 times.
 6398 for (unsigned int i = 0; i < b2_temp.num_spheres; ++i) {
184
3/6
✓ Branch 1 taken 5106 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5106 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5106 times.
✗ Branch 8 not taken.
 5106 b2_temp.spheres[i].o = R0 * b2_temp.spheres[i].o + T0;
185 }
186
187
1/2
✓ Branch 1 taken 1292 times.
✗ Branch 2 not taken.
 2584 return b1.distance(b2_temp, P, Q);
188 }
189
190 ✗ kIOS translate(const kIOS& bv, const Vec3s& t) {
191 ✗ kIOS res(bv);
192 ✗ for (size_t i = 0; i < res.num_spheres; ++i) {
193 ✗ res.spheres[i].o += t;
194 }
195
196 ✗ translate(res.obb, t);
197 ✗ return res;
198 }
199
200 } // namespace coal
201