Line data Source code
1 : //* This file is part of the MOOSE framework
2 : //* https://mooseframework.inl.gov
3 : //*
4 : //* All rights reserved, see COPYRIGHT for full restrictions
5 : //* https://github.com/idaholab/moose/blob/master/COPYRIGHT
6 : //*
7 : //* Licensed under LGPL 2.1, please see LICENSE for details
8 : //* https://www.gnu.org/licenses/lgpl-2.1.html
9 :
10 : #include "GeometryUtils.h"
11 : #include "MooseUtils.h"
12 :
13 : #include <algorithm>
14 : #include <cmath>
15 : #include <limits>
16 :
17 : namespace geom_utils
18 : {
19 :
20 : bool
21 4 : isPointZero(const Point & pt)
22 : {
23 4 : const Point zero(0.0, 0.0, 0.0);
24 8 : return pt.absolute_fuzzy_equals(zero);
25 : }
26 :
27 : Point
28 4 : unitVector(const Point & pt, const std::string & name)
29 : {
30 4 : if (isPointZero(pt))
31 6 : mooseError("'" + name + "' cannot have zero norm!");
32 :
33 2 : return pt.unit();
34 : }
35 :
36 : Real
37 0 : minDistanceToPoints(const Point & pt,
38 : const std::vector<Point> & candidates,
39 : const unsigned int axis)
40 : {
41 0 : const auto idx = projectedIndices(axis);
42 :
43 0 : Real distance = std::numeric_limits<Real>::max();
44 0 : for (const auto & c : candidates)
45 : {
46 0 : const Real dx = c(idx.first) - pt(idx.first);
47 0 : const Real dy = c(idx.second) - pt(idx.second);
48 0 : const Real d = std::sqrt(dx * dx + dy * dy);
49 0 : distance = std::min(d, distance);
50 : }
51 :
52 0 : return distance;
53 : }
54 :
55 : Point
56 24 : projectPoint(const Real x0, const Real x1, const unsigned int axis)
57 : {
58 24 : const auto i = projectedIndices(axis);
59 24 : Point point;
60 24 : point(i.first) = x0;
61 24 : point(i.second) = x1;
62 24 : point(axis) = 0.0;
63 :
64 48 : return point;
65 : }
66 :
67 : Real
68 102 : projectedLineHalfSpace(Point pt1, Point pt2, Point pt3, const unsigned int axis)
69 : {
70 : // project points onto plane perpendicular to axis
71 102 : pt1(axis) = 0.0;
72 102 : pt2(axis) = 0.0;
73 102 : pt3(axis) = 0.0;
74 :
75 102 : const auto i = projectedIndices(axis);
76 :
77 102 : return (pt1(i.first) - pt3(i.first)) * (pt2(i.second) - pt3(i.second)) -
78 204 : (pt2(i.first) - pt3(i.first)) * (pt1(i.second) - pt3(i.second));
79 : }
80 :
81 : bool
82 26 : pointInPolygon(const Point & point, const std::vector<Point> & corners, const unsigned int axis)
83 : {
84 26 : const auto n_pts = corners.size();
85 :
86 26 : std::vector<bool> negative_half_space;
87 26 : std::vector<bool> positive_half_space;
88 124 : for (const auto i : index_range(corners))
89 : {
90 98 : const int next = (i == n_pts - 1) ? 0 : i + 1;
91 98 : const auto half = projectedLineHalfSpace(point, corners[i], corners[next], axis);
92 98 : negative_half_space.push_back(half < 0);
93 98 : positive_half_space.push_back(half > 0);
94 : }
95 :
96 26 : const bool negative = std::find(negative_half_space.begin(), negative_half_space.end(), true) !=
97 26 : negative_half_space.end();
98 26 : const bool positive = std::find(positive_half_space.begin(), positive_half_space.end(), true) !=
99 26 : positive_half_space.end();
100 :
101 26 : const bool in_polygon = !(negative && positive);
102 26 : if (in_polygon)
103 16 : return true;
104 :
105 10 : if (pointOnEdge(point, corners, axis))
106 0 : return true;
107 :
108 10 : return false;
109 26 : }
110 :
111 : bool
112 10 : pointOnEdge(const Point & point, const std::vector<Point> & corners, const unsigned int axis)
113 : {
114 10 : const auto n_pts = corners.size();
115 10 : const auto idx = projectedIndices(axis);
116 :
117 10 : constexpr Real tol = 1e-8;
118 48 : for (const auto i : index_range(corners))
119 : {
120 38 : const int next = (i == n_pts - 1) ? 0 : i + 1;
121 38 : const auto & pt1 = corners[i];
122 38 : const auto & pt2 = corners[next];
123 38 : const bool close_to_line = projectedDistanceFromLine(point, pt1, pt2, axis) < tol;
124 :
125 : // we can stop early if we know we're not close to the line
126 38 : if (!close_to_line)
127 34 : continue;
128 :
129 : // check that the point is "between" the two points; TODO: first pass
130 : // we can just compare x and y coordinates
131 4 : const bool between_points = (point(idx.first) >= std::min(pt1(idx.first), pt2(idx.first))) &&
132 2 : (point(idx.first) <= std::max(pt1(idx.first), pt2(idx.first))) &&
133 6 : (point(idx.second) >= std::min(pt1(idx.second), pt2(idx.second))) &&
134 0 : (point(idx.second) <= std::max(pt1(idx.second), pt2(idx.second)));
135 :
136 : // point needs to be close to the line AND "between" the two points
137 4 : if (close_to_line && between_points)
138 0 : return true;
139 : }
140 :
141 10 : return false;
142 : }
143 :
144 : std::pair<unsigned int, unsigned int>
145 138 : projectedIndices(const unsigned int axis)
146 : {
147 138 : std::pair<unsigned int, unsigned int> indices;
148 :
149 138 : if (axis == 0)
150 : {
151 0 : indices.first = 1;
152 0 : indices.second = 2;
153 : }
154 138 : else if (axis == 1)
155 : {
156 0 : indices.first = 0;
157 0 : indices.second = 2;
158 : }
159 : else
160 : {
161 138 : indices.first = 0;
162 138 : indices.second = 1;
163 : }
164 :
165 138 : return indices;
166 : }
167 :
168 : Point
169 2 : projectedUnitNormal(Point pt1, Point pt2, const unsigned int axis)
170 : {
171 : // project the points to the plane perpendicular to the axis
172 2 : pt1(axis) = 0.0;
173 2 : pt2(axis) = 0.0;
174 :
175 2 : const auto i = projectedIndices(axis);
176 :
177 2 : const Real dx = pt2(i.first) - pt1(i.first);
178 2 : const Real dy = pt2(i.second) - pt1(i.second);
179 :
180 2 : const Point normal = projectPoint(dy, -dx, axis);
181 2 : const Point gap_line = pt2 - pt1;
182 :
183 2 : const auto cross_product = gap_line.cross(normal);
184 :
185 2 : if (cross_product(axis) > 0)
186 0 : return normal.unit();
187 : else
188 2 : return projectPoint(-dy, dx, axis).unit();
189 : }
190 :
191 : Real
192 56 : distanceFromLine(const Point & pt, const Point & line0, const Point & line1)
193 : {
194 56 : const Point a = pt - line0;
195 56 : const Point b = pt - line1;
196 56 : const Point c = line1 - line0;
197 :
198 56 : return (a.cross(b).norm()) / c.norm();
199 : }
200 :
201 : Real
202 44 : projectedDistanceFromLine(Point pt, Point line0, Point line1, const unsigned int axis)
203 : {
204 : // project all the points to the plane perpendicular to the axis
205 44 : pt(axis) = 0.0;
206 44 : line0(axis) = 0.0;
207 44 : line1(axis) = 0.0;
208 :
209 44 : return distanceFromLine(pt, line0, line1);
210 : }
211 :
212 : std::vector<Point>
213 4 : polygonCorners(const unsigned int num_sides, const Real radius, const unsigned int axis)
214 : {
215 4 : std::vector<Point> corners;
216 4 : const Real theta = 2.0 * M_PI / num_sides;
217 4 : const Real first_angle = M_PI / 2.0 - theta / 2.0;
218 :
219 24 : for (const auto i : make_range(num_sides))
220 : {
221 20 : const Real angle = first_angle + i * theta;
222 20 : const Real x = radius * cos(angle);
223 20 : const Real y = radius * sin(angle);
224 :
225 20 : corners.push_back(projectPoint(x, y, axis));
226 : }
227 :
228 4 : return corners;
229 0 : }
230 :
231 : Point
232 312 : rotatePointAboutAxis(const Point & p, const Real angle, const Point & axis)
233 : {
234 312 : const Real cos_theta = cos(angle);
235 312 : const Real sin_theta = sin(angle);
236 :
237 312 : Point pt;
238 312 : const Real xy = axis(0) * axis(1);
239 312 : const Real xz = axis(0) * axis(2);
240 312 : const Real yz = axis(1) * axis(2);
241 :
242 312 : const Point x_op(cos_theta + axis(0) * axis(0) * (1.0 - cos_theta),
243 624 : xy * (1.0 - cos_theta) - axis(2) * sin_theta,
244 624 : xz * (1.0 - cos_theta) + axis(1) * sin_theta);
245 :
246 624 : const Point y_op(xy * (1.0 - cos_theta) + axis(2) * sin_theta,
247 312 : cos_theta + axis(1) * axis(1) * (1.0 - cos_theta),
248 624 : yz * (1.0 - cos_theta) - axis(0) * sin_theta);
249 :
250 624 : const Point z_op(xz * (1.0 - cos_theta) - axis(1) * sin_theta,
251 624 : yz * (1.0 - cos_theta) + axis(0) * sin_theta,
252 312 : cos_theta + axis(2) * axis(2) * (1.0 - cos_theta));
253 :
254 312 : pt(0) = x_op * p;
255 312 : pt(1) = y_op * p;
256 312 : pt(2) = z_op * p;
257 624 : return pt;
258 : }
259 :
260 : std::vector<Point>
261 2 : boxCorners(const libMesh::BoundingBox & box, const Real factor)
262 : {
263 2 : Point diff = (box.max() - box.min()) / 2.0;
264 2 : const Point origin = box.min() + diff;
265 :
266 : // Rescale side length of box by specified factor
267 2 : diff *= factor;
268 :
269 : // Vectors for sides of box
270 2 : const Point dx(2.0 * diff(0), 0.0, 0.0);
271 2 : const Point dy(0.0, 2.0 * diff(1), 0.0);
272 2 : const Point dz(0.0, 0.0, 2.0 * diff(2));
273 :
274 2 : std::vector<Point> verts(8, origin - diff);
275 2 : const unsigned int pts_per_dim = 2;
276 6 : for (const auto z : make_range(pts_per_dim))
277 12 : for (const auto y : make_range(pts_per_dim))
278 24 : for (const auto x : make_range(pts_per_dim))
279 16 : verts[pts_per_dim * pts_per_dim * z + pts_per_dim * y + x] += x * dx + y * dy + z * dz;
280 :
281 4 : return verts;
282 0 : }
283 :
284 : bool
285 0 : arePointsColinear(const Point & p1, const Point & p2, const Point & p3)
286 : {
287 0 : const Point v1 = p2 - p1;
288 0 : const Point v2 = p3 - p1;
289 0 : const Point cross_prod = v1.cross(v2);
290 :
291 0 : return MooseUtils::absoluteFuzzyEqual(cross_prod.norm(), 0.0);
292 : }
293 :
294 : bool
295 0 : segmentsIntersect(const Point & p1, const Point & p2, const Point & p3, const Point & p4)
296 : {
297 : mooseAssert(
298 : MooseUtils::absoluteFuzzyEqual(p1(2), 0.0) && MooseUtils::absoluteFuzzyEqual(p2(2), 0.0) &&
299 : MooseUtils::absoluteFuzzyEqual(p3(2), 0.0) && MooseUtils::absoluteFuzzyEqual(p4(2), 0.0),
300 : "segmentsIntersect only works in 2D (x-y plane)");
301 :
302 : mooseAssert(MooseUtils::absoluteFuzzyGreaterThan((p1 - p2).norm(), 0.0) &&
303 : MooseUtils::absoluteFuzzyGreaterThan((p3 - p4).norm(), 0.0),
304 : "Zero length segments are not allowed in segmentsIntersect");
305 :
306 0 : const Real a1 = p2(1) - p1(1);
307 0 : const Real b1 = p1(0) - p2(0);
308 0 : const Real c1 = p2(0) * p1(1) - p1(0) * p2(1);
309 :
310 0 : const Real a2 = p4(1) - p3(1);
311 0 : const Real b2 = p3(0) - p4(0);
312 0 : const Real c2 = p4(0) * p3(1) - p3(0) * p4(1);
313 :
314 0 : const Real denom = a1 * b2 - a2 * b1;
315 0 : Point intersection_pt;
316 : // Parallel case
317 0 : if (MooseUtils::absoluteFuzzyEqual(denom, 0.0))
318 : {
319 0 : if (arePointsColinear(p1, p2, p3))
320 : {
321 : // for colinear segments, we construct a "virtual" intersection point using weighted average
322 : // In that case, the virtual point will always lie outside of both segments unless they
323 : // overlap
324 0 : const Point p12 = (p1 + p2) / 2.0;
325 0 : const Point p34 = (p3 + p4) / 2.0;
326 0 : const Real dist12 = (p1 - p2).norm();
327 0 : const Real dist34 = (p3 - p4).norm();
328 0 : intersection_pt = p12 + (p34 - p12) * (dist12 / (dist12 + dist34));
329 : }
330 : else
331 0 : return false;
332 : }
333 : else
334 : {
335 0 : intersection_pt = Point((b1 * c2 - b2 * c1) / denom, (a2 * c1 - a1 * c2) / denom, 0.0);
336 : }
337 :
338 0 : const Real ratio_p1p2 = (intersection_pt - p1) * (p2 - p1) / ((p2 - p1).norm_sq());
339 0 : const Real ratio_p3p4 = (intersection_pt - p3) * (p4 - p3) / ((p4 - p3).norm_sq());
340 :
341 0 : if (MooseUtils::absoluteFuzzyGreaterEqual(ratio_p1p2, 0.0) &&
342 0 : MooseUtils::absoluteFuzzyLessEqual(ratio_p1p2, 1.0) &&
343 0 : MooseUtils::absoluteFuzzyGreaterEqual(ratio_p3p4, 0.0) &&
344 0 : MooseUtils::absoluteFuzzyLessEqual(ratio_p3p4, 1.0))
345 0 : return true;
346 : else
347 0 : return false;
348 : }
349 :
350 : Real
351 24470 : pointSegmentDistanceSq(const Point & point, const Point & a, const Point & b)
352 : {
353 24470 : const Point ab = b - a;
354 24470 : const auto length_sq = ab.norm_sq();
355 24470 : if (length_sq <= std::numeric_limits<Real>::epsilon())
356 0 : return (point - a).norm_sq();
357 :
358 24470 : const auto t = std::clamp(((point - a) * ab) / length_sq, 0.0, 1.0);
359 24470 : const Point projection = a + t * ab;
360 24470 : return (point - projection).norm_sq();
361 : }
362 :
363 : Real
364 0 : pointTriangleDistanceSq(const Point & point, const Point & v0, const Point & v1, const Point & v2)
365 : {
366 0 : const Point ab = v1 - v0;
367 0 : const Point ac = v2 - v0;
368 0 : const Point ap = point - v0;
369 0 : const Real d1 = ab * ap;
370 0 : const Real d2 = ac * ap;
371 0 : if (d1 <= 0.0 && d2 <= 0.0)
372 0 : return (point - v0).norm_sq();
373 :
374 0 : const Point bp = point - v1;
375 0 : const Real d3 = ab * bp;
376 0 : const Real d4 = ac * bp;
377 0 : if (d3 >= 0.0 && d4 <= d3)
378 0 : return (point - v1).norm_sq();
379 :
380 0 : const Real vc = d1 * d4 - d3 * d2;
381 0 : if (vc <= 0.0 && d1 >= 0.0 && d3 <= 0.0)
382 : {
383 0 : const Real v = d1 / (d1 - d3);
384 0 : const Point projection = v0 + v * ab;
385 0 : return (point - projection).norm_sq();
386 : }
387 :
388 0 : const Point cp = point - v2;
389 0 : const Real d5 = ab * cp;
390 0 : const Real d6 = ac * cp;
391 0 : if (d6 >= 0.0 && d5 <= d6)
392 0 : return (point - v2).norm_sq();
393 :
394 0 : const Real vb = d5 * d2 - d1 * d6;
395 0 : if (vb <= 0.0 && d2 >= 0.0 && d6 <= 0.0)
396 : {
397 0 : const Real w = d2 / (d2 - d6);
398 0 : const Point projection = v0 + w * ac;
399 0 : return (point - projection).norm_sq();
400 : }
401 :
402 0 : const Real va = d3 * d6 - d5 * d4;
403 0 : if (va <= 0.0 && (d4 - d3) >= 0.0 && (d5 - d6) >= 0.0)
404 : {
405 0 : const Point bc = v2 - v1;
406 0 : const Real w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
407 0 : const Point projection = v1 + w * bc;
408 0 : return (point - projection).norm_sq();
409 : }
410 :
411 0 : const Real denom = 1.0 / (va + vb + vc);
412 0 : const Real v = vb * denom;
413 0 : const Real w = vc * denom;
414 0 : const Point projection = v0 + ab * v + ac * w;
415 0 : return (point - projection).norm_sq();
416 : }
417 :
418 : Real
419 3360 : solidAngle(const Point & point, const Point & v0, const Point & v1, const Point & v2)
420 : {
421 3360 : const Point a = v0 - point;
422 3360 : const Point b = v1 - point;
423 3360 : const Point c = v2 - point;
424 :
425 3360 : const Real la = a.norm();
426 3360 : const Real lb = b.norm();
427 3360 : const Real lc = c.norm();
428 :
429 3360 : const Real numerator = a * (b.cross(c));
430 3360 : const Real denominator = la * lb * lc + (a * b) * lc + (b * c) * la + (c * a) * lb;
431 :
432 6720 : return 2.0 * std::atan2(numerator, denominator);
433 : }
434 : } // end namespace geom_utils
|
