OpenRadioss 2025.1.11
OpenRadioss project
Loading...
Searching...
No Matches
movfram.F File Reference
#include "implicit_f.inc"
#include "com01_c.inc"
#include "com04_c.inc"
#include "com08_c.inc"
#include "param_c.inc"
#include "task_c.inc"
#include "impl1_c.inc"

Go to the source code of this file.

Functions/Subroutines

subroutine movfra1 (xframe, iframe, x, v, a, ar)
subroutine movfra2 (xframe, iframe, x, v, vr, d)
subroutine movfra_imp (xframe, iframe, x, v, a, vr, ar, d)

Function/Subroutine Documentation

◆ movfra1()

subroutine movfra1 ( dimension(nxframe,numfram+1), intent(inout) xframe,
integer, dimension(liskn,numfram+1), intent(in) iframe,
dimension(3,numnod), intent(in) x,
dimension(3,numnod), intent(in) v,
dimension(3,numnod), intent(in) a,
dimension(3,numnod), intent(in) ar )

Definition at line 30 of file movfram.F.

31C-----------------------------------------------
32C I m p l i c i t T y p e s
33C-----------------------------------------------
34#include "implicit_f.inc"
35C-----------------------------------------------
36C C o m m o n B l o c k s
37C-----------------------------------------------
38#include "com01_c.inc"
39#include "com04_c.inc"
40#include "com08_c.inc"
41#include "param_c.inc"
42#include "task_c.inc"
43C-----------------------------------------------
44C D u m m y A r g u m e n t s
45C-----------------------------------------------
46 INTEGER, intent(IN) :: IFRAME(LISKN,NUMFRAM+1)
47 my_real, intent(INOUT) :: xframe(nxframe,numfram+1)
48 my_real, intent(IN) :: x(3,numnod), v(3,numnod), a(3,numnod), ar(3,numnod)
49C-----------------------------------------------
50C L o c a l V a r i a b l e s
51C-----------------------------------------------
52 INTEGER N, N1, N2, N3, IMOV, IDIR
53 my_real o(3), p(9), pp1, pp3, pp2, pold(9),
54 . dr11, dr12, dr13,
55 . dr21, dr22, dr23,
56 . dr31, dr32, dr33,
57 . xpi, cs, r, sn, rs2, drx, dry, drz, vrx, vry, vrz, dtinv
58C-----------------------------------------------
59C S o u r c e L i n e s
60C-----------------------------------------------
61 pp1 = zero
62 pp2 = zero
63 pp3 = zero
64 p(1:9) = zero
65 IF(ispmd == 0) THEN
66 xpi=pi
67 DO n=1,numfram
68 n1=iframe(1,n+1)
69 n2=iframe(2,n+1)
70 n3=iframe(3,n+1)
71 imov=iframe(5,n+1)
72 IF(n1+n2+n3 == 0) THEN
73 ! Fixed Frame
74 ELSEIF (n2+n3 == 0) THEN
75 ! Node defined frame (moving)
76 xframe(16,n+1)=ar(1,n1)
77 xframe(17,n+1)=ar(2,n1)
78 xframe(18,n+1)=ar(3,n1)
79 IF(nxframe >= 36)THEN
80 xframe(28,n+1)=a(1,n1)
81 xframe(29,n+1)=a(2,n1)
82 xframe(30,n+1)=a(3,n1)
83 ENDIF
84 ELSE
85 ! Moving frame frame
86 ! extract frame orientation wrt global system at time t
87 pold(1)=xframe(1,n+1)
88 pold(2)=xframe(2,n+1)
89 pold(3)=xframe(3,n+1)
90 pold(4)=xframe(4,n+1)
91 pold(5)=xframe(5,n+1)
92 pold(6)=xframe(6,n+1)
93 pold(7)=xframe(7,n+1)
94 pold(8)=xframe(8,n+1)
95 pold(9)=xframe(9,n+1)
96 ! position & orientation wrt global system at time t+1
97 o(1)=x(1,n1)+dt2*(v(1,n1)+dt12*a(1,n1))
98 o(2)=x(2,n1)+dt2*(v(2,n1)+dt12*a(2,n1))
99 o(3)=x(3,n1)+dt2*(v(3,n1)+dt12*a(3,n1))
100 IF (imov == 1) THEN
101 idir = iframe(6,n+1)
102 ! calculation of X'(IDIR=1) Y'(IDIR=2) Z'(IDIR=3)
103 IF (idir == 1) THEN
104 p(1)=x(1,n2)+dt2*(v(1,n2)+dt12*a(1,n2))-o(1)
105 p(2)=x(2,n2)+dt2*(v(2,n2)+dt12*a(2,n2))-o(2)
106 p(3)=x(3,n2)+dt2*(v(3,n2)+dt12*a(3,n2))-o(3)
107 ELSEIF (idir == 2) THEN
108 p(4)=x(1,n2)+dt2*(v(1,n2)+dt12*a(1,n2))-o(1)
109 p(5)=x(2,n2)+dt2*(v(2,n2)+dt12*a(2,n2))-o(2)
110 p(6)=x(3,n2)+dt2*(v(3,n2)+dt12*a(3,n2))-o(3)
111 ELSEIF (idir == 3) THEN
112 p(7)=x(1,n2)+dt2*(v(1,n2)+dt12*a(1,n2))-o(1)
113 p(8)=x(2,n2)+dt2*(v(2,n2)+dt12*a(2,n2))-o(2)
114 p(9)=x(3,n2)+dt2*(v(3,n2)+dt12*a(3,n2))-o(3)
115 ENDIF
116 !calculation of Y0'(IDIR=1) Z0'(IDIR=2) X0'(IDIR=3)
117 IF (idir == 1) THEN
118 p(4)=x(1,n3)+dt2*(v(1,n3)+dt12*a(1,n3))-o(1)
119 p(5)=x(2,n3)+dt2*(v(2,n3)+dt12*a(2,n3))-o(2)
120 p(6)=x(3,n3)+dt2*(v(3,n3)+dt12*a(3,n3))-o(3)
121 ELSEIF (idir == 2) THEN
122 p(7)=x(1,n3)+dt2*(v(1,n3)+dt12*a(1,n3))-o(1)
123 p(8)=x(2,n3)+dt2*(v(2,n3)+dt12*a(2,n3))-o(2)
124 p(9)=x(3,n3)+dt2*(v(3,n3)+dt12*a(3,n3))-o(3)
125 ELSEIF (idir == 3) THEN
126 p(1)=x(1,n3)+dt2*(v(1,n3)+dt12*a(1,n3))-o(1)
127 p(2)=x(2,n3)+dt2*(v(2,n3)+dt12*a(2,n3))-o(2)
128 p(3)=x(3,n3)+dt2*(v(3,n3)+dt12*a(3,n3))-o(3)
129 ENDIF
130 ! if X'=0 then X'=X (IDIR=1)
131 ! if Y'=0 then Y'=Y (IDIR=2)
132 ! if Z'=0 then Z'=Z (IDIR=3)
133 IF (idir == 1) THEN
134 pp1=sqrt(p(1)*p(1)+p(2)*p(2)+p(3)*p(3))
135 IF(pp1 == zero)THEN
136 p(1)=one
137 p(2)=zero
138 p(3)=zero
139 pp1 =one
140 ENDIF
141 ELSEIF (idir == 2) THEN
142 pp1=sqrt(p(4)*p(4)+p(5)*p(5)+p(6)*p(6))
143 IF(pp1 == zero)THEN
144 p(4)=one
145 p(5)=zero
146 p(6)=zero
147 pp1 =one
148 ENDIF
149 ELSEIF (idir == 3) THEN
150 pp1=sqrt(p(7)*p(7)+p(8)*p(8)+p(9)*p(9))
151 IF(pp1==zero)THEN
152 p(7)=one
153 p(8)=zero
154 p(9)=zero
155 pp1 =one
156 ENDIF
157 ENDIF
158 ! calculation of Z'(IDIR=1) X'(IDIR=2) Y'(IDIR=3)
159 IF (idir == 1) THEN
160 p(7)=p(2)*p(6)-p(3)*p(5)
161 p(8)=p(3)*p(4)-p(1)*p(6)
162 p(9)=p(1)*p(5)-p(2)*p(4)
163 pp3=sqrt(p(7)*p(7)+p(8)*p(8)+p(9)*p(9))
164 ELSEIF (idir == 2) THEN
165 p(1)=p(5)*p(9)-p(6)*p(8)
166 p(2)=p(6)*p(7)-p(4)*p(9)
167 p(3)=p(4)*p(8)-p(5)*p(7)
168 pp3=sqrt(p(1)*p(1)+p(2)*p(2)+p(3)*p(3))
169 ELSEIF (idir == 3) THEN
170 p(4)=p(8)*p(3)-p(9)*p(2)
171 p(5)=p(9)*p(1)-p(7)*p(3)
172 p(6)=p(7)*p(2)-p(8)*p(1)
173 pp3=sqrt(p(4)*p(4)+p(5)*p(5)+p(6)*p(6))
174 ENDIF
175 ! if Z'=0 (Y0'=X') then Y0' != X' (IDIR=1)
176 ! if X'=0 (Z0'=Y') then Z0' != Y' (IDIR=2)
177 ! if Y'=0 (X0'=Z') then X0' != Z' (IDIR=3)
178 IF (idir == 1) THEN
179 IF(pp3 == zero)THEN
180 IF(p(1) == zero)THEN
181 p(4)=pp1
182 p(5)=p(2)
183 ELSE
184 p(4)=p(1)
185 p(5)=abs(p(2))+pp1
186 ENDIF
187 p(6)=p(3)
188 p(7)=p(2)*p(6)-p(3)*p(5)
189 p(8)=p(3)*p(4)-p(1)*p(6)
190 p(9)=p(1)*p(5)-p(2)*p(4)
191 pp3=sqrt(p(7)*p(7)+p(8)*p(8)+p(9)*p(9))
192 ENDIF
193 ELSEIF (idir == 2) THEN
194 IF(pp3 == zero)THEN
195 IF(p(4) == zero)THEN
196 p(7)=pp1
197 p(8)=p(5)
198 ELSE
199 p(7)=p(4)
200 p(8)=abs(p(5))+pp1
201 ENDIF
202 p(9)=p(6)
203 p(1)=p(5)*p(9)-p(6)*p(8)
204 p(2)=p(6)*p(7)-p(4)*p(9)
205 p(3)=p(4)*p(8)-p(5)*p(7)
206 pp3=sqrt(p(1)*p(1)+p(2)*p(2)+p(3)*p(3))
207 ENDIF
208 ELSEIF (idir == 3) THEN
209 IF(pp3 == zero)THEN
210 IF(p(7) == zero)THEN
211 p(1)=pp1
212 p(2)=p(8)
213 ELSE
214 p(1)=p(7)
215 p(2)=abs(p(8))+pp1
216 ENDIF
217 p(3)=p(9)
218 p(4)=p(8)*p(3)-p(9)*p(2)
219 p(5)=p(9)*p(1)-p(7)*p(3)
220 p(6)=p(7)*p(2)-p(8)*p(1)
221 pp3=sqrt(p(4)*p(4)+p(5)*p(5)+p(6)*p(6))
222 ENDIF
223 ENDIF
224 ! calculation of Y'(IDIR=1) Z'(IDIR=2) X'(IDIR=3)
225 IF (idir == 1) THEN
226 p(4)=p(8)*p(3)-p(9)*p(2)
227 p(5)=p(9)*p(1)-p(7)*p(3)
228 p(6)=p(7)*p(2)-p(8)*p(1)
229 pp2=sqrt(p(4)*p(4)+p(5)*p(5)+p(6)*p(6))
230 ELSEIF (idir == 2) THEN
231 p(7)=p(2)*p(6)-p(3)*p(5)
232 p(8)=p(3)*p(4)-p(1)*p(6)
233 p(9)=p(1)*p(5)-p(2)*p(4)
234 pp2=sqrt(p(7)*p(7)+p(8)*p(8)+p(9)*p(9))
235 ELSEIF (idir == 3) THEN
236 p(1)=p(5)*p(9)-p(6)*p(8)
237 p(2)=p(6)*p(7)-p(4)*p(9)
238 p(3)=p(4)*p(8)-p(5)*p(7)
239 pp2=sqrt(p(1)*p(1)+p(2)*p(2)+p(3)*p(3))
240 ENDIF
241C
242 ELSEIF (imov == 2) THEN
243 ! calculation of X0'
244 p(1)=x(1,n3)+dt2*(v(1,n3)+dt12*a(1,n3))-o(1)
245 p(2)=x(2,n3)+dt2*(v(2,n3)+dt12*a(2,n3))-o(2)
246 p(3)=x(3,n3)+dt2*(v(3,n3)+dt12*a(3,n3))-o(3)
247 ! calculation of Z'
248 p(7)=x(1,n2)+dt2*(v(1,n2)+dt12*a(1,n2))-o(1)
249 p(8)=x(2,n2)+dt2*(v(2,n2)+dt12*a(2,n2))-o(2)
250 p(9)=x(3,n2)+dt2*(v(3,n2)+dt12*a(3,n2))-o(3)
251 ! if Z'=0 then Z'=Z
252 pp3=sqrt(p(7)*p(7)+p(8)*p(8)+p(9)*p(9))
253 IF(pp3 == zero)THEN
254 p(7)=zero
255 p(8)=zero
256 p(9)=one
257 pp3 =one
258 ENDIF
259 ! calculation of Y = Z x X'
260 p(4)=p(8)*p(3)-p(9)*p(2)
261 p(5)=p(9)*p(1)-p(7)*p(3)
262 p(6)=p(7)*p(2)-p(8)*p(1)
263 pp2=sqrt(p(4)*p(4)+p(5)*p(5)+p(6)*p(6))
264 ! if Y'=0 (X0'=Z') then X0' != Y'
265 IF(pp2 == zero)THEN
266 IF(p(7) == zero)THEN
267 p(1)=pp3
268 p(2)=p(8)
269 ELSE
270 p(1)=p(7)
271 p(2)=abs(p(8))+pp3
272 ENDIF
273 p(3)=p(9)
274 p(4)=p(8)*p(3)-p(9)*p(2)
275 p(5)=p(9)*p(1)-p(7)*p(3)
276 p(6)=p(7)*p(2)-p(8)*p(1)
277 pp2=sqrt(p(4)*p(4)+p(5)*p(5)+p(6)*p(6))
278 ENDIF
279 ! calculation of X' = Y' x Z'
280 p(1)=p(5)*p(9)-p(6)*p(8)
281 p(2)=p(6)*p(7)-p(4)*p(9)
282 p(3)=p(4)*p(8)-p(5)*p(7)
283 pp1=sqrt(p(1)*p(1)+p(2)*p(2)+p(3)*p(3))
284 ENDIF
285 ! norm
286 p(1)=p(1)/pp1
287 p(2)=p(2)/pp1
288 p(3)=p(3)/pp1
289 p(4)=p(4)/pp2
290 p(5)=p(5)/pp2
291 p(6)=p(6)/pp2
292 p(7)=p(7)/pp3
293 p(8)=p(8)/pp3
294 p(9)=p(9)/pp3
295 ! ANGULAR VELOCITY at TIME T+1/2
296 ! rotation increment DR=Transpose( P Transpose(POLD) )
297 dr11=p(1)*pold(1)+p(4)*pold(4)+p(7)*pold(7)
298 dr21=p(1)*pold(2)+p(4)*pold(5)+p(7)*pold(8)
299 dr31=p(1)*pold(3)+p(4)*pold(6)+p(7)*pold(9)
300 dr12=p(2)*pold(1)+p(5)*pold(4)+p(8)*pold(7)
301 dr22=p(2)*pold(2)+p(5)*pold(5)+p(8)*pold(8)
302 dr32=p(2)*pold(3)+p(5)*pold(6)+p(8)*pold(9)
303 dr13=p(3)*pold(1)+p(6)*pold(4)+p(9)*pold(7)
304 dr23=p(3)*pold(2)+p(6)*pold(5)+p(9)*pold(8)
305 dr33=p(3)*pold(3)+p(6)*pold(6)+p(9)*pold(9)
306 ! rotation increment vector.
307 cs=(dr11+dr22+dr33-one)*half
308 IF (cs >= one) THEN
309 drx=(dr23-dr32)*half
310 dry=(dr31-dr13)*half
311 drz=(dr12-dr21)*half
312 ELSEIF (cs <= -one) THEN
313 drx=xpi*sqrt((dr11+one)*half)
314 dry=xpi*sqrt((dr22+one)*half)
315 drz=xpi*sqrt((dr33+one)*half)
316 IF (dr12 < zero .AND. dr23 < zero) THEN
317 dry=-dry
318 ELSEIF (dr23 < zero .AND. dr31 < zero) THEN
319 drz=-drz
320 ELSEIF (dr31 < zero .AND. dr12 < zero) THEN
321 drx=-drx
322 ELSEIF (dr12 < zero) THEN
323 drz=-drz
324 ELSEIF (dr23 < zero) THEN
325 drx=-drx
326 ELSEIF (dr31 < zero) THEN
327 dry=-dry
328 ENDIF
329 ELSE
330 r=acos(cs)
331 sn=sin(r)
332 rs2=r/(two*sn)
333 drx=(dr23-dr32)*rs2
334 dry=(dr31-dr13)*rs2
335 drz=(dr12-dr21)*rs2
336 ENDIF
337 vrx=drx/max(em20,dt2)
338 vry=dry/max(em20,dt2)
339 vrz=drz/max(em20,dt2)
340 ! RETRIEVE ANGULAR ACCELERATION AT TIME T
341 dtinv=one/dt12
342 xframe(16,n+1)=(vrx-xframe(13,n+1))*dtinv
343 xframe(17,n+1)=(vry-xframe(14,n+1))*dtinv
344 xframe(18,n+1)=(vrz-xframe(15,n+1))*dtinv
345 IF(nxframe >= 36)THEN
346 xframe(28,n+1)=a(1,n1)
347 xframe(29,n+1)=a(2,n1)
348 xframe(30,n+1)=a(3,n1)
349 ENDIF
350 ENDIF
351C
352 ENDDO !next N=1,NUMFRAM
353 ENDIF
354
355 ! SPMD : domain P0 sends data to other domains
356 IF(nspmd > 1) CALL spmd_rbcast(xframe,xframe,nxframe,numfram+1,0,2)
357
358 RETURN
my_real
#define my_real
Definition cppsort.cpp:32
max
#define max(a, b)
Definition macros.h:21
spmd_rbcast
subroutine spmd_rbcast(tabi, tabr, n1, n2, from, add)
Definition spmd_rbcast.F:62

