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 |