GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
Line | Branch | Exec | Source |
1 |
/* |
||
2 |
* Software License Agreement (BSD License) |
||
3 |
* |
||
4 |
* Copyright (c) 2021, INRIA. |
||
5 |
* All rights reserved. |
||
6 |
* |
||
7 |
* Redistribution and use in source and binary forms, with or without |
||
8 |
* modification, are permitted provided that the following conditions |
||
9 |
* are met: |
||
10 |
* |
||
11 |
* * Redistributions of source code must retain the above copyright |
||
12 |
* notice, this list of conditions and the following disclaimer. |
||
13 |
* * Redistributions in binary form must reproduce the above |
||
14 |
* copyright notice, this list of conditions and the following |
||
15 |
* disclaimer in the documentation and/or other materials provided |
||
16 |
* with the distribution. |
||
17 |
* * Neither the name of Open Source Robotics Foundation nor the names of its |
||
18 |
* contributors may be used to endorse or promote products derived |
||
19 |
* from this software without specific prior written permission. |
||
20 |
* |
||
21 |
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||
22 |
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||
23 |
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||
24 |
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||
25 |
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||
26 |
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||
27 |
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; |
||
28 |
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER |
||
29 |
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||
30 |
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||
31 |
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||
32 |
* POSSIBILITY OF SUCH DAMAGE. |
||
33 |
*/ |
||
34 |
|||
35 |
/** \author Jia Pan */ |
||
36 |
|||
37 |
#ifndef HPP_FCL_TRAVERSAL_NODE_HFIELD_SHAPE_H |
||
38 |
#define HPP_FCL_TRAVERSAL_NODE_HFIELD_SHAPE_H |
||
39 |
|||
40 |
/// @cond INTERNAL |
||
41 |
|||
42 |
#include <hpp/fcl/collision_data.h> |
||
43 |
#include <hpp/fcl/shape/geometric_shapes.h> |
||
44 |
#include <hpp/fcl/narrowphase/narrowphase.h> |
||
45 |
#include <hpp/fcl/shape/geometric_shapes_utility.h> |
||
46 |
#include <hpp/fcl/internal/traversal_node_base.h> |
||
47 |
#include <hpp/fcl/internal/traversal.h> |
||
48 |
#include <hpp/fcl/hfield.h> |
||
49 |
#include <hpp/fcl/shape/convex.h> |
||
50 |
|||
51 |
namespace hpp { |
||
52 |
namespace fcl { |
||
53 |
|||
54 |
/// @addtogroup Traversal_For_Collision |
||
55 |
/// @{ |
||
56 |
|||
57 |
namespace details { |
||
58 |
template <typename BV> |
||
59 |
Convex<Quadrilateral> buildConvexQuadrilateral(const HFNode<BV>& node, |
||
60 |
const HeightField<BV>& model) { |
||
61 |
const MatrixXf& heights = model.getHeights(); |
||
62 |
const VecXf& x_grid = model.getXGrid(); |
||
63 |
const VecXf& y_grid = model.getYGrid(); |
||
64 |
|||
65 |
const FCL_REAL min_height = model.getMinHeight(); |
||
66 |
|||
67 |
const FCL_REAL x0 = x_grid[node.x_id], x1 = x_grid[node.x_id + 1], |
||
68 |
y0 = y_grid[node.y_id], y1 = y_grid[node.y_id + 1]; |
||
69 |
const Eigen::Block<const MatrixXf, 2, 2> cell = |
||
70 |
heights.block<2, 2>(node.y_id, node.x_id); |
||
71 |
|||
72 |
assert(cell.maxCoeff() > min_height && |
||
73 |
"max_height is lower than min_height"); // Check whether the geometry |
||
74 |
// is degenerated |
||
75 |
|||
76 |
Vec3f* pts = new Vec3f[8]; |
||
77 |
pts[0] = Vec3f(x0, y0, min_height); |
||
78 |
pts[1] = Vec3f(x0, y1, min_height); |
||
79 |
pts[2] = Vec3f(x1, y1, min_height); |
||
80 |
pts[3] = Vec3f(x1, y0, min_height); |
||
81 |
pts[4] = Vec3f(x0, y0, cell(0, 0)); |
||
82 |
pts[5] = Vec3f(x0, y1, cell(1, 0)); |
||
83 |
pts[6] = Vec3f(x1, y1, cell(1, 1)); |
||
84 |
pts[7] = Vec3f(x1, y0, cell(0, 1)); |
||
85 |
|||
86 |
Quadrilateral* polygons = new Quadrilateral[6]; |
||
87 |
polygons[0].set(0, 3, 2, 1); // x+ side |
||
88 |
polygons[1].set(0, 1, 5, 4); // y- side |
||
89 |
polygons[2].set(1, 2, 6, 5); // x- side |
||
90 |
polygons[3].set(2, 3, 7, 6); // y+ side |
||
91 |
polygons[4].set(3, 0, 4, 7); // z- side |
||
92 |
polygons[5].set(4, 5, 6, 7); // z+ side |
||
93 |
|||
94 |
return Convex<Quadrilateral>(true, |
||
95 |
pts, // points |
||
96 |
8, // num points |
||
97 |
polygons, |
||
98 |
6 // number of polygons |
||
99 |
); |
||
100 |
} |
||
101 |
|||
102 |
template <typename BV> |
||
103 |
55656 |
void buildConvexTriangles(const HFNode<BV>& node, const HeightField<BV>& model, |
|
104 |
Convex<Triangle>& convex1, |
||
105 |
Convex<Triangle>& convex2) { |
||
106 |
55656 |
const MatrixXf& heights = model.getHeights(); |
|
107 |
55656 |
const VecXf& x_grid = model.getXGrid(); |
|
108 |
55656 |
const VecXf& y_grid = model.getYGrid(); |
|
109 |
|||
110 |
55656 |
const FCL_REAL min_height = model.getMinHeight(); |
|
111 |
|||
112 |
✓✗✓✗ |
55656 |
const FCL_REAL x0 = x_grid[node.x_id], x1 = x_grid[node.x_id + 1], |
113 |
✓✗✓✗ |
55656 |
y0 = y_grid[node.y_id], y1 = y_grid[node.y_id + 1]; |
114 |
55656 |
const FCL_REAL max_height = node.max_height; |
|
115 |
55656 |
const Eigen::Block<const MatrixXf, 2, 2> cell = |
|
116 |
✓✗ | 55656 |
heights.block<2, 2>(node.y_id, node.x_id); |
117 |
|||
118 |
✗✓ | 55656 |
assert(max_height > min_height && |
119 |
"max_height is lower than min_height"); // Check whether the geometry |
||
120 |
// is degenerated |
||
121 |
HPP_FCL_UNUSED_VARIABLE(max_height); |
||
122 |
|||
123 |
{ |
||
124 |
✓✗✓✓ ✓✗ |
500904 |
Vec3f* pts = new Vec3f[8]; |
125 |
✓✗ | 55656 |
pts[0] = Vec3f(x0, y0, min_height); |
126 |
✓✗ | 55656 |
pts[1] = Vec3f(x0, y1, min_height); |
127 |
✓✗ | 55656 |
pts[2] = Vec3f(x1, y1, min_height); |
128 |
✓✗ | 55656 |
pts[3] = Vec3f(x1, y0, min_height); |
129 |
✓✗✓✗ |
55656 |
pts[4] = Vec3f(x0, y0, cell(0, 0)); |
130 |
✓✗✓✗ |
55656 |
pts[5] = Vec3f(x0, y1, cell(1, 0)); |
131 |
✓✗✓✗ |
55656 |
pts[6] = Vec3f(x1, y1, cell(1, 1)); |
132 |
✓✗✓✗ |
55656 |
pts[7] = Vec3f(x1, y0, cell(0, 1)); |
133 |
|||
134 |
✓✗✓✓ |
500904 |
Triangle* triangles = new Triangle[8]; |
135 |
55656 |
triangles[0].set(0, 1, 3); // bottom |
|
136 |
55656 |
triangles[1].set(4, 5, 7); // top |
|
137 |
55656 |
triangles[2].set(0, 1, 4); |
|
138 |
55656 |
triangles[3].set(4, 1, 5); |
|
139 |
55656 |
triangles[4].set(1, 7, 3); |
|
140 |
55656 |
triangles[5].set(1, 5, 7); |
|
141 |
55656 |
triangles[6].set(0, 3, 7); |
|
142 |
55656 |
triangles[7].set(7, 4, 0); |
|
143 |
|||
144 |
✓✗ | 55656 |
convex1.set(true, |
145 |
pts, // points |
||
146 |
8, // num points |
||
147 |
triangles, |
||
148 |
8 // number of polygons |
||
149 |
); |
||
150 |
} |
||
151 |
|||
152 |
{ |
||
153 |
✓✗✓✓ ✓✗ |
500904 |
Vec3f* pts = new Vec3f[8]; |
154 |
55656 |
memcpy(pts, convex1.points, 8 * sizeof(Vec3f)); |
|
155 |
|||
156 |
✓✗✓✓ |
500904 |
Triangle* triangles = new Triangle[8]; |
157 |
55656 |
triangles[0].set(3, 2, 1); // top |
|
158 |
55656 |
triangles[1].set(5, 6, 7); // bottom |
|
159 |
55656 |
triangles[2].set(1, 2, 5); |
|
160 |
55656 |
triangles[3].set(5, 2, 6); |
|
161 |
55656 |
triangles[4].set(1, 3, 7); |
|
162 |
55656 |
triangles[5].set(1, 7, 5); |
|
163 |
55656 |
triangles[6].set(2, 3, 7); |
|
164 |
55656 |
triangles[7].set(6, 2, 3); |
|
165 |
|||
166 |
✓✗ | 55656 |
convex2.set(true, |
167 |
pts, // points |
||
168 |
8, // num points |
||
169 |
triangles, |
||
170 |
8 // number of polygons |
||
171 |
); |
||
172 |
} |
||
173 |
55656 |
} |
|
174 |
} // namespace details |
||
175 |
|||
176 |
/// @brief Traversal node for collision between height field and shape |
||
177 |
template <typename BV, typename S, |
||
178 |
int _Options = RelativeTransformationIsIdentity> |
||
179 |
class HeightFieldShapeCollisionTraversalNode |
||
180 |
: public CollisionTraversalNodeBase { |
||
181 |
public: |
||
182 |
typedef CollisionTraversalNodeBase Base; |
||
183 |
typedef Eigen::Array<FCL_REAL, 1, 2> Array2d; |
||
184 |
|||
185 |
enum { |
||
186 |
Options = _Options, |
||
187 |
RTIsIdentity = _Options & RelativeTransformationIsIdentity |
||
188 |
}; |
||
189 |
|||
190 |
80 |
HeightFieldShapeCollisionTraversalNode(const CollisionRequest& request) |
|
191 |
✓✗✓✗ |
80 |
: CollisionTraversalNodeBase(request) { |
192 |
80 |
model1 = NULL; |
|
193 |
80 |
model2 = NULL; |
|
194 |
|||
195 |
80 |
num_bv_tests = 0; |
|
196 |
80 |
num_leaf_tests = 0; |
|
197 |
80 |
query_time_seconds = 0.0; |
|
198 |
|||
199 |
80 |
nsolver = NULL; |
|
200 |
✓✗ | 80 |
shape_inflation.setZero(); |
201 |
} |
||
202 |
|||
203 |
/// @brief Whether the BV node in the first BVH tree is leaf |
||
204 |
115796 |
bool isFirstNodeLeaf(unsigned int b) const { |
|
205 |
115796 |
return model1->getBV(b).isLeaf(); |
|
206 |
} |
||
207 |
|||
208 |
/// @brief Obtain the left child of BV node in the first BVH |
||
209 |
57926 |
int getFirstLeftChild(unsigned int b) const { |
|
210 |
57926 |
return static_cast<int>(model1->getBV(b).leftChild()); |
|
211 |
} |
||
212 |
|||
213 |
/// @brief Obtain the right child of BV node in the first BVH |
||
214 |
57926 |
int getFirstRightChild(unsigned int b) const { |
|
215 |
57926 |
return static_cast<int>(model1->getBV(b).rightChild()); |
|
216 |
} |
||
217 |
|||
218 |
/// test between BV b1 and shape |
||
219 |
/// @param b1 BV to test, |
||
220 |
/// @retval sqrDistLowerBound square of a lower bound of the minimal |
||
221 |
/// distance between bounding volumes. |
||
222 |
/// @brief BV culling test in one BVTT node |
||
223 |
60140 |
bool BVDisjoints(unsigned int b1, unsigned int /*b2*/, |
|
224 |
FCL_REAL& sqrDistLowerBound) const { |
||
225 |
✗✓ | 60140 |
if (this->enable_statistics) this->num_bv_tests++; |
226 |
|||
227 |
bool disjoint; |
||
228 |
if (RTIsIdentity) { |
||
229 |
assert(false && "must never happened"); |
||
230 |
disjoint = !this->model1->getBV(b1).bv.overlap( |
||
231 |
this->model2_bv, this->request, sqrDistLowerBound); |
||
232 |
} else { |
||
233 |
60140 |
disjoint = !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), |
|
234 |
60140 |
this->model1->getBV(b1).bv, this->model2_bv, |
|
235 |
this->request, sqrDistLowerBound); |
||
236 |
} |
||
237 |
|||
238 |
✓✓ | 60140 |
if (disjoint) |
239 |
2214 |
internal::updateDistanceLowerBoundFromBV(this->request, *this->result, |
|
240 |
sqrDistLowerBound); |
||
241 |
|||
242 |
✓✓✗✓ |
60140 |
assert(!disjoint || sqrDistLowerBound > 0); |
243 |
60140 |
return disjoint; |
|
244 |
} |
||
245 |
|||
246 |
template <typename Polygone> |
||
247 |
55656 |
bool shapeDistance(const Convex<Polygone>& convex1, |
|
248 |
const Convex<Polygone>& convex2, const Transform3f& tf1, |
||
249 |
const S& shape, const Transform3f& tf2, FCL_REAL& distance, |
||
250 |
Vec3f& c1, Vec3f& c2, Vec3f& normal) const { |
||
251 |
✓✗ | 55656 |
const Transform3f Id; |
252 |
✓✗✓✗ ✓✗ |
55656 |
Vec3f contact2_1, contact2_2, normal2; |
253 |
FCL_REAL distance2; |
||
254 |
bool collision1, collision2; |
||
255 |
if (RTIsIdentity) |
||
256 |
collision1 = !nsolver->shapeDistance(convex1, Id, shape, tf2, distance, |
||
257 |
c1, c2, normal); |
||
258 |
else |
||
259 |
✓✗ | 55656 |
collision1 = !nsolver->shapeDistance(convex1, tf1, shape, tf2, distance, |
260 |
c1, c2, normal); |
||
261 |
|||
262 |
if (RTIsIdentity) |
||
263 |
collision2 = !nsolver->shapeDistance(convex2, Id, shape, tf2, distance2, |
||
264 |
c1, c2, normal); |
||
265 |
else |
||
266 |
✓✗ | 55656 |
collision2 = !nsolver->shapeDistance(convex2, tf1, shape, tf2, distance2, |
267 |
contact2_1, contact2_2, normal2); |
||
268 |
|||
269 |
✓✓✓✗ |
55656 |
if (collision1 && collision2) { |
270 |
✗✓ | 6146 |
if (distance > distance2) // switch values |
271 |
{ |
||
272 |
distance = distance2; |
||
273 |
c1 = contact2_1; |
||
274 |
c2 = contact2_2; |
||
275 |
normal = normal2; |
||
276 |
} |
||
277 |
6146 |
return true; |
|
278 |
✗✓ | 49510 |
} else if (collision1) { |
279 |
return true; |
||
280 |
✗✓ | 49510 |
} else if (collision2) { |
281 |
distance = distance2; |
||
282 |
c1 = contact2_1; |
||
283 |
c2 = contact2_2; |
||
284 |
normal = normal2; |
||
285 |
return true; |
||
286 |
} |
||
287 |
|||
288 |
49510 |
return false; |
|
289 |
} |
||
290 |
|||
291 |
template <typename Polygone> |
||
292 |
bool shapeCollision(const Convex<Polygone>& convex1, |
||
293 |
const Convex<Polygone>& convex2, const Transform3f& tf1, |
||
294 |
const S& shape, const Transform3f& tf2, |
||
295 |
FCL_REAL& distance_lower_bound, Vec3f& contact_point, |
||
296 |
Vec3f& normal) const { |
||
297 |
const Transform3f Id; |
||
298 |
Vec3f contact_point2, normal2; |
||
299 |
FCL_REAL distance_lower_bound2; |
||
300 |
bool collision1, collision2; |
||
301 |
if (RTIsIdentity) |
||
302 |
collision1 = |
||
303 |
nsolver->shapeIntersect(convex1, Id, shape, tf2, distance_lower_bound, |
||
304 |
true, &contact_point, &normal); |
||
305 |
else |
||
306 |
collision1 = nsolver->shapeIntersect(convex1, tf1, shape, tf2, |
||
307 |
distance_lower_bound, true, |
||
308 |
&contact_point, &normal); |
||
309 |
|||
310 |
if (RTIsIdentity) |
||
311 |
collision2 = nsolver->shapeIntersect(convex2, Id, shape, tf2, |
||
312 |
distance_lower_bound2, true, |
||
313 |
&contact_point2, &normal2); |
||
314 |
else |
||
315 |
collision2 = nsolver->shapeIntersect(convex2, tf1, shape, tf2, |
||
316 |
distance_lower_bound2, true, |
||
317 |
&contact_point2, &normal2); |
||
318 |
|||
319 |
if (collision1 && collision2) { |
||
320 |
// In some case, EPA might returns something like |
||
321 |
// -(std::numeric_limits<FCL_REAL>::max)(). |
||
322 |
if (distance_lower_bound != -(std::numeric_limits<FCL_REAL>::max)() && |
||
323 |
distance_lower_bound2 != -(std::numeric_limits<FCL_REAL>::max)()) { |
||
324 |
if (distance_lower_bound > distance_lower_bound2) // switch values |
||
325 |
{ |
||
326 |
distance_lower_bound = distance_lower_bound2; |
||
327 |
contact_point = contact_point2; |
||
328 |
normal = normal2; |
||
329 |
} |
||
330 |
} else if (distance_lower_bound2 != |
||
331 |
-(std::numeric_limits<FCL_REAL>::max)()) { |
||
332 |
distance_lower_bound = distance_lower_bound2; |
||
333 |
contact_point = contact_point2; |
||
334 |
normal = normal2; |
||
335 |
} |
||
336 |
return true; |
||
337 |
} else if (collision1) { |
||
338 |
return true; |
||
339 |
} else if (collision2) { |
||
340 |
distance_lower_bound = distance_lower_bound2; |
||
341 |
contact_point = contact_point2; |
||
342 |
normal = normal2; |
||
343 |
return true; |
||
344 |
} |
||
345 |
|||
346 |
return false; |
||
347 |
} |
||
348 |
|||
349 |
/// @brief Intersection testing between leaves (one Convex and one shape) |
||
350 |
55656 |
void leafCollides(unsigned int b1, unsigned int /*b2*/, |
|
351 |
FCL_REAL& sqrDistLowerBound) const { |
||
352 |
✗✓ | 55656 |
if (this->enable_statistics) this->num_leaf_tests++; |
353 |
✓✗ | 55656 |
const HFNode<BV>& node = this->model1->getBV(b1); |
354 |
|||
355 |
// Split quadrilateral primitives into two convex shapes corresponding to |
||
356 |
// polyhedron with triangular bases. This is essential to keep the convexity |
||
357 |
|||
358 |
// typedef Convex<Quadrilateral> ConvexQuadrilateral; |
||
359 |
// const ConvexQuadrilateral convex = |
||
360 |
// details::buildConvexQuadrilateral(node,*this->model1); |
||
361 |
|||
362 |
typedef Convex<Triangle> ConvexTriangle; |
||
363 |
✓✗✓✗ |
111312 |
ConvexTriangle convex1, convex2; |
364 |
✓✗ | 55656 |
details::buildConvexTriangles(node, *this->model1, convex1, convex2); |
365 |
|||
366 |
FCL_REAL distance; |
||
367 |
// Vec3f contact_point, normal; |
||
368 |
✓✗✓✗ ✓✗ |
55656 |
Vec3f c1, c2, normal; |
369 |
|||
370 |
111312 |
bool collision = |
|
371 |
55656 |
this->shapeDistance(convex1, convex2, this->tf1, *(this->model2), |
|
372 |
✓✗ | 55656 |
this->tf2, distance, c1, c2, normal); |
373 |
|||
374 |
// this->shapeCollision(convex1, convex2, this->tf1, *(this->model2), |
||
375 |
// this->tf2, |
||
376 |
// distance, contact_point, normal); |
||
377 |
|||
378 |
55656 |
FCL_REAL distToCollision = distance - this->request.security_margin; |
|
379 |
✓✓ | 55656 |
if (distToCollision <= this->request.collision_distance_threshold) { |
380 |
32 |
sqrDistLowerBound = 0; |
|
381 |
✓✗ | 32 |
if (this->request.num_max_contacts > this->result->numContacts()) { |
382 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
64 |
this->result->addContact(Contact(this->model1, this->model2, (int)b1, |
383 |
✓✗ | 64 |
(int)Contact::NONE, .5 * (c1 + c2), |
384 |
(c2 - c1).normalized(), -distance)); |
||
385 |
} |
||
386 |
✓✓✗✓ |
55624 |
} else if (collision && this->request.security_margin >= 0) { |
387 |
sqrDistLowerBound = 0; |
||
388 |
if (this->request.num_max_contacts > this->result->numContacts()) { |
||
389 |
this->result->addContact(Contact(this->model1, this->model2, (int)b1, |
||
390 |
(int)Contact::NONE, c1, normal, |
||
391 |
-distance)); |
||
392 |
assert(this->result->isCollision()); |
||
393 |
} |
||
394 |
} else |
||
395 |
55624 |
sqrDistLowerBound = distToCollision * distToCollision; |
|
396 |
|||
397 |
// const Vec3f c1 = contact_point - distance * 0.5 * normal; |
||
398 |
// const Vec3f c2 = contact_point + distance * 0.5 * normal; |
||
399 |
✓✗ | 55656 |
internal::updateDistanceLowerBoundFromLeaf(this->request, *this->result, |
400 |
distToCollision, c1, c2); |
||
401 |
|||
402 |
✓✓✗✓ |
55656 |
assert(this->result->isCollision() || sqrDistLowerBound > 0); |
403 |
} |
||
404 |
|||
405 |
const GJKSolver* nsolver; |
||
406 |
|||
407 |
const HeightField<BV>* model1; |
||
408 |
const S* model2; |
||
409 |
BV model2_bv; |
||
410 |
|||
411 |
Array2d shape_inflation; |
||
412 |
|||
413 |
mutable int num_bv_tests; |
||
414 |
mutable int num_leaf_tests; |
||
415 |
mutable FCL_REAL query_time_seconds; |
||
416 |
}; |
||
417 |
|||
418 |
/// @} |
||
419 |
|||
420 |
/// @addtogroup Traversal_For_Distance |
||
421 |
/// @{ |
||
422 |
|||
423 |
/// @brief Traversal node for distance between height field and shape |
||
424 |
template <typename BV, typename S, |
||
425 |
int _Options = RelativeTransformationIsIdentity> |
||
426 |
class HeightFieldShapeDistanceTraversalNode : public DistanceTraversalNodeBase { |
||
427 |
public: |
||
428 |
typedef DistanceTraversalNodeBase Base; |
||
429 |
|||
430 |
enum { |
||
431 |
Options = _Options, |
||
432 |
RTIsIdentity = _Options & RelativeTransformationIsIdentity |
||
433 |
}; |
||
434 |
|||
435 |
HeightFieldShapeDistanceTraversalNode() : DistanceTraversalNodeBase() { |
||
436 |
model1 = NULL; |
||
437 |
model2 = NULL; |
||
438 |
|||
439 |
num_leaf_tests = 0; |
||
440 |
query_time_seconds = 0.0; |
||
441 |
|||
442 |
rel_err = 0; |
||
443 |
abs_err = 0; |
||
444 |
nsolver = NULL; |
||
445 |
} |
||
446 |
|||
447 |
/// @brief Whether the BV node in the first BVH tree is leaf |
||
448 |
bool isFirstNodeLeaf(unsigned int b) const { |
||
449 |
return model1->getBV(b).isLeaf(); |
||
450 |
} |
||
451 |
|||
452 |
/// @brief Obtain the left child of BV node in the first BVH |
||
453 |
int getFirstLeftChild(unsigned int b) const { |
||
454 |
return model1->getBV(b).leftChild(); |
||
455 |
} |
||
456 |
|||
457 |
/// @brief Obtain the right child of BV node in the first BVH |
||
458 |
int getFirstRightChild(unsigned int b) const { |
||
459 |
return model1->getBV(b).rightChild(); |
||
460 |
} |
||
461 |
|||
462 |
/// @brief BV culling test in one BVTT node |
||
463 |
FCL_REAL BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const { |
||
464 |
return model1->getBV(b1).bv.distance( |
||
465 |
model2_bv); // TODO(jcarpent): tf1 is not taken into account here. |
||
466 |
} |
||
467 |
|||
468 |
/// @brief Distance testing between leaves (one triangle and one shape) |
||
469 |
void leafComputeDistance(unsigned int b1, unsigned int /*b2*/) const { |
||
470 |
if (this->enable_statistics) this->num_leaf_tests++; |
||
471 |
|||
472 |
const BVNode<BV>& node = this->model1->getBV(b1); |
||
473 |
|||
474 |
typedef Convex<Quadrilateral> ConvexQuadrilateral; |
||
475 |
const ConvexQuadrilateral convex = |
||
476 |
details::buildConvexQuadrilateral(node, *this->model1); |
||
477 |
|||
478 |
FCL_REAL d; |
||
479 |
Vec3f closest_p1, closest_p2, normal; |
||
480 |
|||
481 |
nsolver->shapeDistance(convex, this->tf1, *(this->model2), this->tf2, d, |
||
482 |
closest_p1, closest_p2, normal); |
||
483 |
|||
484 |
this->result->update(d, this->model1, this->model2, b1, |
||
485 |
DistanceResult::NONE, closest_p1, closest_p2, normal); |
||
486 |
} |
||
487 |
|||
488 |
/// @brief Whether the traversal process can stop early |
||
489 |
bool canStop(FCL_REAL c) const { |
||
490 |
if ((c >= this->result->min_distance - abs_err) && |
||
491 |
(c * (1 + rel_err) >= this->result->min_distance)) |
||
492 |
return true; |
||
493 |
return false; |
||
494 |
} |
||
495 |
|||
496 |
FCL_REAL rel_err; |
||
497 |
FCL_REAL abs_err; |
||
498 |
|||
499 |
const GJKSolver* nsolver; |
||
500 |
|||
501 |
const HeightField<BV>* model1; |
||
502 |
const S* model2; |
||
503 |
BV model2_bv; |
||
504 |
|||
505 |
mutable int num_bv_tests; |
||
506 |
mutable int num_leaf_tests; |
||
507 |
mutable FCL_REAL query_time_seconds; |
||
508 |
}; |
||
509 |
|||
510 |
/// @} |
||
511 |
|||
512 |
} // namespace fcl |
||
513 |
} // namespace hpp |
||
514 |
|||
515 |
/// @endcond |
||
516 |
|||
517 |
#endif |
Generated by: GCOVR (Version 4.2) |