#
source:
trunk/projects/slim-curve/src/main/c/GCI_Lsqnonneg.c
@
7781

Revision 7781, 20.3 KB checked in by paulbarber, 8 years ago (diff) |
---|

Line | |
---|---|

1 | /* |

2 | This file is part of the SLIM-curve package for exponential curve fitting of spectral lifetime data. |

3 | |

4 | Copyright (c) 2010, 2011, Gray Institute University of Oxford & UW-Madison LOCI. |

5 | |

6 | This program is free software: you can redistribute it and/or modify |

7 | it under the terms of the GNU General Public License as published by |

8 | the Free Software Foundation, either version 3 of the License, or |

9 | (at your option) any later version. |

10 | |

11 | This program is distributed in the hope that it will be useful, |

12 | but WITHOUT ANY WARRANTY; without even the implied warranty of |

13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |

14 | GNU General Public License for more details. |

15 | |

16 | You should have received a copy of the GNU General Public License |

17 | along with this program. If not, see <http://www.gnu.org/licenses/>. |

18 | */ |

19 | |

20 | // Updated file from JG, 2.11.02 |

21 | |

22 | /* Non-neg least squares library function |

23 | |

24 | This code is based on code and descriptions in the book |

25 | Lawson, C.L. and Hanson, R.J., Solving Least Squares Problems, |

26 | Prentice-Hall, 1974 |

27 | |

28 | A brief description of the various algorithms used is included |

29 | within this file, but for a full explanation, please refer to the |

30 | above-mentioned book or a similar reference. |

31 | |

32 | Julian Gilbey, Gray Cancer Institute, September 2002 |

33 | */ |

34 | |

35 | /* Include files we need */ |

36 | #include <stdio.h> /* for NULL */ |

37 | #include <stdlib.h> /* for malloc/free */ |

38 | #include <string.h> /* for memcpy */ |

39 | #include <math.h> /* for fabs and sqrt */ |

40 | #ifndef max |

41 | #define max(a,b) ((a)<(b) ? (b) : (a)) |

42 | #endif |

43 | |

44 | /* The greatest number of variables to be determined. This is to |

45 | allocate an array in advance to save having to use malloc/free */ |

46 | #define MAX_VARS 50 |

47 | |

48 | /* The greatest number of equations to be used. Again, this is |

49 | to allocate an array in advance */ |

50 | #define MAX_EQNS 10000 |

51 | |

52 | /* Some tolerence calculations */ |

53 | /* Approximate machine epsilon */ |

54 | #define EPS 1e-9 |

55 | #define TOL (100*EPS) |

56 | |

57 | |

