GCC Code Coverage Report

Directory: ./
File: src/intersect.cpp
Date: 2025-04-13 14:25:19
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 860819 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 860819 times.
✗ Branch 2 not taken.
 860819 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 860819 times.
✗ Branch 2 not taken.
 860819 Vec3s TMP;
66
67
2/4
✓ Branch 1 taken 860819 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 860819 times.
✗ Branch 5 not taken.
 860819 T = Q - P;
68
1/2
✓ Branch 1 taken 860819 times.
✗ Branch 2 not taken.
 860819 A_dot_A = A.dot(A);
69
1/2
✓ Branch 1 taken 860819 times.
✗ Branch 2 not taken.
 860819 B_dot_B = B.dot(B);
70
1/2
✓ Branch 1 taken 860819 times.
✗ Branch 2 not taken.
 860819 A_dot_B = A.dot(B);
71
1/2
✓ Branch 1 taken 860819 times.
✗ Branch 2 not taken.
 860819 A_dot_T = A.dot(T);
72
1/2
✓ Branch 1 taken 860819 times.
✗ Branch 2 not taken.
 860819 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 860819 Scalar denom = A_dot_A * B_dot_B - A_dot_B * A_dot_B;
83
84 860819 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 513163 times.
✓ Branch 1 taken 347656 times.
✓ Branch 3 taken 5462 times.
✓ Branch 4 taken 507701 times.
✓ Branch 5 taken 353118 times.
✓ Branch 6 taken 507701 times.
 860819 if ((t < 0) || std::isnan(t))
89 353118 t = 0;
90
2/2
✓ Branch 0 taken 376065 times.
✓ Branch 1 taken 131636 times.
 507701 else if (t > 1)
