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