58 | /********************************************************************** |

59 | Function Householder |

60 | |

61 | This function takes as input a vector, a pivot element and a zero |

62 | point, and applies a Householder transformation to the vector to |

63 | make all of its entries zero from the point specified, pivoting |

64 | around the specified pivot entry, modifying the vector in the |

65 | process, as described below. This function can also take a |

66 | collection of other vectors and apply this Householder |

67 | transformation to these vectors. |

68 | |

69 | Here is a brief description of Householder transformations: we are |

70 | looking for an orthogonal transformation Q with |

71 | |

72 | Qv = ( v_0 v_1 ... v_{p-1} -sigma.sqrt(v_p^2+sum_{i=l}^{m-1} v_i^2) |

73 | v_{p+1} ... v_{l-1} 0 ... 0)^T =: y |

74 | |

75 | where sigma = +1 if v_p >= 0 and -1 if v_p < 0. |

76 | |

77 | Here p is the pivot index and l is the zeroing index, where we |

78 | assume 0 <= p <= l < m (where v is an m-vector, zero-based). If |

79 | these assumptions are not true, we take Q=I. |

80 | |

81 | Q is given by Q = I - 2uu^T/|u|^2 for some vector u, but we do |

82 | not actually need to calculate it. We do the following: |

83 | |

84 | Compute: s := -sigma.sqrt(v_p^2+sum_{i=l}^m v_i^2) |

85 | u_i := 0, i=0, ..., p-1 |

86 | u_p := v_p - s |

87 | u_i := 0, i=p+1, ..., l-1 |

88 | u_i := v_i, i=l, ..., m-1 |

89 | b := su_p (so b = -|u|^2/2) |

90 | Q := ( I_m + b^{-1}uu^T if b != 0 |

91 | ( I_m if b = 0 |

92 | |

93 | After this, we place the non-zero components of u and y into the |

94 | storage previously occupied by v; we let v_p := y_p and store u_p |

95 | separately. |

96 | |

97 | To apply this transformation to the set of m-vectors c_j, getting |

98 | d_j = Qc_j, we do: |

99 | |

100 | t_j := b^{-1}(u^T c_j) |

101 | d_j := c_j + t_j u |

102 | |

103 | if b != 0, and d_j := c_j if b = 0. |

104 | |

105 | This function computes u, b, y:=Qv and d_j := Qc_j for all c_j we |

106 | are given. If u and y have already been computed, we can apply |

107 | this Q to new c_j vectors. |

108 | |

109 | Arguments: mode: 1 means calculate u, b, y and then Qc_j |

110 | 2 means u and y are precalculated, just calculate Qc_j |

111 | v, the vector to pivot |

112 | NB this vector is modified during execution of this |

113 | function, as described above |

114 | p, the pivot |

115 | l, the zeroing point |

116 | m, the length of the vector v |

117 | u_p (double *), the calculated value of u_p |

118 | C, double **C, array of pointers to the c_j vectors |

119 | nC, number of c_j vectors |

120 | If C=NULL or nC=0, the code will not attempt to find any |

121 | Qc_j. |

122 | NB the calculated d_j := Qc_j vectors will overwrite the |

123 | C array |

124 | |

125 | Here is the collected algorithm, as outlined above: |

126 | |

127 | 0 // Start here if mode = 1 |

128 | 1 s := sqrt(v_p^2 + sum_{i=l}^{m-1} v_i^2) |

129 | (Use the calculation trick described below under Givens |

130 | transformations to avoid calculation errors) |

131 | 2 if v_p > 0, s := -s |

132 | 3 u_p := v_p - s, v_p := s |

133 | 4 // Start here if mode = 2 |

134 | 5 b := v_p.u_p |

135 | 6 if b=0 or no C, stop |

136 | 7 for each c_j do steps 8-10 |

137 | 8 t_j := (c_j[p].u_p + sum_{i=l}^{m-1} c_j[i].v_i) / b |

138 | 9 c_j[p] := c_j[p] + t_j.u_p |

139 | 10 for i=l,...,m-1, c_j[i] := c_j[i] + t_j.v_i |

140 | |

141 | */ |

142 | |

143 | void Householder(int mode, double *v, int p, int l, int m, double *u_p, |

144 | double *C[], int nC) |

145 | { |

146 | double vmax, vmax_inv, vtmp, s, b, tj; |

147 | double *cj; |

148 | int i, j; |

149 | |

150 | if (0>p || p>=l || l>m) return; |

151 | |

152 | /* find the maximum v_i */ |

153 | vmax = fabs(v[p]); |

154 | if (mode == 1) { |

155 | for (i=l; i<m; i++) |

156 | vmax = max(vmax, fabs(v[i])); |

157 | if (vmax == 0) return; /* oops; not much to do here, really */ |

158 | vmax_inv = 1/vmax; /* only calculate this once; does it matter? */ |

159 | |

160 | /* we now calculate s, which is step 1 in the algorithm */ |

161 | vtmp = v[p]*vmax_inv; |

162 | s = vtmp*vtmp; |

163 | for (i=l; i<m; i++) { |

164 | vtmp = v[i]*vmax_inv; |

165 | s += vtmp*vtmp; |

166 | } |

167 | s = vmax*sqrt(s); |

168 | /* this is step 1 done */ |

169 | if (v[p]>0) s = -s; |

170 | *u_p = v[p]-s; |

171 | v[p] = s; |

172 | } else { |

173 | if (vmax == 0) return; /* same test as before, only now we are |

174 | testing only the v_p element, which is |

175 | actually y_p */ |

176 | } |

177 | |

178 | /* Now calculate the Qc_j vectors */ |

179 | if (nC <= 0 || C==NULL) return; |

180 | b = v[p] * (*u_p); |

181 | if (b >= 0) return; |

182 | |

183 | for (j=0; j<nC; j++) { |

184 | cj = C[j]; |

185 | tj = cj[p] * (*u_p); |

186 | for (i=l; i<m; i++) |

187 | tj += cj[i]*v[i]; |

188 | |

189 | if (tj != 0) { |

190 | tj /= b; |

191 | cj[p] += tj*(*u_p); |

192 | for (i=l; i<m; i++) |

193 | cj[i] += tj*v[i]; |

194 | } |

195 | } |

196 | } |

