MODULE spherical_geom_mod
USE genmod



CONTAINS




SUBROUTINE lonlat2xyz(lon,lat,xyz)
IMPLICIT NONE
  REAL(rstd),INTENT(IN) :: lon
  REAL(rstd),INTENT(IN) :: lat
  REAL(rstd),INTENT(OUT) :: xyz(3)
  
  xyz(1)=cos(lon)*cos(lat)
  xyz(2)=sin(lon)*cos(lat)
  xyz(3)=sin(lat)

END SUBROUTINE lonlat2xyz


SUBROUTINE xyz2lonlat(xyz,lon,lat)
IMPLICIT NONE
  REAL(rstd),INTENT(IN) :: xyz(3)
  REAL(rstd),INTENT(OUT) :: lon
  REAL(rstd),INTENT(OUT) :: lat
  
  REAL(rstd) :: coslat
  REAL(rstd) :: xyzn(3)
  
  xyzn(:)=xyz(:)/sqrt(sum(xyz(:)**2))
  
  lat=asin(xyzn(3))
  coslat=cos(lat)
  IF (abs(coslat)<1e-15) THEN
    lon=0.
  ELSE
    lon=acos(MAX(MIN((xyzn(1)/coslat),1.),-1.))
    IF (xyzn(2)/coslat<0) lon=-lon
  ENDIF
    
END SUBROUTINE xyz2lonlat


SUBROUTINE dist_cart(A,B,d)
USE vector
IMPLICIT NONE
  REAL(rstd),INTENT(IN)  :: A(3)
  REAL(rstd),INTENT(IN)  :: B(3)
  REAL(rstd),INTENT(OUT) :: d
  
   REAL(rstd)  :: n(3)
   CALL cross_product2(A,B,n)
   d=asin(sqrt(sum(n**2)))

END SUBROUTINE dist_cart


SUBROUTINE dist_lonlat(lonA,latA,lonB,latB,d)
IMPLICIT NONE
  REAL(rstd),INTENT(IN)  :: lonA
  REAL(rstd),INTENT(IN)  :: latA
  REAL(rstd),INTENT(IN)  :: lonB
  REAL(rstd),INTENT(IN)  :: latB
  REAL(rstd),INTENT(OUT) :: d
  
  d=acos(MAX(MIN(sin(latA)*sin(latB)+cos(latA)*cos(latB)*cos(lonA-lonB),1.),-1.))
 
END SUBROUTINE dist_lonlat

SUBROUTINE surf_triangle(A,B,C,surf)
  REAL(rstd),INTENT(IN)  :: A(3)
  REAL(rstd),INTENT(IN)  :: B(3)
  REAL(rstd),INTENT(IN)  :: C(3)
  REAL(rstd),INTENT(OUT) :: Surf

  REAL(rstd)  :: AB,AC,BC
  REAL(rstd)  :: s
  
  CALL dist_cart(A,B,AB)
  CALL dist_cart(A,C,AC)
  CALL dist_cart(B,C,BC)
  
  s=(AB+AC+BC)/2
  surf=4*atan(sqrt( tan(s/2) * tan((s-AB)/2)  * tan((s-AC)/2) * tan((s-BC)/2)))
  
END SUBROUTINE surf_triangle 


