GCC Code Coverage Report

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