subroutine aptqprt (ax, ay, az, bx, by, bz, qc, qx, qy, qz, & qxy, qyz, qzx, qxx, qyy, qzz, tol, & niter, nprox, cx, cy, cz, dist, dx, dy, dz, & snx, sny, snz, costh, nerr) ccbeg. cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc c c SUBROUTINE APTQPRT c c call aptqprt (ax, ay, az, bx, by, bz, qc, qx, qy, qz, c qxy, qyz, qzx, qxx, qyy, qzz, tol, c niter, nprox, cx, cy ,cz, dist, dx, dy, dz, c snx, sny, snz, costh, nerr) c c Version: aptqprt Updated 1994 December 20 12:50. c aptqprt Originated 1994 November 3 11:00. c c Author: Arthur L. Edwards, LLNL, L-298, Telephone (925) 422-4123. c c c Purpose: To try to find the proximal point "c" = (cx, cy, cz) on a c quadric surface, nearest point "a" = (ax, ay, az), with an c initial trial direction "b" = (bx, by, bz). c c Note: use APT subroutine aptwhis to find the proximal point. c Use this subroutine only if aptwhis fails. c c The quadric surface is represented by: c c f(x,y,z) = qc + qx * x + qy * y + qz * z + c qxy * x * y + qyz * y * z + qzx * z * x + c qxx * x**2 + qyy * y**2 + qzz * z**2 = 0, c c The method is to try various directions from point "a", starting c with direction "b", and the directions to any extrema and axial c intercept points on the quadric surface, and the x, y and z c directions, saving the direction of the minimum distance. c c Then, for the nearest intersection point c" on the quadric c surface, the center of curvature of the quadric curve through c the quadric surface (in the plane of the trial direction, point c "c" and the normal vector at point "c") is found. If that c center is toward point "a", it is moved an infinite distance in c the opposite direction. The new trial direction points at the c new center of curvature. If that direction fails to decrease c the distance, a new trial direction intermediate between that c direction and the last successful one is tried. If a smaller c distance is found, a new center of curvature is found, and the c process repeated. The number of iterations required is niter. c c The final vector from point "a" to point "c" is vector c "d" = (dx, dy, dz) = (cx - ax, cy - ay, cz - az). c c The vector normal to the quadric surface at point "c" is c "sn" = (snx, sny, snz) = grad f: c snx = qx + 2.0 * qxx * cx + qxy * cy + qzx * cz, c sny = qy + qxy * cx + 2.0 * qyy * cy + qyz * cz, c snz = qz + qzx * cx + qyz * cy + 2.0 * qzz * cz, c and the cosine of the angle between vector "d" and the normal c vector "sn" is costh, which is -1 or 1 if the method converges c (nprox = 1). costh = d dot sn / (|d|*|sn|). c c Any result less than the estimated error in its calculation, c based on tol, will be truncated to zero. c c Flag nerr indicates any input error. c c Input: ax, ay, az, bx, by, bz, qc, qx, qy, qz, qxy, qyz, qzx, c qxx, qyy, qzz, tol. c c Output: niter, nprox, cx, cy, cz, dist, dx, dy, dz, snx, sny, snz, c costh, nerr. c c Calls: aptrkis, aptvusz c c c Glossary: c c ax,ay,az Input The x, y, z coordinates of point "a". c c bx,by,bz Input The x, y, z components of the first trial vector "b". c c costh Output The cosine of the angle between vector "d" (from point c "a" to point "c") and the vector normal to the c quadric surface at point "c". If nprox = 1, costh c should be either -1.0 or 1.0. c c cx,cy,cz Output The x, y, z coordinates of point "c" on the quadric c surface, found nearest to point "a". If nprox = 1, c the proximal point, to within tolerance limit tol. c c dx,dy,dz Output The x, y, z components of the vector from point "a" to c point "c". c c dist Output The distance from point "a" to point "c". c If nprox = 1, the proximal distance, to within c tolerance limit tol. c c nerr Output Indicates an input error, if not 0. c 1 if vector "b" is too small. c c niter Output The number of directions tried before convergence c (nprox = 1) or failure to converge (nprox = 0). c c nprox Output Indicates the results: c 1 if an exact proximal point "c" was found, within c tolerance limit tol. c 0 if the method did not converge in 1000 iterations. c The best results are returned. c Negative if the method did not converge, and the c results are probably bad: c -1 if the coefficients qc, ..., qzz do not describe c a quadric surface, or if all of the initial trial c directions miss. c -2 if the normal vector "sn" was zero. c -3 if the correction to trial vector "d" was too c small. c -4 if the distance did not decrease for 100 c successive iterations. c -5 if a trial vector "d" was zero. c c q.. Input Coefficients of the implicit equation of a quadric c surface in xyz space (qc, qx, qy, qz, qxy, qyz, qzx, c qxx, qyy, qzz). c c snx,... Output The x, y, z components of the vector normal to the c quadric surface at point "c". c c tol Input Numerical tolerance limit. c On Cray computers, recommend 1.e-5 to 1.e-11. c cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc ccend. c.... Local variables. common /laptqprt/ ccx(4) ! X coordinate of surface point. common /laptqprt/ ccy(4) ! Y coordinate of surface point. common /laptqprt/ ccz(4) ! Z coordinate of surface point. common /laptqprt/ ddist(4) ! Distance to quadric surface. common /laptqprt/ aqp(25) ! Type of surface point. character*8 aqp common /laptqprt/ qpx(25) ! Coordinate x of surface point. common /laptqprt/ qpy(25) ! Coordinate y of surface point. common /laptqprt/ qpz(25) ! Coordinate z of surface point. cbugc***DEBUG begins. cbug 9901 format (/ 'aptqprt finding the distance from point "a"' / cbug & ' to the quadric surface with the coefficients qc-qzz,' / cbug & ' and initial trial direction "b", by tangents:' / cbug & ' ax, ay, az =',1p3e22.14 / cbug & ' bx, by, bz =',1p3e22.14 / cbug & ' qc =',1pe22.14 / cbug & ' qx, qy, qz =',1p3e22.14 / cbug & ' qxy,qyz,qzx=',1p3e22.14 / cbug & ' qxx,qyy,qzz=',1p3e22.14 ) cbug write ( 3, 9901) ax, ay, az, bx, by, bz, cbug & qc, qx, qy, qz, qxy, qyz, qzx, qxx, qyy, qzz cbugc***DEBUG ends. c.... Initialize output variables. nerr = 0 niter = 0 nprox = 0 dist = -1.e99 cx = -1.e99 cy = -1.e99 cz = -1.e99 dx = -1.e99 dy = -1.e99 dz = -1.e99 snx = -1.e99 sny = -1.e99 snz = -1.e99 costh = -1.e99 c.... Test for errors. call aptvusz (bx, by, bz, tol, ubx, uby, ubz, blen) if (blen .eq. 0.0) then nerr = 1 nprox = -1 go to 710 endif c.... Initialize local variables. ndecr = 0 ! # of times distance decreased. nsmall = 0 ! # of times correction too small. distmin = 1.e99 ! Minimum distance to quadric surface. c=======================================================================******** c.... Find any extrema and axis intercepts on the quadric surface. call aptqupt (qc, qx, qy, qz, qxy, qyz, qzx, qxx, qyy, qzz, & tol, aqp, qpx, qpy, qpz, nqp, nerr) c... Add specified direction "b", and x, y and z directions. nqp = nqp + 1 qpx(nqp) = ax + bx qpy(nqp) = ay + by qpz(nqp) = az + bz nqp = nqp + 1 qpx(nqp) = ax + 1.0 qpy(nqp) = ay + 0.0 qpz(nqp) = az + 0.0 nqp = nqp + 1 qpx(nqp) = ax + 0.0 qpy(nqp) = ay + 1.0 qpz(nqp) = az + 0.0 nqp = nqp + 1 qpx(nqp) = ax + 0.0 qpy(nqp) = ay + 0.0 qpz(nqp) = az + 1.0 c=======================================================================******** c.... Find the minimum distance to any of the extrema and intercept point. do 110 nd = 1, nqp ! Loop over extrema and intercept points. call aptvdis (ax, ay, az, qpx(nd), qpy(nd), qpz(nd), 1, tol, & vx, vy, vz, vlen, nerra) if (vlen .eq. 0.0) go to 110 ux = vx / vlen uy = vy / vlen uz = vz / vlen cbugcbugc***DEBUG begins. cbugcbug 9931 format (/ ' Initial vx,vy,vz',1p3e20.12) cbugcbug write ( 3, 9931) vx, vy, vz cbugcbugc***DEBUG ends. c.... Find the intersection of the track from point "a" in direction "v". niter = niter + 1 dintmn = -distmin * (1.0 + tol) dintmx = distmin * (1.0 + tol) call aptrkis (ax, ay, az, vx, vy, vz, & qc, qx, qy, qz, qxy, qyz, qzx, qxx, qyy, qzz, & dintmn, dintmx, 1, tol, & nint, sx, sy, sz, distest, & ssnx, ssny, ssnz, cosn, nerra) if (nint .eq. 0) go to 110 if (distest .lt. 0.0) then distest = -distest cosn = -cosn ux = -ux uy = -uy uz = -uz endif ndecr = ndecr + 1 cx = sx cy = sy cz = sz distmin = abs (distest) dist = distest dx = ux * distest dy = uy * distest dz = uz * distest snx = ssnx sny = ssny snz = ssnz costh = cosn cbugcbugc***DEBUG begins. cbugcbug 9932 format (/ 'Proximal x,y,z ?',1p3e20.12 / cbugcbug & 'Proximal dx,dy,dz?',1p3e20.12 / cbugcbug & ' Approx distance?',1pe20.12,' Test',i8 ) cbugcbug 9933 format ( ' Cosine of angle ',1pe20.12,' (from normal)', cbugcbug & 5x,1pe20.12 ) cbugcbug power = 0.0 cbugcbug write ( 3, 9932) cx, cy, cz, dx, dy, dz, dist, ndecr cbugcbug write ( 3, 9933) costh, power cbugcbugc***DEBUG ends. if (distmin .eq. 0.0) then nprox = 1 costh = 1.0 go to 710 endif 110 continue ! End of loop over extrema and intercepts. if (ndecr .eq. 0) then ! No intersections. nprox = -1 go to 710 endif delcos = 1.0 - abs (costh) if (delcos .le. tol) then ! Found an exact proximal point. nprox = 1 go to 710 endif c=======================================================================******** c.... Try to find the proximal point by using the normal vector at the surface c.... and the center of curvature in the plane of the surface point, the c.... normal vector, and the test vector, or by using the tangent plane at the c.... surface point. c.... Return here each time distance decreases. 140 nincr = 0 nmiss = 0 c.... Find the length of the normal vector "sn" at point "c". call aptvusz (snx, sny, snz, tol, usnx, usny, usnz, snlen) if (snlen .eq. 0.0) then nprox = -2 go to 710 endif c.... Find the negative of the component of "d" perpendicular to "sn", c.... call it "ss". This will be the correction to "d" if the surface c.... curves toward point "a". ddotsn = (dx * snx + dy * sny + dz * snz) / snlen ssx = ddotsn * usnx - dx ssy = ddotsn * usny - dy ssz = ddotsn * usnz - dz call aptvusz (ssx, ssy, ssz, tol, usx, usy, usz, sslen) c.... Find the radius of curvature, rcurve, at the surface point "c", for the c.... quadric curve tangent to vector "ss" (in the plane of "d" and "sn"). c.... The value of rcurve is positive if the center is in the same direction c..... from "c" as "sn". Note: rr is the reciprocal of rcurve. rr = -((2.0 * qxx*usx + qxy*usy + qzx*usz) * usx + & ( qxy*usx + 2.0 * qyy*usy + qyz*usz) * usy + & ( qzx*usx + qyz*usy + 2.0 * qzz*usz) * usz) / & snlen c.... Limit the magnitude of the radius of curvature to snlen / (tol + 1.e-99). rrmin = (tol + 1.e-99) / snlen if (abs (rr) .lt. rrmin) then if (rr .ge. 0.0) then rr = rrmin else rr = -rrmin endif endif rcurve = 1.0 / rr c.... Find the vector from the surface point "c" to the center of curvature. c.... This is the correction to the test direction for the curvature method. scx = rcurve * usnx scy = rcurve * usny scz = rcurve * usnz cbugcbugc***DEBUG begins. cbugcbug 9902 format (/ '* Radius of curve ',1pe20.12 / cbugcbug & '* Center of curve ',1p3e20.12 ) cbugcbug 9903 format ( '* New test vector ',1p3e20.12) cbugcbug 9904 format ( '* Chk test vector ',1p3e20.12) cbugcbug cbugcbugc.... Find the center of curvature, cenc, relative to the surface point "c". cbugcbug cbugcbug cencx = cx + scx cbugcbug cency = cy + scy cbugcbug cencz = cz + scz cbugcbug cbugcbug write ( 3, 9902) rcurve, cencx, cency, cencz cbugcbug cbugcbug dnewx = dx + scx cbugcbug dnewy = dy + scy cbugcbug dnewz = dz + scz cbugcbug call aptvusz (dnewx, dnewy, dnewz, tol, cbugcbug & unewx, unewy, unewz, dnewlen) cbugcbug dnewx = unewx * dist cbugcbug dnewy = unewy * dist cbugcbug dnewz = unewz * dist cbugcbug cbugcbug write ( 3, 9903) dnewx, dnewy, dnewz cbugcbug cbugcbug vchkx = cencx - ax cbugcbug vchky = cency - ay cbugcbug vchkz = cencz - az cbugcbug call aptvusz (vchkx, vchky, vchkz, tol, cbugcbug & uchkx, uchky, uchkz, vchklen) cbugcbug vchkx = uchkx * dist cbugcbug vchky = uchky * dist cbugcbug vchkz = uchkz * dist cbugcbug cbugcbug write ( 3, 9904) vchkx, vchky, vchkz cbugcbugc***DEBUG ends. c.... Find the relative direction of direction "d" and vector "sc". scdotd = scx * dx + scy * dy + scz * dz c.... Use the tangent plane method if the quadric curve at point "c" in the c.... plane of vectors "ac" and "sn" is concave relative to point "a". if (scdotd .le. 0.0) then scx = ssx scy = ssy scz = ssz endif c.... Interpolate between last guess and new guess if angle worse. fact = 1.0 ! Multiplier for test vector corrector. if (abs (costh) .lt. abs (cosths)) fact = 0.5 c.... Return here each time distance increases or angle diverges. c.... Find the correction to the last test direction "d". 150 scfx = fact * scx scfy = fact * scy scfz = fact * scz scfsq = scfx**2 + scfy**2 + scfz**2 if (scfsq .le. tol**2 * distmin**2) then nsmall = nsmall + 1 if (nsmall .gt. 1) then nprox = -3 go to 710 endif endif cbugcbugc***DEBUG begins. cbugcbug 9905 format (/ '* distmin,fact,tol',1p3e20.12) cbugcbug 9906 format ( '* old test vector ',1p3e20.12) cbugcbug 9907 format ( '* tot SC ',1p3e20.12) cbugcbug 9908 format ( '* adj SC ',1p3e20.12) cbugcbug 9909 format ( '* PS + tot SC ',1p3e20.12) cbugcbug write ( 3, 9905) distmin, fact, tol cbugcbug write ( 3, 9906) dx, dy, dz cbugcbug write ( 3, 9907) scx, scy, scz cbugcbug write ( 3, 9908) scfx, scfy, scfz cbugcbug vtotx = dx + scx cbugcbug vtoty = dy + scy cbugcbug vtotz = dz + scz cbugcbug write ( 3, 9909) vtotx, vtoty, vtotz cbugcbugc***DEBUG ends. c.... Find the new test direction "dtest". dtestx = dx + scfx dtesty = dy + scfy dtestz = dz + scfz cbugcbugc***DEBUG begins. cbugcbug 9910 format ('* new test vector ',1p3e20.12) cbugcbug 9911 format ('* cosine dtest.NS ',1pe20.12) cbugcbug write ( 3, 9910) dtestx, dtesty, dtestz cbugcbug call aptvang (dtestx, dtesty, dtestz, snx, sny, snz, 1, tol, cbugcbug & cosna, nerra) cbugcbug write ( 3, 9911) cosna cbugcbugc***DEBUG ends. c.... Find the length and unit vector of the new test direction "dtest". call aptvusz (dtestx, dtesty, dtestz, tol, & utestx, utesty, utestz, dtestlen) if (dtestlen .eq. 0.0) then nprox = -5 go to 710 endif c.... Find the intersection "c" of the track from point "a" in the new test c.... direction "dtest". niter = niter + 1 dintmn = -distmin * (1.0 + tol) dintmx = distmin * (1.0 + tol) call aptrkis (ax, ay, az, dtestx, dtesty, dtestz, & qc, qx, qy, qz, & qxy, qyz, qzx, & qxx, qyy, qzz, & dintmn, dintmx, 1, tol, & nint, sx, sy, sz, distest, & snx, sny, snz, cosn, nerra) if (nint .eq. 0) then nincr = nincr + 1 cbugcbugc***DEBUG begins. cbugcbug 9912 format (/ ' Distance increased, or missed. Failure #',i6) cbugcbug 9916 format ('Proximal x,y,z ?',1p3e20.12 / cbugcbug & 'Proximal dx,dy,dz?',1p3e20.12 / cbugcbug & ' Approx distance<',1pe20.12 ) cbugcbug cbugcbug write ( 3, 9912) nincr cbugcbug write ( 3, 9916) sx, sy, sz, cbugcbug & dtestx, dtesty, dtestz, distest cbugcbug write ( 3, 9914) cosn cbugcbugc***DEBUG ends. fact = 0.1 * fact if (nincr .ge. 100) then nprox = -4 go to 710 endif go to 150 endif c.... Found a distance smaller than distmin * (1.0 + tol). if (distest .lt. 0.0) then distest = -distest cosn = -cosn utestx = -utestx utesty = -utesty utestz = -utestz endif cbugcbugc***DEBUG begins. cbugcbug 9913 format ('* norm vect at int',1p3e20.12) cbugcbug 9914 format (' Cosine of angle ',1pe20.12,' (from normal)' ) cbugcbug write ( 3, 9913) snx, sny, snz cbugcbug write ( 3, 9914) cosn cbugcbugc***DEBUG ends. c.... Normalize the test vector "d" to the new distance from "a" to "c". dtestx = utestx * distest dtesty = utesty * distest dtestz = utestz * distest distabs = abs (distest) ndecr = ndecr + 1 if (distabs .lt. distmin) distmin = distabs ! Minimum distance found. dist = distest cosths = costh costh = cosn dx = dtestx dy = dtesty dz = dtestz cx = sx cy = sy cz = sz cbugcbugc***DEBUG begins. cbugcbug 9915 format (/ 'Proximal x,y,z ?',1p3e20.12 / cbugcbug & 'Proximal dx,dy,dz?',1p3e20.12 / cbugcbug & ' Approx distance?',1pe20.12, cbugcbug & ' Iteration # ',i6,'.' ) cbugcbug write ( 3, 9915) cx, cy, cz, dx, dy, dz, dist, niter cbugcbug write ( 3, 9914) costh cbugcbugc***DEBUG ends. cbugcbugc.... DEBUG begins. cbugcbugc.... Find the ratio |d| / |N| = dx / snx = dy / sny = dz / snz. cbugcbug ratx = dx / (snx + 1.e-200) cbugcbug raty = dy / (sny + 1.e-200) cbugcbug ratz = dz / (snz + 1.e-200) cbugcbug snlen = sqrt (snx**2 + sny**2 + snz**2) cbugcbug ratk = -dist / snlen cbugcbug 9917 format ('ratx,y,z= ',1p3e22.14 / cbugcbug & 'ratk, d, N ',1p3e22.14) cbugcbug 9918 format ('Proximal x,y,z ???',1p3e20.12) cbugcbug write ( 3, 9917) ratx, raty, ratz, ratk, dist, snlen cbugcbug cxx = ax + dx cbugcbug cyy = ay + dy cbugcbug czz = az + dz cbugcbug write ( 3, 9918) cxx, cyy, czz cbugcbugc.... DEBUG ends. if ((abs (distabs - distmin) .le. tol * distmin) .and. & ((1.0 - abs (costh)) .le. tol)) then nprox = 1 go to 710 endif ! Tested for final convergence. if (ndecr .ge. 1000) then nprox = 0 go to 710 endif go to 140 c=======================================================================******** 710 continue cbugc***DEBUG begins. cbug 9922 format (/ 'aptqprt results: ndecr=',i8,' niter=',i8, cbug & ' nprox=',i2,' nerr=',i3 / cbug & ' dist =',1pe22.14 / cbug & ' cx, cy, cz =',1p3e22.14 / cbug & ' dx, dy, dz =',1p3e22.14 / cbug & ' snx,sny,snz=',1p3e22.14 / cbug & ' costh =',1pe22.14 ) cbug write ( 3, 9922) ndecr, niter, nprox, nerr, dist, cx, cy, cz, cbug & dx, dy, dz, snx, sny, snz, costh cbugc***DEBUG ends. return c.... End of subroutine aptqprt. (+1 line.) end UCRL-WEB-209832