Télécharger calmat.eso

Retour à la liste

Numérotation des lignes :

  1. C CALMAT SOURCE CHAT 05/01/12 21:47:08 5004
  2. SUBROUTINE CALMAT(KTRAV,IPO1,IPO2,XANG)
  3. IMPLICIT INTEGER(I-N)
  4. IMPLICIT REAL*8(A-H,O-Z)
  5.  
  6. *--------------------------------------------------------------------*
  7. * *
  8. * Calcule les vecteurs et matrices de rotation, de passage, ... *
  9. * *
  10. * Param}tres: *
  11. * *
  12. * e KTRAV segment de travail *
  13. * e IPO1 premier point de l'axe de rotation *
  14. * e IPO2 deuxi}me point de l'axe de rotation, si 3D *
  15. * e XANG angle de rotation *
  16. * *
  17. * Auteur, date de cr{ation: *
  18. * *
  19. * Lionel VIVAN, le 22 mai 1990. *
  20. * *
  21. *--------------------------------------------------------------------*
  22. * *
  23. -INC CCOPTIO
  24. -INC CCREEL
  25. *
  26. DIMENSION XT(3,3),XTM1(3,3),XRT(3,3),RTT(3,3),XIMPT(3,3)
  27. PARAMETER (EPSIL = 1.E-9 , TOLER = 1.E-3 , XUN = 1.D0)
  28. *
  29. SEGMENT MTRAV
  30. REAL*8 XPT(IDIMB),XPTP(IDIMB),XP1PT(IDIMB),XMPT(IDIMB,IDIMB)
  31. ENDSEGMENT
  32. *
  33. IDIMB = IDIM
  34. SEGINI MTRAV
  35. KTRAV = MTRAV
  36. *
  37. XRAD = XANG * XPI / 180.D0
  38. XCOS = COS(XRAD)
  39. XSIN = SIN(XRAD)
  40. IF (IDIM.EQ.2) THEN
  41. XMPT(1,1) = XCOS
  42. XMPT(1,2) = - XSIN
  43. XMPT(2,1) = XSIN
  44. XMPT(2,2) = XCOS
  45. *
  46. CALL EXCOO1(IPO1,X1,Y1,Z1,D1)
  47. IF (IERR.NE.0) RETURN
  48. XP1PT(1) = ((XUN - XCOS) * X1) + (XSIN * Y1)
  49. XP1PT(2) = ((XUN - XCOS) * Y1) - (XSIN * X1)
  50. *
  51. ELSE
  52. CALL EXCOO1(IPO1,X1,Y1,Z1,D1)
  53. CALL EXCOO1(IPO2,X2,Y2,Z2,D2)
  54. IF (IERR.NE.0) RETURN
  55. XP1P2 = X2 - X1
  56. YP1P2 = Y2 - Y1
  57. ZP1P2 = Z2 - Z1
  58. *
  59. * Vecteur R
  60. *
  61. PS = (XP1P2 * XP1P2) + (YP1P2 * YP1P2) + (ZP1P2 * ZP1P2)
  62. IF (PS.LT.EPSIL) THEN
  63. CALL ERREUR(162)
  64. RETURN
  65. ENDIF
  66. PS = SQRT(PS)
  67. XR = XP1P2 / PS
  68. YR = YP1P2 / PS
  69. ZR = ZP1P2 / PS
  70. *
  71. * Vecteur A
  72. *
  73. IF (ABS(ZR).GT.TOLER) THEN
  74. XA1 = X1
  75. YA1 = XR + YR + ZR
  76. ZA1 = Z1 - (YR * (YA1 - Y1) / ZR)
  77. ELSE IF (ABS(YR).GT.TOLER) THEN
  78. ZA1 = Z1
  79. XA1 = XR + YR + ZR
  80. YA1 = Y1 - (XR * (XA1 - X1) / YR)
  81. ELSE
  82. YA1 = Y1
  83. ZA1 = XR + YR + ZR
  84. XA1 = X1 - (ZR * (ZA1 - Z1) / XR)
  85. ENDIF
  86. XP1A = XA1 - X1
  87. YP1A = YA1 - Y1
  88. ZP1A = ZA1 - Z1
  89. PS = (XP1A * XP1A) + (YP1A * YP1A) + (ZP1A * ZP1A)
  90. IF (PS.LT.EPSIL) THEN
  91. CALL ERREUR(162)
  92. RETURN
  93. ENDIF
  94. PS = SQRT(PS)
  95. XA = XP1A / PS
  96. YA = YP1A / PS
  97. ZA = ZP1A / PS
  98. *
  99. * Vecteur B
  100. *
  101. XB = (YR * ZA) - (ZR * YA)
  102. YB = (ZR * XA) - (XR * ZA)
  103. ZB = (XR * YA) - (YR * XA)
  104. *
  105. * Matrice de passage T
  106. *
  107. XT(1,1) = XA
  108. XT(1,2) = YA
  109. XT(1,3) = ZA
  110. XT(2,1) = XB
  111. XT(2,2) = YB
  112. XT(2,3) = ZB
  113. XT(3,1) = XR
  114. XT(3,2) = YR
  115. XT(3,3) = ZR
  116. *
  117. XTM1(1,1) = XA
  118. XTM1(2,1) = YA
  119. XTM1(3,1) = ZA
  120. XTM1(1,2) = XB
  121. XTM1(2,2) = YB
  122. XTM1(3,2) = ZB
  123. XTM1(1,3) = XR
  124. XTM1(2,3) = YR
  125. XTM1(3,3) = ZR
  126. * T
  127. * Matrice de rotation R
  128. *
  129. XRT(1,1) = XCOS
  130. XRT(1,2) = - XSIN
  131. XRT(1,3) = XZERO
  132. XRT(2,1) = XSIN
  133. XRT(2,2) = XCOS
  134. XRT(2,3) = XZERO
  135. XRT(3,1) = XZERO
  136. XRT(3,2) = XZERO
  137. XRT(3,3) = XUN
  138. *
  139. * Matrice P
  140. *
  141. DO 10 I = 1,IDIM
  142. DO 20 J = 1,IDIM
  143. XVAL = XZERO
  144. DO 30 K = 1,IDIM
  145. XVAL = XVAL + XRT(I,K) * XT(K,J)
  146. 30 CONTINUE
  147. * end do
  148. RTT(I,J) = XVAL
  149. 20 CONTINUE
  150. * end do
  151. 10 CONTINUE
  152. * end do
  153. DO 12 I = 1,IDIM
  154. DO 22 J = 1,IDIM
  155. XVAL = XZERO
  156. DO 32 K = 1,IDIM
  157. XVAL = XVAL + XTM1(I,K) * RTT(K,J)
  158. 32 CONTINUE
  159. * end do
  160. XMPT(I,J) = XVAL
  161. 22 CONTINUE
  162. * end do
  163. 12 CONTINUE
  164. * end do
  165. *
  166. XIMPT(1,1) = XUN - XMPT(1,1)
  167. XIMPT(1,2) = - XMPT(1,2)
  168. XIMPT(1,3) = - XMPT(1,3)
  169. XIMPT(2,1) = - XMPT(2,1)
  170. XIMPT(2,2) = XUN - XMPT(2,2)
  171. XIMPT(2,3) = - XMPT(2,3)
  172. XIMPT(3,1) = - XMPT(3,1)
  173. XIMPT(3,2) = - XMPT(3,2)
  174. XIMPT(3,3) = XUN - XMPT(3,3)
  175. *
  176. DO 14 I = 1,IDIM
  177. XP1PT(I) = (XIMPT(I,1) * X1) + (XIMPT(I,2) * Y1) +
  178. & (XIMPT(I,3) * Z1)
  179. 14 CONTINUE
  180. * end do
  181. *
  182. IF (IIMPI.EQ.333) THEN
  183. WRITE(IOIMP,*)'CALMAT : impression du vecteur R'
  184. WRITE(IOIMP,*)'CALMAT : XR =',XR
  185. WRITE(IOIMP,*)'CALMAT : YR =',YR
  186. WRITE(IOIMP,*)'CALMAT : ZR =',ZR
  187. WRITE(IOIMP,*)'CALMAT : impression du vecteur A'
  188. WRITE(IOIMP,*)'CALMAT : XA =',XA
  189. WRITE(IOIMP,*)'CALMAT : YA =',YA
  190. WRITE(IOIMP,*)'CALMAT : ZA =',ZA
  191. WRITE(IOIMP,*)'CALMAT : impression du vecteur B'
  192. WRITE(IOIMP,*)'CALMAT : XB =',XB
  193. WRITE(IOIMP,*)'CALMAT : YB =',YB
  194. WRITE(IOIMP,*)'CALMAT : ZB =',ZB
  195. WRITE(IOIMP,*)' T'
  196. WRITE(IOIMP,*)'CALMAT : impression de la matrice (P)'
  197. DO 40 I = 1,IDIM
  198. DO 42 J = 1,IDIM
  199. WRITE(IOIMP,*)'CALMAT : PT(',I,',',J,') =',XMPT(I,J)
  200. 42 CONTINUE
  201. * end do
  202. 40 CONTINUE
  203. * end do
  204. ENDIF
  205. ENDIF
  206. *
  207. END
  208.  
  209.  
  210.  
  211.  

© Cast3M 2003 - Tous droits réservés.
Mentions légales