subroutine aptquad (px, py, pz, ax, ay, az, bx, by, bz, & cx, cy, cz, dx, dy, dz, noptfd, tol, & dpmin, fdkq, fdlq, qx, qy, qz, & nlimk, nliml, itrun, nside, ncon, nerr) ccbeg. cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc c c SUBROUTINE APTQUAD c c call aptquad (px, py, pz, ax, ay, az, bx, by, bz, c & cx, cy, cz, dx, dy, dz, noptfd, tol, c & dpmin, fdkq, fdlq, qx, qy, qz, c & nlimk, nliml, itrun, nside, ncon, nerr) c c Version: aptquad Updated 1989 November 29 16:10. c aptquad Originated 1989 November 2 14:10. c c Author: Arthur L. Edwards, LLNL, L-298, Telephone (925) 422-4123. c c c Purpose: To find the minimum distance dpmin from the external point c p = (px, py, pz), to the surface bounded by a 3-D quadrangle c with vertices (ax,ay,az), (bx,by,bz), (cx,cy,cz), (dx,dy,dz); c to find the point q = (qx, qy, qz) on the surface nearest the c external point; and to find the fractional distances fdkq and c fdlq of that point between opposite sides of the quadrangle. c Option noptfd allows the ranges of fdkq and fdlq to be limited. c Flag nerr indicates any input error, if not zero. c Result flags are returned. c c The equation of the surface (r, a, b, c, d are vectors) is: c c r = a + (b - a) * fdk + (d - a) * fdl + c (a - b + c - d) * fdk * fdl c c r = a * (1 - fdk)*(1 - fdl) + b * fdk*(1 - fdl) + c c * fdk*fdl + d * (1 - fdk)*fdl c c where r is a vector (x, y, z), and fdk, fdl are fractional c distances between opposite edges. c c Method: Uses functional iteration, tangent plane approximation, and c acceleration. The rate of convergence depends on the problem. c For 2 initial values of fdk, iteratively find fdl nearest c the external point, the best fdk for that fdl, etc. c If two minima are found, use the least value of dpmin. c c Input: px, py, pz, ax, ay, az, bx, by, bz, cx, cy, cz, dx, dy, dz, c tol. c c Output: dpmin, fdkq, fdlq, qx, qy, qz, c nlimk, nliml, itrun, nside, ncon, nerr. c c Calls: aptfdad, aptptln, aptptpl, aptvdis, aptvpln c c c Glossary: c c ax,ay,az Input The x, y, z coordinates of quadrangle vertex "a". c c bx,by,bz Input The x, y, z coordinates of quadrangle vertex "b". c c cx,cy,cz Input The x, y, z coordinates of quadrangle vertex "c". c c dx,dy,dz Input The x, y, z coordinates of quadrangle vertex "d". c c dpmin Output Minimum distance from point p = (px, py, pz) to the c surface bounded by the edges of the 3-D quadrangle c with vertices (ax, ay, az), (bx, by, bz), c (cx, cy, cz) and (dx, dy, dz). c c fdkq Output Fractional distance of the point q = (qx, qy, qz) c between opposite edges of the quadrangle, c from the edge bounded by (ax, ay, az) and c (dx, dy, dz). A value of exactly 0.0 or 1.0 may c indicate actual minimum point outside quadrangle. c Values within tol of 0.0 or 1.0 may be shifted c slightly inside quadrangle. See nlimk. c c fdlq Output Fractional distance of the point q = (qx, qy, qz) c between opposite edges of the quadrangle, c from the edge bounded by (ax, ay, az) and c (bx, by, bz). A value of exactly 0.0 or 1.0 may c indicate actual minimum point outside quadrangle. c Values within tol of 0.0 or 1.0 may be shifted c slightly inside quadrangle. See nliml. c c itrun Output 0 if no change is made in the calculated value of c dpmin, 1 if dpmin is changed to zero, when less than c the estimated error in its calculation. c c ncon Output Error flag. c 1 or 2 if first two guesses fail to converge. c 3 for total failure to find solution. c c nerr Output Indicates an input error, if not 0. c 1 is added if noptfd is not between 0 and 2. c c nlimk Output 0 if fdkq not adjusted. c 1 if near 0.0 or 1.0, shifted inside quadrangle. c 2 if outside range 0.0 to 1.0, shifted to 0.0 or 1.0. c c nliml Output 0 if fdlq not adjusted. c 1 if near 0.0 or 1.0, shifted inside quadrangle. c 2 if outside range 0.0 to 1.0, shifted to 0.0 or 1.0. c c noptfd Input Option to limit the ranges of fdkq, fdlq: c 0 for no limit, c 1 to increase to tol, if in the range from -tol to c tol, and decrease to 1.0 - tol, if in the range c from 1.0 - tol to 1.0 + tol (move a point near an c edge slightly inside the quadrangle), and c 2 to limit to the range from 0.0 to 1.0 (move a c point outside the triangle to an edge). c c nside Output 1 if the point nearest p = (px, py, pz) in the extended c surface through the quadrangle is actually c q = (qx, qy, qz), within tolerance tol. c 0 if q = (qx, qy, qz) is only the point on the c edges of the quadrangle nearest (px, py, pz), but c the vector connecting "p" to "q" is not normal c to the surface. c c px,py,pz Input The x, y, z coordinates of the external point "p". c c qx,qy,qz Output The x, y, z coordinates of the point "q" nearest to c p = (px, py, pz) on the biquadratic surface. c c tol Input Numerical tolerance limit. With 64-bit floating c point numbers, 1.e-5 to 1.e-11 is recommended. c Convergence criterion for dpmin. c cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc ccend. c.... Dimensioned arguments. c.... (None.) c.... Local variables. c---- Convergence acceleration factor. common /laptquad/ accel c---- A very big number. common /laptquad/ big c---- Cosecant of angle between fdk, fdl. common /laptquad/ cscth c---- Square of length of fdl line. common /laptquad/ dksq c---- Increment in x along fdl line. common /laptquad/ dkx c---- Increment in y along fdl line. common /laptquad/ dky c---- Increment in z along fdl line. common /laptquad/ dkz c---- Square of length of fdk line. common /laptquad/ dlsq c---- Increment in x along fdk line. common /laptquad/ dlx c---- Increment in y along fdk line. common /laptquad/ dly c---- Increment in z along fdk line. common /laptquad/ dlz c---- Distance, "p" to tangent plane. common /laptquad/ dpmine c---- Iterate of dpmink. common /laptquad/ dpmink c---- Iterate of dpminl. common /laptquad/ dpminl c---- Previous iterate of dpmink. common /laptquad/ dpminks c---- Previous iterate of dpminl. common /laptquad/ dpminls c---- Distance from vertex to point "q". common /laptquad/ dqv c---- Component x of dqv. common /laptquad/ dqvx c---- Component y of dqv. common /laptquad/ dqvy c---- Component z of dqv. common /laptquad/ dqvz c---- Coordinate x of beginning of line. common /laptquad/ ex c---- Coordinate y of beginning of line. common /laptquad/ ey c---- Coordinate z of beginning of line. common /laptquad/ ez c---- Fractional distance in k direction. common /laptquad/ fdk c---- Estimated fdk. common /laptquad/ fdke c---- Previous iterate of fdk. common /laptquad/ fdks c---- Value of fdk before extrapolation. common /laptquad/ fdkz c---- Fractional distance in l direction. common /laptquad/ fdl c---- Estimated fdl, with tangent plane. common /laptquad/ fdle c---- Previous iterate of fdl. common /laptquad/ fdls c---- Fractional distance, "p" to plane. common /laptquad/ fract c---- A very small number. common /laptquad/ fuz c---- Coordinate x of end of line. common /laptquad/ fx c---- Coordinate y of end of line. common /laptquad/ fy c---- Coordinate z of end of line. common /laptquad/ fz c---- Error flag from aptptln. common /laptquad/ nerrline c---- Index of initial guess. common /laptquad/ ng c---- Number of iterations. common /laptquad/ nit c---- Maximum number of iterations. common /laptquad/ nitmax c---- Tangent plane x nearest "p". common /laptquad/ qxe c---- Iterate of qx. common /laptquad/ qxi c---- Iterate of qy. common /laptquad/ qyi c---- Tangent plane y nearest "p". common /laptquad/ qye c---- Tangent plane z nearest "p". common /laptquad/ qze c---- Iterate of qz. common /laptquad/ qzi c---- Delta (fdk) / delta (dpmink). common /laptquad/ slopek c---- Delta (fdl) / delta (dpminl). common /laptquad/ slopel c---- Magnitude of normal vector. common /laptquad/ vlen c---- Component x of normal vector at "a". common /laptquad/ vnax c---- Component y of normal vector at "a". common /laptquad/ vnay c---- Component z of normal vector at "a". common /laptquad/ vnaz c---- Component x of normal vector at "b". common /laptquad/ vnbx c---- Component y of normal vector at "b". common /laptquad/ vnby c---- Component z of normal vector at "b". common /laptquad/ vnbz c---- Component x of normal vector at "c". common /laptquad/ vncx c---- Component y of normal vector at "c". common /laptquad/ vncy c---- Component z of normal vector at "c". common /laptquad/ vncz c---- Component x of normal vector at "d". common /laptquad/ vndx c---- Component y of normal vector at "d". common /laptquad/ vndy c---- Component z of normal vector at "d". common /laptquad/ vndz c---- Square of normal vector at r. common /laptquad/ vnsq c---- Component x of normal vector at r. common /laptquad/ vnx c---- Component y of normal vector at r. common /laptquad/ vny c---- Component z of normal vector at r. common /laptquad/ vnz cbugc***DEBUG begins. cbugc---- Problem number. cbug common /laptquad/ nprob cbug write ( 3, 9900) px, py, pz, ax, ay, az, bx, by, bz, cbug & cx, cy, cz, dx, dy, dz cbug 9900 format (/ 'aptquad finding distance from point:' / cbug & 'px,py,pz=',1p3e22.14 / cbug & 'to quadrangle with vertices:' / cbug & 'ax,ay,az=',1p3e22.14 / cbug & 'bx,by,bz=',1p3e22.14 / cbug & 'cx,cy,cz=',1p3e22.14 / cbug & 'dx,dy,dz=',1p3e22.14) cbugc***DEBUG ends. c.... Initialize. c---- A very big number. big = 1.e+99 c---- A very small number. fuz = 1.e-99 c---- Maximum number of iterations. nitmax = 200 nerr = 0 ncon = 0 dpmin = big fdkq = -big fdlq = -big qx = -big qy = -big qz = -big c.... Test for input errors. c++++ Bad input. if ((noptfd .lt. 0) .or. (noptfd .gt. 2)) then nerr = 1 go to 210 endif c.... Find the normal vectors at the vertices. call aptvpln (dx, dy, dz, ax, ay, az, bx, by, bz, 1, tol, & vnax, vnay, vnaz, vlen, nerr) call aptvpln (ax, ay, az, bx, by, bz, cx, cy, cz, 1, tol, & vnbx, vnby, vnbz, vlen, nerr) call aptvpln (bx, by, bz, cx, cy, cz, dx, dy, dz, 1, tol, & vncx, vncy, vncz, vlen, nerr) call aptvpln (cx, cy, cz, dx, dy, dz, ax, ay, az, 1, tol, & vndx, vndy, vndz, vlen, nerr) cbugc***DEBUG begins. cbug 9901 format (/ 'normal vectors at vertices (vx, vy, vz):' / cbug & (2x,1p3e22.14)) cbug cbug write ( 3, 9901) vnax, vnay, vnaz, cbug & vnbx, vnby, vnbz, cbug & vncx, vncy, vncz, cbug & vndx, vndy, vndz cbugc***DEBUG ends. c.... See if point "p" is nearest vertex "a". call aptptpl (px, py, pz, ax, ay, az, vnax, vnay, vnaz, 1, tol, & dpmine, qx, qy, qz, itrun, nerr) call aptvdis (qx, qy, qz, ax, ay, az, 1, tol, & dqvx, dqvy, dqvz, dqv, nerr) c---- Point "p" is on normal vector. if (dqv .eq. 0.0) then if (abs (dpmine) .lt. abs (dpmin)) then dpmin = dpmine fdkq = 0.0 fdlq = 0.0 endif c---- Tested dqv at vertex "a". endif c.... See if point "p" is nearest vertex "b". call aptptpl (px, py, pz, bx, by, bz, vnbx, vnby, vnbz, 1, tol, & dpmine, qx, qy, qz, itrun, nerr) call aptvdis (qx, qy, qz, bx, by, bz, 1, tol, & dqvx, dqvy, dqvz, dqv, nerr) c---- Point "p" is on normal vector. if (dqv .eq. 0.0) then if (abs (dpmine) .lt. abs (dpmin)) then dpmin = dpmine fdkq = 1.0 fdlq = 0.0 endif c---- Tested dqv at vertex "b". endif c.... See if point "p" is nearest vertex "c". call aptptpl (px, py, pz, cx, cy, cz, vncx, vncy, vncz, 1, tol, & dpmine, qx, qy, qz, itrun, nerr) call aptvdis (qx, qy, qz, cx, cy, cz, 1, tol, & dqvx, dqvy, dqvz, dqv, nerr) c---- Point "p" is on normal vector. if (dqv .eq. 0.0) then if (abs (dpmine) .lt. abs (dpmin)) then dpmin = dpmine fdkq = 1.0 fdlq = 1.0 endif c---- Tested dqv at vertex "c". endif c.... See if point "p" is nearest vertex "d". call aptptpl (px, py, pz, dx, dy, dz, vndx, vndy, vndz, 1, tol, & dpmine, qx, qy, qz, itrun, nerr) call aptvdis (qx, qy, qz, dx, dy, dz, 1, tol, & dqvx, dqvy, dqvz, dqv, nerr) c---- Point "p" is on normal vector. if (dqv .eq. 0.0) then if (abs (dpmine) .lt. abs (dpmin)) then dpmin = dpmine fdkq = 0.0 fdlq = 1.0 endif c---- Tested dqv at vertex "d". endif c.... See if point "q" is at a vertex. if (dpmin .ne. big) then call aptfdad (fdkq, noptfd, tol, nlimk, nerr) call aptfdad (fdlq, noptfd, tol, nliml, nerr) go to 130 endif c.... Reinitialize. qx = -big qy = -big qz = -big c.... Loop over three initial line segments (fdk = 0.0, 1.0, and 0.5). c.... Try fdk = 0.5 if both 0.0 and 1.0 fail to converge. c---- Loop over 3 initial values of fdk. do 120 ng = 1, 3 dpmink = big dpminl = big fdk = ng - 1 fdl = big if (ng .eq. 3) then if (ncon .eq. 2) then fdk = 0.5 nlimk = 0 else go to 120 endif endif call aptfdad (fdk, noptfd, tol, nlimk, nerr) c.... Iterate for this initial fdk. nit = 0 110 if (nit .ge. nitmax) then ncon = ncon + 1 cbugc***DEBUG begins. cbug 9902 format (/ 'nprob=',i3,' no convergence. nit =',i3,' ncon=',i2) cbug write ( 3, 9902) nprob, nit, ncon cbug write (59, 9902) nprob, nit, ncon cbugc***DEBUG ends. go to 120 endif c============================= find new fdl ============================-------- nit = nit + 1 c.... Save last values of dpminl, fdl. dpminls = dpminl fdls = fdl c.... Find end points of line segment for this fdk. c---- At fdl = 0. ex = ax + (bx - ax) * fdk c---- At fdl = 0. ey = ay + (by - ay) * fdk c---- At fdl = 0. ez = az + (bz - az) * fdk c---- At fdl = 1. fx = dx + (cx - dx) * fdk c---- At fdl = 1. fy = dy + (cy - dy) * fdk c---- At fdl = 1. fz = dz + (cz - dz) * fdk c.... Find better values of dpminl, fdl, qxi, qyi, qzi. call aptptln (px, py, pz, ex, ey, ez, fx, fy, fz, 1, tol, & noptfd, dpminl, fdl, qxi, qyi, qzi, & nliml, itrun, nerrline) c.... Find the normal vector at this fdk, new fdl. vnx = vnax + fdk * (vnbx - vnax) + fdl * (vndx - vnax) vny = vnay + fdk * (vnby - vnay) + fdl * (vndy - vnay) vnz = vnaz + fdk * (vnbz - vnaz) + fdl * (vndz - vnaz) vnsq = vnx**2 + vny**2 + vnz**2 c.... Give proper sign to dpminl. fract = ((px - qxi) * vnx + (py - qyi) * vny + & (pz - qzi) * vnz) / (vnsq + fuz) dpminl = sign (dpminl, fract) cbugc***DEBUG begins. cbug 9903 format (// 'aptquad' / cbug & 'nprob=',i3,' ng=',i1,' nit=',i3,' fdk=',f9.6, cbug & ' fdl=',f9.6,' dpmin=',1pe18.10) cbug 9904 format (13x,'qx=',1pe13.5,' qy=',1pe13.5,' qz= ',1pe13.5) cbug write ( 3, 9903) nprob, ng, nit, fdk, fdl, dpminl cbug write ( 3, 9904) qxi, qyi, qzi cbugc***DEBUG ends. c.... Test for convergence, save best results. if ((dpminl .eq. 0.0) .or. & ((abs (dpminl - dpminls) .le. & 0.5 * tol * (abs (dpminl) + abs (dpminls))))) then if (abs (dpminl) .lt. abs (dpmin)) then dpmin = dpminl fdkq = fdk fdlq = fdl qx = qxi qy = qyi qz = qzi endif cbugc***DEBUG begins. cbug 9905 format ('nprob=',i3,' converged on iteration nit=',i3) cbug write ( 3, "(/)") cbug write ( 3, 9905) nprob, nit cbug write (59, 9905) nprob, nit cbug write ( 3, 9903) nprob, ng, nit, fdk, fdl, dpminl cbug write ( 3, 9904) qxi, qyi, qzi cbug write ( 3, "(/ 10('********'))") cbugc***DEBUG ends. if (dpmin .eq. 0.0) go to 130 go to 120 endif c============================= find new fdk ============================-------- nit = nit + 1 dpminks = dpmink fdks = fdk c.... Find end points of line segment for this fdl. c---- At fdk = 0. ex = ax + (dx - ax) * fdl c---- At fdk = 0. ey = ay + (dy - ay) * fdl c---- At fdk = 0. ez = az + (dz - az) * fdl c---- At fdk = 1. fx = bx + (cx - bx) * fdl c---- At fdk = 1. fy = by + (cy - by) * fdl c---- At fdk = 1. fz = bz + (cz - bz) * fdl c.... Find better values of dpmink, fdk, qxi, qyi, qzi. call aptptln (px, py, pz, ex, ey, ez, fx, fy, fz, 1, tol, & noptfd, dpmink, fdk, qxi, qyi, qzi, & nlimk, itrun, nerrline) c.... Find the normal vector at this fdl, new fdk. vnx = vnax + fdk * (vnbx - vnax) + fdl * (vndx - vnax) vny = vnay + fdk * (vnby - vnay) + fdl * (vndy - vnay) vnz = vnaz + fdk * (vnbz - vnaz) + fdl * (vndz - vnaz) vnsq = vnx**2 + vny**2 + vnz**2 c.... Give proper sign to dpmink. fract = ((px - qxi) * vnx + (py - qyi) * vny + & (pz - qzi) * vnz) / (vnsq + fuz) dpmink = sign (dpmink, fract) cbugc***DEBUG begins. cbug write ( 3, 9903) nprob, ng, nit, fdk, fdl, dpmink cbug write ( 3, 9904) qxi, qyi, qzi cbugc***DEBUG ends. c.... Test for convergence, save best results. if ((dpmink .eq. 0.0) .or. & ((abs (dpmink - dpminks) .le. & 0.5 * tol * (abs (dpmink) + abs (dpminks))))) then if (abs (dpmink) .lt. abs (dpmin)) then dpmin = dpmink fdkq = fdk fdlq = fdl qx = qxi qy = qyi qz = qzi endif cbugc***DEBUG begins. cbug write ( 3, "(/)") cbug write ( 3, 9905) nprob, nit cbug write (59, 9905) nprob, nit cbug write ( 3, 9903) nprob, ng, nit, fdk, fdl, dpmink cbug write ( 3, 9904) qxi, qyi, qzi cbug write ( 3, "(/ 10('********'))") cbugc***DEBUG ends. if (dpmin .eq. 0.0) go to 130 go to 120 endif c============================= guess new fdk ===========================-------- c.... See if ok to extrapolate fdk ahead. if ((nit .ge. 4) .and. (nlimk .ne. 2) .and. & (nliml .ne. 2) .and. & (abs (dpminl) .lt. abs (dpminks))) then c.... Extrapolate. First try tangent plane method. fdkz = fdk c++++ Dist from "p" to tangent plane. dpmine = fract * sqrt (vnsq) slopek = (fdk - fdks) / (dpmink - dpminks + fuz) slopel = (fdl - fdls) / (dpminl - dpminls + fuz) fdke = fdk + 1.3 * slopek * (dpmine - dpmink) fdle = fdl + 1.3 * slopel * (dpmine - dpminl) c.... See if fdle ok. if ((noptfd .ne. 2) .or. & ((fdle .gt. 0.0) .and. (fdle .lt. 1.0))) then c.... See if fdk direction ok. if (sign (1.0, fdke - fdk) .eq. & sign (1.0, fdk - fdks)) then c.... See if fdl direction ok. if (sign (1.0, fdle - fdl) .eq. & sign (1.0, fdl - fdls)) then fdk = amax1 (fdk - 0.1, amin1 (fdke, fdk + 0.1)) cbugc***DEBUG begins. cbug 9906 format (19x,'est fdk=',f9.6,' fdl=',f9.6,' dpmin=',1pe18.10, cbug & ' est') cbug 9907 format (9x,'est qx=',1pe13.5,' qy=',1pe13.5,' qz= ',1pe13.5, cbug & ' est') cbug qxe = px - fract * vnx cbug qye = py - fract * vny cbug qze = pz - fract * vnz cbug write ( 3, 9907) qxe, qye, qze cbug write ( 3, 9906) fdke, fdle, dpmine cbugc***DEBUG ends. c---- If fdl direction ok. endif c---- If fdk direction ok. endif c---- If fdle ok. endif c.... Then try acceleration method. if (abs (fdk - fdkz) .lt. abs (fdkz - fdks)) then if ((noptfd .ne. 2) .or. & ((fdl .gt. 0.0) .and. (fdl .lt. 1.0))) then c.... Find cosecant of angle between fdk lines and fdl lines. dlx = bx - ax + fdl * (ax - bx + cx - dx) dly = by - ay + fdl * (ay - by + cy - dy) dlz = bz - az + fdl * (az - bz + cz - dz) dlsq = dlx**2 + dly**2 + dlz**2 dkx = dx - ax + fdk * (ax - bx + cx - dx) dky = dy - ay + fdk * (ay - by + cy - dy) dkz = dz - az + fdk * (az - bz + cz - dz) dksq = dkx**2 + dky**2 + dkz**2 cscth = sqrt (dlsq * dksq / (vnsq + fuz)) accel = cscth fdke = fdk + accel * (fdk - fdks) fdk = amax1 (fdk - 0.1, amin1 (fdke, fdk + 0.1)) cbugc***DEBUG begins. cbug 9908 format (5x,'cscth=',1pe13.5,' accel=',1pe13.5) cbug write ( 3, 9908) cscth, accel cbugc***DEBUG ends. c---- If fdl between 0.0 and 1.0. endif c---- Used acceleration method. endif if (noptfd .eq. 2) then fdk = amax1( 0.0, amin1 (fdk, 1.0)) endif c---- Extrapolated fdk ahead. endif c=======================================================================-------- c---- End of iteration loop. go to 110 c---- End of loop over 3 starting points. 120 continue c.... Found solution. See if inside quadrangle. 130 nside = 1 c---- Inside is 0.0 to 1.0. if (noptfd .eq. 0) then if ((fdkq .lt. 0.0) .or. (fdkq .gt. 1.0) .or. & (fdlq .lt. 0.0) .or. (fdlq .gt. 1.0)) then nside = 0 endif c---- Inside is tol to 1.0 - tol. else if ((fdkq .lt. tol) .or. (fdkq .gt. 1.0 - tol) .or. & (fdlq .lt. tol) .or. (fdlq .gt. 1.0 - tol)) then nside = 0 endif c---- Tested noptfd. endif 210 return c.... End of subroutine aptquad. (+1 line.) end UCRL-WEB-209832