MAT *ms_mltadd(const MAT *A1, const MAT *A2, double s, MAT *out) #endif { /* register Real *A1_e, *A2_e, *out_e; */ /* register int j; */ int i, m, n; if ( ! A1 || ! A2 ) error(E_NULL,"ms_mltadd"); if ( A1->m != A2->m || A1->n != A2->n ) error(E_SIZES,"ms_mltadd"); if ( out != A1 && out != A2 ) out = m_resize(out,A1->m,A1->n); if ( s == 0.0 ) return m_copy(A1,out); if ( s == 1.0 ) return m_add(A1,A2,out); tracecatch(out = m_copy(A1,out),"ms_mltadd"); m = A1->m; n = A1->n; for ( i = 0; i < m; i++ ) { __mltadd__(out->me[i],A2->me[i],s,(int)n); /************************************************** A1_e = A1->me[i]; A2_e = A2->me[i]; out_e = out->me[i]; for ( j = 0; j < n; j++ ) out_e[j] = A1_e[j] + s*A2_e[j]; **************************************************/ } return out; }
struct mbuf * compat_70_unp_addsockcred(struct lwp *l, struct mbuf *control) { struct sockcred70 *sc; struct mbuf *m; void *p; m = sbcreatecontrol1(&p, SOCKCRED70SIZE(kauth_cred_ngroups(l->l_cred)), SCM_OCREDS, SOL_SOCKET, M_WAITOK); if (m == NULL) return control; sc = p; sc->sc_uid = kauth_cred_getuid(l->l_cred); sc->sc_euid = kauth_cred_geteuid(l->l_cred); sc->sc_gid = kauth_cred_getgid(l->l_cred); sc->sc_egid = kauth_cred_getegid(l->l_cred); sc->sc_ngroups = kauth_cred_ngroups(l->l_cred); for (int i = 0; i < sc->sc_ngroups; i++) sc->sc_groups[i] = kauth_cred_group(l->l_cred, i); return m_add(control, m); }
static void reduce_thread_forces(int n, rvec *f, tensor vir, real *Vcorr, int efpt_ind, real *dvdl, int nthreads, f_thread_t *f_t) { int t, i; /* This reduction can run over any number of threads */ #pragma omp parallel for num_threads(gmx_omp_nthreads_get(emntBonded)) private(t) schedule(static) for (i = 0; i < n; i++) { for (t = 1; t < nthreads; t++) { rvec_inc(f[i], f_t[t].f[i]); } } for (t = 1; t < nthreads; t++) { *Vcorr += f_t[t].Vcorr; *dvdl += f_t[t].dvdl[efpt_ind]; m_add(vir, f_t[t].vir, vir); } }
void compute_globals(FILE *fplog, gmx_global_stat_t gstat, t_commrec *cr, t_inputrec *ir, t_forcerec *fr, gmx_ekindata_t *ekind, t_state *state, t_state *state_global, t_mdatoms *mdatoms, t_nrnb *nrnb, t_vcm *vcm, gmx_wallcycle_t wcycle, gmx_enerdata_t *enerd, tensor force_vir, tensor shake_vir, tensor total_vir, tensor pres, rvec mu_tot, gmx_constr_t constr, globsig_t *gs, gmx_bool bInterSimGS, matrix box, gmx_mtop_t *top_global, gmx_bool *bSumEkinhOld, int flags) { int i, gsi; real gs_buf[eglsNR]; tensor corr_vir, corr_pres; gmx_bool bEner, bPres, bTemp; gmx_bool bStopCM, bGStat, bReadEkin, bEkinAveVel, bScaleEkin, bConstrain; real prescorr, enercorr, dvdlcorr, dvdl_ekin; /* translate CGLO flags to gmx_booleans */ bStopCM = flags & CGLO_STOPCM; bGStat = flags & CGLO_GSTAT; bReadEkin = (flags & CGLO_READEKIN); bScaleEkin = (flags & CGLO_SCALEEKIN); bEner = flags & CGLO_ENERGY; bTemp = flags & CGLO_TEMPERATURE; bPres = (flags & CGLO_PRESSURE); bConstrain = (flags & CGLO_CONSTRAINT); /* we calculate a full state kinetic energy either with full-step velocity verlet or half step where we need the pressure */ bEkinAveVel = (ir->eI == eiVV || (ir->eI == eiVVAK && bPres) || bReadEkin); /* in initalization, it sums the shake virial in vv, and to sums ekinh_old in leapfrog (or if we are calculating ekinh_old) for other reasons */ /* ########## Kinetic energy ############## */ if (bTemp) { /* Non-equilibrium MD: this is parallellized, but only does communication * when there really is NEMD. */ if (PAR(cr) && (ekind->bNEMD)) { accumulate_u(cr, &(ir->opts), ekind); } debug_gmx(); if (bReadEkin) { restore_ekinstate_from_state(cr, ekind, &state_global->ekinstate); } else { calc_ke_part(state, &(ir->opts), mdatoms, ekind, nrnb, bEkinAveVel); } debug_gmx(); } /* Calculate center of mass velocity if necessary, also parallellized */ if (bStopCM) { calc_vcm_grp(0, mdatoms->homenr, mdatoms, state->x, state->v, vcm); } if (bTemp || bStopCM || bPres || bEner || bConstrain) { if (!bGStat) { /* We will not sum ekinh_old, * so signal that we still have to do it. */ *bSumEkinhOld = TRUE; } else { if (gs != NULL) { for (i = 0; i < eglsNR; i++) { gs_buf[i] = gs->sig[i]; } } if (PAR(cr)) { wallcycle_start(wcycle, ewcMoveE); global_stat(fplog, gstat, cr, enerd, force_vir, shake_vir, mu_tot, ir, ekind, constr, bStopCM ? vcm : NULL, gs != NULL ? eglsNR : 0, gs_buf, top_global, state, *bSumEkinhOld, flags); wallcycle_stop(wcycle, ewcMoveE); } if (gs != NULL) { if (MULTISIM(cr) && bInterSimGS) { if (MASTER(cr)) { /* Communicate the signals between the simulations */ gmx_sum_sim(eglsNR, gs_buf, cr->ms); } /* Communicate the signals form the master to the others */ gmx_bcast(eglsNR*sizeof(gs_buf[0]), gs_buf, cr); } for (i = 0; i < eglsNR; i++) { if (bInterSimGS || gs_simlocal[i]) { /* Set the communicated signal only when it is non-zero, * since signals might not be processed at each MD step. */ gsi = (gs_buf[i] >= 0 ? (int)(gs_buf[i] + 0.5) : (int)(gs_buf[i] - 0.5)); if (gsi != 0) { gs->set[i] = gsi; } /* Turn off the local signal */ gs->sig[i] = 0; } } } *bSumEkinhOld = FALSE; } } if (!ekind->bNEMD && debug && bTemp && (vcm->nr > 0)) { correct_ekin(debug, 0, mdatoms->homenr, state->v, vcm->group_p[0], mdatoms->massT, mdatoms->tmass, ekind->ekin); } /* Do center of mass motion removal */ if (bStopCM) { check_cm_grp(fplog, vcm, ir, 1); do_stopcm_grp(0, mdatoms->homenr, mdatoms->cVCM, state->x, state->v, vcm); inc_nrnb(nrnb, eNR_STOPCM, mdatoms->homenr); } if (bEner) { /* Calculate the amplitude of the cosine velocity profile */ ekind->cosacc.vcos = ekind->cosacc.mvcos/mdatoms->tmass; } if (bTemp) { /* Sum the kinetic energies of the groups & calc temp */ /* compute full step kinetic energies if vv, or if vv-avek and we are computing the pressure with IR_NPT_TROTTER */ /* three maincase: VV with AveVel (md-vv), vv with AveEkin (md-vv-avek), leap with AveEkin (md). Leap with AveVel is not supported; it's not clear that it will actually work. bEkinAveVel: If TRUE, we simply multiply ekin by ekinscale to get a full step kinetic energy. If FALSE, we average ekinh_old and ekinh*ekinscale_nhc to get an averaged half step kinetic energy. */ enerd->term[F_TEMP] = sum_ekin(&(ir->opts), ekind, &dvdl_ekin, bEkinAveVel, bScaleEkin); enerd->dvdl_lin[efptMASS] = (double) dvdl_ekin; enerd->term[F_EKIN] = trace(ekind->ekin); } /* ########## Long range energy information ###### */ if (bEner || bPres || bConstrain) { calc_dispcorr(ir, fr, top_global->natoms, box, state->lambda[efptVDW], corr_pres, corr_vir, &prescorr, &enercorr, &dvdlcorr); } if (bEner) { enerd->term[F_DISPCORR] = enercorr; enerd->term[F_EPOT] += enercorr; enerd->term[F_DVDL_VDW] += dvdlcorr; } /* ########## Now pressure ############## */ if (bPres || bConstrain) { m_add(force_vir, shake_vir, total_vir); /* Calculate pressure and apply LR correction if PPPM is used. * Use the box from last timestep since we already called update(). */ enerd->term[F_PRES] = calc_pres(fr->ePBC, ir->nwall, box, ekind->ekin, total_vir, pres); /* Calculate long range corrections to pressure and energy */ /* this adds to enerd->term[F_PRES] and enerd->term[F_ETOT], and computes enerd->term[F_DISPCORR]. Also modifies the total_vir and pres tesors */ m_add(total_vir, corr_vir, total_vir); m_add(pres, corr_pres, pres); enerd->term[F_PDISPCORR] = prescorr; enerd->term[F_PRES] += prescorr; } }
gmx_bool constrain(FILE *fplog, gmx_bool bLog, gmx_bool bEner, struct gmx_constr *constr, t_idef *idef, t_inputrec *ir, gmx_ekindata_t *ekind, t_commrec *cr, gmx_int64_t step, int delta_step, t_mdatoms *md, rvec *x, rvec *xprime, rvec *min_proj, gmx_bool bMolPBC, matrix box, real lambda, real *dvdlambda, rvec *v, tensor *vir, t_nrnb *nrnb, int econq, gmx_bool bPscal, real veta, real vetanew) { gmx_bool bOK, bDump; int start, homenr, nrend; int i, j, d; int ncons, settle_error; tensor vir_r_m_dr; rvec *vstor; real invdt, vir_fac, t; t_ilist *settle; int nsettle; t_pbc pbc, *pbc_null; char buf[22]; t_vetavars vetavar; int nth, th; if (econq == econqForceDispl && !EI_ENERGY_MINIMIZATION(ir->eI)) { gmx_incons("constrain called for forces displacements while not doing energy minimization, can not do this while the LINCS and SETTLE constraint connection matrices are mass weighted"); } bOK = TRUE; bDump = FALSE; start = 0; homenr = md->homenr; nrend = start+homenr; /* set constants for pressure control integration */ init_vetavars(&vetavar, econq != econqCoord, veta, vetanew, ir, ekind, bPscal); if (ir->delta_t == 0) { invdt = 0; } else { invdt = 1/ir->delta_t; } if (ir->efep != efepNO && EI_DYNAMICS(ir->eI)) { /* Set the constraint lengths for the step at which this configuration * is meant to be. The invmasses should not be changed. */ lambda += delta_step*ir->fepvals->delta_lambda; } if (vir != NULL) { clear_mat(vir_r_m_dr); } where(); settle = &idef->il[F_SETTLE]; nsettle = settle->nr/(1+NRAL(F_SETTLE)); if (nsettle > 0) { nth = gmx_omp_nthreads_get(emntSETTLE); } else { nth = 1; } if (nth > 1 && constr->vir_r_m_dr_th == NULL) { snew(constr->vir_r_m_dr_th, nth); snew(constr->settle_error, nth); } settle_error = -1; /* We do not need full pbc when constraints do not cross charge groups, * i.e. when dd->constraint_comm==NULL. * Note that PBC for constraints is different from PBC for bondeds. * For constraints there is both forward and backward communication. */ if (ir->ePBC != epbcNONE && (cr->dd || bMolPBC) && !(cr->dd && cr->dd->constraint_comm == NULL)) { /* With pbc=screw the screw has been changed to a shift * by the constraint coordinate communication routine, * so that here we can use normal pbc. */ pbc_null = set_pbc_dd(&pbc, ir->ePBC, cr->dd, FALSE, box); } else { pbc_null = NULL; } /* Communicate the coordinates required for the non-local constraints * for LINCS and/or SETTLE. */ if (cr->dd) { dd_move_x_constraints(cr->dd, box, x, xprime, econq == econqCoord); } if (constr->lincsd != NULL) { bOK = constrain_lincs(fplog, bLog, bEner, ir, step, constr->lincsd, md, cr, x, xprime, min_proj, box, pbc_null, lambda, dvdlambda, invdt, v, vir != NULL, vir_r_m_dr, econq, nrnb, constr->maxwarn, &constr->warncount_lincs); if (!bOK && constr->maxwarn >= 0) { if (fplog != NULL) { fprintf(fplog, "Constraint error in algorithm %s at step %s\n", econstr_names[econtLINCS], gmx_step_str(step, buf)); } bDump = TRUE; } } if (constr->nblocks > 0) { switch (econq) { case (econqCoord): bOK = bshakef(fplog, constr->shaked, md->invmass, constr->nblocks, constr->sblock, idef, ir, x, xprime, nrnb, constr->lagr, lambda, dvdlambda, invdt, v, vir != NULL, vir_r_m_dr, constr->maxwarn >= 0, econq, &vetavar); break; case (econqVeloc): bOK = bshakef(fplog, constr->shaked, md->invmass, constr->nblocks, constr->sblock, idef, ir, x, min_proj, nrnb, constr->lagr, lambda, dvdlambda, invdt, NULL, vir != NULL, vir_r_m_dr, constr->maxwarn >= 0, econq, &vetavar); break; default: gmx_fatal(FARGS, "Internal error, SHAKE called for constraining something else than coordinates"); break; } if (!bOK && constr->maxwarn >= 0) { if (fplog != NULL) { fprintf(fplog, "Constraint error in algorithm %s at step %s\n", econstr_names[econtSHAKE], gmx_step_str(step, buf)); } bDump = TRUE; } } if (nsettle > 0) { int calcvir_atom_end; if (vir == NULL) { calcvir_atom_end = 0; } else { calcvir_atom_end = md->homenr; } switch (econq) { case econqCoord: #pragma omp parallel for num_threads(nth) schedule(static) for (th = 0; th < nth; th++) { int start_th, end_th; if (th > 0) { clear_mat(constr->vir_r_m_dr_th[th]); } start_th = (nsettle* th )/nth; end_th = (nsettle*(th+1))/nth; if (start_th >= 0 && end_th - start_th > 0) { csettle(constr->settled, end_th-start_th, settle->iatoms+start_th*(1+NRAL(F_SETTLE)), pbc_null, x[0], xprime[0], invdt, v ? v[0] : NULL, calcvir_atom_end, th == 0 ? vir_r_m_dr : constr->vir_r_m_dr_th[th], th == 0 ? &settle_error : &constr->settle_error[th], &vetavar); } } inc_nrnb(nrnb, eNR_SETTLE, nsettle); if (v != NULL) { inc_nrnb(nrnb, eNR_CONSTR_V, nsettle*3); } if (vir != NULL) { inc_nrnb(nrnb, eNR_CONSTR_VIR, nsettle*3); } break; case econqVeloc: case econqDeriv: case econqForce: case econqForceDispl: #pragma omp parallel for num_threads(nth) schedule(static) for (th = 0; th < nth; th++) { int start_th, end_th; if (th > 0) { clear_mat(constr->vir_r_m_dr_th[th]); } start_th = (nsettle* th )/nth; end_th = (nsettle*(th+1))/nth; if (start_th >= 0 && end_th - start_th > 0) { settle_proj(constr->settled, econq, end_th-start_th, settle->iatoms+start_th*(1+NRAL(F_SETTLE)), pbc_null, x, xprime, min_proj, calcvir_atom_end, th == 0 ? vir_r_m_dr : constr->vir_r_m_dr_th[th], &vetavar); } } /* This is an overestimate */ inc_nrnb(nrnb, eNR_SETTLE, nsettle); break; case econqDeriv_FlexCon: /* Nothing to do, since the are no flexible constraints in settles */ break; default: gmx_incons("Unknown constraint quantity for settle"); } } if (settle->nr > 0) { /* Combine virial and error info of the other threads */ for (i = 1; i < nth; i++) { m_add(vir_r_m_dr, constr->vir_r_m_dr_th[i], vir_r_m_dr); settle_error = constr->settle_error[i]; } if (econq == econqCoord && settle_error >= 0) { bOK = FALSE; if (constr->maxwarn >= 0) { char buf[256]; sprintf(buf, "\nstep " "%"GMX_PRId64 ": Water molecule starting at atom %d can not be " "settled.\nCheck for bad contacts and/or reduce the timestep if appropriate.\n", step, ddglatnr(cr->dd, settle->iatoms[settle_error*(1+NRAL(F_SETTLE))+1])); if (fplog) { fprintf(fplog, "%s", buf); } fprintf(stderr, "%s", buf); constr->warncount_settle++; if (constr->warncount_settle > constr->maxwarn) { too_many_constraint_warnings(-1, constr->warncount_settle); } bDump = TRUE; } } } free_vetavars(&vetavar); if (vir != NULL) { switch (econq) { case econqCoord: vir_fac = 0.5/(ir->delta_t*ir->delta_t); break; case econqVeloc: vir_fac = 0.5/ir->delta_t; break; case econqForce: case econqForceDispl: vir_fac = 0.5; break; default: vir_fac = 0; gmx_incons("Unsupported constraint quantity for virial"); } if (EI_VV(ir->eI)) { vir_fac *= 2; /* only constraining over half the distance here */ } for (i = 0; i < DIM; i++) { for (j = 0; j < DIM; j++) { (*vir)[i][j] = vir_fac*vir_r_m_dr[i][j]; } } } if (bDump) { dump_confs(fplog, step, constr->warn_mtop, start, homenr, cr, x, xprime, box); } if (econq == econqCoord) { if (ir->ePull == epullCONSTRAINT) { if (EI_DYNAMICS(ir->eI)) { t = ir->init_t + (step + delta_step)*ir->delta_t; } else { t = ir->init_t; } set_pbc(&pbc, ir->ePBC, box); pull_constraint(ir->pull, md, &pbc, cr, ir->delta_t, t, x, xprime, v, *vir); } if (constr->ed && delta_step > 0) { /* apply the essential dynamcs constraints here */ do_edsam(ir, step, cr, xprime, v, box, constr->ed); } } return bOK; }
int gmx_vanhove(int argc,char *argv[]) { const char *desc[] = { "g_vanhove computes the Van Hove correlation function.", "The Van Hove G(r,t) is the probability that a particle that is at r0", "at time zero can be found at position r0+r at time t.", "g_vanhove determines G not for a vector r, but for the length of r.", "Thus it gives the probability that a particle moves a distance of r", "in time t.", "Jumps across the periodic boundaries are removed.", "Corrections are made for scaling due to isotropic", "or anisotropic pressure coupling.", "[PAR]", "With option [TT]-om[tt] the whole matrix can be written as a function", "of t and r or as a function of sqrt(t) and r (option [TT]-sqrt[tt]).", "[PAR]", "With option [TT]-or[tt] the Van Hove function is plotted for one", "or more values of t. Option [TT]-nr[tt] sets the number of times,", "option [TT]-fr[tt] the number spacing between the times.", "The binwidth is set with option [TT]-rbin[tt]. The number of bins", "is determined automatically.", "[PAR]", "With option [TT]-ot[tt] the integral up to a certain distance", "(option [TT]-rt[tt]) is plotted as a function of time.", "[PAR]", "For all frames that are read the coordinates of the selected particles", "are stored in memory. Therefore the program may use a lot of memory.", "For options [TT]-om[tt] and [TT]-ot[tt] the program may be slow.", "This is because the calculation scales as the number of frames times", "[TT]-fm[tt] or [TT]-ft[tt].", "Note that with the [TT]-dt[tt] option the memory usage and calculation", "time can be reduced." }; static int fmmax=0,ftmax=0,nlev=81,nr=1,fshift=0; static real sbin=0,rmax=2,rbin=0.01,mmax=0,rint=0; t_pargs pa[] = { { "-sqrt", FALSE, etREAL,{&sbin}, "Use sqrt(t) on the matrix axis which binspacing # in sqrt(ps)" }, { "-fm", FALSE, etINT, {&fmmax}, "Number of frames in the matrix, 0 is plot all" }, { "-rmax", FALSE, etREAL, {&rmax}, "Maximum r in the matrix (nm)" }, { "-rbin", FALSE, etREAL, {&rbin}, "Binwidth in the matrix and for -or (nm)" }, { "-mmax", FALSE, etREAL, {&mmax}, "Maximum density in the matrix, 0 is calculate (1/nm)" }, { "-nlevels" ,FALSE, etINT, {&nlev}, "Number of levels in the matrix" }, { "-nr", FALSE, etINT, {&nr}, "Number of curves for the -or output" }, { "-fr", FALSE, etINT, {&fshift}, "Frame spacing for the -or output" }, { "-rt", FALSE, etREAL, {&rint}, "Integration limit for the -ot output (nm)" }, { "-ft", FALSE, etINT, {&ftmax}, "Number of frames in the -ot output, 0 is plot all" } }; #define NPA asize(pa) t_filenm fnm[] = { { efTRX, NULL, NULL, ffREAD }, { efTPS, NULL, NULL, ffREAD }, { efNDX, NULL, NULL, ffOPTRD }, { efXPM, "-om", "vanhove", ffOPTWR }, { efXVG, "-or", "vanhove_r", ffOPTWR }, { efXVG, "-ot", "vanhove_t", ffOPTWR } }; #define NFILE asize(fnm) output_env_t oenv; const char *matfile,*otfile,*orfile; char title[256]; t_topology top; int ePBC; matrix boxtop,box,*sbox,avbox,corr; rvec *xtop,*x,**sx; int isize,nalloc,nallocn,natom; t_trxstatus *status; atom_id *index; char *grpname; int nfr,f,ff,i,m,mat_nx=0,nbin=0,bin,mbin,fbin; real *time,t,invbin=0,rmax2=0,rint2=0,d2; real invsbin=0,matmax,normfac,dt,*tickx,*ticky; char buf[STRLEN],**legend; real **mat=NULL; int *pt=NULL,**pr=NULL,*mcount=NULL,*tcount=NULL,*rcount=NULL; FILE *fp; t_rgb rlo={1,1,1}, rhi={0,0,0}; CopyRight(stderr,argv[0]); parse_common_args(&argc,argv,PCA_CAN_VIEW | PCA_CAN_TIME | PCA_BE_NICE, NFILE,fnm,asize(pa),pa,asize(desc),desc,0,NULL,&oenv); matfile = opt2fn_null("-om",NFILE,fnm); if (opt2parg_bSet("-fr",NPA,pa)) orfile = opt2fn("-or",NFILE,fnm); else orfile = opt2fn_null("-or",NFILE,fnm); if (opt2parg_bSet("-rt",NPA,pa)) otfile = opt2fn("-ot",NFILE,fnm); else otfile = opt2fn_null("-ot",NFILE,fnm); if (!matfile && !otfile && !orfile) { fprintf(stderr, "For output set one (or more) of the output file options\n"); exit(0); } read_tps_conf(ftp2fn(efTPS,NFILE,fnm),title,&top,&ePBC,&xtop,NULL,boxtop, FALSE); get_index(&top.atoms,ftp2fn_null(efNDX,NFILE,fnm),1,&isize,&index,&grpname); nalloc = 0; time = NULL; sbox = NULL; sx = NULL; clear_mat(avbox); natom=read_first_x(oenv,&status,ftp2fn(efTRX,NFILE,fnm),&t,&x,box); nfr = 0; do { if (nfr >= nalloc) { nalloc += 100; srenew(time,nalloc); srenew(sbox,nalloc); srenew(sx,nalloc); } time[nfr] = t; copy_mat(box,sbox[nfr]); /* This assumes that the off-diagonal box elements * are not affected by jumps across the periodic boundaries. */ m_add(avbox,box,avbox); snew(sx[nfr],isize); for(i=0; i<isize; i++) copy_rvec(x[index[i]],sx[nfr][i]); nfr++; } while (read_next_x(oenv,status,&t,natom,x,box)); /* clean up */ sfree(x); close_trj(status); fprintf(stderr,"Read %d frames\n",nfr); dt = (time[nfr-1] - time[0])/(nfr - 1); /* Some ugly rounding to get nice nice times in the output */ dt = (int)(10000.0*dt + 0.5)/10000.0; invbin = 1.0/rbin; if (matfile) { if (fmmax <= 0 || fmmax >= nfr) fmmax = nfr - 1; snew(mcount,fmmax); nbin = (int)(rmax*invbin + 0.5); if (sbin == 0) { mat_nx = fmmax + 1; } else { invsbin = 1.0/sbin; mat_nx = sqrt(fmmax*dt)*invsbin + 1; } snew(mat,mat_nx); for(f=0; f<mat_nx; f++) snew(mat[f],nbin); rmax2 = sqr(nbin*rbin); /* Initialize time zero */ mat[0][0] = nfr*isize; mcount[0] += nfr; } else { fmmax = 0; } if (orfile) { snew(pr,nr); nalloc = 0; snew(rcount,nr); } if (otfile) { if (ftmax <= 0) ftmax = nfr - 1; snew(tcount,ftmax); snew(pt,nfr); rint2 = rint*rint; /* Initialize time zero */ pt[0] = nfr*isize; tcount[0] += nfr; } else { ftmax = 0; } msmul(avbox,1.0/nfr,avbox); for(f=0; f<nfr; f++) { if (f % 100 == 0) fprintf(stderr,"\rProcessing frame %d",f); /* Scale all the configuration to the average box */ m_inv_ur0(sbox[f],corr); mmul_ur0(avbox,corr,corr); for(i=0; i<isize; i++) { mvmul_ur0(corr,sx[f][i],sx[f][i]); if (f > 0) { /* Correct for periodic jumps */ for(m=DIM-1; m>=0; m--) { while(sx[f][i][m] - sx[f-1][i][m] > 0.5*avbox[m][m]) rvec_dec(sx[f][i],avbox[m]); while(sx[f][i][m] - sx[f-1][i][m] <= -0.5*avbox[m][m]) rvec_inc(sx[f][i],avbox[m]); } } } for(ff=0; ff<f; ff++) { fbin = f - ff; if (fbin <= fmmax || fbin <= ftmax) { if (sbin == 0) mbin = fbin; else mbin = (int)(sqrt(fbin*dt)*invsbin + 0.5); for(i=0; i<isize; i++) { d2 = distance2(sx[f][i],sx[ff][i]); if (mbin < mat_nx && d2 < rmax2) { bin = (int)(sqrt(d2)*invbin + 0.5); if (bin < nbin) { mat[mbin][bin] += 1; } } if (fbin <= ftmax && d2 <= rint2) pt[fbin]++; } if (matfile) mcount[mbin]++; if (otfile) tcount[fbin]++; } } if (orfile) { for(fbin=0; fbin<nr; fbin++) { ff = f - (fbin + 1)*fshift; if (ff >= 0) { for(i=0; i<isize; i++) { d2 = distance2(sx[f][i],sx[ff][i]); bin = (int)(sqrt(d2)*invbin); if (bin >= nalloc) { nallocn = 10*(bin/10) + 11; for(m=0; m<nr; m++) { srenew(pr[m],nallocn); for(i=nalloc; i<nallocn; i++) pr[m][i] = 0; } nalloc = nallocn; } pr[fbin][bin]++; } rcount[fbin]++; } } } } fprintf(stderr,"\n"); if (matfile) { matmax = 0; for(f=0; f<mat_nx; f++) { normfac = 1.0/(mcount[f]*isize*rbin); for(i=0; i<nbin; i++) { mat[f][i] *= normfac; if (mat[f][i] > matmax && (f!=0 || i!=0)) matmax = mat[f][i]; } } fprintf(stdout,"Value at (0,0): %.3f, maximum of the rest %.3f\n", mat[0][0],matmax); if (mmax > 0) matmax = mmax; snew(tickx,mat_nx); for(f=0; f<mat_nx; f++) { if (sbin == 0) tickx[f] = f*dt; else tickx[f] = f*sbin; } snew(ticky,nbin+1); for(i=0; i<=nbin; i++) ticky[i] = i*rbin; fp = ffopen(matfile,"w"); write_xpm(fp,MAT_SPATIAL_Y,"Van Hove function","G (1/nm)", sbin==0 ? "time (ps)" : "sqrt(time) (ps^1/2)","r (nm)", mat_nx,nbin,tickx,ticky,mat,0,matmax,rlo,rhi,&nlev); ffclose(fp); } if (orfile) { fp = xvgropen(orfile,"Van Hove function","r (nm)","G (nm\\S-1\\N)",oenv); fprintf(fp,"@ subtitle \"for particles in group %s\"\n",grpname); snew(legend,nr); for(fbin=0; fbin<nr; fbin++) { sprintf(buf,"%g ps",(fbin + 1)*fshift*dt); legend[fbin] = strdup(buf); } xvgr_legend(fp,nr,(const char**)legend,oenv); for(i=0; i<nalloc; i++) { fprintf(fp,"%g",i*rbin); for(fbin=0; fbin<nr; fbin++) fprintf(fp," %g", (real)pr[fbin][i]/(rcount[fbin]*isize*rbin*(i==0 ? 0.5 : 1))); fprintf(fp,"\n"); } ffclose(fp); } if (otfile) { sprintf(buf,"Probability of moving less than %g nm",rint); fp = xvgropen(otfile,buf,"t (ps)","",oenv); fprintf(fp,"@ subtitle \"for particles in group %s\"\n",grpname); for(f=0; f<=ftmax; f++) fprintf(fp,"%g %g\n",f*dt,(real)pt[f]/(tcount[f]*isize)); ffclose(fp); } do_view(oenv, matfile,NULL); do_view(oenv, orfile,NULL); do_view(oenv, otfile,NULL); thanx(stderr); return 0; }
void Vehicle::vehicle_model(veh_str *vehicl, double Sim_Time) { //char dummy[256]; double dp,dr,ds; //double dp1; double u[3]; static double u_old,old_phi,old_theta,old_omegac; double heading_rads; double head_err, depth_err, pitch_err, prop_err; static double cosomega, sinomega; static double costheta, sintheta, tantheta; static double cosphi, sinphi; static double rb2t[9]; //(3,3) static double rt2b[9]; //(3,3) double *pos; double *ang; double *vect; double dpos[3]; double dang[3]; double dvect[6]; double dvec1[6]; double dvec2[6]; double dvec3[6]; double dvec4[6]; double dxth; double dxuv[2]; double dxu1[2]; double dxu2[2]; int i; (*vehicl).phi = (*vehicl).phi*PI/180; // Wei (*vehicl).theta = (*vehicl).theta*PI/180; if (init_flag == 1) { m_invert(Mhinv,Mh,3); m_mul(Ah,Mhinv,Ch,3,3,3); m_mul(Bh,Mhinv,Dh,3,3,1); m_invert(Mdinv,Md,4); m_mul(Ad,Mdinv,Cd,4,4,4); m_mul(Bd,Mdinv,Dd,4,4,1); memset(A, 0, 36*sizeof(double)); A[0] = Xu/(mm - Xud); //(0,0) A[7] = Ah[0]; //(1,1)=(0,0) A[11] = Ah[2]; //(1,5)=(0,2) A[14] = Ad[0]; //(2,2)=(0,0) A[16] = Ad[1]; //(2,4)=(0,1) A[21] = Kp/(Ixx - Kpd); //(3,3) A[23] = (*vehicl).u*mm*zg; //(3,5) A[26] = Ad[4]; //(4,2)=(1,0) A[28] = Ad[5]; //(4,4)=(1,1) A[31] = Ah[6]; //(5,1)=(2,0) A[35] = Ah[8]; //(5,5)=(2,2) memset(B, 0, 18*sizeof(double)); B[0] = Xdp/(mm-Xud); //(0,0) B[1] = Xdr/(mm-Xud); //(0,1) B[2] = Xds/(mm-Xud); //(0,2) B[4] = Bh[0]; //(1,1)=(0,0) B[8] = Bd[0]; //(2,2)=(0,0) B[9] = Kdp/(Ixx-Kpd); //(3,0) B[14] = Bd[1]; //(4,2)=(1,0) B[16] = Bh[2]; //(5,1)=(2,0) memset(C, 0, 18*sizeof(double)); C[5] = Ah[1]; //(1,2)=(0,1) C[7] = Ad[3]; //(2,1)=(0,3) C[9] = Kphi/(Ixx-Kpd); //(3,0) C[13] = Ad[7]; //(4,1)=(1,3) C[17] = Ah[7]; //(5,2)=(2,1) for(i = 0; i < 16; i++) { Cd1[i]=Cd[i]; } for(i = 0; i < 9; i++) { Ch1[i]=Ch[i]; } d2r = PI/180.0; rw2a[0] = 1; //(0,0) rw2a[3] = 0; //(1,0) rw2a[6] = 0; //(2,0) u_old = 100; old_phi = 100; old_theta = 100; old_omegac = 100; } if(abs((int)((*vehicl).u - u_old))>0.2){ u_old = (*vehicl).u; Cd1[1] = (*vehicl).u*mm + Zq; //(0,1) Cd1[4] = (*vehicl).u*(-mm)*xg + Mq; //(1,1) Cd1[11] = -(*vehicl).u; //(2,3) Ch1[2] = Yr - (*vehicl).u*mm; //(0,2) Ch1[5] = Nr - (*vehicl).u*mm*xg; //(1,2) m_mul(Ah,Mhinv,Ch1,3,3,3); m_mul(Ad,Mdinv,Cd1,4,4,4); A[23] = (*vehicl).u*mm*zg; //(3,5) A[35] = Ah[8]; //(5,5)=(2,2) A[16] = Ad[1]; //(2,4)=(0,1) A[28] = Ad[5]; //(4,4)=(1,1) } heading_rads = (*vehicl).omega*d2r; old_phi = (int)((*vehicl).phi); old_theta = (*vehicl).theta; old_omegac = (*vehicl).omega; cosomega = cos(heading_rads); costheta = cos((*vehicl).theta); //wei sinomega = sin(heading_rads); sintheta = sin((*vehicl).theta); cosphi = cos((*vehicl).phi); sinphi = sin((*vehicl).phi); tantheta = tan((*vehicl).theta); rt2b[0] = cosomega*costheta; //(0,0) rt2b[1] = sinomega*costheta; //(0,1) rt2b[2] = -sintheta; //(0,2) rt2b[3] = -sinomega*cosphi+cosomega*sintheta*sinphi; //(1,0) rt2b[4] = cosomega*cosphi+sinomega*sintheta*sinphi; //(1,1) rt2b[5] = costheta*sinphi; //(1,2) rt2b[6] = sinomega*sinphi+cosomega*sintheta*cosphi; //(2,0) rt2b[7] = -cosomega*sinphi+sinomega*sintheta*cosphi; //(2,1) rt2b[8] = costheta*cosphi; //(2,2) m_transpose(rb2t,rt2b,3,3); rw2a[1] = sinphi*tantheta; //(0,1) rw2a[2] = cosphi*tantheta; //(0,2) rw2a[4] = cosphi; //(1,1) rw2a[5] = -sinphi; //(1,2) rw2a[7] = sinphi/costheta; //(2,1) rw2a[8] = cosphi/costheta; //(2,2) pos = &((*vehicl).x); ang = &((*vehicl).phi); vect = &((*vehicl).u); //---------------------------heading control--------------------------------- head_err = ((vehicl->omega_c) - (vehicl->omega)); // degrees head_err = head_err*d2r; // radians while(head_err>PI) head_err = head_err - 2.0*PI; while(head_err<-PI) head_err = head_err + 2.0*PI; dr = head_err*-0.2; // Calculate dr //--------------------------------depth control----------------------------- depth_err = (*vehicl).z_c - (*vehicl).z; (*vehicl).theta_c = (-0.772*depth_err); if((*vehicl).theta_c>0.5) (*vehicl).theta_c = 0.5; if((*vehicl).theta_c<-0.5) (*vehicl).theta_c = -0.5; pitch_err = ((*vehicl).theta_c - (*vehicl).theta); ds = ((*vehicl).xth)*Cth + Dth*pitch_err; // Calculate ds if(ds>0.8) // saturation ds = 0.8; if(ds<-0.8) ds = -0.8; //-----------------------------propulsion control--------------------------- prop_err = (*vehicl).vel_c - (*vehicl).u; dp = Cu[0]*(*vehicl).xu1 + Cu[1]*(*vehicl).xu2 + prop_err*Du; // Calculate dp u[0] = dp; u[1] = dr; u[2] = ds; m_mul(dpos,rb2t,vect,3,3,1); m_mul(dang,rw2a,(vect+3),3,3,1); m_mul(dvec1,A,vect,6,6,1); m_mul(dvec2,B,u,6,3,1); m_mul(dvec3,C,ang,6,3,1); m_add(dvec4,dvec1,dvec2,6,1); m_add(dvect,dvec3,dvec4,6,1); m_mul(dxu1,Au,&((*vehicl).xu1),2,2,1); m_muls(dxu2,Bu,prop_err,2,1); m_add(dxuv,dxu1,dxu2,2,1); dxth = (Ath*((*vehicl).xth) + Bth*pitch_err); // jay (*vehicl).xth += dxth*dt; /* for (i=0;i<3;i++) { // jay pos[i] += dpos[i]*dt; } */ pos[0] += dpos[0]*dt; // x pos[1] += (-1)*dpos[1]*dt; // y, invert because of axis flip pos[2] += dpos[2]*dt; // z // for (i=0;i<2;i++){ //jay ang[i] += dang[i]*dt; //Wei } ang[2] += dang[2]*dt*180.0/PI; (*vehicl).phi = ang[0]*180/PI; // Wei (*vehicl).theta = ang[1]*180/PI; (*vehicl).omega = ang[2]; for (int j=0;j<6;j++){ vect[j] += dvect[j]*dt; } (*vehicl).xu1 += dxuv[0]*dt; (*vehicl).xu2 += dxuv[1]*dt; //-------------------------------------------------------------------------- if(((*vehicl).src_find_tm) > Sim_Time){ // if not source found (*vehicl).dsf += ((*vehicl).u)*(dt);} // integrated path length to source if(((*vehicl).plm_find_tm) > Sim_Time){ // if not plume found (*vehicl).dpf += ((*vehicl).u)*(dt);} // integrated path length to plume if(!((*vehicl).src_fnd_flg)){ // if src fnd has not been declared (*vehicl).dsd += ((*vehicl).u)*(dt);} // integrated path length to plume while((*vehicl).omega >= 180.0) (*vehicl).omega = (*vehicl).omega - 360.0; while((*vehicl).omega < -180.0) (*vehicl).omega = (*vehicl).omega + 360.0; }
void m_add_mailaddr(struct mproc *m, const struct mailaddr *maddr) { m_add(m, maddr, sizeof(*maddr)); }
/* TODO Specialize this routine into init-time and loop-time versions? e.g. bReadEkin is only true when restoring from checkpoint */ void compute_globals(FILE *fplog, gmx_global_stat *gstat, t_commrec *cr, t_inputrec *ir, t_forcerec *fr, gmx_ekindata_t *ekind, t_state *state, t_mdatoms *mdatoms, t_nrnb *nrnb, t_vcm *vcm, gmx_wallcycle_t wcycle, gmx_enerdata_t *enerd, tensor force_vir, tensor shake_vir, tensor total_vir, tensor pres, rvec mu_tot, gmx_constr_t constr, gmx::SimulationSignaller *signalCoordinator, matrix box, int *totalNumberOfBondedInteractions, gmx_bool *bSumEkinhOld, int flags) { tensor corr_vir, corr_pres; gmx_bool bEner, bPres, bTemp; gmx_bool bStopCM, bGStat, bReadEkin, bEkinAveVel, bScaleEkin, bConstrain; real prescorr, enercorr, dvdlcorr, dvdl_ekin; /* translate CGLO flags to gmx_booleans */ bStopCM = flags & CGLO_STOPCM; bGStat = flags & CGLO_GSTAT; bReadEkin = (flags & CGLO_READEKIN); bScaleEkin = (flags & CGLO_SCALEEKIN); bEner = flags & CGLO_ENERGY; bTemp = flags & CGLO_TEMPERATURE; bPres = (flags & CGLO_PRESSURE); bConstrain = (flags & CGLO_CONSTRAINT); /* we calculate a full state kinetic energy either with full-step velocity verlet or half step where we need the pressure */ bEkinAveVel = (ir->eI == eiVV || (ir->eI == eiVVAK && bPres) || bReadEkin); /* in initalization, it sums the shake virial in vv, and to sums ekinh_old in leapfrog (or if we are calculating ekinh_old) for other reasons */ /* ########## Kinetic energy ############## */ if (bTemp) { /* Non-equilibrium MD: this is parallellized, but only does communication * when there really is NEMD. */ if (PAR(cr) && (ekind->bNEMD)) { accumulate_u(cr, &(ir->opts), ekind); } if (!bReadEkin) { calc_ke_part(state, &(ir->opts), mdatoms, ekind, nrnb, bEkinAveVel); } } /* Calculate center of mass velocity if necessary, also parallellized */ if (bStopCM) { calc_vcm_grp(0, mdatoms->homenr, mdatoms, state->x, state->v, vcm); } if (bTemp || bStopCM || bPres || bEner || bConstrain) { if (!bGStat) { /* We will not sum ekinh_old, * so signal that we still have to do it. */ *bSumEkinhOld = TRUE; } else { gmx::ArrayRef<real> signalBuffer = signalCoordinator->getCommunicationBuffer(); if (PAR(cr)) { wallcycle_start(wcycle, ewcMoveE); global_stat(gstat, cr, enerd, force_vir, shake_vir, mu_tot, ir, ekind, constr, bStopCM ? vcm : NULL, signalBuffer.size(), signalBuffer.data(), totalNumberOfBondedInteractions, *bSumEkinhOld, flags); wallcycle_stop(wcycle, ewcMoveE); } signalCoordinator->finalizeSignals(); *bSumEkinhOld = FALSE; } } if (!ekind->bNEMD && debug && bTemp && (vcm->nr > 0)) { correct_ekin(debug, 0, mdatoms->homenr, state->v, vcm->group_p[0], mdatoms->massT, mdatoms->tmass, ekind->ekin); } /* Do center of mass motion removal */ if (bStopCM) { check_cm_grp(fplog, vcm, ir, 1); do_stopcm_grp(0, mdatoms->homenr, mdatoms->cVCM, state->x, state->v, vcm); inc_nrnb(nrnb, eNR_STOPCM, mdatoms->homenr); } if (bEner) { /* Calculate the amplitude of the cosine velocity profile */ ekind->cosacc.vcos = ekind->cosacc.mvcos/mdatoms->tmass; } if (bTemp) { /* Sum the kinetic energies of the groups & calc temp */ /* compute full step kinetic energies if vv, or if vv-avek and we are computing the pressure with inputrecNptTrotter */ /* three maincase: VV with AveVel (md-vv), vv with AveEkin (md-vv-avek), leap with AveEkin (md). Leap with AveVel is not supported; it's not clear that it will actually work. bEkinAveVel: If TRUE, we simply multiply ekin by ekinscale to get a full step kinetic energy. If FALSE, we average ekinh_old and ekinh*ekinscale_nhc to get an averaged half step kinetic energy. */ enerd->term[F_TEMP] = sum_ekin(&(ir->opts), ekind, &dvdl_ekin, bEkinAveVel, bScaleEkin); enerd->dvdl_lin[efptMASS] = (double) dvdl_ekin; enerd->term[F_EKIN] = trace(ekind->ekin); } /* ########## Long range energy information ###### */ if (bEner || bPres || bConstrain) { calc_dispcorr(ir, fr, box, state->lambda[efptVDW], corr_pres, corr_vir, &prescorr, &enercorr, &dvdlcorr); } if (bEner) { enerd->term[F_DISPCORR] = enercorr; enerd->term[F_EPOT] += enercorr; enerd->term[F_DVDL_VDW] += dvdlcorr; } /* ########## Now pressure ############## */ if (bPres || bConstrain) { m_add(force_vir, shake_vir, total_vir); /* Calculate pressure and apply LR correction if PPPM is used. * Use the box from last timestep since we already called update(). */ enerd->term[F_PRES] = calc_pres(fr->ePBC, ir->nwall, box, ekind->ekin, total_vir, pres); /* Calculate long range corrections to pressure and energy */ /* this adds to enerd->term[F_PRES] and enerd->term[F_ETOT], and computes enerd->term[F_DISPCORR]. Also modifies the total_vir and pres tesors */ m_add(total_vir, corr_vir, total_vir); m_add(pres, corr_pres, pres); enerd->term[F_PDISPCORR] = prescorr; enerd->term[F_PRES] += prescorr; } }
// default implementation calling low-level _sys->continuous() //-------------------------------------------------------------------------- void Omu_IntODE::syseq(double t, const VECP y, const VECP u, VECP f) { #ifdef OMU_WITH_ADOLC if (!_sys->has_low_level_continuous()) { // call faster version if no user defined low-level continuous syseq_forward(t, y, u, f); return; } #endif int i, j; Omu_StateVec &xt = *_xt_ptr; Omu_DependentVec &Ft = *_Ft_ptr; // // prepare call arguments // for (i = 0; i < _nd; i++) { xt[i] = u[i]; } for (i = 0; i < _n; i++) { xt[_nd + i] = y[i]; } for (i = 0; i < _nu; i++) { _ut[i] = u[_nd + i]; } if (_sa) { for (i = _nd; i < _nxt; i++) { for (j = 0; j < _nx; j++) { xt.Sx[i][j] = y[(1 + j) * _n + i - _nd]; } for (j = 0; j < _nu; j++) { xt.Su[i][j] = y[(1 + _nx + j) * _n + i - _nd]; } } } // // evaluate residual // Ft.set_required_J(_sa); // an integrator may request the Jacobian _sys->continuous(_kk, t, xt, _ut, _dxt, Ft); for (i = _nd; i < _nxt; i++) { // f = F * -(dF/ddx)^(-1) f[i - _nd] = Ft[i] / -Ft.Jdx[i][i]; } if (_sa) { m_mlt(Ft.Jx, xt.Sx, _Yx); m_mlt(Ft.Jx, xt.Su, _Yu); m_add(_Yu, Ft.Ju, _Yu); for (i = _nd; i < _nxt; i++) { for (j = 0; j < _nx; j++) { f[(1 + j) * _n + i - _nd] = _Yx[i][j] / -Ft.Jdx[i][i]; } for (j = 0; j < _nu; j++) { f[(1 + _nx + j) * _n + i - _nd] = _Yu[i][j] / -Ft.Jdx[i][i]; } } } _res_evals++; if (_sa) _sen_evals++; }
void do_force(FILE *fplog,t_commrec *cr, t_inputrec *inputrec, int step,t_nrnb *nrnb,gmx_wallcycle_t wcycle, gmx_localtop_t *top, gmx_groups_t *groups, matrix box,rvec x[],history_t *hist, rvec f[],rvec buf[], tensor vir_force, t_mdatoms *mdatoms, gmx_enerdata_t *enerd,t_fcdata *fcd, real lambda,t_graph *graph, t_forcerec *fr,gmx_vsite_t *vsite,rvec mu_tot, real t,FILE *field,gmx_edsam_t ed, int flags) { static rvec box_size; int cg0,cg1,i,j; int start,homenr; static double mu[2*DIM]; rvec mu_tot_AB[2]; bool bSepDVDL,bStateChanged,bNS,bFillGrid,bCalcCGCM,bBS,bDoForces; matrix boxs; real e,v,dvdl; t_pbc pbc; float cycles_ppdpme,cycles_pme,cycles_force; start = mdatoms->start; homenr = mdatoms->homenr; bSepDVDL = (fr->bSepDVDL && do_per_step(step,inputrec->nstlog)); clear_mat(vir_force); if (PARTDECOMP(cr)) { pd_cg_range(cr,&cg0,&cg1); } else { cg0 = 0; if (DOMAINDECOMP(cr)) cg1 = cr->dd->ncg_tot; else cg1 = top->cgs.nr; if (fr->n_tpi > 0) cg1--; } bStateChanged = (flags & GMX_FORCE_STATECHANGED); bNS = (flags & GMX_FORCE_NS); bFillGrid = (bNS && bStateChanged); bCalcCGCM = (bFillGrid && !DOMAINDECOMP(cr)); bDoForces = (flags & GMX_FORCE_FORCES); if (bStateChanged) { update_forcerec(fplog,fr,box); /* Calculate total (local) dipole moment in a temporary common array. * This makes it possible to sum them over nodes faster. */ calc_mu(start,homenr, x,mdatoms->chargeA,mdatoms->chargeB,mdatoms->nChargePerturbed, mu,mu+DIM); } if (fr->ePBC != epbcNONE) { /* Compute shift vectors every step, * because of pressure coupling or box deformation! */ if (DYNAMIC_BOX(*inputrec) && bStateChanged) calc_shifts(box,fr->shift_vec); if (bCalcCGCM) { put_charge_groups_in_box(fplog,cg0,cg1,fr->ePBC,box, &(top->cgs),x,fr->cg_cm); inc_nrnb(nrnb,eNR_CGCM,homenr); inc_nrnb(nrnb,eNR_RESETX,cg1-cg0); } else if (EI_ENERGY_MINIMIZATION(inputrec->eI) && graph) { unshift_self(graph,box,x); } } else if (bCalcCGCM) { calc_cgcm(fplog,cg0,cg1,&(top->cgs),x,fr->cg_cm); inc_nrnb(nrnb,eNR_CGCM,homenr); } if (bCalcCGCM) { if (PAR(cr)) { move_cgcm(fplog,cr,fr->cg_cm); } if (gmx_debug_at) pr_rvecs(debug,0,"cgcm",fr->cg_cm,top->cgs.nr); } #ifdef GMX_MPI if (!(cr->duty & DUTY_PME)) { /* Send particle coordinates to the pme nodes. * Since this is only implemented for domain decomposition * and domain decomposition does not use the graph, * we do not need to worry about shifting. */ wallcycle_start(wcycle,ewcPP_PMESENDX); GMX_MPE_LOG(ev_send_coordinates_start); bBS = (inputrec->nwall == 2); if (bBS) { copy_mat(box,boxs); svmul(inputrec->wall_ewald_zfac,boxs[ZZ],boxs[ZZ]); } gmx_pme_send_x(cr,bBS ? boxs : box,x,mdatoms->nChargePerturbed,lambda); GMX_MPE_LOG(ev_send_coordinates_finish); wallcycle_stop(wcycle,ewcPP_PMESENDX); } #endif /* GMX_MPI */ /* Communicate coordinates and sum dipole if necessary */ if (PAR(cr)) { wallcycle_start(wcycle,ewcMOVEX); if (DOMAINDECOMP(cr)) { dd_move_x(cr->dd,box,x,buf); } else { move_x(fplog,cr,GMX_LEFT,GMX_RIGHT,x,nrnb); } /* When we don't need the total dipole we sum it in global_stat */ if (NEED_MUTOT(*inputrec)) gmx_sumd(2*DIM,mu,cr); wallcycle_stop(wcycle,ewcMOVEX); } for(i=0; i<2; i++) for(j=0;j<DIM;j++) mu_tot_AB[i][j] = mu[i*DIM + j]; if (fr->efep == efepNO) copy_rvec(mu_tot_AB[0],mu_tot); else for(j=0; j<DIM; j++) mu_tot[j] = (1.0 - lambda)*mu_tot_AB[0][j] + lambda*mu_tot_AB[1][j]; /* Reset energies */ reset_energies(&(inputrec->opts),fr,bNS,enerd,MASTER(cr)); if (bNS) { wallcycle_start(wcycle,ewcNS); if (graph && bStateChanged) /* Calculate intramolecular shift vectors to make molecules whole */ mk_mshift(fplog,graph,fr->ePBC,box,x); /* Reset long range forces if necessary */ if (fr->bTwinRange) { clear_rvecs(fr->f_twin_n,fr->f_twin); clear_rvecs(SHIFTS,fr->fshift_twin); } /* Do the actual neighbour searching and if twin range electrostatics * also do the calculation of long range forces and energies. */ dvdl = 0; ns(fplog,fr,x,f,box,groups,&(inputrec->opts),top,mdatoms, cr,nrnb,step,lambda,&dvdl,&enerd->grpp,bFillGrid,bDoForces); if (bSepDVDL) fprintf(fplog,sepdvdlformat,"LR non-bonded",0,dvdl); enerd->dvdl_lr = dvdl; enerd->term[F_DVDL] += dvdl; wallcycle_stop(wcycle,ewcNS); } if (DOMAINDECOMP(cr)) { if (!(cr->duty & DUTY_PME)) { wallcycle_start(wcycle,ewcPPDURINGPME); dd_force_flop_start(cr->dd,nrnb); } } /* Start the force cycle counter. * This counter is stopped in do_forcelow_level. * No parallel communication should occur while this counter is running, * since that will interfere with the dynamic load balancing. */ wallcycle_start(wcycle,ewcFORCE); if (bDoForces) { /* Reset PME/Ewald forces if necessary */ if (fr->bF_NoVirSum) { GMX_BARRIER(cr->mpi_comm_mygroup); if (fr->bDomDec) clear_rvecs(fr->f_novirsum_n,fr->f_novirsum); else clear_rvecs(homenr,fr->f_novirsum+start); GMX_BARRIER(cr->mpi_comm_mygroup); } /* Copy long range forces into normal buffers */ if (fr->bTwinRange) { for(i=0; i<fr->f_twin_n; i++) copy_rvec(fr->f_twin[i],f[i]); for(i=0; i<SHIFTS; i++) copy_rvec(fr->fshift_twin[i],fr->fshift[i]); } else { if (DOMAINDECOMP(cr)) clear_rvecs(cr->dd->nat_tot,f); else clear_rvecs(mdatoms->nr,f); clear_rvecs(SHIFTS,fr->fshift); } clear_rvec(fr->vir_diag_posres); GMX_BARRIER(cr->mpi_comm_mygroup); } if (inputrec->ePull == epullCONSTRAINT) clear_pull_forces(inputrec->pull); /* update QMMMrec, if necessary */ if(fr->bQMMM) update_QMMMrec(cr,fr,x,mdatoms,box,top); if ((flags & GMX_FORCE_BONDED) && top->idef.il[F_POSRES].nr > 0) { /* Position restraints always require full pbc */ set_pbc(&pbc,inputrec->ePBC,box); v = posres(top->idef.il[F_POSRES].nr,top->idef.il[F_POSRES].iatoms, top->idef.iparams_posres, (const rvec*)x,fr->f_novirsum,fr->vir_diag_posres, inputrec->ePBC==epbcNONE ? NULL : &pbc,lambda,&dvdl, fr->rc_scaling,fr->ePBC,fr->posres_com,fr->posres_comB); if (bSepDVDL) { fprintf(fplog,sepdvdlformat, interaction_function[F_POSRES].longname,v,dvdl); } enerd->term[F_POSRES] += v; enerd->term[F_DVDL] += dvdl; inc_nrnb(nrnb,eNR_POSRES,top->idef.il[F_POSRES].nr/2); } /* Compute the bonded and non-bonded forces */ do_force_lowlevel(fplog,step,fr,inputrec,&(top->idef), cr,nrnb,wcycle,mdatoms,&(inputrec->opts), x,hist,f,enerd,fcd,box,lambda,graph,&(top->excls),mu_tot_AB, flags,&cycles_force); GMX_BARRIER(cr->mpi_comm_mygroup); if (ed) { do_flood(fplog,cr,x,f,ed,box,step); } if (DOMAINDECOMP(cr)) { dd_force_flop_stop(cr->dd,nrnb); if (wcycle) dd_cycles_add(cr->dd,cycles_force,ddCyclF); } if (bDoForces) { /* Compute forces due to electric field */ calc_f_el(MASTER(cr) ? field : NULL, start,homenr,mdatoms->chargeA,x,f,inputrec->ex,inputrec->et,t); /* When using PME/Ewald we compute the long range virial there. * otherwise we do it based on long range forces from twin range * cut-off based calculation (or not at all). */ /* Communicate the forces */ if (PAR(cr)) { wallcycle_start(wcycle,ewcMOVEF); if (DOMAINDECOMP(cr)) { dd_move_f(cr->dd,f,buf,fr->fshift); /* Position restraint do not introduce inter-cg forces */ if (EEL_FULL(fr->eeltype) && cr->dd->n_intercg_excl) dd_move_f(cr->dd,fr->f_novirsum,buf,NULL); } else { move_f(fplog,cr,GMX_LEFT,GMX_RIGHT,f,buf,nrnb); } wallcycle_stop(wcycle,ewcMOVEF); } } if (bDoForces) { if (vsite) { wallcycle_start(wcycle,ewcVSITESPREAD); spread_vsite_f(fplog,vsite,x,f,fr->fshift,nrnb, &top->idef,fr->ePBC,fr->bMolPBC,graph,box,cr); wallcycle_stop(wcycle,ewcVSITESPREAD); } /* Calculation of the virial must be done after vsites! */ calc_virial(fplog,mdatoms->start,mdatoms->homenr,x,f, vir_force,graph,box,nrnb,fr,inputrec->ePBC); } if (inputrec->ePull == epullUMBRELLA || inputrec->ePull == epullCONST_F) { /* Calculate the center of mass forces, this requires communication, * which is why pull_potential is called close to other communication. * The virial contribution is calculated directly, * which is why we call pull_potential after calc_virial. */ set_pbc(&pbc,inputrec->ePBC,box); dvdl = 0; enerd->term[F_COM_PULL] = pull_potential(inputrec->ePull,inputrec->pull,mdatoms,&pbc, cr,t,lambda,x,f,vir_force,&dvdl); if (bSepDVDL) fprintf(fplog,sepdvdlformat,"Com pull",enerd->term[F_COM_PULL],dvdl); enerd->term[F_DVDL] += dvdl; } if (!(cr->duty & DUTY_PME)) { cycles_ppdpme = wallcycle_stop(wcycle,ewcPPDURINGPME); dd_cycles_add(cr->dd,cycles_ppdpme,ddCyclPPduringPME); } #ifdef GMX_MPI if (PAR(cr) && !(cr->duty & DUTY_PME)) { /* In case of node-splitting, the PP nodes receive the long-range * forces, virial and energy from the PME nodes here. */ wallcycle_start(wcycle,ewcPP_PMEWAITRECVF); dvdl = 0; gmx_pme_receive_f(cr,fr->f_novirsum,fr->vir_el_recip,&e,&dvdl, &cycles_pme); if (bSepDVDL) fprintf(fplog,sepdvdlformat,"PME mesh",e,dvdl); enerd->term[F_COUL_RECIP] += e; enerd->term[F_DVDL] += dvdl; if (wcycle) dd_cycles_add(cr->dd,cycles_pme,ddCyclPME); wallcycle_stop(wcycle,ewcPP_PMEWAITRECVF); } #endif if (bDoForces && fr->bF_NoVirSum) { if (vsite) { /* Spread the mesh force on virtual sites to the other particles... * This is parallellized. MPI communication is performed * if the constructing atoms aren't local. */ wallcycle_start(wcycle,ewcVSITESPREAD); spread_vsite_f(fplog,vsite,x,fr->f_novirsum,NULL,nrnb, &top->idef,fr->ePBC,fr->bMolPBC,graph,box,cr); wallcycle_stop(wcycle,ewcVSITESPREAD); } /* Now add the forces, this is local */ if (fr->bDomDec) { sum_forces(0,fr->f_novirsum_n,f,fr->f_novirsum); } else { sum_forces(start,start+homenr,f,fr->f_novirsum); } if (EEL_FULL(fr->eeltype)) { /* Add the mesh contribution to the virial */ m_add(vir_force,fr->vir_el_recip,vir_force); } if (debug) pr_rvecs(debug,0,"vir_force",vir_force,DIM); } /* Sum the potential energy terms from group contributions */ sum_epot(&(inputrec->opts),enerd); if (fr->print_force >= 0 && bDoForces) print_large_forces(stderr,mdatoms,cr,step,fr->print_force,x,f); }
void Ukf(VEC *omega, VEC *mag_vec, VEC *mag_vec_I, VEC *sun_vec, VEC *sun_vec_I, VEC *Torq_ext, double t, double h, int eclipse, VEC *state, VEC *st_error, VEC *residual, int *P_flag, double sim_time) { static VEC *omega_prev = VNULL, *mag_vec_prev = VNULL, *sun_vec_prev = VNULL, *q_s_c = VNULL, *x_prev = VNULL, *Torq_prev, *x_m_o; static MAT *Q = {MNULL}, *R = {MNULL}, *Pprev = {MNULL}; static double alpha, kappa, lambda, sqrt_lambda, w_m_0, w_c_0, w_i, beta; static int n_states, n_sig_pts, n_err_states, iter_num, initialize=0; VEC *x = VNULL, *x_priori = VNULL, *x_err_priori = VNULL, *single_sig_pt = VNULL, *v_temp = VNULL, *q_err_quat = VNULL, *err_vec = VNULL, *v_temp2 = VNULL, *x_ang_vel = VNULL, *meas = VNULL, *meas_priori = VNULL, *v_temp3 = VNULL, *x_posteriori_err = VNULL, *x_b_m = VNULL, *x_b_g = VNULL; MAT *sqrt_P = {MNULL}, *P = {MNULL}, *P_priori = {MNULL}, *sig_pt = {MNULL}, *sig_vec_mat = {MNULL}, *err_sig_pt_mat = {MNULL}, *result = {MNULL}, *result_larger = {MNULL}, *result1 = {MNULL}, *Meas_err_mat = {MNULL}, *P_zz = {MNULL}, *iP_vv = {MNULL}, *P_xz = {MNULL}, *K = {MNULL}, *result2 = {MNULL}, *result3 = {MNULL}, *C = {MNULL}; int update_mag_vec, update_sun_vec, update_omega, i, j; double d_res; if (inertia == MNULL) { inertia = m_get(3,3); m_ident(inertia); inertia->me[0][0] = 0.007; inertia->me[1][1] = 0.014; inertia->me[2][2] = 0.015; } if (initialize == 0){ iter_num = 1; n_states = (7+6); n_err_states = (6+6); n_sig_pts = 2*n_err_states+1; alpha = sqrt(3); kappa = 3 - n_states; lambda = alpha*alpha * (n_err_states+kappa) - n_err_states; beta = -(1-(alpha*alpha)); w_m_0 = (lambda)/(n_err_states + lambda); w_c_0 = (lambda/(n_err_states + lambda)) + (1 - (alpha*alpha) + beta); w_i = 0.5/(n_err_states +lambda); initialize = 1; sqrt_lambda = (lambda+n_err_states); if(q_s_c == VNULL) { q_s_c = v_get(4); q_s_c->ve[0] = -0.020656; q_s_c->ve[1] = 0.71468; q_s_c->ve[2] = -0.007319; q_s_c->ve[3] = 0.6991; } if(Torq_prev == VNULL) { Torq_prev = v_get(3); v_zero(Torq_prev); } quat_normalize(q_s_c); } result = m_get(9,9); m_zero(result); result1 = m_get(n_err_states, n_err_states); m_zero(result1); if(x_m_o == VNULL) { x_m_o = v_get(n_states); v_zero(x_m_o); } x = v_get(n_states); v_zero(x); x_err_priori = v_get(n_err_states); v_zero(x_err_priori); x_ang_vel = v_get(3); v_zero(x_ang_vel); sig_pt = m_get(n_states, n_err_states); m_zero(sig_pt); if (C == MNULL) { C = m_get(9, 12); m_zero(C); } if (P_priori == MNULL) { P_priori = m_get(n_err_states, n_err_states); m_zero(P_priori); } if (Q == MNULL) { Q = m_get(n_err_states, n_err_states); m_ident(Q); // Q->me[0][0] = 0.0001; Q->me[1][1] = 0.0001; Q->me[2][2] = 0.0001; Q->me[3][3] = 0.0001; Q->me[4][4] = 0.0001; Q->me[5][5] = 0.0001; Q->me[6][6] = 0.000001; Q->me[7][7] = 0.000001; Q->me[8][8] = 0.000001; Q->me[9][9] = 0.000001; Q->me[10][10] = 0.000001; Q->me[11][11] = 0.000001; } if( Pprev == MNULL) { Pprev = m_get(n_err_states, n_err_states); m_ident(Pprev); Pprev->me[0][0] = 1e-3; Pprev->me[1][1] = 1e-3; Pprev->me[2][2] = 1e-3; Pprev->me[3][3] = 1e-3; Pprev->me[4][4] = 1e-3; Pprev->me[5][5] = 1e-3; Pprev->me[6][6] = 1e-4; Pprev->me[7][7] = 1e-4; Pprev->me[8][8] = 1e-4; Pprev->me[9][9] = 1e-3; Pprev->me[10][10] = 1e-3; Pprev->me[11][11] = 1e-3; } if (R == MNULL) { R = m_get(9,9); m_ident(R); R->me[0][0] = 0.034; R->me[1][1] = 0.034; R->me[2][2] = 0.034; R->me[3][3] = 0.00027; R->me[4][4] = 0.00027; R->me[5][5] = 0.00027; R->me[6][6] = 0.000012; R->me[7][7] = 0.000012; R->me[8][8] = 0.000012; } if(eclipse==0) { R->me[0][0] = 0.00034; R->me[1][1] = 0.00034; R->me[2][2] = 0.00034; R->me[3][3] = 0.00027; R->me[4][4] = 0.00027; R->me[5][5] = 0.00027; R->me[6][6] = 0.0000012; R->me[7][7] = 0.0000012; R->me[8][8] = 0.0000012; Q->me[0][0] = 0.00001; Q->me[1][1] = 0.00001; Q->me[2][2] = 0.00001; Q->me[3][3] = 0.0001;//0.000012;//0.0175;//1e-3; Q->me[4][4] = 0.0001;//0.0175;//1e-3; Q->me[5][5] = 0.0001;//0.0175;//1e-3; Q->me[6][6] = 0.0000000001;//1e-6; Q->me[7][7] = 0.0000000001; Q->me[8][8] = 0.0000000001; Q->me[9][9] = 0.0000000001; Q->me[10][10] = 0.0000000001; Q->me[11][11] = 0.0000000001; } else { R->me[0][0] = 0.34; R->me[1][1] = 0.34; R->me[2][2] = 0.34; R->me[3][3] = 0.0027; R->me[4][4] = 0.0027; R->me[5][5] = 0.0027; R->me[6][6] = 0.0000012; R->me[7][7] = 0.0000012; R->me[8][8] = 0.0000012; Q->me[0][0] = 0.00001; Q->me[1][1] = 0.00001; Q->me[2][2] = 0.00001; Q->me[3][3] = 0.0001; Q->me[4][4] = 0.0001; Q->me[5][5] = 0.0001; Q->me[6][6] = 0.0000000001; Q->me[7][7] = 0.0000000001; Q->me[8][8] = 0.0000000001; Q->me[9][9] = 0.0000000001; Q->me[10][10] = 0.0000000001; Q->me[11][11] = 0.0000000001; } if(omega_prev == VNULL) { omega_prev = v_get(3); v_zero(omega_prev); } if(mag_vec_prev == VNULL) { mag_vec_prev = v_get(3); v_zero(mag_vec_prev); } if(sun_vec_prev == VNULL) { sun_vec_prev = v_get(3); v_zero(sun_vec_prev); } if (err_sig_pt_mat == MNULL) { err_sig_pt_mat = m_get(n_err_states, n_sig_pts); m_zero(err_sig_pt_mat); } if(q_err_quat == VNULL) { q_err_quat = v_get(4); // q_err_quat = v_resize(q_err_quat,4); v_zero(q_err_quat); } if(err_vec == VNULL) { err_vec = v_get(3); v_zero(err_vec); } v_temp = v_get(9); v_resize(v_temp,3); if(x_prev == VNULL) { x_prev = v_get(n_states); v_zero(x_prev); x_prev->ve[3] = 1; quat_mul(x_prev,q_s_c,x_prev); x_prev->ve[4] = 0.0; x_prev->ve[5] = 0.0; x_prev->ve[6] = 0.0; x_prev->ve[7] = 0.0; x_prev->ve[8] = 0.0; x_prev->ve[9] = 0.0; x_prev->ve[10] = 0.0; x_prev->ve[11] = 0.0; x_prev->ve[12] = 0.0; } sqrt_P = m_get(n_err_states, n_err_states); m_zero(sqrt_P); //result = m_resize(result, n_err_states, n_err_states); result_larger = m_get(n_err_states, n_err_states); int n, m; for(n = 0; n < result->n; n++) { for(m = 0; m < result->m; m++) { result_larger->me[m][n] = result->me[m][n]; } } //v_resize(v_temp, n_err_states); V_FREE(v_temp); v_temp = v_get(n_err_states); symmeig(Pprev, result_larger, v_temp); i = 0; for (j=0;j<n_err_states;j++){ if(v_temp->ve[j]>=0); else{ i = 1; } } m_copy(Pprev, result1); sm_mlt(sqrt_lambda, result1, result_larger); catchall(CHfactor(result_larger), printerr(sim_time)); for(i=0; i<n_err_states; i++){ for(j=i+1; j<n_err_states; j++){ result_larger->me[i][j] = 0; } } expandstate(result_larger, x_prev, sig_pt); sig_vec_mat = m_get(n_states, n_sig_pts); m_zero(sig_vec_mat); for(j = 0; j<(n_err_states+1); j++) { for(i = 0; i<n_states; i++) { if(j==0) { sig_vec_mat->me[i][j] = x_prev->ve[i]; } else if(j>0) { sig_vec_mat->me[i][j] = sig_pt->me[i][j-1]; } } } sm_mlt(-1,result_larger,result_larger); expandstate(result_larger, x_prev, sig_pt); for(j = (n_err_states+1); j<n_sig_pts; j++) { for(i = 0; i<n_states; i++) { sig_vec_mat->me[i][j] = sig_pt->me[i][j-(n_err_states+1)]; } } single_sig_pt = v_get(n_states); quat_rot_vec(q_s_c, Torq_ext); for(j=0; j<(n_sig_pts); j++) { //v_temp = v_resize(v_temp,n_states); V_FREE(v_temp); v_temp = v_get(n_states); get_col(sig_vec_mat, j, single_sig_pt); v_copy(single_sig_pt, v_temp); rk4(t, v_temp, h, Torq_prev); set_col(sig_vec_mat, j, v_temp); } v_copy(Torq_ext, Torq_prev); x_priori = v_get(n_states); v_zero(x_priori); v_resize(v_temp,n_states); v_zero(v_temp); for(j=0; j<n_sig_pts; j++) { get_col( sig_vec_mat, j, v_temp); if(j == 0) { v_mltadd(x_priori, v_temp, w_m_0, x_priori); } else { v_mltadd(x_priori, v_temp, w_i, x_priori); } } v_copy(x_priori, v_temp); v_resize(v_temp,4); quat_normalize(v_temp);//zaroori hai ye for(i=0; i<4; i++) { x_priori->ve[i] = v_temp->ve[i]; } v_resize(v_temp, n_states); v_copy(x_priori, v_temp); v_resize(v_temp, 4); quat_inv(v_temp, v_temp); for(i=0; i<3; i++) { x_ang_vel->ve[i] = x_priori->ve[i+4]; } x_b_m = v_get(3); v_zero(x_b_m); x_b_g = v_get(3); v_zero(x_b_g); /////////////////////////check it!!!!!!!! checked... doesnt change much the estimate for(i=0; i<3; i++) { x_b_m->ve[i] = x_priori->ve[i+7]; x_b_g->ve[i] = x_priori->ve[i+10]; } v_temp2 = v_get(n_states); v_zero(v_temp2); for(j=0; j<n_sig_pts; j++) { v_resize(v_temp2, n_states); get_col( sig_vec_mat, j, v_temp2); for(i=0; i<3; i++) { err_vec->ve[i] = v_temp2->ve[i+4]; } v_resize(v_temp2, 4); quat_mul(v_temp2, v_temp, q_err_quat); v_resize(q_err_quat, n_err_states); v_sub(err_vec, x_ang_vel, err_vec); for(i=3; i<6; i++) { q_err_quat->ve[i] = err_vec->ve[i-3]; } for(i=0; i<3; i++) { err_vec->ve[i] = v_temp2->ve[i+7]; } v_sub(err_vec, x_b_m, err_vec); for(i=6; i<9; i++) { q_err_quat->ve[i] = err_vec->ve[i-6]; } for(i=0; i<3; i++) { err_vec->ve[i] = v_temp2->ve[i+10]; } v_sub(err_vec, x_b_g, err_vec); for(i=9; i<12; i++) { q_err_quat->ve[i] = err_vec->ve[i-9]; } set_col(err_sig_pt_mat, j, q_err_quat); if(j==0){ v_mltadd(x_err_priori, q_err_quat, w_m_0, x_err_priori); } else{ v_mltadd(x_err_priori, q_err_quat, w_i, x_err_priori); } } v_resize(v_temp,n_err_states); for (j=0;j<13;j++) { get_col(err_sig_pt_mat, j, v_temp); v_sub(v_temp, x_err_priori, v_temp); get_dyad(v_temp, v_temp, result_larger); if(j==0){ sm_mlt(w_c_0, result_larger, result_larger); } else{ sm_mlt(w_i, result_larger, result_larger); } m_add(P_priori, result_larger, P_priori); } symmeig(P_priori, result_larger, v_temp); i = 0; for (j=0;j<n_err_states;j++){ if(v_temp->ve[j]>=0); else{ i = 1; } } m_add(P_priori, Q, P_priori); v_resize(v_temp,3); meas = v_get(9); if (!(is_vec_equal(sun_vec, sun_vec_prev)) /*&& (eclipse==0)*/ ){ update_sun_vec =1; v_copy(sun_vec, sun_vec_prev); v_copy(sun_vec, v_temp); normalize_vec(v_temp); quat_rot_vec(q_s_c, v_temp); normalize_vec(v_temp); for(i = 0; i<3;i++){ meas->ve[i] = v_temp->ve[i]; } } else{ update_sun_vec =0; for(i = 0; i<3;i++){ meas->ve[i] = 0; } } if (!(is_vec_equal(mag_vec, mag_vec_prev)) ){ update_mag_vec =1; v_copy(mag_vec, mag_vec_prev); v_copy(mag_vec, v_temp); normalize_vec(v_temp); quat_rot_vec(q_s_c, v_temp); normalize_vec(v_temp); for(i=3; i<6; i++){ meas->ve[i] = v_temp->ve[i-3]; } } else{ update_mag_vec =0; for(i=3; i<6; i++){ meas->ve[i] = 0;//mag_vec_prev->ve[i-3]; } } if (!(is_vec_equal(omega, omega_prev) ) ){ update_omega =1; v_copy(omega, omega_prev); v_copy(omega, v_temp); quat_rot_vec(q_s_c, v_temp); for(i=6; i<9; i++){ meas->ve[i] = v_temp->ve[i-6]; } } else{ update_omega =0; for(i=6; i<9; i++){ meas->ve[i] = 0; } } v_resize(v_temp, 9); v_resize(v_temp2, n_states); v_temp3 = v_get(3); Meas_err_mat = m_get(9, n_sig_pts); m_zero(Meas_err_mat); meas_priori = v_get(9); v_zero(meas_priori); for(j=0; j<n_sig_pts; j++) { get_col( sig_vec_mat, j, v_temp2); if(update_omega){ for(i=6;i<9;i++){ v_temp->ve[i] = v_temp2->ve[i-2] + x_b_g->ve[i-6]; } } else{ for(i=6;i<9;i++){ v_temp->ve[i] = 0; } } v_resize(v_temp2, 4); if(update_sun_vec){ for(i=0;i<3;i++){ v_temp3->ve[i] = sun_vec_I->ve[i]; } quat_rot_vec(v_temp2, v_temp3); normalize_vec(v_temp3); for(i=0;i<3;i++){ v_temp->ve[i] = v_temp3->ve[i]; } } else{ for(i=0;i<3;i++){ v_temp->ve[i] = 0; } } if(update_mag_vec){ for(i=0;i<3;i++){ v_temp3->ve[i] = mag_vec_I->ve[i]; } normalize_vec(v_temp3); for(i=0;i<3;i++){ v_temp3->ve[i] = v_temp3->ve[i] + x_b_m->ve[i]; } quat_rot_vec(v_temp2, v_temp3); normalize_vec(v_temp3); for(i=3;i<6;i++){ v_temp->ve[i] = v_temp3->ve[i-3]; } } else{ for(i=3;i<6;i++){ v_temp->ve[i] = 0; } } set_col(Meas_err_mat, j, v_temp); if(j==0){ v_mltadd(meas_priori, v_temp, w_m_0, meas_priori); } else{ v_mltadd(meas_priori, v_temp, w_i, meas_priori); } } v_resize(v_temp, 9); m_resize(result_larger, 9, 9); m_zero(result_larger); P_zz = m_get(9, 9); m_zero(P_zz); iP_vv = m_get(9, 9); m_zero(iP_vv); P_xz = m_get(n_err_states, 9); m_zero(P_xz); v_resize(v_temp2, n_err_states); result1 = m_resize(result1,n_err_states,9); for (j=0; j<n_sig_pts; j++) { get_col( Meas_err_mat, j, v_temp); get_col( err_sig_pt_mat, j, v_temp2); v_sub(v_temp, meas_priori, v_temp); get_dyad(v_temp, v_temp, result_larger); get_dyad(v_temp2, v_temp, result1); if(j==0){ sm_mlt(w_c_0, result_larger, result_larger); sm_mlt(w_c_0, result1, result1); } else{ sm_mlt(w_i, result_larger, result_larger); sm_mlt(w_i, result1, result1); } m_add(P_zz, result_larger, P_zz); m_add(P_xz, result1, P_xz); } symmeig(P_zz, result_larger, v_temp); i = 0; for (j=0; j<9; j++){ if(v_temp->ve[j]>=0); else{ i = 1; } } m_add(P_zz, R, P_zz); m_inverse(P_zz, iP_vv); K = m_get(n_err_states, 9); m_zero(K); m_mlt(P_xz, iP_vv, K); if(x_posteriori_err == VNULL) { x_posteriori_err = v_get(n_err_states); v_zero(x_posteriori_err); } v_resize(v_temp,9); v_sub(meas, meas_priori, v_temp); v_copy(v_temp, residual); mv_mlt(K, v_temp, x_posteriori_err); v_resize(v_temp2,3); for(i=0;i<3;i++){ v_temp2->ve[i] = x_posteriori_err->ve[i]; } for(i=4; i<n_states; i++){ x_prev->ve[i] = (x_posteriori_err->ve[i-1] + x_priori->ve[i]); } d_res = v_norm2(v_temp2); v_resize(v_temp2,4); if(d_res<=1 /*&& d_res!=0*/){ v_temp2->ve[0] = v_temp2->ve[0]; v_temp2->ve[1] = v_temp2->ve[1]; v_temp2->ve[2] = v_temp2->ve[2]; v_temp2->ve[3] = sqrt(1-d_res); } else//baad main daala hai { v_temp2->ve[0] = (v_temp2->ve[0])/(sqrt(1+d_res)); v_temp2->ve[1] = (v_temp2->ve[1])/(sqrt(1+d_res)); v_temp2->ve[2] = (v_temp2->ve[2])/(sqrt(1+d_res)); v_temp2->ve[3] = 1/sqrt(1 + d_res); } v_resize(x_posteriori_err, n_states); for(i=(n_states-1); i>3; i--){ x_posteriori_err->ve[i] = x_posteriori_err->ve[i-1]; } for(i=0; i<4; i++){ x_posteriori_err->ve[i] = v_temp2->ve[i]; } quat_mul(v_temp2, x_priori, v_temp2); for(i=0;i<4;i++){ x_prev->ve[i] = v_temp2->ve[i]; } m_resize(result_larger, n_err_states, 9); m_mlt(K, P_zz, result_larger); result2 = m_get(9, n_err_states); m_transp(K,result2); m_resize(result1, n_err_states, n_err_states); m_mlt(result_larger, result2, result1); v_resize(v_temp, n_err_states); m_sub(P_priori, result1, Pprev); symmeig(Pprev, result1 , v_temp); i = 0; for (j=0;j<n_err_states;j++){ if(v_temp->ve[j]>=0); else{ i = 1; } } v_copy(x_prev, v_temp); v_resize(v_temp,4); v_copy(x_prev, v_temp2); v_resize(v_temp2,4); v_copy(x_prev, x_m_o); //v_resize(x_m_o, 4); v_resize(v_temp,3); quat_inv(q_s_c, v_temp2); v_copy( x_prev, state); quat_mul(state, v_temp2, state); for(i=0; i<3; i++){ v_temp->ve[i] = state->ve[i+4]; } quat_rot_vec(v_temp2, v_temp); for(i=0; i<3; i++){ state->ve[i+4] = v_temp->ve[i]; } v_copy( x_posteriori_err, st_error); iter_num++; V_FREE(x); V_FREE(x_priori); V_FREE(x_err_priori); V_FREE(single_sig_pt); V_FREE(v_temp); V_FREE(q_err_quat); V_FREE(err_vec); V_FREE(v_temp2); V_FREE(x_ang_vel); V_FREE(meas); V_FREE(meas_priori); V_FREE(v_temp3); V_FREE(x_posteriori_err); V_FREE(x_b_m); V_FREE(x_b_g); M_FREE(sqrt_P); M_FREE(P); M_FREE(P_priori); M_FREE(sig_pt); M_FREE(sig_vec_mat); M_FREE(err_sig_pt_mat); M_FREE(result); M_FREE(result_larger); M_FREE(result1); M_FREE(Meas_err_mat); M_FREE(P_zz); M_FREE(iP_vv); M_FREE(P_xz); M_FREE(K); M_FREE(result2); M_FREE(result3); }
gmx_bool constrain_lincs(FILE *fplog,gmx_bool bLog,gmx_bool bEner, t_inputrec *ir, gmx_large_int_t step, struct gmx_lincsdata *lincsd,t_mdatoms *md, t_commrec *cr, rvec *x,rvec *xprime,rvec *min_proj, matrix box,t_pbc *pbc, real lambda,real *dvdlambda, real invdt,rvec *v, gmx_bool bCalcVir,tensor vir_r_m_dr, int econq, t_nrnb *nrnb, int maxwarn,int *warncount) { char buf[STRLEN],buf2[22],buf3[STRLEN]; int i,warn,p_imax,error; real ncons_loc,p_ssd,p_max=0; rvec dx; gmx_bool bOK; bOK = TRUE; if (lincsd->nc == 0 && cr->dd == NULL) { if (bLog || bEner) { lincsd->rmsd_data[0] = 0; if (ir->eI == eiSD2 && v == NULL) { i = 2; } else { i = 1; } lincsd->rmsd_data[i] = 0; } return bOK; } if (econq == econqCoord) { if (ir->efep != efepNO) { if (md->nMassPerturbed && lincsd->matlam != md->lambda) { set_lincs_matrix(lincsd,md->invmass,md->lambda); } for(i=0; i<lincsd->nc; i++) { lincsd->bllen[i] = lincsd->bllen0[i] + lambda*lincsd->ddist[i]; } } if (lincsd->ncg_flex) { /* Set the flexible constraint lengths to the old lengths */ if (pbc != NULL) { for(i=0; i<lincsd->nc; i++) { if (lincsd->bllen[i] == 0) { pbc_dx_aiuc(pbc,x[lincsd->bla[2*i]],x[lincsd->bla[2*i+1]],dx); lincsd->bllen[i] = norm(dx); } } } else { for(i=0; i<lincsd->nc; i++) { if (lincsd->bllen[i] == 0) { lincsd->bllen[i] = sqrt(distance2(x[lincsd->bla[2*i]], x[lincsd->bla[2*i+1]])); } } } } if (bLog && fplog) { cconerr(cr->dd,lincsd->nc,lincsd->bla,lincsd->bllen,xprime,pbc, &ncons_loc,&p_ssd,&p_max,&p_imax); } /* This warn var can be updated by multiple threads * at the same time. But as we only need to detect * if a warning occured or not, this is not an issue. */ warn = -1; /* The OpenMP parallel region of constrain_lincs for coords */ #pragma omp parallel num_threads(lincsd->nth) { int th=gmx_omp_get_thread_num(); clear_mat(lincsd->th[th].vir_r_m_dr); do_lincs(x,xprime,box,pbc,lincsd,th, md->invmass,cr, bCalcVir || (ir->efep != efepNO), ir->LincsWarnAngle,&warn, invdt,v,bCalcVir, th==0 ? vir_r_m_dr : lincsd->th[th].vir_r_m_dr); } if (ir->efep != efepNO) { real dt_2,dvdl=0; dt_2 = 1.0/(ir->delta_t*ir->delta_t); for(i=0; (i<lincsd->nc); i++) { dvdl -= lincsd->mlambda[i]*dt_2*lincsd->ddist[i]; } *dvdlambda += dvdl; } if (bLog && fplog && lincsd->nc > 0) { fprintf(fplog," Rel. Constraint Deviation: RMS MAX between atoms\n"); fprintf(fplog," Before LINCS %.6f %.6f %6d %6d\n", sqrt(p_ssd/ncons_loc),p_max, ddglatnr(cr->dd,lincsd->bla[2*p_imax]), ddglatnr(cr->dd,lincsd->bla[2*p_imax+1])); } if (bLog || bEner) { cconerr(cr->dd,lincsd->nc,lincsd->bla,lincsd->bllen,xprime,pbc, &ncons_loc,&p_ssd,&p_max,&p_imax); /* Check if we are doing the second part of SD */ if (ir->eI == eiSD2 && v == NULL) { i = 2; } else { i = 1; } lincsd->rmsd_data[0] = ncons_loc; lincsd->rmsd_data[i] = p_ssd; } else { lincsd->rmsd_data[0] = 0; lincsd->rmsd_data[1] = 0; lincsd->rmsd_data[2] = 0; } if (bLog && fplog && lincsd->nc > 0) { fprintf(fplog, " After LINCS %.6f %.6f %6d %6d\n\n", sqrt(p_ssd/ncons_loc),p_max, ddglatnr(cr->dd,lincsd->bla[2*p_imax]), ddglatnr(cr->dd,lincsd->bla[2*p_imax+1])); } if (warn >= 0) { if (maxwarn >= 0) { cconerr(cr->dd,lincsd->nc,lincsd->bla,lincsd->bllen,xprime,pbc, &ncons_loc,&p_ssd,&p_max,&p_imax); if (MULTISIM(cr)) { sprintf(buf3," in simulation %d", cr->ms->sim); } else { buf3[0] = 0; } sprintf(buf,"\nStep %s, time %g (ps) LINCS WARNING%s\n" "relative constraint deviation after LINCS:\n" "rms %.6f, max %.6f (between atoms %d and %d)\n", gmx_step_str(step,buf2),ir->init_t+step*ir->delta_t, buf3, sqrt(p_ssd/ncons_loc),p_max, ddglatnr(cr->dd,lincsd->bla[2*p_imax]), ddglatnr(cr->dd,lincsd->bla[2*p_imax+1])); if (fplog) { fprintf(fplog,"%s",buf); } fprintf(stderr,"%s",buf); lincs_warning(fplog,cr->dd,x,xprime,pbc, lincsd->nc,lincsd->bla,lincsd->bllen, ir->LincsWarnAngle,maxwarn,warncount); } bOK = (p_max < 0.5); } if (lincsd->ncg_flex) { for(i=0; (i<lincsd->nc); i++) if (lincsd->bllen0[i] == 0 && lincsd->ddist[i] == 0) lincsd->bllen[i] = 0; } } else { /* The OpenMP parallel region of constrain_lincs for derivatives */ #pragma omp parallel num_threads(lincsd->nth) { int th=gmx_omp_get_thread_num(); do_lincsp(x,xprime,min_proj,pbc,lincsd,th, md->invmass,econq,ir->efep != efepNO ? dvdlambda : NULL, bCalcVir,th==0 ? vir_r_m_dr : lincsd->th[th].vir_r_m_dr); } } if (bCalcVir && lincsd->nth > 1) { for(i=1; i<lincsd->nth; i++) { m_add(vir_r_m_dr,lincsd->th[i].vir_r_m_dr,vir_r_m_dr); } } /* count assuming nit=1 */ inc_nrnb(nrnb,eNR_LINCS,lincsd->nc); inc_nrnb(nrnb,eNR_LINCSMAT,(2+lincsd->nOrder)*lincsd->ncc); if (lincsd->ntriangle > 0) { inc_nrnb(nrnb,eNR_LINCSMAT,lincsd->nOrder*lincsd->ncc_triangle); } if (v) { inc_nrnb(nrnb,eNR_CONSTR_V,lincsd->nc*2); } if (bCalcVir) { inc_nrnb(nrnb,eNR_CONSTR_VIR,lincsd->nc); } return bOK; }
int * breakout(double * ts, int n, int min_size, double beta, int degree, int *ol){ if (!ts || min_size < 5 || n < 2 * min_size || !ol){ return NULL; } double (*G)(double); switch(degree){ case 1 : G = Linear; break; case 2 : G = Quadratic; break; default : G = Const; break; } int * prev = (int*)malloc(sizeof(int) * (n + 1)); memset(prev, 0, sizeof(int) * (n + 1)); int * num = (int*)malloc(sizeof(int) * (n + 1)); memset(num, 0, sizeof(int) * (n + 1)); double * F = (double*)malloc(sizeof(double) * (n + 1)); memset(F, 0, sizeof(double) * (n + 1)); MTrace * m1 = m_create((CMP_FN)cmp_fn, NULL); MTrace * m2 = m_create((CMP_FN)cmp_fn, NULL); for (int s = 2 * min_size; s < n + 1; ++s){ m_clear(m1); m_clear(m2); for (int i = 0; i < min_size - 1; ++i){ m_add(m1, ts + i); } for (int i = min_size - 1; i < s; ++i){ m_add(m2, ts + i); } for (int t = min_size; t < s - min_size + 1; ++t){ m_add(m1, ts + t - 1); m_remove(m2, ts + t - 1); if (prev[t] > prev[t - 1]){ for (int i = prev[t - 1]; i < prev[t]; ++i){ m_remove(m1, ts + i); } } if (prev[t] < prev[t - 1]){ for (int i = prev[t]; i < prev[t - 1]; ++i){ m_add(m1, ts + i); } } double lm = *(double*)get_median(m1); double rm = *(double*)get_median(m2); double normalize = ((t - prev[t]) * (s - t)) \ / ((double)(s - prev[t]) * (s - prev[t])); double tmp_s = F[t] + normalize * (lm - rm) * (lm - rm) - beta * G(num[t]); if (tmp_s > F[s]){ num[s] = num[t] + 1; F[s] = tmp_s; prev[s] = t; } } } int k = num[n]; *ol = k; int * re = (int*)malloc(sizeof(int) * k); memset(re, 0, sizeof(int) * k); int i = n; while(i > 0){ if (prev[i]) re[--k] = prev[i]; i = prev[i]; } free(prev); prev = NULL; free(num); num = NULL; free(F); F = NULL; m_free(m1); m1 = NULL; m_free(m2); m2 = NULL; return re; }
/** * ahrs_step() takes the values from the IMU and produces the * new estimated attitude. */ void ahrs_step( v_t angles_out, f_t dt, f_t p, f_t q, f_t r, f_t ax, f_t ay, f_t az, f_t heading ) { m_t C; m_t A; m_t AT; v_t X_dot; m_t temp; v_t Xm; v_t Xe; /* Construct the quaternion omega matrix */ rotate2omega( A, p, q, r ); /* X_dot = AX */ mv_mult( X_dot, A, X, 4, 4 ); /* X += X_dot dt */ v_scale( X_dot, X_dot, dt, 4 ); v_add( X, X_dot, X, 4 ); v_normq( X, X ); /* printf( "X =" ); Vprint( X, 4 ); printf( "\n" ); */ /* AT = transpose(A) */ m_transpose( AT, A, 4, 4 ); /* P = A * P * AT + Q */ m_mult( temp, A, P, 4, 4, 4 ); m_mult( P, temp, AT, 4, 4, 4 ); m_add( P, Q, P, 4, 4 ); compute_c( C, X ); /* Compute the euler angles of the measured attitude */ accel2euler( Xm, ax, ay, az, heading ); /* * Compute the euler angles of the estimated attitude * Xe = quat2euler( X ) */ quat2euler( Xe, X ); /* Kalman filter update */ kalman_update( P, R, C, X, Xe, Xm ); /* Estimated attitude extraction */ quat2euler( angles_out, X ); }
void m_add_msgid(struct mproc *m, uint32_t v) { m_add(m, &v, sizeof(v)); }
void m_add_sockaddr(struct mproc *m, const struct sockaddr *sa) { m_add_size(m, sa->sa_len); m_add(m, sa, sa->sa_len); }
void m_add_int(struct mproc *m, int v) { m_add(m, &v, sizeof(v)); };
// Veh_Integral void Vehicle::Update() { double wind_dir, hdng, rand_wind=0.0; double headrot[4]; double senspostang[2], vehsenspos[2]; double chemDensity; Point3D vpos; double u_wind[3]; int i_v,iii,jjj, j; static int detect_yet; double Sim_Time; if(ed == NULL) return; // if no enviromental data then return Sim_Time = ed->GetSimTime(); // save last pos GetPos(last_p); // Simulate the batch of vehicles for(i_v = 0; i_v < NUM_VEH; i_v++) { if(!(veh[i_v].src_fnd_flg)) // if not done { // following are the lines where the vehicle simulation interfaces // with the wind and concentration sensor models. The wind speed // and concentration can be evaluated at any position of interest hdng = veh[i_v].omega; headrot[0] = cos(hdng); headrot[1] = -sin(hdng); headrot[2] = sin(hdng); headrot[3] = cos(hdng); double sensposbody[2]; for(j = 0; j < NUM_SENS; j++) { sensposbody[0] = 0; sensposbody[1] = 0; // (-ns+j)*lngth/ns; m_mul(senspostang, headrot, sensposbody, 2, 2, 1); m_add(vehsenspos, &(veh[i_v].x), senspostang, 2, 1); //veh[i_v].snsd[j] = chem->ReadConc(vehsenspos, u_wind); { vpos.x = vehsenspos[0]; vpos.y = vehsenspos[1]; vpos.z = 0.0; // vehsenspos[2] ed->ApplyWind(vpos, u_wind); chemDensity = ed->GetChemDensity(vpos); veh[i_v].snsd[j] = chemDensity; vehsenspos[0] = vpos.x; vehsenspos[1] = vpos.y; //vehsenspos[2] = vpos.z; } } if (i_v==0) rand_wind = 5.0*nrand(); wind_dir = atan2(u_wind[1],u_wind[0])*180/PI + rand_wind; // following is the call to the strategy strat->in[0] = veh[i_v].x; strat->in[1] = (-1)*veh[i_v].y; // invert because of axis flip strat->in[2] = sqrt(u_wind[0]*u_wind[0] + u_wind[1]*u_wind[1]); strat->in[3] = wind_dir; strat->in[4] = Sim_Time; strat->in[5] = veh[i_v].u; strat->in[6] = veh[i_v].omega; // added by JES. copy global to fit older code double src_loc[2]; src_loc[0] = srcloc.x; src_loc[1] = srcloc.y; // for(j = 7; j < 7 + NUM_SENS; j++) { strat->in[j] = veh[i_v].snsd[j-7]; } strat->out[0]= veh[i_v].vel_c; // Initialize output values strat->out[1]= veh[i_v].omega_c; // Initialize output values strat->out[2]= veh[i_v].src_fnd_flg; // Initialize output values //-------------------------Call Strategy---------------------------------------------- MyStratFunc(strat->in, strat->out, &(strat->mem[N_STRAT_MEM*i_v]), dt, radius, threshold, Sim_Time, currentBehaviorName ); //-------------------------Return from strategy---------------------------------------------- veh[i_v].vel_c = strat->out[0]; veh[i_v].omega_c = strat->out[1]; // The source found flag once set is set for the run veh[i_v].src_fnd_flg = ((strat->out[2])||(veh[i_v].src_fnd_flg)); x_left=0.0; y_top=-50; d_x=1; d_y=1; if( veh[i_v].snsd[0] > threshold) { veh[i_v].det_tm = Sim_Time; // store most recent detection time if(veh[i_v].plm_find_tm > Sim_Time) veh[i_v].plm_find_tm = Sim_Time; // record time of first detection } if( ( hdist(&(veh[i_v].x), src_loc) < radius) && // near the source ( veh[i_v].src_find_tm > Sim_Time) ) // for the first time { veh[i_v].src_find_tm = Sim_Time; // store the frst tm near src fnd_cnt++; } // if near source if( (veh[i_v].src_fnd_flg) && (veh[i_v].src_dclr_tm > Sim_Time) ) { // Graphics -------------------------------------------- if( (batch_not == 1) && (i_v ==0 ) ) { for(iii = 0; iii < GRID_X; iii++) { for(jjj = 0; jjj < GRID_Y; jjj++) { bck_grnd[iii][jjj] = (float)(strat->mem[1000+iii*100+jjj]); } } // RESET! veh[i_v].x = vehicle_pos_x; // ((double)random(100))*(x_max-x_min)/100.0 + x_min; veh[i_v].y = vehicle_pos_y; // ((double)random(100))*(y_max-y_min)/100.0 + y_min; veh[i_v].z = vehicle_pos_z; // ((double)random(100))*(z_max-z_min)/100.0 + z_min; veh[i_v].omega_c = nrand()*180.0; veh[i_v].dsd = 0.0; veh[i_v].dsf = 0.0; veh[i_v].dpf = 0.0; veh[i_v].det_tm = -100; veh[i_v].plm_find_tm = 1e8; veh[i_v].src_find_tm = 1e8; veh[i_v].src_dclr_tm = 1e8; veh[i_v].src_dclr_d = 0.0; veh[i_v].snsd[NS + 1] = 0; veh[i_v].src_fnd_flg = 0; } // -------------------------------------------- else { veh[i_v].src_dclr_tm = Sim_Time; // store tm of first src found flag veh[i_v].src_dclr_d = hdist(&(veh[i_v].x), src_loc); veh[i_v].x_real=veh[i_v].x; veh[i_v].y_real=veh[i_v].y; veh[i_v].vel_c = 0; // stop this vehicle done_cnt++; // veh is done } } // if src fnd flag vehicle_model(&(veh[i_v]), Sim_Time); // advance vehicle to next time //Graphics -------------------------------------------- if((batch_not==1)&&(i_v==0)) { // Have we detected anything this interval detect_yet = ( (veh[i_v].snsd[0] > threshold) || (detect_yet) ); veh_plot_cnt++; // track interval if(veh_plot_cnt >= VEH_MAX_PLT_CNT) { veh_plot_cnt = 0; if(vh_count < VH_M) { det_flag[vh_count] = detect_yet; detect_yet = 0; veh_omega_flag[vh_count] = veh[0].omega*PI/180.0; vh_x[vh_count] = veh[0].x; vh_y[vh_count] = veh[0].y; vh_count++; } else { vehicle_update(); det_flag[VH_M-1] = detect_yet; detect_yet = 0; veh_omega_flag[VH_M-1] = veh[0].omega*PI/180.0; vh_x[VH_M-1] = veh[0].x; vh_y[VH_M-1] = veh[0].y; } } // veh_plot_cnt } // not batch // -------------------------------------------- } // if not vehicle done } // for each vehicle init_flag = 0; }
void m_add_u32(struct mproc *m, uint32_t u32) { m_add(m, &u32, sizeof(u32)); };
/* This function takes a pointer to a block of memory and inserts it into * the chain of memory chunks */ Static void __insert_chunk(mem * mem_chunk) { register mem *p1, *p2; if (chunk_list == 0) { /* Simple case first */ m_next(mem_chunk) = chunk_list = mem_chunk; noise("FIRST CHUNK", mem_chunk); return; } p1 = mem_chunk; p2 = chunk_list; do { if (p1 > p2) { /* We're at the top of the chain, p1 is higher */ if (m_next(p2) <= p2) { if (m_add(p2, m_size(p2)) == p1) { /* Good, stick 'em together */ noise("INSERT CHUNK", mem_chunk); m_size(p2) += m_size(p1); noise("JOIN 1", p2); } else { m_next(p1) = m_next(p2); m_next(p2) = p1; noise("INSERT CHUNK", mem_chunk); noise("FROM", p2); } return; } if (m_next(p2) > p1) { /* In chain, p1 between p2 and next */ m_next(p1) = m_next(p2); m_next(p2) = p1; noise("INSERT CHUNK", mem_chunk); noise("FROM", p2); /* Try to join above */ if (m_add(p1, m_size(p1)) == m_next(p1)) { m_size(p1) += m_size(m_next(p1)); m_next(p1) = m_next(m_next(p1)); noise("JOIN 2", p1); } /* Try to join below */ if (m_add(p2, m_size(p2)) == p1) { m_size(p2) += m_size(p1); m_next(p2) = m_next(p1); noise("JOIN 3", p2); } chunk_list = p2; /* Make sure it's valid */ return; } } else if (p1 < p2) { if (m_next(p2) <= p2 && p1 < m_next(p2)) { /* At top of chain, next is bottom of chain, * p1 is below next */ m_next(p1) = m_next(p2); m_next(p2) = p1; noise("INSERT CHUNK", mem_chunk); noise("FROM", p2); chunk_list = p2; if (m_add(p1, m_size(p1)) == m_next(p1)) { if (p2 == m_next(p1)) chunk_list = p1; m_size(p1) += m_size(m_next(p1)); m_next(p1) = m_next(m_next(p1)); noise("JOIN 4", p1); } return; } } chunk_list = p2; /* Save for search */ p2 = m_next(p2); } while (p2 != chunk_list); /* If we get here we have a problem, ignore it, maybe it'll go away */ noise("DROPPED CHUNK", mem_chunk); }
void m_add_size(struct mproc *m, size_t sz) { m_add(m, &sz, sizeof(sz)); };
void m_add_time(struct mproc *m, time_t v) { m_add(m, &v, sizeof(v)); };
void m_add_timeval(struct mproc *m, struct timeval *tv) { m_add(m, tv, sizeof(*tv)); }
static void do_my_pme(FILE *fp,real tm,gmx_bool bVerbose,t_inputrec *ir, rvec x[],rvec xbuf[],rvec f[], real charge[],real qbuf[],real qqbuf[], matrix box,gmx_bool bSort, t_commrec *cr,t_nsborder *nsb,t_nrnb *nrnb, t_block *excl,real qtot, t_forcerec *fr,int index[],FILE *fp_xvg, int ngroups,unsigned short cENER[]) { real ener,vcorr,q,xx,dvdl=0,vdip,vcharge; tensor vir,vir_corr,vir_tot; rvec mu_tot[2]; int i,m,ii,ig,jg; real **epme,*qptr; /* Initiate local variables */ fr->f_el_recip = f; clear_mat(vir); clear_mat(vir_corr); if (ngroups > 1) { fprintf(fp,"There are %d energy groups\n",ngroups); snew(epme,ngroups); for(i=0; (i<ngroups); i++) snew(epme[i],ngroups); } /* Put x is in the box, this part needs to be parallellized properly */ /*put_atoms_in_box(box,nsb->natoms,x);*/ /* Here sorting of X (and q) is done. * Alternatively, one could just put the atoms in one of the * cr->nnodes slabs. That is much cheaper than sorting. */ for(i=0; (i<nsb->natoms); i++) index[i] = i; if (bSort) { xptr = x; qsort(index,nsb->natoms,sizeof(index[0]),comp_xptr); xptr = NULL; /* To trap unintentional use of the ptr */ } /* After sorting we only need the part that is to be computed on * this processor. We also compute the mu_tot here (system dipole) */ clear_rvec(mu_tot[0]); for(i=START(nsb); (i<START(nsb)+HOMENR(nsb)); i++) { ii = index[i]; q = charge[ii]; qbuf[i] = q; for(m=0; (m<DIM); m++) { xx = x[ii][m]; xbuf[i][m] = xx; mu_tot[0][m] += q*xx; } clear_rvec(f[ii]); } copy_rvec(mu_tot[0],mu_tot[1]); if (debug) { pr_rvec(debug,0,"qbuf",qbuf,nsb->natoms,TRUE); pr_rvecs(debug,0,"xbuf",xbuf,nsb->natoms); pr_rvecs(debug,0,"box",box,DIM); } for(ig=0; (ig<ngroups); ig++) { for(jg=ig; (jg<ngroups); jg++) { if (ngroups > 1) { for(i=START(nsb); (i<START(nsb)+HOMENR(nsb)); i++) { if ((cENER[i] == ig) || (cENER[i] == jg)) qqbuf[i] = qbuf[i]; else qqbuf[i] = 0; } qptr = qqbuf; } else qptr = qbuf; ener = do_pme(fp,bVerbose,ir,xbuf,f,qptr,qptr,box,cr, nsb,nrnb,vir,fr->ewaldcoeff,FALSE,0,&dvdl,FALSE); vcorr = ewald_LRcorrection(fp,nsb,cr,fr,qptr,qptr,excl,xbuf,box,mu_tot, ir->ewald_geometry,ir->epsilon_surface, 0,&dvdl,&vdip,&vcharge); gmx_sum(1,&ener,cr); gmx_sum(1,&vcorr,cr); if (ngroups > 1) epme[ig][jg] = ener+vcorr; } } if (ngroups > 1) { if (fp_xvg) fprintf(fp_xvg,"%10.3f",tm); for(ig=0; (ig<ngroups); ig++) { for(jg=ig; (jg<ngroups); jg++) { if (ig != jg) epme[ig][jg] -= epme[ig][ig]+epme[jg][jg]; if (fp_xvg) fprintf(fp_xvg," %12.5e",epme[ig][jg]); } } if (fp_xvg) fprintf(fp_xvg,"\n"); } else { fprintf(fp,"Time: %10.3f Energy: %12.5e Correction: %12.5e Total: %12.5e\n", tm,ener,vcorr,ener+vcorr); if (fp_xvg) fprintf(fp_xvg,"%10.3f %12.5e %12.5e %12.5e\n",tm,ener+vcorr,vdip,vcharge); if (bVerbose) { m_add(vir,vir_corr,vir_tot); gmx_sum(9,vir_tot[0],cr); pr_rvecs(fp,0,"virial",vir_tot,DIM); } fflush(fp); } }
void m_add_data(struct mproc *m, const void *v, size_t len) { m_add_size(m, len); m_add(m, v, len); };
real sum_ekin(t_grpopts *opts, gmx_ekindata_t *ekind, real *dekindlambda, gmx_bool bEkinAveVel, gmx_bool bSaveEkinOld, gmx_bool bScaleEkin) { int i, j, m, ngtc; real T, ek; t_grp_tcstat *tcstat; real nrdf, nd, *ndf; ngtc = opts->ngtc; ndf = opts->nrdf; T = 0; nrdf = 0; clear_mat(ekind->ekin); for (i = 0; (i < ngtc); i++) { nd = ndf[i]; tcstat = &ekind->tcstat[i]; /* Sometimes a group does not have degrees of freedom, e.g. * when it consists of shells and virtual sites, then we just * set the temperatue to 0 and also neglect the kinetic * energy, which should be zero anyway. */ if (nd > 0) { if (bEkinAveVel) { if (!bScaleEkin) { /* in this case, kinetic energy is from the current velocities already */ msmul(tcstat->ekinf, tcstat->ekinscalef_nhc, tcstat->ekinf); } } else { /* Calculate the full step Ekin as the average of the half steps */ for (j = 0; (j < DIM); j++) { for (m = 0; (m < DIM); m++) { tcstat->ekinf[j][m] = 0.5*(tcstat->ekinh[j][m]*tcstat->ekinscaleh_nhc + tcstat->ekinh_old[j][m]); } } } m_add(tcstat->ekinf, ekind->ekin, ekind->ekin); tcstat->Th = calc_temp(trace(tcstat->ekinh), nd); tcstat->T = calc_temp(trace(tcstat->ekinf), nd); /* after the scaling factors have been multiplied in, we can remove them */ if (bEkinAveVel) { tcstat->ekinscalef_nhc = 1.0; } else { tcstat->ekinscaleh_nhc = 1.0; } } else { tcstat->T = 0; tcstat->Th = 0; } T += nd*tcstat->T; nrdf += nd; } if (nrdf > 0) { T /= nrdf; } if (dekindlambda) { *dekindlambda = 0.5*(ekind->dekindl + ekind->dekindl_old); } return T; }
void m_add_evpid(struct mproc *m, uint64_t v) { m_add(m, &v, sizeof(v)); }
iterator<Pruner> insert(Feature const& interval, int32_t coverage=0, Pruner const& prune=Pruner()) { m_type = LT_UNADD; iterator<Pruner> newFeature = iterator<Pruner>(prune, m_color, m_add(interval, m_bottom, prune)); switch(m_type) { case LT_UNADD: { break; } case LT_ERROR: { // throw exception break; } case LT_GENERATOR: { // add coverage break; } case LT_CANDIDATE: { iterator<Pruner> tmp = newFeature; newFeature = insert((*newFeature).label() + interval, 0, prune); Node<Feature>::iterator edge = (*tmp).begin(), last = (*tmp).end(); while(edge != last) { (*newFeature).orphan(*tmp); if(&(*edge).target() != &(*newFeature)) { bool found = false; Node<Feature>::iterator child = (*newFeature).begin(), final = (*newFeature).end(); while(child != final) { if(&(*edge).target() == &(*child).target()) { found = true; break; } ++child; } if(!found) { (*newFeature).adopt((*edge).target()); } } (*tmp).orphan(edge); ++edge; } //delete &(*tmp); break; } case LT_NEW: { // return newFeature break; } default: { std::cerr << "unknown m_type" << std::endl; } }
real sum_ekin(bool bFirstStep, t_grpopts *opts,gmx_ekindata_t *ekind, tensor ekin,real *dekindlambda) { int i,j,m,ngtc; real T,ek; t_grp_tcstat *tcstat; real nrdf,nd,*ndf; ngtc = opts->ngtc; ndf = opts->nrdf; clear_mat(ekin); T = 0; nrdf = 0; for(i=0; (i<ngtc); i++) { tcstat = &ekind->tcstat[i]; nd = ndf[i]; /* Sometimes a group does not have degrees of freedom, e.g. * when it consists of shells and virtual sites, then we just * set the temperatue to 0 and also neglect the kinetic * energy, which should be zero anyway. */ if (nd > 0) { if (bFirstStep) { /* This Ekin is only used for reporting the initial temperature * or when doing mdrun -rerun. */ copy_mat(tcstat->ekinh,tcstat->ekin); } else { /* Calculate the full step Ekin as the average of the half steps */ for(j=0; (j<DIM); j++) for(m=0; (m<DIM); m++) tcstat->ekin[j][m] = 0.5*(tcstat->ekinh[j][m] + tcstat->ekinh_old[j][m]); } m_add(tcstat->ekin,ekin,ekin); ek = 0; for(m=0; (m<DIM); m++) ek += tcstat->ekinh[m][m]; tcstat->Th = calc_temp(ek,nd); ek = 0; for(m=0; (m<DIM); m++) ek += tcstat->ekin[m][m]; tcstat->T = calc_temp(ek,nd); } else { tcstat->T = 0; tcstat->Th = 0; } T += nd*tcstat->T; nrdf += nd; } if (nrdf > 0) T/=nrdf; if (dekindlambda) *dekindlambda = 0.5*(ekind->dekindl + ekind->dekindl_old); return T; }