-
Notifications
You must be signed in to change notification settings - Fork 20
/
Copy pathdeuler.f
180 lines (156 loc) · 5.13 KB
/
deuler.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
SUBROUTINE sla_DEULER (ORDER, PHI, THETA, PSI, RMAT)
*+
* - - - - - - -
* D E U L E R
* - - - - - - -
*
* Form a rotation matrix from the Euler angles - three successive
* rotations about specified Cartesian axes (double precision)
*
* Given:
* ORDER c*(*) specifies about which axes the rotations occur
* PHI d 1st rotation (radians)
* THETA d 2nd rotation ( " )
* PSI d 3rd rotation ( " )
*
* Returned:
* RMAT d(3,3) rotation matrix
*
* A rotation is positive when the reference frame rotates
* anticlockwise as seen looking towards the origin from the
* positive region of the specified axis.
*
* The characters of ORDER define which axes the three successive
* rotations are about. A typical value is 'ZXZ', indicating that
* RMAT is to become the direction cosine matrix corresponding to
* rotations of the reference frame through PHI radians about the
* old Z-axis, followed by THETA radians about the resulting X-axis,
* then PSI radians about the resulting Z-axis.
*
* The axis names can be any of the following, in any order or
* combination: X, Y, Z, uppercase or lowercase, 1, 2, 3. Normal
* axis labelling/numbering conventions apply; the xyz (=123)
* triad is right-handed. Thus, the 'ZXZ' example given above
* could be written 'zxz' or '313' (or even 'ZxZ' or '3xZ'). ORDER
* is terminated by length or by the first unrecognized character.
*
* Fewer than three rotations are acceptable, in which case the later
* angle arguments are ignored. If all rotations are zero, the
* identity matrix is produced.
*
* P.T.Wallace Starlink 23 May 1997
*
* Copyright (C) 1997 Rutherford Appleton Laboratory
*
* License:
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program (see SLA_CONDITIONS); if not, write to the
* Free Software Foundation, Inc., 59 Temple Place, Suite 330,
* Boston, MA 02111-1307 USA
*
*-
IMPLICIT NONE
CHARACTER*(*) ORDER
DOUBLE PRECISION PHI,THETA,PSI,RMAT(3,3)
INTEGER J,I,L,N,K
DOUBLE PRECISION RESULT(3,3),ROTN(3,3),ANGLE,S,C,W,WM(3,3)
CHARACTER AXIS
* Initialize result matrix
DO J=1,3
DO I=1,3
IF (I.NE.J) THEN
RESULT(I,J) = 0D0
ELSE
RESULT(I,J) = 1D0
END IF
END DO
END DO
* Establish length of axis string
L = LEN(ORDER)
* Look at each character of axis string until finished
DO N=1,3
IF (N.LE.L) THEN
* Initialize rotation matrix for the current rotation
DO J=1,3
DO I=1,3
IF (I.NE.J) THEN
ROTN(I,J) = 0D0
ELSE
ROTN(I,J) = 1D0
END IF
END DO
END DO
* Pick up the appropriate Euler angle and take sine & cosine
IF (N.EQ.1) THEN
ANGLE = PHI
ELSE IF (N.EQ.2) THEN
ANGLE = THETA
ELSE
ANGLE = PSI
END IF
S = SIN(ANGLE)
C = COS(ANGLE)
* Identify the axis
AXIS = ORDER(N:N)
IF (AXIS.EQ.'X'.OR.
: AXIS.EQ.'x'.OR.
: AXIS.EQ.'1') THEN
* Matrix for x-rotation
ROTN(2,2) = C
ROTN(2,3) = S
ROTN(3,2) = -S
ROTN(3,3) = C
ELSE IF (AXIS.EQ.'Y'.OR.
: AXIS.EQ.'y'.OR.
: AXIS.EQ.'2') THEN
* Matrix for y-rotation
ROTN(1,1) = C
ROTN(1,3) = -S
ROTN(3,1) = S
ROTN(3,3) = C
ELSE IF (AXIS.EQ.'Z'.OR.
: AXIS.EQ.'z'.OR.
: AXIS.EQ.'3') THEN
* Matrix for z-rotation
ROTN(1,1) = C
ROTN(1,2) = S
ROTN(2,1) = -S
ROTN(2,2) = C
ELSE
* Unrecognized character - fake end of string
L = 0
END IF
* Apply the current rotation (matrix ROTN x matrix RESULT)
DO I=1,3
DO J=1,3
W = 0D0
DO K=1,3
W = W+ROTN(I,K)*RESULT(K,J)
END DO
WM(I,J) = W
END DO
END DO
DO J=1,3
DO I=1,3
RESULT(I,J) = WM(I,J)
END DO
END DO
END IF
END DO
* Copy the result
DO J=1,3
DO I=1,3
RMAT(I,J) = RESULT(I,J)
END DO
END DO
END