Directory: | ./ |
---|---|
File: | include/coal/internal/intersect.hxx |
Date: | 2025-04-01 09:23:31 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 156 | 174 | 89.7% |
Branches: | 175 | 326 | 53.7% |
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 | #ifndef COAL_INTERSECT_HXX | ||
39 | #define COAL_INTERSECT_HXX | ||
40 | |||
41 | #include "coal/internal/intersect.h" | ||
42 | #include "coal/internal/tools.h" | ||
43 | |||
44 | #include <iostream> | ||
45 | #include <limits> | ||
46 | #include <vector> | ||
47 | #include <cmath> | ||
48 | |||
49 | namespace coal { | ||
50 | |||
51 | template <typename Scalar> | ||
52 | 42338 | inline typename Project<Scalar>::ProjectResult Project<Scalar>::projectLine( | |
53 | const Vec3& a, const Vec3& b, const Vec3& p) { | ||
54 | 42338 | ProjectResult res; | |
55 | |||
56 |
2/4✓ Branch 1 taken 42338 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 42338 times.
✗ Branch 5 not taken.
|
42338 | const Vec3 d = b - a; |
57 |
1/2✓ Branch 1 taken 42338 times.
✗ Branch 2 not taken.
|
42338 | const Scalar l = d.squaredNorm(); |
58 | |||
59 |
1/2✓ Branch 0 taken 42338 times.
✗ Branch 1 not taken.
|
42338 | if (l > 0) { |
60 |
2/4✓ Branch 1 taken 42338 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 42338 times.
✗ Branch 5 not taken.
|
42338 | const Scalar t = (p - a).dot(d); |
61 |
4/4✓ Branch 0 taken 20313 times.
✓ Branch 1 taken 22025 times.
✓ Branch 2 taken 11047 times.
✓ Branch 3 taken 10978 times.
|
42338 | res.parameterization[1] = (t >= l) ? 1 : ((t <= 0) ? 0 : (t / l)); |
62 | 42338 | res.parameterization[0] = 1 - res.parameterization[1]; | |
63 |
2/2✓ Branch 0 taken 20313 times.
✓ Branch 1 taken 22025 times.
|
42338 | if (t >= l) { |
64 |
2/4✓ Branch 1 taken 20313 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20313 times.
✗ Branch 5 not taken.
|
20313 | res.sqr_distance = (p - b).squaredNorm(); |
65 | 20313 | res.encode = 2; /* 0x10 */ | |
66 |
2/2✓ Branch 0 taken 11047 times.
✓ Branch 1 taken 10978 times.
|
22025 | } else if (t <= 0) { |
67 |
2/4✓ Branch 1 taken 11047 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11047 times.
✗ Branch 5 not taken.
|
11047 | res.sqr_distance = (p - a).squaredNorm(); |
68 | 11047 | res.encode = 1; /* 0x01 */ | |
69 | } else { | ||
70 |
4/8✓ Branch 1 taken 10978 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10978 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10978 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10978 times.
✗ Branch 11 not taken.
|
10978 | res.sqr_distance = (a + d * res.parameterization[1] - p).squaredNorm(); |
71 | 10978 | res.encode = 3; /* 0x00 */ | |
72 | } | ||
73 | } | ||
74 | |||
75 | 84676 | return res; | |
76 | } | ||
77 | |||
78 | template <typename Scalar> | ||
79 | 122577 | inline typename Project<Scalar>::ProjectResult Project<Scalar>::projectTriangle( | |
80 | const Vec3& a, const Vec3& b, const Vec3& c, const Vec3& p) { | ||
81 | 122577 | ProjectResult res; | |
82 | |||
83 | static const size_t nexti[3] = {1, 2, 0}; | ||
84 | 122577 | const Vec3* vt[] = {&a, &b, &c}; | |
85 |
6/12✓ Branch 1 taken 122577 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 122577 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 122577 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 122577 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 122577 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 122577 times.
✗ Branch 17 not taken.
|
122577 | const Vec3 dl[] = {a - b, b - c, c - a}; |
86 |
1/2✓ Branch 1 taken 122577 times.
✗ Branch 2 not taken.
|
122577 | const Vec3& n = dl[0].cross(dl[1]); |
87 |
1/2✓ Branch 1 taken 122577 times.
✗ Branch 2 not taken.
|
122577 | const Scalar l = n.squaredNorm(); |
88 | |||
89 |
1/2✓ Branch 0 taken 122577 times.
✗ Branch 1 not taken.
|
122577 | if (l > 0) { |
90 | 122577 | Scalar mindist = -1; | |
91 |
2/2✓ Branch 0 taken 367731 times.
✓ Branch 1 taken 122577 times.
|
490308 | for (size_t i = 0; i < 3; ++i) { |
92 |
5/8✓ Branch 1 taken 367731 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 367731 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 367731 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 42335 times.
✓ Branch 10 taken 325396 times.
|
367731 | if ((*vt[i] - p).dot(dl[i].cross(n)) > |
93 | 0) // origin is to the outside part of the triangle edge, then the | ||
94 | // optimal can only be on the edge | ||
95 | { | ||
96 | 42335 | size_t j = nexti[i]; | |
97 |
1/2✓ Branch 1 taken 42335 times.
✗ Branch 2 not taken.
|
42335 | ProjectResult res_line = projectLine(*vt[i], *vt[j], p); |
98 | |||
99 |
3/4✓ Branch 0 taken 924 times.
✓ Branch 1 taken 41411 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 924 times.
|
42335 | if (mindist < 0 || res_line.sqr_distance < mindist) { |
100 | 41411 | mindist = res_line.sqr_distance; | |
101 | 41411 | res.encode = | |
102 |
2/2✓ Branch 0 taken 21348 times.
✓ Branch 1 taken 20063 times.
|
41411 | static_cast<unsigned int>(((res_line.encode & 1) ? 1 << i : 0) + |
103 |
2/2✓ Branch 0 taken 31040 times.
✓ Branch 1 taken 10371 times.
|
41411 | ((res_line.encode & 2) ? 1 << j : 0)); |
104 | 41411 | res.parameterization[i] = res_line.parameterization[0]; | |
105 | 41411 | res.parameterization[j] = res_line.parameterization[1]; | |
106 | 41411 | res.parameterization[nexti[j]] = 0; | |
107 | } | ||
108 | } | ||
109 | } | ||
110 | |||
111 |
2/2✓ Branch 0 taken 81166 times.
✓ Branch 1 taken 41411 times.
|
122577 | if (mindist < 0) // the origin project is within the triangle |
112 | { | ||
113 |
2/4✓ Branch 1 taken 81166 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 81166 times.
✗ Branch 5 not taken.
|
81166 | Scalar d = (a - p).dot(n); |
114 | 81166 | Scalar s = std::sqrt(l); | |
115 |
2/4✓ Branch 1 taken 81166 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 81166 times.
✗ Branch 5 not taken.
|
81166 | Vec3 p_to_project = n * (d / l); |
116 |
1/2✓ Branch 1 taken 81166 times.
✗ Branch 2 not taken.
|
81166 | mindist = p_to_project.squaredNorm(); |
117 | 81166 | res.encode = 7; // m = 0x111 | |
118 |
4/8✓ Branch 1 taken 81166 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 81166 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 81166 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 81166 times.
✗ Branch 11 not taken.
|
81166 | res.parameterization[0] = dl[1].cross(b - p - p_to_project).norm() / s; |
119 |
4/8✓ Branch 1 taken 81166 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 81166 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 81166 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 81166 times.
✗ Branch 11 not taken.
|
81166 | res.parameterization[1] = dl[2].cross(c - p - p_to_project).norm() / s; |
120 | 81166 | res.parameterization[2] = | |
121 | 81166 | 1 - res.parameterization[0] - res.parameterization[1]; | |
122 | } | ||
123 | |||
124 | 122577 | res.sqr_distance = mindist; | |
125 | } | ||
126 | |||
127 | 245154 | return res; | |
128 | } | ||
129 | |||
130 | template <typename Scalar> | ||
131 | inline typename Project<Scalar>::ProjectResult | ||
132 | 15 | Project<Scalar>::projectTetrahedra(const Vec3& a, const Vec3& b, const Vec3& c, | |
133 | const Vec3& d, const Vec3& p) { | ||
134 | 15 | ProjectResult res; | |
135 | |||
136 | static const size_t nexti[] = {1, 2, 0}; | ||
137 | 15 | const Vec3* vt[] = {&a, &b, &c, &d}; | |
138 |
6/12✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 15 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 15 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 15 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 15 times.
✗ Branch 17 not taken.
|
15 | const Vec3 dl[3] = {a - d, b - d, c - d}; |
139 |
1/2✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
|
15 | Scalar vl = triple(dl[0], dl[1], dl[2]); |
140 |
5/10✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 15 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 15 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 15 times.
✗ Branch 14 not taken.
|
15 | bool ng = (vl * (a - p).dot((b - c).cross(a - b))) <= 0; |
141 |
5/6✓ Branch 0 taken 8 times.
✓ Branch 1 taken 7 times.
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 8 times.
✓ Branch 5 taken 7 times.
|
23 | if (ng && |
142 | 8 | std::abs(vl) > 0) // abs(vl) == 0, the tetrahedron is degenerated; if ng | |
143 | // is false, then the last vertex in the tetrahedron | ||
144 | // does not grow toward the origin (in fact origin is | ||
145 | // on the other side of the abc face) | ||
146 | { | ||
147 | 8 | Scalar mindist = -1; | |
148 | |||
149 |
2/2✓ Branch 0 taken 24 times.
✓ Branch 1 taken 8 times.
|
32 | for (size_t i = 0; i < 3; ++i) { |
150 | 24 | size_t j = nexti[i]; | |
151 |
3/6✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24 times.
✗ Branch 8 not taken.
|
24 | Scalar s = vl * (d - p).dot(dl[i].cross(dl[j])); |
152 |
2/2✓ Branch 0 taken 12 times.
✓ Branch 1 taken 12 times.
|
24 | if (s > 0) // the origin is to the outside part of a triangle face, then |
153 | // the optimal can only be on the triangle face | ||
154 | { | ||
155 |
1/2✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
|
12 | ProjectResult res_triangle = projectTriangle(*vt[i], *vt[j], d, p); |
156 |
3/4✓ Branch 0 taken 5 times.
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 5 times.
|
12 | if (mindist < 0 || res_triangle.sqr_distance < mindist) { |
157 | 7 | mindist = res_triangle.sqr_distance; | |
158 | 7 | res.encode = | |
159 |
2/2✓ Branch 0 taken 4 times.
✓ Branch 1 taken 3 times.
|
7 | static_cast<unsigned int>((res_triangle.encode & 1 ? 1 << i : 0) + |
160 |
2/2✓ Branch 0 taken 5 times.
✓ Branch 1 taken 2 times.
|
7 | (res_triangle.encode & 2 ? 1 << j : 0) + |
161 | 7 | (res_triangle.encode & 4 ? 8 : 0)); | |
162 | 7 | res.parameterization[i] = res_triangle.parameterization[0]; | |
163 | 7 | res.parameterization[j] = res_triangle.parameterization[1]; | |
164 | 7 | res.parameterization[nexti[j]] = 0; | |
165 | 7 | res.parameterization[3] = res_triangle.parameterization[2]; | |
166 | } | ||
167 | } | ||
168 | } | ||
169 | |||
170 |
2/2✓ Branch 0 taken 1 times.
✓ Branch 1 taken 7 times.
|
8 | if (mindist < 0) { |
171 | 1 | mindist = 0; | |
172 | 1 | res.encode = 15; | |
173 |
4/8✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
|
1 | res.parameterization[0] = triple(c - p, b - p, d - p) / vl; |
174 |
4/8✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
|
1 | res.parameterization[1] = triple(a - p, c - p, d - p) / vl; |
175 |
4/8✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
|
1 | res.parameterization[2] = triple(b - p, a - p, d - p) / vl; |
176 | 1 | res.parameterization[3] = | |
177 | 1 | 1 - (res.parameterization[0] + res.parameterization[1] + | |
178 | 1 | res.parameterization[2]); | |
179 | } | ||
180 | |||
181 | 8 | res.sqr_distance = mindist; | |
182 |
1/2✓ Branch 0 taken 7 times.
✗ Branch 1 not taken.
|
7 | } else if (!ng) { |
183 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | res = projectTriangle(a, b, c, p); |
184 | 7 | res.parameterization[3] = 0; | |
185 | } | ||
186 | 30 | return res; | |
187 | } | ||
188 | |||
189 | template <typename Scalar> | ||
190 | inline typename Project<Scalar>::ProjectResult | ||
191 | 7 | Project<Scalar>::projectLineOrigin(const Vec3& a, const Vec3& b) { | |
192 | 7 | ProjectResult res; | |
193 | |||
194 |
2/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
|
7 | const Vec3 d = b - a; |
195 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | const Scalar l = d.squaredNorm(); |
196 | |||
197 |
1/2✓ Branch 0 taken 7 times.
✗ Branch 1 not taken.
|
7 | if (l > 0) { |
198 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | const Scalar t = -a.dot(d); |
199 |
2/4✗ Branch 0 not taken.
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 7 times.
|
7 | res.parameterization[1] = (t >= l) ? 1 : ((t <= 0) ? 0 : (t / l)); |
200 | 7 | res.parameterization[0] = 1 - res.parameterization[1]; | |
201 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 7 times.
|
7 | if (t >= l) { |
202 | ✗ | res.sqr_distance = b.squaredNorm(); | |
203 | ✗ | res.encode = 2; /* 0x10 */ | |
204 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 7 times.
|
7 | } else if (t <= 0) { |
205 | ✗ | res.sqr_distance = a.squaredNorm(); | |
206 | ✗ | res.encode = 1; /* 0x01 */ | |
207 | } else { | ||
208 |
3/6✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
|
7 | res.sqr_distance = (a + d * res.parameterization[1]).squaredNorm(); |
209 | 7 | res.encode = 3; /* 0x00 */ | |
210 | } | ||
211 | } | ||
212 | |||
213 | 14 | return res; | |
214 | } | ||
215 | |||
216 | template <typename Scalar> | ||
217 | inline typename Project<Scalar>::ProjectResult | ||
218 | 376296 | Project<Scalar>::projectTriangleOrigin(const Vec3& a, const Vec3& b, | |
219 | const Vec3& c) { | ||
220 | 376296 | ProjectResult res; | |
221 | |||
222 | static const size_t nexti[3] = {1, 2, 0}; | ||
223 | 376296 | const Vec3* vt[] = {&a, &b, &c}; | |
224 |
6/12✓ Branch 1 taken 376296 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 376296 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 376296 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 376296 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 376296 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 376296 times.
✗ Branch 17 not taken.
|
376296 | const Vec3 dl[] = {a - b, b - c, c - a}; |
225 |
1/2✓ Branch 1 taken 376296 times.
✗ Branch 2 not taken.
|
376296 | const Vec3& n = dl[0].cross(dl[1]); |
226 |
1/2✓ Branch 1 taken 376296 times.
✗ Branch 2 not taken.
|
376296 | const Scalar l = n.squaredNorm(); |
227 | |||
228 |
1/2✓ Branch 0 taken 376296 times.
✗ Branch 1 not taken.
|
376296 | if (l > 0) { |
229 | 376296 | Scalar mindist = -1; | |
230 |
2/2✓ Branch 0 taken 1128888 times.
✓ Branch 1 taken 376296 times.
|
1505184 | for (size_t i = 0; i < 3; ++i) { |
231 |
4/6✓ Branch 1 taken 1128888 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1128888 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 7 times.
✓ Branch 7 taken 1128881 times.
|
1128888 | if (vt[i]->dot(dl[i].cross(n)) > |
232 | 0) // origin is to the outside part of the triangle edge, then the | ||
233 | // optimal can only be on the edge | ||
234 | { | ||
235 | 7 | size_t j = nexti[i]; | |
236 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | ProjectResult res_line = projectLineOrigin(*vt[i], *vt[j]); |
237 | |||
238 |
1/4✗ Branch 0 not taken.
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
|
7 | if (mindist < 0 || res_line.sqr_distance < mindist) { |
239 | 7 | mindist = res_line.sqr_distance; | |
240 | 7 | res.encode = | |
241 |
1/2✓ Branch 0 taken 7 times.
✗ Branch 1 not taken.
|
7 | static_cast<unsigned int>(((res_line.encode & 1) ? 1 << i : 0) + |
242 |
1/2✓ Branch 0 taken 7 times.
✗ Branch 1 not taken.
|
7 | ((res_line.encode & 2) ? 1 << j : 0)); |
243 | 7 | res.parameterization[i] = res_line.parameterization[0]; | |
244 | 7 | res.parameterization[j] = res_line.parameterization[1]; | |
245 | 7 | res.parameterization[nexti[j]] = 0; | |
246 | } | ||
247 | } | ||
248 | } | ||
249 | |||
250 |
2/2✓ Branch 0 taken 376289 times.
✓ Branch 1 taken 7 times.
|
376296 | if (mindist < 0) // the origin project is within the triangle |
251 | { | ||
252 |
1/2✓ Branch 1 taken 376289 times.
✗ Branch 2 not taken.
|
376289 | Scalar d = a.dot(n); |
253 | 376289 | Scalar s = std::sqrt(l); | |
254 |
2/4✓ Branch 1 taken 376289 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 376289 times.
✗ Branch 5 not taken.
|
376289 | Vec3 o_to_project = n * (d / l); |
255 |
1/2✓ Branch 1 taken 376289 times.
✗ Branch 2 not taken.
|
376289 | mindist = o_to_project.squaredNorm(); |
256 | 376289 | res.encode = 7; // m = 0x111 | |
257 |
3/6✓ Branch 1 taken 376289 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 376289 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 376289 times.
✗ Branch 8 not taken.
|
376289 | res.parameterization[0] = dl[1].cross(b - o_to_project).norm() / s; |
258 |
3/6✓ Branch 1 taken 376289 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 376289 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 376289 times.
✗ Branch 8 not taken.
|
376289 | res.parameterization[1] = dl[2].cross(c - o_to_project).norm() / s; |
259 | 376289 | res.parameterization[2] = | |
260 | 376289 | 1 - res.parameterization[0] - res.parameterization[1]; | |
261 | } | ||
262 | |||
263 | 376296 | res.sqr_distance = mindist; | |
264 | } | ||
265 | |||
266 | 752592 | return res; | |
267 | } | ||
268 | |||
269 | template <typename Scalar> | ||
270 | inline typename Project<Scalar>::ProjectResult | ||
271 | 21109 | Project<Scalar>::projectTetrahedraOrigin(const Vec3& a, const Vec3& b, | |
272 | const Vec3& c, const Vec3& d) { | ||
273 | 21109 | ProjectResult res; | |
274 | |||
275 | static const size_t nexti[] = {1, 2, 0}; | ||
276 | 21109 | const Vec3* vt[] = {&a, &b, &c, &d}; | |
277 |
6/12✓ Branch 1 taken 21109 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 21109 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 21109 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 21109 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 21109 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 21109 times.
✗ Branch 17 not taken.
|
21109 | const Vec3 dl[3] = {a - d, b - d, c - d}; |
278 |
1/2✓ Branch 1 taken 21109 times.
✗ Branch 2 not taken.
|
21109 | Scalar vl = triple(dl[0], dl[1], dl[2]); |
279 |
4/8✓ Branch 1 taken 21109 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 21109 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 21109 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 21109 times.
✗ Branch 11 not taken.
|
21109 | bool ng = (vl * a.dot((b - c).cross(a - b))) <= 0; |
280 |
3/6✓ Branch 0 taken 21109 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 21109 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 21109 times.
✗ Branch 5 not taken.
|
42218 | if (ng && |
281 | 21109 | std::abs(vl) > 0) // abs(vl) == 0, the tetrahedron is degenerated; if ng | |
282 | // is false, then the last vertex in the tetrahedron | ||
283 | // does not grow toward the origin (in fact origin is | ||
284 | // on the other side of the abc face) | ||
285 | { | ||
286 | 21109 | Scalar mindist = -1; | |
287 | |||
288 |
2/2✓ Branch 0 taken 63327 times.
✓ Branch 1 taken 21109 times.
|
84436 | for (size_t i = 0; i < 3; ++i) { |
289 | 63327 | size_t j = nexti[i]; | |
290 |
2/4✓ Branch 1 taken 63327 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 63327 times.
✗ Branch 5 not taken.
|
63327 | Scalar s = vl * d.dot(dl[i].cross(dl[j])); |
291 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 63327 times.
|
63327 | if (s > 0) // the origin is to the outside part of a triangle face, then |
292 | // the optimal can only be on the triangle face | ||
293 | { | ||
294 | ✗ | ProjectResult res_triangle = projectTriangleOrigin(*vt[i], *vt[j], d); | |
295 | ✗ | if (mindist < 0 || res_triangle.sqr_distance < mindist) { | |
296 | ✗ | mindist = res_triangle.sqr_distance; | |
297 | ✗ | res.encode = | |
298 | ✗ | static_cast<unsigned int>((res_triangle.encode & 1 ? 1 << i : 0) + | |
299 | ✗ | (res_triangle.encode & 2 ? 1 << j : 0) + | |
300 | ✗ | (res_triangle.encode & 4 ? 8 : 0)); | |
301 | ✗ | res.parameterization[i] = res_triangle.parameterization[0]; | |
302 | ✗ | res.parameterization[j] = res_triangle.parameterization[1]; | |
303 | ✗ | res.parameterization[nexti[j]] = 0; | |
304 | ✗ | res.parameterization[3] = res_triangle.parameterization[2]; | |
305 | } | ||
306 | } | ||
307 | } | ||
308 | |||
309 |
1/2✓ Branch 0 taken 21109 times.
✗ Branch 1 not taken.
|
21109 | if (mindist < 0) { |
310 | 21109 | mindist = 0; | |
311 | 21109 | res.encode = 15; | |
312 |
1/2✓ Branch 1 taken 21109 times.
✗ Branch 2 not taken.
|
21109 | res.parameterization[0] = triple(c, b, d) / vl; |
313 |
1/2✓ Branch 1 taken 21109 times.
✗ Branch 2 not taken.
|
21109 | res.parameterization[1] = triple(a, c, d) / vl; |
314 |
1/2✓ Branch 1 taken 21109 times.
✗ Branch 2 not taken.
|
21109 | res.parameterization[2] = triple(b, a, d) / vl; |
315 | 21109 | res.parameterization[3] = | |
316 | 21109 | 1 - (res.parameterization[0] + res.parameterization[1] + | |
317 | 21109 | res.parameterization[2]); | |
318 | } | ||
319 | |||
320 | 21109 | res.sqr_distance = mindist; | |
321 | ✗ | } else if (!ng) { | |
322 | ✗ | res = projectTriangleOrigin(a, b, c); | |
323 | ✗ | res.parameterization[3] = 0; | |
324 | } | ||
325 | 42218 | return res; | |
326 | } | ||
327 | |||
328 | } // namespace coal | ||
329 | |||
330 | #endif // COAL_INTERSECT_HXX | ||
331 |