OpenRadioss 2025.1.11
OpenRadioss project
Loading...
Searching...
No Matches
sms_fixvel.F
Go to the documentation of this file.
1Copyright> OpenRadioss
2Copyright> Copyright (C) 1986-2025 Altair Engineering Inc.
3Copyright>
4Copyright> This program is free software: you can redistribute it and/or modify
5Copyright> it under the terms of the GNU Affero General Public License as published by
6Copyright> the Free Software Foundation, either version 3 of the License, or
7Copyright> (at your option) any later version.
8Copyright>
9Copyright> This program is distributed in the hope that it will be useful,
10Copyright> but WITHOUT ANY WARRANTY; without even the implied warranty of
11Copyright> MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12Copyright> GNU Affero General Public License for more details.
13Copyright>
14Copyright> You should have received a copy of the GNU Affero General Public License
15Copyright> along with this program. If not, see <https://www.gnu.org/licenses/>.
16Copyright>
17Copyright>
18Copyright> Commercial Alternative: Altair Radioss Software
19Copyright>
20Copyright> As an alternative to this open-source version, Altair also offers Altair Radioss
21Copyright> software under a commercial license. Contact Altair to discuss further if the
22Copyright> commercial version may interest you: https://www.altair.com/radioss/.
23!||====================================================================
24!|| sms_fixvel ../engine/source/ams/sms_fixvel.F
25!||--- called by ------------------------------------------------------
26!|| sms_encin_2 ../engine/source/ams/sms_encin_2.F
27!|| sms_mass_scale_2 ../engine/source/ams/sms_mass_scale_2.F
28!|| sms_pcg ../engine/source/ams/sms_pcg.F
29!||--- calls -----------------------------------------------------
30!|| vinter ../engine/source/tools/curve/vinter.F
31!||--- uses -----------------------------------------------------
32!|| sensor_mod ../common_source/modules/sensor_mod.F90
33!||====================================================================
34 SUBROUTINE sms_fixvel(IBFV ,A ,V ,NPC ,TF ,
35 2 VEL ,MS ,X ,SKEW ,SENSOR_TAB,
36 3 WEIGHT ,D ,IFRAME ,XFRAME ,NSENSOR ,
37 4 IT ,DIAG_SMS,NODNX_SMS ,CPTREAC ,
38 5 NODREAC,FTHREAC ,AR ,VR ,DR ,
39 6 IN ,RBY ,WFEXT)
40C-----------------------------------------------
41C M o d u l e s
42C-----------------------------------------------
43 USE sensor_mod
44C-----------------------------------------------
45C I m p l i c i t T y p e s
46C-----------------------------------------------
47#include "implicit_f.inc"
48C-----------------------------------------------
49C G l o b a l P a r a m e t e r s
50C-----------------------------------------------
51#include "mvsiz_p.inc"
52C-----------------------------------------------
53C C o m m o n B l o c k s
54C-----------------------------------------------
55#include "com01_c.inc"
56#include "com04_c.inc"
57!#include "com06_c.inc"
58#include "com08_c.inc"
59#include "param_c.inc"
60#include "parit_c.inc"
61C-----------------------------------------------
62C D u m m y A r g u m e n t s
63C-----------------------------------------------
64 INTEGER ,INTENT(IN) :: NSENSOR
65 INTEGER NPC(*),CPTREAC,NODREAC(*)
66 INTEGER IBFV(NIFV,*),WEIGHT(*),IFRAME(LISKN,*),IT,NODNX_SMS(*)
67 my_real
68 . A(3,*), V(3,*), TF(*), VEL(LFXVELR,*), MS(*), X(3,*), D(3,*),
69 . skew(lskew,*),xframe(nxframe,*),
70 . diag_sms(*),fthreac(6,*),
71 . ar(3,*), vr(3,*), dr(3,*), in(*), rby(nrby,*)
72 TYPE (SENSOR_STR_) ,DIMENSION(NSENSOR) ,INTENT(IN) :: SENSOR_TAB
73 DOUBLE PRECISION,INTENT(INOUT) :: WFEXT
74C-----------------------------------------------
75C L o c a l V a r i a b l e s
76C-----------------------------------------------
77 INTEGER N, I, ISK, J, L, K1, K2, K3, ISENS,K,
78 . ii, ic, nn, ideb, nr, nsk, nfk, ifm, n0,
79 . ilenc(mvsiz), iposc(mvsiz), iadc(mvsiz),
80 . lc(mvsiz), index(mvsiz), icoor
81C REAL
82 my_real
83 . ax, axi, aold, vv, a0, aa, fac,facx,startt, stopt, ts,
84 . dydx,dw,dw2,dd,rx,ry,rz,vf,vfx,vfy,vfz,afx,afy,afz,
85 . yc(mvsiz), tsc(mvsiz), dydxc(mvsiz),aold0(3,mvsiz),
86 . r, cth, sth, skdir(3), fmdir(3), hx, hy, hz, coef,
87 . er(3),et(3),alpha, nor1, nor2, nor3, yimp
88C-----------------------------------------------
89 IF(n2d==1)THEN
90 ax=one
91 ELSE
92 ax=zero
93 ENDIF
94C
95 IF(it==0)THEN
96C
97C------------------
98 ideb = 0
99C
100 DO nn=1,nfxvel,nvsiz
101 IF (ibfv(8,nn)==1) GOTO 100
102 ic = 0
103 IF (nsensor>0) THEN
104 DO ii = 1, min(nfxvel-ideb,nvsiz)
105 n = ii+ideb
106 startt = vel(2,n)
107 stopt = vel(3,n)
108 facx = vel(5,n)
109 IF (tt<startt) cycle
110 IF (tt>stopt) cycle
111 i=iabs(ibfv(1,n))
112C
113 IF(nodnx_sms(i)==0) cycle
114 isens = ibfv(4,n)
115 IF (isens > 0) THEN
116 ts = tt-sensor_tab(isens)%TSTART
117 IF (ts < zero) cycle
118 ELSE
119 ts = tt
120 ENDIF
121 ic = ic + 1
122 index(ic) = n
123 IF(ibfv(7,n)==1) THEN
124 tsc(ic) = (ts+half*dt2)*facx
125 ELSE
126 tsc(ic) = (ts+dt2)*facx
127 ENDIF
128 END DO
129 ELSE
130C sans sensor
131 DO ii = 1, min(nfxvel-ideb,nvsiz)
132 n = ii+ideb
133 startt = vel(2,n)
134 stopt = vel(3,n)
135 facx = vel(5,n)
136 IF (tt<startt) cycle
137 IF (tt>stopt) cycle
138 i=iabs(ibfv(1,n))
139C
140 IF(nodnx_sms(i)==0) cycle
141 ic = ic + 1
142 index(ic) = n
143 IF(ibfv(7,n)==1) THEN
144 tsc(ic) = (tt + half*dt2)*facx
145 ELSE
146 tsc(ic) = (tt+dt2)*facx
147 ENDIF
148 END DO
149 ENDIF
150C
151 IF (cptreac/=0) THEN
152 DO ii=1,ic
153 n = index(ii)
154 i=iabs(ibfv(1,n))
155 aold0(1,ii)=a(1,i)
156 aold0(2,ii)=a(2,i)
157 aold0(3,ii)=a(3,i)
158 ENDDO
159 ENDIF
160C
161 ideb = ideb + min(nfxvel-ideb,nvsiz)
162C-------
163 IF(ncycle==0)THEN
164 DO ii=1,ic
165 n = index(ii)
166 l = ibfv(3,n)
167 lc(ii) = l
168 iposc(ii) = 0
169 iadc(ii) = npc(l) / 2 + 1
170 ilenc(ii) = npc(l+1) / 2 - iadc(ii) - iposc(ii)
171 ENDDO
172 ELSE
173 DO ii=1,ic
174 n = index(ii)
175 l = ibfv(3,n)
176 lc(ii) = l
177 iposc(ii) = ibfv(5,n)
178 iadc(ii) = npc(l) / 2 + 1
179 ilenc(ii) = npc(l+1) / 2 - iadc(ii) - iposc(ii)
180 ENDDO
181 ENDIF
182 CALL vinter(tf,iadc,iposc,ilenc,ic,tsc,dydxc,yc)
183 IF(ivector==0) THEN
184 DO ii=1,ic
185 n = index(ii)
186 i=iabs(ibfv(1,n))
187 ibfv(5,n) = iposc(ii)
188 fac = vel(1,n)
189 yimp = vel(6,n)
190c VEL(6,N) imposed displacement at t-dt, is also used in fixvel
191c => imposed displacement at t will be stored in fixvel
192c VEL(6,N) = YC(II)
193 dw = vel(4,n)
194 wfext=wfext + dw
195 yc(ii) = yc(ii) * fac
196 yimp = yimp * fac
197 isk=ibfv(2,n)/10
198 ifm = ibfv(9,n)
199 j=ibfv(2,n)
200 icoor = ibfv(10,n)
201 skdir=zero
202 fmdir=zero
203 r=one
204 IF (ifm<=1) j=j-10*isk
205 IF(j<=3)THEN
206 axi=one-ax+ax*x(2,i)
207 IF(isk<=1.AND.ifm<=1.AND.icoor==0)THEN
208 IF(ibfv(7,n)==2) yc(ii)=(yc(ii)-d(j,i))/dt2
209 IF(ibfv(7,n)>=1) yc(ii)=(yc(ii)-v(j,i))/dt12
210 aold=a(j,i)
211 a(j,i)=diag_sms(i)*yc(ii)
212 dw = fourth*axi*weight(i) *
213 . (yc(ii)*dt12 + two*v(j,i))*(ms(i)*yc(ii)-aold)
214 ELSEIF (isk>1.AND.icoor==0) THEN
215 k1=3*j-2
216 k2=3*j-1
217 k3=3*j
218 dd = skew(k1,isk)*d(1,i) +
219 . skew(k2,isk)*d(2,i) +
220 . skew(k3,isk)*d(3,i)
221 vv = skew(k1,isk)*v(1,i) +
222 . skew(k2,isk)*v(2,i) +
223 . skew(k3,isk)*v(3,i)
224 a0 = skew(k1,isk)*a(1,i) +
225 . skew(k2,isk)*a(2,i) +
226 . skew(k3,isk)*a(3,i)
227 IF(ibfv(7,n)==2) yc(ii)=(yc(ii)-dd)/dt2
228 IF(ibfv(7,n)>=1) yc(ii)=(yc(ii)-vv)/dt12
229 aa = diag_sms(i)*yc(ii) - a0
230 a(1,i)=a(1,i)+skew(k1,isk)*aa
231 a(2,i)=a(2,i)+skew(k2,isk)*aa
232 a(3,i)=a(3,i)+skew(k3,isk)*aa
233c ?
234 dw = fourth*axi*weight(i) *
235 . (yc(ii)*dt12+two*vv)*(ms(i)*yc(ii)-a0)
236 ELSEIF ((isk<=1.AND.ifm<=1.AND.icoor==1) .OR.
237 & (isk>1 .AND. icoor==1)) THEN
238 coef = (skew(10,isk)-x(1,i))*skew(7,isk) +
239 & (skew(11,isk)-x(2,i))*skew(8,isk) +
240 & (skew(12,isk)-x(3,i))*skew(9,isk)
241 hx = coef*skew(7,isk)+x(1,i)-skew(10,isk)
242 hy = coef*skew(8,isk)+x(2,i)-skew(11,isk)
243 hz = coef*skew(9,isk)+x(3,i)-skew(12,isk)
244 r = sqrt(hx*hx+hy*hy+hz*hz)
245 IF(r<=em20) THEN
246 cth = zero
247 sth = zero
248 ELSE
249 cth = (skew(1,isk)*hx+skew(2,isk)*hy+skew(3,isk)*hz)/r
250 sth = (skew(4,isk)*hx+skew(5,isk)*hy+skew(6,isk)*hz)/r
251 ENDIF
252 IF (j==1) THEN
253 skdir(1) =cth*skew(1,isk)+ sth*skew(4,isk)
254 skdir(2) =cth*skew(2,isk)+ sth*skew(5,isk)
255 skdir(3) =cth*skew(3,isk)+ sth*skew(6,isk)
256 ELSEIF(j==2) THEN
257 er(1) =cth*skew(1,isk)+ sth*skew(4,isk)
258 er(2) =cth*skew(2,isk)+ sth*skew(5,isk)
259 er(3) =cth*skew(3,isk)+ sth*skew(6,isk)
260 nor1=sqrt(er(1)*er(1)+er(2)*er(2)+er(3)*er(3))
261 er=er/max(nor1,em20)
262 et(1) =cth*skew(4,isk)- sth*skew(1,isk)
263 et(2) =cth*skew(5,isk)- sth*skew(2,isk)
264 et(3) =cth*skew(6,isk)- sth*skew(3,isk)
265 nor2=sqrt(et(1)*et(1)+et(2)*et(2)+et(3)*et(3))
266 et=et/max(nor2,em20)
267c-----------------vitesse angulaire imposee---------
268 alpha=-yc(ii)*dt2/two
269c-----------------sinon-----------------------------
270c ALPHA=-YC(II)*DT2/(TWO*R)
271 skdir=alpha*er+et
272 ELSEIF(j==3) THEN
273 skdir(1)=skew(7,isk)
274 skdir(2)=skew(8,isk)
275 skdir(3)=skew(9,isk)
276 IF(ibfv(7,n)==2) yimp= skdir(1)*d(1,i) +
277 & skdir(2)*d(2,i) +
278 & skdir(3)*d(3,i)
279 ENDIF
280 nor3 = sqrt(skdir(1)*skdir(1)
281 & +skdir(2)*skdir(2)
282 & +skdir(3)*skdir(3))
283 skdir=skdir/max(nor3,em20)
284 vv = v(1,i)*skdir(1) + v(2,i)*skdir(2) + v(3,i)*skdir(3)
285 a0 = a(1,i)*skdir(1) + a(2,i)*skdir(2) + a(3,i)*skdir(3)
286 IF(ibfv(7,n)==2) yc(ii)=(yc(ii)-yimp)/dt2
287 IF(ibfv(7,n)>=1) THEN
288 IF (j==2) THEN
289c YC(II)=(YC(II)-VV)/DT12
290c-----------------vitesse angulaire imposee--------
291 yc(ii)=(r*yc(ii)-vv)/dt12
292c----------------------------------------------------
293 ELSE
294 yc(ii)=(yc(ii)-vv)/dt12
295 ENDIF
296 ENDIF
297 aa = diag_sms(i)*yc(ii) - a0
298 IF(r<=em20) aa=zero
299 a(1,i)=a(1,i)+skdir(1)*aa
300 a(2,i)=a(2,i)+skdir(2)*aa
301 a(3,i)=a(3,i)+skdir(3)*aa
302 dw = fourth*axi*weight(i) *
303 . (yc(ii)*dt12+two*vv)*(ms(i)*yc(ii)-a0)
304 ELSEIF (ifm>1.AND.icoor==0) THEN
305C Fixed velocity in moving frame (transl)
306 k1=3*j-2
307 k2=3*j-1
308 k3=3*j
309 rx = x(1,i) - xframe(10,ifm)
310 ry = x(2,i) - xframe(11,ifm)
311 rz = x(3,i) - xframe(12,ifm)
312 vfx = xframe(31,ifm)+xframe(14,ifm)*rz-xframe(15,ifm)*ry
313 vfy = xframe(32,ifm)+xframe(15,ifm)*rx-xframe(13,ifm)*rz
314 vfz = xframe(33,ifm)+xframe(13,ifm)*ry-xframe(14,ifm)*rx
315 vv = xframe(k1,ifm)*v(1,i)
316 . + xframe(k2,ifm)*v(2,i)
317 . + xframe(k3,ifm)*v(3,i)
318 vf = xframe(k1,ifm)*vfx
319 . + xframe(k2,ifm)*vfy
320 . + xframe(k3,ifm)*vfz
321 a0 = xframe(k1,ifm)*a(1,i)
322 . + xframe(k2,ifm)*a(2,i)
323 . + xframe(k3,ifm)*a(3,i)
324 yc(ii)=(yc(ii)-vv+vf)/dt12
325 aa = diag_sms(i)*yc(ii) - a0
326 a(1,i)=a(1,i)+xframe(k1,ifm)*aa
327 a(2,i)=a(2,i)+xframe(k2,ifm)*aa
328 a(3,i)=a(3,i)+xframe(k3,ifm)*aa
329c ?
330 dw = fourth*axi*weight(i) *
331 . (yc(ii)*dt12+two*vv)*(ms(i)*yc(ii)-a0)
332 ELSEIF (ifm>1.AND.icoor==1) THEN
333 coef = (xframe(10,ifm)-x(1,i))*xframe(7,ifm) +
334 & (xframe(11,ifm)-x(2,i))*xframe(8,ifm) +
335 & (xframe(12,ifm)-x(3,i))*xframe(9,ifm)
336 hx = coef*xframe(7,ifm)+x(1,i)-xframe(10,ifm)
337 hy = coef*xframe(8,ifm)+x(2,i)-xframe(11,ifm)
338 hz = coef*xframe(9,ifm)+x(3,i)-xframe(12,ifm)
339 r = sqrt(hx*hx+hy*hy+hz*hz)
340 IF(r<=em20) THEN
341 cth = zero
342 sth = zero
343 ELSE
344 cth = (xframe(1,ifm)*hx +
345 & xframe(2,ifm)*hy +
346 & xframe(3,ifm)*hz)/r
347 sth = (xframe(4,ifm)*hx +
348 & xframe(5,ifm)*hy +
349 & xframe(6,ifm)*hz)/r
350 ENDIF
351 IF (j==1) THEN
352 fmdir(1) =cth*xframe(1,ifm)+ sth*xframe(4,ifm)
353 fmdir(2) =cth*xframe(2,ifm)+ sth*xframe(5,ifm)
354 fmdir(3) =cth*xframe(3,ifm)+ sth*xframe(6,ifm)
355 ELSEIF (j==2) THEN
356 er(1) =cth*xframe(1,ifm)+ sth*xframe(4,ifm)
357 er(2) =cth*xframe(2,ifm)+ sth*xframe(5,ifm)
358 er(3) =cth*xframe(3,ifm)+ sth*xframe(6,ifm)
359 nor1=sqrt(er(1)*er(1)+er(2)*er(2)+er(3)*er(3))
360 er=er/max(nor1,em20)
361 et(1) =cth*xframe(4,ifm)- sth*xframe(1,ifm)
362 et(2) =cth*xframe(5,ifm)- sth*xframe(2,ifm)
363 et(3) =cth*xframe(6,ifm)- sth*xframe(3,ifm)
364 nor2=sqrt(et(1)*et(1)+et(2)*et(2)+et(3)*et(3))
365 et=et/max(nor2,em20)
366c---------------vitesse angulaire imposee--------
367 alpha=-yc(ii)*dt2/two
368c-----------------------------------------------
369c ALPHA=-YC(II)*DT2/(TWO*R)
370 fmdir=alpha*er+et
371 ELSEIF (j==3) THEN
372 fmdir(1)=xframe(7,ifm)
373 fmdir(2)=xframe(8,ifm)
374 fmdir(3)=xframe(9,ifm)
375 ENDIF
376 nor3 = sqrt(fmdir(1)*fmdir(1)
377 & +fmdir(2)*fmdir(2)
378 & +fmdir(3)*fmdir(3))
379 fmdir=fmdir/max(nor3,em20)
380 rx = x(1,i) - xframe(10,ifm)
381 ry = x(2,i) - xframe(11,ifm)
382 rz = x(3,i) - xframe(12,ifm)
383 vfx = xframe(31,ifm)+xframe(14,ifm)*rz-xframe(15,ifm)*ry
384 vfy = xframe(32,ifm)+xframe(15,ifm)*rx-xframe(13,ifm)*rz
385 vfz = xframe(33,ifm)+xframe(13,ifm)*ry-xframe(14,ifm)*rx
386 vv = fmdir(1)*v(1,i) + fmdir(2)*v(2,i) + fmdir(3)*v(3,i)
387 vf = fmdir(1)*vfx + fmdir(2)*vfy + fmdir(3)*vfz
388 a0 = fmdir(1)*a(1,i) + fmdir(2)*a(2,i) + fmdir(3)*a(3,i)
389 IF (j==2) THEN
390c YC(II)=(YC(II)-VV+VF)/DT12
391c---------------vitesse angulaire imposee--------
392 yc(ii)=(r*yc(ii)-vv+vf)/dt12
393c-------------------------------------------------
394 ELSE
395 yc(ii)=(yc(ii)-vv+vf)/dt12
396 ENDIF
397 aa = diag_sms(i)*yc(ii) - a0
398 IF(r<=em20) aa=zero
399 a(1,i)=a(1,i)+fmdir(1)*aa
400 a(2,i)=a(2,i)+fmdir(2)*aa
401 a(3,i)=a(3,i)+fmdir(3)*aa
402 dw = fourth*axi*weight(i) *
403 . (yc(ii)*dt12+two*vv)*(ms(i)*yc(ii)-a0)
404 ENDIF
405 ELSEIF(j<=6)THEN
406 j = j - 3
407 IF(isk<=1.AND.ifm<=1.AND.icoor==0)THEN
408 IF(ibfv(7,n)==2) yc(ii)=(yc(ii)-dr(j,i))/dt2
409 IF(ibfv(7,n)>=1) yc(ii)=(yc(ii)-vr(j,i))/dt12
410 aold = ar(j,i)
411 ar(j,i)= in(i)*yc(ii)
412 IF(ibfv(6,n)==0)THEN
413 dw=fourth *
414 . (yc(ii)*dt12 + two*vr(j,i))*(ar(j,i)-aold) *
415 . weight(i)
416 ELSE
417 nr = ibfv(6,n)
418 dw= fourth *
419 . (yc(ii)*dt12 + two*vr(j,i))*weight(i)*(ar(j,i)-aold)*
420 . (rby(16+j,nr) + rby(19+j,nr) + rby(22+j,nr))/max(in(i),em30)
421 ENDIF
422 ELSEIF (isk>1.AND.icoor==0) THEN
423 k1=3*j-2
424 k2=3*j-1
425 k3=3*j
426 IF(ibfv(7,n)==2) THEN
427 dd = skew(k1,isk)*dr(1,i) +
428 . skew(k2,isk)*dr(2,i) +
429 . skew(k3,isk)*dr(3,i)
430 END IF
431 vv = skew(k1,isk)*vr(1,i) +
432 . skew(k2,isk)*vr(2,i) +
433 . skew(k3,isk)*vr(3,i)
434 a0 = skew(k1,isk)*ar(1,i) +
435 . skew(k2,isk)*ar(2,i) +
436 . skew(k3,isk)*ar(3,i)
437 IF(ibfv(7,n)==2) yc(ii)=(yc(ii)-dd)/dt2
438 IF(ibfv(7,n)>=1) yc(ii)=(yc(ii)-vv)/dt12
439 aa = in(i)*yc(ii) - a0
440 ar(1,i)=ar(1,i)+skew(k1,isk)*aa
441 ar(2,i)=ar(2,i)+skew(k2,isk)*aa
442 ar(3,i)=ar(3,i)+skew(k3,isk)*aa
443 IF(ibfv(6,n)==0)THEN
444 dw= fourth*(yc(ii)*dt12+two*vv)*aa*weight(i)
445 ELSE
446 nr = ibfv(6,n)
447 dw= fourth*(yc(ii)*dt12+two*vv)*weight(i)*aa*
448 . ((rby(17,nr)*skew(k1,isk)
449 . +rby(18,nr)*skew(k2,isk)
450 . +rby(19,nr)*skew(k3,isk))*skew(k1,isk) +
451 . (rby(20,nr)*skew(k1,isk)
452 . +rby(21,nr)*skew(k2,isk)
453 . +rby(22,nr)*skew(k3,isk))*skew(k2,isk) +
454 . (rby(23,nr)*skew(k1,isk)
455 . +rby(24,nr)*skew(k2,isk)
456 . +rby(25,nr)*skew(k3,isk))*skew(k3,isk))
457 . /max(in(i),em30)
458 ENDIF
459 ELSEIF ((isk<=1.AND.ifm<=1.AND.icoor==1) .OR.
460 & (isk>1.AND.icoor==1)) THEN
461 coef = (skew(10,isk)-x(1,i))*skew(7,isk) +
462 & (skew(11,isk)-x(2,i))*skew(8,isk) +
463 & (skew(12,isk)-x(3,i))*skew(9,isk)
464 hx = coef*skew(7,isk)+x(1,i)-skew(10,isk)
465 hy = coef*skew(8,isk)+x(2,i)-skew(11,isk)
466 hz = coef*skew(9,isk)+x(3,i)-skew(12,isk)
467 r = sqrt(hx*hx+hy*hy+hz*hz)
468 IF(r<=em20) THEN
469 cth = zero
470 sth = zero
471 ELSE
472 cth = (skew(1,isk)*hx+skew(2,isk)*hy+skew(3,isk)*hz)/r
473 sth = (skew(4,isk)*hx+skew(5,isk)*hy+skew(6,isk)*hz)/r
474 ENDIF
475 IF (j==1) THEN
476 skdir(1) =cth*skew(1,isk)+ sth*skew(4,isk)
477 skdir(2) =cth*skew(2,isk)+ sth*skew(5,isk)
478 skdir(3) =cth*skew(3,isk)+ sth*skew(6,isk)
479 ELSEIF(j==2) THEN
480 skdir(1) =cth*skew(4,isk)- sth*skew(1,isk)
481 skdir(2) =cth*skew(5,isk)- sth*skew(2,isk)
482 skdir(3) =cth*skew(6,isk)- sth*skew(3,isk)
483 ELSEIF(j==3) THEN
484 skdir(1)=skew(7,isk)
485 skdir(2)=skew(8,isk)
486 skdir(3)=skew(9,isk)
487 IF(ibfv(7,n)==2) yimp= skdir(1)*dr(1,i) +
488 & skdir(2)*dr(2,i) +
489 & skdir(3)*dr(3,i)
490 ENDIF
491 nor3 = sqrt(skdir(1)*skdir(1)
492 & +skdir(2)*skdir(2)
493 & +skdir(3)*skdir(3))
494 skdir=skdir/max(nor3,em20)
495 IF(ibfv(7,n)==2) yc(ii)=(yc(ii)-yimp)/dt2
496 vv =vr(1,i)*skdir(1)+ vr(2,i)*skdir(2)+ vr(3,i)*skdir(3)
497 a0 =ar(1,i)*skdir(1)+ ar(2,i)*skdir(2)+ ar(3,i)*skdir(3)
498 IF(ibfv(7,n)>=1) yc(ii)=(yc(ii)-vv)/dt12
499 aa = in(i)*yc(ii) - a0
500 IF(r<=em20) aa=zero
501 ar(1,i)=ar(1,i)+skdir(1)*aa
502 ar(2,i)=ar(2,i)+skdir(2)*aa
503 ar(3,i)=ar(3,i)+skdir(3)*aa
504 IF(ibfv(6,n)==0)THEN
505 dw= fourth*(yc(ii)*dt12+two*vv)*aa*weight(i)
506 ELSE
507 nr = ibfv(6,n)
508 dw= fourth*(yc(ii)*dt12+two*vv)*weight(i)*aa*
509 & ((rby(17,nr)*skdir(1)
510 & +rby(18,nr)*skdir(2)
511 & +rby(19,nr)*skdir(3))*skdir(1) +
512 & (rby(20,nr)*skdir(1)
513 & +rby(21,nr)*skdir(2)
514 & +rby(22,nr)*skdir(3))*skdir(2) +
515 & (rby(23,nr)*skdir(1)
516 & +rby(24,nr)*skdir(2)
517 & +rby(25,nr)*skdir(3))*skdir(3))
518 . /max(in(i),em30)
519 ENDIF
520 ELSEIF (ifm>1.AND.icoor==0) THEN
521C Fixed velocity in moving frame (rota)
522 k1=3*j-2
523 k2=3*j-1
524 k3=3*j
525 vv = xframe(k1,ifm)*vr(1,i)
526 . + xframe(k2,ifm)*vr(2,i)
527 . + xframe(k3,ifm)*vr(3,i)
528 vf = xframe(k1,ifm)*xframe(13,ifm)
529 . + xframe(k2,ifm)*xframe(14,ifm)
530 . + xframe(k3,ifm)*xframe(15,ifm)
531 a0 = xframe(k1,ifm)*ar(1,i)
532 . + xframe(k2,ifm)*ar(2,i)
533 . + xframe(k3,ifm)*ar(3,i)
534 aa = in(i)*(yc(ii)-vv+vf)/dt12 - a0
535 ar(1,i)=ar(1,i)+xframe(k1,ifm)*aa
536 ar(2,i)=ar(2,i)+xframe(k2,ifm)*aa
537 ar(3,i)=ar(3,i)+xframe(k3,ifm)*aa
538 IF(ibfv(6,n)==0)THEN
539 dw= fourth*(yc(ii)+vv)*aa*weight(i)
540 ELSE
541 nr = ibfv(6,n)
542 dw= fourth*(yc(ii)+vv)*weight(i)*aa*
543 . ((rby(17,nr)*xframe(k1,ifm)
544 . +rby(18,nr)*xframe(k2,ifm)
545 . +rby(19,nr)*xframe(k3,ifm))*xframe(k1,ifm) +
546 . (rby(20,nr)*xframe(k1,ifm)
547 . +rby(21,nr)*xframe(k2,ifm)
548 . +rby(22,nr)*xframe(k3,ifm))*xframe(k2,ifm) +
549 . (rby(23,nr)*xframe(k1,ifm)
550 . +rby(24,nr)*xframe(k2,ifm)
551 . +rby(25,nr)*xframe(k3,ifm))*xframe(k3,ifm))
552 . /max(in(i),em30)
553 ENDIF
554 ELSEIF (ifm>1.AND.icoor==1) THEN
555 coef = (xframe(10,ifm)-x(1,i))*xframe(7,ifm) +
556 & (xframe(11,ifm)-x(2,i))*xframe(8,ifm) +
557 & (xframe(12,ifm)-x(3,i))*xframe(9,ifm)
558 hx = coef*xframe(7,ifm)+x(1,i)-xframe(10,ifm)
559 hy = coef*xframe(8,ifm)+x(2,i)-xframe(11,ifm)
560 hz = coef*xframe(9,ifm)+x(3,i)-xframe(12,ifm)
561 r = sqrt(hx*hx+hy*hy+hz*hz)
562 IF(r<=em20) THEN
563 cth = zero
564 sth = zero
565 ELSE
566 cth = (xframe(1,ifm)*hx +
567 & xframe(2,ifm)*hy +
568 & xframe(3,ifm)*hz)/r
569 sth = (xframe(4,ifm)*hx +
570 & xframe(5,ifm)*hy +
571 & xframe(6,ifm)*hz)/r
572 ENDIF
573 IF (j==1) THEN
574 fmdir(1) =cth*xframe(1,ifm)+ sth*xframe(4,ifm)
575 fmdir(2) =cth*xframe(2,ifm)+ sth*xframe(5,ifm)
576 fmdir(3) =cth*xframe(3,ifm)+ sth*xframe(6,ifm)
577 ELSEIF (j==2) THEN
578 fmdir(1) =cth*xframe(4,ifm)- sth*xframe(1,ifm)
579 fmdir(2) =cth*xframe(5,ifm)- sth*xframe(2,ifm)
580 fmdir(3) =cth*xframe(6,ifm)- sth*xframe(3,ifm)
581 ELSEIF (j==3) THEN
582 fmdir(1)=xframe(7,ifm)
583 fmdir(2)=xframe(8,ifm)
584 fmdir(3)=xframe(9,ifm)
585 ENDIF
586 nor3 = sqrt(fmdir(1)*fmdir(1)
587 & +fmdir(2)*fmdir(2)
588 & +fmdir(3)*fmdir(3))
589 fmdir=fmdir/max(nor3,em20)
590 vv = fmdir(1)*vr(1,i) +
591 & fmdir(2)*vr(2,i) +
592 & fmdir(3)*vr(3,i)
593 vf = fmdir(1)*xframe(13,ifm) +
594 & fmdir(2)*xframe(14,ifm) +
595 & fmdir(3)*xframe(15,ifm)
596 a0 = fmdir(1)*ar(1,i) +
597 & fmdir(2)*ar(2,i) +
598 & fmdir(3)*ar(3,i)
599 aa = in(i)*(yc(ii)-vv+vf)/dt12 - a0
600 IF(r<=em20) aa=zero
601 ar(1,i)=ar(1,i)+fmdir(1)*aa
602 ar(2,i)=ar(2,i)+fmdir(2)*aa
603 ar(3,i)=ar(3,i)+fmdir(3)*aa
604 IF(ibfv(6,n)==0)THEN
605 dw= fourth*(yc(ii)+vv)*aa*weight(i)
606 ELSE
607 nr = ibfv(6,n)
608 dw= fourth*(yc(ii)+vv)*weight(i)*aa*
609 & ((rby(17,nr)*fmdir(1)
610 & +rby(18,nr)*fmdir(2)
611 & +rby(19,nr)*fmdir(3))*fmdir(1) +
612 & (rby(20,nr)*fmdir(1)
613 & +rby(21,nr)*fmdir(2)
614 & +rby(22,nr)*fmdir(3))*fmdir(2) +
615 & (rby(23,nr)*fmdir(1)
616 & +rby(24,nr)*fmdir(2)
617 & +rby(25,nr)*fmdir(3))*fmdir(3))
618 . /max(in(i),em30)
619 ENDIF
620 ENDIF
621 ENDIF
622 wfext = wfext + dt1*dw
623 vel(4,n) = dt2*dw
624 ENDDO
625 ELSE
626C partie vectorielle
627 nsk=0
628 nfk=0
629 DO ii=1,ic
630 n = index(ii)
631 ifm = ibfv(9,n)
632 isk = ibfv(2,n)/10
633 icoor=ibfv(10,n)
634 IF (ifm>1) THEN
635 nfk = 1
636 ELSEIF (isk>1) THEN
637 nsk=1
638 ENDIF
639 ENDDO
640
641 IF(nsk==0.AND.nfk==0.AND.icoor==0)THEN
642#include "vectorize.inc"
643 DO ii=1,ic
644 n = index(ii)
645 i=iabs(ibfv(1,n))
646 ibfv(5,n) = iposc(ii)
647 fac = vel(1,n)
648 dw = vel(4,n)
649 wfext=wfext + dw
650 yc(ii) = yc(ii) * fac
651 isk=ibfv(2,n)/10
652 j=ibfv(2,n)-10*isk
653 IF(j<=3)THEN
654 axi=one-ax+ax*x(2,i)
655 IF(ibfv(7,n)==2) yc(ii)=(yc(ii)-d(j,i))/dt2
656 IF(ibfv(7,n)>=1) yc(ii)=(yc(ii)-v(j,i))/dt12
657 aold=a(j,i)
658 a(j,i)=diag_sms(i)*yc(ii)
659 dw = fourth*axi*weight(i) *
660 . (yc(ii)*dt12 + two*v(j,i))*(ms(i)*yc(ii)-aold)
661 ELSEIF(j<=6)THEN
662 j = j - 3
663 IF(ibfv(7,n)==2) yc(ii)=(yc(ii)-dr(j,i))/dt2
664 IF(ibfv(7,n)>=1) yc(ii)=(yc(ii)-vr(j,i))/dt12
665 aold=ar(j,i)
666 ar(j,i)= in(i)*yc(ii)
667 IF(ibfv(6,n)==0)THEN
668 dw=fourth *
669 . (ar(j,i)*dt12+two*vr(j,i))*(ar(j,i)-aold) *
670 . weight(i)
671 ELSE
672 nr = ibfv(6,n)
673 dw= fourth *
674 . (ar(j,i)*dt12+two*vr(j,i))*weight(i)*(ar(j,i)-aold)*
675 . (rby(16+j,nr) + rby(19+j,nr) + rby(22+j,nr))
676 . /max(in(i),em30)
677 ENDIF
678 ENDIF
679 wfext=wfext + dt1*dw
680 vel(4,n) = dt2*dw
681 ENDDO
682 ELSEIF (nsk==1.AND.nfk==0.AND.icoor==0) THEN
683 DO k=1,3
684#include "vectorize.inc"
685 DO ii=1,ic
686 n = index(ii)
687 i=iabs(ibfv(1,n))
688 isk=ibfv(2,n)/10
689 j=ibfv(2,n)-10*isk
690 IF(j==k)THEN
691 ibfv(5,n) = iposc(ii)
692 fac = vel(1,n)
693 dw = vel(4,n)
694 wfext=wfext + dw
695 yc(ii) = yc(ii) * fac
696 axi=one-ax+ax*x(2,i)
697 k1=3*j-2
698 k2=3*j-1
699 k3=3*j
700 dd = skew(k1,isk)*d(1,i) +
701 . skew(k2,isk)*d(2,i) +
702 . skew(k3,isk)*d(3,i)
703 vv = skew(k1,isk)*v(1,i) +
704 . skew(k2,isk)*v(2,i) +
705 . skew(k3,isk)*v(3,i)
706 a0 = skew(k1,isk)*a(1,i) +
707 . skew(k2,isk)*a(2,i) +
708 . skew(k3,isk)*a(3,i)
709 IF(ibfv(7,n)==2) yc(ii)=(yc(ii)-dd)/dt2
710 IF(ibfv(7,n)>=1) yc(ii)=(yc(ii)-vv)/dt12
711 aa = diag_sms(i)*yc(ii) - a0
712 a(1,i)=a(1,i)+skew(k1,isk)*aa
713 a(2,i)=a(2,i)+skew(k2,isk)*aa
714 a(3,i)=a(3,i)+skew(k3,isk)*aa
715c ?
716 dw = fourth*axi*weight(i) *
717 . (yc(ii)*dt12+two*vv)*(ms(i)*yc(ii)-a0)
718 wfext=wfext + dt1*dw
719 vel(4,n) = dt2*dw
720 ENDIF
721 ENDDO
722 ENDDO
723 IF (iroddl>=0)THEN
724 DO k=4,3+3*iroddl
725#include "vectorize.inc"
726 DO ii=1,ic
727 n = index(ii)
728 isk=ibfv(2,n)/10
729 j=ibfv(2,n)-10*isk
730 IF(j==k)THEN
731 ibfv(5,n) = iposc(ii)
732 fac = vel(1,n)
733 dw = vel(4,n)
734 wfext=wfext + dw
735 yc(ii) = yc(ii) * fac
736 i=iabs(ibfv(1,n))
737 j = j - 3
738 k1=3*j-2
739 k2=3*j-1
740 k3=3*j
741 IF(ibfv(7,n)==2) THEN
742 dd = skew(k1,isk)*dr(1,i) +
743 . skew(k2,isk)*dr(2,i) +
744 . skew(k3,isk)*dr(3,i)
745 END IF
746 vv = skew(k1,isk)*vr(1,i) +
747 . skew(k2,isk)*vr(2,i) +
748 . skew(k3,isk)*vr(3,i)
749 a0 = skew(k1,isk)*ar(1,i) +
750 . skew(k2,isk)*ar(2,i) +
751 . skew(k3,isk)*ar(3,i)
752 IF(ibfv(7,n)==2) yc(ii)=(yc(ii)-dd)/dt2
753 IF(ibfv(7,n)>=1) yc(ii)=(yc(ii)-vv)/dt12
754 aa = in(i)*yc(ii) - a0
755 ar(1,i)=ar(1,i)+skew(k1,isk)*aa
756 ar(2,i)=ar(2,i)+skew(k2,isk)*aa
757 ar(3,i)=ar(3,i)+skew(k3,isk)*aa
758 IF(ibfv(6,n)==0)THEN
759 dw= fourth*(yc(ii)*dt12+two*vv)*aa*weight(i)
760 ELSE
761 nr = ibfv(6,n)
762 dw= fourth*(yc(ii)*dt12+two*vv)*weight(i)*aa*
763 . ((rby(17,nr)*skew(k1,isk)
764 . +rby(18,nr)*skew(k2,isk)
765 . +rby(19,nr)*skew(k3,isk))*skew(k1,isk) +
766 . (rby(20,nr)*skew(k1,isk)
767 . +rby(21,nr)*skew(k2,isk)
768 . +rby(22,nr)*skew(k3,isk))*skew(k2,isk) +
769 . (rby(23,nr)*skew(k1,isk)
770 . +rby(24,nr)*skew(k2,isk)
771 . +rby(25,nr)*skew(k3,isk))*skew(k3,isk))
772 . /max(in(i),em30)
773 ENDIF
774 wfext=wfext + dt1*dw
775 vel(4,n) = dt2*dw
776 ENDIF
777 ENDDO
778 ENDDO
779 ENDIF
780 ELSEIF ((nsk==0.AND.nfk==0.AND.icoor==1).OR.
781 & (nsk==1.AND.nfk==0.AND.icoor==1)) THEN
782 DO k=1,3
783#include "vectorize.inc"
784 DO ii=1,ic
785 n = index(ii)
786 i=iabs(ibfv(1,n))
787 isk=ibfv(2,n)/10
788 j=ibfv(2,n)-10*isk
789 skdir=zero
790 r=one
791 IF(j==k)THEN
792 ibfv(5,n) = iposc(ii)
793 fac = vel(1,n)
794 yimp = vel(6,n)
795c VEL(6,N) = YC(II)
796 dw = vel(4,n)
797 wfext=wfext + dw
798 yc(ii) = yc(ii) * fac
799 yimp = yimp * fac
800 axi=one-ax+ax*x(2,i)
801 coef = (skew(10,isk)-x(1,i))*skew(7,isk) +
802 & (skew(11,isk)-x(2,i))*skew(8,isk) +
803 & (skew(12,isk)-x(3,i))*skew(9,isk)
804 hx = coef*skew(7,isk)+x(1,i)-skew(10,isk)
805 hy = coef*skew(8,isk)+x(2,i)-skew(11,isk)
806 hz = coef*skew(9,isk)+x(3,i)-skew(12,isk)
807 r = sqrt(hx*hx+hy*hy+hz*hz)
808 IF(r<=em20) THEN
809 cth = zero
810 sth = zero
811 ELSE
812 cth = (skew(1,isk)*hx +
813 & skew(2,isk)*hy +
814 & skew(3,isk)*hz)/r
815 sth = (skew(4,isk)*hx +
816 & skew(5,isk)*hy +
817 & skew(6,isk)*hz)/r
818 ENDIF
819 IF (j==1) THEN
820 skdir(1) =cth*skew(1,isk)+ sth*skew(4,isk)
821 skdir(2) =cth*skew(2,isk)+ sth*skew(5,isk)
822 skdir(3) =cth*skew(3,isk)+ sth*skew(6,isk)
823 ELSEIF(j==2) THEN
824 er(1) =cth*skew(1,isk)+ sth*skew(4,isk)
825 er(2) =cth*skew(2,isk)+ sth*skew(5,isk)
826 er(3) =cth*skew(3,isk)+ sth*skew(6,isk)
827 nor1=sqrt(er(1)*er(1)+er(2)*er(2)+er(3)*er(3))
828 er=er/max(nor1,em20)
829 et(1) =cth*skew(4,isk)- sth*skew(1,isk)
830 et(2) =cth*skew(5,isk)- sth*skew(2,isk)
831 et(3) =cth*skew(6,isk)- sth*skew(3,isk)
832 nor2=sqrt(et(1)*et(1)+et(2)*et(2)+et(3)*et(3))
833 et=et/max(nor2,em20)
834c-----------------vitesse angulaire imposee---------
835 alpha=-yc(ii)*dt2/two
836c-----------------sinon-----------------------------
837c ALPHA=-YC(II)*DT2/(TWO*R)
838 skdir=alpha*er+et
839 ELSEIF(j==3) THEN
840 skdir(1)=skew(7,isk)
841 skdir(2)=skew(8,isk)
842 skdir(3)=skew(9,isk)
843 IF(ibfv(7,n)==2) yimp= skdir(1)*d(1,i) +
844 & skdir(2)*d(2,i) +
845 & skdir(3)*d(3,i)
846 ENDIF
847 nor3 = sqrt(skdir(1)*skdir(1)
848 & +skdir(2)*skdir(2)
849 & +skdir(3)*skdir(3))
850 skdir=skdir/max(nor3,em20)
851 vv = v(1,i)*skdir(1) +
852 & v(2,i)*skdir(2) +
853 & v(3,i)*skdir(3)
854 a0 = a(1,i)*skdir(1) +
855 & a(2,i)*skdir(2) +
856 & a(3,i)*skdir(3)
857 IF(ibfv(7,n)==2) yc(ii)=(yc(ii)-yimp)/dt2
858 IF(ibfv(7,n)>=1) THEN
859 IF (j==2) THEN
860c YC(II)=(YC(II)-VV)/DT12
861c------------------vitesse angulaire imposee--------
862 yc(ii)=(r*yc(ii)-vv)/dt12
863c----------------------------------------------------
864 ELSE
865 yc(ii)=(yc(ii)-vv)/dt12
866 ENDIF
867 ENDIF
868 aa = diag_sms(i)*yc(ii) - a0
869 IF(r<=em20) aa=zero
870 a(1,i)=a(1,i)+skdir(1)*aa
871 a(2,i)=a(2,i)+skdir(2)*aa
872 a(3,i)=a(3,i)+skdir(3)*aa
873 dw = fourth*axi*weight(i) *
874 . (yc(ii)*dt12+two*vv)*(ms(i)*yc(ii)-a0)
875 wfext=wfext + dt1*dw
876 vel(4,n) = dt2*dw
877 ENDIF
878 ENDDO
879 ENDDO
880 IF (iroddl>=0)THEN
881 DO k=4,3+3*iroddl
882#include "vectorize.inc"
883 DO ii=1,ic
884 n = index(ii)
885 isk=ibfv(2,n)/10
886 j=ibfv(2,n)-10*isk
887 skdir=zero
888 r=one
889 IF(j==k)THEN
890 ibfv(5,n) = iposc(ii)
891 fac = vel(1,n)
892 yimp = vel(6,n)
893 vel(6,n) = yc(ii)
894 dw = vel(4,n)
895 wfext=wfext + dw
896 yc(ii) = yc(ii) * fac
897 yimp = yimp * fac
898 i=iabs(ibfv(1,n))
899 j = j - 3
900 coef = (skew(10,isk)-x(1,i))*skew(7,isk) +
901 & (skew(11,isk)-x(2,i))*skew(8,isk) +
902 & (skew(12,isk)-x(3,i))*skew(9,isk)
903 hx = coef*skew(7,isk)+x(1,i)-skew(10,isk)
904 hy = coef*skew(8,isk)+x(2,i)-skew(11,isk)
905 hz = coef*skew(9,isk)+x(3,i)-skew(12,isk)
906 r = sqrt(hx*hx+hy*hy+hz*hz)
907 IF(r<=em20) THEN
908 cth = zero
909 sth = zero
910 ELSE
911 cth = (skew(1,isk)*hx +
912 & skew(2,isk)*hy +
913 & skew(3,isk)*hz)/r
914 sth = (skew(4,isk)*hx +
915 & skew(5,isk)*hy +
916 & skew(6,isk)*hz)/r
917 ENDIF
918 IF (j==1) THEN
919 skdir(1) =cth*skew(1,isk)+ sth*skew(4,isk)
920 skdir(2) =cth*skew(2,isk)+ sth*skew(5,isk)
921 skdir(3) =cth*skew(3,isk)+ sth*skew(6,isk)
922 ELSEIF(j==2) THEN
923 skdir(1) =cth*skew(4,isk)- sth*skew(1,isk)
924 skdir(2) =cth*skew(5,isk)- sth*skew(2,isk)
925 skdir(3) =cth*skew(6,isk)- sth*skew(3,isk)
926 ELSEIF(j==3) THEN
927 skdir(1)=skew(7,isk)
928 skdir(2)=skew(8,isk)
929 skdir(3)=skew(9,isk)
930 IF(ibfv(7,n)==2) yimp= skdir(1)*dr(1,i) +
931 & skdir(2)*dr(2,i) +
932 & skdir(3)*dr(3,i)
933 ENDIF
934 nor3 = sqrt(skdir(1)*skdir(1)
935 & +skdir(2)*skdir(2)
936 & +skdir(3)*skdir(3))
937 skdir=skdir/max(nor3,em20)
938 IF(ibfv(7,n)==2) yc(ii)=(yc(ii)-yimp)/dt2
939 vv = vr(1,i)*skdir(1) +
940 & vr(2,i)*skdir(2) +
941 & vr(3,i)*skdir(3)
942 a0 = ar(1,i)*skdir(1) +
943 & ar(2,i)*skdir(2) +
944 & ar(3,i)*skdir(3)
945 IF(ibfv(7,n)>=1) yc(ii)=(yc(ii)-vv)/dt12
946 aa = in(i)*yc(ii) - a0
947 IF(r<=em20) aa=zero
948 ar(1,i)=ar(1,i)+skdir(1)*aa
949 ar(2,i)=ar(2,i)+skdir(2)*aa
950 ar(3,i)=ar(3,i)+skdir(3)*aa
951 IF(ibfv(6,n)==0)THEN
952 dw= fourth*(yc(ii)*dt12+two*vv)*aa*weight(i)
953 ELSE
954 nr = ibfv(6,n)
955 dw= fourth*(yc(ii)*dt12+two*vv)*weight(i)*aa*
956 & ((rby(17,nr)*skdir(1)
957 & +rby(18,nr)*skdir(2)
958 & +rby(19,nr)*skdir(3))*skdir(1) +
959 & (rby(20,nr)*skdir(1)
960 & +rby(21,nr)*skdir(2)
961 & +rby(22,nr)*skdir(3))*skdir(2) +
962 & (rby(23,nr)*skdir(1)
963 & +rby(24,nr)*skdir(2)
964 & +rby(25,nr)*skdir(3))*skdir(3))
965 . /max(in(i),em30)
966 ENDIF
967 wfext=wfext + dt1*dw
968 vel(4,n) = dt2*dw
969 ENDIF
970 ENDDO
971 ENDDO
972 ENDIF
973 ELSEIF (nfk==1.AND.icoor==0) THEN
974 DO k=1,3
975#include "vectorize.inc"
976 DO ii=1,ic
977 n = index(ii)
978 ifm = ibfv(9,n)
979 i=iabs(ibfv(1,n))
980 IF (ifm>1) THEN
981 j = ibfv(2,n)
982 IF(j==k)THEN
983 ibfv(5,n) = iposc(ii)
984 fac = vel(1,n)
985 dw = vel(4,n)
986 wfext=wfext + dw
987 yc(ii) = yc(ii) * fac
988 axi=1.-ax+ax*x(2,i)
989 k1=3*j-2
990 k2=3*j-1
991 k3=3*j
992 rx = x(1,i) - xframe(10,ifm)
993 ry = x(2,i) - xframe(11,ifm)
994 rz = x(3,i) - xframe(12,ifm)
995 vfx = vfx + xframe(31,ifm)
996 vfy = vfy + xframe(32,ifm)
997 vfz = vfz + xframe(33,ifm)
998 vfx = xframe(14,ifm)*rz-xframe(15,ifm)*ry
999 vfy = xframe(15,ifm)*rx-xframe(13,ifm)*rz
1000 vfz = xframe(13,ifm)*ry-xframe(14,ifm)*rx
1001 vv = xframe(k1,ifm)*v(1,i)
1002 . + xframe(k2,ifm)*v(2,i)
1003 . + xframe(k3,ifm)*v(3,i)
1004 vf = xframe(k1,ifm)*vfx
1005 . + xframe(k2,ifm)*vfy
1006 . + xframe(k3,ifm)*vfz
1007 a0 = xframe(k1,ifm)*a(1,i)
1008 . + xframe(k2,ifm)*a(2,i)
1009 . + xframe(k3,ifm)*a(3,i)
1010 yc(ii)=(yc(ii)-vv+vf)/dt12
1011 aa = diag_sms(i)*yc(ii) - a0
1012 a(1,i)=a(1,i)+xframe(k1,ifm)*aa
1013 a(2,i)=a(2,i)+xframe(k2,ifm)*aa
1014 a(3,i)=a(3,i)+xframe(k3,ifm)*aa
1015c ?
1016 dw = fourth*axi*weight(i) *
1017 . (yc(ii)*dt12+two*vv)*(ms(i)*yc(ii)-a0)
1018 wfext=wfext + dt1*dw
1019 vel(4,n) = dt2*dw
1020 ENDIF
1021 ELSE
1022 isk=ibfv(2,n)/10
1023 j=ibfv(2,n)-10*isk
1024 IF(j==k)THEN
1025 ibfv(5,n) = iposc(ii)
1026 fac = vel(1,n)
1027 dw = vel(4,n)
1028 wfext=wfext + dw
1029 yc(ii) = yc(ii) * fac
1030 i=iabs(ibfv(1,n))
1031 axi=1.-ax+ax*x(2,i)
1032 k1=3*j-2
1033 k2=3*j-1
1034 k3=3*j
1035 dd = skew(k1,isk)*d(1,i) +
1036 . skew(k2,isk)*d(2,i) +
1037 . skew(k3,isk)*d(3,i)
1038 vv = skew(k1,isk)*v(1,i) +
1039 . skew(k2,isk)*v(2,i) +
1040 . skew(k3,isk)*v(3,i)
1041 a0 = skew(k1,isk)*a(1,i) +
1042 . skew(k2,isk)*a(2,i) +
1043 . skew(k3,isk)*a(3,i)
1044 IF(ibfv(7,n)==2) yc(ii)=(yc(ii)-dd)/dt2
1045 IF(ibfv(7,n)>=1) yc(ii)=(yc(ii)-vv)/dt12
1046 aa = diag_sms(i)*yc(ii) - a0
1047 a(1,i)=a(1,i)+skew(k1,isk)*aa
1048 a(2,i)=a(2,i)+skew(k2,isk)*aa
1049 a(3,i)=a(3,i)+skew(k3,isk)*aa
1050c ?
1051 dw = fourth*axi*weight(i) *
1052 . (yc(ii)*dt12+two*vv)*(ms(i)*yc(ii)-a0)
1053 wfext=wfext + dt1*dw
1054 vel(4,n) = dt2*dw
1055 ENDIF
1056 ENDIF
1057 ENDDO
1058 ENDDO
1059 IF (iroddl>=0)THEN
1060 DO k=4,3+3*iroddl
1061#include "vectorize.inc"
1062 DO ii=1,ic
1063 n = index(ii)
1064 ifm = ibfv(9,n)
1065 IF (ifm>1) THEN
1066 j=ibfv(2,n)
1067 IF(j==k)THEN
1068 ibfv(5,n) = iposc(ii)
1069 fac = vel(1,n)
1070 dw = vel(4,n)
1071 wfext=wfext + dw
1072 yc(ii) = yc(ii) * fac
1073 i=iabs(ibfv(1,n))
1074 j = j - 3
1075 k1=3*j-2
1076 k2=3*j-1
1077 k3=3*j
1078 vv = xframe(k1,ifm)*vr(1,i)
1079 . + xframe(k2,ifm)*vr(2,i)
1080 . + xframe(k3,ifm)*vr(3,i)
1081 vf = xframe(k1,ifm)*xframe(13,ifm)
1082 . + xframe(k2,ifm)*xframe(14,ifm)
1083 . + xframe(k3,ifm)*xframe(15,ifm)
1084 a0 = xframe(k1,ifm)*ar(1,i)
1085 . + xframe(k2,ifm)*ar(2,i)
1086 . + xframe(k3,ifm)*ar(3,i)
1087 aa = in(i)*(yc(ii)-vv+vf)/dt12 - a0
1088 ar(1,i)=ar(1,i)+xframe(k1,ifm)*aa
1089 ar(2,i)=ar(2,i)+xframe(k2,ifm)*aa
1090 ar(3,i)=ar(3,i)+xframe(k3,ifm)*aa
1091 IF(ibfv(6,n)==0)THEN
1092 dw= fourth*(yc(ii)+vv)*aa*weight(i)
1093 ELSE
1094 nr = ibfv(6,n)
1095 dw= fourth*(yc(ii)+vv)*weight(i)*aa*
1096 . ((rby(17,nr)*xframe(k1,ifm)
1097 . +rby(18,nr)*xframe(k2,ifm)
1098 . +rby(19,nr)*xframe(k3,ifm))*xframe(k1,ifm) +
1099 . (rby(20,nr)*xframe(k1,ifm)
1100 . +rby(21,nr)*xframe(k2,ifm)
1101 . +rby(22,nr)*xframe(k3,ifm))*xframe(k2,ifm) +
1102 . (rby(23,nr)*xframe(k1,ifm)
1103 . +rby(24,nr)*xframe(k2,ifm)
1104 . +rby(25,nr)*xframe(k3,ifm))*xframe(k3,ifm))
1105 . /max(in(i),em30)
1106 ENDIF
1107 wfext=wfext + dt1*dw
1108 vel(4,n) = dt2*dw
1109 ENDIF
1110 ELSE
1111 isk=ibfv(2,n)/10
1112 j=ibfv(2,n)-10*isk
1113 IF(j==k)THEN
1114 ibfv(5,n) = iposc(ii)
1115 fac = vel(1,n)
1116 dw = vel(4,n)
1117 wfext=wfext + dw
1118 yc(ii) = yc(ii) * fac
1119 i=iabs(ibfv(1,n))
1120 j = j - 3
1121 k1=3*j-2
1122 k2=3*j-1
1123 k3=3*j
1124 IF(ibfv(7,n)==2) THEN
1125 dd = skew(k1,isk)*dr(1,i) +
1126 . skew(k2,isk)*dr(2,i) +
1127 . skew(k3,isk)*dr(3,i)
1128 END IF
1129 vv = skew(k1,isk)*vr(1,i) +
1130 . skew(k2,isk)*vr(2,i) +
1131 . skew(k3,isk)*vr(3,i)
1132 a0 = skew(k1,isk)*ar(1,i) +
1133 . skew(k2,isk)*ar(2,i) +
1134 . skew(k3,isk)*ar(3,i)
1135 IF(ibfv(7,n)==2) yc(ii)=(yc(ii)-dd)/dt2
1136 IF(ibfv(7,n)>=1) yc(ii)=(yc(ii)-vv)/dt12
1137 aa = in(i)*yc(ii) - a0
1138 ar(1,i)=ar(1,i)+skew(k1,isk)*aa
1139 ar(2,i)=ar(2,i)+skew(k2,isk)*aa
1140 ar(3,i)=ar(3,i)+skew(k3,isk)*aa
1141 IF(ibfv(6,n)==0)THEN
1142 dw=fourth*(yc(ii)*dt12+two*vv)*aa*weight(i)
1143 ELSE
1144 nr = ibfv(6,n)
1145 dw= fourth*(yc(ii)*dt12+two*vv)*weight(i)*aa*
1146 . ((rby(17,nr)*skew(k1,isk)
1147 . +rby(18,nr)*skew(k2,isk)
1148 . +rby(19,nr)*skew(k3,isk))*skew(k1,isk) +
1149 . (rby(20,nr)*skew(k1,isk)
1150 . +rby(21,nr)*skew(k2,isk)
1151 . +rby(22,nr)*skew(k3,isk))*skew(k2,isk) +
1152 . (rby(23,nr)*skew(k1,isk)
1153 . +rby(24,nr)*skew(k2,isk)
1154 . +rby(25,nr)*skew(k3,isk))*skew(k3,isk))
1155 . /max(in(i),em30)
1156 ENDIF
1157 wfext=wfext + dt1*dw
1158 vel(4,n) = dt2*dw
1159 ENDIF
1160 ENDIF
1161 ENDDO
1162 ENDDO
1163 ENDIF
1164 ELSEIF (nfk==1.AND.icoor==1) THEN
1165 DO k=1,3
1166#include "vectorize.inc"
1167 DO ii=1,ic
1168 n = index(ii)
1169 ifm = ibfv(9,n)
1170 i=iabs(ibfv(1,n))
1171 IF (ifm>1) THEN
1172 j = ibfv(2,n)
1173 fmdir=zero
1174 r=one
1175 IF(j==k)THEN
1176 ibfv(5,n) = iposc(ii)
1177 fac = vel(1,n)
1178 dw = vel(4,n)
1179 wfext=wfext + dw
1180 yc(ii) = yc(ii) * fac
1181 axi=1.-ax+ax*x(2,i)
1182 coef = (xframe(10,ifm)-x(1,i))*xframe(7,ifm) +
1183 & (xframe(11,ifm)-x(2,i))*xframe(8,ifm) +
1184 & (xframe(12,ifm)-x(3,i))*xframe(9,ifm)
1185 hx = coef*xframe(7,ifm)+x(1,i)-xframe(10,ifm)
1186 hy = coef*xframe(8,ifm)+x(2,i)-xframe(11,ifm)
1187 hz = coef*xframe(9,ifm)+x(3,i)-xframe(12,ifm)
1188 r = sqrt(hx*hx+hy*hy+hz*hz)
1189 IF(r<=em20) THEN
1190 cth = zero
1191 sth = zero
1192 ELSE
1193 cth = (xframe(1,ifm)*hx +
1194 & xframe(2,ifm)*hy +
1195 & xframe(3,ifm)*hz)/r
1196 sth = (xframe(4,ifm)*hx +
1197 & xframe(5,ifm)*hy +
1198 & xframe(6,ifm)*hz)/r
1199 ENDIF
1200 IF (j==1) THEN
1201 fmdir(1) =cth*xframe(1,ifm)+ sth*xframe(4,ifm)
1202 fmdir(2) =cth*xframe(2,ifm)+ sth*xframe(5,ifm)
1203 fmdir(3) =cth*xframe(3,ifm)+ sth*xframe(6,ifm)
1204 ELSEIF (j==2) THEN
1205 er(1) =cth*xframe(1,ifm)+ sth*xframe(4,ifm)
1206 er(2) =cth*xframe(2,ifm)+ sth*xframe(5,ifm)
1207 er(3) =cth*xframe(3,ifm)+ sth*xframe(6,ifm)
1208 nor1=sqrt(er(1)*er(1)+er(2)*er(2)+er(3)*er(3))
1209 er=er/max(nor1,em20)
1210 et(1) =cth*xframe(4,ifm)- sth*xframe(1,ifm)
1211 et(2) =cth*xframe(5,ifm)- sth*xframe(2,ifm)
1212 et(3) =cth*xframe(6,ifm)- sth*xframe(3,ifm)
1213 nor2=sqrt(et(1)*et(1)+et(2)*et(2)+et(3)*et(3))
1214 et=et/max(nor2,em20)
1215c--------------------vitesse angulaire imposee--------
1216 alpha=-yc(ii)*dt2/two
1217c-----------------------------------------------
1218c ALPHA=-YC(II)*DT2/(TWO*R)
1219 fmdir=alpha*er+et
1220 ELSEIF (j==3) THEN
1221 fmdir(1)=xframe(7,ifm)
1222 fmdir(2)=xframe(8,ifm)
1223 fmdir(3)=xframe(9,ifm)
1224 ENDIF
1225 nor3 = sqrt(fmdir(1)*fmdir(1)
1226 & +fmdir(2)*fmdir(2)
1227 & +fmdir(3)*fmdir(3))
1228 fmdir=fmdir/max(nor3,em20)
1229 rx = x(1,i) - xframe(10,ifm)
1230 ry = x(2,i) - xframe(11,ifm)
1231 rz = x(3,i) - xframe(12,ifm)
1232 vfx = xframe(31,ifm) +
1233 & xframe(14,ifm)*rz - xframe(15,ifm)*ry
1234 vfy = xframe(32,ifm) +
1235 & xframe(15,ifm)*rx - xframe(13,ifm)*rz
1236 vfz = xframe(33,ifm) +
1237 & xframe(13,ifm)*ry-xframe(14,ifm)*rx
1238 vv = fmdir(1)*v(1,i) +
1239 & fmdir(2)*v(2,i) +
1240 & fmdir(3)*v(3,i)
1241 vf = fmdir(1)*vfx + fmdir(2)*vfy + fmdir(3)*vfz
1242 a0 = fmdir(1)*a(1,i) +
1243 & fmdir(2)*a(2,i) +
1244 & fmdir(3)*a(3,i)
1245 IF (j==2) THEN
1246c YC(II)=(YC(II)-VV+VF)/DT12
1247c------------------vitesse angulaire imposee--------
1248 yc(ii)=(r*yc(ii)-vv+vf)/dt12
1249c-------------------------------------------------
1250 ELSE
1251 yc(ii)=(yc(ii)-vv+vf)/dt12
1252 ENDIF
1253 aa = diag_sms(i)*yc(ii) - a0
1254 IF(r<=em20) aa=zero
1255 a(1,i)=a(1,i)+fmdir(1)*aa
1256 a(2,i)=a(2,i)+fmdir(2)*aa
1257 a(3,i)=a(3,i)+fmdir(3)*aa
1258 dw = fourth*axi*weight(i) *
1259 . (yc(ii)*dt12+two*vv)*(ms(i)*yc(ii)-a0)
1260 wfext=wfext + dt1*dw
1261 vel(4,n) = dt2*dw
1262 ENDIF
1263 ELSE
1264 isk=ibfv(2,n)/10
1265 j=ibfv(2,n)-10*isk
1266 skdir=zero
1267 r=one
1268 IF(j==k)THEN
1269 ibfv(5,n) = iposc(ii)
1270 fac = vel(1,n)
1271 yimp = vel(6,n)
1272c VEL(6,N) = YC(II)
1273 dw = vel(4,n)
1274 wfext=wfext + dw
1275 yc(ii) = yc(ii) * fac
1276 yimp = yimp * fac
1277 i=iabs(ibfv(1,n))
1278 axi=1.-ax+ax*x(2,i)
1279 coef = (skew(10,isk)-x(1,i))*skew(7,isk) +
1280 & (skew(11,isk)-x(2,i))*skew(8,isk) +
1281 & (skew(12,isk)-x(3,i))*skew(9,isk)
1282 hx = coef*skew(7,isk)+x(1,i)-skew(10,isk)
1283 hy = coef*skew(8,isk)+x(2,i)-skew(11,isk)
1284 hz = coef*skew(9,isk)+x(3,i)-skew(12,isk)
1285 r = sqrt(hx*hx+hy*hy+hz*hz)
1286 IF(r<=em20) THEN
1287 cth = zero
1288 sth = zero
1289 ELSE
1290 cth = (skew(1,isk)*hx +
1291 & skew(2,isk)*hy +
1292 & skew(3,isk)*hz)/r
1293 sth = (skew(4,isk)*hx +
1294 & skew(5,isk)*hy +
1295 & skew(6,isk)*hz)/r
1296 ENDIF
1297 IF (j==1) THEN
1298 skdir(1) =cth*skew(1,isk)+ sth*skew(4,isk)
1299 skdir(2) =cth*skew(2,isk)+ sth*skew(5,isk)
1300 skdir(3) =cth*skew(3,isk)+ sth*skew(6,isk)
1301 ELSEIF(j==2) THEN
1302 er(1) =cth*skew(1,isk)+ sth*skew(4,isk)
1303 er(2) =cth*skew(2,isk)+ sth*skew(5,isk)
1304 er(3) =cth*skew(3,isk)+ sth*skew(6,isk)
1305 nor1=sqrt(er(1)*er(1)+er(2)*er(2)+er(3)*er(3))
1306 er=er/max(nor1,em20)
1307 et(1) =cth*skew(4,isk)- sth*skew(1,isk)
1308 et(2) =cth*skew(5,isk)- sth*skew(2,isk)
1309 et(3) =cth*skew(6,isk)- sth*skew(3,isk)
1310 nor2=sqrt(et(1)*et(1)+et(2)*et(2)+et(3)*et(3))
1311 et=et/max(nor2,em20)
1312c-----------------vitesse angulaire imposee---------
1313 alpha=-yc(ii)*dt2/two
1314c-----------------sinon-----------------------------
1315c ALPHA=-YC(II)*DT2/(TWO*R)
1316 skdir=alpha*er+et
1317 ELSEIF(j==3) THEN
1318 skdir(1)=skew(7,isk)
1319 skdir(2)=skew(8,isk)
1320 skdir(3)=skew(9,isk)
1321 IF(ibfv(7,n)==2) yimp= skdir(1)*d(1,i) +
1322 & skdir(2)*d(2,i) +
1323 & skdir(3)*d(3,i)
1324 ENDIF
1325 nor3 = sqrt(skdir(1)*skdir(1)
1326 & +skdir(2)*skdir(2)
1327 & +skdir(3)*skdir(3))
1328 skdir=skdir/max(nor3,em20)
1329 vv = v(1,i)*skdir(1) +
1330 & v(2,i)*skdir(2) +
1331 & v(3,i)*skdir(3)
1332 a0 = a(1,i)*skdir(1) +
1333 & a(2,i)*skdir(2) +
1334 & a(3,i)*skdir(3)
1335 IF(ibfv(7,n)==2) yc(ii)=(yc(ii)-yimp)/dt2
1336 IF(ibfv(7,n)>=1) THEN
1337 IF (j==2) THEN
1338c YC(II)=(YC(II)-VV)/DT12
1339c------------------vitesse angulaire imposee--------
1340 yc(ii)=(r*yc(ii)-vv)/dt12
1341c----------------------------------------------------
1342 ELSE
1343 yc(ii)=(yc(ii)-vv)/dt12
1344 ENDIF
1345 ENDIF
1346 aa = diag_sms(i)*yc(ii) - a0
1347 IF(r<=em20) aa=zero
1348 a(1,i)=a(1,i)+skdir(1)*aa
1349 a(2,i)=a(2,i)+skdir(2)*aa
1350 a(3,i)=a(3,i)+skdir(3)*aa
1351 dw = fourth*axi*weight(i) *
1352 . (yc(ii)*dt12+two*vv)*(ms(i)*yc(ii)-a0)
1353 wfext=wfext + dt1*dw
1354 vel(4,n) = dt2*dw
1355 ENDIF
1356 ENDIF
1357 ENDDO
1358 ENDDO
1359 IF (iroddl>=0)THEN
1360 DO k=4,3+3*iroddl
1361#include "vectorize.inc"
1362 DO ii=1,ic
1363 n = index(ii)
1364 ifm = ibfv(9,n)
1365 IF (ifm>1) THEN
1366 j=ibfv(2,n)
1367 fmdir=zero
1368 r=one
1369 IF(j==k)THEN
1370 ibfv(5,n) = iposc(ii)
1371 fac = vel(1,n)
1372 dw = vel(4,n)
1373 wfext=wfext + dw
1374 yc(ii) = yc(ii) * fac
1375 i=iabs(ibfv(1,n))
1376 j = j - 3
1377 coef = (xframe(10,ifm)-x(1,i))*xframe(7,ifm) +
1378 & (xframe(11,ifm)-x(2,i))*xframe(8,ifm) +
1379 & (xframe(12,ifm)-x(3,i))*xframe(9,ifm)
1380 hx = coef*xframe(7,ifm)+x(1,i)-xframe(10,ifm)
1381 hy = coef*xframe(8,ifm)+x(2,i)-xframe(11,ifm)
1382 hz = coef*xframe(9,ifm)+x(3,i)-xframe(12,ifm)
1383 r = sqrt(hx*hx+hy*hy+hz*hz)
1384 IF(r<=em20) THEN
1385 cth = zero
1386 sth = zero
1387 ELSE
1388 cth = (xframe(1,ifm)*hx +
1389 & xframe(2,ifm)*hy +
1390 & xframe(3,ifm)*hz)/r
1391 sth = (xframe(4,ifm)*hx +
1392 & xframe(5,ifm)*hy +
1393 & xframe(6,ifm)*hz)/r
1394 ENDIF
1395 IF (j==1) THEN
1396 fmdir(1) =cth*xframe(1,ifm)+ sth*xframe(4,ifm)
1397 fmdir(2) =cth*xframe(2,ifm)+ sth*xframe(5,ifm)
1398 fmdir(3) =cth*xframe(3,ifm)+ sth*xframe(6,ifm)
1399 ELSEIF (j==2) THEN
1400 fmdir(1) =cth*xframe(4,ifm)- sth*xframe(1,ifm)
1401 fmdir(2) =cth*xframe(5,ifm)- sth*xframe(2,ifm)
1402 fmdir(3) =cth*xframe(6,ifm)- sth*xframe(3,ifm)
1403 ELSEIF (j==3) THEN
1404 fmdir(1)=xframe(7,ifm)
1405 fmdir(2)=xframe(8,ifm)
1406 fmdir(3)=xframe(9,ifm)
1407 ENDIF
1408 nor3 = sqrt(fmdir(1)*fmdir(1)
1409 & +fmdir(2)*fmdir(2)
1410 & +fmdir(3)*fmdir(3))
1411 fmdir=fmdir/max(nor3,em20)
1412 vv = fmdir(1)*vr(1,i) +
1413 & fmdir(2)*vr(2,i) +
1414 & fmdir(3)*vr(3,i)
1415 vf = fmdir(1)*xframe(13,ifm) +
1416 & fmdir(2)*xframe(14,ifm) +
1417 & fmdir(3)*xframe(15,ifm)
1418 a0 = fmdir(1)*ar(1,i) +
1419 & fmdir(2)*ar(2,i) +
1420 & fmdir(3)*ar(3,i)
1421 aa = in(i)*(yc(ii)-vv+vf)/dt12 - a0
1422 IF(r<=em20) aa=zero
1423 ar(1,i)=ar(1,i)+fmdir(1)*aa
1424 ar(2,i)=ar(2,i)+fmdir(2)*aa
1425 ar(3,i)=ar(3,i)+fmdir(3)*aa
1426 IF(ibfv(6,n)==0)THEN
1427 dw= fourth*(yc(ii)+vv)*aa*weight(i)
1428 ELSE
1429 nr = ibfv(6,n)
1430 dw= fourth*(yc(ii)+vv)*weight(i)*aa*
1431 & ((rby(17,nr)*fmdir(1)
1432 & +rby(18,nr)*fmdir(2)
1433 & +rby(19,nr)*fmdir(3))*fmdir(1) +
1434 & (rby(20,nr)*fmdir(1)
1435 & +rby(21,nr)*fmdir(2)
1436 & +rby(22,nr)*fmdir(3))*fmdir(2) +
1437 & (rby(23,nr)*fmdir(1)
1438 & +rby(24,nr)*fmdir(2)
1439 & +rby(25,nr)*fmdir(3))*fmdir(3))
1440 . /max(in(i),em30)
1441 ENDIF
1442 wfext=wfext + dt1*dw
1443 vel(4,n) = dt2*dw
1444 ENDIF
1445 ELSE
1446 isk=ibfv(2,n)/10
1447 j=ibfv(2,n)-10*isk
1448 skdir=zero
1449 r=one
1450 IF(j==k)THEN
1451 ibfv(5,n) = iposc(ii)
1452 fac = vel(1,n)
1453 yimp = vel(6,n)
1454 vel(6,n) = yc(ii)
1455 dw = vel(4,n)
1456 wfext=wfext + dw
1457 yc(ii) = yc(ii) * fac
1458 yimp = yimp * fac
1459 i=iabs(ibfv(1,n))
1460 j = j - 3
1461 coef = (skew(10,isk)-x(1,i))*skew(7,isk) +
1462 & (skew(11,isk)-x(2,i))*skew(8,isk) +
1463 & (skew(12,isk)-x(3,i))*skew(9,isk)
1464 hx = coef*skew(7,isk)+x(1,i)-skew(10,isk)
1465 hy = coef*skew(8,isk)+x(2,i)-skew(11,isk)
1466 hz = coef*skew(9,isk)+x(3,i)-skew(12,isk)
1467 r = sqrt(hx*hx+hy*hy+hz*hz)
1468 IF(r<=em20) THEN
1469 cth = zero
1470 sth = zero
1471 ELSE
1472 cth = (skew(1,isk)*hx +
1473 & skew(2,isk)*hy +
1474 & skew(3,isk)*hz)/r
1475 sth = (skew(4,isk)*hx +
1476 & skew(5,isk)*hy +
1477 & skew(6,isk)*hz)/r
1478 ENDIF
1479 IF (j==1) THEN
1480 skdir(1) =cth*skew(1,isk)+ sth*skew(4,isk)
1481 skdir(2) =cth*skew(2,isk)+ sth*skew(5,isk)
1482 skdir(3) =cth*skew(3,isk)+ sth*skew(6,isk)
1483 ELSEIF(j==2) THEN
1484 skdir(1) =cth*skew(4,isk)- sth*skew(1,isk)
1485 skdir(2) =cth*skew(5,isk)- sth*skew(2,isk)
1486 skdir(3) =cth*skew(6,isk)- sth*skew(3,isk)
1487 ELSEIF(j==3) THEN
1488 skdir(1)=skew(7,isk)
1489 skdir(2)=skew(8,isk)
1490 skdir(3)=skew(9,isk)
1491 IF(ibfv(7,n)==2) yimp= skdir(1)*dr(1,i) +
1492 & skdir(2)*dr(2,i) +
1493 & skdir(3)*dr(3,i)
1494 ENDIF
1495 nor3 = sqrt(skdir(1)*skdir(1)
1496 & +skdir(2)*skdir(2)
1497 & +skdir(3)*skdir(3))
1498 skdir=skdir/max(nor3,em20)
1499
1500 IF(ibfv(7,n)==2) yc(ii)=(yc(ii)-yimp)/dt2
1501 vv = vr(1,i)*skdir(1) +
1502 & vr(2,i)*skdir(2) +
1503 & vr(3,i)*skdir(3)
1504 a0 = ar(1,i)*skdir(1) +
1505 & ar(2,i)*skdir(2) +
1506 & ar(3,i)*skdir(3)
1507 IF(ibfv(7,n)>=1) yc(ii)=(yc(ii)-vv)/dt12
1508 aa = in(i)*yc(ii) - a0
1509 IF(r<=em20) aa=zero
1510 ar(1,i)=ar(1,i)+skdir(1)*aa
1511 ar(2,i)=ar(2,i)+skdir(2)*aa
1512 ar(3,i)=ar(3,i)+skdir(3)*aa
1513 IF(ibfv(6,n)==0)THEN
1514 dw= fourth*(yc(ii)*dt12+two*vv)*
1515 & aa*weight(i)
1516 ELSE
1517 nr = ibfv(6,n)
1518 dw= fourth*(yc(ii)*dt12+two*vv)*weight(i)*aa*
1519 & ((rby(17,nr)*skdir(1)
1520 & +rby(18,nr)*skdir(2)
1521 & +rby(19,nr)*skdir(3))*skdir(1) +
1522 & (rby(20,nr)*skdir(1)
1523 & +rby(21,nr)*skdir(2)
1524 & +rby(22,nr)*skdir(3))*skdir(2) +
1525 & (rby(23,nr)*skdir(1)
1526 & +rby(24,nr)*skdir(2)
1527 & +rby(25,nr)*skdir(3))*skdir(3))
1528 . /max(in(i),em30)
1529 ENDIF
1530 wfext=wfext + dt1*dw
1531 vel(4,n) = dt2*dw
1532 ENDIF
1533 ENDIF
1534 ENDDO
1535 ENDDO
1536 ENDIF
1537 ENDIF
1538 ENDIF
1539C
1540 IF (cptreac/=0) THEN
1541 DO ii=1,ic
1542 n = index(ii)
1543 i=iabs(ibfv(1,n))
1544 IF (nodreac(i)/=0) THEN
1545 fthreac(1,nodreac(i)) = fthreac(1,nodreac(i)) + (a(1,i) -
1546 & aold0(1,ii))*dt12
1547 fthreac(2,nodreac(i)) = fthreac(2,nodreac(i)) + (a(2,i) -
1548 & aold0(2,ii))*dt12
1549 fthreac(3,nodreac(i)) = fthreac(3,nodreac(i)) + (a(3,i) -
1550 & aold0(3,ii))*dt12
1551 ENDIF
1552 ENDDO
1553 ENDIF
1554C
1555 100 CONTINUE
1556 ENDDO
1557C
1558 ELSE
1559C------------------
1560 ideb = 0
1561C
1562 DO nn=1,nfxvel,nvsiz
1563 IF (ibfv(8,nn)==1) GOTO 200
1564 ic = 0
1565 IF (nsensor>0) THEN
1566 DO 210 ii = 1, min(nfxvel-ideb,nvsiz)
1567 n = ii+ideb
1568 startt = vel(2,n)
1569 stopt = vel(3,n)
1570 facx = vel(5,n)
1571 IF(tt<startt)GOTO 210
1572 IF(tt>stopt) GOTO 210
1573 i=iabs(ibfv(1,n))
1574C
1575 IF(nodnx_sms(i)==0) cycle
1576 isens = ibfv(4,n)
1577 IF(isens==0)THEN
1578 ts=tt
1579 ELSE
1580 ts = tt-sensor_tab(isens)%TSTART
1581 IF(ts<0.0)GOTO 210
1582 ENDIF
1583 ic = ic + 1
1584 index(ic) = n
1585210 CONTINUE
1586 ELSE
1587C sans sensor
1588 DO 220 ii = 1, min(nfxvel-ideb,nvsiz)
1589 n = ii+ideb
1590 startt = vel(2,n)
1591 stopt = vel(3,n)
1592 facx = vel(5,n)
1593 IF(tt<startt)GOTO 220
1594 IF(tt>stopt) GOTO 220
1595 i=iabs(ibfv(1,n))
1596C
1597 IF(nodnx_sms(i)==0) cycle
1598 ic = ic + 1
1599 index(ic) = n
1600220 CONTINUE
1601 ENDIF
1602C
1603 IF (cptreac/=0) THEN
1604 DO ii=1,ic
1605 n = index(ii)
1606 i=iabs(ibfv(1,n))
1607 aold0(1,ii)=a(1,i)
1608 aold0(2,ii)=a(2,i)
1609 aold0(3,ii)=a(3,i)
1610 ENDDO
1611 ENDIF
1612C
1613 ideb = ideb + min(nfxvel-ideb,nvsiz)
1614C-------
1615 IF(ivector==0) THEN
1616 DO ii=1,ic
1617 n = index(ii)
1618 fac = vel(1,n)
1619 yc(ii) = zero
1620 i=iabs(ibfv(1,n))
1621 isk=ibfv(2,n)/10
1622 ifm = ibfv(9,n)
1623 j=ibfv(2,n)
1624 icoor = ibfv(10,n)
1625 skdir=zero
1626 fmdir=zero
1627 r=one
1628 IF (ifm<=1) j=j-10*isk
1629 IF(j<=3)THEN
1630 IF(isk<=1.AND.ifm<=1.AND.icoor==0)THEN
1631 a(j,i)=zero
1632 ELSEIF (isk>1.AND.icoor==0) THEN
1633 k1=3*j-2
1634 k2=3*j-1
1635 k3=3*j
1636 a0 = skew(k1,isk)*a(1,i) +
1637 . skew(k2,isk)*a(2,i) +
1638 . skew(k3,isk)*a(3,i)
1639 aa = yc(ii) - a0
1640 a(1,i)=a(1,i)+skew(k1,isk)*aa
1641 a(2,i)=a(2,i)+skew(k2,isk)*aa
1642 a(3,i)=a(3,i)+skew(k3,isk)*aa
1643 ELSEIF ((isk<=1.AND.ifm<=1.AND.icoor==1) .OR.
1644 & (isk>1 .AND. icoor==1)) THEN
1645 coef = (skew(10,isk)-x(1,i))*skew(7,isk) +
1646 & (skew(11,isk)-x(2,i))*skew(8,isk) +
1647 & (skew(12,isk)-x(3,i))*skew(9,isk)
1648 hx = coef*skew(7,isk)+x(1,i)-skew(10,isk)
1649 hy = coef*skew(8,isk)+x(2,i)-skew(11,isk)
1650 hz = coef*skew(9,isk)+x(3,i)-skew(12,isk)
1651 r = sqrt(hx*hx+hy*hy+hz*hz)
1652 IF(r<=em20) THEN
1653 cth = zero
1654 sth = zero
1655 ELSE
1656 cth = (skew(1,isk)*hx +
1657 & skew(2,isk)*hy +
1658 & skew(3,isk)*hz)/r
1659 sth = (skew(4,isk)*hx +
1660 & skew(5,isk)*hy +
1661 & skew(6,isk)*hz)/r
1662 ENDIF
1663 IF (j==1) THEN
1664 skdir(1) =cth*skew(1,isk)+ sth*skew(4,isk)
1665 skdir(2) =cth*skew(2,isk)+ sth*skew(5,isk)
1666 skdir(3) =cth*skew(3,isk)+ sth*skew(6,isk)
1667 ELSEIF(j==2) THEN
1668 skdir(1) =cth*skew(4,isk)- sth*skew(1,isk)
1669 skdir(2) =cth*skew(5,isk)- sth*skew(2,isk)
1670 skdir(3) =cth*skew(6,isk)- sth*skew(3,isk)
1671 ELSEIF(j==3) THEN
1672 skdir(1)=skew(7,isk)
1673 skdir(2)=skew(8,isk)
1674 skdir(3)=skew(9,isk)
1675 ENDIF
1676 nor3 = sqrt(skdir(1)*skdir(1)
1677 & +skdir(2)*skdir(2)
1678 & +skdir(3)*skdir(3))
1679 skdir=skdir/max(nor3,em20)
1680 a0 = a(1,i)*skdir(1) +
1681 & a(2,i)*skdir(2) +
1682 & a(3,i)*skdir(3)
1683 aa = - a0
1684 IF(r<=em20) aa=zero
1685 a(1,i)=a(1,i)+skdir(1)*aa
1686 a(2,i)=a(2,i)+skdir(2)*aa
1687 a(3,i)=a(3,i)+skdir(3)*aa
1688 ELSEIF (ifm>1.AND.icoor==0) THEN
1689C Fixed velocity in moving frame (transl)
1690 k1=3*j-2
1691 k2=3*j-1
1692 k3=3*j
1693 a0 = xframe(k1,ifm)*a(1,i)
1694 . + xframe(k2,ifm)*a(2,i)
1695 . + xframe(k3,ifm)*a(3,i)
1696 aa = - a0
1697 a(1,i)=a(1,i)+xframe(k1,ifm)*aa
1698 a(2,i)=a(2,i)+xframe(k2,ifm)*aa
1699 a(3,i)=a(3,i)+xframe(k3,ifm)*aa
1700 ELSEIF (ifm>1.AND.icoor==1) THEN
1701 coef = (xframe(10,ifm)-x(1,i))*xframe(7,ifm) +
1702 & (xframe(11,ifm)-x(2,i))*xframe(8,ifm) +
1703 & (xframe(12,ifm)-x(3,i))*xframe(9,ifm)
1704 hx = coef*xframe(7,ifm)+x(1,i)-xframe(10,ifm)
1705 hy = coef*xframe(8,ifm)+x(2,i)-xframe(11,ifm)
1706 hz = coef*xframe(9,ifm)+x(3,i)-xframe(12,ifm)
1707 r = sqrt(hx*hx+hy*hy+hz*hz)
1708 IF(r<=em20) THEN
1709 cth = zero
1710 sth = zero
1711 ELSE
1712 cth = (xframe(1,ifm)*hx +
1713 & xframe(2,ifm)*hy +
1714 & xframe(3,ifm)*hz)/r
1715 sth = (xframe(4,ifm)*hx +
1716 & xframe(5,ifm)*hy +
1717 & xframe(6,ifm)*hz)/r
1718 ENDIF
1719 IF (j==1) THEN
1720 fmdir(1) =cth*xframe(1,ifm)+ sth*xframe(4,ifm)
1721 fmdir(2) =cth*xframe(2,ifm)+ sth*xframe(5,ifm)
1722 fmdir(3) =cth*xframe(3,ifm)+ sth*xframe(6,ifm)
1723 ELSEIF (j==2) THEN
1724 fmdir(1) =cth*xframe(4,ifm)- sth*xframe(1,ifm)
1725 fmdir(2) =cth*xframe(5,ifm)- sth*xframe(2,ifm)
1726 fmdir(3) =cth*xframe(6,ifm)- sth*xframe(3,ifm)
1727 ELSEIF (j==3) THEN
1728 fmdir(1)=xframe(7,ifm)
1729 fmdir(2)=xframe(8,ifm)
1730 fmdir(3)=xframe(9,ifm)
1731 ENDIF
1732 nor3 = sqrt(fmdir(1)*fmdir(1)
1733 & +fmdir(2)*fmdir(2)
1734 & +fmdir(3)*fmdir(3))
1735 fmdir=fmdir/max(nor3,em20)
1736 a0 = fmdir(1)*a(1,i) +
1737 & fmdir(2)*a(2,i) +
1738 & fmdir(3)*a(3,i)
1739 aa = - a0
1740 IF(r<=em20) aa=zero
1741 a(1,i)=a(1,i)+fmdir(1)*aa
1742 a(2,i)=a(2,i)+fmdir(2)*aa
1743 a(3,i)=a(3,i)+fmdir(3)*aa
1744 ENDIF
1745 ELSEIF(j<=6)THEN
1746 cycle
1747 ENDIF
1748 ENDDO
1749 ELSE
1750C partie vectorielle
1751 nsk=0
1752 nfk=0
1753 DO ii=1,ic
1754 n = index(ii)
1755 ifm = ibfv(9,n)
1756 isk = ibfv(2,n)/10
1757 icoor=ibfv(10,n)
1758 IF (ifm>1) THEN
1759 nfk = 1
1760 ELSEIF (isk>1) THEN
1761 nsk=1
1762 ENDIF
1763 ENDDO
1764 IF(nsk==0.AND.nfk==0.AND.icoor==0)THEN
1765#include "vectorize.inc"
1766 DO ii=1,ic
1767 n = index(ii)
1768 yc(ii) = zero
1769 i=iabs(ibfv(1,n))
1770 isk=ibfv(2,n)/10
1771 j=ibfv(2,n)-10*isk
1772 IF(j<=3)THEN
1773 a(j,i)=zero
1774 ELSEIF(j<=6)THEN
1775 cycle
1776 ENDIF
1777 ENDDO
1778 ELSEIF (nsk==1.AND.nfk==0.AND.icoor==0) THEN
1779 DO k=1,3
1780#include "vectorize.inc"
1781 DO ii=1,ic
1782 n = index(ii)
1783 isk=ibfv(2,n)/10
1784 j=ibfv(2,n)-10*isk
1785 IF(j==k)THEN
1786 ibfv(5,n) = iposc(ii)
1787 yc(ii) = zero
1788 i=iabs(ibfv(1,n))
1789 axi=one-ax+ax*x(2,i)
1790 k1=3*j-2
1791 k2=3*j-1
1792 k3=3*j
1793 a0 = skew(k1,isk)*a(1,i) +
1794 . skew(k2,isk)*a(2,i) +
1795 . skew(k3,isk)*a(3,i)
1796 aa = - a0
1797 a(1,i)=a(1,i)+skew(k1,isk)*aa
1798 a(2,i)=a(2,i)+skew(k2,isk)*aa
1799 a(3,i)=a(3,i)+skew(k3,isk)*aa
1800 ENDIF
1801 ENDDO
1802 ENDDO
1803 ELSEIF ((nsk==0.AND.nfk==0.AND.icoor==1).OR.
1804 & (nsk==1.AND.nfk==0.AND.icoor==1)) THEN
1805 DO k=1,3
1806#include "vectorize.inc"
1807 DO ii=1,ic
1808 n = index(ii)
1809 isk=ibfv(2,n)/10
1810 j=ibfv(2,n)-10*isk
1811 skdir=zero
1812 r=one
1813 IF(j==k)THEN
1814 ibfv(5,n) = iposc(ii)
1815 yc(ii) = zero
1816 i=iabs(ibfv(1,n))
1817 axi=one-ax+ax*x(2,i)
1818 coef = (skew(10,isk)-x(1,i))*skew(7,isk) +
1819 & (skew(11,isk)-x(2,i))*skew(8,isk) +
1820 & (skew(12,isk)-x(3,i))*skew(9,isk)
1821 hx = coef*skew(7,isk)+x(1,i)-skew(10,isk)
1822 hy = coef*skew(8,isk)+x(2,i)-skew(11,isk)
1823 hz = coef*skew(9,isk)+x(3,i)-skew(12,isk)
1824 r = sqrt(hx*hx+hy*hy+hz*hz)
1825 IF(r<=em20) THEN
1826 cth = zero
1827 sth = zero
1828 ELSE
1829 cth = (skew(1,isk)*hx +
1830 & skew(2,isk)*hy +
1831 & skew(3,isk)*hz)/r
1832 sth = (skew(4,isk)*hx +
1833 & skew(5,isk)*hy +
1834 & skew(6,isk)*hz)/r
1835 ENDIF
1836 IF (j==1) THEN
1837 skdir(1) =cth*skew(1,isk)+ sth*skew(4,isk)
1838 skdir(2) =cth*skew(2,isk)+ sth*skew(5,isk)
1839 skdir(3) =cth*skew(3,isk)+ sth*skew(6,isk)
1840 ELSEIF(j==2) THEN
1841 skdir(1) =cth*skew(4,isk)- sth*skew(1,isk)
1842 skdir(2) =cth*skew(5,isk)- sth*skew(2,isk)
1843 skdir(3) =cth*skew(6,isk)- sth*skew(3,isk)
1844 ELSEIF(j==3) THEN
1845 skdir(1)=skew(7,isk)
1846 skdir(2)=skew(8,isk)
1847 skdir(3)=skew(9,isk)
1848 ENDIF
1849 nor3 = sqrt(skdir(1)*skdir(1)
1850 & +skdir(2)*skdir(2)
1851 & +skdir(3)*skdir(3))
1852 skdir=skdir/max(nor3,em20)
1853 a0 = a(1,i)*skdir(1) +
1854 & a(2,i)*skdir(2) +
1855 & a(3,i)*skdir(3)
1856 aa = - a0
1857 IF(r<=em20) aa=zero
1858 a(1,i)=a(1,i)+skdir(1)*aa
1859 a(2,i)=a(2,i)+skdir(2)*aa
1860 a(3,i)=a(3,i)+skdir(3)*aa
1861 ENDIF
1862 ENDDO
1863 ENDDO
1864 ELSEIF (nfk==1.AND.icoor==0) THEN
1865 DO k=1,3
1866#include "vectorize.inc"
1867 DO ii=1,ic
1868 n = index(ii)
1869 ifm = ibfv(9,n)
1870 IF (ifm>1) THEN
1871 j = ibfv(2,n)
1872 IF(j==k)THEN
1873 ibfv(5,n) = iposc(ii)
1874 yc(ii) = zero
1875 i=iabs(ibfv(1,n))
1876 k1=3*j-2
1877 k2=3*j-1
1878 k3=3*j
1879 a0 = xframe(k1,ifm)*a(1,i)
1880 . + xframe(k2,ifm)*a(2,i)
1881 . + xframe(k3,ifm)*a(3,i)
1882 aa = yc(ii) - a0
1883 a(1,i)=a(1,i)+xframe(k1,ifm)*aa
1884 a(2,i)=a(2,i)+xframe(k2,ifm)*aa
1885 a(3,i)=a(3,i)+xframe(k3,ifm)*aa
1886 ENDIF
1887 ELSE
1888 isk=ibfv(2,n)/10
1889 j=ibfv(2,n)-10*isk
1890 IF(j==k)THEN
1891 ibfv(5,n) = iposc(ii)
1892 yc(ii) = zero
1893 i=iabs(ibfv(1,n))
1894 k1=3*j-2
1895 k2=3*j-1
1896 k3=3*j
1897 a0 = skew(k1,isk)*a(1,i) +
1898 . skew(k2,isk)*a(2,i) +
1899 . skew(k3,isk)*a(3,i)
1900 aa = yc(ii) - a0
1901 a(1,i)=a(1,i)+skew(k1,isk)*aa
1902 a(2,i)=a(2,i)+skew(k2,isk)*aa
1903 a(3,i)=a(3,i)+skew(k3,isk)*aa
1904 ENDIF
1905 ENDIF
1906 ENDDO
1907 ENDDO
1908 ELSEIF (nfk==1.AND.icoor==1) THEN
1909 DO k=1,3
1910#include "vectorize.inc"
1911 DO ii=1,ic
1912 n = index(ii)
1913 ifm = ibfv(9,n)
1914 IF (ifm>1) THEN
1915 j = ibfv(2,n)
1916 fmdir=zero
1917 r=one
1918 IF(j==k)THEN
1919 ibfv(5,n) = iposc(ii)
1920 yc(ii) = zero
1921 i=iabs(ibfv(1,n))
1922 coef = (xframe(10,ifm)-x(1,i))*xframe(7,ifm) +
1923 & (xframe(11,ifm)-x(2,i))*xframe(8,ifm) +
1924 & (xframe(12,ifm)-x(3,i))*xframe(9,ifm)
1925 hx = coef*xframe(7,ifm)+x(1,i)-xframe(10,ifm)
1926 hy = coef*xframe(8,ifm)+x(2,i)-xframe(11,ifm)
1927 hz = coef*xframe(9,ifm)+x(3,i)-xframe(12,ifm)
1928 r = sqrt(hx*hx+hy*hy+hz*hz)
1929 IF(r<=em20) THEN
1930 cth = zero
1931 sth = zero
1932 ELSE
1933 cth = (xframe(1,ifm)*hx +
1934 & xframe(2,ifm)*hy +
1935 & xframe(3,ifm)*hz)/r
1936 sth = (xframe(4,ifm)*hx +
1937 & xframe(5,ifm)*hy +
1938 & xframe(6,ifm)*hz)/r
1939 ENDIF
1940 IF (j==1) THEN
1941 fmdir(1) =cth*xframe(1,ifm)+ sth*xframe(4,ifm)
1942 fmdir(2) =cth*xframe(2,ifm)+ sth*xframe(5,ifm)
1943 fmdir(3) =cth*xframe(3,ifm)+ sth*xframe(6,ifm)
1944 ELSEIF (j==2) THEN
1945 fmdir(1) =cth*xframe(4,ifm)- sth*xframe(1,ifm)
1946 fmdir(2) =cth*xframe(5,ifm)- sth*xframe(2,ifm)
1947 fmdir(3) =cth*xframe(6,ifm)- sth*xframe(3,ifm)
1948 ELSEIF (j==3) THEN
1949 fmdir(1)=xframe(7,ifm)
1950 fmdir(2)=xframe(8,ifm)
1951 fmdir(3)=xframe(9,ifm)
1952 ENDIF
1953 nor3 = sqrt(fmdir(1)*fmdir(1)
1954 & +fmdir(2)*fmdir(2)
1955 & +fmdir(3)*fmdir(3))
1956 fmdir=fmdir/max(nor3,em20)
1957 a0 = fmdir(1)*a(1,i) +
1958 & fmdir(2)*a(2,i) +
1959 & fmdir(3)*a(3,i)
1960 aa = - a0
1961 IF(r<=em20) aa=zero
1962 a(1,i)=a(1,i)+fmdir(1)*aa
1963 a(2,i)=a(2,i)+fmdir(2)*aa
1964 a(3,i)=a(3,i)+fmdir(3)*aa
1965 ENDIF
1966 ELSE
1967 isk=ibfv(2,n)/10
1968 j=ibfv(2,n)-10*isk
1969 skdir=zero
1970 r=one
1971 IF(j==k)THEN
1972 ibfv(5,n) = iposc(ii)
1973 yc(ii) = zero
1974 i=iabs(ibfv(1,n))
1975 coef = (skew(10,isk)-x(1,i))*skew(7,isk) +
1976 & (skew(11,isk)-x(2,i))*skew(8,isk) +
1977 & (skew(12,isk)-x(3,i))*skew(9,isk)
1978 hx = coef*skew(7,isk)+x(1,i)-skew(10,isk)
1979 hy = coef*skew(8,isk)+x(2,i)-skew(11,isk)
1980 hz = coef*skew(9,isk)+x(3,i)-skew(12,isk)
1981 r = sqrt(hx*hx+hy*hy+hz*hz)
1982 IF(r<=em20) THEN
1983 cth = zero
1984 sth = zero
1985 ELSE
1986 cth = (skew(1,isk)*hx +
1987 & skew(2,isk)*hy +
1988 & skew(3,isk)*hz)/r
1989 sth = (skew(4,isk)*hx +
1990 & skew(5,isk)*hy +
1991 & skew(6,isk)*hz)/r
1992 ENDIF
1993 IF (j==1) THEN
1994 skdir(1) =cth*skew(1,isk)+ sth*skew(4,isk)
1995 skdir(2) =cth*skew(2,isk)+ sth*skew(5,isk)
1996 skdir(3) =cth*skew(3,isk)+ sth*skew(6,isk)
1997 ELSEIF(j==2) THEN
1998 skdir(1) =cth*skew(4,isk)- sth*skew(1,isk)
1999 skdir(2) =cth*skew(5,isk)- sth*skew(2,isk)
2000 skdir(3) =cth*skew(6,isk)- sth*skew(3,isk)
2001 ELSEIF(j==3) THEN
2002 skdir(1)=skew(7,isk)
2003 skdir(2)=skew(8,isk)
2004 skdir(3)=skew(9,isk)
2005 ENDIF
2006 nor3 = sqrt(skdir(1)*skdir(1)
2007 & +skdir(2)*skdir(2)
2008 & +skdir(3)*skdir(3))
2009 skdir=skdir/max(nor3,em20)
2010 a0 = a(1,i)*skdir(1) +
2011 & a(2,i)*skdir(2) +
2012 & a(3,i)*skdir(3)
2013 aa = yc(ii) - a0
2014 IF(r<=em20) aa=zero
2015 a(1,i)=a(1,i)+skdir(1)*aa
2016 a(2,i)=a(2,i)+skdir(2)*aa
2017 a(3,i)=a(3,i)+skdir(3)*aa
2018 ENDIF
2019 ENDIF
2020 ENDDO
2021 ENDDO
2022 ENDIF
2023 ENDIF
2024
2025 IF (cptreac/=0 .AND. it>0) THEN
2026 DO ii=1,ic
2027 n = index(ii)
2028 i=iabs(ibfv(1,n))
2029 IF (nodreac(i)/=0) THEN
2030 fthreac(1,nodreac(i)) = fthreac(1,nodreac(i)) + (a(1,i) -
2031 & aold0(1,ii))*ms(i)*dt12
2032 fthreac(2,nodreac(i)) = fthreac(2,nodreac(i)) + (a(2,i) -
2033 & aold0(2,ii))*ms(i)*dt12
2034 fthreac(3,nodreac(i)) = fthreac(3,nodreac(i)) + (a(3,i) -
2035 & aold0(3,ii))*ms(i)*dt12
2036 ENDIF
2037 ENDDO
2038 ENDIF
2039C
2040 200 CONTINUE
2041 ENDDO
2042 END IF
2043C
2044 RETURN
2045 END
alpha
#define alpha
Definition eval.h:35
min
#define min(a, b)
Definition macros.h:20
max
#define max(a, b)
Definition macros.h:21
sms_fixvel
subroutine sms_fixvel(ibfv, a, v, npc, tf, vel, ms, x, skew, sensor_tab, weight, d, iframe, xframe, nsensor, it, diag_sms, nodnx_sms, cptreac, nodreac, fthreac, ar, vr, dr, in, rby, wfext)
Definition sms_fixvel.F:40
vinter
subroutine vinter(tf, iad, ipos, ilen, nel, x, dydx, y)
Definition vinter.F:72