static enum efp_result
set_coord_points(struct frag *frag, const double *coord)
{
if (frag->n_atoms < 3) {
efp_log("fragment must contain at least three atoms");
return EFP_RESULT_FATAL;
}
double ref[9] = {
frag->lib->atoms[0].x, frag->lib->atoms[0].y, frag->lib->atoms[0].z,
frag->lib->atoms[1].x, frag->lib->atoms[1].y, frag->lib->atoms[1].z,
frag->lib->atoms[2].x, frag->lib->atoms[2].y, frag->lib->atoms[2].z
};
vec_t p1;
mat_t rot1, rot2;
efp_points_to_matrix(coord, &rot1);
efp_points_to_matrix(ref, &rot2);
rot2 = mat_transpose(&rot2);
frag->rotmat = mat_mat(&rot1, &rot2);
p1 = mat_vec(&frag->rotmat, VEC(frag->lib->atoms[0].x));
/* center of mass */
frag->x = coord[0] - p1.x;
frag->y = coord[1] - p1.y;
frag->z = coord[2] - p1.z;
update_fragment(frag);
return EFP_RESULT_SUCCESS;
}
static void
add_hydrogens(struct sys *sys, int i, int j, int k, int offset, int count)
{
mat_t rotmat;
vec_t pi, pj, pk;
pi = sys_get_atom_xyz(sys, i);
if (j < 0) {
pj = vec_random();
pj = vec_add(&pj, &pi);
} else
pj = sys_get_atom_xyz(sys, j);
if (k < 0) {
pk = vec_random();
pk = vec_add(&pk, &pi);
} else
pk = sys_get_atom_xyz(sys, k);
rotmat = mat_align(&pi, &pj, &pk);
for (k = 0; k < count; k++) {
pk = h_table[offset + k];
pk = mat_vec(&rotmat, &pk);
pk = vec_add(&pk, &pi);
sys_add_atom(sys, "H", pk);
j = sys_get_atom_count(sys) - 1;
graph_edge_create(sys->graph, i, j, 1);
}
}
void MatrixIdentConcat::Vp_StMtV(
VectorMutable* y, value_type a, BLAS_Cpp::Transp M_trans
, const Vector& x, value_type b
) const
{
AbstractLinAlgPack::Vp_MtV_assert_compatibility(y,*this,M_trans,x);
mat_vec( y, a, alpha(), D(), D_trans(), D_rng(), I_rng(), M_trans, x, b );
}
int main(int argc, char *argv[]) {
CASSERTION_INIT(argc, argv);
load();
run();
mat_vec();
CASSERTION_RESULTS();
return EXIT_SUCCESS;
}
static void
compute_rhs(const struct efp *efp, vec_t *id, bool conj)
{
for (size_t i = 0, idx = 0; i < efp->n_frag; i++) {
const struct frag *frag = efp->frags + i;
for (size_t j = 0; j < frag->n_polarizable_pts; j++, idx++) {
const struct polarizable_pt *pt = frag->polarizable_pts + j;
vec_t field = vec_add(&pt->elec_field, &pt->elec_field_wf);
if (conj)
id[idx] = mat_trans_vec(&pt->tensor, &field);
else
id[idx] = mat_vec(&pt->tensor, &field);
}
}
}
static void
compute_id_range(struct efp *efp, size_t from, size_t to, void *data)
{
double conv = 0.0;
vec_t *id_new, *id_conj_new;
id_new = ((struct id_work_data *)data)->id_new;
id_conj_new = ((struct id_work_data *)data)->id_conj_new;
#ifdef _OPENMP
#pragma omp parallel for schedule(dynamic) reduction(+:conv)
#endif
for (size_t i = from; i < to; i++) {
struct frag *frag = efp->frags + i;
for (size_t j = 0; j < frag->n_polarizable_pts; j++) {
struct polarizable_pt *pt = frag->polarizable_pts + j;
size_t idx = frag->polarizable_offset + j;
vec_t field, field_conj;
/* electric field from other induced dipoles */
get_induced_dipole_field(efp, i, pt, &field, &field_conj);
/* add field that doesn't change during scf */
field.x += pt->elec_field.x + pt->elec_field_wf.x;
field.y += pt->elec_field.y + pt->elec_field_wf.y;
field.z += pt->elec_field.z + pt->elec_field_wf.z;
field_conj.x += pt->elec_field.x + pt->elec_field_wf.x;
field_conj.y += pt->elec_field.y + pt->elec_field_wf.y;
field_conj.z += pt->elec_field.z + pt->elec_field_wf.z;
id_new[idx] = mat_vec(&pt->tensor, &field);
id_conj_new[idx] = mat_trans_vec(&pt->tensor, &field_conj);
conv += vec_dist(&id_new[idx], &efp->indip[idx]);
conv += vec_dist(&id_conj_new[idx], &efp->indipconj[idx]);
}
}
((struct id_work_data *)data)->conv += conv;
}
static void remove_system_drift(struct md *md)
{
vec_t cp = get_system_com(md);
vec_t cv = get_system_com_velocity(md);
vec_t am = get_system_angular_momentum(md);
mat_t inertia = get_system_inertia_tensor(md);
mat_t inertia_inv = mat_zero;
if (inertia.xx < EPSILON ||
inertia.yy < EPSILON ||
inertia.zz < EPSILON) {
inertia_inv.xx = inertia.xx < EPSILON ? 0.0 : 1.0 / inertia.xx;
inertia_inv.yy = inertia.yy < EPSILON ? 0.0 : 1.0 / inertia.yy;
inertia_inv.zz = inertia.zz < EPSILON ? 0.0 : 1.0 / inertia.zz;
}
else {
inertia_inv = mat_inv(&inertia);
}
vec_t av = mat_vec(&inertia_inv, &am);
for (size_t i = 0; i < md->n_bodies; i++) {
struct body *body = md->bodies + i;
vec_t pos = wrap(md, &body->pos);
vec_t cross = {
av.y * (pos.z - cp.z) - av.z * (pos.y - cp.y),
av.z * (pos.x - cp.x) - av.x * (pos.z - cp.z),
av.x * (pos.y - cp.y) - av.y * (pos.x - cp.x)
};
body->vel.x -= cv.x + cross.x;
body->vel.y -= cv.y + cross.y;
body->vel.z -= cv.z + cross.z;
}
vec_t cv2 = get_system_com_velocity(md);
vec_t am2 = get_system_angular_momentum(md);
assert(vec_len(&cv2) < EPSILON && vec_len(&am2) < EPSILON);
}
static void rotate_step(size_t a1, size_t a2, double angle, vec_t *angmom, mat_t *rotmat)
{
mat_t rot = { 1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0 };
double cosa = cos(angle);
double sina = sin(angle);
mat_set(&rot, a1, a1, cosa);
mat_set(&rot, a2, a2, cosa);
mat_set(&rot, a1, a2, sina);
mat_set(&rot, a2, a1, -sina);
*angmom = mat_vec(&rot, angmom);
/* transpose */
mat_set(&rot, a1, a2, -sina);
mat_set(&rot, a2, a1, sina);
*rotmat = mat_mat(rotmat, &rot);
}
static enum efp_result
set_coord_points(struct frag *frag, const double *coord)
{
/* allow fragments with less than 3 atoms by using multipole points of
* ghost atoms; multipole points have the same coordinates as atoms */
if (frag->n_multipole_pts < 3) {
efp_log("fragment must contain at least three atoms");
return EFP_RESULT_FATAL;
}
double ref[9] = {
frag->lib->multipole_pts[0].x,
frag->lib->multipole_pts[0].y,
frag->lib->multipole_pts[0].z,
frag->lib->multipole_pts[1].x,
frag->lib->multipole_pts[1].y,
frag->lib->multipole_pts[1].z,
frag->lib->multipole_pts[2].x,
frag->lib->multipole_pts[2].y,
frag->lib->multipole_pts[2].z
};
vec_t p1;
mat_t rot1, rot2;
efp_points_to_matrix(coord, &rot1);
efp_points_to_matrix(ref, &rot2);
rot2 = mat_transpose(&rot2);
frag->rotmat = mat_mat(&rot1, &rot2);
p1 = mat_vec(&frag->rotmat, VEC(frag->lib->multipole_pts[0].x));
/* center of mass */
frag->x = coord[0] - p1.x;
frag->y = coord[1] - p1.y;
frag->z = coord[2] - p1.z;
update_fragment(frag);
return EFP_RESULT_SUCCESS;
}
void knnc(double *array_vec,int *nb_col,int *nb_row, int*k, int *corre_flag, double *dist, double *dist_bound)
{
int missing,i,j,ii;
int count;
double value;
double *temp;
double *row_nb;
double ** array;
int *miss_pos;
int index;
int *n_position;
int* nb_neighboors;
int min=0;
int max=*k-1;
array=dmatrix(*nb_row,*nb_col);
/** contain the row numbers of the missing values **/
miss_pos=ivector(*nb_col, code_miss);
/** contains the distances of the neighboors **/
temp=dvector(*k,code_miss);
/** contains the row numbers of the neighboors **/
row_nb=dvector(*k,code_miss);
/** initilize all the distances with the missing codes **/
init_dvector(dist, nb_row, code_miss);
n_position=ivector(*nb_row, code_miss); /** positions of potential neighboors **/
nb_neighboors=ivector(1, code_miss); /** number of neighboors **/
/** coerce the vector into a two dimmensional array **/
vec_mat(array_vec,nb_row,nb_col,array);
neighboors(array, nb_row, nb_col, n_position, nb_neighboors);
if(*nb_neighboors==0) /** Stop if no neighboors **/
{
error("No rows without missing values");
}
else
{
if(*nb_neighboors<*k) /** If less than k neighboors give a warning **/
warning("Only %d neighboors could be used", *nb_neighboors);
for(i=0;i<*nb_row;i++)
{
/** Check for missing values **/
missing=is_na(array[i],nb_col,miss_pos);
if (missing==1 && miss_pos[*nb_col-1]==code_miss) /**at least one missing value at most nb_col**/
{
if(*corre_flag==1 && miss_pos[*nb_col-2]!=code_miss) /** Give a warning if based on correlation and only one observation **/
warning("Could not estimate the missing values for the row %d\n One observation is not enough to compute the sample correlation", i+1);
else
{
count=0;
for(j=0;j<*nb_neighboors;j++) /** loop on the neighboors only **/
{
index=n_position[j];
if(*corre_flag==0)
value=distance(array[i],array[index],nb_col); /** compute the distance **/
else
value=-correlation(array[i],array[index],nb_col); /** compute the correlation **/
if(value!=code_miss)
{
if (count<*k) /** store the first k **/
{
temp[count]=value;
row_nb[count]=index;
count++;
}
else
{
quicksort2(temp,row_nb,&min,&max); /** sort the neighboors to keep the kth nearest **/
if (temp[*k-1]>value) /** keep it if the distance is shorter **/
{
temp[*k-1]=value;
row_nb[*k-1]=index;
}
}
}
}
if(*corre_flag==0)
{
fill_up(array,row_nb,nb_col,k,i,miss_pos,temp, dist_bound); /** fill up the missing values by the averaging the distance**/
dist[i]=mean_vec(temp, k); /** Compute the average distances **/
}
else
{
fill_up_corr(array,row_nb, nb_col, k,i, miss_pos, temp, dist_bound); /** fill up the missing values based on correlations**/
dist[i]=-mean_vec(temp, k); /** Compute the average distances **/
}
init_dvector(row_nb, k, code_miss); /** initialize row_nb with missing codes **/
init_dvector(temp, k, code_miss); /** initialize temp with missing codes **/
}
}
else if(missing==1 && miss_pos[*nb_col-1]!=code_miss)
warning("Could not estimate the missing values for the row %d\n The row only contains missing values", i+1);
}
}
mat_vec(array_vec, nb_row, nb_col,array); /** recoerce the matrix into a vector **/
/** free the memory **/
free_dmatrix(array,*nb_row);
Free(miss_pos);
Free(temp);
Free(row_nb);
Free(n_position);
Free(nb_neighboors);
}
Need ompcore(double D[], double x[], double DtX[], double XtX[], double G[], mwSize n, mwSize m, mwSize L,
int T, double eps, int gamma_mode, int profile, double msg_delta, int erroromp)
{
profdata pd;
/* mxArray *Gamma;*/
mwIndex i, j, signum, pos, *ind, *gammaIr, *gammaJc, gamma_count;
mwSize allocated_coefs, allocated_cols;
int DtX_specified, XtX_specified, batchomp, standardomp, *selected_atoms,*times_atoms ;
double *alpha, *r, *Lchol, *c, *Gsub, *Dsub, sum, *gammaPr, *tempvec1, *tempvec2;
double eps2, resnorm, delta, deltaprev, secs_remain;
int mins_remain, hrs_remain;
clock_t lastprint_time, starttime;
Need my;
/*** status flags ***/
DtX_specified = (DtX!=0); /* indicates whether D'*x was provided */
XtX_specified = (XtX!=0); /* indicates whether sum(x.*x) was provided */
standardomp = (G==0); /* batch-omp or standard omp are selected depending on availability of G */
batchomp = !standardomp;
/*** allocate output matrix ***/
if (gamma_mode == FULL_GAMMA) {
/* allocate full matrix of size m X L */
Gamma = mxCreateDoubleMatrix(m, L, mxREAL);
gammaPr = mxGetPr(Gamma);
gammaIr = 0;
gammaJc = 0;
}
else {
/* allocate sparse matrix with room for allocated_coefs nonzeros */
/* for error-omp, begin with L*sqrt(n)/2 allocated nonzeros, otherwise allocate L*T nonzeros */
allocated_coefs = erroromp ? (mwSize)(ceil(L*sqrt((double)n)/2.0) + 1.01) : L*T;
Gamma = mxCreateSparse(m, L, allocated_coefs, mxREAL);
gammaPr = mxGetPr(Gamma);
gammaIr = mxGetIr(Gamma);
gammaJc = mxGetJc(Gamma);
gamma_count = 0;
gammaJc[0] = 0;
}
/*** helper arrays ***/
alpha = (double*)mxMalloc(m*sizeof(double)); /* contains D'*residual */
ind = (mwIndex*)mxMalloc(n*sizeof(mwIndex)); /* indices of selected atoms */
selected_atoms = (int*)mxMalloc(m*sizeof(int)); /* binary array with 1's for selected atoms */
times_atoms = (int*)mxMalloc(m*sizeof(int));
c = (double*)mxMalloc(n*sizeof(double)); /* orthogonal projection result */
/* current number of columns in Dsub / Gsub / Lchol */
allocated_cols = erroromp ? (mwSize)(ceil(sqrt((double)n)/2.0) + 1.01) : T;
/* Cholesky decomposition of D_I'*D_I */
Lchol = (double*)mxMalloc(n*allocated_cols*sizeof(double));
/* temporary vectors for various computations */
tempvec1 = (double*)mxMalloc(m*sizeof(double));
tempvec2 = (double*)mxMalloc(m*sizeof(double));
if (batchomp) {
/* matrix containing G(:,ind) - the columns of G corresponding to the selected atoms, in order of selection */
Gsub = (double*)mxMalloc(m*allocated_cols*sizeof(double));
}
else {
/* matrix containing D(:,ind) - the selected atoms from D, in order of selection */
Dsub = (double*)mxMalloc(n*allocated_cols*sizeof(double));
/* stores the residual */
r = (double*)mxMalloc(n*sizeof(double));
}
if (!DtX_specified) {
/* contains D'*x for the current signal */
DtX = (double*)mxMalloc(m*sizeof(double));
}
/*** initializations for error omp ***/
if (erroromp) {
eps2 = eps*eps; /* compute eps^2 */
if (T<0 || T>n) { /* unspecified max atom num - set max atoms to n */
T = n;
}
}
/*** initialize timers ***/
initprofdata(&pd); /* initialize profiling counters */
starttime = clock(); /* record starting time for eta computations */
lastprint_time = starttime; /* time of last status display */
/********************** perform omp for each signal **********************/
for (signum=0; signum<L; ++signum) {
/* initialize residual norm and deltaprev for error-omp */
if (erroromp) {
if (XtX_specified) {
resnorm = XtX[signum];
}
else {
resnorm = dotprod(x+n*signum, x+n*signum, n);
addproftime(&pd, XtX_TIME);
}
deltaprev = 0; /* delta tracks the value of gamma'*G*gamma */
}
else {
/* ignore residual norm stopping criterion */
eps2 = 0;
resnorm = 1;
}
if (resnorm>eps2 && T>0) {
/* compute DtX */
if (!DtX_specified) {
matT_vec(1, D, x+n*signum, DtX, n, m);
addproftime(&pd, DtX_TIME);
}
/* initialize alpha := DtX */
memcpy(alpha, DtX + m*signum*DtX_specified, m*sizeof(double));
/* mark all atoms as unselected */
for (i=0; i<m; ++i) {
selected_atoms[i] = 0;
}
for (i=0; i<m; ++i) {
times_atoms[i] = 0;
}
}
/* main loop */
i=0;
while (resnorm>eps2 && i<T) {
/* index of next atom */
pos = maxabs(alpha, m);
addproftime(&pd, MAXABS_TIME);
/* stop criterion: selected same atom twice, or inner product too small */
if (selected_atoms[pos] || alpha[pos]*alpha[pos]<1e-14) {
break;
}
/* mark selected atom */
ind[i] = pos;
selected_atoms[pos] = 1;
times_atoms[pos]++;
/* matrix reallocation */
if (erroromp && i>=allocated_cols) {
allocated_cols = (mwSize)(ceil(allocated_cols*MAT_INC_FACTOR) + 1.01);
Lchol = (double*)mxRealloc(Lchol,n*allocated_cols*sizeof(double));
batchomp ? (Gsub = (double*)mxRealloc(Gsub,m*allocated_cols*sizeof(double))) :
(Dsub = (double*)mxRealloc(Dsub,n*allocated_cols*sizeof(double))) ;
}
/* append column to Gsub or Dsub */
if (batchomp) {
memcpy(Gsub+i*m, G+pos*m, m*sizeof(double));
}
else {
memcpy(Dsub+i*n, D+pos*n, n*sizeof(double));
}
/*** Cholesky update ***/
if (i==0) {
*Lchol = 1;
}
else {
/* incremental Cholesky decomposition: compute next row of Lchol */
if (standardomp) {
matT_vec(1, Dsub, D+n*pos, tempvec1, n, i); /* compute tempvec1 := Dsub'*d where d is new atom */
addproftime(&pd, DtD_TIME);
}
else {
vec_assign(tempvec1, Gsub+i*m, ind, i); /* extract tempvec1 := Gsub(ind,i) */
}
backsubst('L', Lchol, tempvec1, tempvec2, n, i); /* compute tempvec2 = Lchol \ tempvec1 */
for (j=0; j<i; ++j) { /* write tempvec2 to end of Lchol */
Lchol[j*n+i] = tempvec2[j];
}
/* compute Lchol(i,i) */
sum = 0;
for (j=0; j<i; ++j) { /* compute sum of squares of last row without Lchol(i,i) */
sum += SQR(Lchol[j*n+i]);
}
if ( (1-sum) <= 1e-14 ) { /* Lchol(i,i) is zero => selected atoms are dependent */
break;
}
Lchol[i*n+i] = sqrt(1-sum);
}
addproftime(&pd, LCHOL_TIME);
i++;
/* perform orthogonal projection and compute sparse coefficients */
vec_assign(tempvec1, DtX + m*signum*DtX_specified, ind, i); /* extract tempvec1 = DtX(ind) */
cholsolve('L', Lchol, tempvec1, c, n, i); /* solve LL'c = tempvec1 for c */
addproftime(&pd, COMPCOEF_TIME);
/* update alpha = D'*residual */
if (standardomp) {
mat_vec(-1, Dsub, c, r, n, i); /* compute r := -Dsub*c */
vec_sum(1, x+n*signum, r, n); /* compute r := x+r */
/*memcpy(r, x+n*signum, n*sizeof(double)); /* assign r := x */
/*mat_vec1(-1, Dsub, c, 1, r, n, i); /* compute r := r-Dsub*c */
addproftime(&pd, COMPRES_TIME);
matT_vec(1, D, r, alpha, n, m); /* compute alpha := D'*r */
addproftime(&pd, DtR_TIME);
/* update residual norm */
if (erroromp) {
resnorm = dotprod(r, r, n);
addproftime(&pd, UPDATE_RESNORM_TIME);
}
}
else {
mat_vec(1, Gsub, c, tempvec1, m, i); /* compute tempvec1 := Gsub*c */
memcpy(alpha, DtX + m*signum*DtX_specified, m*sizeof(double)); /* set alpha = D'*x */
vec_sum(-1, tempvec1, alpha, m); /* compute alpha := alpha - tempvec1 */
addproftime(&pd, UPDATE_DtR_TIME);
/* update residual norm */
if (erroromp) {
vec_assign(tempvec2, tempvec1, ind, i); /* assign tempvec2 := tempvec1(ind) */
delta = dotprod(c,tempvec2,i); /* compute c'*tempvec2 */
resnorm = resnorm - delta + deltaprev; /* residual norm update */
deltaprev = delta;
addproftime(&pd, UPDATE_RESNORM_TIME);
}
}
}
/*** generate output vector gamma ***/
if (gamma_mode == FULL_GAMMA) { /* write the coefs in c to their correct positions in gamma */
for (j=0; j<i; ++j) {
gammaPr[m*signum + ind[j]] = c[j];
}
}
else {
/* sort the coefs by index before writing them to gamma */
quicksort(ind,c,i);
addproftime(&pd, INDEXSORT_TIME);
/* gamma is full - reallocate */
if (gamma_count+i >= allocated_coefs) {
while(gamma_count+i >= allocated_coefs) {
allocated_coefs = (mwSize)(ceil(GAMMA_INC_FACTOR*allocated_coefs) + 1.01);
}
mxSetNzmax(Gamma, allocated_coefs);
mxSetPr(Gamma, mxRealloc(gammaPr, allocated_coefs*sizeof(double)));
mxSetIr(Gamma, mxRealloc(gammaIr, allocated_coefs*sizeof(mwIndex)));
gammaPr = mxGetPr(Gamma);
gammaIr = mxGetIr(Gamma);
}
/* append coefs to gamma and update the indices */
for (j=0; j<i; ++j) {
gammaPr[gamma_count] = c[j];
gammaIr[gamma_count] = ind[j];
gamma_count++;
}
gammaJc[signum+1] = gammaJc[signum] + i;
}
/*** display status messages ***/
if (msg_delta>0 && (clock()-lastprint_time)/(double)CLOCKS_PER_SEC >= msg_delta)
{
lastprint_time = clock();
/* estimated remainig time */
secs2hms( ((L-signum-1)/(double)(signum+1)) * ((lastprint_time-starttime)/(double)CLOCKS_PER_SEC) ,
&hrs_remain, &mins_remain, &secs_remain);
mexPrintf("omp: signal %d / %d, estimated remaining time: %02d:%02d:%05.2f\n",
signum+1, L, hrs_remain, mins_remain, secs_remain);
mexEvalString("drawnow;");
}
}
/* end omp */
/*** print final messages ***/
if (msg_delta>0) {
mexPrintf("omp: signal %d / %d\n", signum, L);
}
if (profile) {
printprofinfo(&pd, erroromp, batchomp, L);
}
/* free memory */
if (!DtX_specified) {
mxFree(DtX);
}
if (standardomp) {
mxFree(r);
mxFree(Dsub);
}
else {
mxFree(Gsub);
}
mxFree(tempvec2);
mxFree(tempvec1);
mxFree(Lchol);
mxFree(c);
mxFree(selected_atoms);
mxFree(ind);
mxFree(alpha);
my.qGamma=Gamma;
my.qtimes__atoms=times__atoms;
/*return Gamma;*/
return my;
}
double R_rlm_rand(double *X, double *y, int *N, int *P,
int *Boot_Samp, int *Nres,
int *M, int *size_boot, double *ours, double *full,
double *Beta_m, double *Beta_s, double *Scale,
int *Seed, int *calc_full,
double *C, double *Psi_c, int *max_it,
int *converged_mm,
int *groups, int *n_group, int *k_fast_s)
{
void initialize_mat(double **a, int n, int m);
void initialize_vec(double *a, int n);
void R_S_rlm(double *X, double *y, int *n, int *P,
int *nres, int *max_it,
double *SCale, double *beta_s, double *beta_m,
int *converged_mm,
int *seed_rand, double *C, double *Psi_c,
int *Groups, int *N_group, int *K_fast_s);
double Psi_reg(double,double);
double Psi_reg_prime(double,double);
double Chi_prime(double,double);
double Chi(double,double);
void sampler_i(int, int, int *);
int inverse(double **,double **, int);
void matias_vec_vec(double **, double *, double *, int);
void scalar_mat(double **, double, double **, int, int);
void scalar_vec(double *, double, double *, int);
void sum_mat(double **,double **, double **, int, int);
void sum_vec(double *, double *, double *, int);
void dif_mat(double **, double **, double **, int , int );
void dif_vec(double *, double *, double *, int);
void mat_vec(double **, double *, double *, int, int);
void mat_mat(double **, double **, double **, int, int, int);
// void disp_vec(double *, int);
// void disp_mat(double **, int, int);
// void disp_mat_i(int **, int, int);
// void disp_vec(double *, int);
/* double **xb; */
double *Xb, **xb;
int **boot_samp;
double **x, **x2, **x3, **x4, *beta_m, *beta_s,*beta_aux;
double *Fi, *res, *res_s, *w, *ww, dummyscale, scale;
double *v, *v2, *v_aux, *yb; // , timefinish, timestart;
double u,u2,s,c,Psi_constant;
// double test_chi=0, test_psi=0;
int n,p,m,seed; // ,*indices;
int nboot=*size_boot;
// int fake_p = 0;
register int i,j,k;
setbuf(stdout,NULL);
c = *C; Psi_constant = *Psi_c;
n = *N; p = *P; m = *M; seed = *Seed;
boot_samp = (int **) malloc(m * sizeof(int*) );
for(i=0;i<m;i++)
boot_samp[i] = (int*) malloc(nboot *sizeof(int));
// indices = (int *) malloc( n * sizeof(int) );
v = (double *) malloc( p * sizeof(double) );
v2 = (double *) malloc( p * sizeof(double) );
v_aux = (double *) malloc( p * sizeof(double) );
yb = (double *) malloc( n * sizeof(double) );
Xb = (double*) malloc( n * p * sizeof(double) );
x = (double **) malloc ( n * sizeof(double *) );
xb = (double **) malloc ( n * sizeof(double *) );
Fi = (double *) malloc ( n * sizeof(double) );
res = (double *) malloc ( n * sizeof(double) );
res_s = (double *) malloc ( n * sizeof(double) );
ww = (double *) malloc ( n * sizeof(double) );
w = (double *) malloc ( n * sizeof(double) );
x2 = (double **) malloc ( p * sizeof(double *) );
x3 = (double **) malloc ( p * sizeof(double *) );
x4 = (double **) malloc ( p * sizeof(double *) );
beta_aux = (double *) malloc( p * sizeof(double) );
beta_m = (double *) malloc( p * sizeof(double) );
beta_s = (double *) malloc( p * sizeof(double) );
for(i=0;i<n;i++) {
x[i] = (double*) malloc (p * sizeof(double) );
xb[i] = (double*) malloc ((p+1) * sizeof(double) );
};
for(i=0;i<p;i++) {
x2[i] = (double*) malloc (p * sizeof(double) );
x3[i] = (double*) malloc (p * sizeof(double) );
x4[i] = (double*) malloc (p * sizeof(double) );
};
/* copy X into x for easier handling */
for(i=0;i<n;i++)
for(j=0;j<p;j++)
x[i][j]=X[j*n+i];
/* calculate robust regression estimates */
for(i=0;i<m;i++)
for(j=0;j<nboot;j++)
boot_samp[i][j]=Boot_Samp[j*m+i]-1;
R_S_rlm(X, y, N, P, Nres, max_it, &scale, Beta_s, Beta_m,
converged_mm, &seed, &c,
Psi_c, groups, n_group, k_fast_s);
*Scale = scale;
/* get M-fitted values in Fi */
mat_vec(x,Beta_m,Fi,n,p);
/* get residuals of M-est in res */
dif_vec(y,Fi,res,n);
/* get S-fitted values in res_s */
mat_vec(x,Beta_s,res_s,n,p);
/* get residuals of S-est in res_s */
dif_vec(y,res_s,res_s,n);
/* set auxiliary matrices to zero */
initialize_mat(x3, p, p);
initialize_mat(x4, p, p);
initialize_vec(v, p);
u2 = 0.0;
/* calculate correction matrix */
for(i=0;i<n;i++) {
u = res[i]/scale ;
w[i] = Psi_reg(u,Psi_constant)/res[i];
matias_vec_vec(x2,x[i],x[i],p);
scalar_mat(x2,Psi_reg_prime(u,Psi_constant),
x2,p,p);
sum_mat(x3,x2,x3,p,p);
matias_vec_vec(x2,x[i],x[i],p);
scalar_mat(x2,w[i],x2,p,p);
sum_mat(x4,x2,x4,p,p);
scalar_vec(x[i],Psi_reg_prime(u,Psi_constant)*u,v_aux,p);
sum_vec(v,v_aux,v,p);
u2 += Chi_prime(u, c) * u;
};
/* scalar_vec(v, .5 * (double) (n-p) * scale / u2 , v, p); */
scalar_vec(v, .5 * (double) n * scale / u2 , v, p);
inverse(x3,x2,p);
mat_mat(x2,x4,x3,p,p,p);
mat_vec(x2,v,v2,p,p);
scalar_mat(x3,scale,x3,p,p);
/* the correction matrix is now in x3 */
/* the correction vector is now in v2 */
/* start the bootstrap replications */
for(i=0;i<m;i++) {
/* change the seed! */
++seed;
// sampler_i(n,nboot,indices);
// for(j=0;j<nboot; j++)
// indices[j]=boot_samp[i][j];
/* get pseudo observed y's */
for(j=0;j<nboot;j++) /* xb[j][p] = */
yb[j] = y[boot_samp[i][j]];
for(j=0;j<nboot;j++)
for(k=0;k<p;k++) {
// xb[j][k] = x[boot_samp[i][j]][k];
// Xb[k*nboot+j] = X[k*n + indices[j]];
Xb[k*nboot+j] = x[boot_samp[i][j]][k];
xb[j][k] = Xb[k*nboot+j];
};
/* calculate full bootstrap estimate */
if( *calc_full == 1 )
R_S_rlm(Xb,yb,&nboot,P,Nres,max_it,&dummyscale,
beta_s,beta_m,converged_mm,&seed,&c,
Psi_c, groups, n_group, k_fast_s);
/* void R_S_rlm(double *X, double *y, int *n, int *P,
int *nres, int *max_it,
double *SCale, double *beta_s, double *beta_m,
int *converged_mm,
int *seed_rand, double *C, double *Psi_c,
int *Groups, int *N_group, int *K_fast_s) */
/* double *C, double *Psi_c, int *max_it,
int *groups, int *n_group, int *k_fast_s); */
// HERE
/* disp_mat(xb, nboot,p); */
// disp_vec(yb,nboot);
// Rprintf("\nfull scale: %f", dummyscale);
/* calculate robust bootsrap */
scalar_vec(v,0.0,v,p); /* v <- 0 */
scalar_mat(x2,0.0,x2,p,p); /* x2 <- 0 */
s = 0.0;
for(j=0;j<nboot;j++) {
scalar_vec(xb[j],yb[j]*w[boot_samp[i][j]],v_aux,p);
sum_vec(v,v_aux,v,p);
matias_vec_vec(x4,xb[j],xb[j],p);
scalar_mat(x4,w[boot_samp[i][j]],x4,p,p);
sum_mat(x2,x4,x2,p,p);
s += Chi(res_s[boot_samp[i][j]] / scale , c);
};
/* s = s * scale / .5 / (double) (nboot - p) ; */
s = s * scale / .5 / (double) n;
inverse(x2,x4,p); /* x4 <- x2^-1 */
mat_vec(x4,v,v_aux,p,p); /* v_aux <- x4 * v */
dif_vec(v_aux,Beta_m,v_aux,p); /* v_aux <- v_aux - beta_m */
/* v has the robust bootstrapped vector, correct it */
mat_vec(x3,v_aux,v,p,p); /* v <- x3 * v_aux */
scalar_vec(v2,s-scale,v_aux,p);
sum_vec(v_aux,v,v,p);
/* store the betas (splus-wise!) */
for(j=0;j<p;j++) {
ours[j*m+i]=v[j];
if( *calc_full == 1 )
// full[j*m+i]=beta_m[j]-Beta_m[j];
full[j*m+i]=beta_m[j];
};
};
for(i=0;i<m;i++)
free(boot_samp[i]);
free(boot_samp);
for(i=0;i<n;i++) {
free(x[i]);
free(xb[i]);
};
for(i=0;i<p;i++) {
free(x2[i]);
free(x3[i]);
free(x4[i]);
};
free(x) ;free(x2);free(xb);
free(x3);free(x4);
free(beta_aux);free(beta_m);free(beta_s);
free(w);free(ww);free(Fi);free(res);
free(v);free(v2);free(v_aux);free(yb);
free(res_s);
free(Xb);
return(0);
}
