GCC Code Coverage Report


Directory: ./
File: src/shape/geometric_shapes_utility.cpp
Date: 2025-04-01 09:23:31
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