cs_lu.c
Go to the documentation of this file.
00001 #include "cs.h"
00002 /* [L,U,pinv]=lu(A, [q lnz unz]). lnz and unz can be guess */
00003 csn *cs_lu (const cs *A, const css *S, double tol)
00004 {
00005     cs *L, *U ;
00006     csn *N ;
00007     double pivot, *Lx, *Ux, *x,  a, t ;
00008     int *Lp, *Li, *Up, *Ui, *pinv, *xi, *q, n, ipiv, k, top, p, i, col, lnz,unz;
00009     if (!CS_CSC (A) || !S) return (NULL) ;          /* check inputs */
00010     n = A->n ;
00011     q = S->q ; lnz = (int)S->lnz ; unz = (int)S->unz ;
00012     x = (double*) cs_malloc (n, sizeof (double)) ;            /* get double workspace */
00013     xi = (int*) cs_malloc (2*n, sizeof (int)) ;            /* get int workspace */
00014     N = (csn*) cs_calloc (1, sizeof (csn)) ;               /* allocate result */
00015     if (!x || !xi || !N) return (cs_ndone (N, NULL, xi, x, 0)) ;
00016     N->L = L = cs_spalloc (n, n, lnz, 1, 0) ;       /* allocate result L */
00017     N->U = U = cs_spalloc (n, n, unz, 1, 0) ;       /* allocate result U */
00018     N->pinv = pinv = (int*) cs_malloc (n, sizeof (int)) ;  /* allocate result pinv */
00019     if (!L || !U || !pinv) return (cs_ndone (N, NULL, xi, x, 0)) ;
00020     Lp = L->p ; Up = U->p ;
00021     for (i = 0 ; i < n ; i++) x [i] = 0 ;           /* clear workspace */
00022     for (i = 0 ; i < n ; i++) pinv [i] = -1 ;       /* no rows pivotal yet */
00023     for (k = 0 ; k <= n ; k++) Lp [k] = 0 ;         /* no cols of L yet */
00024     lnz = unz = 0 ;
00025     for (k = 0 ; k < n ; k++)       /* compute L(:,k) and U(:,k) */
00026     {
00027         /* --- Triangular solve --------------------------------------------- */
00028         Lp [k] = lnz ;              /* L(:,k) starts here */
00029         Up [k] = unz ;              /* U(:,k) starts here */
00030         if ((lnz + n > L->nzmax && !cs_sprealloc (L, 2*L->nzmax + n)) ||
00031             (unz + n > U->nzmax && !cs_sprealloc (U, 2*U->nzmax + n)))
00032         {
00033             return (cs_ndone (N, NULL, xi, x, 0)) ;
00034         }
00035         Li = L->i ; Lx = L->x ; Ui = U->i ; Ux = U->x ;
00036         col = q ? (q [k]) : k ;
00037         top = cs_spsolve (L, A, col, xi, x, pinv, 1) ;  /* x = L\A(:,col) */
00038         /* --- Find pivot --------------------------------------------------- */
00039         ipiv = -1 ;
00040         a = -1 ;
00041         for (p = top ; p < n ; p++)
00042         {
00043             i = xi [p] ;            /* x(i) is nonzero */
00044             if (pinv [i] < 0)       /* row i is not yet pivotal */
00045             {
00046                 if ((t = fabs (x [i])) > a)
00047                 {
00048                     a = t ;         /* largest pivot candidate so far */
00049                     ipiv = i ;
00050                 }
00051             }
00052             else                    /* x(i) is the entry U(pinv[i],k) */
00053             {
00054                 Ui [unz] = pinv [i] ;
00055                 Ux [unz++] = x [i] ;
00056             }
00057         }
00058         if (ipiv == -1 || a <= 0) return (cs_ndone (N, NULL, xi, x, 0)) ;
00059         if (pinv [col] < 0 && fabs (x [col]) >= a*tol) ipiv = col ;
00060         /* --- Divide by pivot ---------------------------------------------- */
00061         pivot = x [ipiv] ;          /* the chosen pivot */
00062         Ui [unz] = k ;              /* last entry in U(:,k) is U(k,k) */
00063         Ux [unz++] = pivot ;
00064         pinv [ipiv] = k ;           /* ipiv is the kth pivot row */
00065         Li [lnz] = ipiv ;           /* first entry in L(:,k) is L(k,k) = 1 */
00066         Lx [lnz++] = 1 ;
00067         for (p = top ; p < n ; p++) /* L(k+1:n,k) = x / pivot */
00068         {
00069             i = xi [p] ;
00070             if (pinv [i] < 0)       /* x(i) is an entry in L(:,k) */
00071             {
00072                 Li [lnz] = i ;      /* save unpermuted row in L */
00073                 Lx [lnz++] = x [i] / pivot ;    /* scale pivot column */
00074             }
00075             x [i] = 0 ;             /* x [0..n-1] = 0 for next k */
00076         }
00077     }
00078     /* --- Finalize L and U ------------------------------------------------- */
00079     Lp [n] = lnz ;
00080     Up [n] = unz ;
00081     Li = L->i ;                     /* fix row indices of L for final pinv */
00082     for (p = 0 ; p < lnz ; p++) Li [p] = pinv [Li [p]] ;
00083     cs_sprealloc (L, 0) ;           /* remove extra space from L and U */
00084     cs_sprealloc (U, 0) ;
00085     return (cs_ndone (N, NULL, xi, x, 1)) ;     /* success */
00086 }


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Thu Aug 27 2015 11:58:04