GCC Code Coverage Report

Directory: ./
File: src/shape/geometric_shapes_utility.cpp
Date: 2025-04-13 14:25:19
Exec Total Coverage
Lines: 287 706 40.7%
Branches: 311 2866 10.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/shape/geometric_shapes_utility.h"
39 #include "coal/internal/BV_fitter.h"
40 #include "coal/internal/tools.h"
41
42 namespace coal {
43
44 namespace details {
45
46 202 std::vector<Vec3s> getBoundVertices(const Box& box, const Transform3s& tf) {
47
1/2
✓ Branch 2 taken 202 times.
✗ Branch 3 not taken.
 202 std::vector<Vec3s> result(8);
48
1/2
✓ Branch 1 taken 202 times.
✗ Branch 2 not taken.
 202 Scalar a = box.halfSide[0];
49
1/2
✓ Branch 1 taken 202 times.
✗ Branch 2 not taken.
 202 Scalar b = box.halfSide[1];
50
1/2
✓ Branch 1 taken 202 times.
✗ Branch 2 not taken.
 202 Scalar c = box.halfSide[2];
51
2/4
✓ Branch 1 taken 202 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 202 times.
✗ Branch 5 not taken.
 202 result[0] = tf.transform(Vec3s(a, b, c));
52
2/4
✓ Branch 1 taken 202 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 202 times.
✗ Branch 5 not taken.
 202 result[1] = tf.transform(Vec3s(a, b, -c));
53
2/4
✓ Branch 1 taken 202 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 202 times.
✗ Branch 5 not taken.
 202 result[2] = tf.transform(Vec3s(a, -b, c));
54
2/4
✓ Branch 1 taken 202 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 202 times.
✗ Branch 5 not taken.
 202 result[3] = tf.transform(Vec3s(a, -b, -c));
55
2/4
✓ Branch 1 taken 202 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 202 times.
✗ Branch 5 not taken.
 202 result[4] = tf.transform(Vec3s(-a, b, c));
56
2/4
✓ Branch 1 taken 202 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 202 times.
✗ Branch 5 not taken.
 202 result[5] = tf.transform(Vec3s(-a, b, -c));
57
2/4
✓ Branch 1 taken 202 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 202 times.
✗ Branch 5 not taken.
 202 result[6] = tf.transform(Vec3s(-a, -b, c));
58
2/4
✓ Branch 1 taken 202 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 202 times.
✗ Branch 5 not taken.
 202 result[7] = tf.transform(Vec3s(-a, -b, -c));
59
60 404 return result;
61 }
62
63 // we use icosahedron to bound the sphere
64 17 std::vector<Vec3s> getBoundVertices(const Sphere& sphere,
65 const Transform3s& tf) {
66
1/2
✓ Branch 2 taken 17 times.
✗ Branch 3 not taken.
 17 std::vector<Vec3s> result(12);
67 17 const Scalar m = (1 + sqrt(Scalar(5))) / Scalar(2);
68 17 Scalar edge_size = sphere.radius * 6 / (sqrt(Scalar(27)) + sqrt(Scalar(15)));
69
70 17 Scalar a = edge_size;
71 17 Scalar b = m * edge_size;
72
2/4
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17 times.
✗ Branch 5 not taken.
 17 result[0] = tf.transform(Vec3s(0, a, b));
73
2/4
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17 times.
✗ Branch 5 not taken.
 17 result[1] = tf.transform(Vec3s(0, -a, b));
74
2/4
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17 times.
✗ Branch 5 not taken.
 17 result[2] = tf.transform(Vec3s(0, a, -b));
75
2/4
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17 times.
✗ Branch 5 not taken.
 17 result[3] = tf.transform(Vec3s(0, -a, -b));
76
2/4
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17 times.
✗ Branch 5 not taken.
 17 result[4] = tf.transform(Vec3s(a, b, 0));
77
2/4
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17 times.
✗ Branch 5 not taken.
 17 result[5] = tf.transform(Vec3s(-a, b, 0));
78
2/4
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17 times.
✗ Branch 5 not taken.
 17 result[6] = tf.transform(Vec3s(a, -b, 0));
79
2/4
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17 times.
✗ Branch 5 not taken.
 17 result[7] = tf.transform(Vec3s(-a, -b, 0));
80
2/4
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17 times.
✗ Branch 5 not taken.
 17 result[8] = tf.transform(Vec3s(b, 0, a));
81
2/4
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17 times.
✗ Branch 5 not taken.
 17 result[9] = tf.transform(Vec3s(b, 0, -a));
82
2/4
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17 times.
✗ Branch 5 not taken.
 17 result[10] = tf.transform(Vec3s(-b, 0, a));
83
2/4
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17 times.
✗ Branch 5 not taken.
 17 result[11] = tf.transform(Vec3s(-b, 0, -a));
84
85 34 return result;
86 }
87
88 // we use scaled icosahedron to bound the ellipsoid
89 ✗ std::vector<Vec3s> getBoundVertices(const Ellipsoid& ellipsoid,
90 const Transform3s& tf) {
91 ✗ std::vector<Vec3s> result(12);
92 ✗ const Scalar phi = (1 + sqrt(Scalar(5))) / Scalar(2);
93
94 ✗ const Scalar a = sqrt(Scalar(3)) / (phi * phi);
95 ✗ const Scalar b = phi * a;
96
97 ✗ const Scalar& A = ellipsoid.radii[0];
98 ✗ const Scalar& B = ellipsoid.radii[1];
99 ✗ const Scalar& C = ellipsoid.radii[2];
100
101 ✗ Scalar Aa = A * a;
102 ✗ Scalar Ab = A * b;
103 ✗ Scalar Ba = B * a;
104 ✗ Scalar Bb = B * b;
105 ✗ Scalar Ca = C * a;
106 ✗ Scalar Cb = C * b;
107 ✗ result[0] = tf.transform(Vec3s(0, Ba, Cb));
108 ✗ result[1] = tf.transform(Vec3s(0, -Ba, Cb));
109 ✗ result[2] = tf.transform(Vec3s(0, Ba, -Cb));
110 ✗ result[3] = tf.transform(Vec3s(0, -Ba, -Cb));
111 ✗ result[4] = tf.transform(Vec3s(Aa, Bb, 0));
112 ✗ result[5] = tf.transform(Vec3s(-Aa, Bb, 0));
113 ✗ result[6] = tf.transform(Vec3s(Aa, -Bb, 0));
114 ✗ result[7] = tf.transform(Vec3s(-Aa, -Bb, 0));
115 ✗ result[8] = tf.transform(Vec3s(Ab, 0, Ca));
116 ✗ result[9] = tf.transform(Vec3s(Ab, 0, -Ca));
117 ✗ result[10] = tf.transform(Vec3s(-Ab, 0, Ca));
118 ✗ result[11] = tf.transform(Vec3s(-Ab, 0, -Ca));
119
120 ✗ return result;
121 }
122
123 2 std::vector<Vec3s> getBoundVertices(const Capsule& capsule,
124 const Transform3s& tf) {
125
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 std::vector<Vec3s> result(36);
126 2 const Scalar m = (1 + sqrt(Scalar(5))) / Scalar(2);
127
128 2 Scalar hl = capsule.halfLength;
129 2 Scalar edge_size = capsule.radius * 6 / (sqrt(Scalar(27)) + sqrt(Scalar(15)));
130 2 Scalar a = edge_size;
131 2 Scalar b = m * edge_size;
132 2 Scalar r2 = capsule.radius * 2 / sqrt(Scalar(3));
133
134
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 result[0] = tf.transform(Vec3s(0, a, b + hl));
135
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 result[1] = tf.transform(Vec3s(0, -a, b + hl));
136
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 result[2] = tf.transform(Vec3s(0, a, -b + hl));
137
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 result[3] = tf.transform(Vec3s(0, -a, -b + hl));
138
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 result[4] = tf.transform(Vec3s(a, b, hl));
139
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 result[5] = tf.transform(Vec3s(-a, b, hl));
140
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 result[6] = tf.transform(Vec3s(a, -b, hl));
141
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 result[7] = tf.transform(Vec3s(-a, -b, hl));
142
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 result[8] = tf.transform(Vec3s(b, 0, a + hl));
143
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 result[9] = tf.transform(Vec3s(b, 0, -a + hl));
144
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 result[10] = tf.transform(Vec3s(-b, 0, a + hl));
145
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 result[11] = tf.transform(Vec3s(-b, 0, -a + hl));
146
147
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 result[12] = tf.transform(Vec3s(0, a, b - hl));
148
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 result[13] = tf.transform(Vec3s(0, -a, b - hl));
149
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 result[14] = tf.transform(Vec3s(0, a, -b - hl));
150
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 result[15] = tf.transform(Vec3s(0, -a, -b - hl));
151
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 result[16] = tf.transform(Vec3s(a, b, -hl));
152
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 result[17] = tf.transform(Vec3s(-a, b, -hl));
153
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 result[18] = tf.transform(Vec3s(a, -b, -hl));
154
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 result[19] = tf.transform(Vec3s(-a, -b, -hl));
155
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 result[20] = tf.transform(Vec3s(b, 0, a - hl));
156
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 result[21] = tf.transform(Vec3s(b, 0, -a - hl));
157
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 result[22] = tf.transform(Vec3s(-b, 0, a - hl));
158
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 result[23] = tf.transform(Vec3s(-b, 0, -a - hl));
159
160 2 Scalar c = Scalar(0.5) * r2;
161 2 Scalar d = capsule.radius;
162
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 result[24] = tf.transform(Vec3s(r2, 0, hl));
163
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 result[25] = tf.transform(Vec3s(c, d, hl));
164
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 result[26] = tf.transform(Vec3s(-c, d, hl));
165
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 result[27] = tf.transform(Vec3s(-r2, 0, hl));
166
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 result[28] = tf.transform(Vec3s(-c, -d, hl));
167
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 result[29] = tf.transform(Vec3s(c, -d, hl));
168
169
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 result[30] = tf.transform(Vec3s(r2, 0, -hl));
170
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 result[31] = tf.transform(Vec3s(c, d, -hl));
171
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 result[32] = tf.transform(Vec3s(-c, d, -hl));
172
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 result[33] = tf.transform(Vec3s(-r2, 0, -hl));
173
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 result[34] = tf.transform(Vec3s(-c, -d, -hl));
174
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 result[35] = tf.transform(Vec3s(c, -d, -hl));
175
176 4 return result;
177 }
178
179 2 std::vector<Vec3s> getBoundVertices(const Cone& cone, const Transform3s& tf) {
180
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 std::vector<Vec3s> result(7);
181
182 2 Scalar hl = cone.halfLength;
183 2 Scalar r2 = cone.radius * 2 / sqrt(Scalar(3));
184 2 Scalar a = Scalar(0.5) * r2;
185 2 Scalar b = cone.radius;
186
187
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 result[0] = tf.transform(Vec3s(r2, 0, -hl));
188
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 result[1] = tf.transform(Vec3s(a, b, -hl));
189
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 result[2] = tf.transform(Vec3s(-a, b, -hl));
190
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 result[3] = tf.transform(Vec3s(-r2, 0, -hl));
191
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 result[4] = tf.transform(Vec3s(-a, -b, -hl));
192
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 result[5] = tf.transform(Vec3s(a, -b, -hl));
193
194
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 result[6] = tf.transform(Vec3s(0, 0, hl));
195
196 4 return result;
197 }
198
199 5 std::vector<Vec3s> getBoundVertices(const Cylinder& cylinder,
200 const Transform3s& tf) {
201
1/2
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
 5 std::vector<Vec3s> result(12);
202
203 5 Scalar hl = cylinder.halfLength;
204 5 Scalar r2 = cylinder.radius * 2 / sqrt(Scalar(3));
205 5 Scalar a = Scalar(0.5) * r2;
206 5 Scalar b = cylinder.radius;
207
208
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 5 result[0] = tf.transform(Vec3s(r2, 0, -hl));
209
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 5 result[1] = tf.transform(Vec3s(a, b, -hl));
210
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 5 result[2] = tf.transform(Vec3s(-a, b, -hl));
211
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 5 result[3] = tf.transform(Vec3s(-r2, 0, -hl));
212
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 5 result[4] = tf.transform(Vec3s(-a, -b, -hl));
213
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 5 result[5] = tf.transform(Vec3s(a, -b, -hl));
214
215
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 5 result[6] = tf.transform(Vec3s(r2, 0, hl));
216
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 5 result[7] = tf.transform(Vec3s(a, b, hl));
217
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 5 result[8] = tf.transform(Vec3s(-a, b, hl));
218
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 5 result[9] = tf.transform(Vec3s(-r2, 0, hl));
219
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 5 result[10] = tf.transform(Vec3s(-a, -b, hl));
220
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 5 result[11] = tf.transform(Vec3s(a, -b, hl));
221
222 10 return result;
223 }
224
225 ✗ std::vector<Vec3s> getBoundVertices(const ConvexBase& convex,
226 const Transform3s& tf) {
227 ✗ std::vector<Vec3s> result(convex.num_points);
228 ✗ const std::vector<Vec3s>& points_ = *(convex.points);
229 ✗ for (std::size_t i = 0; i < convex.num_points; ++i) {
230 ✗ result[i] = tf.transform(points_[i]);
231 }
232
233 ✗ return result;
234 }
235
236 ✗ std::vector<Vec3s> getBoundVertices(const TriangleP& triangle,
237 const Transform3s& tf) {
238 ✗ std::vector<Vec3s> result(3);
239 ✗ result[0] = tf.transform(triangle.a);
240 ✗ result[1] = tf.transform(triangle.b);
241 ✗ result[2] = tf.transform(triangle.c);
242
243 ✗ return result;
244 }
245
246 } // namespace details
247
248 6420 Halfspace transform(const Halfspace& a, const Transform3s& tf) {
249 /// suppose the initial halfspace is n * x <= d
250 /// after transform (R, T), x --> x' = R x + T
251 /// and the new half space becomes n' * x' <= d'
252 /// where n' = R * n
253 /// and d' = d + n' * T
254
255
2/4
✓ Branch 2 taken 6420 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6420 times.
✗ Branch 6 not taken.
 6420 Vec3s n = tf.getRotation() * a.n;
256
1/2
✓ Branch 2 taken 6420 times.
✗ Branch 3 not taken.
 6420 Scalar d = a.d + n.dot(tf.getTranslation());
257
1/2
✓ Branch 1 taken 6420 times.
✗ Branch 2 not taken.
 6420 Halfspace result(n, d);
258
1/2
✓ Branch 2 taken 6420 times.
✗ Branch 3 not taken.
 6420 result.setSweptSphereRadius(a.getSweptSphereRadius());
259
260 12840 return result;
261 }
262
263 61 Plane transform(const Plane& a, const Transform3s& tf) {
264 /// suppose the initial halfspace is n * x <= d
265 /// after transform (R, T), x --> x' = R x + T
266 /// and the new half space becomes n' * x' <= d'
267 /// where n' = R * n
268 /// and d' = d + n' * T
269
270
2/4
✓ Branch 2 taken 61 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 61 times.
✗ Branch 6 not taken.
 61 Vec3s n = tf.getRotation() * a.n;
271
1/2
✓ Branch 2 taken 61 times.
✗ Branch 3 not taken.
 61 Scalar d = a.d + n.dot(tf.getTranslation());
272
1/2
✓ Branch 1 taken 61 times.
✗ Branch 2 not taken.
 61 Plane result(n, d);
273
1/2
✓ Branch 2 taken 61 times.
✗ Branch 3 not taken.
 61 result.setSweptSphereRadius(a.getSweptSphereRadius());
274
275 122 return result;
276 }
277
278 5546 std::array<Halfspace, 2> transformToHalfspaces(const Plane& a,
279 const Transform3s& tf) {
280 // A plane can be represented by two halfspaces
281
282
2/4
✓ Branch 2 taken 5546 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5546 times.
✗ Branch 6 not taken.
 5546 Vec3s n = tf.getRotation() * a.n;
283
1/2
✓ Branch 2 taken 5546 times.
✗ Branch 3 not taken.
 5546 Scalar d = a.d + n.dot(tf.getTranslation());
284
4/8
✓ Branch 1 taken 5546 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5546 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5546 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5546 times.
✗ Branch 11 not taken.
 5546 std::array<Halfspace, 2> result = {Halfspace(n, d), Halfspace(-n, -d)};
285
1/2
✓ Branch 3 taken 5546 times.
✗ Branch 4 not taken.
 5546 result[0].setSweptSphereRadius(a.getSweptSphereRadius());
286
1/2
✓ Branch 3 taken 5546 times.
✗ Branch 4 not taken.
 5546 result[1].setSweptSphereRadius(a.getSweptSphereRadius());
287
288 11092 return result;
289 }
290
291 template <>
292 15122 void computeBV<AABB, Box>(const Box& s, const Transform3s& tf, AABB& bv) {
293 15122 const Matrix3s& R = tf.getRotation();
294 15122 const Vec3s& T = tf.getTranslation();
295
296
3/6
✓ Branch 1 taken 15122 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15122 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 15122 times.
✗ Branch 8 not taken.
 15122 Vec3s v_delta(R.cwiseAbs() * s.halfSide);
297
2/4
✓ Branch 1 taken 15122 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15122 times.
✗ Branch 5 not taken.
 15122 bv.max_ = T + v_delta;
298
2/4
✓ Branch 1 taken 15122 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15122 times.
✗ Branch 5 not taken.
 15122 bv.min_ = T - v_delta;
299 15122 }
300
301 template <>
302 16958 void computeBV<AABB, Sphere>(const Sphere& s, const Transform3s& tf, AABB& bv) {
303 16958 const Vec3s& T = tf.getTranslation();
304
305
2/4
✓ Branch 1 taken 16958 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16958 times.
✗ Branch 5 not taken.
 16958 Vec3s v_delta(Vec3s::Constant(s.radius));
306
2/4
✓ Branch 1 taken 16958 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16958 times.
✗ Branch 5 not taken.
 16958 bv.max_ = T + v_delta;
307
2/4
✓ Branch 1 taken 16958 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16958 times.
✗ Branch 5 not taken.
 16958 bv.min_ = T - v_delta;
308 16958 }
309
310 template <>
311 1 void computeBV<AABB, Ellipsoid>(const Ellipsoid& e, const Transform3s& tf,
312 AABB& bv) {
313 1 const Matrix3s& R = tf.getRotation();
314 1 const Vec3s& T = tf.getTranslation();
315
316
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 Vec3s v_delta = R * e.radii;
317
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 bv.max_ = T + v_delta;
318
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 bv.min_ = T - v_delta;
319 1 }
320
321 template <>
322 10011 void computeBV<AABB, Capsule>(const Capsule& s, const Transform3s& tf,
323 AABB& bv) {
324 10011 const Matrix3s& R = tf.getRotation();
325 10011 const Vec3s& T = tf.getTranslation();
326
327
6/12
✓ Branch 1 taken 10011 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10011 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10011 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10011 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10011 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 10011 times.
✗ Branch 17 not taken.
 10011 Vec3s v_delta(R.col(2).cwiseAbs() * s.halfLength + Vec3s::Constant(s.radius));
328
2/4
✓ Branch 1 taken 10011 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10011 times.
✗ Branch 5 not taken.
 10011 bv.max_ = T + v_delta;
329
2/4
✓ Branch 1 taken 10011 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10011 times.
✗ Branch 5 not taken.
 10011 bv.min_ = T - v_delta;
330 10011 }
331
332 template <>
333 1 void computeBV<AABB, Cone>(const Cone& s, const Transform3s& tf, AABB& bv) {
334 1 const Matrix3s& R = tf.getRotation();
335 1 const Vec3s& T = tf.getTranslation();
336
337
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 Scalar x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) +
338
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 fabs(R(0, 2) * s.halfLength);
339
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 Scalar y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) +
340
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 fabs(R(1, 2) * s.halfLength);
341
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 Scalar z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) +
342
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 fabs(R(2, 2) * s.halfLength);
343
344
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Vec3s v_delta(x_range, y_range, z_range);
345
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 bv.max_ = T + v_delta;
346
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 bv.min_ = T - v_delta;
347 1 }
348
349 template <>
350 8913 void computeBV<AABB, Cylinder>(const Cylinder& s, const Transform3s& tf,
351 AABB& bv) {
352 8913 const Matrix3s& R = tf.getRotation();
353 8913 const Vec3s& T = tf.getTranslation();
354
355
2/4
✓ Branch 1 taken 8913 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8913 times.
✗ Branch 5 not taken.
 8913 Scalar x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) +
