subroutine aptintq (px, py, pz, & ac, ax, ay, az, axy, ayz, azx, axx, ayy, azz, & bc, bx, by, bz, bxy, byz, bzx, bxx, byy, bzz, & tol, aquint, tax, tay, taz, tbx, tby, tbz, & dx, dy, dz, dist, nerr) ccbeg. cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc c c SUBROUTINE APTINTQ c c call aptintq (px, py, pz, c & ac, ax, ay, az, axy, ayz, azx, axx, ayy, azz, c & bc, bx, by, bz, bxy, byz, bzx, bxx, byy, bzz, c & tol, aquint, tax, tay, taz, tbx, tby, tbz, c & dx, dy, dz, dist, nerr) c c Version: aptintq Updated 1998 May 14 17:20. c aptintq Originated 1997 December 18 11:50. c c Author: Arthur L. Edwards, LLNL, L-298, Telephone (925) 422-4123. c c c Purpose: To search for, starting with point p = (px, py, pz), the c proximal points ta = (tax, tay, taz) and tb = (tbx, tby, tbz), c and the direction d = (dx, dy, dz) and distance dist between c ta and tb, or an intersection point (ta = tb), of the two c quadric surfaces with the implicit equations: c c A = ac + ax * x + ay * y + az * z c + axy * x * y + ayz * y * z + azx * z * x c + axx * x**2 + ayy * y**2 + azz * z**2 = 0 c B = bc + bx * x + by * y + bz * z c + bxy * x * y + byz * y * z + bzx * z * x c + bxx * x**2 + byy * y**2 + bzz * z**2 = 0 c c The two quadric surfaces have the normal vectors: c c an = (anx, any, anz) = grad (A) c anx = ax + 2 * axx * x + axy * y + azx * z c any = ay + axy * x + 2 * ayy * y + ayz * z c anz = az + azx * x + ayz * y + 2 * azz * z c c bn = (bnx, bny, bnz) = grad (B) c bnx = bx + 2 * bxx * x + bxy * y + bzx * z c bny = by + bxy * x + 2 * byy * y + byz * z c bnz = bz + bzx * x + byz * y + 2 * bzz * z c c The method is iterative, starting with the initial guess c (x, y, z) = (px, py, pz). Each step of the iteration, the c estimated intersection or proximal point is successively c replaced by its proximal point on the other surface (which c works well for surfaces that are highly curved or meet nearly c orthogonally). c c Convergence is determined by the precision limit tol, which c should be in the range from 1.e-5 to 1.e-15 for 64-bit c floating point arithmetic. c c Input: px, py, pz, c ac, ax, ay, az, axy, ayz, azx, axx, ayy, azz, c bc, bx, by, bz, bxy, byz, bzx, bxx, byy, bzz, tol. c c Output: aquint, tax, tay, taz, tbx, tby, tbz, nerr. c c Calls: aptplpl, aptvdis, aptwhis, aptvaxb c c c Glossary: c c ac, ... Input Coefficients of the implicit equation for the first c quadric surface: c ac + ax * x + ay * y + az * z + c axy * x * y + ayz * y * z + azx * z * x + c axx * x**2 + ayy * y**2 + azz * z**2 = 0 c c aquint Output An 8-character ASCII word indicating the nature of c points "ta" and "tb" on the two quadric surfaces: c c "intcurve" Two coincident points on an intersection c curve of the two surfaces. c "proximal" Two nearest points on non-intersecting c surfaces. c "tangent" Two coincident points at a tangency of c the two surfaces. c "unknown" If nerr is not zero, method failed. c If nerr = 0, ambiguous results (probably c a tangency) c c bc, ... Input Coefficients of the implicit equation for the second c quadric surface: c bc + bx * x + by * y + bz * z + c bxy * x * y + byz * y * z + bzx * z * x + c bxx * x**2 + byy * y**2 + bzz * z**2 = 0 c c dist Output The distance from point "ta" to point "tb". c Zero if the surfaces intersect at point "ta". c c dx,dy,dz Output The x, y and z components of the vector from point c "ta" to point "tb". All zero if the surfaces c intersect at point "ta". c c nerr Output Indicates an input error, if not zero: c 1 if an equation of a quadric surface is bad. c 2 if the method fails to converge in 1000 iterations, c which may indicate too small a value of tol. c 3 if the the method diverges. c c px,py,pz Input The x, y, z coordinates of point "p", the initial guess c for the intersection point. c c tax,y,z Output Coordinates x, y and z at an intersection point of c surfaces A and B (dist = 0) or proximal point c on surface A (dist not zero), if nerr = 0. c Unless the two surfaces are tangent at a single c point, an intersection point is not unique, but c only one point on a continuous intersection curve, c usually one near point "p". c c tbx,y,z Output Coordinates x, y and z at an intersection point of c surfaces A and B (dist = 0) or proximal point c on surface B (dist not zero), if nerr = 0. c Unless the two surfaces are tangent at a single c point, an intersection point is not unique, but c only one point on a continuous intersection curve, c usually one near point "p". c c tol Input Numerical tolerance limit. With 64-bit floating point c numbers, 1.e-5 to 1.e-11 is recommended. c Used both as an absolute and relative convergence c criterion. Changes in point coordinates less then c 10 * tol are treated as insignificant. c cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc ccend. c.... Arguments. character*8 aquint ! "intcurve", "proximal" or "tangent". c.... Local variables. common /ltintq/ dprox(4) ! Distance to a proximal point. common /ltintq/ xx(4) ! Coordinate x of a proximal point. common /ltintq/ yy(4) ! Coordinate x of a proximal point. common /ltintq/ zz(4) ! Coordinate x of a proximal point. c=======================================================================******** c.... Initialize. nerr = 0 tolx = tol if (tolx .eq. 0.0) tolx = 1.e-11 tax = -1.e99 tay = -1.e99 taz = -1.e99 tbx = -1.e99 tby = -1.e99 tbz = -1.e99 dx = -1.e99 dy = -1.e99 dz = -1.e99 dist = -1.e99 aquint = "unknown" c=======================================================================******** cbugc***DEBUG begins. cbug 9901 format (/ 'aptintq finding intersection point of two', cbug & ' quadric surfaces.', cbug & ' tol=',1pe13.5) cbug 9902 format ('Initial guess:' / cbug & ' px, py, pz =',1p3e22.14 ) cbug 9903 format ('Quadric ac =',1pe22.14 / cbug & ' ax,ay,az =',1p3e22.14 / cbug & ' axy,ayz,azx=',1p3e22.14 / cbug & ' axx,ayy,azz=',1p3e22.14 ) cbug 9904 format ('Quadric bc =',1pe22.14 / cbug & ' bx,by,bz =',1p3e22.14 / cbug & ' bxy,byz,bzx=',1p3e22.14 / cbug & ' bxx,byy,bzz=',1p3e22.14 ) cbug write ( 3, 9901) tolx cbug write ( 3, 9902) px, py, pz cbug write ( 3, 9903) ac, ax, ay, az, axy, ayz, azx, axx, ayy, azz cbug write ( 3, 9904) bc, bx, by, bz, bxy, byz, bzx, bxx, byy, bzz cbugc***DEBUG ends. c.... Initialize for the iteration. tax = px tay = py taz = pz tbx = px tby = py tbz = pz sidea = -1.e99 sideb = -1.e99 kprox = 0 ! 1 if two proximal points found. kintr = 0 ! 1 if an intersection found. nitex = 100 ! Iteration to begin extrapolation method. nitstop = 1000 ! Iteration to stop. cbugcbugc***DEBUG begins. cbugcbug nit = 0 cbugcbug write ( 3, 9911) nit, tax, tay, taz cbugcbug write ( 3, 9912) nit, tbx, tby, tbz cbugcbugc***DEBUG ends. c=======================================================================******** c.... Iterate to find the solution. do 380 nit = 1, 1000 ! Iterate to find solution. c.... Replace point "ta" with the point on surface A proximal to point "tb". taxs = tax tays = tay tazs = taz sidebs = sideb call aptwhis (tbx, tby, tbz, & ac, ax, ay, az, axy, ayz, azx, axx, ayy, azz, & tolx, sideb, anx, any, anz, & nprox, xx, yy, zz, dprox, nerra) if ((nerra .ne. 0) .or. & (nprox .le. 0)) then cbugcbugc***DEBUG begins. cbugcbug 9913 format (/ 'DEBUG: nerra=',i2,' nprox=',i2) cbugcbug write ( 3, 9913) nerra, nprox cbugcbugc***DEBUG ends. nerr = 1 go to 999 endif tax = xx(1) tay = yy(1) taz = zz(1) c.... Find the vector "an" normal to surface A at new point "ta". call aptwhis (tax, tay, taz, & ac, ax, ay, az, axy, ayz, azx, axx, ayy, azz, & tolx, sideaa, anx, any, anz, & nprox, xx, yy, zz, dprox, nerra) cbugcbugc***DEBUG begins. cbugcbug 9911 format (/ i4,1x,'tax,y,z =',1p3e22.14 ) cbugcbug write ( 3, 9911) nit, tax, tay, taz cbugcbugc***DEBUG ends. c.... Replace point "tb" with the point on surface B proximal to point "ta". tbxs = tbx tbys = tby tbzs = tbz sideas = sidea call aptwhis (tax, tay, taz, & bc, bx, by, bz, bxy, byz, bzx, bxx, byy, bzz, & tolx, sidea, bnx, bny, bnz, & nprox, xx, yy, zz, dprox, nerra) if ((nerra .ne. 0) .or. & (nprox .le. 0)) then cbugcbugc***DEBUG begins. cbugcbug write ( 3, 9913) nerra, nprox cbugcbugc***DEBUG ends. nerr = 1 go to 999 endif tbx = xx(1) tby = yy(1) tbz = zz(1) c.... Find the vector "bn" normal to surface B at new point "tb". call aptwhis (tbx, tby, tbz, & bc, bx, by, bz, bxy, byz, bzx, bxx, byy, bzz, & tolx, sidebb, bnx, bny, bnz, & nprox, xx, yy, zz, dprox, nerra) cbugcbugc***DEBUG begins. cbugcbug 9912 format (i4,1x,'tbx,y,z =',1p3e22.14 ) cbugcbug write ( 3, 9912) nit, tbx, tby, tbz cbugcbugc***DEBUG ends. cbugcbugc***DEBUG begins. cbugcbug 9801 format (i4,1x,'dtax,y,z=',1p3e22.14 ) cbugcbug 9802 format (i4,1x,'dtbx,y,z=',1p3e22.14 ) cbugcbug 9803 format (i4,1x,'erax,y,z=',1p3e22.14 ) cbugcbug 9804 format (i4,1x,'erbx,y,z=',1p3e22.14 ) cbugcbug dtax = tax - taxs cbugcbug dtay = tay - tays cbugcbug dtaz = taz - tazs cbugcbug dtbx = tbx - tbxs cbugcbug dtby = tby - tbys cbugcbug dtbz = tbz - tbzs cbugcbug write ( 3, 9801) nit, dtax, dtay, dtaz cbugcbug write ( 3, 9802) nit, dtbx, dtby, dtbz cbugcbug erax = tolx * (10.0 + abs (taxs)) cbugcbug eray = tolx * (10.0 + abs (tays)) cbugcbug eraz = tolx * (10.0 + abs (tazs)) cbugcbug erbx = tolx * (10.0 + abs (tbxs)) cbugcbug erby = tolx * (10.0 + abs (tbys)) cbugcbug erbz = tolx * (10.0 + abs (tbzs)) cbugcbug write ( 3, 9803) nit, erax, eray, eraz cbugcbug write ( 3, 9804) nit, erbx, erby, erbz cbugcbugc***DEBUG ends. c.... See if the test points have converged. if (kprox .eq. 2) go to 410 if ((abs (tax - taxs) .le. tolx * (10.0 + abs (taxs))) .and. & (abs (tay - tays) .le. tolx * (10.0 + abs (tays))) .and. & (abs (taz - tazs) .le. tolx * (10.0 + abs (tazs))) .and. & (abs (tbx - tbxs) .le. tolx * (10.0 + abs (tbxs))) .and. & (abs (tby - tbys) .le. tolx * (10.0 + abs (tbys))) .and. & (abs (tbz - tbzs) .le. tolx * (10.0 + abs (tbzs)))) then cbugcbugc***DEBUG begins. cbugcbug 9915 format ('Converged in',i5,' iterations (ta and tb).') cbugcbug write ( 3, 9915) nit cbugcbugc***DEBUG ends. kprox = kprox + 1 endif c.... Try method of extrapolating to an intersection point. if ((nit .eq. nitex) .and. (kprox .eq. 0)) then ! Try extrapolation. cold call aptlnln (taxs, tays, tazs, tax, tay, taz, cold & tbxs, tbys, tbzs, tbx, tby, tbz, 1, tolx, cold & dpmin, fracab, fraccd, cold & taxe, taye, taze, tbxe, tbye, tbze, cold & itrun, ipar, nerr) call aptplpl (tax, tay, taz, anx, any, anz, & tbx, tby, tbz, bnx, bny, bnz, & 1, tolx, & taxe, taye, taze, tbxe, tbye, tbze, & ux, uy, uz, ipar, dpmin, itrun, nerra) if ((ipar .eq. 0) .and. & (itrun .eq. 0) .and. & (nerra .eq. 0)) then cbugcbugc***DEBUG begins. cbugcbug 9711 format (i4,1x,'tbx,y,ze=',1p3e22.14 ) cbugcbug 9712 format (i4,1x,'tbx,y,ze=',1p3e22.14 ) cbugcbug write ( 3, 9711) nit, taxe, taye, taze cbugcbug write ( 3, 9712) nit, tbxe, tbye, tbze cbugcbugc***DEBUG ends. tax = taxe tay = taye taz = taze tbx = tbxe tby = tbye tbz = tbze sideas = -1.e99 sidebs = -1.e99 nitex = nitex + 2 endif endif ! Tried extrapolation method. c.... See if the test points are at an intersection point. cbugcbugc***DEBUG begins. cbugcbug 9921 format (i4,1x,'sidea,b =',1p2e22.14 ) cbugcbug write ( 3, 9921) nit, sidea, sideb cbugcbugc***DEBUG ends. if ((sidea .eq. 0.0) .and. & (sideb .eq. 0.0)) then cbugcbugc***DEBUG begins. cbugcbug 9914 format ('Converged in',i5,' iterations (sidea and sideb).') cbugcbug write ( 3, 9914) nit cbugcbugc***DEBUG ends. kintr = 1 if (kprox .gt. 0) go to 410 endif c.... See if the method is diverging. if ((abs (sidea) .gt. abs (sideas)) .and. & (abs (sideb) .gt. abs (sidebs))) then cbugcbugc***DEBUG begins. cbugcbug 9931 format ('aptintq diverging. sidea and sideb increasing.') cbugcbug write ( 3, 9931) cbugcbugc***DEBUG ends. nerr = 3 go to 999 endif 380 continue ! End of each iteration. nit = nit - 1 cbugcbugc***DEBUG begins. cbugcbug 9961 format (/ 'aptintq failed to converge in',i5,' iterations.') cbugcbug write ( 3, 9961) nit cbugcbugc***DEBUG ends. nerr = 2 ! Failed to converge. go to 999 410 continue c.... Find the distance between points "ta" and "tb". call aptvdis (tax, tay, taz, tbx, tby, tbz, 1, tolx, & dx, dy, dz, dist, nerra) c.... Find the cross product of the two normal vectors at "ta" and "tb". call aptvaxb (anx, any, anz, bnx, bny, bnz, 1, tolx, & dnx, dny, dnz, vlength, nerra) c.... Find the nature of points "ta" and "tb". if ((sidea .eq. 0.0) .and. (sideb .eq. 0.0)) then if (dist .eq. 0.0) then if (vlength .eq. 0.0) then aquint = "tangent" else aquint = "intcurve" endif endif elseif ((sidea .ne. 0.0) .and. (sideb .ne. 0.0)) then if ((dist .ne. 0.0) .and. (vlength .eq. 0.0)) then aquint = "proximal" endif endif c=======================================================================******** 999 continue cbugc***DEBUG begins. cbug 9991 format (/ 'aptintq results. nerr=',i2,' nit=',i4, cbug & ' kprox=',i2,' kintr=',i2,' aquint=',a8) cbug 9992 format ('tax,tay,taz =',1p3e22.14 ) cbug 9993 format ('tbx,tby,tbz =',1p3e22.14 ) cbug 9994 format ('sidea, sideb =',1p2e22.14) cbug 9995 format ('vlength =',1pe22.14 ) cbug 9996 format ('dx, dy, dz =',1p3e22.14 / cbug & 'dist =',1pe22.14 ) cbug 9997 format ('anx, any, anz=',1p3e22.14 ) cbug 9998 format ('bnx, bny, bnz=',1p3e22.14 ) cbug write ( 3, 9991) nerr, nit, kprox, kintr, aquint cbug write ( 3, 9992) tax, tay, taz cbug write ( 3, 9993) tbx, tby, tbz cbug write ( 3, 9994) sidea, sideb cbug write ( 3, 9995) vlength cbug write ( 3, 9996) dx, dy, dz, dist cbug write ( 3, 9997) anx, any, anz cbug write ( 3, 9998) bnx, bny, bnz cbugc***DEBUG ends. return c.... End of subroutine aptintq. (+1 line) end UCRL-WEB-209832