◆ movfra2()

subroutine movfra2 ( dimension(nxframe,numfram+1), intent(inout) xframe,
integer, dimension(liskn,numfram+1), intent(in) iframe,
dimension(3,numnod), intent(in) x,
dimension(3,numnod), intent(in) v,
dimension(3,numnod), intent(in) vr,
dimension(3,numnod), intent(in) d )

Definition at line 370 of file movfram.F.

371C-----------------------------------------------
372C I m p l i c i t T y p e s
373C-----------------------------------------------
374#include "implicit_f.inc"
375C-----------------------------------------------
376C C o m m o n B l o c k s
377C-----------------------------------------------
378#include "com01_c.inc"
379#include "com04_c.inc"
380#include "com08_c.inc"
381#include "param_c.inc"
382#include "task_c.inc"
383C-----------------------------------------------
384C D u m m y A r g u m e n t s
385C-----------------------------------------------
386 INTEGER,intent(IN) :: IFRAME(LISKN,NUMFRAM+1)
387 my_real, intent(INOUT) :: xframe(nxframe,numfram+1)
388 my_real, intent(IN) :: x(3,numnod), v(3,numnod), vr(3,numnod), d(3,numnod)
389C-----------------------------------------------
390C L o c a l V a r i a b l e s
391C-----------------------------------------------
392 INTEGER N, N1, N2, N3, K, IMOV, IDIR
393 my_real o(3), p(9), pp1, pp3, pp2, pold(9),vrn(3),
394 .
395 .
396 .
397 . xpi, vrx, vry, vrz
398C-----------------------------------------------
399C S o u r c e L i n e s
400C-----------------------------------------------
401 pp1 = zero
402 pp2 = zero
403 pp3 = zero
404 p(1:9) = zero
405 IF(ispmd==0) THEN
406 xpi=pi
407 DO n=1,numfram
408 n1=iframe(1,n+1)
409 n2=iframe(2,n+1)
410 n3=iframe(3,n+1)
411 imov=iframe(5,n+1)
412 ! reference frame mobile
413 IF(n1+n2+n3==0)cycle
414 ! extract frame orientation wrt global system at time t-1.
415 pold(1)=xframe(1,n+1)
416 pold(2)=xframe(2,n+1)
417 pold(3)=xframe(3,n+1)
418 pold(4)=xframe(4,n+1)
419 pold(5)=xframe(5,n+1)
420 pold(6)=xframe(6,n+1)
421 pold(7)=xframe(7,n+1)
422 pold(8)=xframe(8,n+1)
423 pold(9)=xframe(9,n+1)
424 IF (n2+n3==0) THEN
425 ! Node defined frame
426 vrn(1)=pold(1)*vr(1,n1)+pold(2)*vr(2,n1)+pold(3)*vr(3,n1)
427 vrn(2)=pold(4)*vr(1,n1)+pold(5)*vr(2,n1)+pold(6)*vr(3,n1)
428 vrn(3)=pold(7)*vr(1,n1)+pold(8)*vr(2,n1)+pold(9)*vr(3,n1)
429 CALL rotbmr (vrn ,pold ,dt2)
430 DO k=1,9
431 xframe(k,n+1)=pold(k)
432 ENDDO
433 xframe(10,n+1)=x(1,n1)
434 xframe(11,n+1)=x(2,n1)
435 xframe(12,n+1)=x(3,n1)
436 xframe(13,n+1)=vr(1,n1)
437 xframe(14,n+1)=vr(2,n1)
438 xframe(15,n+1)=vr(3,n1)
439 IF(nxframe>=36)THEN
440 xframe(31,n+1)=v(1,n1)
441 xframe(32,n+1)=v(2,n1)
442 xframe(33,n+1)=v(3,n1)
443 xframe(34,n+1)=d(1,n1)
444 xframe(35,n+1)=d(2,n1)
445 xframe(36,n+1)=d(3,n1)
446 ENDIF
447 ELSE
448 ! MOVING FRAME
449 ! retrieve position & orientation wrt global system at time t
450 o(1)=x(1,n1)
451 o(2)=x(2,n1)
452 o(3)=x(3,n1)
453 IF (imov == 1) THEN
454 idir = iframe(6,n+1)
455 ! calculation of X'(IDIR=1) Y'(IDIR=2) Z'(IDIR=3)
456 IF (idir == 1) THEN
457 p(1)=x(1,n2)-x(1,n1)
458 p(2)=x(2,n2)-x(2,n1)
459 p(3)=x(3,n2)-x(3,n1)
460 ELSEIF (idir == 2) THEN
461 p(4)=x(1,n2)-x(1,n1)
462 p(5)=x(2,n2)-x(2,n1)
463 p(6)=x(3,n2)-x(3,n1)
464 ELSEIF (idir == 3) THEN
465 p(7)=x(1,n2)-x(1,n1)
466 p(8)=x(2,n2)-x(2,n1)
467 p(9)=x(3,n2)-x(3,n1)
468 ENDIF
469 ! calculation of Y0'(IDIR=1) Z0'(IDIR=2) X0'(IDIR=3)
470 IF (idir == 1) THEN
471 p(4)=x(1,n3)-x(1,n1)
472 p(5)=x(2,n3)-x(2,n1)
473 p(6)=x(3,n3)-x(3,n1)
474 ELSEIF (idir == 2) THEN
475 p(7)=x(1,n3)-x(1,n1)
476 p(8)=x(2,n3)-x(2,n1)
477 p(9)=x(3,n3)-x(3,n1)
478 ELSEIF (idir == 3) THEN
479 p(1)=x(1,n3)-x(1,n1)
480 p(2)=x(2,n3)-x(2,n1)
481 p(3)=x(3,n3)-x(3,n1)
482 ENDIF
483 ! if X'=0 then X'=X(IDIR=1)
484 ! if Y'=0 then Y'=Y(IDIR=2)
485 ! if Z'=0 then Z'=Z(IDIR=3)
486 IF (idir == 1) THEN
487 pp1=sqrt(p(1)*p(1)+p(2)*p(2)+p(3)*p(3))
488 IF(pp1 == zero)THEN
489 p(1)=one
490 p(2)=zero
491 p(3)=zero
492 pp1 =one
493 ENDIF
494 ELSEIF (idir == 2) THEN
495 pp1=sqrt(p(4)*p(4)+p(5)*p(5)+p(6)*p(6))
496 IF(pp1 == zero)THEN
497 p(4)=one
498 p(5)=zero
499 p(6)=zero
500 pp1 =one
501 ENDIF
502 ELSEIF (idir == 3) THEN
503 pp1=sqrt(p(7)*p(7)+p(8)*p(8)+p(9)*p(9))
504 IF(pp1 == zero)THEN
505 p(7)=one
506 p(8)=zero
507 p(9)=zero
508 pp1 =one
509 ENDIF
510 ENDIF
511 ! calculation of Z'(IDIR=1) DE X'(IDIR=2) DE Y'(IDIR=3)
512 IF (idir == 1) THEN
513 p(7)=p(2)*p(6)-p(3)*p(5)
514 p(8)=p(3)*p(4)-p(1)*p(6)
515 p(9)=p(1)*p(5)-p(2)*p(4)
516 pp3=sqrt(p(7)*p(7)+p(8)*p(8)+p(9)*p(9))
517 ELSEIF (idir == 2) THEN
518 p(1)=p(5)*p(9)-p(6)*p(8)
519 p(2)=p(6)*p(7)-p(4)*p(9)
520 p(3)=p(4)*p(8)-p(5)*p(7)
521 pp3=sqrt(p(1)*p(1)+p(2)*p(2)+p(3)*p(3))
522 ELSEIF (idir == 3) THEN
523 p(4)=p(8)*p(3)-p(9)*p(2)
524 p(5)=p(9)*p(1)-p(7)*p(3)
525 p(6)=p(7)*p(2)-p(8)*p(1)
526 pp3=sqrt(p(4)*p(4)+p(5)*p(5)+p(6)*p(6))
527 ENDIF
528 ! if Z'=0 (Y0'=X') then Y0' != X'(IDIR=1)
529 ! if X'=0 (Z0'=Y') then Z0' != Y'(IDIR=2)
530 ! if Y'=0 (X0'=Z') then X0' != Z'(IDIR=3)
531 IF (idir == 1) THEN
532 IF(pp3 == zero)THEN
533 IF(p(1) == zero)THEN
534 p(4)=pp1
535 p(5)=p(2)
536 ELSE
537 p(4)=p(1)
538 p(5)=abs(p(2))+pp1
539 ENDIF
540 p(6)=p(3)
541 p(7)=p(2)*p(6)-p(3)*p(5)
542 p(8)=p(3)*p(4)-p(1)*p(6)
543 p(9)=p(1)*p(5)-p(2)*p(4)
544 pp3=sqrt(p(7)*p(7)+p(8)*p(8)+p(9)*p(9))
545 ENDIF
546 ELSEIF (idir == 2) THEN
547 IF(pp3 == zero)THEN
548 IF(p(4) == zero)THEN
549 p(7)=pp1
550 p(8)=p(5)
551 ELSE
552 p(7)=p(1)
553 p(8)=abs(p(5))+pp1
554 ENDIF
555 p(9)=p(6)
556 p(1)=p(5)*p(9)-p(6)*p(8)
557 p(2)=p(6)*p(7)-p(4)*p(9)
558 p(3)=p(4)*p(8)-p(5)*p(7)
559 pp3=sqrt(p(1)*p(1)+p(2)*p(2)+p(3)*p(3))
560 ENDIF
561 ELSEIF (idir == 3) THEN
562 IF(pp3 == zero)THEN
563 IF(p(7) == zero)THEN
564 p(1)=pp1
565 p(2)=p(8)
566 ELSE
567 p(1)=p(4)
568 p(2)=abs(p(8))+pp1
569 ENDIF
570 p(3)=p(9)
571 p(4)=p(8)*p(3)-p(9)*p(2)
572 p(5)=p(9)*p(1)-p(7)*p(3)
573 p(6)=p(7)*p(2)-p(8)*p(1)
574 pp3=sqrt(p(4)*p(4)+p(5)*p(5)+p(6)*p(6))
575 ENDIF
576 ENDIF
577 ! calculation of Y'(IDIR=1) Z'(IDIR=2) X'(IDIR=3)
578 IF (idir == 1) THEN
579 p(4)=p(8)*p(3)-p(9)*p(2)
580 p(5)=p(9)*p(1)-p(7)*p(3)
581 p(6)=p(7)*p(2)-p(8)*p(1)
582 pp2=sqrt(p(4)*p(4)+p(5)*p(5)+p(6)*p(6))
583 ELSEIF (idir == 2) THEN
584 p(7)=p(2)*p(6)-p(3)*p(5)
585 p(8)=p(3)*p(4)-p(1)*p(6)
586 p(9)=p(1)*p(5)-p(2)*p(4)
587 pp2=sqrt(p(7)*p(7)+p(8)*p(8)+p(9)*p(9))
588 ELSEIF (idir == 3) THEN
589 p(1)=p(5)*p(9)-p(6)*p(8)
590 p(2)=p(6)*p(7)-p(4)*p(9)
591 p(3)=p(4)*p(8)-p(5)*p(7)
592 pp2=sqrt(p(1)*p(1)+p(2)*p(2)+p(3)*p(3))
593 ENDIF
594
595 ELSEIF (imov == 2) THEN
596 ! calculation of X0'
597 p(1)=x(1,n3)-x(1,n1)
598 p(2)=x(2,n3)-x(2,n1)
599 p(3)=x(3,n3)-x(3,n1)
600 ! calculation of Z'
601 p(7)=x(1,n2)-x(1,n1)
602 p(8)=x(2,n2)-x(2,n1)
603 p(9)=x(3,n2)-x(3,n1)
604 ! if Z'=0 then Z'=Z
605 pp3=sqrt(p(7)*p(7)+p(8)*p(8)+p(9)*p(9))
606 IF(pp3==zero)THEN
607 p(7)=zero
608 p(8)=zero
609 p(9)=one
610 pp3 =one
611 ENDIF
612 ! calculation of Y' = Z' x X0'
613 p(4)=p(8)*p(3)-p(9)*p(2)
614 p(5)=p(9)*p(1)-p(7)*p(3)
615 p(6)=p(7)*p(2)-p(8)*p(1)
616 pp2=sqrt(p(4)*p(4)+p(5)*p(5)+p(6)*p(6))
617 ! if Y'=0 (X0'=Z') then X0' != Y'
618 IF(pp2 == zero)THEN
619 IF(p(7) == zero)THEN
620 p(1)=pp3
621 p(2)=p(8)
622 ELSE
623 p(1)=p(7)
624 p(2)=abs(p(8))+pp3
625 ENDIF
626 p(3)=p(9)
627 p(4)=p(8)*p(3)-p(9)*p(2)
628 p(5)=p(9)*p(1)-p(7)*p(3)
629 p(6)=p(7)*p(2)-p(8)*p(1)
630 pp2=sqrt(p(4)*p(4)+p(5)*p(5)+p(6)*p(6))
631 ENDIF
632 ! calculation of X' = Y' x Z'
633 p(1)=p(5)*p(9)-p(6)*p(8)
634 p(2)=p(6)*p(7)-p(4)*p(9)
635 p(3)=p(4)*p(8)-p(5)*p(7)
636 pp1=sqrt(p(1)*p(1)+p(2)*p(2)+p(3)*p(3))
637 ENDIF
638 ! NORM
639 p(1)=p(1)/pp1
640 p(2)=p(2)/pp1
641 p(3)=p(3)/pp1
642 p(4)=p(4)/pp2
643 p(5)=p(5)/pp2
644 p(6)=p(6)/pp2
645 p(7)=p(7)/pp3
646 p(8)=p(8)/pp3
647 p(9)=p(9)/pp3
648 DO k=1,9
649 xframe(k,n+1)=p(k)
650 ENDDO
651 xframe(10,n+1)=o(1)
652 xframe(11,n+1)=o(2)
653 xframe(12,n+1)=o(3)
654 IF(nxframe >= 36)THEN
655 xframe(31,n+1)=v(1,n1)
656 xframe(32,n+1)=v(2,n1)
657 xframe(33,n+1)=v(3,n1)
658 xframe(34,n+1)=d(1,n1)
659 xframe(35,n+1)=d(2,n1)
660 xframe(36,n+1)=d(3,n1)
661 ENDIF
662 ! retrieve ANGULAR VELOCITY at TIME T-1/2
663 vrx=xframe(13,n+1)+dt12*xframe(16,n+1)
664 vry=xframe(14,n+1)+dt12*xframe(17,n+1)
665 vrz=xframe(15,n+1)+dt12*xframe(18,n+1)
666 xframe(13,n+1)=vrx
667 xframe(14,n+1)=vry
668 xframe(15,n+1)=vrz
669 ENDIF
670 ENDDO ! next N=1,NUMFRAM
671 ENDIF
672
673 ! SPMD : domain P0 sends data to other domains
674 IF(nspmd > 1) CALL spmd_rbcast(xframe,xframe,nxframe,numfram+1,0,2)
675
676 RETURN
rotbmr
subroutine rotbmr(vr, rby, dt)
Definition rotbmr.F:35

