LP-311 Remove basic/advanced stabilization tab auto-switch (autotune/txpid lock issues)
[librepilot.git] / ground / gcs / src / libs / eigen / blas / drotmg.f
blob3ae647b0870112cd4bb62e2decbdd9183ee7171a
1 SUBROUTINE DROTMG(DD1,DD2,DX1,DY1,DPARAM)
2 * .. Scalar Arguments ..
3 DOUBLE PRECISION DD1,DD2,DX1,DY1
4 * ..
5 * .. Array Arguments ..
6 DOUBLE PRECISION DPARAM(5)
7 * ..
9 * Purpose
10 * =======
12 * CONSTRUCT THE MODIFIED GIVENS TRANSFORMATION MATRIX H WHICH ZEROS
13 * THE SECOND COMPONENT OF THE 2-VECTOR (DSQRT(DD1)*DX1,DSQRT(DD2)*
14 * DY2)**T.
15 * WITH DPARAM(1)=DFLAG, H HAS ONE OF THE FOLLOWING FORMS..
17 * DFLAG=-1.D0 DFLAG=0.D0 DFLAG=1.D0 DFLAG=-2.D0
19 * (DH11 DH12) (1.D0 DH12) (DH11 1.D0) (1.D0 0.D0)
20 * H=( ) ( ) ( ) ( )
21 * (DH21 DH22), (DH21 1.D0), (-1.D0 DH22), (0.D0 1.D0).
22 * LOCATIONS 2-4 OF DPARAM CONTAIN DH11, DH21, DH12, AND DH22
23 * RESPECTIVELY. (VALUES OF 1.D0, -1.D0, OR 0.D0 IMPLIED BY THE
24 * VALUE OF DPARAM(1) ARE NOT STORED IN DPARAM.)
26 * THE VALUES OF GAMSQ AND RGAMSQ SET IN THE DATA STATEMENT MAY BE
27 * INEXACT. THIS IS OK AS THEY ARE ONLY USED FOR TESTING THE SIZE
28 * OF DD1 AND DD2. ALL ACTUAL SCALING OF DATA IS DONE USING GAM.
31 * Arguments
32 * =========
34 * DD1 (input/output) DOUBLE PRECISION
36 * DD2 (input/output) DOUBLE PRECISION
38 * DX1 (input/output) DOUBLE PRECISION
40 * DY1 (input) DOUBLE PRECISION
42 * DPARAM (input/output) DOUBLE PRECISION array, dimension 5
43 * DPARAM(1)=DFLAG
44 * DPARAM(2)=DH11
45 * DPARAM(3)=DH21
46 * DPARAM(4)=DH12
47 * DPARAM(5)=DH22
49 * =====================================================================
51 * .. Local Scalars ..
52 DOUBLE PRECISION DFLAG,DH11,DH12,DH21,DH22,DP1,DP2,DQ1,DQ2,DTEMP,
53 + DU,GAM,GAMSQ,ONE,RGAMSQ,TWO,ZERO
54 INTEGER IGO
55 * ..
56 * .. Intrinsic Functions ..
57 INTRINSIC DABS
58 * ..
59 * .. Data statements ..
61 DATA ZERO,ONE,TWO/0.D0,1.D0,2.D0/
62 DATA GAM,GAMSQ,RGAMSQ/4096.D0,16777216.D0,5.9604645D-8/
63 * ..
65 IF (.NOT.DD1.LT.ZERO) GO TO 10
66 * GO ZERO-H-D-AND-DX1..
67 GO TO 60
68 10 CONTINUE
69 * CASE-DD1-NONNEGATIVE
70 DP2 = DD2*DY1
71 IF (.NOT.DP2.EQ.ZERO) GO TO 20
72 DFLAG = -TWO
73 GO TO 260
74 * REGULAR-CASE..
75 20 CONTINUE
76 DP1 = DD1*DX1
77 DQ2 = DP2*DY1
78 DQ1 = DP1*DX1
80 IF (.NOT.DABS(DQ1).GT.DABS(DQ2)) GO TO 40
81 DH21 = -DY1/DX1
82 DH12 = DP2/DP1
84 DU = ONE - DH12*DH21
86 IF (.NOT.DU.LE.ZERO) GO TO 30
87 * GO ZERO-H-D-AND-DX1..
88 GO TO 60
89 30 CONTINUE
90 DFLAG = ZERO
91 DD1 = DD1/DU
92 DD2 = DD2/DU
93 DX1 = DX1*DU
94 * GO SCALE-CHECK..
95 GO TO 100
96 40 CONTINUE
97 IF (.NOT.DQ2.LT.ZERO) GO TO 50
98 * GO ZERO-H-D-AND-DX1..
99 GO TO 60
100 50 CONTINUE
101 DFLAG = ONE
102 DH11 = DP1/DP2
103 DH22 = DX1/DY1
104 DU = ONE + DH11*DH22
105 DTEMP = DD2/DU
106 DD2 = DD1/DU
107 DD1 = DTEMP
108 DX1 = DY1*DU
109 * GO SCALE-CHECK
110 GO TO 100
111 * PROCEDURE..ZERO-H-D-AND-DX1..
112 60 CONTINUE
113 DFLAG = -ONE
114 DH11 = ZERO
115 DH12 = ZERO
116 DH21 = ZERO
117 DH22 = ZERO
119 DD1 = ZERO
120 DD2 = ZERO
121 DX1 = ZERO
122 * RETURN..
123 GO TO 220
124 * PROCEDURE..FIX-H..
125 70 CONTINUE
126 IF (.NOT.DFLAG.GE.ZERO) GO TO 90
128 IF (.NOT.DFLAG.EQ.ZERO) GO TO 80
129 DH11 = ONE
130 DH22 = ONE
131 DFLAG = -ONE
132 GO TO 90
133 80 CONTINUE
134 DH21 = -ONE
135 DH12 = ONE
136 DFLAG = -ONE
137 90 CONTINUE
138 GO TO IGO(120,150,180,210)
139 * PROCEDURE..SCALE-CHECK
140 100 CONTINUE
141 110 CONTINUE
142 IF (.NOT.DD1.LE.RGAMSQ) GO TO 130
143 IF (DD1.EQ.ZERO) GO TO 160
144 ASSIGN 120 TO IGO
145 * FIX-H..
146 GO TO 70
147 120 CONTINUE
148 DD1 = DD1*GAM**2
149 DX1 = DX1/GAM
150 DH11 = DH11/GAM
151 DH12 = DH12/GAM
152 GO TO 110
153 130 CONTINUE
154 140 CONTINUE
155 IF (.NOT.DD1.GE.GAMSQ) GO TO 160
156 ASSIGN 150 TO IGO
157 * FIX-H..
158 GO TO 70
159 150 CONTINUE
160 DD1 = DD1/GAM**2
161 DX1 = DX1*GAM
162 DH11 = DH11*GAM
163 DH12 = DH12*GAM
164 GO TO 140
165 160 CONTINUE
166 170 CONTINUE
167 IF (.NOT.DABS(DD2).LE.RGAMSQ) GO TO 190
168 IF (DD2.EQ.ZERO) GO TO 220
169 ASSIGN 180 TO IGO
170 * FIX-H..
171 GO TO 70
172 180 CONTINUE
173 DD2 = DD2*GAM**2
174 DH21 = DH21/GAM
175 DH22 = DH22/GAM
176 GO TO 170
177 190 CONTINUE
178 200 CONTINUE
179 IF (.NOT.DABS(DD2).GE.GAMSQ) GO TO 220
180 ASSIGN 210 TO IGO
181 * FIX-H..
182 GO TO 70
183 210 CONTINUE
184 DD2 = DD2/GAM**2
185 DH21 = DH21*GAM
186 DH22 = DH22*GAM
187 GO TO 200
188 220 CONTINUE
189 IF (DFLAG) 250,230,240
190 230 CONTINUE
191 DPARAM(3) = DH21
192 DPARAM(4) = DH12
193 GO TO 260
194 240 CONTINUE
195 DPARAM(2) = DH11
196 DPARAM(5) = DH22
197 GO TO 260
198 250 CONTINUE
199 DPARAM(2) = DH11
200 DPARAM(3) = DH21
201 DPARAM(4) = DH12
202 DPARAM(5) = DH22
203 260 CONTINUE
204 DPARAM(1) = DFLAG
205 RETURN