dist_node_seg4n.F File Reference

#include "implicit_f.inc"

Go to the source code of this file.

Functions/Subroutines
subroutine	dist_node_seg4n (dist, dmin, dmax, nod_x, nod_y, nod_z, ax, ay, az, bx, by, bz, cx, cy, cz, dx, dy, dz)

Function/Subroutine Documentation

dist_node_seg4n()

subroutine dist_node_seg4n	(	intent(out)	dist,
		intent(in)	dmin,
		intent(in)	dmax,
		intent(in)	nod_x,
		intent(in)	nod_y,
		intent(in)	nod_z,
		intent(in)	ax,
		intent(in)	ay,
		intent(in)	az,
		intent(in)	bx,
		intent(in)	by,
		intent(in)	bz,
		intent(in)	cx,
		intent(in)	cy,
		intent(in)	cz,
		intent(in)	dx,
		intent(in)	dy,
		intent(in)	dz )

Definition at line 28 of file dist_node_seg4n.F.

c-----------------------------------------------------------------------
c Calculates distance between NODE(NOD_X,NOD_Y,NOD_Z) and a 3 node segment (A-B-C)
c returns distance to plane if node projection is inside the segment
c otherwise : check closest distance to edges and segment points
c-----------------------------------------------------------------------
C   I m p l i c i t   T y p e s
C-----------------------------------------------
#include      "implicit_f.inc"
C-----------------------------------------------
C   D u m m y   A r g u m e n t s
C-----------------------------------------------
      my_real , INTENT(IN)  :: dmin,dmax,nod_x,nod_y,nod_z,
     .                         ax,ay,az,bx,by,bz,cx,cy,cz,dx,dy,dz
      my_real , INTENT(OUT) :: dist
C----------------------------------------------------------
C Local Variables
C----------------------------------------------------------
      INTEGER :: OK
      my_real :: abx,aby,abz,acx,acy,acz,bcx,bcy,bcz,cdx,cdy,cdz,dax,day,daz,
     .   ux,uy,uz,vx,vy,vz,wx,wy,wz,qx,qy,qz,qa_x,qa_y,qa_z,qb_x,qb_y,qb_z,
     .   qc_x,qc_y,qc_z,mx,my,mz,mpx,mpy,mpz,
     .   amx,amy,amz,bmx,bmy,bmz,cmx,cmy,cmz,dmx,dmy,dmz, 
     .   n1x,n1y,n1z,n2x,n2y,n2z,n3x,n3y,n3z,n4x,n4y,n4z,
     .   sx,sy,sz,d,d1,d2,d3,d4,norm1,a1,a2,norm2,norm3,norm4
C=======================================================================
c     segment barycenter
      mx = fourth*(ax + bx + cx + dx)
      my = fourth*(ay + by + cy + dy)
      mz = fourth*(az + bz + cz + dz)
      mpx = nod_x - mx
      mpy = nod_y - my
      mpz = nod_z - mz
c     Divide 4N segment into 4 segments 3N with common node in barycenter (possible warping)
c     A-B-M, B-C-M,C-D-M, D-M-A
c     calculate distance to 4 planes defined by each segment
      abx = bx - ax
      aby = by - ay
      abz = bz - az
      amx = mx - ax
      amy = my - ay
      amz = mz - az
      n1x = amy*abz - amz*aby
      n1y = amz*abx - amx*abz
      n1z = amx*aby - amy*abx
      norm1 = sqrt(n1x*n1x + n1y*n1y + n1z*n1z)
      d1    = abs(n1x*mpx + n1y*mpy + n1z*mpz) / norm1
c      
      bcx = cx - bx
      bcy = cy - by
      bcz = cz - bz
      bmx = mx - bx
      bmy = my - by
      bmz = mz - bz
      n2x = bmy*bcz - bmz*bcy
      n2y = bmz*bcx - bmx*bcz
      n2z = bmx*bcy - bmy*bcx
      norm2 = sqrt(n2x*n2x + n2y*n2y + n2z*n2z)
      d2    = abs(n2x*mpx + n2y*mpy + n2z*mpz) / norm2
c      
      cdx = dx - cx
      cdy = dy - cy
      cdz = dz - cz
      cmx = mx - cx
      cmy = my - cy
      cmz = mz - cz
      n3x = cmy*cdz - cmz*cdy
      n3y = cmz*cdx - cmx*cdz
      n3z = cmx*cdy - cmy*cdx
      norm3 = sqrt(n3x*n3x + n3y*n3y + n3z*n3z)
      d3    = abs(n3x*mpx + n3y*mpy + n3z*mpz) / norm3
c      
      dax = ax - dx
      day = ay - dy
      daz = az - dz
      dmx = mx - dx
      dmy = my - cy
      dmz = mz - cz
      n4x = dmy*daz - dmz*day
      n4y = dmz*dax - dmx*daz
      n4z = dmx*day - dmy*dax
      norm4 = sqrt(n4x*n4x + n4y*n4y + n4z*n4z)
      d4    = abs(n4x*mpx + n4y*mpy + n4z*mpz) / norm4
c---------------------------------------------------     
      dist = min(d1,d2)
      dist = min(dist,d3)
      dist = min(dist,d4)      
      IF (dist > dmax .or. dist < dmin) RETURN  ! distance is outside of valid range
