Line data Source code
1 : ! ***********************************************************************
2 : !
3 : ! Copyright (C) 2022 The MESA Team
4 : !
5 : ! This program is free software: you can redistribute it and/or modify
6 : ! it under the terms of the GNU Lesser General Public License
7 : ! as published by the Free Software Foundation,
8 : ! either version 3 of the License, or (at your option) any later version.
9 : !
10 : ! This program is distributed in the hope that it will be useful,
11 : ! but WITHOUT ANY WARRANTY; without even the implied warranty of
12 : ! MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
13 : ! See the GNU Lesser General Public License for more details.
14 : !
15 : ! You should have received a copy of the GNU Lesser General Public License
16 : ! along with this program. If not, see <https://www.gnu.org/licenses/>.
17 : !
18 : ! ***********************************************************************
19 :
20 : module eos_blend
21 : use const_def, only: dp
22 : use math_lib
23 : use auto_diff
24 : use eos_def
25 :
26 : implicit none
27 : private
28 : public :: is_contained, min_distance_to_polygon
29 :
30 : contains
31 :
32 : !! Determines which quadrant the given point is in.
33 : !!
34 : !! @param p The coordinates of the point (x,y).
35 : !! @param q The quadrant index.
36 3860072 : integer function quadrant(p) result(q)
37 : real(dp), intent(in) :: p(2)
38 :
39 3860072 : if (p(1) >= 0d0 .and. p(2) >= 0d0) then
40 : q = 1
41 2301872 : else if (p(1) < 0d0 .and. p(2) >= 0d0) then
42 : q = 2
43 1194344 : else if (p(1) < 0d0 .and. p(2) < 0d0) then
44 : q = 3
45 : else
46 918592 : q = 4
47 : end if
48 :
49 3860072 : end function quadrant
50 :
51 : !! Determines the winding number of a polygon around the origin.
52 : !!
53 : !! Implements the winding number algorithm of
54 : !! Moscato, Titolo, Feliu, and Munoz (https://shemesh.larc.nasa.gov/people/cam/publications/FM2019-draft.pdf)
55 : !!
56 : !! @param num_points The number of points specifying the polygon.
57 : !! @param coords The coordinates of the polygon. Given as an array of shape (num_coords, 2) storing (x,y) pairs.
58 : !! @param w The winding number of the polygon about the origin.
59 241257 : integer function winding_number(num_points, coords) result(w)
60 : integer, intent(in) :: num_points
61 : real(dp), intent(in) :: coords(num_points, 2)
62 1206285 : real(dp) :: determinant, x_start(2), x_end(2), dist(2)
63 : integer :: i, q_start, q_end, increment
64 :
65 : w = 0
66 2171293 : do i=1,num_points
67 5790108 : x_start = coords(i,1:2)
68 :
69 1930036 : if (i == num_points) then
70 723771 : x_end = coords(1,1:2)
71 : else
72 5066337 : x_end = coords(i+1,1:2)
73 : end if
74 :
75 1930036 : dist(1) = x_end(1) - x_start(1)
76 1930036 : dist(2) = x_end(2) - x_start(2)
77 :
78 1930036 : determinant = x_start(2) * dist(1) - x_start(1) * dist(2)
79 :
80 1930036 : q_start = quadrant(x_start)
81 1930036 : q_end = quadrant(x_end)
82 :
83 1930036 : if (q_start == q_end) then
84 : increment = 0
85 938260 : else if (q_end == mod(q_start,4) + 1) then
86 : increment = 1
87 448084 : else if (q_start == mod(q_end, 4) + 1) then
88 : increment = -1
89 26748 : else if (determinant < 0d0) then
90 : increment = 2
91 : else
92 0 : increment = -2
93 : end if
94 :
95 2171293 : w = w + increment
96 : end do
97 241257 : end function winding_number
98 :
99 : !! Determines if a point is contained within a polygon specified by connecting a sequence of points.
100 : !! This is done by checking the winding number of the polygon around the point.
101 : !!
102 : !! @param num_points The number of points specifying the polygon.
103 : !! @param coords The coordinates of the polygon. An array of shape (num_points, 2) storing (x,y) pairs.
104 : !! @param p The point whose distance to compute.
105 : !! @param contained Whether or not the point p is contained in the polygon.
106 241257 : logical function is_contained(num_points, coords, p) result(contained)
107 : integer, intent(in) :: num_points
108 : real(dp), intent(in) :: coords(num_points,2)
109 : type(auto_diff_real_2var_order1), intent(in) :: p(2)
110 :
111 : integer :: i, w
112 4583843 : real(dp) :: diff(num_points,2)
113 :
114 2171293 : do i=1,num_points
115 1930036 : diff(i,1) = coords(i,1) - p(1)%val
116 2171293 : diff(i,2) = coords(i,2) - p(2)%val
117 : end do
118 :
119 241257 : w = winding_number(num_points, diff)
120 241257 : if (w /= 0) then
121 : contained = .true.
122 : else
123 210669 : contained = .false.
124 : end if
125 :
126 241257 : end function is_contained
127 :
128 : !! Computes the minimum distance from a given point to a given line segment.
129 : !!
130 : !! @param line_start The coordinates of the start of the line segment (x,y).
131 : !! @param line_end The coordinates of the end of the line segment (x,y).
132 : !! @param p The point whose distance to compute.
133 1930036 : type(auto_diff_real_2var_order1) function min_distance_from_point_to_line_segment(line_start, line_end, p) result(d)
134 : real(dp), intent(in) :: line_start(2), line_end(2)
135 : type(auto_diff_real_2var_order1), intent(in) :: p(2)
136 : real(dp), parameter :: eps = 1e-10 ! To avoid singularity in the derivatives near edges and corners.
137 :
138 : type(auto_diff_real_2var_order1) :: diff_line(2), diff_start(2), diff_end(2)
139 : type(auto_diff_real_2var_order1) :: length_squared, lambda, nearest_point_on_line(2)
140 :
141 :
142 1930036 : diff_start(1) = p(1) - line_start(1)
143 1930036 : diff_start(2) = p(2) - line_start(2)
144 :
145 1930036 : diff_end(1) = p(1) - line_end(1)
146 1930036 : diff_end(2) = p(2) - line_end(2)
147 :
148 1930036 : diff_line(1) = line_end(1) - line_start(1)
149 1930036 : diff_line(2) = line_end(2) - line_start(2)
150 :
151 1930036 : length_squared = pow2(diff_line(1)) + pow2(diff_line(2))
152 :
153 : ! First, find the nearest_point_on_line. We parameterize this by
154 : ! (nearest) = (start) + lambda (end - start)
155 : !
156 : ! We can then pretend the line is infinite, solve for lambda, and then restrict it to lie in [0,1].
157 1930036 : lambda = (diff_start(1) * diff_line(1) + diff_start(2) * diff_line(2)) / length_squared
158 :
159 1930036 : if (lambda < 0d0) then ! Nearest point is line_start
160 427032 : d = sqrt(pow2(diff_start(1)) + pow2(diff_start(2)))
161 1503004 : else if (lambda > 1d0) then ! Nearest point is line_end
162 574339 : d = sqrt(pow2(diff_end(1)) + pow2(diff_end(2)))
163 : else ! Nearest point is interior to the line segment
164 928665 : nearest_point_on_line(1) = line_start(1) + lambda * diff_line(1)
165 928665 : nearest_point_on_line(2) = line_start(2) + lambda * diff_line(2)
166 928665 : nearest_point_on_line(1) = nearest_point_on_line(1) - p(1)
167 928665 : nearest_point_on_line(2) = nearest_point_on_line(2) - p(2)
168 928665 : d = sqrt(pow2(nearest_point_on_line(1)) + pow2(nearest_point_on_line(2)) + pow2(eps))
169 : end if
170 :
171 1930036 : end function min_distance_from_point_to_line_segment
172 :
173 : !! Computes the distance to the nearest line segment.
174 : !! This is done by looping over segments, computing the minimum distance to each, and
175 : !! returning the smallest of those differences.
176 : !!
177 : !! @param num_points The number of points specifying the polygon.
178 : !! @param coords The coordinates of the polygon. An array of shape (num_points, 2) storing (x,y) pairs.
179 : !! @param p The point whose distance to compute.
180 241257 : type(auto_diff_real_2var_order1) function min_distance_to_polygon(num_points, coords, p) result(d)
181 : integer, intent(in) :: num_points
182 : real(dp), intent(in) :: coords(num_points, 2)
183 : type(auto_diff_real_2var_order1), intent(in) :: p(2)
184 :
185 1206285 : real(dp) :: line_start(2), line_end(2)
186 : type(auto_diff_real_2var_order1) :: line_dist
187 : integer :: i
188 :
189 241257 : d = 1d99
190 2171293 : do i=1,num_points
191 1930036 : line_start(1) = coords(i,1)
192 1930036 : line_start(2) = coords(i,2)
193 :
194 1930036 : if (i == num_points) then
195 241257 : line_end(1) = coords(1,1)
196 241257 : line_end(2) = coords(1,2)
197 : else
198 1688779 : line_end(1) = coords(i+1,1)
199 1688779 : line_end(2) = coords(i+1,2)
200 : end if
201 :
202 1930036 : line_dist = min_distance_from_point_to_line_segment(line_start, line_end, p)
203 2171293 : d = min(d, line_dist)
204 : end do
205 :
206 241257 : end function min_distance_to_polygon
207 :
208 : end module eos_blend
|
