
**** basic geometrical routines


      SUBROUTINE SMER(THETA,FI,DIR)

! calculates unit vector from angles theta and phi

      REAL THETA, FI, DIR(3)

      DIR(1)=COS(FI)*SIN(THETA)
      DIR(2)=SIN(FI)*SIN(THETA)
      DIR(3)=COS(THETA)
      RETURN
      END



      SUBROUTINE PREMICA(RINIT,DIR,RFINL,PATH,IZB)

! to calculate the second point or the distance between two points
! of line given by:
! RINIT   initial point (input)
! DIR     unit vector of line direction (input)
! RFINL   final point (input/output)
! PATH    distance between initial and final points (input/output)
! depending on parameter
! IZB =0  PATH given
!     =1  RFINL(1) given
!     =2  RFINL(2) given
!     =3  RFINL(3) given
! calculates three other unknowns

      REAL RINIT(3), DIR(3), RFINL(3), PATH
      INTEGER IZB

      IF(IZB.NE.0) THEN
         IF(DIR(IZB).NE.0) THEN
	    PATH=(RFINL(IZB)-RINIT(IZB))/DIR(IZB)
         ELSE
	    PRINT*,'line perpendicular to axis:',IZB
	    STOP '**ERROR STOP**'
         ENDIF
      ENDIF
      DO I=1,3
         RFINL(I)=RINIT(I)+PATH*DIR(I)
      ENDDO
      RETURN
      END



      SUBROUTINE SET_PREMICA(R1,R2,DIR)

! set line thru points R1(), R2()
! output is unit vector DIR()
! points must be different

      REAL R1(3), R2(3), DIR(3)

      DIRN=0.
      DO I=1,3
         DIR(I)=R2(I)-R1(I)
         DIRN=DIRN+DIR(I)**2
      ENDDO
      DIRN=SQRT(DIRN)
      DO I=1,3
         DIR(I)=DIR(I)/DIRN
      ENDDO
      RETURN
      END



      SUBROUTINE ROTACIJA(SMER_Z,VIN,VOUT)
      
! rotation of the system, i.e.
! transformation of a vector from system S' whoose z axis is given in system S
! by unit vector SMER_Z()
! VIN()  are the coordinates of a vector in S'
! VOUT() are the coordinates of a vector in S

      REAL SMER_Z(3), VIN(3), VOUT(3)
      REAL A(3,3)

      IF(SMER_Z(3).EQ.1) THEN
         VOUT(1)=VIN(1)
         VOUT(2)=VIN(2)
         VOUT(3)=VIN(3)
         RETURN
      ENDIF
      IF(SMER_Z(3).EQ.-1) THEN
         VOUT(1)=-VIN(1)
         VOUT(2)= VIN(2)
         VOUT(3)=-VIN(3)
         RETURN
      ENDIF
      STH=SQRT(1.-SMER_Z(3)**2)
      CFI=SMER_Z(1)/STH
      SFI=SMER_Z(2)/STH
      A(1,1)=CFI*SMER_Z(3)
      A(1,2)=-SFI
      A(1,3)=SMER_Z(1)
      A(2,1)=SFI*SMER_Z(3)
      A(2,2)=CFI
      A(2,3)=SMER_Z(2)
      A(3,1)=-STH
      A(3,2)=0.
      A(3,3)=SMER_Z(3)
      DO I=1,3
         VOUT(I)=0.
         DO J=1,3
	    VOUT(I)=VOUT(I)+A(I,J)*VIN(J)
         ENDDO
      ENDDO
      RETURN
      END

! the same routine as previous but faster

      SUBROUTINE ROTATE_TO_FIXED(SMER_Z,VIN,VOUT)

