| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| 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 <hpp/fcl/BVH/BVH_utility.h> |
||
39 |
#include <hpp/fcl/narrowphase/narrowphase.h> |
||
40 |
#include <hpp/fcl/shape/geometric_shapes_utility.h> |
||
41 |
|||
42 |
namespace hpp { |
||
43 |
namespace fcl { |
||
44 |
|||
45 |
namespace details { |
||
46 |
template <typename BV> |
||
47 |
80 |
BVHModel<BV>* BVHExtract(const BVHModel<BV>& model, const Transform3f& pose, |
|
48 |
const AABB& _aabb) { |
||
49 |
✗✓ | 80 |
assert(model.getModelType() == BVH_MODEL_TRIANGLES); |
50 |
80 |
const Matrix3f& q = pose.getRotation(); |
|
51 |
✓✗✓✗ ✓✗ |
80 |
AABB aabb = translate(_aabb, -pose.getTranslation()); |
52 |
|||
53 |
✓✗ | 80 |
Transform3f box_pose; |
54 |
✓✗ | 160 |
Box box; |
55 |
✓✗ | 80 |
constructBox(_aabb, box, box_pose); |
56 |
✓✗✓✗ |
80 |
box_pose = pose.inverseTimes(box_pose); |
57 |
|||
58 |
✓✗ | 80 |
GJKSolver gjk; |
59 |
|||
60 |
// Check what triangles should be kept. |
||
61 |
// TODO use the BV hierarchy |
||
62 |
✓✗ | 160 |
std::vector<bool> keep_vertex(model.num_vertices, false); |
63 |
✓✗ | 160 |
std::vector<bool> keep_tri(model.num_tris, false); |
64 |
80 |
unsigned int ntri = 0; |
|
65 |
✓✓ | 1040 |
for (unsigned int i = 0; i < model.num_tris; ++i) { |
66 |
960 |
const Triangle& t = model.tri_indices[i]; |
|
67 |
|||
68 |
960 |
bool keep_this_tri = |
|
69 |
✓✗✓✗ ✗✓ |
960 |
keep_vertex[t[0]] || keep_vertex[t[1]] || keep_vertex[t[2]]; |
70 |
|||
71 |
✓✗ | 960 |
if (!keep_this_tri) { |
72 |
✓✓ | 2720 |
for (unsigned int j = 0; j < 3; ++j) { |
73 |
✓✗✓✗ ✓✗✓✓ |
2240 |
if (aabb.contain(q * model.vertices[t[j]])) { |
74 |
480 |
keep_this_tri = true; |
|
75 |
480 |
break; |
|
76 |
} |
||
77 |
} |
||
78 |
960 |
const Vec3f& p0 = model.vertices[t[0]]; |
|
79 |
960 |
const Vec3f& p1 = model.vertices[t[1]]; |
|
80 |
960 |
const Vec3f& p2 = model.vertices[t[2]]; |
|
81 |
✓✗✓✗ ✓✗ |
960 |
Vec3f c1, c2, normal; |
82 |
FCL_REAL distance; |
||
83 |
✓✓ | 1440 |
if (!keep_this_tri && |
84 |
✓✗✓✗ ✓✓✓✓ |
1440 |
gjk.shapeTriangleInteraction(box, box_pose, p0, p1, p2, Transform3f(), |
85 |
distance, c1, c2, normal)) { |
||
86 |
32 |
keep_this_tri = true; |
|
87 |
} |
||
88 |
} |
||
89 |
✓✓ | 960 |
if (keep_this_tri) { |
90 |
512 |
keep_vertex[t[0]] = keep_vertex[t[1]] = keep_vertex[t[2]] = true; |
|
91 |
512 |
keep_tri[i] = true; |
|
92 |
512 |
ntri++; |
|
93 |
} |
||
94 |
} |
||
95 |
|||
96 |
✓✓ | 80 |
if (ntri == 0) return NULL; |
97 |
|||
98 |
✓✗✓✗ |
64 |
BVHModel<BV>* new_model(new BVHModel<BV>()); |
99 |
✓✗ | 64 |
new_model->beginModel(ntri, std::min(ntri * 3, model.num_vertices)); |
100 |
✓✗ | 128 |
std::vector<unsigned int> idxConversion(model.num_vertices); |
101 |
✗✓ | 64 |
assert(new_model->num_vertices == 0); |
102 |
✓✓ | 2368 |
for (unsigned int i = 0; i < keep_vertex.size(); ++i) { |
103 |
✓✓ | 2304 |
if (keep_vertex[i]) { |
104 |
1536 |
idxConversion[i] = new_model->num_vertices; |
|
105 |
✓✗ | 1536 |
new_model->vertices[new_model->num_vertices] = model.vertices[i]; |
106 |
1536 |
new_model->num_vertices++; |
|
107 |
} |
||
108 |
} |
||
109 |
✗✓ | 64 |
assert(new_model->num_tris == 0); |
110 |
✓✓ | 832 |
for (unsigned int i = 0; i < keep_tri.size(); ++i) { |
111 |
✓✓ | 768 |
if (keep_tri[i]) { |
112 |
512 |
new_model->tri_indices[new_model->num_tris].set( |
|
113 |
512 |
idxConversion[model.tri_indices[i][0]], |
|
114 |
512 |
idxConversion[model.tri_indices[i][1]], |
|
115 |
512 |
idxConversion[model.tri_indices[i][2]]); |
|
116 |
512 |
new_model->num_tris++; |
|
117 |
} |
||
118 |
} |
||
119 |
✓✗✗✓ |
64 |
if (new_model->endModel() != BVH_OK) { |
120 |
delete new_model; |
||
121 |
return NULL; |
||
122 |
} |
||
123 |
64 |
return new_model; |
|
124 |
} |
||
125 |
} // namespace details |
||
126 |
|||
127 |
template <> |
||
128 |
5 |
BVHModel<OBB>* BVHExtract(const BVHModel<OBB>& model, const Transform3f& pose, |
|
129 |
const AABB& aabb) { |
||
130 |
5 |
return details::BVHExtract(model, pose, aabb); |
|
131 |
} |
||
132 |
template <> |
||
133 |
5 |
BVHModel<AABB>* BVHExtract(const BVHModel<AABB>& model, const Transform3f& pose, |
|
134 |
const AABB& aabb) { |
||
135 |
5 |
return details::BVHExtract(model, pose, aabb); |
|
136 |
} |
||
137 |
template <> |
||
138 |
5 |
BVHModel<RSS>* BVHExtract(const BVHModel<RSS>& model, const Transform3f& pose, |
|
139 |
const AABB& aabb) { |
||
140 |
5 |
return details::BVHExtract(model, pose, aabb); |
|
141 |
} |
||
142 |
template <> |
||
143 |
5 |
BVHModel<kIOS>* BVHExtract(const BVHModel<kIOS>& model, const Transform3f& pose, |
|
144 |
const AABB& aabb) { |
||
145 |
5 |
return details::BVHExtract(model, pose, aabb); |
|
146 |
} |
||
147 |
template <> |
||
148 |
5 |
BVHModel<OBBRSS>* BVHExtract(const BVHModel<OBBRSS>& model, |
|
149 |
const Transform3f& pose, const AABB& aabb) { |
||
150 |
5 |
return details::BVHExtract(model, pose, aabb); |
|
151 |
} |
||
152 |
template <> |
||
153 |
5 |
BVHModel<KDOP<16> >* BVHExtract(const BVHModel<KDOP<16> >& model, |
|
154 |
const Transform3f& pose, const AABB& aabb) { |
||
155 |
5 |
return details::BVHExtract(model, pose, aabb); |
|
156 |
} |
||
157 |
template <> |
||
158 |
5 |
BVHModel<KDOP<18> >* BVHExtract(const BVHModel<KDOP<18> >& model, |
|
159 |
const Transform3f& pose, const AABB& aabb) { |
||
160 |
5 |
return details::BVHExtract(model, pose, aabb); |
|
161 |
} |
||
162 |
template <> |
||
163 |
5 |
BVHModel<KDOP<24> >* BVHExtract(const BVHModel<KDOP<24> >& model, |
|
164 |
const Transform3f& pose, const AABB& aabb) { |
||
165 |
5 |
return details::BVHExtract(model, pose, aabb); |
|
166 |
} |
||
167 |
|||
168 |
1777280 |
void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, |
|
169 |
unsigned int n, Matrix3f& M) { |
||
170 |
✓✗✓✗ |
1777280 |
Vec3f S1(Vec3f::Zero()); |
171 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1777280 |
Vec3f S2[3] = {Vec3f::Zero(), Vec3f::Zero(), Vec3f::Zero()}; |
172 |
|||
173 |
✓✓ | 1777280 |
if (ts) { |
174 |
✓✓ | 11844083 |
for (unsigned int i = 0; i < n; ++i) { |
175 |
✓✗ | 10080352 |
const Triangle& t = (indices) ? ts[indices[i]] : ts[i]; |
176 |
|||
177 |
10080352 |
const Vec3f& p1 = ps[t[0]]; |
|
178 |
10080352 |
const Vec3f& p2 = ps[t[1]]; |
|
179 |
10080352 |
const Vec3f& p3 = ps[t[2]]; |
|
180 |
|||
181 |
✓✗✓✗ ✓✗✓✗ |
10080352 |
S1[0] += (p1[0] + p2[0] + p3[0]); |
182 |
✓✗✓✗ ✓✗✓✗ |
10080352 |
S1[1] += (p1[1] + p2[1] + p3[1]); |
183 |
✓✗✓✗ ✓✗✓✗ |
10080352 |
S1[2] += (p1[2] + p2[2] + p3[2]); |
184 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
10080352 |
S2[0][0] += (p1[0] * p1[0] + p2[0] * p2[0] + p3[0] * p3[0]); |
185 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
10080352 |
S2[1][1] += (p1[1] * p1[1] + p2[1] * p2[1] + p3[1] * p3[1]); |
186 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
10080352 |
S2[2][2] += (p1[2] * p1[2] + p2[2] * p2[2] + p3[2] * p3[2]); |
187 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
10080352 |
S2[0][1] += (p1[0] * p1[1] + p2[0] * p2[1] + p3[0] * p3[1]); |
188 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
10080352 |
S2[0][2] += (p1[0] * p1[2] + p2[0] * p2[2] + p3[0] * p3[2]); |
189 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
10080352 |
S2[1][2] += (p1[1] * p1[2] + p2[1] * p2[2] + p3[1] * p3[2]); |
190 |
|||
191 |
✗✓ | 10080352 |
if (ps2) { |
192 |
const Vec3f& p1 = ps2[t[0]]; |
||
193 |
const Vec3f& p2 = ps2[t[1]]; |
||
194 |
const Vec3f& p3 = ps2[t[2]]; |
||
195 |
|||
196 |
S1[0] += (p1[0] + p2[0] + p3[0]); |
||
197 |
S1[1] += (p1[1] + p2[1] + p3[1]); |
||
198 |
S1[2] += (p1[2] + p2[2] + p3[2]); |
||
199 |
|||
200 |
S2[0][0] += (p1[0] * p1[0] + p2[0] * p2[0] + p3[0] * p3[0]); |
||
201 |
S2[1][1] += (p1[1] * p1[1] + p2[1] * p2[1] + p3[1] * p3[1]); |
||
202 |
S2[2][2] += (p1[2] * p1[2] + p2[2] * p2[2] + p3[2] * p3[2]); |
||
203 |
S2[0][1] += (p1[0] * p1[1] + p2[0] * p2[1] + p3[0] * p3[1]); |
||
204 |
S2[0][2] += (p1[0] * p1[2] + p2[0] * p2[2] + p3[0] * p3[2]); |
||
205 |
S2[1][2] += (p1[1] * p1[2] + p2[1] * p2[2] + p3[1] * p3[2]); |
||
206 |
} |
||
207 |
} |
||
208 |
} else { |
||
209 |
✓✓ | 226977 |
for (unsigned int i = 0; i < n; ++i) { |
210 |
✗✓ | 213428 |
const Vec3f& p = (indices) ? ps[indices[i]] : ps[i]; |
211 |
✓✗ | 213428 |
S1 += p; |
212 |
✓✗✓✗ ✓✗ |
213428 |
S2[0][0] += (p[0] * p[0]); |
213 |
✓✗✓✗ ✓✗ |
213428 |
S2[1][1] += (p[1] * p[1]); |
214 |
✓✗✓✗ ✓✗ |
213428 |
S2[2][2] += (p[2] * p[2]); |
215 |
✓✗✓✗ ✓✗ |
213428 |
S2[0][1] += (p[0] * p[1]); |
216 |
✓✗✓✗ ✓✗ |
213428 |
S2[0][2] += (p[0] * p[2]); |
217 |
✓✗✓✗ ✓✗ |
213428 |
S2[1][2] += (p[1] * p[2]); |
218 |
|||
219 |
✗✓ | 213428 |
if (ps2) // another frame |
220 |
{ |
||
221 |
const Vec3f& p = (indices) ? ps2[indices[i]] : ps2[i]; |
||
222 |
S1 += p; |
||
223 |
S2[0][0] += (p[0] * p[0]); |
||
224 |
S2[1][1] += (p[1] * p[1]); |
||
225 |
S2[2][2] += (p[2] * p[2]); |
||
226 |
S2[0][1] += (p[0] * p[1]); |
||
227 |
S2[0][2] += (p[0] * p[2]); |
||
228 |
S2[1][2] += (p[1] * p[2]); |
||
229 |
} |
||
230 |
} |
||
231 |
} |
||
232 |
|||
233 |
✗✓✓✓ |
1777280 |
unsigned int n_points = ((ps2) ? 2 : 1) * ((ts) ? 3 : 1) * n; |
234 |
|||
235 |
✓✗✓✗ ✓✗✓✗ |
1777280 |
M(0, 0) = S2[0][0] - S1[0] * S1[0] / n_points; |
236 |
✓✗✓✗ ✓✗✓✗ |
1777280 |
M(1, 1) = S2[1][1] - S1[1] * S1[1] / n_points; |
237 |
✓✗✓✗ ✓✗✓✗ |
1777280 |
M(2, 2) = S2[2][2] - S1[2] * S1[2] / n_points; |
238 |
✓✗✓✗ ✓✗✓✗ |
1777280 |
M(0, 1) = S2[0][1] - S1[0] * S1[1] / n_points; |
239 |
✓✗✓✗ ✓✗✓✗ |
1777280 |
M(1, 2) = S2[1][2] - S1[1] * S1[2] / n_points; |
240 |
✓✗✓✗ ✓✗✓✗ |
1777280 |
M(0, 2) = S2[0][2] - S1[0] * S1[2] / n_points; |
241 |
✓✗✓✗ |
1777280 |
M(1, 0) = M(0, 1); |
242 |
✓✗✓✗ |
1777280 |
M(2, 0) = M(0, 2); |
243 |
✓✗✓✗ |
1777280 |
M(2, 1) = M(1, 2); |
244 |
1777280 |
} |
|
245 |
|||
246 |
/** @brief Compute the RSS bounding volume parameters: radius, rectangle size |
||
247 |
* and the origin. The bounding volume axes are known. |
||
248 |
*/ |
||
249 |
1335766 |
void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, |
|
250 |
unsigned int* indices, unsigned int n, |
||
251 |
const Matrix3f& axes, Vec3f& origin, |
||
252 |
FCL_REAL l[2], FCL_REAL& r) { |
||
253 |
1335766 |
bool indirect_index = true; |
|
254 |
✓✓ | 1335766 |
if (!indices) indirect_index = false; |
255 |
|||
256 |
✗✓✓✓ |
1335766 |
unsigned int size_P = ((ps2) ? 2 : 1) * ((ts) ? 3 : 1) * n; |
257 |
|||
258 |
✓✗✓✗ |
1335766 |
FCL_REAL(*P)[3] = new FCL_REAL[size_P][3]; |
259 |
|||
260 |
1335766 |
int P_id = 0; |
|
261 |
|||
262 |
✓✓ | 1335766 |
if (ts) { |
263 |
✓✓ | 8501055 |
for (unsigned int i = 0; i < n; ++i) { |
264 |
✓✗ | 7191670 |
unsigned int index = indirect_index ? indices[i] : i; |
265 |
7191670 |
const Triangle& t = ts[index]; |
|
266 |
|||
267 |
✓✓ | 28766680 |
for (Triangle::index_type j = 0; j < 3; ++j) { |
268 |
21575010 |
Triangle::index_type point_id = t[j]; |
|
269 |
21575010 |
const Vec3f& p = ps[point_id]; |
|
270 |
✓✗✓✗ ✓✗✓✗ |
21575010 |
Vec3f v(p[0], p[1], p[2]); |
271 |
✓✗✓✗ |
21575010 |
P[P_id][0] = axes.col(0).dot(v); |
272 |
✓✗✓✗ |
21575010 |
P[P_id][1] = axes.col(1).dot(v); |
273 |
✓✗✓✗ |
21575010 |
P[P_id][2] = axes.col(2).dot(v); |
274 |
21575010 |
P_id++; |
|
275 |
} |
||
276 |
|||
277 |
✗✓ | 7191670 |
if (ps2) { |
278 |
for (Triangle::index_type j = 0; j < 3; ++j) { |
||
279 |
Triangle::index_type point_id = t[j]; |
||
280 |
const Vec3f& p = ps2[point_id]; |
||
281 |
// FIXME Is this right ????? |
||
282 |
Vec3f v(p[0], p[1], p[2]); |
||
283 |
P[P_id][0] = axes.col(0).dot(v); |
||
284 |
P[P_id][1] = axes.col(0).dot(v); |
||
285 |
P[P_id][2] = axes.col(1).dot(v); |
||
286 |
P_id++; |
||
287 |
} |
||
288 |
} |
||
289 |
} |
||
290 |
} else { |
||
291 |
✓✓ | 276759 |
for (unsigned int i = 0; i < n; ++i) { |
292 |
✗✓ | 250378 |
unsigned int index = indirect_index ? indices[i] : i; |
293 |
|||
294 |
250378 |
const Vec3f& p = ps[index]; |
|
295 |
✓✗✓✗ ✓✗✓✗ |
250378 |
Vec3f v(p[0], p[1], p[2]); |
296 |
✓✗✓✗ |
250378 |
P[P_id][0] = axes.col(0).dot(v); |
297 |
✓✗✓✗ |
250378 |
P[P_id][1] = axes.col(1).dot(v); |
298 |
✓✗✓✗ |
250378 |
P[P_id][2] = axes.col(2).dot(v); |
299 |
250378 |
P_id++; |
|
300 |
|||
301 |
✗✓ | 250378 |
if (ps2) { |
302 |
const Vec3f& v = ps2[index]; |
||
303 |
P[P_id][0] = axes.col(0).dot(v); |
||
304 |
P[P_id][1] = axes.col(1).dot(v); |
||
305 |
P[P_id][2] = axes.col(2).dot(v); |
||
306 |
P_id++; |
||
307 |
} |
||
308 |
} |
||
309 |
} |
||
310 |
|||
311 |
FCL_REAL minx, maxx, miny, maxy, minz, maxz; |
||
312 |
|||
313 |
FCL_REAL cz, radsqr; |
||
314 |
|||
315 |
1335766 |
minz = maxz = P[0][2]; |
|
316 |
|||
317 |
✓✓ | 21825388 |
for (unsigned int i = 1; i < size_P; ++i) { |
318 |
20489622 |
FCL_REAL z_value = P[i][2]; |
|
319 |
✓✓ | 20489622 |
if (z_value < minz) |
320 |
1668573 |
minz = z_value; |
|
321 |
✓✓ | 18821049 |
else if (z_value > maxz) |
322 |
1650791 |
maxz = z_value; |
|
323 |
} |
||
324 |
|||
325 |
1335766 |
r = (FCL_REAL)0.5 * (maxz - minz); |
|
326 |
1335766 |
radsqr = r * r; |
|
327 |
1335766 |
cz = (FCL_REAL)0.5 * (maxz + minz); |
|
328 |
|||
329 |
// compute an initial norm of rectangle along x direction |
||
330 |
|||
331 |
// find minx and maxx as starting points |
||
332 |
|||
333 |
1335766 |
unsigned int minindex = 0, maxindex = 0; |
|
334 |
FCL_REAL mintmp, maxtmp; |
||
335 |
1335766 |
mintmp = maxtmp = P[0][0]; |
|
336 |
|||
337 |
✓✓ | 21825388 |
for (unsigned int i = 1; i < size_P; ++i) { |
338 |
20489622 |
FCL_REAL x_value = P[i][0]; |
|
339 |
✓✓ | 20489622 |
if (x_value < mintmp) { |
340 |
1720586 |
minindex = i; |
|
341 |
1720586 |
mintmp = x_value; |
|
342 |
✓✓ | 18769036 |
} else if (x_value > maxtmp) { |
343 |
1759224 |
maxindex = i; |
|
344 |
1759224 |
maxtmp = x_value; |
|
345 |
} |
||
346 |
} |
||
347 |
|||
348 |
FCL_REAL x, dz; |
||
349 |
1335766 |
dz = P[minindex][2] - cz; |
|
350 |
1335766 |
minx = P[minindex][0] + std::sqrt(std::max<FCL_REAL>(radsqr - dz * dz, 0)); |
|
351 |
1335766 |
dz = P[maxindex][2] - cz; |
|
352 |
1335766 |
maxx = P[maxindex][0] - std::sqrt(std::max<FCL_REAL>(radsqr - dz * dz, 0)); |
|
353 |
|||
354 |
// grow minx/maxx |
||
355 |
|||
356 |
✓✓ | 23161154 |
for (unsigned int i = 0; i < size_P; ++i) { |
357 |
✓✓ | 21825388 |
if (P[i][0] < minx) { |
358 |
3130279 |
dz = P[i][2] - cz; |
|
359 |
3130279 |
x = P[i][0] + std::sqrt(std::max<FCL_REAL>(radsqr - dz * dz, 0)); |
|
360 |
✓✓ | 3130279 |
if (x < minx) minx = x; |
361 |
✓✓ | 18695109 |
} else if (P[i][0] > maxx) { |
362 |
3097454 |
dz = P[i][2] - cz; |
|
363 |
3097454 |
x = P[i][0] - std::sqrt(std::max<FCL_REAL>(radsqr - dz * dz, 0)); |
|
364 |
✓✓ | 3097454 |
if (x > maxx) maxx = x; |
365 |
} |
||
366 |
} |
||
367 |
|||
368 |
// compute an initial norm of rectangle along y direction |
||
369 |
|||
370 |
// find miny and maxy as starting points |
||
371 |
|||
372 |
1335766 |
minindex = maxindex = 0; |
|
373 |
1335766 |
mintmp = maxtmp = P[0][1]; |
|
374 |
✓✓ | 21825388 |
for (unsigned int i = 1; i < size_P; ++i) { |
375 |
20489622 |
FCL_REAL y_value = P[i][1]; |
|
376 |
✓✓ | 20489622 |
if (y_value < mintmp) { |
377 |
2366846 |
minindex = i; |
|
378 |
2366846 |
mintmp = y_value; |
|
379 |
✓✓ | 18122776 |
} else if (y_value > maxtmp) { |
380 |
1711318 |
maxindex = i; |
|
381 |
1711318 |
maxtmp = y_value; |
|
382 |
} |
||
383 |
} |
||
384 |
|||
385 |
FCL_REAL y; |
||
386 |
1335766 |
dz = P[minindex][2] - cz; |
|
387 |
1335766 |
miny = P[minindex][1] + std::sqrt(std::max<FCL_REAL>(radsqr - dz * dz, 0)); |
|
388 |
1335766 |
dz = P[maxindex][2] - cz; |
|
389 |
1335766 |
maxy = P[maxindex][1] - std::sqrt(std::max<FCL_REAL>(radsqr - dz * dz, 0)); |
|
390 |
|||
391 |
// grow miny/maxy |
||
392 |
|||
393 |
✓✓ | 23161154 |
for (unsigned int i = 0; i < size_P; ++i) { |
394 |
✓✓ | 21825388 |
if (P[i][1] < miny) { |
395 |
3037848 |
dz = P[i][2] - cz; |
|
396 |
3037848 |
y = P[i][1] + std::sqrt(std::max<FCL_REAL>(radsqr - dz * dz, 0)); |
|
397 |
✓✓ | 3037848 |
if (y < miny) miny = y; |
398 |
✓✓ | 18787540 |
} else if (P[i][1] > maxy) { |
399 |
3018530 |
dz = P[i][2] - cz; |
|
400 |
3018530 |
y = P[i][1] - std::sqrt(std::max<FCL_REAL>(radsqr - dz * dz, 0)); |
|
401 |
✓✓ | 3018530 |
if (y > maxy) maxy = y; |
402 |
} |
||
403 |
} |
||
404 |
|||
405 |
// corners may have some points which are not covered - grow lengths if |
||
406 |
// necessary quite conservative (can be improved) |
||
407 |
FCL_REAL dx, dy, u, t; |
||
408 |
1335766 |
FCL_REAL a = std::sqrt((FCL_REAL)0.5); |
|
409 |
✓✓ | 23161154 |
for (unsigned int i = 0; i < size_P; ++i) { |
410 |
✓✓ | 21825388 |
if (P[i][0] > maxx) { |
411 |
✓✓ | 2810376 |
if (P[i][1] > maxy) { |
412 |
451253 |
dx = P[i][0] - maxx; |
|
413 |
451253 |
dy = P[i][1] - maxy; |
|
414 |
451253 |
u = dx * a + dy * a; |
|
415 |
451253 |
t = (a * u - dx) * (a * u - dx) + (a * u - dy) * (a * u - dy) + |
|
416 |
451253 |
(cz - P[i][2]) * (cz - P[i][2]); |
|
417 |
451253 |
u = u - std::sqrt(std::max<FCL_REAL>(radsqr - t, 0)); |
|
418 |
✓✓ | 451253 |
if (u > 0) { |
419 |
94203 |
maxx += u * a; |
|
420 |
94203 |
maxy += u * a; |
|
421 |
} |
||
422 |
✓✓ | 2359123 |
} else if (P[i][1] < miny) { |
423 |
453390 |
dx = P[i][0] - maxx; |
|
424 |
453390 |
dy = P[i][1] - miny; |
|
425 |
453390 |
u = dx * a - dy * a; |
|
426 |
453390 |
t = (a * u - dx) * (a * u - dx) + (-a * u - dy) * (-a * u - dy) + |
|
427 |
453390 |
(cz - P[i][2]) * (cz - P[i][2]); |
|
428 |
453390 |
u = u - std::sqrt(std::max<FCL_REAL>(radsqr - t, 0)); |
|
429 |
✓✓ | 453390 |
if (u > 0) { |
430 |
99637 |
maxx += u * a; |
|
431 |
99637 |
miny -= u * a; |
|
432 |
} |
||
433 |
} |
||
434 |
✓✓ | 19015012 |
} else if (P[i][0] < minx) { |
435 |
✓✓ | 2814641 |
if (P[i][1] > maxy) { |
436 |
434752 |
dx = P[i][0] - minx; |
|
437 |
434752 |
dy = P[i][1] - maxy; |
|
438 |
434752 |
u = dy * a - dx * a; |
|
439 |
434752 |
t = (-a * u - dx) * (-a * u - dx) + (a * u - dy) * (a * u - dy) + |
|
440 |
434752 |
(cz - P[i][2]) * (cz - P[i][2]); |
|
441 |
434752 |
u = u - std::sqrt(std::max<FCL_REAL>(radsqr - t, 0)); |
|
442 |
✓✓ | 434752 |
if (u > 0) { |
443 |
90920 |
minx -= u * a; |
|
444 |
90920 |
maxy += u * a; |
|
445 |
} |
||
446 |
✓✓ | 2379889 |
} else if (P[i][1] < miny) { |
447 |
439485 |
dx = P[i][0] - minx; |
|
448 |
439485 |
dy = P[i][1] - miny; |
|
449 |
439485 |
u = -dx * a - dy * a; |
|
450 |
439485 |
t = (-a * u - dx) * (-a * u - dx) + (-a * u - dy) * (-a * u - dy) + |
|
451 |
439485 |
(cz - P[i][2]) * (cz - P[i][2]); |
|
452 |
439485 |
u = u - std::sqrt(std::max<FCL_REAL>(radsqr - t, 0)); |
|
453 |
✓✓ | 439485 |
if (u > 0) { |
454 |
109445 |
minx -= u * a; |
|
455 |
109445 |
miny -= u * a; |
|
456 |
} |
||
457 |
} |
||
458 |
} |
||
459 |
} |
||
460 |
|||
461 |
✓✗✓✗ ✓✗✓✗ |
1335766 |
origin.noalias() = axes * Vec3f(minx, miny, cz); |
462 |
|||
463 |
1335766 |
l[0] = std::max<FCL_REAL>(maxx - minx, 0); |
|
464 |
1335766 |
l[1] = std::max<FCL_REAL>(maxy - miny, 0); |
|
465 |
|||
466 |
✓✗ | 1335766 |
delete[] P; |
467 |
1335766 |
} |
|
468 |
|||
469 |
/** @brief Compute the bounding volume extent and center for a set or subset of |
||
470 |
* points. The bounding volume axes are known. |
||
471 |
*/ |
||
472 |
19868 |
static inline void getExtentAndCenter_pointcloud(Vec3f* ps, Vec3f* ps2, |
|
473 |
unsigned int* indices, |
||
474 |
unsigned int n, Matrix3f& axes, |
||
475 |
Vec3f& center, Vec3f& extent) { |
||
476 |
19868 |
bool indirect_index = true; |
|
477 |
✓✗ | 19868 |
if (!indices) indirect_index = false; |
478 |
|||
479 |
19868 |
FCL_REAL real_max = (std::numeric_limits<FCL_REAL>::max)(); |
|
480 |
|||
481 |
✓✗ | 19868 |
Vec3f min_coord(real_max, real_max, real_max); |
482 |
✓✗ | 19868 |
Vec3f max_coord(-real_max, -real_max, -real_max); |
483 |
|||
484 |
✓✓ | 81018 |
for (unsigned int i = 0; i < n; ++i) { |
485 |
✗✓ | 61150 |
unsigned int index = indirect_index ? indices[i] : i; |
486 |
|||
487 |
61150 |
const Vec3f& p = ps[index]; |
|
488 |
✓✗✓✗ ✓✗ |
61150 |
Vec3f proj(axes.transpose() * p); |
489 |
|||
490 |
✓✓ | 244600 |
for (int j = 0; j < 3; ++j) { |
491 |
✓✗✓✗ ✓✓✓✗ ✓✗ |
183450 |
if (proj[j] > max_coord[j]) max_coord[j] = proj[j]; |
492 |
✓✗✓✗ ✓✓✓✗ ✓✗ |
183450 |
if (proj[j] < min_coord[j]) min_coord[j] = proj[j]; |
493 |
} |
||
494 |
|||
495 |
✗✓ | 61150 |
if (ps2) { |
496 |
const Vec3f& v = ps2[index]; |
||
497 |
proj.noalias() = axes.transpose() * v; |
||
498 |
|||
499 |
for (int j = 0; j < 3; ++j) { |
||
500 |
if (proj[j] > max_coord[j]) max_coord[j] = proj[j]; |
||
501 |
if (proj[j] < min_coord[j]) min_coord[j] = proj[j]; |
||
502 |
} |
||
503 |
} |
||
504 |
} |
||
505 |
|||
506 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
19868 |
center.noalias() = axes * (max_coord + min_coord) / 2; |
507 |
|||
508 |
✓✗✓✗ ✓✗✓✗ |
19868 |
extent.noalias() = (max_coord - min_coord) / 2; |
509 |
19868 |
} |
|
510 |
|||
511 |
/** @brief Compute the bounding volume extent and center for a set or subset of |
||
512 |
* points. The bounding volume axes are known. |
||
513 |
*/ |
||
514 |
1467062 |
static inline void getExtentAndCenter_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts, |
|
515 |
unsigned int* indices, |
||
516 |
unsigned int n, Matrix3f& axes, |
||
517 |
Vec3f& center, Vec3f& extent) { |
||
518 |
1467062 |
bool indirect_index = true; |
|
519 |
✗✓ | 1467062 |
if (!indices) indirect_index = false; |
520 |
|||
521 |
1467062 |
FCL_REAL real_max = (std::numeric_limits<FCL_REAL>::max)(); |
|
522 |
|||
523 |
✓✗ | 1467062 |
Vec3f min_coord(real_max, real_max, real_max); |
524 |
✓✗ | 1467062 |
Vec3f max_coord(-real_max, -real_max, -real_max); |
525 |
|||
526 |
✓✓ | 9665898 |
for (unsigned int i = 0; i < n; ++i) { |
527 |
✓✗ | 8198836 |
unsigned int index = indirect_index ? indices[i] : i; |
528 |
8198836 |
const Triangle& t = ts[index]; |
|
529 |
|||
530 |
✓✓ | 32795344 |
for (Triangle::index_type j = 0; j < 3; ++j) { |
531 |
24596508 |
Triangle::index_type point_id = t[j]; |
|
532 |
24596508 |
const Vec3f& p = ps[point_id]; |
|
533 |
✓✗✓✗ ✓✗ |
24596508 |
Vec3f proj(axes.transpose() * p); |
534 |
|||
535 |
✓✓ | 98386032 |
for (int k = 0; k < 3; ++k) { |
536 |
✓✗✓✗ ✓✓✓✗ ✓✗ |
73789524 |
if (proj[k] > max_coord[k]) max_coord[k] = proj[k]; |
537 |
✓✗✓✗ ✓✓✓✗ ✓✗ |
73789524 |
if (proj[k] < min_coord[k]) min_coord[k] = proj[k]; |
538 |
} |
||
539 |
} |
||
540 |
|||
541 |
✗✓ | 8198836 |
if (ps2) { |
542 |
for (Triangle::index_type j = 0; j < 3; ++j) { |
||
543 |
Triangle::index_type point_id = t[j]; |
||
544 |
const Vec3f& p = ps2[point_id]; |
||
545 |
Vec3f proj(axes.transpose() * p); |
||
546 |
|||
547 |
for (int k = 0; k < 3; ++k) { |
||
548 |
if (proj[k] > max_coord[k]) max_coord[k] = proj[k]; |
||
549 |
if (proj[k] < min_coord[k]) min_coord[k] = proj[k]; |
||
550 |
} |
||
551 |
} |
||
552 |
} |
||
553 |
} |
||
554 |
|||
555 |
✓✗✓✗ ✓✗ |
1467062 |
Vec3f o((max_coord + min_coord) / 2); |
556 |
|||
557 |
✓✗✓✗ ✓✗ |
1467062 |
center.noalias() = axes * o; |
558 |
|||
559 |
✓✗✓✗ ✓✗✓✗ |
1467062 |
extent.noalias() = (max_coord - min_coord) / 2; |
560 |
1467062 |
} |
|
561 |
|||
562 |
1486930 |
void getExtentAndCenter(Vec3f* ps, Vec3f* ps2, Triangle* ts, |
|
563 |
unsigned int* indices, unsigned int n, Matrix3f& axes, |
||
564 |
Vec3f& center, Vec3f& extent) { |
||
565 |
✓✓ | 1486930 |
if (ts) |
566 |
1467062 |
getExtentAndCenter_mesh(ps, ps2, ts, indices, n, axes, center, extent); |
|
567 |
else |
||
568 |
19868 |
getExtentAndCenter_pointcloud(ps, ps2, indices, n, axes, center, extent); |
|
569 |
1486930 |
} |
|
570 |
|||
571 |
6540 |
void circumCircleComputation(const Vec3f& a, const Vec3f& b, const Vec3f& c, |
|
572 |
Vec3f& center, FCL_REAL& radius) { |
||
573 |
✓✗✓✗ |
6540 |
Vec3f e1 = a - c; |
574 |
✓✗✓✗ |
6540 |
Vec3f e2 = b - c; |
575 |
✓✗ | 6540 |
FCL_REAL e1_len2 = e1.squaredNorm(); |
576 |
✓✗ | 6540 |
FCL_REAL e2_len2 = e2.squaredNorm(); |
577 |
✓✗ | 6540 |
Vec3f e3 = e1.cross(e2); |
578 |
✓✗ | 6540 |
FCL_REAL e3_len2 = e3.squaredNorm(); |
579 |
✓✗✓✗ |
6540 |
radius = e1_len2 * e2_len2 * (e1 - e2).squaredNorm() / e3_len2; |
580 |
6540 |
radius = std::sqrt(radius) * 0.5; |
|
581 |
|||
582 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
6540 |
center = (e2 * e1_len2 - e1 * e2_len2).cross(e3) * (0.5 * 1 / e3_len2) + c; |
583 |
6540 |
} |
|
584 |
|||
585 |
667935 |
static inline FCL_REAL maximumDistance_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts, |
|
586 |
unsigned int* indices, |
||
587 |
unsigned int n, |
||
588 |
const Vec3f& query) { |
||
589 |
667935 |
bool indirect_index = true; |
|
590 |
✗✓ | 667935 |
if (!indices) indirect_index = false; |
591 |
|||
592 |
667935 |
FCL_REAL maxD = 0; |
|
593 |
✓✓ | 4008555 |
for (unsigned int i = 0; i < n; ++i) { |
594 |
✓✗ | 3340620 |
unsigned int index = indirect_index ? indices[i] : i; |
595 |
3340620 |
const Triangle& t = ts[index]; |
|
596 |
|||
597 |
✓✓ | 13362480 |
for (Triangle::index_type j = 0; j < 3; ++j) { |
598 |
10021860 |
Triangle::index_type point_id = t[j]; |
|
599 |
10021860 |
const Vec3f& p = ps[point_id]; |
|
600 |
|||
601 |
✓✗ | 10021860 |
FCL_REAL d = (p - query).squaredNorm(); |
602 |
✓✓ | 10021860 |
if (d > maxD) maxD = d; |
603 |
} |
||
604 |
|||
605 |
✗✓ | 3340620 |
if (ps2) { |
606 |
for (Triangle::index_type j = 0; j < 3; ++j) { |
||
607 |
Triangle::index_type point_id = t[j]; |
||
608 |
const Vec3f& p = ps2[point_id]; |
||
609 |
|||
610 |
FCL_REAL d = (p - query).squaredNorm(); |
||
611 |
if (d > maxD) maxD = d; |
||
612 |
} |
||
613 |
} |
||
614 |
} |
||
615 |
|||
616 |
667935 |
return std::sqrt(maxD); |
|
617 |
} |
||
618 |
|||
619 |
3 |
static inline FCL_REAL maximumDistance_pointcloud(Vec3f* ps, Vec3f* ps2, |
|
620 |
unsigned int* indices, |
||
621 |
unsigned int n, |
||
622 |
const Vec3f& query) { |
||
623 |
3 |
bool indirect_index = true; |
|
624 |
✓✗ | 3 |
if (!indices) indirect_index = false; |
625 |
|||
626 |
3 |
FCL_REAL maxD = 0; |
|
627 |
✓✓ | 39 |
for (unsigned int i = 0; i < n; ++i) { |
628 |
✗✓ | 36 |
unsigned int index = indirect_index ? indices[i] : i; |
629 |
|||
630 |
36 |
const Vec3f& p = ps[index]; |
|
631 |
✓✗ | 36 |
FCL_REAL d = (p - query).squaredNorm(); |
632 |
✓✓ | 36 |
if (d > maxD) maxD = d; |
633 |
|||
634 |
✗✓ | 36 |
if (ps2) { |
635 |
const Vec3f& v = ps2[index]; |
||
636 |
FCL_REAL d = (v - query).squaredNorm(); |
||
637 |
if (d > maxD) maxD = d; |
||
638 |
} |
||
639 |
} |
||
640 |
|||
641 |
3 |
return std::sqrt(maxD); |
|
642 |
} |
||
643 |
|||
644 |
667938 |
FCL_REAL maximumDistance(Vec3f* ps, Vec3f* ps2, Triangle* ts, |
|
645 |
unsigned int* indices, unsigned int n, |
||
646 |
const Vec3f& query) { |
||
647 |
✓✓ | 667938 |
if (ts) |
648 |
667935 |
return maximumDistance_mesh(ps, ps2, ts, indices, n, query); |
|
649 |
else |
||
650 |
3 |
return maximumDistance_pointcloud(ps, ps2, indices, n, query); |
|
651 |
} |
||
652 |
|||
653 |
} // namespace fcl |
||
654 |
|||
655 |
} // namespace hpp |
| Generated by: GCOVR (Version 4.2) |