| 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 | 860923 | 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 860923 times.
✗ Branch 2 not taken.
|
860923 | 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 860923 times.
✗ Branch 2 not taken.
|
860923 | Vec3s TMP; |
| 66 | |||
| 67 |
2/4✓ Branch 1 taken 860923 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 860923 times.
✗ Branch 5 not taken.
|
860923 | T = Q - P; |
| 68 |
1/2✓ Branch 1 taken 860923 times.
✗ Branch 2 not taken.
|
860923 | A_dot_A = A.dot(A); |
| 69 |
1/2✓ Branch 1 taken 860923 times.
✗ Branch 2 not taken.
|
860923 | B_dot_B = B.dot(B); |
| 70 |
1/2✓ Branch 1 taken 860923 times.
✗ Branch 2 not taken.
|
860923 | A_dot_B = A.dot(B); |
| 71 |
1/2✓ Branch 1 taken 860923 times.
✗ Branch 2 not taken.
|
860923 | A_dot_T = A.dot(T); |
| 72 |
1/2✓ Branch 1 taken 860923 times.
✗ Branch 2 not taken.
|
860923 | 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 | 860923 | Scalar denom = A_dot_A * B_dot_B - A_dot_B * A_dot_B; | |
| 83 | |||
| 84 | 860923 | 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 513088 times.
✓ Branch 1 taken 347835 times.
✓ Branch 3 taken 5465 times.
✓ Branch 4 taken 507623 times.
✓ Branch 5 taken 353300 times.
✓ Branch 6 taken 507623 times.
|
860923 | if ((t < 0) || std::isnan(t)) |
| 89 | 353300 | t = 0; | |
| 90 |
2/2✓ Branch 0 taken 375763 times.
✓ Branch 1 taken 131860 times.
|
507623 | else if (t > 1) |
| 91 | 375763 | t = 1; | |
| 92 | |||
| 93 | // find u for point on ray Q,B closest to point at t | ||
| 94 | |||
| 95 | 860923 | 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 519323 times.
✓ Branch 1 taken 341600 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 519323 times.
✓ Branch 5 taken 341600 times.
✓ Branch 6 taken 519323 times.
|
860923 | if ((u <= 0) || std::isnan(u)) { |
| 102 |
1/2✓ Branch 1 taken 341600 times.
✗ Branch 2 not taken.
|
341600 | Y = Q; |
| 103 | |||
| 104 | 341600 | t = A_dot_T / A_dot_A; | |
| 105 | |||
| 106 |
5/6✓ Branch 0 taken 234018 times.
✓ Branch 1 taken 107582 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 234018 times.
✓ Branch 5 taken 107582 times.
✓ Branch 6 taken 234018 times.
|
341600 | if ((t <= 0) || std::isnan(t)) { |
| 107 |
1/2✓ Branch 1 taken 107582 times.
✗ Branch 2 not taken.
|
107582 | X = P; |
| 108 |
2/4✓ Branch 1 taken 107582 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 107582 times.
✗ Branch 5 not taken.
|
107582 | VEC = Q - P; |
| 109 |
2/2✓ Branch 0 taken 195468 times.
✓ Branch 1 taken 38550 times.
|
234018 | } else if (t >= 1) { |
| 110 |
2/4✓ Branch 1 taken 195468 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 195468 times.
✗ Branch 5 not taken.
|
195468 | X = P + A; |
| 111 |
2/4✓ Branch 1 taken 195468 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 195468 times.
✗ Branch 5 not taken.
|
195468 | VEC = Q - X; |
| 112 | } else { | ||
| 113 |
3/6✓ Branch 1 taken 38550 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 38550 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 38550 times.
✗ Branch 8 not taken.
|
38550 | X = P + A * t; |
| 114 |
1/2✓ Branch 1 taken 38550 times.
✗ Branch 2 not taken.
|
38550 | TMP = T.cross(A); |
| 115 |
1/2✓ Branch 1 taken 38550 times.
✗ Branch 2 not taken.
|
38550 | VEC = A.cross(TMP); |
| 116 | } | ||
| 117 |
2/2✓ Branch 0 taken 460058 times.
✓ Branch 1 taken 59265 times.
|
519323 | } else if (u >= 1) { |
| 118 |
2/4✓ Branch 1 taken 460058 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 460058 times.
✗ Branch 5 not taken.
|
460058 | Y = Q + B; |
| 119 | |||
| 120 | 460058 | t = (A_dot_B + A_dot_T) / A_dot_A; | |
| 121 | |||
| 122 |
5/6✓ Branch 0 taken 248704 times.
✓ Branch 1 taken 211354 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 248704 times.
✓ Branch 5 taken 211354 times.
✓ Branch 6 taken 248704 times.
|
460058 | if ((t <= 0) || std::isnan(t)) { |
| 123 |
1/2✓ Branch 1 taken 211354 times.
✗ Branch 2 not taken.
|
211354 | X = P; |
| 124 |
2/4✓ Branch 1 taken 211354 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 211354 times.
✗ Branch 5 not taken.
|
211354 | VEC = Y - P; |
| 125 |
2/2✓ Branch 0 taken 196325 times.
✓ Branch 1 taken 52379 times.
|
248704 | } else if (t >= 1) { |
| 126 |
2/4✓ Branch 1 taken 196325 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 196325 times.
✗ Branch 5 not taken.
|
196325 | X = P + A; |
| 127 |
2/4✓ Branch 1 taken 196325 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 196325 times.
✗ Branch 5 not taken.
|
196325 | VEC = Y - X; |
| 128 | } else { | ||
| 129 |
3/6✓ Branch 1 taken 52379 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 52379 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 52379 times.
✗ Branch 8 not taken.
|
52379 | X = P + A * t; |
| 130 |
2/4✓ Branch 1 taken 52379 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 52379 times.
✗ Branch 5 not taken.
|
52379 | T = Y - P; |
| 131 |
1/2✓ Branch 1 taken 52379 times.
✗ Branch 2 not taken.
|
52379 | TMP = T.cross(A); |
| 132 |
1/2✓ Branch 1 taken 52379 times.
✗ Branch 2 not taken.
|
52379 | VEC = A.cross(TMP); |
| 133 | } | ||
| 134 | } else { | ||
| 135 |
3/6✓ Branch 1 taken 59265 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 59265 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 59265 times.
✗ Branch 8 not taken.
|
59265 | Y = Q + B * u; |
| 136 | |||
| 137 |
5/6✓ Branch 0 taken 45169 times.
✓ Branch 1 taken 14096 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 45169 times.
✓ Branch 5 taken 14096 times.
✓ Branch 6 taken 45169 times.
|
59265 | if ((t <= 0) || std::isnan(t)) { |
| 138 |
1/2✓ Branch 1 taken 14096 times.
✗ Branch 2 not taken.
|
14096 | X = P; |
| 139 |
1/2✓ Branch 1 taken 14096 times.
✗ Branch 2 not taken.
|
14096 | TMP = T.cross(B); |
| 140 |
1/2✓ Branch 1 taken 14096 times.
✗ Branch 2 not taken.
|
14096 | VEC = B.cross(TMP); |
| 141 |
2/2✓ Branch 0 taken 28735 times.
✓ Branch 1 taken 16434 times.
|
45169 | } else if (t >= 1) { |
| 142 |
2/4✓ Branch 1 taken 28735 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 28735 times.
✗ Branch 5 not taken.
|
28735 | X = P + A; |
| 143 |
2/4✓ Branch 1 taken 28735 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 28735 times.
✗ Branch 5 not taken.
|
28735 | T = Q - X; |
| 144 |
1/2✓ Branch 1 taken 28735 times.
✗ Branch 2 not taken.
|
28735 | TMP = T.cross(B); |
| 145 |
1/2✓ Branch 1 taken 28735 times.
✗ Branch 2 not taken.
|
28735 | VEC = B.cross(TMP); |
| 146 | } else { | ||
| 147 |
3/6✓ Branch 1 taken 16434 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16434 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 16434 times.
✗ Branch 8 not taken.
|
16434 | X = P + A * t; |
| 148 |
1/2✓ Branch 1 taken 16434 times.
✗ Branch 2 not taken.
|
16434 | VEC = A.cross(B); |
| 149 |
3/4✓ Branch 1 taken 16434 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 8148 times.
✓ Branch 4 taken 8286 times.
|
16434 | if (VEC.dot(T) < 0) { |
| 150 |
2/4✓ Branch 1 taken 8148 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8148 times.
✗ Branch 5 not taken.
|
8148 | VEC = VEC * (-1); |
| 151 | } | ||
| 152 | } | ||
| 153 | } | ||
| 154 | 860923 | } | |
| 155 | |||
| 156 | 338977 | 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 1016931 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1016931 times.
✓ Branch 4 taken 338977 times.
|
1355908 | Vec3s Sv[3]; |
| 161 |
3/4✓ Branch 1 taken 1016931 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1016931 times.
✓ Branch 4 taken 338977 times.
|
1355908 | Vec3s Tv[3]; |
| 162 |
1/2✓ Branch 1 taken 338977 times.
✗ Branch 2 not taken.
|
338977 | Vec3s VEC; |
| 163 | |||
| 164 |
2/4✓ Branch 1 taken 338977 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 338977 times.
✗ Branch 5 not taken.
|
338977 | Sv[0] = S[1] - S[0]; |
| 165 |
2/4✓ Branch 1 taken 338977 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 338977 times.
✗ Branch 5 not taken.
|
338977 | Sv[1] = S[2] - S[1]; |
| 166 |
2/4✓ Branch 1 taken 338977 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 338977 times.
✗ Branch 5 not taken.
|
338977 | Sv[2] = S[0] - S[2]; |
| 167 | |||
| 168 |
2/4✓ Branch 1 taken 338977 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 338977 times.
✗ Branch 5 not taken.
|
338977 | Tv[0] = T[1] - T[0]; |
| 169 |
2/4✓ Branch 1 taken 338977 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 338977 times.
✗ Branch 5 not taken.
|
338977 | Tv[1] = T[2] - T[1]; |
| 170 |
2/4✓ Branch 1 taken 338977 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 338977 times.
✗ Branch 5 not taken.
|
338977 | 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 338977 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 338977 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 338977 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 338977 times.
✗ Branch 11 not taken.
|
338977 | Vec3s V, Z, minP, minQ; |
| 181 | Scalar mindd; | ||
| 182 | 338977 | int shown_disjoint = 0; | |
| 183 | |||
| 184 |
2/4✓ Branch 1 taken 338977 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 338977 times.
✗ Branch 5 not taken.
|
338977 | mindd = (S[0] - T[0]).squaredNorm() + 1; // Set first minimum safely high |
| 185 | |||
| 186 |
2/2✓ Branch 0 taken 471566 times.
✓ Branch 1 taken 3412 times.
|
474978 | for (int i = 0; i < 3; ++i) { |
| 187 |
2/2✓ Branch 0 taken 860923 times.
✓ Branch 1 taken 136001 times.
|
996924 | 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 860923 times.
✗ Branch 2 not taken.
|
860923 | segPoints(S[i], Sv[i], T[j], Tv[j], VEC, P, Q); |
| 191 | |||
| 192 |
2/4✓ Branch 1 taken 860923 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 860923 times.
✗ Branch 5 not taken.
|
860923 | V = Q - P; |
| 193 |
1/2✓ Branch 1 taken 860923 times.
✗ Branch 2 not taken.
|
860923 | 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 714002 times.
✓ Branch 1 taken 146921 times.
|
860923 | if (dd <= mindd) { |
| 199 |
1/2✓ Branch 1 taken 714002 times.
✗ Branch 2 not taken.
|
714002 | minP = P; |
| 200 |
1/2✓ Branch 1 taken 714002 times.
✗ Branch 2 not taken.
|
714002 | minQ = Q; |
| 201 | 714002 | mindd = dd; | |
| 202 | |||
| 203 |
2/4✓ Branch 1 taken 714002 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 714002 times.
✗ Branch 5 not taken.
|
714002 | Z = S[(i + 2) % 3] - P; |
| 204 |
1/2✓ Branch 1 taken 714002 times.
✗ Branch 2 not taken.
|
714002 | Scalar a = Z.dot(VEC); |
| 205 |
2/4✓ Branch 1 taken 714002 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 714002 times.
✗ Branch 5 not taken.
|
714002 | Z = T[(j + 2) % 3] - Q; |
| 206 |
1/2✓ Branch 1 taken 714002 times.
✗ Branch 2 not taken.
|
714002 | Scalar b = Z.dot(VEC); |
| 207 | |||
| 208 |
4/4✓ Branch 0 taken 435091 times.
✓ Branch 1 taken 278911 times.
✓ Branch 2 taken 335565 times.
✓ Branch 3 taken 99526 times.
|
714002 | if ((a <= 0) && (b >= 0)) return dd; |
| 209 | |||
| 210 |
1/2✓ Branch 1 taken 378437 times.
✗ Branch 2 not taken.
|
378437 | Scalar p = V.dot(VEC); |
| 211 | |||
| 212 |
2/2✓ Branch 0 taken 99390 times.
✓ Branch 1 taken 279047 times.
|
378437 | if (a < 0) a = 0; |
| 213 |
2/2✓ Branch 0 taken 238879 times.
✓ Branch 1 taken 139558 times.
|
378437 | if (b > 0) b = 0; |
| 214 |
2/2✓ Branch 0 taken 348780 times.
✓ Branch 1 taken 29657 times.
|
378437 | 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 3412 times.
✗ Branch 2 not taken.
|
3412 | Vec3s Sn; |
| 236 | Scalar Snl; | ||
| 237 | |||
| 238 |
1/2✓ Branch 1 taken 3412 times.
✗ Branch 2 not taken.
|
3412 | Sn = Sv[0].cross(Sv[1]); // Compute normal to S triangle |
| 239 |
1/2✓ Branch 1 taken 3412 times.
✗ Branch 2 not taken.
|
3412 | 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 3412 times.
✗ Branch 1 not taken.
|
3412 | if (Snl > 1e-15) { |
| 244 | // Get projection lengths of T points | ||
| 245 | |||
| 246 |
1/2✓ Branch 1 taken 3412 times.
✗ Branch 2 not taken.
|
3412 | Vec3s Tp; |
| 247 | |||
| 248 |
2/4✓ Branch 1 taken 3412 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3412 times.
✗ Branch 5 not taken.
|
3412 | V = S[0] - T[0]; |
| 249 |
2/4✓ Branch 1 taken 3412 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3412 times.
✗ Branch 5 not taken.
|
3412 | Tp[0] = V.dot(Sn); |
| 250 | |||
| 251 |
2/4✓ Branch 1 taken 3412 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3412 times.
✗ Branch 5 not taken.
|
3412 | V = S[0] - T[1]; |
| 252 |
2/4✓ Branch 1 taken 3412 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3412 times.
✗ Branch 5 not taken.
|
3412 | Tp[1] = V.dot(Sn); |
| 253 | |||
| 254 |
2/4✓ Branch 1 taken 3412 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3412 times.
✗ Branch 5 not taken.
|
3412 | V = S[0] - T[2]; |
| 255 |
2/4✓ Branch 1 taken 3412 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3412 times.
✗ Branch 5 not taken.
|
3412 | Tp[2] = V.dot(Sn); |
| 256 | |||
| 257 | // If Sn is a separating direction, | ||
| 258 | // find point with smallest projection | ||
| 259 | |||
| 260 | 3412 | int point = -1; | |
| 261 |
11/14✓ Branch 1 taken 3412 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1275 times.
✓ Branch 4 taken 2137 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 2961 times.
|
3412 | 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 2961 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2137 times.
✓ Branch 4 taken 824 times.
✓ Branch 6 taken 2137 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 1615 times.
✓ Branch 9 taken 522 times.
✓ Branch 11 taken 1615 times.
✗ Branch 12 not taken.
✓ Branch 13 taken 1360 times.
✓ Branch 14 taken 255 times.
✓ Branch 15 taken 1360 times.
✓ Branch 16 taken 1601 times.
|
2961 | } else if ((Tp[0] < 0) && (Tp[1] < 0) && (Tp[2] < 0)) { |
| 268 |
4/6✓ Branch 1 taken 1360 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1360 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 810 times.
✓ Branch 7 taken 550 times.
|
1360 | if (Tp[0] > Tp[1]) |
| 269 | 810 | point = 0; | |
| 270 | else | ||
| 271 | 550 | point = 1; | |
| 272 |
4/6✓ Branch 1 taken 1360 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1360 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 519 times.
✓ Branch 7 taken 841 times.
|
1360 | if (Tp[2] > Tp[point]) point = 2; |
| 273 | } | ||
| 274 | |||
| 275 | // If Sn is a separating direction, | ||
| 276 | |||
| 277 |
2/2✓ Branch 0 taken 1811 times.
✓ Branch 1 taken 1601 times.
|
3412 | if (point >= 0) { |
| 278 | 1811 | 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 1811 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1811 times.
✗ Branch 5 not taken.
|
1811 | V = T[point] - S[0]; |
| 284 |
1/2✓ Branch 1 taken 1811 times.
✗ Branch 2 not taken.
|
1811 | Z = Sn.cross(Sv[0]); |
| 285 |
3/4✓ Branch 1 taken 1811 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1726 times.
✓ Branch 4 taken 85 times.
|
1811 | if (V.dot(Z) > 0) { |
| 286 |
2/4✓ Branch 1 taken 1726 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1726 times.
✗ Branch 5 not taken.
|
1726 | V = T[point] - S[1]; |
| 287 |
1/2✓ Branch 1 taken 1726 times.
✗ Branch 2 not taken.
|
1726 | Z = Sn.cross(Sv[1]); |
| 288 |
3/4✓ Branch 1 taken 1726 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1628 times.
✓ Branch 4 taken 98 times.
|
1726 | if (V.dot(Z) > 0) { |
| 289 |
2/4✓ Branch 1 taken 1628 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1628 times.
✗ Branch 5 not taken.
|
1628 | V = T[point] - S[2]; |
| 290 |
1/2✓ Branch 1 taken 1628 times.
✗ Branch 2 not taken.
|
1628 | Z = Sn.cross(Sv[2]); |
| 291 |
3/4✓ Branch 1 taken 1628 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1592 times.
✓ Branch 4 taken 36 times.
|
1628 | 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 1592 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1592 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1592 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1592 times.
✗ Branch 11 not taken.
|
1592 | P = T[point] + Sn * (Tp[point] / Snl); |
| 295 |
1/2✓ Branch 1 taken 1592 times.
✗ Branch 2 not taken.
|
1592 | Q = T[point]; |
| 296 |
2/4✓ Branch 1 taken 1592 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1592 times.
✗ Branch 5 not taken.
|
1592 | return (P - Q).squaredNorm(); |
| 297 | } | ||
| 298 | } | ||
| 299 | } | ||
| 300 | } | ||
| 301 | } | ||
| 302 | |||
| 303 |
1/2✓ Branch 1 taken 1820 times.
✗ Branch 2 not taken.
|
1820 | Vec3s Tn; |
| 304 | Scalar Tnl; | ||
| 305 | |||
| 306 |
1/2✓ Branch 1 taken 1820 times.
✗ Branch 2 not taken.
|
1820 | Tn = Tv[0].cross(Tv[1]); |
| 307 |
1/2✓ Branch 1 taken 1820 times.
✗ Branch 2 not taken.
|
1820 | Tnl = Tn.dot(Tn); |
| 308 | |||
| 309 |
1/2✓ Branch 0 taken 1820 times.
✗ Branch 1 not taken.
|
1820 | if (Tnl > 1e-15) { |
| 310 |
1/2✓ Branch 1 taken 1820 times.
✗ Branch 2 not taken.
|
1820 | Vec3s Sp; |
| 311 | |||
| 312 |
2/4✓ Branch 1 taken 1820 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1820 times.
✗ Branch 5 not taken.
|
1820 | V = T[0] - S[0]; |
| 313 |
2/4✓ Branch 1 taken 1820 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1820 times.
✗ Branch 5 not taken.
|
1820 | Sp[0] = V.dot(Tn); |
| 314 | |||
| 315 |
2/4✓ Branch 1 taken 1820 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1820 times.
✗ Branch 5 not taken.
|
1820 | V = T[0] - S[1]; |
| 316 |
2/4✓ Branch 1 taken 1820 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1820 times.
✗ Branch 5 not taken.
|
1820 | Sp[1] = V.dot(Tn); |
| 317 | |||
| 318 |
2/4✓ Branch 1 taken 1820 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1820 times.
✗ Branch 5 not taken.
|
1820 | V = T[0] - S[2]; |
| 319 |
2/4✓ Branch 1 taken 1820 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1820 times.
✗ Branch 5 not taken.
|
1820 | Sp[2] = V.dot(Tn); |
| 320 | |||
| 321 | 1820 | int point = -1; | |
| 322 |
11/14✓ Branch 1 taken 1820 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 766 times.
✓ Branch 4 taken 1054 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 1774 times.
|
1820 | 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 1774 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1054 times.
✓ Branch 4 taken 720 times.
✓ Branch 6 taken 1054 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 403 times.
✓ Branch 9 taken 651 times.
✓ Branch 11 taken 403 times.
✗ Branch 12 not taken.
✓ Branch 13 taken 288 times.
✓ Branch 14 taken 115 times.
✓ Branch 15 taken 288 times.
✓ Branch 16 taken 1486 times.
|
1774 | } else if ((Sp[0] < 0) && (Sp[1] < 0) && (Sp[2] < 0)) { |
| 329 |
4/6✓ Branch 1 taken 288 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 288 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 131 times.
✓ Branch 7 taken 157 times.
|
288 | if (Sp[0] > Sp[1]) |
| 330 | 131 | point = 0; | |
| 331 | else | ||
| 332 | 157 | point = 1; | |
| 333 |
4/6✓ Branch 1 taken 288 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 288 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 100 times.
✓ Branch 7 taken 188 times.
|
288 | if (Sp[2] > Sp[point]) point = 2; |
| 334 | } | ||
| 335 | |||
| 336 |
2/2✓ Branch 0 taken 334 times.
✓ Branch 1 taken 1486 times.
|
1820 | if (point >= 0) { |
| 337 | 334 | shown_disjoint = 1; | |
| 338 | |||
| 339 |
2/4✓ Branch 1 taken 334 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 334 times.
✗ Branch 5 not taken.
|
334 | V = S[point] - T[0]; |
| 340 |
1/2✓ Branch 1 taken 334 times.
✗ Branch 2 not taken.
|
334 | Z = Tn.cross(Tv[0]); |
| 341 |
2/4✓ Branch 1 taken 334 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 334 times.
✗ Branch 4 not taken.
|
334 | if (V.dot(Z) > 0) { |
| 342 |
2/4✓ Branch 1 taken 334 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 334 times.
✗ Branch 5 not taken.
|
334 | V = S[point] - T[1]; |
| 343 |
1/2✓ Branch 1 taken 334 times.
✗ Branch 2 not taken.
|
334 | Z = Tn.cross(Tv[1]); |
| 344 |
2/4✓ Branch 1 taken 334 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 334 times.
✗ Branch 4 not taken.
|
334 | if (V.dot(Z) > 0) { |
| 345 |
2/4✓ Branch 1 taken 334 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 334 times.
✗ Branch 5 not taken.
|
334 | V = S[point] - T[2]; |
| 346 |
1/2✓ Branch 1 taken 334 times.
✗ Branch 2 not taken.
|
334 | Z = Tn.cross(Tv[2]); |
| 347 |
2/4✓ Branch 1 taken 334 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 334 times.
✗ Branch 4 not taken.
|
334 | if (V.dot(Z) > 0) { |
| 348 |
1/2✓ Branch 1 taken 334 times.
✗ Branch 2 not taken.
|
334 | P = S[point]; |
| 349 |
4/8✓ Branch 1 taken 334 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 334 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 334 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 334 times.
✗ Branch 11 not taken.
|
334 | Q = S[point] + Tn * (Sp[point] / Tnl); |
| 350 |
2/4✓ Branch 1 taken 334 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 334 times.
✗ Branch 5 not taken.
|
334 | 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 | 338977 | 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 1016931 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1016931 times.
✓ Branch 4 taken 338977 times.
|
1355908 | Vec3s S[3]; |
| 375 |
3/4✓ Branch 1 taken 1016931 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1016931 times.
✓ Branch 4 taken 338977 times.
|
1355908 | Vec3s T[3]; |
| 376 |
1/2✓ Branch 1 taken 338977 times.
✗ Branch 2 not taken.
|
338977 | S[0] = S1; |
| 377 |
1/2✓ Branch 1 taken 338977 times.
✗ Branch 2 not taken.
|
338977 | S[1] = S2; |
| 378 |
1/2✓ Branch 1 taken 338977 times.
✗ Branch 2 not taken.
|
338977 | S[2] = S3; |
| 379 |
1/2✓ Branch 1 taken 338977 times.
✗ Branch 2 not taken.
|
338977 | T[0] = T1; |
| 380 |
1/2✓ Branch 1 taken 338977 times.
✗ Branch 2 not taken.
|
338977 | T[1] = T2; |
| 381 |
1/2✓ Branch 1 taken 338977 times.
✗ Branch 2 not taken.
|
338977 | T[2] = T3; |
| 382 | |||
| 383 |
1/2✓ Branch 1 taken 338977 times.
✗ Branch 2 not taken.
|
677954 | 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 | 335376 | 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 335376 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 335376 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 335376 times.
✗ Branch 8 not taken.
|
335376 | Vec3s T1_transformed = R * T1 + Tl; |
| 414 |
3/6✓ Branch 1 taken 335376 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 335376 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 335376 times.
✗ Branch 8 not taken.
|
335376 | Vec3s T2_transformed = R * T2 + Tl; |
| 415 |
3/6✓ Branch 1 taken 335376 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 335376 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 335376 times.
✗ Branch 8 not taken.
|
335376 | Vec3s T3_transformed = R * T3 + Tl; |
| 416 |
1/2✓ Branch 1 taken 335376 times.
✗ Branch 2 not taken.
|
335376 | return sqrTriDistance(S1, S2, S3, T1_transformed, T2_transformed, |
| 417 | 670752 | 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 |