! transformation of a vector from current system S' to fixed system S 
! SMER_Z() is unit vector of z-axis of S' given in fixed system S 
! VIN()  are the coordinates of a vector in S'
! VOUT() are the coordinates of a vector in S

      REAL SMER_Z(3), VIN(3), VOUT(3)

      IF(SMER_Z(3).EQ.1) THEN
         VOUT(1)=VIN(1)
         VOUT(2)=VIN(2)
         VOUT(3)=VIN(3)
      ELSE IF(SMER_Z(3).EQ.-1) THEN
         VOUT(1)=-VIN(1)
         VOUT(2)= VIN(2)
         VOUT(3)=-VIN(3)
      ELSE
         CTH=SMER_Z(3)
         STH=SQRT(1.-SMER_Z(3)**2)
         CFI=SMER_Z(1)/STH
         SFI=SMER_Z(2)/STH
       ! rotation by Theta around y axis
         X=CTH*VIN(1)+STH*VIN(3)
         Y=VIN(2)
         Z=-STH*VIN(1)+CTH*VIN(3)
       ! rotation by Fi around z axis
         VOUT(1)=CFI*X-SFI*Y
         VOUT(2)=SFI*X+CFI*Y
         VOUT(3)=Z
      ENDIF
      RETURN
      END


      SUBROUTINE ROTATE_FROM_FIXED(SMER_Z,VIN,VOUT)

! transformation of a vector from fixed system S to current system S'
! SMER_Z() is unit vector of z-axis of S' given in fixed system S 
! VIN()  are the coordinates of a vector in S
! VOUT() are the coordinates of a vector in S'

      REAL SMER_Z(3), VIN(3), VOUT(3)

      IF(SMER_Z(3).EQ.1) THEN
         VOUT(1)=VIN(1)
         VOUT(2)=VIN(2)
         VOUT(3)=VIN(3)
      ELSE IF(SMER_Z(3).EQ.-1) THEN
         VOUT(1)=-VIN(1)
         VOUT(2)= VIN(2)
         VOUT(3)=-VIN(3)
      ELSE
         CTH=SMER_Z(3)
         STH=SQRT(1.-SMER_Z(3)**2)
         CFI=SMER_Z(1)/STH
         SFI=SMER_Z(2)/STH
	! rotation by -Fi around z axis
         X=CFI*VIN(1)+SFI*VIN(2)
         Y=-SFI*VIN(1)+CFI*VIN(2)
         Z=VIN(3)
	! rotation by -Theta around y axis
         VOUT(1)=CTH*X-STH*Z
         VOUT(2)=Y
         VOUT(3)=STH*X+CTH*Z
      ENDIF
      RETURN
      END



      SUBROUTINE PRESEK_SFERA(R,DIR,RADIJ,RC,R1,R2,SEKA)

! crossection of a line R(),DIR() and sphere of radius RADIJ and center RC()
! R1() is concave R2() convex crossection if SEKA=.true.

      REAL R(3), DIR(3), RADIJ, RC(3), R1(3), R2(3)
      LOGICAL SEKA
      REAL R0(3)

      DO I=1,3
         R0(I)=R(I)-RC(I)
      ENDDO

      RDIR=0.
      RR=0.
      DO I=1,3
	 RDIR=RDIR+R0(I)*DIR(I)
	 RR=RR+R0(I)**2
      ENDDO
      D=RDIR**2+RADIJ**2-RR
      IF(D.LT.0) THEN
         SEKA=.FALSE.
         RETURN
      ELSE
         SEKA=.TRUE.
         D=SQRT(D)
         DO I=1,3
            R1(I)=R(I)+(D-RDIR)*DIR(I)
            R2(I)=R(I)-(D+RDIR)*DIR(I)
         ENDDO
      ENDIF
      RETURN
      END



      SUBROUTINE PRESEK_ELIPSOID(R,DIR,RADIJ,S,R1,R2,SEKA)

