46
47
48
49#include "implicit_f.inc"
50
51
52
53#include "mvsiz_p.inc"
54
55
56
57#include "vect01_c.inc"
58
59
60
61 INTEGER NEL
62
64 . x(3,*),v(3,*),w(3,*), vis(*),
65 . x1(*), x2(*), x3(*), x4(*), x5(*), x6(*),
66 . y1(*), y2(*), y3(*), y4(*), y5(*), y6(*),
67 . z1(*), z2(*), z3(*), z4(*), z5(*), z6(*),
68 . vx1(*), vx2(*), vx3(*), vx4(*), vx5(*), vx6(*),
69 . vy1(*), vy2(*), vy3(*), vy4(*), vy5(*), vy6(*),
70 . vz1(*), vz2(*), vz3(*), vz4(*), vz5(*), vz6(*),
71 . vd2(*), offg(*), off(*), rho(*), rhoo(*),
72 . r11(mvsiz),r12(mvsiz),r13(mvsiz),
73 . r21(mvsiz),r22(mvsiz),r23(mvsiz),
74 . r31(mvsiz),r32(mvsiz),r33(mvsiz),
75 . rx(mvsiz) , ry(mvsiz) , rz(mvsiz) ,
76 . sx(mvsiz) , sy(mvsiz) , sz(mvsiz) ,
77 . tx(mvsiz) , ty(mvsiz) , tz(mvsiz) ,
78 . gama0(nel,6),gama(mvsiz,6),
79 . vgax(*), vgay(*), vgaz(*), vga2(*),di(mvsiz,6),
80 . xgax(*), xgay(*), xgaz(*),
81 . xgxa2(mvsiz),xgya2(mvsiz),xgza2(mvsiz),
82 . xgxya(mvsiz),xgyza(mvsiz),xgzxa(mvsiz),gama_r(nel,6)
83 double precision
84 . sav(nel,15)
85 INTEGER NC1(*), NC2(*), NC3(*), NC4(*),
86 . NC5(*), NC6(*), MXT(*), NGL(*),NGEO(*)
87 INTEGER IXS(NIXS,*),IOUTPRT,IPARG(*)
88
89
90
91
92 INTEGER I
93
95 . dt05
97 . g11(mvsiz),g12(mvsiz),g13(mvsiz),
98 . g21(mvsiz),g22(mvsiz),g23(mvsiz),
99 . g31(mvsiz),g32(mvsiz),g33(mvsiz),
100 . t11(mvsiz),t12(mvsiz),t13(mvsiz),
101 . t21(mvsiz),t22(mvsiz),t23(mvsiz),
102 . t31(mvsiz),t32(mvsiz),t33(mvsiz)
104 . xl,yl,zl
106 . xx,yy,zz,xy,xz,yz,rtr(6),abc,xxyz2,zzxy2,yyxz2,deta
108 . off_l
109
110 off_l = zero
111
112 DO i=lft,llt
113 vis(i)=zero
114 ngeo(i)=ixs(10,i)
115 ngl(i)=ixs(11,i)
116 mxt(i)=ixs(1,i)
117 nc1(i)=ixs(2,i)
118 nc2(i)=ixs(3,i)
119 nc3(i)=ixs(4,i)
120 nc4(i)=ixs(6,i)
121 nc5(i)=ixs(7,i)
122 nc6(i)=ixs(8,i)
123 rhoo(i)=rho(i)
124 ENDDO
125
126
127
128 DO i=lft,llt
129 x1(i)=x(1,nc1(i))
130 y1(i)=x(2,nc1(i))
131 z1(i)=x(3,nc1(i))
132 x2(i)=x(1,nc2(i))
133 y2(i)=x(2,nc2(i))
134 z2(i)=x(3,nc2(i))
135 x3(i)=x(1,nc3(i))
136 y3(i)=x(2,nc3(i))
137 z3(i)=x(3,nc3(i))
138 x4(i)=x(1,nc4(i))
139 y4(i)=x(2,nc4(i))
140 z4(i)=x(3,nc4(i))
141 x5(i)=x(1,nc5(i))
142 y5(i)=x(2,nc5(i))
143 z5(i)=x(3,nc5(i))
144 x6(i)=x(1,nc6(i))
145 y6(i)=x(2,nc6(i))
146 z6(i)=x(3,nc6(i))
147 off(i) =
min(one,abs(offg(i)))
148 off_l =
min(off_l,offg(i))
149 ENDDO
150
151 DO i=lft,llt
152 vx1(i)=v(1,nc1(i))
153 vy1(i)=v(2,nc1(i))
154 vz1(i)=v(3,nc1(i))
155 vx2(i)=v(1,nc2(i))
156 vy2(i)=v(2,nc2(i))
157 vz2(i)=v(3,nc2(i))
158 vx3(i)=v(1,nc3(i))
159 vy3(i)=v(2,nc3(i))
160 vz3(i)=v(3,nc3(i))
161 vx4(i)=v(1,nc4(i))
162 vy4(i)=v(2,nc4(i))
163 vz4(i)=v(3,nc4(i))
164 vx5(i)=v(1,nc5(i))
165 vy5(i)=v(2,nc5(i))
166 vz5(i)=v(3,nc5(i))
167 vx6(i)=v(1,nc6(i))
168 vy6(i)=v(2,nc6(i))
169 vz6(i)=v(3,nc6(i))
170 ENDDO
171 IF(off_l<zero)THEN
172 DO i=lft,llt
173 IF(offg(i)<zero)THEN
174 vx1(i)=zero
175 vy1(i)=zero
176 vz1(i)=zero
177 vx2(i)=zero
178 vy2(i)=zero
179 vz2(i)=zero
180 vx3(i)=zero
181 vy3(i)=zero
182 vz3(i)=zero
183 vx4(i)=zero
184 vy4(i)=zero
185 vz4(i)=zero
186 vx5(i)=zero
187 vy5(i)=zero
188 vz5(i)=zero
189 vx6(i)=zero
190 vy6(i)=zero
191 vz6(i)=zero
192 ENDIF
193 ENDDO
194 ENDIF
195
196
197
198 IF(ioutprt/=0)THEN
199 DO i=lft,llt
200 vgax(i)=vx1(i)+vx2(i)+vx3(i)+vx4(i)+vx5(i)+vx6(i)
201 vgay(i)=vy1(i)+vy2(i)+vy3(i)+vy4(i)+vy5(i)+vy6(i)
202 vgaz(i)=vz1(i)+vz2(i)+vz3(i)+vz4(i)+vz5(i)+vz6(i)
203 vga2(i)=vx1(i)*vx1(i)+vx2(i)*vx2(i)+vx3(i)*vx3(i)+vx4(i)*vx4(i)
204 1 +vx5(i)*vx5(i)+vx6(i)*vx6(i)
205 2 +vy1(i)*vy1(i)+vy2(i)*vy2(i)+vy3(i)*vy3(i)+vy4(i)*vy4(i)
206 3 +vy5(i)*vy5(i)+vy6(i)*vy6(i)
207 4 +vz1(i)*vz1(i)+vz2(i)*vz2(i)+vz3(i)*vz3(i)+vz4(i)*vz4(i)
208 5 +vz5(i)*vz5(i)+vz6(i)*vz6(i)
209 ENDDO
210 IF(iparg(80)==1) THEN
211 DO i=lft,llt
212 xgax(i)=x1(i)+x2(i)+x3(i)+x4(i)+x5(i)+x6(i)
213 xgay(i)=y1(i)+y2(i)+y3(i)+y4(i)+y5(i)+y6(i)
214 xgaz(i)=z1(i)+z2(i)+z3(i)+z4(i)+z5(i)+z6(i)
215 xgxa2(i)=x1(i)**2+x2(i)**2+x3(i)**2+x4(i)**2
216 1 +x5(i)**2+x6(i)**2
217 xgya2(i)=y1(i)**2+y2(i)**2+y3(i)**2+y4(i)**2
218 1 +y5(i)**2+y6(i)**2
219 xgza2(i)=z1(i)**2+z2(i)**2+z3(i)**2+z4(i)**2
220 1 +z5(i)**2+z6(i)**2
221 xgxya(i)=x1(i)*y1(i)+x2(i)*y2(i)+x3(i)*y3(i)+x4(i)*y4(i)
222 1 +x5(i)*y5(i)+x6(i)*y6(i)
223 xgyza(i)=y1(i)*z1(i)+y2(i)*z2(i)+y3(i)*z3(i)+y4(i)*z4(i)
224 1 +y5(i)*z5(i)+y6(i)*z6(i)
225 xgzxa(i)=z1(i)*x1(i)+z2(i)*x2(i)+z3(i)*x3(i)+z4(i)*x4(i)
226 1 +z5(i)*x5(i)+z6(i)*x6(i)
227 ENDDO
228 ENDIF
229 ENDIF
230
231 DO i=lft,llt
232 xl=one_over_6*(x1(i)+x2(i)+x3(i)+x4(i)+x5(i)+x6(i))
233 yl=one_over_6*(y1(i)+y2(i)+y3(i)+y4(i)+y5(i)+y6(i))
234 zl=one_over_6*(z1(i)+z2(i)+z3(i)+z4(i)+z5(i)+z6(i))
235 x1(i)=x1(i)-xl
236 y1(i)=y1(i)-yl
237 z1(i)=z1(i)-zl
238 x2(i)=x2(i)-xl
239 y2(i)=y2(i)-yl
240 z2(i)=z2(i)-zl
241 x3(i)=x3(i)-xl
242 y3(i)=y3(i)-yl
243 z3(i)=z3(i)-zl
244 x4(i)=x4(i)-xl
245 y4(i)=y4(i)-yl
246 z4(i)=z4(i)-zl
247 x5(i)=x5(i)-xl
248 y5(i)=y5(i)-yl
249 z5(i)=z5(i)-zl
250 x6(i)=x6(i)-xl
251 y6(i)=y6(i)-yl
252 z6(i)=z6(i)-zl
253 ENDDO
254
255
256
258 1 x1, x2, x3, x4,
259 2 x5, x6, y1, y2,
260 3 y3, y4, y5, y6,
261 4 z1, z2, z3, z4,
262 5 z5, z6, r11, r12,
263 6 r13, r21, r22, r23,
264 7 r31, r32, r33, rx,
265 8 ry, rz, sx, sy,
266 9 sz, tx, ty, tz,
267 a nel)
268
269 gama_r(lft:llt,1) = r11(lft:llt)
270 gama_r(lft:llt,2) = r21(lft:llt)
271 gama_r(lft:llt,3) = r31(lft:llt)
272 gama_r(lft:llt,4) = r12(lft:llt)
273 gama_r(lft:llt,5) = r22(lft:llt)
274 gama_r(lft:llt,6) = r32(lft:llt)
275
276
277
278
279 IF(ismstr<=4.AND.jlag>0) THEN
280 DO i=lft,llt
281 IF(offg(i)>one)THEN
282 x1(i)=sav(i,1)
283 y1(i)=sav(i,2)
284 z1(i)=sav(i,3)
285 x2(i)=sav(i,4)
286 y2(i)=sav(i,5)
287 z2(i)=sav(i,6)
288 x3(i)=sav(i,7)
289 y3(i)=sav(i,8)
290 z3(i)=sav(i,9)
291 x4(i)=sav(i,10)
292 y4(i)=sav(i,11)
293 z4(i)=sav(i,12)
294 x5(i)=sav(i,13)
295 y5(i)=sav(i,14)
296 z5(i)=sav(i,15)
297 x6(i)=zero
298 y6(i)=zero
299 z6(i)=zero
300 off(i) = offg(i) -one
301 xl=one_over_6*(x1(i)+x2(i)+x3(i)+x4(i)+x5(i)+x6(i))
302 yl=one_over_6*(y1(i)+y2(i)+y3(i)+y4(i)+y5(i)+y6(i))
303 zl=one_over_6*(z1(i)+z2(i)+z3(i)+z4(i)+z5(i)+z6(i))
304 x1(i)=x1(i)-xl
305 y1(i)=y1(i)-yl
306 z1(i)=z1(i)-zl
307 x2(i)=x2(i)-xl
308 y2(i)=y2(i)-yl
309 z2(i)=z2(i)-zl
310 x3(i)=x3(i)-xl
311 y3(i)=y3(i)-yl
312 z3(i)=z3(i)-zl
313 x4(i)=x4(i)-xl
314 y4(i)=y4(i)-yl
315 z4(i)=z4(i)-zl
316 x5(i)=x5(i)-xl
317 y5(i)=y5(i)-yl
318 z5(i)=z5(i)-zl
319 x6(i)=x6(i)-xl
320 y6(i)=y6(i)-yl
321 z6(i)=z6(i)-zl
322 ELSE
323 xl=r11(i)*x1(i)+r21(i)*y1(i)+r31(i)*z1(i)
324 yl=r12(i)*x1(i)+r22(i)*y1(i)+r32(i)*z1(i)
325 zl=r13(i)*x1(i)+r23(i)*y1(i)+r33(i)*z1(i)
326 x1(i)=xl
327 y1(i)=yl
328 z1(i)=zl
329 xl=r11(i)*x2(i)+r21(i)*y2(i)+r31(i)*z2(i)
330 yl=r12(i)*x2(i)+r22(i)*y2(i)+r32(i)*z2(i)
331 zl=r13(i)*x2(i)+r23(i)*y2(i)+r33(i)*z2(i)
332 x2(i)=xl
333 y2(i)=yl
334 z2(i)=zl
335 xl=r11(i)*x3(i)+r21(i)*y3(i)+r31(i)*z3(i)
336 yl=r12(i)*x3(i)+r22(i)*y3(i)+r32(i)*z3(i)
337 zl=r13(i)*x3(i)+r23(i)*y3(i)+r33(i)*z3(i)
338 x3(i)=xl
339 y3(i)=yl
340 z3(i)=zl
341 xl=r11(i)*x4(i)+r21(i)*y4(i)+r31(i)*z4(i)
342 yl=r12(i)*x4(i)+r22(i)*y4(i)+r32(i)*z4(i)
343
344 x4(i)=xl
345 y4(i)=yl
346 z4(i)=-z1(i)
347 xl=r11(i)*x5(i)+r21(i)*y5(i)+r31(i)*z5(i)
348 yl=r12(i)*x5(i)+r22(i)*y5(i)+r32(i)*z5(i)
349
350 x5(i)=xl
351 y5(i)=yl
352 z5(i)=-z2(i)
353 xl=r11(i)*x6(i)+r21(i)*y6(i)+r31(i)*z6(i)
354 yl=r12(i)*x6(i)+r22(i)*y6(i)+r32(i)*z6(i)
355
356 x6(i)=xl
357 y6(i)=yl
358 z6(i)=-z3(i)
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375 off(i) = offg(i)
376 ENDIF
377 ENDDO
378
379 ELSE
381 1 r11, r12, r13, r21,
382 2 r22, r23, r31, r32,
383 3 r33, x1, y1, z1,
384 4 nel)
386 1 r11, r12, r13, r21,
387 2 r22, r23, r31, r32,
388 3 r33, x2, y2, z2,
389 4 nel)
391 1 r11, r12, r13, r21,
392 2 r22, r23, r31, r32,
393 3 r33, x3, y3, z3,
394 4 nel)
395 DO i=lft,llt
396 xl=r11(i)*x4(i)+r21(i)*y4(i)+r31(i)*z4(i)
397 yl=r12(i)*x4(i)+r22(i)*y4(i)+r32(i)*z4(i)
398
399 x4(i)=xl
400 y4(i)=yl
401 z4(i)=-z1(i)
402 xl=r11(i)*x5(i)+r21(i)*y5(i)+r31(i)*z5(i)
403 yl=r12(i)*x5(i)+r22(i)*y5(i)+r32(i)*z5(i)
404
405 x5(i)=xl
406 y5(i)=yl
407 z5(i)=-z2(i)
408 xl=r11(i)*x6(i)+r21(i)*y6(i)+r31(i)*z6(i)
409 yl=r12(i)*x6(i)+r22(i)*y6(i)+r32(i)*z6(i)
410
411 x6(i)=xl
412 y6(i)=yl
413 z6(i)=-z3(i)
414 off(i) =
min(one,offg(i))
415 ENDDO
416
417 ENDIF
418
419
420
422 1 r11, r12, r13, r21,
423 2 r22, r23, r31, r32,
424 3 r33, vx1, vy1, vz1,
425 4 nel)
427 1 r11, r12, r13, r21,
428 2 r22, r23, r31, r32,
429 3 r33, vx2, vy2, vz2,
430 4 nel)
432 1 r11, r12, r13, r21,
433 2 r22, r23, r31, r32,
434 3 r33, vx3, vy3, vz3,
435 4 nel)
437 1 r11, r12, r13, r21,
438 2 r22, r23, r31, r32,
439 3 r33, vx4, vy4, vz4,
440 4 nel)
442 1 r11, r12, r13, r21,
443 2 r22, r23, r31, r32,
444 3 r33, vx5, vy5, vz5,
445 4 nel)
447 1 r11, r12, r13, r21,
448 2 r22, r23, r31, r32,
449 3 r33, vx6, vy6, vz6,
450 4 nel)
451
452 DO i=lft,llt
453 vd2(i) = zero
454 ENDDO
455
456 DO i=lft,llt
457 xx = x1(i)*x1(i)+x2(i)*x2(i)+x3(i)*x3(i)
458 1 +x4(i)*x4(i)+x5(i)*x5(i)+x6(i)*x6(i)
459 yy = y1(i)*y1(i)+y2(i)*y2(i)+y3(i)*y3(i)
460 1 +y4(i)*y4(i)+y5(i)*y5(i)+y6(i)*y6(i)
461 xy = x1(i)*y1(i)+x2(i)*y2(i)+x3(i)*y3(i)
462 1 +x4(i)*y4(i)+x5(i)*y5(i)+x6(i)*y6(i)
463 xz = x1(i)*z1(i)+x2(i)*z2(i)+x3(i)*z3(i)
464 1 +x4(i)*z4(i)+x5(i)*z5(i)+x6(i)*z6(i)
465 yz = y1(i)*z1(i)+y2(i)*z2(i)+y3(i)*z3(i)
466 1 +y4(i)*z4(i)+y5(i)*z5(i)+y6(i)*z6(i)
467 zz = z1(i)*z1(i)+z2(i)*z2(i)+z3(i)*z3(i)
468 1 +z4(i)*z4(i)+z5(i)*z5(i)+z6(i)*z6(i)
469 rtr(1)= yy+zz
470 rtr(2)= xx+zz
471 rtr(3)= xx+yy
472 rtr(4)= -xy
473 rtr(5)= -xz
474 rtr(6)= -yz
475
476 abc = rtr(1)*rtr(2)*rtr(3)
477 xxyz2 = rtr(1)*rtr(6)*rtr(6)
478 yyxz2 = rtr(2)*rtr(5)*rtr(5)
479 zzxy2 = rtr(3)*rtr(4)*rtr(4)
480 deta = abc + two*rtr(4)*rtr(5)*rtr(6)-xxyz2-yyxz2-zzxy2
481 IF (deta<em20) THEN
482 deta=one
483 ELSE
484 deta=one/deta
485 ENDIF
486 di(i,1) = (abc-xxyz2)*deta/rtr(1)
487 di(i,2) = (abc-yyxz2)*deta/rtr(2)
488 di(i,3) = (abc-zzxy2)*deta/rtr(3)
489 di(i,4) = (rtr(5)*rtr(6)-rtr(4)*rtr(3))*deta
490 di(i,5) = (rtr(6)*rtr(4)-rtr(5)*rtr(2))*deta
491 di(i,6) = (rtr(4)*rtr(5)-rtr(6)*rtr(1))*deta
492 ENDDO
494 1 x1, x2, x3, x4,
495 2 x5, x6, y1, y2,
496 3 y3, y4, y5, y6,
497 4 z1, z2, z3, z4,
498 5 z5, z6, vx1, vx2,
499 6 vx3, vx4, vx5, vx6,
500 7 vy1, vy2, vy3, vy4,
501 8 vy5, vy6, vz1, vz2,
502 9 vz3, vz4, vz5, vz6,
503 a di, nel)
504 RETURN
subroutine s6proj3(x1, x2, x3, x4, x5, x6, y1, y2, y3, y4, y5, y6, z1, z2, z3, z4, z5, z6, vx1, vx2, vx3, vx4, vx5, vx6, vy1, vy2, vy3, vy4, vy5, vy6, vz1, vz2, vz3, vz4, vz5, vz6, di, nel)
subroutine s6cortho3(x1, x2, x3, x4, x5, x6, y1, y2, y3, y4, y5, y6, z1, z2, z3, z4, z5, z6, rx, ry, rz, sx, sy, sz, tx, ty, tz, e1x, e1y, e1z, e2x, e2y, e2z, e3x, e3y, e3z)
subroutine vrrota3(r11, r12, r13, r21, r22, r23, r31, r32, r33, x1, y1, z1, nel)