subroutine aptparq (px, py, pz, vx, vy, vz, ax, ay, az,
& qc, qx, qy, qz, qxy, qyz, qzx,
& qxx, qyy, qzz, tol,
& nints, atype, ptx, pty, ptz,
& vtx, vty, vtz, vtlen, dint, tint, nerr)
ccbeg.
cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
c
c SUBROUTINE APTPARP
c
c call aptparq (px, py, pz, vx, vy, vz, ax, ay, az,
c & qc, qx, qy, qz, qxy, qyz, qzx, qxx, qyy, qzz, tol,
c & nints, atype, ptx, pty, ptz,
c & vtx, vty, vtz, vtlen, dint, tint, nerr)
c
c Version: aptparq Updated 1997 April 10 17:40.
c aptparq Originated 1997 April 10 17:40.
c
c Author: Arthur L. Edwards, LLNL, L-298, Telephone (925) 422-4123.
c
c
c Purpose: For the particle with initial coordinates p = (px, py, pz),
c initial velocity v = (vx, vy, vz) and constant acceleration
c a = (ax, ay, az), and for the quadric surface with the implicit
c equation qc + qx*x + qy*y + qz*z + qxy*x*y + qyz*y*z + qzx*z*x +
c qxx*x**2 + qyy*y**2 + qzz*z**2 = 0, to find the number nints
c (0, 1, 2, 3 or 4) of intersections of the particle path with
c the quadric surface, and for each such intersection point,
c the intersection type ('simple' or 'tangent'),
c the coordinates pt(n) = (ptx(n), pty(n), ptz(n)),
c the velocities vt(n) = (vtx(n), vty(n), vtz(n)),
c the velocity magnitudes vtlen(n),
c the path lengths to intersection dint(n), and
c the times to intersection tint(n), all for n = 1, nints.
c Flag nerr indicates any input error.
c
c The x, y and z components of the particle velocity and
c coordinates are given by:
c vtx = vx + ax*t
c vty = vy + ay*t
c vtz = vz + az*t
c ptx = px + vx*t + 0.5*ax*t
c pty = py + vy*t + 0.5*ay*t
c ptz = pz + vz*t + 0.5*az*t
c
c The times of any intersections of the particle path with the
c quadric surface, tint, are obtained by substituting ptx, pty
c and ptz for x, y and z in the equation of the quadric surface,
c and solving the resulting quartic, cubic, quadratic or linear
c equation for the times. There may be 0, 1, 2, 3 or 4 such
c intersections. The path distances dint are found in aptparb.
c
c Flag nerr indicates any input error.
c
c History:
c
c Input: px, py, pz, vx, vy, vz, qc, qx, qy, qz, qxy, qyz, qzx,
c qxx, qyy, qzz, tol.
c
c Output: nints, atype, ptx, pty, ptz, vtx, vty, vtz, vtlen,
c dint, tint, nerr.
c
c Calls: aptcubs, aptparb, aptqrts, aptquar
c
c
c Glossary:
c
c atype Output The intersection types, 'simple' or 'tangent'.
c Size 4.
c
c ax,ay,az Input The x, y and z components of the constant acceleration
c vector "a".
c
c dint Output The distance along the particle path to the quadric
c surface, if nints is positive. Size = 4.
c
c nerr Output Indicates an input error, if not zero.
c 1 if equation for intersection time is null.
c 2 if equation for intersection time is impossible.
c
c nints Output The number of intersections of the particle path and
c the quadric surface. 0 if no intersections occur.
c
c ptx,y,z Output The x, y and z coordinates of the intersection points
c on the particle path and in the quadric surface,
c if nints is positive. Size = 4.
c
c pz,py,pz Input The initial x, y and z coordinates of the particle.
c
c qc-qzz Input The coefficients of the implicit equation for the
c quartic surface.
c
c tint Output The times along the particle path to the
c intersection(s) with the quadric surface, if nints is
c positive. Size = 4.
c
c tol Input Numerical tolerance limit. With 64-bit floating point
c numbers, recommend 1.e-5 to 1.e-11.
c
c vtx,y,z Output The x, y and z components of the particle velocity
c at points pt, if nints is positive. Size = 4.
c
c vx,vy,vz Input The initial x, y and z components of the particle
c velocity.
ccend.
c.... Dimensioned arguments.
dimension atype(4) ! Intersection type 'simple' or 'tangent'.
character*8 atype ! Intersection type 'simple' or 'tangent'.
dimension dint(4) ! Path distance to intersection.
dimension ptx(4) ! Coordinate x of int point.
dimension pty(4) ! Coordinate y of int point.
dimension ptz(4) ! Coordinate z of int point.
dimension tint(4) ! Time to intersection.
dimension timag(4) ! Imaginary component of time.
dimension vtlen(4) ! Magnitude of particle velocity.
dimension vtx(4) ! Component x of particle velocity.
dimension vty(4) ! Component y of particle velocity.
dimension vtz(4) ! Component z of particle velocity.
cbugc***DEBUG begins.
cbug 9901 format (/ 'aptparq finding intersection point' /
cbug & ' of particle path and quadric surface.',
cbug & ' tol= ',1pe22.14)
cbug 9902 format (' Particle at p with velocity v, acceleration a:' /
cbug & ' px,py,pz= ',1p3e22.14 /
cbug & ' vx,vy,vz= ',1p3e22.14 /
cbug & ' ax,ay,az= ',1p3e22.14 )
cbug 9903 format (' Quadric surface with implicit equation',
cbug & ' coefficients:' /
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) tol
cbug write ( 3, 9902) px, py, pz, vx, vy, vz, ax, ay, az
cbug write ( 3, 9903) qc, qx, qy, qz, qxy, qyz, qzx, qxx, qyy, qzz
cbugc***DEBUG ends.
c.... Initialize.
nints = 0
do 100 n = 1, 4
atype(n) = 'unknown'
dint(n) = -1.e99
ptx(n) = -1.e99
pty(n) = -1.e99
ptz(n) = -1.e99
tint(n) = -1.e99
vtlen(n) = -1.e99
vtx(n) = -1.e99
vty(n) = -1.e99
vtz(n) = -1.e99
100 continue
c.... Find the coefficients of the times in the implicit quadric equation.
c0 = qc +
& qx *px + qy *py + qz *pz +
& qxy*px*py + qyz*py*pz + qzx*pz*px +
& qxx*px*px + qyy*py*py + qzz*pz*pz
e0 = abs (qc) +
& abs (qx *px) + abs (qy *py) + abs (qz *pz) +
& abs (qxy*px*py) + abs (qyz*py*pz) + abs (qzx*pz*px) +
& abs (qxx*px*px) + abs (qyy*py*py) + abs (qzz*pz*pz)
if (abs (c0) .le. tol * e0) c0 = 0.0
c1 = qx*vx + qy*vy + qz*vz +
& qxy*(px*vy + py*vx) +
& qyz*(py*vz + pz*vy) +
& qzx*(pz*vx + px*vz) +
& 2.0*(qxx*px*vx + qyy*py*vy + qzz*pz*vz)
e1 = abs (qx*vx) + abs (qy*vy) + abs (qz*vz) +
& abs (qxy)*(abs (px*vy) + abs (py*vx)) +
& abs (qyz)*(abs (py*vz) + abs (pz*vy)) +
& abs (qzx)*(abs (pz*vx) + abs (px*vz)) +
& 2.0*(abs (qxx*px*vx) + abs (qyy*py*vy) + abs (qzz*pz*vz))
if (abs (c1) .le. tol * e1) c1 = 0.0
c2 = 0.5* (qx*ax + qy*ay + qz*az) +
& 0.5*qxy*(px*ay + 2.0*vx*vy + py*ax) +
& 0.5*qyz*(py*az + 2.0*vy*vz + pz*ay) +
& 0.5*qzx*(pz*ax + 2.0*vz*vx + px*az) +
& qxx*(px*ax + vx*vx) +
& qyy*(py*ay + vy*vy) +
& qzz*(pz*az + vz*vz)
e2 = 0.5* ((abs (qx*ax) + abs (qy*ay) + abs (qz*az)) +
& abs (qxy)*(abs (px*ay) + 2.0*abs (vx*vy) + abs (py*ax)) +
& abs (qyz)*(abs (py*az) + 2.0*abs (vy*vz) + abs (pz*ay)) +
& abs (qzx)*(abs (pz*ax) + 2.0*abs (vz*vx) + abs (px*az))) +
& abs (qxx)*(abs (px*ax) + abs (vx*vx)) +
& abs (qyy)*(abs (py*ay) + abs (vy*vy)) +
& abs (qzz)*(abs (pz*az) + abs (vz*vz))
if (abs (c2) .le. tol * e2) c2 = 0.0
c3 = 0.5*qxy*(vx*ay + vy*ax) +
& 0.5*qyz*(vy*az + vz*ay) +
& 0.5*qzx*(vz*ax + vx*az) +
& qxx*vx*ax + qyy*vy*ay + qzz*vz*az
e3 = 0.5*abs (qxy)*(abs (vx*ay) + abs (vy*ax)) +
& 0.5*abs (qyz)*(abs (vy*az) + abs (vz*ay)) +
& 0.5*abs (qzx)*(abs (vz*ax) + abs (vx*az)) +
& abs (qxx*vx*ax) + abs (qyy*vy*ay) + abs (qzz*vz*az)
if (abs (c3) .le. tol * e3) c3 = 0.0
c4 = 0.25*(qxy*ax*ay + qyz*ay*az + qzx*az*ax +
& qxx*ax*ax + qyy*ay*ay + qzz*az*az)
e4 = 0.25*(abs (qxy*ax*ay) + abs (qyz*ay*az) + abs (qzx*az*ax) +
& abs (qxx*ax*ax) + abs (qyy*ay*ay) + abs (qzz*az*az))
if (abs (c4) .le. tol * e4) c4 = 0.0
cbugcbugc***DEBUG begins.
cbugcbug 9904 format ('Coefficients of the equation for the',
cbugcbug & ' intersection times:' /
cbugcbug & ' set c0 =',1pe20.12 /
cbugcbug & ' set c1 =',1pe20.12 /
cbugcbug & ' set c2 =',1pe20.12 /
cbugcbug & ' set c3 =',1pe20.12 /
cbugcbug & ' set c4 =',1pe20.12 )
cbugcbug
cbugcbug write ( 3, 9904) c0, c1, c2, c3, c4
cbugcbugc***DEBUG ends.
c.... Find any intersections of the particle path and the quadric.
if (c4 .ne. 0.0) then ! Solve quartic equation.
cbugcbugc***DEBUG begins.
cbugcbug 9905 format ('aptparq DEBUG. Solving quartic.')
cbugcbug write ( 3, 9905)
cbugcbugc***DEBUG ends
c0 = c0 / c4
c1 = c1 / c4
c2 = c2 / c4
c3 = c3 / c4
call aptquar (c0, c1, c2, c3, tol,
& nints, nrooti, tint, timag, atype, itrun)
cbugcbugc***DEBUG begins.
cbugcbug 9805 format ('aptparq DEBUG. Solved quartic. nints =',i3 )
cbugcbug write ( 3, 9805) nints
cbugcbugc***DEBUG ends
elseif (c3 .ne. 0.0) then ! Solve cubic equation.
cbugcbugc***DEBUG begins.
cbugcbug 9906 format ('aptparq DEBUG. Solving cubic.')
cbugcbug write ( 3, 9906)
cbugcbugc***DEBUG ends
c0 = c0 / c3
c1 = c1 / c3
c2 = c2 / c3
call aptcubs (c0, c1, c2, tol,
& nints, tint, timag, atype, itrun)
elseif (c2 .ne. 0.0) then ! Solve quadratic equation.
cbugcbugc***DEBUG begins.
cbugcbug 9907 format ('aptparq DEBUG. Solving quadratic.')
cbugcbug write ( 3, 9907)
cbugcbugc***DEBUG ends
call aptqrts (0, c2, c1, c0, qq, tol,
& nints, tint(1), tint(2), itrun)
if (nints .eq. 1) then
atype(1) = 'tangent'
elseif (nints .eq. 2) then
atype(1) = 'simple'
atype(2) = 'simple'
endif
elseif (c1 .ne. 0.0) then ! Solve linear equation.
cbugcbugc***DEBUG begins.
cbugcbug 9908 format ('aptparq DEBUG. Solving linear.')
cbugcbug write ( 3, 9908)
cbugcbugc***DEBUG ends
nints = 1
tint(1) = -c0 / c1
elseif (c0 .ne. 0.0) then ! Impossible equation.
nerr = 2
cbugcbugc***DEBUG begins.
cbugcbug 9909 format ('aptparq DEBUG. Impossible equation.')
cbugcbug write ( 3, 9909)
cbugcbugc***DEBUG ends
else ! Null equation.
nerr = 1
cbugcbugc***DEBUG begins.
cbugcbug 9910 format ('aptparq DEBUG. Null equation.')
cbugcbug write ( 3, 9910)
cbugcbugc***DEBUG ends
endif ! Tested order of equation for time.
c.... Find path, speed, velocity and position at intersections.
if (nints .gt. 0) then
do 850 n = 1, nints
if (atype(n) .eq. 'single') atype(n) = 'simple'
if (atype(n) .eq. 'dbl max') atype(n) = 'tangent'
if (atype(n) .eq. 'dbl min') atype(n) = 'tangent'
if (atype(n) .eq. 'triple') atype(n) = 'tangent'
if (atype(n) .eq. 'quadrpl') atype(n) = 'tangent'
call aptparb (px, py, pz, vx, vy, vz, ax, ay, az,
& tint(n), tol,
& ptx(n), pty(n), ptz(n),
& vtx(n), vty(n), vtz(n),
& vtlen(n), dint(n), ntype)
850 continue
endif
cbugc***DEBUG begins.
cbug 9912 format (/ 'aptparq results: nerr=',i2,' nints=',i2 )
cbug write ( 3, 9912) nerr, nints
cbug if (nints .gt. 0) then ! One or more intersections.
cbug 9913 format (/ 'Intersection type ',a8 /
cbug & 'Time, path, speed ',1p3e20.12 /
cbug & 'Velocity (xyz) ',1p3e20.12 /
cbug & 'Position (xyz) ',1p3e20.12 )
cbug write ( 3, 9913) (atype(n), tint(n), dint(n), vtlen(n),
cbug & vtx(n), vty(n), vtz(n),
cbug & ptx(n), pty(n), ptz(n),
cbug & n = 1, nints)
cbug endif ! Tested number of intersections.
cbugc***DEBUG ends.
return
c.... End of subroutine aptparq. (+1 line.)
end
UCRL-WEB-209832