forked from emolch/kiwi
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathgeometry.f90
322 lines (236 loc) · 10.9 KB
/
geometry.f90
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
!
! Copyright 2011 Sebastian Heimann
!
! Licensed under the Apache License, Version 2.0 (the "License");
! you may not use this file except in compliance with the License.
! You may obtain a copy of the License at
!
! http://www.apache.org/licenses/LICENSE-2.0
!
! Unless required by applicable law or agreed to in writing, software
! distributed under the License is distributed on an "AS IS" BASIS,
! WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
! See the License for the specific language governing permissions and
! limitations under the License.
!
module geometry
use constants
implicit none
private
type, public :: t_halfspace
real, dimension(3) :: point
real, dimension(3) :: normal
end type
type, public :: t_circle
! Untransformed circle has unit radius and lies in x-y plane.
real, dimension(3) :: center
real, dimension(3,3) :: transform
end type
type, public :: t_polygon
real, dimension(:,:), allocatable :: points
end type
public point_in_halfspace
public get_piercingpoint
public nearest_point_on_polygon
public circle_to_polygon
public trim_polygon
public copy_polygon
public polygon_box
public polygon_area
public polygon_destroy
interface trim_polygon
module procedure trim_polygon_one
module procedure trim_polygon_more
end interface
contains
pure function point_in_halfspace( point, halfspace )
! Decide if point is in the halfspace.
!
! Returns true if the point is in the halfspace, from which the normal
! vector is pointing away, or if it lies on the borderplane.
real, dimension(3), intent(in) :: point
type(t_halfspace), intent(in) :: halfspace
logical :: point_in_halfspace
point_in_halfspace = &
(dot_product( halfspace%normal, halfspace%point(:)-point(:) ) >= 0.0)
end function
pure subroutine get_piercingpoint( point_a, point_b, halfspace, piercingpoint, &
between_ab, parallel, a_inside_, b_inside_ )
! Get the piercing point of line between a and b with halfspace border.
real, dimension(3), intent(in) :: point_a
real, dimension(3), intent(in) :: point_b
type(t_halfspace), intent(in) :: halfspace
real, dimension(3), intent(out) :: piercingpoint
logical, intent(out) :: between_ab, parallel
logical, intent(out), optional :: a_inside_, b_inside_
logical :: a_inside, b_inside
real :: lambda_a, lambda_b, lambda_ab
real, dimension(3) :: ab
ab(:) = point_b(:) - point_a(:)
lambda_a = dot_product( halfspace%normal, halfspace%point(:) - point_a(:) )
lambda_b = dot_product( halfspace%normal, halfspace%point(:) - point_b(:) )
lambda_ab = dot_product( halfspace%normal, ab )
a_inside = (lambda_a >= 0.)
b_inside = (lambda_b >= 0.)
if (present(a_inside_)) a_inside_ = a_inside
if (present(b_inside_)) b_inside_ = b_inside
between_ab = ((a_inside .and. .not. b_inside) .or. &
(b_inside .and. .not. a_inside))
parallel = ( lambda_ab*lambda_ab < dot_product(ab,ab) / 2**(digits(lambda_ab)) )
if (parallel .and. between_ab) then
! AB lies (almost) in plane
if (abs(lambda_a) <= abs(lambda_b)) then
piercingpoint(:) = point_a(:)
else
piercingpoint(:) = point_b(:)
end if
return
end if
if (parallel .and. .not. between_ab) then
piercingpoint = (/0.,0.,0./)
return
end if
piercingpoint(:) = point_a(:) + ab(:)*lambda_a/lambda_ab
end subroutine
pure function nearest_point_on_polygon( polygon, point ) result(nearest_point)
type(t_polygon), intent(in) :: polygon
real, dimension(3), intent(in) :: point
real, dimension(3) :: nearest_point
type(t_halfspace) :: halfspace
real, dimension(3) :: piercingpoint
logical :: does_pierce, parallel
integer :: npoints, ipoint, jpoint
real :: distsquare, comp_distsquare
nearest_point = point
if (.not. allocated(polygon%points)) return
if (size(polygon%points,2) == 0) return
npoints = size(polygon%points,2)
halfspace%point = point
distsquare = huge(distsquare)
nearest_point = polygon%points(:,1)
if (size(polygon%points,2) == 1) return
do ipoint=1,npoints
jpoint = mod(ipoint,npoints)+1
halfspace%normal = polygon%points(:,jpoint) - polygon%points(:,ipoint)
call get_piercingpoint( polygon%points(:,ipoint), polygon%points(:,jpoint), &
halfspace, piercingpoint, &
does_pierce, parallel )
comp_distsquare = vecsqr(piercingpoint-point)
if (does_pierce .and. comp_distsquare < distsquare) then
distsquare = comp_distsquare
nearest_point = piercingpoint
end if
comp_distsquare = vecsqr(polygon%points(:,ipoint)-point)
if (comp_distsquare < distsquare) then
distsquare = comp_distsquare
nearest_point = polygon%points(:,ipoint)
end if
end do
end function
pure function vecsqr( a )
real, dimension(3), intent(in) :: a
real :: vecsqr
vecsqr = dot_product(a,a)
end function
pure subroutine circle_to_polygon( circle, npoints, polygon )
! Create a polygon approximating the circle.
type(t_circle), intent(in) :: circle
integer, intent(in) :: npoints
type(t_polygon), intent(inout) :: polygon
integer :: i
if (allocated(polygon%points)) deallocate(polygon%points)
allocate(polygon%points(3,npoints))
do i=1,npoints
polygon%points(:,i) = matmul(circle%transform,(/cos(i*2.*pi/npoints), &
sin(i*2.*pi/npoints), &
0./)) + circle%center(:)
end do
end subroutine
pure subroutine trim_polygon_one( polygon, halfspace, polygon_trimmed )
! Cut off parts of polygon, which lie outside of halfspace.
type(t_polygon), intent(in) :: polygon
type(t_halfspace), intent(in) :: halfspace
type(t_polygon), intent(inout) :: polygon_trimmed
real, dimension(3,size(polygon%points,2)) :: piercingpoints
logical, dimension(size(polygon%points,2)) :: does_pierce, point_inside
integer :: npoints, npoints_trimmed, ipoint, jpoint
logical :: parallel, b_inside
npoints = size(polygon%points,2)
if (allocated(polygon_trimmed%points)) deallocate(polygon_trimmed%points)
npoints_trimmed = 0
do ipoint=1,npoints
jpoint = mod(ipoint,npoints)+1
call get_piercingpoint( polygon%points(:,ipoint), polygon%points(:,jpoint), &
halfspace, piercingpoints(:,ipoint), &
does_pierce(ipoint), parallel, &
point_inside(ipoint), b_inside )
if (point_inside(ipoint)) npoints_trimmed = npoints_trimmed + 1
if (does_pierce(ipoint)) npoints_trimmed = npoints_trimmed + 1
end do
allocate(polygon_trimmed%points(3,npoints_trimmed))
jpoint = 1
do ipoint=1,npoints
if (point_inside(ipoint)) then
polygon_trimmed%points(:,jpoint) = polygon%points(:,ipoint)
jpoint = jpoint + 1
end if
if (does_pierce(ipoint)) then
polygon_trimmed%points(:,jpoint) = piercingpoints(:,ipoint)
jpoint = jpoint + 1
end if
end do
end subroutine
pure subroutine trim_polygon_more( polygon, halfspaces, polygon_trimmed )
! Cut off parts of polygon, which lie outside of halfspaces.
type(t_polygon), intent(in) :: polygon
type(t_halfspace), dimension(:), intent(in) :: halfspaces
type(t_polygon), intent(inout) :: polygon_trimmed
type(t_polygon) :: temp
integer :: icon
call copy_polygon( polygon, temp )
do icon=1,size(halfspaces)
if (icon /= 1) call copy_polygon( polygon_trimmed, temp )
call trim_polygon( temp, halfspaces(icon), polygon_trimmed )
end do
end subroutine
pure subroutine polygon_box( polygon, mi, ma )
type(t_polygon), intent(in) :: polygon
real, intent(out), dimension(3) :: mi, ma
mi = minval(polygon%points,2)
ma = maxval(polygon%points,2)
end subroutine
pure subroutine copy_polygon( a, b )
type(t_polygon), intent(in) :: a
type(t_polygon), intent(inout) :: b
if (allocated(b%points)) deallocate(b%points)
allocate(b%points(3,size(a%points,2)))
b%points = a%points
end subroutine
pure function polygon_area( polygon ) result(area)
type(t_polygon), intent(in) :: polygon
real :: area
real :: area_xy, area_yz, area_zx
integer :: ip, jp, np
np = size(polygon%points,2)
area_xy = 0.
area_yz = 0.
area_zx = 0.
if (.not. allocated(polygon%points)) return
np = size(polygon%points,2)
if (np <= 2) return
do ip=1,np
jp = mod(ip,np)+1
area_xy = area_xy + (polygon%points(1,ip)-polygon%points(1,jp))* &
(polygon%points(2,ip)+polygon%points(2,jp))*0.5
area_yz = area_yz + (polygon%points(2,ip)-polygon%points(2,jp))* &
(polygon%points(3,ip)+polygon%points(3,jp))*0.5
area_zx = area_zx + (polygon%points(3,ip)-polygon%points(3,jp))* &
(polygon%points(1,ip)+polygon%points(1,jp))*0.5
end do
area = sqrt(area_xy**2 + area_yz**2 + area_zx**2)
end function
pure subroutine polygon_destroy( poly )
type(t_polygon), intent(inout) :: poly
if (allocated(poly%points)) deallocate( poly%points )
end subroutine
end module