-
Notifications
You must be signed in to change notification settings - Fork 14
/
Copy pathquaternion.f
584 lines (490 loc) · 24.6 KB
/
quaternion.f
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
********************************************************************************
** FICHE F.5. QUATERNION PARAMETER PREDICTOR-CORRECTOR ALGORITHM **
** This FORTRAN code is intended to illustrate points made in the text. **
** To our knowledge it works correctly. However it is the responsibility of **
** the user to test it, if it is to be used in a research application. **
********************************************************************************
C *******************************************************************
C ** RIGID MOLECULE ROTATION USING QUATERNION PREDICTOR-CORRECTOR. **
C ** **
C ** REFERENCES: **
C ** **
C ** EVANS AND MURAD, MOLEC. PHYS. 34, 327, 1977. **
C ** GEAR, NUMERICAL INITIAL VALUE PROBLEMS IN ORDINARY **
C ** DIFFERENTIAL EQUATIONS, PRENTICE-HALL, 1971. **
C ** **
C ** SUPPLIED ROUTINES: **
C ** **
C ** SUBROUTINE PREDIC ( DT ) **
C ** PREDICTS POSITIONS, VELOCITIES ETC. AT NEXT STEP. **
C ** SUBROUTINE MOLATM **
C ** CONVERTS MOLECULAR COORDINATES TO ATOM POSITIONS. **
C ** SUBROUTINE ATMMOL **
C ** CONVERTS ATOMIC FORCES TO MOLECULAR FORCES AND TORQUES. **
C ** SUBROUTINE CORREC ( DT, M, IXX, IYY, IZZ, K ) **
C ** CORRECTS POSITIONS, VELOCITIES ETC. **
C ** **
C ** PRINCIPAL VARIABLES: **
C ** **
C ** INTEGER N NUMBER OF MOLECULES **
C ** REAL DT TIMESTEP **
C ** REAL M MOLECULAR MASS **
C ** REAL IXX,IYY,IZZ PRINCIPAL INERTIAS **
C ** REAL K KINETIC ENERGY **
C ** REAL RX (N),RY (N),RZ (N) C-O-M POSITIONS **
C ** REAL RX1(N),RY1(N),RZ1(N) FIRST DERIVATIVES **
C ** REAL RX2(N),RY2(N),RZ2(N) SECOND DERIVATIVES **
C ** REAL RX3(N),RY3(N),RZ3(N) THIRD DERIVATIVES **
C ** REAL FX (N),FY (N),FZ (N) TOTAL FORCES **
C ** REAL QW (N),QX (N),QY (N),QZ (N) QUATERNION PARAMETERS **
C ** REAL QW1(N),QX1(N),QY1(N),QZ1(N) FIRST DERIVATIVES **
C ** REAL QW2(N),QX2(N),QY2(N),QZ2(N) SECOND DERIVATIVES **
C ** REAL QW3(N),QX3(N),QY3(N),QZ3(N) THIRD DERIVATIVES **
C ** REAL QW4(N),QX4(N),QY4(N),QZ4(N) FOURTH DERIVATIVES **
C ** REAL OX (N),OY (N),OZ (N) ANGULAR VELOCITIES **
C ** REAL OX1(N),OY1(N),OZ1(N) FIRST DERIVATIVES **
C ** REAL OX2(N),OY2(N),OZ2(N) SECOND DERIVATIVES **
C ** REAL OX3(N),OY3(N),OZ3(N) THIRD DERIVATIVES **
C ** REAL OX4(N),OY4(N),OZ4(N) FOURTH DERIVATIVES **
C ** REAL TX (N),TY (N),TZ (N) TOTAL TORQUES **
C ** **
C ** USAGE: **
C ** **
C ** THE PREDICTOR ROUTINE IS CALLED TO ADVANCE THE POSITIONS, **
C ** QUATERNIONS, AND ANGULAR VELOCITIES GIVEN THE CURRENT VALUES **
C ** OF THESE QUANTITIES AND THEIR SUCCESSIVE TIME DERIVATIVES. **
C ** FOLLOWING THIS, SUBROUTINE MOLATM USES THE QUATERNIONS TO **
C ** OBTAIN THE ATOM OR SITE POSITIONS RSX,RSY,RSZ. THESE ARE FED **
C ** INTO A FORCE ROUTINE (NOT SUPPLIED HERE) WHICH GIVES THE **
C ** FORCE ACTING ON EACH SITE OR ATOM. IN TURN, THESE ARE **
C ** CONVERTED INTO THE TOTAL FORCE AND TORQUE ACTING ON EACH **
C ** MOLECULE, BY SUBROUTINE ATMMOL, AND THESE ARE USED IN THE **
C ** CORRECTOR STAGE. **
C *******************************************************************
SUBROUTINE PREDIC ( DT )
COMMON / BLOCK1 / RX , RY , RZ , RX1, RY1, RZ1,
: RX2, RY2, RZ2, RX3, RY3, RZ3,
: FX , FY , FZ
COMMON / BLOCK2 / QW , QX , QY , QZ , QW1, QX1, QY1, QZ1,
: QW2, QX2, QY2, QZ2, QW3, QX3, QY3, QZ3,
: QW4, QX4, QY4, QZ4,
: OX , OY , OZ , OX1, OY1, OZ1,
: OX2, OY2, OZ2, OX3, OY3, OZ3,
: OX4, OY4, OZ4, TX, TY, TZ
C *******************************************************************
C ** PREDICTOR ROUTINE **
C ** **
C ** WE ADOPT A 5-VALUE METHOD FOR REORIENTATIONAL VARIABLES **
C ** EMPLOYING BODY-FIXED ANGULAR VELOCITIES AND QUATERNIONS, **
C ** AND A 4-VALUE METHOD FOR CENTRE-OF-MASS (C-O-M) TRANSLATION. **
C ** THE PREDICTOR STAGE IS A SIMPLE TAYLOR SERIES. **
C *******************************************************************
INTEGER N
PARAMETER ( N = 108 )
REAL DT
REAL RX (N), RY (N), RZ (N)
REAL RX1(N), RY1(N), RZ1(N)
REAL RX2(N), RY2(N), RZ2(N)
REAL RX3(N), RY3(N), RZ3(N)
REAL FX (N), FY (N), FZ (N)
REAL QW (N), QX (N), QY (N), QZ (N)
REAL QW1(N), QX1(N), QY1(N), QZ1(N)
REAL QW2(N), QX2(N), QY2(N), QZ2(N)
REAL QW3(N), QX3(N), QY3(N), QZ3(N)
REAL QW4(N), QX4(N), QY4(N), QZ4(N)
REAL OX (N), OY (N), OZ (N)
REAL OX1(N), OY1(N), OZ1(N)
REAL OX2(N), OY2(N), OZ2(N)
REAL OX3(N), OY3(N), OZ3(N)
REAL OX4(N), OY4(N), OZ4(N)
REAL TX (N), TY (N), TZ (N)
INTEGER I
REAL C1, C2, C3, C4
C *******************************************************************
C1 = DT
C2 = C1 * DT / 2.0
C3 = C2 * DT / 3.0
C4 = C3 * DT / 4.0
DO 100 I = 1, N
RX (I) = RX (I) + C1*RX1(I) + C2*RX2(I) + C3*RX3(I)
RY (I) = RY (I) + C1*RY1(I) + C2*RY2(I) + C3*RY3(I)
RZ (I) = RZ (I) + C1*RZ1(I) + C2*RZ2(I) + C3*RZ3(I)
RX1(I) = RX1(I) + C1*RX2(I) + C2*RX3(I)
RY1(I) = RY1(I) + C1*RY2(I) + C2*RY3(I)
RZ1(I) = RZ1(I) + C1*RZ2(I) + C2*RZ3(I)
RX2(I) = RX2(I) + C1*RX3(I)
RY2(I) = RY2(I) + C1*RY3(I)
RZ2(I) = RZ2(I) + C1*RZ3(I)
QW(I) = QW(I) + C1*QW1(I) + C2*QW2(I) + C3*QW3(I) + C4*QW4(I)
QX(I) = QX(I) + C1*QX1(I) + C2*QX2(I) + C3*QX3(I) + C4*QX4(I)
QY(I) = QY(I) + C1*QY1(I) + C2*QY2(I) + C3*QY3(I) + C4*QY4(I)
QZ(I) = QZ(I) + C1*QZ1(I) + C2*QZ2(I) + C3*QZ3(I) + C4*QZ4(I)
QW1(I) = QW1(I) + C1*QW2(I) + C2*QW3(I) + C3*QW4(I)
QX1(I) = QX1(I) + C1*QX2(I) + C2*QX3(I) + C3*QX4(I)
QY1(I) = QY1(I) + C1*QY2(I) + C2*QY3(I) + C3*QY4(I)
QZ1(I) = QZ1(I) + C1*QZ2(I) + C2*QZ3(I) + C3*QZ4(I)
QW2(I) = QW2(I) + C1*QW3(I) + C2*QW4(I)
QX2(I) = QX2(I) + C1*QX3(I) + C2*QX4(I)
QY2(I) = QY2(I) + C1*QY3(I) + C2*QY4(I)
QZ2(I) = QZ2(I) + C1*QZ3(I) + C2*QZ4(I)
QW3(I) = QW3(I) + C1*QW4(I)
QX3(I) = QX3(I) + C1*QX4(I)
QY3(I) = QY3(I) + C1*QY4(I)
QZ3(I) = QZ3(I) + C1*QZ4(I)
OX(I) = OX(I) + C1*OX1(I) + C2*OX2(I) + C3*OX3(I) + C4*OX4(I)
OY(I) = OY(I) + C1*OY1(I) + C2*OY2(I) + C3*OY3(I) + C4*OY4(I)
OZ(I) = OZ(I) + C1*OZ1(I) + C2*OZ2(I) + C3*OZ3(I) + C4*OZ4(I)
OX1(I) = OX1(I) + C1*OX2(I) + C2*OX3(I) + C3*OX4(I)
OY1(I) = OY1(I) + C1*OY2(I) + C2*OY3(I) + C3*OY4(I)
OZ1(I) = OZ1(I) + C1*OZ2(I) + C2*OZ3(I) + C3*OZ4(I)
OX2(I) = OX2(I) + C1*OX3(I) + C2*OX4(I)
OY2(I) = OY2(I) + C1*OY3(I) + C2*OY4(I)
OZ2(I) = OZ2(I) + C1*OZ3(I) + C2*OZ4(I)
OX3(I) = OX3(I) + C1*OX4(I)
OY3(I) = OY3(I) + C1*OY4(I)
OZ3(I) = OZ3(I) + C1*OZ4(I)
100 CONTINUE
RETURN
END
SUBROUTINE MOLATM
COMMON / BLOCK1 / RX , RY , RZ , RX1, RY1, RZ1,
: RX2, RY2, RZ2, RX3, RY3, RZ3,
: FX , FY , FZ
COMMON / BLOCK2 / QW , QX , QY , QZ , QW1, QX1, QY1, QZ1,
: QW2, QX2, QY2, QZ2, QW3, QX3, QY3, QZ3,
: QW4, QX4, QY4, QZ4,
: OX , OY , OZ , OX1, OY1, OZ1,
: OX2, OY2, OZ2, OX3, OY3, OZ3,
: OX4, OY4, OZ4, TX , TY , TZ
COMMON / BLOCK3 / RSX, RSY, RSZ, FSX, FSY, FSZ
COMMON / BLOCK4 / DX , DY , DZ
C *******************************************************************
C ** CONVERSION OF MOLECULAR COORDINATES TO ATOM OR SITE POSITIONS **
C ** **
C ** PRINCIPAL VARIABLES: **
C ** **
C ** INTEGER N NUMBER OF MOLECULES **
C ** INTEGER NA NUMBER OF ATOMS PER MOL **
C ** REAL RSX(N,NA),RSY(N,NA),RSZ(N,NA) ATOM POSITIONS **
C ** REAL DX(NA),DY(NA),DZ(NA) ATOM POSITIONS IN MOLEC **
C ** REAL AXX,AXY,AXZ ETC. ROTATION MATRIX **
C ** THE VARIABLES DX,DY,DZ ARE ACTUALLY THE POSITION VECTORS OF **
C ** EACH ATOM IN THE MOLECULE RELATIVE TO THE CENTRE OF MASS IN **
C ** THE UNROTATED, BODY-FIXED, AXIS SYSTEM. **
C ** **
C ** USAGE: **
C ** **
C ** THE TRANSPOSE OF THE ROTATION MATRIX IS USED TO OBTAIN THE **
C ** POSITIONS OF EACH ATOM FROM THE CENTRE-OF-MASS POSITION AND **
C ** THE BODY-FIXED ATOM POSITION VECTORS (KNOWN FROM THE START). **
C ** THESE MAY THEN BE FED INTO THE FORCE ROUTINE. **
C ** FOR THIS EXAMPLE WE TAKE (NONLINEAR) TRIATOMIC MOLECULES. **
C *******************************************************************
INTEGER N
PARAMETER ( N = 108 )
INTEGER NA
PARAMETER ( NA = 3 )
REAL RX (N), RY (N), RZ (N)
REAL RX1(N), RY1(N), RZ1(N)
REAL RX2(N), RY2(N), RZ2(N)
REAL RX3(N), RY3(N), RZ3(N)
REAL FX (N), FY (N), FZ (N)
REAL QW (N), QX (N), QY (N), QZ (N)
REAL QW1(N), QX1(N), QY1(N), QZ1(N)
REAL QW2(N), QX2(N), QY2(N), QZ2(N)
REAL QW3(N), QX3(N), QY3(N), QZ3(N)
REAL QW4(N), QX4(N), QY4(N), QZ4(N)
REAL OX (N), OY (N), OZ (N)
REAL OX1(N), OY1(N), OZ1(N)
REAL OX2(N), OY2(N), OZ2(N)
REAL OX3(N), OY3(N), OZ3(N)
REAL OX4(N), OY4(N), OZ4(N)
REAL TX (N), TY (N), TZ (N)
REAL RSX(N,NA), RSY(N,NA), RSZ(N,NA)
REAL FSX(N,NA), FSY(N,NA), FSZ(N,NA)
REAL DX(NA), DY(NA), DZ(NA)
INTEGER I, A
REAL AXX, AXY, AXZ, AYX, AYY, AYZ, AZX, AZY, AZZ
C *******************************************************************
C ** LOOP OVER ALL MOLECULES **
DO 200 I = 1, N
C ** CALCULATE ROTATION MATRIX ELEMENTS **
AXX = QW(I) ** 2 + QX(I) ** 2 - QY(I) ** 2 - QZ(I) ** 2
AXY = 2.0 * ( QX(I) * QY(I) + QW(I) * QZ(I) )
AXZ = 2.0 * ( QX(I) * QZ(I) - QW(I) * QY(I) )
AYX = 2.0 * ( QX(I) * QY(I) - QW(I) * QZ(I) )
AYY = QW(I) ** 2 - QX(I) ** 2 + QY(I) ** 2 - QZ(I) ** 2
AYZ = 2.0 * ( QY(I) * QZ(I) + QW(I) * QX(I) )
AZX = 2.0 * ( QX(I) * QZ(I) + QW(I) * QY(I) )
AZY = 2.0 * ( QY(I) * QZ(I) - QW(I) * QX(I) )
AZZ = QW(I) ** 2 - QX(I) ** 2 - QY(I) ** 2 + QZ(I) ** 2
C ** LOOP OVER ALL SITES IN MOLECULE **
DO 199 A = 1, NA
RSX(I,A) = RX(I) + AXX * DX(A) + AYX * DY(A) + AZX * DZ(A)
RSY(I,A) = RY(I) + AXY * DX(A) + AYY * DY(A) + AZY * DZ(A)
RSZ(I,A) = RZ(I) + AXZ * DX(A) + AYZ * DY(A) + AZZ * DZ(A)
199 CONTINUE
200 CONTINUE
RETURN
END
SUBROUTINE ATMMOL
COMMON / BLOCK1 / RX , RY , RZ , RX1, RY1, RZ1,
: RX2, RY2, RZ2, RX3, RY3, RZ3,
: FX , FY , FZ
COMMON / BLOCK2 / QW , QX , QY , QZ , QW1, QX1, QY1, QZ1,
: QW2, QX2, QY2, QZ2, QW3, QX3, QY3, QZ3,
: QW4, QX4, QY4, QZ4,
: OX , OY , OZ , OX1, OY1, OZ1,
: OX2, OY2, OZ2, OX3, OY3, OZ3,
: OX4, OY4, OZ4, TX, TY, TZ
COMMON / BLOCK3 / RSX, RSY, RSZ, FSX, FSY, FSZ
C *******************************************************************
C ** CONVERTS ATOMIC SITE FORCES TO MOLECULAR FORCE AND TORQUE. **
C ** **
C ** PRINCIPAL VARIABLES: **
C ** **
C ** INTEGER N NUMBER OF MOLECULES **
C ** INTEGER NA NUMBER OF ATOMS PER MOL **
C ** REAL RSX(N,NA),RSY(N,NA),RSZ(N,NA) ATOM POSITIONS **
C ** REAL FSX(N,NA),FSY(N,NA),FSZ(N,NA) ATOM FORCES **
C ** REAL AXX,AXY,AXZ ETC. ROTATION MATRIX **
C ** **
C ** USAGE: **
C ** **
C ** THE ROTATION MATRIX IS USED TO CONVERT THE TORQUES FROM **
C ** SPACE-FIXED TO BODY-FIXED AXES PRIOR TO THE CORRECTOR STEP **
C *******************************************************************
INTEGER N
PARAMETER ( N = 108 )
INTEGER NA
PARAMETER ( NA = 3 )
REAL RX (N), RY (N), RZ (N)
REAL RX1(N), RY1(N), RZ1(N)
REAL RX2(N), RY2(N), RZ2(N)
REAL RX3(N), RY3(N), RZ3(N)
REAL FX (N), FY (N), FZ (N)
REAL QW (N), QX (N), QY (N), QZ (N)
REAL QW1(N), QX1(N), QY1(N), QZ1(N)
REAL QW2(N), QX2(N), QY2(N), QZ2(N)
REAL QW3(N), QX3(N), QY3(N), QZ3(N)
REAL QW4(N), QX4(N), QY4(N), QZ4(N)
REAL OX (N), OY (N), OZ (N)
REAL OX1(N), OY1(N), OZ1(N)
REAL OX2(N), OY2(N), OZ2(N)
REAL OX3(N), OY3(N), OZ3(N)
REAL OX4(N), OY4(N), OZ4(N)
REAL TX (N), TY (N), TZ (N)
REAL RSX(N,NA), RSY(N,NA), RSZ(N,NA)
REAL FSX(N,NA), FSY(N,NA), FSZ(N,NA)
INTEGER I, A
REAL AXX, AXY, AXZ, AYX, AYY, AYZ, AZX, AZY, AZZ
REAL FXI, FYI, FZI, TXI, TYI, TZI
REAL RXI, RYI, RZI, QWI, QXI, QYI, QZI
REAL RSXIA, RSYIA, RSZIA, FSXIA, FSYIA, FSZIA
C *******************************************************************
C ** LOOP OVER MOLECULES **
DO 300 I = 1, N
FXI = 0.0
FYI = 0.0
FZI = 0.0
TXI = 0.0
TYI = 0.0
TZI = 0.0
RXI = RX(I)
RYI = RY(I)
RZI = RZ(I)
QWI = QW(I)
QXI = QX(I)
QYI = QY(I)
QZI = QZ(I)
C ** LOOP OVER SITES IN A MOLECULE **
DO 299 A = 1, NA
FSXIA = FSX(I,A)
FSYIA = FSY(I,A)
FSZIA = FSZ(I,A)
RSXIA = RSX(I,A) - RXI
RSYIA = RSY(I,A) - RYI
RSZIA = RSZ(I,A) - RZI
C ** TOTAL FORCE AND TORQUE CONTRIBUTIONS **
FXI = FXI + FSXIA
FYI = FYI + FSYIA
FZI = FZI + FSZIA
TXI = TXI + RSYIA * FSZIA - RSZIA * FSYIA
TYI = TYI + RSZIA * FSXIA - RSXIA * FSZIA
TZI = TZI + RSXIA * FSYIA - RSYIA * FSXIA
299 CONTINUE
C ** STORE TOTAL FORCE **
FX(I) = FXI
FY(I) = FYI
FZ(I) = FZI
C ** CALCULATE ROTATION MATRIX ELEMENTS **
AXX = QWI ** 2 + QXI ** 2 - QYI ** 2 - QZI ** 2
AXY = 2.0 * ( QXI * QYI + QWI * QZI )
AXZ = 2.0 * ( QXI * QZI - QWI * QYI )
AYX = 2.0 * ( QXI * QYI - QWI * QZI )
AYY = QWI ** 2 - QXI ** 2 + QYI ** 2 - QZI ** 2
AYZ = 2.0 * ( QYI * QZI + QWI * QXI )
AZX = 2.0 * ( QXI * QZI + QWI * QYI )
AZY = 2.0 * ( QYI * QZI - QWI * QXI )
AZZ = QWI ** 2 - QXI ** 2 - QYI ** 2 + QZI ** 2
C ** CONVERT TORQUE TO BODY-FIXED COORDINATES **
TX(I) = AXX * TXI + AXY * TYI + AXZ * TZI
TY(I) = AYX * TXI + AYY * TYI + AYZ * TZI
TZ(I) = AZX * TXI + AZY * TYI + AZZ * TZI
300 CONTINUE
RETURN
END
SUBROUTINE CORREC ( DT, M, IXX, IYY, IZZ, K )
COMMON / BLOCK1 / RX , RY , RZ , RX1, RY1, RZ1,
: RX2, RY2, RZ2, RX3, RY3, RZ3,
: FX , FY , FZ
COMMON / BLOCK2 / QW , QX , QY , QZ , QW1, QX1, QY1, QZ1,
: QW2, QX2, QY2, QZ2, QW3, QX3, QY3, QZ3,
: QW4, QX4, QY4, QZ4,
: OX , OY , OZ , OX1, OY1, OZ1,
: OX2, OY2, OZ2, OX3, OY3, OZ3,
: OX4, OY4, OZ4, TX, TY, TZ
C *******************************************************************
C ** CORRECTS TRANSLATIONAL AND ROTATIONAL VARIABLES. **
C ** **
C ** THE CORRECTOR STAGE USES GEAR COEFFICIENTS (SEE REF ABOVE). **
C ** FOR TIMESTEP-SCALED VARIABLES THESE WOULD BE AS FOLLOWS. **
C ** FOR TRANSLATIONAL ALGORITHM, 4-VALUE METHOD, 2ND-ORDER D.E. **
C ** COEFFICIENTS ARE 1/6, 5/6, 1, 1/3 **
C ** FOR ROTATIONAL ALGORITHM, 5-VALUE METHOD, 1ST-ORDER D.E. **
C ** COEFFICIENTS ARE 251/720, 1, 11/12, 1/3, 1/24. **
C ** **
C ** PRINCIPAL VARIABLES: **
C ** **
C ** REAL GEART0, GEART1, GEART3 TRANSLATIONAL COEFFTS **
C ** REAL GEARR0, GEARR2, GEARR3, GEARR4 ROTATIONAL COEFFTS **
C ** **
C ** USAGE: **
C ** **
C ** THIS ROUTINE IS CALLED AFTER EVALUATION OF FORCES AND TORQUES **
C ** AND THE CONVERSION OF TORQUES INTO BODY-FIXED AXES. **
C ** IT ALSO RETURNS THE KINETIC ENERGY. **
C *******************************************************************
INTEGER N
PARAMETER ( N = 108 )
REAL DT, M, IXX, IYY, IZZ, K
REAL RX (N), RY (N), RZ (N)
REAL RX1(N), RY1(N), RZ1(N)
REAL RX2(N), RY2(N), RZ2(N)
REAL RX3(N), RY3(N), RZ3(N)
REAL FX (N), FY (N), FZ (N)
REAL QW (N), QX (N), QY (N), QZ (N)
REAL QW1(N), QX1(N), QY1(N), QZ1(N)
REAL QW2(N), QX2(N), QY2(N), QZ2(N)
REAL QW3(N), QX3(N), QY3(N), QZ3(N)
REAL QW4(N), QX4(N), QY4(N), QZ4(N)
REAL OX (N), OY (N), OZ (N)
REAL OX1(N), OY1(N), OZ1(N)
REAL OX2(N), OY2(N), OZ2(N)
REAL OX3(N), OY3(N), OZ3(N)
REAL OX4(N), OY4(N), OZ4(N)
REAL TX (N), TY (N), TZ (N)
INTEGER I
REAL C1, C2, C3, C4
REAL CORRW, CORRX, CORRY, CORRZ
REAL RX2I, RY2I, RZ2I
REAL QW1I, QX1I, QY1I, QZ1I, OX1I, OY1I, OZ1I
REAL CTRAN0, CTRAN1, CTRAN3
REAL CROT0, CROT2, CROT3, CROT4
REAL GEART0, GEART1, GEART3
PARAMETER ( GEART0 = 1.0 / 6.0,
: GEART1 = 5.0 / 6.0,
: GEART3 = 1.0 / 3.0 )
REAL GEARR0, GEARR2, GEARR3, GEARR4
PARAMETER ( GEARR0 = 251.0 / 720.0,
: GEARR2 = 11.0 / 12.0,
: GEARR3 = 1.0 / 3.0,
: GEARR4 = 1.0 / 24.0 )
C *******************************************************************
C1 = DT
C2 = C1 * DT / 2.0
C3 = C2 * DT / 3.0
C4 = C3 * DT / 4.0
CTRAN0 = GEART0 * C2
CTRAN1 = GEART1 * C2 / C1
CTRAN3 = GEART3 * C2 / C3
CROT0 = GEARR0 * C1
CROT2 = GEARR2 * C1 / C2
CROT3 = GEARR3 * C1 / C3
CROT4 = GEARR4 * C1 / C4
DO 400 I = 1, N
RX2I = FX(I) / M
RY2I = FY(I) / M
RZ2I = FZ(I) / M
CORRX = RX2I - RX2(I)
CORRY = RY2I - RY2(I)
CORRZ = RZ2I - RZ2(I)
RX (I) = RX (I) + CTRAN0 * CORRX
RY (I) = RY (I) + CTRAN0 * CORRY
RZ (I) = RZ (I) + CTRAN0 * CORRZ
RX1(I) = RX1(I) + CTRAN1 * CORRX
RY1(I) = RY1(I) + CTRAN1 * CORRY
RZ1(I) = RZ1(I) + CTRAN1 * CORRZ
RX2(I) = RX2I
RY2(I) = RY2I
RZ2(I) = RZ2I
RX3(I) = RX3(I) + CTRAN3 * CORRX
RY3(I) = RY3(I) + CTRAN3 * CORRY
RZ3(I) = RZ3(I) + CTRAN3 * CORRZ
K = K + M * ( RX1(I) ** 2 + RY1(I) ** 2 + RZ1(I) ** 2 )
QW1I = ( - QX(I)*OX(I) - QY(I)*OY(I) - QZ(I)*OZ(I) ) * 0.5
QX1I = ( QW(I)*OX(I) - QZ(I)*OY(I) + QY(I)*OZ(I) ) * 0.5
QY1I = ( QZ(I)*OX(I) + QW(I)*OY(I) - QX(I)*OZ(I) ) * 0.5
QZ1I = ( - QY(I)*OX(I) + QX(I)*OY(I) + QW(I)*OZ(I) ) * 0.5
CORRW = QW1I - QW1(I)
CORRX = QX1I - QX1(I)
CORRY = QY1I - QY1(I)
CORRZ = QZ1I - QZ1(I)
QW (I) = QW (I) + CROT0 * CORRW
QX (I) = QX (I) + CROT0 * CORRX
QY (I) = QY (I) + CROT0 * CORRY
QZ (I) = QZ (I) + CROT0 * CORRZ
QW1(I) = QW1I
QX1(I) = QX1I
QY1(I) = QY1I
QZ1(I) = QZ1I
QW2(I) = QW2(I) + CROT2 * CORRW
QX2(I) = QX2(I) + CROT2 * CORRX
QY2(I) = QY2(I) + CROT2 * CORRY
QZ2(I) = QZ2(I) + CROT2 * CORRZ
QW3(I) = QW3(I) + CROT3 * CORRW
QX3(I) = QX3(I) + CROT3 * CORRX
QY3(I) = QY3(I) + CROT3 * CORRY
QZ3(I) = QZ3(I) + CROT3 * CORRZ
QW4(I) = QW4(I) + CROT4 * CORRW
QX4(I) = QX4(I) + CROT4 * CORRX
QY4(I) = QY4(I) + CROT4 * CORRY
QZ4(I) = QZ4(I) + CROT4 * CORRZ
OX1I = ( TX(I) + OY(I) * OZ(I) * (IYY-IZZ) ) / IXX
OY1I = ( TY(I) + OZ(I) * OX(I) * (IZZ-IXX) ) / IYY
OZ1I = ( TZ(I) + OX(I) * OY(I) * (IXX-IYY) ) / IZZ
CORRX = OX1I - OX1(I)
CORRY = OY1I - OY1(I)
CORRZ = OZ1I - OZ1(I)
OX (I) = OX (I) + CROT0 * CORRX
OY (I) = OY (I) + CROT0 * CORRY
OZ (I) = OZ (I) + CROT0 * CORRZ
OX1(I) = OX1I
OY1(I) = OY1I
OZ1(I) = OZ1I
OX2(I) = OX2(I) + CROT2 * CORRX
OY2(I) = OY2(I) + CROT2 * CORRY
OZ2(I) = OZ2(I) + CROT2 * CORRZ
OX3(I) = OX3(I) + CROT3 * CORRX
OY3(I) = OY3(I) + CROT3 * CORRY
OZ3(I) = OZ3(I) + CROT3 * CORRZ
OX4(I) = OX4(I) + CROT4 * CORRX
OY4(I) = OY4(I) + CROT4 * CORRY
OZ4(I) = OZ4(I) + CROT4 * CORRZ
K = K + IXX * OX(I) ** 2
: + IYY * OY(I) ** 2
: + IZZ * OZ(I) ** 2
400 CONTINUE
K = 0.5 * K
RETURN
END