! crossection of a line R(),DIR() and ellipsoid 
! given by RADIJ and inverse scale factors S()
! R1() is concave R2() convex crossection if SEKA=.true.

      REAL R(3), DIR(3), RADIJ, S(3), R1(3), R2(3)
      LOGICAL SEKA
      REAL R0(3)

      DO I=1,3
         R0(I)=R(I)
      ENDDO

      RDIR=0.
      RR=0.
      SS=0.
      DO I=1,3
	 RDIR=RDIR+R0(I)*DIR(I)*S(I)**2
	 RR=RR+(R0(I)*S(I))**2
	 SS=SS+(DIR(I)*S(I))**2
      ENDDO
      D=RDIR**2+(RADIJ**2-RR)*SS
      IF(D.LT.0) THEN
         SEKA=.FALSE.
         RETURN
      ELSE
         SEKA=.TRUE.
         D=SQRT(D)
         DO I=1,3
            R1(I)=R(I)+(D-RDIR)/SS*DIR(I)
            R2(I)=R(I)-(D+RDIR)/SS*DIR(I)
         ENDDO
      ENDIF
      RETURN
      END



      SUBROUTINE ODBOJ_NA_SFERI(R,DIR,RADIJ,RC)

! geometrical reflection on a sphere of radius RADIJ and with center at RC()
! R() on input:  point where a ray hits the sphere
!     on output: focus, i.e. a point where reflected ray crosses the optical
!		 axis (opt. axis is paralel to impact ray)
! DIR() on input: ray's direction (unit vector) before reflection
!       on output: ray's direction (unit vector) after reflection


      REAL R(3), DIR(3), RADIJ, RC(3)
      REAL N(3)

      DO I=1,3
         N(I)=R(I)-RC(I)
      ENDDO
      CALL ENOTNI_VEKT(N)	! normala
      CALL SKAL_PROD(DIR,N,A)
      DO I=1,3
         R(I)=RADIJ/2./A*DIR(I)+RC(I)
         DIR(I)=DIR(I)-2.*A*N(I)
      ENDDO
      RETURN
      END



      SUBROUTINE PRESEK_VALJ(R,DIR,RADIJ,R1,R2,SEKA)

! cross section of a line R(),DIR() with a cylinder of radius RADIJ and
! axis along z
! R1() is concave R2() convex crossection if SEKA=.true.

      REAL R(3), DIR(3), RADIJ, R1(3), R2(3)
      LOGICAL SEKA

      A=DIR(1)*R(1)+DIR(2)*R(2)
      B=DIR(1)**2+DIR(2)**2
      IF(B.EQ.0) THEN
         SEKA=.FALSE.
         RETURN
      ENDIF
      D2=A**2-B*(R(1)**2+R(2)**2-RADIJ**2)
      IF(D2.LT.0) THEN
         SEKA=.FALSE.
         RETURN
      ELSE
         SEKA=.TRUE.
         D=SQRT(D2)
         PATH=(D-A)/B
         CALL PREMICA(R,DIR,R1,PATH,0)
         PATH=-(D+A)/B
         CALL PREMICA(R,DIR,R2,PATH,0)
         RETURN
      ENDIF
      END



      SUBROUTINE PRESEK_ELIPS_VALJ(R,DIR,RADIJ,S,R1,R2,SEKA)

