GCC Code Coverage Report

Directory: ./
File: src/intersect.cpp
Date: 2025-04-01 09:23:31
Exec Total Coverage
Lines: 177 206 85.9%
Branches: 334 616 54.2%
Line Branch Exec Source
1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011-2014, Willow Garage, Inc.
5 * Copyright (c) 2014-2015, Open Source Robotics Foundation
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of Open Source Robotics Foundation nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 */
35
36 /** \author Jia Pan */
37
38 #include "coal/internal/intersect.h"
39 #include "coal/internal/tools.h"
40
41 #include <iostream>
42 #include <limits>
43 #include <vector>
44 #include <cmath>
45
46 namespace coal {
47
48 ✗ bool Intersect::buildTrianglePlane(const Vec3s& v1, const Vec3s& v2,
49 const Vec3s& v3, Vec3s* n, Scalar* t) {
50 ✗ Vec3s n_ = (v2 - v1).cross(v3 - v1);
51 ✗ Scalar norm2 = n_.squaredNorm();
52 ✗ if (norm2 > 0) {
53 ✗ *n = n_ / sqrt(norm2);
54 ✗ *t = n->dot(v1);
55 ✗ return true;
56 }
57 ✗ return false;
58 }
59
60 863428 void TriangleDistance::segPoints(const Vec3s& P, const Vec3s& A, const Vec3s& Q,
61 const Vec3s& B, Vec3s& VEC, Vec3s& X,
62 Vec3s& Y) {
63
1/2
✓ Branch 1 taken 863428 times.
✗ Branch 2 not taken.
 863428 Vec3s T;
64 Scalar A_dot_A, B_dot_B, A_dot_B, A_dot_T, B_dot_T;
65
1/2
✓ Branch 1 taken 863428 times.
✗ Branch 2 not taken.
 863428 Vec3s TMP;
66
67
2/4
✓ Branch 1 taken 863428 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 863428 times.
✗ Branch 5 not taken.
 863428 T = Q - P;
68
1/2
✓ Branch 1 taken 863428 times.
✗ Branch 2 not taken.
 863428 A_dot_A = A.dot(A);
69
1/2
✓ Branch 1 taken 863428 times.
✗ Branch 2 not taken.
 863428 B_dot_B = B.dot(B);
70
1/2
✓ Branch 1 taken 863428 times.
✗ Branch 2 not taken.
 863428 A_dot_B = A.dot(B);
71
1/2
✓ Branch 1 taken 863428 times.
✗ Branch 2 not taken.
 863428 A_dot_T = A.dot(T);
72
1/2
✓ Branch 1 taken 863428 times.
✗ Branch 2 not taken.
 863428 B_dot_T = B.dot(T);
73
74 // t parameterizes ray P,A
75 // u parameterizes ray Q,B
76
77 Scalar t, u;
78
79 // compute t for the closest point on ray P,A to
80 // ray Q,B
81
82 863428 Scalar denom = A_dot_A * B_dot_B - A_dot_B * A_dot_B;
83
84 863428 t = (A_dot_T * B_dot_B - B_dot_T * A_dot_B) / denom;
85
86 // clamp result so t is on the segment P,A
87
88
6/6
✓ Branch 0 taken 514785 times.
✓ Branch 1 taken 348643 times.
✓ Branch 3 taken 5462 times.
✓ Branch 4 taken 509323 times.
✓ Branch 5 taken 354105 times.
✓ Branch 6 taken 509323 times.
 863428 if ((t < 0) || std::isnan(t))
89 354105 t = 0;
90
2/2
✓ Branch 0 taken 377618 times.
✓ Branch 1 taken 131705 times.
 509323 else if (t > 1)
91 377618 t = 1;
92
93 // find u for point on ray Q,B closest to point at t
94
95 863428 u = (t * A_dot_B - B_dot_T) / B_dot_B;
96
97 // if u is on segment Q,B, t and u correspond to
98 // closest points, otherwise, clamp u, recompute and
99 // clamp t
100
101
5/6
✓ Branch 0 taken 520802 times.
✓ Branch 1 taken 342626 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 520802 times.
✓ Branch 5 taken 342626 times.
✓ Branch 6 taken 520802 times.
 863428 if ((u <= 0) || std::isnan(u)) {
102
1/2
✓ Branch 1 taken 342626 times.
✗ Branch 2 not taken.
 342626 Y = Q;
103
104 342626 t = A_dot_T / A_dot_A;
105
106
5/6
✓ Branch 0 taken 234676 times.
✓ Branch 1 taken 107950 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 234676 times.
✓ Branch 5 taken 107950 times.
✓ Branch 6 taken 234676 times.
 342626 if ((t <= 0) || std::isnan(t)) {
107
1/2
✓ Branch 1 taken 107950 times.
✗ Branch 2 not taken.
 107950 X = P;
108
2/4
✓ Branch 1 taken 107950 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 107950 times.
✗ Branch 5 not taken.
 107950 VEC = Q - P;
109
2/2
✓ Branch 0 taken 196196 times.
✓ Branch 1 taken 38480 times.
 234676 } else if (t >= 1) {
110
2/4
✓ Branch 1 taken 196196 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 196196 times.
✗ Branch 5 not taken.
 196196 X = P + A;
111
2/4
✓ Branch 1 taken 196196 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 196196 times.
✗ Branch 5 not taken.
 196196 VEC = Q - X;
112 } else {
113
3/6
✓ Branch 1 taken 38480 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 38480 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 38480 times.
✗ Branch 8 not taken.
 38480 X = P + A * t;
114
1/2
✓ Branch 1 taken 38480 times.
✗ Branch 2 not taken.
 38480 TMP = T.cross(A);
115
1/2
✓ Branch 1 taken 38480 times.
✗ Branch 2 not taken.
 38480 VEC = A.cross(TMP);
116 }
117
2/2
✓ Branch 0 taken 461339 times.
✓ Branch 1 taken 59463 times.
 520802 } else if (u >= 1) {
118
2/4
✓ Branch 1 taken 461339 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 461339 times.
✗ Branch 5 not taken.
 461339 Y = Q + B;
119
120 461339 t = (A_dot_B + A_dot_T) / A_dot_A;
121
122
5/6
✓ Branch 0 taken 249655 times.
✓ Branch 1 taken 211684 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 249655 times.
✓ Branch 5 taken 211684 times.
✓ Branch 6 taken 249655 times.
 461339 if ((t <= 0) || std::isnan(t)) {
123
1/2
✓ Branch 1 taken 211684 times.
✗ Branch 2 not taken.
 211684 X = P;
124
2/4
✓ Branch 1 taken 211684 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 211684 times.
✗ Branch 5 not taken.
 211684 VEC = Y - P;
125
2/2
✓ Branch 0 taken 197404 times.
✓ Branch 1 taken 52251 times.
 249655 } else if (t >= 1) {
126
2/4
✓ Branch 1 taken 197404 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 197404 times.
✗ Branch 5 not taken.
 197404 X = P + A;
127
2/4
✓ Branch 1 taken 197404 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 197404 times.
✗ Branch 5 not taken.
 197404 VEC = Y - X;
128 } else {
129
3/6
✓ Branch 1 taken 52251 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 52251 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 52251 times.
✗ Branch 8 not taken.
 52251 X = P + A * t;
130
2/4
✓ Branch 1 taken 52251 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 52251 times.
✗ Branch 5 not taken.
 52251 T = Y - P;
131
1/2
✓ Branch 1 taken 52251 times.
✗ Branch 2 not taken.
 52251 TMP = T.cross(A);
132
1/2
✓ Branch 1 taken 52251 times.
✗ Branch 2 not taken.
 52251 VEC = A.cross(TMP);
133 }
134 } else {
135
3/6
✓ Branch 1 taken 59463 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 59463 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 59463 times.
✗ Branch 8 not taken.
 59463 Y = Q + B * u;
136
137
5/6
✓ Branch 0 taken 45255 times.
✓ Branch 1 taken 14208 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 45255 times.
✓ Branch 5 taken 14208 times.
✓ Branch 6 taken 45255 times.
 59463 if ((t <= 0) || std::isnan(t)) {
138
1/2
✓ Branch 1 taken 14208 times.
✗ Branch 2 not taken.
 14208 X = P;
139
1/2
✓ Branch 1 taken 14208 times.
✗ Branch 2 not taken.
 14208 TMP = T.cross(B);
140
1/2
✓ Branch 1 taken 14208 times.
✗ Branch 2 not taken.
 14208 VEC = B.cross(TMP);
141
2/2
✓ Branch 0 taken 28834 times.
✓ Branch 1 taken 16421 times.
 45255 } else if (t >= 1) {
142
2/4
✓ Branch 1 taken 28834 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 28834 times.
✗ Branch 5 not taken.
 28834 X = P + A;
143
2/4
✓ Branch 1 taken 28834 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 28834 times.
✗ Branch 5 not taken.
 28834 T = Q - X;
144
1/2
✓ Branch 1 taken 28834 times.
✗ Branch 2 not taken.
 28834 TMP = T.cross(B);
145
1/2
✓ Branch 1 taken 28834 times.
✗ Branch 2 not taken.
 28834 VEC = B.cross(TMP);
146 } else {
147
3/6
✓ Branch 1 taken 16421 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16421 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 16421 times.
✗ Branch 8 not taken.
 16421 X = P + A * t;
148
1/2
✓ Branch 1 taken 16421 times.
✗ Branch 2 not taken.
 16421 VEC = A.cross(B);
149
3/4
✓ Branch 1 taken 16421 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 8141 times.
✓ Branch 4 taken 8280 times.
 16421 if (VEC.dot(T) < 0) {
150
2/4
✓ Branch 1 taken 8141 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8141 times.
✗ Branch 5 not taken.
 8141 VEC = VEC * (-1);
151 }
152 }
153 }
154 863428 }
155
156 340088 Scalar TriangleDistance::sqrTriDistance(const Vec3s S[3], const Vec3s T[3],
157 Vec3s& P, Vec3s& Q) {
158 // Compute vectors along the 6 sides
159
160
3/4
✓ Branch 1 taken 1020264 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1020264 times.
✓ Branch 4 taken 340088 times.
 1360352 Vec3s Sv[3];
161
3/4
✓ Branch 1 taken 1020264 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1020264 times.
✓ Branch 4 taken 340088 times.
 1360352 Vec3s Tv[3];
162
1/2
✓ Branch 1 taken 340088 times.
✗ Branch 2 not taken.
 340088 Vec3s VEC;
163
164
2/4
✓ Branch 1 taken 340088 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 340088 times.
✗ Branch 5 not taken.
 340088 Sv[0] = S[1] - S[0];
165
2/4
✓ Branch 1 taken 340088 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 340088 times.
✗ Branch 5 not taken.
 340088 Sv[1] = S[2] - S[1];
166
2/4
✓ Branch 1 taken 340088 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 340088 times.
✗ Branch 5 not taken.
 340088 Sv[2] = S[0] - S[2];
167
168
2/4
✓ Branch 1 taken 340088 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 340088 times.
✗ Branch 5 not taken.
 340088 Tv[0] = T[1] - T[0];
169
2/4
✓ Branch 1 taken 340088 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 340088 times.
✗ Branch 5 not taken.
 340088 Tv[1] = T[2] - T[1];
170
2/4
✓ Branch 1 taken 340088 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 340088 times.
✗ Branch 5 not taken.
 340088 Tv[2] = T[0] - T[2];
171
172 // For each edge pair, the vector connecting the closest points
173 // of the edges defines a slab (parallel planes at head and tail
174 // enclose the slab). If we can show that the off-edge vertex of
175 // each triangle is outside of the slab, then the closest points
176 // of the edges are the closest points for the triangles.
177 // Even if these tests fail, it may be helpful to know the closest
178 // points found, and whether the triangles were shown disjoint
179
180
4/8
✓ Branch 1 taken 340088 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 340088 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 340088 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 340088 times.
✗ Branch 11 not taken.
 340088 Vec3s V, Z, minP, minQ;
181 Scalar mindd;
182 340088 int shown_disjoint = 0;
183
184
2/4
✓ Branch 1 taken 340088 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 340088 times.
✗ Branch 5 not taken.
 340088 mindd = (S[0] - T[0]).squaredNorm() + 1; // Set first minimum safely high
185
186
2/2
✓ Branch 0 taken 473004 times.
✓ Branch 1 taken 3415 times.
 476419 for (int i = 0; i < 3; ++i) {
187
2/2
✓ Branch 0 taken 863428 times.
✓ Branch 1 taken 136331 times.
 999759 for (int j = 0; j < 3; ++j) {
188 // Find closest points on edges i & j, plus the
189 // vector (and distance squared) between these points
190
1/2
✓ Branch 1 taken 863428 times.
✗ Branch 2 not taken.
 863428 segPoints(S[i], Sv[i], T[j], Tv[j], VEC, P, Q);
191
192
2/4
✓ Branch 1 taken 863428 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 863428 times.
✗ Branch 5 not taken.
 863428 V = Q - P;
193
1/2
✓ Branch 1 taken 863428 times.
✗ Branch 2 not taken.
 863428 Scalar dd = V.dot(V);
194
195 // Verify this closest point pair only if the distance
196 // squared is less than the minimum found thus far.
197
198
2/2
✓ Branch 0 taken 716285 times.
✓ Branch 1 taken 147143 times.
 863428 if (dd <= mindd) {
199
1/2
✓ Branch 1 taken 716285 times.
✗ Branch 2 not taken.
 716285 minP = P;
200
1/2
✓ Branch 1 taken 716285 times.
✗ Branch 2 not taken.
 716285 minQ = Q;
201 716285 mindd = dd;
202
203
2/4
✓ Branch 1 taken 716285 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 716285 times.
✗ Branch 5 not taken.
 716285 Z = S[(i + 2) % 3] - P;
204
1/2
✓ Branch 1 taken 716285 times.
✗ Branch 2 not taken.
 716285 Scalar a = Z.dot(VEC);
205
2/4
✓ Branch 1 taken 716285 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 716285 times.
✗ Branch 5 not taken.
 716285 Z = T[(j + 2) % 3] - Q;
206
1/2
✓ Branch 1 taken 716285 times.
✗ Branch 2 not taken.
 716285 Scalar b = Z.dot(VEC);
207
208
4/4
✓ Branch 0 taken 436584 times.
✓ Branch 1 taken 279701 times.
✓ Branch 2 taken 336673 times.
✓ Branch 3 taken 99911 times.
 716285 if ((a <= 0) && (b >= 0)) return dd;
209
210
1/2
✓ Branch 1 taken 379612 times.
✗ Branch 2 not taken.
 379612 Scalar p = V.dot(VEC);
211
212
2/2
✓ Branch 0 taken 99775 times.
✓ Branch 1 taken 279837 times.
 379612 if (a < 0) a = 0;
213
2/2
✓ Branch 0 taken 239538 times.
✓ Branch 1 taken 140074 times.
 379612 if (b > 0) b = 0;
214
2/2
✓ Branch 0 taken 349955 times.
✓ Branch 1 taken 29657 times.
 379612 if ((p - a + b) > 0) shown_disjoint = 1;
215 }
216 }
217 }
218
219 // No edge pairs contained the closest points.
220 // either:
221 // 1. one of the closest points is a vertex, and the
222 // other point is interior to a face.
223 // 2. the triangles are overlapping.
224 // 3. an edge of one triangle is parallel to the other's face. If
225 // cases 1 and 2 are not true, then the closest points from the 9
226 // edge pairs checks above can be taken as closest points for the
227 // triangles.
228 // 4. possibly, the triangles were degenerate. When the
229 // triangle points are nearly colinear or coincident, one
230 // of above tests might fail even though the edges tested
231 // contain the closest points.
232
233 // First check for case 1
234
235
1/2
✓ Branch 1 taken 3415 times.
✗ Branch 2 not taken.
 3415 Vec3s Sn;
236 Scalar Snl;
237
238
1/2
✓ Branch 1 taken 3415 times.
✗ Branch 2 not taken.
 3415 Sn = Sv[0].cross(Sv[1]); // Compute normal to S triangle
239
1/2
✓ Branch 1 taken 3415 times.
✗ Branch 2 not taken.
 3415 Snl = Sn.dot(Sn); // Compute square of length of normal
240
241 // If cross product is long enough,
242
243
1/2
✓ Branch 0 taken 3415 times.
✗ Branch 1 not taken.
 3415 if (Snl > 1e-15) {
244 // Get projection lengths of T points
245
246
1/2
✓ Branch 1 taken 3415 times.
✗ Branch 2 not taken.
 3415 Vec3s Tp;
247
248
2/4
✓ Branch 1 taken 3415 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3415 times.
✗ Branch 5 not taken.
 3415 V = S[0] - T[0];
249
2/4
✓ Branch 1 taken 3415 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3415 times.
✗ Branch 5 not taken.
 3415 Tp[0] = V.dot(Sn);
250
251
2/4
✓ Branch 1 taken 3415 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3415 times.
✗ Branch 5 not taken.
 3415 V = S[0] - T[1];
252
2/4
✓ Branch 1 taken 3415 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3415 times.
✗ Branch 5 not taken.
 3415 Tp[1] = V.dot(Sn);
253
254
2/4
✓ Branch 1 taken 3415 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3415 times.
✗ Branch 5 not taken.
 3415 V = S[0] - T[2];
255
2/4
✓ Branch 1 taken 3415 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3415 times.
✗ Branch 5 not taken.
 3415 Tp[2] = V.dot(Sn);
256
257 // If Sn is a separating direction,
258 // find point with smallest projection
259
260 3415 int point = -1;
261
11/14
✓ Branch 1 taken 3415 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1275 times.
✓ Branch 4 taken 2140 times.
✓ Branch 6 taken 1275 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 678 times.
✓ Branch 9 taken 597 times.
✓ Branch 11 taken 678 times.
✗ Branch 12 not taken.
✓ Branch 13 taken 451 times.
✓ Branch 14 taken 227 times.
✓ Branch 15 taken 451 times.
✓ Branch 16 taken 2964 times.
 3415 if ((Tp[0] > 0) && (Tp[1] > 0) && (Tp[2] > 0)) {
262
4/6
✓ Branch 1 taken 451 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 451 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 272 times.
✓ Branch 7 taken 179 times.
 451 if (Tp[0] < Tp[1])
263 272 point = 0;
264 else
265 179 point = 1;
266
4/6
✓ Branch 1 taken 451 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 451 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 153 times.
✓ Branch 7 taken 298 times.
 451 if (Tp[2] < Tp[point]) point = 2;
267
11/14
✓ Branch 1 taken 2964 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2140 times.
✓ Branch 4 taken 824 times.
✓ Branch 6 taken 2140 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 1616 times.
✓ Branch 9 taken 524 times.
✓ Branch 11 taken 1616 times.
✗ Branch 12 not taken.
✓ Branch 13 taken 1361 times.
✓ Branch 14 taken 255 times.
✓ Branch 15 taken 1361 times.
✓ Branch 16 taken 1603 times.
 2964 } else if ((Tp[0] < 0) && (Tp[1] < 0) && (Tp[2] < 0)) {
268
4/6
✓ Branch 1 taken 1361 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1361 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 813 times.
✓ Branch 7 taken 548 times.
 1361 if (Tp[0] > Tp[1])
269 813 point = 0;
270 else
271 548 point = 1;
272
4/6
✓ Branch 1 taken 1361 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1361 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 515 times.
✓ Branch 7 taken 846 times.
 1361 if (Tp[2] > Tp[point]) point = 2;
273 }
274
275 // If Sn is a separating direction,
276
277
2/2
✓ Branch 0 taken 1812 times.
✓ Branch 1 taken 1603 times.
 3415 if (point >= 0) {
278 1812 shown_disjoint = 1;
279
280 // Test whether the point found, when projected onto the
281 // other triangle, lies within the face.
282
283
2/4
✓ Branch 1 taken 1812 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1812 times.
✗ Branch 5 not taken.
 1812 V = T[point] - S[0];
284
1/2
✓ Branch 1 taken 1812 times.
✗ Branch 2 not taken.
 1812 Z = Sn.cross(Sv[0]);
285
3/4
✓ Branch 1 taken 1812 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1727 times.
✓ Branch 4 taken 85 times.
 1812 if (V.dot(Z) > 0) {
286
2/4
✓ Branch 1 taken 1727 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1727 times.
✗ Branch 5 not taken.
 1727 V = T[point] - S[1];
287
1/2
✓ Branch 1 taken 1727 times.
✗ Branch 2 not taken.
 1727 Z = Sn.cross(Sv[1]);
288
3/4
✓ Branch 1 taken 1727 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1627 times.
✓ Branch 4 taken 100 times.
 1727 if (V.dot(Z) > 0) {
289
2/4
✓ Branch 1 taken 1627 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1627 times.
✗ Branch 5 not taken.
 1627 V = T[point] - S[2];
290
1/2
✓ Branch 1 taken 1627 times.
✗ Branch 2 not taken.
 1627 Z = Sn.cross(Sv[2]);
291
3/4
✓ Branch 1 taken 1627 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1589 times.
✓ Branch 4 taken 38 times.
 1627 if (V.dot(Z) > 0) {
292 // T[point] passed the test - it's a closest point for
293 // the T triangle; the other point is on the face of S
294
4/8
✓ Branch 1 taken 1589 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1589 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1589 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1589 times.
✗ Branch 11 not taken.
 1589 P = T[point] + Sn * (Tp[point] / Snl);
295
1/2
✓ Branch 1 taken 1589 times.
✗ Branch 2 not taken.
 1589 Q = T[point];
296
2/4
✓ Branch 1 taken 1589 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1589 times.
✗ Branch 5 not taken.
 1589 return (P - Q).squaredNorm();
297 }
298 }
299 }
300 }
301 }
302
303
1/2
✓ Branch 1 taken 1826 times.
✗ Branch 2 not taken.
 1826 Vec3s Tn;
304 Scalar Tnl;
305
306
1/2
✓ Branch 1 taken 1826 times.
✗ Branch 2 not taken.
 1826 Tn = Tv[0].cross(Tv[1]);
307
1/2
✓ Branch 1 taken 1826 times.
✗ Branch 2 not taken.
 1826 Tnl = Tn.dot(Tn);
308
309
1/2
✓ Branch 0 taken 1826 times.
✗ Branch 1 not taken.
 1826 if (Tnl > 1e-15) {
310
1/2
✓ Branch 1 taken 1826 times.
✗ Branch 2 not taken.
 1826 Vec3s Sp;
311
312
2/4
✓ Branch 1 taken 1826 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1826 times.
✗ Branch 5 not taken.
 1826 V = T[0] - S[0];
313
2/4
✓ Branch 1 taken 1826 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1826 times.
✗ Branch 5 not taken.
 1826 Sp[0] = V.dot(Tn);
314
315
2/4
✓ Branch 1 taken 1826 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1826 times.
✗ Branch 5 not taken.
 1826 V = T[0] - S[1];
316
2/4
✓ Branch 1 taken 1826 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1826 times.
✗ Branch 5 not taken.
 1826 Sp[1] = V.dot(Tn);
317
318
2/4
✓ Branch 1 taken 1826 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1826 times.
✗ Branch 5 not taken.
 1826 V = T[0] - S[2];
319
2/4
✓ Branch 1 taken 1826 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1826 times.
✗ Branch 5 not taken.
 1826 Sp[2] = V.dot(Tn);
320
321 1826 int point = -1;
322
11/14
✓ Branch 1 taken 1826 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 766 times.
✓ Branch 4 taken 1060 times.
✓ Branch 6 taken 766 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 162 times.
✓ Branch 9 taken 604 times.
✓ Branch 11 taken 162 times.
✗ Branch 12 not taken.
✓ Branch 13 taken 46 times.
✓ Branch 14 taken 116 times.
✓ Branch 15 taken 46 times.
✓ Branch 16 taken 1780 times.
 1826 if ((Sp[0] > 0) && (Sp[1] > 0) && (Sp[2] > 0)) {
323
4/6
✓ Branch 1 taken 46 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 46 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 14 times.
✓ Branch 7 taken 32 times.
 46 if (Sp[0] < Sp[1])
324 14 point = 0;
325 else
326 32 point = 1;
327
4/6
✓ Branch 1 taken 46 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 46 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 5 times.
✓ Branch 7 taken 41 times.
 46 if (Sp[2] < Sp[point]) point = 2;
328
11/14
✓ Branch 1 taken 1780 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1060 times.
✓ Branch 4 taken 720 times.
✓ Branch 6 taken 1060 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 409 times.
✓ Branch 9 taken 651 times.
✓ Branch 11 taken 409 times.
✗ Branch 12 not taken.
✓ Branch 13 taken 294 times.
✓ Branch 14 taken 115 times.
✓ Branch 15 taken 294 times.
✓ Branch 16 taken 1486 times.
 1780 } else if ((Sp[0] < 0) && (Sp[1] < 0) && (Sp[2] < 0)) {
329
4/6
✓ Branch 1 taken 294 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 294 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 137 times.
✓ Branch 7 taken 157 times.
 294 if (Sp[0] > Sp[1])
330 137 point = 0;
331 else
332 157 point = 1;
333
4/6
✓ Branch 1 taken 294 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 294 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 104 times.
✓ Branch 7 taken 190 times.
 294 if (Sp[2] > Sp[point]) point = 2;
334 }
335
336
2/2
✓ Branch 0 taken 340 times.
✓ Branch 1 taken 1486 times.
 1826 if (point >= 0) {
337 340 shown_disjoint = 1;
338
339
2/4
✓ Branch 1 taken 340 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 340 times.
✗ Branch 5 not taken.
 340 V = S[point] - T[0];
340
1/2
✓ Branch 1 taken 340 times.
✗ Branch 2 not taken.
 340 Z = Tn.cross(Tv[0]);
341
2/4
✓ Branch 1 taken 340 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 340 times.
✗ Branch 4 not taken.
 340 if (V.dot(Z) > 0) {
342
2/4
✓ Branch 1 taken 340 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 340 times.
✗ Branch 5 not taken.
 340 V = S[point] - T[1];
343
1/2
✓ Branch 1 taken 340 times.
✗ Branch 2 not taken.
 340 Z = Tn.cross(Tv[1]);
344
2/4
✓ Branch 1 taken 340 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 340 times.
✗ Branch 4 not taken.
 340 if (V.dot(Z) > 0) {
345
2/4
✓ Branch 1 taken 340 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 340 times.
✗ Branch 5 not taken.
 340 V = S[point] - T[2];
346
1/2
✓ Branch 1 taken 340 times.
✗ Branch 2 not taken.
 340 Z = Tn.cross(Tv[2]);
347
2/4
✓ Branch 1 taken 340 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 340 times.
✗ Branch 4 not taken.
 340 if (V.dot(Z) > 0) {
348
1/2
✓ Branch 1 taken 340 times.
✗ Branch 2 not taken.
 340 P = S[point];
349
4/8
✓ Branch 1 taken 340 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 340 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 340 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 340 times.
✗ Branch 11 not taken.
 340 Q = S[point] + Tn * (Sp[point] / Tnl);
350
2/4
✓ Branch 1 taken 340 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 340 times.
✗ Branch 5 not taken.
 340 return (P - Q).squaredNorm();
351 }
352 }
353 }
354 }
355 }
356
357 // Case 1 can't be shown.
358 // If one of these tests showed the triangles disjoint,
359 // we assume case 3 or 4, otherwise we conclude case 2,
360 // that the triangles overlap.
361
362
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1486 times.
 1486 if (shown_disjoint) {
363 ✗ P = minP;
364 ✗ Q = minQ;
365 ✗ return mindd;
366 } else
367 1486 return 0;
368 }
369
370 340088 Scalar TriangleDistance::sqrTriDistance(const Vec3s& S1, const Vec3s& S2,
371 const Vec3s& S3, const Vec3s& T1,
372 const Vec3s& T2, const Vec3s& T3,
373 Vec3s& P, Vec3s& Q) {
374
3/4
✓ Branch 1 taken 1020264 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1020264 times.
✓ Branch 4 taken 340088 times.
 1360352 Vec3s S[3];
375
3/4
✓ Branch 1 taken 1020264 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1020264 times.
✓ Branch 4 taken 340088 times.
 1360352 Vec3s T[3];
376
1/2
✓ Branch 1 taken 340088 times.
✗ Branch 2 not taken.
 340088 S[0] = S1;
377
1/2
✓ Branch 1 taken 340088 times.
✗ Branch 2 not taken.
 340088 S[1] = S2;
378
1/2
✓ Branch 1 taken 340088 times.
✗ Branch 2 not taken.
 340088 S[2] = S3;
379
1/2
✓ Branch 1 taken 340088 times.
✗ Branch 2 not taken.
 340088 T[0] = T1;
380
1/2
✓ Branch 1 taken 340088 times.
✗ Branch 2 not taken.
 340088 T[1] = T2;
381
1/2
✓ Branch 1 taken 340088 times.
✗ Branch 2 not taken.
 340088 T[2] = T3;
382
383
1/2
✓ Branch 1 taken 340088 times.
✗ Branch 2 not taken.
 680176 return sqrTriDistance(S, T, P, Q);
384 }
385
386 ✗ Scalar TriangleDistance::sqrTriDistance(const Vec3s S[3], const Vec3s T[3],
387 const Matrix3s& R, const Vec3s& Tl,
388 Vec3s& P, Vec3s& Q) {
389 ✗ Vec3s T_transformed[3];
390 ✗ T_transformed[0] = R * T[0] + Tl;
391 ✗ T_transformed[1] = R * T[1] + Tl;
392 ✗ T_transformed[2] = R * T[2] + Tl;
393
394 ✗ return sqrTriDistance(S, T_transformed, P, Q);
395 }
396
397 ✗ Scalar TriangleDistance::sqrTriDistance(const Vec3s S[3], const Vec3s T[3],
398 const Transform3s& tf, Vec3s& P,
399 Vec3s& Q) {
400 ✗ Vec3s T_transformed[3];
401 ✗ T_transformed[0] = tf.transform(T[0]);
402 ✗ T_transformed[1] = tf.transform(T[1]);
403 ✗ T_transformed[2] = tf.transform(T[2]);
404
405 ✗ return sqrTriDistance(S, T_transformed, P, Q);
406 }
407
408 336487 Scalar TriangleDistance::sqrTriDistance(const Vec3s& S1, const Vec3s& S2,
409 const Vec3s& S3, const Vec3s& T1,
410 const Vec3s& T2, const Vec3s& T3,
411 const Matrix3s& R, const Vec3s& Tl,
412 Vec3s& P, Vec3s& Q) {
413
3/6
✓ Branch 1 taken 336487 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 336487 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 336487 times.
✗ Branch 8 not taken.
 336487 Vec3s T1_transformed = R * T1 + Tl;
414
3/6
✓ Branch 1 taken 336487 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 336487 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 336487 times.
✗ Branch 8 not taken.
 336487 Vec3s T2_transformed = R * T2 + Tl;
415
3/6
✓ Branch 1 taken 336487 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 336487 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 336487 times.
✗ Branch 8 not taken.
 336487 Vec3s T3_transformed = R * T3 + Tl;
416
1/2
✓ Branch 1 taken 336487 times.
✗ Branch 2 not taken.
 336487 return sqrTriDistance(S1, S2, S3, T1_transformed, T2_transformed,
417 672974 T3_transformed, P, Q);
418 }
419
420 ✗ Scalar TriangleDistance::sqrTriDistance(const Vec3s& S1, const Vec3s& S2,
421 const Vec3s& S3, const Vec3s& T1,
422 const Vec3s& T2, const Vec3s& T3,
423 const Transform3s& tf, Vec3s& P,
424 Vec3s& Q) {
425 ✗ Vec3s T1_transformed = tf.transform(T1);
426 ✗ Vec3s T2_transformed = tf.transform(T2);
427 ✗ Vec3s T3_transformed = tf.transform(T3);
428 ✗ return sqrTriDistance(S1, S2, S3, T1_transformed, T2_transformed,
429 ✗ T3_transformed, P, Q);
430 }
431
432 } // namespace coal
433