check_if_visible Subroutine

  SUBROUTINE check_if_visible(X,V,threshold_angle,bool,angle,XC)
    IMPLICIT NONE
    REAL(rp), DIMENSION(3), INTENT(IN) :: X
    REAL(rp), DIMENSION(3), INTENT(IN) :: V
    REAL(rp), INTENT(IN) :: threshold_angle
    LOGICAL, INTENT(OUT) :: bool
    REAL(rp), INTENT(OUT) :: angle
    REAL(rp), DIMENSION(3), OPTIONAL, INTENT(OUT) :: XC
    REAL(rp), DIMENSION(3) :: vec
    REAL(rp) :: a, b, c, ciw, dis, disiw
    REAL(rp) :: sp, sn, s, psi


    a = V(1)**2 + V(2)**2
    b = 2.0_rp*(X(1)*V(1) + X(2)*V(2))
    c = X(1)**2 + X(2)**2 - cam%position(1)**2
    ciw = X(1)**2 + X(2)**2 - cam%Riw**2

    dis = b**2 - 4.0_rp*a*c
    disiw = b**2 - 4.0_rp*a*ciw

    if ((dis .LT. 0.0_rp).OR.(disiw .GE. 0.0_rp)) then
       bool = .FALSE. ! The particle is not visible
    else
       sp = 0.5_rp*(-b + SQRT(dis))/a
       sn = 0.5_rp*(-b - SQRT(dis))/a
       s = MAX(sp,sn)

       ! Rotation angle along z-axis so that v is directed to the camera
       if (PRESENT(XC)) then
          XC(1) = X(1) + s*V(1)
          XC(2) = X(2) + s*V(2)
          XC(3) = X(3) + s*V(3)
          angle = ATAN2(XC(2),XC(1))
       else
          angle = ATAN2(X(2) + s*V(2),X(1) + s*V(1))
       end if
       if (angle.LT.0.0_rp) angle = angle + 2.0_rp*C_PI

       vec(1) = cam%position(1)*COS(angle) - X(1)
       vec(2) = cam%position(1)*SIN(angle) - X(2)
       vec(3) = cam%position(2) - X(3)

       vec = vec/SQRT(DOT_PRODUCT(vec,vec))

       psi = ACOS(DOT_PRODUCT(vec,V))

       if (psi.LE.threshold_angle) then
          bool = .TRUE. ! The particle is visible
       else
          bool = .FALSE. ! The particle is not visible
       end if
    end if

  END SUBROUTINE check_if_visible