! cross section of a line R(),DIR() with an eliptical cylinder with
! axis along z and: x**2+(S*y)**2=RADIJ**2
! R1() is concave R2() convex crossection if SEKA=.true.

      REAL R(3), DIR(3), RADIJ, S, R1(3), R2(3)
      LOGICAL SEKA

      A=DIR(1)*R(1)+DIR(2)*R(2)*S**2
      B=DIR(1)**2+(S*DIR(2))**2
      IF(B.EQ.0) THEN
         SEKA=.FALSE.
         RETURN
      ENDIF
      D2=A**2-B*(R(1)**2+(S*R(2))**2-RADIJ**2)
      IF(D2.LT.0) THEN
         SEKA=.FALSE.
         RETURN
      ELSE
         SEKA=.TRUE.
         D=SQRT(D2)
         PATH=(D-A)/B
         CALL PREMICA(R,DIR,R1,PATH,0)
         PATH=-(D+A)/B
         CALL PREMICA(R,DIR,R2,PATH,0)
         RETURN
      ENDIF
      END


      SUBROUTINE PRESEK_PARALGM(R,DIR,R0,R1,R2,RX,PATH,ALFA,BETA,SEKA)

      REAL R(3), DIR(3)         ! premica 
      REAL R0(3), R1(3), R2(3)  ! paralelogram v prostoru
      REAL RX(3)                ! presecisce z ravnino, ce SEKA=.true.
      REAL PATH                 ! razdalja od R do RX, v smeri DIR
      REAL ALFA, BETA           ! ce sta med 0 in 1, je presecisce znotraj
      LOGICAL SEKA              ! premica seka ravnino, ce .true.

      REAL V1(3), V2(3), VX(3), VP(3), VN(3)

      DO I=1,3
         V1(I)=R1(I)-R0(I)
         V2(I)=R2(I)-R0(I)
         VP(I)=R(I)-R0(I)
      ENDDO
      CALL VEKT_PROD(V1,V2,VN)
      CALL SKAL_PROD(VN,DIR,S)
      IF(S.EQ.0) THEN
         SEKA=.FALSE.
         RETURN
      ELSE
         SEKA=.TRUE.
      ENDIF
      CALL SKAL_PROD(VP,VN,PATH)
      PATH=-PATH/S
      DO I=1,3
         VX(I)=VP(I)+PATH*DIR(I)
         RX(I)=VX(I)+R0(I)
      ENDDO
      CALL SKAL_PROD(V1,V1,A)
      CALL SKAL_PROD(V2,V2,B)
      CALL SKAL_PROD(V1,V2,AB)

      CALL SKAL_PROD(V1,VX,X1)
      CALL SKAL_PROD(V2,VX,X2)
      
      IF(AB.EQ.0) THEN
         ALFA=X1/A
         BETA=X2/B
      ELSE
         D=A*B-AB**2
         ALFA=(X1*B-X2*AB)/D
         BETA=(X2*A-X1*AB)/D
      ENDIF

      RETURN
      END



**** 3D vector routines

      SUBROUTINE VEKT_PROD(A,B,C)

! vector product: C = A x B

      REAL A(3), B(3), C(3)

      C(1)=A(2)*B(3)-A(3)*B(2)
      C(2)=A(3)*B(1)-A(1)*B(3)
      C(3)=A(1)*B(2)-A(2)*B(1)

      RETURN
      END		



      SUBROUTINE SKAL_PROD(A,B,S)

! scalar product: s=A.B

      REAL A(3), B(3), S

      S=A(1)*B(1)+A(2)*B(2)+A(3)*B(3)
      
      RETURN
      END


      SUBROUTINE NORMA_VEKT(A,D)

! norm of a vector A

      REAL A(3), D
      
      D=SQRT(A(1)**2+A(2)**2+A(3)**2)
      
      RETURN
      END


      SUBROUTINE ENOTNI_VEKT(A)

! unit vector from vector A

      REAL A(3)

      CALL NORMA_VEKT(A,D)
      IF(D.EQ.0) RETURN
      DO I=1,3
         A(I)=A(I)/D
      ENDDO

      RETURN
      END


      SUBROUTINE V_SFERNE_K(R,RR,TH,FI)

! to spherical koordinates

      REAL R(3)                 ! vector (x,y,z)
      REAL RR, TH, FI
      
      FI=ATAN2(R(2),R(1))
      RR=SQRT(R(1)**2+R(2)**2+R(3)**2)
      TH=ACOS(R(3)/RR)
      
      RETURN
      END


      REAL FUNCTION VDIFF2(P,Q)

      REAL P(3), Q(3)

      VDIFF2=(P(1)-Q(1))**2+(P(2)-Q(2))**2+(P(3)-Q(3))**2
      RETURN
      END


