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 : *
45 : CALL EXCOO1(IPO1,X1,Y1,Z1,D1)
46 : IF (IERR.NE.0) RETURN
47 : XP1PT(1) = ((XUN - XCOS) * X1) + (XSIN * Y1)
48 : XP1PT(2) = ((XUN - XCOS) * Y1) - (XSIN * X1)
49 : *
50 : ELSE
50 : CALL EXCOO1(IPO1,X1,Y1,Z1,D1)
50 : CALL EXCOO1(IPO2,X2,Y2,Z2,D2)
51 : IF (IERR.NE.0) RETURN
52 : XP1P2 = X2 - X1
53 : YP1P2 = Y2 - Y1
54 : ZP1P2 = Z2 - Z1
55 : *
56 : * Vecteur R
57 : *
58 : PS = (XP1P2 * XP1P2) + (YP1P2 * YP1P2) + (ZP1P2 * ZP1P2)
59 : IF (PS.LT.EPSIL) THEN
59 : CALL ERREUR(162)
60 : RETURN
61 : ENDIF
62 : PS = SQRT(PS)
63 : XR = XP1P2 / PS
64 : YR = YP1P2 / PS
65 : ZR = ZP1P2 / PS
66 : *
67 : * Vecteur A
68 : *
69 : IF (ABS(ZR).GT.TOLER) THEN
70 : XA1 = X1
71 : YA1 = XR + YR + ZR
72 : ZA1 = Z1 - (YR * (YA1 - Y1) / ZR)
73 : ELSE IF (ABS(YR).GT.TOLER) THEN
74 : ZA1 = Z1
75 : XA1 = XR + YR + ZR
76 : YA1 = Y1 - (XR * (XA1 - X1) / YR)
77 : ELSE
78 : YA1 = Y1
79 : ZA1 = XR + YR + ZR
80 : XA1 = X1 - (ZR * (ZA1 - Z1) / XR)
81 : ENDIF
82 : XP1A = XA1 - X1
83 : YP1A = YA1 - Y1
84 : ZP1A = ZA1 - Z1
85 : PS = (XP1A * XP1A) + (YP1A * YP1A) + (ZP1A * ZP1A)
86 : IF (PS.LT.EPSIL) THEN
86 : CALL ERREUR(162)
87 : RETURN
88 : ENDIF
89 : PS = SQRT(PS)
90 : XA = XP1A / PS
91 : YA = YP1A / PS
92 : ZA = ZP1A / PS
93 : *
94 : * Vecteur B
95 : *
96 : XB = (YR * ZA) - (ZR * YA)
97 : YB = (ZR * XA) - (XR * ZA)
98 : ZB = (XR * YA) - (YR * XA)
99 : *
100 : * Matrice de passage T
101 : *
102 : XT(1,1) = XA
103 : XT(1,2) = YA
104 : XT(1,3) = ZA
105 : XT(2,1) = XB
106 : XT(2,2) = YB
107 : XT(2,3) = ZB
108 : XT(3,1) = XR
109 : XT(3,2) = YR
110 : XT(3,3) = ZR
111 : *
112 : XTM1(1,1) = XA
113 : XTM1(2,1) = YA
114 : XTM1(3,1) = ZA
115 : XTM1(1,2) = XB
116 : XTM1(2,2) = YB
117 : XTM1(3,2) = ZB
118 : XTM1(1,3) = XR
119 : XTM1(2,3) = YR
120 : XTM1(3,3) = ZR
121 : * T
122 : * Matrice de rotation R
123 : *
124 : XRT(1,1) = XCOS
125 : XRT(1,2) = - XSIN
126 : XRT(1,3) = XZERO
127 : XRT(2,1) = XSIN
128 : XRT(2,2) = XCOS
129 : XRT(2,3) = XZERO
130 : XRT(3,1) = XZERO
131 : XRT(3,2) = XZERO
132 : XRT(3,3) = XUN
133 : *
134 : * Matrice P
135 : *
136 : DO 10 I = 1,IDIM
137 : DO 20 J = 1,IDIM
138 : XVAL = XZERO
139 : DO 30 K = 1,IDIM
140 : XVAL = XVAL + XRT(I,K) * XT(K,J)
141 : 30 CONTINUE
142 : * end do
143 : RTT(I,J) = XVAL
144 : 20 CONTINUE
145 : * end do
146 : 10 CONTINUE
147 : * end do
148 : DO 12 I = 1,IDIM
149 : DO 22 J = 1,IDIM
150 : XVAL = XZERO
151 : DO 32 K = 1,IDIM
152 : XVAL = XVAL + XTM1(I,K) * RTT(K,J)
153 : 32 CONTINUE
154 : * end do
155 : XMPT(I,J) = XVAL
156 : 22 CONTINUE
157 : * end do
158 : 12 CONTINUE
159 : * end do
160 : *
161 : XIMPT(1,1) = XUN - XMPT(1,1)
162 : XIMPT(1,2) = - XMPT(1,2)
163 : XIMPT(1,3) = - XMPT(1,3)
164 : XIMPT(2,1) = - XMPT(2,1)
165 : XIMPT(2,2) = XUN - XMPT(2,2)
166 : XIMPT(2,3) = - XMPT(2,3)
167 : XIMPT(3,1) = - XMPT(3,1)
168 : XIMPT(3,2) = - XMPT(3,2)
169 : XIMPT(3,3) = XUN - XMPT(3,3)
170 : *
171 : DO 14 I = 1,IDIM
172 : XP1PT(I) = (XIMPT(I,1) * X1) + (XIMPT(I,2) * Y1) +
173 : & (XIMPT(I,3) * Z1)
174 : 14 CONTINUE
175 : * end do
176 : *
177 : IF (IIMPI.EQ.333) THEN
178 : WRITE(IOIMP,*)'CALMAT : impression du vecteur R'
179 : WRITE(IOIMP,*)'CALMAT : XR =',XR
180 : WRITE(IOIMP,*)'CALMAT : YR =',YR
181 : WRITE(IOIMP,*)'CALMAT : ZR =',ZR
182 : WRITE(IOIMP,*)'CALMAT : impression du vecteur A'
183 : WRITE(IOIMP,*)'CALMAT : XA =',XA
184 : WRITE(IOIMP,*)'CALMAT : YA =',YA
185 : WRITE(IOIMP,*)'CALMAT : ZA =',ZA
186 : WRITE(IOIMP,*)'CALMAT : impression du vecteur B'
187 : WRITE(IOIMP,*)'CALMAT : XB =',XB
188 : WRITE(IOIMP,*)'CALMAT : YB =',YB
189 : WRITE(IOIMP,*)'CALMAT : ZB =',ZB
190 : WRITE(IOIMP,*)' T'
191 : WRITE(IOIMP,*)'CALMAT : impression de la matrice (P)'
192 : DO 40 I = 1,IDIM
193 : DO 42 J = 1,IDIM
194 : WRITE(IOIMP,*)'CALMAT : PT(',I,',',J,') =',XMPT(I,J)
195 : 42 CONTINUE
196 : * end do
197 : 40 CONTINUE
198 : * end do
199 : ENDIF
200 : ENDIF
201 : *
202 : END
203 :
204 :
205 :
© Cast3M 2003 - All rights reserved.
Disclaimer