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;
}
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 dIsPositiveDefinite (const dReal *A, int n)
{
dReal *Acopy;
dAASSERT (n > 0 && A);
int nskip = dPAD (n);
Acopy = (dReal*) ALLOCA (nskip*n * sizeof(dReal));
memcpy (Acopy,A,nskip*n * sizeof(dReal));
return dFactorCholesky (Acopy,n);
}
int _dIsPositiveDefinite (const dReal *A, int n, void *tmpbuf/*[nskip*(n+1)]*/)
{
dAASSERT (n > 0 && A);
size_t FactorCholesky_size = _dEstimateFactorCholeskyTmpbufSize(n);
dIASSERT(FactorCholesky_size % sizeof(dReal) == 0);
const int nskip = dPAD (n);
const int nskip_mul_n = nskip*n;
dReal *tmp = tmpbuf ? (dReal *)tmpbuf : (dReal*) ALLOCA (FactorCholesky_size + nskip_mul_n*sizeof(dReal));
dReal *Acopy = (dReal *)((char *)tmp + FactorCholesky_size);
memcpy (Acopy, A, nskip_mul_n * sizeof(dReal));
return dFactorCholesky (Acopy, n, tmp);
}
void testCholeskyFactorization()
{
dReal A[MSIZE4*MSIZE], B[MSIZE4*MSIZE], C[MSIZE4*MSIZE], diff;
HEADER;
dMakeRandomMatrix (A,MSIZE,MSIZE,1.0);
dMultiply2 (B,A,A,MSIZE,MSIZE,MSIZE);
memcpy (A,B,MSIZE4*MSIZE*sizeof(dReal));
if (dFactorCholesky (B,MSIZE)) printf ("\tpassed (1)\n");
else printf ("\tFAILED (1)\n");
dClearUpperTriangle (B,MSIZE);
dMultiply2 (C,B,B,MSIZE,MSIZE,MSIZE);
diff = dMaxDifference(A,C,MSIZE,MSIZE);
printf ("\tmaximum difference = %.6e - %s (2)\n",diff,
diff > tol ? "FAILED" : "passed");
}
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;
}
