Directory: | ./ |
---|---|
File: | include/coal/shape/geometric_shape_to_BVH_model.h |
Date: | 2025-04-01 09:23:31 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 187 | 189 | 98.9% |
Branches: | 154 | 236 | 65.3% |
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 | #ifndef COAL_GEOMETRIC_SHAPE_TO_BVH_MODEL_H | ||
39 | #define COAL_GEOMETRIC_SHAPE_TO_BVH_MODEL_H | ||
40 | |||
41 | #include "coal/shape/geometric_shapes.h" | ||
42 | #include "coal/BVH/BVH_model.h" | ||
43 | #include <boost/math/constants/constants.hpp> | ||
44 | |||
45 | namespace coal { | ||
46 | |||
47 | /// @brief Generate BVH model from box | ||
48 | template <typename BV> | ||
49 | 1866 | void generateBVHModel(BVHModel<BV>& model, const Box& shape, | |
50 | const Transform3s& pose) { | ||
51 |
1/2✓ Branch 1 taken 1866 times.
✗ Branch 2 not taken.
|
1866 | Scalar a = shape.halfSide[0]; |
52 |
1/2✓ Branch 1 taken 1866 times.
✗ Branch 2 not taken.
|
1866 | Scalar b = shape.halfSide[1]; |
53 |
1/2✓ Branch 1 taken 1866 times.
✗ Branch 2 not taken.
|
1866 | Scalar c = shape.halfSide[2]; |
54 |
1/2✓ Branch 2 taken 1866 times.
✗ Branch 3 not taken.
|
1866 | std::vector<Vec3s> points(8); |
55 |
1/2✓ Branch 2 taken 1866 times.
✗ Branch 3 not taken.
|
1866 | std::vector<Triangle> tri_indices(12); |
56 |
1/2✓ Branch 1 taken 1866 times.
✗ Branch 2 not taken.
|
1866 | points[0] = Vec3s(a, -b, c); |
57 |
1/2✓ Branch 1 taken 1866 times.
✗ Branch 2 not taken.
|
1866 | points[1] = Vec3s(a, b, c); |
58 |
1/2✓ Branch 1 taken 1866 times.
✗ Branch 2 not taken.
|
1866 | points[2] = Vec3s(-a, b, c); |
59 |
1/2✓ Branch 1 taken 1866 times.
✗ Branch 2 not taken.
|
1866 | points[3] = Vec3s(-a, -b, c); |
60 |
1/2✓ Branch 1 taken 1866 times.
✗ Branch 2 not taken.
|
1866 | points[4] = Vec3s(a, -b, -c); |
61 |
1/2✓ Branch 1 taken 1866 times.
✗ Branch 2 not taken.
|
1866 | points[5] = Vec3s(a, b, -c); |
62 |
1/2✓ Branch 1 taken 1866 times.
✗ Branch 2 not taken.
|
1866 | points[6] = Vec3s(-a, b, -c); |
63 |
1/2✓ Branch 1 taken 1866 times.
✗ Branch 2 not taken.
|
1866 | points[7] = Vec3s(-a, -b, -c); |
64 | |||
65 | 1866 | tri_indices[0].set(0, 4, 1); | |
66 | 1866 | tri_indices[1].set(1, 4, 5); | |
67 | 1866 | tri_indices[2].set(2, 6, 3); | |
68 | 1866 | tri_indices[3].set(3, 6, 7); | |
69 | 1866 | tri_indices[4].set(3, 0, 2); | |
70 | 1866 | tri_indices[5].set(2, 0, 1); | |
71 | 1866 | tri_indices[6].set(6, 5, 7); | |
72 | 1866 | tri_indices[7].set(7, 5, 4); | |
73 | 1866 | tri_indices[8].set(1, 5, 2); | |
74 | 1866 | tri_indices[9].set(2, 5, 6); | |
75 | 1866 | tri_indices[10].set(3, 7, 0); | |
76 | 1866 | tri_indices[11].set(0, 7, 4); | |
77 | |||
78 |
2/2✓ Branch 1 taken 14928 times.
✓ Branch 2 taken 1866 times.
|
16794 | for (unsigned int i = 0; i < points.size(); ++i) { |
79 |
2/4✓ Branch 2 taken 14928 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 14928 times.
✗ Branch 8 not taken.
|
14928 | points[i] = pose.transform(points[i]).eval(); |
80 | } | ||
81 | |||
82 |
1/2✓ Branch 1 taken 1866 times.
✗ Branch 2 not taken.
|
1866 | model.beginModel(); |
83 |
1/2✓ Branch 1 taken 1866 times.
✗ Branch 2 not taken.
|
1866 | model.addSubModel(points, tri_indices); |
84 |
1/2✓ Branch 1 taken 1866 times.
✗ Branch 2 not taken.
|
1866 | model.endModel(); |
85 |
1/2✓ Branch 1 taken 1866 times.
✗ Branch 2 not taken.
|
1866 | model.computeLocalAABB(); |
86 | 1866 | } | |
87 | |||
88 | /// @brief Generate BVH model from sphere, given the number of segments along | ||
89 | /// longitude and number of rings along latitude. | ||
90 | template <typename BV> | ||
91 | 369 | void generateBVHModel(BVHModel<BV>& model, const Sphere& shape, | |
92 | const Transform3s& pose, unsigned int seg, | ||
93 | unsigned int ring) { | ||
94 | 369 | std::vector<Vec3s> points; | |
95 | 369 | std::vector<Triangle> tri_indices; | |
96 | |||
97 | 369 | Scalar r = shape.radius; | |
98 | Scalar phi, phid; | ||
99 | 369 | const Scalar pi = boost::math::constants::pi<Scalar>(); | |
100 | 369 | phid = pi * 2 / Scalar(seg); | |
101 | 369 | phi = 0; | |
102 | |||
103 | Scalar theta, thetad; | ||
104 | 369 | thetad = pi / Scalar(ring + 1); | |
105 | 369 | theta = 0; | |
106 | |||
107 |
2/2✓ Branch 0 taken 5887 times.
✓ Branch 1 taken 369 times.
|
6256 | for (unsigned int i = 0; i < ring; ++i) { |
108 | 5887 | Scalar theta_ = theta + thetad * Scalar(i + 1); | |
109 |
2/2✓ Branch 0 taken 94077 times.
✓ Branch 1 taken 5887 times.
|
99964 | for (unsigned int j = 0; j < seg; ++j) { |
110 |
1/2✓ Branch 1 taken 94077 times.
✗ Branch 2 not taken.
|
94077 | points.push_back(Vec3s(r * sin(theta_) * cos(phi + Scalar(j) * phid), |
111 | ✗ | r * sin(theta_) * sin(phi + Scalar(j) * phid), | |
112 |
1/2✓ Branch 1 taken 94077 times.
✗ Branch 2 not taken.
|
188154 | r * cos(theta_))); |
113 | } | ||
114 | } | ||
115 |
2/4✓ Branch 1 taken 369 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 369 times.
✗ Branch 5 not taken.
|
369 | points.push_back(Vec3s(0, 0, r)); |
116 |
2/4✓ Branch 1 taken 369 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 369 times.
✗ Branch 5 not taken.
|
369 | points.push_back(Vec3s(0, 0, -r)); |
117 | |||
118 |
2/2✓ Branch 0 taken 5518 times.
✓ Branch 1 taken 369 times.
|
5887 | for (unsigned int i = 0; i < ring - 1; ++i) { |
119 |
2/2✓ Branch 0 taken 88190 times.
✓ Branch 1 taken 5518 times.
|
93708 | for (unsigned int j = 0; j < seg; ++j) { |
120 | unsigned int a, b, c, d; | ||
121 | 88190 | a = i * seg + j; | |
122 |
2/2✓ Branch 0 taken 5518 times.
✓ Branch 1 taken 82672 times.
|
88190 | b = (j == seg - 1) ? (i * seg) : (i * seg + j + 1); |
123 | 88190 | c = (i + 1) * seg + j; | |
124 |
2/2✓ Branch 0 taken 5518 times.
✓ Branch 1 taken 82672 times.
|
88190 | d = (j == seg - 1) ? ((i + 1) * seg) : ((i + 1) * seg + j + 1); |
125 |
2/4✓ Branch 1 taken 88190 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 88190 times.
✗ Branch 5 not taken.
|
88190 | tri_indices.push_back(Triangle(a, c, b)); |
126 |
2/4✓ Branch 1 taken 88190 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 88190 times.
✗ Branch 5 not taken.
|
88190 | tri_indices.push_back(Triangle(b, c, d)); |
127 | } | ||
128 | } | ||
129 | |||
130 |
2/2✓ Branch 0 taken 5887 times.
✓ Branch 1 taken 369 times.
|
6256 | for (unsigned int j = 0; j < seg; ++j) { |
131 | unsigned int a, b; | ||
132 | 5887 | a = j; | |
133 |
2/2✓ Branch 0 taken 5518 times.
✓ Branch 1 taken 369 times.
|
5887 | b = (j == seg - 1) ? 0 : (j + 1); |
134 |
2/4✓ Branch 1 taken 5887 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5887 times.
✗ Branch 5 not taken.
|
5887 | tri_indices.push_back(Triangle(ring * seg, a, b)); |
135 | |||
136 | 5887 | a = (ring - 1) * seg + j; | |
137 |
2/2✓ Branch 0 taken 369 times.
✓ Branch 1 taken 5518 times.
|
5887 | b = (j == seg - 1) ? (ring - 1) * seg : ((ring - 1) * seg + j + 1); |
138 |
2/4✓ Branch 1 taken 5887 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5887 times.
✗ Branch 5 not taken.
|
5887 | tri_indices.push_back(Triangle(a, ring * seg + 1, b)); |
139 | } | ||
140 | |||
141 |
2/2✓ Branch 1 taken 94815 times.
✓ Branch 2 taken 369 times.
|
95184 | for (unsigned int i = 0; i < points.size(); ++i) { |
142 |
1/2✓ Branch 2 taken 94815 times.
✗ Branch 3 not taken.
|
94815 | points[i] = pose.transform(points[i]); |
143 | } | ||
144 | |||
145 |
1/2✓ Branch 1 taken 369 times.
✗ Branch 2 not taken.
|
369 | model.beginModel(); |
146 |
1/2✓ Branch 1 taken 369 times.
✗ Branch 2 not taken.
|
369 | model.addSubModel(points, tri_indices); |
147 |
1/2✓ Branch 1 taken 369 times.
✗ Branch 2 not taken.
|
369 | model.endModel(); |
148 |
1/2✓ Branch 1 taken 369 times.
✗ Branch 2 not taken.
|
369 | model.computeLocalAABB(); |
149 | 369 | } | |
150 | |||
151 | /// @brief Generate BVH model from sphere | ||
152 | /// The difference between generateBVHModel is that it gives the number of | ||
153 | /// triangles faces N for a sphere with unit radius. For sphere of radius r, | ||
154 | /// then the number of triangles is r * r * N so that the area represented by a | ||
155 | /// single triangle is approximately the same.s | ||
156 | template <typename BV> | ||
157 | 1 | void generateBVHModel(BVHModel<BV>& model, const Sphere& shape, | |
158 | const Transform3s& pose, | ||
159 | unsigned int n_faces_for_unit_sphere) { | ||
160 | 1 | Scalar r = shape.radius; | |
161 | 1 | Scalar n_low_bound = | |
162 | 1 | std::sqrt((Scalar)n_faces_for_unit_sphere / Scalar(2.)) * r * r; | |
163 | 1 | unsigned int ring = (unsigned int)ceil(n_low_bound); | |
164 | 1 | unsigned int seg = (unsigned int)ceil(n_low_bound); | |
165 | |||
166 | 1 | generateBVHModel(model, shape, pose, seg, ring); | |
167 | 1 | } | |
168 | |||
169 | /// @brief Generate BVH model from cylinder, given the number of segments along | ||
170 | /// circle and the number of segments along axis. | ||
171 | template <typename BV> | ||
172 | 369 | void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape, | |
173 | const Transform3s& pose, unsigned int tot, | ||
174 | unsigned int h_num) { | ||
175 | 369 | std::vector<Vec3s> points; | |
176 | 369 | std::vector<Triangle> tri_indices; | |
177 | |||
178 | 369 | Scalar r = shape.radius; | |
179 | 369 | Scalar h = shape.halfLength; | |
180 | Scalar phi, phid; | ||
181 | 369 | const Scalar pi = boost::math::constants::pi<Scalar>(); | |
182 | 369 | phid = pi * 2 / Scalar(tot); | |
183 | 369 | phi = 0; | |
184 | |||
185 | 369 | Scalar hd = 2 * h / Scalar(h_num); | |
186 | |||
187 |
2/2✓ Branch 0 taken 5932 times.
✓ Branch 1 taken 369 times.
|
6301 | for (unsigned int i = 0; i < tot; ++i) |
188 |
1/2✓ Branch 1 taken 5932 times.
✗ Branch 2 not taken.
|
5932 | points.push_back(Vec3s(r * cos(phi + phid * Scalar(i)), |
189 |
1/2✓ Branch 1 taken 5932 times.
✗ Branch 2 not taken.
|
11864 | r * sin(phi + phid * Scalar(i)), h)); |
190 | |||
191 |
2/2✓ Branch 0 taken 5521 times.
✓ Branch 1 taken 369 times.
|
5890 | for (unsigned int i = 0; i < h_num - 1; ++i) { |
192 |
2/2✓ Branch 0 taken 88520 times.
✓ Branch 1 taken 5521 times.
|
94041 | for (unsigned int j = 0; j < tot; ++j) { |
193 |
1/2✓ Branch 1 taken 88520 times.
✗ Branch 2 not taken.
|
88520 | points.push_back(Vec3s(r * cos(phi + phid * Scalar(j)), |
194 | ✗ | r * sin(phi + phid * Scalar(j)), | |
195 |
1/2✓ Branch 1 taken 88520 times.
✗ Branch 2 not taken.
|
177040 | h - Scalar(i + 1) * hd)); |
196 | } | ||
197 | } | ||
198 | |||
199 |
2/2✓ Branch 0 taken 5932 times.
✓ Branch 1 taken 369 times.
|
6301 | for (unsigned int i = 0; i < tot; ++i) |
200 |
1/2✓ Branch 1 taken 5932 times.
✗ Branch 2 not taken.
|
5932 | points.push_back(Vec3s(r * cos(phi + phid * Scalar(i)), |
201 |
1/2✓ Branch 1 taken 5932 times.
✗ Branch 2 not taken.
|
11864 | r * sin(phi + phid * Scalar(i)), -h)); |
202 | |||
203 |
2/4✓ Branch 1 taken 369 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 369 times.
✗ Branch 5 not taken.
|
369 | points.push_back(Vec3s(0, 0, h)); |
204 |
2/4✓ Branch 1 taken 369 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 369 times.
✗ Branch 5 not taken.
|
369 | points.push_back(Vec3s(0, 0, -h)); |
205 | |||
206 |
2/2✓ Branch 0 taken 5932 times.
✓ Branch 1 taken 369 times.
|
6301 | for (unsigned int i = 0; i < tot; ++i) { |
207 |
3/4✓ Branch 0 taken 5563 times.
✓ Branch 1 taken 369 times.
✓ Branch 3 taken 5932 times.
✗ Branch 4 not taken.
|
5932 | Triangle tmp((h_num + 1) * tot, i, ((i == tot - 1) ? 0 : (i + 1))); |
208 |
1/2✓ Branch 1 taken 5932 times.
✗ Branch 2 not taken.
|
5932 | tri_indices.push_back(tmp); |
209 | } | ||
210 | |||
211 |
2/2✓ Branch 0 taken 5932 times.
✓ Branch 1 taken 369 times.
|
6301 | for (unsigned int i = 0; i < tot; ++i) { |
212 | 11864 | Triangle tmp((h_num + 1) * tot + 1, | |
213 |
3/4✓ Branch 0 taken 5563 times.
✓ Branch 1 taken 369 times.
✓ Branch 3 taken 5932 times.
✗ Branch 4 not taken.
|
5932 | h_num * tot + ((i == tot - 1) ? 0 : (i + 1)), h_num * tot + i); |
214 |
1/2✓ Branch 1 taken 5932 times.
✗ Branch 2 not taken.
|
5932 | tri_indices.push_back(tmp); |
215 | } | ||
216 | |||
217 |
2/2✓ Branch 0 taken 5890 times.
✓ Branch 1 taken 369 times.
|
6259 | for (unsigned int i = 0; i < h_num; ++i) { |
218 |
2/2✓ Branch 0 taken 94452 times.
✓ Branch 1 taken 5890 times.
|
100342 | for (unsigned int j = 0; j < tot; ++j) { |
219 | unsigned int a, b, c, d; | ||
220 | 94452 | a = j; | |
221 |
2/2✓ Branch 0 taken 88562 times.
✓ Branch 1 taken 5890 times.
|
94452 | b = (j == tot - 1) ? 0 : (j + 1); |
222 | 94452 | c = j + tot; | |
223 |
2/2✓ Branch 0 taken 88562 times.
✓ Branch 1 taken 5890 times.
|
94452 | d = (j == tot - 1) ? tot : (j + 1 + tot); |
224 | |||
225 | 94452 | unsigned int start = i * tot; | |
226 |
2/4✓ Branch 1 taken 94452 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 94452 times.
✗ Branch 5 not taken.
|
94452 | tri_indices.push_back(Triangle(start + b, start + a, start + c)); |
227 |
2/4✓ Branch 1 taken 94452 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 94452 times.
✗ Branch 5 not taken.
|
94452 | tri_indices.push_back(Triangle(start + b, start + c, start + d)); |
228 | } | ||
229 | } | ||
230 | |||
231 |
2/2✓ Branch 1 taken 101122 times.
✓ Branch 2 taken 369 times.
|
101491 | for (unsigned int i = 0; i < points.size(); ++i) { |
232 |
1/2✓ Branch 2 taken 101122 times.
✗ Branch 3 not taken.
|
101122 | points[i] = pose.transform(points[i]); |
233 | } | ||
234 | |||
235 |
1/2✓ Branch 1 taken 369 times.
✗ Branch 2 not taken.
|
369 | model.beginModel(); |
236 |
1/2✓ Branch 1 taken 369 times.
✗ Branch 2 not taken.
|
369 | model.addSubModel(points, tri_indices); |
237 |
1/2✓ Branch 1 taken 369 times.
✗ Branch 2 not taken.
|
369 | model.endModel(); |
238 |
1/2✓ Branch 1 taken 369 times.
✗ Branch 2 not taken.
|
369 | model.computeLocalAABB(); |
239 | 369 | } | |
240 | |||
241 | /// @brief Generate BVH model from cylinder | ||
242 | /// Difference from generateBVHModel: is that it gives the circle split number | ||
243 | /// tot for a cylinder with unit radius. For cylinder with larger radius, the | ||
244 | /// number of circle split number is r * tot. | ||
245 | template <typename BV> | ||
246 | 1 | void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape, | |
247 | const Transform3s& pose, | ||
248 | unsigned int tot_for_unit_cylinder) { | ||
249 | 1 | Scalar r = shape.radius; | |
250 | 1 | Scalar h = 2 * shape.halfLength; | |
251 | |||
252 | 1 | const Scalar pi = boost::math::constants::pi<Scalar>(); | |
253 | 1 | unsigned int tot = (unsigned int)(Scalar(tot_for_unit_cylinder) * r); | |
254 | 1 | Scalar phid = pi * 2 / Scalar(tot); | |
255 | |||
256 | 1 | Scalar circle_edge = phid * r; | |
257 | 1 | unsigned int h_num = (unsigned int)ceil(h / circle_edge); | |
258 | |||
259 | 1 | generateBVHModel(model, shape, pose, tot, h_num); | |
260 | 1 | } | |
261 | |||
262 | /// @brief Generate BVH model from cone, given the number of segments along | ||
263 | /// circle and the number of segments along axis. | ||
264 | template <typename BV> | ||
265 | 2 | void generateBVHModel(BVHModel<BV>& model, const Cone& shape, | |
266 | const Transform3s& pose, unsigned int tot, | ||
267 | unsigned int h_num) { | ||
268 | 2 | std::vector<Vec3s> points; | |
269 | 2 | std::vector<Triangle> tri_indices; | |
270 | |||
271 | 2 | Scalar r = shape.radius; | |
272 | 2 | Scalar h = shape.halfLength; | |
273 | |||
274 | Scalar phi, phid; | ||
275 | 2 | const Scalar pi = boost::math::constants::pi<Scalar>(); | |
276 | 2 | phid = pi * 2 / Scalar(tot); | |
277 | 2 | phi = 0; | |
278 | |||
279 | 2 | Scalar hd = 2 * h / Scalar(h_num); | |
280 | |||
281 |
2/2✓ Branch 0 taken 16 times.
✓ Branch 1 taken 2 times.
|
18 | for (unsigned int i = 0; i < h_num - 1; ++i) { |
282 | 16 | Scalar h_i = h - Scalar(i + 1) * hd; | |
283 | 16 | Scalar rh = r * (Scalar(0.5) - h_i / h / 2); | |
284 |
2/2✓ Branch 0 taken 440 times.
✓ Branch 1 taken 16 times.
|
456 | for (unsigned int j = 0; j < tot; ++j) { |
285 |
1/2✓ Branch 1 taken 440 times.
✗ Branch 2 not taken.
|
440 | points.push_back(Vec3s(rh * cos(phi + phid * Scalar(j)), |
286 |
1/2✓ Branch 1 taken 440 times.
✗ Branch 2 not taken.
|
880 | rh * sin(phi + phid * Scalar(j)), h_i)); |
287 | } | ||
288 | } | ||
289 | |||
290 |
2/2✓ Branch 0 taken 60 times.
✓ Branch 1 taken 2 times.
|
62 | for (unsigned int i = 0; i < tot; ++i) |
291 |
1/2✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
|
60 | points.push_back(Vec3s(r * cos(phi + phid * Scalar(i)), |
292 |
1/2✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
|
120 | r * sin(phi + phid * Scalar(i)), -h)); |
293 | |||
294 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
|
2 | points.push_back(Vec3s(0, 0, h)); |
295 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
|
2 | points.push_back(Vec3s(0, 0, -h)); |
296 | |||
297 |
2/2✓ Branch 0 taken 60 times.
✓ Branch 1 taken 2 times.
|
62 | for (unsigned int i = 0; i < tot; ++i) { |
298 |
3/4✓ Branch 0 taken 58 times.
✓ Branch 1 taken 2 times.
✓ Branch 3 taken 60 times.
✗ Branch 4 not taken.
|
60 | Triangle tmp(h_num * tot, i, (i == tot - 1) ? 0 : (i + 1)); |
299 |
1/2✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
|
60 | tri_indices.push_back(tmp); |
300 | } | ||
301 | |||
302 |
2/2✓ Branch 0 taken 60 times.
✓ Branch 1 taken 2 times.
|
62 | for (unsigned int i = 0; i < tot; ++i) { |
303 | 120 | Triangle tmp(h_num * tot + 1, | |
304 |
1/2✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
|
60 | (h_num - 1) * tot + ((i == tot - 1) ? 0 : (i + 1)), |
305 |
2/2✓ Branch 0 taken 58 times.
✓ Branch 1 taken 2 times.
|
60 | (h_num - 1) * tot + i); |
306 |
1/2✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
|
60 | tri_indices.push_back(tmp); |
307 | } | ||
308 | |||
309 |
2/2✓ Branch 0 taken 16 times.
✓ Branch 1 taken 2 times.
|
18 | for (unsigned int i = 0; i < h_num - 1; ++i) { |
310 |
2/2✓ Branch 0 taken 440 times.
✓ Branch 1 taken 16 times.
|
456 | for (unsigned int j = 0; j < tot; ++j) { |
311 | unsigned int a, b, c, d; | ||
312 | 440 | a = j; | |
313 |
2/2✓ Branch 0 taken 424 times.
✓ Branch 1 taken 16 times.
|
440 | b = (j == tot - 1) ? 0 : (j + 1); |
314 | 440 | c = j + tot; | |
315 |
2/2✓ Branch 0 taken 424 times.
✓ Branch 1 taken 16 times.
|
440 | d = (j == tot - 1) ? tot : (j + 1 + tot); |
316 | |||
317 | 440 | unsigned int start = i * tot; | |
318 |
2/4✓ Branch 1 taken 440 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 440 times.
✗ Branch 5 not taken.
|
440 | tri_indices.push_back(Triangle(start + b, start + a, start + c)); |
319 |
2/4✓ Branch 1 taken 440 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 440 times.
✗ Branch 5 not taken.
|
440 | tri_indices.push_back(Triangle(start + b, start + c, start + d)); |
320 | } | ||
321 | } | ||
322 | |||
323 |
2/2✓ Branch 1 taken 504 times.
✓ Branch 2 taken 2 times.
|
506 | for (unsigned int i = 0; i < points.size(); ++i) { |
324 |
1/2✓ Branch 2 taken 504 times.
✗ Branch 3 not taken.
|
504 | points[i] = pose.transform(points[i]); |
325 | } | ||
326 | |||
327 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | model.beginModel(); |
328 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | model.addSubModel(points, tri_indices); |
329 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | model.endModel(); |
330 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | model.computeLocalAABB(); |
331 | 2 | } | |
332 | |||
333 | /// @brief Generate BVH model from cone | ||
334 | /// Difference from generateBVHModel: is that it gives the circle split number | ||
335 | /// tot for a cylinder with unit radius. For cone with larger radius, the number | ||
336 | /// of circle split number is r * tot. | ||
337 | template <typename BV> | ||
338 | 1 | void generateBVHModel(BVHModel<BV>& model, const Cone& shape, | |
339 | const Transform3s& pose, unsigned int tot_for_unit_cone) { | ||
340 | 1 | Scalar r = shape.radius; | |
341 | 1 | Scalar h = 2 * shape.halfLength; | |
342 | |||
343 | 1 | const Scalar pi = boost::math::constants::pi<Scalar>(); | |
344 | 1 | unsigned int tot = (unsigned int)(Scalar(tot_for_unit_cone) * r); | |
345 | 1 | Scalar phid = pi * 2 / Scalar(tot); | |
346 | |||
347 | 1 | Scalar circle_edge = phid * r; | |
348 | 1 | unsigned int h_num = (unsigned int)ceil(h / circle_edge); | |
349 | |||
350 | 1 | generateBVHModel(model, shape, pose, tot, h_num); | |
351 | 1 | } | |
352 | |||
353 | } // namespace coal | ||
354 | |||
355 | #endif | ||
356 |