GCC Code Coverage Report

Directory: ./
File: include/coal/internal/traversal_node_hfield_shape.h
Date: 2025-04-01 09:23:31
Exec Total Coverage
Lines: 238 248 96.0%
Branches: 196 396 49.5%
Line Branch Exec Source
1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2021-2024, 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 COAL_TRAVERSAL_NODE_HFIELD_SHAPE_H
38 #define COAL_TRAVERSAL_NODE_HFIELD_SHAPE_H
39
40 /// @cond INTERNAL
41
42 #include "coal/collision_data.h"
43 #include "coal/shape/geometric_shapes.h"
44 #include "coal/narrowphase/narrowphase.h"
45 #include "coal/shape/geometric_shapes_utility.h"
46 #include "coal/internal/shape_shape_func.h"
47 #include "coal/internal/traversal_node_base.h"
48 #include "coal/internal/traversal.h"
49 #include "coal/internal/intersect.h"
50 #include "coal/hfield.h"
51 #include "coal/shape/convex.h"
52
53 namespace coal {
54
55 /// @addtogroup Traversal_For_Collision
56 /// @{
57
58 namespace details {
59 template <typename BV>
60 Convex<Quadrilateral> buildConvexQuadrilateral(const HFNode<BV>& node,
61 const HeightField<BV>& model) {
62 const MatrixXs& heights = model.getHeights();
63 const VecXs& x_grid = model.getXGrid();
64 const VecXs& y_grid = model.getYGrid();
65
66 const Scalar min_height = model.getMinHeight();
67
68 const Scalar x0 = x_grid[node.x_id], x1 = x_grid[node.x_id + 1],
69 y0 = y_grid[node.y_id], y1 = y_grid[node.y_id + 1];
70 const Eigen::Block<const MatrixXs, 2, 2> cell =
71 heights.block<2, 2>(node.y_id, node.x_id);
72
73 assert(cell.maxCoeff() > min_height &&
74 "max_height is lower than min_height"); // Check whether the geometry
75 // is degenerated
76
77 std::shared_ptr<std::vector<Vec3s>> pts(new std::vector<Vec3s>({
78 Vec3s(x0, y0, min_height),
79 Vec3s(x0, y1, min_height),
80 Vec3s(x1, y1, min_height),
81 Vec3s(x1, y0, min_height),
82 Vec3s(x0, y0, cell(0, 0)),
83 Vec3s(x0, y1, cell(1, 0)),
84 Vec3s(x1, y1, cell(1, 1)),
85 Vec3s(x1, y0, cell(0, 1)),
86 }));
87
88 std::shared_ptr<std::vector<Quadrilateral>> polygons(
89 new std::vector<Quadrilateral>(6));
90 (*polygons)[0].set(0, 3, 2, 1); // x+ side
91 (*polygons)[1].set(0, 1, 5, 4); // y- side
92 (*polygons)[2].set(1, 2, 6, 5); // x- side
93 (*polygons)[3].set(2, 3, 7, 6); // y+ side
94 (*polygons)[4].set(3, 0, 4, 7); // z- side
95 (*polygons)[5].set(4, 5, 6, 7); // z+ side
96
97 return Convex<Quadrilateral>(pts, // points
98 8, // num points
99 polygons,
100 6 // number of polygons
101 );
102 }
103
104 enum class FaceOrientationConvexPart1 {
105 BOTTOM = 0,
106 TOP = 1,
107 WEST = 2,
108 SOUTH_EAST = 4,
109 NORTH = 8,
110 };
111
112 enum class FaceOrientationConvexPart2 {
113 BOTTOM = 0,
114 TOP = 1,
115 SOUTH = 2,
116 NORTH_WEST = 4,
117 EAST = 8,
118 };
119
120 template <typename BV>
121 32204 void buildConvexTriangles(const HFNode<BV>& node, const HeightField<BV>& model,
122 Convex<Triangle>& convex1, int& convex1_active_faces,
123 Convex<Triangle>& convex2,
124 int& convex2_active_faces) {
125 32204 const MatrixXs& heights = model.getHeights();
126 32204 const VecXs& x_grid = model.getXGrid();
127 32204 const VecXs& y_grid = model.getYGrid();
128
129 32204 const Scalar min_height = model.getMinHeight();
130
131
2/4
✓ Branch 1 taken 30591 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30591 times.
✗ Branch 5 not taken.
 32204 const Scalar x0 = x_grid[node.x_id], x1 = x_grid[node.x_id + 1],
132
2/4
✓ Branch 1 taken 30591 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30591 times.
✗ Branch 5 not taken.
 32204 y0 = y_grid[node.y_id], y1 = y_grid[node.y_id + 1];
133 32204 const Scalar max_height = node.max_height;
134 const Eigen::Block<const MatrixXs, 2, 2> cell =
135
1/2
✓ Branch 1 taken 30591 times.
✗ Branch 2 not taken.
 32204 heights.block<2, 2>(node.y_id, node.x_id);
136
137 32204 const int contact_active_faces = node.contact_active_faces;
138 32204 convex1_active_faces = 0;
139 32204 convex2_active_faces = 0;
140
141 typedef HFNodeBase::FaceOrientation FaceOrientation;
142
143
1/2
✓ Branch 1 taken 30591 times.
✗ Branch 2 not taken.
 32204 if (contact_active_faces & FaceOrientation::TOP) {
144 32204 convex1_active_faces |= int(details::FaceOrientationConvexPart1::TOP);
145 32204 convex2_active_faces |= int(details::FaceOrientationConvexPart2::TOP);
146 }
147
148
1/2
✓ Branch 1 taken 30591 times.
✗ Branch 2 not taken.
 32204 if (contact_active_faces & FaceOrientation::BOTTOM) {
149 32204 convex1_active_faces |= int(details::FaceOrientationConvexPart1::BOTTOM);
150 32204 convex2_active_faces |= int(details::FaceOrientationConvexPart2::BOTTOM);
151 }
152
153 // Specific orientation for Convex1
154
2/2
✓ Branch 1 taken 686 times.
✓ Branch 2 taken 29905 times.
 32204 if (contact_active_faces & FaceOrientation::WEST) {
155 783 convex1_active_faces |= int(details::FaceOrientationConvexPart1::WEST);
156 }
157
158
2/2
✓ Branch 1 taken 550 times.
✓ Branch 2 taken 30041 times.
 32204 if (contact_active_faces & FaceOrientation::NORTH) {
159 617 convex1_active_faces |= int(details::FaceOrientationConvexPart1::NORTH);
160 }
161
162 // Specific orientation for Convex2
163
2/2
✓ Branch 1 taken 417 times.
✓ Branch 2 taken 30174 times.
 32204 if (contact_active_faces & FaceOrientation::EAST) {
164 422 convex2_active_faces |= int(details::FaceOrientationConvexPart2::EAST);
165 }
166
167
2/2
✓ Branch 1 taken 474 times.
✓ Branch 2 taken 30117 times.
 32204 if (contact_active_faces & FaceOrientation::SOUTH) {
168 485 convex2_active_faces |= int(details::FaceOrientationConvexPart2::SOUTH);
169 }
170
171
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 30591 times.
 32204 assert(max_height > min_height &&
172 "max_height is lower than min_height"); // Check whether the geometry
173 // is degenerated
174 COAL_UNUSED_VARIABLE(max_height);
175
176 {
177
6/12
✓ Branch 1 taken 30591 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30591 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 30591 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 30591 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 30591 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 30591 times.
✗ Branch 18 not taken.
 64408 std::shared_ptr<std::vector<Vec3s>> pts(new std::vector<Vec3s>({
178 Vec3s(x0, y0, min_height), // A
179 Vec3s(x0, y1, min_height), // B
180 Vec3s(x1, y0, min_height), // C
181
2/4
✓ Branch 1 taken 30591 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30591 times.
✗ Branch 5 not taken.
 32204 Vec3s(x0, y0, cell(0, 0)), // D
182
2/4
✓ Branch 1 taken 30591 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30591 times.
✗ Branch 5 not taken.
 32204 Vec3s(x0, y1, cell(1, 0)), // E
183
2/4
✓ Branch 1 taken 30591 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30591 times.
✗ Branch 5 not taken.
 32204 Vec3s(x1, y0, cell(0, 1)), // F
184 }));
185
186
1/2
✓ Branch 2 taken 30591 times.
✗ Branch 3 not taken.
 64408 std::shared_ptr<std::vector<Triangle>> triangles(
187
2/4
✓ Branch 1 taken 30591 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30591 times.
✗ Branch 5 not taken.
 32204 new std::vector<Triangle>(8));
188 32204 (*triangles)[0].set(0, 2, 1); // bottom
189 32204 (*triangles)[1].set(3, 4, 5); // top
190 32204 (*triangles)[2].set(0, 1, 3); // West 1
191 32204 (*triangles)[3].set(3, 1, 4); // West 2
192 32204 (*triangles)[4].set(1, 2, 5); // South-East 1
193 32204 (*triangles)[5].set(1, 5, 4); // South-East 1
194 32204 (*triangles)[6].set(0, 5, 2); // North 1
195 32204 (*triangles)[7].set(5, 0, 3); // North 2
196
197
1/2
✓ Branch 3 taken 30591 times.
✗ Branch 4 not taken.
 32204 convex1.set(pts, // points
198 6, // num points
199 triangles,
200 8 // number of polygons
201 );
202 32204 }
203
204 {
205
6/12
✓ Branch 1 taken 30591 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30591 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 30591 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 30591 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 30591 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 30591 times.
✗ Branch 18 not taken.
 64408 std::shared_ptr<std::vector<Vec3s>> pts(new std::vector<Vec3s>({
206 Vec3s(x0, y1, min_height), // A
207 Vec3s(x1, y1, min_height), // B
208 Vec3s(x1, y0, min_height), // C
209
2/4
✓ Branch 1 taken 30591 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30591 times.
✗ Branch 5 not taken.
 32204 Vec3s(x0, y1, cell(1, 0)), // D
210
2/4
✓ Branch 1 taken 30591 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30591 times.
✗ Branch 5 not taken.
 32204 Vec3s(x1, y1, cell(1, 1)), // E
211
2/4
✓ Branch 1 taken 30591 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30591 times.
✗ Branch 5 not taken.
 32204 Vec3s(x1, y0, cell(0, 1)), // F
212 }));
213
214
1/2
✓ Branch 2 taken 30591 times.
✗ Branch 3 not taken.
 64408 std::shared_ptr<std::vector<Triangle>> triangles(
215
2/4
✓ Branch 1 taken 30591 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30591 times.
✗ Branch 5 not taken.
 32204 new std::vector<Triangle>(8));
216 32204 (*triangles)[0].set(2, 1, 0); // bottom
217 32204 (*triangles)[1].set(3, 4, 5); // top
218 32204 (*triangles)[2].set(0, 1, 3); // South 1
219 32204 (*triangles)[3].set(3, 1, 4); // South 2
220 32204 (*triangles)[4].set(0, 5, 2); // North West 1
221 32204 (*triangles)[5].set(0, 3, 5); // North West 2
222 32204 (*triangles)[6].set(1, 2, 5); // East 1
223 32204 (*triangles)[7].set(4, 1, 2); // East 2
224
225
1/2
✓ Branch 3 taken 30591 times.
✗ Branch 4 not taken.
 32204 convex2.set(pts, // points
226 6, // num points
227 triangles,
228 8 // number of polygons
229 );
230 32204 }
231 32204 }
232
233 122551 inline Vec3s projectTriangle(const Vec3s& pointA, const Vec3s& pointB,
234 const Vec3s& pointC, const Vec3s& point) {
235 const Project<Scalar>::ProjectResult result =
236
1/2
✓ Branch 1 taken 122551 times.
✗ Branch 2 not taken.
 122551 Project<Scalar>::projectTriangle(pointA, pointB, pointC, point);
237
2/4
✓ Branch 1 taken 122551 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 122551 times.
✗ Branch 5 not taken.
 122551 Vec3s res = result.parameterization[0] * pointA +
238
2/4
✓ Branch 1 taken 122551 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 122551 times.
✗ Branch 5 not taken.
 245102 result.parameterization[1] * pointB +
239
2/4
✓ Branch 1 taken 122551 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 122551 times.
✗ Branch 5 not taken.
 245102 result.parameterization[2] * pointC;
240
241 245102 return res;
242 }
243
244 inline Vec3s projectTetrahedra(const Vec3s& pointA, const Vec3s& pointB,
245 const Vec3s& pointC, const Vec3s& pointD,
246 const Vec3s& point) {
247 const Project<Scalar>::ProjectResult result =
248 Project<Scalar>::projectTetrahedra(pointA, pointB, pointC, pointD, point);
249 Vec3s res = result.parameterization[0] * pointA +
250 result.parameterization[1] * pointB +
251 result.parameterization[2] * pointC +
252 result.parameterization[3] * pointD;
253
254 return res;
255 }
256
257 87 inline Vec3s computeTriangleNormal(const Triangle& triangle,
258 const std::vector<Vec3s>& points) {
259
1/2
✓ Branch 3 taken 87 times.
✗ Branch 4 not taken.
 87 const Vec3s pointA = points[triangle[0]];
260
1/2
✓ Branch 3 taken 87 times.
✗ Branch 4 not taken.
 87 const Vec3s pointB = points[triangle[1]];
261
1/2
✓ Branch 3 taken 87 times.
✗ Branch 4 not taken.
 87 const Vec3s pointC = points[triangle[2]];
262
263
4/8
✓ Branch 1 taken 87 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 87 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 87 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 87 times.
✗ Branch 11 not taken.
 87 const Vec3s normal = (pointB - pointA).cross(pointC - pointA).normalized();
264
4/8
✓ Branch 1 taken 87 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 87 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 87 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 87 times.
✗ Branch 10 not taken.
 87 assert(!normal.array().isNaN().any() && "normal is ill-defined");
265
266 174 return normal;
267 }
268
269 122551 inline Vec3s projectPointOnTriangle(const Vec3s& contact_point,
270 const Triangle& triangle,
271 const std::vector<Vec3s>& points) {
272
1/2
✓ Branch 3 taken 122551 times.
✗ Branch 4 not taken.
 122551 const Vec3s pointA = points[triangle[0]];
273
1/2
✓ Branch 3 taken 122551 times.
✗ Branch 4 not taken.
 122551 const Vec3s pointB = points[triangle[1]];
274
1/2
✓ Branch 3 taken 122551 times.
✗ Branch 4 not taken.
 122551 const Vec3s pointC = points[triangle[2]];
275
276 const Vec3s contact_point_projected =
277
1/2
✓ Branch 1 taken 122551 times.
✗ Branch 2 not taken.
 122551 projectTriangle(pointA, pointB, pointC, contact_point);
278
279 245102 return contact_point_projected;
280 }
281
282 122464 inline Scalar distanceContactPointToTriangle(const Vec3s& contact_point,
283 const Triangle& triangle,
284 const std::vector<Vec3s>& points) {
285 const Vec3s contact_point_projected =
286
1/2
✓ Branch 1 taken 122464 times.
✗ Branch 2 not taken.
 122464 projectPointOnTriangle(contact_point, triangle, points);
287
2/4
✓ Branch 1 taken 122464 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 122464 times.
✗ Branch 5 not taken.
 122464 return (contact_point_projected - contact_point).norm();
288 }
289
290 122410 inline Scalar distanceContactPointToFace(const size_t face_id,
291 const Vec3s& contact_point,
292 const Convex<Triangle>& convex,
293 size_t& closest_face_id) {
294
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 122410 times.
 122410 assert((face_id >= 0 && face_id < 8) && "face_id should be in [0;7]");
295
296 122410 const std::vector<Vec3s>& points = *(convex.points);
297
2/2
✓ Branch 0 taken 122356 times.
✓ Branch 1 taken 54 times.
 122410 if (face_id <= 1) {
298 122356 const Triangle& triangle = (*(convex.polygons))[face_id];
299 122356 closest_face_id = face_id;
300 122356 return distanceContactPointToTriangle(contact_point, triangle, points);
301 } else {
302 54 const Triangle& triangle1 = (*(convex.polygons))[face_id];
303 const Scalar distance_to_triangle1 =
304 54 distanceContactPointToTriangle(contact_point, triangle1, points);
305
306 54 const Triangle& triangle2 = (*(convex.polygons))[face_id + 1];
307 const Scalar distance_to_triangle2 =
308 54 distanceContactPointToTriangle(contact_point, triangle2, points);
309
310
2/2
✓ Branch 0 taken 20 times.
✓ Branch 1 taken 34 times.
 54 if (distance_to_triangle1 > distance_to_triangle2) {
311 20 closest_face_id = face_id + 1;
312 20 return distance_to_triangle2;
313 } else {
314 34 closest_face_id = face_id;
315 34 return distance_to_triangle1;
316 }
317 }
318 }
319
320 template <typename Polygone, typename Shape>
321 122360 bool binCorrection(const Convex<Polygone>& convex,
322 const int convex_active_faces, const Shape& shape,
323 const Transform3s& shape_pose, Scalar& distance,
324 Vec3s& contact_1, Vec3s& contact_2, Vec3s& normal,
325 Vec3s& face_normal, const bool is_collision) {
326 122360 const Scalar prec = Scalar(1e-12);
327 122360 const std::vector<Vec3s>& points = *(convex.points);
328
329 122360 bool hfield_witness_is_on_bin_side = true;
330
331 // int closest_face_id_bottom_face = -1;
332 // int closest_face_id_top_face = -1;
333
334 122360 std::vector<size_t> active_faces;
335
1/2
✓ Branch 1 taken 61180 times.
✗ Branch 2 not taken.
 122360 active_faces.reserve(5);
336
1/2
✓ Branch 1 taken 61180 times.
✗ Branch 2 not taken.
 122360 active_faces.push_back(0);
337
1/2
✓ Branch 1 taken 61180 times.
✗ Branch 2 not taken.
 122360 active_faces.push_back(1);
338
339
3/4
✓ Branch 0 taken 1158 times.
✓ Branch 1 taken 60022 times.
✓ Branch 3 taken 1158 times.
✗ Branch 4 not taken.
 122360 if (convex_active_faces & 2) active_faces.push_back(2);
340
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 61180 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 122360 if (convex_active_faces & 4) active_faces.push_back(4);
341
3/4
✓ Branch 0 taken 965 times.
✓ Branch 1 taken 60215 times.
✓ Branch 3 taken 965 times.
✗ Branch 4 not taken.
 122360 if (convex_active_faces & 8) active_faces.push_back(6);
342
343 122360 Triangle face_triangle;
344 122360 Scalar shortest_distance_to_face = (std::numeric_limits<Scalar>::max)();
345
1/2
✓ Branch 1 taken 61180 times.
✗ Branch 2 not taken.
 122360 face_normal = normal;
346
2/2
✓ Branch 5 taken 122410 times.
✓ Branch 6 taken 65 times.
 244950 for (const size_t active_face : active_faces) {
347 size_t closest_face_id;
348
1/2
✓ Branch 1 taken 122410 times.
✗ Branch 2 not taken.
 244820 const Scalar distance_to_face = distanceContactPointToFace(
349 active_face, contact_1, convex, closest_face_id);
350
351 244820 const bool contact_point_is_on_face = distance_to_face <= prec;
352
2/2
✓ Branch 0 taken 61115 times.
✓ Branch 1 taken 61295 times.
 244820 if (contact_point_is_on_face) {
353 122230 hfield_witness_is_on_bin_side = false;
354 122230 face_triangle = (*(convex.polygons))[closest_face_id];
355 122230 shortest_distance_to_face = distance_to_face;
356 122230 break;
357
2/2
✓ Branch 0 taken 61241 times.
✓ Branch 1 taken 54 times.
 122590 } else if (distance_to_face < shortest_distance_to_face) {
358 122482 face_triangle = (*(convex.polygons))[closest_face_id];
359 122482 shortest_distance_to_face = distance_to_face;
360 }
361 }
362
363 // We correct only if there is a collision with the bin
364
2/2
✓ Branch 0 taken 87 times.
✓ Branch 1 taken 61093 times.
 122360 if (is_collision) {
365
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 87 times.
 174 if (!face_triangle.isValid())
366 ✗ COAL_THROW_PRETTY("face_triangle is not initialized", std::logic_error);
367
368
1/2
✓ Branch 3 taken 87 times.
✗ Branch 4 not taken.
 174 const Vec3s face_pointA = points[face_triangle[0]];
369
1/2
✓ Branch 1 taken 87 times.
✗ Branch 2 not taken.
 174 face_normal = computeTriangleNormal(face_triangle, points);
370
371 174 int hint = 0;
372 // Since we compute the support manually, we need to take into account the
373 // sphere swept radius of the shape.
374 // TODO: take into account the swept-sphere radius of the bin.
375
4/8
✓ Branch 1 taken 87 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 87 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 87 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 87 times.
✗ Branch 11 not taken.
 174 const Vec3s _support = getSupport<details::SupportOptions::WithSweptSphere>(
376
1/2
✓ Branch 2 taken 87 times.
✗ Branch 3 not taken.
 174 &shape, -shape_pose.rotation().transpose() * face_normal, hint);
377
2/4
✓ Branch 1 taken 87 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 87 times.
✗ Branch 5 not taken.
 174 const Vec3s support =
378
1/2
✓ Branch 3 taken 87 times.
✗ Branch 4 not taken.
 174 shape_pose.rotation() * _support + shape_pose.translation();
379
380 // Project support into the inclined bin having triangle
381
1/2
✓ Branch 1 taken 87 times.
✗ Branch 2 not taken.
 174 const Scalar offset_plane = face_normal.dot(face_pointA);
382
1/2
✓ Branch 1 taken 87 times.
✗ Branch 2 not taken.
 174 const Plane projection_plane(face_normal, offset_plane);
383 174 const Scalar distance_support_projection_plane =
384
1/2
✓ Branch 1 taken 87 times.
✗ Branch 2 not taken.
 174 projection_plane.signedDistance(support);
385
386
3/6
✓ Branch 1 taken 87 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 87 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 87 times.
✗ Branch 8 not taken.
 174 const Vec3s projected_support =
387 support - distance_support_projection_plane * face_normal;
388
389 // We need now to project the projected in the triangle shape
390
1/2
✓ Branch 1 taken 87 times.
✗ Branch 2 not taken.
 174 contact_1 =
391 projectPointOnTriangle(projected_support, face_triangle, points);
392
3/6
✓ Branch 1 taken 87 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 87 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 87 times.
✗ Branch 8 not taken.
 174 contact_2 = contact_1 + distance_support_projection_plane * face_normal;
393
1/2
✓ Branch 1 taken 87 times.
✗ Branch 2 not taken.
 174 normal = face_normal;
394 174 distance = -std::fabs(distance_support_projection_plane);
395 }
396
397 122360 return hfield_witness_is_on_bin_side;
398 }
399
400 template <typename Polygone, typename Shape, int Options>
401 61180 bool shapeDistance(const GJKSolver* nsolver, const CollisionRequest& request,
402 const Convex<Polygone>& convex1,
403 const int convex1_active_faces,
404 const Convex<Polygone>& convex2,
405 const int convex2_active_faces, const Transform3s& tf1,
406 const Shape& shape, const Transform3s& tf2, Scalar& distance,
407 Vec3s& c1, Vec3s& c2, Vec3s& normal, Vec3s& normal_top,
408 bool& hfield_witness_is_on_bin_side) {
409 enum { RTIsIdentity = Options & RelativeTransformationIsIdentity };
410
411
1/2
✓ Branch 1 taken 30590 times.
✗ Branch 2 not taken.
 61180 const Transform3s Id;
412 // The solver `nsolver` has already been set up by the collision request
413 // `request`. If GJK early stopping is enabled through `request`, it will be
414 // used.
415 // The only thing we need to make sure is that in case of collision, the
416 // penetration information is computed (as we do bins comparison).
417 61180 const bool compute_penetration = true;
418
4/8
✓ Branch 1 taken 30590 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30590 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 30590 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 30590 times.
✗ Branch 11 not taken.
 61180 Vec3s contact1_1, contact1_2, contact2_1, contact2_2;
419
4/8
✓ Branch 1 taken 30590 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30590 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 30590 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 30590 times.
✗ Branch 11 not taken.
 61180 Vec3s normal1, normal1_top, normal2, normal2_top;
420 Scalar distance1, distance2;
421
422 if (RTIsIdentity) {
423 distance1 = internal::ShapeShapeDistance<Convex<Polygone>, Shape>(
424 &convex1, Id, &shape, tf2, nsolver, compute_penetration, contact1_1,
425 contact1_2, normal1);
426 } else {
427
1/2
✓ Branch 1 taken 30590 times.
✗ Branch 2 not taken.
 61180 distance1 = internal::ShapeShapeDistance<Convex<Polygone>, Shape>(
428 &convex1, tf1, &shape, tf2, nsolver, compute_penetration, contact1_1,
429 contact1_2, normal1);
430 }
431 61180 bool collision1 = (distance1 - request.security_margin <=
432 61180 request.collision_distance_threshold);
433
434 bool hfield_witness_is_on_bin_side1 =
435
1/2
✓ Branch 1 taken 30590 times.
✗ Branch 2 not taken.
 61180 binCorrection(convex1, convex1_active_faces, shape, tf2, distance1,
436 contact1_1, contact1_2, normal1, normal1_top, collision1);
437
438 if (RTIsIdentity) {
439 distance2 = internal::ShapeShapeDistance<Convex<Polygone>, Shape>(
440 &convex2, Id, &shape, tf2, nsolver, compute_penetration, contact2_1,
441 contact2_2, normal2);
442 } else {
443
1/2
✓ Branch 1 taken 30590 times.
✗ Branch 2 not taken.
 61180 distance2 = internal::ShapeShapeDistance<Convex<Polygone>, Shape>(
444 &convex2, tf1, &shape, tf2, nsolver, compute_penetration, contact2_1,
445 contact2_2, normal2);
446 }
447 61180 bool collision2 = (distance2 - request.security_margin <=
448 61180 request.collision_distance_threshold);
449
450 bool hfield_witness_is_on_bin_side2 =
451
1/2
✓ Branch 1 taken 30590 times.
✗ Branch 2 not taken.
 61180 binCorrection(convex2, convex2_active_faces, shape, tf2, distance2,
452 contact2_1, contact2_2, normal2, normal2_top, collision2);
453
454
4/4
✓ Branch 0 taken 33 times.
✓ Branch 1 taken 30557 times.
✓ Branch 2 taken 31 times.
✓ Branch 3 taken 2 times.
 61180 if (collision1 && collision2) {
455
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 31 times.
 62 if (distance1 > distance2) // switch values
456 {
457 ✗ distance = distance2;
458 ✗ c1 = contact2_1;
459 ✗ c2 = contact2_2;
460 ✗ normal = normal2;
461 ✗ normal_top = normal2_top;
462 ✗ hfield_witness_is_on_bin_side = hfield_witness_is_on_bin_side2;
463 } else {
464 62 distance = distance1;
465
1/2
✓ Branch 1 taken 31 times.
✗ Branch 2 not taken.
 62 c1 = contact1_1;
466
1/2
✓ Branch 1 taken 31 times.
✗ Branch 2 not taken.
 62 c2 = contact1_2;
467
1/2
✓ Branch 1 taken 31 times.
✗ Branch 2 not taken.
 62 normal = normal1;
468
1/2
✓ Branch 1 taken 31 times.
✗ Branch 2 not taken.
 62 normal_top = normal1_top;
469 62 hfield_witness_is_on_bin_side = hfield_witness_is_on_bin_side1;
470 }
471 62 return true;
472
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 30557 times.
 61118 } else if (collision1) {
473 4 distance = distance1;
474
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 c1 = contact1_1;
475
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 c2 = contact1_2;
476
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 normal = normal1;
477
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 normal_top = normal1_top;
478 4 hfield_witness_is_on_bin_side = hfield_witness_is_on_bin_side1;
479 4 return true;
480
2/2
✓ Branch 0 taken 23 times.
✓ Branch 1 taken 30534 times.
 61114 } else if (collision2) {
481 46 distance = distance2;
482
1/2
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
 46 c1 = contact2_1;
483
1/2
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
 46 c2 = contact2_2;
484
1/2
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
 46 normal = normal2;
485
1/2
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
 46 normal_top = normal2_top;
486 46 hfield_witness_is_on_bin_side = hfield_witness_is_on_bin_side2;
487 46 return true;
488 }
489
490
2/2
✓ Branch 0 taken 12197 times.
✓ Branch 1 taken 18337 times.
 61068 if (distance1 > distance2) // switch values
491 {
492 24394 distance = distance2;
493
1/2
✓ Branch 1 taken 12197 times.
✗ Branch 2 not taken.
 24394 c1 = contact2_1;
494
1/2
✓ Branch 1 taken 12197 times.
✗ Branch 2 not taken.
 24394 c2 = contact2_2;
495
1/2
✓ Branch 1 taken 12197 times.
✗ Branch 2 not taken.
 24394 normal = normal2;
496
1/2
✓ Branch 1 taken 12197 times.
✗ Branch 2 not taken.
 24394 normal_top = normal2_top;
497 24394 hfield_witness_is_on_bin_side = hfield_witness_is_on_bin_side2;
498 } else {
499 36674 distance = distance1;
500
1/2
✓ Branch 1 taken 18337 times.
✗ Branch 2 not taken.
 36674 c1 = contact1_1;
501
1/2
✓ Branch 1 taken 18337 times.
✗ Branch 2 not taken.
 36674 c2 = contact1_2;
502
1/2
✓ Branch 1 taken 18337 times.
✗ Branch 2 not taken.
 36674 normal = normal1;
503
1/2
✓ Branch 1 taken 18337 times.
✗ Branch 2 not taken.
 36674 normal_top = normal1_top;
504 36674 hfield_witness_is_on_bin_side = hfield_witness_is_on_bin_side1;
505 }
506 61068 return false;
507 }
508
509 } // namespace details
510
511 /// @brief Traversal node for collision between height field and shape
512 template <typename BV, typename S,
513 int _Options = RelativeTransformationIsIdentity>
514 class HeightFieldShapeCollisionTraversalNode
515 : public CollisionTraversalNodeBase {
516 public:
517 typedef CollisionTraversalNodeBase Base;
518
519 enum {
520 Options = _Options,
521 RTIsIdentity = _Options & RelativeTransformationIsIdentity
522 };
523
524 110 HeightFieldShapeCollisionTraversalNode(const CollisionRequest& request)
525
1/2
✓ Branch 2 taken 55 times.
✗ Branch 3 not taken.
 110 : CollisionTraversalNodeBase(request) {
526 110 model1 = NULL;
527 110 model2 = NULL;
528
529 110 num_bv_tests = 0;
530 110 num_leaf_tests = 0;
531 110 query_time_seconds = 0.0;
532
533 110 nsolver = NULL;
534 110 count = 0;
535 }
536
537 /// @brief Whether the BV node in the first BVH tree is leaf
538 127594 bool isFirstNodeLeaf(unsigned int b) const {
539 127594 return model1->getBV(b).isLeaf();
540 }
541
542 /// @brief Obtain the left child of BV node in the first BVH
543 63822 int getFirstLeftChild(unsigned int b) const {
544 63822 return static_cast<int>(model1->getBV(b).leftChild());
545 }
546
547 /// @brief Obtain the right child of BV node in the first BVH
548 63822 int getFirstRightChild(unsigned int b) const {
549 63822 return static_cast<int>(model1->getBV(b).rightChild());
550 }
551
552 /// test between BV b1 and shape
553 /// @param b1 BV to test,
554 /// @retval sqrDistLowerBound square of a lower bound of the minimal
555 /// distance between bounding volumes.
556 /// @brief BV culling test in one BVTT node
557 66478 bool BVDisjoints(unsigned int b1, unsigned int /*b2*/,
558 Scalar& sqrDistLowerBound) const {
559
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 33239 times.
 66478 if (this->enable_statistics) this->num_bv_tests++;
560
561 bool disjoint;
562 if (RTIsIdentity) {
563 assert(false && "must never happened");
564 disjoint = !this->model1->getBV(b1).bv.overlap(
565 this->model2_bv, this->request, sqrDistLowerBound);
566 } else {
567 66478 disjoint = !overlap(this->tf1.getRotation(), this->tf1.getTranslation(),
568 66478 this->model1->getBV(b1).bv, this->model2_bv,
569 this->request, sqrDistLowerBound);
570 }
571
572
2/2
✓ Branch 0 taken 1328 times.
✓ Branch 1 taken 31911 times.
 66478 if (disjoint)
573 2656 internal::updateDistanceLowerBoundFromBV(this->request, *this->result,
574 sqrDistLowerBound);
575
576
3/4
✓ Branch 0 taken 1328 times.
✓ Branch 1 taken 31911 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1328 times.
 66478 assert(!disjoint || sqrDistLowerBound > 0);
577 66478 return disjoint;
578 }
579
580 /// @brief Intersection testing between leaves (one Convex and one shape)
581 61116 void leafCollides(unsigned int b1, unsigned int /*b2*/,
582 Scalar& sqrDistLowerBound) const {
583 61116 count++;
584
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 30558 times.
 61116 if (this->enable_statistics) this->num_leaf_tests++;
585
1/2
✓ Branch 1 taken 30558 times.
✗ Branch 2 not taken.
 61116 const HFNode<BV>& node = this->model1->getBV(b1);
586
587 // Split quadrilateral primitives into two convex shapes corresponding to
588 // polyhedron with triangular bases. This is essential to keep the convexity
589
590 // typedef Convex<Quadrilateral> ConvexQuadrilateral;
591 // const ConvexQuadrilateral convex =
592 // details::buildConvexQuadrilateral(node,*this->model1);
593
594 typedef Convex<Triangle> ConvexTriangle;
595
2/4
✓ Branch 1 taken 30558 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30558 times.
✗ Branch 5 not taken.
 61116 ConvexTriangle convex1, convex2;
596 int convex1_active_faces, convex2_active_faces;
597 // TODO: inherit from hfield's inflation here
598
1/2
✓ Branch 1 taken 30558 times.
✗ Branch 2 not taken.
 61116 details::buildConvexTriangles(node, *this->model1, convex1,
599 convex1_active_faces, convex2,
600 convex2_active_faces);
601
602 // Compute aabb_local for BoundingVolumeGuess case in the GJK solver
603
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 30558 times.
 61116 if (nsolver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
604 ✗ convex1.computeLocalAABB();
605 ✗ convex2.computeLocalAABB();
606 }
607
608 Scalar distance;
609 // Vec3s contact_point, normal;
610
4/8
✓ Branch 1 taken 30558 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30558 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 30558 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 30558 times.
✗ Branch 11 not taken.
 61116 Vec3s c1, c2, normal, normal_face;
611 bool hfield_witness_is_on_bin_side;
612
613 122232 bool collision = details::shapeDistance<Triangle, S, Options>(
614 61116 nsolver, this->request, convex1, convex1_active_faces, convex2,
615
1/2
✓ Branch 1 taken 30558 times.
✗ Branch 2 not taken.
 61116 convex2_active_faces, this->tf1, *(this->model2), this->tf2, distance,
616 c1, c2, normal, normal_face, hfield_witness_is_on_bin_side);
617
618 61116 Scalar distToCollision = distance - this->request.security_margin;
619
2/2
✓ Branch 0 taken 24 times.
✓ Branch 1 taken 30534 times.
 61116 if (distToCollision <= this->request.collision_distance_threshold) {
620 48 sqrDistLowerBound = 0;
621
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 48 if (this->result->numContacts() < this->request.num_max_contacts) {
622
4/8
✓ Branch 2 taken 24 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 24 times.
✓ Branch 8 taken 24 times.
✗ Branch 9 not taken.
 48 if (normal_face.isApprox(normal) &&
623 ✗ (collision || !hfield_witness_is_on_bin_side)) {
624
2/4
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
 48 this->result->addContact(Contact(this->model1, this->model2, (int)b1,
625 (int)Contact::NONE, c1, c2, normal,
626 distance));
627
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 24 times.
 48 assert(this->result->isCollision());
628 }
629 }
630 } else
631 61068 sqrDistLowerBound = distToCollision * distToCollision;
632
633 // const Vec3s c1 = contact_point - distance * 0.5 * normal;
634 // const Vec3s c2 = contact_point + distance * 0.5 * normal;
635
1/2
✓ Branch 1 taken 30558 times.
✗ Branch 2 not taken.
 61116 internal::updateDistanceLowerBoundFromLeaf(this->request, *this->result,
636 distToCollision, c1, c2, normal);
637
638
3/4
✓ Branch 1 taken 30534 times.
✓ Branch 2 taken 24 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 30534 times.
 61116 assert(this->result->isCollision() || sqrDistLowerBound > 0);
639 }
640
641 const GJKSolver* nsolver;
642
643 const HeightField<BV>* model1;
644 const S* model2;
645 BV model2_bv;
646
647 mutable int num_bv_tests;
648 mutable int num_leaf_tests;
649 mutable Scalar query_time_seconds;
650 mutable int count;
651 };
652
653 /// @}
654
655 /// @addtogroup Traversal_For_Distance
656 /// @{
657
658 /// @brief Traversal node for distance between height field and shape
659 template <typename BV, typename S,
660 int _Options = RelativeTransformationIsIdentity>
661 class HeightFieldShapeDistanceTraversalNode : public DistanceTraversalNodeBase {
662 public:
663 typedef DistanceTraversalNodeBase Base;
664
665 enum {
666 Options = _Options,
667 RTIsIdentity = _Options & RelativeTransformationIsIdentity
668 };
669
670 HeightFieldShapeDistanceTraversalNode() : DistanceTraversalNodeBase() {
671 model1 = NULL;
672 model2 = NULL;
673
674 num_leaf_tests = 0;
675 query_time_seconds = 0.0;
676
677 rel_err = 0;
678 abs_err = 0;
679 nsolver = NULL;
680 }
681
682 /// @brief Whether the BV node in the first BVH tree is leaf
683 bool isFirstNodeLeaf(unsigned int b) const {
684 return model1->getBV(b).isLeaf();
685 }
686
687 /// @brief Obtain the left child of BV node in the first BVH
688 int getFirstLeftChild(unsigned int b) const {
689 return model1->getBV(b).leftChild();
690 }
691
692 /// @brief Obtain the right child of BV node in the first BVH
693 int getFirstRightChild(unsigned int b) const {
694 return model1->getBV(b).rightChild();
695 }
696
697 /// @brief BV culling test in one BVTT node
698 Scalar BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const {
699 return model1->getBV(b1).bv.distance(
700 model2_bv); // TODO(jcarpent): tf1 is not taken into account here.
701 }
702
703 /// @brief Distance testing between leaves (one bin of the height field and
704 /// one shape)
705 /// TODO(louis): deal with Hfield-Shape distance just like in Hfield-Shape
706 /// collision (bin correction etc).
707 void leafComputeDistance(unsigned int b1, unsigned int /*b2*/) const {
708 if (this->enable_statistics) this->num_leaf_tests++;
709
710 const BVNode<BV>& node = this->model1->getBV(b1);
711
712 typedef Convex<Quadrilateral> ConvexQuadrilateral;
713 const ConvexQuadrilateral convex =
714 details::buildConvexQuadrilateral(node, *this->model1);
715
716 Vec3s p1, p2, normal;
717 const Scalar distance =
718 internal::ShapeShapeDistance<ConvexQuadrilateral, S>(
719 &convex, this->tf1, this->model2, this->tf2, this->nsolver,
720 this->request.enable_signed_distance, p1, p2, normal);
721
722 this->result->update(distance, this->model1, this->model2, b1,
723 DistanceResult::NONE, p1, p2, normal);
724 }
725
726 /// @brief Whether the traversal process can stop early
727 bool canStop(Scalar c) const {
728 if ((c >= this->result->min_distance - abs_err) &&
729 (c * (1 + rel_err) >= this->result->min_distance))
730 return true;
731 return false;
732 }
733
734 Scalar rel_err;
735 Scalar abs_err;
736
737 const GJKSolver* nsolver;
738
739 const HeightField<BV>* model1;
740 const S* model2;
741 BV model2_bv;
742
743 mutable int num_bv_tests;
744 mutable int num_leaf_tests;
745 mutable Scalar query_time_seconds;
746 };
747
748 /// @}
749
750 } // namespace coal
751
752 /// @endcond
753
754 #endif
755