static int CheckSuperskyMetrics(
const gsl_matrix *rssky_metric,
const double rssky_metric_ref[4][4],
const gsl_matrix *rssky_transf,
const double rssky_transf_ref[6][3],
const double phys_mismatch[NUM_POINTS][NUM_POINTS],
const double phys_mismatch_tol
)
{
// Check supersky metrics
{
gsl_matrix_const_view rssky_metric_ref_view = gsl_matrix_const_view_array( ( const double * )rssky_metric_ref, 4, 4 );
const double err = XLALCompareMetrics( rssky_metric, &rssky_metric_ref_view.matrix ), err_tol = 1e-6;
XLAL_CHECK( err <= err_tol, XLAL_ETOL, "'rssky_metric' check failed: err = %0.3e > %0.3e = err_tol", err, err_tol );
}
{
XLAL_CHECK( rssky_transf->size1 == 6 && rssky_transf->size2 == 3, XLAL_ESIZE );
const double err_tol = 1e-5;
for ( size_t i = 0; i < rssky_transf->size1; ++i ) {
for ( size_t j = 0; j < rssky_transf->size2; ++j ) {
const double rssky_transf_ij = gsl_matrix_get( rssky_transf, i, j );
const double rssky_transf_ref_ij = rssky_transf_ref[i][j];
CHECK_RELERR( rssky_transf_ij, rssky_transf_ref_ij, err_tol );
}
}
}
// Check round-trip conversions of each test point
{
gsl_matrix *GAMAT( rssky_points, 4, NUM_POINTS );
for ( size_t j = 0; j < NUM_POINTS; ++j ) {
gsl_vector_view rssky_point = gsl_matrix_column( rssky_points, j );
PulsarDopplerParams XLAL_INIT_DECL( new_phys_point );
XLAL_CHECK( XLALConvertPhysicalToSuperskyPoint( &rssky_point.vector, &phys_points[j], rssky_transf ) == XLAL_SUCCESS, XLAL_EFUNC );
XLAL_CHECK( XLALConvertSuperskyToPhysicalPoint( &new_phys_point, &rssky_point.vector, rssky_transf ) == XLAL_SUCCESS, XLAL_EFUNC );
XLAL_CHECK( CompareDoppler( &phys_points[j], &new_phys_point ) == EXIT_SUCCESS, XLAL_EFUNC );
}
gsl_matrix *intm_phys_points = NULL;
gsl_matrix *new_rssky_points = NULL;
XLAL_CHECK( XLALConvertSuperskyToPhysicalPoints( &intm_phys_points, rssky_points, rssky_transf ) == XLAL_SUCCESS, XLAL_EFUNC );
XLAL_CHECK( XLALConvertPhysicalToSuperskyPoints( &new_rssky_points, intm_phys_points, rssky_transf ) == XLAL_SUCCESS, XLAL_EFUNC );
const double err_tol = 1e-6;
for ( size_t i = 0; i < 4; ++i ) {
for ( size_t j = 0; j < NUM_POINTS; ++j ) {
const double rssky_points_ij = gsl_matrix_get( rssky_points, i, j );
const double new_rssky_points_ij = gsl_matrix_get( new_rssky_points, i, j );
CHECK_RELERR( rssky_points_ij, new_rssky_points_ij, err_tol );
}
}
GFMAT( rssky_points, intm_phys_points, new_rssky_points );
}
// Check mismatches between pairs of points
{
gsl_vector *GAVEC( rssky_point_i, 4 );
gsl_vector *GAVEC( rssky_point_j, 4 );
gsl_vector *GAVEC( temp, 4 );
for ( size_t i = 0; i < NUM_POINTS; ++i ) {
XLAL_CHECK( XLALConvertPhysicalToSuperskyPoint( rssky_point_i, &phys_points[i], rssky_transf ) == XLAL_SUCCESS, XLAL_EFUNC );
for ( size_t j = 0; j < NUM_POINTS; ++j ) {
XLAL_CHECK( XLALConvertPhysicalToSuperskyPoint( rssky_point_j, &phys_points[j], rssky_transf ) == XLAL_SUCCESS, XLAL_EFUNC );
gsl_vector_sub( rssky_point_j, rssky_point_i );
gsl_blas_dgemv( CblasNoTrans, 1.0, rssky_metric, rssky_point_j, 0.0, temp );
double mismatch = 0.0;
gsl_blas_ddot( rssky_point_j, temp, &mismatch );
CHECK_RELERR( mismatch, phys_mismatch[i][j], phys_mismatch_tol );
}
}
GFVEC( rssky_point_i, rssky_point_j, temp );
}
return XLAL_SUCCESS;
}
int
gsl_linalg_matmult_mod (const gsl_matrix * A, gsl_linalg_matrix_mod_t modA,
const gsl_matrix * B, gsl_linalg_matrix_mod_t modB,
gsl_matrix * C)
{
if (modA == GSL_LINALG_MOD_NONE && modB == GSL_LINALG_MOD_NONE)
{
return gsl_linalg_matmult (A, B, C);
}
else
{
size_t dim1_A = A->size1;
size_t dim2_A = A->size2;
size_t dim1_B = B->size1;
size_t dim2_B = B->size2;
size_t dim1_C = C->size1;
size_t dim2_C = C->size2;
if (modA & GSL_LINALG_MOD_TRANSPOSE)
SWAP_SIZE_T (dim1_A, dim2_A);
if (modB & GSL_LINALG_MOD_TRANSPOSE)
SWAP_SIZE_T (dim1_B, dim2_B);
if (dim2_A != dim1_B || dim1_A != dim1_C || dim2_B != dim2_C)
{
GSL_ERROR ("matrix sizes are not conformant", GSL_EBADLEN);
}
else
{
double a, b;
double temp;
size_t i, j, k;
size_t a1, a2, b1, b2;
for (i = 0; i < dim1_C; i++)
{
for (j = 0; j < dim2_C; j++)
{
a1 = i;
a2 = 0;
b1 = 0;
b2 = j;
if (modA & GSL_LINALG_MOD_TRANSPOSE)
SWAP_SIZE_T (a1, a2);
if (modB & GSL_LINALG_MOD_TRANSPOSE)
SWAP_SIZE_T (b1, b2);
a = gsl_matrix_get (A, a1, a2);
b = gsl_matrix_get (B, b1, b2);
temp = a * b;
for (k = 1; k < dim2_A; k++)
{
a1 = i;
a2 = k;
b1 = k;
b2 = j;
if (modA & GSL_LINALG_MOD_TRANSPOSE)
SWAP_SIZE_T (a1, a2);
if (modB & GSL_LINALG_MOD_TRANSPOSE)
SWAP_SIZE_T (b1, b2);
a = gsl_matrix_get (A, a1, a2);
b = gsl_matrix_get (B, b1, b2);
temp += a * b;
}
gsl_matrix_set (C, i, j, temp);
}
}
return GSL_SUCCESS;
}
}
}
double
SymmNMF(gsl_matrix *V,gsl_matrix *H,double beta,int maxiter)
{
gsl_matrix *VV=NULL,*HV=NULL,*HH=NULL,*HHH=NULL;
double u,v,w,r2,r2_old=0,mse=0;
size_t i,j,n,r,iter;
/*
** initialization, matrix H is n x r (with r small, and V = n x n)
*/
n = V->size1;
r = H->size2;
VV = gsl_matrix_calloc (n,n);
if (VV == NULL) VError(" err allocating matrix VV");
HH = gsl_matrix_calloc (r,r);
if (HH == NULL) VError(" err allocating matrix HH");
HHH = gsl_matrix_calloc (n,r);
if (HHH == NULL) VError(" err allocating matrix HHH");
HV = gsl_matrix_calloc (r,n);
if (HV == NULL) VError(" err allocating matrix HV");
/*
** main iteration
*/
fprintf(stderr,"symmetric NMF...\n");
r2_old = r2 = 0;
for (iter=0; iter < maxiter; iter++) {
if (iter%5 == 0) {
r2 = SCheckResults(V,VV,H,&mse);
fprintf(stderr," %5d %10.7f %f\n",iter,r2,mse);
if (ABS(r2-r2_old) < 1.0e-6 && iter > 20) goto ende;
if (r2_old > r2 && iter > 5) goto ende;
r2_old = r2;
}
/* WH */
gsl_blas_dgemm(CblasTrans,CblasTrans,1.0,H,V,0.0,HV);
/* H H^t H */
gsl_blas_dgemm(CblasTrans,CblasNoTrans,1.0,H,H,0.0,HH);
gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1.0,H,HH,0.0,HHH);
/* update H */
for (i=0; i<H->size1; i++) {
for (j=0; j<H->size2; j++) {
u = gsl_matrix_get(H,i,j);
v = gsl_matrix_get(HV,j,i); /* fast to use transpose */
w = gsl_matrix_get(HHH,i,j);
if (w != 0) u *= (1.0 - beta + beta*v/w);
gsl_matrix_set(H,i,j,u);
}
}
}
ende:
gsl_matrix_free(HV);
gsl_matrix_free(HH);
gsl_matrix_free(HHH);
fprintf(stderr," symm ok, r2=%f\n",r2);
return r2;
}
void MAIAllocator::Setup() {
//////// initialization of dynamic data structures
Hmat = gsl_matrix_complex_alloc(N(),M());
Hchan = gsl_vector_complex_alloc(N());
Hperm = gsl_matrix_uint_alloc(N(),M());
p = gsl_permutation_alloc(N());
huserabs = gsl_vector_alloc(N());
nextcarr = gsl_vector_uint_alloc(M());
usedcarr = gsl_vector_uint_alloc(N());
errs = gsl_vector_uint_alloc(M());
habs = gsl_matrix_alloc(N(),M());
huu = gsl_matrix_complex_alloc(N(),M());
framecount = 0;
ericount = 0;
csicount = 0;
noDecisions = 0;
ostringstream cmd;
//
// time
//
time(&reporttime);
//
// Random Generator
//
ran = gsl_rng_alloc( gsl_rng_default );
// SIGNATURE FREQUENCIES INITIAL SETUP
signature_frequencies = gsl_matrix_uint_alloc(M(),J());
signature_frequencies_init = gsl_matrix_uint_alloc(M(),J());
signature_powers = gsl_matrix_alloc(M(),J());
for (int i=0; i<M(); i++)
for (int j=0; j<J(); j++)
gsl_matrix_uint_set(signature_frequencies_init,i,j,(j*M()+i) % N());
//
// INITIAL ALLOCATION
//
gsl_matrix_uint_memcpy(signature_frequencies,
signature_frequencies_init);
// maximum initial powers for all carriers
gsl_matrix_set_all(signature_powers,INIT_CARR_POWER);
gsl_vector_uint_set_zero(errs);
//
//
// FFT Transform Matrix
//
//
transform_mat = gsl_matrix_complex_calloc(N(),N());
double fftarg=-2.0*double(M_PI/N());
double fftamp=1.0/sqrt(double(N()));
for (int i=0; i<N(); i++)
for (int j=0; j<N(); j++)
gsl_matrix_complex_set(transform_mat,i,j,
gsl_complex_polar(fftamp,fftarg*i*j) );
switch (Mode()) {
case 0:
cout << BlockName << " - Allocator type FIXED_ALLOCATION selected" << endl;
break;
case 1:
cout << BlockName << " - Allocator type GIVE_BEST_CARR selected" << endl;
break;
case 2:
cout << BlockName << " - Allocator type SWAP_BAD_GOOD selected" << endl;
break;
case 3:
cout << BlockName << " - Allocator type BEST_OVERLAP selected" << endl;
break;
case 4:
cout << BlockName << " - Allocator type SOAR_AI selected" << endl;
//
// SOAR INITIALIZATION
//
max_errors = MAX_ERROR_RATE * ERROR_REPORT_INTERVAL * Nb() * K();
cout << BlockName << " - Max errors tuned to " << max_errors << " errors/frame." << endl;
//
// first we initialize the vectors and matrices
// in the order of appearance in the header file.
//
umapUserVec = vector < Identifier * > (M());
umapUserUidVec = vector < IntElement * > (M());
umapUserErrsVec = vector < IntElement * > (M());
umapUserPowerVec = vector < FloatElement * > (M());
umapUserCarrMat = vector < Identifier * > (M()*J());
umapUserCarrCidMat = vector < IntElement * > (M()*J());
umapUserCarrPowerMat = vector < FloatElement * > (M()*J());
chansCoeffMat = vector < Identifier * > (M()*N());
chansCoeffUserMat = vector < IntElement * > (M()*N());
chansCoeffCarrMat = vector < IntElement * > (M()*N());
chansCoeffValueMat = vector < FloatElement * > (M()*N());
carmapCarrVec = vector < Identifier * > (N());
carmapCarrCidVec = vector < IntElement * > (N());
//
// then we create an instance of the Soar kernel in our process
//
pKernel = Kernel::CreateKernelInNewThread() ;
//pKernel = Kernel::CreateRemoteConnection() ;
// Check that nothing went wrong. We will always get back a kernel object
// even if something went wrong and we have to abort.
if (pKernel->HadError())
{
cerr << BlockName << ".SOAR - "
<< pKernel->GetLastErrorDescription() << endl ;
exit(1);
}
// We check if an agent has been prevoiusly created, otherwise we create it
// NOTE: We don't delete the agent pointer. It's owned by the kernel
pAgent = pKernel->GetAgent("AIAllocator") ;
if (! pKernel->IsAgentValid(pAgent)) {
pAgent = pKernel->CreateAgent("AIAllocator") ;
}
// Check that nothing went wrong
// NOTE: No agent gets created if there's a problem, so we have to check for
// errors through the kernel object.
if (pKernel->HadError())
{
cerr << BlockName << ".SOAR - " << pKernel->GetLastErrorDescription() << endl ;
exit(1);
}
//
// load productions
//
pAgent->LoadProductions(SoarFn());
// spawn debugger
#ifdef SPAWN_DEBUGGER
pAgent->SpawnDebugger();
#endif
// Check that nothing went wrong
// NOTE: No agent gets created if there's a problem, so we have to check for
// errors through the kernel object.
if (pKernel->HadError())
{
cerr << BlockName << ".SOAR - "
<< pKernel->GetLastErrorDescription() << endl ;
exit(1);
}
// keypress
//cout << "pause maillocator:203 ... (press ENTER key)" << endl;
//cin.ignore();
//
// we can now generate initial input link structure
//
// NO MORE adjust max-nil-output-cycle
//cmd << "max-nil-output-cycles " << 120;
//pAgent->ExecuteCommandLine(cmd.str().c_str());
// the input-link
pInputLink = pAgent->GetInputLink();
// input-time
input_time = 0;
inputTime = pAgent->CreateIntWME(pInputLink,"input-time",input_time);
// the usrmap structure (common wmes)
umap = pAgent->CreateIdWME(pInputLink,"usrmap");
// BITS_PER_REPORT = ERROR_REPORT_INTERVAL * Nb() * K()
// MAX_ERRORS = MAX_ERROR_RATE * BITS_PER_REPORT
umapMaxerr = pAgent->CreateIntWME(umap,"maxerr",max_errors);
umapPstep = pAgent->CreateFloatWME(umap,"pstep",POWER_STEP);
umapPmax = pAgent->CreateFloatWME(umap,"pmax",MAX_POWER);
// the channels
chans = pAgent->CreateIdWME(pInputLink,"channels");
// the carmap
carmap = pAgent->CreateIdWME(pInputLink,"carmap");
// the usrmap structure (users substructure)
for (int i=0;i<M();i++) { // user loop
umapUserVec[i] = pAgent->CreateIdWME(umap,"user");
umapUserUidVec[i] = pAgent->CreateIntWME(umapUserVec[i],"uid",i);
umapUserErrsVec[i] = pAgent->CreateIntWME(umapUserVec[i],"errs",int(0));
umapUserPowerVec[i] = pAgent->CreateFloatWME(umapUserVec[i],"power",J());
// update the current allocation
for (int j=0;j<J();j++) { // allocated carriers loop
unsigned int usedcarr = gsl_matrix_uint_get(signature_frequencies,i,j);
double usedpow = gsl_matrix_get(signature_powers,i,j);
umapUserCarrMat[i*J()+j] = pAgent->CreateIdWME(umapUserVec[i],"carr");
umapUserCarrCidMat[i*J()+j] =
pAgent->CreateIntWME(umapUserCarrMat[i*J()+j],"cid",usedcarr);
umapUserCarrPowerMat[i*J()+j] =
pAgent->CreateFloatWME(umapUserCarrMat[i*J()+j],"power",usedpow);
} // allocated carriers loop
// the channels
for (int j=0;j<N();j++) { // all channels loop
chansCoeffMat[i*N()+j] = pAgent->CreateIdWME(chans,"coeff");
chansCoeffUserMat[i*N()+j] = pAgent->CreateIntWME(chansCoeffMat[i*N()+j],"user",i);
chansCoeffCarrMat[i*N()+j] = pAgent->CreateIntWME(chansCoeffMat[i*N()+j],"carr",j);
chansCoeffValueMat[i*N()+j] = pAgent->CreateFloatWME(chansCoeffMat[i*N()+j],"value",0.0);
} // all channels loop
} // user loop
// the carmap structure
for (int j=0;j<N();j++) { // all carriers loop
carmapCarrVec[j] = pAgent->CreateIdWME(carmap,"carr");
carmapCarrCidVec[j] = pAgent->CreateIntWME(carmapCarrVec[j],"cid",j);
} // all carriers loop
//
// END OF SOAR INITIALIZAZION
//
break;
default:
cerr << BlockName << " - Unhandled allocator type !" << endl;
exit(1);
}
//////// rate declaration for ports
}
int
gsl_linalg_QR_update (gsl_matrix * Q, gsl_matrix * R,
gsl_vector * w, const gsl_vector * v)
{
const size_t M = R->size1;
const size_t N = R->size2;
if (Q->size1 != M || Q->size2 != M)
{
GSL_ERROR ("Q matrix must be M x M if R is M x N", GSL_ENOTSQR);
}
else if (w->size != M)
{
GSL_ERROR ("w must be length M if R is M x N", GSL_EBADLEN);
}
else if (v->size != N)
{
GSL_ERROR ("v must be length N if R is M x N", GSL_EBADLEN);
}
else
{
size_t j, k;
double w0;
/* Apply Given's rotations to reduce w to (|w|, 0, 0, ... , 0)
J_1^T .... J_(n-1)^T w = +/- |w| e_1
simultaneously applied to R, H = J_1^T ... J^T_(n-1) R
so that H is upper Hessenberg. (12.5.2) */
for (k = M - 1; k > 0; k--) /* loop from k = M-1 to 1 */
{
double c, s;
double wk = gsl_vector_get (w, k);
double wkm1 = gsl_vector_get (w, k - 1);
gsl_linalg_givens (wkm1, wk, &c, &s);
gsl_linalg_givens_gv (w, k - 1, k, c, s);
apply_givens_qr (M, N, Q, R, k - 1, k, c, s);
}
w0 = gsl_vector_get (w, 0);
/* Add in w v^T (Equation 12.5.3) */
for (j = 0; j < N; j++)
{
double r0j = gsl_matrix_get (R, 0, j);
double vj = gsl_vector_get (v, j);
gsl_matrix_set (R, 0, j, r0j + w0 * vj);
}
/* Apply Givens transformations R' = G_(n-1)^T ... G_1^T H
Equation 12.5.4 */
for (k = 1; k < GSL_MIN(M,N+1); k++)
{
double c, s;
double diag = gsl_matrix_get (R, k - 1, k - 1);
double offdiag = gsl_matrix_get (R, k, k - 1);
gsl_linalg_givens (diag, offdiag, &c, &s);
apply_givens_qr (M, N, Q, R, k - 1, k, c, s);
gsl_matrix_set (R, k, k - 1, 0.0); /* exact zero of G^T */
}
return GSL_SUCCESS;
}
}
cxform_t* seperateColors(image_t*img)
{
int t;
int size = img->width*img->height;
double corr[4][4];
double e[4][4];
double i[4][4];
memset(&corr, 0, sizeof(corr));
cxform_t*cxform = (cxform_t*)malloc(sizeof(cxform_t));
cxform_t inverse;
for(t=0;t<size;t++) {
U8*pixel = (U8*)&img->data[t];
int u,v;
for(u=0;u<4;u++)
for(v=0;v<4;v++) {
corr[u][v] += pixel[u]*pixel[v];
}
}
gsl_matrix *v= gsl_matrix_alloc(4, 4);
gsl_vector *s = gsl_vector_alloc(4);
gsl_vector *tmp= gsl_vector_alloc(4);
gsl_matrix_view m = gsl_matrix_view_array((double*)&corr, 4, 4);
gsl_linalg_SV_decomp(&m.matrix, v, s, tmp);
int x,y,z;
double min=500000,max=-500000;
double rad[4];
for(y=0;y<4;y++) {
double val = gsl_vector_get(s, y);
double min=0,max=0;
for(x=0;x<4;x++) {
i[x][y] = gsl_matrix_get(v, x, y);
e[y][x] = gsl_matrix_get(&m.matrix, x, y);
if(i[x][y]<0) {
min += i[x][y];
} else {
max += i[x][y];
}
}
rad[y] = ((-min)>max?-min:max) * 2.01;
}
/*double tst[4][4];
for(y=0;y<4;y++) {
for(x=0;x<4;x++) {
double sum = 0;
for(z=0;z<4;z++) {
sum += e[x][z] * i[z][y] * 256;
}
printf("%8.4f ", sum);
}
printf("\n");
}*/
gsl_matrix_free(v);
gsl_vector_free(s);
gsl_vector_free(tmp);
cxform_t tr;
tr.rr = i[0][0]/rad[0]; tr.rg = i[1][0]/rad[0]; tr.rb = i[2][0]/rad[0]; tr.ra = i[3][0]/rad[0]; tr.tr = 128;
tr.gr = i[0][1]/rad[1]; tr.gg = i[1][1]/rad[1]; tr.gb = i[2][1]/rad[1]; tr.ga = i[3][1]/rad[1]; tr.tg = 128;
tr.br = i[0][2]/rad[2]; tr.bg = i[1][2]/rad[2]; tr.bb = i[2][2]/rad[2]; tr.ba = i[3][2]/rad[2]; tr.tb = 128;
tr.ar = i[0][3]/rad[3]; tr.ag = i[1][3]/rad[3]; tr.ab = i[2][3]/rad[3]; tr.aa = i[3][3]/rad[3]; tr.ta = 128;
cxform->rr = e[0][0]*rad[0]; cxform->rg = e[1][0]*rad[1]; cxform->rb = e[2][0]*rad[2]; cxform->ra = e[3][0]*rad[3];
cxform->gr = e[0][1]*rad[0]; cxform->gg = e[1][1]*rad[1]; cxform->gb = e[2][1]*rad[2]; cxform->ga = e[3][1]*rad[3];
cxform->br = e[0][2]*rad[0]; cxform->bg = e[1][2]*rad[1]; cxform->bb = e[2][2]*rad[2]; cxform->ba = e[3][2]*rad[3];
cxform->ar = e[0][3]*rad[0]; cxform->ag = e[1][3]*rad[1]; cxform->ab = e[2][3]*rad[2]; cxform->aa = e[3][3]*rad[3];
cxform->tr = - (cxform->rr * tr.tr + cxform->rg * tr.tg + cxform->rb * tr.tb + cxform->ra * tr.ta);
cxform->tg = - (cxform->gr * tr.tr + cxform->gg * tr.tg + cxform->gb * tr.tb + cxform->ga * tr.ta);
cxform->tb = - (cxform->br * tr.tr + cxform->bg * tr.tg + cxform->bb * tr.tb + cxform->ba * tr.ta);
cxform->ta = - (cxform->ar * tr.tr + cxform->ag * tr.tg + cxform->ab * tr.tb + cxform->aa * tr.ta);
gfximage_transform(img, &tr);
//gfximage_transform(img, cxform);
/*gfxcolor_t red;
//red.r = 255; red.g = red.b = red.a = 0;
red.r = lrand48();
red.g = lrand48();
red.b = lrand48();
red.a = lrand48();
printf("%02x%02x%02x%02x\n", red.r,red.g,red.b,red.a);
gfxcolor_transform(&red, &tr);
printf("%02x%02x%02x%02x\n", red.r,red.g,red.b,red.a);
gfxcolor_transform(&red, cxform);
printf("%02x%02x%02x%02x\n", red.r,red.g,red.b,red.a);
RGBA cmin = {255,255,255,255};
RGBA cmax = {0,0,0,0};
for(t=0;t<size;t++) {
if(img->data[t].r < cmin.r) cmin.r = img->data[t].r;
if(img->data[t].g < cmin.g) cmin.g = img->data[t].g;
if(img->data[t].b < cmin.b) cmin.b = img->data[t].b;
if(img->data[t].a < cmin.a) cmin.a = img->data[t].a;
if(img->data[t].r > cmax.r) cmax.r = img->data[t].r;
if(img->data[t].g > cmax.g) cmax.g = img->data[t].g;
if(img->data[t].b > cmax.b) cmax.b = img->data[t].b;
if(img->data[t].a > cmax.a) cmax.a = img->data[t].a;
}*/
return cxform;
}
static VALUE rb_gsl_math_eval2(double (*func)(const double, const double), VALUE xx,
VALUE yy)
{
VALUE x, y, ary;
size_t i, j, size;
gsl_vector *v = NULL, *v2 = NULL, *vnew = NULL;
gsl_matrix *m = NULL, *m2 = NULL, *mnew = NULL;
#ifdef HAVE_NARRAY_H
struct NARRAY *nax, *nay;
double *ptr1, *ptr2, *ptr3;
#endif
if (CLASS_OF(xx) == rb_cRange) xx = rb_gsl_range2ary(xx);
switch (TYPE(xx)) {
case T_FIXNUM:
case T_BIGNUM:
case T_FLOAT:
Need_Float(yy);
return rb_float_new((*func)(NUM2DBL(xx), NUM2DBL(yy)));
break;
case T_ARRAY:
Check_Type(yy, T_ARRAY);
size = RARRAY_LEN(xx);
if (size != RARRAY_LEN(yy)) rb_raise(rb_eRuntimeError, "array sizes are different.");
ary = rb_ary_new2(size);
for (i = 0; i < size; i++) {
x = rb_ary_entry(xx, i);
y = rb_ary_entry(yy, i);
Need_Float(x); Need_Float(y);
rb_ary_store(ary, i, rb_float_new((*func)(RFLOAT_VALUE(x), RFLOAT_VALUE(y))));
}
return ary;
break;
default:
#ifdef HAVE_NARRAY_H
if (NA_IsNArray(xx)) {
GetNArray(xx, nax);
GetNArray(yy, nay);
ptr1 = (double*) nax->ptr;
ptr2 = (double*) nay->ptr;
size = nax->total;
ary = na_make_object(NA_DFLOAT, nax->rank, nax->shape, CLASS_OF(xx));
ptr3 = NA_PTR_TYPE(ary, double*);
for (i = 0; i < size; i++) ptr3[i] = (*func)(ptr1[i], ptr2[i]);
return ary;
}
#endif
if (VECTOR_P(xx)) {
CHECK_VECTOR(yy);
Data_Get_Struct(xx, gsl_vector, v);
Data_Get_Struct(yy, gsl_vector, v2);
vnew = gsl_vector_alloc(v->size);
for (i = 0; i < v->size; i++) {
gsl_vector_set(vnew, i, (*func)(gsl_vector_get(v, i), gsl_vector_get(v2, i)));
}
return Data_Wrap_Struct(cgsl_vector, 0, gsl_vector_free, vnew);
} else if (MATRIX_P(xx)) {
CHECK_MATRIX(yy);
Data_Get_Struct(xx, gsl_matrix, m);
Data_Get_Struct(yy, gsl_matrix, m2);
mnew = gsl_matrix_alloc(m->size1, m->size2);
for (i = 0; i < m->size1; i++) {
for (j = 0; j < m->size2; j++) {
gsl_matrix_set(mnew, i, j, (*func)(gsl_matrix_get(m, i, j), gsl_matrix_get(m2, i, j)));
}
}
return Data_Wrap_Struct(cgsl_matrix, 0, gsl_matrix_free, mnew);
} else {
rb_raise(rb_eTypeError,
"wrong argument type %s "
"(Array or Vector or Matrix expected)", rb_class2name(CLASS_OF(xx)));
}
break;
}
/* never reach here */
return Qnil;
}
int Holling2(double t, const double y[], double ydot[], void *params){
double alpha = 0.3; // respiration
double lambda = 0.65; // ecologic efficiency
double hand = 0.35; // handling time
double beta = 0.5; // intraspecific competition
double aij = 6.0; // attack rate
//double migratingPop = 0.01;
int i, j,l = 0; // Hilfsvariablen
double rowsum = 0;
//double colsum = 0;
// int test = 0;
//
// if(test<5)
// {
// printf("Richtiges Holling");
// }
// test++;
//-- Struktur zerlegen-------------------------------------------------------------------------------------------------------------------------------
struct foodweb *nicheweb = (struct foodweb *)params; // pointer cast from (void*) to (struct foodweb*)
//printf("t in Holling 2=%f\n", t);
gsl_vector *network = (nicheweb->network); // Inhalt: A+linksA+Y+linksY+Massen+Trophische_Level = (Rnum+S)²+1+Y²+1+(Rnum+S)+S
int S = nicheweb->S;
int Y = nicheweb->Y;
int Rnum = nicheweb->Rnum;
//double d = nicheweb->d;
int Z = nicheweb->Z;
//double dij = pow(10, d);
double Bmigr = gsl_vector_get(network, (Rnum+S)*(S+Rnum)+1+Y*Y+1+(Rnum+S)+S);
//printf("Bmigr ist %f\n", Bmigr);
double nu,mu, tau;
int SpeciesNumber;
tau = gsl_vector_get(nicheweb->migrPara,0);
mu = gsl_vector_get(nicheweb->migrPara,1);
// if((int)nu!=0)
// {
// printf("nu ist nicht null sondern %f\n",nu);
// }
nu = gsl_vector_get(nicheweb->migrPara,2);
SpeciesNumber = gsl_vector_get(nicheweb->migrPara,3);
double tlast = gsl_vector_get(nicheweb->migrPara,4);
// if(SpeciesNumber!= 0)
// {
// //printf("SpeciesNumber %i\n", SpeciesNumber);
// }
//printf("t oben %f\n",t);
//int len = (Rnum+S)*(Rnum+S)+2+Y*Y+(Rnum+S)+S;
gsl_vector_view A_view = gsl_vector_subvector(network, 0, (Rnum+S)*(Rnum+S)); // Fressmatrix A als Vektor
gsl_matrix_view EA_mat = gsl_matrix_view_vector(&A_view.vector, (Rnum+S), (Rnum+S)); // A als Matrix_view
gsl_matrix *EAmat = &EA_mat.matrix; // A als Matrix
gsl_vector_view D_view = gsl_vector_subvector(network, (Rnum+S)*(Rnum+S)+1, Y*Y); // Migrationsmatrix D als Vektor
gsl_matrix_view ED_mat = gsl_matrix_view_vector(&D_view.vector, Y, Y); // D als Matrixview
gsl_matrix *EDmat = &ED_mat.matrix; // D als Matrix
gsl_vector_view M_vec = gsl_vector_subvector(network, ((Rnum+S)*(Rnum+S))+1+(Y*Y)+1, (Rnum+S)); // Massenvektor
gsl_vector *Mvec = &M_vec.vector;
//-- verändere zu dem gewünschten Zeitpunkt Migrationsmatrix
if( (t > tau) && (tlast < tau))
{
//printf("mu ist %f\n", gsl_vector_get(nicheweb->migrPara,1));
//printf("nu ist %f\n", nu);
gsl_vector_set(nicheweb->migrPara,4,t);
//printf("Setze Link für gewünschte Migration\n");
// printf("t oben %f\n",t);
// printf("tlast oben %f\n",tlast);
gsl_matrix_set(EDmat, nu, mu, 1.);
//int m;
// for(l = 0; l< Y;l++)
// {
// for(m=0;m<Y;m++)
// {
// printf("%f\t",gsl_matrix_get(EDmat,l,m));
// }
// printf("\n");
// }
}
else
{
gsl_matrix_set_zero(EDmat);
}
// printf("\ncheckpoint Holling2 I\n");
// printf("\nS = %i\n", S);
// printf("\nS + Rnum = %i\n", S+Rnum);
//
// printf("\nSize A_view = %i\n", (int)A_view.vector.size);
// printf("\nSize D_view = %i\n", (int)D_view.vector.size);
// printf("\nSize M_vec = %i\n", (int)M_vec.vector.size);
// for(i=0; i<(Rnum+S)*Y; i++){
// printf("\ny = %f\n", y[i]);
// }
// for(i=0; i<(Rnum+S)*Y; i++){
// printf("\nydot = %f\n", ydot[i]);
// }
//--zusätzliche Variablen anlegen-------------------------------------------------------------------------------------------------------------
double ytemp[(Rnum+S)*Y];
for(i=0; i<(Rnum+S)*Y; i++) ytemp[i] = y[i]; // temp array mit Kopie der Startwerte
for(i=0; i<(Rnum+S)*Y; i++) ydot[i] = 0; // Ergebnis, in das evolve_apply schreibt
gsl_vector_view yfddot_vec = gsl_vector_view_array(ydot, (Rnum+S)*Y); //Notiz: vector_view_array etc. arbeiten auf den original Daten der ihnen zugeordneten Arrays/Vektoren!
gsl_vector *yfddotvec = &yfddot_vec.vector; // zum einfacheren Rechnen ydot über vector_view_array ansprechen
gsl_vector_view yfd_vec = gsl_vector_view_array(ytemp, (Rnum+S)*Y);
gsl_vector *yfdvec = &yfd_vec.vector; // Startwerte der Populationen
//-- neue Objekte zum Rechnen anlegen--------------------------------------------------------------------------------------------------------
gsl_matrix *AFgsl = gsl_matrix_calloc(Rnum+S, Rnum+S); // matrix of foraging efforts
// gsl_matrix *ADgsl = gsl_matrix_calloc(Y,Y); // matrix of migration efforts
gsl_matrix *Emat = gsl_matrix_calloc(Rnum+S, Rnum+S); // gsl objects for calculations of populations
gsl_vector *tvec = gsl_vector_calloc(Rnum+S);
gsl_vector *rvec = gsl_vector_calloc(Rnum+S);
gsl_vector *svec = gsl_vector_calloc(Rnum+S);
// gsl_matrix *Dmat = gsl_matrix_calloc(Y,Y); // gsl objects for calculations of migration
// gsl_vector *d1vec = gsl_vector_calloc(Y);
gsl_vector *d2vec = gsl_vector_calloc(Y);
gsl_vector *d3vec = gsl_vector_calloc(Y);
// printf("\ncheckpoint Holling2 III\n");
//-- Einzelne Patches lösen------------------------------------------------------------------------------------------------------------
for(l=0; l<Y; l++) // start of patch solving
{
gsl_matrix_set_zero(AFgsl); // Objekte zum Rechnen vor jedem Patch nullen
gsl_matrix_set_zero(Emat);
gsl_vector_set_zero(tvec);
gsl_vector_set_zero(rvec);
gsl_vector_set_zero(svec);
gsl_vector_view ydot_vec = gsl_vector_subvector(yfddotvec, (Rnum+S)*l, (Rnum+S)); // enthält ydot von Patch l
gsl_vector *ydotvec = &ydot_vec.vector;
gsl_vector_view y_vec = gsl_vector_subvector(yfdvec, (Rnum+S)*l, (Rnum+S)); // enthält Startwerte der Population in l
gsl_vector *yvec = &y_vec.vector;
gsl_matrix_memcpy(AFgsl, EAmat);
for(i=0; i<Rnum+S; i++)
{
gsl_vector_view rowA = gsl_matrix_row(AFgsl,i);
rowsum = gsl_blas_dasum(&rowA.vector);
if(rowsum !=0 )
{
for(j=0; j<Rnum+S; j++)
gsl_matrix_set(AFgsl, i, j, (gsl_matrix_get(AFgsl,i,j)/rowsum)); // normiere Beute Afgsl = A(Beutelinks auf 1 normiert) = f(i,j)
}
}
gsl_matrix_memcpy(Emat, EAmat); // Emat = A
gsl_matrix_scale(Emat, aij); // Emat(i,j) = a(i,j)
gsl_matrix_mul_elements(Emat, AFgsl); // Emat(i,j) = a(i,j)*f(i,j)
gsl_vector_memcpy(svec, yvec); // s(i) = y(i)
gsl_vector_scale(svec, hand); // s(i) = y(i)*h
gsl_blas_dgemv(CblasNoTrans, 1, Emat, svec, 0, rvec); // r(i) = Sum_k h*a(i,k)*f(i,k)*y(k)
gsl_vector_add_constant(rvec, 1); // r(i) = 1+Sum_k h*a(i,k)*f(i,k)*y(k)
gsl_vector_memcpy(tvec, Mvec); // t(i) = masse(i)^(-0.25)
gsl_vector_div(tvec, rvec); // t(i) = masse(i)^(-0.25)/(1+Sum_k h*a(i,k)*f(i,k)*y(k))
gsl_vector_mul(tvec, yvec); // t(i) = masse(i)^(-0.25)*y(i)/(1+Sum_k h*a(i,k)*f(i,k)*y(k))
gsl_blas_dgemv(CblasTrans, 1, Emat, tvec, 0, rvec); // r(i) = Sum_j a(j,i)*f(j,i)*t(j)
gsl_vector_mul(rvec, yvec); // r(i) = Sum_j a(j,i)*f(j,i)*t(j)*y(i) [rvec: Praedation]
gsl_blas_dgemv(CblasNoTrans, lambda, Emat, yvec, 0, ydotvec); // ydot(i) = Sum_j lambda*a(i,j)*f(i,j)*y(j)
gsl_vector_mul(ydotvec, tvec); // ydot(i) = Sum_j lambda*a(i,j)*f(i,j)*y(j)*t(i)
gsl_vector_memcpy(svec, Mvec);
gsl_vector_scale(svec, alpha); // s(i) = alpha*masse^(-0.25) [svec=Respiration bzw. Mortalitaet]
gsl_vector_memcpy(tvec, Mvec);
gsl_vector_scale(tvec, beta); // t(i) = beta*masse^(-0.25)
gsl_vector_mul(tvec, yvec); // t(i) = beta*y(i)
gsl_vector_add(svec, tvec); // s(i) = alpha*masse^(-0.25)+beta*y(i)
gsl_vector_mul(svec, yvec); // s(i) = alpha*masse^(-0.25)*y(i)+beta*y(i)*y(i)
gsl_vector_add(svec, rvec); // [svec: Respiration, competition und Praedation]
gsl_vector_sub(ydotvec, svec); // ydot(i) = Fressen-Respiration-Competition-Praedation
for(i=0; i<Rnum; i++)
gsl_vector_set(ydotvec, i, 0.0); // konstante Ressourcen
}// Ende Einzelpatch, Ergebnis steht in ydotvec
// printf("\ncheckpoint Holling2 IV\n");
//-- Migration lösen---------------------------------------------------------------------------------------------------------
gsl_vector *ydottest = gsl_vector_calloc(Y);
double ydotmigr = gsl_vector_get(nicheweb->migrPara, 5);
// int count=0,m;
// for(l = 0; l< Y;l++)
// {
// for(m=0;m<Y;m++)
// {
// count += gsl_matrix_get(EDmat,l,m);
// }
// }
// if(count!=0)
// {
// //printf("count %i\n",count);
// //printf("t unten %f\n",t);
// //printf("tau %f\n",tau);
// for(l = 0; l< Y;l++)
// {
// for(m=0;m<Y;m++)
// {
// printf("%f\t",gsl_matrix_get(EDmat,l,m));
// }
// printf("\n");
// }
// }
double max = gsl_matrix_max(EDmat);
for(l = Rnum; l< Rnum+S; l++) // start of migration solving
{
if(l == SpeciesNumber+Rnum && max !=0 )
{
//printf("max ist %f\n",max);
//printf("l ist %i\n",l);
// gsl_matrix_set_zero(ADgsl); // reset gsl objects for every patch
// gsl_matrix_set_zero(Dmat);
// gsl_vector_set_zero(d1vec);
gsl_vector_set_zero(d2vec);
gsl_vector_set_zero(d3vec);
gsl_vector_set_zero(ydottest);
// Untervektor von yfddot (enthält ydot[]) mit offset l (Rnum...Rnum+S) und Abstand zwischen den Elementen (stride) von Rnum+S.
// Dies ergibt gerade die Größe einer Spezies in jedem Patch in einem Vektor
gsl_vector_view dydot_vec = gsl_vector_subvector_with_stride(yfddotvec, l, (Rnum+S), Y); // ydot[]
gsl_vector *dydotvec = &dydot_vec.vector;
/*
gsl_vector_view dy_vec = gsl_vector_subvector_with_stride(yfdvec, l, (Rnum+S), Y); // Startgrößen der Spezies pro Patch
gsl_vector *dyvec = &dy_vec.vector;
*/
// gsl_matrix_memcpy(ADgsl, EDmat); // ADgsl = D
//
// if(nicheweb->M == 1) // umschalten w: patchwise (Abwanderung aus jedem Patch gleich), sonst linkwise (Abwanderung pro link gleich)
// {
// for(i=0; i<Y; i++)
// {
// gsl_vector_view colD = gsl_matrix_column(ADgsl, i); // Spalte i aus Migrationsmatrix
// colsum = gsl_blas_dasum(&colD.vector);
// if(colsum!=0)
// {
// for(j=0;j<Y;j++)
// gsl_matrix_set(ADgsl,j,i,(gsl_matrix_get(ADgsl,j,i)/colsum)); // ADgsl: D mit normierten Links
// }
// }
// }
//
// gsl_matrix_memcpy(Dmat, EDmat); // Dmat = D
// gsl_matrix_scale(Dmat, dij); // Dmat(i,j) = d(i,j) (Migrationsstärke)
// gsl_matrix_mul_elements(Dmat, ADgsl); // Dmat(i,j) = d(i,j)*xi(i,j) (skalierte und normierte Migrationsmatrix)
//
// gsl_vector_set_all(d1vec, 1/gsl_vector_get(Mvec, l)); // d1(i)= m(l)^0.25
// gsl_vector_mul(d1vec, dyvec); // d1(i)= m(l)^0.25*y(i)
// gsl_blas_dgemv(CblasNoTrans, 1, Dmat, d1vec, 0, d2vec); // d2(i)= Sum_j d(i,j)*xi(i,j)*m(l)^0.25*y(j)
//
// gsl_vector_set_all(d1vec, 1); // d1(i)= 1
// gsl_blas_dgemv(CblasTrans, 1, Dmat, d1vec, 0, d3vec); // d3(i)= Sum_j d(i,j)*xi(i,j)
// gsl_vector_scale(d3vec, 1/gsl_vector_get(Mvec,l)); // d3(i)= Sum_j d(i,j)*xi(i,j)*m(l)^0.25
// gsl_vector_mul(d3vec, dyvec); // d3(i)= Sum_j d(i,j)*xi(i,j)*m(l)^0.25*y(i)
//
gsl_vector_set(d2vec,nu,Bmigr);
gsl_vector_set(d3vec,mu,Bmigr);
gsl_vector_add(ydottest,d2vec);
gsl_vector_sub(ydottest,d3vec);
//printf("d2vec ist %f\n",gsl_vector_get(d2vec,0));
//printf("d3vec ist %f\n",gsl_vector_get(d3vec,0));
//if(gsl_vector_get(ydottest,mu)!=0)
//{
ydotmigr += gsl_vector_get(ydottest,nu);
// printf("ydotmigr ist %f\n",ydotmigr);
gsl_vector_set(nicheweb->migrPara,5,ydotmigr);
// if(ydotmigr !=0)
// {
// printf("ydottest aufaddiert ist %f\n",ydotmigr);
// printf("ydottest aufaddiert ist %f\n",gsl_vector_get(nicheweb->migrPara,5));
// }
gsl_vector_add(dydotvec, d2vec); //
gsl_vector_sub(dydotvec, d3vec); // Ergebnis in dydotvec (also ydot[]) = Sum_j d(i,j)*xi(i,j)*m(l)^0.25*y(j) - Sum_j d(i,j)*xi(i,j)*m(l)^0.25*y(i)
}
}// Patch i gewinnt das was aus allen j Patches zuwandert und verliert was von i auswandert
//printf("ydot ist %f\n",gsl_vector_get(ydottest,0));
//printf("\ncheckpoint Holling2 V\n");
/*
for(i=0; i<(Rnum+S)*Y; i++){
printf("\ny = %f\tydot=%f\n", y[i], ydot[i]);
}
*/
//--check for fixed point attractor-----------------------------------------------------------------------------------
if(t>7800){
gsl_vector_set(nicheweb->fixpunkte, 0, 0);
gsl_vector_set(nicheweb->fixpunkte, 1, 0);
gsl_vector_set(nicheweb->fixpunkte, 2, 0);
int fix0 = (int)gsl_vector_get(nicheweb->fixpunkte, 0);
int fix1 = (int)gsl_vector_get(nicheweb->fixpunkte, 1);
int fix2 = (int)gsl_vector_get(nicheweb->fixpunkte, 2);
//printf("t unten = %f\n", t);
for(i=0; i<(Rnum+S)*Y; i++)
{
if(y[i] <= 0)
{
fix0++;
fix1++;
fix2++;
}
else
{
if((ydot[i]/y[i]<0.0001) || (ydot[i]<0.0001)) fix0++;
if(ydot[i]/y[i]<0.0001) fix1++;
if(ydot[i]<0.0001) fix2++;
}
}
if(fix0==(Rnum+S)*Y) gsl_vector_set(nicheweb->fixpunkte, 3, 1);
if(fix1==(Rnum+S)*Y) gsl_vector_set(nicheweb->fixpunkte, 4, 1);
if(fix2==(Rnum+S)*Y) gsl_vector_set(nicheweb->fixpunkte, 5, 1);
}
//--Speicher leeren-----------------------------------------------------------------------------------------------------
gsl_matrix_free(Emat);
// gsl_matrix_free(Dmat);
gsl_matrix_free(AFgsl);
// gsl_matrix_free(ADgsl);
gsl_vector_free(tvec);
gsl_vector_free(rvec);
gsl_vector_free(svec);
// gsl_vector_free(d1vec);
gsl_vector_free(d2vec);
gsl_vector_free(d3vec);
gsl_vector_free(ydottest);
// printf("\nCheckpoint Holling2 VI\n");
return GSL_SUCCESS;
}
int proj_gauss_mixtures_IDL(double * ydata, double * ycovar,
double * projection, double * logweights,
int N, int dy,
double * amp, double * xmean,
double * xcovar, int d, int K,
char * fixamp, char * fixmean,
char * fixcovar,
double * avgloglikedata, double tol,
int maxiter, char likeonly, double w,
char * logfilename, int slen, int splitnmerge,
char * convlogfilename, int convloglen,
char noprojection,char diagerrors,
char noweights){
//Set up logfiles
bool keeplog = true;
char logname[slen+1];
char convlogname[convloglen+1];
int ss;
if (*logfilename == 0 || likeonly != 0 || slen == 0)
keeplog = false;
else {
for (ss = 0; ss != slen; ++ss)
logname[ss] = (char) *(logfilename++);
for (ss = 0; ss != convloglen; ++ss)
convlogname[ss] = (char) *(convlogfilename++);
logfilename -= slen;
convlogfilename -= slen;
logname[slen] = '\0';
convlogname[convloglen] = '\0';
}
if (keeplog) {
logfile = fopen(logname,"a");
if (logfile == NULL) return -1;
convlogfile = fopen(convlogname,"w");
if (convlogfile == NULL) return -1;
}
if (keeplog){
time_t now;
time(&now);
fprintf(logfile,"#----------------------------------\n");
fprintf(logfile,"#\n#%s\n",asctime(localtime(&now)));
fprintf(logfile,"#----------------------------------\n");
fflush(logfile);
}
//Copy everything into the right formats
struct datapoint * data = (struct datapoint *) malloc( N * sizeof (struct datapoint) );
struct gaussian * gaussians = (struct gaussian *) malloc (K * sizeof (struct gaussian) );
bool noproj= (bool) noprojection;
bool noweight= (bool) noweights;
bool diagerrs= (bool) diagerrors;
int ii, jj,dd1,dd2;
for (ii = 0; ii != N; ++ii){
data->ww = gsl_vector_alloc(dy);
if ( ! noweight ) data->logweight = *(logweights++);
if ( diagerrs ) data->SS = gsl_matrix_alloc(dy,1);
else data->SS = gsl_matrix_alloc(dy,dy);
if ( ! noproj ) data->RR = gsl_matrix_alloc(dy,d);
for (dd1 = 0; dd1 != dy;++dd1)
gsl_vector_set(data->ww,dd1,*(ydata++));
if ( diagerrs)
for (dd1 = 0; dd1 != dy; ++dd1)
gsl_matrix_set(data->SS,dd1,0,*(ycovar++));
else
for (dd1 = 0; dd1 != dy; ++dd1)
for (dd2 = 0; dd2 != dy; ++dd2)
gsl_matrix_set(data->SS,dd1,dd2,*(ycovar++));
if ( ! noproj )
for (dd1 = 0; dd1 != dy; ++dd1)
for (dd2 = 0; dd2 != d; ++dd2)
gsl_matrix_set(data->RR,dd1,dd2,*(projection++));
else data->RR= NULL;
++data;
}
data -= N;
ydata -= N*dy;
if ( diagerrs ) ycovar -= N*dy;
else ycovar -= N*dy*dy;
if ( ! noproj ) projection -= N*dy*d;
for (jj = 0; jj != K; ++jj){
gaussians->mm = gsl_vector_alloc(d);
gaussians->VV = gsl_matrix_alloc(d,d);
gaussians->alpha = *(amp++);
for (dd1 = 0; dd1 != d; ++dd1)
gsl_vector_set(gaussians->mm,dd1,*(xmean++));
for (dd1 = 0; dd1 != d; ++dd1)
for (dd2 = 0; dd2 != d; ++dd2)
gsl_matrix_set(gaussians->VV,dd1,dd2,*(xcovar++));
++gaussians;
}
gaussians -= K;
amp -= K;
xmean -= K*d;
xcovar -= K*d*d;
//Print the initial model parameters to the logfile
int kk;
if (keeplog){
fprintf(logfile,"#\n#Using %i Gaussians and w = %f\n\n",K,w);
fprintf(logfile,"#\n#Initial model parameters used:\n\n");
for (kk=0; kk != K; ++kk){
fprintf(logfile,"#Gaussian ");
fprintf(logfile,"%i",kk);
fprintf(logfile,"\n");
fprintf(logfile,"#amp\t=\t");
fprintf(logfile,"%f",(*gaussians).alpha);
fprintf(logfile,"\n");
fprintf(logfile,"#mean\t=\t");
for (dd1=0; dd1 != d; ++dd1){
fprintf(logfile,"%f",gsl_vector_get(gaussians->mm,dd1));
if (dd1 < d-1) fprintf(logfile,"\t");
}
fprintf(logfile,"\n");
fprintf(logfile,"#covar\t=\t");
for (dd1=0; dd1 != d; ++dd1)
fprintf(logfile,"%f\t",gsl_matrix_get(gaussians->VV,dd1,dd1));
for (dd1=0; dd1 != d-1; ++dd1)
for (dd2=dd1+1; dd2 != d; ++dd2){
fprintf(logfile,"%f\t",gsl_matrix_get(gaussians->VV,dd1,dd2));
}
++gaussians;
fprintf(logfile,"\n#\n");
}
gaussians -= K;
fflush(logfile);
}
//Then run projected_gauss_mixtures
proj_gauss_mixtures(data,N,gaussians,K,(bool *) fixamp,
(bool *) fixmean, (bool *) fixcovar,avgloglikedata,
tol,(long long int) maxiter, (bool) likeonly, w,
splitnmerge,keeplog,logfile,convlogfile,noproj,diagerrs,
noweight);
//Print the final model parameters to the logfile
if (keeplog){
fprintf(logfile,"\n#Final model parameters obtained:\n\n");
for (kk=0; kk != K; ++kk){
fprintf(logfile,"#Gaussian ");
fprintf(logfile,"%i",kk);
fprintf(logfile,"\n");
fprintf(logfile,"#amp\t=\t");
fprintf(logfile,"%f",(*gaussians).alpha);
fprintf(logfile,"\n");
fprintf(logfile,"#mean\t=\t");
for (dd1=0; dd1 != d; ++dd1){
fprintf(logfile,"%f",gsl_vector_get(gaussians->mm,dd1));
if (dd1 < d-1) fprintf(logfile,"\t");
}
fprintf(logfile,"\n");
fprintf(logfile,"#covar\t=\t");
for (dd1=0; dd1 != d; ++dd1)
fprintf(logfile,"%f\t",gsl_matrix_get(gaussians->VV,dd1,dd1));
for (dd1=0; dd1 != d-1; ++dd1)
for (dd2=dd1+1; dd2 != d; ++dd2){
fprintf(logfile,"%f\t",gsl_matrix_get(gaussians->VV,dd1,dd2));
}
++gaussians;
fprintf(logfile,"\n#\n");
}
gaussians -= K;
fflush(logfile);
}
//Then update the arrays given to us by IDL
for (jj = 0; jj != K; ++jj){
*(amp++) = gaussians->alpha;
for (dd1 = 0; dd1 != d; ++dd1)
*(xmean++) = gsl_vector_get(gaussians->mm,dd1);
for (dd1 = 0; dd1 != d; ++dd1)
for (dd2 = 0; dd2 != d; ++dd2)
*(xcovar++) = gsl_matrix_get(gaussians->VV,dd1,dd2);
++gaussians;
}
gaussians -= K;
amp -= K;
xmean -= K*d;
xcovar -= K*d*d;
//And free any memory we allocated
for (ii = 0; ii != N; ++ii){
gsl_vector_free(data->ww);
gsl_matrix_free(data->SS);
if ( ! noproj ) gsl_matrix_free(data->RR);
++data;
}
data -= N;
free(data);
for (jj = 0; jj != K; ++jj){
gsl_vector_free(gaussians->mm);
gsl_matrix_free(gaussians->VV);
++gaussians;
}
gaussians -= K;
free(gaussians);
if (keeplog){
fclose(logfile);
fclose(convlogfile);
}
return 0;
}
static int
covar_QRPT (gsl_matrix * r, gsl_permutation * perm,
const double epsrel, gsl_matrix * covar)
{
/* Form the inverse of R in the full upper triangle of R */
double tolr = epsrel * fabs(gsl_matrix_get(r, 0, 0));
const size_t n = r->size2;
size_t i, j, k;
size_t kmax = 0;
for (k = 0 ; k < n ; k++)
{
double rkk = gsl_matrix_get(r, k, k);
if (fabs(rkk) <= tolr)
{
break;
}
gsl_matrix_set(r, k, k, 1.0/rkk);
for (j = 0; j < k ; j++)
{
double t = gsl_matrix_get(r, j, k) / rkk;
gsl_matrix_set (r, j, k, 0.0);
for (i = 0; i <= j; i++)
{
double rik = gsl_matrix_get (r, i, k);
double rij = gsl_matrix_get (r, i, j);
gsl_matrix_set (r, i, k, rik - t * rij);
}
}
kmax = k;
}
/* Form the full upper triangle of the inverse of R^T R in the full
upper triangle of R */
for (k = 0; k <= kmax ; k++)
{
for (j = 0; j < k; j++)
{
double rjk = gsl_matrix_get (r, j, k);
for (i = 0; i <= j ; i++)
{
double rij = gsl_matrix_get (r, i, j);
double rik = gsl_matrix_get (r, i, k);
gsl_matrix_set (r, i, j, rij + rjk * rik);
}
}
{
double t = gsl_matrix_get (r, k, k);
for (i = 0; i <= k; i++)
{
double rik = gsl_matrix_get (r, i, k);
gsl_matrix_set (r, i, k, t * rik);
};
}
}
/* Form the full lower triangle of the covariance matrix in the
strict lower triangle of R and in w */
for (j = 0 ; j < n ; j++)
{
size_t pj = gsl_permutation_get (perm, j);
for (i = 0; i <= j; i++)
{
size_t pi = gsl_permutation_get (perm, i);
double rij;
if (j > kmax)
{
gsl_matrix_set (r, i, j, 0.0);
rij = 0.0 ;
}
else
{
rij = gsl_matrix_get (r, i, j);
}
if (pi > pj)
{
gsl_matrix_set (r, pi, pj, rij);
}
else if (pi < pj)
{
gsl_matrix_set (r, pj, pi, rij);
}
}
{
double rjj = gsl_matrix_get (r, j, j);
gsl_matrix_set (covar, pj, pj, rjj);
}
}
/* symmetrize the covariance matrix */
for (j = 0 ; j < n ; j++)
{
for (i = 0; i < j ; i++)
{
double rji = gsl_matrix_get (r, j, i);
gsl_matrix_set (covar, j, i, rji);
gsl_matrix_set (covar, i, j, rji);
}
}
return GSL_SUCCESS;
}
// Print shifted shape parameters. Given that the data read in at the
// outset were scaled, data will need to be read in again and appropriately
// shifted. This can be done on-the-fly given that the shifts have been
// computed and stored in the matrix S.
void shapeAlign::printShiftedProfiles(void){
// Loop over all shape files
for (size_t i = 0; i < m; i++){
// Generate an output file name
int lastidx = shapeFiles[i].find_last_of(".");
string shapeOutFile = shapeFiles[i].substr(0,lastidx) + ".aligned" +
shapeFiles[i].substr(lastidx,shapeFiles[i].length());
ofstream shapeOut(shapeOutFile.c_str());
if (shapeOut.is_open()){
// Open the original shape data file
ifstream shapeFile(shapeFiles[i].c_str());
int idx = 0;
string line;
// Loop through all sites in the original shape data file
while(getline(shapeFile,line)){
stringstream linestream(line);
string s;
vector <string> temp;
string name;
int ctr = 0; // Column counter
while(linestream >> s){
// Clip the first three columns
if (ctr==0) name = s;
if (ctr >2) temp.push_back(s);
ctr++;
}
// Get rid of the last two columns
temp.pop_back();
temp.pop_back();
// Get the optimal shift stored in S
int shift = gsl_matrix_get(S,idx,cIdx);
// If the shape data need to be reversed w.r.t. centroid,
// do this now
if (gsl_matrix_get(R,idx,cIdx))
std::reverse(temp.begin(),temp.end());
// Output data -- again, the first three columns and the
// last two columns need to be trimmed because these
// contain non-numeric information. There are two cases
// depending on whether the columns were reversed or not
shapeOut << name << "\t";
for (size_t j = 0; j < temp.size(); j++){
if (j > 0) shapeOut << "\t";
if (j + shift >= 0 && j + shift < temp.size()){
shapeOut << temp[j+shift];
} else {
shapeOut << "NA";
}
}
shapeOut << endl;
idx++;
}
shapeFile.close();
} else {
int
gsl_eigen_symm (gsl_matrix * A, gsl_vector * eval,
gsl_eigen_symm_workspace * w)
{
if (A->size1 != A->size2)
{
GSL_ERROR ("matrix must be square to compute eigenvalues", GSL_ENOTSQR);
}
else if (eval->size != A->size1)
{
GSL_ERROR ("eigenvalue vector must match matrix size", GSL_EBADLEN);
}
else
{
const size_t N = A->size1;
double *const d = w->d;
double *const sd = w->sd;
size_t a, b;
/* handle special case */
if (N == 1)
{
double A00 = gsl_matrix_get (A, 0, 0);
gsl_vector_set (eval, 0, A00);
return GSL_SUCCESS;
}
/* use sd as the temporary workspace for the decomposition,
since we can discard the tau result immediately if we are not
computing eigenvectors */
{
gsl_vector_view d_vec = gsl_vector_view_array (d, N);
gsl_vector_view sd_vec = gsl_vector_view_array (sd, N - 1);
gsl_vector_view tau = gsl_vector_view_array (sd, N - 1);
gsl_linalg_symmtd_decomp (A, &tau.vector);
gsl_linalg_symmtd_unpack_T (A, &d_vec.vector, &sd_vec.vector);
}
/* Make an initial pass through the tridiagonal decomposition
to remove off-diagonal elements which are effectively zero */
chop_small_elements (N, d, sd);
/* Progressively reduce the matrix until it is diagonal */
b = N - 1;
while (b > 0)
{
if (sd[b - 1] == 0.0 || isnan(sd[b - 1]))
{
b--;
continue;
}
/* Find the largest unreduced block (a,b) starting from b
and working backwards */
a = b - 1;
while (a > 0)
{
if (sd[a - 1] == 0.0)
{
break;
}
a--;
}
{
const size_t n_block = b - a + 1;
double *d_block = d + a;
double *sd_block = sd + a;
/* apply QR reduction with implicit deflation to the
unreduced block */
qrstep (n_block, d_block, sd_block, NULL, NULL);
/* remove any small off-diagonal elements */
chop_small_elements (n_block, d_block, sd_block);
}
}
{
gsl_vector_view d_vec = gsl_vector_view_array (d, N);
gsl_vector_memcpy (eval, &d_vec.vector);
}
return GSL_SUCCESS;
}
}
void Module_DLT::rq_decomp(double* solucion,
gsl_matrix* R_prima,
gsl_matrix* Q_prima,
gsl_vector* x
){
/*
int i, j, lotkin_signum, frank_signum;
int DIM = 3;
gsl_matrix *lotkin_a, *frank_a;
gsl_vector *x, *lotkin_b, *frank_b, *lotkin_x, *frank_x;
gsl_vector *lotkin_tau, *frank_tau;
/* allocate a, x, b
lotkin_a = gsl_matrix_alloc(DIM, DIM);
frank_a = gsl_matrix_alloc(DIM, DIM);
x = gsl_vector_alloc(DIM);
lotkin_b = gsl_vector_alloc(DIM);
frank_b = gsl_vector_alloc(DIM);
lotkin_x = gsl_vector_alloc(DIM);
frank_x = gsl_vector_alloc(DIM);
/* set x = [1 2 ... DIM]
for(i = 0; i < DIM; i++)
gsl_vector_set(x, i, (double)i);
/* set Lotkin matrix */
/* a_ij = 1 (i = 1) or 1/(i+j-1) (i != 1)
for(i = 0; i < DIM; i++)
gsl_matrix_set(lotkin_a, 0, i, 1.0);
for(i = 1; i < DIM; i++)
for(j = 0; j < DIM; j++)
gsl_matrix_set(lotkin_a, i, j, 1.0 / (double)(i + j + 1));
/* set Frank matrix
/* a_ij = DIM - min(i,j) + 1
for(i = 0; i < DIM; i++)
for(j = 0; j < DIM; j++)
gsl_matrix_set(frank_a, i, j, (double)DIM - (double)GSL_MAX(i, j) );
*/
/* set A matrix
gsl_matrix_set(lotkin_a, 0, 0, 12);
gsl_matrix_set(lotkin_a, 0, 1, 6);
gsl_matrix_set(lotkin_a, 0, 2, -4);
gsl_matrix_set(lotkin_a, 1, 0, -51);
gsl_matrix_set(lotkin_a, 1, 1, 167);
gsl_matrix_set(lotkin_a, 1, 2, 24);
gsl_matrix_set(lotkin_a, 2, 0, 4);
gsl_matrix_set(lotkin_a, 2, 1, -68);
gsl_matrix_set(lotkin_a, 2, 2, -41);
/* Print matrix
for(i = 0; i < DIM; i++)
{
printf("%3d: ", i);
for(j = 0; j < DIM; j++)
printf("%g ", gsl_matrix_get(lotkin_a, i, j));
printf("\n");
}
printf("\n");
/* b = A * x
gsl_blas_dgemv(CblasNoTrans, 1.0, lotkin_a, x, 0.0, lotkin_b);
/* QR decomposition and solve
lotkin_tau = gsl_vector_alloc(DIM);
gsl_linalg_QR_decomp(lotkin_a, lotkin_tau);
gsl_linalg_QR_solve(lotkin_a, lotkin_tau, lotkin_b, lotkin_x);
gsl_vector_free(lotkin_tau);
/* Print solution matrix
for(i = 0; i < DIM; i++)
{
printf("%3d: ", i);
for(j = 0; j < DIM; j++)
printf("%g ", gsl_matrix_get(lotkin_a, i, j));
printf("\n");
}
printf("\n");
for(i = 0; i < DIM; i++)
{
printf("%3d: ", i);
for(j = 0; j < DIM; j++)
//printf("%g ", gsl_vector_get(lotkin_x, i, j));
printf("\n");
}
/* free a, x, b
gsl_matrix_free(lotkin_a);
gsl_vector_free(x);
gsl_vector_free(lotkin_b);
gsl_vector_free(lotkin_x);
*/
/*
gsl_matrix* C = gsl_matrix_alloc(3,3);
/* Compute C = A B
gsl_blas_dgemm (CblasNoTrans, CblasNoTrans,
1.0, R_prima, Q_prima,
0.0, C);
camera->rt11 = gsl_matrix_get(C, 0, 0);
camera->rt12 = gsl_matrix_get(C, 0, 1);
camera->rt13 = gsl_matrix_get(C, 0, 2);
camera->rt21 = gsl_matrix_get(C, 1, 0);
camera->rt22 = gsl_matrix_get(C, 1, 1);
camera->rt23 = gsl_matrix_get(C, 1, 2);
camera->rt31 = gsl_matrix_get(C, 2, 0);
camera->rt32 = gsl_matrix_get(C, 2, 1);
camera->rt33 = gsl_matrix_get(C, 2, 2);
camera->rt41 = 0;
camera->rt42 = 0;
camera->rt43 = 0;
camera->rt44 = 1;
**/
std::cout << "RQ_Decomp" << std::endl;
int n,mm,s,signum ;
gsl_matrix *M,*Q,*R;
gsl_vector* tau;
double tmp,det;
/* para invertir las matriz M,Q,R */
gsl_permutation* p = gsl_permutation_alloc (3);
gsl_permutation* p2 = gsl_permutation_alloc (3);
gsl_permutation* p3 = gsl_permutation_alloc (3);
gsl_matrix* M_prima = gsl_matrix_alloc(3,3);
gsl_matrix* Q_prima_tmp = gsl_matrix_alloc(3,3);
/* para resolver el centro de la camara usando Mx=C
donde C es el verctor p4 de la matriz P */
gsl_vector* p4 = gsl_vector_alloc(3);
gsl_matrix* temp = gsl_matrix_alloc(3,3);
gsl_matrix* I_C = gsl_matrix_alloc(3,4);
gsl_matrix* test = gsl_matrix_alloc(3,4);
M = gsl_matrix_alloc(3,3);
Q = gsl_matrix_alloc(3,3);
R = gsl_matrix_alloc(3,3);
tau = gsl_vector_alloc(3);
/* Copiamos la submatriz 3x3 Izq de la solucion P a la matriz M */
gsl_matrix_set(M,0,0,solucion[0]);
gsl_matrix_set(M,0,1,solucion[1]);
gsl_matrix_set(M,0,2,solucion[2]);
gsl_matrix_set(M,1,0,solucion[4]);
gsl_matrix_set(M,1,1,solucion[5]);
gsl_matrix_set(M,1,2,solucion[6]);
gsl_matrix_set(M,2,0,solucion[8]);
gsl_matrix_set(M,2,1,solucion[9]);
gsl_matrix_set(M,2,2,solucion[10]);
/* Copiamos el vector p4 */
gsl_vector_set(p4,0,solucion[3]);
gsl_vector_set(p4,1,solucion[7]);
gsl_vector_set(p4,2,solucion[11]);
/* invertimos la matriz M */
gsl_linalg_LU_decomp (M, p, &s);
gsl_linalg_LU_solve(M,p,p4,x);
gsl_linalg_LU_invert (M, p, M_prima);
/* Hacemos una descomposicion a la matriz M invertida */
gsl_linalg_QR_decomp (M_prima,tau);
gsl_linalg_QR_unpack (M_prima,tau,Q,R);
/* Invertimos R */
gsl_linalg_LU_decomp (R, p2, &s);
gsl_linalg_LU_invert (R, p2, R_prima);
/* Invertimos Q */
gsl_linalg_LU_decomp (Q, p3, &s);
gsl_linalg_LU_invert (Q, p3, Q_prima);
gsl_matrix_memcpy(Q_prima_tmp, Q_prima);
std::cout << "Calculamos" << std::endl;
if (DEBUG) {
/** checking results:
If the rq decompsition is correct we should obtain
the decomposed matrix:
orig_matrix = K*R*T
where T = (I|C)
*/
gsl_matrix_set(I_C,0,3,gsl_vector_get(x,0));
gsl_matrix_set(I_C,1,3,gsl_vector_get(x,1));
gsl_matrix_set(I_C,2,3,gsl_vector_get(x,2));
gsl_matrix_set(I_C,0,0,1);
gsl_matrix_set(I_C,0,1,0);
gsl_matrix_set(I_C,0,2,0);
gsl_matrix_set(I_C,1,0,0);
gsl_matrix_set(I_C,1,1,1);
gsl_matrix_set(I_C,1,2,0);
gsl_matrix_set(I_C,2,0,0);
gsl_matrix_set(I_C,2,1,0);
gsl_matrix_set(I_C,2,2,1);
gsl_linalg_matmult(R_prima,Q_prima,temp);
gsl_linalg_matmult(temp,I_C,test);
printf(" Result -> \n");
for (n=0; n<3; n++){
// for (mm=0; mm<4; mm++){
for (mm=0; mm<3; mm++){
printf(" %g \t",gsl_matrix_get(temp,n,mm));
// se debe sacar test
}
printf("\n");
}
}
/* El elemento (3,3) de la matriz R tiene que ser 1
para ello tenemos que normalizar la matriz dividiendo
entre este elemento
*/
tmp = gsl_matrix_get(R_prima,2,2);
for (n=0; n<3; n++)
for (mm=0; mm<3; mm++){
gsl_matrix_set(R_prima,n,mm, gsl_matrix_get(R_prima,n,mm)/tmp);
}
/* Si obtenemos valores negativos en la
diagonal de K tenemos que cambiar de signo la columna de K y la fila de Q
correspondiente
*/
if (DEBUG)
print_matrix(R_prima);
if (DEBUG)
print_matrix(Q_prima);
if (gsl_matrix_get(R_prima,0,0)<0){
if (DEBUG) printf(" distancia focat 0,0 negativa\n");
gsl_matrix_set(R_prima,0,0,
abs(gsl_matrix_get(R_prima,0,0))
);
for (n=0;n<3;n++)
gsl_matrix_set(Q_prima,0,n,
gsl_matrix_get(Q_prima,0,n)*-1
);
}
if (DEBUG) printf("R_prima\n");
print_matrix(R_prima);
if (DEBUG) printf("Q_prima\n");
print_matrix(Q_prima);
if (gsl_matrix_get(R_prima,1,1)<0){
if (DEBUG) printf(" distancia focal 1,1 negativa\n");
for (n=0;n<3;n++){
gsl_matrix_set(Q_prima,1,n,
gsl_matrix_get(Q_prima,1,n)*-1
);
gsl_matrix_set(R_prima,n,1,
gsl_matrix_get(R_prima,n,1)*-1
);
}
}
if (DEBUG) printf("R_prima\n");
print_matrix(R_prima);
if (DEBUG) printf("Q_prima\n");
print_matrix(Q_prima);
/*Finalmente, si Q queda con determinante -1 cambiamos de signo
todos sus elementos para obtener una rotación sin "reflexion".
NOTA: Este trozo de codigo lo he desactivado debido a que si lo
hacemos obtenemos una orientacion equivocada a la hora de dibujarla
con OGL
*/
gsl_linalg_LU_decomp (Q_prima_tmp, p3, &s);
signum=1;
det = gsl_linalg_LU_det(Q_prima_tmp,signum);
if (-1 == det && 0){
if (DEBUG) printf("Q has a negatif det");
for (n=0;n<3;n++)
for (mm=0;mm<3;mm++)
gsl_matrix_set(Q_prima,n,mm,gsl_matrix_get(Q_prima,n,mm)*-1);
}
}
void Module_DLT::solve_equation_system(){
if (counter_points_image == NUM_POINTSS){
int k,i,j;
int **linear_equation;
double a_data[11*NUM_EQU];
double b_data[NUM_EQU];
linear_equation = (int**)malloc(2*sizeof(int*));
linear_equation[0] = (int*)malloc(12*sizeof(int));
linear_equation[1] = (int*)malloc(12*sizeof(int));
/*
/* recorremos los dos arrays con los puntos almacenados, y vamos obteniendo
las ecuaciones para cada par de puntos. Cada par de puntos da lugar a dos ecuaciones.*/
for (i=0; i<NUM_POINTSS; i++){
// std::cout << "Punto" << pnts_en_objeto_de_control[i].x << " " << points_image[i].x << std::endl;
// FIXME
Tpoint points_selected_over_input_image;
points_selected_over_input_image.u = points_image[i].x;
points_selected_over_input_image.v = points_image[i].y;
get_equation(pnts_en_objeto_de_control[i], points_selected_over_input_image, (int **)linear_equation);
/** copiamos la ecuacion obtenida al sistema lineal sobredimensionado */
for (j=0; j<12; j++){
system_of_linear_equations[i*2][j] = linear_equation[0][j];
system_of_linear_equations[i*2+1][j] = linear_equation[1][j];
// std::cout << " i:" << i << " j:" << j << " ecu1:" << system_of_linear_equations[i*2][j] << " ecu2:" << system_of_linear_equations[i*2+1][j] << std::endl;
}
}
//** free ? */
free(linear_equation[0]);
free(linear_equation[1]);
free(linear_equation);
/** copy matrix "A" (system of linear equations) */
// std::cout << " Matrix A " << std::endl;
k = 0;
for (i=0; i<NUM_EQU; i++){
for (j=0; j<11; j++){
//std::cout << "i:" << i << " j:" << j << std::endl;
a_data[k++] = system_of_linear_equations[i][j];
// std::cout << system_of_linear_equations[i][j] << ", ";
}
// std::cout << std::endl;
}
/** copy vector "b" (last column) */
// std::cout << " Vector b " << std::endl;
for (j=0; j<NUM_EQU; j++){
b_data[j] = system_of_linear_equations[j][11];
// std::cout << b_data[j] << ", ";
}
// std::cout << std::endl;
//** resolve Ax=b
linear_multiple_regression(a_data, b_data);
rq_decomp(solution_matrix, K, R, X);
// std::cout << "K" << std::endl;
for (i=0; i<3; i++){
for (j=0; j<3; j++){
printf("%g ", gsl_matrix_get(K, i, j));
//std::cout << gsl_matrix_get(K, i, j) << std::endl;
}
// std::cout << std::endl;
}
// std::cout << std::endl << "R" << std::endl;
for (i=0; i<3; i++){
for (j=0; j<3; j++){
printf("%g ", gsl_matrix_get(R, i, j));
//std::cout << gsl_matrix_get(R, i, j) << std::endl;
}
// std::cout << std::endl;
}
// std::cout << std::endl << "X" << std::endl;
for (i=0; i<3; i++){
printf("%g ", gsl_vector_get(X, i));
//std::cout << gsl_matrix_get(R, i, j) << std::endl;
// std::cout << std::endl;
}
std::cout << "Solution Matrix" << std::endl;
for (i=0; i<3; i++){
for (j=0; j<3; j++){
std::cout << solution_matrix[i*3+j] << std::endl;
}
std::cout << std::endl;
}
}
}
void
test_longley ()
{
size_t i, j;
{
gsl_multifit_linear_workspace * work =
gsl_multifit_linear_alloc (longley_n, longley_p);
gsl_matrix_view X = gsl_matrix_view_array (longley_x, longley_n, longley_p);
gsl_vector_view y = gsl_vector_view_array (longley_y, longley_n);
gsl_vector * c = gsl_vector_alloc (longley_p);
gsl_matrix * cov = gsl_matrix_alloc (longley_p, longley_p);
gsl_vector_view diag;
double chisq;
double expected_c[7] = { -3482258.63459582,
15.0618722713733,
-0.358191792925910E-01,
-2.02022980381683,
-1.03322686717359,
-0.511041056535807E-01,
1829.15146461355 };
double expected_sd[7] = { 890420.383607373,
84.9149257747669,
0.334910077722432E-01,
0.488399681651699,
0.214274163161675,
0.226073200069370,
455.478499142212 } ;
double expected_chisq = 836424.055505915;
gsl_multifit_linear (&X.matrix, &y.vector, c, cov, &chisq, work);
gsl_test_rel (gsl_vector_get(c,0), expected_c[0], 1e-10, "longley gsl_fit_multilinear c0") ;
gsl_test_rel (gsl_vector_get(c,1), expected_c[1], 1e-10, "longley gsl_fit_multilinear c1") ;
gsl_test_rel (gsl_vector_get(c,2), expected_c[2], 1e-10, "longley gsl_fit_multilinear c2") ;
gsl_test_rel (gsl_vector_get(c,3), expected_c[3], 1e-10, "longley gsl_fit_multilinear c3") ;
gsl_test_rel (gsl_vector_get(c,4), expected_c[4], 1e-10, "longley gsl_fit_multilinear c4") ;
gsl_test_rel (gsl_vector_get(c,5), expected_c[5], 1e-10, "longley gsl_fit_multilinear c5") ;
gsl_test_rel (gsl_vector_get(c,6), expected_c[6], 1e-10, "longley gsl_fit_multilinear c6") ;
diag = gsl_matrix_diagonal (cov);
gsl_test_rel (gsl_vector_get(&diag.vector,0), pow(expected_sd[0],2.0), 1e-10, "longley gsl_fit_multilinear cov00") ;
gsl_test_rel (gsl_vector_get(&diag.vector,1), pow(expected_sd[1],2.0), 1e-10, "longley gsl_fit_multilinear cov11") ;
gsl_test_rel (gsl_vector_get(&diag.vector,2), pow(expected_sd[2],2.0), 1e-10, "longley gsl_fit_multilinear cov22") ;
gsl_test_rel (gsl_vector_get(&diag.vector,3), pow(expected_sd[3],2.0), 1e-10, "longley gsl_fit_multilinear cov33") ;
gsl_test_rel (gsl_vector_get(&diag.vector,4), pow(expected_sd[4],2.0), 1e-10, "longley gsl_fit_multilinear cov44") ;
gsl_test_rel (gsl_vector_get(&diag.vector,5), pow(expected_sd[5],2.0), 1e-10, "longley gsl_fit_multilinear cov55") ;
gsl_test_rel (gsl_vector_get(&diag.vector,6), pow(expected_sd[6],2.0), 1e-10, "longley gsl_fit_multilinear cov66") ;
gsl_test_rel (chisq, expected_chisq, 1e-10, "longley gsl_fit_multilinear chisq") ;
gsl_vector_free(c);
gsl_matrix_free(cov);
gsl_multifit_linear_free (work);
}
{
gsl_multifit_linear_workspace * work =
gsl_multifit_linear_alloc (longley_n, longley_p);
gsl_matrix_view X = gsl_matrix_view_array (longley_x, longley_n, longley_p);
gsl_vector_view y = gsl_vector_view_array (longley_y, longley_n);
gsl_vector * w = gsl_vector_alloc (longley_n);
gsl_vector * c = gsl_vector_alloc (longley_p);
gsl_matrix * cov = gsl_matrix_alloc (longley_p, longley_p);
double chisq;
double expected_c[7] = { -3482258.63459582,
15.0618722713733,
-0.358191792925910E-01,
-2.02022980381683,
-1.03322686717359,
-0.511041056535807E-01,
1829.15146461355 };
double expected_cov[7][7] = { { 8531122.56783558,
-166.727799925578, 0.261873708176346, 3.91188317230983,
1.1285582054705, -0.889550869422687, -4362.58709870581},
{-166.727799925578, 0.0775861253030891, -1.98725210399982e-05,
-0.000247667096727256, -6.82911920718824e-05, 0.000136160797527761,
0.0775255245956248},
{0.261873708176346, -1.98725210399982e-05, 1.20690316701888e-08,
1.66429546772984e-07, 3.61843600487847e-08, -6.78805814483582e-08,
-0.00013158719037715},
{3.91188317230983, -0.000247667096727256, 1.66429546772984e-07,
2.56665052544717e-06, 6.96541409215597e-07, -9.00858307771567e-07,
-0.00197260370663974},
{1.1285582054705, -6.82911920718824e-05, 3.61843600487847e-08,
6.96541409215597e-07, 4.94032602583969e-07, -9.8469143760973e-08,
-0.000576921112208274},
{-0.889550869422687, 0.000136160797527761, -6.78805814483582e-08,
-9.00858307771567e-07, -9.8469143760973e-08, 5.49938542664952e-07,
0.000430074434198215},
{-4362.58709870581, 0.0775255245956248, -0.00013158719037715,
-0.00197260370663974, -0.000576921112208274, 0.000430074434198215,
2.23229587481535 }} ;
double expected_chisq = 836424.055505915;
gsl_vector_set_all (w, 1.0);
gsl_multifit_wlinear (&X.matrix, w, &y.vector, c, cov, &chisq, work);
gsl_test_rel (gsl_vector_get(c,0), expected_c[0], 1e-10, "longley gsl_fit_wmultilinear c0") ;
gsl_test_rel (gsl_vector_get(c,1), expected_c[1], 1e-10, "longley gsl_fit_wmultilinear c1") ;
gsl_test_rel (gsl_vector_get(c,2), expected_c[2], 1e-10, "longley gsl_fit_wmultilinear c2") ;
gsl_test_rel (gsl_vector_get(c,3), expected_c[3], 1e-10, "longley gsl_fit_wmultilinear c3") ;
gsl_test_rel (gsl_vector_get(c,4), expected_c[4], 1e-10, "longley gsl_fit_wmultilinear c4") ;
gsl_test_rel (gsl_vector_get(c,5), expected_c[5], 1e-10, "longley gsl_fit_wmultilinear c5") ;
gsl_test_rel (gsl_vector_get(c,6), expected_c[6], 1e-10, "longley gsl_fit_wmultilinear c6") ;
for (i = 0; i < longley_p; i++)
{
for (j = 0; j < longley_p; j++)
{
gsl_test_rel (gsl_matrix_get(cov,i,j), expected_cov[i][j], 1e-7,
"longley gsl_fit_wmultilinear cov(%d,%d)", i, j) ;
}
}
gsl_test_rel (chisq, expected_chisq, 1e-10, "longley gsl_fit_wmultilinear chisq") ;
gsl_vector_free(w);
gsl_vector_free(c);
gsl_matrix_free(cov);
gsl_multifit_linear_free (work);
}
}
double mget(const gsl_matrix* m, int i, int j)
{
return(gsl_matrix_get(m, i, j));
}
/// get an element
/// @param i :: The row
/// @param j :: The column
double GSLMatrix::get(size_t i, size_t j) const {
if (i < m_matrix->size1 && j < m_matrix->size2)
return gsl_matrix_get(m_matrix, i, j);
throw std::out_of_range("GSLMatrix indices are out of range.");
}
int main(int argc, char *argv[])
{
int all; /*Nombre d'atomes dans pdb*/
int atom; /*Nombre de carbone CA*/
int help_flag = 0;
char file_name[500];
char file_eigen[500] = "eigen.dat";
char check_name[500];
char check_eigen[500] = "eigen_t.dat";
int verbose = 0;
int mode = 6;
float vinit = 1; // Valeur de base
float bond_factor = 1; // Facteur pour poid des bond strechcing
float angle_factor = 1; // Facteur pour poid des angles
double K_phi1 = 1; // Facteurs pour angles dièdres
double K_phi3 = 0.5;
float init_templaate = 1;
float kp_factor = 1; // Facteur pour poid des angles dièdres
char inputname[500] ="none";
char matrix_name[500];
int i, j, k, l;
int nconn;
int lig = 0;
int lig_t = 0;
float factor = 1.0;
for (i = 1;i < argc;i++)
{
if (strcmp("-i",argv[i]) == 0) {strcpy(file_name,argv[i+1]);--help_flag;}
if (strcmp("-t",argv[i]) == 0) {strcpy(check_name,argv[i+1]);--help_flag;}
if (strcmp("-init",argv[i]) == 0) {float temp;sscanf(argv[i+1],"%f",&temp);vinit = temp;}
if (strcmp("-kr",argv[i]) == 0) {float temp;sscanf(argv[i+1],"%f",&temp);bond_factor = temp;}
if (strcmp("-kt",argv[i]) == 0) {float temp;sscanf(argv[i+1],"%f",&temp);angle_factor = temp;}
if (strcmp("-kpf",argv[i]) == 0) {float temp;sscanf(argv[i+1],"%f",&temp); kp_factor = temp;}
if (strcmp("-m",argv[i]) == 0) {strcpy(matrix_name,argv[i+1]);help_flag = 0;}
if (strcmp("-lig",argv[i]) == 0) {lig = 1;}
if (strcmp("-ligt",argv[i]) == 0) {lig_t = 1;}
if (strcmp("-h",argv[i]) == 0) {help_flag = 1;}
if (strcmp("-v",argv[i]) == 0) {verbose = 1;}
}
if (help_flag == 1)
{
printf("****************************\nHelp Section\n-i\tFile Input (PDB)\n-o\tOutput Name Motion\n-ieig\tFile Name Eigen\n-v\tVerbose\n-sp\tSuper Node Mode (CA, N, C)\n-m\tMode\n-nm\tNombre de mode\n-lig\tTient compte duligand (sauf HOH)\n-prox\tAffiche la probabilite que deux CA puissent interagir\n-prev\tAffiche les directions principales du mouvement de chaque CA ponderees par leur ecart-type\n****************************\n");
return(0);
}
//***************************************************
//* *
//*Builds a structure contaning information on the initial pdb structure
//* *
//***************************************************
all = count_atom(file_name);
nconn = count_connect(file_name);
if (verbose == 1) {printf("Connect:%d\n",nconn);}
if (verbose == 1) {printf("Assigning Structure\n\tAll Atom\n");}
// Array with all connects
int **connect_h=(int **)malloc(nconn*sizeof(int *));
for(i=0;i<nconn;i++) { connect_h[i]=(int *)malloc(6*sizeof(int));}
assign_connect(file_name,connect_h);
// Assigns all the atoms
struct pdb_atom strc_all[all];
atom = build_all_strc(file_name,strc_all); // Retourne le nombre de Node
if (atom > 800) {printf("Too much nodes .... To fix, ask [email protected]\n");return(1);}
if (verbose == 1) {printf(" Atom:%d\n",all);}
check_lig(strc_all,connect_h,nconn,all);
// Assigns all Nodes
if (verbose == 1) {printf(" CA Structure\n");}
if (verbose == 1) {printf(" Node:%d\n",atom);}
struct pdb_atom strc_node[atom];
atom = build_cord_CA(strc_all, strc_node,all,lig,connect_h,nconn);
if (verbose == 1) {printf(" Assign Node:%d\n",atom);}
// Free Connect
//for(i=0;i<nconn;i++) {printf("I:%d\n",i);free(connect_h[i]);}
//free(connect_h);
printf("Check 1\n");
//***************************************************
//* *
//*Builds a structure contaning information on the target pdb structure
//* *
//***************************************************
nconn = 0;
int all_t = count_atom(check_name);
nconn = count_connect(check_name);
if (verbose == 1) {printf("Connect:%d\n",nconn);}
if (verbose == 1) {printf("Assigning Structure\n\tAll Atom\n");}
// Array with all connects
int **connect_t=(int **)malloc(nconn*sizeof(int *));
for(i=0;i<nconn;i++) { connect_t[i]=(int *)malloc(6*sizeof(int));}
assign_connect(check_name,connect_t);
// Assigns all the atoms
struct pdb_atom strc_all_t[all_t];
int atom_t = build_all_strc(check_name,strc_all_t); // Retourne le nombre de Node
if (atom_t > 800) {printf("Too much node.... To fix, ask [email protected]\n");return(1);}
if (verbose == 1) {printf(" Atom:%d\n",all_t);}
check_lig(strc_all_t,connect_t,nconn,all_t);
// Assigns all Nodes
if (verbose == 1) {printf(" CA Structure\n");}
if (verbose == 1) {printf(" Node:%d\n",atom_t);}
struct pdb_atom strc_node_t[atom_t];
atom_t = build_cord_CA(strc_all_t, strc_node_t,all_t,lig_t,connect_t,nconn);
if (verbose == 1) {printf(" Assign Node:%d\n",atom_t);}
printf("Check 2\n");
//***************************************************
//* *
//*Aligns both structures *
//* *
//***************************************************
int align[atom];
int score = node_align(strc_node,atom,strc_node_t,atom_t,align);
printf("RMSD:%8.5f Score: %d/%d\n",sqrt(rmsd_no(strc_node,strc_node_t,atom, align)),score,atom);
if ((float)score/(float)atom < 0.8)
{
printf("Low Score... Will try an homemade alignement !!!\n");
score = node_align_onechain(strc_node,atom,strc_node_t,atom_t,align);
printf("RMSD:%8.5f Score: %d/%d\n",sqrt(rmsd_no(strc_node,strc_node_t,atom, align)),score,atom);
}
if ((float)score/(float)atom < 0.8)
{
printf("Low Score... Will try an homemade alignement !!!\n");
score = node_align_low(strc_node,atom,strc_node_t,atom_t,align);
printf("RMSD:%8.5f Score: %d/%d\n",sqrt(rmsd_no(strc_node,strc_node_t,atom, align)),score,atom);
}
printf("RMSD:%8.5f Score: %d/%d\n",sqrt(rmsd_yes(strc_node,strc_node_t,atom, align,strc_all,all)),score,atom);
printf("Check 4\n");
//***************************************************
//* *
//*Build hessian matrices *
//* *
//***************************************************
double **hessian=(double **)malloc(3*atom*sizeof(double *)); // Matrix of the Hessian 1 2 3 (bond, angle, dihedral)
for(i=0;i<3*atom;i++) { hessian[i]=(double *)malloc(3*atom*sizeof(double));}
for(i=0;i<3*atom;i++)for(j=0;j<(3*atom);j++){hessian[i][j]=0;}
gsl_matrix *hess = gsl_matrix_alloc(3*atom,3*atom);
gsl_matrix_set_all(hess, 0);
gsl_matrix *hess_t = gsl_matrix_alloc(3*atom_t,3*atom_t);
gsl_matrix_set_all(hess_t, 0);
assign_atom_type(strc_all, all);
if (strcmp(inputname,"none") == 0) {} else {assign_lig_type(strc_all, all, inputname);}
gsl_matrix *vcon = gsl_matrix_alloc(all,all);
gsl_matrix *inter_m = gsl_matrix_alloc(8,8);
gsl_matrix *templaate = gsl_matrix_alloc(atom*3, atom*3);
gsl_matrix_set_all(templaate,vinit);
gsl_matrix_set_all(vcon,0);
if (verbose == 1) {printf("Do Vcon !!!\n");}
vcon_file_dom(strc_all,vcon,all);
if (verbose == 1) {printf("Reading Interaction Matrix %s\n",matrix_name);}
load_matrix(inter_m,matrix_name);
//write_matrix("vcon_vince.dat", vcon,all,all);
if (verbose == 1) {printf("Building templaate\n");}
all_interaction(strc_all,all, atom, templaate,lig,vcon,inter_m,strc_node);
gsl_matrix_scale (templaate, init_templaate);
if (verbose == 1) {printf("Building Hessian\n");}
if (verbose == 1) {printf("\tCovalent Bond Potential\n");}
build_1st_matrix(strc_node,hessian,atom,bond_factor);
if (verbose == 1) {printf("\tAngle Potential\n");}
build_2_matrix(strc_node,hessian,atom,angle_factor);
if (verbose == 1) {printf("\tDihedral Potential\n");}
build_3_matrix(strc_node, hessian,atom,K_phi1/2+K_phi3*9/2,kp_factor);
if (verbose == 1) {printf("\tNon Local Interaction Potential\n");}
build_4h_matrix(strc_node,hessian,atom,1.0,templaate);
if (verbose == 1) {printf("\tAssigning Array\n");}
assignArray(hess,hessian,3*atom,3*atom);
gsl_matrix_free(vcon);
gsl_matrix_free(templaate);
double **hessian_t=(double **)malloc(3*atom_t*sizeof(double *)); // Matrix of the Hessian 1 2 3 (bond, angle, dihedral)
for(i=0;i<3*atom_t;i++) { hessian_t[i]=(double *)malloc(3*atom_t*sizeof(double));}
for(i=0;i<3*atom_t;i++)for(j=0;j<(3*atom_t);j++){hessian_t[i][j]=0;}
assign_atom_type(strc_all_t, all_t);
if (strcmp(inputname,"none") == 0) {} else {assign_lig_type(strc_all_t, all_t, inputname);}
gsl_matrix *vcon_t = gsl_matrix_alloc(all_t,all_t);
gsl_matrix *templaate_t = gsl_matrix_alloc(atom_t*3, atom_t*3);
gsl_matrix_set_all(templaate_t,vinit);
gsl_matrix_set_all(vcon_t,0);
if (verbose == 1) {printf("Do Vcon !!!\n");}
vcon_file_dom(strc_all_t,vcon_t,all_t);
//write_matrix("vcon_vince.dat", vcon,all,all);
if (verbose == 1) {printf("Building templaate\n");}
all_interaction(strc_all_t,all_t, atom_t, templaate_t,lig,vcon_t,inter_m,strc_node_t);
gsl_matrix_scale (templaate_t, init_templaate);
if (verbose == 1) {printf("Building Hessian\n");}
if (verbose == 1) {printf("\tCovalent Bond Potential\n");}
build_1st_matrix(strc_node_t,hessian_t,atom_t,bond_factor);
if (verbose == 1) {printf("\tAngle Potential\n");}
build_2_matrix(strc_node_t,hessian_t,atom_t,angle_factor);
if (verbose == 1) {printf("\tDihedral Potential\n");}
build_3_matrix(strc_node_t, hessian_t,atom_t,K_phi1/2+K_phi3*9/2,kp_factor);
if (verbose == 1) {printf("\tNon Local Interaction Potential\n");}
build_4h_matrix(strc_node_t,hessian_t,atom_t,1.0,templaate_t);
if (verbose == 1) {printf("\tAssigning Array\n");}
assignArray(hess_t,hessian_t,3*atom_t,3*atom_t);
gsl_matrix_free(vcon_t);
gsl_matrix_free(templaate_t);
printf("Check 5\n");
//***************************************************
//* *
//*Build mini hessian matrices *
//* *
//***************************************************
gsl_matrix *mini_hess = gsl_matrix_alloc(3*score, 3*score);
gsl_matrix_set_all(mini_hess, 0);
gsl_matrix *mini_hess_t = gsl_matrix_alloc(3*score, 3*score);
gsl_matrix_set_all(mini_hess_t, 0);
int sup_line = 0;
int sup_to_node[score];
for(i = 0; i < atom; i++)
{
if(align[i] == -1) {continue;}
sup_to_node[sup_line] = i;
int sup_col = 0;
for(j = 0; j < atom; j++)
{
if(align[j] == -1) {continue;}
for(k = 0; k < 3; k++)
{
for(l = 0; l < 3; l++)
{
gsl_matrix_set(mini_hess, 3*sup_line + k, 3*sup_col + l, gsl_matrix_get(hess, 3*i + k, 3*j + l));
gsl_matrix_set(mini_hess_t, 3*sup_line + k, 3*sup_col + l, gsl_matrix_get(hess_t, 3*align[i] + k, 3*align[j] + l));
}
}
sup_col ++;
}
sup_line++;
}
gsl_matrix_free(hess);
gsl_matrix_free(hess_t);
printf("Check 6\n");
gsl_vector *mini_eval = gsl_vector_alloc(3*score);
gsl_matrix *mini_evec = gsl_matrix_alloc (3*score,3*score);
gsl_vector *mini_eval_t = gsl_vector_alloc(3*score);
gsl_matrix *mini_evec_t = gsl_matrix_alloc (3*score,3*score);
diagonalyse_matrix(mini_hess,3*score,mini_eval,mini_evec);
diagonalyse_matrix(mini_hess_t,3*score,mini_eval_t,mini_evec_t);
gsl_matrix_set_all(mini_hess, 0);
gsl_matrix_set_all(mini_hess_t, 0);
for(i = 0; i < 3*score; i++)
{
if(gsl_vector_get(mini_eval, i) > 0.0000001)
{
gsl_vector_set(mini_eval, i, 1.0 / gsl_vector_get(mini_eval, i));
for(j = 0; j < 3*score; j++)
{
for(k = 0; k < 3*score; k++)
{
gsl_matrix_set(mini_hess, j, k, gsl_matrix_get(mini_hess, j, k) + gsl_matrix_get(mini_evec, j, i)*gsl_matrix_get(mini_evec, k, i)*gsl_vector_get(mini_eval, i));
}
}
}
}
for(i = 0; i < 3*score; i++)
{
if(gsl_vector_get(mini_eval_t, i) > 0.0000001)
{
gsl_vector_set(mini_eval_t, i, 1.0 / gsl_vector_get(mini_eval_t, i));
for(j = 0; j < 3*score; j++)
{
for(k = 0; k < 3*score; k++)
{
gsl_matrix_set(mini_hess_t, j, k, gsl_matrix_get(mini_hess_t, j, k) + gsl_matrix_get(mini_evec_t, j, i)*gsl_matrix_get(mini_evec_t, k, i)*gsl_vector_get(mini_eval_t, i));
}
}
}
}
printf("Check 7\n");
printf("Dynamic distance : %1.10f\n", cmp_gauss(mini_hess, mini_eval, mini_hess_t, mini_eval_t, 3*score));
gsl_matrix_free(mini_hess);
gsl_matrix_free(mini_evec);
gsl_vector_free(mini_eval);
gsl_matrix_free(mini_hess_t);
gsl_matrix_free(mini_evec_t);
gsl_vector_free(mini_eval_t);
free(connect_h);
return(1);
}
double PostCal::likelihood(int * configure, double * stat, double NCP) {
int coutOne = 0;
int gsl_tmp = 0;
double matDet = 0;
double res = 0;
gsl_matrix * statMatrix = gsl_matrix_calloc (snpCount, 1);
gsl_matrix * statMatrixtTran = gsl_matrix_calloc (1, snpCount);
gsl_matrix * configMatrix = gsl_matrix_calloc (snpCount, 1);
gsl_matrix * tmpResultMatrix = gsl_matrix_calloc (snpCount, 1);
gsl_matrix * tmpResultMatrix1 = gsl_matrix_calloc (snpCount, snpCount);
gsl_matrix * tmpResultMatrix2 = gsl_matrix_calloc (snpCount, snpCount);
gsl_matrix * tmpResultMatrix1N = gsl_matrix_calloc (1, snpCount);
gsl_matrix * tmpResultMatrix11 = gsl_matrix_calloc (1, 1);
for(int i = 0; i < snpCount; i++) {
gsl_matrix_set(statMatrix,i,0,stat[i]);
gsl_matrix_set(statMatrixtTran,0,i,stat[i]);
coutOne += configure[i];
}
gsl_matrix_memcpy(tmpResultMatrix1, sigmaMatrix);
for(int i = 0; i < snpCount; i++) {
if(configure[i] == 1){
gsl_matrix_memcpy(tmpResultMatrix2, matrixPossibleCases[i]);
gsl_matrix_scale(tmpResultMatrix2, NCP);
gsl_matrix_add(tmpResultMatrix1, tmpResultMatrix2);
}
}
gsl_matrix_memcpy(tmpResultMatrix2, tmpResultMatrix1);
gsl_permutation *p = gsl_permutation_alloc(snpCount);
gsl_linalg_LU_decomp(tmpResultMatrix2, p, &gsl_tmp );
matDet = gsl_linalg_LU_det(tmpResultMatrix2,gsl_tmp);
//gsl_linalg_cholesky_decomp(tmpResultMatrix1);
//gsl_linalg_cholesky_invert(tmpResultMatrix1);
gsl_linalg_LU_invert(tmpResultMatrix2, p, tmpResultMatrix1);
gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1.0, statMatrixtTran, tmpResultMatrix1, 0.0, tmpResultMatrix1N);
gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1.0, tmpResultMatrix1N, statMatrix, 0.0, tmpResultMatrix11);
res = gsl_matrix_get(tmpResultMatrix11,0,0);
gsl_matrix_free(tmpResultMatrix);
gsl_matrix_free(tmpResultMatrix1);
gsl_matrix_free(tmpResultMatrix2);
gsl_matrix_free(tmpResultMatrix1N);
gsl_matrix_free(tmpResultMatrix11);
gsl_matrix_free(statMatrix);
gsl_matrix_free(statMatrixtTran);
gsl_matrix_free(configMatrix);
gsl_permutation_free(p);
if(baseValue == 0)
baseValue = res;
res = res - baseValue;
if(matDet==0) {
cout << "Error the matrix is singular and we fail to fix it." << endl;
exit(0);
}
/*
We compute the log of -res/2-log(det) to see if it is too big or not.
In the case it is too big we just make it a MAX value.
*/
double tmplogDet = log(sqrt(abs(matDet)));
double tmpFinalRes = -res/2 - tmplogDet;
if(tmpFinalRes > 700) {
return(exp(700));
}
return( exp(-res/2)/sqrt(abs(matDet)) );
}
int
gsl_linalg_QRPT_update (gsl_matrix * Q, gsl_matrix * R,
const gsl_permutation * p,
gsl_vector * w, const gsl_vector * v)
{
if (Q->size1 != Q->size2 || R->size1 != R->size2)
{
return GSL_ENOTSQR;
}
else if (R->size1 != Q->size2 || v->size != Q->size2 || w->size != Q->size2)
{
return GSL_EBADLEN;
}
else
{
size_t j, k;
const size_t M = Q->size1;
const size_t N = Q->size2;
double w0;
/* Apply Given's rotations to reduce w to (|w|, 0, 0, ... , 0)
J_1^T .... J_(n-1)^T w = +/- |w| e_1
simultaneously applied to R, H = J_1^T ... J^T_(n-1) R
so that H is upper Hessenberg. (12.5.2) */
for (k = N - 1; k > 0; k--)
{
double c, s;
double wk = gsl_vector_get (w, k);
double wkm1 = gsl_vector_get (w, k - 1);
create_givens (wkm1, wk, &c, &s);
apply_givens_vec (w, k - 1, k, c, s);
apply_givens_qr (M, N, Q, R, k - 1, k, c, s);
}
w0 = gsl_vector_get (w, 0);
/* Add in w v^T (Equation 12.5.3) */
for (j = 0; j < N; j++)
{
double r0j = gsl_matrix_get (R, 0, j);
size_t p_j = gsl_permutation_get (p, j);
double vj = gsl_vector_get (v, p_j);
gsl_matrix_set (R, 0, j, r0j + w0 * vj);
}
/* Apply Givens transformations R' = G_(n-1)^T ... G_1^T H
Equation 12.5.4 */
for (k = 1; k < N; k++)
{
double c, s;
double diag = gsl_matrix_get (R, k - 1, k - 1);
double offdiag = gsl_matrix_get (R, k, k - 1);
create_givens (diag, offdiag, &c, &s);
apply_givens_qr (M, N, Q, R, k - 1, k, c, s);
}
return GSL_SUCCESS;
}
}
static VALUE rb_gsl_pow_int(VALUE obj, VALUE xx, VALUE nn)
{
VALUE x, ary, argv[2];
size_t i, j, size;
int n;
gsl_vector *v = NULL, *vnew = NULL;
gsl_matrix *m = NULL, *mnew = NULL;
#ifdef HAVE_NARRAY_H
struct NARRAY *na;
double *ptr1, *ptr2;
#endif
if (CLASS_OF(xx) == rb_cRange) xx = rb_gsl_range2ary(xx);
switch (TYPE(xx)) {
case T_FIXNUM:
case T_BIGNUM:
case T_FLOAT:
return rb_float_new(gsl_pow_int(NUM2DBL(xx), FIX2INT(nn)));
break;
case T_ARRAY:
CHECK_FIXNUM(nn);
n = FIX2INT(nn);
size = RARRAY_LEN(xx);
ary = rb_ary_new2(size);
for (i = 0; i < size; i++) {
x = rb_ary_entry(xx, i);
Need_Float(x);
rb_ary_store(ary, i, rb_float_new(gsl_pow_int(RFLOAT_VALUE(x), n)));
}
return ary;
break;
default:
#ifdef HAVE_NARRAY_H
if (NA_IsNArray(xx)) {
CHECK_FIXNUM(nn);
n = FIX2INT(nn);
GetNArray(xx, na);
ptr1 = (double*) na->ptr;
size = na->total;
ary = na_make_object(NA_DFLOAT, na->rank, na->shape, CLASS_OF(xx));
ptr2 = NA_PTR_TYPE(ary, double*);
for (i = 0; i < size; i++) ptr2[i] = gsl_pow_int(ptr1[i], n);
return ary;
}
#endif
if (VECTOR_P(xx)) {
CHECK_FIXNUM(nn);
n = FIX2INT(nn);
Data_Get_Struct(xx, gsl_vector, v);
vnew = gsl_vector_alloc(v->size);
for (i = 0; i < v->size; i++) {
gsl_vector_set(vnew, i, gsl_pow_int(gsl_vector_get(v, i), n));
}
return Data_Wrap_Struct(cgsl_vector, 0, gsl_vector_free, vnew);
} else if (MATRIX_P(xx)) {
CHECK_FIXNUM(nn);
n = FIX2INT(nn);
Data_Get_Struct(xx, gsl_matrix, m);
mnew = gsl_matrix_alloc(m->size1, m->size2);
for (i = 0; i < m->size1; i++) {
for (j = 0; j < m->size2; j++) {
gsl_matrix_set(mnew, i, j, gsl_pow_int(gsl_matrix_get(m, i, j), n));
}
}
return Data_Wrap_Struct(cgsl_matrix, 0, gsl_matrix_free, mnew);
} else if (COMPLEX_P(xx) || VECTOR_COMPLEX_P(xx) || MATRIX_COMPLEX_P(xx)) {
argv[0] = xx;
argv[1] = nn;
return rb_gsl_complex_pow_real(2, argv, obj);
} else {
rb_raise(rb_eTypeError, "wrong argument type %s (Array or Vector or Matrix expected)", rb_class2name(CLASS_OF(xx)));
}
break;
}
/* never reach here */
return Qnil;
}
int
gsl_linalg_QRPT_decomp (gsl_matrix * A, gsl_vector * tau, gsl_permutation * p, int *signum, gsl_vector * norm)
{
const size_t M = A->size1;
const size_t N = A->size2;
if (tau->size != GSL_MIN (M, N))
{
GSL_ERROR ("size of tau must be MIN(M,N)", GSL_EBADLEN);
}
else if (p->size != N)
{
GSL_ERROR ("permutation size must be N", GSL_EBADLEN);
}
else if (norm->size != N)
{
GSL_ERROR ("norm size must be N", GSL_EBADLEN);
}
else
{
size_t i;
*signum = 1;
gsl_permutation_init (p); /* set to identity */
/* Compute column norms and store in workspace */
for (i = 0; i < N; i++)
{
gsl_vector_view c = gsl_matrix_column (A, i);
double x = gsl_blas_dnrm2 (&c.vector);
gsl_vector_set (norm, i, x);
}
for (i = 0; i < GSL_MIN (M, N); i++)
{
/* Bring the column of largest norm into the pivot position */
double max_norm = gsl_vector_get(norm, i);
size_t j, kmax = i;
for (j = i + 1; j < N; j++)
{
double x = gsl_vector_get (norm, j);
if (x > max_norm)
{
max_norm = x;
kmax = j;
}
}
if (kmax != i)
{
gsl_matrix_swap_columns (A, i, kmax);
gsl_permutation_swap (p, i, kmax);
gsl_vector_swap_elements(norm,i,kmax);
(*signum) = -(*signum);
}
/* Compute the Householder transformation to reduce the j-th
column of the matrix to a multiple of the j-th unit vector */
{
gsl_vector_view c_full = gsl_matrix_column (A, i);
gsl_vector_view c = gsl_vector_subvector (&c_full.vector,
i, M - i);
double tau_i = gsl_linalg_householder_transform (&c.vector);
gsl_vector_set (tau, i, tau_i);
/* Apply the transformation to the remaining columns */
if (i + 1 < N)
{
gsl_matrix_view m = gsl_matrix_submatrix (A, i, i + 1, M - i, N - (i+1));
gsl_linalg_householder_hm (tau_i, &c.vector, &m.matrix);
}
}
/* Update the norms of the remaining columns too */
if (i + 1 < M)
{
for (j = i + 1; j < N; j++)
{
double y = 0;
double x = gsl_vector_get (norm, j);
if (x > 0.0)
{
double temp= gsl_matrix_get (A, i, j) / x;
if (fabs (temp) >= 1)
y = 0.0;
else
y = y * sqrt (1 - temp * temp);
/* recompute norm to prevent loss of accuracy */
if (fabs (y / x) < sqrt (20.0) * GSL_SQRT_DBL_EPSILON)
{
gsl_vector_view c_full = gsl_matrix_column (A, j);
gsl_vector_view c =
gsl_vector_subvector(&c_full.vector,
i+1, M - (i+1));
y = gsl_blas_dnrm2 (&c.vector);
}
gsl_vector_set (norm, j, y);
}
}
}
}
return GSL_SUCCESS;
}
}
void MAIAllocator::Run() {
// fetch channel matrix
gsl_matrix_complex hmm = min1.GetDataObj();
// hmm : channel coeffs matrix h(n) (M**2xN)
// ij
// ch matrix structure
//
// +- -+
// | h(0) . . . . h(n) | |
// | 11 11 | |
// | | | Rx1
// | h(0) . . . . h(n) | |
// | 12 12 | |
// | |
// | h(0) . . . . h(n) | |
// | 21 21 | |
// | | | Rx2
// | h(0) . . . . h(n) | |
// | 22 22 | |
// +- -+
//
// where h(n) represents the channel impulse response
// ij
//
// at time n, from tx_i to rx_j
// the matrix has MxM rows and N comumns.
// The (i,j) channel is locater at row i*M+j
// with i,j in the range [0,M-1] and rows counting from 0
//
//
// fetch error report
// e(u) = errors for user u in the last ERROR_REPORT_INTERVAL (ERI) frames
gsl_vector_uint temperr = vin2.GetDataObj();
// update error reports at receiver rx_m every ERI
if (ericount % ERROR_REPORT_INTERVAL == 0) { // once every ERU
if (temperr.size == M()) {
gsl_vector_uint_memcpy(errs,&temperr);
}
ericount = 0;
}
//
// every DECISION_INTERVAL frames we updates the CSI knowledge
//
if (framecount % DECISION_INTERVAL == 0) {
for (int u=0;u<M();u++) { // user loop
// extract time domain response from hmm corresponding to txn-->rxn channel
gsl_vector_complex_const_view hii = gsl_matrix_complex_const_row(&hmm,u*M()+u);
// copy the N-sized vector hii into u-th column of huu
gsl_matrix_complex_set_col(huu,u,&hii.vector);
} // user loop
//cout << "maiallocator:453 - CSI update received" << endl;
// huu matrix structure
//
// +- -+
// | h(0) . . . . h(n) |
// | 11 uu |
// | |
// | h(n) . . . . h(n) |
// | 11 uu |
// +- -+
//
// where h(n) represents the channel impulse response
// ii
//
// at time n, from tx_u to rx_u
// the matrix has N rows and M columns.
//
// ATTENTION! user_0 channel response is the first column
//
// Hmat(NxM) = Fourier( huu(NxM) )
//
gsl_blas_zgemm(CblasNoTrans,
CblasNoTrans,
gsl_complex_rect(1,0),
transform_mat,
huu,
gsl_complex_rect(0,0),
Hmat);
#ifdef SHOW_MATRIX
cout << "Hmat(freq,user) (frame:" << framecount << ") = " << endl;
gsl_matrix_complex_show(Hmat);
#endif
//
// ***********************************************************
// CARRIER ALLOCATION STRATEGIES
// ***********************************************************
//
switch (Mode()) {
case 0: // FIXED_ALLOCATION
break;
case 1: // GIVE_BEST_CARR
//
// SORT CARRIERS OF EACH USERS
//
// uses Hmat: the frequency responses of channel tx_n --> rx_n
//
// starting from user u ...
// find the best (in u ranking) unused carrier and assign it to u
// next user until no more available carriers
for(int u=0; u<M(); u++) { // cycle through users
gsl_vector_complex_const_view huser
= gsl_matrix_complex_const_column(Hmat,u);
gsl_vector_uint_view sortindu = gsl_matrix_uint_column(Hperm,u);
for (int j=0; j<N(); j++) {
double currpower
= gsl_complex_abs2(gsl_vector_complex_get(&huser.vector,j));
gsl_vector_set(huserabs,j,currpower);
}
// sort over c using abs(h(u,c))
gsl_sort_vector_index(p,huserabs);
for (int j=0; j<N(); j++) {
uint currindex = p->data[j];
gsl_vector_uint_set(&sortindu.vector,j,currindex);
}
}
//
// FIND INITIAL USER RANDOMLY
//
curruser = gsl_rng_uniform_int(ran,M());
//
// ASSIGN FREQUENCIES
//
gsl_vector_uint_set_all(nextcarr,0);
gsl_vector_uint_set_all(usedcarr,0);
for (int j=0; j<J(); j++) {
for (int uu=0; uu<M(); uu++) {
int u = (uu+curruser) % M();
int isassigned = 0;
while (! isassigned) {
int tag = gsl_vector_uint_get(nextcarr,u);
gsl_vector_uint_set(nextcarr,u,++tag);
int carrier = gsl_matrix_uint_get(Hperm,N()-tag,u);
if (! gsl_vector_uint_get(usedcarr,carrier)) {
isassigned = 1;
gsl_vector_uint_set(usedcarr,carrier,isassigned);
gsl_matrix_uint_set(signature_frequencies,u,j,carrier);
} else if (tag==N()) {
cerr << "Block: " << BlockName << " allocation problem." << endl;
exit(1);
}
}
}
}
//
// show channels and permutations
//
// gsl_matrix_complex_show(Hmat);
//gsl_matrix_uint_show(Hperm);
//gsl_matrix_uint_show(signature_frequencies);
break;
case 2: // SWAP_BAD_GOOD
//
// SWAP_BAD_GOOD
//
// sort carriers for each user
// choose randomly a starting user u
// for each user starting with u
// swap worst carrier used by u with best carrier if used by others
// sort carriers
for(int u=0; u<M(); u++) {
gsl_vector_complex_const_view huser
= gsl_matrix_complex_const_column(Hmat,u);
gsl_vector_uint_view sortindu = gsl_matrix_uint_column(Hperm,u);
gsl_vector_view huserabs = gsl_matrix_column(habs,u);
for (int j=0; j<N(); j++) {
double currpower
= gsl_complex_abs2(gsl_vector_complex_get(&huser.vector,j));
gsl_vector_set(&huserabs.vector,j,currpower);
}
//
// sort channels for user <u>
//
gsl_sort_vector_index(p,&huserabs.vector);
for (int j=0; j<N(); j++) {
uint currindex = p->data[j];
gsl_vector_uint_set(&sortindu.vector,j,currindex);
}
}
//
// Hperm(N,USERS) contains sorted channels index for each users
// habs(N,USERS) contains channel energy per each user
//
//
// FIND INITIAL USER RANDOMLY for fairness
//
curruser = gsl_rng_uniform_int(ran,M());
//
// ASSIGN FREQUENCIES
//
//
// for each user ...
//
for (int uu=0; uu<M(); uu++) {
int u = (uu+curruser) % M();
//
// worst allocated channel for user u
//
double worstvalue=GSL_POSINF;
unsigned int worstjindex;
for (int j=0; j<J(); j++) {
unsigned int chind = gsl_matrix_uint_get(signature_frequencies,u,j);
double currh = gsl_matrix_get(habs,chind,u);
if (currh < worstvalue) {
worstvalue = currh;
worstjindex = j;
}
}
//
// find best channel allocated by other users
//
//
double bestvalue=0;
unsigned int bestuser, bestjindex;
for (int uuu=0; uuu<M()-1; uuu++) {
unsigned int otheru = (uuu+u) % M();
for (int j=0; j<J(); j++) {
unsigned int chind
= gsl_matrix_uint_get(signature_frequencies,otheru,j);
double currh = gsl_matrix_get(habs,chind,otheru);
if (currh > bestvalue) {
bestvalue = currh;
bestjindex = j;
bestuser = otheru;
}
}
}
//
// finally the swap !
//
unsigned int chind
= gsl_matrix_uint_get(signature_frequencies,u,worstjindex);
gsl_matrix_uint_set(signature_frequencies,u,worstjindex,
gsl_matrix_uint_get(signature_frequencies,
bestuser,bestjindex));
gsl_matrix_uint_set(signature_frequencies,bestuser,bestjindex,chind);
// cout << "\n\nProcessing user " << u << endl
// << "\tSwapped " << u << "." << worstjindex
// << " <-> " << bestuser << "." << bestjindex << endl;
}
break;
case 3: // BEST_OVERLAP
//
// SORT CARRIERS OF EACH USERS
//
gsl_matrix_uint_memcpy(signature_frequencies,
signature_frequencies_init);
for(int u=0; u<M(); u++) {
gsl_vector_complex_const_view huser
= gsl_matrix_complex_const_column(Hmat,u);
gsl_vector_uint_view sortindu = gsl_matrix_uint_column(Hperm,u);
for (int j=0; j<N(); j++) {
double currpower = gsl_complex_abs2(gsl_vector_complex_get(&huser.vector,
j));
gsl_vector_set(huserabs,j,currpower);
}
gsl_sort_vector_index(p,huserabs);
for (int j=0; j<N(); j++) {
uint currindex = p->data[j];
gsl_vector_uint_set(&sortindu.vector,j,currindex);
}
}
//
// each user take his best carriers allowing carrier overlap
//
for (int u=0; u<M(); u++) {
for (int j=0; j<J(); j++) {
int carrier = gsl_matrix_uint_get(Hperm,N()-j-1,u);
gsl_matrix_uint_set(signature_frequencies,u,j,carrier);
}
}
//
// show channels and permutations
//
//gsl_matrix_complex_show(Hmat);
//gsl_matrix_uint_show(Hperm);
//gsl_matrix_uint_show(signature_frequencies);
break;
case 4: // SOAR_AI
//
// SOAR
//
// agent crai5
// bases the decisions on the frequency response tx_m --> rx_m in Hmat(N,M)
// for each user it proposes a swap between carriers if the instantaneous impulse channel response
// is better
//
// agent crai6
// for each user it proposes a swap of allocated carriers with one other users
// error report is the metric for correct decisions (RL)
#ifdef PAUSED
// keypress
cout << "pause maillocator: before decision loop ... (press ENTER key)" << endl;
cin.ignore();
#endif
// Every DECISION_INTERVAL we increase the input-time and allow decisions
if (framecount % DECISION_INTERVAL == 0) {
pAgent->Update(inputTime,++input_time);
pAgent->Commit();
}
// run agent till output
noDecisions = 0;
numberCommands=0;
while (! (noDecisions) ) { // main decisional loop
//
// INPUT LINK Update
//
UpdateInputLink();
//pAgent->RunSelf(1);
pAgent->RunSelfTilOutput();
numberCommands = pAgent->GetNumberCommands() ;
#ifdef PAUSED
// keypress
cout << "pause maillocator: after RunSelfTilOutput() ... (press ENTER key)" << endl;
cin.ignore();
#endif
// loop through received commands
for (int cmd = 0 ; cmd < numberCommands ; cmd++) {
Identifier* pCommand = pAgent->GetCommand(cmd) ;
string name = pCommand->GetCommandName() ;
if (name == "assign-free") {
std::string sUid = pCommand->GetParameterValue("uid");
std::string sDeassign = pCommand->GetParameterValue("deassign");
std::string sAssign = pCommand->GetParameterValue("assign");
#ifdef SHOW_SOAR
cout << "assign-free command received [ u:"
<< sUid << " , -"
<< sDeassign << " , +"
<< sAssign << " ]"
<< endl;
#endif
AssignFree(sUid,sDeassign,sAssign);
pCommand->AddStatusComplete();
} else if (name == "swap-carriers") {
std::string sU1 = pCommand->GetParameterValue("u1");
std::string sC1 = pCommand->GetParameterValue("c1");
std::string sU2 = pCommand->GetParameterValue("u2");
std::string sC2 = pCommand->GetParameterValue("c2");
#ifdef SHOW_SOAR
cout << "swap-carriers command received [ u1:"
<< sU1 << " , c1:"
<< sC1 << " , u2:"
<< sU2 << " , c2:"
<< sC2 << " ]" << endl;
#endif
SwapCarriers(sU1,sC1,sU2,sC2);
pCommand->AddStatusComplete();
} else if (name == "increase-power") {
std::string sUid = pCommand->GetParameterValue("uid");
std::string sCid = pCommand->GetParameterValue("cid");
#ifdef SHOW_SOAR
cout << "increase-power command received [ u:"
<< sUid << " , c:"
<< sCid << " ]" << endl;
#endif
IncreasePower(sUid,sCid);
pCommand->AddStatusComplete();
break;
} else if (name == "no-choices") {
#ifdef SHOW_SOAR
cout << "no-choices command received" << endl;
#endif
noDecisions = 1;
pCommand->AddStatusComplete();
break;
} else {
#ifdef SHOW_SOAR
cout << "ignoring unknown output command from SOAR" << endl;
#endif
break;
}
// cout << "framecount = " << framecount << endl;
} // end command loop
} // while (! (noDecisions) )
break;
} // switch (Mode())
} // if DECISION_INTERVAL % 0
//
// every 10s dump frame count
//
time(&nowtime);
if (difftime(nowtime,reporttime) > TIMEDELTA) {
reporttime = nowtime;
cout << "frame:" << framecount << "\r";
cout.flush();
}
//////// production of data
framecount++;
ericount++;
mout1.DeliverDataObj( *signature_frequencies );
mout2.DeliverDataObj( *signature_powers );
#ifdef SHOW_MATRIX
cout << "signature frequencies (frame:" << framecount-1 << ") = " << endl;
gsl_matrix_uint_show(signature_frequencies);
#endif
}
void MultiPeakFit::generateFitCurve()
{
ApplicationWindow *app = (ApplicationWindow *)parent();
if (!d_gen_function)
d_points = d_n;
gsl_matrix * m = gsl_matrix_alloc (d_points, d_peaks);
if (!m){
QMessageBox::warning(app, tr("MantidPlot - Fit Error"), tr("Could not allocate enough memory for the fit curves!"));
return;
}
QVarLengthArray<double> X(d_points), Y(d_points);//double X[d_points], Y[d_points];
int i, j;
int peaks_aux = d_peaks;
if (d_peaks == 1)
peaks_aux--;
if (d_gen_function){
double step = (d_x[d_n-1] - d_x[0])/(d_points-1);
for (i = 0; i<d_points; i++){
double x = d_x[0] + i*step;
X[i] = x;
double yi = 0;
for (j=0; j<d_peaks; j++){
double y = evalPeak(d_results, x, j);
gsl_matrix_set(m, i, j, y + d_results[d_p - 1]);
yi += y;
}
Y[i] = yi + d_results[d_p - 1];//add offset
}
customizeFitResults();
if (d_graphics_display){
if (!d_output_graph)
d_output_graph = createOutputGraph()->activeGraph();
if (d_peaks > 1)
insertFitFunctionCurve(QString(objectName()) + tr("Fit"), X.data(), Y.data(), 2);//insertFitFunctionCurve(QString(objectName()) + tr("Fit"), X, Y, 2);
else
insertFitFunctionCurve(QString(objectName()) + tr("Fit"), X.data(), Y.data());//insertFitFunctionCurve(QString(objectName()) + tr("Fit"), X, Y);
if (generate_peak_curves){
for (i=0; i<peaks_aux; i++){//add the peak curves
for (j=0; j<d_points; j++)
Y[j] = gsl_matrix_get (m, j, i);
insertPeakFunctionCurve(X.data(), Y.data(), i);//insertPeakFunctionCurve(X, Y, i);
}
}
d_output_graph->replot();
}
} else {
QString tableName = app->generateUniqueName(tr("Fit"));
QString dataSet;
if (d_curve)
dataSet = d_curve->title().text();
else
dataSet = d_y_col_name;
QString label = d_explanation + " " + tr("fit of") + " " + dataSet;
d_result_table = app->newHiddenTable(tableName, label, d_points, peaks_aux + 2);
QStringList header = QStringList() << "1";
for (i = 0; i<peaks_aux; i++)
header << tr("peak") + QString::number(i+1);
header << "2";
d_result_table->setHeader(header);
QLocale locale = app->locale();
for (i = 0; i<d_points; i++){
X[i] = d_x[i];
d_result_table->setText(i, 0, locale.toString(X[i], 'e', d_prec));
double yi=0;
for (j=0; j<d_peaks; j++){
double diff = X[i] - d_results[3*j + 1];
double w = d_results[3*j + 2];
double y_aux = 0;
if (d_profile == Gauss)
y_aux += sqrt(M_2_PI)*d_results[3*j]/w*exp(-2*diff*diff/(w*w));
else
y_aux += M_2_PI*d_results[3*j]*w/(4*diff*diff+w*w);
yi += y_aux;
y_aux += d_results[d_p - 1];
d_result_table->setText(i, j+1, locale.toString(y_aux, 'e', d_prec));
gsl_matrix_set(m, i, j, y_aux);
}
Y[i] = yi + d_results[d_p - 1];//add offset
if (d_peaks > 1)
d_result_table->setText(i, d_peaks+1, locale.toString(Y[i], 'e', d_prec));
}
customizeFitResults();
if (d_graphics_display){
if (!d_output_graph)
d_output_graph = createOutputGraph()->activeGraph();
label = tableName + "_2";
DataCurve *c = new DataCurve(d_result_table, tableName + "_1", label);
if (d_peaks > 1)
c->setPen(QPen(ColorBox::color(d_curveColorIndex), 2));
else
c->setPen(QPen(ColorBox::color(d_curveColorIndex), 1));
c->setData(X.data(), Y.data(), d_points);//c->setData(X, Y, d_points);
d_output_graph->insertPlotItem(c, Graph::Line);
d_output_graph->addFitCurve(c);
if (generate_peak_curves){
for (i=0; i<peaks_aux; i++){//add the peak curves
for (j=0; j<d_points; j++)
Y[j] = gsl_matrix_get (m, j, i);
label = tableName + "_" + tr("peak") + QString::number(i+1);
c = new DataCurve(d_result_table, tableName + "_1", label);
c->setPen(QPen(ColorBox::color(d_peaks_color), 1));
c->setData(X.data(), Y.data(), d_points);//c->setData(X, Y, d_points);
d_output_graph->insertPlotItem(c, Graph::Line);
d_output_graph->addFitCurve(c);
}
}
d_output_graph->replot();
}
}
gsl_matrix_free(m);
}
double Matrix::get ( const size_t row, const size_t column ) const {
return gsl_matrix_get( &matrix, row, column );
}
void SteadyState::setupSSmatrix()
{
#ifdef USE_GSL
if ( numVarPools_ == 0 || nReacs_ == 0 )
return;
int nTot = numVarPools_ + nReacs_;
gsl_matrix* N = gsl_matrix_calloc (numVarPools_, nReacs_);
if ( LU_ ) { // Clear out old one.
gsl_matrix_free( LU_ );
}
LU_ = gsl_matrix_calloc (numVarPools_, nTot);
vector< int > entry = Field< vector< int > >::get(
stoich_, "matrixEntry" );
vector< unsigned int > colIndex = Field< vector< unsigned int > >::get(
stoich_, "columnIndex" );
vector< unsigned int > rowStart = Field< vector< unsigned int > >::get(
stoich_, "rowStart" );
// cout << endl << endl;
for ( unsigned int i = 0; i < numVarPools_; ++i ) {
gsl_matrix_set (LU_, i, i + nReacs_, 1 );
unsigned int k = rowStart[i];
// cout << endl << i << ": ";
for ( unsigned int j = 0; j < nReacs_; ++j ) {
double x = 0;
if ( j == colIndex[k] && k < rowStart[i+1] ) {
x = entry[k++];
}
// cout << " " << x;
gsl_matrix_set (N, i, j, x);
gsl_matrix_set (LU_, i, j, x );
}
}
cout << endl << endl;
rank_ = myGaussianDecomp( LU_ );
unsigned int nConsv = numVarPools_ - rank_;
if ( nConsv == 0 ) {
cout << "SteadyState::setupSSmatrix(): Number of conserved species == 0. Aborting\n";
return;
}
if ( Nr_ ) { // Clear out old one.
gsl_matrix_free( Nr_ );
}
Nr_ = gsl_matrix_calloc ( rank_, nReacs_ );
// Fill up Nr.
for ( unsigned int i = 0; i < rank_; i++)
for ( unsigned int j = i; j < nReacs_; j++)
gsl_matrix_set (Nr_, i, j, gsl_matrix_get( LU_, i, j ) );
if ( gamma_ ) { // Clear out old one.
gsl_matrix_free( gamma_ );
}
gamma_ = gsl_matrix_calloc (nConsv, numVarPools_ );
// Fill up gamma
for ( unsigned int i = rank_; i < numVarPools_; ++i )
for ( unsigned int j = 0; j < numVarPools_; ++j )
gsl_matrix_set( gamma_, i - rank_, j,
gsl_matrix_get( LU_, i, j + nReacs_ ) );
// Fill up boundary condition values
total_.resize( nConsv );
total_.assign( nConsv, 0.0 );
/*
cout << "S = (";
for ( unsigned int j = 0; j < numVarPools_; ++j )
cout << s_->S()[ j ] << ", ";
cout << "), Sinit = ( ";
for ( unsigned int j = 0; j < numVarPools_; ++j )
cout << s_->Sinit()[ j ] << ", ";
cout << ")\n";
*/
Id ksolve = Field< Id >::get( stoich_, "ksolve" );
vector< double > nVec =
LookupField< unsigned int, vector< double > >::get(
ksolve,"nVec", 0 );
if ( nVec.size() >= numVarPools_ ) {
for ( unsigned int i = 0; i < nConsv; ++i )
for ( unsigned int j = 0; j < numVarPools_; ++j )
total_[i] += gsl_matrix_get( gamma_, i, j ) * nVec[ j ];
isSetup_ = 1;
} else {
cout << "Error: SteadyState::setupSSmatrix(): unable to get"
"pool numbers from ksolve.\n";
isSetup_ = 0;
}
gsl_matrix_free( N );
#endif
}
/** ****************************************************************************************************
***** calc an individual logistic regression model
*******************************************************************************************************/
void calc_node_Score_binary_rv_R(network *dag, datamatrix *obsdata, int nodeid, int errverbose,
datamatrix *designmatrix, const double priormean, const double priorsd, const double priorgamshape, const double priorgamscale,
const int maxiters, const double epsabs,int storeModes, double epsabs_inner, int maxiters_inner, double finitestepsize, int verbose,
double h_guess, double h_epsabs, int maxiters_hessian, int ModesONLY,
double max_hessian_error,double myfactor_brent, int maxiters_hessian_brent, double num_intervals_brent)
{
#ifdef NOPRIOR
Rprintf("############ Warning - Priors turned off - use only for checking mlik value! ################\n");
#endif
int i,status=GSL_SUCCESS,sss,index=0,iter;
/*int j;*/
gsl_vector *myBeta,*vectmp1,*vectmp2,*vectmp1long,*vectmp2long,*localbeta,*localbeta2,/* *dgvalues,*/ *finitefactors,/* *factorindexes,*/ *finitestepsize_vec=0,*nmstepsize=0;/** this will hold the parameter point estimates + 1 is for precision of rv's */
struct fnparams gparams;/** for passing to the gsl zero finding functions */
double gvalue;double nm_size=0.0;
gsl_matrix *mattmp2,*mattmp3,*mattmp4,*hessgvalues,*hessgvalues3pt;
double mydet=0.0,logscore=0.0;/*,logscore3pt=0.0;*/
gsl_permutation *initsperm;
gsl_permutation *perm=0;
const gsl_multimin_fminimizer_type *T;
gsl_multimin_fminimizer *s;
gsl_multimin_function F;
double lower,upper,lower_f,upper_f;int found=0; double delta=0.0,new_f_min=0.0, finitestepsize_nm=0.0, increLogscale=0.0, best_Error=0.0,best_h=0.0;
const gsl_min_fminimizer_type *T1;
gsl_min_fminimizer *s1;
int n,m;
/* double min_error,cur_error,accurate_logscore=0,accurate_logscore3pt=0,bestsize=0,lowerend,upperend,h_guess,h_epsabs;*/
/*const gsl_min_fminimizer_type *T;
gsl_min_fminimizer *s;
gsl_function F;*/
/*double h_lowerbound[1],h_upperbound[1],h_guess_array[1];
int h_nbd[1];*/
int nDim;/** dimension of optim problem */
int *nbd;/** nbd is an integer array of dimension nDim.
On entry nbd represents the type of bounds imposed on the variables, and must be specified as follows:
nbd(i)=0 if x(i) is unbounded,
1 if x(i) has only a lower bound,
2 if x(i) has both lower and upper bounds, and
3 if x(i) has only an upper bound.
On exit nbd is unchanged.*/
double *lowerbounds,*upperbounds; /* h_gvalue;*//*,lowestHesserror,beststepsize;*/
int failcode;/** check code see R ?optim - if non-zero then a problem **/
double factr=1e-07;/** error size scaler - this is the default value*/
double pgtol=1e-07;/** default value is zero - this is the gradient tolerance - mmm what does that actually mean? */
int fncount,grcount;/** hold number of evaluations */
char msg[60];/** error message */
int trace=errverbose;/** like verbose */
int nREPORT=1000;/** report freq*/
int lmm=5;/** see R ?optim - number of function evals to store - default is 5 */
/** want to find the modes of the function g(betas) where betas=b_0,b_1,,...,tau, the latter being precision */
/** g(betas) is not differentiable as it contains integrals (which themselves need Laplace estimates **/
/** SETUP things which are the same across all data chunks - groups */
/** build design matrix which is designmatrix->datamatrix=X, designmatrix->Y=Y plus priors designmatrix->priorsd, designmatrix->priormean **/
/** NOTE: design matrix here does include the random effect term **/
/** note - numparams does NOT include precision term - numpars +1 */
build_designmatrix_rv(dag,obsdata,priormean, priorsd,priorgamshape,priorgamscale,designmatrix,nodeid,storeModes);
nDim=designmatrix->numparams+1;
lowerbounds=(double *)R_alloc(nDim,sizeof(double*));
upperbounds=(double *)R_alloc(nDim,sizeof(double*));
nbd=(int *)R_alloc(nDim,sizeof(int*));
for(i=0;i<nDim-1;i++){lowerbounds[i]=-DBL_MAX;
upperbounds[i]=DBL_MAX;
nbd[i]=0;}
nbd[nDim-1]=1;lowerbounds[nDim-1]=0.001;/** lower bound for precision */
finitefactors = gsl_vector_alloc(7);/** used to change stepsize in hessian estimate **/
gsl_vector_set(finitefactors,0,1.0E-03);gsl_vector_set(finitefactors,1,1.0E-02);gsl_vector_set(finitefactors,2,1.0E-01);
gsl_vector_set(finitefactors,3,1.0);gsl_vector_set(finitefactors,4,1.0E+01);gsl_vector_set(finitefactors,5,1.0E+02);
gsl_vector_set(finitefactors,6,1.0E+03);
/*factorindexes = gsl_vector_alloc(7);*//** used to change stepsize in hessian estimate **/
/*for(i=0;i<7;i++){gsl_vector_set(factorindexes,i,i);}*/
/** change finite.step.size by 0.1,1, and 10 factors respectively **/
/*dgvalues = gsl_vector_alloc (designmatrix->numparams+1);*//** inc rv precision */
myBeta = gsl_vector_alloc (designmatrix->numparams+1);/** inc rv precision */
vectmp1 = gsl_vector_alloc (designmatrix->numparams);/** scratch space **/
vectmp2 = gsl_vector_alloc (designmatrix->numparams);/** scratch space **/
mattmp2 = gsl_matrix_alloc (obsdata->numDataPts,designmatrix->numparams);
mattmp3 = gsl_matrix_alloc (designmatrix->numparams,designmatrix->numparams);
mattmp4 = gsl_matrix_alloc (designmatrix->numparams,designmatrix->numparams);
initsperm = gsl_permutation_alloc (designmatrix->numparams);/** for use with initial guesses */
vectmp1long = gsl_vector_alloc (obsdata->numDataPts);/** scratch space **/
vectmp2long = gsl_vector_alloc (obsdata->numDataPts);
localbeta = gsl_vector_alloc (designmatrix->numparams);/** scratch space in later functions - excl. precision **/
localbeta2 = gsl_vector_alloc (designmatrix->numparams+1);/** scratch space in later functions - inc. precision **/
hessgvalues = gsl_matrix_alloc (designmatrix->numparams+1,designmatrix->numparams+1);
hessgvalues3pt = gsl_matrix_alloc (designmatrix->numparams+1,designmatrix->numparams+1);
gparams.designdata=designmatrix;
gparams.vectmp1=vectmp1;
gparams.vectmp2=vectmp2;
gparams.mattmp2=mattmp2;
gparams.mattmp3=mattmp3;
gparams.mattmp4=mattmp4;
gparams.perm=initsperm;
gparams.vectmp1long=vectmp1long;
gparams.vectmp2long=vectmp2long;
gparams.beta=localbeta;
gparams.betaincTau=localbeta2;
gparams.epsabs_inner=epsabs_inner;
gparams.maxiters_inner=maxiters_inner;
gparams.verbose=verbose;
gparams.finitestepsize=finitestepsize;
dag->nodeScoresErrCode[nodeid]=0;/** reset error code to no error **/
/*status=GSL_SUCCESS;*/
generate_rv_inits(myBeta,&gparams);
/*Rprintf("starting optimisation\n");*/
/** run a loop over different stepsize - starting with the smallest first as this is more likely successful **/
for(i=0;i<finitefactors->size;i++){/** go forwards through the factors so start with SMALLEST STEPSIZE */
/*Rprintf("step size iteration %d\n",i);*/
failcode=0;/** reset*/
gparams.finitestepsize=gsl_vector_get(finitefactors,i)*finitestepsize;
lbfgsb(nDim, lmm, myBeta->data, lowerbounds, upperbounds, nbd, &gvalue, &g_outer_R,
&rv_dg_outer_R, &failcode,
&gparams,
factr,
pgtol, &fncount, &grcount,
maxiters, msg, trace, nREPORT);
if(!failcode){dag->nodeScoresErrCode[nodeid]=0;/*bestsize=gparams.finitestepsize;*/break;}/** break out of for loop if no error as we are done **/
} /** end of for loop so now have mode estimates */
if(failcode){Rprintf("%s at node %d\n",msg,nodeid+1);/** notify if there is an error and set final error code **/
dag->nodeScoresErrCode[nodeid]=1;
}
gparams.finitestepsize=finitestepsize;/** reset */
if(storeModes){/** keep a copy of the parameter modes found for use later in other function calls etc**/
index=0; /*Rprintf("size of beta=%d %f %f\n",myBeta->size, gsl_vector_get(myBeta,0),gsl_vector_get(myBeta,1));*/
for(i=0;i<dag->numNodes+3;i++){/** roll myBeta into dag->modes into the appropriate columns**/
if(gsl_matrix_get(dag->modes,nodeid,i)!=DBL_MAX){
gsl_matrix_set(dag->modes,nodeid,i,gsl_vector_get(myBeta,index++));}}
/*for(i=0;i<dag->numNodes+3;i++){Rprintf("%e ",gsl_matrix_get(dag->modes,nodeid,i));}Rprintf("\n");*/
}
if(!ModesONLY){/** only want modes so can skip the rest **/
/** now compute the hessian at the step size with lowest error **/
/*Rprintf("starting hessian estimation\n");*/
n=obsdata->numDataPts;
m=designmatrix->numparams+1;/** inc precision */
perm = gsl_permutation_alloc (m);
/** just re-use as much of existing gparams as possible - so names are meaningless e.g. betafixed is actually gvalue */
gparams.betaincTau=myBeta;
gparams.nDim=n;
gparams.mDim=m;
gparams.perm=perm;
gparams.mattmp2=hessgvalues;
gparams.mattmp3=hessgvalues3pt;
gparams.betafixed=gvalue;
F.f = &compute_mlik_nm;
F.params = &gparams;
F.n = 1;
T = gsl_multimin_fminimizer_nmsimplex2;
s = gsl_multimin_fminimizer_alloc (T, 1);
finitestepsize_vec = gsl_vector_alloc (1);
gsl_vector_set (finitestepsize_vec, 0, h_guess);
nmstepsize = gsl_vector_alloc (1);
gsl_vector_set_all (nmstepsize, h_guess);
gsl_multimin_fminimizer_set (s, &F, finitestepsize_vec,nmstepsize);
status = GSL_SUCCESS;
iter=0;
do
{
iter++;/*Rprintf("iter=%d\n",iter);*/
status = gsl_multimin_fminimizer_iterate (s);
if (status)
break;
nm_size = gsl_multimin_fminimizer_size (s);
status = gsl_multimin_test_size (nm_size, h_epsabs);
/*
if (status == GSL_SUCCESS)
{
Rprintf ("converged to minimum at\n");
}
*/
/*Rprintf ("iter=%5d error in mlik=%3.5e using fin.diff step= %3.2e nmsize=%3.2e\n", iter,s->fval,gsl_vector_get (s->x, 0),nm_size);*/
}
while (status == GSL_CONTINUE && iter < maxiters_hessian);
if( (status != GSL_SUCCESS)){/*actual_status=status;*//** copy for use later **/
status=GSL_FAILURE;} /** solution failed to achieve a value below h_epsabs **/
finitestepsize=gsl_vector_get(s->x,0);/** get best fin.diff stepsize **/
finitestepsize_nm=finitestepsize;/** save nelder mead estimate */
dag->hessianError[nodeid]= s->fval;/** get fin.diff error **/
gsl_multimin_fminimizer_free (s);
/** README - it might be possible to avoid the brent by increasing the epsabs error in nelder mead (and no. of iterations), although for hard cases
this probably will not work but may give a little greater accuracy for easier cases, These are the hessian.params arg in R */
if(dag->hessianError[nodeid]!=DBL_MAX && dag->hessianError[nodeid]>max_hessian_error){Rprintf("Error in mlik = %e > tolerance of %e so continuing optimisation using Brent initial guess h=%e\n",
dag->hessianError[nodeid],max_hessian_error,finitestepsize);
/* Rprintf("stepsize after NM= %e\n",finitestepsize);*/
T1 = gsl_min_fminimizer_brent;
s1 = gsl_min_fminimizer_alloc (T1);
/** must find lower and upper such that f(lower)<f(finitestepsize)<f(upper) **/
/** use an interval of lower=finitestepsize/FACTOR, upper=finitestepsize*FACTOR and then start at the lower end and travel up until
find suitable endpoints - seems to work but not exactly fast!**/
best_Error=dag->hessianError[nodeid];/** original error from nelder */
best_h=finitestepsize; /** original stepsize from nelder */
found=0;/** flag for found good result */
lower=finitestepsize/myfactor_brent;
upper=myfactor_brent*finitestepsize;
lower_f=compute_mlik_brent(lower, &gparams);/** value at lower point **/
upper_f=compute_mlik_brent(upper, &gparams);/** value at higher point **/
increLogscale=(gsl_sf_log(upper)-gsl_sf_log(lower))/num_intervals_brent;/** on a log scale */
for(delta=gsl_sf_log(lower)+increLogscale;delta<gsl_sf_log(upper);delta+=increLogscale){/** linear increments on a log scale **/
R_CheckUserInterrupt();/** allow an interupt from R console */
/** find a point which has f(x) lower than f(lower) and f(upper) **/
new_f_min=compute_mlik_brent(gsl_sf_exp(delta), &gparams);
/* Rprintf("lower=%e, delta=%e, upper=%e\n",lower,gsl_sf_exp(delta),upper);*/
if(lower_f>new_f_min && new_f_min<upper_f && get_best_stepsize(gsl_sf_exp(delta),lower,upper,maxiters_hessian_brent,&gparams, &compute_mlik_brent,
s1,&finitestepsize,&(dag->hessianError[nodeid]) )<=max_hessian_error){/** have an interval suitable for bracketing **/
/** above is address so can store error withouth rerunning function */
/*finitestepsize=delta;*/
found=1;
status=GSL_SUCCESS;
break;/** break out of delta - so have found new x_min **/
} else {/** have not got a good enough error but save the best error and stepsize so far found **/
if(dag->hessianError[nodeid]<best_Error){best_Error=dag->hessianError[nodeid];
best_h=finitestepsize;
}
}
} /** end of search for interval and good error **/
if(!found){/** have not found a suitably small error but may have found a better error than nelder mead **/
/** best_Error will either be the original nelder mean value or better, and best_h is the corresponding stepsize**/
dag->hessianError[nodeid]=best_Error;
finitestepsize=best_h;
/** reset back to nelder-mead estimate **/
status=GSL_FAILURE;/** set to failure since we did not achieve the lower error asked for */
Rprintf("failed to meet tolerance of %e and using best error estimate found of %e\n",max_hessian_error,dag->hessianError[nodeid]);}
gsl_min_fminimizer_free (s1);
} /** end of error being too large **/
if(dag->hessianError[nodeid]==DBL_MAX){/** in this case nelder mead could not estimate the hessian error so abort as something is probably
very wrong here */
error("");}/** use the R tryCatch rather than the switch for status below **/
switch(status){ /** choose which type of node we have */
case GSL_SUCCESS:{
/** successful finite diff so now do final computation with the optimal step size **/
/*Rprintf("search for optimal step size : status = %s at nodeid %d\n", gsl_strerror (status),nodeid+1);*/
rv_hessg_outer(myBeta,&gparams, hessgvalues,finitestepsize,hessgvalues3pt);/** EDIT BACK to "finitestepsize" start with LARGEST STEPSIZE **/
/* Rprintf("HESSIAN using stepsize=%e\n",finitestepsize);
for(i1=0;i1<hessgvalues->size1;i1++){
for(i2=0;i2<hessgvalues->size2;i2++){Rprintf("%e ",gsl_matrix_get(hessgvalues,i1,i2));}Rprintf("\n");} */
status=gsl_linalg_LU_decomp(hessgvalues,perm,&sss);
mydet=gsl_linalg_LU_lndet(hessgvalues);/** compute determinant but this might be a nan - overflow? gsl_linalg_LU_lndet*/
logscore= -n*gvalue-0.5*mydet+(m/2.0)*log((2.0*M_PI)/n);/** this is the final value */
if(gsl_isnan(logscore)){logscore= R_NaN;dag->nodeScoresErrCode[nodeid]=2;}
dag->nodeScores[nodeid]=logscore;
break;
}
case GSL_FAILURE: {/** the minimiser did not find a minimum meeting the accuracy requirements and so may be unreliable **/
Rprintf ("-- ERROR! -- search for optimal step size error: status = %s at nodeid %d\n", gsl_strerror (status),nodeid+1);
rv_hessg_outer(myBeta,&gparams, hessgvalues,finitestepsize,hessgvalues3pt);/** start with LARGEST STEPSIZE **/
/*Rprintf("HESSIAN using stepsize=%e\n",finitestepsize);
for(i1=0;i1<hessgvalues->size1;i1++){
for(i2=0;i2<hessgvalues->size2;i2++){Rprintf("%e ",gsl_matrix_get(hessgvalues,i1,i2));}Rprintf("\n");} */
status=gsl_linalg_LU_decomp(hessgvalues,perm,&sss);
mydet=gsl_linalg_LU_lndet(hessgvalues);/** compute determinant but this might be a nan - overflow? gsl_linalg_LU_lndet*/
logscore= -n*gvalue-0.5*mydet+(m/2.0)*log((2.0*M_PI)/n);/** this is the final value */
dag->nodeScoresErrCode[nodeid]=4;
if(gsl_isnan(logscore)){logscore= R_NaN;dag->nodeScoresErrCode[nodeid]=2;}
dag->nodeScores[nodeid]=logscore;
break;
}
default:{Rprintf("got case %s\n",gsl_strerror (status)); error("in default switch in calc_node_Score_binary_rv_R() - should never get here!");}
}
/** try the bounded search for h stepsize rather than one-dim min which needs bound specified **/
} /** end of ModesONLY **/
/** now free up allocated memory **/
for(i=0;i<designmatrix->numUnqGrps;i++){gsl_matrix_free(designmatrix->array_of_designs[i]);
gsl_vector_free(designmatrix->array_of_Y[i]);}
gsl_vector_free(designmatrix->priormean);
gsl_vector_free(designmatrix->priorsd);
gsl_vector_free(designmatrix->priorgamshape);
gsl_vector_free(designmatrix->priorgamscale);
gsl_vector_free(designmatrix->Y);
gsl_matrix_free(designmatrix->datamatrix_noRV);
/*gsl_vector_free(dgvalues);*/
gsl_vector_free(myBeta);
gsl_vector_free(vectmp1);
gsl_vector_free(vectmp2);
gsl_matrix_free(mattmp2);
gsl_matrix_free(mattmp3);
gsl_matrix_free(mattmp4);
gsl_permutation_free(initsperm);
gsl_vector_free(vectmp1long);
gsl_vector_free(vectmp2long);
gsl_vector_free(localbeta);
gsl_vector_free(localbeta2);
gsl_matrix_free(hessgvalues);
gsl_matrix_free(hessgvalues3pt);
gsl_vector_free(finitefactors);
/*gsl_vector_free(factorindexes);*/
if(!ModesONLY){/** didn't allocate these so don't unallocate! */
gsl_permutation_free(perm);
gsl_vector_free(finitestepsize_vec);
gsl_vector_free(nmstepsize);}
/*if(!failcode){*//*}*/
/*dag->nodeScores[nodeid]=logscore;*/
}
/**
* The settle function computes the steady state nearest the initial
* conditions.
*/
void SteadyState::settle( bool forceSetup )
{
#ifdef USE_GSL
gsl_set_error_handler_off();
if ( !isInitialized_ ) {
cout << "Error: SteadyState object has not been initialized. No calculations done\n";
return;
}
if ( forceSetup || isSetup_ == 0 ) {
setupSSmatrix();
}
// Setting up matrices and vectors for the calculation.
unsigned int nConsv = numVarPools_ - rank_;
double * T = (double *) calloc( nConsv, sizeof( double ) );
unsigned int i, j;
Id ksolve = Field< Id >::get( stoich_, "ksolve" );
struct reac_info ri;
ri.rank = rank_;
ri.num_reacs = nReacs_;
ri.num_mols = numVarPools_;
ri.T = T;
ri.Nr = Nr_;
ri.gamma = gamma_;
ri.pool = &pool_;
ri.nVec =
LookupField< unsigned int, vector< double > >::get(
ksolve,"nVec", 0 );
ri.convergenceCriterion = convergenceCriterion_;
// Fill up boundary condition values
if ( reassignTotal_ ) { // The user has defined new conservation values.
for ( i = 0; i < nConsv; ++i )
T[i] = total_[i];
reassignTotal_ = 0;
} else {
for ( i = 0; i < nConsv; ++i )
for ( j = 0; j < numVarPools_; ++j )
T[i] += gsl_matrix_get( gamma_, i, j ) * ri.nVec[ j ];
total_.assign( T, T + nConsv );
}
vector< double > repair( numVarPools_, 0.0 );
for ( unsigned int j = 0; j < numVarPools_; ++j )
repair[j] = ri.nVec[j];
int status = iterate( gsl_multiroot_fsolver_hybrids, &ri, maxIter_ );
if ( status ) // It failed. Fall back with the Newton method
status = iterate( gsl_multiroot_fsolver_dnewton, &ri, maxIter_ );
status_ = string( gsl_strerror( status ) );
nIter_ = ri.nIter;
if ( status == GSL_SUCCESS && isSolutionPositive( ri.nVec ) ) {
solutionStatus_ = 0; // Good solution
LookupField< unsigned int, vector< double > >::set(
ksolve,"nVec", 0, ri.nVec );
classifyState( T );
} else {
cout << "Warning: SteadyState iteration failed, status = " <<
status_ << ", nIter = " << nIter_ << endl;
// Repair the mess
for ( unsigned int j = 0; j < numVarPools_; ++j )
ri.nVec[j] = repair[j];
solutionStatus_ = 1; // Steady state failed.
LookupField< unsigned int, vector< double > >::set(
ksolve,"nVec", 0, ri.nVec );
}
// Clean up.
free( T );
#endif
}
/**
* Compute the fourier decomposition of array to -m:m in angular components and 1:nmax in radial components
*
* Note that the gridding scheme used here is defined on [-1..1] x [-1..1], the cmx and cmy specified here should
* be given in these units. The function "compute_com" defined below can be used to do this.
*
* @arg mmax - largest angular moment computed
* @arg nmax - largest radial moment computed
* @arg array - 2d matrix (npts x npts) of energy density in the event
* @arg npts - number of points in in the array
* @arg AmnReal - 2d matrix ((2*mmax+1) x nmax), filled with Real parts of the coeffs on return
* @arg AmnIm - 2d matrix ((2*mmax+1) x nmax), filled with Im parts of the coeffs on return
* @arg cmx - x location of the CM of the event
* @arg cmy - y location of the CM of the event
*/
void compute_amn(int mmax, int nmax, gsl_matrix *array, int npts, gsl_matrix* AmnReal, gsl_matrix* AmnIm, double cmx, double cmy)
{
int i,j,k,l;
int nm, nn;
int mtemp, ntemp;
double dx;// = 2/((double)npts-1);
double dxy;// = 4/pow(((double)npts-1),2.0);
double xv, yv;
double coeff = 0;
double phiMod, phiRe, phiIm;
double ftemp;
double AmnRealAcc, AmnImAcc;
// for compensated summation
double alphaRe, alphaIm;
double epsRe, epsIm;
double rzero = fabs(xmin);
dx = 2*rzero/((double)npts-1);
dxy = 4*pow(rzero,2.0)/pow(((double)npts-1),2.0);
//printf("# xmin: %lf dx: %lf dxy: %lf\n", xmin, dx, dxy);
gsl_vector *xvec = gsl_vector_alloc(npts);
gsl_matrix * rMat = gsl_matrix_alloc(npts, npts);
gsl_matrix * thMat = gsl_matrix_alloc(npts, npts);
gsl_matrix *lamMat = NULL;
gsl_matrix_set_zero(rMat);
gsl_matrix_set_zero(thMat);
gsl_vector_set_zero(xvec);
nm = 2*mmax+1;
nn = nmax;
lamMat = gsl_matrix_alloc(nm, nn);
// fill in r and Theta matrices
for(i = 0; i < npts; i++)
gsl_vector_set(xvec ,i, xmin + dx*i);
for(i = 0; i < npts; i++){
xv = gsl_vector_get(xvec, i);
for(j = 0; j < npts;j ++){
yv = gsl_vector_get(xvec, j);
gsl_matrix_set(rMat, i, j, sqrt((xv-cmx)*(xv-cmx) + (yv-cmy)*(yv-cmy)));
gsl_matrix_set(thMat, i, j, atan2((yv-cmy), (xv-cmx)));
}
}
// fill in lambda matrix
for(i=0; i < nm; i++){
for(j = 0; j < nn; j++){
ntemp = j + 1;
mtemp = -1.0 * mmax + i;
gsl_matrix_set(lamMat, i, j, gsl_sf_bessel_zero_Jnu(fabs(mtemp), ntemp));
}
}
for(i = 0; i < nm; i++){
for(j = 0; j < nn; j++){
AmnImAcc = 0.0;
AmnRealAcc = 0.0;
epsRe = 0.0;
epsIm = 0.0;
ntemp = j + 1;
mtemp = -1.0*mmax + i;
// note that we have to scale the coeff by rzero, then the system is properly scale invariant
coeff = pow(rzero,2)*sqrt(M_PI)*gsl_sf_bessel_Jn(fabs(mtemp)+1, gsl_matrix_get(lamMat, i, j));
// now loop over the grid, a lot
for(k = 0; k < npts; k++){
for(l = 0; l < npts; l++){
phiMod = gsl_sf_bessel_Jn(mtemp, gsl_matrix_get(lamMat, i, j)*gsl_matrix_get(rMat, k, l)/rzero) / coeff;
phiRe = phiMod * cos(mtemp*gsl_matrix_get(thMat, k, l));
phiIm = phiMod * sin(mtemp*gsl_matrix_get(thMat, k, l));
ftemp = gsl_matrix_get(array, k, l);
/* kahan compensated summation (http://en.wikipedia.org/wiki/Kahan_summation_algorithm)
* we're adding up a lot of little numbers here
* this trick keeps accumulation errors from, well, accumulating
*/
alphaRe = AmnRealAcc;
epsRe += ftemp * phiRe;
AmnRealAcc = alphaRe + epsRe;
epsRe += (alphaRe - AmnRealAcc);
alphaIm = AmnImAcc;
epsIm += ftemp * phiIm;
AmnImAcc = alphaIm + epsIm;
epsIm += (alphaIm - AmnImAcc);
}
}
// and save the coeffs
gsl_matrix_set(AmnReal, i, j, AmnRealAcc*dxy);
gsl_matrix_set(AmnIm, i, j, -1.0 * AmnImAcc*dxy);
}
}
gsl_matrix_free(rMat);
gsl_matrix_free(thMat);
gsl_vector_free(xvec);
gsl_matrix_free(lamMat);
}
void
test_filip ()
{
size_t i, j;
{
gsl_multifit_linear_workspace * work =
gsl_multifit_linear_alloc (filip_n, filip_p);
gsl_matrix * X = gsl_matrix_alloc (filip_n, filip_p);
gsl_vector_view y = gsl_vector_view_array (filip_y, filip_n);
gsl_vector * c = gsl_vector_alloc (filip_p);
gsl_matrix * cov = gsl_matrix_alloc (filip_p, filip_p);
gsl_vector_view diag;
double chisq;
double expected_c[11] = { -1467.48961422980,
-2772.17959193342,
-2316.37108160893,
-1127.97394098372,
-354.478233703349,
-75.1242017393757,
-10.8753180355343,
-1.06221498588947,
-0.670191154593408E-01,
-0.246781078275479E-02,
-0.402962525080404E-04 };
double expected_sd[11] = { 298.084530995537,
559.779865474950,
466.477572127796,
227.204274477751,
71.6478660875927,
15.2897178747400,
2.23691159816033,
0.221624321934227,
0.142363763154724E-01,
0.535617408889821E-03,
0.896632837373868E-05 };
double expected_chisq = 0.795851382172941E-03;
for (i = 0 ; i < filip_n; i++)
{
for (j = 0; j < filip_p; j++)
{
gsl_matrix_set(X, i, j, pow(filip_x[i], j));
}
}
gsl_multifit_linear (X, &y.vector, c, cov, &chisq, work);
gsl_test_rel (gsl_vector_get(c,0), expected_c[0], 1e-7, "filip gsl_fit_multilinear c0") ;
gsl_test_rel (gsl_vector_get(c,1), expected_c[1], 1e-7, "filip gsl_fit_multilinear c1") ;
gsl_test_rel (gsl_vector_get(c,2), expected_c[2], 1e-7, "filip gsl_fit_multilinear c2") ;
gsl_test_rel (gsl_vector_get(c,3), expected_c[3], 1e-7, "filip gsl_fit_multilinear c3") ;
gsl_test_rel (gsl_vector_get(c,4), expected_c[4], 1e-7, "filip gsl_fit_multilinear c4") ;
gsl_test_rel (gsl_vector_get(c,5), expected_c[5], 1e-7, "filip gsl_fit_multilinear c5") ;
gsl_test_rel (gsl_vector_get(c,6), expected_c[6], 1e-7, "filip gsl_fit_multilinear c6") ;
gsl_test_rel (gsl_vector_get(c,7), expected_c[7], 1e-7, "filip gsl_fit_multilinear c7") ;
gsl_test_rel (gsl_vector_get(c,8), expected_c[8], 1e-7, "filip gsl_fit_multilinear c8") ;
gsl_test_rel (gsl_vector_get(c,9), expected_c[9], 1e-7, "filip gsl_fit_multilinear c9") ;
gsl_test_rel (gsl_vector_get(c,10), expected_c[10], 1e-7, "filip gsl_fit_multilinear c10") ;
diag = gsl_matrix_diagonal (cov);
gsl_test_rel (gsl_vector_get(&diag.vector,0), pow(expected_sd[0],2.0), 1e-6, "filip gsl_fit_multilinear cov00") ;
gsl_test_rel (gsl_vector_get(&diag.vector,1), pow(expected_sd[1],2.0), 1e-6, "filip gsl_fit_multilinear cov11") ;
gsl_test_rel (gsl_vector_get(&diag.vector,2), pow(expected_sd[2],2.0), 1e-6, "filip gsl_fit_multilinear cov22") ;
gsl_test_rel (gsl_vector_get(&diag.vector,3), pow(expected_sd[3],2.0), 1e-6, "filip gsl_fit_multilinear cov33") ;
gsl_test_rel (gsl_vector_get(&diag.vector,4), pow(expected_sd[4],2.0), 1e-6, "filip gsl_fit_multilinear cov44") ;
gsl_test_rel (gsl_vector_get(&diag.vector,5), pow(expected_sd[5],2.0), 1e-6, "filip gsl_fit_multilinear cov55") ;
gsl_test_rel (gsl_vector_get(&diag.vector,6), pow(expected_sd[6],2.0), 1e-6, "filip gsl_fit_multilinear cov66") ;
gsl_test_rel (gsl_vector_get(&diag.vector,7), pow(expected_sd[7],2.0), 1e-6, "filip gsl_fit_multilinear cov77") ;
gsl_test_rel (gsl_vector_get(&diag.vector,8), pow(expected_sd[8],2.0), 1e-6, "filip gsl_fit_multilinear cov88") ;
gsl_test_rel (gsl_vector_get(&diag.vector,9), pow(expected_sd[9],2.0), 1e-6, "filip gsl_fit_multilinear cov99") ;
gsl_test_rel (gsl_vector_get(&diag.vector,10), pow(expected_sd[10],2.0), 1e-6, "filip gsl_fit_multilinear cov1010") ;
gsl_test_rel (chisq, expected_chisq, 1e-7, "filip gsl_fit_multilinear chisq") ;
gsl_vector_free(c);
gsl_matrix_free(cov);
gsl_matrix_free(X);
gsl_multifit_linear_free (work);
}
{
gsl_multifit_linear_workspace * work =
gsl_multifit_linear_alloc (filip_n, filip_p);
gsl_matrix * X = gsl_matrix_alloc (filip_n, filip_p);
gsl_vector_view y = gsl_vector_view_array (filip_y, filip_n);
gsl_vector * w = gsl_vector_alloc (filip_n);
gsl_vector * c = gsl_vector_alloc (filip_p);
gsl_matrix * cov = gsl_matrix_alloc (filip_p, filip_p);
double chisq;
double expected_c[11] = { -1467.48961422980,
-2772.17959193342,
-2316.37108160893,
-1127.97394098372,
-354.478233703349,
-75.1242017393757,
-10.8753180355343,
-1.06221498588947,
-0.670191154593408E-01,
-0.246781078275479E-02,
-0.402962525080404E-04 };
/* computed using GNU Calc */
double expected_cov[11][11] ={ { 7.9269341767252183262588583867942e9, 1.4880416622254098343441063389706e10, 1.2385811858111487905481427591107e10, 6.0210784406215266653697715794241e9, 1.8936652526181982747116667336389e9, 4.0274900618493109653998118587093e8, 5.8685468011819735806180092394606e7, 5.7873451475721689084330083708901e6, 3.6982719848703747920663262917032e5, 1.3834818802741350637527054170891e4, 2.301758578713219280719633494302e2 },
{ 1.4880416622254098334697515488559e10, 2.7955091668548290835529555438088e10, 2.3286604504243362691678565997033e10, 1.132895006796272983689297219686e10, 3.5657281653312473123348357644683e9, 7.5893300392314445528176646366087e8, 1.1066654886143524811964131660002e8, 1.0921285448484575110763947787775e7, 6.9838139975394769253353547606971e5, 2.6143091775349597218939272614126e4, 4.3523386330348588614289505633539e2 },
{ 1.2385811858111487890788272968677e10, 2.3286604504243362677757802422747e10, 1.9412787917766676553608636489674e10, 9.4516246492862131849077729250098e9, 2.9771226694709917550143152097252e9, 6.3413035086730038062129508949859e8, 9.2536164488309401636559552742339e7, 9.1386304643423333815338760248027e6, 5.8479478338916429826337004060941e5, 2.1905933113294737443808429764554e4, 3.6493161325305557266196635180155e2 },
{ 6.0210784406215266545770691532365e9, 1.1328950067962729823273441573365e10, 9.4516246492862131792040001429636e9, 4.6053152992000107509329772255094e9, 1.4517147860312147098138030287038e9, 3.0944988323328589376402579060072e8, 4.5190223822292688669369522708712e7, 4.4660958693678497534529855690752e6, 2.8599340736122198213681258676423e5, 1.0720394998549386596165641244705e4, 1.7870937745661967319298031044424e2 },
{ 1.8936652526181982701620450132636e9, 3.5657281653312473058825073094524e9, 2.9771226694709917514149924058297e9, 1.451714786031214708936087401632e9, 4.5796563896564815123074920050827e8, 9.7693972414561515534525103622773e7, 1.427717861635658545863942948444e7, 1.4120161287735817621354292900338e6, 9.0484361228623960006818614875557e4, 3.394106783764852373199087455398e3, 5.6617406468519495376287407526295e1 },
{ 4.0274900618493109532650887473599e8, 7.589330039231444534478894935778e8, 6.3413035086730037947153564986653e8, 3.09449883233285893390542947998e8, 9.7693972414561515475770399055121e7, 2.0855726248311948992114244257719e7, 3.0501263034740400533872858749566e6, 3.0187475839310308153394428784224e5, 1.9358204633534233524477930175632e4, 7.2662989867560017077361942813911e2, 1.2129002231061036467607394277965e1 },
{ 5.868546801181973559370854830868e7, 1.1066654886143524778548044386795e8, 9.2536164488309401413296494869777e7, 4.5190223822292688587853853162072e7, 1.4277178616356585441556046753562e7, 3.050126303474040051574715592746e6, 4.4639982579046340884744460329946e5, 4.4212093985989836047285007760238e4, 2.8371395028774486687625333589972e3, 1.0656694507620102300567296504381e2, 1.7799982046359973175080475654123e0 },
{ 5.7873451475721688839974153925406e6, 1.0921285448484575071271480643397e7, 9.1386304643423333540728480344578e6, 4.4660958693678497427674903565664e6, 1.4120161287735817596182229182587e6, 3.0187475839310308117812257613082e5, 4.4212093985989836021482392757677e4, 4.3818874017028389517560906916315e3, 2.813828775753142855163154605027e2, 1.0576188138416671883232607188969e1, 1.7676976288918295012452853715408e-1 },
{ 3.6982719848703747742568351456818e5, 6.9838139975394768959780068745979e5, 5.8479478338916429616547638954781e5, 2.8599340736122198128717796825489e5, 9.0484361228623959793493985226792e4, 1.9358204633534233490579641064343e4, 2.8371395028774486654873647731797e3, 2.8138287757531428535592907878017e2, 1.8081118503579798222896804627964e1, 6.8005074291434681866415478598732e-1, 1.1373581557749643543869665860719e-2 },
{ 1.3834818802741350562839757244708e4, 2.614309177534959709397445440919e4, 2.1905933113294737352721470167247e4, 1.0720394998549386558251721913182e4, 3.3941067837648523632905604575131e3, 7.2662989867560016909534954790835e2, 1.0656694507620102282337905013451e2, 1.0576188138416671871337685672492e1, 6.8005074291434681828743281967838e-1, 2.5593857187900736057022477529078e-2, 4.2831487599116264442963102045936e-4 },
{ 2.3017585787132192669801658674163e2, 4.3523386330348588381716460685124e2, 3.6493161325305557094116270974735e2, 1.7870937745661967246233792737255e2, 5.6617406468519495180024059284629e1, 1.2129002231061036433003571679329e1, 1.7799982046359973135014027410646e0, 1.7676976288918294983059118597214e-1, 1.137358155774964353146460100337e-2, 4.283148759911626442000316269063e-4, 7.172253875245080423800933453952e-6 } };
double expected_chisq = 0.795851382172941E-03;
for (i = 0 ; i < filip_n; i++)
{
for (j = 0; j < filip_p; j++)
{
gsl_matrix_set(X, i, j, pow(filip_x[i], j));
}
}
gsl_vector_set_all (w, 1.0);
gsl_multifit_wlinear (X, w, &y.vector, c, cov, &chisq, work);
gsl_test_rel (gsl_vector_get(c,0), expected_c[0], 1e-7, "filip gsl_fit_multilinear c0") ;
gsl_test_rel (gsl_vector_get(c,1), expected_c[1], 1e-7, "filip gsl_fit_multilinear c1") ;
gsl_test_rel (gsl_vector_get(c,2), expected_c[2], 1e-7, "filip gsl_fit_multilinear c2") ;
gsl_test_rel (gsl_vector_get(c,3), expected_c[3], 1e-7, "filip gsl_fit_multilinear c3") ;
gsl_test_rel (gsl_vector_get(c,4), expected_c[4], 1e-7, "filip gsl_fit_multilinear c4") ;
gsl_test_rel (gsl_vector_get(c,5), expected_c[5], 1e-7, "filip gsl_fit_multilinear c5") ;
gsl_test_rel (gsl_vector_get(c,6), expected_c[6], 1e-7, "filip gsl_fit_multilinear c6") ;
gsl_test_rel (gsl_vector_get(c,7), expected_c[7], 1e-7, "filip gsl_fit_multilinear c7") ;
gsl_test_rel (gsl_vector_get(c,8), expected_c[8], 1e-7, "filip gsl_fit_multilinear c8") ;
gsl_test_rel (gsl_vector_get(c,9), expected_c[9], 1e-7, "filip gsl_fit_multilinear c9") ;
gsl_test_rel (gsl_vector_get(c,10), expected_c[10], 1e-7, "filip gsl_fit_multilinear c10") ;
for (i = 0; i < filip_p; i++)
{
for (j = 0; j < filip_p; j++)
{
gsl_test_rel (gsl_matrix_get(cov,i,j), expected_cov[i][j], 1e-6,
"filip gsl_fit_wmultilinear cov(%d,%d)", i, j) ;
}
}
gsl_test_rel (chisq, expected_chisq, 1e-7, "filip gsl_fit_multilinear chisq") ;
gsl_vector_free(w);
gsl_vector_free(c);
gsl_matrix_free(cov);
gsl_matrix_free(X);
gsl_multifit_linear_free (work);
}
}
