Télécharger joifrm.eso

Retour à la liste

Numérotation des lignes :

  1. C JOIFRM SOURCE FD218221 14/10/24 21:15:05 8188
  2. SUBROUTINE JOIFRM(IWRK,KERRE,R,IDIM)
  3. C-----------------------------------------------------------------------
  4. C ROUTINE DE REMISE A JOUR DES VECTEURS DEFINISSANT LE REPERE LOCAL
  5. C POUR UN ELEMENT JOI1
  6. C-----------------------------------------------------------------------
  7. C ENTREE
  8. C IWRK POINTEUR SUR SEGMENT DE TRAVAIL
  9. C IL CONTIENT NOTAMMENT LES DEPLACEMENTS ET ROTATIONS
  10. C DES NOEUDS DU JOINT
  11. C R LES 2 VECTEURS DEFINISSANT LE REPERE DU JOINT
  12. C IDIM DIMENSION DE L'ESPACE
  13. C SORTIE
  14. C KERRE INDICE D'ERREUR ( 0 SI TOUT EST OK )
  15. C ( 1 SI JOINT DE TAILLE NULLE )
  16. C
  17. C-----------------------------------------------------------------------
  18. C
  19. C La position initiale du joint est donnee par ses deux noeuds A et B
  20. C
  21. C La position deformee du joint A'B' est deduite de IWRK (grace a la
  22. C table XDDL) qui contient les deplacements et les rotations de A et B
  23. C
  24. C Le repere du joint subit deux rotations :
  25. C - celle due aux deplacements (UX, UY, UZ) des noeuds qui amenent le
  26. C segment AB dans la position deplacee A'B'
  27. C - en dimension 3 seulement, celle due aux rotations (RX, RY, RZ) des
  28. C noeuds, seule la rotation autour de l'axe AB est appliquee
  29. C
  30. C
  31. C
  32. IMPLICIT INTEGER(I-N)
  33. IMPLICIT REAL*8(A-H,O-Z)
  34. SEGMENT/IWRK/(XDDL(LRE)*D,XE(3,NBNN)*D,WORK(LW)*D)
  35. DIMENSION R(*),R1(3),P(3,3),VAB(3),VABN(3),VAB1(3),W(3)
  36. PARAMETER (TOL1=1D-13)
  37. C
  38. KERRE=0
  39. C
  40. C --------------------------------------------------------
  41. C ETAPE 0 : Verification sur la position initiale du joint
  42. C --------------------------------------------------------
  43. C
  44. C Vecteur AB (position initiale du joint)
  45. DO I=1,IDIM
  46. VAB(I) = XE(I,2) - XE(I,1)
  47. ENDDO
  48. DAB = 0D0
  49. DO I=1,IDIM
  50. DAB = DAB + (VAB(I)*VAB(I))
  51. ENDDO
  52. DAB = SQRT(DAB)
  53. C Si les noeuds A et B sont initialement confondus, les matrices de
  54. C rotation ne sont pas definies --> on sort avec KERRE=1
  55. IF (DAB.LT.TOL1) THEN
  56. KERRE=1
  57. GOTO 2
  58. ENDIF
  59. C Vecteur AB norme
  60. DO I=1,IDIM
  61. VABN(I) = VAB(I) / DAB
  62. ENDDO
  63. C
  64. C -----------------------------------------------------------------
  65. C ETAPE 1 : ROTATION du repere due aux ddl de rotation (RX, RY, RZ)
  66. C en 3D seulement
  67. C -----------------------------------------------------------------
  68. C
  69. IF (IDIM.EQ.3) THEN
  70. C Vecteur de rotation unitaire (c'est l'axe AB)
  71. DO I=1,IDIM
  72. W(I) = VABN(I)
  73. ENDDO
  74. C Angles de rotation moyens du joint autour des directions X, Y et Z
  75. AX = 0.5 * (XDDL(4) + XDDL(10))
  76. AY = 0.5 * (XDDL(5) + XDDL(11))
  77. AZ = 0.5 * (XDDL(6) + XDDL(12))
  78. C Angle de rotation moyen autour de l'axe AB
  79. A = (VABN(1)*AX) + (VABN(2)*AY) + (VABN(3)*AZ)
  80. CA = COS(A)
  81. SA = SIN(A)
  82. C Si l'angle de rotation est nul, pas de matrice de rotation a calculer
  83. IF (ABS(A).LT.TOL1) GOTO 1
  84. C Matrice de rotation associee (d'axe W et d'angle A)
  85. P(1,1) = CA + ((1D0-CA)*(W(1)*W(1))) + (SA*0D0)
  86. P(1,2) = 0D0 + ((1D0-CA)*(W(1)*W(2))) + (SA*(-1D0*W(3)))
  87. P(1,3) = 0D0 + ((1D0-CA)*(W(1)*W(3))) + (SA*( 1D0*W(2)))
  88. P(2,1) = 0D0 + ((1D0-CA)*(W(1)*W(2))) + (SA*( 1D0*W(3)))
  89. P(2,2) = CA + ((1D0-CA)*(W(2)*W(2))) + (SA*0D0)
  90. P(2,3) = 0D0 + ((1D0-CA)*(W(2)*W(3))) + (SA*(-1D0*W(1)))
  91. P(3,1) = 0D0 + ((1D0-CA)*(W(1)*W(3))) + (SA*(-1D0*W(2)))
  92. P(3,2) = 0D0 + ((1D0-CA)*(W(2)*W(3))) + (SA*( 1D0*W(1)))
  93. P(3,3) = CA + ((1D0-CA)*(W(3)*W(3))) + (SA*0D0)
  94. C Rotation du 1er vecteur de R puis normalisation
  95. R1(1) = (P(1,1)*R(1)) + (P(1,2)*R(2)) + (P(1,3)*R(3))
  96. R1(2) = (P(2,1)*R(1)) + (P(2,2)*R(2)) + (P(2,3)*R(3))
  97. R1(3) = (P(3,1)*R(1)) + (P(3,2)*R(2)) + (P(3,3)*R(3))
  98. R(1) = R1(1)
  99. R(2) = R1(2)
  100. R(3) = R1(3)
  101. DR = SQRT((R(1)*R(1))+(R(2)*R(2))+(R(3)*R(3)))
  102. R(1) = R(1) / DR
  103. R(2) = R(2) / DR
  104. R(3) = R(3) / DR
  105. C Rotation du 2eme vecteur de R puis normalisation
  106. R1(1) = (P(1,1)*R(4)) + (P(1,2)*R(5)) + (P(1,3)*R(6))
  107. R1(2) = (P(2,1)*R(4)) + (P(2,2)*R(5)) + (P(2,3)*R(6))
  108. R1(3) = (P(3,1)*R(4)) + (P(3,2)*R(5)) + (P(3,3)*R(6))
  109. R(4) = R1(1)
  110. R(5) = R1(2)
  111. R(6) = R1(3)
  112. DR = SQRT((R(4)*R(4))+(R(5)*R(5))+(R(6)*R(6)))
  113. R(4) = R(4) / DR
  114. R(5) = R(5) / DR
  115. R(6) = R(6) / DR
  116. 1 CONTINUE
  117. ENDIF
  118. C
  119. C
  120. C --------------------------------------------------------------------
  121. C ETAPE 2 : ROTATION du repere due aux ddl de deplacement (UX, UY, UZ)
  122. C --------------------------------------------------------------------
  123. C
  124. C Vecteur A'B' (position deformee du joint)
  125. IF (IDIM.EQ.2) THEN
  126. VAB1(1) = VAB(1) + XDDL(4) - XDDL(1)
  127. VAB1(2) = VAB(2) + XDDL(5) - XDDL(2)
  128. ENDIF
  129. IF (IDIM.EQ.3) THEN
  130. VAB1(1) = VAB(1) + XDDL(7) - XDDL(1)
  131. VAB1(2) = VAB(2) + XDDL(8) - XDDL(2)
  132. VAB1(3) = VAB(3) + XDDL(9) - XDDL(3)
  133. ENDIF
  134. DAB1 = 0D0
  135. DO I=1,IDIM
  136. DAB1 = DAB1 + (VAB1(I)*VAB1(I))
  137. ENDDO
  138. DAB1 = SQRT(DAB1)
  139. C Si les noeuds A' et B' sont confondus, la matrice de rotation n'est
  140. C pas definie --> on sort avec KERRE=1
  141. IF (DAB1.LT.TOL1) THEN
  142. KERRE=1
  143. GOTO 2
  144. ENDIF
  145. C Vecteur rotation unitaire (W = AB ^ A'B')
  146. C En dimension 2, c'est l'axe Z, mais on tient compte du signe
  147. IF (IDIM.EQ.2) THEN
  148. SIG = SIGN (1D0,((VAB(1)*VAB1(2)) - (VAB(2)*VAB1(1))))
  149. ENDIF
  150. IF (IDIM.EQ.3) THEN
  151. W(1) = (VAB(2)*VAB1(3)) - (VAB(3)*VAB1(2))
  152. W(2) = (VAB(3)*VAB1(1)) - (VAB(1)*VAB1(3))
  153. W(3) = (VAB(1)*VAB1(2)) - (VAB(2)*VAB1(1))
  154. DW = SQRT((W(1)*W(1))+(W(2)*W(2))+(W(3)*W(3)))
  155. IF (DW.GT.TOL1) THEN
  156. W(1) = W(1) / DW
  157. W(2) = W(2) / DW
  158. W(3) = W(3) / DW
  159. ENDIF
  160. ENDIF
  161. C Angle de rotation autour de W ( AB . A'B' = |AB| . |A'B'| . cos a )
  162. PS = 0D0
  163. DO I=1,IDIM
  164. PS = PS + (VAB(I)*VAB1(I))
  165. ENDDO
  166. CA = PS / (DAB*DAB1)
  167. IF (CA.GT.1D0) THEN
  168. CA = 1D0
  169. ENDIF
  170. IF (CA.LT.-1D0) THEN
  171. CA = -1D0
  172. ENDIF
  173. A = ACOS(CA)
  174. IF (IDIM.EQ.2) THEN
  175. A = SIG * A
  176. CA = COS(A)
  177. ENDIF
  178. SA = SIN(A)
  179. C Si l'angle de rotation est nul, pas de rotation
  180. IF (ABS(A).LT.TOL1) GOTO 2
  181. C Matrice de rotation associee (d'axe W et d'angle A)
  182. IF (IDIM.EQ.2) THEN
  183. P(1,1) = CA
  184. P(1,2) = (-1D0) * SA
  185. P(2,1) = SA
  186. P(2,2) = CA
  187. ENDIF
  188. IF (IDIM.EQ.3) THEN
  189. P(1,1) = CA + ((1D0-CA)*(W(1)*W(1))) + (SA*0D0)
  190. P(1,2) = 0D0 + ((1D0-CA)*(W(1)*W(2))) + (SA*(-1D0*W(3)))
  191. P(1,3) = 0D0 + ((1D0-CA)*(W(1)*W(3))) + (SA*( 1D0*W(2)))
  192. P(2,1) = 0D0 + ((1D0-CA)*(W(1)*W(2))) + (SA*( 1D0*W(3)))
  193. P(2,2) = CA + ((1D0-CA)*(W(2)*W(2))) + (SA*0D0)
  194. P(2,3) = 0D0 + ((1D0-CA)*(W(2)*W(3))) + (SA*(-1D0*W(1)))
  195. P(3,1) = 0D0 + ((1D0-CA)*(W(1)*W(3))) + (SA*(-1D0*W(2)))
  196. P(3,2) = 0D0 + ((1D0-CA)*(W(2)*W(3))) + (SA*( 1D0*W(1)))
  197. P(3,3) = CA + ((1D0-CA)*(W(3)*W(3))) + (SA*0D0)
  198. ENDIF
  199. C Rotation du 1er vecteur de R puis normalisation
  200. DO I=1,IDIM
  201. R1(I) = 0D0
  202. DO J=1,IDIM
  203. R1(I) = R1(I) + (P(I,J)*R(J))
  204. ENDDO
  205. ENDDO
  206. DO I=1,IDIM
  207. R(I) = R1(I)
  208. ENDDO
  209. DR = 0D0
  210. DO I=1,IDIM
  211. DR = DR + (R(I)*R(I))
  212. ENDDO
  213. DR = SQRT(DR)
  214. DO I=1,IDIM
  215. R(I) = R(I) / DR
  216. ENDDO
  217. C Rotation du 2eme vecteur de R puis normalisation
  218. DO I=1,IDIM
  219. R1(I) = 0D0
  220. DO J=1,IDIM
  221. R1(I) = R1(I) + (P(I,J)*R(J+3))
  222. ENDDO
  223. ENDDO
  224. DO I=1,IDIM
  225. R(I+3) = R1(I)
  226. ENDDO
  227. DR = 0D0
  228. DO I=1,IDIM
  229. DR = DR + (R(I+3)*R(I+3))
  230. ENDDO
  231. DR = SQRT(DR)
  232. DO I=1,IDIM
  233. R(I+3) = R(I+3) / DR
  234. ENDDO
  235. C
  236. 2 CONTINUE
  237. 999 RETURN
  238. END
  239.  
  240.  
  241.  
  242.  

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