Sources Esope | Cast3M

pcone
C PCONE     SOURCE    PV        20/03/24    21:20:04     10554          
C    CE SOUS-PROGRAMME RAMENNE UN CONE SUR SES COORDONNEES PROPRES
C
      SUBROUTINE PCONE(IOP,FER,XPROJ,NDEB,NUMNP,ISOM,IP2,tcval,isens)
      IMPLICIT INTEGER(I-N)
      IMPLICIT REAL*8 (A-H,O-Z)
-INC SMCOORD
 
-INC PPARAM
-INC CCOPTIO
      SEGMENT/FER/(NFI(ITT),MAI(IPP),ITOUR)
      SEGMENT XPROJ(3,IMAX)
      real*8 tcval(*)
*       tcval(1)  2     3     4   5      6         7
*      SAVE XORIG,YORIG,ZORIG,XAXE,YAXE,ZAXE,ISENS,SANGS
*         tcval(8)  9   10    11
*      SAVE XSOM,YSOM,ZSOM,CANGS
      IF (IOP.EQ.2) GOTO 100
      IMCT=MAI(ITOUR+1)
      INCT=MAI(1)+1
      IMAX=IMCT**2
      CALL LIRENT(IMAX,0,IRETOU)
      IF (IRETOU.NE.0) IMAX=MAX(1,IMAX)
      NDEB=IMCT+1
      SEGINI XPROJ
      SEGACT MCOORD
C   AXE DU CONE
      IREF=ISOM*4-3
      XSOM=XCOOR(IREF)
      YSOM=XCOOR(IREF+1)
      ZSOM=XCOOR(IREF+2)
      tcval(8)=xsom
      tcval(9)=ysom
      tcval(10)=Zsom
      IREF=IP2*4-3
      XP2=XCOOR(IREF)
      YP2=XCOOR(IREF+1)
      ZP2=XCOOR(IREF+2)
      XAXE=XP2-XSOM
      YAXE=YP2-YSOM
      ZAXE=ZP2-ZSOM
      DAXE=SQRT(XAXE**2+YAXE**2+ZAXE**2)
      IF (DAXE.EQ.0.) CALL ERREUR(21)
      IF (IERR.NE.0) RETURN
      XAXE=XAXE/DAXE
      YAXE=YAXE/DAXE
      ZAXE=ZAXE/DAXE
      tcval(4)=xaxe
      tcval(5)=yaxe
      tcval(6)=zaxe
C  DEROULONS LE CONE
      IREF=4*NFI(IMCT)-3
      XV1=XCOOR(IREF)-XSOM
      YV1=XCOOR(IREF+1)-YSOM
      ZV1=XCOOR(IREF+2)-ZSOM
      PV1=XV1*XAXE+YV1*YAXE+ZV1*ZAXE
      XV1=XV1-PV1*XAXE
      YV1=YV1-PV1*YAXE
      ZV1=ZV1-PV1*ZAXE
      DV1=SQRT(XV1**2+YV1**2+ZV1**2)
      IF (DV1.EQ.0.) CALL ERREUR(21)
      IF (IERR.NE.0) RETURN
      XV1=XV1/DV1
      YV1=YV1/DV1
      ZV1=ZV1/DV1
      XORIG=XV1
      YORIG=YV1
      ZORIG=ZV1
      tcval(1)=xorig
      tcval(2)=yorig
      tcval(3)=zorig
      ANGSOM=0
      COT=0
      DO 1 I=INCT,max(IMCT,mai(itour+2))
      II=NFI(I)
      IREF=4*II-3
      XV2=XCOOR(IREF)-XSOM
      YV2=XCOOR(IREF+1)-YSOM
      ZV2=XCOOR(IREF+2)-ZSOM
      XPROJ(1,I)=SQRT(XV2**2+YV2**2+ZV2**2)
      XPROJ(3,I)=XCOOR(IREF+3)
      XI=XPROJ(1,I)
