C CALMAT SOURCE CHAT 05/01/12 21:47:08 5004 SUBROUTINE CALMAT(KTRAV,IPO1,IPO2,XANG) IMPLICIT INTEGER(I-N) IMPLICIT REAL*8(A-H,O-Z) *--------------------------------------------------------------------* * * * Calcule les vecteurs et matrices de rotation, de passage, ... * * * * Param}tres: * * * * e KTRAV segment de travail * * e IPO1 premier point de l'axe de rotation * * e IPO2 deuxi}me point de l'axe de rotation, si 3D * * e XANG angle de rotation * * * * Auteur, date de cr{ation: * * * * Lionel VIVAN, le 22 mai 1990. * * * *--------------------------------------------------------------------* * * -INC PPARAM -INC CCOPTIO -INC CCREEL * DIMENSION XT(3,3),XTM1(3,3),XRT(3,3),RTT(3,3),XIMPT(3,3) PARAMETER (EPSIL = 1.E-9 , TOLER = 1.E-3 , XUN = 1.D0) * SEGMENT MTRAV REAL*8 XPT(IDIMB),XPTP(IDIMB),XP1PT(IDIMB),XMPT(IDIMB,IDIMB) ENDSEGMENT * IDIMB = IDIM SEGINI MTRAV KTRAV = MTRAV * XRAD = XANG * XPI / 180.D0 XCOS = COS(XRAD) XSIN = SIN(XRAD) IF (IDIM.EQ.2) THEN XMPT(1,1) = XCOS XMPT(1,2) = - XSIN XMPT(2,1) = XSIN XMPT(2,2) = XCOS * CALL EXCOO1(IPO1,X1,Y1,Z1,D1) IF (IERR.NE.0) RETURN XP1PT(1) = ((XUN - XCOS) * X1) + (XSIN * Y1) XP1PT(2) = ((XUN - XCOS) * Y1) - (XSIN * X1) * ELSE CALL EXCOO1(IPO1,X1,Y1,Z1,D1) CALL EXCOO1(IPO2,X2,Y2,Z2,D2) IF (IERR.NE.0) RETURN XP1P2 = X2 - X1 YP1P2 = Y2 - Y1 ZP1P2 = Z2 - Z1 * * Vecteur R * PS = (XP1P2 * XP1P2) + (YP1P2 * YP1P2) + (ZP1P2 * ZP1P2) IF (PS.LT.EPSIL) THEN CALL ERREUR(162) RETURN ENDIF PS = SQRT(PS) XR = XP1P2 / PS YR = YP1P2 / PS ZR = ZP1P2 / PS * * Vecteur A * IF (ABS(ZR).GT.TOLER) THEN XA1 = X1 YA1 = XR + YR + ZR ZA1 = Z1 - (YR * (YA1 - Y1) / ZR) ELSE IF (ABS(YR).GT.TOLER) THEN ZA1 = Z1 XA1 = XR + YR + ZR YA1 = Y1 - (XR * (XA1 - X1) / YR) ELSE YA1 = Y1 ZA1 = XR + YR + ZR XA1 = X1 - (ZR * (ZA1 - Z1) / XR) ENDIF XP1A = XA1 - X1 YP1A = YA1 - Y1 ZP1A = ZA1 - Z1 PS = (XP1A * XP1A) + (YP1A * YP1A) + (ZP1A * ZP1A) IF (PS.LT.EPSIL) THEN CALL ERREUR(162) RETURN ENDIF PS = SQRT(PS) XA = XP1A / PS YA = YP1A / PS ZA = ZP1A / PS * * Vecteur B * XB = (YR * ZA) - (ZR * YA) YB = (ZR * XA) - (XR * ZA) ZB = (XR * YA) - (YR * XA) * * Matrice de passage T * XT(1,1) = XA XT(1,2) = YA XT(1,3) = ZA XT(2,1) = XB XT(2,2) = YB XT(2,3) = ZB XT(3,1) = XR XT(3,2) = YR XT(3,3) = ZR * XTM1(1,1) = XA XTM1(2,1) = YA XTM1(3,1) = ZA XTM1(1,2) = XB XTM1(2,2) = YB XTM1(3,2) = ZB XTM1(1,3) = XR XTM1(2,3) = YR XTM1(3,3) = ZR * T * Matrice de rotation R * XRT(1,1) = XCOS XRT(1,2) = - XSIN XRT(1,3) = XZERO XRT(2,1) = XSIN XRT(2,2) = XCOS XRT(2,3) = XZERO XRT(3,1) = XZERO XRT(3,2) = XZERO XRT(3,3) = XUN * * Matrice P * DO 10 I = 1,IDIM DO 20 J = 1,IDIM XVAL = XZERO DO 30 K = 1,IDIM XVAL = XVAL + XRT(I,K) * XT(K,J) 30 CONTINUE * end do RTT(I,J) = XVAL 20 CONTINUE * end do 10 CONTINUE * end do DO 12 I = 1,IDIM DO 22 J = 1,IDIM XVAL = XZERO DO 32 K = 1,IDIM XVAL = XVAL + XTM1(I,K) * RTT(K,J) 32 CONTINUE * end do XMPT(I,J) = XVAL 22 CONTINUE * end do 12 CONTINUE * end do * XIMPT(1,1) = XUN - XMPT(1,1) XIMPT(1,2) = - XMPT(1,2) XIMPT(1,3) = - XMPT(1,3) XIMPT(2,1) = - XMPT(2,1) XIMPT(2,2) = XUN - XMPT(2,2) XIMPT(2,3) = - XMPT(2,3) XIMPT(3,1) = - XMPT(3,1) XIMPT(3,2) = - XMPT(3,2) XIMPT(3,3) = XUN - XMPT(3,3) * DO 14 I = 1,IDIM XP1PT(I) = (XIMPT(I,1) * X1) + (XIMPT(I,2) * Y1) + & (XIMPT(I,3) * Z1) 14 CONTINUE * end do * IF (IIMPI.EQ.333) THEN WRITE(IOIMP,*)'CALMAT : impression du vecteur R' WRITE(IOIMP,*)'CALMAT : XR =',XR WRITE(IOIMP,*)'CALMAT : YR =',YR WRITE(IOIMP,*)'CALMAT : ZR =',ZR WRITE(IOIMP,*)'CALMAT : impression du vecteur A' WRITE(IOIMP,*)'CALMAT : XA =',XA WRITE(IOIMP,*)'CALMAT : YA =',YA WRITE(IOIMP,*)'CALMAT : ZA =',ZA WRITE(IOIMP,*)'CALMAT : impression du vecteur B' WRITE(IOIMP,*)'CALMAT : XB =',XB WRITE(IOIMP,*)'CALMAT : YB =',YB WRITE(IOIMP,*)'CALMAT : ZB =',ZB WRITE(IOIMP,*)' T' WRITE(IOIMP,*)'CALMAT : impression de la matrice (P)' DO 40 I = 1,IDIM DO 42 J = 1,IDIM WRITE(IOIMP,*)'CALMAT : PT(',I,',',J,') =',XMPT(I,J) 42 CONTINUE * end do 40 CONTINUE * end do ENDIF ENDIF * END