◆ movfra_imp()

subroutine movfra_imp ( dimension(nxframe,numfram+1), intent(inout) xframe,
integer, dimension(liskn,numfram+1), intent(in) iframe,
dimension(3,numnod), intent(in) x,
dimension(3,numnod), intent(in) v,
dimension(3,numnod), intent(in) a,
dimension(3,numnod), intent(in) vr,
dimension(3,numnod), intent(in) ar,
dimension(3,numnod), intent(in) d )

Definition at line 687 of file movfram.F.

688C-----------------------------------------------
689C I m p l i c i t T y p e s
690C-----------------------------------------------
691#include "implicit_f.inc"
692C-----------------------------------------------
693C C o m m o n B l o c k s
694C-----------------------------------------------
695#include "com01_c.inc"
696#include "com04_c.inc"
697#include "com08_c.inc"
698#include "param_c.inc"
699#include "task_c.inc"
700#include "impl1_c.inc"
701C-----------------------------------------------
702C D u m m y A r g u m e n t s
703C-----------------------------------------------
704 INTEGER, intent(in) :: IFRAME(LISKN,NUMFRAM+1)
705 my_real, intent(INOUT) :: xframe(nxframe,numfram+1)
706 my_real, intent(IN) :: x(3,numnod), v(3,numnod), a(3,numnod), vr(3,numnod), ar(3,numnod), d(3,numnod)
707C-----------------------------------------------
708C L o c a l V a r i a b l e s
709C-----------------------------------------------
710 INTEGER N, N1, N2, N3, K, IMOV
711 my_real o(3), p(9), pp1, pp3, pp2, pold(9),
712 . dr11, dr12, dr13,
713 . dr21, dr22, dr23,
714 . dr31, dr32, dr33,
715 . xpi, cs, r, sn, rs2, drx, dry, drz, vrx, vry, vrz, dtinv, vrn(3)
716C-----------------------------------------------
717C S o u r c e L i n e s
718C-----------------------------------------------
719 pp1 = zero
720 pp2 = zero
721 pp3 = zero
722 p(1:9) = zero
723 IF(ispmd == 0) THEN
724 xpi=pi
725 DO n=1,numfram
726 n1=iframe(1,n+1)
727 n2=iframe(2,n+1)
728 n3=iframe(3,n+1)
729 imov=iframe(5,n+1)
730 IF(n1+n2+n3 == 0) THEN
731 ! Fixed Frame
732 ELSEIF (n2+n3 == 0) THEN
733 ! Node defined frame
734 IF (inconv == 1) THEN
735 pold(1)=xframe(1,n+1)
736 pold(2)=xframe(2,n+1)
737 pold(3)=xframe(3,n+1)
738 pold(4)=xframe(4,n+1)
739 pold(5)=xframe(5,n+1)
740 pold(6)=xframe(6,n+1)
741 pold(7)=xframe(7,n+1)
742 pold(8)=xframe(8,n+1)
743 pold(9)=xframe(9,n+1)
744 vrn(1)=pold(1)*vr(1,n1)+pold(2)*vr(2,n1)+pold(3)*vr(3,n1)
745 vrn(2)=pold(4)*vr(1,n1)+pold(5)*vr(2,n1)+pold(6)*vr(3,n1)
746 vrn(3)=pold(7)*vr(1,n1)+pold(8)*vr(2,n1)+pold(9)*vr(3,n1)
747 CALL rotbmr (vrn ,pold ,dt2)
748 DO k=1,9
749 xframe(k,n+1)=pold(k)
750 ENDDO
751 END IF !(INCONV==1) THEN
752 xframe(10,n+1)=x(1,n1)
753 xframe(11,n+1)=x(2,n1)
754 xframe(12,n+1)=x(3,n1)
755 xframe(13,n+1)=vr(1,n1)
756 xframe(14,n+1)=vr(2,n1)
757 xframe(15,n+1)=vr(3,n1)
758 xframe(16,n+1)=ar(1,n1)
759 xframe(17,n+1)=ar(2,n1)
760 xframe(18,n+1)=ar(3,n1)
761 IF(nxframe >= 36)THEN
762 xframe(28,n+1)=a(1,n1)
763 xframe(29,n+1)=a(2,n1)
764 xframe(30,n+1)=a(3,n1)
765 xframe(31,n+1)=v(1,n1)
766 xframe(32,n+1)=v(2,n1)
767 xframe(33,n+1)=v(3,n1)
768 xframe(34,n+1)=d(1,n1)
769 xframe(35,n+1)=d(2,n1)
770 xframe(36,n+1)=d(3,n1)
771 ENDIF
772 ELSE
773 ! Moving frame frame
774 ! extract frame orientation wrt global system at time t
775 pold(1)=xframe(1,n+1)
776 pold(2)=xframe(2,n+1)
777 pold(3)=xframe(3,n+1)
778 pold(4)=xframe(4,n+1)
779 pold(5)=xframe(5,n+1)
780 pold(6)=xframe(6,n+1)
781 pold(7)=xframe(7,n+1)
782 pold(8)=xframe(8,n+1)
783 pold(9)=xframe(9,n+1)
784 ! position & orientation wrt global system at time t+1
785 o(1)=x(1,n1)
786 o(2)=x(2,n1)
787 o(3)=x(3,n1)
788 IF (imov == 1) THEN
789 ! calculation of X'
790 p(1)=x(1,n2)-o(1)
791 p(2)=x(2,n2)-o(2)
792 p(3)=x(3,n2)-o(3)
793 ! calculation of Y0'
794 p(4)=x(1,n3)-o(1)
795 p(5)=x(2,n3)-o(2)
796 p(6)=x(3,n3)-o(3)
797 ! if X'=0 then X'=X
798 pp1=sqrt(p(1)*p(1)+p(2)*p(2)+p(3)*p(3))
799 IF(pp1 == zero)THEN
800 p(1)=one
801 p(2)=zero
802 p(3)=zero
803 pp1 =one
804 ENDIF
805 ! calculation of Z'
806 p(7)=p(2)*p(6)-p(3)*p(5)
807 p(8)=p(3)*p(4)-p(1)*p(6)
808 p(9)=p(1)*p(5)-p(2)*p(4)
809 pp3=sqrt(p(7)*p(7)+p(8)*p(8)+p(9)*p(9))
810 ! if Z'=0 (Y0'=X') then Y0' != X'
811 IF(pp3 == zero)THEN
812 IF(p(1) == zero)THEN
813 p(4)=pp1
814 p(5)=p(2)
815 ELSE
816 p(4)=p(1)
817 p(5)=abs(p(2))+pp1
818 ENDIF
819 p(6)=p(3)
820 p(7)=p(2)*p(6)-p(3)*p(5)
821 p(8)=p(3)*p(4)-p(1)*p(6)
822 p(9)=p(1)*p(5)-p(2)*p(4)
823 pp3=sqrt(p(7)*p(7)+p(8)*p(8)+p(9)*p(9))
824 ENDIF
825 ! calculation of Y'
826 p(4)=p(8)*p(3)-p(9)*p(2)
827 p(5)=p(9)*p(1)-p(7)*p(3)
828 p(6)=p(7)*p(2)-p(8)*p(1)
829 pp2=sqrt(p(4)*p(4)+p(5)*p(5)+p(6)*p(6))
830
831 ELSEIF (imov == 2) THEN
832 ! calculation of X0'
833 p(1)=x(1,n3)-o(1)
834 p(2)=x(2,n3)-o(2)
835 p(3)=x(3,n3)-o(3)
836 ! calculation of Z'
837 p(7)=x(1,n2)-o(1)
838 p(8)=x(2,n2)-o(2)
839 p(9)=x(3,n2)-o(3)
840 ! if Z'=0 then Z'=Z
841 pp3=sqrt(p(7)*p(7)+p(8)*p(8)+p(9)*p(9))
842 IF(pp3 == zero)THEN
843 p(7)=zero
844 p(8)=zero
845 p(9)=one
846 pp3 =one
847 ENDIF
848 ! calculation of Y=ZxX'
849 p(4)=p(8)*p(3)-p(9)*p(2)
850 p(5)=p(9)*p(1)-p(7)*p(3)
851 p(6)=p(7)*p(2)-p(8)*p(1)
852 pp2=sqrt(p(4)*p(4)+p(5)*p(5)+p(6)*p(6))
853 ! if Y'=0 (X0'=Z') then X0' != Y'
854 IF(pp2 == zero)THEN
855 IF(p(7)==zero)THEN
856 p(1)=pp3
857 p(2)=p(8)
858 ELSE
859 p(1)=p(7)
860 p(2)=abs(p(8))+pp3
861 ENDIF
862 p(3)=p(9)
863 p(4)=p(8)*p(3)-p(9)*p(2)
864 p(5)=p(9)*p(1)-p(7)*p(3)
865 p(6)=p(7)*p(2)-p(8)*p(1)
866 pp2=sqrt(p(4)*p(4)+p(5)*p(5)+p(6)*p(6))
867 ENDIF
868 ! calculation of X' = Y' x Z'
869 p(1)=p(5)*p(9)-p(6)*p(8)
870 p(2)=p(6)*p(7)-p(4)*p(9)
871 p(3)=p(4)*p(8)-p(5)*p(7)
872 pp1=sqrt(p(1)*p(1)+p(2)*p(2)+p(3)*p(3))
873 ENDIF
874 ! norm
875 p(1)=p(1)/pp1
876 p(2)=p(2)/pp1
877 p(3)=p(3)/pp1
878 p(4)=p(4)/pp2
879 p(5)=p(5)/pp2
880 p(6)=p(6)/pp2
881 p(7)=p(7)/pp3
882 p(8)=p(8)/pp3
883 p(9)=p(9)/pp3
884 IF (inconv == 1) THEN
885 xframe(10,n+1)=x(1,n1)
886 xframe(11,n+1)=x(2,n1)
887 xframe(12,n+1)=x(3,n1)
888 DO k=1,9
889 xframe(k,n+1)=p(k)
890 ENDDO
891 END IF !(INCONV==1) THEN
892 ! angular velocity at time t+1/2
893 ! rotation increment DR=Transpose( P Transpose(POLD) )
894 dr11=p(1)*pold(1)+p(4)*pold(4)+p(7)*pold(7)
895 dr21=p(1)*pold(2)+p(4)*pold(5)+p(7)*pold(8)
896 dr31=p(1)*pold(3)+p(4)*pold(6)+p(7)*pold(9)
897 dr12=p(2)*pold(1)+p(5)*pold(4)+p(8)*pold(7)
898 dr22=p(2)*pold(2)+p(5)*pold(5)+p(8)*pold(8)
899 dr32=p(2)*pold(3)+p(5)*pold(6)+p(8)*pold(9)
900 dr13=p(3)*pold(1)+p(6)*pold(4)+p(9)*pold(7)
901 dr23=p(3)*pold(2)+p(6)*pold(5)+p(9)*pold(8)
902 dr33=p(3)*pold(3)+p(6)*pold(6)+p(9)*pold(9)
903 ! rotation increment vector.
904 cs=(dr11+dr22+dr33-one)*half
905 IF (cs >= one) THEN
906 drx=(dr23-dr32)*half
907 dry=(dr31-dr13)*half
908 drz=(dr12-dr21)*half
909 ELSEIF (cs <= -one) THEN
910 drx=xpi*sqrt((dr11+one)*half)
911 dry=xpi*sqrt((dr22+one)*half)
912 drz=xpi*sqrt((dr33+one)*half)
913 IF (dr12 < zero .AND. dr23 < zero) THEN
914 dry=-dry
915 ELSEIF (dr23<zero.AND.dr31 < zero) THEN
916 drz=-drz
917 ELSEIF (dr31 < zero .AND. dr12 < zero) THEN
918 drx=-drx
919 ELSEIF (dr12 < zero) THEN
920 drz=-drz
921 ELSEIF (dr23 < zero) THEN
922 drx=-drx
923 ELSEIF (dr31 < zero) THEN
924 dry=-dry
925 ENDIF
926 ELSE
927 r=acos(cs)
928 sn=sin(r)
929 rs2=r/(two*sn)
930 drx=(dr23-dr32)*rs2
931 dry=(dr31-dr13)*rs2
932 drz=(dr12-dr21)*rs2
933 ENDIF
934
935 vrx=drx/max(em20,dt2)
936 vry=dry/max(em20,dt2)
937 vrz=drz/max(em20,dt2)
938
939 xframe(13,n+1)=vrx
940 xframe(14,n+1)=vry
941 xframe(15,n+1)=vrz
942
943 ! retrieve angular acceleration at time t
944 dtinv=one/dt12
945 xframe(16,n+1)=(vrx-xframe(13,n+1))*dtinv
946 xframe(17,n+1)=(vry-xframe(14,n+1))*dtinv
947 xframe(18,n+1)=(vrz-xframe(15,n+1))*dtinv
948 IF(nxframe >= 36)THEN
949 xframe(28,n+1)=a(1,n1)
950 xframe(29,n+1)=a(2,n1)
951 xframe(30,n+1)=a(3,n1)
952 xframe(31,n+1)=v(1,n1)
953 xframe(32,n+1)=v(2,n1)
954 xframe(33,n+1)=v(3,n1)
955 xframe(34,n+1)=d(1,n1)
956 xframe(35,n+1)=d(2,n1)
957 xframe(36,n+1)=d(3,n1)
958 ENDIF
959 ENDIF !(N1+N2+N3==0)
960
961 ENDDO !next N=1,NUMFRAM
962 ENDIF
963
964 ! SPMD : domain P0 sends data to other domains
965 IF(nspmd > 1) CALL spmd_rbcast(xframe,xframe,nxframe,numfram+1,0,2)
966
967 RETURN