SUBROUTINE div_arc(A,B,frac,C)
IMPLICIT NONE
  REAL(rstd),INTENT(IN)  :: A(3)
  REAL(rstd),INTENT(IN)  :: B(3)
  REAL(rstd),INTENT(IN)  :: frac
  REAL(rstd),INTENT(OUT)  :: C(3)
  
  REAL(rstd) :: d
  REAL(rstd) :: M(3,3)
  REAL(rstd) :: alpha(3,3)
  INTEGER    :: IPIV(3)
  INTEGER    :: info
  REAL(rstd) :: xa,xb,xc
  REAL(rstd) :: ya,yb,yc
  REAL(rstd) :: za,zb,zc
  REAL(rstd) :: alpha_A,alpha_B,alpha_C
  REAL(rstd) :: x,y,z
  REAL(rstd) :: a1,a2,a3
  REAL(rstd) :: b1,b2,b3
  
  
  xa=A(1) ; ya=A(2) ; za=A(3)
  xb=B(1) ; yb=B(2) ; zb=B(3)

  CALL dist_cart(A,B,d)

  C(1)=cos(frac*d) 
  C(2)=cos((1-frac)*d)
  C(3)=0.

  xc=ya*zb-yb*za ; yc=-(xa*zb-xb*za) ; zc=xa*yb-xb*ya
  
  M(1,1)=xa ; M(1,2)=ya ; M(1,3)=za
  M(2,1)=xb ; M(2,2)=yb ; M(2,3)=zb
  M(3,1)=xc ; M(3,2)=yc ; M(3,3)=zc
  
  CALL DGESV(3,1,M,3,IPIV,C,3,info)
  
END SUBROUTINE div_arc

SUBROUTINE div_arc_bis(A,B,frac,C)
IMPLICIT NONE
  REAL(rstd),INTENT(IN)  :: A(3)
  REAL(rstd),INTENT(IN)  :: B(3)
  REAL(rstd),INTENT(IN)  :: frac
  REAL(rstd),INTENT(OUT)  :: C(3)
  
   C=A*(1-frac)+B*frac  
   C=C/sqrt(sum(C**2))
END SUBROUTINE div_arc_bis


  SUBROUTINE circonscrit(A0,B0,C0,Center)
  USE vector
  IMPLICIT NONE
    REAL(rstd), INTENT(IN)  :: A0(3),B0(3),C0(3)
    REAL(rstd), INTENT(OUT) :: Center(3)
    
    REAL(rstd)  :: a(3),b(3),c(3)
    
    a=A0/sqrt(sum(A0**2))
    b=B0/sqrt(sum(B0**2))
    c=C0/sqrt(sum(C0**2))
    
    CALL Cross_product2(b-a,c-b,center)
    center=center/sqrt(sum(center**2))
    
  END SUBROUTINE circonscrit
    

  SUBROUTINE centroide(A,B,C,Center)
  IMPLICIT NONE
    REAL(rstd), INTENT(IN)  :: A(3),B(3),C(3)
    REAL(rstd), INTENT(OUT) :: Center(3)
  
    REAL(rstd)  :: AB05(3),AC05(3),BC05(3)
    REAL(rstd)  :: Vab(3),Vbc(3),Vca(3)
    REAL(rstd)  :: t
    INTEGER :: n1,n2
       
    AB05=(A+B)/2.
    AC05=(A+C)/2.
    BC05=(B+C)/2.
    
    CALL director(A,B,C,Vab)
!    PRINT *,'director',sum((B-A)*Vab)
    CALL director(B,C,A,Vbc)
!    PRINT *,'director',sum((C-B)*Vbc)
    CALL director(C,A,B,Vca)
!    PRINT *,'director',sum((A-C)*Vca)
   
    n1=1 ; n2=2
    
    t=(Vbc(n1)*(BC05(n2)-AB05(n2))+(AB05(n1)-BC05(n1))*Vbc(n2))/(Vbc(n1)*Vab(n2)-Vab(n1)*Vbc(n2))
    Center=AB05+t*Vab
    Center=Center/sqrt(sum(Center**2))
!    PRINT*,"Center :",A
!    PRINT*,"Center :",B
!    PRINT*,"Center :",C
!    PRINT*,"Center :",Center
   
  CONTAINS 
  
    SUBROUTINE director(A,B,C,V)
    IMPLICIT NONE
    REAL(rstd), INTENT(IN)  :: A(3),B(3),C(3)
    REAL(rstd), INTENT(OUT) :: V(3)
    REAL(rstd) :: AB(3),AC(3)
    REAL(rstd) :: t
    
      AB=B-A
      AC=C-A
    
      t=sum(AB*AC)/sum(AB**2)
      V=AB*t-AC
    END SUBROUTINE director
       
  END SUBROUTINE centroide

END MODULE spherical_geom_mod