diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/mvlmm.cpp | 2763 |
1 files changed, 1430 insertions, 1333 deletions
diff --git a/src/mvlmm.cpp b/src/mvlmm.cpp index 1c65dd1..5eb7e38 100644 --- a/src/mvlmm.cpp +++ b/src/mvlmm.cpp @@ -27,12 +27,12 @@ #include <stdlib.h> #include <bitset> #include <cstring> +#include <assert.h> #include "gsl/gsl_vector.h" #include "gsl/gsl_matrix.h" #include "gsl/gsl_linalg.h" #include "gsl/gsl_blas.h" - #include "gsl/gsl_cdf.h" #include "gsl/gsl_roots.h" #include "gsl/gsl_min.h" @@ -953,8 +953,9 @@ void CalcHiQi (const gsl_vector *eval, const gsl_matrix *X, mat_dd, 0.0, &Hi_k.matrix); } - // Calculate Qi, and multiply I\otimes UtVeh on both side - // and calculate logdet_Q, don't forget to substract c_size*logdet_Ve. + // Calculate Qi, and multiply I\o times UtVeh on both side and + // calculate logdet_Q, don't forget to substract + // c_size*logdet_Ve. logdet_Q=CalcQi (eval, D_l, X, Qi)-(double)c_size*logdet_Ve; for (size_t i=0; i<c_size; i++) { @@ -1262,59 +1263,83 @@ void Calc_xHiDHiDHiy (const gsl_vector *eval, const gsl_matrix *Hi, size_t n_size=eval->size, d_size=Hiy->size1; - double delta, d_Hiy_i, d_Hiy_j, d_Hi_i1i2, d_Hi_i1j2, d_Hi_j1i2, d_Hi_j1j2; + double delta, d_Hiy_i, d_Hiy_j, d_Hi_i1i2, d_Hi_i1j2; + double d_Hi_j1i2, d_Hi_j1j2; for (size_t k=0; k<n_size; k++) { - delta=gsl_vector_get (eval, k); - - gsl_vector_const_view xHi_col_i=gsl_matrix_const_column (xHi, k*d_size+i1); - gsl_vector_const_view xHi_col_j=gsl_matrix_const_column (xHi, k*d_size+j1); - - d_Hiy_i=gsl_matrix_get (Hiy, i2, k); - d_Hiy_j=gsl_matrix_get (Hiy, j2, k); - - d_Hi_i1i2=gsl_matrix_get (Hi, i1, k*d_size+i2); - d_Hi_i1j2=gsl_matrix_get (Hi, i1, k*d_size+j2); - d_Hi_j1i2=gsl_matrix_get (Hi, j1, k*d_size+i2); - d_Hi_j1j2=gsl_matrix_get (Hi, j1, k*d_size+j2); - - if (i1==j1) { - gsl_blas_daxpy (delta*delta*d_Hi_j1i2*d_Hiy_j, &xHi_col_i.vector, xHiDHiDHiy_gg); - gsl_blas_daxpy (d_Hi_j1i2*d_Hiy_j, &xHi_col_i.vector, xHiDHiDHiy_ee); - gsl_blas_daxpy (delta*d_Hi_j1i2*d_Hiy_j, &xHi_col_i.vector, xHiDHiDHiy_ge); - - if (i2!=j2) { - gsl_blas_daxpy (delta*delta*d_Hi_j1j2*d_Hiy_i, &xHi_col_i.vector, xHiDHiDHiy_gg); - gsl_blas_daxpy (d_Hi_j1j2*d_Hiy_i, &xHi_col_i.vector, xHiDHiDHiy_ee); - gsl_blas_daxpy (delta*d_Hi_j1j2*d_Hiy_i, &xHi_col_i.vector, xHiDHiDHiy_ge); - } - } else { - gsl_blas_daxpy (delta*delta*d_Hi_j1i2*d_Hiy_j, &xHi_col_i.vector, xHiDHiDHiy_gg); - gsl_blas_daxpy (d_Hi_j1i2*d_Hiy_j, &xHi_col_i.vector, xHiDHiDHiy_ee); - gsl_blas_daxpy (delta*d_Hi_j1i2*d_Hiy_j, &xHi_col_i.vector, xHiDHiDHiy_ge); - - gsl_blas_daxpy (delta*delta*d_Hi_i1i2*d_Hiy_j, &xHi_col_j.vector, xHiDHiDHiy_gg); - gsl_blas_daxpy (d_Hi_i1i2*d_Hiy_j, &xHi_col_j.vector, xHiDHiDHiy_ee); - gsl_blas_daxpy (delta*d_Hi_i1i2*d_Hiy_j, &xHi_col_j.vector, xHiDHiDHiy_ge); - - if (i2!=j2) { - gsl_blas_daxpy (delta*delta*d_Hi_j1j2*d_Hiy_i, &xHi_col_i.vector, xHiDHiDHiy_gg); - gsl_blas_daxpy (d_Hi_j1j2*d_Hiy_i, &xHi_col_i.vector, xHiDHiDHiy_ee); - gsl_blas_daxpy (delta*d_Hi_j1j2*d_Hiy_i, &xHi_col_i.vector, xHiDHiDHiy_ge); - - gsl_blas_daxpy (delta*delta*d_Hi_i1j2*d_Hiy_i, &xHi_col_j.vector, xHiDHiDHiy_gg); - gsl_blas_daxpy (d_Hi_i1j2*d_Hiy_i, &xHi_col_j.vector, xHiDHiDHiy_ee); - gsl_blas_daxpy (delta*d_Hi_i1j2*d_Hiy_i, &xHi_col_j.vector, xHiDHiDHiy_ge); - } - } + delta=gsl_vector_get (eval, k); + + gsl_vector_const_view xHi_col_i= + gsl_matrix_const_column (xHi, k*d_size+i1); + gsl_vector_const_view xHi_col_j= + gsl_matrix_const_column (xHi, k*d_size+j1); + + d_Hiy_i=gsl_matrix_get (Hiy, i2, k); + d_Hiy_j=gsl_matrix_get (Hiy, j2, k); + + d_Hi_i1i2=gsl_matrix_get (Hi, i1, k*d_size+i2); + d_Hi_i1j2=gsl_matrix_get (Hi, i1, k*d_size+j2); + d_Hi_j1i2=gsl_matrix_get (Hi, j1, k*d_size+i2); + d_Hi_j1j2=gsl_matrix_get (Hi, j1, k*d_size+j2); + + if (i1==j1) { + gsl_blas_daxpy (delta*delta*d_Hi_j1i2*d_Hiy_j, &xHi_col_i.vector, + xHiDHiDHiy_gg); + gsl_blas_daxpy (d_Hi_j1i2*d_Hiy_j, &xHi_col_i.vector, + xHiDHiDHiy_ee); + gsl_blas_daxpy (delta*d_Hi_j1i2*d_Hiy_j, &xHi_col_i.vector, + xHiDHiDHiy_ge); + + if (i2!=j2) { + gsl_blas_daxpy (delta*delta*d_Hi_j1j2*d_Hiy_i, + &xHi_col_i.vector, xHiDHiDHiy_gg); + gsl_blas_daxpy (d_Hi_j1j2*d_Hiy_i, &xHi_col_i.vector, + xHiDHiDHiy_ee); + gsl_blas_daxpy (delta*d_Hi_j1j2*d_Hiy_i, &xHi_col_i.vector, + xHiDHiDHiy_ge); + } + } else { + gsl_blas_daxpy (delta*delta*d_Hi_j1i2*d_Hiy_j, &xHi_col_i.vector, + xHiDHiDHiy_gg); + gsl_blas_daxpy (d_Hi_j1i2*d_Hiy_j, &xHi_col_i.vector, + xHiDHiDHiy_ee); + gsl_blas_daxpy (delta*d_Hi_j1i2*d_Hiy_j, &xHi_col_i.vector, + xHiDHiDHiy_ge); + + gsl_blas_daxpy (delta*delta*d_Hi_i1i2*d_Hiy_j, &xHi_col_j.vector, + xHiDHiDHiy_gg); + gsl_blas_daxpy (d_Hi_i1i2*d_Hiy_j, &xHi_col_j.vector, + xHiDHiDHiy_ee); + gsl_blas_daxpy (delta*d_Hi_i1i2*d_Hiy_j, &xHi_col_j.vector, + xHiDHiDHiy_ge); + + if (i2!=j2) { + gsl_blas_daxpy (delta*delta*d_Hi_j1j2*d_Hiy_i, + &xHi_col_i.vector, xHiDHiDHiy_gg); + gsl_blas_daxpy (d_Hi_j1j2*d_Hiy_i, &xHi_col_i.vector, + xHiDHiDHiy_ee); + gsl_blas_daxpy (delta*d_Hi_j1j2*d_Hiy_i, &xHi_col_i.vector, + xHiDHiDHiy_ge); + + gsl_blas_daxpy (delta*delta*d_Hi_i1j2*d_Hiy_i, + &xHi_col_j.vector, xHiDHiDHiy_gg); + gsl_blas_daxpy (d_Hi_i1j2*d_Hiy_i, &xHi_col_j.vector, + xHiDHiDHiy_ee); + gsl_blas_daxpy (delta*d_Hi_i1j2*d_Hiy_i, &xHi_col_j.vector, + xHiDHiDHiy_ge); + } + } } - + return; } -void Calc_xHiDHiDHix (const gsl_vector *eval, const gsl_matrix *Hi, const gsl_matrix *xHi, const size_t i1, const size_t j1, const size_t i2, const size_t j2, gsl_matrix *xHiDHiDHix_gg, gsl_matrix *xHiDHiDHix_ee, gsl_matrix *xHiDHiDHix_ge) -{ +void Calc_xHiDHiDHix (const gsl_vector *eval, const gsl_matrix *Hi, + const gsl_matrix *xHi, const size_t i1, const size_t j1, + const size_t i2, const size_t j2, + gsl_matrix *xHiDHiDHix_gg, gsl_matrix *xHiDHiDHix_ee, + gsl_matrix *xHiDHiDHix_ge) { gsl_matrix_set_zero(xHiDHiDHix_gg); gsl_matrix_set_zero(xHiDHiDHix_ee); gsl_matrix_set_zero(xHiDHiDHix_ge); @@ -1328,10 +1353,14 @@ void Calc_xHiDHiDHix (const gsl_vector *eval, const gsl_matrix *Hi, const gsl_ma for (size_t k=0; k<n_size; k++) { delta=gsl_vector_get (eval, k); - gsl_vector_const_view xHi_col_i1=gsl_matrix_const_column (xHi, k*d_size+i1); - gsl_vector_const_view xHi_col_j1=gsl_matrix_const_column (xHi, k*d_size+j1); - gsl_vector_const_view xHi_col_i2=gsl_matrix_const_column (xHi, k*d_size+i2); - gsl_vector_const_view xHi_col_j2=gsl_matrix_const_column (xHi, k*d_size+j2); + gsl_vector_const_view xHi_col_i1= + gsl_matrix_const_column (xHi, k*d_size+i1); + gsl_vector_const_view xHi_col_j1= + gsl_matrix_const_column (xHi, k*d_size+j1); + gsl_vector_const_view xHi_col_i2= + gsl_matrix_const_column (xHi, k*d_size+i2); + gsl_vector_const_view xHi_col_j2= + gsl_matrix_const_column (xHi, k*d_size+j2); d_Hi_i1i2=gsl_matrix_get (Hi, i1, k*d_size+i2); d_Hi_i1j2=gsl_matrix_get (Hi, i1, k*d_size+j2); @@ -1339,37 +1368,41 @@ void Calc_xHiDHiDHix (const gsl_vector *eval, const gsl_matrix *Hi, const gsl_ma d_Hi_j1j2=gsl_matrix_get (Hi, j1, k*d_size+j2); if (i1==j1) { - gsl_matrix_set_zero (mat_dcdc); - gsl_blas_dger (d_Hi_j1i2, &xHi_col_i1.vector, &xHi_col_j2.vector, mat_dcdc); - - gsl_matrix_add(xHiDHiDHix_ee, mat_dcdc); - gsl_matrix_scale(mat_dcdc, delta); - gsl_matrix_add(xHiDHiDHix_ge, mat_dcdc); - gsl_matrix_scale(mat_dcdc, delta); - gsl_matrix_add(xHiDHiDHix_gg, mat_dcdc); - - if (i2!=j2) { - gsl_matrix_set_zero (mat_dcdc); - gsl_blas_dger (d_Hi_j1j2, &xHi_col_i1.vector, &xHi_col_i2.vector, mat_dcdc); - - gsl_matrix_add(xHiDHiDHix_ee, mat_dcdc); - gsl_matrix_scale(mat_dcdc, delta); - gsl_matrix_add(xHiDHiDHix_ge, mat_dcdc); - gsl_matrix_scale(mat_dcdc, delta); - gsl_matrix_add(xHiDHiDHix_gg, mat_dcdc); - } + gsl_matrix_set_zero (mat_dcdc); + gsl_blas_dger (d_Hi_j1i2, &xHi_col_i1.vector, + &xHi_col_j2.vector, mat_dcdc); + + gsl_matrix_add(xHiDHiDHix_ee, mat_dcdc); + gsl_matrix_scale(mat_dcdc, delta); + gsl_matrix_add(xHiDHiDHix_ge, mat_dcdc); + gsl_matrix_scale(mat_dcdc, delta); + gsl_matrix_add(xHiDHiDHix_gg, mat_dcdc); + + if (i2!=j2) { + gsl_matrix_set_zero (mat_dcdc); + gsl_blas_dger (d_Hi_j1j2, &xHi_col_i1.vector, + &xHi_col_i2.vector, mat_dcdc); + + gsl_matrix_add(xHiDHiDHix_ee, mat_dcdc); + gsl_matrix_scale(mat_dcdc, delta); + gsl_matrix_add(xHiDHiDHix_ge, mat_dcdc); + gsl_matrix_scale(mat_dcdc, delta); + gsl_matrix_add(xHiDHiDHix_gg, mat_dcdc); + } } else { - gsl_matrix_set_zero (mat_dcdc); - gsl_blas_dger (d_Hi_j1i2, &xHi_col_i1.vector, &xHi_col_j2.vector, mat_dcdc); - - gsl_matrix_add(xHiDHiDHix_ee, mat_dcdc); - gsl_matrix_scale(mat_dcdc, delta); - gsl_matrix_add(xHiDHiDHix_ge, mat_dcdc); - gsl_matrix_scale(mat_dcdc, delta); - gsl_matrix_add(xHiDHiDHix_gg, mat_dcdc); - - gsl_matrix_set_zero (mat_dcdc); - gsl_blas_dger (d_Hi_i1i2, &xHi_col_j1.vector, &xHi_col_j2.vector, mat_dcdc); + gsl_matrix_set_zero (mat_dcdc); + gsl_blas_dger (d_Hi_j1i2, &xHi_col_i1.vector, + &xHi_col_j2.vector, mat_dcdc); + + gsl_matrix_add(xHiDHiDHix_ee, mat_dcdc); + gsl_matrix_scale(mat_dcdc, delta); + gsl_matrix_add(xHiDHiDHix_ge, mat_dcdc); + gsl_matrix_scale(mat_dcdc, delta); + gsl_matrix_add(xHiDHiDHix_gg, mat_dcdc); + + gsl_matrix_set_zero (mat_dcdc); + gsl_blas_dger (d_Hi_i1i2, &xHi_col_j1.vector, + &xHi_col_j2.vector, mat_dcdc); gsl_matrix_add(xHiDHiDHix_ee, mat_dcdc); gsl_matrix_scale(mat_dcdc, delta); @@ -1378,23 +1411,25 @@ void Calc_xHiDHiDHix (const gsl_vector *eval, const gsl_matrix *Hi, const gsl_ma gsl_matrix_add(xHiDHiDHix_gg, mat_dcdc); if (i2!=j2) { - gsl_matrix_set_zero (mat_dcdc); - gsl_blas_dger (d_Hi_j1j2, &xHi_col_i1.vector, &xHi_col_i2.vector, mat_dcdc); - - gsl_matrix_add(xHiDHiDHix_ee, mat_dcdc); - gsl_matrix_scale(mat_dcdc, delta); - gsl_matrix_add(xHiDHiDHix_ge, mat_dcdc); - gsl_matrix_scale(mat_dcdc, delta); - gsl_matrix_add(xHiDHiDHix_gg, mat_dcdc); - - gsl_matrix_set_zero (mat_dcdc); - gsl_blas_dger (d_Hi_i1j2, &xHi_col_j1.vector, &xHi_col_i2.vector, mat_dcdc); - - gsl_matrix_add(xHiDHiDHix_ee, mat_dcdc); - gsl_matrix_scale(mat_dcdc, delta); - gsl_matrix_add(xHiDHiDHix_ge, mat_dcdc); - gsl_matrix_scale(mat_dcdc, delta); - gsl_matrix_add(xHiDHiDHix_gg, mat_dcdc); + gsl_matrix_set_zero (mat_dcdc); + gsl_blas_dger (d_Hi_j1j2, &xHi_col_i1.vector, + &xHi_col_i2.vector, mat_dcdc); + + gsl_matrix_add(xHiDHiDHix_ee, mat_dcdc); + gsl_matrix_scale(mat_dcdc, delta); + gsl_matrix_add(xHiDHiDHix_ge, mat_dcdc); + gsl_matrix_scale(mat_dcdc, delta); + gsl_matrix_add(xHiDHiDHix_gg, mat_dcdc); + + gsl_matrix_set_zero (mat_dcdc); + gsl_blas_dger (d_Hi_i1j2, &xHi_col_j1.vector, + &xHi_col_i2.vector, mat_dcdc); + + gsl_matrix_add(xHiDHiDHix_ee, mat_dcdc); + gsl_matrix_scale(mat_dcdc, delta); + gsl_matrix_add(xHiDHiDHix_ge, mat_dcdc); + gsl_matrix_scale(mat_dcdc, delta); + gsl_matrix_add(xHiDHiDHix_gg, mat_dcdc); } } } @@ -1404,10 +1439,9 @@ void Calc_xHiDHiDHix (const gsl_vector *eval, const gsl_matrix *Hi, const gsl_ma return; } - - -void Calc_traceHiD (const gsl_vector *eval, const gsl_matrix *Hi, const size_t i, const size_t j, double &tHiD_g, double &tHiD_e) -{ +void Calc_traceHiD (const gsl_vector *eval, const gsl_matrix *Hi, + const size_t i, const size_t j, double &tHiD_g, + double &tHiD_e) { tHiD_g=0.0; tHiD_e=0.0; @@ -1430,9 +1464,10 @@ void Calc_traceHiD (const gsl_vector *eval, const gsl_matrix *Hi, const size_t i return; } - -void Calc_traceHiDHiD (const gsl_vector *eval, const gsl_matrix *Hi, const size_t i1, const size_t j1, const size_t i2, const size_t j2, double &tHiDHiD_gg, double &tHiDHiD_ee, double &tHiDHiD_ge) -{ +void Calc_traceHiDHiD (const gsl_vector *eval, const gsl_matrix *Hi, + const size_t i1, const size_t j1, const size_t i2, + const size_t j2, double &tHiDHiD_gg, double &tHiDHiD_ee, + double &tHiDHiD_ge) { tHiDHiD_gg=0.0; tHiDHiD_ee=0.0; tHiDHiD_ge=0.0; @@ -1449,48 +1484,54 @@ void Calc_traceHiDHiD (const gsl_vector *eval, const gsl_matrix *Hi, const size_ d_Hi_j1j2=gsl_matrix_get (Hi, j1, k*d_size+j2); if (i1==j1) { - tHiDHiD_gg+=delta*delta*d_Hi_i1j2*d_Hi_j1i2; - tHiDHiD_ee+=d_Hi_i1j2*d_Hi_j1i2; - tHiDHiD_ge+=delta*d_Hi_i1j2*d_Hi_j1i2; - - if (i2!=j2) { - tHiDHiD_gg+=delta*delta*d_Hi_i1i2*d_Hi_j1j2; - tHiDHiD_ee+=d_Hi_i1i2*d_Hi_j1j2; - tHiDHiD_ge+=delta*d_Hi_i1i2*d_Hi_j1j2; - } + tHiDHiD_gg+=delta*delta*d_Hi_i1j2*d_Hi_j1i2; + tHiDHiD_ee+=d_Hi_i1j2*d_Hi_j1i2; + tHiDHiD_ge+=delta*d_Hi_i1j2*d_Hi_j1i2; + + if (i2!=j2) { + tHiDHiD_gg+=delta*delta*d_Hi_i1i2*d_Hi_j1j2; + tHiDHiD_ee+=d_Hi_i1i2*d_Hi_j1j2; + tHiDHiD_ge+=delta*d_Hi_i1i2*d_Hi_j1j2; + } } else { - tHiDHiD_gg+=delta*delta*(d_Hi_i1j2*d_Hi_j1i2+d_Hi_j1j2*d_Hi_i1i2); - tHiDHiD_ee+=(d_Hi_i1j2*d_Hi_j1i2+d_Hi_j1j2*d_Hi_i1i2); - tHiDHiD_ge+=delta*(d_Hi_i1j2*d_Hi_j1i2+d_Hi_j1j2*d_Hi_i1i2); - - if (i2!=j2) { - tHiDHiD_gg+=delta*delta*(d_Hi_i1i2*d_Hi_j1j2+d_Hi_j1i2*d_Hi_i1j2); - tHiDHiD_ee+=(d_Hi_i1i2*d_Hi_j1j2+d_Hi_j1i2*d_Hi_i1j2); - tHiDHiD_ge+=delta*(d_Hi_i1i2*d_Hi_j1j2+d_Hi_j1i2*d_Hi_i1j2); - } + tHiDHiD_gg+=delta*delta*(d_Hi_i1j2*d_Hi_j1i2+d_Hi_j1j2* + d_Hi_i1i2); + tHiDHiD_ee+=(d_Hi_i1j2*d_Hi_j1i2+d_Hi_j1j2*d_Hi_i1i2); + tHiDHiD_ge+=delta*(d_Hi_i1j2*d_Hi_j1i2+d_Hi_j1j2*d_Hi_i1i2); + + if (i2!=j2) { + tHiDHiD_gg+=delta*delta*(d_Hi_i1i2*d_Hi_j1j2+d_Hi_j1i2* + d_Hi_i1j2); + tHiDHiD_ee+=(d_Hi_i1i2*d_Hi_j1j2+d_Hi_j1i2*d_Hi_i1j2); + tHiDHiD_ge+=delta*(d_Hi_i1i2*d_Hi_j1j2 + + d_Hi_j1i2*d_Hi_i1j2); + } } } return; } - -//trace(PD)=trace((Hi-HixQixHi)D)=trace(HiD)-trace(HixQixHiD) -void Calc_tracePD (const gsl_vector *eval, const gsl_matrix *Qi, const gsl_matrix *Hi, const gsl_matrix *xHiDHix_all_g, const gsl_matrix *xHiDHix_all_e, const size_t i, const size_t j, double &tPD_g, double &tPD_e) -{ +// trace(PD) = trace((Hi-HixQixHi)D)=trace(HiD) - trace(HixQixHiD) +void Calc_tracePD (const gsl_vector *eval, const gsl_matrix *Qi, + const gsl_matrix *Hi, const gsl_matrix *xHiDHix_all_g, + const gsl_matrix *xHiDHix_all_e, const size_t i, + const size_t j, double &tPD_g, double &tPD_e) { size_t dc_size=Qi->size1, d_size=Hi->size1; size_t v=GetIndex(i, j, d_size); double d; - //calculate the first part: trace(HiD) + // Calculate the first part: trace(HiD). Calc_traceHiD (eval, Hi, i, j, tPD_g, tPD_e); - //calculate the second part: -trace(HixQixHiD) + // Calculate the second part: -trace(HixQixHiD). for (size_t k=0; k<dc_size; k++) { gsl_vector_const_view Qi_row=gsl_matrix_const_row (Qi, k); - gsl_vector_const_view xHiDHix_g_col=gsl_matrix_const_column (xHiDHix_all_g, v*dc_size+k); - gsl_vector_const_view xHiDHix_e_col=gsl_matrix_const_column (xHiDHix_all_e, v*dc_size+k); + gsl_vector_const_view xHiDHix_g_col = + gsl_matrix_const_column (xHiDHix_all_g, v*dc_size+k); + gsl_vector_const_view xHiDHix_e_col = + gsl_matrix_const_column (xHiDHix_all_e, v*dc_size+k); gsl_blas_ddot(&Qi_row.vector, &xHiDHix_g_col.vector, &d); tPD_g-=d; @@ -1501,77 +1542,86 @@ void Calc_tracePD (const gsl_vector *eval, const gsl_matrix *Qi, const gsl_matri return; } - - -//trace(PDPD)=trace((Hi-HixQixHi)D(Hi-HixQixHi)D) -//=trace(HiDHiD)-trace(HixQixHiDHiD)-trace(HiDHixQixHiD)+trace(HixQixHiDHixQixHiD) -void Calc_tracePDPD (const gsl_vector *eval, const gsl_matrix *Qi, const gsl_matrix *Hi, const gsl_matrix *xHi, const gsl_matrix *QixHiDHix_all_g, const gsl_matrix *QixHiDHix_all_e, const gsl_matrix *xHiDHiDHix_all_gg, const gsl_matrix *xHiDHiDHix_all_ee, const gsl_matrix *xHiDHiDHix_all_ge, const size_t i1, const size_t j1, const size_t i2, const size_t j2, double &tPDPD_gg, double &tPDPD_ee, double &tPDPD_ge) -{ +// trace(PDPD) = trace((Hi-HixQixHi)D(Hi-HixQixHi)D) +// = trace(HiDHiD) - trace(HixQixHiDHiD) +// - trace(HiDHixQixHiD) + trace(HixQixHiDHixQixHiD) +void Calc_tracePDPD (const gsl_vector *eval, const gsl_matrix *Qi, + const gsl_matrix *Hi, const gsl_matrix *xHi, + const gsl_matrix *QixHiDHix_all_g, + const gsl_matrix *QixHiDHix_all_e, + const gsl_matrix *xHiDHiDHix_all_gg, + const gsl_matrix *xHiDHiDHix_all_ee, + const gsl_matrix *xHiDHiDHix_all_ge, + const size_t i1, const size_t j1, + const size_t i2, const size_t j2, + double &tPDPD_gg, double &tPDPD_ee, + double &tPDPD_ge) { size_t dc_size=Qi->size1, d_size=Hi->size1; size_t v_size=d_size*(d_size+1)/2; size_t v1=GetIndex(i1, j1, d_size), v2=GetIndex(i2, j2, d_size); double d; - //calculate the first part: trace(HiDHiD) - Calc_traceHiDHiD (eval, Hi, i1, j1, i2, j2, tPDPD_gg, tPDPD_ee, tPDPD_ge); + // Calculate the first part: trace(HiDHiD). + Calc_traceHiDHiD (eval, Hi, i1, j1, i2, j2, tPDPD_gg, tPDPD_ee, + tPDPD_ge); - //calculate the second and third parts: -trace(HixQixHiDHiD)-trace(HiDHixQixHiD) + // Calculate the second and third parts: + // -trace(HixQixHiDHiD) - trace(HiDHixQixHiD) for (size_t i=0; i<dc_size; i++) { - gsl_vector_const_view Qi_row=gsl_matrix_const_row (Qi, i); - gsl_vector_const_view xHiDHiDHix_gg_col=gsl_matrix_const_column (xHiDHiDHix_all_gg, (v1*v_size+v2)*dc_size+i); - gsl_vector_const_view xHiDHiDHix_ee_col=gsl_matrix_const_column (xHiDHiDHix_all_ee, (v1*v_size+v2)*dc_size+i); - gsl_vector_const_view xHiDHiDHix_ge_col=gsl_matrix_const_column (xHiDHiDHix_all_ge, (v1*v_size+v2)*dc_size+i); - - gsl_blas_ddot(&Qi_row.vector, &xHiDHiDHix_gg_col.vector, &d); - tPDPD_gg-=d*2.0; - gsl_blas_ddot(&Qi_row.vector, &xHiDHiDHix_ee_col.vector, &d); - tPDPD_ee-=d*2.0; - gsl_blas_ddot(&Qi_row.vector, &xHiDHiDHix_ge_col.vector, &d); - tPDPD_ge-=d*2.0; - /* - gsl_vector_const_view xHiDHiDHix_gg_row=gsl_matrix_const_row (xHiDHiDHix_gg, i); - gsl_vector_const_view xHiDHiDHix_ee_row=gsl_matrix_const_row (xHiDHiDHix_ee, i); - gsl_vector_const_view xHiDHiDHix_ge_row=gsl_matrix_const_row (xHiDHiDHix_ge, i); - - gsl_blas_ddot(&Qi_row.vector, &xHiDHiDHix_gg_row.vector, &d); - tPDPD_gg-=d; - gsl_blas_ddot(&Qi_row.vector, &xHiDHiDHix_ee_row.vector, &d); - tPDPD_ee-=d; - gsl_blas_ddot(&Qi_row.vector, &xHiDHiDHix_ge_row.vector, &d); - tPDPD_ge-=d; - */ + gsl_vector_const_view Qi_row=gsl_matrix_const_row (Qi, i); + gsl_vector_const_view xHiDHiDHix_gg_col= + gsl_matrix_const_column(xHiDHiDHix_all_gg, + (v1*v_size+v2)*dc_size+i); + gsl_vector_const_view xHiDHiDHix_ee_col = + gsl_matrix_const_column(xHiDHiDHix_all_ee, + (v1*v_size+v2)*dc_size+i); + gsl_vector_const_view xHiDHiDHix_ge_col = + gsl_matrix_const_column(xHiDHiDHix_all_ge, + (v1*v_size+v2)*dc_size+i); + + gsl_blas_ddot(&Qi_row.vector, &xHiDHiDHix_gg_col.vector, &d); + tPDPD_gg-=d*2.0; + gsl_blas_ddot(&Qi_row.vector, &xHiDHiDHix_ee_col.vector, &d); + tPDPD_ee-=d*2.0; + gsl_blas_ddot(&Qi_row.vector, &xHiDHiDHix_ge_col.vector, &d); + tPDPD_ge-=d*2.0; } - //calculate the fourth part: trace(HixQixHiDHixQixHiD) + // Calculate the fourth part: trace(HixQixHiDHixQixHiD). for (size_t i=0; i<dc_size; i++) { - //gsl_vector_const_view QixHiDHix_g_row1=gsl_matrix_const_subrow (QixHiDHix_all_g, i, v1*dc_size, dc_size); - //gsl_vector_const_view QixHiDHix_e_row1=gsl_matrix_const_subrow (QixHiDHix_all_e, i, v1*dc_size, dc_size); - - gsl_vector_const_view QixHiDHix_g_fullrow1=gsl_matrix_const_row (QixHiDHix_all_g, i); - gsl_vector_const_view QixHiDHix_e_fullrow1=gsl_matrix_const_row (QixHiDHix_all_e, i); - gsl_vector_const_view QixHiDHix_g_row1=gsl_vector_const_subvector (&QixHiDHix_g_fullrow1.vector, v1*dc_size, dc_size); - gsl_vector_const_view QixHiDHix_e_row1=gsl_vector_const_subvector (&QixHiDHix_e_fullrow1.vector, v1*dc_size, dc_size); - - gsl_vector_const_view QixHiDHix_g_col2=gsl_matrix_const_column (QixHiDHix_all_g, v2*dc_size+i); - gsl_vector_const_view QixHiDHix_e_col2=gsl_matrix_const_column (QixHiDHix_all_e, v2*dc_size+i); - gsl_blas_ddot(&QixHiDHix_g_row1.vector, &QixHiDHix_g_col2.vector, &d); - tPDPD_gg+=d; - gsl_blas_ddot(&QixHiDHix_e_row1.vector, &QixHiDHix_e_col2.vector, &d); - tPDPD_ee+=d; - gsl_blas_ddot(&QixHiDHix_g_row1.vector, &QixHiDHix_e_col2.vector, &d); - tPDPD_ge+=d; + gsl_vector_const_view QixHiDHix_g_fullrow1 = + gsl_matrix_const_row (QixHiDHix_all_g, i); + gsl_vector_const_view QixHiDHix_e_fullrow1 = + gsl_matrix_const_row (QixHiDHix_all_e, i); + gsl_vector_const_view QixHiDHix_g_row1 = + gsl_vector_const_subvector (&QixHiDHix_g_fullrow1.vector, + v1*dc_size, dc_size); + gsl_vector_const_view QixHiDHix_e_row1 = + gsl_vector_const_subvector (&QixHiDHix_e_fullrow1.vector, + v1*dc_size, dc_size); + + gsl_vector_const_view QixHiDHix_g_col2 = + gsl_matrix_const_column (QixHiDHix_all_g, v2*dc_size+i); + gsl_vector_const_view QixHiDHix_e_col2 = + gsl_matrix_const_column (QixHiDHix_all_e, v2*dc_size+i); + + gsl_blas_ddot(&QixHiDHix_g_row1.vector,&QixHiDHix_g_col2.vector,&d); + tPDPD_gg+=d; + gsl_blas_ddot(&QixHiDHix_e_row1.vector,&QixHiDHix_e_col2.vector,&d); + tPDPD_ee+=d; + gsl_blas_ddot(&QixHiDHix_g_row1.vector,&QixHiDHix_e_col2.vector,&d); + tPDPD_ge+=d; } return; } - - -//calculate (xHiDHiy) for every pair of i j -void Calc_xHiDHiy_all (const gsl_vector *eval, const gsl_matrix *xHi, const gsl_matrix *Hiy, gsl_matrix *xHiDHiy_all_g, gsl_matrix *xHiDHiy_all_e) -{ +// Calculate (xHiDHiy) for every pair (i,j). +void Calc_xHiDHiy_all (const gsl_vector *eval, const gsl_matrix *xHi, + const gsl_matrix *Hiy, gsl_matrix *xHiDHiy_all_g, + gsl_matrix *xHiDHiy_all_e) { gsl_matrix_set_zero(xHiDHiy_all_g); gsl_matrix_set_zero(xHiDHiy_all_e); @@ -1579,48 +1629,51 @@ void Calc_xHiDHiy_all (const gsl_vector *eval, const gsl_matrix *xHi, const gsl_ size_t v; for (size_t i=0; i<d_size; i++) { - for (size_t j=0; j<d_size; j++) { - if (j<i) {continue;} - v=GetIndex(i, j, d_size); - - gsl_vector_view xHiDHiy_g=gsl_matrix_column (xHiDHiy_all_g, v); - gsl_vector_view xHiDHiy_e=gsl_matrix_column (xHiDHiy_all_e, v); - - Calc_xHiDHiy (eval, xHi, Hiy, i, j, &xHiDHiy_g.vector, &xHiDHiy_e.vector); - } + for (size_t j=0; j<d_size; j++) { + if (j<i) {continue;} + v=GetIndex(i, j, d_size); + + gsl_vector_view xHiDHiy_g=gsl_matrix_column (xHiDHiy_all_g, v); + gsl_vector_view xHiDHiy_e=gsl_matrix_column (xHiDHiy_all_e, v); + + Calc_xHiDHiy (eval, xHi, Hiy, i, j, &xHiDHiy_g.vector, + &xHiDHiy_e.vector); + } } return; } - -//calculate (xHiDHix) for every pair of i j -void Calc_xHiDHix_all (const gsl_vector *eval, const gsl_matrix *xHi, gsl_matrix *xHiDHix_all_g, gsl_matrix *xHiDHix_all_e) -{ - gsl_matrix_set_zero(xHiDHix_all_g); - gsl_matrix_set_zero(xHiDHix_all_e); - - size_t d_size=xHi->size2/eval->size, dc_size=xHi->size1; - size_t v; - - for (size_t i=0; i<d_size; i++) { - for (size_t j=0; j<d_size; j++) { - if (j<i) {continue;} - v=GetIndex(i, j, d_size); - - gsl_matrix_view xHiDHix_g=gsl_matrix_submatrix (xHiDHix_all_g, 0, v*dc_size, dc_size, dc_size); - gsl_matrix_view xHiDHix_e=gsl_matrix_submatrix (xHiDHix_all_e, 0, v*dc_size, dc_size, dc_size); - - Calc_xHiDHix (eval, xHi, i, j, &xHiDHix_g.matrix, &xHiDHix_e.matrix); - } - } - return; +// Calculate (xHiDHix) for every pair (i,j). +void Calc_xHiDHix_all (const gsl_vector *eval, const gsl_matrix *xHi, + gsl_matrix *xHiDHix_all_g, gsl_matrix *xHiDHix_all_e) { + gsl_matrix_set_zero(xHiDHix_all_g); + gsl_matrix_set_zero(xHiDHix_all_e); + + size_t d_size=xHi->size2/eval->size, dc_size=xHi->size1; + size_t v; + + for (size_t i=0; i<d_size; i++) { + for (size_t j=0; j<d_size; j++) { + if (j<i) {continue;} + v=GetIndex(i, j, d_size); + + gsl_matrix_view xHiDHix_g = + gsl_matrix_submatrix (xHiDHix_all_g, 0, v*dc_size, dc_size, dc_size); + gsl_matrix_view xHiDHix_e = + gsl_matrix_submatrix (xHiDHix_all_e, 0, v*dc_size, dc_size, dc_size); + + Calc_xHiDHix (eval, xHi, i, j, &xHiDHix_g.matrix, &xHiDHix_e.matrix); + } + } + return; } - - -//calculate (xHiDHiy) for every pair of i j -void Calc_xHiDHiDHiy_all (const size_t v_size, const gsl_vector *eval, const gsl_matrix *Hi, const gsl_matrix *xHi, const gsl_matrix *Hiy, gsl_matrix *xHiDHiDHiy_all_gg, gsl_matrix *xHiDHiDHiy_all_ee, gsl_matrix *xHiDHiDHiy_all_ge) -{ +// Calculate (xHiDHiy) for every pair (i,j). +void Calc_xHiDHiDHiy_all (const size_t v_size, const gsl_vector *eval, + const gsl_matrix *Hi, const gsl_matrix *xHi, + const gsl_matrix *Hiy, gsl_matrix *xHiDHiDHiy_all_gg, + gsl_matrix *xHiDHiDHiy_all_ee, + gsl_matrix *xHiDHiDHiy_all_ge) { gsl_matrix_set_zero(xHiDHiDHiy_all_gg); gsl_matrix_set_zero(xHiDHiDHiy_all_ee); gsl_matrix_set_zero(xHiDHiDHiy_all_ge); @@ -1629,31 +1682,36 @@ void Calc_xHiDHiDHiy_all (const size_t v_size, const gsl_vector *eval, const gsl size_t v1, v2; for (size_t i1=0; i1<d_size; i1++) { - for (size_t j1=0; j1<d_size; j1++) { - if (j1<i1) {continue;} - v1=GetIndex(i1, j1, d_size); - - for (size_t i2=0; i2<d_size; i2++) { - for (size_t j2=0; j2<d_size; j2++) { - if (j2<i2) {continue;} - v2=GetIndex(i2, j2, d_size); - - gsl_vector_view xHiDHiDHiy_gg=gsl_matrix_column (xHiDHiDHiy_all_gg, v1*v_size+v2); - gsl_vector_view xHiDHiDHiy_ee=gsl_matrix_column (xHiDHiDHiy_all_ee, v1*v_size+v2); - gsl_vector_view xHiDHiDHiy_ge=gsl_matrix_column (xHiDHiDHiy_all_ge, v1*v_size+v2); - - Calc_xHiDHiDHiy (eval, Hi, xHi, Hiy, i1, j1, i2, j2, &xHiDHiDHiy_gg.vector, &xHiDHiDHiy_ee.vector, &xHiDHiDHiy_ge.vector); - } - } - } + for (size_t j1=0; j1<d_size; j1++) { + if (j1<i1) {continue;} + v1=GetIndex(i1, j1, d_size); + + for (size_t i2=0; i2<d_size; i2++) { + for (size_t j2=0; j2<d_size; j2++) { + if (j2<i2) {continue;} + v2=GetIndex(i2, j2, d_size); + + gsl_vector_view xHiDHiDHiy_gg = + gsl_matrix_column (xHiDHiDHiy_all_gg, v1*v_size+v2); + gsl_vector_view xHiDHiDHiy_ee = + gsl_matrix_column (xHiDHiDHiy_all_ee, v1*v_size+v2); + gsl_vector_view xHiDHiDHiy_ge = + gsl_matrix_column (xHiDHiDHiy_all_ge, v1*v_size+v2); + + Calc_xHiDHiDHiy (eval, Hi, xHi, Hiy, i1, j1, i2, j2, &xHiDHiDHiy_gg.vector, &xHiDHiDHiy_ee.vector, &xHiDHiDHiy_ge.vector); + } + } + } } return; } - -//calculate (xHiDHix) for every pair of i j -void Calc_xHiDHiDHix_all (const size_t v_size, const gsl_vector *eval, const gsl_matrix *Hi, const gsl_matrix *xHi, gsl_matrix *xHiDHiDHix_all_gg, gsl_matrix *xHiDHiDHix_all_ee, gsl_matrix *xHiDHiDHix_all_ge) -{ +// Calculate (xHiDHix) for every pair (i,j). +void Calc_xHiDHiDHix_all (const size_t v_size, const gsl_vector *eval, + const gsl_matrix *Hi, const gsl_matrix *xHi, + gsl_matrix *xHiDHiDHix_all_gg, + gsl_matrix *xHiDHiDHix_all_ee, + gsl_matrix *xHiDHiDHix_all_ge) { gsl_matrix_set_zero(xHiDHiDHix_all_gg); gsl_matrix_set_zero(xHiDHiDHix_all_ee); gsl_matrix_set_zero(xHiDHiDHix_all_ge); @@ -1662,205 +1720,181 @@ void Calc_xHiDHiDHix_all (const size_t v_size, const gsl_vector *eval, const gsl size_t v1, v2; for (size_t i1=0; i1<d_size; i1++) { - for (size_t j1=0; j1<d_size; j1++) { - if (j1<i1) {continue;} - v1=GetIndex(i1, j1, d_size); - - for (size_t i2=0; i2<d_size; i2++) { - for (size_t j2=0; j2<d_size; j2++) { - if (j2<i2) {continue;} - v2=GetIndex(i2, j2, d_size); - - if (v2<v1) {continue;} - - gsl_matrix_view xHiDHiDHix_gg1=gsl_matrix_submatrix (xHiDHiDHix_all_gg, 0, (v1*v_size+v2)*dc_size, dc_size, dc_size); - gsl_matrix_view xHiDHiDHix_ee1=gsl_matrix_submatrix (xHiDHiDHix_all_ee, 0, (v1*v_size+v2)*dc_size, dc_size, dc_size); - gsl_matrix_view xHiDHiDHix_ge1=gsl_matrix_submatrix (xHiDHiDHix_all_ge, 0, (v1*v_size+v2)*dc_size, dc_size, dc_size); - - Calc_xHiDHiDHix (eval, Hi, xHi, i1, j1, i2, j2, &xHiDHiDHix_gg1.matrix, &xHiDHiDHix_ee1.matrix, &xHiDHiDHix_ge1.matrix); - - if (v2!=v1) { - gsl_matrix_view xHiDHiDHix_gg2=gsl_matrix_submatrix (xHiDHiDHix_all_gg, 0, (v2*v_size+v1)*dc_size, dc_size, dc_size); - gsl_matrix_view xHiDHiDHix_ee2=gsl_matrix_submatrix (xHiDHiDHix_all_ee, 0, (v2*v_size+v1)*dc_size, dc_size, dc_size); - gsl_matrix_view xHiDHiDHix_ge2=gsl_matrix_submatrix (xHiDHiDHix_all_ge, 0, (v2*v_size+v1)*dc_size, dc_size, dc_size); - - gsl_matrix_memcpy (&xHiDHiDHix_gg2.matrix, &xHiDHiDHix_gg1.matrix); - gsl_matrix_memcpy (&xHiDHiDHix_ee2.matrix, &xHiDHiDHix_ee1.matrix); - gsl_matrix_memcpy (&xHiDHiDHix_ge2.matrix, &xHiDHiDHix_ge1.matrix); - } - } - } - } - } - - - /* - size_t n_size=eval->size; - double delta, d_Hi_ij; - - gsl_matrix *mat_dcdc=gsl_matrix_alloc (dc_size, dc_size); - gsl_matrix *mat_dcdc_temp=gsl_matrix_alloc (dc_size, dc_size); - - for (size_t k=0; k<n_size; k++) { - delta=gsl_vector_get (eval, k); - - for (size_t i1=0; i1<d_size; i1++) { - for (size_t j2=0; j2<d_size; j2++) { - gsl_vector_const_view xHi_col_i=gsl_matrix_const_column (xHi, k*d_size+i1); - gsl_vector_const_view xHi_col_j=gsl_matrix_const_column (xHi, k*d_size+j2); - - gsl_matrix_set_zero (mat_dcdc); - gsl_blas_dger (1.0, &xHi_col_i.vector, &xHi_col_j.vector, mat_dcdc); - - for (size_t j1=0; j1<d_size; j1++) { - for (size_t i2=0; i2<d_size; i2++) { - d_Hi_ij=gsl_matrix_get (Hi, j1, k*d_size+i2); - - v1=GetIndex(i1, j1, d_size); - v2=GetIndex(i2, j2, d_size); - - gsl_matrix_view xHiDHiDHix_gg=gsl_matrix_submatrix (xHiDHiDHix_all_gg, 0, (v1*v_size+v2)*dc_size, dc_size, dc_size); - gsl_matrix_view xHiDHiDHix_ee=gsl_matrix_submatrix (xHiDHiDHix_all_ee, 0, (v1*v_size+v2)*dc_size, dc_size, dc_size); - gsl_matrix_view xHiDHiDHix_ge=gsl_matrix_submatrix (xHiDHiDHix_all_ge, 0, (v1*v_size+v2)*dc_size, dc_size, dc_size); - - gsl_matrix_memcpy (mat_dcdc_temp, mat_dcdc); - - gsl_matrix_scale (mat_dcdc_temp, d_Hi_ij); - gsl_matrix_add(&xHiDHiDHix_ee.matrix, mat_dcdc_temp); - gsl_matrix_scale(mat_dcdc_temp, delta); - gsl_matrix_add(&xHiDHiDHix_ge.matrix, mat_dcdc_temp); - gsl_matrix_scale(mat_dcdc_temp, delta); - gsl_matrix_add(&xHiDHiDHix_gg.matrix, mat_dcdc_temp); - } - } - } - } - } - - for (size_t i1=0; i1<d_size; i1++) { - for (size_t j1=0; j1<d_size; j1++) { - v1=GetIndex(i1, j1, d_size); - - for (size_t i2=0; i2<d_size; i2++) { - for (size_t j2=0; j2<d_size; j2++) { - v2=GetIndex(i2, j2, d_size); - - if (i1!=j1 && i2!=j2) {continue;} - - gsl_matrix_view xHiDHiDHix_gg=gsl_matrix_submatrix (xHiDHiDHix_all_gg, 0, (v1*v_size+v2)*dc_size, dc_size, dc_size); - gsl_matrix_view xHiDHiDHix_ee=gsl_matrix_submatrix (xHiDHiDHix_all_ee, 0, (v1*v_size+v2)*dc_size, dc_size, dc_size); - gsl_matrix_view xHiDHiDHix_ge=gsl_matrix_submatrix (xHiDHiDHix_all_ge, 0, (v1*v_size+v2)*dc_size, dc_size, dc_size); - - if ( (i1==j1 && i2!=j2) || (i1!=j1 && i2==j2) ) { - gsl_matrix_scale (&xHiDHiDHix_gg.matrix, 0.5); - gsl_matrix_scale (&xHiDHiDHix_ee.matrix, 0.5); - gsl_matrix_scale (&xHiDHiDHix_ge.matrix, 0.5); - } else { - gsl_matrix_scale (&xHiDHiDHix_gg.matrix, 0.25); - gsl_matrix_scale (&xHiDHiDHix_ee.matrix, 0.25); - gsl_matrix_scale (&xHiDHiDHix_ge.matrix, 0.25); - } - } - } - } + for (size_t j1=0; j1<d_size; j1++) { + if (j1<i1) {continue;} + v1=GetIndex(i1, j1, d_size); + + for (size_t i2=0; i2<d_size; i2++) { + for (size_t j2=0; j2<d_size; j2++) { + if (j2<i2) {continue;} + v2=GetIndex(i2, j2, d_size); + + if (v2<v1) {continue;} + + gsl_matrix_view xHiDHiDHix_gg1 = + gsl_matrix_submatrix (xHiDHiDHix_all_gg, 0, + (v1*v_size+v2)*dc_size, + dc_size, dc_size); + gsl_matrix_view xHiDHiDHix_ee1 = + gsl_matrix_submatrix (xHiDHiDHix_all_ee, 0, + (v1*v_size+v2)*dc_size, + dc_size, dc_size); + gsl_matrix_view xHiDHiDHix_ge1 = + gsl_matrix_submatrix (xHiDHiDHix_all_ge, 0, + (v1*v_size+v2)*dc_size, + dc_size, dc_size); + + Calc_xHiDHiDHix (eval, Hi, xHi, i1, j1, i2, j2, + &xHiDHiDHix_gg1.matrix, + &xHiDHiDHix_ee1.matrix, + &xHiDHiDHix_ge1.matrix); + + if (v2!=v1) { + gsl_matrix_view xHiDHiDHix_gg2 = + gsl_matrix_submatrix (xHiDHiDHix_all_gg, 0, + (v2*v_size+v1)*dc_size, + dc_size, dc_size); + gsl_matrix_view xHiDHiDHix_ee2 = + gsl_matrix_submatrix (xHiDHiDHix_all_ee, 0, + (v2*v_size+v1)*dc_size, + dc_size, dc_size); + gsl_matrix_view xHiDHiDHix_ge2 = + gsl_matrix_submatrix (xHiDHiDHix_all_ge, 0, + (v2*v_size+v1)*dc_size, + dc_size, dc_size); + + gsl_matrix_memcpy (&xHiDHiDHix_gg2.matrix, + &xHiDHiDHix_gg1.matrix); + gsl_matrix_memcpy (&xHiDHiDHix_ee2.matrix, + &xHiDHiDHix_ee1.matrix); + gsl_matrix_memcpy (&xHiDHiDHix_ge2.matrix, + &xHiDHiDHix_ge1.matrix); + } + } + } + } } - gsl_matrix_free (mat_dcdc); - gsl_matrix_free (mat_dcdc_temp); - */ - return; } - - -//calculate (xHiDHix)Qi(xHiy) for every pair of i, j -void Calc_xHiDHixQixHiy_all (const gsl_matrix *xHiDHix_all_g, const gsl_matrix *xHiDHix_all_e, const gsl_vector *QixHiy, gsl_matrix *xHiDHixQixHiy_all_g, gsl_matrix *xHiDHixQixHiy_all_e) -{ +// Calculate (xHiDHix)Qi(xHiy) for every pair (i,j). +void Calc_xHiDHixQixHiy_all (const gsl_matrix *xHiDHix_all_g, + const gsl_matrix *xHiDHix_all_e, + const gsl_vector *QixHiy, + gsl_matrix *xHiDHixQixHiy_all_g, + gsl_matrix *xHiDHixQixHiy_all_e) { size_t dc_size=xHiDHix_all_g->size1; size_t v_size=xHiDHix_all_g->size2/dc_size; for (size_t i=0; i<v_size; i++) { - gsl_matrix_const_view xHiDHix_g=gsl_matrix_const_submatrix (xHiDHix_all_g, 0, i*dc_size, dc_size, dc_size); - gsl_matrix_const_view xHiDHix_e=gsl_matrix_const_submatrix (xHiDHix_all_e, 0, i*dc_size, dc_size, dc_size); + gsl_matrix_const_view xHiDHix_g = + gsl_matrix_const_submatrix (xHiDHix_all_g, 0, i*dc_size, + dc_size, dc_size); + gsl_matrix_const_view xHiDHix_e = + gsl_matrix_const_submatrix (xHiDHix_all_e, 0, i*dc_size, + dc_size, dc_size); - gsl_vector_view xHiDHixQixHiy_g=gsl_matrix_column (xHiDHixQixHiy_all_g, i); - gsl_vector_view xHiDHixQixHiy_e=gsl_matrix_column (xHiDHixQixHiy_all_e, i); + gsl_vector_view xHiDHixQixHiy_g = + gsl_matrix_column (xHiDHixQixHiy_all_g, i); + gsl_vector_view xHiDHixQixHiy_e = + gsl_matrix_column (xHiDHixQixHiy_all_e, i); - gsl_blas_dgemv (CblasNoTrans, 1.0, &xHiDHix_g.matrix, QixHiy, 0.0, &xHiDHixQixHiy_g.vector); - gsl_blas_dgemv (CblasNoTrans, 1.0, &xHiDHix_e.matrix, QixHiy, 0.0, &xHiDHixQixHiy_e.vector); + gsl_blas_dgemv (CblasNoTrans, 1.0, &xHiDHix_g.matrix, + QixHiy, 0.0, &xHiDHixQixHiy_g.vector); + gsl_blas_dgemv (CblasNoTrans, 1.0, &xHiDHix_e.matrix, + QixHiy, 0.0, &xHiDHixQixHiy_e.vector); } return; } -//calculate Qi(xHiDHiy) and Qi(xHiDHix)Qi(xHiy) for each pair of i j (i<=j) -void Calc_QiVec_all (const gsl_matrix *Qi, const gsl_matrix *vec_all_g, const gsl_matrix *vec_all_e, gsl_matrix *Qivec_all_g, gsl_matrix *Qivec_all_e) -{ +// Calculate Qi(xHiDHiy) and Qi(xHiDHix)Qi(xHiy) for each pair of i,j (i<=j). +void Calc_QiVec_all (const gsl_matrix *Qi, const gsl_matrix *vec_all_g, + const gsl_matrix *vec_all_e, gsl_matrix *Qivec_all_g, + gsl_matrix *Qivec_all_e) { for (size_t i=0; i<vec_all_g->size2; i++) { - gsl_vector_const_view vec_g=gsl_matrix_const_column (vec_all_g, i); - gsl_vector_const_view vec_e=gsl_matrix_const_column (vec_all_e, i); - - gsl_vector_view Qivec_g=gsl_matrix_column (Qivec_all_g, i); - gsl_vector_view Qivec_e=gsl_matrix_column (Qivec_all_e, i); - - gsl_blas_dgemv (CblasNoTrans, 1.0, Qi, &vec_g.vector, 0.0, &Qivec_g.vector); - gsl_blas_dgemv (CblasNoTrans, 1.0, Qi, &vec_e.vector, 0.0, &Qivec_e.vector); + gsl_vector_const_view vec_g=gsl_matrix_const_column (vec_all_g, i); + gsl_vector_const_view vec_e=gsl_matrix_const_column (vec_all_e, i); + + gsl_vector_view Qivec_g=gsl_matrix_column (Qivec_all_g, i); + gsl_vector_view Qivec_e=gsl_matrix_column (Qivec_all_e, i); + + gsl_blas_dgemv(CblasNoTrans,1.0,Qi,&vec_g.vector,0.0, + &Qivec_g.vector); + gsl_blas_dgemv(CblasNoTrans,1.0,Qi,&vec_e.vector,0.0, + &Qivec_e.vector); } return; } - -//calculate Qi(xHiDHix) for each pair of i j (i<=j) -void Calc_QiMat_all (const gsl_matrix *Qi, const gsl_matrix *mat_all_g, const gsl_matrix *mat_all_e, gsl_matrix *Qimat_all_g, gsl_matrix *Qimat_all_e) -{ +// Calculate Qi(xHiDHix) for each pair of i,j (i<=j). +void Calc_QiMat_all (const gsl_matrix *Qi, const gsl_matrix *mat_all_g, + const gsl_matrix *mat_all_e, gsl_matrix *Qimat_all_g, + gsl_matrix *Qimat_all_e) { size_t dc_size=Qi->size1; size_t v_size=mat_all_g->size2/mat_all_g->size1; for (size_t i=0; i<v_size; i++) { - gsl_matrix_const_view mat_g=gsl_matrix_const_submatrix (mat_all_g, 0, i*dc_size, dc_size, dc_size); - gsl_matrix_const_view mat_e=gsl_matrix_const_submatrix (mat_all_e, 0, i*dc_size, dc_size, dc_size); - - gsl_matrix_view Qimat_g=gsl_matrix_submatrix (Qimat_all_g, 0, i*dc_size, dc_size, dc_size); - gsl_matrix_view Qimat_e=gsl_matrix_submatrix (Qimat_all_e, 0, i*dc_size, dc_size, dc_size); - - gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1.0, Qi, &mat_g.matrix, 0.0, &Qimat_g.matrix); - gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1.0, Qi, &mat_e.matrix, 0.0, &Qimat_e.matrix); + gsl_matrix_const_view mat_g = + gsl_matrix_const_submatrix (mat_all_g, 0, i*dc_size, + dc_size, dc_size); + gsl_matrix_const_view mat_e = + gsl_matrix_const_submatrix (mat_all_e, 0, i*dc_size, + dc_size, dc_size); + + gsl_matrix_view Qimat_g = + gsl_matrix_submatrix (Qimat_all_g, 0, i*dc_size, dc_size, + dc_size); + gsl_matrix_view Qimat_e = + gsl_matrix_submatrix (Qimat_all_e, 0, i*dc_size, dc_size, + dc_size); + + gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1.0, Qi, + &mat_g.matrix, 0.0, &Qimat_g.matrix); + gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1.0, Qi, + &mat_e.matrix, 0.0, &Qimat_e.matrix); } return; } - - -//calculate yPDPy -//yPDPy=y(Hi-HixQixHi)D(Hi-HixQixHi)y -//=ytHiDHiy -//-(yHix)Qi(xHiDHiy)-(yHiDHix)Qi(xHiy) -//+(yHix)Qi(xHiDHix)Qi(xtHiy) -void Calc_yPDPy (const gsl_vector *eval, const gsl_matrix *Hiy, const gsl_vector *QixHiy, const gsl_matrix *xHiDHiy_all_g, const gsl_matrix *xHiDHiy_all_e, const gsl_matrix *xHiDHixQixHiy_all_g, const gsl_matrix *xHiDHixQixHiy_all_e, const size_t i, const size_t j, double &yPDPy_g, double &yPDPy_e) -{ +// Calculate yPDPy +// yPDPy = y(Hi-HixQixHi)D(Hi-HixQixHi)y +// = ytHiDHiy - (yHix)Qi(xHiDHiy) - (yHiDHix)Qi(xHiy) +// + (yHix)Qi(xHiDHix)Qi(xtHiy) +void Calc_yPDPy (const gsl_vector *eval, const gsl_matrix *Hiy, + const gsl_vector *QixHiy, const gsl_matrix *xHiDHiy_all_g, + const gsl_matrix *xHiDHiy_all_e, + const gsl_matrix *xHiDHixQixHiy_all_g, + const gsl_matrix *xHiDHixQixHiy_all_e, + const size_t i, const size_t j, + double &yPDPy_g, double &yPDPy_e) { size_t d_size=Hiy->size1; size_t v=GetIndex(i, j, d_size); double d; - //first part: ytHiDHiy + // First part: ytHiDHiy. Calc_yHiDHiy (eval, Hiy, i, j, yPDPy_g, yPDPy_e); - //second and third parts: -(yHix)Qi(xHiDHiy)-(yHiDHix)Qi(xHiy) - gsl_vector_const_view xHiDHiy_g=gsl_matrix_const_column (xHiDHiy_all_g, v); - gsl_vector_const_view xHiDHiy_e=gsl_matrix_const_column (xHiDHiy_all_e, v); + // Second and third parts: -(yHix)Qi(xHiDHiy)-(yHiDHix)Qi(xHiy) + gsl_vector_const_view xHiDHiy_g = + gsl_matrix_const_column (xHiDHiy_all_g, v); + gsl_vector_const_view xHiDHiy_e = + gsl_matrix_const_column (xHiDHiy_all_e, v); gsl_blas_ddot(QixHiy, &xHiDHiy_g.vector, &d); yPDPy_g-=d*2.0; gsl_blas_ddot(QixHiy, &xHiDHiy_e.vector, &d); yPDPy_e-=d*2.0; - //fourth part: +(yHix)Qi(xHiDHix)Qi(xHiy) - gsl_vector_const_view xHiDHixQixHiy_g=gsl_matrix_const_column (xHiDHixQixHiy_all_g, v); - gsl_vector_const_view xHiDHixQixHiy_e=gsl_matrix_const_column (xHiDHixQixHiy_all_e, v); + // Fourth part: +(yHix)Qi(xHiDHix)Qi(xHiy). + gsl_vector_const_view xHiDHixQixHiy_g = + gsl_matrix_const_column (xHiDHixQixHiy_all_g, v); + gsl_vector_const_view xHiDHixQixHiy_e = + gsl_matrix_const_column (xHiDHixQixHiy_all_e, v); gsl_blas_ddot(QixHiy, &xHiDHixQixHiy_g.vector, &d); yPDPy_g+=d; @@ -1870,15 +1904,33 @@ void Calc_yPDPy (const gsl_vector *eval, const gsl_matrix *Hiy, const gsl_vector return; } -//calculate yPDPDPy=y(Hi-HixQixHi)D(Hi-HixQixHi)D(Hi-HixQixHi)y -//yPDPDPy=yHiDHiDHiy -//-(yHix)Qi(xHiDHiDHiy)-(yHiDHiDHix)Qi(xHiy) -//-(yHiDHix)Qi(xHiDHiy) -//+(yHix)Qi(xHiDHix)Qi(xHiDHiy)+(yHiDHix)Qi(xHiDHix)Qi(xHiy) -//+(yHix)Qi(xHiDHiDHix)Qi(xHiy) -//-(yHix)Qi(xHiDHix)Qi(xHiDHix)Qi(xHiy) -void Calc_yPDPDPy (const gsl_vector *eval, const gsl_matrix *Hi, const gsl_matrix *xHi, const gsl_matrix *Hiy, const gsl_vector *QixHiy, const gsl_matrix *xHiDHiy_all_g, const gsl_matrix *xHiDHiy_all_e, const gsl_matrix *QixHiDHiy_all_g, const gsl_matrix *QixHiDHiy_all_e, const gsl_matrix *xHiDHixQixHiy_all_g, const gsl_matrix *xHiDHixQixHiy_all_e, const gsl_matrix *QixHiDHixQixHiy_all_g, const gsl_matrix *QixHiDHixQixHiy_all_e, const gsl_matrix *xHiDHiDHiy_all_gg, const gsl_matrix *xHiDHiDHiy_all_ee, const gsl_matrix *xHiDHiDHiy_all_ge, const gsl_matrix *xHiDHiDHix_all_gg, const gsl_matrix *xHiDHiDHix_all_ee, const gsl_matrix *xHiDHiDHix_all_ge, const size_t i1, const size_t j1, const size_t i2, const size_t j2, double &yPDPDPy_gg, double &yPDPDPy_ee, double &yPDPDPy_ge) -{ +// calculate yPDPDPy = y(Hi-HixQixHi)D(Hi-HixQixHi)D(Hi-HixQixHi)y +// yPDPDPy = yHiDHiDHiy +// - (yHix)Qi(xHiDHiDHiy)-(yHiDHiDHix)Qi(xHiy) +// - (yHiDHix)Qi(xHiDHiy) +// + (yHix)Qi(xHiDHix)Qi(xHiDHiy) +// + (yHiDHix)Qi(xHiDHix)Qi(xHiy) +// + (yHix)Qi(xHiDHiDHix)Qi(xHiy) +// - (yHix)Qi(xHiDHix)Qi(xHiDHix)Qi(xHiy) +void Calc_yPDPDPy (const gsl_vector *eval, const gsl_matrix *Hi, + const gsl_matrix *xHi, const gsl_matrix *Hiy, + const gsl_vector *QixHiy, const gsl_matrix *xHiDHiy_all_g, + const gsl_matrix *xHiDHiy_all_e, + const gsl_matrix *QixHiDHiy_all_g, + const gsl_matrix *QixHiDHiy_all_e, + const gsl_matrix *xHiDHixQixHiy_all_g, + const gsl_matrix *xHiDHixQixHiy_all_e, + const gsl_matrix *QixHiDHixQixHiy_all_g, + const gsl_matrix *QixHiDHixQixHiy_all_e, + const gsl_matrix *xHiDHiDHiy_all_gg, + const gsl_matrix *xHiDHiDHiy_all_ee, + const gsl_matrix *xHiDHiDHiy_all_ge, + const gsl_matrix *xHiDHiDHix_all_gg, + const gsl_matrix *xHiDHiDHix_all_ee, + const gsl_matrix *xHiDHiDHix_all_ge, + const size_t i1, const size_t j1, const size_t i2, + const size_t j2, double &yPDPDPy_gg, double &yPDPDPy_ee, + double &yPDPDPy_ge) { size_t d_size=Hi->size1, dc_size=xHi->size1; size_t v1=GetIndex(i1, j1, d_size), v2=GetIndex(i2, j2, d_size); size_t v_size=d_size*(d_size+1)/2; @@ -1887,17 +1939,25 @@ void Calc_yPDPDPy (const gsl_vector *eval, const gsl_matrix *Hi, const gsl_matri gsl_vector *xHiDHiDHixQixHiy=gsl_vector_alloc (dc_size); - //first part: yHiDHiDHiy - Calc_yHiDHiDHiy (eval, Hi, Hiy, i1, j1, i2, j2, yPDPDPy_gg, yPDPDPy_ee, yPDPDPy_ge); - - //second and third parts: -(yHix)Qi(xHiDHiDHiy)-(yHiDHiDHix)Qi(xHiy) - gsl_vector_const_view xHiDHiDHiy_gg1=gsl_matrix_const_column (xHiDHiDHiy_all_gg, v1*v_size+v2); - gsl_vector_const_view xHiDHiDHiy_ee1=gsl_matrix_const_column (xHiDHiDHiy_all_ee, v1*v_size+v2); - gsl_vector_const_view xHiDHiDHiy_ge1=gsl_matrix_const_column (xHiDHiDHiy_all_ge, v1*v_size+v2); - - gsl_vector_const_view xHiDHiDHiy_gg2=gsl_matrix_const_column (xHiDHiDHiy_all_gg, v2*v_size+v1); - gsl_vector_const_view xHiDHiDHiy_ee2=gsl_matrix_const_column (xHiDHiDHiy_all_ee, v2*v_size+v1); - gsl_vector_const_view xHiDHiDHiy_ge2=gsl_matrix_const_column (xHiDHiDHiy_all_ge, v2*v_size+v1); + // First part: yHiDHiDHiy. + Calc_yHiDHiDHiy (eval, Hi, Hiy, i1, j1, i2, j2, yPDPDPy_gg, + yPDPDPy_ee, yPDPDPy_ge); + + // Second and third parts: + // -(yHix)Qi(xHiDHiDHiy) - (yHiDHiDHix)Qi(xHiy). + gsl_vector_const_view xHiDHiDHiy_gg1 = + gsl_matrix_const_column (xHiDHiDHiy_all_gg, v1*v_size+v2); + gsl_vector_const_view xHiDHiDHiy_ee1 = + gsl_matrix_const_column (xHiDHiDHiy_all_ee, v1*v_size+v2); + gsl_vector_const_view xHiDHiDHiy_ge1 = + gsl_matrix_const_column (xHiDHiDHiy_all_ge, v1*v_size+v2); + + gsl_vector_const_view xHiDHiDHiy_gg2 = + gsl_matrix_const_column (xHiDHiDHiy_all_gg, v2*v_size+v1); + gsl_vector_const_view xHiDHiDHiy_ee2 = + gsl_matrix_const_column (xHiDHiDHiy_all_ee, v2*v_size+v1); + gsl_vector_const_view xHiDHiDHiy_ge2 = + gsl_matrix_const_column (xHiDHiDHiy_all_ge, v2*v_size+v1); gsl_blas_ddot(QixHiy, &xHiDHiDHiy_gg1.vector, &d); yPDPDPy_gg-=d; @@ -1913,11 +1973,15 @@ void Calc_yPDPDPy (const gsl_vector *eval, const gsl_matrix *Hi, const gsl_matri gsl_blas_ddot(QixHiy, &xHiDHiDHiy_ge2.vector, &d); yPDPDPy_ge-=d; - //fourth part: -(yHiDHix)Qi(xHiDHiy) - gsl_vector_const_view xHiDHiy_g1=gsl_matrix_const_column (xHiDHiy_all_g, v1); - gsl_vector_const_view xHiDHiy_e1=gsl_matrix_const_column (xHiDHiy_all_e, v1); - gsl_vector_const_view QixHiDHiy_g2=gsl_matrix_const_column (QixHiDHiy_all_g, v2); - gsl_vector_const_view QixHiDHiy_e2=gsl_matrix_const_column (QixHiDHiy_all_e, v2); + // Fourth part: - (yHiDHix)Qi(xHiDHiy). + gsl_vector_const_view xHiDHiy_g1 = + gsl_matrix_const_column (xHiDHiy_all_g, v1); + gsl_vector_const_view xHiDHiy_e1 = + gsl_matrix_const_column (xHiDHiy_all_e, v1); + gsl_vector_const_view QixHiDHiy_g2 = + gsl_matrix_const_column (QixHiDHiy_all_g, v2); + gsl_vector_const_view QixHiDHiy_e2 = + gsl_matrix_const_column (QixHiDHiy_all_e, v2); gsl_blas_ddot(&xHiDHiy_g1.vector, &QixHiDHiy_g2.vector, &d); yPDPDPy_gg-=d; @@ -1926,14 +1990,22 @@ void Calc_yPDPDPy (const gsl_vector *eval, const gsl_matrix *Hi, const gsl_matri gsl_blas_ddot(&xHiDHiy_g1.vector, &QixHiDHiy_e2.vector, &d); yPDPDPy_ge-=d; - //fifth and sixth parts: +(yHix)Qi(xHiDHix)Qi(xHiDHiy)+(yHiDHix)Qi(xHiDHix)Qi(xHiy) - gsl_vector_const_view QixHiDHiy_g1=gsl_matrix_const_column (QixHiDHiy_all_g, v1); - gsl_vector_const_view QixHiDHiy_e1=gsl_matrix_const_column (QixHiDHiy_all_e, v1); - - gsl_vector_const_view xHiDHixQixHiy_g1=gsl_matrix_const_column (xHiDHixQixHiy_all_g, v1); - gsl_vector_const_view xHiDHixQixHiy_e1=gsl_matrix_const_column (xHiDHixQixHiy_all_e, v1); - gsl_vector_const_view xHiDHixQixHiy_g2=gsl_matrix_const_column (xHiDHixQixHiy_all_g, v2); - gsl_vector_const_view xHiDHixQixHiy_e2=gsl_matrix_const_column (xHiDHixQixHiy_all_e, v2); + // Fifth and sixth parts: + // + (yHix)Qi(xHiDHix)Qi(xHiDHiy) + + // (yHiDHix)Qi(xHiDHix)Qi(xHiy) + gsl_vector_const_view QixHiDHiy_g1 = + gsl_matrix_const_column (QixHiDHiy_all_g, v1); + gsl_vector_const_view QixHiDHiy_e1 = + gsl_matrix_const_column (QixHiDHiy_all_e, v1); + + gsl_vector_const_view xHiDHixQixHiy_g1 = + gsl_matrix_const_column (xHiDHixQixHiy_all_g, v1); + gsl_vector_const_view xHiDHixQixHiy_e1 = + gsl_matrix_const_column (xHiDHixQixHiy_all_e, v1); + gsl_vector_const_view xHiDHixQixHiy_g2 = + gsl_matrix_const_column (xHiDHixQixHiy_all_g, v2); + gsl_vector_const_view xHiDHixQixHiy_e2 = + gsl_matrix_const_column (xHiDHixQixHiy_all_e, v2); gsl_blas_ddot(&xHiDHixQixHiy_g1.vector, &QixHiDHiy_g2.vector, &d); yPDPDPy_gg+=d; @@ -1950,50 +2022,70 @@ void Calc_yPDPDPy (const gsl_vector *eval, const gsl_matrix *Hi, const gsl_matri gsl_blas_ddot(&xHiDHixQixHiy_e2.vector, &QixHiDHiy_g1.vector, &d); yPDPDPy_ge+=d; - //seventh part: +(yHix)Qi(xHiDHiDHix)Qi(xHiy) - gsl_matrix_const_view xHiDHiDHix_gg=gsl_matrix_const_submatrix (xHiDHiDHix_all_gg, 0, (v1*v_size+v2)*dc_size, dc_size, dc_size); - gsl_matrix_const_view xHiDHiDHix_ee=gsl_matrix_const_submatrix (xHiDHiDHix_all_ee, 0, (v1*v_size+v2)*dc_size, dc_size, dc_size); - gsl_matrix_const_view xHiDHiDHix_ge=gsl_matrix_const_submatrix (xHiDHiDHix_all_ge, 0, (v1*v_size+v2)*dc_size, dc_size, dc_size); - - gsl_blas_dgemv (CblasNoTrans, 1.0, &xHiDHiDHix_gg.matrix, QixHiy, 0.0, xHiDHiDHixQixHiy); + // Seventh part: + (yHix)Qi(xHiDHiDHix)Qi(xHiy) + gsl_matrix_const_view xHiDHiDHix_gg = + gsl_matrix_const_submatrix (xHiDHiDHix_all_gg, 0, + (v1*v_size+v2)*dc_size, + dc_size, dc_size); + gsl_matrix_const_view xHiDHiDHix_ee = + gsl_matrix_const_submatrix (xHiDHiDHix_all_ee, 0, + (v1*v_size+v2)*dc_size, + dc_size, dc_size); + gsl_matrix_const_view xHiDHiDHix_ge = + gsl_matrix_const_submatrix (xHiDHiDHix_all_ge, 0, + (v1*v_size+v2)*dc_size, + dc_size, dc_size); + + gsl_blas_dgemv (CblasNoTrans, 1.0, &xHiDHiDHix_gg.matrix, + QixHiy, 0.0, xHiDHiDHixQixHiy); gsl_blas_ddot(xHiDHiDHixQixHiy, QixHiy, &d); yPDPDPy_gg+=d; - gsl_blas_dgemv (CblasNoTrans, 1.0, &xHiDHiDHix_ee.matrix, QixHiy, 0.0, xHiDHiDHixQixHiy); + gsl_blas_dgemv (CblasNoTrans, 1.0, &xHiDHiDHix_ee.matrix, + QixHiy, 0.0, xHiDHiDHixQixHiy); gsl_blas_ddot(xHiDHiDHixQixHiy, QixHiy, &d); yPDPDPy_ee+=d; - gsl_blas_dgemv (CblasNoTrans, 1.0, &xHiDHiDHix_ge.matrix, QixHiy, 0.0, xHiDHiDHixQixHiy); + gsl_blas_dgemv (CblasNoTrans, 1.0, &xHiDHiDHix_ge.matrix, + QixHiy, 0.0, xHiDHiDHixQixHiy); gsl_blas_ddot(xHiDHiDHixQixHiy, QixHiy, &d); yPDPDPy_ge+=d; - //eighth part: -(yHix)Qi(xHiDHix)Qi(xHiDHix)Qi(xHiy) - gsl_vector_const_view QixHiDHixQixHiy_g1=gsl_matrix_const_column (QixHiDHixQixHiy_all_g, v1); - gsl_vector_const_view QixHiDHixQixHiy_e1=gsl_matrix_const_column (QixHiDHixQixHiy_all_e, v1); + // Eighth part: - (yHix)Qi(xHiDHix)Qi(xHiDHix)Qi(xHiy). + gsl_vector_const_view QixHiDHixQixHiy_g1 = + gsl_matrix_const_column (QixHiDHixQixHiy_all_g, v1); + gsl_vector_const_view QixHiDHixQixHiy_e1 = + gsl_matrix_const_column (QixHiDHixQixHiy_all_e, v1); - gsl_blas_ddot(&QixHiDHixQixHiy_g1.vector, &xHiDHixQixHiy_g2.vector, &d); + gsl_blas_ddot(&QixHiDHixQixHiy_g1.vector,&xHiDHixQixHiy_g2.vector,&d); yPDPDPy_gg-=d; - gsl_blas_ddot(&QixHiDHixQixHiy_e1.vector, &xHiDHixQixHiy_e2.vector, &d); + gsl_blas_ddot(&QixHiDHixQixHiy_e1.vector,&xHiDHixQixHiy_e2.vector,&d); yPDPDPy_ee-=d; - gsl_blas_ddot(&QixHiDHixQixHiy_g1.vector, &xHiDHixQixHiy_e2.vector, &d); + gsl_blas_ddot(&QixHiDHixQixHiy_g1.vector,&xHiDHixQixHiy_e2.vector,&d); yPDPDPy_ge-=d; - //free memory + // Free memory. gsl_vector_free(xHiDHiDHixQixHiy); return; } - -//calculate Edgeworth correctation factors for small samples -//notation and method follows Thomas J. Rothenberg, Econometirca 1984; 52 (4) -//M=xHiDHix -void CalcCRT (const gsl_matrix *Hessian_inv, const gsl_matrix *Qi, const gsl_matrix *QixHiDHix_all_g, const gsl_matrix *QixHiDHix_all_e, const gsl_matrix *xHiDHiDHix_all_gg, const gsl_matrix *xHiDHiDHix_all_ee, const gsl_matrix *xHiDHiDHix_all_ge, const size_t d_size, double &crt_a, double &crt_b, double &crt_c) -{ +// Calculate Edgeworth correctation factors for small samples notation +// and method follows Thomas J. Rothenberg, Econometirca 1984; 52 (4) +// M=xHiDHix +void CalcCRT (const gsl_matrix *Hessian_inv, const gsl_matrix *Qi, + const gsl_matrix *QixHiDHix_all_g, + const gsl_matrix *QixHiDHix_all_e, + const gsl_matrix *xHiDHiDHix_all_gg, + const gsl_matrix *xHiDHiDHix_all_ee, + const gsl_matrix *xHiDHiDHix_all_ge, + const size_t d_size, double &crt_a, + double &crt_b, double &crt_c) { crt_a=0.0; crt_b=0.0; crt_c=0.0; size_t dc_size=Qi->size1, v_size=Hessian_inv->size1/2; size_t c_size=dc_size/d_size; double h_gg, h_ge, h_ee, d, B=0.0, C=0.0, D=0.0; - double trCg1, trCe1, trCg2, trCe2, trB_gg, trB_ge, trB_ee, trCC_gg, trCC_ge, trCC_ee, trD_gg=0.0, trD_ge=0.0, trD_ee=0.0; + double trCg1, trCe1, trCg2, trCe2, trB_gg, trB_ge, trB_ee; + double trCC_gg, trCC_ge, trCC_ee, trD_gg=0.0, trD_ge=0.0, trD_ee=0.0; gsl_matrix *QiMQi_g1=gsl_matrix_alloc (dc_size, dc_size); gsl_matrix *QiMQi_e1=gsl_matrix_alloc (dc_size, dc_size); @@ -2018,10 +2110,12 @@ void CalcCRT (const gsl_matrix *Hessian_inv, const gsl_matrix *Qi, const gsl_mat gsl_matrix *M_dd=gsl_matrix_alloc (d_size, d_size); gsl_matrix *M_dcdc=gsl_matrix_alloc (dc_size, dc_size); - //invert Qi_sub to Qi_si + // Invert Qi_sub to Qi_si. gsl_matrix *Qi_sub=gsl_matrix_alloc (d_size, d_size); - gsl_matrix_const_view Qi_s=gsl_matrix_const_submatrix (Qi, (c_size-1)*d_size, (c_size-1)*d_size, d_size, d_size); + gsl_matrix_const_view Qi_s = + gsl_matrix_const_submatrix (Qi, (c_size-1)*d_size, + (c_size-1)*d_size, d_size, d_size); int sig; gsl_permutation * pmt=gsl_permutation_alloc (d_size); @@ -2033,191 +2127,240 @@ void CalcCRT (const gsl_matrix *Hessian_inv, const gsl_matrix *Qi, const gsl_mat gsl_permutation_free(pmt); gsl_matrix_free(Qi_sub); - //calculate correctation factors + // Calculate correction factors. for (size_t v1=0; v1<v_size; v1++) { - //calculate Qi(xHiDHix)Qi, and subpart of it - gsl_matrix_const_view QiM_g1=gsl_matrix_const_submatrix (QixHiDHix_all_g, 0, v1*dc_size, dc_size, dc_size); - gsl_matrix_const_view QiM_e1=gsl_matrix_const_submatrix (QixHiDHix_all_e, 0, v1*dc_size, dc_size, dc_size); - - gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiM_g1.matrix, Qi, 0.0, QiMQi_g1); - gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiM_e1.matrix, Qi, 0.0, QiMQi_e1); - - gsl_matrix_view QiMQi_g1_s=gsl_matrix_submatrix (QiMQi_g1, (c_size-1)*d_size, (c_size-1)*d_size, d_size, d_size); - gsl_matrix_view QiMQi_e1_s=gsl_matrix_submatrix (QiMQi_e1, (c_size-1)*d_size, (c_size-1)*d_size, d_size, d_size); - - /* - for (size_t i=0; i<d_size; i++) { - for (size_t j=0; j<d_size; j++) { - cout<<setprecision(6)<<gsl_matrix_get(&QiMQi_g1_s.matrix, i, j)<<"\t"; - } - cout<<endl; - } -*/ - //calculate trCg1 and trCe1 - gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiMQi_g1_s.matrix, Qi_si, 0.0, QiMQisQisi_g1); - trCg1=0.0; - for (size_t k=0; k<d_size; k++) { - trCg1-=gsl_matrix_get (QiMQisQisi_g1, k, k); - } - - gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiMQi_e1_s.matrix, Qi_si, 0.0, QiMQisQisi_e1); - trCe1=0.0; - for (size_t k=0; k<d_size; k++) { - trCe1-=gsl_matrix_get (QiMQisQisi_e1, k, k); - } - /* - cout<<v1<<endl; - cout<<"trCg1 = "<<trCg1<<", trCe1 = "<<trCe1<<endl; - */ - for (size_t v2=0; v2<v_size; v2++) { - if (v2<v1) {continue;} - - //calculate Qi(xHiDHix)Qi, and subpart of it - gsl_matrix_const_view QiM_g2=gsl_matrix_const_submatrix (QixHiDHix_all_g, 0, v2*dc_size, dc_size, dc_size); - gsl_matrix_const_view QiM_e2=gsl_matrix_const_submatrix (QixHiDHix_all_e, 0, v2*dc_size, dc_size, dc_size); - - gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiM_g2.matrix, Qi, 0.0, QiMQi_g2); - gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiM_e2.matrix, Qi, 0.0, QiMQi_e2); - - gsl_matrix_view QiMQi_g2_s=gsl_matrix_submatrix (QiMQi_g2, (c_size-1)*d_size, (c_size-1)*d_size, d_size, d_size); - gsl_matrix_view QiMQi_e2_s=gsl_matrix_submatrix (QiMQi_e2, (c_size-1)*d_size, (c_size-1)*d_size, d_size, d_size); - - //calculate trCg2 and trCe2 - gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiMQi_g2_s.matrix, Qi_si, 0.0, QiMQisQisi_g2); - trCg2=0.0; - for (size_t k=0; k<d_size; k++) { - trCg2-=gsl_matrix_get (QiMQisQisi_g2, k, k); - } - - gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiMQi_e2_s.matrix, Qi_si, 0.0, QiMQisQisi_e2); - trCe2=0.0; - for (size_t k=0; k<d_size; k++) { - trCe2-=gsl_matrix_get (QiMQisQisi_e2, k, k); - } - - //calculate trCC_gg, trCC_ge, trCC_ee - gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, QiMQisQisi_g1, QiMQisQisi_g2, 0.0, M_dd); - trCC_gg=0.0; - for (size_t k=0; k<d_size; k++) { - trCC_gg+=gsl_matrix_get (M_dd, k, k); - } - - gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, QiMQisQisi_g1, QiMQisQisi_e2, 0.0, M_dd); - gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, QiMQisQisi_e1, QiMQisQisi_g2, 1.0, M_dd); - trCC_ge=0.0; - for (size_t k=0; k<d_size; k++) { - trCC_ge+=gsl_matrix_get (M_dd, k, k); - } - - gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, QiMQisQisi_e1, QiMQisQisi_e2, 0.0, M_dd); - trCC_ee=0.0; - for (size_t k=0; k<d_size; k++) { - trCC_ee+=gsl_matrix_get (M_dd, k, k); - } - - //calculate Qi(xHiDHix)Qi(xHiDHix)Qi, and subpart of it - gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiM_g1.matrix, QiMQi_g2, 0.0, QiMQiMQi_gg); - gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiM_g1.matrix, QiMQi_e2, 0.0, QiMQiMQi_ge); - gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiM_e1.matrix, QiMQi_g2, 1.0, QiMQiMQi_ge); - gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiM_e1.matrix, QiMQi_e2, 0.0, QiMQiMQi_ee); - - gsl_matrix_view QiMQiMQi_gg_s=gsl_matrix_submatrix (QiMQiMQi_gg, (c_size-1)*d_size, (c_size-1)*d_size, d_size, d_size); - gsl_matrix_view QiMQiMQi_ge_s=gsl_matrix_submatrix (QiMQiMQi_ge, (c_size-1)*d_size, (c_size-1)*d_size, d_size, d_size); - gsl_matrix_view QiMQiMQi_ee_s=gsl_matrix_submatrix (QiMQiMQi_ee, (c_size-1)*d_size, (c_size-1)*d_size, d_size, d_size); - - //and part of trB_gg, trB_ge, trB_ee - gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiMQiMQi_gg_s.matrix, Qi_si, 0.0, M_dd); - trB_gg=0.0; - for (size_t k=0; k<d_size; k++) { - d=gsl_matrix_get (M_dd, k, k); - trB_gg-=d; - } - - gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiMQiMQi_ge_s.matrix, Qi_si, 0.0, M_dd); - trB_ge=0.0; - for (size_t k=0; k<d_size; k++) { - d=gsl_matrix_get (M_dd, k, k); - trB_ge-=d; - } - - gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiMQiMQi_ee_s.matrix, Qi_si, 0.0, M_dd); - trB_ee=0.0; - for (size_t k=0; k<d_size; k++) { - d=gsl_matrix_get (M_dd, k, k); - trB_ee-=d; - } - - //calculate Qi(xHiDHiDHix)Qi, and subpart of it - gsl_matrix_const_view MM_gg=gsl_matrix_const_submatrix (xHiDHiDHix_all_gg, 0, (v1*v_size+v2)*dc_size, dc_size, dc_size); - gsl_matrix_const_view MM_ge=gsl_matrix_const_submatrix (xHiDHiDHix_all_ge, 0, (v1*v_size+v2)*dc_size, dc_size, dc_size); - gsl_matrix_const_view MM_ee=gsl_matrix_const_submatrix (xHiDHiDHix_all_ee, 0, (v1*v_size+v2)*dc_size, dc_size, dc_size); - - gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Qi, &MM_gg.matrix, 0.0, M_dcdc); - gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, M_dcdc, Qi, 0.0, QiMMQi_gg); - gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Qi, &MM_ge.matrix, 0.0, M_dcdc); - gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, M_dcdc, Qi, 0.0, QiMMQi_ge); - gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Qi, &MM_ee.matrix, 0.0, M_dcdc); - gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, M_dcdc, Qi, 0.0, QiMMQi_ee); - - gsl_matrix_view QiMMQi_gg_s=gsl_matrix_submatrix (QiMMQi_gg, (c_size-1)*d_size, (c_size-1)*d_size, d_size, d_size); - gsl_matrix_view QiMMQi_ge_s=gsl_matrix_submatrix (QiMMQi_ge, (c_size-1)*d_size, (c_size-1)*d_size, d_size, d_size); - gsl_matrix_view QiMMQi_ee_s=gsl_matrix_submatrix (QiMMQi_ee, (c_size-1)*d_size, (c_size-1)*d_size, d_size, d_size); - - //calculate the other part of trB_gg, trB_ge, trB_ee - gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiMMQi_gg_s.matrix, Qi_si, 0.0, M_dd); - for (size_t k=0; k<d_size; k++) { - trB_gg+=gsl_matrix_get (M_dd, k, k); - } - gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiMMQi_ge_s.matrix, Qi_si, 0.0, M_dd); - for (size_t k=0; k<d_size; k++) { - trB_ge+=2.0*gsl_matrix_get (M_dd, k, k); - } - gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiMMQi_ee_s.matrix, Qi_si, 0.0, M_dd); - for (size_t k=0; k<d_size; k++) { - trB_ee+=gsl_matrix_get (M_dd, k, k); - } - - - //calculate trD_gg, trD_ge, trD_ee - trD_gg=2.0*trB_gg; - trD_ge=2.0*trB_ge; - trD_ee=2.0*trB_ee; - - //calculate B, C and D - h_gg=-1.0*gsl_matrix_get (Hessian_inv, v1, v2); - h_ge=-1.0*gsl_matrix_get (Hessian_inv, v1, v2+v_size); - h_ee=-1.0*gsl_matrix_get (Hessian_inv, v1+v_size, v2+v_size); - - B+=h_gg*trB_gg+h_ge*trB_ge+h_ee*trB_ee; - C+=h_gg*(trCC_gg+0.5*trCg1*trCg2)+h_ge*(trCC_ge+0.5*trCg1*trCe2+0.5*trCe1*trCg2)+h_ee*(trCC_ee+0.5*trCe1*trCe2); - D+=h_gg*(trCC_gg+0.5*trD_gg)+h_ge*(trCC_ge+0.5*trD_ge)+h_ee*(trCC_ee+0.5*trD_ee); + + // Calculate Qi(xHiDHix)Qi, and subpart of it. + gsl_matrix_const_view QiM_g1 = + gsl_matrix_const_submatrix (QixHiDHix_all_g, 0, v1*dc_size, + dc_size, dc_size); + gsl_matrix_const_view QiM_e1 = + gsl_matrix_const_submatrix (QixHiDHix_all_e, 0, v1*dc_size, + dc_size, dc_size); + + gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiM_g1.matrix, + Qi, 0.0, QiMQi_g1); + gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiM_e1.matrix, + Qi, 0.0, QiMQi_e1); + + gsl_matrix_view QiMQi_g1_s = + gsl_matrix_submatrix (QiMQi_g1, (c_size-1)*d_size, + (c_size-1)*d_size, d_size, d_size); + gsl_matrix_view QiMQi_e1_s = + gsl_matrix_submatrix (QiMQi_e1, (c_size-1)*d_size, + (c_size-1)*d_size, d_size, d_size); + + // Calculate trCg1 and trCe1. + gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiMQi_g1_s.matrix, + Qi_si, 0.0, QiMQisQisi_g1); + trCg1=0.0; + for (size_t k=0; k<d_size; k++) { + trCg1-=gsl_matrix_get (QiMQisQisi_g1, k, k); + } - if (v1!=v2) { - B+=h_gg*trB_gg+h_ge*trB_ge+h_ee*trB_ee; - C+=h_gg*(trCC_gg+0.5*trCg1*trCg2)+h_ge*(trCC_ge+0.5*trCg1*trCe2+0.5*trCe1*trCg2)+h_ee*(trCC_ee+0.5*trCe1*trCe2); - D+=h_gg*(trCC_gg+0.5*trD_gg)+h_ge*(trCC_ge+0.5*trD_ge)+h_ee*(trCC_ee+0.5*trD_ee); - } + gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiMQi_e1_s.matrix, + Qi_si, 0.0, QiMQisQisi_e1); + trCe1=0.0; + for (size_t k=0; k<d_size; k++) { + trCe1-=gsl_matrix_get (QiMQisQisi_e1, k, k); + } - /* - cout<<v1<<"\t"<<v2<<endl; - cout<<h_gg<<"\t"<<h_ge<<"\t"<<h_ee<<endl; - cout<<trB_gg<<"\t"<<trB_ge<<"\t"<<trB_ee<<endl; - cout<<trCg1<<"\t"<<trCe1<<"\t"<<trCg2<<"\t"<<trCe2<<endl; - cout<<trCC_gg<<"\t"<<trCC_ge<<"\t"<<trCC_ee<<endl; - cout<<trD_gg<<"\t"<<trD_ge<<"\t"<<trD_ee<<endl; - */ - } + for (size_t v2=0; v2<v_size; v2++) { + if (v2<v1) {continue;} + + // Calculate Qi(xHiDHix)Qi, and subpart of it. + gsl_matrix_const_view QiM_g2 = + gsl_matrix_const_submatrix (QixHiDHix_all_g, 0, v2*dc_size, + dc_size, dc_size); + gsl_matrix_const_view QiM_e2 = + gsl_matrix_const_submatrix (QixHiDHix_all_e, 0, v2*dc_size, + dc_size, dc_size); + + gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiM_g2.matrix, + Qi, 0.0, QiMQi_g2); + gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiM_e2.matrix, + Qi, 0.0, QiMQi_e2); + + gsl_matrix_view QiMQi_g2_s = + gsl_matrix_submatrix (QiMQi_g2, (c_size-1)*d_size, + (c_size-1)*d_size, d_size, d_size); + gsl_matrix_view QiMQi_e2_s = + gsl_matrix_submatrix (QiMQi_e2, (c_size-1)*d_size, + (c_size-1)*d_size, d_size, d_size); + + // Calculate trCg2 and trCe2. + gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, + &QiMQi_g2_s.matrix, Qi_si, 0.0, QiMQisQisi_g2); + trCg2=0.0; + for (size_t k=0; k<d_size; k++) { + trCg2-=gsl_matrix_get (QiMQisQisi_g2, k, k); + } + + gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, + &QiMQi_e2_s.matrix, Qi_si, 0.0, QiMQisQisi_e2); + trCe2=0.0; + for (size_t k=0; k<d_size; k++) { + trCe2-=gsl_matrix_get (QiMQisQisi_e2, k, k); + } + + // Calculate trCC_gg, trCC_ge, trCC_ee. + gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, + QiMQisQisi_g1, QiMQisQisi_g2, 0.0, M_dd); + trCC_gg=0.0; + for (size_t k=0; k<d_size; k++) { + trCC_gg+=gsl_matrix_get (M_dd, k, k); + } + + gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, QiMQisQisi_g1, + QiMQisQisi_e2, 0.0, M_dd); + gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, QiMQisQisi_e1, + QiMQisQisi_g2, 1.0, M_dd); + trCC_ge=0.0; + for (size_t k=0; k<d_size; k++) { + trCC_ge+=gsl_matrix_get (M_dd, k, k); + } + + gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, QiMQisQisi_e1, + QiMQisQisi_e2, 0.0, M_dd); + trCC_ee=0.0; + for (size_t k=0; k<d_size; k++) { + trCC_ee+=gsl_matrix_get (M_dd, k, k); + } + + // Calculate Qi(xHiDHix)Qi(xHiDHix)Qi, and subpart of it. + gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiM_g1.matrix, + QiMQi_g2, 0.0, QiMQiMQi_gg); + gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiM_g1.matrix, + QiMQi_e2, 0.0, QiMQiMQi_ge); + gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiM_e1.matrix, + QiMQi_g2, 1.0, QiMQiMQi_ge); + gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiM_e1.matrix, + QiMQi_e2, 0.0, QiMQiMQi_ee); + + gsl_matrix_view QiMQiMQi_gg_s = + gsl_matrix_submatrix (QiMQiMQi_gg, (c_size-1)*d_size, + (c_size-1)*d_size, d_size, d_size); + gsl_matrix_view QiMQiMQi_ge_s = + gsl_matrix_submatrix (QiMQiMQi_ge, (c_size-1)*d_size, + (c_size-1)*d_size, d_size, d_size); + gsl_matrix_view QiMQiMQi_ee_s = + gsl_matrix_submatrix (QiMQiMQi_ee, (c_size-1)*d_size, + (c_size-1)*d_size, d_size, d_size); + + // and part of trB_gg, trB_ge, trB_ee. + gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, + &QiMQiMQi_gg_s.matrix, Qi_si, 0.0, M_dd); + trB_gg=0.0; + for (size_t k=0; k<d_size; k++) { + d=gsl_matrix_get (M_dd, k, k); + trB_gg-=d; + } + + gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, + &QiMQiMQi_ge_s.matrix, Qi_si, 0.0, M_dd); + trB_ge=0.0; + for (size_t k=0; k<d_size; k++) { + d=gsl_matrix_get (M_dd, k, k); + trB_ge-=d; + } + + gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, + &QiMQiMQi_ee_s.matrix, Qi_si, 0.0, M_dd); + trB_ee=0.0; + for (size_t k=0; k<d_size; k++) { + d=gsl_matrix_get (M_dd, k, k); + trB_ee-=d; + } + + // Calculate Qi(xHiDHiDHix)Qi, and subpart of it. + gsl_matrix_const_view MM_gg = + gsl_matrix_const_submatrix (xHiDHiDHix_all_gg, 0, + (v1*v_size+v2)*dc_size, dc_size, + dc_size); + gsl_matrix_const_view MM_ge = + gsl_matrix_const_submatrix (xHiDHiDHix_all_ge, 0, + (v1*v_size+v2)*dc_size, dc_size, + dc_size); + gsl_matrix_const_view MM_ee = + gsl_matrix_const_submatrix (xHiDHiDHix_all_ee, 0, + (v1*v_size+v2)*dc_size, dc_size, + dc_size); + + gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Qi, + &MM_gg.matrix, 0.0, M_dcdc); + gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, M_dcdc, Qi, 0.0, + QiMMQi_gg); + gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Qi, + &MM_ge.matrix, 0.0, M_dcdc); + gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, M_dcdc, + Qi, 0.0, QiMMQi_ge); + gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Qi, + &MM_ee.matrix, 0.0, M_dcdc); + gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, M_dcdc, Qi, + 0.0, QiMMQi_ee); + + gsl_matrix_view QiMMQi_gg_s = + gsl_matrix_submatrix (QiMMQi_gg, (c_size-1)*d_size, + (c_size-1)*d_size, d_size, d_size); + gsl_matrix_view QiMMQi_ge_s = + gsl_matrix_submatrix (QiMMQi_ge, (c_size-1)*d_size, + (c_size-1)*d_size, d_size, d_size); + gsl_matrix_view QiMMQi_ee_s = + gsl_matrix_submatrix (QiMMQi_ee, (c_size-1)*d_size, + (c_size-1)*d_size, d_size, d_size); + + // Calculate the other part of trB_gg, trB_ge, trB_ee. + gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, + &QiMMQi_gg_s.matrix, Qi_si, 0.0, M_dd); + for (size_t k=0; k<d_size; k++) { + trB_gg+=gsl_matrix_get (M_dd, k, k); + } + gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, + &QiMMQi_ge_s.matrix, Qi_si, 0.0, M_dd); + for (size_t k=0; k<d_size; k++) { + trB_ge+=2.0*gsl_matrix_get (M_dd, k, k); + } + gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, + &QiMMQi_ee_s.matrix, Qi_si, 0.0, M_dd); + for (size_t k=0; k<d_size; k++) { + trB_ee+=gsl_matrix_get (M_dd, k, k); + } + + // Calculate trD_gg, trD_ge, trD_ee. + trD_gg=2.0*trB_gg; + trD_ge=2.0*trB_ge; + trD_ee=2.0*trB_ee; + + //calculate B, C and D + h_gg=-1.0*gsl_matrix_get (Hessian_inv, v1, v2); + h_ge=-1.0*gsl_matrix_get (Hessian_inv, v1, v2+v_size); + h_ee=-1.0*gsl_matrix_get (Hessian_inv, v1+v_size, v2+v_size); + + B+=h_gg*trB_gg+h_ge*trB_ge+h_ee*trB_ee; + C+=h_gg*(trCC_gg+0.5*trCg1*trCg2) + + h_ge*(trCC_ge+0.5*trCg1*trCe2+0.5*trCe1*trCg2) + + h_ee*(trCC_ee+0.5*trCe1*trCe2); + D+=h_gg*(trCC_gg+0.5*trD_gg) + + h_ge*(trCC_ge+0.5*trD_ge) + h_ee*(trCC_ee+0.5*trD_ee); + + if (v1!=v2) { + B+=h_gg*trB_gg+h_ge*trB_ge+h_ee*trB_ee; + C+=h_gg*(trCC_gg+0.5*trCg1*trCg2) + + h_ge*(trCC_ge+0.5*trCg1*trCe2+0.5*trCe1*trCg2) + + h_ee*(trCC_ee+0.5*trCe1*trCe2); + D+=h_gg*(trCC_gg+0.5*trD_gg) + + h_ge*(trCC_ge+0.5*trD_ge) + + h_ee*(trCC_ee+0.5*trD_ee); + } + } } - //calculate a, b, c from B C D + // Calculate a, b, c from B C D. crt_a=2.0*D-C; crt_b=2.0*B; crt_c=C; - /* - cout<<B<<"\t"<<C<<"\t"<<D<<endl; - cout<<setprecision(6)<<crt_a<<"\t"<<crt_b<<"\t"<<crt_c<<endl; - */ - //free matrix memory + + // Free matrix memory. gsl_matrix_free(QiMQi_g1); gsl_matrix_free(QiMQi_e1); gsl_matrix_free(QiMQi_g2); @@ -2244,14 +2387,19 @@ void CalcCRT (const gsl_matrix *Hessian_inv, const gsl_matrix *Qi, const gsl_mat return; } - - - - -//calculate first-order and second-order derivatives -void CalcDev (const char func_name, const gsl_vector *eval, const gsl_matrix *Qi, const gsl_matrix *Hi, const gsl_matrix *xHi, const gsl_matrix *Hiy, const gsl_vector *QixHiy, gsl_vector *gradient, gsl_matrix *Hessian_inv, double &crt_a, double &crt_b, double &crt_c) -{ - if (func_name!='R' && func_name!='L' && func_name!='r' && func_name!='l') {cout<<"func_name only takes 'R' or 'L': 'R' for log-restricted likelihood, 'L' for log-likelihood."<<endl; return;} +// Calculate first-order and second-order derivatives. +void CalcDev (const char func_name, const gsl_vector *eval, + const gsl_matrix *Qi, const gsl_matrix *Hi, + const gsl_matrix *xHi, const gsl_matrix *Hiy, + const gsl_vector *QixHiy, gsl_vector *gradient, + gsl_matrix *Hessian_inv, double &crt_a, double &crt_b, + double &crt_c) { + if (func_name!='R' && func_name!='L' && func_name!='r' && + func_name!='l') { + cout<<"func_name only takes 'R' or 'L': 'R' for " << + "log-restricted likelihood, 'L' for log-likelihood."<<endl; + return; + } size_t dc_size=Qi->size1, d_size=Hi->size1; size_t c_size=dc_size/d_size; @@ -2275,137 +2423,140 @@ void CalcDev (const char func_name, const gsl_vector *eval, const gsl_matrix *Qi gsl_matrix *QixHiDHixQixHiy_all_g=gsl_matrix_alloc (dc_size, v_size); gsl_matrix *QixHiDHixQixHiy_all_e=gsl_matrix_alloc (dc_size, v_size); - gsl_matrix *xHiDHiDHiy_all_gg=gsl_matrix_alloc (dc_size, v_size*v_size); - gsl_matrix *xHiDHiDHiy_all_ee=gsl_matrix_alloc (dc_size, v_size*v_size); - gsl_matrix *xHiDHiDHiy_all_ge=gsl_matrix_alloc (dc_size, v_size*v_size); - gsl_matrix *xHiDHiDHix_all_gg=gsl_matrix_alloc (dc_size, v_size*v_size*dc_size); - gsl_matrix *xHiDHiDHix_all_ee=gsl_matrix_alloc (dc_size, v_size*v_size*dc_size); - gsl_matrix *xHiDHiDHix_all_ge=gsl_matrix_alloc (dc_size, v_size*v_size*dc_size); - - //calculate xHiDHiy_all, xHiDHix_all and xHiDHixQixHiy_all + gsl_matrix *xHiDHiDHiy_all_gg = + gsl_matrix_alloc (dc_size, v_size*v_size); + gsl_matrix *xHiDHiDHiy_all_ee = + gsl_matrix_alloc (dc_size, v_size*v_size); + gsl_matrix *xHiDHiDHiy_all_ge = + gsl_matrix_alloc (dc_size, v_size*v_size); + gsl_matrix *xHiDHiDHix_all_gg = + gsl_matrix_alloc (dc_size, v_size*v_size*dc_size); + gsl_matrix *xHiDHiDHix_all_ee = + gsl_matrix_alloc (dc_size, v_size*v_size*dc_size); + gsl_matrix *xHiDHiDHix_all_ge = + gsl_matrix_alloc (dc_size, v_size*v_size*dc_size); + + // Calculate xHiDHiy_all, xHiDHix_all and xHiDHixQixHiy_all. Calc_xHiDHiy_all (eval, xHi, Hiy, xHiDHiy_all_g, xHiDHiy_all_e); Calc_xHiDHix_all (eval, xHi, xHiDHix_all_g, xHiDHix_all_e); - Calc_xHiDHixQixHiy_all (xHiDHix_all_g, xHiDHix_all_e, QixHiy, xHiDHixQixHiy_all_g, xHiDHixQixHiy_all_e); - - Calc_xHiDHiDHiy_all (v_size, eval, Hi, xHi, Hiy, xHiDHiDHiy_all_gg, xHiDHiDHiy_all_ee, xHiDHiDHiy_all_ge); - Calc_xHiDHiDHix_all (v_size, eval, Hi, xHi, xHiDHiDHix_all_gg, xHiDHiDHix_all_ee, xHiDHiDHix_all_ge); - - //calculate QixHiDHiy_all, QixHiDHix_all and QixHiDHixQixHiy_all - Calc_QiVec_all (Qi, xHiDHiy_all_g, xHiDHiy_all_e, QixHiDHiy_all_g, QixHiDHiy_all_e); - Calc_QiVec_all (Qi, xHiDHixQixHiy_all_g, xHiDHixQixHiy_all_e, QixHiDHixQixHiy_all_g, QixHiDHixQixHiy_all_e); - Calc_QiMat_all (Qi, xHiDHix_all_g, xHiDHix_all_e, QixHiDHix_all_g, QixHiDHix_all_e); - - double tHiD_g, tHiD_e, tPD_g, tPD_e, tHiDHiD_gg, tHiDHiD_ee, tHiDHiD_ge, tPDPD_gg, tPDPD_ee, tPDPD_ge; + Calc_xHiDHixQixHiy_all (xHiDHix_all_g, xHiDHix_all_e, QixHiy, + xHiDHixQixHiy_all_g, xHiDHixQixHiy_all_e); + + Calc_xHiDHiDHiy_all (v_size, eval, Hi, xHi, Hiy, xHiDHiDHiy_all_gg, + xHiDHiDHiy_all_ee, xHiDHiDHiy_all_ge); + Calc_xHiDHiDHix_all (v_size, eval, Hi, xHi, xHiDHiDHix_all_gg, + xHiDHiDHix_all_ee, xHiDHiDHix_all_ge); + + // Calculate QixHiDHiy_all, QixHiDHix_all and QixHiDHixQixHiy_all. + Calc_QiVec_all (Qi, xHiDHiy_all_g, xHiDHiy_all_e, QixHiDHiy_all_g, + QixHiDHiy_all_e); + Calc_QiVec_all (Qi, xHiDHixQixHiy_all_g, xHiDHixQixHiy_all_e, + QixHiDHixQixHiy_all_g, QixHiDHixQixHiy_all_e); + Calc_QiMat_all (Qi, xHiDHix_all_g, xHiDHix_all_e, QixHiDHix_all_g, + QixHiDHix_all_e); + + double tHiD_g, tHiD_e, tPD_g, tPD_e, tHiDHiD_gg, tHiDHiD_ee; + double tHiDHiD_ge, tPDPD_gg, tPDPD_ee, tPDPD_ge; double yPDPy_g, yPDPy_e, yPDPDPy_gg, yPDPDPy_ee, yPDPDPy_ge; - //calculate gradient and Hessian for Vg + // Calculate gradient and Hessian for Vg. for (size_t i1=0; i1<d_size; i1++) { - for (size_t j1=0; j1<d_size; j1++) { - if (j1<i1) {continue;} - v1=GetIndex (i1, j1, d_size); - - Calc_yPDPy (eval, Hiy, QixHiy, xHiDHiy_all_g, xHiDHiy_all_e, xHiDHixQixHiy_all_g, xHiDHixQixHiy_all_e, i1, j1, yPDPy_g, yPDPy_e); - - if (func_name=='R' || func_name=='r') { - Calc_tracePD (eval, Qi, Hi, xHiDHix_all_g, xHiDHix_all_e, i1, j1, tPD_g, tPD_e); - //cout<<i1<<" "<<j1<<" "<<yPDPy_g<<" "<<yPDPy_e<<" "<<tPD_g<<" "<<tPD_e<<endl; - - dev1_g=-0.5*tPD_g+0.5*yPDPy_g; - dev1_e=-0.5*tPD_e+0.5*yPDPy_e; - } else { - Calc_traceHiD (eval, Hi, i1, j1, tHiD_g, tHiD_e); - - dev1_g=-0.5*tHiD_g+0.5*yPDPy_g; - dev1_e=-0.5*tHiD_e+0.5*yPDPy_e; - } - - gsl_vector_set (gradient, v1, dev1_g); - gsl_vector_set (gradient, v1+v_size, dev1_e); - - for (size_t i2=0; i2<d_size; i2++) { - for (size_t j2=0; j2<d_size; j2++) { - if (j2<i2) {continue;} - v2=GetIndex (i2, j2, d_size); - - if (v2<v1) {continue;} - - Calc_yPDPDPy (eval, Hi, xHi, Hiy, QixHiy, xHiDHiy_all_g, xHiDHiy_all_e, QixHiDHiy_all_g, QixHiDHiy_all_e, xHiDHixQixHiy_all_g, xHiDHixQixHiy_all_e, QixHiDHixQixHiy_all_g, QixHiDHixQixHiy_all_e, xHiDHiDHiy_all_gg, xHiDHiDHiy_all_ee, xHiDHiDHiy_all_ge, xHiDHiDHix_all_gg, xHiDHiDHix_all_ee, xHiDHiDHix_all_ge, i1, j1, i2, j2, yPDPDPy_gg, yPDPDPy_ee, yPDPDPy_ge); - - //cout<<i1<<" "<<j1<<" "<<i2<<" "<<j2<<" "<<yPDPDPy_gg<<" "<<yPDPDPy_ee<<" "<<yPDPDPy_ge<<endl; - //AI for reml - if (func_name=='R' || func_name=='r') { - Calc_tracePDPD (eval, Qi, Hi, xHi, QixHiDHix_all_g, QixHiDHix_all_e, xHiDHiDHix_all_gg, xHiDHiDHix_all_ee, xHiDHiDHix_all_ge, i1, j1, i2, j2, tPDPD_gg, tPDPD_ee, tPDPD_ge); - - dev2_gg=0.5*tPDPD_gg-yPDPDPy_gg; - dev2_ee=0.5*tPDPD_ee-yPDPDPy_ee; - dev2_ge=0.5*tPDPD_ge-yPDPDPy_ge; - /* - dev2_gg=-0.5*yPDPDPy_gg; - dev2_ee=-0.5*yPDPDPy_ee; - dev2_ge=-0.5*yPDPDPy_ge; - */ - } else { - Calc_traceHiDHiD (eval, Hi, i1, j1, i2, j2, tHiDHiD_gg, tHiDHiD_ee, tHiDHiD_ge); - - dev2_gg=0.5*tHiDHiD_gg-yPDPDPy_gg; - dev2_ee=0.5*tHiDHiD_ee-yPDPDPy_ee; - dev2_ge=0.5*tHiDHiD_ge-yPDPDPy_ge; - } + for (size_t j1=0; j1<d_size; j1++) { + if (j1<i1) {continue;} + v1=GetIndex (i1, j1, d_size); + + Calc_yPDPy (eval, Hiy, QixHiy, xHiDHiy_all_g, xHiDHiy_all_e, + xHiDHixQixHiy_all_g, xHiDHixQixHiy_all_e, i1, j1, + yPDPy_g, yPDPy_e); + + if (func_name=='R' || func_name=='r') { + Calc_tracePD (eval, Qi, Hi, xHiDHix_all_g, xHiDHix_all_e, + i1, j1, tPD_g, tPD_e); - //set up Hessian - gsl_matrix_set (Hessian, v1, v2, dev2_gg); - gsl_matrix_set (Hessian, v1+v_size, v2+v_size, dev2_ee); - gsl_matrix_set (Hessian, v1, v2+v_size, dev2_ge); - gsl_matrix_set (Hessian, v2+v_size, v1, dev2_ge); - - if (v1!=v2) { - gsl_matrix_set (Hessian, v2, v1, dev2_gg); - gsl_matrix_set (Hessian, v2+v_size, v1+v_size, dev2_ee); - gsl_matrix_set (Hessian, v2, v1+v_size, dev2_ge); - gsl_matrix_set (Hessian, v1+v_size, v2, dev2_ge); - } - } - } + dev1_g=-0.5*tPD_g+0.5*yPDPy_g; + dev1_e=-0.5*tPD_e+0.5*yPDPy_e; + } else { + Calc_traceHiD (eval, Hi, i1, j1, tHiD_g, tHiD_e); + + dev1_g=-0.5*tHiD_g+0.5*yPDPy_g; + dev1_e=-0.5*tHiD_e+0.5*yPDPy_e; + } + + gsl_vector_set (gradient, v1, dev1_g); + gsl_vector_set (gradient, v1+v_size, dev1_e); + + for (size_t i2=0; i2<d_size; i2++) { + for (size_t j2=0; j2<d_size; j2++) { + if (j2<i2) {continue;} + v2=GetIndex (i2, j2, d_size); + + if (v2<v1) {continue;} + + Calc_yPDPDPy (eval, Hi, xHi, Hiy, QixHiy, xHiDHiy_all_g, + xHiDHiy_all_e, QixHiDHiy_all_g, QixHiDHiy_all_e, + xHiDHixQixHiy_all_g, xHiDHixQixHiy_all_e, + QixHiDHixQixHiy_all_g, QixHiDHixQixHiy_all_e, + xHiDHiDHiy_all_gg, xHiDHiDHiy_all_ee, + xHiDHiDHiy_all_ge, xHiDHiDHix_all_gg, + xHiDHiDHix_all_ee, xHiDHiDHix_all_ge, i1, j1, + i2, j2, yPDPDPy_gg, yPDPDPy_ee, yPDPDPy_ge); + + // AI for REML. + if (func_name=='R' || func_name=='r') { + Calc_tracePDPD (eval, Qi, Hi, xHi, QixHiDHix_all_g, + QixHiDHix_all_e, xHiDHiDHix_all_gg, + xHiDHiDHix_all_ee, xHiDHiDHix_all_ge, i1, j1, + i2, j2, tPDPD_gg, tPDPD_ee, tPDPD_ge); + + dev2_gg=0.5*tPDPD_gg-yPDPDPy_gg; + dev2_ee=0.5*tPDPD_ee-yPDPDPy_ee; + dev2_ge=0.5*tPDPD_ge-yPDPDPy_ge; + } else { + Calc_traceHiDHiD (eval, Hi, i1, j1, i2, j2, tHiDHiD_gg, + tHiDHiD_ee, tHiDHiD_ge); + + dev2_gg=0.5*tHiDHiD_gg-yPDPDPy_gg; + dev2_ee=0.5*tHiDHiD_ee-yPDPDPy_ee; + dev2_ge=0.5*tHiDHiD_ge-yPDPDPy_ge; } - } - - /* - cout<<"Hessian: "<<endl; - for (size_t i=0; i<2*v_size; i++) { - for (size_t j=0; j<2*v_size; j++) { - cout<<gsl_matrix_get(Hessian, i, j)<<"\t"; + + // Set up Hessian. + gsl_matrix_set (Hessian, v1, v2, dev2_gg); + gsl_matrix_set (Hessian, v1+v_size, v2+v_size, dev2_ee); + gsl_matrix_set (Hessian, v1, v2+v_size, dev2_ge); + gsl_matrix_set (Hessian, v2+v_size, v1, dev2_ge); + + if (v1!=v2) { + gsl_matrix_set (Hessian, v2, v1, dev2_gg); + gsl_matrix_set (Hessian, v2+v_size, v1+v_size, dev2_ee); + gsl_matrix_set (Hessian, v2, v1+v_size, dev2_ge); + gsl_matrix_set (Hessian, v1+v_size, v2, dev2_ge); } - cout<<endl; + } + } + } } - */ - - - //Invert Hessian + + // Invert Hessian. int sig; gsl_permutation * pmt=gsl_permutation_alloc (v_size*2); LUDecomp (Hessian, pmt, &sig); LUInvert (Hessian, pmt, Hessian_inv); - /* - cout<<"Hessian Inverse: "<<endl; - for (size_t i=0; i<2*v_size; i++) { - for (size_t j=0; j<2*v_size; j++) { - cout<<gsl_matrix_get(Hessian_inv, i, j)<<"\t"; - } - cout<<endl; - } - */ + gsl_permutation_free(pmt); gsl_matrix_free(Hessian); - //calculate Edgeworth correction factors - //after inverting Hessian + // Calculate Edgeworth correction factors after inverting + // Hessian. if (c_size>1) { - CalcCRT (Hessian_inv, Qi, QixHiDHix_all_g, QixHiDHix_all_e, xHiDHiDHix_all_gg, xHiDHiDHix_all_ee, xHiDHiDHix_all_ge, d_size, crt_a, crt_b, crt_c); + CalcCRT(Hessian_inv, Qi, QixHiDHix_all_g, QixHiDHix_all_e, + xHiDHiDHix_all_gg, xHiDHiDHix_all_ee, xHiDHiDHix_all_ge, + d_size, crt_a, crt_b, crt_c); } else { - crt_a=0.0; crt_b=0.0; crt_c=0.0; + crt_a=0.0; crt_b=0.0; crt_c=0.0; } - + gsl_matrix_free(xHiDHiy_all_g); gsl_matrix_free(xHiDHiy_all_e); gsl_matrix_free(xHiDHix_all_g); @@ -2430,10 +2581,9 @@ void CalcDev (const char func_name, const gsl_vector *eval, const gsl_matrix *Qi return; } - -//update Vg, Ve -void UpdateVgVe (const gsl_matrix *Hessian_inv, const gsl_vector *gradient, const double step_scale, gsl_matrix *V_g, gsl_matrix *V_e) -{ +// Update Vg, Ve. +void UpdateVgVe (const gsl_matrix *Hessian_inv, const gsl_vector *gradient, + const double step_scale, gsl_matrix *V_g, gsl_matrix *V_e) { size_t v_size=gradient->size/2, d_size=V_g->size1; size_t v; @@ -2441,7 +2591,7 @@ void UpdateVgVe (const gsl_matrix *Hessian_inv, const gsl_vector *gradient, cons double d; - //vectorize Vg and Ve + // Vectorize Vg and Ve. for (size_t i=0; i<d_size; i++) { for (size_t j=0; j<d_size; j++) { if (j<i) {continue;} @@ -2455,9 +2605,10 @@ void UpdateVgVe (const gsl_matrix *Hessian_inv, const gsl_vector *gradient, cons } } - gsl_blas_dgemv (CblasNoTrans, -1.0*step_scale, Hessian_inv, gradient, 1.0, vec_v); + gsl_blas_dgemv (CblasNoTrans, -1.0*step_scale, Hessian_inv, + gradient, 1.0, vec_v); - //save Vg and Ve + // Save Vg and Ve. for (size_t i=0; i<d_size; i++) { for (size_t j=0; j<d_size; j++) { if (j<i) {continue;} @@ -2478,19 +2629,24 @@ void UpdateVgVe (const gsl_matrix *Hessian_inv, const gsl_vector *gradient, cons return; } - - - - - -double MphNR (const char func_name, const size_t max_iter, const double max_prec, const gsl_vector *eval, const gsl_matrix *X, const gsl_matrix *Y, gsl_matrix *Hi_all, gsl_matrix *xHi_all, gsl_matrix *Hiy_all, gsl_matrix *V_g, gsl_matrix *V_e, gsl_matrix *Hessian_inv, double &crt_a, double &crt_b, double &crt_c) -{ - if (func_name!='R' && func_name!='L' && func_name!='r' && func_name!='l') {cout<<"func_name only takes 'R' or 'L': 'R' for log-restricted likelihood, 'L' for log-likelihood."<<endl; return 0.0;} +double MphNR (const char func_name, const size_t max_iter, + const double max_prec, const gsl_vector *eval, + const gsl_matrix *X, const gsl_matrix *Y, gsl_matrix *Hi_all, + gsl_matrix *xHi_all, gsl_matrix *Hiy_all, gsl_matrix *V_g, + gsl_matrix *V_e, gsl_matrix *Hessian_inv, double &crt_a, + double &crt_b, double &crt_c) { + if (func_name!='R' && func_name!='L' && func_name!='r' && + func_name!='l') { + cout<<"func_name only takes 'R' or 'L': 'R' for log-restricted "<< + "likelihood, 'L' for log-likelihood."<<endl; + return 0.0; + } size_t n_size=eval->size, c_size=X->size1, d_size=Y->size1; size_t dc_size=d_size*c_size; size_t v_size=d_size*(d_size+1)/2; - double logdet_H, logdet_Q, yPy, logl_const, logl_old=0.0, logl_new=0.0, step_scale; + double logdet_H, logdet_Q, yPy, logl_const; + double logl_old=0.0, logl_new=0.0, step_scale; int sig; size_t step_iter, flag_pd; @@ -2506,80 +2662,84 @@ double MphNR (const char func_name, const size_t max_iter, const double max_prec gsl_vector *gradient=gsl_vector_alloc (v_size*2); - //calculate |XXt| and (XXt)^{-1} + // Calculate |XXt| and (XXt)^{-1}. gsl_blas_dsyrk (CblasUpper, CblasNoTrans, 1.0, X, 0.0, XXt); for (size_t i=0; i<c_size; ++i) { - for (size_t j=0; j<i; ++j) { - gsl_matrix_set (XXt, i, j, gsl_matrix_get (XXt, j, i)); - } + for (size_t j=0; j<i; ++j) { + gsl_matrix_set (XXt, i, j, gsl_matrix_get (XXt, j, i)); + } } gsl_permutation * pmt=gsl_permutation_alloc (c_size); LUDecomp (XXt, pmt, &sig); gsl_permutation_free (pmt); -// LUInvert (XXt, pmt, XXti); - //calculate the constant for logl + // Calculate the constant for logl. if (func_name=='R' || func_name=='r') { - logl_const=-0.5*(double)(n_size-c_size)*(double)d_size*log(2.0*M_PI)+0.5*(double)d_size*LULndet (XXt); + logl_const=-0.5*(double)(n_size-c_size) * + (double)d_size*log(2.0*M_PI) + + 0.5*(double)d_size*LULndet (XXt); } else { - logl_const=-0.5*(double)n_size*(double)d_size*log(2.0*M_PI); + logl_const=-0.5*(double)n_size*(double)d_size*log(2.0*M_PI); } - //optimization iterations - + + // Optimization iterations. for (size_t t=0; t<max_iter; t++) { gsl_matrix_memcpy (Vg_save, V_g); gsl_matrix_memcpy (Ve_save, V_e); step_scale=1.0; step_iter=0; do { - gsl_matrix_memcpy (V_g, Vg_save); - gsl_matrix_memcpy (V_e, Ve_save); - - //update Vg, Ve, and invert Hessian - if (t!=0) {UpdateVgVe (Hessian_inv, gradient, step_scale, V_g, V_e);} - - //check if both Vg and Ve are positive definite - flag_pd=1; - gsl_matrix_memcpy (V_temp, V_e); - EigenDecomp(V_temp, U_temp, D_temp, 0); - for (size_t i=0; i<d_size; i++) { - if (gsl_vector_get (D_temp, i)<=0) {flag_pd=0;} - } - gsl_matrix_memcpy (V_temp, V_g); - EigenDecomp(V_temp, U_temp, D_temp, 0); - for (size_t i=0; i<d_size; i++) { - if (gsl_vector_get (D_temp, i)<=0) {flag_pd=0;} - } - - //if flag_pd==1 continue to calculate quantities and logl - if (flag_pd==1) { - CalcHiQi (eval, X, V_g, V_e, Hi_all, Qi, logdet_H, logdet_Q); - Calc_Hiy_all (Y, Hi_all, Hiy_all); - Calc_xHi_all (X, Hi_all, xHi_all); - - //calculate QixHiy and yPy - Calc_xHiy (Y, xHi_all, xHiy); - gsl_blas_dgemv (CblasNoTrans, 1.0, Qi, xHiy, 0.0, QixHiy); - - gsl_blas_ddot (QixHiy, xHiy, &yPy); - yPy=Calc_yHiy (Y, Hiy_all)-yPy; - - //calculate log likelihood/restricted likelihood value - if (func_name=='R' || func_name=='r') { - logl_new=logl_const-0.5*logdet_H-0.5*logdet_Q-0.5*yPy; - } else { - logl_new=logl_const-0.5*logdet_H-0.5*yPy; - } - } - - step_scale/=2.0; - step_iter++; - - //cout<<t<<"\t"<<step_iter<<"\t"<<logl_old<<"\t"<<logl_new<<"\t"<<flag_pd<<endl; - } while ( (flag_pd==0 || logl_new<logl_old || logl_new-logl_old>10 ) && step_iter<10 && t!=0); + gsl_matrix_memcpy (V_g, Vg_save); + gsl_matrix_memcpy (V_e, Ve_save); + + // Update Vg, Ve, and invert Hessian. + if (t!=0) { + UpdateVgVe (Hessian_inv, gradient, step_scale, V_g, V_e); + } + + // Check if both Vg and Ve are positive definite. + flag_pd=1; + gsl_matrix_memcpy (V_temp, V_e); + EigenDecomp(V_temp, U_temp, D_temp, 0); + for (size_t i=0; i<d_size; i++) { + if (gsl_vector_get (D_temp, i)<=0) {flag_pd=0;} + } + gsl_matrix_memcpy (V_temp, V_g); + EigenDecomp(V_temp, U_temp, D_temp, 0); + for (size_t i=0; i<d_size; i++) { + if (gsl_vector_get (D_temp, i)<=0) {flag_pd=0;} + } + + // If flag_pd==1, continue to calculate quantities + // and logl. + if (flag_pd==1) { + CalcHiQi(eval,X,V_g,V_e,Hi_all,Qi,logdet_H,logdet_Q); + Calc_Hiy_all (Y, Hi_all, Hiy_all); + Calc_xHi_all (X, Hi_all, xHi_all); + + // Calculate QixHiy and yPy. + Calc_xHiy (Y, xHi_all, xHiy); + gsl_blas_dgemv (CblasNoTrans, 1.0, Qi, xHiy, 0.0, QixHiy); + + gsl_blas_ddot (QixHiy, xHiy, &yPy); + yPy=Calc_yHiy (Y, Hiy_all)-yPy; + + // Calculate log likelihood/restricted likelihood value. + if (func_name=='R' || func_name=='r') { + logl_new=logl_const-0.5*logdet_H-0.5*logdet_Q-0.5*yPy; + } else { + logl_new=logl_const-0.5*logdet_H-0.5*yPy; + } + } + + step_scale/=2.0; + step_iter++; + + } while ( (flag_pd==0 || logl_new<logl_old || + logl_new-logl_old>10 ) && step_iter<10 && t!=0); - //terminate if change is small + // Terminate if change is small. if (t!=0) { if (logl_new<logl_old || flag_pd==0) { gsl_matrix_memcpy (V_g, Vg_save); @@ -2594,39 +2754,12 @@ double MphNR (const char func_name, const size_t max_iter, const double max_prec logl_old=logl_new; - CalcDev (func_name, eval, Qi, Hi_all, xHi_all, Hiy_all, QixHiy, gradient, Hessian_inv, crt_a, crt_b, crt_c); - - - //output estimates in each iteration - /* - cout<<func_name<<" iteration = "<<t<<" log-likelihood = "<<logl_old<<"\t"<<logl_new<<endl; - - cout<<"Vg: "<<endl; - for (size_t i=0; i<d_size; i++) { - for (size_t j=0; j<d_size; j++) { - cout<<gsl_matrix_get(V_g, i, j)<<"\t"; - } - cout<<endl; - } - cout<<"Ve: "<<endl; - for (size_t i=0; i<d_size; i++) { - for (size_t j=0; j<d_size; j++) { - cout<<gsl_matrix_get(V_e, i, j)<<"\t"; - } - cout<<endl; - } - cout<<"Hessian: "<<endl; - for (size_t i=0; i<Hessian_inv->size1; i++) { - for (size_t j=0; j<Hessian_inv->size2; j++) { - cout<<gsl_matrix_get(Hessian_inv, i, j)<<"\t"; - } - cout<<endl; - } - */ + CalcDev (func_name, eval, Qi, Hi_all, xHi_all, Hiy_all, + QixHiy, gradient, Hessian_inv, crt_a, crt_b, crt_c); } - //mutiply Hessian_inv with -1.0 - //now Hessian_inv is the variance matrix + // Mutiply Hessian_inv with -1.0. + // Now Hessian_inv is the variance matrix. gsl_matrix_scale (Hessian_inv, -1.0); gsl_matrix_free(Vg_save); @@ -2645,13 +2778,14 @@ double MphNR (const char func_name, const size_t max_iter, const double max_prec return logl_new; } - - - - -//initialize Vg, Ve and B -void MphInitial(const size_t em_iter, const double em_prec, const size_t nr_iter, const double nr_prec, const gsl_vector *eval, const gsl_matrix *X, const gsl_matrix *Y, const double l_min, const double l_max, const size_t n_region, gsl_matrix *V_g, gsl_matrix *V_e, gsl_matrix *B) -{ +// Initialize Vg, Ve and B. +void MphInitial(const size_t em_iter, const double em_prec, + const size_t nr_iter, const double nr_prec, + const gsl_vector *eval, const gsl_matrix *X, + const gsl_matrix *Y, const double l_min, const double l_max, + const size_t n_region, gsl_matrix *V_g, gsl_matrix *V_e, + gsl_matrix *B) { + gsl_matrix_set_zero (V_g); gsl_matrix_set_zero (V_e); gsl_matrix_set_zero (B); @@ -2660,7 +2794,8 @@ void MphInitial(const size_t em_iter, const double em_prec, const size_t nr_iter double a, b, c; double lambda, logl, vg, ve; - //Initial the diagonal elements of Vg and Ve using univariate LMM and REML estimates + // Initialize the diagonal elements of Vg and Ve using univariate + // LMM and REML estimates. gsl_matrix *Xt=gsl_matrix_alloc (n_size, c_size); gsl_vector *beta_temp=gsl_vector_alloc(c_size); gsl_vector *se_beta_temp=gsl_vector_alloc(c_size); @@ -2668,154 +2803,104 @@ void MphInitial(const size_t em_iter, const double em_prec, const size_t nr_iter gsl_matrix_transpose_memcpy (Xt, X); for (size_t i=0; i<d_size; i++) { - gsl_vector_const_view Y_row=gsl_matrix_const_row (Y, i); - CalcLambda ('R', eval, Xt, &Y_row.vector, l_min, l_max, n_region, lambda, logl); - CalcLmmVgVeBeta (eval, Xt, &Y_row.vector, lambda, vg, ve, beta_temp, se_beta_temp); - - gsl_matrix_set(V_g, i, i, vg); - gsl_matrix_set(V_e, i, i, ve); + gsl_vector_const_view Y_row=gsl_matrix_const_row (Y, i); + CalcLambda ('R', eval, Xt, &Y_row.vector, l_min, l_max, + n_region, lambda, logl); + CalcLmmVgVeBeta (eval, Xt, &Y_row.vector, lambda, vg, ve, + beta_temp, se_beta_temp); + + gsl_matrix_set(V_g, i, i, vg); + gsl_matrix_set(V_e, i, i, ve); } - + gsl_matrix_free (Xt); gsl_vector_free (beta_temp); gsl_vector_free (se_beta_temp); - //if number of phenotypes is above four, then obtain the off diagonal elements with two trait models + // If number of phenotypes is above four, then obtain the off + // diagonal elements with two trait models. if (d_size>4) { - //first obtain good initial values - //large matrices for EM - gsl_matrix *U_hat=gsl_matrix_alloc (2, n_size); - gsl_matrix *E_hat=gsl_matrix_alloc (2, n_size); - gsl_matrix *OmegaU=gsl_matrix_alloc (2, n_size); - gsl_matrix *OmegaE=gsl_matrix_alloc (2, n_size); - gsl_matrix *UltVehiY=gsl_matrix_alloc (2, n_size); - gsl_matrix *UltVehiBX=gsl_matrix_alloc (2, n_size); - gsl_matrix *UltVehiU=gsl_matrix_alloc (2, n_size); - gsl_matrix *UltVehiE=gsl_matrix_alloc (2, n_size); - - //large matrices for NR - gsl_matrix *Hi_all=gsl_matrix_alloc (2, 2*n_size); //each dxd block is H_k^{-1} - gsl_matrix *Hiy_all=gsl_matrix_alloc (2, n_size); //each column is H_k^{-1}y_k - gsl_matrix *xHi_all=gsl_matrix_alloc (2*c_size, 2*n_size); //each dcxdc block is x_k\otimes H_k^{-1} - gsl_matrix *Hessian=gsl_matrix_alloc (6, 6); - - //2 by n matrix of Y - gsl_matrix *Y_sub=gsl_matrix_alloc (2, n_size); - gsl_matrix *Vg_sub=gsl_matrix_alloc (2, 2); - gsl_matrix *Ve_sub=gsl_matrix_alloc (2, 2); - gsl_matrix *B_sub=gsl_matrix_alloc (2, c_size); - - for (size_t i=0; i<d_size; i++) { - gsl_vector_view Y_sub1=gsl_matrix_row (Y_sub, 0); - gsl_vector_const_view Y_1=gsl_matrix_const_row (Y, i); - gsl_vector_memcpy (&Y_sub1.vector, &Y_1.vector); - - for (size_t j=i+1; j<d_size; j++) { - gsl_vector_view Y_sub2=gsl_matrix_row (Y_sub, 1); - gsl_vector_const_view Y_2=gsl_matrix_const_row (Y, j); - gsl_vector_memcpy (&Y_sub2.vector, &Y_2.vector); - - gsl_matrix_set_zero (Vg_sub); - gsl_matrix_set_zero (Ve_sub); - gsl_matrix_set (Vg_sub, 0, 0, gsl_matrix_get (V_g, i, i)); - gsl_matrix_set (Ve_sub, 0, 0, gsl_matrix_get (V_e, i, i)); - gsl_matrix_set (Vg_sub, 1, 1, gsl_matrix_get (V_g, j, j)); - gsl_matrix_set (Ve_sub, 1, 1, gsl_matrix_get (V_e, j, j)); - - logl=MphEM ('R', em_iter, em_prec, eval, X, Y_sub, U_hat, E_hat, OmegaU, OmegaE, UltVehiY, UltVehiBX, UltVehiU, UltVehiE, Vg_sub, Ve_sub, B_sub); - logl=MphNR ('R', nr_iter, nr_prec, eval, X, Y_sub, Hi_all, xHi_all, Hiy_all, Vg_sub, Ve_sub, Hessian, a, b, c); - - gsl_matrix_set(V_g, i, j, gsl_matrix_get (Vg_sub, 0, 1)); - gsl_matrix_set(V_g, j, i, gsl_matrix_get (Vg_sub, 0, 1)); - - gsl_matrix_set(V_e, i, j, ve=gsl_matrix_get (Ve_sub, 0, 1)); - gsl_matrix_set(V_e, j, i, ve=gsl_matrix_get (Ve_sub, 0, 1)); - } - } + + // First obtain good initial values. + // Large matrices for EM. + gsl_matrix *U_hat=gsl_matrix_alloc (2, n_size); + gsl_matrix *E_hat=gsl_matrix_alloc (2, n_size); + gsl_matrix *OmegaU=gsl_matrix_alloc (2, n_size); + gsl_matrix *OmegaE=gsl_matrix_alloc (2, n_size); + gsl_matrix *UltVehiY=gsl_matrix_alloc (2, n_size); + gsl_matrix *UltVehiBX=gsl_matrix_alloc (2, n_size); + gsl_matrix *UltVehiU=gsl_matrix_alloc (2, n_size); + gsl_matrix *UltVehiE=gsl_matrix_alloc (2, n_size); + + // Large matrices for NR. Each dxd block is H_k^{-1}. + gsl_matrix *Hi_all=gsl_matrix_alloc (2, 2*n_size); - //free matrices - gsl_matrix_free(U_hat); - gsl_matrix_free(E_hat); - gsl_matrix_free(OmegaU); - gsl_matrix_free(OmegaE); - gsl_matrix_free(UltVehiY); - gsl_matrix_free(UltVehiBX); - gsl_matrix_free(UltVehiU); - gsl_matrix_free(UltVehiE); - - gsl_matrix_free(Hi_all); - gsl_matrix_free(Hiy_all); - gsl_matrix_free(xHi_all); - gsl_matrix_free(Hessian); - - gsl_matrix_free(Y_sub); - gsl_matrix_free(Vg_sub); - gsl_matrix_free(Ve_sub); - gsl_matrix_free(B_sub); + // Each column is H_k^{-1}y_k. + gsl_matrix *Hiy_all=gsl_matrix_alloc (2, n_size); - /* - //second, maximize a increasingly large matrix - for (size_t i=1; i<d_size; i++) { - //large matrices for EM - gsl_matrix *U_hat=gsl_matrix_alloc (i+1, n_size); - gsl_matrix *E_hat=gsl_matrix_alloc (i+1, n_size); - gsl_matrix *OmegaU=gsl_matrix_alloc (i+1, n_size); - gsl_matrix *OmegaE=gsl_matrix_alloc (i+1, n_size); - gsl_matrix *UltVehiY=gsl_matrix_alloc (i+1, n_size); - gsl_matrix *UltVehiBX=gsl_matrix_alloc (i+1, n_size); - gsl_matrix *UltVehiU=gsl_matrix_alloc (i+1, n_size); - gsl_matrix *UltVehiE=gsl_matrix_alloc (i+1, n_size); - - //large matrices for NR - gsl_matrix *Hi_all=gsl_matrix_alloc (i+1, (i+1)*n_size); //each dxd block is H_k^{-1} - gsl_matrix *Hiy_all=gsl_matrix_alloc (i+1, n_size); //each column is H_k^{-1}y_k - gsl_matrix *xHi_all=gsl_matrix_alloc ((i+1)*c_size, (i+1)*n_size); //each dcxdc block is x_k\otimes H_k^{-1} - gsl_matrix *Hessian=gsl_matrix_alloc ((i+1)*(i+2), (i+1)*(i+2)); - - //(i+1) by n matrix of Y - gsl_matrix *Y_sub=gsl_matrix_alloc (i+1, n_size); - gsl_matrix *Vg_sub=gsl_matrix_alloc (i+1, i+1); - gsl_matrix *Ve_sub=gsl_matrix_alloc (i+1, i+1); - gsl_matrix *B_sub=gsl_matrix_alloc (i+1, c_size); - - gsl_matrix_const_view Y_sub_view=gsl_matrix_const_submatrix (Y, 0, 0, i+1, n_size); - gsl_matrix_view Vg_sub_view=gsl_matrix_submatrix (V_g, 0, 0, i+1, i+1); - gsl_matrix_view Ve_sub_view=gsl_matrix_submatrix (V_e, 0, 0, i+1, i+1); - - gsl_matrix_memcpy (Y_sub, &Y_sub_view.matrix); - gsl_matrix_memcpy (Vg_sub, &Vg_sub_view.matrix); - gsl_matrix_memcpy (Ve_sub, &Ve_sub_view.matrix); - - logl=MphEM ('R', em_iter, em_prec, eval, X, Y_sub, U_hat, E_hat, OmegaU, OmegaE, UltVehiY, UltVehiBX, UltVehiU, UltVehiE, Vg_sub, Ve_sub, B_sub); - logl=MphNR ('R', nr_iter, nr_prec, eval, X, Y_sub, Hi_all, xHi_all, Hiy_all, Vg_sub, Ve_sub, Hessian, crt_a, crt_b, crt_c); - - gsl_matrix_memcpy (&Vg_sub_view.matrix, Vg_sub); - gsl_matrix_memcpy (&Ve_sub_view.matrix, Ve_sub); - - //free matrices - gsl_matrix_free(U_hat); - gsl_matrix_free(E_hat); - gsl_matrix_free(OmegaU); - gsl_matrix_free(OmegaE); - gsl_matrix_free(UltVehiY); - gsl_matrix_free(UltVehiBX); - gsl_matrix_free(UltVehiU); - gsl_matrix_free(UltVehiE); - - gsl_matrix_free(Hi_all); - gsl_matrix_free(Hiy_all); - gsl_matrix_free(xHi_all); - gsl_matrix_free(Hessian); - - gsl_matrix_free(Y_sub); - gsl_matrix_free(Vg_sub); - gsl_matrix_free(Ve_sub); - gsl_matrix_free(B_sub); - } - */ + // Each dcxdc block is x_k\otimes H_k^{-1}. + gsl_matrix *xHi_all=gsl_matrix_alloc (2*c_size, 2*n_size); + gsl_matrix *Hessian=gsl_matrix_alloc (6, 6); + + // 2 by n matrix of Y. + gsl_matrix *Y_sub=gsl_matrix_alloc (2, n_size); + gsl_matrix *Vg_sub=gsl_matrix_alloc (2, 2); + gsl_matrix *Ve_sub=gsl_matrix_alloc (2, 2); + gsl_matrix *B_sub=gsl_matrix_alloc (2, c_size); + + for (size_t i=0; i<d_size; i++) { + gsl_vector_view Y_sub1=gsl_matrix_row (Y_sub, 0); + gsl_vector_const_view Y_1=gsl_matrix_const_row (Y, i); + gsl_vector_memcpy (&Y_sub1.vector, &Y_1.vector); + + for (size_t j=i+1; j<d_size; j++) { + gsl_vector_view Y_sub2=gsl_matrix_row (Y_sub, 1); + gsl_vector_const_view Y_2=gsl_matrix_const_row (Y, j); + gsl_vector_memcpy (&Y_sub2.vector, &Y_2.vector); + + gsl_matrix_set_zero (Vg_sub); + gsl_matrix_set_zero (Ve_sub); + gsl_matrix_set (Vg_sub, 0, 0, gsl_matrix_get (V_g, i, i)); + gsl_matrix_set (Ve_sub, 0, 0, gsl_matrix_get (V_e, i, i)); + gsl_matrix_set (Vg_sub, 1, 1, gsl_matrix_get (V_g, j, j)); + gsl_matrix_set (Ve_sub, 1, 1, gsl_matrix_get (V_e, j, j)); + + logl=MphEM ('R', em_iter, em_prec, eval, X, Y_sub, U_hat, + E_hat, OmegaU, OmegaE, UltVehiY, UltVehiBX, + UltVehiU, UltVehiE, Vg_sub, Ve_sub, B_sub); + logl=MphNR ('R', nr_iter, nr_prec, eval, X, Y_sub, Hi_all, + xHi_all, Hiy_all, Vg_sub, Ve_sub, Hessian, a, b, c); + + gsl_matrix_set(V_g, i, j, gsl_matrix_get (Vg_sub, 0, 1)); + gsl_matrix_set(V_g, j, i, gsl_matrix_get (Vg_sub, 0, 1)); + + gsl_matrix_set(V_e, i, j, ve=gsl_matrix_get (Ve_sub, 0, 1)); + gsl_matrix_set(V_e, j, i, ve=gsl_matrix_get (Ve_sub, 0, 1)); + } + } + + // Free matrices. + gsl_matrix_free(U_hat); + gsl_matrix_free(E_hat); + gsl_matrix_free(OmegaU); + gsl_matrix_free(OmegaE); + gsl_matrix_free(UltVehiY); + gsl_matrix_free(UltVehiBX); + gsl_matrix_free(UltVehiU); + gsl_matrix_free(UltVehiE); + + gsl_matrix_free(Hi_all); + gsl_matrix_free(Hiy_all); + gsl_matrix_free(xHi_all); + gsl_matrix_free(Hessian); + + gsl_matrix_free(Y_sub); + gsl_matrix_free(Vg_sub); + gsl_matrix_free(Ve_sub); + gsl_matrix_free(B_sub); } - //calculate B hat using GSL estimate + // Calculate B hat using GSL estimate. gsl_matrix *UltVehiY=gsl_matrix_alloc (d_size, n_size); gsl_vector *D_l=gsl_vector_alloc (d_size); @@ -2829,43 +2914,42 @@ void MphInitial(const size_t em_iter, const double em_prec, const size_t nr_iter double logdet_Ve, logdet_Q, dl, d, delta, dx, dy; - //eigen decomposition and calculate log|Ve| + // Eigen decomposition and calculate log|Ve|. logdet_Ve=EigenProc (V_g, V_e, D_l, UltVeh, UltVehi); - //calculate Qi and log|Q| + // Calculate Qi and log|Q|. logdet_Q=CalcQi (eval, D_l, X, Qi); - //calculate UltVehiY - gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, UltVehi, Y, 0.0, UltVehiY); + // Calculate UltVehiY. + gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1.0,UltVehi,Y,0.0,UltVehiY); //calculate XHiy for (size_t i=0; i<d_size; i++) { - dl=gsl_vector_get(D_l, i); - - for (size_t j=0; j<c_size; j++) { - d=0.0; - for (size_t k=0; k<n_size; k++) { - delta=gsl_vector_get(eval, k); - dx=gsl_matrix_get(X, j, k); - dy=gsl_matrix_get(UltVehiY, i, k); - - //if (delta==0) {continue;} - d+=dy*dx/(delta*dl+1.0); - } - gsl_vector_set(XHiy, j*d_size+i, d); - } + dl=gsl_vector_get(D_l, i); + + for (size_t j=0; j<c_size; j++) { + d=0.0; + for (size_t k=0; k<n_size; k++) { + delta=gsl_vector_get(eval, k); + dx=gsl_matrix_get(X, j, k); + dy=gsl_matrix_get(UltVehiY, i, k); + d+=dy*dx/(delta*dl+1.0); + } + gsl_vector_set(XHiy, j*d_size+i, d); + } } gsl_blas_dgemv(CblasNoTrans, 1.0, Qi, XHiy, 0.0, beta); - //multiply beta by UltVeh and save to B + // Multiply beta by UltVeh and save to B. for (size_t i=0; i<c_size; i++) { - gsl_vector_view B_col=gsl_matrix_column (B, i); - gsl_vector_view beta_sub=gsl_vector_subvector (beta, i*d_size, d_size); - gsl_blas_dgemv(CblasTrans, 1.0, UltVeh, &beta_sub.vector, 0.0, &B_col.vector); + gsl_vector_view B_col=gsl_matrix_column (B, i); + gsl_vector_view beta_sub=gsl_vector_subvector(beta,i*d_size,d_size); + gsl_blas_dgemv(CblasTrans, 1.0, UltVeh, &beta_sub.vector, 0.0, + &B_col.vector); } - //free memory + // Free memory. gsl_matrix_free(UltVehiY); gsl_vector_free(D_l); @@ -2878,12 +2962,10 @@ void MphInitial(const size_t em_iter, const double em_prec, const size_t nr_iter return; } - - -//p value correction -//mode=1 Wald; mode=2 LRT; mode=3 SCORE; -double PCRT (const size_t mode, const size_t d_size, const double p_value, const double crt_a, const double crt_b, const double crt_c) -{ +// p-value correction +// mode=1 Wald; mode=2 LRT; mode=3 SCORE; +double PCRT (const size_t mode, const size_t d_size, const double p_value, + const double crt_a, const double crt_b, const double crt_c) { double p_crt=0.0, chisq_crt=0.0, q=(double)d_size; double chisq=gsl_cdf_chisq_Qinv(p_value, (double)d_size ); @@ -2894,53 +2976,45 @@ double PCRT (const size_t mode, const size_t d_size, const double p_value, const } else if (mode==2) { chisq_crt=chisq/(1.0+crt_a/(2.0*q) ); } else { - /* - double a=-1.0*crt_c/(2.0*q*(q+2.0)); - double b=1.0+(crt_a-crt_b)/(2.0*q); - chisq_crt=(-1.0*b+sqrt(b*b+4.0*a*chisq))/(2.0*a); - */ chisq_crt=chisq; } p_crt=gsl_cdf_chisq_Q (chisq_crt, (double)d_size ); - //cout<<crt_a<<"\t"<<crt_b<<"\t"<<crt_c<<endl; - //cout<<setprecision(10)<<p_value<<"\t"<<p_crt<<endl; - return p_crt; } -// WJA added -#include <assert.h> -void MVLMM::Analyzebgen (const gsl_matrix *U, const gsl_vector *eval, const gsl_matrix *UtW, const gsl_matrix *UtY) -{ +// WJA added. +void MVLMM::Analyzebgen (const gsl_matrix *U, const gsl_vector *eval, + const gsl_matrix *UtW, const gsl_matrix *UtY) { string file_bgen=file_oxford+".bgen"; ifstream infile (file_bgen.c_str(), ios::binary); - if (!infile) {cout<<"error reading bgen file:"<<file_bgen<<endl; return;} + if (!infile) { + cout<<"error reading bgen file:"<<file_bgen<<endl; + return; + } clock_t time_start=clock(); time_UtX=0; time_opt=0; string line; - //create a large matrix + // Create a large matrix. size_t msize=10000; gsl_matrix *Xlarge=gsl_matrix_alloc (U->size1, msize); gsl_matrix *UtXlarge=gsl_matrix_alloc (U->size1, msize); gsl_matrix_set_zero(Xlarge); - // double lambda_mle=0, lambda_remle=0, beta=0, se=0, ; double logl_H0=0.0, logl_H1=0.0, p_wald=0, p_lrt=0, p_score=0; double crt_a, crt_b, crt_c; int n_miss, c_phen; double geno, x_mean; size_t c=0; - // double s=0.0; size_t n_size=UtY->size1, d_size=UtY->size2, c_size=UtW->size2; size_t dc_size=d_size*(c_size+1), v_size=d_size*(d_size+1)/2; - //large matrices for EM + // Large matrices for EM. gsl_matrix *U_hat=gsl_matrix_alloc (d_size, n_size); gsl_matrix *E_hat=gsl_matrix_alloc (d_size, n_size); gsl_matrix *OmegaU=gsl_matrix_alloc (d_size, n_size); @@ -2950,12 +3024,15 @@ void MVLMM::Analyzebgen (const gsl_matrix *U, const gsl_vector *eval, const gsl_ gsl_matrix *UltVehiU=gsl_matrix_alloc (d_size, n_size); gsl_matrix *UltVehiE=gsl_matrix_alloc (d_size, n_size); - //large matrices for NR - gsl_matrix *Hi_all=gsl_matrix_alloc (d_size, d_size*n_size); //each dxd block is H_k^{-1} - gsl_matrix *Hiy_all=gsl_matrix_alloc (d_size, n_size); //each column is H_k^{-1}y_k - gsl_matrix *xHi_all=gsl_matrix_alloc (dc_size, d_size*n_size); //each dcxdc block is x_k\otimes H_k^{-1} - gsl_matrix *Hessian=gsl_matrix_alloc (v_size*2, v_size*2); + // Large matrices for NR. Each dxd block is H_k^{-1}. + gsl_matrix *Hi_all=gsl_matrix_alloc (d_size, d_size*n_size); + + // Each column is H_k^{-1}y_k. + gsl_matrix *Hiy_all=gsl_matrix_alloc (d_size, n_size); + // Each dcxdc block is x_k\otimes H_k^{-1}. + gsl_matrix *xHi_all=gsl_matrix_alloc (dc_size, d_size*n_size); + gsl_matrix *Hessian=gsl_matrix_alloc (v_size*2, v_size*2); gsl_vector *x=gsl_vector_alloc (n_size); gsl_vector *x_miss=gsl_vector_alloc (n_size); @@ -2967,7 +3044,7 @@ void MVLMM::Analyzebgen (const gsl_matrix *U, const gsl_vector *eval, const gsl_ gsl_vector *beta=gsl_vector_alloc (d_size); gsl_matrix *Vbeta=gsl_matrix_alloc (d_size, d_size); - //null estimates for initial values + // Null estimates for initial values. gsl_matrix *V_g_null=gsl_matrix_alloc (d_size, d_size); gsl_matrix *V_e_null=gsl_matrix_alloc (d_size, d_size); gsl_matrix *B_null=gsl_matrix_alloc (d_size, c_size+1); @@ -2975,7 +3052,8 @@ void MVLMM::Analyzebgen (const gsl_matrix *U, const gsl_vector *eval, const gsl_ gsl_matrix_view X_sub=gsl_matrix_submatrix (X, 0, 0, c_size, n_size); gsl_matrix_view B_sub=gsl_matrix_submatrix (B, 0, 0, d_size, c_size); - gsl_matrix_view xHi_all_sub=gsl_matrix_submatrix (xHi_all, 0, 0, d_size*c_size, d_size*n_size); + gsl_matrix_view xHi_all_sub = + gsl_matrix_submatrix (xHi_all, 0, 0, d_size*c_size, d_size*n_size); gsl_matrix_transpose_memcpy (Y, UtY); @@ -2986,30 +3064,37 @@ void MVLMM::Analyzebgen (const gsl_matrix *U, const gsl_vector *eval, const gsl_ gsl_vector_view B_col=gsl_matrix_column(B, c_size); gsl_vector_set_zero(&B_col.vector); - MphInitial(em_iter, em_prec, nr_iter, nr_prec, eval, &X_sub.matrix, Y, l_min, l_max, n_region, V_g, V_e, &B_sub.matrix); - logl_H0=MphEM ('R', em_iter, em_prec, eval, &X_sub.matrix, Y, U_hat, E_hat, OmegaU, OmegaE, UltVehiY, UltVehiBX, UltVehiU, UltVehiE, V_g, V_e, &B_sub.matrix); - logl_H0=MphNR ('R', nr_iter, nr_prec, eval, &X_sub.matrix, Y, Hi_all, &xHi_all_sub.matrix, Hiy_all, V_g, V_e, Hessian, crt_a, crt_b, crt_c); - MphCalcBeta (eval, &X_sub.matrix, Y, V_g, V_e, UltVehiY, &B_sub.matrix, se_B_null); + MphInitial(em_iter, em_prec, nr_iter, nr_prec, eval, &X_sub.matrix, + Y, l_min, l_max, n_region, V_g, V_e, &B_sub.matrix); + logl_H0=MphEM ('R', em_iter, em_prec, eval, &X_sub.matrix, Y, U_hat, + E_hat, OmegaU, OmegaE, UltVehiY, UltVehiBX, UltVehiU, + UltVehiE, V_g, V_e, &B_sub.matrix); + logl_H0=MphNR ('R', nr_iter, nr_prec, eval, &X_sub.matrix, Y, + Hi_all, &xHi_all_sub.matrix, Hiy_all, V_g, V_e, + Hessian, crt_a, crt_b, crt_c); + MphCalcBeta (eval, &X_sub.matrix, Y, V_g, V_e, UltVehiY, + &B_sub.matrix, se_B_null); c=0; Vg_remle_null.clear(); Ve_remle_null.clear(); for (size_t i=0; i<d_size; i++) { - for (size_t j=i; j<d_size; j++) { - Vg_remle_null.push_back(gsl_matrix_get (V_g, i, j) ); - Ve_remle_null.push_back(gsl_matrix_get (V_e, i, j) ); - VVg_remle_null.push_back(gsl_matrix_get (Hessian, c, c) ); - VVe_remle_null.push_back(gsl_matrix_get (Hessian, c+v_size, c+v_size) ); - c++; - } + for (size_t j=i; j<d_size; j++) { + Vg_remle_null.push_back(gsl_matrix_get (V_g, i, j) ); + Ve_remle_null.push_back(gsl_matrix_get (V_e, i, j) ); + VVg_remle_null.push_back(gsl_matrix_get (Hessian, c, c) ); + VVe_remle_null.push_back(gsl_matrix_get (Hessian, c+v_size, + c+v_size) ); + c++; + } } beta_remle_null.clear(); se_beta_remle_null.clear(); for (size_t i=0; i<se_B_null->size1; i++) { - for (size_t j=0; j<se_B_null->size2; j++) { - beta_remle_null.push_back(gsl_matrix_get(B, i, j) ); - se_beta_remle_null.push_back(gsl_matrix_get(se_B_null, i, j) ); - } + for (size_t j=0; j<se_B_null->size2; j++) { + beta_remle_null.push_back(gsl_matrix_get(B, i, j) ); + se_beta_remle_null.push_back(gsl_matrix_get(se_B_null, i, j) ); + } } logl_remle_H0=logl_H0; @@ -3040,91 +3125,96 @@ void MVLMM::Analyzebgen (const gsl_matrix *U, const gsl_vector *eval, const gsl_ } cout<<"se(Ve): "<<endl; for (size_t i=0; i<d_size; i++) { - for (size_t j=0; j<=i; j++) { - c=GetIndex(i, j, d_size); - cout<<sqrt(gsl_matrix_get(Hessian, c+v_size, c+v_size))<<"\t"; - } - cout<<endl; + for (size_t j=0; j<=i; j++) { + c=GetIndex(i, j, d_size); + cout<<sqrt(gsl_matrix_get(Hessian, c+v_size, c+v_size))<<"\t"; + } + cout<<endl; } cout<<"REMLE likelihood = "<<logl_H0<<endl; - logl_H0=MphEM ('L', em_iter, em_prec, eval, &X_sub.matrix, Y, U_hat, E_hat, OmegaU, OmegaE, UltVehiY, UltVehiBX, UltVehiU, UltVehiE, V_g, V_e, &B_sub.matrix); - logl_H0=MphNR ('L', nr_iter, nr_prec, eval, &X_sub.matrix, Y, Hi_all, &xHi_all_sub.matrix, Hiy_all, V_g, V_e, Hessian, crt_a, crt_b, crt_c); - MphCalcBeta (eval, &X_sub.matrix, Y, V_g, V_e, UltVehiY, &B_sub.matrix, se_B_null); + logl_H0=MphEM ('L', em_iter, em_prec, eval, &X_sub.matrix, Y, U_hat, + E_hat, OmegaU, OmegaE, UltVehiY, UltVehiBX, UltVehiU, + UltVehiE, V_g, V_e, &B_sub.matrix); + logl_H0=MphNR ('L', nr_iter, nr_prec, eval, &X_sub.matrix, Y, + Hi_all, &xHi_all_sub.matrix, Hiy_all, V_g, V_e, + Hessian, crt_a, crt_b, crt_c); + MphCalcBeta (eval, &X_sub.matrix, Y, V_g, V_e, UltVehiY, + &B_sub.matrix, se_B_null); c=0; Vg_mle_null.clear(); Ve_mle_null.clear(); for (size_t i=0; i<d_size; i++) { - for (size_t j=i; j<d_size; j++) { - Vg_mle_null.push_back(gsl_matrix_get (V_g, i, j) ); - Ve_mle_null.push_back(gsl_matrix_get (V_e, i, j) ); - VVg_mle_null.push_back(gsl_matrix_get (Hessian, c, c) ); - VVe_mle_null.push_back(gsl_matrix_get (Hessian, c+v_size, c+v_size) ); - c++; - } + for (size_t j=i; j<d_size; j++) { + Vg_mle_null.push_back(gsl_matrix_get (V_g, i, j) ); + Ve_mle_null.push_back(gsl_matrix_get (V_e, i, j) ); + VVg_mle_null.push_back(gsl_matrix_get (Hessian, c, c) ); + VVe_mle_null.push_back(gsl_matrix_get(Hessian,c+v_size,c+v_size)); + c++; + } } beta_mle_null.clear(); se_beta_mle_null.clear(); for (size_t i=0; i<se_B_null->size1; i++) { - for (size_t j=0; j<se_B_null->size2; j++) { - beta_mle_null.push_back(gsl_matrix_get(B, i, j) ); - se_beta_mle_null.push_back(gsl_matrix_get(se_B_null, i, j) ); - } + for (size_t j=0; j<se_B_null->size2; j++) { + beta_mle_null.push_back(gsl_matrix_get(B, i, j) ); + se_beta_mle_null.push_back(gsl_matrix_get(se_B_null, i, j) ); + } } logl_mle_H0=logl_H0; cout<<"MLE estimate for Vg in the null model: "<<endl; for (size_t i=0; i<d_size; i++) { - for (size_t j=0; j<=i; j++) { - cout<<gsl_matrix_get(V_g, i, j)<<"\t"; - } - cout<<endl; + for (size_t j=0; j<=i; j++) { + cout<<gsl_matrix_get(V_g, i, j)<<"\t"; + } + cout<<endl; } cout<<"se(Vg): "<<endl; for (size_t i=0; i<d_size; i++) { - for (size_t j=0; j<=i; j++) { - c=GetIndex(i, j, d_size); - cout<<sqrt(gsl_matrix_get(Hessian, c, c))<<"\t"; - } - cout<<endl; + for (size_t j=0; j<=i; j++) { + c=GetIndex(i, j, d_size); + cout<<sqrt(gsl_matrix_get(Hessian, c, c))<<"\t"; + } + cout<<endl; } cout<<"MLE estimate for Ve in the null model: "<<endl; for (size_t i=0; i<d_size; i++) { - for (size_t j=0; j<=i; j++) { - cout<<gsl_matrix_get(V_e, i, j)<<"\t"; - } - cout<<endl; + for (size_t j=0; j<=i; j++) { + cout<<gsl_matrix_get(V_e, i, j)<<"\t"; + } + cout<<endl; } cout<<"se(Ve): "<<endl; for (size_t i=0; i<d_size; i++) { - for (size_t j=0; j<=i; j++) { - c=GetIndex(i, j, d_size); - cout<<sqrt(gsl_matrix_get(Hessian, c+v_size, c+v_size))<<"\t"; - } - cout<<endl; + for (size_t j=0; j<=i; j++) { + c=GetIndex(i, j, d_size); + cout<<sqrt(gsl_matrix_get(Hessian, c+v_size, c+v_size))<<"\t"; + } + cout<<endl; } cout<<"MLE likelihood = "<<logl_H0<<endl; vector<double> v_beta, v_Vg, v_Ve, v_Vbeta; for (size_t i=0; i<d_size; i++) { - v_beta.push_back(0.0); + v_beta.push_back(0.0); } for (size_t i=0; i<d_size; i++) { - for (size_t j=i; j<d_size; j++) { - v_Vg.push_back(0.0); - v_Ve.push_back(0.0); - v_Vbeta.push_back(0.0); - } + for (size_t j=i; j<d_size; j++) { + v_Vg.push_back(0.0); + v_Ve.push_back(0.0); + v_Vbeta.push_back(0.0); + } } gsl_matrix_memcpy (V_g_null, V_g); gsl_matrix_memcpy (V_e_null, V_e); gsl_matrix_memcpy (B_null, B); - // read in header + // Read in header. uint32_t bgen_snp_block_offset; uint32_t bgen_header_length; uint32_t bgen_nsamples; @@ -3142,11 +3232,11 @@ void MVLMM::Analyzebgen (const gsl_matrix *U, const gsl_vector *eval, const gsl_ infile.read(reinterpret_cast<char*>(&bgen_flags),4); bgen_snp_block_offset-=4; bool CompressedSNPBlocks=bgen_flags&0x1; -// bool LongIds=bgen_flags&0x4; infile.ignore(bgen_snp_block_offset); - double bgen_geno_prob_AA, bgen_geno_prob_AB, bgen_geno_prob_BB, bgen_geno_prob_non_miss; + double bgen_geno_prob_AA, bgen_geno_prob_AB, bgen_geno_prob_BB; + double bgen_geno_prob_non_miss; uint32_t bgen_N; uint16_t bgen_LS; @@ -3162,234 +3252,241 @@ void MVLMM::Analyzebgen (const gsl_matrix *U, const gsl_vector *eval, const gsl_ string id; string rs; string chr; - std::cout<<"Warning: WJA hard coded SNP missingness threshold of 10%"<<std::endl; - - + std::cout<<"Warning: WJA hard coded SNP missingness threshold "<< + "of 10%"<<std::endl; - //start reading genotypes and analyze + // Start reading genotypes and analyze. size_t csnp=0, t_last=0; for (size_t t=0; t<indicator_snp.size(); ++t) { if (indicator_snp[t]==0) {continue;} t_last++; } for (size_t t=0; t<indicator_snp.size(); ++t) { + if (t%d_pace==0 || t==(ns_total-1)) { + ProgressBar ("Reading SNPs ", t, ns_total-1); + } + if (indicator_snp[t]==0) {continue;} + + // Read SNP header. + id.clear(); + rs.clear(); + chr.clear(); + bgen_A_allele.clear(); + bgen_B_allele.clear(); + + infile.read(reinterpret_cast<char*>(&bgen_N),4); + infile.read(reinterpret_cast<char*>(&bgen_LS),2); + + id.resize(bgen_LS); + infile.read(&id[0], bgen_LS); + + infile.read(reinterpret_cast<char*>(&bgen_LR),2); + rs.resize(bgen_LR); + infile.read(&rs[0], bgen_LR); + + infile.read(reinterpret_cast<char*>(&bgen_LC),2); + chr.resize(bgen_LC); + infile.read(&chr[0], bgen_LC); + + infile.read(reinterpret_cast<char*>(&bgen_SNP_pos),4); + + infile.read(reinterpret_cast<char*>(&bgen_LA),4); + bgen_A_allele.resize(bgen_LA); + infile.read(&bgen_A_allele[0], bgen_LA); + + infile.read(reinterpret_cast<char*>(&bgen_LB),4); + bgen_B_allele.resize(bgen_LB); + infile.read(&bgen_B_allele[0], bgen_LB); + + uint16_t unzipped_data[3*bgen_N]; + + if (indicator_snp[t]==0) { + if(CompressedSNPBlocks) + infile.read(reinterpret_cast<char*>(&bgen_P),4); + else + bgen_P=6*bgen_N; + + infile.ignore(static_cast<size_t>(bgen_P)); + + continue; + } + if(CompressedSNPBlocks) { -// if (t>1) {break;} - if (t%d_pace==0 || t==(ns_total-1)) {ProgressBar ("Reading SNPs ", t, ns_total-1);} - if (indicator_snp[t]==0) {continue;} - // read SNP header - id.clear(); - rs.clear(); - chr.clear(); - bgen_A_allele.clear(); - bgen_B_allele.clear(); - - infile.read(reinterpret_cast<char*>(&bgen_N),4); - infile.read(reinterpret_cast<char*>(&bgen_LS),2); - - id.resize(bgen_LS); - infile.read(&id[0], bgen_LS); - - infile.read(reinterpret_cast<char*>(&bgen_LR),2); - rs.resize(bgen_LR); - infile.read(&rs[0], bgen_LR); - - infile.read(reinterpret_cast<char*>(&bgen_LC),2); - chr.resize(bgen_LC); - infile.read(&chr[0], bgen_LC); - - infile.read(reinterpret_cast<char*>(&bgen_SNP_pos),4); - - infile.read(reinterpret_cast<char*>(&bgen_LA),4); - bgen_A_allele.resize(bgen_LA); - infile.read(&bgen_A_allele[0], bgen_LA); - - - infile.read(reinterpret_cast<char*>(&bgen_LB),4); - bgen_B_allele.resize(bgen_LB); - infile.read(&bgen_B_allele[0], bgen_LB); - - - - - uint16_t unzipped_data[3*bgen_N]; - - if (indicator_snp[t]==0) { - if(CompressedSNPBlocks) - infile.read(reinterpret_cast<char*>(&bgen_P),4); - else - bgen_P=6*bgen_N; - - infile.ignore(static_cast<size_t>(bgen_P)); - - continue; - } - - - if(CompressedSNPBlocks) - { - - - infile.read(reinterpret_cast<char*>(&bgen_P),4); - uint8_t zipped_data[bgen_P]; - - unzipped_data_size=6*bgen_N; - - infile.read(reinterpret_cast<char*>(zipped_data),bgen_P); - - int result=uncompress(reinterpret_cast<Bytef*>(unzipped_data), reinterpret_cast<uLongf*>(&unzipped_data_size), reinterpret_cast<Bytef*>(zipped_data), static_cast<uLong> (bgen_P)); - assert(result == Z_OK); - - } - else - { - - bgen_P=6*bgen_N; - infile.read(reinterpret_cast<char*>(unzipped_data),bgen_P); - } - - x_mean=0.0; c_phen=0; n_miss=0; - gsl_vector_set_zero(x_miss); - for (size_t i=0; i<bgen_N; ++i) { - if (indicator_idv[i]==0) {continue;} - - - bgen_geno_prob_AA=static_cast<double>(unzipped_data[i*3])/32768.0; - bgen_geno_prob_AB=static_cast<double>(unzipped_data[i*3+1])/32768.0; - bgen_geno_prob_BB=static_cast<double>(unzipped_data[i*3+2])/32768.0; - // WJA - bgen_geno_prob_non_miss=bgen_geno_prob_AA+bgen_geno_prob_AB+bgen_geno_prob_BB; - if (bgen_geno_prob_non_miss<0.9) {gsl_vector_set(x_miss, c_phen, 0.0); n_miss++;} - else { - - bgen_geno_prob_AA/=bgen_geno_prob_non_miss; - bgen_geno_prob_AB/=bgen_geno_prob_non_miss; - bgen_geno_prob_BB/=bgen_geno_prob_non_miss; + infile.read(reinterpret_cast<char*>(&bgen_P),4); + uint8_t zipped_data[bgen_P]; + + unzipped_data_size=6*bgen_N; + + infile.read(reinterpret_cast<char*>(zipped_data),bgen_P); + + int result=uncompress(reinterpret_cast<Bytef*>(unzipped_data), + reinterpret_cast<uLongf*>(&unzipped_data_size), + reinterpret_cast<Bytef*>(zipped_data), + static_cast<uLong> (bgen_P)); + assert(result == Z_OK); + + } else { + + bgen_P=6*bgen_N; + infile.read(reinterpret_cast<char*>(unzipped_data),bgen_P); + } + + x_mean=0.0; c_phen=0; n_miss=0; + gsl_vector_set_zero(x_miss); + for (size_t i=0; i<bgen_N; ++i) { + if (indicator_idv[i]==0) {continue;} + + bgen_geno_prob_AA = + static_cast<double>(unzipped_data[i*3])/32768.0; + bgen_geno_prob_AB = + static_cast<double>(unzipped_data[i*3+1])/32768.0; + bgen_geno_prob_BB = + static_cast<double>(unzipped_data[i*3+2])/32768.0; + + // WJA. + bgen_geno_prob_non_miss=bgen_geno_prob_AA + + bgen_geno_prob_AB+bgen_geno_prob_BB; + if (bgen_geno_prob_non_miss<0.9) { + gsl_vector_set(x_miss, c_phen, 0.0); + n_miss++; + } + else { + + bgen_geno_prob_AA/=bgen_geno_prob_non_miss; + bgen_geno_prob_AB/=bgen_geno_prob_non_miss; + bgen_geno_prob_BB/=bgen_geno_prob_non_miss; + + geno=2.0*bgen_geno_prob_BB+bgen_geno_prob_AB; + + gsl_vector_set(x, c_phen, geno); + gsl_vector_set(x_miss, c_phen, 1.0); + x_mean+=geno; + } + c_phen++; + } + + x_mean/=static_cast<double>(ni_test-n_miss); + + for (size_t i=0; i<ni_test; ++i) { + if (gsl_vector_get (x_miss, i)==0) {gsl_vector_set(x, i, x_mean);} + } - geno=2.0*bgen_geno_prob_BB+bgen_geno_prob_AB; + gsl_vector_view Xlarge_col=gsl_matrix_column (Xlarge, csnp%msize); + gsl_vector_memcpy (&Xlarge_col.vector, x); + csnp++; - gsl_vector_set(x, c_phen, geno); - gsl_vector_set(x_miss, c_phen, 1.0); - x_mean+=geno; - } - c_phen++; + if (csnp%msize==0 || csnp==t_last ) { + size_t l=0; + if (csnp%msize==0) {l=msize;} else {l=csnp%msize;} + + gsl_matrix_view Xlarge_sub = + gsl_matrix_submatrix(Xlarge, 0, 0, Xlarge->size1, l); + gsl_matrix_view UtXlarge_sub = + gsl_matrix_submatrix(UtXlarge, 0, 0, UtXlarge->size1, l); + + time_start=clock(); + eigenlib_dgemm ("T", "N", 1.0, U, &Xlarge_sub.matrix, 0.0, + &UtXlarge_sub.matrix); + time_UtX+=(clock()-time_start)/(double(CLOCKS_PER_SEC)*60.0); + + gsl_matrix_set_zero (Xlarge); + + for (size_t i=0; i<l; i++) { + gsl_vector_view UtXlarge_col=gsl_matrix_column (UtXlarge, i); + gsl_vector_memcpy (&X_row.vector, &UtXlarge_col.vector); + + // Initial values. + gsl_matrix_memcpy (V_g, V_g_null); + gsl_matrix_memcpy (V_e, V_e_null); + gsl_matrix_memcpy (B, B_null); + + time_start=clock(); + + // 3 is before 1 + if (a_mode==3 || a_mode==4) { + p_score=MphCalcP (eval, &X_row.vector, &X_sub.matrix, Y, + V_g_null, V_e_null, UltVehiY, beta, Vbeta); + if (p_score<p_nr && crt==1) { + logl_H1=MphNR ('R', 1, nr_prec*10, eval, X, Y, Hi_all, + xHi_all, Hiy_all, V_g, V_e, Hessian, crt_a, + crt_b, crt_c); + p_score=PCRT (3, d_size, p_score, crt_a, crt_b, crt_c); + } + } + + if (a_mode==2 || a_mode==4) { + logl_H1=MphEM ('L', em_iter/10, em_prec*10, eval, X, Y, + U_hat, E_hat, OmegaU, OmegaE, UltVehiY, + UltVehiBX, UltVehiU, UltVehiE, V_g, V_e, B); + + // Calculate beta and Vbeta. + p_lrt=MphCalcP (eval, &X_row.vector, &X_sub.matrix, Y, V_g, + V_e, UltVehiY, beta, Vbeta); + p_lrt=gsl_cdf_chisq_Q (2.0*(logl_H1-logl_H0), (double)d_size ); + + if (p_lrt<p_nr) { + logl_H1=MphNR ('L', nr_iter/10, nr_prec*10, eval, X, Y, + Hi_all, xHi_all, Hiy_all, V_g, V_e, Hessian, + crt_a, crt_b, crt_c); + + // Calculate beta and Vbeta. + p_lrt=MphCalcP (eval, &X_row.vector, &X_sub.matrix, Y, V_g, + V_e, UltVehiY, beta, Vbeta); + p_lrt=gsl_cdf_chisq_Q (2.0*(logl_H1-logl_H0), + (double)d_size ); + + if (crt==1) { + p_lrt=PCRT (2, d_size, p_lrt, crt_a, crt_b, crt_c); + } } - - x_mean/=static_cast<double>(ni_test-n_miss); - - for (size_t i=0; i<ni_test; ++i) { - if (gsl_vector_get (x_miss, i)==0) {gsl_vector_set(x, i, x_mean);} - //geno=gsl_vector_get(x, i); - //if (x_mean>1) { - //gsl_vector_set(x, i, 2-geno); - //} + } + + if (a_mode==1 || a_mode==4) { + logl_H1=MphEM ('R', em_iter/10, em_prec*10, eval, X, Y, U_hat, + E_hat, OmegaU, OmegaE, UltVehiY, UltVehiBX, + UltVehiU, UltVehiE, V_g, V_e, B); + p_wald=MphCalcP (eval, &X_row.vector, &X_sub.matrix, Y, V_g, + V_e, UltVehiY, beta, Vbeta); + + if (p_wald<p_nr) { + logl_H1=MphNR ('R', nr_iter/10, nr_prec*10, eval, X, Y, + Hi_all, xHi_all, Hiy_all, V_g, V_e, Hessian, + crt_a, crt_b, crt_c); + p_wald=MphCalcP (eval, &X_row.vector, &X_sub.matrix, Y, + V_g, V_e, UltVehiY, beta, Vbeta); + + if (crt==1) { + p_wald=PCRT (1, d_size, p_wald, crt_a, crt_b, crt_c); + } } + } - /* - //calculate statistics - time_start=clock(); - gsl_blas_dgemv (CblasTrans, 1.0, U, x, 0.0, &X_row.vector); - time_UtX+=(clock()-time_start)/(double(CLOCKS_PER_SEC)*60.0); - */ - - gsl_vector_view Xlarge_col=gsl_matrix_column (Xlarge, csnp%msize); - gsl_vector_memcpy (&Xlarge_col.vector, x); - csnp++; - - if (csnp%msize==0 || csnp==t_last ) { - size_t l=0; - if (csnp%msize==0) {l=msize;} else {l=csnp%msize;} - - gsl_matrix_view Xlarge_sub=gsl_matrix_submatrix(Xlarge, 0, 0, Xlarge->size1, l); - gsl_matrix_view UtXlarge_sub=gsl_matrix_submatrix(UtXlarge, 0, 0, UtXlarge->size1, l); + time_opt+=(clock()-time_start)/(double(CLOCKS_PER_SEC)*60.0); - time_start=clock(); - eigenlib_dgemm ("T", "N", 1.0, U, &Xlarge_sub.matrix, 0.0, &UtXlarge_sub.matrix); - time_UtX+=(clock()-time_start)/(double(CLOCKS_PER_SEC)*60.0); - - gsl_matrix_set_zero (Xlarge); - - for (size_t i=0; i<l; i++) { - gsl_vector_view UtXlarge_col=gsl_matrix_column (UtXlarge, i); - gsl_vector_memcpy (&X_row.vector, &UtXlarge_col.vector); - - //initial values - gsl_matrix_memcpy (V_g, V_g_null); - gsl_matrix_memcpy (V_e, V_e_null); - gsl_matrix_memcpy (B, B_null); - - time_start=clock(); - - //3 is before 1 - if (a_mode==3 || a_mode==4) { - p_score=MphCalcP (eval, &X_row.vector, &X_sub.matrix, Y, V_g_null, V_e_null, UltVehiY, beta, Vbeta); - if (p_score<p_nr && crt==1) { - logl_H1=MphNR ('R', 1, nr_prec*10, eval, X, Y, Hi_all, xHi_all, Hiy_all, V_g, V_e, Hessian, crt_a, crt_b, crt_c); - p_score=PCRT (3, d_size, p_score, crt_a, crt_b, crt_c); - } - } - - if (a_mode==2 || a_mode==4) { - logl_H1=MphEM ('L', em_iter/10, em_prec*10, eval, X, Y, U_hat, E_hat, OmegaU, OmegaE, UltVehiY, UltVehiBX, UltVehiU, UltVehiE, V_g, V_e, B); - //calculate beta and Vbeta - p_lrt=MphCalcP (eval, &X_row.vector, &X_sub.matrix, Y, V_g, V_e, UltVehiY, beta, Vbeta); - p_lrt=gsl_cdf_chisq_Q (2.0*(logl_H1-logl_H0), (double)d_size ); - - if (p_lrt<p_nr) { - logl_H1=MphNR ('L', nr_iter/10, nr_prec*10, eval, X, Y, Hi_all, xHi_all, Hiy_all, V_g, V_e, Hessian, crt_a, crt_b, crt_c); - //calculate beta and Vbeta - p_lrt=MphCalcP (eval, &X_row.vector, &X_sub.matrix, Y, V_g, V_e, UltVehiY, beta, Vbeta); - p_lrt=gsl_cdf_chisq_Q (2.0*(logl_H1-logl_H0), (double)d_size ); - - if (crt==1) { - p_lrt=PCRT (2, d_size, p_lrt, crt_a, crt_b, crt_c); - } - } - } - - if (a_mode==1 || a_mode==4) { - logl_H1=MphEM ('R', em_iter/10, em_prec*10, eval, X, Y, U_hat, E_hat, OmegaU, OmegaE, UltVehiY, UltVehiBX, UltVehiU, UltVehiE, V_g, V_e, B); - p_wald=MphCalcP (eval, &X_row.vector, &X_sub.matrix, Y, V_g, V_e, UltVehiY, beta, Vbeta); - - if (p_wald<p_nr) { - logl_H1=MphNR ('R', nr_iter/10, nr_prec*10, eval, X, Y, Hi_all, xHi_all, Hiy_all, V_g, V_e, Hessian, crt_a, crt_b, crt_c); - p_wald=MphCalcP (eval, &X_row.vector, &X_sub.matrix, Y, V_g, V_e, UltVehiY, beta, Vbeta); - - if (crt==1) { - p_wald=PCRT (1, d_size, p_wald, crt_a, crt_b, crt_c); - } - } - } - - //if (x_mean>1) {gsl_vector_scale(beta, -1.0);} - - time_opt+=(clock()-time_start)/(double(CLOCKS_PER_SEC)*60.0); - - //store summary data - //SUMSTAT SNPs={snpInfo[t].get_chr(), snpInfo[t].get_rs(), snpInfo[t].get_pos(), n_miss, beta, se, lambda_remle, lambda_mle, p_wald, p_lrt, p_score}; - for (size_t i=0; i<d_size; i++) { - v_beta[i]=gsl_vector_get (beta, i); - } - - c=0; - for (size_t i=0; i<d_size; i++) { - for (size_t j=i; j<d_size; j++) { - v_Vg[c]=gsl_matrix_get (V_g, i, j); - v_Ve[c]=gsl_matrix_get (V_e, i, j); - v_Vbeta[c]=gsl_matrix_get (Vbeta, i, j); - c++; - } - } - - MPHSUMSTAT SNPs={v_beta, p_wald, p_lrt, p_score, v_Vg, v_Ve, v_Vbeta}; - sumStat.push_back(SNPs); - } - } + // Store summary data. + for (size_t i=0; i<d_size; i++) { + v_beta[i]=gsl_vector_get (beta, i); + } + + c=0; + for (size_t i=0; i<d_size; i++) { + for (size_t j=i; j<d_size; j++) { + v_Vg[c]=gsl_matrix_get (V_g, i, j); + v_Ve[c]=gsl_matrix_get (V_e, i, j); + v_Vbeta[c]=gsl_matrix_get (Vbeta, i, j); + c++; + } + } + + MPHSUMSTAT SNPs={v_beta, p_wald, p_lrt, p_score, v_Vg, v_Ve, + v_Vbeta}; + sumStat.push_back(SNPs); + } + } } cout<<endl; - infile.close(); infile.clear(); @@ -3429,11 +3526,13 @@ void MVLMM::Analyzebgen (const gsl_matrix *U, const gsl_vector *eval, const gsl_ return; } -void MVLMM::AnalyzeBimbam (const gsl_matrix *U, const gsl_vector *eval, const gsl_matrix *UtW, const gsl_matrix *UtY) -{ +void MVLMM::AnalyzeBimbam (const gsl_matrix *U, const gsl_vector *eval, + const gsl_matrix *UtW, const gsl_matrix *UtY) { igzstream infile (file_geno.c_str(), igzstream::in); -// ifstream infile (file_geno.c_str(), ifstream::in); - if (!infile) {cout<<"error reading genotype file:"<<file_geno<<endl; return;} + if (!infile) { + cout<<"error reading genotype file:"<<file_geno<<endl; + return; + } clock_t time_start=clock(); time_UtX=0; time_opt=0; @@ -3441,24 +3540,22 @@ void MVLMM::AnalyzeBimbam (const gsl_matrix *U, const gsl_vector *eval, const gs string line; char *ch_ptr; - // double lambda_mle=0, lambda_remle=0, beta=0, se=0, ; double logl_H0=0.0, logl_H1=0.0, p_wald=0, p_lrt=0, p_score=0; double crt_a, crt_b, crt_c; int n_miss, c_phen; double geno, x_mean; size_t c=0; - // double s=0.0; size_t n_size=UtY->size1, d_size=UtY->size2, c_size=UtW->size2; size_t dc_size=d_size*(c_size+1), v_size=d_size*(d_size+1)/2; - //create a large matrix + // Create a large matrix. size_t msize=10000; gsl_matrix *Xlarge=gsl_matrix_alloc (U->size1, msize); gsl_matrix *UtXlarge=gsl_matrix_alloc (U->size1, msize); gsl_matrix_set_zero(Xlarge); - //large matrices for EM + // Large matrices for EM. gsl_matrix *U_hat=gsl_matrix_alloc (d_size, n_size); gsl_matrix *E_hat=gsl_matrix_alloc (d_size, n_size); gsl_matrix *OmegaU=gsl_matrix_alloc (d_size, n_size); |