void testLDLTAddTL()
{
int i,j;
dReal A[MSIZE4*MSIZE], L[MSIZE4*MSIZE], d[MSIZE], a[MSIZE],
DL[MSIZE4*MSIZE], ATEST[MSIZE4*MSIZE], diff;
HEADER;
dMakeRandomMatrix (A,MSIZE,MSIZE,1.0);
dMultiply2 (L,A,A,MSIZE,MSIZE,MSIZE);
memcpy (A,L,MSIZE4*MSIZE*sizeof(dReal));
dFactorLDLT (L,d,MSIZE,MSIZE4);
// delete first row and column of factorization
for (i=0; i<MSIZE; i++) a[i] = -A[i*MSIZE4];
a[0] += 1;
dLDLTAddTL (L,d,a,MSIZE,MSIZE4);
for (i=1; i<MSIZE; i++) L[i*MSIZE4] = 0;
d[0] = 1;
// get modified L*D*L'
dClearUpperTriangle (L,MSIZE);
for (i=0; i<MSIZE; i++) L[i*MSIZE4+i] = 1.0;
dSetZero (DL,MSIZE4*MSIZE);
for (i=0; i<MSIZE; i++) {
for (j=0; j<MSIZE; j++) DL[i*MSIZE4+j] = L[i*MSIZE4+j] / d[j];
}
dMultiply2 (ATEST,L,DL,MSIZE,MSIZE,MSIZE);
// compare it to A with its first row/column removed
for (i=1; i<MSIZE; i++) A[i*MSIZE4] = A[i] = 0;
A[0] = 1;
diff = dMaxDifference(A,ATEST,MSIZE,MSIZE);
printf ("\tmaximum difference = %.6e - %s\n",diff,
diff > tol ? "FAILED" : "passed");
}
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");
}
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");
}
void testFastLDLTFactorization()
{
int i,j;
dReal A[MSIZE4*MSIZE], L[MSIZE4*MSIZE], DL[MSIZE4*MSIZE],
ATEST[MSIZE4*MSIZE], d[MSIZE], diff;
HEADER;
dMakeRandomMatrix (A,MSIZE,MSIZE,1.0);
dMultiply2 (L,A,A,MSIZE,MSIZE,MSIZE);
memcpy (A,L,MSIZE4*MSIZE*sizeof(dReal));
dFactorLDLT (L,d,MSIZE,MSIZE4);
dClearUpperTriangle (L,MSIZE);
for (i=0; i<MSIZE; i++) L[i*MSIZE4+i] = 1.0;
dSetZero (DL,MSIZE4*MSIZE);
for (i=0; i<MSIZE; i++) {
for (j=0; j<MSIZE; j++) DL[i*MSIZE4+j] = L[i*MSIZE4+j] / d[j];
}
dMultiply2 (ATEST,L,DL,MSIZE,MSIZE,MSIZE);
diff = dMaxDifference(A,ATEST,MSIZE,MSIZE);
printf ("\tmaximum difference = %.6e - %s\n",diff,
diff > tol ? "FAILED" : "passed");
}
extern "C" ODE_API int dTestSolveLCP()
{
const int n = 100;
//size_t memreq = EstimateTestSolveLCPMemoryReq(n);
//dxWorldProcessMemArena *arena = dxAllocateTemporaryWorldProcessMemArena(memreq, nullptr, nullptr);
//if (arena == nullptr) {
// return 0;
//}
//int i,nskip = dPAD(n);
int i;
int nskip = n;
#ifdef dDOUBLE
const dReal tol = REAL(1e-9);
#endif
#ifdef dSINGLE
const dReal tol = REAL(1e-4);
#endif
printf ("dTestSolveLCP()\n");
dReal *A = new dReal[n*nskip];
dReal *x = new dReal[n];
dReal *b = new dReal[n];
dReal *w = new dReal[n];
dReal *lo = new dReal[n];
dReal *hi = new dReal[n];
dReal *A2 = new dReal[n*nskip];
dReal *b2 = new dReal[n];
dReal *lo2 = new dReal[n];
dReal *hi2 = new dReal[n];
dReal *tmp1 = new dReal[n];
dReal *tmp2 = new dReal[n];
// double total_time = 0;
for (int count=0; count < 1000; count++) {
// form (A,b) = a random positive definite LCP problem
dMakeRandomMatrix (A2,n,n,1.0);
dMultiply2 (A,A2,A2,n,n,n);
dMakeRandomMatrix (x,n,1,1.0);
dMultiply0 (b,A,x,n,n,1);
for (i=0; i<n; i++) b[i] += (dRandReal()*REAL(0.2))-REAL(0.1);
// choose `nub' in the range 0..n-1
int nub = 50; //dRandInt (n);
// make limits
for (i=0; i<nub; i++) lo[i] = -dInfinity;
for (i=0; i<nub; i++) hi[i] = dInfinity;
//for (i=nub; i<n; i++) lo[i] = 0;
//for (i=nub; i<n; i++) hi[i] = dInfinity;
//for (i=nub; i<n; i++) lo[i] = -dInfinity;
//for (i=nub; i<n; i++) hi[i] = 0;
for (i=nub; i<n; i++) lo[i] = -(dRandReal()*REAL(1.0))-REAL(0.01);
for (i=nub; i<n; i++) hi[i] = (dRandReal()*REAL(1.0))+REAL(0.01);
// set a few limits to lo=hi=0
/*
for (i=0; i<10; i++) {
int j = dRandInt (n-nub) + nub;
lo[j] = 0;
hi[j] = 0;
}
*/
// solve the LCP. we must make copy of A,b,lo,hi (A2,b2,lo2,hi2) for
// SolveLCP() to permute. also, we'll clear the upper triangle of A2 to
// ensure that it doesn't get referenced (if it does, the answer will be
// wrong).
memcpy (A2,A,n*nskip*sizeof(dReal));
dClearUpperTriangle (A2,n);
memcpy (b2,b,n*sizeof(dReal));
memcpy (lo2,lo,n*sizeof(dReal));
memcpy (hi2,hi,n*sizeof(dReal));
dSetZero (x,n);
dSetZero (w,n);
dSolveLCP (n,A2,x,b2,w,nub,lo2,hi2,0);
// check the solution
dMultiply0 (tmp1,A,x,n,n,1);
for (i=0; i<n; i++) tmp2[i] = b[i] + w[i];
dReal diff = dMaxDifference (tmp1,tmp2,n,1);
// printf ("\tA*x = b+w, maximum difference = %.6e - %s (1)\n",diff,
// diff > tol ? "FAILED" : "passed");
if (diff > tol) dDebug (0,"A*x = b+w, maximum difference = %.6e",diff);
int n1=0,n2=0,n3=0;
for (i=0; i<n; i++) {
if (x[i]==lo[i] && w[i] >= 0) {
n1++; // ok
}
else if (x[i]==hi[i] && w[i] <= 0) {
n2++; // ok
}
else if (x[i] >= lo[i] && x[i] <= hi[i] && w[i] == 0) {
n3++; // ok
}
else {
dDebug (0,"FAILED: i=%d x=%.4e w=%.4e lo=%.4e hi=%.4e",i,
x[i],w[i],lo[i],hi[i]);
}
}
// pacifier
printf ("passed: NL=%3d NH=%3d C=%3d ",n1,n2,n3);
}
delete[] A;
delete[] x;
delete[] b;
delete[] w;
delete[] lo ;
delete[] hi ;
delete[] A2 ;
delete[] b2 ;
delete[] lo2;
delete[] hi2;
delete[] tmp1;
delete[] tmp2;
return 1;
}
extern "C" ODE_API int dTestSolveLCP()
{
const int n = 100;
size_t memreq = EstimateTestSolveLCPMemoryReq(n);
dxWorldProcessMemArena *arena = dxAllocateTemporaryWorldProcessMemArena(memreq, NULL, NULL);
if (arena == NULL) {
return 0;
}
int i,nskip = dPAD(n);
#ifdef dDOUBLE
const dReal tol = REAL(1e-9);
#endif
#ifdef dSINGLE
const dReal tol = REAL(1e-4);
#endif
printf ("dTestSolveLCP()\n");
dReal *A = arena->AllocateArray<dReal> (n*nskip);
dReal *x = arena->AllocateArray<dReal> (n);
dReal *b = arena->AllocateArray<dReal> (n);
dReal *w = arena->AllocateArray<dReal> (n);
dReal *lo = arena->AllocateArray<dReal> (n);
dReal *hi = arena->AllocateArray<dReal> (n);
dReal *A2 = arena->AllocateArray<dReal> (n*nskip);
dReal *b2 = arena->AllocateArray<dReal> (n);
dReal *lo2 = arena->AllocateArray<dReal> (n);
dReal *hi2 = arena->AllocateArray<dReal> (n);
dReal *tmp1 = arena->AllocateArray<dReal> (n);
dReal *tmp2 = arena->AllocateArray<dReal> (n);
double total_time = 0;
for (int count=0; count < 1000; count++) {
BEGIN_STATE_SAVE(arena, saveInner) {
// form (A,b) = a random positive definite LCP problem
dMakeRandomMatrix (A2,n,n,1.0);
dMultiply2 (A,A2,A2,n,n,n);
dMakeRandomMatrix (x,n,1,1.0);
dMultiply0 (b,A,x,n,n,1);
for (i=0; i<n; i++) b[i] += (dRandReal()*REAL(0.2))-REAL(0.1);
// choose `nub' in the range 0..n-1
int nub = 50; //dRandInt (n);
// make limits
for (i=0; i<nub; i++) lo[i] = -dInfinity;
for (i=0; i<nub; i++) hi[i] = dInfinity;
//for (i=nub; i<n; i++) lo[i] = 0;
//for (i=nub; i<n; i++) hi[i] = dInfinity;
//for (i=nub; i<n; i++) lo[i] = -dInfinity;
//for (i=nub; i<n; i++) hi[i] = 0;
for (i=nub; i<n; i++) lo[i] = -(dRandReal()*REAL(1.0))-REAL(0.01);
for (i=nub; i<n; i++) hi[i] = (dRandReal()*REAL(1.0))+REAL(0.01);
// set a few limits to lo=hi=0
/*
for (i=0; i<10; i++) {
int j = dRandInt (n-nub) + nub;
lo[j] = 0;
hi[j] = 0;
}
*/
// solve the LCP. we must make copy of A,b,lo,hi (A2,b2,lo2,hi2) for
// SolveLCP() to permute. also, we'll clear the upper triangle of A2 to
// ensure that it doesn't get referenced (if it does, the answer will be
// wrong).
memcpy (A2,A,n*nskip*sizeof(dReal));
dClearUpperTriangle (A2,n);
memcpy (b2,b,n*sizeof(dReal));
memcpy (lo2,lo,n*sizeof(dReal));
memcpy (hi2,hi,n*sizeof(dReal));
dSetZero (x,n);
dSetZero (w,n);
dStopwatch sw;
dStopwatchReset (&sw);
dStopwatchStart (&sw);
dSolveLCP (arena,n,A2,x,b2,w,nub,lo2,hi2,0);
dStopwatchStop (&sw);
double time = dStopwatchTime(&sw);
total_time += time;
double average = total_time / double(count+1) * 1000.0;
// check the solution
dMultiply0 (tmp1,A,x,n,n,1);
for (i=0; i<n; i++) tmp2[i] = b[i] + w[i];
dReal diff = dMaxDifference (tmp1,tmp2,n,1);
// printf ("\tA*x = b+w, maximum difference = %.6e - %s (1)\n",diff,
// diff > tol ? "FAILED" : "passed");
if (diff > tol) dDebug (0,"A*x = b+w, maximum difference = %.6e",diff);
int n1=0,n2=0,n3=0;
for (i=0; i<n; i++) {
if (x[i]==lo[i] && w[i] >= 0) {
n1++; // ok
}
else if (x[i]==hi[i] && w[i] <= 0) {
n2++; // ok
}
else if (x[i] >= lo[i] && x[i] <= hi[i] && w[i] == 0) {
n3++; // ok
}
else {
dDebug (0,"FAILED: i=%d x=%.4e w=%.4e lo=%.4e hi=%.4e",i,
x[i],w[i],lo[i],hi[i]);
}
}
// pacifier
printf ("passed: NL=%3d NH=%3d C=%3d ",n1,n2,n3);
printf ("time=%10.3f ms avg=%10.4f\n",time * 1000.0,average);
} END_STATE_SAVE(arena, saveInner);
}
void testLDLTRemove()
{
int i,j,r,p[MSIZE];
dReal A[MSIZE4*MSIZE], L[MSIZE4*MSIZE], d[MSIZE],
L2[MSIZE4*MSIZE], d2[MSIZE], DL2[MSIZE4*MSIZE],
Atest1[MSIZE4*MSIZE], Atest2[MSIZE4*MSIZE], diff, maxdiff;
HEADER;
// make array of A row pointers
dReal *Arows[MSIZE];
for (i=0; i<MSIZE; i++) Arows[i] = A+i*MSIZE4;
// fill permutation vector
for (i=0; i<MSIZE; i++) p[i]=i;
dMakeRandomMatrix (A,MSIZE,MSIZE,1.0);
dMultiply2 (L,A,A,MSIZE,MSIZE,MSIZE);
memcpy (A,L,MSIZE4*MSIZE*sizeof(dReal));
dFactorLDLT (L,d,MSIZE,MSIZE4);
maxdiff = 1e10;
for (r=0; r<MSIZE; r++) {
// get Atest1 = A with row/column r removed
memcpy (Atest1,A,MSIZE4*MSIZE*sizeof(dReal));
dRemoveRowCol (Atest1,MSIZE,MSIZE4,r);
// test that the row/column removal worked
int bad = 0;
for (i=0; i<MSIZE; i++) {
for (j=0; j<MSIZE; j++) {
if (i != r && j != r) {
int ii = i;
int jj = j;
if (ii >= r) ii--;
if (jj >= r) jj--;
if (A[i*MSIZE4+j] != Atest1[ii*MSIZE4+jj]) bad = 1;
}
}
}
if (bad) printf ("\trow/col removal FAILED for row %d\n",r);
// zero out last row/column of Atest1
for (i=0; i<MSIZE; i++) {
Atest1[(MSIZE-1)*MSIZE4+i] = 0;
Atest1[i*MSIZE4+MSIZE-1] = 0;
}
// get L2*D2*L2' = adjusted factorization to remove that row
memcpy (L2,L,MSIZE4*MSIZE*sizeof(dReal));
memcpy (d2,d,MSIZE*sizeof(dReal));
dLDLTRemove (/*A*/ Arows,p,L2,d2,MSIZE,MSIZE,r,MSIZE4);
// get Atest2 = L2*D2*L2'
dClearUpperTriangle (L2,MSIZE);
for (i=0; i<(MSIZE-1); i++) L2[i*MSIZE4+i] = 1.0;
for (i=0; i<MSIZE; i++) L2[(MSIZE-1)*MSIZE4+i] = 0;
d2[MSIZE-1] = 1;
dSetZero (DL2,MSIZE4*MSIZE);
for (i=0; i<(MSIZE-1); i++) {
for (j=0; j<MSIZE-1; j++) DL2[i*MSIZE4+j] = L2[i*MSIZE4+j] / d2[j];
}
dMultiply2 (Atest2,L2,DL2,MSIZE,MSIZE,MSIZE);
diff = dMaxDifference(Atest1,Atest2,MSIZE,MSIZE);
if (diff < maxdiff) maxdiff = diff;
/*
dPrintMatrix (Atest1,MSIZE,MSIZE);
printf ("\n");
dPrintMatrix (Atest2,MSIZE,MSIZE);
printf ("\n");
*/
}
printf ("\tmaximum difference = %.6e - %s\n",maxdiff,
maxdiff > tol ? "FAILED" : "passed");
}