91 376065 t = 1;
92
93 // find u for point on ray Q,B closest to point at t
94
95 860819 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 519176 times.
✓ Branch 1 taken 341643 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 519176 times.
✓ Branch 5 taken 341643 times.
✓ Branch 6 taken 519176 times.
 860819 if ((u <= 0) || std::isnan(u)) {
102
1/2
✓ Branch 1 taken 341643 times.
✗ Branch 2 not taken.
 341643 Y = Q;
103
104 341643 t = A_dot_T / A_dot_A;
105
106
5/6
✓ Branch 0 taken 234057 times.
✓ Branch 1 taken 107586 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 234057 times.
✓ Branch 5 taken 107586 times.
✓ Branch 6 taken 234057 times.
 341643 if ((t <= 0) || std::isnan(t)) {
107
1/2
✓ Branch 1 taken 107586 times.
✗ Branch 2 not taken.
 107586 X = P;
108
2/4
✓ Branch 1 taken 107586 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 107586 times.
✗ Branch 5 not taken.
 107586 VEC = Q - P;
109
2/2
✓ Branch 0 taken 195641 times.
✓ Branch 1 taken 38416 times.
 234057 } else if (t >= 1) {
110
2/4
✓ Branch 1 taken 195641 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 195641 times.
✗ Branch 5 not taken.
 195641 X = P + A;
111
2/4
✓ Branch 1 taken 195641 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 195641 times.
✗ Branch 5 not taken.
 195641 VEC = Q - X;
112 } else {
113
3/6
✓ Branch 1 taken 38416 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 38416 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 38416 times.
✗ Branch 8 not taken.
 38416 X = P + A * t;
114
1/2
✓ Branch 1 taken 38416 times.
✗ Branch 2 not taken.
 38416 TMP = T.cross(A);
115
1/2
✓ Branch 1 taken 38416 times.
✗ Branch 2 not taken.
 38416 VEC = A.cross(TMP);
116 }
117
2/2
✓ Branch 0 taken 459875 times.
✓ Branch 1 taken 59301 times.
 519176 } else if (u >= 1) {
118
2/4
✓ Branch 1 taken 459875 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 459875 times.
✗ Branch 5 not taken.
 459875 Y = Q + B;
119
120 459875 t = (A_dot_B + A_dot_T) / A_dot_A;
121
122
5/6
✓ Branch 0 taken 248647 times.
✓ Branch 1 taken 211228 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 248647 times.
✓ Branch 5 taken 211228 times.
✓ Branch 6 taken 248647 times.
 459875 if ((t <= 0) || std::isnan(t)) {
123
1/2
✓ Branch 1 taken 211228 times.
✗ Branch 2 not taken.
 211228 X = P;
124
2/4
✓ Branch 1 taken 211228 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 211228 times.
✗ Branch 5 not taken.
 211228 VEC = Y - P;
125
2/2
✓ Branch 0 taken 196474 times.
✓ Branch 1 taken 52173 times.
 248647 } else if (t >= 1) {
126
2/4
✓ Branch 1 taken 196474 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 196474 times.
✗ Branch 5 not taken.
 196474 X = P + A;
127
2/4
✓ Branch 1 taken 196474 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 196474 times.
✗ Branch 5 not taken.
 196474 VEC = Y - X;
128 } else {
129
3/6
✓ Branch 1 taken 52173 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 52173 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 52173 times.
✗ Branch 8 not taken.
 52173 X = P + A * t;
130
2/4
✓ Branch 1 taken 52173 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 52173 times.
✗ Branch 5 not taken.
 52173 T = Y - P;
131
1/2
✓ Branch 1 taken 52173 times.
✗ Branch 2 not taken.
 52173 TMP = T.cross(A);
132
1/2
✓ Branch 1 taken 52173 times.
✗ Branch 2 not taken.
 52173 VEC = A.cross(TMP);
133 }
134 } else {
135
3/6
✓ Branch 1 taken 59301 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 59301 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 59301 times.
✗ Branch 8 not taken.
 59301 Y = Q + B * u;
136
137
5/6
✓ Branch 0 taken 45154 times.
✓ Branch 1 taken 14147 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 45154 times.
✓ Branch 5 taken 14147 times.
✓ Branch 6 taken 45154 times.
 59301 if ((t <= 0) || std::isnan(t)) {
138
1/2
✓ Branch 1 taken 14147 times.
✗ Branch 2 not taken.
 14147 X = P;
139
1/2
✓ Branch 1 taken 14147 times.
✗ Branch 2 not taken.
 14147 TMP = T.cross(B);
140
1/2
✓ Branch 1 taken 14147 times.
✗ Branch 2 not taken.
 14147 VEC = B.cross(TMP);
141
2/2
✓ Branch 0 taken 28753 times.
✓ Branch 1 taken 16401 times.
 45154 } else if (t >= 1) {
142
2/4
✓ Branch 1 taken 28753 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 28753 times.
✗ Branch 5 not taken.
 28753 X = P + A;
143
2/4
✓ Branch 1 taken 28753 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 28753 times.
✗ Branch 5 not taken.
 28753 T = Q - X;
144
1/2
✓ Branch 1 taken 28753 times.
✗ Branch 2 not taken.
 28753 TMP = T.cross(B);
145
1/2
✓ Branch 1 taken 28753 times.
✗ Branch 2 not taken.
 28753 VEC = B.cross(TMP);
146 } else {
147
3/6
✓ Branch 1 taken 16401 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16401 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 16401 times.
✗ Branch 8 not taken.
 16401 X = P + A * t;
148
1/2
✓ Branch 1 taken 16401 times.
✗ Branch 2 not taken.
 16401 VEC = A.cross(B);
149
3/4
✓ Branch 1 taken 16401 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 8133 times.
✓ Branch 4 taken 8268 times.
 16401 if (VEC.dot(T) < 0) {
150
2/4
✓ Branch 1 taken 8133 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8133 times.
✗ Branch 5 not taken.
 8133 VEC = VEC * (-1);
151 }
152 }
153 }
154 860819 }
155
156 339012 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 1017036 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1017036 times.
✓ Branch 4 taken 339012 times.
 1356048 Vec3s Sv[3];
161
3/4
✓ Branch 1 taken 1017036 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1017036 times.
✓ Branch 4 taken 339012 times.
 1356048 Vec3s Tv[3];
162
1/2
✓ Branch 1 taken 339012 times.
✗ Branch 2 not taken.
 339012 Vec3s VEC;
163
164
2/4
✓ Branch 1 taken 339012 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 339012 times.
✗ Branch 5 not taken.
 339012 Sv[0] = S[1] - S[0];
165
2/4
✓ Branch 1 taken 339012 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 339012 times.
✗ Branch 5 not taken.
 339012 Sv[1] = S[2] - S[1];
166
2/4
✓ Branch 1 taken 339012 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 339012 times.
✗ Branch 5 not taken.
 339012 Sv[2] = S[0] - S[2];
167
168
2/4
✓ Branch 1 taken 339012 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 339012 times.
✗ Branch 5 not taken.
 339012 Tv[0] = T[1] - T[0];
169
2/4
✓ Branch 1 taken 339012 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 339012 times.
✗ Branch 5 not taken.
 339012 Tv[1] = T[2] - T[1];
170
2/4
✓ Branch 1 taken 339012 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 339012 times.
✗ Branch 5 not taken.
 339012 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 339012 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 339012 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 339012 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 339012 times.