c---------------------------------------------------
c     check if projection is inside sub-segments
c     Q = projection of NODE on segment plane using normal(NPX,NPY,NPZ)
c---------------------------------------------------
c     1st sub-segment :
      qx = nod_x - d1*n1x / norm1
      qy = nod_y - d1*n1y / norm1
      qz = nod_z - d1*n1z / norm1
c
      sx = (qy - my) * (az - mz) - (qz - mz) * (ay - my)
      sy = (qz - mz) * (ax - mx) - (qx - mx) * (az - mz)
      sz = (qx - mx) * (ay - my) - (qy - my) * (ax - mx)
      a1 = sqrt(sx*sx + sy*sy + sz*sz) * half
      sx = (by - my) * (qz - mz) - (bz - mz) * (qy - my)
      sy = (bz - mz) * (qx - mx) - (bx - mx) * (qz - mz)
      sz = (bx - mx) * (qy - my) - (by - my) * (qx - mx)
      a2 = sqrt(sx*sx + sy*sy + sz*sz) * half
      ok = 0
      IF (a1 + a2 < half * norm1) THEN
        dist = d1
        ok   = 1
      END IF
c      
      IF (ok == 0) THEN
c       Test 2nd sub-segment :
        qx = nod_x - d2*n2x / norm2
        qy = nod_y - d2*n2y / norm2
        qz = nod_z - d2*n2z / norm2
c
        sx = (qy - my) * (bz - mz) - (qz - mz) * (by - my)
        sy = (qz - mz) * (bx - mx) - (qx - mx) * (bz - mz)
        sz = (qx - mx) * (by - my) - (qy - my) * (bx - mx)
        a1 = sqrt(sx*sx + sy*sy + sz*sz) * half
        sx = (cy - my) * (qz - mz) - (cz - mz) * (qy - my)
        sy = (cz - mz) * (qx - mx) - (cx - mx) * (qz - mz)
        sz = (cx - mx) * (qy - my) - (cy - my) * (qx - mx)
        a2 = sqrt(sx*sx + sy*sy + sz*sz) * half
        IF (a1 + a2 < half * norm2) THEN
          dist = d2
          ok   = 1
        END IF
      END IF
      IF (ok == 0) THEN
c       Test 3rd sub-segment :
        qx = nod_x - d3*n3x / norm3
        qy = nod_y - d3*n3y / norm3
        qz = nod_z - d3*n3z / norm3
c
        sx = (qy - my) * (cz - mz) - (qz - mz) * (cy - my)
        sy = (qz - mz) * (cx - mx) - (qx - mx) * (cz - mz)
        sz = (qx - mx) * (cy - my) - (qy - my) * (cx - mx)
        a1 = sqrt(sx*sx + sy*sy + sz*sz) * half
        sx = (dy - my) * (qz - mz) - (dz - mz) * (qy - my)
        sy = (dz - mz) * (qx - mx) - (dx - mx) * (qz - mz)
        sz = (dx - mx) * (qy - my) - (dy - my) * (qx - mx)
        a2 = sqrt(sx*sx + sy*sy + sz*sz) * half
        IF (a1 + a2 < half * norm3) THEN
          dist = d3
          ok   = 1
        END IF
      END IF
      IF (ok == 0) THEN
c       Test 4th sub-segment :
        qx = nod_x - d3*n3x / norm4
        qy = nod_y - d3*n3y / norm4
        qz = nod_z - d3*n3z / norm4
c
        sx = (qy - my) * (dz - mz) - (qz - mz) * (dy - my)
        sy = (qz - mz) * (dx - mx) - (qx - mx) * (dz - mz)
        sz = (qx - mx) * (dy - my) - (qy - my) * (dx - mx)
        a1 = sqrt(sx*sx + sy*sy + sz*sz) * half
        sx = (ay - my) * (qz - mz) - (az - mz) * (qy - my)
        sy = (az - mz) * (qx - mx) - (ax - mx) * (qz - mz)
        sz = (ax - mx) * (qy - my) - (ay - my) * (qx - mx)
        a2 = sqrt(sx*sx + sy*sy + sz*sz) * half
        IF (a1 + a2 < half * norm4) THEN
          dist = d3
          ok   = 1
        END IF
      END IF
c---------------------------------------------------
c     check distance to all nodes including barycenter
c---------------------------------------------------
      IF (ok == 0) THEN
        dist  = sqrt(mpx**2 + mpy**2 + mpz**2)
        d1 = sqrt((nod_x - ax)**2 + (nod_y - ay)**2 + (nod_z - az)**2)
        d2 = sqrt((nod_x - bx)**2 + (nod_y - by)**2 + (nod_z - bz)**2)
        d3 = sqrt((nod_x - cx)**2 + (nod_y - cy)**2 + (nod_z - cz)**2)
        d4 = sqrt((nod_x - dx)**2 + (nod_y - dy)**2 + (nod_z - dz)**2)
        dist = min(dist, d1)
        dist = min(dist, d2)
        dist = min(dist, d3)
        dist = min(dist, d4)
      END IF
c
c     we skip checking distance to external edges (for the moment)
c
c-----------
      RETURN