V3FIT
qrsolv.f
1  SUBROUTINE qrsolv(n, r, ldr, ipvt, diag, qtb, x, sdiag, wa)
2  USE stel_kinds
3  IMPLICIT NONE
4 C-----------------------------------------------
5 C D u m m y A r g u m e n t s
6 C-----------------------------------------------
7  INTEGER :: n, ldr
8  INTEGER, DIMENSION(n), INTENT(in) :: ipvt
9  REAL(rprec), DIMENSION(ldr,n), INTENT(inout) :: r
10  REAL(rprec), DIMENSION(n) :: diag, qtb, x, sdiag, wa
11 C-----------------------------------------------
12 C L o c a l P a r a m e t e r s
13 C-----------------------------------------------
14  REAL(rprec), PARAMETER :: one = 1, zero = 0
15 C-----------------------------------------------
16 C L o c a l V a r i a b l e s
17 C-----------------------------------------------
18  INTEGER :: j, jp1, k, kp1, l, nsing, l1
19  REAL(rprec) :: COS, cotan, qtbpj, SIN, SUM0, TAN, temp
20  REAL(rprec) , ALLOCATABLE :: temp1u(:)
21 C-----------------------------------------------
22 c
23 c SUBROUTINE qrsolv
24 c
25 c given an m by n matrix a, an n by n diagonal matrix d,
26 c and an m-vector b, the problem is to determine an x which
27 c solves the system
28 c
29 c a*x = b , d*x = 0 ,
30 c
31 c in the least squares sense.
32 c
33 c this SUBROUTINE completes the solution of the problem
34 c IF it is provided with the necessary information from the
35 c qr factorization, with column pivoting, of a. that is, IF
36 c a*p = q*r, WHERE p is a permutation matrix, q has orthogonal
37 c columns, and r is an upper triangular matrix with diagonal
38 c elements of nonincreasing magnitude, THEN qrsolv EXPects
39 c the full upper triangle of r, the permutation matrix p,
40 c and the first n components of (q TRANSPOSE)*b. the system
41 c a*x = b, d*x = 0, is THEN equivalent to
42 c
43 c t t
44 c r*z = q *b , p *d*p*z = 0 ,
45 c
46 c where x = p*z. If this system does not have full rank,
47 c then a least squares solution is obtained. on output qrsolv
48 c also provides an upper triangular matrix s such that
49 c
50 c t t t
51 c p *(a *a + d*d)*p = s *s .
52 c
53 c s is computed within qrsolv and may be of separate interest.
54 c
55 c the SUBROUTINE statement is
56 c
57 c SUBROUTINE qrsolv(n,r,ldr,ipvt,diag,qtb,x,sdiag,wa)
58 c
59 c WHERE
60 c
61 c n is a positive INTEGER input variable set to the order of r.
62 c
63 c r is an n by n array. on input the full upper triangle
64 c must contain the full upper triangle of the matrix r.
65 c on output the full upper triangle is unaltered, and the
66 c strict lower triangle CONTAINS the strict upper triangle
67 c (transposed) of the upper triangular matrix s.
68 c
69 c ldr is a positive INTEGER input variable not less than n
70 c which specifies the leading DIMENSION of the array r.
71 c
72 c ipvt is an INTEGER input array of length n which defines the
73 c permutation matrix p such that a*p = q*r. column j of p
74 c is column ipvt(j) of the identity matrix.
75 c
76 c diag is an input array of length n which must contain the
77 c diagonal elements of the matrix d.
78 c
79 c qtb is an input array of length n which must contain the first
80 c n elements of the vector (q TRANSPOSE)*b.
81 c
82 c x is an output array of length n which CONTAINS the least
83 c squares solution of the system a*x = b, d*x = 0.
84 c
85 c sdiag is an output array of length n which CONTAINS the
86 c diagonal elements of the upper triangular matrix s.
87 c
88 c wa is a work array of length n.
89 c
90 c subprograms called
91 c
92 c fortran-supplied ... ABS,sqrt
93 c
94 c argonne national laboratory. MINpack project. march 1980.
95 c burton s. garbow, kenneth e. hillstrom, jorge j. more
96 c
97 c **********
98 c
99 c copy r and (q TRANSPOSE)*b to preserve input and initialize s.
100 c in particular, SAVE the diagonal elements of r in x.
101 c
102  DO j = 1, n
103  r(j:n,j) = r(j,j:n)
104  x(j) = r(j,j)
105  END DO
106  wa(1:n) = qtb(1:n)
107 c
108 c eliminate the diagonal matrix d using a givens rotation.
109 c
110  DO j = 1, n
111 c
112 c prepare the row of d to be eliminated, locating the
113 c diagonal element using p from the qr factorization.
114 c
115  l = ipvt(j)
116  IF (diag(l) .ne. zero) THEN
117  sdiag(j+1:n) = zero
118  sdiag(j) = diag(l)
119 c
120 c the transformations to eliminate the row of d
121 c modify ONLY a single element of (q TRANSPOSE)*b
122 c beyond the first n, which is initially zero.
123 c
124  qtbpj = zero
125  DO k = j, n
126 c
127 c determine a givens rotation which eliminates the
128 c appropriate element in the current row of d.
129 c
130  IF (sdiag(k) .ne. zero) THEN
131  IF (abs(r(k,k)) .lt. abs(sdiag(k))) THEN
132  cotan = r(k,k)/sdiag(k)
133  sin = one/sqrt(one + cotan*cotan)
134  cos = sin*cotan
135  ELSE
136  tan = sdiag(k)/r(k,k)
137  cos = one/sqrt(one + tan*tan)
138  sin = cos*tan
139  END IF
140 c
141 c compute the modified diagonal element of r and
142 c the modified element of ((q TRANSPOSE)*b,0).
143 c
144  r(k,k) = cos*r(k,k) + sin*sdiag(k)
145  temp = cos*wa(k) + sin*qtbpj
146  qtbpj = (-sin*wa(k)) + cos*qtbpj
147  wa(k) = temp
148 c
149 c accumulate the tranformation in the row of s.
150 c
151  kp1 = k + 1
152  IF (n .ge. kp1) THEN
153  l1 = n-kp1+1
154  ALLOCATE (temp1u(l1))
155  temp1u(:l1) = cos*r(kp1:n,k) + sin*sdiag(kp1:n)
156  sdiag(kp1:n) = (-sin*r(kp1:n,k)) + cos*sdiag(kp1:n)
157  r(kp1:n,k) = temp1u(:l1)
158  DEALLOCATE (temp1u)
159  END IF
160  END IF
161  END DO
162  END IF
163 c
164 c store the diagonal element of s and restore
165 c the corresponding diagonal element of r.
166 c
167  sdiag(j) = r(j,j)
168  r(j,j) = x(j)
169  END DO
170 c
171 c solve the triangular system for z. IF the system is
172 c singular, then obtain a least squares solution.
173 c
174  nsing = n
175  DO j = 1, n
176  IF (sdiag(j).eq.zero .and. nsing.eq.n) nsing = j - 1
177  IF (nsing .lt. n) wa(j) = zero
178  END DO
179  IF (nsing .ge. 1) THEN
180  DO k = 1, nsing
181  j = nsing - k + 1
182  jp1 = j + 1
183  IF (nsing .lt. jp1) THEN
184  sum0 = zero
185  ELSE
186  sum0 = sum(r(jp1:nsing,j)*wa(jp1:nsing))
187  END IF
188  wa(j) = (wa(j)-sum0)/sdiag(j)
189  END DO
190  END IF
191 c
192 c permute the components of z back to components of x.
193 c
194  DO j = 1, n
195  l = ipvt(j)
196  x(l) = wa(j)
197  END DO
198 
199  END SUBROUTINE qrsolv