00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include "csparse_helper.h"
00018
00019 #include <cassert>
00020
00021 namespace AISNavigation {
00022
00023 int cs_cholsolsymb(const cs *A, double *b, const css* S, double* x, int* work)
00024 {
00025 csn *N ;
00026 int n, ok ;
00027 if (!CS_CSC (A) || !b || ! S || !x) {
00028 fprintf(stderr, "%s: No valid input!\n", __PRETTY_FUNCTION__);
00029 assert(0);
00030 return (0) ;
00031 }
00032 n = A->n ;
00033 N = cs_chol_workspace (A, S, work, x) ;
00034 if (!N) {
00035 fprintf(stderr, "%s: cholesky failed!\n", __PRETTY_FUNCTION__);
00036
00037 }
00038 ok = (N != NULL) ;
00039 if (ok)
00040 {
00041 cs_ipvec (S->pinv, b, x, n) ;
00042 cs_lsolve (N->L, x) ;
00043 cs_ltsolve (N->L, x) ;
00044 cs_pvec (S->pinv, x, b, n) ;
00045 }
00046 cs_nfree (N) ;
00047 return (ok) ;
00048 }
00049
00050 int cs_cholsolinvblocksymb(const cs *A, double **block, int r1, int c1, int r2,int c2, double* y, const css* S, double* x, double* b, double* temp, int* work)
00051 {
00052 csn *N ;
00053 int n, ok, i, j ;
00054 if (!CS_CSC (A) || !block || !y || !x || !b || !temp) return (0) ;
00055 n = A->n ;
00056 if(r2<=r1||c2<=c1) return (0);
00057 if(c1<0 || c2>n)
00058 return 0;
00059 if(r1<0 || r2>n)
00060 return 0;
00061
00062 N = cs_chol_workspace (A, S, work, x) ;
00063 ok = (N != NULL) ;
00064 if (ok)
00065 {
00066
00067 cs_ipvec (S->pinv, y, x, n) ;
00068 cs_lsolve (N->L, x) ;
00069 cs_ltsolve (N->L, x) ;
00070 cs_pvec (S->pinv, x, y, n) ;
00071
00072
00073 for (i=0; i<n; i++){
00074 b[i]=0.;
00075 }
00076 for (i=c1; i<c2; i++){
00077 b[i]=1.;
00078
00079 cs_ipvec (S->pinv, b, x, n) ;
00080 cs_lsolve (N->L, x) ;
00081 cs_ltsolve (N->L, x) ;
00082 cs_pvec (S->pinv, x, temp, n) ;
00083 for (j=r1; j<r2; j++){
00084 block[j-r1][i-c1]=temp[j];
00085 }
00086 b[i]=0.;
00087 }
00088 }
00089 cs_nfree (N) ;
00090 return (ok) ;
00091 }
00092
00093
00094 csn* cs_chol_workspace (const cs *A, const css *S, int* cin, double* xin)
00095 {
00096 double d, lki, *Lx, *x, *Cx ;
00097 int top, i, p, k, n, *Li, *Lp, *cp, *pinv, *s, *c, *parent, *Cp, *Ci ;
00098 cs *L, *C, *E ;
00099 csn *N ;
00100 if (!CS_CSC (A) || !S || !S->cp || !S->parent) return (NULL) ;
00101 n = A->n ;
00102 N = (csn*) cs_calloc (1, sizeof (csn)) ;
00103 c = cin ;
00104 x = xin ;
00105 cp = S->cp ; pinv = S->pinv ; parent = S->parent ;
00106 C = pinv ? cs_symperm (A, pinv, 1) : ((cs *) A) ;
00107 E = pinv ? C : NULL ;
00108 if (!N || !c || !x || !C) return (cs_ndone (N, E, NULL, NULL, 0)) ;
00109 s = c + n ;
00110 Cp = C->p ; Ci = C->i ; Cx = C->x ;
00111 N->L = L = cs_spalloc (n, n, cp [n], 1, 0) ;
00112 if (!L) return (cs_ndone (N, E, NULL, NULL, 0)) ;
00113 Lp = L->p ; Li = L->i ; Lx = L->x ;
00114 for (k = 0 ; k < n ; k++) Lp [k] = c [k] = cp [k] ;
00115 for (k = 0 ; k < n ; k++)
00116 {
00117
00118 top = cs_ereach (C, k, parent, s, c) ;
00119 x [k] = 0 ;
00120 for (p = Cp [k] ; p < Cp [k+1] ; p++)
00121 {
00122 if (Ci [p] <= k) x [Ci [p]] = Cx [p] ;
00123 }
00124 d = x [k] ;
00125 x [k] = 0 ;
00126
00127 for ( ; top < n ; top++)
00128 {
00129 i = s [top] ;
00130 lki = x [i] / Lx [Lp [i]] ;
00131 x [i] = 0 ;
00132 for (p = Lp [i] + 1 ; p < c [i] ; p++)
00133 {
00134 x [Li [p]] -= Lx [p] * lki ;
00135 }
00136 d -= lki * lki ;
00137 p = c [i]++ ;
00138 Li [p] = k ;
00139 Lx [p] = lki ;
00140 }
00141
00142 if (d <= 0) return (cs_ndone (N, E, NULL, NULL, 0)) ;
00143 p = c [k]++ ;
00144 Li [p] = k ;
00145 Lx [p] = sqrt (d) ;
00146 }
00147 Lp [n] = cp [n] ;
00148 return (cs_ndone (N, E, NULL, NULL, 1)) ;
00149 }
00150
00151 }