void LUinvert(dmatrix lu, dmatrix inv, int dim, int *ndx){
int r,c;
dvector b = newdvector(dim);
for (c=0; c<dim; c++){
b[c]=1.0;
LUsolve(lu,dim,ndx,b);
for (r=0; r<dim; r++) inv[r][c]= b[r];
}
freevector((void*)b);
}
MAT *m_inverse(const MAT *A, MAT *out)
{
unsigned int i;
char MatrixTempBuffer[ 4000 ];
VEC *tmp = VNULL, *tmp2 = VNULL;
MAT *A_cp = MNULL;
PERM *pivot = PNULL;
if ( ! A )
error(E_NULL,"m_inverse");
if ( A->m != A->n )
error(E_SQUARE,"m_inverse");
if ( ! out || out->m < A->m || out->n < A->n )
out = m_resize(out,A->m,A->n);
if( SET_MAT_SIZE( A->m, A->n ) < 1000 )
mat_get( &A_cp, (void *)( MatrixTempBuffer + 2000 ), A->m, A->n );
else
A_cp = matrix_get( A->m, A->n );
A_cp = m_copy( A, A_cp );
if( SET_VEC_SIZE( A->m ) < 1000 ) {
vec_get( &tmp, (void *)MatrixTempBuffer, A->m );
vec_get( &tmp2, (void *)(MatrixTempBuffer + 1000), A->m );
} else {
tmp = v_get( A->m );
tmp2 = v_get( A->m );
}
if( SET_PERM_SIZE( A->m ) < 1000 ) {
perm_get( &pivot, (void *)( MatrixTempBuffer + 3000 ), A->m );
} else {
pivot = px_get( A->m );
}
LUfactor(A_cp,pivot);
//tracecatch_matrix(LUfactor(A_cp,pivot),"m_inverse");
for ( i = 0; i < A->n; i++ ){
v_zero(tmp);
tmp->ve[i] = 1.0;
LUsolve(A_cp,pivot,tmp,tmp2);
//tracecatch_matrix(LUsolve(A_cp,pivot,tmp,tmp2),"m_inverse");
set_col(out,i,tmp2);
}
if( tmp != (VEC *)(MatrixTempBuffer ) ) // память выделялась, надо освободить
V_FREE(tmp);
if( tmp2 != (VEC *)(MatrixTempBuffer + 1000) ) // память выделялась, надо освободить
V_FREE(tmp2);
if( A_cp != (MAT *)(MatrixTempBuffer + 2000) ) // память выделялась, надо освободить
M_FREE(A_cp);
if( pivot != (PERM *)(MatrixTempBuffer + 3000) ) // память выделялась, надо освободить
PX_FREE( pivot );
return out;
}
void solveAndPrint(int N, double **AMatrix, double *bMatrix, double *xMatrix){
int j = 0;
LUdecomp *LUdecomp = malloc(sizeof(LUdecomp));
LUdecomp = LUdecompose(2*N, AMatrix); // perform decomp
LUsolve(LUdecomp, bMatrix, xMatrix); // solve x
for(int i = 0; i < 2*N; i++){ // print to stdout
printf("%.10f ", xMatrix[i]); //print
j++;
if(j == 3){ // 3 values, move to next row
j = 0;
printf("\n");
}
}
printf("1.0000000000"); // print 1 at the end
}
Matrix
LUinverse(Matrix A, int *indexarray, Matrix out)
{
int i, j, dim = A->dim;
Vector tmp, tmp2;
if (!out)
out = new_matrix(dim);
tmp = new_vector(dim);
tmp2 = new_vector(dim);
for (i = 0; i < dim; i++) {
for (j = 0; j < dim; j++)
tmp->ve[j] = 0.;
tmp->ve[i] = 1.;
if (LUsolve(A, indexarray, tmp, tmp2) == -1)
return NULL;
for (j = 0; j < dim; j++)
M_VAL(out, j, i) = tmp2->ve[j];
}
return out;
}
MAT *m_inverse(const MAT *A, MAT *out)
#endif
{
int i;
STATIC VEC *tmp = VNULL, *tmp2 = VNULL;
STATIC MAT *A_cp = MNULL;
STATIC PERM *pivot = PNULL;
if ( ! A )
error(E_NULL,"m_inverse");
if ( A->m != A->n )
error(E_SQUARE,"m_inverse");
if ( ! out || out->m < A->m || out->n < A->n )
out = m_resize(out,A->m,A->n);
A_cp = m_resize(A_cp,A->m,A->n);
A_cp = m_copy(A,A_cp);
tmp = v_resize(tmp,A->m);
tmp2 = v_resize(tmp2,A->m);
pivot = px_resize(pivot,A->m);
MEM_STAT_REG(A_cp,TYPE_MAT);
MEM_STAT_REG(tmp, TYPE_VEC);
MEM_STAT_REG(tmp2,TYPE_VEC);
MEM_STAT_REG(pivot,TYPE_PERM);
tracecatch(LUfactor(A_cp,pivot),"m_inverse");
for ( i = 0; i < A->n; i++ )
{
v_zero(tmp);
tmp->ve[i] = 1.0;
tracecatch(LUsolve(A_cp,pivot,tmp,tmp2),"m_inverse");
set_col(out,i,tmp2);
}
#ifdef THREADSAFE
V_FREE(tmp); V_FREE(tmp2);
M_FREE(A_cp); PX_FREE(pivot);
#endif
return out;
}
double LUcondest(const MAT *LU, PERM *pivot)
#endif
{
STATIC VEC *y = VNULL, *z = VNULL;
Real cond_est, L_norm, U_norm, sum, tiny;
int i, j, n;
if ( ! LU || ! pivot )
error(E_NULL,"LUcondest");
if ( LU->m != LU->n )
error(E_SQUARE,"LUcondest");
if ( LU->n != pivot->size )
error(E_SIZES,"LUcondest");
tiny = 10.0/HUGE_VAL;
n = LU->n;
y = v_resize(y,n);
z = v_resize(z,n);
MEM_STAT_REG(y,TYPE_VEC);
MEM_STAT_REG(z,TYPE_VEC);
for ( i = 0; i < n; i++ )
{
sum = 0.0;
for ( j = 0; j < i; j++ )
sum -= LU->me[j][i]*y->ve[j];
sum -= (sum < 0.0) ? 1.0 : -1.0;
if ( fabs(LU->me[i][i]) <= tiny*fabs(sum) )
return HUGE_VAL;
y->ve[i] = sum / LU->me[i][i];
}
Catch(E_SING,
LTsolve(LU,y,y,1.0);
LUsolve(LU,pivot,y,z);
,
return HUGE_VAL);
void OrthoBuilderGSh::buildSolution( vector<VarVect>* _mesh )
{
vector<vector<PL_NUM>> M;
vector<PL_NUM> f11;
vector<PL_NUM> x1;
vector<PL_NUM> res;
vector<PL_NUM> res2;
vector<PL_NUM> dx1;
int msize = eq_num / 2; //caution here!
M.resize( msize, vector<PL_NUM>( msize, 0.0) );
f11.resize( msize, 0.0 );
x1.resize( msize, 0.0 );
res.resize( msize, 0.0 );
res2.resize( msize, 0.0 );
dx1.resize( msize, 0.0 );
//simply supported plate NO CURRENT PASSING THROUGH THE BOUNDARY
int totLines = eq_num / EQ_NUM;
int _a = EQ_NUM / 2;
for( int line = 0; line < totLines; ++line )
{
for( int vNum = 0; vNum < eq_num / 2; ++vNum )
{
M[line * _a + 0][vNum] = solInfoMap[Km - 1].zi[vNum][line * EQ_NUM + 0]; //TODO potential lags here!
M[line * _a + 1][vNum] = solInfoMap[Km - 1].zi[vNum][line * EQ_NUM + 1];
M[line * _a + 2][vNum] = solInfoMap[Km - 1].zi[vNum][line * EQ_NUM + 4];
M[line * _a + 3][vNum] = solInfoMap[Km - 1].zi[vNum][line * EQ_NUM + 6];
M[line * _a + 4][vNum] = solInfoMap[Km - 1].zi[vNum][line * EQ_NUM + 8];
}
/*f11[line * _a + 0] = -solInfoMap[Km - 1].z5[line * EQ_NUM + 0];
f11[line * _a + 1] = -solInfoMap[Km - 1].z5[line * EQ_NUM + 1];
f11[line * _a + 2] = -solInfoMap[Km - 1].z5[line * EQ_NUM + 4];
f11[line * _a + 3] = -solInfoMap[Km - 1].z5[line * EQ_NUM + 6];
f11[line * _a + 4] = -solInfoMap[Km - 1].z5[line * EQ_NUM + 8];*/
f11[line * _a + 0] = -z5[Km - 1][line * EQ_NUM + 0];
f11[line * _a + 1] = -z5[Km - 1][line * EQ_NUM + 1];
f11[line * _a + 2] = -z5[Km - 1][line * EQ_NUM + 4];
f11[line * _a + 3] = -z5[Km - 1][line * EQ_NUM + 6];
f11[line * _a + 4] = -z5[Km - 1][line * EQ_NUM + 8];
}
LUsolve( M, f11, &x1 );
//refinement. I do not know the theoretical source of this procedure yet. just rewrote it
//TODO test this
for( int i = 0; i < eq_num / 2; ++i )
{
res[i] = f11[i];
for( int j = 0; j < eq_num / 2; ++j )
{
res[i] -= M[i][j] * x1[j];
}
}
LUsolve( M, res, &dx1 );
for( int i = 0; i < eq_num / 2; ++i )
{
x1[i] += dx1[i];
}
PL_NUM ndx = 0.0;
PL_NUM ndx2 = 0.0;
PL_NUM temp;
for( int i = 0; i < eq_num / 2; ++i )
{
ndx += dx1[i] * dx1[i];
}
temp = ndx; //FIXME may be we don't need temp
while( fabs( ndx - ndx2 ) > 0.0 ) //FIXME may be > DELTA ??
{
ndx = temp; //may be just ndx = ndx2
for( int i = 0; i < eq_num / 2; ++i )
{
res2[i] = res[i]; //FIXME may be we do not need res2 here. use just res
for( int j = 0; j < eq_num / 2; ++j )
{
res2[i] -= M[i][j] * dx1[j];
}
res[i] = res2[i];
}
LUsolve( M, res2, &dx1 );
for( int i = 0; i < eq_num / 2; ++i )
{
x1[i] += dx1[i];
}
for( int i = 0; i < eq_num / 2; ++i )
{
res2[i] = res[i];
for( int j = 0; j < eq_num / 2; ++j )
{
res2[i] -= M[i][j] * dx1[j];
}
res[i] = res2[i];
}
LUsolve( M, res2, &dx1 );
for( int i = 0; i < eq_num / 2; ++i )
{
x1[i] += dx1[i];
}
ndx2 = 0.0;
for( int i = 0; i < eq_num / 2; ++i )
{
ndx2 += dx1[i] * dx1[i];
}
temp = ndx2;
}
//refinement is over
//now we determine coefficients for base solutions
//the right-hand side:
for( int i = 0; i < eq_num / 2; ++i )
{
solInfoMap[Km - 1].C[i] = x1[i];
}
//all the other points:
for( int _x = Km - 2; _x >= 0; --_x )
{
for( int i = eq_num / 2 - 1; i >= 0; --i )
{
solInfoMap[_x].C[i] = solInfoMap[_x + 1].C[i] - solInfoMap[_x + 1].o[eq_num / 2 * ( eq_num / 2 + 1 ) / 2 + i];
for( int j = eq_num / 2 - 1; j > i; --j )
{
solInfoMap[_x].C[i] -= solInfoMap[_x + 1].o[j * ( j + 1 ) / 2 + i] * solInfoMap[_x].C[j];
}
solInfoMap[_x].C[i] /= solInfoMap[_x + 1].o[i * ( i + 1 ) / 2 + i];
}
}
//now using the coefficients we write down the solution
for( int _x = 0; _x < Km; ++_x )
{
for( int i = 0; i < eq_num; ++i )
{
(*_mesh)[_x].Nk1[i] = 0.0;
for( int vNum = 0; vNum < eq_num / 2; ++vNum )
{
(*_mesh)[_x].Nk1[i] += solInfoMap[_x].C[vNum] * solInfoMap[_x].zi[vNum][i]; //FIXME lags may happen here
}
/*(*_mesh)[_x].Nk1[i] += solInfoMap[_x].z5[i];*/
(*_mesh)[_x].Nk1[i] += z5[_x][i];
}
}
//force the BCs to be zero at y == a/2
//TODO why do we need this??
for( int line = 0; line < eq_num / EQ_NUM; ++line )
{
(*_mesh)[Km - 1].Nk1[line * EQ_NUM + 0] = 0.0;
(*_mesh)[Km - 1].Nk1[line * EQ_NUM + 1] = 0.0;
(*_mesh)[Km - 1].Nk1[line * EQ_NUM + 4] = 0.0;
(*_mesh)[Km - 1].Nk1[line * EQ_NUM + 6] = 0.0;
(*_mesh)[Km - 1].Nk1[line * EQ_NUM + 8] = 0.0;
}
}
//FIXME: add LOCALFUNC? (OR GLOBAL?)
BYTE* RECURSIVE_SUBROUTINE(BYTE t, list *cur_guess, list *guesses, list_node *first_guess)
{
if ( t == key_length)
{
unsigned char* key=NULL;
// generate key from the cur_guess (push guesses into a matrix and use some matrix solving library?)
MAT* A;
VEC *x,*b;
PERM* pivot;
A=m_get(key_length,key_length);
b=v_get(key_length);
x=v_get(key_length);
int c=0,r=0;
for(list_node* iter=list_head(cur_guess); iter != NULL && r<key_length; iter=list_node_next(iter)){
for(c=list_node_data_ptr(iter,byte_sum_guess_t)->i1;
c <= list_node_data_ptr(iter,byte_sum_guess_t)->i2;
++c){
A->me[r][c]=1;
}
b->ve[r]=list_node_data_ptr(iter,byte_sum_guess_t)->value;
++r;
}
//Calculate matrix determinant
SQRMATRIX A_det;
SQRMATRIX_CreateMatrix(&A_det,key_length);
for(r=0;r<key_length;++r){
for(c=0;c<key_length;++c){
A_det.array[r][c]=A->me[r][c];
}
}
int det;
det=SQRMATRIX_CalcDeterminant(&A_det);
//TODO: return this later
SQRMATRIX_DestroyMatrix(&A_det);
if(det==0){//If determinant is 0 continue to next guess
++count_bad_matrix;
#ifdef __DEBUG
//SQRMATRIX_DisplayMatrix(&A_det);
v_output(b);
#endif
DEBUG_PRINT("Matrix determinant is 0\n");
}else{
++count_guesses;
pivot = px_get(A->m);
LUfactor(A,pivot);
x=LUsolve(A,pivot,b,VNULL);
PX_FREE(pivot);
//test key (use our RC4 impl)
key=(unsigned char*)malloc(sizeof(unsigned char)*key_length);
for(int i=0;i<key_length;++i){
key[i]=x->ve[i];
}
int res=rc4_test_key(key);
if(res){
printf("MAZAL TOV! we got the right key.\n");
print_key(key);
printf("\n");
}else{
printf("Tried key: ");
print_key(key);
printf("\n");
free(key);key=NULL;
}
}
//release matrix vars
M_FREE(A);
V_FREE(x);
V_FREE(b);
return key;
}
byte_sum_guess_t cur;
//list *new_list_head=guesses;
//TODO: (later) add a for loop running along the "lambeda_t" values here, for the initial impl we'll try the best guess
//for ()
//{
for(int i=0; i<LAMBDA_T; ++i){
cur = *(list_node_data_ptr(first_guess, byte_sum_guess_t));
list_node *biatch = list_add_head(cur_guess, &cur);
BYTE* res=RECURSIVE_SUBROUTINE(t+1, cur_guess, guesses, list_node_next(first_guess));
if(res!=NULL){
return res;
}
list_del(cur_guess,biatch);
first_guess = list_node_next(first_guess);
}
return NULL;
//TODO: do something to find the next guess and link it to the current guess
//when I say something I mean find best guess (i.e best weight) of all the guesses that give us new information
//(i.e not linearily dependent in our byte values and sums matrix that can be deduced from the cur_guess)
//see also the note above (intuition)
//IMPORTANT!
//explaination how cur_guess is a matrix: each entry in cur_guess contains a list of bytes that are part of the sum and a
//guess as to the value of the sum. if this matrix is solvable then solving it should give us a value for each byte of the
//key thus the entire key
//note: we probably should change cur_guess to a smarter database (for example a (L)x(L+1) matrix as an array?) but we
//need to consider that we need to keep the ability to backtrack without making it too expensive
//TODO: if weight of the guess is too small -> return FAIL (section 4.6)
//These are based on section 4.4
//correct suggestions (this can be done later, basic alg should work without it)
//adjust weights (this can be done later, basic alg should work without it)
//merge counters (section 4.2), also skip for initial impl? need to check
//go to next iteration in the recurtion
//RECURSIVE_SUBROUTINE(t+1, cur_guess, );
//} end of for
}
/* MAIN PROGRAM */
int main() { /*------------------------------------------------------------------------*/
/* variables */
const int tag = 1; // tape tag
const int size = 5; // system size
const int indep = size*size+size; // # of indeps
const int depen = size; // # of deps
double A[size][size], a1[size], a2[size], // passive variables
b[size], x[size];
adouble **AA, *AAp, *Abx; // active variables
double *args = myalloc1(indep); // arguments
double **jac = myalloc2(depen,indep); // the Jacobian
double *laghessvec = myalloc1(indep); // Hessian-vector product
int i,j;
/*------------------------------------------------------------------------*/
/* Info */
fprintf(stdout,"LINEAR SYSTEM SOLVING by "
"LU-DECOMPOSITION (ADOL-C Example)\n\n");
/*------------------------------------------------------------------------*/
/* Allocation und initialization of the system matrix */
AA = new adouble*[size];
AAp = new adouble[size*size];
for (i=0; i<size; i++) {
AA[i] = AAp;
AAp += size;
}
Abx = new adouble[size];
for(i=0; i<size; i++) {
a1[i] = i*0.25;
a2[i] = i*0.33;
}
for(i=0; i<size; i++) {
for(j=0; j<size; j++)
A[i][j] = a1[i]*a2[j];
A[i][i] += i+1;
b[i] = -i-1;
}
/*------------------------------------------------------------------------*/
/* Taping the computation of the determinant */
trace_on(tag);
/* marking indeps */
for(i=0; i<size; i++)
for(j=0; j<size; j++)
AA[i][j] <<= (args[i*size+j] = A[i][j]);
for(i=0; i<size; i++)
Abx[i] <<= (args[size*size+i] = b[i]);
/* LU-factorization and computation of solution */
LUfact(size,AA);
LUsolve(size,AA,Abx);
/* marking deps */
for (i=0; i<size; i++)
Abx[i] >>= x[i];
trace_off();
fprintf(stdout," x[0] (original): %16.4le\n",x[0]);
/*------------------------------------------------------------------------*/
/* Recomputation */
function(tag,depen,indep,args,x);
fprintf(stdout," x[0] (from tape): %16.4le\n",x[0]);
/*------------------------------------------------------------------------*/
/* Computation of Jacobian */
jacobian(tag,depen,indep,args,jac);
fprintf(stdout," Jacobian:\n");
for (i=0; i<depen; i++) {
for (j=0; j<indep; j++)
fprintf(stdout," %14.6le",jac[i][j]);
fprintf(stdout,"\n");
}
/*------------------------------------------------------------------------*/
/* Computation of Lagrange-Hessian-vector product */
lagra_hess_vec(tag,depen,indep,args,args,x,laghessvec);
fprintf(stdout," Part of Lagrange-Hessian-vector product:\n");
for (i=0; i<size; i++) {
for (j=0; j<size; j++)
fprintf(stdout," %14.6le",laghessvec[i*size+j]);
fprintf(stdout,"\n");
}
/*------------------------------------------------------------------------*/
/* Tape-documentation */
tape_doc(tag,depen,indep,args,x);
/*------------------------------------------------------------------------*/
/* Tape statistics */
int tape_stats[STAT_SIZE];
tapestats(tag,tape_stats);
fprintf(stdout,"\n independents %d\n",tape_stats[NUM_INDEPENDENTS]);
fprintf(stdout," dependents %d\n",tape_stats[NUM_DEPENDENTS]);
fprintf(stdout," operations %d\n",tape_stats[NUM_OPERATIONS]);
fprintf(stdout," operations buffer size %d\n",tape_stats[OP_BUFFER_SIZE]);
fprintf(stdout," locations buffer size %d\n",tape_stats[LOC_BUFFER_SIZE]);
fprintf(stdout," constants buffer size %d\n",tape_stats[VAL_BUFFER_SIZE]);
fprintf(stdout," maxlive %d\n",tape_stats[NUM_MAX_LIVES]);
fprintf(stdout," valstack size %d\n\n",tape_stats[TAY_STACK_SIZE]);
/*------------------------------------------------------------------------*/
/* That's it */
return 1;
}
double multivariateregression(uint nvariables, uint nsamples, dmatrix x, dvector w, dvector y, dvector Fy){
int d=0;
double xtwj;
dmatrix Xt = newdmatrix(nvariables,nsamples);
dmatrix XtWX = newdmatrix(nvariables, nvariables);
dvector XtWY = newdvector(nvariables);
ivector indx = newivector(nvariables);
//cout << "calculating Xt" << endl;
for(uint i=0; i<nsamples; i++){
for(uint j=0; j<nvariables; j++){
Xt[j][i] = x[i][j];
}
}
//cout << "calculating XtWX and XtWY" << endl;
for(uint i=0; i<nsamples; i++){
for(uint j=0; j<nvariables; j++){
xtwj = Xt[j][i] * w[i];
XtWY[j] += xtwj * y[i];
for(uint jj=0; jj<=j; jj++){
XtWX[j][jj] += xtwj * Xt[jj][i];
}
}
}
LUdecomposition(XtWX, nvariables, indx, &d);
LUsolve(XtWX, nvariables, indx, XtWY);
//cout << "Estimated parameters:" << endl;
//for (uint i=0; i < nvariables; i++){
// cout << "Parameter " << i << " = " << XtWY[i] << endl;
//}
dvector fit = newdvector(nsamples);
dvector residual = newdvector(nsamples);
dvector indL = newdvector(nsamples);
double variance= 0.0;
double logL=0.0;
for (uint i=0; i<nsamples; i++){
fit[i]= 0.0;
for (uint j=0; j<nvariables; j++){
fit[i] += Xt[j][i] * XtWY[j];
residual[i] = y[i]-fit[i];
variance += w[i]*pow(residual[i],2.0);
}
Fy[i] = Lnormal(residual[i],variance);
indL[i] += w[i]*Fy[i];
logL += log(indL[i]);
}
//cout << "Estimated response:" << endl;
//printdvector(fit,nsamples);
//cout << "Residuals:" << endl;
//printdvector(residual,nsamples);
//cout << "Estimated Fy:" << endl;
//printdvector(Fy,nsamples);
//cout << "Variance: " << variance << endl;
//cout << "Loglikelihood: " << logL << endl;
freematrix((void**)Xt,nvariables);
freematrix((void**)XtWX, nvariables);
freevector((void*)XtWY);
freevector((void*)fit);
freevector((void*)residual);
freevector((void*)indL);
return logL;
}
int main(){
/*
Create N
Create matrix to hold the original set of x,y coordinates (xy_source)
Create matrix to hold the transformed set of x,y coordinates (xy_target)
*/
int N = 0;
//get N from first line of text file
scanf("%d", &N);
double xy_source[N][2];
double xy_target[N][2];
//get the x and y values from input file
for(int i = 0; i < N; i++){
scanf("%lf %lf", &xy_source[i][0], &xy_source[i][1]);
}
for(int i = 0; i < N; i++){
scanf("%lf %lf", &xy_target[i][0], &xy_target[i][1]);
}
//Create matrix to hold A
double** A = (double**)malloc(2*N*sizeof(double*));
//make room on the heap for A
for(int i = 0; i < 2*N; i++)
A[i] = (double*)malloc(8*sizeof(double));
/*
begin filling Matrix A
*/
int count = 0;
//column 1
for(int j = 0; j < 2*N; j ++)
if(j % 2 == 0)
A[j][0] = xy_source[count++][0];
else
A[j][0] = 0;
count = 0;
//column 2
for(int j = 0; j < 2*N; j++)
if(j % 2 == 0)
A[j][1] = xy_source[count++][1];
else
A[j][1] = 0;
count = 0;
//column 3
for(int j = 0; j < 2*N; j++)
if(j % 2 == 0)
A[j][2] = 1;
else
A[j][2] = 0;
count = 0;
//column 4
for(int j = 0; j < 2*N; j++)
if(j % 2 == 0)
A[j][3] = 0;
else
A[j][3] = xy_source[count++][0];
count = 0;
//column 5
for(int j = 0; j < 2*N; j++)
if(j % 2 ==0)
A[j][4] = 0;
else
A[j][4] = xy_source[count++][1];
count = 0;
//column 6
for(int j = 0; j < 2*N; j++)
if(j % 2 == 0)
A[j][5] = 0;
else
A[j][5] = 1;
count = 0;
//column 7
for(int j = 0; j < 2*N; j++){
if(j % 2 == 0){
A[j][6] = (-1)*(xy_source[count][0])*(xy_target[count][0]);
}else{
A[j][6] = (-1)*(xy_source[count][0])*(xy_target[count][1]);
count++;
}
}
count = 0;
//column 8
for(int j = 0; j < 2*N; j++){
if(j % 2 == 0){
A[j][7] = (-1)*(xy_source[count][1])*(xy_target[count][0]);
}else{
A[j][7] = (-1)*(xy_source[count][1])*(xy_target[count][1]);
count++;
}
}
//Create and make room on heap for matrix b
double* b = (double*)malloc(2*N*sizeof(double));
//populate matrix b from xy_target
count = 0;
for(int j = 0; j < 2*N; j += 2){
b[j] = xy_target[count][0];
b[j+1] = xy_target[count++][1];
}
//Create and make room on heap for matrix x
double* x = (double*)malloc(8*sizeof(double));
for(int j = 0; j < 8; j++)
x[j] = 0.0;
/*
Create matrix to hold A transpose A
*/
double** A_T_A = (double**)malloc(8*sizeof(double*));
for(int i = 0; i < 8; i++){
A_T_A[i] = (double*)malloc(8*sizeof(double));
}
//Create matrix to hold A transpose b
double* A_T_b = (double*)malloc(8*sizeof(double));
/*
If N > 4 we have an overdetermined system and must use the method of least-squares
*/
if(N > 4){
double sum = 0.0;
for(int i = 0; i < 8; i++){
for(int j = i; j < 8; j++){
sum = 0.0;
for(int k = 0; k < 2*N; k++){
sum += A[k][i]*A[k][j];
}
A_T_A[i][j] = sum;
}
}
for(int i = 0; i < 8; i++)
for(int j = 0; j < i; j++)
A_T_A[i][j] = A_T_A[j][i];
sum = 0.0;
for(int i = 0; i < 8; i++){
sum = 0.0;
for(int j = 0; j < 2*N; j++)
sum += A[j][i] * b[j];
A_T_b[i] = sum;
}
}
if(N == 4){
LUdecomp* A_decompose = LUdecompose(2*N, (const double**)A);
LUsolve(A_decompose, b, x);
}else{
LUdecomp* A_T_decompose = LUdecompose(8, (const double**)A_T_A);
LUsolve(A_T_decompose, A_T_b, x);
}
/*
Print out the homography in row major order
*/
for(int i = 0; i < 3; i++)
printf("%lf ", x[i]);
printf("\n");
for(int i = 3; i < 6; i++)
printf("%lf ", x[i]);
printf("\n");
for(int i = 6; i < 8; i++)
printf("%lf ", x[i]);
double one = 1.0;
printf("%lf ", one);
return 0;
}
void
test(const int n, const int lb, const int ub, bool dumpfull) {
// Generate a special-band-matrix mfull With lb lower diagonals, ub upper
// diagonals and the elements of the highest upper diagonal extended across
// each row
MAT *mfull = m_get(n, n);
//m_rand(mfull);
randlist(mfull->base, (mfull->n)*(mfull->n));
double **me = mfull->me;
for(int i = 0; i < n; i++) {
for(int j = 0; j < i - lb; j++)
me[i][j] = 0.0;
for(int j = i+ub+1; j < n; j++)
me[i][j] = me[i][i+ub];
}
// Copy matrix mfull to a compactly stored version
// mcmpct
// First lb columns padding for later use
// Next lb columns for lower diagonals
// Next column for diagonal
// Next ub columns for upper diagonals
// as the highest upper diagonal of the same row
const int mm = 2*lb+ub+1;
double mcmpct[n][mm];
zero(&mcmpct[0][0], n*mm);
for(int i = 0; i < n; i++)
for(int j = MAX(i-lb, 0); j < MIN(i+ub+1, n); j++)
mcmpct[i][j-i+lb+lb] = me[i][j];
// Replace unused values with NAN to be sure they aren't used
for(int k = 0; k < n; k++)
for(int i = 0; i < lb; i++)
mcmpct[k][i] = NAN;
for(int k = 0; k < lb; k++)
for(int i = 0; i < lb-k; i++)
mcmpct[k][i+lb] = NAN;
for(int k=n-1; k >= n-ub; k--)
for(int i = n-1-k+1+lb; i < mm; i++)
mcmpct[k][i+lb] = NAN;
// Generate start vector x1 for test
VEC *x1 = v_get(n);
randlist(x1->ve, n);
// Calculate mfull*x1 = dfull
VEC *dfull = v_get(n);
mv_mlt(mfull, x1, dfull);
// Calculate mcmpct*x1 = dcmpct
double dcmpct[n];
bdspecLUmlt(&mcmpct[0][0], n, lb, ub, x1->ve, dcmpct);
if(dumpfull) {
printf("Vector x (random values)\n");
printf("========================\n");
v_out(x1->ve, n);
printf("Matrix A (random values)\n");
printf("========================\n");
printf("Full NxN Meschach Matrix\n");
m_out(mfull->base, n, n);
printf("Compact bdspec Array\n");
m_out(&mcmpct[0][0], n, mm);
printf("Vector d = A*x\n");
printf("==============\n");
printf("Calculated from Full Meschach Matrix:\n");
v_out(dfull->ve, n);
printf("Calculated from Compact bdspec Array:\n");
v_out(dcmpct, n);
printf("L2 norm of difference between Meschach and bdspec calculations of d\n");
}
double ddiff = v_diff(dfull->ve, dcmpct, n);
printf("d diff=%6.0E ", ddiff);
if(ddiff*ddiff > DBL_EPSILON*v_normsq(dfull->ve, n))
printf("FAIL,");
else
printf("PASS,");
PERM *p = px_get(n);
LUfactor(mfull, p);
int indx[n];
bdspecLUfactormeschscale(&mcmpct[0][0], n, lb, ub, indx);
VEC *yfull = v_get(n);
catchall(LUsolve(mfull, p, dfull, yfull), printf("--matrix singular--:\n"));
double ycmpct[n];
for(int i = 0; i < n; i++)
ycmpct[i] = dcmpct[i];
bdspecLUsolve(&mcmpct[0][0], n, lb, ub, indx, ycmpct);
if(dumpfull) {
printf("\n\n");
printf("LU Factorization\n");
printf("================\n");
printf("Meschach LU Array\n");
m_out(mfull->base, n, n);
printf("Compact bdspec LU Array\n");
m_out(&mcmpct[0][0], n, mm);
printf("Permutation\n");
printf("===========\n");
printf("Meschach permutation vector\n");
p_out((int *) p->pe, n);
printf("bdspec indx vector\n");
p_out(indx, n);
printf("A*y = d Solved for y\n");
printf("====================\n");
printf("Meschach result\n");
v_out(yfull->ve, n);
printf("bdspec result\n");
v_out(ycmpct, n);
printf("L2 norm of difference between Meschach and bdspec calculations of y:\n");
}
double ydiff = v_diff(yfull->ve, ycmpct, n);
printf("y diff=%6.0E ", ydiff);
if(ydiff*ydiff > DBL_EPSILON*v_normsq(yfull->ve, n))
printf("FAIL,");
else
printf("PASS,");
if(dumpfull) {
printf("\n\n");
printf("L2 norm of error = y-x\n");
printf("======================\n");
}
double x1normsq = v_normsq(x1->ve, n);
double mescherr = v_diff(yfull->ve, x1->ve, n);
printf("mesch err=%6.0E ", mescherr);
if(mescherr*mescherr > DBL_EPSILON*x1normsq)
printf("FAIL,");
else
printf("PASS,");
double bdspecerr = v_diff(ycmpct, x1->ve, n);
printf("bdspec err=%6.0E ", bdspecerr);
if(bdspecerr*bdspecerr > DBL_EPSILON*x1normsq)
printf("FAIL ");
else
printf("PASS ");
if(dumpfull) {
printf("\n\n");
}
fflush(stdout);
}
