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"); }