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 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 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 testInvertPDMatrix()
{
int i,j,ok;
dReal A[MSIZE4*MSIZE], Ainv[MSIZE4*MSIZE], I[MSIZE4*MSIZE];
HEADER;
dMakeRandomMatrix (A,MSIZE,MSIZE,1.0);
dMultiply2 (Ainv,A,A,MSIZE,MSIZE,MSIZE);
memcpy (A,Ainv,MSIZE4*MSIZE*sizeof(dReal));
dSetZero (Ainv,MSIZE4*MSIZE);
if (dInvertPDMatrix (A,Ainv,MSIZE))
printf ("\tpassed (1)\n"); else printf ("\tFAILED (1)\n");
dMultiply0 (I,A,Ainv,MSIZE,MSIZE,MSIZE);
// compare with identity
ok = 1;
for (i=0; i<MSIZE; i++) {
for (j=0; j<MSIZE; j++) {
if (i != j) if (cmp (I[i*MSIZE4+j],0.0)==0) ok = 0;
}
}
for (i=0; i<MSIZE; i++) {
if (cmp (I[i*MSIZE4+i],1.0)==0) ok = 0;
}
if (ok) printf ("\tpassed (2)\n"); else printf ("\tFAILED (2)\n");
}
void testMatrixMultiply()
{
// A is 2x3, B is 3x4, B2 is B except stored columnwise, C is 2x4
dReal A[8],B[12],A2[12],B2[16],C[8];
int i;
HEADER;
dSetZero (A,8);
for (i=0; i<3; i++) A[i] = i+2;
for (i=0; i<3; i++) A[i+4] = i+3+2;
for (i=0; i<12; i++) B[i] = i+8;
dSetZero (A2,12);
for (i=0; i<6; i++) A2[i+2*(i/2)] = A[i+i/3];
dSetZero (B2,16);
for (i=0; i<12; i++) B2[i+i/3] = B[i];
dMultiply0 (C,A,B,2,3,4);
if (C[0] != 116 || C[1] != 125 || C[2] != 134 || C[3] != 143 ||
C[4] != 224 || C[5] != 242 || C[6] != 260 || C[7] != 278)
printf ("\tFAILED (1)\n"); else printf ("\tpassed (1)\n");
dMultiply1 (C,A2,B,2,3,4);
if (C[0] != 160 || C[1] != 172 || C[2] != 184 || C[3] != 196 ||
C[4] != 196 || C[5] != 211 || C[6] != 226 || C[7] != 241)
printf ("\tFAILED (2)\n"); else printf ("\tpassed (2)\n");
dMultiply2 (C,A,B2,2,3,4);
if (C[0] != 83 || C[1] != 110 || C[2] != 137 || C[3] != 164 ||
C[4] != 164 || C[5] != 218 || C[6] != 272 || C[7] != 326)
printf ("\tFAILED (3)\n"); else printf ("\tpassed (3)\n");
}
void testIsPositiveDefinite()
{
dReal A[MSIZE4*MSIZE], B[MSIZE4*MSIZE];
HEADER;
dMakeRandomMatrix (A,MSIZE,MSIZE,1.0);
dMultiply2 (B,A,A,MSIZE,MSIZE,MSIZE);
printf ("\t%s\n",dIsPositiveDefinite(A,MSIZE) ? "FAILED (1)":"passed (1)");
printf ("\t%s\n",dIsPositiveDefinite(B,MSIZE) ? "passed (2)":"FAILED (2)");
}
void testSolveLDLT()
{
dReal A[MSIZE4*MSIZE], L[MSIZE4*MSIZE], d[MSIZE], x[MSIZE],
b[MSIZE], btest[MSIZE], diff;
HEADER;
dMakeRandomMatrix (A,MSIZE,MSIZE,1.0);
dMultiply2 (L,A,A,MSIZE,MSIZE,MSIZE);
memcpy (A,L,MSIZE4*MSIZE*sizeof(dReal));
dMakeRandomMatrix (b,MSIZE,1,1.0);
memcpy (x,b,MSIZE*sizeof(dReal));
dFactorLDLT (L,d,MSIZE,MSIZE4);
dSolveLDLT (L,d,x,MSIZE,MSIZE4);
dMultiply2 (btest,A,x,MSIZE,MSIZE,1);
diff = dMaxDifference(b,btest,MSIZE,1);
printf ("\tmaximum difference = %.6e - %s\n",diff,
diff > tol ? "FAILED" : "passed");
}
void testRtoQandQtoR()
{
HEADER;
dMatrix3 R,I,R2;
dQuaternion q;
int i;
// test makeRandomRotation()
makeRandomRotation (R);
dMultiply2 (I,R,R,3,3,3);
printf ("\tmakeRandomRotation() - %s (1)\n",
cmpIdentityMat3(I) ? "passed" : "FAILED");
// test QtoR() on random normalized quaternions
int ok = 1;
for (i=0; i<100; i++) {
dMakeRandomVector (q,4,1.0);
dNormalize4 (q);
dQtoR (q,R);
dMultiply2 (I,R,R,3,3,3);
if (cmpIdentityMat3(I)==0) ok = 0;
}
printf ("\tQtoR() orthonormality %s (2)\n", ok ? "passed" : "FAILED");
// test R -> Q -> R works
dReal maxdiff=0;
for (i=0; i<100; i++) {
makeRandomRotation (R);
dRtoQ (R,q);
dQtoR (q,R2);
dReal diff = dMaxDifference (R,R2,3,3);
if (diff > maxdiff) maxdiff = diff;
}
printf ("\tmaximum difference = %e - %s (3)\n",maxdiff,
(maxdiff > 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");
}
void testSmallMatrixMultiply()
{
dMatrix3 A,B,C,A2;
dVector3 a,a2,x;
HEADER;
dMakeRandomMatrix (A,3,3,1.0);
dMakeRandomMatrix (B,3,3,1.0);
dMakeRandomMatrix (C,3,3,1.0);
dMakeRandomMatrix (x,3,1,1.0);
// dMultiply0_331()
dMultiply0_331 (a,B,x);
dMultiply0 (a2,B,x,3,3,1);
printf ("\t%s (1)\n",(dMaxDifference (a,a2,3,1) > tol) ? "FAILED" :
"passed");
// dMultiply1_331()
dMultiply1_331 (a,B,x);
dMultiply1 (a2,B,x,3,3,1);
printf ("\t%s (2)\n",(dMaxDifference (a,a2,3,1) > tol) ? "FAILED" :
"passed");
// dMultiply0_133
dMultiply0_133 (a,x,B);
dMultiply0 (a2,x,B,1,3,3);
printf ("\t%s (3)\n",(dMaxDifference (a,a2,1,3) > tol) ? "FAILED" :
"passed");
// dMultiply0_333()
dMultiply0_333 (A,B,C);
dMultiply0 (A2,B,C,3,3,3);
printf ("\t%s (4)\n",(dMaxDifference (A,A2,3,3) > tol) ? "FAILED" :
"passed");
// dMultiply1_333()
dMultiply1_333 (A,B,C);
dMultiply1 (A2,B,C,3,3,3);
printf ("\t%s (5)\n",(dMaxDifference (A,A2,3,3) > tol) ? "FAILED" :
"passed");
// dMultiply2_333()
dMultiply2_333 (A,B,C);
dMultiply2 (A2,B,C,3,3,3);
printf ("\t%s (6)\n",(dMaxDifference (A,A2,3,3) > tol) ? "FAILED" :
"passed");
}
void testQuaternionMultiply()
{
HEADER;
dMatrix3 RA,RB,RC,Rtest;
dQuaternion qa,qb,qc;
dReal diff,maxdiff=0;
for (int i=0; i<100; i++) {
makeRandomRotation (RB);
makeRandomRotation (RC);
dRtoQ (RB,qb);
dRtoQ (RC,qc);
dMultiply0 (RA,RB,RC,3,3,3);
dQMultiply0 (qa,qb,qc);
dQtoR (qa,Rtest);
diff = dMaxDifference (Rtest,RA,3,3);
if (diff > maxdiff) maxdiff = diff;
dMultiply1 (RA,RB,RC,3,3,3);
dQMultiply1 (qa,qb,qc);
dQtoR (qa,Rtest);
diff = dMaxDifference (Rtest,RA,3,3);
if (diff > maxdiff) maxdiff = diff;
dMultiply2 (RA,RB,RC,3,3,3);
dQMultiply2 (qa,qb,qc);
dQtoR (qa,Rtest);
diff = dMaxDifference (Rtest,RA,3,3);
if (diff > maxdiff) maxdiff = diff;
dMultiply0 (RA,RC,RB,3,3,3);
transpose3x3 (RA);
dQMultiply3 (qa,qb,qc);
dQtoR (qa,Rtest);
diff = dMaxDifference (Rtest,RA,3,3);
if (diff > maxdiff) maxdiff = diff;
}
printf ("\tmaximum difference = %e - %s\n",maxdiff,
(maxdiff > 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");
}
