```001:       SUBROUTINE SGGLSE( M, N, P, A, LDA, B, LDB, C, D, X, WORK, LWORK,
002:      \$                   INFO )
003: *
004: *  -- LAPACK driver routine (version 3.2) --
005: *  -- LAPACK is a software package provided by Univ. of Tennessee,    --
006: *  -- Univ. of California Berkeley, Univ. of Colorado Denver and NAG Ltd..--
007: *     November 2006
008: *
009: *     .. Scalar Arguments ..
010:       INTEGER            INFO, LDA, LDB, LWORK, M, N, P
011: *     ..
012: *     .. Array Arguments ..
013:       REAL               A( LDA, * ), B( LDB, * ), C( * ), D( * ),
014:      \$                   WORK( * ), X( * )
015: *     ..
016: *
017: *  Purpose
018: *  =======
019: *
020: *  SGGLSE solves the linear equality-constrained least squares (LSE)
021: *  problem:
022: *
023: *          minimize || c - A*x ||_2   subject to   B*x = d
024: *
025: *  where A is an M-by-N matrix, B is a P-by-N matrix, c is a given
026: *  M-vector, and d is a given P-vector. It is assumed that
027: *  P <= N <= M+P, and
028: *
029: *           rank(B) = P and  rank( (A) ) = N.
030: *                                ( (B) )
031: *
032: *  These conditions ensure that the LSE problem has a unique solution,
033: *  which is obtained using a generalized RQ factorization of the
034: *  matrices (B, A) given by
035: *
036: *     B = (0 R)*Q,   A = Z*T*Q.
037: *
038: *  Arguments
039: *  =========
040: *
041: *  M       (input) INTEGER
042: *          The number of rows of the matrix A.  M >= 0.
043: *
044: *  N       (input) INTEGER
045: *          The number of columns of the matrices A and B. N >= 0.
046: *
047: *  P       (input) INTEGER
048: *          The number of rows of the matrix B. 0 <= P <= N <= M+P.
049: *
050: *  A       (input/output) REAL array, dimension (LDA,N)
051: *          On entry, the M-by-N matrix A.
052: *          On exit, the elements on and above the diagonal of the array
053: *          contain the min(M,N)-by-N upper trapezoidal matrix T.
054: *
055: *  LDA     (input) INTEGER
056: *          The leading dimension of the array A. LDA >= max(1,M).
057: *
058: *  B       (input/output) REAL array, dimension (LDB,N)
059: *          On entry, the P-by-N matrix B.
060: *          On exit, the upper triangle of the subarray B(1:P,N-P+1:N)
061: *          contains the P-by-P upper triangular matrix R.
062: *
063: *  LDB     (input) INTEGER
064: *          The leading dimension of the array B. LDB >= max(1,P).
065: *
066: *  C       (input/output) REAL array, dimension (M)
067: *          On entry, C contains the right hand side vector for the
068: *          least squares part of the LSE problem.
069: *          On exit, the residual sum of squares for the solution
070: *          is given by the sum of squares of elements N-P+1 to M of
071: *          vector C.
072: *
073: *  D       (input/output) REAL array, dimension (P)
074: *          On entry, D contains the right hand side vector for the
075: *          constrained equation.
076: *          On exit, D is destroyed.
077: *
078: *  X       (output) REAL array, dimension (N)
079: *          On exit, X is the solution of the LSE problem.
080: *
081: *  WORK    (workspace/output) REAL array, dimension (MAX(1,LWORK))
082: *          On exit, if INFO = 0, WORK(1) returns the optimal LWORK.
083: *
084: *  LWORK   (input) INTEGER
085: *          The dimension of the array WORK. LWORK >= max(1,M+N+P).
086: *          For optimum performance LWORK >= P+min(M,N)+max(M,N)*NB,
087: *          where NB is an upper bound for the optimal blocksizes for
088: *          SGEQRF, SGERQF, SORMQR and SORMRQ.
089: *
090: *          If LWORK = -1, then a workspace query is assumed; the routine
091: *          only calculates the optimal size of the WORK array, returns
092: *          this value as the first entry of the WORK array, and no error
093: *          message related to LWORK is issued by XERBLA.
094: *
095: *  INFO    (output) INTEGER
096: *          = 0:  successful exit.
097: *          < 0:  if INFO = -i, the i-th argument had an illegal value.
098: *          = 1:  the upper triangular factor R associated with B in the
099: *                generalized RQ factorization of the pair (B, A) is
100: *                singular, so that rank(B) < P; the least squares
101: *                solution could not be computed.
102: *          = 2:  the (N-P) by (N-P) part of the upper trapezoidal factor
103: *                T associated with A in the generalized RQ factorization
104: *                of the pair (B, A) is singular, so that
105: *                rank( (A) ) < N; the least squares solution could not
106: *                    ( (B) )
107: *                be computed.
108: *
109: *  =====================================================================
110: *
111: *     .. Parameters ..
112:       REAL               ONE
113:       PARAMETER          ( ONE = 1.0E+0 )
114: *     ..
115: *     .. Local Scalars ..
116:       LOGICAL            LQUERY
117:       INTEGER            LOPT, LWKMIN, LWKOPT, MN, NB, NB1, NB2, NB3,
118:      \$                   NB4, NR
119: *     ..
120: *     .. External Subroutines ..
121:       EXTERNAL           SAXPY, SCOPY, SGEMV, SGGRQF, SORMQR, SORMRQ,
122:      \$                   STRMV, STRTRS, XERBLA
123: *     ..
124: *     .. External Functions ..
125:       INTEGER            ILAENV
126:       EXTERNAL           ILAENV
127: *     ..
128: *     .. Intrinsic Functions ..
129:       INTRINSIC          INT, MAX, MIN
130: *     ..
131: *     .. Executable Statements ..
132: *
133: *     Test the input parameters
134: *
135:       INFO = 0
136:       MN = MIN( M, N )
137:       LQUERY = ( LWORK.EQ.-1 )
138:       IF( M.LT.0 ) THEN
139:          INFO = -1
140:       ELSE IF( N.LT.0 ) THEN
141:          INFO = -2
142:       ELSE IF( P.LT.0 .OR. P.GT.N .OR. P.LT.N-M ) THEN
143:          INFO = -3
144:       ELSE IF( LDA.LT.MAX( 1, M ) ) THEN
145:          INFO = -5
146:       ELSE IF( LDB.LT.MAX( 1, P ) ) THEN
147:          INFO = -7
148:       END IF
149: *
150: *     Calculate workspace
151: *
152:       IF( INFO.EQ.0) THEN
153:          IF( N.EQ.0 ) THEN
154:             LWKMIN = 1
155:             LWKOPT = 1
156:          ELSE
157:             NB1 = ILAENV( 1, 'SGEQRF', ' ', M, N, -1, -1 )
158:             NB2 = ILAENV( 1, 'SGERQF', ' ', M, N, -1, -1 )
159:             NB3 = ILAENV( 1, 'SORMQR', ' ', M, N, P, -1 )
160:             NB4 = ILAENV( 1, 'SORMRQ', ' ', M, N, P, -1 )
161:             NB = MAX( NB1, NB2, NB3, NB4 )
162:             LWKMIN = M + N + P
163:             LWKOPT = P + MN + MAX( M, N )*NB
164:          END IF
165:          WORK( 1 ) = LWKOPT
166: *
167:          IF( LWORK.LT.LWKMIN .AND. .NOT.LQUERY ) THEN
168:             INFO = -12
169:          END IF
170:       END IF
171: *
172:       IF( INFO.NE.0 ) THEN
173:          CALL XERBLA( 'SGGLSE', -INFO )
174:          RETURN
175:       ELSE IF( LQUERY ) THEN
176:          RETURN
177:       END IF
178: *
179: *     Quick return if possible
180: *
181:       IF( N.EQ.0 )
182:      \$   RETURN
183: *
184: *     Compute the GRQ factorization of matrices B and A:
185: *
186: *            B*Q' = (  0  T12 ) P   Z'*A*Q' = ( R11 R12 ) N-P
187: *                     N-P  P                  (  0  R22 ) M+P-N
188: *                                               N-P  P
189: *
190: *     where T12 and R11 are upper triangular, and Q and Z are
191: *     orthogonal.
192: *
193:       CALL SGGRQF( P, M, N, B, LDB, WORK, A, LDA, WORK( P+1 ),
194:      \$             WORK( P+MN+1 ), LWORK-P-MN, INFO )
195:       LOPT = WORK( P+MN+1 )
196: *
197: *     Update c = Z'*c = ( c1 ) N-P
198: *                       ( c2 ) M+P-N
199: *
200:       CALL SORMQR( 'Left', 'Transpose', M, 1, MN, A, LDA, WORK( P+1 ),
201:      \$             C, MAX( 1, M ), WORK( P+MN+1 ), LWORK-P-MN, INFO )
202:       LOPT = MAX( LOPT, INT( WORK( P+MN+1 ) ) )
203: *
204: *     Solve T12*x2 = d for x2
205: *
206:       IF( P.GT.0 ) THEN
207:          CALL STRTRS( 'Upper', 'No transpose', 'Non-unit', P, 1,
208:      \$                B( 1, N-P+1 ), LDB, D, P, INFO )
209: *
210:          IF( INFO.GT.0 ) THEN
211:             INFO = 1
212:             RETURN
213:          END IF
214: *
215: *        Put the solution in X
216: *
217:          CALL SCOPY( P, D, 1, X( N-P+1 ), 1 )
218: *
219: *        Update c1
220: *
221:          CALL SGEMV( 'No transpose', N-P, P, -ONE, A( 1, N-P+1 ), LDA,
222:      \$               D, 1, ONE, C, 1 )
223:       END IF
224: *
225: *     Solve R11*x1 = c1 for x1
226: *
227:       IF( N.GT.P ) THEN
228:          CALL STRTRS( 'Upper', 'No transpose', 'Non-unit', N-P, 1,
229:      \$                A, LDA, C, N-P, INFO )
230: *
231:          IF( INFO.GT.0 ) THEN
232:             INFO = 2
233:             RETURN
234:          END IF
235: *
236: *        Put the solution in X
237: *
238:          CALL SCOPY( N-P, C, 1, X, 1 )
239:       END IF
240: *
241: *     Compute the residual vector:
242: *
243:       IF( M.LT.N ) THEN
244:          NR = M + P - N
245:          IF( NR.GT.0 )
246:      \$      CALL SGEMV( 'No transpose', NR, N-M, -ONE, A( N-P+1, M+1 ),
247:      \$                  LDA, D( NR+1 ), 1, ONE, C( N-P+1 ), 1 )
248:       ELSE
249:          NR = P
250:       END IF
251:       IF( NR.GT.0 ) THEN
252:          CALL STRMV( 'Upper', 'No transpose', 'Non unit', NR,
253:      \$               A( N-P+1, N-P+1 ), LDA, D, 1 )
254:          CALL SAXPY( NR, -ONE, D, 1, C( N-P+1 ), 1 )
255:       END IF
256: *
257: *     Backward transformation x = Q'*x
258: *
259:       CALL SORMRQ( 'Left', 'Transpose', N, 1, P, B, LDB, WORK( 1 ), X,
260:      \$             N, WORK( P+MN+1 ), LWORK-P-MN, INFO )
261:       WORK( 1 ) = P + MN + MAX( LOPT, INT( WORK( P+MN+1 ) ) )
262: *
263:       RETURN
264: *
265: *     End of SGGLSE
266: *
267:       END
268: ```