void testCholeskySolve() { dReal A[MSIZE4*MSIZE], L[MSIZE4*MSIZE], b[MSIZE],x[MSIZE],btest[MSIZE],diff; HEADER; // get A,L = PD matrix dMakeRandomMatrix (A,MSIZE,MSIZE,1.0); dMultiply2 (L,A,A,MSIZE,MSIZE,MSIZE); memcpy (A,L,MSIZE4*MSIZE*sizeof(dReal)); // get b,x = right hand side dMakeRandomMatrix (b,MSIZE,1,1.0); memcpy (x,b,MSIZE*sizeof(dReal)); // factor L if (dFactorCholesky (L,MSIZE)) printf ("\tpassed (1)\n"); else printf ("\tFAILED (1)\n"); dClearUpperTriangle (L,MSIZE); // solve A*x = b dSolveCholesky (L,x,MSIZE); // compute A*x and compare it with b dMultiply2 (btest,A,x,MSIZE,MSIZE,1); diff = dMaxDifference(b,btest,MSIZE,1); printf ("\tmaximum difference = %.6e - %s (2)\n",diff, diff > tol ? "FAILED" : "passed"); }
int _dInvertPDMatrix (const dReal *A, dReal *Ainv, int n, void *tmpbuf/*[nskip*(n+2)]*/) { dAASSERT (n > 0 && A && Ainv); bool success = false; size_t FactorCholesky_size = _dEstimateFactorCholeskyTmpbufSize(n); size_t SolveCholesky_size = _dEstimateSolveCholeskyTmpbufSize(n); size_t MaxCholesky_size = FactorCholesky_size > SolveCholesky_size ? FactorCholesky_size : SolveCholesky_size; dIASSERT(MaxCholesky_size % sizeof(dReal) == 0); const int nskip = dPAD (n); const int nskip_mul_n = nskip*n; dReal *tmp = tmpbuf ? (dReal *)tmpbuf : (dReal*) ALLOCA (MaxCholesky_size + (nskip + nskip_mul_n)*sizeof(dReal)); dReal *X = (dReal *)((char *)tmp + MaxCholesky_size); dReal *L = X + nskip; memcpy (L, A, nskip_mul_n*sizeof(dReal)); if (dFactorCholesky (L,n,tmp)) { dSetZero (Ainv,nskip_mul_n); // make sure all padding elements set to 0 dReal *aa = Ainv, *xi = X, *xiend = X + n; for (; xi != xiend; ++aa, ++xi) { dSetZero(X, n); *xi = REAL(1.0); dSolveCholesky (L,X,n,tmp); dReal *a = aa; const dReal *x = X, *xend = X + n; for (; x!=xend; a+=nskip, ++x) { *a = *x; } } success = true; } return success ? 1 : 0; }
int dInvertPDMatrix (const dReal *A, dReal *Ainv, int n) { int i,j,nskip; dReal *L,*x; dAASSERT (n > 0 && A && Ainv); nskip = dPAD (n); L = (dReal*) ALLOCA (nskip*n*sizeof(dReal)); memcpy (L,A,nskip*n*sizeof(dReal)); x = (dReal*) ALLOCA (n*sizeof(dReal)); if (dFactorCholesky (L,n)==0) return 0; dSetZero (Ainv,n*nskip); // make sure all padding elements set to 0 for (i=0; i<n; i++) { for (j=0; j<n; j++) x[j] = 0; x[i] = 1; dSolveCholesky (L,x,n); for (j=0; j<n; j++) Ainv[j*nskip+i] = x[j]; } return 1; }