356
1/2
✓ Branch 1 taken 8913 times.
✗ Branch 2 not taken.
 8913 fabs(R(0, 2) * s.halfLength);
357
2/4
✓ Branch 1 taken 8913 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8913 times.
✗ Branch 5 not taken.
 8913 Scalar y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) +
358
1/2
✓ Branch 1 taken 8913 times.
✗ Branch 2 not taken.
 8913 fabs(R(1, 2) * s.halfLength);
359
2/4
✓ Branch 1 taken 8913 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8913 times.
✗ Branch 5 not taken.
 8913 Scalar z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) +
360
1/2
✓ Branch 1 taken 8913 times.
✗ Branch 2 not taken.
 8913 fabs(R(2, 2) * s.halfLength);
361
362
1/2
✓ Branch 1 taken 8913 times.
✗ Branch 2 not taken.
 8913 Vec3s v_delta(x_range, y_range, z_range);
363
2/4
✓ Branch 1 taken 8913 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8913 times.
✗ Branch 5 not taken.
 8913 bv.max_ = T + v_delta;
364
2/4
✓ Branch 1 taken 8913 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8913 times.
✗ Branch 5 not taken.
 8913 bv.min_ = T - v_delta;
365 8913 }
366
367 template <>
368 1 void computeBV<AABB, ConvexBase>(const ConvexBase& s, const Transform3s& tf,
369 AABB& bv) {
370 1 const Matrix3s& R = tf.getRotation();
371 1 const Vec3s& T = tf.getTranslation();
372
373
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 AABB bv_;
374 1 const std::vector<Vec3s>& points_ = *(s.points);
375
2/2
✓ Branch 0 taken 8 times.
✓ Branch 1 taken 1 times.
 9 for (std::size_t i = 0; i < s.num_points; ++i) {
376
3/6
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 8 times.
✗ Branch 9 not taken.
 8 Vec3s new_p = R * points_[i] + T;
377
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 bv_ += new_p;
378 }
379
380
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 bv = bv_;
381 1 }
382
383 template <>
384 1 void computeBV<AABB, TriangleP>(const TriangleP& s, const Transform3s& tf,
385 AABB& bv) {
386
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 bv = AABB(tf.transform(s.a), tf.transform(s.b), tf.transform(s.c));
387 1 }
388
389 template <>
390 1 void computeBV<AABB, Halfspace>(const Halfspace& s, const Transform3s& tf,
391 AABB& bv) {
392
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Halfspace new_s = transform(s, tf);
393 1 const Vec3s& n = new_s.n;
394 1 const Scalar& d = new_s.d;
395
396
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 AABB bv_;
397
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 bv_.min_ = Vec3s::Constant(-(std::numeric_limits<Scalar>::max)());
398
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 bv_.max_ = Vec3s::Constant((std::numeric_limits<Scalar>::max)());
399
3/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 1 times.
 1 if (n[1] == (Scalar)0.0 && n[2] == (Scalar)0.0) {
400 // normal aligned with x axis
401 ✗ if (n[0] < 0)
402 ✗ bv_.min_[0] = -d;
403 ✗ else if (n[0] > 0)
404 ✗ bv_.max_[0] = d;
405
3/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 1 times.
 1 } else if (n[0] == (Scalar)0.0 && n[2] == (Scalar)0.0) {
406 // normal aligned with y axis
407 ✗ if (n[1] < 0)
408 ✗ bv_.min_[1] = -d;
409 ✗ else if (n[1] > 0)
410 ✗ bv_.max_[1] = d;
411
3/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 1 times.
 1 } else if (n[0] == (Scalar)0.0 && n[1] == (Scalar)0.0) {
412 // normal aligned with z axis
413 ✗ if (n[2] < 0)
414 ✗ bv_.min_[2] = -d;
415 ✗ else if (n[2] > 0)
416 ✗ bv_.max_[2] = d;
417 }
418
419
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 bv = bv_;
420 1 }
421
422 template <>
423 1 void computeBV<AABB, Plane>(const Plane& s, const Transform3s& tf, AABB& bv) {
424
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Plane new_s = transform(s, tf);
425 1 const Vec3s& n = new_s.n;
426 1 const Scalar& d = new_s.d;
427
428
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 AABB bv_;
429
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 bv_.min_ = Vec3s::Constant(-(std::numeric_limits<Scalar>::max)());
430
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 bv_.max_ = Vec3s::Constant((std::numeric_limits<Scalar>::max)());
431
3/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 1 times.
 1 if (n[1] == (Scalar)0.0 && n[2] == (Scalar)0.0) {
432 // normal aligned with x axis
433 ✗ if (n[0] < 0) {
434 ✗ bv_.min_[0] = bv_.max_[0] = -d;
435 ✗ } else if (n[0] > 0) {
436 ✗ bv_.min_[0] = bv_.max_[0] = d;
437 }
438
3/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 1 times.
 1 } else if (n[0] == (Scalar)0.0 && n[2] == (Scalar)0.0) {
439 // normal aligned with y axis
440 ✗ if (n[1] < 0) {
441 ✗ bv_.min_[1] = bv_.max_[1] = -d;
442 ✗ } else if (n[1] > 0) {
443 ✗ bv_.min_[1] = bv_.max_[1] = d;
444 }
445
3/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 1 times.
 1 } else if (n[0] == (Scalar)0.0 && n[1] == (Scalar)0.0) {
446 // normal aligned with z axis
447 ✗ if (n[2] < 0) {
448 ✗ bv_.min_[2] = bv_.max_[2] = -d;
449 ✗ } else if (n[2] > 0) {
450 ✗ bv_.min_[2] = bv_.max_[2] = d;
451 }
452 }
453
454
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 bv = bv_;
455 1 }
456
457 template <>
458 2 void computeBV<OBB, Box>(const Box& s, const Transform3s& tf, OBB& bv) {
459
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
 2 if (s.getSweptSphereRadius() > 0) {
460 ✗ COAL_THROW_PRETTY("Swept-sphere radius not yet supported.",
461 std::runtime_error);
462 }
463 2 const Matrix3s& R = tf.getRotation();
464 2 const Vec3s& T = tf.getTranslation();
465
466 2 bv.To = T;
467 2 bv.axes = R;
468 2 bv.extent = s.halfSide;
469 2 }
470
471 template <>
472 1002 void computeBV<OBB, Sphere>(const Sphere& s, const Transform3s& tf, OBB& bv) {
473
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1002 times.
 1002 if (s.getSweptSphereRadius() > 0) {
474 ✗ COAL_THROW_PRETTY("Swept-sphere radius not yet supported.",
475 std::runtime_error);
476 }
477 1002 const Vec3s& T = tf.getTranslation();
478
479
1/2
✓ Branch 2 taken 1002 times.
✗ Branch 3 not taken.
 1002 bv.To.noalias() = T;
480 1002 bv.axes.setIdentity();
481 1002 bv.extent.setConstant(s.radius);
482 1002 }
483
484 template <>
485 1002 void computeBV<OBB, Capsule>(const Capsule& s, const Transform3s& tf, OBB& bv) {
486
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1002 times.
 1002 if (s.getSweptSphereRadius() > 0) {
487 ✗ COAL_THROW_PRETTY("Swept-sphere radius not yet supported.",
488 std::runtime_error);
489 }
490 1002 const Matrix3s& R = tf.getRotation();
491 1002 const Vec3s& T = tf.getTranslation();
492
493
1/2
✓ Branch 2 taken 1002 times.
✗ Branch 3 not taken.
 1002 bv.To.noalias() = T;
494
1/2
✓ Branch 2 taken 1002 times.
✗ Branch 3 not taken.
 1002 bv.axes.noalias() = R;
495
2/4
✓ Branch 2 taken 1002 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1002 times.
✗ Branch 6 not taken.
 1002 bv.extent << s.radius, s.radius, s.halfLength + s.radius;
496 1002 }
497
498 template <>
499 1002 void computeBV<OBB, Cone>(const Cone& s, const Transform3s& tf, OBB& bv) {
500
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1002 times.
 1002 if (s.getSweptSphereRadius() > 0) {
501 ✗ COAL_THROW_PRETTY("Swept-sphere radius not yet supported.",
502 std::runtime_error);
503 }
504 1002 const Matrix3s& R = tf.getRotation();
505 1002 const Vec3s& T = tf.getTranslation();
506
507
1/2
✓ Branch 2 taken 1002 times.
✗ Branch 3 not taken.
 1002 bv.To.noalias() = T;
508
1/2
✓ Branch 2 taken 1002 times.
✗ Branch 3 not taken.
 1002 bv.axes.noalias() = R;
509
2/4
✓ Branch 2 taken 1002 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1002 times.
✗ Branch 6 not taken.
 1002 bv.extent << s.radius, s.radius, s.halfLength;
510 1002 }
511
512 template <>
513 1003 void computeBV<OBB, Cylinder>(const Cylinder& s, const Transform3s& tf,
514 OBB& bv) {
515
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1003 times.
 1003 if (s.getSweptSphereRadius() > 0) {
516 ✗ COAL_THROW_PRETTY("Swept-sphere radius not yet supported.",
517 std::runtime_error);
518 }
519 1003 const Matrix3s& R = tf.getRotation();
520 1003 const Vec3s& T = tf.getTranslation();
521
522
1/2
✓ Branch 2 taken 1003 times.
✗ Branch 3 not taken.
 1003 bv.To.noalias() = T;
523
1/2
✓ Branch 2 taken 1003 times.
✗ Branch 3 not taken.
 1003 bv.axes.noalias() = R;
524
2/4
✓ Branch 2 taken 1003 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1003 times.
✗ Branch 6 not taken.
 1003 bv.extent << s.radius, s.radius, s.halfLength;
525 1003 }
526
527 template <>
528 ✗ void computeBV<OBB, ConvexBase>(const ConvexBase& s, const Transform3s& tf,
529 OBB& bv) {
530 ✗ if (s.getSweptSphereRadius() > 0) {
531 ✗ COAL_THROW_PRETTY("Swept-sphere radius not yet supported.",
532 std::runtime_error);
533 }
534 ✗ const Matrix3s& R = tf.getRotation();
535 ✗ const Vec3s& T = tf.getTranslation();
536
537 ✗ fit(s.points->data(), s.num_points, bv);
538
539 ✗ bv.axes.applyOnTheLeft(R);
540
541 ✗ bv.To = R * bv.To + T;
542 }
543
544 template <>
545 4 void computeBV<OBB, Halfspace>(const Halfspace& s, const Transform3s&,
546 OBB& bv) {
547
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 4 times.
 4 if (s.getSweptSphereRadius() > 0) {
548 ✗ COAL_THROW_PRETTY("Swept-sphere radius not yet supported.",
549 std::runtime_error);
550 }
551 /// Half space can only have very rough OBB
552 4 bv.axes.setIdentity();
553 4 bv.To.setZero();
554
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 4 bv.extent.setConstant(((std::numeric_limits<Scalar>::max)()));
555 4 }
556
557 template <>
558 4 void computeBV<RSS, Halfspace>(const Halfspace& s, const Transform3s&,
559 RSS& bv) {
560
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 4 times.
 4 if (s.getSweptSphereRadius() > 0) {
561 ✗ COAL_THROW_PRETTY("Swept-sphere radius not yet supported.",
562 std::runtime_error);
563 }
564 /// Half space can only have very rough RSS
565 4 bv.axes.setIdentity();
566 4 bv.Tr.setZero();
567 4 bv.length[0] = bv.length[1] = bv.radius =
568 4 (std::numeric_limits<Scalar>::max)();
569 4 }
570
571 template <>
572 4 void computeBV<OBBRSS, Halfspace>(const Halfspace& s, const Transform3s& tf,
573 OBBRSS& bv) {
574
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 4 times.
 4 if (s.getSweptSphereRadius() > 0) {
575 ✗ COAL_THROW_PRETTY("Swept-sphere radius not yet supported.",
576 std::runtime_error);
577 }
578 4 computeBV<OBB, Halfspace>(s, tf, bv.obb);
579 4 computeBV<RSS, Halfspace>(s, tf, bv.rss);
580 4 }
581
582 template <>
583 ✗ void computeBV<kIOS, Halfspace>(const Halfspace& s, const Transform3s& tf,
584 kIOS& bv) {
585 ✗ if (s.getSweptSphereRadius() > 0) {
586 ✗ COAL_THROW_PRETTY("Swept-sphere radius not yet supported.",
587 std::runtime_error);
588 }
589 ✗ bv.num_spheres = 1;
590 ✗ computeBV<OBB, Halfspace>(s, tf, bv.obb);
591 ✗ bv.spheres[0].o = Vec3s();
592 ✗ bv.spheres[0].r = (std::numeric_limits<Scalar>::max)();
593 }
594
595 template <>
596 ✗ void computeBV<KDOP<16>, Halfspace>(const Halfspace& s, const Transform3s& tf,
597 KDOP<16>& bv) {
598 ✗ if (s.getSweptSphereRadius() > 0) {
599 ✗ COAL_THROW_PRETTY("Swept-sphere radius not yet supported.",
600 std::runtime_error);
601 }
602 ✗ Halfspace new_s = transform(s, tf);
603 ✗ const Vec3s& n = new_s.n;
604 ✗ const Scalar& d = new_s.d;
605
606 ✗ const short D = 8;
607 ✗ for (short i = 0; i < D; ++i)
608 ✗ bv.dist(i) = -(std::numeric_limits<Scalar>::max)();
609 ✗ for (short i = D; i < 2 * D; ++i)
610 ✗ bv.dist(i) = (std::numeric_limits<Scalar>::max)();
611
612 ✗ if (n[1] == (Scalar)0.0 && n[2] == (Scalar)0.0) {
613 ✗ if (n[0] > 0)
614 ✗ bv.dist(D) = d;
615 else
616 ✗ bv.dist(0) = -d;
617 ✗ } else if (n[0] == (Scalar)0.0 && n[2] == (Scalar)0.0) {
618 ✗ if (n[1] > 0)
619 ✗ bv.dist(D + 1) = d;
620 else
621 ✗ bv.dist(1) = -d;
622 ✗ } else if (n[0] == (Scalar)0.0 && n[1] == (Scalar)0.0) {
623 ✗ if (n[2] > 0)
624 ✗ bv.dist(D + 2) = d;
625 else
626 ✗ bv.dist(2) = -d;
627 ✗ } else if (n[2] == (Scalar)0.0 && n[0] == n[1]) {
628 ✗ if (n[0] > 0)
629 ✗ bv.dist(D + 3) = n[0] * d * 2;
630 else
631 ✗ bv.dist(3) = n[0] * d * 2;
632 ✗ } else if (n[1] == (Scalar)0.0 && n[0] == n[2]) {
633 ✗ if (n[1] > 0)
634 ✗ bv.dist(D + 4) = n[0] * d * 2;
635 else
636 ✗ bv.dist(4) = n[0] * d * 2;
637 ✗ } else if (n[0] == (Scalar)0.0 && n[1] == n[2]) {
638 ✗ if (n[1] > 0)
639 ✗ bv.dist(D + 5) = n[1] * d * 2;
640 else
641 ✗ bv.dist(5) = n[1] * d * 2;
642 ✗ } else if (n[2] == (Scalar)0.0 && n[0] + n[1] == (Scalar)0.0) {
643 ✗ if (n[0] > 0)
644 ✗ bv.dist(D + 6) = n[0] * d * 2;
645 else
646 ✗ bv.dist(6) = n[0] * d * 2;
647 ✗ } else if (n[1] == (Scalar)0.0 && n[0] + n[2] == (Scalar)0.0) {
648 ✗ if (n[0] > 0)
649 ✗ bv.dist(D + 7) = n[0] * d * 2;
650 else
651 ✗ bv.dist(7) = n[0] * d * 2;
652 }
653 }
654
655 template <>
656 ✗ void computeBV<KDOP<18>, Halfspace>(const Halfspace& s, const Transform3s& tf,
657 KDOP<18>& bv) {
658 ✗ if (s.getSweptSphereRadius() > 0) {
659 ✗ COAL_THROW_PRETTY("Swept-sphere radius not yet supported.",
660 std::runtime_error);
661 }
662 ✗ Halfspace new_s = transform(s, tf);
663 ✗ const Vec3s& n = new_s.n;
664 ✗ const Scalar& d = new_s.d;
665
666 ✗ const short D = 9;
667
668 ✗ for (short i = 0; i < D; ++i)
669 ✗ bv.dist(i) = -(std::numeric_limits<Scalar>::max)();
670 ✗ for (short i = D; i < 2 * D; ++i)
671 ✗ bv.dist(i) = (std::numeric_limits<Scalar>::max)();
672
673 ✗ if (n[1] == (Scalar)0.0 && n[2] == (Scalar)0.0) {
674 ✗ if (n[0] > 0)
675 ✗ bv.dist(D) = d;
676 else
677 ✗ bv.dist(0) = -d;
678 ✗ } else if (n[0] == (Scalar)0.0 && n[2] == (Scalar)0.0) {
679 ✗ if (n[1] > 0)
680 ✗ bv.dist(D + 1) = d;
681 else
682 ✗ bv.dist(1) = -d;
683 ✗ } else if (n[0] == (Scalar)0.0 && n[1] == (Scalar)0.0) {
684 ✗ if (n[2] > 0)
685 ✗ bv.dist(D + 2) = d;
686 else
687 ✗ bv.dist(2) = -d;
688 ✗ } else if (n[2] == (Scalar)0.0 && n[0] == n[1]) {
689 ✗ if (n[0] > 0)
690 ✗ bv.dist(D + 3) = n[0] * d * 2;
691 else
692 ✗ bv.dist(3) = n[0] * d * 2;
693 ✗ } else if (n[1] == (Scalar)0.0 && n[0] == n[2]) {
694 ✗ if (n[1] > 0)
695 ✗ bv.dist(D + 4) = n[0] * d * 2;
696 else
697 ✗ bv.dist(4) = n[0] * d * 2;
698 ✗ } else if (n[0] == (Scalar)0.0 && n[1] == n[2]) {
699 ✗ if (n[1] > 0)
700 ✗ bv.dist(D + 5) = n[1] * d * 2;
701 else
702 ✗ bv.dist(5) = n[1] * d * 2;
703 ✗ } else if (n[2] == (Scalar)0.0 && n[0] + n[1] == (Scalar)0.0) {
704 ✗ if (n[0] > 0)
705 ✗ bv.dist(D + 6) = n[0] * d * 2;
706 else
707 ✗ bv.dist(6) = n[0] * d * 2;
708 ✗ } else if (n[1] == (Scalar)0.0 && n[0] + n[2] == (Scalar)0.0) {
709 ✗ if (n[0] > 0)
710 ✗ bv.dist(D + 7) = n[0] * d * 2;
711 else
712 ✗ bv.dist(7) = n[0] * d * 2;
713 ✗ } else if (n[0] == (Scalar)0.0 && n[1] + n[2] == (Scalar)0.0) {
714 ✗ if (n[1] > 0)
715 ✗ bv.dist(D + 8) = n[1] * d * 2;
716 else
717 ✗ bv.dist(8) = n[1] * d * 2;
718 }
719 }
720
721 template <>
722 ✗ void computeBV<KDOP<24>, Halfspace>(const Halfspace& s, const Transform3s& tf,
723 KDOP<24>& bv) {
724 ✗ if (s.getSweptSphereRadius() > 0) {
725 ✗ COAL_THROW_PRETTY("Swept-sphere radius not yet supported.",
726 std::runtime_error);
727 }
728 ✗ Halfspace new_s = transform(s, tf);
729 ✗ const Vec3s& n = new_s.n;
730 ✗ const Scalar& d = new_s.d;
731
732 ✗ const short D = 12;
733
734 ✗ for (short i = 0; i < D; ++i)
735 ✗ bv.dist(i) = -(std::numeric_limits<Scalar>::max)();
736 ✗ for (short i = D; i < 2 * D; ++i)
737 ✗ bv.dist(i) = (std::numeric_limits<Scalar>::max)();
738
739 ✗ if (n[1] == (Scalar)0.0 && n[2] == (Scalar)0.0) {
740 ✗ if (n[0] > 0)
741 ✗ bv.dist(D) = d;
742 else
743 ✗ bv.dist(0) = -d;
744 ✗ } else if (n[0] == (Scalar)0.0 && n[2] == (Scalar)0.0) {
745 ✗ if (n[1] > 0)
746 ✗ bv.dist(D + 1) = d;
747 else
748 ✗ bv.dist(1) = -d;
749 ✗ } else if (n[0] == (Scalar)0.0 && n[1] == (Scalar)0.0) {
750 ✗ if (n[2] > 0)
751 ✗ bv.dist(D + 2) = d;
752 else
753 ✗ bv.dist(2) = -d;
754 ✗ } else if (n[2] == (Scalar)0.0 && n[0] == n[1]) {
755 ✗ if (n[0] > 0)
756 ✗ bv.dist(D + 3) = n[0] * d * 2;
757 else
758 ✗ bv.dist(3) = n[0] * d * 2;
759 ✗ } else if (n[1] == (Scalar)0.0 && n[0] == n[2]) {
760 ✗ if (n[1] > 0)
761 ✗ bv.dist(D + 4) = n[0] * d * 2;
762 else
763 ✗ bv.dist(4) = n[0] * d * 2;
764 ✗ } else if (n[0] == (Scalar)0.0 && n[1] == n[2]) {
765 ✗ if (n[1] > 0)
766 ✗ bv.dist(D + 5) = n[1] * d * 2;
767 else
768 ✗ bv.dist(5) = n[1] * d * 2;
769 ✗ } else if (n[2] == (Scalar)0.0 && n[0] + n[1] == (Scalar)0.0) {
770 ✗ if (n[0] > 0)
771 ✗ bv.dist(D + 6) = n[0] * d * 2;
772 else
773 ✗ bv.dist(6) = n[0] * d * 2;
774 ✗ } else if (n[1] == (Scalar)0.0 && n[0] + n[2] == (Scalar)0.0) {
775 ✗ if (n[0] > 0)
776 ✗ bv.dist(D + 7) = n[0] * d * 2;
777 else
778 ✗ bv.dist(7) = n[0] * d * 2;
779 ✗ } else if (n[0] == (Scalar)0.0 && n[1] + n[2] == (Scalar)0.0) {
780 ✗ if (n[1] > 0)
781 ✗ bv.dist(D + 8) = n[1] * d * 2;
782 else
783 ✗ bv.dist(8) = n[1] * d * 2;
784 ✗ } else if (n[0] + n[2] == (Scalar)0.0 && n[0] + n[1] == (Scalar)0.0) {
785 ✗ if (n[0] > 0)
786 ✗ bv.dist(D + 9) = n[0] * d * 3;
787 else
788 ✗ bv.dist(9) = n[0] * d * 3;
789 ✗ } else if (n[0] + n[1] == (Scalar)0.0 && n[1] + n[2] == (Scalar)0.0) {
790 ✗ if (n[0] > 0)
791 ✗ bv.dist(D + 10) = n[0] * d * 3;
792 else
793 ✗ bv.dist(10) = n[0] * d * 3;
794 ✗ } else if (n[0] + n[1] == (Scalar)0.0 && n[0] + n[2] == (Scalar)0.0) {
795 ✗ if (n[1] > 0)
796 ✗ bv.dist(D + 11) = n[1] * d * 3;
797 else
798 ✗ bv.dist(11) = n[1] * d * 3;
799 }
800 }
801
802 template <>
803 ✗ void computeBV<OBB, Plane>(const Plane& s, const Transform3s& tf, OBB& bv) {
804 ✗ if (s.getSweptSphereRadius() > 0) {
805 ✗ COAL_THROW_PRETTY("Swept-sphere radius not yet supported.",
806 std::runtime_error);
807 }
808 ✗ Vec3s n = tf.getRotation() * s.n;
809 ✗ generateCoordinateSystem(n, bv.axes.col(1), bv.axes.col(2));
810 ✗ bv.axes.col(0).noalias() = n;
811
812 ✗ bv.extent << 0, (std::numeric_limits<Scalar>::max)(),
813 ✗ (std::numeric_limits<Scalar>::max)();
814
815 ✗ Vec3s p = s.n * s.d;
816 bv.To =
817 ✗ tf.transform(p); /// n'd' = R * n * (d + (R * n) * T) = R * (n * d) + T
818 }
819
820 template <>
821 ✗ void computeBV<RSS, Plane>(const Plane& s, const Transform3s& tf, RSS& bv) {
822 ✗ if (s.getSweptSphereRadius() > 0) {
823 ✗ COAL_THROW_PRETTY("Swept-sphere radius not yet supported.",
824 std::runtime_error);
825 }
826 ✗ Vec3s n = tf.getRotation() * s.n;
827
828 ✗ generateCoordinateSystem(n, bv.axes.col(1), bv.axes.col(2));
829 ✗ bv.axes.col(0).noalias() = n;
830
831 ✗ bv.length[0] = (std::numeric_limits<Scalar>::max)();
832 ✗ bv.length[1] = (std::numeric_limits<Scalar>::max)();
833
834 ✗ bv.radius = 0;
835
836 ✗ Vec3s p = s.n * s.d;
837 ✗ bv.Tr = tf.transform(p);
838 }
839
840 template <>
841 ✗ void computeBV<OBBRSS, Plane>(const Plane& s, const Transform3s& tf,
842 OBBRSS& bv) {
843 ✗ if (s.getSweptSphereRadius() > 0) {
844 ✗ COAL_THROW_PRETTY("Swept-sphere radius not yet supported.",
845 std::runtime_error);
846 }
847 ✗ computeBV<OBB, Plane>(s, tf, bv.obb);
848 ✗ computeBV<RSS, Plane>(s, tf, bv.rss);
849 }
850
851 template <>
852 ✗ void computeBV<kIOS, Plane>(const Plane& s, const Transform3s& tf, kIOS& bv) {
853 ✗ if (s.getSweptSphereRadius() > 0) {
854 ✗ COAL_THROW_PRETTY("Swept-sphere radius not yet supported.",
855 std::runtime_error);
856 }
857 ✗ bv.num_spheres = 1;
858 ✗ computeBV<OBB, Plane>(s, tf, bv.obb);
859 ✗ bv.spheres[0].o = Vec3s();
860 ✗ bv.spheres[0].r = (std::numeric_limits<Scalar>::max)();
861 }
862
863 template <>
864 ✗ void computeBV<KDOP<16>, Plane>(const Plane& s, const Transform3s& tf,
865 KDOP<16>& bv) {
866 ✗ if (s.getSweptSphereRadius() > 0) {
867 ✗ COAL_THROW_PRETTY("Swept-sphere radius not yet supported.",
868 std::runtime_error);
869 }
870 ✗ Plane new_s = transform(s, tf);
871 ✗ const Vec3s& n = new_s.n;
872 ✗ const Scalar& d = new_s.d;
873
874 ✗ const short D = 8;
875
876 ✗ for (short i = 0; i < D; ++i)
877 ✗ bv.dist(i) = -(std::numeric_limits<Scalar>::max)();
878 ✗ for (short i = D; i < 2 * D; ++i)
879 ✗ bv.dist(i) = (std::numeric_limits<Scalar>::max)();
880
881 ✗ if (n[1] == (Scalar)0.0 && n[2] == (Scalar)0.0) {
882 ✗ if (n[0] > 0)
883 ✗ bv.dist(0) = bv.dist(D) = d;
884 else
885 ✗ bv.dist(0) = bv.dist(D) = -d;
886 ✗ } else if (n[0] == (Scalar)0.0 && n[2] == (Scalar)0.0) {
887 ✗ if (n[1] > 0)
888 ✗ bv.dist(1) = bv.dist(D + 1) = d;
889 else
890 ✗ bv.dist(1) = bv.dist(D + 1) = -d;
891 ✗ } else if (n[0] == (Scalar)0.0 && n[1] == (Scalar)0.0) {
892 ✗ if (n[2] > 0)
893 ✗ bv.dist(2) = bv.dist(D + 2) = d;
894 else
895 ✗ bv.dist(2) = bv.dist(D + 2) = -d;
896 ✗ } else if (n[2] == (Scalar)0.0 && n[0] == n[1]) {
897 ✗ bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2;
898 ✗ } else if (n[1] == (Scalar)0.0 && n[0] == n[2]) {
899 ✗ bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2;
900 ✗ } else if (n[0] == (Scalar)0.0 && n[1] == n[2]) {
901 ✗ bv.dist(6) = bv.dist(D + 5) = n[1] * d * 2;
902 ✗ } else if (n[2] == (Scalar)0.0 && n[0] + n[1] == (Scalar)0.0) {
903 ✗ bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2;
904 ✗ } else if (n[1] == (Scalar)0.0 && n[0] + n[2] == (Scalar)0.0) {
905 ✗ bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2;
906 }
907 }
908
909 template <>
910 ✗ void computeBV<KDOP<18>, Plane>(const Plane& s, const Transform3s& tf,
911 KDOP<18>& bv) {
912 ✗ if (s.getSweptSphereRadius() > 0) {
913 ✗ COAL_THROW_PRETTY("Swept-sphere radius not yet supported.",
914 std::runtime_error);
915 }
916 ✗ Plane new_s = transform(s, tf);
917 ✗ const Vec3s& n = new_s.n;
918 ✗ const Scalar& d = new_s.d;
919
920 ✗ const short D = 9;
921
922 ✗ for (short i = 0; i < D; ++i)
923 ✗ bv.dist(i) = -(std::numeric_limits<Scalar>::max)();
924 ✗ for (short i = D; i < 2 * D; ++i)
925 ✗ bv.dist(i) = (std::numeric_limits<Scalar>::max)();
926
927 ✗ if (n[1] == (Scalar)0.0 && n[2] == (Scalar)0.0) {
928 ✗ if (n[0] > 0)
929 ✗ bv.dist(0) = bv.dist(D) = d;
930 else
931 ✗ bv.dist(0) = bv.dist(D) = -d;
932 ✗ } else if (n[0] == (Scalar)0.0 && n[2] == (Scalar)0.0) {
933 ✗ if (n[1] > 0)
934 ✗ bv.dist(1) = bv.dist(D + 1) = d;
935 else
936 ✗ bv.dist(1) = bv.dist(D + 1) = -d;
937 ✗ } else if (n[0] == (Scalar)0.0 && n[1] == (Scalar)0.0) {
938 ✗ if (n[2] > 0)
939 ✗ bv.dist(2) = bv.dist(D + 2) = d;
940 else
941 ✗ bv.dist(2) = bv.dist(D + 2) = -d;
942 ✗ } else if (n[2] == (Scalar)0.0 && n[0] == n[1]) {
943 ✗ bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2;
944 ✗ } else if (n[1] == (Scalar)0.0 && n[0] == n[2]) {
945 ✗ bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2;
946 ✗ } else if (n[0] == (Scalar)0.0 && n[1] == n[2]) {
947 ✗ bv.dist(5) = bv.dist(D + 5) = n[1] * d * 2;
948 ✗ } else if (n[2] == (Scalar)0.0 && n[0] + n[1] == (Scalar)0.0) {
949 ✗ bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2;
950 ✗ } else if (n[1] == (Scalar)0.0 && n[0] + n[2] == (Scalar)0.0) {
951 ✗ bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2;
952 ✗ } else if (n[0] == (Scalar)0.0 && n[1] + n[2] == (Scalar)0.0) {
953 ✗ bv.dist(8) = bv.dist(D + 8) = n[1] * d * 2;
954 }
955 }
956
957 template <>
958 ✗ void computeBV<KDOP<24>, Plane>(const Plane& s, const Transform3s& tf,
959 KDOP<24>& bv) {
960 ✗ if (s.getSweptSphereRadius() > 0) {
961 ✗ COAL_THROW_PRETTY("Swept-sphere radius not yet supported.",
962 std::runtime_error);
963 }
964 ✗ Plane new_s = transform(s, tf);
965 ✗ const Vec3s& n = new_s.n;
966 ✗ const Scalar& d = new_s.d;
967
968 ✗ const short D = 12;
969
970 ✗ for (short i = 0; i < D; ++i)
971 ✗ bv.dist(i) = -(std::numeric_limits<Scalar>::max)();
972 ✗ for (short i = D; i < 2 * D; ++i)
973 ✗ bv.dist(i) = (std::numeric_limits<Scalar>::max)();
974
975 ✗ if (n[1] == (Scalar)0.0 && n[2] == (Scalar)0.0) {
976 ✗ if (n[0] > 0)
977 ✗ bv.dist(0) = bv.dist(D) = d;
978 else
979 ✗ bv.dist(0) = bv.dist(D) = -d;
980 ✗ } else if (n[0] == (Scalar)0.0 && n[2] == (Scalar)0.0) {
981 ✗ if (n[1] > 0)
982 ✗ bv.dist(1) = bv.dist(D + 1) = d;
983 else
984 ✗ bv.dist(1) = bv.dist(D + 1) = -d;
985 ✗ } else if (n[0] == (Scalar)0.0 && n[1] == (Scalar)0.0) {
986 ✗ if (n[2] > 0)
987 ✗ bv.dist(2) = bv.dist(D + 2) = d;
988 else
989 ✗ bv.dist(2) = bv.dist(D + 2) = -d;
990 ✗ } else if (n[2] == (Scalar)0.0 && n[0] == n[1]) {
991 ✗ bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2;
992 ✗ } else if (n[1] == (Scalar)0.0 && n[0] == n[2]) {
993 ✗ bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2;
994 ✗ } else if (n[0] == (Scalar)0.0 && n[1] == n[2]) {
995 ✗ bv.dist(5) = bv.dist(D + 5) = n[1] * d * 2;
996 ✗ } else if (n[2] == (Scalar)0.0 && n[0] + n[1] == (Scalar)0.0) {
997 ✗ bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2;
998 ✗ } else if (n[1] == (Scalar)0.0 && n[0] + n[2] == (Scalar)0.0) {
999 ✗ bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2;
1000 ✗ } else if (n[0] == (Scalar)0.0 && n[1] + n[2] == (Scalar)0.0) {
1001 ✗ bv.dist(8) = bv.dist(D + 8) = n[1] * d * 2;
1002 ✗ } else if (n[0] + n[2] == (Scalar)0.0 && n[0] + n[1] == (Scalar)0.0) {
1003 ✗ bv.dist(9) = bv.dist(D + 9) = n[0] * d * 3;
1004 ✗ } else if (n[0] + n[1] == (Scalar)0.0 && n[1] + n[2] == (Scalar)0.0) {
1005 ✗ bv.dist(10) = bv.dist(D + 10) = n[0] * d * 3;
1006 ✗ } else if (n[0] + n[1] == (Scalar)0.0 && n[0] + n[2] == (Scalar)0.0) {
1007 ✗ bv.dist(11) = bv.dist(D + 11) = n[1] * d * 3;
1008 }
1009 }
1010
1011 40 void constructBox(const AABB& bv, Box& box, Transform3s& tf) {
1012
3/6
✓ Branch 2 taken 40 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 40 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 40 times.
✗ Branch 9 not taken.
 40 box = Box(bv.max_ - bv.min_);
1013
2/4
✓ Branch 2 taken 40 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 40 times.
✗ Branch 6 not taken.
 40 tf = Transform3s(bv.center());
1014 40 }
1015
1016 ✗ void constructBox(const OBB& bv, Box& box, Transform3s& tf) {
1017 ✗ box = Box(bv.extent * 2);
1018 ✗ tf = Transform3s(bv.axes, bv.To);
1019 }
1020
1021 ✗ void constructBox(const OBBRSS& bv, Box& box, Transform3s& tf) {
1022 ✗ box = Box(bv.obb.extent * 2);
1023 ✗ tf = Transform3s(bv.obb.axes, bv.obb.To);
1024 }
1025
1026 ✗ void constructBox(const kIOS& bv, Box& box, Transform3s& tf) {
1027 ✗ box = Box(bv.obb.extent * 2);
1028 ✗ tf = Transform3s(bv.obb.axes, bv.obb.To);
1029 }
1030
1031 ✗ void constructBox(const RSS& bv, Box& box, Transform3s& tf) {
1032 ✗ box = Box(bv.width(), bv.height(), bv.depth());
1033 ✗ tf = Transform3s(bv.axes, bv.Tr);
1034 }
1035
1036 ✗ void constructBox(const KDOP<16>& bv, Box& box, Transform3s& tf) {
1037 ✗ box = Box(bv.width(), bv.height(), bv.depth());
1038 ✗ tf = Transform3s(bv.center());
1039 }
1040
1041 ✗ void constructBox(const KDOP<18>& bv, Box& box, Transform3s& tf) {
1042 ✗ box = Box(bv.width(), bv.height(), bv.depth());
1043 ✗ tf = Transform3s(bv.center());
1044 }
1045
1046 ✗ void constructBox(const KDOP<24>& bv, Box& box, Transform3s& tf) {
1047 ✗ box = Box(bv.width(), bv.height(), bv.depth());
1048 ✗ tf = Transform3s(bv.center());
1049 }
1050
1051 2589 void constructBox(const AABB& bv, const Transform3s& tf_bv, Box& box,
1052 Transform3s& tf) {
1053
3/6
✓ Branch 2 taken 2589 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2589 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2589 times.
✗ Branch 9 not taken.
 2589 box = Box(bv.max_ - bv.min_);
1054
3/6
✓ Branch 2 taken 2589 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2589 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2589 times.
✗ Branch 9 not taken.
 2589 tf = tf_bv * Transform3s(bv.center());
1055 2589 }
1056
1057 ✗ void constructBox(const OBB& bv, const Transform3s& tf_bv, Box& box,
1058 Transform3s& tf) {
1059 ✗ box = Box(bv.extent * 2);
1060 ✗ tf = tf_bv * Transform3s(bv.axes, bv.To);
1061 }
1062
1063 ✗ void constructBox(const OBBRSS& bv, const Transform3s& tf_bv, Box& box,
1064 Transform3s& tf) {
1065 ✗ box = Box(bv.obb.extent * 2);
1066 ✗ tf = tf_bv * Transform3s(bv.obb.axes, bv.obb.To);
1067 }
1068
1069 ✗ void constructBox(const kIOS& bv, const Transform3s& tf_bv, Box& box,
1070 Transform3s& tf) {
1071 ✗ box = Box(bv.obb.extent * 2);
1072 ✗ tf = tf_bv * Transform3s(bv.obb.axes, bv.obb.To);
1073 }
1074
1075 ✗ void constructBox(const RSS& bv, const Transform3s& tf_bv, Box& box,
1076 Transform3s& tf) {
1077 ✗ box = Box(bv.width(), bv.height(), bv.depth());
1078 ✗ tf = tf_bv * Transform3s(bv.axes, bv.Tr);
1079 }
1080
1081 ✗ void constructBox(const KDOP<16>& bv, const Transform3s& tf_bv, Box& box,
1082 Transform3s& tf) {
1083 ✗ box = Box(bv.width(), bv.height(), bv.depth());
1084 ✗ tf = tf_bv * Transform3s(bv.center());
1085 }
1086
1087 ✗ void constructBox(const KDOP<18>& bv, const Transform3s& tf_bv, Box& box,
1088 Transform3s& tf) {
1089 ✗ box = Box(bv.width(), bv.height(), bv.depth());
1090 ✗ tf = tf_bv * Transform3s(bv.center());
1091 }
1092
1093 ✗ void constructBox(const KDOP<24>& bv, const Transform3s& tf_bv, Box& box,
1094 Transform3s& tf) {
1095 ✗ box = Box(bv.width(), bv.height(), bv.depth());
1096 ✗ tf = tf_bv * Transform3s(bv.center());
1097 }
1098
1099 } // namespace coal
1100