197 | |

198 | |

199 | /********************************************************************** |

200 | Functions GivensCalc, GivensApply |

201 | |

202 | The GivensCalc function takes as input a 2-vector, and computes |

203 | the Givens rotation for this vector. |

204 | |

205 | If the vector is v = (v_1 v_2)^T != 0, then the Givens rotation is |

206 | the 2x2 (special) orthogonal matrix |

207 | |

208 | G = ( c s ) |

209 | ( -s c ) |

210 | |

211 | (with c^2 + s^2 = 1) such that Gv = (r 0)^T, where |

212 | r=|v|=sqrt(v_1^2 + v_2^2). |

213 | |

214 | This function calculates c, s and r, avoiding overflow and |

215 | underflow issues, and returns these values. (We can allow r to |

216 | overwrite v_1 or v_2 if we wish.) The calculation is as follows: |

217 | |

218 | To compute r=sqrt(x^2 + y^2): |

219 | t := max {|x|, |y|} |

220 | u := min {|x|, |y|} |

221 | r := ( t.sqrt(1+(u/t)^2) if t != 0 |

222 | ( 0 if t = 0 |

223 | |

224 | The function GivensApply applies a Givens rotation to a |

225 | 2-vector. No rocket science there. |

226 | |

227 | Here, then, are the algorithms we use: |

228 | |

229 | Calculate a Givens rotation for the vector (v_1 v_2)^T |

230 | (GivensCalc): |

231 | |

232 | Arguments: v_1, v_2, the input vector |

233 | c, s, r, pointers to the answers. r may point to v_1 or v_2 |

234 | |

235 | 1 if |v_1| <= |v_2| go to step 8 |

236 | 2 w := v_2/v_1 |

237 | 3 q := sqrt(1+w^2) |

238 | 4 c := 1/q |

239 | 5 if v_1 < 0, c := -c |

240 | 6 s := wc |

241 | 7 r := |v_1|.q and stop |

242 | 8 if v_2 != 0, go to step 10 |

243 | 9 [v_1=v_2=0] c := 1, s:= 0, r := 0, stop |

244 | 10 w := v_1/v_2 |

245 | 11 q := sqrt(1+w^2) |

246 | 12 s := 1/q |

247 | 13 if v_1 < 0, s := -s |

248 | 14 c := ws |

249 | 15 r := |v_2|.q and stop |

250 | |

251 | Apply the Givens rotation G to the vector (z_1 z_2)^T |

252 | (GivensApply): |

253 | |

254 | Arguments: c, s, components of the Givens rotation matrix |

255 | z_1, z_2, (double *) the vector to apply it to; these |

256 | will be overwritten! |

257 | |

258 | 1 w := z_1.c + z_2.s |

259 | 2 z_2 := -z_1.s + z_2.c |

260 | 3 z_1 := w |

261 | |

262 | */ |

263 | |

264 | void GivensCalc(double v_1, double v_2, double *c, double *s, double *r) |

265 | { |

266 | double w, q; |

267 | |

268 | if (fabs(v_1) > fabs(v_2)) { |

269 | w = v_2/v_1; |

270 | q = sqrt(1+w*w); |

271 | *c = 1/q; |

272 | if (v_1 < 0) *c = - (*c); |

273 | *s = w*(*c); |

274 | *r = fabs(v_1)*q; |

275 | } else { |

276 | if (v_2 == 0) { |

277 | *c = 1; *s = 0; *r = 0; |

278 | } else { |

279 | w = v_1/v_2; |

280 | q = sqrt(1+w*w); |

281 | *s = 1/q; |

282 | if (v_2 < 0) *s = - (*s); |

283 | *c = w*(*s); |

284 | *r = fabs(v_2)*q; |

285 | } |

286 | } |

287 | } |

