void solve(matrix_double A, double *B)
{
int d, changes[A.nrows];
printf("before ludcmp:\n");
printf("A:\n");
print_matrix_double(A);
printf("B:\n");
print_array_double(A.nrows, B);
if (! ludcmp(A,changes, &d)) {
lusolve(A, B, changes);
printf("after ludcmp:\n");
printf("A:\n");
print_matrix_double(A);
printf("B:\n");
print_array_double(A.nrows, B);
}
}
/* power and temp should both be alloced using hotspot_vector.
* 'b' is the 'thermal conductance' matrix. i.e, b * temp = power
* => temp = invb * power. instead of computing invb, we have
* stored the LUP decomposition of B in 'lu' and 'p'. Using
* forward and backward substitution, we can then solve the
* equation b * temp = power.
*/
void steady_state_temp_block(block_model_t *model, double *power, double *temp)
{
int i;
//printf("I am right!");
if (!model->r_ready)
fatal("R model not ready\n");
/* set power numbers for the virtual nodes */
set_internal_power_block(model, power);
/*
* find temperatures (spd flag is set to 1 by the same argument
* as mentioned in the populate_R_model_block function)
*/
//for(i=0;i<30;i++)
// printf("%.4f ",power[i]);
lusolve(model->lu, model->n_nodes, model->p, power, temp, 1);
//for(i=0;i<30;i++)
// printf("%.4f ",temp[i]);
}
/* INV = M^-1, INV, M are n by n matrices */
void matinv(double **INV, double **M, int n)
{
int *p, i, j;
double *col, *x;
p = ivector(n);
col = vector(n);
x = vector(n);
lupdcmp(M, n, p);
for (j = 0; j < n; j++) {
for (i = 0; i < n; i++) col[i]=0.0;
col[j] = 1.0;
lusolve(M, n, p, col, x);
for (i = 0; i < n; i++) INV[i][j]=x[i];
}
free_ivector(p);
free_vector(col);
free_vector(x);
}
/*************************
* determine inverse of a *
* binary matrix *
*************************/
void inversematrix(int *matrix, int *inversematrix, int dimension)
{
int row, col, k;
int *rhs, *roworder;
/* allocate memory */
rhs = (int *)allocate(dimension * sizeof(int));
roworder = (int *)allocate(dimension * sizeof(int));
/* determine lu decomposition of matrix */
for (row = 0; row < dimension; row++)
roworder[row] = row;
ludecomposition(matrix, roworder, dimension);
for (col = 0; col < dimension; col++)
{
/* clear field */
for (k = 0; k < dimension; k++)
rhs[k] = 0;
rhs[col] = 1;
lusolve(matrix, rhs, roworder, dimension);
for (k = 0; k < dimension; k++)
{
*(inversematrix + k * dimension + col) = rhs[k];
}
}
/* free space */
free(rhs);
free(roworder);
}
int main(int argc, char** argv)
{
int i, j, N, flag;
Matrix A=NULL, Q=NULL;
Vector b, grid, e, lambda=NULL;
double time, sum, h, tol=1e-4;
if (argc < 3) {
printf("need two parameters, N and flag [and tolerance]\n");
printf(" - N is the problem size (in each direction\n");
printf(" - flag = 1 -> Dense LU\n");
printf(" - flag = 2 -> Dense Cholesky\n");
printf(" - flag = 3 -> Full Gauss-Jacobi iterations\n");
printf(" - flag = 4 -> Full Gauss-Jacobi iterations using BLAS\n");
printf(" - flag = 5 -> Full Gauss-Seidel iterations\n");
printf(" - flag = 6 -> Full Gauss-Seidel iterations using BLAS\n");
printf(" - flag = 7 -> Full CG iterations\n");
printf(" - flag = 8 -> Matrix-less Gauss-Jacobi iterations\n");
printf(" - flag = 9 -> Matrix-less Gauss-Seidel iterations\n");
printf(" - flag = 10 -> Matrix-less Red-Black Gauss-Seidel iterations\n");
printf(" - flag = 11 -> Diagonalization\n");
printf(" - flag = 12 -> Diagonalization - FST\n");
printf(" - flag = 13 -> Matrix-less CG iterations\n");
return 1;
}
N=atoi(argv[1]);
flag=atoi(argv[2]);
if (argc > 3)
tol = atof(argv[3]);
if (N < 0) {
printf("invalid problem size given\n");
return 2;
}
if (flag < 0 || flag > 13) {
printf("invalid flag given\n");
return 3;
}
if (flag == 10 && (N-1)%2 != 0) {
printf("need an even size for red-black iterations\n");
return 4;
}
if (flag == 12 && (N & (N-1)) != 0) {
printf("need a power-of-two for fst-based diagonalization\n");
return 5;
}
h = 1.0/N;
grid = equidistantMesh(0.0, 1.0, N);
b = createVector(N-1);
e = createVector(N-1);
evalMeshInternal(b, grid, source);
evalMeshInternal(e, grid, exact);
scaleVector(b, pow(h, 2));
axpy(b, e, alpha);
if (flag < 8) {
A = createMatrix(N-1,N-1);
diag(A, -1, -1.0);
diag(A, 0, 2.0+alpha);
diag(A, 1, -1.0);
}
if (flag >= 11 && flag < 13)
lambda = generateEigenValuesP1D(N-1);
if (flag == 11)
Q = generateEigenMatrixP1D(N-1);
time = WallTime();
if (flag == 1) {
int* ipiv=NULL;
lusolve(A, b, &ipiv);
free(ipiv);
} else if (flag == 2)
llsolve(A,b,0);
else if (flag == 3)
printf("Gauss-Jacobi used %i iterations\n",
GaussJacobi(A, b, tol, 10000000));
else if (flag == 4)
printf("Gauss-Jacobi used %i iterations\n",
GaussJacobiBlas(A, b, tol, 10000000));
else if (flag == 5)
printf("Gauss-Seidel used %i iterations\n",
GaussSeidel(A, b, tol, 10000000));
else if (flag == 6)
printf("Gauss-Seidel used %i iterations\n",
GaussSeidelBlas(A, b, tol, 10000000));
else if (flag == 7)
printf("CG used %i iterations\n", cg(A, b, 1e-8));
else if (flag == 8)
printf("Gauss-Jacobi used %i iterations\n",
GaussJacobiPoisson1D(b, tol, 10000000));
else if (flag == 9)
printf("Gauss-Jacobi used %i iterations\n",
GaussSeidelPoisson1D(b, tol, 10000000));
else if (flag == 10)
printf("Gauss-Jacobi used %i iterations\n",
GaussSeidelPoisson1Drb(b, tol, 10000000));
else if (flag == 11)
DiagonalizationPoisson1D(b,lambda,Q);
else if (flag == 12)
DiagonalizationPoisson1Dfst(b,lambda);
else if (flag == 13)
printf("CG used %i iterations\n", cgMatrixFree(Poisson1D, b, tol));
printf("elapsed: %f\n", WallTime()-time);
evalMeshInternal(e, grid, exact);
axpy(b,e,-1.0);
printf("max error: %e\n", maxNorm(b));
if (A)
freeMatrix(A);
if (Q)
freeMatrix(Q);
freeVector(grid);
freeVector(b);
freeVector(e);
if (lambda)
freeVector(lambda);
return 0;
}
/* Apply the method developed by Matthew Brown (see BMVC 02 paper) to
fit a 3D quadratic function through the DOG function values around
the location (s,r,c), i.e., (scale,row,col), at which a peak has
been detected. Return the interpolated peak position as a vector
in "offset", which gives offset from position (s,r,c). The
returned value is the interpolated DOG magnitude at this peak.
*/
float FitQuadratic(float offset[3], flimage* dogs, int s, int r, int c)
{
float g[3];
flimage *dog0, *dog1, *dog2;
int i;
//s = 1; r = 128; c = 128;
float ** H = allocate_float_matrix(3, 3);
/* Select the dog images at peak scale, dog1, as well as the scale
below, dog0, and scale above, dog2. */
dog0 = &dogs[s-1];
dog1 = &dogs[s];
dog2 = &dogs[s+1];
/* Fill in the values of the gradient from pixel differences. */
g[0] = ((*dog2)(c,r) - (*dog0)(c,r)) / 2.0;
g[1] = ((*dog1)(c,r+1) - (*dog1)(c,r-1)) / 2.0;
g[2] = ((*dog1)(c+1,r) - (*dog1)(c-1,r)) / 2.0;
/* Fill in the values of the Hessian from pixel differences. */
H[0][0] = (*dog0)(c,r) - 2.0 * (*dog1)(c,r) + (*dog2)(c,r);
H[1][1] = (*dog1)(c,r-1) - 2.0 * (*dog1)(c,r) + (*dog1)(c,r+1);
H[2][2] = (*dog1)(c-1,r) - 2.0 * (*dog1)(c,r) + (*dog1)(c+1,r);
H[0][1] = H[1][0] = ( ((*dog2)(c,r+1) - (*dog2)(c,r-1)) -
((*dog0)(c,r+1) - (*dog0)(c,r-1)) ) / 4.0;
H[0][2] = H[2][0] = ( ((*dog2)(c+1,r) - (*dog2)(c-1,r)) -
((*dog0)(c+1,r) - (*dog0)(c-1,r)) ) / 4.0;
H[1][2] = H[2][1] = ( ((*dog1)(c+1,r+1) - (*dog1)(c-1,r+1)) -
((*dog1)(c+1,r-1) - (*dog1)(c-1,r-1)) ) / 4.0;
/* Solve the 3x3 linear sytem, Hx = -g. Result, x, gives peak offset.
Note that SolveLinearSystem destroys contents of H. */
offset[0] = - g[0];
offset[1] = - g[1];
offset[2] = - g[2];
// for(i=0; i < 3; i++){
//
// for(j=0; j < 3; j++) printf("%f ", H[i][j]);
// printf("\n");
// }
// printf("\n");
//
// for(i=0; i < 3; i++) printf("%f ", offset[i]);
// printf("\n");
float solution[3];
lusolve(H, solution, offset,3);
// printf("\n");
// for(i=0; i < 3; i++) printf("%f ", solution[i]);
// printf("\n");
desallocate_float_matrix(H,3,3);
delete[] H; /*memcheck*/
/* Also return value of DOG at peak location using initial value plus
0.5 times linear interpolation with gradient to peak position
(this is correct for a quadratic approximation).
*/
for(i=0; i < 3; i++) offset[i] = solution[i];
return ((*dog1)(c,r) + 0.5 * (solution[0]*g[0]+solution[1]*g[1]+solution[2]*g[2]));
}
double regression(int Nind, int Nmark, cvector cofactor, MQMMarkerMatrix marker, vector y,
vector *weight, ivector ind, int Naug, double *variance,
vector Fy, bool biasadj, bool fitQTL, bool dominance, bool verbose) {
debug_trace("regression IN\n");
/*
cofactor[j] at locus j:
MNOCOF: no cofactor at locus j
MCOF: cofactor at locus j
MSEX: QTL at locus j, but QTL effect is not included in the model
MQTL: QTL at locu j and QTL effect is included in the model
*/
//Calculate the dimensions of the designMatrix
int dimx=designmatrixdimensions(cofactor,Nmark,dominance);
int j, jj;
const int dimx_alloc = dimx+2;
//Allocate structures
matrix XtWX = newmatrix(dimx_alloc, dimx_alloc);
cmatrix Xt = newcmatrix(dimx_alloc, Naug);
vector XtWY = newvector(dimx_alloc);
//Reset dimension designmatrix
dimx = 1;
for (j=0; j<Nmark; j++){
if ((cofactor[j]==MCOF)||(cofactor[j]==MQTL)) dimx+= (dominance ? 2 : 1);
}
cvector xtQTL = newcvector(dimx);
int jx=0;
for (int i=0; i<Naug; i++) Xt[jx][i]= MH;
xtQTL[jx]= MNOCOF;
for (j=0; j<Nmark; j++)
if (cofactor[j]==MCOF) { // cofactor (not a QTL moving along the chromosome)
jx++;
xtQTL[jx]= MCOF;
if (dominance) {
for (int i=0; i<Naug; i++)
if (marker[j][i]==MH) {
Xt[jx][i]=48; //ASCII code 47, 48 en 49 voor -1, 0, 1;
Xt[jx+1][i]=49;
} else if (marker[j][i]==MAA) {
Xt[jx][i]=47; // '/' stands for -1
Xt[jx+1][i]=48;
} else {
Xt[jx][i]=49;
Xt[jx+1][i]=48;
}
jx++;
xtQTL[jx]= MCOF;
} else {
for (int i=0; i<Naug; i++) {
if (marker[j][i]==MH) {
Xt[jx][i]=48; //ASCII code 47, 48 en 49 voor -1, 0, 1;
} else if (marker[j][i]==MAA) {
Xt[jx][i]=47; // '/' stands for -1
} else {
Xt[jx][i]=49;
}
}
}
} else if (cofactor[j]==MQTL) { // QTL
jx++;
xtQTL[jx]= MSEX;
if (dominance) {
jx++;
xtQTL[jx]= MQTL;
}
}
//Rprintf("calculate xtwx and xtwy\n");
/* calculate xtwx and xtwy */
double xtwj, yi, wi, calc_i;
for (j=0; j<dimx; j++) {
XtWY[j]= 0.0;
for (jj=0; jj<dimx; jj++) XtWX[j][jj]= 0.0;
}
if (!fitQTL){
for (int i=0; i<Naug; i++) {
yi= y[i];
wi= (*weight)[i];
//in the original version when we enable Dominance , we crash around here
for (j=0; j<dimx; j++) {
xtwj= ((double)Xt[j][i]-48.0)*wi;
XtWY[j]+= xtwj*yi;
for (jj=0; jj<=j; jj++) XtWX[j][jj]+= xtwj*((double)Xt[jj][i]-48.0);
}
}
}else{ // QTL is moving along the chromosomes
for (int i=0; i<Naug; i++) {
wi= (*weight)[i]+ (*weight)[i+Naug]+ (*weight)[i+2*Naug];
yi= y[i];
//Changed <= to < to prevent chrashes, this could make calculations a tad different then before
for (j=0; j<dimx; j++){
if (xtQTL[j]<=MCOF) {
xtwj= ((double)Xt[j][i]-48.0)*wi;
XtWY[j]+= xtwj*yi;
for (jj=0; jj<=j; jj++)
if (xtQTL[jj]<=MCOF) XtWX[j][jj]+= xtwj*((double)Xt[jj][i]-48.0);
else if (xtQTL[jj]==MSEX) // QTL: additive effect if QTL=MCOF or MSEX
{ // QTL==MAA
XtWX[j][jj]+= ((double)(Xt[j][i]-48.0))*(*weight)[i]*(47.0-48.0);
// QTL==MBB
XtWX[j][jj]+= ((double)(Xt[j][i]-48.0))*(*weight)[i+2*Naug]*(49.0-48.0);
} else // (xtQTL[jj]==MNOTAA) QTL: dominance effect only if QTL=MCOF
{ // QTL==MH
XtWX[j][jj]+= ((double)(Xt[j][i]-48.0))*(*weight)[i+Naug]*(49.0-48.0);
}
} else if (xtQTL[j]==MSEX) { // QTL: additive effect if QTL=MCOF or MSEX
xtwj= -1.0*(*weight)[i]; // QTL==MAA
XtWY[j]+= xtwj*yi;
for (jj=0; jj<j; jj++) XtWX[j][jj]+= xtwj*((double)Xt[jj][i]-48.0);
XtWX[j][j]+= xtwj*-1.0;
xtwj= 1.0*(*weight)[i+2*Naug]; // QTL==MBB
XtWY[j]+= xtwj*yi;
for (jj=0; jj<j; jj++) XtWX[j][jj]+= xtwj*((double)Xt[jj][i]-48.0);
XtWX[j][j]+= xtwj*1.0;
} else { // (xtQTL[j]==MQTL) QTL: dominance effect only if QTL=MCOF
xtwj= 1.0*(*weight)[i+Naug]; // QTL==MCOF
XtWY[j]+= xtwj*yi;
// j-1 is for additive effect, which is orthogonal to dominance effect
for (jj=0; jj<j-1; jj++) XtWX[j][jj]+= xtwj*((double)Xt[jj][i]-48.0);
XtWX[j][j]+= xtwj*1.0;
}
}
}
}
for (j=0; j<dimx; j++){
for (jj=j+1; jj<dimx; jj++){
XtWX[j][jj]= XtWX[jj][j];
}
}
int d;
ivector indx= newivector(dimx);
/* solve equations */
ludcmp(XtWX, dimx, indx, &d);
lusolve(XtWX, dimx, indx, XtWY);
double* indL = (double *)R_alloc(Nind, sizeof(double));
int newNaug = ((!fitQTL) ? Naug : 3*Naug);
vector fit = newvector(newNaug);
vector resi = newvector(newNaug);
debug_trace("Calculate residuals\n");
if (*variance<0) {
*variance= 0.0;
if (!fitQTL)
for (int i=0; i<Naug; i++) {
fit[i]= 0.0;
for (j=0; j<dimx; j++)
fit[i]+=((double)Xt[j][i]-48.0)*XtWY[j];
resi[i]= y[i]-fit[i];
*variance += (*weight)[i]*pow(resi[i], 2.0);
}
else
for (int i=0; i<Naug; i++) {
fit[i]= 0.0;
fit[i+Naug]= 0.0;
fit[i+2*Naug]= 0.0;
for (j=0; j<dimx; j++)
if (xtQTL[j]<=MCOF) {
calc_i =((double)Xt[j][i]-48.0)*XtWY[j];
fit[i]+= calc_i;
fit[i+Naug]+= calc_i;
fit[i+2*Naug]+= calc_i;
} else if (xtQTL[j]==MSEX) {
fit[i]+=-1.0*XtWY[j];
fit[i+2*Naug]+=1.0*XtWY[j];
} else
fit[i+Naug]+=1.0*XtWY[j];
resi[i]= y[i]-fit[i];
resi[i+Naug]= y[i]-fit[i+Naug];
resi[i+2*Naug]= y[i]-fit[i+2*Naug];
*variance +=(*weight)[i]*pow(resi[i], 2.0);
*variance +=(*weight)[i+Naug]*pow(resi[i+Naug], 2.0);
*variance +=(*weight)[i+2*Naug]*pow(resi[i+2*Naug], 2.0);
}
*variance/= (!biasadj ? Nind : Nind-dimx); // to compare results with Johan; variance/=Nind;
if (!fitQTL)
for (int i=0; i<Naug; i++) Fy[i]= Lnormal(resi[i], *variance);
else
for (int i=0; i<Naug; i++) {
Fy[i] = Lnormal(resi[i], *variance);
Fy[i+Naug] = Lnormal(resi[i+Naug], *variance);
Fy[i+2*Naug]= Lnormal(resi[i+2*Naug], *variance);
}
} else {
if (!fitQTL)
for (int i=0; i<Naug; i++) {
fit[i]= 0.0;
for (j=0; j<dimx; j++)
fit[i]+=((double)Xt[j][i]-48.0)*XtWY[j];
resi[i]= y[i]-fit[i];
Fy[i] = Lnormal(resi[i], *variance); // ????
}
else
for (int i=0; i<Naug; i++) {
fit[i]= 0.0;
fit[i+Naug]= 0.0;
fit[i+2*Naug]= 0.0;
for (j=0; j<dimx; j++)
if (xtQTL[j]<=MCOF) {
calc_i =((double)Xt[j][i]-48.0)*XtWY[j];
fit[i]+= calc_i;
fit[i+Naug]+= calc_i;
fit[i+2*Naug]+= calc_i;
} else if (xtQTL[j]==MSEX) {
fit[i]+=-1.0*XtWY[j];
fit[i+2*Naug]+=1.0*XtWY[j];
} else
fit[i+Naug]+=1.0*XtWY[j];
resi[i]= y[i]-fit[i];
resi[i+Naug]= y[i]-fit[i+Naug];
resi[i+2*Naug]= y[i]-fit[i+2*Naug];
Fy[i] = Lnormal(resi[i], *variance);
Fy[i+Naug] = Lnormal(resi[i+Naug], *variance);
Fy[i+2*Naug]= Lnormal(resi[i+2*Naug], *variance);
}
}
/* calculation of logL */
debug_trace("calculate logL\n");
double logL=0.0;
for (int i=0; i<Nind; i++) {
indL[i]= 0.0;
}
if (!fitQTL) {
for (int i=0; i<Naug; i++) indL[ind[i]]+=(*weight)[i]*Fy[i];
} else {
for (int i=0; i<Naug; i++) {
indL[ind[i]]+=(*weight)[i]* Fy[i];
indL[ind[i]]+=(*weight)[i+Naug]* Fy[i+Naug];
indL[ind[i]]+=(*weight)[i+2*Naug]*Fy[i+2*Naug];
}
}
for (int i=0; i<Nind; i++) { //Sum up log likelihoods for each individual
logL+= log(indL[i]);
}
return (double)logL;
}
