Source code for the minimum distance between two rods

The following is the FORTRAN source code for the calculation of the minimum distance between two rods in three dimensions. For full details see . This code has also been implemented in C.

C  SUBROUTINE TO EVALUATE THE SHORTEST DISTANCE BETWEEN TWO RODS
C  OF DIFFERENT LENGTH
C  R12= VECTOR CONNECTING THE GEOMETRICAL CENTERS OF THE TWO RODS
C  U1 = UNITARY VECTOR DEFINIG THE ORIENTATION OF ROD1
C  U2 = UNITARY VECTOR DEFINIG THE ORIENTATION OF ROD2
C  XL1D2= HALF OF THE LENGTH OF ROD1
C  XL2D2= HALF OF THE LENGTH OF ROD2
C  RO2 = SQUARE OF THE SHORTEST DISTANCE BETWEEN THE TWO RODS
      SUBROUTINE SDM(R12,U1,U2,XL1D2,XL2D2,RO2)
      DIMENSION R12(3),U1(3),U2(3)
C *****************
C STEP 1
C  SETTING TO ITS VALUE SOME VARIABLES
      R122=R12(1)**2+R12(2)**2+R12(3)**2
      R12EU1=R12(1)*U1(1)+R12(2)*U1(2)+R12(3)*U1(3)
      R12EU2=R12(1)*U2(1)+R12(2)*U2(2)+R12(3)*U2(3)
      U1EU2=U1(1)*U2(1)+U1(2)*U2(2)+U1(3)*U2(3)
      CC=1.-U1EU2**2
C CHECKING WHETHER THE RODS ARE OR NOT PARALLEL
      IF (CC.LT.1.E-6) THEN
        IF (R12EU1.NE.0.) THEN
            XLANDA=SIGN(XL1D2,R12EU1)
            GO TO 10
        ELSE
            XLANDA=0.
            XMU=0.
            GO TO 20
        ENDIF
      ENDIF
C   EVALUATION OF XLANDA PRIMA AND XMU PRIMA
      XLANDA=(R12EU1-U1EU2*R12EU2)/CC
      XMU=(-R12EU2+U1EU2*R12EU1)/CC
C END OF STEP 1   OF THE ALGORITHM
C ******************************
C STEP 2
C   CHECKING WHETHER (XLANDA PRIMA,XMUPRIMA) BELONGS  TO THE
C   RECTANGLE DEFINED BY THE TWO SEGMENTS IN THE XLANDA,XMU SPACE
      IF ((ABS(XLANDA).LE.XL1D2).AND.(ABS(XMU).LE.XL2D2)) GO TO 20
C END OF STEP 2 OF THE ALGORITHM
C *********************************
      AUXI1=ABS(XLANDA)-XL1D2
      AUXI2=ABS(XMU)-XL2D2
C **********************
C STEP 3
C   ASIGNEMENT OF THE SIDE OF THE SQUARE WHERE THE SHORTEST DISTANCE
C   BETWEEN THE RODS STANDS
        IF ( AUXI1.GT.AUXI2) THEN
           XLANDA=SIGN(XL1D2,XLANDA)
C END OF STEP 3
C **********************
C STEP 4
C  LOOKING FOR THE LOCAL MINIMA OF THE LINE WHICH CONTAINS
C  THE SELECTED SIDE OF THE SQUARE
10         XMU=XLANDA*U1EU2-R12EU2
C  IF THE LOCAL MINIMUM DOES NOT BELONG TO THE SIDE OF THE SQUARE
C  THEN GIVE US THE NEAREST CORNER OF THE SQUARE ON THE STUDIED LINE
           IF ( ABS(XMU).GT.XL2D2) XMU=SIGN(XL2D2,XMU)
C END OF STEP 4
C ************************
C STEP 3
C   ASIGNEMENT OF THE SIDE OF THE SQUARE WHERE THE SHORTEST DISTANCE
C   BETWEEN THE RODS STANDS
        ELSE
           XMU=SIGN(XL2D2,XMU)
C END OF STEP 3
C **********************
C STEP 4
C  LOOKING FOR THE LOCAL MINIMA OF THE LINE WHICH CONTAINS
C  THE SELECTED SIDE OF THE SQUARE
           XLANDA=XMU*U1EU2+R12EU1
C  IF THE LOCAL MINIMUM DOES NOT BELONG TO THE SIDE OF THE SQUARE
C  THEN GIVE US THE NEAREST CORNER OF THE SQUARE ON THE STUDIED LINE
           IF ( ABS(XLANDA).GT.XL1D2) XLANDA=SIGN(XL1D2,XLANDA)
        ENDIF
C END OF STEP 4
C ***********************
C STEP 5
C ONCE WE KNOW THE COORDINATES (XLANDA,XMU) IN THE XLANDA,XMU
C SPACE WHERE STANDS THE SHORTEST DISTANCE BETWEEN THE SEGMENTS
C EVALUATION OF THE SHORTEST DISTANCE
20    RO2=R122+XLANDA**2+XMU**2-2.*XLANDA*XMU*U1EU2
     1    +2.*XMU*R12EU2-2.*XLANDA*R12EU1
C END OF STEP 5
C **********************
      RETURN
      END