288 | |

289 | |

290 | void GivensApply(double c, double s, double *z_1, double *z_2) |

291 | { |

292 | double w; |

293 | |

294 | w = (*z_1)*c + (*z_2)*s; |

295 | *z_2 = -(*z_1)*s + (*z_2)*c; |

296 | *z_1 = w; |

297 | } |

298 | |

299 | |

300 | /********************************************************************** |

301 | Function GCI_lsqnonneg |

302 | |

303 | This function solves the non-negative least squares problem: |

304 | minimise |Ax-b| subject to x >= 0 (where |v| is the 2-norm of the |

305 | vector v). |

306 | |

307 | Arguments: A, an m x n matrix, in the form double A[n][m], so |

308 | the columns of A are A[0], A[1], ..., A[n-1] |

309 | b, the m-vector |

310 | |

311 | !!! NB: A and B will both be modified unless preserve is |

312 | !!! non-zero (see below). |

313 | |

314 | x, the solution will be placed here |

315 | m ) the dimensions of A |

316 | n ) |

317 | preserve, copy A and b before solving the problem |

318 | rnorm, (double *) the value of |Ax-b| with the |

319 | determined x if the function was successful or if the |

320 | iteration count was exceeded. This can be NULL. |

321 | lambda, an n-vector which will contain the dual vector |

322 | on completion (that is, the Lagrange multipliers). |

323 | This can be NULL. |

324 | |

325 | On exit: |

326 | The return value will be 0 on success, and negative if |

327 | a problem occurred: |

328 | -1: m > MAX_EQNS or m <= 0 |

329 | -2: n > MAX_VARS or n <= 0 |

330 | -3: iteration count exceeded: more than 3*n iterations |

331 | performed |

332 | -4: memory allocation problems |

333 | |

334 | The algorithm is similar to the simplex algorithm, and uses |

335 | Lagrange multipliers. |

336 | |

337 | 1 Set P:={}, Z:={1,2,...,n}, x:=0 |

338 | 2 Compute w := A^T (b-Ax) |

339 | 3 If Z={} or if w_j <= 0 for all j in Z, stop |

340 | 4 Find t in Z such that w_t = max { w_j : j in Z } |

341 | 5 Move index t from Z to P |

342 | 6 Let A_P denote the m x n matrix defined by |

343 | |

344 | ( column j of A if j in P |

345 | column j of A_P := ( |

346 | ( 0 if j in Z |

347 | |

348 | Compute the n-vector z as a solution of the regular least squares |

349 | problem minimise | A_P z - b |. Note that only the components |

350 | z_j, j in P, are determined by this problem. Define z_j:=0 for j in Z. |

351 | 7 If z_j>0 for all j in P, then set x:=z and goto step 2 |

352 | 8 Find an index q in P such that |

353 | x_q/(x_q-z_q) = min { x_j/(x_j-z_j) : z_j <= 0, j in P } |

354 | 9 Set alpha := x_q/(x_q-z_q) |

355 | 10 Set x := x + alpha(z-x) |

356 | 11 Move from set P to set Z all j in P for which x_j=0. Go to step 6. |

357 | |

358 | On termination, x satisfies x_j>0 for j in P and x_j=0 for j in Z, and |

359 | x is a solution to the least squares problem minimise |A_P z - b|. |

360 | |

361 | Step 6 (finding the solution to the least squares problem |

362 | |A_P z - b|) is performed using a QR factorisation of A_P, which in |

363 | turn is calculated using Householder matrices and Givens rotations, |

364 | and clever tricks for keeping track of the QR factorisation of A_P |

365 | as P changes, as we describe below. |

366 | */ |

367 | |

368 | #define Amatrix(row,col) A[col][row] |

369 | |