C   XI EST LE RAYON EN COORDONNEES POLAIRES    QU'EST L'ANGLE?
      PROJ=XV2*XAXE+YV2*YAXE+ZV2*ZAXE
      if (i.le.imct) ANGSOM=ANGSOM+PROJ/XI
      XV2=XV2-PROJ*XAXE
      YV2=YV2-PROJ*YAXE
      ZV2=ZV2-PROJ*ZAXE
      DV2=SQRT(XV2**2+YV2**2+ZV2**2)
      IF (i.le.imct.and.DV2.EQ.0.) CALL ERREUR(21)
      IF (IERR.NE.0) RETURN
      XV2=XV2/DV2
      YV2=YV2/DV2
      ZV2=ZV2/DV2
      ANG=ATAN2(XAXE*(YV1*ZV2-ZV1*YV2)+YAXE*(ZV1*XV2-XV1*ZV2)+ZAXE*
     # (XV1*YV2-YV1*XV2),XV1*XV2+YV1*YV2+ZV1*ZV2)
      COT=COT+ANG
      XPROJ(2,I)=COT
      XV1=XV2
      YV1=YV2
      ZV1=ZV2
   1  CONTINUE
      CANGS=ANGSOM/(IMCT-INCT+1)
      SANGS=SQRT(1-CANGS**2)
      tcval(7)=sangs
      tcval(11)=cangs
      DO 2 I=INCT,max(IMCT,mai(itour+2))
      XPROJ(2,I)=XPROJ(2,I)*SANGS
      RR=XPROJ(1,I)
      XPROJ(1,I)=RR*COS(XPROJ(2,I))
      XPROJ(2,I)=RR*SIN(XPROJ(2,I))
      II=NFI(I)
      NFI(I)=I
      IREF=4*II-3
      XV=XCOOR(IREF)-XSOM
      YV=XCOOR(IREF+1)-YSOM
      ZV=XCOOR(IREF+2)-ZSOM
      SCAL=XV*XAXE+YV*YAXE+ZV*ZAXE
      DV=SQRT(XV**2+YV**2+ZV**2)
      CAN=SCAL/DV
      RAP=CANGS/CAN
      IF (i.le.imct.and.(RAP.GT.1.01.OR.RAP.LT.0.99)) CALL ERREUR(21)
   2  CONTINUE
C  IL FAUT TOURNER DANS LE BON SENS
      SURF=0
      DO 3 IT=1,ITOUR
      II1=MAI(IT-1+1)+1
      II2=MAI(IT+1)
      XV1=XPROJ(1,II2)
      YV1=XPROJ(2,II2)
      DO 3 I=II1,II2
      XV2=XPROJ(1,I)
      YV2=XPROJ(2,I)
      IF (XPROJ(3,I).EQ.0) XPROJ(3,I)=SQRT((XV2-XV1)**2+(YV2-YV1)**2)
      SURF=SURF+XV1*YV2-XV2*YV1
      XV1=XV2
      YV1=YV2
   3  CONTINUE
      ISENS=1
      IF (SURF.GT.0.) GOTO 5
      ISENS=-1
      DO 4 I=INCT,max(IMCT,mai(itour+2))
      XPROJ(1,I)=-XPROJ(1,I)
   4  CONTINUE
   5  CONTINUE
      RETURN
C     TRANSFORMATION INVERSE
 100  CONTINUE
      xorig=tcval(1)
      yorig=tcval(2)
      zorig=tcval(3)
      xaxe=tcval(4)
      yaxe=tcval(5)
      zaxe=tcval(6)
      sangs=tcval(7)
      xsom=tcval(8)
      ysom=tcval(9)
      zsom=tcval(10)
      cangs=tcval(11)
      SEGACT MCOORD*mod
      XREP1=XORIG
      YREP1=YORIG
      ZREP1=ZORIG
      XREP2=YAXE*ZREP1-ZAXE*YREP1
      YREP2=ZAXE*XREP1-XAXE*ZREP1
      ZREP2=XAXE*YREP1-YAXE*XREP1
      IF (NUMNP.LT.NDEB) GOTO 102
      NBPTA=nbpts 
      NBPTS=NBPTA+NUMNP-NDEB+1
      SEGADJ MCOORD
      DO 101 I=NDEB,NUMNP
      R=SQRT(XPROJ(1,I)**2+XPROJ(2,I)**2)
      IF (R.EQ.0.) CALL ERREUR(21)
      IF (IERR.NE.0.) RETURN
      ANG=ATAN2(XPROJ(2,I),XPROJ(1,I)*ISENS)
      XPROJ(1,I)=R
      XPROJ(2,I)=ANG/SANGS
      SI=SIN(XPROJ(2,I))
      CO=COS(XPROJ(2,I))
      XCOOR(NBPTA*4+1)=(XREP1*CO+XREP2*SI)*R*SANGS+R*CANGS*XAXE+XSOM
      XCOOR(NBPTA*4+2)=(YREP1*CO+YREP2*SI)*R*SANGS+R*CANGS*YAXE+YSOM
      XCOOR(NBPTA*4+3)=(ZREP1*CO+ZREP2*SI)*R*SANGS+R*CANGS*ZAXE+ZSOM
      XCOOR(NBPTA*4+4)=XPROJ(3,I)
      NBPTA=NBPTA+1
 101  CONTINUE
 102  CONTINUE
      SEGSUP XPROJ
      RETURN
      END