✗ Branch 11 not taken.
 339012 Vec3s V, Z, minP, minQ;
181 Scalar mindd;
182 339012 int shown_disjoint = 0;
183
184
2/4
✓ Branch 1 taken 339012 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 339012 times.
✗ Branch 5 not taken.
 339012 mindd = (S[0] - T[0]).squaredNorm() + 1; // Set first minimum safely high
185
186
2/2
✓ Branch 0 taken 471556 times.
✓ Branch 1 taken 3408 times.
 474964 for (int i = 0; i < 3; ++i) {
187
2/2
✓ Branch 0 taken 860819 times.
✓ Branch 1 taken 135952 times.
 996771 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 860819 times.
✗ Branch 2 not taken.
 860819 segPoints(S[i], Sv[i], T[j], Tv[j], VEC, P, Q);
191
192
2/4
✓ Branch 1 taken 860819 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 860819 times.
✗ Branch 5 not taken.
 860819 V = Q - P;
193
1/2
✓ Branch 1 taken 860819 times.
✗ Branch 2 not taken.
 860819 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 714014 times.
✓ Branch 1 taken 146805 times.
 860819 if (dd <= mindd) {
199
1/2
✓ Branch 1 taken 714014 times.
✗ Branch 2 not taken.
 714014 minP = P;
200
1/2
✓ Branch 1 taken 714014 times.
✗ Branch 2 not taken.
 714014 minQ = Q;
201 714014 mindd = dd;
202
203
2/4
✓ Branch 1 taken 714014 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 714014 times.
✗ Branch 5 not taken.
 714014 Z = S[(i + 2) % 3] - P;
204
1/2
✓ Branch 1 taken 714014 times.
✗ Branch 2 not taken.
 714014 Scalar a = Z.dot(VEC);
205
2/4
✓ Branch 1 taken 714014 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 714014 times.
✗ Branch 5 not taken.
 714014 Z = T[(j + 2) % 3] - Q;
206
1/2
✓ Branch 1 taken 714014 times.
✗ Branch 2 not taken.
 714014 Scalar b = Z.dot(VEC);
207
208
4/4
✓ Branch 0 taken 435120 times.
✓ Branch 1 taken 278894 times.
✓ Branch 2 taken 335604 times.
✓ Branch 3 taken 99516 times.
 714014 if ((a <= 0) && (b >= 0)) return dd;
209
210
1/2
✓ Branch 1 taken 378410 times.
✗ Branch 2 not taken.
 378410 Scalar p = V.dot(VEC);
211
212
2/2
✓ Branch 0 taken 99380 times.
✓ Branch 1 taken 279030 times.
 378410 if (a < 0) a = 0;
213
2/2
✓ Branch 0 taken 238868 times.
✓ Branch 1 taken 139542 times.
 378410 if (b > 0) b = 0;
214
2/2
✓ Branch 0 taken 348753 times.
✓ Branch 1 taken 29657 times.
 378410 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 3408 times.
✗ Branch 2 not taken.
 3408 Vec3s Sn;
236 Scalar Snl;
237
238
1/2
✓ Branch 1 taken 3408 times.
✗ Branch 2 not taken.
 3408 Sn = Sv[0].cross(Sv[1]); // Compute normal to S triangle
239
1/2
✓ Branch 1 taken 3408 times.
✗ Branch 2 not taken.
 3408 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 3408 times.
✗ Branch 1 not taken.
 3408 if (Snl > 1e-15) {
244 // Get projection lengths of T points
245
246
1/2
✓ Branch 1 taken 3408 times.
✗ Branch 2 not taken.
 3408 Vec3s Tp;
247
248
2/4
✓ Branch 1 taken 3408 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3408 times.
✗ Branch 5 not taken.
 3408 V = S[0] - T[0];
249
2/4
✓ Branch 1 taken 3408 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3408 times.
✗ Branch 5 not taken.
 3408 Tp[0] = V.dot(Sn);
250
251
2/4
✓ Branch 1 taken 3408 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3408 times.
✗ Branch 5 not taken.
 3408 V = S[0] - T[1];
252
2/4
✓ Branch 1 taken 3408 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3408 times.
✗ Branch 5 not taken.
 3408 Tp[1] = V.dot(Sn);
253
254
2/4
✓ Branch 1 taken 3408 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3408 times.
✗ Branch 5 not taken.
 3408 V = S[0] - T[2];
255
2/4
✓ Branch 1 taken 3408 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3408 times.
✗ Branch 5 not taken.
 3408 Tp[2] = V.dot(Sn);
256
257 // If Sn is a separating direction,
258 // find point with smallest projection
259
260 3408 int point = -1;
261
11/14
✓ Branch 1 taken 3408 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1275 times.
✓ Branch 4 taken 2133 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 2957 times.
 3408 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 2957 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2133 times.
✓ Branch 4 taken 824 times.
✓ Branch 6 taken 2133 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 1609 times.
✓ Branch 9 taken 524 times.
✓ Branch 11 taken 1609 times.
✗ Branch 12 not taken.
✓ Branch 13 taken 1354 times.
✓ Branch 14 taken 255 times.
✓ Branch 15 taken 1354 times.
✓ Branch 16 taken 1603 times.
 2957 } else if ((Tp[0] < 0) && (Tp[1] < 0) && (Tp[2] < 0)) {
268
4/6
✓ Branch 1 taken 1354 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1354 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 810 times.
✓ Branch 7 taken 544 times.
 1354 if (Tp[0] > Tp[1])
269 810 point = 0;
270 else
271 544 point = 1;
272
4/6
✓ Branch 1 taken 1354 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1354 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 514 times.
✓ Branch 7 taken 840 times.
 1354 if (Tp[2] > Tp[point]) point = 2;
273 }
274
275 // If Sn is a separating direction,
276
277
2/2
✓ Branch 0 taken 1805 times.
✓ Branch 1 taken 1603 times.
 3408 if (point >= 0) {
278 1805 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 1805 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1805 times.
✗ Branch 5 not taken.
 1805 V = T[point] - S[0];
284
1/2
✓ Branch 1 taken 1805 times.
✗ Branch 2 not taken.
 1805 Z = Sn.cross(Sv[0]);
285
3/4
✓ Branch 1 taken 1805 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1723 times.
✓ Branch 4 taken 82 times.
 1805 if (V.dot(Z) > 0) {
286
2/4
✓ Branch 1 taken 1723 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1723 times.
✗ Branch 5 not taken.
 1723 V = T[point] - S[1];
287
1/2
✓ Branch 1 taken 1723 times.
✗ Branch 2 not taken.
 1723 Z = Sn.cross(Sv[1]);
288
3/4
✓ Branch 1 taken 1723 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1626 times.
✓ Branch 4 taken 97 times.
 1723 if (V.dot(Z) > 0) {
289
2/4
✓ Branch 1 taken 1626 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1626 times.
✗ Branch 5 not taken.
 1626 V = T[point] - S[2];
290
1/2
✓ Branch 1 taken 1626 times.
✗ Branch 2 not taken.
 1626 Z = Sn.cross(Sv[2]);
291
3/4
✓ Branch 1 taken 1626 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1589 times.
✓ Branch 4 taken 37 times.
 1626 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 1819 times.
✗ Branch 2 not taken.
 1819 Vec3s Tn;
304 Scalar Tnl;
305
306
1/2
✓ Branch 1 taken 1819 times.
✗ Branch 2 not taken.
 1819 Tn = Tv[0].cross(Tv[1]);
307
1/2
✓ Branch 1 taken 1819 times.
✗ Branch 2 not taken.
 1819 Tnl = Tn.dot(Tn);
308
309
1/2
✓ Branch 0 taken 1819 times.
✗ Branch 1 not taken.
 1819 if (Tnl > 1e-15) {
310
1/2
✓ Branch 1 taken 1819 times.
✗ Branch 2 not taken.
 1819 Vec3s Sp;
311
312
2/4
✓ Branch 1 taken 1819 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1819 times.
✗ Branch 5 not taken.
 1819 V = T[0] - S[0];
313
2/4
✓ Branch 1 taken 1819 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1819 times.
✗ Branch 5 not taken.
 1819 Sp[0] = V.dot(Tn);
314
315
2/4
✓ Branch 1 taken 1819 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1819 times.
✗ Branch 5 not taken.
 1819 V = T[0] - S[1];
316
2/4
✓ Branch 1 taken 1819 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1819 times.
✗ Branch 5 not taken.
 1819 Sp[1] = V.dot(Tn);
317
318
2/4
✓ Branch 1 taken 1819 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1819 times.
✗ Branch 5 not taken.
 1819 V = T[0] - S[2];
319
2/4
✓ Branch 1 taken 1819 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1819 times.
✗ Branch 5 not taken.
 1819 Sp[2] = V.dot(Tn);
320
321 1819 int point = -1;
322
11/14
✓ Branch 1 taken 1819 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 766 times.
✓ Branch 4 taken 1053 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 1773 times.
 1819 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 1773 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1053 times.
✓ Branch 4 taken 720 times.
✓ Branch 6 taken 1053 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 402 times.
✓ Branch 9 taken 651 times.
✓ Branch 11 taken 402 times.
✗ Branch 12 not taken.
✓ Branch 13 taken 287 times.
✓ Branch 14 taken 115 times.
✓ Branch 15 taken 287 times.
✓ Branch 16 taken 1486 times.
 1773 } else if ((Sp[0] < 0) && (Sp[1] < 0) && (Sp[2] < 0)) {
329
4/6
✓ Branch 1 taken 287 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 287 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 133 times.
✓ Branch 7 taken 154 times.
 287 if (Sp[0] > Sp[1])
330 133 point = 0;
331 else
332 154 point = 1;
333
4/6
✓ Branch 1 taken 287 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 287 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 102 times.
✓ Branch 7 taken 185 times.
 287 if (Sp[2] > Sp[point]) point = 2;
334 }
335
336
2/2
✓ Branch 0 taken 333 times.
✓ Branch 1 taken 1486 times.
 1819 if (point >= 0) {
337 333 shown_disjoint = 1;
338
339
2/4
✓ Branch 1 taken 333 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 333 times.
✗ Branch 5 not taken.
 333 V = S[point] - T[0];
340
1/2
✓ Branch 1 taken 333 times.
✗ Branch 2 not taken.
 333 Z = Tn.cross(Tv[0]);
341
2/4
✓ Branch 1 taken 333 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 333 times.
✗ Branch 4 not taken.
 333 if (V.dot(Z) > 0) {
342
2/4
✓ Branch 1 taken 333 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 333 times.
✗ Branch 5 not taken.
 333 V = S[point] - T[1];
343
1/2
✓ Branch 1 taken 333 times.
✗ Branch 2 not taken.
 333 Z = Tn.cross(Tv[1]);
344
2/4
✓ Branch 1 taken 333 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 333 times.
✗ Branch 4 not taken.
 333 if (V.dot(Z) > 0) {
345
2/4
✓ Branch 1 taken 333 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 333 times.
✗ Branch 5 not taken.
 333 V = S[point] - T[2];
346
1/2
✓ Branch 1 taken 333 times.
✗ Branch 2 not taken.
 333 Z = Tn.cross(Tv[2]);
347
2/4
✓ Branch 1 taken 333 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 333 times.
✗ Branch 4 not taken.
 333 if (V.dot(Z) > 0) {
348
1/2
✓ Branch 1 taken 333 times.
✗ Branch 2 not taken.
 333 P = S[point];
349
4/8
✓ Branch 1 taken 333 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 333 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 333 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 333 times.
✗ Branch 11 not taken.
 333 Q = S[point] + Tn * (Sp[point] / Tnl);
350
2/4
✓ Branch 1 taken 333 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 333 times.
✗ Branch 5 not taken.
 333 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 339012 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 1017036 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1017036 times.
✓ Branch 4 taken 339012 times.
 1356048 Vec3s S[3];
375
3/4
✓ Branch 1 taken 1017036 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1017036 times.
✓ Branch 4 taken 339012 times.
 1356048 Vec3s T[3];
376
1/2
✓ Branch 1 taken 339012 times.
✗ Branch 2 not taken.
 339012 S[0] = S1;
377
1/2
✓ Branch 1 taken 339012 times.
✗ Branch 2 not taken.
 339012 S[1] = S2;
378
1/2
✓ Branch 1 taken 339012 times.
✗ Branch 2 not taken.
 339012 S[2] = S3;
379
1/2
✓ Branch 1 taken 339012 times.
✗ Branch 2 not taken.
 339012 T[0] = T1;
380
1/2
✓ Branch 1 taken 339012 times.
✗ Branch 2 not taken.
 339012 T[1] = T2;
381
1/2
✓ Branch 1 taken 339012 times.
✗ Branch 2 not taken.
 339012 T[2] = T3;
382
383
1/2
✓ Branch 1 taken 339012 times.
✗ Branch 2 not taken.
 678024 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 335411 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 335411 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 335411 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 335411 times.
✗ Branch 8 not taken.
 335411 Vec3s T1_transformed = R * T1 + Tl;
414
3/6
✓ Branch 1 taken 335411 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 335411 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 335411 times.
✗ Branch 8 not taken.
 335411 Vec3s T2_transformed = R * T2 + Tl;
415
3/6
✓ Branch 1 taken 335411 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 335411 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 335411 times.
✗ Branch 8 not taken.
 335411 Vec3s T3_transformed = R * T3 + Tl;
416
1/2
✓ Branch 1 taken 335411 times.
✗ Branch 2 not taken.
 335411 return sqrTriDistance(S1, S2, S3, T1_transformed, T2_transformed,
417 670822 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