LP-311 Remove basic/advanced stabilization tab auto-switch (autotune/txpid lock issues)
[librepilot.git] / ground / gcs / src / libs / eigen / blas / drotm.f
blob63a3b1134f3b985c3e800b9c7ca058c1c4e95cab
1 SUBROUTINE DROTM(N,DX,INCX,DY,INCY,DPARAM)
2 * .. Scalar Arguments ..
3 INTEGER INCX,INCY,N
4 * ..
5 * .. Array Arguments ..
6 DOUBLE PRECISION DPARAM(5),DX(*),DY(*)
7 * ..
9 * Purpose
10 * =======
12 * APPLY THE MODIFIED GIVENS TRANSFORMATION, H, TO THE 2 BY N MATRIX
14 * (DX**T) , WHERE **T INDICATES TRANSPOSE. THE ELEMENTS OF DX ARE IN
15 * (DY**T)
17 * DX(LX+I*INCX), I = 0 TO N-1, WHERE LX = 1 IF INCX .GE. 0, ELSE
18 * LX = (-INCX)*N, AND SIMILARLY FOR SY USING LY AND INCY.
19 * WITH DPARAM(1)=DFLAG, H HAS ONE OF THE FOLLOWING FORMS..
21 * DFLAG=-1.D0 DFLAG=0.D0 DFLAG=1.D0 DFLAG=-2.D0
23 * (DH11 DH12) (1.D0 DH12) (DH11 1.D0) (1.D0 0.D0)
24 * H=( ) ( ) ( ) ( )
25 * (DH21 DH22), (DH21 1.D0), (-1.D0 DH22), (0.D0 1.D0).
26 * SEE DROTMG FOR A DESCRIPTION OF DATA STORAGE IN DPARAM.
28 * Arguments
29 * =========
31 * N (input) INTEGER
32 * number of elements in input vector(s)
34 * DX (input/output) DOUBLE PRECISION array, dimension N
35 * double precision vector with N elements
37 * INCX (input) INTEGER
38 * storage spacing between elements of DX
40 * DY (input/output) DOUBLE PRECISION array, dimension N
41 * double precision vector with N elements
43 * INCY (input) INTEGER
44 * storage spacing between elements of DY
46 * DPARAM (input/output) DOUBLE PRECISION array, dimension 5
47 * DPARAM(1)=DFLAG
48 * DPARAM(2)=DH11
49 * DPARAM(3)=DH21
50 * DPARAM(4)=DH12
51 * DPARAM(5)=DH22
53 * =====================================================================
55 * .. Local Scalars ..
56 DOUBLE PRECISION DFLAG,DH11,DH12,DH21,DH22,TWO,W,Z,ZERO
57 INTEGER I,KX,KY,NSTEPS
58 * ..
59 * .. Data statements ..
60 DATA ZERO,TWO/0.D0,2.D0/
61 * ..
63 DFLAG = DPARAM(1)
64 IF (N.LE.0 .OR. (DFLAG+TWO.EQ.ZERO)) GO TO 140
65 IF (.NOT. (INCX.EQ.INCY.AND.INCX.GT.0)) GO TO 70
67 NSTEPS = N*INCX
68 IF (DFLAG) 50,10,30
69 10 CONTINUE
70 DH12 = DPARAM(4)
71 DH21 = DPARAM(3)
72 DO 20 I = 1,NSTEPS,INCX
73 W = DX(I)
74 Z = DY(I)
75 DX(I) = W + Z*DH12
76 DY(I) = W*DH21 + Z
77 20 CONTINUE
78 GO TO 140
79 30 CONTINUE
80 DH11 = DPARAM(2)
81 DH22 = DPARAM(5)
82 DO 40 I = 1,NSTEPS,INCX
83 W = DX(I)
84 Z = DY(I)
85 DX(I) = W*DH11 + Z
86 DY(I) = -W + DH22*Z
87 40 CONTINUE
88 GO TO 140
89 50 CONTINUE
90 DH11 = DPARAM(2)
91 DH12 = DPARAM(4)
92 DH21 = DPARAM(3)
93 DH22 = DPARAM(5)
94 DO 60 I = 1,NSTEPS,INCX
95 W = DX(I)
96 Z = DY(I)
97 DX(I) = W*DH11 + Z*DH12
98 DY(I) = W*DH21 + Z*DH22
99 60 CONTINUE
100 GO TO 140
101 70 CONTINUE
102 KX = 1
103 KY = 1
104 IF (INCX.LT.0) KX = 1 + (1-N)*INCX
105 IF (INCY.LT.0) KY = 1 + (1-N)*INCY
107 IF (DFLAG) 120,80,100
108 80 CONTINUE
109 DH12 = DPARAM(4)
110 DH21 = DPARAM(3)
111 DO 90 I = 1,N
112 W = DX(KX)
113 Z = DY(KY)
114 DX(KX) = W + Z*DH12
115 DY(KY) = W*DH21 + Z
116 KX = KX + INCX
117 KY = KY + INCY
118 90 CONTINUE
119 GO TO 140
120 100 CONTINUE
121 DH11 = DPARAM(2)
122 DH22 = DPARAM(5)
123 DO 110 I = 1,N
124 W = DX(KX)
125 Z = DY(KY)
126 DX(KX) = W*DH11 + Z
127 DY(KY) = -W + DH22*Z
128 KX = KX + INCX
129 KY = KY + INCY
130 110 CONTINUE
131 GO TO 140
132 120 CONTINUE
133 DH11 = DPARAM(2)
134 DH12 = DPARAM(4)
135 DH21 = DPARAM(3)
136 DH22 = DPARAM(5)
137 DO 130 I = 1,N
138 W = DX(KX)
139 Z = DY(KY)
140 DX(KX) = W*DH11 + Z*DH12
141 DY(KY) = W*DH21 + Z*DH22
142 KX = KX + INCX
143 KY = KY + INCY
144 130 CONTINUE
145 140 CONTINUE
146 RETURN