370 | int GCI_lsqnonneg(double **A_orig, double *b_orig, double *x, int m, int n, |

371 | int preserve, double *rnorm_orig, double *lambda) |

372 | { |

373 | double *AA[MAX_VARS], bb[MAX_EQNS], **A, *b; /* to preserve originals */ |

374 | double rnorm2, *rnorm; /* in case rnorm_orig is NULL */ |

375 | double ww[MAX_VARS], z[MAX_EQNS], *w; |

376 | double *C[MAX_VARS]; /* for passing to Householder */ |

377 | double wmax, asave, u_p, unorm, sum; /* miscellany */ |

378 | int iter, itmax; |

379 | int index[MAX_VARS], p; |

380 | int i, ii, j, jj, k, l, q, zmax, index_zmax; |

381 | |

382 | /* Meanings of variables: |

383 | index[] is an array containing information on the sets Z and P. |

384 | index[0..p-1] is the set P |

385 | index[p..n-1] is the set Z |

386 | |

387 | z[] is working space |

388 | */ |

389 | |

390 | /* Check variables */ |

391 | if (m > MAX_EQNS || m <= 0) return -1; |

392 | if (n > MAX_VARS || n <= 0) return -2; |

393 | w = (lambda == NULL) ? ww : lambda; |

394 | rnorm = (rnorm_orig == NULL) ? &rnorm2 : rnorm_orig; |

395 | if (preserve) { |

396 | /* We allocate one long array and split it into pieces */ |

397 | if ((AA[0] = malloc(m*n*sizeof(double))) == NULL) |

398 | return -4; |

399 | for (i=0; i<n; i++) { |

400 | AA[i] = AA[0]+m*i; |

401 | for (j=0; j<m; j++) |

402 | AA[i][j] = A_orig[i][j]; |

403 | } |

404 | for (j=0; j<m; j++) |

405 | bb[j] = b_orig[j]; |

406 | A = AA; |

407 | b = bb; |

408 | } else { |

409 | A = A_orig; |

410 | b = b_orig; |

411 | } |

412 | |

413 | iter = 0; |

414 | itmax = 3*n; |

415 | |

416 | /* Initialise the index and x arrays */ |

417 | p = 0; |

418 | for (i=0; i<n; i++) { |

419 | x[i] = 0; |

420 | index[i] = i; |

421 | } |

422 | |

423 | /* Main loop */ |

424 | while (p<n && p<m) { |

425 | /* Compute the dual (negative gradient) vector w=A^T(b-Ax) |

426 | We are only interested in the components of this vector in Z, |

427 | so we only calculate these. It turns out that for these |

428 | components, we have w_j=(A^T.b)_j, and that (A^T.Ax)_j=0, |

429 | remembering that A is continually modified. I really don't |

430 | yet know why this is true. */ |

431 | |

432 | for (i=p; i<n; i++) { |

433 | j = index[i]; |

434 | sum = 0; |

435 | for (k=p; k<m; k++) |

436 | sum += Amatrix(k,j) * b[k]; |

437 | w[j] = sum; |

438 | } |

439 | |

440 | wmax = 0; |

441 | while (wmax == 0) { |

442 | /* We now find the maximum positive w_j. */ |

443 | zmax = -1; |

444 | for (i=p; i<n; i++) { |

445 | j = index[i]; |

446 | if (w[j]>wmax) { |

447 | wmax = w[j]; |

448 | zmax = i; |

449 | } |

450 | } |

451 | |

452 | if (wmax <= 0) /* all w_j non-positive, so done */ |

453 | goto terminate; |

454 | |

455 | /* We move index[zmax] from Z to P |

456 | We begin the transformation and check the new diagonal element |

457 | to avoid near linear dependence */ |

458 | index_zmax = index[zmax]; |

459 | asave = Amatrix(p,index_zmax); |

460 | |

461 | Householder(1, A[index_zmax], p, p+1, m, &u_p, NULL, 0); |

462 | /* u has ended up in the index_zmax-th column of A */ |

463 | unorm = 0; |

464 | for (k=0; k<p; k++) |

465 | unorm += Amatrix(k,index_zmax)*Amatrix(k,index_zmax); |

466 | unorm = sqrt(unorm); |

467 | |

468 | /* Is the index_zmax-th column sufficiently independent |

469 | (whatever that means)? */ |

470 | if ((unorm != 0 && fabs(Amatrix(p,index_zmax)/unorm) < TOL) || |

471 | (unorm == 0 && Amatrix(p,index_zmax) == 0)) { |

472 | /* no, it isn't */ |

473 | Amatrix(p,index_zmax) = asave; |

474 | w[index_zmax] = 0; |

475 | wmax = 0; |

476 | } else { |

477 | /* it is: copy b[] into z[], update z[] and solve for ztest, |

478 | the proposed new value for x[index_zmax] */ |

479 | double ztest; |

480 | for (i=0; i<m; i++) |

481 | z[i] = b[i]; |

482 | C[0] = z; /* Need to pass Householder an array of pointers */ |

483 | Householder(2, A[index_zmax], p, p+1, m, &u_p, C, 1); |

484 | ztest = z[p] / Amatrix(p,index_zmax); |

485 | if (ztest <= 0) { |

486 | /* nah, this won't do, try a different zmax */ |

487 | Amatrix(p,index_zmax) = asave; |

488 | w[index_zmax] = 0; |

489 | wmax = 0; |

490 | } |

491 | /* We've now got an acceptable index to move from Z to P! |

492 | We'll leave this while (wmax==0) loop now, then. */ |

493 | } |

494 | } |

495 | |

496 | /* And take heed of the fact that we've got our index */ |

497 | for (i=0; i<m; i++) |

498 | b[i] = z[i]; |

499 | index[zmax] = index[p]; |

500 | index[p] = index_zmax; |

501 | p++; |

502 | |

503 | /* We now apply our Householder transformation to all of the |

504 | columns of A which correspond to index[p], ..., index[n-1]. We |

505 | could call the Householder function for each column separately, |

506 | but it's probably more efficient (fewer function calls) to |

507 | bundle the columns up into one new temporary array. Note that |

508 | we've incremented p! */ |

509 | if (p<n) { |

510 | for (i=p; i<n; i++) { |

511 | C[i-p] = A[index[i]]; /* the index[i]-th column of A */ |

512 | } |

513 | Householder(2, A[index_zmax], p-1, p, m, &u_p, C, n-p); |

514 | } |

515 | |

516 | /* ... zero the index_zmax-th column of A below the |

517 | diagonal, and set w[index_zmax]=0 ... */ |

518 | for (i=p; i<m; i++) |

519 | Amatrix(i,index_zmax) = 0; |

520 | w[index_zmax] = 0; |

521 | |

522 | /* ... and solve the triangular system to solve the least squares |

523 | problem |A_P.z-b| (step 6 of algorithm), putting the solution |

524 | into z. Note also that a copy of b is still stored in z when |

525 | we begin. |

526 | |

527 | We have the matrix A_P which has j-th column being index[j], |

528 | j=0, ..., p-1. The j-th column is zero below the diagonal. So |

529 | we solve it by back-substitution. */ |

530 | |

531 | for (i=p-1; i>=0; i--) { |

532 | j = index[i]; |

533 | z[i] /= Amatrix(i,j); |

534 | for (k=0; k<i; k++) |

535 | z[k] -= Amatrix(k,j) * z[i]; |

536 | } |

537 | |

538 | /* This next part is called the "secondary loop" in the book */ |

539 | while (1) { |

540 | double alpha, tmp; |

541 | |

542 | if (++iter > itmax) /* over the iteration count, oops */ |

543 | goto terminate; |

544 | |

545 | alpha = 2; /* greater than the values we will be |

546 | comparing, which will all be <= 1 */ |

547 | for (i=0; i<p; i++) |

548 | if (z[i] <= 0) { |

549 | tmp = x[index[i]] / (x[index[i]]-z[i]); /* This is <= 1 */ |

550 | if (tmp<alpha) { |

551 | alpha = tmp; |

552 | q = i; /* keep track of the minimum index */ |

553 | } |

554 | } |

555 | |

556 | /* Were all z_j>0 for j in P? If so, set x:=z and break out of |

557 | this inner loop, returning to the main loop. */ |

558 | if (alpha > 1) { |

559 | /* the next four lines of code corresponds to lines 244-247 in |

560 | the Fortran 66 version */ |

561 | for (i=0; i<p; i++) { |

562 | x[index[i]] = z[i]; |

563 | } |

564 | break; |

565 | } |

566 | |

567 | /* Interpolate between old x and new z using 0<alpha<=1 */ |

568 | for (i=0; i<p; i++) |

569 | x[index[i]] += alpha * (z[i] - x[index[i]]); |

570 | |

571 | /* Now we have x[index[q]]=0, so modify A and b and the index |

572 | arrays to move index[q] and any other i with x[i]=0 from set |

573 | P to set Z */ |

574 | |

575 | while (q >= 0) { |

576 | i = index[q]; |

577 | x[i] = 0; /* ensure that rounding errors don't creep in */ |

578 | /* now use Givens rotations to fix up A and b */ |

579 | for (jj=q+1; jj<p; jj++) { |

580 | double c, s; |

581 | ii = index[jj]; |

582 | index[jj-1] = ii; |

583 | GivensCalc(Amatrix(jj-1,ii), Amatrix(jj,ii), |

584 | &c, &s, &Amatrix(jj-1,ii)); |

585 | Amatrix(jj,ii) = 0; |

586 | |

587 | for (l=0; l<n; l++) |

588 | if (l != ii) |

589 | GivensApply(c, s, &Amatrix(jj-1,l), &Amatrix(jj,l)); |

590 | GivensApply(c, s, &b[jj-1], &b[jj]); |

591 | } |

592 | /* We've taken one element out of P */ |

593 | p--; |

594 | index[p] = i; |

595 | |

596 | /* Check that the remaining coefficients in set P are |

597 | feasible, that is, >=0. They should be, because of |

598 | the way alpha was determined. If they are not, |

599 | this is due to rounding error, so we set any |

600 | non-positive elements to zero and move them too |

601 | from set P to set Z */ |

602 | for (q=p-1; q>=0; q--) |

603 | if (x[index[q]] <= 0) break; |

604 | /* now if q>=0, then x[index[q]] <= 0, |

605 | and if q<0, we are done */ |

606 | } /* end of while (q >= 0) loop */ |

607 | |

608 | /* OK, all x[i]=0 have now had their indices moved from P |

609 | to Z. We can now solve the least squares problem |

610 | |Az-b| once again, ready for the next secondary loop. |

611 | So we simply copy b into z and solve the triangular |

612 | system, as before. We could reduce the code size a |

613 | fraction by having this code at the beginning of the |

614 | loop, but that would make the loop exit code (checking |

615 | iter) appear in the middle, which would be a tad |

616 | confusing. */ |

617 | |

618 | for (i=0; i<m; i++) |

619 | z[i] = b[i]; |

620 | for (i=p-1; i>=0; i--) { |

621 | j = index[i]; |

622 | z[i] /= Amatrix(i,j); |

623 | for (k=0; k<i; k++) |

624 | z[k] -= Amatrix(k,j) * z[i]; |

625 | } |

626 | } /* end of secondary loop */ |

627 | } /* end of primary loop */ |

628 | |

629 | terminate: |

630 | sum = 0; |

631 | if (p < m) { |

632 | for (i=p; i<m; i++) |

633 | sum += b[i] * b[i]; |

634 | } else { |

635 | /* this is to get the w (dual) vector correct, in case we ever |

636 | decide to give it as an output to this function. */ |

637 | for (i=0; i<n; i++) |

638 | w[i] = 0; |

639 | } |

640 | |

641 | (*rnorm) = sqrt(sum); |

642 | |

643 | if (preserve) |

644 | free(AA[0]); |

645 | |

646 | return (iter > itmax) ? -3 : 0; |

647 | } |

648 | |

649 | |

650 | // Emacs settings: |

651 | // Local variables: |

652 | // mode: c |

653 | // c-basic-offset: 4 |

654 | // tab-width: 4 |

655 | // End: |

**Note:**See TracBrowser for help on using the repository browser.