File: /nfs/home/0/users/jenkins/mfix.git/model/des/des_stl_functions_mod.f

1     !vvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvv!
2     !                                                                      !
3     !  Module name: des_stl_functions                                      !
4     !  Author: Rahul Garg                                 Date: 24-Oct-13  !
5     !                                                                      !
6     !  Purpose: This module containd routines for geometric interaction    !
7     !  required for STL files.                                             !
8     !                                                                      !
9     !^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^!
10           MODULE des_stl_functions
11     
12           IMPLICIT NONE
13     
14     ! Use this module only to define functions and subroutines.
15           CONTAINS
16     
17     
18     !vvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvv!
19     !                                                                      !
20     !  Subroutine: TestTriangleAABB                                        !
21     !  Author: Rahul Garg                                 Date: 24-Oct-13  !
22     !                                                                      !
23     !  Purpose:                                                            !
24     !                                                                      !
25     !^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^!
26           SUBROUTINE TestTriangleAABB(vertices, tri_norm, box_origin, &
27              box_extents, sa_exist, sa, i,j,k)
28     
29           USE stl
30     
31           Implicit none
32     
33           !Separating axis test algorithm from Realtimecollision book by
34           !Christer Ericson
35           double precision, intent(in), dimension(3) :: tri_norm
36           double precision, intent(in), dimension(3,3) :: vertices
37           double precision, intent(in), dimension(3) :: box_origin, box_extents
38           logical, intent(out) :: sa_exist
39     !      double precision, intent(out) :: proj_box, proj_tri(3)
40           Integer, intent(out) :: sa
41           Integer, intent(in) :: i,j,k
42     
43           double precision :: p0, p1, p2, r, e0, e1, e2
44     
45           double precision :: c_orig(3), c(3)
46           double precision, dimension(3) :: vert0, vert1, vert2
47     
48           !box face normals
49           double precision, dimension(3) :: u0, u1, u2
50     
51           double precision, dimension(3) :: v0, v1, v2
52           double precision, dimension(3) :: f0, f1, f2
53     
54           !13 possible Separating axes
55           double precision, dimension(9, 3) :: sep_axis
56           !triangle plane n and d
57           double precision :: tol_tri_aabb_proj
58     
59           double precision :: axis_magsq, s
60           integer :: count
61           sa_exist = .false.
62           sa = 0
63           tol_tri_aabb_proj = 1e-06
64     
65           vert0 = vertices(1,:)
66           vert1 = vertices(2,:)
67           vert2 = vertices(3,:)
68     
69           !Initially set e0, e1, and e2 to box extents
70     
71           !Compute box center and extents (if not already given in that format)
72           c_orig(:) = box_origin(:) + box_extents(:)* 0.5d0
73     
74           !Translate triangle as conceptually moving AABB's center to origin
75     
76           v0 = vert0 - c_orig
77           v1 = vert1 - c_orig
78           v2 = vert2 - c_orig
79     
80           !Scale everything by box_extents
81           v0(1) = v0(1)/box_extents(1)
82           v1(1) = v1(1)/box_extents(1)
83           v2(1) = v2(1)/box_extents(1)
84     
85           v0(2) = v0(2)/box_extents(2)
86           v1(2) = v1(2)/box_extents(2)
87           v2(2) = v2(2)/box_extents(2)
88     
89           v0(3) = v0(3)/box_extents(3)
90           v1(3) = v1(3)/box_extents(3)
91           v2(3) = v2(3)/box_extents(3)
92     
93           !set the box origin to (0,0,0)
94           c = 0.d0
95           !e0, e1, and e2 are half box extents which equals 0.5d0 in the scaled co-ordinates
96           e0 = 0.5d0
97           e1 = 0.5d0
98           e2 = 0.5d0
99           u0 = (/ 1.d0, 0.d0, 0.d0/)
100           u1 = (/ 0.d0, 1.d0, 0.d0/)
101           u2 = (/ 0.d0, 0.d0, 1.d0/)
102     
103     !!$  write(*,'(A20,5(2x,g17.8))') 'box center orgi = ',c_orig(:)
104     !!$  write(*,'(A20,5(2x,g17.8))') 'box center new = ',c(:)
105     !!$  write(*,'(A20,5(2x,g17.8))') 'box extents = ',e0,e1,e2
106     !!$  write(*,'(A20,5(2x,g17.8))') 'tri vert1 = ',v0(:)
107     !!$  write(*,'(A20,5(2x,g17.8))') 'tri vert2 = ',v1(:)
108     !!$  write(*,'(A20,5(2x,g17.8))') 'tri vert3 = ',v2(:)
109     
110           !Compute edge vectors for triangle
111           f0 = v1 - v0
112           f1 = v2 - v1
113           f2 = v0 - v2
114           !Category 3 (edge-edge cross products)...
115           !total of 9 (3 from triangle cross 3 independent from box)
116           sep_axis(1, 1:3) = (/0.d0, -f0(3), f0(2)/)
117           sep_axis(2, 1:3) = (/0.d0, -f1(3), f1(2)/)
118           sep_axis(3, 1:3) = (/0.d0, -f2(3), f2(2)/)
119           sep_axis(4, 1:3) = (/f0(3),  0.d0, -f0(1)/)
120           sep_axis(5, 1:3) = (/f1(3),  0.d0, -f1(1)/)
121           sep_axis(6, 1:3) = (/f2(3),  0.d0, -f2(1)/)
122           sep_axis(7, 1:3) = (/-f0(2), f0(1), 0.d0/)
123           sep_axis(8, 1:3) = (/-f1(2), f1(1), 0.d0/)
124           sep_axis(9, 1:3) = (/-f2(2), f2(1), 0.d0/)
125     
126           do count = 1, 9
127              axis_magsq = (sep_axis(count,1)**2+sep_axis(count,2)**2 &
128              +sep_axis(count,3)**2)
129              if(axis_magsq < TOL_STL*TOL_STL) THEN
130                 !nearly a zero vector. This will happen if the cross-products
131                 !from category 3 are formed from co-planar vectors
132                 !WRITE(*,'(/5x,A20, i2,/5x, A, 3(2x,g17.8))') 'Axis number:', count, &
133                 !     'Ignored due to zero vec:', sep_axis(count,:)
134                 CYCLE
135              endif
136     
137              r = e0*abs(dot_product(u0(:), sep_axis(count,:)))             &
138                 + e1*abs(dot_product(u1(:), sep_axis(count,:)))            &
139                 + e2*abs(dot_product(u2(:), sep_axis(count,:)))
140     
141              p0 = dot_product(v0(:), sep_axis(count, :))
142              p1 = dot_product(v1(:), sep_axis(count, :))
143              p2 = dot_product(v2(:), sep_axis(count, :))
144     
145             IF(.FALSE.) THEN
146                 WRITE(*,'(A6,5(2X,G17.8))')'C:',C_ORIG
147                 WRITE(*,'(A6,5(2X,G17.8))')'VORIG:',VERT0
148                 WRITE(*,'(A6,5(2X,G17.8))')'V0:',V0
149                 WRITE(*,'(A6,5(2X,G17.8))')'V1:',V1
150                 WRITE(*,'(A6,5(2X,G17.8))')'EXTENTS:',E0,E1,E2
151                 WRITE(*,'(A6,5(2X,G17.8))')'SA:', COUNT, SEP_AXIS(COUNT,:)
152                 WRITE(*,'(A6,5(2X,G17.8))')'DATA:',  R, P0, P1, P2
153                 WRITE(*,'(A6,(2X,G17.8),"<",2(2X,G17.8), ">", 2X,G17.8)')  &
154                    'DATA2:',  MAX(P0, P1, P2)*1E8, -R*1E8, &
155                    MIN(P0, P1, P2)*1E8,R*1E8
156     
157                 WRITE(*,'(A6,5(2X,L2))') 'DATA2:', MAX(P0, P1, P2)< -R,&
158                    MIN(P0, P1, P2) > R
159     
160                 WRITE(*,'(A6,(2X,G17.8),"<",2(2X,G17.8), ">", 2X,G17.8)')&
161                    'DATA2:', (MAX(P0, P1, P2)+TOL_TRI_AABB_PROJ)*1E8,&
162                    -R*1E8, MIN(P0, P1, P2)*1E8, (R+TOL_TRI_AABB_PROJ)*1E8
163                 WRITE(*,'(A6,5(2X,L2))') 'DATA2:', MAX(P0, P1, P2)+&
164                    TOL_TRI_AABB_PROJ< -R, MIN(P0, P1, P2) > R+TOL_TRI_AABB_PROJ
165              ENDIF
166     
167              IF (MAX(P0, P1, P2) +TOL_TRI_AABB_PROJ < -R .OR. &
168                 MIN(P0,P1,P2) > R+TOL_TRI_AABB_PROJ) THEN
169                 SA = COUNT
170                 SA_EXIST = .TRUE.
171                 RETURN
172              ENDIF
173           ENDDO
174     
175     ! Test the three axes corresponding to the face normals of AABB b (category 1).
176     ! Could have been through dot products like above, but explotiing the
177     ! the simple definition of box face normals to optimize the code.
178     
179     ! Exit if...
180     
181     ! ... [-e0, e0] and [min(v0.x,v1.x,v2.x), max(v0.x,v1.x,v2.x)] do not overlap
182           IF (MAX(V0(1), V1(1), V2(1)) + TOL_TRI_AABB_PROJ< -E0 .OR.       &
183              MIN(V0(1), V1(1), V2(1)) > E0+TOL_TRI_AABB_PROJ) THEN
184              SA = 10
185              SA_EXIST = .TRUE.
186              RETURN
187           ENDIF
188     
189     !... [-e1, e1] and [min(v0.y,v1.y,v2.y), max(v0.y,v1.y,v2.y)] do not overlap
190           IF (MAX(V0(2), V1(2), V2(2)) +TOL_TRI_AABB_PROJ< -E1 .OR.        &
191              MIN(V0(2), V1(2), V2(2)) > E1+TOL_TRI_AABB_PROJ) THEN
192              SA = 11
193              SA_EXIST = .TRUE.
194              RETURN
195           ENDIF
196     
197     ! ... [-e2, e2] and [min(v0.z,v1.z,v2.z), max(v0.z,v1.z,v2.z)] do not overlap
198           IF (MAX(V0(3), V1(3), V2(3)) +TOL_TRI_AABB_PROJ< -E2 .OR.        &
199              MIN(V0(3), V1(3), V2(3)) > E2+TOL_TRI_AABB_PROJ) THEN
200              SA = 12
201              SA_EXIST = .TRUE.
202              RETURN
203           ENDIF
204     
205           R = E0*ABS(DOT_PRODUCT(U0(:), TRI_NORM(:)))                      &
206              + E1*ABS(DOT_PRODUCT(U1(:), TRI_NORM(:)))                     &
207              + E2*ABS(DOT_PRODUCT(U2(:), TRI_NORM(:)))
208     
209     ! Compute distance of box center from plane
210     ! s = Dot_product(tri_norm(:), c(:) - v0(:))
211     ! c = zero in the shfted co-ordinates
212           s = Dot_product(tri_norm(:), v0(:))
213     
214     !INTERSECTION OCCURS WHEN DISTANCE S FALLS WITHIN [-R,+R] INTERVAL
215           IF(ABS(S) > R+ TOL_TRI_AABB_PROJ) THEN
216              !PROJECTIONS DO NOT INTERSECT, SO THE SEPARATING AXIS EXISTS
217              SA_EXIST = .TRUE.
218              SA = 13
219           ENDIF
220           END SUBROUTINE TestTriangleAABB
221     
222     
223     
224     
225     
226     !vvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvv!
227     !                                                                      !
228     !  Subroutine: TestTriangleAABB                                        !
229     !  Author: Rahul Garg                                 Date: 24-Oct-13  !
230     !                                                                      !
231     !  Purpose:                                                            !
232     !                                                                      !
233     !^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^!
234           Subroutine ClosestPtPointTriangle(pointp, points, closest_point)
235           USE param1, only: zero, one
236           USE discretelement, only: dimn
237     
238           IMPLICIT NONE
239           !point a, pointb, and pointc are the three nodes of the triangle
240           !point p is the sphere center
241           double precision, intent(in), dimension(3,3) :: points
242           double precision, intent(in), dimension(dimn) :: pointp
243           double precision, intent(out), dimension(dimn) ::  closest_point
244           !Local variables
245           double precision, dimension(3) :: pointa, pointb, pointc
246           double precision, dimension(dimn) :: ab, ac, ap, bp,cp
247           double precision :: d1, d2, d3, d4, vc, v, d5, d6, vb, w, va, denom
248     
249           pointa = points(1,:)
250           pointb = points(2,:)
251           pointc = points(3,:)
252     
253           ab = pointb - pointa
254           ac = pointc - pointa
255           ap = pointp - pointa
256           d1 = DOT_PRODUCT(ab, ap)
257           d2 = DOT_PRODUCT(ac, ap)
258     
259           IF(d1 <= Zero .AND. d2 <= zero) then
260              closest_point = pointa ! barycentric coordinates (1,0,0)
261              return
262           end if
263     
264           !Check if P in vertex region outside B
265           bp = pointp - pointb
266           d3 = DOT_PRODUCT(ab, bp);
267           d4 = DOT_PRODUCT(ac, bp);
268           if (d3 >= zero .and. d4 <= d3) then
269              closest_point = pointb !barycentric coordinates (0,1,0)
270              return
271           endif
272     
273           ! Check if P in edge region of AB, if so return projection of P onto AB
274           vc = d1*d4 - d3*d2;
275           if (vc <= zero .and. d1 >= zero .and. d3 <= zero) then
276              v = d1 / (d1 - d3);
277              closest_point =  pointa + v * ab; ! barycentric coordinates (1-v,v,0)
278              return
279           end if
280     
281           !Check if P in vertex region outside C
282           cp = pointp - pointc
283           d5 = DOT_PRODUCT(ab, cp)
284           d6 = DOT_PRODUCT(ac, cp)
285           if (d6 >= zero .and. d5 <= d6) then
286              closest_point  = pointc ! barycentric coordinates (0,0,1)
287              return
288           endif
289     
290           !Check if P in edge region of AC, if so return projection of P onto AC
291           vb = d5*d2 - d1*d6
292     
293           if (vb <= zero .and. d2 >= zero .and. d6 <= zero) then
294              w = d2 / (d2 - d6)
295              closest_point = pointa + w * ac ! barycentric coordinates (1-w,0,w)
296              return
297           end if
298     
299           !Check if P in edge region of BC, if so return projection of P onto BC
300           va = d3*d6 - d5*d4
301           if (va <= zero .and.(d4 - d3) >= zero .and. (d5 - d6) >= zero) then
302              w = (d4 - d3) / ((d4 - d3) + (d5 - d6))
303              closest_point = pointb + w * (pointc - pointb) !barycentric coordinates (0,1-w,w)
304              return
305           end if
306     
307     
308           !P inside face region. Compute Q through its barycentric coordinates (u,v,w)
309           denom = one / (va + vb + vc)
310           v = vb * denom
311           w = vc * denom
312           closest_point = pointa + ab * v + ac * w; ! = u*a + v*b + w*c, u = va * denom = 1.0f - v - w
313           return
314           end Subroutine ClosestPtPointTriangle
315     
316     !vvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvv!
317     !                                                                      !
318     !  Subroutine: TestTriangleAABB                                        !
319     !  Author: Rahul Garg                                 Date: 24-Oct-13  !
320     !                                                                      !
321     !  Purpose:                                                            !
322     !                                                                      !
323     !^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^!
324           SUBROUTINE CHECKPTONTRIANGLE(POINTP, POINTS, ON_TRIAN)
325     
326     ! Global parameters:
327     !---------------------------------------------------------------------//
328     ! DEM array spatial array dimension.
329           use discretelement, only: DIMN
330     ! Tolerance surround STL
331           use stl, only: tol_stl
332     ! Common values in working percision
333           use param1, only : ONE
334     
335     
336           IMPLICIT NONE
337     
338     
339     ! Dummy arguments
340     !---------------------------------------------------------------------//
341     ! the three nodes of the triangle
342           DOUBLE PRECISION, INTENT(IN) :: POINTS(3,3)
343     ! the test point
344           DOUBLE PRECISION, INTENT(IN) :: POINTP(DIMN)
345     ! Flag that the point lies on the triangle
346           LOGICAL, INTENT(OUT)::  ON_TRIAN
347     
348     ! Local variables
349     !---------------------------------------------------------------------//
350     ! triangle edges
351           DOUBLE PRECISION, DIMENSION(DIMN) :: V0, V1, V2
352           DOUBLE PRECISION :: D00, D01, D11, D20, D21, DENOM
353     ! barcycentric coordinates
354           DOUBLE PRECISION :: V, W
355     
356     
357           V0 = POINTS(2,:) - POINTS(1,:)
358           V1 = POINTS(3,:) - POINTS(1,:)
359           V2 = POINTP - POINTS(1,:)
360     
361           D00 = dot_product(V0, V0)
362           D01 = dot_product(V0, V1)
363           D11 = dot_product(V1, V1)
364           D20 = dot_product(V2, V0)
365           D21 = dot_product(V2, V1)
366     
367           DENOM = D00*D11 - D01*D01
368     
369           V = (D11*D20 - D01*D21)/DENOM
370           W = (D00*D21 - D01*D20)/DENOM
371     
372           ON_TRIAN = ((V>=-TOL_STL) .AND. (W>=-TOL_STL) .AND.              &
373              ((V+W)<=ONE+TOL_STL))
374     
375           RETURN
376           END SUBROUTINE CHECKPTONTRIANGLE
377     
378     
379     !vvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvv!
380     !                                                                      !
381     !  Subroutine: intersectLnPlane                                        !
382     !  Author: Rahul Garg                                 Date: 24-Oct-13  !
383     !                                                                      !
384     !  Purpose:                                                            !
385     !                                                                      !
386     !^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^!
387           Subroutine intersectLnPlane(ref_line, dir_line, ref_plane,       &
388              norm_plane, line_param)
389     
390           USE discretelement, only: dimn
391           USE param1, only: zero
392           IMPLICIT NONE
393           !reference point and direction of the line
394           double precision, intent(in), dimension(dimn) :: ref_line,  dir_line
395           !reference point and normal of the plane
396           double precision, intent(in), dimension(dimn) :: ref_plane, norm_plane
397     
398           !line is parameterized as p = p_ref + t * dir_line, t is line_param
399           double precision, intent(out) :: line_param
400     
401           !local vars
402           double precision :: denom
403     
404           denom = DOT_PRODUCT(dir_line, norm_plane)
405           if(denom*denom.gt.zero) then
406              line_param = DOT_PRODUCT(ref_plane(:) - ref_line(:), norm_plane(:))
407              line_param = line_param/denom
408           endif
409           return
410           end subroutine intersectLnPlane
411     
412           subroutine set_facet_type_normal(facet_type)
413             USE discretelement, only: Facet_type_normal
414             Implicit none
415             integer, intent(out) :: facet_type
416             facet_type = FACET_type_NORMAL
417           end subroutine set_facet_type_normal
418     
419           subroutine set_facet_type_po(facet_type)
420             USE discretelement, only: Facet_type_po
421             Implicit none
422             integer, intent(out) :: facet_type
423             facet_type = FACET_type_PO
424           end subroutine set_facet_type_po
425     
426           subroutine set_facet_type_mi(facet_type)
427             USE discretelement, only: Facet_type_mi
428             Implicit none
429             integer, intent(out) :: facet_type
430             facet_type = FACET_type_MI
431           end subroutine set_facet_type_mi
432     
433     
434           END MODULE DES_STL_FUNCTIONS
435     
436     
437