Directory: | ./ |
---|---|
File: | Geometry/geo_utilities.cpp |
Date: | 2024-04-14 07:32:34 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 233 | 365 | 63.8% |
Branches: | 172 | 478 | 36.0% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // ______ ______ _ _ _____ ______ | ||
2 | // | ____| ____| | (_)/ ____| | ____| | ||
3 | // | |__ | |__ | | _| (___ ___| |__ | ||
4 | // | __| | __| | | | |\___ \ / __| __| | ||
5 | // | | | |____| |____| |____) | (__| |____ | ||
6 | // |_| |______|______|_|_____/ \___|______| | ||
7 | // Finite Elements for Life Sciences and Engineering | ||
8 | // | ||
9 | // License: LGL2.1 License | ||
10 | // FELiScE default license: LICENSE in root folder | ||
11 | // | ||
12 | // Main authors: V.Martin, L.Boilevin-Kayl | ||
13 | // | ||
14 | |||
15 | // System includes | ||
16 | #include <numeric> | ||
17 | |||
18 | // External includes | ||
19 | |||
20 | // Project includes | ||
21 | #include "FiniteElement/geoElement.hpp" | ||
22 | #include "Geometry/geometricMeshRegion.hpp" | ||
23 | #include "Geometry/geo_utilities.hpp" | ||
24 | #include "Tools/math_utilities.hpp" | ||
25 | |||
26 | namespace felisce { | ||
27 | |||
28 | ✗ | bool GeoUtilities::isPointInElement(const GeometricMeshRegion::ElementType eltType, const std::vector<Point*>& pointsOfElt, const Point& point, const double tol) | |
29 | { | ||
30 | // TODO D.C. i don't like this function at all | ||
31 | // we should rewrite it using polymorphism | ||
32 | ✗ | TypeShape elShape = static_cast<TypeShape>( GeometricMeshRegion::eltEnumToFelNameGeoEle[eltType].second->shape().typeShape() ); | |
33 | |||
34 | ✗ | switch ( elShape ) { | |
35 | |||
36 | ✗ | case Segment: { | |
37 | std::array<double, 2> bary; | ||
38 | ✗ | return isPointInSeg(pointsOfElt, point, bary, tol); | |
39 | |||
40 | break; | ||
41 | } | ||
42 | |||
43 | ✗ | case Triangle: { | |
44 | std::array<double, 3> bary; | ||
45 | ✗ | return isPointInTri(pointsOfElt, point, bary, tol); | |
46 | |||
47 | break; | ||
48 | } | ||
49 | |||
50 | ✗ | case Tetrahedron: { | |
51 | std::array<double, 4> bary; | ||
52 | ✗ | return isPointInTetra(pointsOfElt, point, bary, tol); | |
53 | |||
54 | break; | ||
55 | } | ||
56 | |||
57 | ✗ | case NullShape: | |
58 | case Node: | ||
59 | case Quadrilateral: | ||
60 | case Hexahedron: | ||
61 | case Prism: | ||
62 | case Pyramid: | ||
63 | |||
64 | ✗ | FEL_ERROR("Not implemented for this element Shape") | |
65 | ✗ | break; | |
66 | } | ||
67 | |||
68 | ✗ | return false; | |
69 | } | ||
70 | |||
71 | /***********************************************************************************/ | ||
72 | /***********************************************************************************/ | ||
73 | |||
74 | 19 | bool GeoUtilities::isPointInTetra(const std::vector<Point*>& eltPoints, const Point& p, std::array<double, 4>& bary, const double tol) | |
75 | { | ||
76 | // Compute inverse of tetrahedron volume | ||
77 |
1/2✓ Branch 5 taken 19 times.
✗ Branch 6 not taken.
|
19 | const double ivo = 1. / GeoUtilities::tetraVolume(*eltPoints[0],*eltPoints[1],*eltPoints[2],*eltPoints[3]); |
78 | |||
79 | // Compute barycentric coordinates | ||
80 |
1/2✓ Branch 1 taken 19 times.
✗ Branch 2 not taken.
|
19 | GeoUtilities::tetraBary(eltPoints, p, bary); |
81 | |||
82 | // Normalize barycentric coordinates | ||
83 | 19 | std::for_each(bary.begin(), bary.end(), [&](auto& val){ val *= ivo; }); | |
84 | |||
85 | // Check if point is inside | ||
86 |
2/2✓ Branch 2 taken 58 times.
✓ Branch 3 taken 9 times.
|
67 | for (auto it = bary.begin(); it != bary.end(); ++it) |
87 |
2/2✓ Branch 0 taken 10 times.
✓ Branch 1 taken 48 times.
|
58 | if ( *it < - tol ) |
88 | // Point is outside | ||
89 | 10 | return false; | |
90 | |||
91 | // Point is inside | ||
92 | 9 | return true; | |
93 | } | ||
94 | |||
95 | /***********************************************************************************/ | ||
96 | /***********************************************************************************/ | ||
97 | |||
98 | 20 | bool GeoUtilities::isPointInTetra(const std::vector<Point*>& eltPoints, const Point& p, std::array<double, 4>& bary, Location& location, const double tol) | |
99 | { | ||
100 | // Compute element volume | ||
101 |
1/2✓ Branch 5 taken 20 times.
✗ Branch 6 not taken.
|
20 | const double ivo = 1. / GeoUtilities::tetraVolume(*eltPoints[0], *eltPoints[1], *eltPoints[2], *eltPoints[3]); |
102 | |||
103 | // Compute barycentric coordinates | ||
104 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | GeoUtilities::tetraBary(eltPoints, p, bary); |
105 | |||
106 | // Normalize barycentric coordinates | ||
107 | 20 | std::for_each(bary.begin(), bary.end(), [&](auto& val){ val *= ivo; }); | |
108 | |||
109 | // Check if point is inside | ||
110 | 20 | unsigned short int nbrBarPos = 0, nbrBarEps = 0; | |
111 | std::array<unsigned short int,4> idxBarPos, idxBarEps; | ||
112 | |||
113 |
2/2✓ Branch 1 taken 64 times.
✓ Branch 2 taken 10 times.
|
74 | for (std::size_t i = 0; i < bary.size(); ++i) { |
114 |
2/2✓ Branch 1 taken 23 times.
✓ Branch 2 taken 41 times.
|
64 | if ( bary[i] > tol ) { |
115 | 23 | idxBarPos[nbrBarPos] = i; | |
116 | 23 | nbrBarPos++; | |
117 | } | ||
118 |
2/2✓ Branch 2 taken 31 times.
✓ Branch 3 taken 10 times.
|
41 | else if ( std::abs(bary[i]) <= tol ) { |
119 | 31 | idxBarEps[nbrBarEps] = i; | |
120 | 31 | nbrBarEps++; | |
121 | } | ||
122 | else { | ||
123 | // Point is outside | ||
124 | 10 | return false; | |
125 | } | ||
126 | } | ||
127 | |||
128 |
2/6✗ Branch 0 not taken.
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
10 | switch (nbrBarPos) { |
129 | ✗ | case 0 : | |
130 | ✗ | FEL_ERROR("GeoUtilities::isPointInTetra : Bary cannot be all <= tol !"); | |
131 | ✗ | break; | |
132 | |||
133 | 5 | case 1 : | |
134 | 5 | location.first = Position::OnVertex; | |
135 | 5 | location.second = std::array<unsigned short int,4>{3, 2, 0, 1}[idxBarPos[0]]; | |
136 | 5 | break; | |
137 | |||
138 | 5 | case 2 : | |
139 | 5 | location.first = Position::OnEdge; | |
140 | 5 | location.second = std::array<unsigned short int,16>{ 6, 0, 3, 1, /**/ 0, 6, 4, 2, /**/ 3, 4, 6, 5, /**/ 1, 2, 5, 6}[4*idxBarEps[0]+idxBarEps[1]]; | |
141 | 5 | break; | |
142 | |||
143 | ✗ | case 3 : | |
144 | ✗ | location.first = Position::OnFace; | |
145 | ✗ | location.second = idxBarEps[0]; | |
146 | ✗ | break; | |
147 | |||
148 | ✗ | case 4 : | |
149 | ✗ | location.first = Position::Inside; | |
150 | ✗ | location.second = 0; | |
151 | ✗ | break; | |
152 | |||
153 | ✗ | default: | |
154 | ✗ | FEL_ERROR("GeoUtilities::isPointInTetra : Error !"); | |
155 | } | ||
156 | |||
157 | // Point is inside | ||
158 | 10 | return true; | |
159 | } | ||
160 | /***********************************************************************************/ | ||
161 | /***********************************************************************************/ | ||
162 | |||
163 | 2831 | bool GeoUtilities::isPointInTri(const std::vector<Point*>& eltPoints, const Point& p, std::array<double, 3>& bary, const double tol) | |
164 | { | ||
165 | Point normal; | ||
166 | |||
167 | // Compute triangle volume | ||
168 |
1/2✓ Branch 4 taken 2831 times.
✗ Branch 5 not taken.
|
2831 | GeoUtilities::triNormal(*eltPoints[0],*eltPoints[1],*eltPoints[2], normal); |
169 | |||
170 | // Compute inverse norm and volume | ||
171 |
1/2✓ Branch 1 taken 2831 times.
✗ Branch 2 not taken.
|
2831 | const double ino = 1. / normal.norm(); |
172 | 2831 | const double ivo = ino / 0.5; | |
173 | |||
174 | // Normalize normal | ||
175 | 2831 | normal *= ino; | |
176 | |||
177 | // Compute barycentric coordinates | ||
178 |
1/2✓ Branch 1 taken 2831 times.
✗ Branch 2 not taken.
|
2831 | GeoUtilities::triBary(eltPoints, p, normal, bary); |
179 | |||
180 | // Normalize barycentric coordinates | ||
181 | 2831 | std::for_each(bary.begin(), bary.end(), [&](auto& val){ val *= ivo; }); | |
182 | |||
183 | // Check if point is inside | ||
184 |
2/2✓ Branch 2 taken 8457 times.
✓ Branch 3 taken 2810 times.
|
11267 | for (auto it = bary.begin(); it != bary.end(); ++it) |
185 |
2/2✓ Branch 0 taken 21 times.
✓ Branch 1 taken 8436 times.
|
8457 | if ( *it < - tol ) |
186 | // Point is outside | ||
187 | 21 | return false; | |
188 | |||
189 | // Point is inside | ||
190 | 2810 | return true; | |
191 | } | ||
192 | |||
193 | /***********************************************************************************/ | ||
194 | /***********************************************************************************/ | ||
195 | |||
196 | 4608 | bool GeoUtilities::isPointInTri(const std::vector<Point*>& eltPoints, const Point& p, std::array<double, 3>& bary, Location& location, const double tol) | |
197 | { | ||
198 | Point normal; | ||
199 | |||
200 | // Compute triangle volume | ||
201 |
1/2✓ Branch 4 taken 4608 times.
✗ Branch 5 not taken.
|
4608 | GeoUtilities::triNormal(*eltPoints[0],*eltPoints[1],*eltPoints[2], normal); |
202 | |||
203 | // Compute inverse norm and volume | ||
204 |
1/2✓ Branch 1 taken 4608 times.
✗ Branch 2 not taken.
|
4608 | const double ino = 1. / normal.norm(); |
205 | 4608 | const double ivo = ino / 0.5; | |
206 | |||
207 | // Normalize normal | ||
208 | 4608 | normal *= ino; | |
209 | |||
210 | // Compute barycentric coordinates | ||
211 |
1/2✓ Branch 1 taken 4608 times.
✗ Branch 2 not taken.
|
4608 | GeoUtilities::triBary(eltPoints, p, normal, bary); |
212 | |||
213 | // Normalize barycentric coordinates | ||
214 | 4608 | std::for_each(bary.begin(), bary.end(), [&](auto& val){ val *= ivo; }); | |
215 | |||
216 | // Check if point is inside | ||
217 | 4608 | unsigned short int nbrBarPos = 0, nbrBarEps = 0; | |
218 | unsigned short int idxBarPos, idxBarEps; | ||
219 | |||
220 |
2/2✓ Branch 1 taken 11722 times.
✓ Branch 2 taken 2506 times.
|
14228 | for (std::size_t i = 0; i < bary.size(); ++i) { |
221 |
2/2✓ Branch 1 taken 8462 times.
✓ Branch 2 taken 3260 times.
|
11722 | if ( bary[i] > tol ) { |
222 | 8462 | nbrBarPos++; | |
223 | 8462 | idxBarPos = i; | |
224 | } | ||
225 |
2/2✓ Branch 2 taken 1158 times.
✓ Branch 3 taken 2102 times.
|
3260 | else if ( std::abs(bary[i]) <= tol ) { |
226 | 1158 | nbrBarEps++; | |
227 | 1158 | idxBarEps = i; | |
228 | } | ||
229 | else { | ||
230 | // Point is outside | ||
231 | 2102 | return false; | |
232 | } | ||
233 | } | ||
234 | |||
235 |
3/5✗ Branch 0 not taken.
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 1154 times.
✓ Branch 3 taken 1350 times.
✗ Branch 4 not taken.
|
2506 | switch (nbrBarPos) { |
236 | ✗ | case 0 : | |
237 | ✗ | FEL_ERROR("GeoUtilities::isPointInTri : Bary cannot be all <= tol !"); | |
238 | ✗ | break; | |
239 | |||
240 | 2 | case 1 : | |
241 | 2 | location.first = Position::OnVertex; | |
242 | 2 | location.second = std::array<unsigned short int,3>{2, 0, 1}[idxBarPos]; | |
243 | 2 | break; | |
244 | |||
245 | 1154 | case 2 : | |
246 | 1154 | location.first = Position::OnEdge; | |
247 | 1154 | location.second = idxBarEps; | |
248 | 1154 | break; | |
249 | |||
250 | 1350 | case 3 : | |
251 | 1350 | location.first = Position::Inside; | |
252 | 1350 | location.second = 0; | |
253 | 1350 | break; | |
254 | |||
255 | ✗ | default: | |
256 | ✗ | FEL_ERROR("GeoUtilities::isPointInTri : Error !"); | |
257 | } | ||
258 | |||
259 | // Point is inside | ||
260 | 2506 | return true; | |
261 | } | ||
262 | |||
263 | /***********************************************************************************/ | ||
264 | /***********************************************************************************/ | ||
265 | |||
266 | 27 | bool GeoUtilities::isPointInSeg(const std::vector<Point*>& eltPoints, const Point& p, std::array<double, 2>& bary, const double tol) | |
267 | { | ||
268 |
1/2✓ Branch 3 taken 27 times.
✗ Branch 4 not taken.
|
27 | const Point edge = *eltPoints[1] - *eltPoints[0]; |
269 |
1/2✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
|
27 | const Point vec0 = p - *eltPoints[0]; |
270 |
1/2✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
|
27 | const Point vec1 = p - *eltPoints[1]; |
271 | |||
272 | // Compute inverse of segment length | ||
273 |
1/2✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
|
27 | const double ivo = edge.norm(); |
274 | |||
275 | // Compute barycentric coordinates | ||
276 | 27 | bary[0] = ( edge * vec0 ); | |
277 | 27 | bary[1] = - ( edge * vec1 ); | |
278 | |||
279 | |||
280 | // Normalize barycentric coordinates | ||
281 | 27 | std::for_each(bary.begin(), bary.end(), [&](auto& val){ val *= ivo; }); | |
282 | |||
283 | // Check if point is inside | ||
284 |
2/2✓ Branch 2 taken 47 times.
✓ Branch 3 taken 13 times.
|
60 | for (auto it = bary.begin(); it != bary.end(); ++it) |
285 |
2/2✓ Branch 0 taken 14 times.
✓ Branch 1 taken 33 times.
|
47 | if ( *it < - tol ) |
286 | // Point is outside | ||
287 | 14 | return false; | |
288 | |||
289 | // Point is inside | ||
290 | 13 | return true; | |
291 | } | ||
292 | |||
293 | /***********************************************************************************/ | ||
294 | /***********************************************************************************/ | ||
295 | |||
296 | 31 | void GeoUtilities::getClosestProjectionOnElement(const GeometricMeshRegion::ElementType eltType, const std::vector<Point*>& pointsOfElt, const Point& point, Point& projection, const double tol) | |
297 | { | ||
298 | // TODO D.C. i don't like this function at all | ||
299 | // we should rewrite it using polymorphism | ||
300 | |||
301 | 31 | TypeShape elShape = static_cast<TypeShape>( GeometricMeshRegion::eltEnumToFelNameGeoEle[eltType].second->shape().typeShape() ); | |
302 | |||
303 |
2/4✓ Branch 0 taken 10 times.
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
|
31 | switch ( elShape ) { |
304 | |||
305 | 10 | case Segment: { | |
306 | |||
307 | 10 | getClosestProjectionSeg(pointsOfElt, point, projection, tol); | |
308 | |||
309 | 10 | break; | |
310 | } | ||
311 | |||
312 | 21 | case Triangle: { | |
313 | |||
314 | 21 | getClosestProjectionTri(pointsOfElt, point, projection, tol); | |
315 | |||
316 | 21 | break; | |
317 | } | ||
318 | |||
319 | ✗ | case NullShape: | |
320 | case Node: | ||
321 | case Quadrilateral: | ||
322 | case Tetrahedron: | ||
323 | case Hexahedron: | ||
324 | case Prism: | ||
325 | case Pyramid: | ||
326 | |||
327 | ✗ | FEL_ERROR("Not implemented for this element Shape"); | |
328 | ✗ | break; | |
329 | } | ||
330 | 31 | } | |
331 | |||
332 | /***********************************************************************************/ | ||
333 | /***********************************************************************************/ | ||
334 | |||
335 | 27 | void GeoUtilities::getClosestProjectionSeg(const std::vector<Point*>& pointsOfElt, const Point& point, Point& projection, const double tol) | |
336 | { | ||
337 | std::array<double, 2> bary; | ||
338 |
3/4✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 13 times.
✓ Branch 4 taken 14 times.
|
27 | if ( isPointInSeg(pointsOfElt, point, bary, tol) ) { |
339 | 13 | projection = point; | |
340 | 13 | return; | |
341 | } | ||
342 | |||
343 |
2/2✓ Branch 1 taken 7 times.
✓ Branch 2 taken 7 times.
|
14 | if ( bary[0] < - tol ) |
344 | 7 | projection = *pointsOfElt[0]; | |
345 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | else if ( bary[1] < - tol ) |
346 | 7 | projection = *pointsOfElt[1]; | |
347 | else | ||
348 | ✗ | FEL_ERROR("It is not possible to have 2 negative bary."); | |
349 | } | ||
350 | |||
351 | /***********************************************************************************/ | ||
352 | /***********************************************************************************/ | ||
353 | |||
354 | 21 | void GeoUtilities::getClosestProjectionTri(const std::vector<Point*>& pointsOfElt, const Point& point, Point& projection, const double tol) | |
355 | { | ||
356 | std::array<double, 3> bary; | ||
357 |
3/4✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✓ Branch 4 taken 20 times.
|
21 | if ( isPointInTri(pointsOfElt, point, bary, tol) ) { |
358 | 1 | projection = point; | |
359 | 1 | return; | |
360 | } | ||
361 | |||
362 | 20 | std::size_t nbrPos = 0; | |
363 | 20 | std::size_t idxPos = 0, idxNeg = 0; | |
364 |
2/2✓ Branch 1 taken 60 times.
✓ Branch 2 taken 20 times.
|
80 | for (std::size_t i = 0; i < bary.size(); ++i){ |
365 |
2/2✓ Branch 1 taken 37 times.
✓ Branch 2 taken 23 times.
|
60 | if ( bary[i] > - tol ){ |
366 | 37 | nbrPos++; | |
367 | 37 | idxPos = i; | |
368 | } | ||
369 | else { | ||
370 | 23 | idxNeg = i; | |
371 | } | ||
372 | } | ||
373 | |||
374 |
2/3✓ Branch 0 taken 3 times.
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
|
20 | switch (nbrPos) { |
375 | |||
376 | // Two bary negative -> closest point is one of the triangle vertices | ||
377 | 3 | case 1: { | |
378 | |||
379 | 3 | unsigned short int idxVer = GeometricMeshRegion::m_eltTypMapVerToFac[GeometricMeshRegion::Tria3][idxPos][0]; | |
380 | 3 | projection = *pointsOfElt[idxVer]; | |
381 | |||
382 | 3 | break; | |
383 | } | ||
384 | |||
385 | // One bary negative -> closest point it's on the edge or it is one of the edge extreme points | ||
386 | 17 | case 2: { | |
387 | |||
388 |
1/2✓ Branch 3 taken 17 times.
✗ Branch 4 not taken.
|
17 | auto idxVer = GeometricMeshRegion::m_eltTypMapFacToVer[GeometricMeshRegion::Tria3][idxNeg]; |
389 | |||
390 | // Get edge | ||
391 |
1/2✓ Branch 6 taken 17 times.
✗ Branch 7 not taken.
|
17 | std::vector<Point*> edge{ pointsOfElt[idxVer[0]], pointsOfElt[idxVer[1]] }; |
392 | |||
393 | // Project point on edge | ||
394 |
1/2✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
|
17 | GeoUtilities::projectOnLine(edge, point, projection); |
395 | |||
396 | // Get closest projection on edge | ||
397 |
1/2✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
|
17 | getClosestProjectionSeg(edge, projection, projection, tol); |
398 | |||
399 | 17 | break; | |
400 | 17 | } | |
401 | |||
402 | ✗ | default: | |
403 | ✗ | FEL_ERROR("It is not possible to have 3 negative bary."); | |
404 | ✗ | break; | |
405 | } | ||
406 | } | ||
407 | |||
408 | /***********************************************************************************/ | ||
409 | /***********************************************************************************/ | ||
410 | |||
411 | 432 | void GeoUtilities::computeElementNormal(const GeometricMeshRegion::ElementType eltType, const std::vector<Point*>& pointsOfElt, Point& normal) | |
412 | { | ||
413 | // TODO D.C. i don't like this function at all | ||
414 | // we should remove it and use polymorphism instead | ||
415 | 432 | TypeShape elShape = static_cast<TypeShape>( GeometricMeshRegion::eltEnumToFelNameGeoEle[eltType].second->shape().typeShape() ); | |
416 | |||
417 | // TODO here we work on the shape... so we need only the points of the shape (Tria9 -> only three vertices, are the first three nodes???) | ||
418 | |||
419 |
1/4✓ Branch 0 taken 432 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
|
432 | switch ( elShape ) { |
420 | |||
421 | 432 | case Segment: | |
422 | |||
423 | 432 | normal.x() = pointsOfElt[0]->y() - pointsOfElt[1]->y(); | |
424 | 432 | normal.y() = pointsOfElt[1]->x() - pointsOfElt[0]->x(); | |
425 | 432 | normal.z() = 0.; | |
426 | 432 | normal *= 1. / normal.norm(); | |
427 | |||
428 | 432 | break; | |
429 | |||
430 | ✗ | case Triangle: | |
431 | |||
432 | ✗ | MathUtilities::CrossProduct(normal.getCoor(), (*pointsOfElt[0] - *pointsOfElt[1]).getCoor(), (*pointsOfElt[0] - *pointsOfElt[2]).getCoor()); | |
433 | |||
434 | ✗ | normal *= 1. / normal.norm(); | |
435 | |||
436 | ✗ | break; | |
437 | |||
438 | ✗ | case NullShape: | |
439 | case Node: | ||
440 | case Quadrilateral: | ||
441 | case Tetrahedron: | ||
442 | case Hexahedron: | ||
443 | case Prism: | ||
444 | case Pyramid: | ||
445 | |||
446 | ✗ | FEL_ERROR("Not implemented for this element Shape") | |
447 | ✗ | break; | |
448 | } | ||
449 | 432 | } | |
450 | |||
451 | /***********************************************************************************/ | ||
452 | /***********************************************************************************/ | ||
453 | |||
454 | 7439 | void GeoUtilities::triNormal(const Point& p0, const Point& p1, const Point& p2, Point& norm) | |
455 | { | ||
456 |
5/10✓ Branch 2 taken 7439 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7439 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7439 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7439 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7439 times.
✗ Branch 15 not taken.
|
7439 | MathUtilities::CrossProduct(norm.getCoor(), (p1 - p0).getCoor(), (p2 - p0).getCoor()); |
457 | 7439 | return; | |
458 | } | ||
459 | |||
460 | /***********************************************************************************/ | ||
461 | /***********************************************************************************/ | ||
462 | |||
463 | 195 | double GeoUtilities::tetraVolume(const Point& p0, const Point& p1, const Point& p2, const Point& p3) | |
464 | { | ||
465 | 195 | Point cross(0.); | |
466 | |||
467 |
1/2✓ Branch 1 taken 195 times.
✗ Branch 2 not taken.
|
195 | Point p0p1( p1 - p0 ); |
468 |
1/2✓ Branch 1 taken 195 times.
✗ Branch 2 not taken.
|
195 | Point p0p2( p2 - p0 ); |
469 |
1/2✓ Branch 1 taken 195 times.
✗ Branch 2 not taken.
|
195 | Point p0p3( p3 - p0 ); |
470 | |||
471 |
4/8✓ Branch 1 taken 195 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 195 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 195 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 195 times.
✗ Branch 11 not taken.
|
195 | MathUtilities::CrossProduct(cross.getCoor(), p0p2.getCoor(), p0p3.getCoor()); |
472 | |||
473 |
3/6✓ Branch 1 taken 195 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 195 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 195 times.
✗ Branch 10 not taken.
|
195 | return std::inner_product(p0p1.getCoor().begin(), p0p1.getCoor().end(), cross.getCoor().begin(), 0.) / 6.; |
474 | } | ||
475 | |||
476 | /***********************************************************************************/ | ||
477 | /***********************************************************************************/ | ||
478 | |||
479 | 22317 | double GeoUtilities::triVolume(const Point& p0, const Point& p1, const Point& p2, const Point& norm) | |
480 | { | ||
481 | Point cross; | ||
482 |
6/12✓ Branch 1 taken 22317 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 22317 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 22317 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 22317 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 22317 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 22317 times.
✗ Branch 17 not taken.
|
22317 | MathUtilities::CrossProduct(cross.getCoor(), (p1 - p0).getCoor(), (p2 - p0).getCoor()); |
483 | |||
484 | 22317 | return 0.5 * (cross * norm); | |
485 | } | ||
486 | |||
487 | /***********************************************************************************/ | ||
488 | /***********************************************************************************/ | ||
489 | |||
490 | 39 | void GeoUtilities::tetraBary(const std::vector<Point*>& eltPoints, const Point& point, std::array<double, 4>& bary) | |
491 | { | ||
492 | // bary.resize(4); | ||
493 | |||
494 | 39 | bary[0] = tetraVolume(*eltPoints[0], *eltPoints[1], *eltPoints[2], point); | |
495 | 39 | bary[1] = tetraVolume(*eltPoints[0], *eltPoints[3], *eltPoints[1], point); | |
496 | 39 | bary[2] = tetraVolume(*eltPoints[1], *eltPoints[3], *eltPoints[2], point); | |
497 | 39 | bary[3] = tetraVolume(*eltPoints[0], *eltPoints[2], *eltPoints[3], point); | |
498 | |||
499 | 39 | return; | |
500 | } | ||
501 | |||
502 | /***********************************************************************************/ | ||
503 | /***********************************************************************************/ | ||
504 | |||
505 | 7439 | void GeoUtilities::triBary(const std::vector<Point*>& eltPoints, const Point& point, const Point& norm, std::array<double, 3>& bary) | |
506 | { | ||
507 | // bary.resize(4); | ||
508 | |||
509 | 7439 | bary[0] = triVolume(*eltPoints[0], *eltPoints[1], point, norm); | |
510 | 7439 | bary[1] = triVolume(*eltPoints[1], *eltPoints[2], point, norm); | |
511 | 7439 | bary[2] = triVolume(*eltPoints[2], *eltPoints[0], point, norm); | |
512 | |||
513 | 7439 | return; | |
514 | } | ||
515 | |||
516 | /***********************************************************************************/ | ||
517 | /***********************************************************************************/ | ||
518 | |||
519 | 52 | void GeoUtilities::projectOnGeomEntity(const felInt dim, const std::vector<Point*>& eltPoints, const Point& point, Point& projection) | |
520 | { | ||
521 |
2/3✓ Branch 0 taken 16 times.
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
|
52 | switch ( dim ) { |
522 | |||
523 | 16 | case 2 : | |
524 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 16 times.
|
16 | FEL_ASSERT( eltPoints.size() >= 2 ); |
525 | 16 | GeoUtilities::projectOnLine(eltPoints, point, projection); | |
526 | 16 | break; | |
527 | |||
528 | 36 | case 3 : | |
529 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 36 times.
|
36 | FEL_ASSERT( eltPoints.size() >= 3 ); |
530 | 36 | GeoUtilities::projectOnPlane(eltPoints, point, projection); | |
531 | 36 | break; | |
532 | |||
533 | ✗ | default: | |
534 | ✗ | FEL_ERROR("Not implemented yet!"); | |
535 | ✗ | break; | |
536 | } | ||
537 | 52 | } | |
538 | |||
539 | /***********************************************************************************/ | ||
540 | /***********************************************************************************/ | ||
541 | |||
542 | 33 | void GeoUtilities::projectOnLine(const std::vector<Point*>& eltPoints, const Point& point, Point& projection) | |
543 | { | ||
544 |
1/2✓ Branch 3 taken 33 times.
✗ Branch 4 not taken.
|
33 | Point edge = *eltPoints[1] - *eltPoints[0]; |
545 |
1/2✓ Branch 2 taken 33 times.
✗ Branch 3 not taken.
|
33 | Point vec0 = point - *eltPoints[0]; |
546 | |||
547 | // Normaliza edge | ||
548 |
1/2✓ Branch 1 taken 33 times.
✗ Branch 2 not taken.
|
33 | edge /= edge.norm(); |
549 | |||
550 | // Compute projection | ||
551 |
1/2✓ Branch 4 taken 33 times.
✗ Branch 5 not taken.
|
33 | projection = *eltPoints[0] + (edge * vec0) * edge; |
552 | 33 | } | |
553 | |||
554 | /***********************************************************************************/ | ||
555 | /***********************************************************************************/ | ||
556 | |||
557 | 36 | void GeoUtilities::projectOnPlane(const std::vector<Point*>& eltPoints, const Point& point, Point& projection) | |
558 | { | ||
559 | // Compute plane normal | ||
560 | Point normal; | ||
561 |
6/12✓ Branch 3 taken 36 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 36 times.
✗ Branch 7 not taken.
✓ Branch 11 taken 36 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 36 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 36 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 36 times.
✗ Branch 21 not taken.
|
36 | MathUtilities::CrossProduct(normal.getCoor(), (*eltPoints[0] - *eltPoints[1]).getCoor(), (*eltPoints[0] - *eltPoints[2]).getCoor()); |
562 |
1/2✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
|
36 | normal *= 1. / normal.norm(); |
563 | |||
564 | // Projection of the point on the plane | ||
565 |
1/2✓ Branch 2 taken 36 times.
✗ Branch 3 not taken.
|
36 | double coeff = normal * ( point - (*eltPoints[0]) ); |
566 | |||
567 | // Compute the coordinates of the point projected on the plane | ||
568 |
1/2✓ Branch 2 taken 36 times.
✗ Branch 3 not taken.
|
36 | projection = point - normal * coeff; |
569 | 36 | } | |
570 | |||
571 | /***********************************************************************************/ | ||
572 | /***********************************************************************************/ | ||
573 | |||
574 | 2101 | int GeoUtilities::computeSegmentsIntersection(const Point& P0, const Point& P1, const Point& Q0, const Point& Q1, Point& intPnt0, Point& intPnt1, const double tol) | |
575 | { | ||
576 | // computeSegmentsIntersection(): find the intersection of 2 finite segments | ||
577 | // Input: two finite segments S1 and S2, made respectively of p0Seg and P1 for the first segment (given in pointors) and of Q0 and Q1 for the second segment (given in pointors) | ||
578 | // Output: intPnt0 = intersection point (when it exists) (given in pointor) | ||
579 | // intPnt1 = endpoint of intersection segment [made of intPnt0 and intPnt1] (when it exists) (given in pointor) | ||
580 | // Return: 0 = disjoint (no intersection) | ||
581 | // 1 = intersect in unique point intPnt0 | ||
582 | // 2 = overlap in segment from intPnt0 to intPnt1 | ||
583 | |||
584 | std::array<double, 2> bary; | ||
585 | |||
586 | Point Crss, Ctmp, Ctmp1, Ctmp2; | ||
587 |
1/2✓ Branch 1 taken 2101 times.
✗ Branch 2 not taken.
|
2101 | const Point Seg0 = P1 - P0; |
588 |
1/2✓ Branch 1 taken 2101 times.
✗ Branch 2 not taken.
|
2101 | const Point Seg1 = Q1 - Q0; |
589 |
1/2✓ Branch 1 taken 2101 times.
✗ Branch 2 not taken.
|
2101 | const Point Q0P0 = P0 - Q0; |
590 | |||
591 | // Test if they are parallel (includes either being a point) | ||
592 |
4/8✓ Branch 1 taken 2101 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2101 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2101 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2101 times.
✗ Branch 11 not taken.
|
2101 | MathUtilities::CrossProduct(Crss.getCoor(), Seg0.getCoor(), Seg1.getCoor()); |
593 | |||
594 |
1/2✓ Branch 1 taken 2101 times.
✗ Branch 2 not taken.
|
2101 | const double cnorm = Crss.norm(); |
595 |
1/2✓ Branch 1 taken 2101 times.
✗ Branch 2 not taken.
|
2101 | const double lSeg0 = Seg0.norm(); |
596 |
1/2✓ Branch 1 taken 2101 times.
✗ Branch 2 not taken.
|
2101 | const double lSeg1 = Seg1.norm(); |
597 | |||
598 | // Check if they are degenerate points: | ||
599 | // both segments are points | ||
600 |
1/4✗ Branch 0 not taken.
✓ Branch 1 taken 2101 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
|
2101 | if ( lSeg0 < tol && lSeg1 < tol ) { |
601 | |||
602 | // They are distinct points | ||
603 | ✗ | if ( Q0P0.norm() > tol ) | |
604 | ✗ | return 0; | |
605 | |||
606 | // They are the same point | ||
607 | ✗ | intPnt0 = P0; | |
608 | ✗ | return 1; | |
609 | } | ||
610 | |||
611 | // S1 is a single point | ||
612 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 2101 times.
|
2101 | if ( lSeg0 < tol ) { |
613 | // but is not in S2 | ||
614 | Point proj; | ||
615 | ✗ | GeoUtilities::projectOnLine( {const_cast<Point*>(&Q0), const_cast<Point*>(&Q1)}, P0, proj); | |
616 | |||
617 | ✗ | if ( P0.dist(proj) < tol && GeoUtilities::isPointInSeg( {const_cast<Point*>(&Q0), const_cast<Point*>(&Q1)}, proj, bary, tol) ) {// TODO don't like const_cast | |
618 | ✗ | intPnt0 = P0; | |
619 | ✗ | return 1; | |
620 | } | ||
621 | ✗ | return 0; | |
622 | } | ||
623 | |||
624 | // S2 a single point | ||
625 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 2101 times.
|
2101 | if ( lSeg1 < tol ) { |
626 | // but is not in S1 | ||
627 | Point proj; | ||
628 | ✗ | GeoUtilities::projectOnLine( {const_cast<Point*>(&P0), const_cast<Point*>(&P1)}, Q0, proj); | |
629 | |||
630 | ✗ | if ( Q0.dist(proj) < tol && GeoUtilities::isPointInSeg({const_cast<Point*>(&P0), const_cast<Point*>(&P1)}, proj, bary, tol) ) {// TODO don't like const_cast | |
631 | ✗ | intPnt0 = Q0; | |
632 | ✗ | return 1; | |
633 | } | ||
634 | |||
635 | ✗ | return 0; | |
636 | } | ||
637 | |||
638 | // The two segments are parallel or collinear | ||
639 |
2/2✓ Branch 0 taken 400 times.
✓ Branch 1 taken 1701 times.
|
2101 | if ( cnorm < tol * lSeg0 * lSeg1 ) { |
640 | |||
641 | // They are NOT collinear | ||
642 |
4/8✓ Branch 1 taken 400 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 400 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 400 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 400 times.
✗ Branch 11 not taken.
|
400 | MathUtilities::CrossProduct(Ctmp1.getCoor(), Seg0.getCoor(), Q0P0.getCoor()); |
643 |
4/8✓ Branch 1 taken 400 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 400 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 400 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 400 times.
✗ Branch 11 not taken.
|
400 | MathUtilities::CrossProduct(Ctmp2.getCoor(), Seg1.getCoor(), Q0P0.getCoor()); |
644 |
4/14✓ Branch 1 taken 400 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 400 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 400 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✓ Branch 16 taken 400 times.
✗ Branch 17 not taken.
|
400 | if ( Ctmp1.norm() > tol*lSeg0*Q0P0.norm() || Ctmp2.norm() > tol*lSeg1*Q0P0.norm() ) |
645 | 400 | return 0; | |
646 | |||
647 | // They are collinear segments | ||
648 | double t0, t1; | ||
649 | |||
650 | // Compute position of P0 and P1 on Seg1 | ||
651 | ✗ | t0 = ( P0 - Q0 ) * Seg1 / ( lSeg1 * lSeg1 ); | |
652 | ✗ | t1 = ( P1 - Q0 ) * Seg1 / ( lSeg1 * lSeg1 ); | |
653 | |||
654 | // Reorient ( as if Seg0 and Seg1 were pointing in the same direction ) | ||
655 | ✗ | if ( t0 > t1 ) | |
656 | ✗ | std::swap(t0,t1); | |
657 | |||
658 | // No overlap | ||
659 | ✗ | if ( ( t0 > 1. + tol ) || ( t1 < - tol ) ) | |
660 | ✗ | return 0; | |
661 | |||
662 | // Clip intersected segment | ||
663 | ✗ | t0 = t0 < - tol ? 0 : t0; | |
664 | ✗ | t1 = t1 > 1. + tol ? 1 : t1; | |
665 | |||
666 | // The two segments overlap in a valid subsegment | ||
667 | ✗ | intPnt0 = Q0 + t0 * Seg1; | |
668 | ✗ | intPnt1 = Q0 + t1 * Seg1; | |
669 | ✗ | return 2; | |
670 | } | ||
671 | |||
672 | // The two segments are not in the same plane | ||
673 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 1701 times.
|
1701 | if ( std::fabs(Crss*Q0P0) > tol*cnorm ) |
674 | ✗ | return 0; | |
675 | |||
676 | // The segments are skew and may intersect in a point | ||
677 | // Compute the intersect parameter for the first segment | ||
678 |
4/8✓ Branch 1 taken 1701 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1701 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1701 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1701 times.
✗ Branch 11 not taken.
|
1701 | MathUtilities::CrossProduct(Ctmp.getCoor(), Seg1.getCoor(), Q0P0.getCoor()); |
679 | 1701 | const double sI = ( Ctmp * Crss ) / ( cnorm * cnorm ); | |
680 | |||
681 | // No intersect with the first segment | ||
682 |
4/4✓ Branch 0 taken 1101 times.
✓ Branch 1 taken 600 times.
✓ Branch 2 taken 50 times.
✓ Branch 3 taken 1051 times.
|
1701 | if ( sI < - tol || sI > 1. + tol ) |
683 | 650 | return 0; | |
684 | |||
685 | // Compute the intersect parameter for the second segment | ||
686 |
4/8✓ Branch 1 taken 1051 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1051 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1051 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1051 times.
✗ Branch 11 not taken.
|
1051 | MathUtilities::CrossProduct(Ctmp.getCoor(), Seg0.getCoor(), Q0P0.getCoor()); |
687 | 1051 | const double tI = ( Ctmp * Crss ) / ( cnorm * cnorm ); | |
688 | |||
689 | // No intersect with the second segment | ||
690 |
2/4✓ Branch 0 taken 1051 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 1051 times.
|
1051 | if ( tI < - tol || tI > 1. + tol ) |
691 | ✗ | return 0; | |
692 | |||
693 | // Compute the intersect point with the first segment | ||
694 |
1/2✓ Branch 2 taken 1051 times.
✗ Branch 3 not taken.
|
1051 | intPnt0 = P0 + sI * Seg0; |
695 | |||
696 | 1051 | return 1; | |
697 | } | ||
698 | |||
699 | /***********************************************************************************/ | ||
700 | /***********************************************************************************/ | ||
701 | |||
702 | namespace { | ||
703 | |||
704 | 15 | int computeRayOrSegWithTriangleOrPlaneIntersection(const Point& P0, const Point& P1, const Point& T0, const Point& T1, const Point& T2, Point& intPnt, bool isSeg, bool isTri, const double tol) | |
705 | { | ||
706 | // computeRayOrSegWithTriangleOrPlaneIntersection(): find the 3D intersection of a ray/segment with a triangle/plane | ||
707 | // Input: a ray R made of two points P0 and P1, and a triangle T made of three points T0, T1 and T2 | ||
708 | // Output: intPnt = intersection point (when it exists) | ||
709 | // Return: -1 = triangle is degenerate (a segment or point but not a triangle) | ||
710 | // 0 = disjoint (no intersection) | ||
711 | // 1 = intersection in unique point given in intPnt | ||
712 | // 2 = are in the same plane | ||
713 | |||
714 | std::array<double, 3> bary; | ||
715 | |||
716 | Point Crss; | ||
717 |
1/2✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
|
15 | const Point Seg = P1 - P0; |
718 |
1/2✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
|
15 | const Point Edg0 = T1 - T0; |
719 |
1/2✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
|
15 | const Point Edg1 = T2 - T0; |
720 |
1/2✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
|
15 | const double lSeg = Seg.norm(); |
721 |
1/2✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
|
15 | const double lEdg0 = Edg0.norm(); |
722 |
1/2✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
|
15 | const double lEdg1 = Edg1.norm(); |
723 | |||
724 | // Segment is degenerate | ||
725 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 15 times.
|
15 | if ( lSeg < tol ) |
726 | ✗ | return -1; | |
727 | |||
728 | // We compute the normal of the triangle | ||
729 |
4/8✓ 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.
|
15 | MathUtilities::CrossProduct(Crss.getCoor(), Edg0.getCoor(), Edg1.getCoor()); |
730 | |||
731 | // Triangle is degenerate | ||
732 |
2/4✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 15 times.
|
15 | if ( Crss.norm() < tol * lEdg0 * lEdg1 ) |
733 | ✗ | return -1; | |
734 | |||
735 | // Normalize the normal vector | ||
736 |
1/2✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
|
15 | Crss /= Crss.norm(); |
737 | |||
738 |
1/2✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
|
15 | const double N = Crss * ( T0 - P0 ); |
739 | 15 | const double D = Crss * Seg; | |
740 | |||
741 | // Ray is parallel to the plane | ||
742 |
2/2✓ Branch 0 taken 10 times.
✓ Branch 1 taken 5 times.
|
15 | if ( std::fabs(D) < tol * lSeg ) { |
743 | // Ray lies in the plane | ||
744 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 10 times.
|
10 | if ( std::fabs(N) < tol * lEdg0 * lEdg1 ) |
745 | ✗ | return 2; | |
746 | // Ray disjoint from the plane | ||
747 | else | ||
748 | 10 | return 0; | |
749 | } | ||
750 | |||
751 | // Get intersect point of ray with plane | ||
752 | 5 | const double R = N / D; | |
753 | |||
754 | // If the intersection is done with a ray (= infinite segment) | ||
755 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 5 times.
|
5 | if ( !isSeg ) { |
756 | ✗ | if ( R < - tol ) | |
757 | // No intersection | ||
758 | ✗ | return 0; | |
759 | } | ||
760 | // If the intersection is done with a finite segment | ||
761 | else { | ||
762 |
2/4✓ Branch 0 taken 5 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 5 times.
|
5 | if ( ( R < - tol ) || ( R > 1 + tol ) ) { |
763 | // No intersection | ||
764 | ✗ | return 0; | |
765 | } | ||
766 | } | ||
767 | |||
768 | // Compute intersection point | ||
769 |
1/2✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
|
5 | intPnt = P0 + R * Seg; |
770 | |||
771 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 5 times.
|
5 | if ( !isTri ) |
772 | ✗ | return 1; | |
773 | |||
774 | // is intPnt inside the triangle? | ||
775 |
3/6✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 5 times.
✗ Branch 10 not taken.
|
5 | if ( GeoUtilities::isPointInTri({const_cast<Point*>(&T0),const_cast<Point*>(&T1),const_cast<Point*>(&T2)}, intPnt, bary, tol) ) // TODO don't like const_cast |
776 | 5 | return 1; | |
777 | |||
778 | ✗ | return 0; | |
779 | } | ||
780 | |||
781 | } | ||
782 | |||
783 | /***********************************************************************************/ | ||
784 | /***********************************************************************************/ | ||
785 | |||
786 | ✗ | int GeoUtilities::computeRayTriangleIntersection(const Point& P0, const Point& P1, const Point& T0, const Point& T1, const Point& T2, Point& intPnt, const double tol) | |
787 | { | ||
788 | ✗ | return computeRayOrSegWithTriangleOrPlaneIntersection(P0, P1, T0, T1, T2, intPnt, false, true, tol); | |
789 | } | ||
790 | |||
791 | /***********************************************************************************/ | ||
792 | /***********************************************************************************/ | ||
793 | |||
794 | 15 | int GeoUtilities::computeSegmentTriangleIntersection(const Point& P0, const Point& P1, const Point& T0, const Point& T1, const Point& T2, Point& intPnt, const double tol) | |
795 | { | ||
796 | 15 | return computeRayOrSegWithTriangleOrPlaneIntersection(P0, P1, T0, T1, T2, intPnt, true, true, tol); | |
797 | } | ||
798 | |||
799 | /***********************************************************************************/ | ||
800 | /***********************************************************************************/ | ||
801 | |||
802 | ✗ | int GeoUtilities::computeRayPlaneIntersection(const Point& P0, const Point& P1, const Point& T0, const Point& T1, const Point& T2, Point& intPnt, const double tol) | |
803 | { | ||
804 | ✗ | return computeRayOrSegWithTriangleOrPlaneIntersection(P0, P1, T0, T1, T2, intPnt, false, false, tol); | |
805 | } | ||
806 | |||
807 | /***********************************************************************************/ | ||
808 | /***********************************************************************************/ | ||
809 | |||
810 | ✗ | int GeoUtilities::computeSegmentPlaneIntersection(const Point& P0, const Point& P1, const Point& T0, const Point& T1, const Point& T2, Point& intPnt, const double tol) | |
811 | { | ||
812 | ✗ | return computeRayOrSegWithTriangleOrPlaneIntersection(P0, P1, T0, T1, T2, intPnt, true, false, tol); | |
813 | } | ||
814 | |||
815 | /***********************************************************************************/ | ||
816 | /***********************************************************************************/ | ||
817 | |||
818 | ✗ | int GeoUtilities::computePlanesIntersection(const Point& P0, const Point& P1, const Point& P2, const Point& Q0, const Point& Q1, const Point& Q2, Point& p0Line, Point& p1Line, const double tol) | |
819 | { | ||
820 | // computePlanesIntersection(): find the 3D intersection of two planes | ||
821 | // Input: two planes P (= plane0) and Q (= plane1) defined respectively by P0, P1 and P2 for the first plane (given in pointors) and Q0, Q1 and Q2 for the second plane | ||
822 | // Output: p0Line and p1Line = the intersection line (when it exists) defined by the two points p0Line and p1Line | ||
823 | // Return: 0 = disjoint (no intersection point) | ||
824 | // 1 = the two planes coincide | ||
825 | // 2 = intersection in the unique line L defined by p0Line and p1Line | ||
826 | |||
827 | Point normalP, normalQ, intPnt, crss; | ||
828 | |||
829 | // we compute the normal of the first plane | ||
830 | ✗ | MathUtilities::CrossProduct(normalP.getCoor(), (P1-P0).getCoor(), (P2-P0).getCoor()); | |
831 | |||
832 | // we compute the normal of the second plane | ||
833 | ✗ | MathUtilities::CrossProduct(normalQ.getCoor(), (Q1-Q0).getCoor(), (Q2-Q0).getCoor()); | |
834 | |||
835 | // we compute the cross product of the two normals | ||
836 | ✗ | MathUtilities::CrossProduct(crss.getCoor(), normalP.getCoor(), normalQ.getCoor()); | |
837 | |||
838 | // test if the two planes are parallel | ||
839 | ✗ | if ( crss.norm() < tol ) { | |
840 | |||
841 | // test if disjoint or coincide | ||
842 | ✗ | if ( std::fabs( normalP * (Q0 - P0) ) < tol ) | |
843 | // plane0 and plane1 coincide | ||
844 | ✗ | return 1; | |
845 | else | ||
846 | // plane0 and plane1 are disjoint | ||
847 | ✗ | return 0; | |
848 | } | ||
849 | |||
850 | // plane0 and plane1 intersect in a line | ||
851 | ✗ | auto& cpCoor = crss.getCoor(); | |
852 | ✗ | const int maxc = std::distance(cpCoor.begin(), std::max_element(cpCoor.begin(), cpCoor.end(), [](double a, double b) { return std::fabs(a) < std::fabs(b); } ) ); | |
853 | |||
854 | // next, to get a point on the intersect line | ||
855 | // zero the max coord, and solve for the other two | ||
856 | ✗ | const double d1 = - ( normalP * P0 ); | |
857 | ✗ | const double d2 = - ( normalQ * Q0 ); | |
858 | |||
859 | // select max coordinate | ||
860 | ✗ | switch ( maxc ) { | |
861 | |||
862 | // intersect with x = 0 | ||
863 | ✗ | case 0: | |
864 | ✗ | intPnt.coor(0) = 0; | |
865 | ✗ | intPnt.coor(1) = ( + d2*normalP.coor(2) - d1*normalQ.coor(2) ) / crss.coor(0); | |
866 | ✗ | intPnt.coor(2) = ( - d2*normalP.coor(1) + d1*normalQ.coor(1) ) / crss.coor(0); | |
867 | ✗ | break; | |
868 | |||
869 | // intersect with y = 0 | ||
870 | ✗ | case 1: | |
871 | ✗ | intPnt.coor(0) = ( - d2*normalP.coor(2) + d1*normalQ.coor(2) ) / crss.coor(1); | |
872 | ✗ | intPnt.coor(1) = 0; | |
873 | ✗ | intPnt.coor(2) = ( + d2*normalP.coor(0) - d1*normalQ.coor(0) ) / crss.coor(1); | |
874 | ✗ | break; | |
875 | |||
876 | // intersect with z = 0 | ||
877 | ✗ | case 2: | |
878 | ✗ | intPnt.coor(0) = ( + d2*normalP.coor(1) - d1*normalQ.coor(1) ) / crss.coor(2); | |
879 | ✗ | intPnt.coor(1) = ( - d2*normalP.coor(0) + d1*normalQ.coor(0) ) / crss.coor(2); | |
880 | ✗ | intPnt.coor(2) = 0; | |
881 | ✗ | break; | |
882 | } | ||
883 | |||
884 | //L.P0 = iP; | ||
885 | //L.P1 = iP + u; | ||
886 | ✗ | p0Line = intPnt; | |
887 | ✗ | p1Line = intPnt + crss; | |
888 | |||
889 | ✗ | return 2; | |
890 | } | ||
891 | |||
892 | /***********************************************************************************/ | ||
893 | /***********************************************************************************/ | ||
894 | |||
895 | ✗ | void GeoUtilities::findMatchingPoints(GeometricMeshRegion* mesh1, const std::vector<int>& label1, GeometricMeshRegion* mesh2, const std::vector<int>& label2, std::map<felInt,felInt>& link, const double tol) | |
896 | { | ||
897 | ✗ | std::set<felInt> list1, list2; | |
898 | |||
899 | // Extract list of point from mesh1 | ||
900 | ✗ | mesh1->extractVerticesFromBoundary(label1, list1); | |
901 | |||
902 | // Extract list of point from mesh2 | ||
903 | ✗ | mesh2->extractVerticesFromBoundary(label2, list2); | |
904 | |||
905 | // Create map | ||
906 | ✗ | auto& listPoints1 = mesh1->listPoints(); | |
907 | ✗ | auto& listPoints2 = mesh2->listPoints(); | |
908 | ✗ | for (auto it1 = list1.begin(); it1 != list1.end(); ++it1) { | |
909 | ✗ | Point& point1 = listPoints1[*it1]; | |
910 | |||
911 | ✗ | for (auto it2 = list2.begin(); it2 != list2.end(); ++it2) { | |
912 | ✗ | Point& point2 = listPoints2[*it2]; | |
913 | |||
914 | ✗ | if ( point1.dist(point2) < tol ) | |
915 | ✗ | link.insert(std::make_pair(*it1, *it2)); | |
916 | } | ||
917 | } | ||
918 | } | ||
919 | |||
920 | } | ||
921 |